38 #include <visp3/core/vpConfig.h> 41 #include <pcl/point_cloud.h> 44 #include <visp3/core/vpDisplay.h> 45 #include <visp3/core/vpExponentialMap.h> 46 #include <visp3/core/vpTrackingException.h> 47 #include <visp3/mbt/vpMbDepthDenseTracker.h> 48 #include <visp3/mbt/vpMbtXmlGenericParser.h> 50 #if DEBUG_DISPLAY_DEPTH_DENSE 51 #include <visp3/gui/vpDisplayGDI.h> 52 #include <visp3/gui/vpDisplayX.h> 56 : m_depthDenseHiddenFacesDisplay(), m_depthDenseI_dummyVisibility(), m_depthDenseListOfActiveFaces(),
57 m_denseDepthNbFeatures(0), m_depthDenseFaces(), m_depthDenseSamplingStepX(2), m_depthDenseSamplingStepY(2),
58 m_error_depthDense(), m_L_depthDense(), m_robust_depthDense(), m_w_depthDense(), m_weightedError_depthDense()
59 #if DEBUG_DISPLAY_DEPTH_DENSE
61 m_debugDisp_depthDense(NULL), m_debugImage_depthDense()
68 #if defined(VISP_HAVE_X11) && DEBUG_DISPLAY_DEPTH_DENSE 70 #elif defined(VISP_HAVE_GDI) && DEBUG_DISPLAY_DEPTH_DENSE 81 #if DEBUG_DISPLAY_DEPTH_DENSE 82 delete m_debugDisp_depthDense;
88 if (polygon.
nbpt < 3) {
107 for (
unsigned int i = 0; i < nbpt - 1; i++) {
121 pts[0] = polygon.
p[0];
122 pts[1] = polygon.
p[1];
123 pts[2] = polygon.
p[2];
133 bool changed =
false;
147 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it =
m_depthDenseFaces.begin();
157 double normRes_1 = -1;
158 unsigned int iter = 0;
169 bool isoJoIdentity_ =
true;
176 bool reStartFromLastIncrement =
false;
179 if (!reStartFromLastIncrement) {
184 if (!isoJoIdentity_) {
193 isoJoIdentity_ =
true;
200 if (isoJoIdentity_) {
214 isoJoIdentity_ =
false;
219 double num = 0.0, den = 0.0;
227 for (
unsigned int j = 0; j < 6; j++) {
239 normRes = sqrt(num / den);
268 unsigned int start_index = 0;
281 start_index += error.
getRows();
292 const bool displayFullModel)
296 bool changed =
false;
306 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it =
m_depthDenseFaces.begin();
309 face_normal->
display(I, cMo_, c, col, thickness, displayFullModel);
319 const bool displayFullModel)
323 bool changed =
false;
335 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it =
m_depthDenseFaces.begin();
338 face_normal->
display(I, cMo_, c, col, thickness, displayFullModel);
352 bool reInitialisation =
false;
356 #ifdef VISP_HAVE_OGRE 381 #ifdef VISP_HAVE_XML2 392 std::cout <<
" *********** Parsing XML for Mb Depth Dense Tracker ************ " << std::endl;
393 xmlp.
parse(configFile);
395 std::cerr <<
"Exception: " << e.
what() << std::endl;
417 std::cerr <<
"You need the libXML2 to read the config file " << configFile << std::endl;
437 #if defined(VISP_HAVE_PCL) 452 for (std::vector<vpMbtFaceDepthDense *>::iterator it =
m_depthDenseFaces.begin();
478 #ifdef VISP_HAVE_OGRE 488 #ifdef VISP_HAVE_OGRE 499 #if defined(VISP_HAVE_PCL) 513 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it =
m_depthDenseFaces.begin();
515 (*it)->setScanLineVisibilityTest(v);
521 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it =
m_depthDenseFaces.begin();
537 #if DEBUG_DISPLAY_DEPTH_DENSE 538 if (!m_debugDisp_depthDense->isInitialised()) {
539 m_debugImage_depthDense.resize(point_cloud->height, point_cloud->width);
540 m_debugDisp_depthDense->init(m_debugImage_depthDense, 50, 0,
"Debug display dense depth tracker");
543 m_debugImage_depthDense = 0;
544 std::vector<std::vector<vpImagePoint> > roiPts_vec;
547 for (std::vector<vpMbtFaceDepthDense *>::iterator it =
m_depthDenseFaces.begin();
552 #if DEBUG_DISPLAY_DEPTH_DENSE 553 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
556 #
if DEBUG_DISPLAY_DEPTH_DENSE
558 m_debugImage_depthDense, roiPts_vec_
564 #if DEBUG_DISPLAY_DEPTH_DENSE 565 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
571 #if DEBUG_DISPLAY_DEPTH_DENSE 574 for (
size_t i = 0; i < roiPts_vec.size(); i++) {
575 if (roiPts_vec[i].empty())
578 for (
size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
581 vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
591 const unsigned int height)
595 #if DEBUG_DISPLAY_DEPTH_DENSE 596 if (!m_debugDisp_depthDense->isInitialised()) {
597 m_debugImage_depthDense.resize(height, width);
598 m_debugDisp_depthDense->init(m_debugImage_depthDense, 50, 0,
"Debug display dense depth tracker");
601 m_debugImage_depthDense = 0;
602 std::vector<std::vector<vpImagePoint> > roiPts_vec;
605 for (std::vector<vpMbtFaceDepthDense *>::iterator it =
m_depthDenseFaces.begin();
610 #if DEBUG_DISPLAY_DEPTH_DENSE 611 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
615 #
if DEBUG_DISPLAY_DEPTH_DENSE
617 m_debugImage_depthDense, roiPts_vec_
623 #if DEBUG_DISPLAY_DEPTH_DENSE 624 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
630 #if DEBUG_DISPLAY_DEPTH_DENSE 633 for (
size_t i = 0; i < roiPts_vec.size(); i++) {
634 if (roiPts_vec[i].empty())
637 for (
size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
640 vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
652 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it =
m_depthDenseFaces.begin();
654 (*it)->setCameraParameters(camera);
660 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it =
m_depthDenseFaces.begin();
662 (*it)->setDepthDenseFilteringMaxDistance(maxDistance);
668 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it =
m_depthDenseFaces.begin();
670 (*it)->setDepthDenseFilteringMethod(method);
676 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it =
m_depthDenseFaces.begin();
678 (*it)->setDepthDenseFilteringMinDistance(minDistance);
684 if (occupancyRatio < 0.0 || occupancyRatio > 1.0) {
685 std::cerr <<
"occupancyRatio < 0.0 || occupancyRatio > 1.0" << std::endl;
689 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it =
m_depthDenseFaces.begin();
691 (*it)->setDepthDenseFilteringOccupancyRatio(occupancyRatio);
712 const unsigned int height)
722 const double ,
const int ,
const std::string & )
728 const int ,
const std::string & )
unsigned int getNbFeatures() const
bool m_computeInteraction
void setWindowName(const Ogre::String &n)
vpMbtTukeyEstimator< double > m_robust_depthDense
Tukey M-Estimator.
Implementation of a matrix and operations on matrices.
double m_distNearClip
Distance for near clipping.
virtual void setScanLineVisibilityTest(const bool &v)
vpMbHiddenFaces< vpMbtPolygon > m_depthDenseHiddenFacesDisplay
Set of faces describing the object used only for display with scan line.
double getFarClippingDistance() const
virtual void computeVVSWeights()
void setTracked(const bool tracked)
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")
unsigned int getWidth() const
vpCameraParameters m_cam
Camera intrinsic parameters.
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
double getNearClippingDistance() const
vpColVector m_weightedError_depthDense
Weighted error.
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual void loadConfigFile(const std::string &configFile)
virtual ~vpMbDepthDenseTracker()
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
void getCameraParameters(vpCameraParameters &_cam) const
virtual void track(const vpImage< unsigned char > &)
bool hasNearClippingDistance() const
Display for windows using GDI (available on any windows 32 platform).
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.
double getAngleAppear() const
bool m_useScanLine
Scan line visibility.
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
vpHomogeneousMatrix cMo
The current pose.
vpColVector m_error_depthDense
(s - s*)
void setOgreShowConfigDialog(const bool showConfigDialog)
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
error that can be emited by ViSP classes.
vpPoint * p
corners in the object frame
bool useOgre
Use Ogre3d for visibility tests.
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 initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
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.
void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces< vpMbtPolygon > *const faces, int polygon=-1, std::string name="")
static void flush(const vpImage< unsigned char > &I)
void addFace(vpMbtPolygon &polygon, const bool alreadyClose)
Class that defines what is a point.
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)
vpMbtPolygon * m_polygon
Polygon defining the face.
vpPlane m_planeObject
Plane equation described in the object frame.
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
double distFarClip
Distance for near clipping.
vpAROgre * getOgreContext()
const char * what() const
bool hasFarClippingDistance() const
unsigned int m_denseDepthNbFeatures
Nb features.
void setDepthDenseSamplingStepX(const unsigned int stepX)
void insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
vpMatrix oJo
The Degrees of Freedom to estimate.
Implementation of a polygon of the model used by the model-based tracker.
void setAngleDisappear(const double &adisappear)
unsigned int m_clippingFlag
Flags specifying which clipping to used.
vpMatrix m_L_depthDense
Interaction matrix.
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)
std::string getName() const
virtual void setDepthDenseFilteringOccupancyRatio(const double occupancyRatio)
bool useScanLine
Use Scanline for visibility tests.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double sqr(double x)
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &error)
static void display(const vpImage< unsigned char > &I)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
double m_distFarClip
Distance for near clipping.
Generic class defining intrinsic camera parameters.
virtual void setDepthDenseFilteringMethod(const int method)
void resize(const unsigned int h, const unsigned int w)
resize the image : Image initialization
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
double m_lambda
Gain of the virtual visual servoing stage.
unsigned int m_depthDenseSamplingStepX
Sampling step in x-direction.
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
unsigned int getRows() const
vpImage< unsigned char > m_depthDenseI_dummyVisibility
Dummy image used to compute the visibility.
double getAngleDisappear() const
double angleAppears
Angle used to detect a face appearance.
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
virtual void testTracking()
void setAngleAppear(const double &aappear)
static double rad(double deg)
virtual void setOgreVisibilityTest(const bool &v)
void insert(unsigned int i, const vpColVector &v)
void displayFeature(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const double scale=0.05, const unsigned int thickness=1)
void setUseDepthDenseTracking(const std::string &name, const bool &useDepthDenseTracking)
virtual void computeVVSInit()
virtual void resetTracker()
virtual void setDepthDenseFilteringMaxDistance(const double maxDistance)
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
void setDepthDenseSamplingStepY(const unsigned int stepY)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
unsigned int nbpt
Number of points used to define the polygon.
double m_stopCriteriaEpsilon
Epsilon threshold to stop the VVS optimization loop.
static double deg(double rad)
void computeVisibility(const unsigned int width, const unsigned int height)
virtual void setOgreVisibilityTest(const bool &v)
bool displayFeatures
If true, the features are displayed.
bool ogreShowConfigDialog
Implementation of column vector and the associated operations.
void setDepthDenseSamplingStep(const unsigned int stepX, const unsigned int stepY)
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=0, const std::string &name="")
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)
vpHomogeneousMatrix inverse() const
unsigned int getDepthDenseSamplingStepX() const
vpColVector m_w_depthDense
Robust weights.
static vpHomogeneousMatrix direct(const vpColVector &v)
std::vector< vpMbtFaceDepthDense * > m_depthDenseFaces
List of faces.
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 void computeVVSInteractionMatrixAndResidu()
unsigned int getDepthDenseSamplingStepY() const
unsigned int getHeight() const
unsigned int getNbPoint() const
void setCameraParameters(const vpCameraParameters &_cam)
bool getFovClipping() const
virtual void setScanLineVisibilityTest(const bool &v)
virtual void setClipping(const unsigned int &flags)
virtual void init(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
This class defines the container for a plane geometrical structure.
unsigned int clippingFlag
Flags specifying which clipping to used.
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void setFarClippingDistance(const double &dist)
virtual void setCameraParameters(const vpCameraParameters &camera)
double distNearClip
Distance for near clipping.
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
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)
virtual void setDepthDenseFilteringMinDistance(const double minDistance)
void resize(const unsigned int i, const bool flagNullify=true)
bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud, const unsigned int stepX, const unsigned int stepY, const vpImage< bool > *mask=NULL)
unsigned int m_depthDenseSamplingStepY
Sampling step in y-direction.
virtual void setNearClippingDistance(const double &dist)
void parse(const std::string &filename)
void computeFov(const unsigned int &w, const unsigned int &h)
std::vector< vpMbtFaceDepthDense * > m_depthDenseListOfActiveFaces
List of current active (visible and features extracted) faces.