VINS初始化之外参在线标定

VINS初始化之外参在线标定

一、理论推导

估计旋转外参数Qbc具体公式推导：

二、代码实现

initial_ex_rotation.cpp

if (ESTIMATE_EXTRINSIC == 2)
    {
   
        cout << "calibrating extrinsic param, rotation movement is needed" << endl;
        if (frame_count != 0)
        {
   
            vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
            Matrix3d calib_ric;
            if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
            {
   
                // ROS_WARN("initial extrinsic rotation calib success");
                // ROS_WARN_STREAM("initial extrinsic rotation: " << endl
                                                            //    << calib_ric);
                                                            
                ric[0] = calib_ric;
                RIC[0] = calib_ric;
                ESTIMATE_EXTRINSIC = 1;
            }
        }
    }

/**
 * @Description:  在线标定相机到imu之间的相对旋转Rbc
 * @param:corres: 两帧之间多个匹配点的归一化坐标
 * @param:delta_q_imu: 两帧陀螺仪积分得到的相对旋转
 * @return:calib_ric_result: 标定结果
 */
int flag = 0;
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
   
    frame_count++;
    // std::cout << "frame_count"<<frame_count << std::endl;
    Rc.push_back(solveRelativeR(corres));
    Rimu.push_back(delta_q_imu.toRotationMatrix());

    // std::cout << "ric" << ric << std::endl;

    Rc_g.push_back(ric.inverse() * delta_q_imu * ric);

    // std::cout << "Rc: " <<Rc.at(flag)<< std::endl;
    // std::cout << "Rc_g: " << Rc_g.at(flag)<< std::endl;

    flag++;
    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]);

        double angular_distance = 180 / M_PI * r1.angularDistance(r2);
        //ROS_DEBUG("%d %f", i, angular_distance);

        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;//鲁棒核权重
        ++sum_ok;
        Matrix4d L, R;

        double w = Quaterniond(Rc[i]).w();
        Vector3d q