PCL 轴角转四元数

一、算法原理

1、概述

   已知旋转轴 $\mathbf{u}=(u_x,u_y,u_z)$（单位向量）和旋转角度 $\theta$，根据四元数旋转原理， 轴角到对应的四元数 $q=(w,x,y,z)$的转换公式为：

$$q=\cos \left(\frac{\theta }{2}\right)+\sin \left(\frac{\theta }{2}\right)(u_x\mathbf{i}+u_y\mathbf{j}+u_z\mathbf{k})\text{\hspace{1em}(1)}$$

式(1)展开后得到四元数的四个分量：
$$\{\begin{array}{l}w=\cos \left(\frac{\theta }{2}\right)\\ x=u_x\sin \left(\frac{\theta }{2}\right)\\ y=u_y\sin \left(\frac{\theta }{2}\right)\\ z=u_z\sin \left(\frac{\theta }{2}\right)\end{array}\text{\hspace{1em}(2)}$$

可得四元数分量的模长为1：
$$\begin{array}{rl}\Vert q{\Vert }^2& =w^2+x^2+y^2+z^2\\ & ={\cos }^2\left(\frac{\theta }{2}\right)+{\sin }^2\left(\frac{\theta }{2}\right)(u_x^2+u_y^2+u_z^2)\\ & ={\cos }^2\left(\frac{\theta }{2}\right)+{\sin }^2\left(\frac{\theta }{2}\right)\cdot 1\\ & =1\end{array}\text{\hspace{1em}(3)}$$

   其中， $w$ 为四元数的实部，表示旋转角度的一半； $(x,y,z)$ 为虚部表示旋转轴的方向；当 $\theta =0$ 时，$q=(1,0,0,0)$ 表示无旋转。

2、注意事项

⚠️ 注意事项

1. 奇异值处理：当 $\Vert \mathbf{u}\Vert <ϵ$ 时返回单位四元数
2. 连续性：当 $\theta$ 从 $2{\pi }^{-}$ 到 $0^{+}$ 时，四元数 $w$ 分量需保持连续
3. 性能优化：在实时系统中可预先计算 $\sin /\cos$ 半角值

二、代码实现

#include <pcl/common/eigen.h>
#include <Eigen/Geometry>

Eigen::Quaternionf axisAngleToQuaternion(const Eigen::Vector3f& axis, float angle_rad) 
{
    // 确保旋转轴是单位向量
    Eigen::Vector3f normalized_axis = axis.normalized();
    
    // 计算半角三角函数值
    float half_angle = angle_rad / 2.0f;
    float cos_half = std::cos(half_angle);
    float sin_half = std::sin(half_angle);
    
    // 构造四元数 (w, x, y, z)
    return Eigen::Quaternionf(
        cos_half, 
        sin_half * normalized_axis.x(),
        sin_half * normalized_axis.y(),
        sin_half * normalized_axis.z()
    ).normalized(); // 最终归一化确保精度
}

// 示例使用
int main() 
{
    // 定义绕Y轴旋转45°
    Eigen::Vector3f axis(0, 1, 0);
    float angle_deg = 45.0f;
    float angle_rad = angle_deg * M_PI / 180.0f;
    
    Eigen::Quaternionf q = axisAngleToQuaternion(axis, angle_rad);
    
    std::cout << "Resulting quaternion: (w,x,y,z) = ("
              << q.w() << ", " << q.x() << ", " 
              << q.y() << ", " << q.z() << ")" << std::endl;
    return 0;
}

关键说明

1. 轴归一化：必须显式归一化输入旋转轴，防止非单位向量导致错误
2. 角度单位：输入角度需为弧度制，如使用度数需转换（deg * π/180）
3. 数值稳定性：当 $\theta \approx 0$ 时，直接返回单位四元数 $(1,0,0,0)$
4. 四元数归一化：最终调用.normalized()消除浮点计算误差

三、结果展示