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

PCL 生成任意椭球点云

news 2025/6/29 6:13:37

一、算法原理

1、椭球定义

椭球是三维空间中的二次曲面，其标准方程为：
$\frac{x^2}{a^2} + \frac{y^2}{b^2} + \frac{z^2}{c^2} = 1\tag{1}$
式中， $(x, y, z)$ 是椭球表面点的坐标， $a, b, c$ 是三个半轴长度（分别对应x、y、z方向），中心点位于坐标原点 $(0, 0, 0)$ 。

2、任意位姿的椭球

实际应用中，椭球可能位于任意位置并具有任意旋转姿态。因此，椭球表面点的通用参数化方程为：
$p(\theta, \phi) = c + a\cos\theta\sin\phi \cdot \vec{u} + b\sin\theta\sin\phi \cdot \vec{v} + c\cos\phi \cdot \vec{w}\tag{2}$
式中， $\theta \in [0, 2\pi)$ ：方位角（xy平面内的角度）， $\phi \in [0, \pi]$ ：极角（从z轴正方向开始）。

3、推导步骤

建立局部坐标系：
$\begin{cases} \vec{u} = \frac{\text{给定方向向量}}{||\text{给定方向向量}||} \\ \vec{v} = \frac{\vec{u} \times \text{全局参考向量}}{||\vec{u} \times \text{全局参考向量}||} \quad (\text{如全局参考向量}(1,0,0)) \\ \vec{w} = \vec{u} \times \vec{v} \end{cases}$
将单位球面上的点 $x_s, y_s, z_s)$ 映射到椭球：
$\begin{pmatrix} x \\ y \\ z \end{pmatrix} = c + \begin{bmatrix} \vec{u} & \vec{v} & \vec{w} \end{bmatrix} \begin{pmatrix} a x_s \\ b y_s \\ c z_s \end{pmatrix}$

4、关键公式总结

概念	公式
方向向量归一化	$\vec{u} = \frac{\vec{u}_{\text{input}}}{\|\vec{u}_{\text{input}}\|}$
正交向量计算	$\vec{v} = \frac{\vec{u} \times \vec{r}}{\|\vec{u} \times \vec{r}\|}$
第三个轴向	$\vec{w} = \vec{u} \times \vec{v}$
表面点坐标	$a\cos\theta\sin\phi \cdot \vec{u} + b\sin\theta\sin\phi \cdot \vec{v} + c\cos\phi \cdot \vec{w}$

二、代码实现

#include <vector>
#include <iostream>
#include <corecrt_math_defines.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>int main() 
{// 定义椭球参数Eigen::Vector3f center(1.0, 2.0, 3.0); // 中心点float a = 2.0, b = 1.0, c = 1.5;       // 半轴长度Eigen::Vector3f u_dir(1, 1, 0);        // 初始方向向量// 计算正交基u_dir.normalize(); // u = u/||u||Eigen::Vector3f global_ref(1, 0, 0);Eigen::Vector3f v_dir = u_dir.cross(global_ref).normalized(); // v = (u×ref)/||u×ref||Eigen::Vector3f w_dir = u_dir.cross(v_dir); // w = u×v// 生成点云pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);const int num_points = 10000; // 点云密度for (int i = 0; i < num_points; ++i) {float theta = 2 * M_PI * rand() / RAND_MAX; // [0, 2π]float phi = M_PI * rand() / RAND_MAX;       // [0, π]// 计算点坐标Eigen::Vector3f point = center+ a * cos(theta) * sin(phi) * u_dir+ b * sin(theta) * sin(phi) * v_dir+ c * cos(phi) * w_dir;cloud->push_back(pcl::PointXYZ(point.x(), point.y(), point.z()));}boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));viewer->setWindowName("椭球点云");viewer->addPointCloud<pcl::PointXYZ>(cloud, "cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "cloud");while (!viewer->wasStopped()){viewer->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100000));}return 0;
}