Skip to content

Commit

Permalink
Fixed trampoline class to avoid copying frames, fixed rendering extra…
Browse files Browse the repository at this point in the history 
…ction potential segfault
  • Loading branch information
@SamFlt
SamFlt committed Sep 24, 2024
1 parent dff31fd commit 4dfd6f3
Show file tree
Hide file tree
Showing 5 changed files with 133 additions and 60 deletions.
98 changes: 52 additions & 46 deletions modules/python/bindings/include/rbt/feature_tracker.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<unsigned char> &I, const vpImage<vpRGBa> &IRGB, const vpImage<unsigned char> &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
{
Expand All @@ -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) */
);
}

};
Expand Down
2 changes: 1 addition & 1 deletion modules/python/examples/realsense-mbt.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
39 changes: 38 additions & 1 deletion modules/python/examples/realsense-rbt.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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:
Expand Down Expand Up @@ -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)
Expand Down
15 changes: 14 additions & 1 deletion modules/tracker/rbt/include/visp3/rbt/vpRBTrackerLogger.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -121,7 +128,11 @@ class VISP_EXPORT vpRBTrackerLogger
double m_maskTime;
double m_driftTime;
std::map<int, std::vector<double>> m_trackerVVSIterTimes;

std::map<int, double> m_trackerIterStartTime;

std::map<int, double> m_trackerFeatureExtractionTime;

std::map<int, double> m_trackerFeatureTrackingTime;
std::map<int, double> m_trackerInitVVSTime;
std::map<int, int> m_trackerNumFeatures;
Expand All @@ -140,6 +151,7 @@ std::ostream &operator<<(std::ostream &out, const vpRBTrackerLogger &timer)

out << "Trackers: " << std::endl;
for (const std::pair<const int, std::vector<double>> &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;
Expand All @@ -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;
Expand Down
39 changes: 28 additions & 11 deletions modules/tracker/rbt/src/core/vpRBTracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -245,11 +245,14 @@ void vpRBTracker::track(vpRBFeatureTrackerInput &input)
}
m_logger.setSilhouetteTime(m_logger.endTimer());

int id = 0;
for (std::shared_ptr<vpRBFeatureTracker> &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<vpRBFeatureTracker> &tracker : m_trackers) {
m_logger.startTimer();
try {
Expand Down Expand Up @@ -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<vpPanda3DDepthCannyFilter>()->setEdgeThreshold((frame.renders.zFar - frame.renders.zNear) * thresholdValue);
}
else {
m_renderer.getRenderer<vpPanda3DDepthCannyFilter>()->setEdgeThreshold(thresholdValue);
bool shouldRenderSilhouette = m_renderer.getRenderer<vpPanda3DDepthCannyFilter>() != nullptr;
if (shouldRenderSilhouette) {
// For silhouette extraction, update depth difference threshold
double thresholdValue = m_depthSilhouetteSettings.getThreshold();
if (m_depthSilhouetteSettings.thresholdIsRelative()) {
m_renderer.getRenderer<vpPanda3DDepthCannyFilter>()->setEdgeThreshold((frame.renders.zFar - frame.renders.zNear) * thresholdValue);
}
else {
m_renderer.getRenderer<vpPanda3DDepthCannyFilter>()->setEdgeThreshold(thresholdValue);
}

}

// Call Panda renderer
Expand All @@ -418,13 +425,23 @@ void vpRBTracker::updateRender(vpRBFeatureTrackerInput &frame)
#pragma omp section
#endif
{
m_renderer.getRenderer<vpPanda3DGeometryRenderer>()->getRender(frame.renders.normals, frame.renders.depth, frame.renders.boundingBox, m_imageHeight, m_imageWidth);
m_renderer.getRenderer<vpPanda3DGeometryRenderer>()->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<vpPanda3DDepthCannyFilter>()->getRender(frame.renders.silhouetteCanny, frame.renders.isSilhouette, frame.renders.boundingBox, m_imageHeight, m_imageWidth);
if (shouldRenderSilhouette) {
m_renderer.getRenderer<vpPanda3DDepthCannyFilter>()->getRender(
frame.renders.silhouetteCanny,
frame.renders.isSilhouette,
frame.renders.boundingBox,
m_imageHeight, m_imageWidth);
}
}
// #pragma omp section
// {
Expand Down

0 comments on commit 4dfd6f3

Please sign in to comment.