首页 > 其他分享 >VINS中旋转外参初始化

VINS中旋转外参初始化

时间:2023-08-27 16:45:44浏览次数:38  
标签:初始化 外参 right mathbf begin ric VINS array left

VINS 中的旋转外参初始化

​ 为了使这个两个传感器融合,我们首先需要做的事情是将两个传感器的数据对齐,除了时间上的对齐,还有空间上的对齐。空间上的对齐通俗的讲就是将一个传感器获取的数据统一到另一个传感器的坐标系中,其关键在于确定这两个传感器之前的外参,本文将详细介绍 VINS_Monocamera-imu 的旋转外参标定算法原理并对其代码进行解读(VINS-Fusion中也是一样的)。

​ 相机与 IMU 之间的相对旋转如下图所示:

image

​ 上图表示相机和 IMU 集成的系统从到的运动,其中视觉可以通过特征匹配求得到时刻的旋转增量,同时 IMU 也可以通过积分得到到时刻的旋转增量。两个求得的旋转增量应当是相同的,因此分别将在自己坐标系下求得的增量转移到对方的坐标系下,即经过 \(\mathbf{q}_{bc}\) 变换后应当还是相同的。

​ 相邻两时刻 \(k\) , \(k + 1\) 之间有: IMU 旋转积分 \(\mathbf{q}_{b_{k} b_{k+1}}\) ,视觉测量 \(\mathbf{q}_{c_{k} c_{k+1}}\)。则有:

\[\mathbf{q}_{b_{k} b_{k+1}} \otimes \mathbf{q}_{b c}=\mathbf{q}_{b c} \otimes \mathbf{q}_{c_{k} c_{k+1}} \]

​ 上面的式子可以写成:

\[\left(\left[\mathbf{q}_{b_{k} b_{k+1}}\right]_{L}-\left[\mathbf{q}_{c_{k} c_{k+1}}\right]_{R}\right) \mathbf{q}_{b c}=\mathbf{Q}_{k+1}^{k} \cdot \mathbf{q}_{b c}=\mathbf{0} \]

​ 其中 \(\left[ \dot{\ \ \ } \right]_L, \left[ \dot{\ \ \ } \right]_R\) 表示 left and right quaternion multiplication,不清楚可以看附录,其实就是四元数乘法换了个表达形式。

​ 将多个时刻线性方程累计起来,并加上鲁棒核权重得到:

\[\begin{array}{c} {\left[\begin{array}{c} w_{1}^{0} \cdot \mathbf{Q}_{1}^{0} \\ w_{2}^{1} \cdot \mathbf{Q}_{2}^{1} \\ \vdots \\ w_{N}^{N-1} \cdot \mathbf{Q}_{N}^{N-1} \end{array}\right] \mathbf{q}_{b c}=\mathbf{Q}_{N} \cdot \mathbf{q}_{b c}=\mathbf{0}} \\ w_{k+1}^{k}=\left\{\begin{array}{ll} 1, & r_{k+1}^{k}<\text { threshold } \\ \frac{\text { threshold }}{r_{k+1}^{k}}, & \text { otherwise } \end{array}\right. \end{array} \]

该线性方程为超定方程,有最小二乘解。采用 SVD 进行求解。

  • 代码
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
    frame_count++;
    Rc.push_back(solveRelativeR(corres));                               // 对极几何计算出的 R,t 约束
    Rimu.push_back(delta_q_imu.toRotationMatrix());     // IMU 预积分的得到的 R,t 约束
    Rc_g.push_back(ric.inverse() * delta_q_imu * ric);       // 将imu预积分转换到相机坐标系

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

        // A.angularDistance(B):是求两个旋转之间的角度差,用弧度表示
        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; // huber 核函数作加权
        ++sum_ok;
        // L R 分别为左乘和右乘矩阵
        Matrix4d L, R;

        double w = Quaterniond(Rc[i]).w();
        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);
    }

    // svd 分解中最小奇异值对应的右奇异向量作为旋转四元数
    JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
    Matrix<double, 4, 1> x = svd.matrixV().col(3);
    Quaterniond estimated_R(x);
    // 所求的 x 是q^b_c,在最后需要转换成旋转矩阵并求逆。
    ric = estimated_R.toRotationMatrix().inverse();
    //cout << svd.singularValues().transpose() << endl;
    //cout << ric << endl;
    Vector3d ric_cov;
    ric_cov = svd.singularValues().tail<3>();
    // 至少迭代计算了WINDOW_SIZE次,且R的奇异值大于0.25才认为标定成功
    if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
    {
        calib_ric_result = ric;
        return true;
    }
    else
        return false;
}

Appendix

  • The product of two quaternions is bi-linear and can be expressed as two equivalent matrix products, namely

\[\mathbf{q}_{1} \otimes \mathbf{q}_{2}=\left[\mathbf{q}_{1}\right]_{L} \mathbf{q}_{2} \quad \text { and } \quad \mathbf{q}_{1} \otimes \mathbf{q}_{2}=\left[\mathbf{q}_{2}\right]_{R} \mathbf{q}_{1}, \]

\(\quad\)where \([\mathbf{q}]_{L}\) and \([\mathbf{q}]_{R}\) are respectively the left- and right- quaternion-product matrices

