Vins Theory and Code detail 4 - Initialization

VINSThéorie et détails du Code4——Initialisation

1.      Initialisation monoculaire visuelle pure basée sur une fenêtre coulissante

    Avant d'introduire l'initialisation visuelle pure, parlons d'abord de la raison de l'initialisation？Initialiser ce qu'il faut faire？Et le rôle de l'initialisation？La raison de notre initialisation est que le système à couplage étroit inertiel monoculaire est un système très non linéaire,Tout d'abord, le monoculaire est incapable d'obtenir l'échelle absolue dans l'espace,EtIMUEt il doit y avoir un biais,L'accélération gravitationnelle est également nécessaire pour résoudre plus tard（Y compris la taille et l'orientation）,Pour les conditions sensibles à la vitesse,Comme un drone,Et des informations précises sur la vitesse,Donc,,Comment calculer efficacement ces quantités avant le traitement du système à couplage serré,Il est important pour la robustesse de l'ensemble du système à couplage serré（En fait, cela peut être interprété comme un étalonnage de caméra,Pas d'étalonnage correct des paramètres internes de la caméra,La caméra ne peut pas être positionnée,Et il y a de fortes chances qu'il se pende）.Donc initialiser ce qu'il faut faire est en fait assez simple,Est de calculer l'échelle absolues、Décalage gyroscopiquebg、Décalage d'accélérationba、Accélération gravitationnelleGEt chaqueIMULa vitesse du momentv,VINSIl est souligné que les valeurs de biais de l'accéléromètre sont généralement couplées à l'accélération gravitationnelle（C'est - à - dire absorbé par l'accélération gravitationnelle）,L'accélération gravitationnelle est beaucoup plus grande que son décalage d'accélération,Et le décalage de l'accéléromètre est faible pendant le temps d'initialisation,C'est dur de calculer,Donc ignorer l'effet du décalage de l'accéléromètre,Ne plus calculer dans l'initialisation.Le rôle de l'initialisation est évident, Influence directe sur la robustesse et la précision de positionnement de l'ensemble du système de couplage serré , Et l'initialisation prend généralement beaucoup de temps ,VINS Ça prendra une dizaine de secondes. ,ORB_SLAM2UnionIMU L'heure est fixée à 15 Secondes pour terminer l'initialisation .Pas grand - chose à dire,Aller directement au sujet.

         Initialisation visuelle pure à la VCommandéASection, Construisez d'abord une fenêtre coulissante , Contient un ensemble de cadres de données . Les références dans cet article sont faites à l'utilisation de modèles géométriques antipolaires 5 Résolution de la transformation relative de la caméra monoculaire par la méthode des points , Déplacement comprenant une rotation relative et aucune information d'échelle . En fait, chaque modèle monoculaire utilise essentiellement la géométrie polaire opposée pour résoudre la transformation relative de deux cadres dans l'initialisation , Il est important de noter ici que la rotation est invariante d'échelle （ En fait, c'est la rotation de l'unit é. , Aucune information sur l'échelle , Réfléchis bien, n'est - ce pas? ？）. Et puis la triangulation donne 3dCoordonnées des points,Voilà.3d Points et autres cadres dans la fenêtre coulissante 2d Le point est fait PNP Résoudre pour obtenir toutes les poses et les points caractéristiques dans la fenêtre coulissante 3dCoordonnées,Jusqu'ici., L'initialisation visuelle pure est terminée .C'est si simple？Bien sûr., Après tout, ce n'est qu'une simple initialisation visuelle , Et ce qui est vraiment compliqué, c'est l'initialisation conjointe de l'inertie visuelle , C'est là que nous avons commencé à mettre l'accent et la difficulté , Donc les points de connaissance suivants doivent commencer la spiritualité ！

 

2.      Initialisation conjointe de l'inertie visuelle

    L'Union d'inertie visuelle est initialisée dans VCommandéBSection, Ici, l'auteur donne le nom de la définition Visual-Inertia Alignment, C'est - à - dire l'initialisation conjointe de l'inertie visuelle （EtORBSLAM2+IMUDans son article, Le nom défini par l'auteur s'appelle IMU initialization,C'est - à - dire:IMUInitialisation）, Pourquoi définir un tel nom , Je pense qu'il y a deux significations , La première étape consiste à utiliser simultanément IMU Rotation des mesures et rotation des mesures visuelles , C'est - à - dire combiner les données visuelles et inertielles . La deuxième mesure ici S La valeur de est plus que IMUDe, Ou la vision et IMU L'échelle de l'ensemble du système . Lors de l'initialisation de chaque processus dans une explication spécifique , Il est nécessaire d'avoir une vue d'ensemble , La définition physique de l'initialisation est en fait l'étalonnage des paramètres inhérents , La définition d'un modèle mathématique est en fait une formule （6） Pour résoudre l'équation matricielle de ,Et la formule（6） En fait, ça vient de l'original. PVQFormule intégrale,Parmi euxQ La rotation correspond au gyroscope ,EtPV Correspond à un accéléromètre ,Si je ne comprends pas,C'est pas grave., Après avoir vu l'ensemble du processus de dérivation ci - dessous, je suis sûr que vous serez très intelligent .

