如何用OpenCV/PCL可视化激光雷达点云？已用cv_bridge与RViz

Hey there! Let's break down how to visualize your LiDAR point cloud using PCL and OpenCV, building right on the ROS code you've already shared. I'll cover both 3D visualization with PCL and 2D visualization (like depth maps or projected points) with OpenCV.

1. 3D Visualization with PCL #

PCL (Point Cloud Library) is perfect for interactive 3D point cloud visualization. Here's how to integrate it into your existing ROS node:

First, make sure you have python-pcl installed (you can grab it via pip install python-pcl or your system's package manager). Then update your code like this:

import pcl
import pcl.pcl_visualization
import ros_numpy
import numpy as np
from sensor_msgs.msg import Image, PointCloud2
from cv_bridge import CvBridge
import message_filters

class YourNode:
    def __init__(self): 
        self.bridge = CvBridge() 
        self.depth_sub = message_filters.Subscriber("/os1_cloud_node/points", PointCloud2) 
        self.image_sub = message_filters.Subscriber("/pylon_camera_node/image_rect_color", Image) 
        ts = message_filters.ApproximateTimeSynchronizer([self.image_sub, self.depth_sub], 10, 1) 
        ts.registerCallback(self.callback) 

        # Initialize PCL visualizer once (avoid creating new windows every frame)
        self.pcl_vis = pcl.pcl_visualization.CloudViewing()
        self.first_frame = True

    def callback(self, color, depth): 
        # Your existing code to convert PointCloud2 to numpy array
        pc = ros_numpy.numpify(depth) 
        height = pc.shape[0] 
        width = pc.shape[1] 
        np_points = np.zeros((height * width, 3), dtype=np.float32) 
        np_points[:, 0] = np.resize(pc['x'], height * width) 
        np_points[:, 1] = np.resize(pc['y'], height * width) 
        np_points[:, 2] = np.resize(pc['z'], height * width)

        # Convert numpy array to PCL PointCloud
        cloud = pcl.PointCloud()
        cloud.from_array(np_points)

        # Update or initialize the visualization
        if self.first_frame:
            self.pcl_vis.ShowMonochromeCloud(cloud, b"lidar_cloud")
            self.first_frame = False
        else:
            self.pcl_vis.UpdateMonochromeCloud(cloud, b"lidar_cloud")
        
        # Keep the visualization window alive
        if not self.pcl_vis.WasStopped():
            self.pcl_vis.SpinOnce()

Notes for PCL Visualization: #

• This creates a persistent window that updates with each new point cloud frame.
• You can switch to colored clouds if your point cloud has RGB data (use ShowColorCloud instead).
• For extra features like axes or point size adjustments, check out PCL's visualization API.

2. 2D Visualization with OpenCV #

OpenCV is great for 2D representations, like pseudo-colored depth maps or projecting 3D points onto your camera image.

2.1 Pseudo-Colored Depth Map #

Since your point cloud's z value represents depth, you can convert this to a visually intuitive color map:

import cv2

def callback(self, color, depth): 
    # Your existing point cloud to numpy code
    pc = ros_numpy.numpify(depth) 
    height = pc.shape[0] 
    width = pc.shape[1] 

    # Extract depth values and reshape to 2D image
    depth_data = pc['z'].reshape(height, width)

    # Normalize depth to 0-255 (required for OpenCV's color maps)
    normalized_depth = cv2.normalize(depth_data, None, 0, 255, cv2.NORM_MINMAX, dtype=cv2.CV_8U)

    # Apply a color map (JET is common for depth visualization)
    colored_depth = cv2.applyColorMap(normalized_depth, cv2.COLORMAP_JET)

    # Display the result
    cv2.imshow("Colored Depth Map", colored_depth)
    cv2.waitKey(1)  # Keep the window responsive

2.2 Project Point Cloud onto Camera Image #

If you have your camera's intrinsic parameters (calibration data), you can project 3D point cloud points onto your 2D color image. This helps correlate LiDAR data with visual features:

First, you'll need your camera's intrinsic matrix K (you can get this from the /camera_info topic or your calibration files). Example intrinsic matrix:

# Replace with your actual camera intrinsics (fx, fy, cx, cy)
K = np.array([[1000.0, 0.0, 640.0],
              [0.0, 1000.0, 360.0],
              [0.0, 0.0, 1.0]], dtype=np.float32)

Then update your callback:

def callback(self, color, depth): 
    # Convert ROS image to OpenCV format
    cv_color = self.bridge.imgmsg_to_cv2(color, "bgr8")
    h, w = cv_color.shape[:2]

    # Your existing point cloud to numpy code
    pc = ros_numpy.numpify(depth) 
    height = pc.shape[0] 
    width = pc.shape[1] 
    np_points = np.zeros((height * width, 3), dtype=np.float32) 
    np_points[:, 0] = np.resize(pc['x'], height * width) 
    np_points[:, 1] = np.resize(pc['y'], height * width) 
    np_points[:, 2] = np.resize(pc['z'], height * width)

    # Filter out invalid points (z <= 0 means no depth data)
    valid_mask = np_points[:, 2] > 0
    valid_points = np_points[valid_mask]

    # Project 3D points to 2D image coordinates: uv = K * (X/Z, Y/Z, 1)^T
    uv_coords = np.dot(K, (valid_points.T / valid_points[:, 2].T)).T
    uv_coords = uv_coords[:, :2].astype(np.int32)

    # Filter points that fall outside the image bounds
    in_bounds = (uv_coords[:, 0] >= 0) & (uv_coords[:, 0] < w) & (uv_coords[:, 1] >= 0) & (uv_coords[:, 1] < h)
    valid_uv = uv_coords[in_bounds]

    # Draw points on the color image (green circles)
    for (u, v) in valid_uv:
        cv2.circle(cv_color, (u, v), 1, (0, 255, 0), -1)

    # Display the result
    cv2.imshow("LiDAR Points Projected on Image", cv_color)
    cv2.waitKey(1)

Notes for OpenCV Visualization: #

• Ensure your LiDAR and camera are spatially calibrated (extrinsic parameters) for accurate projection—without this, points won't align correctly with the image.
• Add cv2.destroyAllWindows() when shutting down your node to clean up OpenCV windows properly.

Final Tips #

• You can combine both visualization methods in the same callback if needed.
• For real-time performance, avoid redundant computations (like resizing arrays multiple times) and keep window updates efficient.

内容的提问来源于stack exchange，提问作者Priya Narayanan