Skip to content

GitLab

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

update FuzzyPSReg-O2S

parent 3fbacddf
Hide whitespace changes
Inline Side-by-side
Showing with 90 additions and 30 deletions
FuzzyPSReg-O2S/demo1.m
View file @ e421a5c7
......@@ -5,17 +5,17 @@ Po = load('data/Po1.txt')'; % object model (3 x N_Po)
Ps = load('data/Ps1.txt')'; % scene scan (3 x N_Ps)
%%%%%% Parameter setting
N_Co = 50; % number of fuzzy clusters of Po
N_Cs = 50; % number of fuzzy clusters of Ps
gs_Po = 0.005; % grid step of for the box grid filter to downsample Po for improving efficiency
gs_Ps = 0.005; % grid step of for the box grid filter to downsample Ps for improving efficiency
p1 = 1.05; % (1 + dJ_1) for the extended registration quality assessment
p2 = 3; % (1 + dJ_1) for the extended registration quality assessment
r_inits = [0 0 0;0 pi 0]'; % N_r initial rotations (3 x N_r) of Po
% used in the multi-start local optimization
trimRatio1 = 0.4; % \xi_1, trimming ratio of Po in the coarse registration
N_Co = 50; % number of fuzzy clusters of Po
N_Cs = 50; % number of fuzzy clusters of Ps
gs_Po = 0.005; % grid step of for the box grid filter to downsample Po for improving efficiency
gs_Ps = 0.005; % grid step of for the box grid filter to downsample Ps for improving efficiency
p1 = 1.05; % (1 + dJ_1) for the extended registration quality assessment
p2 = 3; % (1 + dJ_1) for the extended registration quality assessment
r_inits = [0 0 0;0 pi 0]'; % N_r initial rotations (3 x N_r) of Po in the multi-start local optimization
%%%%%% Fuzzy cluster-based point set registration for object pose estimation
[R,t] = fuzzyPSReg_O2S(Po,Ps,N_Co,N_Cs,gs_Po,gs_Ps,p1,p2,r_inits);
[R,t] = fuzzyPSReg_O2S(Po,Ps,trimRatio1,N_Co,N_Cs,gs_Po,gs_Ps,p1,p2,r_inits);
PoRnT = R*Po + t; % transformed object model
......
FuzzyPSReg-O2S/fuzzyPSReg_O2S.m
View file @ e421a5c7
%%%%%% The main function
function [R,t,timeReg] = fuzzyPSReg_O2S(Po,Ps,N_Co,N_Cs,gs_Po,gs_Ps,p1,p2,r_inits)
function [R,t,timeReg] = fuzzyPSReg_O2S(Po,Ps,trimRatio1,N_Co,N_Cs,gs_Po,gs_Ps,p1,p2,r_inits)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% FuzzyPSReg-O2S: Fuzzy cluster-based point set registration for registering an object to a scene.
% This function estimates the rigid transformation parameters, R and t, to register the two given 3D
% point sets Po and Ps, where Po (3 x N_Po) is the object model and Ps (3 x N_Ps) is the scene scan.
%
% Inputs:
% 'Po' object model (3 x N_Po, full model).
% 'Po' object model (3 x N_Po).
%
% 'Ps' scene scan (3 x N_Ps, partial scan).
% 'Ps' scene scan (3 x N_Ps).
%
% 'trimRatio1' trimming ratio \xi_1 of Po in the multi-start optimization
% of the coarse registration. trimRatio1 = 0 for no trimming case.
%
% 'N_Co' number of fuzzy clusters of Po.
%
......@@ -24,7 +27,8 @@ function [R,t,timeReg] = fuzzyPSReg_O2S(Po,Ps,N_Co,N_Cs,gs_Po,gs_Ps,p1,p2,r_init
%
% 'p2' (1 + dJ_2) for the extended registration quality assessment.
%
% 'r_inits' N_r initial rotations (3 x N_r) of Po used in the multi-start local optimization.
% 'r_inits' N_r initial rotations (3 x N_r) of Po in the multi-start local optimization
% of the fine registration.
%
% Outputs:
% 'R' 3 x 3 rotation matrix to transform Po for registration.
......@@ -53,10 +57,8 @@ if ( N_r == 0 )||( D_r ~= 3 )
error('Incorrect setting for r_inits. ');
end
fprintf('Registration starts. \n\n')
objMax = max(Po,[],2);
objMin = min(Po,[],2);
objMax = max(Po,[],2); % users can choose a proper value for objMax
objMin = min(Po,[],2); % users can choose a proper value for objMin
objMax2Min = objMax - objMin;
objMax_s = objMax - 0.1*objMax2Min;
objMin_s = objMin + 0.1*objMax2Min;
......@@ -73,12 +75,11 @@ gs_PF1 = gs_Ps;
N_CF1 = N_Cs;
C_M1 = Co_fcm;
N_CM1 = N_Co;
trimRatio1 = 0.4; % trimming ratio 1, users can choose a proper value
N_CM1Trim = round( N_CM1*(1-trimRatio1) );
C_F2 = Co_gk;
N_CF2 = N_Co;
gs_PM2 = gs_Ps;
trimRatio2 = 0; % trimming ratio 2
trimRatio2 = 0; % trimming ratio \xi_2
timeReg = 0;
N_PsMin = 3000; % threshold for the number of points in Ps
......@@ -86,6 +87,8 @@ zeros6xNr = zeros(6,N_r);
largeNum1xNr = 10^6*ones(1,N_r);
options = optimoptions('fminunc','SpecifyObjectiveGradient',true,'Display','off');
fprintf('Registration starts. \n\n')
while (1)
if N_PF1 < N_PsMin
......@@ -102,17 +105,20 @@ while (1)
N_PsMin,objMax,objMin,objMax_s,objMin_s);
timeReg = timeReg + toc;
fprintf('The coarse registration is complete. \n')
if flag_ms == 0
continue
end
% R1
% t1
ones1xNPF1 = ones(1,N_PF1);
inv_P_F1 = R1'*(P_F1 - t1*ones1xNPF1);
idxs_PM2 = ( (inv_P_F1(1,:)<objMax(1))&(inv_P_F1(1,:)>objMin(1))...
&(inv_P_F1(2,:)<objMax(2))&(inv_P_F1(2,:)>objMin(2))...
&(inv_P_F1(3,:)<objMax(3))&(inv_P_F1(3,:)>objMin(3)) );
N_PM2 = sum(idxs_PM2);
if N_PM2 <= 1000
fprintf(' The result is not good, prune the scan and rerun the coarse registration. \n\n')
P_F1(:,idxs_PM2) = [];
......@@ -132,6 +138,7 @@ while (1)
fprintf('A fine registration starts. \n')
rt2s = zeros6xNr;
rt2Costs = largeNum1xNr;
tic
for i = 1:N_r
rt2_init = [r_inits(:,i)' 0 0 0]';
......@@ -183,7 +190,9 @@ while (1)
if isempty(choice)
choice = 1;
end
if choice == 1
fprintf('The result needs further correction. FuzzyPSReg-SS is applied. \n')
close
inv_P_F1 = R'*(P_F1 - t*ones1xNPF1);
......@@ -191,10 +200,11 @@ while (1)
&(inv_P_F1(2,:)<objMax_l(2))&(inv_P_F1(2,:)>objMin_l(2))...
&(inv_P_F1(3,:)<objMax_l(3))&(inv_P_F1(3,:)>objMin_l(3)));
P_M2 = inv_P_F1(:,idxs_PM2);
plot4checking(Po,P_M2)
while (1)
trimRatio3 = input('Please set a proper trimming ratio for FuzzyPSReg-SS: ');
if ( trimRatio3 < 0 )||( trimRatio3 >= 1 )
if ( trimRatio3 < 0 )||( trimRatio3 >= 1 ) % trimming ratio \xi_3
fprintf('Incorrect setting for the trimming ratio. \n\n')
else
fprintf('\n')
......@@ -205,7 +215,7 @@ while (1)
fprintf('A registration using FuzzyPSReg-SS starts. \n')
tic
[R3,t3] = fuzzyPSReg_SS(Co_fcm,AFPCD_fcm,Co_gk,pInvCov_gk,N_Co,P_M2,trimRatio3,objMax2Min,gs_PM2);
[R3,t3] = fuzzyPSReg_SS(Co_fcm,AFPCD_fcm,Co_gk,pInvCov_gk,N_Co,P_M2,trimRatio3,gs_PM2);
timeReg = timeReg + toc;
fprintf('The registration using FuzzyPSReg-SS is complete. \n\n')
% R3
......@@ -213,11 +223,14 @@ while (1)
R = R*R3';
t = t - R*t3;
plot4checking(Po,P_M2,R3,t3)
% R
% t
break
else
fprintf('The result is wrong. Thus, the scan is pruned for a new round. \n\n')
close
%%%%%%
......@@ -230,6 +243,7 @@ while (1)
&(inv_P_F1(3,:)<objMax_s(3))&(inv_P_F1(3,:)>objMin_s(3)));
P_F1(:,idxs_PM2) = [];
N_PF1 = size(P_F1,2);
end
else
......@@ -256,11 +270,15 @@ end
function Pds = downsamplePoints(P,gs) % point set downsampling using box grid filter
if gs == 0 % no downsampling is applied when gs is 0
Pds = P;
else
ptCloud = pointCloud(P');
ptCloud = pcdownsample(ptCloud,'gridAverage',gs);
Pds = double(ptCloud.Location'); % 3 x N_Pds
end
end
......@@ -449,7 +467,6 @@ while (1)
[rtfs(:,i), rtfCosts(i)]= fminunc(@(rt)fuzzyMetricFCM(rt,C_F,C_M,...
N_CF,N_CM,N_CMTrim),rts(:,idx_minCost),options);
end
[rtCost,idx_minCost] = min(rtfCosts);
rt = rtfs(:,idx_minCost);
......@@ -702,17 +719,24 @@ end
end
function [R,t] = fuzzyPSReg_SS(C_F_fcm,AFPCD_fcm,C_F_gk,pInvCov_gk,N_C,P_M,trimRatio,objMax2Min,gs_PM)
function [R,t] = fuzzyPSReg_SS(C_F_fcm,AFPCD_fcm,C_F_gk,pInvCov_gk,N_C,P_M,trimRatio,gs_PM)
[C_F_fcm,P_M,tMidF,tMidM,scaleFactor] = normPoints(C_F_fcm,P_M);
C_F_gk = scaleFactor*(C_F_gk - tMidF);
sf2 = scaleFactor^2;
AFPCD_fcm = AFPCD_fcm*sf2;
pInvCov_gk = pInvCov_gk*sf2;
P_M = downsamplePoints(P_M,scaleFactor*gs_PM);
[C_M,~,~] = fuzzyClusteringFCM(P_M,N_C,100);
RRange = pi;
TRange = 0.8*max(objMax2Min);
TRange = 0.5;
sigma_r = 0.01;
sigma_t = TRange/4;
epsilon = 0;
P_M = downsamplePoints(P_M,gs_PM);
[C_M,~,~] = fuzzyClusteringFCM(P_M,N_C,100);
rt = globalSearch(C_F_fcm,C_M,AFPCD_fcm,trimRatio,RRange,TRange,sigma_r,sigma_t,epsilon);
N_PM = size(P_M,2);
......@@ -727,7 +751,44 @@ if theta == 0
else
R = axang2rotm([r'/theta theta]);
end
t = rt(4:6);
t = tMidF - R*tMidM + rt(4:6)/scaleFactor;
end
function [P_F,P_M,tMidF,tMidM,scaleFactor] = normPoints(P_F,P_M) % normalization of P_F and P_M
choice = 1; % choice = {0, 1, 2, 3}
if choice == 0 % no change is applied to the two point sets
tMidF = [0 0 0]';
tMidM = [0 0 0]';
scaleFactor = 1;
fprintf('The two point sets are not normalized for this registration. \n');
else
if choice == 1 % method 1
tMidF = 0.5*(max(P_F,[],2) + min(P_F,[],2));
tMidM = 0.5*(max(P_M,[],2) + min(P_M,[],2));
elseif choice == 2 % method 2
tMidF = sum(P_F,2)/size(P_F,2);
tMidM = sum(P_M,2)/size(P_M,2);
elseif choice == 3 % method 3
tMidF = [0 0 0]';
tMidM = [0 0 0]';
else
error('Incorrect choice for the normalization method. ')
end
P_F = P_F - tMidF;
P_M = P_M - tMidM;
scaleFactor = 1/max(max(abs(P_F),[],'all'), max(abs(P_M),[],'all'));
P_F = scaleFactor*P_F; % normalized fixed set
P_M = scaleFactor*P_M; % normalized moving set
end
end
......@@ -1025,7 +1086,6 @@ elseif nargin == 2
else
error('Incorrect setting ')
end
figure
hold on
plot3(P_F(1,:),P_F(2,:),P_F(3,:),'.r','MarkerSize',4);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment