2021SC@SDUSC

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

写在前面

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

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

作为铺垫，我在上周学习了回环检测的一些前置知识（mapOptmization中也有和回环检测相关的部分，但是感觉理解的不是很透彻，所以先阅读visual部分的回环检测代码，希望能从中获取一些灵感），对回环检测已经有了初步的了解。相关知识见上一篇博客分析的前置知识部分：https://blog.csdn.net/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.csdn.net/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 extractorstring 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.0399258932468764k2: 0.15160828121223818p1: 0.00017756967825777937p2: -0.0011531239076798612
projection_parameters:gamma1: 669.8940458885896gamma2: 669.1450614220616u0: 377.9459252967363v0: 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-matrixrows: 3cols: 3dt: ddata: [ 0, 0, -1, -1, 0, 0, 0, 1, 0]#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrixrows: 3cols: 1dt: ddata: [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 patterncv::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
工具类

ROS-3DSLAM（十）lvi-sam源代码阅读8相关推荐

ROS源代码阅读（9）——DWA算法
2021SC@SDUSC ROS源代码阅读(9) 后面主要看dwa算法 DWAPlannerROS是封装类,提供了与move_base的接口,而DWAPlanner是具体实现类,它非常依赖costma ...
XV6源代码阅读-中断与系统调用
XV6源代码阅读-中断与系统调用 Exercise1 源代码阅读 1.启动部分: bootasm.S bootmain.c 和xv6初始化模块:main.c bootasm.S 由16位和32位汇编混 ...
非常好！！！Linux源代码阅读——环境准备【转】
Linux源代码阅读--环境准备转自:http://home.ustc.edu.cn/~boj/courses/linux_kernel/0_prepare.html 目录 Linux 系统环境准备 ...
Tomcat源代码阅读系列之八：Tomcat 设计模式总结
本篇我们将来分析一下Tomcat中所涉及到设计模式,本文我们将主要来分析外观模式 , 观察者模式 , 责任链模式 , 模板方法模式 , 命令模式 . 在开始本文之前,笔者先说明一下对于设计模式的一点 ...
Linux 源代码阅读知识点及要求
说明:1.本次源代码阅读,以Linux 最新的稳定版本(2.6)为主: 2.源代码下载地址: 在官方站点 www.kernel.org 上最新稳定版本是 2.6.13.2: 在清华的 ftp 上随时都 ...
【转】Tomcat总体结构（Tomcat源代码阅读系列之二）
本文是Tomcat源代码阅读系列的第二篇文章,我们在本系列的第一篇文章:在IntelliJ IDEA 和 Eclipse运行tomcat 7源代码一文中介绍了如何在intelliJ IDEA 和 Ec ...
Linux 下源代码阅读工具 —— vim + TagList + CTags
为什么不采用 Windows 下较为著名的源代码阅读软件 SourceInsight, 其一,其在 Linux 下的安装较为繁琐: 其二,切换代码时背景色的变化会为人的眼部产生极为不舒服的感觉: 其三 ...
XV6源代码阅读-虚拟内存管理
Exercise1 源代码阅读内存管理部分: kalloc.c vm.c 以及相关其他文件代码 kalloc.c:char * kalloc(void)负责在需要的时候为用户空间.内核栈.页表页以及 ...
ubuntu上安装源代码阅读工具Source-Navigator
在ubuntu中,一款相对比较方便的源代码阅读编辑工具Source-Navigator . 1,下载 http://sourcenav.sourceforge.net/download.html 下载 ...

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