Vins-Fusion源碼：https://github.com/HKUST-Aerial-Robotics/VINS-Fusion

摘要

應(yīng)項(xiàng)目需要，側(cè)重學(xué)習(xí)stereo+gps融合

轉(zhuǎn)載幾篇寫的比較好的博客

1.萌新學(xué)VINS-Fusion（三）------雙目和GPS融合

主函數(shù)文件
與GPS融合的程序入口在KITTIGPSTest.cpp文件中，數(shù)據(jù)為KITTI數(shù)據(jù)集
數(shù)據(jù)集為 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函數(shù)與之前的main函數(shù)相似，都要進(jìn)行ros節(jié)點(diǎn)初始化，然后讀取參數(shù)，區(qū)別在于該數(shù)據(jù)集的圖像和gps數(shù)據(jù)均為文件讀取方式，將gps數(shù)據(jù)封裝進(jìn)ros 的sensor_msgs::NavSatFix 數(shù)據(jù)類型里，以gps為topic播發(fā)出去，而雙目圖像則直接放到estimator進(jìn)行vio，將vio得到的結(jié)果再播發(fā)出去，方便后面的訂閱和與GPS的融合。

global_fusion
所有的與gps的融合在global_fusion文件夾中，該部分的文件入口是一個(gè)rosnode文件globalOptNode.cpp，主函數(shù)很短，如下

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;
}

代碼很簡(jiǎn)單，訂閱topic（/gps）在GPS_callback回調(diào)函數(shù)中接收并處理gps數(shù)據(jù)，訂閱topic（/vins_estimator/odometry）在vio_callback回調(diào)函數(shù)中接收并處理vio的定位數(shù)據(jù)。
并且生成了三個(gè)發(fā)布器，用于將結(jié)果展示在rviz上。

GlobalOptimization類
所有的融合算法都是在GlobalOptimization類中，對(duì)應(yīng)的文件為globalOpt.h和globalOpt.cpp，并且ceres優(yōu)化器的相關(guān)定義在Factors.h中。
GlobalOptimization類中的函數(shù)和變量如下

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兩個(gè)函數(shù)將回調(diào)函數(shù)中的gps和vio數(shù)據(jù)導(dǎo)入，getGlobalOdom為獲取融合后位姿函數(shù)。
GPS2XYZ函數(shù)是將GPS的經(jīng)緯高坐標(biāo)轉(zhuǎn)換成當(dāng)前的坐標(biāo)系的函數(shù)，updateGlobalPath顧名思義更新全局位姿函數(shù)。
融合算法的實(shí)現(xiàn)主要就是在optimize函數(shù)中，接下來(lái)進(jìn)行詳細(xì)介紹。

注意其中幾個(gè)變量localPoseMap中保存著vio的位姿，GPSPositionMap中保存著gps數(shù)據(jù)，globalPoseMap中保存著優(yōu)化后的全局位姿。

融合算法（optimize函數(shù)）

void GlobalOptimization::optimize()
{
while(true)
{
if(newGPS)
{
newGPS = false;
printf("global optimization
");
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 
", iterGPS->second[3]);
problem.AddResidualBlock(gps_function, loss_function, t_array[i]);

}

}
//mPoseMap.unlock();
ceres::Solve(options, &problem, &summary);
//std::cout << summary.BriefReport() << "
";

// 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 
", globalOptimizationTime.toc());
mPoseMap.unlock();
}
std::chrono::milliseconds dura(2000);
std::this_thread::sleep_for(dura);
}
return;
}

首先呢，判斷是否有g(shù)ps數(shù)據(jù)，整體的算法就是在ceres架構(gòu)下的優(yōu)化算法。
所以總體的步驟就是ceres優(yōu)化的步驟，首先添加優(yōu)化參數(shù)塊（AddParameterBlock函數(shù)），參數(shù)為globalPoseMap中的6維位姿（代碼中旋轉(zhuǎn)用四元數(shù)表示，所以共7維）。
之后添加CostFunction，通過(guò)構(gòu)建factor重載operator方法實(shí)現(xiàn)（具體實(shí)現(xiàn)需要學(xué)習(xí)ceres庫(kù)）。該部分有兩個(gè)factor，一個(gè)是位姿圖優(yōu)化，另一個(gè)則是利用gps進(jìn)行位置約束。
將factor添加后，進(jìn)行ceres求解，更新此時(shí)gps和vio間的坐標(biāo)系轉(zhuǎn)換參數(shù)，之后再利用updateGlobalPath函數(shù)更新位姿。

