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