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>
55 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
59 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
60 (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
65 #include <visp3/core/vpUniRand.h>
102 DEMENTHON_LAGRANGE_VIRTUAL_VS,
111 CHECK_DEGENERATE_POINTS
126 std::vector<vpPoint> c3d;
128 bool computeCovariance;
133 unsigned int ransacNbInlierConsensus;
137 std::vector<vpPoint> ransacInliers;
139 std::vector<unsigned int> ransacInlierIndex;
141 double ransacThreshold;
144 double distanceToPlaneForCoplanarityTest;
149 std::vector<vpPoint> listOfPoints;
151 bool useParallelRansac;
153 int nbParallelRansacThreads;
162 RansacFunctor(
const vpHomogeneousMatrix &cMo_,
unsigned int ransacNbInlierConsensus_,
const int ransacMaxTrials_,
163 double ransacThreshold_,
unsigned int initial_seed_,
bool checkDegeneratePoints_,
164 const std::vector<vpPoint> &listOfUniquePoints_,
bool (*func_)(
const vpHomogeneousMatrix &))
165 : m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(
false),
166 m_func(func_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0), m_ransacMaxTrials(ransacMaxTrials_),
167 m_ransacNbInlierConsensus(ransacNbInlierConsensus_), m_ransacThreshold(ransacThreshold_),
168 m_uniRand(initial_seed_)
171 void operator()() { m_foundSolution = poseRansacImpl(); }
174 bool getResult()
const {
return m_foundSolution; }
176 std::vector<unsigned int> getBestConsensus()
const {
return m_best_consensus; }
180 unsigned int getNbInliers()
const {
return m_nbInliers; }
183 std::vector<unsigned int> m_best_consensus;
184 bool m_checkDegeneratePoints;
186 bool m_foundSolution;
188 std::vector<vpPoint> m_listOfUniquePoints;
189 unsigned int m_nbInliers;
190 int m_ransacMaxTrials;
191 unsigned int m_ransacNbInlierConsensus;
192 double m_ransacThreshold;
195 bool poseRansacImpl();
207 vpPose(
const std::vector<vpPoint> &lP);
209 void addPoint(
const vpPoint &P);
210 void addPoints(
const std::vector<vpPoint> &lP);
218 bool coplanar(
int &coplanar_plane_type,
double *p_a = NULL,
double *p_b = NULL,
double *p_c = NULL,
double *p_d = NULL);
224 void poseLagrangePlan(
vpHomogeneousMatrix &cMo,
bool *p_isPlan = NULL,
double *p_a = NULL,
double *p_b = NULL,
double *p_c = NULL,
double *p_d = NULL);
231 void setDementhonSvThreshold(
const double &svThresh);
232 void setDistanceToPlaneForCoplanarityTest(
double d);
249 if (t > std::numeric_limits<double>::epsilon()) {
280 if (!computeCovariance)
281 vpTRACE(
"Warning : The covariance matrix has not been computed. See "
282 "setCovarianceComputation() to do it.");
284 return covarianceMatrix;
338 std::vector<vpPoint> vectorOfPoints(listP.begin(), listP.end());
339 return vectorOfPoints;
342 static bool computePlanarObjectPoseFromRGBD(
const vpImage<float> &depthMap,
const std::vector<vpImagePoint> &corners,
345 double *confidence_index = NULL);
347 static bool computePlanarObjectPoseFromRGBD(
const vpImage<float> &depthMap,
348 const std::vector<std::vector<vpImagePoint> > &corners,
350 const std::vector<std::vector<vpPoint> > &point3d,
352 bool coplanar_points =
true);
353 static int computeRansacIterations(
double probability,
double epsilon,
const int sampleSize = 4,
354 int maxIterations = 2000);
361 static void findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
362 const unsigned int &numberOfInlierToReachAConsensus,
const double &threshold,
364 const int &maxNbTrials = 10000,
bool useParallelRansac =
true,
unsigned int nthreads = 0,
370 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
371 (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
387 template <
typename DataId>
389 const vpPlane &plane_in_camera_frame,
const std::map<DataId, vpPoint> &pts,
390 const std::map<DataId, vpImagePoint> &ips,
const vpCameraParameters &camera_intrinsics,
391 std::optional<vpHomogeneousMatrix> cMo_init = std::nullopt,
bool enable_vvs =
true)
393 if (cMo_init && !enable_vvs) {
396 "It doesn't make sense to use an initialized pose without enabling VVS to compute the pose from 4 points"));
408 for (
const auto &[ip_id, ip_unused] : ips) {
410 if (pts.find(ip_id) == end(pts)) {
412 "Cannot compute pose with points and image points which do not have the same IDs"));
416 std::vector<vpPoint> P {}, Q {};
420 for (
const auto &pt_map : pts) {
421 if (ips.find(pt_map.first) != end(ips)) {
425 const auto Z = plane_in_camera_frame.
computeZ(x, y);
432 P.emplace_back(x * Z, y * Z, Z);
448 static std::optional<vpHomogeneousMatrix> poseVirtualVSWithDepth(
const std::vector<vpPoint> &points,
452 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
457 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 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)
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.