感謝“slam萌新”，本篇博客部分參考：
https://blog.csdn.net/weixin_41843971/article/details/86748719
歡迎討論。
慣性視覺里程結果與GPS松耦合：
github開源的是KITTI的數據集的測試代碼。跟着程序走一遍。
KITTIGPSTest.cpp
主程序入口:
vins_estimator包下面的KITTIGPSTest.cpp,主要作用：
（1）開啓estimator類，進行vio里程估計
（2）從文件中讀取視頻圖片列表，讀入estimator類中
（3）從文件中讀取GPS數據列表，直接通過ROS發佈出去
具體的，從文件中獲取圖像和GPS數據當前的時間戳信息。以第一幀圖像和第一個GPS時間早的作爲基準時間，之後，左右雙目的圖像通過estimator.inputImage（）讀入里程計中，里程計Estimator類內部會定時發送VIO計算的結果。同時main函數中會在圖片讀入里程計的時刻發佈同一時刻的GPS信息。（每一幀圖片都有對應一條GPS信息，時間戳設置爲一致的）
GPS融合主要發生在global_fusion包中。
global_fusion
程序同步開啓了global_fusion節點。
rosrun global_fusion global_fusion_node
程序入口globalOptNode.cpp
程序定義了一個GlobalOptimization類globalEstimator來進行融合處理。
程序有三個回調函數：
（1）publish_car_model():根據最終vio與GPS融合的定位結果，發佈在特定位置的一個炫酷的小車模型。
（2）GPS_callback()：通過globalEstimator.inputGPS（），放入全局融合器中。
（3）vio_callback()：通過globalEstimator.inputOdom（），放入全局融合器中。並且從全局融合器globalEstimator中取出最終位姿融合的結果，調用publish_car_model()發佈出來。
最重要類的核心爲 GlobalOptimization類 和類內的optimize（）函數。
GlobalOptimization類
類成員：
(1)map類型
localPoseMap中保存着vio的位姿
GPSPositionMap中保存着gps數據
globalPoseMap中保存着優化後的全局位姿
以上類成員中map的格式:
map<double,vector<double>> =<t,vector<x,y,z,qw,qx,qy,qz>>
(2)bool類型
initGPS:第一個GPS信號觸發
newGPS:有新的GPS信號後觸發
(3)GeographicLib::LocalCartesian 類型
geoconverter GPS經緯度信號轉化爲X,Y,Z用到的第三方庫
當該類已進行初始化，就同步開啓了新線程optimize（），對兩個結果不斷進行優化。
optimize（）
（1）當有新的GPS信號到來時候，進行GPS與視覺慣性的融合
（2）建立ceres的problem
- lossfunction 設置爲Huberloss
- addParameterBlock添加優化的變量 ,優化的變量是q_array以及t_array。即globalPoseMap保存下來的每幀圖像的位姿信息。其中參數變量的多少是由localPoseMap來決定的。即VIO有多少個數據，全局也就有多少個。迭代器指向的first爲時間，second爲7變量的位姿。其中在添加q_array由於維度只有三維，因此增加了local_parameterization來進行約束。
- 接着開始添加殘差項，總共有兩個殘差項分別是：vio factor以及gps factor
– vio factor：殘差項的costfunction創建由 relativeRTError來提供。觀測值由vio的結果提供。此時計算的是以i時刻作爲參考，從i到j這兩個時刻的位移值以及四元數的旋轉值作爲觀測值傳遞進入代價函數中。 此時iPj代表了i到j的位移，iQj代表了i到j的四元數變換。添加殘差項的時候，需要添加當前i時刻的位姿以及j時刻的位姿。即用觀測值來估計i時刻的位姿以及j時刻的位姿。
– gps factor：殘差項的costfunction創建由 TError來提供。觀測值由Gps數據的結果提供。添加殘差項的時候，只需要添加當前i時刻的位姿。 - 求解非線性優化方程
- 求解出來後，把t_array和q_array（即兩個優化的變量）賦值給globalposeMap。並且根據最新解算出來的結果（即i=length-1時刻最新的結果），跟新GPS到vio這兩個獨立體系之間座標轉換關係。
TError及RelativeRTError
直觀上理解：
{0, 1，2，3，4, 5，6…}
要估計出這些時刻，每個時刻的位姿。
我有的是兩個方面的觀測值，一方面是GPS得到的每個時刻的位置（x,y,z）（並且GPS信號可以提供在該時刻得到這個位置的精度posAccuracy），沒有累計誤差，精度較低。另一方面是VIO得到的每個時刻的位置（x,y,z）以及對應的姿態四元數（w,x,y,z）,存在累計誤差，短時間內精度較高。爲了得到更好的一個融合結果，因此我們採用這個策略：觀測值中，初始位置由GPS提供，並且vio觀測值信任的是i到j時刻的位移以及姿態變化量。 並不信任vio得到的一個絕對位移量以及絕對的旋轉姿態量。只信任短期的i到j的變化量，用這個變化量作爲整個代價函數的觀測值，進行求解。
因此兩個殘差項TError及RelativeRTError分別對應的就是GPS位置信號以及vio短時間內估計的i到j時刻的位姿變化量對最終結果的影響。迭代求解的過程中均採用了AutoDiffCostFunction自動求解微分來進行迭代。
（1）TError
TError（x,y,z,accuracy），最後一項是定位精度，可以由GPS系統提供。殘差除了直接觀測值與真值相減以外，還除了這個accuracy作爲分母。意味着精度越高，accuracy越小，對結果的影響就越大。
（2）RelativeRTError
RelativeRTError（dx,dy,dz,dqw,dqx,dqy,dqz，t_var,q_var），最後兩項是位移以及旋轉之間的權重分配比例，並且爲了使得與TError對應。在程序中，應該是根據經驗把最後兩項設置成一個常值，分別對應0.1以及0.01。殘差的得到就根據實際值與觀測值之間的偏差直接得出。