ROS-3DSLAM（十）lvi-sam源代码阅读8

2021SC@SDUSC

（十）lvi-sam源代码阅读8

写在前面

本周开始阅读lvi-sam项目中的视觉部分。我主要负责visual_loop，也就是回环检测的部分。前几周一直在阅读lidar部分mapOptmization.cpp，代码量较大所以多花了一些时间，其他队友已经开始了visual部分的进度。

mapOptmization部分还有一小部分没有阅读完，为了补齐跟队友的进度，所以先开始了visual部分的阅读，没有阅读完的代码将在之后穿插阅读。

作为铺垫，我在上周学习了回环检测的一些前置知识（mapOptmization中也有和回环检测相关的部分，但是感觉理解的不是很透彻，所以先阅读visual部分的回环检测代码，希望能从中获取一些灵感），对回环检测已经有了初步的了解。相关知识见上一篇博客分析的前置知识部分：https://blog.youkuaiyun.com/qq_38170211/article/details/121324281

visual_loop

整个文件夹粗略看起来有些杂乱，理不出什么头绪来。所以决定先从节点的入口开始分析，逐步树立整个节点的逻辑框架。

loop_detection_node.cpp

main函数

该文件内有main函数，是整个节点的入口。阅读完整个main函数，大致可以分为如下几个部分：

初始化ros节点

//首先，初始化ROS节点，虽然这里的初始化的时候，有vins这个名字，但是前面的blog也说了，这个名字是被launch文件中覆盖了。然后获取操作这个节点的句柄
ros::init(argc, argv, "vins");
ros::NodeHandle n;
ROS_INFO("\033[1;32m----> Visual Loop Detection Started.\033[0m");
//设置消息日志级别
//https://blog.youkuaiyun.com/qq_35508344/article/details/77720249
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn);

加载参数

// Load params
//加载相关的参数
std::string config_file;
/*
    在参数服务器中找到参数vins_config_file，将value（一个路径）赋给config_file
        <!-- VINS config -->
        <param name="vins_config_file" type="string" value="$(find lvi_sam)/config/params_camera.yaml" />  
*/
n.getParam("vins_config_file", config_file);
//根据路径名打开文件（read模式）
cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
if(!fsSettings.isOpened())
{
    std::cerr << "ERROR: Wrong path to settings" << std::endl;
}
usleep(100);

// Initialize global params 初始化全局参数
// 与闭环相关的参数
fsSettings["project_name"] >> PROJECT_NAME;  
fsSettings["image_topic"]  >> IMAGE_TOPIC;  
fsSettings["loop_closure"] >> LOOP_CLOSURE;
fsSettings["skip_time"]    >> SKIP_TIME;
fsSettings["skip_dist"]    >> SKIP_DIST;
fsSettings["debug_image"]  >> DEBUG_IMAGE;
fsSettings["match_image_scale"] >> MATCH_IMAGE_SCALE;

//LOOP_CLOSURE 值为1时代表启动闭环检测（默认为1）
if (LOOP_CLOSURE)
{
    //ros中用于获取某个功能包的绝对路径，当功能包不存在时，该函数返回一个空的字符串。
    //这里是返回lvi-sam功能包（也就是本项目）的绝对路径
    string pkg_path = ros::package::getPath(PROJECT_NAME);

    // initialize vocabulary
    /*
        vocabulary_file: "/config/brief_k10L6.bin"
        brief_pattern_file: "/config/brief_pattern.yml"
    */
    string vocabulary_file;
    fsSettings["vocabulary_file"] >> vocabulary_file;  
    vocabulary_file = pkg_path + vocabulary_file;
    // 需要分析 词袋模型
    loopDetector.loadVocabulary(vocabulary_file);

    // initialize brief extractor
    string brief_pattern_file;
    fsSettings["brief_pattern_file"] >> brief_pattern_file;  
    brief_pattern_file = pkg_path + brief_pattern_file;
    //需要分析
    briefExtractor = BriefExtractor(brief_pattern_file);

    // initialize camera model
    //camera model 需要分析
    m_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(config_file.c_str());
}

vins_config_file

run.launch -> module_sam.launch -> params_camera.yaml

阅读params_camera.yaml文件，里面存储了一系列的参数，例如imu的噪声默认值等等。（类比lidar中，utility.h通过params_lidar.yaml进行参数配置）

%YAML:1.0

# Project
project_name: "lvi_sam"

