Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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 
101 void vpImageTools::changeLUT(vpImage<unsigned char> &I, unsigned char A, unsigned char A_star, unsigned char B,
102  unsigned char B_star)
103 {
104  // Test if input values are valid
105  if (B <= A) {
106  vpERROR_TRACE("Bad gray levels");
108  }
109  unsigned char v;
110 
111  double factor = (double)(B_star - A_star) / (double)(B - A);
112 
113  for (unsigned int i = 0; i < I.getHeight(); i++)
114  for (unsigned int j = 0; j < I.getWidth(); j++) {
115  v = I[i][j];
116 
117  if (v <= A)
118  I[i][j] = A_star;
119  else if (v >= B)
120  I[i][j] = B_star;
121  else
122  I[i][j] = (unsigned char)(A_star + factor * (v - A));
123  }
124 }
125 
139  vpImage<unsigned char> &Idiff)
140 {
141  if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
142  throw(vpException(vpException::dimensionError, "The two images have not the same size"));
143  }
144 
145  if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth())) {
146  Idiff.resize(I1.getHeight(), I1.getWidth());
147  }
148 
149  SimdImageDifference(I1.bitmap, I2.bitmap, I1.getSize(), Idiff.bitmap);
150 }
151 
166 {
167  if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
168  throw(vpException(vpException::dimensionError, "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, saturate ? SimdOperationBinary8uSaturatedAddition : SimdOperationBinary8uAddition);
299 }
300 
315  vpImage<unsigned char> &Ires, bool saturate)
316 {
317  if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
318  throw(vpException(vpException::dimensionError, "The two images do not have the same size"));
319  }
320 
321  if ((I1.getHeight() != Ires.getHeight()) || (I1.getWidth() != Ires.getWidth())) {
322  Ires.resize(I1.getHeight(), I1.getWidth());
323  }
324 
325  typedef Simd::View<Simd::Allocator> View;
326  View img1(I1.getWidth(), I1.getHeight(), I1.getWidth(), View::Gray8, I1.bitmap);
327  View img2(I2.getWidth(), I2.getHeight(), I2.getWidth(), View::Gray8, I2.bitmap);
328  View imgAdd(Ires.getWidth(), Ires.getHeight(), Ires.getWidth(), View::Gray8, Ires.bitmap);
329 
330  Simd::OperationBinary8u(img1, img2, imgAdd, saturate ? SimdOperationBinary8uSaturatedSubtraction : SimdOperationBinary8uSubtraction);
331 }
332 
344 void vpImageTools::initUndistortMap(const vpCameraParameters &cam, unsigned int width, unsigned int height,
345  vpArray2D<int> &mapU, vpArray2D<int> &mapV,
346  vpArray2D<float> &mapDu, vpArray2D<float> &mapDv)
347 {
348  mapU.resize(height, width, false, false);
349  mapV.resize(height, width, false, false);
350  mapDu.resize(height, width, false, false);
351  mapDv.resize(height, width, false, false);
352 
354  bool is_KannalaBrandt = (projModel==vpCameraParameters::ProjWithKannalaBrandtDistortion); // Check the projection model used
355 
356  float u0 = static_cast<float>(cam.get_u0());
357  float v0 = static_cast<float>(cam.get_v0());
358  float px = static_cast<float>(cam.get_px());
359  float py = static_cast<float>(cam.get_py());
360  float kud;
361  std::vector<double> dist_coefs;
362 
363  if(!is_KannalaBrandt)
364  kud = static_cast<float>(cam.get_kud());
365 
366  else
367  dist_coefs = cam.getKannalaBrandtDistortionCoefficients();
368 
369  if (!is_KannalaBrandt && std::fabs(static_cast<double>(kud)) <= std::numeric_limits<double>::epsilon()) {
370  // There is no need to undistort the image (Perpective projection)
371  for (unsigned int i = 0; i < height; i++) {
372  for (unsigned int j = 0; j < width; j++) {
373  mapU[i][j] = static_cast<int>(j);
374  mapV[i][j] = static_cast<int>(i);
375  mapDu[i][j] = 0;
376  mapDv[i][j] = 0;
377  }
378  }
379 
380  return;
381  }
382 
383  float invpx, invpy;
384  float kud_px2 = 0., kud_py2 = 0., deltau_px, deltav_py;
385  float fr1, fr2;
386  float deltav, deltau;
387  float u_float, v_float;
388  int u_round, v_round;
389  double r, scale;
390  double theta, theta_d;
391  double theta2, theta4, theta6, theta8;
392 
393  invpx = 1.0f / px;
394  invpy = 1.0f / py;
395 
396  if(!is_KannalaBrandt)
397  {
398  kud_px2 = kud * invpx * invpx;
399  kud_py2 = kud * invpy * invpy;
400  }
401 
402  for (unsigned int v = 0; v < height; v++) {
403  deltav = v - v0;
404 
405  if(!is_KannalaBrandt)
406  fr1 = 1.0f + kud_py2 * deltav * deltav;
407  else
408  deltav_py = deltav * invpy;
409 
410  for (unsigned int u = 0; u < width; u++) {
411  // computation of u,v : corresponding pixel coordinates in I.
412  deltau = u - u0;
413  if(!is_KannalaBrandt)
414  {
415  fr2 = fr1 + kud_px2 * deltau * deltau;
416 
417  u_float = deltau * fr2 + u0;
418  v_float = deltav * fr2 + v0;
419  }
420 
421  else
422  {
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 +
433  dist_coefs[2]*theta6 + 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())) {
492  throw vpException(vpException::dimensionError, "Error: in vpImageTools::normalizedCorrelation(): "
493  "image dimension mismatch between I1=%ux%u and I2=%ux%u",
494  I1.getHeight(), I1.getWidth(), I2.getHeight(), I2.getWidth());
495  }
496 
497  const double a = I1.getMeanValue();
498  const double b = I2.getMeanValue();
499 
500  double ab = 0.0;
501  double a2 = 0.0;
502  double b2 = 0.0;
503 
504  SimdNormalizedCorrelation(I1.bitmap, a, I2.bitmap, b, I1.getSize(), a2, b2, ab, useOptimized);
505 
506  return ab / sqrt(a2 * b2);
507 }
508 
516 {
517  unsigned int height = I.getHeight(), width = I.getWidth();
518  V.resize(width); // resize and nullify
519 
520  for (unsigned int i = 0; i < height; ++i)
521  for (unsigned int j = 0; j < width; ++j)
522  V[j] += I[i][j];
523  for (unsigned int j = 0; j < width; ++j)
524  V[j] /= height;
525 }
526 
532 {
533  double s = I.getSum();
534  for (unsigned int i = 0; i < I.getHeight(); ++i)
535  for (unsigned int j = 0; j < I.getWidth(); ++j)
536  I(i, j, I(i, j) / s);
537 }
538 
547  const vpImageInterpolationType &method)
548 {
549  switch (method) {
551  return I(vpMath::round(point.get_i()), vpMath::round(point.get_j()));
552  case INTERPOLATION_LINEAR: {
553  int x1 = (int)floor(point.get_i());
554  int x2 = (int)ceil(point.get_i());
555  int y1 = (int)floor(point.get_j());
556  int y2 = (int)ceil(point.get_j());
557  double v1, v2;
558  if (x1 == x2) {
559  v1 = I(x1, y1);
560  v2 = I(x1, y2);
561  } else {
562  v1 = (x2 - point.get_i()) * I(x1, y1) + (point.get_i() - x1) * I(x2, y1);
563  v2 = (x2 - point.get_i()) * I(x1, y2) + (point.get_i() - x1) * I(x2, y2);
564  }
565  if (y1 == y2)
566  return v1;
567  return (y2 - point.get_j()) * v1 + (point.get_j() - y1) * v2;
568  }
569  case INTERPOLATION_CUBIC: {
571  "vpImageTools::interpolate(): bi-cubic interpolation is not implemented.");
572  }
573  default: {
574  throw vpException(vpException::notImplementedError, "vpImageTools::interpolate(): invalid interpolation type");
575  }
576  }
577 }
578 
586 {
587  unsigned int x_d = vpMath::round(r.getHeight());
588  unsigned int y_d = vpMath::round(r.getWidth());
589  double x1 = r.getTopLeft().get_i();
590  double y1 = r.getTopLeft().get_j();
591  double t = r.getOrientation();
592  Dst.resize(x_d, y_d);
593  for (unsigned int x = 0; x < x_d; ++x) {
594  for (unsigned int y = 0; y < y_d; ++y) {
595  Dst(x, y,
596  (unsigned char)interpolate(Src, vpImagePoint(x1 + x * cos(t) + y * sin(t), y1 - x * sin(t) + y * cos(t)),
598  }
599  }
600 }
601 
609 {
610  unsigned int x_d = vpMath::round(r.getHeight());
611  unsigned int y_d = vpMath::round(r.getWidth());
612  double x1 = r.getTopLeft().get_i();
613  double y1 = r.getTopLeft().get_j();
614  double t = r.getOrientation();
615  Dst.resize(x_d, y_d);
616  for (unsigned int x = 0; x < x_d; ++x) {
617  for (unsigned int y = 0; y < y_d; ++y) {
618  Dst(x, y, interpolate(Src, vpImagePoint(x1 + x * cos(t) + y * sin(t), y1 - x * sin(t) + y * cos(t)),
620  }
621  }
622 }
623 
639  vpImage<double> &I_score, unsigned int step_u, unsigned int step_v,
640  bool useOptimized)
641 {
642  if (I.getSize() == 0) {
643  std::cerr << "Error, input image is empty." << std::endl;
644  return;
645  }
646 
647  if (I_tpl.getSize() == 0) {
648  std::cerr << "Error, template image is empty." << std::endl;
649  return;
650  }
651 
652  if (I_tpl.getHeight() > I.getHeight() || I_tpl.getWidth() > I.getWidth()) {
653  std::cerr << "Error, template image is bigger than input image." << std::endl;
654  return;
655  }
656 
657  vpImage<double> I_double, I_tpl_double;
658  vpImageConvert::convert(I, I_double);
659  vpImageConvert::convert(I_tpl, I_tpl_double);
660 
661  unsigned int height_tpl = I_tpl.getHeight(), width_tpl = I_tpl.getWidth();
662  I_score.resize(I.getHeight() - height_tpl, I.getWidth() - width_tpl, 0.0);
663 
664  if (useOptimized) {
665  vpImage<double> II, IIsq;
666  integralImage(I, II, IIsq);
667 
668  vpImage<double> II_tpl, IIsq_tpl;
669  integralImage(I_tpl, II_tpl, IIsq_tpl);
670 
671  // zero-mean template image
672  const double sum2 = (II_tpl[height_tpl][width_tpl] + II_tpl[0][0] - II_tpl[0][width_tpl] - II_tpl[height_tpl][0]);
673  const double mean2 = sum2 / I_tpl.getSize();
674  for (unsigned int cpt = 0; cpt < I_tpl_double.getSize(); cpt++) {
675  I_tpl_double.bitmap[cpt] -= mean2;
676  }
677 
678 #if defined _OPENMP && _OPENMP >= 200711 // OpenMP 3.1
679 #pragma omp parallel for schedule(dynamic)
680  for (unsigned int i = 0; i < I.getHeight() - height_tpl; i += step_v) {
681  for (unsigned int j = 0; j < I.getWidth() - width_tpl; j += step_u) {
682  I_score[i][j] = normalizedCorrelation(I_double, I_tpl_double, II, IIsq, II_tpl, IIsq_tpl, i, j);
683  }
684  }
685 #else
686  // error C3016: 'i': index variable in OpenMP 'for' statement must have signed integral type
687  int end = (int)((I.getHeight() - height_tpl) / step_v) + 1;
688  std::vector<unsigned int> vec_step_v((size_t)end);
689  for (unsigned int cpt = 0, idx = 0; cpt < I.getHeight() - height_tpl; cpt += step_v, idx++) {
690  vec_step_v[(size_t)idx] = cpt;
691  }
692 #if defined _OPENMP // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas]
693 #pragma omp parallel for schedule(dynamic)
694 #endif
695  for (int cpt = 0; cpt < end; cpt++) {
696  for (unsigned int j = 0; j < I.getWidth() - width_tpl; j += step_u) {
697  I_score[vec_step_v[cpt]][j] =
698  normalizedCorrelation(I_double, I_tpl_double, II, IIsq, II_tpl, IIsq_tpl, vec_step_v[cpt], j);
699  }
700  }
701 #endif
702  } else {
703  vpImage<double> I_cur;
704 
705  for (unsigned int i = 0; i < I.getHeight() - height_tpl; i += step_v) {
706  for (unsigned int j = 0; j < I.getWidth() - width_tpl; j += step_u) {
707  vpRect roi(vpImagePoint(i, j), vpImagePoint(i + height_tpl - 1, j + width_tpl - 1));
708  vpImageTools::crop(I_double, roi, I_cur);
709 
710  I_score[i][j] = vpImageTools::normalizedCorrelation(I_cur, I_tpl_double, useOptimized);
711  }
712  }
713  }
714 }
715 
716 // Reference:
717 // http://blog.demofox.org/2015/08/15/resizing-images-with-bicubic-interpolation/
718 // t is a value that goes from 0 to 1 to interpolate in a C1 continuous way
719 // across uniformly sampled data points. when t is 0, this will return B.
720 // When t is 1, this will return C. In between values will return an
721 // interpolation between B and C. A and B are used to calculate the slopes at
722 // the edges.
723 float vpImageTools::cubicHermite(const float A, const float B, const float C, const float D, const float t)
724 {
725  float a = (-A + 3.0f * B - 3.0f * C + D) / 2.0f;
726  float b = A + 2.0f * C - (5.0f * B + D) / 2.0f;
727  float c = (-A + C) / 2.0f;
728  float d = B;
729 
730  return a * t * t * t + b * t * t + c * t + d;
731 }
732 
733 int vpImageTools::coordCast(double x)
734 {
735  return x < 0 ? -1 : static_cast<int>(x);
736 }
737 
738 double vpImageTools::lerp(double A, double B, double t) {
739  return A * (1.0 - t) + B * t;
740 }
741 
742 float vpImageTools::lerp(float A, float B, float t) {
743  return A * (1.0f - t) + B * t;
744 }
745 
746 int64_t vpImageTools::lerp2(int64_t A, int64_t B, int64_t t, int64_t t_1) {
747  return A * t_1 + B * t;
748 }
749 
751  const vpImage<double> &II, const vpImage<double> &IIsq,
752  const vpImage<double> &II_tpl, const vpImage<double> &IIsq_tpl,
753  unsigned int i0, unsigned int j0)
754 {
755  double ab = 0.0;
756  SimdNormalizedCorrelation2(I1.bitmap, I1.getWidth(), I2.bitmap, I2.getWidth(), I2.getHeight(), i0, j0, ab);
757 
758  unsigned int height_tpl = I2.getHeight(), width_tpl = I2.getWidth();
759  const double sum1 =
760  (II[i0 + height_tpl][j0 + width_tpl] + II[i0][j0] - II[i0][j0 + width_tpl] - II[i0 + height_tpl][j0]);
761  const double sum2 = (II_tpl[height_tpl][width_tpl] + II_tpl[0][0] - II_tpl[0][width_tpl] - II_tpl[height_tpl][0]);
762 
763  double a2 = ((IIsq[i0 + I2.getHeight()][j0 + I2.getWidth()] + IIsq[i0][j0] - IIsq[i0][j0 + I2.getWidth()] -
764  IIsq[i0 + I2.getHeight()][j0]) -
765  (1.0 / I2.getSize()) * vpMath::sqr(sum1));
766 
767  double b2 = ((IIsq_tpl[I2.getHeight()][I2.getWidth()] + IIsq_tpl[0][0] - IIsq_tpl[0][I2.getWidth()] -
768  IIsq_tpl[I2.getHeight()][0]) -
769  (1.0 / I2.getSize()) * vpMath::sqr(sum2));
770  return ab / sqrt(a2 * b2);
771 }
772 
784  const vpArray2D<float> &mapDu, const vpArray2D<float> &mapDv, vpImage<unsigned char> &Iundist)
785 {
786  Iundist.resize(I.getHeight(), I.getWidth());
787 
788 #if defined _OPENMP // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas]
789 #pragma omp parallel for schedule(dynamic)
790 #endif
791  for (int i_ = 0; i_ < static_cast<int>(I.getHeight()); i_++) {
792  const unsigned int i = static_cast<unsigned int>(i_);
793  for (unsigned int j = 0; j < I.getWidth(); j++) {
794 
795  int u_round = mapU[i][j];
796  int v_round = mapV[i][j];
797 
798  float du = mapDu[i][j];
799  float dv = mapDv[i][j];
800 
801  if (0 <= u_round && 0 <= v_round && u_round < static_cast<int>(I.getWidth()) - 1
802  && v_round < static_cast<int>(I.getHeight()) - 1) {
803  // process interpolation
804  float col0 = lerp(I[v_round][u_round], I[v_round][u_round + 1], du);
805  float col1 = lerp(I[v_round + 1][u_round], I[v_round + 1][u_round + 1], du);
806  float value = lerp(col0, col1, dv);
807 
808  Iundist[i][j] = static_cast<unsigned char>(value);
809  } else {
810  Iundist[i][j] = 0;
811  }
812  }
813  }
814 }
815 
826 void vpImageTools::remap(const vpImage<vpRGBa> &I, const vpArray2D<int> &mapU, const vpArray2D<int> &mapV,
827  const vpArray2D<float> &mapDu, const vpArray2D<float> &mapDv, vpImage<vpRGBa> &Iundist)
828 {
829  Iundist.resize(I.getHeight(), I.getWidth());
830 
831 #if defined _OPENMP // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas]
832 #pragma omp parallel for schedule(dynamic)
833 #endif
834  for (int i = 0; i < static_cast<int>(I.getHeight()); i++) {
835  SimdRemap(reinterpret_cast<unsigned char *>(I.bitmap), 4, I.getWidth(), I.getHeight(), i*I.getWidth(), mapU.data, mapV.data,
836  mapDu.data, mapDv.data, reinterpret_cast<unsigned char *>(Iundist.bitmap));
837  }
838 }
839 
840 void vpImageTools::resizeSimdlib(const vpImage<vpRGBa> &Isrc, unsigned int resizeWidth,
841  unsigned int resizeHeight, vpImage<vpRGBa> &Idst,
842  int method)
843 {
844  Idst.resize(resizeHeight, resizeWidth);
845 
846  typedef Simd::View<Simd::Allocator> View;
847  View src(Isrc.getWidth(), Isrc.getHeight(), Isrc.getWidth() * sizeof(vpRGBa), View::Bgra32, Isrc.bitmap);
848  View dst(Idst.getWidth(), Idst.getHeight(), Idst.getWidth() * sizeof(vpRGBa), View::Bgra32, Idst.bitmap);
849 
850  Simd::Resize(src, dst, method == INTERPOLATION_LINEAR ? SimdResizeMethodBilinear : SimdResizeMethodArea);
851 }
852 
853 void vpImageTools::resizeSimdlib(const vpImage<unsigned char> &Isrc, unsigned int resizeWidth,
854  unsigned int resizeHeight, vpImage<unsigned char> &Idst,
855  int method)
856 {
857  Idst.resize(resizeHeight, resizeWidth);
858 
859  typedef Simd::View<Simd::Allocator> View;
860  View src(Isrc.getWidth(), Isrc.getHeight(), Isrc.getWidth(), View::Gray8, Isrc.bitmap);
861  View dst(Idst.getWidth(), Idst.getHeight(), Idst.getWidth(), View::Gray8, Idst.bitmap);
862 
863  Simd::Resize(src, dst, method == INTERPOLATION_LINEAR ? SimdResizeMethodBilinear : SimdResizeMethodArea);
864 }
865 
866 bool vpImageTools::checkFixedPoint(unsigned int x, unsigned int y, const vpMatrix &T, bool affine)
867 {
868  double a0 = T[0][0]; double a1 = T[0][1]; double a2 = T[0][2];
869  double a3 = T[1][0]; double a4 = T[1][1]; double a5 = T[1][2];
870  double a6 = affine ? 0.0 : T[2][0];
871  double a7 = affine ? 0.0 : T[2][1];
872  double a8 = affine ? 1.0 : T[2][2];
873 
874  double w = a6 * x + a7 * y + a8;
875  double x2 = (a0 * x + a1 * y + a2) / w;
876  double y2 = (a3 * x + a4 * y + a5) / w;
877 
878  const double limit = 1 << 15;
879  return (vpMath::abs(x2) < limit) && (vpMath::abs(y2) < limit);
880 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
static void extract(const vpImage< unsigned char > &Src, vpImage< unsigned char > &Dst, const vpRectOriented &r)
double get_i() const
Definition: vpImagePoint.h:203
static void imageDifferenceAbsolute(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, vpImage< unsigned char > &Idiff)
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
static void imageAdd(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, vpImage< unsigned char > &Ires, bool saturate=false)
vpCameraParametersProjType get_projModel() const
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:800
unsigned char B
Blue component.
Definition: vpRGBa.h:150
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:115
Type * bitmap
points toward the bitmap
Definition: vpImage.h:143
double getOrientation() const
Get the rectangle orientation (rad).
#define vpERROR_TRACE
Definition: vpDebug.h:393
error that can be emited by ViSP classes.
Definition: vpException.h:71
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
Implementation of a generic 2D array used as base class for matrices and vectors. ...
Definition: vpArray2D.h:131
double getSum() const
Definition: vpImage.h:1579
Error that can be emited by the vpImage class and its derivates.
static void normalize(vpImage< double > &I)
unsigned char G
Green component.
Definition: vpRGBa.h:149
static void imageDifference(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, vpImage< unsigned char > &Idiff)
static double interpolate(const vpImage< unsigned char > &I, const vpImagePoint &point, const vpImageInterpolationType &method=INTERPOLATION_NEAREST)
Definition: vpRGBa.h:66
static Type abs(const Type &x)
Definition: vpMath.h:160
static double normalizedCorrelation(const vpImage< double > &I1, const vpImage< double > &I2, bool useOptimized=true)
double getWidth() const
Get the rectangle width.
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 double sqr(double x)
Definition: vpMath.h:116
double get_j() const
Definition: vpImagePoint.h:214
unsigned char A
Additionnal component.
Definition: vpRGBa.h:151
Generic class defining intrinsic camera parameters.
std::vector< double > getKannalaBrandtDistortionCoefficients() const
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)
Type getMeanValue() const
Return the mean value of the bitmap.
Definition: vpImage.h:907
static int round(double x)
Definition: vpMath.h:247
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 columnMean(const vpImage< double > &I, vpRowVector &result)
vpImagePoint getTopLeft() const
Get the top-left corner.
unsigned int getHeight() const
Definition: vpImage.h:188
static void integralImage(const vpImage< unsigned char > &I, vpImage< double > &II, vpImage< double > &IIsq)
double getHeight() const
Get the rectangle height.
static void crop(const vpImage< Type > &I, double roi_top, double roi_left, unsigned int roi_height, unsigned int roi_width, vpImage< Type > &crop, unsigned int v_scale=1, unsigned int h_scale=1)
Definition: vpImageTools.h:305
unsigned int getSize() const
Definition: vpImage.h:227
unsigned char R
Red component.
Definition: vpRGBa.h:148
double get_kud() const
Defines a rectangle in the plane.
Definition: vpRect.h:79
static void imageSubtract(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, vpImage< unsigned char > &Ires, bool saturate=false)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
static void changeLUT(vpImage< unsigned char > &I, unsigned char A, unsigned char newA, unsigned char B, unsigned char newB)
unsigned int getWidth() const
Definition: vpImage.h:246
void resize(unsigned int i, bool flagNullify=true)
Definition: vpRowVector.h:271
Defines an oriented rectangle in the plane.