【matlab数据处理】旋转三维空间中的一组点

要求：将空间中位于平面“-0.4y+z-35=0”上的点云旋转，使之与xoy平面平行。

平面上点的初始分布

 

编程思路：1.计算出点云所在平面的法向量a与xoy平面法向量b的外积，并转化为单位向量，作为旋转轴；计算出点云所在平面的法向量a与xoy平面法向量b的夹角，作为旋转角。

2.根据旋转轴和旋转角求解旋转矩阵R（基于罗德里格斯方法）。将某点左乘旋转矩阵R可得旋转之后点的位置。使用for循环依次计算出单个点旋转之后的位置，并组合成新的矩阵。

代码：

function [outputArg1,outputArg2] = rotate(matrix)
%ROTATE 输入参数：坐标矩阵，每一列代表空间中的一个点
num_cols = size(matrix,2); % 统计坐标矩阵的列数
plane_a = [0,-0.4,1]; % 点云所在处的平面法向量
plane_b = [0,0,1]; % xoy平面法向量
direction_vector = cross(plane_a,plane_b); % 计算平面法向量a与平面法向量b的外积，作为旋转轴  
unit_direction_vector = direction_vector/norm(direction_vector); % 转化为单位方向向量
unit_direction_vector = unit_direction_vector'; % 将单位方向向量转置成列向量
% 计算两向量的夹角
norm_a = norm(plane_a); % 求向量a的模
norm_b = norm(plane_b); % 求向量b的模
dot_vector = dot(plane_a, plane_b); % 计算点积
angle = acos(dot_vector/norm_a*norm_b);
% 计算旋转后的点坐标
m_initial = matrix(:,1); % 取坐标矩阵的第一列作为初始值
R = eye(3)*cos(angle)+(1-cos(angle))*(unit_direction_vector*unit_direction_vector')+sin(angle)*[0 -1*unit_direction_vector(3,1) unit_direction_vector(2,1);unit_direction_vector(3,1) 0 -1*unit_direction_vector(1,1);-1*unit_direction_vector(2,1) unit_direction_vector(1,1) 0]; %基于罗德里格斯旋转公式求解旋转矩阵
Vrot1 = R*m_initial;
% 单次计算只是旋转一个点。使用for循环计算多个点并组合成新的坐标矩阵，便于绘图
for i = 2:num_cols
  append = matrix(:,i);
  R = eye(3)*cos(angle)+(1-cos(angle))*(unit_direction_vector*unit_direction_vector')+sin(angle)*[0 -1*unit_direction_vector(3,1) unit_direction_vector(2,1);unit_direction_vector(3,1) 0 -1*unit_direction_vector(1,1);-1*unit_direction_vector(2,1) unit_direction_vector(1,1) 0]; %基于罗德里格斯旋转公式求解旋转矩阵
  Vrot_append = R*append;
  Vrot1 = horzcat(Vrot1,Vrot_append);
end
outputArg1 = Vrot1;
outputArg2 = angle;
end

在命令行中输入以下命令可获得旋转后的坐标信息和夹角。原始数据保存在矩阵“xyz”中。

>> [outputArg1,outputArg2] = rotate(xyz)

在命令行中输入以下命令可获得旋转后的图像。旋转后的数据保存在矩阵“ans”中。

>> scatter3(ans(1,:),ans(2,:),ans(3,:))
>> axis([-100 100 -100 100 0 80])

旋转之后的点云

 参考：

[1]罗德里格旋转公式、定轴旋转矩阵 - 小时百科

[2]罗德里格旋转公式_百度百科

[3]【matlab点云数据处理】平面点/点云旋转至平行于xoy平面_倾斜平面调整成平行于xoy平面-CSDN博客

[4]matlab cross()函数叉乘 计算过程详解-CSDN博客

[5]Matlab标准化（单位化）一个向量_matlab向量单位化-CSDN博客

[6]MATLAB中范数norm()函数精讲_matlab norm-CSDN博客