（1）      Étalonnage du décalage gyroscopique

La rotation peut être obtenue de deux façons , L'un est la mesure gyroscopique , L'une est l'observation visuelle . Les deux doivent être de la même taille, selon la compréhension normale （ Supposons qu'il n'y ait pas d'erreur ）, Mais il doit y avoir une erreur dans la réalité , Nous allons voir les erreurs respectives . L'erreur du gyroscope a deux parties pour mesurer le bruit et le décalage du gyroscope , Le bruit peut être ignoré temporairement （ Après tout, c'est trop petit ）, Et l'erreur de vision n'est que le bruit d'observation （ Ou vous pouvez ignorer ）, La valeur absolue de la différence entre les deux est donc le décalage gyroscopique , L'ensemble de la fenêtre coulissante a été dévié pour former un modèle d'erreur minimale ：

    Formule15 Le produit généralisé de la rotation Quaternion des premier et deuxième termes du premier terme dans la première équation donne la caméra à partir de bkÀbk+1 Rotation relative en dessous （bk+1Sous le système de coordonnées）, Le troisième élément est que le gyroscope bk+1Àbk Rotation relative en dessous （bkSous le système de coordonnées）, Ils font un produit généralisé , C'est d'abord à partir de bkÀbk+1Rotation,Et puis à partir debk+1ÀbkRotation, C'est l'équivalent d'un mauvais travail （OA+AO=0）, La deuxième formule est l'Approximation linéaire de premier ordre mentionnée dans la pré - intégrale précédente . Puis prenez les moindres carrés ,Bien sûr, vous pouvez aussi utiliserSVD Résolution par décomposition et autres méthodes . Il est à noter qu'après avoir obtenu le décalage gyroscopique, le décalage gyroscopique doit être remplacé par le décalage gyroscopique dans la pré - intégration et la valeur de la pré - intégration doit être calculée à nouveau , Ce sera plus précis .

（1）      Vitesse、 Accélération gravitationnelle et étalonnage de l'échelle

Ici, l'auteur unifie ces trois variables d'état en un seul vecteur d'état ,Comme la formule16Comme indiqué：

La vitesse est à bkSous les coordonnées, Accélération gravitationnelle dans le système de coordonnées initial de la caméra ,Comme mentionné précédemment, Pour résoudre plusieurs quantités P、V Le modèle mathématique calcule , Tenir compte de deux touches consécutives dans la fenêtre coulissante bkEtbk+1, Voici la formule de cet article 17Et19Dérivation de：

    Après déduction de la formule, on obtient la formule dans le document 17、18Et19, Nous nous concentrons sur la raison pour laquelle nous dérivons , Et les équations de mouvement dérivées . D'abord, pourquoi faire une telle déduction? , Cela dépend entièrement de la façon dont le vecteur d'état est défini , Le côté gauche de l'équation que nous allons finalement obtenir doit être exprimé sous la forme d'un vecteur d'état , Et que d'autres quantités sont connues. （DeIMU Pré - intégration et suivi visuel ）, Il est donc nécessaire d'effectuer un tel changement dans l'équation , Pour satisfaire une telle relation . Et puis la forme finale nous pouvons voir que la dimension formelle finale du vecteur d'état est （n+1）*3+3+1, Les dimensions de l'équation de mouvement générée par deux cadres consécutifs sont 3+3+3+1（vbkbk,vbk+1bk+1,gc0,s）, Comparez les dimensions pour voir ce qui en résulte H La matrice doit être une matrice symétrique définie positive , Il est donc possible d'utiliser rapidement CholeskyDécomposition.

 

（1）       Optimisation de la gravité

    En fait, on a obtenu la taille et la direction de l'accélération gravitationnelle. , Pourquoi faut - il encore optimiser la gravité ici ？La raison est simple, La gravité calculée ici absorbe le biais de l'accéléromètre gravitationnel , Bien qu'il ne soit pas nécessaire de calculer le décalage de l'accéléromètre gravitationnel , Mais la gravité doit encore être optimisée , En parlant d'optimisation de l'accélération gravitationnelle , Ça doit contenir deux quantités ,Taille et orientation, Trois dimensions. , Mais en général, la taille est connue （C'est réglé pour9.8）, .Donc tout ce que nous avons à faire est d'optimiser la direction , Est un vecteur bidimensionnel , La figure ci - dessous montre comment optimiser la gravité et b1,b2 Modèle de détermination de la direction du vecteur unitaire .

 Détails du Code：

