vpPose.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.
 */

 
#ifndef VP_POSE_H
#define VP_POSE_H
 
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/core/vpPlane.h>
#include <visp3/core/vpPoint.h>
#include <visp3/core/vpRGBa.h>
 
#include <list>
#include <math.h>
#include <vector>
 
// Check if std:c++17 or higher.
// Here we cannot use (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) in the declaration of the class
#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
#include <map>
#include <optional>
#endif
 
#include <visp3/core/vpUniRand.h>
 
BEGIN_VISP_NAMESPACE

class VISP_EXPORT vpPose
{
public:
  typedef enum
  {
    LAGRANGE,             
    DEMENTHON,            
    LOWE,                 
    RANSAC,               
    LAGRANGE_LOWE,        
    DEMENTHON_LOWE,       
    VIRTUAL_VS,           
    DEMENTHON_VIRTUAL_VS, 
    LAGRANGE_VIRTUAL_VS,  
    DEMENTHON_LAGRANGE_VIRTUAL_VS 
  } vpPoseMethodType;

  enum RANSAC_FILTER_FLAGS
  {
    NO_FILTER, 
    PREFILTER_DEGENERATE_POINTS, 
    CHECK_DEGENERATE_POINTS      
  };
 
  unsigned int npt;         
  std::list<vpPoint> listP; 
 
  double residual; 
 
public:
  // Typedef a function that checks if a pose is valid.
  typedef bool (*FuncCheckValidityPose)(const vpHomogeneousMatrix &);

  vpPose();
 
#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
  vpPose(const vpPose &) = default;
  vpPose &operator=(const vpPose &) = default;
#endif

  VP_EXPLICIT vpPose(const std::vector<vpPoint> &lP);

  virtual ~vpPose();

  void addPoint(const vpPoint &P);

  void addPoints(const std::vector<vpPoint> &lP);

  void clearPoint();

  bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, FuncCheckValidityPose func = nullptr);

  bool computePoseDementhonLagrangeVVS(vpHomogeneousMatrix &cMo);

  double computeResidual(const vpHomogeneousMatrix &cMo) const;

  double computeResidual(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam) const;

  double computeResidual(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, vpColVector &squaredResidual) const;

  bool coplanar(int &coplanar_plane_type, double *p_a = nullptr, double *p_b = nullptr, double *p_c = nullptr, double *p_d = nullptr);

  void displayModel(vpImage<unsigned char> &I, vpCameraParameters &cam, vpColor col = vpColor::none);

  void displayModel(vpImage<vpRGBa> &I, vpCameraParameters &cam, vpColor col = vpColor::none);

  void poseDementhonPlan(vpHomogeneousMatrix &cMo);

  void poseDementhonNonPlan(vpHomogeneousMatrix &cMo);

  void poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan = nullptr, double *p_a = nullptr, double *p_b = nullptr,
                        double *p_c = nullptr, double *p_d = nullptr);

  void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo);

  void poseLowe(vpHomogeneousMatrix &cMo);

  bool poseRansac(vpHomogeneousMatrix &cMo, FuncCheckValidityPose func = nullptr);

  void poseVirtualVSrobust(vpHomogeneousMatrix &cMo);

  void poseVirtualVS(vpHomogeneousMatrix &cMo);

  void printPoint();

  void setDementhonSvThreshold(const double &svThresh);

  void setDistToPlaneForCoplanTest(double d);

  void setLambda(double lambda) { m_lambda = lambda; }

  void setVvsEpsilon(const double eps)
  {
    if (eps >= 0) {
      vvsEpsilon = eps;
    }
    else {
      throw vpException(vpException::badValue, "Epsilon value must be >= 0.");
    }
  }

  void setVvsIterMax(int nb) { vvsIterMax = nb; }

  void setRansacNbInliersToReachConsensus(const unsigned int &nbC) { ransacNbInlierConsensus = nbC; }

  void setRansacThreshold(const double &t)
  {
    // Test whether or not t is > 0
    if (t > std::numeric_limits<double>::epsilon()) {
      ransacThreshold = t;
    }
    else {
      throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
    }
  }

  void setRansacMaxTrials(const int &rM) { ransacMaxTrials = rM; }

  unsigned int getRansacNbInliers() const { return static_cast<unsigned int>(ransacInliers.size()); }

  std::vector<unsigned int> getRansacInlierIndex() const { return ransacInlierIndex; }

  std::vector<vpPoint> getRansacInliers() const { return ransacInliers; }

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

  vpMatrix getCovarianceMatrix() const
  {
    if (!computeCovariance) {
      std::cout << "Warning: The covariance matrix has not been computed. See setCovarianceComputation() to do it." << std::endl;
    }
    return covarianceMatrix;
  }

  inline void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag) { ransacFlag = flag; }

  inline int getNbParallelRansacThreads() const { return nbParallelRansacThreads; }

  inline void setNbParallelRansacThreads(int nb) { nbParallelRansacThreads = nb; }

  inline bool getUseParallelRansac() const { return useParallelRansac; }

  inline void setUseParallelRansac(bool use) { useParallelRansac = use; }

  std::vector<vpPoint> getPoints() const
  {
    std::vector<vpPoint> vectorOfPoints(listP.begin(), listP.end());
    return vectorOfPoints;
  }

  static bool computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap, const std::vector<vpImagePoint> &corners,
                                              const vpCameraParameters &colorIntrinsics,
                                              const std::vector<vpPoint> &point3d, vpHomogeneousMatrix &cMo,
                                              double *confidence_index = nullptr);

  static bool computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap,
                                              const std::vector<std::vector<vpImagePoint> > &corners,
                                              const vpCameraParameters &colorIntrinsics,
                                              const std::vector<std::vector<vpPoint> > &point3d,
                                              vpHomogeneousMatrix &cMo, double *confidence_index = nullptr,
                                              bool coplanar_points = true);

  static int computeRansacIterations(double probability, double epsilon, const int sampleSize = 4,
                                     int maxIterations = 2000);

  static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
                      vpColor col = vpColor::none);

  static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
                      vpColor col = vpColor::none);

  static void findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
                        const unsigned int &numberOfInlierToReachAConsensus, const double &threshold,
                        unsigned int &ninliers, std::vector<vpPoint> &listInliers, vpHomogeneousMatrix &cMo,
                        const int &maxNbTrials = 10000, bool useParallelRansac = true, unsigned int nthreads = 0,
                        FuncCheckValidityPose func = nullptr);
 
#ifdef VISP_HAVE_HOMOGRAPHY
  static double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx,
                                  const vpCameraParameters &cam, vpHomogeneousMatrix &cMo);
#endif
 
  // Check if std:c++17 or higher.
  // Here we cannot use (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) in the declaration of the class
