InsertLaserIntoImage

 INSERTLASERINTOIMAGE reprojects the laser points onto the image.
 
 INSERTLASERINTOIMAGE reprojects the laser points onto the image. It takes
 as input the extrinsic laser-camera parameters and the camera parameters.
 It also takes as input the standard deviations of the errors in the
 parameters.

0001 function InsertLaserIntoImage( im, angleVector, rangeVector, delta, phi, f, c, k, alpha,deltae,rote,fe,ce,ke,alphae)
0002 % INSERTLASERINTOIMAGE reprojects the laser points onto the image.
0003 %
0004 % INSERTLASERINTOIMAGE reprojects the laser points onto the image. It takes
0005 % as input the extrinsic laser-camera parameters and the camera parameters.
0006 % It also takes as input the standard deviations of the errors in the
0007 % parameters.
0008 
0009 % INPUTS:
0010 %     im: the image selected for reprojection.
0011 %
0012 %     angleVector: row vector of the angles of the laser data.
0013 %
0014 %     rangeVector: row vector, same size as angleVector, containing the
0015 %     corresponding ranges.
0016 %
0017 %     delta: translation 3x1 vector of the laser-camera calibration.
0018 %
0019 %     phi: rotation 3x3 matrix of the laser-camera calibraion.
0020 %
0021 %     f: focal length vector (from camera calibration).
0022 %
0023 %     c: principal point vector (from camera calibration).
0024 %
0025 %     k: distortion vector (from camera calibration).
0026 %
0027 %     alpha: skew coefficient (from camera calibration).;
0028 %
0029 %     deltae: standard deviation of error in delta.
0030 %
0031 %     rote: standard deviation of error in angles forming phi.
0032 %
0033 %     fe: standard deviation of error in f.
0034 %
0035 %     ce: standard deviation of error in c.
0036 %
0037 %     ke: standard deviation of error in ke.
0038 %
0039 %     alphae: standard deviation of error in alpha
0040 
0041 
0042 % get rotation vector
0043 rot=dcm2angvec(phi);
0044 phiinv=inv(phi);
0045 
0046 %% Get points
0047 
0048 % change to cartesian coords
0049 [zl,xl]=pol2cart(angleVector,rangeVector);
0050 
0051 Lpts=[xl;zeros(size(xl));zl];
0052 
0053 % change to mm (camera parameters in mm)
0054 Lpts=Lpts.*1000;
0055 delta=delta.*1000;
0056 deltae=deltae.*1000;
0057 
0058 % apply laser to camera transformation
0059 Cpts=phiinv*Lpts+repmat(delta,1,size(Lpts,2));
0060 
0061 xc=Cpts(1,:);
0062 yc=Cpts(2,:);
0063 zc=Cpts(3,:);
0064 
0065 
0066 %normalise over Z (in this frame);
0067 a=xc./zc;
0068 b=yc./zc;
0069 
0070 % add distortion
0071 
0072 r = sqrt(a.^2 + b.^2);
0073 
0074 ad = a.*(1 + k(1).*r.^2 + k(2).*r.^4 + k(5).*r.^6) +  2.*k(3).*a.*b + k(4).*(r.^2 + 2.*a.^2);
0075 bd = b.*(1 + k(1).*r.^2 + k(2).*r.^4 + k(5).*r.^6) +  k(3).*(r.^2 + 2.*b.^2) + 2.*k(4).*a.*b;
0076 
0077 
0078 % image coordinates
0079 x = f(1).*(ad + alpha.*bd) + c(1) + 1; % add 1 for matlab coords
0080 y = f(2).*bd + c(2) + 1; % add 1 for matlab coords
0081 
0082 
0083 
0084 %% Get error (uncertainty)
0085 
0086 % get rotation derivative matrices
0087 % check conventions in labbook
0088 
0089 [phi,phix,phiy,phiz,dphix,dphiy,dphiz]=angvec2dcm(rot);
0090    
0091 % partial derivatives of points per delta params
0092 dpcddeltax=repmat([1;0;0],1,size(Lpts,2));
0093 dpcddeltay=repmat([0;1;0],1,size(Lpts,2));
0094 dpcddeltaz=repmat([0;0;1],1,size(Lpts,2));
0095 
0096 % partial derivatives of points per rot params
0097 dpcdrot1=-phiinv*(phiz*phiy*dphix)*phiinv*Lpts;
0098 dpcdrot2=-phiinv*(phiz*dphiy*phix)*phiinv*Lpts;
0099 dpcdrot3=-phiinv*(dphiz*phiy*phix)*phiinv*Lpts;
0100 
0101 dxcddeltax=dpcddeltax(1,:);
0102 dycddeltax=dpcddeltax(2,:);
0103 dzcddeltax=dpcddeltax(3,:);
0104 
0105 dxcddeltay=dpcddeltay(1,:);
0106 dycddeltay=dpcddeltay(2,:);
0107 dzcddeltay=dpcddeltay(3,:);
0108 
0109 dxcddeltaz=dpcddeltaz(1,:);
0110 dycddeltaz=dpcddeltaz(2,:);
0111 dzcddeltaz=dpcddeltaz(3,:);
0112 
0113 dxcdrot1=dpcdrot1(1,:);
0114 dycdrot1=dpcdrot1(2,:);
0115 dzcdrot1=dpcdrot1(3,:);
0116 
0117 dxcdrot2=dpcdrot2(1,:);
0118 dycdrot2=dpcdrot2(2,:);
0119 dzcdrot2=dpcdrot2(3,:);
0120 
0121 dxcdrot3=dpcdrot3(1,:);
0122 dycdrot3=dpcdrot3(2,:);
0123 dzcdrot3=dpcdrot3(3,:);
0124 
0125 % derivatives over xc
0126 dadxc=1./zc;
0127 % dbdxc=0;
0128 drdxc=a.*dadxc./r;
0129 daddxc=dadxc.*(1 + k(1).*r.^2 + k(2).*r.^4 + k(5).*r.^6)...
0130     +a.*(2.*k(1).*r.*drdxc + 4.*k(2).*r.^3.*drdxc + 6.*k(5).*r.^5.*drdxc)...
0131     +2.*k(3).*dadxc.*b +k(4).*(2.*r.*drdxc +4.*a.*dadxc);
0132 dbddxc=b.*(2.*k(1).*r.*drdxc + 4.*k(2).*r.^3.*drdxc + 6.*k(5).*r.^5.*drdxc)...
0133     +k(3).*2.*r.*drdxc +2.*k(4).*b.*dadxc;
0134 
0135 % derivatives over yc
0136 % dadyc=0;
0137 dbdyc=1./zc;
0138 drdyc=b.*dbdyc./r;
0139 daddyc=a.*(2.*k(1).*r.*drdyc + 4.*k(2).*r.^3.*drdyc + 6.*k(5).*r.^5.*drdyc)...
0140     +2.*k(3).*a.*dbdyc +k(4).*2.*r.*drdyc;
0141 dbddyc=dbdyc.*(1 + k(1).*r.^2 + k(2).*r.^4 + k(5).*r.^6)...
0142     +b.*(2.*k(1).*r.*drdyc + 4.*k(2).*r.^3.*drdyc + 6.*k(5).*r.^5.*drdyc)...
0143     +k(3).*(2.*r.*drdyc+4.*b.*dbdyc) +2.*k(4).*a.*dbdyc;
0144 
0145 % derivatives over zc
0146 dadzc=-xc./(zc.^2);
0147 dbdzc=-yc./(zc.^2);
0148 drdzc=(a.*dadzc+b.*dbdzc)./r;
0149 daddzc=dadzc.*(1 + k(1).*r.^2 + k(2).*r.^4 + k(5).*r.^6)...
0150     +a.*(2.*k(1).*r.*drdzc + 4.*k(2).*r.^3.*drdzc + 6.*k(5).*r.^5.*drdzc)...
0151     +2.*k(3).*(a.*dbdzc + b.*dadzc) + k(4).*(2.*r.*drdzc+4.*a.*dadzc);
0152 dbddzc=dbdzc.*(1 + k(1).*r.^2 + k(2).*r.^4 + k(5).*r.^6)...
0153     +b.*(2.*k(1).*r.*drdzc + 4.*k(2).*r.^3.*drdzc + 6.*k(5).*r.^5.*drdzc)...
0154     +k(3).*(2.*r.*drdzc+4.*b.*dbdzc)+2.*k(4).*(a.*dbdzc+b.*dadzc);
0155 
0156 %
0157 % x Jacobian
0158 %
0159 dxdxc=f(1).*(daddxc+alpha.*dbddxc);
0160 dxdyc=f(1).*(daddyc+alpha.*dbddyc);
0161 dxdzc=f(1).*(daddzc+alpha.*dbddzc);
0162 
0163 dxddeltax=dxdxc.*dxcddeltax+dxdyc.*dycddeltax+dxdzc.*dzcddeltax;
0164 dxddeltay=dxdxc.*dxcddeltay+dxdyc.*dycddeltay+dxdzc.*dzcddeltay;
0165 dxddeltaz=dxdxc.*dxcddeltaz+dxdyc.*dycddeltaz+dxdzc.*dzcddeltaz;
0166 
0167 dxdrot1=dxdxc.*dxcdrot1+dxdyc.*dycdrot1+dxdzc.*dzcdrot1;
0168 dxdrot2=dxdxc.*dxcdrot2+dxdyc.*dycdrot2+dxdzc.*dzcdrot2;
0169 dxdrot3=dxdxc.*dxcdrot3+dxdyc.*dycdrot3+dxdzc.*dzcdrot3;
0170 
0171 dxdf1=ad.*alpha.*bd;
0172 dxdf2=zeros(size(ad));
0173 dxdc1=ones(size(ad));
0174 dxdc2=zeros(size(ad));
0175 dxdk1=f(1).*(a.*r.^2 + alpha.*b.*r.^2);
0176 dxdk2=f(1).*(a.*r.^4 + alpha.*b.*r.^4);
0177 dxdk3=f(1).*(2.*a.*b + alpha.*(r.^2+2.*b.^2));
0178 dxdk4=f(1).*(r.^2+2.*a.^2+alpha.*(2.*a.*b));
0179 dxdk5=f(1).*(a.*r.^6 + alpha.*(b.*r.^6));
0180 dxdalpha=f(1).*bd;
0181 
0182 Jx=[dxddeltax',dxddeltay',dxddeltaz',dxdrot1',dxdrot2',dxdrot3',dxdf1',dxdf2',dxdc1',dxdc2',dxdk1',dxdk2',dxdk3',dxdk4',dxdk5',dxdalpha'];
0183 
0184 % y Jacobian
0185 
0186 dydxc=f(2).*dbddxc;
0187 dydyc=f(2).*dbddyc;
0188 dydzc=f(2).*dbddzc;
0189 
0190 dyddeltax=dydxc.*dxcddeltax+dydyc.*dycddeltax+dydzc.*dzcddeltax;
0191 dyddeltay=dydxc.*dxcddeltay+dydyc.*dycddeltay+dydzc.*dzcddeltay;
0192 dyddeltaz=dydxc.*dxcddeltaz+dydyc.*dycddeltaz+dydzc.*dzcddeltaz;
0193 
0194 dydrot1=dydxc.*dxcdrot1+dydyc.*dycdrot1+dydzc.*dzcdrot1;
0195 dydrot2=dydxc.*dxcdrot2+dydyc.*dycdrot2+dydzc.*dzcdrot2;
0196 dydrot3=dydxc.*dxcdrot3+dydyc.*dycdrot3+dydzc.*dzcdrot3;
0197 
0198 
0199 dydf1=zeros(size(bd));
0200 dydf2=bd;
0201 dydc1=zeros(size(bd));
0202 dydc2=ones(size(bd));
0203 dydk1=f(2).*b.*r.^2;
0204 dydk2=f(2).*b.*r.^4;
0205 dydk3=f(2).*(r.^2+2.*b.^2);
0206 dydk4=f(2).*(2.*a.*b);
0207 dydk5=f(2).*b.*r.^6;
0208 dydalpha=zeros(size(bd));
0209 
0210 Jy=[dyddeltax',dyddeltay',dyddeltaz',dydrot1',dydrot2',dydrot3',dydf1',dydf2',dydc1',dydc2',dydk1',dydk2',dydk3',dydk4',dydk5',dydalpha'];
0211 
0212 % error matrix
0213 Q=diag([deltae',rote',fe',ce',ke',alphae'].^2); % square errors
0214 % Q=diag([deltae',rote',zeros(size(fe')),zeros(size(ce')),zeros(size(ke')),zeros(size(alphae'))].^2); % square errors
0215 % Q=diag([1,0,0,0,0,0,0,0,0,0,0].^2); % square errors
0216 % Q1(4:6,4:6)=Q1(4:6,4:6)*1e6;
0217 % Q1(4:6,1:3)=Q1(4:6,1:3)*1e3;
0218 % Q1(1:3,4:6)=Q1(1:3,4:6)*1e3;
0219 % Q(1:6,1:6)=Q1;
0220 
0221 unx2=diag(Jx*Q*Jx');
0222 uny2=diag(Jy*Q*Jy');
0223 
0224 errorx=sqrt(unx2)';
0225 errory=sqrt(uny2)';
0226 
0227 
0228 %get only pixels within the image
0229 validindices=find(x<=size(im,2) & x>0 & y<=size(im,1) & y>0);
0230 % invind=x>size(im,2) | x<1 | y>size(im,1) | y<1;
0231 
0232 px=x(validindices);
0233 py=y(validindices);
0234 perrorx=errorx(validindices);
0235 perrory=errory(validindices);
0236 
0237 %% Display
0238 
0239 figure;
0240 warning('off','Images:initSize:adjustingMag');
0241 imshow(im,[]);
0242 warning('on','Images:initSize:adjustingMag');
0243 hold on;
0244 c=lines(length(px));
0245 scatter(px,py,[],c,'+');
0246 nspdpts=20;
0247 c=repmat(c,nspdpts,1);
0248 % xspd=repmat(x,nspdpts,1)+repmat(errorx,nspdpts,1).*randn(nspdpts,length(x));
0249 % yspd=repmat(y,nspdpts,1)+repmat(errory,nspdpts,1).*randn(nspdpts,length(y));
0250 
0251 % scatter(xspd(:),yspd(:),'+');
0252 ellipse(2.*perrorx,2.*perrory,zeros(size(x)),px,py,c);
0253 % keyboard;
0254 
0255