 |
Visual Servoing Platform
version 3.3.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_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_featuresToBeDisplayedDepthNormal(), 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];
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);
265 unsigned int cpt = 0;
270 (*it)->computeInteractionMatrix(
m_cMo, L_face, features_face);
283 bool displayFullModel)
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)
312 for (
size_t i = 0; i < models.size(); i++) {
322 for (
size_t i = 0; i < features.size(); i++) {
323 vpImagePoint im_centroid(features[i][1], features[i][2]);
324 vpImagePoint im_extremity(features[i][3], features[i][4]);
332 std::vector<std::vector<double> > features;
334 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
338 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
360 bool displayFullModel)
362 std::vector<std::vector<double> > models;
366 bool changed =
false;
376 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
379 std::vector<std::vector<double> > modelLines = face_normal->
getModelForDisplay(width, height, cMo, cam, displayFullModel);
380 models.insert(models.end(), modelLines.begin(), modelLines.end());
392 bool reInitialisation =
false;
396 #ifdef VISP_HAVE_OGRE
421 #ifdef VISP_HAVE_PUGIXML
436 std::cout <<
" *********** Parsing XML for Mb Depth Tracker ************ " << std::endl;
437 xmlp.
parse(configFile);
439 std::cerr <<
"Exception: " << e.
what() << std::endl;
465 std::cerr <<
"pugixml third-party is not properly built to read config file: " << configFile << std::endl;
485 #if defined(VISP_HAVE_PCL)
525 #ifdef VISP_HAVE_OGRE
536 #ifdef VISP_HAVE_OGRE
554 #if defined(VISP_HAVE_PCL)
568 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
570 (*it)->setScanLineVisibilityTest(v);
576 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
593 #if DEBUG_DISPLAY_DEPTH_NORMAL
594 if (!m_debugDisp_depthNormal->isInitialised()) {
595 m_debugImage_depthNormal.resize(point_cloud->height, point_cloud->width);
596 m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0,
"Debug display normal depth tracker");
599 m_debugImage_depthNormal = 0;
600 std::vector<std::vector<vpImagePoint> > roiPts_vec;
610 #if DEBUG_DISPLAY_DEPTH_NORMAL
611 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
615 #
if DEBUG_DISPLAY_DEPTH_NORMAL
617 m_debugImage_depthNormal, roiPts_vec_
624 #if DEBUG_DISPLAY_DEPTH_NORMAL
625 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
631 #if DEBUG_DISPLAY_DEPTH_NORMAL
634 for (
size_t i = 0; i < roiPts_vec.size(); i++) {
635 if (roiPts_vec[i].empty())
638 for (
size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
641 vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
656 #if DEBUG_DISPLAY_DEPTH_NORMAL
657 if (!m_debugDisp_depthNormal->isInitialised()) {
658 m_debugImage_depthNormal.resize(height, width);
659 m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0,
"Debug display normal depth tracker");
662 m_debugImage_depthNormal = 0;
663 std::vector<std::vector<vpImagePoint> > roiPts_vec;
673 #if DEBUG_DISPLAY_DEPTH_NORMAL
674 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
679 #
if DEBUG_DISPLAY_DEPTH_NORMAL
681 m_debugImage_depthNormal, roiPts_vec_
688 #if DEBUG_DISPLAY_DEPTH_NORMAL
689 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
695 #if DEBUG_DISPLAY_DEPTH_NORMAL
698 for (
size_t i = 0; i < roiPts_vec.size(); i++) {
699 if (roiPts_vec[i].empty())
702 for (
size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
705 vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
717 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
719 (*it)->setCameraParameters(cam);
725 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
727 (*it)->setFaceCentroidMethod(method);
736 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
738 (*it)->setFeatureEstimationMethod(method);
746 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
748 (*it)->setPclPlaneEstimationMethod(method);
756 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
758 (*it)->setPclPlaneEstimationRansacMaxIter(maxIter);
766 for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it =
m_depthNormalFaces.begin();
768 (*it)->setPclPlaneEstimationRansacThreshold(threshold);
774 if (stepX == 0 || stepY == 0) {
775 std::cerr <<
"stepX and stepY must be greater than zero!" << std::endl;
819 const double ,
const int ,
const std::string & )
825 const int ,
const std::string & )
std::vector< std::vector< double > > getFeaturesForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double scale=0.05)
void setTracked(bool tracked)
void setPclPlaneEstimationRansacMaxIter(int maxIter)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
void setAngleAppear(const double &aappear)
bool m_depthNormalUseRobust
If true, use Tukey robust M-Estimator.
bool m_computeInteraction
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
double distNearClip
Distance for near clipping.
static double sqr(double x)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
vpHomogeneousMatrix m_cMo
The current pose.
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
virtual void track(const vpImage< unsigned char > &)
vpCameraParameters m_cam
The camera parameters.
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
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 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)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
virtual void setDepthNormalSamplingStep(unsigned int stepX, unsigned int stepY)
static double rad(double deg)
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Generic class defining intrinsic camera parameters.
vpCameraParameters m_cam
Camera intrinsic parameters.
int getDepthNormalPclPlaneEstimationMethod() const
double getAngleDisappear() const
virtual void resetTracker()
virtual void setDepthNormalPclPlaneEstimationMethod(int method)
static double deg(double rad)
void setFeatureEstimationMethod(const vpFeatureEstimationType &method)
virtual void testTracking()
virtual void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
unsigned int getDepthNormalSamplingStepX() const
double m_distNearClip
Distance for near clipping.
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
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)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
virtual void setDepthNormalPclPlaneEstimationRansacThreshold(double thresold)
void setDepthNormalPclPlaneEstimationRansacThreshold(double threshold)
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.
virtual std::vector< std::vector< double > > getFeaturesForDisplayDepthNormal()
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.
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
vpPoint * p
corners in the object frame
int m_depthNormalPclPlaneEstimationMethod
PCL plane estimation method.
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)
double getDepthNormalPclPlaneEstimationRansacThreshold() const
void setUseDepthNormalTracking(const std::string &name, const bool &useDepthNormalTracking)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
void setPclPlaneEstimationRansacThreshold(double threshold)
double m_lambda
Gain of the virtual visual servoing stage.
Implementation of column vector and the associated operations.
void setPclPlaneEstimationMethod(int method)
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
virtual void setDepthNormalPclPlaneEstimationRansacMaxIter(int maxIter)
double getNearClippingDistance() const
Implementation of a matrix and operations on matrices.
void setThreshold(double noise_threshold)
Implementation of a polygon of the model used by the model-based tracker.
virtual void setScanLineVisibilityTest(const bool &v)
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
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.
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
static void display(const vpImage< unsigned char > &I)
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
void computeVisibility(unsigned int width, unsigned int height)
double distFarClip
Distance for near clipping.
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
void parse(const std::string &filename)
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)
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
double m_depthNormalPclPlaneEstimationRansacThreshold
PCL RANSAC threshold.
virtual void setCameraParameters(const vpCameraParameters &camera)
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
virtual ~vpMbDepthNormalTracker()
static bool equal(double x, double y, double s=0.001)
virtual void init(const vpImage< unsigned char > &I)
double angleAppears
Angle used to detect a face appearance.
void getCameraParameters(vpCameraParameters &cam) const
unsigned int nbpt
Number of points used to define the polygon.
void setDepthNormalPclPlaneEstimationRansacMaxIter(int maxIter)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void setDepthNormalPclPlaneEstimationMethod(int method)
vpMatrix m_L_depthNormal
Interaction matrix.
void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces< vpMbtPolygon > *const faces, int polygon=-1, std::string name="")
void setCameraParameters(const vpCameraParameters &cam)
static vpHomogeneousMatrix direct(const vpColVector &v)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
unsigned int getNbPoint() const
double getFarClippingDistance() const
bool hasFarClippingDistance() const
void setOgreShowConfigDialog(bool showConfigDialog)
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
void resize(unsigned int i, bool flagNullify=true)
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
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false)
unsigned int m_depthNormalSamplingStepX
Sampling step in x-direction.
unsigned int clippingFlag
Flags specifying which clipping to used.
static const vpColor blue
void addFace(vpMbtPolygon &polygon, bool alreadyClose)
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
Class that defines what is a point.
Class to define colors available for display functionnalities.
Implementation of an homogeneous matrix and operations on such kind of matrices.
double m_stopCriteriaEpsilon
Epsilon threshold to stop the VVS optimization loop.
void setDepthNormalSamplingStepX(unsigned int stepX)
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 setDepthNormalSamplingStepY(unsigned int stepY)
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)
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
double angleDisappears
Angle used to detect a face disappearance.
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
unsigned int getWidth() const
unsigned int getRows() const