3——IMU預(yù)積分

論文內(nèi)容：

1.??????論文第IV點的B部分IMU預(yù)積分，

??? IMU預(yù)積分的作用是計算出IMU數(shù)據(jù)的觀測值（就是IMU預(yù)積分值）以及殘差的協(xié)方差矩陣和雅各比矩陣，那就要清楚的明白為什么要計算這三個量？計算出這三個量為什么就可以和視覺觀測值進行耦合？如果你現(xiàn)在回答不出來，請好好想一想自己以前學(xué)到的知識，關(guān)于視覺的這三個量，視覺中觀測值是用來計算殘差的（也就是誤差），殘差的雅各比矩陣是優(yōu)化中下降的方向（也就是梯度），很少提及的協(xié)方差矩陣（但很重要）其實是觀測值對應(yīng)的權(quán)值（因為有很多觀測值），現(xiàn)在是不是很清楚明白了？具體使用來說，這三個量為后面的聯(lián)合初始化提供初值以及后端優(yōu)化提供IMU的約束關(guān)系。原始陀螺儀和加速度計的觀測值數(shù)據(jù)：

第一個式子等式左邊是加速度測量值（你可以從加速度計中讀到的值），等式右邊是加速度真實值（其實就是準(zhǔn)確的值，我們需要得到的是這個真實值）加上加速度計的偏置、重力加速度和加速度噪聲項。第二個式子等式左邊是陀螺儀測量值，等式右邊是陀螺儀真實值加上陀螺儀偏置和陀螺儀噪聲項。這里的值都是IMU（body）幀坐標(biāo)系下的。這里假設(shè)噪聲是服從高斯正態(tài)分布，而偏置服從隨機游走模型。

???? 由上面最原始的式子積分就可以計算出下一時刻的p、v和q:

這里等式左邊的值都是世界坐標(biāo)系下（W）bk+1幀的值。從整個式子可以看出來這里的狀態(tài)傳播需要bk幀下的旋轉(zhuǎn)，平移和速度，當(dāng)這些開始的狀態(tài)發(fā)生改變的時候，就需要重新傳播IMU觀測值，也就是說狀態(tài)傳播方程要重新計算和修改，我們想要一次性就求出bk和bk+1之間的狀態(tài)傳播，因此選用預(yù)積分模型（其實這里我也沒有完全搞明白，但是有一點是明白的，這里是在世界坐標(biāo)系下求解狀態(tài)，但是由條件里需要世界坐標(biāo)系下的旋轉(zhuǎn)，明顯沖突啊，因此可以使用預(yù)積分將世界坐標(biāo)系下的狀態(tài)轉(zhuǎn)換到IMU的bk幀坐標(biāo)系下，最初提出預(yù)積分的外國大牛市將世界坐標(biāo)系轉(zhuǎn)換到求狀態(tài)的變化量，其實兩者的原理都一樣，預(yù)積分求的值都是變化量），兩邊同時乘以世界到bk幀坐標(biāo)系的轉(zhuǎn)換，如下圖所示，然后提出等式右邊只與加速度和角速度有關(guān)的量進行積分，如公式5和6

到這里其實只要求出公式6中的積分值，真的預(yù)積分的值就得到了，這里bk是參考幀，從式6中可以看出，在bk到bk+1幀間，這里要求的三個臨時狀態(tài)量只與IMU的偏置有關(guān)系，而與其他狀態(tài)無關(guān)，也就是說每個式子相當(dāng)于一個二元一次方程（f(x,y)=ax+by+c,x相當(dāng)于加速度計偏置，y相當(dāng)于陀螺儀偏置），這里就是為了求解這個二元一次方程，當(dāng)這里的偏置變化特別小的時候，我們可以使用一階線性展開來調(diào)整臨時狀態(tài)量，如下公式12 所示：

