舱口检测：从点云到矩形定位的射线投影算法

舱口检测：从点云到矩形定位的射线投影算法

基于深度相机点云，利用射线投影与连通域分析实现舱口矩形区域的自动检测。

---

1. 整体 Pipeline

点云 (PCD)
  │
  ▼
[Step 1] 甲板平面检测 + 高度图 → 二值俯视图
  │
  ▼
[Step 13] 垂线检测 + 射线投影 + 矩形检测 → 检测框坐标

run_step1_and_step13.sh 串联了这两个核心步骤。Step 1 负责把原始点云变成一张"干净"的二值俯视图，Step 13 则在这张图上找出舱口的精确矩形边界。

---

2. Step 1：甲板平面检测与高度图生成

2.1 点云下采样

原始点云通常密度极高，直接处理开销大。首先用 VoxelGrid 体素滤波器做下采样，网格尺寸 0.02 m，保证均匀采样。

2.2 RANSAC 平面检测

用 PCL 的 RANSAC（SACSegmentation）检测甲板平面：

ax + by + cz + d = 0

• 距离阈值：0.05 m（点到这个平面的距离 ≤ 5 cm 视为内点）
• 最大迭代：1000 次
• 平面方程同时用于判断每个点是否在甲板上方

2.3 甲板上方点过滤

已知平面方程后，计算每个点到平面的有符号距离：

dist = (a·x + b·y + c·z + d) / ||normal||

• dist > 0 且 dist ≤ 3.0 m：保留（舱口结构物在此范围内）
• 其余点丢弃，得到 no_deck 点云（去除甲板，只剩舱口等凸出物）

2.4 平面正交基投影

为将 3D 点云投影到 2D 俯视图，先构建甲板平面的局部坐标系：

// 法向量 n 与垂直方向(0,0,1)叉乘得到 u，u 与 n 叉乘得到 v
u = n × (0,0,1)   // 第一个基向量
v = n × u         // 第二个基向量

任意 3D 点 P 投影到平面后的 2D 坐标：

x_2d = P' · u
y_2d = P' · v

其中 P' 是 P 沿法向量方向最近到平面的投影点。

2.5 高度图生成

将 2D 投影坐标离散化为像素，每个像素记录该位置的最高点高度：

if (height_map[py][px] == 0 || current_height > height_map[py][px]) {
    height_map[py][px] = current_height;
}

分辨率 0.02 m/px，生成高度图后做二值化：≥ 0.5 m → 前景（舱口结构），< 0.5 m → 背景（空白区域）。

最终输出 step1_full_topview_binary.png，即为后续 Step 13 的输入底图。

---

3. Step 13：垂线检测 + 射线投影 + 矩形检测

3.1 膨胀（Dilation）

二值图中的舱口轮廓可能断裂、有缺口。先用矩形核膨胀：

kernel = getStructuringElement(MORPH_RECT, {20, 20});
dilate(binary, dilated, kernel);

20×20 核覆盖约 40 cm 范围，足以将相邻舱口轮廓桥接起来。

3.2 形态学开运算（Opening）

膨胀会把无关区域也连接起来。开运算（先腐蚀后膨胀）去掉毛刺和孤岛噪声：

kernel = getStructuringElement(MORPH_RECT, {5, 5});
morphologyEx(dilated, opened, MORPH_OPEN, kernel);

3.3 边缘带宽过滤（Distance Transform）

这是关键一步------只保留距离前景边界一定范围内的像素（约 5 px ≈ 10 cm），过滤掉舱口内部的杂散像素。这样做有两个好处：

1. 大幅减少后续 Canny 边缘的噪声
2. 保证检测到的边缘恰好是舱口外轮廓

实现上用 distanceTransform 计算每个像素到最近零像素的距离，再阈值过滤：

distanceTransform(opened, dist, DIST_L2, DIST_MASK_3);
threshold(dist, band_mask, edge_band_px, 255, THRESH_BINARY);
bitwise_and(opened, band_mask, band_region);

3.4 Canny 边缘检测

对边缘带宽区域运行 Canny，提取精确的边缘像素。

3.5 HoughLinesP 直线检测

用概率 Hough 变换从 Canny 边缘中提取直线段：

HoughLinesP(edges, lines, rho=1, theta=π/180,
            threshold=100, minLength=min_len, maxGap=max_gap);

3.6 垂线过滤（基于参考方向）

绿线图已知参考主方向（ref_angle），目标垂线方向为 ref_angle + π/2。过滤所有 Hough 直线，保留方向与目标垂线方向差 ≤ 10° 的线段：

angleDiff = |normalize(angle) - normalize(perp_angle)|
if (angleDiff <= 10°)  keep

3.7 线段延长

Hough 检测到的线段可能只是真实垂线的一部分。将两端各延长一定长度：

// 每端延长 extend_length_m（如 5m）
Line extendLine(Line l, int extend_px) {
    double len = sqrt(dx²+dy²);
    double ux = dx/len, uy = dy/len;
    return {
        (int)(l.x1 - ux*extend_px), (int)(l.y1 - uy*extend_px),
        (int)(l.x2 + ux*extend_px), (int)(l.y2 + uy*extend_px)
    };
}

3.8 线段合并（Union-Find Style）

多条相邻且近似平行的垂线段应合并为一条。先判断两条线是否可以合并：