#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))

  template <typename DataId>
  static std::optional<vpHomogeneousMatrix> computePlanarObjectPoseWithAtLeast3Points(
    const vpPlane &plane_in_camera_frame, const std::map<DataId, vpPoint> &pts,
    const std::map<DataId, vpImagePoint> &ips, const vpCameraParameters &camera_intrinsics,
    std::optional<vpHomogeneousMatrix> cMo_init = std::nullopt, bool enable_vvs = true)
  {
    if (cMo_init && (!enable_vvs)) {
      throw(vpException(
        vpException::fatalError,
        "It doesn't make sense to use an initialized pose without enabling VVS to compute the pose from 4 points"));
    }
 
    // Check if detection and model fit
    // - The next line produces an internal compiler error with Visual Studio 2017:
    //   modules\vision\include\visp3/vision/vpPose.h(404): fatal error C1001: An internal error has occurred in the
    //   compiler.
    //    To work around this problem, try simplifying or changing the program near the locations listed above.
    //   Please choose the Technical Support command on the Visual C++
    //    Help menu, or open the Technical Support help file for more information
    // - Note that the next line builds with Visual Studio 2022.
    // for ([[maybe_unused]] const auto &[ip_id, _] : ips) {
    for (const auto &[ip_id, ip_unused] : ips) {
      (void)ip_unused;
      if (pts.find(ip_id) == end(pts)) {
        throw(vpException(vpException::fatalError,
                          "Cannot compute pose with points and image points which do not have the same IDs"));
      }
    }
 
    std::vector<vpPoint> P {}, Q {};
    // The next line in C++17 produces a build error with Visual Studio 2017, that's why we
    // use rather C++11 to loop through std::map
    // for (auto [pt_id, pt] : pts) {
    for (const auto &pt_map : pts) {
      if (ips.find(pt_map.first) != end(ips)) {
        double x = 0, y = 0;
        vpPoint pt = pt_map.second;
        vpPixelMeterConversion::convertPoint(camera_intrinsics, ips.at(pt_map.first), x, y);
        const auto Z = plane_in_camera_frame.computeZ(x, y);
 
        pt.set_x(x);
        pt.set_y(y);
        pt.set_Z(Z);
 
        Q.push_back(pt);
        P.emplace_back(x * Z, y * Z, Z);
      }
    }
 
    if (Q.size() < 3) {
      return std::nullopt;
    }
 
    auto cMo = cMo_init.value_or(vpHomogeneousMatrix::compute3d3dTransformation(P, Q));
    if (!cMo.isValid()) {
      return std::nullopt;
    }
 
    return enable_vvs ? vpPose::poseVirtualVSWithDepth(Q, cMo).value_or(cMo) : cMo;
  }

  static std::optional<vpHomogeneousMatrix> poseVirtualVSWithDepth(const std::vector<vpPoint> &points,
                                                                   const vpHomogeneousMatrix &cMo);
#endif
 
protected:
  double m_lambda; 
  double m_dementhonSvThresh; 

  double computeResidualDementhon(const vpHomogeneousMatrix &cMo);

  int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo);
 
private:
  void callLagrangePose(vpHomogeneousMatrix &cMo);

  int vvsIterMax;
  std::vector<vpPoint> c3d;
  bool computeCovariance;
  vpMatrix covarianceMatrix;
  unsigned int ransacNbInlierConsensus;
  int ransacMaxTrials;
  std::vector<vpPoint> ransacInliers;
  std::vector<unsigned int> ransacInlierIndex;
  double ransacThreshold;
  double distToPlaneForCoplanarityTest;
  RANSAC_FILTER_FLAGS ransacFlag;
  std::vector<vpPoint> listOfPoints;
  bool useParallelRansac;
  int nbParallelRansacThreads;
  double vvsEpsilon;

  class vpRansacFunctor
  {
  public:
    vpRansacFunctor(const vpHomogeneousMatrix &cMo_, unsigned int ransacNbInlierConsensus_, const int ransacMaxTrials_,
      double ransacThreshold_, unsigned int initial_seed_, bool checkDegeneratePoints_,
      const std::vector<vpPoint> &listOfUniquePoints_, FuncCheckValidityPose func_)
      : m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(false),
      m_func(func_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0), m_ransacMaxTrials(ransacMaxTrials_),
      m_ransacNbInlierConsensus(ransacNbInlierConsensus_), m_ransacThreshold(ransacThreshold_),
      m_uniRand(initial_seed_)
    { }

    void operator()() { m_foundSolution = poseRansacImpl(); }

    bool getResult() const { return m_foundSolution; }

    std::vector<unsigned int> getBestConsensus() const { return m_best_consensus; }

    vpHomogeneousMatrix getEstimatedPose() const { return m_cMo; }

    unsigned int getNbInliers() const { return m_nbInliers; }
 
  private:
    std::vector<unsigned int> m_best_consensus; 
    bool m_checkDegeneratePoints; 
    vpHomogeneousMatrix m_cMo; 
    bool m_foundSolution; 
    FuncCheckValidityPose m_func; 
    std::vector<vpPoint> m_listOfUniquePoints; 
    unsigned int m_nbInliers; 
    int m_ransacMaxTrials; 
    unsigned int m_ransacNbInlierConsensus; 
    double m_ransacThreshold; 
    vpUniRand m_uniRand; 

    bool poseRansacImpl();
  };
};
 
END_VISP_NAMESPACE
 
#endif