如何申请网站,注册邮箱企业邮箱,py和php做网站哪个好,购物网站最近浏览怎么做目录 一、算法原理二、代码实现三、结果展示 一、算法原理 四元数 q ( w , x , y , z ) q (w, x, y, z) q(w,x,y,z) 表示旋转时#xff0c;其中 w w w 是实部#xff0c; ( x , y , z ) (x, y, z) (x,y,z) 是虚部。将其转换为轴角表示#xff08;旋转轴 u \mathbf{u} u… 目录 一、算法原理二、代码实现三、结果展示 一、算法原理 四元数 q ( w , x , y , z ) q (w, x, y, z) q(w,x,y,z) 表示旋转时其中 w w w 是实部 ( x , y , z ) (x, y, z) (x,y,z) 是虚部。将其转换为轴角表示旋转轴 u \mathbf{u} u 和旋转角 θ \theta θ的推导过程如下 角度提取 旋转角度 θ \theta θ 与四元数实部的关系为 θ 2 arccos ( w ) \theta 2 \arccos(w) θ2arccos(w) 这里 arccos \arccos arccos 的值域为 [ 0 , π ] [0, \pi] [0,π]因此 θ ∈ [ 0 , 2 π ] \theta \in [0, 2\pi] θ∈[0,2π]。 旋转轴提取 旋转轴由四元数的虚部归一化得到 u ( x , y , z ) ∥ ( x , y , z ) ∥ \mathbf{u} \frac{(x, y, z)}{\|(x, y, z)\|} u∥(x,y,z)∥(x,y,z) 根据四元数性质 ∥ q ∥ 1 \|q\|1 ∥q∥1虚部的模长为 ∥ ( x , y , z ) ∥ 1 − w 2 sin ( θ / 2 ) \|(x, y, z)\| \sqrt{1 - w^2} \sin(\theta/2) ∥(x,y,z)∥1−w2 sin(θ/2) 因此旋转轴可表示为 u ( x , y , z ) sin ( θ / 2 ) \mathbf{u} \frac{(x, y, z)}{\sin(\theta/2)} usin(θ/2)(x,y,z) 特殊情况处理 当 w ± 1 w \pm 1 w±1 时对应 θ 0 \theta 0 θ0 或 2 π 2\pi 2π有 sin ( θ / 2 ) 0 \sin(\theta/2) 0 sin(θ/2)0此时旋转轴不唯一。按照惯例选择任意单位向量通常取 ( 1 , 0 , 0 ) (1,0,0) (1,0,0) u { ( x , y , z ) sin ( θ / 2 ) if ∣ w ∣ 1 ( 1 , 0 , 0 ) if w ± 1 \mathbf{u} \begin{cases} \frac{(x,y,z)}{\sin(\theta/2)} \text{if } |w| 1 \\ (1,0,0) \text{if } w \pm 1 \end{cases} u{sin(θ/2)(x,y,z)(1,0,0)if ∣w∣1if w±1
关键说明
归一化必要性输入四元数必须归一化未归一化的四元数会导致错误结果角度范围返回的 θ ∈ [ 0 , 2 π ] \theta \in [0, 2\pi] θ∈[0,2π]实际应用时可能需要映射到 [ − π , π ] [-\pi, \pi] [−π,π]死区处理当 sin ( θ / 2 ) ≈ 0 \sin(\theta/2) \approx 0 sin(θ/2)≈0 时阈值 ϵ 10 − 6 \epsilon10^{-6} ϵ10−6启用默认轴坐标系一致性PCL使用右手坐标系旋转方向遵循右手定则
数学验证方法 验证转换正确性的方法
往返验证轴角→四元数→轴角检查结果一致性旋转不变性选择一个测试向量 v v v验证 q v q − 1 R ( θ , u ) v q v q^{-1} R(\theta, \mathbf{u}) v qvq−1R(θ,u)v迹验证检查 tr ( R ) 1 2 cos θ \text{tr}(R) 1 2\cos\theta tr(R)12cosθ其中 R R R 是四元数对应的旋转矩阵 ⚠️ 注意事项 当 θ ≈ 0 \theta \approx 0 θ≈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.hvoid 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