總而言之，VF的和GPS的融合也是一個(gè)優(yōu)化框架下的松組合，利用GPS的位置約束，使得位姿圖優(yōu)化可以不依賴回環(huán)，這是一大優(yōu)勢(shì)。

============================================================================================================================================

以下內(nèi)容摘自：https://blog.csdn.net/huanghaihui_123/article/details/87183055

慣性視覺(jué)里程結(jié)果與GPS松耦合：

rosrun global_fusion global_fusion_node

github開(kāi)源的是KITTI的數(shù)據(jù)集的測(cè)試代碼。跟著程序走一遍。
KITTIGPSTest.cpp

主程序入口:
vins_estimator包下面的KITTIGPSTest.cpp,主要作用：
（1）開(kāi)啟estimator類，進(jìn)行vio里程估計(jì)
（2）從文件中讀取視頻圖片列表，讀入estimator類中
（3）從文件中讀取GPS數(shù)據(jù)列表，直接通過(guò)ROS發(fā)布出去

具體的，從文件中獲取圖像和GPS數(shù)據(jù)當(dāng)前的時(shí)間戳信息。以第一幀圖像和第一個(gè)GPS時(shí)間早的作為基準(zhǔn)時(shí)間，之后，左右雙目的圖像通過(guò)estimator.inputImage（）讀入里程計(jì)中，里程計(jì)Estimator類內(nèi)部會(huì)定時(shí)發(fā)送VIO計(jì)算的結(jié)果。同時(shí)main函數(shù)中會(huì)在圖片讀入里程計(jì)的時(shí)刻發(fā)布同一時(shí)刻的GPS信息。（每一幀圖片都有對(duì)應(yīng)一條GPS信息，時(shí)間戳設(shè)置為一致的）

GPS融合主要發(fā)生在global_fusion包中。
global_fusion

程序同步開(kāi)啟了global_fusion節(jié)點(diǎn)。

程序入口globalOptNode.cpp
程序定義了一個(gè)GlobalOptimization類globalEstimator來(lái)進(jìn)行融合處理。
程序有三個(gè)回調(diào)函數(shù)：
（1）publish_car_model():根據(jù)最終vio與GPS融合的定位結(jié)果，發(fā)布在特定位置的一個(gè)炫酷的小車模型。
（2）GPS_callback()：通過(guò)globalEstimator.inputGPS（），放入全局融合器中。
（3）vio_callback()：通過(guò)globalEstimator.inputOdom（），放入全局融合器中。并且從全局融合器globalEstimator中取出最終位姿融合的結(jié)果，調(diào)用publish_car_model()發(fā)布出來(lái)。

最重要類的核心為 GlobalOptimization類 和類內(nèi)的optimize（）函數(shù)。
GlobalOptimization類

類成員：
(1)map類型
localPoseMap中保存著vio的位姿
GPSPositionMap中保存著gps數(shù)據(jù)
globalPoseMap中保存著優(yōu)化后的全局位姿

以上類成員中map的格式:

map<double,vector<double>> =<t,vector<x,y,z,qw,qx,qy,qz>>

(2)bool類型
initGPS:第一個(gè)GPS信號(hào)觸發(fā)
newGPS:有新的GPS信號(hào)后觸發(fā)

(3)GeographicLib::LocalCartesian 類型
geoconverter GPS經(jīng)緯度信號(hào)轉(zhuǎn)化為X,Y,Z用到的第三方庫(kù)
當(dāng)該類已進(jìn)行初始化，就同步開(kāi)啟了新線程optimize（），對(duì)兩個(gè)結(jié)果不斷進(jìn)行優(yōu)化。
optimize（）

（1）當(dāng)有新的GPS信號(hào)到來(lái)時(shí)候，進(jìn)行GPS與視覺(jué)慣性的融合
（2）建立ceres的problem

