45 #ifndef vpMbTracker_hh
46 #define vpMbTracker_hh
58 #include <visp3/core/vpCameraParameters.h>
59 #include <visp3/core/vpColVector.h>
60 #include <visp3/core/vpHomogeneousMatrix.h>
61 #include <visp3/core/vpImage.h>
62 #include <visp3/core/vpImagePoint.h>
63 #include <visp3/core/vpMatrix.h>
64 #include <visp3/core/vpPoint.h>
65 #include <visp3/core/vpPolygon.h>
66 #include <visp3/core/vpRGBa.h>
67 #include <visp3/core/vpRobust.h>
68 #include <visp3/mbt/vpMbHiddenFaces.h>
69 #include <visp3/mbt/vpMbtPolygon.h>
71 #include <visp3/mbt/vpMbtDistanceCircle.h>
72 #include <visp3/mbt/vpMbtDistanceCylinder.h>
73 #include <visp3/mbt/vpMbtDistanceLine.h>
75 #ifdef VISP_HAVE_COIN3D
83 #include <Inventor/VRMLnodes/SoVRMLGroup.h>
84 #include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
85 #include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
113 typedef enum { GAUSS_NEWTON_OPT = 0, LEVENBERG_MARQUARDT_OPT = 1 } vpMbtOptimizationMethod;
126 std::string modelFileName;
129 bool modelInitialised;
132 std::string poseSavingFilename;
134 bool computeCovariance;
139 bool computeProjError;
142 double projectionError;
144 bool displayFeatures;
146 vpMbtOptimizationMethod m_optimizationMethod;
153 double angleDisappears;
159 unsigned int clippingFlag;
162 bool ogreShowConfigDialog;
166 unsigned int nbPoints;
168 unsigned int nbLines;
170 unsigned int nbPolygonLines;
172 unsigned int nbPolygonPoints;
174 unsigned int nbCylinders;
176 unsigned int nbCircles;
181 bool applyLodSettingInConfig;
183 double minLineLengthThresholdGeneral;
185 double minPolygonAreaThresholdGeneral;
188 std::map<std::string, std::string> mapOfParameterNames;
191 bool m_computeInteraction;
195 unsigned int m_maxIter;
197 double m_stopCriteriaEpsilon;
202 std::vector<vpMbtDistanceLine *> m_projectionErrorLines;
204 std::vector<vpMbtDistanceCylinder *> m_projectionErrorCylinders;
206 std::vector<vpMbtDistanceCircle *> m_projectionErrorCircles;
209 bool m_projectionErrorOgreShowConfigDialog;
211 vpMe m_projectionErrorMe;
213 unsigned int m_projectionErrorKernelSize;
219 bool m_projectionErrorDisplay;
221 unsigned int m_projectionErrorDisplayLength;
223 unsigned int m_projectionErrorDisplayThickness;
238 virtual inline double getAngleAppear()
const {
return angleAppears; }
241 virtual inline double getAngleDisappear()
const {
return angleDisappears; }
248 virtual void getCameraParameters(
vpCameraParameters &camera)
const { camera = this->cam; }
256 virtual inline unsigned int getClipping()
const {
return clippingFlag; }
265 virtual vpMatrix getCovarianceMatrix()
const
267 if (!computeCovariance) {
270 std::cerr <<
"Warning : The covariance matrix has not been computed. "
271 "See setCovarianceComputation() to do it."
275 return covarianceMatrix;
284 virtual inline double getInitialMu()
const {
return m_initialMu; }
291 virtual inline double getLambda()
const {
return m_lambda; }
298 virtual inline unsigned int getMaxIter()
const {
return m_maxIter; }
310 virtual double getProjectionError()
const {
return projectionError; }
339 virtual inline double getFarClippingDistance()
const {
return distFarClip; }
368 virtual inline unsigned int getNbPolygon()
const {
return static_cast<unsigned int>(faces.
size()); }
375 virtual inline double getNearClippingDistance()
const {
return distNearClip; }
384 virtual inline vpMbtOptimizationMethod getOptimizationMethod()
const {
return m_optimizationMethod; }
395 virtual inline vpMbtPolygon *getPolygon(
const unsigned int index)
397 if (index >= static_cast<unsigned int>(faces.
size())) {
404 virtual std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
405 getPolygonFaces(
const bool orderPolygons =
true,
const bool useVisibility =
true,
const bool clipPolygon =
false);
425 virtual inline double getStopCriteriaEpsilon()
const {
return m_stopCriteriaEpsilon; }
429 #ifdef VISP_HAVE_MODULE_GUI
430 virtual void initClick(
const vpImage<unsigned char> &I,
const std::string &initFile,
const bool displayHelp =
false,
433 const std::string &displayFile =
"");
437 virtual void initFromPoints(
const vpImage<unsigned char> &I,
const std::vector<vpImagePoint> &points2D_list,
438 const std::vector<vpPoint> &points3D_list);
455 virtual inline void setAngleAppear(
const double &a) { angleAppears = a; }
466 virtual inline void setAngleDisappear(
const double &a) { angleDisappears = a; }
473 virtual void setCameraParameters(
const vpCameraParameters &camera) { this->cam = camera; }
475 virtual void setClipping(
const unsigned int &flags);
485 virtual void setCovarianceComputation(
const bool &flag) { computeCovariance = flag; }
503 virtual void setDisplayFeatures(
const bool displayF) { displayFeatures = displayF; }
505 virtual void setEstimatedDoF(
const vpColVector &v);
507 virtual void setFarClippingDistance(
const double &dist);
514 virtual inline void setInitialMu(
const double mu) { m_initialMu = mu; }
521 virtual inline void setLambda(
const double gain) { m_lambda = gain; }
523 virtual void setLod(
const bool useLod,
const std::string &name =
"");
530 virtual inline void setMaxIter(
const unsigned int max) { m_maxIter = max; }
532 virtual void setMinLineLengthThresh(
const double minLineLengthThresh,
const std::string &name =
"");
534 virtual void setMinPolygonAreaThresh(
const double minPolygonAreaThresh,
const std::string &name =
"");
536 virtual void setNearClippingDistance(
const double &dist);
543 virtual inline void setOptimizationMethod(
const vpMbtOptimizationMethod &opt) { m_optimizationMethod = opt; }
545 void setProjectionErrorMovingEdge(
const vpMe &me);
547 void setProjectionErrorKernelSize(
const unsigned int &size);
549 virtual void setMask(
const vpImage<bool> &mask) { m_mask = &mask; }
557 virtual inline void setStopCriteriaEpsilon(
const double eps) { m_stopCriteriaEpsilon = eps; }
570 virtual void setProjectionErrorComputation(
const bool &flag) { computeProjError = flag; }
575 virtual void setProjectionErrorDisplay(
const bool display) { m_projectionErrorDisplay = display; }
580 virtual void setProjectionErrorDisplayArrowLength(
const unsigned int length) { m_projectionErrorDisplayLength = length; }
585 virtual void setProjectionErrorDisplayArrowThickness(
const unsigned int thickness) { m_projectionErrorDisplayThickness = thickness; }
587 virtual void setScanLineVisibilityTest(
const bool &v) { useScanLine = v; }
589 virtual void setOgreVisibilityTest(
const bool &v);
591 void savePose(
const std::string &filename)
const;
593 #ifdef VISP_HAVE_OGRE
613 virtual void setNbRayCastingAttemptsForVisibility(
const unsigned int &attempts)
629 inline virtual void setOgreShowConfigDialog(
const bool showConfigDialog) { ogreShowConfigDialog = showConfigDialog; }
641 inline void setPoseSavingFilename(
const std::string &filename) { poseSavingFilename = filename; }
658 const vpColor &col,
const unsigned int thickness = 1,
const bool displayFullModel =
false) = 0;
672 const vpColor &col,
const unsigned int thickness = 1,
const bool displayFullModel =
false) = 0;
688 virtual void loadConfigFile(
const std::string &configFile);
693 virtual void resetTracker() = 0;
712 virtual void testTracking() = 0;
723 void addPolygon(
const std::vector<vpPoint> &corners,
const int idFace = -1,
const std::string &polygonName =
"",
724 const bool useLod =
false,
const double minPolygonAreaThreshold = 2500.0,
725 const double minLineLengthThreshold = 50.0);
726 void addPolygon(
const vpPoint &p1,
const vpPoint &p2,
const vpPoint &p3,
const double radius,
const int idFace = -1,
727 const std::string &polygonName =
"",
const bool useLod =
false,
728 const double minPolygonAreaThreshold = 2500.0);
729 void addPolygon(
const vpPoint &p1,
const vpPoint &p2,
const int idFace = -1,
const std::string &polygonName =
"",
730 const bool useLod =
false,
const double minLineLengthThreshold = 50);
731 void addPolygon(
const std::vector<std::vector<vpPoint> > &listFaces,
const int idFace = -1,
732 const std::string &polygonName =
"",
const bool useLod =
false,
733 const double minLineLengthThreshold = 50);
735 void addProjectionErrorCircle(
const vpPoint &P1,
const vpPoint &P2,
const vpPoint &P3,
const double r,
int idFace = -1,
736 const std::string &name =
"");
737 void addProjectionErrorCylinder(
const vpPoint &P1,
const vpPoint &P2,
const double r,
int idFace = -1,
const std::string &name =
"");
738 void addProjectionErrorLine(
vpPoint &p1,
vpPoint &p2,
int polygon = -1, std::string name =
"");
740 void addProjectionErrorPolygon(
const std::vector<vpPoint> &corners,
const int idFace = -1,
const std::string &polygonName =
"",
741 const bool useLod =
false,
const double minPolygonAreaThreshold = 2500.0,
742 const double minLineLengthThreshold = 50.0);
743 void addProjectionErrorPolygon(
const vpPoint &p1,
const vpPoint &p2,
const vpPoint &p3,
const double radius,
const int idFace = -1,
744 const std::string &polygonName =
"",
const bool useLod =
false,
745 const double minPolygonAreaThreshold = 2500.0);
746 void addProjectionErrorPolygon(
const vpPoint &p1,
const vpPoint &p2,
const int idFace = -1,
const std::string &polygonName =
"",
747 const bool useLod =
false,
const double minLineLengthThreshold = 50);
748 void addProjectionErrorPolygon(
const std::vector<std::vector<vpPoint> > &listFaces,
const int idFace = -1,
749 const std::string &polygonName =
"",
const bool useLod =
false,
750 const double minLineLengthThreshold = 50);
752 void createCylinderBBox(
const vpPoint &p1,
const vpPoint &p2,
const double &radius,
753 std::vector<std::vector<vpPoint> > &listFaces);
755 virtual void computeCovarianceMatrixVVS(
const bool isoJoIdentity_,
const vpColVector &w_true,
764 virtual void computeVVSCheckLevenbergMarquardt(
const unsigned int iter,
vpColVector &error,
766 double &mu,
bool &reStartFromLastIncrement,
768 virtual void computeVVSInit() = 0;
769 virtual void computeVVSInteractionMatrixAndResidu() = 0;
770 virtual void computeVVSPoseEstimation(
const bool isoJoIdentity_,
const unsigned int iter,
vpMatrix &L,
vpMatrix <L,
776 #ifdef VISP_HAVE_COIN3D
777 virtual void extractGroup(SoVRMLGroup *sceneGraphVRML2,
vpHomogeneousMatrix &transform,
int &idFace);
778 virtual void extractFaces(SoVRMLIndexedFaceSet *face_set,
vpHomogeneousMatrix &transform,
int &idFace,
779 const std::string &polygonName =
"");
780 virtual void extractLines(SoVRMLIndexedLineSet *line_set,
int &idFace,
const std::string &polygonName =
"");
781 virtual void extractCylinders(SoVRMLIndexedFaceSet *face_set,
vpHomogeneousMatrix &transform,
int &idFace,
782 const std::string &polygonName =
"");
785 vpPoint getGravityCenter(
const std::vector<vpPoint> &_pts)
const;
799 virtual void initCircle(
const vpPoint &p1,
const vpPoint &p2,
const vpPoint &p3,
const double radius,
800 const int idFace = 0,
const std::string &name =
"") = 0;
811 virtual void initCylinder(
const vpPoint &p1,
const vpPoint &p2,
const double radius,
const int idFace = 0,
812 const std::string &name =
"") = 0;
826 virtual void initFaceFromCorners(
vpMbtPolygon &polygon) = 0;
827 virtual void initFaceFromLines(
vpMbtPolygon &polygon) = 0;
829 void initProjectionErrorCircle(
const vpPoint &p1,
const vpPoint &p2,
const vpPoint &p3,
const double radius,
830 const int idFace = 0,
const std::string &name =
"");
831 void initProjectionErrorCylinder(
const vpPoint &p1,
const vpPoint &p2,
const double radius,
const int idFace = 0,
832 const std::string &name =
"");
833 void initProjectionErrorFaceFromCorners(
vpMbtPolygon &polygon);
834 void initProjectionErrorFaceFromLines(
vpMbtPolygon &polygon);
836 virtual void loadVRMLModel(
const std::string &modelFile);
837 virtual void loadCAOModel(
const std::string &modelFile, std::vector<std::string> &vectorOfModelFilename,
838 int &startIdFace,
const bool verbose =
false,
const bool parent =
true,
842 void projectionErrorResetMovingEdges();
845 void removeComment(std::ifstream &fileId);
847 inline bool parseBoolean(std::string &input)
849 std::transform(input.begin(), input.end(), input.begin(), ::tolower);
850 std::istringstream is(input);
854 is >> (input.size() > 1 ? std::boolalpha : std::noboolalpha) >> b;
858 std::map<std::string, std::string> parseParameters(std::string &endLine);
860 inline std::string <rim(std::string &s)
const
862 s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun<int, int>(std::isspace))));
866 inline std::string &rtrim(std::string &s)
const
868 s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun<int, int>(std::isspace))).base(), s.end());
872 inline std::string &trim(std::string &s)
const {
return ltrim(rtrim(s)); }