1 /**************************************************************************** 2 * 3 * ViSP, open source Visual Servoing Platform software. 4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved. 5 * 6 * This software is free software; you can redistribute it and/or modify 7 * it under the terms of the GNU General Public License as published by 8 * the Free Software Foundation; either version 2 of the License, or 9 * (at your option) any later version. 10 * See the file LICENSE.txt at the root directory of this source 11 * distribution for additional information about the GNU GPL. 12 * 13 * For using ViSP with software that can not be combined with the GNU 14 * GPL, please contact Inria about acquiring a ViSP Professional 15 * Edition License. 16 * 17 * See http://visp.inria.fr for more information. 18 * 19 * This software was developed at: 20 * Inria Rennes - Bretagne Atlantique 21 * Campus Universitaire de Beaulieu 22 * 35042 Rennes Cedex 23 * France 24 * 25 * If you have questions regarding the use of this file, please contact 26 * Inria at visp@inria.fr 27 * 28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE 29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. 30 * 31 * Description: 32 * Pose computation. 33 * 34 * Authors: 35 * Eric Marchand 36 * Francois Chaumette 37 * Aurelien Yol 38 * 39 *****************************************************************************/ 40 41 /*! 42 \file vpPose.h 43 \brief Tools for pose computation (pose from point only). 44 */ 45 46 #ifndef _vpPose_h_ 47 #define _vpPose_h_ 48 49 #include <visp3/core/vpHomogeneousMatrix.h> 50 #include <visp3/core/vpPoint.h> 51 #include <visp3/core/vpRGBa.h> 52 #include <visp3/vision/vpHomography.h> 53 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS 54 #include <visp3/core/vpList.h> 55 #endif 56 #include <visp3/core/vpThread.h> 57 58 #include <list> 59 #include <math.h> 60 #include <vector> 61 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) 62 #include <atomic> 63 #endif 64 65 #include <visp3/core/vpUniRand.h> 66 67 /*! 68 \class vpPose 69 \ingroup group_vision_pose 70 \brief Class used for pose computation from N points (pose from point only). 71 Some of the algorithms implemented in this class are described in 72 \cite Marchand16a. 73 74 \note It is also possible to estimate a pose from other features using 75 vpPoseFeatures class. 76 77 To see how to use this class you can follow the \ref tutorial-pose-estimation. 78 */ 79 80 class VISP_EXPORT vpPose 81 { 82 public: 83 //! Methods that could be used to estimate the pose from points. 84 typedef enum { 85 LAGRANGE, /*!< Linear Lagrange approach (doesn't need an initialization) */ 86 DEMENTHON, /*!< Linear Dementhon aproach (doesn't need an initialization) */ 87 LOWE, /*!< Lowe aproach based on a Levenberg Marquartd non linear 88 minimization scheme that needs an initialization from Lagrange or 89 Dementhon aproach */ 90 RANSAC, /*!< Robust Ransac aproach (doesn't need an initialization) */ 91 LAGRANGE_LOWE, /*!< Non linear Lowe aproach initialized by Lagrange 92 approach */ 93 DEMENTHON_LOWE, /*!< Non linear Lowe aproach initialized by Dementhon 94 approach */ 95 VIRTUAL_VS, /*!< Non linear virtual visual servoing approach that needs an 96 initialization from Lagrange or Dementhon aproach */ 97 DEMENTHON_VIRTUAL_VS, /*!< Non linear virtual visual servoing approach 98 initialized by Dementhon approach */ 99 LAGRANGE_VIRTUAL_VS /*!< Non linear virtual visual servoing approach 100 initialized by Lagrange approach */ 101 } vpPoseMethodType; 102 103 enum RANSAC_FILTER_FLAGS { 104 NO_FILTER, 105 PREFILTER_DEGENERATE_POINTS, /*!< Remove degenerate points (same 3D or 2D coordinates) before the RANSAC. */ 106 CHECK_DEGENERATE_POINTS /*!< Check for degenerate points during the RANSAC. */ 107 }; 108 109 unsigned int npt; //!< Number of point used in pose computation 110 std::list<vpPoint> listP; //!< Array of point (use here class vpPoint) 111 112 double residual; //!< Residual in meter 113 114 protected: 115 double lambda; //!< parameters use for the virtual visual servoing approach 116 117 private: 118 //! define the maximum number of iteration in VVS 119 int vvsIterMax; 120 //! variable used in the Dementhon approach 121 std::vector<vpPoint> c3d; 122 //! Flag used to specify if the covariance matrix has to be computed or not. 123 bool computeCovariance; 124 //! Covariance matrix 125 vpMatrix covarianceMatrix; 126 //! Found a solution when there are at least a minimum number of points in 127 //! the consensus set 128 unsigned int ransacNbInlierConsensus; 129 //! Maximum number of iterations for the RANSAC 130 int ransacMaxTrials; 131 //! List of inlier points 132 std::vector<vpPoint> ransacInliers; 133 //! List of inlier point indexes (from the input list) 134 std::vector<unsigned int> ransacInlierIndex; 135 //! RANSAC threshold to consider a sample inlier or not 136 double ransacThreshold; 137 //! Minimal distance point to plane to consider if the point belongs or not 138 //! to the plane 139 double distanceToPlaneForCoplanarityTest; 140 //! RANSAC flag to remove or not degenerate points 141 RANSAC_FILTER_FLAGS ransacFlag; 142 //! List of points used for the RANSAC (std::vector is contiguous whereas 143 //! std::list is a linked list) 144 std::vector<vpPoint> listOfPoints; 145 //! If true, use a parallel RANSAC implementation 146 bool useParallelRansac; 147 //! Number of threads to spawn for the parallel RANSAC implementation 148 int nbParallelRansacThreads; 149 //! Stop the optimization loop when the residual change (|r-r_prec|) <= 150 //! epsilon 151 double vvsEpsilon; 152 153 // For parallel RANSAC 154 class RansacFunctor 155 { 156 public: RansacFunctor(const vpHomogeneousMatrix & cMo_,unsigned int ransacNbInlierConsensus_,const int ransacMaxTrials_,double ransacThreshold_,unsigned int initial_seed_,bool checkDegeneratePoints_,const std::vector<vpPoint> & listOfUniquePoints_,bool (* func_)(const vpHomogeneousMatrix &))157 RansacFunctor(const vpHomogeneousMatrix &cMo_, unsigned int ransacNbInlierConsensus_, 158 const int ransacMaxTrials_, double ransacThreshold_, unsigned int initial_seed_, 159 bool checkDegeneratePoints_, const std::vector<vpPoint> &listOfUniquePoints_, 160 bool (*func_)(const vpHomogeneousMatrix &)) 161 : 162 m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(false), 163 m_func(func_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0), 164 m_ransacMaxTrials(ransacMaxTrials_), m_ransacNbInlierConsensus(ransacNbInlierConsensus_), 165 m_ransacThreshold(ransacThreshold_), m_uniRand(initial_seed_) 166 { 167 } 168 operator()169 void operator()() { m_foundSolution = poseRansacImpl(); } 170 171 // Access the return value. getResult()172 bool getResult() const { return m_foundSolution; } 173 getBestConsensus()174 std::vector<unsigned int> getBestConsensus() const { return m_best_consensus; } 175 getEstimatedPose()176 vpHomogeneousMatrix getEstimatedPose() const { return m_cMo; } 177 getNbInliers()178 unsigned int getNbInliers() const { return m_nbInliers; } 179 180 private: 181 std::vector<unsigned int> m_best_consensus; 182 bool m_checkDegeneratePoints; 183 vpHomogeneousMatrix m_cMo; 184 bool m_foundSolution; 185 bool (*m_func)(const vpHomogeneousMatrix &); 186 std::vector<vpPoint> m_listOfUniquePoints; 187 unsigned int m_nbInliers; 188 int m_ransacMaxTrials; 189 unsigned int m_ransacNbInlierConsensus; 190 double m_ransacThreshold; 191 vpUniRand m_uniRand; 192 193 bool poseRansacImpl(); 194 }; 195 196 protected: 197 double computeResidualDementhon(const vpHomogeneousMatrix &cMo); 198 199 // method used in poseDementhonPlan() 200 int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo); 201 202 public: 203 vpPose(); 204 vpPose(const std::vector<vpPoint>& lP); 205 virtual ~vpPose(); 206 void addPoint(const vpPoint &P); 207 void addPoints(const std::vector<vpPoint> &lP); 208 void clearPoint(); 209 210 bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL); 211 double computeResidual(const vpHomogeneousMatrix &cMo) const; 212 bool coplanar(int &coplanar_plane_type); 213 void displayModel(vpImage<unsigned char> &I, vpCameraParameters &cam, vpColor col = vpColor::none); 214 void displayModel(vpImage<vpRGBa> &I, vpCameraParameters &cam, vpColor col = vpColor::none); 215 216 void poseDementhonPlan(vpHomogeneousMatrix &cMo); 217 void poseDementhonNonPlan(vpHomogeneousMatrix &cMo); 218 void poseLagrangePlan(vpHomogeneousMatrix &cMo); 219 void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo); 220 void poseLowe(vpHomogeneousMatrix &cMo); 221 bool poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL); 222 void poseVirtualVSrobust(vpHomogeneousMatrix &cMo); 223 void poseVirtualVS(vpHomogeneousMatrix &cMo); 224 void printPoint(); 225 void setDistanceToPlaneForCoplanarityTest(double d); setLambda(double a)226 void setLambda(double a) { lambda = a; } setVvsEpsilon(const double eps)227 void setVvsEpsilon(const double eps) 228 { 229 if (eps >= 0) { 230 vvsEpsilon = eps; 231 } else { 232 throw vpException(vpException::badValue, "Epsilon value must be >= 0."); 233 } 234 } setVvsIterMax(int nb)235 void setVvsIterMax(int nb) { vvsIterMax = nb; } 236 setRansacNbInliersToReachConsensus(const unsigned int & nbC)237 void setRansacNbInliersToReachConsensus(const unsigned int &nbC) { ransacNbInlierConsensus = nbC; } setRansacThreshold(const double & t)238 void setRansacThreshold(const double &t) 239 { 240 // Test whether or not t is > 0 241 if (t > std::numeric_limits<double>::epsilon()) { 242 ransacThreshold = t; 243 } else { 244 throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance."); 245 } 246 } setRansacMaxTrials(const int & rM)247 void setRansacMaxTrials(const int &rM) { ransacMaxTrials = rM; } getRansacNbInliers()248 unsigned int getRansacNbInliers() const { return (unsigned int)ransacInliers.size(); } getRansacInlierIndex()249 std::vector<unsigned int> getRansacInlierIndex() const { return ransacInlierIndex; } getRansacInliers()250 std::vector<vpPoint> getRansacInliers() const { return ransacInliers; } 251 252 /*! 253 Set if the covariance matrix has to be computed in the Virtual Visual 254 Servoing approach. 255 256 \param flag : True if the covariance has to be computed, false otherwise. 257 */ setCovarianceComputation(const bool & flag)258 void setCovarianceComputation(const bool &flag) { computeCovariance = flag; } 259 260 /*! 261 Get the covariance matrix computed in the Virtual Visual Servoing 262 approach. 263 264 \warning The compute covariance flag has to be true if you want to compute 265 the covariance matrix. 266 267 \sa setCovarianceComputation 268 */ getCovarianceMatrix()269 vpMatrix getCovarianceMatrix() const 270 { 271 if (!computeCovariance) 272 vpTRACE("Warning : The covariance matrix has not been computed. See " 273 "setCovarianceComputation() to do it."); 274 275 return covarianceMatrix; 276 } 277 278 /*! 279 Set RANSAC filter flag. 280 281 \param flag : RANSAC flag to use to prefilter or perform degenerate configuration check. 282 \sa RANSAC_FILTER_FLAGS 283 \warning Prefilter degenerate points consists to not add subsequent degenerate points. This means that 284 it is possible to discard a valid point and keep an invalid point if the invalid point 285 is added first. It is faster to prefilter for duplicate points instead of checking for degenerate 286 configuration at each time. 287 \note By default the flag is set to NO_FILTER. 288 */ setRansacFilterFlag(const RANSAC_FILTER_FLAGS & flag)289 inline void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag) { ransacFlag = flag; } 290 291 /*! 292 Get the number of threads for the parallel RANSAC implementation. 293 294 \sa setNbParallelRansacThreads 295 */ getNbParallelRansacThreads()296 inline int getNbParallelRansacThreads() const { return nbParallelRansacThreads; } 297 298 /*! 299 Set the number of threads for the parallel RANSAC implementation. 300 301 \note You have to enable the parallel version with setUseParallelRansac(). 302 If the number of threads is 0, the number of threads to use is 303 automatically determined with C++11. 304 \sa setUseParallelRansac 305 */ setNbParallelRansacThreads(int nb)306 inline void setNbParallelRansacThreads(int nb) { nbParallelRansacThreads = nb; } 307 308 /*! 309 \return True if the parallel RANSAC version should be used (depends also to C++11 availability). 310 311 \sa setUseParallelRansac 312 */ getUseParallelRansac()313 inline bool getUseParallelRansac() const { return useParallelRansac; } 314 315 /*! 316 Set if parallel RANSAC version should be used or not (only if C++11). 317 318 \note Need C++11 or higher. 319 */ setUseParallelRansac(bool use)320 inline void setUseParallelRansac(bool use) { useParallelRansac = use; } 321 322 /*! 323 Get the vector of points. 324 325 \return The vector of points. 326 */ getPoints()327 std::vector<vpPoint> getPoints() const 328 { 329 std::vector<vpPoint> vectorOfPoints(listP.begin(), listP.end()); 330 return vectorOfPoints; 331 } 332 333 static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size, 334 vpColor col = vpColor::none); 335 static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size, 336 vpColor col = vpColor::none); 337 static double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx, 338 vpCameraParameters &cam, vpHomogeneousMatrix &cMo); 339 340 static int computeRansacIterations(double probability, double epsilon, const int sampleSize = 4, 341 int maxIterations = 2000); 342 343 static void findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D, 344 const unsigned int &numberOfInlierToReachAConsensus, const double &threshold, 345 unsigned int &ninliers, std::vector<vpPoint> &listInliers, vpHomogeneousMatrix &cMo, 346 const int &maxNbTrials=10000, bool useParallelRansac=true, unsigned int nthreads=0, 347 bool (*func)(const vpHomogeneousMatrix &) = NULL); 348 349 static bool computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap, const std::vector<vpImagePoint> &corners, 350 const vpCameraParameters &colorIntrinsics, const std::vector<vpPoint> &point3d, vpHomogeneousMatrix &cMo, 351 double *confidence_index = NULL); 352 353 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) 354 /*! 355 @name Deprecated functions 356 */ 357 //@{ 358 vp_deprecated void init(); 359 //@} 360 #endif 361 }; 362 363 #endif 364