VINS-Course代码解析——run_euroc前端数据处理
vins_mono总框架如下：
主要分为三大块：
我们先从主函数(main)入手：
主函数中有三个线程，读取完数据集和配置文件的路径后就会进入这三个线程，如下图：
thd_BackEnd线程：
thd_PubImuData线程：从MH_05_imu0.txt文件中获取imu信息，并检查imu数据是否乱序（根据时间戳），最后将imu装进队列imu_buf中并发布。
thd_PubImageData线程：从MH_05_cam0.txt文件中获取image信息，并检查image数据是否乱序，再对图像进行特征点的处理(提取，LK光流法跟踪）后，将image数据装进队列feature_buf中并发布。
根据上面陈述，我们现在大概明白了这三个线程在做什么事情。但在开始介绍这三个线程前，我们还有一些重要的事情需要提前介绍清楚。读入的imu和image是什么样的？存放在队列里的imu和image元素有哪些？
- 读入的imu数据：截取了7行MH_05_imu0.txt中的imu数据，红框中的数据是时间戳（获取imu的时刻），蓝框是陀螺仪的角速度，绿框是加速度计的加速度，都是测量值含噪声的。
- 读入的image数据：红框是时间戳，篮框数据是每一帧观测到的图像名称。
- imu_buf数据：队列中每一个元素类型是IMU_MSG。
shared_ptr<IMU_MSG> imu_msg(new IMU_MSG()); // imu数据指针
imu_msg->header = dStampSec; // 分别赋值 时间戳
imu_msg->linear_acceleration = vAcc; // 加速度
imu_msg->angular_velocity = vGyr; // 角速度
- feature_buf数据：特征点id，矫正后归一化平面的3D点(x,y,z=1)，像素2D点(u,v)，像素的速度(vx,vy)，封装成sensor_msgs::PointCloudPtr类型
介绍完这些细节，让我们开始介绍这三个线程。
1. thd_PubImuData线程：
我们根据线程调用的函数知道其调用的是PubImuData函数
std::thread thd_PubImuData(PubImuData); // 获取IMU数据的线程
读取imu数据文件中的IMU数据，并把数据打包成(时间dStampNSec，角速度vGyr，加速度vAcc)的形式调用系统函数PubImuData进行处理：
void PubImuData()
{
string sImu_data_file = sConfig_path + "MH_05_imu0.txt";
cout << "1 PubImuData start sImu_data_filea: " << sImu_data_file << endl;
ifstream fsImu;
fsImu.open(sImu_data_file.c_str());
if (!fsImu.is_open())
{
cerr << "Failed to open imu file! " << sImu_data_file << endl;
return;
}
std::string sImu_line;
double dStampNSec = 0.0; //时间戳
Vector3d vAcc; //加速度
Vector3d vGyr; //角速度
while (std::getline(fsImu, sImu_line) && !sImu_line.empty()) // read imu data
{
std::istringstream ssImuData(sImu_line);
ssImuData >> dStampNSec >> vGyr.x() >> vGyr.y() >> vGyr.z() >> vAcc.x() >> vAcc.y() >> vAcc.z(); //打包数据
// cout << "Imu t: " << fixed << dStampNSec << " gyr: " << vGyr.transpose() << " acc: " << vAcc.transpose() << endl;
pSystem->PubImuData(dStampNSec / 1e9, vGyr, vAcc);
usleep(5000*nDelayTimes); //imu获取数据频率
}
fsImu.close();
}
系统的PubImuData函数
pSystem->PubImuData(dStampNSec / 1e9, vGyr, vAcc);
对从文件读取出来的IMU数据进行检测，检测imu数据是否乱序，装进去imu_buf中，并唤醒条件变量去获取数据
详细代码如下：
void System::PubImuData(double dStampSec, const Eigen::Vector3d &vGyr,
const Eigen::Vector3d &vAcc)
{
// 将IMU数据读入，并放进自定义的类IMU_MSG中(IMU预处理)
shared_ptr<IMU_MSG> imu_msg(new IMU_MSG()); // imu数据指针
imu_msg->header = dStampSec; // 分别赋值 时间戳
imu_msg->linear_acceleration = vAcc; // 加速度
imu_msg->angular_velocity = vGyr; // 角速度
//last_imu_t是上一次的imu数据时间
if (dStampSec <= last_imu_t)
{
// 检测imu数据是否乱序
cerr << "imu message in disorder!" << endl;
return;
}
last_imu_t = dStampSec;
m_buf.lock();
imu_buf.push(imu_msg); //！把imu数据放入队列中，最后根据时间戳找对应的imu数据
m_buf.unlock();
con.notify_one(); // 唤醒
}
2.thd_PubImageData线程
std::thread thd_PubImageData(PubImageData); //获取图像数据的线程
先从MH_05_cam0.txt文件读取数据，打包成（时间dStampNSec，图像sImgFileName）。再调用系统的PubImageData函数进行图像数据的处理。
void PubImageData()
{
// 获取图像数据
string sImage_file = sConfig_path + "MH_05_cam0.txt";
cout << "1 PubImageData start sImage_file: " << sImage_file << endl;
ifstream fsImage;
fsImage.open(sImage_file.c_str());
if (!fsImage.is_open())
{
cerr << "Failed to open image file! " << sImage_file << endl;
return;
}
std::string sImage_line;
double dStampNSec; //时间戳
string sImgFileName; //图像名
while (std::getline(fsImage, sImage_line) && !sImage_line.empty())
{
std::istringstream ssImuData(sImage_line);
ssImuData >> dStampNSec >> sImgFileName;
string imagePath = sData_path + "cam0/data/" + sImgFileName;
Mat img = imread(imagePath.c_str(), 0);
if (img.empty())
{
cerr << "image is empty! path: " << imagePath << endl;
return;
}
pSystem->PubImageData(dStampNSec / 1e9, img); //进行特征点的处理(提取，LK光流法跟踪)，并保存
usleep(50000*nDelayTimes);
}
fsImage.close();
}
系统的PubImageData函数
2.1 跳过前两帧图像
void System::PubImageData(double dStampSec, Mat &img) // 时间戳&图像
{
// 预处理
// init_feature的初始值设置的是0
if (!init_feature) //第一帧图像数据不包含光流信息
{
cout << "1 PubImageData skip the first detected feature, which doesn't contain optical flow speed" << endl;
init_feature = 1;
return; // 跳过第一帧
}
// first_image_flag的初始值设置的是true
if (first_image_flag) // 第二帧图像
{
cout << "2 PubImageData first_image_flag" << endl;
first_image_flag = false;
first_image_time = dStampSec;
last_image_time = dStampSec;
return; // 跳过第二帧
}
2.2检测相机数据是否乱序，乱序则重置：
if (dStampSec - last_image_time > 1.0 || dStampSec < last_image_time)
{
cerr << "3 PubImageData image discontinue! reset the feature tracker!" << endl;
first_image_flag = true;
last_image_time = 0; // 从初始时刻开始，重置
pub_count = 1;
return;
}
2.3控制发布的频率在FREQ (config文件中已设置)
发布频率控制（不是每来一张图像都要发布，但是都要传入readImage()进行处理），保证每秒钟处理的图像不超过FREQ，此处为每秒10帧.
此时发布图像的数量/（当前图像时间戳 - 第一帧图像时间戳）<= 发布频率
last_image_time = dStampSec;
// frequency control
if (round(1.0 * pub_count / (dStampSec - first_image_time)) <= FREQ)
{
// 控制频率在FREQ
PUB_THIS_FRAME = true; // 发布flag
// reset the frequency control
// 时间间隔内的发布频率十分接近设定频率时，更新时间间隔起始时刻，并将数据发布次数置0
if (abs(1.0 * pub_count / (dStampSec - first_image_time) - FREQ) < 0.01 * FREQ)
{
first_image_time = dStampSec;
pub_count = 0;
}
}
else
{
PUB_THIS_FRAME = false; // 不发布flag
}
2.4 readImage()函数
经过上述四步以后就进入了图像处理的最关键的部分，进入readImage函数：
trackerData[0].readImage(img, dStampSec);
在feature_tracker.cpp中，这个函数最为重要，主要是进行特征点处理，并保存。
- cur_img ： 上一帧图像
- forw_img ： 当前帧图像
- cur_pts ： 上一帧的点坐标
- forw_pts ： 当前帧的点坐标
- ids ： 每个点的id号
- track_cnt ： 每个点被追踪的次数
- cur_un_pts ： 最新帧的归一化坐标
readImage流程及函数介绍：
- createCLAHE：自适应局部直方图均衡化，为了增强图像对比度更加有利于获取图像纹理信息。
//直方图均衡化
if (EQUALIZE)
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
TicToc t_c;
clahe->apply(_img, img);
}
else
img = _img;
- calcOpticalFlowPyrLK：光流追踪，得到前一帧图像在当前帧的光流信息。
if (cur_pts.size() > 0)
{
TicToc t_o;
vector<uchar> status;
vector<float> err;
// LK光流跟踪
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3); // LK光流跟踪
//根据状态信息更新失败的点以及在边缘处的点
for (int i = 0; i < int(forw_pts.size()); i++)
if (status[i] && !inBorder(forw_pts[i]))
status[i] = 0;
//根据跟踪的状态进行去除失败的信息
reduceVector(prev_pts, status);
reduceVector(cur_pts, status); // 二维坐标
reduceVector(forw_pts, status);
reduceVector(ids, status);
reduceVector(cur_un_pts, status);
reduceVector(track_cnt, status);
//ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
}
- rejectWithF：基础矩阵剔除异常点。
rejectWithF();
- setMask：不在已有点附近提取新的特征点。
setMask();
- goodFeaturesToTrack：提取shi-Tomas角点。
int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());
if (n_max_cnt > 0)
{
if(mask.empty())
cout << "mask is empty " << endl;
if (mask.type() != CV_8UC1)
cout << "mask type wrong " << endl;
if (mask.size() != forw_img.size())
cout << "wrong size " << endl;
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
}
else
n_pts.clear();
当前帧 forw 的数据赋给上一帧 cur
prev_img = cur_img;
prev_pts = cur_pts;
prev_un_pts = cur_un_pts;
cur_img = forw_img;
cur_pts = forw_pts;
调用undistortedPoints() 函数根据不同的相机模型进行去畸变矫正和深度归一化，计算速度
undistortedPoints(); // ？坐标——>归一化平面的2d图像坐标
prev_time = cur_time;
然后回到PubImageData函数，接着往下看。
2.5 更新全局ID，将新提取的特征点赋予全局id
for (unsigned int i = 0;; i++)
{ bool completed = false;
// 这里，成功跟踪才会使completed=true
completed |= trackerData[0].updateID(i);
if (!completed)
break;
}
图解：（绿色是跟踪成功的特征点，其余的是新添加的，球内数字是id，球上数字是跟踪次数）
2.6 将readImage()求解出的值封装，并发布
封装的值都包括：特征点id，矫正后归一化平面的3D点(x,y,z=1)，像素2D点(u,v)，像素的速度(vx,vy)，封装成sensor_msgs::PointCloudPtr类型的feature_points实例中，将预处理的结果保存在feature_buf中。
if (PUB_THIS_FRAME)
{
pub_count++;
shared_ptr<IMG_MSG> feature_points(new IMG_MSG());
feature_points->header = dStampSec;
vector<set<int>> hash_ids(NUM_OF_CAM);
for (int i = 0; i < NUM_OF_CAM; i++)
{
auto &un_pts = trackerData[i].cur_un_pts; // 归一化坐标
auto &cur_pts = trackerData[i].cur_pts; //保存，当前帧的特征点二维图像坐标
auto &ids = trackerData[i].ids;
auto &pts_velocity = trackerData[i].pts_velocity; // 速度
for (unsigned int j = 0; j < ids.size(); j++)
{
if (trackerData[i].track_cnt[j] > 1)
{
int p_id = ids[j];
hash_ids[i].insert(p_id);
double x = un_pts[j].x; //归一化坐标
double y = un_pts[j].y;
double z = 1;
feature_points->points.push_back(Vector3d(x, y, z)); // 归一化坐标
feature_points->id_of_point.push_back(p_id * NUM_OF_CAM + i);
feature_points->u_of_point.push_back(cur_pts[j].x); //像素坐标
feature_points->v_of_point.push_back(cur_pts[j].y);
feature_points->velocity_x_of_point.push_back(pts_velocity[j].x);
feature_points->velocity_y_of_point.push_back(pts_velocity[j].y);
}
}
// skip the first image; since no optical speed on frist image
if (!init_pub)
{
cout << "4 PubImage init_pub skip the first image!" << endl;
init_pub = 1;
}
else
{
m_buf.lock();
feature_buf.push(feature_points); // 预处理的结果
// cout << "5 PubImage t : " << fixed << feature_points->header
// << " feature_buf size: " << feature_buf.size() << endl;
m_buf.unlock();
con.notify_one();
}
}
}
至此，获取图像数据的线程全部解析完成！
对于thd_BackEnd线程，将换一篇文章继续介绍…