Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMbDepthDenseTracker.cpp
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 * Model-based tracker using depth dense features.
33 *
34*****************************************************************************/
35
36#include <iostream>
37
38#include <visp3/core/vpConfig.h>
39
40#ifdef VISP_HAVE_PCL
41#include <pcl/point_cloud.h>
42#endif
43
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>
49
50#if DEBUG_DISPLAY_DEPTH_DENSE
51#include <visp3/gui/vpDisplayGDI.h>
52#include <visp3/gui/vpDisplayX.h>
53#endif
54
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
60 ,
61 m_debugDisp_depthDense(NULL), m_debugImage_depthDense()
62#endif
63{
64#ifdef VISP_HAVE_OGRE
65 faces.getOgreContext()->setWindowName("MBT Depth Dense");
66#endif
67
68#if defined(VISP_HAVE_X11) && DEBUG_DISPLAY_DEPTH_DENSE
69 m_debugDisp_depthDense = new vpDisplayX;
70#elif defined(VISP_HAVE_GDI) && DEBUG_DISPLAY_DEPTH_DENSE
71 m_debugDisp_depthDense = new vpDisplayGDI;
72#endif
73}
74
76{
77 for (size_t i = 0; i < m_depthDenseFaces.size(); i++) {
78 delete m_depthDenseFaces[i];
79 }
80
81#if DEBUG_DISPLAY_DEPTH_DENSE
82 delete m_debugDisp_depthDense;
83#endif
84}
85
86void vpMbDepthDenseTracker::addFace(vpMbtPolygon &polygon, bool alreadyClose)
87{
88 if (polygon.nbpt < 3) {
89 return;
90 }
91
92 // Copy hidden faces
94
96 normal_face->m_hiddenFace = &faces;
97 normal_face->m_polygon = &polygon;
98 normal_face->m_cam = m_cam;
99 normal_face->m_useScanLine = useScanLine;
100 normal_face->m_clippingFlag = clippingFlag;
101 normal_face->m_distNearClip = distNearClip;
102 normal_face->m_distFarClip = distFarClip;
103
104 // Add lines that compose the face
105 unsigned int nbpt = polygon.getNbPoint();
106 if (nbpt > 0) {
107 for (unsigned int i = 0; i < nbpt - 1; i++) {
108 normal_face->addLine(polygon.p[i], polygon.p[i + 1], &m_depthDenseHiddenFacesDisplay, m_rand, polygon.getIndex(),
109 polygon.getName());
110 }
111
112 if (!alreadyClose) {
113 // Add last line that closes the face
114 normal_face->addLine(polygon.p[nbpt - 1], polygon.p[0], &m_depthDenseHiddenFacesDisplay, m_rand,
115 polygon.getIndex(), polygon.getName());
116 }
117 }
118
119 // Construct a vpPlane in object frame
120 vpPoint pts[3];
121 pts[0] = polygon.p[0];
122 pts[1] = polygon.p[1];
123 pts[2] = polygon.p[2];
124 normal_face->m_planeObject = vpPlane(pts[0], pts[1], pts[2], vpPlane::object_frame);
125
126 m_depthDenseFaces.push_back(normal_face);
127}
128
129void vpMbDepthDenseTracker::computeVisibility(unsigned int width, unsigned int height)
130{
131 bool changed = false;
132 faces.setVisible(width, height, m_cam, m_cMo, angleAppears, angleDisappears, changed);
133
134 if (useScanLine) {
135 // if (clippingFlag <= 2) {
136 // cam.computeFov(width, height);
137 // }
138
140 faces.computeScanLineRender(m_cam, width, height);
141 }
142
143 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
144 ++it) {
145 vpMbtFaceDepthDense *face_normal = *it;
146 face_normal->computeVisibility();
147 }
148}
149
151{
152 double normRes = 0;
153 double normRes_1 = -1;
154 unsigned int iter = 0;
155
157
159 vpMatrix LTL;
160 vpColVector LTR, v;
161
162 double mu = m_initialMu;
163 vpHomogeneousMatrix cMo_prev;
164
165 bool isoJoIdentity = m_isoJoIdentity; // Backup since it can be modified if L is not full rank
166 if (isoJoIdentity)
167 oJo.eye();
168
170 vpMatrix L_true, LVJ_true;
171
172 while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) {
174
175 bool reStartFromLastIncrement = false;
176 computeVVSCheckLevenbergMarquardt(iter, m_error_depthDense, error_prev, cMo_prev, mu, reStartFromLastIncrement);
177
178 if (!reStartFromLastIncrement) {
180
181 if (computeCovariance) {
182 L_true = m_L_depthDense;
183 if (!isoJoIdentity) {
184 cVo.buildFrom(m_cMo);
185 LVJ_true = (m_L_depthDense * cVo * oJo);
186 }
187 }
188
189 // Compute DoF only once
190 if (iter == 0) {
191 // If all the 6 dof should be estimated, we check if the interaction
192 // matrix is full rank. If not we remove automatically the dof that
193 // cannot be estimated. This is particularly useful when considering
194 // circles (rank 5) and cylinders (rank 4)
195 if (isoJoIdentity) {
196 cVo.buildFrom(m_cMo);
197
198 vpMatrix K; // kernel
199 unsigned int rank = (m_L_depthDense * cVo).kernel(K);
200 if (rank == 0) {
201 throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
202 }
203
204 if (rank != 6) {
205 vpMatrix I; // Identity
206 I.eye(6);
207 oJo = I - K.AtA();
208
209 isoJoIdentity = false;
210 }
211 }
212 }
213
214 double num = 0.0, den = 0.0;
215 for (unsigned int i = 0; i < m_L_depthDense.getRows(); i++) {
216 // Compute weighted errors and stop criteria
219 den += m_w_depthDense[i];
220
221 // weight interaction matrix
222 for (unsigned int j = 0; j < 6; j++) {
223 m_L_depthDense[i][j] *= m_w_depthDense[i];
224 }
225 }
226
228 m_error_depthDense, error_prev, LTR, mu, v);
229
230 cMo_prev = m_cMo;
232
233 normRes_1 = normRes;
234 normRes = sqrt(num / den);
235 }
236
237 iter++;
238 }
239
240 computeCovarianceMatrixVVS(isoJoIdentity, m_w_depthDense, cMo_prev, L_true, LVJ_true, m_error_depthDense);
241}
242
244{
246
247 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseListOfActiveFaces.begin();
248 it != m_depthDenseListOfActiveFaces.end(); ++it) {
249 vpMbtFaceDepthDense *face = *it;
251 }
252
256
258 m_w_depthDense = 1;
259}
260
262{
263 unsigned int start_index = 0;
264 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseListOfActiveFaces.begin();
265 it != m_depthDenseListOfActiveFaces.end(); ++it) {
266 vpMbtFaceDepthDense *face = *it;
267
268 vpMatrix L_face;
269 vpColVector error;
270
271 face->computeInteractionMatrixAndResidu(m_cMo, L_face, error);
272
273 m_error_depthDense.insert(start_index, error);
274 m_L_depthDense.insert(L_face, start_index, 0);
275
276 start_index += error.getRows();
277 }
278}
279
284
286 const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
287 bool displayFullModel)
288{
289 std::vector<std::vector<double> > models =
290 vpMbDepthDenseTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
291
292 for (size_t i = 0; i < models.size(); i++) {
293 if (vpMath::equal(models[i][0], 0)) {
294 vpImagePoint ip1(models[i][1], models[i][2]);
295 vpImagePoint ip2(models[i][3], models[i][4]);
296 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
297 }
298 }
299}
300
302 const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
303 bool displayFullModel)
304{
305 std::vector<std::vector<double> > models =
306 vpMbDepthDenseTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
307
308 for (size_t i = 0; i < models.size(); i++) {
309 if (vpMath::equal(models[i][0], 0)) {
310 vpImagePoint ip1(models[i][1], models[i][2]);
311 vpImagePoint ip2(models[i][3], models[i][4]);
312 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
313 }
314 }
315}
316
332std::vector<std::vector<double> > vpMbDepthDenseTracker::getModelForDisplay(unsigned int width, unsigned int height,
333 const vpHomogeneousMatrix &cMo,
334 const vpCameraParameters &cam,
335 bool displayFullModel)
336{
337 std::vector<std::vector<double> > models;
338
339 vpCameraParameters c = cam;
340
341 bool changed = false;
343
344 if (useScanLine) {
345 c.computeFov(width, height);
346
349 }
350
351 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
352 ++it) {
353 vpMbtFaceDepthDense *face_dense = *it;
354 std::vector<std::vector<double> > modelLines =
355 face_dense->getModelForDisplay(width, height, cMo, cam, displayFullModel);
356 models.insert(models.end(), modelLines.begin(), modelLines.end());
357 }
358
359 return models;
360}
361
363{
364 if (!modelInitialised) {
365 throw vpException(vpException::fatalError, "model not initialized");
366 }
367
368 bool reInitialisation = false;
369 if (!useOgre) {
371 } else {
372#ifdef VISP_HAVE_OGRE
373 if (!faces.isOgreInitialised()) {
377 // Turn off Ogre config dialog display for the next call to this
378 // function since settings are saved in the ogre.cfg file and used
379 // during the next call
380 ogreShowConfigDialog = false;
381 }
382
384#else
386#endif
387 }
388
389 if (useScanLine || clippingFlag > 3)
391
393}
394
395void vpMbDepthDenseTracker::loadConfigFile(const std::string &configFile, bool verbose)
396{
398 xmlp.setVerbose(verbose);
402
405
406 try {
407 if (verbose) {
408 std::cout << " *********** Parsing XML for Mb Depth Dense Tracker ************ " << std::endl;
409 }
410 xmlp.parse(configFile);
411 } catch (const vpException &e) {
412 std::cerr << "Exception: " << e.what() << std::endl;
413 throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str());
414 }
415
416 vpCameraParameters camera;
417 xmlp.getCameraParameters(camera);
418 setCameraParameters(camera);
419
422
423 if (xmlp.hasNearClippingDistance())
425
426 if (xmlp.hasFarClippingDistance())
428
429 if (xmlp.getFovClipping())
431
433}
434
435void vpMbDepthDenseTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
436 const vpHomogeneousMatrix &cMo, bool verbose)
437{
438 m_cMo.eye();
439
440 for (size_t i = 0; i < m_depthDenseFaces.size(); i++) {
441 delete m_depthDenseFaces[i];
442 m_depthDenseFaces[i] = NULL;
443 }
444
445 m_depthDenseFaces.clear();
446
447 loadModel(cad_name, verbose);
448 initFromPose(I, cMo);
449}
450
451#if defined(VISP_HAVE_PCL)
452void vpMbDepthDenseTracker::reInitModel(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
453 const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose)
454{
455 vpImage<unsigned char> I_dummy(point_cloud->height, point_cloud->width);
456 reInitModel(I_dummy, cad_name, cMo, verbose);
457}
458
459#endif
460
462{
463 m_cMo.eye();
464
465 for (std::vector<vpMbtFaceDepthDense *>::iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
466 ++it) {
467 vpMbtFaceDepthDense *normal_face = *it;
468 delete normal_face;
469 normal_face = NULL;
470 }
471
472 m_depthDenseFaces.clear();
473
475 computeCovariance = false;
476
479
481
482 m_lambda = 1.0;
483 m_maxIter = 30;
484
485 faces.reset();
486
488
489 useScanLine = false;
490
491#ifdef VISP_HAVE_OGRE
492 useOgre = false;
493#endif
494
496}
497
499{
500 m_cam = cam;
501
502 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
503 ++it) {
504 (*it)->setCameraParameters(cam);
505 }
506}
507
509{
510 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
511 ++it) {
512 (*it)->setDepthDenseFilteringMaxDistance(maxDistance);
513 }
514}
515
517{
518 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
519 ++it) {
520 (*it)->setDepthDenseFilteringMethod(method);
521 }
522}
523
525{
526 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
527 ++it) {
528 (*it)->setDepthDenseFilteringMinDistance(minDistance);
529 }
530}
531
533{
534 if (occupancyRatio < 0.0 || occupancyRatio > 1.0) {
535 std::cerr << "occupancyRatio < 0.0 || occupancyRatio > 1.0" << std::endl;
536 return;
537 }
538
539 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
540 ++it) {
541 (*it)->setDepthDenseFilteringOccupancyRatio(occupancyRatio);
542 }
543}
544
545#ifdef VISP_HAVE_PCL
546void vpMbDepthDenseTracker::segmentPointCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
547{
549
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");
554 }
555
556 m_debugImage_depthDense = 0;
557 std::vector<std::vector<vpImagePoint> > roiPts_vec;
558#endif
559
560 for (std::vector<vpMbtFaceDepthDense *>::iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
561 ++it) {
562 vpMbtFaceDepthDense *face = *it;
563
564 if (face->isVisible() && face->isTracked()) {
565#if DEBUG_DISPLAY_DEPTH_DENSE
566 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
567#endif
569#if DEBUG_DISPLAY_DEPTH_DENSE
570 ,
571 m_debugImage_depthDense, roiPts_vec_
572#endif
573 ,
574 m_mask)) {
575 m_depthDenseListOfActiveFaces.push_back(*it);
576
577#if DEBUG_DISPLAY_DEPTH_DENSE
578 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
579#endif
580 }
581 }
582 }
583
584#if DEBUG_DISPLAY_DEPTH_DENSE
585 vpDisplay::display(m_debugImage_depthDense);
586
587 for (size_t i = 0; i < roiPts_vec.size(); i++) {
588 if (roiPts_vec[i].empty())
589 continue;
590
591 for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
592 vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2);
593 }
594 vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
595 vpColor::red, 2);
596 }
597
598 vpDisplay::flush(m_debugImage_depthDense);
599#endif
600}
601#endif
602
603void vpMbDepthDenseTracker::segmentPointCloud(const std::vector<vpColVector> &point_cloud, unsigned int width,
604 unsigned int height)
605{
607
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");
612 }
613
614 m_debugImage_depthDense = 0;
615 std::vector<std::vector<vpImagePoint> > roiPts_vec;
616#endif
617
618 for (std::vector<vpMbtFaceDepthDense *>::iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
619 ++it) {
620 vpMbtFaceDepthDense *face = *it;
621
622 if (face->isVisible() && face->isTracked()) {
623#if DEBUG_DISPLAY_DEPTH_DENSE
624 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
625#endif
626 if (face->computeDesiredFeatures(m_cMo, width, height, point_cloud, m_depthDenseSamplingStepX,
628#if DEBUG_DISPLAY_DEPTH_DENSE
629 ,
630 m_debugImage_depthDense, roiPts_vec_
631#endif
632 ,
633 m_mask)) {
634 m_depthDenseListOfActiveFaces.push_back(*it);
635
636#if DEBUG_DISPLAY_DEPTH_DENSE
637 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
638#endif
639 }
640 }
641 }
642
643#if DEBUG_DISPLAY_DEPTH_DENSE
644 vpDisplay::display(m_debugImage_depthDense);
645
646 for (size_t i = 0; i < roiPts_vec.size(); i++) {
647 if (roiPts_vec[i].empty())
648 continue;
649
650 for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
651 vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2);
652 }
653 vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
654 vpColor::red, 2);
655 }
656
657 vpDisplay::flush(m_debugImage_depthDense);
658#endif
659}
660
662{
664#ifdef VISP_HAVE_OGRE
665 faces.getOgreContext()->setWindowName("MBT Depth Dense");
666#endif
667}
668
670{
671 m_cMo = cdMo;
672 init(I);
673}
674
676{
677 m_cMo = cdMo;
679 init(m_I);
680}
681
682#if defined(VISP_HAVE_PCL)
683void vpMbDepthDenseTracker::setPose(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
684 const vpHomogeneousMatrix &cdMo)
685{
686 m_I.resize(point_cloud->height, point_cloud->width);
687 m_cMo = cdMo;
688 init(m_I);
689}
690#endif
691
693{
695
696 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
697 ++it) {
698 (*it)->setScanLineVisibilityTest(v);
699 }
700}
701
702void vpMbDepthDenseTracker::setUseDepthDenseTracking(const std::string &name, const bool &useDepthDenseTracking)
703{
704 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
705 ++it) {
706 vpMbtFaceDepthDense *face = *it;
707 if (face->m_polygon->getName() == name) {
708 face->setTracked(useDepthDenseTracking);
709 }
710 }
711}
712
714
716{
717 throw vpException(vpException::fatalError, "Cannot track with a grayscale image!");
718}
719
721{
722 throw vpException(vpException::fatalError, "Cannot track with a color image!");
723}
724
725#ifdef VISP_HAVE_PCL
726void vpMbDepthDenseTracker::track(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
727{
728 segmentPointCloud(point_cloud);
729
730 computeVVS();
731
732 computeVisibility(point_cloud->width, point_cloud->height);
733}
734#endif
735
736void vpMbDepthDenseTracker::track(const std::vector<vpColVector> &point_cloud, unsigned int width, unsigned int height)
737{
738 segmentPointCloud(point_cloud, width, height);
739
740 computeVVS();
741
742 computeVisibility(width, height);
743}
744
745void vpMbDepthDenseTracker::initCircle(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const vpPoint & /*p3*/,
746 double /*radius*/, int /*idFace*/, const std::string & /*name*/)
747{
748 throw vpException(vpException::fatalError, "vpMbDepthDenseTracker::initCircle() should not be called!");
749}
750
751void vpMbDepthDenseTracker::initCylinder(const vpPoint & /*p1*/, const vpPoint & /*p2*/, double /*radius*/,
752 int /*idFace*/, const std::string & /*name*/)
753{
754 throw vpException(vpException::fatalError, "vpMbDepthDenseTracker::initCylinder() should not be called!");
755}
756
758
void setWindowName(const Ogre::String &n)
Definition vpAROgre.h:269
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
unsigned int getRows() const
Definition vpArray2D.h:290
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.
Definition vpColor.h:152
static const vpColor red
Definition vpColor.h:211
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...
Definition vpDisplayX.h:132
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.
Definition vpException.h:59
@ ioError
I/O error.
Definition vpException.h:79
@ fatalError
Fatal error.
Definition vpException.h:84
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.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition vpImage.h:795
unsigned int getHeight() const
Definition vpImage.h:184
static double rad(double deg)
Definition vpMath.h:116
static double sqr(double x)
Definition vpMath.h:124
static bool equal(double x, double y, double threshold=0.001)
Definition vpMath.h:369
static double deg(double rad)
Definition vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void eye()
Definition vpMatrix.cpp:446
vpMatrix AtA() const
Definition vpMatrix.cpp:625
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()
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)
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 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 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.
bool modelInitialised
virtual void computeVVSPoseEstimation(const bool isoJoIdentity, unsigned int iter, vpMatrix &L, vpMatrix &LTL, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector &LTR, 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
int getIndex() const
Parse an Xml file to extract configuration parameters of a mbtConfig object.
void getCameraParameters(vpCameraParameters &cam) const
void setDepthDenseSamplingStepY(unsigned int stepY)
void setAngleDisappear(const double &adisappear)
void setDepthDenseSamplingStepX(unsigned int stepX)
void setAngleAppear(const double &aappear)
unsigned int getDepthDenseSamplingStepY() const
void parse(const std::string &filename)
unsigned int getDepthDenseSamplingStepX() const
void setCameraParameters(const vpCameraParameters &cam)
This class defines the container for a plane geometrical structure.
Definition vpPlane.h:54
@ object_frame
Definition vpPlane.h:65
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
unsigned int nbpt
Number of points used to define the polygon.
Definition vpPolygon3D.h:71
unsigned int getNbPoint() const
vpPoint * p
corners in the object frame
Definition vpPolygon3D.h:76
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)