vpPoseFeatures.h

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2025 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.
 *
 * Description:
 * Pose computation from any features.
 */

 
#ifndef vpPoseFeatures_HH
#define vpPoseFeatures_HH
 
#include <visp3/core/vpConfig.h>
 
#if defined(VISP_HAVE_MODULE_VISUAL_FEATURES) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
 
#include <visp3/core/vpCircle.h>
#include <visp3/core/vpCylinder.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpExponentialMap.h>
#include <visp3/core/vpForwardProjection.h>
#include <visp3/core/vpLine.h>
#include <visp3/core/vpPoint.h>
#include <visp3/core/vpRobust.h>
#include <visp3/core/vpSphere.h>
#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureEllipse.h>
#include <visp3/visual_features/vpFeaturePoint.h>
 
#include <iostream>
#include <vector>
#include <tuple>
 
BEGIN_VISP_NAMESPACE
 
#ifndef DOXYGEN_SHOULD_SKIP_THIS
//#################################################
//##  Call a function with a tuple as parameters
//#################################################
template <unsigned int N> struct vpDesiredFeatureBuilderWithTuple
{
  template <typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args>
  static void buildDesiredFeatureWithTuple(featureType &feature, RetType(*f)(ArgsF...), const std::tuple<ArgsT...> &t,
                                           Args &&...args)
  {
    vpDesiredFeatureBuilderWithTuple<N - 1>::buildDesiredFeatureWithTuple(feature, f, t, std::get<N - 1>(t), args...);
  }
};
 
template <> struct vpDesiredFeatureBuilderWithTuple<0>
{
  template <typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args>
  static void buildDesiredFeatureWithTuple(featureType & /* feature */, RetType(*f)(ArgsF...),
                                           const std::tuple<ArgsT...> & /* t */, Args &&...args)
  {
    f(args...);
  }
};
 
template <> struct vpDesiredFeatureBuilderWithTuple<1>
{
  template <typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args>
  static void buildDesiredFeatureWithTuple(featureType &feature, RetType(*f)(ArgsF...), const std::tuple<ArgsT...> &t,
                                           Args &&...args)
  {
    vpDesiredFeatureBuilderWithTuple<0>::buildDesiredFeatureWithTuple(feature, f, t, feature, args...);
  }
};
 
template <typename featureType, typename RetType, typename... Args, typename... ArgsFunc>
void buildDesiredFeatureWithTuple(featureType &feature, RetType(*f)(ArgsFunc...), std::tuple<Args...> const &t)
{
  vpDesiredFeatureBuilderWithTuple<sizeof...(Args)>::buildDesiredFeatureWithTuple(feature, f, t);
}
 
//#################################################
//##  Call a function with a tuple as parameters
//##  Object Mode
//#################################################
 
template <unsigned int N> struct vpDesiredFeatureBuilderObjectWithTuple
{
  template <typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT,
    typename... Args>
  static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType(objType:: *f)(ArgsF...),
                                                 const std::tuple<ArgsT...> &t, Args &&...args)
  {
    vpDesiredFeatureBuilderObjectWithTuple<N - 1>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t,
                                                                                      std::get<N - 1>(t), args...);
  }
};
 
template <> struct vpDesiredFeatureBuilderObjectWithTuple<0>
{
  template <typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT,
    typename... Args>
  static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType & /*feature*/,
                                                 RetType(objType:: *f)(ArgsF...), const std::tuple<ArgsT...> & /* t */,
                                                 Args &&...args)
  {
    (obj->*f)(args...);
  }
};
 
template <> struct vpDesiredFeatureBuilderObjectWithTuple<1>
{
  template <typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT,
    typename... Args>
  static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType(objType:: *f)(ArgsF...),
                                                 const std::tuple<ArgsT...> &t, Args &&...args)
  {
    vpDesiredFeatureBuilderObjectWithTuple<0>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t, feature, args...);
  }
};
 
template <typename objType, typename featureType, typename RetType, typename... Args, typename... ArgsFunc>
void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType(objType:: *f)(ArgsFunc...),
                                        std::tuple<Args...> const &t)
{
  vpDesiredFeatureBuilderObjectWithTuple<sizeof...(Args)>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t);
}
 
//#####################################################
//##  Call un function with a tuple as parameters
//##  Track all the parameters with the cMo
//##  Except the first one (must be de "BasicFeature"
//#####################################################
 
