% NORMALISE2DPTS - normalises 2D homogeneous points%% Function translates and normalises a set of 2D homogeneous points % so that their centroid is at the origin and their mean distance from % the origin is sqrt(2).  This process typically improves the% conditioning of any equations used to solve homographies, fundamental% matrices etc.%% Usage:   [newpts, T] = normalise2dpts(pts)%% Argument:%   pts -  3xN array of 2D homogeneous coordinates%% Returns:%   newpts -  3xN array of transformed 2D homogeneous coordinates.  The%             scaling parameter is normalised to 1 unless the point is at%             infinity. %   T      -  The 3x3 transformation matrix, newpts = T*pts%           % If there are some points at infinity the normalisation transform% is calculated using just the finite points.  Being a scaling and% translating transform this will not affect the points at infinity.% Peter Kovesi% School of Computer Science & Software Engineering% The University of Western Australia% pk at csse uwa edu au% http://www.csse.uwa.edu.au/~pk%% May 2003      - Original version% February 2004 - Modified to deal with points at infinity.function [newpts, T] = normalise2dpts(pts)    if size(pts,1) ~= 3        error('pts must be 3xN');    end        % Find the indices of the points that are not at infinity    finiteind = find(abs(pts(3,:)) > eps);        if length(finiteind) ~= size(pts,2)        warning('Some points are at infinity');    end        % For the finite points ensure homogeneous coords have scale of 1    pts(1,finiteind) = pts(1,finiteind)./pts(3,finiteind);    pts(2,finiteind) = pts(2,finiteind)./pts(3,finiteind);    pts(3,finiteind) = 1;        c = mean(pts(1:2,finiteind)')';            % Centroid of finite points    newp(1,finiteind) = pts(1,finiteind)-c(1); % Shift origin to centroid.    newp(2,finiteind) = pts(2,finiteind)-c(2);        meandist = mean(sqrt(newp(1,finiteind).^2 + newp(2,finiteind).^2));        scale = sqrt(2)/meandist;        T = [scale   0   -scale*c(1)         0     scale -scale*c(2)         0       0      1      ];        newpts = T*pts;