所以要想求出這個臨時的狀態(tài)量，就必須求出等式右邊的兩部分值，第一個部分還是原來的積分形式（就像公式6那樣），是預(yù)積分的主體，論文中使用的是最簡單的歐拉積分法進行展開（取第i時刻值的斜率乘以時間差加上i時刻的初值，就得到i+1時刻的值），但是在代碼中作者也提出了采用的是中值積分（顧名思義這里的斜率取得是i和i+1中點（2i+1）/2的時刻斜率）.公式7是采用歐拉積分的結(jié)果。這里前面有一定的說明，一開始abkbk，bbkbk等是零，旋轉(zhuǎn)是單位旋轉(zhuǎn)，注意整個過程把噪聲設(shè)為0

這里解釋一下，實際上bk和bk+1間有很多個時刻，這里的計算是先設(shè)abkbk，也即abk1為0 然后計算abk2 然后計算abk3 直到abkend

? 第二部分其實就是對應(yīng)的一階偏導(dǎo)（對加速度計偏置和陀螺儀偏置的），一階偏導(dǎo)的求法在下面進行介紹，到這里我們已經(jīng)求出了臨時狀態(tài)量的測量值，也就可以求出狀態(tài)量測量值。

論文到這一步預(yù)積分其實已經(jīng)做了一半了，也就是完成了測量值的求解，還差什么呢？當(dāng)然是協(xié)方差矩陣了，下面重點求解協(xié)方差矩陣，順便把上面沒有求出的陀螺儀和加速度計偏置的雅各比矩陣求出來。

如何求協(xié)方差矩陣呢？怎么從數(shù)學(xué)的定義里去求呢？這里要用到SLAM中的神作state estimation for robotic，建立一個線性高斯誤差狀態(tài)傳播方程，由線性高斯系統(tǒng)的協(xié)方差，就可以推導(dǎo)出方程協(xié)方差矩陣了，也就是測量狀態(tài)的協(xié)方差矩陣了。也就是說還是需要前面求解狀態(tài)測量值的公式6。注意代碼中真正求解公式6使用的是中值法，所以為了和代碼中相一致，下面的求解過程我也才用中值法的方式，為求解需求我們先補充點干貨：

首先需要將上面的四自由度的旋轉(zhuǎn)轉(zhuǎn)換成三個維度的狀態(tài)量，這是由于四自由度的旋轉(zhuǎn)存在過參數(shù)化的情況，因此將誤差看成是一個擾動定義式8：

然后有下面的兩張圖定義出來離散狀態(tài)下的預(yù)積分過程：

最后得到圖中的線性誤差狀態(tài)傳播模型，由此得到IMU預(yù)積分測量值的協(xié)方差矩陣和雅各比矩陣，預(yù)積分的雅各比矩陣直接代入到公式12中計算出更加精確的傳播狀態(tài)值，而協(xié)方差矩陣自然是在后端優(yōu)化中使用。

??????? 需要格外注意上面求出的雅各比矩陣是預(yù)積分值的雅各比，我們還需要求一個IMU殘差的雅各比矩陣，WHAT？還有兩個雅各比矩陣，驚不驚喜意不意外？但情況確實如此，所以還不趕快從床上爬起來擼一把公式。上面求得的雅各比矩陣是用來計算預(yù)積分值時用到的，下面要求的IMU殘差的雅各比矩陣是在緊耦合的時候做下降梯度，在最前面已經(jīng)提到過。在求殘差的雅各比之前，再提一下殘差是如何計算的吧，預(yù)積分相當(dāng)于測量值（就是真值，因為沒有比這個更準(zhǔn)確的值了，那當(dāng)然就是真值了），要估計的狀態(tài)就是估計值，所以預(yù)積分測量值減去狀態(tài)估計值就是殘差，在后面會提到需要估計的IMU估計值有p,v,q,ba,bg。P和q的估計值初始值比較好得到（和視覺相關(guān)，可以直接用視覺的初值），而v,ba,bg這三個量的估計值初始值就比較難得到了，因為視覺沒有這三個初始量，就會用到下面的聯(lián)合初始化得到初始的這三個量。下面直接上殘差公式和要優(yōu)化的狀態(tài)量：

