Good evening, we have a point cloud and have applied an first rotation to align the point cloud along the y-axis. Now we have a problem with a second rotation

clear all
close all 
clc
%%
movingReg=pcread('v65_ts40_repeatibility_four9.ply');
figure, pcshow(movingReg), view([0 0 ]), axis on, xlabel('x [mm]'), ylabel('y [mm]'), zlabel('z [mm]')

%% roi dove ci sono i  quattro fori 
roi = [360 540 -155 -130 40 115]; % DA CAD 
indices = findPointsInROI(movingReg,roi);
ptCloudB = select(movingReg,indices);
figure, pcshow(ptCloudB)

%% punto medio della nuvola di punti
centro=[mean(ptCloudB.Location(:,1)) mean(ptCloudB.Location(:,2)) mean(ptCloudB.Location(:,3))];

%% codice dato da Paolo per l'allineamento della nuvola 
XYZ=[ptCloudB.Location(:,1) ptCloudB.Location(:,2) ptCloudB.Location(:,3)];
xyz0=mean(XYZ,1);
A=XYZ-xyz0;
[~,~,V]=svd(A,0);
a=cross(V(:,3),[0;0;1]);
T=makehgtform('axisrotate', a, -atan2(norm(a),V(3,3)));
R=T(1:3,1:3);
A_rot = A*R;
% A_rot_pt=pointCloud(A_rot);
A_rot_p(:,1)=A_rot(:,1);
A_rot_p(:,3)=A_rot(:,2);
A_rot_p(:,2)=A_rot(:,3);
A_rot_p=pointCloud(A_rot_p);
figure, pcshow(A_rot_p),axis on, xlabel('x [mm]'), ylabel('y [mm]'), zlabel('z [mm]')


%% applico una seconda svd

XYZ2=[A_rot_p.Location(:,1) A_rot_p.Location(:,2) A_rot_p.Location(:,3)];
xyz0_2=mean(XYZ2,1);
A=XYZ2-xyz0_2;
[~,~,V]=svd(A,0);
a=cross(V(:,1),[0;1;0]);%cross(V(:,2),[0;1;0]);
% T=makehgtform('zrotate', -atan2(norm(a),V(2,1)));
if (V(1,2))>0
    T=makehgtform('axisrotate', [0 1 0], -atan2(norm(a),V(1,2)));
else
    T=makehgtform('axisrotate', [0 1 0], -atan2(norm(a),-V(1,2)));
end

[Umar]

Hi Alessandro,

It appears that you are calculating the rotation matrix based on the SVD, to apply the second rotation, you need to calculate the center of the selected point cloud using the mean of its locations, perform an initial alignment of the point cloud using SVD to obtain a rotation matrix. Then, apply the first rotation to the point cloud data. Afterwards, calculate second rotation based on another SVD operation to further align the point cloud and finally, determine the axis and angle of rotation for the second transformation based on specific conditions related to the SVD results. See below example code snippet.

% Calculate the center of the point cloud
center = mean(pointCloud, 1);
% Perform initial alignment using SVD
[U, ~, V] = svd((pointCloud - center)' * (referencePointCloud - mean(referencePointCloud, 1)));
% Apply the first rotation
rotatedPointCloud = (pointCloud - center) * V * U';
% Calculate second rotation using SVD
[U2, ~, V2] = svd(rotatedPointCloud' * referencePointCloud);
% Determine axis and angle of rotation for the second transformation
R = V2 * U2';

The above code snippet will help you effectively apply a second rotation to align a point cloud with another reference point cloud. Hope this answers your question.

Answer 1

[William Rose]

@Alessandro,

You can find the directions of the principal axes by eigenvalue decomposition of the covariance matrix of the points. The matrix of eigenvectors is itself a rotation matrix (assuming the eignvectors are scaled to unit length). You can use the matrix of eigenvectors to rotate the points so that the principal axes are aligned with the x,y,z axes. Example:

Make a set of N points in a rectangular prism and rotate them in 3D:

N=5000;
pts0=[3*rand(1,N); rand(1,N); rand(1,N)/3];     % N random points in rectangular prism 
ax0=[0,1,0,0; 0,0,1,0; 0,0,0,1];                % points to define unit vectors i,j,k, for plotting
                                                % note that i,j,k are the priniple axes of pts0,
                                                % due to the way we
                                                % constructed pts0.
% Rotate by 15d about x, then by 30 about y, then by 45 about z:
R1=rotm(so3([45,30,15]*pi/180,'eul'));  % default order=zyx: R1=Rz*Ry*Rx
pts1=R1*pts0;                           % rotate all points by R1
ax1=R1*ax0;                             % rotate i,j,k by R1

Find the principal axes of the rotated points:

[V,D]=eig(cov(pts1'));                  % columns of V are principal axes
[Ds,ind]=sort(diag(D),'descend');       % sort eigenvalues largest to smallest
Vs=V(:,ind);                            % sort eigenvectors same way

Vs above is a rotation matrix. It may not exactly match R1, due to the random distribution of points, but when N is large, the difference between R1 and Vs will be small (except maybe for a change of sign of which way the vectors point). To rotate the points so that the principal axes align with the x,y,z axes, rotate by the inverse of Vs. (For a rotation matrix, inverse equals transpose.) I will also rotate the rotated i,j,k, just for display purposes.

pts2=Vs'*pts1;
ax2=Vs'*ax1;

Done.

In this example, det(V)=-1, which means V is a rotation and a reflection. We don't like reflections. This can happen because the order of eigenvectors returned by eig() is not guaranteed to be in any specific order. In this example, the matrix Vs of eigenvectors, sorted in order of descending eigenvalue, has det(Vs)=1, which indicates that Vs is a rotation without a reflection. Therefore we use Vs, rather than V, to rotate the points into alignment. I recommend checking the determinant of V or Vs and adjusting as needed to avoid reflections.

Display the points, before and after rotation by Vs'. The code below plots pts1 (the tilted rectangular prism) and its principal axes (red, green, blue) on the left, and it plots pts2 (the points after rotation into alignment) on the right.

figure; 
subplot(121)
plot3(pts1(1,:),pts1(2,:),pts1(3,:),'.','Color',[.5,.5,.5]); hold on
plot3(ax1(1,[1,2]),ax1(2,[1,2]),ax1(3,[1,2]),'-r*','LineWidth',2)
plot3(ax1(1,[1,3]),ax1(2,[1,3]),ax1(3,[1,3]),'-g*','LineWidth',2)
plot3(ax1(1,[1,4]),ax1(2,[1,4]),ax1(3,[1,4]),'-b*','LineWidth',2)
hold off; axis equal; grid on
xlabel('X'); ylabel('Y'); zlabel('Z'); title('Pts1')
subplot(122)
plot3(pts2(1,:),pts2(2,:),pts2(3,:),'.','Color',[.5,.5,.5]); hold on
plot3(ax2(1,[1,2]),ax2(2,[1,2]),ax2(3,[1,2]),'-r*','LineWidth',2)
plot3(ax2(1,[1,3]),ax2(2,[1,3]),ax2(3,[1,3]),'-g*','LineWidth',2)
plot3(ax2(1,[1,4]),ax2(2,[1,4]),ax2(3,[1,4]),'-b*','LineWidth',2)
hold off; axis equal; grid on
xlabel('X'); ylabel('Y'); zlabel('Z'); title('Pts2')

Above left: Pts1=points of a tilted rectangular prism, and principal axes (RGB). Above right: Pts2=same points, after rotation to align principal axes with x,y,z. If you run the code in Matlab, you can select the 3D rotate tool in the plot window, then click and drag to see each plot from different angles.