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

最後是全局優化，然後再次更新變量；