% Project 3 — 3D Capture & Registration with Synthetic Data 
%{ Description: Work with synthetic 3D point clouds to learn how 3D scanning workflows function. 
% You'll create a point cloud, generate a transformed copy, then register them using ICP (Iterative Closest Point). 
% Finally, you'll merge the datasets and build a simple 3D surface. Skills: point cloud processing,
%  registration, 3D visualization %}

%% 1. Generate synthetic cube surface point cloud

n = 3000;                 % Number of random samples
X = rand(n,3);            % Uniform points in unit cube [0,1]^3

% Keep only points near the cube surfaces
mask = X(:,1)<0.05 | X(:,1)>0.95 | ...
       X(:,2)<0.05 | X(:,2)>0.95 | ...
       X(:,3)<0.05 | X(:,3)>0.95;

X = X(mask,:);            % Surface-only points
PC1 = pointCloud(X);      % Reference point cloud

% Visualize the synthetic cube
figure;
pcshow(PC1);
title('Synthetic Cube Surface Point Cloud');
xlabel('X'); ylabel('Y'); zlabel('Z');
axis equal;
%% 2. Create a transformed copy (second scan)

theta = deg2rad(15);      % Rotation angle (15 degrees)
Ry = [ cos(theta) 0 sin(theta);
       0          1 0;
      -sin(theta) 0 cos(theta) ];

t = [0.2 0.05 -0.1];      % Translation vector

% Apply transformation and add Gaussian noise
Y = (Ry * X')' + t + 0.005 * randn(size(X));
PC2 = pointCloud(Y);

% Visualize both scans together (misaligned)
figure;
pcshowpair(PC1, PC2);
title('Two Views Before Registration');
xlabel('X'); ylabel('Y'); zlabel('Z');
axis equal;
%% 3. Downsample point clouds

gridSize = 0.02;

PC1d = pcdownsample(PC1, 'gridAverage', gridSize);
PC2d = pcdownsample(PC2, 'gridAverage', gridSize);

% Visualize downsampled clouds
figure;
pcshowpair(PC1d, PC2d);
title('Downsampled Point Clouds (Before ICP)');
axis equal;
%% 4. Register point clouds using ICP

[tform, PC2reg, rmse] = pcregistericp( ...
    PC2d, PC1d, ...
    'Metric', 'pointToPlane', ...
    'MaxIterations', 60);

% Display estimated transformation
disp('Estimated transformation matrix:');
disp(tform.T);

fprintf('ICP RMSE: %.5f\n', rmse);
%% 5. Visualize ICP results

figure;

subplot(1,2,1);
pcshowpair(PC1d, PC2d);
title('Before ICP');
axis equal;

subplot(1,2,2);
pcshowpair(PC1d, PC2reg);
title(sprintf('After ICP (RMSE = %.4f)', rmse));
axis equal;
%% 6. Merge point clouds

PCmerged = pcmerge(PC1, PC2reg, 0.01);

figure;
pcshow(PCmerged);
title('Merged Point Cloud');
xlabel('X'); ylabel('Y'); zlabel('Z');
axis equal;
%% 7. Surface approximation using alpha shape (optional)

pts = PCmerged.Location;
shp = alphaShape(pts, 0.05);

figure;
plot(shp);
title('Reconstructed Surface (Alpha Shape)');
axis equal;