Note:：Allez.IMUPré - intégration

Étapes2： Traitement des données caractéristiques de l'image

Les données ne sont pas des images. , Au lieu de cela, les coordonnées du Plan normalisé qui ont été tracées et appariées . Stocke les caractéristiques du cadre courant dans imageMoyenne,image Le premier type d'élément pour est le numéro de la caméra , Représente l'image du cadre （De0C'est parti.）, Le deuxième élément est la normalisation des coordonnées des points caractéristiques et des numéros des points caractéristiques （De1C'est parti.）, Puis directement dans le thread qui traite les données caractéristiques de l'image estimator.processImage(image, img_msg->header).

1）Estimator::processImage(constmap<int, vector<pair<int, Vector3d>>> &image, conststd_msgs::Header &header) Méthode de traitement des données caractéristiques de l'image

         Tout d'abord, les données de caractéristiques d'image entrantes sont jugées comme des images clés en fonction de la parallaxe , Choisissez de laisser tomber le cadre actuel （Mais gardezIMUDonnées） Ou jeter le cadre le plus ancien de la fenêtre coulissante .

Étapes1： Enregistrer les données d'image et le temps dans la classe Image Frame ： Enregistrer d'abord les données et le temps sur l'objet du cadre d'image imageframeMoyenne（ImageFrame L'objet contient des points caractéristiques ,Temps,PositionR,t, Objets pré - intégrés pre_integration,Est - ce une image clé）, En même temps, sauvegardez la valeur temporaire de la composante pré - intégrale dans cet objet （ La valeur initiale de la pré - intégration temporaire ici est juste avant IMU Calculé au moment de la pré - intégration ）, Puis l'objet du cadre d'image imageframeEnregistrer àall_image_frameDans l'objet（imageframeConteneur pour）, Mettre à jour les valeurs initiales provisoires de pré - intégration .

Étapes2： Calibrer la caméra et IMUParamètres externes pour： Ensuite, calibrer les paramètres externes s'il n'y a pas de paramètres externes , Sauter cette étape si le paramètre est passé （Par défaut, Si c'est votre propre appareil ,Peut être réglé à2 Étalonnage en ligne des références externes ）.

Étapes3： Initialiser le système en même temps BAOptimisation： Lorsque le solveur est initialisable （ L'état initial est initialisable , L'initialisation réussie est définie à l'état non initialisable ）,Juge le courantframe_countOui NonWINDOW_SIZE, Assurez - vous qu'il y ait suffisamment de frame Participer à l'initialisation ,Ici.frame_count Est le nombre de cadres d'image dans la fenêtre coulissante ,Initialisé au début0, Le nombre total de cadres de fenêtre coulissante est 10. Il y a des paramètres externes et l'horodatage actuel du cadre est supérieur à l'horodatage d'initialisation 0.1Secondes,Pour initialiser.

Étapes3.1：initialStructure()Initialisation du système,Initialiser d'abordVision-only SFM,Puis initialiserVisual-Inertial Alignment, Constitue l'ensemble du processus d'initialisation .

①GarantieIMU Plein exercice , Jugement par accélération linéaire , Écart type de l'accélération de la ligne de passage initiale （Degré de dispersion） Garantie de jugement IMU Plein exercice , L'écart type d'accélération est supérieur à 0.25Représentantimu Motivation suffisante , Assez pour initialiser .

② Initialisation visuelle pure ,C'est exact.SlidingWindow Résolution du cadre d'image et de la posture de la caméra sfmQuestions, La solution ici est Images clés Position et coordonnées des points caractéristiques .

Étapes1.Construire d'abordSFMFeatureObjetsfm_f,SFMFeature Le tableau contient l'état du point caractéristique （ Si elle est triangulée ）,id,2dPoint,3d Coordonnées et profondeur , Enregistrer les informations de fonctionnalité dans le gestionnaire de fonctionnalités à SFMFeatureObjetsfm_fMoyennesfm_f.push_back(tmp_feature).

