45 #ifndef vpMbTracker_hh
46 #define vpMbTracker_hh
52 #include <visp3/core/vpCameraParameters.h>
53 #include <visp3/core/vpColVector.h>
54 #include <visp3/core/vpHomogeneousMatrix.h>
55 #include <visp3/core/vpImage.h>
56 #include <visp3/core/vpImagePoint.h>
57 #include <visp3/core/vpMatrix.h>
58 #include <visp3/core/vpPoint.h>
59 #include <visp3/core/vpPolygon.h>
60 #include <visp3/core/vpRGBa.h>
61 #include <visp3/core/vpRobust.h>
62 #include <visp3/mbt/vpMbHiddenFaces.h>
63 #include <visp3/mbt/vpMbtPolygon.h>
65 #include <visp3/mbt/vpMbtDistanceCircle.h>
66 #include <visp3/mbt/vpMbtDistanceCylinder.h>
67 #include <visp3/mbt/vpMbtDistanceLine.h>
69 #ifdef VISP_HAVE_COIN3D
77 #include <Inventor/VRMLnodes/SoVRMLGroup.h>
78 #include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
79 #include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
107 typedef enum { GAUSS_NEWTON_OPT = 0, LEVENBERG_MARQUARDT_OPT = 1 } vpMbtOptimizationMethod;
256 virtual inline unsigned int getClipping()
const {
return clippingFlag; }
267 if (!computeCovariance) {
270 std::cerr <<
"Warning : The covariance matrix has not been computed. "
271 "See setCovarianceComputation() to do it."
275 return covarianceMatrix;
291 virtual inline double getLambda()
const {
return m_lambda; }
298 virtual inline unsigned int getMaxIter()
const {
return m_maxIter; }
368 virtual inline unsigned int getNbPolygon()
const {
return static_cast<unsigned int>(faces.
size()); }
397 if (index >=
static_cast<unsigned int>(faces.
size())) {
404 virtual std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
405 getPolygonFaces(
bool orderPolygons =
true,
bool useVisibility =
true,
bool clipPolygon =
false);
429 #ifdef VISP_HAVE_MODULE_GUI
430 virtual void initClick(
const vpImage<unsigned char> &I,
const std::string &initFile,
bool displayHelp =
false,
432 virtual void initClick(
const vpImage<vpRGBa> &I_color,
const std::string &initFile,
bool displayHelp =
false,
436 const std::string &displayFile =
"");
437 virtual void initClick(
const vpImage<vpRGBa> &I_color,
const std::vector<vpPoint> &points3D_list,
438 const std::string &displayFile =
"");
442 virtual void initFromPoints(
const vpImage<vpRGBa> &I_color,
const std::string &initFile);
444 virtual void initFromPoints(
const vpImage<unsigned char> &I,
const std::vector<vpImagePoint> &points2D_list,
445 const std::vector<vpPoint> &points3D_list);
446 virtual void initFromPoints(
const vpImage<vpRGBa> &I_color,
const std::vector<vpImagePoint> &points2D_list,
447 const std::vector<vpPoint> &points3D_list);
450 virtual void initFromPose(
const vpImage<vpRGBa> &I_color,
const std::string &initFile);
458 virtual void loadModel(
const std::string &modelFile,
bool verbose =
false,
490 virtual void setClipping(
const unsigned int &flags);
520 virtual void setEstimatedDoF(
const vpColVector &v);
522 virtual void setFarClippingDistance(
const double &dist);
536 virtual inline void setLambda(
double gain) { m_lambda = gain; }
538 virtual void setLod(
bool useLod,
const std::string &name =
"");
545 virtual inline void setMaxIter(
unsigned int max) { m_maxIter = max; }
547 virtual void setMinLineLengthThresh(
double minLineLengthThresh,
const std::string &name =
"");
549 virtual void setMinPolygonAreaThresh(
double minPolygonAreaThresh,
const std::string &name =
"");
551 virtual void setNearClippingDistance(
const double &dist);
560 void setProjectionErrorMovingEdge(
const vpMe &me);
562 void setProjectionErrorKernelSize(
const unsigned int &size);
597 m_projectionErrorDisplayLength = length;
605 m_projectionErrorDisplayThickness = thickness;
610 virtual void setOgreVisibilityTest(
const bool &v);
612 void savePose(
const std::string &filename)
const;
614 #ifdef VISP_HAVE_OGRE
679 const vpColor &col,
unsigned int thickness = 1,
bool displayFullModel =
false) = 0;
693 const vpColor &col,
unsigned int thickness = 1,
bool displayFullModel =
false) = 0;
695 virtual std::vector<std::vector<double> >
getModelForDisplay(
unsigned int width,
unsigned int height,
698 bool displayFullModel =
false) = 0;
715 virtual void loadConfigFile(
const std::string &configFile,
bool verbose =
true);
769 void addPolygon(
const std::vector<vpPoint> &corners,
int idFace = -1,
const std::string &polygonName =
"",
770 bool useLod =
false,
double minPolygonAreaThreshold = 2500.0,
double minLineLengthThreshold = 50.0);
771 void addPolygon(
const vpPoint &p1,
const vpPoint &p2,
const vpPoint &p3,
double radius,
int idFace = -1,
772 const std::string &polygonName =
"",
bool useLod =
false,
double minPolygonAreaThreshold = 2500.0);
773 void addPolygon(
const vpPoint &p1,
const vpPoint &p2,
int idFace = -1,
const std::string &polygonName =
"",
774 bool useLod =
false,
double minLineLengthThreshold = 50);
775 void addPolygon(
const std::vector<std::vector<vpPoint> > &listFaces,
int idFace = -1,
776 const std::string &polygonName =
"",
bool useLod =
false,
double minLineLengthThreshold = 50);
778 void addProjectionErrorCircle(
const vpPoint &P1,
const vpPoint &P2,
const vpPoint &P3,
double r,
int idFace = -1,
779 const std::string &name =
"");
780 void addProjectionErrorCylinder(
const vpPoint &P1,
const vpPoint &P2,
double r,
int idFace = -1,
781 const std::string &name =
"");
782 void addProjectionErrorLine(
vpPoint &p1,
vpPoint &p2,
int polygon = -1, std::string name =
"");
784 void addProjectionErrorPolygon(
const std::vector<vpPoint> &corners,
int idFace = -1,
785 const std::string &polygonName =
"",
bool useLod =
false,
786 double minPolygonAreaThreshold = 2500.0,
const double minLineLengthThreshold = 50.0);
787 void addProjectionErrorPolygon(
const vpPoint &p1,
const vpPoint &p2,
const vpPoint &p3,
double radius,
788 int idFace = -1,
const std::string &polygonName =
"",
bool useLod =
false,
789 double minPolygonAreaThreshold = 2500.0);
790 void addProjectionErrorPolygon(
const vpPoint &p1,
const vpPoint &p2,
int idFace = -1,
791 const std::string &polygonName =
"",
bool useLod =
false,
792 double minLineLengthThreshold = 50);
793 void addProjectionErrorPolygon(
const std::vector<std::vector<vpPoint> > &listFaces,
int idFace = -1,
794 const std::string &polygonName =
"",
bool useLod =
false,
795 double minLineLengthThreshold = 50);
797 void createCylinderBBox(
const vpPoint &p1,
const vpPoint &p2,
const double &radius,
798 std::vector<std::vector<vpPoint> > &listFaces);
800 virtual void computeCovarianceMatrixVVS(
const bool isoJoIdentity,
const vpColVector &w_true,
809 virtual void computeVVSCheckLevenbergMarquardt(
unsigned int iter,
vpColVector &error,
const vpColVector &m_error_prev,
811 bool &reStartFromLastIncrement,
vpColVector *
const w =
nullptr,
815 virtual void computeVVSPoseEstimation(
const bool isoJoIdentity,
unsigned int iter,
vpMatrix &L,
vpMatrix <L,
821 #ifdef VISP_HAVE_COIN3D
822 virtual void extractGroup(SoVRMLGroup *sceneGraphVRML2,
vpHomogeneousMatrix &transform,
int &idFace);
823 virtual void extractFaces(SoVRMLIndexedFaceSet *face_set,
vpHomogeneousMatrix &transform,
int &idFace,
824 const std::string &polygonName =
"");
825 virtual void extractLines(SoVRMLIndexedLineSet *line_set,
int &idFace,
const std::string &polygonName =
"");
826 virtual void extractCylinders(SoVRMLIndexedFaceSet *face_set,
vpHomogeneousMatrix &transform,
int &idFace,
827 const std::string &polygonName =
"");
830 vpPoint getGravityCenter(
const std::vector<vpPoint> &_pts)
const;
846 const std::string &name =
"") = 0;
848 #ifdef VISP_HAVE_MODULE_GUI
850 const std::string &initFile,
bool displayHelp =
false,
854 const std::vector<vpPoint> &points3D_list,
const std::string &displayFile =
"");
858 const std::string &initFile);
861 const std::vector<vpImagePoint> &points2D_list,
862 const std::vector<vpPoint> &points3D_list);
865 const std::string &initFile);
878 const std::string &name =
"") = 0;
895 void initProjectionErrorCircle(
const vpPoint &p1,
const vpPoint &p2,
const vpPoint &p3,
double radius,
int idFace = 0,
896 const std::string &name =
"");
897 void initProjectionErrorCylinder(
const vpPoint &p1,
const vpPoint &p2,
double radius,
int idFace = 0,
898 const std::string &name =
"");
899 void initProjectionErrorFaceFromCorners(
vpMbtPolygon &polygon);
900 void initProjectionErrorFaceFromLines(
vpMbtPolygon &polygon);
902 virtual void loadVRMLModel(
const std::string &modelFile);
903 virtual void loadCAOModel(
const std::string &modelFile, std::vector<std::string> &vectorOfModelFilename,
904 int &startIdFace,
bool verbose =
false,
bool parent =
true,
908 void projectionErrorResetMovingEdges();
909 void projectionErrorVisibleFace(
unsigned int width,
unsigned int height,
const vpHomogeneousMatrix &_cMo);
911 void removeComment(std::ifstream &fileId);
913 std::map<std::string, std::string> parseParameters(std::string &endLine);
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Class to define RGB colors available for display functionalities.
error that can be emitted by ViSP classes.
@ dimensionError
Bad dimension.
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
void setGoodNbRayCastingAttemptsRatio(const double &ratio)
unsigned int size() const
Main methods for a model-based tracker.
virtual double getNearClippingDistance() const
virtual void setProjectionErrorDisplayArrowLength(unsigned int length)
double m_lambda
Gain of the virtual visual servoing stage.
virtual void setMaxIter(unsigned int max)
virtual void display(const vpImage< vpRGBa > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)=0
vpCameraParameters m_projectionErrorCam
Camera parameters used for projection error computation.
unsigned int nbPolygonPoints
Number of polygon points in CAO model.
virtual void setPose(const vpImage< vpRGBa > &I_color, const vpHomogeneousMatrix &cdMo)=0
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
virtual void track(const vpImage< unsigned char > &I)=0
virtual double getInitialMu() const
virtual void resetTracker()=0
virtual void setOgreShowConfigDialog(bool showConfigDialog)
virtual double getAngleAppear() const
virtual void setMask(const vpImage< bool > &mask)
virtual void getCameraParameters(vpCameraParameters &cam) const
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
std::vector< vpMbtDistanceCylinder * > m_projectionErrorCylinders
Distance cylinder primitives for projection error.
virtual void init(const vpImage< unsigned char > &I)=0
virtual void setDisplayFeatures(bool displayF)
virtual vpHomogeneousMatrix getPose() const
bool useLodGeneral
True if LOD mode is enabled.
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
std::map< std::string, std::string > mapOfParameterNames
bool m_computeInteraction
vpMatrix oJo
The Degrees of Freedom to estimate.
virtual void getPose(vpHomogeneousMatrix &cMo) const
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)=0
unsigned int nbLines
Number of lines in CAO model.
virtual void initFaceFromLines(vpMbtPolygon &polygon)=0
vpUniRand m_rand
Random number generator used in vpMbtDistanceLine::buildFrom()
vpMatrix covarianceMatrix
Covariance matrix.
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
virtual vpColVector getError() const =0
vpHomogeneousMatrix m_cMo
The current pose.
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")=0
virtual vpMbtOptimizationMethod getOptimizationMethod() const
vpMatrix m_SobelX
Sobel kernel in X.
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")=0
void setPoseSavingFilename(const std::string &filename)
unsigned int nbPoints
Number of points in CAO model.
vpCameraParameters m_cam
The camera parameters.
double m_stopCriteriaEpsilon
Epsilon threshold to stop the VVS optimization loop.
std::string modelFileName
bool useOgre
Use Ogre3d for visibility tests.
virtual double getAngleDisappear() const
virtual void setStopCriteriaEpsilon(const double eps)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
virtual void setCameraParameters(const vpCameraParameters &cam)
virtual void setAngleDisappear(const double &a)
virtual void setCovarianceComputation(const bool &flag)
virtual void setInitialMu(double mu)
virtual void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
virtual void setScanLineVisibilityTest(const bool &v)
virtual vpMbHiddenFaces< vpMbtPolygon > & getFaces()
std::vector< vpMbtDistanceCircle * > m_projectionErrorCircles
Distance circle primitive for projection error.
std::string poseSavingFilename
unsigned int nbPolygonLines
Number of polygon lines in CAO model.
virtual vpColVector getRobustWeights() const =0
unsigned int m_projectionErrorDisplayThickness
Thickness of the arrows used to show the gradient and model orientation.
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)=0
virtual vpMatrix getCovarianceMatrix() const
bool displayFeatures
If true, the features are displayed.
virtual void setProjectionErrorDisplay(bool display)
double angleDisappears
Angle used to detect a face disappearance.
virtual unsigned int getNbPolygon() const
virtual void setLambda(double gain)
bool applyLodSettingInConfig
virtual double getProjectionError() const
double distFarClip
Distance for near clipping.
bool m_isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
virtual vpMbtPolygon * getPolygon(unsigned int index)
virtual double getStopCriteriaEpsilon() const
bool useScanLine
Use Scanline for visibility tests.
vpMatrix m_SobelY
Sobel kernel in Y.
virtual void setProjectionErrorComputation(const bool &flag)
double angleAppears
Angle used to detect a face appearance.
virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt)
virtual void computeVVSInteractionMatrixAndResidu()=0
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)=0
bool m_projectionErrorOgreShowConfigDialog
virtual unsigned int getMaxIter() const
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
virtual double getLambda() const
virtual void setProjectionErrorDisplayArrowThickness(unsigned int thickness)
virtual void setAngleAppear(const double &a)
virtual void setGoodNbRayCastingAttemptsRatio(const double &ratio)
virtual void computeVVSInit()=0
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
std::vector< vpMbtDistanceLine * > m_projectionErrorLines
Distance line primitives for projection error.
double distNearClip
Distance for near clipping.
bool m_sodb_init_called
Flag that indicates that SoDB::init(); was called.
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
virtual void testTracking()=0
bool ogreShowConfigDialog
unsigned int nbCylinders
Number of cylinders in CAO model.
virtual void track(const vpImage< vpRGBa > &I)=0
unsigned int clippingFlag
Flags specifying which clipping to used.
unsigned int m_projectionErrorKernelSize
Kernel size used to compute the gradient orientation.
unsigned int nbCircles
Number of circles in CAO model.
vpMe m_projectionErrorMe
Moving-Edges parameters for projection error.
virtual double getFarClippingDistance() const
vpMbHiddenFaces< vpMbtPolygon > m_projectionErrorFaces
Set of faces describing the object, used for projection error.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)=0
virtual unsigned int getClipping() const
Implementation of a polygon of the model used by the model-based tracker.
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Implementation of a pose vector and operations on poses.
Contains an M-estimator and various influence function.
Class for generating random numbers with uniform probability density.