vpMomentObject.cpp

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Object input structure used by moments.
 *
 * Authors:
 * Filip Novotny
 *
 *****************************************************************************/

#include <visp3/core/vpMomentBasic.h>
#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/core/vpConfig.h>
#include <stdexcept>

#include <cmath>
#include <limits>

#ifdef VISP_HAVE_OPENMP
#include <omp.h>
#endif
#include <cassert>

double vpMomentObject::calc_mom_polygon(unsigned int p, unsigned int q, const std::vector<vpPoint>& points)
{
  unsigned int i,k,l;
  double den,mom;
  double x_p_k;
  double y_l;
  double y_q_l;

  den = static_cast<double>( (p+q+2)*(p+q+1)*vpMath::comb(p+q,p) );

  mom = 0.0;
  for (i=1;i<=points.size()-1;i++)
  {
    double s = 0.0;
    double x_k=1;
    for (k=0;k<=p;k++)
    {
      y_l=1;
      x_p_k = pow(points[i-1].get_x(), (int)(p-k));
      for (l=0;l<=q;l++)
      {
        y_q_l=pow(points[i-1].get_y(), (int)(q-l));

        s += static_cast<double>(
              vpMath::comb(k+l,l)
              *vpMath::comb(p+q-k-l,q-l)
              *x_k
              *x_p_k
              *y_l
              *y_q_l );

        y_l*=points[i].get_y();

      }
      x_k*=points[i].get_x();

    }

    s *= ((points[i-1].get_x())*(points[i].get_y())-(points[i].get_x())*(points[i-1].get_y()));
    mom += s;
  }
  mom /= den;
  return(mom);
}

void vpMomentObject::cacheValues(std::vector<double>& cache,double x, double y){
    cache[0]=1;

    for(unsigned int i=1;i<order;i++)
        cache[i]=cache[i-1]*x;

    for(unsigned int j=order;j<order*order;j+=order)
        cache[j]=cache[j-order]*y;

    for(unsigned int j=1;j<order;j++){
        for(unsigned int i=1;i<order-j;i++){
            cache[j*order+i] = cache[j*order]*cache[i];
        }
    }
}

void vpMomentObject::cacheValues(std::vector<double>& cache,double x, double y, double IntensityNormalized) {

    cache[0]=IntensityNormalized;

    double invIntensityNormalized = 0.;
    if (std::fabs(IntensityNormalized)>=std::numeric_limits<double>::epsilon())
         invIntensityNormalized = 1.0/IntensityNormalized;

    for(unsigned int i=1;i<order;i++)
        cache[i]=cache[i-1]*x;

    for(unsigned int j=order;j<order*order;j+=order)
        cache[j]=cache[j-order]*y;

    for(unsigned int j=1;j<order;j++){
        for(unsigned int i=1;i<order-j;i++){
            cache[j*order+i] = cache[j*order]*cache[i]*invIntensityNormalized;
        }
    }
}


void vpMomentObject::fromVector(std::vector<vpPoint>& points){
  if(type==vpMomentObject::DENSE_POLYGON){
    if(std::fabs(points.rbegin()->get_x()-points.begin()->get_x())>std::numeric_limits<double>::epsilon() ||
       std::fabs(points.rbegin()->get_y()-points.begin()->get_y())>std::numeric_limits<double>::epsilon()){
      points.resize(points.size()+1);
      points[points.size()-1] = points[0];
    }
    for(unsigned int j=0;j<order*order;j++){
      values[j]=calc_mom_polygon(j%order,j/order,points);
    }
  } else {
    std::vector<double> cache(order*order,0.);
    values.assign(order*order,0);
    for(unsigned int i=0;i<points.size();i++){
      cacheValues(cache,points[i].get_x(),points[i].get_y());
      for(unsigned int j=0;j<order;j++){
        for(unsigned int k=0;k<order-j;k++){
          values[j*order+k]+=cache[j*order+k];
        }
      }
    }
  }
}