需要求解的殘差雅各比矩陣是殘差對估計狀態(tài)量的一階偏導(dǎo)，殘差向量有三個，狀態(tài)向量有2*5=10個。所以需要計算殘差向量對狀態(tài)向量的一階偏導(dǎo)。首先需要提出的是對于偏置求偏導(dǎo)是比較復(fù)雜的，所以對于預(yù)積分的計算采取的是一階泰勒展開，這樣就相對簡單了。也就是論文中的公式（12）。

可以看出旋轉(zhuǎn)四元數(shù)殘差包含的狀態(tài)量只有qi,qj,big這三個變量，也就是一個三元函數(shù)求偏導(dǎo)過程。

（2）速度殘差的雅各比矩陣

未完待續(xù)，其實可以自己推導(dǎo)試試。

（3）位移殘差的雅各比矩陣

未完待續(xù)，其實可以自己推到試試。

到這里恭喜你已經(jīng)完成了數(shù)據(jù)前端處理的所有步驟，下面直接進入初始化的過程吧！

1.??????estimator_node.cpp系統(tǒng)入口

首先初始化設(shè)置節(jié)點vins_estimator，同時讀取參數(shù)和設(shè)置相應(yīng)的參數(shù)，為節(jié)點發(fā)布相應(yīng)的話題，為節(jié)點訂閱三個話題，分別用來接收和保存IMU數(shù)據(jù)、圖像特征數(shù)據(jù)和原始圖像數(shù)據(jù)，分別是在三個回調(diào)函數(shù)中imu_callback、feature_callback和raw_image_callback，每當(dāng)訂閱的節(jié)點由數(shù)據(jù)送過來就會進入到相應(yīng)的回調(diào)函數(shù)中。

（1）??????接收IMU數(shù)據(jù)

imu_callback函數(shù)中首先執(zhí)行imu_buf.push(imu_msg);將IMU數(shù)據(jù)保存到imu_buf中，同時執(zhí)行con.notify_one();喚醒作用于process線程中的獲取觀測值數(shù)據(jù)的函數(shù)，這里喚醒以及互斥鎖的作用很重要到下面真正要使用的時候在詳細(xì)討論，然后預(yù)測未考慮觀測噪聲的p、v、q值，同時將發(fā)布最新的IMU測量值消息（pvq值），這里計算得到的pvq是估計值，注意是沒有觀測噪聲和偏置的結(jié)果，作用是與下面預(yù)積分計算得到的pvq（考慮了觀測噪聲和偏置）做差得到殘差。

（2）??????接收原始圖像和圖像特征點數(shù)據(jù)

feature_callback和raw_image_callback函數(shù)中主要是將特征數(shù)據(jù)和原始圖像數(shù)據(jù)分別保存到feature_buf和image_buf中，在feature_callback也用到了con.notify_one()和互斥鎖;。

2.??????process()處理觀測值數(shù)據(jù)線程

（1）??????得到觀測值（IMU數(shù)據(jù)和圖像特征點數(shù)據(jù)）

定義觀測值數(shù)據(jù)類measurements，包含了一組IMU數(shù)據(jù)和一幀圖像數(shù)據(jù)的組合的容器，這里比較有意思的是使用了互斥鎖和條件等待的功能，互斥鎖用來鎖住當(dāng)前代碼段，條件等待是等待上面兩個接收數(shù)據(jù)完成就會被喚醒，然后從imu_buf和feature_buf中提取觀測數(shù)據(jù)measurements = getMeasurements()，需要注意的是在提取觀測值數(shù)據(jù)的時候用到的互斥鎖會鎖住imu_buf和feature_buf等到提取完成才釋放，也就是說在提取的過程中上面兩個回調(diào)函數(shù)是無法接收數(shù)據(jù)的，同時上面兩個回調(diào)函數(shù)接收數(shù)據(jù)的時候也使用了互斥鎖，鎖住了imu_buf和feature_buf，這里也不能提取imu_buf和feature_buf中的數(shù)據(jù)。因此整個數(shù)據(jù)獲取的過程是：回調(diào)函數(shù)接收數(shù)據(jù)，接收完一組數(shù)據(jù)喚醒提取數(shù)據(jù)的線程，提取數(shù)據(jù)的線程提取完數(shù)據(jù)后，回調(diào)函數(shù)就可以繼續(xù)接收數(shù)據(jù)，依次往復(fù)。這就是線程間通信的曼妙??！

