Visual Servoing Platform  version 3.2.1 under development (2019-07-16)
vpMomentObject.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  * 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 
63 double vpMomentObject::calc_mom_polygon(unsigned int p, unsigned int q, const std::vector<vpPoint> &points)
64 {
65  unsigned int i, k, l;
66  double den, mom;
67  double x_p_k;
68  double y_l;
69  double y_q_l;
70 
71  den = static_cast<double>((p + q + 2) * (p + q + 1) * vpMath::comb(p + q, p));
72 
73  mom = 0.0;
74  for (i = 1; i <= points.size() - 1; i++) {
75  double s = 0.0;
76  double x_k = 1;
77  for (k = 0; k <= p; k++) {
78  y_l = 1;
79  x_p_k = pow(points[i - 1].get_x(), (int)(p - k));
80  for (l = 0; l <= q; l++) {
81  y_q_l = pow(points[i - 1].get_y(), (int)(q - l));
82 
83  s += static_cast<double>(vpMath::comb(k + l, l) * vpMath::comb(p + q - k - l, q - l) * x_k * x_p_k * y_l *
84  y_q_l);
85 
86  y_l *= points[i].get_y();
87  }
88  x_k *= points[i].get_x();
89  }
90 
91  s *= ((points[i - 1].get_x()) * (points[i].get_y()) - (points[i].get_x()) * (points[i - 1].get_y()));
92  mom += s;
93  }
94  mom /= den;
95  return (mom);
96 }
97 
111 void vpMomentObject::cacheValues(std::vector<double> &cache, double x, double y)
112 {
113  cache[0] = 1;
114 
115  for (unsigned int i = 1; i < order; i++)
116  cache[i] = cache[i - 1] * x;
117 
118  for (unsigned int j = order; j < order * order; j += order)
119  cache[j] = cache[j - order] * y;
120 
121  for (unsigned int j = 1; j < order; j++) {
122  for (unsigned int i = 1; i < order - j; i++) {
123  cache[j * order + i] = cache[j * order] * cache[i];
124  }
125  }
126 }
127 
132 void vpMomentObject::cacheValues(std::vector<double> &cache, double x, double y, double IntensityNormalized)
133 {
134 
135  cache[0] = IntensityNormalized;
136 
137  double invIntensityNormalized = 0.;
138  if (std::fabs(IntensityNormalized) >= std::numeric_limits<double>::epsilon())
139  invIntensityNormalized = 1.0 / IntensityNormalized;
140 
141  for (unsigned int i = 1; i < order; i++)
142  cache[i] = cache[i - 1] * x;
143 
144  for (unsigned int j = order; j < order * order; j += order)
145  cache[j] = cache[j - order] * y;
146 
147  for (unsigned int j = 1; j < order; j++) {
148  for (unsigned int i = 1; i < order - j; i++) {
149  cache[j * order + i] = cache[j * order] * cache[i] * invIntensityNormalized;
150  }
151  }
152 }
153 
230 void vpMomentObject::fromVector(std::vector<vpPoint> &points)
231 {
233  if (std::fabs(points.rbegin()->get_x() - points.begin()->get_x()) > std::numeric_limits<double>::epsilon() ||
234  std::fabs(points.rbegin()->get_y() - points.begin()->get_y()) > std::numeric_limits<double>::epsilon()) {
235  points.resize(points.size() + 1);
236  points[points.size() - 1] = points[0];
237  }
238  for (unsigned int j = 0; j < order * order; j++) {
239  values[j] = calc_mom_polygon(j % order, j / order, points);
240  }
241  } else {
242  std::vector<double> cache(order * order, 0.);
243  values.assign(order * order, 0);
244  for (unsigned int i = 0; i < points.size(); i++) {
245  cacheValues(cache, points[i].get_x(), points[i].get_y());
246  for (unsigned int j = 0; j < order; j++) {
247  for (unsigned int k = 0; k < order - j; k++) {
248  values[j * order + k] += cache[j * order + k];
249  }
250  }
251  }
252  }
253 }
254 
289 void vpMomentObject::fromImage(const vpImage<unsigned char> &image, unsigned char threshold,
290  const vpCameraParameters &cam)
291 {
292 #ifdef VISP_HAVE_OPENMP
293 #pragma omp parallel shared(threshold)
294  {
295  std::vector<double> curvals(order * order);
296  curvals.assign(order * order, 0.);
297 
298 #pragma omp for nowait // automatically organize loop counter between threads
299  for (int i = 0; i < (int)image.getCols(); i++) {
300  for (int j = 0; j < (int)image.getRows(); j++) {
301  unsigned int i_ = static_cast<unsigned int>(i);
302  unsigned int j_ = static_cast<unsigned int>(j);
303  if (image[j_][i_] > threshold) {
304  double x = 0;
305  double y = 0;
306  vpPixelMeterConversion::convertPoint(cam, i_, j_, x, y);
307 
308  double yval = 1.;
309  for (unsigned int k = 0; k < order; k++) {
310  double xval = 1.;
311  for (unsigned int l = 0; l < order - k; l++) {
312  curvals[(k * order + l)] += (xval * yval);
313  xval *= x;
314  }
315  yval *= y;
316  }
317  }
318  }
319  }
320 
321 #pragma omp master // only set this variable in master thread
322  {
323  values.assign(order * order, 0.);
324  }
325 
326 #pragma omp barrier
327  for (unsigned int k = 0; k < order; k++) {
328  for (unsigned int l = 0; l < order - k; l++) {
329 #pragma omp atomic
330  values[k * order + l] += curvals[k * order + l];
331  }
332  }
333  }
334 #else
335  std::vector<double> cache(order * order, 0.);
336  values.assign(order * order, 0);
337  for (unsigned int i = 0; i < image.getCols(); i++) {
338  for (unsigned int j = 0; j < image.getRows(); j++) {
339  if (image[j][i] > threshold) {
340  double x = 0;
341  double y = 0;
342  vpPixelMeterConversion::convertPoint(cam, i, j, x, y);
343  cacheValues(cache, x, y);
344  for (unsigned int k = 0; k < order; k++) {
345  for (unsigned int l = 0; l < order - k; l++) {
346  values[k * order + l] += cache[k * order + l];
347  }
348  }
349  }
350  }
351  }
352 #endif
353 
354  // Normalisation equivalent to sampling interval/pixel size delX x delY
355  double norm_factor = 1. / (cam.get_px() * cam.get_py());
356  for (std::vector<double>::iterator it = values.begin(); it != values.end(); ++it) {
357  *it = (*it) * norm_factor;
358  }
359 }
360 
374  vpCameraImgBckGrndType bg_type, bool normalize_with_pix_size)
375 {
376  std::vector<double> cache(order * order, 0.);
377  values.assign(order * order, 0);
378 
379  // (x,y) - Pixel co-ordinates in metres
380  double x = 0;
381  double y = 0;
382  // for indexing into cache[] and values[]
383  unsigned int idx = 0;
384  unsigned int kidx = 0;
385 
386  double intensity = 0;
387 
388  // double Imax = static_cast<double>(image.getMaxValue());
389 
390  double iscale = 1.0;
391  if (flg_normalize_intensity) { // This makes the image a probability density
392  // function
393  double Imax = 255.; // To check the effect of gray level change. ISR Coimbra
394  iscale = 1.0 / Imax;
395  }
396 
397  if (bg_type == vpMomentObject::WHITE) {
399  for (unsigned int j = 0; j < image.getRows(); j++) {
400  for (unsigned int i = 0; i < image.getCols(); i++) {
401  x = 0;
402  y = 0;
403  intensity = (double)(image[j][i]) * iscale;
404  double intensity_white = 1. - intensity;
405 
406  vpPixelMeterConversion::convertPoint(cam, i, j, x, y);
407  cacheValues(cache, x, y, intensity_white); // Modify 'cache' which has
408  // x^p*y^q to x^p*y^q*(1 -
409  // I(x,y))
410 
411  // Copy to "values"
412  for (unsigned int k = 0; k < order; k++) {
413  kidx = k * order;
414  for (unsigned int l = 0; l < order - k; l++) {
415  idx = kidx + l;
416  values[idx] += cache[idx];
417  }
418  }
419  }
420  }
421  } else {
423  for (unsigned int j = 0; j < image.getRows(); j++) {
424  for (unsigned int i = 0; i < image.getCols(); i++) {
425  x = 0;
426  y = 0;
427  intensity = (double)(image[j][i]) * iscale;
428  vpPixelMeterConversion::convertPoint(cam, i, j, x, y);
429 
430  // Cache values for fast moment calculation
431  cacheValues(cache, x, y,
432  intensity); // Modify 'cache' which has x^p*y^q to x^p*y^q*I(x,y)
433 
434  // Copy to moments array 'values'
435  for (unsigned int k = 0; k < order; k++) {
436  kidx = k * order;
437  for (unsigned int l = 0; l < order - k; l++) {
438  idx = kidx + l;
439  values[idx] += cache[idx];
440  }
441  }
442  }
443  }
444  }
445 
446  if (normalize_with_pix_size) {
447  // Normalisation equivalent to sampling interval/pixel size delX x delY
448  double norm_factor = 1. / (cam.get_px() * cam.get_py());
449  for (std::vector<double>::iterator it = values.begin(); it != values.end(); ++it) {
450  *it = (*it) * norm_factor;
451  }
452  }
453 }
454 
459 void vpMomentObject::init(unsigned int orderinp)
460 {
461  order = orderinp + 1;
463  flg_normalize_intensity = true; // By default, the intensity values are normalized
464  values.resize((order + 1) * (order + 1));
465  values.assign((order + 1) * (order + 1), 0);
466 }
467 
472 {
473  order = objin.getOrder() + 1;
474  type = objin.getType();
476  values.resize(objin.values.size());
477  values = objin.values;
478 }
479 
495 vpMomentObject::vpMomentObject(unsigned int max_order)
497 {
498  init(max_order);
499 }
500 
506 {
507  init(srcobj);
508 }
509 
532 const std::vector<double> &vpMomentObject::get() const { return values; }
533 
540 double vpMomentObject::get(unsigned int i, unsigned int j) const
541 {
542  assert(i + j <= getOrder());
543  if (i + j >= order)
544  throw vpException(vpException::badValue, "The requested value has not "
545  "been computed, you should "
546  "specify a higher order.");
547 
548  return values[j * order + i];
549 }
550 
557 void vpMomentObject::set(unsigned int i, unsigned int j, const double &value_ij)
558 {
559  assert(i + j <= getOrder());
560  if (i + j >= order)
561  throw vpException(vpException::badValue, "The requested value cannot be set, you should specify "
562  "a higher order for the moment object.");
563  values[j * order + i] = value_ij;
564 }
565 
582 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentObject &m)
583 {
584  for (unsigned int i = 0; i < m.values.size(); i++) {
585 
586  if (i % (m.order) == 0)
587  os << std::endl;
588 
589  if ((i % (m.order) + i / (m.order)) < m.order)
590  os << m.values[i];
591  else
592  os << "x";
593 
594  os << "\t";
595  }
596 
597  return os;
598 }
599 
605 void vpMomentObject::printWithIndices(const vpMomentObject &momobj, std::ostream &os)
606 {
607  std::vector<double> moment = momobj.get();
608  os << std::endl << "Order of vpMomentObject: " << momobj.getOrder() << std::endl;
609  // Print out values. This is same as printing using operator <<
610  for (unsigned int k = 0; k <= momobj.getOrder(); k++) {
611  for (unsigned int l = 0; l < (momobj.getOrder() + 1) - k; l++) {
612  os << "m[" << l << "," << k << "] = " << moment[k * (momobj.getOrder() + 1) + l] << "\t";
613  }
614  os << std::endl;
615  }
616  os << std::endl;
617 }
618 
647 {
648  std::vector<double> moment = momobj.get();
649  unsigned int order = momobj.getOrder();
650  vpMatrix M(order + 1, order + 1);
651  for (unsigned int k = 0; k <= order; k++) {
652  for (unsigned int l = 0; l < (order + 1) - k; l++) {
653  M[l][k] = moment[k * (order + 1) + l];
654  }
655  }
656  return M;
657 }
658 
668 {
669  // deliberate empty
670 }
unsigned int getCols() const
Definition: vpImage.h:177
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpMomentObject(unsigned int order)
static vpMatrix convertTovpMatrix(const vpMomentObject &momobj)
static void printWithIndices(const vpMomentObject &momobj, std::ostream &os)
error that can be emited by ViSP classes.
Definition: vpException.h:71
Class for generic objects.
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
const std::vector< double > & get() const
double get_py() const
virtual ~vpMomentObject()
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpMomentObject &v)
void fromImage(const vpImage< unsigned char > &image, unsigned char threshold, const vpCameraParameters &cam)
void init(unsigned int orderinp)
unsigned int getRows() const
Definition: vpImage.h:216
void cacheValues(std::vector< double > &cache, double x, double y)
void set(unsigned int i, unsigned int j, const double &value_ij)
Generic class defining intrinsic camera parameters.
bool flg_normalize_intensity
unsigned int order
void fromVector(std::vector< vpPoint > &points)
double get_px() const
std::vector< double > values
vpObjectType getType() const
vpObjectType type
static long double comb(unsigned int n, unsigned int p)
Definition: vpMath.h:226
unsigned int getOrder() const