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