 |
Visual Servoing Platform
version 3.2.0
|
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) {
472 plan.changeFrame(cMcd);
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 setCameraParameters(const vpCameraParameters &_cam)
void preTracking(const vpImage< unsigned char > &I)
int getBlockSize() const
Get the size of the averaging block used to track the features.
void setOgreShowConfigDialog(const bool showConfigDialog)
Error that can be emited by the vpTracker class and its derivates.
bool hasEnoughPoints() const
virtual void loadConfigFile(const std::string &configFile)
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void resize(const unsigned int i, const bool flagNullify=true)
virtual void setLod(const bool useLod, const std::string &name="")
void parse(const std::string &filename)
bool m_computeInteraction
void setTracked(const bool &track)
vpColVector m_w_klt
Robust weights.
void setMinDistance(double minDistance)
vpColVector m_error_klt
(s - s*)
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)
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker)
int getNbFeatures() const
Get the number of current features.
unsigned int setVisibleOgre(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
void displayOgre(const vpHomogeneousMatrix &cMo)
unsigned int getWindowSize() const
void setThreshold(const double noise_threshold)
void track(const cv::Mat &I)
std::vector< vpImagePoint > getKltImagePoints() const
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
void setMaxFeatures(const int maxCount)
bool useOgre
Use Ogre3d for visibility tests.
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static double rad(double deg)
vpHomogeneousMatrix cMo
The current pose.
double getMinPolygonAreaThreshold() const
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
unsigned int getMaxFeatures() const
Generic class defining intrinsic camera parameters.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void init(const vpKltOpencv &_tracker, const vpHomogeneousMatrix &cMo)
void displayPrimitive(const vpImage< unsigned char > &_I)
void removeOutliers(const vpColVector &weight, const double &threshold_outlier)
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
vpMatrix m_L_klt
Interaction matrix.
bool hasEnoughPoints() const
unsigned int m_nbFaceUsed
double getFarClippingDistance() const
static double deg(double rad)
unsigned int getBlockSize() const
void getCameraParameters(vpCameraParameters &_cam) const
void setQuality(double qualityLevel)
unsigned int getCols() const
int getWindowSize() const
Get the window size used to refine the corner locations.
void setHarrisFreeParameter(double harris_k)
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
bool hasFarClippingDistance() const
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
vpColVector & normalize()
vpKltOpencv tracker
Points tracker.
void changeFrame(const vpHomogeneousMatrix &cMo)
int getMaxFeatures() const
Get the list of lost feature.
void computeInteractionMatrixAndResidu(vpColVector &_R, vpMatrix &_J)
virtual void computeVVSInit()
vpMbScanLine & getMbScanLineRenderer()
Manage a circle used in the model-based tracker.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
void setAngleAppear(const double &aappear)
void setUseKltTracking(const std::string &name, const bool &useKltTracking)
bool getFovClipping() const
unsigned int getCurrentNumberPoints() const
Class that consider the case of a translation vector.
void setCameraParameters(const vpCameraParameters &cam)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r)
std::string getName() const
void displayPrimitive(const vpImage< unsigned char > &_I)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void initFaceFromLines(vpMbtPolygon &polygon)
vpPoint * p
corners in the object frame
virtual void setCameraParameters(const vpCameraParameters &_cam)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
vpCameraParameters cam
The camera parameters.
double getAngleAppear() const
virtual void track(const vpImage< unsigned char > &I)
double m_lambda
Gain of the virtual visual servoing stage.
unsigned int getMaskBorder() const
Implementation of column vector and the associated operations.
virtual void init(const vpImage< unsigned char > &I)
bool useScanLine
Use Scanline for visibility tests.
void setWindowSize(const unsigned int &w)
virtual void reinit(const vpImage< unsigned char > &I)
void setWindowName(const Ogre::String &n)
void extract(vpRotationMatrix &R) const
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
unsigned int getRows() const
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)
void buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r)
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Implementation of a matrix and operations on matrices.
void setPyramidLevels(const unsigned int &pL)
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
virtual void testTracking()
vpMatrix get_K_inverse() const
Implementation of a polygon of the model used by the model-based tracker.
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="")
void init(const vpKltOpencv &_tracker, const vpImage< bool > *mask=NULL)
std::map< int, int > & getCurrentPointsInd()
unsigned int getCurrentNumberPoints() const
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
vpColVector m_weightedError_klt
Weighted error.
unsigned int maskBorder
Erosion of the mask.
virtual void loadConfigFile(const std::string &configFile)
void setHarrisParam(const double &hp)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
virtual void setKltOpencv(const vpKltOpencv &t)
void setMinDistance(const double &mD)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
void setTrackerId(int tid)
unsigned int getInitialNumberPoint() const
bool displayFeatures
If true, the features are displayed.
vpHomogeneousMatrix ctTc0
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
Implementation of a rotation matrix and operations on such kind of matrices.
virtual unsigned int getNbPolygon() const
vpMatrix oJo
The Degrees of Freedom to estimate.
bool applyLodSettingInConfig
int getPyramidLevels() const
Get the list of features id.
unsigned int getHeight() const
bool hasNearClippingDistance() const
std::map< int, vpImagePoint > & getCurrentPoints()
Definition of the vpSubMatrix vpSubMatrix class provides a mask on a vpMatrix all properties of vpMat...
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)
void setBlockSize(const int blockSize)
void computePolygonClipped(const vpCameraParameters &cam=vpCameraParameters())
std::list< vpMbtDistanceKltCylinder * > kltCylinders
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
unsigned int getInitialNumberPoint() const
void setMaxFeatures(const unsigned int &mF)
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
void setWindowSize(const int winSize)
Implementation of an homography and operations on homographies.
double angleAppears
Angle used to detect a face appearance.
void setInitialGuess(const std::vector< cv::Point2f > &guess_pts)
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
double getHarrisParam() const
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)
std::map< int, vpImagePoint > getKltImagePointsWithId() const
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 >())
static vpHomogeneousMatrix direct(const vpColVector &v)
double getMinDistance() const
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
std::list< vpMbtDistanceCircle * > circles_disp
Vector of the circles used here only to display the full model.
double getHarrisFreeParameter() const
Get the free parameter of the Harris detector.
unsigned int getNbPoint() const
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
void getFeature(const int &index, long &id, float &x, float &y) const
double getAngleDisappear() const
virtual ~vpMbKltTracker()
void removeOutliers(const vpColVector &weight, const double &threshold_outlier)
void resize(unsigned int n_data)
Resize containers for sort methods.
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
double getMinDistance() const
void setPyramidLevels(const int pyrMaxLevel)
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker, const vpImage< bool > *mask=NULL)
void setName(const std::string &circle_name)
This class defines the container for a plane geometrical structure.
virtual void setClipping(const unsigned int &flags)
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
bool ogreShowConfigDialog
double getQuality() const
void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, const std::string &name="")
vpRobust m_robust_klt
Robust.
vpHomogeneousMatrix c0Mo
Initial pose.
void setBlockSize(const unsigned int &bs)
unsigned int clippingFlag
Flags specifying which clipping to used.
void updateMask(cv::Mat &mask, unsigned char _nb=255, unsigned int _shiftBorder=0)
void setUseHarris(const int useHarrisDetector)
vpAROgre * getOgreContext()
virtual void setNearClippingDistance(const double &dist)
std::list< vpMbtDistanceKltPoints * > kltPolygons
virtual void setFarClippingDistance(const double &dist)
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
vpHomogeneousMatrix inverse() const
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMc0, vpColVector &_R, vpMatrix &_J)
void computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &cHc0)
double getNearClippingDistance() 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)
Class that defines what is a point.
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Class to define colors available for display functionnalities.
bool useScanLine
Use scanline rendering.
unsigned int setVisible(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setMaskBorder(const unsigned int &mb)
unsigned int getPyramidLevels() const
double getQuality() const
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
std::vector< int > listIndicesCylinderBBox
Pointer to the polygon that define a face.
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, const double, const int, const std::string &name="")
error that can be emited by ViSP classes.
void computeFov(const unsigned int &w, const unsigned int &h)
Parse an Xml file to extract configuration parameters of a Mbt Klt object.
bool useScanLine
Use scanline rendering.
void setCameraParameters(const vpCameraParameters &camera)
cv::Mat cur
Temporary OpenCV image for fast conversion.
double angleDisappears
Angle used to detect a face disappearance.
void updateMask(cv::Mat &mask, unsigned char _nb=255, unsigned int _shiftBorder=0)
void setQuality(const double &q)
double getMinLineLengthThreshold() const
bool useLodGeneral
True if LOD mode is enabled.
unsigned int getWidth() const
void setAngleDisappear(const double &adisappear)
unsigned int getRows() const
virtual void initCylinder(const vpPoint &, const vpPoint &, const double, const int, const std::string &name="")