MSCKF-VIO源碼框架及C++知識點總結

摘要

閱讀源碼是最有效的學習方式，不僅可以從中理清作者的思路以及工程實現細節，還可以學到相應的編程知識，可謂一舉多得。本文主要從代碼架構以及c++實現兩個方向對msckv-vio進行解剖，有關msckf-vio的介紹詳見知乎專欄MSCKF那些事。有關數學推導以及流程，知乎專欄裏已經很詳細很棒棒的了，在此不做重複性的工作，力求方便後來者快速入門。本人也是一邊看着知乎專欄的介紹，一邊閱讀源碼的。技術博客，本身入門菜鳥一個，但必定盡心盡力，也難免有錯誤之處，敬請留言批評指出，不甚感激，亦歡迎多多交流。
知乎專欄MSCKF那些事
 msckf-vio github地址
 本人註釋過的源碼
 參考論文1：Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight
參考論文2：Paper：A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation
參考論文3：Report:A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation
參考論文4：Observability-constrained Vision-aided Inertial Navigation

MSCKF-VIO程序架構

MSCKF-VIO是雙目版的MSCKF，一個基於卡爾曼濾波的VIO系統，注意只是一個里程計，沒有迴環和地圖複用，所以算不上一個Slam框架。
MSCKF-VIO是我見過的最簡單的一個VIO框架，只有5個cpp程序文件，整個算法實現部分全部在msckf_vio和image_processor兩個cpp文件裏,一共只有程序可讀性非常好，對於初學者而言是很好的入門級算法框架，而且根據論文後面的評測，該算法擁有與主流優化算法VINS-Mono（關閉loop closure功能）相當的性能（不過一個是單目，一個是雙目，比較有點牽強，但濾波VIO能達到這樣的效果已經非常棒了），且與基於雙目優化的OKVIS有相當的性能，但計算效率卻非常高，能達到20Hz，實現了性能與計算效率上的平衡。
代碼結構非常清晰，分爲前端和後端兩個部分，分別在image_processor.cpp和msckf-vio.cpp程序文件裏。

前端

前端流程圖

前端主要進行視覺特徵的提取，跟蹤匹配以及移除outlier。

函數功能解讀

下面就各個函數模塊進行功能解剖：
createImagePyramids():調用opencv的創建金字塔函數，創建3層雙目圖像cam0，cam1金字塔。

initializeFirstFrame():收到第一幀圖像之後進行初始化，主要步驟包括：
提取FAST角點—>KL光流法進行雙目匹配—>圖像區域分成4x5塊，並將匹配的點分配到各個塊中—>方塊裏選取response較大的2個角點。經過這幾步,使得提取的特徵點儘可能均勻分佈,且角點響應強度好。

trackFeatures():跟蹤上一幀中的特徵點，主要分以下步驟：
利用IMU角速度積分得到旋轉矩陣,預測上一幀cam0中的特徵點在當前cam0中的位置—>KL光流追蹤特徵點,並去除界外的點以及未被跟蹤上的點—>去除外點，這又包括三個步驟：
step1：去除當前幀cam0中與cam1匹配不上的點
step2：利用RANSAC算法去除當前幀cam0中與前一幀cam0基礎矩陣約束失效的匹配點
step3：利用RANSAC算法去除當前幀cam1中與前一幀cam1基礎矩陣約束失效的匹配點
這裏用的ransac算法一開始的時候沒看懂，後來通過閱讀2-point RANSAC明白就是基於F矩陣的RANSAC算法，只不過它利用了兩幀間的陀螺儀積分得到的旋轉矩陣，所以RANSAC的時候只需要計算平移矩陣，加速了這個過程。經過這一步得到的匹配關係有如下特點：和前一幀匹配上，雙目能匹配上，基礎矩陣幾何約束。

addNewFeatures() :經過上面的匹配和篩選，當前幀的特徵點數量一般會低於最低數量要求，所以該步驟會檢測cam0已經追蹤的特徵點之外的新的fast特徵點,然後在cam1中尋找匹配點,並在圖像塊中添加response較大的特徵點，這樣就能保證新的幀裏有足夠多且分佈均勻的特徵點。

