从角速度到旋转矩阵和Jacobian

ImuTypes.cc

IntegratedRotation::IntegratedRotation(const Eigen::Vector3f &angVel, const Bias &imuBias, const float &time) {
    //这个方法主要是构建 旋转测量量，并未开始计算积分，公式 15-29
    // delta_R_ij(测量) = 连乘 Exp(( w - b) * delta_t)


    // ( w - b)
    const float x = (angVel(0)-imuBias.bwx)*time;//公式 15-29 的 （w-b）* delta_t
    const float y = (angVel(1)-imuBias.bwy)*time;
    const float z = (angVel(2)-imuBias.bwz)*time;

    //distance用来判断是否要用罗德里格斯公式，或是用简化公式
    const float d2 = x*x+y*y+z*z;// distance的平方
    const float d = sqrt(d2); //distance

    Eigen::Vector3f v;
    v << x, y, z;


    Eigen::Matrix3f W = Sophus::SO3f::hat(v); //公式 15-1 将向量转成反对称矩阵
    if(d<eps)//旋转角度很小，distance < 一个很小的量，所以W也是很小的值 ，这个时候罗德里格斯公式和以下的几乎一样
    {
        deltaR = Eigen::Matrix3f::Identity() + W;
        rightJ = Eigen::Matrix3f::Identity();
    }
    else
    { // 将反对称矩阵转换成旋转向量
        deltaR = Eigen::Matrix3f::Identity() + W*sin(d)/d + W*W*(1.0f-cos(d))/d2; //罗德里格斯公式，公式 15-3
        rightJ = Eigen::Matrix3f::Identity() - W*(1.0f-cos(d))/d2 + W*W*(d-sin(d))/(d2*d);//右雅克比公式，公式 15-8
    }
}