VINS-Mono 笔记

常用操作

Clone the repository and catkin_make

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

Run VINS-Mono with EuRoC bag.

Open three terminals, launch the vins_estimator , rviz and play the bag file respectively. Take MH_01 for example

roslaunch vins_estimator euroc.launch 
roslaunch vins_estimator vins_rviz.launch
rosbag play ~/dateset/MH_01_easy.bag 

与 Ground Truth 对比

roslaunch benchmark_publisher publish.launch sequence_name:=MH_01_easy

保存 pose graph

如果需要保存地图，需要设置config文件/VINS-Mono/config/euroc/euroc_config.yaml中的pose_graph_save_path（地图保存地址）为地图保存文件夹（默认的是作者自己的路径）。

程序运行完之后，在运行roslaunch vins_estimator euroc.launch的窗口输入s后回车，即可完成保存。

pose_graph_save_path: "/home/melodic/output/vins-mono_pose_graph/" # save and load path

加载地图

在地图保存的config文件中设置load_previous_pose_graph参数为1,则再次运行roslaunch vins_estimator euroc.launch时会加载之前保存的地图。

总览

系统框架

系统主要包括五个部分：数据预处理、初始化、后端非线性优化、闭环检测、位姿图优化。

• A、数据预处理(前端)
  IMU预积分、Harris角点提取和跟踪
• B、初始化
  手眼标定法获得外参旋转量、纯视觉SfM、Visual-Inertial对齐(重力方向、速度、尺度等)
• C、紧耦合单目VIO
  IMU误差、重投影误差、先验误差、滑窗优化、关键帧获取IMU测量残差、视觉测量残差、边缘化、纯运动视觉惯性BA、IMU前向传递以达到IMU速率状态估计、故障检测与恢复、
• D、重定位
  Fast特征提取 BRIEF描述子、DBoW2词袋
• E、4自由度全局位姿图优化

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-QUvr6GXN-1588600985321)(00_系统框架.assets/640.png)]

论文框架

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-OycBxPSm-1588600985323)(00_系统框架.assets/640.jpg)]

代码结构

代码中一共有6个main()函数:

ar_demo/src/ar_demo_node.cpp
benchmark_publisher/src/benchmark_publisher_node.cpp
camera_model/src/intrinsic_calib.cc
feature_tracker/src/feature_tracker_node.cpp
pose_graph/src/pose_graph_node.cpp
vins_estimator/src/estimator_node.cpp

代码文件目录

vins_estimator/src
factor：
imu_factor.h			// IMU残差、雅可比
intergration_base.h		 // IMU预积分
marginalization.cpp/.h	 // 边缘化
pose_local_parameterization.cpp/.h	 // 局部参数化
projection_factor.cpp/.h			// 视觉残差
initial：
initial_alignment.cpp/.h		// 视觉IMU校准(陀螺仪偏置、尺度、重力加速度和速度)
initial_ex_rotation.cpp/.h		// 相机和IMU外参标定
initial_sfm.cpp/.h				// 纯视觉SFM、三角化、PNP
solve_5pts.cpp/.h				// 5点法求基本矩阵得到Rt
utility：
CameraPoseVisualization.cpp/.h	// 相机位姿可视化
tic_toc.h				// 记录时间
utility.cpp/.h			// 各种四元数、矩阵转换
visualization.cpp/.h	// 各种数据发布
estimator.cpp/.h		// 紧耦合的VIO状态估计器实现
estimator_node.cpp		// ROS节点函数，回调函数
feature_manager.cpp/.h	// 特征点管理，三角化，关键帧等
parameters.cpp/.h		// 读取参数

VINS-Mono rostopic list

