https://zhuanlan.zhihu.com/p/75492883

一、简介

源代码：VINS - Fusion

数据集：KITTI 数据

程序入口：globalOptNode.cpp

二、程序解读

2.1 主函数

int main(int argc, char **argv)
{//初始化ros::init(argc, argv, "globalEstimator");ros::NodeHandle n("~");//全局优化global_path = &globalEstimator.global_path;//订阅GPS信息ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback);//订阅VIO信息ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);//发布信息， //这些发布的数据，rviz 订阅然后显示，看一下 rviz的配置文件就知道了！//配置文件在config里面的 vins_rviz_config.rvizpub_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上。

2.2 GPS 消息订阅

ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback)

void GPS_callback(const sensor_msgs::NavSatFixConstPtr &GPS_msg)
{m_buf.lock();gpsQueue.push(GPS_msg);m_buf.unlock();
}

理解：GPS回调函数，简单，只是把GPS消息存储到了全局变量 gpsQueue 队列里面

2.3 订阅VIO信息

ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);

(1) 获取 VIO位姿 localPoseMap[t] 和世界坐标系下的globalPoseMap[t]

void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{double t = pose_msg->header.stamp.toSec();last_vio_t = t;// 获取VIO输出的位置(三维向量)，姿态(四元数)Eigen::Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z);Eigen::Quaterniond vio_q;vio_q.w() = pose_msg->pose.pose.orientation.w;vio_q.x() = pose_msg->pose.pose.orientation.x;vio_q.y() = pose_msg->pose.pose.orientation.y;vio_q.z() = pose_msg->pose.pose.orientation.z;//位姿传入global Estimator中globalEstimator.inputOdom(t, vio_t, vio_q);m_buf.lock();

globalEstimator.inputOdom(t, vio_t, vio_q) 程序如下：

void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ)
{mPoseMap.lock();// 把vio直接输出的位姿存入 localPoseMap 中vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(), OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};localPoseMap[t] = localPose;Eigen::Quaterniond globalQ;// 把VIO转换到GPS坐标系下，准确的说是转换到以第一帧GPS为原点的坐标系下// 转换之后的位姿插入到globalPoseMap 中globalQ = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomQ;//全局姿态Eigen::Vector3d globalP = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomP + WGPS_T_WVIO.block<3, 1>(0, 3);//优化后的位姿vector<double> globalPose{globalP.x(), globalP.y(), globalP.z(),globalQ.w(), globalQ.x(), globalQ.y(), globalQ.z()};//优化后的位姿赋值给全局变量 globalPoseMap                          globalPoseMap[t] = globalPose;lastP = globalP;//更新lastQ = globalQ;//把最新的全局姿态插入轨迹当中(第一个VIO数据当作第一个全局位姿)geometry_msgs::PoseStamped pose_stamped;pose_stamped.header.stamp = ros::Time(t);pose_stamped.header.frame_id = "world";pose_stamped.pose.position.x = lastP.x();pose_stamped.pose.position.y = lastP.y();pose_stamped.pose.position.z = lastP.z();pose_stamped.pose.orientation.x = lastQ.x();pose_stamped.pose.orientation.y = lastQ.y();pose_stamped.pose.orientation.z = lastQ.z();pose_stamped.pose.orientation.w = lastQ.w();global_path.header = pose_stamped.header;global_path.poses.push_back(pose_stamped);mPoseMap.unlock();
}

理解：

localPoseMap[t] ： VIO 的位姿

globalPoseMap[t] ：VIO 的位姿经过 坐标变换 转到世界坐标系下的位姿，也是GPS和VIO融合后的位姿！

这个函数把vio的结果存储在 localPoseMap 中，然后使用外参 WGPS_T_WVIO 把VIO的结果转换到GPS坐标系下存储在 globalPoseMap 中。

注意，此时我们考虑刚开始gps没对齐时，此时外参 WGPS_T_WVIO 是单位矩阵，所以globalPoseMap里的位姿和VIO的结果一样。

