两口气梳理完VDO-SLAM源码（一）

第一口气就先写这么多了，第二口气想整理一下全局优化的一些内容，不过估计得过一段时间了。这一篇记录纯属个人理解，如有不同，欢迎讨论~ 关于论文中使用光流进行动态物体追踪的部分，我目前还不太能确定理解的是否正确，有知道的小伙伴麻烦指点一下~

---

一. 主函数vdo_slam.cc

从源码可以看到一些ORBSLAM的痕迹，主函数中，最关键的的地方就是SLAM.TrackRGBD(imRGB,imD_f,imFlow,imSem,mTcw_gt,vObjPose_gt,tframe,imTraj,nImages);，从这里就可以进入整个系统内部。这里要理解两件事：

1. 这里的位姿真值矩阵怎么得到，有什么作用？
位姿真值的文件就是在数据集的官网找到的，有的因为坐标系不同可能需要转换一下。在源码里面，这个真值只是被用来算一下误差结果，不会影响到Tracking的过程等。具体内容可参考issue#21

2. 光流信息是什么样的，之后会被怎么使用？
光流信息是通过PWC-NET的pytorch版本得到的.flo文件。在源码里面，当前帧对应的光流信息，是当前帧和下一帧的匹配关系。

二. Tracking::GrabImageRGBD()

整体流程

从System::TrackRGBD()进入，就可以看到Tracking::GrabImageRGBD()，这里先把里面的任务都罗列一下，和ORBSLAM一样的就先不写了，其中Tracking::DynObjTracking()是重点函数，里面的细节放到下一大点记录：

• 预处理深度图：注意对于双目模式，这里深度图的每一个像素存储的是视差值。

• 更新语义mask信息Tracking::UpdateMask()：根据上一帧的语义分割图和光流的结果，可以给当前帧的语义分割图恢复一些语义标签的信息。

• 构造当前帧：这里和ORBSLAM也比较类似，差异主要体现在提取特征点的地方。背景和物体的特征点和相关信息会存入不同的变量中。

  第一步：这里先对属于背景的特征点进行处理，作者设置了两个选项:
             1）使用ORB特征提取特征点，
             2）或者把随机采样点作为特征点，
         之后还根据特征点位置存储对应的深度（视差）值。
  第二步：对灰度图进行采样，每隔三行三列取点，如果属于objrct上的点就存储下来，并保存好匹配点，深度，语义标签等信息
  

• 更新信息：把上一帧中由光流得到的背景&object匹配点作为当前帧背景&object特征点的坐标，并存储对应的深度信息等。关联位姿真值，这里是为了在每一帧都输出一个RPE。

• 进入追踪状态Tracking：：Track()，最终目的就是得到相机位姿和object的信息

  第一步：如有必要，进行初始化。注意这里的背景点和object特征点构造的3D点会存入不同的容器。
  第二步：先对相机位姿进行处理，利用的是前后帧的匹配信息
         1) 使用P3P+RANSAC方法和匀速运动模型分别求解相机初始位姿，最后哪种方法的内点数更多就取哪个的结果
         2) 在初始估计的基础上再进行位姿优化，这里有类似ORBSLAM中的优化，还有结合了光流的位姿优化这两种供选择
         3) 按照匀速运动模型，更新速度信息
         ( 4)根据传入的相机位姿gt值，来计算相对平移误差和相对旋转误差 )
  第三步：处理object
  			1) 对特征点计算稀疏的场景流 Tracking::GetSceneFlowObj()。场景流可以作为判断动态object的依据。
  			   当前帧的场景流=当前帧的特征点世界坐标-前一帧对应特征点世界坐标
  			2) 对物体进行追踪，即找到前后帧两个物体的对应关系 Tracking::DynObjTracking() 
  			3) 对物体进行运动估计。这里起其实就是先根据物体前后帧的3D-2D匹配求出一个T，再用上之前求的相机位姿，
  			   最终求得物体在世界坐标系下前后帧的变换T。
  第四步：更新变量Tracking::RenewFrameInfo()，类似操作分别对静态点和动态物体点进行
          1) 保存上一帧的关键点的内点，并用光流计算在下一帧中的位置，
          2) 对于每一个Obj，如果目前已追踪的特征点达不到固定值，就从当前帧提取的orb特征点中/采样点中才提取，直到足够数量
          3) 此步骤仅对于动态物体：更新新出现的物体，找到新出现的label，并把新物体上的关键点加入变量
          4) 给每一个关键点存储对应的深度值，在已知关键点像素坐标，深度，相机pose的情况下，求出当前帧关键点对应的世界坐标
  第五步：局部优化
  第六步：全局优化
  

• 可视化实现，如可视化特征点位置、object的bounding box和速度等

• 返回当前帧的最终位姿

变量说明

Frame类

