lidar-camera-fusion

name	lidar-camera-fusion
description	激光-相机融合技能 - 深度学习融合、几何投影融合、3D检测、ROS2标定与同步
argument-hint	激光相机融合 OR lidar camera fusion OR 深度学习融合 OR 3D检测 OR 多传感器融合
user-invocable	true

激光-相机融合技能

用于实现激光雷达与相机的深度融合，涵盖几何投影融合、深度学习融合（CNN/Transformer）、3D目标检测和 ROS2 集成

---

何时使用

当需要以下帮助时使用此技能：

• 激光雷达与相机外参标定
• 几何投影融合（LiDAR → 图像）
• 深度学习融合检测（PointPillars + 图像）
• 3D 目标检测（BEV + 相机融合）
• 多模态感知系统 ROS2 部署

---

快速参考

融合策略对比

融合层级:
├── 原始数据层 (Early Fusion) → 点云 + 图像原始数据 concat
├── 特征层 (Deep Fusion) → 点云特征 + 图像特征在网络内融合
├── 决策层 (Late Fusion) → 各自检测结果加权融合
└── 混合融合 (Hybrid) → 多层融合组合

核心依赖

# 基础
sudo apt install -y ros-humble-depthimage-to-laserscan
sudo apt install -y ros-humble-pointcloud-to-laserscan

# 标定
sudo apt install -y ros-humble-calibration-camera-lidar

# 深度学习（可选）
pip install open3d torch torchvision

核心话题

话题	类型	说明
/camera/color/image_raw	sensor_msgs/Image	RGB 图像
/camera/depth/image_rect_raw	sensor_msgs/Image	深度图
/velodyne_points	sensor_msgs/PointCloud2	激光点云
/ detections_3d	DetectionArray	3D 检测结果

---

外参标定

标定板法（棋盘格 + 激光）

#!/usr/bin/env python3
"""相机-激光雷达外参标定"""

import numpy as np
import open3d as o3d
import cv2
from pathlib import Path


class CameraLidarCalibrator:
    """相机-激光雷达标定"""

    def __init__(self, camera_intrinsics, resolution=(1920, 1080)):
        self.K = camera_intrinsics  # 内参矩阵
        self.resolution = resolution
        self.T_cam_lidar = None  # 外参：激光雷达到相机的变换

    def calibrate_with_board(
        self,
        lidar_points: np.ndarray,
        board_corners_3d: np.ndarray,
        image: np.ndarray
    ) -> np.ndarray:
        """
        使用棋盘格标定板标定

        Args:
            lidar_points: 标定板上的激光点云 Nx3
            board_corners_3d: 标定板3D角点（从相机视角计算）4x3
            image: 相机图像

        Returns:
            T_lidar_cam: 4x4 激光雷达到相机的变换矩阵
        """
        # 步骤1: 检测图像中的棋盘格角点
        ret, img_corners = cv2.findChessboardCorners(
            image, (9, 6), None
        )

        # 步骤2: 亚像素精化
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        img_corners = cv2.cornerSubPix(
            cv2.cvtColor(image, cv2.COLOR_BGR2GRAY),
            img_corners, (11, 11), (-1, -1), criteria
        )

        # 步骤3: PnP 求解相机位姿
        object_points = np.zeros((*, ), np.float32)
        object_points[:, :] = np.mgrid[:, :].T.reshape(-, ) * 
        _, rvec, tvec = cv2.solvePnP(
            object_points, img_corners, .K, 
        )
        R_cam_board, _ = cv2.Rodrigues(rvec)

        
        
        normal = R_cam_board[:, ]  
        d = -normal @ (R_cam_board @ np.array([, , ]) + tvec.flatten())

        
        lidar_on_board = []
         pt  lidar_points:
            t = -(normal @ pt + d) / (normal @ normal)
            proj = pt + t * normal
            lidar_on_board.append(proj)

        lidar_on_board = np.array(lidar_on_board)

        
        
        T_cam_board = np.eye()
        T_cam_board[:, :] = R_cam_board
        T_cam_board[:, ] = tvec.flatten()

        T_board_cam = np.linalg.inv(T_cam_board)
        lidar_in_board = (T_board_cam @ np.hstack([lidar_on_board, np.ones(((lidar_on_board), ))]).T).T

        
        

         T_cam_lidar

     () -> np.ndarray:
        

        
        points_cam = (T_lidar_cam @ np.hstack([points, np.ones(((points), ))]).T).T

        
        valid = points_cam[:, ] > 
        points_cam = points_cam[valid]

        
        uv = (.K @ points_cam[:, :].T).T
        uv[:, ] /= uv[:, ]
        uv[:, ] /= uv[:, ]

         uv[:, :].astype(np.int32)


 ():
    
    
    

---

几何投影融合

LiDAR → 相机图像投影

import numpy as np
import open3d as o3d
from typing import Tuple, List


