标定相机和IMU 之间外参的旋转矩阵

pangdawa

已于 2023-06-15 13:54:53 修改

阅读量2k

点赞数

分类专栏： slam C++ Tools 文章标签：矩阵数码相机线性代数

于 2019-02-27 11:56:45 首次发布

本文链接：https://blog.csdn.net/haithink/article/details/80594243

版权

C++ 同时被 3 个专栏收录

56 篇文章 0 订阅

订阅专栏

Tools

31 篇文章 0 订阅

订阅专栏

slam

7 篇文章 0 订阅

订阅专栏

2023-6-15 13:52:40
时间太久远了，这个代码估计来自VINS-MONO或者VINS-mobile之类的代码吧。

using namespace Eigen;
bool CalibrationExRotation(int frame_count, std::vector<Matrix3d> Rc, 
							std::vector<Matrix3d> Rimu,
							Matrix3d &calib_ric_result)
{
    //Rc.push_back(solveRelativeR(corres));
    //Rimu.push_back(delta_q_imu.toRotationMatrix());
    //Rc_g.push_back(ric.inverse() * delta_q_imu * ric);
	//std::vector<Matrix3d> Rc_g;

    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);

        double huber = 1;//angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
        ++sum_ok;
        Matrix4d L, R;

        double w = Quaterniond(Rc[i]).w();
		Eigen::Vector3d q = Quaterniond(Rc[i]).vec();
        L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q);
        L.block<3, 1>(0, 3) = q;
        L.block<1, 3>(3, 0) = -q.transpose();
        L(3, 3) = w;

        Quaterniond R_ij(Rimu[i]);
        w = R_ij.w();
        q = R_ij.vec();
        R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q);
        R.block<3, 1>(0, 3) = q;
        R.block<1, 3>(3, 0) = -q.transpose();
        R(3, 3) = w;

        A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
    }

    JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
    Matrix<double, 4, 1> x = svd.matrixV().col(3);
    Quaterniond estimated_R(x);
    Matrix3d ric = estimated_R.toRotationMatrix().inverse();
    //cout << svd.singularValues().transpose() << endl;
    //cout << ric << endl;
	Eigen::Vector3d ric_cov;
    ric_cov = svd.singularValues().tail<3>();
	int WINDOW_SIZE = 250;
    if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
    {
        calib_ric_result = ric;
        return true;
    }
    else
        return false;
}