sync_process()
对两个imgBuf里的图像进行双目时间匹配(通过判断双目图像时间之差 <3ms),扔掉匹配不到的老帧。
double time0 = img0_buf.front()->header.stamp.toSec();
double time1 = img1_buf.front()->header.stamp.toSec();
if(time0 < time1 - 0.003)
{
img0_buf.pop();
}
else if(time0 > time1 + 0.003)
{
img1_buf.pop();
}
不断丢弃老帧,直到image0
和image1
能匹配的上。然后将双目图像,时间戳输入到估计器中,然后进行光流法跟踪等一系列后续计算。inputImage()
详见下面。
estimator.inputImage(time, image0, image1);
sync_process()
到此结束
Estimator::inputImage()
首先按照如下格式建立图像特征帧,存储每一帧的所有特征点的信息(camera_id xyz_uv_velocity)
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
然后帧间光流法跟踪、检测特征点、左右目光流法匹配、计算像素速度、画图等,详见trackImage
featureFrame = featureTracker.trackImage(t, _img, _img1)
如果要画图,用featureTracker.getTrackImage()
得到trackImage()
中处理的imTrack
(左右目的拼接图、画上特征点、帧间变化轨迹),然后发布话题。
将featureFrame,和时间戳组成pair,放到featureBuf中(里面说过):
if(inputImageCnt % 2 == 0)
{
mBuf.lock();
featureBuf.push(make_pair(t, featureFrame));
mBuf.unlock();
}
最后执行imu预积分等后续所有操作,详见processMeasurements:
processMeasurements()
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)