Skip to content

Point Cloud Processing Fundamentals

Overview

A point cloud is a collection of discrete points in 3D space, where each point contains position coordinates \((x, y, z)\) and optional attributes (intensity, color, normals, etc.). Point cloud processing is the foundation of 3D LiDAR applications, covering core operations such as filtering, segmentation, registration, and feature extraction.

Point Cloud Data Structures

Basic Representation

A point cloud is essentially an \(N \times D\) matrix, where \(N\) is the number of points and \(D\) is the dimension per point:

\[ \mathbf{P} = \begin{bmatrix} x_1 & y_1 & z_1 & i_1 & \cdots \\ x_2 & y_2 & z_2 & i_2 & \cdots \\ \vdots & \vdots & \vdots & \vdots & \ddots \\ x_N & y_N & z_N & i_N & \cdots \end{bmatrix} \]

Organized vs. Unorganized Point Clouds

Type Shape Description Source
Unorganized \((N, D)\) No spatial ordering Most LiDARs
Organized \((H, W, D)\) Row-column structure like an image Ouster, depth cameras

Organized point clouds can leverage image processing methods (e.g., 2D convolution), and neighbor lookup is \(O(1)\).

Common File Formats

Format Extension Description
PCD .pcd PCL native format, supports ASCII/Binary
PLY .ply Stanford format, good compatibility
LAS/LAZ .las/.laz Aerial survey standard, LAZ is compressed
XYZ .xyz Plain text, simplest
numpy .npy/.npz Common in Python ecosystem

PCL (Point Cloud Library)

PCL is a comprehensive open-source point cloud processing library written in C++, and the standard choice for point cloud processing in the ROS ecosystem.

Installation

# Ubuntu
sudo apt install libpcl-dev

# For use in ROS2
sudo apt install ros-humble-pcl-ros ros-humble-pcl-conversions

Basic Operations

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>

// Create point cloud
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(
    new pcl::PointCloud<pcl::PointXYZI>);

// Read PCD file
pcl::io::loadPCDFile("input.pcd", *cloud);

// Basic info
std::cout << "Points: " << cloud->size() << std::endl;
std::cout << "Organized: " << cloud->isOrganized() << std::endl;

// Save
pcl::io::savePCDFileBinary("output.pcd", *cloud);

Voxel Grid Downsampling

Divides space into voxels (3D grid), replacing points within each voxel with their centroid:

\[ \mathbf{p}_{\text{voxel}} = \frac{1}{|\mathcal{V}|} \sum_{i \in \mathcal{V}} \mathbf{p}_i \]
#include <pcl/filters/voxel_grid.h>

pcl::VoxelGrid<pcl::PointXYZI> voxel_filter;
voxel_filter.setInputCloud(cloud);
voxel_filter.setLeafSize(0.1f, 0.1f, 0.1f);  // 10cm voxel

pcl::PointCloud<pcl::PointXYZI>::Ptr filtered(
    new pcl::PointCloud<pcl::PointXYZI>);
voxel_filter.filter(*filtered);

PassThrough Filter

Crops point cloud by coordinate range:

#include <pcl/filters/passthrough.h>

pcl::PassThrough<pcl::PointXYZI> pass;
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(-1.0, 3.0);  // Keep points with z in [-1, 3]
pass.filter(*filtered);

Statistical Outlier Removal

Removes outliers based on average distance from each point to its \(k\) nearest neighbors:

\[ d_i = \frac{1}{k} \sum_{j=1}^{k} \| \mathbf{p}_i - \mathbf{p}_{j} \| \]

If \(d_i > \mu + n \cdot \sigma\), the point is classified as an outlier.

#include <pcl/filters/statistical_outlier_removal.h>

pcl::StatisticalOutlierRemoval<pcl::PointXYZI> sor;
sor.setInputCloud(cloud);
sor.setMeanK(50);           // k nearest neighbors
sor.setStddevMulThresh(1.0); // n standard deviations
sor.filter(*filtered);

RANSAC Plane Fitting

Uses RANSAC (Random Sample Consensus) to detect planes from point clouds. Plane equation:

\[ ax + by + cz + d = 0 \]
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ModelCoefficients.h>

pcl::SACSegmentation<pcl::PointXYZI> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.05);  // Inlier distance threshold 5cm

pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);