1）? getMeasurements()返回觀測值數(shù)據(jù)

函數(shù)的作用顧名思義，就是得到一組IMU數(shù)據(jù)和圖像特征數(shù)據(jù)組合的容器。首先保證存在IMU數(shù)據(jù)和圖像特征數(shù)據(jù)，然后還要判斷圖像特征數(shù)據(jù)和IMU數(shù)據(jù)是否對齊。這里使用的是隊列數(shù)據(jù)結(jié)構(gòu)（先進先出front是先進的數(shù)據(jù)，back是后進的數(shù)據(jù)），需要滿足兩個條件就能保證數(shù)據(jù)對齊，第一是IMU最后一個數(shù)據(jù)的時間要大于圖像特征最開始數(shù)據(jù)的時間，第二是IMU最開始數(shù)據(jù)的時間要小于圖像特征最開始數(shù)據(jù)的時間。滿足數(shù)據(jù)對齊就可以數(shù)據(jù)從隊列中按對齊的方式取出來。這里知道把緩存中的圖像特征數(shù)據(jù)或者IMU數(shù)據(jù)取完，才能夠跳出此函數(shù)，返回數(shù)據(jù)。

（2）??????處理IMU數(shù)據(jù)和圖像特征數(shù)據(jù)

步驟1：處理IMU數(shù)據(jù)

遍歷調(diào)用send_imu(imu_msg)將單個IMU數(shù)據(jù)的dt，線加速度值和角加速度值計算出來送給優(yōu)化器處理，優(yōu)化器調(diào)用estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry,rz));方法。

1）Estimator::processIMU(doubledt, const Vector3d &linear_acceleration, const Vector3d&angular_velocity)處理IMU數(shù)據(jù)方法

步驟1調(diào)用imu的預(yù)積分，調(diào)用push_back函數(shù)，函數(shù)中將時間，加速度和角速度分別存入相應(yīng)的緩存中，同時調(diào)用了propagation函數(shù) ,計算對應(yīng)的狀態(tài)量、協(xié)方差和雅可比矩陣

①propagate(double _dt, const Eigen::Vector3d &_acc_1, constEigen::Vector3d &_gyr_1)

預(yù)積分傳播方程，在預(yù)積分傳播方程propagate中使用中點積分方法midPointIntegration計算預(yù)積分的測量值，中點積分法中主要包含兩個部分，分別是得到狀態(tài)變化量result_delta_q，result_delta_p，result_delta_v，result_linearized_ba，result_linearized_bg和得到跟新協(xié)方差矩陣和雅可比矩陣（注意，雖然得到了雅各比矩陣和協(xié)方差矩陣，但是還沒有求殘差和修正偏置一階項的狀態(tài)變量），由于使用的是中點積分，所以需要上一個時刻的IMU數(shù)據(jù)，包括測量值加速度和角速度以及狀態(tài)變化量，初始值由構(gòu)造函數(shù)提供。需要注意的是這里定義的delta_p等是累積的變化量，也就是說是從i時刻到當(dāng)前時刻的變化量，這個才是最終要求的結(jié)果（為修正偏置一階項），而result_delta_q等只是一個暫時的變量，最后殘差和雅可比矩陣、協(xié)方差矩陣保存在pre_integrations中，還有一個函數(shù)這里暫時還沒有用到，是在優(yōu)化的時候才被調(diào)用的，但是其屬于預(yù)積分的內(nèi)容，evaluate函數(shù)在這個函數(shù)里面進行了狀態(tài)變化量的偏置一階修正以及殘差的計算。

