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_utilities_camera.h,v 1.3 2011/06/17 14:03:31 mbansal Exp $ */ 18e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 19e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen#ifndef DB_UTILITIES_CAMERA 20e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen#define DB_UTILITIES_CAMERA 21e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 22e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen#include "db_utilities.h" 23e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 24e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 25e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 26e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/***************************************************************** 27e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen* Lean and mean begins here * 28e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen*****************************************************************/ 29e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 30e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * \defgroup LMCamera (LM) Camera Utilities 31e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 32e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*\{*/ 33e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 34e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen#include "db_utilities.h" 35e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 36e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen#define DB_RADDISTMODE_BOUGEUT 4 37e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen#define DB_RADDISTMODE_2NDORDER 5 38e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen#define DB_RADDISTMODE_IDENTITY 6 39e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 40e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 41e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenGive reasonable guess of the calibration matrix for normalization purposes. 42e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenUse real K matrix when doing real geometry. 43e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chenfocal length = (w+h)/2.0*f_correction. 44e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen\param K calibration matrix (out) 45e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen\param Kinv inverse of K (out) 46e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen\param im_width image width 47e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen\param im_height image height 48e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen\param f_correction focal length correction factor 49e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen\param field set to 1 if this is a field image (fy = fx/2) 50e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen\return K(3x3) intrinsic calibration matrix 51e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen*/ 52e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenDB_API void db_Approx3DCalMat(double K[9],double Kinv[9],int im_width,int im_height,double f_correction=1.0,int field=0); 53e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 54e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 55e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Make a 2x2 identity matrix 56e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 57e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chenvoid inline db_Identity2x2(double A[4]) 58e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 59e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen A[0]=1;A[1]=0; 60e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen A[2]=0;A[3]=1; 61e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 62e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 63e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Make a 3x3 identity matrix 64e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 65e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chenvoid inline db_Identity3x3(double A[9]) 66e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 67e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen A[0]=1;A[1]=0;A[2]=0; 68e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen A[3]=0;A[4]=1;A[5]=0; 69e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen A[6]=0;A[7]=0;A[8]=1; 70e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 71e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 72e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Invert intrinsic calibration matrix K(3x3) 73e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen If fx or fy is 0, I is returned. 74e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 75e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chenvoid inline db_InvertCalibrationMatrix(double Kinv[9],const double K[9]) 76e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 77e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double a,b,c,d,e,f,ainv,dinv,adinv; 78e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 79e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a=K[0];b=K[1];c=K[2];d=K[4];e=K[5];f=K[8]; 80e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen if((a==0.0)||(d==0.0)) db_Identity3x3(Kinv); 81e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen else 82e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 83e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Kinv[3]=0.0; 84e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Kinv[6]=0.0; 85e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Kinv[7]=0.0; 86e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Kinv[8]=1.0; 87e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 88e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen ainv=1.0/a; 89e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen dinv=1.0/d; 90e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen adinv=ainv*dinv; 91e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Kinv[0]=f*ainv; 92e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Kinv[1]= -b*f*adinv; 93e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Kinv[2]=(b*e-c*d)*adinv; 94e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Kinv[4]=f*dinv; 95e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Kinv[5]= -e*dinv; 96e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 97e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 98e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 99e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen De-homogenize image point: xd(1:2) = xs(1:2)/xs(3). 100e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen If xs(3) is 0, xd will become 0 101e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen \param xd destination point 102e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen \param xs source point 103e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 104e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chenvoid inline db_DeHomogenizeImagePoint(double xd[2],const double xs[3]) 105e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 106e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double temp,div; 107e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 108e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen temp=xs[2]; 109e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen if(temp!=0) 110e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 111e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen div=1.0/temp; 112e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen xd[0]=xs[0]*div;xd[1]=xs[1]*div; 113e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 114e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen else 115e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 116e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen xd[0]=0.0;xd[1]=0.0; 117e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 118e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 119e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 120e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 121e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 122e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Orthonormalize 3D rotation R 123e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 124e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_OrthonormalizeRotation(double R[9]) 125e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 126e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double s,mult; 127e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*Normalize first vector*/ 128e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen s=db_sqr(R[0])+db_sqr(R[1])+db_sqr(R[2]); 129e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen mult=sqrt(1.0/(s?s:1)); 130e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen R[0]*=mult; R[1]*=mult; R[2]*=mult; 131e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*Subtract scalar product from second vector*/ 132e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen s=R[0]*R[3]+R[1]*R[4]+R[2]*R[5]; 133e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen R[3]-=s*R[0]; R[4]-=s*R[1]; R[5]-=s*R[2]; 134e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*Normalize second vector*/ 135e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen s=db_sqr(R[3])+db_sqr(R[4])+db_sqr(R[5]); 136e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen mult=sqrt(1.0/(s?s:1)); 137e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen R[3]*=mult; R[4]*=mult; R[5]*=mult; 138e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*Get third vector by vector product*/ 139e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen R[6]=R[1]*R[5]-R[4]*R[2]; 140e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen R[7]=R[2]*R[3]-R[5]*R[0]; 141e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen R[8]=R[0]*R[4]-R[3]*R[1]; 142e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 143e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 144e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenUpdate a rotation with the update dx=[sin(phi) sin(ohm) sin(kap)] 145e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen*/ 146e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_UpdateRotation(double R_p_dx[9],double R[9],const double dx[3]) 147e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 148e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double R_temp[9]; 149e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*Update rotation*/ 150e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_IncrementalRotationMatrix(R_temp,dx); 151e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_Multiply3x3_3x3(R_p_dx,R_temp,R); 152e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 153e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 154e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Compute xp = Hx for inhomogenous image points. 155e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 156e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_ImageHomographyInhomogenous(double xp[2],const double H[9],const double x[2]) 157e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 158e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double x3,m; 159e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 160e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen x3=H[6]*x[0]+H[7]*x[1]+H[8]; 161e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen if(x3!=0.0) 162e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 163e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen m=1.0/x3; 164e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen xp[0]=m*(H[0]*x[0]+H[1]*x[1]+H[2]); 165e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen xp[1]=m*(H[3]*x[0]+H[4]*x[1]+H[5]); 166e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 167e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen else 168e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 169e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen xp[0]=xp[1]=0.0; 170e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 171e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 172e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline double db_FocalFromCamRotFocalHomography(const double H[9]) 173e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 174e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double k1,k2; 175e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 176e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen k1=db_sqr(H[2])+db_sqr(H[5]); 177e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen k2=db_sqr(H[6])+db_sqr(H[7]); 178e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen if(k1>=k2) 179e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 180e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen return(db_SafeSqrt(db_SafeDivision(k1,1.0-db_sqr(H[8])))); 181e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 182e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen else 183e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 184e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen return(db_SafeSqrt(db_SafeDivision(1.0-db_sqr(H[8]),k2))); 185e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 186e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 187e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 188e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline double db_FocalAndRotFromCamRotFocalHomography(double R[9],const double H[9]) 189e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 190e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double back,fi; 191e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 192e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen back=db_FocalFromCamRotFocalHomography(H); 193e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen fi=db_SafeReciprocal(back); 194e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen R[0]=H[0]; R[1]=H[1]; R[2]=fi*H[2]; 195e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen R[3]=H[3]; R[4]=H[4]; R[5]=fi*H[5]; 196e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen R[6]=back*H[6]; R[7]=back*H[7]; R[8]=H[8]; 197e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen return(back); 198e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 199e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 200e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenCompute Jacobian at zero of three coordinates dR*x with 201e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chenrespect to the update dR([sin(phi) sin(ohm) sin(kap)]) given x. 202e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 203e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenThe Jacobian at zero of the homogenous coordinates with respect to 204e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen [sin(phi) sin(ohm) sin(kap)] is 205e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen\code 206e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen [-rx2 0 rx1 ] 207e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen [ 0 rx2 -rx0 ] 208e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen [ rx0 -rx1 0 ]. 209e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen\endcode 210e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 211e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen*/ 212e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_JacobianOfRotatedPointStride(double J[9],const double x[3],int stride) 213e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 214e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*The Jacobian at zero of the homogenous coordinates with respect to 215e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen [sin(phi) sin(ohm) sin(kap)] is 216e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen [-rx2 0 rx1 ] 217e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen [ 0 rx2 -rx0 ] 218e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen [ rx0 -rx1 0 ]*/ 219e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 220e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen J[0]= -x[stride<<1]; 221e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen J[1]=0; 222e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen J[2]= x[stride]; 223e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen J[3]=0; 224e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen J[4]= x[stride<<1]; 225e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen J[5]= -x[0]; 226e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen J[6]= x[0]; 227e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen J[7]= -x[stride]; 228e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen J[8]=0; 229e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 230e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 231e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Invert an affine (if possible) 232e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen \param Hinv inverted matrix 233e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen \param H input matrix 234e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen \return true if success and false if matrix is ill-conditioned (det < 1e-7) 235e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 236e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline bool db_InvertAffineTransform(double Hinv[9],const double H[9]) 237e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 238e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double det=H[0]*H[4]-H[3]*H[1]; 239e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen if (det<1e-7) 240e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 241e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_Copy9(Hinv,H); 242e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen return false; 243e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 244e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen else 245e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 246e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Hinv[0]=H[4]/det; 247e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Hinv[1]=-H[1]/det; 248e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Hinv[3]=-H[3]/det; 249e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Hinv[4]=H[0]/det; 250e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Hinv[2]= -Hinv[0]*H[2]-Hinv[1]*H[5]; 251e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Hinv[5]= -Hinv[3]*H[2]-Hinv[4]*H[5]; 252e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 253e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen return true; 254e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 255e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 256e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 257e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenUpdate of upper 2x2 is multiplication by 258e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen\code 259e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen[s 0][ cos(theta) sin(theta)] 260e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen[0 s][-sin(theta) cos(theta)] 261e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen\endcode 262e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen*/ 263e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_MultiplyScaleOntoImageHomography(double H[9],double s) 264e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 265e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 266e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen H[0]*=s; 267e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen H[1]*=s; 268e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen H[3]*=s; 269e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen H[4]*=s; 270e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 271e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 272e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenUpdate of upper 2x2 is multiplication by 273e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen\code 274e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen[s 0][ cos(theta) sin(theta)] 275e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen[0 s][-sin(theta) cos(theta)] 276e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen\endcode 277e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen*/ 278e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_MultiplyRotationOntoImageHomography(double H[9],double theta) 279e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 280e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double c,s,H0,H1; 281e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 282e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 283e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen c=cos(theta); 284e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen s=db_SafeSqrt(1.0-db_sqr(c)); 285e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen H0= c*H[0]+s*H[3]; 286e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen H[3]= -s*H[0]+c*H[3]; 287e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen H[0]=H0; 288e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen H1=c*H[1]+s*H[4]; 289e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen H[4]= -s*H[1]+c*H[4]; 290e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen H[1]=H1; 291e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 292e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 293e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_UpdateImageHomographyAffine(double H_p_dx[9],const double H[9],const double dx[6]) 294e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 295e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_AddVectors6(H_p_dx,H,dx); 296e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_Copy3(H_p_dx+6,H+6); 297e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 298e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 299e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_UpdateImageHomographyProjective(double H_p_dx[9],const double H[9],const double dx[8],int frozen_coord) 300e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 301e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen int i,j; 302e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 303e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen for(j=0,i=0;i<9;i++) 304e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 305e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen if(i!=frozen_coord) 306e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 307e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen H_p_dx[i]=H[i]+dx[j]; 308e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen j++; 309e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 310e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen else H_p_dx[i]=H[i]; 311e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 312e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 313e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 314e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_UpdateRotFocalHomography(double H_p_dx[9],const double H[9],const double dx[4]) 315e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 316e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double f,fp,fpi; 317e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double R[9],dR[9]; 318e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 319e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*Updated matrix is diag(f+df,f+df)*dR*R*diag(1/(f+df),1/(f+df),1)*/ 320e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen f=db_FocalAndRotFromCamRotFocalHomography(R,H); 321e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_IncrementalRotationMatrix(dR,dx); 322e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_Multiply3x3_3x3(H_p_dx,dR,R); 323e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen fp=f+dx[3]; 324e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen fpi=db_SafeReciprocal(fp); 325e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen H_p_dx[2]*=fp; 326e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen H_p_dx[5]*=fp; 327e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen H_p_dx[6]*=fpi; 328e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen H_p_dx[7]*=fpi; 329e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 330e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 331e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*\}*/ 332e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen#endif /* DB_UTILITIES_CAMERA */ 333