bool canMerge(a, b):
    1. 方向差 ≤ 5°
    2. 垂直距离（b中点到a直线的距离）≤ max_dist_px
    3. 纵向投影有重叠或有gap但gap ≤ max_gap_px

满足条件则合并，取两端点的纵向极值作为新端点：

Line mergeLines(a, b):
    // 投影到 a 的方向，取 p 坐标的 min 和 max
    new.p1 = min(project(a, a.p1), project(a, b.p1), project(a, b.p2))
    new.p2 = max(...)
    return new

3.9 坐标系变换（no_deck → 11b → step1）

检测在 no_deck 坐标系的低分辨率图上进行，最终结果需要叠加到 step1 的高分辨率俯视图上。三套坐标系通过世界坐标中转：

no_deck 像素 ──(分辨率)──► 世界坐标 (x, y) [m]
                                      │
                                      ▼
                     世界坐标 (x, y) ──(分辨率)──► 11b 像素 / step1 像素

每个坐标系各自保存参数文件（x_min, x_max, y_min, y_max, resolution, width, height）。

3.10 射线投影检测（核心）

这是算法最核心的部分------从黄点（已知的舱口参考点）向 360° 方向发射射线，追踪与绿线、蓝线、白色前景区域的交点。

射线追踪

每条射线以步长 1 px 向外延伸，检查当前像素是否属于：

停止原因	条件	含义
白色像素 > 0	前景区域	射线到达舱口内部
蓝线	dist(line, p) < blue_th	射线命中垂线边缘
绿线	dist(line, p) < green_th	射线命中水平参考线边缘

射线终点取为在线段上的最近点（而非采样点），保证精度：

cv::Point closestPointOnLine(p, line) {
    double t = clamp(dot(P-P1, P2-P1) / |P2-P1|², 0, 1);
    return P1 + t * (P2 - P1);
}

射线坐标系投影

每条射线停止后，将射线上所有像素投影到蓝线局部坐标系：

s = x·ux + y·uy   （沿蓝线方向）
t = x·vx + y·vy   （垂直蓝线方向）

s 轴平行于蓝线，t 轴垂直于蓝线（舱口宽度方向）。

3.11 连通域 + 收缩贴合（Shrink-to-Ray）

所有黄色射线构成连通区域。对每个连通域：

1. 投影极值 ：取 s ∈ [s_min, s_max]、t ∈ [t_min, t_max] 作为初始矩形
2. 收缩贴合 ：沿 t 方向逐步收缩上下边缘，直到边缘与 ≥ 80% 的黄射线像素接触

// 检查上边/下边在给定收缩量下是否接触 ≥80% 黄射线
auto checkEdge = [&](double t_edge, double shrink_px) -> bool {
    int contact = 0;
    for each s_sample in [s_min, s_max]:
        // 在 t_check = t_edge + shrink_px 处扫描
        // 统计有多少采样点落在黄射线 mask 上
        if (rayMask[int(s·ux + t_check·vx)][int(s·uy + t_check·vy)] == 255)
            contact++;
    return contact / (num_samples+1) >= 0.8;
};

// 二分搜索最大收缩量
double top_shrink = binarySearch(checkEdge, 0, t_max - t_min);
double bottom_shrink = binarySearch(checkEdge, 0, t_max - t_min);

这步相当于让矩形边缘"吸附"到黄射线覆盖区域的真实边界，80% 阈值保证了鲁棒性------既有足够多的射线支撑，又不会过度膨胀。

3.12 合并相邻连通域 + 尺寸过滤

将 s 和 t 区间均有重叠的相邻连通域合并，最后过滤：长 > 15 m 且 宽 > 8 m 的矩形才保留。

---

4. 关键参数一览

参数	值	含义
膨胀核	20×20 px	连接断裂的舱口轮廓
开运算核	5×5 px	去除毛刺噪声
边缘带宽	5 px	只保留距轮廓边界 ≤5px 的区域
Canny 阈值	50 / 150	边缘检测灵敏度
垂线方向容差	10°	保留与目标垂直方向差 ≤10° 的线
射线线宽阈值	绿 1.5px / 蓝 2.5px	射线停止判定
收缩贴合阈值	≥80% 接触	边缘吸附判定
尺寸过滤	长>15m & 宽>8m	最终矩形过滤

---

5. 输出结果

• step1_full_topview_binary.png --- 甲板平面检测后的二值俯视图
• 13_09_11b_rays.png --- 11b 坐标系下的射线检测结果
• 13_09_step1_rays.png --- 叠加到 step1 俯视图的检测框（红/蓝/橙/绿/紫色矩形 + 尺寸标注）
• 13_10_rects.txt --- 矩形参数（s_min, s_max, t_min, t_max, width_m, height_m）

---

6. 总结

整个方法的核心思路是：

1. 甲板平面投影：将 3D 点云正交投影到甲板平面坐标系，生成 2D 高度图
2. 垂线特征提取：膨胀 + 开运算 + 边缘带宽过滤 + Hough 直线检测，精确提取垂直边缘
3. 射线投影吸附：从已知黄点发射射线，用收缩贴合算法让矩形边缘自动贴合到真实射线边界
4. 多坐标系统一：通过世界坐标中转，实现了 no_deck → 11b → step1 三套坐标系的精确变换