| /* |
| * 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 */ |