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