2 estimator_node

详情链接：https://blog.csdn.net/qq_41839222/article/details/86293038

2.0.1 imu_callback

将imu_msg缓存到queue<sensor_msgs::ImuConstPtr> imu_buf，根据imu_msg预测当前时刻的tmp_P，tmp_V，tmp_Q，其中tmp_P，tmp_V，tmp_Q是相对量，因为没有绝对的基准，此处应该是在初始化中获取绝对信息，此处待确认？如果系统已经完成初始化(NON_LINEAR)，则发布预测的PVQ信息到"topic:/vins_estimator/imu_propagate"。

2.0.2 feature_callback

将feature_msg缓冲到queue<sensor_msgs::PointCloudConstPtr> feature_buf。

其中feature_msg信息如下：

    feature_points->header = img_msg->header;
    feature_points->header.frame_id = "world";
    feature_points->points.push_back(p);
        p.x = un_pts[j].x;
        p.y = un_pts[j].y;
        p.z = 1;
    feature_points->channels.push_back(id_of_point);
    feature_points->channels.push_back(u_of_point);
    feature_points->channels.push_back(v_of_point);
    feature_points->channels.push_back(velocity_x_of_point);
    feature_points->channels.push_back(velocity_y_of_point);

2.0.3 restart_callback

收到reset指令时，清空缓存器feature_buf和imu_buf。同时清空估计器estimator.clearState，并重置一些基本变量estimator.setParameter，这些重置的信息由配置文件提供。

2.0.4 relocalization_callback

将从"topic:/pose_graph/match_points"收到的points_msg缓冲到queue<sensor_msgs::PointCloudConstPtr> relo_buf。

2.1 Estimator::processIMU

2.1.1 getMeasurements

将IMU和img_msg进行对齐并组合，返回std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements。

2.1.2

积分出来的是前后两帧之间的 IMU增量信息：
$\hat{\alpha}_{i+1}^{b_k}=\hat{\alpha}_{i}^{b_k}+\hat{\beta}_{i}^{b_k}\delta t+\frac{1}{2}\overline{\hat{a}}_i\delta t^2$
$\hat{\beta}_{i+1}^{b_k}=\hat{\beta}_{i}^{b_k}+\overline{\hat{a}}_i\delta t$
$\hat{\gamma}_{i+1}^{b_k}=\hat{\gamma}_{i}^{b_k}\otimes\hat{\gamma}_{i+1}^{i}=\hat{\gamma}_{i}^{b_k}\otimes \left[\begin{matrix} 1\\ \frac{1}{2}\overline{\hat{\omega}}_i \delta t \end{matrix} \right]$
其中
$\overline{\hat{a}}_i=\frac{1}{2}[q_i(\hat{a}_i-b_{a_i})+q_{i+1}(\hat{a}_{i+1}-b_{a_i})]$
$\overline{\hat{\omega}}=\frac{1}{2}(\hat{\omega}_i + \hat{\omega}_{i+1})-b_{\omega_i}$
PVQ增量误差在离散形式下的矩阵形式:
$\left[ \begin{matrix} \delta \alpha_{k+1}\\ \delta \theta_{k+1}\\ \delta \beta_{k+1}\\ \delta b_{a_{k+1}}\\ \delta b_{{\omega}_{k+1}} \end{matrix} \right]=\left[ \begin{matrix} I & f_{01} & \delta t & f_{03} & f_{04}\\ 0 & f_{11} & 0 & 0 & -\delta t\\ 0 & f_{21} & I & f_{23} & f_{24}\\ 0 & 0 & 0 & I & 0\\ 0 & 0 & 0 & 0 & I \end{matrix} \right]\left[ \begin{matrix} \delta \alpha_{k}\\ \delta \theta_{k}\\ \delta \beta_{k}\\ \delta b_{a_{k}}\\ \delta b_{{\omega}_{k}} \end{matrix} \right] + \left[ \begin{matrix} v_{00} & v_{01} & v_{02} & v_{03} & 0 & 0\\ 0 & \frac{-\delta t}{2} & 0 & \frac{-\delta t}{2} & 0 & 0\\ -\frac{R_k\delta t}{2} & v_{21} & -\frac{R_{k+1}\delta t}{2} & v_{23} & 0 & 0\\ 0 & 0 & 0 & 0 & \delta t & 0\\ 0 & 0 & 0 & 0 & 0 & \delta t \end{matrix} \right]\left[ \begin{matrix} n_{a_k}\\ n_{\omega_k}\\ n_{a_{k+1}}\\ n_{\omega_{k+1}}\\ n_{a}\\ n_{\omega}\\ \end{matrix} \right]$

将公式简写为： $\delta z_{k+1}^{15\times 1} = F^{15\times 15}\delta z_{k}^{15\times 1} + V^{15\times 18}Q^{18\times 1}$

