LearnVIORB代码解析

主要是各个公式与代码之间的变换：

IMU器件的测量模型：

是陀螺仪实际的旋转值，是陀螺仪的测量值，在测量值与真实值之间相差bias 和噪声

aw 是物体的加速度在世界座标系下的值，换言之，也可以理解为肉眼可见的加速度，当物体放在桌子上静止时，aw为0，而这时加速度计却能测量到有一个与重力方向相反的力作用于物体，也就是桌子对于物体的向上的支撑力； gw 是世界座标系下重力加速度， aw - gw 就是除重力外实际的运动加速度，由世界座标经由R转换到b座标系下，加上噪声和偏差的影响后是最后的加速度计的测量值；

运动模型的微分方程如下：

使用欧拉积分，得到运动方程的离散形式如下：

其中表示t时刻角速度矢量在b座标系下的座标，表示旋转矢量在b系下的座标，

这里EXP() 罗德里格斯公式将旋转矢量转换至t时刻；

这里的ω，a 都是理想值，因此将IMU模型的运动方程带入后，再经过化简，即可得到如下：

下划线的都是测量值，即我们从ＩＭＵ中直接读出的数据；
1. 第一部分：ＩＭＵ的预积分

这里需要注意的是　这部分仅仅是ＩＭＵ的预积分而已，并没有涉及到其他陀螺仪或者加速度计的偏差计算等，只是对p，v，r的imu数据进行一个预先积分：
预积分噪声的协方差形式：

如上是最后的公式，其中deltaR 是旋转的预积分变化量， (f - b) 是加速度的测量值

对应到代码部分，是文件IMUPreintegrator.cpp中的update()函数：

// noise covariance propagation of delta measurements
    // err_k+1 = A*err_k + B*err_gyro + C*err_acc
    Matrix3d I3x3 = Matrix3d::Identity();
    Matrix<double,9,9> A = Matrix<double,9,9>::Identity();
    A.block<3,3>(6,6) = dR.transpose();
    A.block<3,3>(3,6) = -_delta_R*skew(acc)*dt;
    A.block<3,3>(0,6) = -0.5*_delta_R*skew(acc)*dt2;
    A.block<3,3>(0,3) = I3x3*dt;
    Matrix<double,9,3> Bg = Matrix<double,9,3>::Zero();
    Bg.block<3,3>(6,0) = Jr*dt;
    Matrix<double,9,3> Ca = Matrix<double,9,3>::Zero();
    Ca.block<3,3>(3,0) = _delta_R*dt;
    Ca.block<3,3>(0,0) = 0.5*_delta_R*dt2;
    _cov_P_V_Phi = A*_cov_P_V_Phi*A.transpose() +
        Bg*IMUData::getGyrMeasCov()*Bg.transpose() +
        Ca*IMUData::getAccMeasCov()*Ca.transpose();

这里与公式有一些的差别，主要是矩阵快的顺序，代码中矩阵A,B的形式如下：

而这里计算出的协方差矩阵将用于后期的优化时，计算出信息矩阵进行边缘化：

设置信息矩阵代码：

Matrix9d CovPRV = pKF1->GetIMUPreInt().getCovPVPhi(); //获得协方差
            CovPRV.col(3).swap(CovPRV.col(6));
            CovPRV.col(4).swap(CovPRV.col(7));
            CovPRV.col(5).swap(CovPRV.col(8));
            CovPRV.row(3).swap(CovPRV.row(6));
            CovPRV.row(4).swap(CovPRV.row(7));
            CovPRV.row(5).swap(CovPRV.row(8));
            epvr->setInformation(CovPRV.inverse());

接下来说PVR的预积分更新，首先预积分更新时公式如下：

因为k+1 时刻p的更新需要 k时刻的v和R，因此在代码中率先更新了p：同样也在预积分文件：

void IMUPreintegrator::update(const Vector3d& omega, const Vector3d& acc, const double& dt)中
_delta_P += _delta_V*dt + 0.5*_delta_R*acc*dt2;    // P_k+1 = P_k + V_k*dt + R_k*a_k*dt*dt/2
_delta_V += _delta_R*acc*dt;
_delta_R = normalizeRotationM(_delta_R*dR);  // normalize rotation, in case of numerical error accumulation

