5. ORB-SLAM3源码阅读笔记 -ImuTypes

该文件包含的类主要是用来处理imu数据、标定的外参和噪声、预积分，以及一些李群李代数常用函数。下面一一介绍。很多用到的公式都来自这篇经典论文(以下简称forster论文)：

1、Point类

//imu加速度、角速度、时间戳数据
class Point
{
public:
    Point(const float &acc_x, const float &acc_y, const float &acc_z,
             const float &ang_vel_x, const float &ang_vel_y, const float &ang_vel_z,
             const double &timestamp): a(acc_x,acc_y,acc_z), w(ang_vel_x,ang_vel_y,ang_vel_z), t(timestamp){}
    Point(const cv::Point3f Acc, const cv::Point3f Gyro, const double &timestamp):
        a(Acc.x,Acc.y,Acc.z), w(Gyro.x,Gyro.y,Gyro.z), t(timestamp){}
public:
    cv::Point3f a;//加速度
    cv::Point3f w;//角速度
    double t;//时间戳
};

2、Bias类

class Bias
{
    //序列化，将一个类的实例转换成二进制数据流或XML格式。也可以通过反序列化得到原来的类实例
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive & ar, const unsigned int version)
    {
        ar & bax;
        ar & bay;
        ar & baz;
        ar & bwx;
        ar & bwy;
        ar & bwz;
    }
public:
    Bias():bax(0),bay(0),baz(0),bwx(0),bwy(0),bwz(0){}
    Bias(const float &b_acc_x, const float &b_acc_y, const float &b_acc_z,
            const float &b_ang_vel_x, const float &b_ang_vel_y, const float &b_ang_vel_z):
            bax(b_acc_x), bay(b_acc_y), baz(b_acc_z), bwx(b_ang_vel_x), bwy(b_ang_vel_y), bwz(b_ang_vel_z){}
    void CopyFrom(Bias &b);//复制b
    friend std::ostream& operator<< (std::ostream &out, const Bias &b);//将运算符<<重载，使bias可以通过<<符号读取数据
public:
    float bax, bay, baz;//加速度三个方向的bias
    float bwx, bwy, bwz;//角速度三个方向的bias
};

3、Calib类

public:
    Calib(const cv::Mat &Tbc_, const float &ng, const float &na, const float &ngw, const float &naw)
    {Set(Tbc_,ng,na,ngw,naw);}
    Calib(const Calib &calib);
    Calib(){}
    void Set(const cv::Mat &Tbc_, const float &ng, const float &na, const float &ngw, const float &naw);
public:
    cv::Mat Tcb;//imu坐标系到相机坐标系的变换矩阵
    cv::Mat Tbc;//相机坐标系到imu坐标系的变换矩阵
    cv::Mat Cov, CovWalk;//imu数据的高斯噪声协方差(6*6)、imu的bias随机游走的协方差(6*6)，这两个都是由标定得到 

void Calib::Set(const cv::Mat &Tbc_, const float &ng, const float &na, const float &ngw, const float &naw)
{
    //相机->imu的变换矩阵
    Tbc = Tbc_.clone();
    //imu->相机的变换矩阵
    Tcb = cv::Mat::eye(4,4,CV_32F);
    Tcb.rowRange(0,3).colRange(0,3) = Tbc.rowRange(0,3).colRange(0,3).t();
    Tcb.rowRange(0,3).col(3) = -Tbc.rowRange(0,3).colRange(0,3).t()*Tbc.rowRange(0,3).col(3);
    Cov = cv::Mat::eye(6,6,CV_32F);//na、ng、naw、ngw都是标定的imu参数
    const float ng2 = ng*ng;       //用它们来构造协方差矩阵
    const float na2 = na*na;       //后面预积分时会用到
    Cov.at<float>(0,0) = ng2;
    Cov.at<float>(1,1) = ng2;
    Cov.at<float>(2,2) = ng2;
    Cov.at<float>(3,3) = na2;
    Cov.at<float>(4,4) = na2;
    Cov.at<float>(5,5) = na2;
    CovWalk = cv::Mat::eye(6,6,CV_32F);
    const float ngw2 = ngw*ngw;
    const float naw2 = naw*naw;
    CovWalk.at<float>(0,0) = ngw2;
    CovWalk.at<float>(1,1) = ngw2;
    CovWalk.at<float>(2,2) = ngw2;
    CovWalk.at<float>(3,3) = naw2;
    CovWalk.at<float>(4,4) = naw2;
    CovWalk.at<float>(5,5) = naw2;
}

