From 232d361e6f3e066b1dbc04c60eb12cf7b07328ce Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Thu, 13 Oct 2022 14:11:35 +0200
Subject: [PATCH] taking only needed measure and covariance
---
...ssor_tracker_feature_landmark_external.cpp | 24 +++++++++++++++++--
1 file changed, 22 insertions(+), 2 deletions(-)
diff --git a/src/processor/processor_tracker_feature_landmark_external.cpp b/src/processor/processor_tracker_feature_landmark_external.cpp
index 69d09ec77..4f4e90e5c 100644
--- a/src/processor/processor_tracker_feature_landmark_external.cpp
+++ b/src/processor/processor_tracker_feature_landmark_external.cpp
@@ -43,6 +43,7 @@ void ProcessorTrackerFeatureLandmarkExternal::preProcess()
{
assert(unmatched_detections_incoming_.empty());
+ auto dim = getProblem()->getDim();
auto cap_landmarks = std::dynamic_pointer_cast<CaptureLandmarksExternal>(incoming_ptr_);
if (not cap_landmarks)
throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::preProcess incoming_ptr_ should be of type 'CaptureLandmarksExternal'");
@@ -57,10 +58,29 @@ void ProcessorTrackerFeatureLandmarkExternal::preProcess()
or ids.count(detection.id))
continue;
+ Eigen::VectorXd meas;
+ Eigen::MatrixXd cov;
+ if (not params_tfle_->use_orientation)
+ {
+ assert(detection.measure.size() >= dim);
+ assert(detection.covariance.rows() >= dim and detection.covariance.rows() == detection.covariance.cols());
+
+ meas = detection.measure.head(dim);
+ cov = detection.covariance.topLeftCorner(dim,dim);
+ }
+ else
+ {
+ assert(detection.measure.size() == (dim == 2 ? 3 : 7));
+ assert(detection.covariance.rows() == (dim == 2 ? 3 : 6) and detection.covariance.rows() == detection.covariance.cols());
+
+ meas = detection.measure;
+ cov = detection.covariance;
+ }
+
FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(cap_landmarks,
"FeatureLandmarkExternal",
- detection.measure,
- detection.covariance);
+ meas,
+ cov);
ftr->setLandmarkId(detection.id);
if (detection.id != -1 and detection.id != 0)
--
GitLab