vpTutoParabolaModel.h

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2024 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 https://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.
 */
 
#ifndef VP_PARABOLA_MODEL_H
#define VP_PARABOLA_MODEL_H
 
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpMatrix.h>
 
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
#ifndef DOXYGEN_SHOULD_SKIP_THIS
namespace tutorial
{
class vpTutoParabolaModel
{
public:
  inline vpTutoParabolaModel(const unsigned int &degree, const unsigned int &height, const unsigned int &width)
    : m_degree(degree)
    , m_height(static_cast<unsigned int>(height))
    , m_width(static_cast<unsigned int>(width))
    , m_coeffs(degree + 1, 0.)
  { }
 
  inline vpTutoParabolaModel(const VISP_NAMESPACE_ADDRESSING vpColVector &coeffs, const unsigned int &height, const unsigned int &width)
    : m_degree(coeffs.size() - 1)
    , m_height(static_cast<unsigned int>(height))
    , m_width(static_cast<unsigned int>(width))
    , m_coeffs(coeffs)
  { }
 
  inline vpTutoParabolaModel(const VISP_NAMESPACE_ADDRESSING vpMatrix &coeffs, const unsigned int &height, const unsigned int &width)
    : m_degree(coeffs.getRows() - 1)
    , m_height(static_cast<unsigned int>(height))
    , m_width(static_cast<unsigned int>(width))
    , m_coeffs(coeffs.getCol(0))
  { }
 
  inline double eval(const double &u) const
  {
    double normalizedU = u / m_width;
    double v = 0.;
    for (unsigned int i = 0; i <= m_degree; ++i) {
      v += m_coeffs[i] * std::pow(normalizedU, i);
    }
    v *= m_height;
    return v;
  }
 
  inline VISP_NAMESPACE_ADDRESSING vpColVector toVpColVector() const
  {
    return m_coeffs;
  }
 
  inline vpTutoParabolaModel &operator=(const vpTutoParabolaModel &other)
  {
    m_degree = other.m_degree;
    m_height = other.m_height;
    m_width = other.m_width;
    m_coeffs = other.m_coeffs;
    return *this;
  }
 
  static void fillSystem(const unsigned int &degree, const double &height, const double &width, const std::vector<VISP_NAMESPACE_ADDRESSING vpImagePoint> &pts, VISP_NAMESPACE_ADDRESSING vpMatrix &A, VISP_NAMESPACE_ADDRESSING vpMatrix &b)
  {
    const unsigned int nbPts = static_cast<unsigned int>(pts.size());
    const unsigned int nbCoeffs = degree + 1;
    std::vector<VISP_NAMESPACE_ADDRESSING vpImagePoint> normalizedPts;
    // Normalization to avoid numerical instability
    for (const auto &pt: pts) {
      normalizedPts.push_back(VISP_NAMESPACE_ADDRESSING vpImagePoint(pt.get_i() / height, pt.get_j() / width));
    }
    A.resize(nbPts, nbCoeffs, 1.); // Contains the u^i
    b.resize(nbPts, 1); // Contains the v coordinates
    for (unsigned int i = 0; i < nbPts; ++i) {
      double u = normalizedPts[i].get_u();
      double v = normalizedPts[i].get_v();
      for (unsigned int j = 0; j < nbCoeffs; ++j) {
        A[i][j] = std::pow(u, j);
      }
      b[i][0] = v;
    }
  }
 
  friend std::ostream &operator<<(std::ostream &os, const vpTutoParabolaModel &model)
  {
    os << "Highest degree = " << model.m_degree << std::endl;
    os << "Coeffs = [ " << model.m_coeffs.transpose() << " ]" << std::endl;
    return os;
  }
 
private:
  unsigned int m_degree; 
  unsigned int m_height; 
  unsigned int m_width; 
  VISP_NAMESPACE_ADDRESSING vpColVector m_coeffs; 
};
}
#endif
#endif
#endif