pruneGridFeatures(): 如果網格里特徵點太多,則去除lifetime較小的特徵點,優先保留一直在被跟蹤的特徵點。特徵點每被跟蹤一次，其lifetime就會加1。當前幀圖像塊裏的特徵點來自追蹤+新產生，而產生新特徵點的過程保證了圖像塊中的特徵點數量不會低於最少特徵點數的要求，所以驟步起作用的前提在於前一幀不同網格的太多特徵點投影到當前幀同一個分塊圖像中來了。

publish() :發佈當期幀特徵點的去畸變歸一化平面上的座標以及跟蹤信息，每個座標有4個參數，代表該特徵點在雙目歸一化平面上的座標。跟蹤信息包括該幀的時間戳，跟蹤前後的特徵點數量，雙目匹配去除外點前後的特徵點數量，RANSAC去除外點前後的特徵點數量。

更新:狀態更新將當前幀的信息賦予上一幀，然後對當前幀信息重置。
通過以上步驟分析，可知跟蹤的約束很多，保證了跟蹤的準確性，論文裏也提及前端耗時佔整個算法的80%。

前端各主要函數模塊耗時分析

函數	平均耗時(s)
createImagePyramids()	0.00377
initializeFirstFrame()	0.01900
trackFeatures()	0.00455
addNewFeatures()	0.00677
pruneGridFeatures()	1.866e-6
publish()	8.542e-5
total_time	0.01526

備註：
用了四捨五入，可能total_time會比前面的和小一點點。
測試條件：Dell G7-7588 ； intel i7-8750H @2.2GHz； ubuntu6.04
數據集：msckf-vio的github上推薦的EuRoC ROS Bags其中的Vicon Room 1 01，一共2892幀圖像。

從表中可以看出前端大部分時間耗在trackFeatures() 和 addNewFeatures() 兩部分，如果要加速應該在這部分進行優化，每秒能處理65幀，已經很高了。initializeFirstFrame雖然耗時多，但只會執行一次，不會對運行效率產生影響。

後端

後端進行的任務主要有重力和陀螺儀bias的初始化以及EKF狀態更新。
重力和陀螺儀bias的初始化較簡單，initializeGravityAndBias()，靜止狀態下利用前200個imu數據，計算出陀螺儀的均值即爲bias初始值（靜止狀態下陀螺儀真實值爲0），加速度測量均值作爲重力在imu座標系下的近似真實值，根據重力在世界座標系下的真實值以及在imu座標系下的測量值，可以求出初始狀態時imu系相對世界座標系的pose，初始化時世界座標系原點與imu系原點重合。所以一定要注意，初始化的時候要使系統處於靜止狀態，否則會初始化失敗，前端能匹配成功，但後端沒有里程計輸出的。

後端流程圖

EKF過程如下：

圖中省略了一個is_first_img，判斷是否爲第一幀，是第一幀圖像就將IMU狀態的時間設爲這幀圖像，並將is_first_img=false，之後就不會再進入（如果系統收到重置信號，is_first_img會變成true）

函數功能解讀

下面就各個函數模塊進行功能解剖：
batchImuProcessing(): 處理當前幀與上一幀之間的IMU數據，得到當前幀IMU的位姿以及方差，其數學推導見開頭提及的知乎專欄，不同的一點是程序有Modify the transition matrix，與msckf-vio論文提及的III-C:Observability Constraint有關,這塊內容知乎專欄沒講清楚，也比較難懂，其數學推導詳見參考論文4：Observability-constrained Vision-aided Inertial Navigation

stateAugmentation(): 收到新的圖像cam，就要將原來的協方差矩陣擴增，已知IMU的狀態及其協方差矩陣，IMU-CAM之間的位姿變換關係，可以得到cam的位姿及協方差矩陣，其數學推導見MSCKF那些事（六）算法詳解4：State Augmentation。在這裏當前幀的位姿與IMU積分推導的位姿存在協方差，後面對當前幀位姿的更新動力來源與該協方差而不是來自於觀測值；而IMU的位姿又與前一幀時刻的相機位姿存在協方差，之後IMU位姿更新的直接動力來自於該協方差。

addFeatureObservations(msg):添加當前幀裏的特徵：如果是新的特徵點,則將其加入到map_server中，如果是之前存在的特徵點,則將當前幀加入到該點的觀測之中。

