CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 11:43:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

aruco_map: fix publishing detected markers count

Browse Source
This commit is contained in:
Oleg Kalachev
2023-08-01 17:09:10 +07:00
parent 2f8915ce31
commit 7b591d350c
1 changed files with 10 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
aruco_pose/src/aruco_map.cpp
View File

@@ -181,7 +181,16 @@ public:
if (put_markers_count_to_covariance_) { if (put_markers_count_to_covariance_) {
// HACK: pass markers count using covariance field // HACK: pass markers count using covariance field
pose_.pose.covariance[0] = markers->markers.size(); int valid_markers = 0;
for (auto const &marker : markers->markers) {
for (auto const &board_marker : board_->ids) {
if (board_marker == marker.id) {
valid_markers++;
break;
}
}
}
pose_.pose.covariance[0] = valid_markers;
} }
if (known_vertical_.empty()) { if (known_vertical_.empty()) {