Visual Servoing Platform  version 3.6.1 under development (2024-10-15)
vpImageTools.h
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 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 
41 #ifndef VP_IMAGE_TOOLS_H
42 #define VP_IMAGE_TOOLS_H
43 
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 
66 BEGIN_VISP_NAMESPACE
75 class VISP_EXPORT vpImageTools
76 {
77 public:
79  {
83  INTERPOLATION_AREA
84  };
85 
86  template <class Type>
87  static inline void binarise(vpImage<Type> &I, Type threshold1, Type threshold2, Type value1, Type value2, Type value3,
88  bool useLUT = true);
89  static void changeLUT(vpImage<unsigned char> &I, unsigned char A, unsigned char newA, unsigned char B,
90  unsigned char newB);
91 
92  template <class Type>
93  static void crop(const vpImage<Type> &I, double roi_top, double roi_left, unsigned int roi_height,
94  unsigned int roi_width, vpImage<Type> &crop, unsigned int v_scale = 1, unsigned int h_scale = 1);
95 
96  static void columnMean(const vpImage<double> &I, vpRowVector &result);
97 
98  template <class Type>
99  static void crop(const vpImage<Type> &I, const vpImagePoint &topLeft, unsigned int roi_height, unsigned int roi_width,
100  vpImage<Type> &crop, unsigned int v_scale = 1, unsigned int h_scale = 1);
101  template <class Type>
102  static void crop(const vpImage<Type> &I, const vpRect &roi, vpImage<Type> &crop, unsigned int v_scale = 1,
103  unsigned int h_scale = 1);
104  template <class Type>
105  static void crop(const unsigned char *bitmap, unsigned int width, unsigned int height, const vpRect &roi,
106  vpImage<Type> &crop, unsigned int v_scale = 1, unsigned int h_scale = 1);
107 
108  static void extract(const vpImage<unsigned char> &src, vpImage<unsigned char> &dst, const vpRectOriented &r);
109  static void extract(const vpImage<unsigned char> &src, vpImage<double> &dst, const vpRectOriented &r);
110 
111  template <class Type> static void flip(const vpImage<Type> &I, vpImage<Type> &newI);
112 
113  template <class Type> static void flip(vpImage<Type> &I);
114 
115  static void imageDifference(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
116  vpImage<unsigned char> &Idiff);
117  static void imageDifference(const vpImage<vpRGBa> &I1, const vpImage<vpRGBa> &I2, vpImage<vpRGBa> &Idiff);
118 
119  static void imageDifferenceAbsolute(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
120  vpImage<unsigned char> &Idiff);
121  static void imageDifferenceAbsolute(const vpImage<double> &I1, const vpImage<double> &I2, vpImage<double> &Idiff);
122  static void imageDifferenceAbsolute(const vpImage<vpRGBa> &I1, const vpImage<vpRGBa> &I2, vpImage<vpRGBa> &Idiff);
123 
124  static void imageAdd(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2, vpImage<unsigned char> &Ires,
125  bool saturate = false);
126 
127  static void imageSubtract(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
128  vpImage<unsigned char> &Ires, bool saturate = false);
129 
130  static int inMask(const vpImage<unsigned char> &I, const vpImage<unsigned char> &mask, vpImage<unsigned char> &I_mask);
131  static int inMask(const vpImage<vpRGBa> &I, const vpImage<unsigned char> &mask, vpImage<vpRGBa> &I_mask);
132 
133  static int inRange(const unsigned char *hue, const unsigned char *saturation, const unsigned char *value,
134  const vpColVector &hsv_range, unsigned char *mask, unsigned int size);
135  static int inRange(const unsigned char *hue, const unsigned char *saturation, const unsigned char *value,
136  const std::vector<int> &hsv_range, unsigned char *mask, unsigned int size);
137  static void initUndistortMap(const vpCameraParameters &cam, unsigned int width, unsigned int height,
138  vpArray2D<int> &mapU, vpArray2D<int> &mapV, vpArray2D<float> &mapDu,
139  vpArray2D<float> &mapDv);
140 
141  static double interpolate(const vpImage<unsigned char> &I, const vpImagePoint &point,
142  const vpImageInterpolationType &method = INTERPOLATION_NEAREST);
143 
144  static void integralImage(const vpImage<unsigned char> &I, vpImage<double> &II, vpImage<double> &IIsq);
145 
146  static double normalizedCorrelation(const vpImage<double> &I1, const vpImage<double> &I2, bool useOptimized = true);
147 
148  static void normalize(vpImage<double> &I);
149 
150  static void remap(const vpImage<unsigned char> &I, const vpArray2D<int> &mapU, const vpArray2D<int> &mapV,
151  const vpArray2D<float> &mapDu, const vpArray2D<float> &mapDv, vpImage<unsigned char> &Iundist);
152  static void remap(const vpImage<vpRGBa> &I, const vpArray2D<int> &mapU, const vpArray2D<int> &mapV,
153  const vpArray2D<float> &mapDu, const vpArray2D<float> &mapDv, vpImage<vpRGBa> &Iundist);
154 
155  template <class Type>
156  static void resize(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int width, unsigned int height,
157  const vpImageInterpolationType &method = INTERPOLATION_NEAREST, unsigned int nThreads = 0);
158 
159  template <class Type>
160  static void resize(const vpImage<Type> &I, vpImage<Type> &Ires,
161  const vpImageInterpolationType &method = INTERPOLATION_NEAREST, unsigned int nThreads = 0);
162 
163  static void templateMatching(const vpImage<unsigned char> &I, const vpImage<unsigned char> &I_tpl,
164  vpImage<double> &I_score, unsigned int step_u, unsigned int step_v,
165  bool useOptimized = true);
166 
167  template <class Type>
168  static void undistort(const vpImage<Type> &I, const vpCameraParameters &cam, vpImage<Type> &newI,
169  unsigned int nThreads = 2);
170 
171  template <class Type>
172  static void undistort(const vpImage<Type> &I, vpArray2D<int> mapU, vpArray2D<int> mapV, vpArray2D<float> mapDu,
173  vpArray2D<float> mapDv, vpImage<Type> &newI);
174 
175  template <class Type>
176  static void warpImage(const vpImage<Type> &src, const vpMatrix &T, vpImage<Type> &dst,
177  const vpImageInterpolationType &interpolation = INTERPOLATION_NEAREST,
178  bool fixedPointArithmetic = true, bool pixelCenter = false);
179 
180 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
185  template <class Type>
186  VP_DEPRECATED static void createSubImage(const vpImage<Type> &I, unsigned int i_sub, unsigned int j_sub,
187  unsigned int nrow_sub, unsigned int ncol_sub, vpImage<Type> &S);
188 
189  template <class Type>
190  VP_DEPRECATED static void createSubImage(const vpImage<Type> &I, const vpRect &rect, vpImage<Type> &S);
192 #endif
193 
194 private:
195  // Cubic interpolation
196  static float cubicHermite(const float A, const float B, const float C, const float D, const float t);
197 
198  template <class Type> static Type getPixelClamped(const vpImage<Type> &I, float u, float v);
199 
200  static int coordCast(double x);
201 
202  // Linear interpolation
203  static double lerp(double A, double B, double t);
204  static float lerp(float A, float B, float t);
205  static int64_t lerp2(int64_t A, int64_t B, int64_t t, int64_t t_1);
206 
207  static double normalizedCorrelation(const vpImage<double> &I1, const vpImage<double> &I2, const vpImage<double> &II,
208  const vpImage<double> &IIsq, const vpImage<double> &II_tpl,
209  const vpImage<double> &IIsq_tpl, unsigned int i0, unsigned int j0);
210 
211  template <class Type>
212  static void resizeBicubic(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int i, unsigned int j, float u,
213  float v, float xFrac, float yFrac);
214 
215  template <class Type>
216  static void resizeBilinear(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int i, unsigned int j, float u,
217  float v, float xFrac, float yFrac);
218 
219  template <class Type>
220  static void resizeNearest(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int i, unsigned int j, float u,
221  float v);
222 
223 #if defined(VISP_HAVE_SIMDLIB)
224  static void resizeSimdlib(const vpImage<vpRGBa> &Isrc, unsigned int resizeWidth, unsigned int resizeHeight,
225  vpImage<vpRGBa> &Idst, int method);
226  static void resizeSimdlib(const vpImage<unsigned char> &Isrc, unsigned int resizeWidth, unsigned int resizeHeight,
227  vpImage<unsigned char> &Idst, int method);
228 #endif
229 
230  template <class Type>
231  static void warpNN(const vpImage<Type> &src, const vpMatrix &T, vpImage<Type> &dst, bool affine, bool centerCorner,
232  bool fixedPoint);
233 
234  template <class Type>
235  static void warpLinear(const vpImage<Type> &src, const vpMatrix &T, vpImage<Type> &dst, bool affine,
236  bool centerCorner, bool fixedPoint);
237 
238  static bool checkFixedPoint(unsigned int x, unsigned int y, const vpMatrix &T, bool affine);
239 
240  static void warpLinearFixedPointNotCenter(const vpImage<vpRGBa> &src, const vpMatrix &T, vpImage<vpRGBa> &dst, bool affine);
241 };
242 
243 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
263 template <class Type>
264 void vpImageTools::createSubImage(const vpImage<Type> &I, unsigned int roi_top, unsigned int roi_left,
265  unsigned int roi_height, unsigned int roi_width, vpImage<Type> &crop)
266 {
267  vpImageTools::crop(I, roi_top, roi_left, roi_height, roi_width, crop);
268 }
269 
285 template <class Type> void vpImageTools::createSubImage(const vpImage<Type> &I, const vpRect &roi, vpImage<Type> &crop)
286 {
287  vpImageTools::crop(I, roi, crop);
288 }
289 
290 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
291 
314 template <class Type>
315 void vpImageTools::crop(const vpImage<Type> &I, double roi_top, double roi_left, unsigned int roi_height,
316  unsigned int roi_width, vpImage<Type> &crop, unsigned int v_scale, unsigned int h_scale)
317 {
318  int i_min = std::max<int>(static_cast<int>(ceil(roi_top / v_scale)), 0);
319  int j_min = std::max<int>(static_cast<int>(ceil(roi_left / h_scale)), 0);
320  int i_max = std::min<int>(static_cast<int>(ceil(roi_top + roi_height) / v_scale), static_cast<int>(I.getHeight() / v_scale));
321  int j_max = std::min<int>(static_cast<int>(ceil((roi_left + roi_width) / h_scale)), static_cast<int>(I.getWidth() / h_scale));
322 
323  unsigned int i_min_u = static_cast<unsigned int>(i_min);
324  unsigned int j_min_u = static_cast<unsigned int>(j_min);
325 
326  unsigned int r_width = static_cast<unsigned int>(j_max - j_min);
327  unsigned int r_height = static_cast<unsigned int>(i_max - i_min);
328 
329  crop.resize(r_height, r_width);
330 
331  if ((v_scale == 1) && (h_scale == 1)) {
332  for (unsigned int i = 0; i < r_height; ++i) {
333  void *src = (void *)(I[i + i_min_u] + j_min_u);
334  void *dst = (void *)(crop[i]);
335  memcpy(dst, src, r_width * sizeof(Type));
336  }
337  }
338  else if (h_scale == 1) {
339  for (unsigned int i = 0; i < r_height; ++i) {
340  void *src = (void *)(I[(i + i_min_u) * v_scale] + j_min_u);
341  void *dst = (void *)(crop[i]);
342  memcpy(dst, src, r_width * sizeof(Type));
343  }
344  }
345  else {
346  for (unsigned int i = 0; i < r_height; ++i) {
347  for (unsigned int j = 0; j < r_width; ++j) {
348  crop[i][j] = I[(i + i_min_u) * v_scale][(j + j_min_u) * h_scale];
349  }
350  }
351  }
352 }
353 
371 template <class Type>
372 void vpImageTools::crop(const vpImage<Type> &I, const vpImagePoint &topLeft, unsigned int roi_height,
373  unsigned int roi_width, vpImage<Type> &crop, unsigned int v_scale, unsigned int h_scale)
374 {
375  vpImageTools::crop(I, topLeft.get_i(), topLeft.get_j(), roi_height, roi_width, crop, v_scale, h_scale);
376 }
377 
394 template <class Type>
395 void vpImageTools::crop(const vpImage<Type> &I, const vpRect &roi, vpImage<Type> &crop, unsigned int v_scale,
396  unsigned int h_scale)
397 {
398  vpImageTools::crop(I, roi.getTop(), roi.getLeft(), static_cast<unsigned int>(roi.getHeight()), static_cast<unsigned int>(roi.getWidth()), crop,
399  v_scale, h_scale);
400 }
401 
419 template <class Type>
420 void vpImageTools::crop(const unsigned char *bitmap, unsigned int width, unsigned int height, const vpRect &roi,
421  vpImage<Type> &crop, unsigned int v_scale, unsigned int h_scale)
422 {
423  int i_min = std::max<int>(static_cast<int>(ceil(roi.getTop() / v_scale)), 0);
424  int j_min = std::max<int>(static_cast<int>(ceil(roi.getLeft() / h_scale)), 0);
425  int i_max = std::min<int>(static_cast<int>(ceil((roi.getTop() + roi.getHeight()) / v_scale)), static_cast<int>(height / v_scale));
426  int j_max = std::min<int>(static_cast<int>(ceil((roi.getLeft() + roi.getWidth()) / h_scale)), static_cast<int>(width / h_scale));
427 
428  unsigned int i_min_u = static_cast<unsigned int>(i_min);
429  unsigned int j_min_u = static_cast<unsigned int>(j_min);
430 
431  unsigned int r_width = static_cast<unsigned int>(j_max - j_min);
432  unsigned int r_height = static_cast<unsigned int>(i_max - i_min);
433 
434  crop.resize(r_height, r_width);
435 
436  if (v_scale == 1 && h_scale == 1) {
437  for (unsigned int i = 0; i < r_height; ++i) {
438  void *src = (void *)(bitmap + ((((i + i_min_u) * width) + j_min_u) * sizeof(Type)));
439  void *dst = (void *)(crop[i]);
440  memcpy(dst, src, r_width * sizeof(Type));
441  }
442  }
443  else if (h_scale == 1) {
444  for (unsigned int i = 0; i < r_height; ++i) {
445  void *src = (void *)(bitmap + (((((i + i_min_u) * width) * v_scale) + j_min_u) * sizeof(Type)));
446  void *dst = (void *)(crop[i]);
447  memcpy(dst, src, r_width * sizeof(Type));
448  }
449  }
450  else {
451  for (unsigned int i = 0; i < r_height; ++i) {
452  unsigned int i_src = (((i + i_min_u) * width) * v_scale) + (j_min_u * h_scale);
453  for (unsigned int j = 0; j < r_width; ++j) {
454  void *src = (void *)(bitmap + ((i_src + (j * h_scale)) * sizeof(Type)));
455  void *dst = (void *)(&crop[i][j]);
456  memcpy(dst, src, sizeof(Type));
457  }
458  }
459  }
460 }
461 
472 template <class Type>
473 inline void vpImageTools::binarise(vpImage<Type> &I, Type threshold1, Type threshold2, Type value1, Type value2,
474  Type value3, bool useLUT)
475 {
476  if (useLUT) {
477  std::cerr << "LUT not available for this type ! Will use the iteration method." << std::endl;
478  }
479 
480  Type v;
481  Type *p = I.bitmap;
482  Type *pend = I.bitmap + (I.getWidth() * I.getHeight());
483  for (; p < pend; ++p) {
484  v = *p;
485  if (v < threshold1) {
486  *p = value1;
487  }
488  else if (v > threshold2) {
489  *p = value3;
490  }
491  else {
492  *p = value2;
493  }
494  }
495 }
496 
507 template <>
508 inline void vpImageTools::binarise(vpImage<unsigned char> &I, unsigned char threshold1, unsigned char threshold2,
509  unsigned char value1, unsigned char value2, unsigned char value3, bool useLUT)
510 {
511  if (useLUT) {
512  // Construct the LUT
513  const unsigned int sizeLut = 256;
514  unsigned char lut[sizeLut];
515  for (unsigned int i = 0; i < sizeLut; ++i) {
516  lut[i] = i < threshold1 ? value1 : (i > threshold2 ? value3 : value2);
517  }
518 
519  I.performLut(lut);
520  }
521  else {
522  unsigned char *p = I.bitmap;
523  unsigned char *pend = I.bitmap + (I.getWidth() * I.getHeight());
524  for (; p < pend; ++p) {
525  unsigned char v = *p;
526  if (v < threshold1) {
527  *p = value1;
528  }
529  else if (v > threshold2) {
530  *p = value3;
531  }
532  else {
533  *p = value2;
534  }
535  }
536  }
537 }
538 
539 #ifdef VISP_HAVE_THREADS
540 
541 #ifndef DOXYGEN_SHOULD_SKIP_THIS
542 template <class Type> class vpUndistortInternalType
543 {
544 public:
545  Type *src;
546  Type *dst;
547  unsigned int width;
548  unsigned int height;
549  vpCameraParameters cam;
550  unsigned int nthreads;
551  unsigned int threadid;
552 
553 public:
554  vpUndistortInternalType() : src(nullptr), dst(nullptr), width(0), height(0), cam(), nthreads(0), threadid(0) { }
555 
556  vpUndistortInternalType(const vpUndistortInternalType<Type> &u) { *this = u; }
557  vpUndistortInternalType &operator=(const vpUndistortInternalType<Type> &u)
558  {
559  src = u.src;
560  dst = u.dst;
561  width = u.width;
562  height = u.height;
563  cam = u.cam;
564  nthreads = u.nthreads;
565  threadid = u.threadid;
566 
567  return *this;
568  }
569 
570  static void vpUndistort_threaded(vpUndistortInternalType<Type> &undistortSharedData);
571 };
572 
573 template <class Type> void vpUndistortInternalType<Type>::vpUndistort_threaded(vpUndistortInternalType<Type> &undistortSharedData)
574 {
575  int offset = (int)undistortSharedData.threadid;
576  int width = (int)undistortSharedData.width;
577  int height = (int)undistortSharedData.height;
578  int nthreads = (int)undistortSharedData.nthreads;
579 
580  double u0 = undistortSharedData.cam.get_u0();
581  double v0 = undistortSharedData.cam.get_v0();
582  double px = undistortSharedData.cam.get_px();
583  double py = undistortSharedData.cam.get_py();
584  double kud = undistortSharedData.cam.get_kud();
585 
586  double invpx = 1.0 / px;
587  double invpy = 1.0 / py;
588 
589  double kud_px2 = kud * invpx * invpx;
590  double kud_py2 = kud * invpy * invpy;
591 
592  Type *dst = undistortSharedData.dst + (height / nthreads * offset) * width;
593  Type *src = undistortSharedData.src;
594 
595  for (double v = height / nthreads * offset; v < height / nthreads * (offset + 1); ++v) {
596  double deltav = v - v0;
597  // double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy));
598  double fr1 = 1.0 + kud_py2 * deltav * deltav;
599 
600  for (double u = 0; u < width; ++u) {
601  // computation of u,v : corresponding pixel coordinates in I.
602  double deltau = u - u0;
603  // double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx));
604  double fr2 = fr1 + kud_px2 * deltau * deltau;
605 
606  double u_double = deltau * fr2 + u0;
607  double v_double = deltav * fr2 + v0;
608 
609  // computation of the bilinear interpolation
610 
611  // declarations
612  int u_round = (int)(u_double);
613  int v_round = (int)(v_double);
614  if (u_round < 0.f)
615  u_round = -1;
616  if (v_round < 0.f)
617  v_round = -1;
618  double du_double = (u_double)-(double)u_round;
619  double dv_double = (v_double)-(double)v_round;
620  Type v01;
621  Type v23;
622  if ((0 <= u_round) && (0 <= v_round) && (u_round < ((width)-1)) && (v_round < ((height)-1))) {
623  // process interpolation
624  const Type *_mp = &src[v_round * width + u_round];
625  v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
626  _mp += width;
627  v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
628  *dst = (Type)(v01 + ((v23 - v01) * dv_double));
629  }
630  else {
631  *dst = 0;
632  }
633  dst++;
634  }
635  }
636 }
637 #endif // DOXYGEN_SHOULD_SKIP_THIS
638 #endif // VISP_HAVE_THREADS
639 
663 template <class Type>
665  unsigned int nThreads)
666 {
667 #if defined(VISP_HAVE_THREADS)
668  //
669  // Optimized version using pthreads
670  //
671  unsigned int width = I.getWidth();
672  unsigned int height = I.getHeight();
673 
674  undistI.resize(height, width);
675 
676  double kud = cam.get_kud();
677 
678  // if (kud == 0) {
679  if (std::fabs(kud) <= std::numeric_limits<double>::epsilon()) {
680  // There is no need to undistort the image
681  undistI = I;
682  return;
683  }
684 
685  unsigned int nthreads = nThreads;
686  std::vector<std::thread *> threadpool;
687 
688  vpUndistortInternalType<Type> *undistortSharedData = new vpUndistortInternalType<Type>[nthreads];
689 
690  for (unsigned int i = 0; i < nthreads; ++i) {
691  // Each thread works on a different set of data.
692  undistortSharedData[i].src = I.bitmap;
693  undistortSharedData[i].dst = undistI.bitmap;
694  undistortSharedData[i].width = I.getWidth();
695  undistortSharedData[i].height = I.getHeight();
696  undistortSharedData[i].cam = cam;
697  undistortSharedData[i].nthreads = nthreads;
698  undistortSharedData[i].threadid = i;
699  std::thread *undistort_thread = new std::thread(&vpUndistortInternalType<Type>::vpUndistort_threaded, std::ref(undistortSharedData[i]));
700  threadpool.push_back(undistort_thread);
701  }
702  /* Wait on the other threads */
703 
704  for (unsigned int i = 0; i < nthreads; ++i) {
705  threadpool[i]->join();
706  }
707 
708  for (unsigned int i = 0; i < nthreads; ++i) {
709  delete threadpool[i];
710  }
711 
712  delete[] undistortSharedData;
713 #else // VISP_HAVE_THREADS
714  (void)nThreads;
715  //
716  // optimized version without pthreads
717  //
718  unsigned int width = I.getWidth();
719  unsigned int height = I.getHeight();
720 
721  undistI.resize(height, width);
722 
723  double u0 = cam.get_u0();
724  double v0 = cam.get_v0();
725  double px = cam.get_px();
726  double py = cam.get_py();
727  double kud = cam.get_kud();
728 
729  /*
730  // if (kud == 0) {
731  */
732  if (std::fabs(kud) <= std::numeric_limits<double>::epsilon()) {
733  // There is no need to undistort the image
734  undistI = I;
735  return;
736  }
737 
738  double invpx = 1.0 / px;
739  double invpy = 1.0 / py;
740 
741  double kud_px2 = kud * invpx * invpx;
742  double kud_py2 = kud * invpy * invpy;
743 
744  Type *dst = undistI.bitmap;
745  for (double v = 0; v < height; ++v) {
746  double deltav = v - v0;
747  /*
748  // double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy));
749  */
750  double fr1 = 1.0 + (kud_py2 * deltav * deltav);
751 
752  for (double u = 0; u < width; ++u) {
753  /*
754  // computation of u,v : corresponding pixel coordinates in I.
755  */
756  double deltau = u - u0;
757  /*
758  // double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx));
759  */
760  double fr2 = fr1 + (kud_px2 * deltau * deltau);
761 
762  double u_double = (deltau * fr2) + u0;
763  double v_double = (deltav * fr2) + v0;
764 
765  // printf("[%g][%g] %g %g : ", u, v, u_double, v_double );
766 
767  // computation of the bilinear interpolation
768 
769  // declarations
770  int u_round = static_cast<int>(u_double);
771  int v_round = static_cast<int>(v_double);
772  if (u_round < 0.f) {
773  u_round = -1;
774  }
775  if (v_round < 0.f) {
776  v_round = -1;
777  }
778  double du_double = u_double-static_cast<double>(u_round);
779  double dv_double = v_double-static_cast<double>(v_round);
780  Type v01;
781  Type v23;
782  if ((0 <= u_round) && (0 <= v_round) && (u_round < ((static_cast<int>(width)) - 1)) && (v_round < ((static_cast<int>(height)) - 1))) {
783  // process interpolation
784  const Type *v_mp = &I[static_cast<unsigned int>(v_round)][static_cast<unsigned int>(u_round)];
785  v01 = static_cast<Type>(v_mp[0] + ((v_mp[1] - v_mp[0]) * du_double));
786  v_mp += width;
787  v23 = static_cast<Type>(v_mp[0] + ((v_mp[1] - v_mp[0]) * du_double));
788  *dst = static_cast<Type>(v01 + ((v23 - v01) * dv_double));
789  /*
790  // printf("R %d G %d B %d\n", dst->R, dst->G, dst->B);
791  */
792  }
793  else {
794  *dst = 0;
795  }
796  ++dst;
797  }
798  }
799 #endif // VISP_HAVE_THREADS
800 }
801 
816 template <class Type>
818  vpArray2D<float> mapDv, vpImage<Type> &newI)
819 {
820  remap(I, mapU, mapV, mapDu, mapDv, newI);
821 }
822 
829 template <class Type> void vpImageTools::flip(const vpImage<Type> &I, vpImage<Type> &newI)
830 {
831  unsigned int height = I.getHeight(), width = I.getWidth();
832  newI.resize(height, width);
833 
834  for (unsigned int i = 0; i < height; ++i) {
835  memcpy(newI.bitmap + (i * width), I.bitmap + ((height - 1 - i) * width), width * sizeof(Type));
836  }
837 }
838 
874 template <class Type> void vpImageTools::flip(vpImage<Type> &I)
875 {
876  unsigned int height = I.getHeight(), width = I.getWidth();
877  vpImage<Type> Ibuf;
878  Ibuf.resize(1, width);
879 
880  const unsigned int halfHeight = height / 2;
881  for (unsigned int i = 0; i < halfHeight; ++i) {
882  memcpy(Ibuf.bitmap, I.bitmap + (i * width), width * sizeof(Type));
883 
884  memcpy(I.bitmap + (i * width), I.bitmap + ((height - 1 - i) * width), width * sizeof(Type));
885  memcpy(I.bitmap + ((height - 1 - i) * width), Ibuf.bitmap, width * sizeof(Type));
886  }
887 }
888 
889 template <class Type> Type vpImageTools::getPixelClamped(const vpImage<Type> &I, float u, float v)
890 {
891  int x = vpMath::round(u);
892  int y = vpMath::round(v);
893  x = std::max<int>(0, std::min<int>(x, static_cast<int>(I.getWidth()) - 1));
894  y = std::max<int>(0, std::min<int>(y, static_cast<int>(I.getHeight()) - 1));
895 
896  return I[y][x];
897 }
898 
899 // Reference:
900 // http://blog.demofox.org/2015/08/15/resizing-images-with-bicubic-interpolation/
901 template <class Type>
902 void vpImageTools::resizeBicubic(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int i, unsigned int j, float u,
903  float v, float xFrac, float yFrac)
904 {
905  // 1st row
906  Type p00 = getPixelClamped(I, u - 1, v - 1);
907  Type p01 = getPixelClamped(I, u + 0, v - 1);
908  Type p02 = getPixelClamped(I, u + 1, v - 1);
909  Type p03 = getPixelClamped(I, u + 2, v - 1);
910 
911  // 2nd row
912  Type p10 = getPixelClamped(I, u - 1, v + 0);
913  Type p11 = getPixelClamped(I, u + 0, v + 0);
914  Type p12 = getPixelClamped(I, u + 1, v + 0);
915  Type p13 = getPixelClamped(I, u + 2, v + 0);
916 
917  // 3rd row
918  Type p20 = getPixelClamped(I, u - 1, v + 1);
919  Type p21 = getPixelClamped(I, u + 0, v + 1);
920  Type p22 = getPixelClamped(I, u + 1, v + 1);
921  Type p23 = getPixelClamped(I, u + 2, v + 1);
922 
923  // 4th row
924  Type p30 = getPixelClamped(I, u - 1, v + 2);
925  Type p31 = getPixelClamped(I, u + 0, v + 2);
926  Type p32 = getPixelClamped(I, u + 1, v + 2);
927  Type p33 = getPixelClamped(I, u + 2, v + 2);
928 
929  float col0 = cubicHermite(p00, p01, p02, p03, xFrac);
930  float col1 = cubicHermite(p10, p11, p12, p13, xFrac);
931  float col2 = cubicHermite(p20, p21, p22, p23, xFrac);
932  float col3 = cubicHermite(p30, p31, p32, p33, xFrac);
933  float value = cubicHermite(col0, col1, col2, col3, yFrac);
934  Ires[i][j] = vpMath::saturate<Type>(value);
935 }
936 
937 template <>
938 inline void vpImageTools::resizeBicubic(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &Ires, unsigned int i, unsigned int j,
939  float u, float v, float xFrac, float yFrac)
940 {
941  // 1st row
942  vpRGBa p00 = getPixelClamped(I, u - 1, v - 1);
943  vpRGBa p01 = getPixelClamped(I, u + 0, v - 1);
944  vpRGBa p02 = getPixelClamped(I, u + 1, v - 1);
945  vpRGBa p03 = getPixelClamped(I, u + 2, v - 1);
946 
947  // 2nd row
948  vpRGBa p10 = getPixelClamped(I, u - 1, v + 0);
949  vpRGBa p11 = getPixelClamped(I, u + 0, v + 0);
950  vpRGBa p12 = getPixelClamped(I, u + 1, v + 0);
951  vpRGBa p13 = getPixelClamped(I, u + 2, v + 0);
952 
953  // 3rd row
954  vpRGBa p20 = getPixelClamped(I, u - 1, v + 1);
955  vpRGBa p21 = getPixelClamped(I, u + 0, v + 1);
956  vpRGBa p22 = getPixelClamped(I, u + 1, v + 1);
957  vpRGBa p23 = getPixelClamped(I, u + 2, v + 1);
958 
959  // 4th row
960  vpRGBa p30 = getPixelClamped(I, u - 1, v + 2);
961  vpRGBa p31 = getPixelClamped(I, u + 0, v + 2);
962  vpRGBa p32 = getPixelClamped(I, u + 1, v + 2);
963  vpRGBa p33 = getPixelClamped(I, u + 2, v + 2);
964 
965  const int nbChannels = 3;
966  for (int c = 0; c < nbChannels; ++c) {
967  float col0 = cubicHermite(static_cast<float>(reinterpret_cast<unsigned char *>(&p00)[c]),
968  static_cast<float>(reinterpret_cast<unsigned char *>(&p01)[c]),
969  static_cast<float>(reinterpret_cast<unsigned char *>(&p02)[c]),
970  static_cast<float>(reinterpret_cast<unsigned char *>(&p03)[c]), xFrac);
971  float col1 = cubicHermite(static_cast<float>(reinterpret_cast<unsigned char *>(&p10)[c]),
972  static_cast<float>(reinterpret_cast<unsigned char *>(&p11)[c]),
973  static_cast<float>(reinterpret_cast<unsigned char *>(&p12)[c]),
974  static_cast<float>(reinterpret_cast<unsigned char *>(&p13)[c]), xFrac);
975  float col2 = cubicHermite(static_cast<float>(reinterpret_cast<unsigned char *>(&p20)[c]),
976  static_cast<float>(reinterpret_cast<unsigned char *>(&p21)[c]),
977  static_cast<float>(reinterpret_cast<unsigned char *>(&p22)[c]),
978  static_cast<float>(reinterpret_cast<unsigned char *>(&p23)[c]), xFrac);
979  float col3 = cubicHermite(static_cast<float>(reinterpret_cast<unsigned char *>(&p30)[c]),
980  static_cast<float>(reinterpret_cast<unsigned char *>(&p31)[c]),
981  static_cast<float>(reinterpret_cast<unsigned char *>(&p32)[c]),
982  static_cast<float>(reinterpret_cast<unsigned char *>(&p33)[c]), xFrac);
983  float value = cubicHermite(col0, col1, col2, col3, yFrac);
984 
985  reinterpret_cast<unsigned char *>(&Ires[i][j])[c] = vpMath::saturate<unsigned char>(value);
986  }
987 }
988 
989 template <class Type>
990 void vpImageTools::resizeBilinear(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int i, unsigned int j, float u,
991  float v, float xFrac, float yFrac)
992 {
993  int u0 = static_cast<int>(u);
994  int v0 = static_cast<int>(v);
995 
996  int u1 = std::min<int>(static_cast<int>(I.getWidth()) - 1, u0 + 1);
997  int v1 = v0;
998 
999  int u2 = u0;
1000  int v2 = std::min<int>(static_cast<int>(I.getHeight()) - 1, v0 + 1);
1001 
1002  int u3 = u1;
1003  int v3 = v2;
1004 
1005  float col0 = lerp(I[v0][u0], I[v1][u1], xFrac);
1006  float col1 = lerp(I[v2][u2], I[v3][u3], xFrac);
1007  float value = lerp(col0, col1, yFrac);
1008 
1009  Ires[i][j] = vpMath::saturate<Type>(value);
1010 }
1011 
1012 template <>
1013 inline void vpImageTools::resizeBilinear(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &Ires, unsigned int i,
1014  unsigned int j, float u, float v, float xFrac, float yFrac)
1015 {
1016  int u0 = static_cast<int>(u);
1017  int v0 = static_cast<int>(v);
1018 
1019  int u1 = std::min<int>(static_cast<int>(I.getWidth()) - 1, u0 + 1);
1020  int v1 = v0;
1021 
1022  int u2 = u0;
1023  int v2 = std::min<int>(static_cast<int>(I.getHeight()) - 1, v0 + 1);
1024 
1025  int u3 = u1;
1026  int v3 = v2;
1027 
1028  const int nbChannels = 3;
1029  for (int c = 0; c < nbChannels; ++c) {
1030  float col0 = lerp(static_cast<float>(reinterpret_cast<const unsigned char *>(&I[v0][u0])[c]),
1031  static_cast<float>(reinterpret_cast<const unsigned char *>(&I[v1][u1])[c]), xFrac);
1032  float col1 = lerp(static_cast<float>(reinterpret_cast<const unsigned char *>(&I[v2][u2])[c]),
1033  static_cast<float>(reinterpret_cast<const unsigned char *>(&I[v3][u3])[c]), xFrac);
1034  float value = lerp(col0, col1, yFrac);
1035 
1036  reinterpret_cast<unsigned char *>(&Ires[i][j])[c] = vpMath::saturate<unsigned char>(value);
1037  }
1038 }
1039 
1040 template <class Type>
1041 void vpImageTools::resizeNearest(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int i, unsigned int j, float u,
1042  float v)
1043 {
1044  Ires[i][j] = getPixelClamped(I, u, v);
1045 }
1046 
1065 template <class Type>
1066 void vpImageTools::resize(const vpImage<Type> &I, vpImage<Type> &Ires, unsigned int width, unsigned int height,
1067  const vpImageInterpolationType &method, unsigned int nThreads)
1068 {
1069  Ires.resize(height, width);
1070 
1071  vpImageTools::resize(I, Ires, method, nThreads);
1072 }
1073 
1091 template <class Type>
1093  unsigned int
1094 #if defined(_OPENMP)
1095  nThreads
1096 #endif
1097 )
1098 {
1099  const unsigned int minWidth = 2, minHeight = 2;
1100  if ((I.getWidth() < minWidth) || (I.getHeight() < minHeight) || (Ires.getWidth() < minWidth) || (Ires.getHeight() < minHeight)) {
1101  std::cerr << "Input or output image is too small!" << std::endl;
1102  return;
1103  }
1104 
1105  if (method == INTERPOLATION_AREA) {
1106  std::cerr << "INTERPOLATION_AREA is not implemented for this type." << std::endl;
1107  return;
1108  }
1109 
1110  const float scaleY = I.getHeight() / static_cast<float>(Ires.getHeight());
1111  const float scaleX = I.getWidth() / static_cast<float>(Ires.getWidth());
1112  const float half = 0.5f;
1113  const int ires_height = static_cast<int>(Ires.getHeight());
1114 #if defined(_OPENMP)
1115  if (nThreads > 0) {
1116  omp_set_num_threads(static_cast<int>(nThreads));
1117  }
1118 #pragma omp parallel for schedule(dynamic)
1119 #endif
1120  for (int i = 0; i < ires_height; ++i) {
1121  const float v = ((i + half) * scaleY) - half;
1122  const float v0 = std::floor(v);
1123  const float yFrac = v - v0;
1124 
1125  unsigned int ires_width = static_cast<unsigned int>(Ires.getWidth());
1126  for (unsigned int j = 0; j < ires_width; ++j) {
1127  const float u = ((j + half) * scaleX) - half;
1128  const float u0 = std::floor(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  }
1134  else if (method == INTERPOLATION_LINEAR) {
1135  resizeBilinear(I, Ires, static_cast<unsigned int>(i), j, u0, v0, xFrac, yFrac);
1136  }
1137  else if (method == INTERPOLATION_CUBIC) {
1138  resizeBicubic(I, Ires, static_cast<unsigned int>(i), j, u, v, xFrac, yFrac);
1139  }
1140  }
1141  }
1142 }
1143 
1144 #if defined(VISP_HAVE_SIMDLIB)
1145 template <>
1147  const vpImageInterpolationType &method,
1148  unsigned int
1149 #if defined(_OPENMP)
1150  nThreads
1151 #endif
1152 )
1153 {
1154  const unsigned int minWidth = 2, minHeight = 2;
1155 
1156  if ((I.getWidth() < minWidth) || (I.getHeight() < minHeight) || (Ires.getWidth() < minWidth) || (Ires.getHeight() < minHeight)) {
1157  std::cerr << "Input or output image is too small!" << std::endl;
1158  return;
1159  }
1160 
1161  if (method == INTERPOLATION_AREA) {
1162  resizeSimdlib(I, Ires.getWidth(), Ires.getHeight(), Ires, INTERPOLATION_AREA);
1163  }
1164  else if (method == INTERPOLATION_LINEAR) {
1165  resizeSimdlib(I, Ires.getWidth(), Ires.getHeight(), Ires, INTERPOLATION_LINEAR);
1166  }
1167  else {
1168  const float scaleY = I.getHeight() / static_cast<float>(Ires.getHeight());
1169  const float scaleX = I.getWidth() / static_cast<float>(Ires.getWidth());
1170  const float half = 0.5f;
1171  const int ires_height = static_cast<int>(Ires.getHeight());
1172 #if defined(_OPENMP)
1173  if (nThreads > 0) {
1174  omp_set_num_threads(static_cast<int>(nThreads));
1175  }
1176 #pragma omp parallel for schedule(dynamic)
1177 #endif
1178  for (int i = 0; i < ires_height; ++i) {
1179  float v = ((i + half) * scaleY) - half;
1180  float yFrac = v - static_cast<int>(v);
1181 
1182  unsigned int ires_width = static_cast<unsigned int>(Ires.getWidth());
1183  for (unsigned int j = 0; j < ires_width; ++j) {
1184  float u = ((j + half) * scaleX) - half;
1185  float xFrac = u - static_cast<int>(u);
1186 
1187  if (method == INTERPOLATION_NEAREST) {
1188  resizeNearest(I, Ires, static_cast<unsigned int>(i), j, u, v);
1189  }
1190  else if (method == INTERPOLATION_CUBIC) {
1191  resizeBicubic(I, Ires, static_cast<unsigned int>(i), j, u, v, xFrac, yFrac);
1192  }
1193  }
1194  }
1195  }
1196 }
1197 
1198 template <>
1200  const vpImageInterpolationType &method,
1201  unsigned int
1202 #if defined(_OPENMP)
1203  nThreads
1204 #endif
1205 )
1206 {
1207  const unsigned int minWidth = 2, minHeight = 2;
1208 
1209  if ((I.getWidth() < minWidth) || (I.getHeight() < minHeight) || (Ires.getWidth() < minWidth) || (Ires.getHeight() < minHeight)) {
1210  std::cerr << "Input or output image is too small!" << std::endl;
1211  return;
1212  }
1213 
1214  if (method == INTERPOLATION_AREA) {
1215  resizeSimdlib(I, Ires.getWidth(), Ires.getHeight(), Ires, INTERPOLATION_AREA);
1216  }
1217  else if (method == INTERPOLATION_LINEAR) {
1218  resizeSimdlib(I, Ires.getWidth(), Ires.getHeight(), Ires, INTERPOLATION_LINEAR);
1219  }
1220  else {
1221  const float scaleY = I.getHeight() / static_cast<float>(Ires.getHeight());
1222  const float scaleX = I.getWidth() / static_cast<float>(Ires.getWidth());
1223  const float half = 0.5f;
1224  const int ires_height = static_cast<int>(Ires.getHeight());
1225 #if defined(_OPENMP)
1226  if (nThreads > 0) {
1227  omp_set_num_threads(static_cast<int>(nThreads));
1228  }
1229 #pragma omp parallel for schedule(dynamic)
1230 #endif
1231  for (int i = 0; i < ires_height; ++i) {
1232  float v = ((i + half) * scaleY) - half;
1233  float yFrac = v - static_cast<int>(v);
1234 
1235  unsigned int ires_width = static_cast<unsigned int>(Ires.getWidth());
1236  for (unsigned int j = 0; j < ires_width; ++j) {
1237  float u = ((j + half) * scaleX) - half;
1238  float xFrac = u - static_cast<int>(u);
1239 
1240  if (method == INTERPOLATION_NEAREST) {
1241  resizeNearest(I, Ires, static_cast<unsigned int>(i), j, u, v);
1242  }
1243  else if (method == INTERPOLATION_CUBIC) {
1244  resizeBicubic(I, Ires, static_cast<unsigned int>(i), j, u, v, xFrac, yFrac);
1245  }
1246  }
1247  }
1248  }
1249 }
1250 #endif
1251 
1252 #include <visp3/core/vpImageTools_warp.h>
1253 
1254 END_VISP_NAMESPACE
1255 #endif
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:145
Generic class defining intrinsic camera parameters.
double get_kud() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
double get_j() const
Definition: vpImagePoint.h:125
double get_i() const
Definition: vpImagePoint.h:114
Various image tools; sub-image extraction, modification of the look up table, binarisation....
Definition: vpImageTools.h:76
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:315
static void flip(const vpImage< Type > &I, vpImage< Type > &newI)
Definition: vpImageTools.h:829
static void resize(const vpImage< Type > &I, vpImage< Type > &Ires, unsigned int width, unsigned int height, const vpImageInterpolationType &method=INTERPOLATION_NEAREST, unsigned int nThreads=0)
static void remap(const vpImage< unsigned char > &I, const vpArray2D< int > &mapU, const vpArray2D< int > &mapV, const vpArray2D< float > &mapDu, const vpArray2D< float > &mapDv, vpImage< unsigned char > &Iundist)
static void binarise(vpImage< Type > &I, Type threshold1, Type threshold2, Type value1, Type value2, Type value3, bool useLUT=true)
Definition: vpImageTools.h:473
@ INTERPOLATION_LINEAR
Definition: vpImageTools.h:81
@ INTERPOLATION_NEAREST
Definition: vpImageTools.h:80
static void undistort(const vpImage< Type > &I, const vpCameraParameters &cam, vpImage< Type > &newI, unsigned int nThreads=2)
Definition: vpImageTools.h:664
Definition of the vpImage class member functions.
Definition: vpImage.h:131
unsigned int getWidth() const
Definition: vpImage.h:242
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:542
void performLut(const Type(&lut)[256], unsigned int nbThreads=1)
Definition: vpImage_lut.h:176
Type * bitmap
points toward the bitmap
Definition: vpImage.h:135
unsigned int getHeight() const
Definition: vpImage.h:181
static int round(double x)
Definition: vpMath.h:410
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
Definition: vpRGBa.h:65
Defines an oriented rectangle in the plane.
Defines a rectangle in the plane.
Definition: vpRect.h:79
double getWidth() const
Definition: vpRect.h:227
double getLeft() const
Definition: vpRect.h:173
double getHeight() const
Definition: vpRect.h:166
double getTop() const
Definition: vpRect.h:192
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:124