Visual Servoing Platform version 3.5.0
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
63double 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
111void 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
132void 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
228void vpMomentObject::fromVector(std::vector<vpPoint> &points)
229{
231 if (std::fabs(points.rbegin()->get_x() - points.begin()->get_x()) > std::numeric_limits<double>::epsilon() ||
232 std::fabs(points.rbegin()->get_y() - points.begin()->get_y()) > std::numeric_limits<double>::epsilon()) {
233 points.resize(points.size() + 1);
234 points[points.size() - 1] = points[0];
235 }
236 for (unsigned int j = 0; j < order * order; j++) {
237 values[j] = calc_mom_polygon(j % order, j / order, points);
238 }
239 } else {
240 std::vector<double> cache(order * order, 0.);
241 values.assign(order * order, 0);
242 for (unsigned int i = 0; i < points.size(); i++) {
243 cacheValues(cache, points[i].get_x(), points[i].get_y());
244 for (unsigned int j = 0; j < order; j++) {
245 for (unsigned int k = 0; k < order - j; k++) {
246 values[j * order + k] += cache[j * order + k];
247 }
248 }
249 }
250 }
251}
252
287void vpMomentObject::fromImage(const vpImage<unsigned char> &image, unsigned char threshold,
288 const vpCameraParameters &cam)
289{
290#ifdef VISP_HAVE_OPENMP
291#pragma omp parallel shared(threshold)
292 {
293 std::vector<double> curvals(order * order);
294 curvals.assign(order * order, 0.);
295
296#pragma omp for nowait // automatically organize loop counter between threads
297 for (int i = 0; i < (int)image.getCols(); i++) {
298 for (int j = 0; j < (int)image.getRows(); j++) {
299 unsigned int i_ = static_cast<unsigned int>(i);
300 unsigned int j_ = static_cast<unsigned int>(j);
301 if (image[j_][i_] > threshold) {
302 double x = 0;
303 double y = 0;
304 vpPixelMeterConversion::convertPoint(cam, i_, j_, x, y);
305
306 double yval = 1.;
307 for (unsigned int k = 0; k < order; k++) {
308 double xval = 1.;
309 for (unsigned int l = 0; l < order - k; l++) {
310 curvals[(k * order + l)] += (xval * yval);
311 xval *= x;
312 }
313 yval *= y;
314 }
315 }
316 }
317 }
318
319#pragma omp master // only set this variable in master thread
320 {
321 values.assign(order * order, 0.);
322 }
323
324#pragma omp barrier
325 for (unsigned int k = 0; k < order; k++) {
326 for (unsigned int l = 0; l < order - k; l++) {
327#pragma omp atomic
328 values[k * order + l] += curvals[k * order + l];
329 }
330 }
331 }
332#else
333 std::vector<double> cache(order * order, 0.);
334 values.assign(order * order, 0);
335 for (unsigned int i = 0; i < image.getCols(); i++) {
336 for (unsigned int j = 0; j < image.getRows(); j++) {
337 if (image[j][i] > threshold) {
338 double x = 0;
339 double y = 0;
341 cacheValues(cache, x, y);
342 for (unsigned int k = 0; k < order; k++) {
343 for (unsigned int l = 0; l < order - k; l++) {
344 values[k * order + l] += cache[k * order + l];
345 }
346 }
347 }
348 }
349 }
350#endif
351
352 // Normalisation equivalent to sampling interval/pixel size delX x delY
353 double norm_factor = 1. / (cam.get_px() * cam.get_py());
354 for (std::vector<double>::iterator it = values.begin(); it != values.end(); ++it) {
355 *it = (*it) * norm_factor;
356 }
357}
358
371 vpCameraImgBckGrndType bg_type, bool normalize_with_pix_size)
372{
373 std::vector<double> cache(order * order, 0.);
374 values.assign(order * order, 0);
375
376 // (x,y) - Pixel co-ordinates in metres
377 double x = 0;
378 double y = 0;
379 // for indexing into cache[] and values[]
380 unsigned int idx = 0;
381 unsigned int kidx = 0;
382
383 double intensity = 0;
384
385 // double Imax = static_cast<double>(image.getMaxValue());
386
387 double iscale = 1.0;
388 if (flg_normalize_intensity) { // This makes the image a probability density
389 // function
390 double Imax = 255.; // To check the effect of gray level change. ISR Coimbra
391 iscale = 1.0 / Imax;
392 }
393
394 if (bg_type == vpMomentObject::WHITE) {
396 for (unsigned int j = 0; j < image.getRows(); j++) {
397 for (unsigned int i = 0; i < image.getCols(); i++) {
398 x = 0;
399 y = 0;
400 intensity = (double)(image[j][i]) * iscale;
401 double intensity_white = 1. - intensity;
402
404 cacheValues(cache, x, y, intensity_white); // Modify 'cache' which has
405 // x^p*y^q to x^p*y^q*(1 -
406 // I(x,y))
407
408 // Copy to "values"
409 for (unsigned int k = 0; k < order; k++) {
410 kidx = k * order;
411 for (unsigned int l = 0; l < order - k; l++) {
412 idx = kidx + l;
413 values[idx] += cache[idx];
414 }
415 }
416 }
417 }
418 } else {
420 for (unsigned int j = 0; j < image.getRows(); j++) {
421 for (unsigned int i = 0; i < image.getCols(); i++) {
422 x = 0;
423 y = 0;
424 intensity = (double)(image[j][i]) * iscale;
426
427 // Cache values for fast moment calculation
428 cacheValues(cache, x, y,
429 intensity); // Modify 'cache' which has x^p*y^q to x^p*y^q*I(x,y)
430
431 // Copy to moments array 'values'
432 for (unsigned int k = 0; k < order; k++) {
433 kidx = k * order;
434 for (unsigned int l = 0; l < order - k; l++) {
435 idx = kidx + l;
436 values[idx] += cache[idx];
437 }
438 }
439 }
440 }
441 }
442
443 if (normalize_with_pix_size) {
444 // Normalisation equivalent to sampling interval/pixel size delX x delY
445 double norm_factor = 1. / (cam.get_px() * cam.get_py());
446 for (std::vector<double>::iterator it = values.begin(); it != values.end(); ++it) {
447 *it = (*it) * norm_factor;
448 }
449 }
450}
451
456void vpMomentObject::init(unsigned int orderinp)
457{
458 order = orderinp + 1;
460 flg_normalize_intensity = true; // By default, the intensity values are normalized
461 values.resize((order + 1) * (order + 1));
462 values.assign((order + 1) * (order + 1), 0);
463}
464
469{
470 order = objin.getOrder() + 1;
471 type = objin.getType();
473 values.resize(objin.values.size());
474 values = objin.values;
475}
476
492vpMomentObject::vpMomentObject(unsigned int max_order)
493 : flg_normalize_intensity(true), order(max_order + 1), type(vpMomentObject::DENSE_FULL_OBJECT), values()
494{
495 init(max_order);
496}
497
502 : flg_normalize_intensity(true), order(1), type(vpMomentObject::DENSE_FULL_OBJECT), values()
503{
504 init(srcobj);
505}
506
528const std::vector<double> &vpMomentObject::get() const { return values; }
529
536double vpMomentObject::get(unsigned int i, unsigned int j) const
537{
538 assert(i + j <= getOrder());
539 if (i + j >= order)
540 throw vpException(vpException::badValue, "The requested value has not "
541 "been computed, you should "
542 "specify a higher order.");
543
544 return values[j * order + i];
545}
546
553void vpMomentObject::set(unsigned int i, unsigned int j, const double &value_ij)
554{
555 assert(i + j <= getOrder());
556 if (i + j >= order)
557 throw vpException(vpException::badValue, "The requested value cannot be set, you should specify "
558 "a higher order for the moment object.");
559 values[j * order + i] = value_ij;
560}
561
577VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentObject &m)
578{
579 for (unsigned int i = 0; i < m.values.size(); i++) {
580
581 if (i % (m.order) == 0)
582 os << std::endl;
583
584 if ((i % (m.order) + i / (m.order)) < m.order)
585 os << m.values[i];
586 else
587 os << "x";
588
589 os << "\t";
590 }
591
592 return os;
593}
594
600void vpMomentObject::printWithIndices(const vpMomentObject &momobj, std::ostream &os)
601{
602 std::vector<double> moment = momobj.get();
603 os << std::endl << "Order of vpMomentObject: " << momobj.getOrder() << std::endl;
604 // Print out values. This is same as printing using operator <<
605 for (unsigned int k = 0; k <= momobj.getOrder(); k++) {
606 for (unsigned int l = 0; l < (momobj.getOrder() + 1) - k; l++) {
607 os << "m[" << l << "," << k << "] = " << moment[k * (momobj.getOrder() + 1) + l] << "\t";
608 }
609 os << std::endl;
610 }
611 os << std::endl;
612}
613
641{
642 std::vector<double> moment = momobj.get();
643 unsigned int order = momobj.getOrder();
644 vpMatrix M(order + 1, order + 1);
645 for (unsigned int k = 0; k <= order; k++) {
646 for (unsigned int l = 0; l < (order + 1) - k; l++) {
647 M[l][k] = moment[k * (order + 1) + l];
648 }
649 }
650 return M;
651}
652
662{
663 // deliberate empty
664}
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:493
Generic class defining intrinsic camera parameters.
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
unsigned int getCols() const
Definition: vpImage.h:179
unsigned int getRows() const
Definition: vpImage.h:218
static long double comb(unsigned int n, unsigned int p)
Definition: vpMath.h:232
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Class for generic objects.
static void printWithIndices(const vpMomentObject &momobj, std::ostream &os)
unsigned int getOrder() const
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)