38 #include <visp3/core/vpConfig.h>
40 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
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>
58 m_depthNormalHiddenFacesDisplay(), m_depthNormalListOfActiveFaces(), m_depthNormalListOfDesiredFeatures(),
59 m_depthNormalFaces(), m_depthNormalPclPlaneEstimationMethod(2), m_depthNormalPclPlaneEstimationRansacMaxIter(200),
60 m_depthNormalPclPlaneEstimationRansacThreshold(0.001), m_depthNormalSamplingStepX(2), m_depthNormalSamplingStepY(2),
61 m_depthNormalUseRobust(false), m_error_depthNormal(), m_featuresToBeDisplayedDepthNormal(), m_L_depthNormal(),
62 m_robust_depthNormal(), m_w_depthNormal(), m_weightedError_depthNormal()
63 #if DEBUG_DISPLAY_DEPTH_NORMAL
65 m_debugDisp_depthNormal(nullptr), m_debugImage_depthNormal()
72 #if defined(VISP_HAVE_X11) && DEBUG_DISPLAY_DEPTH_NORMAL
74 #elif defined(VISP_HAVE_GDI) && DEBUG_DISPLAY_DEPTH_NORMAL
88 if (polygon.
nbpt < 3) {
111 for (
unsigned int i = 0; i < nbpt - 1; i++) {
125 pts[0] = polygon.
p[0];
126 pts[1] = polygon.
p[1];
127 pts[2] = polygon.
p[2];
135 bool changed =
false;
147 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
157 double normRes_1 = -1;
158 unsigned int iter = 0;
180 bool reStartFromLastIncrement =
false;
183 if (!reStartFromLastIncrement) {
189 if (!isoJoIdentity) {
215 isoJoIdentity =
false;
220 double num = 0.0, den = 0.0;
228 for (
unsigned int j = 0; j < 6; j++) {
240 normRes = sqrt(num / den);
265 unsigned int cpt = 0;
270 (*it)->computeInteractionMatrix(
m_cMo, L_face, features_face);
283 bool displayFullModel)
285 std::vector<std::vector<double> > models =
288 for (
size_t i = 0; i < models.size(); i++) {
298 for (
size_t i = 0; i < features.size(); i++) {
299 vpImagePoint im_centroid(features[i][1], features[i][2]);
300 vpImagePoint im_extremity(features[i][3], features[i][4]);
309 bool displayFullModel)
311 std::vector<std::vector<double> > models =
314 for (
size_t i = 0; i < models.size(); i++) {
324 for (
size_t i = 0; i < features.size(); i++) {
325 vpImagePoint im_centroid(features[i][1], features[i][2]);
326 vpImagePoint im_extremity(features[i][3], features[i][4]);
335 std::vector<std::vector<double> > features;
337 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
341 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
365 bool displayFullModel)
367 std::vector<std::vector<double> > models;
371 bool changed =
false;
381 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
384 std::vector<std::vector<double> > modelLines =
386 models.insert(models.end(), modelLines.begin(), modelLines.end());
398 bool reInitialisation =
false;
403 #ifdef VISP_HAVE_OGRE
428 #if defined(VISP_HAVE_PUGIXML)
444 std::cout <<
" *********** Parsing XML for Mb Depth Tracker ************ " << std::endl;
446 xmlp.
parse(configFile);
449 std::cerr <<
"Exception: " << e.
what() << std::endl;
497 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
515 normal_face =
nullptr;
536 #ifdef VISP_HAVE_OGRE
547 #ifdef VISP_HAVE_OGRE
565 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
579 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
581 (*it)->setScanLineVisibilityTest(v);
587 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
598 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
604 #if DEBUG_DISPLAY_DEPTH_NORMAL
605 if (!m_debugDisp_depthNormal->isInitialised()) {
606 m_debugImage_depthNormal.resize(point_cloud->height, point_cloud->width);
607 m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0,
"Debug display normal depth tracker");
610 m_debugImage_depthNormal = 0;
611 std::vector<std::vector<vpImagePoint> > roiPts_vec;
621 #if DEBUG_DISPLAY_DEPTH_NORMAL
622 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
626 #
if DEBUG_DISPLAY_DEPTH_NORMAL
628 m_debugImage_depthNormal, roiPts_vec_
635 #if DEBUG_DISPLAY_DEPTH_NORMAL
636 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
642 #if DEBUG_DISPLAY_DEPTH_NORMAL
645 for (
size_t i = 0; i < roiPts_vec.size(); i++) {
646 if (roiPts_vec[i].empty())
649 for (
size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
652 vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
667 #if DEBUG_DISPLAY_DEPTH_NORMAL
668 if (!m_debugDisp_depthNormal->isInitialised()) {
669 m_debugImage_depthNormal.resize(height, width);
670 m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0,
"Debug display normal depth tracker");
673 m_debugImage_depthNormal = 0;
674 std::vector<std::vector<vpImagePoint> > roiPts_vec;
684 #if DEBUG_DISPLAY_DEPTH_NORMAL
685 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
690 #
if DEBUG_DISPLAY_DEPTH_NORMAL
692 m_debugImage_depthNormal, roiPts_vec_
699 #if DEBUG_DISPLAY_DEPTH_NORMAL
700 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
706 #if DEBUG_DISPLAY_DEPTH_NORMAL
709 for (
size_t i = 0; i < roiPts_vec.size(); i++) {
710 if (roiPts_vec[i].empty())
713 for (
size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
716 vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
730 #if DEBUG_DISPLAY_DEPTH_NORMAL
731 if (!m_debugDisp_depthNormal->isInitialised()) {
732 m_debugImage_depthNormal.resize(height, width);
733 m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0,
"Debug display normal depth tracker");
736 m_debugImage_depthNormal = 0;
737 std::vector<std::vector<vpImagePoint> > roiPts_vec;
747 #if DEBUG_DISPLAY_DEPTH_NORMAL
748 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
753 #
if DEBUG_DISPLAY_DEPTH_NORMAL
755 m_debugImage_depthNormal, roiPts_vec_
762 #if DEBUG_DISPLAY_DEPTH_NORMAL
763 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
769 #if DEBUG_DISPLAY_DEPTH_NORMAL
772 for (
size_t i = 0; i < roiPts_vec.size(); i++) {
773 if (roiPts_vec[i].empty())
776 for (
size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
779 vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
791 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
793 (*it)->setCameraParameters(cam);
799 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
801 (*it)->setFaceCentroidMethod(method);
810 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
812 (*it)->setFeatureEstimationMethod(method);
820 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
822 (*it)->setPclPlaneEstimationMethod(method);
830 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
832 (*it)->setPclPlaneEstimationRansacMaxIter(maxIter);
840 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
842 (*it)->setPclPlaneEstimationRansacThreshold(threshold);
848 if (stepX == 0 || stepY == 0) {
849 std::cerr <<
"stepX and stepY must be greater than zero!" << std::endl;
871 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
892 double ,
int ,
const std::string & )
898 int ,
const std::string & )
void setWindowName(const Ogre::String &n)
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
unsigned int getRows() const
Generic class defining intrinsic camera parameters.
void computeFov(const unsigned int &w, const unsigned int &h)
Implementation of column vector and the associated operations.
void insert(unsigned int i, const vpColVector &v)
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionalities.
static const vpColor blue
Display for windows using GDI (available on any windows 32 platform).
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
static void display(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, bool segment=true)
static void flush(const vpImage< unsigned char > &I)
static void displayArrow(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color=vpColor::white, unsigned int w=4, unsigned int h=2, unsigned int thickness=1)
error that can be emitted by ViSP classes.
const char * what() const
static vpHomogeneousMatrix direct(const vpColVector &v)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
unsigned int getWidth() const
unsigned int getHeight() const
static double rad(double deg)
static double sqr(double x)
static bool equal(double x, double y, double threshold=0.001)
static double deg(double rad)
Implementation of a matrix and operations on matrices.
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
virtual void computeVVSInteractionMatrixAndResidu() VP_OVERRIDE
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="") VP_OVERRIDE
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false) VP_OVERRIDE
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="") VP_OVERRIDE
vpMatrix m_L_depthNormal
Interaction matrix.
vpRobust m_robust_depthNormal
Robust.
virtual void setDepthNormalSamplingStep(unsigned int stepX, unsigned int stepY)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false) VP_OVERRIDE
vpMbtFaceDepthNormal::vpFeatureEstimationType m_depthNormalFeatureEstimationMethod
Method to estimate the desired features.
std::vector< vpMbtFaceDepthNormal * > m_depthNormalFaces
List of faces.
int m_depthNormalPclPlaneEstimationRansacMaxIter
PCL RANSAC maximum number of iterations.
unsigned int m_depthNormalSamplingStepY
Sampling step in y-direction.
virtual void setScanLineVisibilityTest(const bool &v) VP_OVERRIDE
double m_depthNormalPclPlaneEstimationRansacThreshold
PCL RANSAC threshold.
virtual void setDepthNormalPclPlaneEstimationRansacThreshold(double thresold)
virtual void resetTracker() VP_OVERRIDE
void addFace(vpMbtPolygon &polygon, bool alreadyClose)
bool m_depthNormalUseRobust
If true, use Tukey robust M-Estimator.
virtual void track(const vpImage< unsigned char > &) VP_OVERRIDE
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false)
virtual std::vector< std::vector< double > > getFeaturesForDisplayDepthNormal()
vpColVector m_w_depthNormal
Robust weights.
virtual void init(const vpImage< unsigned char > &I) VP_OVERRIDE
vpColVector m_error_depthNormal
(s - s*)
vpColVector m_weightedError_depthNormal
Weighted error.
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo) VP_OVERRIDE
virtual void setOgreVisibilityTest(const bool &v) VP_OVERRIDE
std::vector< vpMbtFaceDepthNormal * > m_depthNormalListOfActiveFaces
List of current active (visible and with features extracted) faces.
virtual void loadConfigFile(const std::string &configFile, bool verbose=true) VP_OVERRIDE
virtual void initFaceFromCorners(vpMbtPolygon &polygon) VP_OVERRIDE
virtual void setCameraParameters(const vpCameraParameters &camera) VP_OVERRIDE
int m_depthNormalPclPlaneEstimationMethod
PCL plane estimation method.
virtual void setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method)
virtual void initFaceFromLines(vpMbtPolygon &polygon) VP_OVERRIDE
virtual void setDepthNormalPclPlaneEstimationMethod(int method)
std::vector< vpColVector > m_depthNormalListOfDesiredFeatures
List of desired features.
virtual void computeVVSInit() VP_OVERRIDE
virtual void testTracking() VP_OVERRIDE
void setUseDepthNormalTracking(const std::string &name, const bool &useDepthNormalTracking)
void computeVisibility(unsigned int width, unsigned int height)
vpMbHiddenFaces< vpMbtPolygon > m_depthNormalHiddenFacesDisplay
Set of faces describing the object used only for display with scan line.
virtual void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
virtual void setDepthNormalPclPlaneEstimationRansacMaxIter(int maxIter)
virtual ~vpMbDepthNormalTracker() VP_OVERRIDE
unsigned int m_depthNormalSamplingStepX
Sampling step in x-direction.
vpAROgre * getOgreContext()
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
void setOgreShowConfigDialog(bool showConfigDialog)
double m_lambda
Gain of the virtual visual servoing stage.
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
bool m_computeInteraction
vpMatrix oJo
The Degrees of Freedom to estimate.
vpUniRand m_rand
Random number generator used in vpMbtDistanceLine::buildFrom()
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
vpHomogeneousMatrix m_cMo
The current pose.
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=nullptr, const vpColVector *const m_w_prev=nullptr)
vpCameraParameters m_cam
The camera parameters.
double m_stopCriteriaEpsilon
Epsilon threshold to stop the VVS optimization loop.
bool useOgre
Use Ogre3d for visibility tests.
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
virtual void setScanLineVisibilityTest(const bool &v)
virtual void setOgreVisibilityTest(const bool &v)
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
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=nullptr, vpColVector *const m_w_prev=nullptr)
bool displayFeatures
If true, the features are displayed.
double angleDisappears
Angle used to detect a face disappearance.
virtual void setNearClippingDistance(const double &dist)
virtual void setFarClippingDistance(const double &dist)
double distFarClip
Distance for near clipping.
bool m_isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
bool useScanLine
Use Scanline for visibility tests.
virtual void setClipping(const unsigned int &flags)
double angleAppears
Angle used to detect a face appearance.
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
double distNearClip
Distance for near clipping.
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.
double m_distNearClip
Distance for near clipping.
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces< vpMbtPolygon > *const faces, vpUniRand &rand_gen, int polygon=-1, std::string name="")
double m_distFarClip
Distance for near clipping.
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
std::vector< std::vector< double > > getFeaturesForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double scale=0.05)
void setPclPlaneEstimationMethod(int method)
void setPclPlaneEstimationRansacThreshold(double threshold)
vpMbtPolygon * m_polygon
Polygon defining the face.
vpCameraParameters m_cam
Camera intrinsic parameters.
vpPlane m_planeObject
Plane equation described in the object frame.
unsigned int m_clippingFlag
Flags specifying which clipping to used.
void setTracked(bool tracked)
void setFeatureEstimationMethod(const vpFeatureEstimationType &method)
void setPclPlaneEstimationRansacMaxIter(int maxIter)
bool m_useScanLine
Scan line visibility.
bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud, vpColVector &desired_features, unsigned int stepX, unsigned int stepY, const vpImage< bool > *mask=nullptr)
Implementation of a polygon of the model used by the model-based tracker.
std::string getName() const
Parse an Xml file to extract configuration parameters of a mbtConfig object.
void setDepthNormalPclPlaneEstimationRansacMaxIter(int maxIter)
int getDepthNormalPclPlaneEstimationRansacMaxIter() const
unsigned int getDepthNormalSamplingStepX() const
void getCameraParameters(vpCameraParameters &cam) const
double getAngleAppear() const
unsigned int getDepthNormalSamplingStepY() const
void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
vpMbtFaceDepthNormal::vpFeatureEstimationType getDepthNormalFeatureEstimationMethod() const
void setDepthNormalPclPlaneEstimationMethod(int method)
void setDepthNormalSamplingStepX(unsigned int stepX)
void setAngleDisappear(const double &adisappear)
double getAngleDisappear() const
int getDepthNormalPclPlaneEstimationMethod() const
void setAngleAppear(const double &aappear)
double getDepthNormalPclPlaneEstimationRansacThreshold() const
void parse(const std::string &filename)
double getNearClippingDistance() const
bool hasNearClippingDistance() const
bool hasFarClippingDistance() const
void setDepthNormalPclPlaneEstimationRansacThreshold(double threshold)
void setCameraParameters(const vpCameraParameters &cam)
double getFarClippingDistance() const
bool getFovClipping() const
void setVerbose(bool verbose)
void setDepthNormalSamplingStepY(unsigned int stepY)
This class defines the container for a plane geometrical structure.
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
unsigned int nbpt
Number of points used to define the polygon.
unsigned int getNbPoint() const
vpPoint * p
corners in the object frame
void setMinMedianAbsoluteDeviation(double mad_min)
vpVelocityTwistMatrix & build(const vpTranslationVector &t, const vpRotationMatrix &R)