步驟2預(yù)積分公式（3）未考慮誤差，提供imu計算的當(dāng)前旋轉(zhuǎn)，位置，速度，作為優(yōu)化的初值

步驟2：處理圖像特征數(shù)據(jù)

這里進來的數(shù)據(jù)不是圖像數(shù)據(jù)，而是前面已經(jīng)跟蹤匹配好的歸一化平面坐標(biāo)。將當(dāng)前幀的特征存放在image中，image的第一個元素類型是相機的編號，代表是第幾幀圖像（從0開始），第二個元素是歸一化特征點坐標(biāo)和特征點編號（從1開始），然后直接進入到處理圖像特征數(shù)據(jù)的線程中estimator.processImage(image, img_msg->header)。

1）Estimator::processImage(constmap<int, vector<pair<int, Vector3d>>> &image, conststd_msgs::Header &header)處理圖像特征數(shù)據(jù)方法

???????? 首先對進來的圖像特征數(shù)據(jù)根據(jù)視差判斷是否是關(guān)鍵幀，選擇丟棄當(dāng)前幀（但保留IMU數(shù)據(jù)）或者丟棄滑動窗口中最老的一幀。

步驟1：將圖像數(shù)據(jù)和時間存到圖像幀類中：首先將數(shù)據(jù)和時間保存到圖像幀的對象imageframe中（ImageFrame對象中包含特征點，時間，位姿R，t，預(yù)積分對象pre_integration，是否是關(guān)鍵幀）,同時將臨時的預(yù)積分值保存到此對象中（這里的臨時預(yù)積分初值就是在前面IMU預(yù)積分的時候計算的），然后將圖像幀的對象imageframe保存到all_image_frame對象中（imageframe的容器），更新臨時預(yù)積分初始值。

步驟2：標(biāo)定相機和IMU的外參數(shù)：接著如果沒有外部參數(shù)就標(biāo)定外部參數(shù)，參數(shù)傳遞有的話就跳過這一步（默認(rèn)有，如果是自己的設(shè)備，可以設(shè)置為2對外參進行在線標(biāo)定）。

步驟3：初始化系統(tǒng)同時進行BA優(yōu)化：當(dāng)求解器處于可初始化狀態(tài)時（初始狀態(tài)是可初始化，初始化成功就設(shè)置為不可初始化狀態(tài)），判斷當(dāng)前frame_count是否達到WINDOW_SIZE，確保有足夠的frame參與初始化，這里的frame_count是滑動窗口中圖像幀的數(shù)量，一開始被初始化為0，滑動窗口總幀數(shù)是10。有外部參數(shù)同時當(dāng)前幀時間戳大于初始化時間戳0.1秒，就進行初始化操作。

步驟3.1：initialStructure()系統(tǒng)初始化，首先初始化Vision-only SFM，然后初始化Visual-Inertial Alignment，構(gòu)成整個初始化過程。

①保證IMU充分運動，通過線加速度判斷，一開始通過線加速度的標(biāo)準(zhǔn)差（離散程度）判斷保證IMU充分運動，加速度標(biāo)準(zhǔn)差大于0.25則代表imu充分激勵，足夠初始化。

②純視覺初始化，對SlidingWindow中的圖像幀和相機姿態(tài)求解sfm問題，這里解決的是關(guān)鍵幀的位姿和特征點坐標(biāo)。

步驟1.首先構(gòu)建SFMFeature對象sfm_f，SFMFeature數(shù)組中包含了特征點狀態(tài)（是否被三角化），id，2d點，3d坐標(biāo)以及深度，將特征管理器中的特征信息保存到SFMFeature對象sfm_f中sfm_f.push_back(tmp_feature)。

