getRMatrix(pointCloud)

function h = getRMatrix(pointCloud)
%GETRMATRIX
%   h = GETRMATRIX(POINTCLOUD) computes a rotation matrix from two 
%   orthogonal planes in a 3D point cloud. GETRMATRIX shows a 3D point cloud 
%   and lets the user select a plane by clicking on two points. The 
%   selected plane is highlighted. 
%   POINTCLOUD should be a 3*N matrix, represending N 3D points.
%   The rotation matrix R is stored in the csv file ROTMATRIX.CSV
%   Handle to the figure is returned.
%
%   other functions required:
%       callbackClickARectangleUpperLeft mouse click callback function
%       UPPERLEFTINDEX returns the upper left point of the plane
%
%       callbackClickARectangleLowerRight mouse click callback function
%       LOWERRIGHTINDEX returns the lower rigth point of the plane.
%       SELECTEDPOINT returns the points inside the plane.
%       
%   To test this function ... 
%       load RotMatrixEx
%       h = getRMatrix(pointCloud)
%       now select one plane and push on "plane 1" button, select another
%       plane and push on "plane 2" button. Finally, click on "compute"
%       button to obtain the rotation matrix.
%       "New selection" resets the graph.
%
%       The point cloud can be rotated using the figure and camera toolbar.
%
%   To turn off the callback ...
%       set(h, 'WindowButtonDownFcn',''); 
%       set(h, 'WindowButtonUpFcn','');
%
%   by Gemma Parra
%   http://www.ot.utoronto.ca/iatsl/people/gparra.html
%   Intelligent Assistive Technology and Systems Lab (IATSL)
%   University of Toronto
%   June 5, 2012
%
%   This function uses the CLICKA3DPOINT developed by Babak Taati File ID: #7594

    if nargin ~= 1
        error('Requires one input arguments.')
    end

    if size(pointCloud, 1)~=3
        error('Input point cloud must be a 3*N matrix.');
    end

    %% GUI creation
    f = figure('Visible','off','Position',[360,500,450,285]); 
    
    hplane1 = uicontrol('Style','pushbutton',...
            'String','Plane 1',...
            'Position',[315,220,70,25], ...
            'Callback',{@hplane1_Callback}        );

    hplane2 = uicontrol('Style','pushbutton',...
            'String','Plane 2',...
            'Position',[315,180,70,25], ...
            'Callback',{@hplane2_Callback}        );

    hmatrix = uicontrol('Style','pushbutton',...
            'String','Compute',...
            'Position',[315,140,70,25], ...
            'Callback',{@hmatrix_Callback}        );
        
    hnews = uicontrol('Style','pushbutton',...
            'String','New selection',...
            'Position',[315,100,70,25], ...
            'Callback',{@newselection_Callback}        );    
    

    %% Configurations
    ha = axes('Units','pixels','Position',[50,60,200,185]);
    align([hplane1, hplane2, hmatrix,hnews],'Center','None');
    set([hplane1, hplane2, hmatrix, ha, hnews],'Units','normalized');
    set(f,'Name','Getting the Rotation Matrix by Gemma Parra')
    movegui(f,'center')
    set(f,'Visible','on')

    %% Global variables
    global UpperLeftIndex 
    global LowerRightIndex
    global selectedPoint 
    UpperLeftIndex = [];
    LowerRightIndex = [];
    selectedPoint = [];
    
    %% Show the point cloud
    h = gcf;
    plot3(pointCloud(1,:), pointCloud(2,:), pointCloud(3,:), '.', 'MarkerSize', 1); 
    set(h,'Toolbar','figure');  % Display the standard toolbar
    cameratoolbar('Show'); 
    grid on;
    hold on; 
    
    %% Set the callback, pass pointCloud to the callback function
    set(h, 'WindowButtonDownFcn', {@callbackClickARectangleUpperLeft, pointCloud}); 
    set(h, 'WindowButtonUpFcn', {@callbackClickARectangleLowerRight, pointCloud}); 

    %% Callbacks
    function hplane1_Callback(hObject, eventdata)
        global v1
        plane1 = selectedPoint;
        selectedPoint = [];
        A = plane1';
        pointCount = size(A,1);
        B = A - repmat(mean(A), pointCount, 1);
        C = cov(B);
        [V, D] = eig(C);
        v1 = V(:,1);
    end
    function hplane2_Callback(hObject, eventdata)
        global v2
        plane2 = selectedPoint;
        selectedPoint = [];
        A = plane2';
        pointCount = size(A,1);
        B = A - repmat(mean(A), pointCount, 1);
        C = cov(B);
        [V, D] = eig(C);
        v2 = V(:,1);
    end
    function hmatrix_Callback(hObject, eventdata)
        global v1
        global v2
        x_axis = [];
        y_axis = [];
        z_axis = [];
        %% Looking for the x_axis
        [mx1,imx1] = max(v1);
        [mx2,imx2] = max(v2);
        if (imx1 == 1)
            x_axis = v1;
        elseif (imx2 == 1)
            x_axis = v2;            
        end
         %% Looking for the y_axis
        if (imx1 == 2)
            y_axis = v1;
        elseif (imx2 == 2)
            y_axis = v2;            
        end
        %% Computing the z_axis
        clc
        if (~isempty(y_axis) && ~isempty(x_axis))
            y_axis = y_axis - dot(x_axis, y_axis) * x_axis;
            y_axis = y_axis / norm(y_axis);
            z_axis = cross(x_axis, y_axis);
        else
            sprintf('ERROR: 2 orthogonal planes must be selected\n')
        end        
        R = [x_axis, y_axis, z_axis] 
        csvwrite('rotmatrix.csv',R);
        v1 = [];
        v2 = [];
    end
    function newselection_Callback(hObject, eventdata)
        hold off
        plot3(pointCloud(1,:), pointCloud(2,:), pointCloud(3,:), '.', 'MarkerSize', 1); 
        hold on
    end
end