Code covered by the BSD License  

Highlights from
Adjust Plane to Given Normal

image thumbnail

Adjust Plane to Given Normal

by

 

27 Mar 2013 (Updated )

Adjust a more or less planar point cloud so that it fits a given normal vector.

adjustPlanarPointCloud(originalPlane, varargin)
function [adjustedPlane, normalOriginalPlane] = adjustPlanarPointCloud(originalPlane, varargin)
%Adjust a more or less planar point cloud so that it fits a given normal
%vector.
%E.g.: adjust a planar point cloud described by 3d coordinates so that it
%is described by 2d coordinates (and z = const) or transform it back from
%2d to its original 3d state.
%
%Inputs and outputs:
%
%   *Plane          -   minimum is a 1x3 matrix representing a point cloud
%   normal*Plane    -   vector with 3 entries defining the normal
%
%Adjust it to x,y-coordinates with z = const:
%
%   function [adjustedPlane, normalOriginalPlane] =
%   adjustPlanarPointCloud(originalPlane)
%
%originalPlane is a matrix with the minimum size 3x3, with no maximum side
%to either dimension. Each three succesive columns (e.g.
%originalPlane(:,1:3), originalPlane(:,4:6)) are treated as a
%x,y,z-coordinate to which the transformation is applied. If the number of
%columns is not a multiple of three, the remaining one or two columns are
%ignored (but not deleted). So if the columns correspond to e.g. a point
%coordinate, a vector and a scalar [x,y,z,u,v,w,abs] the coordinate and
%vector are transformed, the remaining scalar just copied (handy for the
%magnitude of the vector which doesn't change).
%Output is the adjusted plane (z = const) and the normal of the
%originalPlane which is calculated by fitNormal() by Dan Couture. This
%syntax is equivalent to calling the second example with
%normalAdjustedPlane = [0 0 1].
%
%
%Adjust a planar point cloud to match a given normal:
%
%   function [adjustedPlane, normalOriginalPlane] =
%   adjustPlanarPointCloud(originalPlane, normalAdjustedPlane)
%
%Reverses the first operation when supplied with the right normal to match.
%
%
%Adjust a planar point cloud to match a given normal and explicitly state
%what is considered the normal of the input plane:
%
%   function [adjustedPlane, normalOriginalPlane] =
%   adjustPlanarPointCloud(originalPlane, normalAdjustedPlane, normalOriginalPlane)
%
%If you already know the normal and don't want to calculate it again. Or if
%you want to rotate an arbitrarily shaped point cloud around an arbitrary
%axis. Or if this function prints a warning telling you to do that.
%
%by Bastian Tietjen, released March 2013 under a Simplified BSD licence.

%--------set defaults---------------------------------------------

numberOfVarArgs = length(varargin);

%check for more than two optional inputs
if numberOfVarArgs > 2
    error('adjustPlanarPointCloud:TooManyInputs', ...
        'just two optional inputs');
    
%if the normal of the input plane is not supplied we need at least
%three coordinates to calculate it
elseif numberOfVarArgs < 2 && 3 > size(originalPlane, 1)
        error('adjustPlanarPointCloud:NoPlaneDefined', ...
        'originalPlane must contain at least 3 coordinates');
end

%set default values

%planes will be adjusted to this normal vector and usually the normal of
%the input plane is calculated
optionalArgs = {[0 0 1], [0 0 0]};

%overwrite the defaults depending on the number of optional inputs
optionalArgs(1:numberOfVarArgs) = varargin;

%place the optionalArgs in variables
[normalAdjustedPlane, normalOriginalPlane] = optionalArgs{:};
%--------set defaults end-----------------------------------------

    
    %generate normal if it hasn't been supplied
    if numberOfVarArgs < 2
        
        try
            normalOriginalPlane = fitNormal(originalPlane(:,1:3));
        catch err
            disp([char(10), 'Something went wrong . Try to supply the normal of the input plane:'])
            disp(['[... , ...] = adjustPlanarPointCloud(... , ... , [0 0 1])', char(10)])
            
            rethrow(err)
        end
    end
    
    %normal to spherical coordinates
    [aziOrig, eleOrig, ~] = cart2sph(normalOriginalPlane(1),normalOriginalPlane(2),normalOriginalPlane(3));
    
    %check if the normal is probably [0 0 1], if yes print a warning that
    %the points were rotated around their normal and how to avoid that
    if aziOrig ~= 0 && abs(normalOriginalPlane(1)) < 1e-12 && ...
            abs(normalOriginalPlane(2)) < 1e-12 && ...
            abs(1 - normalOriginalPlane(3)) < 1e-12
        
        disp([char(10), 'Warning: The points seem to lie in the x,y plane. However, I ', ...
            'rotated them by ', char(10), num2str(aziOrig*180/pi), ' degrees around their ', ...
            'normal. If that was wrong, use : '])
        disp('[... , ...] = adjustPlanarPointCloud(... , ... , [0 0 1])')
        
    end
    
    
    %normal to match to spherical coordinates
    [aziAdj, eleAdj, ~] = cart2sph(normalAdjustedPlane(1),normalAdjustedPlane(2),normalAdjustedPlane(3));
    
    %calculate the sines and cosines of both azimuths and elevations
    cosAziOrig = cos(aziOrig);
    sinAziOrig = sin(aziOrig);
    cosEleOrig = cos(pi/2-eleOrig);
    sinEleOrig = sin(pi/2-eleOrig);
    
    cosAziAdj = cos(-aziAdj);
    sinAziAdj = sin(-aziAdj);
    cosEleAdj = cos(-(pi/2-eleAdj));
    sinEleAdj = sin(-(pi/2-eleAdj));

    %create 4 transformation matrices:
    %1. aziOrig around z
    T_1 = [cosAziOrig, sinAziOrig, 0; ...
        -sinAziOrig, cosAziOrig, 0; ...
        0 0 1];
    
    %2. (pi/2-eleOrig) around y
    T_2 = [cosEleOrig, 0, -sinEleOrig; ...
        0, 1, 0; ...
        sinEleOrig, 0, cosEleOrig];
    
    %3. -(pi/2-eleAdj) around y
    T_3 = [cosEleAdj, 0, -sinEleAdj; ...
        0, 1, 0; ...
        sinEleAdj, 0, cosEleAdj];
    
    %4. -aziAdj around z
    T_4 = [cosAziAdj, sinAziAdj, 0; ...
        -sinAziAdj, cosAziAdj, 0; ...
        0 0 1];
    
    %4 * 3 * 2 * 1 = T
    T = T_4 * T_3 * T_2 * T_1;
    
    %call transformCartCoord with T
    adjustedPlane = transformCartCoord(originalPlane, T);

end

Contact us