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.rviz
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上。
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_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);
}
理解:
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 t
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]);
//更新世界坐标系与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的优化结束!其实是很容易的一件事情,也可能是我看程序亭容易的,但是自己写的话也会有很多的问题!
还要加强程序编写能力!