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*/
db_CauchyDerivative(double J[4],double fp[2],const double f[2],double one_over_scale2)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
db_SquaredReprojectionErrorHomography(const double y[2],const double H[9],const double x[3])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
db_SquaredInhomogenousHomographyError(const double y[2],const double H[9],const double x[2])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*/
db_ExpCauchyInhomogenousHomographyError(const double y[2],const double H[9],const double x[2],double one_over_scale2)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*/
db_DerivativeInhomHomographyError(double Jf_dx[18],double f[2],const double y[2],const double H[9],const double x[2])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*/
db_DerivativeCauchyInhomHomographyReprojection(double Jf_dx[18],double f[2],const double y[2],const double H[9],const double x[2],double one_over_scale2)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*/
db_DerivativeInhomRotationReprojection(double Jf_dx[6],double f[2],const double y[2],const double R[9],const double x[2])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*/
db_DerivativeCauchyInhomRotationReprojection(double Jf_dx[6],double f[2],const double y[2],const double R[9],const double x[2],double one_over_scale2)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