测试2d形状匹配后位姿refine(Iterative Closest Point algorithm based on maximum correntropy criterion)

以下是 基于 maximum correntropy criterion的icp(https://github.com/Xuguanglin/ICPMCC) 的pose refine与

https://github.com/meiqua/shape_based_matching 里面的ICP2D_Point2Plane pose refine

的对比测试结果:

蓝色为优化前,绿色为优化后，上图为ICP2D_Point2Plane ,下图为ICPMCC

模板一：

模板二:

总体而言，两者文字模板的效果都比非文字模板差

ICP_MCC_pt2l 失败的情况:

仅将src.ply的所有点的x,y值加+4/+1,结果变为正常(询问meiqua,他提示:比如多给几个初始位置ICP一下,不知道有没有更加鲁棒的方法?):

 

 

clc;
clear;
close all;
ptCloud_src = pcread('e:/src.ply');
Location_src=ptCloud_src.Location;
Location_src_2d=[Location_src(:,1),Location_src(:,2)];
ptCloud_dst = pcread('e:/dst.ply');
Location_dst=ptCloud_dst.Location;
Location_dst_2d=[Location_dst(:,1),Location_dst(:,2)];
Normal_dst=ptCloud_dst.Normal;
Normal_dst_2d=[Normal_dst(:,1),Normal_dst(:,2)];
ICPPlot(Location_src_2d,Location_dst_2d);
axis equal;
axis off;
title('ICP_MCC_pt2pl 2d initial state');

Dim=2;
R = eye(Dim);
mX = mean(Location_src_2d);
mY = mean(Location_dst_2d);
t = (mY - mX)';
t=[0;0];
[R4, t4, TCorr4, MSE4, TData4] = ICP_MCC_pt2pl(Location_src_2d,Location_dst_2d ,R,t,Normal_dst_2d);

Rt=[[R4;[0,0]],[t4;1]];

dlmwrite('e:/Rt.csv', Rt, 'precision', 8); 
RtTransform=Rt*[Location_src_2d,ones(size(Location_src_2d,1),1)]';
RtTransform=RtTransform';
RtTransform=RtTransform(:,[1,2]);
ICPPlot(RtTransform,Location_dst_2d);
axis equal;
axis off;
title('ICPMCC p2l 2d Rt result');
pause;