One thing to change in the function rotate_homogeneous_matrix: transformation matrices to be calculated before the transformed matrix. See my attemp to visualization after the fix. You can see the original coordinate was rotated 90' on Y axis of world coordinate.
Camera = [ 1 0 0 0;
0 1 0 0;
0 0 1 1;
0 0 0 1];
% Example usage
temp = Camera;
pose = rotate_homogeneous_matrix(temp, "Y", 90);
%% Let's see how transformation worked
figure;
p0 = Camera(1:3, end);
p1 = pose(1:3, end);
fig1 = plot3(p0(1), p0(2), p0(3), 'r*');
hold on;
plot3(p1(1), p1(2), p1(3), 'b*');
text(p0(1)+0.2, p0(2)+0.2, p0(3)+0.2,'original')
text(p1(1)+0.2, p1(2)+0.2, p1(3)+0.2,'transformed')
ax = gca;
quiverSize = max([range(ax.XLim) range(ax.YLim) range(ax.ZLim)])/5;
quiver3(p0(1), p0(2), p0(3), Camera(1, 1), Camera(2, 1), Camera(3, 1), quiverSize ,'SeriesIndex', 1, 'LineWidth',1.5);
quiver3(p0(1), p0(2), p0(3), Camera(1, 2), Camera(2, 2), Camera(3, 2), quiverSize ,'SeriesIndex', 2, 'LineWidth',1.5);
quiver3(p0(1), p0(2), p0(3), Camera(1, 3), Camera(2, 3), Camera(3, 3), quiverSize ,'SeriesIndex', 3, 'LineWidth',1.5);
quiver3(p1(1), p1(2), p1(3), pose(1, 1), pose(2, 1), pose(3, 1), quiverSize ,'SeriesIndex', 1, 'LineWidth',1.5);
quiver3(p1(1), p1(2), p1(3), pose(1, 2), pose(2, 2), pose(3, 2), quiverSize ,'SeriesIndex', 2, 'LineWidth',1.5);
quiver3(p1(1), p1(2), p1(3), pose(1, 3), pose(2, 3), pose(3, 3), quiverSize ,'SeriesIndex', 3, 'LineWidth',1.5);
% Adding axis and legend info
fig1.Parent.XLabel.String = "X";
fig1.Parent.YLabel.String = "Y";
fig1.Parent.ZLabel.String = "Z";
fig1.Parent.XLim = [min(min(p0,p1))-1 max(max(p0,p1))+1];
fig1.Parent.YLim = [min(min(p0,p1))-1 max(max(p0,p1))+1];
fig1.Parent.ZLim = [min(min(p0,p1))-1 max(max(p0,p1))+1];
view([26.7891, 25.8500])
hold off
grid on
legend(ax,{'','','X','Y','Z'})
function rotated_matrix = rotate_homogeneous_matrix(original_matrix, axis, angle_degrees)
% Ensure the matrix is 4x4
if size(original_matrix) ~= [4, 4]
error('Input matrix must be a 4x4 homogeneous matrix');
end
% Convert angle to radians
angle_radians = deg2rad(angle_degrees);
% Create a copy of the matrix to avoid modifying the original
rotated_matrix = original_matrix;
% Define rotation matrices based on the specified axis
if axis == 'X'
rotation_matrix = [
1, 0, 0, 0;
0, cos(angle_radians), -sin(angle_radians), 0;
0, sin(angle_radians), cos(angle_radians), 0;
0, 0, 0, 1
];
elseif axis == 'Y'
rotation_matrix = [
cos(angle_radians), 0, sin(angle_radians), 0;
0, 1, 0, 0;
-sin(angle_radians), 0, cos(angle_radians), 0;
0, 0, 0, 1
];
elseif axis == 'Z'
rotation_matrix = [
cos(angle_radians), -sin(angle_radians), 0, 0;
sin(angle_radians), cos(angle_radians), 0, 0;
0, 0, 1, 0;
0, 0, 0, 1
];
else
error('Invalid axis. Use ''X'', ''Y'', or ''Z''.');
end
% Apply rotation
% rotated_matrix = rotated_matrix * rotation_matrix; % original
rotated_matrix = rotation_matrix * rotated_matrix;
end