一般来说（原作者在github上的说法），名字后面带Tmp的，是从当前帧通过ORB特征检测到的或采样到的关键点的延伸变量。名字相同但不带Tmp后缀的，是由上一帧关键点+上一帧光流信息追踪得来的关键点的延伸变量。但是要注意的是代码里面真的有很多tmp变量，以及不同时候还会互相交换…具体的只能自己看代码的时候才能扯清楚了

变量名	类型	含义
mvStatKeysTmp		当前帧通过ORB特征检测到的/采样到的关键点
mvStatKeys		当前帧中的关键点，但这些点是由上一帧的关键点通过上一帧的光流信息追踪得到的匹配点
mvCorres	std::vector<cv::KeyPoint>	已知当前帧中的关键点和光流信息，可以算出下一帧中的匹配点坐标
mvFlowNext	std::vector<cv::Point2f>	保存当前帧对应的.flo文件的光流信息
mvObjKeys		在构造帧的时候存储的是obj上隔点采样到的关键点，在GrabImageRGBD函数中，这个变量被赋给了Tracking类的变量，后来存储的是上一帧+光流追踪得到的当前帧关键点
vSpeed	vector<cv::Point_<float>>	存储obj的速度，存了一个估计值，一个计算出的速度真值

三. Tracking::Track()中与Object相关的部分

A.场景流计算Tracking::GetSceneFlowObj();

场景流的计算本身不难，就是当前帧特征点的3D世界坐标 - 上一帧该特征点的3D世界坐标的差值。

//UnprojectStereoObject函数都是直接对对象帧的mvObjKeys[i]进行处理
cv::Mat x3D_p = mLastFrame.UnprojectStereoObject(i,0);
cv::Mat x3D_c = mCurrentFrame.UnprojectStereoObject(i,0);

• x3D_p:上一帧图像Obj关键点

• x3D_c: 当前帧图像内，从上一帧Obj中由光流追踪到的，和上一帧采样点一一对应的特征点坐标。这里最重要的一行代码是进入Track()之前的cv::Mat Tracking::GrabImageRGBD()中的

  mCurrentFrame.mvObjKeys = mLastFrame.mvObjCorres;
  

B. 物体追踪Tracking::DynObjTracking()

这里是对之前语义mask中的那些先验动态物体进行追踪。流程如下：

• 按照标签值对关键点索引分类存储： 遍历所有object关键点的语义标签，记录第k个语义标签(比如有N个物体，1~n表示物体编号，0表示背景，-1表示外点)对应的所有关键点的index，存在变量Posi中。
  举例：Posi[2]中储存着所有语义标签2(标签数值从小到大排序第3)的所有关键点的id。

• 根据边界范围筛选Obj的关键点： 如果语义标签为i的object，有超过一半的关键点落在了图像的边界处(范围自行设置)，那么这个object所有的关键点就要被剔除掉。重要变量如下：最后使用变量, 用存储对应的合格种类的语义标签。

  变量名	类型	含义
  ObjId	vector<vector<int>>	存储每一个合格种类的obj的所有关键点index
  sem_posi	vector<int>	每一个合格种类Obj对应的具体语义标签值
  vObjLabel	vector<int>	被筛除掉的关键点会被视为外点，被标记为-1

  举例：筛选之后，合格的标签是1,3,5，那么ObjId[0]里存的就是对应标签值1的所有关键点索引，而sem_posi[0]的值为1。

• 根据计算的场景流筛选出动态Obj的关键点： 利用Tracking::GetSceneFlowObj()计算的场景流信息进行动态物体筛选。计算每一个object上采样点的场景流强度(配置文件中阈值设为0.12)，大于阈值的点认为是动态点，如果一个物体上的静态点超过总数的30%，认为该物体是静态背景，vObjLabel置为0。
  有个小地方，计算场景流时只用到了x和z的值，根据作者跑的数据集，感觉是因为y轴是垂直方向，对于在平地上移动的情况来说，y轴方向影响比较小，可以忽略。

  float sf_norm = std::sqrt(
      mCurrentFrame.vFlow_3d[ObjId[i][j]].x*mCurrentFrame.vFlow_3d[ObjId[i][j]].x + 
      mCurrentFrame.vFlow_3d[ObjId[i][j]].z*mCurrentFrame.vFlow_3d[ObjId[i][j]].z);
  

• 根据Obj上关键的平均深度和数量筛选： 如果一个obj上所有关键点的平均深度大于阈值，即离相机太远了，就被视为不可靠的外点； 或者如果object上的关键点个数太少(<150)也被视为外点。
  最终，通过筛选的关键点信息会被存进ObjIdNew和SemPosNew，这个里面只保留了之后要追踪的动态物体，这两个变量可以对应前面的ObjId和sem_posi理解。

• 更新动态Obj的追踪ID： 这里我个人的理解就是直接利用光流，把前后帧semanticMask上的两个标签值关联起来。论文中写的说可能还会有noise等情况，在代码中还进行了一个排序操作，但是通过数据集的输出时，变量Lb_last中都只有一种数值。这里还仍然留有一些疑惑，希望知道的小伙伴能够在评论区留言交流一下~
  按照我目前理解的这个方式，如果第n帧出现了一个新物体，那么就要到第n+1帧才会在图片中出现。

