Visual Servoing Platform  version 3.6.1 under development (2024-07-17)
vpMomentObject.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  * Object input structure used by moments.
33  *
34  * Authors:
35  * Filip Novotny
36  *
37 *****************************************************************************/
38 
39 #include <stdexcept>
40 #include <visp3/core/vpCameraParameters.h>
41 #include <visp3/core/vpConfig.h>
42 #include <visp3/core/vpMomentBasic.h>
43 #include <visp3/core/vpMomentObject.h>
44 #include <visp3/core/vpPixelMeterConversion.h>
45 
46 #include <cmath>
47 #include <limits>
48 
49 #ifdef VISP_HAVE_OPENMP
50 #include <omp.h>
51 #endif
52 #include <cassert>
53 
64 double vpMomentObject::calc_mom_polygon(unsigned int p, unsigned int q, const std::vector<vpPoint> &points)
65 {
66  unsigned int i, k, l;
67  double den, mom;
68  double x_p_k;
69  double y_l;
70  double y_q_l;
71 
72  den = static_cast<double>((p + q + 2) * (p + q + 1) * vpMath::comb(p + q, p));
73 
74  mom = 0.0;
75  for (i = 1; i <= points.size() - 1; i++) {
76  double s = 0.0;
77  double x_k = 1;
78  for (k = 0; k <= p; k++) {
79  y_l = 1;
80  x_p_k = pow(points[i - 1].get_x(), (int)(p - k));
81  for (l = 0; l <= q; l++) {
82  y_q_l = pow(points[i - 1].get_y(), (int)(q - l));
83 
84  s += static_cast<double>(vpMath::comb(k + l, l) * vpMath::comb(p + q - k - l, q - l) * x_k * x_p_k * y_l *
85  y_q_l);
86 
87  y_l *= points[i].get_y();
88  }
89  x_k *= points[i].get_x();
90  }
91 
92  s *= ((points[i - 1].get_x()) * (points[i].get_y()) - (points[i].get_x()) * (points[i - 1].get_y()));
93  mom += s;
94  }
95  mom /= den;
96  return (mom);
97 }
98 
112 void vpMomentObject::cacheValues(std::vector<double> &cache, double x, double y)
113 {
114  cache[0] = 1;
115 
116  for (unsigned int i = 1; i < order; i++)
117  cache[i] = cache[i - 1] * x;
118 
119  for (unsigned int j = order; j < order * order; j += order)
120  cache[j] = cache[j - order] * y;
121 
122  for (unsigned int j = 1; j < order; j++) {
123  for (unsigned int i = 1; i < order - j; i++) {
124  cache[j * order + i] = cache[j * order] * cache[i];
125  }
126  }
127 }
128 
133 void vpMomentObject::cacheValues(std::vector<double> &cache, double x, double y, double IntensityNormalized)
134 {
135 
136  cache[0] = IntensityNormalized;
137 
138  double invIntensityNormalized = 0.;
139  if (std::fabs(IntensityNormalized) >= std::numeric_limits<double>::epsilon())
140  invIntensityNormalized = 1.0 / IntensityNormalized;
141 
142  for (unsigned int i = 1; i < order; i++)
143  cache[i] = cache[i - 1] * x;
144 
145  for (unsigned int j = order; j < order * order; j += order)
146  cache[j] = cache[j - order] * y;
147 
148  for (unsigned int j = 1; j < order; j++) {
149  for (unsigned int i = 1; i < order - j; i++) {
150  cache[j * order + i] = cache[j * order] * cache[i] * invIntensityNormalized;
151  }
152  }
153 }
154 
236 void vpMomentObject::fromVector(std::vector<vpPoint> &points)
237 {
239  if (std::fabs(points.rbegin()->get_x() - points.begin()->get_x()) > std::numeric_limits<double>::epsilon() ||
240  std::fabs(points.rbegin()->get_y() - points.begin()->get_y()) > std::numeric_limits<double>::epsilon()) {
241  points.resize(points.size() + 1);
242  points[points.size() - 1] = points[0];
243  }
244  for (unsigned int j = 0; j < order * order; j++) {
245  values[j] = calc_mom_polygon(j % order, j / order, points);
246  }
247  }
248  else {
249  std::vector<double> cache(order * order, 0.);
250  values.assign(order * order, 0);
251  for (unsigned int i = 0; i < points.size(); i++) {
252  cacheValues(cache, points[i].get_x(), points[i].get_y());
253  for (unsigned int j = 0; j < order; j++) {
254  for (unsigned int k = 0; k < order - j; k++) {
255  values[j * order + k] += cache[j * order + k];
256  }
257  }
258  }
259  }
260 }
261 
300 void vpMomentObject::fromImage(const vpImage<unsigned char> &image, unsigned char threshold,
301  const vpCameraParameters &cam)
302 {
303 #ifdef VISP_HAVE_OPENMP
304 #pragma omp parallel shared(threshold)
305  {
306  std::vector<double> curvals(order * order);
307  curvals.assign(order * order, 0.);
308 
309 #pragma omp for nowait // automatically organize loop counter between threads
310  for (int i = 0; i < (int)image.getCols(); i++) {
311  for (int j = 0; j < (int)image.getRows(); j++) {
312  unsigned int i_ = static_cast<unsigned int>(i);
313  unsigned int j_ = static_cast<unsigned int>(j);
314  if (image[j_][i_] > threshold) {
315  double x = 0;
316  double y = 0;
317  vpPixelMeterConversion::convertPoint(cam, i_, j_, x, y);
318 
319  double yval = 1.;
320  for (unsigned int k = 0; k < order; k++) {
321  double xval = 1.;
322  for (unsigned int l = 0; l < order - k; l++) {
323  curvals[(k * order + l)] += (xval * yval);
324  xval *= x;
325  }
326  yval *= y;
327  }
328  }
329  }
330  }
331 
332 #pragma omp master // only set this variable in master thread
333  {
334  values.assign(order * order, 0.);
335  }
336 
337 #pragma omp barrier
338  for (unsigned int k = 0; k < order; k++) {
339  for (unsigned int l = 0; l < order - k; l++) {
340 #pragma omp atomic
341  values[k * order + l] += curvals[k * order + l];
342  }
343  }
344  }
345 #else
346  std::vector<double> cache(order * order, 0.);
347  values.assign(order * order, 0);
348  for (unsigned int i = 0; i < image.getCols(); i++) {
349  for (unsigned int j = 0; j < image.getRows(); j++) {
350  if (image[j][i] > threshold) {
351  double x = 0;
352  double y = 0;
353  vpPixelMeterConversion::convertPoint(cam, i, j, x, y);
354  cacheValues(cache, x, y);
355  for (unsigned int k = 0; k < order; k++) {
356  for (unsigned int l = 0; l < order - k; l++) {
357  values[k * order + l] += cache[k * order + l];
358  }
359  }
360  }
361  }
362  }
363 #endif
364 
365  // Normalization equivalent to sampling interval/pixel size delX x delY
366  double norm_factor = 1. / (cam.get_px() * cam.get_py());
367  for (std::vector<double>::iterator it = values.begin(); it != values.end(); ++it) {
368  *it = (*it) * norm_factor;
369  }
370 }
371 
384  vpCameraImgBckGrndType bg_type, bool normalize_with_pix_size)
385 {
386  std::vector<double> cache(order * order, 0.);
387  values.assign(order * order, 0);
388 
389  // (x,y) - Pixel co-ordinates in metres
390  double x = 0;
391  double y = 0;
392  // for indexing into cache[] and values[]
393  unsigned int idx = 0;
394  unsigned int kidx = 0;
395 
396  double intensity = 0;
397 
398  // double Imax = static_cast<double>(image.getMaxValue());
399 
400  double iscale = 1.0;
401  if (flg_normalize_intensity) { // This makes the image a probability density
402  // function
403  double Imax = 255.; // To check the effect of gray level change. ISR Coimbra
404  iscale = 1.0 / Imax;
405  }
406 
407  if (bg_type == vpMomentObject::WHITE) {
409  for (unsigned int j = 0; j < image.getRows(); j++) {
410  for (unsigned int i = 0; i < image.getCols(); i++) {
411  x = 0;
412  y = 0;
413  intensity = (double)(image[j][i]) * iscale;
414  double intensity_white = 1. - intensity;
415 
416  vpPixelMeterConversion::convertPoint(cam, i, j, x, y);
417  cacheValues(cache, x, y, intensity_white); // Modify 'cache' which has
418  // x^p*y^q to x^p*y^q*(1 -
419  // I(x,y))
420 
421  // Copy to "values"
422  for (unsigned int k = 0; k < order; k++) {
423  kidx = k * order;
424  for (unsigned int l = 0; l < order - k; l++) {
425  idx = kidx + l;
426  values[idx] += cache[idx];
427  }
428  }
429  }
430  }
431  }
432  else {
434  for (unsigned int j = 0; j < image.getRows(); j++) {
435  for (unsigned int i = 0; i < image.getCols(); i++) {
436  x = 0;
437  y = 0;
438  intensity = (double)(image[j][i]) * iscale;
439  vpPixelMeterConversion::convertPoint(cam, i, j, x, y);
440 
441  // Cache values for fast moment calculation
442  cacheValues(cache, x, y,
443  intensity); // Modify 'cache' which has x^p*y^q to x^p*y^q*I(x,y)
444 
445  // Copy to moments array 'values'
446  for (unsigned int k = 0; k < order; k++) {
447  kidx = k * order;
448  for (unsigned int l = 0; l < order - k; l++) {
449  idx = kidx + l;
450  values[idx] += cache[idx];
451  }
452  }
453  }
454  }
455  }
456 
457  if (normalize_with_pix_size) {
458  // Normalization equivalent to sampling interval/pixel size delX x delY
459  double norm_factor = 1. / (cam.get_px() * cam.get_py());
460  for (std::vector<double>::iterator it = values.begin(); it != values.end(); ++it) {
461  *it = (*it) * norm_factor;
462  }
463  }
464 }
465 
470 void vpMomentObject::init(unsigned int orderinp)
471 {
472  order = orderinp + 1;
474  flg_normalize_intensity = true; // By default, the intensity values are normalized
475  values.resize((order + 1) * (order + 1));
476  values.assign((order + 1) * (order + 1), 0);
477 }
478 
483 {
484  order = objin.getOrder() + 1;
485  type = objin.getType();
487  values.resize(objin.values.size());
488  values = objin.values;
489 }
490 
506 vpMomentObject::vpMomentObject(unsigned int max_order)
507  : flg_normalize_intensity(true), order(max_order + 1), type(vpMomentObject::DENSE_FULL_OBJECT), values()
508 {
509  init(max_order);
510 }
511 
516  : flg_normalize_intensity(true), order(1), type(vpMomentObject::DENSE_FULL_OBJECT), values()
517 {
518  init(srcobj);
519 }
520 
542 const std::vector<double> &vpMomentObject::get() const { return values; }
543 
550 double vpMomentObject::get(unsigned int i, unsigned int j) const
551 {
552  assert(i + j <= getOrder());
553  if (i + j >= order)
554  throw vpException(vpException::badValue, "The requested value has not "
555  "been computed, you should "
556  "specify a higher order.");
557 
558  return values[j * order + i];
559 }
560 
567 void vpMomentObject::set(unsigned int i, unsigned int j, const double &value_ij)
568 {
569  assert(i + j <= getOrder());
570  if (i + j >= order)
571  throw vpException(vpException::badValue, "The requested value cannot be set, you should specify "
572  "a higher order for the moment object.");
573  values[j * order + i] = value_ij;
574 }
575 
581 void vpMomentObject::printWithIndices(const vpMomentObject &momobj, std::ostream &os)
582 {
583  std::vector<double> moment = momobj.get();
584  os << std::endl << "Order of vpMomentObject: " << momobj.getOrder() << std::endl;
585  // Print out values. This is same as printing using operator <<
586  for (unsigned int k = 0; k <= momobj.getOrder(); k++) {
587  for (unsigned int l = 0; l < (momobj.getOrder() + 1) - k; l++) {
588  os << "m[" << l << "," << k << "] = " << moment[k * (momobj.getOrder() + 1) + l] << "\t";
589  }
590  os << std::endl;
591  }
592  os << std::endl;
593 }
594 
622 {
623  std::vector<double> moment = momobj.get();
624  unsigned int order = momobj.getOrder();
625  vpMatrix M(order + 1, order + 1);
626  for (unsigned int k = 0; k <= order; k++) {
627  for (unsigned int l = 0; l < (order + 1) - k; l++) {
628  M[l][k] = moment[k * (order + 1) + l];
629  }
630  }
631  return M;
632 }
633 
643 {
644  // deliberate empty
645 }
646 
662 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentObject &m)
663 {
664  for (unsigned int i = 0; i < m.values.size(); ++i) {
665 
666  if (i % (m.order) == 0)
667  os << std::endl;
668 
669  if ((i % (m.order) + i / (m.order)) < m.order) {
670  os << m.values[i];
671  }
672  else {
673  os << "x";
674  }
675 
676  os << "\t";
677  }
678 
679  return os;
680 }
681 END_VISP_NAMESPACE
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:611
Generic class defining intrinsic camera parameters.
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:73
unsigned int getCols() const
Definition: vpImage.h:171
unsigned int getRows() const
Definition: vpImage.h:212
static long double comb(unsigned int n, unsigned int p)
Definition: vpMath.h:394
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
Class for generic objects.
static void printWithIndices(const vpMomentObject &momobj, std::ostream &os)
unsigned int getOrder() const
VP_EXPLICIT vpMomentObject(unsigned int order)
unsigned int order
virtual ~vpMomentObject()
void cacheValues(std::vector< double > &cache, double x, double y)
vpObjectType type
bool flg_normalize_intensity
void set(unsigned int i, unsigned int j, const double &value_ij)
@ WHITE
Not functional right now.
const std::vector< double > & get() const
static vpMatrix convertTovpMatrix(const vpMomentObject &momobj)
std::vector< double > values
vpObjectType getType() const
void fromImage(const vpImage< unsigned char > &image, unsigned char threshold, const vpCameraParameters &cam)
void fromVector(std::vector< vpPoint > &points)
void init(unsigned int orderinp)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)