4、IntegratedRotation类

//这个类是为了根据角速度计算deltaR(单位时间内的旋转量)以及相应的右乘雅克比rightJ
IntegratedRotation::IntegratedRotation(const cv::Point3f &angVel, const Bias &imuBias, const float &time):
    deltaT(time)
{
    //角速度减去偏置，再乘上时间，就是该时间内的旋转，xyz构成旋转向量
    const float x = (angVel.x-imuBias.bwx)*time;
    const float y = (angVel.y-imuBias.bwy)*time;
    const float z = (angVel.z-imuBias.bwz)*time;
    cv::Mat I = cv::Mat::eye(3,3,CV_32F);
    //计算旋转向量的模值
    const float d2 = x*x+y*y+z*z;
    const float d = sqrt(d2);
    //旋转向量(so3)写成反对称形式
    cv::Mat W = (cv::Mat_<float>(3,3) << 0, -z, y,
                 z, 0, -x,
                 -y,  x, 0);
    // eps = 1e-4 是一个小量，在ImuTypes.cc文件开头有定义
    if(d<eps)//旋转比较小，旋转向量到旋转矩阵的指数映射采用一阶近似
    {
        //forster 经典预积分论文公式（4）
        deltaR = I + W;
        //小量时，右扰动 Jr = I
        rightJ = cv::Mat::eye(3,3,CV_32F);
    }
    else
    {
        //forster 经典预积分论文公式（3）
        deltaR = I + W*sin(d)/d + W*W*(1.0f-cos(d))/d2;
        //forster 经典预积分论文公式（8）
        rightJ = I - W*(1.0f-cos(d))/d2 + W*W*(d-sin(d))/(d2*d);
    }
}

5、Preintegrated类(预积分)

Preintegrated::Preintegrated(const Bias &b_, const Calib &calib)//构造函数
{
    Nga = calib.Cov.clone();//从标定得到的imu数据协方差矩阵
    NgaWalk = calib.CovWalk.clone();//从标定得到的bias随机游走协方差矩阵
    Initialize(b_);//根据bias初始化预积分相关的变量，bias改变时预积分会改变
}
void Preintegrated::Initialize(const Bias &b_)
{
    dR = cv::Mat::eye(3,3,CV_32F);//旋转预积分初始值为单位阵
    dV = cv::Mat::zeros(3,1,CV_32F);//速度预积分初始为0
    dP = cv::Mat::zeros(3,1,CV_32F);//位置预积分初始为0
    JRg = cv::Mat::zeros(3,3,CV_32F);//旋转预积分对delta(bg)的雅克比，在bias改变后预积分量一阶近似更新模型中使用
    JVg = cv::Mat::zeros(3,3,CV_32F);//速度预积分对delta(bg)的雅克比
    JVa = cv::Mat::zeros(3,3,CV_32F);//速度预积分对delta(ba)的雅克比
    JPg = cv::Mat::zeros(3,3,CV_32F);//位置预积分对delta(bg)的雅克比
    JPa = cv::Mat::zeros(3,3,CV_32F);//位置预积分对delta(ba)的雅克比
    C = cv::Mat::zeros(15,15,CV_32F);//协方差传递所需的A,B矩阵，A为左上角9*9，B为右下角6*6
    Info=cv::Mat();//信息矩阵(协方差矩阵的逆)
    db = cv::Mat::zeros(6,1,CV_32F);//bias的变化量
    b=b_;
    bu=b_;//updated bias
    avgA = cv::Mat::zeros(3,1,CV_32F);//平均加速度，用于判断加速度是否变化
    avgW = cv::Mat::zeros(3,1,CV_32F);//平均角速度
    dT=0.0f;//时间间隔
    mvMeasurements.clear();
}

利用新的imu数据更新预积分量

