萌新学VINS-Fusion（三）------双目和GPS融合

2023-05-16

转载自：https://blog.csdn.net/weixin_41843971/article/details/86748719

萌新学VINS-Fusion（三）------双目和GPS融合

slam萌新 2019-02-02 13:55:26 6057 收藏 26

文章标签：视觉slam 计算机视觉无人驾驶

版权

虽然要过年了，而且每天还要在家里小店打工，但是网红之路不能停对吧，这篇博客写关于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函数更新位姿。

总而言之，VF的和GPS的融合也是一个优化框架下的松组合，利用GPS的位置约束，使得位姿图优化可以不依赖回环，这是一大优势。

谢谢大家，比心（有理解不到位或错误，欢迎指正，虚心学习）

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)