Visual Servoing Platform  version 3.3.1 under development (2020-05-29)
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 undistort(const vpImage<Type> &I, vpArray2D<int> mapU, vpArray2D<int> mapV, vpArray2D<float> mapDu,
169  vpArray2D<float> mapDv, vpImage<Type> &newI);
170 
171  template <class Type>
172  static void warpImage(const vpImage<Type> &src, const vpMatrix &T, vpImage<Type> &dst,
173  const vpImageInterpolationType &interpolation=INTERPOLATION_NEAREST,
174  bool fixedPointArithmetic=true, bool pixelCenter=false);
175 
176 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
177 
181  template <class Type>
182  vp_deprecated static void createSubImage(const vpImage<Type> &I, unsigned int i_sub, unsigned int j_sub,
183  unsigned int nrow_sub, unsigned int ncol_sub, vpImage<Type> &S);
184 
185  template <class Type>
186  vp_deprecated static void createSubImage(const vpImage<Type> &I, const vpRect &rect, vpImage<Type> &S);
188 #endif
189 
190 private:
191  // Cubic interpolation
192  static float cubicHermite(const float A, const float B, const float C, const float D, const float t);
193 
194  template <class Type> static Type getPixelClamped(const vpImage<Type> &I, float u, float v);
195 
196  static int coordCast(double x);
197 
198  // Linear interpolation
199  static double lerp(double A, double B, double t);
200  static float lerp(float A, float B, float t);
201  static int64_t lerp2(int64_t A, int64_t B, int64_t t, int64_t t_1);
202 
203  static double normalizedCorrelation(const vpImage<double> &I1, const vpImage<double> &I2, const vpImage<double> &II,
204  const vpImage<double> &IIsq, const vpImage<double> &II_tpl,
205  const vpImage<double> &IIsq_tpl, unsigned int i0, unsigned int j0);
206 
207  template <class Type>
208  static void resizeBicubic(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 resizeBilinear(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int i, unsigned int j,
213  float u, float v, float xFrac, float yFrac);
214 
215  template <class Type>
216  static void resizeNearest(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int i, unsigned int j,
217  float u, float v);
218 
219  template <class Type>
220  static void warpNN(const vpImage<Type> &src, const vpMatrix &T, vpImage<Type> &dst, bool affine, bool centerCorner, bool fixedPoint);
221 
222  template <class Type>
223  static void warpLinear(const vpImage<Type> &src, const vpMatrix &T, vpImage<Type> &dst, bool affine, bool centerCorner, bool fixedPoint);
224 
225  static bool checkFixedPoint(unsigned int x, unsigned int y, const vpMatrix &T, bool affine);
226 };
227 
228 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
229 
248 template <class Type>
249 void vpImageTools::createSubImage(const vpImage<Type> &I, unsigned int roi_top, unsigned int roi_left,
250  unsigned int roi_height, unsigned int roi_width, vpImage<Type> &crop)
251 {
252  vpImageTools::crop(I, roi_top, roi_left, roi_height, roi_width, crop);
253 }
254 
270 template <class Type> void vpImageTools::createSubImage(const vpImage<Type> &I, const vpRect &roi, vpImage<Type> &crop)
271 {
272  vpImageTools::crop(I, roi, crop);
273 }
274 
275 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
276 
299 template <class Type>
300 void vpImageTools::crop(const vpImage<Type> &I, double roi_top, double roi_left, unsigned int roi_height,
301  unsigned int roi_width, vpImage<Type> &crop, unsigned int v_scale, unsigned int h_scale)
302 {
303  int i_min = (std::max)((int)(ceil(roi_top / v_scale)), 0);
304  int j_min = (std::max)((int)(ceil(roi_left / h_scale)), 0);
305  int i_max = (std::min)((int)(ceil((roi_top + roi_height)) / v_scale), (int)(I.getHeight() / v_scale));
306  int j_max = (std::min)((int)(ceil((roi_left + roi_width) / h_scale)), (int)(I.getWidth() / h_scale));
307 
308  unsigned int i_min_u = (unsigned int)i_min;
309  unsigned int j_min_u = (unsigned int)j_min;
310 
311  unsigned int r_width = (unsigned int)(j_max - j_min);
312  unsigned int r_height = (unsigned int)(i_max - i_min);
313 
314  crop.resize(r_height, r_width);
315 
316  if (v_scale == 1 && h_scale == 1) {
317  for (unsigned int i = 0; i < r_height; i++) {
318  void *src = (void *)(I[i + i_min_u] + j_min_u);
319  void *dst = (void *)crop[i];
320  memcpy(dst, src, r_width * sizeof(Type));
321  }
322  } else if (h_scale == 1) {
323  for (unsigned int i = 0; i < r_height; i++) {
324  void *src = (void *)(I[(i + i_min_u) * v_scale] + j_min_u);
325  void *dst = (void *)crop[i];
326  memcpy(dst, src, r_width * sizeof(Type));
327  }
328  } else {
329  for (unsigned int i = 0; i < r_height; i++) {
330  for (unsigned int j = 0; j < r_width; j++) {
331  crop[i][j] = I[(i + i_min_u) * v_scale][(j + j_min_u) * h_scale];
332  }
333  }
334  }
335 }
336 
355 template <class Type>
356 void vpImageTools::crop(const vpImage<Type> &I, const vpImagePoint &topLeft, unsigned int roi_height,
357  unsigned int roi_width, vpImage<Type> &crop, unsigned int v_scale, unsigned int h_scale)
358 {
359  vpImageTools::crop(I, topLeft.get_i(), topLeft.get_j(), roi_height, roi_width, crop, v_scale, h_scale);
360 }
361 
379 template <class Type>
380 void vpImageTools::crop(const vpImage<Type> &I, const vpRect &roi, vpImage<Type> &crop, unsigned int v_scale,
381  unsigned int h_scale)
382 {
383  vpImageTools::crop(I, roi.getTop(), roi.getLeft(), (unsigned int)roi.getHeight(), (unsigned int)roi.getWidth(), crop,
384  v_scale, h_scale);
385 }
386 
404 template <class Type>
405 void vpImageTools::crop(const unsigned char *bitmap, unsigned int width, unsigned int height, const vpRect &roi,
406  vpImage<Type> &crop, unsigned int v_scale, unsigned int h_scale)
407 {
408  int i_min = (std::max)((int)(ceil(roi.getTop() / v_scale)), 0);
409  int j_min = (std::max)((int)(ceil(roi.getLeft() / h_scale)), 0);
410  int i_max = (std::min)((int)(ceil((roi.getTop() + roi.getHeight()) / v_scale)), (int)(height / v_scale));
411  int j_max = (std::min)((int)(ceil((roi.getLeft() + roi.getWidth()) / h_scale)), (int)(width / h_scale));
412 
413  unsigned int i_min_u = (unsigned int)i_min;
414  unsigned int j_min_u = (unsigned int)j_min;
415 
416  unsigned int r_width = (unsigned int)(j_max - j_min);
417  unsigned int r_height = (unsigned int)(i_max - i_min);
418 
419  crop.resize(r_height, r_width);
420 
421  if (v_scale == 1 && h_scale == 1) {
422  for (unsigned int i = 0; i < r_height; i++) {
423  void *src = (void *)(bitmap + ((i + i_min_u) * width + j_min_u) * sizeof(Type));
424  void *dst = (void *)crop[i];
425  memcpy(dst, src, r_width * sizeof(Type));
426  }
427  } else if (h_scale == 1) {
428  for (unsigned int i = 0; i < r_height; i++) {
429  void *src = (void *)(bitmap + ((i + i_min_u) * width * v_scale + j_min_u) * sizeof(Type));
430  void *dst = (void *)crop[i];
431  memcpy(dst, src, r_width * sizeof(Type));
432  }
433  } else {
434  for (unsigned int i = 0; i < r_height; i++) {
435  unsigned int i_src = (i + i_min_u) * width * v_scale + j_min_u * h_scale;
436  for (unsigned int j = 0; j < r_width; j++) {
437  void *src = (void *)(bitmap + (i_src + j * h_scale) * sizeof(Type));
438  void *dst = (void *)&crop[i][j];
439  memcpy(dst, src, sizeof(Type));
440  }
441  }
442  }
443 }
444 
455 template <class Type>
456 inline void vpImageTools::binarise(vpImage<Type> &I, Type threshold1, Type threshold2, Type value1, Type value2,
457  Type value3, bool useLUT)
458 {
459  if (useLUT) {
460  std::cerr << "LUT not available for this type ! Will use the iteration method." << std::endl;
461  }
462 
463  Type v;
464  Type *p = I.bitmap;
465  Type *pend = I.bitmap + I.getWidth() * I.getHeight();
466  for (; p < pend; p++) {
467  v = *p;
468  if (v < threshold1)
469  *p = value1;
470  else if (v > threshold2)
471  *p = value3;
472  else
473  *p = value2;
474  }
475 }
476 
487 template <>
488 inline void vpImageTools::binarise(vpImage<unsigned char> &I, unsigned char threshold1, unsigned char threshold2,
489  unsigned char value1, unsigned char value2, unsigned char value3, bool useLUT)
490 {
491  if (useLUT) {
492  // Construct the LUT
493  unsigned char lut[256];
494  for (unsigned int i = 0; i < 256; i++) {
495  lut[i] = i < threshold1 ? value1 : (i > threshold2 ? value3 : value2);
496  }
497 
498  I.performLut(lut);
499  } else {
500  unsigned char *p = I.bitmap;
501  unsigned char *pend = I.bitmap + I.getWidth() * I.getHeight();
502  for (; p < pend; p++) {
503  unsigned char v = *p;
504  if (v < threshold1)
505  *p = value1;
506  else if (v > threshold2)
507  *p = value3;
508  else
509  *p = value2;
510  }
511  }
512 }
513 
514 #ifdef VISP_HAVE_PTHREAD
515 
516 #ifndef DOXYGEN_SHOULD_SKIP_THIS
517 template <class Type> class vpUndistortInternalType
518 {
519 public:
520  Type *src;
521  Type *dst;
522  unsigned int width;
523  unsigned int height;
524  vpCameraParameters cam;
525  unsigned int nthreads;
526  unsigned int threadid;
527 
528 public:
529  vpUndistortInternalType() : src(NULL), dst(NULL), width(0), height(0), cam(), nthreads(0), threadid(0) {}
530 
531  vpUndistortInternalType(const vpUndistortInternalType<Type> &u) { *this = u; }
532  vpUndistortInternalType &operator=(const vpUndistortInternalType<Type> &u)
533  {
534  src = u.src;
535  dst = u.dst;
536  width = u.width;
537  height = u.height;
538  cam = u.cam;
539  nthreads = u.nthreads;
540  threadid = u.threadid;
541 
542  return *this;
543  }
544 
545  static void *vpUndistort_threaded(void *arg);
546 };
547 
548 template <class Type> void *vpUndistortInternalType<Type>::vpUndistort_threaded(void *arg)
549 {
550  vpUndistortInternalType<Type> *undistortSharedData = static_cast<vpUndistortInternalType<Type> *>(arg);
551  int offset = (int)undistortSharedData->threadid;
552  int width = (int)undistortSharedData->width;
553  int height = (int)undistortSharedData->height;
554  int nthreads = (int)undistortSharedData->nthreads;
555 
556  double u0 = undistortSharedData->cam.get_u0();
557  double v0 = undistortSharedData->cam.get_v0();
558  double px = undistortSharedData->cam.get_px();
559  double py = undistortSharedData->cam.get_py();
560  double kud = undistortSharedData->cam.get_kud();
561 
562  double invpx = 1.0 / px;
563  double invpy = 1.0 / py;
564 
565  double kud_px2 = kud * invpx * invpx;
566  double kud_py2 = kud * invpy * invpy;
567 
568  Type *dst = undistortSharedData->dst + (height / nthreads * offset) * width;
569  Type *src = undistortSharedData->src;
570 
571  for (double v = height / nthreads * offset; v < height / nthreads * (offset + 1); v++) {
572  double deltav = v - v0;
573  // double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy));
574  double fr1 = 1.0 + kud_py2 * deltav * deltav;
575 
576  for (double u = 0; u < width; u++) {
577  // computation of u,v : corresponding pixel coordinates in I.
578  double deltau = u - u0;
579  // double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx));
580  double fr2 = fr1 + kud_px2 * deltau * deltau;
581 
582  double u_double = deltau * fr2 + u0;
583  double v_double = deltav * fr2 + v0;
584 
585  // computation of the bilinear interpolation
586 
587  // declarations
588  int u_round = (int)(u_double);
589  int v_round = (int)(v_double);
590  if (u_round < 0.f)
591  u_round = -1;
592  if (v_round < 0.f)
593  v_round = -1;
594  double du_double = (u_double) - (double)u_round;
595  double dv_double = (v_double) - (double)v_round;
596  Type v01;
597  Type v23;
598  if ((0 <= u_round) && (0 <= v_round) && (u_round < ((width)-1)) && (v_round < ((height)-1))) {
599  // process interpolation
600  const Type *_mp = &src[v_round * width + u_round];
601  v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
602  _mp += width;
603  v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
604  *dst = (Type)(v01 + ((v23 - v01) * dv_double));
605  } else {
606  *dst = 0;
607  }
608  dst++;
609  }
610  }
611 
612  pthread_exit((void *)0);
613  return NULL;
614 }
615 #endif // DOXYGEN_SHOULD_SKIP_THIS
616 #endif // VISP_HAVE_PTHREAD
617 
644 template <class Type>
646  unsigned int nThreads)
647 {
648 #ifdef VISP_HAVE_PTHREAD
649  //
650  // Optimized version using pthreads
651  //
652  unsigned int width = I.getWidth();
653  unsigned int height = I.getHeight();
654 
655  undistI.resize(height, width);
656 
657  double kud = cam.get_kud();
658 
659  // if (kud == 0) {
660  if (std::fabs(kud) <= std::numeric_limits<double>::epsilon()) {
661  // There is no need to undistort the image
662  undistI = I;
663  return;
664  }
665 
666  unsigned int nthreads = nThreads;
667  pthread_attr_t attr;
668  pthread_t *callThd = new pthread_t[nthreads];
669  pthread_attr_init(&attr);
670  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
671 
672  vpUndistortInternalType<Type> *undistortSharedData;
673  undistortSharedData = new vpUndistortInternalType<Type>[nthreads];
674 
675  for (unsigned int i = 0; i < nthreads; i++) {
676  // Each thread works on a different set of data.
677  // vpTRACE("create thread %d", i);
678  undistortSharedData[i].src = I.bitmap;
679  undistortSharedData[i].dst = undistI.bitmap;
680  undistortSharedData[i].width = I.getWidth();
681  undistortSharedData[i].height = I.getHeight();
682  undistortSharedData[i].cam = cam;
683  undistortSharedData[i].nthreads = nthreads;
684  undistortSharedData[i].threadid = i;
685  pthread_create(&callThd[i], &attr, &vpUndistortInternalType<Type>::vpUndistort_threaded, &undistortSharedData[i]);
686  }
687  pthread_attr_destroy(&attr);
688  /* Wait on the other threads */
689 
690  for (unsigned int i = 0; i < nthreads; i++) {
691  // vpTRACE("join thread %d", i);
692  pthread_join(callThd[i], NULL);
693  }
694 
695  delete[] callThd;
696  delete[] undistortSharedData;
697 #else // VISP_HAVE_PTHREAD
698  (void)nThreads;
699  //
700  // optimized version without pthreads
701  //
702  unsigned int width = I.getWidth();
703  unsigned int height = I.getHeight();
704 
705  undistI.resize(height, width);
706 
707  double u0 = cam.get_u0();
708  double v0 = cam.get_v0();
709  double px = cam.get_px();
710  double py = cam.get_py();
711  double kud = cam.get_kud();
712 
713  // if (kud == 0) {
714  if (std::fabs(kud) <= std::numeric_limits<double>::epsilon()) {
715  // There is no need to undistort the image
716  undistI = I;
717  return;
718  }
719 
720  double invpx = 1.0 / px;
721  double invpy = 1.0 / py;
722 
723  double kud_px2 = kud * invpx * invpx;
724  double kud_py2 = kud * invpy * invpy;
725 
726  Type *dst = undistI.bitmap;
727  for (double v = 0; v < height; v++) {
728  double deltav = v - v0;
729  // double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy));
730  double fr1 = 1.0 + kud_py2 * deltav * deltav;
731 
732  for (double u = 0; u < width; u++) {
733  // computation of u,v : corresponding pixel coordinates in I.
734  double deltau = u - u0;
735  // double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx));
736  double fr2 = fr1 + kud_px2 * deltau * deltau;
737 
738  double u_double = deltau * fr2 + u0;
739  double v_double = deltav * fr2 + v0;
740 
741  // printf("[%g][%g] %g %g : ", u, v, u_double, v_double );
742 
743  // computation of the bilinear interpolation
744 
745  // declarations
746  int u_round = (int)(u_double);
747  int v_round = (int)(v_double);
748  if (u_round < 0.f)
749  u_round = -1;
750  if (v_round < 0.f)
751  v_round = -1;
752  double du_double = (u_double) - (double)u_round;
753  double dv_double = (v_double) - (double)v_round;
754  Type v01;
755  Type v23;
756  if ((0 <= u_round) && (0 <= v_round) && (u_round < (((int)width) - 1)) && (v_round < (((int)height) - 1))) {
757  // process interpolation
758  const Type *_mp = &I[(unsigned int)v_round][(unsigned int)u_round];
759  v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
760  _mp += width;
761  v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
762  *dst = (Type)(v01 + ((v23 - v01) * dv_double));
763  // printf("R %d G %d B %d\n", dst->R, dst->G, dst->B);
764  } else {
765  *dst = 0;
766  }
767  dst++;
768  }
769  }
770 #endif // VISP_HAVE_PTHREAD
771 
772 #if 0
773  // non optimized version
774  int width = I.getWidth();
775  int height = I.getHeight();
776 
777  undistI.resize(height,width);
778 
779  double u0 = cam.get_u0();
780  double v0 = cam.get_v0();
781  double px = cam.get_px();
782  double py = cam.get_py();
783  double kd = cam.get_kud();
784 
785  if (kd == 0) {
786  // There is no need to undistort the image
787  undistI = I;
788  return;
789  }
790 
791  for(int v = 0 ; v < height; v++){
792  for(int u = 0; u < height; u++){
793  double r2 = vpMath::sqr(((double)u - u0)/px) +
794  vpMath::sqr(((double)v-v0)/py);
795  double u_double = ((double)u - u0)*(1.0+kd*r2) + u0;
796  double v_double = ((double)v - v0)*(1.0+kd*r2) + v0;
797  undistI[v][u] = I.getPixelBI((float)u_double,(float)v_double);
798  }
799  }
800 #endif
801 }
802 
817 template <class Type>
819  vpArray2D<float> mapDv, vpImage<Type> &newI)
820 {
821  remap(I, mapU, mapV, mapDu, mapDv, newI);
822 }
823 
831 template <class Type> void vpImageTools::flip(const vpImage<Type> &I, vpImage<Type> &newI)
832 {
833  unsigned int height = 0, width = 0;
834 
835  height = I.getHeight();
836  width = I.getWidth();
837  newI.resize(height, width);
838 
839  for (unsigned int i = 0; i < height; i++) {
840  memcpy(newI.bitmap + i * width, I.bitmap + (height - 1 - i) * width, width * sizeof(Type));
841  }
842 }
843 
875 template <class Type> void vpImageTools::flip(vpImage<Type> &I)
876 {
877  unsigned int height = 0, width = 0;
878  unsigned int i = 0;
879  vpImage<Type> Ibuf;
880 
881  height = I.getHeight();
882  width = I.getWidth();
883  Ibuf.resize(1, width);
884 
885  for (i = 0; i < height / 2; i++) {
886  memcpy(Ibuf.bitmap, I.bitmap + i * width, width * sizeof(Type));
887 
888  memcpy(I.bitmap + i * width, I.bitmap + (height - 1 - i) * width, width * sizeof(Type));
889  memcpy(I.bitmap + (height - 1 - i) * width, Ibuf.bitmap, width * sizeof(Type));
890  }
891 }
892 
893 template <class Type> Type vpImageTools::getPixelClamped(const vpImage<Type> &I, float u, float v)
894 {
895  unsigned int i, j;
896  if (u < 0.f)
897  j = 0;
898  else if (u > static_cast<float>(I.getWidth()) - 1.f)
899  j = I.getWidth() - 1;
900  else
901  j = static_cast<unsigned int>(u);
902 
903  if (v < 0.f)
904  i = 0;
905  else if (v > static_cast<float>(I.getHeight()) - 1.f)
906  i = I.getHeight() - 1;
907  else
908  i = static_cast<unsigned int>(v);
909 
910  return I[i][j];
911 }
912 
913 // Reference:
914 // http://blog.demofox.org/2015/08/15/resizing-images-with-bicubic-interpolation/
915 template <class Type>
916 void vpImageTools::resizeBicubic(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int i,
917  unsigned int j, float u, float v, float xFrac, float yFrac)
918 {
919  // 1st row
920  Type p00 = getPixelClamped(I, u - 1, v - 1);
921  Type p01 = getPixelClamped(I, u + 0, v - 1);
922  Type p02 = getPixelClamped(I, u + 1, v - 1);
923  Type p03 = getPixelClamped(I, u + 2, v - 1);
924 
925  // 2nd row
926  Type p10 = getPixelClamped(I, u - 1, v + 0);
927  Type p11 = getPixelClamped(I, u + 0, v + 0);
928  Type p12 = getPixelClamped(I, u + 1, v + 0);
929  Type p13 = getPixelClamped(I, u + 2, v + 0);
930 
931  // 3rd row
932  Type p20 = getPixelClamped(I, u - 1, v + 1);
933  Type p21 = getPixelClamped(I, u + 0, v + 1);
934  Type p22 = getPixelClamped(I, u + 1, v + 1);
935  Type p23 = getPixelClamped(I, u + 2, v + 1);
936 
937  // 4th row
938  Type p30 = getPixelClamped(I, u - 1, v + 2);
939  Type p31 = getPixelClamped(I, u + 0, v + 2);
940  Type p32 = getPixelClamped(I, u + 1, v + 2);
941  Type p33 = getPixelClamped(I, u + 2, v + 2);
942 
943  float col0 = cubicHermite(p00, p01, p02, p03, xFrac);
944  float col1 = cubicHermite(p10, p11, p12, p13, xFrac);
945  float col2 = cubicHermite(p20, p21, p22, p23, xFrac);
946  float col3 = cubicHermite(p30, p31, p32, p33, xFrac);
947  float value = cubicHermite(col0, col1, col2, col3, yFrac);
948  Ires[i][j] = vpMath::saturate<Type>(value);
949 }
950 
951 template <>
952 inline void vpImageTools::resizeBicubic(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &Ires, unsigned int i,
953  unsigned int j, float u, float v, float xFrac, float yFrac)
954 {
955  // 1st row
956  vpRGBa p00 = getPixelClamped(I, u - 1, v - 1);
957  vpRGBa p01 = getPixelClamped(I, u + 0, v - 1);
958  vpRGBa p02 = getPixelClamped(I, u + 1, v - 1);
959  vpRGBa p03 = getPixelClamped(I, u + 2, v - 1);
960 
961  // 2nd row
962  vpRGBa p10 = getPixelClamped(I, u - 1, v + 0);
963  vpRGBa p11 = getPixelClamped(I, u + 0, v + 0);
964  vpRGBa p12 = getPixelClamped(I, u + 1, v + 0);
965  vpRGBa p13 = getPixelClamped(I, u + 2, v + 0);
966 
967  // 3rd row
968  vpRGBa p20 = getPixelClamped(I, u - 1, v + 1);
969  vpRGBa p21 = getPixelClamped(I, u + 0, v + 1);
970  vpRGBa p22 = getPixelClamped(I, u + 1, v + 1);
971  vpRGBa p23 = getPixelClamped(I, u + 2, v + 1);
972 
973  // 4th row
974  vpRGBa p30 = getPixelClamped(I, u - 1, v + 2);
975  vpRGBa p31 = getPixelClamped(I, u + 0, v + 2);
976  vpRGBa p32 = getPixelClamped(I, u + 1, v + 2);
977  vpRGBa p33 = getPixelClamped(I, u + 2, v + 2);
978 
979  for (int c = 0; c < 3; c++) {
980  float col0 = cubicHermite(static_cast<float>(reinterpret_cast<unsigned char *>(&p00)[c]),
981  static_cast<float>(reinterpret_cast<unsigned char *>(&p01)[c]),
982  static_cast<float>(reinterpret_cast<unsigned char *>(&p02)[c]),
983  static_cast<float>(reinterpret_cast<unsigned char *>(&p03)[c]), xFrac);
984  float col1 = cubicHermite(static_cast<float>(reinterpret_cast<unsigned char *>(&p10)[c]),
985  static_cast<float>(reinterpret_cast<unsigned char *>(&p11)[c]),
986  static_cast<float>(reinterpret_cast<unsigned char *>(&p12)[c]),
987  static_cast<float>(reinterpret_cast<unsigned char *>(&p13)[c]), xFrac);
988  float col2 = cubicHermite(static_cast<float>(reinterpret_cast<unsigned char *>(&p20)[c]),
989  static_cast<float>(reinterpret_cast<unsigned char *>(&p21)[c]),
990  static_cast<float>(reinterpret_cast<unsigned char *>(&p22)[c]),
991  static_cast<float>(reinterpret_cast<unsigned char *>(&p23)[c]), xFrac);
992  float col3 = cubicHermite(static_cast<float>(reinterpret_cast<unsigned char *>(&p30)[c]),
993  static_cast<float>(reinterpret_cast<unsigned char *>(&p31)[c]),
994  static_cast<float>(reinterpret_cast<unsigned char *>(&p32)[c]),
995  static_cast<float>(reinterpret_cast<unsigned char *>(&p33)[c]), xFrac);
996  float value = cubicHermite(col0, col1, col2, col3, yFrac);
997 
998  reinterpret_cast<unsigned char *>(&Ires[i][j])[c] = vpMath::saturate<unsigned char>(value);
999  }
1000 }
1001 
1002 template <class Type>
1003 void vpImageTools::resizeBilinear(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int i,
1004  unsigned int j, float u, float v, float xFrac, float yFrac)
1005 {
1006  unsigned int u0 = static_cast<unsigned int>(u);
1007  unsigned int v0 = static_cast<unsigned int>(v);
1008 
1009  unsigned int u1 = (std::min)(I.getWidth() - 1, static_cast<unsigned int>(u) + 1);
1010  unsigned int v1 = v0;
1011 
1012  unsigned int u2 = u0;
1013  unsigned int v2 = (std::min)(I.getHeight() - 1, static_cast<unsigned int>(v) + 1);
1014 
1015  unsigned int u3 = u1;
1016  unsigned int v3 = v2;
1017 
1018  float col0 = lerp(I[v0][u0], I[v1][u1], xFrac);
1019  float col1 = lerp(I[v2][u2], I[v3][u3], xFrac);
1020  float value = lerp(col0, col1, yFrac);
1021 
1022  Ires[i][j] = vpMath::saturate<Type>(value);
1023 }
1024 
1025 template <>
1026 inline void vpImageTools::resizeBilinear(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &Ires, unsigned int i,
1027  unsigned int j, float u, float v, float xFrac, float yFrac)
1028 {
1029  unsigned int u0 = static_cast<unsigned int>(u);
1030  unsigned int v0 = static_cast<unsigned int>(v);
1031 
1032  unsigned int u1 = (std::min)(I.getWidth() - 1, static_cast<unsigned int>(u) + 1);
1033  unsigned int v1 = v0;
1034 
1035  unsigned int u2 = u0;
1036  unsigned int v2 = (std::min)(I.getHeight() - 1, static_cast<unsigned int>(v) + 1);
1037 
1038  unsigned int u3 = (std::min)(I.getWidth() - 1, static_cast<unsigned int>(u) + 1);
1039  unsigned int v3 = (std::min)(I.getHeight() - 1, static_cast<unsigned int>(v) + 1);
1040 
1041  for (int c = 0; c < 3; c++) {
1042  float col0 = lerp(static_cast<float>(reinterpret_cast<const unsigned char *>(&I[v0][u0])[c]),
1043  static_cast<float>(reinterpret_cast<const unsigned char *>(&I[v1][u1])[c]), xFrac);
1044  float col1 = lerp(static_cast<float>(reinterpret_cast<const unsigned char *>(&I[v2][u2])[c]),
1045  static_cast<float>(reinterpret_cast<const unsigned char *>(&I[v3][u3])[c]), xFrac);
1046  float value = lerp(col0, col1, yFrac);
1047 
1048  reinterpret_cast<unsigned char *>(&Ires[i][j])[c] = vpMath::saturate<unsigned char>(value);
1049  }
1050 }
1051 
1052 template <class Type>
1053 void vpImageTools::resizeNearest(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int i,
1054  unsigned int j, float u, float v)
1055 {
1056  Ires[i][j] = getPixelClamped(I, u, v);
1057 }
1058 
1072 template <class Type>
1073 void vpImageTools::resize(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int width,
1074  unsigned int height, const vpImageInterpolationType &method,
1075  unsigned int nThreads)
1076 {
1077  Ires.resize(height, width);
1078 
1079  vpImageTools::resize(I, Ires, method, nThreads);
1080 }
1081 
1094 template <class Type>
1096  unsigned int
1097  #if defined _OPENMP
1098  nThreads
1099  #endif
1100  )
1101 {
1102  if (I.getWidth() < 2 || I.getHeight() < 2 || Ires.getWidth() < 2 || Ires.getHeight() < 2) {
1103  std::cerr << "Input or output image is too small!" << std::endl;
1104  return;
1105  }
1106 
1107  float scaleY = (I.getHeight() - 1) / static_cast<float>(Ires.getHeight() - 1);
1108  float scaleX = (I.getWidth() - 1) / static_cast<float>(Ires.getWidth() - 1);
1109 
1110  if (method == INTERPOLATION_NEAREST) {
1111  scaleY = I.getHeight() / static_cast<float>(Ires.getHeight() - 1);
1112  scaleX = I.getWidth() / static_cast<float>(Ires.getWidth() - 1);
1113  }
1114 
1115 #if defined _OPENMP
1116  if (nThreads > 0) {
1117  omp_set_num_threads(static_cast<int>(nThreads));
1118  }
1119  #pragma omp parallel for schedule(dynamic)
1120 #endif
1121  for (int i = 0; i < static_cast<int>(Ires.getHeight()); i++) {
1122  float v = i * scaleY;
1123  float yFrac = v - static_cast<int>(v);
1124 
1125  for (unsigned int j = 0; j < Ires.getWidth(); j++) {
1126  float u = j * scaleX;
1127  float xFrac = u - static_cast<int>(u);
1128 
1129  if (method == INTERPOLATION_NEAREST) {
1130  resizeNearest(I, Ires, static_cast<unsigned int>(i), j, u, v);
1131  } else if (method == INTERPOLATION_LINEAR) {
1132  resizeBilinear(I, Ires, static_cast<unsigned int>(i), j, u, v, xFrac, yFrac);
1133  } else if (method == INTERPOLATION_CUBIC) {
1134  resizeBicubic(I, Ires, static_cast<unsigned int>(i), j, u, v, xFrac, yFrac);
1135  }
1136  }
1137  }
1138 }
1139 
1140 template <> inline
1142  const vpImageInterpolationType &method, unsigned int
1143  #if defined _OPENMP
1144  nThreads
1145  #endif
1146  )
1147 {
1148  if (I.getWidth() < 2 || I.getHeight() < 2 || Ires.getWidth() < 2 || Ires.getHeight() < 2) {
1149  std::cerr << "Input or output image is too small!" << std::endl;
1150  return;
1151  }
1152 
1153  if (method == INTERPOLATION_NEAREST || method == INTERPOLATION_CUBIC) {
1154  float scaleY = (I.getHeight() - 1) / static_cast<float>(Ires.getHeight() - 1);
1155  float scaleX = (I.getWidth() - 1) / static_cast<float>(Ires.getWidth() - 1);
1156 
1157  if (method == INTERPOLATION_NEAREST) {
1158  scaleY = I.getHeight() / static_cast<float>(Ires.getHeight() - 1);
1159  scaleX = I.getWidth() / static_cast<float>(Ires.getWidth() - 1);
1160  }
1161 
1162  #if defined _OPENMP
1163  if (nThreads > 0) {
1164  omp_set_num_threads(static_cast<int>(nThreads));
1165  }
1166  #pragma omp parallel for schedule(dynamic)
1167  #endif
1168  for (int i = 0; i < static_cast<int>(Ires.getHeight()); i++) {
1169  float v = i * scaleY;
1170  float yFrac = v - static_cast<int>(v);
1171 
1172  for (unsigned int j = 0; j < Ires.getWidth(); j++) {
1173  float u = j * scaleX;
1174  float xFrac = u - static_cast<int>(u);
1175 
1176  if (method == INTERPOLATION_NEAREST) {
1177  resizeNearest(I, Ires, static_cast<unsigned int>(i), j, u, v);
1178  } else if (method == INTERPOLATION_CUBIC) {
1179  resizeBicubic(I, Ires, static_cast<unsigned int>(i), j, u, v, xFrac, yFrac);
1180  }
1181  }
1182  }
1183  } else if (method == INTERPOLATION_LINEAR) {
1184  const int32_t precision = 1 << 16;
1185  int64_t scaleY = static_cast<int64_t>((I.getHeight() - 1) / static_cast<float>(Ires.getHeight() - 1) * precision);
1186  int64_t scaleX = static_cast<int64_t>((I.getWidth() - 1) / static_cast<float>(Ires.getWidth() - 1) * precision);
1187 
1188 #if defined _OPENMP
1189  if (nThreads > 0) {
1190  omp_set_num_threads(static_cast<int>(nThreads));
1191  }
1192 #pragma omp parallel for schedule(dynamic)
1193 #endif
1194  for (int i = 0; i < static_cast<int>(Ires.getHeight()); i++) {
1195  int64_t v = i * scaleY;
1196  int64_t vround = v & (~0xFFFF);
1197  int64_t rratio = v - vround;
1198  int64_t y_ = v >> 16;
1199  int64_t rfrac = precision - rratio;
1200 
1201  for (unsigned int j = 0; j < Ires.getWidth(); j++) {
1202  int64_t u = j * scaleX;
1203  int64_t uround = u & (~0xFFFF);
1204  int64_t cratio = u - uround;
1205  int64_t x_ = u >> 16;
1206  int64_t cfrac = precision - cratio;
1207 
1208  if (y_ + 1 < static_cast<int64_t>(I.getHeight()) && x_ + 1 < static_cast<int64_t>(I.getWidth())) {
1209  uint16_t up = vpEndian::reinterpret_cast_uchar_to_uint16_LE(I.bitmap + y_ * I.getWidth() + x_);
1210  uint16_t down = vpEndian::reinterpret_cast_uchar_to_uint16_LE(I.bitmap + (y_ + 1) * I.getWidth() + x_);
1211 
1212  Ires[i][j] = static_cast<unsigned char>((((up & 0x00FF) * rfrac + (down & 0x00FF) * rratio) * cfrac +
1213  ((up >> 8) * rfrac + (down >> 8) * rratio) * cratio) >> 32);
1214  } else if (y_ + 1 < static_cast<int64_t>(I.getHeight())) {
1215  Ires[i][j] = static_cast<unsigned char>(((*(I.bitmap + y_ * I.getWidth() + x_)
1216  * rfrac + *(I.bitmap + (y_ + 1) * I.getWidth() + x_) * rratio)) >> 16);
1217  } else if (x_ + 1 < static_cast<int64_t>(I.getWidth())) {
1218  uint16_t up = vpEndian::reinterpret_cast_uchar_to_uint16_LE(I.bitmap + y_ * I.getWidth() + x_);
1219  Ires[i][j] = static_cast<unsigned char>(((up & 0x00FF) * cfrac + (up >> 8) * cratio) >> 16);
1220  } else {
1221  Ires[i][j] = *(I.bitmap + y_ * I.getWidth() + x_);
1222  }
1223  }
1224  }
1225  }
1226 }
1227 
1228 template <> inline
1230  const vpImageInterpolationType &method, unsigned int
1231  #if defined _OPENMP
1232  nThreads
1233  #endif
1234  )
1235 {
1236  if (I.getWidth() < 2 || I.getHeight() < 2 || Ires.getWidth() < 2 || Ires.getHeight() < 2) {
1237  std::cerr << "Input or output image is too small!" << std::endl;
1238  return;
1239  }
1240 
1241  if (method == INTERPOLATION_NEAREST || method == INTERPOLATION_CUBIC) {
1242  float scaleY = (I.getHeight() - 1) / static_cast<float>(Ires.getHeight() - 1);
1243  float scaleX = (I.getWidth() - 1) / static_cast<float>(Ires.getWidth() - 1);
1244 
1245  if (method == INTERPOLATION_NEAREST) {
1246  scaleY = I.getHeight() / static_cast<float>(Ires.getHeight() - 1);
1247  scaleX = I.getWidth() / static_cast<float>(Ires.getWidth() - 1);
1248  }
1249 
1250  #if defined _OPENMP
1251  if (nThreads > 0) {
1252  omp_set_num_threads(static_cast<int>(nThreads));
1253  }
1254  #pragma omp parallel for schedule(dynamic)
1255  #endif
1256  for (int i = 0; i < static_cast<int>(Ires.getHeight()); i++) {
1257  float v = i * scaleY;
1258  float yFrac = v - static_cast<int>(v);
1259 
1260  for (unsigned int j = 0; j < Ires.getWidth(); j++) {
1261  float u = j * scaleX;
1262  float xFrac = u - static_cast<int>(u);
1263 
1264  if (method == INTERPOLATION_NEAREST) {
1265  resizeNearest(I, Ires, static_cast<unsigned int>(i), j, u, v);
1266  } else if (method == INTERPOLATION_CUBIC) {
1267  resizeBicubic(I, Ires, static_cast<unsigned int>(i), j, u, v, xFrac, yFrac);
1268  }
1269  }
1270  }
1271  } else {
1272  const int32_t precision = 1 << 16;
1273  int64_t scaleY = static_cast<int64_t>((I.getHeight() - 1) / static_cast<float>(Ires.getHeight() - 1) * precision);
1274  int64_t scaleX = static_cast<int64_t>((I.getWidth() - 1) / static_cast<float>(Ires.getWidth() - 1) * precision);
1275 
1276 #if defined _OPENMP
1277  if (nThreads > 0) {
1278  omp_set_num_threads(static_cast<int>(nThreads));
1279  }
1280 #pragma omp parallel for schedule(dynamic)
1281 #endif
1282  for (int i = 0; i < static_cast<int>(Ires.getHeight()); i++) {
1283  int64_t v = i * scaleY;
1284  int64_t vround = v & (~0xFFFF);
1285  int64_t rratio = v - vround;
1286  int64_t y_ = v >> 16;
1287  int64_t rfrac = precision - rratio;
1288 
1289  for (unsigned int j = 0; j < Ires.getWidth(); j++) {
1290  int64_t u = j * scaleX;
1291  int64_t uround = u & (~0xFFFF);
1292  int64_t cratio = u - uround;
1293  int64_t x_ = u >> 16;
1294  int64_t cfrac = precision - cratio;
1295 
1296  if (y_ + 1 < static_cast<int64_t>(I.getHeight()) && x_ + 1 < static_cast<int64_t>(I.getWidth())) {
1297  int64_t col0 = lerp2((I.bitmap + y_ * I.getWidth() + x_)->R, (I.bitmap + (y_ + 1) * I.getWidth() + x_)->R, rratio, rfrac);
1298  int64_t col1 = lerp2((I.bitmap + y_ * I.getWidth() + x_ + 1)->R, (I.bitmap + (y_ + 1) * I.getWidth() + x_ + 1)->R, rratio, rfrac);
1299  int64_t valueR = lerp2(col0, col1, cratio, cfrac);
1300 
1301  col0 = lerp2((I.bitmap + y_ * I.getWidth() + x_)->G, (I.bitmap + (y_ + 1) * I.getWidth() + x_)->G, rratio, rfrac);
1302  col1 = lerp2((I.bitmap + y_ * I.getWidth() + x_ + 1)->G, (I.bitmap + (y_ + 1) * I.getWidth() + x_ + 1)->G, rratio, rfrac);
1303  int64_t valueG = lerp2(col0, col1, cratio, cfrac);
1304 
1305  col0 = lerp2((I.bitmap + y_ * I.getWidth() + x_)->B, (I.bitmap + (y_ + 1) * I.getWidth() + x_)->B, rratio, rfrac);
1306  col1 = lerp2((I.bitmap + y_ * I.getWidth() + x_ + 1)->B, (I.bitmap + (y_ + 1) * I.getWidth() + x_ + 1)->B, rratio, rfrac);
1307  int64_t valueB = lerp2(col0, col1, cratio, cfrac);
1308 
1309  Ires[i][j] = vpRGBa(static_cast<unsigned char>(valueR >> 32),
1310  static_cast<unsigned char>(valueG >> 32),
1311  static_cast<unsigned char>(valueB >> 32));
1312  } else if (y_ + 1 < static_cast<int64_t>(I.getHeight())) {
1313  int64_t valueR = lerp2((I.bitmap + y_ * I.getWidth() + x_)->R, (I.bitmap + (y_ + 1) * I.getWidth() + x_)->R, rratio, rfrac);
1314  int64_t valueG = lerp2((I.bitmap + y_ * I.getWidth() + x_)->G, (I.bitmap + (y_ + 1) * I.getWidth() + x_)->G, rratio, rfrac);
1315  int64_t valueB = lerp2((I.bitmap + y_ * I.getWidth() + x_)->B, (I.bitmap + (y_ + 1) * I.getWidth() + x_)->B, rratio, rfrac);
1316 
1317  Ires[i][j] = vpRGBa(static_cast<unsigned char>(valueR >> 16),
1318  static_cast<unsigned char>(valueG >> 16),
1319  static_cast<unsigned char>(valueB >> 16));
1320  } else if (x_ + 1 < static_cast<int64_t>(I.getWidth())) {
1321  int64_t valueR = lerp2((I.bitmap + x_)->R, (I.bitmap + x_ + 1)->R, cratio, cfrac);
1322  int64_t valueG = lerp2((I.bitmap + x_)->G, (I.bitmap + x_ + 1)->G, cratio, cfrac);
1323  int64_t valueB = lerp2((I.bitmap + x_)->B, (I.bitmap + x_ + 1)->B, cratio, cfrac);
1324 
1325  Ires[i][j] = vpRGBa(static_cast<unsigned char>(valueR >> 16),
1326  static_cast<unsigned char>(valueG >> 16),
1327  static_cast<unsigned char>(valueB >> 16));
1328  } else {
1329  Ires[i][j] = *(I.bitmap + y_ * I.getWidth() + x_);
1330  }
1331  }
1332  }
1333  }
1334 }
1335 
1350 template <class Type>
1352  const vpImageInterpolationType &interpolation,
1353  bool fixedPointArithmetic, bool pixelCenter)
1354 {
1355  if ((T.getRows() != 2 && T.getRows() != 3) || T.getCols() != 3) {
1356  std::cerr << "Input transformation must be a (2x3) or (3x3) matrix." << std::endl;
1357  return;
1358  }
1359 
1360  if (src.getSize() == 0) {
1361  return;
1362  }
1363 
1364  const bool affine = (T.getRows() == 2);
1365  const bool interp_NN = (interpolation == INTERPOLATION_NEAREST) || (interpolation == INTERPOLATION_CUBIC);
1366 
1367  if (dst.getSize() == 0) {
1368  dst.resize(src.getHeight(), src.getWidth(), Type(0));
1369  }
1370 
1371  vpMatrix M = T;
1372  if (affine) {
1373  double D = M[0][0] * M[1][1] - M[0][1] * M[1][0];
1374  D = !vpMath::nul(D, std::numeric_limits<double>::epsilon()) ? 1.0 / D : 0;
1375  double A11 = M[1][1] * D, A22 = M[0][0] * D;
1376  M[0][0] = A11; M[0][1] *= -D;
1377  M[1][0] *= -D; M[1][1] = A22;
1378  double b1 = -M[0][0] * M[0][2] - M[0][1] * M[1][2];
1379  double b2 = -M[1][0] * M[0][2] - M[1][1] * M[1][2];
1380  M[0][2] = b1; M[1][2] = b2;
1381  } else {
1382  M = T.inverseByLU();
1383  }
1384 
1385  if (fixedPointArithmetic && !pixelCenter) {
1386  fixedPointArithmetic = checkFixedPoint(0, 0, M, affine) &&
1387  checkFixedPoint(dst.getWidth()-1, 0, M, affine) &&
1388  checkFixedPoint(0, dst.getHeight()-1, M, affine) &&
1389  checkFixedPoint(dst.getWidth() - 1, dst.getHeight() - 1, M, affine);
1390  }
1391 
1392  if (interp_NN) {
1393  //nearest neighbor interpolation
1394  warpNN(src, M, dst, affine, pixelCenter, fixedPointArithmetic);
1395  } else {
1396  //bilinear interpolation
1397  warpLinear(src, M, dst, affine, pixelCenter, fixedPointArithmetic);
1398  }
1399 }
1400 
1401 template <class Type>
1402 void vpImageTools::warpNN(const vpImage<Type> &src, const vpMatrix &T, vpImage<Type> &dst, bool affine,
1403  bool centerCorner, bool fixedPoint)
1404 {
1405  if (fixedPoint && !centerCorner) {
1406  const int nbits = 16;
1407  const int32_t precision = 1 << nbits;
1408  const float precision_1 = 1 / static_cast<float>(precision);
1409 
1410  int32_t a0_i32 = static_cast<int32_t>(T[0][0] * precision);
1411  int32_t a1_i32 = static_cast<int32_t>(T[0][1] * precision);
1412  int32_t a2_i32 = static_cast<int32_t>(T[0][2] * precision);
1413  int32_t a3_i32 = static_cast<int32_t>(T[1][0] * precision);
1414  int32_t a4_i32 = static_cast<int32_t>(T[1][1] * precision);
1415  int32_t a5_i32 = static_cast<int32_t>(T[1][2] * precision);
1416  int32_t a6_i32 = T.getRows() == 3 ? static_cast<int32_t>(T[2][0] * precision) : 0;
1417  int32_t a7_i32 = T.getRows() == 3 ? static_cast<int32_t>(T[2][1] * precision) : 0;
1418  int32_t a8_i32 = T.getRows() == 3 ? static_cast<int32_t>(T[2][2] * precision) : 1;
1419 
1420  int32_t height_1_i32 = static_cast<int32_t>((src.getHeight() - 1) * precision) + 0x8000;
1421  int32_t width_1_i32 = static_cast<int32_t>((src.getWidth() - 1) * precision) + 0x8000;
1422 
1423  if (affine) {
1424  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1425  int32_t xi = a2_i32;
1426  int32_t yi = a5_i32;
1427 
1428  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1429  if (yi >= 0 && yi < height_1_i32 && xi >= 0 && xi < width_1_i32) {
1430  float x_ = (xi >> nbits) + (xi & 0xFFFF) * precision_1;
1431  float y_ = (yi >> nbits) + (yi & 0xFFFF) * precision_1;
1432 
1433  int x = vpMath::round(x_);
1434  int y = vpMath::round(y_);
1435  dst[i][j] = src[y][x];
1436  }
1437 
1438  xi += a0_i32;
1439  yi += a3_i32;
1440  }
1441 
1442  a2_i32 += a1_i32;
1443  a5_i32 += a4_i32;
1444  }
1445  } else {
1446  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1447  int64_t xi = a2_i32;
1448  int64_t yi = a5_i32;
1449  int64_t wi = a8_i32;
1450 
1451  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1452  if (wi != 0 && yi >= 0 && yi <= (static_cast<int>(src.getHeight()) - 1)*wi &&
1453  xi >= 0 && xi <= (static_cast<int>(src.getWidth()) - 1)*wi) {
1454  float w_ = (wi >> nbits) + (wi & 0xFFFF) * precision_1;
1455  float x_ = ((xi >> nbits) + (xi & 0xFFFF) * precision_1) / w_;
1456  float y_ = ((yi >> nbits) + (yi & 0xFFFF) * precision_1) / w_;
1457 
1458  int x = vpMath::round(x_);
1459  int y = vpMath::round(y_);
1460 
1461  dst[i][j] = src[y][x];
1462  }
1463 
1464  xi += a0_i32;
1465  yi += a3_i32;
1466  wi += a6_i32;
1467  }
1468 
1469  a2_i32 += a1_i32;
1470  a5_i32 += a4_i32;
1471  a8_i32 += a7_i32;
1472  }
1473  }
1474  } else {
1475  double a0 = T[0][0]; double a1 = T[0][1]; double a2 = T[0][2];
1476  double a3 = T[1][0]; double a4 = T[1][1]; double a5 = T[1][2];
1477  double a6 = affine ? 0.0 : T[2][0];
1478  double a7 = affine ? 0.0 : T[2][1];
1479  double a8 = affine ? 1.0 : T[2][2];
1480 
1481  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1482  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1483  double x = a0 * (centerCorner ? j + 0.5 : j) + a1 * (centerCorner ? i + 0.5 : i) + a2;
1484  double y = a3 * (centerCorner ? j + 0.5 : j) + a4 * (centerCorner ? i + 0.5 : i) + a5;
1485  double w = a6 * (centerCorner ? j + 0.5 : j) + a7 * (centerCorner ? i + 0.5 : i) + a8;
1486 
1487  if (vpMath::nul(w, std::numeric_limits<double>::epsilon())) {
1488  w = 1.0;
1489  }
1490 
1491  int x_ = centerCorner ? coordCast(x / w) : vpMath::round(x / w);
1492  int y_ = centerCorner ? coordCast(y / w) : vpMath::round(y / w);
1493 
1494  if (x_ >= 0 && x_ < static_cast<int>(src.getWidth()) &&
1495  y_ >= 0 && y_ < static_cast<int>(src.getHeight())) {
1496  dst[i][j] = src[y_][x_];
1497  }
1498  }
1499  }
1500  }
1501 }
1502 
1503 template <class Type>
1504 void vpImageTools::warpLinear(const vpImage<Type> &src, const vpMatrix &T, vpImage<Type> &dst, bool affine,
1505  bool centerCorner, bool fixedPoint)
1506 {
1507  if (fixedPoint && !centerCorner) {
1508  const int nbits = 16;
1509  const int64_t precision = 1 << nbits;
1510  const float precision_1 = 1 / static_cast<float>(precision);
1511  const int64_t precision2 = 1ULL << (2 * nbits);
1512  const float precision_2 = 1 / static_cast<float>(precision2);
1513 
1514  int64_t a0_i64 = static_cast<int64_t>(T[0][0] * precision);
1515  int64_t a1_i64 = static_cast<int64_t>(T[0][1] * precision);
1516  int64_t a2_i64 = static_cast<int64_t>(T[0][2] * precision);
1517  int64_t a3_i64 = static_cast<int64_t>(T[1][0] * precision);
1518  int64_t a4_i64 = static_cast<int64_t>(T[1][1] * precision);
1519  int64_t a5_i64 = static_cast<int64_t>(T[1][2] * precision);
1520  int64_t a6_i64 = T.getRows() == 3 ? static_cast<int64_t>(T[2][0] * precision) : 0;
1521  int64_t a7_i64 = T.getRows() == 3 ? static_cast<int64_t>(T[2][1] * precision) : 0;
1522  int64_t a8_i64 = T.getRows() == 3 ? static_cast<int64_t>(T[2][2] * precision) : 1;
1523 
1524  int64_t height_i64 = static_cast<int64_t>(src.getHeight() * precision);
1525  int64_t width_i64 = static_cast<int64_t>(src.getWidth() * precision);
1526 
1527  if (affine) {
1528  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1529  int64_t xi_ = a2_i64;
1530  int64_t yi_ = a5_i64;
1531 
1532  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1533  if (yi_ >= 0 && yi_ < height_i64 && xi_ >= 0 && xi_ < width_i64) {
1534  const int64_t xi_lower = xi_ & (~0xFFFF);
1535  const int64_t yi_lower = yi_ & (~0xFFFF);
1536 
1537  const int64_t t = yi_ - yi_lower;
1538  const int64_t t_1 = precision - t;
1539  const int64_t s = xi_ - xi_lower;
1540  const int64_t s_1 = precision - s;
1541 
1542  const int x_ = static_cast<int>(xi_ >> nbits);
1543  const int y_ = static_cast<int>(yi_ >> nbits);
1544 
1545  if (y_ < static_cast<int>(src.getHeight())-1 && x_ < static_cast<int>(src.getWidth())-1) {
1546  const Type val00 = src[y_][x_];
1547  const Type val01 = src[y_][x_+1];
1548  const Type val10 = src[y_+1][x_];
1549  const Type val11 = src[y_+1][x_+1];
1550  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);
1551  const float interp = (interp_i64 >> (nbits*2)) + (interp_i64 & 0xFFFFFFFF) * precision_2;
1552  dst[i][j] = vpMath::saturate<Type>(interp);
1553  } else if (y_ < static_cast<int>(src.getHeight())-1) {
1554  const Type val00 = src[y_][x_];
1555  const Type val10 = src[y_+1][x_];
1556  const int64_t interp_i64 = static_cast<int64_t>(t_1*val00 + t*val10);
1557  const float interp = (interp_i64 >> nbits) + (interp_i64 & 0xFFFF) * precision_1;
1558  dst[i][j] = vpMath::saturate<Type>(interp);
1559  } else if (x_ < static_cast<int>(src.getWidth())-1) {
1560  const Type val00 = src[y_][x_];
1561  const Type val01 = src[y_][x_+1];
1562  const int64_t interp_i64 = static_cast<int64_t>(s_1*val00 + s*val01);
1563  const float interp = (interp_i64 >> nbits) + (interp_i64 & 0xFFFF) * precision_1;
1564  dst[i][j] = vpMath::saturate<Type>(interp);
1565  } else {
1566  dst[i][j] = src[y_][x_];
1567  }
1568  }
1569 
1570  xi_ += a0_i64;
1571  yi_ += a3_i64;
1572  }
1573 
1574  a2_i64 += a1_i64;
1575  a5_i64 += a4_i64;
1576  }
1577  } else {
1578  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1579  int64_t xi = a2_i64;
1580  int64_t yi = a5_i64;
1581  int64_t wi = a8_i64;
1582 
1583  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1584  if (wi != 0 && yi >= 0 && yi <= (static_cast<int>(src.getHeight()) - 1)*wi &&
1585  xi >= 0 && xi <= (static_cast<int>(src.getWidth()) - 1)*wi) {
1586  const float wi_ = (wi >> nbits) + (wi & 0xFFFF) * precision_1;
1587  const float xi_ = ((xi >> nbits) + (xi & 0xFFFF) * precision_1) / wi_;
1588  const float yi_ = ((yi >> nbits) + (yi & 0xFFFF) * precision_1) / wi_;
1589 
1590  const int x_ = static_cast<int>(xi_);
1591  const int y_ = static_cast<int>(yi_);
1592 
1593  const float t = yi_ - y_;
1594  const float s = xi_ - x_;
1595 
1596  if (y_ < static_cast<int>(src.getHeight()) - 1 && x_ < static_cast<int>(src.getWidth()) - 1) {
1597  const Type val00 = src[y_][x_];
1598  const Type val01 = src[y_][x_ + 1];
1599  const Type val10 = src[y_ + 1][x_];
1600  const Type val11 = src[y_ + 1][x_ + 1];
1601  const float col0 = lerp(val00, val01, s);
1602  const float col1 = lerp(val10, val11, s);
1603  const float interp = lerp(col0, col1, t);
1604  dst[i][j] = vpMath::saturate<Type>(interp);
1605  } else if (y_ < static_cast<int>(src.getHeight()) - 1) {
1606  const Type val00 = src[y_][x_];
1607  const Type val10 = src[y_ + 1][x_];
1608  const float interp = lerp(val00, val10, t);
1609  dst[i][j] = vpMath::saturate<Type>(interp);
1610  } else if (x_ < static_cast<int>(src.getWidth()) - 1) {
1611  const Type val00 = src[y_][x_];
1612  const Type val01 = src[y_][x_ + 1];
1613  const float interp = lerp(val00, val01, s);
1614  dst[i][j] = vpMath::saturate<Type>(interp);
1615  } else {
1616  dst[i][j] = src[y_][x_];
1617  }
1618  }
1619 
1620  xi += a0_i64;
1621  yi += a3_i64;
1622  wi += a6_i64;
1623  }
1624 
1625  a2_i64 += a1_i64;
1626  a5_i64 += a4_i64;
1627  a8_i64 += a7_i64;
1628  }
1629  }
1630  } else {
1631  double a0 = T[0][0]; double a1 = T[0][1]; double a2 = T[0][2];
1632  double a3 = T[1][0]; double a4 = T[1][1]; double a5 = T[1][2];
1633  double a6 = affine ? 0.0 : T[2][0];
1634  double a7 = affine ? 0.0 : T[2][1];
1635  double a8 = affine ? 1.0 : T[2][2];
1636 
1637  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1638  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1639  double x = a0 * (centerCorner ? j + 0.5 : j) + a1 * (centerCorner ? i + 0.5 : i) + a2;
1640  double y = a3 * (centerCorner ? j + 0.5 : j) + a4 * (centerCorner ? i + 0.5 : i) + a5;
1641  double w = a6 * (centerCorner ? j + 0.5 : j) + a7 * (centerCorner ? i + 0.5 : i) + a8;
1642  if (vpMath::nul(w, std::numeric_limits<double>::epsilon())) {
1643  w = 1;
1644  }
1645 
1646  x = x / w - (centerCorner ? 0.5 : 0);
1647  y = y / w - (centerCorner ? 0.5 : 0);
1648 
1649  int x_lower = static_cast<int>(x);
1650  int y_lower = static_cast<int>(y);
1651 
1652  if (y_lower >= static_cast<int>(src.getHeight()) || x_lower >= static_cast<int>(src.getWidth()) ||
1653  y < 0 || x < 0) {
1654  continue;
1655  }
1656 
1657  double s = x - x_lower;
1658  double t = y - y_lower;
1659 
1660  if (y_lower < static_cast<int>(src.getHeight())-1 && x_lower < static_cast<int>(src.getWidth())-1) {
1661  const Type val00 = src[y_lower][x_lower];
1662  const Type val01 = src[y_lower][x_lower + 1];
1663  const Type val10 = src[y_lower + 1][x_lower];
1664  const Type val11 = src[y_lower + 1][x_lower + 1];
1665  const double col0 = lerp(val00, val01, s);
1666  const double col1 = lerp(val10, val11, s);
1667  const double interp = lerp(col0, col1, t);
1668  dst[i][j] = vpMath::saturate<Type>(interp);
1669  } else if (y_lower < static_cast<int>(src.getHeight())-1) {
1670  const Type val00 = src[y_lower][x_lower];
1671  const Type val10 = src[y_lower + 1][x_lower];
1672  const double interp = lerp(val00, val10, t);
1673  dst[i][j] = vpMath::saturate<Type>(interp);
1674  } else if (x_lower < static_cast<int>(src.getWidth())-1) {
1675  const Type val00 = src[y_lower][x_lower];
1676  const Type val01 = src[y_lower][x_lower + 1];
1677  const double interp = lerp(val00, val01, s);
1678  dst[i][j] = vpMath::saturate<Type>(interp);
1679  } else {
1680  dst[i][j] = src[y_lower][x_lower];
1681  }
1682  }
1683  }
1684  }
1685 }
1686 
1687 template <> inline
1688 void vpImageTools::warpLinear(const vpImage<vpRGBa> &src, const vpMatrix &T, vpImage<vpRGBa> &dst, bool affine,
1689  bool centerCorner, bool fixedPoint)
1690 {
1691  if (fixedPoint && !centerCorner) {
1692  const int nbits = 16;
1693  const int64_t precision = 1 << nbits;
1694  const float precision_1 = 1 / static_cast<float>(precision);
1695  const int64_t precision2 = 1ULL << (2 * nbits);
1696  const float precision_2 = 1 / static_cast<float>(precision2);
1697 
1698  int64_t a0_i64 = static_cast<int64_t>(T[0][0] * precision);
1699  int64_t a1_i64 = static_cast<int64_t>(T[0][1] * precision);
1700  int64_t a2_i64 = static_cast<int64_t>(T[0][2] * precision);
1701  int64_t a3_i64 = static_cast<int64_t>(T[1][0] * precision);
1702  int64_t a4_i64 = static_cast<int64_t>(T[1][1] * precision);
1703  int64_t a5_i64 = static_cast<int64_t>(T[1][2] * precision);
1704  int64_t a6_i64 = T.getRows() == 3 ? static_cast<int64_t>(T[2][0] * precision) : 0;
1705  int64_t a7_i64 = T.getRows() == 3 ? static_cast<int64_t>(T[2][1] * precision) : 0;
1706  int64_t a8_i64 = precision;
1707 
1708  int64_t height_i64 = static_cast<int64_t>(src.getHeight() * precision);
1709  int64_t width_i64 = static_cast<int64_t>(src.getWidth() * precision);
1710 
1711  if (affine) {
1712  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1713  int64_t xi = a2_i64;
1714  int64_t yi = a5_i64;
1715 
1716  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1717  if (yi >= 0 && yi < height_i64 && xi >= 0 && xi < width_i64) {
1718  const int64_t xi_lower = xi & (~0xFFFF);
1719  const int64_t yi_lower = yi & (~0xFFFF);
1720 
1721  const int64_t t = yi - yi_lower;
1722  const int64_t t_1 = precision - t;
1723  const int64_t s = xi - xi_lower;
1724  const int64_t s_1 = precision - s;
1725 
1726  const int x_ = static_cast<int>(xi >> nbits);
1727  const int y_ = static_cast<int>(yi >> nbits);
1728 
1729  if (y_ < static_cast<int>(src.getHeight())-1 && x_ < static_cast<int>(src.getWidth())-1) {
1730  const vpRGBa val00 = src[y_][x_];
1731  const vpRGBa val01 = src[y_][x_+1];
1732  const vpRGBa val10 = src[y_+1][x_];
1733  const vpRGBa val11 = src[y_+1][x_+1];
1734  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);
1735  const float interpR = (interpR_i64 >> (nbits*2)) + (interpR_i64 & 0xFFFFFFFF) * precision_2;
1736 
1737  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);
1738  const float interpG = (interpG_i64 >> (nbits * 2)) + (interpG_i64 & 0xFFFFFFFF) * precision_2;
1739 
1740  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);
1741  const float interpB = (interpB_i64 >> (nbits * 2)) + (interpB_i64 & 0xFFFFFFFF) * precision_2;
1742 
1743  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR),
1744  vpMath::saturate<unsigned char>(interpG),
1745  vpMath::saturate<unsigned char>(interpB),
1746  255);
1747  } else if (y_ < static_cast<int>(src.getHeight())-1) {
1748  const vpRGBa val00 = src[y_][x_];
1749  const vpRGBa val10 = src[y_+1][x_];
1750  const int64_t interpR_i64 = static_cast<int64_t>(t_1*val00.R + t*val10.R);
1751  const float interpR = (interpR_i64 >> nbits) + (interpR_i64 & 0xFFFF) * precision_1;
1752 
1753  const int64_t interpG_i64 = static_cast<int64_t>(t_1*val00.G + t * val10.G);
1754  const float interpG = (interpG_i64 >> nbits) + (interpG_i64 & 0xFFFF) * precision_1;
1755 
1756  const int64_t interpB_i64 = static_cast<int64_t>(t_1*val00.B + t * val10.B);
1757  const float interpB = (interpB_i64 >> nbits) + (interpB_i64 & 0xFFFF) * precision_1;
1758 
1759  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR),
1760  vpMath::saturate<unsigned char>(interpG),
1761  vpMath::saturate<unsigned char>(interpB),
1762  255);
1763  } else if (x_ < static_cast<int>(src.getWidth())-1) {
1764  const vpRGBa val00 = src[y_][x_];
1765  const vpRGBa val01 = src[y_][x_+1];
1766  const int64_t interpR_i64 = static_cast<int64_t>(s_1*val00.R + s*val01.R);
1767  const float interpR = (interpR_i64 >> nbits) + (interpR_i64 & 0xFFFF) * precision_1;
1768 
1769  const int64_t interpG_i64 = static_cast<int64_t>(s_1*val00.G + s * val01.G);
1770  const float interpG = (interpG_i64 >> nbits) + (interpG_i64 & 0xFFFF) * precision_1;
1771 
1772  const int64_t interpB_i64 = static_cast<int64_t>(s_1*val00.B + s * val01.B);
1773  const float interpB = (interpB_i64 >> nbits) + (interpB_i64 & 0xFFFF) * precision_1;
1774 
1775  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR),
1776  vpMath::saturate<unsigned char>(interpG),
1777  vpMath::saturate<unsigned char>(interpB),
1778  255);
1779  } else {
1780  dst[i][j] = src[y_][x_];
1781  }
1782  }
1783 
1784  xi += a0_i64;
1785  yi += a3_i64;
1786  }
1787 
1788  a2_i64 += a1_i64;
1789  a5_i64 += a4_i64;
1790  }
1791  } else {
1792  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1793  int64_t xi = a2_i64;
1794  int64_t yi = a5_i64;
1795  int64_t wi = a8_i64;
1796 
1797  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1798  if (yi >= 0 && yi <= (static_cast<int>(src.getHeight()) - 1)*wi &&
1799  xi >= 0 && xi <= (static_cast<int>(src.getWidth()) - 1)*wi) {
1800  const float wi_ = (wi >> nbits) + (wi & 0xFFFF) * precision_1;
1801  const float xi_ = ((xi >> nbits) + (xi & 0xFFFF) * precision_1) / wi_;
1802  const float yi_ = ((yi >> nbits) + (yi & 0xFFFF) * precision_1) / wi_;
1803 
1804  const int x_ = static_cast<int>(xi_);
1805  const int y_ = static_cast<int>(yi_);
1806 
1807  const float t = yi_ - y_;
1808  const float s = xi_ - x_;
1809 
1810  if (y_ < static_cast<int>(src.getHeight()) - 1 && x_ < static_cast<int>(src.getWidth()) - 1) {
1811  const vpRGBa val00 = src[y_][x_];
1812  const vpRGBa val01 = src[y_][x_ + 1];
1813  const vpRGBa val10 = src[y_ + 1][x_];
1814  const vpRGBa val11 = src[y_ + 1][x_ + 1];
1815  const float colR0 = lerp(val00.R, val01.R, s);
1816  const float colR1 = lerp(val10.R, val11.R, s);
1817  const float interpR = lerp(colR0, colR1, t);
1818 
1819  const float colG0 = lerp(val00.G, val01.G, s);
1820  const float colG1 = lerp(val10.G, val11.G, s);
1821  const float interpG = lerp(colG0, colG1, t);
1822 
1823  const float colB0 = lerp(val00.B, val01.B, s);
1824  const float colB1 = lerp(val10.B, val11.B, s);
1825  const float interpB = lerp(colB0, colB1, t);
1826 
1827  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR),
1828  vpMath::saturate<unsigned char>(interpG),
1829  vpMath::saturate<unsigned char>(interpB),
1830  255);
1831  } else if (y_ < static_cast<int>(src.getHeight()) - 1) {
1832  const vpRGBa val00 = src[y_][x_];
1833  const vpRGBa val10 = src[y_ + 1][x_];
1834  const float interpR = lerp(val00.R, val10.R, t);
1835  const float interpG = lerp(val00.G, val10.G, t);
1836  const float interpB = lerp(val00.B, val10.B, t);
1837 
1838  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR),
1839  vpMath::saturate<unsigned char>(interpG),
1840  vpMath::saturate<unsigned char>(interpB),
1841  255);
1842  } else if (x_ < static_cast<int>(src.getWidth()) - 1) {
1843  const vpRGBa val00 = src[y_][x_];
1844  const vpRGBa val01 = src[y_][x_ + 1];
1845  const float interpR = lerp(val00.R, val01.R, s);
1846  const float interpG = lerp(val00.G, val01.G, s);
1847  const float interpB = lerp(val00.B, val01.B, s);
1848 
1849  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR),
1850  vpMath::saturate<unsigned char>(interpG),
1851  vpMath::saturate<unsigned char>(interpB),
1852  255);
1853  } else {
1854  dst[i][j] = src[y_][x_];
1855  }
1856  }
1857 
1858  xi += a0_i64;
1859  yi += a3_i64;
1860  wi += a6_i64;
1861  }
1862 
1863  a2_i64 += a1_i64;
1864  a5_i64 += a4_i64;
1865  a8_i64 += a7_i64;
1866  }
1867  }
1868  } else {
1869  double a0 = T[0][0]; double a1 = T[0][1]; double a2 = T[0][2];
1870  double a3 = T[1][0]; double a4 = T[1][1]; double a5 = T[1][2];
1871  double a6 = affine ? 0.0 : T[2][0];
1872  double a7 = affine ? 0.0 : T[2][1];
1873  double a8 = affine ? 1.0 : T[2][2];
1874 
1875  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1876  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1877  double x = a0 * (centerCorner ? j + 0.5 : j) + a1 * (centerCorner ? i + 0.5 : i) + a2;
1878  double y = a3 * (centerCorner ? j + 0.5 : j) + a4 * (centerCorner ? i + 0.5 : i) + a5;
1879  double w = a6 * (centerCorner ? j + 0.5 : j) + a7 * (centerCorner ? i + 0.5 : i) + a8;
1880 
1881  x = x / w - (centerCorner ? 0.5 : 0);
1882  y = y / w - (centerCorner ? 0.5 : 0);
1883 
1884  int x_lower = static_cast<int>(x);
1885  int y_lower = static_cast<int>(y);
1886 
1887  if (y_lower >= static_cast<int>(src.getHeight()) || x_lower >= static_cast<int>(src.getWidth()) ||
1888  y < 0 || x < 0) {
1889  continue;
1890  }
1891 
1892  double s = x - x_lower;
1893  double t = y - y_lower;
1894 
1895  if (y_lower < static_cast<int>(src.getHeight())-1 && x_lower < static_cast<int>(src.getWidth())-1) {
1896  const vpRGBa val00 = src[y_lower][x_lower];
1897  const vpRGBa val01 = src[y_lower][x_lower +1];
1898  const vpRGBa val10 = src[y_lower +1][x_lower];
1899  const vpRGBa val11 = src[y_lower +1][x_lower +1];
1900  const double colR0 = lerp(val00.R, val01.R, s);
1901  const double colR1 = lerp(val10.R, val11.R, s);
1902  const double interpR = lerp(colR0, colR1, t);
1903 
1904  const double colG0 = lerp(val00.G, val01.G, s);
1905  const double colG1 = lerp(val10.G, val11.G, s);
1906  const double interpG = lerp(colG0, colG1, t);
1907 
1908  const double colB0 = lerp(val00.B, val01.B, s);
1909  const double colB1 = lerp(val10.B, val11.B, s);
1910  const double interpB = lerp(colB0, colB1, t);
1911 
1912  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR),
1913  vpMath::saturate<unsigned char>(interpG),
1914  vpMath::saturate<unsigned char>(interpB),
1915  255);
1916  } else if (y_lower < static_cast<int>(src.getHeight())-1) {
1917  const vpRGBa val00 = src[y_lower][x_lower];
1918  const vpRGBa val10 = src[y_lower +1][x_lower];
1919  const double interpR = lerp(val00.R, val10.R, t);
1920  const double interpG = lerp(val00.G, val10.G, t);
1921  const double interpB = lerp(val00.B, val10.B, t);
1922 
1923  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR),
1924  vpMath::saturate<unsigned char>(interpG),
1925  vpMath::saturate<unsigned char>(interpB),
1926  255);
1927  } else if (x_lower < static_cast<int>(src.getWidth())-1) {
1928  const vpRGBa val00 = src[y_lower][x_lower];
1929  const vpRGBa val01 = src[y_lower][x_lower +1];
1930  const double interpR = lerp(val00.R, val01.R, s);
1931  const double interpG = lerp(val00.G, val01.G, s);
1932  const double interpB = lerp(val00.B, val01.B, s);
1933 
1934  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR),
1935  vpMath::saturate<unsigned char>(interpG),
1936  vpMath::saturate<unsigned char>(interpB),
1937  255);
1938  } else {
1939  dst[i][j] = src[y_lower][x_lower];
1940  }
1941  }
1942  }
1943  }
1944 }
1945 
1946 #endif
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:156
void performLut(const Type(&lut)[256], unsigned int nbThreads=1)
Definition: vpImage.h:1759
VISP_EXPORT uint16_t reinterpret_cast_uchar_to_uint16_LE(unsigned char *const ptr)
Definition: vpEndian.cpp:111
double get_i() const
Definition: vpImagePoint.h:203
double getTop() const
Definition: vpRect.h:193
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:881
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:143
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:228
static bool nul(double x, double s=0.001)
Definition: vpMath.h:291
static double sqr(double x)
Definition: vpMath.h:116
double get_j() const
Definition: vpImagePoint.h:214
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:456
static void flip(const vpImage< Type > &I, vpImage< Type > &newI)
Definition: vpImageTools.h:831
double getLeft() const
Definition: vpRect.h:174
static void undistort(const vpImage< Type > &I, const vpCameraParameters &cam, vpImage< Type > &newI, unsigned int nThreads=2)
Definition: vpImageTools.h:645
static int round(double x)
Definition: vpMath.h:245
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:188
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:300
unsigned int getSize() const
Definition: vpImage.h:227
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:167
double get_kud() const
Defines a rectangle in the plane.
Definition: vpRect.h:79
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
unsigned int getWidth() const
Definition: vpImage.h:246
Definition of the vpImage class member functions.
Definition: vpImage.h:126
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)