From 5af8c39756e527c4a72fd24348b82475c6831196 Mon Sep 17 00:00:00 2001
From: joan vallve <jvallve@iri.upc.edu>
Date: Thu, 1 Feb 2024 11:30:24 +0100
Subject: [PATCH] capture_landmark_external added field type
---
.../core/capture/capture_landmarks_external.h | 45 +++++++++++--------
src/capture/capture_landmarks_external.cpp | 7 +--
2 files changed, 31 insertions(+), 21 deletions(-)
diff --git a/include/core/capture/capture_landmarks_external.h b/include/core/capture/capture_landmarks_external.h
index 58fa079d4..76d8549a9 100644
--- a/include/core/capture/capture_landmarks_external.h
+++ b/include/core/capture/capture_landmarks_external.h
@@ -21,41 +21,50 @@
//--------LICENSE_END--------
#pragma once
-//Wolf includes
+// Wolf includes
#include "core/capture/capture_base.h"
-namespace wolf {
+namespace wolf
+{
struct LandmarkDetection
{
int id; // id of landmark
+ int type; // type of landmark
Eigen::VectorXd measure; // either pose or position
Eigen::MatrixXd covariance; // covariance of the measure
- double quality; // [0, 1] quality of the detection
+ double quality; // [0, 1] quality of the detection
};
WOLF_PTR_TYPEDEFS(CaptureLandmarksExternal);
-//class CaptureLandmarksExternal
+// class CaptureLandmarksExternal
class CaptureLandmarksExternal : public CaptureBase
{
- protected:
- std::vector<LandmarkDetection> detections_;
-
- public:
+ protected:
+ std::vector<LandmarkDetection> detections_;
- CaptureLandmarksExternal(const TimeStamp& _ts,
- SensorBasePtr _sensor_ptr,
- const std::vector<int>& _ids = {},
- const std::vector<Eigen::VectorXd>& _detections = {},
- const std::vector<Eigen::MatrixXd>& _covs = {},
- const std::vector<double>& _qualities = {});
+ public:
+ CaptureLandmarksExternal(const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
+ const std::vector<int>& _ids = {},
+ const std::vector<int>& _types = {},
+ const std::vector<Eigen::VectorXd>& _detections = {},
+ const std::vector<Eigen::MatrixXd>& _covs = {},
+ const std::vector<double>& _qualities = {});
- ~CaptureLandmarksExternal() override;
+ ~CaptureLandmarksExternal() override;
- std::vector<LandmarkDetection> getDetections() const {return detections_;};
+ std::vector<LandmarkDetection> getDetections() const
+ {
+ return detections_;
+ };
- void addDetection(const int& _id, const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const double& quality);
+ void addDetection(const int& _id,
+ const int& _type,
+ const Eigen::VectorXd& _detection,
+ const Eigen::MatrixXd& _cov,
+ const double& quality);
};
-} //namespace wolf
+} // namespace wolf
diff --git a/src/capture/capture_landmarks_external.cpp b/src/capture/capture_landmarks_external.cpp
index ce850621a..2f2e5a3f9 100644
--- a/src/capture/capture_landmarks_external.cpp
+++ b/src/capture/capture_landmarks_external.cpp
@@ -26,6 +26,7 @@ namespace wolf{
CaptureLandmarksExternal::CaptureLandmarksExternal(const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
const std::vector<int>& _ids,
+ const std::vector<int>& _types,
const std::vector<Eigen::VectorXd>& _detections,
const std::vector<Eigen::MatrixXd>& _covs,
const std::vector<double>& _qualities) :
@@ -37,7 +38,7 @@ CaptureLandmarksExternal::CaptureLandmarksExternal(const TimeStamp& _ts,
throw std::runtime_error("CaptureLandmarksExternal constructor: '_ids', '_detections', '_covs', '_qualities' should have the same size.");
for (auto i = 0; i < _detections.size(); i++)
- addDetection(_ids.at(i), _detections.at(i), _covs.at(i), _qualities.at(i));
+ addDetection(_ids.at(i), _types.at(i), _detections.at(i), _covs.at(i), _qualities.at(i));
}
CaptureLandmarksExternal::~CaptureLandmarksExternal()
@@ -45,9 +46,9 @@ CaptureLandmarksExternal::~CaptureLandmarksExternal()
//
}
-void CaptureLandmarksExternal::addDetection(const int& _id, const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const double& _quality)
+void CaptureLandmarksExternal::addDetection(const int& _id, const int& _type, const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const double& _quality)
{
- detections_.push_back(LandmarkDetection{_id, _detection, _cov, _quality});
+ detections_.push_back(LandmarkDetection{_id, _type, _detection, _cov, _quality});
}
} // namespace wolf
--
GitLab