托盘识别

概述 / Overview

"托盘识别"是把 2D 或 3D 激光雷达点云中属于托盘（标准木托盘、塑料托盘、卷料 V 形托架等）的部分识别出来，给出 中心位姿 与 可信度 的算法集合。它是自动识别托盘取放货的底层模块，本身不涉及任何运动控制；只负责"看见在哪"。

"Pallet recognition" is the family of algorithms that extract, from 2D or 3D lidar clouds, the points belonging to a pallet (standard wooden / plastic pallet, V-cradle for coils, etc.) and output a centre pose with a confidence score. It is the perception module under auto-pallet pick & place; it does no motion control, only "where is it".

算法分类 / Algorithm taxonomy

类别 / Class	输入 / Input	输出 / Output	适用 / Suitable
几何检测器 / Geometric detector	2D 激光帧 / one 2D lidar frame	两个特征点 + 中轴线 / two feature points + midline	叉孔标准托盘 / standard fork-pocket pallets
边沿拟合 / Edge fit	2D 激光帧	托盘前沿线段 / pallet front edge segment	实心边、无孔结构 / solid edges, no pockets
V 槽检测 / V-groove detector	2D 激光帧	V 槽顶点 + 张角 / V apex + opening angle	卷料托架、双车联动 V 槽 / coil cradle, twin-car V-feature
三脚架检测 / 3-leg detector	2D 激光帧	3 个腿点 + 中心 / 3 legs + centroid	三腿 KIVA、T 形托盘 / 3-leg KIVA, T-shape
3D 模板匹配 / 3D template match	3D 点云 / 3D cloud	6-DoF 位姿 / 6-DoF pose	异型托盘、堆垛 / irregular / stacked

主入口类 / Main entry classes (Clumsy):

`LidarDetectTray` — 通用两点 / 三点检测器；`blobDist`, `BlobPtCount`, `PillarFindingScope` 控制聚类粒度。
`LidarBlobPairDetector` — 两-blob 配对，含车头朝向先验。
`LidarDetectPallet` — 托盘叉孔专用，提供 `widthMin/widthMax` 接受区间。
`Lidar2dDetect3LegTray` — 3 腿托架专用。
`LidarDetect3DTemplate` — 3D 点云模板（基于 CycleGUI 录制的 reference 点云）。

工作原理（几何检测器为例）/ How it works (geometric detector)

以最常用的"叉孔托盘两点检测"为例：

把当前激光帧的点云转到车体坐标系。
Transform the current lidar frame into the vehicle frame.
用 `angleStart/angleEnd/minDist/maxDist` 把雷达视场窄化到托盘可能出现的区域。
Narrow the FOV via `angleStart/angleEnd/minDist/maxDist` to the slot where the pallet is expected.
沿距离方向做 1D 直方图聚类（bin = `resolution`），找出 2 个相邻峰，作为两个候选叉孔中心。
1D distance histogram (bin = `resolution`); locate 2 adjacent peaks → 2 candidate pocket centres.
用宽度先验（`widthMin ≤ |Δ| ≤ widthMax`）剔除假阳性。
Reject candidates outside the prior width interval.
用最小二乘拟合两个候选点周边的局部点云（`PillarFindingScope` 半径），得到精化后的 (x, y) 与角度。
Refine each candidate by least-squares fit over a neighbourhood of radius `PillarFindingScope`; output (x, y) and the line connecting them.
计算置信度（=点云密度 × 几何吻合度）。
Confidence = local density × geometric fit quality.

最小二乘的核心代码（取自 `AutoShelfFetchingManyLegs.cs` 中的 `CalcLine`）：

private (double dx, double dy, float fx, float fy, double A, double B, double C)
    CalcLine(IEnumerable<float2> pts)
{
    var arr = pts as float2[] ?? pts.ToArray();
    float fx  = arr.Average(f => f.x);
    float fy  = arr.Average(f => f.y);
    float fxx = arr.Average(f => f.x * f.x);
    float fxy = arr.Average(f => f.x * f.y);
    float fyy = arr.Average(f => f.y * f.y);
    float a = fxx - fx * fx, b = fxy - fx * fy, c = fyy - fy * fy;
    double sqt = Math.Sqrt((a - c) * (a - c) + 4 * b * b);
    double l1 = a + c + sqt;
    double dx, dy;
    if (Math.Abs(a - l1 / 2) > Math.Abs(c - l1 / 2)) { dy = l1/2 - a; dx = b; }
    else                                              { dx = l1/2 - c; dy = b; }
    double n = Math.Sqrt(dx*dx + dy*dy); dx /= n; dy /= n;
    return (dx, dy, fx, fy, dy, -dx, dx*fy - dy*fx);
}

标定与精度 / Calibration & accuracy

托盘识别的精度主要受三项约束： The accuracy of pallet recognition is bounded by three calibration items:

激光雷达外参 / Lidar extrinsics — 车体到雷达的 (Δx, Δy, Δθ)。用外参标定工具。误差 > 5 mm / 1° 时，托盘姿态会有可观偏差。
激光雷达水平度 / Lidar horizontality — 倾斜会引入距离偏移；卡车上需要定期复检。
托盘几何先验 / Pallet geometry priors — `width`, `widthMin/Max`, `PillarFindingScope` 等都要按实测调；不要直接抄默认值。

调试时先用 CAD 工具在 SimpleComposer 中可视化点云与检测结果。

When debugging, render the live cloud and the detector output on the SimpleComposer CAD canvas first; this is usually faster than re-tuning blind.

失败模式 / Failure modes

两个反光板被误识为叉孔 / Two reflectors mistaken for pockets: 启用 `intensityMin` 阈值过滤。
并排两个托盘被合并 / Two side-by-side pallets merged: 调小 `resolution`（更细的 bin），并启用 `widthMax` 上限。
托盘前沿被部分遮挡 / Front edge partially occluded: 启用 `PartialOccludeFallback`（用 3D 雷达 ROI 提升）或要求 `confidence > 0.7`。
环境强光斑反射 / Strong light glints: 强度归一化（`ReflexRange`）+ 中值滤波。