罗德里格斯（Rodrigues）旋转公式原理及实现

应用：1）求空间两向量的变换矩阵；2）求空间一直线的旋转矩阵。

一、问题描述：

1. 已知三维空间中的源向量 p 和目标向量 q ，计算旋转矩阵 R ，使得源向量 p 在 旋转矩阵R 的作用下，能够与目标向量 q 对齐（平行）；
2. 已知三维空间中的源向量 p 和目标向量 q ，以及一锚点a，计算变换矩阵T ，使得源向量 p 在矩阵 T 的作用下，能够与目标向量 q 对齐（平行），且矩阵 T 的作用不会改变锚点a 的位置（即 a 为旋转中心）；
3. 已知三维空间中的两点 a 和 b，求得 a b 连线的中点及和向量 $ab$ 相对于基坐标的旋转矩阵（和 1. 类似）。

二、解决方案推导

1. 已知两向量，计算能够使两向量对齐的刚体变换矩阵
2. 罗德里格斯（Rodrigues）旋转公式推导
3. 罗德里格斯公式附图推导，理解

三、罗德里格斯公式

1. 两种表达：
   $$R=[I+(1-\cos (\theta ))\ast N^2+\sin (\theta )\ast N]$$
   $$R=\cos (\theta )\ast I+(1-\cos (\theta ))\ast n\ast n^T+\sin (\theta )\ast n$$
   式中，矩阵 N 是向量 n 的反对称矩阵形式，即
   $$N=n^{\wedge }=\left[\begin{array}{ccc}0& -n_z& n_y\\ n_z& 0& -n_x\\ -n_y& n_x& 0\end{array}\right]$$

2. 罗德里格斯公式，将3D旋转表达成了 $(n^{\wedge },\theta )$ 的形式，一般记作
   $$\omega =\theta \ast n={\left({\omega }_x,{\omega }_y,{\omega }_z\right)}^T$$
   式中，$\theta =|q|=\sqrt{q_x^2+q_y^2+q_z^2}$
   这种表示旋转的形式非常简洁，但存在奇异问题，主要在于：

   1. 旋转角度为 $\theta$ 和 $\theta +2k\pi$ 的结果是一样的；
   2. $(n,\theta )$ 和 $(-n,-\theta )$ 的结果是一样的。

   对于非常小的旋转，旋转矩阵 R 和 罗德里格斯向量 q 存在线性关系，推导如下：
   $$\begin{array}{rl}& R(q)=R(n,\theta )=I+(1-cos\theta )\ast N^2+sin\theta \ast N\\ & \approx I+sin\theta \ast N\\ & \approx I+\theta \ast N=\\ & =I+q^{\wedge }\\ & =\left[\begin{array}{ccc}1& -q_z& q_y\\ q_z& 1& -q_x\\ -q_y& q_x& 1\end{array}\right]\end{array}$$

四、代码实现

1. 问题1：
   方案一：利用罗德里格斯旋转公式求解：

   	//源 100，0，0
   	//目标 400，400，400 		
   	//锚点 0，0，0
   
   	Eigen::Vector3d point_anchor = { 0,0,0 };
   	double vec_src[3] = { 100,0,0 };
   	double vec_dst[3] = { 400,400,400 };
   	double axis_rotation[3] = { 0 };
   
   	vtkMath::Cross(vec_src, vec_dst, axis_rotation);
   
   	double norm = vtkMath::Norm(axis_rotation);
   
   	axis_rotation[0] = axis_rotation[0] / norm;
   	axis_rotation[1] = axis_rotation[1] / norm;
   	axis_rotation[2] = axis_rotation[2] / norm;
   
   	double angle = acos(vtkMath::Dot(vec_src, vec_dst) / (vtkMath::Norm(vec_src) * vtkMath::Norm(vec_dst)));
   
   	angle = angle * 180 / 3.1415926;
   
   	double angle2 = vtkMath::AngleBetweenVectors(vec_src, vec_dst);
   
   	Eigen::Matrix3d N;
   
   	N << 0, -axis_rotation[2], axis_rotation[1],
   		axis_rotation[2], 0, -axis_rotation[0],
   		-axis_rotation[1], axis_rotation[0], 0;
   
   	Eigen::Matrix3d R;
   
   	Eigen::Matrix3d I;
   
   	I.Identity();
   	
   	//罗德里格斯旋转公式
   	R = I + sin(angle) * N + (1 - cos(angle)) * N * N;
   
   	cout << R << angle;
   
   	Eigen::Matrix4d Matrix_Result;
   
   	Matrix_Result << R, -R * point_anchor + point_anchor,
   		0, 0, 0, 1;
   
   

   方案二：利用Eigen 库内封装的函数求解

   //创建两个向量
   Eigen::Vector3d VectorBefore(a, b, c);
   Eigen::Vector3d VectorAfter(x, y, z);
   
   //创建旋转矩阵、变换矩阵、位移向量
   Eigen::Matrix3d rotationMatrix;
   Eigen::Matrix4d transformMatrix;
   transformMatrix.setIdentity();
   Eigen::Vector3d t(0, 0, 0);
   
   //求两向量旋转矩阵
   rotationMatrix = Eigen::Quaterniond::FromTwoVectors(VectorBefore, VectorAfter).toRotationMatrix();
   
   //旋转矩阵和平移向量凑成变换矩阵
   transformMatrix.block<3, 3>(0, 0) = rotationMatrix;
   transformMatrix.topRightCorner(3, 1) = t;
   
   //用于点云变换
   pcl::transformPointCloud(*cloud_in, *cloud_out, transformMatrix);
   
   

2. 问题2：

   point_anchor = [10;20;30];			%  锚点
   vec_src = [sqrt(3)/3;sqrt(3)/3;sqrt(3)/3];		% 源向量
   vec_dst = [0;0;1];					% 目标向量
   axis_rotation = cross(vec_src,vec_dst);				% 叉积
   axis_rotation = axis_rotation/norm(axis_rotation);			% 单位法向量
   angle = acos(dot(vec_src,vec_dst));				% 计算旋转角
   axis_rotation_x = axis_rotation(1);
   axis_rotation_y = axis_rotation(2);
   axis_rotation_z = axis_rotation(3);
   N = [0,-axis_rotation_z,axis_rotation_y;		% 单位法向量对应的反对称矩阵形式
       axis_rotation_z,0,-axis_rotation_x;
       -axis_rotation_y,axis_rotation_x,0];
   R=eye(3)+sin(angle)*N+(1-cos(angle))*N*N;		% 根据罗德里格斯旋转公式求解得到的旋转矩阵
   
   T = [R,-R*point_anchor+point_anchor;			% 最终需要的刚体变换矩阵
       0,0,0,1]
   
   

3. 问题3：

本文参考文档均以超链接形式在文中给出。
以上内容根据自己理解和实践所写，如有错误，请批评指正。