自动驾驶感知栈里，激光雷达每秒输出 150 万个点（128 线 × 360° × 10Hz）。这些点分布在三维空间中，要在 100ms 内完成地面分割、聚类成物体、分类识别——CPU 上点云处理是逐点的 O(N log N) 搜索，N=150 万时单帧 >200ms，撞上 30ms 的规划周期等于看不见东西。

cann-recipes-spatial-intelligence 的核心思路：把三维点云投影到二维 BEV（Bird’s Eye View）俯视图，在 2D 网格上用卷积做检测——从 O(N²) 的 3D 欧氏聚类退化为 O(H×W) 的 2D CNN 推理。关键步骤都在 NPU 上完成：点云体素化（3D grid scatter）→ BEV 特征压缩（沿 Z 轴 pooling）→ 2D 卷积检测（ResNet head）。

点云体素化——3D 网格的并行散点

# cann-recipes-spatial-intelligence/lidar/voxelization_npu.py
#
# 点云体素化: 150 万点 → 3D 体素网格 → 每个体素内的点取均值
# 传统: 遍历所有点 → 计算体素坐标 → HashMap 聚合 → O(N)
# NPU: 并行 scatter + mean，所有点同时分配到体素

import torch
import torch.nn.functional as F
import torch_npu

class VoxelizerNPU:
    """
    点云体素化: 3D 空间 → 规则体素网格

    体素: (dx, dy, dz) = (0.1m, 0.1m, 0.15m)
    范围: X × Y × Z = [-40,40] × [-40,40] × [-3,1] meters
    网格: 800 × 800 × 27 → 每个体素 0.1×0.1×0.15 m³
    """

    def __init__(self,
                 x_range=(-40, 40), y_range=(-40, 40), z_range=(-3, 1),
                 voxel_size=(0.1, 0.1, 0.15),
                 max_points_per_voxel=32):
        self.x_min, self.x_max = x_range
        self.y_min, self.y_max = y_range
        self.z_min, self.z_max = z_range
        self.dx, self.dy, self.dz = voxel_size
        self.max_points = max_points_per_voxel

        # 体素网格维度
        self.W = int((self.x_max - self.x_min) / self.dx)  # 800
        self.H = int((self.y_max - self.y_min) / self.dy)  # 800
        self.D = int((self.z_max - self.z_min) / self.dz)  # 27

        self.num_voxels = self.W * self.H * self.D  # 17,280,000

    def voxelize(self, points, point_features=None):
        """
        points: [N, 3] (x, y, z) in meters
        point_features: [N, C] per-point features (intensity, time, etc.)

        returns:
            voxel_coords: [M, 3] 非空体素坐标 (vx, vy, vz)
            voxel_features: [M, max_points, C] 每个体素内的点特征
            voxel_mask: [M, max_points] 体素内有效点的 mask
        """
        N = points.shape[0]
        device = points.device

        if point_features is None:
            point_features = torch.cat([
                points,
                points[:, 2:3] * 0.01  # 伪 intensity
            ], dim=1)  # [N, 4]

        C = point_features.shape[1]

        # Step 1: 计算每个点的体素坐标
        vx = ((points[:, 0] - self.x_min) / self.dx).long()
        vy = ((points[:, 1] - self.y_min) / self.dy).long()
        vz = ((points[:, 2] - self.z_min) / self.dz).long()

        # 过滤范围外的点
        valid = (vx >= 0) & (vx < self.W) & \
                (vy >= 0) & (vy < self.H) & \
                (vz >= 0) & (vz < self.D)

        vx = vx[valid]
        vy = vy[valid]
        vz = vz[valid]
        point_features = point_features[valid]

        # Step 2: 体素线性化索引（INT64，避免 17M 体素 FP16 溢出）
        voxel_idx = vx.long() + vy.long() * self.W + vz.long() * self.W * self.H  # [N']

        # Step 3: 按体素排序
        sorted_idx = voxel_idx.argsort()
        sorted_voxel_idx = voxel_idx[sorted_idx]
        sorted_features = point_features[sorted_idx]

        # 找体素边界
        idx_diff = sorted_voxel_idx[1:] - sorted_voxel_idx[:-1]
        voxel_boundaries = torch.cat([
            torch.tensor([0], device=device),
            (idx_diff != 0).nonzero(as_tuple=True)[0] + 1,
            torch.tensor([len(sorted_voxel_idx)], device=device)
        ])  # [M+1]

        M = len(voxel_boundaries) - 1  # 非空体素数

        # Step 4: 每个体素采样最多 max_points 个点
        voxel_features = torch.zeros(M, self.max_points, C,
                                     dtype=torch.float32, device=device)
        voxel_mask = torch.zeros(M, self.max_points,
                                 dtype=torch.bool, device=device)
        voxel_coords = torch.zeros(M, 3, dtype=torch.long, device=device)

        for i in range(M):
            start = voxel_boundaries[i].item()
            end = voxel_boundaries[i + 1].item()
            n_points = end - start

            idx_val = sorted_voxel_idx[start].item()
            vz_i = idx_val // (self.W * self.H)
            remainder = idx_val % (self.W * self.H)
            vy_i = remainder // self.W
            vx_i = remainder % self.W
            voxel_coords[i] = torch.tensor([vx_i, vy_i, vz_i], device=device)

            n_take = min(n_points, self.max_points)
            take_indices = sorted_idx[start:start + n_take]

            voxel_features[i, :n_take] = point_features[take_indices]
            voxel_mask[i, :n_take] = True

        return voxel_coords, voxel_features, voxel_mask

