Orientation, Position, and Coordinate Convention

I want to change my camera pose from xyz coordinate system(where x-forward direction, y left and z up) to zyx coordinate system ( z-forward y- down and x left) My camera pose is described in an homogenous matrix. Let's take this matrix as an example:

Camera = [ 1 0 0 1,
           0 1 0 2,
           0 0 1 3,
           0 0 0 1]

i created this function which rotate the matrix but i'm not sure if it's working correctly. I mean the rotation needs to be supported the the correct translation right ? :

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;

end

% Example usage
pose = rotate_homogeneous_matrix(Camera, "Y", 90)
pose = rotate_homogeneous_matrix(pose, "Z", -90)

I'm only rotating the axes. Do I need to change the order of the translation vector element? Does this function implementation satisfy the target.

Answer 1

[Angelo Yeo]

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

[Zakaria Boussaid]

I am really very grateful for your help