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> 33using namespace std; 34#endif /*VERBOSE*/ 35 36inline 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 74inline 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.*/ 112inline 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*/ 193inline 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 226void 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 281inline 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 299inline 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 311inline 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 327inline 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 343inline 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 366inline 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 417inline 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 513inline 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 567void 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} 623void 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