void Preintegrated::IntegrateNewMeasurement(const cv::Point3f &acceleration, const cv::Point3f &angVel, const float &dt)
{
    //将imu数据构造成一个integrable结构体，保存到mvMeasurements中
    mvMeasurements.push_back(integrable(acceleration,angVel,dt));
    // Position is updated firstly, as it depends on previously computed velocity and rotation.
    // Velocity is updated secondly, as it depends on previously computed rotation.
    // Rotation is the last to be updated.
    //计算协方差传递所需的A、B矩阵，下面的计算详见forster论文附录A.7~A.9
    cv::Mat A = cv::Mat::eye(9,9,CV_32F);
    cv::Mat B = cv::Mat::zeros(9,6,CV_32F);
    // 减去偏置后的加速度、角速度
    cv::Mat acc = (cv::Mat_<float>(3,1) << acceleration.x-b.bax,acceleration.y-b.bay, acceleration.z-b.baz);
    cv::Mat accW = (cv::Mat_<float>(3,1) << angVel.x-b.bwx, angVel.y-b.bwy, angVel.z-b.bwz);
    // 计算平均加速度和角速度
    avgA = (dT*avgA + dR*acc*dt)/(dT+dt);
    avgW = (dT*avgW + accW*dt)/(dT+dt);
    //更新P、V的预积分量，forster论文公式(26)
    dP = dP + dV*dt + 0.5f*dR*acc*dt*dt;
    dV = dV + dR*acc*dt;
    // Compute velocity and position parts of matrices A and B (rely on non-updated delta rotation)
    cv::Mat Wacc = (cv::Mat_<float>(3,3) << 0, -acc.at<float>(2), acc.at<float>(1),
                                                   acc.at<float>(2), 0, -acc.at<float>(0),
                                                   -acc.at<float>(1), acc.at<float>(0), 0);
    A.rowRange(3,6).colRange(0,3) = -dR*dt*Wacc;
    A.rowRange(6,9).colRange(0,3) = -0.5f*dR*dt*dt*Wacc;
    A.rowRange(6,9).colRange(3,6) = cv::Mat::eye(3,3,CV_32F)*dt;
    B.rowRange(3,6).colRange(3,6) = dR*dt;
    B.rowRange(6,9).colRange(3,6) = 0.5f*dR*dt*dt;
    // Update position and velocity jacobians wrt bias correction
    // 预积分量对bias变化量的雅克比，ORB-SLAM3对forster论文附录A.20做了变形，递归更新，详见 https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/212
    JPa = JPa + JVa*dt -0.5f*dR*dt*dt;
    JPg = JPg + JVg*dt -0.5f*dR*dt*dt*Wacc*JRg;
    JVa = JVa - dR*dt;
    JVg = JVg - dR*dt*Wacc*JRg;
    // Update delta rotation
    //对角速度进行积分，得到旋转变化量
    IntegratedRotation dRi(angVel,b,dt);
    // 旧的旋转预积分量乘上旋转变化量，归一化使其符合旋转矩阵的格式
    dR = NormalizeRotation(dR*dRi.deltaR);
    // Compute rotation parts of matrices A and B
    A.rowRange(0,3).colRange(0,3) = dRi.deltaR.t();
    B.rowRange(0,3).colRange(0,3) = dRi.rightJ*dt;
    // Update covariance
    C.rowRange(0,9).colRange(0,9) = A*C.rowRange(0,9).colRange(0,9)*A.t() + B*Nga*B.t();
    C.rowRange(9,15).colRange(9,15) = C.rowRange(9,15).colRange(9,15) + NgaWalk;
    // Update rotation jacobian wrt bias correction
    JRg = dRi.deltaR.t()*JRg - dRi.rightJ*dt;
    // Total integrated time
    dT += dt;
}

bias更新后预积分量利用一阶近似模型更新，减小计算量(forster论文公式(36))

//bias更新之后新的旋转预积分(一阶线性模型)
cv::Mat Preintegrated::GetDeltaRotation(const Bias &b_)
{
    std::unique_lock<std::mutex> lock(mMutex);
    // 计算偏置的变化量
    cv::Mat dbg = (cv::Mat_<float>(3,1) << b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz);
    return NormalizeRotation(dR*ExpSO3(JRg*dbg));//一阶近似更新
}
//bias更新之后新的速度预积分(一阶线性模型)
cv::Mat Preintegrated::GetDeltaVelocity(const Bias &b_)
{
    std::unique_lock<std::mutex> lock(mMutex);
    cv::Mat dbg = (cv::Mat_<float>(3,1) << b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz);
    cv::Mat dba = (cv::Mat_<float>(3,1) << b_.bax-b.bax,b_.bay-b.bay,b_.baz-b.baz);
    return dV + JVg*dbg + JVa*dba;//一阶近似更新
}
//bias更新之后新的平移预积分(一阶线性模型)
cv::Mat Preintegrated::GetDeltaPosition(const Bias &b_)
{
    std::unique_lock<std::mutex> lock(mMutex);
    cv::Mat dbg = (cv::Mat_<float>(3,1) << b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz);
    cv::Mat dba = (cv::Mat_<float>(3,1) << b_.bax-b.bax,b_.bay-b.bay,b_.baz-b.baz);
    return dP + JPg*dbg + JPa*dba;//一阶近似更新
}