ROS-3DSLAM（8）：视觉部分visual_estimator第二节：initial 2

2021@SDUSC
2021年11月12日星期四——2021年11月13日星期六

一、背景简介：

结束了对于initial_alignment的初步阅读，下一步还要继续对于initial文件夹下的其他文件的分析，毕竟我们的最终目的还是对于函数esitimator中的的processImage()做分析。

这次的计划是针对Initial_ex_rotation和solve_5ph给出分析。

二、Initial_ex_rotation分析：

A、功能分析

函数的功能是求解从相机（Camera）坐标系到IMU坐标系的相对旋 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-mHOutEzL-1636789051650)(https://www.zhihu.com/equation?tex=R_%7Bbc%7D)]，在该函数中用的ric表示的。

经过我的搜索，发现这一部分被称为SLAM的基础的算法，也算是著名的部分经典部分了。

这部分的原理介绍网上有很多，我在这里就不搬过来了，只是简单地给出自己的说明：

首先利用相机两帧的位姿计算出来一个旋转量R1，或利用imu预积分得到R2，经过计算得到R1‘，根据两者之间的差距计算得出最准确的RC。

B、代码分析

1、首先是头文件

开头的注释说明了代码的功能，在后面给出了五个函数的声明（不包含类的构造函数）

/* This class help you to calibrate extrinsic rotation between imu and camera when your totally don't konw the extrinsic parameter */
class InitialEXRotation
{
public:
	InitialEXRotation();
    bool CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result);
private:
	Matrix3d solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres);

    double testTriangulation(const vector<cv::Point2f> &l,
                             const vector<cv::Point2f> &r,
                             cv::Mat_<double> R, cv::Mat_<double> t);
    void decomposeE(cv::Mat E,
                    cv::Mat_<double> &R1, cv::Mat_<double> &R2,
                    cv::Mat_<double> &t1, cv::Mat_<double> &t2);
    int frame_count;

    vector< Matrix3d > Rc;
    vector< Matrix3d > Rimu;
    vector< Matrix3d > Rc_g;
    Matrix3d ric;
};

2、CalibrationExRotation，最重要的函数

#include "initial_ex_rotation.h"
//构造函数，初始化各个参数，MatrixXd::Identity(rows,cols)               // eye(rows,cols)      //单位矩阵  
InitialEXRotation::InitialEXRotation(){
   
    frame_count = 0;
    Rc.push_back(Matrix3d::Identity());
    Rc_g.push_back(Matrix3d::Identity());
    Rimu.push_back(Matrix3d::Identity());
    ric = Matrix3d::Identity();
}
//先要弄清楚这些传进来的参数是什么, 一共有三个传进来的参数
//corres 两帧之间匹配的特征点 归一化坐标，通过对极约束？，构建本质矩阵E，分解得到两帧之间的旋转矩阵， solveRelativeR(corres) 函数实现，也就是开头提到的通过相机获得的旋转量。
//delta_q_imu 为相邻两时刻 IMU预积分的旋转量，四元数表示传入进来的 匹配的归一化坐标，IMU预计分旋转量，会先存在vector中，因为需要多对信息，构建超定方程矩阵A
//calib_ric_result 为待求的外参数
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
   
    frame_count++;
    //利用下面的函数计算得到旋转量1
    Rc.push_back(solveRelativeR(corres));
    //将原来的四元数转化成了旋转矩阵，方便后面的计算。
    Rimu.push_back(delta_q_imu.toRotationMatrix());
    //每帧IMU相对于起始帧IMU的R，初始化为IMU预积分
    Rc_g.push_back(ric.inverse() * delta_q_imu * ric);
	
    //构建公式当中出现的矩阵
    Eigen::MatrixXd A(frame_count * 4, 4);
    A.setZero();
    
    int sum_ok = 0;
    for (int i = 1; i <= frame_count; i++)
    {
   
        Quaterniond r1(Rc[i]);
        Quaterniond r2(Rc_g[i]);
        
        /* angularDistance就是计算两个坐标系之间相对旋转矩阵在做轴角变换后(u * theta)
        的角度theta, theta越小说明两个坐标系的姿态越接近，这个角度距离用于后面计算权重，
        这里计算权重就是为了降低外点的干扰，意思就是为了防止出现误差非常大的R_bk+1^bk和 
        R_ck+1^ck约束导致估计的结果偏差太大*/

        double angular_distance = 180 / M_PI * r1.angularDistance(r2);
        ROS_DEBUG(
            "%d %f", i, angular_distance);
		//核函数 加权 
        double huber = angular_distance