C. 物体运动估计

这一节代码还比较好理解。目的是求在世界坐标下，Obj在前后帧的变换情况，以及Obj的速度。
注意一点：如果给系统的相机pose真值是错误的数的话，那么输出窗口中Obj的速度真值也会错误。原因在github的说明里写的很清楚了，文件夹里有一个object_pose.txt文件，里面给的物体信息t1-t3，r1都是参考的相机坐标系的。跑程序的时候，会把这个值利用相机pose真值转到世界坐标系下计算出Obj在世界坐标系下真正的运动情况。
这里摘录两行最重要的地方：

// 对应论文公式24
sp_est_v = mCurrentFrame.vObjMod[i].rowRange(0,3).col(3) - (cv::Mat::eye(3,3,CV_32F)-mCurrentFrame.vObjMod[i].rowRange(0,3).colRange(0,3))*ObjCentre3D_pre;
// TODO 正常的m/s 换算成km/h 应该就是乘以3.6 这里是假设时间间隔是0.1s??
float sp_est_norm = std::sqrt( sp_est_v.at<float>(0)*sp_est_v.at<float>(0) + sp_est_v.at<float>(1)*sp_est_v.at<float>(1) + sp_est_v.at<float>(2)*sp_est_v.at<float>(2) )*36;

cout << "estimated and ground truth object speed: " << sp_est_norm << "km/h " << sp_gt_norm << "km/h " << endl;


// 对应公式23
cv::Mat H_p_c_body_est = L_w_p_inv*mCurrentFrame.vObjMod[i]*L_w_p;
cv::Mat RePoEr = Converter::toInvMatrix(H_p_c_body_est)*H_p_c_body;

---

※补充说明

在VDO-SLAM的源码中，有一些不影响主线的代码，看的时候有一些疑问，而作者也在github上做出了解答，所以在这里也记录下来。

1.计算相机RPE

在函数Tracking::Track()中，有一段作者自己用于debug的计算相机相对位姿误差的代码。

// ----------- compute camera pose error ----------

cv::Mat T_lc_inv = mCurrentFrame.mTcw*Converter::toInvMatrix(mLastFrame.mTcw);
cv::Mat T_lc_gt = mLastFrame.mTcw_gt*Converter::toInvMatrix(mCurrentFrame.mTcw_gt);
cv::Mat RePoEr_cam = T_lc_inv*T_lc_gt;

//相对平移误差的rmse
float t_rpe_cam = std::sqrt( RePoEr_cam.at<float>(0,3)*RePoEr_cam.at<float>(0,3) + RePoEr_cam.at<float>(1,3)*RePoEr_cam.at<float>(1,3) + RePoEr_cam.at<float>(2,3)*RePoEr_cam.at<float>(2,3) );
float trace_rpe_cam = 0;
for (int i = 0; i < 3; ++i)
{
    
    
    // 这里的计算是作者用于debug的，详情可以看https://github.com/halajun/VDO_SLAM/issues/17
    if (RePoEr_cam.at<float>(i,i)>1.0)
        //
        trace_rpe_cam = trace_rpe_cam + 1.0-(RePoEr_cam.at<float>(i,i)-1.0);
    else
        trace_rpe_cam = trace_rpe_cam + RePoEr_cam.at<float>(i,i);
}
cout << std::fixed << std::setprecision(6);
// 计算相对旋转误差的rmse
float r_rpe_cam = acos( (trace_rpe_cam -1.0)/2.0 )*180.0/3.1415926;

cout << "the relative pose error of estimated camera pose, " << "t: " << t_rpe_cam <<  " R: " << r_rpe_cam << endl;

这个里面对trace_rpe_cam的计算，看上去让人比较困惑，作者在github给出的解答大意如下：在极少数情况下RePoEr_cam可能不是一个正定矩阵(不过会很接近正定)，这时对角线元素会稍微比1大一点，但这样acos( (trace_rpe_cam -1.0)/2.0 )结果就是一个非实数了。为了避免这个轴角变成一个虚数，就这么写了。但是标准方法是线找到一个最接近RePoEr_cam的正交矩阵，再进行后面的运算。作者的建议是如果要评估VDO-SLAM的结果，最好是用别的评估工具。

扩展：如何计算一个矩阵的近似的正交矩阵？

http://people.csail.mit.edu/bkph/articles/Nearest_Orthonormal_Matrix.pdf

2.关于Oxford Multimotion数据集语义分割的结果

提问者疑惑 COCO 数据集中并没有 “cubes” 或者 "boxes"这样的语义标签，想知道作者是怎么得到mask的。
答： 对于这个不太复杂的数据集，作者用了传统视觉的方法， 结合box的颜色信息+大津算法+multi-label processing