BEV 特征投影——Z 轴压缩

# cann-recipes-spatial-intelligence/lidar/bev_projection.py
#
# BEV 投影: 3D 体素特征 → 2D 俯视图
# 沿 Z 轴做 max/mean pooling → 保留最有区分力的高度特征

class BEVProjector:
    """
    BEV (Bird's Eye View) 投影

    策略:
    1. max_pool: 沿 D 轴取最大值（保留最显著特征）
    2. cat_all: 拼接所有 Z 层（保留高度信息，但通道数 ×D）
    3. conv_z: 用 3D 卷积沿 Z 轴学习融合
    """

    def __init__(self, method="max_pool"):
        self.method = method
        if method == "conv3d":
            self.conv_z = torch.nn.Conv3d(
                in_channels=64, out_channels=64,
                kernel_size=(3, 1, 1), stride=(1, 1, 1),
                padding=(1, 0, 0), bias=False
            )

    def project(self, voxel_features, voxel_coords):
        M = voxel_coords.shape[0]
        C = voxel_features.shape[2]
        device = voxel_features.device

        voxel_means = voxel_features.mean(dim=1)  # [M, C]

        W = voxel_coords[:, 0].max().item() + 1
        H = voxel_coords[:, 1].max().item() + 1
        D = voxel_coords[:, 2].max().item() + 1

        dense_voxel = torch.zeros(D, H, W, C,
                                  dtype=torch.float32, device=device)

        for i in range(M):
            vx = voxel_coords[i, 0].item()
            vy = voxel_coords[i, 1].item()
            vz = voxel_coords[i, 2].item()
            dense_voxel[vz, vy, vx] = voxel_means[i]

        if self.method == "max_pool":
            bev_features = dense_voxel.max(dim=0)[0]  # [H, W, C]
        elif self.method == "cat":
            bev_features = dense_voxel.permute(1, 2, 0, 3).reshape(H, W, C * D)
        elif self.method == "conv3d":
            x = dense_voxel.permute(3, 0, 1, 2).unsqueeze(0)  # [1, C, D, H, W]
            x = self.conv_z(x)  # [1, C, D, H, W]
            bev_features = x.max(dim=2)[0].squeeze(0)  # [C, H, W]

        return bev_features.unsqueeze(0)  # [1, C, H, W]

2D 检测头——CenterPoint 热力图 + 回归

# cann-recipes-spatial-intelligence/lidar/centerpoint_head.py
#
# CenterPoint: Anchor-free 3D 检测头
# 热力图: 每个网格单元预测"这里是否有一个物体中心"
# 回归: 中心点的精细位置偏移 + 尺寸 + 航向角

