Commit e23254bd authored by Qianfang Liao's avatar Qianfang Liao
Browse files

add matlab code

parent c188a6af
clc
clear
P_F = load('../data/fixedSet1.txt')'; % fixed point set P_F (3 x N_PF)
P_M = load('../data/movingSet1.txt')'; % moving point set P_M (3 x N_PM)
%%%%%% Initial pose setting for P_M
% r0 = -pi + 2*pi*rand(1,3);
% theta = norm(r0);
% P_M = axang2rotm([r0/theta, theta])*P_M;
%%%%%% Parameter setting
noisyF = 0; % 1/0 for the condition of fixed set with/without noisy outliers
noisyM = 0; % 1/0 for the condition of moving set with/without noisy outliers
trimRatio = 0; % trimming ratio for partially overlapping point sets
N_C = 50; % number of fuzzy clusters in coarse registration
gsF = 0.003; % grid step for downsampling fixed set (using box grid filter) for fuzzy clustering
gsM = 0.0019; % grid step for downsampling moving set (using box grid filter) for fuzzy clustering
gsF_fine = 0.0065; % grid step for downsampling fixed set (using box grid filter) for fine registration
gsM_fine = 0.0035; % grid step for downsampling moving set (using box grid filter) for fine registration
RRange = pi; % half side-length of the initial rotation cube [-RRange, RRange]^3 (radians)
TRange = 0.5; % half side-length of the initial translation cube [-TRange, TRange]^3
sigma_r = 0.01; % half side-length of the minimum rotation cube for stopping global search
sigma_t = TRange/2; % half side-length of the minimum translation cube for stopping inner search
epsilon = 0; % threshold value for stopping global search/inner search
%%%%%% Fuzzy Cluster-based Point Set Registration
[R,t,timeReg] = fuzzyPSReg(P_F,P_M,noisyF,noisyM,trimRatio,N_C,gsF,gsM,gsF_fine,gsM_fine,...
RRange,TRange,sigma_r,sigma_t,epsilon);
P_MRnT = R*P_M + t; % transformed moving set
%%%%%% Plot the point sets
figure
subplot(121)
hold on
title('initial poses');
plot3(P_F(3,:),P_F(1,:),P_F(2,:),'.r','MarkerSize',2);
plot3(P_M(3,:),P_M(1,:),P_M(2,:),'.b','MarkerSize',2);
subplot(122)
hold on
title('after registration');
plot3(P_F(3,:),P_F(1,:),P_F(2,:),'.r','MarkerSize',2);
plot3(P_MRnT(3,:),P_MRnT(1,:),P_MRnT(2,:),'.b','MarkerSize',2);
clc
clear
P_F = load('../data/fixedSet2.txt')'; % fixed point set P_F (3 x N_PF)
P_M = load('../data/movingSet2.txt')'; % moving point set P_M (3 x N_PM)
%%%%%% Initial pose setting for P_M
r0 = [0 0.5*pi 0];
theta = norm(r0);
P_M = axang2rotm([r0/theta, theta])*P_M;
%%%%%% Parameter setting
noisyF = 0; % 1/0 for the condition of fixed set with/without noisy outliers
noisyM = 0; % 1/0 for the condition of moving set with/without noisy outliers
trimRatio = 0; % trimming ratio for partially overlapping point sets
N_C = 80; % number of fuzzy clusters in coarse registration
gsF = 70; % grid step for downsampling fixed set (using box grid filter) for fuzzy clustering
gsM = 70; % grid step for downsampling moving set (using box grid filter) for fuzzy clustering
gsF_fine = 173; % grid step for downsampling fixed set (using box grid filter) for fine registration
gsM_fine = 194; % grid step for downsampling moving set (using box grid filter) for fine registration
RRange = pi; % half side-length of the initial rotation cube [-RRange, RRange]^3 (radians)
TRange = 0.2; % half side-length of the initial translation cube [-TRange, TRange]^3
sigma_r = 0.01; % half side-length of the minimum rotation cube for stopping global search
sigma_t = TRange/4; % half side-length of the minimum translation cube for stopping inner search
epsilon = 0; % threshold value for stopping global search/inner search
%%%%%% Fuzzy Cluster-based Point Set Registration
[R,t,timeReg] = fuzzyPSReg(P_F,P_M,noisyF,noisyM,trimRatio,N_C,gsF,gsM,gsF_fine,gsM_fine,...
RRange,TRange,sigma_r,sigma_t,epsilon);
P_MRnT = R*P_M + t; % transformed moving set
%%%%%% Plot the point sets
figure
subplot(121)
hold on
title('initial poses');
plot3(P_F(3,:),P_F(1,:),P_F(2,:),'.r','MarkerSize',2);
plot3(P_M(3,:),P_M(1,:),P_M(2,:),'.b','MarkerSize',2);
subplot(122)
hold on
title('after registration');
plot3(P_F(3,:),P_F(1,:),P_F(2,:),'.r','MarkerSize',2);
plot3(P_MRnT(3,:),P_MRnT(1,:),P_MRnT(2,:),'.b','MarkerSize',2);
clc
clear
P_F = load('../data/fixedSet3.txt')'; % fixed point set P_F (3 x N_PF)
P_M = load('../data/movingSet3.txt')'; % moving point set P_M (3 x N_PM)
%%%%%% Initial pose setting for P_M
r0 = -pi + 2*pi*rand(1,3);
theta = norm(r0);
P_M = axang2rotm([r0/theta, theta])*P_M;
%%%%%% Parameter setting
noisyF = 0; % 1/0 for the condition of fixed set with/without noisy outliers
noisyM = 0; % 1/0 for the condition of moving set with/without noisy outliers
trimRatio = 0.005; % trimming ratio for partially overlapping point sets
N_C = 60; % number of fuzzy clusters in coarse registration
gsF = 0.3; % grid step for downsampling fixed set (using box grid filter) for fuzzy clustering
gsM = 0.3; % grid step for downsampling moving set (using box grid filter) for fuzzy clustering
gsF_fine = 0.65; % grid step for downsampling fixed set (using box grid filter) for fine registration
gsM_fine = 0.55; % grid step for downsampling moving set (using box grid filter) for fine registration
RRange = pi; % half side-length of the initial rotation cube [-RRange, RRange]^3 (radians)
TRange = 0.2; % half side-length of the initial translation cube [-TRange, TRange]^3
sigma_r = 0.01; % half side-length of the minimum rotation cube for stopping global search
sigma_t = TRange/4; % half side-length of the minimum translation cube for stopping inner search
epsilon = 0; % threshold value for stopping global search/inner search
%%%%%% Fuzzy Cluster-based Point Set Registration
[R,t,timeReg] = fuzzyPSReg(P_F,P_M,noisyF,noisyM,trimRatio,N_C,gsF,gsM,gsF_fine,gsM_fine,...
RRange,TRange,sigma_r,sigma_t,epsilon);
P_MRnT = R*P_M + t; % transformed moving set
%%%%%% Plot the point sets
figure
subplot(121)
hold on
title('initial poses');
plot3(P_F(1,:),P_F(2,:),P_F(3,:),'.r','MarkerSize',2);
plot3(P_M(1,:),P_M(2,:),P_M(3,:),'.b','MarkerSize',2);
subplot(122)
hold on
title('after registration');
plot3(P_F(1,:),P_F(2,:),P_F(3,:),'.r','MarkerSize',2);
plot3(P_MRnT(1,:),P_MRnT(2,:),P_MRnT(3,:),'.b','MarkerSize',2);
clc
clear
P_F = load('../data/fixedSet4.txt')'; % fixed point set P_F (3 x N_PF)
P_M = load('../data/movingSet4.txt')'; % moving point set P_M (3 x N_PM)
%%%%%% Initial pose setting for P_M
r0 = -pi + 2*pi*rand(1,3);
theta = norm(r0);
P_M = axang2rotm([r0/theta, theta])*P_M;
%%%%%% Parameter setting
noisyF = 0; % 1/0 for the condition of fixed set with/without noisy outliers
noisyM = 0; % 1/0 for the condition of moving set with/without noisy outliers
trimRatio = 0.2; % trimming ratio for partially overlapping point sets
N_C = 80; % number of fuzzy clusters in coarse registration
gsF = 0.0018; % grid step for downsampling fixed set (using box grid filter) for fuzzy clustering
gsM = 0.0018; % grid step for downsampling moving set (using box grid filter) for fuzzy clustering
gsF_fine = 0.0044; % grid step for downsampling fixed set (using box grid filter) for fine registration
gsM_fine = 0.0044; % grid step for downsampling moving set (using box grid filter) for fine registration
RRange = pi; % half side-length of the initial rotation cube [-RRange, RRange]^3 (radians)
TRange = 0.5; % half side-length of the initial translation cube [-TRange, TRange]^3
sigma_r = 0.01; % half side-length of the minimum rotation cube for stopping global search
sigma_t = TRange/2; % half side-length of the minimum translation cube for stopping inner search
epsilon = 0; % threshold value for stopping global search/inner search
%%%%%% Fuzzy Cluster-based Point Set Registration
[R,t,timeReg] = fuzzyPSReg(P_F,P_M,noisyF,noisyM,trimRatio,N_C,gsF,gsM,gsF_fine,gsM_fine,...
RRange,TRange,sigma_r,sigma_t,epsilon);
P_MRnT = R*P_M + t; % transformed moving set
%%%%%% Plot the point sets
figure
subplot(121)
hold on
title('initial poses');
plot3(P_F(1,:),P_F(2,:),P_F(3,:),'.r','MarkerSize',2);
plot3(P_M(1,:),P_M(2,:),P_M(3,:),'.b','MarkerSize',2);
subplot(122)
hold on
title('after registration');
plot3(P_F(1,:),P_F(2,:),P_F(3,:),'.r','MarkerSize',2);
plot3(P_MRnT(1,:),P_MRnT(2,:),P_MRnT(3,:),'.b','MarkerSize',2);
clc
clear
P_F = load('../data/fixedSet5.txt')'; % fixed point set P_F (3 x N_PF)
P_M = load('../data/movingSet5.txt')'; % moving point set P_M (3 x N_PM)
%%%%%% Initial pose setting for P_M
% r0 = -pi + 2*pi*rand(1,3);
% theta = norm(r0);
% P_M = axang2rotm([r0/theta, theta])*P_M;
%%%%%% Parameter setting
noisyF = 1; % 1/0 for the condition of fixed set with/without noisy outliers
noisyM = 1; % 1/0 for the condition of moving set with/without noisy outliers
trimRatio = 0.2; % trimming ratio for partially overlapping point sets
N_C = 80; % number of fuzzy clusters in coarse registration
gsF = 0.0023; % grid step for downsampling fixed set (using box grid filter) for fuzzy clustering
gsM = 0.0023; % grid step for downsampling moving set (using box grid filter) for fuzzy clustering
gsF_fine = 0.0041; % grid step for downsampling fixed set (using box grid filter) for fine registration
gsM_fine = 0.0041; % grid step for downsampling moving set (using box grid filter) for fine registration
RRange = pi; % half side-length of the initial rotation cube [-RRange, RRange]^3 (radians)
TRange = 0.5; % half side-length of the initial translation cube [-TRange, TRange]^3
sigma_r = 0.01; % half side-length of the minimum rotation cube for stopping global search
sigma_t = TRange/4; % half side-length of the minimum translation cube for stopping inner search
epsilon = 0; % threshold value for stopping global search/inner search
%%%%%% Fuzzy Cluster-based Point Set Registration
[R,t,timeReg] = fuzzyPSReg(P_F,P_M,noisyF,noisyM,trimRatio,N_C,gsF,gsM,gsF_fine,gsM_fine,...
RRange,TRange,sigma_r,sigma_t,epsilon);
P_MRnT = R*P_M + t; % transformed moving set
%%%%%% Plot the point sets
figure
subplot(121)
hold on
title('initial poses');
plot3(P_F(1,:),P_F(2,:),P_F(3,:),'.r','MarkerSize',2);
plot3(P_M(1,:),P_M(2,:),P_M(3,:),'.b','MarkerSize',2);
subplot(122)
hold on
title('after registration');
plot3(P_F(1,:),P_F(2,:),P_F(3,:),'.r','MarkerSize',2);
plot3(P_MRnT(1,:),P_MRnT(2,:),P_MRnT(3,:),'.b','MarkerSize',2);
function [C,AFPCD,Pp] = fuzzyClustering(P,N_C,noisy) % fuzzy c-means clustering
N_P = size(P,2);
if ( N_C < 2 )||( N_C > N_P )
error('Incorrect setting for N_C. ');
end
ones1xNP = ones(1,N_P);
onesNCx1 = ones(N_C,1);
maxIter = 100; % maximum number of iteration
minDiff = 0; % minimum difference
obj = 0; % value of objective function (distance loss)
U = rand(N_C,N_P);
U = U./(onesNCx1*sum(U)); % random initialization for fuzzy membership matrix
dist2 = zeros(N_C,N_P);
for kIter = 1:maxIter
U2 = U.^2; % m = 2
rowSumU2 = sum(U2,2); % N_C x 1
C = P*U2'./([1 1 1]'*rowSumU2'); % updated fuzzy cluster centers
for i = 1:N_C
dist2(i,:) = sum((C(:,i)*ones1xNP - P).^2); % squared distance matrix
end
obj_1 = obj;
U2D2 = U2.*dist2;
obj = sum(U2D2,'all');
if ( kIter > 1 )&&( abs(obj - obj_1) <= minDiff )
break;
end
if kIter == maxIter
break;
end
temp = 1./dist2;
U = temp./(onesNCx1*sum(temp));
end
if noisy == 0
AFPCD = obj/N_P;
Pp = P;
else % noisy outlier pruning
%%%%%% first step
eta2 = sum(U2D2,2)./rowSumU2;
normD2 = dist2./(eta2*ones1xNP);
minND2 = min(normD2);
minND2_index = find(minND2 < 1);
N_Pp = size(minND2_index,2);
Pp = P(:,minND2_index);
U2D2p = U2D2(:,minND2_index);
obj_Pp = sum(U2D2p);
%%%%%% second step
pruneRatio = 0.15;
[obj_Pp_sort,Pp_index] = sort(obj_Pp);
N_Pp = round( N_Pp*(1-pruneRatio) );
AFPCD = sum(obj_Pp_sort(1:N_Pp))/N_Pp;
Pp = Pp(:,Pp_index(1:N_Pp)); % pruned point set
end
function [J,g] = fuzzyMetric(rt,C_F,C_M,N_CF,N_CM,N_CMTrim) % fuzzy cluster-based metric for registration
dist2 = zeros(N_CF,N_CM);
ones1xNCM = ones(1,N_CM);
r = rt(1:3);
theta = norm(r);
if theta == 0
theta = 10^(-6);
end
inv_theta = 1/theta;
sinth = sin(theta);
costh = cos(theta);
M = inv_theta*[0 -r(3) r(2);r(3) 0 -r(1);-r(2) r(1) 0];
R = [1 0 0;0 1 0;0 0 1] + M*sinth + M*M*(1 - costh);
t = rt(4:6);
C_MRnT = R*C_M + t*ones1xNCM;
for i = 1:N_CF
dist2(i,:) = sum((C_F(:,i)*ones1xNCM - C_MRnT).^2);
end
obj_CM = 1./sum(1./dist2);
if N_CM == N_CMTrim
J = sum(obj_CM);
else
[obj_CM_sort,CM_index] = sort(obj_CM);
J = sum(obj_CM_sort(1:N_CMTrim));
end
if nargout > 1
g = [0 0 0 0 0 0]';
dC_MRnT = [0 0 0;0 0 0;0 0 0;1 0 0;0 1 0;0 0 1];
ones1xNCF = ones(1,N_CF);
inv_theta2 = inv_theta*inv_theta;
term01 = inv_theta*sinth;
term02 = inv_theta2*(1 - costh);
term03 = inv_theta2*(costh - term01);
term04 = 2*inv_theta2*inv_theta2*(costh - 1) + inv_theta2*term01;
rxterm02 = r*term02;
if N_CM == N_CMTrim
for j = 1:N_CM
C_Mj = C_M(:,j);
rDotC_Mj = r(1)*C_Mj(1) + r(2)*C_Mj(2) + r(3)*C_Mj(3);
term05 = rDotC_Mj*term02;
term06 = rDotC_Mj*term04;
C_Mjxterm01 = C_Mj*term01;
term1_1 = r(1)*term06 - C_Mjxterm01(1) + (C_Mj(3)*r(2) - C_Mj(2)*r(3))*term03;
term1_2 = r(2)*term06 - C_Mjxterm01(2) + (C_Mj(1)*r(3) - C_Mj(3)*r(1))*term03;
term1_3 = r(3)*term06 - C_Mjxterm01(3) + (C_Mj(2)*r(1) - C_Mj(1)*r(2))*term03;
dC_MRnT(1,1) = r(1)*term1_1 + C_Mj(1)*rxterm02(1) + term05;
dC_MRnT(2,1) = r(2)*term1_1 + C_Mj(2)*rxterm02(1) + C_Mjxterm01(3);
dC_MRnT(3,1) = r(3)*term1_1 + C_Mj(3)*rxterm02(1) - C_Mjxterm01(2);
dC_MRnT(1,2) = r(1)*term1_2 + C_Mj(1)*rxterm02(2) - C_Mjxterm01(3);
dC_MRnT(2,2) = r(2)*term1_2 + C_Mj(2)*rxterm02(2) + term05;
dC_MRnT(3,2) = r(3)*term1_2 + C_Mj(3)*rxterm02(2) + C_Mjxterm01(1);
dC_MRnT(1,3) = r(1)*term1_3 + C_Mj(1)*rxterm02(3) + C_Mjxterm01(2);
dC_MRnT(2,3) = r(2)*term1_3 + C_Mj(2)*rxterm02(3) - C_Mjxterm01(1);
dC_MRnT(3,3) = r(3)*term1_3 + C_Mj(3)*rxterm02(3) + term05;
dd2 = 2*dC_MRnT*(C_MRnT(:,j)*ones1xNCF - C_F);
g = g + (obj_CM(j)^2)*(dd2*(1./dist2(:,j).^2));
end
else
for j = 1:N_CMTrim
j1 = CM_index(j);
C_Mj = C_M(:,j1);
rDotC_Mj = r(1)*C_Mj(1) + r(2)*C_Mj(2) + r(3)*C_Mj(3);
term05 = rDotC_Mj*term02;
term06 = rDotC_Mj*term04;
C_Mjxterm01 = C_Mj*term01;
term1_1 = r(1)*term06 - C_Mjxterm01(1) + (C_Mj(3)*r(2) - C_Mj(2)*r(3))*term03;
term1_2 = r(2)*term06 - C_Mjxterm01(2) + (C_Mj(1)*r(3) - C_Mj(3)*r(1))*term03;
term1_3 = r(3)*term06 - C_Mjxterm01(3) + (C_Mj(2)*r(1) - C_Mj(1)*r(2))*term03;
dC_MRnT(1,1) = r(1)*term1_1 + C_Mj(1)*rxterm02(1) + term05;
dC_MRnT(2,1) = r(2)*term1_1 + C_Mj(2)*rxterm02(1) + C_Mjxterm01(3);
dC_MRnT(3,1) = r(3)*term1_1 + C_Mj(3)*rxterm02(1) - C_Mjxterm01(2);
dC_MRnT(1,2) = r(1)*term1_2 + C_Mj(1)*rxterm02(2) - C_Mjxterm01(3);
dC_MRnT(2,2) = r(2)*term1_2 + C_Mj(2)*rxterm02(2) + term05;
dC_MRnT(3,2) = r(3)*term1_2 + C_Mj(3)*rxterm02(2) + C_Mjxterm01(1);
dC_MRnT(1,3) = r(1)*term1_3 + C_Mj(1)*rxterm02(3) + C_Mjxterm01(2);
dC_MRnT(2,3) = r(2)*term1_3 + C_Mj(2)*rxterm02(3) - C_Mjxterm01(1);
dC_MRnT(3,3) = r(3)*term1_3 + C_Mj(3)*rxterm02(3) + term05;
dd2 = 2*dC_MRnT*(C_MRnT(:,j1)*ones1xNCF - C_F);
g = g + (obj_CM_sort(j)^2)*(dd2*(1./dist2(:,j1).^2));
end
end
end
%%%%%% The main function
function [R,t,timeReg] = fuzzyPSReg(P_F,P_M,noisyF,noisyM,trimRatio,N_C,gsF,gsM,gsF_fine,gsM_fine,...
RRange,TRange,sigma_r,sigma_t,epsilon)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% fuzzyPSReg, Fuzzy Cluster-based Point Set Registration. This function estimates the 3D Euclidean
% transformation parameters R and t to rigidly register the two given 3D point sets P_F and P_M,
% where P_F (3 x N_PF) is the fixed set and P_M (3 x N_PM) is the moving set.
%
% Inputs:
% 'P_F' fixed point set (3 x N_PF).
%
% 'P_M' moving point set (3 x N_PM).
%
% 'noisyF' 1/0 for the condition of fixed set with/without noisy outliers.
%
% 'noisyM' 1/0 for the condition of moving set with/without noisy outliers.
%
% 'trimRatio' trimming ratio for registering partially overlapping point sets,
% trimRatio = 0 for no trimming case.
%
% 'N_C' number of fuzzy clusters for coarse registration.
%
% 'gsF' grid step for downsampling fixed set (using box grid filter)
% to improve efficiency of fuzzy clustering.
%
% 'gsM' grid step for downsampling moving set (using box grid filter)
% to improve efficiency of fuzzy clustering.
%
% 'gsF_fine' grid step for downsampling fixed set (using box grid filter)
% to improve efficiency of fine registration.
%
% 'gsM_fine' grid step for downsampling moving set (using box grid filter)
% to improve efficiency of fine registration.
%
% 'RRange' half side-length of the initial rotation cube [-RRange, RRange]^3 (radians).
%
% 'TRange' half side-length of the initial translation cube [-TRange, TRange]^3.
%
% 'sigma_r' half side-length of the minimum rotation cube for stopping global search.
%
% 'sigma_t' half side-length of the minimum translation cube for stopping inner search.
%
% 'epsilon' threshold value for stopping global search/inner search.
%
% Outputs:
% 'R' 3 x 3 rotation matrix to transform moving set for registration.
%
% 't' 3 x 1 translation vector to transform moving set for registration.
%
% 'timeReg' time cost (sec) of registration, including fuzzy clustering,
% noisy outlier pruning if any, coarse and fine registrations.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N_PF = size(P_F,2);
N_PM = size(P_M,2);
if ( N_PF == 0 )||( N_PM == 0 )||( size(P_F,1) ~= 3 )||( size(P_M,1) ~= 3 )
error('Something wrong with the input point sets. ');
end
if ( noisyF ~= 0 )&&( noisyF ~= 1 )
error('Incorrect setting for noisyF. ');
end
if ( noisyM ~= 0 )&&( noisyM ~= 1 )
error('Incorrect setting for noisyM. ');
end
if ( trimRatio < 0 )||( trimRatio >= 1 )
error('Incorrect setting for trimRatio. ');
end
fprintf('Registration starts. \n\n');
%%%%%% function normPoints normalizes the two point sets to include all the points in cube [-1, 1]^3
[P_F,P_M,tMidF,tMidM,scaleFactor] = normPoints(P_F,P_M);
%%%%%% Now P_F and P_M denote the normalized point sets
%%%%%% Pds_F and Pds_M are the downsampled normalized point sets for fuzzy clustering to improve efficiency
if noisyF == 0
Pds_F = downsamplePoints(P_F,scaleFactor*gsF);
else
Pds_F = P_F; % no downsampling is applied if there are noisy outliers
end
if noisyM == 0
Pds_M = downsamplePoints(P_M,scaleFactor*gsM);
else
Pds_M = P_M; % no downsampling is applied if there are noisy outliers
end
%%%%%% Strategy 1, N_CF = N_CM
N_CF = N_C; % number of fuzzy clusters of fixed set for coarse registration
N_CM = N_C; % number of fuzzy clusters of moving set for coarse registration
fprintf('Fuzzy clustering ... \n');
%%%%%%%%%%%%%%%%%%%%%%%%%% Fuzzy clustering
tic
[C_F,AFPCD_F,Pp_F] = fuzzyClustering(Pds_F,N_CF,noisyF);
[C_M,AFPCD_M,Pp_M] = fuzzyClustering(Pds_M,N_CM,noisyM);
timeFuzzyClustering = toc;
%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Fuzzy clustering is complete. \n\n');
%%%%%% Strategy 1
if AFPCD_F >= AFPCD_M
baseIsF = 1;
AFPCD = AFPCD_F;
else
baseIsF = 0;
AFPCD = AFPCD_M;
temp = C_M;
C_M = C_F;
C_F = temp;
end
fprintf('Coarse registration ... \n');
%%%%%%%%%%%%%%%%%%%%%%%%%% Coarse registration
[rt,timeCoarseReg] = outerSearch(C_F,C_M,AFPCD,trimRatio,RRange,TRange,sigma_r,sigma_t,epsilon);
%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Coarse registration is complete. \n\n');
fprintf('Fine registration ... \n');
%%%%%%%%%%%%%%%%%%%%%%%%%% Fine registration
if baseIsF == 1
if noisyF == 0
C_F_fine = downsamplePoints(P_F, scaleFactor*gsF_fine);
else
C_F_fine = downsamplePoints(Pp_F,scaleFactor*gsF_fine);
end
if noisyM == 0
C_M_fine = downsamplePoints(P_M, scaleFactor*gsM_fine);
else
C_M_fine = downsamplePoints(Pp_M,scaleFactor*gsM_fine);
end
else
if noisyF == 0
C_M_fine = downsamplePoints(P_F, scaleFactor*gsF_fine);
else
C_M_fine = downsamplePoints(Pp_F,scaleFactor*gsF_fine);
end
if noisyM == 0
C_F_fine = downsamplePoints(P_M, scaleFactor*gsM_fine);
else
C_F_fine = downsamplePoints(Pp_M,scaleFactor*gsM_fine);
end
end
N_CF_fine = size(C_F_fine,2);
N_CM_fine = size(C_M_fine,2);
%%%%%% Equation (18), which is just a suggestion. Users can freely choose a suitable trimRatio_fine
if trimRatio == 0
trimRatio_fine = 0;
elseif trimRatio < 0.1
trimRatio_fine = 0.75*trimRatio + 0.075;
elseif trimRatio < 0.2
trimRatio_fine = 0.5*trimRatio + 0.1;
else
trimRatio_fine = trimRatio;
end
N_CMTrim_fine = round( N_CM_fine*(1-trimRatio_fine) );
options = optimoptions('fminunc','SpecifyObjectiveGradient',true,'Display','off');
tic
rt = fminunc(@(rt)fuzzyMetric(rt,C_F_fine,C_M_fine,N_CF_fine,N_CM_fine,N_CMTrim_fine),rt,options);
timeFineReg = toc;
%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Fine registration is complete. \n\n');
if baseIsF == 1
theta = norm(rt(1:3));
if theta == 0
R = [1 0 0;0 1 0;0 0 1];
else
R = axang2rotm([rt(1:3)'/theta, theta]);
end
else % find the inverse transformation if baseIsF is 0
theta = norm(rt(1:3));
if theta == 0
R = [1 0 0;0 1 0;0 0 1];
rt = -rt;
else
R = axang2rotm([rt(1:3)'/theta, theta]);
R = R^(-1);
rt = [-rt(1:3); -R*rt(4:6)];
end
end
t = tMidF - R*tMidM + rt(4:6)/scaleFactor;