1 /* 2 * Copyright (C) 2011 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 /* $Id: db_metrics.h,v 1.3 2011/06/17 14:03:31 mbansal Exp $ */ 18 19 #ifndef DB_METRICS 20 #define DB_METRICS 21 22 23 24 /***************************************************************** 25 * Lean and mean begins here * 26 *****************************************************************/ 27 28 #include "db_utilities.h" 29 /*! 30 * \defgroup LMMetrics (LM) Metrics 31 */ 32 /*\{*/ 33 34 35 36 37 /*! 38 Compute function value fp and Jacobian J of robustifier given input value f*/ 39 inline void db_CauchyDerivative(double J[4],double fp[2],const double f[2],double one_over_scale2) 40 { 41 double x2,y2,r,r2,r2s,one_over_r2,fu,r_fu,one_over_r_fu; 42 double one_plus_r2s,half_dfu_dx,half_dfu_dy,coeff,coeff2,coeff3; 43 int at_zero; 44 45 /*The robustifier takes the input (x,y) and makes a new 46 vector (xp,yp) where 47 xp=sqrt(log(1+(x^2+y^2)*one_over_scale2))*x/sqrt(x^2+y^2) 48 yp=sqrt(log(1+(x^2+y^2)*one_over_scale2))*y/sqrt(x^2+y^2) 49 The new vector has the property 50 xp^2+yp^2=log(1+(x^2+y^2)*one_over_scale2) 51 i.e. when it is square-summed it gives the robust 52 reprojection error 53 Define 54 r2=(x^2+y^2) and 55 r2s=r2*one_over_scale2 56 fu=log(1+r2s)/r2 57 then 58 xp=sqrt(fu)*x 59 yp=sqrt(fu)*y 60 and 61 d(r2)/dx=2x 62 d(r2)/dy=2y 63 and 64 dfu/dx=d(r2)/dx*(r2s/(1+r2s)-log(1+r2s))/(r2*r2) 65 dfu/dy=d(r2)/dy*(r2s/(1+r2s)-log(1+r2s))/(r2*r2) 66 and 67 d(xp)/dx=1/(2sqrt(fu))*(dfu/dx)*x+sqrt(fu) 68 d(xp)/dy=1/(2sqrt(fu))*(dfu/dy)*x 69 d(yp)/dx=1/(2sqrt(fu))*(dfu/dx)*y 70 d(yp)/dy=1/(2sqrt(fu))*(dfu/dy)*y+sqrt(fu) 71 */ 72 73 x2=db_sqr(f[0]); 74 y2=db_sqr(f[1]); 75 r2=x2+y2; 76 r=sqrt(r2); 77 78 if(r2<=0.0) at_zero=1; 79 else 80 { 81 one_over_r2=1.0/r2; 82 r2s=r2*one_over_scale2; 83 one_plus_r2s=1.0+r2s; 84 fu=log(one_plus_r2s)*one_over_r2; 85 r_fu=sqrt(fu); 86 if(r_fu<=0.0) at_zero=1; 87 else 88 { 89 one_over_r_fu=1.0/r_fu; 90 fp[0]=r_fu*f[0]; 91 fp[1]=r_fu*f[1]; 92 /*r2s is always >= 0*/ 93 coeff=(r2s/one_plus_r2s*one_over_r2-fu)*one_over_r2; 94 half_dfu_dx=f[0]*coeff; 95 half_dfu_dy=f[1]*coeff; 96 coeff2=one_over_r_fu*half_dfu_dx; 97 coeff3=one_over_r_fu*half_dfu_dy; 98 99 J[0]=coeff2*f[0]+r_fu; 100 J[1]=coeff3*f[0]; 101 J[2]=coeff2*f[1]; 102 J[3]=coeff3*f[1]+r_fu; 103 at_zero=0; 104 } 105 } 106 if(at_zero) 107 { 108 /*Close to zero the robustifying mapping 109 becomes identity*sqrt(one_over_scale2)*/ 110 fp[0]=0.0; 111 fp[1]=0.0; 112 J[0]=sqrt(one_over_scale2); 113 J[1]=0.0; 114 J[2]=0.0; 115 J[3]=J[0]; 116 } 117 } 118 119 inline double db_SquaredReprojectionErrorHomography(const double y[2],const double H[9],const double x[3]) 120 { 121 double x0,x1,x2,mult; 122 double sd; 123 124 x0=H[0]*x[0]+H[1]*x[1]+H[2]*x[2]; 125 x1=H[3]*x[0]+H[4]*x[1]+H[5]*x[2]; 126 x2=H[6]*x[0]+H[7]*x[1]+H[8]*x[2]; 127 mult=1.0/((x2!=0.0)?x2:1.0); 128 sd=db_sqr((y[0]-x0*mult))+db_sqr((y[1]-x1*mult)); 129 130 return(sd); 131 } 132 133 inline double db_SquaredInhomogenousHomographyError(const double y[2],const double H[9],const double x[2]) 134 { 135 double x0,x1,x2,mult; 136 double sd; 137 138 x0=H[0]*x[0]+H[1]*x[1]+H[2]; 139 x1=H[3]*x[0]+H[4]*x[1]+H[5]; 140 x2=H[6]*x[0]+H[7]*x[1]+H[8]; 141 mult=1.0/((x2!=0.0)?x2:1.0); 142 sd=db_sqr((y[0]-x0*mult))+db_sqr((y[1]-x1*mult)); 143 144 return(sd); 145 } 146 147 /*! 148 Return a constant divided by likelihood of a Cauchy distributed 149 reprojection error given the image point y, homography H, image point 150 point x and the squared scale coefficient one_over_scale2=1.0/(scale*scale) 151 where scale is the half width at half maximum (hWahM) of the 152 Cauchy distribution*/ 153 inline double db_ExpCauchyInhomogenousHomographyError(const double y[2],const double H[9],const double x[2], 154 double one_over_scale2) 155 { 156 double sd; 157 sd=db_SquaredInhomogenousHomographyError(y,H,x); 158 return(1.0+sd*one_over_scale2); 159 } 160 161 /*! 162 Compute residual vector f between image point y and homography Hx of 163 image point x. Also compute Jacobian of f with respect 164 to an update dx of H*/ 165 inline void db_DerivativeInhomHomographyError(double Jf_dx[18],double f[2],const double y[2],const double H[9], 166 const double x[2]) 167 { 168 double xh,yh,zh,mult,mult2,xh_mult2,yh_mult2; 169 /*The Jacobian of the inhomogenous coordinates with respect to 170 the homogenous is 171 [1/zh 0 -xh/(zh*zh)] 172 [ 0 1/zh -yh/(zh*zh)] 173 The Jacobian of the homogenous coordinates with respect to dH is 174 [x0 x1 1 0 0 0 0 0 0] 175 [ 0 0 0 x0 x1 1 0 0 0] 176 [ 0 0 0 0 0 0 x0 x1 1] 177 The output Jacobian is minus their product, i.e. 178 [-x0/zh -x1/zh -1/zh 0 0 0 x0*xh/(zh*zh) x1*xh/(zh*zh) xh/(zh*zh)] 179 [ 0 0 0 -x0/zh -x1/zh -1/zh x0*yh/(zh*zh) x1*yh/(zh*zh) yh/(zh*zh)]*/ 180 181 /*Compute warped point, which is the same as 182 homogenous coordinates of reprojection*/ 183 xh=H[0]*x[0]+H[1]*x[1]+H[2]; 184 yh=H[3]*x[0]+H[4]*x[1]+H[5]; 185 zh=H[6]*x[0]+H[7]*x[1]+H[8]; 186 mult=1.0/((zh!=0.0)?zh:1.0); 187 /*Compute inhomogenous residual*/ 188 f[0]=y[0]-xh*mult; 189 f[1]=y[1]-yh*mult; 190 /*Compute Jacobian*/ 191 mult2=mult*mult; 192 xh_mult2=xh*mult2; 193 yh_mult2=yh*mult2; 194 Jf_dx[0]= -x[0]*mult; 195 Jf_dx[1]= -x[1]*mult; 196 Jf_dx[2]= -mult; 197 Jf_dx[3]=0; 198 Jf_dx[4]=0; 199 Jf_dx[5]=0; 200 Jf_dx[6]=x[0]*xh_mult2; 201 Jf_dx[7]=x[1]*xh_mult2; 202 Jf_dx[8]=xh_mult2; 203 Jf_dx[9]=0; 204 Jf_dx[10]=0; 205 Jf_dx[11]=0; 206 Jf_dx[12]=Jf_dx[0]; 207 Jf_dx[13]=Jf_dx[1]; 208 Jf_dx[14]=Jf_dx[2]; 209 Jf_dx[15]=x[0]*yh_mult2; 210 Jf_dx[16]=x[1]*yh_mult2; 211 Jf_dx[17]=yh_mult2; 212 } 213 214 /*! 215 Compute robust residual vector f between image point y and homography Hx of 216 image point x. Also compute Jacobian of f with respect 217 to an update dH of H*/ 218 inline void db_DerivativeCauchyInhomHomographyReprojection(double Jf_dx[18],double f[2],const double y[2],const double H[9], 219 const double x[2],double one_over_scale2) 220 { 221 double Jf_dx_loc[18],f_loc[2]; 222 double J[4],J0,J1,J2,J3; 223 224 /*Compute reprojection Jacobian*/ 225 db_DerivativeInhomHomographyError(Jf_dx_loc,f_loc,y,H,x); 226 /*Compute robustifier Jacobian*/ 227 db_CauchyDerivative(J,f,f_loc,one_over_scale2); 228 229 /*Multiply the robustifier Jacobian with 230 the reprojection Jacobian*/ 231 J0=J[0];J1=J[1];J2=J[2];J3=J[3]; 232 Jf_dx[0]=J0*Jf_dx_loc[0]; 233 Jf_dx[1]=J0*Jf_dx_loc[1]; 234 Jf_dx[2]=J0*Jf_dx_loc[2]; 235 Jf_dx[3]= J1*Jf_dx_loc[12]; 236 Jf_dx[4]= J1*Jf_dx_loc[13]; 237 Jf_dx[5]= J1*Jf_dx_loc[14]; 238 Jf_dx[6]=J0*Jf_dx_loc[6]+J1*Jf_dx_loc[15]; 239 Jf_dx[7]=J0*Jf_dx_loc[7]+J1*Jf_dx_loc[16]; 240 Jf_dx[8]=J0*Jf_dx_loc[8]+J1*Jf_dx_loc[17]; 241 Jf_dx[9]= J2*Jf_dx_loc[0]; 242 Jf_dx[10]=J2*Jf_dx_loc[1]; 243 Jf_dx[11]=J2*Jf_dx_loc[2]; 244 Jf_dx[12]= J3*Jf_dx_loc[12]; 245 Jf_dx[13]= J3*Jf_dx_loc[13]; 246 Jf_dx[14]= J3*Jf_dx_loc[14]; 247 Jf_dx[15]=J2*Jf_dx_loc[6]+J3*Jf_dx_loc[15]; 248 Jf_dx[16]=J2*Jf_dx_loc[7]+J3*Jf_dx_loc[16]; 249 Jf_dx[17]=J2*Jf_dx_loc[8]+J3*Jf_dx_loc[17]; 250 } 251 /*! 252 Compute residual vector f between image point y and rotation of 253 image point x by R. Also compute Jacobian of f with respect 254 to an update dx of R*/ 255 inline void db_DerivativeInhomRotationReprojection(double Jf_dx[6],double f[2],const double y[2],const double R[9], 256 const double x[2]) 257 { 258 double xh,yh,zh,mult,mult2,xh_mult2,yh_mult2; 259 /*The Jacobian of the inhomogenous coordinates with respect to 260 the homogenous is 261 [1/zh 0 -xh/(zh*zh)] 262 [ 0 1/zh -yh/(zh*zh)] 263 The Jacobian at zero of the homogenous coordinates with respect to 264 [sin(phi) sin(ohm) sin(kap)] is 265 [-rx2 0 rx1 ] 266 [ 0 rx2 -rx0 ] 267 [ rx0 -rx1 0 ] 268 The output Jacobian is minus their product, i.e. 269 [1+xh*xh/(zh*zh) -xh*yh/(zh*zh) -yh/zh] 270 [xh*yh/(zh*zh) -1-yh*yh/(zh*zh) xh/zh]*/ 271 272 /*Compute rotated point, which is the same as 273 homogenous coordinates of reprojection*/ 274 xh=R[0]*x[0]+R[1]*x[1]+R[2]; 275 yh=R[3]*x[0]+R[4]*x[1]+R[5]; 276 zh=R[6]*x[0]+R[7]*x[1]+R[8]; 277 mult=1.0/((zh!=0.0)?zh:1.0); 278 /*Compute inhomogenous residual*/ 279 f[0]=y[0]-xh*mult; 280 f[1]=y[1]-yh*mult; 281 /*Compute Jacobian*/ 282 mult2=mult*mult; 283 xh_mult2=xh*mult2; 284 yh_mult2=yh*mult2; 285 Jf_dx[0]= 1.0+xh*xh_mult2; 286 Jf_dx[1]= -yh*xh_mult2; 287 Jf_dx[2]= -yh*mult; 288 Jf_dx[3]= -Jf_dx[1]; 289 Jf_dx[4]= -1-yh*yh_mult2; 290 Jf_dx[5]= xh*mult; 291 } 292 293 /*! 294 Compute robust residual vector f between image point y and rotation of 295 image point x by R. Also compute Jacobian of f with respect 296 to an update dx of R*/ 297 inline void db_DerivativeCauchyInhomRotationReprojection(double Jf_dx[6],double f[2],const double y[2],const double R[9], 298 const double x[2],double one_over_scale2) 299 { 300 double Jf_dx_loc[6],f_loc[2]; 301 double J[4],J0,J1,J2,J3; 302 303 /*Compute reprojection Jacobian*/ 304 db_DerivativeInhomRotationReprojection(Jf_dx_loc,f_loc,y,R,x); 305 /*Compute robustifier Jacobian*/ 306 db_CauchyDerivative(J,f,f_loc,one_over_scale2); 307 308 /*Multiply the robustifier Jacobian with 309 the reprojection Jacobian*/ 310 J0=J[0];J1=J[1];J2=J[2];J3=J[3]; 311 Jf_dx[0]=J0*Jf_dx_loc[0]+J1*Jf_dx_loc[3]; 312 Jf_dx[1]=J0*Jf_dx_loc[1]+J1*Jf_dx_loc[4]; 313 Jf_dx[2]=J0*Jf_dx_loc[2]+J1*Jf_dx_loc[5]; 314 Jf_dx[3]=J2*Jf_dx_loc[0]+J3*Jf_dx_loc[3]; 315 Jf_dx[4]=J2*Jf_dx_loc[1]+J3*Jf_dx_loc[4]; 316 Jf_dx[5]=J2*Jf_dx_loc[2]+J3*Jf_dx_loc[5]; 317 } 318 319 320 321 /*! 322 // remove the outliers whose projection error is larger than pre-defined 323 */ 324 inline int db_RemoveOutliers_Homography(const double H[9], double *x_i,double *xp_i, double *wp,double *im, double *im_p, double *im_r, double *im_raw,double *im_raw_p,int point_count,double scale, double thresh=DB_OUTLIER_THRESHOLD) 325 { 326 double temp_valueE, t2; 327 int c; 328 int k1=0; 329 int k2=0; 330 int k3=0; 331 int numinliers=0; 332 int ind1; 333 int ind2; 334 int ind3; 335 int isinlier; 336 337 // experimentally determined 338 t2=1.0/(thresh*thresh*thresh*thresh); 339 340 // count the inliers 341 for(c=0;c<point_count;c++) 342 { 343 ind1=c<<1; 344 ind2=c<<2; 345 ind3=3*c; 346 347 temp_valueE=db_SquaredInhomogenousHomographyError(im_p+ind3,H,im+ind3); 348 349 isinlier=((temp_valueE<=t2)?1:0); 350 351 // if it is inlier, then copy the 3d and 2d correspondences 352 if (isinlier) 353 { 354 numinliers++; 355 356 x_i[k1]=x_i[ind1]; 357 x_i[k1+1]=x_i[ind1+1]; 358 359 xp_i[k1]=xp_i[ind1]; 360 xp_i[k1+1]=xp_i[ind1+1]; 361 362 k1=k1+2; 363 364 // original normalized pixel coordinates 365 im[k3]=im[ind3]; 366 im[k3+1]=im[ind3+1]; 367 im[k3+2]=im[ind3+2]; 368 369 im_r[k3]=im_r[ind3]; 370 im_r[k3+1]=im_r[ind3+1]; 371 im_r[k3+2]=im_r[ind3+2]; 372 373 im_p[k3]=im_p[ind3]; 374 im_p[k3+1]=im_p[ind3+1]; 375 im_p[k3+2]=im_p[ind3+2]; 376 377 // left and right raw pixel coordinates 378 im_raw[k3] = im_raw[ind3]; 379 im_raw[k3+1] = im_raw[ind3+1]; 380 im_raw[k3+2] = im_raw[ind3+2]; // the index 381 382 im_raw_p[k3] = im_raw_p[ind3]; 383 im_raw_p[k3+1] = im_raw_p[ind3+1]; 384 im_raw_p[k3+2] = im_raw_p[ind3+2]; // the index 385 386 k3=k3+3; 387 388 // 3D coordinates 389 wp[k2]=wp[ind2]; 390 wp[k2+1]=wp[ind2+1]; 391 wp[k2+2]=wp[ind2+2]; 392 wp[k2+3]=wp[ind2+3]; 393 394 k2=k2+4; 395 396 } 397 } 398 399 return numinliers; 400 } 401 402 403 404 405 406 /*\}*/ 407 408 #endif /* DB_METRICS */ 409
