Register the point cloud data sets
3 views (last 30 days)
Show older comments
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;
0 Comments
Accepted Answer
Matt J
on 2 Sep 2023
Have you looked at this,
If you don't have the Computer Vision Toolbox, there are also File Exchange offerings, like this one,
0 Comments
More Answers (0)
See Also
Categories
Find more on Point Cloud Processing in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!