main()函数的最后一句，std::thread sync_thread{sync_process};
可以看出，只有time0在time1前后0.03s内的两幅图片，才会被发布estimator.inputImage(time, image0, image1);。
// extract images with same timestamp from two topics
void sync_process()
{
while(1)
{
if(STEREO) // STEREO 取值为1 表示双目，为0 表示单目
{
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 sync tolerance
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();
//printf("find img0 and img1\n");
}
}
m_buf.unlock();
if(!image0.empty())
estimator.inputImage(time, image0, image1);
}
std::chrono::milliseconds dura(2); // 2ms
std::this_thread::sleep_for(dura);
}
}
进入estimator类中查看inputImage()类成员函数：
int inputImageCnt;是一个类数据成员
对于双目调用featureFrame = featureTracker.trackImage(t, _img, _img1);，前面阅读的代码派上用场了！这一句话完成了解析上13页（四、前端视觉处理）的部分。
SHOW_TRACK变量的含义？如果为真，则发布消息pubTrackImage(imgTrack, t);
imgTrack为FeatureTracker类的数据成员。
queue<pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > > featureBuf; 为Estimator类的一个数据成员，是一个队列。
进入到Estimator类的一个函数成员void processMeasurements();
void Estimator::inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1)
{
inputImageCnt++;
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
TicToc featureTrackerTime;
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());
}
}
进入该函数会一直在此处循环，通过break方式退出的条件是
USE_IMU的含义？推测此处的取值为true
下面进入Estimator类的另一个成员函数，getIMUInterval()，bool Estimator::getIMUInterval(double t0, double t1, vector<pair<double, Eigen::Vector3d>> &accVector, vector<pair<double, Eigen::Vector3d>> &gyrVector)
该函数的目的很明确，将时间区间[t0,t1]之间的陀螺仪、加速度计输出的数据从accBuf和gyrBu取出存放在accVector和gyrVector两个vector中。
如果还未初始化，即initFirstPoseFlag为false，则首先进行初始化。
感觉这个地方使用了双矢量定姿（一个是加速度计输出的平均值，一个是重力矢量）。还有些细节的地方没有理解（例如yaw用来干什么？），暂时略过。
最后，初始姿态保存为了Rs[0]，是Estimator的一个类数据成员Matrix3d Rs[(WINDOW_SIZE + 1)];
接下来调用了processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);，完成了之前介绍过的解析4页（二、IMU预积分）的当前时刻及帧间PVQ的求取，还有PVQ增量的误差、协方差及Jacobian。
接下来调用了processImage(feature.second, feature.first);，有一种感觉，这里就是处理的核心了！去功能模块仔细阅读。
void Estimator::processMeasurements()
{
while (1)
{
//printf("process measurments\n");
pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > feature;
vector<pair<double, Eigen::Vector3d>> accVector, gyrVector;
if(!featureBuf.empty())
{
feature = featureBuf.front();
curTime = feature.first + td;
while(1)
{
if ((!USE_IMU || IMUAvailable(feature.first + td)))
break;
else
{
printf("wait for imu ... \n");
if (! MULTIPLE_THREAD)
return;
std::chrono::milliseconds dura(5);
std::this_thread::sleep_for(dura);
}
}
mBuf.lock();
if(USE_IMU)
getIMUInterval(prevTime, curTime, accVector, gyrVector);
featureBuf.pop();
mBuf.unlock();
if(USE_IMU)
{
if(!initFirstPoseFlag)
initFirstIMUPose(accVector);
for(size_t i = 0; i < accVector.size(); i++)
{
double dt;
if(i == 0)
dt = accVector[i].first - prevTime;
else if (i == accVector.size() - 1)
dt = curTime - accVector[i - 1].first;
else
dt = accVector[i].first - accVector[i - 1].first;
processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);
}
}
mProcess.lock();
processImage(feature.second, feature.first);
prevTime = curTime;
printStatistics(*this, 0);
std_msgs::Header header;
header.frame_id = "world";
header.stamp = ros::Time(feature.first);
pubOdometry(*this, header);
pubKeyPoses(*this, header);
pubCameraPose(*this, header);
pubPointCloud(*this, header);
pubKeyframe(*this);
pubTF(*this, header);
mProcess.unlock();
}
if (! MULTIPLE_THREAD)
break;
std::chrono::milliseconds dura(2);
std::this_thread::sleep_for(dura);
}
}
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
