%Program entry script
close all; 
clear;
clc;

%1.Import and replace 3-D point clouds: model_clouds is the geometric model, original_clouds is sensor measurements
model_clouds = dlmread('santan_B.txt');
initial_clouds = dlmread('santan_A.txt');
initial_clouds(:,3) = initial_clouds(:,3) - 1000;
%2.Randomly generated test sparse point cloud
%id = randi([1,length(model_clouds)],1,30);
%original_clouds = model_clouds(1:30,:);
%original_clouds = model_clouds(id,:);

%3.Randomly rotate and move point cloud original_clouds to obtain the test data to be registered
%[initial_clouds, r_true, t_true] = randtrans(original_clouds); 
%r_true = r_true';
%t_true = -t_true;
%r_true
%t_true

%4.SPR solve
%R_ans and T_ans are the resultant rotation and translation to register initial_clouds to model_clouds
[R_res,T_res,res_clouds] = SPR(initial_clouds,model_clouds);
%output R,T
R_res
T_res
%5. Visualize
figure

scatter3(model_clouds(:,1), model_clouds(:,2), model_clouds(:,3),'.r')
hold on
scatter3(initial_clouds(:,1), initial_clouds(:,2), initial_clouds(:,3),'b','fill')
scatter3(res_clouds(:,1), res_clouds(:,2), res_clouds(:,3),'k','fill')
axis equal