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