应用：1）求空间两向量的变换矩阵；2）求空间一直线的旋转矩阵。
一、问题描述：
- 已知三维空间中的源向量 p 和目标向量 q ，计算旋转矩阵 R ，使得源向量 p 在 旋转矩阵R 的作用下，能够与目标向量 q 对齐（平行）；
- 已知三维空间中的源向量 p 和目标向量 q ，以及一锚点a，计算变换矩阵T ，使得源向量 p 在矩阵 T 的作用下，能够与目标向量 q 对齐（平行），且矩阵 T 的作用不会改变锚点a 的位置（即 a 为旋转中心）；
- 已知三维空间中的两点 a 和 b，求得 a b 连线的中点及和向量 a b ⃗ \vec{ab}ab 相对于基坐标的旋转矩阵（和 1. 类似）。
二、解决方案推导
三、罗德里格斯公式
两种表达：
R = [ I + ( 1 − cos ( θ ) ) ∗ N 2 + sin ( θ ) ∗ N ] R = [I+(1-\cos (\theta)) * N^{2}+\sin (\theta) * N]R=[I+(1−cos(θ))∗N2+sin(θ)∗N]
R = cos ( θ ) ∗ I + ( 1 − cos ( θ ) ) ∗ n ∗ n T + sin ( θ ) ∗ n R = \cos (\theta) * I+(1-\cos (\theta)) * n * n^{T}+\sin (\theta) * nR=cos(θ)∗I+(1−cos(θ))∗n∗nT+sin(θ)∗n
式中，矩阵 N 是向量 n 的反对称矩阵形式，即
N = n ∧ = [ 0 − n z n y n z 0 − n x − n y n x 0 ] N = n^{\wedge} = \left[ \begin{matrix} 0 & -n_z & n_y \\ n_z & 0 & -n_x \\ -n_y & n_x & 0 \end{matrix} \right]N=n∧=⎣⎡0nz−ny−nz0nxny−nx0⎦⎤罗德里格斯公式，将3D旋转表达成了 ( n ∧ , θ ) (n^{\wedge}, \theta)(n∧,θ) 的形式，一般记作
ω = θ ∗ n = ( ω x , ω y , ω z ) T \omega = \theta * n = \left(\omega_{x}, \omega_{y}, \omega_{z}\right)^{T}ω=θ∗n=(ωx,ωy,ωz)T
式中，θ = ∣ q ∣ = q x 2 + q y 2 + q z 2 \theta=|q|=\sqrt{q^2_x+q^2_y+q^2_z}θ=∣q∣=qx2+qy2+qz2
这种表示旋转的形式非常简洁，但存在奇异问题，主要在于：- 旋转角度为 θ \thetaθ 和 θ + 2 k π \theta+2k\piθ+2kπ 的结果是一样的；
- ( n , θ ) (n,\theta)(n,θ) 和 ( − n , − θ ) (-n,-\theta)(−n,−θ) 的结果是一样的。
对于非常小的旋转，旋转矩阵 R 和 罗德里格斯向量 q 存在线性关系，推导如下：
R ( q ) = R ( n , θ ) = I + ( 1 − c o s θ ) ∗ N 2 + s i n θ ∗ N ≈ I + s i n θ ∗ N ≈ I + θ ∗ N = = I + q ∧ = [ 1 − q z q y q z 1 − q x − q y q x 1 ] \begin{aligned} & R(q)=R(n,θ)=I+(1−cosθ)∗N^2+sinθ∗N \\ & ≈I+sinθ∗N \\ & ≈I+θ∗N= \\ & = I+q^{\wedge} \\ & =\left[ \begin{matrix} 1 & -q_z & q_y \\ q_z & 1 & -q_x \\ -q_y & q_x & 1 \end{matrix} \right] \end{aligned}R(q)=R(n,θ)=I+(1−cosθ)∗N2+sinθ∗N≈I+sinθ∗N≈I+θ∗N==I+q∧=⎣⎡1qz−qy−qz1qxqy−qx1⎦⎤
四、代码实现
问题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::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);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：
本文参考文档均以超链接形式在文中给出。
以上内容根据自己理解和实践所写，如有错误，请批评指正。