cube-slam详解

原文链接
问题：使用深度学习的目标检测得到个框，怎么将这些框用上并集成到slam中做物体级别的slam?
卡耐基梅隆大学的Shichao Yang, Sebastian Scherer，在论文CubeSLAM: Monocular 3D Object SLAM中给出了答案。并且结合orbslam的代码已经开源。
地址：https://github.com/shichaoy/cube slam
１．总体贡献及思路
（１）由图像平面的框得到3d世界里的立方体。
（２）建立物体级别的整套观测误差函数，并放入ORBSLAM2的BA过程中统一优化

1.得到3d世界里立方体

步骤：（１）得到立方体的消失点（３个）
　　　（２）由图像上的一个点结合三个消失点得到剩下的７个点，并得到在图像上的点。
　　　（３）由2D平面内的立方体顶点，估计立方体的９自由度位姿
　　　 (4)　不同角度可能会估计出来不同的立方体，要打分选出最好的一个立方体，通过距离，角度，形状来打分
　
如何得到立方体的消失点？
如果只考虑平面的物体，可以直接得到三个消失点，具体步骤?

2.已知立方体参数化加入到BA优化中

（１）相机-物体
将立方体路标投影到图像平面来获得2D的bbox，然后与检测的bbox比较。
（2）物体-点的约束
Points and objects can provide constraints for each other(物体和点之间有关系，点在立方体内)
若点P属于物体，则应该在3D物体内部。首先将点转换到立体坐标系，然后与立方体的大小比较来获得三维误差
（3）相机-点
我们在基于特征的SLAM中使用标准3D点重投影误差

3.立方体残差和雅克比推导

（１）相机-物体

如图，残差就为两个框相减。具体参数为中心点之差和长和宽之差。
优化代码解析：
g2o用setMeasurement设置观测，用设置addVertex优化变量
用e->setMeasurement(local_object->bbox_vec);设置长宽，中心点的观测
用optimizer.addVertex(vObject);设置对象节点

边类型为typedef g2o::EdgeSE3CuboidFixScaleProj g2o_camera_obj_2d_edge;
因此，优化变量为立方体的参数，keyframe的姿态，观测为图像检测的2d框。
残差g2o代码实现g2o_Object中就如上公式
残差为立方体的８个点投影到图像平面上得到四个点，转为矩形后求与观测矩形的差

Vector4d rect_project = global_cube.projectOntoImageBbox(cam_pose_Tcw, Kalib); // center, width, height
_error = rect_project - _measurement;

代码没有重载::linearizeOplus()方法，是用g2o的自动求导

（2）物体-点的约束
首先观测为物体的３d点，为世界坐标系下的位置
优化变量为立方体（９维度），如下

SE3Quat pose;   // 6 dof for object, object to world by default
Vector3d scale; // [length, width, height]  half!

边为EdgePointCuboidOnlyObjectFixScale
残差为
e o p = m a x ( ∣ T o − 1 P − d m , 0 ∣ ) e_{op}=max(|T_{o}^{-1}P-d_m,0|) eop​=max(∣To−1​P−dm​,0∣)
其中 d m d_m dm​表示立方体的长宽高，
T o − 1 P T_{o}^{-1}P To−1​P表示将物体上的点转换到立方体中心坐标系，max操作符代表根据不同的距离的取不同的值。
如果在立方体内，差为０，太远设置为固定值，中等距离乘以系数
如代码

// if point is within the cube, error=0, otherwise penalty how far it is outside cubefor (int i = 0; i < 3; i++){if (local_pt(i) < this->scale(i))error(i) = 0;else if (local_pt(i) < (max_outside_margin_ratio + 1) * this->scale(i))error(i) = local_pt(i) - this->scale(i);elseerror(i) = max_outside_margin_ratio * this->scale(i); // if points two far, give a constant error, don't optimize.}

雅克比也是自动求导。
（3）相机-点
这个使用的orb默认的残差。

注意残差这边有疑问的点有：都是用的自动求导，没有手动求导。立方体的８个点投影到图像平面上的实现还没有完全弄清楚。

４消失点（vanishing point）概念及获取

One of the distinguishing features of perspective projection is that the image of an
object that stretches off to infinity can have finite extent. For example, an infinite scene
line is imaged as a line terminating in a vanishing point. Similarly, parallel world lines,
such as railway lines, are imaged as converging lines, and their image intersection is
the vanishing point for the direction of the railway
透视投影的一个显著特征是，一个物体延伸到无限远处的图像可以有有限的范围。例如，一条无限的风景线被成像为一条终止于消失点的线。同样地，平行的世界线，如铁路线，被成像为会聚线，它们的图像交点是铁路方向的消失点

简单的理解就是假设无限远的铁轨在图像平面上会消失，铁轨的线就是消失线，铁轨的两条线的在图像上的交点就是消失点。如图

因此，如果需要得到图像上消失点，就需要图像上的两条平行直线。
如果能在图像上框出来个立方体，就能够得到３个消失点。如果已知３d世界里的立方体，可以将立方体的边投影到２d图像上得到三对平行的直线。
由消失点公式
$$v=Kd$$
K为内参数，$d$为3d空间中的方向向量，$v$为图像平面上的uv坐标。
三个正交的方向向量可以组合成$R$
V P i = K R i i ϵ 0 , 1 , 2 VP_{i}=KR_{i}\\ i\epsilon {0,1,2} VPi​=KRi​iϵ0,1,2
注意 V P i VP_{i} VPi​为图像平面上的消失点。

参考
Multiple View Geometry in Computer Vision (Second Edition)

原文链接

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！