icpCpp(model,data,weights,randvec,sizerand,treeptr,iter)

function [TR, TT] = icpCpp(model,data,weights,randvec,sizerand,treeptr,iter)
% ICP - Iterative Closest Point algorithm, c++ implementation.
% Handles only points in R^3.
% Makes use of a kd-tree for closest-point search.
%
%   Usage:
%
%   [R, T] = icpCpp(model,data,weights,randvec,sizerand,treeptr,max_iter)
%
%   ICP finds the transformation of points in data to fit points in model.
%   Fit with respect to minimize a weighted sum of squares
%   for distances between the data points and the corresponding closest model points.
% 
%   INPUT:
% 
%   model - matrix with model points, [Pm_1 Pm_2 ... Pm_nmod]
%   data - matrix with data points,   [Pd_1 Pd_2 ... Pd_ndat]
%   weights - weights (>0) corresponding to points in data
%   randvec - uint32(randperm(size(data,2))-1)
%   sizerand - number of matched points in each iteration
%   treeptr - pointer to the kd-tree. Notice that model must be in
%             transpose when the kd-tree is created.
%   iter - Number of iterations.
%
%   OUTPUT:
%
%   R - rotation matrix and
%   T - translation vector so
%
%           newdata = R*data + T ,
%
%   where newdata is the transformed data points.
%
%   Compile c++ files first by running make. 
%   Run icp_demo for an example.
%
%   Written by Per Bergstrm 2007-10-09