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 */ 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 */ 78 inline double db_sqr(double a) 79 { 80 return(a*a); 81 } 82 83 /*! 84 * Square a long. 85 */ 86 inline long db_sqr(long a) 87 { 88 return(a*a); 89 } 90 91 /*! 92 * Square an int. 93 */ 94 inline long db_sqr(int a) 95 { 96 return(a*a); 97 } 98 99 /*! 100 * Maximum of two doubles. 101 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 164 inline int db_absi(int a) 165 { 166 if(a<0) return(-a); 167 else return(a); 168 } 169 /*! 170 * Absolute value. 171 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 } 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 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 */ 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 */ 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 */ 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 */ 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 */ 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
