Register the point cloud data sets

Hello,

Requesting help with point cloud registeration. In the below code, I have set 1-- random surface of point cloud generated. I have applied rotation to produce set 2. I am looking to register the set2 in set 1 cordinate system. What is the best approach to get the best fit? I tried eucleadian distance approach, obviously it lacks accuracy. Any help would be greatly appractied. Below is my attempt. Thank you!!

close all
clear
clc
%% 1. Random Data Generation 

    points =10; % Number of data points
    bc = 20; % should be even
    z_scalefactor=1000;

    %Random data gen and plot
    y=rand(points+bc, points+bc);
    figure(1)
    subplot(1,3,1)
    mesh(y)
    title("Randomly generated data")

   %% 2. Data Filtering
    %k is created for the convolution operation. 
      k= (1/bc^2)*ones(bc); 
    
     Ysmooth1 = conv2(y,k,'same'); %Applying Filter2
     subplot(1,3,2)
     mesh(Ysmooth1)
     title('Box carr filter ( filter2)')

%% 3. Surface Data Extraction

    for ii = (bc/2): points+bc/2 -1
        for jj=(bc/2): points+bc/2 - 1
            Zdata(ii-bc/2+1,jj-bc/2+1)=Ysmooth1(ii,jj);
        end
    end

 %% 4. Visualization of the Generated Data
    subplot(1,3,3)
    Surfaceplot = mesh(Zdata- mean(Zdata));
    title('Z data extraction to obtain the surface')
title('Z data representing the surface')

% Mean subtracted data obtained from above surface plot -mesh plot
generated_x= Surfaceplot.XData;
generated_y = Surfaceplot.YData;
generated_z = Surfaceplot.ZData;


% Flatten the arrays using nested loops
%	Convert Domain to 1d arrays 
index = 1;
for row = 1:length(generated_y) %rows
    for col = 1:length(generated_x)
        x_row_domain(index) = generated_x(col);
        y_row_domain(index) = generated_y(row);
        z_row_domain(index) = generated_z(row, col);
        index = index + 1;
    end
end


%% 7.Rotation of the Domain Data 

%5.1 Defining the rotation angles
alpha_rot =2.0*(pi/180); %Rotation angle in radians around X-axis
beta_rot =  2.0*(pi/180); %Rotation angle in radians around Y-axis
gama_rot =2.0*(pi/180); %Rotation angle in radians around Z-axis

%5.2 Creating rotation matrices 
Rx = [ 1      0               0;
          0  cos(alpha_rot)  -sin(alpha_rot);
          0  sin(alpha_rot)    cos(alpha_rot)] ;

Ry = [cos(beta_rot)  0  sin(beta_rot);
             0          1       0;     
        -sin(beta_rot)  0   cos(beta_rot) ];

Rz = [cos(gama_rot) -sin(gama_rot) 0;
         sin(gama_rot)   cos(gama_rot) 0;
               0                0             1];



%Reference Surface
% 5.3 Generating roi surface for rotation 
    domain_surface = [x_row_domain;y_row_domain;z_row_domain];

    %5.4 Trnasformation Matrix
      R_trans =Rz*Ry*Rx;

% 5.4 Applying Rotation
    rotated_surface = R_trans*domain_surface;

%5.5 Plotting
% z_row = z_row*z_scalefactor;
    fig2= figure(2);
    fig2.WindowState = 'maximized';
    subplot(1,2,1)
    scatter3(x_row_domain,y_row_domain,z_row_domain'.')
    title('ROI before rotation ')
    subplot(1,2,2)
    scatter3(rotated_surface(1,:),rotated_surface(2,:),rotated_surface(3,:),'*','r')
    title('Domain after applying rotation')


    %%
% Initializing ICP parameters
maxIterations = 110;
tolerance = 1e-6; % set the convergence criteria for ICP.



% Initializing rotation and trnaslation - transformation guesses
 
R = [1 0 0;   % Initializing rotation matrix guess- no rotation initially.
        0 1 0;
        0 0 1];

T=  [0;0;0]; % Initializing translation vector guess- no translation initially.




% This loop estimates the transformation (R and t) that minimizes the distance between corresponding points.

for iteration = 1:maxIterations   
    % Transform rotated_surface using current transformation
    transformed_surface = R * rotated_surface + T; %  represents the rotated and translated domain after
                                                                           % applying the current transformation.
    
     

%For each point in rotated_surface, find the closest point in domain_surface based on Euclidean distance
   [r,c] = size(domain_surface);
  [rr, cc]= size(rotated_surface);
    for i = 1:cc
        min_distance = 10;
        for j = 1:c
            distance = norm(rotated_surface(:, i) - domain_surface(:, j));
            if distance < min_distance
                min_distance = distance
                
                closest_indices(i) = j;
            end
        end
    end

      closest_points = domain_surface(:, closest_indices);


 %%   

    
    % Calculate centroids of both point clouds
    centroid_domain = mean(domain_surface, 2);
    centroid_closest = mean(closest_points, 2);
    
    % Compute covariance matrix
    H = (rotated_surface - centroid_domain) * (closest_points - centroid_closest)';
    
    % Perform Singular Value Decomposition
    [U, ~, V] = svd(H);
    
    % Calculate optimal rotation matrix and translation vector
    R_new = V * U';
    t_new = centroid_domain - R_new * centroid_closest;
    
    % Update transformation
    R = R_new * R;
    T = R_new * T + t_new;
    
    % Check for convergence
    if norm(t_new) < tolerance
        break;
    end
end

% Apply the final transformation to rotated_surface
final_transformed_surface = R * rotated_surface + T;


% Visualization of ICP Results
fig3= figure(3);
fig3.WindowState = 'maximized';
 scatter3(rotated_surface(1,:),rotated_surface(2,:),rotated_surface(3,:),'*','r')
hold on
scatter3(final_transformed_surface(1,:), final_transformed_surface(2,:), final_transformed_surface(3,:), 'o','b')
xlabel('X');
ylabel('Y');
zlabel('Z');
legend('Original Rotated Points', 'Transformed Domain Points');
title('ICP Registration Results');
grid on;

Answer 1

[Matt J]

Have you looked at this,

https://www.mathworks.com/help/vision/ug/3-d-point-cloud-registration-and-stitching.html#StitchPointCloudsExample-2

If you don't have the Computer Vision Toolbox, there are also File Exchange offerings, like this one,

https://www.mathworks.com/matlabcentral/fileexchange/12627-iterative-closest-point-method