Visual Servoing Platform  version 3.6.1 under development (2024-04-18)
vpImageTools.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
36 #include <visp3/core/vpCPUFeatures.h>
37 #include <visp3/core/vpImageConvert.h>
38 #include <visp3/core/vpImageTools.h>
39 #include <visp3/core/vpImageException.h>
40 
41 #if defined(VISP_HAVE_SIMDLIB)
42 #include <Simd/SimdLib.hpp>
43 #endif
44 
100 void vpImageTools::changeLUT(vpImage<unsigned char> &I, unsigned char A, unsigned char A_star, unsigned char B,
101  unsigned char B_star)
102 {
103  // Test if input values are valid
104  if (B <= A) {
105  vpERROR_TRACE("Bad gray levels");
107  }
108  unsigned char v;
109 
110  double factor = static_cast<double>((B_star - A_star) / static_cast<double>((B - A)));
111 
112  unsigned int i_height = I.getHeight();
113  unsigned int i_width = I.getWidth();
114  for (unsigned int i = 0; i < i_height; ++i) {
115  for (unsigned int j = 0; j < i_width; ++j) {
116  v = I[i][j];
117 
118  if (v <= A) {
119  I[i][j] = A_star;
120  }
121  else if (v >= B) {
122  I[i][j] = B_star;
123  }
124  else {
125  I[i][j] = static_cast<unsigned char>(A_star + (factor * (v - A)));
126  }
127  }
128  }
129 }
130 
144  vpImage<unsigned char> &Idiff)
145 {
146  if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
147  throw(vpException(vpException::dimensionError, "The two images have not the same size"));
148  }
149 
150  if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth())) {
151  Idiff.resize(I1.getHeight(), I1.getWidth());
152  }
153 
154 #if defined(VISP_HAVE_SIMDLIB)
155  SimdImageDifference(I1.bitmap, I2.bitmap, I1.getSize(), Idiff.bitmap);
156 #else
157  for (unsigned int i = 0; i < I1.getSize(); ++i) {
158  int diff = I1.bitmap[i] - I2.bitmap[i] + 128;
159  Idiff.bitmap[i] = static_cast<unsigned char>(std::max<unsigned char>(std::min<unsigned char>(diff, 255), 0));
160  }
161 #endif
162 }
163 
178 {
179  if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
181  "Cannot compute image difference. The two images "
182  "(%ux%u) and (%ux%u) have not the same size",
183  I1.getWidth(), I1.getHeight(), I2.getWidth(), I2.getHeight()));
184  }
185 
186  if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth())) {
187  Idiff.resize(I1.getHeight(), I1.getWidth());
188  }
189 
190 #if defined(VISP_HAVE_SIMDLIB)
191  SimdImageDifference(reinterpret_cast<unsigned char *>(I1.bitmap), reinterpret_cast<unsigned char *>(I2.bitmap),
192  I1.getSize() * 4, reinterpret_cast<unsigned char *>(Idiff.bitmap));
193 #else
194  for (unsigned int i = 0; i < I1.getSize() * 4; ++i) {
195  int diffR = I1.bitmap[i].R - I2.bitmap[i].R + 128;
196  int diffG = I1.bitmap[i].G - I2.bitmap[i].G + 128;
197  int diffB = I1.bitmap[i].B - I2.bitmap[i].B + 128;
198  int diffA = I1.bitmap[i].A - I2.bitmap[i].A + 128;
199  Idiff.bitmap[i].R = static_cast<unsigned char>(vpMath::maximum(vpMath::minimum(diffR, 255), 0));
200  Idiff.bitmap[i].G = static_cast<unsigned char>(vpMath::maximum(vpMath::minimum(diffG, 255), 0));
201  Idiff.bitmap[i].B = static_cast<unsigned char>(vpMath::maximum(vpMath::minimum(diffB, 255), 0));
202  Idiff.bitmap[i].A = static_cast<unsigned char>(vpMath::maximum(vpMath::minimum(diffA, 255), 0));
203  }
204 #endif
205 }
206 
218  vpImage<unsigned char> &Idiff)
219 {
220  if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
221  throw(vpException(vpException::dimensionError, "The two images do not have the same size"));
222  }
223 
224  if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth())) {
225  Idiff.resize(I1.getHeight(), I1.getWidth());
226  }
227 
228  unsigned int n = I1.getHeight() * I1.getWidth();
229  for (unsigned int b = 0; b < n; ++b) {
230  int diff = I1.bitmap[b] - I2.bitmap[b];
231  Idiff.bitmap[b] = static_cast<unsigned char>(vpMath::abs(diff));
232  }
233 }
234 
243 {
244  if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
245  throw(vpException(vpException::dimensionError, "The two images do not have the same size"));
246  }
247 
248  if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth())) {
249  Idiff.resize(I1.getHeight(), I1.getWidth());
250  }
251 
252  unsigned int n = I1.getHeight() * I1.getWidth();
253  for (unsigned int b = 0; b < n; ++b) {
254  Idiff.bitmap[b] = vpMath::abs(I1.bitmap[b] - I2.bitmap[b]);
255  }
256 }
257 
272 {
273  if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
274  throw(vpException(vpException::dimensionError, "The two images do not have the same size"));
275  }
276 
277  if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth())) {
278  Idiff.resize(I1.getHeight(), I1.getWidth());
279  }
280 
281  unsigned int n = I1.getHeight() * I1.getWidth();
282  for (unsigned int b = 0; b < n; ++b) {
283  int diffR = I1.bitmap[b].R - I2.bitmap[b].R;
284  int diffG = I1.bitmap[b].G - I2.bitmap[b].G;
285  int diffB = I1.bitmap[b].B - I2.bitmap[b].B;
286  // --comment: int diffA eq I1 dot bitmap[b] dot A minus I2 dot bitmap[b] dot A
287  Idiff.bitmap[b].R = static_cast<unsigned char>(vpMath::abs(diffR));
288  Idiff.bitmap[b].G = static_cast<unsigned char>(vpMath::abs(diffG));
289  Idiff.bitmap[b].B = static_cast<unsigned char>(vpMath::abs(diffB));
290  // --comment: Idiff dot bitmap[b] dot A eq diffA
291  Idiff.bitmap[b].A = 0;
292  }
293 }
294 
309  vpImage<unsigned char> &Ires, bool saturate)
310 {
311  if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
312  throw(vpException(vpException::dimensionError, "The two images do not have the same size"));
313  }
314 
315  if ((I1.getHeight() != Ires.getHeight()) || (I1.getWidth() != Ires.getWidth())) {
316  Ires.resize(I1.getHeight(), I1.getWidth());
317  }
318 
319 #if defined(VISP_HAVE_SIMDLIB)
320  typedef Simd::View<Simd::Allocator> View;
321  View img1(I1.getWidth(), I1.getHeight(), I1.getWidth(), View::Gray8, I1.bitmap);
322  View img2(I2.getWidth(), I2.getHeight(), I2.getWidth(), View::Gray8, I2.bitmap);
323  View imgAdd(Ires.getWidth(), Ires.getHeight(), Ires.getWidth(), View::Gray8, Ires.bitmap);
324 
325  Simd::OperationBinary8u(img1, img2, imgAdd,
326  saturate ? SimdOperationBinary8uSaturatedAddition : SimdOperationBinary8uAddition);
327 #else
328  unsigned char *ptr_I1 = I1.bitmap;
329  unsigned char *ptr_I2 = I2.bitmap;
330  unsigned char *ptr_Ires = Ires.bitmap;
331  for (unsigned int cpt = 0; cpt < Ires.getSize(); cpt++, ++ptr_I1, ++ptr_I2, ++ptr_Ires) {
332  *ptr_Ires = saturate ? vpMath::saturate<unsigned char>((short int)*ptr_I1 + (short int)*ptr_I2) : *ptr_I1 + *ptr_I2;
333  }
334 #endif
335 }
336 
351  vpImage<unsigned char> &Ires, bool saturate)
352 {
353  if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
354  throw(vpException(vpException::dimensionError, "The two images do not have the same size"));
355  }
356 
357  if ((I1.getHeight() != Ires.getHeight()) || (I1.getWidth() != Ires.getWidth())) {
358  Ires.resize(I1.getHeight(), I1.getWidth());
359  }
360 
361 #if defined(VISP_HAVE_SIMDLIB)
362  typedef Simd::View<Simd::Allocator> View;
363  View img1(I1.getWidth(), I1.getHeight(), I1.getWidth(), View::Gray8, I1.bitmap);
364  View img2(I2.getWidth(), I2.getHeight(), I2.getWidth(), View::Gray8, I2.bitmap);
365  View imgAdd(Ires.getWidth(), Ires.getHeight(), Ires.getWidth(), View::Gray8, Ires.bitmap);
366 
367  Simd::OperationBinary8u(img1, img2, imgAdd,
368  saturate ? SimdOperationBinary8uSaturatedSubtraction : SimdOperationBinary8uSubtraction);
369 #else
370  unsigned char *ptr_I1 = I1.bitmap;
371  unsigned char *ptr_I2 = I2.bitmap;
372  unsigned char *ptr_Ires = Ires.bitmap;
373  for (unsigned int cpt = 0; cpt < Ires.getSize(); cpt++, ++ptr_I1, ++ptr_I2, ++ptr_Ires) {
374  *ptr_Ires = saturate ?
375  vpMath::saturate<unsigned char>(static_cast<short int>(*ptr_I1) - static_cast<short int>(*ptr_I2)) :
376  *ptr_I1 - *ptr_I2;
377  }
378 #endif
379 }
380 
392 void vpImageTools::initUndistortMap(const vpCameraParameters &cam, unsigned int width, unsigned int height,
393  vpArray2D<int> &mapU, vpArray2D<int> &mapV, vpArray2D<float> &mapDu,
394  vpArray2D<float> &mapDv)
395 {
396  mapU.resize(height, width, false, false);
397  mapV.resize(height, width, false, false);
398  mapDu.resize(height, width, false, false);
399  mapDv.resize(height, width, false, false);
400 
402  bool is_KannalaBrandt =
403  (projModel == vpCameraParameters::ProjWithKannalaBrandtDistortion); // Check the projection model used
404 
405  float u0 = static_cast<float>(cam.get_u0());
406  float v0 = static_cast<float>(cam.get_v0());
407  float px = static_cast<float>(cam.get_px());
408  float py = static_cast<float>(cam.get_py());
409  float kud = 0;
410  std::vector<double> dist_coefs;
411 
412  if (!is_KannalaBrandt) {
413  kud = static_cast<float>(cam.get_kud());
414  }
415  else {
416  dist_coefs = cam.getKannalaBrandtDistortionCoefficients();
417  }
418 
419  if ((!is_KannalaBrandt) && (std::fabs(static_cast<double>(kud)) <= std::numeric_limits<double>::epsilon())) {
420  // There is no need to undistort the image (Perpective projection)
421  for (unsigned int i = 0; i < height; ++i) {
422  for (unsigned int j = 0; j < width; ++j) {
423  mapU[i][j] = static_cast<int>(j);
424  mapV[i][j] = static_cast<int>(i);
425  mapDu[i][j] = 0;
426  mapDv[i][j] = 0;
427  }
428  }
429 
430  return;
431  }
432 
433  float invpx, invpy;
434  float kud_px2 = 0., kud_py2 = 0., deltau_px, deltav_py = 0;
435  float fr1 = 0, fr2;
436  float deltav, deltau;
437  float u_float, v_float;
438  int u_round, v_round;
439  double r, scale;
440  double theta, theta_d;
441  double theta2, theta4, theta6, theta8;
442 
443  invpx = 1.0f / px;
444  invpy = 1.0f / py;
445 
446  if (!is_KannalaBrandt) {
447  kud_px2 = kud * invpx * invpx;
448  kud_py2 = kud * invpy * invpy;
449  }
450 
451  for (unsigned int v = 0; v < height; ++v) {
452  deltav = v - v0;
453 
454  if (!is_KannalaBrandt) {
455  fr1 = 1.0f + (kud_py2 * deltav * deltav);
456  }
457  else {
458  deltav_py = deltav * invpy;
459  }
460 
461  for (unsigned int u = 0; u < width; ++u) {
462  // computation of u,v : corresponding pixel coordinates in I.
463  deltau = u - u0;
464  if (!is_KannalaBrandt) {
465  fr2 = fr1 + (kud_px2 * deltau * deltau);
466 
467  u_float = (deltau * fr2) + u0;
468  v_float = (deltav * fr2) + v0;
469  }
470 
471  else {
472  deltau_px = deltau * invpx;
473  r = sqrt(vpMath::sqr(deltau_px) + vpMath::sqr(deltav_py));
474  theta = atan(r);
475 
476  theta2 = vpMath::sqr(theta);
477  theta4 = vpMath::sqr(theta2);
478  theta6 = theta2 * theta4;
479  theta8 = vpMath::sqr(theta4);
480 
481  theta_d = theta * (1 + (dist_coefs[0] * theta2) + (dist_coefs[1] * theta4) + (dist_coefs[2] * theta6) +
482  (dist_coefs[3] * theta8));
483 
484  // --comment: scale eq (r == 0) 1.0 otherwise theta_d / r
485  scale = (std::fabs(r) < std::numeric_limits<double>::epsilon()) ? 1.0 : (theta_d / r);
486  u_float = static_cast<float>((deltau * scale) + u0);
487  v_float = static_cast<float>((deltav * scale) + v0);
488  }
489 
490  u_round = static_cast<int>(u_float);
491  v_round = static_cast<int>(v_float);
492 
493  mapU[v][u] = u_round;
494  mapV[v][u] = v_round;
495 
496  mapDu[v][u] = u_float - u_round;
497  mapDv[v][u] = v_float - v_round;
498  }
499  }
500 }
501 
514 {
515  if (I.getSize() == 0) {
516  std::cerr << "Error, input image is empty." << std::endl;
517  return;
518  }
519 
520  II.resize(I.getHeight() + 1, I.getWidth() + 1, 0.0);
521  IIsq.resize(I.getHeight() + 1, I.getWidth() + 1, 0.0);
522 
523  unsigned int ii_height = II.getHeight();
524  unsigned int ii_width = II.getWidth();
525  for (unsigned int i = 1; i < ii_height; ++i) {
526  for (unsigned int j = 1; j < ii_width; ++j) {
527  II[i][j] = (I[i - 1][j - 1] + II[i - 1][j] + II[i][j - 1]) - II[i - 1][j - 1];
528  IIsq[i][j] = (vpMath::sqr(I[i - 1][j - 1]) + IIsq[i - 1][j] + IIsq[i][j - 1]) - IIsq[i - 1][j - 1];
529  }
530  }
531 }
532 
540 double vpImageTools::normalizedCorrelation(const vpImage<double> &I1, const vpImage<double> &I2, bool useOptimized)
541 {
542  if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
544  "Error: in vpImageTools::normalizedCorrelation(): "
545  "image dimension mismatch between I1=%ux%u and I2=%ux%u",
546  I1.getHeight(), I1.getWidth(), I2.getHeight(), I2.getWidth());
547  }
548 
549  const double a = I1.getMeanValue();
550  const double b = I2.getMeanValue();
551 
552  double ab = 0.0;
553  double a2 = 0.0;
554  double b2 = 0.0;
555 
556 #if defined(VISP_HAVE_SIMDLIB)
557  SimdNormalizedCorrelation(I1.bitmap, a, I2.bitmap, b, I1.getSize(), a2, b2, ab, useOptimized);
558 #else
559  for (unsigned int cpt = 0; cpt < I1.getSize(); ++cpt) {
560  ab += (I1.bitmap[cpt] - a) * (I2.bitmap[cpt] - b);
561  a2 += vpMath::sqr(I1.bitmap[cpt] - a);
562  b2 += vpMath::sqr(I2.bitmap[cpt] - b);
563  }
564  (void)useOptimized;
565 #endif
566 
567  return ab / sqrt(a2 * b2);
568 }
569 
577 {
578  unsigned int height = I.getHeight(), width = I.getWidth();
579  V.resize(width); // resize and nullify
580 
581  for (unsigned int i = 0; i < height; ++i) {
582  for (unsigned int j = 0; j < width; ++j) {
583  V[j] += I[i][j];
584  }
585  }
586  for (unsigned int j = 0; j < width; ++j) {
587  V[j] /= height;
588  }
589 }
590 
596 {
597  double s = I.getSum();
598  unsigned int i_height = I.getHeight();
599  unsigned int i_width = I.getWidth();
600  for (unsigned int i = 0; i < i_height; ++i) {
601  for (unsigned int j = 0; j < i_width; ++j) {
602  I(i, j, I(i, j) / s);
603  }
604  }
605 }
606 
615  const vpImageInterpolationType &method)
616 {
617  switch (method) {
619  return I(vpMath::round(point.get_i()), vpMath::round(point.get_j()));
620  case INTERPOLATION_LINEAR: {
621  int x1 = static_cast<int>(floor(point.get_i()));
622  int x2 = static_cast<int>(ceil(point.get_i()));
623  int y1 = static_cast<int>(floor(point.get_j()));
624  int y2 = static_cast<int>(ceil(point.get_j()));
625  double v1, v2;
626  if (x1 == x2) {
627  v1 = I(x1, y1);
628  v2 = I(x1, y2);
629  }
630  else {
631  v1 = ((x2 - point.get_i()) * I(x1, y1)) + ((point.get_i() - x1) * I(x2, y1));
632  v2 = ((x2 - point.get_i()) * I(x1, y2)) + ((point.get_i() - x1) * I(x2, y2));
633  }
634  if (y1 == y2) {
635  return v1;
636  }
637  return ((y2 - point.get_j()) * v1) + ((point.get_j() - y1) * v2);
638  }
639  case INTERPOLATION_CUBIC: {
641  "vpImageTools::interpolate(): bi-cubic interpolation is not implemented.");
642  }
643  default: {
644  throw vpException(vpException::notImplementedError, "vpImageTools::interpolate(): invalid interpolation type");
645  }
646  }
647 }
648 
656 {
657  unsigned int x_d = vpMath::round(r.getHeight());
658  unsigned int y_d = vpMath::round(r.getWidth());
659  double x1 = r.getTopLeft().get_i();
660  double y1 = r.getTopLeft().get_j();
661  double t = r.getOrientation();
662  double cos_t = cos(t);
663  double sin_t = sin(t);
664  dst.resize(x_d, y_d);
665  for (unsigned int x = 0; x < x_d; ++x) {
666  double x_cos_t = x * cos_t;
667  double x_sin_t = x * sin_t;
668  for (unsigned int y = 0; y < y_d; ++y) {
669  dst(x, y,
670  static_cast<unsigned char>(interpolate(src, vpImagePoint(x1 + x_cos_t + (y * sin_t), (y1 - x_sin_t) + (y * cos_t)),
672  }
673  }
674 }
675 
683 {
684  unsigned int x_d = vpMath::round(r.getHeight());
685  unsigned int y_d = vpMath::round(r.getWidth());
686  double x1 = r.getTopLeft().get_i();
687  double y1 = r.getTopLeft().get_j();
688  double t = r.getOrientation();
689  double cos_t = cos(t);
690  double sin_t = sin(t);
691  dst.resize(x_d, y_d);
692  for (unsigned int x = 0; x < x_d; ++x) {
693  double x_cos_t = x * cos_t;
694  double x_sin_t = x * sin_t;
695  for (unsigned int y = 0; y < y_d; ++y) {
696  dst(x, y,
697  interpolate(src, vpImagePoint(x1 + x_cos_t + (y * sin_t), (y1 - x_sin_t) + (y * cos_t)),
699  }
700  }
701 }
702 
718  vpImage<double> &I_score, unsigned int step_u, unsigned int step_v,
719  bool useOptimized)
720 {
721  if (I.getSize() == 0) {
722  std::cerr << "Error, input image is empty." << std::endl;
723  return;
724  }
725 
726  if (I_tpl.getSize() == 0) {
727  std::cerr << "Error, template image is empty." << std::endl;
728  return;
729  }
730 
731  if ((I_tpl.getHeight() > I.getHeight()) || (I_tpl.getWidth() > I.getWidth())) {
732  std::cerr << "Error, template image is bigger than input image." << std::endl;
733  return;
734  }
735 
736  vpImage<double> I_double, I_tpl_double;
737  vpImageConvert::convert(I, I_double);
738  vpImageConvert::convert(I_tpl, I_tpl_double);
739 
740  unsigned int height_tpl = I_tpl.getHeight(), width_tpl = I_tpl.getWidth();
741  I_score.resize(I.getHeight() - height_tpl, I.getWidth() - width_tpl, 0.0);
742 
743  if (useOptimized) {
744  vpImage<double> II, IIsq;
745  integralImage(I, II, IIsq);
746 
747  vpImage<double> II_tpl, IIsq_tpl;
748  integralImage(I_tpl, II_tpl, IIsq_tpl);
749 
750  // zero-mean template image
751  const double sum2 = (((II_tpl[height_tpl][width_tpl] + II_tpl[0][0]) - II_tpl[0][width_tpl]) - II_tpl[height_tpl][0]);
752  const double mean2 = sum2 / I_tpl.getSize();
753  unsigned int i_tpl_double_size = I_tpl_double.getSize();
754  for (unsigned int cpt = 0; cpt < i_tpl_double_size; ++cpt) {
755  I_tpl_double.bitmap[cpt] -= mean2;
756  }
757 
758 #if defined(_OPENMP) && (_OPENMP >= 200711) // OpenMP 3.1
759 #pragma omp parallel for schedule(dynamic)
760  for (unsigned int i = 0; i < I.getHeight() - height_tpl; i += step_v) {
761  for (unsigned int j = 0; j < I.getWidth() - width_tpl; j += step_u) {
762  I_score[i][j] = normalizedCorrelation(I_double, I_tpl_double, II, IIsq, II_tpl, IIsq_tpl, i, j);
763  }
764  }
765 #else
766  // error C3016: 'i': index variable in OpenMP 'for' statement must have signed integral type
767  int end = static_cast<int>((I.getHeight() - height_tpl) / step_v) + 1;
768  std::vector<unsigned int> vec_step_v(static_cast<size_t>(end));
769  unsigned int i_height = I.getHeight();
770  for (unsigned int cpt = 0, idx = 0; cpt < (i_height - height_tpl); cpt += step_v, ++idx) {
771  vec_step_v[static_cast<size_t>(idx)] = cpt;
772  }
773 #if defined(_OPENMP) // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas]
774 #pragma omp parallel for schedule(dynamic)
775 #endif
776  for (int cpt = 0; cpt < end; ++cpt) {
777  unsigned int i_width = I.getWidth();
778  for (unsigned int j = 0; j < (i_width - width_tpl); j += step_u) {
779  I_score[vec_step_v[cpt]][j] =
780  normalizedCorrelation(I_double, I_tpl_double, II, IIsq, II_tpl, IIsq_tpl, vec_step_v[cpt], j);
781  }
782  }
783 #endif
784  }
785  else {
786  vpImage<double> I_cur;
787 
788  unsigned int i_height = I.getHeight();
789  unsigned int i_width = I.getWidth();
790  for (unsigned int i = 0; i < (i_height - height_tpl); i += step_v) {
791  for (unsigned int j = 0; j < (i_width - width_tpl); j += step_u) {
792  vpRect roi(vpImagePoint(i, j), vpImagePoint(((i + height_tpl) - 1), ((j + width_tpl) - 1)));
793  vpImageTools::crop(I_double, roi, I_cur);
794 
795  I_score[i][j] = vpImageTools::normalizedCorrelation(I_cur, I_tpl_double, useOptimized);
796  }
797  }
798  }
799 }
800 
801 // Reference:
802 // http://blog.demofox.org/2015/08/15/resizing-images-with-bicubic-interpolation/
803 // t is a value that goes from 0 to 1 to interpolate in a C1 continuous way
804 // across uniformly sampled data points. when t is 0, this will return B.
805 // When t is 1, this will return C. In between values will return an
806 // interpolation between B and C. A and B are used to calculate the slopes at
807 // the edges.
808 float vpImageTools::cubicHermite(const float A, const float B, const float C, const float D, const float t)
809 {
810  float a = (((-A + (3.0f * B)) - (3.0f * C)) + D) / 2.0f;
811  float b = (A + (2.0f * C)) - (((5.0f * B) + D) / 2.0f);
812  float c = (-A + C) / 2.0f;
813  float d = B;
814 
815  return (a * t * t * t) + (b * t * t) + (c * t) + d;
816 }
817 
818 int vpImageTools::coordCast(double x) { return x < 0 ? -1 : static_cast<int>(x); }
819 
820 double vpImageTools::lerp(double A, double B, double t) { return (A * (1.0 - t)) + (B * t); }
821 
822 float vpImageTools::lerp(float A, float B, float t) { return (A * (1.0f - t)) + (B * t); }
823 
824 int64_t vpImageTools::lerp2(int64_t A, int64_t B, int64_t t, int64_t t_1) { return (A * t_1) + (B * t); }
825 
827  const vpImage<double> &II, const vpImage<double> &IIsq,
828  const vpImage<double> &II_tpl, const vpImage<double> &IIsq_tpl,
829  unsigned int i0, unsigned int j0)
830 {
831  double ab = 0.0;
832 
833 #if defined(VISP_HAVE_SIMDLIB)
834  SimdNormalizedCorrelation2(I1.bitmap, I1.getWidth(), I2.bitmap, I2.getWidth(), I2.getHeight(), i0, j0, ab);
835 #else
836  for (unsigned int i = 0; i < I2.getHeight(); ++i) {
837  for (unsigned int j = 0; j < I2.getWidth(); ++j) {
838  ab += (I1[i0 + i][j0 + j]) * I2[i][j];
839  }
840  }
841 #endif
842 
843  unsigned int height_tpl = I2.getHeight(), width_tpl = I2.getWidth();
844  const double sum1 =
845  (((II[i0 + height_tpl][j0 + width_tpl] + II[i0][j0]) - II[i0][j0 + width_tpl]) - II[i0 + height_tpl][j0]);
846  const double sum2 = (((II_tpl[height_tpl][width_tpl] + II_tpl[0][0]) - II_tpl[0][width_tpl]) - II_tpl[height_tpl][0]);
847 
848  double a2 = ((((IIsq[i0 + I2.getHeight()][j0 + I2.getWidth()] + IIsq[i0][j0]) - IIsq[i0][j0 + I2.getWidth()]) -
849  IIsq[i0 + I2.getHeight()][j0]) -
850  ((1.0 / I2.getSize()) * vpMath::sqr(sum1)));
851 
852  double b2 = ((((IIsq_tpl[I2.getHeight()][I2.getWidth()] + IIsq_tpl[0][0]) - IIsq_tpl[0][I2.getWidth()]) -
853  IIsq_tpl[I2.getHeight()][0]) -
854  ((1.0 / I2.getSize()) * vpMath::sqr(sum2)));
855  return ab / sqrt(a2 * b2);
856 }
857 
869  const vpArray2D<float> &mapDu, const vpArray2D<float> &mapDv, vpImage<unsigned char> &Iundist)
870 {
871  Iundist.resize(I.getHeight(), I.getWidth());
872 
873 #if defined(_OPENMP) // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas]
874 #pragma omp parallel for schedule(dynamic)
875 #endif
876  for (int i_ = 0; i_ < static_cast<int>(I.getHeight()); ++i_) {
877  const unsigned int i = static_cast<unsigned int>(i_);
878  unsigned int i_width = I.getWidth();
879  for (unsigned int j = 0; j < i_width; ++j) {
880 
881  int u_round = mapU[i][j];
882  int v_round = mapV[i][j];
883 
884  float du = mapDu[i][j];
885  float dv = mapDv[i][j];
886 
887  if ((0 <= u_round) && (0 <= v_round) && (u_round < (static_cast<int>(I.getWidth()) - 1)) &&
888  (v_round < (static_cast<int>(I.getHeight()) - 1))) {
889  // process interpolation
890  float col0 = lerp(I[v_round][u_round], I[v_round][u_round + 1], du);
891  float col1 = lerp(I[v_round + 1][u_round], I[v_round + 1][u_round + 1], du);
892  float value = lerp(col0, col1, dv);
893 
894  Iundist[i][j] = static_cast<unsigned char>(value);
895  }
896  else {
897  Iundist[i][j] = 0;
898  }
899  }
900  }
901 }
902 
913 void vpImageTools::remap(const vpImage<vpRGBa> &I, const vpArray2D<int> &mapU, const vpArray2D<int> &mapV,
914  const vpArray2D<float> &mapDu, const vpArray2D<float> &mapDv, vpImage<vpRGBa> &Iundist)
915 {
916  Iundist.resize(I.getHeight(), I.getWidth());
917 
918 #if defined(_OPENMP) // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas]
919 #pragma omp parallel for schedule(dynamic)
920 #endif
921  for (int i = 0; i < static_cast<int>(I.getHeight()); ++i) {
922 #if defined(VISP_HAVE_SIMDLIB)
923  SimdRemap(reinterpret_cast<unsigned char *>(I.bitmap), 4, I.getWidth(), I.getHeight(), i * I.getWidth(), mapU.data,
924  mapV.data, mapDu.data, mapDv.data, reinterpret_cast<unsigned char *>(Iundist.bitmap));
925 #else
926  const unsigned int i_ = static_cast<unsigned int>(i);
927  for (unsigned int j = 0; j < I.getWidth(); ++j) {
928 
929  int u_round = mapU[i_][j];
930  int v_round = mapV[i_][j];
931 
932  float du = mapDu[i_][j];
933  float dv = mapDv[i_][j];
934 
935  if (0 <= u_round && 0 <= v_round && u_round < static_cast<int>(I.getWidth()) - 1
936  && v_round < static_cast<int>(I.getHeight()) - 1) {
937  // process interpolation
938  float col0 = lerp(I[v_round][u_round].R, I[v_round][u_round + 1].R, du);
939  float col1 = lerp(I[v_round + 1][u_round].R, I[v_round + 1][u_round + 1].R, du);
940  float value = lerp(col0, col1, dv);
941 
942  Iundist[i][j].R = static_cast<unsigned char>(value);
943 
944  col0 = lerp(I[v_round][u_round].G, I[v_round][u_round + 1].G, du);
945  col1 = lerp(I[v_round + 1][u_round].G, I[v_round + 1][u_round + 1].G, du);
946  value = lerp(col0, col1, dv);
947 
948  Iundist[i][j].G = static_cast<unsigned char>(value);
949 
950  col0 = lerp(I[v_round][u_round].B, I[v_round][u_round + 1].B, du);
951  col1 = lerp(I[v_round + 1][u_round].B, I[v_round + 1][u_round + 1].B, du);
952  value = lerp(col0, col1, dv);
953 
954  Iundist[i][j].B = static_cast<unsigned char>(value);
955 
956  col0 = lerp(I[v_round][u_round].A, I[v_round][u_round + 1].A, du);
957  col1 = lerp(I[v_round + 1][u_round].A, I[v_round + 1][u_round + 1].A, du);
958  value = lerp(col0, col1, dv);
959 
960  Iundist[i][j].A = static_cast<unsigned char>(value);
961  }
962  else {
963  Iundist[i][j] = 0;
964  }
965  }
966 #endif
967  }
968 }
969 
970 #if defined(VISP_HAVE_SIMDLIB)
971 void vpImageTools::resizeSimdlib(const vpImage<vpRGBa> &Isrc, unsigned int resizeWidth, unsigned int resizeHeight,
972  vpImage<vpRGBa> &Idst, int method)
973 {
974  Idst.resize(resizeHeight, resizeWidth);
975 
976  typedef Simd::View<Simd::Allocator> View;
977  View src(Isrc.getWidth(), Isrc.getHeight(), Isrc.getWidth() * sizeof(vpRGBa), View::Bgra32, Isrc.bitmap);
978  View dst(Idst.getWidth(), Idst.getHeight(), Idst.getWidth() * sizeof(vpRGBa), View::Bgra32, Idst.bitmap);
979 
980  Simd::Resize(src, dst, method == INTERPOLATION_LINEAR ? SimdResizeMethodBilinear : SimdResizeMethodArea);
981 }
982 
983 void vpImageTools::resizeSimdlib(const vpImage<unsigned char> &Isrc, unsigned int resizeWidth,
984  unsigned int resizeHeight, vpImage<unsigned char> &Idst, int method)
985 {
986  Idst.resize(resizeHeight, resizeWidth);
987 
988  typedef Simd::View<Simd::Allocator> View;
989  View src(Isrc.getWidth(), Isrc.getHeight(), Isrc.getWidth(), View::Gray8, Isrc.bitmap);
990  View dst(Idst.getWidth(), Idst.getHeight(), Idst.getWidth(), View::Gray8, Idst.bitmap);
991 
992  Simd::Resize(src, dst, method == INTERPOLATION_LINEAR ? SimdResizeMethodBilinear : SimdResizeMethodArea);
993 }
994 #endif
995 
996 bool vpImageTools::checkFixedPoint(unsigned int x, unsigned int y, const vpMatrix &T, bool affine)
997 {
998  double a0 = T[0][0];
999  double a1 = T[0][1];
1000  double a2 = T[0][2];
1001  double a3 = T[1][0];
1002  double a4 = T[1][1];
1003  double a5 = T[1][2];
1004  double a6 = affine ? 0.0 : T[2][0];
1005  double a7 = affine ? 0.0 : T[2][1];
1006  double a8 = affine ? 1.0 : T[2][2];
1007 
1008  double w = (a6 * x) + (a7 * y) + a8;
1009  double x2 = ((a0 * x) + (a1 * y) + a2) / w;
1010  double y2 = ((a3 * x) + (a4 * y) + a5) / w;
1011 
1012  const double limit = 1 << 15;
1013  return (vpMath::abs(x2) < limit) && (vpMath::abs(y2) < limit);
1014 }
1015 
1024 {
1025  if ((I.getHeight() != mask.getHeight()) || (I.getWidth() != mask.getWidth())) {
1027  "Error in vpImageTools::inMask(): image (%dx%d) and mask (%dx%d) size doesn't match",
1028  I.getWidth(), I.getHeight(), mask.getWidth(), mask.getHeight()));
1029  }
1030  vpRGBa black(0, 0, 0);
1031  I_mask.resize(I.getHeight(), I.getWidth());
1032  int cpt_in_mask = 0;
1033  int size_ = static_cast<int>(I.getSize());
1034 #if defined(_OPENMP)
1035 #pragma omp parallel for reduction(+:cpt_in_mask)
1036 #endif
1037  for (int i = 0; i < size_; ++i) {
1038  if (mask.bitmap[i] == 0) {
1039  I_mask.bitmap[i] = black;
1040  }
1041  else {
1042  I_mask.bitmap[i] = I.bitmap[i];
1043  ++cpt_in_mask;
1044  }
1045  }
1046  return cpt_in_mask;
1047 }
1048 
1057 {
1058  if ((I.getHeight() != mask.getHeight()) || (I.getWidth() != mask.getWidth())) {
1060  "Error in vpImageTools::inMask(): image (%dx%d) and mask (%dx%d) size doesn't match",
1061  I.getWidth(), I.getHeight(), mask.getWidth(), mask.getHeight()));
1062  }
1063  I_mask.resize(I.getHeight(), I.getWidth());
1064  int cpt_in_mask = 0;
1065  int size_ = static_cast<int>(I.getSize());
1066 #if defined(_OPENMP)
1067 #pragma omp parallel for reduction(+:cpt_in_mask)
1068 #endif
1069  for (int i = 0; i < size_; ++i) {
1070  if (mask.bitmap[i] == 0) {
1071  I_mask.bitmap[i] = 0;
1072  }
1073  else {
1074  I_mask.bitmap[i] = I.bitmap[i];
1075  ++cpt_in_mask;
1076  }
1077  }
1078  return cpt_in_mask;
1079 }
1080 
1097 int vpImageTools::inRange(const unsigned char *hue, const unsigned char *saturation, const unsigned char *value,
1098  const vpColVector &hsv_range, unsigned char *mask, unsigned int size)
1099 {
1100  if ((hue == nullptr) || (saturation == nullptr) || (value == nullptr)) {
1102  "Error in vpImageTools::inRange(): hsv pointer are empty"));
1103  }
1104  else if (hsv_range.size() != 6) {
1106  "Error in vpImageTools::inRange(): wrong values vector size (%d)", hsv_range.size()));
1107  }
1108  unsigned char h_low = static_cast<unsigned char>(hsv_range[0]);
1109  unsigned char h_high = static_cast<unsigned char>(hsv_range[1]);
1110  unsigned char s_low = static_cast<unsigned char>(hsv_range[2]);
1111  unsigned char s_high = static_cast<unsigned char>(hsv_range[3]);
1112  unsigned char v_low = static_cast<unsigned char>(hsv_range[4]);
1113  unsigned char v_high = static_cast<unsigned char>(hsv_range[5]);
1114  int size_ = static_cast<int>(size);
1115  int cpt_in_range = 0;
1116 #if defined(_OPENMP)
1117 #pragma omp parallel for reduction(+:cpt_in_range)
1118 #endif
1119  for (int i = 0; i < size_; ++i) {
1120  if ((h_low <= hue[i]) && (hue[i] <= h_high) &&
1121  (s_low <= saturation[i]) && (saturation[i] <= s_high) &&
1122  (v_low <= value[i]) && (value[i] <= v_high)) {
1123  mask[i] = 255;
1124  ++cpt_in_range;
1125  }
1126  else {
1127  mask[i] = 0;
1128  }
1129  }
1130  return cpt_in_range;
1131 }
1132 
1150 int vpImageTools::inRange(const unsigned char *hue, const unsigned char *saturation, const unsigned char *value,
1151  const std::vector<int> &hsv_range, unsigned char *mask, unsigned int size)
1152 {
1153  if ((hue == nullptr) || (saturation == nullptr) || (value == nullptr)) {
1155  "Error in vpImageTools::inRange(): hsv pointer are empty"));
1156  }
1157  else if (hsv_range.size() != 6) {
1159  "Error in vpImageTools::inRange(): wrong values vector size (%d)", hsv_range.size()));
1160  }
1161  unsigned char h_low = static_cast<unsigned char>(hsv_range[0]);
1162  unsigned char h_high = static_cast<unsigned char>(hsv_range[1]);
1163  unsigned char s_low = static_cast<unsigned char>(hsv_range[2]);
1164  unsigned char s_high = static_cast<unsigned char>(hsv_range[3]);
1165  unsigned char v_low = static_cast<unsigned char>(hsv_range[4]);
1166  unsigned char v_high = static_cast<unsigned char>(hsv_range[5]);
1167  int size_ = static_cast<int>(size);
1168  int cpt_in_range = 0;
1169 #if defined(_OPENMP)
1170 #pragma omp parallel for reduction(+:cpt_in_range)
1171 #endif
1172  for (int i = 0; i < size_; ++i) {
1173  if ((h_low <= hue[i]) && (hue[i] <= h_high) &&
1174  (s_low <= saturation[i]) && (saturation[i] <= s_high) &&
1175  (v_low <= value[i]) && (value[i] <= v_high)) {
1176  mask[i] = 255;
1177  ++cpt_in_range;
1178  }
1179  else {
1180  mask[i] = 0;
1181  }
1182  }
1183  return cpt_in_range;
1184 }
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:126
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:139
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:339
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:326
Generic class defining intrinsic camera parameters.
std::vector< double > getKannalaBrandtDistortionCoefficients() const
@ ProjWithKannalaBrandtDistortion
Projection with Kannala-Brandt distortion model.
vpCameraParametersProjType get_projModel() const
double get_kud() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ dimensionError
Bad dimension.
Definition: vpException.h:83
@ notImplementedError
Not implemented.
Definition: vpException.h:81
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Error that can be emitted by the vpImage class and its derivatives.
@ notInitializedError
Image not initialized.
@ incorrectInitializationError
Wrong image initialization.
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
static void imageDifference(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, vpImage< unsigned char > &Idiff)
static void templateMatching(const vpImage< unsigned char > &I, const vpImage< unsigned char > &I_tpl, vpImage< double > &I_score, unsigned int step_u, unsigned int step_v, bool useOptimized=true)
static void initUndistortMap(const vpCameraParameters &cam, unsigned int width, unsigned int height, vpArray2D< int > &mapU, vpArray2D< int > &mapV, vpArray2D< float > &mapDu, vpArray2D< float > &mapDv)
static void crop(const vpImage< Type > &I, double roi_top, double roi_left, unsigned int roi_height, unsigned int roi_width, vpImage< Type > &crop, unsigned int v_scale=1, unsigned int h_scale=1)
Definition: vpImageTools.h:312
static double interpolate(const vpImage< unsigned char > &I, const vpImagePoint &point, const vpImageInterpolationType &method=INTERPOLATION_NEAREST)
static void integralImage(const vpImage< unsigned char > &I, vpImage< double > &II, vpImage< double > &IIsq)
static void extract(const vpImage< unsigned char > &src, vpImage< unsigned char > &dst, const vpRectOriented &r)
static void imageSubtract(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, vpImage< unsigned char > &Ires, bool saturate=false)
static void imageAdd(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, vpImage< unsigned char > &Ires, bool saturate=false)
static int inMask(const vpImage< unsigned char > &I, const vpImage< unsigned char > &mask, vpImage< unsigned char > &I_mask)
static void columnMean(const vpImage< double > &I, vpRowVector &result)
static void normalize(vpImage< double > &I)
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)
@ INTERPOLATION_LINEAR
Definition: vpImageTools.h:80
@ INTERPOLATION_NEAREST
Definition: vpImageTools.h:79
static void changeLUT(vpImage< unsigned char > &I, unsigned char A, unsigned char newA, unsigned char B, unsigned char newB)
static double normalizedCorrelation(const vpImage< double > &I1, const vpImage< double > &I2, bool useOptimized=true)
static int inRange(const unsigned char *hue, const unsigned char *saturation, const unsigned char *value, const vpColVector &hsv_range, unsigned char *mask, unsigned int size)
static void imageDifferenceAbsolute(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, vpImage< unsigned char > &Idiff)
double getSum(const vpImage< bool > *p_mask=nullptr, unsigned int *nbValidPoints=nullptr) const
Compute the sum of image intensities.
Definition: vpImage.h:2064
unsigned int getWidth() const
Definition: vpImage.h:245
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:783
unsigned int getSize() const
Definition: vpImage.h:224
Type * bitmap
points toward the bitmap
Definition: vpImage.h:139
unsigned int getHeight() const
Definition: vpImage.h:184
double getMeanValue(const vpImage< bool > *p_mask=nullptr, unsigned int *nbValidPoints=nullptr) const
Return the mean value of the bitmap.
Definition: vpImage.h:954
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:252
static double sqr(double x)
Definition: vpMath.h:201
static Type abs(const Type &x)
Definition: vpMath.h:267
static int round(double x)
Definition: vpMath.h:403
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:260
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
Definition: vpRGBa.h:61
unsigned char B
Blue component.
Definition: vpRGBa.h:139
unsigned char R
Red component.
Definition: vpRGBa.h:137
unsigned char G
Green component.
Definition: vpRGBa.h:138
unsigned char A
Additionnal component.
Definition: vpRGBa.h:140
Defines an oriented rectangle in the plane.
double getHeight() const
Get the rectangle height.
double getOrientation() const
Get the rectangle orientation (rad).
double getWidth() const
Get the rectangle width.
vpImagePoint getTopLeft() const
Get the top-left corner.
Defines a rectangle in the plane.
Definition: vpRect.h:76
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:107
void resize(unsigned int i, bool flagNullify=true)
Definition: vpRowVector.h:259
#define vpERROR_TRACE
Definition: vpDebug.h:382