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