void vpMomentObject::fromImage(const vpImage<unsigned char>& image, unsigned char threshold, const vpCameraParameters& cam){
#ifdef VISP_HAVE_OPENMP
  #pragma omp parallel shared(threshold)
  {
    std::vector<double> curvals(order*order);
    curvals.assign(order*order,0.);

    #pragma omp for nowait//automatically organize loop counter between threads
    for(int i=0;i<(int)image.getCols();i++){
      for(int j=0;j<(int)image.getRows();j++){
        unsigned int i_ = static_cast<unsigned int>(i);
        unsigned int j_ = static_cast<unsigned int>(j);
        if(image[j_][i_]>threshold){
          double x=0;
          double y=0;
          vpPixelMeterConversion::convertPoint(cam,i_,j_,x,y);

          double yval=1.;
          for(unsigned int k=0;k<order;k++){
            double xval=1.;
            for(unsigned int l=0;l<order-k;l++){
              curvals[(k*order+l)]+=(xval*yval);
              xval*=x;
            }
            yval*=y;
          }
        }
      }
    }

    #pragma omp master //only set this variable in master thread
    {
      values.assign(order*order, 0.);
    }

    #pragma omp barrier
    for(unsigned int k=0;k<order;k++){
      for(unsigned int l=0;l<order-k;l++){
        #pragma omp atomic
        values[k*order+l]+= curvals[k*order+l];
      }
    }

  }
#else
    std::vector<double> cache(order*order,0.);
    values.assign(order*order,0);
    for(unsigned int i=0;i<image.getCols();i++){
        for(unsigned int j=0;j<image.getRows();j++){
            if(image[j][i]>threshold){
                double x=0;
                double y=0;
                vpPixelMeterConversion::convertPoint(cam,i,j,x,y);
                cacheValues(cache,x,y);
                for(unsigned int k=0;k<order;k++){
                    for(unsigned int l=0;l<order-k;l++){
                        values[k*order+l]+=cache[k*order+l];
                    }
                }
            }
        }
    }
#endif

    //Normalisation equivalent to sampling interval/pixel size delX x delY
    double norm_factor = 1./(cam.get_px()*cam.get_py());
    for (std::vector<double>::iterator it = values.begin(); it!=values.end(); ++it) {
        *it = (*it) * norm_factor;
    }
}

void vpMomentObject::fromImage(const vpImage<unsigned char>& image, const vpCameraParameters& cam,
    vpCameraImgBckGrndType bg_type, bool normalize_with_pix_size)
{
  std::vector<double> cache(order*order,0.);
  values.assign(order*order,0);

  // (x,y) - Pixel co-ordinates in metres
  double x=0;
  double y=0;
  //for indexing into cache[] and values[]
  unsigned int idx = 0;
  unsigned int kidx = 0;

  double intensity = 0;

  //double Imax = static_cast<double>(image.getMaxValue());

  double iscale = 1.0;
  if (flg_normalize_intensity) {                                            // This makes the image a probability density function
    double Imax = 255.;                                                     // To check the effect of gray level change. ISR Coimbra
    iscale = 1.0/Imax;
  }

  if (bg_type == vpMomentObject::WHITE) {
      for(unsigned int j=0;j<image.getRows();j++){
          for(unsigned int i=0;i<image.getCols();i++){
              x = 0;
              y = 0;
              intensity = (double)(image[j][i])*iscale;
              double intensity_white = 1. - intensity;

              vpPixelMeterConversion::convertPoint(cam,i,j,x,y);
              cacheValues(cache,x,y, intensity_white);          // Modify 'cache' which has x^p*y^q to x^p*y^q*(1 - I(x,y))

              // Copy to "values"
              for(unsigned int k=0;k<order;k++){
                  kidx = k*order;
                  for(unsigned int l=0;l<order-k;l++){
                      idx = kidx+l;
                      values[idx]+= cache[idx];
                  }
              }
          }
      }
  }
  else {
      for(unsigned int j=0;j<image.getRows();j++){
          for(unsigned int i=0;i<image.getCols();i++){
              x = 0;
              y = 0;
              intensity = (double)(image[j][i])*iscale;
              vpPixelMeterConversion::convertPoint(cam,i,j,x,y);

              // Cache values for fast moment calculation
              cacheValues(cache,x,y, intensity);                    // Modify 'cache' which has x^p*y^q to x^p*y^q*I(x,y)

              // Copy to moments array 'values'
              for(unsigned int k=0;k<order;k++){
                  kidx = k*order;
                  for(unsigned int l=0;l<order-k;l++){
                      idx = kidx+l;
                      values[idx]+= cache[idx];
                  }
              }

          }
      }
  }

  if (normalize_with_pix_size){
      // Normalisation equivalent to sampling interval/pixel size delX x delY
      double norm_factor = 1./(cam.get_px()*cam.get_py());
      for (std::vector<double>::iterator it = values.begin(); it!=values.end(); ++it) {
          *it = (*it) * norm_factor;
      }
  }
}