removeLostFeatures():這個函數名取得有點名不副實，正如論文裏所講的，如果有特徵點丟失跟蹤了，則用這些特徵點來進行EKF更新，該函數就是EKF更新過程，分爲如下三步：
step1):三角化當前幀裏track lost的特徵點,能三角化的特徵點被觀察次數超過2次,且通過相機運動檢測,三角化推導詳見msckf論文的Appendix
step2):計算失去跟蹤的特徵點的觀測殘差對相機位姿和特徵點三維座標的雅克比矩陣,最終得到EKF裏的觀測方程
step3):EKF狀態更新
其中的數學推導詳見MSCKF那些事（七）算法詳解5：Measurement Update

pruneCamStateBuffer(): 改步移除狀態窗口中多餘的相機狀態，如果相機狀態達到上限，則每次移除兩個，所以程序運行的時候可以看到狀態窗口中相機狀態數量一直維持在比上限少1個或者2個（不計重置）。分爲以下n步:
step1):選出要移除的兩幀圖像I1,I2:比較最新第2,3幀相較最新第4幀的移動量,如果發現移動量太小,則移除這些幀,否則移除最老的幀
step2):map_serve裏的特徵點除去I1,I2觀測,如果不能被滑動窗口中的觀測(此時還沒移除I1,I2)初始化,則去除該特徵點。
step3): 求狀態偏差對I1,I2偏差的雅可比矩陣以及相應的觀測誤差，類似之前的方法進行EKF更新,這個對狀態估計的優化性能有多少,值得懷疑。
step4): 移除I1,I2及其對應的協方差

publish(msg->header.stamp) : 發佈狀態消息:IMU位姿T_i_w（相對世界座標系的旋轉矩陣+位置），在世界座標系中的速度及協方差矩陣，初始化了的特徵點的三維座標。

onlineReset() :如果系統的位置標準差超過一定的閾值,就對系統進行重置。對state_server內的相機清零,map_server的特徵點清零,IMU狀態的方差重新初始化,其中pose和position的方差設爲0,但IMU的位置,速度,pose並沒變

後端各主要函數模塊耗時分析

函數	平均耗時(s)
batchImuProcessing()	0.00215
stateAugmentation()	0.00016
addFeatureObservations()	3.63e-5
removeLostFeatures()	0.00210
pruneCamStateBuffer()	0.00892
publish()	2.69e-5
total_time	0.01340
onlineReset()次數	0

分析：對比可知，後端耗時略低於前端，每秒可以處理74幀數據，這與論文裏講的前端消耗80%的時間不符，這只是一家之言，如果您有不同的結論，歡迎在評論區指出，不甚感激！另外後端主要耗時在removeLostFeatures()和pruneCamStateBuffer()上，前者是論文提及的EKF濾波過程，後者是移除多餘的幀，同時還運行了EKF過程，這個函數一般各一次才運行一次，也就是說實際改函數運行時間爲表格中的兩倍，單獨記錄的結果驗證了這個推斷，如果後端要做優化，可以從這兩個函數，特別是後一個函數處着手。但後端速度已經很快了，沒有加速的必要。

運行過程分析

看論文時一直以爲每次接收到新的幀，EKF算法會用當前幀的觀測來立即更新該幀的位姿，其實不然，先看下列一段運行時狀態窗口內的相機ID記錄：

============================================
這是第19次進入FeatureCallback函數;擴增之前cam數:18其ID分別是
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 
擴增之後最新一幀圖像由IMU計算得到的cam位姿爲Q:
-0.579322 -0.591993  0.385008  0.407062位置:-0.00106238  -0.0685355  -0.0234336
擴增之後cam數:19其ID分別是
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 
修正的IMU旋轉量-0.000428545   0.00073752  0.000376173  1
修正的IMU位移 0.00839804   -0.004347 -0.00124921
最新幀的上一幀的位置修正量:
 0.00758024 -0.00415305 -0.00120079四元數修正量
0.000679556 0.000407112 0.000366278           1
最新幀的位置修正量:
  0.0083858 -0.00432602  -0.0013043四元數修正量
