aruco_map: fix publishing detected markers count

CopterExpress · Aug 1, 2023 · 7b591d3 · 7b591d3
1 parent 2f8915c
commit 7b591d3
Showing 1 changed file with 10 additions and 1 deletion.
diff --git a/aruco_pose/src/aruco_map.cpp b/aruco_pose/src/aruco_map.cpp
@@ -181,7 +181,16 @@ class ArucoMap : public nodelet::Nodelet {
 
 		if (put_markers_count_to_covariance_) {
 			// 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()) {