#common parameters
imu_topic: "/imu_raw"
image_topic: "/camera/image_raw"
point_cloud_topic: "lvi_sam/lidar/deskew/cloud_deskewed"

# Lidar Params
use_lidar: 1                     # whether use depth info from lidar or not
lidar_skip: 3                    # skip this amount of scans
align_camera_lidar_estimation: 1 # align camera and lidar estimation for visualization

# lidar to camera extrinsic
lidar_to_cam_tx: 0.05
lidar_to_cam_ty: -0.07
lidar_to_cam_tz: -0.07
lidar_to_cam_rx: 0.0
lidar_to_cam_ry: 0.0
lidar_to_cam_rz: -0.04

# camera model
model_type: MEI
camera_name: camera

# Mono camera config
image_width: 720
image_height: 540
mirror_parameters:
   xi: 1.9926618269451453
distortion_parameters:
   k1: -0.0399258932468764
   k2: 0.15160828121223818
   p1: 0.00017756967825777937
   p2: -0.0011531239076798612
projection_parameters:
   gamma1: 669.8940458885896
   gamma2: 669.1450614220616
   u0: 377.9459252967363
   v0: 279.63655686698144
fisheye_mask: "/config/fisheye_mask_720x540.jpg"

#imu parameters       The more accurate parameters you provide, the worse performance
acc_n: 0.02         # accelerometer measurement noise standard deviation.
gyr_n: 0.01         # gyroscope measurement noise standard deviation.
acc_w: 0.002        # accelerometer bias random work noise standard deviation.
gyr_w: 4.0e-5       # gyroscope bias random work noise standard deviation.
g_norm: 9.805       #

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 
#Rotation from camera frame to imu frame, imu^R_cam
extrinsicRotation: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 0, 0, -1, 
           -1, 0, 0, 
           0, 1, 0]

#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [0.006422381632411965, 0.019939800449065116, 0.03364235163589248]

#feature traker paprameters
max_cnt: 150            # max feature number in feature tracking
min_dist: 20            # min distance between two features 
freq: 20                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 
F_threshold: 1.0        # ransac threshold (pixel)
show_track: 1           # publish tracking image as topic
equalize: 1             # if image is too dark or light, trun on equalize to find enough features
fisheye: 1              # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points

#optimization parameters
max_solver_time: 0.035   # max solver itration time (ms), to guarantee real time
max_num_iterations: 10   # max solver itrations, to guarantee real time
keyframe_parallax: 10.0  # keyframe selection threshold (pixel)

#unsynchronization parameters
estimate_td: 0           # online estimate time offset between camera and imu
td: 0                    # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

#rolling shutter parameters
rolling_shutter: 0       # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0    # unit: s. rolling shutter read out time per frame (from data sheet). 

#loop closure parameters
loop_closure: 1                    # start loop closure
skip_time: 0.0
skip_dist: 0.0
debug_image: 0                      # save raw image in loop detector for visualization prupose; you can close this function by setting 0 
match_image_scale: 0.5
vocabulary_file: "/config/brief_k10L6.bin"
brief_pattern_file: "/config/brief_pattern.yml"

cv::FileStorage

https://www.cnblogs.com/feifanrensheng/p/9132209.html

OpenCV的许多应用都需要使用数据的存储于读取，OpenCV通过XML/YAML格式实现数据持久化。

构造函数：cv::FileStorage(const string& source, int flags， const string& encoding=string());

参数：

**source –**存储或读取数据的文件名（字符串），其扩展名(.xml 或 .yml/.yaml)决定文件格式。

flags – 操作模式，包括：

• FileStorage::READ 打开文件进行读操作
• FileStorage::WRITE 打开文件进行写操作
• FileStorage::APPEND打开文件进行附加操作
• FileStorage::MEMORY 从source读数据，或向内部缓存写入数据（由FileStorage::release返回）

loopDetector.loadVocabulary

loopDetector是文件中定义的一个成员变量：LoopDetector loopDetector;

main函数加载参数时涉及到了对loop_detection.h文件中的定义的类的方法：LoopDetector::loadVocabulary、loopDetector.loadVocabulary(vocabulary_file)的调用。vocabulary_file中存储了一个.bin文件的路径（暂时认为是一个提前得到的字典）

根据函数的字面意思，初步认为该函数与回环检测中的词袋模型中加载字典有关。

loop_detection.h

BriefVocabulary* voc;

void loadVocabulary(std::string voc_path);

成员变量 voc，认为是针对该项目的字典。

loop_detection.cpp

