diff options
Diffstat (limited to 'jni/feature_stab/db_vlvm/db_utilities_camera.h')
| -rw-r--r-- | jni/feature_stab/db_vlvm/db_utilities_camera.h | 332 |
1 files changed, 332 insertions, 0 deletions
diff --git a/jni/feature_stab/db_vlvm/db_utilities_camera.h b/jni/feature_stab/db_vlvm/db_utilities_camera.h new file mode 100644 index 000000000..26ba4420a --- /dev/null +++ b/jni/feature_stab/db_vlvm/db_utilities_camera.h @@ -0,0 +1,332 @@ +/* + * 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 */ |