• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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 /*!
41 Give reasonable guess of the calibration matrix for normalization purposes.
42 Use real K matrix when doing real geometry.
43 focal 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 */
52 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);
53 
54 /*!
55  Make a 2x2 identity matrix
56  */
db_Identity2x2(double A[4])57 void 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  */
db_Identity3x3(double A[9])65 void 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  */
db_InvertCalibrationMatrix(double Kinv[9],const double K[9])75 void 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  */
db_DeHomogenizeImagePoint(double xd[2],const double xs[3])104 void 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  */
db_OrthonormalizeRotation(double R[9])124 inline 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 /*!
144 Update a rotation with the update dx=[sin(phi) sin(ohm) sin(kap)]
145 */
db_UpdateRotation(double R_p_dx[9],double R[9],const double dx[3])146 inline 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  */
db_ImageHomographyInhomogenous(double xp[2],const double H[9],const double x[2])156 inline 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 }
db_FocalFromCamRotFocalHomography(const double H[9])172 inline 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 
db_FocalAndRotFromCamRotFocalHomography(double R[9],const double H[9])188 inline 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 /*!
200 Compute Jacobian at zero of three coordinates dR*x with
201 respect to the update dR([sin(phi) sin(ohm) sin(kap)]) given x.
202 
203 The 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 */
db_JacobianOfRotatedPointStride(double J[9],const double x[3],int stride)212 inline 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  */
db_InvertAffineTransform(double Hinv[9],const double H[9])236 inline 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 /*!
257 Update 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 */
db_MultiplyScaleOntoImageHomography(double H[9],double s)263 inline 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 /*!
272 Update 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 */
db_MultiplyRotationOntoImageHomography(double H[9],double theta)278 inline 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 
db_UpdateImageHomographyAffine(double H_p_dx[9],const double H[9],const double dx[6])293 inline 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 
db_UpdateImageHomographyProjective(double H_p_dx[9],const double H[9],const double dx[8],int frozen_coord)299 inline 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 
db_UpdateRotFocalHomography(double H_p_dx[9],const double H[9],const double dx[4])314 inline 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