VINS System::ProcessBackEnd()

vins 通过IMU和Image的两个线程处理，将imu数据存储到imu_buf中，image数据存储到image_buf中，接下来通过void System::ProcessBackEnd()函数来对数据进行处理，从而获得最终的里程计估计。接下来对这一函数进行介绍

第一段是对imu和image的两个buff的读取，首先是构造了一个pair，第一项是IMU的ector，第二项是单独的Img信息，因为两个图像之间会有多个imu信息，因此前一个是vector，后面是单独的图像

    cout << "1 ProcessBackEnd start" << endl;while (bStart_backend){// cout << "1 process()" << endl;vector<pair<vector<ImuConstPtr>, ImgConstPtr>> measurements;unique_lock<mutex> lk(m_buf);con.wait(lk, [&] {return (measurements = getMeasurements()).size() != 0; //获取measurement函数});if( measurements.size() > 1){cout << "1 getMeasurements size: " << measurements.size() << " imu sizes: " << measurements[0].first.size()<< " feature_buf size: " <<  feature_buf.size()<< " imu_buf size: " << imu_buf.size() << endl;}lk.unlock();m_estimator.lock();

对于get::Measurements()函数用于将之前存储的imu和image信息整理成为measurement，对这一函数进行单独解释，其实就是从image的buffer中获取一帧图像以及时间，然后把这个图像pop掉，然后从imu中获取时间小于这一帧图像的所有measurement，并且将其pop出来，然后将IMU和image集合在一起，组成pair

vector<pair<vector<ImuConstPtr>, ImgConstPtr>> System::getMeasurements()
{vector<pair<vector<ImuConstPtr>, ImgConstPtr>> measurements;while (true){if (imu_buf.empty() || feature_buf.empty()){// cerr << "1 imu_buf.empty() || feature_buf.empty()" << endl;return measurements;}if (!(imu_buf.back()->header > feature_buf.front()->header + estimator.td)){cerr << "wait for imu, only should happen at the beginning sum_of_wait: " << sum_of_wait << endl;sum_of_wait++;return measurements;}if (!(imu_buf.front()->header < feature_buf.front()->header + estimator.td)){cerr << "throw img, only should happen at the beginning" << endl;feature_buf.pop();continue;}ImgConstPtr img_msg = feature_buf.front();feature_buf.pop();vector<ImuConstPtr> IMUs;while (imu_buf.front()->header < img_msg->header + estimator.td){IMUs.emplace_back(imu_buf.front());imu_buf.pop();}// cout << "1 getMeasurements IMUs size: " << IMUs.size() << endl;IMUs.emplace_back(imu_buf.front());if (IMUs.empty()){cerr << "no imu between two image" << endl;}// cout << "1 getMeasurements img t: " << fixed << img_msg->header//     << " imu begin: "<< IMUs.front()->header //     << " end: " << IMUs.back()->header//     << endl;measurements.emplace_back(IMUs, img_msg);}return measurements;
}

因为只有一个摄像头，所以mearurements的size是1。首先是从measurement中获取图像的数据，然后从imu中获取小于这个t的所有数据，所以就存在一个问题，那就是我们之前组成pair的时候不就是imu的时间小于image的时间吗，这里是因为考虑了的image的时间与imu的时间可能不对齐，因此需要考虑偏差的纠正estimator.td, 也就是当这个时间小于0的时候就会出现这个问题，如果这个是0，就不会有后面的那一个else

            auto img_msg = measurement.second;double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;for (auto &imu_msg : measurement.first){double t = imu_msg->header;double img_t = img_msg->header + estimator.td;if (t <= img_t){if (current_time < 0)current_time = t;double dt = t - current_time;assert(dt >= 0);current_time = t;dx = imu_msg->linear_acceleration.x();dy = imu_msg->linear_acceleration.y();dz = imu_msg->linear_acceleration.z();rx = imu_msg->angular_velocity.x();ry = imu_msg->angular_velocity.y();rz = imu_msg->angular_velocity.z();estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));// printf("1 BackEnd imu: dt:%f a: %f %f %f w: %f %f %f\n",dt, dx, dy, dz, rx, ry, rz);}

对于提取出来的imu的数据加速度dx, dy, dz 以及角速度rx, ry, rz,此后estimator会通过processIMU函数来对这一帧的IMU数据进行处理，这个函数比较复杂，另外起一篇进行描述。

接下来是对图像数据的处理，对一幅图像，处理它的所有的点，因为我们是当目相机，所以camera_id的值都是0，pair的first是feature_id, pair的第二项是一个7x1的矩阵，第一个是camera_id，都为0，后面分别是归一化平面中的点x,y,z，像素坐标值，p_u,p_v和速度v_x, v_y，以及像素的速度velocity_x, velocity_y，将信息存储到image中。

接下来estimator会对image进行处理。现在的image是一个map，key是point的id，map到的是这个id的点的信息，归一化坐标、像素坐标、速度。

            map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;for (unsigned int i = 0; i < img_msg->points.size(); i++) {int v = img_msg->id_of_point[i] + 0.5;int feature_id = v / NUM_OF_CAM;int camera_id = v % NUM_OF_CAM;double x = img_msg->points[i].x();double y = img_msg->points[i].y();double z = img_msg->points[i].z();double p_u = img_msg->u_of_point[i];double p_v = img_msg->v_of_point[i];double velocity_x = img_msg->velocity_x_of_point[i];double velocity_y = img_msg->velocity_y_of_point[i];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;image[feature_id].emplace_back(camera_id, xyz_uv_velocity);}TicToc t_processImage;estimator.processImage(image, img_msg->header);

VINS System::ProcessBackEnd()相关推荐

VINS简化版本梳理
VINS-Course是深蓝学院VIO课程中开源的一个vins-mono的简化版本,但vins系统本身较为复杂,因此对该套代码进行简单梳理,用作记录.本文章主要解释其euroc数据示例,也就是run_ ...
【VINS-Mono】
run_euro.cpp 在run_euroc.cpp函数中开了四个线程,分别对应四个函数,处理线程,对应 System::ProcessBackEnd,用作对imu数据以及image数据的后台处理优 ...
SLAM/VIO/VINS AR/VR
https://zhuanlan.zhihu.com/p/34995102 一.前言这篇文章作为我的硕士期间学习总结,将从导航定位层面介绍SLAM技术,并给初学者一些学习建议.不会涉及非常深的理论方 ...
px4+vins+ego单机鲁棒飞行一（px4+mavros篇）
px4+vins+ego单机鲁棒飞行一(px4+mavros篇) 一.mavors安装二.mavros中的重要插件Plugins 三.mavros中的重要topic 四.mavros中的tf树五. ...
VINS on Wheels
摘要在本文中,我们提出了一个视觉辅助惯性导航系统(VINS),用于定位轮式机器人. 特别是,我们证明当VINS部署在地面车辆上(该地面车辆被限制为沿着直线或圆弧移动)时,具有额外的不可观察方向(un ...
SoC（System on chip）与NoC（network-on-chip)
SoC(System on chip)与NoC(network-on-chip) NoC是相对于SoC的新一代片上互连技术,要深入了解NoC必须深刻认识SoC,故本文组织结构为:  SoC架构  ...
如何使用Nsight System？
如何使用Nsight System?
SOC，System on-a-Chip技术初步
SOC,System on-a-Chip技术初步 S O C(拼作S-O-C)是一种集成电路,它包含了电子系统在单个芯片上所需的所有电路和组件.它可以与传统的计算机系统形成对比,后者由许多不同的组件组 ...
System.err: java.lang.UnsatisfiedLinkError: dlopen failed: library “libc++_shared.so“ not found
Android Studio 配置OpenCV 的时候出现这样的提示黄色警告libc++_shared.so" not found : System.err: java.lang.Unsa ...

VINS System::ProcessBackEnd()

VINS System::ProcessBackEnd()相关推荐

最新文章

热门文章