vpMomentObject.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 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 <stdexcept>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMomentBasic.h>
#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpPixelMeterConversion.h>

#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
}