PCL 点云旋转的轴角表示法

一、算法原理

1、轴角表示法

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

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

2、旋转矩阵推导

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

其中：

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

R = [ cos ⁡ θ + v x 2 ( 1 − cos ⁡ θ ) v x v y ( 1 − cos ⁡ θ ) − v z sin ⁡ θ v x v z ( 1 − cos ⁡ θ ) + v y sin ⁡ θ v y v x ( 1 − cos ⁡ θ ) + v z sin ⁡ θ cos ⁡ θ + v y 2 ( 1 − cos ⁡ θ ) v y v z ( 1 − cos ⁡ θ ) − v x sin ⁡ θ v z v x ( 1 − cos ⁡ θ ) − v y sin ⁡ θ v z v y ( 1 − cos ⁡ θ ) + v x sin ⁡ θ cos ⁡ θ + v z 2 ( 1 − cos ⁡ θ ) ] \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} R= cosθ+vx2(1−cosθ)vyvx(1−cosθ)+vzsinθvzvx(1−cosθ)−vysinθvxvy(1−cosθ)−vzsinθcosθ+vy2(1−cosθ)vzvy(1−cosθ)+vxsinθvxvz(1−cosθ)+vysinθvyvz(1−cosθ)−vxsinθcosθ+vz2(1−cosθ)

数学验证

当 θ = 0 \theta=0 θ=0 时：
R = I (单位矩阵) \mathbf{R} = \mathbf{I} \quad \text{(单位矩阵)} R=I(单位矩阵)

当 θ = π \theta=\pi θ=π 时：
R = 2 v v T − I (绕轴旋转180°) \mathbf{R} = 2\mathbf{v}\mathbf{v}^T - \mathbf{I} \quad \text{(绕轴旋转180°)} R=2vvT−I(绕轴旋转180°)

应用场景

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

3、点云旋转公式

对点云中任意点 p = ( x , y , z ) \mathbf{p} = (x, y, z) p=(x,y,z)，旋转后坐标：
p ′ = R ⋅ p + t \mathbf{p}' = \mathbf{R} \cdot \mathbf{p} + \mathbf{t} p′=R⋅p+t

其中 t \mathbf{t} t 是平移向量（若需平移）

二、代码实现

cpp 复制代码

#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;
}