点云向量 旋转 罗德里格斯（Rodrigues）变换Matlab

代码

旋转向量要求

步骤

已知点云的两个平面方程（即法向量），来进行点云的旋转平移变换，步骤如下：
1.通过法向量，来求旋转向量和旋转角
2.通过罗德里格斯（Rodrigues）法，将旋转向量变换为旋转矩阵，PS：旋转向量要满足：为单位向量，模为 θ（theta）；Rodrigues是定义的一个函数，matlab里没有，要自己生成，其代码在第一个链接里：代码链接
3.将点云左乘旋转矩阵：RX
下面的代码为旋转向量转旋转矩阵的代码，在点云坐标系转换中：A,B即为已知的两个平面的法向量，RV为所求的符合Rodrigues要求的旋转向量，R为所求的变换矩阵

% 求向量A和向量B的旋转矩阵
 clear
 A = [-0.9772    0.0953   -0.1896]';
 B = [-0.9937    0.0999    0.0501]';
 C = cross(B, A);
 theta = acos((A'*B) / ( norm(A)*norm(B) ));
 Rv = C / norm(C) * theta;
 R = rodrigues(Rv);