Package Summary
Tags | No category tags. |
Version | 0.0.0 |
License | TODO |
Build type | CATKIN |
Use | RECOMMENDED |
Repository Summary
Checkout URI | https://github.com/mrwangmaomao/vslam.git |
VCS Type | git |
VCS Version | master |
Last Updated | 2019-04-01 |
Dev Status | UNMAINTAINED |
CI status | No Continuous Integration |
Released | UNRELEASED |
Tags | No category tags. |
Contributing |
Help Wanted (0)
Good First Issues (0) Pull Requests to Review (0) |
Package Description
Additional Links
Maintainers
- qintong
Authors
视觉惯组系统
1 各文件作用
- factor 实现IMU、camera等残差模型,涉及了ceres优化库
- initial 系统初始化,外参标定,SFM
- loop_closure 闭环检测,这里主要利用DBOW2作者的一个demo程序
- utility 相机显示,四元数转换,系统rviz可视化
- estimator_node (main()函数) 执行多线程measurement_process、loop_detection、pose_graph
- feature_manager.cpp特征点管理,三角化,关键帧操作
- parameters.cpp外部系统设置参数输入
2 estimator_node文件
main函数
step1
初始化”vins_estimator”节点,读取配置文件参数。
step2
发布Rviz相关话题 订阅 “图像特征点数据” “复位estimator” “根据回环检测信息进行重定位”三个话题
step3
执行process()线程函数。主要处理VIO后端,包括IMU预积分、松耦合初始化和局部BA
- 调用匿名函数,从缓存队列中读取IMU数据和图像特征点数据,如果measurements为空,则匿名函数返回false,调用wait(lock),释放m_buf(为了使图像和IMU回调函数可以访问缓存队列),阻塞当前线程,等待被con.notify_one()唤醒直到measurements不为空时(成功从缓存队列获取数据),匿名函数返回true,则可以退出while循环。
- 遍历数组中各帧的IMU数据,进行预积分。如果第一帧IMU数据的时间戳晚于图像特征点的时间戳,需要对IMU数据进行插值得到图像帧时间戳对应的IMU数据,然后进行预积分estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
- 检测是否有回环 重定位,如果有重定位,需要estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
- 将图像特征点数据存到一个map容器中,键是特征点id,键值是特征点。执行处理图像程序estimator.processImage(image, img_msg->header);
- 每处理完一帧图像特征点数据,发布话题
- 检测是否需要优化,VINS系统完成滑动窗口优化后,用优化后的结果,更新里程计数据update();
线程锁的使用
|线程锁|作用| |-|-| |m_buf|处理多个线程使用imu_buf和feature_buf的冲突| |m_state|处理多个线程使用当前里程计信息(即tmp_P、tmp_Q、tmp_V)的冲突| |m_estimator|处理多个线程使用VINS系统对象(即Estimator类的实例estimator)的冲突|
3 预积分 factor/integration_base.h
参考:
https://blog.csdn.net/LilyNothing/article/details/79113841
https://blog.csdn.net/lancelot_vim/article/details/77888788
为什么需要预积分
在两帧视觉帧之间,往往有很多IMU的采集数据,其中计算速度积分的时候需要相机当前位姿,将当前位置引入到世界坐标系$C_Ws$,从p时刻数字积分一次得到p+1时刻,然后根据公式乘以p时刻相机位姿得到世界坐标系,最终导致要对k时刻开始的每一个时刻的相机pose求导,计算代价很大,由于状态变了,积分的值也会跟着变,每优化一次就要重新算一次积分。
中点积分
\(w=(w_0+w_1)/2+bg -->更新角速率\)
\(R=R_0 \times \frac{w}{2} -->更新旋转\)
\(a_0=R_0(a_0^{'}-ba) -->上一次载体的加速度\)
\(a_1=R_0(a_1^{'}-ba) -->当前载体的加速度\)
\(a=(a_0+a_1)/2 -->更新加速度\)
\(\Delta P=\Delta P^{'}+\Delta V^{'}\times dt + \frac{1}{2}a\Delta t^2 -->更新位移\)
\(\Delta V=\Delta V^{'} + a\Delta t -->更新速度\)
对应代码(factor/interation_base.h midPointIntegration()函数)
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;
误差状态方程
公式的推导详细看VINS-Mono详解 中的2.2和2.3部分。 雅克比矩阵初始为单位矩阵,协方差矩阵初始为0矩阵。
当加速度计和陀螺仪的偏置发生微小改变时,就可以根据雅克比矩阵对预计积分项进行修正,避免重复积分。
设置F和V矩阵
MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt +
-0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt +
-0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
F.block<3, 3>(6, 6) = Matrix3d::Identity();
F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;
F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;
F.block<3, 3>(9, 9) = Matrix3d::Identity();
F.block<3, 3>(12, 12) = Matrix3d::Identity();
//cout<<"A"<<endl<<A<<endl;
MatrixXd V = MatrixXd::Zero(15,18);
V.block<3, 3>(0, 0) = 0.25 * delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 3) = 0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * 0.5 * _dt;
V.block<3, 3>(0, 6) = 0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 9) = V.block<3, 3>(0, 3);
V.block<3, 3>(3, 3) = 0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(3, 9) = 0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(6, 0) = 0.5 * delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 3) = 0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * 0.5 * _dt;
V.block<3, 3>(6, 6) = 0.5 * result_delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 9) = V.block<3, 3>(6, 3);
V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;
更新雅克比矩阵和协方差矩阵
jacobian = F * jacobian; //更新雅可比矩阵
covariance = F * covariance * F.transpose() + V * noise * V.transpose(); //更新协方差矩阵
计算残差
Eigen::Vector3d dba = Bai - linearized_ba;
Eigen::Vector3d dbg = Bgi - linearized_bg;
Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);//对旋转进行校正
Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;//对速度进行校正
Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;//对位移进行校正
//计算残差
residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;
residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();
residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;
4 滑动窗口的维护 Estimator::slideWindow()
窗口的大小默认是10。
每当进来一个新的图像帧的时候,首先判断它与窗口里面存储的之前的那一帧的的相对位移,就是与第10帧的特征点的相对位置,用特征点的相对位移来表示。
如果特征点的平均相对位移大于某一个阈值,就认为是一个新的关键帧。就把这个新的关键帧压入到窗口里面,就是压入到第10个位置,然后其它的关键帧都往前移动。第一个位置的关键帧被移出去,边缘化。
如果不是新的关键帧,就把之前的第10帧边缘化掉,这个新的一帧替换成为第10帧。
总之,无论是哪种情况,这个新的一帧肯定都会成为窗口里面的第10帧。
边缘化,是在优化之后才进行的,而且最新的这帧上面观察到的新的特征点并不参与优化。所以,优化的时候,是包括最新的这一帧的11帧的姿态,以及前10帧的特征点在每一帧的投影,包括它们在最新这帧的投影点。所以para_Pose之类的待优化变量的数组长度是11。
5 处理接收的图像帧数据 Estimator::processImage()
step1 计算平均视差 FeatureManager::addFeatureCheckParallax()
当前帧图像特征点存入feature中后,并不会立即判断是否将当前帧添加为新的关键帧,而是去判断当前帧的前一帧(第2最新帧)。当前帧图像要在下一次接收到图像时进行判断(那个时候,当前帧已经变成了第2最新帧)。
计算第2最新帧与第3最新帧之间的平均视差(当前帧是第1最新帧),然后判断是否把第2最新帧添加为关键帧。f_manager.addFeatureCheckParallax(frame_count, image, td)
相机与IMU外参的在线标定。
在未完成初始化时,如果窗口没有塞满,不进行初始化,直到窗口塞满时进行初始化initialStructure()。如果初始化不成功,则滑动窗口。如果初始化成功,进行紧耦合优化 solveOdometry(),对窗口进行滑动,去除滑出了滑动窗口的特征点。
在完成初始化时,进行紧耦合优化 solveOdometry(),对窗口进行滑动,去除滑出了滑动窗口的特征点。同时判断系统是否失效。
6 系统初始化
用SFM求解滑动窗口中所有帧的位姿和所有路标点的3D位置。将SFM的结果与IMU预积分的值进行对齐,实现对陀螺仪偏置的矫正。
求解每一帧对应的速度,求解重力向量方向,恢复单目相机的尺度因子。在初始化过程中,没有对加速度计的偏置进行较正,因为重力是初始化过程中待求的量,而加速度计偏置与重力耦合,而且系统的加速度相对于重力加速度很小,所以加速度计偏置在初始化过程中很难观测,因此不考虑加速度计偏置的矫正。
6.1 通过重力variance确保IMU有足够的excitation
6.2 选定帧在滑动窗口中的帧号
6.3 SFM
先在关键帧窗口里面找到第1帧,第1帧与最后一帧有足够的像素位移,并且能用8点法计算出旋转和位移。
以1帧的姿态为世界坐标系。先从1帧开始与最后一帧进行三角定位。
再用pnp估计出下一帧的位姿,下一帧与最后一帧三角定位出更多的三维点。
重复上一步骤到倒数第二帧。
从2帧开始往第一帧,逐帧pnp,再与第1帧进行三角定位得到更多三维点。
每帧pnp的位姿初值都用上一个关键帧的位姿,剩下的那些还没有被三角定位的特征点,通过它被观察到的第一帧和最后一帧进行三角定位。
固定住第一帧的位置和姿态,固定住最后一帧的位置,这时候图像的位姿和点的位置都不太难,所以用ceres统一一起优化图像位姿和三维点位置,优化重投影误差。优化的测量值是特征点在每帧中被观察到的位置,可以转成重投影误差约束。有关的自变量是每帧图像的位姿,特征点的三维坐标。
优化完成之后,即用ceres优化出这些关键帧的位姿和地图点后,再用pnp算出在这段时间区域内的所有图像的位姿。每个图像的计算都用下一个关键帧的位姿来当pnp的初值。
6.4 通过PnP求解所有帧
6.5 视觉SFM的结果与IMU预积分结果对齐visualInitialAlign()(这部分内容详细看论文)
视觉SFM的结果与IMU预积分结果对齐 initial/initial_aligment.cpp 根据视觉SFM的结果来校正陀螺仪的Bias,注意得到了新的Bias后对应的预积分需要repropagate,对应代码中的solveGyroscopeBias()函数
初始化速度、重力向量和尺度因子,这一部分对应代码initial_aligment.cpp中的LinearAlignment()
重力细化
7 相机和IMU外参的旋转
用机器人手眼标定的方法计算出外参中的旋转。
其中$q_{b_{k+1}^{b_k}}$是陀螺仪预积分得到的,$q_{C_{k+1}^{C_k}}$是用8点法对前后帧对应的特征点进行计算得到的,
8 VIO紧耦合优化
VINS-Mono优化的状态向量包括滑动窗口内的n+1个关键帧时刻IMU坐标系的位置、姿态、旋转、加速度计偏置和陀螺仪偏置,Camera到IMU的外参,m+1个3D路标点的逆深度。
8.1 滑动窗口紧耦合优化
rviz可视化
注册发布话题,主要用于可视化
话题 | 名称 |
---|---|
pub_latest_odometry | imu_propagate |
pub_path | path |
relocalization_path | relocalization_path |
pub_odometry | odometry |
pub_point_cloud | point_cloud |
pub_margin_cloud | point_cloud |
pub_key_poses | key_poses |
pub_camera_pose | camera_pose |
pub_camera_pose_visual | camera_pose_visual |
pub_keyframe_pose | keyframe_pose |
pub_keyframe_point | keyframe_point |
pub_extrinsic | extrinsic |
pub_relo_relative_pose | relo_relative_pose |
Wiki Tutorials
Launch files
- launch/black_box.launch
-
- config_path [default: $(find feature_tracker)/../config/black_box/black_box_config.yaml]
- vins_path [default: $(find feature_tracker)/../config/../]
- launch/euroc.launch
-
- config_path [default: $(find feature_tracker)/../config/euroc/euroc_config.yaml]
- vins_path [default: $(find feature_tracker)/../config/../]
- launch/tum.launch
-
- config_path [default: $(find feature_tracker)/../config/tum/tum_config.yaml]
- vins_path [default: $(find feature_tracker)/../config/../]
- launch/cla.launch
-
- config_path [default: $(find feature_tracker)/../config/cla/cla_config.yaml]
- vins_path [default: $(find feature_tracker)/../config/../]
- launch/realsense_fisheye.launch
-
- config_path [default: $(find feature_tracker)/../config/realsense/realsense_fisheye_config.yaml]
- vins_path [default: $(find feature_tracker)/../config/../]
- launch/vins_rviz.launch
- launch/realsense_color.launch
-
- config_path [default: $(find feature_tracker)/../config/realsense/realsense_color_config.yaml]
- vins_path [default: $(find feature_tracker)/../config/../]
- launch/euroc_no_extrinsic_param.launch
-
- config_path [default: $(find feature_tracker)/../config/euroc/euroc_config_no_extrinsic.yaml]
- vins_path [default: $(find feature_tracker)/../config/../]
- launch/3dm.launch
-
- config_path [default: $(find feature_tracker)/../config/3dm/3dm_config.yaml]
- vins_path [default: $(find feature_tracker)/../config/../]