Partie 2 : Optimisation de pose
Une fois nos features détectées et associées, on va chercher à estimer le mouvement effectué entre les deux observations.
Après la Partie 1 qui concernait la détection et association de features, on s’intéresse ici à l’estimation de notre position et orientation dans le temps.
Choisir le modèle de pose à optimiser
Une pose est la position et l’orientation de notre robot dans l’espace. On peut l’exprimer de la manière et dans le repère qui nous arrange le plus (j’ai déjà vu du sphérique, ici, on préfèrera le cartésien).
La position de notre observateur sera un vecteur 3, avec l’origine centrée sur le point de départ de notre robot. L’orientation est souvent représentée par une matrice de rotation, ou un quaternion. Je préfère utiliser les quaternions, plus faciles à manipuler que les matrices de rotation.
En vérité, le choix de notre modèle de pose dépendra surtout de la technique de régression qu’on souhaite appliquer.
On remarquera cependant que l’utilisation d’un quaternion ou d’une matrice de rotation “surparamétrisée” le problème : on se retrouve avec \(N \geq 4\) variables pour définir une orientation a 3 degrés de liberté sur l’axe \(x\), \(y\) et \(z\) en Cartésien). On reviendra sur ce point plus tard, mais il est fondamental.
La représentation
En effet cette représentation n’est pas surparamétrisée, mais a son propre lot de problèmes. Ajouter les rotations successives n’est pas vraiment trivial, et chaque composante est définie sur un domaine dépendant d’un nombre non rationnel (PI).
Il se trouve qu’une matrice de rotation est déjà une représentation de rotation utilisant les angles sur X Y et Z, mais sous une forme bien plus utile : voir ce lien pour s’instruire sur le sujet !
Optimisation de pose
On va chercher ici à estimer la transformation entre nos deux sets de features associées, c’est-à-dire la rotation et transformation à appliquer à chaque features du set 2 pour retrouver le set 1. À ma connaissance, les résultats peuvent être obtenus sous la forme d’une pose relative au set 1, ou d’une pose absolue (par rapport au point 0 du monde).
Généralement, optimiser une pose absolue possède moins d’applications que de récupérer une série de poses relatives, et est plus lourd et moins précis (localement) qu’une pose relative. On renverra à l’utilisateur la pose absolu.
Idéalement notre map devrait être définie par rapport aux poses relatives, afin d’optimiser la position des éléments de notre map quand on effectue une fermeture de boucle.
Méthode d’optimisation
L’optimisation va se baser sur la transformation entre deux sets de features associés pour estimer les paramètres du mouvement. Dans la littérature, on observe plusieurs techniques, la plupart utilisant des régressions linéaires comme la descente de gradient, Newton-Gauss, le Bundle Adjustment, …
Une de mes favorites est la représentation du problème sous forme de graph: On considère la pose de notre observer comme un point d’un graph relié à la pose précédente. Chaque pose est reliée aux features associées, également reliées entre elles. Certains liens sont marqués “elastiques”, c’est-à-dire optimisables. Les liens entre les features sont toujours considérés rigides, les optimiser n’a pas vraiment d’utilité. Si on note tous les liens, à part ceux entre les poses et leurs features, comme étant élastiques, on obtient après optimisation la pose finale. On peut aussi noter les liens entre les poses et les features comme étant élastiques: l’optimisation sera plus longue, mais on en sortira aussi une erreur associée à chaque feature, ce qui peut être utile en construisant la map. Cet algorithme est implémenté dans g2o sous le nom SparseOptimizer (voir un exemple d’utilisation dans Light Visual Tracking (LVT), une méthode d’odomètre visuelle performante dans les environnements avec beaucoup de features).
J’ai également pu lire un papier proposant une méthode optimisant d’abord la rotation de la pose, et utilisant cette rotation comme constante de l’optimisation par bundle adjustment de la position (voir RGB-D SLAM with Structural Regularities (2021))
Le bundle adjustment consiste à effectuer une série d’optimisations en ajustant entre chaque itération les paramètres finaux. Dans le cas d’un SLAM basé sur des surfels (petites surfaces autour d’un point feature), les surfels faisant partie du même “objet” peuvent être fusionnés entre chaque étape de l’optimisation (voir BAD: Bundle Adjusted DirectRGBD-SLAM (2019) pour la méthode complète).
Afin d’obtenir la pose relative de mon robot, j’utilise l’algorithme de Levenberg-Marquardt, implémenté avec la librairie Eigen. C’est un algorithme absolument formidable, basé sur une optimisation de Newton-Gauss pour converger rapidement, suivie d’une descente de gradient pour l’ajustement final, plus précis. Il a cependant ses limites : il est bien meilleur pou optimiser des problèmes linéaires (ce qu’une estimation de pose n’est pas), et nécessite une estimation de la solution de base pour converger suffisamment vite (à mon goût).
Fonctionnellement, l’algorithme appellera une fonction d’optimisation avec des paramètres (ici, position et rotation) variés au fur et à mesure de l’optimisation, et nous renverrons un vecteur de taille \(N\), contenant l’ensemble des erreurs.
Estimation des paramètres initiaux
Pour fournir une estimation assez proche de la réalité en entrée de l’algorithme, j’utilise un modèle à vélocité constante, basée sur les dernières estimations de pose pour estimer la prochaine. Ce modèle de vélocité est approximatif, mais suffit à se rapprocher de la solution pour économiser quelques itérations.
Je n’ai pas encore trouvé de meilleure méthode.
Linéarisation du quaternion
Pour la question de la linéarité du problème, c’est un peu plus complexe. On a 7 paramètres à optimiser: les 3 de la position, déjà linéaire, et les 4 paramètres de notre quaternion. Ici, c’est le quaternion qui pose problème: ses paramètres sont entre 0 et 1, et leur transformation n’est absolument pas linéaire.
Il existe plusieurs techniques pour contourner ce problème de linéarisation. La plus facile à appliquer est surement celle-ci: On va exprimer le paramètre \(w\) du quaternion en fonction des trois autres (\(x\), \(y\), \(z\)). Notre problème n’est plus surparamètrisé, et la représentation finale est plus facilement optimisable. $$ k_w = \sqrt{1 - \frac{x^2 + y^2 + z^2}{4}}, k_x = \frac{x}{2}, k_y = \frac{y}{2}, k_z = \frac{z}{2} $$ On optimisera les paramètres \(x\), \(y\) et \(z\).
Cette représentation, même si elle est correctement paramétrisée, n’est pas encore linéaire. J’ai lu certaines publications qui avançaient qu’elle n’était valable que pour les petites variations de rotation, mais je ne peux pas confirmer ni infirmer cette proposition (voir Robust RGB-D Odometry Under Depth Uncertainty for Structured Environments)
On peut aussi citer les transformations exponentielles du quaternion, et bien d’autres (voir Quaternion Kinematics for the Error-State Kalman Filter (2017) qui recense la plupart des transformations que j’ai pu voir).
Pour ma part, je m’oriente vers la méthode décrite dans Using Quaternions for Parametrizing 3D Rotations in Unconstrained Non Linear Optimisation (2001). Cette méthode me semble plus intuitive que celles présentées précédemment: Plutôt qu’optimiser les paramètres du quaternion pour en trouver un autre, on va optimiser les déplacements à effectuer sur l’hyperplan tangent à la sphère unitaire contenant notre quaternion de départ, pour obtenir notre quaternion final. Ces déplacements sont donc linéaires, et facilement optimisables. J’émets tout de même certaines réserves concernant l’optimisation de rotations éloignées les unes des autres. La distance sur le plan tangent est à mon avis limitant dans le cas d’une rotation importante.
Formulation mathématique simplifié
Prenons notre quaternion initial \(k_0 = (k_w, k_x, k_y, k_z) \).
On va calculer une base de l’hyperplan \(M\) a partir de \(k_0\): $$ M = \begin{pmatrix} - \frac{k_x}{k_w} & -\frac{k_y}{k_w} & -\frac{k_z}{k_w} \\ 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{pmatrix} $$ Les colonnes de \(M\) représentent une partie des paramètres de l’hyperplan.
On récupère ensuite la matrice \(U\), la matrice de décomposition en valeurs propres de \(M\): $$ M = U \sum{} V^* $$
Nous allons maintenant choisir un vecteur \(v\) a trois dimensions, qui sera l’entrée de notre paramétrisation. \(U\) sera une matrice \(4 * 3\). On peut obtenir un quaternion transformé en effectuant l’opération $$ v_4 = U v $$ $$ k_f = \sin{(\lVert v_4 \rVert)} * v_4 + \cos{(\lVert v_4 \rVert)} * k_0 $$ On peut remarquer que fixer toutes les composantes de \(v\) a 0 produira le quaternion \(k_0\).
Durant l’optimisation, nous avons simplement à transformer v en quaternion \(k_f\), et utiliser ce quaternion pour notre optimisation.
Scoring
Pour estimer la qualité de l’estimation de notre pose, nous devons mettre en place une série de métriques, appliqué à chaque paire de points associés. Notre fonction d’optimisation renvoi un vecteur de taille N, avec N le nombre de features associées. Pour que l’optimisation soit terminée, les valeurs de notre vecteur doivent se rapprocher le plus possible de 0.
On notera \(P_W\) notre point 3D dans l’espace, et \(P_A\) sa feature 2D associée.
Projection du point de référence en repère caméra
On va commencer par transformer notre vecteur \(v\) en quaternion avec la formule vue précédemment. On calcule ensuite la matrice de rotation \(R_w\) à partir de notre quaternion \(k_f\), et on transforme cette matrice en matrice de transformation world to screen, à partir de la position \(T_W\): $$ T_s = -R_w^t * T_w $$ $$ TR_s = (R_w, T_S) $$ On va ensuite projeter notre point de référence sur l’écran, en utilisant la matrice de transformation \(RT_s\) (Matrice 4x3), qui transforme les points depuis des coordonnées 3D vers une projection 3D en espace caméra \(P_c\). $$ P_c = TR_s * P_W; $$ On notera \(d\) la composante 3 de \(P_c\). $$ P_s = \frac{c_f * P_c}{d} + c_c $$ Ou \(c_f\) et \(c_c\) sont des vecteurs 2D représentants respectivement la distance focale et la distance de ce point focal par rapport au centre de la caméra (paramètres intrinsèques à la caméra utilisée).
Notre point 2D \(P_s\) est la projection de notre point initial \(P_W\) dans le repère caméra.
Comparaison des deux features associées
On peut ensuite comparer la distance entre \(P_s\) et \(P_A\), avec un calcul de distance. L’échelle de notre variable en sortie n’a pas vraiment d’importance.
J’ai commencé par utiliser la distance de Manhattan, qui fourni des résultats rapides et plus que satisfaisants. Certains papiers proposent d’utiliser la distance de Mahalanobis, bien plus robuste aux distributions variées de points, mais plus lourde à calculer.
Pondération des distances
Notre étape d’optimisation pourrait s’arrêter là, mais on peut ajouter une pondération pour éliminer encore plus l’influence des outliers. On pourra utiliser un estimateur comme Huber, Cauchy, SC-DCS, ou un spécial celui comme présenté dans At All Costs: A Comparison of Robust Cost Functions for Camera Correspondence Outliers (2015) par exemple.
On pourra également relancer l’optimisation en enlevant les points considérés comme outlier pendant la première optimisation. Le traitement sera plus lourd, mais l’estimation finale sera bien meilleure.