vpPoseFeatures.h

/****************************************************************************
 *
 * 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:
 * Pose computation from any features.
 *
 * Authors:
 * Aurelien Yol
 *
 *****************************************************************************/

#ifndef vpPoseFeatures_HH
#define vpPoseFeatures_HH

#include <visp3/core/vpConfig.h>

#ifdef VISP_HAVE_MODULE_VISUAL_FEATURES

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

#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
#include <tuple>

#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 desiredFeature;
  featureType currentFeature;
  std::tuple<Args...> *tuple;
  RetType (*func_ptr)(Args...);

public:
  vpPoseSpecificFeatureTemplate(RetType (*f_ptr)(Args...), Args &&...args)
  {
    func_ptr = f_ptr; // std::move(f_ptr);
    tuple = new std::tuple<Args...>(args...);
  }

  virtual ~vpPoseSpecificFeatureTemplate() { delete tuple; }

  virtual void createDesired() { buildDesiredFeatureWithTuple(desiredFeature, func_ptr, *tuple); }

  virtual vpColVector error()
  {
    // std::cout << "Getting S... : " << std::get<0>(*tuple).get_s() <<
    // std::endl;
    return currentFeature.error(desiredFeature);
  }

  virtual vpMatrix currentInteraction() { return currentFeature.interaction(); }

  virtual void createCurrent(const vpHomogeneousMatrix &cMo)
  {
    buildCurrentFeatureWithTuple(currentFeature, cMo, func_ptr, *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 desiredFeature;
  featureType currentFeature;
  std::tuple<Args...> *tuple;
  RetType (ObjectType::*func_ptr)(Args...);
  ObjectType *obj;

public:
  vpPoseSpecificFeatureTemplateObject(ObjectType *o, RetType (ObjectType::*f_ptr)(Args...), Args &&...args)
  {
    func_ptr = f_ptr; // std::move(f_ptr);
    tuple = new std::tuple<Args...>(args...);
    obj = o;
  }

  virtual ~vpPoseSpecificFeatureTemplateObject() { delete tuple; }

  virtual void createDesired() { buildDesiredFeatureObjectWithTuple(obj, desiredFeature, func_ptr, *tuple); }

  virtual vpColVector error() { return currentFeature.error(desiredFeature); }

  virtual vpMatrix currentInteraction() { return currentFeature.interaction(); }

  virtual void createCurrent(const vpHomogeneousMatrix &cMo)
  {
    buildCurrentFeatureObjectWithTuple(obj, currentFeature, cMo, func_ptr, *tuple);
  }
};
#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
#endif // (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)

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

private:
#ifndef DOXYGEN_SHOULD_SKIP_THIS
  template <typename FeatureType, typename FirstParamType> struct vpDuo {
    FeatureType *desiredFeature;
    FirstParamType firstParam;
    vpDuo() : desiredFeature(NULL), firstParam() {}
  };

  template <typename FeatureType, typename FirstParamType, typename SecondParamType> struct vpTrio {
    FeatureType *desiredFeature;
    FirstParamType firstParam;
    SecondParamType secondParam;

    vpTrio() : desiredFeature(NULL), firstParam(), secondParam() {}
  };
#endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS

  unsigned int maxSize;
  unsigned int totalSize;
  unsigned int vvsIterMax;
  double lambda;

  bool verbose;

  bool computeCovariance;
  vpMatrix covarianceMatrix;

  // vpFeaturePoint
  std::vector<vpDuo<vpFeaturePoint, vpPoint> > featurePoint_Point_list;
  // vpFeaturePoint3D
  std::vector<vpDuo<vpFeaturePoint3D, vpPoint> > featurePoint3D_Point_list;
  // vpFeatureVanishingPoint
  std::vector<vpDuo<vpFeatureVanishingPoint, vpPoint> > featureVanishingPoint_Point_list;
  std::vector<vpTrio<vpFeatureVanishingPoint, vpLine, vpLine> > featureVanishingPoint_DuoLine_list;
  // vpFeatureEllipse
  std::vector<vpDuo<vpFeatureEllipse, vpSphere> > featureEllipse_Sphere_list;
  std::vector<vpDuo<vpFeatureEllipse, vpCircle> > featureEllipse_Circle_list;
  // vpFeatureLine
  std::vector<vpDuo<vpFeatureLine, vpLine> > featureLine_Line_list;
  std::vector<vpTrio<vpFeatureLine, vpCylinder, int> > featureLine_DuoLineInt_List;
  // vpFeatureSegment
  std::vector<vpTrio<vpFeatureSegment, vpPoint, vpPoint> > featureSegment_DuoPoints_list;

#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
  // Specific features
  std::vector<vpPoseSpecificFeature *> featureSpecific_list;
#endif

public:
  vpPoseFeatures();
  virtual ~vpPoseFeatures();

  // ! Features addition
  void addFeaturePoint(const vpPoint &);

  void addFeaturePoint3D(const vpPoint &);

  void addFeatureVanishingPoint(const vpPoint &);
  void addFeatureVanishingPoint(const vpLine &, const vpLine &);

  void addFeatureEllipse(const vpCircle &);
  void addFeatureEllipse(const vpSphere &);

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

  void addFeatureSegment(vpPoint &, vpPoint &);

#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
  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);
#endif

  void clear();

  // ! Pose computation
  void computePose(vpHomogeneousMatrix &cMo, const vpPoseFeaturesMethodType &type = VIRTUAL_VS);

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

    return covarianceMatrix;
  }

  double getLambda() { return lambda; }

  unsigned int getVVSIterMax() { return vvsIterMax; }

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

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

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

  void setVerbose(const bool &mode) { verbose = mode; }

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

  void computePoseVVS(vpHomogeneousMatrix &cMo);
  void computePoseRobustVVS(vpHomogeneousMatrix &cMo);
};

#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)

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;
  featureSpecific_list.push_back(
      new vpPoseSpecificFeatureTemplate<featureType, RetType, ArgsFunc...>(fct_ptr, std::forward<ArgsFunc>(args)...));

  featureSpecific_list.back()->createDesired();

  totalSize++;
  if (featureSpecific_list.size() > maxSize)
    maxSize = static_cast<unsigned int>(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;
  featureSpecific_list.push_back(new vpPoseSpecificFeatureTemplateObject<ObjType, featureType, RetType, ArgsFunc...>(
      obj, fct_ptr, std::forward<ArgsFunc>(args)...));

  featureSpecific_list.back()->createDesired();

  totalSize++;
  if (featureSpecific_list.size() > maxSize)
    maxSize = static_cast<unsigned int>(featureSpecific_list.size());
}
#endif

#endif //#ifdef VISP_HAVE_MODULE_VISUAL_FEATURES

#endif