fix appending features behind all the zeros occurring at coordinates … #508
Conversation
| 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); |
There was a problem hiding this comment.
What actually goes wrong here is that msg.features.resize(inFeatures->trackedFeatures.size()) expands the msg.features vector with value-initialized feature elements. That causes all the zero features mentioned in this issue. Then later on msg.features.emplace_back(ft) appends the actual features.
So replacing msg.features.resize with msg.features.reserve might solve the problem too.
There was a problem hiding this comment.
I agree with @wouterio, although vec[i] might be faster than emplace_back in some cases, I would leave it as is and only change the resize part
There was a problem hiding this comment.
He @Serafadam, thanks for addressing this!
There was a problem hiding this comment.
I also expect emplace_back to be the right choice here, because the link you mentioned concludes with this:
In principle, if the type stored in the vector is a simple type (not a class or a struct), the loop is often vectorizable and one could expect to see similar numbers as we have seen here. With larger classes, it is the opposite: vectorization doesn’t pay off because of the low memory efficiency, so the operator[] version will in principle slower.
Since the features we're inserting are class/struct types, so we can expect emplace_back to be faster. But of course:
as always with software performance, the runtime numbers are the ones that give the final verdict.
Overview
Author: Owen Chen
Issue
Issue link (if present):
Issue description: There are a lot of zero points (almost 50% of total features) at coordinate(0,0) sending out from camera_node (depthai_bridge)
To quick duplicate this issue, run
$ ros2 run depthai_examples feature_tracker
When you run ros2 topic echo /features_right or /featueres_left, you will get
stamp:
sec: 0
nanosec: 0
frame_id: ''
position:
x: 0.0
y: 0.0
z: 0.0
id: 0
age: 0
harris_score: 0.0
tracking_error: 0.0
stamp:
sec: 0
nanosec: 0
frame_id: ''
position:
x: 0.0
y: 0.0
z: 0.0
id: 0
age: 0
harris_score: 0.0
tracking_error: 0.0
I also wrote ros node to subscribe /features_right or /features_left raw data, almost 50% of total features are appending zero
keypoint x: 0, y: 0
keypoint x: 0, y: 0
keypoint x: 0, y: 0
keypoint x: 0, y: 0
keypoint x: 0, y: 0
keypoint x: 0, y: 0
keypoint x: 0, y: 0
keypoint x: 129.689, y: 82.449
keypoint x: 139.115, y: 79.3655
keypoint x: 130.634, y: 36.6569
keypoint x: 118.756, y: 94.4681
keypoint x: 142.136, y: 92.7242
keypoint x: 136.938, y: 71.487
....etc
Related PRs
Changes
ROS distro: humble
List of changes:
changed implementation in TrackedFeaturesConverter.cpp to fix appending features behind all the zeros occurring at coordinates (0,0)
Testing
Hardware used: Luxonis OAK-D Stereo Camera with Lenovo notebook
Depthai library version: 2.23.0.0
Visuals from testing
Add screenshots/gifs/videos from RVIZ or other visualizers demonstrating the effect of the changes when applicable.