melodic@Z2:~$ rostopic list
/benchmark_publisher/path
/cam0/image_raw
/cam1/image_raw
/clicked_point
/clock
/feature_tracker/feature
/feature_tracker/feature_img
/feature_tracker/restart
/imu0
/initialpose
/leica/position
/move_base_simple/goal
/pose_graph/base_path
/pose_graph/camera_pose_visual
/pose_graph/key_odometrys
/pose_graph/key_odometrys_array
/pose_graph/match_image
/pose_graph/match_points
/pose_graph/no_loop_path
/pose_graph/path_1
/pose_graph/path_2
/pose_graph/path_3
/pose_graph/path_4
/pose_graph/path_5
/pose_graph/path_6
/pose_graph/path_7
/pose_graph/path_8
/pose_graph/path_9
/pose_graph/pose_graph
/pose_graph/pose_graph_path
/rosout
/rosout_agg
/tf
/tf_static
/vins_estimator/camera_pose
/vins_estimator/camera_pose_visual
/vins_estimator/extrinsic
/vins_estimator/history_cloud
/vins_estimator/imu_propagate
/vins_estimator/key_poses
/vins_estimator/keyframe_point
/vins_estimator/keyframe_pose
/vins_estimator/odometry
/vins_estimator/path
/vins_estimator/point_cloud
/vins_estimator/relo_relative_pose
/vins_estimator/relocalization_path

数据结构

函数

sensor_msgs::PointCloud()

sensor_msgs::PointCloud中所有的信息都是在void img_callback(const sensor_msgs::ImageConstPtr &img_msg)中处理得到的，实际依赖的是FeatureTracker类，处理图像的入口是void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)。

sensor_msgs::PointCloud{
	std_msgs/Header header;
	
    // points中存储了所有当前图像中追踪到的角点的图像归一化坐标
	geometry_msgs/Point32[] points;
	
    // channels中存储了关于该角点的相关信息: 角点的id、像素横纵坐标、xy方向速度5个
	sensor_msgs/ChannelFloat32[] channels;
    	/* 在channels中存储了关于该角点的相关信息，这里共存储了5种信息。
		1.角点的id		2.角点像素坐标的横坐标		3.角点像素坐标的纵坐标
		4.角点在x方向的速度		5.角点在y方向的速度			*/
}

midPointIntegration()

midPointIntegration(
	// 中点积分 输入:
	_dt,		// 当前IMU数据和前一个IMU数据的时间差
	acc_0, 		// 前一个IMU加速度计数据
	gyr_0, 		// 前一个IMU陀螺仪数据
	_acc_1, 	// 当前IMU加速度计数据
	_gyr_1, 	// 当前IMU加速度计数据
	delta_p, 	// 前一个IMU 平移预计分测量值
	delta_q, 	// 前一个IMU 旋转预积分测量值
	delta_v,	// 前一个IMU 速度预积分测量值
    linearized_ba, linearized_bg, 	// 前一个ba和bg
    // 输出
	result_delta_p, result_delta_q, result_delta_v, // 当前IMU预计分测量值
    result_linearized_ba, result_linearized_bg, 	// 当前ba和bg
	1
);

void Estimator::optimization(
	// 残差项的构造和求解
)

Class

Class FeatureTracker

FeatureTracker类中几个比较重要的数据结构：

void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time){
	camodocal::CameraPtr m_camera; 	//相机模型，保存了相机的内参和相机投影方程
    //原始图像数据。prev_img好像没啥用，cur_img保存上一帧图像，forw_img保存当前帧。
	cv::Mat prev_img, cur_img, forw_img;
	vector prev_pts, cur_pts, forw_pts; // 图像中的角点坐标
	vector track_cnt; 		//当前追踪到的角点一共被多少帧图像追踪到
	vector ids; 			//当前追踪到的角点的ID，这个ID非常关键，保存了帧与帧之间角点的匹配关系。
}

class IntegrationBase

class IntegrationBase{
    Eigen::Vector3d delta_p; 	// 平移预积分
    Eigen::Quaterniond delta_q; // 旋转预积分
    Eigen::Vector3d delta_v; 	// 速度预积分
    Eigen::Matrix jacobian, covariance; // 雅可比矩阵和协方差矩阵
}

Class map

// 键是时间戳，值是图像帧，图像帧中保存了图像帧的位姿，预积分量和关于角点的信息
map< double, ImageFrame > all_image_frame; 

Class ImageFrame

class ImageFrame
{
	map> > > points;
	double t;
	Matrix3d R;
	Vector3d T;
	IntegrationBase *pre_integration;
	bool is_key_frame;
};

class FeatureManager

class FeatureManager{
	// 通过FeatureManager可以查询滑动窗口内所有的角点信息，以及这些角点被滑动窗口内的哪些帧观测到了
	...
    list feature; 
}

