Vins-Fusion源碼：https://github.com/HKUST-Aerial-Robotics/VINS-Fusion
摘要
應項目需要，側重學習stereo+gps融合
轉載幾篇寫的比較好的博客
1. 萌新學VINS-Fusion（三）------雙目和GPS融合
主函數文件
與GPS融合的程序入口在KITTIGPSTest.cpp文件中，數據為KITTI數據集
數據集為 http://www.cvlibs.net/datasets/kitti/raw_data.php
以 [2011_10_03_drive_0027_synced]為例
https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_10_03_drive_0027/2011_10_03_drive_0027_sync.zip
main函數與之前的main函數相似，都要進行ros節點初始化，然后讀取參數，區別在於該數據集的圖像和gps數據均為文件讀取方式，將gps數據封裝進ros 的sensor_msgs::NavSatFix 數據類型里，以gps為topic播發出去，而雙目圖像則直接放到estimator進行vio，將vio得到的結果再播發出去，方便后面的訂閱和與GPS的融合。
global_fusion
所有的與gps的融合在global_fusion文件夾中，該部分的文件入口是一個rosnode文件globalOptNode.cpp，主函數很短，如下
int main(int argc, char **argv) { ros::init(argc, argv, "globalEstimator"); ros::NodeHandle n("~"); global_path = &globalEstimator.global_path; ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback); ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback); pub_global_path = n.advertise<nav_msgs::Path>("global_path", 100); pub_global_odometry = n.advertise<nav_msgs::Odometry>("global_odometry", 100); pub_car = n.advertise<visualization_msgs::MarkerArray>("car_model", 1000); ros::spin(); return 0; }
代碼很簡單，訂閱topic（/gps）在GPS_callback回調函數中接收並處理gps數據，訂閱topic（/vins_estimator/odometry）在vio_callback回調函數中接收並處理vio的定位數據。
並且生成了三個發布器，用於將結果展示在rviz上。
GlobalOptimization類
所有的融合算法都是在GlobalOptimization類中，對應的文件為globalOpt.h和globalOpt.cpp，並且ceres優化器的相關定義在Factors.h中。
GlobalOptimization類中的函數和變量如下
class GlobalOptimization { public: GlobalOptimization(); ~GlobalOptimization(); void inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy); void inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ); void getGlobalOdom(Eigen::Vector3d &odomP, Eigen::Quaterniond &odomQ); nav_msgs::Path global_path; private: void GPS2XYZ(double latitude, double longitude, double altitude, double* xyz); void optimize(); void updateGlobalPath(); // format t, tx,ty,tz,qw,qx,qy,qz
map<double, vector<double>> localPoseMap; map<double, vector<double>> globalPoseMap; map<double, vector<double>> GPSPositionMap; bool initGPS; bool newGPS; GeographicLib::LocalCartesian geoConverter; std::mutex mPoseMap; Eigen::Matrix4d WGPS_T_WVIO; Eigen::Vector3d lastP; Eigen::Quaterniond lastQ; std::thread threadOpt; };
inputGPS和inputOdom兩個函數將回調函數中的gps和vio數據導入，getGlobalOdom為獲取融合后位姿函數。
GPS2XYZ函數是將GPS的經緯高坐標轉換成當前的坐標系的函數，updateGlobalPath顧名思義更新全局位姿函數。
融合算法的實現主要就是在optimize函數中，接下來進行詳細介紹。
注意其中幾個變量localPoseMap中保存着vio的位姿，GPSPositionMap中保存着gps數據，globalPoseMap中保存着優化后的全局位姿。
融合算法（optimize函數）
void GlobalOptimization::optimize() { while(true) { if(newGPS) { newGPS = false; printf("global optimization\n"); TicToc globalOptimizationTime; ceres::Problem problem; ceres::Solver::Options options; options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; //options.minimizer_progress_to_stdout = true; //options.max_solver_time_in_seconds = SOLVER_TIME * 3;
options.max_num_iterations = 5; ceres::Solver::Summary summary; ceres::LossFunction *loss_function; loss_function = new ceres::HuberLoss(1.0); ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization(); //add param
mPoseMap.lock(); int length = localPoseMap.size(); // w^t_i w^q_i
double t_array[length][3]; double q_array[length][4]; map<double, vector<double>>::iterator iter; iter = globalPoseMap.begin(); for (int i = 0; i < length; i++, iter++) { t_array[i][0] = iter->second[0]; t_array[i][1] = iter->second[1]; t_array[i][2] = iter->second[2]; q_array[i][0] = iter->second[3]; q_array[i][1] = iter->second[4]; q_array[i][2] = iter->second[5]; q_array[i][3] = iter->second[6]; problem.AddParameterBlock(q_array[i], 4, local_parameterization); problem.AddParameterBlock(t_array[i], 3); } map<double, vector<double>>::iterator iterVIO, iterVIONext, iterGPS; int i = 0; for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end(); iterVIO++, i++) { //vio factor
iterVIONext = iterVIO; iterVIONext++; if(iterVIONext != localPoseMap.end()) { Eigen::Matrix4d wTi = Eigen::Matrix4d::Identity(); Eigen::Matrix4d wTj = Eigen::Matrix4d::Identity(); wTi.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIO->second[3], iterVIO->second[4], iterVIO->second[5], iterVIO->second[6]).toRotationMatrix(); wTi.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIO->second[0], iterVIO->second[1], iterVIO->second[2]); wTj.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIONext->second[3], iterVIONext->second[4], iterVIONext->second[5], iterVIONext->second[6]).toRotationMatrix(); wTj.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIONext->second[0], iterVIONext->second[1], iterVIONext->second[2]); Eigen::Matrix4d iTj = wTi.inverse() * wTj; Eigen::Quaterniond iQj; iQj = iTj.block<3, 3>(0, 0); Eigen::Vector3d iPj = iTj.block<3, 1>(0, 3); ceres::CostFunction* vio_function = RelativeRTError::Create(iPj.x(), iPj.y(), iPj.z(), iQj.w(), iQj.x(), iQj.y(), iQj.z(), 0.1, 0.01); problem.AddResidualBlock(vio_function, NULL, q_array[i], t_array[i], q_array[i+1], t_array[i+1]); } //gps factor
double t = iterVIO->first; iterGPS = GPSPositionMap.find(t); if (iterGPS != GPSPositionMap.end()) { ceres::CostFunction* gps_function = TError::Create(iterGPS->second[0], iterGPS->second[1], iterGPS->second[2], iterGPS->second[3]); //printf("inverse weight %f \n", iterGPS->second[3]);
problem.AddResidualBlock(gps_function, loss_function, t_array[i]); } } //mPoseMap.unlock();
ceres::Solve(options, &problem, &summary); //std::cout << summary.BriefReport() << "\n"; // update global pose //mPoseMap.lock();
iter = globalPoseMap.begin(); for (int i = 0; i < length; i++, iter++) { vector<double> globalPose{t_array[i][0], t_array[i][1], t_array[i][2], q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]}; iter->second = globalPose; if(i == length - 1) { Eigen::Matrix4d WVIO_T_body = Eigen::Matrix4d::Identity(); Eigen::Matrix4d WGPS_T_body = Eigen::Matrix4d::Identity(); double t = iter->first; WVIO_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(localPoseMap[t][3], localPoseMap[t][4], localPoseMap[t][5], localPoseMap[t][6]).toRotationMatrix(); WVIO_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(localPoseMap[t][0], localPoseMap[t][1], localPoseMap[t][2]); WGPS_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(globalPose[3], globalPose[4], globalPose[5], globalPose[6]).toRotationMatrix(); WGPS_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(globalPose[0], globalPose[1], globalPose[2]); WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse(); } } updateGlobalPath(); //printf("global time %f \n", globalOptimizationTime.toc());
mPoseMap.unlock(); } std::chrono::milliseconds dura(2000); std::this_thread::sleep_for(dura); } return; }
首先呢，判斷是否有gps數據，整體的算法就是在ceres架構下的優化算法。
所以總體的步驟就是ceres優化的步驟，首先添加優化參數塊（AddParameterBlock函數），參數為globalPoseMap中的6維位姿（代碼中旋轉用四元數表示，所以共7維）。
之后添加CostFunction，通過構建factor重載operator方法實現（具體實現需要學習ceres庫）。該部分有兩個factor，一個是位姿圖優化，另一個則是利用gps進行位置約束。
將factor添加后，進行ceres求解，更新此時gps和vio間的坐標系轉換參數，之后再利用updateGlobalPath函數更新位姿。
總而言之，VF的和GPS的融合也是一個優化框架下的松組合，利用GPS的位置約束，使得位姿圖優化可以不依賴回環，這是一大優勢。
============================================================================================================================================
以下內容摘自：https://blog.csdn.net/huanghaihui_123/article/details/87183055
慣性視覺里程結果與GPS松耦合：
rosrun global_fusion global_fusion_node
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節點。
程序入口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。殘差的得到就根據實際值與觀測值之間的偏差直接得出。