程序设计的三种基本结构,金蝶科目代码结构设置
VINS-Mono和VINS-Mobile是香港科技大学沈副劲动队开源的单眼视觉惯导SLAM方案。 在基于优化和滑动窗口的VIO中，使用IMU预积分构建紧密的组合框架。 并具有自动初始化、在线外参标定、重定位、闭环检测、全局姿态图优化功能。
方案最大的贡献是构建了有效的融合算法，视觉闭环等模块使用了比较常用的算法。
系列博客结合项目组发布的paper，从代码层面，对系统的各个模块进行逐级分析，以达到对单眼VIO整体的掌握，帮助理解各类算法，针对应用场景的视觉惯导SLAM系统最终目标是在ar APP中使用(Android )。
系统pipeline
主要分为五个部分
1 .传感器数据处理：
单用途照相机monocular camera 3360 featuredetectionandtrackingimu : pre-integration2.初始化：
3 .仅使用可视构建的SfM将SfM结果与IMU预积分结果对齐3 .基于滑动窗口的非线性优化：
4 .闭环检测：
5. 4自由度全局姿态图优化：
主要依赖的库只有OpenCV、Eigen和Ceres Solver，代码目录如下
核心算法位于feature_tracker和vins_estimator包中。
按照REDEME步骤在EuRoC/MH_05_difficult.bag中记录的数据结果如下：
使用rqt_graph获得系统节点和主题之间的关系：
rosbag将记录的imu数据和单镜头相机取得的图像数据分别投稿到/imu0和/cam0/image_raw的话题； /feature_tracker节点通过订阅/cam0/image_raw主题获取图像数据，/vins_estimator节点通过订阅/imu0主题获取imu数据
因此，/feature_tracker节点负责视觉提取和跟踪，/vins_estimator是融合系统的主要部分。
为了便于查看代码，整理了各部分的体系结构图(正在更新) :
进程：
系统入口是feature_tracker_node.cpp文件中的main函数
1 .首先创建feature_tracker节点，并从配置文件中读取信息(parameters.cpp )。 这包括在ROS上发出订阅的主题名称。 图像大小； 特征跟踪参数； 需要添加鱼眼mask以去除边缘噪声吗； % YAML :1.0 # commonparametersimu _ topic : '/IMU0' image _ topic 3360 '/cam0/image _ raw ' # cameracalibrationmodel _ type : pinhole camera _ name : camera image _ width :752 image _ height :480 distortion _ Nath :-1.578 e-04 projection _ parameters : FX :616 e 02f y :4.603 e 02cx 3360.6363 02 # extrinsicparameterbetweeeenimuandcandcandcandcandcation 0 haveanaccurateextrinsicparameters.wewilltrustthefollowingimu ^ r _ cam，imu^T_cam， don ' tchangeit.#1haveaninitialguessaboutextrinsicparameters.wewilloptimizearoundyourinitialguess.# 2 don ' tknowanythingaboutextrinsicparameters.youdon ' tneedtogiver， t.wewilltrytocalibrateit.dosomerotationmovementatbeginning.ex _ calib _ result _ path 3360 '/config/euroc/ex _ ex theextrinsiccalibrationresultwillbewrittenvins _ folder _ pathex _ calib _ result _ path.# ifyouchoosult
e down the following matrix.#Rotation from camera frame to imu frame, imu^R_camextrinsicRotation: !!opencv-matrix rows: 3 cols: 3 dt: d data: [0, -1, 0, 1, 0, 0, 0, 0, 1]#Translation from camera frame to imu frame, imu^T_camextrinsicTranslation: !!opencv-matrix rows: 3 cols: 1 dt: d data: [-0.02,-0.06, 0.01]#feature traker paprametersmax_cnt: 150 # max feature number in feature trackingmin_dist: 30 # min distance between two features freq: 10 # 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 topicequalize: 1 # if image is too dark or light, trun on equalize to find enough featuresfisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points#optimization parametersmax_solver_time: 0.04 # max solver itration time (ms), to guarantee real timemax_num_iterations: 8 # max solver itrations, to guarantee real timekeyframe_parallax: 10.0 # keyframe selection threshold (pixel)#imu parameters The more accurate parameters you provide, the better performanceacc_n: 0.2 # accelerometer measurement noise standard deviation. #0.2gyr_n: 0.02 # gyroscope measurement noise standard deviation. #0.05acc_w: 0.0002 # accelerometer bias random work noise standard deviation. #0.02gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5g_norm: 9.81007 # gravity magnitude#loop closure parametersloop_closure: 1 #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly; #also give the camera calibration file same as feature_tracker nodepattern_file: "/support_files/brief_pattern.yml"voc_file: "/support_files/brief_k10L6.bin"min_loop_num: 25
该config.yaml文件中的其他参数在vins_estimator_node中被读取，属于融合算法的参数。
优化参数（最大求解时间以保证实时性，不卡顿；最大迭代次数，避免冗余计算；视差阈值，用于选取sliding window中的关键帧）；imu参数，包括加速度计陀螺仪的测量噪声标准差、零偏随机游走噪声标准差，重力值（imu放火星上需要改变）；imu和camera之间的外参R，t；可选（0）已知精确的外参，运行中无需改变,（1）已知外参初值，运行中优化，（2）什么都不知道，在线初始化中标定闭环参数，包括brief描述子的pattern文件（前端视觉使用光流跟踪，不需要计算描述子），针对场景训练好的DBow二进制字典文件；2. 监听IMAGE_TOPIC, 有图像信息发布到IMAGE_TOPIC上时，执行回调函数： ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
3. img_callback()
前端视觉的算法基本在这个回调函数中，步骤为：
1. 频率控制，保证每秒钟处理的image不多于FREQ；
2. 对于单目：
1）. readImage;
2）. showUndistortion(可选);
3）. 将特征点矫正（相机模型camodocal）后归一化平面的3D点（此时没有尺度信息，3D点p.z=1），像素2D点，以及特征的id，封装成ros的sensor_msgs::PointCloud消息类型;
3. 将处理完的图像信息用PointCloud和Image的消息类型，发布到"feature"和"feature_img"的topic：
pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
4. 包含的视觉算法：
1. CLAHE(Contrast Limited Adaptive Histogram Equalization)
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
2. Optical Flow(光流追踪)
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
3. 根据匹配点计算Fundamental Matrix， 然后用Ransac剔除不符合Fundamental Matrix的外点
cv::findFundamentalMat(un_prev_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
4. 特征点检测：goodFeaturesToTrack， 使用Shi-Tomasi的改进版Harris corner
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.1, MIN_DIST, mask);
特征点之间保证了最小距离30个像素，跟踪成功的特征点需要经过rotation-compensated旋转补偿的视差计算，视差在30个像素以上的特征点才会去参与三角化和后续的优化，保证了所有的特征点质量都是比较高的，同时降低了计算量。