globalPoseMap 是存储融合优化后的位姿的，这也符合逻辑：在没有gps数据时，融合结果完全和VIO一样，而且频率也比GPS位姿更新频率高！

而外参 WGPS_T_WVIO 也是我们需要优化的参数！

(2) 寻找与VIO时间戳相对应的GPS消息

      while(!gpsQueue.empty())//有GPS数据时执行{//获取最早的GPS数据和其时间sensor_msgs::NavSatFixConstPtr GPS_msg = gpsQueue.front();double gps_t = GPS_msg->header.stamp.toSec();printf("vio t: %f, gps t: %f \n", t, gps_t);// +- 10ms的时间偏差if(gps_t >= t - 0.01 && gps_t <= t + 0.01){//gps的经纬度，海拔高度double latitude  = GPS_msg->latitude;double longitude = GPS_msg->longitude;double altitude  = GPS_msg->altitude;//gps 数据的方差double pos_accuracy = GPS_msg->position_covariance[0];if(pos_accuracy <= 0)pos_accuracy = 1;globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);gpsQueue.pop();//注意这里 break;//此处break,意味只存储了一个GPS数据后就break了}else if(gps_t < t - 0.01)gpsQueue.pop();else if(gps_t > t + 0.01)break;}m_buf.unlock();

子程序 globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);

void GlobalOptimization::inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy)
{double xyz[3];GPS2XYZ(latitude, longitude, altitude, xyz);// 存入经纬度计算出的平面坐标，存入GPSPositionMap中vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy};GPSPositionMap[t] = tmp; newGPS = true;
}

目的：GPS的经纬高转换成 XYZ，并放入全局变量 GPSPositionMap[t] 里面！

注意：经纬度表示的是地球上的坐标，而地球是一个球形，需要首先把经纬度转化到平面坐标系上，值得一提的是，

GPS2XYZ() 并非把经纬度转化到世界坐标系下(以0经度，0纬度为原点)，而是以第一帧GPS数据为坐标原点！

输入VIO数据的 10ms内的GPS数据进行坐标系转换,GPS2XYZ函数将GPS的经纬度坐标转换为ENU坐标,并且第一帧

的GPS数据作为原点[0,0,0]，转换之后的gps数据和协方差一起存入到GPSPositionMap中；这时的 GPSPositionMap 便是观测值！

且此时，newGPS = true，意味满足优化的条件！后面我们会感受到程序设计的巧妙！

(3) 发布和保存 GPS和VIO融合后的位姿globalPoseMap[t]

Eigen::Vector3d global_t;Eigen:: Quaterniond global_q;globalEstimator.getGlobalOdom(global_t, global_q);nav_msgs::Odometry odometry;odometry.header = pose_msg->header;odometry.header.frame_id = "world";odometry.child_frame_id = "world";odometry.pose.pose.position.x = global_t.x();odometry.pose.pose.position.y = global_t.y();odometry.pose.pose.position.z = global_t.z();odometry.pose.pose.orientation.x = global_q.x();odometry.pose.pose.orientation.y = global_q.y();odometry.pose.pose.orientation.z = global_q.z();odometry.pose.pose.orientation.w = global_q.w();// 广播轨迹（略）......pub_global_odometry.publish(odometry);pub_global_path.publish(*global_path);publish_car_model(t, global_t, global_q);// 位姿写入文本文件（略）......std::ofstream foutC("/home/vins_fusion/output/vio_global.csv", ios::app);foutC.setf(ios::fixed, ios::floatfield);foutC.precision(0);foutC << pose_msg->header.stamp.toSec() * 1e9 << ",";foutC.precision(5);foutC << global_t.x() << ","<< global_t.y() << ","<< global_t.z() << ","<< global_q.w() << ","<< global_q.x() << ","<< global_q.y() << ","<< global_q.z() << endl;foutC.close();
}

子程序 globalEstimator.getGlobalOdom(global_t, global_q);

void GlobalOptimization::getGlobalOdom(Eigen::Vector3d &odomP, Eigen::Quaterniond &odomQ)
{odomP = lastP;odomQ = lastQ;
}

