参考的博客:https://blog.csdn.net/subiluo/article/details/105429471
http://www.luyixian.cn/news_show_313718.aspx
state: 状态量,位姿,速度,bias等
Local Factor: 局部观测约束,VIO相对位姿变换, 计算的是相邻两帧之间位姿的残差
1.代码的输入输出
输入输出的定义都在globalOptNode.cpp这个文件里面
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);
vio的输入函数
void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
//printf("vio_callback! \n");
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();
//寻找与VIO时间戳相对应的GPS信息
while(!gpsQueue.empty())
{
//获得最老的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 sync tolerance
if(gps_t >= t - 0.01 && gps_t <= t + 0.01)
{
//printf("receive GPS with timestamp %f\n", GPS_msg->header.stamp.toSec());
//GPS的经纬度,海拔高度
double latitude = GPS_msg->latitude;
double longitude = GPS_msg->longitude;
double altitude = GPS_msg->altitude;
//int numSats = GPS_msg->status.service;
//GPS数据的方差
double pos_accuracy = GPS_msg->position_covariance[0];
if(pos_accuracy <= 0)
pos_accuracy = 1;
//printf("receive covariance %lf \n", pos_accuracy);
//if(GPS_msg->status.status > 8)
//向globalEstimator中输入GPS数据
globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);
gpsQueue.pop();
//此处break,意味只存储了一个GPS数据后就break了。后来想明白了GPS不同于imu,是绝对位