camlasercalib

 CAMLASERCALIB performs the final laser-camera calibration step.
 
 CAMLASERCALIB runs the final optimisation of the calibration process.
 
 Transformation equation:
     Lpts=phi*(Lptsc-delta)
     where:
         Lpts: is the coordinates of the laser points in the sensor frame.
 
         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:
     [delta,phi]=camlasercalib(Lpts,Nc,deltaest,phiest);
 
 INPUTS:
     Lpts: Calibration laser points.
 
     Nc: Normal vectors of laser points (in camera frame).
 
     deltaest: initial estimate of translation offest.
 
     phiest: initial estimate of rotaion matrix.
 
 OUTPUTS:
     delta: 3x1 translation offset vector.
 
     phi: 3x3 rotation matrix.

0001 function [delta,phi]=camlasercalib(Lpts,Nc,deltaest,phiest)
0002 % CAMLASERCALIB performs the final laser-camera calibration step.
0003 %
0004 % CAMLASERCALIB runs the final optimisation of the calibration process.
0005 %
0006 % Transformation equation:
0007 %     Lpts=phi*(Lptsc-delta)
0008 %     where:
0009 %         Lpts: is the coordinates of the laser points in the sensor frame.
0010 %
0011 %         Lptsc: is the coordinates of the laser points in the camera
0012 %         frame.
0013 %
0014 %         delta: represent the translation offset between the two
0015 %         coordinate frames. delta represents the position of the laser
0016 %         range finder origin in the camera coordinate frame.
0017 %
0018 %         phi: is the rotation matrix which, when applied, aligns the
0019 %         camera coordinate frame to the sensor coordinate frame.
0020 %
0021 % USAGE:
0022 %     [delta,phi]=camlasercalib(Lpts,Nc,deltaest,phiest);
0023 %
0024 % INPUTS:
0025 %     Lpts: Calibration laser points.
0026 %
0027 %     Nc: Normal vectors of laser points (in camera frame).
0028 %
0029 %     deltaest: initial estimate of translation offest.
0030 %
0031 %     phiest: initial estimate of rotaion matrix.
0032 %
0033 % OUTPUTS:
0034 %     delta: 3x1 translation offset vector.
0035 %
0036 %     phi: 3x3 rotation matrix.
0037 
0038 
0039 if ~exist('deltaest','var') || isempty(deltaest)
0040     deltaest=[0;0;0];
0041     phiest=angvec2dcm([0;0;0]);
0042 end
0043 
0044 % make sure delta is a col vec
0045 deltaest=deltaest(:);
0046 
0047 phideltaest0=[deltaest,rodrigues(phiest)];
0048 
0049 %Last optimisation uses the Levenberg Marquardt method
0050 options = optimset('LevenbergMarquardt','on','Display','off');
0051 options = optimset(options, 'MaxFunEvals', 10000000);
0052 % phideltaest0=phideltaest;
0053 phideltaest= lsqnonlin(@(phideltaest)geterroropt(phideltaest, Lpts, Nc), phideltaest0,[],[],options);
0054 delta = phideltaest(:,1);
0055 phi = rodrigues(phideltaest(:,2));