目的：实现全局位姿 globalPoseMap[t] 的更新

到此为止，订阅VIO的程序结束，而下面就是GPS/VIO融合后需要发布的数据，如：

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

疑问：GPS和VIO融合的程序在哪？

答：在main 函数里面的 global_path = &globalEstimator.global_path;

GlobalOptimization::GlobalOptimization()
{initGPS = false;newGPS = false;WGPS_T_WVIO = Eigen::Matrix4d::Identity();threadOpt = std::thread(&GlobalOptimization::optimize, this);
}

这一部分其实在 main() 最前面的，只是被我放进了最后面，即使放在了最前面融合程序也不会执行，

因为 newGPS = fause, 进不去 GlobalOptimization::optimize() 里面！

2.4 Ceres 优化 VIO 和 GPS数据

1、理论

这一块在进行程序讲解之前先进行理论的梳理！如下图：

要估计出 x0、x1、x2 . . . xn 这些时刻的位姿，我有的是两个方面的观测值，一方面是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时刻的位姿变化量对最终结果的影响。

建议：仔细理解上面一段话的意思，后面看程序会很清晰！还有就是把Ceres 这个工具搞明白！

2、程序

（1）ceres构建最小二乘问题，参考

void GlobalOptimization::optimize()
{while(true){if(newGPS)//得到GPS信号，并且与vio结果时间同步后{newGPS = false;printf("global optimization\n");TicToc globalOptimizationTime;//定义损失函数，首先使用ceres构建最小二乘问题，这个没啥可说的状态量赋初值，添加参数块。ceres::Problem problem;ceres::Solver::Options options;options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;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，迭代的初始值是globalPoseMap中的值，也就是VIO转换到GPS坐标系下的值。mPoseMap.lock();int length = localPoseMap.size(); //VIO位姿的个数// w^t_i   w^q_idouble 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);}

理解：

ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();

这段代码的目的是为了解决姿态有四个变量，但是只有三个自由度问题，姿态用四元数表示的！

int length = localPoseMap.size();

订阅VIO信息时，VIO位姿的个数，也是后面我们需要优化参数块的个数！

优化的变量是 旋转q_array[length][4] 和 平移t_array[length][3]; 他们的初值是VIO的第一帧数据，

也等价于第一个全局位姿 globalPoseMap.begin()； 然后把他们加入优化参数块即可！

（2）添加 VIO两帧之间的残差项

            map<double, vector<double>>::iterator iterVIO, iterVIONext, iterGPS;int i = 0;//然后添加残差：for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end(); iterVIO++, i++){//vio factor，添加VIO残差，观测量是两帧VIO数据之差，是相对的。而下面的GPS是绝对的iterVIONext = iterVIO;iterVIONext++;if(iterVIONext != localPoseMap.end()){Eigen::Matrix4d wTi = Eigen::Matrix4d::Identity();Eigen::Matrix4d wTj = Eigen::Matrix4d::Identity();//i，对应的四元素wTi.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIO->second[3], iterVIO->second[4], iterVIO->second[5], iterVIO->second[6]).toRotationMatrix();//i，对应的位置wTi.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIO->second[0], iterVIO->second[1], iterVIO->second[2]);//j，对应的四元素wTj.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIONext->second[3], iterVIONext->second[4], iterVIONext->second[5], iterVIONext->second[6]).toRotationMatrix();//j，对应的位置                                           wTj.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIONext->second[0], iterVIONext->second[1], iterVIONext->second[2]);//i 到 j 转换的四元素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);//计算两帧VIO之间的相对差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]);}

理解：

wTi：i 时刻VIO结果的旋转R和平移t, 构成 Ti

wTj：i 下一时刻VIO结果的旋转R和平移t, 构成 Tj

iTj: j 时刻转换到 i时刻的 4*4 矩阵，包含旋转iQj 和 平移iPj

然后根据上面的信息，构造相邻两帧 VIO数据之间的损失函数，如下程序：

ceres::CostFunction* vio_function =

RelativeRTError::Create( iPj.x(), iPj.y(), iPj.z(), iQj.w(), iQj.x(), iQj.y(), iQj.z(), 0.1, 0.01);

具体的程序在 Factors.h 里面的结构体，如下：

struct RelativeRTError
{//结构体初始化RelativeRTError(double t_x, double t_y, double t_z, double q_w, double q_x, double q_y, double q_z,double t_var, double q_var):t_x(t_x), t_y(t_y), t_z(t_z), q_w(q_w), q_x(q_x), q_y(q_y), q_z(q_z),t_var(t_var), q_var(q_var){}//构建代价函数template <typename T>bool operator()(const T* const w_q_i, const T* ti, const T* w_q_j, const T* tj, T* residuals) const{T t_w_ij[3]; //VIO和GPS优化后，世界坐标系 i、j帧的位置增量t_w_ij[0] = tj[0] - ti[0];t_w_ij[1] = tj[1] - ti[1];t_w_ij[2] = tj[2] - ti[2];T i_q_w[4]; //i帧的四元数逆QuaternionInverse(w_q_i, i_q_w);//世界坐标系下，i、j帧的位置增量t_w_ij，经i_q_w 转换到i帧的坐标系，与 t_x 求差！T t_i_ij[3];ceres::QuaternionRotatePoint(i_q_w, t_w_ij, t_i_ij);//四元素旋转，得到姿态//残差定义为增量差residuals[0] = (t_i_ij[0] - T(t_x)) / T(t_var);residuals[1] = (t_i_ij[1] - T(t_y)) / T(t_var);residuals[2] = (t_i_ij[2] - T(t_z)) / T(t_var);T relative_q[4]; //传入VIO观测的四元数增量relative_q[0] = T(q_w);relative_q[1] = T(q_x);relative_q[2] = T(q_y);relative_q[3] = T(q_z);T q_i_j[4]; //状态量计算的四元数增量ceres::QuaternionProduct(i_q_w, w_q_j, q_i_j);T relative_q_inv[4];QuaternionInverse(relative_q, relative_q_inv);T error_q[4]; //状态量计算的增量乘上测量量的逆，定义了残差ceres::QuaternionProduct(relative_q_inv, q_i_j, error_q); residuals[3] = T(2) * error_q[1] / T(q_var);residuals[4] = T(2) * error_q[2] / T(q_var);residuals[5] = T(2) * error_q[3] / T(q_var);return true;}static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,const double q_w, const double q_x, const double q_y, const double q_z,const double t_var, const double q_var) {return (new ceres::AutoDiffCostFunction<RelativeRTError, 6, 4, 3, 4, 3>(new RelativeRTError(t_x, t_y, t_z, q_w, q_x, q_y, q_z, t_var, q_var)));}double t_x, t_y, t_z, t_norm;double q_w, q_x, q_y, q_z;double t_var, q_var;};

注意：哪些是VIO的两帧间的变化，哪些是优化后 globalPoseMap 的位姿态！

思路：VIO和GPS优化后的POS增量投影到 VIO下，然后和VIO下的POS 增量作差，求残差！

我以前看这些代码很迷糊，但是你把Ceres熟悉之后，在明白VIO和GPS的优化原理，就很简单，下图是自己画的示意图：

VIO的数据和状态量的残差定义为：（j时刻的状态量 - i时刻的状态量）得到的增量-vio增量; 其意味着融合后的位置必须

和GPS位置尽可能重合，而两帧间的增量要和VIO尽可能相等。这对理解坐标转换至关重要，这样的处理意味着vio的数据

并不对融合后的绝对位置做约束，只要求融合后的位置增量和vio的增量误差尽可能小, 所以融合后的位置会在GPS坐标系下。

（3）GPS残差项

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]);problem.AddResidualBlock(gps_function, loss_function, t_array[i]);}}
//优化完成后，再根据优化结果更新姿态就ok啦。为了防止VIO漂移过大，每次优化完成还需要计算一下VIO到GPS坐标系的变换。ceres::Solve(options, &problem, &summary);

