40 #include <visp3/core/vpImageConvert.h> 41 #include <visp3/core/vpTrackingException.h> 42 #include <visp3/core/vpVelocityTwistMatrix.h> 43 #include <visp3/mbt/vpMbKltTracker.h> 45 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 47 #if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin) 48 #include <TargetConditionals.h> 53 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
58 c0Mo(), firstInitialisation(true), maskBorder(5), threshold_outlier(0.5), percentGood(0.6), ctTc0(), tracker(),
59 kltPolygons(), kltCylinders(), circles_disp(), m_nbInfos(0), m_nbFaceUsed(0), m_L_klt(), m_error_klt(), m_w_klt(),
60 m_weightedError_klt(), m_robust_klt()
89 #if (VISP_HAVE_OPENCV_VERSION < 0x020408) 97 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it =
kltPolygons.begin(); it !=
kltPolygons.end(); ++it) {
99 if (kltpoly != NULL) {
109 if (kltPolyCylinder != NULL) {
110 delete kltPolyCylinder;
112 kltPolyCylinder = NULL;
134 bool reInitialisation =
false;
138 #ifdef VISP_HAVE_OGRE 173 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408) 174 cv::Mat mask((
int)I.
getRows(), (int)I.
getCols(), CV_8UC1, cv::Scalar(0));
176 IplImage *mask = cvCreateImage(cvSize((
int)I.
getWidth(), (int)I.
getHeight()), IPL_DEPTH_8U, 1);
185 unsigned char val = 255 ;
186 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it =
kltPolygons.begin(); it !=
kltPolygons.end(); ++it) {
199 kltPolyCylinder = *it;
204 if (
faces[indCylBBox]->isVisible() &&
faces[indCylBBox]->getNbPoint() > 2u) {
205 faces[indCylBBox]->computePolygonClipped(
cam);
220 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it =
kltPolygons.begin(); it !=
kltPolygons.end(); ++it) {
229 kltPolyCylinder = *it;
235 #if (VISP_HAVE_OPENCV_VERSION < 0x020408) 236 cvReleaseImage(&mask);
248 #if (VISP_HAVE_OPENCV_VERSION < 0x020408) 250 cvReleaseImage(&
cur);
256 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it =
kltPolygons.begin(); it !=
kltPolygons.end(); ++it) {
258 if (kltpoly != NULL) {
268 if (kltPolyCylinder != NULL) {
269 delete kltPolyCylinder;
271 kltPolyCylinder = NULL;
319 #ifdef VISP_HAVE_OGRE 334 std::vector<vpImagePoint> kltPoints;
355 std::map<int, vpImagePoint> kltPoints;
397 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it =
kltPolygons.begin(); it !=
kltPolygons.end(); ++it) {
423 std::cout <<
"WARNING: Cannot set pose when model contains cylinder(s). " 424 "This feature is not implemented yet." 426 std::cout <<
"Tracker will be reinitialized with the given pose." << std::endl;
432 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408) 433 std::vector<cv::Point2f> init_pts;
434 std::vector<long> init_ids;
435 std::vector<cv::Point2f> guess_pts;
437 unsigned int nbp = 0;
438 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it =
kltPolygons.begin(); it !=
kltPolygons.end(); ++it) {
444 CvPoint2D32f *init_pts = NULL;
447 unsigned int iter_pts = 0;
449 CvPoint2D32f *guess_pts = NULL;
464 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it =
kltPolygons.begin(); it !=
kltPolygons.end(); ++it) {
477 double invDc = 1.0 / plan.
getD();
481 vpGEMM(cdtc, Nc, -invDc, cdRc, 1.0, cdHc, VP_GEMM_B_T);
488 std::map<int, vpImagePoint>::const_iterator iter = kltpoly->
getCurrentPoints().begin();
491 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408) 493 if (std::find(init_ids.begin(), init_ids.end(), (long)(kltpoly->
getCurrentPointsInd())[(
int)iter->first]) !=
496 if (std::find(init_ids.begin(), init_ids.end(),
507 cdp[0] = iter->second.get_j();
508 cdp[1] = iter->second.get_i();
511 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408) 512 cv::Point2f p((
float)cdp[0], (
float)cdp[1]);
513 init_pts.push_back(p);
520 init_pts[iter_pts].x = (float)cdp[0];
521 init_pts[iter_pts].y = (float)cdp[1];
525 double p_mu_t_2 = cdp[0] * cdGc[2][0] + cdp[1] * cdGc[2][1] + cdGc[2][2];
527 if (fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()) {
533 cdp[0] = (cdp[0] * cdGc[0][0] + cdp[1] * cdGc[0][1] + cdGc[0][2]) / p_mu_t_2;
534 cdp[1] = (cdp[0] * cdGc[1][0] + cdp[1] * cdGc[1][1] + cdGc[1][2]) / p_mu_t_2;
537 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408) 538 cv::Point2f p_guess((
float)cdp[0], (
float)cdp[1]);
539 guess_pts.push_back(p_guess);
541 guess_pts[iter_pts].x = (float)cdp[0];
542 guess_pts[iter_pts++].y = (float)cdp[1];
550 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408) 568 bool reInitialisation =
false;
572 #ifdef VISP_HAVE_OGRE 586 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it =
kltPolygons.begin(); it !=
kltPolygons.end(); ++it) {
645 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it =
kltPolygons.begin(); it !=
kltPolygons.end(); ++it) {
679 bool reInitialisation =
false;
681 unsigned int initialNumber = 0;
682 unsigned int currentNumber = 0;
683 unsigned int shift = 0;
685 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it =
kltPolygons.begin(); it !=
kltPolygons.end(); ++it) {
720 double value =
percentGood * (double)initialNumber;
721 if ((
double)currentNumber < value) {
724 reInitialisation =
true;
729 #ifdef VISP_HAVE_OGRE 738 if (reInitialisation)
761 double normRes_1 = -1;
762 unsigned int iter = 0;
766 while (((
int)((normRes - normRes_1) * 1e8) != 0) && (iter <
m_maxIter)) {
769 bool reStartFromLastIncrement =
false;
771 if (reStartFromLastIncrement) {
775 if (!reStartFromLastIncrement) {
797 for (
unsigned int j = 0; j < 6; j++) {
835 unsigned int shift = 0;
838 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it =
kltPolygons.begin(); it !=
kltPolygons.end(); ++it) {
948 #ifdef VISP_HAVE_XML2 963 std::cout <<
" *********** Parsing XML for MBT KLT Tracker ************ " << std::endl;
964 xmlp.
parse(configFile.c_str());
966 vpERROR_TRACE(
"Can't open XML file \"%s\"\n ", configFile.c_str());
1010 vpTRACE(
"You need the libXML2 to read the config file %s", configFile.c_str());
1027 const bool displayFullModel)
1049 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it =
kltPolygons.begin(); it !=
kltPolygons.end(); ++it) {
1052 kltpoly->
display(I, cMo_, camera, col, thickness, displayFullModel);
1064 kltPolyCylinder->
display(I, cMo_, camera, col, thickness, displayFullModel);
1071 (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1074 #ifdef VISP_HAVE_OGRE 1093 const bool displayFullModel)
1115 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it =
kltPolygons.begin(); it !=
kltPolygons.end(); ++it) {
1118 kltpoly->
display(I, cMo_, camera, col, thickness, displayFullModel);
1130 kltPolyCylinder->
display(I, cMo_, camera, col, thickness, displayFullModel);
1137 (*it)->display(I, cMo_, camera, col, thickness);
1140 #ifdef VISP_HAVE_OGRE 1156 unsigned int nbTotalPoints = 0;
1158 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it =
kltPolygons.begin(); it !=
kltPolygons.end(); ++it) {
1173 if (nbTotalPoints < 10) {
1174 std::cerr <<
"test tracking failed (too few points to realize a good tracking)." << std::endl;
1176 "test tracking failed (too few points to realize a good tracking).");
1191 const std::string & )
1221 const int ,
const std::string &name)
1235 const std::string &name)
1237 bool already_here =
false;
1252 if (!already_here) {
1280 #if (VISP_HAVE_OPENCV_VERSION < 0x020408) 1282 cvReleaseImage(&
cur);
1290 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it =
kltPolygons.begin(); it !=
kltPolygons.end(); ++it) {
1292 if (kltpoly != NULL) {
1302 if (kltPolyCylinder != NULL) {
1303 delete kltPolyCylinder;
1305 kltPolyCylinder = NULL;
1333 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it =
kltPolygons.begin(); it !=
kltPolygons.end(); ++it) {
1341 #elif !defined(VISP_BUILD_SHARED_LIBS) 1344 void dummy_vpMbKltTracker(){};
1345 #endif // VISP_HAVE_OPENCV virtual void setKltOpencv(const vpKltOpencv &t)
void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, const std::string &name="")
bool m_computeInteraction
virtual void track(const vpImage< unsigned char > &I)
unsigned int getCols() const
vpMatrix get_K_inverse() const
void setWindowName(const Ogre::String &n)
int getPyramidLevels() const
Get the list of features id.
unsigned int getMaskBorder() const
void setQuality(const double &q)
Implementation of a matrix and operations on matrices.
void displayPrimitive(const vpImage< unsigned char > &_I)
void setTracked(const bool &track)
virtual ~vpMbKltTracker()
double getHarrisParam() const
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker)
void setCameraParameters(const vpCameraParameters &camera)
void removeOutliers(const vpColVector &weight, const double &threshold_outlier)
void setUseKltTracking(const std::string &name, const bool &useKltTracking)
void setHarrisFreeParameter(double harris_k)
int getMaxFeatures() const
Get the list of lost feature.
unsigned int getWidth() const
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void getCameraParameters(vpCameraParameters &_cam) const
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
void removeOutliers(const vpColVector &weight, const double &threshold_outlier)
Implementation of an homogeneous matrix and operations on such kind of matrices.
std::vector< int > listIndicesCylinderBBox
Pointer to the polygon that define a face.
double getHarrisFreeParameter() const
Get the free parameter of the Harris detector.
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void loadConfigFile(const std::string &configFile)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
Class to define colors available for display functionnalities.
void setMaxFeatures(const int maxCount)
std::list< vpMbtDistanceKltCylinder * > kltCylinders
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
virtual void initCylinder(const vpPoint &, const vpPoint &, const double, const int, const std::string &name="")
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
vpHomogeneousMatrix cMo
The current pose.
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
unsigned int getBlockSize() const
bool hasEnoughPoints() const
double getNearClippingDistance() const
void setOgreShowConfigDialog(const bool showConfigDialog)
void setMinDistance(double minDistance)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r)
error that can be emited by ViSP classes.
vpMbScanLine & getMbScanLineRenderer()
vpColVector m_weightedError_klt
Weighted error.
vpPoint * p
corners in the object frame
unsigned int getMaxFeatures() const
void init(const vpKltOpencv &_tracker, const vpHomogeneousMatrix &cMo)
bool useOgre
Use Ogre3d for visibility tests.
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
Definition of the vpSubMatrix vpSubMatrix class provides a mask on a vpMatrix all properties of vpMat...
unsigned int setVisibleOgre(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
virtual void reinit(const vpImage< unsigned char > &I)
virtual void computeVVSInteractionMatrixAndResidu()
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
virtual void testTracking()
bool hasNearClippingDistance() const
Class that defines what is a point.
virtual void loadConfigFile(const std::string &configFile)
unsigned int getInitialNumberPoint() const
vpCameraParameters cam
The camera parameters.
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int setVisible(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
Implementation of a rotation matrix and operations on such kind of matrices.
void setQuality(double qualityLevel)
vpHomogeneousMatrix ctTc0
std::map< int, int > & getCurrentPointsInd()
double getFarClippingDistance() const
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
bool hasFarClippingDistance() const
Implementation of an homography and operations on homographies.
vpAROgre * getOgreContext()
void changeFrame(const vpHomogeneousMatrix &cMo)
unsigned int getRows() const
std::vector< vpImagePoint > getKltImagePoints() const
void setPyramidLevels(const unsigned int &pL)
vpRobust m_robust_klt
Robust.
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
vpColVector & normalize()
Parse an Xml file to extract configuration parameters of a Mbt Klt object.Data parser for the KLT mod...
Manage a circle used in the model-based tracker.
vpMatrix oJo
The Degrees of Freedom to estimate.
unsigned int getWindowSize() const
Error that can be emited by the vpTracker class and its derivates.
double getAngleDisappear() const
Implementation of a polygon of the model used by the model-based tracker.
virtual void setCameraParameters(const vpCameraParameters &_cam)
void computePolygonClipped(const vpCameraParameters &cam=vpCameraParameters())
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMc0, vpColVector &_R, vpMatrix &_J)
void setName(const std::string &circle_name)
std::string getName() const
double getQuality() const
void changeFrame(const vpHomogeneousMatrix &cMo)
void displayPrimitive(const vpImage< unsigned char > &_I)
void setWindowSize(const unsigned int &w)
bool useScanLine
Use Scanline for visibility tests.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
void setAngleDisappear(const double &adisappear)
double getMinPolygonAreaThreshold() const
void setTrackerId(int tid)
unsigned int maskBorder
Erosion of the mask.
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
Generic class defining intrinsic camera parameters.
int getBlockSize() const
Get the size of the averaging block used to track the features.
unsigned int m_nbFaceUsed
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
std::map< int, vpImagePoint > getKltImagePointsWithId() const
unsigned int getInitialNumberPoint() const
void extract(vpRotationMatrix &R) const
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
double m_lambda
Gain of the virtual visual servoing stage.
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
void setHarrisParam(const double &hp)
bool hasEnoughPoints() const
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
unsigned int getRows() const
void setMinDistance(const double &mD)
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
double angleAppears
Angle used to detect a face appearance.
vpKltOpencv tracker
Points tracker.
void setInitialGuess(const std::vector< cv::Point2f > &guess_pts)
virtual bool isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), const vpImage< unsigned char > &I=vpImage< unsigned char >())
vpColVector getNormal() const
void setPyramidLevels(const int pyrMaxLevel)
bool useScanLine
Use scanline rendering.
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
void init(const vpKltOpencv &_tracker, const vpImage< bool > *mask=NULL)
std::map< int, vpImagePoint > & getCurrentPoints()
static double rad(double deg)
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="")
void buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
cv::Mat cur
Temporary OpenCV image for fast conversion.
double getMinLineLengthThreshold() const
void setMaxFeatures(const unsigned int &mF)
void computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &cHc0)
vpColVector m_w_klt
Robust weights.
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
std::list< vpMbtDistanceCircle * > circles_disp
Vector of the circles used here only to display the full model.
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, const double, const int, const std::string &name="")
void setWindowSize(const int winSize)
double getQuality() const
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
static double deg(double rad)
virtual void setCameraParameters(const vpCameraParameters &_cam)
bool displayFeatures
If true, the features are displayed.
void setCameraParameters(const vpCameraParameters &cam)
bool ogreShowConfigDialog
void preTracking(const vpImage< unsigned char > &I)
bool applyLodSettingInConfig
Implementation of column vector and the associated operations.
void setAngleAppear(const double &aappear)
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, const unsigned int iter, vpMatrix &L, vpMatrix <L, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector <R, double &mu, vpColVector &v, const vpColVector *const w=NULL, vpColVector *const m_w_prev=NULL)
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
vpHomogeneousMatrix inverse() const
void setBlockSize(const int blockSize)
static vpHomogeneousMatrix direct(const vpColVector &v)
void getFeature(const int &index, long &id, float &x, float &y) const
int getNbFeatures() const
Get the number of current features.
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
double angleDisappears
Angle used to detect a face disappearance.
virtual unsigned int getNbPolygon() const
virtual void initFaceFromLines(vpMbtPolygon &polygon)
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
virtual void computeVVSInit()
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
unsigned int getHeight() const
unsigned int getNbPoint() const
virtual void setClipping(const unsigned int &flags)
virtual void init(const vpImage< unsigned char > &I)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void setUseHarris(const int useHarrisDetector)
unsigned int getCurrentNumberPoints() const
void setBlockSize(const unsigned int &bs)
This class defines the container for a plane geometrical structure.
unsigned int clippingFlag
Flags specifying which clipping to used.
double getMinDistance() const
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker, const vpImage< bool > *mask=NULL)
void displayOgre(const vpHomogeneousMatrix &cMo)
double getAngleAppear() const
unsigned int getCurrentNumberPoints() const
vpColVector m_error_klt
(s - s*)
void setThreshold(const double noise_threshold)
void updateMask(cv::Mat &mask, unsigned char _nb=255, unsigned int _shiftBorder=0)
virtual void setFarClippingDistance(const double &dist)
double getMinDistance() const
void resize(unsigned int n_data)
Resize containers for sort methods.
bool getFovClipping() const
bool useLodGeneral
True if LOD mode is enabled.
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
Class that consider the case of a translation vector.
virtual void computeVVSCheckLevenbergMarquardt(const unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
std::list< vpMbtDistanceKltPoints * > kltPolygons
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
void setMaskBorder(const unsigned int &mb)
void computeInteractionMatrixAndResidu(vpColVector &_R, vpMatrix &_J)
vpHomogeneousMatrix c0Mo
Initial pose.
int getWindowSize() const
Get the window size used to refine the corner locations.
void track(const cv::Mat &I)
unsigned int getPyramidLevels() const
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
bool useScanLine
Use scanline rendering.
void updateMask(cv::Mat &mask, unsigned char _nb=255, unsigned int _shiftBorder=0)
void resize(const unsigned int i, const bool flagNullify=true)
virtual void setLod(const bool useLod, const std::string &name="")
virtual void setNearClippingDistance(const double &dist)
void parse(const std::string &filename)
vpMatrix m_L_klt
Interaction matrix.
void computeFov(const unsigned int &w, const unsigned int &h)