vins fusion 学习（更新中）

vins fusion 学习（更新中）

RVIZ图像：
绿色的是里程计路径
图像中红色的是特征点
红色框是相机
白色的小点是图像中的特征点对应到空间中的特征点

使用rosrun rqt_graph rqt_graph得到节点订阅图

可以看到rosbag发布了以下数据

• imu：imu0
• 双目图像：cam0（虽然我们暂时跑的是一个单目+imu的工程yaml配置，也就是第二个图像cam1没有用上）

按照流程来说

首先对拿到的图像做trackImage函数，这个函数有默认参数，适应于单目或者双目

/**

• 跟踪一帧图像，提取当前帧特征点
• 1、用前一帧运动估计特征点在当前帧中的位置
• 2、LK光流跟踪前一帧的特征点，正反向，删除跟丢的点；如果是双目，进行左右匹配，只删右目跟丢的特征点
• 3、对于前后帧用LK光流跟踪到的匹配特征点，计算基础矩阵，用极线约束进一步剔除outlier点（代码注释掉了）
• 4、如果特征点不够，剩余的用角点来凑；更新特征点跟踪次数
• 5、计算特征点归一化相机平面坐标，并计算相对与前一帧移动速度
• 6、保存当前帧特征点数据（归一化相机平面坐标，像素坐标，归一化相机平面移动速度）
• 7、展示，左图特征点用颜色区分跟踪次数（红色少，蓝色多），画个箭头指向前一帧特征点位置，如果是双目，右图画个绿色点
  */
  map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> FeatureTracker::trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1)

调用的语句是featureFrame = featureTracker.trackImage(t, _img);

然后会把featureFramepush到对应的featureBuf里

其中对于拿到的map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>>的值的意义是

ref: https://blog.csdn.net/hltt3838/article/details/109392565

---

后面就是processMeasurements函数进行处理

注意processMeasurements可以选择成多线程处理，如果不选择，就是直接跟在上面的函数后进行处理

首先相机20hz,imu200hz,那么一帧相机对应10帧imu，就先根据上一帧相机时间和当前帧相机时间取出这一段时间内的imu数据（理论上是10帧，但也可能因为错位或者处理不及时是别的，很正常）

然后把上面的数据使用for循环给到processIMU函数进行预积分处理
在processIMU这个函数里，会通过预积分给到 Matrix3d的旋转Rs[10]，Vector3d的位置Ps[10]，Vector3d的速度Vs[10]

---

然后就是最关键的processImage函数
void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const double header)
这个函数的第一个输入参数其实就是之前的featureFrame，第二个是对应的时间戳

首先由addFeatureCheckParallax函数根据和上一帧特征点的关系（多种情况，暂且不做复杂讨论）确定当前帧是否是关键帧
然后把对应标志位marginalization_flag设置为MARGIN_OLD或者MARGIN_SECOND_NEW

• 如果是关键帧，就是设置为MARGIN_OLD，然后移除掉滑动窗口的最早的一帧
• 如果不是，就移除掉滑动窗口最近的一帧（把当前帧当成最近的一帧）

---

三角化函数FeatureManager::triangulate

首先遍历所有特征点
然后会看当前特征点的深度是否大于0，因为默认深度是-1，如果大于0就说明这个特征点三角化过了

如果当前是双目，并且这个特征点同时在两个双目被观测到了（is_stereo==true）
就会走 if(STEREO && it_per_id.feature_per_frame[0].is_stereo)
一些变量的意思：
Ps[imu_i]: imu_i时刻的imu位姿,即imu与世界坐标系间的位移
Rs[imu_i]: imu_i时刻的imu旋转矩阵,即imu坐标系相对世界坐标系的旋转
tic[0]: 左目相机相对imu的位移变换
ric[0]: 左目相机相对imu的旋转变换

经过一通操作，求出左目相机的Tcw和右目的Tcw
然后取出这个特征点分别在左右目上的特征点的归一化坐标
根据上面四个内容，调用triangulatePoint使用SVD计算三角化点
计算出的point3d是世界坐标系下的点也就是twp
然后利用point3d计算出tcp，然后第三个变量就是相机坐标系下的深度z

三角化的目的就是计算出特征点的深度，不论是单目还是双目
计算出来后更改变量it_per_id.estimated_depth注意这里仍然是计算出来的深度，不是逆深度

---

ceres优化部分：关于重投影的残差添加

针对不同种类的情况进行分类：
首先如果STEREO && it_per_frame.is_stereo，也就是双目并且当前特征点在双目上都有
然后会根据当前帧是不是特征点的首帧

class ProjectionOneFrameTwoCamFactor : public ceres::SizedCostFunction<2, 7, 7, 1, 1>
problem.AddResidualBlock(f, loss_function, para_Ex_Pose[0], para_Ex_Pose[1], para_Feature[feature_index], para_Td[0]);

bool ProjectionOneFrameTwoCamFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    TicToc tic_toc;

    // 优化变量：外参（左目），外参（右目），特征点逆深度，相机与IMU时差
    Eigen::Vector3d tic(parameters[0][0], parameters[0][1], parameters[0][2]);
    Eigen::Quaterniond qic(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

    Eigen::Vector3d tic2(parameters[1][0], parameters[1][1], parameters[1][2]);
    Eigen::Quaterniond qic2(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);

    double inv_dep_i = parameters[2][0];

    double td = parameters[3][0];

    // 匹配点ij, 归一化相机点时差校正，对应到采集时刻的位置，因为IMU数据是对应图像采集时刻的
    Eigen::Vector3d pts_i_td, pts_j_td;
    pts_i_td = pts_i - (td - td_i) * velocity_i;
    pts_j_td = pts_j - (td - td_j) * velocity_j;

    // 左目归一化相机点
    Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i;
    // 左目IMU点
    Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
    Eigen::Vector3d pts_imu_j = pts_imu_i;
    Eigen::Vector3d pts_camera_j = qic2.inverse() * (pts_imu_j - tic2);
    Eigen::Map<Eigen::Vector2d> residual(residuals);

#ifdef UNIT_SPHERE_ERROR 
    residual =  tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized());
#else
    double dep_j = pts_camera_j.z();
    residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>();
#endif

    residual = sqrt_info * residual;

    if (jacobians)
    {
        Eigen::Matrix3d ric = qic.toRotationMatrix();
        Eigen::Matrix3d ric2 = qic2.toRotationMatrix();
        Eigen::Matrix<double, 2, 3> reduce(2, 3);
#ifdef UNIT_SPHERE_ERROR
        double norm = pts_camera_j.norm();
        Eigen::Matrix3d norm_jaco;
        double x1, x2, x3;
        x1 = pts_camera_j(0);
        x2 = pts_camera_j(1);
        x3 = pts_camera_j(2);
        norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), - x1 * x2 / pow(norm, 3),            - x1 * x3 / pow(norm, 3),
                     - x1 * x2 / pow(norm, 3),            1.0 / norm - x2 * x2 / pow(norm, 3), - x2 * x3 / pow(norm, 3),
                     - x1 * x3 / pow(norm, 3),            - x2 * x3 / pow(norm, 3),            1.0 / norm - x3 * x3 / pow(norm, 3);
        reduce = tangent_base * norm_jaco;
#else
        reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j),
            0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j);
#endif
        reduce = sqrt_info * reduce;

        if (jacobians[0])
        {
            Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_ex_pose(jacobians[0]);
            Eigen::Matrix<double, 3, 6> jaco_ex;
            jaco_ex.leftCols<3>() = ric2.transpose(); 
            jaco_ex.rightCols<3>() = ric2.transpose() * ric * -Utility::skewSymmetric(pts_camera_i);
            jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex;
            jacobian_ex_pose.rightCols<1>().setZero();
        }
        if (jacobians[1])
        {
            Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_ex_pose1(jacobians[1]);
            Eigen::Matrix<double, 3, 6> jaco_ex;
            jaco_ex.leftCols<3>() = - ric2.transpose();
            jaco_ex.rightCols<3>() = Utility::skewSymmetric(pts_camera_j);
            jacobian_ex_pose1.leftCols<6>() = reduce * jaco_ex;
            jacobian_ex_pose1.rightCols<1>().setZero();
        }
        if (jacobians[2])
        {
            Eigen::Map<Eigen::Vector2d> jacobian_feature(jacobians[2]);
#if 1
            jacobian_feature = reduce * ric2.transpose() * ric * pts_i * -1.0 / (inv_dep_i * inv_dep_i);
#else
            jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i;
#endif
        }
        if (jacobians[3])
        {
            Eigen::Map<Eigen::Vector2d> jacobian_td(jacobians[3]);
            jacobian_td = reduce * ric2.transpose() * ric * velocity_i / inv_dep_i * -1.0  +
                          sqrt_info * velocity_j.head(2);
        }
    }
    sum_t += tic_toc.toc();

    return true;
}

---

使用自己的BA替换ceres

下图显示的是双目+IMU,数据集是euroc的MH01

RPE w.r.t. translation part (m)
for delta = 1 (frames) using consecutive pairs
(with SE(3) Umeyama alignment)

       max      0.116119
      mean      0.022300
    median      0.022353
       min      0.000021
      rmse      0.027533
       sse      1.325852
       std      0.016148

如果只使用双目，不使用IMU，出来的数据结果还是很差的

---

下面的是自己的BA

RPE w.r.t. translation part (m)
for delta = 1 (frames) using consecutive pairs
(with SE(3) Umeyama alignment)

       max      0.130366
      mean      0.022690
    median      0.022445
       min      0.000013
      rmse      0.028078
       sse      1.378852
       std      0.016539

---

双目+IIMU ceres去掉先验，从DOGLEG改成LM

RPE w.r.t. translation part (m)
for delta = 1 (frames) using consecutive pairs
(with SE(3) Umeyama alignment)

       max      0.128625
      mean      0.022243
    median      0.022075
       min      0.000019
      rmse      0.027585
       sse      1.331591
       std      0.016314