写在前面
上一节介绍了vins_estimator这个node的main( )函数和几个回调函数,现在接下来继续介绍process( )函数吧。
一、process( )主线程
void process()
{
while (true)
{
//[1]进入循环构建measurements复合数据类型,将imu和图像数据组合起来
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
std::unique_lock<std::mutex> lk(m_buf);//锁定m_buf
//当 getMeasurements()).size() = 0时wait() 阻塞当前线程,并且在收到imucallback,imagecallback的通知后getMeasurements()).size() != 0即有数据时时才会被解除阻塞
con.wait(lk, [&]
{
return (measurements = getMeasurements()).size() != 0;
});
// lambda函数的语法定义:(采用了追踪返回类型的方式声明其返回值)
// [capture](parameters) mutable -> return-type{statement;}
// [],捕捉列表,捕捉上下文中的变量以供lambda函数使用
/*[] 表示 空捕捉
*[=] 表示值 传递方式捕捉所有父作用域的变量(包括this)
*[&] 表示引用传递方式捕捉所有父作用域的变量(包括this)
*[var] 表示值 传递方式捕捉变量var
*[&var]表示引用传递方式捕捉变量var
*[this]表示值 传递方式捕捉当前this指针 (this。函数体内可以使用Lambda所在类中的成员变量。)
*其它组合形式:
* */
lk.unlock();
//[2]遍历measurements,其实就是遍历获取每一个img_msg和其对应的imu_msg对数据进行处理
m_estimator.lock();//上锁
for (auto &measurement : measurements)
{
auto img_msg = measurement.second;
double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
//[3]遍历和当前img_msg时间上“对齐”的IMU数据,处理imu数据processIMU()
for (auto &imu_msg : measurement.first)
{
double t = imu_msg->header.stamp.toSec();
double img_t = img_msg->header.stamp.toSec() + estimator.td;
//由于一帧image图像消息对应多个IMU消息,所以针对每一个IMU消息的时间戳,
// 需要判断IMU的时间戳和image消息的时间戳之间的大小关系,也就是需要判断IMU消息早于image消息先收到,
// 还是IMU消息晚于image消息收到。
if (t <= img_t)//imu的数据比图像数据早到
{
//第一次的时候current_time值为-1
if (current_time < 0)
current_time = t;//imu的采样时间
double dt = t - current_time;
ROS_ASSERT(dt >= 0);
current_time = t;
//dx,dy,dz分别是IMU在三个轴方向上的线加速度
dx = imu_msg->linear_acceleration.x;