VINS技術路線
轉自：https://blog.csdn.net/wangshuailpp/article/details/78461171寫在前面：本文整和自己的思路，希望對學習VINS或者VIO的同學有所幫助，如果你覺得文章寫的對你的理解有一點幫助，可以推薦給周圍的小夥伴們，當然，如果你有任何問題想要交流，歡迎隨時探討。話不多說，下面上正文。
VINS代碼地址：https://github.com/HKUST-Aerial-Robotics/VINS-Mono
參考文檔：1.VINS-Mono A Robust and Versatile Monocular Visual-Inertial State Estimator
2.Quaternion kinematics for the error-state Kalman filter
如果想單獨瞭解某一塊內容，可以看分開討論的部分，內容相同：
VINS理論與代碼詳解3——IMU預積分（最近更新IMU殘差雅各比計算推導）
VINS理論與代碼詳解5——基於滑動窗口的單目視覺緊耦合後端優化模型（最近更新視覺雅各比計算推導）
最近整理了ORB_SLAM2視覺慣性緊耦合的理論分析，想了解的可以點開下面的連接：
如果只想單獨瞭解ORB_SLAM2視覺慣性緊耦合的理論分析某一塊的話，可以看分開討論的部分，內容都是相同的：
ORB_SLAM2視覺慣性緊耦合定位技術路線與代碼詳解0——整體框架與理論基礎知識
ORB_SLAM2視覺慣性緊耦合定位技術路線與代碼詳解1——IMU流型預積分
ORB_SLAM2視覺慣性緊耦合定位技術路線與代碼詳解2——IMU初始化
ORB_SLAM2視覺慣性緊耦合定位技術路線與代碼詳解3——緊耦合優化模型
在這些博客中經常提到的一些關鍵數學知識總結到下面，隨時更新歡迎閱讀指正：
視覺SLAM常見的QR分解SVD分解等矩陣分解方式求解滿秩和虧秩最小二乘問題（最全的方法分析總結）
凸函數的Hessian矩陣與高斯牛頓下降法增量矩陣半正定性的理解
VINS代碼主要包含在兩個文件中，分別是feature_tracker和vins_estimate，feature_tracker就像文件的名字一樣，總體的作用是接收圖像，使用KLT光流算法蹤；vins_estimate包含相機和IMU數據的前端預處理（也就是預積分過程）、單目慣性聯合初始化（在線的標定過程）、基於滑動窗口的BA聯合優化、全局的圖優化和迴環檢測等。要想真正的理解一個SLAM框架，必須真正搞懂其對應的算法模型，然後才能研究其代碼邏輯，最後做到相得益彰的效果，因此本次講解主要是結合論文中的理論知識這和兩個文件中的代碼進行詳細的探討。整體的框架都比較熟悉，如下圖所示，第一部分是Measuremen Preprocessing：觀測值數據預處理，包含圖像數據跟蹤IMU數據預積分；第二部分是Initialization：初始化，包含單純的視覺初始化和視覺慣性聯合初始化；第三部分Local Visual-Inertia BA and Relocalization：局部BA聯合優化和重定位，包含一個基於滑動窗口的BA優化模型；第四部分Global Pose Graph Optimization：全局圖優化，只對全局的位姿進行優化；第五部分Loop detection：迴環檢測。
一．Feature_tracker文件夾中
首先講第一部分，也就是純粹的圖像處理部分內容，在論文中的第IV點觀測值預處理的A部分視覺前端處理，爲了更好的理解代碼，有必要將論文中的相關內容和大家討論一番。
論文內容：每當進入新的圖像，都會使用KLT稀疏光流法進行跟蹤，同時提取100-300個角點信息，我的理解是角點是用來建立圖像，光流跟蹤是用來快速定位。同時在這裏還進行了關鍵幀的選取，主要是兩個剔除關鍵幀的策略，分別是平均視差法和跟蹤質量法。平均視差法：如果當前幀的和上一個關鍵幀跟蹤點的平均視差超出了一個設定的閾值，就將當前幀設爲關鍵幀。這裏有一個問題，就是旋轉和平移都會產生視差（不只是平移哦），當出現純旋轉的時候特徵點無法被三角化，無法計算出旋轉值，也就無法計算跟蹤點間的平均視差，爲了解決這一問題，採用短時的陀螺儀觀測值來補償旋轉，從而計算出視差，這一過程只應用到平均視差的計算，不會影響真實的旋轉結果。
具體代碼實現：主要負責圖像角點提取和光流跟蹤，只有一個主線程。主要是三個源程序，分別是feature_tracker、feature_tracker_node以及parameters。feature_tracker_node是特徵跟蹤線程的系統入口，feature_tracker是特徵跟蹤算法的具體實現，parameters是設備等參數的讀取和存放。
1. feature_tracker_node.cpp系統入口
（1） main()函數
步驟1：readParameters(n);讀取參數，是config->euroc->euroc_config.yaml中的一些配置參數。
步驟2: trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);在這裏NUM_OF_CAM設置成常量1，只有一個攝像頭（單目），讀取相機內參。
步驟3:判斷是否加入魚眼mask來去除邊緣噪聲
步驟4: ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100,img_callback); 訂閱話題和發佈話題，監聽IMAGE_TOPIC（/cam0/image_raw），有圖像發佈到這個話題上的時候，執行回調函數，這裏直接進入到img_callback函數中接收圖像，前端視覺的算法基本在這個回調函數中。
1） img_callback(const sensor_msgs::ImageConstPtr &img_msg)接收圖像
步驟1: 頻率控制，保證每秒鐘處理的image不多於FREQ，這裏將平率控制在10hz以內。
步驟2: 處理單目相機
步驟2.1: trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW *(i + 1)));讀取到的圖像數據存儲到trackerData中，讀取完之後如果圖像太亮或太黑（EQUALIZE=1），使用createCLAHE對圖像進行自適應直方圖均衡化，如果圖像正常，設置成當前圖像。在讀取圖像的時候進行光流跟蹤和特徵點的提取。FeatureTracker類中處理的主要函數就是readImage()，這裏涉及到3個img(prev_img, cur_img, forw_img)和pts(prev_pts,cur_pts, forw_pts)，兩者是相似的。剛開始看不是太好理解，cur和forw分別是LK光流跟蹤的前後兩幀，forw纔是真正的“當前”幀，cur實際上是上一幀，而prev是上一次發佈的幀，它實際上是光流跟蹤以後，prev和forw根據Fundamental Matrix做RANSAC剔除outlier用的，也就是rejectWithF()函數. readImage()的處理流程爲:
①先調用cv::CLAHE對圖像做直方圖均衡化(如果EQUALIZE=1，表示太亮或則太暗)
PS：CLAHE是一種直方圖均衡算法，能有效的增強或改善圖像（局部）對比度，從而獲取更多圖像相關邊緣信息有利於分割，比如在書架識別系統的書脊切割中，使用CLAHE可以比傳統的直方圖增強方法達到更好的增強書脊邊界直線的效果，從而有利於後續的書脊邊界直線的檢測和提取。還能夠有效改善AHE中放大噪聲的問題，雖然在實際中應用不多，但是效果確實不錯。
②調用calcOpticalFlowPyrLK()跟蹤cur_pts到forw_pts,根據status,把跟蹤失敗的點剔除(注意:prev, cur,forw, ids, track_cnt都要剔除),這裏還加了個inBorder判斷,把跟蹤到圖像邊緣的點也剔除掉.
③如果不需要發佈特徵點,則到這步就完了,把當前幀forw賦給上一幀cur, 然後退出.如果需要發佈特徵點(PUB_THIS_FRAME=1), 則執行下面的步驟
④先調用rejectWithF()對prev_pts和forw_pts做ransac剔除outlier.(實際就是調用了findFundamentalMat函數), 在光流追蹤成功就記被追蹤+1，數值代表被追蹤的次數，數值越大，說明被追蹤的就越久
⑤調用setMask(), 先對跟蹤點forw_pts按跟蹤次數降排序, 然後依次選點, 選一個點, 在mask中將該點周圍一定半徑的區域設爲0, 後面不再選取該區域內的點. 有點類似與non-max suppression, 但區別是這裏保留track_cnt最高的點.
⑥在mask中不爲0的區域,調用goodFeaturesToTrack提取新的角點n_pts, 通過addPoints()函數push到forw_pts中, id初始化-1,track_cnt初始化爲1.
整體來說需要注意的是：光流跟蹤在②中完成，角點提取在⑥中完成
步驟2.2:判斷是否需要顯示畸變。
步驟2.3:將特徵點矯正（相機模型camodocal）後歸一化平面的3D點（此時沒有尺度信息，3D點p.z=1），像素2D點，以及特徵的id，封裝成ros的sensor_msgs::PointCloud消息類型的feature_points實例中；將圖像封裝到cv_bridge::CvImageConstPtr類型的ptr實例中
步驟3: 發佈消息的數據
pub_img.publish(feature_points);
pub_match.publish(ptr->toImageMsg())
將處理完的圖像信息用PointCloud實例feature_points和Image的實例ptr消息類型，發佈到"feature"和"feature_img"的topic（此步驟在main函數中完成）
至此，已經將圖像數據包裝成特徵點數據和圖像數據發佈出來了，下面就是在開一個線程，發佈一個話題，接收這兩種消息，也就是下面的vins_esitimate文件中做的事。
下面是具體的流程圖：
二．Vins_estimate文件夾中
下面到第二部分了，也是整個框架的重點和難點，估計要花很長的時間學習和總計理論知識和代碼邏輯，這一部分還是從論文的理論知識講起，然後再附上代碼消化理解。
論文內容：
1. 論文第IV點的B部分IMU預積分，
IMU預積分的作用是計算出IMU數據的觀測值（就是IMU預積分值）以及殘差的協方差矩陣和雅各比矩陣，那就要清楚的明白爲什麼要計算這三個量？計算出這三個量爲什麼就可以和視覺觀測值進行耦合？如果你現在回答不出來，請好好想一想自己以前學到的知識，關於視覺的這三個量，視覺中觀測值是用來計算殘差的（也就是誤差），殘差的雅各比矩陣是優化中下降的方向（也就是梯度），很少提及的協方差矩陣（但很重要）其實是觀測值對應的權值（因爲有很多觀測值），現在是不是很清楚明白了？具體使用來說，這三個量爲後面的聯合初始化提供初值以及後端優化提供IMU的約束關係。原始陀螺儀和加速度計的觀測值數據：
第一個式子等式左邊是加速度測量值（你可以從加速度計中讀到的值，帶尖括號的是帶誤差的數據），等式右邊是加速度真實值（其實就是準確的值，我們需要得到的是這個真實值）加上加速度計的偏置、重力加速度（注意是世界座標系下）和加速度噪聲項。第二個式子等式左邊是陀螺儀測量值，等式右邊是陀螺儀真實值加上陀螺儀偏置和陀螺儀噪聲項。這裏的值都是IMU（body）幀座標系下的。這裏假設噪聲是服從高斯正態分佈，而偏置服從隨機遊走模型。
有上面最原始的式子積分就可以計算出下一時刻的p、v和q:
狀態量上標代表的是所處的座標系，下標是具體哪一幀數據。這裏等式左邊的值都是世界座標系下（W）bk+1幀的值。從整個式子可以看出來這裏的狀態傳播需要bk幀下的旋轉，平移和速度，當這些開始的狀態發生改變的時候，就需要重新傳播IMU觀測值，也就是說狀態傳播方程要重新計算和修改，我們想要一次性就求出bk和bk+1之間的狀態傳播，因此選用預積分模型（其實這裏我也沒有完全搞明白，但是有一點是明白的，這裏是在世界座標系下求解狀態，但是由條件裏需要世界座標系下的旋轉，明顯衝突啊，因此可以使用預積分將世界座標系下的狀態轉換到IMU的bk幀座標系下，最初提出預積分的外國大佬是將世界座標系轉換到求狀態的變化量，其實兩者的原理都一樣，預積分求的值都是變化量），兩邊同時乘以世界到bk幀座標系的轉換，如下圖所示，然後提出等式右邊只與加速度和角速度有關的量進行積分，如公式6：
到這裏其實只要求出公式6中的積分值，真的預積分的值就得到了，這裏bk是參考幀，從式6中可以看出，在bk到bk+1幀間，這裏要求的三個臨時狀態量只與IMU的偏置有關係，而與其他狀態無關，也就是說每個式子相當於一個二元一次方程（f(x,y)=ax+by+c,x相當於加速度計偏置，y相當於陀螺儀偏置），這裏就是爲了求解這個二元一次方程，當這裏的偏置變化特別小的時候，我們可以使用一階線性展開來調整臨時狀態量，這裏提一下雅可比矩陣就是一階偏導，如下公式12 所示：
所以要想求出這個臨時的狀態量，就必須求出等式右邊的兩部分值，第一個部分還是原來的積分形式（就像公式6那樣），是預積分的主體，論文中使用的是最簡單的歐拉積分法進行展開（取第i時刻值的斜率乘以時間差加上i時刻的初值，就得到i+1時刻的值），但是在代碼中作者也提出了採用的是中值積分（顧名思義這裏的斜率取得是i和i+1中點（2i+1）/2的時刻斜率）.公式7是採用歐拉積分的結果。這裏前面有一定的說明，一開始abkbk，bbkbk等是零，旋轉是單位旋轉，注意整個過程把噪聲設爲0。
第二部分其實就是對應的一階偏導（對加速度計偏置和陀螺儀偏置的），一階偏導的求法在下面進行介紹，到這裏我們已經求出了臨時狀態量的測量值，也就可以求出狀態量測量值。
論文到這一步預積分其實已經做了一半了，也就是完成了測量值的求解，還差什麼呢？當然是協方差矩陣了，下面重點求解協方差矩陣，順便把上面沒有求出的陀螺儀和加速度計偏置的雅各比矩陣求出來。
如何求協方差矩陣呢？怎麼從數學的定義裏去求呢？這裏要用到SLAM中的神作state estimation for robotic，建立一個線性高斯誤差狀態傳播方程，由線性高斯系統的協方差，就可以推導出方程協方差矩陣了，也就是測量狀態的協方差矩陣了。也就是說還是需要前面求解狀態測量值的公式6。注意代碼中真正求解公式6使用的是中值法，所以爲了和代碼中相一致，下面的求解過程我也才用中值法的方式，爲求解需求我們先補充點乾貨：
首先需要將上面的四自由度的旋轉轉換成三個維度的狀態量，這是由於四自由度的旋轉存在過參數數化的情況，過參數化求解會需要引入額外的約束（單位四元數性質），因此將誤差看成是一個擾動定義式8：
然後有下面的兩張圖定義出來離散狀態下的預積分過程：
然後有下面的兩張圖定義出來離散狀態下的預積分過程：
最後得到圖中的線性誤差狀態傳播模型，由此得到IMU預積分測量值的協方差矩陣和雅各比矩陣，預積分的雅各比矩陣直接代入到公式12中計算出更加精確的傳播狀態值，而協方差矩陣自然是在後端優化中使用。
需要格外注意上面求出的雅各比矩陣是預積分值的雅各比，我們還需要求一個IMU殘差的雅各比矩陣，WHAT？還有兩個雅各比矩陣，驚不驚喜意不意外？但情況確實如此，所以還不趕快從牀上爬起來繼續擼一把公式。上面求得的雅各比矩陣是用來計算預積分值時用到的，下面要求的IMU殘差的雅各比矩陣是在緊耦合的時候做下降梯度，在最前面已經提到過。在求殘差的雅各比之前，再提一下殘差是如何計算的吧，預積分相當於測量值（就是真值，因爲沒有比這個更準確的值了，那當然就是真值了），要估計的狀態就是估計值，所以預積分測量值減去狀態估計值就是殘差，在後面會提到需要估計的IMU估計值有p,v,q,ba,bg。P和q的估計值初始值比較好得到（和視覺相關，可以直接用視覺的初值），而v,ba,bg這三個量的估計值初始值就比較難得到了，因爲視覺沒有這三個初始量，就會用到下面的聯合初始化得到初始的這三個量。下面直接上殘差公式和要優化的狀態量：
需要求解的殘差雅各比矩陣是殘差對估計狀態量的一階偏導，殘差向量有三個，狀態向量有2*5=10個。所以需要計算殘差向量對狀態向量的一階偏導。首先需要提出的是對於偏置求偏導是比較複雜的，所以對於預積分的計算採取的是一階泰勒展開，這樣就相對簡單了。也就是論文中的公式（12）。
（1）旋轉四元數殘差的雅各比矩陣
可以看出旋轉四元數殘差包含的狀態量只有qi,qj,big這三個變量，也就是一個三元函數求偏導過程。
（2）速度殘差的雅各比矩陣
未完待續，其實可以自己推導試試。
（3）位移殘差的雅各比矩陣
未完待續，其實可以自己推到試試。
到這裏恭喜你已經完成了數據前端處理的所有步驟，下面直接進入初始化的過程吧！
2. 基於滑動窗口的純視覺單目初始化
在介紹純視覺初始化前我們首先講一講爲什麼要初始化？初始化要做什麼？以及初始化的作用？我們初始化的原因是單目慣性緊耦合系統是一個非線性程度很高的系統，首先單目是無法獲得空間中的絕對尺度，而IMU又必然存在偏置，在後面進行求解的時候還需要用到重力加速度（包括大小和方向），對於速度比較敏感的條件下，比如說無人機，又要精確的速度信息，因此，如何有效的在緊耦合系統處理之前計算出這些量，對整個緊耦合系統的魯棒性有着重大的意義（其實這裏就可以理解成相機標定一樣，沒有正確的標定好相機的內參，相機在進行定位的時候必然不準，而且很有可能會掛掉）。所以初始化要做的事其實說起來很簡單，就是計算出絕對尺度s、陀螺儀偏置bg、加速度偏置ba、重力加速度G和每個IMU時刻的速度v，VINS中重點說明了加速度計偏置值一般都會和重力加速度耦合到一起（也就是被重力加速度給吸收掉），重力加速度的量級要遠大於其加速度偏置，而且在初始化時間內加速度計偏置比較小，很難真正的計算得到，因此忽略加速度計偏置的影響，在初始化中不再計算。初始化的作用是不言而喻的，直接影響整個緊耦合系統的魯棒性以及定位精度，並且初始化一般都需要一個比較漫長的時間，VINS大概需要十秒左右，ORB_SLAM2結合IMU的時間設定在15秒完成初始化。話不多說，直接進入正題。
純視覺初始化在第V點的A部分，首先構建一個滑動窗口，包含一組數據幀。論文中提及使用的是對極幾何模型的5點法求解單目相機的相對變換，包括相對旋轉和無尺度信息的位移。其實基本上每個單目模型都是使用對極幾何在初始化中求解兩幀的相對變換，這裏需要注意的是旋轉是具有尺度不變性的（其實就是單位旋轉，不會有尺度信息，你仔細想想是不是？），至於爲什麼單目沒有尺度信息，我想羅嗦一句，但其實很多學習單目視覺SLAM的人都沒有真正搞明白過，單目視覺沒有尺度的源頭是最開始兩幀間的對極幾何求位姿，再具體點就是求F/E或者H的時候需要將其降參一位，F從6維降到5維，H從9維降到8維，這裏所降的維度就是尺度，而且必須要降。然後三角化得到相應的3d點座標，有這些3d點和滑動窗口中其他的幀的2d點就可以進行PNP求解獲得滑動窗口中的所有的位姿和特徵點3d座標，至此，純視覺初始化就完成了。是不是很簡單？當然啊，畢竟只是簡單的視覺初始化，而真正複雜的是視覺慣性聯合初始化，也就是我們初始化的重點和難點，所以下面的知識點一定要打起精神學啦！
3. 視覺慣性聯合初始化
視覺慣性聯合初始化在第V點的B部分，這裏作者給定義的名字叫Visual-Inertia Alignment，即視覺慣性聯合初始化（而在ORBSLAM2+IMU的論文裏，作者定義的名稱就叫IMU initialization，即IMU初始化），爲什麼定義這樣一個名詞，我覺得有兩個意義，第一在進行陀螺儀偏置初始化的時候要同時使用到IMU測量的旋轉和視覺測量的旋轉，也就是要聯合視覺和慣性的數據。第二這裏求得的尺度S的值不僅僅是IMU的，還是視覺和IMU整個系統的尺度。在具體的講解初始化每個過程的時候，有必要來個總體的概括，初始化在物理意義上的定義其實就是固有參數的標定，在數學模型上的定義其實就是公式（6）的矩陣方程求解，而公式（6）其實就是來自於最原始的PVQ積分公式，其中Q旋轉對應着陀螺儀，而PV對應着加速度計，如果不明白的話，不要緊，看完下面的整體推導過程相信聰明的你一定會茅塞頓開。
（1） 陀螺儀偏置標定
旋轉我們可以通過兩種方式求得，一種是陀螺儀測量值，一種就是視覺觀測值。按照正常的理解兩者的大小一定是相等的（假設沒有誤差），但實際情況肯定有誤差，我們就來看看各自的誤差。陀螺儀的誤差有兩部分測量噪聲和陀螺儀偏置，噪聲暫時可以忽略（畢竟太小），而視覺的誤差就只有觀測噪聲（也可以忽略不管），因此兩者差值的絕對值就是陀螺儀偏置，將整個滑動窗口的所有的旋轉做差構成了一個最小化誤差模型：
公式15中第一個式子的第一項和第二項作四元數旋轉的廣義乘積就可以得到bk+1座標系下相機從bk+1到bk下的相對旋轉（相對bk+1），第三項是bk座標系下陀螺儀從b到bk k+1下的相對旋轉（相對bk），兩者在做廣義乘積，就是首先從bk到bk+1旋轉，然後再從bk+1到bk旋轉，相當於做差（OA+AO=0），第二個式子就是前面預積分提到的一階線性近似。然後取最小二乘，當然也可以使用SVD分解等方法求解。注意在求得陀螺儀偏置之後要再次將陀螺儀偏置代入到預積分中再求一次預積分的值，會更加精確。
（2） 速度、重力加速度和尺度標定
作者在這裏將這三個狀態量統一到一個狀態向量中，如公式16所示：
速度的是在bk座標系下的，重力加速度在初始相機座標系下，就像前面提到的，求解着幾個量是由P、V數學模型求得，在滑動窗口中考慮到兩個連續關鍵幀bk和bk+1，下面進行論文中公式17和19的推導：
公式推導之後就會得到論文中的公式17、18和19，我們重點關注下爲什麼要這樣推導，以及推導得到的運動方程關係。首先爲什麼要進行這樣的推導，這完全取決於狀態向量的定義方式，我們最終要得到的方程形式左邊一定是以狀態向量的形式來表達的，而且還要滿足其他量都是已知的（從IMU預積分和視覺跟蹤得到），因此就需要將方程進行如此的變化，才能滿足這樣的關係。然後是最後的形式我們可以看到狀態向量最終的形式維度是（n+1）*3+3+1，兩個連續幀產生的運動方程的維度是3+3+3+1（vbkbk，vbk+1bk+1，gc0，s），gc0是第一個相機座標系下的重力加速度，剩下的就是解最小二乘問題了，論文中採用的是快速的Cholesky分解。對於最小二乘求解這類方程如果有不懂的可以翻看我的另一篇博客，希望可以幫到你：
http://blog.csdn.net/wangshuailpp/article/details/80209863
（3） 重力優化
上面其實已經得到了重力加速度的大小和方向，這裏爲什麼還需要對重力進行優化呢？理由很簡單，這裏計算的重力吸收了重力加速度計的偏置，雖然不需要計算重力加速度計的偏置，但重力還是需要優化的，說到優化重力加速度，肯定包含兩個量，大小和方向，也就是三個維度，但是一般來說大小是確定已知的（這裏設爲9.8），因此其實我們要做的就是優化方向，是一個兩維的向量，下圖是優化重力的方法以及b1,b2單位向量的方向確定模型。
4. 基於滑動窗口的單目視覺緊耦合後端優化模型
終於講到了真正的視覺慣性緊耦合系統了，（在這裏插一句，算是寫博客和學習一個SLAM系統的心得吧，一開始我們可能抱着一個必勝的心態打算征服所有的知識點和難點，但到了整個過程的中期，總會出現或多或少的問題，阻礙着我們前進的步伐，你可能想放鬆警惕，不想太深究具體的公式和內容，急功心切的想看到終點到底是什麼，但是我要提醒大家，也包括自己，最美好的以及最有價值的永遠是過程，是在過程中學到的可以爲一生所用的經驗，只是一味着衝到終點，而忘記了過程，你最終會發現得到的只有失望。如果你不想失望的話，那就再接再厲，給自己繼續加油吧！）到這裏纔是整個系統的重點，前面所有提及的其實都是鬆耦合方式，目的是給整個系統提供優化初值和狀態。視覺慣性緊耦合優化模型在第VI部分，我理解的緊耦合系統是將視覺和慣性的原始觀測量進行有效的組合，也就是在數據處理前就進行數據融合，VINS中是將滑動窗口內的狀態量整合到一個狀態向量中，如公式21所示：
第一個式子是滑動窗口內整個狀態向量，其中n是幀數，m是滑動窗口中的特徵點總數，維度是15*n+6+m，第二個式子xk是在第k幀圖像捕獲到的IMU狀態，包括位姿，速度，旋轉，加速度計和陀螺儀偏置，第三個式子是相機外參，λ是特徵點深度值得逆（大家一定會問，問什麼這裏只用特徵點深度值的逆，也就是逆深度，主要是考慮穩定和滿足高斯系統，不懂得可以參考高博的十四講）。從狀態向量就可以看出xk只與IMU項以及Marginalization有關，特徵點的深度值只與camera和Marginalization有關，所以下面建立BA緊耦合模型的時候按照這個思路，下面建立一個視覺慣性的BA優化模型以及魯棒核函數模型：
從公式中可以明顯的看出來，BA優化模型被分成了三個部分，分別是Marginalization（邊緣化）殘差部分（從滑動窗口中去掉的位姿和特徵點的約束），IMU殘差部分（滑動窗口中相鄰幀間的IMU產生）和視覺代價誤差函數部分（滑動窗口中特徵點在相機下的投影產生），其中魯棒核函數針對代價函數設定（具體作用請參見視覺SLAM十四講）。下面具體介紹着三個部分。
（1） IMU觀測值殘差
考慮到的是兩個連續幀bk和bk+1之間的觀測值，與前面的預積分模型相同，如公式24所示，還記得在IMU預積分的時候求得到協方差矩陣和觀測值嗎？那裏求得的觀測值就是測量值，協方差矩陣就是這裏的協方差矩陣，而公式24前面的項就是預測的值，也稱估計值，估計值和測量值之間的差值就是殘差，其實這裏的公式24是由公式5的右邊左移得到，你發現了嗎？
（2） 視覺觀測值殘差
與傳統的針孔相機模型定義的重投影誤差不同，論文中使用的是單位半球體的相機觀測殘差，是一個寬視野魚眼或者全方位相機。相機的殘差模型如下公式25所示：
第一個式子就是殘差的表達式，第二個式子是魚眼相機反投影函數將觀測到的像素座標轉換成單位向量的觀測值數據，b1和b2是此單位向量的切平面上的一組基。第三個式子是重投影估計模型。其實VINS代碼中也可以使用普通的針孔相機模型。
（3） Marginalization
這一部分借鑑博客：http://blog.csdn.net/q597967420/article/details/76099443
sliding windowsbounding邊界化了優化問題中pose的個數, 從而防止pose和特徵的個數隨時間不斷增加, 使得優化問題始終在一個有限的複雜度內, 不會隨時間不斷增長。然而, 將pose移出windows時, 有些約束會被丟棄掉, 這樣勢必會導致求解的精度下降, 而且當MAV進行一些退化運動(如: 勻速運動)時, 沒有歷史信息做約束的話是無法求解的. 所以, 在移出位姿或特徵的時候, 需要將相關聯的約束轉變成一個約束項作爲prior放到優化問題中. 這就是marginalization要做的事情。
VINS-MONO中,爲了處理一些懸停的case,引入了一個two-way marginalization, 簡單來說就是:如果倒數第二幀是關鍵幀, 則將最舊的pose移出sliding window, 將最舊幀相關聯的視覺和慣性數據邊緣化掉，也就是MARGIN_OLD，作爲一部分先驗值,如果倒數第二幀不是關鍵幀, 則將倒數第二幀pose移出sliding window,將倒數第二幀的視覺觀測值直接捨棄，保留相關聯的IMU數據， 也就是MARGIN_NEW。選取關鍵幀的策略是視差足夠大，在懸停等運動較小的情況下, 會頻繁的MARGIN_NEW, 這樣也就保留了那些比較舊但是視差比較大的pose. 這種情況如果一直MARGIN_OLD的話, 視覺約束不夠強, 狀態估計會受IMU積分誤差影響, 具有較大的累積誤差。
到這裏整體VINS理論的框架基本上算是介紹完畢了，對於後面的重定位，全局位姿優化和迴環檢測等有時間再做下討論，現在有沒有一個比較清晰的思路？如果還比較困惑的話，那就趕緊進行下面的代碼實戰環節吧。下面就從代碼角度來分析VINS的整個第二部分。
（4）單目視覺雅可比計算（參考賀大佬）
視覺SLAM十四講中第162頁有提到，將空間座標系下的3D點投影到像素座標系下做最小二乘，求出該最小二乘模型的雅各比矩陣既可以得到高斯牛頓或者LM方法的下降梯度，從而優化位姿和路標點，具體的內容請參照書本。需要注意的是VINS在計算視覺雅各比時與書上有三點不同：
①VINS中由feature_tracker傳過來的像素點座標吸收了內參K且做了歸一化，因此是吸收內參的歸一化圖像座標。
②VINS中使用Q旋轉四元數來優化位姿，因此在計算位姿對應的雅各比矩陣時和書上的公式不完全相同。
③VINS是基於滑動窗口，優化的位姿包含滑動窗口中的11幀位姿，但同時是依靠特徵點來確定所優化哪兩幀的位姿，每次傳入視覺殘差模型是遍歷特徵點來確定的，設定i幀爲最開始觀測到次特徵點的相機，則j（不等於i）就爲其他共視關鍵幀。
4.1視覺j幀殘差計算（其實也可以計算i幀殘差，過程基本相似）
明白上面三個不同點就可以進行下面的公式推導了，首先下圖是殘差計算模型，VINS選取的是j幀圖像的殘差計算模型。
4.2整體雅各比公式
傳入到上面的j幀視覺殘差模型的優化狀態量分別是7維的i幀位姿，7維的j幀位姿，7維的相機和IMU外參數以及1爲的特徵點逆深度。因此需要計算的雅各比如下圖所示：
VINS系統的所有結果的參考座標都是世界座標，同時都是對應IMU時刻在世界座標系下的旋轉，平移，速度等，而且逆深度的值時相對於i時刻的相機，因此我們需要整理4.1得到的i時刻的關鍵幀對應到j時刻的歸一化相機座標Pj的重投影方程如下圖所示：（我們需要求的是Pj對於i、j時刻關鍵幀，相機外參以及i時刻的逆深度，所以需要構建這樣一個重投影方程，才能滿足偏導數求解）
由最終的公式可以看到Pj由i，j時刻的位姿（旋轉和平移），相機外參（旋轉和平移）以及逆深度表示，下面可以直接求雅各比（偏導數）。
4.3視覺i幀的狀態量（位移和旋轉）在Pj下的雅各比
這裏和視覺SLAM十四講中有出入，但是解算原理基本相同，利用了旋轉四元數和差乘的性質。得到下面的偏導後整個J[0]就可以相乘得到了。
4.4視覺j幀的狀態量、相機和IMU之間外參數（位移和旋轉）在Pj下的雅各比
這裏的雅各比推導過程就不重複了，和上面的過程相似，就直接給結果了
4.6特徵點你深度在Pj下的雅各比
逆深度求偏導就比較簡單了，就是一個反函數求偏導過程，得到下面的偏導後整個J[3]就可以相乘得到了。
至此，整個基於滑動窗口的視覺殘差和雅各比模型就完成了。其協方差矩陣較爲簡單，代碼中直接賦值即可，不需要計算。
第二部分的系統入口是estimator_node.cpp，（整體介紹未完待續）
1. estimator_node.cpp系統入口
首先初始化設置節點vins_estimator，同時讀取參數和設置相應的參數，爲節點發布相應的話題，爲節點訂閱三個話題，分別用來接收和保存IMU數據、圖像特徵數據和原始圖像數據，分別是在三個回調函數中imu_callback、feature_callback和raw_image_callback，每當訂閱的節點由數據送過來就會進入到相應的回調函數中。
（1） 接收IMU數據
imu_callback函數中首先執行imu_buf.push(imu_msg);將IMU數據保存到imu_buf中，同時執行con.notify_one();喚醒作用於process線程中的獲取觀測值數據的函數，這裏喚醒以及互斥鎖的作用很重要到下面真正要使用的時候在詳細討論，然後預測未考慮觀測噪聲的p、v、q值，同時將發佈最新的IMU測量值消息（pvq值），這裏計算得到的pvq是估計值，注意是沒有觀測噪聲和偏置的結果，作用是與下面預積分計算得到的pvq（考慮了觀測噪聲和偏置）做差得到殘差。
（2） 接收原始圖像和圖像特徵點數據
feature_callback和raw_image_callback函數中主要是將特徵數據和原始圖像數據分別保存到feature_buf和image_buf中，在feature_callback也用到了con.notify_one()和互斥鎖;。
2. process()處理觀測值數據線程
（1） 得到觀測值（IMU數據和圖像特徵點數據）
定義觀測值數據類measurements，包含了一組IMU數據和一幀圖像數據的組合的容器，這裏比較有意思的是使用了互斥鎖和條件等待的功能，互斥鎖用來鎖住當前代碼段，條件等待是等待上面兩個接收數據完成就會被喚醒，然後從imu_buf和feature_buf中提取觀測數據measurements = getMeasurements()，需要注意的是在提取觀測值數據的時候用到的互斥鎖會鎖住imu_buf和feature_buf等到提取完成才釋放，也就是說在提取的過程中上面兩個回調函數是無法接收數據的，同時上面兩個回調函數接收數據的時候也使用了互斥鎖，鎖住了imu_buf和feature_buf，這裏也不能提取imu_buf和feature_buf中的數據。因此整個數據獲取的過程是：回調函數接收數據，接收完一組數據喚醒提取數據的線程，提取數據的線程提取完數據後，回調函數就可以繼續接收數據，依次往復。這就是線程間通信的曼妙啊！
1） getMeasurements()返回觀測值數據
函數的作用顧名思義，就是得到一組IMU數據和圖像特徵數據組合的容器。首先保證存在IMU數據和圖像特徵數據，然後還要判斷圖像特徵數據和IMU數據是否對齊。這裏使用的是隊列數據結構（先進先出front是先進的數據，back是後進的數據），需要滿足兩個條件就能保證數據對齊，第一是IMU最後一個數據的時間要大於圖像特徵最開始數據的時間，第二是IMU最開始數據的時間要小於圖像特徵最開始數據的時間。滿足數據對齊就可以數據從隊列中按對齊的方式取出來。這裏知道把緩存中的圖像特徵數據或者IMU數據取完，才能夠跳出此函數，返回數據。
（2） 處理IMU數據和圖像特徵數據
步驟1：處理IMU數據
遍歷調用send_imu(imu_msg)將單個IMU數據的dt，線加速度值和角加速度值計算出來送給優化器處理，優化器調用estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry,rz));方法。
1）Estimator::processIMU(doubledt, const Vector3d &linear_acceleration, const Vector3d&angular_velocity)處理IMU數據方法
步驟1調用imu的預積分，調用push_back函數，函數中將時間，加速度和角速度分別存入相應的緩存中，同時調用了propagation函數 ,計算對應的狀態量、協方差和雅可比矩陣
①propagate(double _dt, const Eigen::Vector3d &_acc_1, constEigen::Vector3d &_gyr_1)
預積分傳播方程，在預積分傳播方程propagate中使用中點積分方法midPointIntegration計算預積分的測量值，中點積分法中主要包含兩個部分，分別是得到狀態變化量result_delta_q，result_delta_p，result_delta_v，result_linearized_ba，result_linearized_bg和得到跟新協方差矩陣和雅可比矩陣（注意，雖然得到了雅各比矩陣和協方差矩陣，但是還沒有求殘差和修正偏置一階項的狀態變量），由於使用的是中點積分，所以需要上一個時刻的IMU數據，包括測量值加速度和角速度以及狀態變化量，初始值由構造函數提供。需要注意的是這裏定義的delta_p等是累積的變化量，也就是說是從i時刻到當前時刻的變化量，這個纔是最終要求的結果（爲修正偏置一階項），而result_delta_q等只是一個暫時的變量，最後殘差和雅可比矩陣、協方差矩陣保存在pre_integrations中，還有一個函數這裏暫時還沒有用到，是在優化的時候才被調用的，但是其屬於預積分的內容，evaluate函數在這個函數裏面進行了狀態變化量的偏置一階修正以及殘差的計算。
步驟2預積分公式（3）未考慮誤差，提供imu計算的當前旋轉，位置，速度，作爲優化的初值
步驟2：處理圖像特徵數據
這裏進來的數據不是圖像數據哦，而是前面已經跟蹤匹配好的歸一化平面座標。將當前幀的特徵存放在image中，image的第一個元素類型是特徵點的編號，第二個元素是相機編號，第三個是特徵點座標，然後直接進入到處理圖像特徵數據的線程中estimator.processImage(image, img_msg->header)。
1）Estimator::processImage(constmap<int, vector<pair<int, Vector3d>>> &image, conststd_msgs::Header &header)處理圖像特徵數據方法
首先對進來的圖像特徵數據根據視差判斷是否是關鍵幀，選擇丟棄當前幀（但保留IMU數據）或者丟棄滑動窗口中最老的一幀。
步驟1：將圖像數據和時間存到圖像幀類中：首先將數據和時間保存到圖像幀的對象imageframe中（ImageFrame對象中包含特徵點，時間，位姿R，t，預積分對象pre_integration，是否是關鍵幀）,同時將臨時的預積分值保存到此對象中（這裏的臨時預積分初值就是在前面IMU預積分的時候計算的），然後將圖像幀的對象imageframe保存到all_image_frame對象中（imageframe的容器），更新臨時預積分初始值。
步驟2：標定相機和IMU的外參數：接着如果沒有外部參數就標定外部參數，參數傳遞有的話就跳過這一步（默認有，如果是自己的設備，可以設置爲2對外參進行在線標定）。
步驟3：初始化系統同時進行BA優化：當求解器處於可初始化狀態時（初始狀態是可初始化，初始化成功就設置爲不可初始化狀態），判斷當前frame_count是否達到WINDOW_SIZE，確保有足夠的frame參與初始化，這裏的frame_count是滑動窗口中圖像幀的數量，一開始被初始化爲0，滑動窗口總幀數是10。有外部參數同時當前幀時間戳大於初始化時間戳0.1秒，就進行初始化操作。
步驟3.1：initialStructure()系統初始化，首先初始化Vision-only SFM，然後初始化Visual-Inertial Alignment，構成整個初始化過程。
①保證IMU充分運動，通過線加速度判斷，一開始通過線加速度的標準差（離散程度）判斷保證IMU充分運動，加速度標準差大於0.25則代表imu充分激勵，足夠初始化。
②純視覺初始化，對SlidingWindow中的圖像幀和相機姿態求解sfm問題，這裏解決的是關鍵幀的位姿和特徵點座標。
步驟1.首先構建SFMFeature對象sfm_f，SFMFeature數組中包含了特徵點狀態（是否被三角化），id，2d點，3d座標以及深度，將特徵管理器中的特徵信息保存到SFMFeature對象sfm_f中sfm_f.push_back(tmp_feature)。
步驟2.接着由對極約束中的F矩陣恢復出R、t，主要調用方法relativePose(relative_R, relative_T, l)。relativePose方法中首先通過FeatureManeger獲取（滑動窗口中）第i幀和最後一幀的特徵匹配corres，當corres匹配足夠大時，考察最新的keyFrame和slidingwindow中某個keyFrame之間有足夠feature匹配和足夠大的視差（id爲l=i），滿足這兩個條件，然後這兩幀之間通過五點法恢復出R，t並且三角化出3D的特徵點feature point，這裏是使用solveRelativeRT(corres, relative_R, relative_T)，solveRelativeRT方法定義在solv_5pts.cpp類中，由對極約束中的F矩陣恢復出R、t，直接調用opencv中的方法，沒什麼好說的，這裏值得注意的是，這種relativePose得到的位姿是第l幀的，第l幀的篩選是從第一幀開始到滑動窗口所有幀中一開始滿足平均視差足夠大的幀，這裏的第l幀會作爲參考幀到下面的全局SFM使用。到這裏就已經得到圖像的特徵點2d座標的提取，相機第l幀和最後一幀之間的旋轉和平移（注意暫時還沒有得到特徵的3d點座標），有了這些信息就可以構建全局的SFM類GlobalSFM sfm，在這裏調用sfm.construct(frame_count + 1, Q, T, l,relative_R, relative_T,sfm_f,sfm_tracked_points)，這裏以第l幀作爲參考幀，在進行PNP求解之前，需要判斷當前幀數要大於第l幀，這保證了第l幀直接跳過PNP步驟，首先執行下面的第l幀和最後一幀的三角化，得到共視的特徵點，供下面第l+1幀和最後一幀求解PNP，然後利用pnp求解l+1幀到最後一幀的位姿R_initial,P_initial，最後的位姿都保存在Pose中，一次循環，得到l+1，l+2…n-1幀的位姿。跳出步驟2 的循環後，至此得到了l+1，l+2…n-1幀的位姿以及l+1，l+2…幀與n-1幀的特徵點三角化。然後再三角化l幀和i幀（在第l幀和最後一幀之間的幀）之間的3d座標，（這裏不明白爲什麼要做兩次，是可以三角化出更多的特徵點嗎？？？？），接着PNP求解l-1，l-2…0幀和l幀之間的位姿已經三角化相應的特徵點座標，最後三角化其他所有的特徵點。至此得到了滑動窗口中所有相機的位姿以及特徵點的3d座標。第6部就是進行BA優化，使用的是ceres優化位姿和特徵點，這裏可以參考視覺SLAM第十講中的內容，優化方式相同。
步驟4：visualInitialAlign中調用VisualIMUAlignment方法，真正的視覺慣性聯合初始化，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矩陣的轉置得到的AT*A一定是可逆的，因此就可以直接兩邊同乘以其逆即可，相應的說明詳見LDLT方法。得到陀螺儀偏置之後將其值保存到前面定義的Bgs[]中，最後在重新計算一次預積分。
步驟4.2：LinearAlignment計算尺度，重力加速度和速度。論文中給出的公式是相鄰兩個速度的模型，映射到整個n+1個速度模型中，A矩陣一定是一個正定矩陣（實對稱矩陣），代碼中定義的A和b即是最總的H和b，tmp_A和tmp_b相鄰速度間的臨時變量。最後的求解方法：x = A.ldlt().solve(b)；然後調用RefineGravity重新計算重力加速度方向，得到最優解。
步驟4.3：將所有的狀態量由相機座標C0轉換到世界座標W下，這裏世界座標系的定義是根據重力加速度的方向和初始相機C0座標系的方向定義的R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; 定義的rot_diff就是座標旋轉矩陣q_wc0。最後將Ps[i]、Rs[i]和Vs[i]由相機C0座標系旋轉到定義的世界座標下，這裏需要說明，一般的SLAM系統世界座標系即是C0座標系，這裏自定義了一個世界座標系，那麼最後得到的結果都是以這個世界座標系爲參考，如果想要和真值進行比較就需要注意座標系問題了。
2） 基於滑動窗口的緊耦合優化
這部分主要是在solveOdometry()和slideWindow()方法中，當初始化完成後就會調用這兩個方法。
步驟1：solveOdometry()進行BA優化，在內部調用了BA優化的方法optimization()
步驟1.未完待續