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/vpMbDepthDenseTracker.h>
48#include <visp3/mbt/vpMbtXmlGenericParser.h>
50#if DEBUG_DISPLAY_DEPTH_DENSE
51#include <visp3/gui/vpDisplayGDI.h>
52#include <visp3/gui/vpDisplayX.h>
56 : m_depthDenseHiddenFacesDisplay(), m_depthDenseListOfActiveFaces(), m_denseDepthNbFeatures(0), m_depthDenseFaces(),
57 m_depthDenseSamplingStepX(2), m_depthDenseSamplingStepY(2), m_error_depthDense(), m_L_depthDense(),
58 m_robust_depthDense(), m_w_depthDense(), m_weightedError_depthDense()
59#if DEBUG_DISPLAY_DEPTH_DENSE
61 m_debugDisp_depthDense(NULL), m_debugImage_depthDense()
68#if defined(VISP_HAVE_X11) && DEBUG_DISPLAY_DEPTH_DENSE
70#elif defined(VISP_HAVE_GDI) && DEBUG_DISPLAY_DEPTH_DENSE
81#if DEBUG_DISPLAY_DEPTH_DENSE
82 delete m_debugDisp_depthDense;
88 if (polygon.
nbpt < 3) {
107 for (
unsigned int i = 0; i < nbpt - 1; i++) {
121 pts[0] = polygon.
p[0];
122 pts[1] = polygon.
p[1];
123 pts[2] = polygon.
p[2];
131 bool changed =
false;
153 double normRes_1 = -1;
154 unsigned int iter = 0;
175 bool reStartFromLastIncrement =
false;
178 if (!reStartFromLastIncrement) {
183 if (!isoJoIdentity) {
209 isoJoIdentity =
false;
214 double num = 0.0, den = 0.0;
222 for (
unsigned int j = 0; j < 6; j++) {
234 normRes = sqrt(num / den);
263 unsigned int start_index = 0;
276 start_index += error.
getRows();
287 bool displayFullModel)
289 std::vector<std::vector<double> > models =
292 for (
size_t i = 0; i < models.size(); i++) {
303 bool displayFullModel)
305 std::vector<std::vector<double> > models =
308 for (
size_t i = 0; i < models.size(); i++) {
335 bool displayFullModel)
337 std::vector<std::vector<double> > models;
341 bool changed =
false;
354 std::vector<std::vector<double> > modelLines =
356 models.insert(models.end(), modelLines.begin(), modelLines.end());
368 bool reInitialisation =
false;
408 std::cout <<
" *********** Parsing XML for Mb Depth Dense Tracker ************ " << std::endl;
410 xmlp.
parse(configFile);
412 std::cerr <<
"Exception: " << e.
what() << std::endl;
451#if defined(VISP_HAVE_PCL)
504 (*it)->setCameraParameters(cam);
512 (*it)->setDepthDenseFilteringMaxDistance(maxDistance);
520 (*it)->setDepthDenseFilteringMethod(method);
528 (*it)->setDepthDenseFilteringMinDistance(minDistance);
534 if (occupancyRatio < 0.0 || occupancyRatio > 1.0) {
535 std::cerr <<
"occupancyRatio < 0.0 || occupancyRatio > 1.0" << std::endl;
541 (*it)->setDepthDenseFilteringOccupancyRatio(occupancyRatio);
550#if DEBUG_DISPLAY_DEPTH_DENSE
551 if (!m_debugDisp_depthDense->isInitialised()) {
552 m_debugImage_depthDense.resize(point_cloud->height, point_cloud->width);
553 m_debugDisp_depthDense->init(m_debugImage_depthDense, 50, 0,
"Debug display dense depth tracker");
556 m_debugImage_depthDense = 0;
557 std::vector<std::vector<vpImagePoint> > roiPts_vec;
565#if DEBUG_DISPLAY_DEPTH_DENSE
566 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
569#
if DEBUG_DISPLAY_DEPTH_DENSE
571 m_debugImage_depthDense, roiPts_vec_
577#if DEBUG_DISPLAY_DEPTH_DENSE
578 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
584#if DEBUG_DISPLAY_DEPTH_DENSE
587 for (
size_t i = 0; i < roiPts_vec.size(); i++) {
588 if (roiPts_vec[i].empty())
591 for (
size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
594 vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
608#if DEBUG_DISPLAY_DEPTH_DENSE
609 if (!m_debugDisp_depthDense->isInitialised()) {
610 m_debugImage_depthDense.resize(height, width);
611 m_debugDisp_depthDense->init(m_debugImage_depthDense, 50, 0,
"Debug display dense depth tracker");
614 m_debugImage_depthDense = 0;
615 std::vector<std::vector<vpImagePoint> > roiPts_vec;
623#if DEBUG_DISPLAY_DEPTH_DENSE
624 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
628#
if DEBUG_DISPLAY_DEPTH_DENSE
630 m_debugImage_depthDense, roiPts_vec_
636#if DEBUG_DISPLAY_DEPTH_DENSE
637 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
643#if DEBUG_DISPLAY_DEPTH_DENSE
646 for (
size_t i = 0; i < roiPts_vec.size(); i++) {
647 if (roiPts_vec[i].empty())
650 for (
size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
653 vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
682#if defined(VISP_HAVE_PCL)
686 m_I.
resize(point_cloud->height, point_cloud->width);
698 (*it)->setScanLineVisibilityTest(v);
746 double ,
int ,
const std::string & )
752 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.
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)
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 ...
Definition of the vpImage class member functions.
unsigned int getWidth() const
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
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)
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
vpMbtTukeyEstimator< double > m_robust_depthDense
Tukey M-Estimator.
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
virtual void setDepthDenseFilteringMaxDistance(double maxDistance)
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
void computeVisibility(unsigned int width, unsigned int height)
virtual void setDepthDenseFilteringMethod(int method)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void computeVVSInit()
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false)
vpColVector m_weightedError_depthDense
Weighted error.
unsigned int m_depthDenseSamplingStepY
Sampling step in y-direction.
virtual void init(const vpImage< unsigned char > &I)
virtual void resetTracker()
vpMbHiddenFaces< vpMbtPolygon > m_depthDenseHiddenFacesDisplay
Set of faces describing the object used only for display with scan line.
virtual void setOgreVisibilityTest(const bool &v)
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
void setUseDepthDenseTracking(const std::string &name, const bool &useDepthDenseTracking)
unsigned int m_depthDenseSamplingStepX
Sampling step in x-direction.
virtual void testTracking()
virtual void computeVVSWeights()
virtual void setScanLineVisibilityTest(const bool &v)
vpColVector m_error_depthDense
(s - s*)
virtual void track(const vpImage< unsigned char > &)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
vpMatrix m_L_depthDense
Interaction matrix.
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void setCameraParameters(const vpCameraParameters &camera)
std::vector< vpMbtFaceDepthDense * > m_depthDenseListOfActiveFaces
List of current active (visible and features extracted) faces.
void setDepthDenseSamplingStep(unsigned int stepX, unsigned int stepY)
void addFace(vpMbtPolygon &polygon, bool alreadyClose)
virtual void setDepthDenseFilteringMinDistance(double minDistance)
unsigned int m_denseDepthNbFeatures
Nb features.
vpColVector m_w_depthDense
Robust weights.
virtual ~vpMbDepthDenseTracker()
virtual void setDepthDenseFilteringOccupancyRatio(double occupancyRatio)
std::vector< vpMbtFaceDepthDense * > m_depthDenseFaces
List of faces.
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)
vpAROgre * getOgreContext()
void setOgreShowConfigDialog(bool showConfigDialog)
double m_lambda
Gain of the virtual visual servoing stage.
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)
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.
vpCameraParameters m_cam
The camera parameters.
double m_stopCriteriaEpsilon
Epsilon threshold to stop the VVS optimization loop.
bool useOgre
Use Ogre3d for visibility tests.
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.
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.
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
double m_distFarClip
Distance for near clipping.
vpPlane m_planeObject
Plane equation described in the object frame.
bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud, unsigned int stepX, unsigned int stepY, const vpImage< bool > *mask=NULL)
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
unsigned int getNbFeatures() const
void setTracked(bool tracked)
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &error)
vpMbtPolygon * m_polygon
Polygon defining the face.
bool m_useScanLine
Scan line visibility.
unsigned int m_clippingFlag
Flags specifying which clipping to used.
vpCameraParameters m_cam
Camera intrinsic parameters.
double m_distNearClip
Distance for near clipping.
void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces< vpMbtPolygon > *const faces, vpUniRand &rand_gen, int polygon=-1, std::string name="")
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 getCameraParameters(vpCameraParameters &cam) const
void setDepthDenseSamplingStepY(unsigned int stepY)
double getAngleAppear() const
void setAngleDisappear(const double &adisappear)
double getAngleDisappear() const
void setDepthDenseSamplingStepX(unsigned int stepX)
void setAngleAppear(const double &aappear)
unsigned int getDepthDenseSamplingStepY() const
void parse(const std::string &filename)
double getNearClippingDistance() const
bool hasNearClippingDistance() const
bool hasFarClippingDistance() const
unsigned int getDepthDenseSamplingStepX() const
void setCameraParameters(const vpCameraParameters &cam)
double getFarClippingDistance() const
bool getFovClipping() const
void setVerbose(bool verbose)
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
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)