ViSP
2.9.0
|
#include <vpMbKltTracker.h>
Public Member Functions | |
vpMbKltTracker () | |
virtual | ~vpMbKltTracker () |
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 | display (const vpImage< vpRGBa > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false) |
virtual void | loadConfigFile (const std::string &configFile) |
void | loadConfigFile (const char *configFile) |
virtual double | getAngleAppear () const |
virtual double | getAngleDisappear () const |
virtual unsigned int | getClipping () const |
vpMbHiddenFaces< vpMbtKltPolygon > & | getFaces () |
virtual double | getFarClippingDistance () const |
CvPoint2D32f * | getKltPoints () |
std::vector< vpImagePoint > | getKltImagePoints () const |
std::map< int, vpImagePoint > | getKltImagePointsWithId () const |
vpKltOpencv | getKltOpencv () const |
virtual double | getLambda () const |
unsigned int | getMaskBorder () const |
virtual unsigned int | getMaxIter () const |
int | getNbKltPoints () const |
virtual double | getNearClippingDistance () const |
double | getThresholdAcceptation () const |
void | resetTracker () |
virtual void | setAngleAppear (const double &a) |
virtual void | setAngleDisappear (const double &a) |
void | setCameraParameters (const vpCameraParameters &cam) |
virtual void | setClipping (const unsigned int &flags) |
virtual void | setFarClippingDistance (const double &dist) |
void | setKltOpencv (const vpKltOpencv &t) |
virtual void | setLambda (const double gain) |
void | setMaskBorder (const unsigned int &e) |
virtual void | setMaxIter (const unsigned int max) |
virtual void | setNearClippingDistance (const double &dist) |
virtual void | setOgreVisibilityTest (const bool &v) |
virtual void | setPose (const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo) |
void | setThresholdAcceptation (const double th) |
virtual void | testTracking () |
virtual void | track (const vpImage< unsigned char > &I) |
virtual void | getCameraParameters (vpCameraParameters &camera) const |
virtual vpMatrix | getCovarianceMatrix () const |
void | getPose (vpHomogeneousMatrix &cMo_) const |
vpHomogeneousMatrix | getPose () const |
virtual void | initClick (const vpImage< unsigned char > &I, const std::string &initFile, const bool displayHelp=false) |
virtual void | initClick (const vpImage< unsigned char > &I, const std::vector< vpPoint > &points3D_list, const std::string &displayFile="") |
virtual void | initFromPoints (const vpImage< unsigned char > &I, const std::string &initFile) |
virtual void | initFromPoints (const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &points2D_list, const std::vector< vpPoint > &points3D_list) |
virtual void | initFromPose (const vpImage< unsigned char > &I, const std::string &initFile) |
virtual void | initFromPose (const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo) |
virtual void | initFromPose (const vpImage< unsigned char > &I, const vpPoseVector &cPo) |
virtual void | loadModel (const std::string &modelFile) |
void | savePose (const std::string &filename) |
virtual void | setCovarianceComputation (const bool &flag) |
void | setDisplayFeatures (const bool displayF) |
void | setPoseSavingFilename (const std::string &filename) |
Protected Member Functions | |
virtual void | init (const vpImage< unsigned char > &I) |
virtual void | reinit (const vpImage< unsigned char > &I) |
void | computeVVS (const unsigned int &nbInfos, vpColVector &w) |
virtual void | initFaceFromCorners (const std::vector< vpPoint > &corners, const unsigned int indexFace=-1) |
virtual void | initCylinder (const vpPoint &, const vpPoint &, const double, const unsigned int) |
void | preTracking (const vpImage< unsigned char > &I, unsigned int &nbInfos, unsigned int &nbFaceUsed) |
bool | postTracking (const vpImage< unsigned char > &I, vpColVector &w) |
void | computeJTR (const vpMatrix &J, const vpColVector &R, vpMatrix &JTR) |
virtual void | extractGroup (SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, unsigned int &indexFace) |
virtual void | extractFaces (SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, unsigned int &indexFace) |
virtual void | extractLines (SoVRMLIndexedLineSet *line_set) |
virtual void | extractCylinders (SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform) |
vpPoint | getGravityCenter (const std::vector< vpPoint > &_pts) |
virtual void | loadVRMLModel (const std::string &modelFile) |
virtual void | loadCAOModel (const std::string &modelFile) |
Protected Attributes | |
IplImage * | cur |
vpHomogeneousMatrix | c0Mo |
double | angleAppears |
double | angleDisappears |
bool | compute_interaction |
bool | firstInitialisation |
unsigned int | maskBorder |
double | lambda |
unsigned int | maxIter |
double | threshold_outlier |
double | percentGood |
bool | useOgre |
vpHomogeneousMatrix | ctTc0 |
vpKltOpencv | tracker |
vpMbHiddenFaces< vpMbtKltPolygon > | faces |
bool | firstTrack |
double | distNearClip |
double | distFarClip |
unsigned int | clippingFlag |
vpCameraParameters | cam |
vpHomogeneousMatrix | cMo |
std::string | modelFileName |
bool | modelInitialised |
std::string | poseSavingFilename |
bool | computeCovariance |
vpMatrix | covarianceMatrix |
bool | displayFeatures |
Model based tracker using only KLT.
The tracker requires the knowledge of the 3D model that could be provided in a vrml or in a cao file. The cao format is described in loadCAOModel(). It may also use an xml file used to tune the behavior of the tracker and an init file used to compute the pose at the very first image.
The following code shows the simplest way to use the tracker. The Tutorial: Model-based tracking is also a good starting point to use this class.
The tracker can also be used without display, in that case the initial pose must be known (object always at the same initial pose for example) or computed using another method:
Finally it can be used not to track an object but just to display a model at a given pose:
Definition at line 227 of file vpMbKltTracker.h.
vpMbKltTracker::vpMbKltTracker | ( | ) |
Definition at line 46 of file vpMbKltTracker.cpp.
References angleAppears, angleDisappears, faces, vpMbHiddenFaces< PolygonType >::getOgreContext(), vpMath::rad(), vpKltOpencv::setBlockSize(), vpKltOpencv::setHarrisFreeParameter(), vpKltOpencv::setMaxFeatures(), vpKltOpencv::setMinDistance(), vpKltOpencv::setPyramidLevels(), vpKltOpencv::setQuality(), vpKltOpencv::setTrackerId(), vpKltOpencv::setUseHarris(), vpAROgre::setWindowName(), vpKltOpencv::setWindowSize(), and tracker.
|
virtual |
|
protectedinherited |
Compute , with J the interaction matrix and R the vector of residu.
vpMatrixException::incorrectMatrixSizeError | if the sizes of the matrices do not allow the computation. |
interaction | : The interaction matrix (size Nx6). |
error | : The residu vector (size Nx1). |
JTR | : The resulting JTR matrix (size 6x1). |
Definition at line 1433 of file vpMbTracker.cpp.
References vpMatrix::getCols(), vpMatrix::getRows(), vpMatrixException::incorrectMatrixSizeError, and vpMatrix::resize().
Referenced by vpMbEdgeKltTracker::computeVVS(), computeVVS(), and vpMbEdgeTracker::computeVVS().
|
protected |
Realize the VVS loop for the tracking
nbInfos | : Size of the features |
w | : weight of the features after M-Estimation. |
Definition at line 558 of file vpMbKltTracker.cpp.
References vpMatrix::AtA(), c0Mo, vpMbTracker::cam, vpMbTracker::cMo, compute_interaction, vpMbTracker::computeCovariance, vpMatrix::computeCovarianceMatrix(), vpMbTracker::computeJTR(), vpMbTracker::covarianceMatrix, ctTc0, vpMatrix::diag(), vpExponentialMap::direct(), faces, vpCameraParameters::get_px(), vpMatrix::getRows(), vpHomogeneousMatrix::inverse(), lambda, maxIter, vpRobust::MEstimator(), vpMatrix::pseudoInverse(), vpColVector::resize(), vpMatrix::resize(), vpRobust::setIteration(), vpRobust::setThreshold(), vpMbHiddenFaces< PolygonType >::size(), and vpRobust::TUKEY.
Referenced by vpMbEdgeKltTracker::track(), and track().
|
virtual |
Display the 3D model at a given position using the given camera parameters
I | : The image. |
cMo_ | : Pose used to project the 3D model into the image. |
camera | : The camera parameters. |
col | : The desired color. |
thickness | : The thickness of the lines. |
displayFullModel | : Boolean to say if all the model has to be displayed. |
Implements vpMbTracker.
Reimplemented in vpMbEdgeKltTracker.
Definition at line 801 of file vpMbKltTracker.cpp.
References clippingFlag, vpCameraParameters::computeFov(), vpMbTracker::displayFeatures, vpDisplay::displayLine(), vpMbHiddenFaces< PolygonType >::displayOgre(), vpMbtPolygon::DOWN_CLIPPING, faces, vpMbtPolygon::FAR_CLIPPING, vpImage< Type >::getHeight(), vpImage< Type >::getWidth(), vpMbtPolygon::LEFT_CLIPPING, vpMbtPolygon::NEAR_CLIPPING, vpMbtPolygon::RIGHT_CLIPPING, vpMbHiddenFaces< PolygonType >::size(), vpMbtPolygon::UP_CLIPPING, and useOgre.
|
virtual |
Display the 3D model at a given position using the given camera parameters
I | : The color image. |
cMo_ | : Pose used to project the 3D model into the image. |
camera | : The camera parameters. |
col | : The desired color. |
thickness | : The thickness of the lines. |
displayFullModel | : Boolean to say if all the model has to be displayed. |
Implements vpMbTracker.
Reimplemented in vpMbEdgeKltTracker.
Definition at line 855 of file vpMbKltTracker.cpp.
References clippingFlag, vpCameraParameters::computeFov(), vpMbTracker::displayFeatures, vpDisplay::displayLine(), vpMbHiddenFaces< PolygonType >::displayOgre(), vpMbtPolygon::DOWN_CLIPPING, faces, vpMbtPolygon::FAR_CLIPPING, vpImage< Type >::getHeight(), vpImage< Type >::getWidth(), vpMbtPolygon::LEFT_CLIPPING, vpMbtPolygon::NEAR_CLIPPING, vpMbtPolygon::RIGHT_CLIPPING, vpMbHiddenFaces< PolygonType >::size(), vpMbtPolygon::UP_CLIPPING, and useOgre.
|
protectedvirtualinherited |
Extract a cylinder to track from the VMRL model. This method calls the initCylinder() method implemented in the child class.
face_set | : Pointer to the cylinder in the vrml format. |
transform | : Transformation matrix applied to the cylinder. |
Definition at line 1282 of file vpMbTracker.cpp.
References vpException::badValue, vpException::dimensionError, vpPoint::get_oX(), vpPoint::get_oY(), vpPoint::get_oZ(), vpMbTracker::getGravityCenter(), vpMbTracker::initCylinder(), vpMath::maximum(), vpPoint::setWorldCoordinates(), and vpMatrix::sumSquare().
Referenced by vpMbTracker::extractGroup().
|
protectedvirtualinherited |
Extract a face of the object to track from the VMRL model. This method calls the initFaceFromCorners() method implemented in the child class.
face_set | : Pointer to the face in the vrml format. |
transform | : Transformation matrix applied to the face. |
indexFace | : Face index. |
Definition at line 1228 of file vpMbTracker.cpp.
References vpMbTracker::initFaceFromCorners(), and vpPoint::setWorldCoordinates().
Referenced by vpMbTracker::extractGroup().
|
protectedvirtualinherited |
Extract a VRML object Group.
sceneGraphVRML2 | : Current node (either Transform, or Group node). |
transform | : Transformation matrix for this group. |
indexFace | : Index of the face. |
Definition at line 1141 of file vpMbTracker.cpp.
References vpMbTracker::extractCylinders(), vpMbTracker::extractFaces(), and vpMbTracker::extractLines().
Referenced by vpMbTracker::loadVRMLModel().
|
protectedvirtualinherited |
Extract a line of the object to track from the VMRL model. This method calls the initFaceFromCorners() method implemented in the child class.
line_set | : Pointer to the line in the vrml format. |
Definition at line 1378 of file vpMbTracker.cpp.
References vpMbTracker::initFaceFromCorners(), and vpPoint::setWorldCoordinates().
Referenced by vpMbTracker::extractGroup().
|
inlinevirtual |
Return the angle used to test polygons appearance.
Reimplemented in vpMbEdgeKltTracker.
Definition at line 289 of file vpMbKltTracker.h.
Referenced by vpMbEdgeKltTracker::getAngleAppear().
|
inlinevirtual |
Return the angle used to test polygons disappearance.
Reimplemented in vpMbEdgeKltTracker.
Definition at line 292 of file vpMbKltTracker.h.
Referenced by vpMbEdgeKltTracker::getAngleDisappear().
|
inlinevirtualinherited |
Get the camera parameters.
camera | : copy of the camera parameters used by the tracker. |
Definition at line 158 of file vpMbTracker.h.
|
inlinevirtual |
Get the clipping used.
Reimplemented in vpMbEdgeKltTracker.
Definition at line 301 of file vpMbKltTracker.h.
|
inlinevirtualinherited |
|
inline |
Return a reference to the faces structure.
Definition at line 304 of file vpMbKltTracker.h.
|
inlinevirtual |
Get the far distance for clipping.
Reimplemented in vpMbEdgeKltTracker.
Definition at line 311 of file vpMbKltTracker.h.
Referenced by vpMbEdgeKltTracker::getFarClippingDistance().
Compute the center of gravity of a set of point. This is used in the cylinder extraction to find the center of the circles.
vpException::dimensionError | if the set is empty. |
pts | : Set of point to extract the center of gravity. |
Definition at line 1349 of file vpMbTracker.cpp.
References vpException::dimensionError, vpPoint::get_oX(), and vpPoint::setWorldCoordinates().
Referenced by vpMbTracker::extractCylinders().
std::vector< vpImagePoint > vpMbKltTracker::getKltImagePoints | ( | ) | const |
Get the current list of KLT points.
Definition at line 197 of file vpMbKltTracker.cpp.
References vpKltOpencv::getFeature(), vpKltOpencv::getNbFeatures(), and tracker.
std::map< int, vpImagePoint > vpMbKltTracker::getKltImagePointsWithId | ( | ) | const |
Get the current list of KLT points and their id.
Definition at line 218 of file vpMbKltTracker.cpp.
References vpKltOpencv::getFeature(), vpKltOpencv::getNbFeatures(), and tracker.
|
inline |
Get the klt tracker at the current state.
Definition at line 329 of file vpMbKltTracker.h.
|
inline |
Get the current list of KLT points.
Definition at line 318 of file vpMbKltTracker.h.
References vpKltOpencv::getFeatures().
|
inlinevirtual |
Get the value of the gain used to compute the control law.
Reimplemented in vpMbEdgeKltTracker.
Definition at line 336 of file vpMbKltTracker.h.
|
inline |
Get the erosion of the mask used on the Model faces.
Definition at line 343 of file vpMbKltTracker.h.
|
inlinevirtual |
Get the maximum iteration of the virtual visual servoing stage.
Reimplemented in vpMbEdgeKltTracker.
Definition at line 350 of file vpMbKltTracker.h.
|
inline |
Get the current number of klt points.
Definition at line 357 of file vpMbKltTracker.h.
References vpKltOpencv::getNbFeatures().
|
inlinevirtual |
Get the near distance for clipping.
Reimplemented in vpMbEdgeKltTracker.
Definition at line 364 of file vpMbKltTracker.h.
Referenced by vpMbEdgeKltTracker::getNearClippingDistance().
|
inlineinherited |
Get the current pose between the object and the camera. cMo is the matrix which can be used to express coordinates from the object frame to camera frame.
cMo_ | : the pose |
Definition at line 177 of file vpMbTracker.h.
|
inlineinherited |
Get the current pose between the object and the camera. cMo is the matrix which can be used to express coordinates from the object frame to camera frame.
Definition at line 186 of file vpMbTracker.h.
|
inline |
Get the threshold for the acceptation of a point.
Definition at line 371 of file vpMbKltTracker.h.
|
protectedvirtual |
Initialise the tracking.
I | : Input image. |
Implements vpMbTracker.
Reimplemented in vpMbEdgeKltTracker.
Definition at line 84 of file vpMbKltTracker.cpp.
References angleAppears, angleDisappears, vpMbTracker::cam, vpMbTracker::cMo, faces, vpException::fatalError, vpImage< Type >::getHeight(), vpImage< Type >::getWidth(), vpMbHiddenFaces< PolygonType >::initOgre(), vpMbHiddenFaces< PolygonType >::isOgreInitialised(), vpMbTracker::modelInitialised, reinit(), vpMbHiddenFaces< PolygonType >::setBackgroundSizeOgre(), vpMbHiddenFaces< PolygonType >::setVisible(), vpMbHiddenFaces< PolygonType >::setVisibleOgre(), and useOgre.
Referenced by vpMbEdgeKltTracker::init().
|
virtualinherited |
Initialise the tracking by clicking on the image points corresponding to the 3D points (object frame) in the file initFile. The structure of this file is (without the comments):
I | : Input image |
initFile | : File containing the points where to click |
displayHelp | : Optionnal display of an image ( 'initFile.ppm' ). This image may be used to show where to click. |
vpException::ioError | : The file specified in initFile doesn't exist. |
Definition at line 131 of file vpMbTracker.cpp.
References vpPose::addPoint(), vpException::badValue, vpHomogeneousMatrix::buildFrom(), vpMouseButton::button1, vpMbTracker::cam, vpPose::clearPoint(), vpMbTracker::cMo, vpPose::computePose(), vpPose::computeResidual(), vpPixelMeterConversion::convertPoint(), vpPose::DEMENTHON, vpMbTracker::display(), vpDisplay::display(), vpDisplay::displayCharString(), vpDisplay::displayCross(), vpDisplay::displayFrame(), vpDisplay::displayPoint(), vpDisplay::flush(), vpDisplay::getClick(), vpColor::green, vpDisplayOpenCV::init(), vpMbTracker::init(), vpException::ioError, vpPose::LAGRANGE, vpMbTracker::poseSavingFilename, vpImageIo::read(), vpColor::red, vpMbTracker::savePose(), vpPoint::set_x(), vpPoint::set_y(), vpHomogeneousMatrix::setIdentity(), vpPoint::setWorldCoordinates(), and vpPose::VIRTUAL_VS.
|
virtualinherited |
Initialise the tracking by clicking on the image points corresponding to the 3D points (object frame) in the list points3D_list.
I | : Input image |
points3D_list | : List of the 3D points (object frame). |
displayFile | : Path to the image used to display the help. |
Definition at line 352 of file vpMbTracker.cpp.
References vpPose::addPoint(), vpMouseButton::button1, vpMbTracker::cam, vpPose::clearPoint(), vpMbTracker::cMo, vpPose::computePose(), vpPose::computeResidual(), vpPixelMeterConversion::convertPoint(), vpPose::DEMENTHON, vpMbTracker::display(), vpDisplay::display(), vpDisplay::displayCharString(), vpDisplay::displayCross(), vpDisplay::displayFrame(), vpDisplay::displayPoint(), vpDisplay::flush(), vpDisplay::getClick(), vpColor::green, vpDisplayOpenCV::init(), vpMbTracker::init(), vpPose::LAGRANGE, vpImageIo::read(), vpColor::red, vpPoint::set_x(), vpPoint::set_y(), and vpPose::VIRTUAL_VS.
|
inlineprotectedvirtual |
Add a cylinder to track from two points on the axis (defining the length of the cylinder) and its radius.
p1 | : First point on the axis. |
p2 | : Second point on the axis. |
radius | : Radius of the cylinder. |
indexCylinder | : Index of the cylinder. |
Implements vpMbTracker.
Reimplemented in vpMbEdgeKltTracker.
Definition at line 446 of file vpMbKltTracker.h.
|
protectedvirtual |
Initialise a new face from the coordinates given in parameter.
corners | : Coordinates of the corners of the face in the object frame. |
indexFace | : index of the face (depends on the vrml file organization). |
Implements vpMbTracker.
Reimplemented in vpMbEdgeKltTracker.
Definition at line 439 of file vpMbKltTracker.cpp.
References vpMbtPolygon::addPoint(), vpMbHiddenFaces< PolygonType >::addPolygon(), vpMbTracker::cam, clippingFlag, distFarClip, distNearClip, faces, vpMbtPolygon::FAR_CLIPPING, vpMbHiddenFaces< PolygonType >::getPolygon(), vpMbtPolygon::NEAR_CLIPPING, vpMbtPolygon::NO_CLIPPING, vpMbtPolygon::setIndex(), and vpMbtPolygon::setNbPoint().
Referenced by vpMbEdgeKltTracker::initFaceFromCorners().
|
virtualinherited |
Initialise the tracking by reading the 3D points (object frame) and the image points in initFile. The structure of this file is (without the comments):
I | : Input image |
initFile | : Path to the file containing all the points. |
Definition at line 469 of file vpMbTracker.cpp.
References vpPose::addPoint(), vpException::badValue, vpMbTracker::cam, vpMbTracker::cMo, vpPose::computePose(), vpPose::computeResidual(), vpPixelMeterConversion::convertPoint(), vpPose::DEMENTHON, vpMbTracker::init(), vpException::ioError, vpPose::LAGRANGE, vpPoint::set_x(), vpPoint::set_y(), vpPoint::setWorldCoordinates(), vpPose::VIRTUAL_VS, and vpERROR_TRACE.
|
virtualinherited |
Initialise the tracking with the list of image points (points2D_list) and the list of corresponding 3D points (object frame) (points3D_list).
I | : Input image |
points2D_list | : List of image points. |
points3D_list | : List of 3D points (object frame). |
Definition at line 554 of file vpMbTracker.cpp.
References vpPose::addPoint(), vpMbTracker::cam, vpMbTracker::cMo, vpPose::computePose(), vpPose::computeResidual(), vpPixelMeterConversion::convertPoint(), vpPose::DEMENTHON, vpMbTracker::init(), vpPose::LAGRANGE, vpPoint::set_x(), vpPoint::set_y(), vpPoint::setWorldCoordinates(), vpPose::VIRTUAL_VS, and vpERROR_TRACE.
|
virtualinherited |
Initialise the tracking thanks to the pose in vpPoseVector format, and read in the file initFile. The structure of this file is (without the comments):
Where the three firsts lines refer to the translation and the three last to the rotation in thetaU parametrisation (see vpThetaUVector).
I | : Input image |
initFile | : Path to the file containing the pose. |
Definition at line 609 of file vpMbTracker.cpp.
References vpHomogeneousMatrix::buildFrom(), vpMbTracker::cMo, vpMbTracker::init(), and vpException::ioError.
Referenced by vpMbTracker::initFromPose(), and vpMbEdgeTracker::reInitModel().
|
virtualinherited |
Initialise the tracking thanks to the pose.
I | : Input image |
cMo_ | : Pose matrix. |
Definition at line 644 of file vpMbTracker.cpp.
References vpMbTracker::cMo, and vpMbTracker::init().
|
virtualinherited |
Initialise the tracking thanks to the pose vector.
I | : Input image |
cPo | : Pose vector. |
Definition at line 656 of file vpMbTracker.cpp.
References vpMbTracker::initFromPose().
|
protectedvirtualinherited |
Load a 3D model contained in a .cao file.
the structure of the file is (without the comments) :
modelFile | : Full name of the .CAO file containing the model. |
Definition at line 838 of file vpMbTracker.cpp.
References vpException::badValue, vpMbTracker::initCylinder(), vpMbTracker::initFaceFromCorners(), vpException::ioError, vpPoint::setWorldCoordinates(), and vpTRACE.
Referenced by vpMbTracker::loadModel().
|
virtual |
Load the xml configuration file. An example of such a file is provided in loadConfigFile(const char*) documentation. From the configuration file initialize the parameters corresponding to the objects: KLT, camera.
configFile | : full name of the xml file. |
Implements vpMbTracker.
Reimplemented in vpMbEdgeKltTracker.
Definition at line 684 of file vpMbKltTracker.cpp.
void vpMbKltTracker::loadConfigFile | ( | const char * | configFile | ) |
Load the xml configuration file. From the configuration file initialize the parameters corresponding to the objects: KLT, camera.
vpException::ioError | if the file has not been properly parsed (file not found or wrong format for the data). |
configFile | : full name of the xml file. |
The XML configuration file has the following form:
Definition at line 736 of file vpMbKltTracker.cpp.
References angleAppears, angleDisappears, clippingFlag, vpMath::deg(), vpMbtPolygon::FOV_CLIPPING, vpMbXmlParser::getAngleAppear(), vpMbXmlParser::getAngleDisappear(), vpMbtKltXmlParser::getBlockSize(), vpMbXmlParser::getCameraParameters(), vpMbXmlParser::getFarClippingDistance(), vpMbXmlParser::getFovClipping(), vpMbtKltXmlParser::getHarrisParam(), vpMbtKltXmlParser::getMaskBorder(), vpMbtKltXmlParser::getMaxFeatures(), vpMbtKltXmlParser::getMinDistance(), vpMbXmlParser::getNearClippingDistance(), vpMbtKltXmlParser::getPyramidLevels(), vpMbtKltXmlParser::getQuality(), vpMbtKltXmlParser::getWindowSize(), vpMbXmlParser::hasFarClippingDistance(), vpMbXmlParser::hasNearClippingDistance(), vpException::ioError, maskBorder, vpMbtKltXmlParser::parse(), vpMath::rad(), vpMbXmlParser::setAngleAppear(), vpMbXmlParser::setAngleDisappear(), vpMbtKltXmlParser::setBlockSize(), vpKltOpencv::setBlockSize(), setCameraParameters(), setClipping(), setFarClippingDistance(), vpKltOpencv::setHarrisFreeParameter(), vpMbtKltXmlParser::setHarrisParam(), vpMbtKltXmlParser::setMaskBorder(), vpMbtKltXmlParser::setMaxFeatures(), vpKltOpencv::setMaxFeatures(), vpMbtKltXmlParser::setMinDistance(), vpKltOpencv::setMinDistance(), setNearClippingDistance(), vpMbtKltXmlParser::setPyramidLevels(), vpKltOpencv::setPyramidLevels(), vpMbtKltXmlParser::setQuality(), vpKltOpencv::setQuality(), vpMbtKltXmlParser::setWindowSize(), vpKltOpencv::setWindowSize(), tracker, vpERROR_TRACE, and vpTRACE.
|
virtualinherited |
Load a 3D model from the file in parameter. This file must either be a vrml file (.wrl) or a CAO file (.cao). CAO format is described in the loadCAOModel() method.
vpException::ioError | if the file cannot be open, or if its extension is not wrl or cao. |
modelFile | : the file containing the model. |
Reimplemented in vpMbEdgeTracker, and vpMbEdgeKltTracker.
Definition at line 705 of file vpMbTracker.cpp.
References vpIoTools::checkFilename(), vpException::ioError, vpMbTracker::loadCAOModel(), vpMbTracker::loadVRMLModel(), vpMbTracker::modelFileName, and vpMbTracker::modelInitialised.
Referenced by vpMbEdgeKltTracker::loadModel(), and vpMbEdgeTracker::loadModel().
|
protectedvirtualinherited |
Load the 3D model of the object from a vrml file. Only LineSet and FaceSet are extracted from the vrml file.
vpException::fatalError | if the file cannot be open. |
modelFile | : The full name of the file containing the 3D model. |
Definition at line 763 of file vpMbTracker.cpp.
References vpMbTracker::extractGroup(), vpException::fatalError, and vpERROR_TRACE.
Referenced by vpMbTracker::loadModel().
|
protected |
Realize the post tracking operations. Mostly visibility tests
Definition at line 500 of file vpMbKltTracker.cpp.
References angleAppears, angleDisappears, vpMbTracker::cam, vpMbTracker::cMo, faces, percentGood, vpMbHiddenFaces< PolygonType >::setVisible(), vpMbHiddenFaces< PolygonType >::setVisibleOgre(), vpMbHiddenFaces< PolygonType >::size(), threshold_outlier, and useOgre.
Referenced by vpMbEdgeKltTracker::postTracking(), and track().
|
protected |
Realize the pre tracking operations
I | : The input image. |
nbInfos | : Size of the features. |
nbFaceUsed | : Number of face used for the tracking. |
Definition at line 474 of file vpMbKltTracker.cpp.
References vpImageConvert::convert(), cur, faces, firstTrack, vpMbHiddenFaces< PolygonType >::size(), vpKltOpencv::track(), and tracker.
Referenced by vpMbEdgeKltTracker::track(), and track().
|
protectedvirtual |
Definition at line 111 of file vpMbKltTracker.cpp.
References c0Mo, vpMbTracker::cMo, vpImageConvert::convert(), ctTc0, cur, faces, firstTrack, vpImage< Type >::getHeight(), vpImage< Type >::getWidth(), vpKltOpencv::initTracking(), maskBorder, vpHomogeneousMatrix::setIdentity(), vpMbHiddenFaces< PolygonType >::size(), and tracker.
Referenced by init(), setPose(), vpMbEdgeKltTracker::track(), and track().
|
virtual |
Reset the tracker. The model is removed and the pose is set to identity. The tracker needs to be initialized with a new model and a new pose.
Implements vpMbTracker.
Definition at line 145 of file vpMbKltTracker.cpp.
References angleAppears, angleDisappears, clippingFlag, vpMbTracker::cMo, compute_interaction, vpMbTracker::computeCovariance, cur, faces, firstInitialisation, firstTrack, lambda, maskBorder, maxIter, vpMbtPolygon::NO_CLIPPING, percentGood, vpMath::rad(), vpMbHiddenFaces< PolygonType >::reset(), vpKltOpencv::setBlockSize(), vpKltOpencv::setHarrisFreeParameter(), vpHomogeneousMatrix::setIdentity(), vpKltOpencv::setMaxFeatures(), vpKltOpencv::setMinDistance(), vpKltOpencv::setPyramidLevels(), vpKltOpencv::setQuality(), vpKltOpencv::setTrackerId(), vpKltOpencv::setUseHarris(), vpKltOpencv::setWindowSize(), threshold_outlier, tracker, and useOgre.
Referenced by vpMbEdgeKltTracker::resetTracker().
|
inherited |
Save the pose in the given filename
filename | : Path to the file used to save the pose. |
Definition at line 667 of file vpMbTracker.cpp.
References vpPoseVector::buildFrom(), and vpMbTracker::cMo.
Referenced by vpMbTracker::initClick().
|
inlinevirtual |
Set the angle used to test polygons appearance. If the angle between the normal of the polygon and the line going from the camera to the polygon center has a value lower than this parameter, the polygon is considered as appearing. The polygon will then be tracked.
a | : new angle in radian. |
Reimplemented in vpMbEdgeKltTracker.
Definition at line 384 of file vpMbKltTracker.h.
Referenced by vpMbEdgeKltTracker::setAngleAppear().
|
inlinevirtual |
Set the angle used to test polygons disappearance. If the angle between the normal of the polygon and the line going from the camera to the polygon center has a value greater than this parameter, the polygon is considered as disappearing. The tracking of the polygon will then be stopped.
a | : new angle in radian. |
Reimplemented in vpMbEdgeKltTracker.
Definition at line 395 of file vpMbKltTracker.h.
Referenced by vpMbEdgeKltTracker::setAngleDisappear().
|
virtual |
Set the camera parameters.
camera | : the new camera parameters. |
Reimplemented from vpMbTracker.
Definition at line 253 of file vpMbKltTracker.cpp.
References vpMbTracker::cam, faces, and vpMbHiddenFaces< PolygonType >::size().
Referenced by loadConfigFile(), and vpMbEdgeKltTracker::setCameraParameters().
|
virtual |
Specify which clipping to use.
flags | : New clipping flags. |
Reimplemented in vpMbEdgeKltTracker.
Definition at line 425 of file vpMbKltTracker.cpp.
References clippingFlag, faces, and vpMbHiddenFaces< PolygonType >::size().
Referenced by loadConfigFile(), vpMbEdgeKltTracker::loadConfigFile(), and vpMbEdgeKltTracker::setClipping().
|
inlinevirtualinherited |
Set if the covaraince matrix has to be computed.
flag | : True if the covariance has to be computed, false otherwise |
Definition at line 240 of file vpMbTracker.h.
|
inlineinherited |
Enable to display the features.
displayF | : set it to true to display the features. |
Definition at line 247 of file vpMbTracker.h.
|
virtual |
Set the far distance for clipping.
dist | : Far clipping value. |
Reimplemented in vpMbEdgeKltTracker.
Definition at line 381 of file vpMbKltTracker.cpp.
References clippingFlag, distFarClip, distNearClip, faces, vpMbtPolygon::FAR_CLIPPING, vpMbtPolygon::NEAR_CLIPPING, vpMbHiddenFaces< PolygonType >::size(), and vpTRACE.
Referenced by loadConfigFile(), and vpMbEdgeKltTracker::setFarClippingDistance().
void vpMbKltTracker::setKltOpencv | ( | const vpKltOpencv & | t | ) |
Set the new value of the klt tracker.
t | : Klt tracker containing the new values. |
Definition at line 237 of file vpMbKltTracker.cpp.
References vpKltOpencv::getBlockSize(), vpKltOpencv::getHarrisFreeParameter(), vpKltOpencv::getMaxFeatures(), vpKltOpencv::getMinDistance(), vpKltOpencv::getPyramidLevels(), vpKltOpencv::getQuality(), vpKltOpencv::getWindowSize(), vpKltOpencv::setBlockSize(), vpKltOpencv::setHarrisFreeParameter(), vpKltOpencv::setMaxFeatures(), vpKltOpencv::setMinDistance(), vpKltOpencv::setPyramidLevels(), vpKltOpencv::setQuality(), vpKltOpencv::setWindowSize(), and tracker.
|
inlinevirtual |
Set the value of the gain used to compute the control law.
gain | : the desired value for the gain. |
Reimplemented in vpMbEdgeKltTracker.
Definition at line 410 of file vpMbKltTracker.h.
Referenced by vpMbEdgeKltTracker::setLambda().
|
inline |
Set the erosion of the mask used on the Model faces.
e | : The desired erosion. |
Definition at line 417 of file vpMbKltTracker.h.
|
inlinevirtual |
Set the maximum iteration of the virtual visual servoing stage.
max | : the desired number of iteration |
Reimplemented in vpMbEdgeKltTracker.
Definition at line 424 of file vpMbKltTracker.h.
Referenced by vpMbEdgeKltTracker::vpMbEdgeKltTracker().
|
virtual |
Set the near distance for clipping.
dist | : Near clipping value. |
Reimplemented in vpMbEdgeKltTracker.
Definition at line 402 of file vpMbKltTracker.cpp.
References clippingFlag, distFarClip, distNearClip, faces, vpMbtPolygon::FAR_CLIPPING, vpMbtPolygon::NEAR_CLIPPING, vpMbHiddenFaces< PolygonType >::size(), and vpTRACE.
Referenced by loadConfigFile(), and vpMbEdgeKltTracker::setNearClippingDistance().
|
virtual |
Use Ogre3D for visibility tests
v | : True to use it, False otherwise |
Reimplemented in vpMbEdgeKltTracker.
Definition at line 269 of file vpMbKltTracker.cpp.
References useOgre.
Referenced by vpMbEdgeKltTracker::setOgreVisibilityTest().
|
virtual |
Set the pose to be used in entry of the next call to the track() function. This pose will be just used once.
I | : image corresponding to the desired pose. |
cdMo | : Pose to affect. |
Implements vpMbTracker.
Reimplemented in vpMbEdgeKltTracker.
Definition at line 290 of file vpMbKltTracker.cpp.
References angleAppears, angleDisappears, vpMbTracker::cam, vpPlane::changeFrame(), vpMbTracker::cMo, vpException::divideByZeroError, vpHomogeneousMatrix::extract(), faces, firstTrack, vpCameraParameters::get_K(), vpCameraParameters::get_K_inverse(), vpPlane::getD(), vpKltOpencv::getMaxFeatures(), vpPlane::getNormal(), vpHomogeneousMatrix::inverse(), vpColVector::normalize(), reinit(), vpKltOpencv::setInitialGuess(), vpMbHiddenFaces< PolygonType >::setVisible(), vpMbHiddenFaces< PolygonType >::setVisibleOgre(), vpMbHiddenFaces< PolygonType >::size(), tracker, and useOgre.
Referenced by vpMbEdgeKltTracker::setPose().
|
inlineinherited |
Set the filename used to save the initial pose computed using the initClick() method. It is also used to read a previous pose in the same method. If the file is not set then, the initClick() method will create a .0.pos file in the root directory. This directory is the path to the file given to the method initClick() used to know the coordinates in the object frame.
filename | : The new filename. |
Definition at line 269 of file vpMbTracker.h.
|
inline |
Set the threshold for the acceptation of a point.
th | : Threshold for the weight below which a point is rejected. |
Definition at line 437 of file vpMbKltTracker.h.
|
virtual |
Test the quality of the tracking. The tracking is supposed to fail if less than 10 points are tracked.
vpTrackingException::fatalError | if the test fails. |
Implements vpMbTracker.
Reimplemented in vpMbEdgeKltTracker.
Definition at line 907 of file vpMbKltTracker.cpp.
References faces, vpTrackingException::fatalError, and vpMbHiddenFaces< PolygonType >::size().
|
virtual |
Realize the tracking of the object in the image
vpException | : if the tracking is supposed to have failed |
I | : the input image |
Implements vpMbTracker.
Reimplemented in vpMbEdgeKltTracker.
Definition at line 654 of file vpMbKltTracker.cpp.
References computeVVS(), vpTrackingException::notEnoughPointError, postTracking(), preTracking(), reinit(), and vpERROR_TRACE.
|
protected |
Angle used to detect a face appearance.
Definition at line 235 of file vpMbKltTracker.h.
Referenced by init(), loadConfigFile(), vpMbEdgeKltTracker::loadConfigFile(), postTracking(), resetTracker(), setPose(), and vpMbKltTracker().
|
protected |
Angle used to detect a face disappearance.
Definition at line 237 of file vpMbKltTracker.h.
Referenced by init(), loadConfigFile(), vpMbEdgeKltTracker::loadConfigFile(), postTracking(), resetTracker(), setPose(), and vpMbKltTracker().
|
protected |
Initial pose.
Definition at line 233 of file vpMbKltTracker.h.
Referenced by computeVVS(), and reinit().
|
protectedinherited |
The camera parameters.
Definition at line 106 of file vpMbTracker.h.
Referenced by vpMbEdgeTracker::addCylinder(), vpMbEdgeTracker::addLine(), vpMbEdgeKltTracker::computeVVS(), computeVVS(), vpMbEdgeTracker::computeVVS(), vpMbEdgeTracker::downScale(), init(), vpMbEdgeTracker::init(), vpMbTracker::initClick(), initFaceFromCorners(), vpMbTracker::initFromPoints(), vpMbEdgeKltTracker::loadConfigFile(), vpMbEdgeTracker::loadConfigFile(), postTracking(), vpMbEdgeKltTracker::setCameraParameters(), setCameraParameters(), setPose(), vpMbEdgeTracker::upScale(), and vpMbEdgeTracker::visibleFace().
|
protected |
Flags specifying which clipping to used.
Definition at line 267 of file vpMbKltTracker.h.
Referenced by display(), vpMbEdgeKltTracker::getClipping(), initFaceFromCorners(), loadConfigFile(), vpMbEdgeKltTracker::loadConfigFile(), resetTracker(), setClipping(), setFarClippingDistance(), and setNearClippingDistance().
|
protectedinherited |
The current pose.
Definition at line 108 of file vpMbTracker.h.
Referenced by vpMbEdgeKltTracker::computeVVS(), computeVVS(), vpMbEdgeTracker::computeVVS(), init(), vpMbEdgeKltTracker::init(), vpMbEdgeTracker::init(), vpMbTracker::initClick(), vpMbTracker::initFromPoints(), vpMbTracker::initFromPose(), vpMbEdgeKltTracker::postTracking(), postTracking(), reinit(), vpMbEdgeTracker::reInitLevel(), vpMbEdgeTracker::reInitModel(), resetTracker(), vpMbEdgeTracker::resetTracker(), vpMbTracker::savePose(), vpMbEdgeKltTracker::setPose(), setPose(), vpMbEdgeTracker::setPose(), vpMbEdgeKltTracker::track(), vpMbEdgeTracker::track(), vpMbEdgeKltTracker::trackFirstLoop(), vpMbEdgeTracker::trackMovingEdge(), and vpMbEdgeTracker::updateMovingEdge().
|
protected |
If true, compute the interaction matrix at each iteration of the minimization. Otherwise, compute it only on the first iteration.
Definition at line 239 of file vpMbKltTracker.h.
Referenced by computeVVS(), and resetTracker().
|
protectedinherited |
Flag used to specify if the covariance matrix has to be computed or not.
Definition at line 116 of file vpMbTracker.h.
Referenced by vpMbEdgeKltTracker::computeVVS(), computeVVS(), vpMbEdgeTracker::computeVVS(), resetTracker(), and vpMbEdgeKltTracker::vpMbEdgeKltTracker().
|
protectedinherited |
Covariance matrix.
Definition at line 118 of file vpMbTracker.h.
Referenced by vpMbEdgeKltTracker::computeVVS(), computeVVS(), and vpMbEdgeTracker::computeVVS().
|
protected |
The estimated displacement of the pose between the current instant and the initial position.
Definition at line 255 of file vpMbKltTracker.h.
Referenced by vpMbEdgeKltTracker::computeVVS(), computeVVS(), and reinit().
|
protected |
Temporary OpenCV image for fast conversion.
Definition at line 231 of file vpMbKltTracker.h.
Referenced by preTracking(), reinit(), resetTracker(), and ~vpMbKltTracker().
|
protectedinherited |
If true, the features are displayed.
Definition at line 120 of file vpMbTracker.h.
Referenced by vpMbEdgeKltTracker::display(), display(), vpMbEdgeKltTracker::postTracking(), and vpMbEdgeTracker::track().
|
protected |
Distance for near clipping.
Definition at line 265 of file vpMbKltTracker.h.
Referenced by initFaceFromCorners(), setFarClippingDistance(), and setNearClippingDistance().
|
protected |
Distance for near clipping.
Definition at line 263 of file vpMbKltTracker.h.
Referenced by initFaceFromCorners(), setFarClippingDistance(), and setNearClippingDistance().
|
protected |
Set of faces describing the object.
Definition at line 259 of file vpMbKltTracker.h.
Referenced by vpMbEdgeKltTracker::computeVVS(), computeVVS(), vpMbEdgeKltTracker::display(), display(), vpMbEdgeKltTracker::getFaces(), init(), vpMbEdgeKltTracker::init(), initFaceFromCorners(), vpMbEdgeKltTracker::postTracking(), postTracking(), preTracking(), reinit(), resetTracker(), setCameraParameters(), setClipping(), setFarClippingDistance(), setNearClippingDistance(), vpMbEdgeKltTracker::setPose(), setPose(), testTracking(), vpMbEdgeKltTracker::track(), vpMbEdgeKltTracker::vpMbEdgeKltTracker(), and vpMbKltTracker().
|
protected |
Flag to specify whether the init method is called the first or not (specific calls to realize in this case).
Definition at line 241 of file vpMbKltTracker.h.
Referenced by resetTracker().
|
protected |
First track() called.
Definition at line 261 of file vpMbKltTracker.h.
Referenced by preTracking(), reinit(), resetTracker(), vpMbEdgeKltTracker::setPose(), and setPose().
|
protected |
The gain of the virtual visual servoing stage.
Definition at line 245 of file vpMbKltTracker.h.
Referenced by computeVVS(), and resetTracker().
|
protected |
Erosion of the mask.
Definition at line 243 of file vpMbKltTracker.h.
Referenced by loadConfigFile(), vpMbEdgeKltTracker::loadConfigFile(), reinit(), and resetTracker().
|
protected |
The maximum iteration of the virtual visual servoing stage.
Definition at line 247 of file vpMbKltTracker.h.
Referenced by computeVVS(), vpMbEdgeKltTracker::getMaxIter(), and resetTracker().
|
protectedinherited |
The name of the file containing the model (it is used to create a file name.0.pos used to store the compute pose in the initClick method).
Definition at line 110 of file vpMbTracker.h.
Referenced by vpMbTracker::loadModel().
|
protectedinherited |
Flag used to ensure that the CAD model is loaded before the initialisation.
Definition at line 112 of file vpMbTracker.h.
Referenced by init(), and vpMbTracker::loadModel().
|
protected |
Percentage of good points, according to the initial number, that must have the tracker.
Definition at line 251 of file vpMbKltTracker.h.
Referenced by postTracking(), and resetTracker().
|
protectedinherited |
Filename used to save the initial pose computed using the initClick() method. It is also used to read a previous pose in the same method.
Definition at line 114 of file vpMbTracker.h.
Referenced by vpMbTracker::initClick().
|
protected |
Threshold below which the weight associated to a point to consider this one as an outlier.
Definition at line 249 of file vpMbKltTracker.h.
Referenced by postTracking(), and resetTracker().
|
protected |
Points tracker.
Definition at line 257 of file vpMbKltTracker.h.
Referenced by getKltImagePoints(), getKltImagePointsWithId(), loadConfigFile(), vpMbEdgeKltTracker::loadConfigFile(), preTracking(), reinit(), resetTracker(), setKltOpencv(), setPose(), and vpMbKltTracker().
|
protected |
Use Ogre3d for visibility tests.
Definition at line 253 of file vpMbKltTracker.h.
Referenced by vpMbEdgeKltTracker::display(), display(), init(), postTracking(), resetTracker(), setOgreVisibilityTest(), and setPose().