刚体变换的MATLAB代码

以下是一个在MATLAB中执行刚体变换的示例代码：

% 定义初始位置和旋转
p0 = [1; 2; 3];
R0 = [1 0 0; 0 1 0; 0 0 1];

% 定义位移向量和旋转矩阵
t = [4; 5; 6];
theta = pi/4;
axis = [1; 1; 0];
R = RodriguesRotation(axis, theta);

% 执行刚体变换
p = R*p0 + t;

function R = RodriguesRotation(axis, theta)
% 计算罗德里格斯公式
K = [0 -axis(3) axis(2); axis(3) 0 -axis(1); -axis(2) axis(1) 0];
R = eye(3) + sin(theta)*K + (1-cos(theta))*K*K;
end

% 输出结果
disp(p);