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