geom2d: add registerICP, for registration by Iterative Closest Point …

…algorithm.

matGeom/geom2d/Contents.m

@@ -133,6 +133,7 @@
 %   createBasisTransform  - Compute matrix for transforming a basis into another basis
 %   createLineReflection  - Create the the 3x3 matrix of a line reflection
 %   fitAffineTransform2d  - Fit an affine transform using two point sets
+%   registerICP           - Fit affine transform by Iterative Closest Point algorithm
 %   polynomialTransform2d - Apply a polynomial transform to a set of points
 %   fitPolynomialTransform2d - Coefficients of polynomial transform between two point sets
 %
@@ -225,4 +226,3 @@
 
 %% Others...
 
-

matGeom/geom2d/fitAffineTransform2d.m

@@ -13,7 +13,7 @@
 %
 %   See also
 %     transforms2d, transformPoint, transformVector,
-%     fitPolynomialTransform2d
+%     fitPolynomialTransform2d, registerICP
 %
 
 % ------

matGeom/geom2d/registerICP.m

@@ -0,0 +1,49 @@
+function [trans, points] = registerICP(points, target, varargin)
+%REGISTERICP Fit affine transform by Iterative Closest Point algorithm
+%
+%   TRANS = registerICP(POINTS, TARGET)
+%   Computes the affine transform that maps the shape defines by POINTS
+%   onto the shape defined by the points TARGET. Both POINTS and TARGET are
+%   N-by-2 array of point coordinates, not necessarily the same size.
+%   The result TRANS is a 3-by-3 affine transform.
+%
+%   TRANS = registerICP(POINTS, TARGET, NITER)
+%   Specifies the number of iterations for the algorithm.
+%
+%   [TRANS, POINTS2] = registerICP(...)
+%   Also returns the set of transformed points.
+%
+%   Example
+%   registerICP
+%
+%   See also
+%     tansforms2d, fitAffineTransform2d
+%
+
+% ------
+% Author: David Legland
+% e-mail: [email protected]
+% Created: 2015-02-24,    using Matlab 8.4.0.150421 (R2014b)
+% Copyright 2015 INRA - Cepia Software Platform.
+
+
+nIter = 10;
+if ~isempty(varargin)
+    nIter = varargin{1};
+end
+
+% keep original points to transform them at each
+trans = [1 0 0;0 1 0;0 0 1];
+
+for i = 1:nIter
+    % identify target points for each source point
+    inds = findClosestPoint(points, target);
+    corrPoints = target(inds, :);
+
+    % compute transform for current iteration
+    trans_i = fitAffineTransform2d(points, corrPoints);
+
+    % apply transform, and update cumulated transform
+    points = transformPoint(points, trans_i);
+    trans = trans_i * trans;
+end