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 */
61inline 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 */
78inline double db_sqr(double a)
79{
80    return(a*a);
81}
82
83/*!
84 * Square a long.
85 */
86inline long db_sqr(long a)
87{
88    return(a*a);
89}
90
91/*!
92 * Square an int.
93 */
94inline long db_sqr(int a)
95{
96    return(a*a);
97}
98
99/*!
100 * Maximum of two doubles.
101 */
102inline 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 */
110inline 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 */
120inline 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 */
129inline 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 */
137inline 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 */
146inline 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 */
156inline 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 */
164inline int db_absi(int a)
165{
166    if(a<0) return(-a);
167    else return(a);
168}
169/*!
170 * Absolute value.
171 */
172inline float db_absf(float a)
173{
174    if(a<0) return(-a);
175    else return(a);
176}
177
178/*!
179 * Absolute value.
180 */
181inline 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 */
191inline 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 */
200inline 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 */
209inline 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 */
218inline double db_SafeSqrtReciprocal(double a)
219{
220    return((a>0.0)?(sqrt(1.0/a)):1.0);
221}
222/*!
223 * Cube root.
224 */
225inline 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 */
233inline 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 */
240inline 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 */
249inline 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 */
260void 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 */
269void 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 */
279void 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 */
289inline 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 */
296inline 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 */
305inline 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 */
314inline 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 */
322inline 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}
328inline 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 */
345inline 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 */
355inline 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 */
363inline 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 */
371inline 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 */
378inline 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 */
386inline 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,
397copying, simple pyramid construction and LUT warping. These images are used
398by db_CornerDetector_u and db_Matcher_u. The db_Image class is an attempt
399to 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 */
409DB_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 */
418DB_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 */
424DB_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 */
431DB_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 */
440DB_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 */
446DB_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 */
457DB_API void db_CopyImage_u(unsigned char **d,const unsigned char * const *s,int w,int h,int over_allocation=0);
458
459DB_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 */
482inline 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 */
505void inline db_Zero2(double x[2])
506{
507    x[0]=x[1]=0;
508}
509/*!
510 * Zero out 3 vector in place.
511 */
512void 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 */
519void 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 */
526void 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 */
552DB_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
555DB_API void db_PrintDoubleVector(double *a,long size);
556DB_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