目錄
- 滑動視窗内的純視覺SfM
- 視覺慣性對齊
- 小結
單目緊耦合的VIO是一個非線性系統，需要有一個精确的初始值。VINS-Mono的初始化采用松耦合的方式将純視覺SfM和IMU預積分對齊，獲得狀态估計器的初始值。該部分對應源代碼的bool Estimator::initialStructure()函數。
滑動視窗内的純視覺SfM
為了維持有限的計算複雜度，VINS-Mono采用了滑動視窗優化的方法，隻對滑動視窗内的相機位姿和路标點位置進行優化。當圖像幀數量累積到足夠視窗大小時，系統開始初始化。在進行純視覺SfM之前，先求IMU預積分中的速度預積分量的标準差，判斷IMU是否有充分的運動，隻有得到了足夠的激勵才進行後續步驟。首先是純視覺SfM，即隻利用圖像幀估計出滑動視窗内相機的運動軌迹及特征點的深度。
由于特征跟蹤前端（feature tracker）已經對每一幀圖像進行了特征提取和追蹤，每一個特征點在滑動視窗内的所有被觀測幀的資訊都被儲存在一個特征管理的類中，我們可以在滑動視窗中找到與目前幀存在足夠視差（>20pixels）和特征比對對數(>30)的一幀，将該幀作為視覺SfM的參考幀，利用五點法計算兩幀之間的本質矩陣，進而恢複出相對旋轉和平移，其中平移的尺度因子待後續的視覺慣性對齊步驟估計。
得到了參考幀和目前幀的相對位姿後，可三角化兩幀之間共同觀測的特征點，得到特征點在參考幀的相機坐标系下的三維坐标。之後，利用三角化後的特征點及其在其他幀上的歸一化坐标，通過PnP求解得到其他幀相對參考幀的位姿，并繼續三角化其他未恢複深度資訊的特征點，最終得到滑動視窗内所有圖像幀的位姿和特征點的三維坐标。在VINS-Mono的initial_sfm.cpp中，還使用ceres對滑窗内所有相機位姿進行了一次全局BA，其中固定參考幀位姿不變。
最後将所有相機位姿轉換到IMU坐标系下，友善接下來進行的視覺慣性對齊。
視覺慣性對齊
該步驟目的是計算陀螺儀的偏置，滑動視窗内各圖像幀的速度，重力分量和尺度因子。該部分對應estimator.cpp中的bool Estimator::visualInitialAlign()函數。
對于連續的兩幀，它們的相對旋轉既可以從視覺SfM中得到，也可以通過IMU預積分得到，而IMU預積分中則包含了陀螺儀的偏置，是以，通過最小化以下代價函數可以估計出陀螺儀的偏置。對應initial_aligment.cpp中的void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)函數。陀螺儀偏置更新後，相應地更新所有的預積分量。
下一步則是計算相機尺度因子、相機參考坐标系下的重力加速度和每一幀的速度，同樣地也是通過建構觀測方程，建構最小二乘問題來求解。
上一步求解重力加速度時是将其作為三自由度的變量來計算的，而重力加速度的模通常是已知的，是以實際上隻有兩個自由度。于是将重力加速度重新表示為其模乘以機關方向向量加上其正切平面上的任意一對帶系數的正交基。之後，重新求解上述變量，得到細化後的重力加速度。
求解得到上述變量後，将所有幀的位置、速度和特征點深度按照尺度縮放，将重力向量旋轉到Z軸上可得到相機參考坐标系與世界坐标系的旋轉矩陣，進而可以将所有變量從相機參考坐标系旋轉到世界坐标系上。至此，完成初始化流程。
小結
個人了解，VIO系統的初始化，或者說系統本身，就是将相機（視覺）和IMU兩種不同的傳感器的優勢結合起來，為後續的估計提供一個良好的初始狀态。其中視覺提供的旋轉限制可以為IMU估計陀螺儀偏置，而IMU的加速度計提供的位移限制和對重力的感覺，可以為視覺提供尺度因子，得到絕對尺度的位置、速度和特征點深度資訊，并且利用重力向量将相機參考坐标系與世界坐标系對齊。