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