Étapes2. Ensuite, par la contrainte de la paire F Matrice restaurée R、t, Méthode d'appel principale relativePose(relative_R, relative_T, l).relativePoseLa méthode passe d'abord parFeatureManegerAccès（Dans la fenêtre coulissante）Noi Correspondance des caractéristiques du cadre et du dernier cadre corres,Quandcorres Quand la correspondance est assez grande , Regardez les derniers keyFrameEtsliding windowL'un d'eux.keyFrame Il y a assez entre feature Parallaxe correspondant et assez grand （idPourl=i）,Ces deux conditions sont remplies, Ensuite, les images sont récupérées en cinq points R,t Et trianguler 3DPoints caractéristiques defeature point,Voici l'utilisationsolveRelativeRT(corres, relative_R, relative_T),solveRelativeRTDéfinition de la méthodesolv_5pts.cppDans la classe, Limité par la paire de pôles F Matrice restaurée R、t,Appel directopencvMéthodes,Il n'y a rien à dire, Ce qui est remarquable ici,Ce genre derelativePose La position obtenue est lFrame,Nol Le filtrage des cadres est effectué à partir du premier cadre jusqu'au début de tous les cadres de la fenêtre coulissante qui satisfont à une parallaxe moyenne suffisamment grande ,Le numéro icil Le cadre sert de cadre de référence pour le Global suivant SFMUtiliser. Ici vous avez les points caractéristiques de l'image 2d Extraction des coordonnées , Caméra No. l Rotation et traduction entre le cadre et le dernier （ Notez qu'il n'y a pas de signature pour le moment. 3dCoordonnées des points）, Avec cette information, on peut construire une vue d'ensemble SFMCatégorieGlobalSFM sfm,Appelez icisfm.construct(frame_count + 1, Q, T,l,relative_R, relative_T,sfm_f, sfm_tracked_points), Ici, par lCadre comme cadre de référence,En coursPNPAvant de résoudre, Le nombre actuel de cadres doit être jugé supérieur à lCadre,Cela garantit quel Saut de cadre direct PNPÉtapes, Commencez par le paragraphe suivant l Triangulation du cadre et du dernier , Obtenir les points caractéristiques de la vue commune , Pour les paragraphes suivants l+1 Résolution du cadre et du dernier cadre PNP,Et utiliserpnpRésoudrel+1 Position du cadre au dernier R_initial, P_initial, Les positions finales sont sauvegardées dans PoseMoyenne,Cycle primaire,Je l'ai.l+1,l+2…n-1 Position du cadre . Sautez à l'étape 2 Après le cycle de , Et voilà l+1,l+2…n-1 Position du cadre et l+1,l+2…Cadre etn-1 Triangulation des points caractéristiques du cadre . Puis triangulation lCadres etiCadre（Enl Le cadre entre le cadre et le dernier ）Entre3dCoordonnées,（ Je ne vois pas pourquoi on fait ça deux fois. , Est - il possible de trianguler plus de points caractéristiques ？？？？）,Et voilà.PNPRésoudrel-1,l-2…0Cadres etl La posture entre les cadres a triangulé les coordonnées des points caractéristiques correspondants , Triangulation finale de tous les autres points caractéristiques . Vous obtenez ici la position de toutes les caméras dans la fenêtre coulissante et les points caractéristiques 3dCoordonnées.No6 Le Ministère est en train de BAOptimisation,Utiliséceres Optimiser la posture et les points caractéristiques , Voir ici SLAM Le contenu de la leçon 10 , Même optimisation .

Étapes4：visualInitialAlignAppel moyenVisualIMUAlignmentMéthodes, Initialisation conjointe de l'inertie visuelle réelle ,imu Alignement visuel , Obtenir des mesures absolues, etc .Cette méthode est définie dansinitial/initial_alignment.hMoyenne.

Étapes4.1：solveGyroscopeBias Calculer le décalage gyroscopique , Le modèle de calcul de l'ensemble de la méthode est donné dans le document ,UtiliserLTLD Méthodes pour résoudre le problème des moindres carrés ,delta_bg = A.ldlt().solve(b);Ici.A +=tmp_A.transpose() * tmp_A,b += tmp_A.transpose() * tmp_b,En fait, c'est le traitement.AT*A*x=AT*bQuestions, Traitement direct du problème général des moindres carrés Ax=bC'est - à - direAx-b=0C'est tout.,Voici l'utilisationLDLTMéthodes, Les deux côtés sont multipliés par A La transposition de la matrice AT*AÇa doit être réversible, Donc vous pouvez multiplier les deux côtés directement par leur inverse , Les instructions correspondantes sont détaillées dans LDLTMéthodes. Après avoir obtenu le décalage gyroscopique, sauvegardez sa valeur dans Bgs[]Moyenne, Enfin, Nous recalculons une pré - intégrale .

Étapes4.2：LinearAlignment Échelle de calcul , Accélération et vitesse de gravité . La formule donnée dans cet article est un modèle de deux vitesses adjacentes , Mapping to the whole n+1 Parmi les modèles de vitesse ,A La matrice doit être positive et définie （Matrice symétrique réelle）,Défini dans le CodeAEtb C'est le plus grand HEtb,tmp_AEttmp_b Variable temporaire entre les vitesses adjacentes . La solution finale ：x = A.ldlt().solve(b);Puis appelezRefineGravity Recalculer la direction de l'accélération gravitationnelle ,Obtenir la solution optimale.