template <unsigned int N> struct vpCurrentFeatureBuilderWithTuple
{
  template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
    typename... ArgsF>
  static void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType(*f)(ArgsF...),
                                           std::tuple<ArgsTuple...> &t, ArgsDecomposed &&...args)
  {
    auto proj = std::get<N - 1>(t);
    proj.track(cMo);
    vpCurrentFeatureBuilderWithTuple<N - 1>::buildCurrentFeatureWithTuple(feature, cMo, f, t, proj, args...);
  }
};
 
template <> struct vpCurrentFeatureBuilderWithTuple<0>
{
  template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
    typename... ArgsF>
  static void buildCurrentFeatureWithTuple(featureType & /*feature*/, const vpHomogeneousMatrix & /*cMo*/,
                                           RetType(*f)(ArgsF...), std::tuple<ArgsTuple...> &, ArgsDecomposed &&...args)
  {
    f(args...);
  }
};
 
template <> struct vpCurrentFeatureBuilderWithTuple<1>
{
  template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
    typename... ArgsF>
  static void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType(*f)(ArgsF...),
                                           std::tuple<ArgsTuple...> &t, ArgsDecomposed &&...args)
  {
    vpCurrentFeatureBuilderWithTuple<0>::buildCurrentFeatureWithTuple(feature, cMo, f, t, feature, args...);
  }
};
 
template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc>
void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType(*f)(ArgsFunc...),
                                  std::tuple<ArgsTuple...> &t)
{
  vpCurrentFeatureBuilderWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureWithTuple(feature, cMo, f, t);
}
 
//#####################################################
//##  Call un function with a tuple as parameters
//##  Track all the parameters with the cMo
//##  Except the first one (must be de "BasicFeature"
//##  Object Mode
//#####################################################
 
template <unsigned int N> struct vpCurrentFeatureBuilderObjectWithTuple
{
  template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
    typename... ArgsF>
  static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo,
                                                 RetType(objType:: *f)(ArgsF...), std::tuple<ArgsTuple...> &t,
                                                 ArgsDecomposed &&...args)
  {
    auto proj = std::get<N - 1>(t);
    proj.track(cMo);
    vpCurrentFeatureBuilderObjectWithTuple<N - 1>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, proj,
                                                                                      args...);
  }
};
 
template <> struct vpCurrentFeatureBuilderObjectWithTuple<0>
{
  template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
    typename... ArgsF>
  static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType & /*feature*/,
                                                 const vpHomogeneousMatrix & /*cMo*/, RetType(objType:: *f)(ArgsF...),
                                                 std::tuple<ArgsTuple...> &, ArgsDecomposed &&...args)
  {
    (obj->*f)(args...);
  }
};
 
template <> struct vpCurrentFeatureBuilderObjectWithTuple<1>
{
  template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
    typename... ArgsF>
  static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo,
                                                 RetType(objType:: *f)(ArgsF...), std::tuple<ArgsTuple...> &t,
                                                 ArgsDecomposed &&...args)
  {
    vpCurrentFeatureBuilderObjectWithTuple<0>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, feature,
                                                                                  args...);
  }
};
 
template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc>
void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo,
                                        RetType(objType:: *f)(ArgsFunc...), std::tuple<ArgsTuple...> &t)
{
  vpCurrentFeatureBuilderObjectWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f,
                                                                                                   t);
}
 
//#################################################
//##  Call that will be used in our vpPoseFeatures
//##  to store the specific features.
//#################################################
class VISP_EXPORT vpPoseSpecificFeature
{
public:
  vpPoseSpecificFeature() { }
  virtual ~vpPoseSpecificFeature() { }
 
  virtual vpColVector error() = 0;
  virtual vpMatrix currentInteraction() = 0;
  virtual void createDesired() = 0;
  virtual void createCurrent(const vpHomogeneousMatrix &cMo) = 0;
};
 
//#################################################
//##  Template for all kind of specific features
//#################################################

template <typename featureType, typename RetType, typename... Args>
class vpPoseSpecificFeatureTemplate : public vpPoseSpecificFeature
{
private:
  featureType m_desiredFeature;
  featureType m_currentFeature;
  std::tuple<Args...> *m_tuple;
  RetType(*func_ptr)(Args...);
 
public:
  vpPoseSpecificFeatureTemplate(RetType(*f_ptr)(Args...), Args &&...args)
  {
    func_ptr = f_ptr; // std::move(f_ptr);
    m_tuple = new std::tuple<Args...>(args...);
  }
  virtual ~vpPoseSpecificFeatureTemplate() VP_OVERRIDE
  {
    delete m_tuple;
  }
 