class FeaturePerId

class FeaturePerId{
    // 以feature_id为索引，并保存了出现该角点的第一帧的id，
	...
    const int feature_id;
    int start_frame;
    vector feature_per_frame;	
}

class FeaturePerFrame

class FeaturePerFrame{
	// 保存了归一化坐标，图像坐标以及深度	
	...
    Vector3d point;
    Vector2d uv;
    double z;
}

class Estimator

class Estimator{
    Vector3d Ps[(WINDOW_SIZE + 1)]; 	// 平移
    Vector3d Vs[(WINDOW_SIZE + 1)]; 	// 速度
    Matrix3d Rs[(WINDOW_SIZE + 1)]; 	// 旋转
    Vector3d Bas[(WINDOW_SIZE + 1)]; 	// 加速度偏差
    Vector3d Bgs[(WINDOW_SIZE + 1)]; 	// 陀螺仪偏差
}

vector

vector sfm_f

利用FeatureManager f_manager保存的信息，可以得到vector sfm_f，作为后续单目初始化的数据关联。

vector sfm_f;
struct SFMFeature
{
    bool state;
    int id; // 角点的id
    vector< pair > observation; // 保存了这个角点在一些列图像中的归一化坐标，以及那些图像帧的id。
    double position[3]; // 保存角点的三维坐标
    double depth;
};

bool

bool Estimator::relativePose()

bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)
// 在真正进行初始化之前，会对视差进行检查，依赖的函数的是
// 当平均视差大于30并且通过基础矩阵求解得到的内点数目大于12，可以认为当前的角点匹配足够支持单目初始化

基础知识

闭环检测中使用的特征点

闭环检测中每一个关键帧有两类特征点：
goodFeatureToTrack检测到的点及光流track到的点，这些点在FeatureTracker类中得到，KeyFrame类中提取的是FAST特征点。

两类特征点都计算了BRIEF描述子，但她们特点不同各有用处：
对于FAST点来说，其数量较多，主要用于DBOW2中的query，具体代码在PoseGraph::detectLoop()函数中。
detecLoop获得了可能存在闭环的之前的关键帧的index之后，就要用KeyFrame::findConnection()来确定是否是一个真正的闭环，这个时候就用到了第一类特征点。
具体来说，在old keyframe的FAST特征点中找跟当前keyframe的第一类特征点匹配的点，使用的方法是计算BRIEF描述子的汉明距离，代码见KeyFrame::searchByBRIEFDes()函数。
第一类特征点有比较精确的深度信息，它在世界坐标系下的3维坐标是已知的，在old keyframe中找到它们的匹配点就可以用solvePnP的方法获得当前keyframe和old keyframe的相对位姿。如果两个关键帧之间的yaw和T小于某个阈值才认为是一个真正的闭环。

需要注意的是FAST点和第一类点是通过不同方式检测出来的点，它们往往是不重合的。所以searchByBRIEFDes的时候效果应该不会特别好吧。如果想优化这部分的，可以在这方面动动脑筋。

Question

系统

为什么选择单目？

为什么选择非线性优化？

前端

feature_tracker 提取的是什么角点特征？Harris or Shi-Tomasi？用来做什么？

Harris角点

这两句什么意思？

#ifdef EIGEN_DONT_PARALLELIZE
    ROS_DEBUG("EIGEN_DONT_PARALLELIZE");
#endif
    ROS_WARN("waiting for image and imu...");

status是在哪里判断的？

更新特征点的意义是什么？为什么需要更新？

特征跟踪与特征匹配的区别是什么？

卷帘式快门与全局快门

estimator

为什么main()里面没有多线程处理的代码？

ROS里面每一个回调函数都是一个线程

m_buf.lock()与m_buf.unlock();

Mutex类互斥线程锁，防止读取数据时出错（待更新）。
把一段代码定义为互斥段（critical section），互斥段在一个时刻内只允许一个线程进入执行，而其他线程必须等待。
在进行多线程编程的时候，最好是加上锁，这是为了保护当前线程中一些代码中一些变量值不会被别的线程改变，也就是说你给当前线程加个锁那么就只能当解锁之前其他线程才能进入被保护的这段代码，加锁就是加一个互斥体