步驟2.接著由對極約束中的F矩陣恢復(fù)出R、t，主要調(diào)用方法relativePose(relative_R, relative_T, l)。relativePose方法中首先通過FeatureManeger獲?。ɑ瑒哟翱谥校┑趇幀和最后一幀的特征匹配corres，當(dāng)corres匹配足夠大時，考察最新的keyFrame和sliding window中某個keyFrame之間有足夠feature匹配和足夠大的視差（id為l=i），滿足這兩個條件，然后這兩幀之間通過五點法恢復(fù)出R，t并且三角化出3D的特征點feature point，這里是使用solveRelativeRT(corres, relative_R, relative_T)，solveRelativeRT方法定義在solv_5pts.cpp類中，由對極約束中的F矩陣恢復(fù)出R、t，直接調(diào)用opencv中的方法，沒什么好說的，這里值得注意的是，這種relativePose得到的位姿是第l幀的，第l幀的篩選是從第一幀開始到滑動窗口所有幀中一開始滿足平均視差足夠大的幀，這里的第l幀會作為參考幀到下面的全局SFM使用。到這里就已經(jīng)得到圖像的特征點2d坐標(biāo)的提取，相機第l幀和最后一幀之間的旋轉(zhuǎn)和平移（注意暫時還沒有得到特征的3d點坐標(biāo)），有了這些信息就可以構(gòu)建全局的SFM類GlobalSFM sfm，在這里調(diào)用sfm.construct(frame_count + 1, Q, T,l,relative_R, relative_T,sfm_f, sfm_tracked_points)，這里以第l幀作為參考幀，在進行PNP求解之前，需要判斷當(dāng)前幀數(shù)要大于第l幀，這保證了第l幀直接跳過PNP步驟，首先執(zhí)行下面的第l幀和最后一幀的三角化，得到共視的特征點，供下面第l+1幀和最后一幀求解PNP，然后利用pnp求解l+1幀到最后一幀的位姿R_initial, P_initial，最后的位姿都保存在Pose中，一次循環(huán)，得到l+1，l+2…n-1幀的位姿。跳出步驟2 的循環(huán)后，至此得到了l+1，l+2…n-1幀的位姿以及l(fā)+1，l+2…幀與n-1幀的特征點三角化。然后再三角化l幀和i幀（在第l幀和最后一幀之間的幀）之間的3d坐標(biāo)，（這里不明白為什么要做兩次，是可以三角化出更多的特征點嗎？？？？），接著PNP求解l-1，l-2…0幀和l幀之間的位姿已經(jīng)三角化相應(yīng)的特征點坐標(biāo)，最后三角化其他所有的特征點。至此得到了滑動窗口中所有相機的位姿以及特征點的3d坐標(biāo)。第6部就是進行BA優(yōu)化，使用的是ceres優(yōu)化位姿和特征點，這里可以參考視覺SLAM第十講中的內(nèi)容，優(yōu)化方式相同。

步驟4：visualInitialAlign中調(diào)用VisualIMUAlignment方法，真正的視覺慣性聯(lián)合初始化，imu與視覺對齊，獲取絕對尺度等。這個方法定義在initial/initial_alignment.h中。

步驟4.1：solveGyroscopeBias計算陀螺儀偏置，整個方法的計算模型由論文中給出，使用LTLD方法求解最小二乘問題，delta_bg = A.ldlt().solve(b);這里A +=tmp_A.transpose() * tmp_A，b += tmp_A.transpose() * tmp_b，其實就是處理AT*A*x=AT*b問題，一般的最小二乘問題直接處理Ax=b也就是Ax-b=0即可，這里是使用LDLT方法，兩邊同乘以A矩陣的轉(zhuǎn)置得到的AT*A一定是可逆的，因此就可以直接兩邊同乘以其逆即可，相應(yīng)的說明詳見LDLT方法。得到陀螺儀偏置之后將其值保存到前面定義的Bgs[]中，最后在重新計算一次預(yù)積分。

步驟4.2：LinearAlignment計算尺度，重力加速度和速度。論文中給出的公式是相鄰兩個速度的模型，映射到整個n+1個速度模型中，A矩陣一定是一個正定矩陣（實對稱矩陣），代碼中定義的A和b即是最總的H和b，tmp_A和tmp_b相鄰速度間的臨時變量。最后的求解方法：x = A.ldlt().solve(b)；然后調(diào)用RefineGravity重新計算重力加速度方向，得到最優(yōu)解。