注意这里有一个地方跟公式中的不一样，就是公式中陀螺仪和加速计的测量值 ωk 和 fk 都没有像公式中一样减去偏差和噪声，而是直接使用了acc加速度和 _deltaR旋转角度，

这是因为在update() 函数调用的时候出入的参数即为减去误差的参数，如Frame.cc中的调用：
IMUPreInt.update(imu._g - bg, imu._a - ba, dt); 其中bg和ba即为陀螺仪和加速度即的偏差；

综上总结，类IMUPreintegrator主要是用于计算当前帧所对应的所有IMU数据的预积分量，这些预积分量表明的是从上一帧到当前帧的位姿变化和速度，这里的位姿包括了旋转R，位移p，既然是位姿变换，所以应该是表示为DeltaR 和DeltaP，同时还要deltaV；

IMUPreintegrator::update() 只是利用一组IMU数据进行增量的更新，完整的增量计算过程在作者新定义的类KeyFrameInit中

ComputePreInt() 函数，具体的执行过程如下：

mIMUPreInt.reset();  // 先对当前帧对应的预积分进行重置，就是将各个变量都设置为0
if(mvIMUData.empty())
    return;
// remember to consider the gap between the last KF and the first IMU
{
   const IMUData& imu = mvIMUData.front();
   double dt = std::max(0., imu._t - mpPrevKeyFrame->mTimeStamp);
   mIMUPreInt.update(imu._g - bg,imu._a ,dt);  // Acc bias not considered here
}
// integrate each imu  叠加关键帧与关键帧之间的所有imu数据，
for(size_t i=0; i<mvIMUData.size(); i++)
{
    const IMUData& imu = mvIMUData[i];
    double nextt;
    if(i==mvIMUData.size()-1)
         nextt = mTimeStamp;         // last IMU, next is this KeyFrame
    else
         nextt = mvIMUData[i+1]._t;  // regular condition, next is imu data
    // delta time
    double dt = std::max(0., nextt - imu._t);
    // update pre-integrator
    mIMUPreInt.update(imu._g - bg,imu._a ,dt);
}

计算出的位姿增量和速度增量都保存在该关键帧对应的预积分变量中：

_delta_P   _delta_V  _delta_R   _cov_P_V_Phi    _delta_time
_J_P_Biasg   _J_P_Biasa   _J_V_Biasg   _J_V_Biasa    _J_R_Biasg

接下来就是对增量的应用：

以ｉｍｕ的初始化为例，在localMapping 中TryInitVIO()实现了对以上增量的应用，初始化分为四个步骤：

1. 利用两个关键帧之间由视觉计算出的旋转增量，与IMU计算出的旋转增量，_delta_P 之间进行优化，最小化两者之间的差值来计算出陀螺仪的偏差biasg， (这里当然是假设在这两个关键帧之间biasg是常量)，这部分对应的就是论文中的公式9；

2. 已经获得了陀螺仪的偏差，接下来要对scale和重力进行计算，这里假设的是没有加速度偏差biasa，因此在下面的公式中也就自然把前面公式中的ba 进行了省略；因为单目是没有正确的scale的，因此其计算出的相机的位姿中的p，也就是T中的最后一列的量其实是与实际世界中的位移量差一个scale的，因此这里了计算scale主要用到的就是IMU预积分得到的位移项p，也就是论文中公式3的最后一个式子以及速度的迭代公式，得到公式11；接下来就是解线性方程，对三个关键帧之间的两组IMU数据进行计算求解出scale 和gw ，具体的公式如下：

这里主要强调的pc2，pc1 就是关键帧2和1对应的位移，需要从视觉计算出的T中获取；而带△的△ p，△ v，都是之前IMU预积分数据计算出的增量，这两个的区别也可以从其对应的代码看出，如下：