\[[\mathbf{q}]_{L}=\left[\begin{array}{cccc} q_{w} & -q_{x} & -q_{y} & -q_{z} \\ q_{x} & q_{w} & -q_{z} & q_{y} \\ q_{y} & q_{z} & q_{w} & -q_{x} \\ q_{z} & -q_{y} & q_{x} & q_{w} \end{array}\right], \quad[\mathbf{q}]_{R}=\left[\begin{array}{cccc} q_{w} & -q_{x} & -q_{y} & -q_{z} \\ q_{x} & q_{w} & q_{z} & -q_{y} \\ q_{y} & -q_{z} & q_{w} & q_{x} \\ q_{z} & q_{y} & -q_{x} & q_{w} \end{array}\right], \]

\(\quad\)then

\[[\mathbf{q}]_{L}=q_{w} \mathbf{I}+\left[\begin{array}{cc} 0 & -\mathbf{q}_{v}^{\top} \\ \mathbf{q}_{v} & {\left[\mathbf{q}_{v}\right]_{\times}} \end{array}\right], \quad[\mathbf{q}]_{R}=q_{w} \mathbf{I}+\left[\begin{array}{cc} 0 & -\mathbf{q}_{v}^{\top} \\ \mathbf{q}_{v} & -\left[\mathbf{q}_{v}\right]_{\times} \end{array}\right] \]

标签:初始化,外参,right,mathbf,begin,ric,VINS,array,left
From: https://www.cnblogs.com/weihao-ysgs/p/vins-parameter-init.html

相关文章

  • 链栈的定义、初始化、出栈、入栈等操作
    #include<iostream>usingnamespacestd;/*链栈的定义*/typedefstructsNode{chardata;structsNode*next;}sNode;typedefsNode*linkStack;/*初始化链栈*/voidinitStack_L(linkStack&S){S=newsNode;S->next=NULL;}/*建立一个链栈......
  • 顺序栈的定义、初始化、出栈、入栈等操作 C++代码实现
     #include<iostream>usingnamespacestd;/*顺序栈的定义*/#defineStack_Size100typedefstructsqStack{char*elem;inttop;intstackSize;//栈数组长度}sqStack;/*顺序栈的初始化*/voidinitStack_Sq(sqStack&S){S.elem=ne......
  • 双链表的定义、初始化、插入、删除,C++代码实现的算法
    #include<iostream>usingnamespacestd;/*双向链表类型定义*/typedefstructduNode{chardata;structduNode*prior;structduNode*next;}duNode;typedefduNode*duLinklist;//指针类型,故访问它的成员用“->”。/*初始化双向链表*/voidinitLinkl......
  • 顺序表的定义、初始化、及插入、删除、查询操作,将算法转化成具体的代码
    #include<iostream>usingnamespacestd;#defineLIST_INIT_SIZE100#defineLISTINCREMENT10intOK=1;intOVERFLOW=0;intERROR=0;/*线性表的定义*/typedefstruct{char*elem;intlength;//当前长度intlistsize;//线性表的长度}S......
  • 类定义、属性、初始化和析构知识点总结
    一:前言:为什么要学类?   类是一个独立存放变量(属性/方法)的空   1.简化代码,提升效率,避免代码重复写入。如用户注册、校验、登录方法可以放在一个类中,需要哪个方法就调用哪个类===》建立模型框架(建立一个方法)===》很多鼠标(实例化)===》都是独立的2.面向对象:直接给我......
  • C++构造函数、析构函数、初始化列表
    构造函数构造函数就是与类名同名的成员函数,当实例化对象时它会自动执行,当构造函数执行结束后,对象才完成实例化任务:一般负责对类对象进行初始化、资源分配class类名{int*p;public:类名(参数){p=newint;}}......
  • unordered_set 的初始化方法
    unordered_set是一个哈希表的实现,因此初始化其实就是给它分配一定的空间,并且指定哈希表中每个元素的存储方式。unordered_set的初始化方式有以下几种:无参构造函数std::unordered_set<int>mySet;默认情况下,unordered_set会分配一定的内存,并且使用默认的哈希函数和比较函......
  • 类和对象(对象的初始化和清理)
    对象的初始化和清理是两个非常重要的安全问题:一个对象或者变量没有初始状态,对其使用后果是未知的。同样的使用完一个对象或者变量,没有及时清理,也会造成一定的安全问题。c++利用构造函数和析构函数解决上述问题。构造函数:主要作用在于创建对象时为对象的成员属性赋值,构造函数由编译......
  • C++ 成员初始化列表
    成员初始化列表成员初始化列表时对象初始化成员变量的一种方法。使用方式如下(两种书写习惯)ClassName(args):varName1(value),...,varNameN(value){//这是一个构造函数//...CODE...}ClassName(args):varName1(value),...,varNameN(value){//这......
  • C++ 变量初始化总结
    堆空间,new操作初始化1、对于有自己写构造函数的类,不论类型名后面有没有括号()或者数组[],都用构造函数进行初始化,如果构造函数delete,则编译报错;2、如果没有构造函数,则不加括号的new只分配内存空间,不进行内存的初始化,3、而加了括号()的new会在分配内存的同时初始化为0。栈空间......