萌新学VINS-Fusion（三）------双目和GPS融合
转载自：https://blog.csdn.net/weixin_41843971/article/details/86748719
萌新学VINS-Fusion（三）------双目和GPS融合
slam萌新 2019-02-02 13:55:26 6057 收藏 26
版权
虽然要过年了，而且每天还要在家里小店打工，但是网红之路不能停对吧，这篇博客写关于VINS-Fusion和GPS的融合。其实我之前改出过一个加了GPS的VIO，并且也开源了，之前没有找到合适的数据集，自己造的数据，效果一般，近期我准备跑一下看看效果
最近我在之前代码基础上做了修改，还在测试效果，之后可能会开源，希望大家关注、点赞、收藏，素质三连。
我的开源代码地址
https://github.com/grn213331/remove_ros_VINS-position-constraint
Vins-Fusion源码：https://github.com/HKUST-Aerial-Robotics/VINS-Fusion
主函数文件
与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函数更新位姿。