From 64943d6f28f02822e380ec13f863fa64efe59bcf Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Thu, 13 Oct 2022 14:10:29 +0200
Subject: [PATCH] makeposdef covariances
---
src/subscriber_landmarks.cpp | 4 +++-
1 file changed, 3 insertions(+), 1 deletion(-)
diff --git a/src/subscriber_landmarks.cpp b/src/subscriber_landmarks.cpp
index 7c2ac35..5acb060 100644
--- a/src/subscriber_landmarks.cpp
+++ b/src/subscriber_landmarks.cpp
@@ -26,6 +26,7 @@
* WOLF includes *
**************************/
#include <core/capture/capture_landmarks_external.h>
+#include <core/math/covariance.h>
/**************************
* ROS includes *
@@ -50,7 +51,7 @@ void SubscriberLandmarks::initialize(ros::NodeHandle& nh, const std::string& top
void SubscriberLandmarks::callback(const wolf_ros_node::LandmarkDetectionArray::ConstPtr& msg)
{
- ROS_DEBUG("SubscriberLandmarks::callback");
+ ROS_INFO("SubscriberLandmarks::callback");
updateLastHeader(msg->header);
@@ -92,6 +93,7 @@ void SubscriberLandmarks::callback(const wolf_ros_node::LandmarkDetectionArray::
cov = Eigen::Map<const Eigen::Matrix6d>(msg->detections.at(i).pose.covariance.data());
// fill capture
+ makePosDef(cov);
cap->addDetection(msg->detections.at(i).id, meas, cov, msg->detections.at(i).quality);
}
--
GitLab