42 #include <visp3/core/vpCameraParameters.h>
43 #include <visp3/core/vpHomogeneousMatrix.h>
44 #include <visp3/core/vpPixelMeterConversion.h>
45 #include <visp3/core/vpPoint.h>
46 #include <visp3/core/vpRGBa.h>
47 #include <visp3/vision/vpHomography.h>
48 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
49 #include <visp3/core/vpList.h>
58 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
63 #include <visp3/core/vpUniRand.h>
99 DEMENTHON_LAGRANGE_VIRTUAL_VS,
111 CHECK_DEGENERATE_POINTS
128 vpPose(
const std::vector<vpPoint> &lP);
143 void addPoint(
const vpPoint &P);
153 void addPoints(
const std::vector<vpPoint> &lP);
257 bool coplanar(
int &coplanar_plane_type,
double *p_a =
nullptr,
double *p_b =
nullptr,
double *p_c =
nullptr,
double *p_d =
nullptr);
295 void poseLagrangePlan(
vpHomogeneousMatrix &cMo,
bool *p_isPlan =
nullptr,
double *p_a =
nullptr,
double *p_b =
nullptr,
296 double *p_c =
nullptr,
double *p_d =
nullptr);
352 void setDementhonSvThreshold(
const double &svThresh);
357 void setDistanceToPlaneForCoplanarityTest(
double d);
393 if (t > std::numeric_limits<double>::epsilon()) {
440 if (!computeCovariance)
441 vpTRACE(
"Warning : The covariance matrix has not been computed. See "
442 "setCovarianceComputation() to do it.");
444 return covarianceMatrix;
498 std::vector<vpPoint> vectorOfPoints(listP.begin(), listP.end());
499 return vectorOfPoints;
522 static bool computePlanarObjectPoseFromRGBD(
const vpImage<float> &depthMap,
const std::vector<vpImagePoint> &corners,
525 double *confidence_index =
nullptr);
559 static bool computePlanarObjectPoseFromRGBD(
const vpImage<float> &depthMap,
560 const std::vector<std::vector<vpImagePoint> > &corners,
562 const std::vector<std::vector<vpPoint> > &point3d,
564 bool coplanar_points =
true);
585 static int computeRansacIterations(
double probability,
double epsilon,
const int sampleSize = 4,
586 int maxIterations = 2000);
644 static void findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
645 const unsigned int &numberOfInlierToReachAConsensus,
const double &threshold,
647 const int &maxNbTrials = 10000,
bool useParallelRansac =
true,
unsigned int nthreads = 0,
674 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
690 template <
typename DataId>
691 static std::optional<vpHomogeneousMatrix> computePlanarObjectPoseWithAtLeast3Points(
692 const vpPlane &plane_in_camera_frame,
const std::map<DataId, vpPoint> &pts,
693 const std::map<DataId, vpImagePoint> &ips,
const vpCameraParameters &camera_intrinsics,
694 std::optional<vpHomogeneousMatrix> cMo_init = std::nullopt,
bool enable_vvs =
true)
696 if (cMo_init && !enable_vvs) {
699 "It doesn't make sense to use an initialized pose without enabling VVS to compute the pose from 4 points"));
711 for (
const auto &[ip_id, ip_unused] : ips) {
713 if (pts.find(ip_id) == end(pts)) {
715 "Cannot compute pose with points and image points which do not have the same IDs"));
719 std::vector<vpPoint> P {}, Q {};
723 for (
const auto &pt_map : pts) {
724 if (ips.find(pt_map.first) != end(ips)) {
728 const auto Z = plane_in_camera_frame.
computeZ(x, y);
735 P.emplace_back(x * Z, y * Z, Z);
748 return enable_vvs ? vpPose::poseVirtualVSWithDepth(Q, cMo).value_or(cMo) : cMo;
760 static std::optional<vpHomogeneousMatrix> poseVirtualVSWithDepth(
const std::vector<vpPoint> &points,
788 std::vector<vpPoint> c3d;
790 bool computeCovariance;
795 unsigned int ransacNbInlierConsensus;
799 std::vector<vpPoint> ransacInliers;
801 std::vector<unsigned int> ransacInlierIndex;
803 double ransacThreshold;
806 double distanceToPlaneForCoplanarityTest;
811 std::vector<vpPoint> listOfPoints;
813 bool useParallelRansac;
815 int nbParallelRansacThreads;
823 class vpRansacFunctor
829 vpRansacFunctor(
const vpHomogeneousMatrix &cMo_,
unsigned int ransacNbInlierConsensus_,
const int ransacMaxTrials_,
830 double ransacThreshold_,
unsigned int initial_seed_,
bool checkDegeneratePoints_,
831 const std::vector<vpPoint> &listOfUniquePoints_,
bool (*func_)(
const vpHomogeneousMatrix &))
832 : m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(false),
833 m_func(func_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0), m_ransacMaxTrials(ransacMaxTrials_),
834 m_ransacNbInlierConsensus(ransacNbInlierConsensus_), m_ransacThreshold(ransacThreshold_),
835 m_uniRand(initial_seed_)
841 void operator()() { m_foundSolution = poseRansacImpl(); }
846 bool getResult()
const {
return m_foundSolution; }
851 std::vector<unsigned int> getBestConsensus()
const {
return m_best_consensus; }
861 unsigned int getNbInliers()
const {
return m_nbInliers; }
864 std::vector<unsigned int> m_best_consensus;
865 bool m_checkDegeneratePoints;
867 bool m_foundSolution;
869 std::vector<vpPoint> m_listOfUniquePoints;
870 unsigned int m_nbInliers;
871 int m_ransacMaxTrials;
872 unsigned int m_ransacNbInlierConsensus;
873 double m_ransacThreshold;
880 bool poseRansacImpl();
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Class to define RGB colors available for display functionalities.
static const vpColor none
error that can be emitted by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
Implementation of an homogeneous matrix and operations on such kind of matrices.
static vpHomogeneousMatrix compute3d3dTransformation(const std::vector< vpPoint > &p, const std::vector< vpPoint > &q)
Implementation of a matrix and operations on matrices.
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
This class defines the container for a plane geometrical structure.
double computeZ(double x, double y) const
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
void set_x(double x)
Set the point x coordinate in the image plane.
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
void set_y(double y)
Set the point y coordinate in the image plane.
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
void setRansacMaxTrials(const int &rM)
double m_lambda
Parameters use for the virtual visual servoing approach.
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
vpMatrix getCovarianceMatrix() const
bool getUseParallelRansac() const
vpPoseMethodType
Methods that could be used to estimate the pose from points.
void setCovarianceComputation(const bool &flag)
unsigned int npt
Number of point used in pose computation.
int getNbParallelRansacThreads() const
std::vector< vpPoint > getPoints() const
std::vector< unsigned int > getRansacInlierIndex() const
double m_dementhonSvThresh
SVD threshold use for the pseudo-inverse computation in poseDementhonPlan.
std::list< vpPoint > listP
Array of point (use here class vpPoint)
void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag)
void setVvsIterMax(int nb)
@ PREFILTER_DEGENERATE_POINTS
@ NO_FILTER
No filter is applied.
unsigned int getRansacNbInliers() const
void setUseParallelRansac(bool use)
std::vector< vpPoint > getRansacInliers() const
void setLambda(double lambda)
void setVvsEpsilon(const double eps)
void setNbParallelRansacThreads(int nb)
double residual
Residual in meter.
void setRansacThreshold(const double &t)
Class for generating random numbers with uniform probability density.