再来看一下 struct TError 结构体

struct TError
{//定义数据的 t_x 、t_y 、 t_z 和 var 的析构函数TError(double t_x, double t_y, double t_z, double var):t_x(t_x), t_y(t_y), t_z(t_z), var(var){}template <typename T>bool operator()(const T* tj, T* residuals) const{residuals[0] = (tj[0] - T(t_x)) / T(var);residuals[1] = (tj[1] - T(t_y)) / T(var);residuals[2] = (tj[2] - T(t_z)) / T(var);return true;}//损失函数static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z, const double var) {return (new ceres::AutoDiffCostFunction<TError, 3, 3>(new TError(t_x, t_y, t_z, var)));}double t_x, t_y, t_z, var;};

理解：根据程序可知，残差是：VIO/GPS融合后的位置（优化变量） - gps观测值，到此，优化部分结束！

思考：自己想一下，优化结束了干啥？当然是数据的更新了，这个很简单吧，不然你优化的目的是干嘛的呢！

那我们需要更新哪些数据呢？这个其实也不难，我们先看程序部分

（4）GPS残差项

            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(); //T R tEigen::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]);//更新世界坐标系与VIO间的外参，不再是单位矩阵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;
}

理解：

经过VIO和GPS优化后，我们可以得到一个更加准确的全局位姿，globalPose ！因为全局位姿准确了，所以我们可以

