From 7b591d350cd25da103bc54c1997679493b35da48 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Tue, 1 Aug 2023 17:09:10 +0700
Subject: [PATCH] aruco_map: fix publishing detected markers count

---
 aruco_pose/src/aruco_map.cpp | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/aruco_pose/src/aruco_map.cpp b/aruco_pose/src/aruco_map.cpp
index 4bc216c7..e4292c0e 100644
--- a/aruco_pose/src/aruco_map.cpp
+++ b/aruco_pose/src/aruco_map.cpp
@@ -181,7 +181,16 @@ public:
 
 		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()) {