  virtual void createDesired() VP_OVERRIDE
  {
    buildDesiredFeatureWithTuple(m_desiredFeature, func_ptr, *m_tuple);
  }
 
  virtual vpColVector error() VP_OVERRIDE
  {
    // std::cout << "Getting S... : " << std::get<0>(*tuple).get_s() <<
    // std::endl;
    return m_currentFeature.error(m_desiredFeature);
  }
 
  virtual vpMatrix currentInteraction() VP_OVERRIDE
  {
    return m_currentFeature.interaction();
  }
 
  virtual void createCurrent(const vpHomogeneousMatrix &cMo) VP_OVERRIDE
  {
    buildCurrentFeatureWithTuple(m_currentFeature, cMo, func_ptr, *m_tuple);
  }
};
 
//#################################################
//##  Template for all kind of specific features
//##  Object Mode
//#################################################

template <typename ObjectType, typename featureType, typename RetType, typename... Args>
class vpPoseSpecificFeatureTemplateObject : public vpPoseSpecificFeature
{
private:
  featureType m_desiredFeature;
  featureType m_currentFeature;
  std::tuple<Args...> *m_tuple;
  RetType(ObjectType:: *func_ptr)(Args...);
  ObjectType *m_obj;
 
public:
  vpPoseSpecificFeatureTemplateObject(ObjectType *o, RetType(ObjectType:: *f_ptr)(Args...), Args &&...args)
  {
    func_ptr = f_ptr; // std::move(f_ptr);
    m_tuple = new std::tuple<Args...>(args...);
    m_obj = o;
  }
 
  virtual ~vpPoseSpecificFeatureTemplateObject() VP_OVERRIDE
  {
    delete m_tuple;
  }
 
  virtual void createDesired() VP_OVERRIDE
  {
    buildDesiredFeatureObjectWithTuple(m_obj, m_desiredFeature, func_ptr, *m_tuple);
  }
 
  virtual vpColVector error() VP_OVERRIDE
  {
    return m_currentFeature.error(m_desiredFeature);
  }
 
  virtual vpMatrix currentInteraction() VP_OVERRIDE
  {
    return m_currentFeature.interaction();
  }
 
  virtual void createCurrent(const vpHomogeneousMatrix &cMo) VP_OVERRIDE
  {
    buildCurrentFeatureObjectWithTuple(m_obj, m_currentFeature, cMo, func_ptr, *m_tuple);
  }
};
#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS

class VISP_EXPORT vpPoseFeatures
{
public:
  typedef enum
  {
    VIRTUAL_VS,       
    ROBUST_VIRTUAL_VS 
  } vpPoseFeaturesMethodType;

  vpPoseFeatures();

  virtual ~vpPoseFeatures();
 
#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
  vpPoseFeatures(const vpPoseFeatures &) = default;
  vpPoseFeatures &operator=(const vpPoseFeatures &) = default;
#endif

  void addFeaturePoint(const vpPoint &p);

  void addFeaturePoint3D(const vpPoint &p);

  void addFeatureVanishingPoint(const vpPoint &p);

  void addFeatureVanishingPoint(const vpLine &l1, const vpLine &l2);

  void addFeatureEllipse(const vpCircle &c);

  void addFeatureEllipse(const vpSphere &s);

  void addFeatureLine(const vpLine &l);

  void addFeatureLine(const vpCylinder &c, const int &line);

  void addFeatureSegment(vpPoint &P1, vpPoint &P2);

  template <typename RetType, typename... ArgsFunc, typename... Args>
  void addSpecificFeature(RetType(*fct_ptr)(ArgsFunc...), Args &&...args);

  template <typename ObjType, typename RetType, typename... ArgsFunc, typename... Args>
  void addSpecificFeature(ObjType *obj, RetType(ObjType:: *fct_ptr)(ArgsFunc...), Args &&...args);

  void clear();

  void computePose(vpHomogeneousMatrix &cMo, const vpPoseFeaturesMethodType &type = VIRTUAL_VS);

  vpMatrix getCovarianceMatrix() const
  {
    if (!m_computeCovariance)
      vpTRACE("Warning : The covariance matrix has not been computed. See "
              "setCovarianceComputation() to do it.");
 
    return m_covarianceMatrix;
  }

  double getLambda() { return m_lambda; }

  unsigned int getVVSIterMax() { return m_vvsIterMax; }