用来更新VIO到GPS的转换矩阵WGPS_T_WVIO，这是很容易理解的事情！因为开始我们的转换矩阵WGPS_T_WVIO

设置的是单位矩阵，有人就问，为啥是单位矩阵，这不就意味这开始VIO和GPS是重和的嘛，上面其实已经解释了，第一：没有GPS数据

情况下，VIO结果就是全局的位姿！第二：不是单位矩阵，那你说是什么矩阵？你说呀！你又说不出来更好的！第三：我们后面不是优化了嘛，

开始的单位矩阵仅仅是一个初始的值，设置合理即可！

注意：

WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse()

这导致了WGPS_T_WVIO这个外参包括了VIO计算的误差。什么意思呢，本来当vio足够精准时，

这样算出的WGPS_T_WVIO肯定是一个固定值。实际把WGPS_T_WVIO输出看，会随着时间一直变化。

变化原因是因为vio的结果和融合后的结果均带误差，WGPS_T_body * WVIO_T_body.inverse() 使得误差包含在里面了。

既然，转换矩阵 WGPS_T_WVIO 更新结束了，下面就需要更新全局位姿了！

void GlobalOptimization::updateGlobalPath()
{global_path.poses.clear();map<double, vector<double>>::iterator iter;for (iter = globalPoseMap.begin(); iter != globalPoseMap.end(); iter++){geometry_msgs::PoseStamped pose_stamped;pose_stamped.header.stamp = ros::Time(iter->first);pose_stamped.header.frame_id = "world";pose_stamped.pose.position.x = iter->second[0];pose_stamped.pose.position.y = iter->second[1];pose_stamped.pose.position.z = iter->second[2];pose_stamped.pose.orientation.w = iter->second[3];pose_stamped.pose.orientation.x = iter->second[4];pose_stamped.pose.orientation.y = iter->second[5];pose_stamped.pose.orientation.z = iter->second[6];global_path.poses.push_back(pose_stamped);}
}

到此，VIO和GPS的优化结束！其实是很容易的一件事情，也可能是我看程序亭容易的，但是自己写的话也会有很多的问题！

还要加强程序编写能力！

VINS - Fusion GPS/VIO 融合二、数据融合相关推荐