// coefficients->values = {a, b, c, d}
std::cout << "Plane coefficients: " 
          << coefficients->values[0] << ", "
          << coefficients->values[1] << ", "
          << coefficients->values[2] << ", "
          << coefficients->values[3] << std::endl;

Normal Estimation

Estimates normal direction by fitting a plane to the local neighborhood of each point:

\[ \mathbf{n}_i = \text{eigenvector}_{\min}\left( \text{Cov}(\mathcal{N}_i) \right) \]

Where \(\mathcal{N}_i\) is the \(k\) nearest neighbors of point \(\mathbf{p}_i\), and \(\text{Cov}\) is the covariance matrix.

#include <pcl/features/normal_3d.h>

pcl::NormalEstimation<pcl::PointXYZI, pcl::Normal> ne;
ne.setInputCloud(cloud);

pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(
    new pcl::search::KdTree<pcl::PointXYZI>);
ne.setSearchMethod(tree);
ne.setKSearch(20);  // Use 20 neighbors

pcl::PointCloud<pcl::Normal>::Ptr normals(
    new pcl::PointCloud<pcl::Normal>);
ne.compute(*normals);

Euclidean Cluster Extraction

Distance-based clustering that segments a point cloud into separate objects:

#include <pcl/segmentation/extract_clusters.h>

pcl::EuclideanClusterExtraction<pcl::PointXYZI> ec;
ec.setClusterTolerance(0.5);  // Inter-point distance threshold 50cm
ec.setMinClusterSize(100);     // Minimum cluster size
ec.setMaxClusterSize(25000);   // Maximum cluster size
ec.setInputCloud(filtered);

std::vector<pcl::PointIndices> cluster_indices;
ec.extract(cluster_indices);

std::cout << "Found " << cluster_indices.size() << " clusters" << std::endl;

Open3D

Open3D is a Python/C++ 3D data processing library, particularly suited for visualization and rapid prototyping.

Installation

pip install open3d

Basic Operations

import open3d as o3d
import numpy as np

# Read point cloud
pcd = o3d.io.read_point_cloud("input.pcd")
print(f"Points: {len(pcd.points)}")

# Create from numpy
points = np.random.rand(10000, 3)
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points)

# Visualize
o3d.visualization.draw_geometries([pcd])

Voxel Downsampling

# Voxel downsampling
downsampled = pcd.voxel_down_sample(voxel_size=0.05)
print(f"Points after downsampling: {len(downsampled.points)}")

Normal Estimation and Visualization

# Normal estimation
pcd.estimate_normals(
    search_param=o3d.geometry.KDTreeSearchParamHybrid(
        radius=0.1, max_nn=30
    )
)

# Visualize (with normals)
o3d.visualization.draw_geometries(
    [pcd], point_show_normal=True
)

ICP Point Cloud Registration

ICP (Iterative Closest Point) aligns two point cloud frames:

\[ T^* = \arg\min_{T} \sum_{i} \| T \cdot \mathbf{p}_i^s - \mathbf{p}_{c(i)}^t \|^2 \]

Where \(T\) is the rigid body transformation and \(c(i)\) is the closest point correspondence.

# Point-to-point ICP
source = o3d.io.read_point_cloud("source.pcd")
target = o3d.io.read_point_cloud("target.pcd")

# Initial transform (can be from coarse registration or identity)
init_transform = np.eye(4)

# Execute ICP
result = o3d.pipelines.registration.registration_icp(
    source, target,
    max_correspondence_distance=0.5,
    init=init_transform,
    estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPoint(),
    criteria=o3d.pipelines.registration.ICPConvergenceCriteria(
        max_iteration=200
    )
)

print(f"Fitness: {result.fitness:.4f}")
print(f"RMSE: {result.inlier_rmse:.4f}")
print(f"Transformation matrix:\n{result.transformation}")

# Apply transformation
source.transform(result.transformation)

Point-to-Plane ICP

A higher-accuracy ICP variant that leverages normal information:

\[ T^* = \arg\min_{T} \sum_{i} \left( (T \cdot \mathbf{p}_i^s - \mathbf{p}_{c(i)}^t) \cdot \mathbf{n}_{c(i)}^t \right)^2 \]
# Normals must be estimated first
source.estimate_normals(
    search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
target.estimate_normals(
    search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))

result = o3d.pipelines.registration.registration_icp(
    source, target,
    max_correspondence_distance=0.5,
    init=init_transform,
    estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPlane()
)

