51 #include <visp3/core/vpHomogeneousMatrix.h>
52 #include <visp3/core/vpPixelMeterConversion.h>
53 #include <visp3/core/vpPoint.h>
54 #include <visp3/core/vpRGBa.h>
55 #include <visp3/vision/vpHomography.h>
56 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
57 #include <visp3/core/vpList.h>
59 #include <visp3/core/vpThread.h>
64 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
68 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
69 (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
74 #include <visp3/core/vpUniRand.h>
106 DEMENTHON_VIRTUAL_VS,
110 DEMENTHON_LAGRANGE_VIRTUAL_VS,
118 CHECK_DEGENERATE_POINTS
133 std::vector<vpPoint> c3d;
135 bool computeCovariance;
140 unsigned int ransacNbInlierConsensus;
144 std::vector<vpPoint> ransacInliers;
146 std::vector<unsigned int> ransacInlierIndex;
148 double ransacThreshold;
151 double distanceToPlaneForCoplanarityTest;
156 std::vector<vpPoint> listOfPoints;
158 bool useParallelRansac;
160 int nbParallelRansacThreads;
169 RansacFunctor(
const vpHomogeneousMatrix &cMo_,
unsigned int ransacNbInlierConsensus_,
const int ransacMaxTrials_,
170 double ransacThreshold_,
unsigned int initial_seed_,
bool checkDegeneratePoints_,
171 const std::vector<vpPoint> &listOfUniquePoints_,
bool (*func_)(
const vpHomogeneousMatrix &))
172 : m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(
false),
173 m_func(func_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0), m_ransacMaxTrials(ransacMaxTrials_),
174 m_ransacNbInlierConsensus(ransacNbInlierConsensus_), m_ransacThreshold(ransacThreshold_),
175 m_uniRand(initial_seed_)
179 void operator()() { m_foundSolution = poseRansacImpl(); }
182 bool getResult()
const {
return m_foundSolution; }
184 std::vector<unsigned int> getBestConsensus()
const {
return m_best_consensus; }
188 unsigned int getNbInliers()
const {
return m_nbInliers; }
191 std::vector<unsigned int> m_best_consensus;
192 bool m_checkDegeneratePoints;
194 bool m_foundSolution;
196 std::vector<vpPoint> m_listOfUniquePoints;
197 unsigned int m_nbInliers;
198 int m_ransacMaxTrials;
199 unsigned int m_ransacNbInlierConsensus;
200 double m_ransacThreshold;
203 bool poseRansacImpl();
215 vpPose(
const std::vector<vpPoint> &lP);
217 void addPoint(
const vpPoint &P);
218 void addPoints(
const std::vector<vpPoint> &lP);
224 bool coplanar(
int &coplanar_plane_type,
double *p_a = NULL,
double *p_b = NULL,
double *p_c = NULL,
double *p_d = NULL);
230 void poseLagrangePlan(
vpHomogeneousMatrix &cMo,
bool *p_isPlan = NULL,
double *p_a = NULL,
double *p_b = NULL,
double *p_c = NULL,
double *p_d = NULL);
237 void setDementhonSvThreshold(
const double& svThresh);
238 void setDistanceToPlaneForCoplanarityTest(
double d);
254 if (t > std::numeric_limits<double>::epsilon()) {
284 if (!computeCovariance)
285 vpTRACE(
"Warning : The covariance matrix has not been computed. See "
286 "setCovarianceComputation() to do it.");
288 return covarianceMatrix;
342 std::vector<vpPoint> vectorOfPoints(listP.begin(), listP.end());
343 return vectorOfPoints;
346 static bool computePlanarObjectPoseFromRGBD(
const vpImage<float> &depthMap,
const std::vector<vpImagePoint> &corners,
349 double *confidence_index = NULL);
351 static bool computePlanarObjectPoseFromRGBD(
const vpImage<float> &depthMap,
352 const std::vector<std::vector<vpImagePoint> > &corners,
354 const std::vector<std::vector<vpPoint> > &point3d,
356 bool coplanar_points =
true);
357 static int computeRansacIterations(
double probability,
double epsilon,
const int sampleSize = 4,
358 int maxIterations = 2000);
365 static void findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
366 const unsigned int &numberOfInlierToReachAConsensus,
const double &threshold,
368 const int &maxNbTrials = 10000,
bool useParallelRansac =
true,
unsigned int nthreads = 0,
374 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
375 (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
391 template <
typename DataId>
393 const vpPlane &plane_in_camera_frame,
const std::map<DataId, vpPoint> &pts,
394 const std::map<DataId, vpImagePoint> &ips,
const vpCameraParameters &camera_intrinsics,
395 std::optional<vpHomogeneousMatrix> cMo_init = std::nullopt,
bool enable_vvs =
true)
397 if (cMo_init && !enable_vvs) {
400 "It doesn't make sense to use an initialized pose without enabling VVS to compute the pose from 4 points"));
412 for (
const auto &[ip_id, ip_unused] : ips) {
414 if (pts.find(ip_id) == end(pts)) {
416 "Cannot compute pose with points and image points which do not have the same IDs"));
420 std::vector<vpPoint> P{}, Q{};
424 for (
const auto &pt_map : pts) {
425 if (ips.find(pt_map.first) != end(ips)) {
429 const auto Z = plane_in_camera_frame.
computeZ(x, y);
436 P.emplace_back(x * Z, y * Z, Z);
452 static std::optional<vpHomogeneousMatrix> poseVirtualVSWithDepth(
const std::vector<vpPoint> &points,
456 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
461 vp_deprecated
void init();
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Class to define RGB colors available for display functionnalities.
static const vpColor none
error that can be emited 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)
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
vpMatrix getCovarianceMatrix() const
bool getUseParallelRansac() const
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)
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 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
unsigned int getRansacNbInliers() const
void setUseParallelRansac(bool use)
static std::optional< vpHomogeneousMatrix > poseVirtualVSWithDepth(const std::vector< vpPoint > &points, const vpHomogeneousMatrix &cMo)
std::vector< vpPoint > getRansacInliers() const
double lambda
parameters use for the virtual visual servoing approach
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.