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
");
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;
}
首先呢，判断是否有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。残差的得到就根据实际值与观测值之间的偏差直接得出。