Visual Servoing Platform  version 3.5.0
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 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:304
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:88
double get_j() const
Definition: vpImagePoint.h:214
double get_i() const
Definition: vpImagePoint.h:203
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:305
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:1579
unsigned int getWidth() const
Definition: vpImage.h:246
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:800
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:116
static Type abs(const Type &x)
Definition: vpMath.h:160
static int round(double x)
Definition: vpMath.h:247
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