 |
Visual Servoing Platform
version 3.2.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_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 & )
void setOgreShowConfigDialog(const bool showConfigDialog)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
void setCameraParameters(const vpCameraParameters &_cam)
vpMbHiddenFaces< vpMbtPolygon > m_depthDenseHiddenFacesDisplay
Set of faces describing the object used only for display with scan line.
void setAngleAppear(const double &aappear)
void getCameraParameters(vpCameraParameters &_cam) const
void resize(const unsigned int i, const bool flagNullify=true)
void parse(const std::string &filename)
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.
void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces< vpMbtPolygon > *const faces, int polygon=-1, std::string name="")
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 setVisibleOgre(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
void setAngleDisappear(const double &adisappear)
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
bool useOgre
Use Ogre3d for visibility tests.
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
double m_distNearClip
Distance for near clipping.
static double rad(double deg)
vpHomogeneousMatrix cMo
The current pose.
std::vector< vpMbtFaceDepthDense * > m_depthDenseListOfActiveFaces
List of current active (visible and features extracted) faces.
Generic class defining intrinsic camera parameters.
unsigned int getDepthDenseSamplingStepY() const
vpPlane m_planeObject
Plane equation described in the object frame.
double getAngleDisappear() const
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
virtual void setDepthDenseFilteringMethod(const int method)
vpCameraParameters m_cam
Camera intrinsic parameters.
virtual void setDepthDenseFilteringMinDistance(const double minDistance)
static double deg(double rad)
virtual void computeVVSWeights()
void setTracked(const bool tracked)
virtual ~vpMbDepthDenseTracker()
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &error)
vpColVector m_w_depthDense
Robust weights.
vpImage< unsigned char > m_depthDenseI_dummyVisibility
Dummy image used to compute the visibility.
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)
Display for windows using GDI (available on any windows 32 platform).
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
void addFace(vpMbtPolygon &polygon, const bool alreadyClose)
virtual void testTracking()
vpPoint * p
corners in the object frame
virtual void track(const vpImage< unsigned char > &)
virtual void resetTracker()
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 initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")
vpCameraParameters cam
The camera parameters.
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 computeVisibility(const unsigned int width, const unsigned int height)
void setWindowName(const Ogre::String &n)
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
double getNearClippingDistance() const
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.
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.
static void display(const vpImage< unsigned char > &I)
virtual void setDepthDenseFilteringOccupancyRatio(const double occupancyRatio)
double distFarClip
Distance for near clipping.
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
bool displayFeatures
If true, the features are displayed.
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)
void setDepthDenseSamplingStepY(const unsigned int stepY)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
double angleAppears
Angle used to detect a face appearance.
unsigned int getNbFeatures() const
unsigned int nbpt
Number of points used to define the polygon.
virtual void setScanLineVisibilityTest(const bool &v)
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)
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
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
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
void setDepthDenseSamplingStep(const unsigned int stepX, const unsigned int stepY)
virtual void setDepthDenseFilteringMaxDistance(const double maxDistance)
vpColVector m_error_depthDense
(s - s*)
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.
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
unsigned int m_denseDepthNbFeatures
Nb features.
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.
double m_stopCriteriaEpsilon
Epsilon threshold to stop the VVS optimization loop.
void setDepthDenseSamplingStepX(const unsigned int stepX)
virtual void setScanLineVisibilityTest(const bool &v)
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
void resize(const unsigned int h, const unsigned int w)
resize the image : Image initialization
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)
error that can be emited by ViSP classes.
void computeFov(const unsigned int &w, const unsigned int &h)
void displayFeature(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const double scale=0.05, const unsigned int thickness=1)
double angleDisappears
Angle used to detect a face disappearance.
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false)
unsigned int getWidth() const
unsigned int getRows() const