├── weight.m ├── match_points.m ├── gt_ini4.txt ├── README.md ├── rotation_matrix.m ├── symm_po_pl.m ├── RSICP_demo.m └── RSICP.m /weight.m: -------------------------------------------------------------------------------- 1 | function w = weight(r,u) 2 | 3 | w = exp(-r.^2./(2*u^2)); 4 | 5 | end -------------------------------------------------------------------------------- /match_points.m: -------------------------------------------------------------------------------- 1 | function [p1,p2,n1,n2,r] = match_points(SP,TP,SN,TN,Btree) 2 | 3 | idx = knnsearch(Btree, SP'); 4 | 5 | p1 = SP; 6 | n1 = SN; 7 | 8 | p2 = TP(:,idx); 9 | n2 = TN(:,idx); 10 | 11 | r = sqrt(sum((p1-p2).^2)); 12 | end 13 | -------------------------------------------------------------------------------- /gt_ini4.txt: -------------------------------------------------------------------------------- 1 | 9.7517260e-01 -2.2142237e-01 -3.2463150e-03 0.0000000e+00 2 | 2.2105101e-01 9.7245247e-01 7.3977358e-02 0.0000000e+00 3 | -1.3223355e-02 -7.2858293e-02 9.9725464e-01 0.0000000e+00 4 | 0.0000000e+00 0.0000000e+00 0.0000000e+00 1.0000000e+00 5 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RSICP 2 | 3 | The demo code of our ISPRS 2022 paper. If it is helpful for you, please cite our work: 4 | 5 | Li, J., Hu, Q., Zhang, Y., & Ai, M. (2022). Robust symmetric iterative closest point. ISPRS Journal of Photogrammetry and Remote Sensing, 185, 219-231. 6 | 7 | Li, J., Hu, Q., Ai, M., & Wang, S. (2021). A geometric estimation technique based on adaptive M-estimators: Algorithm and applications. IEEE Journal of Selected Topics in Applied Earth Observations and Remote Sensing, 14, 5613-5626. 8 | -------------------------------------------------------------------------------- /rotation_matrix.m: -------------------------------------------------------------------------------- 1 | function R = rotation_matrix(angle, rot) 2 | 3 | rx = rot(1); ry = rot(2); rz = rot(3); 4 | l = sqrt(rx*rx + ry*ry + rz*rz); 5 | if l==0 6 | R = eye(4); 7 | end 8 | l1 = 1/l; x = rx * l1; y = ry * l1; z = rz * l1; 9 | s = sin(angle); c = cos(angle); 10 | xs = x*s; ys = y*s; zs = z*s; c1 = 1 - c; 11 | xx = c1*x*x; yy = c1*y*y; zz = c1*z*z; 12 | xy = c1*x*y; xz = c1*x*z; yz = c1*y*z; 13 | R = [xx+c, xy+zs, xz-ys, 0, 14 | xy-zs, yy+c, yz+xs, 0, 15 | xz+ys, yz-xs, zz+c, 0, 16 | 0, 0, 0, 1]'; 17 | end -------------------------------------------------------------------------------- /symm_po_pl.m: -------------------------------------------------------------------------------- 1 | function T = symm_po_pl(p1,p2,n1,n2,w) 2 | 3 | w = w/sum(w); 4 | w2 = sqrt(w); 5 | meanp1 = mean(p1,2); 6 | meanp2 = mean(p2,2); 7 | p1 = p1-meanp1; 8 | p2 = p2-meanp2; 9 | 10 | n = n1+n2; 11 | 12 | c = cross(p1,n); 13 | d = p2-p1; 14 | 15 | A = [c' n']; 16 | b = dot(d,n)'; 17 | 18 | wm = repmat(w2',1,6); 19 | A = A.*wm; 20 | b = b.*w2'; 21 | AA = A'*A; 22 | Ab = A'*b; 23 | 24 | [U,S,V] = svd(AA); 25 | S1 = inv(S); 26 | x = U*S1*U'*Ab; 27 | 28 | rot = x(1:3); trans = x(4:6); 29 | rotangle = norm(rot); 30 | TR = rotation_matrix(rotangle, rot); 31 | trans = trans + meanp2 - TR(1:3,1:3) * meanp1; 32 | T = [TR(1:3,1:3) trans; 0 0 0 1]; 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /RSICP_demo.m: -------------------------------------------------------------------------------- 1 | clc;clear;close all; 2 | 3 | source_file = 'Depth_0000_ini4.ply'; 4 | target_file = 'Depth_0001.ply'; 5 | gt = load('gt_ini4.txt'); 6 | Tini_gt = gt; 7 | 8 | source = pcread(source_file); 9 | target = pcread(target_file); 10 | 11 | SP = double(source.Location'); %source points 12 | SN = double(source.Normal'); %source normals 13 | TP = double(target.Location'); %source points 14 | TN = double(target.Normal'); %source normals 15 | 16 | scaleS = norm(max(SP,[],2)-min(SP,[],2)); 17 | scaleT = norm(max(TP,[],2)-min(TP,[],2)); 18 | scale = max(scaleS,scaleT); 19 | 20 | SP = SP/scale; 21 | TP = TP/scale; 22 | 23 | meanS = mean(SP,2); 24 | meanT = mean(TP,2); 25 | 26 | SP = SP-repmat(meanS,1,size(SP,2)); 27 | TP = TP-repmat(meanT,1,size(TP,2)); 28 | 29 | t1 = clock; 30 | [T0, count] = RSICP(SP,TP,SN,TN); 31 | t2 = clock; 32 | time = etime(t2,t1); 33 | trans = T0(1:3,4); 34 | trans = trans + meanT - T0(1:3,1:3) * meanS; 35 | trans = trans*scale; 36 | T0(1:3,4)=trans; 37 | 38 | SP = double(source.Location'); 39 | P1 = T0(1:3,1:3)*SP+repmat(T0(1:3,4),1,size(SP,2)); 40 | P2 = Tini_gt(1:3,1:3)*SP+repmat(Tini_gt(1:3,4),1,size(SP,2)); 41 | rmse = sqrt(sum(sum((P1-P2).^2))/size(SP,2)) 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /RSICP.m: -------------------------------------------------------------------------------- 1 | function [T, count] = RSICP(SP,TP,SN,TN) 2 | 3 | T = eye(4); 4 | Btree = KDTreeSearcher(TP'); 5 | [~, dist] = knnsearch(Btree,TP','k',7); 6 | dist = dist(:,2:7); 7 | u2 = median(median(dist,2))/(3*sqrt(3)); 8 | [p1,p2,n1,n2,r] = match_points(SP,TP,SN,TN,Btree); 9 | u1 = 3*median(r); 10 | 11 | stop1 = 0; count = 0; 12 | while(~stop1) 13 | for i=1:100 14 | w = weight(r,u1); 15 | T0 = symm_po_pl(p1,p2,n1,n2,w); 16 | T = T0*T; 17 | p12 = T*[SP;ones(1,size(SP,2))]; p1 = p12(1:3,:); 18 | n1 = T(1:3,1:3)*SN; 19 | [p1,p2,n1,n2,r] = match_points(p1,TP,n1,TN,Btree); 20 | stop2 = norm(T0-eye(4)); 21 | if stop2 < 1e-5 22 | break; 23 | end 24 | end 25 | if abs(u1-u2)<1e-6 26 | stop1 = 1; 27 | end 28 | count = count+i; 29 | u1 = u1/4; 30 | if u1