//position of camera center
        cv::Mat pc1 = Twc1.rowRange(0,3).col(3);
        cv::Mat pc2 = Twc2.rowRange(0,3).col(3);
        cv::Mat pc3 = Twc3.rowRange(0,3).col(3);
// Pre-integrated measurements
        cv::Mat dp12 = Converter::toCvMat(pKF2->mIMUPreInt.getDeltaP());
        cv::Mat dv12 = Converter::toCvMat(pKF2->mIMUPreInt.getDeltaV());
        cv::Mat dp23 = Converter::toCvMat(pKF3->mIMUPreInt.getDeltaP());

接下来就是按照AX=B的公式分别计算A和B，进行SVD分解即可；

        cv::Mat lambda = (pc2-pc1)*dt23 + (pc2-pc3)*dt12;
        cv::Mat beta = 0.5*I3*(dt12*dt12*dt23 + dt12*dt23*dt23);
        cv::Mat gamma = (Rc3-Rc2)*pcb*dt12 + (Rc1-Rc2)*pcb*dt23 + Rc1*Rcb*dp12*dt23 - Rc2*Rcb*dp23*dt12 - Rc1*Rcb*dv12*dt12*dt23;
        lambda.copyTo(A.rowRange(3*i+0,3*i+3).col(0));
        beta.copyTo(A.rowRange(3*i+0,3*i+3).colRange(1,4));
        gamma.copyTo(B.rowRange(3*i+0,3*i+3));

SVD最后计算出的结果保存在sstar 和 gwstar中，分别表示尺度和重力加速度的方向；

3. 正如步骤2中的所说，假设是加速度偏差为0，但是实际上加速度的偏差并不为0，且和重力加速度的方向难以区分，因此第三步主要是用于计算加速度的偏差，以及重新计算重力和尺度；

这里引入一个常量，重力加速度的大小，在地球上基本都是确定的G = 9.8，同时引入一个新的座标系惯性座标系 I . 假设在惯性座标系中存在重力加速度的方向为 gI = {0，0，1}，同时我们上一步已经求出了重力加速度在世界座标系中的向量表示gwstar，接下来可以求出两个座标系之间的关系，主要是一个旋转量，假设从惯性座标系到世界座标系中存在旋转矩阵RwI ，同时旋转也可以用一个旋转轴和一个旋转角度表示；如下：

其中vhat即为旋转轴，theta为旋转角度，两者对应的公式如下：

对于旋转轴和旋转角度的理解参考向量的叉乘和点乘: 求两个向量旋转时的旋转轴，也就是同时垂直于两个向量的单位向量，因此即为gi 和 gw 叉乘后归一化的量；

同时若两个向量之间的夹角为theta，则两个向量的内积公式为 gi * gw *cos(theta) , 叉乘公式为 gi*gw*sin(theta) 因此 theta = arctan( sin(theta)/cos(theta) ) = 上面theta的公式；

有了旋转轴和旋转向量，即可求出两个座标系之间的旋转矩阵

接下重新计算重力加速度 gw；gw可以表示为：

在对旋转Rwi进行优化时，可以加入扰动delta_theta, 因为绕z轴的旋转与重力加速度gw的方向一致，因此其实对gw没有影响，因此扰动只出现在x，y方向上，所以也可以表示成如下的形式：

将上式进行变换后即可重新带入式11中进行计算如下：

所有未知量和已知量都在公式18中，与第二步不同的是这里未知量的个数为6个，这里同样用三个连续帧之间的IMU数据进行SVD分解，分别求出scale* , thetaxy*, biasa*;

代码部分：

第一部分，求出惯性座标系与世界座标系之间的旋转矩阵Rwi ：

// Use gravity magnitude 9.8 as constraint
    // gI = [0;0;1], the normalized gravity vector in an inertial frame, NED type with no orientation.
    cv::Mat gI = cv::Mat::zeros(3,1,CV_32F);
    gI.at<float>(2) = 1;
    // Normalized approx. gravity vecotr in world frame
    cv::Mat gwn = gwstar/cv::norm(gwstar);
    // Debug log
    //cout<<"gw normalized: "<<gwn<<endl;
    // vhat = (gI x gw) / |gI x gw|
    cv::Mat gIxgwn = gI.cross(gwn);
    double normgIxgwn = cv::norm(gIxgwn);
    cv::Mat vhat = gIxgwn/normgIxgwn;  //旋转轴
    double theta = std::atan2(normgIxgwn,gI.dot(gwn)); //旋转角度 arctan(sin(theta) / cos(theta))

第二部分：按照公式计算C和D，组成方程Cx = D然后进行SVD分解：

cv::Mat lambda = (pc2-pc1)*dt23 + (pc2-pc3)*dt12;
cv::Mat phi = - 0.5*(dt12*dt12*dt23 + dt12*dt23*dt23)*Rwi*SkewSymmetricMatrix(GI);  
// note: this has a '-', different to paper
cv::Mat zeta = Rc2*Rcb*Jpba23*dt12 + Rc1*Rcb*Jvba12*dt12*dt23 - Rc1*Rcb*Jpba12*dt23;
cv::Mat psi = (Rc1-Rc2)*pcb*dt23 + Rc1*Rcb*dp12*dt23 - (Rc2-Rc3)*pcb*dt12
              - Rc2*Rcb*dp23*dt12 - Rc1*Rcb*dv12*dt23*dt12 - 0.5*Rwi*GI*(dt12*dt12*dt23 
              + dt12*dt23*dt23);

SVD分解后可以获得dthetaxy和dbiasa_ 与之前的Rwi相乘，得到惯性系与世界座标系的旋转关系 Rwi_ ，利用旋转矩阵与gi 即可求出最终的重力加速度在世界座标系下的座标；

cv::Mat dthetaxy = y.rowRange(1,3);
cv::Mat dbiasa_ = y.rowRange(3,6);
Vector3d dbiasa_eig = Converter::toVector3d(dbiasa_);

// dtheta = [dx;dy;0]
cv::Mat dtheta = cv::Mat::zeros(3,1,CV_32F);
dthetaxy.copyTo(dtheta.rowRange(0,2));
Eigen::Vector3d dthetaeig = Converter::toVector3d(dtheta);

// Rwi_ = Rwi*exp(dtheta)
Eigen::Matrix3d Rwieig_ = RWIeig*Sophus::SO3::exp(dthetaeig).matrix();
cv::Mat Rwi_ = Converter::toCvMat(Rwieig_);

// gravity vector in world frame
cv::Mat gw = Rwi_*GI

4. 速度估计；

将vb从上述等式中提出来，同时忽略掉扰动项delta_theta的影响，得到公式对应的代码如下：

// 公式(3)中式3的变形，其中 wPb - wPbnext = scale*(wPc - wPcnext) + (Rwc - Rwcnext)*pcb
 cv::Mat vel = - 1./dt*( scale*(wPc - wPcnext) + (Rwc - Rwcnext)*pcb + Rwc*Rcb*(dp + Jpba*dbiasa_) + 0.5*gw*dt*dt );

至此，IMU完成初始化的部分，接下来主要就是更新之前的单目计算出的关键帧的位姿，都乘以计算出的scale，同时重新计算所有关键帧对应的预积分；

vector<KeyFrame*> mspKeyFrames = mpMap->GetAllKeyFrames();
for(std::vector<KeyFrame*>::iterator sit=mspKeyFrames.begin(), send=mspKeyFrames.end(); sit!=send; sit++)

更新地图点：

vector<MapPoint*> mspMapPoints = mpMap->GetAllMapPoints();
for(std::vector<MapPoint*>::iterator sit=mspMapPoints.begin(), send=mspMapPoints.end(); sit!=send; sit++)

最后是全局优化，然后再次更新变量；

LearnVIORB代码解析

cartographer編譯過程遇到未定義的dlclose@@GLIBC_2.2.5

【從理論到代碼】旋轉矩陣與歐拉角一

Unknown CMake command "rosbuild_add_executable".

SLAM中直接法分類及對應的項目

ORB_SLAM2中的疑難雜症

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結