Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
vpImageTools.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Image tools.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #ifndef vpImageTools_H
40 #define vpImageTools_H
41 
49 #include <visp3/core/vpImage.h>
50 
51 #ifdef VISP_HAVE_PTHREAD
52 #include <pthread.h>
53 #endif
54 
55 #include <visp3/core/vpCameraParameters.h>
56 #include <visp3/core/vpImageException.h>
57 #include <visp3/core/vpMath.h>
58 #include <visp3/core/vpRect.h>
59 #include <visp3/core/vpRectOriented.h>
60 
61 #include <fstream>
62 #include <iostream>
63 #include <math.h>
64 #include <string.h>
65 
66 #if defined _OPENMP
67 #include <omp.h>
68 #endif
69 
79 class VISP_EXPORT vpImageTools
80 {
81 public:
85  INTERPOLATION_CUBIC
86  };
87 
88  template <class Type>
89  static inline void binarise(vpImage<Type> &I, Type threshold1, Type threshold2, Type value1, Type value2, Type value3,
90  bool useLUT = true);
91  static void changeLUT(vpImage<unsigned char> &I, unsigned char A, unsigned char newA, unsigned char B,
92  unsigned char newB);
93 
94  template <class Type>
95  static void crop(const vpImage<Type> &I, double roi_top, double roi_left, unsigned int roi_height,
96  unsigned int roi_width, vpImage<Type> &crop, unsigned int v_scale = 1, unsigned int h_scale = 1);
97 
98  static void columnMean(const vpImage<double> &I, vpRowVector &result);
99 
100  template <class Type>
101  static void crop(const vpImage<Type> &I, const vpImagePoint &topLeft, unsigned int roi_height, unsigned int roi_width,
102  vpImage<Type> &crop, unsigned int v_scale = 1, unsigned int h_scale = 1);
103  template <class Type>
104  static void crop(const vpImage<Type> &I, const vpRect &roi, vpImage<Type> &crop, unsigned int v_scale = 1,
105  unsigned int h_scale = 1);
106  template <class Type>
107  static void crop(const unsigned char *bitmap, unsigned int width, unsigned int height, const vpRect &roi,
108  vpImage<Type> &crop, unsigned int v_scale = 1, unsigned int h_scale = 1);
109 
110  static void extract(const vpImage<unsigned char> &Src, vpImage<unsigned char> &Dst, const vpRectOriented &r);
111  static void extract(const vpImage<unsigned char> &Src, vpImage<double> &Dst, const vpRectOriented &r);
112 
113  template <class Type> static void flip(const vpImage<Type> &I, vpImage<Type> &newI);
114 
115  template <class Type> static void flip(vpImage<Type> &I);
116 
117  static void imageDifference(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
118  vpImage<unsigned char> &Idiff);
119  static void imageDifference(const vpImage<vpRGBa> &I1, const vpImage<vpRGBa> &I2, vpImage<vpRGBa> &Idiff);
120 
121  static void imageDifferenceAbsolute(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
122  vpImage<unsigned char> &Idiff);
123  static void imageDifferenceAbsolute(const vpImage<double> &I1, const vpImage<double> &I2, vpImage<double> &Idiff);
124  static void imageDifferenceAbsolute(const vpImage<vpRGBa> &I1, const vpImage<vpRGBa> &I2, vpImage<vpRGBa> &Idiff);
125 
126  static void imageAdd(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2, vpImage<unsigned char> &Ires,
127  bool saturate = false);
128 
129  static void imageSubtract(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
130  vpImage<unsigned char> &Ires, bool saturate = false);
131 
132  static void initUndistortMap(const vpCameraParameters &cam, unsigned int width, unsigned int height,
133  vpArray2D<int> &mapU, vpArray2D<int> &mapV,
134  vpArray2D<float> &mapDu, vpArray2D<float> &mapDv);
135 
136  static double interpolate(const vpImage<unsigned char> &I, const vpImagePoint &point,
137  const vpImageInterpolationType &method = INTERPOLATION_NEAREST);
138 
139  static void integralImage(const vpImage<unsigned char> &I, vpImage<double> &II, vpImage<double> &IIsq);
140 
141  static double normalizedCorrelation(const vpImage<double> &I1, const vpImage<double> &I2,
142  bool useOptimized = true);
143 
144  static void normalize(vpImage<double> &I);
145 
146  static void remap(const vpImage<unsigned char> &I, const vpArray2D<int> &mapU, const vpArray2D<int> &mapV,
147  const vpArray2D<float> &mapDu, const vpArray2D<float> &mapDv, vpImage<unsigned char> &Iundist);
148  static void remap(const vpImage<vpRGBa> &I, const vpArray2D<int> &mapU, const vpArray2D<int> &mapV,
149  const vpArray2D<float> &mapDu, const vpArray2D<float> &mapDv, vpImage<vpRGBa> &Iundist);
150 
151  template <class Type>
152  static void resize(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int width, unsigned int height,
153  const vpImageInterpolationType &method = INTERPOLATION_NEAREST, unsigned int nThreads=0);
154 
155  template <class Type>
156  static void resize(const vpImage<Type> &I, vpImage<Type> &Ires,
157  const vpImageInterpolationType &method = INTERPOLATION_NEAREST, unsigned int nThreads=0);
158 
159  static void templateMatching(const vpImage<unsigned char> &I, const vpImage<unsigned char> &I_tpl,
160  vpImage<double> &I_score, unsigned int step_u, unsigned int step_v,
161  bool useOptimized = true);
162 
163  template <class Type>
164  static void undistort(const vpImage<Type> &I, const vpCameraParameters &cam, vpImage<Type> &newI,
165  unsigned int nThreads=2);
166 
167  template <class Type>
168  static void warpImage(const vpImage<Type> &src, const vpMatrix &T, vpImage<Type> &dst,
169  const vpImageInterpolationType &interpolation=INTERPOLATION_NEAREST,
170  bool fixedPointArithmetic=true, bool pixelCenter=false);
171 
172 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
173 
177  template <class Type>
178  vp_deprecated static void createSubImage(const vpImage<Type> &I, unsigned int i_sub, unsigned int j_sub,
179  unsigned int nrow_sub, unsigned int ncol_sub, vpImage<Type> &S);
180 
181  template <class Type>
182  vp_deprecated static void createSubImage(const vpImage<Type> &I, const vpRect &rect, vpImage<Type> &S);
184 #endif
185 
186 private:
187  // Cubic interpolation
188  static float cubicHermite(const float A, const float B, const float C, const float D, const float t);
189 
190  template <class Type> static Type getPixelClamped(const vpImage<Type> &I, float u, float v);
191 
192  static int coordCast(double x);
193 
194  // Linear interpolation
195  static double lerp(double A, double B, double t);
196  static float lerp(float A, float B, float t);
197  static int64_t lerp2(int64_t A, int64_t B, int64_t t, int64_t t_1);
198 
199  static double normalizedCorrelation(const vpImage<double> &I1, const vpImage<double> &I2, const vpImage<double> &II,
200  const vpImage<double> &IIsq, const vpImage<double> &II_tpl,
201  const vpImage<double> &IIsq_tpl, unsigned int i0, unsigned int j0);
202 
203  template <class Type>
204  static void resizeBicubic(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int i, unsigned int j,
205  float u, float v, float xFrac, float yFrac);
206 
207  template <class Type>
208  static void resizeBilinear(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int i, unsigned int j,
209  float u, float v, float xFrac, float yFrac);
210 
211  template <class Type>
212  static void resizeNearest(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int i, unsigned int j,
213  float u, float v);
214 
215  template <class Type>
216  static void warpNN(const vpImage<Type> &src, const vpMatrix &T, vpImage<Type> &dst, bool affine, bool centerCorner, bool fixedPoint);
217 
218  template <class Type>
219  static void warpLinear(const vpImage<Type> &src, const vpMatrix &T, vpImage<Type> &dst, bool affine, bool centerCorner, bool fixedPoint);
220 
221  static bool checkFixedPoint(unsigned int x, unsigned int y, const vpMatrix &T, bool affine);
222 };
223 
224 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
225 
244 template <class Type>
245 void vpImageTools::createSubImage(const vpImage<Type> &I, unsigned int roi_top, unsigned int roi_left,
246  unsigned int roi_height, unsigned int roi_width, vpImage<Type> &crop)
247 {
248  vpImageTools::crop(I, roi_top, roi_left, roi_height, roi_width, crop);
249 }
250 
266 template <class Type> void vpImageTools::createSubImage(const vpImage<Type> &I, const vpRect &roi, vpImage<Type> &crop)
267 {
268  vpImageTools::crop(I, roi, crop);
269 }
270 
271 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
272 
295 template <class Type>
296 void vpImageTools::crop(const vpImage<Type> &I, double roi_top, double roi_left, unsigned int roi_height,
297  unsigned int roi_width, vpImage<Type> &crop, unsigned int v_scale, unsigned int h_scale)
298 {
299  int i_min = (std::max)((int)(ceil(roi_top / v_scale)), 0);
300  int j_min = (std::max)((int)(ceil(roi_left / h_scale)), 0);
301  int i_max = (std::min)((int)(ceil((roi_top + roi_height)) / v_scale), (int)(I.getHeight() / v_scale));
302  int j_max = (std::min)((int)(ceil((roi_left + roi_width) / h_scale)), (int)(I.getWidth() / h_scale));
303 
304  unsigned int i_min_u = (unsigned int)i_min;
305  unsigned int j_min_u = (unsigned int)j_min;
306 
307  unsigned int r_width = (unsigned int)(j_max - j_min);
308  unsigned int r_height = (unsigned int)(i_max - i_min);
309 
310  crop.resize(r_height, r_width);
311 
312  if (v_scale == 1 && h_scale == 1) {
313  for (unsigned int i = 0; i < r_height; i++) {
314  void *src = (void *)(I[i + i_min_u] + j_min_u);
315  void *dst = (void *)crop[i];
316  memcpy(dst, src, r_width * sizeof(Type));
317  }
318  } else if (h_scale == 1) {
319  for (unsigned int i = 0; i < r_height; i++) {
320  void *src = (void *)(I[(i + i_min_u) * v_scale] + j_min_u);
321  void *dst = (void *)crop[i];
322  memcpy(dst, src, r_width * sizeof(Type));
323  }
324  } else {
325  for (unsigned int i = 0; i < r_height; i++) {
326  for (unsigned int j = 0; j < r_width; j++) {
327  crop[i][j] = I[(i + i_min_u) * v_scale][(j + j_min_u) * h_scale];
328  }
329  }
330  }
331 }
332 
351 template <class Type>
352 void vpImageTools::crop(const vpImage<Type> &I, const vpImagePoint &topLeft, unsigned int roi_height,
353  unsigned int roi_width, vpImage<Type> &crop, unsigned int v_scale, unsigned int h_scale)
354 {
355  vpImageTools::crop(I, topLeft.get_i(), topLeft.get_j(), roi_height, roi_width, crop, v_scale, h_scale);
356 }
357 
375 template <class Type>
376 void vpImageTools::crop(const vpImage<Type> &I, const vpRect &roi, vpImage<Type> &crop, unsigned int v_scale,
377  unsigned int h_scale)
378 {
379  vpImageTools::crop(I, roi.getTop(), roi.getLeft(), (unsigned int)roi.getHeight(), (unsigned int)roi.getWidth(), crop,
380  v_scale, h_scale);
381 }
382 
400 template <class Type>
401 void vpImageTools::crop(const unsigned char *bitmap, unsigned int width, unsigned int height, const vpRect &roi,
402  vpImage<Type> &crop, unsigned int v_scale, unsigned int h_scale)
403 {
404  int i_min = (std::max)((int)(ceil(roi.getTop() / v_scale)), 0);
405  int j_min = (std::max)((int)(ceil(roi.getLeft() / h_scale)), 0);
406  int i_max = (std::min)((int)(ceil((roi.getTop() + roi.getHeight()) / v_scale)), (int)(height / v_scale));
407  int j_max = (std::min)((int)(ceil((roi.getLeft() + roi.getWidth()) / h_scale)), (int)(width / h_scale));
408 
409  unsigned int i_min_u = (unsigned int)i_min;
410  unsigned int j_min_u = (unsigned int)j_min;
411 
412  unsigned int r_width = (unsigned int)(j_max - j_min);
413  unsigned int r_height = (unsigned int)(i_max - i_min);
414 
415  crop.resize(r_height, r_width);
416 
417  if (v_scale == 1 && h_scale == 1) {
418  for (unsigned int i = 0; i < r_height; i++) {
419  void *src = (void *)(bitmap + ((i + i_min_u) * width + j_min_u) * sizeof(Type));
420  void *dst = (void *)crop[i];
421  memcpy(dst, src, r_width * sizeof(Type));
422  }
423  } else if (h_scale == 1) {
424  for (unsigned int i = 0; i < r_height; i++) {
425  void *src = (void *)(bitmap + ((i + i_min_u) * width * v_scale + j_min_u) * sizeof(Type));
426  void *dst = (void *)crop[i];
427  memcpy(dst, src, r_width * sizeof(Type));
428  }
429  } else {
430  for (unsigned int i = 0; i < r_height; i++) {
431  unsigned int i_src = (i + i_min_u) * width * v_scale + j_min_u * h_scale;
432  for (unsigned int j = 0; j < r_width; j++) {
433  void *src = (void *)(bitmap + (i_src + j * h_scale) * sizeof(Type));
434  void *dst = (void *)&crop[i][j];
435  memcpy(dst, src, sizeof(Type));
436  }
437  }
438  }
439 }
440 
451 template <class Type>
452 inline void vpImageTools::binarise(vpImage<Type> &I, Type threshold1, Type threshold2, Type value1, Type value2,
453  Type value3, bool useLUT)
454 {
455  if (useLUT) {
456  std::cerr << "LUT not available for this type ! Will use the iteration method." << std::endl;
457  }
458 
459  Type v;
460  Type *p = I.bitmap;
461  Type *pend = I.bitmap + I.getWidth() * I.getHeight();
462  for (; p < pend; p++) {
463  v = *p;
464  if (v < threshold1)
465  *p = value1;
466  else if (v > threshold2)
467  *p = value3;
468  else
469  *p = value2;
470  }
471 }
472 
483 template <>
484 inline void vpImageTools::binarise(vpImage<unsigned char> &I, unsigned char threshold1, unsigned char threshold2,
485  unsigned char value1, unsigned char value2, unsigned char value3, bool useLUT)
486 {
487  if (useLUT) {
488  // Construct the LUT
489  unsigned char lut[256];
490  for (unsigned int i = 0; i < 256; i++) {
491  lut[i] = i < threshold1 ? value1 : (i > threshold2 ? value3 : value2);
492  }
493 
494  I.performLut(lut);
495  } else {
496  unsigned char *p = I.bitmap;
497  unsigned char *pend = I.bitmap + I.getWidth() * I.getHeight();
498  for (; p < pend; p++) {
499  unsigned char v = *p;
500  if (v < threshold1)
501  *p = value1;
502  else if (v > threshold2)
503  *p = value3;
504  else
505  *p = value2;
506  }
507  }
508 }
509 
510 #ifdef VISP_HAVE_PTHREAD
511 
512 #ifndef DOXYGEN_SHOULD_SKIP_THIS
513 template <class Type> class vpUndistortInternalType
514 {
515 public:
516  Type *src;
517  Type *dst;
518  unsigned int width;
519  unsigned int height;
520  vpCameraParameters cam;
521  unsigned int nthreads;
522  unsigned int threadid;
523 
524 public:
525  vpUndistortInternalType() : src(NULL), dst(NULL), width(0), height(0), cam(), nthreads(0), threadid(0) {}
526 
527  vpUndistortInternalType(const vpUndistortInternalType<Type> &u) { *this = u; }
528  vpUndistortInternalType &operator=(const vpUndistortInternalType<Type> &u)
529  {
530  src = u.src;
531  dst = u.dst;
532  width = u.width;
533  height = u.height;
534  cam = u.cam;
535  nthreads = u.nthreads;
536  threadid = u.threadid;
537 
538  return *this;
539  }
540 
541  static void *vpUndistort_threaded(void *arg);
542 };
543 
544 template <class Type> void *vpUndistortInternalType<Type>::vpUndistort_threaded(void *arg)
545 {
546  vpUndistortInternalType<Type> *undistortSharedData = static_cast<vpUndistortInternalType<Type> *>(arg);
547  int offset = (int)undistortSharedData->threadid;
548  int width = (int)undistortSharedData->width;
549  int height = (int)undistortSharedData->height;
550  int nthreads = (int)undistortSharedData->nthreads;
551 
552  double u0 = undistortSharedData->cam.get_u0();
553  double v0 = undistortSharedData->cam.get_v0();
554  double px = undistortSharedData->cam.get_px();
555  double py = undistortSharedData->cam.get_py();
556  double kud = undistortSharedData->cam.get_kud();
557 
558  double invpx = 1.0 / px;
559  double invpy = 1.0 / py;
560 
561  double kud_px2 = kud * invpx * invpx;
562  double kud_py2 = kud * invpy * invpy;
563 
564  Type *dst = undistortSharedData->dst + (height / nthreads * offset) * width;
565  Type *src = undistortSharedData->src;
566 
567  for (double v = height / nthreads * offset; v < height / nthreads * (offset + 1); v++) {
568  double deltav = v - v0;
569  // double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy));
570  double fr1 = 1.0 + kud_py2 * deltav * deltav;
571 
572  for (double u = 0; u < width; u++) {
573  // computation of u,v : corresponding pixel coordinates in I.
574  double deltau = u - u0;
575  // double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx));
576  double fr2 = fr1 + kud_px2 * deltau * deltau;
577 
578  double u_double = deltau * fr2 + u0;
579  double v_double = deltav * fr2 + v0;
580 
581  // computation of the bilinear interpolation
582 
583  // declarations
584  int u_round = (int)(u_double);
585  int v_round = (int)(v_double);
586  if (u_round < 0.f)
587  u_round = -1;
588  if (v_round < 0.f)
589  v_round = -1;
590  double du_double = (u_double) - (double)u_round;
591  double dv_double = (v_double) - (double)v_round;
592  Type v01;
593  Type v23;
594  if ((0 <= u_round) && (0 <= v_round) && (u_round < ((width)-1)) && (v_round < ((height)-1))) {
595  // process interpolation
596  const Type *_mp = &src[v_round * width + u_round];
597  v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
598  _mp += width;
599  v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
600  *dst = (Type)(v01 + ((v23 - v01) * dv_double));
601  } else {
602  *dst = 0;
603  }
604  dst++;
605  }
606  }
607 
608  pthread_exit((void *)0);
609  return NULL;
610 }
611 #endif // DOXYGEN_SHOULD_SKIP_THIS
612 #endif // VISP_HAVE_PTHREAD
613 
640 template <class Type>
642  unsigned int nThreads)
643 {
644 #ifdef VISP_HAVE_PTHREAD
645  //
646  // Optimized version using pthreads
647  //
648  unsigned int width = I.getWidth();
649  unsigned int height = I.getHeight();
650 
651  undistI.resize(height, width);
652 
653  double kud = cam.get_kud();
654 
655  // if (kud == 0) {
656  if (std::fabs(kud) <= std::numeric_limits<double>::epsilon()) {
657  // There is no need to undistort the image
658  undistI = I;
659  return;
660  }
661 
662  unsigned int nthreads = nThreads;
663  pthread_attr_t attr;
664  pthread_t *callThd = new pthread_t[nthreads];
665  pthread_attr_init(&attr);
666  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
667 
668  vpUndistortInternalType<Type> *undistortSharedData;
669  undistortSharedData = new vpUndistortInternalType<Type>[nthreads];
670 
671  for (unsigned int i = 0; i < nthreads; i++) {
672  // Each thread works on a different set of data.
673  // vpTRACE("create thread %d", i);
674  undistortSharedData[i].src = I.bitmap;
675  undistortSharedData[i].dst = undistI.bitmap;
676  undistortSharedData[i].width = I.getWidth();
677  undistortSharedData[i].height = I.getHeight();
678  undistortSharedData[i].cam = cam;
679  undistortSharedData[i].nthreads = nthreads;
680  undistortSharedData[i].threadid = i;
681  pthread_create(&callThd[i], &attr, &vpUndistortInternalType<Type>::vpUndistort_threaded, &undistortSharedData[i]);
682  }
683  pthread_attr_destroy(&attr);
684  /* Wait on the other threads */
685 
686  for (unsigned int i = 0; i < nthreads; i++) {
687  // vpTRACE("join thread %d", i);
688  pthread_join(callThd[i], NULL);
689  }
690 
691  delete[] callThd;
692  delete[] undistortSharedData;
693 #else // VISP_HAVE_PTHREAD
694  (void)nThreads;
695  //
696  // optimized version without pthreads
697  //
698  unsigned int width = I.getWidth();
699  unsigned int height = I.getHeight();
700 
701  undistI.resize(height, width);
702 
703  double u0 = cam.get_u0();
704  double v0 = cam.get_v0();
705  double px = cam.get_px();
706  double py = cam.get_py();
707  double kud = cam.get_kud();
708 
709  // if (kud == 0) {
710  if (std::fabs(kud) <= std::numeric_limits<double>::epsilon()) {
711  // There is no need to undistort the image
712  undistI = I;
713  return;
714  }
715 
716  double invpx = 1.0 / px;
717  double invpy = 1.0 / py;
718 
719  double kud_px2 = kud * invpx * invpx;
720  double kud_py2 = kud * invpy * invpy;
721 
722  Type *dst = undistI.bitmap;
723  for (double v = 0; v < height; v++) {
724  double deltav = v - v0;
725  // double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy));
726  double fr1 = 1.0 + kud_py2 * deltav * deltav;
727 
728  for (double u = 0; u < width; u++) {
729  // computation of u,v : corresponding pixel coordinates in I.
730  double deltau = u - u0;
731  // double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx));
732  double fr2 = fr1 + kud_px2 * deltau * deltau;
733 
734  double u_double = deltau * fr2 + u0;
735  double v_double = deltav * fr2 + v0;
736 
737  // printf("[%g][%g] %g %g : ", u, v, u_double, v_double );
738 
739  // computation of the bilinear interpolation
740 
741  // declarations
742  int u_round = (int)(u_double);
743  int v_round = (int)(v_double);
744  if (u_round < 0.f)
745  u_round = -1;
746  if (v_round < 0.f)
747  v_round = -1;
748  double du_double = (u_double) - (double)u_round;
749  double dv_double = (v_double) - (double)v_round;
750  Type v01;
751  Type v23;
752  if ((0 <= u_round) && (0 <= v_round) && (u_round < (((int)width) - 1)) && (v_round < (((int)height) - 1))) {
753  // process interpolation
754  const Type *_mp = &I[(unsigned int)v_round][(unsigned int)u_round];
755  v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
756  _mp += width;
757  v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
758  *dst = (Type)(v01 + ((v23 - v01) * dv_double));
759  // printf("R %d G %d B %d\n", dst->R, dst->G, dst->B);
760  } else {
761  *dst = 0;
762  }
763  dst++;
764  }
765  }
766 #endif // VISP_HAVE_PTHREAD
767 
768 #if 0
769  // non optimized version
770  int width = I.getWidth();
771  int height = I.getHeight();
772 
773  undistI.resize(height,width);
774 
775  double u0 = cam.get_u0();
776  double v0 = cam.get_v0();
777  double px = cam.get_px();
778  double py = cam.get_py();
779  double kd = cam.get_kud();
780 
781  if (kd == 0) {
782  // There is no need to undistort the image
783  undistI = I;
784  return;
785  }
786 
787  for(int v = 0 ; v < height; v++){
788  for(int u = 0; u < height; u++){
789  double r2 = vpMath::sqr(((double)u - u0)/px) +
790  vpMath::sqr(((double)v-v0)/py);
791  double u_double = ((double)u - u0)*(1.0+kd*r2) + u0;
792  double v_double = ((double)v - v0)*(1.0+kd*r2) + v0;
793  undistI[v][u] = I.getPixelBI((float)u_double,(float)v_double);
794  }
795  }
796 #endif
797 }
798 
806 template <class Type> void vpImageTools::flip(const vpImage<Type> &I, vpImage<Type> &newI)
807 {
808  unsigned int height = 0, width = 0;
809 
810  height = I.getHeight();
811  width = I.getWidth();
812  newI.resize(height, width);
813 
814  for (unsigned int i = 0; i < height; i++) {
815  memcpy(newI.bitmap + i * width, I.bitmap + (height - 1 - i) * width, width * sizeof(Type));
816  }
817 }
818 
850 template <class Type> void vpImageTools::flip(vpImage<Type> &I)
851 {
852  unsigned int height = 0, width = 0;
853  unsigned int i = 0;
854  vpImage<Type> Ibuf;
855 
856  height = I.getHeight();
857  width = I.getWidth();
858  Ibuf.resize(1, width);
859 
860  for (i = 0; i < height / 2; i++) {
861  memcpy(Ibuf.bitmap, I.bitmap + i * width, width * sizeof(Type));
862 
863  memcpy(I.bitmap + i * width, I.bitmap + (height - 1 - i) * width, width * sizeof(Type));
864  memcpy(I.bitmap + (height - 1 - i) * width, Ibuf.bitmap, width * sizeof(Type));
865  }
866 }
867 
868 template <class Type> Type vpImageTools::getPixelClamped(const vpImage<Type> &I, float u, float v)
869 {
870  unsigned int i, j;
871  if (u < 0.f)
872  j = 0;
873  else if (u > static_cast<float>(I.getWidth()) - 1.f)
874  j = I.getWidth() - 1;
875  else
876  j = static_cast<unsigned int>(u);
877 
878  if (v < 0.f)
879  i = 0;
880  else if (v > static_cast<float>(I.getHeight()) - 1.f)
881  i = I.getHeight() - 1;
882  else
883  i = static_cast<unsigned int>(v);
884 
885  return I[i][j];
886 }
887 
888 // Reference:
889 // http://blog.demofox.org/2015/08/15/resizing-images-with-bicubic-interpolation/
890 template <class Type>
891 void vpImageTools::resizeBicubic(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int i,
892  unsigned int j, float u, float v, float xFrac, float yFrac)
893 {
894  // 1st row
895  Type p00 = getPixelClamped(I, u - 1, v - 1);
896  Type p01 = getPixelClamped(I, u + 0, v - 1);
897  Type p02 = getPixelClamped(I, u + 1, v - 1);
898  Type p03 = getPixelClamped(I, u + 2, v - 1);
899 
900  // 2nd row
901  Type p10 = getPixelClamped(I, u - 1, v + 0);
902  Type p11 = getPixelClamped(I, u + 0, v + 0);
903  Type p12 = getPixelClamped(I, u + 1, v + 0);
904  Type p13 = getPixelClamped(I, u + 2, v + 0);
905 
906  // 3rd row
907  Type p20 = getPixelClamped(I, u - 1, v + 1);
908  Type p21 = getPixelClamped(I, u + 0, v + 1);
909  Type p22 = getPixelClamped(I, u + 1, v + 1);
910  Type p23 = getPixelClamped(I, u + 2, v + 1);
911 
912  // 4th row
913  Type p30 = getPixelClamped(I, u - 1, v + 2);
914  Type p31 = getPixelClamped(I, u + 0, v + 2);
915  Type p32 = getPixelClamped(I, u + 1, v + 2);
916  Type p33 = getPixelClamped(I, u + 2, v + 2);
917 
918  float col0 = cubicHermite(p00, p01, p02, p03, xFrac);
919  float col1 = cubicHermite(p10, p11, p12, p13, xFrac);
920  float col2 = cubicHermite(p20, p21, p22, p23, xFrac);
921  float col3 = cubicHermite(p30, p31, p32, p33, xFrac);
922  float value = cubicHermite(col0, col1, col2, col3, yFrac);
923  Ires[i][j] = vpMath::saturate<Type>(value);
924 }
925 
926 template <>
927 inline void vpImageTools::resizeBicubic(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &Ires, unsigned int i,
928  unsigned int j, float u, float v, float xFrac, float yFrac)
929 {
930  // 1st row
931  vpRGBa p00 = getPixelClamped(I, u - 1, v - 1);
932  vpRGBa p01 = getPixelClamped(I, u + 0, v - 1);
933  vpRGBa p02 = getPixelClamped(I, u + 1, v - 1);
934  vpRGBa p03 = getPixelClamped(I, u + 2, v - 1);
935 
936  // 2nd row
937  vpRGBa p10 = getPixelClamped(I, u - 1, v + 0);
938  vpRGBa p11 = getPixelClamped(I, u + 0, v + 0);
939  vpRGBa p12 = getPixelClamped(I, u + 1, v + 0);
940  vpRGBa p13 = getPixelClamped(I, u + 2, v + 0);
941 
942  // 3rd row
943  vpRGBa p20 = getPixelClamped(I, u - 1, v + 1);
944  vpRGBa p21 = getPixelClamped(I, u + 0, v + 1);
945  vpRGBa p22 = getPixelClamped(I, u + 1, v + 1);
946  vpRGBa p23 = getPixelClamped(I, u + 2, v + 1);
947 
948  // 4th row
949  vpRGBa p30 = getPixelClamped(I, u - 1, v + 2);
950  vpRGBa p31 = getPixelClamped(I, u + 0, v + 2);
951  vpRGBa p32 = getPixelClamped(I, u + 1, v + 2);
952  vpRGBa p33 = getPixelClamped(I, u + 2, v + 2);
953 
954  for (int c = 0; c < 3; c++) {
955  float col0 = cubicHermite(static_cast<float>(reinterpret_cast<unsigned char *>(&p00)[c]),
956  static_cast<float>(reinterpret_cast<unsigned char *>(&p01)[c]),
957  static_cast<float>(reinterpret_cast<unsigned char *>(&p02)[c]),
958  static_cast<float>(reinterpret_cast<unsigned char *>(&p03)[c]), xFrac);
959  float col1 = cubicHermite(static_cast<float>(reinterpret_cast<unsigned char *>(&p10)[c]),
960  static_cast<float>(reinterpret_cast<unsigned char *>(&p11)[c]),
961  static_cast<float>(reinterpret_cast<unsigned char *>(&p12)[c]),
962  static_cast<float>(reinterpret_cast<unsigned char *>(&p13)[c]), xFrac);
963  float col2 = cubicHermite(static_cast<float>(reinterpret_cast<unsigned char *>(&p20)[c]),
964  static_cast<float>(reinterpret_cast<unsigned char *>(&p21)[c]),
965  static_cast<float>(reinterpret_cast<unsigned char *>(&p22)[c]),
966  static_cast<float>(reinterpret_cast<unsigned char *>(&p23)[c]), xFrac);
967  float col3 = cubicHermite(static_cast<float>(reinterpret_cast<unsigned char *>(&p30)[c]),
968  static_cast<float>(reinterpret_cast<unsigned char *>(&p31)[c]),
969  static_cast<float>(reinterpret_cast<unsigned char *>(&p32)[c]),
970  static_cast<float>(reinterpret_cast<unsigned char *>(&p33)[c]), xFrac);
971  float value = cubicHermite(col0, col1, col2, col3, yFrac);
972 
973  reinterpret_cast<unsigned char *>(&Ires[i][j])[c] = vpMath::saturate<unsigned char>(value);
974  }
975 }
976 
977 template <class Type>
978 void vpImageTools::resizeBilinear(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int i,
979  unsigned int j, float u, float v, float xFrac, float yFrac)
980 {
981  unsigned int u0 = static_cast<unsigned int>(u);
982  unsigned int v0 = static_cast<unsigned int>(v);
983 
984  unsigned int u1 = (std::min)(I.getWidth() - 1, static_cast<unsigned int>(u) + 1);
985  unsigned int v1 = v0;
986 
987  unsigned int u2 = u0;
988  unsigned int v2 = (std::min)(I.getHeight() - 1, static_cast<unsigned int>(v) + 1);
989 
990  unsigned int u3 = u1;
991  unsigned int v3 = v2;
992 
993  float col0 = lerp(I[v0][u0], I[v1][u1], xFrac);
994  float col1 = lerp(I[v2][u2], I[v3][u3], xFrac);
995  float value = lerp(col0, col1, yFrac);
996 
997  Ires[i][j] = vpMath::saturate<Type>(value);
998 }
999 
1000 template <>
1001 inline void vpImageTools::resizeBilinear(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &Ires, unsigned int i,
1002  unsigned int j, float u, float v, float xFrac, float yFrac)
1003 {
1004  unsigned int u0 = static_cast<unsigned int>(u);
1005  unsigned int v0 = static_cast<unsigned int>(v);
1006 
1007  unsigned int u1 = (std::min)(I.getWidth() - 1, static_cast<unsigned int>(u) + 1);
1008  unsigned int v1 = v0;
1009 
1010  unsigned int u2 = u0;
1011  unsigned int v2 = (std::min)(I.getHeight() - 1, static_cast<unsigned int>(v) + 1);
1012 
1013  unsigned int u3 = (std::min)(I.getWidth() - 1, static_cast<unsigned int>(u) + 1);
1014  unsigned int v3 = (std::min)(I.getHeight() - 1, static_cast<unsigned int>(v) + 1);
1015 
1016  for (int c = 0; c < 3; c++) {
1017  float col0 = lerp(static_cast<float>(reinterpret_cast<const unsigned char *>(&I[v0][u0])[c]),
1018  static_cast<float>(reinterpret_cast<const unsigned char *>(&I[v1][u1])[c]), xFrac);
1019  float col1 = lerp(static_cast<float>(reinterpret_cast<const unsigned char *>(&I[v2][u2])[c]),
1020  static_cast<float>(reinterpret_cast<const unsigned char *>(&I[v3][u3])[c]), xFrac);
1021  float value = lerp(col0, col1, yFrac);
1022 
1023  reinterpret_cast<unsigned char *>(&Ires[i][j])[c] = vpMath::saturate<unsigned char>(value);
1024  }
1025 }
1026 
1027 template <class Type>
1028 void vpImageTools::resizeNearest(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int i,
1029  unsigned int j, float u, float v)
1030 {
1031  Ires[i][j] = getPixelClamped(I, u, v);
1032 }
1033 
1047 template <class Type>
1048 void vpImageTools::resize(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int width,
1049  unsigned int height, const vpImageInterpolationType &method,
1050  unsigned int nThreads)
1051 {
1052  Ires.resize(height, width);
1053 
1054  vpImageTools::resize(I, Ires, method, nThreads);
1055 }
1056 
1069 template <class Type>
1071  unsigned int
1072  #if defined _OPENMP
1073  nThreads
1074  #endif
1075  )
1076 {
1077  if (I.getWidth() < 2 || I.getHeight() < 2 || Ires.getWidth() < 2 || Ires.getHeight() < 2) {
1078  std::cerr << "Input or output image is too small!" << std::endl;
1079  return;
1080  }
1081 
1082  float scaleY = (I.getHeight() - 1) / static_cast<float>(Ires.getHeight() - 1);
1083  float scaleX = (I.getWidth() - 1) / static_cast<float>(Ires.getWidth() - 1);
1084 
1085  if (method == INTERPOLATION_NEAREST) {
1086  scaleY = I.getHeight() / static_cast<float>(Ires.getHeight() - 1);
1087  scaleX = I.getWidth() / static_cast<float>(Ires.getWidth() - 1);
1088  }
1089 
1090 #if defined _OPENMP
1091  if (nThreads > 0) {
1092  omp_set_num_threads(static_cast<int>(nThreads));
1093  }
1094  #pragma omp parallel for schedule(dynamic)
1095 #endif
1096  for (int i = 0; i < static_cast<int>(Ires.getHeight()); i++) {
1097  float v = i * scaleY;
1098  float yFrac = v - static_cast<int>(v);
1099 
1100  for (unsigned int j = 0; j < Ires.getWidth(); j++) {
1101  float u = j * scaleX;
1102  float xFrac = u - static_cast<int>(u);
1103 
1104  if (method == INTERPOLATION_NEAREST) {
1105  resizeNearest(I, Ires, static_cast<unsigned int>(i), j, u, v);
1106  } else if (method == INTERPOLATION_LINEAR) {
1107  resizeBilinear(I, Ires, static_cast<unsigned int>(i), j, u, v, xFrac, yFrac);
1108  } else if (method == INTERPOLATION_CUBIC) {
1109  resizeBicubic(I, Ires, static_cast<unsigned int>(i), j, u, v, xFrac, yFrac);
1110  }
1111  }
1112  }
1113 }
1114 
1115 template <> inline
1117  const vpImageInterpolationType &method, unsigned int
1118  #if defined _OPENMP
1119  nThreads
1120  #endif
1121  )
1122 {
1123  if (I.getWidth() < 2 || I.getHeight() < 2 || Ires.getWidth() < 2 || Ires.getHeight() < 2) {
1124  std::cerr << "Input or output image is too small!" << std::endl;
1125  return;
1126  }
1127 
1128  if (method == INTERPOLATION_NEAREST || method == INTERPOLATION_CUBIC) {
1129  float scaleY = (I.getHeight() - 1) / static_cast<float>(Ires.getHeight() - 1);
1130  float scaleX = (I.getWidth() - 1) / static_cast<float>(Ires.getWidth() - 1);
1131 
1132  if (method == INTERPOLATION_NEAREST) {
1133  scaleY = I.getHeight() / static_cast<float>(Ires.getHeight() - 1);
1134  scaleX = I.getWidth() / static_cast<float>(Ires.getWidth() - 1);
1135  }
1136 
1137  #if defined _OPENMP
1138  if (nThreads > 0) {
1139  omp_set_num_threads(static_cast<int>(nThreads));
1140  }
1141  #pragma omp parallel for schedule(dynamic)
1142  #endif
1143  for (int i = 0; i < static_cast<int>(Ires.getHeight()); i++) {
1144  float v = i * scaleY;
1145  float yFrac = v - static_cast<int>(v);
1146 
1147  for (unsigned int j = 0; j < Ires.getWidth(); j++) {
1148  float u = j * scaleX;
1149  float xFrac = u - static_cast<int>(u);
1150 
1151  if (method == INTERPOLATION_NEAREST) {
1152  resizeNearest(I, Ires, static_cast<unsigned int>(i), j, u, v);
1153  } else if (method == INTERPOLATION_CUBIC) {
1154  resizeBicubic(I, Ires, static_cast<unsigned int>(i), j, u, v, xFrac, yFrac);
1155  }
1156  }
1157  }
1158  } else if (method == INTERPOLATION_LINEAR) {
1159  const int precision = 1 << 16;
1160  int64_t scaleY = static_cast<int64_t>((I.getHeight() - 1) / static_cast<float>(Ires.getHeight() - 1) * precision);
1161  int64_t scaleX = static_cast<int64_t>((I.getWidth() - 1) / static_cast<float>(Ires.getWidth() - 1) * precision);
1162 
1163 #if defined _OPENMP
1164  if (nThreads > 0) {
1165  omp_set_num_threads(static_cast<int>(nThreads));
1166  }
1167 #pragma omp parallel for schedule(dynamic)
1168 #endif
1169  for (int i = 0; i < static_cast<int>(Ires.getHeight()); i++) {
1170  int64_t v = i * scaleY;
1171  int64_t vround = v & (~0xFFFF);
1172  int64_t rratio = v - vround;
1173  int64_t y_ = v >> 16;
1174  int64_t rfrac = precision - rratio;
1175 
1176  for (unsigned int j = 0; j < Ires.getWidth(); j++) {
1177  int64_t u = j * scaleX;
1178  int64_t uround = u & (~0xFFFF);
1179  int64_t cratio = u - uround;
1180  int64_t x_ = u >> 16;
1181  int64_t cfrac = precision - cratio;
1182 
1183  if (y_ + 1 < static_cast<int64_t>(I.getHeight()) && x_ + 1 < static_cast<int64_t>(I.getWidth())) {
1184  int64_t up = *reinterpret_cast<uint16_t *>(I.bitmap + y_ * I.getWidth() + x_);
1185  int64_t down = *reinterpret_cast<uint16_t *>(I.bitmap + (y_ + 1) * I.getWidth() + x_);
1186 
1187  Ires[i][j] = static_cast<unsigned char>((((up & 0x00FF) * rfrac + (down & 0x00FF) * rratio) * cfrac +
1188  ((up >> 8) * rfrac + (down >> 8) * rratio) * cratio) >> 32);
1189  } else if (y_ + 1 < static_cast<int64_t>(I.getHeight())) {
1190  Ires[i][j] = static_cast<unsigned char>(((*(I.bitmap + y_ * I.getWidth() + x_)
1191  * rfrac + *(I.bitmap + (y_ + 1) * I.getWidth() + x_) * rratio)) >> 16);
1192  } else if (x_ + 1 < static_cast<int64_t>(I.getWidth())) {
1193  uint16_t up = *reinterpret_cast<uint16_t *>(I.bitmap + y_ * I.getWidth() + x_);
1194  Ires[i][j] = static_cast<unsigned char>(((up & 0x00FF) * cfrac + (up >> 8) * cratio) >> 16);
1195  } else {
1196  Ires[i][j] = *(I.bitmap + y_ * I.getWidth() + x_);
1197  }
1198  }
1199  }
1200  }
1201 }
1202 
1203 template <> inline
1205  const vpImageInterpolationType &method, unsigned int
1206  #if defined _OPENMP
1207  nThreads
1208  #endif
1209  )
1210 {
1211  if (I.getWidth() < 2 || I.getHeight() < 2 || Ires.getWidth() < 2 || Ires.getHeight() < 2) {
1212  std::cerr << "Input or output image is too small!" << std::endl;
1213  return;
1214  }
1215 
1216  if (method == INTERPOLATION_NEAREST || method == INTERPOLATION_CUBIC) {
1217  float scaleY = (I.getHeight() - 1) / static_cast<float>(Ires.getHeight() - 1);
1218  float scaleX = (I.getWidth() - 1) / static_cast<float>(Ires.getWidth() - 1);
1219 
1220  if (method == INTERPOLATION_NEAREST) {
1221  scaleY = I.getHeight() / static_cast<float>(Ires.getHeight() - 1);
1222  scaleX = I.getWidth() / static_cast<float>(Ires.getWidth() - 1);
1223  }
1224 
1225  #if defined _OPENMP
1226  if (nThreads > 0) {
1227  omp_set_num_threads(static_cast<int>(nThreads));
1228  }
1229  #pragma omp parallel for schedule(dynamic)
1230  #endif
1231  for (int i = 0; i < static_cast<int>(Ires.getHeight()); i++) {
1232  float v = i * scaleY;
1233  float yFrac = v - static_cast<int>(v);
1234 
1235  for (unsigned int j = 0; j < Ires.getWidth(); j++) {
1236  float u = j * scaleX;
1237  float xFrac = u - static_cast<int>(u);
1238 
1239  if (method == INTERPOLATION_NEAREST) {
1240  resizeNearest(I, Ires, static_cast<unsigned int>(i), j, u, v);
1241  } else if (method == INTERPOLATION_CUBIC) {
1242  resizeBicubic(I, Ires, static_cast<unsigned int>(i), j, u, v, xFrac, yFrac);
1243  }
1244  }
1245  }
1246  } else {
1247  const int precision = 1 << 16;
1248  int64_t scaleY = static_cast<int64_t>((I.getHeight() - 1) / static_cast<float>(Ires.getHeight() - 1) * precision);
1249  int64_t scaleX = static_cast<int64_t>((I.getWidth() - 1) / static_cast<float>(Ires.getWidth() - 1) * precision);
1250 
1251 #if defined _OPENMP
1252  if (nThreads > 0) {
1253  omp_set_num_threads(static_cast<int>(nThreads));
1254  }
1255 #pragma omp parallel for schedule(dynamic)
1256 #endif
1257  for (int i = 0; i < static_cast<int>(Ires.getHeight()); i++) {
1258  int64_t v = i * scaleY;
1259  int64_t vround = v & (~0xFFFF);
1260  int64_t rratio = v - vround;
1261  int64_t y_ = v >> 16;
1262  int64_t rfrac = precision - rratio;
1263 
1264  for (unsigned int j = 0; j < Ires.getWidth(); j++) {
1265  int64_t u = j * scaleX;
1266  int64_t uround = u & (~0xFFFF);
1267  int64_t cratio = u - uround;
1268  int64_t x_ = u >> 16;
1269  int64_t cfrac = precision - cratio;
1270 
1271  if (y_ + 1 < static_cast<int64_t>(I.getHeight()) && x_ + 1 < static_cast<int64_t>(I.getWidth())) {
1272  int64_t col0 = lerp2((I.bitmap + y_ * I.getWidth() + x_)->R, (I.bitmap + (y_ + 1) * I.getWidth() + x_)->R, rratio, rfrac);
1273  int64_t col1 = lerp2((I.bitmap + y_ * I.getWidth() + x_ + 1)->R, (I.bitmap + (y_ + 1) * I.getWidth() + x_ + 1)->R, rratio, rfrac);
1274  int64_t valueR = lerp2(col0, col1, cratio, cfrac);
1275 
1276  col0 = lerp2((I.bitmap + y_ * I.getWidth() + x_)->G, (I.bitmap + (y_ + 1) * I.getWidth() + x_)->G, rratio, rfrac);
1277  col1 = lerp2((I.bitmap + y_ * I.getWidth() + x_ + 1)->G, (I.bitmap + (y_ + 1) * I.getWidth() + x_ + 1)->G, rratio, rfrac);
1278  int64_t valueG = lerp2(col0, col1, cratio, cfrac);
1279 
1280  col0 = lerp2((I.bitmap + y_ * I.getWidth() + x_)->B, (I.bitmap + (y_ + 1) * I.getWidth() + x_)->B, rratio, rfrac);
1281  col1 = lerp2((I.bitmap + y_ * I.getWidth() + x_ + 1)->B, (I.bitmap + (y_ + 1) * I.getWidth() + x_ + 1)->B, rratio, rfrac);
1282  int64_t valueB = lerp2(col0, col1, cratio, cfrac);
1283 
1284  Ires[i][j] = vpRGBa(static_cast<unsigned char>(valueR >> 32),
1285  static_cast<unsigned char>(valueG >> 32),
1286  static_cast<unsigned char>(valueB >> 32));
1287  } else if (y_ + 1 < static_cast<int64_t>(I.getHeight())) {
1288  int64_t valueR = lerp2((I.bitmap + y_ * I.getWidth() + x_)->R, (I.bitmap + (y_ + 1) * I.getWidth() + x_)->R, rratio, rfrac);
1289  int64_t valueG = lerp2((I.bitmap + y_ * I.getWidth() + x_)->G, (I.bitmap + (y_ + 1) * I.getWidth() + x_)->G, rratio, rfrac);
1290  int64_t valueB = lerp2((I.bitmap + y_ * I.getWidth() + x_)->B, (I.bitmap + (y_ + 1) * I.getWidth() + x_)->B, rratio, rfrac);
1291 
1292  Ires[i][j] = vpRGBa(static_cast<unsigned char>(valueR >> 16),
1293  static_cast<unsigned char>(valueG >> 16),
1294  static_cast<unsigned char>(valueB >> 16));
1295  } else if (x_ + 1 < static_cast<int64_t>(I.getWidth())) {
1296  int64_t valueR = lerp2((I.bitmap + x_)->R, (I.bitmap + x_ + 1)->R, cratio, cfrac);
1297  int64_t valueG = lerp2((I.bitmap + x_)->G, (I.bitmap + x_ + 1)->G, cratio, cfrac);
1298  int64_t valueB = lerp2((I.bitmap + x_)->B, (I.bitmap + x_ + 1)->B, cratio, cfrac);
1299 
1300  Ires[i][j] = vpRGBa(static_cast<unsigned char>(valueR >> 16),
1301  static_cast<unsigned char>(valueG >> 16),
1302  static_cast<unsigned char>(valueB >> 16));
1303  } else {
1304  Ires[i][j] = *(I.bitmap + y_ * I.getWidth() + x_);
1305  }
1306  }
1307  }
1308  }
1309 }
1310 
1325 template <class Type>
1327  const vpImageInterpolationType &interpolation,
1328  bool fixedPointArithmetic, bool pixelCenter)
1329 {
1330  if ((T.getRows() != 2 && T.getRows() != 3) || T.getCols() != 3) {
1331  std::cerr << "Input transformation must be a (2x3) or (3x3) matrix." << std::endl;
1332  return;
1333  }
1334 
1335  if (src.getSize() == 0) {
1336  return;
1337  }
1338 
1339  const bool affine = (T.getRows() == 2);
1340  const bool interp_NN = (interpolation == INTERPOLATION_NEAREST) || (interpolation == INTERPOLATION_CUBIC);
1341 
1342  if (dst.getSize() == 0) {
1343  dst.resize(src.getHeight(), src.getWidth(), Type(0));
1344  }
1345 
1346  vpMatrix M = T;
1347  if (affine) {
1348  double D = M[0][0] * M[1][1] - M[0][1] * M[1][0];
1349  D = !vpMath::nul(D, std::numeric_limits<double>::epsilon()) ? 1.0 / D : 0;
1350  double A11 = M[1][1] * D, A22 = M[0][0] * D;
1351  M[0][0] = A11; M[0][1] *= -D;
1352  M[1][0] *= -D; M[1][1] = A22;
1353  double b1 = -M[0][0] * M[0][2] - M[0][1] * M[1][2];
1354  double b2 = -M[1][0] * M[0][2] - M[1][1] * M[1][2];
1355  M[0][2] = b1; M[1][2] = b2;
1356  } else {
1357  M = T.inverseByLU();
1358  }
1359 
1360  if (fixedPointArithmetic && !pixelCenter) {
1361  fixedPointArithmetic = checkFixedPoint(0, 0, M, affine) &&
1362  checkFixedPoint(dst.getWidth()-1, 0, M, affine) &&
1363  checkFixedPoint(0, dst.getHeight()-1, M, affine) &&
1364  checkFixedPoint(dst.getWidth() - 1, dst.getHeight() - 1, M, affine);
1365  }
1366 
1367  if (interp_NN) {
1368  //nearest neighbor interpolation
1369  warpNN(src, M, dst, affine, pixelCenter, fixedPointArithmetic);
1370  } else {
1371  //bilinear interpolation
1372  warpLinear(src, M, dst, affine, pixelCenter, fixedPointArithmetic);
1373  }
1374 }
1375 
1376 template <class Type>
1377 void vpImageTools::warpNN(const vpImage<Type> &src, const vpMatrix &T, vpImage<Type> &dst, bool affine,
1378  bool centerCorner, bool fixedPoint)
1379 {
1380  if (fixedPoint && !centerCorner) {
1381  const int nbits = 16;
1382  const int32_t precision = 1 << nbits;
1383  const float precision_1 = 1 / static_cast<float>(precision);
1384 
1385  int32_t a0_i32 = static_cast<int32_t>(T[0][0] * precision);
1386  int32_t a1_i32 = static_cast<int32_t>(T[0][1] * precision);
1387  int32_t a2_i32 = static_cast<int32_t>(T[0][2] * precision);
1388  int32_t a3_i32 = static_cast<int32_t>(T[1][0] * precision);
1389  int32_t a4_i32 = static_cast<int32_t>(T[1][1] * precision);
1390  int32_t a5_i32 = static_cast<int32_t>(T[1][2] * precision);
1391  int32_t a6_i32 = T.getRows() == 3 ? static_cast<int32_t>(T[2][0] * precision) : 0;
1392  int32_t a7_i32 = T.getRows() == 3 ? static_cast<int32_t>(T[2][1] * precision) : 0;
1393  int32_t a8_i32 = T.getRows() == 3 ? static_cast<int32_t>(T[2][2] * precision) : 1;
1394 
1395  int32_t height_1_i32 = static_cast<int32_t>((src.getHeight() - 1) * precision) + 0x8000;
1396  int32_t width_1_i32 = static_cast<int32_t>((src.getWidth() - 1) * precision) + 0x8000;
1397 
1398  if (affine) {
1399  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1400  int32_t xi = a2_i32;
1401  int32_t yi = a5_i32;
1402 
1403  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1404  if (yi >= 0 && yi < height_1_i32 && xi >= 0 && xi < width_1_i32) {
1405  float x_ = (xi >> nbits) + (xi & 0xFFFF) * precision_1;
1406  float y_ = (yi >> nbits) + (yi & 0xFFFF) * precision_1;
1407 
1408  int x = vpMath::round(x_);
1409  int y = vpMath::round(y_);
1410  dst[i][j] = src[y][x];
1411  }
1412 
1413  xi += a0_i32;
1414  yi += a3_i32;
1415  }
1416 
1417  a2_i32 += a1_i32;
1418  a5_i32 += a4_i32;
1419  }
1420  } else {
1421  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1422  int64_t xi = a2_i32;
1423  int64_t yi = a5_i32;
1424  int64_t wi = a8_i32;
1425 
1426  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1427  if (wi != 0 && yi >= 0 && yi <= (static_cast<int>(src.getHeight()) - 1)*wi &&
1428  xi >= 0 && xi <= (static_cast<int>(src.getWidth()) - 1)*wi) {
1429  float w_ = (wi >> nbits) + (wi & 0xFFFF) * precision_1;
1430  float x_ = ((xi >> nbits) + (xi & 0xFFFF) * precision_1) / w_;
1431  float y_ = ((yi >> nbits) + (yi & 0xFFFF) * precision_1) / w_;
1432 
1433  int x = vpMath::round(x_);
1434  int y = vpMath::round(y_);
1435 
1436  dst[i][j] = src[y][x];
1437  }
1438 
1439  xi += a0_i32;
1440  yi += a3_i32;
1441  wi += a6_i32;
1442  }
1443 
1444  a2_i32 += a1_i32;
1445  a5_i32 += a4_i32;
1446  a8_i32 += a7_i32;
1447  }
1448  }
1449  } else {
1450  double a0 = T[0][0]; double a1 = T[0][1]; double a2 = T[0][2];
1451  double a3 = T[1][0]; double a4 = T[1][1]; double a5 = T[1][2];
1452  double a6 = affine ? 0.0 : T[2][0];
1453  double a7 = affine ? 0.0 : T[2][1];
1454  double a8 = affine ? 1.0 : T[2][2];
1455 
1456  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1457  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1458  double x = a0 * (centerCorner ? j + 0.5 : j) + a1 * (centerCorner ? i + 0.5 : i) + a2;
1459  double y = a3 * (centerCorner ? j + 0.5 : j) + a4 * (centerCorner ? i + 0.5 : i) + a5;
1460  double w = a6 * (centerCorner ? j + 0.5 : j) + a7 * (centerCorner ? i + 0.5 : i) + a8;
1461 
1462  if (vpMath::nul(w, std::numeric_limits<double>::epsilon())) {
1463  w = 1.0;
1464  }
1465 
1466  int x_ = centerCorner ? coordCast(x / w) : vpMath::round(x / w);
1467  int y_ = centerCorner ? coordCast(y / w) : vpMath::round(y / w);
1468 
1469  if (x_ >= 0 && x_ < static_cast<int>(src.getWidth()) &&
1470  y_ >= 0 && y_ < static_cast<int>(src.getHeight())) {
1471  dst[i][j] = src[y_][x_];
1472  }
1473  }
1474  }
1475  }
1476 }
1477 
1478 template <class Type>
1479 void vpImageTools::warpLinear(const vpImage<Type> &src, const vpMatrix &T, vpImage<Type> &dst, bool affine,
1480  bool centerCorner, bool fixedPoint)
1481 {
1482  if (fixedPoint && !centerCorner) {
1483  const int nbits = 16;
1484  const int64_t precision = 1 << nbits;
1485  const float precision_1 = 1 / static_cast<float>(precision);
1486  const int64_t precision2 = 1ULL << (2 * nbits);
1487  const float precision_2 = 1 / static_cast<float>(precision2);
1488 
1489  int64_t a0_i64 = static_cast<int64_t>(T[0][0] * precision);
1490  int64_t a1_i64 = static_cast<int64_t>(T[0][1] * precision);
1491  int64_t a2_i64 = static_cast<int64_t>(T[0][2] * precision);
1492  int64_t a3_i64 = static_cast<int64_t>(T[1][0] * precision);
1493  int64_t a4_i64 = static_cast<int64_t>(T[1][1] * precision);
1494  int64_t a5_i64 = static_cast<int64_t>(T[1][2] * precision);
1495  int64_t a6_i64 = T.getRows() == 3 ? static_cast<int64_t>(T[2][0] * precision) : 0;
1496  int64_t a7_i64 = T.getRows() == 3 ? static_cast<int64_t>(T[2][1] * precision) : 0;
1497  int64_t a8_i64 = T.getRows() == 3 ? static_cast<int64_t>(T[2][2] * precision) : 1;
1498 
1499  int64_t height_i64 = static_cast<int64_t>(src.getHeight() * precision);
1500  int64_t width_i64 = static_cast<int64_t>(src.getWidth() * precision);
1501 
1502  if (affine) {
1503  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1504  int64_t xi_ = a2_i64;
1505  int64_t yi_ = a5_i64;
1506 
1507  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1508  if (yi_ >= 0 && yi_ < height_i64 && xi_ >= 0 && xi_ < width_i64) {
1509  const int64_t xi_lower = xi_ & (~0xFFFF);
1510  const int64_t yi_lower = yi_ & (~0xFFFF);
1511 
1512  const int64_t t = yi_ - yi_lower;
1513  const int64_t t_1 = precision - t;
1514  const int64_t s = xi_ - xi_lower;
1515  const int64_t s_1 = precision - s;
1516 
1517  const int x_ = static_cast<int>(xi_ >> nbits);
1518  const int y_ = static_cast<int>(yi_ >> nbits);
1519 
1520  if (y_ < static_cast<int>(src.getHeight())-1 && x_ < static_cast<int>(src.getWidth())-1) {
1521  const Type val00 = src[y_][x_];
1522  const Type val01 = src[y_][x_+1];
1523  const Type val10 = src[y_+1][x_];
1524  const Type val11 = src[y_+1][x_+1];
1525  const int64_t interp_i64 = static_cast<int64_t>(s_1*t_1*val00 + s*t_1*val01 + s_1*t*val10 + s*t*val11);
1526  const float interp = (interp_i64 >> (nbits*2)) + (interp_i64 & 0xFFFFFFFF) * precision_2;
1527  dst[i][j] = vpMath::saturate<Type>(interp);
1528  } else if (y_ < static_cast<int>(src.getHeight())-1) {
1529  const Type val00 = src[y_][x_];
1530  const Type val10 = src[y_+1][x_];
1531  const int64_t interp_i64 = static_cast<int64_t>(t_1*val00 + t*val10);
1532  const float interp = (interp_i64 >> nbits) + (interp_i64 & 0xFFFF) * precision_1;
1533  dst[i][j] = vpMath::saturate<Type>(interp);
1534  } else if (x_ < static_cast<int>(src.getWidth())-1) {
1535  const Type val00 = src[y_][x_];
1536  const Type val01 = src[y_][x_+1];
1537  const int64_t interp_i64 = static_cast<int64_t>(s_1*val00 + s*val01);
1538  const float interp = (interp_i64 >> nbits) + (interp_i64 & 0xFFFF) * precision_1;
1539  dst[i][j] = vpMath::saturate<Type>(interp);
1540  } else {
1541  dst[i][j] = src[y_][x_];
1542  }
1543  }
1544 
1545  xi_ += a0_i64;
1546  yi_ += a3_i64;
1547  }
1548 
1549  a2_i64 += a1_i64;
1550  a5_i64 += a4_i64;
1551  }
1552  } else {
1553  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1554  int64_t xi = a2_i64;
1555  int64_t yi = a5_i64;
1556  int64_t wi = a8_i64;
1557 
1558  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1559  if (wi != 0 && yi >= 0 && yi <= (static_cast<int>(src.getHeight()) - 1)*wi &&
1560  xi >= 0 && xi <= (static_cast<int>(src.getWidth()) - 1)*wi) {
1561  const float wi_ = (wi >> nbits) + (wi & 0xFFFF) * precision_1;
1562  const float xi_ = ((xi >> nbits) + (xi & 0xFFFF) * precision_1) / wi_;
1563  const float yi_ = ((yi >> nbits) + (yi & 0xFFFF) * precision_1) / wi_;
1564 
1565  const int x_ = static_cast<int>(xi_);
1566  const int y_ = static_cast<int>(yi_);
1567 
1568  const float t = yi_ - y_;
1569  const float s = xi_ - x_;
1570 
1571  if (y_ < static_cast<int>(src.getHeight()) - 1 && x_ < static_cast<int>(src.getWidth()) - 1) {
1572  const Type val00 = src[y_][x_];
1573  const Type val01 = src[y_][x_ + 1];
1574  const Type val10 = src[y_ + 1][x_];
1575  const Type val11 = src[y_ + 1][x_ + 1];
1576  const float col0 = lerp(val00, val01, s);
1577  const float col1 = lerp(val10, val11, s);
1578  const float interp = lerp(col0, col1, t);
1579  dst[i][j] = vpMath::saturate<Type>(interp);
1580  } else if (y_ < static_cast<int>(src.getHeight()) - 1) {
1581  const Type val00 = src[y_][x_];
1582  const Type val10 = src[y_ + 1][x_];
1583  const float interp = lerp(val00, val10, t);
1584  dst[i][j] = vpMath::saturate<Type>(interp);
1585  } else if (x_ < static_cast<int>(src.getWidth()) - 1) {
1586  const Type val00 = src[y_][x_];
1587  const Type val01 = src[y_][x_ + 1];
1588  const float interp = lerp(val00, val01, s);
1589  dst[i][j] = vpMath::saturate<Type>(interp);
1590  } else {
1591  dst[i][j] = src[y_][x_];
1592  }
1593  }
1594 
1595  xi += a0_i64;
1596  yi += a3_i64;
1597  wi += a6_i64;
1598  }
1599 
1600  a2_i64 += a1_i64;
1601  a5_i64 += a4_i64;
1602  a8_i64 += a7_i64;
1603  }
1604  }
1605  } else {
1606  double a0 = T[0][0]; double a1 = T[0][1]; double a2 = T[0][2];
1607  double a3 = T[1][0]; double a4 = T[1][1]; double a5 = T[1][2];
1608  double a6 = affine ? 0.0 : T[2][0];
1609  double a7 = affine ? 0.0 : T[2][1];
1610  double a8 = affine ? 1.0 : T[2][2];
1611 
1612  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1613  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1614  double x = a0 * (centerCorner ? j + 0.5 : j) + a1 * (centerCorner ? i + 0.5 : i) + a2;
1615  double y = a3 * (centerCorner ? j + 0.5 : j) + a4 * (centerCorner ? i + 0.5 : i) + a5;
1616  double w = a6 * (centerCorner ? j + 0.5 : j) + a7 * (centerCorner ? i + 0.5 : i) + a8;
1617  if (vpMath::nul(w, std::numeric_limits<double>::epsilon())) {
1618  w = 1;
1619  }
1620 
1621  x = x / w - (centerCorner ? 0.5 : 0);
1622  y = y / w - (centerCorner ? 0.5 : 0);
1623 
1624  int x_lower = static_cast<int>(x);
1625  int y_lower = static_cast<int>(y);
1626 
1627  if (y_lower >= static_cast<int>(src.getHeight()) || x_lower >= static_cast<int>(src.getWidth()) ||
1628  y < 0 || x < 0) {
1629  continue;
1630  }
1631 
1632  double s = x - x_lower;
1633  double t = y - y_lower;
1634 
1635  if (y_lower < static_cast<int>(src.getHeight())-1 && x_lower < static_cast<int>(src.getWidth())-1) {
1636  const Type val00 = src[y_lower][x_lower];
1637  const Type val01 = src[y_lower][x_lower + 1];
1638  const Type val10 = src[y_lower + 1][x_lower];
1639  const Type val11 = src[y_lower + 1][x_lower + 1];
1640  const double col0 = lerp(val00, val01, s);
1641  const double col1 = lerp(val10, val11, s);
1642  const double interp = lerp(col0, col1, t);
1643  dst[i][j] = vpMath::saturate<Type>(interp);
1644  } else if (y_lower < static_cast<int>(src.getHeight())-1) {
1645  const Type val00 = src[y_lower][x_lower];
1646  const Type val10 = src[y_lower + 1][x_lower];
1647  const double interp = lerp(val00, val10, t);
1648  dst[i][j] = vpMath::saturate<Type>(interp);
1649  } else if (x_lower < static_cast<int>(src.getWidth())-1) {
1650  const Type val00 = src[y_lower][x_lower];
1651  const Type val01 = src[y_lower][x_lower + 1];
1652  const double interp = lerp(val00, val01, s);
1653  dst[i][j] = vpMath::saturate<Type>(interp);
1654  } else {
1655  dst[i][j] = src[y_lower][x_lower];
1656  }
1657  }
1658  }
1659  }
1660 }
1661 
1662 template <> inline
1663 void vpImageTools::warpLinear(const vpImage<vpRGBa> &src, const vpMatrix &T, vpImage<vpRGBa> &dst, bool affine,
1664  bool centerCorner, bool fixedPoint)
1665 {
1666  if (fixedPoint && !centerCorner) {
1667  const int nbits = 16;
1668  const int64_t precision = 1 << nbits;
1669  const float precision_1 = 1 / static_cast<float>(precision);
1670  const int64_t precision2 = 1ULL << (2 * nbits);
1671  const float precision_2 = 1 / static_cast<float>(precision2);
1672 
1673  int64_t a0_i64 = static_cast<int64_t>(T[0][0] * precision);
1674  int64_t a1_i64 = static_cast<int64_t>(T[0][1] * precision);
1675  int64_t a2_i64 = static_cast<int64_t>(T[0][2] * precision);
1676  int64_t a3_i64 = static_cast<int64_t>(T[1][0] * precision);
1677  int64_t a4_i64 = static_cast<int64_t>(T[1][1] * precision);
1678  int64_t a5_i64 = static_cast<int64_t>(T[1][2] * precision);
1679  int64_t a6_i64 = T.getRows() == 3 ? static_cast<int64_t>(T[2][0] * precision) : 0;
1680  int64_t a7_i64 = T.getRows() == 3 ? static_cast<int64_t>(T[2][1] * precision) : 0;
1681  int64_t a8_i64 = precision;
1682 
1683  int64_t height_i64 = static_cast<int64_t>(src.getHeight() * precision);
1684  int64_t width_i64 = static_cast<int64_t>(src.getWidth() * precision);
1685 
1686  if (affine) {
1687  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1688  int64_t xi = a2_i64;
1689  int64_t yi = a5_i64;
1690 
1691  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1692  if (yi >= 0 && yi < height_i64 && xi >= 0 && xi < width_i64) {
1693  const int64_t xi_lower = xi & (~0xFFFF);
1694  const int64_t yi_lower = yi & (~0xFFFF);
1695 
1696  const int64_t t = yi - yi_lower;
1697  const int64_t t_1 = precision - t;
1698  const int64_t s = xi - xi_lower;
1699  const int64_t s_1 = precision - s;
1700 
1701  const int x_ = static_cast<int>(xi >> nbits);
1702  const int y_ = static_cast<int>(yi >> nbits);
1703 
1704  if (y_ < static_cast<int>(src.getHeight())-1 && x_ < static_cast<int>(src.getWidth())-1) {
1705  const vpRGBa val00 = src[y_][x_];
1706  const vpRGBa val01 = src[y_][x_+1];
1707  const vpRGBa val10 = src[y_+1][x_];
1708  const vpRGBa val11 = src[y_+1][x_+1];
1709  const int64_t interpR_i64 = static_cast<int64_t>(s_1*t_1*val00.R + s * t_1*val01.R + s_1 * t*val10.R + s * t*val11.R);
1710  const float interpR = (interpR_i64 >> (nbits*2)) + (interpR_i64 & 0xFFFFFFFF) * precision_2;
1711 
1712  const int64_t interpG_i64 = static_cast<int64_t>(s_1*t_1*val00.G + s * t_1*val01.G + s_1 * t*val10.G + s * t*val11.G);
1713  const float interpG = (interpG_i64 >> (nbits * 2)) + (interpG_i64 & 0xFFFFFFFF) * precision_2;
1714 
1715  const int64_t interpB_i64 = static_cast<int64_t>(s_1*t_1*val00.B + s * t_1*val01.B + s_1 * t*val10.B + s * t*val11.B);
1716  const float interpB = (interpB_i64 >> (nbits * 2)) + (interpB_i64 & 0xFFFFFFFF) * precision_2;
1717 
1718  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR),
1719  vpMath::saturate<unsigned char>(interpG),
1720  vpMath::saturate<unsigned char>(interpB),
1721  255);
1722  } else if (y_ < static_cast<int>(src.getHeight())-1) {
1723  const vpRGBa val00 = src[y_][x_];
1724  const vpRGBa val10 = src[y_+1][x_];
1725  const int64_t interpR_i64 = static_cast<int64_t>(t_1*val00.R + t*val10.R);
1726  const float interpR = (interpR_i64 >> nbits) + (interpR_i64 & 0xFFFF) * precision_1;
1727 
1728  const int64_t interpG_i64 = static_cast<int64_t>(t_1*val00.G + t * val10.G);
1729  const float interpG = (interpG_i64 >> nbits) + (interpG_i64 & 0xFFFF) * precision_1;
1730 
1731  const int64_t interpB_i64 = static_cast<int64_t>(t_1*val00.B + t * val10.B);
1732  const float interpB = (interpB_i64 >> nbits) + (interpB_i64 & 0xFFFF) * precision_1;
1733 
1734  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR),
1735  vpMath::saturate<unsigned char>(interpG),
1736  vpMath::saturate<unsigned char>(interpB),
1737  255);
1738  } else if (x_ < static_cast<int>(src.getWidth())-1) {
1739  const vpRGBa val00 = src[y_][x_];
1740  const vpRGBa val01 = src[y_][x_+1];
1741  const int64_t interpR_i64 = static_cast<int64_t>(s_1*val00.R + s*val01.R);
1742  const float interpR = (interpR_i64 >> nbits) + (interpR_i64 & 0xFFFF) * precision_1;
1743 
1744  const int64_t interpG_i64 = static_cast<int64_t>(s_1*val00.G + s * val01.G);
1745  const float interpG = (interpG_i64 >> nbits) + (interpG_i64 & 0xFFFF) * precision_1;
1746 
1747  const int64_t interpB_i64 = static_cast<int64_t>(s_1*val00.B + s * val01.B);
1748  const float interpB = (interpB_i64 >> nbits) + (interpB_i64 & 0xFFFF) * precision_1;
1749 
1750  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR),
1751  vpMath::saturate<unsigned char>(interpG),
1752  vpMath::saturate<unsigned char>(interpB),
1753  255);
1754  } else {
1755  dst[i][j] = src[y_][x_];
1756  }
1757  }
1758 
1759  xi += a0_i64;
1760  yi += a3_i64;
1761  }
1762 
1763  a2_i64 += a1_i64;
1764  a5_i64 += a4_i64;
1765  }
1766  } else {
1767  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1768  int64_t xi = a2_i64;
1769  int64_t yi = a5_i64;
1770  int64_t wi = a8_i64;
1771 
1772  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1773  if (yi >= 0 && yi <= (static_cast<int>(src.getHeight()) - 1)*wi &&
1774  xi >= 0 && xi <= (static_cast<int>(src.getWidth()) - 1)*wi) {
1775  const float wi_ = (wi >> nbits) + (wi & 0xFFFF) * precision_1;
1776  const float xi_ = ((xi >> nbits) + (xi & 0xFFFF) * precision_1) / wi_;
1777  const float yi_ = ((yi >> nbits) + (yi & 0xFFFF) * precision_1) / wi_;
1778 
1779  const int x_ = static_cast<int>(xi_);
1780  const int y_ = static_cast<int>(yi_);
1781 
1782  const float t = yi_ - y_;
1783  const float s = xi_ - x_;
1784 
1785  if (y_ < static_cast<int>(src.getHeight()) - 1 && x_ < static_cast<int>(src.getWidth()) - 1) {
1786  const vpRGBa val00 = src[y_][x_];
1787  const vpRGBa val01 = src[y_][x_ + 1];
1788  const vpRGBa val10 = src[y_ + 1][x_];
1789  const vpRGBa val11 = src[y_ + 1][x_ + 1];
1790  const float colR0 = lerp(val00.R, val01.R, s);
1791  const float colR1 = lerp(val10.R, val11.R, s);
1792  const float interpR = lerp(colR0, colR1, t);
1793 
1794  const float colG0 = lerp(val00.G, val01.G, s);
1795  const float colG1 = lerp(val10.G, val11.G, s);
1796  const float interpG = lerp(colG0, colG1, t);
1797 
1798  const float colB0 = lerp(val00.B, val01.B, s);
1799  const float colB1 = lerp(val10.B, val11.B, s);
1800  const float interpB = lerp(colB0, colB1, t);
1801 
1802  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR),
1803  vpMath::saturate<unsigned char>(interpG),
1804  vpMath::saturate<unsigned char>(interpB),
1805  255);
1806  } else if (y_ < static_cast<int>(src.getHeight()) - 1) {
1807  const vpRGBa val00 = src[y_][x_];
1808  const vpRGBa val10 = src[y_ + 1][x_];
1809  const float interpR = lerp(val00.R, val10.R, t);
1810  const float interpG = lerp(val00.G, val10.G, t);
1811  const float interpB = lerp(val00.B, val10.B, t);
1812 
1813  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR),
1814  vpMath::saturate<unsigned char>(interpG),
1815  vpMath::saturate<unsigned char>(interpB),
1816  255);
1817  } else if (x_ < static_cast<int>(src.getWidth()) - 1) {
1818  const vpRGBa val00 = src[y_][x_];
1819  const vpRGBa val01 = src[y_][x_ + 1];
1820  const float interpR = lerp(val00.R, val01.R, s);
1821  const float interpG = lerp(val00.G, val01.G, s);
1822  const float interpB = lerp(val00.B, val01.B, s);
1823 
1824  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR),
1825  vpMath::saturate<unsigned char>(interpG),
1826  vpMath::saturate<unsigned char>(interpB),
1827  255);
1828  } else {
1829  dst[i][j] = src[y_][x_];
1830  }
1831  }
1832 
1833  xi += a0_i64;
1834  yi += a3_i64;
1835  wi += a6_i64;
1836  }
1837 
1838  a2_i64 += a1_i64;
1839  a5_i64 += a4_i64;
1840  a8_i64 += a7_i64;
1841  }
1842  }
1843  } else {
1844  double a0 = T[0][0]; double a1 = T[0][1]; double a2 = T[0][2];
1845  double a3 = T[1][0]; double a4 = T[1][1]; double a5 = T[1][2];
1846  double a6 = affine ? 0.0 : T[2][0];
1847  double a7 = affine ? 0.0 : T[2][1];
1848  double a8 = affine ? 1.0 : T[2][2];
1849 
1850  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1851  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1852  double x = a0 * (centerCorner ? j + 0.5 : j) + a1 * (centerCorner ? i + 0.5 : i) + a2;
1853  double y = a3 * (centerCorner ? j + 0.5 : j) + a4 * (centerCorner ? i + 0.5 : i) + a5;
1854  double w = a6 * (centerCorner ? j + 0.5 : j) + a7 * (centerCorner ? i + 0.5 : i) + a8;
1855 
1856  x = x / w - (centerCorner ? 0.5 : 0);
1857  y = y / w - (centerCorner ? 0.5 : 0);
1858 
1859  int x_lower = static_cast<int>(x);
1860  int y_lower = static_cast<int>(y);
1861 
1862  if (y_lower >= static_cast<int>(src.getHeight()) || x_lower >= static_cast<int>(src.getWidth()) ||
1863  y < 0 || x < 0) {
1864  continue;
1865  }
1866 
1867  double s = x - x_lower;
1868  double t = y - y_lower;
1869 
1870  if (y_lower < static_cast<int>(src.getHeight())-1 && x_lower < static_cast<int>(src.getWidth())-1) {
1871  const vpRGBa val00 = src[y_lower][x_lower];
1872  const vpRGBa val01 = src[y_lower][x_lower +1];
1873  const vpRGBa val10 = src[y_lower +1][x_lower];
1874  const vpRGBa val11 = src[y_lower +1][x_lower +1];
1875  const double colR0 = lerp(val00.R, val01.R, s);
1876  const double colR1 = lerp(val10.R, val11.R, s);
1877  const double interpR = lerp(colR0, colR1, t);
1878 
1879  const double colG0 = lerp(val00.G, val01.G, s);
1880  const double colG1 = lerp(val10.G, val11.G, s);
1881  const double interpG = lerp(colG0, colG1, t);
1882 
1883  const double colB0 = lerp(val00.B, val01.B, s);
1884  const double colB1 = lerp(val10.B, val11.B, s);
1885  const double interpB = lerp(colB0, colB1, t);
1886 
1887  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR),
1888  vpMath::saturate<unsigned char>(interpG),
1889  vpMath::saturate<unsigned char>(interpB),
1890  255);
1891  } else if (y_lower < static_cast<int>(src.getHeight())-1) {
1892  const vpRGBa val00 = src[y_lower][x_lower];
1893  const vpRGBa val10 = src[y_lower +1][x_lower];
1894  const double interpR = lerp(val00.R, val10.R, t);
1895  const double interpG = lerp(val00.G, val10.G, t);
1896  const double interpB = lerp(val00.B, val10.B, t);
1897 
1898  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR),
1899  vpMath::saturate<unsigned char>(interpG),
1900  vpMath::saturate<unsigned char>(interpB),
1901  255);
1902  } else if (x_lower < static_cast<int>(src.getWidth())-1) {
1903  const vpRGBa val00 = src[y_lower][x_lower];
1904  const vpRGBa val01 = src[y_lower][x_lower +1];
1905  const double interpR = lerp(val00.R, val01.R, s);
1906  const double interpG = lerp(val00.G, val01.G, s);
1907  const double interpB = lerp(val00.B, val01.B, s);
1908 
1909  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR),
1910  vpMath::saturate<unsigned char>(interpG),
1911  vpMath::saturate<unsigned char>(interpB),
1912  255);
1913  } else {
1914  dst[i][j] = src[y_lower][x_lower];
1915  }
1916  }
1917  }
1918  }
1919 }
1920 
1921 #endif
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
void performLut(const Type(&lut)[256], unsigned int nbThreads=1)
Definition: vpImage.h:1731
double get_i() const
Definition: vpImagePoint.h:204
double getTop() const
Definition: vpRect.h:192
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:879
unsigned char B
Blue component.
Definition: vpRGBa.h:150
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:115
Type * bitmap
points toward the bitmap
Definition: vpImage.h:141
vpMatrix inverseByLU() const
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of a generic 2D array used as base class for matrices and vectors. ...
Definition: vpArray2D.h:131
unsigned char G
Green component.
Definition: vpRGBa.h:149
Definition: vpRGBa.h:66
unsigned int getCols() const
Definition: vpArray2D.h:279
double getWidth() const
Definition: vpRect.h:227
static bool nul(double x, double s=0.001)
Definition: vpMath.h:287
static double sqr(double x)
Definition: vpMath.h:114
double get_j() const
Definition: vpImagePoint.h:215
Generic class defining intrinsic camera parameters.
static void binarise(vpImage< Type > &I, Type threshold1, Type threshold2, Type value1, Type value2, Type value3, bool useLUT=true)
Definition: vpImageTools.h:452
static void flip(const vpImage< Type > &I, vpImage< Type > &newI)
Definition: vpImageTools.h:806
double getLeft() const
Definition: vpRect.h:173
static void undistort(const vpImage< Type > &I, const vpCameraParameters &cam, vpImage< Type > &newI, unsigned int nThreads=2)
Definition: vpImageTools.h:641
static int round(double x)
Definition: vpMath.h:241
Various image tools; sub-image extraction, modification of the look up table, binarisation...
Definition: vpImageTools.h:79
static void warpImage(const vpImage< Type > &src, const vpMatrix &T, vpImage< Type > &dst, const vpImageInterpolationType &interpolation=INTERPOLATION_NEAREST, bool fixedPointArithmetic=true, bool pixelCenter=false)
unsigned int getHeight() const
Definition: vpImage.h:186
static void crop(const vpImage< Type > &I, double roi_top, double roi_left, unsigned int roi_height, unsigned int roi_width, vpImage< Type > &crop, unsigned int v_scale=1, unsigned int h_scale=1)
Definition: vpImageTools.h:296
unsigned int getSize() const
Definition: vpImage.h:225
unsigned char R
Red component.
Definition: vpRGBa.h:148
static vp_deprecated void createSubImage(const vpImage< Type > &I, unsigned int i_sub, unsigned int j_sub, unsigned int nrow_sub, unsigned int ncol_sub, vpImage< Type > &S)
double getHeight() const
Definition: vpRect.h:166
double get_kud() const
Defines a rectangle in the plane.
Definition: vpRect.h:78
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
unsigned int getWidth() const
Definition: vpImage.h:244
Definition of the vpImage class member functions.
Definition: vpImage.h:124
Defines an oriented rectangle in the plane.
static void resize(const vpImage< Type > &I, vpImage< Type > &Ires, unsigned int width, unsigned int height, const vpImageInterpolationType &method=INTERPOLATION_NEAREST, unsigned int nThreads=0)