则jacobian的迭代公式为： $J_{k+1}^{15\times 15}=FJ_{k}$

acc_0,gyr_0保存上一时刻的imu数据；pre_integrations和tmp_pre_integration用于预积分的对象；
dt_buf[frame_count],linear_acceleration_buf[frame_count],angular_velocity_buf[frame_count]用缓存frame_count帧的dt，linear_acceleration和angular_velocity。外推当前时刻(当前帧)的PVR。预积分操作在pre_integrations的方法push_back()中完成，调用的方法propagate()。在delta_p，delta_q，delta_v，linearized_ba，linearized_bg的基础上基于acc_1和gyr_1进行预积分并叠加上去来求得帧间增量。

2.2 Estimator::processImage

输入：map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>>
<feature_id,<camera_id,<x, y, z, p_u, p_v, velocity_x, velocity_y>>>

FeatureManager::addFeatureCheckParallax：判断当前帧是否为关键帧，并将当前帧的特征点归档到f_manager中。如果为关键帧marginalization_flag = MARGIN_OLD，否则marginalization_flag = MARGIN_SECOND_NEW。
Estimator->“map<double, ImageFrame> all_image_frame”，将image填入ImageFrame中，并插入all_image_frame。
估计相机外参：首先由FeatureManager::getCorresponding获取相邻两帧跟踪的特征点，基于这些确认的特征点使用函数InitialEXRotation::CalibrationExRotation估计相机到世界坐标的转换矩阵，包括旋转和平移。
若solver_flag == INITIAL表示相机尚未完成初始化工作，则进入初始化分支，否则进入分支solver_flag == NON_LINEAR。
当进入solver_flag == INITIAL分支，若完成外参标定且相邻两帧的时间差超过0.1s。则进入Estimator::initialStructur进行初始化工作。

2.2.1 FeatureManager::addFeatureCheckParallax

输入：map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>>
<feature_id,<camera_id,<x, y, z, p_u, p_v, velocity_x, velocity_y>>>

问题：如果某一特征点没有被连续跟踪，则会reset吗？

image中的feature是按照feature_id和camera_id为索引进行归档的。通过遍历image中的feature，将每个feature根据feature_id查找到f_manager中相应的feature；如果没有查找到，则增加feature_id相应的feature到f_manager中。索引到目标feature_id的feature后，则将image中的<x, y, z, p_u, p_v, velocity_x, velocity_y>写入类FeaturePerFrame中，并将其加入到feature中的feature_per_frame。
last_track_num作为计数器，对之前已经存在的feature_id进行计数，该值反映的是前后帧特征点的跟踪计数，而相邻帧特征点直接用来恢复相邻帧的几何信息。
如果frame_count < 2(不存在相邻帧)或last_track_num < 20(相邻帧特征点过少，由此恢复的几何信息不可信)，则返回。
遍历feature_id，如果相邻帧存在该feature_id的特征点，则计算其在相邻两帧的视差(特征点在两帧的像素坐标系坐标直接做差即可得到)。
如果视差过大或者前后两帧跟踪特征点异常少，则将当前帧设定为关键帧。

2.2.2 InitialEXRotation::CalibrationExRotation

详情链接：https://cggos.github.io/slam/vinsmono_note_cg.html

相机与IMU之间的旋转标定非常重要，偏差1-2°系统的精度就会变的极低。

设相机利用对极关系得到的旋转矩阵为 $R_{c_{k+1}}^{c_k}$ ，IMU经过预积分得到的旋转矩阵为 $R_{b_{k+1}}^{b_k}$ ，相机与IMU之间的相对旋转为 $R_c^b$ ，则对于任一帧满足:
$R_{b_k}^{b_{k+1}}R_c^b=R_c^bR_{c_k}^{c_{k+1}}$
将旋转矩阵写为四元数，则上式可以写为:
$q_{b_{k+1}}^{b_k}\otimes q_c^b=q_c^b\otimes q_{c_{k+1}}^{c_k}$
将其写为左乘和右乘的形式:
$([q_{b_{k+1}}^{b_k}]_L-[q_{c_{k+1}}^{c_k}]_R)q_c^b=Q_{k+1}^{k}q_c^b=0$
$[q]_L$ 与 $[q]_R$ 分别表示四元数左乘矩阵和四元数右乘矩阵，其定义为（四元数实部在后）:
$[q]_L=\left[ \begin{matrix} q_w I_3+[q_{xyz}]_{\times} & q_{xyz}\\ -q_{xyz} & q_w\\ \end{matrix} \right]$