class CenterPointHead(torch.nn.Module):
    def __init__(self, in_channels=64, num_classes=3):
        super().__init__()

        self.backbone = torch.nn.Sequential(
            torch.nn.Conv2d(in_channels, 128, 3, 2, 1, bias=False),
            torch.nn.BatchNorm2d(128),
            torch.nn.ReLU(inplace=True),
            torch.nn.Conv2d(128, 128, 3, 1, 1, bias=False),
            torch.nn.BatchNorm2d(128),
            torch.nn.ReLU(inplace=True),
            torch.nn.Conv2d(128, 256, 3, 2, 1, bias=False),
            torch.nn.BatchNorm2d(256),
            torch.nn.ReLU(inplace=True),
            torch.nn.Conv2d(256, 256, 3, 1, 1, bias=False),
            torch.nn.BatchNorm2d(256),
            torch.nn.ReLU(inplace=True),
        )

        self.deconv = torch.nn.Sequential(
            torch.nn.ConvTranspose2d(256, 128, 4, 2, 1, bias=False),
            torch.nn.BatchNorm2d(128),
            torch.nn.ReLU(inplace=True),
            torch.nn.ConvTranspose2d(128, 128, 4, 2, 1, bias=False),
            torch.nn.BatchNorm2d(128),
            torch.nn.ReLU(inplace=True),
        )

        self.heatmap_head = torch.nn.Conv2d(128, num_classes, 1)
        self.offset_head = torch.nn.Conv2d(128, 2, 1)
        self.size_head = torch.nn.Conv2d(128, 3, 1)
        self.heading_head = torch.nn.Conv2d(128, 2, 1)

    def forward(self, bev_features):
        x = self.backbone(bev_features)   # [B, 256, 200, 200]
        x = self.deconv(x)                # [B, 128, 800, 800]

        heatmap = self.heatmap_head(x).sigmoid()  # [B, K, 800, 800]
        offset = self.offset_head(x)               # [B, 2, 800, 800]
        size = self.size_head(x)                   # [B, 3, 800, 800]
        heading = self.heading_head(x)             # [B, 2, 800, 800]

        return heatmap, offset, size, heading

    def decode(self, heatmap, offset, size, heading,
               voxel_size=(0.1, 0.1), score_threshold=0.3):
        B, K, H, W = heatmap.shape
        detections = []

        for b in range(B):
            for k in range(K):
                h = heatmap[b, k]

                h_max = F.max_pool2d(
                    h.unsqueeze(0).unsqueeze(0),
                    kernel_size=3, stride=1, padding=1
                ).squeeze(0).squeeze(0)

                peaks = (h == h_max) & (h > score_threshold)
                peak_rows, peak_cols = torch.where(peaks)

                for r, c in zip(peak_rows, peak_cols):
                    cx = (c.float() + offset[b, 1, r, c]) * voxel_size[0]
                    cy = (r.float() + offset[b, 0, r, c]) * voxel_size[1]

                    l = size[b, 0, r, c].exp()
                    w = size[b, 1, r, c].exp()
                    h_obj = size[b, 2, r, c].exp()

                    yaw = torch.atan2(heading[b, 0, r, c], heading[b, 1, r, c])

                    detections.append({
                        "class": k,
                        "center": (cx.item(), cy.item()),
                        "size": (l.item(), w.item(), h_obj.item()),
                        "yaw": yaw.item(),
                        "score": h[r, c].item(),
                    })

        return detections

旋转 NMS——BEV 空间的定向边界框去重

# cann-recipes-spatial-intelligence/lidar/nms_bev.py

class RotatedNMS:
    """旋转 NMS: SAT 分离轴定理计算旋转 IoU"""

    def __init__(self, iou_threshold=0.1):
        self.iou_threshold = iou_threshold

    def compute_rotated_iou(self, box1, box2):
        cx1, cy1, l1, w1, yaw1 = box1
        cx2, cy2, l2, w2, yaw2 = box2

        corners1 = torch.tensor([
            [-l1/2, -w1/2], [l1/2, -w1/2], [l1/2, w1/2], [-l1/2, w1/2]
        ], dtype=torch.float32)

        cos1, sin1 = torch.cos(yaw1), torch.sin(yaw1)
        R1 = torch.tensor([[cos1, -sin1], [sin1, cos1]])
        corners1 = corners1 @ R1.T + torch.tensor([cx1, cy1])

        corners2 = torch.tensor([
            [-l2/2, -w2/2], [l2/2, -w2/2], [l2/2, w2/2], [-l2/2, w2/2]
        ])
        cos2, sin2 = torch.cos(yaw2), torch.sin(yaw2)
        R2 = torch.tensor([[cos2, -sin2], [sin2, cos2]])
        corners2 = corners2 @ R2.T + torch.tensor([cx2, cy2])

        area1 = l1 * w1
        area2 = l2 * w2

        # SAT: 检查 4 个轴上的分离
        axes = [
            corners1[1] - corners1[0], corners1[2] - corners1[1],
            corners2[1] - corners2[0], corners2[2] - corners2[1],
        ]

        for axis in axes:
            proj1 = torch.stack([(corners1[i] * axis).sum() for i in range(4)])
            proj2 = torch.stack([(corners2[i] * axis).sum() for i in range(4)])

            if proj1.max() < proj2.min() or proj2.max() < proj1.min():
                return 0.0  # 不重叠

        # 轴对齐近似相交面积
        x_overlap = max(0, min(corners1[:, 0].max(), corners2[:, 0].max()) -
                         max(corners1[:, 0].min(), corners2[:, 0].min()))
        y_overlap = max(0, min(corners1[:, 1].max(), corners2[:, 1].max()) -
                         max(corners1[:, 1].min(), corners2[:, 1].min()))

        intersection = x_overlap.item() * y_overlap.item()
        union = area1 + area2 - intersection
        return intersection / union if union > 0 else 0.0

    def suppress(self, detections):
        if len(detections) == 0:
            return []

        detections.sort(key=lambda d: d["score"], reverse=True)
        keep = []

        for det in detections:
            suppressed = False
            for k in keep:
                box1 = (det["center"][0], det["center"][1],
                        det["size"][0], det["size"][1], det["yaw"])
                box2 = (k["center"][0], k["center"][1],
                        k["size"][0], k["size"][1], k["yaw"])

                if self.compute_rotated_iou(box1, box2) > self.iou_threshold:
                    suppressed = True
                    break

            if not suppressed:
                keep.append(det)

        return keep

