Pour inspecter les connexions entre les nœuds après le lancement de VINS-Fusion, on peut exécuter la commande suivante :

rosrun rqt_graph rqt_graph

Configuration des caméras

Les fichiers de configuration des caméras incluent princiaplement la définition des topics ROS, les paramètres intrinsèques et extrinsèques, la matrice de transformation vers le repère monde, ainsi que les réglages pour le suivi de caractéristiques (nombre de points, distance minimale, etc.).

Outils d'évaluation

Les principaux outils pour évaluer les trajectoires sont evo et rpg_trajectory_evaluation.

Flux de données dans le nœud de test ROS

Les données de l'IMU sont traitées dans une fonction de rappel :

void traiter_imu(const sensor_msgs::ImuConstPtr &message_imu)
{
    double temps = message_imu->header.stamp.toSec();
    double ax = message_imu->linear_acceleration.x;
    double ay = message_imu->linear_acceleration.y;
    double az = message_imu->linear_acceleration.z;
    double gx = message_imu->angular_velocity.x;
    double gy = message_imu->angular_velocity.y;
    double gz = message_imu->angular_velocity.z;
    Eigen::Vector3d acceleration(ax, ay, az);
    Eigen::Vector3d vitesse_angulaire(gx, gy, gz);
    estimateur.ajouter_mesure_imu(temps, acceleration, vitesse_angulaire);
}

Pour inspecter les messages ROS publiés ou souscrits, on peut utiliser :

rosmsg show sensor_msgs/Imu
rosmsg show nav_msgs/Odometry

Modèle de caméra sténopé

Les paramètres intrinsèques décrivent la transformation entre le plan de l'image et les pixels (distance focale, échelle, translation). Les paramètres extrinsèques représentent la matrice de transformation entre le repère monde et le repère caméra.

Extraction et suivi de caractéristiques

Le flux de travail de la fonction trackImage est central. L'algorithme de suivi optique Lucas-Kanade est utilisé. Pour améliorer la robustesse, on effectue un suivi bidirectionnel : si la différence entre les positions calculées dans les deux sens dépasse 0.5 pixel, le point est rejeté.

Pour une distribution uniforme des caractéristiques, on peut ajuster le nombre de points à extraire selon la résolution de l'image.

Initialisation et géométrie épipolaire

La géométrie épipolaire permet d'estimer la rotation (R) et la translation (T) entre deux images. La triangulation permet de récupérer la profondeur des points observés. La méthode PnP (Perspective-n-Point) est utilisée pour estimer la pose à partir de correspondances 2D-3D.

Modélisation de l'IMU

L'accéléromètre fonctionne selon le principe du ressort de Hooke et la seconde loi de Newton. Le modèle simplifié de l'IMU nécessite une attention particulière au discrétisation et à la calibration, souvent réalisée via l'analyse de la variance d'Allan.

Optimisation avec Ceres Solver

L'optimisation dans VIO repose sur la minimisation d'un coût incluant plusieurs termes de résidus. Les variables à estimer comprennent les poses des caméras (position et orientation), les coordonnées des points dans le monde, les paramètres extrinsèques, l'échelle (profondeur inverse), et les décalages temporels.

Les étapes avec Ceres Solver consistent à définir les blocs de paramètres, les fonctions de résidus (avec calcul des Jacobien, automatique ou manuel), puis à ajouter ces éléments à l'objet Problem.

Termes de résidus visuels

Le résidu visuel est calculé comme la différence entre les coordonnées pixel observées et reprojetées. Pour un point 3D observé dans l'image de référence i et reprojeté dans l'image j, on applique les transformations suivantes : coordonnées pixel → coordonnées normalisées → repère caméra i → repère monde → repère caméra j → reprojection dans l'image j.

Termes de résidus inertiels

Le terme de résidu inerteil est basé sur la pré-intégration des mesures de l'IMU entre deux poses caméra.

Initialisation et détection de boucles

L'initialisation de la première pose peut être réalisée en estimant la direction de la gravité à partir des mesures de l'accéléromètre, en supposant un mouvement quasi-statique. La détection de boucles permet de corriger la dérive sur le long terme.