Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMbEdgeTracker.h
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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*****************************************************************************/
35
41#ifndef vpMbEdgeTracker_HH
42#define vpMbEdgeTracker_HH
43
44#include <visp3/core/vpPoint.h>
45#include <visp3/mbt/vpMbTracker.h>
46#include <visp3/mbt/vpMbtDistanceCircle.h>
47#include <visp3/mbt/vpMbtDistanceCylinder.h>
48#include <visp3/mbt/vpMbtDistanceLine.h>
49#include <visp3/mbt/vpMbtMeLine.h>
50#include <visp3/me/vpMe.h>
51
52#include <fstream>
53#include <iostream>
54#include <list>
55#include <vector>
56
57#if defined(VISP_HAVE_COIN3D)
58// Inventor includes
59#include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
60#include <Inventor/VRMLnodes/SoVRMLGroup.h>
61#include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
62#include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
63#include <Inventor/VRMLnodes/SoVRMLShape.h>
64#include <Inventor/actions/SoGetMatrixAction.h>
65#include <Inventor/actions/SoGetPrimitiveCountAction.h>
66#include <Inventor/actions/SoSearchAction.h>
67#include <Inventor/actions/SoToVRML2Action.h>
68#include <Inventor/actions/SoWriteAction.h>
69#include <Inventor/misc/SoChildList.h>
70#include <Inventor/nodes/SoSeparator.h>
71#endif
72
73#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC)
74#include <opencv2/imgproc/imgproc.hpp>
75#include <opencv2/imgproc/imgproc_c.h>
76#endif
77
241class VISP_EXPORT vpMbEdgeTracker : public virtual vpMbTracker
242{
243protected:
249 std::vector<std::list<vpMbtDistanceLine *> > lines;
250
252 std::vector<std::list<vpMbtDistanceCircle *> > circles;
253
255 std::vector<std::list<vpMbtDistanceCylinder *> > cylinders;
256
259 unsigned int nline;
260
263 unsigned int ncircle;
264
267 unsigned int ncylinder;
268
270 unsigned int nbvisiblepolygone;
271
275
277 std::vector<bool> scales;
278
281 std::vector<const vpImage<unsigned char> *> Ipyramid;
282
286 unsigned int scaleLevel;
287
290
313 std::vector<std::vector<double> > m_featuresToBeDisplayedEdge;
314
315public:
317 virtual ~vpMbEdgeTracker();
318
321
322 virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
323 const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);
324 virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
325 const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);
326
327 void getLline(std::list<vpMbtDistanceLine *> &linesList, unsigned int level = 0) const;
328 void getLcircle(std::list<vpMbtDistanceCircle *> &circlesList, unsigned int level = 0) const;
329 void getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList, unsigned int level = 0) const;
330
331 virtual std::vector<std::vector<double> > getModelForDisplay(unsigned int width, unsigned int height,
332 const vpHomogeneousMatrix &cMo,
333 const vpCameraParameters &cam,
334 bool displayFullModel = false);
335
342 virtual inline void getMovingEdge(vpMe &p_me) const { p_me = this->me; }
348 virtual inline vpMe getMovingEdge() const { return this->me; }
349
350 virtual unsigned int getNbPoints(unsigned int level = 0) const;
351
357 std::vector<bool> getScales() const { return scales; }
367 inline double getGoodMovingEdgesRatioThreshold() const { return percentageGdPt; }
368
369 virtual inline vpColVector getError() const { return m_error_edge; }
370
371 virtual inline vpColVector getRobustWeights() const { return m_w_edge; }
372
373 virtual void loadConfigFile(const std::string &configFile, bool verbose = true);
374
375 virtual void reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo,
376 bool verbose = false, const vpHomogeneousMatrix &T = vpHomogeneousMatrix());
377 void resetTracker();
378
385 {
386 m_cam = cam;
387
388 for (unsigned int i = 0; i < scales.size(); i += 1) {
389 if (scales[i]) {
390 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
391 (*it)->setCameraParameters(m_cam);
392 }
393
394 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
395 ++it) {
396 (*it)->setCameraParameters(m_cam);
397 }
398
399 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
400 (*it)->setCameraParameters(m_cam);
401 }
402 }
403 }
404 }
405
406 virtual void setClipping(const unsigned int &flags);
407
408 virtual void setFarClippingDistance(const double &dist);
409
410 virtual void setNearClippingDistance(const double &dist);
411
420 virtual void setOgreVisibilityTest(const bool &v)
421 {
423#ifdef VISP_HAVE_OGRE
424 faces.getOgreContext()->setWindowName("MBT Edge");
425#endif
426 }
427
433 virtual void setScanLineVisibilityTest(const bool &v)
434 {
436
437 for (unsigned int i = 0; i < scales.size(); i += 1) {
438 if (scales[i]) {
439 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
440 (*it)->useScanLine = v;
441 }
442 }
443 }
444 }
445
459 void setGoodMovingEdgesRatioThreshold(double threshold) { percentageGdPt = threshold; }
460
461 void setMovingEdge(const vpMe &me);
462
463 virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo);
464 virtual void setPose(const vpImage<vpRGBa> &I_color, const vpHomogeneousMatrix &cdMo);
465
466 void setScales(const std::vector<bool> &_scales);
467
468 void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking);
469
470 virtual void track(const vpImage<unsigned char> &I);
471 virtual void track(const vpImage<vpRGBa> &I);
473
474protected:
477 void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace = -1,
478 const std::string &name = "");
479 void addCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace = -1, const std::string &name = "");
480 void addLine(vpPoint &p1, vpPoint &p2, int polygon = -1, std::string name = "");
481 void addPolygon(vpMbtPolygon &p);
482
483 void cleanPyramid(std::vector<const vpImage<unsigned char> *> &_pyramid);
484 void computeProjectionError(const vpImage<unsigned char> &_I);
485
486 void computeVVS(const vpImage<unsigned char> &_I, unsigned int lvl);
487 void computeVVSFirstPhase(const vpImage<unsigned char> &I, unsigned int iter, double &count, unsigned int lvl = 0);
488 void computeVVSFirstPhaseFactor(const vpImage<unsigned char> &I, unsigned int lvl = 0);
489 void computeVVSFirstPhasePoseEstimation(unsigned int iter, bool &isoJoIdentity);
490 virtual void computeVVSInit();
493 virtual void computeVVSWeights();
495
496 void displayFeaturesOnImage(const vpImage<unsigned char> &I);
497 void displayFeaturesOnImage(const vpImage<vpRGBa> &I);
498 void downScale(const unsigned int _scale);
499 virtual std::vector<std::vector<double> > getFeaturesForDisplayEdge();
500 virtual void init(const vpImage<unsigned char> &I);
501 virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace = 0,
502 const std::string &name = "");
503 virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace = 0,
504 const std::string &name = "");
505 virtual void initFaceFromCorners(vpMbtPolygon &polygon);
506 virtual void initFaceFromLines(vpMbtPolygon &polygon);
507 unsigned int initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders,
508 unsigned int &nberrors_circles);
509 void initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo);
510 void initPyramid(const vpImage<unsigned char> &_I, std::vector<const vpImage<unsigned char> *> &_pyramid);
511 void reInitLevel(const unsigned int _lvl);
512 void reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo);
513 void removeCircle(const std::string &name);
514 void removeCylinder(const std::string &name);
515 void removeLine(const std::string &name);
516 void resetMovingEdge();
517 virtual void testTracking();
518 void trackMovingEdge(const vpImage<unsigned char> &I);
519 void updateMovingEdge(const vpImage<unsigned char> &I);
520 void updateMovingEdgeWeights();
521 void upScale(const unsigned int _scale);
522 void visibleFace(const vpImage<unsigned char> &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline);
524};
525
526#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition of the vpImage class member functions.
Definition vpImage.h:135
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
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)
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
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.
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)
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 vpMe.h:122
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
Contains an M-estimator and various influence function.
Definition vpRobust.h:83