void
vpMomentObject::init(unsigned int orderinp) {
    order = orderinp + 1;
    type = vpMomentObject::DENSE_FULL_OBJECT;
    flg_normalize_intensity = true;                 // By default, the intensity values are normalized
    values.resize((order+1)*(order+1));
    values.assign((order+1)*(order+1),0);
}

void
vpMomentObject::init(const vpMomentObject& objin){
    order = objin.getOrder()+1;
    type = objin.getType();
    flg_normalize_intensity = objin.flg_normalize_intensity;
    values.resize(objin.values.size());
    values = objin.values;
}

vpMomentObject::vpMomentObject(unsigned int max_order)
  : flg_normalize_intensity(true), order(max_order+1), type(vpMomentObject::DENSE_FULL_OBJECT),
    values()
{
    init(max_order);
}

vpMomentObject::vpMomentObject(const vpMomentObject& srcobj)
  : flg_normalize_intensity(true), order(1), type(vpMomentObject::DENSE_FULL_OBJECT),
    values()
{
    init(srcobj);
}

const std::vector<double>& vpMomentObject::get() const {
    return values;

}

double vpMomentObject::get(unsigned int i, unsigned int j) const {
    assert(i+j<=getOrder());
    if(i+j>=order) throw vpException(vpException::badValue,"The requested value has not been computed, you should specify a higher order.");

    return values[j*order+i];
}

void vpMomentObject::set(unsigned int i, unsigned int j, const double& value_ij){
    assert(i+j<=getOrder());
    if(i+j>=order) throw vpException(vpException::badValue,"The requested value cannot be set, you should specify a higher order for the moment object.");
    values[j*order+i] = value_ij;
}

VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentObject& m){
    for(unsigned int i = 0;i<m.values.size();i++){

        if(i%(m.order)==0)
          os << std::endl;

        if((i%(m.order)+i/(m.order))<m.order)
            os << m.values[i] ;
        else
            os << "x";

        os  << "\t";

    }

    return os;
}

void
vpMomentObject::printWithIndices(const vpMomentObject& momobj, std::ostream& os) {
    std::vector<double> moment = momobj.get();
    os << std::endl <<"Order of vpMomentObject: "<<momobj.getOrder()<<std::endl;
    // Print out values. This is same as printing using operator <<
    for(unsigned int k=0; k<=momobj.getOrder(); k++) {
            for(unsigned int l=0; l<(momobj.getOrder()+1)-k; l++){
                    os << "m[" << l << "," << k << "] = " << moment[k*(momobj.getOrder()+1)+ l] << "\t";
            }
            os << std::endl;
    }
    os <<std::endl;
}

vpMatrix
vpMomentObject::convertTovpMatrix(const vpMomentObject& momobj) {
    std::vector<double> moment = momobj.get();
    unsigned int order = momobj.getOrder();
    vpMatrix M(order+1, order+1);
    for(unsigned int k=0; k<=order; k++) {
        for(unsigned int l=0; l<(order+1)-k; l++){
            M[l][k] = moment[k*(order+1)+ l];
        }
    }
    return M;
}

vpMomentObject::~vpMomentObject(){
// deliberate empty
}