/* * Copyright (C) 2011 The Android Open Source Project * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ /* $Id: db_utilities_camera.h,v 1.3 2011/06/17 14:03:31 mbansal Exp $ */ #ifndef DB_UTILITIES_CAMERA #define DB_UTILITIES_CAMERA #include "db_utilities.h" /***************************************************************** * Lean and mean begins here * *****************************************************************/ /*! * \defgroup LMCamera (LM) Camera Utilities */ /*\{*/ #include "db_utilities.h" #define DB_RADDISTMODE_BOUGEUT 4 #define DB_RADDISTMODE_2NDORDER 5 #define DB_RADDISTMODE_IDENTITY 6 /*! Give reasonable guess of the calibration matrix for normalization purposes. Use real K matrix when doing real geometry. focal length = (w+h)/2.0*f_correction. \param K calibration matrix (out) \param Kinv inverse of K (out) \param im_width image width \param im_height image height \param f_correction focal length correction factor \param field set to 1 if this is a field image (fy = fx/2) \return K(3x3) intrinsic calibration matrix */ DB_API void db_Approx3DCalMat(double K[9],double Kinv[9],int im_width,int im_height,double f_correction=1.0,int field=0); /*! Make a 2x2 identity matrix */ void inline db_Identity2x2(double A[4]) { A[0]=1;A[1]=0; A[2]=0;A[3]=1; } /*! Make a 3x3 identity matrix */ void inline db_Identity3x3(double A[9]) { A[0]=1;A[1]=0;A[2]=0; A[3]=0;A[4]=1;A[5]=0; A[6]=0;A[7]=0;A[8]=1; } /*! Invert intrinsic calibration matrix K(3x3) If fx or fy is 0, I is returned. */ void inline db_InvertCalibrationMatrix(double Kinv[9],const double K[9]) { double a,b,c,d,e,f,ainv,dinv,adinv; a=K[0];b=K[1];c=K[2];d=K[4];e=K[5];f=K[8]; if((a==0.0)||(d==0.0)) db_Identity3x3(Kinv); else { Kinv[3]=0.0; Kinv[6]=0.0; Kinv[7]=0.0; Kinv[8]=1.0; ainv=1.0/a; dinv=1.0/d; adinv=ainv*dinv; Kinv[0]=f*ainv; Kinv[1]= -b*f*adinv; Kinv[2]=(b*e-c*d)*adinv; Kinv[4]=f*dinv; Kinv[5]= -e*dinv; } } /*! De-homogenize image point: xd(1:2) = xs(1:2)/xs(3). If xs(3) is 0, xd will become 0 \param xd destination point \param xs source point */ void inline db_DeHomogenizeImagePoint(double xd[2],const double xs[3]) { double temp,div; temp=xs[2]; if(temp!=0) { div=1.0/temp; xd[0]=xs[0]*div;xd[1]=xs[1]*div; } else { xd[0]=0.0;xd[1]=0.0; } } /*! Orthonormalize 3D rotation R */ inline void db_OrthonormalizeRotation(double R[9]) { double s,mult; /*Normalize first vector*/ s=db_sqr(R[0])+db_sqr(R[1])+db_sqr(R[2]); mult=sqrt(1.0/(s?s:1)); R[0]*=mult; R[1]*=mult; R[2]*=mult; /*Subtract scalar product from second vector*/ s=R[0]*R[3]+R[1]*R[4]+R[2]*R[5]; R[3]-=s*R[0]; R[4]-=s*R[1]; R[5]-=s*R[2]; /*Normalize second vector*/ s=db_sqr(R[3])+db_sqr(R[4])+db_sqr(R[5]); mult=sqrt(1.0/(s?s:1)); R[3]*=mult; R[4]*=mult; R[5]*=mult; /*Get third vector by vector product*/ R[6]=R[1]*R[5]-R[4]*R[2]; R[7]=R[2]*R[3]-R[5]*R[0]; R[8]=R[0]*R[4]-R[3]*R[1]; } /*! Update a rotation with the update dx=[sin(phi) sin(ohm) sin(kap)] */ inline void db_UpdateRotation(double R_p_dx[9],double R[9],const double dx[3]) { double R_temp[9]; /*Update rotation*/ db_IncrementalRotationMatrix(R_temp,dx); db_Multiply3x3_3x3(R_p_dx,R_temp,R); } /*! Compute xp = Hx for inhomogenous image points. */ inline void db_ImageHomographyInhomogenous(double xp[2],const double H[9],const double x[2]) { double x3,m; x3=H[6]*x[0]+H[7]*x[1]+H[8]; if(x3!=0.0) { m=1.0/x3; xp[0]=m*(H[0]*x[0]+H[1]*x[1]+H[2]); xp[1]=m*(H[3]*x[0]+H[4]*x[1]+H[5]); } else { xp[0]=xp[1]=0.0; } } inline double db_FocalFromCamRotFocalHomography(const double H[9]) { double k1,k2; k1=db_sqr(H[2])+db_sqr(H[5]); k2=db_sqr(H[6])+db_sqr(H[7]); if(k1>=k2) { return(db_SafeSqrt(db_SafeDivision(k1,1.0-db_sqr(H[8])))); } else { return(db_SafeSqrt(db_SafeDivision(1.0-db_sqr(H[8]),k2))); } } inline double db_FocalAndRotFromCamRotFocalHomography(double R[9],const double H[9]) { double back,fi; back=db_FocalFromCamRotFocalHomography(H); fi=db_SafeReciprocal(back); R[0]=H[0]; R[1]=H[1]; R[2]=fi*H[2]; R[3]=H[3]; R[4]=H[4]; R[5]=fi*H[5]; R[6]=back*H[6]; R[7]=back*H[7]; R[8]=H[8]; return(back); } /*! Compute Jacobian at zero of three coordinates dR*x with respect to the update dR([sin(phi) sin(ohm) sin(kap)]) given x. The Jacobian at zero of the homogenous coordinates with respect to [sin(phi) sin(ohm) sin(kap)] is \code [-rx2 0 rx1 ] [ 0 rx2 -rx0 ] [ rx0 -rx1 0 ]. \endcode */ inline void db_JacobianOfRotatedPointStride(double J[9],const double x[3],int stride) { /*The Jacobian at zero of the homogenous coordinates with respect to [sin(phi) sin(ohm) sin(kap)] is [-rx2 0 rx1 ] [ 0 rx2 -rx0 ] [ rx0 -rx1 0 ]*/ J[0]= -x[stride<<1]; J[1]=0; J[2]= x[stride]; J[3]=0; J[4]= x[stride<<1]; J[5]= -x[0]; J[6]= x[0]; J[7]= -x[stride]; J[8]=0; } /*! Invert an affine (if possible) \param Hinv inverted matrix \param H input matrix \return true if success and false if matrix is ill-conditioned (det < 1e-7) */ inline bool db_InvertAffineTransform(double Hinv[9],const double H[9]) { double det=H[0]*H[4]-H[3]*H[1]; if (det<1e-7) { db_Copy9(Hinv,H); return false; } else { Hinv[0]=H[4]/det; Hinv[1]=-H[1]/det; Hinv[3]=-H[3]/det; Hinv[4]=H[0]/det; Hinv[2]= -Hinv[0]*H[2]-Hinv[1]*H[5]; Hinv[5]= -Hinv[3]*H[2]-Hinv[4]*H[5]; } return true; } /*! Update of upper 2x2 is multiplication by \code [s 0][ cos(theta) sin(theta)] [0 s][-sin(theta) cos(theta)] \endcode */ inline void db_MultiplyScaleOntoImageHomography(double H[9],double s) { H[0]*=s; H[1]*=s; H[3]*=s; H[4]*=s; } /*! Update of upper 2x2 is multiplication by \code [s 0][ cos(theta) sin(theta)] [0 s][-sin(theta) cos(theta)] \endcode */ inline void db_MultiplyRotationOntoImageHomography(double H[9],double theta) { double c,s,H0,H1; c=cos(theta); s=db_SafeSqrt(1.0-db_sqr(c)); H0= c*H[0]+s*H[3]; H[3]= -s*H[0]+c*H[3]; H[0]=H0; H1=c*H[1]+s*H[4]; H[4]= -s*H[1]+c*H[4]; H[1]=H1; } inline void db_UpdateImageHomographyAffine(double H_p_dx[9],const double H[9],const double dx[6]) { db_AddVectors6(H_p_dx,H,dx); db_Copy3(H_p_dx+6,H+6); } inline void db_UpdateImageHomographyProjective(double H_p_dx[9],const double H[9],const double dx[8],int frozen_coord) { int i,j; for(j=0,i=0;i<9;i++) { if(i!=frozen_coord) { H_p_dx[i]=H[i]+dx[j]; j++; } else H_p_dx[i]=H[i]; } } inline void db_UpdateRotFocalHomography(double H_p_dx[9],const double H[9],const double dx[4]) { double f,fp,fpi; double R[9],dR[9]; /*Updated matrix is diag(f+df,f+df)*dR*R*diag(1/(f+df),1/(f+df),1)*/ f=db_FocalAndRotFromCamRotFocalHomography(R,H); db_IncrementalRotationMatrix(dR,dx); db_Multiply3x3_3x3(H_p_dx,dR,R); fp=f+dx[3]; fpi=db_SafeReciprocal(fp); H_p_dx[2]*=fp; H_p_dx[5]*=fp; H_p_dx[6]*=fpi; H_p_dx[7]*=fpi; } /*\}*/ #endif /* DB_UTILITIES_CAMERA */