Visual Servoing Platform  version 3.0.0
vpMomentObject.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Object input structure used by moments.
32  *
33  * Authors:
34  * Filip Novotny
35  *
36  *****************************************************************************/
37 
38 #include <visp3/core/vpMomentBasic.h>
39 #include <visp3/core/vpMomentObject.h>
40 #include <visp3/core/vpCameraParameters.h>
41 #include <visp3/core/vpPixelMeterConversion.h>
42 #include <visp3/core/vpConfig.h>
43 #include <stdexcept>
44 
45 #include <cmath>
46 #include <limits>
47 
48 #ifdef VISP_HAVE_OPENMP
49 #include <omp.h>
50 #endif
51 #include <cassert>
52 
62 double vpMomentObject::calc_mom_polygon(unsigned int p, unsigned int q, const std::vector<vpPoint>& points){
63  unsigned int i,k,l;
64  double den,mom,s;
65  double x_k;
66  double x_p_k;
67  double y_l;
68  double y_q_l;
69 
70  den = static_cast<double>( (p+q+2)*(p+q+1)*vpMath::comb(p+q,p) );
71 
72  mom = 0.0;
73  for (i=1;i<=points.size()-1;i++)
74  {
75  s = 0.0;
76  x_k=1;
77  for (k=0;k<=p;k++)
78  {
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  {
83  y_q_l=pow(points[i-1].get_y(), (int)(q-l));
84 
85  s += static_cast<double>(
86  vpMath::comb(k+l,l)
87  *vpMath::comb(p+q-k-l,q-l)
88  *x_k
89  *x_p_k
90  *y_l
91  *y_q_l );
92 
93  y_l*=points[i].get_y();
94 
95  }
96  x_k*=points[i].get_x();
97 
98  }
99 
100  s *= ((points[i-1].get_x())*(points[i].get_y())-(points[i].get_x())*(points[i-1].get_y()));
101  mom += s;
102  }
103  mom /= den;
104  return(mom);
105 }
106 
120 void vpMomentObject::cacheValues(std::vector<double>& cache,double x, double y){
121  cache[0]=1;
122 
123  for(register unsigned int i=1;i<order;i++)
124  cache[i]=cache[i-1]*x;
125 
126  for(register unsigned int j=order;j<order*order;j+=order)
127  cache[j]=cache[j-order]*y;
128 
129  for(register unsigned int j=1;j<order;j++){
130  for(register unsigned int i=1;i<order-j;i++){
131  cache[j*order+i] = cache[j*order]*cache[i];
132  }
133  }
134 }
135 
140 void vpMomentObject::cacheValues(std::vector<double>& cache,double x, double y, double IntensityNormalized) {
141 
142  cache[0]=IntensityNormalized;
143 
144  double invIntensityNormalized = 0.;
145  if (std::fabs(IntensityNormalized)>=std::numeric_limits<double>::epsilon())
146  invIntensityNormalized = 1.0/IntensityNormalized;
147 
148  for(register unsigned int i=1;i<order;i++)
149  cache[i]=cache[i-1]*x;
150 
151  for(register unsigned int j=order;j<order*order;j+=order)
152  cache[j]=cache[j-order]*y;
153 
154  for(register unsigned int j=1;j<order;j++){
155  for(register unsigned int i=1;i<order-j;i++){
156  cache[j*order+i] = cache[j*order]*cache[i]*invIntensityNormalized;
157  }
158  }
159 }
160 
161 
234 void vpMomentObject::fromVector(std::vector<vpPoint>& points){
236  if(
237  std::abs(points.rbegin()->get_x()-points.begin()->get_x())>std::numeric_limits<double>::epsilon() ||
238  std::abs(points.rbegin()->get_y()-points.begin()->get_y())>std::numeric_limits<double>::epsilon()
239  ){
240  points.resize(points.size()+1);
241  points[points.size()-1] = points[0];
242  }
243  for(register unsigned int j=0;j<order*order;j++){
244  values[j]=calc_mom_polygon(j%order,j/order,points);
245  }
246  }else{
247  std::vector<double> cache(order*order,0.);
248  values.assign(order*order,0);
249  for(register unsigned int i=0;i<points.size();i++){
250  cacheValues(cache,points[i].get_x(),points[i].get_y());
251  for(register unsigned int j=0;j<order;j++){
252  for(register unsigned int k=0;k<order-j;k++){
253  values[j*order+k]+=cache[j*order+k];
254  }
255  }
256  }
257  }
258 }
259 
289 void vpMomentObject::fromImage(const vpImage<unsigned char>& image, unsigned char threshold, const vpCameraParameters& cam){
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  unsigned int i_, j_;
296 
297  #pragma omp for nowait//automatically organize loop counter between threads
298  for(int i=0;i<(int)image.getCols();i++){
299  for(int j=0;j<(int)image.getRows();j++){
300  i_ = static_cast<unsigned int>(i);
301  j_ = static_cast<unsigned int>(j);
302  if(image[j_][i_]>threshold){
303  double x=0;
304  double y=0;
306 
307  double xval=1.;
308  double yval=1.;
309  for(register unsigned int k=0;k<order;k++){
310  xval=1.;
311  for(register 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 
328  for(register unsigned int k=0;k<order;k++){
329  for(register unsigned int l=0;l<order-k;l++){
330  //#pragma omp atomic // Removed since causes a build issue with msvc14 2015
331  values[k*order+l]+= curvals[k*order+l];
332  }
333  }
334 
335  }
336 #else
337  std::vector<double> cache(order*order,0.);
338  values.assign(order*order,0);
339  for(register unsigned int i=0;i<image.getCols();i++){
340  for(register unsigned int j=0;j<image.getRows();j++){
341  if(image[j][i]>threshold){
342  double x=0;
343  double y=0;
345  cacheValues(cache,x,y);
346  for(register unsigned int k=0;k<order;k++){
347  for(register unsigned int l=0;l<order-k;l++){
348  values[k*order+l]+=cache[k*order+l];
349  }
350  }
351  }
352  }
353  }
354 #endif
355 
356  //Normalisation equivalent to sampling interval/pixel size delX x delY
357  double norm_factor = 1./(cam.get_px()*cam.get_py());
358  for (std::vector<double>::iterator it = values.begin(); it!=values.end(); it++) {
359  *it = (*it) * norm_factor;
360  }
361 }
362 
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  double intensity_white = 0;
388 
389  //double Imax = static_cast<double>(image.getMaxValue());
390  double Imax = 255.; // To check the effect of gray level change. ISR Coimbra
391 
392  double iscale = 1.0;
393  if (flg_normalize_intensity) // This makes the image a probability density function
394  iscale = 1.0/Imax;
395 
396  if (bg_type == vpMomentObject::WHITE) {
398  for(register unsigned int j=0;j<image.getRows();j++){
399  for(register unsigned int i=0;i<image.getCols();i++){
400  x = 0;
401  y = 0;
402  intensity = (double)(image[j][i])*iscale;
403  intensity_white = 1. - intensity;
404 
406  cacheValues(cache,x,y, intensity_white); // Modify 'cache' which has x^p*y^q to x^p*y^q*(1 - I(x,y))
407 
408  // Copy to "values"
409  for(register unsigned int k=0;k<order;k++){
410  kidx = k*order;
411  for(register unsigned int l=0;l<order-k;l++){
412  idx = kidx+l;
413  values[idx]+= cache[idx];
414  }
415  }
416  }
417  }
418  }
419  else {
421  for(register unsigned int j=0;j<image.getRows();j++){
422  for(register unsigned int i=0;i<image.getCols();i++){
423  x = 0;
424  y = 0;
425  intensity = (double)(image[j][i])*iscale;
427 
428  // Cache values for fast moment calculation
429  cacheValues(cache,x,y, 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(register unsigned int k=0;k<order;k++){
433  kidx = k*order;
434  for(register unsigned int l=0;l<order-k;l++){
435  idx = kidx+l;
436  values[idx]+= cache[idx];
437  }
438  }
439 
440  }
441  }
442  }
443 
444  if (normalize_with_pix_size){
445  // Normalisation equivalent to sampling interval/pixel size delX x delY
446  double norm_factor = 1./(cam.get_px()*cam.get_py());
447  for (std::vector<double>::iterator it = values.begin(); it!=values.end(); it++) {
448  *it = (*it) * norm_factor;
449  }
450  }
451 }
452 
457 void
458 vpMomentObject::init(unsigned int orderinp) {
459  order = orderinp + 1;
461  flg_normalize_intensity = true; // By default, the intensity values are normalized
462  values.resize((order+1)*(order+1));
463  values.assign((order+1)*(order+1),0);
464 }
465 
469 void
471  order = objin.getOrder()+1;
472  type = objin.getType();
474  values.resize(objin.values.size());
475  values = objin.values;
476 }
477 
491 vpMomentObject::vpMomentObject(unsigned int max_order)
492  : flg_normalize_intensity(true), order(max_order+1), type(vpMomentObject::DENSE_FULL_OBJECT),
493  values()
494 {
495  init(max_order);
496 }
497 
502  : flg_normalize_intensity(true), order(1), type(vpMomentObject::DENSE_FULL_OBJECT),
503  values()
504 {
505  init(srcobj);
506 }
507 
527 const std::vector<double>& vpMomentObject::get() const {
528  return values;
529 
530 }
531 
538 double vpMomentObject::get(unsigned int i, unsigned int j) const {
539  assert(i+j<=getOrder());
540  if(i+j>=order) throw vpException(vpException::badValue,"The requested value has not been computed, you should specify a higher order.");
541 
542  return values[j*order+i];
543 }
544 
551 void vpMomentObject::set(unsigned int i, unsigned int j, const double& value_ij){
552  assert(i+j<=getOrder());
553  if(i+j>=order) throw vpException(vpException::badValue,"The requested value cannot be set, you should specify a higher order for the moment object.");
554  values[j*order+i] = value_ij;
555 }
556 
572 VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentObject& m){
573  for(unsigned int i = 0;i<m.values.size();i++){
574 
575  if(i%(m.order)==0)
576  os << std::endl;
577 
578  if((i%(m.order)+i/(m.order))<m.order)
579  os << m.values[i] ;
580  else
581  os << "x";
582 
583  os << "\t";
584 
585  }
586 
587  return os;
588 }
589 
594 void
595 vpMomentObject::printWithIndices(const vpMomentObject& momobj, std::ostream& os) {
596  std::vector<double> moment = momobj.get();
597  os << std::endl <<"Order of vpMomentObject: "<<momobj.getOrder()<<std::endl;
598  // Print out values. This is same as printing using operator <<
599  for(unsigned int k=0; k<=momobj.getOrder(); k++) {
600  for(unsigned int l=0; l<(momobj.getOrder()+1)-k; l++){
601  os << "m[" << l << "," << k << "] = " << moment[k*(momobj.getOrder()+1)+ l] << "\t";
602  }
603  os << std::endl;
604  }
605  os <<std::endl;
606 }
607 
634 vpMatrix
636  std::vector<double> moment = momobj.get();
637  unsigned int order = momobj.getOrder();
638  vpMatrix M(order+1, order+1);
639  for(unsigned int k=0; k<=order; k++) {
640  for(unsigned int l=0; l<(order+1)-k; l++){
641  M[l][k] = moment[k*(order+1)+ l];
642  }
643  }
644  return M;
645 }
646 
655 // deliberate empty
656 }
unsigned int getCols() const
Definition: vpImage.h:180
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
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:73
Class for generic objects.
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Point coordinates conversion from pixel coordinates to normalized coordinates in meter...
const std::vector< double > & get() const
double get_py() const
virtual ~vpMomentObject()
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:171
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
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:267
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:233
unsigned int getOrder() const