class LidarCameraProjector:
    """激光雷达点云投影到图像"""

    def __init__(
        self,
        K: np.ndarray,
        D: np.ndarray,
        R: np.ndarray,
        t: np.ndarray,
        image_width: int,
        image_height: int
    ):
        """
        Args:
            K: 相机内参 3x3
            D: 畸变系数 5x1
            R: 旋转矩阵 (lidar -> camera) 3x3
            t: 平移向量 (lidar -> camera) 3x1
        """
        self.K = K
        self.D = D
        self.R = R
        self.t = t
        self.width = image_width
        self.height = image_height

        # 构建投影矩阵
        self.extrinsic = np.hstack([R, t])

        # 畸变校正映射
        self.map1, self.map2 = cv2.initUndistortRectifyMap(
            K, D, np.eye(3), K, (image_width, image_height), cv2.CV_16SC2
        )

    def project(self, cloud: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
        """
        将点云投影到图像平面

        Args:
            cloud: Nx3 点云 (lidar坐标系)

        Returns:
            uv: Nx2 像素坐标
            depth: N 对应深度
        """
        # 变换到相机坐标系
        cloud_hom = np.hstack([cloud, np.ones((len(cloud), 1))])  # Nx4
        cloud_cam = (self.extrinsic @ cloud_hom.T).T  

        
        valid = cloud_cam[:, ] > 
        cloud_cam = cloud_cam[valid]

        
        cloud_undist = cv2.undistortPoints(
            cloud_cam[:, :], .K, .D, P=.K
        )

        
        x = cloud_undist[:, , ]
        y = cloud_undist[:, , ]
        z = cloud_cam[:, ]

        
        u = .K[, ] * x / z + .K[, ]
        v = .K[, ] * y / z + .K[, ]

        
        in_image = (u >= ) & (u < .width) & (v >= ) & (v < .height)

        uv = np.column_stack([u[in_image], v[in_image]]).astype(np.int32)
        depth = z[in_image]

         uv, depth

     () -> np.ndarray:
        
        depth_img = np.zeros((.height, .width), dtype=np.float32)

        uv, depth = .project(cloud)

         (u, v), d  (uv, depth):
             depth_img[v, u] ==   depth_img[v, u] > d:
                depth_img[v, u] = d

         depth_img

---

深度学习融合检测

PointPillars + 图像融合

import torch
import torch.nn as nn
import numpy as np


class PointPillarsBackbone(nn.Module):
    """PointPillars 特征提取（简化版）"""

    def __init__(self, in_channels=64):
        super().__init__()
        self.pillar_encode = nn.Conv1d(in_channels, 128, 1)
        self.scales = nn.Sequential(
            nn.Conv2d(128, 128, 3, padding=1, stride=2),
            nn.BatchNorm2d(128),
            nn.ReLU(),
            nn.Conv2d(128, 256, 3, padding=1),
        )

    def forward(self, pillars, coordinates, batch_size=1):
        # pillars: (num_points, channels)
        x = self.pillar_encode(pillars.t())  # (channels, num_points)
        x = x.unsqueeze(0).unsqueeze(0)  # placeholder
        return x


class Fusion3DDetector(nn.Module):
    """融合检测器：PointPillars + 图像"""

    def __init__(self):
        super().__init__()

        # 点云分支
        self.pillar_backbone = PointPillarsBackbone()

        
        .image_backbone = nn.Sequential(
            nn.Conv2d(, , , stride=, padding=),
            nn.BatchNorm2d(),
            nn.ReLU(),
            nn.Conv2d(, , , stride=, padding=),
            nn.Conv2d(, , , padding=),
        )

        
        .fusion_conv = nn.Conv2d( + , , , padding=)

        
        .bbox_head = nn.Conv2d(, , )  
        .conf_head = nn.Conv2d(, , )

     ():
        
        lidar_feat = .pillar_backbone(pillars)

        
        img_feat = .image_backbone(image)

        
        img_feat = torch.nn.functional.interpolate(
            img_feat, size=lidar_feat.shape[-:], mode=
        )

        
        fused = torch.cat([lidar_feat, img_feat], dim=)
        fused = .fusion_conv(fused)

        
        bbox = .bbox_head(fused)
        confidence = torch.sigmoid(.conf_head(fused))

         {: bbox, : confidence}


 ():
    
    
    pillars = preprocess_pointcloud(pcd)  
    img_tensor = torch.from_numpy(image).permute(, , ).unsqueeze().() / 

     torch.no_grad():
        outputs = detector(pillars, img_tensor)

     postprocess_detections(outputs)

Late Fusion（检测结果融合）

from dataclasses import dataclass
from typing import List


@dataclass
class Detection2D:
    bbox: List[float]  # [x1, y1, x2, y2]
    score: float
    class_id: int


@dataclass
class Detection3D:
    position: np.ndarray  # 3D位置
    size: np.ndarray      # (h, w, l)
    orientation: float     # 朝向角
    score: float
    class_id: int
    associated_2d: Detection2D = None


class LateFusionDetector:
    """后融合方案：各自检测后再融合"""

    def __init__(self):
        self.lidar_detector = None  # 点云3D检测器
        self.image_detector = None  # 图像2D检测器

    def fuse(self, cloud, image) -> List[Detection3D]:
        # 1. 各自独立检测
        lidar_detections = self.lidar_detector.detect(cloud)  # List[Detection3D]
        image_detections = self.image_detector.detect(image)  # List[Detection2D]

        # 2. 几何关联：将2D框投影到3D空间
        fused = []
        for det_3d in lidar_detections:
            
            projected_2d = .project_3d_to_2d(det_3d)

            
            matched_2d = .match_2d(projected_2d, image_detections)

             matched_2d:
                det_3d.associated_2d = matched_2d
                det_3d.score =  * det_3d.score +  * matched_2d.score  
                det_3d.class_id = matched_2d.class_id  

            fused.append(det_3d)

         fused

     () -> []:
        
        corners = .compute_box_corners(det)
        uv = .projector.project(corners)
        x1, y1 = uv.(axis=)
        x2, y2 = uv.(axis=)
         [x1, y1, x2, y2]

     () -> Detection2D:
        
        best_iou = 
        best_det = 
         det  image_dets:
            iou = .compute_iou(projected, det.bbox)
             iou > best_iou:
                best_iou = iou
                best_det = det
         best_det


     ():
        x1 = (box1[], box2[])
        y1 = (box1[], box2[])
        x2 = (box1[], box2[])
        y2 = (box1[], box2[])
        inter = (, x2 - x1) * (, y2 - y1)
        area1 = (box1[] - box1[]) * (box1[] - box1[])
        area2 = (box2[] - box2[]) * (box3[] - box2[])
         inter / (area1 + area2 - inter)

---

ROS2 融合节点

融合检测发布节点

#!/usr/bin/env python3
"""激光-相机融合3D检测节点"""

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, PointCloud2
from vision_msgs.msg import Detection3DArray
import numpy as np
import torch


class FusionDetectionNode(Node):
    def __init__(self):
        super().__init__('fusion_detection')

        # 参数
        self.declare_parameter('conf_threshold', 0.3)
        self.declare_parameter('nms_iou_threshold', 0.5)

        # 融合检测器
        self.detector = Fusion3DDetector()
        # self.detector.load_weights('/path/to/weights.pth')

        # 订阅
        self.cloud_sub = self.create_subscription(
            PointCloud2, '/velodyne_points', self.cloud_callback, 10
        )
        self.image_sub = self.create_subscription(
            Image, '/camera/color/image_raw', self.image_callback, 10
        )

        # 发布
        self.det_pub = self.create_publisher(
            Detection3DArray, , 
        )

        .get_logger().info()

     ():
        
        .latest_cloud = msg

     ():
        
          (, ):
            

        
        cloud = .pointcloud2_to_array(.latest_cloud)
        image = .image_to_array(msg)

        
        detections = .detector.detect(cloud, image)

        
        det_array = .to_ros_msg(detections)
        .det_pub.publish(det_array)

     () -> np.ndarray:
        
         sensor_msgs.py3  point_cloud
        pc = point_cloud.read_points(cloud, field_names=(, , ), skip_nans=)
         np.array((pc), dtype=np.float32)

     () -> np.ndarray:
         cv_bridge  CvBridge
        bridge = CvBridge()
         bridge.imgmsg_to_cv2(image, desired_encoding=)

     ():
        
        msg = Detection3DArray()
         det  detections:
            det3d = Detection3D()
            det3d.bbox.center.position.x = det.position[]
            det3d.bbox.center.position.y = det.position[]
            det3d.bbox.center.position.z = det.position[]
            det3d.bbox.size.x, det3d.bbox.size.y, det3d.bbox.size.z = det.size
            msg.detections.append(det3d)
         msg


 ():
    rclpy.init(args=args)
    node = FusionDetectionNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


 __name__ == :
    main()

---

故障排查

问题	原因	解决方案
投影偏差大	外参标定错误	重新标定，验证 R/t 矩阵
深度图有空洞	点云稀疏	使用双边滤波插值或上采样
融合检测漏检	同步失败	检查时间戳同步，添加缓冲
内存爆炸	点云处理过大	体素化下采样（voxel_size=0.1）
GPU 显存不足	模型太大	减小 batch size，量化 INT8

调试命令

# 查看投影效果
ros2 run image_view image_view image:=/lidar_projected_image

# 标定结果验证
ros2 run calibration_camera_lidar viewExtrinsics

# 点云和图像同步检查
ros2 topic hz /velodyne_points /camera/color/image_raw

# RViz 可视化
ros2 run rviz2 rviz2 -d fusion.rviz
# 添加: PointCloud2 + Image + Detection3D

---

相关技能

• perception/kalman-filtering — 卡尔曼滤波状态估计
• perception/lidar-perception — 激光雷达感知
• perception/vision-perception — 视觉感知
• perception/sensor-fusion/multi-object-tracking — 多目标跟踪
• perception/sensor-fusion/spatial-temporal-sync — 时空同步
• perception/edge-inference/tensorrt-deployment — TensorRT 推理加速