Visual Servoing Platform  version 3.5.1 under development (2022-07-05)
vpImageTools.cpp
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 #include <visp3/core/vpCPUFeatures.h>
40 #include <visp3/core/vpImageConvert.h>
41 #include <visp3/core/vpImageTools.h>
42 
43 #include <Simd/SimdLib.hpp>
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 = (double)(B_star - A_star) / (double)(B - A);
111 
112  for (unsigned int i = 0; i < I.getHeight(); i++)
113  for (unsigned int j = 0; j < I.getWidth(); j++) {
114  v = I[i][j];
115 
116  if (v <= A)
117  I[i][j] = A_star;
118  else if (v >= B)
119  I[i][j] = B_star;
120  else
121  I[i][j] = (unsigned char)(A_star + factor * (v - A));
122  }
123 }
124 
138  vpImage<unsigned char> &Idiff)
139 {
140  if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
141  throw(vpException(vpException::dimensionError, "The two images have not the same size"));
142  }
143 
144  if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth())) {
145  Idiff.resize(I1.getHeight(), I1.getWidth());
146  }
147 
148  SimdImageDifference(I1.bitmap, I2.bitmap, I1.getSize(), Idiff.bitmap);
149 }
150 
165 {
166  if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
168  "Cannot compute image difference. The two images "
169  "(%ux%u) and (%ux%u) have not the same size",
170  I1.getWidth(), I1.getHeight(), I2.getWidth(), I2.getHeight()));
171  }
172 
173  if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth())) {
174  Idiff.resize(I1.getHeight(), I1.getWidth());
175  }
176 
177  SimdImageDifference(reinterpret_cast<unsigned char *>(I1.bitmap), reinterpret_cast<unsigned char *>(I2.bitmap),
178  I1.getSize() * 4, reinterpret_cast<unsigned char *>(Idiff.bitmap));
179 }
180 
192  vpImage<unsigned char> &Idiff)
193 {
194  if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
195  throw(vpException(vpException::dimensionError, "The two images do not have the same size"));
196  }
197 
198  if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth())) {
199  Idiff.resize(I1.getHeight(), I1.getWidth());
200  }
201 
202  unsigned int n = I1.getHeight() * I1.getWidth();
203  for (unsigned int b = 0; b < n; b++) {
204  int diff = I1.bitmap[b] - I2.bitmap[b];
205  Idiff.bitmap[b] = static_cast<unsigned char>(vpMath::abs(diff));
206  }
207 }
208 
217 {
218  if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
219  throw(vpException(vpException::dimensionError, "The two images do not have the same size"));
220  }
221 
222  if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth())) {
223  Idiff.resize(I1.getHeight(), I1.getWidth());
224  }
225 
226  unsigned int n = I1.getHeight() * I1.getWidth();
227  for (unsigned int b = 0; b < n; b++) {
228  Idiff.bitmap[b] = vpMath::abs(I1.bitmap[b] - I2.bitmap[b]);
229  }
230 }
231 
246 {
247  if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
248  throw(vpException(vpException::dimensionError, "The two images do not have the same size"));
249  }
250 
251  if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth())) {
252  Idiff.resize(I1.getHeight(), I1.getWidth());
253  }
254 
255  unsigned int n = I1.getHeight() * I1.getWidth();
256  for (unsigned int b = 0; b < n; b++) {
257  int diffR = I1.bitmap[b].R - I2.bitmap[b].R;
258  int diffG = I1.bitmap[b].G - I2.bitmap[b].G;
259  int diffB = I1.bitmap[b].B - I2.bitmap[b].B;
260  // int diffA = I1.bitmap[b].A - I2.bitmap[b].A;
261  Idiff.bitmap[b].R = static_cast<unsigned char>(vpMath::abs(diffR));
262  Idiff.bitmap[b].G = static_cast<unsigned char>(vpMath::abs(diffG));
263  Idiff.bitmap[b].B = static_cast<unsigned char>(vpMath::abs(diffB));
264  // Idiff.bitmap[b].A = diffA;
265  Idiff.bitmap[b].A = 0;
266  }
267 }
268 
283  vpImage<unsigned char> &Ires, bool saturate)
284 {
285  if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
286  throw(vpException(vpException::dimensionError, "The two images do not have the same size"));
287  }
288 
289  if ((I1.getHeight() != Ires.getHeight()) || (I1.getWidth() != Ires.getWidth())) {
290  Ires.resize(I1.getHeight(), I1.getWidth());
291  }
292 
293  typedef Simd::View<Simd::Allocator> View;
294  View img1(I1.getWidth(), I1.getHeight(), I1.getWidth(), View::Gray8, I1.bitmap);
295  View img2(I2.getWidth(), I2.getHeight(), I2.getWidth(), View::Gray8, I2.bitmap);
296  View imgAdd(Ires.getWidth(), Ires.getHeight(), Ires.getWidth(), View::Gray8, Ires.bitmap);
297 
298  Simd::OperationBinary8u(img1, img2, imgAdd,
299  saturate ? SimdOperationBinary8uSaturatedAddition : SimdOperationBinary8uAddition);
300 }
301 
316  vpImage<unsigned char> &Ires, bool saturate)
317 {
318  if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
319  throw(vpException(vpException::dimensionError, "The two images do not have the same size"));
320  }
321 
322  if ((I1.getHeight() != Ires.getHeight()) || (I1.getWidth() != Ires.getWidth())) {
323  Ires.resize(I1.getHeight(), I1.getWidth());
324  }
325 
326  typedef Simd::View<Simd::Allocator> View;
327  View img1(I1.getWidth(), I1.getHeight(), I1.getWidth(), View::Gray8, I1.bitmap);
328  View img2(I2.getWidth(), I2.getHeight(), I2.getWidth(), View::Gray8, I2.bitmap);
329  View imgAdd(Ires.getWidth(), Ires.getHeight(), Ires.getWidth(), View::Gray8, Ires.bitmap);
330 
331  Simd::OperationBinary8u(img1, img2, imgAdd,
332  saturate ? SimdOperationBinary8uSaturatedSubtraction : SimdOperationBinary8uSubtraction);
333 }
334 
346 void vpImageTools::initUndistortMap(const vpCameraParameters &cam, unsigned int width, unsigned int height,
347  vpArray2D<int> &mapU, vpArray2D<int> &mapV, vpArray2D<float> &mapDu,
348  vpArray2D<float> &mapDv)
349 {
350  mapU.resize(height, width, false, false);
351  mapV.resize(height, width, false, false);
352  mapDu.resize(height, width, false, false);
353  mapDv.resize(height, width, false, false);
354 
356  bool is_KannalaBrandt =
357  (projModel == vpCameraParameters::ProjWithKannalaBrandtDistortion); // Check the projection model used
358 
359  float u0 = static_cast<float>(cam.get_u0());
360  float v0 = static_cast<float>(cam.get_v0());
361  float px = static_cast<float>(cam.get_px());
362  float py = static_cast<float>(cam.get_py());
363  float kud;
364  std::vector<double> dist_coefs;
365 
366  if (!is_KannalaBrandt)
367  kud = static_cast<float>(cam.get_kud());
368 
369  else
370  dist_coefs = cam.getKannalaBrandtDistortionCoefficients();
371 
372  if (!is_KannalaBrandt && std::fabs(static_cast<double>(kud)) <= std::numeric_limits<double>::epsilon()) {
373  // There is no need to undistort the image (Perpective projection)
374  for (unsigned int i = 0; i < height; i++) {
375  for (unsigned int j = 0; j < width; j++) {
376  mapU[i][j] = static_cast<int>(j);
377  mapV[i][j] = static_cast<int>(i);
378  mapDu[i][j] = 0;
379  mapDv[i][j] = 0;
380  }
381  }
382 
383  return;
384  }
385 
386  float invpx, invpy;
387  float kud_px2 = 0., kud_py2 = 0., deltau_px, deltav_py;
388  float fr1, fr2;
389  float deltav, deltau;
390  float u_float, v_float;
391  int u_round, v_round;
392  double r, scale;
393  double theta, theta_d;
394  double theta2, theta4, theta6, theta8;
395 
396  invpx = 1.0f / px;
397  invpy = 1.0f / py;
398 
399  if (!is_KannalaBrandt) {
400  kud_px2 = kud * invpx * invpx;
401  kud_py2 = kud * invpy * invpy;
402  }
403 
404  for (unsigned int v = 0; v < height; v++) {
405  deltav = v - v0;
406 
407  if (!is_KannalaBrandt)
408  fr1 = 1.0f + kud_py2 * deltav * deltav;
409  else
410  deltav_py = deltav * invpy;
411 
412  for (unsigned int u = 0; u < width; u++) {
413  // computation of u,v : corresponding pixel coordinates in I.
414  deltau = u - u0;
415  if (!is_KannalaBrandt) {
416  fr2 = fr1 + kud_px2 * deltau * deltau;
417 
418  u_float = deltau * fr2 + u0;
419  v_float = deltav * fr2 + v0;
420  }
421 
422  else {
423  deltau_px = deltau * invpx;
424  r = sqrt(vpMath::sqr(deltau_px) + vpMath::sqr(deltav_py));
425  theta = atan(r);
426 
427  theta2 = vpMath::sqr(theta);
428  theta4 = vpMath::sqr(theta2);
429  theta6 = theta2 * theta4;
430  theta8 = vpMath::sqr(theta4);
431 
432  theta_d = theta * (1 + dist_coefs[0] * theta2 + dist_coefs[1] * theta4 + dist_coefs[2] * theta6 +
433  dist_coefs[3] * theta8);
434 
435  // scale = (r == 0) ? 1.0 : theta_d / r;
436  scale = (std::fabs(r) < std::numeric_limits<double>::epsilon()) ? 1.0 : theta_d / r;
437  u_float = static_cast<float>(deltau * scale + u0);
438  v_float = static_cast<float>(deltav * scale + v0);
439  }
440 
441  u_round = static_cast<int>(u_float);
442  v_round = static_cast<int>(v_float);
443 
444  mapU[v][u] = u_round;
445  mapV[v][u] = v_round;
446 
447  mapDu[v][u] = u_float - u_round;
448  mapDv[v][u] = v_float - v_round;
449  }
450  }
451 }
452 
465 {
466  if (I.getSize() == 0) {
467  std::cerr << "Error, input image is empty." << std::endl;
468  return;
469  }
470 
471  II.resize(I.getHeight() + 1, I.getWidth() + 1, 0.0);
472  IIsq.resize(I.getHeight() + 1, I.getWidth() + 1, 0.0);
473 
474  for (unsigned int i = 1; i < II.getHeight(); i++) {
475  for (unsigned int j = 1; j < II.getWidth(); j++) {
476  II[i][j] = I[i - 1][j - 1] + II[i - 1][j] + II[i][j - 1] - II[i - 1][j - 1];
477  IIsq[i][j] = vpMath::sqr(I[i - 1][j - 1]) + IIsq[i - 1][j] + IIsq[i][j - 1] - IIsq[i - 1][j - 1];
478  }
479  }
480 }
481 
489 double vpImageTools::normalizedCorrelation(const vpImage<double> &I1, const vpImage<double> &I2, bool useOptimized)
490 {
491  if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
493  "Error: in vpImageTools::normalizedCorrelation(): "
494  "image dimension mismatch between I1=%ux%u and I2=%ux%u",
495  I1.getHeight(), I1.getWidth(), I2.getHeight(), I2.getWidth());
496  }
497 
498  const double a = I1.getMeanValue();
499  const double b = I2.getMeanValue();
500 
501  double ab = 0.0;
502  double a2 = 0.0;
503  double b2 = 0.0;
504 
505  SimdNormalizedCorrelation(I1.bitmap, a, I2.bitmap, b, I1.getSize(), a2, b2, ab, useOptimized);
506 
507  return ab / sqrt(a2 * b2);
508 }
509 
517 {
518  unsigned int height = I.getHeight(), width = I.getWidth();
519  V.resize(width); // resize and nullify
520 
521  for (unsigned int i = 0; i < height; ++i)
522  for (unsigned int j = 0; j < width; ++j)
523  V[j] += I[i][j];
524  for (unsigned int j = 0; j < width; ++j)
525  V[j] /= height;
526 }
527 
533 {
534  double s = I.getSum();
535  for (unsigned int i = 0; i < I.getHeight(); ++i)
536  for (unsigned int j = 0; j < I.getWidth(); ++j)
537  I(i, j, I(i, j) / s);
538 }
539 
548  const vpImageInterpolationType &method)
549 {
550  switch (method) {
552  return I(vpMath::round(point.get_i()), vpMath::round(point.get_j()));
553  case INTERPOLATION_LINEAR: {
554  int x1 = (int)floor(point.get_i());
555  int x2 = (int)ceil(point.get_i());
556  int y1 = (int)floor(point.get_j());
557  int y2 = (int)ceil(point.get_j());
558  double v1, v2;
559  if (x1 == x2) {
560  v1 = I(x1, y1);
561  v2 = I(x1, y2);
562  } else {
563  v1 = (x2 - point.get_i()) * I(x1, y1) + (point.get_i() - x1) * I(x2, y1);
564  v2 = (x2 - point.get_i()) * I(x1, y2) + (point.get_i() - x1) * I(x2, y2);
565  }
566  if (y1 == y2)
567  return v1;
568  return (y2 - point.get_j()) * v1 + (point.get_j() - y1) * v2;
569  }
570  case INTERPOLATION_CUBIC: {
572  "vpImageTools::interpolate(): bi-cubic interpolation is not implemented.");
573  }
574  default: {
575  throw vpException(vpException::notImplementedError, "vpImageTools::interpolate(): invalid interpolation type");
576  }
577  }
578 }
579 
587 {
588  unsigned int x_d = vpMath::round(r.getHeight());
589  unsigned int y_d = vpMath::round(r.getWidth());
590  double x1 = r.getTopLeft().get_i();
591  double y1 = r.getTopLeft().get_j();
592  double t = r.getOrientation();
593  Dst.resize(x_d, y_d);
594  for (unsigned int x = 0; x < x_d; ++x) {
595  for (unsigned int y = 0; y < y_d; ++y) {
596  Dst(x, y,
597  (unsigned char)interpolate(Src, vpImagePoint(x1 + x * cos(t) + y * sin(t), y1 - x * sin(t) + y * cos(t)),
599  }
600  }
601 }
602 
610 {
611  unsigned int x_d = vpMath::round(r.getHeight());
612  unsigned int y_d = vpMath::round(r.getWidth());
613  double x1 = r.getTopLeft().get_i();
614  double y1 = r.getTopLeft().get_j();
615  double t = r.getOrientation();
616  Dst.resize(x_d, y_d);
617  for (unsigned int x = 0; x < x_d; ++x) {
618  for (unsigned int y = 0; y < y_d; ++y) {
619  Dst(x, y,
620  interpolate(Src, vpImagePoint(x1 + x * cos(t) + y * sin(t), y1 - x * sin(t) + y * cos(t)),
622  }
623  }
624 }
625 
641  vpImage<double> &I_score, unsigned int step_u, unsigned int step_v,
642  bool useOptimized)
643 {
644  if (I.getSize() == 0) {
645  std::cerr << "Error, input image is empty." << std::endl;
646  return;
647  }
648 
649  if (I_tpl.getSize() == 0) {
650  std::cerr << "Error, template image is empty." << std::endl;
651  return;
652  }
653 
654  if (I_tpl.getHeight() > I.getHeight() || I_tpl.getWidth() > I.getWidth()) {
655  std::cerr << "Error, template image is bigger than input image." << std::endl;
656  return;
657  }
658 
659  vpImage<double> I_double, I_tpl_double;
660  vpImageConvert::convert(I, I_double);
661  vpImageConvert::convert(I_tpl, I_tpl_double);
662 
663  unsigned int height_tpl = I_tpl.getHeight(), width_tpl = I_tpl.getWidth();
664  I_score.resize(I.getHeight() - height_tpl, I.getWidth() - width_tpl, 0.0);
665 
666  if (useOptimized) {
667  vpImage<double> II, IIsq;
668  integralImage(I, II, IIsq);
669 
670  vpImage<double> II_tpl, IIsq_tpl;
671  integralImage(I_tpl, II_tpl, IIsq_tpl);
672 
673  // zero-mean template image
674  const double sum2 = (II_tpl[height_tpl][width_tpl] + II_tpl[0][0] - II_tpl[0][width_tpl] - II_tpl[height_tpl][0]);
675  const double mean2 = sum2 / I_tpl.getSize();
676  for (unsigned int cpt = 0; cpt < I_tpl_double.getSize(); cpt++) {
677  I_tpl_double.bitmap[cpt] -= mean2;
678  }
679 
680 #if defined _OPENMP && _OPENMP >= 200711 // OpenMP 3.1
681 #pragma omp parallel for schedule(dynamic)
682  for (unsigned int i = 0; i < I.getHeight() - height_tpl; i += step_v) {
683  for (unsigned int j = 0; j < I.getWidth() - width_tpl; j += step_u) {
684  I_score[i][j] = normalizedCorrelation(I_double, I_tpl_double, II, IIsq, II_tpl, IIsq_tpl, i, j);
685  }
686  }
687 #else
688  // error C3016: 'i': index variable in OpenMP 'for' statement must have signed integral type
689  int end = (int)((I.getHeight() - height_tpl) / step_v) + 1;
690  std::vector<unsigned int> vec_step_v((size_t)end);
691  for (unsigned int cpt = 0, idx = 0; cpt < I.getHeight() - height_tpl; cpt += step_v, idx++) {
692  vec_step_v[(size_t)idx] = cpt;
693  }
694 #if defined _OPENMP // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas]
695 #pragma omp parallel for schedule(dynamic)
696 #endif
697  for (int cpt = 0; cpt < end; cpt++) {
698  for (unsigned int j = 0; j < I.getWidth() - width_tpl; j += step_u) {
699  I_score[vec_step_v[cpt]][j] =
700  normalizedCorrelation(I_double, I_tpl_double, II, IIsq, II_tpl, IIsq_tpl, vec_step_v[cpt], j);
701  }
702  }
703 #endif
704  } else {
705  vpImage<double> I_cur;
706 
707  for (unsigned int i = 0; i < I.getHeight() - height_tpl; i += step_v) {
708  for (unsigned int j = 0; j < I.getWidth() - width_tpl; j += step_u) {
709  vpRect roi(vpImagePoint(i, j), vpImagePoint(i + height_tpl - 1, j + width_tpl - 1));
710  vpImageTools::crop(I_double, roi, I_cur);
711 
712  I_score[i][j] = vpImageTools::normalizedCorrelation(I_cur, I_tpl_double, useOptimized);
713  }
714  }
715  }
716 }
717 
718 // Reference:
719 // http://blog.demofox.org/2015/08/15/resizing-images-with-bicubic-interpolation/
720 // t is a value that goes from 0 to 1 to interpolate in a C1 continuous way
721 // across uniformly sampled data points. when t is 0, this will return B.
722 // When t is 1, this will return C. In between values will return an
723 // interpolation between B and C. A and B are used to calculate the slopes at
724 // the edges.
725 float vpImageTools::cubicHermite(const float A, const float B, const float C, const float D, const float t)
726 {
727  float a = (-A + 3.0f * B - 3.0f * C + D) / 2.0f;
728  float b = A + 2.0f * C - (5.0f * B + D) / 2.0f;
729  float c = (-A + C) / 2.0f;
730  float d = B;
731 
732  return a * t * t * t + b * t * t + c * t + d;
733 }
734 
735 int vpImageTools::coordCast(double x) { return x < 0 ? -1 : static_cast<int>(x); }
736 
737 double vpImageTools::lerp(double A, double B, double t) { return A * (1.0 - t) + B * t; }
738 
739 float vpImageTools::lerp(float A, float B, float t) { return A * (1.0f - t) + B * t; }
740 
741 int64_t vpImageTools::lerp2(int64_t A, int64_t B, int64_t t, int64_t t_1) { return A * t_1 + B * t; }
742 
744  const vpImage<double> &II, const vpImage<double> &IIsq,
745  const vpImage<double> &II_tpl, const vpImage<double> &IIsq_tpl,
746  unsigned int i0, unsigned int j0)
747 {
748  double ab = 0.0;
749  SimdNormalizedCorrelation2(I1.bitmap, I1.getWidth(), I2.bitmap, I2.getWidth(), I2.getHeight(), i0, j0, ab);
750 
751  unsigned int height_tpl = I2.getHeight(), width_tpl = I2.getWidth();
752  const double sum1 =
753  (II[i0 + height_tpl][j0 + width_tpl] + II[i0][j0] - II[i0][j0 + width_tpl] - II[i0 + height_tpl][j0]);
754  const double sum2 = (II_tpl[height_tpl][width_tpl] + II_tpl[0][0] - II_tpl[0][width_tpl] - II_tpl[height_tpl][0]);
755 
756  double a2 = ((IIsq[i0 + I2.getHeight()][j0 + I2.getWidth()] + IIsq[i0][j0] - IIsq[i0][j0 + I2.getWidth()] -
757  IIsq[i0 + I2.getHeight()][j0]) -
758  (1.0 / I2.getSize()) * vpMath::sqr(sum1));
759 
760  double b2 = ((IIsq_tpl[I2.getHeight()][I2.getWidth()] + IIsq_tpl[0][0] - IIsq_tpl[0][I2.getWidth()] -
761  IIsq_tpl[I2.getHeight()][0]) -
762  (1.0 / I2.getSize()) * vpMath::sqr(sum2));
763  return ab / sqrt(a2 * b2);
764 }
765 
777  const vpArray2D<float> &mapDu, const vpArray2D<float> &mapDv, vpImage<unsigned char> &Iundist)
778 {
779  Iundist.resize(I.getHeight(), I.getWidth());
780 
781 #if defined _OPENMP // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas]
782 #pragma omp parallel for schedule(dynamic)
783 #endif
784  for (int i_ = 0; i_ < static_cast<int>(I.getHeight()); i_++) {
785  const unsigned int i = static_cast<unsigned int>(i_);
786  for (unsigned int j = 0; j < I.getWidth(); j++) {
787 
788  int u_round = mapU[i][j];
789  int v_round = mapV[i][j];
790 
791  float du = mapDu[i][j];
792  float dv = mapDv[i][j];
793 
794  if (0 <= u_round && 0 <= v_round && u_round < static_cast<int>(I.getWidth()) - 1 &&
795  v_round < static_cast<int>(I.getHeight()) - 1) {
796  // process interpolation
797  float col0 = lerp(I[v_round][u_round], I[v_round][u_round + 1], du);
798  float col1 = lerp(I[v_round + 1][u_round], I[v_round + 1][u_round + 1], du);
799  float value = lerp(col0, col1, dv);
800 
801  Iundist[i][j] = static_cast<unsigned char>(value);
802  } else {
803  Iundist[i][j] = 0;
804  }
805  }
806  }
807 }
808 
819 void vpImageTools::remap(const vpImage<vpRGBa> &I, const vpArray2D<int> &mapU, const vpArray2D<int> &mapV,
820  const vpArray2D<float> &mapDu, const vpArray2D<float> &mapDv, vpImage<vpRGBa> &Iundist)
821 {
822  Iundist.resize(I.getHeight(), I.getWidth());
823 
824 #if defined _OPENMP // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas]
825 #pragma omp parallel for schedule(dynamic)
826 #endif
827  for (int i = 0; i < static_cast<int>(I.getHeight()); i++) {
828  SimdRemap(reinterpret_cast<unsigned char *>(I.bitmap), 4, I.getWidth(), I.getHeight(), i * I.getWidth(), mapU.data,
829  mapV.data, mapDu.data, mapDv.data, reinterpret_cast<unsigned char *>(Iundist.bitmap));
830  }
831 }
832 
833 void vpImageTools::resizeSimdlib(const vpImage<vpRGBa> &Isrc, unsigned int resizeWidth, unsigned int resizeHeight,
834  vpImage<vpRGBa> &Idst, int method)
835 {
836  Idst.resize(resizeHeight, resizeWidth);
837 
838  typedef Simd::View<Simd::Allocator> View;
839  View src(Isrc.getWidth(), Isrc.getHeight(), Isrc.getWidth() * sizeof(vpRGBa), View::Bgra32, Isrc.bitmap);
840  View dst(Idst.getWidth(), Idst.getHeight(), Idst.getWidth() * sizeof(vpRGBa), View::Bgra32, Idst.bitmap);
841 
842  Simd::Resize(src, dst, method == INTERPOLATION_LINEAR ? SimdResizeMethodBilinear : SimdResizeMethodArea);
843 }
844 
845 void vpImageTools::resizeSimdlib(const vpImage<unsigned char> &Isrc, unsigned int resizeWidth,
846  unsigned int resizeHeight, vpImage<unsigned char> &Idst, int method)
847 {
848  Idst.resize(resizeHeight, resizeWidth);
849 
850  typedef Simd::View<Simd::Allocator> View;
851  View src(Isrc.getWidth(), Isrc.getHeight(), Isrc.getWidth(), View::Gray8, Isrc.bitmap);
852  View dst(Idst.getWidth(), Idst.getHeight(), Idst.getWidth(), View::Gray8, Idst.bitmap);
853 
854  Simd::Resize(src, dst, method == INTERPOLATION_LINEAR ? SimdResizeMethodBilinear : SimdResizeMethodArea);
855 }
856 
857 bool vpImageTools::checkFixedPoint(unsigned int x, unsigned int y, const vpMatrix &T, bool affine)
858 {
859  double a0 = T[0][0];
860  double a1 = T[0][1];
861  double a2 = T[0][2];
862  double a3 = T[1][0];
863  double a4 = T[1][1];
864  double a5 = T[1][2];
865  double a6 = affine ? 0.0 : T[2][0];
866  double a7 = affine ? 0.0 : T[2][1];
867  double a8 = affine ? 1.0 : T[2][2];
868 
869  double w = a6 * x + a7 * y + a8;
870  double x2 = (a0 * x + a1 * y + a2) / w;
871  double y2 = (a3 * x + a4 * y + a5) / w;
872 
873  const double limit = 1 << 15;
874  return (vpMath::abs(x2) < limit) && (vpMath::abs(y2) < limit);
875 }
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:132
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:306
Generic class defining intrinsic camera parameters.
std::vector< double > getKannalaBrandtDistortionCoefficients() const
vpCameraParametersProjType get_projModel() const
double get_kud() const
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ notImplementedError
Not implemented.
Definition: vpException.h:93
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Error that can be emited by the vpImage class and its derivates.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
double get_j() const
Definition: vpImagePoint.h:132
double get_i() const
Definition: vpImagePoint.h:121
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:306
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 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 void extract(const vpImage< unsigned char > &Src, vpImage< unsigned char > &Dst, const vpRectOriented &r)
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:83
@ INTERPOLATION_NEAREST
Definition: vpImageTools.h:82
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 void imageDifferenceAbsolute(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, vpImage< unsigned char > &Idiff)
Type getMeanValue() const
Return the mean value of the bitmap.
Definition: vpImage.h:907
double getSum() const
Definition: vpImage.h:1589
unsigned int getWidth() const
Definition: vpImage.h:246
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:799
unsigned int getSize() const
Definition: vpImage.h:227
Type * bitmap
points toward the bitmap
Definition: vpImage.h:143
unsigned int getHeight() const
Definition: vpImage.h:188
static double sqr(double x)
Definition: vpMath.h:123
static Type abs(const Type &x)
Definition: vpMath.h:186
static int round(double x)
Definition: vpMath.h:318
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Definition: vpRGBa.h:67
unsigned char B
Blue component.
Definition: vpRGBa.h:150
unsigned char R
Red component.
Definition: vpRGBa.h:148
unsigned char G
Green component.
Definition: vpRGBa.h:149
unsigned char A
Additionnal component.
Definition: vpRGBa.h:151
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:80
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:116
void resize(unsigned int i, bool flagNullify=true)
Definition: vpRowVector.h:271
#define vpERROR_TRACE
Definition: vpDebug.h:393