VINS Fusion GPS融合部分
概述 VINS Fusion在VINS Mono的基础上,添加了GPS等可以获取全局观测信息的传感器,使得VINS可以利用全局信息消除累计误差,进而减小闭环依赖. 局部传感器(如相机,IMU,激光雷达 ...
多传感器信息融合(标定, 数据融合, 任务融合)
文章目录 1. 引言 2. 多传感器标定 2.1 标定场地 2.2 相机到相机 2.2 相机到多线激光雷达标定 2.3 相机到毫米波雷达标定 2.4 相机到IMU标定 2.5 论文总结 3. 数据层融 ...
自动驾驶算法学习：多传感器信息融合(标定, 数据融合, 任务融合)
版权声明:本文为博主转载文章,遵循 CC 4.0 by-sa 版权协议,转载请附上原文出处链接和本声明. 原文链接:https://blog.csdn.net/orange_littlegirl/ar ...
python卡尔曼滤波融合_数据融合之卡尔曼滤波示例
哎,难受,知乎把我的完美格式打乱成这样子. 卡尔曼几乎是做数据融合都知道的一个算法了把.卡尔曼滤波能够实现从有噪声的传感器数据中获取我们的较为准确的信息.而且通过卡尔曼,我们甚至能够去推算一些不可直接 ...
多传感器数据融合二—— 数据关联及数据准备
2.1 多传感器数据关联时的数据准备 2.1.1.预处理一. 点迹过滤:基本依据是运动目标和固定目标跨周期的相关特性不同.** (1)保留雷达扫描5周期的信息,包括各个障碍物的坐标信息: (2)新点 ...
动手学无人驾驶（6）：基于IMU和GPS数据融合的自车定位
在上一篇博文<动手学无人驾驶(5):多传感器数据融合>介绍了如何使用Radar和LiDAR数据对自行车进行追踪,这是对汽车外界运动物体进行定位. 对于自动驾驶的汽车来说,有时也需要对自身进 ...
数据融合(data fusion)原理与方法
数据融合(data fusion)原理与方法数据融合(data fusion)最早被应用于军事领域. 现在数据融合的主要应用领域有:多源影像复合.机器人和智能仪器系统.战场和无人驾驶飞机. ...
417关于ads-b与雷达数据融合
需进行调研和论证内容(标黄): 1.ADS-B与雷达数据融合处理(优先级高):需结合地面一次雷达与ADS-B融合应用模式,对照要求,开展星上数据融合处理方案.算法: 2.数据库部分(优先级低):ADS ...
毕业设计-基于深度学习的数据融合方法研究
目录前言课题背景和意义实现技术思路一.深度学习概述二.基于深度学习的数据融合方法分类 1.基于深度学习特征提取的数据融合方法 2.基于深度学习融合的数据融合方法 3.基于深度学习全过程的数据 ...

VINS - Fusion GPS/VIO 融合二、数据融合

https://zhuanlan.zhihu.com/p/75492883

一、简介

二、程序解读

2.1 主函数

理解：

2.2 GPS 消息订阅

2.3 订阅VIO信息

(1) 获取 VIO位姿 localPoseMap[t] 和世界坐标系下的globalPoseMap[t]

(2) 寻找与VIO时间戳相对应的GPS消息

(3) 发布和保存 GPS和VIO融合后的位姿globalPoseMap[t]

2.4 Ceres 优化 VIO 和 GPS数据

1、理论

2、程序

VINS - Fusion GPS/VIO 融合二、数据融合相关推荐

最新文章

热门文章

VINS - Fusion GPS/VIO 融合 二、数据融合

https://zhuanlan.zhihu.com/p/75492883

一、简介

二、程序解读

2.1 主函数

理解：

2.2 GPS 消息订阅

2.3 订阅VIO信息

(1) 获取 VIO位姿 localPoseMap[t] 和 世界坐标系下的globalPoseMap[t]

(2) 寻找与VIO时间戳相对应的GPS消息

(3) 发布和保存 GPS和VIO融合后的位姿globalPoseMap[t]

2.4 Ceres 优化 VIO 和 GPS数据

1、理论

2、程序

VINS - Fusion GPS/VIO 融合 二、数据融合相关推荐

最新文章

热门文章

VINS - Fusion GPS/VIO 融合二、数据融合

(1) 获取 VIO位姿 localPoseMap[t] 和世界坐标系下的globalPoseMap[t]

VINS - Fusion GPS/VIO 融合二、数据融合相关推荐