Visual Servoing Platform version 3.5.0
vpMbEdgeTracker.h
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Make the complete tracking of an object by using its CAD model
33 *
34 * Authors:
35 * Nicolas Melchior
36 * Romain Tallonneau
37 * Eric Marchand
38 *
39 *****************************************************************************/
40
46#ifndef vpMbEdgeTracker_HH
47#define vpMbEdgeTracker_HH
48
49#include <visp3/core/vpPoint.h>
50#include <visp3/mbt/vpMbTracker.h>
51#include <visp3/mbt/vpMbtDistanceCircle.h>
52#include <visp3/mbt/vpMbtDistanceCylinder.h>
53#include <visp3/mbt/vpMbtDistanceLine.h>
54#include <visp3/mbt/vpMbtMeLine.h>
55#include <visp3/me/vpMe.h>
56
57#include <fstream>
58#include <iostream>
59#include <list>
60#include <vector>
61
62#if defined(VISP_HAVE_COIN3D)
63// Inventor includes
64#include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
65#include <Inventor/VRMLnodes/SoVRMLGroup.h>
66#include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
67#include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
68#include <Inventor/VRMLnodes/SoVRMLShape.h>
69#include <Inventor/actions/SoGetMatrixAction.h>
70#include <Inventor/actions/SoGetPrimitiveCountAction.h>
71#include <Inventor/actions/SoSearchAction.h>
72#include <Inventor/actions/SoToVRML2Action.h>
73#include <Inventor/actions/SoWriteAction.h>
74#include <Inventor/misc/SoChildList.h>
75#include <Inventor/nodes/SoSeparator.h>
76#endif
77
78#ifdef VISP_HAVE_OPENCV
79#if VISP_HAVE_OPENCV_VERSION >= 0x020101
80#include <opencv2/core/core.hpp>
81#include <opencv2/imgproc/imgproc.hpp>
82#include <opencv2/imgproc/imgproc_c.h>
83#else
84#include <cv.h>
85#endif
86#endif
87
252class VISP_EXPORT vpMbEdgeTracker : public virtual vpMbTracker
253{
254protected:
260 std::vector<std::list<vpMbtDistanceLine *> > lines;
261
263 std::vector<std::list<vpMbtDistanceCircle *> > circles;
264
266 std::vector<std::list<vpMbtDistanceCylinder *> > cylinders;
267
270 unsigned int nline;
271
274 unsigned int ncircle;
275
278 unsigned int ncylinder;
279
281 unsigned int nbvisiblepolygone;
282
286
288 std::vector<bool> scales;
289
292 std::vector<const vpImage<unsigned char> *> Ipyramid;
293
297 unsigned int scaleLevel;
298
301
324 std::vector<std::vector<double> > m_featuresToBeDisplayedEdge;
325
326public:
328 virtual ~vpMbEdgeTracker();
329
332
333 virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
334 const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);
335 virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
336 const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);
337
338 void getLline(std::list<vpMbtDistanceLine *> &linesList, unsigned int level = 0) const;
339 void getLcircle(std::list<vpMbtDistanceCircle *> &circlesList, unsigned int level = 0) const;
340 void getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList, unsigned int level = 0) const;
341
342 virtual std::vector<std::vector<double> > getModelForDisplay(unsigned int width, unsigned int height,
343 const vpHomogeneousMatrix &cMo,
344 const vpCameraParameters &cam,
345 bool displayFullModel=false);
346
353 virtual inline void getMovingEdge(vpMe &p_me) const { p_me = this->me; }
359 virtual inline vpMe getMovingEdge() const { return this->me; }
360
361 virtual unsigned int getNbPoints(unsigned int level = 0) const;
362
368 std::vector<bool> getScales() const { return scales; }
378 inline double getGoodMovingEdgesRatioThreshold() const { return percentageGdPt; }
379
380 virtual inline vpColVector getError() const { return m_error_edge; }
381
382 virtual inline vpColVector getRobustWeights() const { return m_w_edge; }
383
384 virtual void loadConfigFile(const std::string &configFile, bool verbose=true);
385
386 virtual void reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
387 const vpHomogeneousMatrix &cMo, bool verbose = false,
389 void resetTracker();
390
397 {
398 m_cam = cam;
399
400 for (unsigned int i = 0; i < scales.size(); i += 1) {
401 if (scales[i]) {
402 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
403 (*it)->setCameraParameters(m_cam);
404 }
405
406 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
407 ++it) {
408 (*it)->setCameraParameters(m_cam);
409 }
410
411 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
412 (*it)->setCameraParameters(m_cam);
413 }
414 }
415 }
416 }
417
418 virtual void setClipping(const unsigned int &flags);
419
420 virtual void setFarClippingDistance(const double &dist);
421
422 virtual void setNearClippingDistance(const double &dist);
423
432 virtual void setOgreVisibilityTest(const bool &v)
433 {
435#ifdef VISP_HAVE_OGRE
436 faces.getOgreContext()->setWindowName("MBT Edge");
437#endif
438 }
439
445 virtual void setScanLineVisibilityTest(const bool &v)
446 {
448
449 for (unsigned int i = 0; i < scales.size(); i += 1) {
450 if (scales[i]) {
451 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
452 (*it)->useScanLine = v;
453 }
454 }
455 }
456 }
457
471 void setGoodMovingEdgesRatioThreshold(double threshold) { percentageGdPt = threshold; }
472
473 void setMovingEdge(const vpMe &me);
474
475 virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo);
476 virtual void setPose(const vpImage<vpRGBa> &I_color, const vpHomogeneousMatrix &cdMo);
477
478 void setScales(const std::vector<bool> &_scales);
479
480 void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking);
481
482 virtual void track(const vpImage<unsigned char> &I);
483 virtual void track(const vpImage<vpRGBa> &I);
485
486protected:
489 void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace = -1,
490 const std::string &name = "");
491 void addCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace = -1, const std::string &name = "");
492 void addLine(vpPoint &p1, vpPoint &p2, int polygon = -1, std::string name = "");
493 void addPolygon(vpMbtPolygon &p);
494
495 void cleanPyramid(std::vector<const vpImage<unsigned char> *> &_pyramid);
496 void computeProjectionError(const vpImage<unsigned char> &_I);
497
498 void computeVVS(const vpImage<unsigned char> &_I, unsigned int lvl);
499 void computeVVSFirstPhase(const vpImage<unsigned char> &I, unsigned int iter, double &count,
500 unsigned int lvl = 0);
501 void computeVVSFirstPhaseFactor(const vpImage<unsigned char> &I, unsigned int lvl = 0);
502 void computeVVSFirstPhasePoseEstimation(unsigned int iter, bool &isoJoIdentity_);
503 virtual void computeVVSInit();
506 virtual void computeVVSWeights();
508
509 void displayFeaturesOnImage(const vpImage<unsigned char> &I);
510 void displayFeaturesOnImage(const vpImage<vpRGBa> &I);
511 void downScale(const unsigned int _scale);
512 virtual std::vector<std::vector<double> > getFeaturesForDisplayEdge();
513 virtual void init(const vpImage<unsigned char> &I);
514 virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
515 int idFace = 0, const std::string &name = "");
516 virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace = 0,
517 const std::string &name = "");
518 virtual void initFaceFromCorners(vpMbtPolygon &polygon);
519 virtual void initFaceFromLines(vpMbtPolygon &polygon);
520 unsigned int initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders,
521 unsigned int &nberrors_circles);
522 void initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo);
523 void initPyramid(const vpImage<unsigned char> &_I, std::vector<const vpImage<unsigned char> *> &_pyramid);
524 void reInitLevel(const unsigned int _lvl);
525 void reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo);
526 void removeCircle(const std::string &name);
527 void removeCylinder(const std::string &name);
528 void removeLine(const std::string &name);
529 void resetMovingEdge();
530 virtual void testTracking();
531 void trackMovingEdge(const vpImage<unsigned char> &I);
532 void updateMovingEdge(const vpImage<unsigned char> &I);
533 void updateMovingEdgeWeights();
534 void upScale(const unsigned int _scale);
535 void visibleFace(const vpImage<unsigned char> &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline);
537};
538
539#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Make the complete tracking of an object by using its CAD model.
double getGoodMovingEdgesRatioThreshold() const
vpColVector m_errorCircles
vpColVector m_w_edge
Robust weights.
virtual void setCameraParameters(const vpCameraParameters &cam)
virtual void setScanLineVisibilityTest(const bool &v)
vpRobust m_robust_edge
Robust.
virtual vpColVector getRobustWeights() const
std::vector< std::list< vpMbtDistanceLine * > > lines
vpMe me
The moving edges parameters.
vpColVector m_wLines
vpColVector m_error_edge
(s - s*)
unsigned int ncylinder
std::vector< std::list< vpMbtDistanceCylinder * > > cylinders
Vector of the tracked cylinders.
std::vector< std::vector< double > > m_featuresToBeDisplayedEdge
Display features.
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
virtual vpColVector getError() const
virtual void setOgreVisibilityTest(const bool &v)
vpRobust m_robustCircles
std::vector< std::list< vpMbtDistanceCircle * > > circles
Vector of the tracked circles.
unsigned int scaleLevel
std::vector< const vpImage< unsigned char > * > Ipyramid
vpColVector m_weightedError_edge
Weighted error.
unsigned int ncircle
std::vector< bool > scales
Vector of scale level to use for the multi-scale tracking.
vpMatrix m_L_edge
Interaction matrix.
vpRobust m_robustCylinders
void setGoodMovingEdgesRatioThreshold(double threshold)
virtual void getMovingEdge(vpMe &p_me) const
vpRobust m_robustLines
virtual vpMe getMovingEdge() const
vpColVector m_wCylinders
unsigned int nbFeaturesForProjErrorComputation
Number of features used in the computation of the projection error.
vpColVector m_factor
Edge VVS variables.
vpColVector m_errorLines
std::vector< bool > getScales() const
vpColVector m_wCircles
unsigned int nline
vpColVector m_errorCylinders
Main methods for a model-based tracker.
Definition: vpMbTracker.h:105
virtual void track(const vpImage< unsigned char > &I)=0
virtual void resetTracker()=0
virtual void init(const vpImage< unsigned char > &I)=0
virtual void initFaceFromLines(vpMbtPolygon &polygon)=0
void addPolygon(const std::vector< vpPoint > &corners, int idFace=-1, const std::string &polygonName="", bool useLod=false, double minPolygonAreaThreshold=2500.0, double minLineLengthThreshold=50.0)
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")=0
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")=0
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)=0
virtual void setScanLineVisibilityTest(const bool &v)
Definition: vpMbTracker.h:601
virtual void setOgreVisibilityTest(const bool &v)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)=0
virtual void setNearClippingDistance(const double &dist)
virtual void setFarClippingDistance(const double &dist)
virtual void setClipping(const unsigned int &flags)
virtual void computeVVSInteractionMatrixAndResidu()=0
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)=0
virtual void computeVVSInit()=0
virtual void testTracking()=0
virtual void initFaceFromCorners(vpMbtPolygon &polygon)=0
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:67
Definition: vpMe.h:61
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
Contains an M-estimator and various influence function.
Definition: vpRobust.h:89