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>
57 m_depthNormalHiddenFacesDisplay(), m_depthNormalListOfActiveFaces(), m_depthNormalListOfDesiredFeatures(),
58 m_depthNormalFaces(), m_depthNormalPclPlaneEstimationMethod(2), m_depthNormalPclPlaneEstimationRansacMaxIter(200),
59 m_depthNormalPclPlaneEstimationRansacThreshold(0.001), m_depthNormalSamplingStepX(2), m_depthNormalSamplingStepY(2),
60 m_depthNormalUseRobust(false), m_error_depthNormal(), m_featuresToBeDisplayedDepthNormal(), m_L_depthNormal(),
61 m_robust_depthNormal(), m_w_depthNormal(), m_weightedError_depthNormal()
62 #if DEBUG_DISPLAY_DEPTH_NORMAL
64 m_debugDisp_depthNormal(nullptr), 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];
134 bool changed =
false;
146 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
156 double normRes_1 = -1;
157 unsigned int iter = 0;
179 bool reStartFromLastIncrement =
false;
182 if (!reStartFromLastIncrement) {
188 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);
264 unsigned int cpt = 0;
269 (*it)->computeInteractionMatrix(
m_cMo, L_face, features_face);
282 bool displayFullModel)
284 std::vector<std::vector<double> > models =
287 for (
size_t i = 0; i < models.size(); i++) {
297 for (
size_t i = 0; i < features.size(); i++) {
298 vpImagePoint im_centroid(features[i][1], features[i][2]);
299 vpImagePoint im_extremity(features[i][3], features[i][4]);
308 bool displayFullModel)
310 std::vector<std::vector<double> > models =
313 for (
size_t i = 0; i < models.size(); i++) {
323 for (
size_t i = 0; i < features.size(); i++) {
324 vpImagePoint im_centroid(features[i][1], features[i][2]);
325 vpImagePoint im_extremity(features[i][3], features[i][4]);
334 std::vector<std::vector<double> > features;
336 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
340 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
364 bool displayFullModel)
366 std::vector<std::vector<double> > models;
370 bool changed =
false;
380 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
383 std::vector<std::vector<double> > modelLines =
385 models.insert(models.end(), modelLines.begin(), modelLines.end());
397 bool reInitialisation =
false;
402 #ifdef VISP_HAVE_OGRE
427 #if defined(VISP_HAVE_PUGIXML)
443 std::cout <<
" *********** Parsing XML for Mb Depth Tracker ************ " << std::endl;
445 xmlp.
parse(configFile);
448 std::cerr <<
"Exception: " << e.
what() << std::endl;
496 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
514 normal_face =
nullptr;
535 #ifdef VISP_HAVE_OGRE
546 #ifdef VISP_HAVE_OGRE
564 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
578 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
580 (*it)->setScanLineVisibilityTest(v);
586 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
597 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
603 #if DEBUG_DISPLAY_DEPTH_NORMAL
604 if (!m_debugDisp_depthNormal->isInitialised()) {
605 m_debugImage_depthNormal.resize(point_cloud->height, point_cloud->width);
606 m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0,
"Debug display normal depth tracker");
609 m_debugImage_depthNormal = 0;
610 std::vector<std::vector<vpImagePoint> > roiPts_vec;
620 #if DEBUG_DISPLAY_DEPTH_NORMAL
621 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
625 #
if DEBUG_DISPLAY_DEPTH_NORMAL
627 m_debugImage_depthNormal, roiPts_vec_
634 #if DEBUG_DISPLAY_DEPTH_NORMAL
635 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
641 #if DEBUG_DISPLAY_DEPTH_NORMAL
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_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
666 #if DEBUG_DISPLAY_DEPTH_NORMAL
667 if (!m_debugDisp_depthNormal->isInitialised()) {
668 m_debugImage_depthNormal.resize(height, width);
669 m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0,
"Debug display normal depth tracker");
672 m_debugImage_depthNormal = 0;
673 std::vector<std::vector<vpImagePoint> > roiPts_vec;
683 #if DEBUG_DISPLAY_DEPTH_NORMAL
684 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
689 #
if DEBUG_DISPLAY_DEPTH_NORMAL
691 m_debugImage_depthNormal, roiPts_vec_
698 #if DEBUG_DISPLAY_DEPTH_NORMAL
699 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
705 #if DEBUG_DISPLAY_DEPTH_NORMAL
708 for (
size_t i = 0; i < roiPts_vec.size(); i++) {
709 if (roiPts_vec[i].empty())
712 for (
size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
715 vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
729 #if DEBUG_DISPLAY_DEPTH_NORMAL
730 if (!m_debugDisp_depthNormal->isInitialised()) {
731 m_debugImage_depthNormal.resize(height, width);
732 m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0,
"Debug display normal depth tracker");
735 m_debugImage_depthNormal = 0;
736 std::vector<std::vector<vpImagePoint> > roiPts_vec;
746 #if DEBUG_DISPLAY_DEPTH_NORMAL
747 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
752 #
if DEBUG_DISPLAY_DEPTH_NORMAL
754 m_debugImage_depthNormal, roiPts_vec_
761 #if DEBUG_DISPLAY_DEPTH_NORMAL
762 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
768 #if DEBUG_DISPLAY_DEPTH_NORMAL
771 for (
size_t i = 0; i < roiPts_vec.size(); i++) {
772 if (roiPts_vec[i].empty())
775 for (
size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
778 vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
790 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
792 (*it)->setCameraParameters(cam);
798 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
800 (*it)->setFaceCentroidMethod(method);
809 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
811 (*it)->setFeatureEstimationMethod(method);
819 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
821 (*it)->setPclPlaneEstimationMethod(method);
829 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
831 (*it)->setPclPlaneEstimationRansacMaxIter(maxIter);
839 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
841 (*it)->setPclPlaneEstimationRansacThreshold(threshold);
847 if (stepX == 0 || stepY == 0) {
848 std::cerr <<
"stepX and stepY must be greater than zero!" << std::endl;
870 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
891 double ,
int ,
const std::string & )
897 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 setScanLineVisibilityTest(const bool &v) vp_override
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo) 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
vpMatrix m_L_depthNormal
Interaction matrix.
vpRobust m_robust_depthNormal
Robust.
virtual void setDepthNormalSamplingStep(unsigned int stepX, unsigned int stepY)
virtual void testTracking() vp_override
vpMbtFaceDepthNormal::vpFeatureEstimationType m_depthNormalFeatureEstimationMethod
Method to estimate the desired features.
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="") vp_override
std::vector< vpMbtFaceDepthNormal * > m_depthNormalFaces
List of faces.
virtual void resetTracker() vp_override
int m_depthNormalPclPlaneEstimationRansacMaxIter
PCL RANSAC maximum number of iterations.
unsigned int m_depthNormalSamplingStepY
Sampling step in y-direction.
virtual void computeVVSInteractionMatrixAndResidu() vp_override
double m_depthNormalPclPlaneEstimationRansacThreshold
PCL RANSAC threshold.
virtual void setDepthNormalPclPlaneEstimationRansacThreshold(double thresold)
void addFace(vpMbtPolygon &polygon, bool alreadyClose)
bool m_depthNormalUseRobust
If true, use Tukey robust M-Estimator.
virtual void setOgreVisibilityTest(const bool &v) vp_override
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false)
virtual void computeVVSInit() vp_override
virtual std::vector< std::vector< double > > getFeaturesForDisplayDepthNormal()
void segmentPointCloud(const std::vector< vpColVector > &point_cloud, unsigned int width, unsigned int height)
vpColVector m_w_depthNormal
Robust weights.
vpColVector m_error_depthNormal
(s - s*)
vpColVector m_weightedError_depthNormal
Weighted error.
std::vector< vpMbtFaceDepthNormal * > m_depthNormalListOfActiveFaces
List of current active (visible and with features extracted) faces.
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false) vp_override
virtual void loadConfigFile(const std::string &configFile, bool verbose=true) vp_override
virtual void initFaceFromCorners(vpMbtPolygon &polygon) vp_override
int m_depthNormalPclPlaneEstimationMethod
PCL plane estimation method.
virtual void init(const vpImage< unsigned char > &I) vp_override
virtual void track(const vpImage< unsigned char > &) vp_override
virtual void setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method)
virtual void setDepthNormalPclPlaneEstimationMethod(int method)
std::vector< vpColVector > m_depthNormalListOfDesiredFeatures
List of desired features.
virtual void initFaceFromLines(vpMbtPolygon &polygon) vp_override
virtual ~vpMbDepthNormalTracker() vp_override
virtual void setCameraParameters(const vpCameraParameters &camera) 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 void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="") 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.
bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height, const std::vector< vpColVector > &point_cloud, vpColVector &desired_features, unsigned int stepX, unsigned int stepY, const vpImage< bool > *mask=nullptr)
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.
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 buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)