CubeSLAM学习

Cube SLAM

此代码包含两种模式：

1. 与ORB SLAM集成的对象SLAM。 请参见orb_object_slam具有ros bag输入的在线SLAM。 读取离线检测到的3D对象
2. 仅适用于多维数据集SLAM的基本实现。 参见object_slam。给定RGB和2D对象检测，该算法从每个帧中检测3D长方体，然后制定对象SLAM以优化相机姿势和长方体姿势。 是主包装。 detect_3d_cuboid是单图像立方体检测的C ++版本，对应于matlab版本。

作者: Shichao Yang

论文:

• CubeSLAM: Monocular 3D Object SLAM, IEEE Transactions on Robotics 2019, S. Yang, S. Scherer PDF

0.安装

环境要求

ROS indigo/kinetic, Ubuntu 14.04/16.04, Opencv 2/3.

ROS melodic 我也测试成功

mkdir -p ~/cubeslam_ws/src
cd ~/cubeslam_ws/src
catkin_init_workspace
git clone git@github.com:shichaoy/cube_slam.git
cd cube_slam

编译 g2o

sh install_dependenices.sh

编译整个工程

cd ~/cubeslam_ws
catkin_make -j4

1.运行

source devel/setup.bash
roslaunch object_slam object_slam_example.launch

You will see results in Rviz. Default rviz file is for ros indigo. A kinetic version is also provided.

To run orb-object SLAM in folder orb_object_slam, download data. See correct path in mono.launch, then run following in two terminal:

roslaunch orb_object_slam mono.launch
rosbag play mono.bag --clock -r 0.5

数据集用的kitii单目

https://drive.google.com/open?id=1FrBdmYxrrM6XeBe_vIXCuBTfZeCMgApL

launch文件的目录做了修改

运行：

roslaunch object_slam object_slam_example.launch

roslaunch orb_object_slam mono.launch
rosbag play mono.bag --clock -r 0.5

结论：

对车辆等物体创建3D的框架，自由度为9个：xyz，rpw，长宽高

对比原始的ORB-SLAM2，初始化效果非常好，原始ORB-SLAM2在该数据集下无法初始化成功

待测试：单目摄像头

3.细节

它发表在 2019年 最近一期的 IEEE Transactions on Robotics上，CubeSLAM用单目相机实现了物体级的建图、定位、和动态物体跟踪。

4.作者思路

传统基于特征点的SLAM基本已什么可做的，点之上的高维表达包括线、面、物体，这些先验暂时无法做到通用，但在室内的结构环境下有不少可以尝试的点。必然需要解决这些物体的观测模型、地图中的表达、在BA中的融合数学模型等等。
CubeSLAM里则提出用立方体来建模物体。它提出了整套观测误差函数，并放入ORBSLAM2的BA过程中统一优化。这里面提出几点很有价值的想法是:

1. 如何从单目利用2D bounding box， 辅助Vanish Points(消影点) 来恢复三维立方体结构
2. 如何在传统SLAM的静态假设上，将动态物体（车辆等）也追踪起来。实现相机位姿估计和运动物体位姿估计。

5.本文贡献

1. 一种高效，准确和鲁棒的单目3D立方体检测方法，无需CAD之类的先验对象模型；
2. 一种在相机，物体和点之间进行新颖测量的对象SLAM方法，可在包括KITTI基准在内的许多数据集上实现更好的姿态估计；
3. 一种在动态场景中利用运动物体改善姿态估计的方法。

6.论文核心

III. Single Image 3D Object Detection

A.3D box proposal generation

3Dbox用9个自由度描述，除了6维刚体变换外，还增加了3个dimension: 即长宽高。

由于一个前端矩形检测的四个端点只能提供4个约束，而一个3Dbox有8个点需要确定，因而需要其他的信息，如下图。

我们使用VP来改变和减少回归参数，而不去预测依赖的维度

VP点：Vanishing Point

VP是平行线的相交点。一个3D cuboid有三个正交轴，因此可以形成3个VPs，而且只与物体旋转有关!

R表示物体的旋转
V P i = K R c o l ( i ) ; i = 1 、 2 、 3 VPi = KRcol(i); i={1、2、3} VPi=KRcol(i);i=1、2、3

1. Get 2D corners from the VP

从VP点得到2Dcorners的8个点

立方体最多能观察到3面，所以情况分为以下三种情况，论文距离说明了怎样计算立方体的8个点

以图a为例，已知A、B、C、D点和VP1、VP2、VP3以及p1点，x表示两条线之间的相交点。
p 2 = ( V P 1 , p 1 ) × ( B , C ) p2 = (VP1,p1) × (B,C) p2=(VP1,p1)×(B,C)

p 3 = ( V P 1 , p 4 ) × ( V P 2 , p 2 ) p3 = (VP1,p4) × (VP2,p2) p3=(VP1,p4)×(VP2,p2)

p 5 = ( V P 3 , p 3 ) × ( C , D ) p5 = (VP3,p3) × (C,D) p5=(VP3,p3)×(C,D)

其余点可以通过类似的方法求出。

2. Get 3D box pose from 2D corners

得到了2D平面内的立方体顶点后，要估计其三维位姿。
将物体分成两类：1). 任意位姿的物体；2). 地面物体

1). 任意位姿的物体

使用PnP solver来求解通常的立体3D位姿和大小，由于单目尺度不确定性，我们需要确定一个尺度因子。数学上来说，立方体的8个3D顶点在

