From 6de9a760259097ec40b238c8e7d3ccbf5c75fc02 Mon Sep 17 00:00:00 2001 From: Owen Chen Date: Mon, 4 Mar 2024 06:56:24 +0100 Subject: [PATCH] fix appending features behind all the zeros occurring at coordinates (0,0) --- .../src/TrackedFeaturesConverter.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/depthai_bridge/src/TrackedFeaturesConverter.cpp b/depthai_bridge/src/TrackedFeaturesConverter.cpp index cb9e40c17..8e9efba0e 100644 --- a/depthai_bridge/src/TrackedFeaturesConverter.cpp +++ b/depthai_bridge/src/TrackedFeaturesConverter.cpp @@ -33,16 +33,14 @@ void TrackedFeaturesConverter::toRosMsg(std::shared_ptr in msg.header.frame_id = _frameName; msg.features.resize(inFeatures->trackedFeatures.size()); - for(const auto& feature : inFeatures->trackedFeatures) { - depthai_ros_msgs::msg::TrackedFeature ft; - ft.header = msg.header; - ft.position.x = feature.position.x; - ft.position.y = feature.position.y; - ft.age = feature.age; - ft.id = feature.id; - ft.harris_score = feature.harrisScore; - ft.tracking_error = feature.trackingError; - msg.features.emplace_back(ft); + for(int i = 0; i < inFeatures->trackedFeatures.size(); ++i) { + msg.features[i].header = msg.header; + msg.features[i].position.x = inFeatures->trackedFeatures[i].position.x; + msg.features[i].position.y = inFeatures->trackedFeatures[i].position.y; + msg.features[i].age = inFeatures->trackedFeatures[i].age; + msg.features[i].id = inFeatures->trackedFeatures[i].id; + msg.features[i].harris_score = inFeatures->trackedFeatures[i].harrisScore; + msg.features[i].tracking_error = inFeatures->trackedFeatures[i].trackingError; } featureMsgs.push_back(msg); }