13-[LVI-SAM]visual

13-[LVI-SAM]visual

2021SC@SDUSC

visual_odometry_初探

​ 上一章已经把visual_feature的大部分代码和功能，虽然没有完全把那一个节点讲明白，但是已经大致清楚整一份代码的思路。至于更多的细枝末节的算法思想，这些涉及比较多的数学知识，脱离了写博客的目的，就不再赘述了，留下了链接，等有空可以自己去探索。

​ 而我们团队其他人还没有完成自己节点部分的分析，于是，经过商量，我来帮另外一个@同学分析他的部分。由于他还在分析initial文件夹下的内容，于是，我从这个节点本身出发，开始分析，避免分析冲突。

文章目录

• visual_odometry_初探
• • 1. 概览
  • 2. 输入输出
  • • a. 输入
    • b. 输出
  • 3. Main函数
  • • a. readParameters()
    • b. estimator.setParameter()
    • c. registerPub()
    • d. odomRegister类
  • 附录：引用

1. 概览

​ 这个节点其实就是通过视觉特征点输出里程计了，与激光有互动的地方主要体现在两点，一是带有激光雷达深度图获取到深度的视觉特征点，二是把之前激光IMU参与的IMUPreIntegration模块输出的odometry结果作为初始化的值。

​ visual_odometry模块其实就是下面的lvi_sam_visual_odometry节点。但在源代码中，这个模块的名字其实是visual_estimator。在图中，可以看到，这个节点有着众多的输入和输出。可以说，是LVI-SAM的视觉核心模块。其中，有和前面的lidar模块中的lvi_sam_mapOptmization和lvi_sam_PreIntegration，以及视觉的其他模块相关联。

​ 这里可以再穿插一个小知识，是我在看这个节点收获到的和ROS相关的知识。有人可能很好奇，为什么节点源代码的文件名和这个图中实际运行时的节点名不一致？在前面blog的源码分析体会中，我写过是因为launch中定义了节点的名字，但是如果看过launch文件可以知道（见下文），它并没有明确指定节点存放在哪个位置。只有在type参数的时候写明了节点。其实，这是在编译的时候就已经指明了节点的type，也可以说是节点的唯一名字。编译的时候，CMakeLists已经列好了编译的源代码文件位置和节点名称（见下文）。

launch文件：

CMakeLists 文件（节选）：

file(GLOB visual_odometry_files
    "src/visual_odometry/visual_estimator
    ESTIMATE_EXTRINSIC = fsSettings["estimate_extrinsic"];
    if (ESTIMATE_EXTRINSIC == 2) // 不提供
    {
        ROS_INFO("have no prior about extrinsic param, calibrate extrinsic param");
        RIC.push_back(Eigen::Matrix3d::Identity());
        TIC.push_back(Eigen::Vector3d::Zero());
        EX_CALIB_RESULT_PATH = pkg_path + "/config/extrinsic_parameter.csv";

    }
    else 
    {
        if ( ESTIMATE_EXTRINSIC == 1) // 不准确
        {
            ROS_INFO(" Optimize extrinsic param around initial guess!");
            EX_CALIB_RESULT_PATH = pkg_path + "/config/extrinsic_parameter.csv";
        }
        if (ESTIMATE_EXTRINSIC == 0) // 准确
            ROS_INFO(" Fix extrinsic param.");

        
        cv::Mat cv_R, cv_T;
        fsSettings["extrinsicRotation"] >> cv_R;
        fsSettings["extrinsicTranslation"] >> cv_T;
        Eigen::Matrix3d eigen_R;
        Eigen::Vector3d eigen_T;
        cv::cv2eigen(cv_R, eigen_R);
        cv::cv2eigen(cv_T, eigen_T);
        Eigen::Quaterniond Q(eigen_R);
        eigen_R = Q.normalized(); // 归一化
        RIC.push_back(eigen_R);
        TIC.push_back(eigen_T);
        ROS_INFO_STREAM("Extrinsic_R : " << std::endl << RIC[0]);
        ROS_INFO_STREAM("Extrinsic_T : " << std::endl << TIC[0].transpose());
        
    } 

    INIT_DEPTH = 5.0;
    BIAS_ACC_THRESHOLD = 0.1;
    BIAS_GYR_THRESHOLD = 0.1;

    
    TD = fsSettings["td"];
    ESTIMATE_TD = fsSettings["estimate_td"];
    if (ESTIMATE_TD)
        ROS_INFO_STREAM("Unsynchronized sensors, online estimate time offset, initial td: " << TD);
    else
        ROS_INFO_STREAM("Synchronized sensors, fix time offset: " << TD);

    ROLLING_SHUTTER = fsSettings["rolling_shutter"];
    if (ROLLING_SHUTTER)
    {
        TR = fsSettings["rolling_shutter_tr"];
        ROS_INFO_STREAM("rolling shutter camera, read out time per line: " << TR);
    }
    else
    {
        TR = 0;
    }
    
    fsSettings.release();
    usleep(100);
}

b. estimator.setParameter()

​ 这里其中一个是设置计时器的参数。另外是，设置视觉测量残差的协方差矩阵

void Estimator::setParameter()
{
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        tic[i] = TIC[i];
        ric[i] = RIC[i];
    }
    
    f_manager.setRic(ric);
    ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    td = TD;
}

c. registerPub()

void registerPub(ros::NodeHandle &n)
{
    
    pub_latest_odometry     = n.advertise               (PROJECT_NAME + "/vins/odometry/imu_propagate", 1000);
    pub_latest_odometry_ros = n.advertise               (PROJECT_NAME + "/vins/odometry/imu_propagate_ros", 1000);
    pub_path                = n.advertise                   (PROJECT_NAME + "/vins/odometry/path", 1000);
    pub_odometry            = n.advertise               (PROJECT_NAME + "/vins/odometry/odometry", 1000);
    pub_point_cloud         = n.advertise          (PROJECT_NAME + "/vins/odometry/point_cloud", 1000);
    pub_margin_cloud        = n.advertise          (PROJECT_NAME + "/vins/odometry/history_cloud", 1000);
    pub_key_poses           = n.advertise       (PROJECT_NAME + "/vins/odometry/key_poses", 1000);
    pub_camera_pose         = n.advertise               (PROJECT_NAME + "/vins/odometry/camera_pose", 1000);
    pub_camera_pose_visual  = n.advertise  (PROJECT_NAME + "/vins/odometry/camera_pose_visual", 1000);
    pub_keyframe_pose       = n.advertise               (PROJECT_NAME + "/vins/odometry/keyframe_pose", 1000);
    pub_keyframe_point      = n.advertise          (PROJECT_NAME + "/vins/odometry/keyframe_point", 1000);
    pub_extrinsic           = n.advertise               (PROJECT_NAME + "/vins/odometry/extrinsic", 1000);

    
    cameraposevisual.setScale(1);
    cameraposevisual.setLineWidth(0.05);
    
    keyframebasevisual.setScale(0.1);
    keyframebasevisual.setLineWidth(0.01);
}

d. odomRegister类

​ 这个类可以把里程计信息从lidar帧的坐标系转到VINS视觉图像帧的坐标系，在initial的文件夹下。这个类原本是可以是发布相关的信息的，但是，作者把相关的东西注释掉了，估计是写程序时的调试信息。因此，这个类其实并不需要把NodeHandle传进去作为参数，只剩下坐标系转换的功能了。这里的内容，也可以看看另外一个同学的blog。

附录：引用

LVI-SAM代码– xuwuzhou的SLAM之路 – 静心，慎思，明辨，笃学

ManiiXu/VINS-Mono-Learning: VINS-Mono