0.000719419 0.000431556 0.000388199           1
觀測涉及到的camID數量爲:9其ID分別是
9 10 11 12 13 14 15 16 17 
EKF更新後最新幀位姿爲Q:  
-0.579425 -0.591315  0.385342  0.407585位置:  0.00732342 -0.0728615 -0.0247379
EKF狀態更新之後cam數:19其ID分別是
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 
去掉多餘的之後cam之後最新幀位姿爲Q:  
-0.579425 -0.591315  0.385342  0.407585位置:  0.00732342 -0.0728615 -0.0247379
去掉多餘的之後cam數:19其ID分別是
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 
============================================
這是第20次進入FeatureCallback函數;擴增之前cam數:19其ID分別是
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 
擴增之後最新一幀圖像由IMU計算得到的cam位姿爲Q:
-0.579454 -0.591189   0.38541  0.407662位置:0.00800001 -0.0735398 -0.0245077
擴增之後cam數:20其ID分別是
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 
EKF更新後最新幀位姿爲Q:  
-0.579454 -0.591189   0.38541  0.407662位置:  0.00800001 -0.0735398 -0.0245077
EKF狀態更新之後cam數:20其ID分別是
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 
修正的IMU旋轉量 3.00171e-05 -3.60735e-05  4.30325e-06    1
修正的IMU位移-0.000353244  0.000264745  3.81908e-05
最新幀的上一幀的位置修正量:
-0.00031921 0.000239285  4.0622e-05
四元數修正量
-3.37154e-05 -2.89993e-05  3.39526e-06            1
最新幀的位置修正量:
-0.000355567  0.000263274  4.20609e-05四元數修正量
 -3.5618e-05 -3.05683e-05   3.5938e-06            1
去掉多餘的之後cam之後最新幀位姿爲Q:  
-0.579459 -0.591213  0.385408  0.407622位置:  0.00764444 -0.0732765 -0.0244656
去掉多餘的之後cam數:18其ID分別是
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 19 
============================================
這是第21次進入FeatureCallback函數;擴增之前cam數:18其ID分別是
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 19 
擴增之後最新一幀圖像由IMU計算得到的cam位姿爲Q:
-0.579557 -0.591114  0.385435    0.4076位置:0.00839501 -0.0739196 -0.0243574
擴增之後cam數:19其ID分別是
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 19 20 
EKF更新後最新幀位姿爲Q:  
-0.579557 -0.591114  0.385435    0.4076位置:  0.00839501 -0.0739196 -0.0243574
EKF狀態更新之後cam數:19其ID分別是
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 19 20 
去掉多餘的之後cam之後最新幀位姿爲Q:  
-0.579557 -0.591114  0.385435    0.4076位置:  0.00839501 -0.0739196 -0.0243574
去掉多餘的之後cam數:19其ID分別是
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 19 20 
============================================
這是第22次進入FeatureCallback函數;擴增之前cam數:19其ID分別是
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 19 20 
擴增之後最新一幀圖像由IMU計算得到的cam位姿爲Q:
-0.579611 -0.591035  0.385434  0.407638位置:0.00920391 -0.0745504 -0.0242512
擴增之後cam數:20其ID分別是
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 19 20 21 
EKF更新後最新幀位姿爲Q:  
-0.579611 -0.591035  0.385434  0.407638位置:  0.00920391 -0.0745504 -0.0242512
EKF狀態更新之後cam數:20其ID分別是
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 19 20 21 
修正的IMU旋轉量  1.5361e-05 -8.06942e-05  3.24655e-05   1
修正的IMU位移-0.000627034  0.000398831  5.94123e-05
最新幀的上一幀的位置修正量:
-0.000563014  0.000356974  6.50779e-05
四元數修正量
 -7.7338e-05 -1.57806e-05  2.91949e-05            1
最新幀的位置修正量:
-0.000635169  0.000398084  6.14035e-05四元數修正量
-8.11996e-05 -1.65586e-05   3.0635e-05            1
去掉多餘的之後cam之後最新幀位姿爲Q:  
-0.579656 -0.591056  0.385408  0.407569位置:  0.00856874 -0.0741523 -0.0241898
去掉多餘的之後cam數:18其ID分別是
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 21 
============================================

從中我們可以看出以下結論：

