本次工作
我首先参照网络上的文档整理了全部的代码，并对于C++和OpenCV的一些操作也进行了详细的注释，并写了这篇的博客进行全部的讲解，其中1-4章节是前端VIO信息，5章节是后端DBOW词袋回环，6-7章节是GPS与VIO融合，8章节是参考文献。
1. 程序入口rosNodeTest.cpp
1.1 定义内容
运行程序时，首先进入的是主程序vins_estimator/src/estimator/rosNodeTest.cpp
里边主要定义了 估计器、 缓存器 、 获取传感器数据的函数 和 一个主函数
// 获得左目的message
void img0_callback(const sensor_msgs::ImageConstPtr &img_msg)
// 获得右目的message
void img1_callback(const sensor_msgs::ImageConstPtr &img_msg)
// 从msg中获取图片，返回值cv::Mat，输入是当前图像msg的指针
cv::Mat getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg)
// 从两个主题中提取具有相同时间戳的图像
// 并将图像输入到估计器中
void sync_process()
// 输入imu的msg信息，进行解算并把imu数据输入到estimator
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
// 把特征点的点云msg输入到estimator
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
// 是否重启estimator，并重新设置参数
void restart_callback(const std_msgs::BoolConstPtr &restart_msg)
// 是否使用IMU
void imu_switch_callback(const std_msgs::BoolConstPtr &switch_msg)
// 相机的开关
void cam_switch_callback(const std_msgs::BoolConstPtr &switch_msg)
int main(int argc, char **argv)
1.2 程序执行
1.2.1 获取参数并设置参数
具体的方法在函数
主函数中，主要是执行以下各个步骤订阅ROS信息，然后进行处理
string config_file = argv[1];
printf("config_file: %s\n", argv[1]);
//config_file: 4VINS_test/0config/yaml_mynt_s1030/mynt_stereo_imu_config.yaml
readParameters(config_file);// 读取参数
estimator.setParameter();// 设置参数
/*
ros::Subscriber subscribe (const std::string &topic, uint32_t queue_size, void(*fp)(M), const TransportHints &transport_hints=TransportHints())
第一个参数是订阅话题的名称；
第二个参数是订阅队列的长度；（如果收到的消息都没来得及处理，那么新消息入队，旧消息就会出队）；
第三个参数是回调函数的指针，指向回调函数来处理接收到的消息！
第四个参数：似乎与延迟有关系，暂时不关心。（该成员函数有13重载）
*/
ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_feature = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
ros::Subscriber sub_img0 = n.subscribe(IMAGE0_TOPIC, 100, img0_callback);
ros::Subscriber sub_img1 = n.subscribe(IMAGE1_TOPIC, 100, img1_callback);
ros::Subscriber sub_restart = n.subscribe("/vins_restart", 100, restart_callback);
ros::Subscriber sub_imu_switch = n.subscribe("/vins_imu_switch", 100, imu_switch_callback);
ros::Subscriber sub_cam_switch = n.subscribe("/vins_cam_switch", 100, cam_switch_callback);
std::thread sync_thread{sync_process}; //创建sync_thread线程，指向sync_process，这里边处理了processMeasurements的线程
ros::spin(); // 用于触发topic, service的响应队列
// 如果你的程序写了相关的消息订阅函数，那么程序在执行过程中，除了主程序以外，ROS还会自动在后台按照你规定的格式，接受订阅的消息，但是所接到的消息并不是
// 立刻就被处理，而是必须要等到ros::spin()或ros::spinOnce()执行的时候才被调用，这就是消息回到函数的原理
其中有几个比较重要的函数，在下方进行说明。
1.2.2 imu_callback
其中imu_callback中订阅imu信息，并将器填充到accBuf和gyrBuf中，之后执行了vins_estimator/src/estimator/estimator.cpp中inputIMU函数的fastPredictIMU、pubLatestOdometry函数
fastPredictIMU使用上一时刻的姿态进行快速的imu预积分，这个信息根据processIMU的最新数据Ps[frame_count]、Rs[frame_count]、Vs[frame_count]、Bas[frame_count]、Bgs[frame_count]来进行预积分，从而保证信息能够正常发布。
// 使用上一时刻的姿态进行快速的imu预积分
// 用来预测最新P,V,Q的姿态
// -latest_p,latest_q,latest_v,latest_acc_0,latest_gyr_0 最新时刻的姿态。这个的作用是为了刷新姿态的输出，但是这个值的误差相对会比较大，是未经过非线性优化获取的初始值。
void Estimator::fastPredictIMU(double t, Eigen::Vector3d linear_acceleration, Eigen::Vector3d angular_velocity)
其中latest_Ba、latest_Bb是在预积分处就已经计算完毕的
// 其中包含了检测关键帧,估计外部参数,初始化,状态估计,划窗等等。
/**
* 处理一帧图像特征
* 1、提取前一帧与当前帧的匹配点
* 2、在线标定外参旋转
* 利用两帧之间的Camera旋转和IMU积分旋转，构建最小二乘问题，SVD求解外参旋转
* 1) Camera系，两帧匹配点计算本质矩阵E，分解得到四个解，根据三角化成功点比例确定最终正确解R、t，得到两帧之间的旋转R
* 2) IMU系，积分计算两帧之间的旋转
* 3) 根据旋转构建最小二乘问题，SVD求解外参旋转
* 3、系统初始化
* 4、3d-2d Pnp求解当前帧位姿
* 5、三角化当前帧特征点
* 6、滑窗执行Ceres优化，边缘化，更新滑窗内图像帧的状态（位姿、速度、偏置、外参、逆深度、相机与IMU时差）
* 7、剔除outlier点
* 8、用当前帧与前一帧位姿变换，估计下一帧位姿，初始化下一帧特征点的位置
* 9、移动滑窗，更新特征点的观测帧集合、观测帧索引（在滑窗中的位置）、首帧观测帧和深度值，删除没有观测帧的特征点
* 10、删除优化后深度值为负的特征点
* @param image 图像帧特征
* @param header 时间戳
*/
void Estimator::processImage
(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const double header)
经过fastPredictIMU函数后我们可以拿到latest_P, latest_Q, latest_V三个变量，之后即可通过pubLatestOdometry广播odom信息
//构建一个odometry的msg并发布
void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, const Eigen::Vector3d &V, double t)
1.2.3 feature_callback
feature_callback的作用是获取点云数据，之后填充featureFrame，并把featureFrame通过inputFeature输入到estimator，且填充了featureBuf
/**
* 订阅一帧跟踪的特征点，包括3D坐标、像素坐标、速度，交给estimator处理
*/
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
for (unsigned int i = 0; i < feature_msg->points.size(); i++)
{
int feature_id = feature_msg->channels[0].values[i];
int camera_id = feature_msg->channels[1].values[i];
double x = feature_msg->points[i].x;
double y = feature_msg->points[i].y;
double z = feature_msg->points[i].z;
double p_u = feature_msg->channels[2].values[i];
double p_v = feature_msg->channels[3].values[i];
double velocity_x = feature_msg->channels[4].values[i];
double velocity_y = feature_msg->channels[5].values[i];
if(feature_msg->channels.size() > 5)
{
double gx = feature_msg->channels[6].values[i];
double gy = feature_msg->channels[7].values[i];
double gz = feature_msg->channels[8].values[i];
pts_gt[feature_id] = Eigen::Vector3d(gx, gy, gz);
//printf("receive pts gt %d %f %f %f\n", feature_id, gx, gy, gz);
}
ROS_ASSERT(z == 1);
Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
featureFrame[feature_id].emplace_back(camera_id, xyz_uv_velocity);
}
double t = feature_msg->header.stamp.toSec();
estimator.inputFeature(t, featureFrame);
return;
}
1.2.4 sync_process
之后通过
// 从两个图像队列中取出最早的一帧，并从队列删除，双目要求两帧时差不得超过0.003s
std::thread sync_thread{sync_process}; //创建sync_thread线程，指向sync_process，这里边处理了measurementpross的线程
进入sync_process进行处理
/**
* 从两个图像队列中取出最早的一帧，并从队列删除，双目要求两帧时差不得超过0.003s
*/
void sync_process()
{
while(1)
{
if(STEREO)
{
cv::Mat image0, image1;
std_msgs::Header header;
double time = 0;
m_buf.lock();
if (!img0_buf.empty() && !img1_buf.empty())
{
double time0 = img0_buf.front()->header.stamp.toSec();
double time1 = img1_buf.front()->header.stamp.toSec();
// 双目相机左右图像时差不得超过0.003s
if(time0 < time1 - 0.003)
{
img0_buf.pop();
printf("throw img0\n");
}
else if(time0 > time1 + 0.003)
{
img1_buf.pop();
printf("throw img1\n");
}
else
{
// 提取缓存队列中最早一帧图像，并从队列中删除
time = img0_buf.front()->header.stamp.toSec();
header = img0_buf.front()->header;
image0 = getImageFromMsg(img0_buf.front());
img0_buf.pop();
image1 = getImageFromMsg(img1_buf.front());
img1_buf.pop();
}
}
m_buf.unlock();
if(!image0.empty())
estimator.inputImage(time, image0, image1);
}
else
{
cv::Mat image;
std_msgs::Header header;
double time = 0;
m_buf.lock();
if(!img0_buf.empty())
{
time = img0_buf.front()->header.stamp.toSec();
header = img0_buf.front()->header;
image = getImageFromMsg(img0_buf.front());
img0_buf.pop();
}
m_buf.unlock();
if(!image.empty())
estimator.inputImage(time, image);
}
std::chrono::milliseconds dura(2);
std::this_thread::sleep_for(dura);
}
}
该函数中，首先对是否双目进行判断。
如果是双目，需要检测同步问题。对双目的时间进行判断，时间间隔小于0.003s的话则使用getImageFromMsg将其输入到image0和image1变量之中。之后estimator.inputImage。
如果是单目，则直接estimator.inputImage
2. 图像输入estimator.cpp
2.1 inputImage
/**
* 输入一帧图像
* 1、featureTracker，提取当前帧特征点
* 2、添加一帧特征点，processMeasurements处理
*/
void Estimator::inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1)
{
inputImageCnt++;
// 特征点id，(x,y,z,pu,pv,vx,vy)
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
TicToc featureTrackerTime;
/**
* 跟踪一帧图像，提取当前帧特征点
* 1、用前一帧运动估计特征点在当前帧中的位置，如果特征点没有速度，就直接用前一帧该点位置
* 2、LK光流跟踪前一帧的特征点，正反向，删除跟丢的点；如果是双目，进行左右匹配，只删右目跟丢的特征点
* 3、对于前后帧用LK光流跟踪到的匹配特征点，计算基础矩阵，用极线约束进一步剔除outlier点（代码注释掉了）
* 4、如果特征点不够，剩余的用角点来凑；更新特征点跟踪次数
* 5、计算特征点归一化相机平面坐标，并计算相对与前一帧移动速度
* 6、保存当前帧特征点数据（归一化相机平面坐标，像素坐标，归一化相机平面移动速度）
* 7、展示，左图特征点用颜色区分跟踪次数（红色少，蓝色多），画个箭头指向前一帧特征点位置，如果是双目，右图画个绿色点
*/
if(_img1.empty())
featureFrame = featureTracker.trackImage(t, _img);
else
featureFrame = featureTracker.trackImage(t, _img, _img1);
//printf("featureTracker time: %f\n", featureTrackerTime.toc());
// 发布跟踪图像
if (SHOW_TRACK)
{
cv::Mat imgTrack = featureTracker.getTrackImage();
pubTrackImage(imgTrack, t);
}
// 添加一帧特征点，处理
if(MULTIPLE_THREAD)
{
if(inputImageCnt % 2 == 0)
{
mBuf.lock();
featureBuf.push(make_pair(t, featureFrame));
mBuf.unlock();
}
}
else
{
mBuf.lock();
featureBuf.push(make_pair(t, featureFrame));
mBuf.unlock();
TicToc processTime;
processMeasurements();
printf("process time: %f\n", processTime.toc());
}
}
当中需要先设置参数，并开启processMeasurements线程
setParameter();
然后追踪图像上的特征trackImage，之后会进行详解,其中得到了featureFrame
if(_img1.empty())
featureFrame = featureTracker.trackImage(t, _img);// 追踪单目
else
featureFrame = featureTracker.trackImage(t, _img, _img1);// 追踪双目
然后，getTrackImage对特征到跟踪的图像进行一些处理。并把追踪的图片imgTrack发布出去.
if (SHOW_TRACK)//这个应该是展示轨迹
{
cv::Mat imgTrack = featureTracker.getTrackImage();
pubTrackImage(imgTrack, t);
}
然后，填充featureBuf
最后执行processMeasurements，这是处理全部量测的线程,IMU的预积分,特征点的处理等等都在这里进行.
2.2 trackImage
根据2.1的讲述，我们发现当中用到了trackImage这个函数，这个函数在vins_estimator/src/featureTracker/feature_tracker.cpp
/**
* 跟踪一帧图像，提取当前帧特征点
* 1、用前一帧运动估计特征点在当前帧中的位置
* 2、LK光流跟踪前一帧的特征点，正反向，删除跟丢的点；如果是双目，进行左右匹配，只删右目跟丢的特征点
* 3、对于前后帧用LK光流跟踪到的匹配特征点，计算基础矩阵，用极线约束进一步剔除outlier点（代码注释掉了）
* 4、如果特征点不够，剩余的用角点来凑；更新特征点跟踪次数
* 5、计算特征点归一化相机平面坐标，并计算相对与前一帧移动速度
* 6、保存当前帧特征点数据（归一化相机平面坐标，像素坐标，归一化相机平面移动速度）
* 7、展示，左图特征点用颜色区分跟踪次数（红色少，蓝色多），画个箭头指向前一帧特征点位置，如果是双目，右图画个绿色点
*/
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> FeatureTracker::trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1)
图像处理可以添加图像处理的部分，比如直方图均衡等等方法。
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));//createCLAHE 直方图均衡
clahe->apply(cur_img, cur_img);
if(!rightImg.empty())
clahe->apply(rightImg, rightImg);
}
2.2.1 hasPrediction
会对上一帧的特征点进行处理，计算出上一帧运动估计特征点在当前帧中的位置。对于前后帧用LK光流跟踪到的匹配特征点，计算基础矩。
if(hasPrediction)
{
cur_pts = predict_pts;
// LK光流跟踪两帧图像特征点，金字塔为1层
cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 1,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);
// 跟踪到的特征点数量
int succ_num = 0;
for (size_t i = 0; i < status.size(); i++)
{
if (status[i])
succ_num++;
}
// 特征点太少，金字塔调整为3层，再跟踪一次
if (succ_num < 10)
cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);
}
2.2.2 if(SHOW_TRACK)
画出追踪情况，就是在图像上的特征点位置出画圈圈，如果是双目的话就连线。
// 展示，左图特征点用颜色区分跟踪次数（红色少，蓝色多），画个箭头指向前一帧特征点位置，如果是双目，右图画个绿色点
if(SHOW_TRACK)
drawTrack(cur_img, rightImg, ids, cur_pts, cur_right_pts, prevLeftPtsMap);
2.2.3 setMask
在已跟踪到角点的位置上，将mask对应位置上设为0,
意为在cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
进行操作时在该点不再重复进行角点检测，这样可以使角点分布更加均匀
具体详情见开源的注释代码。
/**
* 特征点画个圈（半径MIN_DIST）存mask图，同时特征点集合按跟踪次数从大到小重排序
*/
void FeatureTracker::setMask()
2.2.4 goodFeaturesToTrack
如果当前图像的特征点cur_pts数目小于规定的最大特征点数目MAX_CNT，则进行提取。
提取使用的cv::goodFeaturesToTrack。将点保存到n_pts
/* goodFeaturesToTrack
_image：8位或32位浮点型输入图像，单通道
_corners：保存检测出的角点
maxCorners：角点数目最大值，如果实际检测的角点超过此值，则只返回前maxCorners个强角点
qualityLevel：角点的品质因子
minDistance：对于初选出的角点而言，如果在其周围minDistance范围内存在其他更强角点，则将此角点删除
_mask：指定感兴趣区，如不需在整幅图上寻找角点，则用此参数指定ROI
blockSize：计算协方差矩阵时的窗口大小
useHarrisDetector：指示是否使用Harris角点检测，如不指定，则计算shi-tomasi角点
harrisK：Harris角点检测需要的k值 */
cv::goodFeaturesToTrack(cur_img, n_pts, MAX_CNT - cur_pts.size(), 0.01, MIN_DIST, mask);
// mask 这里肯定是指定感兴趣区，如不需在整幅图上寻找角点，则用此参数指定ROI
之后将n_pts保存到cur_pts之中
2.2.5 undistortedPts
将像素座标系下的座标，转换为归一化相机座标系下的座标 即un_pts为归一化相机座标系下的座标。
/**
* 像素点计算归一化相机平面点，带畸变校正
*/
vector<cv::Point2f> FeatureTracker::undistortedPts(vector<cv::Point2f> &pts, camodocal::CameraPtr cam)
{
vector<cv::Point2f> un_pts;
for (unsigned int i = 0; i < pts.size(); i++)
{
// 特征点像素坐标
Eigen::Vector2d a(pts[i].x, pts[i].y);
Eigen::Vector3d b;
// 像素点计算归一化相机平面点，带畸变校正
cam->liftProjective(a, b);
// 归一化相机平面点
un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
}
return un_pts;
}
2.2.6 cam->liftProjective(a, b);
这个函数是对鱼眼相机模型的标定及去畸变过程
/**
* \brief Lifts a point from the image plane to its projective ray
* \param p image coordinates
* \param P coordinates of the projective ray
* 这个函数是对鱼眼相机模型的标定及去畸变过程
*/
void
PinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const
之后通过ptsVelocity计算当前帧相对于前一帧 特征点沿x,y方向的像素移动速度
/**
* 计算当前帧归一化相机平面特征点在x、y方向上的移动速度
* @param pts 当前帧归一化相机平面特征点
*/
vector<cv::Point2f> FeatureTracker::ptsVelocity(vector<int> &ids, vector<cv::Point2f> &pts,
map<int, cv::Point2f> &cur_id_pts, map<int, cv::Point2f> &prev_id_pts)
{
vector<cv::Point2f> pts_velocity;
cur_id_pts.clear();
for (unsigned int i = 0; i < ids.size(); i++)
{
cur_id_pts.insert(make_pair(ids[i], pts[i]));
}
// caculate points velocity
if (!prev_id_pts.empty())
{
double dt = cur_time - prev_time;
// 遍历当前帧归一化相机平面特征点
for (unsigned int i = 0; i < pts.size(); i++)
{
std::map<int, cv::Point2f>::iterator it;
it = prev_id_pts.find(ids[i]);
if (it != prev_id_pts.end())
{
// 计算点在归一化相机平面上x、y方向的移动速度
double v_x = (pts[i].x - it->second.x) / dt;
double v_y = (pts[i].y - it->second.y) / dt;
pts_velocity.push_back(cv::Point2f(v_x, v_y));
}
else
pts_velocity.push_back(cv::Point2f(0, 0));
}
}
else
{
for (unsigned int i = 0; i < cur_pts.size(); i++)
{
pts_velocity.push_back(cv::Point2f(0, 0));
}
}
return pts_velocity;
}
2.3 双目摄像头
如果是双目相机，那么在右目上追踪左目的特征点。使用的函数是calcOpticalFlowPyrLK
/*光流跟踪是在左右两幅图像之间进行cur left ---- cur right
prevImg 第一幅8位输入图像 或 由buildOpticalFlowPyramid()构造的金字塔。
nextImg 第二幅与preImg大小和类型相同的输入图像或金字塔。
prevPts 光流法需要找到的二维点的vector。点坐标必须是单精度浮点数。
nextPts 可以作为输入，也可以作为输出。包含输入特征在第二幅图像中计算出的新位置的二维点（单精度浮点坐标）的输出vector。当使用OPTFLOW_USE_INITIAL_FLOW 标志时，nextPts的vector必须与input的大小相同。
status 输出状态vector(类型：unsigned chars)。如果找到了对应特征的流，则将向量的每个元素设置为1；否则，置0。
err 误差输出vector。vector的每个元素被设置为对应特征的误差，可以在flags参数中设置误差度量的类型；如果没有找到流，则未定义误差（使用status参数来查找此类情况）。
winSize 每级金字塔的搜索窗口大小。
maxLevel 基于最大金字塔层次数。如果设置为0，则不使用金字塔（单级）；如果设置为1，则使用两个级别，等等。如果金字塔被传递到input，那么算法使用的级别与金字塔同级别但不大于MaxLevel。
criteria 指定迭代搜索算法的终止准则（在指定的最大迭代次数标准值（criteria.maxCount）之后，或者当搜索窗口移动小于criteria.epsilon。）
flags 操作标志，可选参数：
OPTFLOW_USE_INITIAL_FLOW：使用初始估计，存储在nextPts中；如果未设置标志，则将prevPts复制到nextPts并被视为初始估计。
OPTFLOW_LK_GET_MIN_EIGENVALS：使用最小本征值作为误差度量（见minEigThreshold描述）；如果未设置标志，则将原始周围的一小部分和移动的点之间的 L1 距离除以窗口中的像素数，作为误差度量。
minEigThreshold
算法所计算的光流方程的2x2标准矩阵的最小本征值（该矩阵称为[Bouguet00]中的空间梯度矩阵）÷ 窗口中的像素数。如果该值小于MinEigThreshold，则过滤掉相应的特征，相应的流也不进行处理。因此可以移除不好的点并提升性能。 */
cv::calcOpticalFlowPyrLK(cur_img, rightImg, cur_pts, cur_right_pts, status, err, cv::Size(21, 21), 3);
2.3.1 if(FLOW_BACK)
如果这个打开，就想前边的左右目图像的位置换一下，在进行一次特征跟踪，目的是反向跟踪，得到左右目都匹配到的点
// 反向LK光流计算一次
if(FLOW_BACK)
{
vector<uchar> reverse_status;
vector<cv::Point2f> reverse_pts = prev_pts;
cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 1,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);
//cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 3);
// 正向、反向都匹配到了，且用正向匹配点反向匹配回来，与原始点距离不超过0.5个像素，认为跟踪到了
for(size_t i = 0; i < status.size(); i++)
{
if(status[i] && reverse_status[i] && distance(prev_pts[i], reverse_pts[i]) <= 0.5)
{
status[i] = 1;
}
else
status[i] = 0;
}
}
之后和单目一样执行undistortedPts 和 ptsVelocity
2.3.2 制作featureFrame
// 添加当前帧特征点（归一化相机平面坐标，像素坐标，归一化相机平面移动速度）
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
其中，camera_id = 0为左目上的点，camera_id = 1，为右目上的点。
3 IMU处理estimator.cpp
3.1 判断IMU数据是否可用
if ((!USE_IMU || IMUAvailable(feature.first + td)))//如果不用imu或者超时
其中
// 判断输入的时间t时候的imu是否可用
bool Estimator::IMUAvailable(double t)
3.2 获得accVector和gyrVector
对imu的时间进行判断，讲队列里的imu数据放入到accVector和gyrVector中
// 对imu的时间进行判断，讲队列里的imu数据放入到accVector和gyrVector中，完成之后返回true
bool Estimator::getIMUInterval(double t0, double t1, vector<pair<double, Eigen::Vector3d>> &accVector,
vector<pair<double, Eigen::Vector3d>> &gyrVector)
3.3 初始化IMU的姿态
initFirstIMUPose,其实很简单,就是求一个姿态角,然后把航向角设为0
//第一帧IMU姿态初始化
// 用初始时刻加速度方向对齐重力加速度方向，得到一个旋转，使得初始IMU的z轴指向重力加速度方向
void Estimator::initFirstIMUPose(vector<pair<double, Eigen::Vector3d>> &accVector)
3.4 处理IMU数据,运行processIMU
/**
* 处理一帧IMU，积分
* 用前一图像帧位姿，前一图像帧与当前图像帧之间的IMU数据，积分计算得到当前图像帧位姿
* Rs，Ps，Vs
* @param t 当前时刻
* @param dt 与前一帧时间间隔
* @param linear_acceleration 当前时刻加速度
* @param angular_velocity 当前时刻角速度
*/
void Estimator::processIMU(double t, double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
其中frame_count是值窗内的第几帧图像
下边是新建一个预积分项目
pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
预积分
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
// push_back重载的时候就已经进行了预积分
其中的push_back
void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
{
dt_buf.push_back(dt);
acc_buf.push_back(acc);
gyr_buf.push_back(gyr);
propagate(dt, acc, gyr);
}
其中的propagate
// IMU预积分传播方程
// 积分计算两个关键帧之间IMU测量的变化量
// 同时维护更新预积分的Jacobian和Covariance,计算优化时必要的参数
/**
* IMU中值积分传播
* 1、前一时刻状态计算当前时刻状态，PVQ，Ba，Bg
* 2、计算当前时刻的误差Jacobian，误差协方差 todo
*/
void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
{
dt = _dt;
acc_1 = _acc_1;
gyr_1 = _gyr_1;
Vector3d result_delta_p;
Quaterniond result_delta_q;
Vector3d result_delta_v;
Vector3d result_linearized_ba;
Vector3d result_linearized_bg;
/**
* 中值积分
* 1、前一时刻状态计算当前时刻状态，PVQ，其中Ba，Bg保持不变
* 2、计算当前时刻的误差Jacobian，误差协方差 todo
*/
midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,
linearized_ba, linearized_bg,
result_delta_p, result_delta_q, result_delta_v,
result_linearized_ba, result_linearized_bg, 1);
//checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v,
// linearized_ba, linearized_bg);
delta_p = result_delta_p;
delta_q = result_delta_q;
delta_v = result_delta_v;
linearized_ba = result_linearized_ba;
linearized_bg = result_linearized_bg;
delta_q.normalize();
sum_dt += dt;
acc_0 = acc_1;
gyr_0 = gyr_1;
}
其中的midPointIntegration.这里边就涉及到了IMU的传播方针和协方差矩阵.雅克比矩阵等等.哪里不懂可以VIO的理论知识。
// 中值积分递推Jacobian和Covariance
// _acc_0上次测量加速度 _acc_1本次测量加速度 delta_p上一次的位移 result_delta_p位置变化量计算结果 update_jacobian是否更新雅克比基本方法就涉及到了IMU的创博方针和器方差矩阵的窗哦sdf
void midPointIntegration(double _dt,
const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)
之后计算对应绝对坐标系下的位置等
// Rs Ps Vs是frame_count这一个图像帧开始的预积分值,是在绝对坐标系下的.
int j = frame_count;
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;//移除了偏执的加速度
Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];//移除了偏执的gyro
Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
Vs[j] += dt * un_acc;
4. 图像处理estimator.cpp
4.1 processImage
函数入口processMeasurements中的processImage(feature.second, feature.first);输入的之后关键帧和时间
/* addFeatureCheckParallax
对当前帧与之前帧进行视差比较，如果是当前帧变化很小，就会删去倒数第二帧，如果变化很大，就删去最旧的帧。并把这一帧作为新的关键帧
这样也就保证了划窗内优化的,除了最后一帧可能不是关键帧外,其余的都是关键帧
VINS里为了控制优化计算量，在实时情况下，只对当前帧之前某一部分帧进行优化，而不是全部历史帧。局部优化帧的数量就是窗口大小。
为了维持窗口大小，需要去除旧的帧添加新的帧，也就是边缘化 Marginalization。到底是删去最旧的帧（MARGIN_OLD）还是删去刚
刚进来窗口倒数第二帧(MARGIN_SECOND_NEW)
如果大于最小像素,则返回true */
/**
* 添加特征点记录，并检查当前帧是否为关键帧
* @param frame_count 当前帧在滑窗中的索引
* @param image 当前帧特征（featureId，cameraId，feature）
*/
bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
判断之后,确定marg掉那个帧
// 检测关键帧
if (f_manager.addFeatureCheckParallax(frame_count, image, td))
{
marginalization_flag = MARGIN_OLD;//新一阵将被作为关键帧!
//printf("keyframe\n");
}
else
{
marginalization_flag = MARGIN_SECOND_NEW;
//printf("non-keyframe\n");
}
4.2 估计相机和IMU的外参
// 当前帧预积分（前一帧与当前帧之间的IMU预积分）
imageframe.pre_integration = tmp_pre_integration;
all_image_frame.insert(make_pair(header, imageframe));
// 重置预积分器
tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
// 估计一个外部参,并把ESTIMATE_EXTRINSIC置1,输出ric和RIC
if(ESTIMATE_EXTRINSIC == 2)
首先找到对应的图像
vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
// 这个里边放的是新图像和上一帧
使用CalibrationExRotation计算参数
/* CalibrationExRotation
当外参完全不知道的时候，可以在线对其进行初步估计,然后在后续优化时，会在optimize函数中再次优化。
输入是新图像和上一阵图像的位姿 和二者之间的imu预积分值,输出旋转矩阵
对应VIO课程第七讲中对外参矩阵的求解 */
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
4.3 如果没有初始化,那么就先进行初始化
4.3.1 单目+IMU的初始化
// monocular + IMU initilization
// 单目+IMU系统初始化
if (!STEREO && USE_IMU)
{
// 要求滑窗满
if (frame_count == WINDOW_SIZE)
{
bool result = false;
// 如果上次初始化没有成功，要求间隔0.1s
if(ESTIMATE_EXTRINSIC != 2 && (header - initial_timestamp) > 0.1)
{
/**
* todo
* 系统初始化
* 1、计算滑窗内IMU加速度的标准差，用于判断移动快慢
* 2、在滑窗中找到与当前帧具有足够大的视差，同时匹配较为准确的一帧，计算相对位姿变换
* 1) 提取滑窗中每帧与当前帧之间的匹配点（要求点在两帧之间一直被跟踪到，属于稳定共视点），超过20个则计算视差
* 2) 两帧匹配点计算本质矩阵E，恢复R、t
* 3) 视差超过30像素，匹配内点数超过12个，则认为符合要求，返回当前帧
* 3、以上面找到的这一帧为参考系，Pnp计算滑窗每帧位姿，然后三角化所有特征点，构建BA（最小化点三角化前后误差）优化每帧位姿
* 1) 3d-2d Pnp求解每帧位姿
* 2) 对每帧与l帧、当前帧三角化
* 3) 构建BA，最小化点三角化前后误差，优化每帧位姿
* 4) 保存三角化点
* 4、对滑窗中所有帧执行Pnp优化位姿
* 5、Camera与IMU初始化，零偏、尺度、重力方向
*/
result = initialStructure();
initial_timestamp = header;
}
if(result)
{
// 滑窗执行Ceres优化，边缘化，更新滑窗内图像帧的状态（位姿、速度、偏置、外参、逆深度、相机与IMU时差）
optimization();
// 用优化后的当前帧位姿更新IMU积分的基础位姿，用于展示IMU轨迹
updateLatestStates();
solver_flag = NON_LINEAR;
// 移动滑窗，更新特征点的观测帧集合、观测帧索引（在滑窗中的位置）、首帧观测帧和深度值，删除没有观测帧的特征点
slideWindow();
ROS_INFO("Initialization finish!");
}
else
// 移动滑窗，更新特征点的观测帧集合、观测帧索引（在滑窗中的位置）、首帧观测帧和深度值，删除没有观测帧的特征点
slideWindow();
}
}
其中的initialStructure还含有很多函数.
// 视觉和惯性的对其,对应https://mp.weixin.qq.com/s/9twYJMOE8oydAzqND0UmFw中的visualInitialAlign
/* visualInitialAlign
很具VIO课程第七讲:一共分为5步:
1估计旋转外参. 2估计陀螺仪bias 3估计中立方向,速度.尺度初始值 4对重力加速度进一步优化 5将轨迹对其到世界坐标系 */
bool Estimator::visualInitialAlign()
// 视觉IMu的对其
bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x)
// 更新得到新的陀螺仪漂移Bgs
// 对应视觉IMU对其的第二部分
// 对应https://mp.weixin.qq.com/s/9twYJMOE8oydAzqND0UmFw中的公式31-34
void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
// 初始化速度、重力和尺度因子
// 对应 https://mp.weixin.qq.com/s/9twYJMOE8oydAzqND0UmFw 中的公式34-36
bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
成功初始化之后则执行，optimization()、 updateLatestStates() 、slideWindow()
if(result)//如果初始化成功
{
optimization();//先进行一次滑动窗口非线性优化，得到当前帧与第一帧的位姿
updateLatestStates();
solver_flag = NON_LINEAR;
slideWindow();//滑动窗口
ROS_INFO("Initialization finish!");
}
else//滑掉这一窗
slideWindow();
updateLatestStates() 和slideWindow()这两个函数比较简单
// 让此时刻的值都等于上一时刻的值,用来更新状态
void Estimator::updateLatestStates()
// 滑动窗口法
void Estimator::slideWindow()
// 道理很简单,就是把前后元素交换
optimization()为基于滑动窗口的紧耦合的非线性优化，残差项的构造和求解
/**
* 滑窗执行Ceres优化，边缘化，更新滑窗内图像帧的状态（位姿、速度、偏置、外参、逆深度、相机与IMU时差）
*/
void Estimator::optimization()
4.3.2 双目+IMU初始化
// stereo + IMU initilization
if(STEREO && USE_IMU)
求解深度
// 双目三角化
// 结果放入了feature的estimated_depth中
void FeatureManager::triangulate(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])
// 利用svd方法对双目进行三角化
void FeatureManager::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
Eigen::Vector2d &point0, Eigen::Vector2d &point1, Eigen::Vector3d &point_3d)
有了深度之后就可以进行PnP求解
// 有了深度，当下一帧照片来到以后就可以利用pnp求解位姿了
void FeatureManager::initFramePoseByPnP(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])
之后求解陀螺仪的偏执,并对IMU预积分值进行重新传播
solveGyroscopeBias(all_image_frame, Bgs);
// 对之前预积分得到的结果进行更新。
// 预积分的好处查看就在于你得到新的Bgs，不需要又重新再积分一遍，可以通过Bgs对位姿，速度的一阶导数，进行线性近似，得到新的Bgs求解出MU的最终结果。
for (int i = 0; i <= WINDOW_SIZE; i++)
{
pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
}
进行优化
optimization();
updateLatestStates();
solver_flag = NON_LINEAR;
slideWindow();
其中的optimization()、 updateLatestStates()、 slideWindow()在下一篇博客进行讲解
4.3.3双目初始化
// stereo only initilization
if(STEREO && !USE_IMU)
{
f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
optimization();
if(frame_count == WINDOW_SIZE)
{
optimization();
updateLatestStates();
solver_flag = NON_LINEAR;
slideWindow();
ROS_INFO("Initialization finish!");
}
}
5. 回环检测pose_graph_node.cpp
本节主要讲loop_fusion包的程序结构，loop_fusion主要作用：利用词袋模型进行图像的回环检测。在vinsmono中，该程序包处于pose_graph包内。vins_fusion与vins_mono一个差别在于，回环检测的点云数据在mono中有回调供给VIO进行非线性优化，而在fusion中，VIO估计完全独立于回环检测的结果。即回环检测的全局估计会受到VIO的影响，但是VIO不受全局估计的影响。（这个意思是fusion我们可以单独使用VIO部分）
5.1 程序入口
5.1读取配置文件
通过fsSetting进行相应的参数配置，其中比较重要的是读入了vocabulary_file,即在support_files里面的brief_k10L6.bin。以及BRIEF_PATTERN_FILE。通过posegraph.loadVocabulary为posegraph的类成员 BriefDatabase db设置属性以及BriefVocabulary voc赋值。以及为BRIEF_PATTERN_FILE赋值。为后期keyframe的构建创造一个基础。
cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
if(!fsSettings.isOpened())
{
std::cerr << "ERROR: Wrong path to settings" << std::endl;
}
cameraposevisual.setScale(0.1);
cameraposevisual.setLineWidth(0.01);
std::string IMAGE_TOPIC;
int LOAD_PREVIOUS_POSE_GRAPH;
ROW = fsSettings["image_height"];
COL = fsSettings["image_width"];
std::string pkg_path = ros::package::getPath("loop_fusion");
string vocabulary_file = pkg_path + "/../support_files/brief_k10L6.bin";
cout << "vocabulary_file" << vocabulary_file << endl;
posegraph.loadVocabulary(vocabulary_file);
BRIEF_PATTERN_FILE = pkg_path + "/../support_files/brief_pattern.yml";
cout << "BRIEF_PATTERN_FILE" << BRIEF_PATTERN_FILE << endl;
5.1.2LOAD_PREVIOUS_POSE_GRAPH
即是否要加载原有的地图信息，如果加载了之前的信息，则posegraph.loadPoseGraph，之前所有的关键帧的序号sequence都设置为0，base_sequence也是0。不过不加载之前的信息，base_sequence=1。
if (LOAD_PREVIOUS_POSE_GRAPH)
{
printf("load pose graph\n");
m_process.lock();
posegraph.loadPoseGraph();
m_process.unlock();
printf("load pose graph finish\n");
load_flag = 1;
}
else
{
printf("no previous pose graph\n");
load_flag = 1;
}
5.1.3 订阅话题，发布话题
// 订阅里程计
ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 2000, vio_callback);
// 订阅图像
ros::Subscriber sub_image = n.subscribe(IMAGE_TOPIC, 2000, image_callback);
// 订阅关键帧位姿
ros::Subscriber sub_pose = n.subscribe("/vins_estimator/keyframe_pose", 2000, pose_callback);
// 订阅外参
ros::Subscriber sub_extrinsic = n.subscribe("/vins_estimator/extrinsic", 2000, extrinsic_callback);
// 订阅关键帧特征点
ros::Subscriber sub_point = n.subscribe("/vins_estimator/keyframe_point", 2000, point_callback);
//
ros::Subscriber sub_margin_point = n.subscribe("/vins_estimator/margin_cloud", 2000, margin_point_callback);
pub_match_img = n.advertise<sensor_msgs::Image>("match_image", 1000);
pub_camera_pose_visual = n.advertise<visualization_msgs::MarkerArray>("camera_pose_visual", 1000);
pub_point_cloud = n.advertise<sensor_msgs::PointCloud>("point_cloud_loop_rect", 1000);
pub_margin_cloud = n.advertise<sensor_msgs::PointCloud>("margin_cloud_loop_rect", 1000);
pub_odometry_rect = n.advertise<nav_msgs::Odometry>("odometry_rect", 1000);
process && command
process主要的作用是开启一个新线程，这一块为程序的主要执行部分，我们下一节详细阐述。而command是开启一个控制台键盘控制线程。键盘控制输入，有提供两种选择，在控制台上写入：‘s’：把当前Posegraph存储起来，并且把整个loop_fusion包的进程关掉。‘n’：图像更新序列,new_sequence()。
void command()
{
while(1)
{
char c = getchar();
if (c == 's')
{
m_process.lock();
posegraph.savePoseGraph();
m_process.unlock();
printf("save pose graph finish\nyou can set 'load_previous_pose_graph' to 1 in the config file to reuse it next time\n");
printf("program shutting down...\n");
ros::shutdown();
}
if (c == 'n')
new_sequence();
std::chrono::milliseconds dura(5);
std::this_thread::sleep_for(dura);
}
}
5.2 process
5.2.1 数据输入
首先process需要先判断image_buf,pose_buf,point_buf三个buff都不为空的时候，才进行运行，否则程序就休息5毫秒，继续监测。
// find out the messages with same time stamp
// 时间戳对齐
m_buf.lock();
if(!image_buf.empty() && !point_buf.empty() && !pose_buf.empty())
其中：
（1）image_buf，存放image_callback()图像回调函数的信息，接收相机话题的原始图片。并且当两张图片的时间戳相差1秒，或者说当图像的时间戳小于上一帧的时间戳，可以认为图像出现新序列，因此new_sequence()
（2）point_buf,存放point_callback()关键帧的三维特征点信息的回调。并且在这个回调函数中，同时利用point_cloud标准数据结构发布特征点三维点云以供可视化操作。发布的话题为：point_cloud_loop_rect。注意，此时的点云信息不是单纯vio得到的点云信息，而是在pose_graph参考坐标系下，关键帧点云的位置。因为vio自己有一个参考坐标系，而pose_graph求解出来的位姿图也有自己的坐标系，这两个坐标系有一个变换关系，写在了pose_graph.r_drift,pose_graph.t_drift里面。
（3）pose_buf，存放pose_callback()回调函数的信息。订阅的是关键帧的vio得到的关键帧的位姿信息。
从上面订阅的信息我们可以看出，只有当相机有关键帧的时候（vins_estimator中有对应的规则判断该帧是不是关键帧），才会进行进一步处理，整个loop_fusion处理的信息不是原始图像，而是一帧帧关键帧。
接着程序开始找一帧keyframe 对应的image_buf，point_buf，pose_buf三者的数据。即point_buf和pose_buf这个存储的点云和位姿是对应（image_buf里面的）哪一帧关键帧。最终的结果
（1）image_msg存放关键帧的图像原始信息
（2）point_msg存放了该关键帧里面的3D点云信息，由VIO以及pose_graph.r_drift, pose_graph.t_drift 解算得到。
（3）pose_msg存放了该关键帧的位姿信息。
if(!image_buf.empty() && !point_buf.empty() && !pose_buf.empty())
{
if (image_buf.front()->header.stamp.toSec() > pose_buf.front()->header.stamp.toSec())
{
pose_buf.pop();
printf("throw pose at beginning\n");
}
else if (image_buf.front()->header.stamp.toSec() > point_buf.front()->header.stamp.toSec())
{
point_buf.pop();
printf("throw point at beginning\n");
}
else if (image_buf.back()->header.stamp.toSec() >= pose_buf.front()->header.stamp.toSec()
&& point_buf.back()->header.stamp.toSec() >= pose_buf.front()->header.stamp.toSec())
{
pose_msg = pose_buf.front();
pose_buf.pop();
while (!pose_buf.empty())
pose_buf.pop();
while (image_buf.front()->header.stamp.toSec() < pose_msg->header.stamp.toSec())
image_buf.pop();
image_msg = image_buf.front();
image_buf.pop();
while (point_buf.front()->header.stamp.toSec() < pose_msg->header.stamp.toSec())
point_buf.pop();
point_msg = point_buf.front();
point_buf.pop();
}
}
5.2.2 关键帧
根据上面得到的三个信息，来构造关键帧！！用数据结构KeyFrame来表示，这里用了十个变量的KeyFrame构造函数来构造这个关键帧，十个变量的KeyFrame来构造函数默认没有brief_descriptor，因为我们VIO前端提取的并不需要特征点的描述子。在构造关键帧的时候，函数同时又增加了阈值大于20的FAST角点，来增加关键帧特征点的数量，同时，对这些特征点用相应的描述子来进行表述。
构造出一个向量容器，用来存放所有特征点的描述子，即KeyFrame类成员 vector<BRIEF::bitset> brief_descriptors（在keyframe.cpp文件中）
注意点：只有当两个关键帧之间的位移量(T - last_t).norm() > SKIP_DIS时候，才会构造新的关键帧。但是程序默认的SKIP_DIS为0。就是除非T和last_t完全相等，否则就会进入构造函数。
构造好的keyframe，通过posegraph.addKeyFrame(keyframe,1)，加入到全局姿态图当中去，第二个参数代表是需要回环检测detect_loop，这里直接默认设置需要。
// 位姿平移量间隔（关键帧）
if((T - last_t).norm() > SKIP_DIS)
{
vector<cv::Point3f> point_3d;
vector<cv::Point2f> point_2d_uv;
vector<cv::Point2f> point_2d_normal;
vector<double> point_id;
// 特征点
for (unsigned int i = 0; i < point_msg->points.size(); i++)
{
cv::Point3f p_3d;
p_3d.x = point_msg->points[i].x;
p_3d.y = point_msg->points[i].y;
p_3d.z = point_msg->points[i].z;
// 世界坐标
point_3d.push_back(p_3d);
cv::Point2f p_2d_uv, p_2d_normal;
double p_id;
p_2d_normal.x = point_msg->channels[i].values[0];
p_2d_normal.y = point_msg->channels[i].values[1];
p_2d_uv.x = point_msg->channels[i].values[2];
p_2d_uv.y = point_msg->channels[i].values[3];
p_id = point_msg->channels[i].values[4];
// 归一化相机平面坐标
point_2d_normal.push_back(p_2d_normal);
// 像素坐标
point_2d_uv.push_back(p_2d_uv);
// 特征点id
point_id.push_back(p_id);
//printf("u %f, v %f \n", p_2d_uv.x, p_2d_uv.y);
}
// 构造关键帧
KeyFrame* keyframe = new KeyFrame(pose_msg->header.stamp.toSec(), frame_index, T, R, image,
point_3d, point_2d_uv, point_2d_normal, point_id, sequence);
m_process.lock();
start_flag = 1;
// 添加关键帧
posegraph.addKeyFrame(keyframe, 1);
m_process.unlock();
frame_index++;
last_t = T;
5.3 PoseGraph
PoseGraph作为loop_fusion整个程序里面最重量级的类。整个程序都是维护这个main函数里面定义的全局变量PoseGraph posegraph来进行的。通过PoseGraph 的类函数，addKeyFrame来进行关键帧的添加。list<KeyFrame*>keyframelist 作为最重要的类成员，整个回环检测的过程就是用来维护这个关键帧链表list，当检测到回环的时候，更新这个链表的数据，即关键帧的位姿。BriefDatabase db 图像数据库信息，通过不断更新这个数据库，可以用来查询，即回环检测有没有回到之前曾经来过的地方。
5.3.1 addKeyFrame
检查该关键帧的序号是否有跳变，在整个loop_fusion里面，每当出现new_sequence()时候，关键帧的sequence会自增1，为什么一直需要关心一个关键帧的sequence呢？因为这里一个关键帧的位姿的参考系是建立在某一个sequence下面的，不同的sequence对应了不同w_t_vio,w_r_vio，就是该sequence 和base_sequence之间坐标系的转换关系。posegraph坐标系是以world frame作为坐标系的，world frame为base sequence(载入原来的图像数据库)或者（没有载入图像数据库的话）用sequence=1的序列作为坐标系。PoesGraph里面的两个类成员 w_t_vio,w_r_vio描述的就是当前序列的第一帧，与世界坐标系之间的转换关系。
//shift to base frame
Vector3d vio_P_cur;
Matrix3d vio_R_cur;
// 新加入一段
if (sequence_cnt != cur_kf->sequence)
{
sequence_cnt++;
sequence_loop.push_back(0);
w_t_vio = Eigen::Vector3d(0, 0, 0);
w_r_vio = Eigen::Matrix3d::Identity();
m_drift.lock();
t_drift = Eigen::Vector3d(0, 0, 0);
r_drift = Eigen::Matrix3d::Identity();
m_drift.unlock();
}
// 转换成世界坐标
cur_kf->getVioPose(vio_P_cur, vio_R_cur);
vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
vio_R_cur = w_r_vio * vio_R_cur;
cur_kf->updateVioPose(vio_P_cur, vio_R_cur);
cur_kf->index = global_index;
global_index++;
int loop_index = -1;
5.3.2 detectLoop
通过detectLoop()，查找到一个最合适得分最高的闭环候选帧（如果没找到，loop_index=-1）,此时本质是通过bag of word中的db.query()来进行查找的。并且无论有没有查找到，都用db.add()把当前关键帧的描述子添加到图像的数据库db中去。
/**
* 闭环检测
* 1、在db中查找当前描述子匹配帧，返回4帧候选帧，要求是当前帧50帧之前的帧
* 2、添加当前帧描述子到db
* 3、计算候选帧匹配得分，最大得分超过0.05，其他得分超过最大得分的0.3倍，认为找到闭环
* 4、返回最早的闭环帧idx
*/
int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index)
{
// put image into image_pool; for visualization
// 图像添加文字，展示特征点数量，加入集合，用于展示
cv::Mat compressed_image;
if (DEBUG_IMAGE)
{
int feature_num = keyframe->keypoints.size();
cv::resize(keyframe->image, compressed_image, cv::Size(376, 240));
putText(compressed_image, "feature_num:" + to_string(feature_num), cv::Point2f(10, 10), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255));
image_pool[frame_index] = compressed_image;
}
TicToc tmp_t;
//first query; then add this frame into database!
QueryResults ret;
TicToc t_query;
// 在db中查找当前描述子匹配帧，返回4帧，要求是当前帧50帧之前的帧
db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);
//printf("query time: %f", t_query.toc());
//cout << "Searching for Image " << frame_index << ". " << ret << endl;
TicToc t_add;
// 添加当前帧描述子到db
db.add(keyframe->brief_descriptors);
//printf("add feature time: %f", t_add.toc());
// ret[0] is the nearest neighbour's score. threshold change with neighour score
bool find_loop = false;
cv::Mat loop_result;
if (DEBUG_IMAGE)
{
// 最接近的一帧图像的得分
loop_result = compressed_image.clone();
if (ret.size() > 0)
putText(loop_result, "neighbour score:" + to_string(ret[0].Score), cv::Point2f(10, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255));
}
// visual loop result
if (DEBUG_IMAGE)
{
// 候选闭环帧，加上得分
for (unsigned int i = 0; i < ret.size(); i++)
{
int tmp_index = ret[i].Id;
auto it = image_pool.find(tmp_index);
cv::Mat tmp_image = (it->second).clone();
putText(tmp_image, "index: " + to_string(tmp_index) + "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255));
cv::hconcat(loop_result, tmp_image, loop_result);
}
}
// a good match with its nerghbour
// 最大得分超过0.05，其他得分超过最大得分的0.3倍，认为找到闭环
if (ret.size() >= 1 &&ret[0].Score > 0.05)
for (unsigned int i = 1; i < ret.size(); i++)
{
//if (ret[i].Score > ret[0].Score * 0.3)
if (ret[i].Score > 0.015)
{
find_loop = true;
int tmp_index = ret[i].Id;
if (DEBUG_IMAGE && 0)
{
auto it = image_pool.find(tmp_index);
cv::Mat tmp_image = (it->second).clone();
putText(tmp_image, "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255));
cv::hconcat(loop_result, tmp_image, loop_result);
}
}
}
/*
if (DEBUG_IMAGE)
{
cv::imshow("loop_result", loop_result);
cv::waitKey(20);
}
*/
// 返回最早的闭环帧idx
if (find_loop && frame_index > 50)
{
int min_index = -1;
for (unsigned int i = 0; i < ret.size(); i++)
{
if (min_index == -1 || (ret[i].Id < min_index && ret[i].Score > 0.015))
min_index = ret[i].Id;
}
return min_index;
}
else
return -1;
}
5.3.2 findConnection
若detectLoop()能够找到相似的一帧，此时，通过findConnection() 判断新旧两帧关联，匹配点大于25对，相对的偏航角位移小于30度并且相对位移少于20m，则返回true,其余情况均返回false。在true的条件下，更新参考系转化的变量。并且把当前帧的序号加入到optimize_buf中去。
// 检测到闭环
if (loop_index != -1)
{
//printf(" %d detect loop with %d \n", cur_kf->index, loop_index);
// 闭环帧
KeyFrame* old_kf = getKeyFrame(loop_index);
/**
* 计算当前帧与闭环帧之间的位姿差，判断是否闭环
* 1、通过brief描述子，计算当前帧与闭环匹配帧之间的匹配点，要求大于25个
* 2、3d-2d Pnp计算匹配闭环帧的位姿
* 3、闭环帧与当前帧之间的位姿误差不能太大，才认为闭环
*/
if (cur_kf->findConnection(old_kf))
{
// 记录历史上最早的闭环帧
if (earliest_loop_index > loop_index || earliest_loop_index == -1)
earliest_loop_index = loop_index;
Vector3d w_P_old, w_P_cur, vio_P_cur;
Matrix3d w_R_old, w_R_cur, vio_R_cur;
// 闭环帧位姿
old_kf->getVioPose(w_P_old, w_R_old);
// 当前帧位姿
cur_kf->getVioPose(vio_P_cur, vio_R_cur);
Vector3d relative_t;
Quaterniond relative_q;
// 当前帧与闭环帧的位姿差量，根据特征点匹配算出来的
relative_t = cur_kf->getLoopRelativeT();
relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix();
// 以闭环帧位姿为基础，加上位姿差量，得到当前帧位姿
w_P_cur = w_R_old * relative_t + w_P_old;
w_R_cur = w_R_old * relative_q;
double shift_yaw;
Matrix3d shift_r;
Vector3d shift_t;
if(use_imu)
{
// 当前帧由闭环匹配算出来的位姿，与里程计位姿，计算偏移量，认为前者是更精确的
shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x();
shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0));
}
else
shift_r = w_R_cur * vio_R_cur.transpose();
shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur;
// shift vio pose of whole sequence to the world frame
// todo
if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0)
{
w_r_vio = shift_r;
w_t_vio = shift_t;
// 当前帧加上闭环偏移量之后的位姿
vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
vio_R_cur = w_r_vio * vio_R_cur;
cur_kf->updateVioPose(vio_P_cur, vio_R_cur);
list<KeyFrame*>::iterator it = keyframelist.begin();
for (; it != keyframelist.end(); it++)
{
// 加上闭环偏移量
if((*it)->sequence == cur_kf->sequence)
{
Vector3d vio_P_cur;
Matrix3d vio_R_cur;
(*it)->getVioPose(vio_P_cur, vio_R_cur);
vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
vio_R_cur = w_r_vio * vio_R_cur;
(*it)->updateVioPose(vio_P_cur, vio_R_cur);
}
}
sequence_loop[cur_kf->sequence] = 1;
}
m_optimize_buf.lock();
// 加入优化队列
optimize_buf.push(cur_kf->index);
m_optimize_buf.unlock();
}
添加到keyframelist
最终，把该帧keyframe 添加到keyframelist中去。从此，keyframelist又增添了一个关键帧元素
m_keyframelist.lock();
Vector3d P;
Matrix3d R;
cur_kf->getVioPose(P, R);
P = r_drift * P + t_drift;
R = r_drift * R;
cur_kf->updatePose(P, R);
Quaterniond Q{R};
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time(cur_kf->time_stamp);
pose_stamped.header.frame_id = "world";
pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X;
pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y;
pose_stamped.pose.position.z = P.z();
pose_stamped.pose.orientation.x = Q.x();
pose_stamped.pose.orientation.y = Q.y();
pose_stamped.pose.orientation.z = Q.z();
pose_stamped.pose.orientation.w = Q.w();
path[sequence_cnt].poses.push_back(pose_stamped);
path[sequence_cnt].header = pose_stamped.header;
if (SAVE_LOOP_PATH)
{
ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
loop_path_file.setf(ios::fixed, ios::floatfield);
loop_path_file.precision(0);
loop_path_file << cur_kf->time_stamp * 1e9 << ",";
loop_path_file.precision(5);
loop_path_file << P.x() << ","
<< P.y() << ","
<< P.z() << ","
<< Q.w() << ","
<< Q.x() << ","
<< Q.y() << ","
<< Q.z() << ","
<< endl;
loop_path_file.close();
}
//draw local connection
if (SHOW_S_EDGE)
{
// 相邻边
list<KeyFrame*>::reverse_iterator rit = keyframelist.rbegin();
for (int i = 0; i < 4; i++)
{
if (rit == keyframelist.rend())
break;
Vector3d conncected_P;
Matrix3d connected_R;
if((*rit)->sequence == cur_kf->sequence)
{
(*rit)->getPose(conncected_P, connected_R);
posegraph_visualization->add_edge(P, conncected_P);
}
rit++;
}
}
if (SHOW_L_EDGE)
{
// 闭环边
if (cur_kf->has_loop)
{
//printf("has loop \n");
KeyFrame* connected_KF = getKeyFrame(cur_kf->loop_index);
Vector3d connected_P,P0;
Matrix3d connected_R,R0;
connected_KF->getPose(connected_P, connected_R);
//cur_kf->getVioPose(P0, R0);
cur_kf->getPose(P0, R0);
if(cur_kf->sequence > 0)
{
//printf("add loop into visual \n");
posegraph_visualization->add_loopedge(P0, connected_P + Vector3d(VISUALIZATION_SHIFT_X, VISUALIZATION_SHIFT_Y, 0));
}
}
}
//posegraph_visualization->add_pose(P + Vector3d(VISUALIZATION_SHIFT_X, VISUALIZATION_SHIFT_Y, 0), Q);
keyframelist.push_back(cur_kf);
publish();
m_keyframelist.unlock();
optimize4DoF
optimize_buf一有东西，意味着该帧已经被检测出回环了，因此就开始优化，优化的对象就是keyframelist中每个关键帧的四个自由度，包括x,y,z,yaw。同样是ceres问题求解
/**
* 构建图优化，优化位姿，（x,y,z,yaw）
*/
void PoseGraph::optimize4DoF()
{
while(true)
{
int cur_index = -1;
int first_looped_index = -1;
m_optimize_buf.lock();
while(!optimize_buf.empty())
{
cur_index = optimize_buf.front();
first_looped_index = earliest_loop_index;
optimize_buf.pop();
}
m_optimize_buf.unlock();
if (cur_index != -1)
{
printf("optimize pose graph \n");
TicToc tmp_t;
m_keyframelist.lock();
// 当前帧
KeyFrame* cur_kf = getKeyFrame(cur_index);
int max_length = cur_index + 1;
// w^t_i w^q_i
double t_array[max_length][3];
Quaterniond q_array[max_length];
double euler_array[max_length][3];
double sequence_array[max_length];
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(0.1);
//loss_function = new ceres::CauchyLoss(1.0);
ceres::LocalParameterization* angle_local_parameterization =
AngleLocalParameterization::Create();
list<KeyFrame*>::iterator it;
int i = 0;
// 遍历关键帧集合，从最早闭环帧到当前帧之间，构建优化图
for (it = keyframelist.begin(); it != keyframelist.end(); it++)
{
if ((*it)->index < first_looped_index)
continue;
(*it)->local_index = i;
Quaterniond tmp_q;
Matrix3d tmp_r;
Vector3d tmp_t;
(*it)->getVioPose(tmp_t, tmp_r);
tmp_q = tmp_r;
t_array[i][0] = tmp_t(0);
t_array[i][1] = tmp_t(1);
t_array[i][2] = tmp_t(2);
q_array[i] = tmp_q;
Vector3d euler_angle = Utility::R2ypr(tmp_q.toRotationMatrix());
euler_array[i][0] = euler_angle.x();
euler_array[i][1] = euler_angle.y();
euler_array[i][2] = euler_angle.z();
sequence_array[i] = (*it)->sequence;
problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization);
problem.AddParameterBlock(t_array[i], 3);
if ((*it)->index == first_looped_index || (*it)->sequence == 0)
{
problem.SetParameterBlockConstant(euler_array[i]);
problem.SetParameterBlockConstant(t_array[i]);
}
//add edge
// 添加边，每一帧与前面4帧建立边
for (int j = 1; j < 5; j++)
{
if (i - j >= 0 && sequence_array[i] == sequence_array[i-j])
{
Vector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());
Vector3d relative_t(t_array[i][0] - t_array[i-j][0], t_array[i][1] - t_array[i-j][1], t_array[i][2] - t_array[i-j][2]);
relative_t = q_array[i-j].inverse() * relative_t;
double relative_yaw = euler_array[i][0] - euler_array[i-j][0];
ceres::CostFunction* cost_function = FourDOFError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
relative_yaw, euler_conncected.y(), euler_conncected.z());
problem.AddResidualBlock(cost_function, NULL, euler_array[i-j],
t_array[i-j],
euler_array[i],
t_array[i]);
}
}
//add loop edge
// 添加闭环边，与闭环帧建立边
if((*it)->has_loop)
{
assert((*it)->loop_index >= first_looped_index);
int connected_index = getKeyFrame((*it)->loop_index)->local_index;
Vector3d euler_conncected = Utility::R2ypr(q_array[connected_index].toRotationMatrix());
Vector3d relative_t;
relative_t = (*it)->getLoopRelativeT();
double relative_yaw = (*it)->getLoopRelativeYaw();
ceres::CostFunction* cost_function = FourDOFWeightError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
relative_yaw, euler_conncected.y(), euler_conncected.z());
problem.AddResidualBlock(cost_function, loss_function, euler_array[connected_index],
t_array[connected_index],
euler_array[i],
t_array[i]);
}
if ((*it)->index == cur_index)
break;
i++;
}
m_keyframelist.unlock();
ceres::Solve(options, &problem, &summary);
//std::cout << summary.BriefReport() << "\n";
//printf("pose optimization time: %f \n", tmp_t.toc());
/*
for (int j = 0 ; j < i; j++)
{
printf("optimize i: %d p: %f, %f, %f\n", j, t_array[j][0], t_array[j][1], t_array[j][2] );
}
*/
m_keyframelist.lock();
i = 0;
// 更新图中顶点位姿，闭环帧到当前帧之间（不包括当前帧）
for (it = keyframelist.begin(); it != keyframelist.end(); it++)
{
if ((*it)->index < first_looped_index)
continue;
Quaterniond tmp_q;
tmp_q = Utility::ypr2R(Vector3d(euler_array[i][0], euler_array[i][1], euler_array[i][2]));
Vector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]);
Matrix3d tmp_r = tmp_q.toRotationMatrix();
(*it)-> updatePose(tmp_t, tmp_r);
if ((*it)->index == cur_index)
break;
i++;
}
Vector3d cur_t, vio_t;
Matrix3d cur_r, vio_r;
// 当前帧优化后位姿 todo
cur_kf->getPose(cur_t, cur_r);
// 当前帧优化前位姿
cur_kf->getVioPose(vio_t, vio_r);
m_drift.lock();
// 计算优化前后位姿差量
yaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(vio_r).x();
r_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0));
t_drift = cur_t - r_drift * vio_t;
m_drift.unlock();
//cout << "t_drift " << t_drift.transpose() << endl;
//cout << "r_drift " << Utility::R2ypr(r_drift).transpose() << endl;
//cout << "yaw drift " << yaw_drift << endl;
it++;
// 更新当前帧之后的关键帧位姿
for (; it != keyframelist.end(); it++)
{
Vector3d P;
Matrix3d R;
(*it)->getVioPose(P, R);
P = r_drift * P + t_drift;
R = r_drift * R;
(*it)->updatePose(P, R);
}
m_keyframelist.unlock();
updatePath();
}
std::chrono::milliseconds dura(2000);
std::this_thread::sleep_for(dura);
}
return;
}
6.全局融合globalOptNode.cpp
6.1 globalEstimator
VIO输出的 nav_msgs::Odometry 类型消息，这个定位信息包含了VIO的位置和姿态，其坐标系原点位于VIO的第一帧处。
GPS输出的sensor_msgs::NavSatFixConstPtr 类型消息，这个是全局定位信息，用经纬度来表示，其坐标原点位于该GPS坐标系下定义的0经度0纬度处。
// 订阅GPS
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);
6.1.1 回调函数
GPS回调函数，很简单，只是把GPS消息存储到了队列里面
/**
* 订阅GPS
*/
void GPS_callback(const sensor_msgs::NavSatFixConstPtr &GPS_msg)
{
//printf("gps_callback! \n");
m_buf.lock();
gpsQueue.push(GPS_msg);
m_buf.unlock();
}
VIO回调函数，请看注释：
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;
Eigen::Quaterniond vio_q;
......
/// 位姿传入global Estimator中
globalEstimator.inputOdom(t, vio_t, vio_q);
m_buf.lock();
// 寻找与VIO时间戳相对应的GPS消息
// 细心的读者可能会疑惑，这里需不需要对GPS和VIO进行硬件上的时间戳同步呢？
// 这个问题请看总结与讨论
while(!gpsQueue.empty())
{
// 获取最老的GPS数据和其时间
sensor_msgs::NavSatFixConstPtr GPS_msg = gpsQueue.front();
double gps_t = GPS_msg->header.stamp.toSec();
// 10ms sync tolerance
// +- 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;
//printf("receive covariance %lf \n", pos_accuracy);
/// GPS_msg->status.status 这个数字代表了GPS的状态(固定解，浮点解等)
/// 具体可以谷歌
// if(GPS_msg->status.status > 8)
// 向globalEstimator中输入GPS数据
globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);
gpsQueue.pop();
break;
}
else if(gps_t < t - 0.01)
gpsQueue.pop();
else if(gps_t > t + 0.01)
break;
}
m_buf.unlock();
......
// 广播轨迹（略）......
pub_global_odometry.publish(odometry);
pub_global_path.publish(*global_path);
publish_car_model(t, global_t, global_q);
// 位姿写入文本文件（略）......
}
可以看出，global Fusion的优化策略是收到一帧VIO数据，就寻找相应的GPS数据来进行优化。我们下面主要来看一下globalEstimator中的inputOdom()和inputGPS()这两个函数
首先看下 inputGPS():
void GlobalOptimization::inputGPS(double t, double latitude,
double longitude,
double altitude,
double posAccuracy)
{
double xyz[3];
// 因为经纬度表示的是地球上的坐标，而地球是一个球形，
// 需要首先把经纬度转化到平面坐标系上
// 值得一提的是，GPS2XYZ()并非把经纬度转化到世界坐标系下(以0经度，0纬度为原点)，
// 而是以第一帧GPS数据为坐标原点，这一点需要额外注意
GPS2XYZ(latitude, longitude, altitude, xyz);
// 存入经纬度计算出的平面坐标，存入GPSPositionMap中
vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy};
GPSPositionMap[t] = tmp;
newGPS = true;
}
再看inputOdom()：
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[t] = globalPose;
lastP = globalP;
lastQ = globalQ;
// 把最新的全局姿态插入轨迹当中(过程略)
......
global_path.poses.push_back(pose_stamped);
mPoseMap.unlock();
}
现在两种数据都收到以后，万事俱备，我们看一下 void GlobalOptimization::optimize()这个函数：
这个函数开了一个线程来做优化：
首先使用ceres构建最小二乘问题，这个没啥可说的
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();
状态量赋初值，添加参数块。可以看出来，迭代的初始值是globalPoseMap中的值，也就是VIO转换到GPS坐标系下的值
// 遍历历史里程计（转ENU系下了），显式添加变量参数
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;
然后添加残差：
for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end(); iterVIO++, i++) {
//vio factor
// 添加VIO残差，观测量是两帧VIO数据之差，是相对的。而下面的GPS是绝对的
iterVIONext = iterVIO;
iterVIONext++;
if (iterVIONext != localPoseMap.end()) {
/// 计算两帧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]);
}
// gps factor
// GPS残差，这个观测量直接就是GPS的测量数据，
// 残差计算的是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坐标系的变换。
7. 总结与讨论
1.思考
根据上文中分析的优化策略，global fusion的应用场景应该是GPS频率较低，VIO频率较高的系统。fusion 默认发布频率位10hz，而现在的GPS可以达到20hz，如果在这种系统上使用，你可能还需要修改下VIO或者GPS频率。
2.GPS与VIO时间不同步
上文提到了，在多传感器融合系统中，传感器往往需要做时钟同步，那么global Fusion需要么？GPS分为为很多种，我们常见的GPS模块精度较低，十几米甚至几十米的误差，这种情况下，同不同步没那么重要了，因为GPS方差太大。另外一种比较常见的是RTK-GPS ，在无遮挡的情况下，室外精度可以达到 2cm之内，输出频率可以达到20hz，这种情况下，不同步时间戳会对系统产生影响，如果VIO要和RTK做松耦合，这点还需要注意。
8.参考文档
浅谈VINS中的global fusion节点
VINS-FUSION代码超详细注释
VINS 论文推导及代码解析
图像回环检测loop_fusion主体
评论（0）
您还未登录，请登录后发表或查看评论