​ 物体坐标系：
[ ± d x ， ± d y ， ± d z ] / 2 [±dx，±dy，±dz]/2 [±dx，±dy，±dz]/2
​ 相机坐标系：
R [ ± d x ， ± d y ， ± d z ] / 2 + t R [±dx，±dy，±dz]/2+t R[±dx，±dy，±dz]/2+t

选择4个相连的顶点如1,2,4,7，然后从上述3D交点投影到2D中去，如顶点1有：
p 1 = π ( R [ d x ， d y ， d z ] / 2 + t ) ) p1 = π (R [dx，dy，dz]/2 + t)) p1=π(R[dx，dy，dz]/2+t))
π是相机的投影函数

因此每个点可以贡献两个约束，4个点即可估计9个自由度的位姿（除了尺度）。

2). 地面物体

地面物体的roll/pitch都是0. 可以大大简化上述计算

we can get eight 2D corners from VP.

可以直接反向投影地面的顶点到三维地平面上，然后计算其他垂直的顶点

比如上述在三维地平面上的顶点5，可以表达为 [n,m] ( 相机帧里的向量和距离 ), 相对应的三维顶点 P5是反向投影射线K^-1p5与地平面的交点:
P5 = −m/(n^T(K⁻¹p5)) * K⁻¹p5

3).Sample VP and Summary

the box estimation problem changes to how to get three VPs and one top 2D corner, because after we get the VP.

VP点由对象旋转矩阵R确定。尽管可以使用深度网络通过大量的数据训练直接预测它们，但出于通用性考虑，我们选择手动对其进行采样，然后对其进行评分（排序）

B.Proposal scoring

在图中，左图先提取了一些直线，右图分别是根据不同的直线生成的立体矩形假设，左上角是最佳的假设。每个不同假设可以得到不同的代价函数值。由下式计算：

立方物体O = {R，t，d} ，I是图像

Distance error

2D立体边界应该与真实图像的边界匹配。通过Canny边缘检测，并生成 DT变换，对于每个可见的立体边，均匀sample 10个点，并综合计算所有距离值，然后除以2d矩形的对角线长度。

Angle alignment error

距离误差与false positive的边很敏感，比如物体表面的纹理。因此，我们也检测长线条段（用LSD检测），然后测量它们的角度是否跟VP点匹配。这些直线首先与3个VP点之一关联，基于点-线支撑关系。对于每个VP，可以找到最远的两条线段，它们有最小和最大的倾角。

这里的含义应该是这两条线的交点应该是消影点VP.
那么关键在于为何点-线支撑关系能找到这样的两条最远线段呢. 参考文献给的

Varsha Hedau, Derek Hoiem, and David Forsyth. Thinking inside the
box: Using appearance models and context based on room geometry.
In European Conference on Computer Vision, pages 224–237. Springer,
2010.

Shape error

前两个的误差可以在2D图像平面内计算。但是，相似的2D立体端点可能生成完全不一样的3D立方体。我们添加一个误差来惩罚有巨大倾斜( 长宽比 ) 的比率。也可以添加更严格的先验，比如某些特殊种类物体的固定长宽值或比例。

IV. Object SLAM

我们延伸单图像三维物体检测到多物体SLAM，来联合优化物体位姿和相机位姿。系统基于ORBSLAM2搭建，包含前端相机追踪和后端BA。

我们主要的挑战是完善BA来联合物体、点和相机位姿，本部分将详细介绍。
其他SLAM应用细节可以见VI-B， 静态物体在本部分介绍，动态物体将在下一节强调。

A. Bundle Adjustment Formulation

分别代表相机-物体，相机-点，物体-点的约束

B. Measurement Errors

1) 相机-物体测量

a ) 3D测量:

第一种是在3D物体检测是准确的时候使用的3D测量，比如使用RGBD相机时。检测的物体位姿，作为物体在当前相机帧的测量。为了计算测量误差，我们转换路标物体到相机帧，然后比较测量误差：

其中 log 将 SE3的误差映射到 6DOF的切向量空间，因而eco₋3D∈R⁹ 。为了改善鲁棒性，我们使用 Huber 鲁棒核函数。

注意这里在没有先验时有歧义，无法分清前向和后向。因而在优化时要沿着高度方向旋转0 ±90 180度来获得最小误差。

b) 2D测量:

对于2d测量，我们将立方体路标投影到图像平面来获得2D的红色bounding box，然后与检测的蓝色box之间比较。简单来说，就是将8个顶点投影到平面内，寻找最小和最大的投影像素x,y坐标来建立一个矩形:

c和s分别是矩形框的中心和边长。定义两个矩形框之间的误差为:

即直接让4D参数相减。
这个测量比起上述的3D测量不确定性更小，因为一般2D检测更加准确。但不同的三维实体也可能有相同的2D投影。

2）物体-点测量

Points and objects can provide constraints for each other

若点P属于物体，则应该在3D物体内部。我们首先将点转换到立体坐标系，然后与立方体的大小比较来获得三维误差：

使用max是鼓励点在立方体内部而非正好在表面。

3） 相机-点测量

We use the standard 3D point re-projection error in feature-based SLAM

重投影误差

C. Data association

使用帧间的点来跟踪检测框。 考虑哪些检测框内离中心在10像素以内的点。哪些匹配点最多，则关联谁。
同时，对于匹配上的点过少的框( 使用对极几何搜索)，它们可以认为是动态物体而去除掉！！

7.评价

目前最先进的单目相机位姿估计，同时提升了3D目标检测的准确性