用CubeSLAM跑自己的数据集

针对CubeSLAM本博客内容如下，主要是阅读论文和代码的一些结果总结，还有一部分总结未完成，同样使用或者对语义slam感兴趣有经验的欢迎交流，该博客后面也会不段更新cubeslam在自己的数据集上的使用结果和一些避坑指南：
 1. cubeSLAM主要贡献
 2. 2D->3D的cube转换
 3. 使用自己的数据集
 
1. 主要贡献：
这篇文章在语义SLAM中好评较多，主要是与常见的ORBSLAM实现来物体级别的结合；
主要分为两部分，第一部分为2d部分，主要是可以通过平面的bbox生成出立体的cube，生成出得box带有9个参数，分别是三维的位置信息(tx,ty,tz)，三维的角度信息，以及三维的尺寸信息（dx,dy,dz）; 第二个贡献就是将3维cube与ORBSLAM的结合，将cube的位姿信息也添加到了相机位姿的优化中，因此，最终的BA方程将包括三个优化项，分别是：相机和物体之间，相机位姿和地图点，以及物体位姿与地图点之间的误差，而原ORBSLAM中只包含有第二项，及地图点在相机位姿下的重投影误差；

2. 2D->3D的cube转换
个人觉得这一部分很重要的原因是因为目前3d的物体检测相对于2d的物体检测来讲，准确度还是有待提高，尤其在SLAM中使用3d点云进行物体检测 与 单纯地使用2d图像进行检测所需要的软件和硬件都有差别，这次测试即为使用2d的boundingbox，传入到orb_object_slam中进行；
首先说一下检测大致的思路：输入为2维的bbox，文件使用格式为(left_up_point_x, left_up_point_y,width,height)的格式， 输出为3d的cube，也就是上面所说的9个参数，cubeobject的位姿+cube的维度；
具体过程：
a. cube的坐标系建立为每一个cube的中心，坐标的各个方向为向右为x，向前为y，向上为z，符合右手坐标系，但是与IMU坐标系相对来讲有一个90度的旋转；这点很重要，因为如果需要运行自己的数据机，就要知道最开始相机位姿与地面之间的关系，也就是后面会提到的Init_to_Ground: 有兴趣的可以直接参考我在git上提出的问题，有作者本人的回答：https://github.com/shichaoy/cube_slam/issues/32
b. 这篇文章使用来vanishing point来恢复cube的三维及信息；
第一步，vanishing point的定义：
我个人的理解，这些点通俗来讲就是物体投影到平面图像之后的各个边的交叉点；（按理说，一个空间中存在的立方体的两个长，两个宽边和两个高边都是彼此平行的，但是投影到平面之后，发生仿射变换，不再保平行，所以，这些边的延长线将会相交到一起）；
关于VP的定义具体可以参考《多视图几何》中的定义，而论文中直接采用来VP的计算为 VPi = KRcol(i); 也就是第i个VP的计算是其相对于相机的旋转矩阵的第i列以及相机的内参K决定的；
第二步，确定VPs后即可通过VPs来计算二维的角点，也就是立方体的各个角点的二维坐标，通过VPs和bbox的交点计算；
第三步，计算3Dbox，也就是当前cube的pose，这里有一个很重要的投影公式，即

该公式即为立方体的各个角点从object坐标系到camera坐标系的再到图像坐标系的一个投影，最终的重投影误差也是由这个公式计算；
这里举个例子，如果按照右侧为x正方向，前侧为y正方向，上方为z正方向，则object坐标系的原点位于该cube的中心，那么右前方上面的角点坐标即为(dx,dy,dz),其他点同理，这是在object coordinate下的坐标，经过旋转矩阵R和平移t即可转移到相机坐标系下，再经过投影公式pai即转换至这些角点再图像中的二维坐标；

第四步：定义cost function：
接下来说到如何去计算该object cube的参数O={R,t,d}; 这里列出cost function如下：

该代价函数主要由三部分组成：距离代价函数，角度代价函数，和形状约束；其中距离代价函数指的是上图中蓝色实线上的点到bbox对角线（也就是右3的那条黑线）之间的距离；角度代价函数即指上图left图像的较长线段与vp角度之间的关系；形状约束主要是指对长宽比过大的cube的惩罚；

这一部分的代码主要集中在detect_3d_cuboid部分，感兴趣的可以对detect_cuboid()函数进一步研究；

3. OBJECT SLAM
这里主要说一下静态物体的objectslam，动态的部分我还没有整理，有整理过的伙伴欢迎交流；SLAM部分主要分为两块，一是代价函数的介绍，二是代价函数详解；
第一部分：代价函数；
原本的ORBSLAM的代价函数为最小化地图点的重投影误差；而添加了物体之后的slam，其代价函数也对应地增加；具体如下：

其中第1和第3个为新增的误差部分；
第一部分物体在相机中的重投影误差；
这一部分可以分为两种情况：一种是使用rgbd相机时，object的位置信息比较精确，这时可以在精确值和测量值之间计算一个误差；二是只有图像信息，比如单目SLAM的情况；可以将object cube的各个顶点的坐标，由object frame 到camera frame经R,t 进行转换，再将相机坐标系下的顶点，经过相机的投影矩阵，即可得到对应的其在图像上的坐标，该坐标与实际的图像坐标之间的误差，即obejct的重投影误差，因此可以作为一个优化项；

第二部分为传统是地图点重投影误差，不再赘述；

第三部分是地图点与object cube之间存在的约束；
基于如下的原理：如果一个点在图像中的坐标位于bbox内部，那么该点也就存在与object cube的内部；一般情况下，orbslam保存的地图点都是在世界坐标系下，将这些地图点由世界坐标系转换至相机坐标系中，再转换至object坐标系中，那么该点在obejct坐标系中的坐标应该是小于各个方向的dimension；因此，这部分也可以构成一个约束项；

在自己的数据集上运行CubeSLAM:

运行之前:

1. 首先说一下cubeslam代码分几个模块,具体每个模块的作用我在刚开始的时候也是比较迷茫地,所以理解起整个项目会比较慢,具体如下:

2. 下面列举一些需要的参数, 可以在对应的launch文件中进行修改,也可以自己在ros_mono.cc中按照自己的需求改动:

1. 先单独运行边缘检测的部分,进行edge_detection, 可以运行单张图片,也可以写一个跑rosbag的,直接将包中的图像全部转换;

 2. filter_2d_obj_txts中存放通过yolo或者其他方法检测出的boundingbox, 文件名和文件格式如下:

 

这里面四个数字分别是boundingbox的左上角坐标x,y 以及boundingbox的宽和高;

当然,这些都可以在代码中灵活修改,具体对应的代码是orb_object_slam中的Tracking.cc中(可以通过搜索filter_2d_obj_txts)

3.  raw_imgs 中存放原图, 这里图片的名称需要和代码中的一致,不太建议修改代码,因为类似的访问太多,比较耗费精力

4. 运行orb_object_slam:

    ./devel/lib/orb_object_slam/ros_mono path_to_vocabulary/Vocabulary/ORBvoc.bin path_to_camera_matrix/Examples/Monocular/Example.yaml start_id

这里的测试中使用的是单目slam，一些参数具体的意义会在后面以表格的形式更新，这里的start_id也是为了方便测试在后期添加的；
在自己的数据集上跑的时候要注意Tracking.cpp中InitToGround的修改，具体参考https://github.com/shichaoy/cube_slam/issues/32；该参数决定来物体的scale，算是间接决定，当参数设置过大时会出现很大的cube，如图：

上图中图像的位置基本正确，只是size很大，同时也会导致整个slam的size出现差别；