当前位置：首页 > news >正文

PCL 计算点云的投影密度

news 2025/6/15 12:58:17

一、算法原理

1. 点云投影

设原始点云集合为：
$\{ \mathbf{p}_i \}_{i=1}^N, \quad \mathbf{p}_i = (x_i, y_i, z_i) \in \mathbb{R}^3$

投影到XY平面（可推广到任意平面）：
$P_{\text{proj}} = \{ (x_i, y_i) \}_{i=1}^N$

2. 网格划分

计算投影点集边界：
$\begin{cases} x_{\min} = \min\limits_{1 \leq i \leq N} x_i \\ x_{\max} = \max\limits_{1 \leq i \leq N} x_i \\ y_{\min} = \min\limits_{1 \leq i \leq N} y_i \\ y_{\max} = \max\limits_{1 \leq i \leq N} y_i \end{cases}$

定义网格尺寸 $s$ （用户参数），计算网格维度：
$\left\lceil \frac{x_{\max} - x_{\min}}{s} \right\rceil, \quad n = \left\lceil \frac{y_{\max} - y_{\min}}{s} \right\rceil$

3. 密度计算

对于网格单元 $(j, k)$ ：

区域范围： $R_{jk} = [x_{\min} + j \cdot s, x_{\min} + (j+1) \cdot s) \times [y_{\min} + k \cdot s, y_{\min} + (k+1) \cdot s)$
点数统计：
$C_{jk} = \sum_{i=1}^{N} \mathbf{1}_{R_{jk}}(x_i, y_i)$
其中 $\mathbf{1}$ 为指示函数：
$\mathbf{1}_{R_{jk}}(x,y) = \begin{cases} 1 & \text{if } (x,y) \in R_{jk} \\ 0 & \text{otherwise} \end{cases}$
点密度：
$D_{jk} = \frac{C_{jk}}{A}, \quad A = s^2 \quad (\text{单位面积})$

4. 完整流程

$\boxed{ \begin{array}{c} \text{输入：点云 } P, \text{ 网格尺寸 } s \\ \downarrow \\ \text{1. 投影：} P_{\text{proj}} = \{(x_i,y_i) | \forall \mathbf{p}_i \in P\} \\ \downarrow \\ \text{2. 边界计算：} x_{\min}, x_{\max}, y_{\min}, y_{\max} \\ \downarrow \\ \text{3. 网格划分：} m \times n \text{ 网格} \\ \downarrow \\ \text{4. 密度计算：} \\ D_{jk} = \dfrac{1}{s^2} \sum\limits_{i=1}^{N} \mathbf{1}_{[x_{\min}+js,\ x_{\min}+(j+1)s)}(x_i) \cdot \mathbf{1}_{[y_{\min}+ks,\ y_{\min}+(k+1)s)}(y_i) \\ \text{for } j=0,\dots,m-1; \ k=0,\dots,n-1 \\ \downarrow \\ \text{输出：密度矩阵 } \mathbf{D} = [D_{jk}]_{m \times n} \end{array} }$

5. 关键公式说明

网格索引映射：
$\left\lfloor \frac{x_i - x_{\min}}{s} \right\rfloor, \quad k = \left\lfloor \frac{y_i - y_{\min}}{s} \right\rfloor$
确保点落入正确网格单元
边界处理：
$\in [0, m-1], \quad k \in [0, n-1]$
超出范围的索引被丢弃
密度单位：
$D_{jk}$ 单位为 $\text{点}/\text{m}^2$ （当 $s$ 以米为单位时），用于量化局部点云密度

此方法通过降维投影和空间离散化，将三维密度计算转化为二维直方图统计，是点云分析的基础操作。

二、代码实现

#include <vector>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>// 计算投影密度
void computeProjectionDensity(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, float grid_size, std::vector<std::vector<float>>& density_grid)
{// 获取点云边界pcl::PointXYZ min_pt, max_pt;pcl::getMinMax3D(*cloud, min_pt, max_pt);// 计算网格维度（XY平面投影）int grid_x = ceil((max_pt.x - min_pt.x) / grid_size);int grid_y = ceil((max_pt.y - min_pt.y) / grid_size);// 初始化密度网格density_grid.resize(grid_x, std::vector<float>(grid_y, 0.0f));// 统计每个网格的点数for (const auto& point : cloud->points) {int x_idx = (point.x - min_pt.x) / grid_size;int y_idx = (point.y - min_pt.y) / grid_size;if (x_idx >= 0 && x_idx < grid_x && y_idx >= 0 && y_idx < grid_y) {density_grid[x_idx][y_idx] += 1.0f;}}// 计算密度（点数/单位面积）float cell_area = grid_size * grid_size;for (int i = 0; i < grid_x; ++i) {for (int j = 0; j < grid_y; ++j) {density_grid[i][j] /= cell_area;}}
}// 使用示例
int main() 
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile<pcl::PointXYZ>("E://data//道路.pcd", *cloud) == -1){PCL_ERROR("Cloudn't read file!");return -1;}std::vector<std::vector<float>> density_grid;computeProjectionDensity(cloud, 0.1f, density_grid); // 网格尺寸0.1m// 输出密度结果for (const auto& row : density_grid) {for (float density : row) {std::cout << density << " ";}std::cout << std::endl;}return 0;
}