Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
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_THREADS
47 #include <thread>
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:
77  {
81  INTERPOLATION_AREA
82  };
83 
84  template <class Type>
85  static inline void binarise(vpImage<Type> &I, Type threshold1, Type threshold2, Type value1, Type value2, Type value3,
86  bool useLUT = true);
87  static void changeLUT(vpImage<unsigned char> &I, unsigned char A, unsigned char newA, unsigned char B,
88  unsigned char newB);
89 
90  template <class Type>
91  static void crop(const vpImage<Type> &I, double roi_top, double roi_left, unsigned int roi_height,
92  unsigned int roi_width, vpImage<Type> &crop, unsigned int v_scale = 1, unsigned int h_scale = 1);
93 
94  static void columnMean(const vpImage<double> &I, vpRowVector &result);
95 
96  template <class Type>
97  static void crop(const vpImage<Type> &I, const vpImagePoint &topLeft, unsigned int roi_height, unsigned int roi_width,
98  vpImage<Type> &crop, unsigned int v_scale = 1, unsigned int h_scale = 1);
99  template <class Type>
100  static void crop(const vpImage<Type> &I, const vpRect &roi, vpImage<Type> &crop, unsigned int v_scale = 1,
101  unsigned int h_scale = 1);
102  template <class Type>
103  static void crop(const unsigned char *bitmap, unsigned int width, unsigned int height, const vpRect &roi,
104  vpImage<Type> &crop, unsigned int v_scale = 1, unsigned int h_scale = 1);
105 
106  static void extract(const vpImage<unsigned char> &Src, vpImage<unsigned char> &Dst, const vpRectOriented &r);
107  static void extract(const vpImage<unsigned char> &Src, vpImage<double> &Dst, const vpRectOriented &r);
108 
109  template <class Type> static void flip(const vpImage<Type> &I, vpImage<Type> &newI);
110 
111  template <class Type> static void flip(vpImage<Type> &I);
112 
113  static void imageDifference(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
114  vpImage<unsigned char> &Idiff);
115  static void imageDifference(const vpImage<vpRGBa> &I1, const vpImage<vpRGBa> &I2, vpImage<vpRGBa> &Idiff);
116 
117  static void imageDifferenceAbsolute(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
118  vpImage<unsigned char> &Idiff);
119  static void imageDifferenceAbsolute(const vpImage<double> &I1, const vpImage<double> &I2, vpImage<double> &Idiff);
120  static void imageDifferenceAbsolute(const vpImage<vpRGBa> &I1, const vpImage<vpRGBa> &I2, vpImage<vpRGBa> &Idiff);
121 
122  static void imageAdd(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2, vpImage<unsigned char> &Ires,
123  bool saturate = false);
124 
125  static void imageSubtract(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
126  vpImage<unsigned char> &Ires, bool saturate = false);
127 
128  static void initUndistortMap(const vpCameraParameters &cam, unsigned int width, unsigned int height,
129  vpArray2D<int> &mapU, vpArray2D<int> &mapV, vpArray2D<float> &mapDu,
130  vpArray2D<float> &mapDv);
131 
132  static double interpolate(const vpImage<unsigned char> &I, const vpImagePoint &point,
133  const vpImageInterpolationType &method = INTERPOLATION_NEAREST);
134 
135  static void integralImage(const vpImage<unsigned char> &I, vpImage<double> &II, vpImage<double> &IIsq);
136 
137  static double normalizedCorrelation(const vpImage<double> &I1, const vpImage<double> &I2, bool useOptimized = true);
138 
139  static void normalize(vpImage<double> &I);
140 
141  static void remap(const vpImage<unsigned char> &I, const vpArray2D<int> &mapU, const vpArray2D<int> &mapV,
142  const vpArray2D<float> &mapDu, const vpArray2D<float> &mapDv, vpImage<unsigned char> &Iundist);
143  static void remap(const vpImage<vpRGBa> &I, const vpArray2D<int> &mapU, const vpArray2D<int> &mapV,
144  const vpArray2D<float> &mapDu, const vpArray2D<float> &mapDv, vpImage<vpRGBa> &Iundist);
145 
146  template <class Type>
147  static void resize(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int width, unsigned int height,
148  const vpImageInterpolationType &method = INTERPOLATION_NEAREST, unsigned int nThreads = 0);
149 
150  template <class Type>
151  static void resize(const vpImage<Type> &I, vpImage<Type> &Ires,
152  const vpImageInterpolationType &method = INTERPOLATION_NEAREST, unsigned int nThreads = 0);
153 
154  static void templateMatching(const vpImage<unsigned char> &I, const vpImage<unsigned char> &I_tpl,
155  vpImage<double> &I_score, unsigned int step_u, unsigned int step_v,
156  bool useOptimized = true);
157 
158  template <class Type>
159  static void undistort(const vpImage<Type> &I, const vpCameraParameters &cam, vpImage<Type> &newI,
160  unsigned int nThreads = 2);
161 
162  template <class Type>
163  static void undistort(const vpImage<Type> &I, vpArray2D<int> mapU, vpArray2D<int> mapV, vpArray2D<float> mapDu,
164  vpArray2D<float> mapDv, vpImage<Type> &newI);
165 
166  template <class Type>
167  static void warpImage(const vpImage<Type> &src, const vpMatrix &T, vpImage<Type> &dst,
168  const vpImageInterpolationType &interpolation = INTERPOLATION_NEAREST,
169  bool fixedPointArithmetic = true, bool pixelCenter = false);
170 
171 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
176  template <class Type>
177  vp_deprecated static void createSubImage(const vpImage<Type> &I, unsigned int i_sub, unsigned int j_sub,
178  unsigned int nrow_sub, unsigned int ncol_sub, vpImage<Type> &S);
179 
180  template <class Type>
181  vp_deprecated static void createSubImage(const vpImage<Type> &I, const vpRect &rect, vpImage<Type> &S);
183 #endif
184 
185 private:
186  // Cubic interpolation
187  static float cubicHermite(const float A, const float B, const float C, const float D, const float t);
188 
189  template <class Type> static Type getPixelClamped(const vpImage<Type> &I, float u, float v);
190 
191  static int coordCast(double x);
192 
193  // Linear interpolation
194  static double lerp(double A, double B, double t);
195  static float lerp(float A, float B, float t);
196  static int64_t lerp2(int64_t A, int64_t B, int64_t t, int64_t t_1);
197 
198  static double normalizedCorrelation(const vpImage<double> &I1, const vpImage<double> &I2, const vpImage<double> &II,
199  const vpImage<double> &IIsq, const vpImage<double> &II_tpl,
200  const vpImage<double> &IIsq_tpl, unsigned int i0, unsigned int j0);
201 
202  template <class Type>
203  static void resizeBicubic(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int i, unsigned int j, float u,
204  float v, float xFrac, float yFrac);
205 
206  template <class Type>
207  static void resizeBilinear(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 resizeNearest(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int i, unsigned int j, float u,
212  float v);
213 
214 #if defined(VISP_HAVE_SIMDLIB)
215  static void resizeSimdlib(const vpImage<vpRGBa> &Isrc, unsigned int resizeWidth, unsigned int resizeHeight,
216  vpImage<vpRGBa> &Idst, int method);
217  static void resizeSimdlib(const vpImage<unsigned char> &Isrc, unsigned int resizeWidth, unsigned int resizeHeight,
218  vpImage<unsigned char> &Idst, int method);
219 #endif
220 
221  template <class Type>
222  static void warpNN(const vpImage<Type> &src, const vpMatrix &T, vpImage<Type> &dst, bool affine, bool centerCorner,
223  bool fixedPoint);
224 
225  template <class Type>
226  static void warpLinear(const vpImage<Type> &src, const vpMatrix &T, vpImage<Type> &dst, bool affine,
227  bool centerCorner, bool fixedPoint);
228 
229  static bool checkFixedPoint(unsigned int x, unsigned int y, const vpMatrix &T, bool affine);
230 };
231 
232 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
252 template <class Type>
253 void vpImageTools::createSubImage(const vpImage<Type> &I, unsigned int roi_top, unsigned int roi_left,
254  unsigned int roi_height, unsigned int roi_width, vpImage<Type> &crop)
255 {
256  vpImageTools::crop(I, roi_top, roi_left, roi_height, roi_width, crop);
257 }
258 
274 template <class Type> void vpImageTools::createSubImage(const vpImage<Type> &I, const vpRect &roi, vpImage<Type> &crop)
275 {
276  vpImageTools::crop(I, roi, crop);
277 }
278 
279 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
280 
303 template <class Type>
304 void vpImageTools::crop(const vpImage<Type> &I, double roi_top, double roi_left, unsigned int roi_height,
305  unsigned int roi_width, vpImage<Type> &crop, unsigned int v_scale, unsigned int h_scale)
306 {
307  int i_min = std::max<int>((int)(ceil(roi_top / v_scale)), 0);
308  int j_min = std::max<int>((int)(ceil(roi_left / h_scale)), 0);
309  int i_max = std::min<int>((int)(ceil((roi_top + roi_height)) / v_scale), (int)(I.getHeight() / v_scale));
310  int j_max = std::min<int>((int)(ceil((roi_left + roi_width) / h_scale)), (int)(I.getWidth() / h_scale));
311 
312  unsigned int i_min_u = (unsigned int)i_min;
313  unsigned int j_min_u = (unsigned int)j_min;
314 
315  unsigned int r_width = (unsigned int)(j_max - j_min);
316  unsigned int r_height = (unsigned int)(i_max - i_min);
317 
318  crop.resize(r_height, r_width);
319 
320  if (v_scale == 1 && h_scale == 1) {
321  for (unsigned int i = 0; i < r_height; i++) {
322  void *src = (void *)(I[i + i_min_u] + j_min_u);
323  void *dst = (void *)crop[i];
324  memcpy(dst, src, r_width * sizeof(Type));
325  }
326  }
327  else if (h_scale == 1) {
328  for (unsigned int i = 0; i < r_height; i++) {
329  void *src = (void *)(I[(i + i_min_u) * v_scale] + j_min_u);
330  void *dst = (void *)crop[i];
331  memcpy(dst, src, r_width * sizeof(Type));
332  }
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>((int)(ceil(roi.getTop() / v_scale)), 0);
413  int j_min = std::max<int>((int)(ceil(roi.getLeft() / h_scale)), 0);
414  int i_max = std::min<int>((int)(ceil((roi.getTop() + roi.getHeight()) / v_scale)), (int)(height / v_scale));
415  int j_max = std::min<int>((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  }
432  else if (h_scale == 1) {
433  for (unsigned int i = 0; i < r_height; i++) {
434  void *src = (void *)(bitmap + ((i + i_min_u) * width * v_scale + j_min_u) * sizeof(Type));
435  void *dst = (void *)crop[i];
436  memcpy(dst, src, r_width * sizeof(Type));
437  }
438  }
439  else {
440  for (unsigned int i = 0; i < r_height; i++) {
441  unsigned int i_src = (i + i_min_u) * width * v_scale + j_min_u * h_scale;
442  for (unsigned int j = 0; j < r_width; j++) {
443  void *src = (void *)(bitmap + (i_src + j * h_scale) * sizeof(Type));
444  void *dst = (void *)&crop[i][j];
445  memcpy(dst, src, sizeof(Type));
446  }
447  }
448  }
449 }
450 
461 template <class Type>
462 inline void vpImageTools::binarise(vpImage<Type> &I, Type threshold1, Type threshold2, Type value1, Type value2,
463  Type value3, bool useLUT)
464 {
465  if (useLUT) {
466  std::cerr << "LUT not available for this type ! Will use the iteration method." << std::endl;
467  }
468 
469  Type v;
470  Type *p = I.bitmap;
471  Type *pend = I.bitmap + I.getWidth() * I.getHeight();
472  for (; p < pend; p++) {
473  v = *p;
474  if (v < threshold1)
475  *p = value1;
476  else if (v > threshold2)
477  *p = value3;
478  else
479  *p = value2;
480  }
481 }
482 
493 template <>
494 inline void vpImageTools::binarise(vpImage<unsigned char> &I, unsigned char threshold1, unsigned char threshold2,
495  unsigned char value1, unsigned char value2, unsigned char value3, bool useLUT)
496 {
497  if (useLUT) {
498  // Construct the LUT
499  unsigned char lut[256];
500  for (unsigned int i = 0; i < 256; i++) {
501  lut[i] = i < threshold1 ? value1 : (i > threshold2 ? value3 : value2);
502  }
503 
504  I.performLut(lut);
505  }
506  else {
507  unsigned char *p = I.bitmap;
508  unsigned char *pend = I.bitmap + I.getWidth() * I.getHeight();
509  for (; p < pend; p++) {
510  unsigned char v = *p;
511  if (v < threshold1)
512  *p = value1;
513  else if (v > threshold2)
514  *p = value3;
515  else
516  *p = value2;
517  }
518  }
519 }
520 
521 #ifdef VISP_HAVE_THREADS
522 
523 #ifndef DOXYGEN_SHOULD_SKIP_THIS
524 template <class Type> class vpUndistortInternalType
525 {
526 public:
527  Type *src;
528  Type *dst;
529  unsigned int width;
530  unsigned int height;
531  vpCameraParameters cam;
532  unsigned int nthreads;
533  unsigned int threadid;
534 
535 public:
536  vpUndistortInternalType() : src(nullptr), dst(nullptr), width(0), height(0), cam(), nthreads(0), threadid(0) { }
537 
538  vpUndistortInternalType(const vpUndistortInternalType<Type> &u) { *this = u; }
539  vpUndistortInternalType &operator=(const vpUndistortInternalType<Type> &u)
540  {
541  src = u.src;
542  dst = u.dst;
543  width = u.width;
544  height = u.height;
545  cam = u.cam;
546  nthreads = u.nthreads;
547  threadid = u.threadid;
548 
549  return *this;
550  }
551 
552  static void vpUndistort_threaded(vpUndistortInternalType<Type> &undistortSharedData);
553 };
554 
555 template <class Type> void vpUndistortInternalType<Type>::vpUndistort_threaded(vpUndistortInternalType<Type> &undistortSharedData)
556 {
557  int offset = (int)undistortSharedData.threadid;
558  int width = (int)undistortSharedData.width;
559  int height = (int)undistortSharedData.height;
560  int nthreads = (int)undistortSharedData.nthreads;
561 
562  double u0 = undistortSharedData.cam.get_u0();
563  double v0 = undistortSharedData.cam.get_v0();
564  double px = undistortSharedData.cam.get_px();
565  double py = undistortSharedData.cam.get_py();
566  double kud = undistortSharedData.cam.get_kud();
567 
568  double invpx = 1.0 / px;
569  double invpy = 1.0 / py;
570 
571  double kud_px2 = kud * invpx * invpx;
572  double kud_py2 = kud * invpy * invpy;
573 
574  Type *dst = undistortSharedData.dst + (height / nthreads * offset) * width;
575  Type *src = undistortSharedData.src;
576 
577  for (double v = height / nthreads * offset; v < height / nthreads * (offset + 1); v++) {
578  double deltav = v - v0;
579  // double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy));
580  double fr1 = 1.0 + kud_py2 * deltav * deltav;
581 
582  for (double u = 0; u < width; u++) {
583  // computation of u,v : corresponding pixel coordinates in I.
584  double deltau = u - u0;
585  // double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx));
586  double fr2 = fr1 + kud_px2 * deltau * deltau;
587 
588  double u_double = deltau * fr2 + u0;
589  double v_double = deltav * fr2 + v0;
590 
591  // computation of the bilinear interpolation
592 
593  // declarations
594  int u_round = (int)(u_double);
595  int v_round = (int)(v_double);
596  if (u_round < 0.f)
597  u_round = -1;
598  if (v_round < 0.f)
599  v_round = -1;
600  double du_double = (u_double)-(double)u_round;
601  double dv_double = (v_double)-(double)v_round;
602  Type v01;
603  Type v23;
604  if ((0 <= u_round) && (0 <= v_round) && (u_round < ((width)-1)) && (v_round < ((height)-1))) {
605  // process interpolation
606  const Type *_mp = &src[v_round * width + u_round];
607  v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
608  _mp += width;
609  v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
610  *dst = (Type)(v01 + ((v23 - v01) * dv_double));
611  }
612  else {
613  *dst = 0;
614  }
615  dst++;
616  }
617  }
618 }
619 #endif // DOXYGEN_SHOULD_SKIP_THIS
620 #endif // VISP_HAVE_THREADS
621 
645 template <class Type>
647  unsigned int nThreads)
648 {
649 #if defined(VISP_HAVE_THREADS)
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  std::vector<std::thread *> threadpool;
669 
670  vpUndistortInternalType<Type> *undistortSharedData = new vpUndistortInternalType<Type>[nthreads];
671 
672  for (unsigned int i = 0; i < nthreads; i++) {
673  // Each thread works on a different set of data.
674  // vpTRACE("create thread %d", i);
675  undistortSharedData[i].src = I.bitmap;
676  undistortSharedData[i].dst = undistI.bitmap;
677  undistortSharedData[i].width = I.getWidth();
678  undistortSharedData[i].height = I.getHeight();
679  undistortSharedData[i].cam = cam;
680  undistortSharedData[i].nthreads = nthreads;
681  undistortSharedData[i].threadid = i;
682  std::thread *undistort_thread = new std::thread(&vpUndistortInternalType<Type>::vpUndistort_threaded, std::ref(undistortSharedData[i]));
683  threadpool.push_back(undistort_thread);
684  }
685  /* Wait on the other threads */
686 
687  for (unsigned int i = 0; i < nthreads; i++) {
688  // vpTRACE("join thread %d", i);
689  threadpool[i]->join();
690  }
691 
692  for (unsigned int i = 0; i < nthreads; i++) {
693  delete threadpool[i];
694  }
695 
696  delete[] undistortSharedData;
697 #else // VISP_HAVE_THREADS
698  (void)nThreads;
699  //
700  // optimized version without pthreads
701  //
702  unsigned int width = I.getWidth();
703  unsigned int height = I.getHeight();
704 
705  undistI.resize(height, width);
706 
707  double u0 = cam.get_u0();
708  double v0 = cam.get_v0();
709  double px = cam.get_px();
710  double py = cam.get_py();
711  double kud = cam.get_kud();
712 
713  // if (kud == 0) {
714  if (std::fabs(kud) <= std::numeric_limits<double>::epsilon()) {
715  // There is no need to undistort the image
716  undistI = I;
717  return;
718  }
719 
720  double invpx = 1.0 / px;
721  double invpy = 1.0 / py;
722 
723  double kud_px2 = kud * invpx * invpx;
724  double kud_py2 = kud * invpy * invpy;
725 
726  Type *dst = undistI.bitmap;
727  for (double v = 0; v < height; v++) {
728  double deltav = v - v0;
729  // double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy));
730  double fr1 = 1.0 + kud_py2 * deltav * deltav;
731 
732  for (double u = 0; u < width; u++) {
733  // computation of u,v : corresponding pixel coordinates in I.
734  double deltau = u - u0;
735  // double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx));
736  double fr2 = fr1 + kud_px2 * deltau * deltau;
737 
738  double u_double = deltau * fr2 + u0;
739  double v_double = deltav * fr2 + v0;
740 
741  // printf("[%g][%g] %g %g : ", u, v, u_double, v_double );
742 
743  // computation of the bilinear interpolation
744 
745  // declarations
746  int u_round = (int)(u_double);
747  int v_round = (int)(v_double);
748  if (u_round < 0.f)
749  u_round = -1;
750  if (v_round < 0.f)
751  v_round = -1;
752  double du_double = (u_double)-(double)u_round;
753  double dv_double = (v_double)-(double)v_round;
754  Type v01;
755  Type v23;
756  if ((0 <= u_round) && (0 <= v_round) && (u_round < (((int)width) - 1)) && (v_round < (((int)height) - 1))) {
757  // process interpolation
758  const Type *_mp = &I[(unsigned int)v_round][(unsigned int)u_round];
759  v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
760  _mp += width;
761  v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
762  *dst = (Type)(v01 + ((v23 - v01) * dv_double));
763  // printf("R %d G %d B %d\n", dst->R, dst->G, dst->B);
764  }
765  else {
766  *dst = 0;
767  }
768  dst++;
769  }
770  }
771 #endif // VISP_HAVE_THREADS
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>
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<int>(0, std::min<int>(x, static_cast<int>(I.getWidth()) - 1));
891  y = std::max<int>(0, std::min<int>(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<int>(static_cast<int>(I.getWidth()) - 1, u0 + 1);
993  int v1 = v0;
994 
995  int u2 = u0;
996  int v2 = std::min<int>(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<int>(static_cast<int>(I.getWidth()) - 1, u0 + 1);
1016  int v1 = v0;
1017 
1018  int u2 = u0;
1019  int v2 = std::min<int>(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>
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  }
1127  else if (method == INTERPOLATION_LINEAR) {
1128  resizeBilinear(I, Ires, static_cast<unsigned int>(i), j, u0, v0, xFrac, yFrac);
1129  }
1130  else if (method == INTERPOLATION_CUBIC) {
1131  resizeBicubic(I, Ires, static_cast<unsigned int>(i), j, u, v, xFrac, yFrac);
1132  }
1133  }
1134  }
1135  }
1136 
1137 #if defined(VISP_HAVE_SIMDLIB)
1138 template <>
1140  const vpImageInterpolationType &method,
1141  unsigned int
1142 #if defined(_OPENMP)
1143  nThreads
1144 #endif
1145 )
1146 {
1147  if (I.getWidth() < 2 || I.getHeight() < 2 || Ires.getWidth() < 2 || Ires.getHeight() < 2) {
1148  std::cerr << "Input or output image is too small!" << std::endl;
1149  return;
1150  }
1151 
1152  if (method == INTERPOLATION_AREA) {
1153  resizeSimdlib(I, Ires.getWidth(), Ires.getHeight(), Ires, INTERPOLATION_AREA);
1154  }
1155  else if (method == INTERPOLATION_LINEAR) {
1156  resizeSimdlib(I, Ires.getWidth(), Ires.getHeight(), Ires, INTERPOLATION_LINEAR);
1157  }
1158  else {
1159  const float scaleY = I.getHeight() / static_cast<float>(Ires.getHeight());
1160  const float scaleX = I.getWidth() / static_cast<float>(Ires.getWidth());
1161  const float half = 0.5f;
1162 
1163 #if defined(_OPENMP)
1164  if (nThreads > 0) {
1165  omp_set_num_threads(static_cast<int>(nThreads));
1166  }
1167 #pragma omp parallel for schedule(dynamic)
1168 #endif
1169  for (int i = 0; i < static_cast<int>(Ires.getHeight()); i++) {
1170  float v = (i + half) * scaleY - half;
1171  float yFrac = v - static_cast<int>(v);
1172 
1173  for (unsigned int j = 0; j < Ires.getWidth(); j++) {
1174  float u = (j + half) * scaleX - half;
1175  float xFrac = u - static_cast<int>(u);
1176 
1177  if (method == INTERPOLATION_NEAREST) {
1178  resizeNearest(I, Ires, static_cast<unsigned int>(i), j, u, v);
1179  }
1180  else if (method == INTERPOLATION_CUBIC) {
1181  resizeBicubic(I, Ires, static_cast<unsigned int>(i), j, u, v, xFrac, yFrac);
1182  }
1183  }
1184  }
1185  }
1186 }
1187 
1188 template <>
1190  const vpImageInterpolationType &method,
1191  unsigned int
1192 #if defined(_OPENMP)
1193  nThreads
1194 #endif
1195 )
1196 {
1197  if (I.getWidth() < 2 || I.getHeight() < 2 || Ires.getWidth() < 2 || Ires.getHeight() < 2) {
1198  std::cerr << "Input or output image is too small!" << std::endl;
1199  return;
1200  }
1201 
1202  if (method == INTERPOLATION_AREA) {
1203  resizeSimdlib(I, Ires.getWidth(), Ires.getHeight(), Ires, INTERPOLATION_AREA);
1204  }
1205  else if (method == INTERPOLATION_LINEAR) {
1206  resizeSimdlib(I, Ires.getWidth(), Ires.getHeight(), Ires, INTERPOLATION_LINEAR);
1207  }
1208  else {
1209  const float scaleY = I.getHeight() / static_cast<float>(Ires.getHeight());
1210  const float scaleX = I.getWidth() / static_cast<float>(Ires.getWidth());
1211  const float half = 0.5f;
1212 
1213 #if defined(_OPENMP)
1214  if (nThreads > 0) {
1215  omp_set_num_threads(static_cast<int>(nThreads));
1216  }
1217 #pragma omp parallel for schedule(dynamic)
1218 #endif
1219  for (int i = 0; i < static_cast<int>(Ires.getHeight()); i++) {
1220  float v = (i + half) * scaleY - half;
1221  float yFrac = v - static_cast<int>(v);
1222 
1223  for (unsigned int j = 0; j < Ires.getWidth(); j++) {
1224  float u = (j + half) * scaleX - half;
1225  float xFrac = u - static_cast<int>(u);
1226 
1227  if (method == INTERPOLATION_NEAREST) {
1228  resizeNearest(I, Ires, static_cast<unsigned int>(i), j, u, v);
1229  }
1230  else if (method == INTERPOLATION_CUBIC) {
1231  resizeBicubic(I, Ires, static_cast<unsigned int>(i), j, u, v, xFrac, yFrac);
1232  }
1233  }
1234  }
1235  }
1236 }
1237 #endif
1238 
1253 template <class Type>
1255  const vpImageInterpolationType &interpolation, bool fixedPointArithmetic, bool pixelCenter)
1256 {
1257  if ((T.getRows() != 2 && T.getRows() != 3) || T.getCols() != 3) {
1258  std::cerr << "Input transformation must be a (2x3) or (3x3) matrix." << std::endl;
1259  return;
1260  }
1261 
1262  if (src.getSize() == 0) {
1263  return;
1264  }
1265 
1266  const bool affine = (T.getRows() == 2);
1267  const bool interp_NN = (interpolation == INTERPOLATION_NEAREST) || (interpolation == INTERPOLATION_CUBIC);
1268 
1269  if (dst.getSize() == 0) {
1270  dst.resize(src.getHeight(), src.getWidth(), Type(0));
1271  }
1272 
1273  vpMatrix M = T;
1274  if (affine) {
1275  double D = M[0][0] * M[1][1] - M[0][1] * M[1][0];
1276  D = !vpMath::nul(D, std::numeric_limits<double>::epsilon()) ? 1.0 / D : 0;
1277  double A11 = M[1][1] * D, A22 = M[0][0] * D;
1278  M[0][0] = A11;
1279  M[0][1] *= -D;
1280  M[1][0] *= -D;
1281  M[1][1] = A22;
1282  double b1 = -M[0][0] * M[0][2] - M[0][1] * M[1][2];
1283  double b2 = -M[1][0] * M[0][2] - M[1][1] * M[1][2];
1284  M[0][2] = b1;
1285  M[1][2] = b2;
1286  }
1287  else {
1288  M = T.inverseByLU();
1289  }
1290 
1291  if (fixedPointArithmetic && !pixelCenter) {
1292  fixedPointArithmetic = checkFixedPoint(0, 0, M, affine) && checkFixedPoint(dst.getWidth() - 1, 0, M, affine) &&
1293  checkFixedPoint(0, dst.getHeight() - 1, M, affine) &&
1294  checkFixedPoint(dst.getWidth() - 1, dst.getHeight() - 1, M, affine);
1295  }
1296 
1297  if (interp_NN) {
1298  // nearest neighbor interpolation
1299  warpNN(src, M, dst, affine, pixelCenter, fixedPointArithmetic);
1300  }
1301  else {
1302  // bilinear interpolation
1303  warpLinear(src, M, dst, affine, pixelCenter, fixedPointArithmetic);
1304  }
1305 }
1306 
1307 template <class Type>
1308 void vpImageTools::warpNN(const vpImage<Type> &src, const vpMatrix &T, vpImage<Type> &dst, bool affine,
1309  bool centerCorner, bool fixedPoint)
1310 {
1311  if (fixedPoint && !centerCorner) {
1312  const int nbits = 16;
1313  const int32_t precision = 1 << nbits;
1314  const float precision_1 = 1 / static_cast<float>(precision);
1315 
1316  int32_t a0_i32 = static_cast<int32_t>(T[0][0] * precision);
1317  int32_t a1_i32 = static_cast<int32_t>(T[0][1] * precision);
1318  int32_t a2_i32 = static_cast<int32_t>(T[0][2] * precision);
1319  int32_t a3_i32 = static_cast<int32_t>(T[1][0] * precision);
1320  int32_t a4_i32 = static_cast<int32_t>(T[1][1] * precision);
1321  int32_t a5_i32 = static_cast<int32_t>(T[1][2] * precision);
1322  int32_t a6_i32 = T.getRows() == 3 ? static_cast<int32_t>(T[2][0] * precision) : 0;
1323  int32_t a7_i32 = T.getRows() == 3 ? static_cast<int32_t>(T[2][1] * precision) : 0;
1324  int32_t a8_i32 = T.getRows() == 3 ? static_cast<int32_t>(T[2][2] * precision) : 1;
1325 
1326  int32_t height_1_i32 = static_cast<int32_t>((src.getHeight() - 1) * precision) + 0x8000;
1327  int32_t width_1_i32 = static_cast<int32_t>((src.getWidth() - 1) * precision) + 0x8000;
1328 
1329  if (affine) {
1330  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1331  int32_t xi = a2_i32;
1332  int32_t yi = a5_i32;
1333 
1334  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1335  if (yi >= 0 && yi < height_1_i32 && xi >= 0 && xi < width_1_i32) {
1336  float x_ = (xi >> nbits) + (xi & 0xFFFF) * precision_1;
1337  float y_ = (yi >> nbits) + (yi & 0xFFFF) * precision_1;
1338 
1339  int x = vpMath::round(x_);
1340  int y = vpMath::round(y_);
1341  dst[i][j] = src[y][x];
1342  }
1343 
1344  xi += a0_i32;
1345  yi += a3_i32;
1346  }
1347 
1348  a2_i32 += a1_i32;
1349  a5_i32 += a4_i32;
1350  }
1351  }
1352  else {
1353  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1354  int64_t xi = a2_i32;
1355  int64_t yi = a5_i32;
1356  int64_t wi = a8_i32;
1357 
1358  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1359  if (wi != 0 && yi >= 0 && yi <= (static_cast<int>(src.getHeight()) - 1) * wi && xi >= 0 &&
1360  xi <= (static_cast<int>(src.getWidth()) - 1) * wi) {
1361  float w_ = (wi >> nbits) + (wi & 0xFFFF) * precision_1;
1362  float x_ = ((xi >> nbits) + (xi & 0xFFFF) * precision_1) / w_;
1363  float y_ = ((yi >> nbits) + (yi & 0xFFFF) * precision_1) / w_;
1364 
1365  int x = vpMath::round(x_);
1366  int y = vpMath::round(y_);
1367 
1368  dst[i][j] = src[y][x];
1369  }
1370 
1371  xi += a0_i32;
1372  yi += a3_i32;
1373  wi += a6_i32;
1374  }
1375 
1376  a2_i32 += a1_i32;
1377  a5_i32 += a4_i32;
1378  a8_i32 += a7_i32;
1379  }
1380  }
1381  }
1382  else {
1383  double a0 = T[0][0];
1384  double a1 = T[0][1];
1385  double a2 = T[0][2];
1386  double a3 = T[1][0];
1387  double a4 = T[1][1];
1388  double a5 = T[1][2];
1389  double a6 = affine ? 0.0 : T[2][0];
1390  double a7 = affine ? 0.0 : T[2][1];
1391  double a8 = affine ? 1.0 : T[2][2];
1392 
1393  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1394  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1395  double x = a0 * (centerCorner ? j + 0.5 : j) + a1 * (centerCorner ? i + 0.5 : i) + a2;
1396  double y = a3 * (centerCorner ? j + 0.5 : j) + a4 * (centerCorner ? i + 0.5 : i) + a5;
1397  double w = a6 * (centerCorner ? j + 0.5 : j) + a7 * (centerCorner ? i + 0.5 : i) + a8;
1398 
1399  if (vpMath::nul(w, std::numeric_limits<double>::epsilon())) {
1400  w = 1.0;
1401  }
1402 
1403  int x_ = centerCorner ? coordCast(x / w) : vpMath::round(x / w);
1404  int y_ = centerCorner ? coordCast(y / w) : vpMath::round(y / w);
1405 
1406  if (x_ >= 0 && x_ < static_cast<int>(src.getWidth()) && y_ >= 0 && y_ < static_cast<int>(src.getHeight())) {
1407  dst[i][j] = src[y_][x_];
1408  }
1409  }
1410  }
1411  }
1412 }
1413 
1414 template <class Type>
1415 void vpImageTools::warpLinear(const vpImage<Type> &src, const vpMatrix &T, vpImage<Type> &dst, bool affine,
1416  bool centerCorner, bool fixedPoint)
1417 {
1418  if (fixedPoint && !centerCorner) {
1419  const int nbits = 16;
1420  const int64_t precision = 1 << nbits;
1421  const float precision_1 = 1 / static_cast<float>(precision);
1422  const int64_t precision2 = 1ULL << (2 * nbits);
1423  const float precision_2 = 1 / static_cast<float>(precision2);
1424 
1425  int64_t a0_i64 = static_cast<int64_t>(T[0][0] * precision);
1426  int64_t a1_i64 = static_cast<int64_t>(T[0][1] * precision);
1427  int64_t a2_i64 = static_cast<int64_t>(T[0][2] * precision);
1428  int64_t a3_i64 = static_cast<int64_t>(T[1][0] * precision);
1429  int64_t a4_i64 = static_cast<int64_t>(T[1][1] * precision);
1430  int64_t a5_i64 = static_cast<int64_t>(T[1][2] * precision);
1431  int64_t a6_i64 = T.getRows() == 3 ? static_cast<int64_t>(T[2][0] * precision) : 0;
1432  int64_t a7_i64 = T.getRows() == 3 ? static_cast<int64_t>(T[2][1] * precision) : 0;
1433  int64_t a8_i64 = T.getRows() == 3 ? static_cast<int64_t>(T[2][2] * precision) : 1;
1434 
1435  int64_t height_i64 = static_cast<int64_t>(src.getHeight() * precision);
1436  int64_t width_i64 = static_cast<int64_t>(src.getWidth() * precision);
1437 
1438  if (affine) {
1439  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1440  int64_t xi_ = a2_i64;
1441  int64_t yi_ = a5_i64;
1442 
1443  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1444  if (yi_ >= 0 && yi_ < height_i64 && xi_ >= 0 && xi_ < width_i64) {
1445  const int64_t xi_lower = xi_ & (~0xFFFF);
1446  const int64_t yi_lower = yi_ & (~0xFFFF);
1447 
1448  const int64_t t = yi_ - yi_lower;
1449  const int64_t t_1 = precision - t;
1450  const int64_t s = xi_ - xi_lower;
1451  const int64_t s_1 = precision - s;
1452 
1453  const int x_ = static_cast<int>(xi_ >> nbits);
1454  const int y_ = static_cast<int>(yi_ >> nbits);
1455 
1456  if (y_ < static_cast<int>(src.getHeight()) - 1 && x_ < static_cast<int>(src.getWidth()) - 1) {
1457  const Type val00 = src[y_][x_];
1458  const Type val01 = src[y_][x_ + 1];
1459  const Type val10 = src[y_ + 1][x_];
1460  const Type val11 = src[y_ + 1][x_ + 1];
1461  const int64_t interp_i64 =
1462  static_cast<int64_t>(s_1 * t_1 * val00 + s * t_1 * val01 + s_1 * t * val10 + s * t * val11);
1463  const float interp = (interp_i64 >> (nbits * 2)) + (interp_i64 & 0xFFFFFFFF) * precision_2;
1464  dst[i][j] = vpMath::saturate<Type>(interp);
1465  }
1466  else if (y_ < static_cast<int>(src.getHeight()) - 1) {
1467  const Type val00 = src[y_][x_];
1468  const Type val10 = src[y_ + 1][x_];
1469  const int64_t interp_i64 = static_cast<int64_t>(t_1 * val00 + t * val10);
1470  const float interp = (interp_i64 >> nbits) + (interp_i64 & 0xFFFF) * precision_1;
1471  dst[i][j] = vpMath::saturate<Type>(interp);
1472  }
1473  else if (x_ < static_cast<int>(src.getWidth()) - 1) {
1474  const Type val00 = src[y_][x_];
1475  const Type val01 = src[y_][x_ + 1];
1476  const int64_t interp_i64 = static_cast<int64_t>(s_1 * val00 + s * val01);
1477  const float interp = (interp_i64 >> nbits) + (interp_i64 & 0xFFFF) * precision_1;
1478  dst[i][j] = vpMath::saturate<Type>(interp);
1479  }
1480  else {
1481  dst[i][j] = src[y_][x_];
1482  }
1483  }
1484 
1485  xi_ += a0_i64;
1486  yi_ += a3_i64;
1487  }
1488 
1489  a2_i64 += a1_i64;
1490  a5_i64 += a4_i64;
1491  }
1492  }
1493  else {
1494  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1495  int64_t xi = a2_i64;
1496  int64_t yi = a5_i64;
1497  int64_t wi = a8_i64;
1498 
1499  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1500  if (wi != 0 && yi >= 0 && yi <= (static_cast<int>(src.getHeight()) - 1) * wi && xi >= 0 &&
1501  xi <= (static_cast<int>(src.getWidth()) - 1) * wi) {
1502  const float wi_ = (wi >> nbits) + (wi & 0xFFFF) * precision_1;
1503  const float xi_ = ((xi >> nbits) + (xi & 0xFFFF) * precision_1) / wi_;
1504  const float yi_ = ((yi >> nbits) + (yi & 0xFFFF) * precision_1) / wi_;
1505 
1506  const int x_ = static_cast<int>(xi_);
1507  const int y_ = static_cast<int>(yi_);
1508 
1509  const float t = yi_ - y_;
1510  const float s = xi_ - x_;
1511 
1512  if (y_ < static_cast<int>(src.getHeight()) - 1 && x_ < static_cast<int>(src.getWidth()) - 1) {
1513  const Type val00 = src[y_][x_];
1514  const Type val01 = src[y_][x_ + 1];
1515  const Type val10 = src[y_ + 1][x_];
1516  const Type val11 = src[y_ + 1][x_ + 1];
1517  const float col0 = lerp(val00, val01, s);
1518  const float col1 = lerp(val10, val11, s);
1519  const float interp = lerp(col0, col1, t);
1520  dst[i][j] = vpMath::saturate<Type>(interp);
1521  }
1522  else if (y_ < static_cast<int>(src.getHeight()) - 1) {
1523  const Type val00 = src[y_][x_];
1524  const Type val10 = src[y_ + 1][x_];
1525  const float interp = lerp(val00, val10, t);
1526  dst[i][j] = vpMath::saturate<Type>(interp);
1527  }
1528  else if (x_ < static_cast<int>(src.getWidth()) - 1) {
1529  const Type val00 = src[y_][x_];
1530  const Type val01 = src[y_][x_ + 1];
1531  const float interp = lerp(val00, val01, s);
1532  dst[i][j] = vpMath::saturate<Type>(interp);
1533  }
1534  else {
1535  dst[i][j] = src[y_][x_];
1536  }
1537  }
1538 
1539  xi += a0_i64;
1540  yi += a3_i64;
1541  wi += a6_i64;
1542  }
1543 
1544  a2_i64 += a1_i64;
1545  a5_i64 += a4_i64;
1546  a8_i64 += a7_i64;
1547  }
1548  }
1549  }
1550  else {
1551  double a0 = T[0][0];
1552  double a1 = T[0][1];
1553  double a2 = T[0][2];
1554  double a3 = T[1][0];
1555  double a4 = T[1][1];
1556  double a5 = T[1][2];
1557  double a6 = affine ? 0.0 : T[2][0];
1558  double a7 = affine ? 0.0 : T[2][1];
1559  double a8 = affine ? 1.0 : T[2][2];
1560 
1561  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1562  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1563  double x = a0 * (centerCorner ? j + 0.5 : j) + a1 * (centerCorner ? i + 0.5 : i) + a2;
1564  double y = a3 * (centerCorner ? j + 0.5 : j) + a4 * (centerCorner ? i + 0.5 : i) + a5;
1565  double w = a6 * (centerCorner ? j + 0.5 : j) + a7 * (centerCorner ? i + 0.5 : i) + a8;
1566  if (vpMath::nul(w, std::numeric_limits<double>::epsilon())) {
1567  w = 1;
1568  }
1569 
1570  x = x / w - (centerCorner ? 0.5 : 0);
1571  y = y / w - (centerCorner ? 0.5 : 0);
1572 
1573  int x_lower = static_cast<int>(x);
1574  int y_lower = static_cast<int>(y);
1575 
1576  if (y_lower >= static_cast<int>(src.getHeight()) || x_lower >= static_cast<int>(src.getWidth()) || y < 0 ||
1577  x < 0) {
1578  continue;
1579  }
1580 
1581  double s = x - x_lower;
1582  double t = y - y_lower;
1583 
1584  if (y_lower < static_cast<int>(src.getHeight()) - 1 && x_lower < static_cast<int>(src.getWidth()) - 1) {
1585  const Type val00 = src[y_lower][x_lower];
1586  const Type val01 = src[y_lower][x_lower + 1];
1587  const Type val10 = src[y_lower + 1][x_lower];
1588  const Type val11 = src[y_lower + 1][x_lower + 1];
1589  const double col0 = lerp(val00, val01, s);
1590  const double col1 = lerp(val10, val11, s);
1591  const double interp = lerp(col0, col1, t);
1592  dst[i][j] = vpMath::saturate<Type>(interp);
1593  }
1594  else if (y_lower < static_cast<int>(src.getHeight()) - 1) {
1595  const Type val00 = src[y_lower][x_lower];
1596  const Type val10 = src[y_lower + 1][x_lower];
1597  const double interp = lerp(val00, val10, t);
1598  dst[i][j] = vpMath::saturate<Type>(interp);
1599  }
1600  else if (x_lower < static_cast<int>(src.getWidth()) - 1) {
1601  const Type val00 = src[y_lower][x_lower];
1602  const Type val01 = src[y_lower][x_lower + 1];
1603  const double interp = lerp(val00, val01, s);
1604  dst[i][j] = vpMath::saturate<Type>(interp);
1605  }
1606  else {
1607  dst[i][j] = src[y_lower][x_lower];
1608  }
1609  }
1610  }
1611  }
1612 }
1613 
1614 template <>
1615 inline void vpImageTools::warpLinear(const vpImage<vpRGBa> &src, const vpMatrix &T, vpImage<vpRGBa> &dst, bool affine,
1616  bool centerCorner, bool fixedPoint)
1617 {
1618  if (fixedPoint && !centerCorner) {
1619  const int nbits = 16;
1620  const int64_t precision = 1 << nbits;
1621  const float precision_1 = 1 / static_cast<float>(precision);
1622  const int64_t precision2 = 1ULL << (2 * nbits);
1623  const float precision_2 = 1 / static_cast<float>(precision2);
1624 
1625  int64_t a0_i64 = static_cast<int64_t>(T[0][0] * precision);
1626  int64_t a1_i64 = static_cast<int64_t>(T[0][1] * precision);
1627  int64_t a2_i64 = static_cast<int64_t>(T[0][2] * precision);
1628  int64_t a3_i64 = static_cast<int64_t>(T[1][0] * precision);
1629  int64_t a4_i64 = static_cast<int64_t>(T[1][1] * precision);
1630  int64_t a5_i64 = static_cast<int64_t>(T[1][2] * precision);
1631  int64_t a6_i64 = T.getRows() == 3 ? static_cast<int64_t>(T[2][0] * precision) : 0;
1632  int64_t a7_i64 = T.getRows() == 3 ? static_cast<int64_t>(T[2][1] * precision) : 0;
1633  int64_t a8_i64 = precision;
1634 
1635  int64_t height_i64 = static_cast<int64_t>(src.getHeight() * precision);
1636  int64_t width_i64 = static_cast<int64_t>(src.getWidth() * precision);
1637 
1638  if (affine) {
1639  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1640  int64_t xi = a2_i64;
1641  int64_t yi = a5_i64;
1642 
1643  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1644  if (yi >= 0 && yi < height_i64 && xi >= 0 && xi < width_i64) {
1645  const int64_t xi_lower = xi & (~0xFFFF);
1646  const int64_t yi_lower = yi & (~0xFFFF);
1647 
1648  const int64_t t = yi - yi_lower;
1649  const int64_t t_1 = precision - t;
1650  const int64_t s = xi - xi_lower;
1651  const int64_t s_1 = precision - s;
1652 
1653  const int x_ = static_cast<int>(xi >> nbits);
1654  const int y_ = static_cast<int>(yi >> nbits);
1655 
1656  if (y_ < static_cast<int>(src.getHeight()) - 1 && x_ < static_cast<int>(src.getWidth()) - 1) {
1657  const vpRGBa val00 = src[y_][x_];
1658  const vpRGBa val01 = src[y_][x_ + 1];
1659  const vpRGBa val10 = src[y_ + 1][x_];
1660  const vpRGBa val11 = src[y_ + 1][x_ + 1];
1661  const int64_t interpR_i64 =
1662  static_cast<int64_t>(s_1 * t_1 * val00.R + s * t_1 * val01.R + s_1 * t * val10.R + s * t * val11.R);
1663  const float interpR = (interpR_i64 >> (nbits * 2)) + (interpR_i64 & 0xFFFFFFFF) * precision_2;
1664 
1665  const int64_t interpG_i64 =
1666  static_cast<int64_t>(s_1 * t_1 * val00.G + s * t_1 * val01.G + s_1 * t * val10.G + s * t * val11.G);
1667  const float interpG = (interpG_i64 >> (nbits * 2)) + (interpG_i64 & 0xFFFFFFFF) * precision_2;
1668 
1669  const int64_t interpB_i64 =
1670  static_cast<int64_t>(s_1 * t_1 * val00.B + s * t_1 * val01.B + s_1 * t * val10.B + s * t * val11.B);
1671  const float interpB = (interpB_i64 >> (nbits * 2)) + (interpB_i64 & 0xFFFFFFFF) * precision_2;
1672 
1673  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR), vpMath::saturate<unsigned char>(interpG),
1674  vpMath::saturate<unsigned char>(interpB), 255);
1675  }
1676  else if (y_ < static_cast<int>(src.getHeight()) - 1) {
1677  const vpRGBa val00 = src[y_][x_];
1678  const vpRGBa val10 = src[y_ + 1][x_];
1679  const int64_t interpR_i64 = static_cast<int64_t>(t_1 * val00.R + t * val10.R);
1680  const float interpR = (interpR_i64 >> nbits) + (interpR_i64 & 0xFFFF) * precision_1;
1681 
1682  const int64_t interpG_i64 = static_cast<int64_t>(t_1 * val00.G + t * val10.G);
1683  const float interpG = (interpG_i64 >> nbits) + (interpG_i64 & 0xFFFF) * precision_1;
1684 
1685  const int64_t interpB_i64 = static_cast<int64_t>(t_1 * val00.B + t * val10.B);
1686  const float interpB = (interpB_i64 >> nbits) + (interpB_i64 & 0xFFFF) * precision_1;
1687 
1688  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR), vpMath::saturate<unsigned char>(interpG),
1689  vpMath::saturate<unsigned char>(interpB), 255);
1690  }
1691  else if (x_ < static_cast<int>(src.getWidth()) - 1) {
1692  const vpRGBa val00 = src[y_][x_];
1693  const vpRGBa val01 = src[y_][x_ + 1];
1694  const int64_t interpR_i64 = static_cast<int64_t>(s_1 * val00.R + s * val01.R);
1695  const float interpR = (interpR_i64 >> nbits) + (interpR_i64 & 0xFFFF) * precision_1;
1696 
1697  const int64_t interpG_i64 = static_cast<int64_t>(s_1 * val00.G + s * val01.G);
1698  const float interpG = (interpG_i64 >> nbits) + (interpG_i64 & 0xFFFF) * precision_1;
1699 
1700  const int64_t interpB_i64 = static_cast<int64_t>(s_1 * val00.B + s * val01.B);
1701  const float interpB = (interpB_i64 >> nbits) + (interpB_i64 & 0xFFFF) * precision_1;
1702 
1703  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR), vpMath::saturate<unsigned char>(interpG),
1704  vpMath::saturate<unsigned char>(interpB), 255);
1705  }
1706  else {
1707  dst[i][j] = src[y_][x_];
1708  }
1709  }
1710 
1711  xi += a0_i64;
1712  yi += a3_i64;
1713  }
1714 
1715  a2_i64 += a1_i64;
1716  a5_i64 += a4_i64;
1717  }
1718  }
1719  else {
1720  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1721  int64_t xi = a2_i64;
1722  int64_t yi = a5_i64;
1723  int64_t wi = a8_i64;
1724 
1725  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1726  if (yi >= 0 && yi <= (static_cast<int>(src.getHeight()) - 1) * wi && xi >= 0 &&
1727  xi <= (static_cast<int>(src.getWidth()) - 1) * wi) {
1728  const float wi_ = (wi >> nbits) + (wi & 0xFFFF) * precision_1;
1729  const float xi_ = ((xi >> nbits) + (xi & 0xFFFF) * precision_1) / wi_;
1730  const float yi_ = ((yi >> nbits) + (yi & 0xFFFF) * precision_1) / wi_;
1731 
1732  const int x_ = static_cast<int>(xi_);
1733  const int y_ = static_cast<int>(yi_);
1734 
1735  const float t = yi_ - y_;
1736  const float s = xi_ - x_;
1737 
1738  if (y_ < static_cast<int>(src.getHeight()) - 1 && x_ < static_cast<int>(src.getWidth()) - 1) {
1739  const vpRGBa val00 = src[y_][x_];
1740  const vpRGBa val01 = src[y_][x_ + 1];
1741  const vpRGBa val10 = src[y_ + 1][x_];
1742  const vpRGBa val11 = src[y_ + 1][x_ + 1];
1743  const float colR0 = lerp(val00.R, val01.R, s);
1744  const float colR1 = lerp(val10.R, val11.R, s);
1745  const float interpR = lerp(colR0, colR1, t);
1746 
1747  const float colG0 = lerp(val00.G, val01.G, s);
1748  const float colG1 = lerp(val10.G, val11.G, s);
1749  const float interpG = lerp(colG0, colG1, t);
1750 
1751  const float colB0 = lerp(val00.B, val01.B, s);
1752  const float colB1 = lerp(val10.B, val11.B, s);
1753  const float interpB = lerp(colB0, colB1, t);
1754 
1755  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR), vpMath::saturate<unsigned char>(interpG),
1756  vpMath::saturate<unsigned char>(interpB), 255);
1757  }
1758  else if (y_ < static_cast<int>(src.getHeight()) - 1) {
1759  const vpRGBa val00 = src[y_][x_];
1760  const vpRGBa val10 = src[y_ + 1][x_];
1761  const float interpR = lerp(val00.R, val10.R, t);
1762  const float interpG = lerp(val00.G, val10.G, t);
1763  const float interpB = lerp(val00.B, val10.B, t);
1764 
1765  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR), vpMath::saturate<unsigned char>(interpG),
1766  vpMath::saturate<unsigned char>(interpB), 255);
1767  }
1768  else if (x_ < static_cast<int>(src.getWidth()) - 1) {
1769  const vpRGBa val00 = src[y_][x_];
1770  const vpRGBa val01 = src[y_][x_ + 1];
1771  const float interpR = lerp(val00.R, val01.R, s);
1772  const float interpG = lerp(val00.G, val01.G, s);
1773  const float interpB = lerp(val00.B, val01.B, s);
1774 
1775  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR), vpMath::saturate<unsigned char>(interpG),
1776  vpMath::saturate<unsigned char>(interpB), 255);
1777  }
1778  else {
1779  dst[i][j] = src[y_][x_];
1780  }
1781  }
1782 
1783  xi += a0_i64;
1784  yi += a3_i64;
1785  wi += a6_i64;
1786  }
1787 
1788  a2_i64 += a1_i64;
1789  a5_i64 += a4_i64;
1790  a8_i64 += a7_i64;
1791  }
1792  }
1793  }
1794  else {
1795  double a0 = T[0][0];
1796  double a1 = T[0][1];
1797  double a2 = T[0][2];
1798  double a3 = T[1][0];
1799  double a4 = T[1][1];
1800  double a5 = T[1][2];
1801  double a6 = affine ? 0.0 : T[2][0];
1802  double a7 = affine ? 0.0 : T[2][1];
1803  double a8 = affine ? 1.0 : T[2][2];
1804 
1805  for (unsigned int i = 0; i < dst.getHeight(); i++) {
1806  for (unsigned int j = 0; j < dst.getWidth(); j++) {
1807  double x = a0 * (centerCorner ? j + 0.5 : j) + a1 * (centerCorner ? i + 0.5 : i) + a2;
1808  double y = a3 * (centerCorner ? j + 0.5 : j) + a4 * (centerCorner ? i + 0.5 : i) + a5;
1809  double w = a6 * (centerCorner ? j + 0.5 : j) + a7 * (centerCorner ? i + 0.5 : i) + a8;
1810 
1811  x = x / w - (centerCorner ? 0.5 : 0);
1812  y = y / w - (centerCorner ? 0.5 : 0);
1813 
1814  int x_lower = static_cast<int>(x);
1815  int y_lower = static_cast<int>(y);
1816 
1817  if (y_lower >= static_cast<int>(src.getHeight()) || x_lower >= static_cast<int>(src.getWidth()) || y < 0 ||
1818  x < 0) {
1819  continue;
1820  }
1821 
1822  double s = x - x_lower;
1823  double t = y - y_lower;
1824 
1825  if (y_lower < static_cast<int>(src.getHeight()) - 1 && x_lower < static_cast<int>(src.getWidth()) - 1) {
1826  const vpRGBa val00 = src[y_lower][x_lower];
1827  const vpRGBa val01 = src[y_lower][x_lower + 1];
1828  const vpRGBa val10 = src[y_lower + 1][x_lower];
1829  const vpRGBa val11 = src[y_lower + 1][x_lower + 1];
1830  const double colR0 = lerp(val00.R, val01.R, s);
1831  const double colR1 = lerp(val10.R, val11.R, s);
1832  const double interpR = lerp(colR0, colR1, t);
1833 
1834  const double colG0 = lerp(val00.G, val01.G, s);
1835  const double colG1 = lerp(val10.G, val11.G, s);
1836  const double interpG = lerp(colG0, colG1, t);
1837 
1838  const double colB0 = lerp(val00.B, val01.B, s);
1839  const double colB1 = lerp(val10.B, val11.B, s);
1840  const double interpB = lerp(colB0, colB1, t);
1841 
1842  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR), vpMath::saturate<unsigned char>(interpG),
1843  vpMath::saturate<unsigned char>(interpB), 255);
1844  }
1845  else if (y_lower < static_cast<int>(src.getHeight()) - 1) {
1846  const vpRGBa val00 = src[y_lower][x_lower];
1847  const vpRGBa val10 = src[y_lower + 1][x_lower];
1848  const double interpR = lerp(val00.R, val10.R, t);
1849  const double interpG = lerp(val00.G, val10.G, t);
1850  const double interpB = lerp(val00.B, val10.B, t);
1851 
1852  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR), vpMath::saturate<unsigned char>(interpG),
1853  vpMath::saturate<unsigned char>(interpB), 255);
1854  }
1855  else if (x_lower < static_cast<int>(src.getWidth()) - 1) {
1856  const vpRGBa val00 = src[y_lower][x_lower];
1857  const vpRGBa val01 = src[y_lower][x_lower + 1];
1858  const double interpR = lerp(val00.R, val01.R, s);
1859  const double interpG = lerp(val00.G, val01.G, s);
1860  const double interpB = lerp(val00.B, val01.B, s);
1861 
1862  dst[i][j] = vpRGBa(vpMath::saturate<unsigned char>(interpR), vpMath::saturate<unsigned char>(interpG),
1863  vpMath::saturate<unsigned char>(interpB), 255);
1864  }
1865  else {
1866  dst[i][j] = src[y_lower][x_lower];
1867  }
1868  }
1869  }
1870  }
1871 }
1872 
1873 #endif
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:125
unsigned int getCols() const
Definition: vpArray2D.h:274
unsigned int getRows() const
Definition: vpArray2D.h:284
Generic class defining intrinsic camera parameters.
double get_kud() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
double get_j() const
Definition: vpImagePoint.h:125
double get_i() const
Definition: vpImagePoint.h:114
Various image tools; sub-image extraction, modification of the look up table, binarisation....
Definition: vpImageTools.h:74
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:304
static void flip(const vpImage< Type > &I, vpImage< Type > &newI)
Definition: vpImageTools.h:831
static void warpImage(const vpImage< Type > &src, const vpMatrix &T, vpImage< Type > &dst, const vpImageInterpolationType &interpolation=INTERPOLATION_NEAREST, bool fixedPointArithmetic=true, bool pixelCenter=false)
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)
static vp_deprecated void createSubImage(const vpImage< Type > &I, const vpRect &rect, vpImage< Type > &S)
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)
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)
static void binarise(vpImage< Type > &I, Type threshold1, Type threshold2, Type value1, Type value2, Type value3, bool useLUT=true)
Definition: vpImageTools.h:462
@ INTERPOLATION_LINEAR
Definition: vpImageTools.h:79
@ INTERPOLATION_NEAREST
Definition: vpImageTools.h:78
static void undistort(const vpImage< Type > &I, const vpCameraParameters &cam, vpImage< Type > &newI, unsigned int nThreads=2)
Definition: vpImageTools.h:646
Definition of the vpImage class member functions.
Definition: vpImage.h:69
unsigned int getWidth() const
Definition: vpImage.h:242
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:780
void performLut(const Type(&lut)[256], unsigned int nbThreads=1)
Definition: vpImage.h:1992
unsigned int getSize() const
Definition: vpImage.h:223
Type * bitmap
points toward the bitmap
Definition: vpImage.h:139
unsigned int getHeight() const
Definition: vpImage.h:184
Provides simple mathematics computation tools that are not available in the C mathematics library (ma...
Definition: vpMath.h:109
static double sqr(double x)
Definition: vpMath.h:201
static bool nul(double x, double threshold=0.001)
Definition: vpMath.h:440
static int round(double x)
Definition: vpMath.h:403
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
vpMatrix inverseByLU() const
Definition: vpRGBa.h:61
unsigned char B
Blue component.
Definition: vpRGBa.h:139
unsigned char R
Red component.
Definition: vpRGBa.h:137
unsigned char G
Green component.
Definition: vpRGBa.h:138
Defines an oriented rectangle in the plane.
Defines a rectangle in the plane.
Definition: vpRect.h:76
double getWidth() const
Definition: vpRect.h:224
double getLeft() const
Definition: vpRect.h:170
double getHeight() const
Definition: vpRect.h:163
double getTop() const
Definition: vpRect.h:189
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:107