Sascha Haeberling | 8bddf8c | 2013-08-14 11:20:34 -0700 | [diff] [blame] | 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 */ |