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