VINS学习杂记

参考学习博客
https://github.com/ManiiXu/VINS-Mono-Learning
1. 地图保存与重利用

在停止图像的输入后，vins_estimator对应的窗口输入 s 即可完成地图保存
重利用：
首先修改对应的config.yaml，设置路径 pose_graph_save_path 后，播放需要建图的bag文件，播放完成在vins_estimator控制台键入‘s’，当前包的所有位姿图会存储下来。

将config.yaml中的load_previous_pose_graph修改为1，然后重新运行vins_estimator，系统则会加载 result/pose_graph下的位姿图， 新的序列会和之前的位姿图相重合。

2.vins中的数据结构
数据结构描述
话题发布：
在feature_trackers_node.cpp中回调函数img_callback的输入，表示一幅图像
在feature_trackers.cpp中img_callback()中创建feature_points并封装，在main()中发布为话题“/feature_tracker/feature”，包含一帧图像中特征点信息
在pose_graph_node.cpp的main()中发布话题“/pose_graph/match_points”
主要包含重定位的两帧间匹配点和匹配关系（变换矩阵）

vins_estimator文件夹目录
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：特征点管理，三角化，关键帧等

全部ROS节点信息：

vins_estimator文件夹
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：特征点管理，三角化，关键帧等

• 小觅相机运行测试

https://blog.csdn.net/fang794735225/article/details/84722728

文中涉及到的相关链接：

下载 MYNT-EYE-SDK-D-SDK：https://github.com/slightech/MYNT-EYE-D-SDK.git
ROS: https://slightech.github.io/MYNT-EYE-D-SDK/ros_install.html
小觅相机专用版的VINS-Mono：https://github.com/slightech/MYNT-EYE-VINS-Sample

建议下载老版本：https://github.com/slightech/MYNT-EYE-VINS-Sample/tree/mynteye-d
1、下载并编译SDK

git clone https://github.com/slightech/MYNT-EYE-D-SDK.git
 
cd MYNT-EYE-D-SDK-master
 
make init  #重新插拔一次相机
 
make all

出错执行
sudo apt-get install ros-kinetic-cv-bridge
make all

2、安装 MYNT-EYE-VINS-Sample

mkdir -p ~/catkin_ws/src
 
cd ~/catkin_ws/src
 
git clone https://github.com/slightech/MYNT-EYE-VINS-Sample/tree/mynteye-d

此时回到工作空间catkin_ws目录下

cd ~/catkin_ws
 
catkin_make
 
source devel/setup.bash
 
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
 
source ~/.bashrc

3、用小觅相机运行VINS-Mono
插入相机，打开摄像头

cd ~/MYNT-EYE-D-SDK-master
 
source ./wrappers/ros/devel/setup.bash
 
roslaunch mynteye_wrapper_d mynteye.launch

重新打开另一个终端，运行MYNT-EYE-VINS-Sample

roslaunch vins_estimator mynteye_d.launch

比赛使用部分：
vins-master中在pose-graph的pose_graph_node中修改，读取经过回环检测后的坐标：
文件开头定义发布节点：
ros::Publisher pub_true_path;
main函数中：
pub_true_path = n.advertise<geometry_msgs::Point>(“true_path”, 1000);

void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)函数中修改：

	//回环检测后的坐标发布
	geometry_msgs::Point pose_pub;
	Vector3d vio_pose;
	vio_pose=vio_t_cam;
	pose_pub.x = vio_pose.x();
	pose_pub.y = vio_pose.y();
	pose_pub.z = vio_pose.z();
	pub_true_path.publish(pose_pub);

在visualization.cpp中可以查看vins_estimator向pose_graph节点发送的节点信息

void registerPub(ros::NodeHandle &n)
{
    pub_latest_odometry = n.advertise<nav_msgs::Odometry>("imu_propagate", 1000);
    pub_path = n.advertise<nav_msgs::Path>("path", 1000);
    pub_odometry = n.advertise<nav_msgs::Odometry>("odometry", 1000);
    pub_point_cloud = n.advertise<sensor_msgs::PointCloud>("point_cloud", 1000);
    pub_margin_cloud = n.advertise<sensor_msgs::PointCloud>("margin_cloud", 1000);
    pub_key_poses = n.advertise<visualization_msgs::Marker>("key_poses", 1000);
    pub_camera_pose = n.advertise<nav_msgs::Odometry>("camera_pose", 1000);
    pub_camera_pose_visual = n.advertise<visualization_msgs::MarkerArray>("camera_pose_visual", 1000);
    pub_keyframe_pose = n.advertise<nav_msgs::Odometry>("keyframe_pose", 1000);
    pub_keyframe_point = n.advertise<sensor_msgs::PointCloud>("keyframe_point", 1000);
    pub_extrinsic = n.advertise<nav_msgs::Odometry>("extrinsic", 1000);

    cameraposevisual.setScale(0.1);
    cameraposevisual.setLineWidth(0.01);
}