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_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(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];
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;
169 bool isoJoIdentity_ =
true;
176 bool reStartFromLastIncrement =
false;
179 if (!reStartFromLastIncrement) {
185 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);
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;
401 #ifdef VISP_HAVE_OGRE
441 std::cout <<
" *********** Parsing XML for Mb Depth Tracker ************ " << std::endl;
443 xmlp.
parse(configFile);
445 std::cerr <<
"Exception: " << e.
what() << std::endl;
488 #if defined(VISP_HAVE_PCL)
527 #ifdef VISP_HAVE_OGRE
538 #ifdef VISP_HAVE_OGRE
556 #if defined(VISP_HAVE_PCL)
570 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
572 (*it)->setScanLineVisibilityTest(v);
578 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
595 #if DEBUG_DISPLAY_DEPTH_NORMAL
596 if (!m_debugDisp_depthNormal->isInitialised()) {
597 m_debugImage_depthNormal.resize(point_cloud->height, point_cloud->width);
598 m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0,
"Debug display normal depth tracker");
601 m_debugImage_depthNormal = 0;
602 std::vector<std::vector<vpImagePoint> > roiPts_vec;
612 #if DEBUG_DISPLAY_DEPTH_NORMAL
613 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
617 #
if DEBUG_DISPLAY_DEPTH_NORMAL
619 m_debugImage_depthNormal, roiPts_vec_
626 #if DEBUG_DISPLAY_DEPTH_NORMAL
627 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
633 #if DEBUG_DISPLAY_DEPTH_NORMAL
636 for (
size_t i = 0; i < roiPts_vec.size(); i++) {
637 if (roiPts_vec[i].empty())
640 for (
size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
643 vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
658 #if DEBUG_DISPLAY_DEPTH_NORMAL
659 if (!m_debugDisp_depthNormal->isInitialised()) {
660 m_debugImage_depthNormal.resize(height, width);
661 m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0,
"Debug display normal depth tracker");
664 m_debugImage_depthNormal = 0;
665 std::vector<std::vector<vpImagePoint> > roiPts_vec;
675 #if DEBUG_DISPLAY_DEPTH_NORMAL
676 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
681 #
if DEBUG_DISPLAY_DEPTH_NORMAL
683 m_debugImage_depthNormal, roiPts_vec_
690 #if DEBUG_DISPLAY_DEPTH_NORMAL
691 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
697 #if DEBUG_DISPLAY_DEPTH_NORMAL
700 for (
size_t i = 0; i < roiPts_vec.size(); i++) {
701 if (roiPts_vec[i].empty())
704 for (
size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
707 vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
719 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
721 (*it)->setCameraParameters(cam);
727 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
729 (*it)->setFaceCentroidMethod(method);
738 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
740 (*it)->setFeatureEstimationMethod(method);
748 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
750 (*it)->setPclPlaneEstimationMethod(method);
758 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
760 (*it)->setPclPlaneEstimationRansacMaxIter(maxIter);
768 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
770 (*it)->setPclPlaneEstimationRansacThreshold(threshold);
776 if (stepX == 0 || stepY == 0) {
777 std::cerr <<
"stepX and stepY must be greater than zero!" << std::endl;
820 double ,
int ,
const std::string & )
826 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 functionnalities.
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 emited 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 s=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)
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
virtual void resetTracker()
vpMatrix m_L_depthNormal
Interaction matrix.
vpRobust m_robust_depthNormal
Robust.
virtual void setDepthNormalSamplingStep(unsigned int stepX, unsigned int stepY)
virtual void computeVVSInit()
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.
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
unsigned int m_depthNormalSamplingStepY
Sampling step in y-direction.
double m_depthNormalPclPlaneEstimationRansacThreshold
PCL RANSAC threshold.
virtual void setDepthNormalPclPlaneEstimationRansacThreshold(double thresold)
virtual void track(const vpImage< unsigned char > &)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
void addFace(vpMbtPolygon &polygon, bool alreadyClose)
bool m_depthNormalUseRobust
If true, use Tukey robust M-Estimator.
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.
vpColVector m_error_depthNormal
(s - s*)
virtual void setScanLineVisibilityTest(const bool &v)
vpColVector m_weightedError_depthNormal
Weighted error.
std::vector< vpMbtFaceDepthNormal * > m_depthNormalListOfActiveFaces
List of current active (visible and with features extracted) faces.
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
virtual void setCameraParameters(const vpCameraParameters &camera)
virtual void setOgreVisibilityTest(const bool &v)
int m_depthNormalPclPlaneEstimationMethod
PCL plane estimation method.
virtual void testTracking()
virtual void computeVVSInteractionMatrixAndResidu()
virtual void setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method)
virtual void setDepthNormalPclPlaneEstimationMethod(int method)
std::vector< vpColVector > m_depthNormalListOfDesiredFeatures
List of desired features.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual ~vpMbDepthNormalTracker()
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
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 init(const vpImage< unsigned char > &I)
virtual void setDepthNormalPclPlaneEstimationRansacMaxIter(int maxIter)
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.
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
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)
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 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)
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)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
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 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 pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud, vpColVector &desired_features, unsigned int stepX, unsigned int stepY, const vpImage< bool > *mask=NULL)
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)