觀測值並沒有涉及到當前幀，當前幀是如何進行更新的呢？查看源碼，狀態協方差矩陣裏，當前幀和上一幀之間的協方差爲零，當前IMU與上一幀的協方差等於上一幀IMU與上一幀圖像位姿之間的協方差，這樣並不是精確的，有可能是這兩個協方差並不那麼好計算的緣故，如此推斷，觀測會更新上一幀，IMU與它的協方差引起當前IMU的更新，而當前圖像幀位姿與IMU之間的協方差更新當前位姿。
當狀態裏的cam數量達到最大數量20時，會一次移除掉兩個，這兩個是從最老的一幀以及當前幀的前兩幀裏挑選，這也印證了前面所說的移除多餘幀的操作正常情況下每隔一次才調用一次，該函數是所有函數裏最耗時的。
pruneCamStateBuffer()函數的意義：一般將此操作視爲優化裏的邊緣化，與removeLostFeatures()類似，都採用了EKF過程，不同的是removeLostFeatures()涉及到的觀測幀是能看到跟蹤丟失的特徵點的圖像幀，數量不定，而pruneCamStateBuffer()涉及到的是要被移除的幀，只有兩幀，但EKF算法的規模是一樣大的，相當於算法進行了兩次EKF過程（這兩個過程有可能涉及到同樣的幀，但map_serve事先已經剔除removeLostFeatures()涉及到的特徵點，所以保證不會重複），如果能將這兩個過程合併，必能提速不少。
本算法採用關鍵幀選取策略剔除多餘幀，可能是最老的一幀，也可能是最新的第二或者第三幀，每次剔除兩幀，下一次就不需要剔除，所以我們可以看到狀態窗口維護的幀ID可能非常老；此外IMU積分時間就是兩幀之間的時間間隔，保證其不會因積分時間長而飄走。
路標點的座標是通過多視圖幾何的方法來獲得的,類似於三角化,不過有多幀觀測該點的相機姿態,不同與優化裏同時優化路標點和相機位姿,如此形成循環,位姿優化的準確性取決於路標點的準確性,而路標點的準確性又取決於觀測到該點的相機位姿的準確性,理論上講,準確性應該不及優化算法.

ROS裏的信息流圖

C++編程知識

閱讀源碼既能有效地瞭解算法本身，特別是其實現細節，另外也非常有助於學習c++，特別是優秀代碼的編程規範，作爲一個半道出家的程序員，我也從中受益匪淺，在此我總結了這套源碼中用到的一些c++編程知識（在此假設讀者您已經具備c++基礎，特別是熟悉瞭解面向對象編程），這些也是我從中學到的新的知識點：

Boost智能指針——shared_ptr： 在ImageProcessor這個類裏有用到智能指針：

typedef boost::shared_ptr<ImageProcessor> Ptr，
typedef boost::shared_ptr<MsckfVio> Ptr， 
boost::shared_ptr<GridFeatures> prev_features_ptr，

主要對象用的都是這類指針。它是可以共享所有權的智能指針，多個這樣的智能指針可以指向同一片地址，且會智能地在合適的時候去自動釋放資源，不需要delete操作。它的實現機制其實比較簡單，就是對指針引用的對象進行引用計數，當有一個新的boost::shared_ptr指針指向一個對象時，就把該對象的引用計數加1，減少一個boost::shared_ptr指針指向一個對象時，就把對該對象的引用計數減1。當一個對象的引用計數變爲0時，就會自動調用其析構函數或者free掉相應的空間。具體詳見有關智能指針（shared_ptr）的討論。

std::map: Map是一種關聯容器，它按照特定順序存儲由鍵值Key和映射值Value組合而成的元素通過健Key值來查找對應的值Value。程序裏用它定義了多種關聯：

 typedef std::map<int, std::vector<FeatureMetaData> > GridFeatures 圖像塊序號及其關聯的特徵點的map對象 ;  
 std::map<FeatureIDType, int> feature_lifetime;將特徵點ID與其生存時間關聯； 
 map<FeatureIDType, Point2f> prev_points;將特徵點ID與點座標關聯；

使用案例：
賦值： prev_points[feature.id] = feature.cam0_point;不需要像數組或者vector那樣需要先知道size，分配空間然後才能複製，map可以直接賦值，非常方便。
訪問： for (const auto& item : *prev_features_ptr) { for (const auto& prev_feature : item.second)}，這個有點類似於vector的用法，通過迭代器訪問，每一個map對象的first是Key值，second是Value值。
查找： if (feature_lifetime.find(feature.id) == feature_lifetime.end()) 在map裏查找鍵Key值對應的對象，如果沒有，則返回end，有則返回指向第一個對象的迭代器。
std::map有如下特性：
1）關聯性：std::map 是一個關聯容器，其中的元素根據鍵來引用，而不是根據索引來引用；
2）有序性：在內部，std::map 中的元素總是按照其內部的比較器（比較器類型由Compare類型參數指定）指示的特定嚴格弱序標準按其鍵排序；
3）唯一性：std::map 中的元素的鍵是唯一的；
4）std::map 通常由二叉搜索樹實現。
更多內容詳見C++ std::map 用法詳解

