PCL 四元数转轴角

一、算法原理

   四元数 $q=(w,x,y,z)$ 表示旋转时，其中 $w$ 是实部，$(x,y,z)$ 是虚部。将其转换为轴角表示（旋转轴 $\mathbf{u}$ 和旋转角 $\theta$）的推导过程如下：

1. 角度提取：
   旋转角度 $\theta$ 与四元数实部的关系为：
   $$\theta =2\arccos (w)$$
   这里 $\arccos$ 的值域为 $[0,\pi ]$，因此 $\theta \in [0,2\pi ]$。

2. 旋转轴提取：
   旋转轴由四元数的虚部归一化得到：
   $$\mathbf{u}=\frac{(x,y,z)}{\Vert (x,y,z)\Vert }$$
   根据四元数性质 $\Vert q\Vert =1$，虚部的模长为：
   $$\Vert (x,y,z)\Vert =\sqrt{1-w^2}=\sin (\theta /2)$$
   因此旋转轴可表示为：
   $$\mathbf{u}=\frac{(x,y,z)}{\sin (\theta /2)}$$

3. 特殊情况处理：
   当 $w=\pm 1$ 时（对应 $\theta =0$ 或 $2\pi$），有 $\sin (\theta /2)=0$，此时旋转轴不唯一。按照惯例选择任意单位向量（通常取 $(1,0,0)$）：
   $$\mathbf{u}=\{\begin{array}{ll}\frac{(x,y,z)}{\sin (\theta /2)}& \text{if }|w|<1\\ (1,0,0)& \text{if }w=\pm 1\end{array}$$

关键说明

1. 归一化必要性：输入四元数必须归一化，未归一化的四元数会导致错误结果
2. 角度范围：返回的 $\theta \in [0,2\pi ]$，实际应用时可能需要映射到 $[-\pi ,\pi ]$
3. 死区处理：当 $\sin (\theta /2)\approx 0$ 时（阈值 $ϵ=10^{-6}$），启用默认轴
4. 坐标系一致性：PCL使用右手坐标系，旋转方向遵循右手定则

数学验证方法
验证转换正确性的方法：

1. 往返验证：轴角→四元数→轴角，检查结果一致性
2. 旋转不变性：选择一个测试向量 $v$，验证 $qv{q}^{-1}=R(\theta ,\mathbf{u})v$
3. 迹验证：检查 $\text{tr}(R)=1+2\cos \theta$，其中 $R$ 是四元数对应的旋转矩阵

⚠️ 注意事项：

• 当 $\theta \approx 0$ 时旋转轴定义不唯一，此时对结果解释需谨慎
• 在SLAM系统中建议保留原始四元数，仅在需要时转换
• 频繁转换时注意浮点精度累积误差

---

二、代码实现

#include <Eigen/Geometry>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/eigen.h>
#include <corecrt_math_defines.h>
#include <pcl/common/transforms.h>void quaternionToAxisAngle(const Eigen::Quaternionf& q, Eigen::Vector3f& axis, float& angle_rad)
{// 四元数归一化（确保单位四元数）Eigen::Quaternionf q_norm = q.normalized();// 计算旋转角度angle_rad = 2.0f * std::acos(q_norm.w());// 计算 sin(θ/2)const float sin_half_angle = std::sqrt(1 - q_norm.w() * q_norm.w());const float epsilon = 1e-6f;  // 浮点精度阈值// 处理奇异情况（无旋转或全旋转）if (std::abs(sin_half_angle) < epsilon) {axis = Eigen::Vector3f::UnitX();  // 默认选择X轴}else {// 虚部归一化得到旋转轴axis = Eigen::Vector3f(q_norm.x(), q_norm.y(), q_norm.z()) / sin_half_angle;}
}// 示例使用
int main() 
{// 创建绕Y轴旋转45°的四元数Eigen::Quaternionf q(Eigen::AngleAxisf(M_PI / 4, Eigen::Vector3f::UnitY()));Eigen::Vector3f axis;float angle;quaternionToAxisAngle(q, axis, angle);std::cout << "Rotation angle: " << angle * 180 / M_PI << " degrees\n";std::cout << "Rotation axis: " << axis.transpose() << std::endl;return 0;
}

三、结果展示

Rotation angle: 45 degrees
Rotation axis: 0 1 0