void LoopDetector::loadVocabulary(std::string voc_path)
{
    voc = new BriefVocabulary(voc_path);
    db.setVocabulary(*voc, false, 0);
}

该函数的目的是将存储在文件中的字典加载到db中。

BriefExtractor

成员变量：BriefExtractor briefExtractor;

parameters.h

class BriefExtractor
{
public:

    DVision::BRIEF m_brief;

    virtual void operator()(const cv::Mat &im, vector<cv::KeyPoint> &keys, vector<DVision::BRIEF::bitset> &descriptors) const
    {
        m_brief.compute(im, keys, descriptors);
    }

    BriefExtractor(){};

    BriefExtractor(const std::string &pattern_file)
    {
        // The DVision::BRIEF extractor computes a random pattern by default when
        // the object is created.
        // We load the pattern that we used to build the vocabulary, to make
        // the descriptors compatible with the predefined vocabulary
		// 默认情况下，当创建对象时，DVision:：BRIEF提取器会计算一个随机模式。我们加载用于构建词汇表的模式，以使描述符与预定义词汇表兼容
        // loads the pattern
        cv::FileStorage fs(pattern_file.c_str(), cv::FileStorage::READ);
        if(!fs.isOpened()) throw string("Could not open file ") + pattern_file;

        vector<int> x1, y1, x2, y2;
        fs["x1"] >> x1;
        fs["x2"] >> x2;
        fs["y1"] >> y1;
        fs["y2"] >> y2;

        m_brief.importPairs(x1, y1, x2, y2);
    }
};

该类也与词袋模型有关

• camodocal::CameraFactory
  • 暂时不做分析

订阅、发布话题

//订阅话题
//原始的图像信息
ros::Subscriber sub_image     = n.subscribe(IMAGE_TOPIC, 30, image_callback);
//以下三个话题在visual_estimator/utility/visualization.cpp中发布的话题 需要分析
ros::Subscriber sub_pose      = n.subscribe(PROJECT_NAME + "/vins/odometry/keyframe_pose",  3, pose_callback);
ros::Subscriber sub_point     = n.subscribe(PROJECT_NAME + "/vins/odometry/keyframe_point", 3, point_callback);
ros::Subscriber sub_extrinsic = n.subscribe(PROJECT_NAME + "/vins/odometry/extrinsic",      3, extrinsic_callback);

//发布话题
pub_match_img = n.advertise<sensor_msgs::Image>             (PROJECT_NAME + "/vins/loop/match_image", 3);
pub_match_msg = n.advertise<std_msgs::Float64MultiArray>    (PROJECT_NAME + "/vins/loop/match_frame", 3);
pub_key_pose  = n.advertise<visualization_msgs::MarkerArray>(PROJECT_NAME + "/vins/loop/keyframe_pose", 3);

if (!LOOP_CLOSURE)
{
    //停止订阅和发布话题
    sub_image.shutdown();
    sub_pose.shutdown();
    sub_point.shutdown();
    sub_extrinsic.shutdown();

    pub_match_img.shutdown();
    pub_match_msg.shutdown();
    pub_key_pose.shutdown();
}

visualization.cpp中发布的话题

本节点中订阅的3个话题都来自于visual_estimator/utility/visualization.cpp中。

void registerPub(ros::NodeHandle &n)
{
    /*
    	pub camera pose
    	消息类型为：nav_msgs::Odometry 包含一个三维点坐标和一个旋转四元数
    */
    pub_keyframe_pose       = n.advertise<nav_msgs::Odometry>               (PROJECT_NAME + "/vins/odometry/keyframe_pose", 1000);
    /*
    	pub 2D-3D points of keyframe 
    	消息类型为：sensor_msgs::PointCloud 包含 3d 点的集合，以及有关每个点的可选附加信息
    	初步认为是相机在该位姿下拍摄的点云图
    */
    pub_keyframe_point      = n.advertise<sensor_msgs::PointCloud>          (PROJECT_NAME + "/vins/odometry/keyframe_point", 1000);
    /*
    	大体阅读了该话题的发布函数，认为该话题发布的消息是：相机从相机坐标系转换到世界坐标系后的位姿信息
    */
    pub_extrinsic           = n.advertise<nav_msgs::Odometry>               (PROJECT_NAME + "/vins/odometry/extrinsic", 1000);
	.
	.
	.
}

多线程调用函数process

//开启一个新的线程，调用process函数
std::thread measurement_process;
//需要分析
measurement_process = std::thread(process);

接下来两个方向

• vins
• 工具类