GETINIESTSCORE gets the score based on the tranformation estimate.
GETINIESTSCORE uses the transformation estimate to score the match of the
points with the initial estimate. The better the estimate the more
accurate the score will be.
USAGE:
scores=getinitestscore(delta,phi,Nci,BoardCorners,angleVector,rangeMatrix);
INPUTS:
delta: 3x1 translation offset vector.
phi: 3x3 rotation matrix.
angleVector: 1xN vector; angleVector lists the angles for the ranges
in rangeMatrix.
rangeMatrix: MxN array; Each row in rangeMatrix contains a laser scan
with ranges at the angles specified in angleVector.
Nci: 3xM vector containing the normal vector of the
calibration plane of the corresponding laser scan.
BoardCorners: is a 1xnoscans array of structures. Each structure has
the following elements:
n_sq_x: number of squares of the calibration chessboard along the
x direction.
n_sq_y: number of squares of the calibration chessboard along the
y direction.
corners: 3x((n_sq_x+1)*(n_sq_y+1)) array with the coordinates of
the chessboard corners in the camera frame.
OUTPUTS:
scores: MxN array with the score for each laser point.
Abdallah Kassir 1/3/20100001 function scores=getinitestscore(delta,phi,Nci,BoardCorners,angleVector,rangeMatrix,debug) 0002 % GETINIESTSCORE gets the score based on the tranformation estimate. 0003 % 0004 % GETINIESTSCORE uses the transformation estimate to score the match of the 0005 % points with the initial estimate. The better the estimate the more 0006 % accurate the score will be. 0007 % 0008 % USAGE: 0009 % scores=getinitestscore(delta,phi,Nci,BoardCorners,angleVector,rangeMatrix); 0010 % 0011 % INPUTS: 0012 % delta: 3x1 translation offset vector. 0013 % 0014 % phi: 3x3 rotation matrix. 0015 % 0016 % angleVector: 1xN vector; angleVector lists the angles for the ranges 0017 % in rangeMatrix. 0018 % 0019 % rangeMatrix: MxN array; Each row in rangeMatrix contains a laser scan 0020 % with ranges at the angles specified in angleVector. 0021 % 0022 % Nci: 3xM vector containing the normal vector of the 0023 % calibration plane of the corresponding laser scan. 0024 % 0025 % BoardCorners: is a 1xnoscans array of structures. Each structure has 0026 % the following elements: 0027 % 0028 % n_sq_x: number of squares of the calibration chessboard along the 0029 % x direction. 0030 % 0031 % n_sq_y: number of squares of the calibration chessboard along the 0032 % y direction. 0033 % 0034 % corners: 3x((n_sq_x+1)*(n_sq_y+1)) array with the coordinates of 0035 % the chessboard corners in the camera frame. 0036 % 0037 % OUTPUTS: 0038 % scores: MxN array with the score for each laser point. 0039 % 0040 % Abdallah Kassir 1/3/2010 0041 0042 0043 0044 noscans=size(rangeMatrix,1); 0045 0046 % intitial estimate score: 0047 % Manhattan distance from centroid of board projected on to board 0048 [z,x]=pol2cart(repmat(angleVector,[noscans,1]),rangeMatrix); 0049 0050 % BoardCorners=GetBoardCorners(); 0051 cameraPlanes = Nci; 0052 0053 scores=zeros(size(rangeMatrix)); 0054 0055 0056 if ~exist('debug','var') || isempty(debug) 0057 debug=0; 0058 end 0059 0060 0061 % camera axis 0062 org=[0;0;0]; 0063 xax=[1;0;0]; 0064 yax=[0;1;0]; 0065 zax=[0;0;1]; 0066 org=phi*(org-delta); 0067 xax=phi*(xax-delta); 0068 yax=phi*(yax-delta); 0069 zax=phi*(zax-delta); 0070 0071 if debug 0072 figure; 0073 end 0074 0075 for cntr=1:noscans 0076 0077 % get norm 0078 if cntr<=size(BoardCorners,2) && ~isnan(BoardCorners(cntr).corners(1)) 0079 0080 lBoardCorners=BoardCorners(cntr).corners; 0081 lBoardCorners=lBoardCorners-repmat(delta,1,size(lBoardCorners,2)); % minus delta 0082 lBoardCorners=phi*lBoardCorners; 0083 0084 % get mean point 0085 meanpt=mean(lBoardCorners,2); 0086 0087 lpts=[x(cntr,:);zeros(size(angleVector));z(cntr,:)]; 0088 lvecs=lpts-repmat(meanpt,[1,length(lpts)]); 0089 0090 % get norm vector 0091 N=cameraPlanes(:,cntr); 0092 0093 N=phi*N; % normal vector no need for delta because delta is the coordinates of the camera origin in the laser frame 0094 0095 % change to unit vector 0096 N=N/norm(N); 0097 0098 % debugging 0099 if debug 0100 clf; 0101 % display board 0102 lBoardCornersx=zeros(BoardCorners(cntr).n_sq_x+1,BoardCorners(cntr).n_sq_y+1); 0103 lBoardCornersy=zeros(BoardCorners(cntr).n_sq_x+1,BoardCorners(cntr).n_sq_y+1); 0104 lBoardCornersz=zeros(BoardCorners(cntr).n_sq_x+1,BoardCorners(cntr).n_sq_y+1); 0105 0106 lBoardCornersx(:)=lBoardCorners(1,:); 0107 lBoardCornersy(:)=lBoardCorners(2,:); 0108 lBoardCornersz(:)=lBoardCorners(3,:); 0109 hold on; 0110 h= mesh(lBoardCornersx,lBoardCornersy,lBoardCornersz); 0111 set(h,'edgecolor','red'); 0112 plot3(x(cntr,:),zeros(size(x(cntr,:))),z(cntr,:),'.'); 0113 xlabel('x'); 0114 ylabel('y'); 0115 zlabel('z'); 0116 axis equal; 0117 title(num2str(cntr)); 0118 0119 0120 plot3([org(1),xax(1)],[org(2),xax(2)],[org(3),xax(3)]);text(xax(1),xax(2),xax(3),'x'); 0121 plot3([org(1),yax(1)],[org(2),yax(2)],[org(3),yax(3)]);text(yax(1),yax(2),yax(3),'y'); 0122 plot3([org(1),zax(1)],[org(2),zax(2)],[org(3),zax(3)]);text(zax(1),zax(2),zax(3),'z'); 0123 plot3([0,N(1)],[0,N(2)],[0,N(3)]);text(N(1),N(2),N(3),'N'); 0124 axis tight; 0125 cameratoolbar(gcf); 0126 cameratoolbar(gcf,'SetCoordSys','y'); 0127 while 1 0128 b=waitforbuttonpress; 0129 if b~=0 0130 btnprsd=get(gcf,'CurrentCharacter'); 0131 if btnprsd=='e' 0132 debug=0; % stop debugging 0133 end 0134 break; 0135 end 0136 end 0137 end 0138 0139 N=repmat(N,[1,length(angleVector)]); 0140 dists=cross(lvecs,N); 0141 dists=sqrt(sum(dists.^2)); 0142 dists=dists+abs(dot(lvecs,N)); 0143 % normalise 0144 dists=dists./max(dists); 0145 scores(cntr,:)=1-dists; 0146 else 0147 scores(cntr,:)=0; 0148 end 0149 end