Ejecute VINS_Fusion

Código de comentario
(visualización, pose_grapf, pose_grapf_node)

Compilar:

    cd ~/catkin_ws/src
    git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
    cd ../
    catkin_make
    source ~/catkin_ws/devel/setup.bash

Ejecuta el código

   // 执行启动文件
    source ~/vins/devel/setup.bash
    roslaunch vins vins_rviz.launch
    // 开环
    source ~/vins/devel/setup.bash
    rosrun vins vins_node ~/vins/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 
    // 闭环优化
    source ~/vins/devel/setup.bash
    (optional) rosrun loop_fusion loop_fusion_node ~/vins/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 
    // 运行ROS bag
    source ~/vins/devel/setup.bash
    rosbag play /home/dyt/compare/MH_01_easy.bag

Ejecute VINS y espere generar datos de tipo tum para comparar utilizando las herramientas evo:

1. La función pubOdometry () en visualization.cpp

ofstream foutC(VINS_RESULT_PATH, ios::app);
        foutC.setf(ios::fixed, ios::floatfield);
        foutC.precision(0);
        foutC << header.stamp.toSec() * 1e9 << ",";
        foutC.precision(5);
        foutC << estimator.Ps[WINDOW_SIZE].x() << ","
              << estimator.Ps[WINDOW_SIZE].y() << ","
              << estimator.Ps[WINDOW_SIZE].z() << ","
              << tmp_Q.w() << ","
              << tmp_Q.x() << ","
              << tmp_Q.y() << ","
              << tmp_Q.z() << ","
              << estimator.Vs[WINDOW_SIZE].x() << ","
              << estimator.Vs[WINDOW_SIZE].y() << ","
              << estimator.Vs[WINDOW_SIZE].z() << "," << endl;
         write result to file

cambie a:

ofstream foutC(VINS_RESULT_PATH, ios::app);
        foutC.setf(ios::fixed, ios::floatfield);
        foutC.precision(0);
        foutC << header.stamp.toSec() << " ";
        foutC.precision(5);
        foutC << estimator.Ps[WINDOW_SIZE].x() << " "
              << estimator.Ps[WINDOW_SIZE].y() << " "
              << estimator.Ps[WINDOW_SIZE].z() << " "
              << tmp_Q.x() << " "
              << tmp_Q.y() << " "
              << tmp_Q.z() << " "
              << tmp_Q.w() << endl;

2. La función updatePath () en pose_graph.cpp

ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
            loop_path_file.setf(ios::fixed, ios::floatfield);
            loop_path_file.precision(0);
            loop_path_file << (*it)->time_stamp * 1e9 << ",";
            loop_path_file.precision(5);
            loop_path_file  << P.x() << ","
                  << P.y() << ","
                  << P.z() << ","
                  << Q.w() << ","
                  << Q.x() << ","
                  << Q.y() << ","
                  << Q.z() << ","
                  << endl;

cambie a:

ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
            loop_path_file.setf(ios::fixed, ios::floatfield);
            loop_path_file.precision(0);
            loop_path_file << (*it)->time_stamp << " ";
            loop_path_file.precision(5);
            loop_path_file  << P.x() << " "
                            << P.y() << " "
                            << P.z() << " "
                            << Q.x() << " "
                            << Q.y() << " "
                            << Q.z() << " "
                            << Q.w() << endl;

3. La función addKeyFrame () en el archivo pose_graph.cpp

ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
        loop_path_file.setf(ios::fixed, ios::floatfield);
        loop_path_file.precision(0);
        loop_path_file << cur_kf->time_stamp * 1e9 << ",";
        loop_path_file.precision(5);
        loop_path_file  << P.x() << ","
              << P.y() << ","
              << P.z() << ","
              << Q.w() << ","
              << Q.x() << ","
              << Q.y() << ","
              << Q.z() << ","
              << endl;

cambie a:

ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
        loop_path_file.setf(ios::fixed, ios::floatfield);
        loop_path_file.precision(0);
        loop_path_file << cur_kf->time_stamp << " ";
        loop_path_file.precision(5);
        loop_path_file  << P.x() << " "
                        << P.y() << " "
                        << P.z() << " "
                        << Q.x() << " "
                        << Q.y() << " "
                        << Q.z() << " "
                        << Q.w() << endl;

4. La función main () en pose_graph_node.cpp se
modifica a un archivo txt:

VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.txt";

Finalmente compila.

Problemas de encuentro:

Al ejecutar la trayectoria de salida de vins para comparar con el valor real, encontrará que algunos datos no se pueden alinear. Debe comprobar la precisión de la función de salida, ya que evo se alinea en función del tiempo. Cuando la precisión de la hora es baja, incluso cuando varios datos corresponden al mismo tiempo, la precisión de la alineación es deficiente.

Análisis de código:

1. KITTIGPSTest.cpp (conjunto de datos KITTI binocular + GPS)
lee los datos del GPS y los envía con el tema correspondiente para prepararse para la fusión global subsiguiente y realizar la fusión de datos

pubGPS = n. anunciar <sensor_msgs :: NavSatFix> ("/ gps", 1000);

2. KITTIOdomTest.cpp (binocular en el conjunto de datos KITTI)

3. rosNodeTest.cpp (conjunto de datos EUROC monocular + IMU)

Ejecute datos de KITTI a bordo:

Primero extrae la información de la IMU

roscore

source devel/setup.bash
roslaunch vins vins_rviz.launch

 
source  devel/setup.bash  
rosrun vins kitti /home/dyt/VINS/src/VINS-Fusion/config/kitti_raw/kitti_10_03_config1.yaml /home/dyt/2011_10_03/2011_10_03_drive_0042_sync/