std::sort: 這是一個排序函數，將vector裏的元素按照規則進行排序，比如源碼裏用到

for (auto& item : grid_new_features)
  std::sort(item.second.begin(), item.second.end(),&ImageProcessor::featureCompareByResponse);

其中：
static bool featureCompareByResponse(const FeatureMetaData& f1,const FeatureMetaData& f2）
                 {return f1.response > f2.response; }`for (auto& item : grid_new_features)

合起來就是按照response從大到小的順尋對item裏的元素進行排序。

std::vector: 容器，類似於一個數組，但其成員可以是任何對象實例，有相應的函數進行初始化，插入元素，移除元素，賦值等等，非常好用。如 std::vector<cv::Mat> curr_cam0_pyramid_就是建立了圖像金字塔的容器，每一個元素代表金字塔裏的一層。
ROS 代碼引入了ROS來方便快捷地處理數據，也是一次很好學習ROS的機會，主要涉及知識點如下：

	1)ros::NodeHandle nh;//獲取節點的句柄,這個是Starting the node
    2)getPrivateNodeHandle() 
    // Get the private node handle (provides this nodelets custom remappings in its private namespace)
    3)ros::Publisher odom_pub  //定義一個發佈者
      ros::Subscriber imu_sub; //定義一個訂閱者
      ros::ServiceServer reset_srv;//定義一個服務端
    4）odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);
     //在ROS MASTER(主機)中註冊，10代表其緩存區大小，odom是發佈的topic名，nav_msgs::Odometry是其消息類型
       imu_sub = nh.subscribe("imu", 100, &MsckfVio::imuCallback, this);
     //在ROS MASTER(主機)中註冊訂閱imu數據信息，參數分別是 訂閱的topic，緩存區大小,回調函數和訂閱該話題的指針,也就是自身,它會給發佈話題的訂閱者數量加1
        reset_srv = nh.advertiseService("reset", &MsckfVio::resetCallback, this);
     //在ROS MASTER中註冊重置reset服務端，參數分別是名稱,回調函數以及調用該服務的對象指針,也就是自身
    5）odom_pub.publish(odom_msg);//發佈消息
    6）nh.param<int>("grid_row", processor_config.grid_row, 4);
    //設置參數，如果參數服務器（由roslaunch傳遞進來的）裏有該參數則設爲此值，否則設爲末尾的默認值

注意,有關圖像消息的訂閱和發佈有所不同

圖像消息的訂閱:
message_filters::Subscriber<sensor_msgs::Image> cam0_img_sub;  
message_filters::Subscriber<sensor_msgs::Image> cam1_img_sub;
message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image> stereo_sub;//聲明定義
cam0_img_sub.subscribe(nh, "cam0_image", 10);//註冊訂閱cam0_image
cam1_img_sub.subscribe(nh, "cam1_image", 10);//註冊訂閱cam1_image
stereo_sub.connectInput(cam0_img_sub, cam1_img_sub);//關聯兩個cam消息
stereo_sub.registerCallback(&ImageProcessor::stereoCallback, this);//回調函數

圖像消息的發佈:
image_transport::Publisher debug_stereo_pub;//定義
debug_stereo_pub = it.advertise("debug_stereo_image", 1);//註冊發佈 debug_stereo_image
if(debug_stereo_pub.getNumSubscribers() > 0){
      debug_stereo_pub.publish(debug_image.toImageMsg()); }  //發佈

此外，不同於我們大多數見過的ros程序，該項目裏並沒有見到ros::init()，可能與其ros節點的初始化方法有關：用getPrivateNodeHandle() 來初始化類對象ros節點nh。

MSCKF-VIO源碼框架及C++知識點總結

MSCKF-VIO源碼框架及C++知識點總結

摘要

MSCKF-VIO程序架構

前端

前端流程圖

函數功能解讀

前端各主要函數模塊耗時分析

後端

後端流程圖

函數功能解讀

後端各主要函數模塊耗時分析

運行過程分析

ROS裏的信息流圖

C++編程知識

通過HPA+CronHPA組合應對業務複雜彈性伸縮場景

Apollo進階課程第四章：高精地圖

Apollo進階課程第七章：感知

Apollo進階課程第六章：Appolo ROS

三維點雲處理技術四：三維點雲數據處理基礎

算法導論第十二章：二叉查找樹

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

linux以太網驅動總結