RANSAC Plane Segmentation

# Plane segmentation
plane_model, inliers = pcd.segment_plane(
    distance_threshold=0.05,
    ransac_n=3,
    num_iterations=1000
)

[a, b, c, d] = plane_model
print(f"Plane equation: {a:.4f}x + {b:.4f}y + {c:.4f}z + {d:.4f} = 0")

# Separate plane and non-plane points
inlier_cloud = pcd.select_by_index(inliers)
outlier_cloud = pcd.select_by_index(inliers, invert=True)

# Color visualization
inlier_cloud.paint_uniform_color([1, 0, 0])   # Plane in red
outlier_cloud.paint_uniform_color([0, 1, 0])   # Non-plane in green
o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud])

DBSCAN Clustering

# DBSCAN clustering
labels = np.array(pcd.cluster_dbscan(
    eps=0.5, min_points=10, print_progress=True
))

max_label = labels.max()
print(f"Point cloud has {max_label + 1} clusters")

# Color visualization
colors = plt.get_cmap("tab20")(labels / (max_label + 1))
pcd.colors = o3d.utility.Vector3dVector(colors[:, :3])
o3d.visualization.draw_geometries([pcd])

ROS2 Point Cloud Processing Integration

PCL-ROS Conversion

#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/msg/point_cloud2.hpp>

// ROS2 PointCloud2 -> PCL
pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_cloud(
    new pcl::PointCloud<pcl::PointXYZI>);
pcl::fromROSMsg(*ros_msg, *pcl_cloud);

// PCL -> ROS2 PointCloud2
sensor_msgs::msg::PointCloud2 output_msg;
pcl::toROSMsg(*pcl_cloud, output_msg);
output_msg.header = ros_msg->header;
publisher->publish(output_msg);

Processing ROS2 Point Clouds in Python

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
import sensor_msgs_py.point_cloud2 as pc2
import numpy as np
import open3d as o3d

class PointCloudProcessor(Node):
    def __init__(self):
        super().__init__('pc_processor')
        self.sub = self.create_subscription(
            PointCloud2, '/lidar/points', self.callback, 10)

    def callback(self, msg: PointCloud2):
        # Convert to numpy
        points = pc2.read_points_numpy(
            msg, field_names=('x', 'y', 'z', 'intensity'))

        xyz = points[:, :3]

        # Convert to Open3D
        pcd = o3d.geometry.PointCloud()
        pcd.points = o3d.utility.Vector3dVector(xyz)

        # Downsample
        downsampled = pcd.voxel_down_sample(voxel_size=0.1)

        self.get_logger().info(
            f'Original: {len(xyz)} pts, Downsampled: {len(downsampled.points)} pts')

Point Cloud Processing Pipeline

graph TD
    A[Raw Point Cloud] --> B[Preprocessing]
    B --> C[Feature Extraction]
    C --> D[High-Level Tasks]

    B --> B1[Distance Filtering]
    B --> B2[Voxel Downsampling]
    B --> B3[Outlier Removal]
    B --> B4[Ground Removal]

    C --> C1[Normal Estimation]
    C --> C2[Keypoint Detection]
    C --> C3[Feature Descriptors<br>FPFH / SHOT]

    D --> D1[Point Cloud Registration<br>ICP / NDT]
    D --> D2[Semantic Segmentation<br>PointNet / PointNet++]
    D --> D3[Object Detection<br>PointPillars / CenterPoint]
    D --> D4[SLAM<br>LIO-SAM / FAST-LIO2]

Common Problems and Optimization

Problem Cause Solution
Slow processing Too many points Voxel downsampling, ROI cropping
Ground misdetected as obstacle Ground not removed RANSAC plane fitting or height-based filtering
Motion distortion Motion during LiDAR scan Motion compensation (using IMU or odometry)
Sparse at long range Point density decreases with distance Multi-frame accumulation, adaptive thresholds
High memory usage Large point cloud data Use float32, voxel filtering, streaming processing

References

  • PCL Official Documentation: https://pointclouds.org/documentation/
  • Open3D Documentation: https://www.open3d.org/docs/
  • ROS2 pcl_ros Package Documentation
  • 3D Point Cloud Analysis - Shan Liu et al.
  • PointNet Paper: Qi et al., CVPR 2017

评论 #