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