VINS-Course代码解析——run_euroc前端数据处理

vins_mono总框架如下:
VINS-Course代码解析——run_euroc前端数据处理_第1张图片
主要分为三大块:
VINS-Course代码解析——run_euroc前端数据处理_第2张图片
我们先从主函数(main)入手:
主函数中有三个线程,读取完数据集和配置文件的路径后就会进入这三个线程,如下图:
VINS-Course代码解析——run_euroc前端数据处理_第3张图片
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元素有哪些?

  1. 读入的imu数据:截取了7行MH_05_imu0.txt中的imu数据,红框中的数据是时间戳(获取imu的时刻),蓝框是陀螺仪的角速度,绿框是加速度计的加速度,都是测量值含噪声的。
    在这里插入图片描述
  2. 读入的image数据:红框是时间戳,篮框数据是每一帧观测到的图像名称。
    VINS-Course代码解析——run_euroc前端数据处理_第4张图片
  3. 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;             // 角速度
  1. 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,球上数字是跟踪次数)
VINS-Course代码解析——run_euroc前端数据处理_第5张图片

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线程,将换一篇文章继续介绍…

你可能感兴趣的:(视觉SLAM)