From 96d630481c66cb5a1f95098c316c2fda46e1d17c Mon Sep 17 00:00:00 2001
From: mederic_fourmy <mederic.fourmy@gmail.com>
Date: Fri, 18 Jan 2019 16:39:14 +0100
Subject: [PATCH] Processor apriltag adapted to feature taking information
matrix instead of covariance
---
src/feature_base.cpp | 4 ++--
src/feature_base.h | 2 +-
src/features/feature_apriltag.cpp | 7 ++++---
src/features/feature_apriltag.h | 2 +-
src/processors/processor_tracker_landmark_apriltag.cpp | 2 +-
5 files changed, 9 insertions(+), 8 deletions(-)
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
index ef70986b1..c63fd2a98 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -6,7 +6,7 @@ namespace wolf {
unsigned int FeatureBase::feature_id_count_ = 0;
-FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_uncertainty, UncertaintyType _uncertainty_is_info) :
+FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_uncertainty, UncertaintyType _uncertainty_type) :
NodeBase("FEATURE", _type),
capture_ptr_(),
is_removing_(false),
@@ -15,7 +15,7 @@ FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXs& _measu
landmark_id_(0),
measurement_(_measurement)
{
- switch (_uncertainty_is_info )
+ switch (_uncertainty_type)
{
case UNCERTAINTY_IS_INFO :
setMeasurementInformation(_meas_uncertainty);
diff --git a/src/feature_base.h b/src/feature_base.h
index 1ea3f475d..1f3a5c063 100644
--- a/src/feature_base.h
+++ b/src/feature_base.h
@@ -50,7 +50,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
* \param _measurement the measurement
* \param _meas_covariance the noise of the measurement
*/
- FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_uncertainty, UncertaintyType _uncertainty_is_info = UNCERTAINTY_IS_COVARIANCE);
+ FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_uncertainty, UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
virtual ~FeatureBase();
void remove();
diff --git a/src/features/feature_apriltag.cpp b/src/features/feature_apriltag.cpp
index d1e05118c..17eb4146c 100644
--- a/src/features/feature_apriltag.cpp
+++ b/src/features/feature_apriltag.cpp
@@ -4,10 +4,11 @@
namespace wolf {
FeatureApriltag::FeatureApriltag(const Eigen::Vector7s & _measurement,
- const Eigen::Matrix6s & _meas_covariance,
+ const Eigen::Matrix6s & _meas_uncertainty,
const int _tag_id,
- const apriltag_detection_t & _det) :
- FeatureBase("APRILTAG", _measurement, _meas_covariance),
+ const apriltag_detection_t & _det,
+ UncertaintyType _uncertainty_type) :
+ FeatureBase("APRILTAG", _measurement, _meas_uncertainty, _uncertainty_type),
tag_id_ (_tag_id),
tag_corners_(4),
detection_ (_det)
diff --git a/src/features/feature_apriltag.h b/src/features/feature_apriltag.h
index 2a181bd6a..24225ae81 100644
--- a/src/features/feature_apriltag.h
+++ b/src/features/feature_apriltag.h
@@ -22,7 +22,7 @@ class FeatureApriltag : public FeatureBase
{
public:
- FeatureApriltag(const Eigen::Vector7s & _measurement, const Eigen::Matrix6s & _meas_covariance, const int _tag_id, const apriltag_detection_t & _det);
+ FeatureApriltag(const Eigen::Vector7s & _measurement, const Eigen::Matrix6s & _meas_covariance, const int _tag_id, const apriltag_detection_t & _det, UncertaintyType _uncertainty_type = UNCERTAINTY_IS_INFO);
virtual ~FeatureApriltag();
/** \brief Returns tag id
diff --git a/src/processors/processor_tracker_landmark_apriltag.cpp b/src/processors/processor_tracker_landmark_apriltag.cpp
index 52c260c84..83289659a 100644
--- a/src/processors/processor_tracker_landmark_apriltag.cpp
+++ b/src/processors/processor_tracker_landmark_apriltag.cpp
@@ -184,7 +184,7 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
}
WOLF_TRACE("cov \n", cov);
// add to detected features list
- detections_incoming_.push_back(std::make_shared<FeatureApriltag>(pose, cov, tag_id, *det));
+ detections_incoming_.push_back(std::make_shared<FeatureApriltag>(pose, cov.inverse(), tag_id, *det, FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO));
}
apriltag_detections_destroy(detections);
--
GitLab