lossfunction 設(shè)置為Huberloss
addParameterBlock添加優(yōu)化的變量 ,優(yōu)化的變量是q_array以及t_array。即globalPoseMap保存下來(lái)的每幀圖像的位姿信息。其中參數(shù)變量的多少是由localPoseMap來(lái)決定的。即VIO有多少個(gè)數(shù)據(jù)，全局也就有多少個(gè)。迭代器指向的first為時(shí)間，second為7變量的位姿。其中在添加q_array由于維度只有三維，因此增加了local_parameterization來(lái)進(jìn)行約束。
接著開(kāi)始添加殘差項(xiàng)，總共有兩個(gè)殘差項(xiàng)分別是：vio factor以及gps factor
– vio factor：殘差項(xiàng)的costfunction創(chuàng)建由 relativeRTError來(lái)提供。觀測(cè)值由vio的結(jié)果提供。此時(shí)計(jì)算的是以i時(shí)刻作為參考，從i到j(luò)這兩個(gè)時(shí)刻的位移值以及四元數(shù)的旋轉(zhuǎn)值作為觀測(cè)值傳遞進(jìn)入代價(jià)函數(shù)中。 此時(shí)iPj代表了i到j(luò)的位移，iQj代表了i到j(luò)的四元數(shù)變換。添加殘差項(xiàng)的時(shí)候，需要添加當(dāng)前i時(shí)刻的位姿以及j時(shí)刻的位姿。即用觀測(cè)值來(lái)估計(jì)i時(shí)刻的位姿以及j時(shí)刻的位姿。
– gps factor：殘差項(xiàng)的costfunction創(chuàng)建由 TError來(lái)提供。觀測(cè)值由Gps數(shù)據(jù)的結(jié)果提供。添加殘差項(xiàng)的時(shí)候，只需要添加當(dāng)前i時(shí)刻的位姿。
求解非線性優(yōu)化方程
求解出來(lái)后，把t_array和q_array（即兩個(gè)優(yōu)化的變量）賦值給globalposeMap。并且根據(jù)最新解算出來(lái)的結(jié)果（即i=length-1時(shí)刻最新的結(jié)果），跟新GPS到vio這兩個(gè)獨(dú)立體系之間坐標(biāo)轉(zhuǎn)換關(guān)系。

TError及RelativeRTError

直觀上理解：
{0, 1，2，3，4, 5，6…}
要估計(jì)出這些時(shí)刻，每個(gè)時(shí)刻的位姿。
我有的是兩個(gè)方面的觀測(cè)值，一方面是GPS得到的每個(gè)時(shí)刻的位置（x,y,z）（并且GPS信號(hào)可以提供在該時(shí)刻得到這個(gè)位置的精度posAccuracy），沒(méi)有累計(jì)誤差，精度較低。另一方面是VIO得到的每個(gè)時(shí)刻的位置（x,y,z）以及對(duì)應(yīng)的姿態(tài)四元數(shù)（w,x,y,z）,存在累計(jì)誤差，短時(shí)間內(nèi)精度較高。為了得到更好的一個(gè)融合結(jié)果，因此我們采用這個(gè)策略：觀測(cè)值中，初始位置由GPS提供，并且vio觀測(cè)值信任的是i到j(luò)時(shí)刻的位移以及姿態(tài)變化量。 并不信任vio得到的一個(gè)絕對(duì)位移量以及絕對(duì)的旋轉(zhuǎn)姿態(tài)量。只信任短期的i到j(luò)的變化量，用這個(gè)變化量作為整個(gè)代價(jià)函數(shù)的觀測(cè)值，進(jìn)行求解。
因此兩個(gè)殘差項(xiàng)TError及RelativeRTError分別對(duì)應(yīng)的就是GPS位置信號(hào)以及vio短時(shí)間內(nèi)估計(jì)的i到j(luò)時(shí)刻的位姿變化量對(duì)最終結(jié)果的影響。迭代求解的過(guò)程中均采用了AutoDiffCostFunction自動(dòng)求解微分來(lái)進(jìn)行迭代。
（1）TError
TError（x,y,z,accuracy），最后一項(xiàng)是定位精度，可以由GPS系統(tǒng)提供。殘差除了直接觀測(cè)值與真值相減以外，還除了這個(gè)accuracy作為分母。意味著精度越高，accuracy越小，對(duì)結(jié)果的影響就越大。
（2）RelativeRTError
RelativeRTError（dx,dy,dz,dqw,dqx,dqy,dqz，t_var,q_var），最后兩項(xiàng)是位移以及旋轉(zhuǎn)之間的權(quán)重分配比例，并且為了使得與TError對(duì)應(yīng)。在程序中，應(yīng)該是根據(jù)經(jīng)驗(yàn)把最后兩項(xiàng)設(shè)置成一個(gè)常值，分別對(duì)應(yīng)0.1以及0.01。殘差的得到就根據(jù)實(shí)際值與觀測(cè)值之間的偏差直接得出。

總結(jié)

以上是生活随笔為你收集整理的vins_fusion学习笔记的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。