PLICP和matlabicp代码

PLicp 算法，maltab和C都有

% Copyright 2011. All rights reserved.
% Institute of Measurement and Control Systems
% Karlsruhe Institute of Technology Germany

% This file is part of libicp.
% Authors: Andreas Geiger

% libicp is free software; you can redistribute it and/or modify it under the
% terms of the GNU General Public License as published by the Free Software
% Foundation; either version 3 of the License or any later version.

% libicp is distributed in the hope that it will be useful but WITHOUT ANY
% WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
% PARTICULAR PURPOSE. See the GNU General Public License for more details.

% You should have received a copy of the GNU General Public License along with
% libicp; if not write to the Free Software Foundation Inc. 51 Franklin
% Street Fifth Floor Boston MA 02110-1301 USA 

% libicp demo file for MATLAB

dbstop error; clear variables; close all;
disp（‘================================‘）;

% create model
M（1:） = 0:pi/20:3*pi;
M（2:） = sin（M（1:））;

% transform model yielding the template
rx = 0.5;
Tr = [cos（rx） -sin（rx） -1;sin（rx） cos（rx） -2;0 0 1];
T  = Tr（1:21:2）*M + Tr（1:23）*ones（1size（M2））;

% fit template to model
% - init with identity transformation （eye（3））
% - no outlier rejection step （-1）
% - use point-to-plane fitting
Tr_fit = icpMex（MTeye（3）-1‘point_to_plane‘）;
T_fit  = Tr_fit（1:21:2）*T + Tr_fit（1:23）*ones（1size（T2））;

% plot
figureaxis equalhold on; ms=8; lw=2; fs=16;
plot（M（1:）M（2:）‘or‘‘MarkerSize‘ms‘LineWidth‘lw）;
plot（T（1:）T（2:）‘sg‘‘MarkerSize‘ms‘LineWidth‘lw）;
plot（T_fit（1:）T_fit（2:）‘xb‘‘MarkerSize‘ms‘LineWidth‘lw）;
legend（‘model‘‘template‘‘fitted template‘‘Location‘‘NorthWest‘）;
set（gca‘FontSize‘fs）;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件         918  2015-05-18 22:08  CMakeLists.txt
     文件        4211  2013-09-09 10:37  README.TXT
     文件        1759  2013-09-09 10:37  matlab\demo_2d.m
     文件        1859  2013-09-09 10:37  matlab\demo_3d.m
     文件        1131  2014-07-11 11:45  matlab\make.m
     文件        2284  2013-09-09 10:37  src\demo.cpp
     文件        2594  2014-07-18 17:52  src\icp.cpp
     文件       12153  2016-02-16 17:57  src\icpPointToPlane.cpp
     文件        7582  2015-05-18 17:11  src\icpPointToPoint.cpp
     文件       20303  2015-05-18 17:15  src\kdtree.cpp
     文件       23182  2015-05-18 17:19  src\matrix.cpp
     文件        2783  2014-07-18 17:51  src\icp.h
     文件        1763  2015-05-18 17:22  src\icpPointToPlane.h
     文件        1320  2013-09-09 10:37  src\icpPointToPoint.h
     文件        5785  2013-09-09 10:37  src\kdtree.h
     文件        5116  2015-04-04 23:01  src\matrix.h
     文件        3524  2013-09-09 10:37  matlab\icpMex.cpp
     文件        3636  2015-03-10 09:41  matlab\sparsifyMex.cpp