 |
Visual Servoing Platform
version 3.3.0
|
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_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];
131 bool changed =
false;
143 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it =
m_depthDenseFaces.begin();
153 double normRes_1 = -1;
154 unsigned int iter = 0;
165 bool isoJoIdentity_ =
true;
172 bool reStartFromLastIncrement =
false;
175 if (!reStartFromLastIncrement) {
180 if (!isoJoIdentity_) {
188 isoJoIdentity_ =
true;
195 if (isoJoIdentity_) {
209 isoJoIdentity_ =
false;
214 double num = 0.0, den = 0.0;
222 for (
unsigned int j = 0; j < 6; j++) {
234 normRes = sqrt(num / den);
263 unsigned int start_index = 0;
276 start_index += error.getRows();
287 bool displayFullModel)
291 for (
size_t i = 0; i < models.size(); i++) {
302 bool displayFullModel)
306 for (
size_t i = 0; i < models.size(); i++) {
331 bool displayFullModel)
333 std::vector<std::vector<double> > models;
337 bool changed =
false;
347 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it =
m_depthDenseFaces.begin();
350 std::vector<std::vector<double> > modelLines = face_dense->
getModelForDisplay(width, height, cMo, cam, displayFullModel);
351 models.insert(models.end(), modelLines.begin(), modelLines.end());
363 bool reInitialisation =
false;
367 #ifdef VISP_HAVE_OGRE
392 #ifdef VISP_HAVE_PUGIXML
403 std::cout <<
" *********** Parsing XML for Mb Depth Dense Tracker ************ " << std::endl;
404 xmlp.
parse(configFile);
406 std::cerr <<
"Exception: " << e.
what() << std::endl;
428 std::cerr <<
"pugixml third-party is not properly built to read config file: " << configFile << std::endl;
448 #if defined(VISP_HAVE_PCL)
463 for (std::vector<vpMbtFaceDepthDense *>::iterator it =
m_depthDenseFaces.begin();
489 #ifdef VISP_HAVE_OGRE
500 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it =
m_depthDenseFaces.begin();
502 (*it)->setCameraParameters(cam);
508 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it =
m_depthDenseFaces.begin();
510 (*it)->setDepthDenseFilteringMaxDistance(maxDistance);
516 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it =
m_depthDenseFaces.begin();
518 (*it)->setDepthDenseFilteringMethod(method);
524 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it =
m_depthDenseFaces.begin();
526 (*it)->setDepthDenseFilteringMinDistance(minDistance);
532 if (occupancyRatio < 0.0 || occupancyRatio > 1.0) {
533 std::cerr <<
"occupancyRatio < 0.0 || occupancyRatio > 1.0" << std::endl;
537 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it =
m_depthDenseFaces.begin();
539 (*it)->setDepthDenseFilteringOccupancyRatio(occupancyRatio);
548 #if DEBUG_DISPLAY_DEPTH_DENSE
549 if (!m_debugDisp_depthDense->isInitialised()) {
550 m_debugImage_depthDense.resize(point_cloud->height, point_cloud->width);
551 m_debugDisp_depthDense->init(m_debugImage_depthDense, 50, 0,
"Debug display dense depth tracker");
554 m_debugImage_depthDense = 0;
555 std::vector<std::vector<vpImagePoint> > roiPts_vec;
558 for (std::vector<vpMbtFaceDepthDense *>::iterator it =
m_depthDenseFaces.begin();
563 #if DEBUG_DISPLAY_DEPTH_DENSE
564 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
567 #
if DEBUG_DISPLAY_DEPTH_DENSE
569 m_debugImage_depthDense, roiPts_vec_
575 #if DEBUG_DISPLAY_DEPTH_DENSE
576 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
582 #if DEBUG_DISPLAY_DEPTH_DENSE
585 for (
size_t i = 0; i < roiPts_vec.size(); i++) {
586 if (roiPts_vec[i].empty())
589 for (
size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
592 vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
606 #if DEBUG_DISPLAY_DEPTH_DENSE
607 if (!m_debugDisp_depthDense->isInitialised()) {
608 m_debugImage_depthDense.resize(height, width);
609 m_debugDisp_depthDense->init(m_debugImage_depthDense, 50, 0,
"Debug display dense depth tracker");
612 m_debugImage_depthDense = 0;
613 std::vector<std::vector<vpImagePoint> > roiPts_vec;
616 for (std::vector<vpMbtFaceDepthDense *>::iterator it =
m_depthDenseFaces.begin();
621 #if DEBUG_DISPLAY_DEPTH_DENSE
622 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
626 #
if DEBUG_DISPLAY_DEPTH_DENSE
628 m_debugImage_depthDense, roiPts_vec_
634 #if DEBUG_DISPLAY_DEPTH_DENSE
635 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
641 #if DEBUG_DISPLAY_DEPTH_DENSE
644 for (
size_t i = 0; i < roiPts_vec.size(); i++) {
645 if (roiPts_vec[i].empty())
648 for (
size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
651 vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
662 #ifdef VISP_HAVE_OGRE
680 #if defined(VISP_HAVE_PCL)
684 m_I.
resize(point_cloud->height, point_cloud->width);
694 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it =
m_depthDenseFaces.begin();
696 (*it)->setScanLineVisibilityTest(v);
702 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it =
m_depthDenseFaces.begin();
745 const double ,
const int ,
const std::string & )
751 const int ,
const std::string & )
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
vpMbHiddenFaces< vpMbtPolygon > m_depthDenseHiddenFacesDisplay
Set of faces describing the object used only for display with scan line.
void setAngleAppear(const double &aappear)
bool m_computeInteraction
bool m_useScanLine
Scan line visibility.
double distNearClip
Distance for near clipping.
static double sqr(double x)
vpMatrix m_L_depthDense
Interaction matrix.
vpHomogeneousMatrix m_cMo
The current pose.
void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces< vpMbtPolygon > *const faces, int polygon=-1, std::string name="")
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud, unsigned int stepX, unsigned int stepY, const vpImage< bool > *mask=NULL)
vpCameraParameters m_cam
The camera parameters.
void setAngleDisappear(const double &adisappear)
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
bool useOgre
Use Ogre3d for visibility tests.
virtual void computeVVSCheckLevenbergMarquardt(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)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
double m_distNearClip
Distance for near clipping.
static double rad(double deg)
std::vector< vpMbtFaceDepthDense * > m_depthDenseListOfActiveFaces
List of current active (visible and features extracted) faces.
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Generic class defining intrinsic camera parameters.
unsigned int getDepthDenseSamplingStepY() const
vpPlane m_planeObject
Plane equation described in the object frame.
double getAngleDisappear() const
vpCameraParameters m_cam
Camera intrinsic parameters.
static double deg(double rad)
virtual void computeVVSWeights()
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
virtual ~vpMbDepthDenseTracker()
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &error)
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
vpColVector m_w_depthDense
Robust weights.
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
virtual void setDepthDenseFilteringMinDistance(double minDistance)
Display for windows using GDI (available on any windows 32 platform).
void addFace(vpMbtPolygon &polygon, bool alreadyClose)
virtual void setDepthDenseFilteringMaxDistance(double maxDistance)
vpColVector m_weightedError_depthDense
Weighted error.
virtual void init(const vpImage< unsigned char > &I)
void setUseDepthDenseTracking(const std::string &name, const bool &useDepthDenseTracking)
std::string getName() const
virtual void computeVVSInteractionMatrixAndResidu()
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
bool getFovClipping() const
virtual void testTracking()
vpPoint * p
corners in the object frame
virtual void track(const vpImage< unsigned char > &)
virtual void resetTracker()
virtual void setDepthDenseFilteringOccupancyRatio(double occupancyRatio)
virtual void computeVVSInit()
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
double m_distFarClip
Distance for near clipping.
double m_lambda
Gain of the virtual visual servoing stage.
Implementation of column vector and the associated operations.
bool useScanLine
Use Scanline for visibility tests.
Parse an Xml file to extract configuration parameters of a mbtConfig object.
void setWindowName(const Ogre::String &n)
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
double getNearClippingDistance() const
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
Implementation of a matrix and operations on matrices.
unsigned int m_depthDenseSamplingStepY
Sampling step in y-direction.
Implementation of a polygon of the model used by the model-based tracker.
std::vector< vpMbtFaceDepthDense * > m_depthDenseFaces
List of faces.
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
void insert(unsigned int i, const vpColVector &v)
vpMbtTukeyEstimator< double > m_robust_depthDense
Tukey M-Estimator.
vpMbtPolygon * m_polygon
Polygon defining the face.
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
void computeVisibility(unsigned int width, unsigned int height)
static void display(const vpImage< unsigned char > &I)
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false)
double distFarClip
Distance for near clipping.
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
void setDepthDenseSamplingStep(unsigned int stepX, unsigned int stepY)
void parse(const std::string &filename)
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, 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)
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
vpMatrix oJo
The Degrees of Freedom to estimate.
const char * what() const
unsigned int getHeight() const
double getAngleAppear() const
virtual void loadConfigFile(const std::string &configFile)
static bool equal(double x, double y, double s=0.001)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
double angleAppears
Angle used to detect a face appearance.
void setDepthDenseSamplingStepY(unsigned int stepY)
void getCameraParameters(vpCameraParameters &cam) const
unsigned int getNbFeatures() const
unsigned int nbpt
Number of points used to define the polygon.
virtual void setScanLineVisibilityTest(const bool &v)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void setCameraParameters(const vpCameraParameters &cam)
static vpHomogeneousMatrix direct(const vpColVector &v)
unsigned int m_clippingFlag
Flags specifying which clipping to used.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
unsigned int getNbPoint() const
double getFarClippingDistance() const
bool hasFarClippingDistance() const
unsigned int getDepthDenseSamplingStepX() const
void setOgreShowConfigDialog(bool showConfigDialog)
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
void resize(unsigned int i, bool flagNullify=true)
bool hasNearClippingDistance() const
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
virtual void setOgreVisibilityTest(const bool &v)
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
unsigned int clippingFlag
Flags specifying which clipping to used.
virtual void setCameraParameters(const vpCameraParameters &camera)
unsigned int m_depthDenseSamplingStepX
Sampling step in x-direction.
virtual void setOgreVisibilityTest(const bool &v)
static void flush(const vpImage< unsigned char > &I)
vpAROgre * getOgreContext()
virtual void setNearClippingDistance(const double &dist)
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
vpColVector m_error_depthDense
(s - s*)
Class that defines what is a point.
Class to define colors available for display functionnalities.
unsigned int m_denseDepthNbFeatures
Nb features.
virtual void setDepthDenseFilteringMethod(int method)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setDepthDenseSamplingStepX(unsigned int stepX)
double m_stopCriteriaEpsilon
Epsilon threshold to stop the VVS optimization loop.
virtual void setScanLineVisibilityTest(const bool &v)
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
error that can be emited by ViSP classes.
void computeFov(const unsigned int &w, const unsigned int &h)
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
void setTracked(bool tracked)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
double angleDisappears
Angle used to detect a face disappearance.
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
unsigned int getWidth() const
unsigned int getRows() const