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,
115 CHECK_DEGENERATE_POINTS
130 std::vector<vpPoint> c3d;
132 bool computeCovariance;
137 unsigned int ransacNbInlierConsensus;
141 std::vector<vpPoint> ransacInliers;
143 std::vector<unsigned int> ransacInlierIndex;
145 double ransacThreshold;
148 double distanceToPlaneForCoplanarityTest;
153 std::vector<vpPoint> listOfPoints;
155 bool useParallelRansac;
157 int nbParallelRansacThreads;
166 RansacFunctor(
const vpHomogeneousMatrix &cMo_,
unsigned int ransacNbInlierConsensus_,
const int ransacMaxTrials_,
167 double ransacThreshold_,
unsigned int initial_seed_,
bool checkDegeneratePoints_,
168 const std::vector<vpPoint> &listOfUniquePoints_,
bool (*func_)(
const vpHomogeneousMatrix &))
169 : m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(
false),
170 m_func(func_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0), m_ransacMaxTrials(ransacMaxTrials_),
171 m_ransacNbInlierConsensus(ransacNbInlierConsensus_), m_ransacThreshold(ransacThreshold_),
172 m_uniRand(initial_seed_)
176 void operator()() { m_foundSolution = poseRansacImpl(); }
179 bool getResult()
const {
return m_foundSolution; }
181 std::vector<unsigned int> getBestConsensus()
const {
return m_best_consensus; }
185 unsigned int getNbInliers()
const {
return m_nbInliers; }
188 std::vector<unsigned int> m_best_consensus;
189 bool m_checkDegeneratePoints;
191 bool m_foundSolution;
193 std::vector<vpPoint> m_listOfUniquePoints;
194 unsigned int m_nbInliers;
195 int m_ransacMaxTrials;
196 unsigned int m_ransacNbInlierConsensus;
197 double m_ransacThreshold;
200 bool poseRansacImpl();
211 vpPose(
const std::vector<vpPoint> &lP);
213 void addPoint(
const vpPoint &P);
214 void addPoints(
const std::vector<vpPoint> &lP);
219 bool coplanar(
int &coplanar_plane_type);
232 void setDistanceToPlaneForCoplanarityTest(
double d);
248 if (t > std::numeric_limits<double>::epsilon()) {
278 if (!computeCovariance)
279 vpTRACE(
"Warning : The covariance matrix has not been computed. See "
280 "setCovarianceComputation() to do it.");
282 return covarianceMatrix;
336 std::vector<vpPoint> vectorOfPoints(listP.begin(), listP.end());
337 return vectorOfPoints;
340 static bool computePlanarObjectPoseFromRGBD(
const vpImage<float> &depthMap,
const std::vector<vpImagePoint> &corners,
343 double *confidence_index = NULL);
345 static bool computePlanarObjectPoseFromRGBD(
const vpImage<float> &depthMap,
346 const std::vector<std::vector<vpImagePoint> > &corners,
348 const std::vector<std::vector<vpPoint> > &point3d,
350 bool coplanar_points =
true);
351 static int computeRansacIterations(
double probability,
double epsilon,
const int sampleSize = 4,
352 int maxIterations = 2000);
359 static void findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
360 const unsigned int &numberOfInlierToReachAConsensus,
const double &threshold,
362 const int &maxNbTrials = 10000,
bool useParallelRansac =
true,
unsigned int nthreads = 0,
368 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
369 (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
385 template <
typename DataId>
387 const vpPlane &plane_in_camera_frame,
const std::map<DataId, vpPoint> &pts,
388 const std::map<DataId, vpImagePoint> &ips,
const vpCameraParameters &camera_intrinsics,
389 std::optional<vpHomogeneousMatrix> cMo_init = std::nullopt,
bool enable_vvs =
true)
391 if (cMo_init && !enable_vvs) {
394 "It doesn't make sense to use an initialized pose without enabling VVS to compute the pose from 4 points"));
406 for (
const auto &[ip_id, ip_unused] : ips) {
408 if (pts.find(ip_id) == end(pts)) {
410 "Cannot compute pose with points and image points which do not have the same IDs"));
414 std::vector<vpPoint> P{}, Q{};
418 for (
const auto &pt_map : pts) {
419 if (ips.find(pt_map.first) != end(ips)) {
423 const auto Z = plane_in_camera_frame.
computeZ(x, y);
430 P.emplace_back(x * Z, y * Z, Z);
446 static std::optional<vpHomogeneousMatrix> poseVirtualVSWithDepth(std::vector<vpPoint> points,
450 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
455 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)
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
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(std::vector< vpPoint > points, 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.