$[q]_R=\left[ \begin{matrix} q_w I_3-[q_{xyz}]_{\times} & q_{xyz}\\ -q_{xyz} & q_w\\ \end{matrix} \right]$
那么对于 n对测量值，则有:
$\left[ \begin{matrix} w_1^0Q_1^0\\ w_2^1Q_2^1\\ \vdots\\ w_{N}^{N-1}Q_{N}^{N-1}\\ \end{matrix} \right]q_{c}^{b}=Q_N q_{c}^{b}=0$
其中 $w_{N}^{N-1}$ 为外点剔除权重，其与相对旋转求得的角度残差有关，N为计算相对旋转需要的测量对数，其由最终的终止条件决定。

InitialEXRotation::solveRelativeR用于计算相邻两帧相机位姿的旋转角 $R_{c_{k+1}}^{c_k}$ ，此外可以通过imu观测量预计分获取观测值 $R_{b_{k+1}}^{b_k}$ ，将这两种信息作为输入即可使用上述算法进行外参标定工作。

###2.1.3 Estimator::initialStructure
MotionEstimator::solveRelativeRT？待补充

根据all_image_frame来计算这个列表期间的加速度及其方差，如果方差<0.25，则认为加速度值太过稳定，建议尝试使加速度变化来更好的指导完成初始化工作。
遍历f_manager中feature，每个feature_id对应一个类型为SFMFeature的对象tmp_feature，然后遍历该feature_id在各帧的特征点，并将其信息以形式pair<int,Vector2d>插入到tmp_feature中的成员变量observation中。其中这里的"int"表示帧的序号。
Estimator::relativePose：在滑窗内从第0开始，依次遍历各帧，寻找与当前帧的匹配特征点数较多的关键帧作为参考帧，并通过求基础矩阵cv::findFundamentalMat计算出当前帧到参考帧的转换矩阵T;然后根据待选参考帧和当前帧之间的特征点通过函数MotionEstimator::solveRelativeRT来求得两帧之间的Rotation和transformation矩阵。当视差和基础矩阵满足要求时退出，此时的左端这一帧记为参考帧，Rotation和transformation即为最新一帧和参考帧间的外参。
GlobalSFM::construct三角化特征点：使用函数GlobalSFM::construct将这些特征点由二维信息恢复出来三维信息，这是通过多帧之间的视差来实现的。
如果是上一步处理过的帧，标记all_image_frame中的is_key_frame为true；若是没有处理过，则is_key_frame为false，此时该帧的pose未知，通过收集该帧具备三维信息特征点的3d和2d坐标，使用cv::solvePnP求解出该帧的pose。至此all_image_frame中的每一帧均有了pose信息用于接下来的数据处理

2.2.3.1 GlobalSFM::construct

输出：每一个关键帧相对于参考帧的pose，包含旋转矩阵Q和平移矩阵T, l表示参考帧的索引，sfm_tracked_points保存的是所有特征点的三维信息

由Estimator::relativePose求得了参考帧到当前帧的Rotation和transformation。首先根据这些信息对参考帧和当前帧赋值，作为已知信息，其中以参考帧作为维持的坐标框架。
首先遍历参考帧(l)到当前帧(frame_num)，对于选定的两帧(遍历帧和当前帧)，如果两帧的pose均已知，则通过函数triangulateTwoFrames来恢复选定的两帧同时跟踪的特征点的三维信息。特征点三维信息的恢复通过调用函数GlobalSFM::triangulatePoint实现的。如果选定帧的pose未知，则先通过GlobalSFM::solveFrameByPnP计算选定帧相对于参考帧的pose。通过遍历选定帧的特征点的相机坐标系坐标pts_2_vector和其对应的由之前的步骤恢复的三维信息pts_3_vector，使用函数cv::solvePnP基于选定帧特征点2d坐标及对应的3d坐标对来获得当前帧的pose。一旦选定帧的pose求定后，进一步通过函数triangulateTwoFrames来恢复选定的两帧同时跟踪的特征点的三维信息。
类似于上一步的操作，再次遍历第l-1帧到第0帧，先求取当前帧的pose，然后基于已知的pose，进一步通过函数triangulateTwoFrames来恢复选定的两帧同时跟踪的特征点的三维信息。
三角化其他没有未恢复的特征点。
BA优化滑窗内所有关键帧的位姿pose(旋转和平移)参数，其中设定参考帧的pose为常量；设定当前帧的平移为常量。
遍历所有特征点，首先拿到该特征点的三维信息，并将三维信息变换到相应帧的相机坐标系，并和该特征点的相机坐标系坐标求差作为残差，通过ceres优化该残差使其值最小，从而实现关键帧pose的优化。
更新每一个关键帧相对于参考帧的pose
将每一个特征点的三维信息保存到“map<int, Vector3d> sfm_tracked_points”中

xiaoooli

发布了21 篇原创文章 · 获赞 0 · 访问量 9489

私信关注

vins之estimator