diff --git a/modules/python/bindings/include/rbt/feature_tracker.hpp b/modules/python/bindings/include/rbt/feature_tracker.hpp index c2b2f76252..e51d96aa64 100644 --- a/modules/python/bindings/include/rbt/feature_tracker.hpp +++ b/modules/python/bindings/include/rbt/feature_tracker.hpp @@ -97,14 +97,14 @@ class TrampolineRBFeatureTracker : public vpRBFeatureTracker override(&frame, cMo, iteration); } } - virtual void display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth, const vpRBFeatureDisplayType type) const VP_OVERRIDE + virtual void display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth) const VP_OVERRIDE { pybind11::gil_scoped_acquire gil; // Acquire the GIL while in this scope. // Try to look up the overridden method on the Python side. pybind11::function override = pybind11::get_override(this, "display"); if (override) { // method is found // Pybind seems to copy the frames, so we pass the pointers - override(cam, &I, &IRGB, &depth, type); + override(cam, &I, &IRGB, &depth); } } virtual const vpMatrix getCovariance() const VP_OVERRIDE diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h index 39238ea161..07e1e9c3aa 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h @@ -128,7 +128,7 @@ class VISP_EXPORT vpRBDenseDepthTracker : public vpRBFeatureTracker void initVVS(const vpRBFeatureTrackerInput &/*frame*/, const vpRBFeatureTrackerInput &/*previousFrame*/, const vpHomogeneousMatrix &/*cMo*/) VP_OVERRIDE { } void computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int iteration) VP_OVERRIDE; - void display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth, const vpRBFeatureDisplayType type) const VP_OVERRIDE; + void display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth) const VP_OVERRIDE; struct vpDepthPoint { diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h index 838e98ee74..dd7a02a7c3 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h @@ -50,12 +50,6 @@ #include VISP_NLOHMANN_JSON(json.hpp) #endif -enum vpRBFeatureDisplayType -{ - SIMPLE = 0, - IMPORTANCE = 1, - ERROR = 2 -}; BEGIN_VISP_NAMESPACE /** @@ -155,8 +149,7 @@ class VISP_EXPORT vpRBFeatureTracker const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, - const vpImage &depth, - const vpRBFeatureDisplayType type) const = 0; + const vpImage &depth) const = 0; /** * @} diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBKltTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBKltTracker.h index a2fa487027..6c0dffd5b5 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBKltTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBKltTracker.h @@ -83,7 +83,7 @@ class VISP_EXPORT vpRBKltTracker : public vpRBFeatureTracker void computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int iteration) VP_OVERRIDE; - void display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth, const vpRBFeatureDisplayType type) const VP_OVERRIDE; + void display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth) const VP_OVERRIDE; /** * \name Settings diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h index c9aa9384bf..97997a5d46 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h @@ -61,6 +61,15 @@ BEGIN_VISP_NAMESPACE + +enum vpRBSilhouetteCCDDisplayType +{ + SIMPLE = 0, + WEIGHT = 1, + ERROR = 2, + INVALID = 3 +}; + class VISP_EXPORT vpCCDParameters { public: @@ -239,6 +248,11 @@ class VISP_EXPORT vpRBSilhouetteCCDTracker : public vpRBFeatureTracker m_minMaskConfidence = confidence; } + void setDisplayType(vpRBSilhouetteCCDDisplayType type) + { + m_displayType = type; + } + /** * @} */ @@ -258,7 +272,7 @@ class VISP_EXPORT vpRBSilhouetteCCDTracker : public vpRBFeatureTracker m_cov = m_sigma; } - void display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth, const vpRBFeatureDisplayType type) const VP_OVERRIDE; + void display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth) const VP_OVERRIDE; #if defined(VISP_HAVE_NLOHMANN_JSON) virtual void loadJsonConfiguration(const nlohmann::json &j) VP_OVERRIDE @@ -303,6 +317,8 @@ class VISP_EXPORT vpRBSilhouetteCCDTracker : public vpRBFeatureTracker bool m_useMask; double m_minMaskConfidence; + + vpRBSilhouetteCCDDisplayType m_displayType; }; END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteMeTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteMeTracker.h index a44437d2d4..fc25cc2a14 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteMeTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteMeTracker.h @@ -82,7 +82,7 @@ class VISP_EXPORT vpRBSilhouetteMeTracker : public vpRBFeatureTracker void computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int iteration) VP_OVERRIDE; - void display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth, const vpRBFeatureDisplayType type) const VP_OVERRIDE; + void display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth) const VP_OVERRIDE; /** * \name Settings diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h index 9d794f7557..838c6ca1fc 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h @@ -211,7 +211,7 @@ class VISP_EXPORT vpRBTracker * @{ */ void displayMask(vpImage &Imask) const; - void display(const vpImage &I, const vpImage &IRGB, const vpImage &depth, const vpRBFeatureDisplayType type); + void display(const vpImage &I, const vpImage &IRGB, const vpImage &depth); /** * @} */ @@ -272,8 +272,6 @@ class VISP_EXPORT vpRBTracker std::shared_ptr m_driftDetector; std::shared_ptr m_odometry; - - // vpRBTrackerFilter m_filter; }; END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/core/vpRBTracker.cpp b/modules/tracker/rbt/src/core/vpRBTracker.cpp index f70cb421dd..dc250cf88d 100644 --- a/modules/tracker/rbt/src/core/vpRBTracker.cpp +++ b/modules/tracker/rbt/src/core/vpRBTracker.cpp @@ -555,7 +555,7 @@ void vpRBTracker::displayMask(vpImage &Imask) const } } -void vpRBTracker::display(const vpImage &I, const vpImage &IRGB, const vpImage &depth, const vpRBFeatureDisplayType type) +void vpRBTracker::display(const vpImage &I, const vpImage &IRGB, const vpImage &depth) { if (m_currentFrame.renders.normals.getSize() == 0) { return; @@ -564,15 +564,13 @@ void vpRBTracker::display(const vpImage &I, const vpImage for (std::shared_ptr &tracker : m_trackers) { if (tracker->featuresShouldBeDisplayed()) { - tracker->display(m_currentFrame.cam, I, IRGB, depth, type); + tracker->display(m_currentFrame.cam, I, IRGB, depth); } } if (m_driftDetector) { m_driftDetector->display(IRGB); } - - // vpDisplay::displayRectangle(IRGB, m_renderer.getBoundingBox(), vpColor::red); } vpObjectCentricRenderer &vpRBTracker::getRenderer() @@ -594,7 +592,6 @@ void vpRBTracker::loadConfigurationFile(const std::string &filename) catch (nlohmann::json::parse_error &e) { std::stringstream msg; msg << "Could not parse JSON file : \n"; - msg << e.what() << std::endl; msg << "Byte position of error: " << e.byte; throw vpException(vpException::ioError, msg.str()); diff --git a/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp b/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp index ecfee46fce..6b3ec0e50f 100644 --- a/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp +++ b/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp @@ -152,8 +152,7 @@ void vpRBDenseDepthTracker::computeVVSIter(const vpRBFeatureTrackerInput &/*fram } void vpRBDenseDepthTracker::display(const vpCameraParameters &/*cam*/, const vpImage &/*I*/, - const vpImage &/*IRGB*/, const vpImage &depth, - const vpRBFeatureDisplayType /*type*/) const + const vpImage &/*IRGB*/, const vpImage &depth) const { for (unsigned int i = 0; i < m_depthPoints.size(); ++i) { const vpDepthPoint &p = m_depthPoints[i]; diff --git a/modules/tracker/rbt/src/features/vpRBKltTracker.cpp b/modules/tracker/rbt/src/features/vpRBKltTracker.cpp index b86a206027..91f284a7bc 100644 --- a/modules/tracker/rbt/src/features/vpRBKltTracker.cpp +++ b/modules/tracker/rbt/src/features/vpRBKltTracker.cpp @@ -311,8 +311,7 @@ void vpRBKltTracker::computeVVSIter(const vpRBFeatureTrackerInput &/*frame*/, co } void vpRBKltTracker::display(const vpCameraParameters &cam, const vpImage &I, - const vpImage &/*IRGB*/, const vpImage &/*depth*/, - const vpRBFeatureDisplayType /*type*/) const + const vpImage &/*IRGB*/, const vpImage &/*depth*/) const { for (const std::pair &p : m_points) { double u = 0.0, v = 0.0; diff --git a/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp b/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp index 1729a3fe4d..c9b5e9166e 100644 --- a/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp +++ b/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp @@ -141,7 +141,7 @@ template class FastVec3 }; -vpRBSilhouetteCCDTracker::vpRBSilhouetteCCDTracker() : vpRBFeatureTracker(), m_vvsConvergenceThreshold(0.0), m_temporalSmoothingFac(0.1), m_useMask(false), m_minMaskConfidence(0.0) +vpRBSilhouetteCCDTracker::vpRBSilhouetteCCDTracker() : vpRBFeatureTracker(), m_vvsConvergenceThreshold(0.0), m_temporalSmoothingFac(0.1), m_useMask(false), m_minMaskConfidence(0.0), m_displayType(vpRBSilhouetteCCDDisplayType::SIMPLE) { } void vpRBSilhouetteCCDTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput & /*previousFrame*/, const vpHomogeneousMatrix &/*cMo*/) @@ -258,12 +258,11 @@ void vpRBSilhouetteCCDTracker::computeVVSIter(const vpRBFeatureTrackerInput &fra } void vpRBSilhouetteCCDTracker::display(const vpCameraParameters &/*cam*/, const vpImage &/*I*/, - const vpImage &IRGB, const vpImage &/*depth*/, - const vpRBFeatureDisplayType type) const + const vpImage &IRGB, const vpImage &/*depth*/) const { unsigned normal_points_number = floor(m_ccdParameters.h / m_ccdParameters.delta_h); unsigned nerror_per_point = 2 * normal_points_number * 3; - if (type == vpRBFeatureDisplayType::SIMPLE) { + if (m_displayType == vpRBSilhouetteCCDDisplayType::SIMPLE) { for (unsigned int i = 0; i < m_controlPoints.size(); ++i) { const vpRBSilhouetteControlPoint &p = m_controlPoints[i]; @@ -274,7 +273,7 @@ void vpRBSilhouetteCCDTracker::display(const vpCameraParameters &/*cam*/, const // vpDisplay::displayArrow(IRGB, p.icpoint, ip2, p.invnormal ? vpColor::red : vpColor::lightBlue); } } - else if (type == vpRBFeatureDisplayType::ERROR) { + else if (m_displayType == vpRBSilhouetteCCDDisplayType::ERROR) { vpColVector errorPerPoint(m_controlPoints.size()); double maxPointError = 0.0; for (unsigned int i = 0; i < m_controlPoints.size(); ++i) { @@ -305,7 +304,7 @@ void vpRBSilhouetteCCDTracker::display(const vpCameraParameters &/*cam*/, const ++idx; } } - else if (type == vpRBFeatureDisplayType::IMPORTANCE) { + else if (m_displayType == vpRBSilhouetteCCDDisplayType::WEIGHT) { vpColVector weightPerPoint(m_controlPoints.size()); for (unsigned int i = 0; i < m_controlPoints.size(); ++i) { double sum = 0.0; @@ -387,7 +386,7 @@ void vpRBSilhouetteCCDTracker::computeLocalStatistics(const vpImage &I, #if VISP_DEBUG_CCD_TRACKER if (std::isnan(nv_ptr[0]) || std::isnan(nv_ptr[1])) { throw vpException(vpException::fatalError, "x: %f, theta = %f", p.xs, p.getTheta()); - } + } #endif int k = 0; @@ -466,7 +465,7 @@ void vpRBSilhouetteCCDTracker::computeLocalStatistics(const vpImage &I, vic_ptr[10 * negative_normal + 9] = exp(-dist2[0] * dist2[0] / (2 * sigma * sigma)) / (sqrt(2 * CV_PI) * sigma); normalized_param[kk][1] += vic_ptr[10 * negative_normal + 7]; } -} + } #ifdef VISP_HAVE_OPENMP #pragma omp parallel for diff --git a/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp b/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp index e9872a95c3..f9e058acf1 100644 --- a/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp +++ b/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp @@ -184,80 +184,14 @@ void vpRBSilhouetteMeTracker::computeVVSIter(const vpRBFeatureTrackerInput &fram } void vpRBSilhouetteMeTracker::display(const vpCameraParameters &/*cam*/, const vpImage &I, - const vpImage &/*IRGB*/, const vpImage &/*depth*/, - const vpRBFeatureDisplayType type) const + const vpImage &/*IRGB*/, const vpImage &/*depth*/) const { - if (type == vpRBFeatureDisplayType::SIMPLE) { - for (const vpRBSilhouetteControlPoint &p: m_controlPoints) { - const vpMeSite &s = p.getSite(); - s.display(I); - // vpImagePoint diff(p.nys * m_me.getRange(), p.nxs * m_me.getRange()); - // vpImagePoint ip2 = p.icpoint + diff; - // vpDisplay::displayLine(I, p.icpoint, ip2, vpColor::lightBlue, 2); - // vpDisplay::displayPoint(I, p.icpoint, vpColor::red, 2); - - } - // vpColor cs[6] = { - // vpColor::red, - // vpColor::blue, - // vpColor::green, - // vpColor::purple, - // vpColor::cyan, - // vpColor::darkGreen - - // }; - // unsigned colorIndex = 0; - // for (const vpTrackedSilhouetteLine &line: m_lines) { - // if (line.getPoints().size() > 10) { - // for (const vpRBSilhouetteControlPoint *p: line.getPoints()) { - // vpDisplay::displayCross(I, p->getSite().m_i, p->getSite().m_j, 3, cs[colorIndex]); - // } - // colorIndex = (colorIndex + 1) % 6; - // } - // //line.getLine().display(I, cam); - // } - } - else if (type == vpRBFeatureDisplayType::IMPORTANCE) { - const double maxWeight = m_weights.getMaxValue(); - unsigned idx = 0; - const vpColor bestColor = vpColor::green; - for (const vpRBSilhouetteControlPoint &p: m_controlPoints) { - const vpMeSite &s = p.getSite(); - - if (s.getState() == vpMeSite::NO_SUPPRESSION) { - double weight = m_weights[idx] / maxWeight; - vpColor c((unsigned char)((double)(bestColor.R) * weight), (unsigned char)((double)(bestColor.G) * weight), (unsigned char)((double)(bestColor.B) * weight)); - vpDisplay::displayCross(I, s.get_i(), s.get_j(), 3, c, 1); - } - else { - s.display(I); - } - ++idx; - } + for (const vpRBSilhouetteControlPoint &p: m_controlPoints) { + const vpMeSite &s = p.getSite(); + s.display(I); } - else if (type == vpRBFeatureDisplayType::ERROR) { - unsigned idx = 0; - const vpColor bestColor = vpColor::green; - double maxError = m_error.getMaxValue(); - - for (const vpRBSilhouetteControlPoint &p: m_controlPoints) { - const vpMeSite &s = p.getSite(); - if (s.getState() == vpMeSite::NO_SUPPRESSION) { - double weight = m_error[idx] / maxError; - vpColor c((unsigned char)((double)(bestColor.R) * weight), (unsigned char)((double)(bestColor.G) * weight), (unsigned char)((double)(bestColor.B) * weight)); - vpDisplay::displayCross(I, s.get_i(), s.get_j(), 3, c, 1); - } - else { - s.display(I); - } - ++idx; - } - } - else { - throw vpException(vpException::notImplementedError, "Display not implemented for unknown type"); - } } END_VISP_NAMESPACE diff --git a/tutorial/tracking/render-based/tutorial-rbt-realsense.cpp b/tutorial/tracking/render-based/tutorial-rbt-realsense.cpp index f0aa875c3f..7b3570286d 100644 --- a/tutorial/tracking/render-based/tutorial-rbt-realsense.cpp +++ b/tutorial/tracking/render-based/tutorial-rbt-realsense.cpp @@ -205,7 +205,7 @@ int main(int argc, const char **argv) vpDisplay::display(IdepthDisplay); vpDisplay::display(Id); // vpDisplay::display(Icol); - tracker.display(Id, Icol, IdepthDisplay, vpRBFeatureDisplayType::SIMPLE); + tracker.display(Id, Icol, IdepthDisplay); vpDisplay::displayFrame(Icol, cMo, cam, 0.05, vpColor::none, 2); vpDisplay::displayText(Id, 20, 5, "Right click to exit", vpColor::red); vpMouseButton::vpMouseButtonType button; diff --git a/tutorial/tracking/render-based/tutorial-rbt-sequence.cpp b/tutorial/tracking/render-based/tutorial-rbt-sequence.cpp index 23f376353f..71619676c0 100644 --- a/tutorial/tracking/render-based/tutorial-rbt-sequence.cpp +++ b/tutorial/tracking/render-based/tutorial-rbt-sequence.cpp @@ -205,7 +205,7 @@ int main(int argc, const char **argv) vpDisplay::flush(IProbaDisplay); vpDisplay::display(Id); // vpDisplay::display(Icol); - tracker.display(Id, Icol, depthDisplay, vpRBFeatureDisplayType::SIMPLE); + tracker.display(Id, Icol, depthDisplay); vpDisplay::displayFrame(Icol, cMo, cam, 0.05, vpColor::none, 2); vpDisplay::flush(Icol);