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;
120 std::string modelFileName;
123 bool modelInitialised;
126 std::string poseSavingFilename;
128 bool computeCovariance;
133 bool computeProjError;
136 double projectionError;
138 bool displayFeatures;
140 vpMbtOptimizationMethod m_optimizationMethod;
147 double angleDisappears;
153 unsigned int clippingFlag;
156 bool ogreShowConfigDialog;
160 unsigned int nbPoints;
162 unsigned int nbLines;
164 unsigned int nbPolygonLines;
166 unsigned int nbPolygonPoints;
168 unsigned int nbCylinders;
170 unsigned int nbCircles;
175 bool applyLodSettingInConfig;
177 double minLineLengthThresholdGeneral;
179 double minPolygonAreaThresholdGeneral;
182 std::map<std::string, std::string> mapOfParameterNames;
185 bool m_computeInteraction;
189 unsigned int m_maxIter;
191 double m_stopCriteriaEpsilon;
196 std::vector<vpMbtDistanceLine *> m_projectionErrorLines;
198 std::vector<vpMbtDistanceCylinder *> m_projectionErrorCylinders;
200 std::vector<vpMbtDistanceCircle *> m_projectionErrorCircles;
203 bool m_projectionErrorOgreShowConfigDialog;
205 vpMe m_projectionErrorMe;
207 unsigned int m_projectionErrorKernelSize;
213 bool m_projectionErrorDisplay;
215 unsigned int m_projectionErrorDisplayLength;
217 unsigned int m_projectionErrorDisplayThickness;
234 virtual inline double getAngleAppear()
const {
return angleAppears; }
237 virtual inline double getAngleDisappear()
const {
return angleDisappears; }
252 virtual inline unsigned int getClipping()
const {
return clippingFlag; }
261 virtual vpMatrix getCovarianceMatrix()
const
263 if (!computeCovariance) {
266 std::cerr <<
"Warning : The covariance matrix has not been computed. "
267 "See setCovarianceComputation() to do it."
271 return covarianceMatrix;
280 virtual inline double getInitialMu()
const {
return m_initialMu; }
287 virtual inline double getLambda()
const {
return m_lambda; }
294 virtual inline unsigned int getMaxIter()
const {
return m_maxIter; }
306 virtual double getProjectionError()
const {
return projectionError; }
335 virtual inline double getFarClippingDistance()
const {
return distFarClip; }
364 virtual inline unsigned int getNbPolygon()
const {
return static_cast<unsigned int>(faces.
size()); }
371 virtual inline double getNearClippingDistance()
const {
return distNearClip; }
380 virtual inline vpMbtOptimizationMethod getOptimizationMethod()
const {
return m_optimizationMethod; }
391 virtual inline vpMbtPolygon *getPolygon(
unsigned int index)
393 if (index >= static_cast<unsigned int>(faces.
size())) {
400 virtual std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
401 getPolygonFaces(
bool orderPolygons =
true,
bool useVisibility =
true,
bool clipPolygon =
false);
421 virtual inline double getStopCriteriaEpsilon()
const {
return m_stopCriteriaEpsilon; }
425 #ifdef VISP_HAVE_MODULE_GUI
426 virtual void initClick(
const vpImage<unsigned char> &I,
const std::string &initFile,
bool displayHelp =
false,
428 virtual void initClick(
const vpImage<vpRGBa> &I_color,
const std::string &initFile,
bool displayHelp =
false,
432 const std::string &displayFile =
"");
433 virtual void initClick(
const vpImage<vpRGBa> &I_color,
const std::vector<vpPoint> &points3D_list,
434 const std::string &displayFile =
"");
438 virtual void initFromPoints(
const vpImage<vpRGBa> &I_color,
const std::string &initFile);
440 virtual void initFromPoints(
const vpImage<unsigned char> &I,
const std::vector<vpImagePoint> &points2D_list,
441 const std::vector<vpPoint> &points3D_list);
442 virtual void initFromPoints(
const vpImage<vpRGBa> &I_color,
const std::vector<vpImagePoint> &points2D_list,
443 const std::vector<vpPoint> &points3D_list);
446 virtual void initFromPose(
const vpImage<vpRGBa> &I_color,
const std::string &initFile);
465 virtual inline void setAngleAppear(
const double &a) { angleAppears = a; }
476 virtual inline void setAngleDisappear(
const double &a) { angleDisappears = a; }
485 virtual void setClipping(
const unsigned int &flags);
495 virtual void setCovarianceComputation(
const bool &flag) { computeCovariance = flag; }
513 virtual void setDisplayFeatures(
bool displayF) { displayFeatures = displayF; }
515 virtual void setEstimatedDoF(
const vpColVector &v);
517 virtual void setFarClippingDistance(
const double &dist);
524 virtual inline void setInitialMu(
double mu) { m_initialMu = mu; }
531 virtual inline void setLambda(
double gain) { m_lambda = gain; }
533 virtual void setLod(
bool useLod,
const std::string &name =
"");
540 virtual inline void setMaxIter(
unsigned int max) { m_maxIter = max; }
542 virtual void setMinLineLengthThresh(
double minLineLengthThresh,
const std::string &name =
"");
544 virtual void setMinPolygonAreaThresh(
double minPolygonAreaThresh,
const std::string &name =
"");
546 virtual void setNearClippingDistance(
const double &dist);
553 virtual inline void setOptimizationMethod(
const vpMbtOptimizationMethod &opt) { m_optimizationMethod = opt; }
555 void setProjectionErrorMovingEdge(
const vpMe &me);
557 void setProjectionErrorKernelSize(
const unsigned int &size);
559 virtual void setMask(
const vpImage<bool> &mask) { m_mask = &mask; }
567 virtual inline void setStopCriteriaEpsilon(
const double eps) { m_stopCriteriaEpsilon = eps; }
580 virtual void setProjectionErrorComputation(
const bool &flag) { computeProjError = flag; }
585 virtual void setProjectionErrorDisplay(
bool display) { m_projectionErrorDisplay = display; }
590 virtual void setProjectionErrorDisplayArrowLength(
unsigned int length) { m_projectionErrorDisplayLength = length; }
595 virtual void setProjectionErrorDisplayArrowThickness(
unsigned int thickness) { m_projectionErrorDisplayThickness = thickness; }
597 virtual void setScanLineVisibilityTest(
const bool &v) { useScanLine = v; }
599 virtual void setOgreVisibilityTest(
const bool &v);
601 void savePose(
const std::string &filename)
const;
603 #ifdef VISP_HAVE_OGRE
623 virtual void setNbRayCastingAttemptsForVisibility(
const unsigned int &attempts)
639 inline virtual void setOgreShowConfigDialog(
bool showConfigDialog) { ogreShowConfigDialog = showConfigDialog; }
651 inline void setPoseSavingFilename(
const std::string &filename) { poseSavingFilename = filename; }
668 const vpColor &col,
unsigned int thickness = 1,
bool displayFullModel =
false) = 0;
682 const vpColor &col,
unsigned int thickness = 1,
bool displayFullModel =
false) = 0;
684 virtual std::vector<std::vector<double> > getModelForDisplay(
unsigned int width,
unsigned int height,
687 bool displayFullModel=
false)=0;
703 virtual void loadConfigFile(
const std::string &configFile);
708 virtual void resetTracker() = 0;
739 virtual void testTracking() = 0;
757 void addPolygon(
const std::vector<vpPoint> &corners,
int idFace = -1,
const std::string &polygonName =
"",
758 bool useLod =
false,
double minPolygonAreaThreshold = 2500.0,
759 double minLineLengthThreshold = 50.0);
760 void addPolygon(
const vpPoint &p1,
const vpPoint &p2,
const vpPoint &p3,
double radius,
int idFace = -1,
761 const std::string &polygonName =
"",
bool useLod =
false,
762 double minPolygonAreaThreshold = 2500.0);
763 void addPolygon(
const vpPoint &p1,
const vpPoint &p2,
int idFace = -1,
const std::string &polygonName =
"",
764 bool useLod =
false,
double minLineLengthThreshold = 50);
765 void addPolygon(
const std::vector<std::vector<vpPoint> > &listFaces,
int idFace = -1,
766 const std::string &polygonName =
"",
bool useLod =
false,
767 double minLineLengthThreshold = 50);
769 void addProjectionErrorCircle(
const vpPoint &P1,
const vpPoint &P2,
const vpPoint &P3,
double r,
int idFace = -1,
770 const std::string &name =
"");
771 void addProjectionErrorCylinder(
const vpPoint &P1,
const vpPoint &P2,
double r,
int idFace = -1,
const std::string &name =
"");
772 void addProjectionErrorLine(
vpPoint &p1,
vpPoint &p2,
int polygon = -1, std::string name =
"");
774 void addProjectionErrorPolygon(
const std::vector<vpPoint> &corners,
int idFace = -1,
const std::string &polygonName =
"",
775 bool useLod =
false,
double minPolygonAreaThreshold = 2500.0,
776 const double minLineLengthThreshold = 50.0);
777 void addProjectionErrorPolygon(
const vpPoint &p1,
const vpPoint &p2,
const vpPoint &p3,
double radius,
int idFace = -1,
778 const std::string &polygonName =
"",
bool useLod =
false,
779 double minPolygonAreaThreshold = 2500.0);
780 void addProjectionErrorPolygon(
const vpPoint &p1,
const vpPoint &p2,
int idFace = -1,
const std::string &polygonName =
"",
781 bool useLod =
false,
double minLineLengthThreshold = 50);
782 void addProjectionErrorPolygon(
const std::vector<std::vector<vpPoint> > &listFaces,
int idFace = -1,
783 const std::string &polygonName =
"",
bool useLod =
false,
784 double minLineLengthThreshold = 50);
786 void createCylinderBBox(
const vpPoint &p1,
const vpPoint &p2,
const double &radius,
787 std::vector<std::vector<vpPoint> > &listFaces);
789 virtual void computeCovarianceMatrixVVS(
const bool isoJoIdentity_,
const vpColVector &w_true,
798 virtual void computeVVSCheckLevenbergMarquardt(
unsigned int iter,
vpColVector &error,
800 double &mu,
bool &reStartFromLastIncrement,
802 virtual void computeVVSInit() = 0;
803 virtual void computeVVSInteractionMatrixAndResidu() = 0;
804 virtual void computeVVSPoseEstimation(
const bool isoJoIdentity_,
unsigned int iter,
vpMatrix &L,
vpMatrix <L,
810 #ifdef VISP_HAVE_COIN3D
811 virtual void extractGroup(SoVRMLGroup *sceneGraphVRML2,
vpHomogeneousMatrix &transform,
int &idFace);
812 virtual void extractFaces(SoVRMLIndexedFaceSet *face_set,
vpHomogeneousMatrix &transform,
int &idFace,
813 const std::string &polygonName =
"");
814 virtual void extractLines(SoVRMLIndexedLineSet *line_set,
int &idFace,
const std::string &polygonName =
"");
815 virtual void extractCylinders(SoVRMLIndexedFaceSet *face_set,
vpHomogeneousMatrix &transform,
int &idFace,
816 const std::string &polygonName =
"");
819 vpPoint getGravityCenter(
const std::vector<vpPoint> &_pts)
const;
835 int idFace = 0,
const std::string &name =
"") = 0;
837 #ifdef VISP_HAVE_MODULE_GUI
842 const std::vector<vpPoint> &points3D_list,
const std::string &displayFile =
"");
846 const std::string &initFile);
849 const std::vector<vpImagePoint> &points2D_list,
const std::vector<vpPoint> &points3D_list);
852 const std::string &initFile);
864 virtual void initCylinder(
const vpPoint &p1,
const vpPoint &p2,
double radius,
int idFace = 0,
865 const std::string &name =
"") = 0;
879 virtual void initFaceFromCorners(
vpMbtPolygon &polygon) = 0;
880 virtual void initFaceFromLines(
vpMbtPolygon &polygon) = 0;
882 void initProjectionErrorCircle(
const vpPoint &p1,
const vpPoint &p2,
const vpPoint &p3,
double radius,
883 int idFace = 0,
const std::string &name =
"");
884 void initProjectionErrorCylinder(
const vpPoint &p1,
const vpPoint &p2,
double radius,
int idFace = 0,
885 const std::string &name =
"");
886 void initProjectionErrorFaceFromCorners(
vpMbtPolygon &polygon);
887 void initProjectionErrorFaceFromLines(
vpMbtPolygon &polygon);
889 virtual void loadVRMLModel(
const std::string &modelFile);
890 virtual void loadCAOModel(
const std::string &modelFile, std::vector<std::string> &vectorOfModelFilename,
891 int &startIdFace,
bool verbose =
false,
bool parent =
true,
895 void projectionErrorResetMovingEdges();
896 void projectionErrorVisibleFace(
unsigned int width,
unsigned int height,
const vpHomogeneousMatrix &_cMo);
898 void removeComment(std::ifstream &fileId);
900 std::map<std::string, std::string> parseParameters(std::string &endLine);