  void setCovarianceComputation(const bool &flag) { m_computeCovariance = flag; }

  void setLambda(const double &val) { m_lambda = val; }

  void setVVSIterMax(const unsigned int &val) { m_vvsIterMax = val; }

  void setVerbose(const bool &mode) { m_verbose = mode; }
 
private:
#ifndef DOXYGEN_SHOULD_SKIP_THIS
  template <typename FeatureType, typename FirstParamType> struct vpDuo
  {
    FeatureType *desiredFeature;
    FirstParamType firstParam;
    vpDuo() : desiredFeature(nullptr), firstParam() { }
  };
 
  template <typename FeatureType, typename FirstParamType, typename SecondParamType> struct vpTrio
  {
    FeatureType *desiredFeature;
    FirstParamType firstParam;
    SecondParamType secondParam;
 
    vpTrio() : desiredFeature(nullptr), firstParam(), secondParam() { }
  };
#endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
 
  unsigned int m_maxSize;
  unsigned int m_totalSize;
  unsigned int m_vvsIterMax;
  double m_lambda;
 
  bool m_verbose;
 
  bool m_computeCovariance;
  vpMatrix m_covarianceMatrix;
 
  // vpFeaturePoint
  std::vector<vpDuo<vpFeaturePoint, vpPoint> > m_featurePoint_Point_list;
  // vpFeaturePoint3D
  std::vector<vpDuo<vpFeaturePoint3D, vpPoint> > m_featurePoint3D_Point_list;
  // vpFeatureVanishingPoint
  std::vector<vpDuo<vpFeatureVanishingPoint, vpPoint> > m_featureVanishingPoint_Point_list;
  std::vector<vpTrio<vpFeatureVanishingPoint, vpLine, vpLine> > m_featureVanishingPoint_DuoLine_list;
  // vpFeatureEllipse
  std::vector<vpDuo<vpFeatureEllipse, vpSphere> > m_featureEllipse_Sphere_list;
  std::vector<vpDuo<vpFeatureEllipse, vpCircle> > m_featureEllipse_Circle_list;
  // vpFeatureLine
  std::vector<vpDuo<vpFeatureLine, vpLine> > m_featureLine_Line_list;
  std::vector<vpTrio<vpFeatureLine, vpCylinder, int> > m_featureLine_DuoLineInt_List;
  // vpFeatureSegment
  std::vector<vpTrio<vpFeatureSegment, vpPoint, vpPoint> > m_featureSegment_DuoPoints_list;
 
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
  // Specific features
  std::vector<vpPoseSpecificFeature *> m_featureSpecific_list;
#endif

  void error_and_interaction(vpHomogeneousMatrix &cMo, vpColVector &err, vpMatrix &L);

  void computePoseVVS(vpHomogeneousMatrix &cMo);

  void computePoseRobustVVS(vpHomogeneousMatrix &cMo);
};

template <typename RetType, typename... ArgsFunc, typename... Args>
void vpPoseFeatures::addSpecificFeature(RetType(*fct_ptr)(ArgsFunc...), Args &&...args)
{
  typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
  typedef typename std::remove_reference<featureTypeReference>::type featureType;
  m_featureSpecific_list.push_back(
      new vpPoseSpecificFeatureTemplate<featureType, RetType, ArgsFunc...>(fct_ptr, std::forward<ArgsFunc>(args)...));
 
  m_featureSpecific_list.back()->createDesired();
 
  m_totalSize++;
  if (m_featureSpecific_list.size() > m_maxSize)
    m_maxSize = static_cast<unsigned int>(m_featureSpecific_list.size());
}

template <typename ObjType, typename RetType, typename... ArgsFunc, typename... Args>
void vpPoseFeatures::addSpecificFeature(ObjType *obj, RetType(ObjType:: *fct_ptr)(ArgsFunc...), Args &&...args)
{
  typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
  typedef typename std::remove_reference<featureTypeReference>::type featureType;
  m_featureSpecific_list.push_back(new vpPoseSpecificFeatureTemplateObject<ObjType, featureType, RetType, ArgsFunc...>(
    obj, fct_ptr, std::forward<ArgsFunc>(args)...));
 
  m_featureSpecific_list.back()->createDesired();
 
  m_totalSize++;
  if (m_featureSpecific_list.size() > m_maxSize)
    m_maxSize = static_cast<unsigned int>(m_featureSpecific_list.size());
}
 
END_VISP_NAMESPACE
#endif
 
#endif