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_rob_image_homography.cpp,v 1.2 2011/06/17 14:03:31 mbansal Exp $ */
18
19 #include "db_utilities.h"
20 #include "db_rob_image_homography.h"
21 #include "db_bundle.h"
22
23
24
25 /*****************************************************************
26 * Lean and mean begins here *
27 *****************************************************************/
28
29 #include "db_image_homography.h"
30
31 #ifdef _VERBOSE_
32 #include <iostream>
33 using namespace std;
34 #endif /*VERBOSE*/
35
db_RobImageHomography_Cost(double H[9],int point_count,double * x_i,double * xp_i,double one_over_scale2)36 inline double db_RobImageHomography_Cost(double H[9],int point_count,double *x_i,double *xp_i,double one_over_scale2)
37 {
38 int c;
39 double back,acc,*x_i_temp,*xp_i_temp;
40
41 for(back=0.0,c=0;c<point_count;)
42 {
43 /*Take log of product of ten reprojection
44 errors to reduce nr of expensive log operations*/
45 if(c+9<point_count)
46 {
47 x_i_temp=x_i+(c<<1);
48 xp_i_temp=xp_i+(c<<1);
49
50 acc=db_ExpCauchyInhomogenousHomographyError(xp_i_temp,H,x_i_temp,one_over_scale2);
51 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+2,H,x_i_temp+2,one_over_scale2);
52 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+4,H,x_i_temp+4,one_over_scale2);
53 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+6,H,x_i_temp+6,one_over_scale2);
54 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+8,H,x_i_temp+8,one_over_scale2);
55 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+10,H,x_i_temp+10,one_over_scale2);
56 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+12,H,x_i_temp+12,one_over_scale2);
57 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+14,H,x_i_temp+14,one_over_scale2);
58 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+16,H,x_i_temp+16,one_over_scale2);
59 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+18,H,x_i_temp+18,one_over_scale2);
60 c+=10;
61 }
62 else
63 {
64 for(acc=1.0;c<point_count;c++)
65 {
66 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i+(c<<1),H,x_i+(c<<1),one_over_scale2);
67 }
68 }
69 back+=log(acc);
70 }
71 return(back);
72 }
73
db_RobImageHomography_Statistics(double H[9],int point_count,double * x_i,double * xp_i,double one_over_scale2,db_Statistics * stat,double thresh=DB_OUTLIER_THRESHOLD)74 inline double db_RobImageHomography_Statistics(double H[9],int point_count,double *x_i,double *xp_i,double one_over_scale2,db_Statistics *stat,double thresh=DB_OUTLIER_THRESHOLD)
75 {
76 int c,i;
77 double t2,frac;
78
79 t2=thresh*thresh;
80 for(i=0,c=0;c<point_count;c++)
81 {
82 i+=(db_SquaredInhomogenousHomographyError(xp_i+(c<<1),H,x_i+(c<<1))*one_over_scale2<=t2)?1:0;
83 }
84 frac=((double)i)/((double)(db_maxi(point_count,1)));
85
86 #ifdef _VERBOSE_
87 std::cout << "Inlier Percentage RobImageHomography: " << frac*100.0 << "% out of " << point_count << " constraints" << std::endl;
88 #endif /*_VERBOSE_*/
89
90 if(stat)
91 {
92 stat->nr_points=point_count;
93 stat->one_over_scale2=one_over_scale2;
94 stat->nr_inliers=i;
95 stat->inlier_fraction=frac;
96
97 stat->cost=db_RobImageHomography_Cost(H,point_count,x_i,xp_i,one_over_scale2);
98 stat->model_dimension=0;
99 /*stat->nr_parameters=;*/
100
101 stat->lambda1=log(4.0);
102 stat->lambda2=log(4.0*((double)db_maxi(1,stat->nr_points)));
103 stat->lambda3=10.0;
104 stat->gric=stat->cost+stat->lambda1*stat->model_dimension*((double)stat->nr_points)+stat->lambda2*((double)stat->nr_parameters);
105 stat->inlier_evidence=((double)stat->nr_inliers)-stat->lambda3*((double)stat->nr_parameters);
106 }
107
108 return(frac);
109 }
110
111 /*Compute min_Jtf and upper right of JtJ. Return cost.*/
db_RobImageHomography_Jacobians(double JtJ[81],double min_Jtf[9],double H[9],int point_count,double * x_i,double * xp_i,double one_over_scale2)112 inline double db_RobImageHomography_Jacobians(double JtJ[81],double min_Jtf[9],double H[9],int point_count,double *x_i,double *xp_i,double one_over_scale2)
113 {
114 double back,Jf_dx[18],f[2],temp,temp2;
115 int i;
116
117 db_Zero(JtJ,81);
118 db_Zero(min_Jtf,9);
119 for(back=0.0,i=0;i<point_count;i++)
120 {
121 /*Compute reprojection error vector and its Jacobian
122 for this point*/
123 db_DerivativeCauchyInhomHomographyReprojection(Jf_dx,f,xp_i+(i<<1),H,x_i+(i<<1),one_over_scale2);
124 /*Perform
125 min_Jtf-=Jf_dx*f[0] and
126 min_Jtf-=(Jf_dx+9)*f[1] to accumulate -Jt%f*/
127 db_RowOperation9(min_Jtf,Jf_dx,f[0]);
128 db_RowOperation9(min_Jtf,Jf_dx+9,f[1]);
129 /*Accumulate upper right of JtJ with outer product*/
130 temp=Jf_dx[0]; temp2=Jf_dx[9];
131 JtJ[0]+=temp*Jf_dx[0]+temp2*Jf_dx[9];
132 JtJ[1]+=temp*Jf_dx[1]+temp2*Jf_dx[10];
133 JtJ[2]+=temp*Jf_dx[2]+temp2*Jf_dx[11];
134 JtJ[3]+=temp*Jf_dx[3]+temp2*Jf_dx[12];
135 JtJ[4]+=temp*Jf_dx[4]+temp2*Jf_dx[13];
136 JtJ[5]+=temp*Jf_dx[5]+temp2*Jf_dx[14];
137 JtJ[6]+=temp*Jf_dx[6]+temp2*Jf_dx[15];
138 JtJ[7]+=temp*Jf_dx[7]+temp2*Jf_dx[16];
139 JtJ[8]+=temp*Jf_dx[8]+temp2*Jf_dx[17];
140 temp=Jf_dx[1]; temp2=Jf_dx[10];
141 JtJ[10]+=temp*Jf_dx[1]+temp2*Jf_dx[10];
142 JtJ[11]+=temp*Jf_dx[2]+temp2*Jf_dx[11];
143 JtJ[12]+=temp*Jf_dx[3]+temp2*Jf_dx[12];
144 JtJ[13]+=temp*Jf_dx[4]+temp2*Jf_dx[13];
145 JtJ[14]+=temp*Jf_dx[5]+temp2*Jf_dx[14];
146 JtJ[15]+=temp*Jf_dx[6]+temp2*Jf_dx[15];
147 JtJ[16]+=temp*Jf_dx[7]+temp2*Jf_dx[16];
148 JtJ[17]+=temp*Jf_dx[8]+temp2*Jf_dx[17];
149 temp=Jf_dx[2]; temp2=Jf_dx[11];
150 JtJ[20]+=temp*Jf_dx[2]+temp2*Jf_dx[11];
151 JtJ[21]+=temp*Jf_dx[3]+temp2*Jf_dx[12];
152 JtJ[22]+=temp*Jf_dx[4]+temp2*Jf_dx[13];
153 JtJ[23]+=temp*Jf_dx[5]+temp2*Jf_dx[14];
154 JtJ[24]+=temp*Jf_dx[6]+temp2*Jf_dx[15];
155 JtJ[25]+=temp*Jf_dx[7]+temp2*Jf_dx[16];
156 JtJ[26]+=temp*Jf_dx[8]+temp2*Jf_dx[17];
157 temp=Jf_dx[3]; temp2=Jf_dx[12];
158 JtJ[30]+=temp*Jf_dx[3]+temp2*Jf_dx[12];
159 JtJ[31]+=temp*Jf_dx[4]+temp2*Jf_dx[13];
160 JtJ[32]+=temp*Jf_dx[5]+temp2*Jf_dx[14];
161 JtJ[33]+=temp*Jf_dx[6]+temp2*Jf_dx[15];
162 JtJ[34]+=temp*Jf_dx[7]+temp2*Jf_dx[16];
163 JtJ[35]+=temp*Jf_dx[8]+temp2*Jf_dx[17];
164 temp=Jf_dx[4]; temp2=Jf_dx[13];
165 JtJ[40]+=temp*Jf_dx[4]+temp2*Jf_dx[13];
166 JtJ[41]+=temp*Jf_dx[5]+temp2*Jf_dx[14];
167 JtJ[42]+=temp*Jf_dx[6]+temp2*Jf_dx[15];
168 JtJ[43]+=temp*Jf_dx[7]+temp2*Jf_dx[16];
169 JtJ[44]+=temp*Jf_dx[8]+temp2*Jf_dx[17];
170 temp=Jf_dx[5]; temp2=Jf_dx[14];
171 JtJ[50]+=temp*Jf_dx[5]+temp2*Jf_dx[14];
172 JtJ[51]+=temp*Jf_dx[6]+temp2*Jf_dx[15];
173 JtJ[52]+=temp*Jf_dx[7]+temp2*Jf_dx[16];
174 JtJ[53]+=temp*Jf_dx[8]+temp2*Jf_dx[17];
175 temp=Jf_dx[6]; temp2=Jf_dx[15];
176 JtJ[60]+=temp*Jf_dx[6]+temp2*Jf_dx[15];
177 JtJ[61]+=temp*Jf_dx[7]+temp2*Jf_dx[16];
178 JtJ[62]+=temp*Jf_dx[8]+temp2*Jf_dx[17];
179 temp=Jf_dx[7]; temp2=Jf_dx[16];
180 JtJ[70]+=temp*Jf_dx[7]+temp2*Jf_dx[16];
181 JtJ[71]+=temp*Jf_dx[8]+temp2*Jf_dx[17];
182 temp=Jf_dx[8]; temp2=Jf_dx[17];
183 JtJ[80]+=temp*Jf_dx[8]+temp2*Jf_dx[17];
184
185 /*Add square-sum to cost*/
186 back+=db_sqr(f[0])+db_sqr(f[1]);
187 }
188
189 return(back);
190 }
191
192 /*Compute min_Jtf and upper right of JtJ. Return cost*/
db_RobCamRotation_Jacobians(double JtJ[9],double min_Jtf[3],double H[9],int point_count,double * x_i,double * xp_i,double one_over_scale2)193 inline double db_RobCamRotation_Jacobians(double JtJ[9],double min_Jtf[3],double H[9],int point_count,double *x_i,double *xp_i,double one_over_scale2)
194 {
195 double back,Jf_dx[6],f[2];
196 int i,j;
197
198 db_Zero(JtJ,9);
199 db_Zero(min_Jtf,3);
200 for(back=0.0,i=0;i<point_count;i++)
201 {
202 /*Compute reprojection error vector and its Jacobian
203 for this point*/
204 j=(i<<1);
205 db_DerivativeCauchyInhomRotationReprojection(Jf_dx,f,xp_i+j,H,x_i+j,one_over_scale2);
206 /*Perform
207 min_Jtf-=Jf_dx*f[0] and
208 min_Jtf-=(Jf_dx+3)*f[1] to accumulate -Jt%f*/
209 db_RowOperation3(min_Jtf,Jf_dx,f[0]);
210 db_RowOperation3(min_Jtf,Jf_dx+3,f[1]);
211 /*Accumulate upper right of JtJ with outer product*/
212 JtJ[0]+=Jf_dx[0]*Jf_dx[0]+Jf_dx[3]*Jf_dx[3];
213 JtJ[1]+=Jf_dx[0]*Jf_dx[1]+Jf_dx[3]*Jf_dx[4];
214 JtJ[2]+=Jf_dx[0]*Jf_dx[2]+Jf_dx[3]*Jf_dx[5];
215 JtJ[4]+=Jf_dx[1]*Jf_dx[1]+Jf_dx[4]*Jf_dx[4];
216 JtJ[5]+=Jf_dx[1]*Jf_dx[2]+Jf_dx[4]*Jf_dx[5];
217 JtJ[8]+=Jf_dx[2]*Jf_dx[2]+Jf_dx[5]*Jf_dx[5];
218
219 /*Add square-sum to cost*/
220 back+=db_sqr(f[0])+db_sqr(f[1]);
221 }
222
223 return(back);
224 }
225
db_RobCamRotation_Polish(double H[9],int point_count,double * x_i,double * xp_i,double one_over_scale2,int max_iterations,double improvement_requirement)226 void db_RobCamRotation_Polish(double H[9],int point_count,double *x_i,double *xp_i,double one_over_scale2,
227 int max_iterations,double improvement_requirement)
228 {
229 int i,update,stop;
230 double lambda,cost,current_cost;
231 double JtJ[9],min_Jtf[3],dx[3],H_p_dx[9];
232
233 lambda=0.001;
234 for(update=1,stop=0,i=0;(stop<2) && (i<max_iterations);i++)
235 {
236 /*if first time since improvement, compute Jacobian and residual*/
237 if(update)
238 {
239 current_cost=db_RobCamRotation_Jacobians(JtJ,min_Jtf,H,point_count,x_i,xp_i,one_over_scale2);
240 update=0;
241 }
242
243 #ifdef _VERBOSE_
244 /*std::cout << "Cost:" << current_cost << " ";*/
245 #endif /*_VERBOSE_*/
246
247 /*Come up with a hypothesis dx
248 based on the current lambda*/
249 db_Compute_dx_3x3(dx,JtJ,min_Jtf,lambda);
250
251 /*Compute Cost(x+dx)*/
252 db_UpdateRotation(H_p_dx,H,dx);
253 cost=db_RobImageHomography_Cost(H_p_dx,point_count,x_i,xp_i,one_over_scale2);
254
255 /*Is there an improvement?*/
256 if(cost<current_cost)
257 {
258 /*improvement*/
259 if(current_cost-cost<current_cost*improvement_requirement) stop++;
260 else stop=0;
261 lambda*=0.1;
262 /*Move to the hypothesised position x+dx*/
263 current_cost=cost;
264 db_Copy9(H,H_p_dx);
265 db_OrthonormalizeRotation(H);
266 update=1;
267
268 #ifdef _VERBOSE_
269 std::cout << "Step" << i << "Imp,Lambda=" << lambda << "Cost:" << current_cost << std::endl;
270 #endif /*_VERBOSE_*/
271 }
272 else
273 {
274 /*no improvement*/
275 lambda*=10.0;
276 stop=0;
277 }
278 }
279 }
280
db_RobImageHomographyFetchJacobian(double ** JtJ_ref,double * min_Jtf,double ** JtJ_temp_ref,double * min_Jtf_temp,int n,int * fetch_vector)281 inline void db_RobImageHomographyFetchJacobian(double **JtJ_ref,double *min_Jtf,double **JtJ_temp_ref,double *min_Jtf_temp,int n,int *fetch_vector)
282 {
283 int i,j,t;
284 double *t1,*t2;
285
286 for(i=0;i<n;i++)
287 {
288 t=fetch_vector[i];
289 min_Jtf[i]=min_Jtf_temp[t];
290 t1=JtJ_ref[i];
291 t2=JtJ_temp_ref[t];
292 for(j=i;j<n;j++)
293 {
294 t1[j]=t2[fetch_vector[j]];
295 }
296 }
297 }
298
db_RobImageHomographyMultiplyJacobian(double ** JtJ_ref,double * min_Jtf,double ** JtJ_temp_ref,double * min_Jtf_temp,double ** JE_dx_ref,int n)299 inline void db_RobImageHomographyMultiplyJacobian(double **JtJ_ref,double *min_Jtf,double **JtJ_temp_ref,double *min_Jtf_temp,double **JE_dx_ref,int n)
300 {
301 double JtJ_JE[72],*JtJ_JE_ref[9];
302
303 db_SetupMatrixRefs(JtJ_JE_ref,9,8,JtJ_JE);
304
305 db_SymmetricExtendUpperToLower(JtJ_temp_ref,9,9);
306 db_MultiplyMatricesAB(JtJ_JE_ref,JtJ_temp_ref,JE_dx_ref,9,9,n);
307 db_UpperMultiplyMatricesAtB(JtJ_ref,JE_dx_ref,JtJ_JE_ref,n,9,n);
308 db_MultiplyMatrixVectorAtb(min_Jtf,JE_dx_ref,min_Jtf_temp,n,9);
309 }
310
db_RobImageHomographyJH_Js(double ** JE_dx_ref,int j,double H[9])311 inline void db_RobImageHomographyJH_Js(double **JE_dx_ref,int j,double H[9])
312 {
313 /*Update of upper 2x2 is multiplication by
314 [s 0][ cos(theta) sin(theta)]
315 [0 s][-sin(theta) cos(theta)]*/
316 JE_dx_ref[0][j]=H[0];
317 JE_dx_ref[1][j]=H[1];
318 JE_dx_ref[2][j]=0;
319 JE_dx_ref[3][j]=H[2];
320 JE_dx_ref[4][j]=H[3];
321 JE_dx_ref[5][j]=0;
322 JE_dx_ref[6][j]=0;
323 JE_dx_ref[7][j]=0;
324 JE_dx_ref[8][j]=0;
325 }
326
db_RobImageHomographyJH_JR(double ** JE_dx_ref,int j,double H[9])327 inline void db_RobImageHomographyJH_JR(double **JE_dx_ref,int j,double H[9])
328 {
329 /*Update of upper 2x2 is multiplication by
330 [s 0][ cos(theta) sin(theta)]
331 [0 s][-sin(theta) cos(theta)]*/
332 JE_dx_ref[0][j]= H[3];
333 JE_dx_ref[1][j]= H[4];
334 JE_dx_ref[2][j]=0;
335 JE_dx_ref[3][j]= -H[0];
336 JE_dx_ref[4][j]= -H[1];
337 JE_dx_ref[5][j]=0;
338 JE_dx_ref[6][j]=0;
339 JE_dx_ref[7][j]=0;
340 JE_dx_ref[8][j]=0;
341 }
342
db_RobImageHomographyJH_Jt(double ** JE_dx_ref,int j,int k,double H[9])343 inline void db_RobImageHomographyJH_Jt(double **JE_dx_ref,int j,int k,double H[9])
344 {
345 JE_dx_ref[0][j]=0;
346 JE_dx_ref[1][j]=0;
347 JE_dx_ref[2][j]=1.0;
348 JE_dx_ref[3][j]=0;
349 JE_dx_ref[4][j]=0;
350 JE_dx_ref[5][j]=0;
351 JE_dx_ref[6][j]=0;
352 JE_dx_ref[7][j]=0;
353 JE_dx_ref[8][j]=0;
354
355 JE_dx_ref[0][k]=0;
356 JE_dx_ref[1][k]=0;
357 JE_dx_ref[2][k]=0;
358 JE_dx_ref[3][k]=0;
359 JE_dx_ref[4][k]=0;
360 JE_dx_ref[5][k]=1.0;
361 JE_dx_ref[6][k]=0;
362 JE_dx_ref[7][k]=0;
363 JE_dx_ref[8][k]=0;
364 }
365
db_RobImageHomographyJH_dRotFocal(double ** JE_dx_ref,int j,int k,int l,int m,double H[9])366 inline void db_RobImageHomographyJH_dRotFocal(double **JE_dx_ref,int j,int k,int l,int m,double H[9])
367 {
368 double f,fi,fi2;
369 double R[9],J[9];
370
371 /*Updated matrix is diag(f+df,f+df)*dR*R*diag(1/(f+df),1/(f+df),1)*/
372 f=db_FocalAndRotFromCamRotFocalHomography(R,H);
373 fi=db_SafeReciprocal(f);
374 fi2=db_sqr(fi);
375 db_JacobianOfRotatedPointStride(J,R,3);
376 JE_dx_ref[0][j]= J[0];
377 JE_dx_ref[1][j]= J[1];
378 JE_dx_ref[2][j]=f* J[2];
379 JE_dx_ref[3][j]= J[3];
380 JE_dx_ref[4][j]= J[4];
381 JE_dx_ref[5][j]=f* J[5];
382 JE_dx_ref[6][j]=fi*J[6];
383 JE_dx_ref[7][j]=fi*J[7];
384 JE_dx_ref[8][j]= J[8];
385 db_JacobianOfRotatedPointStride(J,R+1,3);
386 JE_dx_ref[0][k]= J[0];
387 JE_dx_ref[1][k]= J[1];
388 JE_dx_ref[2][k]=f* J[2];
389 JE_dx_ref[3][k]= J[3];
390 JE_dx_ref[4][k]= J[4];
391 JE_dx_ref[5][k]=f* J[5];
392 JE_dx_ref[6][k]=fi*J[6];
393 JE_dx_ref[7][k]=fi*J[7];
394 JE_dx_ref[8][k]= J[8];
395 db_JacobianOfRotatedPointStride(J,R+2,3);
396 JE_dx_ref[0][l]= J[0];
397 JE_dx_ref[1][l]= J[1];
398 JE_dx_ref[2][l]=f* J[2];
399 JE_dx_ref[3][l]= J[3];
400 JE_dx_ref[4][l]= J[4];
401 JE_dx_ref[5][l]=f* J[5];
402 JE_dx_ref[6][l]=fi*J[6];
403 JE_dx_ref[7][l]=fi*J[7];
404 JE_dx_ref[8][l]= J[8];
405
406 JE_dx_ref[0][m]=0;
407 JE_dx_ref[1][m]=0;
408 JE_dx_ref[2][m]=H[2];
409 JE_dx_ref[3][m]=0;
410 JE_dx_ref[4][m]=0;
411 JE_dx_ref[5][m]=H[5];
412 JE_dx_ref[6][m]= -fi2*H[6];
413 JE_dx_ref[7][m]= -fi2*H[7];
414 JE_dx_ref[8][m]=0;
415 }
416
db_RobImageHomography_Jacobians_Generic(double * JtJ_ref[8],double min_Jtf[8],int * num_param,int * frozen_coord,double H[9],int point_count,double * x_i,double * xp_i,int homography_type,double one_over_scale2)417 inline double db_RobImageHomography_Jacobians_Generic(double *JtJ_ref[8],double min_Jtf[8],int *num_param,int *frozen_coord,double H[9],int point_count,double *x_i,double *xp_i,int homography_type,double one_over_scale2)
418 {
419 double back;
420 int i,j,fetch_vector[8],n;
421 double JtJ_temp[81],min_Jtf_temp[9],JE_dx[72];
422 double *JE_dx_ref[9],*JtJ_temp_ref[9];
423
424 /*Compute cost and JtJ,min_Jtf with respect to H*/
425 back=db_RobImageHomography_Jacobians(JtJ_temp,min_Jtf_temp,H,point_count,x_i,xp_i,one_over_scale2);
426
427 /*Compute JtJ,min_Jtf with respect to the right parameters
428 The formulas are
429 JtJ=transpose(JE_dx)*JtJ*JE_dx and
430 min_Jtf=transpose(JE_dx)*min_Jtf,
431 where the 9xN matrix JE_dx is the Jacobian of H with respect
432 to the update*/
433 db_SetupMatrixRefs(JtJ_temp_ref,9,9,JtJ_temp);
434 db_SetupMatrixRefs(JE_dx_ref,9,8,JE_dx);
435 switch(homography_type)
436 {
437 case DB_HOMOGRAPHY_TYPE_SIMILARITY:
438 case DB_HOMOGRAPHY_TYPE_SIMILARITY_U:
439 n=4;
440 db_RobImageHomographyJH_Js(JE_dx_ref,0,H);
441 db_RobImageHomographyJH_JR(JE_dx_ref,1,H);
442 db_RobImageHomographyJH_Jt(JE_dx_ref,2,3,H);
443 db_RobImageHomographyMultiplyJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,JE_dx_ref,n);
444 break;
445 case DB_HOMOGRAPHY_TYPE_ROTATION:
446 case DB_HOMOGRAPHY_TYPE_ROTATION_U:
447 n=1;
448 db_RobImageHomographyJH_JR(JE_dx_ref,0,H);
449 db_RobImageHomographyMultiplyJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,JE_dx_ref,n);
450 break;
451 case DB_HOMOGRAPHY_TYPE_SCALING:
452 n=1;
453 db_RobImageHomographyJH_Js(JE_dx_ref,0,H);
454 db_RobImageHomographyMultiplyJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,JE_dx_ref,n);
455 break;
456 case DB_HOMOGRAPHY_TYPE_S_T:
457 n=3;
458 db_RobImageHomographyJH_Js(JE_dx_ref,0,H);
459 db_RobImageHomographyJH_Jt(JE_dx_ref,1,2,H);
460 db_RobImageHomographyMultiplyJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,JE_dx_ref,n);
461 break;
462 case DB_HOMOGRAPHY_TYPE_R_T:
463 n=3;
464 db_RobImageHomographyJH_JR(JE_dx_ref,0,H);
465 db_RobImageHomographyJH_Jt(JE_dx_ref,1,2,H);
466 db_RobImageHomographyMultiplyJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,JE_dx_ref,n);
467 break;
468 case DB_HOMOGRAPHY_TYPE_R_S:
469 n=2;
470 db_RobImageHomographyJH_Js(JE_dx_ref,0,H);
471 db_RobImageHomographyJH_JR(JE_dx_ref,1,H);
472 db_RobImageHomographyMultiplyJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,JE_dx_ref,n);
473 break;
474
475 case DB_HOMOGRAPHY_TYPE_TRANSLATION:
476 n=2;
477 fetch_vector[0]=2;
478 fetch_vector[1]=5;
479 db_RobImageHomographyFetchJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,n,fetch_vector);
480 break;
481 case DB_HOMOGRAPHY_TYPE_AFFINE:
482 n=6;
483 fetch_vector[0]=0;
484 fetch_vector[1]=1;
485 fetch_vector[2]=2;
486 fetch_vector[3]=3;
487 fetch_vector[4]=4;
488 fetch_vector[5]=5;
489 db_RobImageHomographyFetchJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,n,fetch_vector);
490 break;
491 case DB_HOMOGRAPHY_TYPE_PROJECTIVE:
492 n=8;
493 *frozen_coord=db_MaxAbsIndex9(H);
494 for(j=0,i=0;i<9;i++) if(i!=(*frozen_coord))
495 {
496 fetch_vector[j]=i;
497 j++;
498 }
499 db_RobImageHomographyFetchJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,n,fetch_vector);
500 break;
501 case DB_HOMOGRAPHY_TYPE_CAMROTATION_F:
502 case DB_HOMOGRAPHY_TYPE_CAMROTATION_F_UD:
503 n=4;
504 db_RobImageHomographyJH_dRotFocal(JE_dx_ref,0,1,2,3,H);
505 db_RobImageHomographyMultiplyJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,JE_dx_ref,n);
506 break;
507 }
508 *num_param=n;
509
510 return(back);
511 }
512
db_ImageHomographyUpdateGeneric(double H_p_dx[9],double H[9],double * dx,int homography_type,int frozen_coord)513 inline void db_ImageHomographyUpdateGeneric(double H_p_dx[9],double H[9],double *dx,int homography_type,int frozen_coord)
514 {
515 switch(homography_type)
516 {
517 case DB_HOMOGRAPHY_TYPE_SIMILARITY:
518 case DB_HOMOGRAPHY_TYPE_SIMILARITY_U:
519 db_Copy9(H_p_dx,H);
520 db_MultiplyScaleOntoImageHomography(H,1.0+dx[0]);
521 db_MultiplyRotationOntoImageHomography(H,dx[1]);
522 H_p_dx[2]+=dx[2];
523 H_p_dx[5]+=dx[3];
524 break;
525 case DB_HOMOGRAPHY_TYPE_ROTATION:
526 case DB_HOMOGRAPHY_TYPE_ROTATION_U:
527 db_MultiplyRotationOntoImageHomography(H,dx[0]);
528 break;
529 case DB_HOMOGRAPHY_TYPE_SCALING:
530 db_MultiplyScaleOntoImageHomography(H,1.0+dx[0]);
531 break;
532 case DB_HOMOGRAPHY_TYPE_S_T:
533 db_Copy9(H_p_dx,H);
534 db_MultiplyScaleOntoImageHomography(H,1.0+dx[0]);
535 H_p_dx[2]+=dx[1];
536 H_p_dx[5]+=dx[2];
537 break;
538 case DB_HOMOGRAPHY_TYPE_R_T:
539 db_Copy9(H_p_dx,H);
540 db_MultiplyRotationOntoImageHomography(H,dx[0]);
541 H_p_dx[2]+=dx[1];
542 H_p_dx[5]+=dx[2];
543 break;
544 case DB_HOMOGRAPHY_TYPE_R_S:
545 db_Copy9(H_p_dx,H);
546 db_MultiplyScaleOntoImageHomography(H,1.0+dx[0]);
547 db_MultiplyRotationOntoImageHomography(H,dx[1]);
548 break;
549 case DB_HOMOGRAPHY_TYPE_TRANSLATION:
550 db_Copy9(H_p_dx,H);
551 H_p_dx[2]+=dx[0];
552 H_p_dx[5]+=dx[1];
553 break;
554 case DB_HOMOGRAPHY_TYPE_AFFINE:
555 db_UpdateImageHomographyAffine(H_p_dx,H,dx);
556 break;
557 case DB_HOMOGRAPHY_TYPE_PROJECTIVE:
558 db_UpdateImageHomographyProjective(H_p_dx,H,dx,frozen_coord);
559 break;
560 case DB_HOMOGRAPHY_TYPE_CAMROTATION_F:
561 case DB_HOMOGRAPHY_TYPE_CAMROTATION_F_UD:
562 db_UpdateRotFocalHomography(H_p_dx,H,dx);
563 break;
564 }
565 }
566
db_RobCamRotation_Polish_Generic(double H[9],int point_count,int homography_type,double * x_i,double * xp_i,double one_over_scale2,int max_iterations,double improvement_requirement)567 void db_RobCamRotation_Polish_Generic(double H[9],int point_count,int homography_type,double *x_i,double *xp_i,double one_over_scale2,
568 int max_iterations,double improvement_requirement)
569 {
570 int i,update,stop,n;
571 int frozen_coord = 0;
572 double lambda,cost,current_cost;
573 double JtJ[72],min_Jtf[9],dx[8],H_p_dx[9];
574 double *JtJ_ref[9],d[8];
575
576 lambda=0.001;
577 for(update=1,stop=0,i=0;(stop<2) && (i<max_iterations);i++)
578 {
579 /*if first time since improvement, compute Jacobian and residual*/
580 if(update)
581 {
582 db_SetupMatrixRefs(JtJ_ref,9,8,JtJ);
583 current_cost=db_RobImageHomography_Jacobians_Generic(JtJ_ref,min_Jtf,&n,&frozen_coord,H,point_count,x_i,xp_i,homography_type,one_over_scale2);
584 update=0;
585 }
586
587 #ifdef _VERBOSE_
588 /*std::cout << "Cost:" << current_cost << " ";*/
589 #endif /*_VERBOSE_*/
590
591 /*Come up with a hypothesis dx
592 based on the current lambda*/
593 db_Compute_dx(dx,JtJ_ref,min_Jtf,lambda,d,n);
594
595 /*Compute Cost(x+dx)*/
596 db_ImageHomographyUpdateGeneric(H_p_dx,H,dx,homography_type,frozen_coord);
597 cost=db_RobImageHomography_Cost(H_p_dx,point_count,x_i,xp_i,one_over_scale2);
598
599 /*Is there an improvement?*/
600 if(cost<current_cost)
601 {
602 /*improvement*/
603 if(current_cost-cost<current_cost*improvement_requirement) stop++;
604 else stop=0;
605 lambda*=0.1;
606 /*Move to the hypothesised position x+dx*/
607 current_cost=cost;
608 db_Copy9(H,H_p_dx);
609 update=1;
610
611 #ifdef _VERBOSE_
612 std::cout << "Step" << i << "Imp,Lambda=" << lambda << "Cost:" << current_cost << std::endl;
613 #endif /*_VERBOSE_*/
614 }
615 else
616 {
617 /*no improvement*/
618 lambda*=10.0;
619 stop=0;
620 }
621 }
622 }
db_RobImageHomography(double H[9],double * im,double * im_p,int nr_points,double K[9],double Kp[9],double * temp_d,int * temp_i,int homography_type,db_Statistics * stat,int max_iterations,int max_points,double scale,int nr_samples,int chunk_size,int outlierremoveflagE,double * wp,double * im_r,double * im_raw,double * im_raw_p,int * finalNumE)623 void db_RobImageHomography(
624 /*Best homography*/
625 double H[9],
626 /*2DPoint to 2DPoint constraints
627 Points are assumed to be given in
628 homogenous coordinates*/
629 double *im, double *im_p,
630 /*Nr of points in total*/
631 int nr_points,
632 /*Calibration matrices
633 used to normalize the points*/
634 double K[9],
635 double Kp[9],
636 /*Pre-allocated space temp_d
637 should point to at least
638 12*nr_samples+10*nr_points
639 allocated positions*/
640 double *temp_d,
641 /*Pre-allocated space temp_i
642 should point to at least
643 max(nr_samples,nr_points)
644 allocated positions*/
645 int *temp_i,
646 int homography_type,
647 db_Statistics *stat,
648 int max_iterations,
649 int max_points,
650 double scale,
651 int nr_samples,
652 int chunk_size,
653 /////////////////////////////////////////////
654 // regular use: set outlierremoveflagE =0;
655 // flag for the outlier removal
656 int outlierremoveflagE,
657 // if flag is 1, then the following variables
658 // need the input
659 //////////////////////////////////////
660 // 3D coordinates
661 double *wp,
662 // its corresponding stereo pair's points
663 double *im_r,
664 // raw image coordinates
665 double *im_raw, double *im_raw_p,
666 // final matches
667 int *finalNumE)
668 {
669 /*Random seed*/
670 int r_seed;
671
672 int point_count_new;
673 /*Counters*/
674 int i,j,c,point_count,hyp_count;
675 int last_hyp,new_last_hyp,last_corr;
676 int pos,point_pos,last_point;
677 /*Accumulator*/
678 double acc;
679 /*Hypothesis pointer*/
680 double *hyp_point;
681 /*Random sample*/
682 int s[4];
683 /*Pivot for hypothesis pruning*/
684 double pivot;
685 /*Best hypothesis position*/
686 int best_pos;
687 /*Best score*/
688 double lowest_cost;
689 /*One over the squared scale of
690 Cauchy distribution*/
691 double one_over_scale2;
692 /*temporary pointers*/
693 double *x_i_temp,*xp_i_temp;
694 /*Temporary space for inverse calibration matrices*/
695 double K_inv[9];
696 double Kp_inv[9];
697 /*Temporary space for homography*/
698 double H_temp[9],H_temp2[9];
699 /*Pointers to homogenous coordinates*/
700 double *x_h_point,*xp_h_point;
701 /*Array of pointers to inhomogenous coordinates*/
702 double *X[3],*Xp[3];
703 /*Similarity parameters*/
704 int orientation_preserving,allow_scaling,allow_rotation,allow_translation,sample_size;
705
706 /*Homogenous coordinates of image points in first image*/
707 double *x_h;
708 /*Homogenous coordinates of image points in second image*/
709 double *xp_h;
710 /*Inhomogenous coordinates of image points in first image*/
711 double *x_i;
712 /*Inhomogenous coordinates of image points in second image*/
713 double *xp_i;
714 /*Homography hypotheses*/
715 double *hyp_H_array;
716 /*Cost array*/
717 double *hyp_cost_array;
718 /*Permutation of the hypotheses*/
719 int *hyp_perm;
720 /*Sample of the points*/
721 int *point_perm;
722 /*Temporary space for quick-select
723 2*nr_samples*/
724 double *temp_select;
725
726 /*Get inverse calibration matrices*/
727 db_InvertCalibrationMatrix(K_inv,K);
728 db_InvertCalibrationMatrix(Kp_inv,Kp);
729 /*Compute scale coefficient*/
730 one_over_scale2=1.0/(scale*scale);
731 /*Initialize random seed*/
732 r_seed=12345;
733 /*Set pointers to pre-allocated space*/
734 hyp_cost_array=temp_d;
735 hyp_H_array=temp_d+nr_samples;
736 temp_select=temp_d+10*nr_samples;
737 x_h=temp_d+12*nr_samples;
738 xp_h=temp_d+12*nr_samples+3*nr_points;
739 x_i=temp_d+12*nr_samples+6*nr_points;
740 xp_i=temp_d+12*nr_samples+8*nr_points;
741 hyp_perm=temp_i;
742 point_perm=temp_i;
743
744 /*Prepare a randomly permuted subset of size
745 point_count from the input points*/
746
747 point_count=db_mini(nr_points,(int)(chunk_size*log((double)nr_samples)/DB_LN2));
748
749 point_count_new = point_count;
750
751 for(i=0;i<nr_points;i++) point_perm[i]=i;
752
753 for(last_point=nr_points-1,i=0;i<point_count;i++,last_point--)
754 {
755 pos=db_RandomInt(r_seed,last_point);
756 point_pos=point_perm[pos];
757 point_perm[pos]=point_perm[last_point];
758
759 /*Normalize image points with calibration
760 matrices and move them to x_h and xp_h*/
761 c=3*point_pos;
762 j=3*i;
763 x_h_point=x_h+j;
764 xp_h_point=xp_h+j;
765 db_Multiply3x3_3x1(x_h_point,K_inv,im+c);
766 db_Multiply3x3_3x1(xp_h_point,Kp_inv,im_p+c);
767
768 db_HomogenousNormalize3(x_h_point);
769 db_HomogenousNormalize3(xp_h_point);
770
771 /*Dehomogenize image points and move them
772 to x_i and xp_i*/
773 c=(i<<1);
774 db_DeHomogenizeImagePoint(x_i+c,x_h_point); // 2-dimension
775 db_DeHomogenizeImagePoint(xp_i+c,xp_h_point); //2-dimension
776 }
777
778
779 /*Generate Hypotheses*/
780 hyp_count=0;
781 switch(homography_type)
782 {
783 case DB_HOMOGRAPHY_TYPE_SIMILARITY:
784 case DB_HOMOGRAPHY_TYPE_SIMILARITY_U:
785 case DB_HOMOGRAPHY_TYPE_TRANSLATION:
786 case DB_HOMOGRAPHY_TYPE_ROTATION:
787 case DB_HOMOGRAPHY_TYPE_ROTATION_U:
788 case DB_HOMOGRAPHY_TYPE_SCALING:
789 case DB_HOMOGRAPHY_TYPE_S_T:
790 case DB_HOMOGRAPHY_TYPE_R_T:
791 case DB_HOMOGRAPHY_TYPE_R_S:
792
793 switch(homography_type)
794 {
795 case DB_HOMOGRAPHY_TYPE_SIMILARITY:
796 orientation_preserving=1;
797 allow_scaling=1;
798 allow_rotation=1;
799 allow_translation=1;
800 sample_size=2;
801 break;
802 case DB_HOMOGRAPHY_TYPE_SIMILARITY_U:
803 orientation_preserving=0;
804 allow_scaling=1;
805 allow_rotation=1;
806 allow_translation=1;
807 sample_size=3;
808 break;
809 case DB_HOMOGRAPHY_TYPE_TRANSLATION:
810 orientation_preserving=1;
811 allow_scaling=0;
812 allow_rotation=0;
813 allow_translation=1;
814 sample_size=1;
815 break;
816 case DB_HOMOGRAPHY_TYPE_ROTATION:
817 orientation_preserving=1;
818 allow_scaling=0;
819 allow_rotation=1;
820 allow_translation=0;
821 sample_size=1;
822 break;
823 case DB_HOMOGRAPHY_TYPE_ROTATION_U:
824 orientation_preserving=0;
825 allow_scaling=0;
826 allow_rotation=1;
827 allow_translation=0;
828 sample_size=2;
829 break;
830 case DB_HOMOGRAPHY_TYPE_SCALING:
831 orientation_preserving=1;
832 allow_scaling=1;
833 allow_rotation=0;
834 allow_translation=0;
835 sample_size=1;
836 break;
837 case DB_HOMOGRAPHY_TYPE_S_T:
838 orientation_preserving=1;
839 allow_scaling=1;
840 allow_rotation=0;
841 allow_translation=1;
842 sample_size=2;
843 break;
844 case DB_HOMOGRAPHY_TYPE_R_T:
845 orientation_preserving=1;
846 allow_scaling=0;
847 allow_rotation=1;
848 allow_translation=1;
849 sample_size=2;
850 break;
851 case DB_HOMOGRAPHY_TYPE_R_S:
852 orientation_preserving=1;
853 allow_scaling=1;
854 allow_rotation=0;
855 allow_translation=0;
856 sample_size=1;
857 break;
858 }
859
860 if(point_count>=sample_size) for(i=0;i<nr_samples;i++)
861 {
862 db_RandomSample(s,3,point_count,r_seed);
863 X[0]= &x_i[s[0]<<1];
864 X[1]= &x_i[s[1]<<1];
865 X[2]= &x_i[s[2]<<1];
866 Xp[0]= &xp_i[s[0]<<1];
867 Xp[1]= &xp_i[s[1]<<1];
868 Xp[2]= &xp_i[s[2]<<1];
869 db_StitchSimilarity2D(&hyp_H_array[9*hyp_count],Xp,X,sample_size,orientation_preserving,
870 allow_scaling,allow_rotation,allow_translation);
871 hyp_count++;
872 }
873 break;
874
875 case DB_HOMOGRAPHY_TYPE_CAMROTATION:
876 if(point_count>=2) for(i=0;i<nr_samples;i++)
877 {
878 db_RandomSample(s,2,point_count,r_seed);
879 db_StitchCameraRotation_2Points(&hyp_H_array[9*hyp_count],
880 &x_h[3*s[0]],&x_h[3*s[1]],
881 &xp_h[3*s[0]],&xp_h[3*s[1]]);
882 hyp_count++;
883 }
884 break;
885
886 case DB_HOMOGRAPHY_TYPE_CAMROTATION_F:
887 if(point_count>=3) for(i=0;i<nr_samples;i++)
888 {
889 db_RandomSample(s,3,point_count,r_seed);
890 hyp_count+=db_StitchRotationCommonFocalLength_3Points(&hyp_H_array[9*hyp_count],
891 &x_h[3*s[0]],&x_h[3*s[1]],&x_h[3*s[2]],
892 &xp_h[3*s[0]],&xp_h[3*s[1]],&xp_h[3*s[2]]);
893 }
894 break;
895
896 case DB_HOMOGRAPHY_TYPE_CAMROTATION_F_UD:
897 if(point_count>=3) for(i=0;i<nr_samples;i++)
898 {
899 db_RandomSample(s,3,point_count,r_seed);
900 hyp_count+=db_StitchRotationCommonFocalLength_3Points(&hyp_H_array[9*hyp_count],
901 &x_h[3*s[0]],&x_h[3*s[1]],&x_h[3*s[2]],
902 &xp_h[3*s[0]],&xp_h[3*s[1]],&xp_h[3*s[2]],NULL,0);
903 }
904 break;
905
906 case DB_HOMOGRAPHY_TYPE_AFFINE:
907 if(point_count>=3) for(i=0;i<nr_samples;i++)
908 {
909 db_RandomSample(s,3,point_count,r_seed);
910 db_StitchAffine2D_3Points(&hyp_H_array[9*hyp_count],
911 &x_h[3*s[0]],&x_h[3*s[1]],&x_h[3*s[2]],
912 &xp_h[3*s[0]],&xp_h[3*s[1]],&xp_h[3*s[2]]);
913 hyp_count++;
914 }
915 break;
916
917 case DB_HOMOGRAPHY_TYPE_PROJECTIVE:
918 default:
919 if(point_count>=4) for(i=0;i<nr_samples;i++)
920 {
921 db_RandomSample(s,4,point_count,r_seed);
922 db_StitchProjective2D_4Points(&hyp_H_array[9*hyp_count],
923 &x_h[3*s[0]],&x_h[3*s[1]],&x_h[3*s[2]],&x_h[3*s[3]],
924 &xp_h[3*s[0]],&xp_h[3*s[1]],&xp_h[3*s[2]],&xp_h[3*s[3]]);
925 hyp_count++;
926 }
927 }
928
929 if(hyp_count)
930 {
931 /*Count cost in chunks and decimate hypotheses
932 until only one remains or the correspondences are
933 exhausted*/
934 for(i=0;i<hyp_count;i++)
935 {
936 hyp_perm[i]=i;
937 hyp_cost_array[i]=0.0;
938 }
939 for(i=0,last_hyp=hyp_count-1;(last_hyp>0) && (i<point_count);i+=chunk_size)
940 {
941 /*Update cost with the next chunk*/
942 last_corr=db_mini(i+chunk_size-1,point_count-1);
943 for(j=0;j<=last_hyp;j++)
944 {
945 hyp_point=hyp_H_array+9*hyp_perm[j];
946 for(c=i;c<=last_corr;)
947 {
948 /*Take log of product of ten reprojection
949 errors to reduce nr of expensive log operations*/
950 if(c+9<=last_corr)
951 {
952 x_i_temp=x_i+(c<<1);
953 xp_i_temp=xp_i+(c<<1);
954
955 acc=db_ExpCauchyInhomogenousHomographyError(xp_i_temp,hyp_point,x_i_temp,one_over_scale2);
956 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+2,hyp_point,x_i_temp+2,one_over_scale2);
957 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+4,hyp_point,x_i_temp+4,one_over_scale2);
958 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+6,hyp_point,x_i_temp+6,one_over_scale2);
959 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+8,hyp_point,x_i_temp+8,one_over_scale2);
960 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+10,hyp_point,x_i_temp+10,one_over_scale2);
961 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+12,hyp_point,x_i_temp+12,one_over_scale2);
962 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+14,hyp_point,x_i_temp+14,one_over_scale2);
963 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+16,hyp_point,x_i_temp+16,one_over_scale2);
964 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+18,hyp_point,x_i_temp+18,one_over_scale2);
965 c+=10;
966 }
967 else
968 {
969 for(acc=1.0;c<=last_corr;c++)
970 {
971 acc*=db_ExpCauchyInhomogenousHomographyError(xp_i+(c<<1),hyp_point,x_i+(c<<1),one_over_scale2);
972 }
973 }
974 hyp_cost_array[j]+=log(acc);
975 }
976 }
977 if (chunk_size<point_count){
978 /*Prune out half of the hypotheses*/
979 new_last_hyp=(last_hyp+1)/2-1;
980 pivot=db_LeanQuickSelect(hyp_cost_array,last_hyp+1,new_last_hyp,temp_select);
981 for(j=0,c=0;(j<=last_hyp) && (c<=new_last_hyp);j++)
982 {
983 if(hyp_cost_array[j]<=pivot)
984 {
985 hyp_cost_array[c]=hyp_cost_array[j];
986 hyp_perm[c]=hyp_perm[j];
987 c++;
988 }
989 }
990 last_hyp=new_last_hyp;
991 }
992 }
993 /*Find the best hypothesis*/
994 lowest_cost=hyp_cost_array[0];
995 best_pos=0;
996 for(j=1;j<=last_hyp;j++)
997 {
998 if(hyp_cost_array[j]<lowest_cost)
999 {
1000 lowest_cost=hyp_cost_array[j];
1001 best_pos=j;
1002 }
1003 }
1004
1005 /*Move the best hypothesis*/
1006 db_Copy9(H_temp,hyp_H_array+9*hyp_perm[best_pos]);
1007
1008 // outlier removal
1009 if (outlierremoveflagE) // no polishment needed
1010 {
1011 point_count_new = db_RemoveOutliers_Homography(H_temp,x_i,xp_i,wp,im,im_p,im_r,im_raw,im_raw_p,point_count,one_over_scale2);
1012 }
1013 else
1014 {
1015 /*Polish*/
1016 switch(homography_type)
1017 {
1018 case DB_HOMOGRAPHY_TYPE_SIMILARITY:
1019 case DB_HOMOGRAPHY_TYPE_SIMILARITY_U:
1020 case DB_HOMOGRAPHY_TYPE_TRANSLATION:
1021 case DB_HOMOGRAPHY_TYPE_ROTATION:
1022 case DB_HOMOGRAPHY_TYPE_ROTATION_U:
1023 case DB_HOMOGRAPHY_TYPE_SCALING:
1024 case DB_HOMOGRAPHY_TYPE_S_T:
1025 case DB_HOMOGRAPHY_TYPE_R_T:
1026 case DB_HOMOGRAPHY_TYPE_R_S:
1027 case DB_HOMOGRAPHY_TYPE_AFFINE:
1028 case DB_HOMOGRAPHY_TYPE_PROJECTIVE:
1029 case DB_HOMOGRAPHY_TYPE_CAMROTATION_F:
1030 case DB_HOMOGRAPHY_TYPE_CAMROTATION_F_UD:
1031 db_RobCamRotation_Polish_Generic(H_temp,db_mini(point_count,max_points),homography_type,x_i,xp_i,one_over_scale2,max_iterations);
1032 break;
1033 case DB_HOMOGRAPHY_TYPE_CAMROTATION:
1034 db_RobCamRotation_Polish(H_temp,db_mini(point_count,max_points),x_i,xp_i,one_over_scale2,max_iterations);
1035 break;
1036 }
1037
1038 }
1039
1040 }
1041 else db_Identity3x3(H_temp);
1042
1043 switch(homography_type)
1044 {
1045 case DB_HOMOGRAPHY_TYPE_PROJECTIVE:
1046 if(stat) stat->nr_parameters=8;
1047 break;
1048 case DB_HOMOGRAPHY_TYPE_AFFINE:
1049 if(stat) stat->nr_parameters=6;
1050 break;
1051 case DB_HOMOGRAPHY_TYPE_SIMILARITY:
1052 case DB_HOMOGRAPHY_TYPE_SIMILARITY_U:
1053 case DB_HOMOGRAPHY_TYPE_CAMROTATION_F:
1054 case DB_HOMOGRAPHY_TYPE_CAMROTATION_F_UD:
1055 if(stat) stat->nr_parameters=4;
1056 break;
1057 case DB_HOMOGRAPHY_TYPE_CAMROTATION:
1058 if(stat) stat->nr_parameters=3;
1059 break;
1060 case DB_HOMOGRAPHY_TYPE_TRANSLATION:
1061 case DB_HOMOGRAPHY_TYPE_S_T:
1062 case DB_HOMOGRAPHY_TYPE_R_T:
1063 case DB_HOMOGRAPHY_TYPE_R_S:
1064 if(stat) stat->nr_parameters=2;
1065 break;
1066 case DB_HOMOGRAPHY_TYPE_ROTATION:
1067 case DB_HOMOGRAPHY_TYPE_ROTATION_U:
1068 case DB_HOMOGRAPHY_TYPE_SCALING:
1069 if(stat) stat->nr_parameters=1;
1070 break;
1071 }
1072
1073 db_RobImageHomography_Statistics(H_temp,db_mini(point_count,max_points),x_i,xp_i,one_over_scale2,stat);
1074
1075 /*Put on the calibration matrices*/
1076 db_Multiply3x3_3x3(H_temp2,H_temp,K_inv);
1077 db_Multiply3x3_3x3(H,Kp,H_temp2);
1078
1079 if (finalNumE)
1080 *finalNumE = point_count_new;
1081
1082 }
1083