4——初始化

1.??????基于滑動窗口的純視覺單目初始化

??? 在介紹純視覺初始化前我們首先講一講為什么要初始化？初始化要做什么？以及初始化的作用？我們初始化的原因是單目慣性緊耦合系統(tǒng)是一個非線性程度很高的系統(tǒng)，首先單目是無法獲得空間中的絕對尺度，而IMU又必然存在偏置，在后面進行求解的時候還需要用到重力加速度（包括大小和方向），對于速度比較敏感的條件下，比如說無人機，又要精確的速度信息，因此，如何有效的在緊耦合系統(tǒng)處理之前計算出這些量，對整個緊耦合系統(tǒng)的魯棒性有著重大的意義（其實這里就可以理解成相機標(biāo)定一樣，沒有正確的標(biāo)定好相機的內(nèi)參，相機在進行定位的時候必然不準(zhǔn)，而且很有可能會掛掉）。所以初始化要做的事其實說起來很簡單，就是計算出絕對尺度s、陀螺儀偏置bg、加速度偏置ba、重力加速度G和每個IMU時刻的速度v，VINS中重點說明了加速度計偏置值一般都會和重力加速度耦合到一起（也就是被重力加速度給吸收掉），重力加速度的量級要遠大于其加速度偏置，而且在初始化時間內(nèi)加速度計偏置比較小，很難真正的計算得到，因此忽略加速度計偏置的影響，在初始化中不再計算。初始化的作用是不言而喻的，直接影響整個緊耦合系統(tǒng)的魯棒性以及定位精度，并且初始化一般都需要一個比較漫長的時間，VINS大概需要十秒左右，ORB_SLAM2結(jié)合IMU的時間設(shè)定在15秒完成初始化。話不多說，直接進入正題。

???????? 純視覺初始化在第V點的A部分，首先構(gòu)建一個滑動窗口，包含一組數(shù)據(jù)幀。論文中提及使用的是對極幾何模型的5點法求解單目相機的相對變換，包括相對旋轉(zhuǎn)和無尺度信息的位移。其實基本上每個單目模型都是使用對極幾何在初始化中求解兩幀的相對變換，這里需要注意的是旋轉(zhuǎn)是具有尺度不變性的（其實就是單位旋轉(zhuǎn)，不會有尺度信息，你仔細(xì)想想是不是？）。然后三角化得到相應(yīng)的3d點坐標(biāo)，有這些3d點和滑動窗口中其他的幀的2d點就可以進行PNP求解獲得滑動窗口中的所有的位姿和特征點3d坐標(biāo)，至此，純視覺初始化就完成了。是不是很簡單？當(dāng)然啊，畢竟只是簡單的視覺初始化，而真正復(fù)雜的是視覺慣性聯(lián)合初始化，也就是我們初始化的重點和難點

2.??????視覺慣性聯(lián)合初始化

??? 視覺慣性聯(lián)合初始化在第V點的B部分，這里作者給定義的名字叫Visual-Inertia Alignment，即視覺慣性聯(lián)合初始化（而在ORBSLAM2+IMU的論文里，作者定義的名稱就叫IMU initialization，即IMU初始化），為什么定義這樣一個名詞，我覺得有兩個意義，第一在進行陀螺儀偏置初始化的時候要同時使用到IMU測量的旋轉(zhuǎn)和視覺測量的旋轉(zhuǎn)，也就是要聯(lián)合視覺和慣性的數(shù)據(jù)。第二這里求得的尺度S的值不僅僅是IMU的，還是視覺和IMU整個系統(tǒng)的尺度。在具體的講解初始化每個過程的時候，有必要來個總體的概括，初始化在物理意義上的定義其實就是固有參數(shù)的標(biāo)定，在數(shù)學(xué)模型上的定義其實就是公式（6）的矩陣方程求解，而公式（6）其實就是來自于最原始的PVQ積分公式，其中Q旋轉(zhuǎn)對應(yīng)著陀螺儀，而PV對應(yīng)著加速度計

