网站浏览构思安卓优化大师2023

一、算法原理

1、轴角表示法

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

• 旋转轴：单位向量 $\mathbf{v}=(v_x,v_y,v_z)$，满足 $||\mathbf{v}||=1$
• 旋转角度：$\theta$（弧度制）
  数学表示为：$(\mathbf{v},\theta )$

2、旋转矩阵推导

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

• $\mathbf{I}$ 是3×3单位矩阵
• $[\mathbf{v}{]}_{\times }$ 是旋转轴的叉乘矩阵：
  $$[\mathbf{v}{]}_{\times }=\left[\begin{array}{ccc}0& -v_z& v_y\\ v_z& 0& -v_x\\ -v_y& v_x& 0\end{array}\right]$$

展开后得到完整旋转矩阵：
$$\mathbf{R}=\left[\begin{array}{ccc}\cos \theta +v_x^2(1-\cos \theta )& v_x{v}_y(1-\cos \theta )-v_z\sin \theta & v_x{v}_z(1-\cos \theta )+v_y\sin \theta \\ v_y{v}_x(1-\cos \theta )+v_z\sin \theta & \cos \theta +v_y^2(1-\cos \theta )& v_y{v}_z(1-\cos \theta )-v_x\sin \theta \\ v_z{v}_x(1-\cos \theta )-v_y\sin \theta & v_z{v}_y(1-\cos \theta )+v_x\sin \theta & \cos \theta +v_z^2(1-\cos \theta )\end{array}\right]$$

数学验证

当 $\theta =0$ 时：
$$\mathbf{R}=\mathbf{I}\text{(单位矩阵)}$$
当 $\theta =\pi$ 时：
$$\mathbf{R}=2\mathbf{v}{\mathbf{v}}^T-\mathbf{I}\text{(绕轴旋转180°)}$$

应用场景

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

3、点云旋转公式

  对点云中任意点 $\mathbf{p}=(x,y,z)$，旋转后坐标：
$${\mathbf{p}}^{\prime }=\mathbf{R}\cdot \mathbf{p}+\mathbf{t}$$
其中 $\mathbf{t}$ 是平移向量（若需平移）

二、代码实现

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

三、结果展示