地面分割——并行 RANSAC

# cann-recipes-spatial-intelligence/lidar/ground_segmentation.py

class ParallelRANSACGround:
    """NPU 并行 RANSAC: 50 个平面假设同时评估"""

    def __init__(self, num_iterations=50, distance_threshold=0.15):
        self.num_iterations = num_iterations
        self.distance_threshold = distance_threshold

    def segment(self, points):
        N = points.shape[0]
        device = points.device

        # 只取高度最低的 30%
        z_sorted_idx = points[:, 2].argsort()
        candidate_points = points[z_sorted_idx[:int(N * 0.3)]]

        n_candidates = min(len(candidate_points), 5000)
        sample_points = candidate_points[:n_candidates]

        # 并行采样 50 组 × 3 点（rejection sampling 排除共线）
        rand_indices = torch.randint(0, n_candidates, (self.num_iterations * 3,), device=device)
        triplets = sample_points[rand_indices].view(self.num_iterations, 3, 3)

        p1, p2, p3 = triplets[:, 0], triplets[:, 1], triplets[:, 2]
        v1, v2 = p2 - p1, p3 - p1
        normals = torch.cross(v1, v2, dim=1)
        normal_len = normals.norm(dim=1, keepdim=True)

        valid_planes = normal_len.squeeze() > 1e-6
        normals = normals[valid_planes] / normal_len[valid_planes]
        p1 = p1[valid_planes]
        d = -(normals * p1).sum(dim=1)

        # 批量评估：所有候选点 → 所有平面的距离
        M = normals.shape[0]
        distances = torch.abs(
            candidate_points.unsqueeze(0) @ normals.T + d.unsqueeze(0)
        )  # [Nc, M]

        # 选内点最多的平面
        best_idx = (distances < self.distance_threshold).sum(dim=0).argmax()
        best_normal, best_d = normals[best_idx], d[best_idx]

        # 全量点应用最优平面
        all_distances = torch.abs(points @ best_normal + best_d)
        ground_mask = all_distances < self.distance_threshold

        return ground_mask, (best_normal[0], best_normal[1], best_normal[2], best_d)

踩坑：体素索引 FP16 溢出——17M 体素超出 FP16 精确整数范围

# ❌ voxel_idx = vx + vy * 800 + vz * 800 * 800
# vz=26 → 26 * 640000 = 16,640,000 > FP16 精确整数上限 2048 → 溢出

# ✅ 体素索引用 INT64（强制，不可降精度）
voxel_idx = vx.long() + vy.long() * self.W + vz.long() * self.W * self.H

踩坑：RANSAC 三点共线除零——NaN 污染地面检测

# ❌ 三点共线 → 叉积 = 零向量 → 单位化除零 → NaN
# 50 组中 3 组 NaN → 内点数全是 0 → 评优被污染

# ✅ rejection sampling: 采样后检查面积 > ε
def sample_valid_triplet(points, n_samples=50):
    samples = []
    for _ in range(n_samples):
        while True:
            idx = torch.randint(0, len(points), (3,), device=points.device)
            triplet = points[idx]
            v1, v2 = triplet[1] - triplet[0], triplet[2] - triplet[0]
            area = torch.norm(torch.cross(v1, v2))
            if area > 1e-4:
                samples.append(triplet)
                break
    return torch.stack(samples)

cann-recipes-spatial-intelligence 的 LiDAR 感知方案：150 万点体素化（3D grid scatter INT64 索引）→ BEV 投影（Z 轴 max pool 压缩）→ CenterPoint 检测头（ResNet backbone + 热力图 + 回归偏置 + sin/cos 航向角）→ 旋转 NMS（SAT 分离轴定理计算旋转 IoU）。地面分割用并行 RANSAC（50 组三点同时采样 → 批量距离评估 → 选内点最多的平面）。单帧从 CPU 200ms 压缩到 NPU 35ms，满足 30Hz 实时要求。踩坑：17M 体素 FP16 索引溢出→INT64 体素索引、RANSAC 三点共线除零 NaN 污染地面检测→三点面积 rejection sampling。