getinitest

 GETINITEST gets an initial estimate for the final step of the laser-camera calibration optimisation.
 
 GETINITEST gets an initial estimate of the laser-camera extrinsic
 parameters. The two inputs deltaest and phiest are optional.
 
 Transformation equation:
     Lpts=phi*(Lptsc-delta)
     where:
         Lpts: is the coordinates of the laser points in the sensor frame.
         The first row is x, the second y and the last row is z. x is
         pointing to the  right of the laser, y downwards and z outwards.
 
         Lptsc: is the coordinates of the laser points in the camera
         frame.
 
         delta: represent the translation offset between the two
         coordinate frames. delta represents the position of the laser
         range finder origin in the camera coordinate frame.
 
         phi: is the rotation matrix which, when applied, aligns the
         camera coordinate frame to the sensor coordinate frame.
 
 USAGE:
     [deltaest,phies]=getinitest(Lpts,Nc);
 
     [deltaest,phies]=getinitest(Lpts,Nc,deltaest,phiest);
 
 INPUTS:
     Lpts: Calibration laser points.
 
     Nc: Normal vectors of laser points (in camera frame).
 
     deltaest: rough initial estimate of translation offest (optional).
 
     phiest: rough initial estimate of rotaion matrix (optional).
 
 Fabio Tozeto Ramos 05/10/05 
 modified by Abdallah Kassir 15/1/2010

0001 function [deltaest,phiest] = getinitest(Pf, N,deltaest,phiest)
0002 % GETINITEST gets an initial estimate for the final step of the laser-camera calibration optimisation.
0003 %
0004 % GETINITEST gets an initial estimate of the laser-camera extrinsic
0005 % parameters. The two inputs deltaest and phiest are optional.
0006 %
0007 % Transformation equation:
0008 %     Lpts=phi*(Lptsc-delta)
0009 %     where:
0010 %         Lpts: is the coordinates of the laser points in the sensor frame.
0011 %         The first row is x, the second y and the last row is z. x is
0012 %         pointing to the  right of the laser, y downwards and z outwards.
0013 %
0014 %         Lptsc: is the coordinates of the laser points in the camera
0015 %         frame.
0016 %
0017 %         delta: represent the translation offset between the two
0018 %         coordinate frames. delta represents the position of the laser
0019 %         range finder origin in the camera coordinate frame.
0020 %
0021 %         phi: is the rotation matrix which, when applied, aligns the
0022 %         camera coordinate frame to the sensor coordinate frame.
0023 %
0024 % USAGE:
0025 %     [deltaest,phies]=getinitest(Lpts,Nc);
0026 %
0027 %     [deltaest,phies]=getinitest(Lpts,Nc,deltaest,phiest);
0028 %
0029 % INPUTS:
0030 %     Lpts: Calibration laser points.
0031 %
0032 %     Nc: Normal vectors of laser points (in camera frame).
0033 %
0034 %     deltaest: rough initial estimate of translation offest (optional).
0035 %
0036 %     phiest: rough initial estimate of rotaion matrix (optional).
0037 %
0038 % Fabio Tozeto Ramos 05/10/05
0039 % modified by Abdallah Kassir 15/1/2010
0040 
0041 % optimisation on linear parameters
0042 % input rotation vector is a vector representing Euler angles.
0043 % The convention used is the 'xyz' convention
0044 % Frame convention is the one used in the matlab camera calibration toolbox
0045 % the offset is the location of the laser's origin in the camera's frame of
0046 % references.
0047 
0048 if exist('deltaest','var')
0049     % make sure offset is a column vectors
0050     deltaest=deltaest(:);
0051 else
0052     deltaest=[0;0;0];
0053     phiest=angvec2dcm([0;0;0]);
0054 end
0055 %
0056 % % change to radians
0057 % rotation=deg2rad(rotation);
0058 
0059 phiestinv=inv(phiest);
0060 
0061 % modified by Abdallah Kassir 15/1/10
0062 H0 = [phiestinv(:,1),phiestinv(:,3),deltaest];
0063 
0064 % turn off warning
0065 warning('off','optim:fmincon:NLPAlgLargeScaleConflict');
0066 Pfhat = [Pf(1,:); Pf(3,:); ones(1,length(Pf))];
0067 options = optimset('LargeScale','on','Display','off');
0068 options = optimset(options, 'MaxFunEvals', 10000000);
0069 options = optimset(options, 'MaxIter', 1000);
0070 H = lsqnonlin(@(H)calibFun0(H, Pfhat, N), H0,[],[],options);
0071 
0072 % extract delta and phi from H
0073 phiest=[H(:,1), cross(-H(:,1),H(:,2)), H(:,2)]'; % inverse is transpose
0074 deltaest=H(:,3); % delta positive
0075 
0076 % makes sure phi is a valid rotation matrix
0077 options = optimset('LargeScale','off','Display','off');
0078 phiest0=phiest;
0079 phiest = fmincon(@(phiest)frobenius_norm(phiest,phiest0),phiest0,[],[],[],[],[],[],@constraint_phi,options);