（1）??????陀螺儀偏置標(biāo)定

旋轉(zhuǎn)我們可以通過兩種方式求得，一種是陀螺儀測量值，一種就是視覺觀測值。按照正常的理解兩者的大小一定是相等的（假設(shè)沒有誤差），但實際情況肯定有誤差，我們就來看看各自的誤差。陀螺儀的誤差有兩部分測量噪聲和陀螺儀偏置，噪聲暫時可以忽略（畢竟太?。曈X的誤差就只有觀測噪聲（也可以忽略不管），因此兩者差值的絕對值就是陀螺儀偏置，將整個滑動窗口的所有的旋轉(zhuǎn)做差構(gòu)成了一個最小化誤差模型：

??? 公式15中第一個式子的第一項和第二項作四元數(shù)旋轉(zhuǎn)的廣義乘積就可以得到相機從bk到bk+1下的相對旋轉(zhuǎn)（bk+1坐標(biāo)系下），第三項是陀螺儀從bk+1到bk下的相對旋轉(zhuǎn)（bk坐標(biāo)系下），兩者在做廣義乘積，就是首先從bk到bk+1旋轉(zhuǎn)，然后再從bk+1到bk旋轉(zhuǎn)，相當(dāng)于做差（OA+AO=0），第二個式子就是前面預(yù)積分提到的一階線性近似。然后取最小二乘，當(dāng)然也可以使用SVD分解等方法求解。注意在求得陀螺儀偏置之后要再次將陀螺儀偏置代入到預(yù)積分中再求一次預(yù)積分的值，會更加精確。

（1）??????速度、重力加速度和尺度標(biāo)定

作者在這里將這三個狀態(tài)量統(tǒng)一到一個狀態(tài)向量中，如公式16所示：

速度的是在bk坐標(biāo)系下的，重力加速度在初始相機坐標(biāo)系下，就像前面提到的，求解著幾個量是由P、V數(shù)學(xué)模型求得，在滑動窗口中考慮到兩個連續(xù)關(guān)鍵幀bk和bk+1，下面進行論文中公式17和19的推導(dǎo)：

??? 公式推導(dǎo)之后就會得到論文中的公式17、18和19，我們重點關(guān)注下為什么要這樣推導(dǎo)，以及推導(dǎo)得到的運動方程關(guān)系。首先為什么要進行這樣的推導(dǎo)，這完全取決于狀態(tài)向量的定義方式，我們最終要得到的方程形式左邊一定是以狀態(tài)向量的形式來表達的，而且還要滿足其他量都是已知的（從IMU預(yù)積分和視覺跟蹤得到），因此就需要將方程進行如此的變化，才能滿足這樣的關(guān)系。然后是最后的形式我們可以看到狀態(tài)向量最終的形式維度是（n+1）*3+3+1，兩個連續(xù)幀產(chǎn)生的運動方程的維度是3+3+3+1（vbkbk，vbk+1bk+1，gc0，s），比較維度就可以看到最終得到的H矩陣一定是一個正定對稱矩陣，因此可以采用快速的Cholesky分解。

（1）??????重力優(yōu)化

??? 上面其實已經(jīng)得到了重力加速度的大小和方向，這里為什么還需要對重力進行優(yōu)化呢？理由很簡單，這里計算的重力吸收了重力加速度計的偏置，雖然不需要計算重力加速度計的偏置，但重力還是需要優(yōu)化的，說到優(yōu)化重力加速度，肯定包含兩個量，大小和方向，也就是三個維度，但是一般來說大小是確定已知的（這里設(shè)為9.8），因此其實我們要做的就是優(yōu)化方向，是一個兩維的向量，下圖是優(yōu)化重力的方法以及b1,b2單位向量的方向確定模型