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.h,v 1.3 2011/06/17 14:03:31 mbansal Exp $ */
18 
19 #ifndef DB_UTILITIES_H
20 #define DB_UTILITIES_H
21 
22 
23 #ifdef _WIN32
24 #pragma warning(disable: 4275)
25 #pragma warning(disable: 4251)
26 #pragma warning(disable: 4786)
27 #pragma warning(disable: 4800)
28 #pragma warning(disable: 4018) /* signed-unsigned mismatch */
29 #endif /* _WIN32 */
30 
31 #ifdef _WIN32
32     #ifdef DBDYNAMIC_EXPORTS
33         #define DB_API __declspec(dllexport)
34     #else
35         #ifdef DBDYNAMIC_IMPORTS
36             #define DB_API __declspec(dllimport)
37         #else
38             #define DB_API
39         #endif
40     #endif
41 #else
42     #define DB_API
43 #endif /* _WIN32 */
44 
45 #ifdef _VERBOSE_
46 #include <iostream>
47 #endif
48 
49 #include <math.h>
50 
51 #include <assert.h>
52 #include "db_utilities_constants.h"
53 /*!
54  * \defgroup LMBasicUtilities (LM) Utility Functions (basic math, linear algebra and array manipulations)
55  */
56 /*\{*/
57 
58 /*!
59  * Round double into int using fld and fistp instructions.
60  */
db_roundi(double x)61 inline int db_roundi (double x) {
62 #ifdef WIN32_ASM
63     int n;
64     __asm
65     {
66         fld x;
67         fistp n;
68     }
69     return n;
70 #else
71     return static_cast<int>(floor(x+0.5));
72 #endif
73 }
74 
75 /*!
76  * Square a double.
77  */
db_sqr(double a)78 inline double db_sqr(double a)
79 {
80     return(a*a);
81 }
82 
83 /*!
84  * Square a long.
85  */
db_sqr(long a)86 inline long db_sqr(long a)
87 {
88     return(a*a);
89 }
90 
91 /*!
92  * Square an int.
93  */
db_sqr(int a)94 inline long db_sqr(int a)
95 {
96     return(a*a);
97 }
98 
99 /*!
100  * Maximum of two doubles.
101  */
db_maxd(double a,double b)102 inline double db_maxd(double a,double b)
103 {
104     if(b>a) return(b);
105     else return(a);
106 }
107 /*!
108  * Minumum of two doubles.
109  */
db_mind(double a,double b)110 inline double db_mind(double a,double b)
111 {
112     if(b<a) return(b);
113     else return(a);
114 }
115 
116 
117 /*!
118  * Maximum of two ints.
119  */
db_maxi(int a,int b)120 inline int db_maxi(int a,int b)
121 {
122     if(b>a) return(b);
123     else return(a);
124 }
125 
126 /*!
127  * Minimum of two numbers.
128  */
db_mini(int a,int b)129 inline int db_mini(int a,int b)
130 {
131     if(b<a) return(b);
132     else return(a);
133 }
134 /*!
135  * Maximum of two numbers.
136  */
db_maxl(long a,long b)137 inline long db_maxl(long a,long b)
138 {
139     if(b>a) return(b);
140     else return(a);
141 }
142 
143 /*!
144  * Minimum of two numbers.
145  */
db_minl(long a,long b)146 inline long db_minl(long a,long b)
147 {
148     if(b<a) return(b);
149     else return(a);
150 }
151 
152 /*!
153  * Sign of a number.
154  * \return -1.0 if negative, 1.0 if positive.
155  */
db_sign(double x)156 inline double db_sign(double x)
157 {
158     if(x>=0.0) return(1.0);
159     else return(-1.0);
160 }
161 /*!
162  * Absolute value.
163  */
db_absi(int a)164 inline int db_absi(int a)
165 {
166     if(a<0) return(-a);
167     else return(a);
168 }
169 /*!
170  * Absolute value.
171  */
db_absf(float a)172 inline float db_absf(float a)
173 {
174     if(a<0) return(-a);
175     else return(a);
176 }
177 
178 /*!
179  * Absolute value.
180  */
db_absd(double a)181 inline double db_absd(double a)
182 {
183     if(a<0) return(-a);
184     else return(a);
185 }
186 
187 /*!
188  * Reciprocal (1/a). Prevents divide by 0.
189  * \return 1/a if a != 0. 1.0 otherwise.
190  */
db_SafeReciprocal(double a)191 inline double db_SafeReciprocal(double a)
192 {
193     return((a!=0.0)?(1.0/a):1.0);
194 }
195 
196 /*!
197  * Division. Prevents divide by 0.
198  * \return a/b if b!=0. a otherwise.
199  */
db_SafeDivision(double a,double b)200 inline double db_SafeDivision(double a,double b)
201 {
202     return((b!=0.0)?(a/b):a);
203 }
204 
205 /*!
206  * Square root. Prevents imaginary output.
207  * \return sqrt(a) if a > 0.0. 0.0 otherewise.
208  */
db_SafeSqrt(double a)209 inline double db_SafeSqrt(double a)
210 {
211     return((a>=0.0)?(sqrt(a)):0.0);
212 }
213 
214 /*!
215  * Square root of a reciprocal. Prevents divide by 0 and imaginary output.
216  * \return sqrt(1/a) if a > 0.0. 1.0 otherewise.
217  */
db_SafeSqrtReciprocal(double a)218 inline double db_SafeSqrtReciprocal(double a)
219 {
220     return((a>0.0)?(sqrt(1.0/a)):1.0);
221 }
222 /*!
223  * Cube root.
224  */
db_CubRoot(double x)225 inline double db_CubRoot(double x)
226 {
227     if(x>=0.0) return(pow(x,1.0/3.0));
228     else return(-pow(-x,1.0/3.0));
229 }
230 /*!
231  * Sum of squares of elements of x.
232  */
db_SquareSum3(const double x[3])233 inline double db_SquareSum3(const double x[3])
234 {
235     return(db_sqr(x[0])+db_sqr(x[1])+db_sqr(x[2]));
236 }
237 /*!
238  * Sum of squares of elements of x.
239  */
db_SquareSum7(double x[7])240 inline double db_SquareSum7(double x[7])
241 {
242     return(db_sqr(x[0])+db_sqr(x[1])+db_sqr(x[2])+
243         db_sqr(x[3])+db_sqr(x[4])+db_sqr(x[5])+
244         db_sqr(x[6]));
245 }
246 /*!
247  * Sum of squares of elements of x.
248  */
db_SquareSum9(double x[9])249 inline double db_SquareSum9(double x[9])
250 {
251     return(db_sqr(x[0])+db_sqr(x[1])+db_sqr(x[2])+
252         db_sqr(x[3])+db_sqr(x[4])+db_sqr(x[5])+
253         db_sqr(x[6])+db_sqr(x[7])+db_sqr(x[8]));
254 }
255 /*!
256  * Copy a vector.
257  * \param xd destination
258  * \param xs source
259  */
db_Copy3(double xd[3],const double xs[3])260 void inline db_Copy3(double xd[3],const double xs[3])
261 {
262     xd[0]=xs[0];xd[1]=xs[1];xd[2]=xs[2];
263 }
264 /*!
265  * Copy a vector.
266  * \param xd destination
267  * \param xs source
268  */
db_Copy6(double xd[6],const double xs[6])269 void inline db_Copy6(double xd[6],const double xs[6])
270 {
271     xd[0]=xs[0];xd[1]=xs[1];xd[2]=xs[2];
272     xd[3]=xs[3];xd[4]=xs[4];xd[5]=xs[5];
273 }
274 /*!
275  * Copy a vector.
276  * \param xd destination
277  * \param xs source
278  */
db_Copy9(double xd[9],const double xs[9])279 void inline db_Copy9(double xd[9],const double xs[9])
280 {
281     xd[0]=xs[0];xd[1]=xs[1];xd[2]=xs[2];
282     xd[3]=xs[3];xd[4]=xs[4];xd[5]=xs[5];
283     xd[6]=xs[6];xd[7]=xs[7];xd[8]=xs[8];
284 }
285 
286 /*!
287  * Scalar product: Transpose(A)*B.
288  */
db_ScalarProduct4(const double A[4],const double B[4])289 inline double db_ScalarProduct4(const double A[4],const double B[4])
290 {
291     return(A[0]*B[0]+A[1]*B[1]+A[2]*B[2]+A[3]*B[3]);
292 }
293 /*!
294  * Scalar product: Transpose(A)*B.
295  */
db_ScalarProduct7(const double A[7],const double B[7])296 inline double db_ScalarProduct7(const double A[7],const double B[7])
297 {
298     return(A[0]*B[0]+A[1]*B[1]+A[2]*B[2]+
299         A[3]*B[3]+A[4]*B[4]+A[5]*B[5]+
300         A[6]*B[6]);
301 }
302 /*!
303  * Scalar product: Transpose(A)*B.
304  */
db_ScalarProduct9(const double A[9],const double B[9])305 inline double db_ScalarProduct9(const double A[9],const double B[9])
306 {
307     return(A[0]*B[0]+A[1]*B[1]+A[2]*B[2]+
308         A[3]*B[3]+A[4]*B[4]+A[5]*B[5]+
309         A[6]*B[6]+A[7]*B[7]+A[8]*B[8]);
310 }
311 /*!
312  * Vector addition: S=A+B.
313  */
db_AddVectors6(double S[6],const double A[6],const double B[6])314 inline void db_AddVectors6(double S[6],const double A[6],const double B[6])
315 {
316     S[0]=A[0]+B[0]; S[1]=A[1]+B[1]; S[2]=A[2]+B[2]; S[3]=A[3]+B[3]; S[4]=A[4]+B[4];
317     S[5]=A[5]+B[5];
318 }
319 /*!
320  * Multiplication: C(3x1)=A(3x3)*B(3x1).
321  */
db_Multiply3x3_3x1(double y[3],const double A[9],const double x[3])322 inline void db_Multiply3x3_3x1(double y[3],const double A[9],const double x[3])
323 {
324     y[0]=A[0]*x[0]+A[1]*x[1]+A[2]*x[2];
325     y[1]=A[3]*x[0]+A[4]*x[1]+A[5]*x[2];
326     y[2]=A[6]*x[0]+A[7]*x[1]+A[8]*x[2];
327 }
db_Multiply3x3_3x3(double C[9],const double A[9],const double B[9])328 inline void db_Multiply3x3_3x3(double C[9], const double A[9],const double B[9])
329 {
330     C[0]=A[0]*B[0]+A[1]*B[3]+A[2]*B[6];
331     C[1]=A[0]*B[1]+A[1]*B[4]+A[2]*B[7];
332     C[2]=A[0]*B[2]+A[1]*B[5]+A[2]*B[8];
333 
334     C[3]=A[3]*B[0]+A[4]*B[3]+A[5]*B[6];
335     C[4]=A[3]*B[1]+A[4]*B[4]+A[5]*B[7];
336     C[5]=A[3]*B[2]+A[4]*B[5]+A[5]*B[8];
337 
338     C[6]=A[6]*B[0]+A[7]*B[3]+A[8]*B[6];
339     C[7]=A[6]*B[1]+A[7]*B[4]+A[8]*B[7];
340     C[8]=A[6]*B[2]+A[7]*B[5]+A[8]*B[8];
341 }
342 /*!
343  * Multiplication: C(4x1)=A(4x4)*B(4x1).
344  */
db_Multiply4x4_4x1(double y[4],const double A[16],const double x[4])345 inline void db_Multiply4x4_4x1(double y[4],const double A[16],const double x[4])
346 {
347     y[0]=A[0]*x[0]+A[1]*x[1]+A[2]*x[2]+A[3]*x[3];
348     y[1]=A[4]*x[0]+A[5]*x[1]+A[6]*x[2]+A[7]*x[3];
349     y[2]=A[8]*x[0]+A[9]*x[1]+A[10]*x[2]+A[11]*x[3];
350     y[3]=A[12]*x[0]+A[13]*x[1]+A[14]*x[2]+A[15]*x[3];
351 }
352 /*!
353  * Scalar multiplication in place: A(3)=mult*A(3).
354  */
db_MultiplyScalar3(double * A,double mult)355 inline void db_MultiplyScalar3(double *A,double mult)
356 {
357     (*A++) *= mult; (*A++) *= mult; (*A++) *= mult;
358 }
359 
360 /*!
361  * Scalar multiplication: A(3)=mult*B(3).
362  */
db_MultiplyScalarCopy3(double * A,const double * B,double mult)363 inline void db_MultiplyScalarCopy3(double *A,const double *B,double mult)
364 {
365     (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult;
366 }
367 
368 /*!
369  * Scalar multiplication: A(4)=mult*B(4).
370  */
db_MultiplyScalarCopy4(double * A,const double * B,double mult)371 inline void db_MultiplyScalarCopy4(double *A,const double *B,double mult)
372 {
373     (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult;
374 }
375 /*!
376  * Scalar multiplication: A(7)=mult*B(7).
377  */
db_MultiplyScalarCopy7(double * A,const double * B,double mult)378 inline void db_MultiplyScalarCopy7(double *A,const double *B,double mult)
379 {
380     (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult;
381     (*A++)=(*B++)*mult; (*A++)=(*B++)*mult;
382 }
383 /*!
384  * Scalar multiplication: A(9)=mult*B(9).
385  */
db_MultiplyScalarCopy9(double * A,const double * B,double mult)386 inline void db_MultiplyScalarCopy9(double *A,const double *B,double mult)
387 {
388     (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult;
389     (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult;
390 }
391 
392 /*!
393  * \defgroup LMImageBasicUtilities (LM) Basic Image Utility Functions
394 
395  Images in db are simply 2D arrays of unsigned char or float types.
396  Only the very basic operations are supported: allocation/deallocation,
397 copying, simple pyramid construction and LUT warping. These images are used
398 by db_CornerDetector_u and db_Matcher_u. The db_Image class is an attempt
399 to wrap these images. It has not been tested well.
400 
401  */
402 /*\{*/
403 /*!
404  * Given a float image array, allocates and returns the set of row poiners.
405  * \param im    image pointer
406  * \param w     image width
407  * \param h     image height
408  */
409 DB_API float** db_SetupImageReferences_f(float *im,int w,int h);
410 /*!
411  * Allocate a float image.
412  * Note: for feature detection images must be overallocated by 256 bytes.
413  * \param w                 width
414  * \param h                 height
415  * \param over_allocation   allocate this many extra bytes at the end
416  * \return row array pointer
417  */
418 DB_API float** db_AllocImage_f(int w,int h,int over_allocation=256);
419 /*!
420  * Free a float image
421  * \param img   row array pointer
422  * \param h     image height (number of rows)
423  */
424 DB_API void db_FreeImage_f(float **img,int h);
425 /*!
426  * Given an unsigned char image array, allocates and returns the set of row poiners.
427  * \param im    image pointer
428  * \param w     image width
429  * \param h     image height
430  */
431 DB_API unsigned char** db_SetupImageReferences_u(unsigned char *im,int w,int h);
432 /*!
433  * Allocate an unsigned char image.
434  * Note: for feature detection images must be overallocated by 256 bytes.
435  * \param w                 width
436  * \param h                 height
437  * \param over_allocation   allocate this many extra bytes at the end
438  * \return row array pointer
439  */
440 DB_API unsigned char** db_AllocImage_u(int w,int h,int over_allocation=256);
441 /*!
442  * Free an unsigned char image
443  * \param img   row array pointer
444  * \param h     image height (number of rows)
445  */
446 DB_API void db_FreeImage_u(unsigned char **img,int h);
447 
448 /*!
449  Copy an image from s to d. Both s and d must be pre-allocated at of the same size.
450  Copy is done row by row.
451  \param s   source
452  \param d   destination
453  \param w   width
454  \param h   height
455  \param over_allocation copy this many bytes after the end of the last line
456  */
457 DB_API void db_CopyImage_u(unsigned char **d,const unsigned char * const *s,int w,int h,int over_allocation=0);
458 
db_BilinearInterpolation(double y,double x,const unsigned char * const * v)459 DB_API inline unsigned char db_BilinearInterpolation(double y, double x, const unsigned char * const * v)
460 {
461     int floor_x=(int) x;
462     int floor_y=(int) y;
463 
464     int ceil_x=floor_x+1;
465     int ceil_y=floor_y+1;
466 
467     unsigned char f00 = v[floor_y][floor_x];
468     unsigned char f01 = v[floor_y][ceil_x];
469     unsigned char f10 = v[ceil_y][floor_x];
470     unsigned char f11 = v[ceil_y][ceil_x];
471 
472     double xl = x-floor_x;
473     double yl = y-floor_y;
474 
475     return (unsigned char)(f00*(1-yl)*(1-xl) + f10*yl*(1-xl) + f01*(1-yl)*xl + f11*yl*xl);
476 }
477 /*\}*/
478 /*!
479  * \ingroup LMRotation
480  * Compute an incremental rotation matrix using the update dx=[sin(phi) sin(ohm) sin(kap)]
481  */
db_IncrementalRotationMatrix(double R[9],const double dx[3])482 inline void db_IncrementalRotationMatrix(double R[9],const double dx[3])
483 {
484     double sp,so,sk,om_sp2,om_so2,om_sk2,cp,co,ck,sp_so,cp_so;
485 
486     /*Store sines*/
487     sp=dx[0]; so=dx[1]; sk=dx[2];
488     om_sp2=1.0-sp*sp;
489     om_so2=1.0-so*so;
490     om_sk2=1.0-sk*sk;
491     /*Compute cosines*/
492     cp=(om_sp2>=0.0)?sqrt(om_sp2):1.0;
493     co=(om_so2>=0.0)?sqrt(om_so2):1.0;
494     ck=(om_sk2>=0.0)?sqrt(om_sk2):1.0;
495     /*Compute matrix*/
496     sp_so=sp*so;
497     cp_so=cp*so;
498     R[0]=sp_so*sk+cp*ck; R[1]=co*sk; R[2]=cp_so*sk-sp*ck;
499     R[3]=sp_so*ck-cp*sk; R[4]=co*ck; R[5]=cp_so*ck+sp*sk;
500     R[6]=sp*co;          R[7]= -so;  R[8]=cp*co;
501 }
502 /*!
503  * Zero out 2 vector in place.
504  */
db_Zero2(double x[2])505 void inline db_Zero2(double x[2])
506 {
507     x[0]=x[1]=0;
508 }
509 /*!
510  * Zero out 3 vector in place.
511  */
db_Zero3(double x[3])512 void inline db_Zero3(double x[3])
513 {
514     x[0]=x[1]=x[2]=0;
515 }
516 /*!
517  * Zero out 4 vector in place.
518  */
db_Zero4(double x[4])519 void inline db_Zero4(double x[4])
520 {
521     x[0]=x[1]=x[2]=x[3]=0;
522 }
523 /*!
524  * Zero out 9 vector in place.
525  */
db_Zero9(double x[9])526 void inline db_Zero9(double x[9])
527 {
528     x[0]=x[1]=x[2]=x[3]=x[4]=x[5]=x[6]=x[7]=x[8]=0;
529 }
530 
531 #define DB_WARP_FAST        0
532 #define DB_WARP_BILINEAR    1
533 
534 /*!
535  * Perform a look-up table warp.
536  * The LUTs must be float images of the same size as source image.
537  * The source value x_s is determined from destination (x_d,y_d) through lut_x
538  * and y_s is determined from lut_y:
539    \code
540    x_s = lut_x[y_d][x_d];
541    y_s = lut_y[y_d][x_d];
542    \endcode
543 
544  * \param src   source image
545  * \param dst   destination image
546  * \param w     width
547  * \param h     height
548  * \param lut_x LUT for x
549  * \param lut_y LUT for y
550  * \param type  warp type (DB_WARP_FAST or DB_WARP_BILINEAR)
551  */
552 DB_API void db_WarpImageLut_u(const unsigned char * const * src,unsigned char ** dst, int w, int h,
553                                const float * const * lut_x, const float * const * lut_y, int type=DB_WARP_BILINEAR);
554 
555 DB_API void db_PrintDoubleVector(double *a,long size);
556 DB_API void db_PrintDoubleMatrix(double *a,long rows,long cols);
557 
558 #include "db_utilities_constants.h"
559 #include "db_utilities_algebra.h"
560 #include "db_utilities_indexing.h"
561 #include "db_utilities_linalg.h"
562 #include "db_utilities_poly.h"
563 #include "db_utilities_geometry.h"
564 #include "db_utilities_random.h"
565 #include "db_utilities_rotation.h"
566 #include "db_utilities_camera.h"
567 
568 #define DB_INVALID (-1)
569 
570 
571 #endif /* DB_UTILITIES_H */
572