diff --git a/modules/python/bindings/include/rbt/feature_tracker.hpp b/modules/python/bindings/include/rbt/feature_tracker.hpp index b7880e3947..a4397e6022 100644 --- a/modules/python/bindings/include/rbt/feature_tracker.hpp +++ b/modules/python/bindings/include/rbt/feature_tracker.hpp @@ -27,88 +27,94 @@ class TrampolineRBFeatureTracker : public vpRBFeatureTracker PYBIND11_OVERRIDE_PURE( bool, /* Return type */ vpRBFeatureTracker, /* Parent class */ - requiresDepth /* Name of function in C++ (must match Python name) */ - ); + requiresDepth, /* Name of function in C++ (must match Python name) */ + ); } virtual bool requiresSilhouetteCandidates() const VP_OVERRIDE { PYBIND11_OVERRIDE_PURE( bool, /* Return type */ vpRBFeatureTracker, /* Parent class */ - requiresSilhouetteCandidates /* Name of function in C++ (must match Python name) */ - ); + requiresSilhouetteCandidates, /* Name of function in C++ (must match Python name) */ + ); } virtual void onTrackingIterStart() VP_OVERRIDE { PYBIND11_OVERRIDE_PURE( void, /* Return type */ vpRBFeatureTracker, /* Parent class */ - onTrackingIterStart /* Name of function in C++ (must match Python name) */ - ); + onTrackingIterStart, /* Name of function in C++ (must match Python name) */ + ); } virtual void onTrackingIterEnd() VP_OVERRIDE { PYBIND11_OVERRIDE_PURE( void, /* Return type */ vpRBFeatureTracker, /* Parent class */ - onTrackingIterEnd /* Name of function in C++ (must match Python name) */ - ); + onTrackingIterEnd, /* Name of function in C++ (must match Python name) */ + ); } virtual void extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE { - PYBIND11_OVERRIDE_PURE( - void, /* Return type */ - vpRBFeatureTracker, /* Parent class */ - extractFeatures, /* Name of function in C++ (must match Python name) */ - frame, previousFrame, cMo - ); + 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, "extractFeatures"); + if (override) { // method is found + // Pybind seems to copy the frames, so we pass the pointers + override(&frame, &previousFrame, cMo); + } } virtual void trackFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE { - PYBIND11_OVERRIDE_PURE( - void, /* Return type */ - vpRBFeatureTracker, /* Parent class */ - trackFeatures, /* Name of function in C++ (must match Python name) */ - frame, previousFrame, cMo - ); + 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, "trackFeatures"); + if (override) { // method is found + // Pybind seems to copy the frames, so we pass the pointers + override(&frame, &previousFrame, cMo); + } } + virtual void initVVS(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE { - PYBIND11_OVERRIDE_PURE( - void, /* Return type */ - vpRBFeatureTracker, /* Parent class */ - initVVS, /* Name of function in C++ (must match Python name) */ - frame, previousFrame, cMo - ); + 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, "initVVS"); + if (override) { // method is found + // Pybind seems to copy the frames, so we pass the pointers + override(&frame, &previousFrame, cMo); + } } virtual void computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int iteration) VP_OVERRIDE { - PYBIND11_OVERRIDE_PURE( - void, /* Return type */ - vpRBFeatureTracker, /* Parent class */ - computeVVSIter, /* Name of function in C++ (must match Python name) */ - frame, cMo, iteration - ); + 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, "computeVVSIter"); + if (override) { // method is found + // Pybind seems to copy the frames, so we pass the pointers + 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 { - PYBIND11_OVERRIDE_PURE( - void, /* Return type */ - vpRBFeatureTracker, /* Parent class */ - display, /* Name of function in C++ (must match Python name) */ - cam, I, IRGB, depth, type - ); + 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, "computeVVSIter"); + if (override) { // method is found + // Pybind seems to copy the frames, so we pass the pointers + override(cam, &I, &IRGB, &depth, type); + } } virtual const vpMatrix getCovariance() const VP_OVERRIDE { PYBIND11_OVERRIDE( vpMatrix, /* Return type */ vpRBFeatureTracker, /* Parent class */ - getCovariance /* Name of function in C++ (must match Python name) */ + getCovariance, /* Name of function in C++ (must match Python name) */ - ); + ); } virtual void updateCovariance(const double lambda) VP_OVERRIDE { @@ -124,24 +130,24 @@ class TrampolineRBFeatureTracker : public vpRBFeatureTracker PYBIND11_OVERRIDE( double, /* Return type */ vpRBFeatureTracker, /* Parent class */ - getVVSTrackerWeight /* Name of function in C++ (must match Python name) */ - ); + getVVSTrackerWeight, /* Name of function in C++ (must match Python name) */ + ); } virtual vpMatrix getLTL() const VP_OVERRIDE { PYBIND11_OVERRIDE( vpMatrix, /* Return type */ vpRBFeatureTracker, /* Parent class */ - getLTL - ); + getLTL, + ); } virtual vpColVector getLTR() const VP_OVERRIDE { PYBIND11_OVERRIDE( vpColVector, /* Return type */ vpRBFeatureTracker, /* Parent class */ - getLTR /* Name of function in C++ (must match Python name) */ - ); + getLTR, /* Name of function in C++ (must match Python name) */ + ); } }; diff --git a/modules/python/examples/realsense-mbt.py b/modules/python/examples/realsense-mbt.py index d222f62ff8..9f0a582fb7 100644 --- a/modules/python/examples/realsense-mbt.py +++ b/modules/python/examples/realsense-mbt.py @@ -195,7 +195,7 @@ def cam_from_rs_profile(profile) -> Tuple[CameraParameters, int, int]: tracker.initClick(I, str(mbt_model.init_file)) start_time = time.time() for frame_data in data_generator: - if frame_data.I_depth is not None: + if I_depth is not None: ImageConvert.createDepthHistogram(frame_data.I_depth, I_depth) Display.display(I) diff --git a/modules/python/examples/realsense-rbt.py b/modules/python/examples/realsense-rbt.py index c5355ac34c..ee52055090 100644 --- a/modules/python/examples/realsense-rbt.py +++ b/modules/python/examples/realsense-rbt.py @@ -47,7 +47,7 @@ from visp.core import Color, Display, ImageConvert from visp.core import ImageGray, ImageUInt16, ImageRGBa, ImageFloat from visp.io import ImageIo -from visp.rbt import RBTracker, RBFeatureDisplayType +from visp.rbt import RBTracker, RBFeatureDisplayType, RBFeatureTracker, RBFeatureTrackerInput from visp.display_utils import get_display import pyrealsense2 as rs @@ -61,8 +61,42 @@ import matplotlib.pyplot as plt +class PyBaseFeatureTracker(RBFeatureTracker): + def __init__(self): + RBFeatureTracker.__init__(self) + def requiresRGB(self) -> bool: + return False + def requiresDepth(self) -> bool: + return False + def requiresSilhouetteCandidates(self) -> bool: + return False + def onTrackingIterStart(self): + self.cov.resize(6, 6) + self.LTL.resize(6, 6) + self.LTR.resize(6) + self.numFeatures = 0 + + def extractFeatures(self, frame: RBFeatureTrackerInput, previousFrame: RBFeatureTrackerInput, cMo: HomogeneousMatrix): + + pass + + def trackFeatures(self, frame: RBFeatureTrackerInput, previousFrame: RBFeatureTrackerInput, cMo: HomogeneousMatrix): + pass + + def initVVS(self, frame: RBFeatureTrackerInput, previousFrame: RBFeatureTrackerInput, cMo: HomogeneousMatrix): + print('INITVVS Was called') + pass + + def computeVVSIter(self, frame: RBFeatureTrackerInput, cMo: HomogeneousMatrix, iteration: int): + pass + + def onTrackingIterEnd(self): + pass + + def display(self, cam: CameraParameters, I: ImageGray, IRGB: ImageRGBa, I_depth: ImageGray, type: RBFeatureDisplayType): + pass @dataclass class FrameData: @@ -126,6 +160,9 @@ def cam_from_rs_profile(profile) -> Tuple[CameraParameters, int, int]: if model_path is not None: tracker.setModelPath(model_path) + custom_feature = PyBaseFeatureTracker() + tracker.addTracker(custom_feature) + cam_color, color_height, color_width = cam_from_rs_profile(cfg.get_stream(rs.stream.color)) tracker.setCameraParameters(cam_color, color_height, color_width) diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBTrackerLogger.h b/modules/tracker/rbt/include/visp3/rbt/vpRBTrackerLogger.h index b4ca76cdd7..ee0461c84d 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBTrackerLogger.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBTrackerLogger.h @@ -61,7 +61,9 @@ class VISP_EXPORT vpRBTrackerLogger { m_renderTime = 0.0; m_silhouetteExtractionTime = 0.0; + m_trackerIterStartTime.clear(); m_trackerFeatureExtractionTime.clear(); + m_trackerFeatureTrackingTime.clear(); m_trackerVVSIterTimes.clear(); } @@ -94,6 +96,11 @@ class VISP_EXPORT vpRBTrackerLogger insertTrackerTime(m_trackerVVSIterTimes, id, elapsed); } + void setTrackerIterStartTime(int id, double elapsed) + { + m_trackerIterStartTime[id] = elapsed; + } + void setTrackerFeatureExtractionTime(int id, double elapsed) { m_trackerFeatureExtractionTime[id] = elapsed; @@ -121,7 +128,11 @@ class VISP_EXPORT vpRBTrackerLogger double m_maskTime; double m_driftTime; std::map> m_trackerVVSIterTimes; + + std::map m_trackerIterStartTime; + std::map m_trackerFeatureExtractionTime; + std::map m_trackerFeatureTrackingTime; std::map m_trackerInitVVSTime; std::map m_trackerNumFeatures; @@ -140,6 +151,7 @@ std::ostream &operator<<(std::ostream &out, const vpRBTrackerLogger &timer) out << "Trackers: " << std::endl; for (const std::pair> &vvsIterData : timer.m_trackerVVSIterTimes) { + double trackingStartTime = timer.m_trackerIterStartTime.find(vvsIterData.first)->second; double featTrackTime = timer.m_trackerFeatureTrackingTime.find(vvsIterData.first)->second; double featExtractionTime = timer.m_trackerFeatureExtractionTime.find(vvsIterData.first)->second; double initVVSTime = timer.m_trackerInitVVSTime.find(vvsIterData.first)->second; @@ -149,8 +161,9 @@ std::ostream &operator<<(std::ostream &out, const vpRBTrackerLogger &timer) ttVVSIter += v; } out << "\t" << vvsIterData.first << std::endl; - out << "\t" << "\t" << "Feature tracking: " << featTrackTime << "ms" << std::endl; + out << "\t" << "\t" << "Tracking initialization: " << trackingStartTime << "ms" << std::endl; out << "\t" << "\t" << "Feature extraction: " << featExtractionTime << "ms" << std::endl; + out << "\t" << "\t" << "Feature tracking: " << featTrackTime << "ms" << std::endl; out << "\t" << "\t" << "VVS init: " << initVVSTime << "ms" << std::endl; out << "\t" << "\t" << "VVS: " << ttVVSIter << "ms (" << vpMath::getMean(vvsIterData.second) << "ms" << "+-" << vpMath::getStdev(vvsIterData.second) << "ms)" << std::endl; diff --git a/modules/tracker/rbt/src/core/vpRBTracker.cpp b/modules/tracker/rbt/src/core/vpRBTracker.cpp index cbe2e7d719..ecb343b905 100644 --- a/modules/tracker/rbt/src/core/vpRBTracker.cpp +++ b/modules/tracker/rbt/src/core/vpRBTracker.cpp @@ -245,11 +245,14 @@ void vpRBTracker::track(vpRBFeatureTrackerInput &input) } m_logger.setSilhouetteTime(m_logger.endTimer()); + int id = 0; for (std::shared_ptr &tracker : m_trackers) { + m_logger.startTimer(); tracker->onTrackingIterStart(); + m_logger.setTrackerIterStartTime(id, m_logger.endTimer()); + id += 1; } - - int id = 0; + id = 0; for (std::shared_ptr &tracker : m_trackers) { m_logger.startTimer(); try { @@ -394,13 +397,17 @@ void vpRBTracker::updateRender(vpRBFeatureTrackerInput &frame) m_rendererSettings.setClippingDistance(frame.renders.zNear, frame.renders.zFar); m_renderer.setRenderParameters(m_rendererSettings); - // For silhouette extraction, update depth difference threshold - double thresholdValue = m_depthSilhouetteSettings.getThreshold(); - if (m_depthSilhouetteSettings.thresholdIsRelative()) { - m_renderer.getRenderer()->setEdgeThreshold((frame.renders.zFar - frame.renders.zNear) * thresholdValue); - } - else { - m_renderer.getRenderer()->setEdgeThreshold(thresholdValue); + bool shouldRenderSilhouette = m_renderer.getRenderer() != nullptr; + if (shouldRenderSilhouette) { + // For silhouette extraction, update depth difference threshold + double thresholdValue = m_depthSilhouetteSettings.getThreshold(); + if (m_depthSilhouetteSettings.thresholdIsRelative()) { + m_renderer.getRenderer()->setEdgeThreshold((frame.renders.zFar - frame.renders.zNear) * thresholdValue); + } + else { + m_renderer.getRenderer()->setEdgeThreshold(thresholdValue); + } + } // Call Panda renderer @@ -418,13 +425,23 @@ void vpRBTracker::updateRender(vpRBFeatureTrackerInput &frame) #pragma omp section #endif { - m_renderer.getRenderer()->getRender(frame.renders.normals, frame.renders.depth, frame.renders.boundingBox, m_imageHeight, m_imageWidth); + m_renderer.getRenderer()->getRender( + frame.renders.normals, + frame.renders.depth, + frame.renders.boundingBox, + m_imageHeight, m_imageWidth); } #ifdef VISP_HAVE_OPENMP #pragma omp section #endif { - m_renderer.getRenderer()->getRender(frame.renders.silhouetteCanny, frame.renders.isSilhouette, frame.renders.boundingBox, m_imageHeight, m_imageWidth); + if (shouldRenderSilhouette) { + m_renderer.getRenderer()->getRender( + frame.renders.silhouetteCanny, + frame.renders.isSilhouette, + frame.renders.boundingBox, + m_imageHeight, m_imageWidth); + } } // #pragma omp section // {