😜 Cube SLAM 代码总结：如何从 2D 目标检测恢复 3D 物体位姿
文章目录
- 注：🌐 Cube SLAM 系列论文，代码注释、总结汇总
- 原始代码：https://github.com/shichaoy/cube_slam
- 个人注释：https://github.com/wuxiaolang/Cube_SLAM_wu
0. 函数
0.1. 函数调用
main_obj.cpp文件中detect_cuboid_obj.detect_cuboid();开始进行物体立方体结构检测；detect_cuboid_obj是立方体检测类（定义在detect_3d_cuboid.h中）detect_3d_cuboid的一个对象；
|
|
0.2. 函数定义
detect_cuboid()函数定义在box_proposal_detail.cpp文件中。- 输入：原始图像
rgb_img； 相机位姿transToWolrd； 2D 检测框信息obj_bbox_coors； 线检测的边缘线信息all_lines_raw； - 输出：立方体提案：
std::vector<ObjectSet>& all_object_cuboids（提案个数等于图像帧数）
1. 线段信息排序
- 首先需要保证作为输入的线段信息矩阵
MatrixXd all_lines_raw中存储的所有线段的两个端点是从左到右排序的（x坐标）； align_left_right_edges()函数在object_3d_util.cpp文件中；- 比如，排序前后：
|
|
- 在原图上绘制检测到的线段：
plot_image_with_edges()函数
2. ground-wall 边界线
- 用
(0,0,1,0)表示地平面ground_plane_world- 平面的表示：一个平面可以用一个齐次向量表示：$\pi = \left ( \pi _{1},\pi _{2},\pi _{3},\pi _{4} \right )^{T} = \left ( n^{T},d \right )^{T}$ ，其中 n 是平面的法向量，d 是它到原点的距离；
|
|
- 计算传感器（相机）的平面
ground_plane_sensor
|
|
- 相机系中的平面与世界系平面的转换关系为： $$\pi_{w}=\left(T_{w, c}^{-}\right)^{T} \cdot \pi_{c}$$
3. 2D 检测框高度采样
3.1 YOLO 2D 检测框原始信息
- 左上角坐标：
(left_x_raw, top_y_raw) - 宽：
obj_width_raw - 高：
obj_height_raw - 右下角坐标：
(right_x_raw, down_y_raw)
|
|
3.2 扩大边界框大小
- 2D 目标检测的检测框可能不准确，是否对检测框的高度进行采样：
std::vector<int> down_expand_sample_all;- 不采样的话：
down_expand_sample_all = 0； - 如果采样高度：
down_expand_sample_all = 0，10，20; - 开关
whether_sample_bbox_height在detect_3d_cuboid.h和main_obj.cpp中；
- 不采样的话：
- 事实上，开启了高度采样之后好像效果更差了？
down_expand_sample_all的 size 是需要计算的次数，不采样时计算一次，采样 10 像素再计算一次，采样 20 像素再计算一次。
3.3 边界距离宽度
- 如下图所示，黄色的框是 YOLO 原始的 2D 检测框，其他颜色的框是拓宽 20 像素的边界框；
- 蓝色、绿色和红色的检测框分别是采样了高度之后，也就是高度采样只往 y 下方进行采样了。
- 蓝色、绿色和红色的检测框分别是采样了高度之后，也就是高度采样只往 y 下方进行采样了。
4. 物体偏航角 yaw 采样
- 物体的偏航角直接初始化为面向相机，与光轴对齐
|
|
- 然后以初始化的 yaw 角为中心的 - 45° 到 + 45° 的 90 ° 范围内，每隔 6 ° 采样一个值，采样得到 15 个偏航角。
|
|
5. 上边缘点采样
- 为确定物体的“顶点”需要从原始边界框的最左边
left_x_raw+5到最右边right_x_raw-5每隔top_sample_resolution（20像素）的距离采样一个点top_x_samples[i].
|
|
- 为确定物体的顶部，提供至少 10 个采样点（后期通过最小误差得到最合适的点），对于越小的物体需要越精细的采样。上边缘采样点如下图所示：
- **疑问：为什么在原始检测框的顶部采样？不在拓宽边界之后的顶部采样呢？效果是否会更好。**
6. 线段的处理
6.1 保留在扩大后边界内的线段
- 线检测得到的所有线段：
all_lines_raw，筛选之后在范围内的线段：all_lines_inside_object； check_inside_box()函数检测线段是否在矩形框内，位于object_3d_util.cpp文件中；- 要求线段的两个端点需要同时都在框内（是否可改进呢）
|
|
6.2 线段合并与剔除
- 短边合并与剔除在
merge_break_lines()函数中实现，位于object_3d_util.cpp文件中；
|
|
6.2.1 线段合并
- 首先要求两条线段的角度误差小于 5 °；
atan2_vector()函数根据线段的水平和竖直投影计算线段的角度 [-90, 90] 范围内；
1 2 3 4 5 6 7// BRIEF 根据线段的 水平和竖直长度x_vec y_vec 计算出角度 all_angles void atan2_vector(const VectorXd& y_vec, const VectorXd& x_vec, VectorXd& all_angles) { all_angles.resize(y_vec.rows()); for (int i=0;i<y_vec.rows();i++) all_angles(i)=std::atan2(y_vec(i),x_vec(i)); // don't need normalize_to_pi, because my edges is from left to right, always [-90 90] }- 计算两条线段的角度差，并保证角度差小于阈值
pre_merge_angle_thre（5 °）
1 2 3// 相邻两条选段的角度差 angle_diff. double diff = std::abs(all_angles(seg1) - all_angles(seg2)); double angle_diff = std::min(diff, M_PI - diff); - 然后要求两条线段的距离差小于 20 像素；
- 计算两条线段的距离：
1 2 3 4// dist_1ed_to_2：线1尾到线2头的距离； // dist_2ed_to_1：线2尾到线1头的距离； double dist_1ed_to_2 = (merge_lines_out.row(seg1).tail(2) - merge_lines_out.row(seg2).head(2)).norm(); double dist_2ed_to_1 = (merge_lines_out.row(seg2).tail(2) - merge_lines_out.row(seg1).head(2)).norm();- 要求“线1尾到线2头的距离”或“线2尾到线1头的距离”小于阈值
pre_merge_dist_thre（20像素）
- 同时满足以上两个条件的两条线段进行合并
|
|
6.2.2 线段剔除
- 计算每条线段的长度，并要求大于长度阈值
edge_length_threshold（30 像素）
|
|
- 最终经过线段合并与筛选之后的效果
7. 线段、边缘检测、距离变换
7.1 计算筛选之后的线段角度和中点
|
|
7.2 Canny 边缘检测
|
|
7.3 构建距离图
- 2D 长方体边缘应与实际图像的边缘匹配。利用 Canny 边缘检测方法构建距离图，然后再长方体边缘倒角距离（Chamfer distance）进行累加求和，再通过 2D 框的大小进行归一化。
|
|
8. 立方体提案生成
8.1 采样相机的翻滚角 roll 和俯仰角 pitch
- 以相机偏角为中心的正负 6 ° 范围内，每隔 3 ° 采样一个值（如果不采样的话就直接使用相机的 roll 和 pitch 角），也就是分别采样了 5 个角度；
|
|
- 同时，保存新的相机位姿，计算新的相机平面
|
|
8.2 计算三个消失点
- 消失点计算函数为
getVanishingPoints()
|
|
- 消失点计算方法： $$v p_{x}=K \cdot R^{-1} \cdot(\cos (y a w), \sin (y a w), 0)^T$$ $$v p_{y}=K \cdot R^{-1} \cdot(-\sin (y a w), \cos (y a w), 0)^T$$ $$v p_{z}=K \cdot R^{-1} \cdot(0,0,1)^T$$
|
|
8.3 寻找形成消失点的边
- 在函数
VP_support_edge_infos()中实现- 输入：消失点的坐标； 每条线段的中点； 每条线段的角度； 角度偏差阈值；
- 消失点 1, 2 的角度偏差阈值为 15 °，消失点 3 的角度偏差阈值为 10 °；
|
|
- 思想：计算消失点-线段中点所构成线段的角度，与检测到的线段的角度进行比较，如果角度差在阈值内，则该条线段可能是形成消失点的线段（理论上消失点和所支持的线段的中点是在一条线上的），记录下这条边的偏角和线段 ID。如下图所示：
- 角度平滑
smooth_jump_angles() - “支撑线”选择，如果检测到多条满足角度要求的边，则选择角度最大和最小的，作为形成消失点的两条“支撑线”，存储在
MatrixXd all_vp_bound_edge_angles矩阵中。
|
|
8.4 计算物体 8 个点的 2D坐标
8.4.1 顶部点 1
- 顶部第一个点由上边缘采样的点确定，可能在检测框顶部的某一个位置，可以参考上面顶边采样的图。
8.4.2 顶部点 2
- 检查消失点 1 在左边还是在右边
seg_hit_boundary()函数两个点构成的射线是否与一条边有交点，没有交集则返回 [-1 -1]
1 2 3Vector2d seg_hit_boundary( const Vector2d& pt_start, const Vector2d& pt_end, const Vector4d& line_segment2 )- 如果消失点 1 与上边缘采样点的射线与边界框的右边界有交点，则位于左边；
- 如果消失点 1 与上边缘采样点的射线与边界框的左边界有交点，则位于右边；
- 参数
vp_1_position表示消失点的左右位置：
1int vp_1_position = 0; // 0 initial as fail, 1 on left 2 on right - 顶部第二个点：即上一步射线与左右边界的交点
8.4.3 第一种情形下的顶部第 3, 4 个点
- 第一种情形：可以观察到物体的三个面；
- 顶部点 4 ：消失点 2 与上边缘采样点的射线与检测框左边（或右边）界的交点；
- 如果没有产生交点，则中断本次检测，说明采样的上边缘点不准确。
- 点 1 与点 4 的距离小于阈值
shorted_edge_thre（默认为 20 ），也失败；
- 顶部点 3：消失点 1 和点 3 构成的射线与消失点 2 和点 2 构成的射线的交点；
- 函数
lineSegmentIntersect()计算两条射线的交点
1 2 3Vector2d lineSegmentIntersect( const Vector2d& pt1_start, const Vector2d& pt1_end, /* 线段 1*/ const Vector2d& pt2_start, const Vector2d& pt2_end, /* 线段 2*/ bool infinite_line)- 如果交点不在检测框内，则失败；
- 如果点 3-4 或点 3-2 的长度小于阈值，则失败；
- 函数
8.4.4 第二种情形下的顶部第 3, 4 个点
- 第二种情形：可以观察到物体的两个面；
- 顶部点 3：消失点 2 与顶部点 2 的连线与检测框左边界（或右）的交点；
- 若没有交点，则失败；
- 若点 2-3 的距离小于阈值，则失败；
- 顶部点 4：消失点 1 和顶部点 3 构成的射线与消失点 2 和顶部点 1 构成的射线的交点；
- 若交点不在检测框内，则失败；
- 如果点 3-4 或点 4-1 的长度小于阈值，则失败；
8.4.5 底部点 5
- 底部点情形 1， 2 的计算方法相同；
- 底部点 5：消失点 3 和顶部点 3 的连线与检测框底边（是否进行了高度采样）的交点；
- 如果没有交点，则失败；
- 如果点 3-5 的距离小于阈值，则失败。
8.4.6 底部点 6
- 底部点 6：消失点 2 和底部点 5 的连线与消失点 3 与顶部点 2 连线的交点；
- 若交点不在检测框内，则失败；
- 若点 6-2 或点 6-5 的距离小于阈值，则失败；
8.4.7 底部点 7
- 底部点 7：消失点 1 和底部点 6 的连线与消失点 3 与顶部点 1 连线的交点；
- 若交点不在检测框内，则失败；
- 若点 7-1 或点 7-6 的距离小于阈值，则失败；
8.4.8 底部点 8
- 底部点 8：消失点 1 和底部点 5 的连线与消失点 2 与顶部点 7 连线的交点；
- 若交点不在检测框内，则失败；
- 若点 8-4 或点 8-5 的距离小于阈值，则失败；
- 物体 8 个点的 2D坐标存储在
box_corners_2d_float矩阵中
|
|
8.5 误差计算（一）
8.5.0 误差存储
- 前四项
|
|
- 往后三项
|
|
- 后两项，采样的相机 roll pitch 角（不采样则直接取相机的角度）
|
|
8.5.1 距离误差
- 距离误差计算函数为：
box_edge_sum_dists()
|
|
- 将 8 个点在图像中的坐标转换成在检测框中的坐标（以检测框左上角为坐标原点）
|
|
- 情形 1：
- 情形 1 下可观察到 3 个面，共 9 条可见边：
1 2visible_edge_pt_ids.resize(9,2); visible_edge_pt_ids << 1,2, 2,3, 3,4, 4,1, 2,6, 3,5, 4,8, 5,8, 5,6;- 形成消失点的线段
1 2 3 4// 三个消失点所需要的两条边 vps_box_edge_pt_ids.resize(3,4); // 1_2 与 8_5 交点得到vp1 4_1 与 5_6 交点得到vp1 vps_box_edge_pt_ids << 1,2,8,5, 4,1,5,6, 4,8,2,6; - 情形 2：
- 情形 2 下可观察到 2 个面，共 7 条可见边：
1 2visible_edge_pt_ids.resize(7,2); visible_edge_pt_ids<<1,2, 2,3, 3,4, 4,1, 2,6, 3,5, 5,6;- 形成消失点的线段
1 2 3// 三个消失点所需要的两条边 vps_box_edge_pt_ids.resize(3,4); vps_box_edge_pt_ids<<1,2,3,4, 4,1,5,6, 3,5,2,6; - 距离误差计算：采样可见边上的点，计算点在距离变换得到的图中的值，累加得到误差值；
- 从前面的距离图中可以看出，距离图显示图像中每一个非零点距离离自己最近的零点的距离，图像上越亮的点，代表了离零点（边缘）的距离越远；（疑问：canny检测没检测出边，导致距离图不准确，但没检测出的边被LSD边缘检测出了呢？那这条边岂不是误差会特别大？？）
- 在可见边（不是检测到的线段，是绘制的边）上采样不同的点：
1Vector2d sample_pt = sample_ind/10.0 * corner_tmp1 + (1-sample_ind/10.0) * corner_tmp2;- 计算每个点的距离值
1float dist1 = dist_map.at<float>(int(sample_pt(1)),int(sample_pt(0)));- 是否加权，然后累加
1 2 3 4 5 6 7 8 9// 是否重新加权 // TODO 第5,6,7条边的测量更值得信赖？？ if (reweight_edge_distance) { if ((4<=edge_id) && (edge_id<=5)) // 对第 5,6 条边 × 1.5 dist1 = dist1 * 3.0 / 2.0; if (6==edge_id) // 对第 7 条边 × 2 dist1 = dist1 * 2.0; }- 误差累加，并求平均
1 2sum_dist = sum_dist + dist1; mean_dist = sum_dist / obj_diaglength_expan； //obj_diaglength_expan是边界框对角线长度.
8.5.2 角度误差
- 角度误差函数：
box_edge_alignment_angle_error()
|
|
- 角度误差 = | 形成消失点的边（2D 点构成）的角度 - 线检测得到的对应边的角度 |
|
|
- 如果没有找到形成消失点的边，则赋予固定的偏差 15 °：
|
|
8.5.3 距离-角度综合评分
- 归一化误差函数：
fuse_normalize_scores_v2()
|
|
- 分别对距离误差和角度误差以递增的方式取所有测量次数的 2/3；
- 再寻找这两个序列的交集，即两个误差都在前 2/3 的测量；
- 对交集中每一次测量归一化两个误差：
|
|
8.6 生成 3D 立方体提案
- 2D 顶点恢复 3D立方体信息：
change_2d_corner_to_3d_object()
|
|
8.6.1 计算立方体底部四个 3D点
plane_hits_3d()函数参数：
|
|
- 首先将 2 * 4 的 4 个 2D 坐标转换成齐次形式 3 × 4;
|
|
- 计算像素点对应的投影射线（ Z 不确定就是一条射线，确定 Z 了就是一个确定的 3D 点）: $$ l_{ray} = K^{-1}\begin{pmatrix} u,v,1\end{pmatrix}^T $$
- 通过相机平面确定底部 3D 点
- 计算相机系下的平面
1 2// 相机系下的地平面 ground_plane_sensor = cam_pose.transToWolrd.transpose()*ground_plane_world;- 计算射线与平面的交点，确定 3D 点：
ray_plane_interact()函数
1 2 3ray_plane_interact( pts_ray, /* 投影射线 */ plane_sensor, /* 相机系下的平面 */ pts_3d_sensor); /* 输出的 3D 点 */- 函数
1 2 3 4 5 6 7 8 9// BRIEF 射线为 3×n ，每列都是从原点开始的一条射线 平面（4*1），计算射线的交点 3×n. //rays is 3*n, each column is a ray staring from origin plane is (4，1） parameters, compute intersection output is 3*n void ray_plane_interact(const MatrixXd &rays, const Eigen::Vector4d &plane, MatrixXd &intersections) { VectorXd frac = -plane[3] / (plane.head(3).transpose() * rays).array(); //n*1 intersections = frac.transpose().replicate<3,1>().array() * rays.array(); }
8.6.2 计算立方体顶部四个 3D点
Matrix3Xd obj_top_pt_world_3d，同上调用上面的函数计算；
8.6.3 物体的 9 自由度表示
- x, y 坐标取四个点的平均值； z 坐标取 z 的一半；
- 方向：取最终提案对应的偏航角采样值；
- 尺度：长宽高；
- 模式：几个面，消失点 1 的位置。
8.6.4 计算 8 个点的 3D世界坐标
|
|
|
|
8.7 误差计算（二）
8.7.1 歪斜比
- 歪斜比 = 长度 / 宽度；
|
|
8.7.2 计算所有误差
- 归一化提案的总体误差：距离+角度+歪斜比误差
|
|
8.7.3 确定最终提案
- 对误差进行排序，选择误差最小的一个作为最终提案；
|
|
8.8 绘制提案
plot_image_with_cuboid()函数完成。
2019.02.22
wuyanminmax@gmail.com