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