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

PCL 点云旋转的轴角表示法

news 来源：原创 2025/6/22 16:05:47

一、算法原理

1、轴角表示法

在点云处理中，轴角旋转是核心的3D变换操作，其数学基础是罗德里格斯旋转公式（Rodrigues’ rotation formula）。该公式将旋转轴和角度转换为旋转矩阵，应用于点云中的每个点。

旋转轴：单位向量 $\mathbf{v} = (v_x, v_y, v_z)$ ，满足 $||\mathbf{v}||=1$
旋转角度： $\theta$ （弧度制）
数学表示为： $(\mathbf{v}, \theta)$

2、旋转矩阵推导

根据罗德里格斯公式，旋转矩阵 $\mathbf{R}$ 的推导如下：
$\mathbf{R} = \mathbf{I} + \sin\theta[\mathbf{v}]_\times + (1 - \cos\theta)[\mathbf{v}]_\times^2$
其中：

$\mathbf{I}$ 是3×3单位矩阵
$[\mathbf{v}]_\times$ 是旋转轴的叉乘矩阵：
$[\mathbf{v}]_\times = \begin{bmatrix} 0 & -v_z & v_y \\ v_z & 0 & -v_x \\ -v_y & v_x & 0 \end{bmatrix}$

展开后得到完整旋转矩阵：
$\mathbf{R} = \begin{bmatrix} \cos\theta + v_x^2(1-\cos\theta) & v_xv_y(1-\cos\theta) - v_z\sin\theta & v_xv_z(1-\cos\theta) + v_y\sin\theta \\ v_yv_x(1-\cos\theta) + v_z\sin\theta & \cos\theta + v_y^2(1-\cos\theta) & v_yv_z(1-\cos\theta) - v_x\sin\theta \\ v_zv_x(1-\cos\theta) - v_y\sin\theta & v_zv_y(1-\cos\theta) + v_x\sin\theta & \cos\theta + v_z^2(1-\cos\theta) \end{bmatrix}$

数学验证

当 $\theta=0$ 时：
$\mathbf{R} = \mathbf{I} \quad \text{(单位矩阵)}$
当 $\theta=\pi$ 时：
$\mathbf{R} = 2\mathbf{v}\mathbf{v}^T - \mathbf{I} \quad \text{(绕轴旋转180°)}$

应用场景

点云配准：在ICP算法中实现初始对齐
三维重建：多视角点云融合
机器人运动规划：工具坐标系变换
虚拟现实：动态场景旋转

3、点云旋转公式

对点云中任意点 $\mathbf{p} = (x, y, z)$ ，旋转后坐标：
$\mathbf{p}' = \mathbf{R} \cdot \mathbf{p} + \mathbf{t}$
其中 $\mathbf{t}$ 是平移向量（若需平移）

二、代码实现

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>// Eigen 几何模块
#include <pcl/common/common.h>using namespace std;int main(int argc, char** argv)
{Eigen::Vector3f v1(2, 4, 7), v2(7, 8, 9); // v1是旋转前的向量，v2是旋转后的向量float RotateRad = pcl::getAngle3D(v1, v2);// 获取旋转夹角Eigen::Vector3f RotateAxis = v1.cross(v2);//叉积，得到旋转轴Eigen::Matrix3f rotation_matrix = Eigen::Matrix3f::Identity();Eigen::AngleAxisf rotation_vector(RotateRad, RotateAxis.normalized()); // 注意：旋转轴必须为单位向量rotation_matrix = rotation_vector.toRotationMatrix();// 使用罗德里格斯公式得到旋转矩阵cout << "rotation matrix =\n" << rotation_matrix << endl;  return 0;
}