PCL 欧拉角转轴角

一、算法原理

   轴角表示法使用旋转轴 $\mathbf{u}=(u_x,u_y,u_z)$ 和旋转角 $\theta$ 描述旋转。欧拉角转轴角的核心思想是：将三个欧拉旋转等效为绕单一轴的旋转。

推导步骤：

1. 欧拉角→旋转矩阵：
   给定欧拉角 $(\alpha ,\beta ,\gamma )$（Z-Y-X顺序），旋转矩阵为：
   $$R=R_z(\gamma )R_y(\beta )R_x(\alpha )$$
   其中基本旋转矩阵：
   R x ( α ) = [ 1 0 0 0 cos ⁡ α − sin ⁡ α 0 sin ⁡ α cos ⁡ α ] , R y ( β ) = [ cos ⁡ β 0 sin ⁡ β 0 1 0 − sin ⁡ β 0 cos ⁡ β ] , R z ( γ ) = [ cos ⁡ γ − sin ⁡ γ 0 sin ⁡ γ cos ⁡ γ 0 0 0 1 ] R_x(\alpha) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\alpha & -\sin\alpha \\ 0 & \sin\alpha & \cos\alpha \end{bmatrix}, \quad R_y(\beta) = \begin{bmatrix} \cos\beta & 0 & \sin\beta \\ 0 & 1 & 0 \\ -\sin\beta & 0 & \cos\beta \end{bmatrix}, \quad R_z(\gamma) = \begin{bmatrix} \cos\gamma & -\sin\gamma & 0 \\ \sin\gamma & \cos\gamma & 0 \\ 0 & 0 & 1 \end{bmatrix} Rx​(α)= ​100​0cosαsinα​0−sinαcosα​ ​,Ry​(β)= ​cosβ0−sinβ​010​sinβ0cosβ​ ​,Rz​(γ)= ​cosγsinγ0​−sinγcosγ0​001​ ​

2. 旋转矩阵→轴角：

   • 旋转角 $\theta$ 通过矩阵迹计算：
     $$\theta =\arccos \left(\frac{\text{tr}(R)-1}{2}\right)=\arccos \left(\frac{R_{11}+R_{22}+R_{33}-1}{2}\right)$$
   • 旋转轴 $\mathbf{u}$ 通过反对称矩阵求解：
     u = 1 2 sin ⁡ θ [ R 32 − R 23 R 13 − R 31 R 21 − R 12 ] , ∥ u ∥ = 1 \mathbf{u} = \frac{1}{2\sin\theta} \begin{bmatrix} R_{32} - R_{23} \\ R_{13} - R_{31} \\ R_{21} - R_{12} \end{bmatrix}, \quad \|\mathbf{u}\| = 1 u=2sinθ1​ ​R32​−R23​R13​−R31​R21​−R12​​ ​,∥u∥=1
   • 特殊情况处理：
     • 当 $\theta =0$ 时：任意轴均可（通常取 $(1,0,0)$）
     • 当 $\theta =\pi$ 时：需特殊计算 $u_x=\pm \sqrt{(R_{11}+1)/2}$ 等

⚠️ 注意：当 $\sin \theta \approx 0$ 时需数值稳定处理（如泰勒展开）。
优势：相比欧拉角，轴角表示无万向节死锁问题，且几何意义直观。

二、代码实现

#include 
#include 
#include 
#include 
#include 
#include 
#include 

// 欧拉角(弧度)转轴角表示
Eigen::AngleAxisd eulerToAxisAngle(double roll, double pitch, double yaw) 
{
    // 1. 欧拉角→旋转矩阵 (Z-Y-X顺序)
    Eigen::Matrix3f rotationMatrix;
    rotationMatrix =
        Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()) *
        Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) *
        Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX());

    // 2. 旋转矩阵→轴角
    Eigen::AngleAxisd axisAngle(rotationMatrix.cast<double>());

    // 3. 处理奇点情况 (θ接近0或π)
    if (axisAngle.angle() < 1e-7) 
    { // 零旋转
        axisAngle = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX());
    }
    else if (M_PI - axisAngle.angle() < 1e-7) 
    { // 180°旋转
        Eigen::Matrix3d R = rotationMatrix.cast<double>();
        Eigen::Vector3d u(
            sqrt((R(0, 0) + 1) / 2),
            sqrt((R(1, 1) + 1) / 2),
            sqrt((R(2, 2) + 1) / 2)
        );
        axisAngle = Eigen::AngleAxisd(M_PI, u.normalized());
    }
    return axisAngle;
}



int main()
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile("target_slice_4000.pcd", *cloud);

    double roll = 0.75;
    double pitch = 0.5;
    double yaw = 0.2;
    // 使用示例
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTran(new pcl::PointCloud<pcl::PointXYZ>);
    Eigen::AngleAxisd axisAngle = eulerToAxisAngle(roll, pitch, yaw);
    Eigen::Affine3f transform = Eigen::Translation3f(0, 0, 0) * axisAngle.cast<float>();
    pcl::transformPointCloud(*cloud, *cloudTran, transform);

    return 0;
}

三、结果展示