 |
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/vpMbDepthNormalTracker.h>
48 #include <visp3/mbt/vpMbtXmlGenericParser.h>
50 #if DEBUG_DISPLAY_DEPTH_NORMAL
51 #include <visp3/gui/vpDisplayGDI.h>
52 #include <visp3/gui/vpDisplayX.h>
57 m_depthNormalHiddenFacesDisplay(), m_depthNormalI_dummyVisibility(), m_depthNormalListOfActiveFaces(),
58 m_depthNormalListOfDesiredFeatures(), m_depthNormalFaces(), m_depthNormalPclPlaneEstimationMethod(2),
59 m_depthNormalPclPlaneEstimationRansacMaxIter(200), m_depthNormalPclPlaneEstimationRansacThreshold(0.001),
60 m_depthNormalSamplingStepX(2), m_depthNormalSamplingStepY(2), m_depthNormalUseRobust(false), m_error_depthNormal(),
61 m_L_depthNormal(), m_robust_depthNormal(), m_w_depthNormal(), m_weightedError_depthNormal()
62 #if DEBUG_DISPLAY_DEPTH_NORMAL
64 m_debugDisp_depthNormal(NULL), m_debugImage_depthNormal()
71 #if defined(VISP_HAVE_X11) && DEBUG_DISPLAY_DEPTH_NORMAL
73 #elif defined(VISP_HAVE_GDI) && DEBUG_DISPLAY_DEPTH_NORMAL
87 if (polygon.
nbpt < 3) {
110 for (
unsigned int i = 0; i < nbpt - 1; i++) {
124 pts[0] = polygon.
p[0];
125 pts[1] = polygon.
p[1];
126 pts[2] = polygon.
p[2];
136 bool changed =
false;
150 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
160 double normRes_1 = -1;
161 unsigned int iter = 0;
173 bool isoJoIdentity_ =
true;
180 bool reStartFromLastIncrement =
false;
183 if (!reStartFromLastIncrement) {
189 if (!isoJoIdentity_) {
198 isoJoIdentity_ =
true;
205 if (isoJoIdentity_) {
219 isoJoIdentity_ =
false;
224 double num = 0.0, den = 0.0;
232 for (
unsigned int j = 0; j < 6; j++) {
244 normRes = sqrt(num / den);
270 unsigned int cpt = 0;
275 (*it)->computeInteractionMatrix(
cMo, L_face, features_face);
288 const bool displayFullModel)
292 bool changed =
false;
302 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
305 face_normal->
display(I, cMo_, c, col, thickness, displayFullModel);
315 const bool displayFullModel)
319 bool changed =
false;
331 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
334 face_normal->
display(I, cMo_, c, col, thickness, displayFullModel);
348 bool reInitialisation =
false;
352 #ifdef VISP_HAVE_OGRE
377 #ifdef VISP_HAVE_XML2
392 std::cout <<
" *********** Parsing XML for Mb Depth Tracker ************ " << std::endl;
393 xmlp.
parse(configFile);
395 std::cerr <<
"Exception: " << e.
what() << std::endl;
421 std::cerr <<
"You need the libXML2 to read the config file " << configFile << std::endl;
441 #if defined(VISP_HAVE_PCL)
481 #ifdef VISP_HAVE_OGRE
492 #ifdef VISP_HAVE_OGRE
503 #if defined(VISP_HAVE_PCL)
517 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
519 (*it)->setScanLineVisibilityTest(v);
525 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
542 #if DEBUG_DISPLAY_DEPTH_NORMAL
543 if (!m_debugDisp_depthNormal->isInitialised()) {
544 m_debugImage_depthNormal.resize(point_cloud->height, point_cloud->width);
545 m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0,
"Debug display normal depth tracker");
548 m_debugImage_depthNormal = 0;
549 std::vector<std::vector<vpImagePoint> > roiPts_vec;
559 #if DEBUG_DISPLAY_DEPTH_NORMAL
560 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
564 #
if DEBUG_DISPLAY_DEPTH_NORMAL
566 m_debugImage_depthNormal, roiPts_vec_
573 #if DEBUG_DISPLAY_DEPTH_NORMAL
574 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
580 #if DEBUG_DISPLAY_DEPTH_NORMAL
583 for (
size_t i = 0; i < roiPts_vec.size(); i++) {
584 if (roiPts_vec[i].empty())
587 for (
size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
590 vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
600 const unsigned int height)
605 #if DEBUG_DISPLAY_DEPTH_NORMAL
606 if (!m_debugDisp_depthNormal->isInitialised()) {
607 m_debugImage_depthNormal.resize(height, width);
608 m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0,
"Debug display normal depth tracker");
611 m_debugImage_depthNormal = 0;
612 std::vector<std::vector<vpImagePoint> > roiPts_vec;
622 #if DEBUG_DISPLAY_DEPTH_NORMAL
623 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
628 #
if DEBUG_DISPLAY_DEPTH_NORMAL
630 m_debugImage_depthNormal, roiPts_vec_
637 #if DEBUG_DISPLAY_DEPTH_NORMAL
638 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
644 #if DEBUG_DISPLAY_DEPTH_NORMAL
647 for (
size_t i = 0; i < roiPts_vec.size(); i++) {
648 if (roiPts_vec[i].empty())
651 for (
size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
654 vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
666 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
668 (*it)->setCameraParameters(camera);
674 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
676 (*it)->setFaceCentroidMethod(method);
685 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
687 (*it)->setFeatureEstimationMethod(method);
695 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
697 (*it)->setPclPlaneEstimationMethod(method);
705 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
707 (*it)->setPclPlaneEstimationRansacMaxIter(maxIter);
715 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
717 (*it)->setPclPlaneEstimationRansacThreshold(thresold);
723 if (stepX == 0 || stepY == 0) {
724 std::cerr <<
"stepX and stepY must be greater than zero!" << std::endl;
753 const unsigned int height)
763 const double ,
const int ,
const std::string & )
769 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)
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)
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)
bool m_depthNormalUseRobust
If true, use Tukey robust M-Estimator.
bool m_computeInteraction
virtual void setDepthNormalPclPlaneEstimationRansacThreshold(const double thresold)
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
virtual void setDepthNormalSamplingStep(const unsigned int stepX, const unsigned int stepY)
void setDepthNormalPclPlaneEstimationRansacThreshold(const double threshold)
double distNearClip
Distance for near clipping.
vpImage< unsigned char > m_depthNormalI_dummyVisibility
Dummy image used to compute the visibility.
static double sqr(double x)
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)
virtual void track(const vpImage< unsigned char > &)
void setThreshold(const double noise_threshold)
void setAngleDisappear(const double &adisappear)
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
bool useOgre
Use Ogre3d for visibility tests.
vpColVector m_weightedError_depthNormal
Weighted error.
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)
static double rad(double deg)
vpHomogeneousMatrix cMo
The current pose.
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")
Generic class defining intrinsic camera parameters.
vpCameraParameters m_cam
Camera intrinsic parameters.
int getDepthNormalPclPlaneEstimationMethod() const
double getAngleDisappear() const
virtual void resetTracker()
void setDepthNormalSamplingStepX(const unsigned int stepX)
bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const unsigned int width, const unsigned int height, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud, vpColVector &desired_features, const unsigned int stepX, const unsigned int stepY, const vpImage< bool > *mask=NULL)
static double deg(double rad)
virtual void setDepthNormalPclPlaneEstimationMethod(const int method)
void setFeatureEstimationMethod(const vpFeatureEstimationType &method)
virtual void testTracking()
virtual void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
void setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
unsigned int getDepthNormalSamplingStepX() const
double m_distNearClip
Distance for near clipping.
void setDepthNormalPclPlaneEstimationMethod(const int method)
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.
Display for windows using GDI (available on any windows 32 platform).
vpMbtFaceDepthNormal::vpFeatureEstimationType m_depthNormalFeatureEstimationMethod
Method to estimate the desired features.
bool m_useScanLine
Scan line visibility.
vpColVector m_error_depthNormal
(s - s*)
void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
vpRobust m_robust_depthNormal
Robust.
unsigned int m_depthNormalSamplingStepY
Sampling step in y-direction.
std::string getName() const
vpColVector m_w_depthNormal
Robust weights.
virtual void loadConfigFile(const std::string &configFile)
virtual void computeVVSInit()
bool getFovClipping() const
virtual void initFaceFromLines(vpMbtPolygon &polygon)
vpMbtFaceDepthNormal::vpFeatureEstimationType getDepthNormalFeatureEstimationMethod() const
std::vector< vpMbtFaceDepthNormal * > m_depthNormalFaces
List of faces.
void setPclPlaneEstimationRansacMaxIter(const int maxIter)
vpPoint * p
corners in the object frame
int m_depthNormalPclPlaneEstimationMethod
PCL plane estimation method.
double getDepthNormalPclPlaneEstimationRansacThreshold() const
vpCameraParameters cam
The camera parameters.
void setUseDepthNormalTracking(const std::string &name, const bool &useDepthNormalTracking)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
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.
unsigned int getDepthNormalSamplingStepY() const
void setWindowName(const Ogre::String &n)
std::vector< vpColVector > m_depthNormalListOfDesiredFeatures
List of desired features.
vpMbtPolygon * m_polygon
Polygon defining the face.
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
int getDepthNormalPclPlaneEstimationRansacMaxIter() const
double getNearClippingDistance() const
Implementation of a matrix and operations on matrices.
Implementation of a polygon of the model used by the model-based tracker.
virtual void setScanLineVisibilityTest(const bool &v)
void insert(unsigned int i, const vpColVector &v)
int m_depthNormalPclPlaneEstimationRansacMaxIter
PCL RANSAC maximum number of iterations.
virtual void setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method)
unsigned int m_clippingFlag
Flags specifying which clipping to used.
double m_distFarClip
Distance for near clipping.
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
static void display(const vpImage< unsigned char > &I)
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=0, const std::string &name="")
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
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)
virtual void setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter)
vpMatrix oJo
The Degrees of Freedom to estimate.
void computeVisibility(const unsigned int width, const unsigned int height)
const char * what() const
unsigned int getHeight() const
double getAngleAppear() const
void displayFeature(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const double scale=0.05, const unsigned int thickness=1)
double m_depthNormalPclPlaneEstimationRansacThreshold
PCL RANSAC threshold.
virtual void setCameraParameters(const vpCameraParameters &camera)
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
void setPclPlaneEstimationMethod(const int method)
virtual ~vpMbDepthNormalTracker()
virtual void init(const vpImage< unsigned char > &I)
double angleAppears
Angle used to detect a face appearance.
void setTracked(const bool tracked)
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)
unsigned int nbpt
Number of points used to define the polygon.
void addFace(vpMbtPolygon &polygon, const bool alreadyClose)
vpMatrix m_L_depthNormal
Interaction matrix.
void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces< vpMbtPolygon > *const faces, int polygon=-1, 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)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false)
unsigned int getNbPoint() const
double getFarClippingDistance() const
bool hasFarClippingDistance() 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 computeVVSInteractionMatrixAndResidu()
void resize(unsigned int n_data)
Resize containers for sort methods.
virtual void setOgreVisibilityTest(const bool &v)
vpPlane m_planeObject
Plane equation described in the object frame.
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 m_depthNormalSamplingStepX
Sampling step in x-direction.
unsigned int clippingFlag
Flags specifying which clipping to used.
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 setDepthNormalSamplingStepY(const unsigned int stepY)
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 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.
virtual void setScanLineVisibilityTest(const bool &v)
std::vector< vpMbtFaceDepthNormal * > m_depthNormalListOfActiveFaces
List of current active (visible and with features extracted) faces.
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
vpMbHiddenFaces< vpMbtPolygon > m_depthNormalHiddenFacesDisplay
Set of faces describing the object used only for display with scan line.
virtual void setOgreVisibilityTest(const bool &v)
error that can be emited by ViSP classes.
void computeFov(const unsigned int &w, const unsigned int &h)
void setPclPlaneEstimationRansacThreshold(const double threshold)
double angleDisappears
Angle used to detect a face disappearance.
unsigned int getWidth() const
unsigned int getRows() const