From 01f84aaf12f2dd4cd1f532d7c6b6666909eca649 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Mon, 4 May 2020 20:11:02 +0200
Subject: [PATCH] Add composite API to FeatureBase and FeatureMotion
---
include/core/feature/feature_base.h | 16 ++--------------
include/core/feature/feature_motion.h | 17 ++++++++++++-----
src/feature/feature_base.cpp | 13 +++++++++++++
src/feature/feature_motion.cpp | 17 +++++++++++++++++
src/processor/processor_diff_drive.cpp | 10 ++++++----
5 files changed, 50 insertions(+), 23 deletions(-)
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index a49d4147c..ab5d5f5a2 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -10,6 +10,7 @@ class FactorBase;
//Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
+#include "core/state_block/state_composite.h"
//std includes
@@ -59,9 +60,9 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
FeatureBase(const std::string& _type,
+ const StateStructure& _structure,
const VectorComposite& _measurement,
const MatrixComposite& _meas_uncertainty,
- const StateStructure& _structure,
UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
virtual ~FeatureBase();
@@ -154,19 +155,6 @@ inline unsigned int FeatureBase::getHits() const
return constrained_by_list_.size();
}
-inline FeatureBase::FeatureBase (const std::string& _type,
- const VectorComposite& _measurement,
- const MatrixComposite& _meas_uncertainty,
- const StateStructure& _structure,
- UncertaintyType _uncertainty_type) :
- FeatureBase(_type,
- _measurement.vector(_structure),
- _meas_uncertainty.matrix(_structure,_structure),
- _uncertainty_type)
-{
- //
-}
-
inline const FactorBasePtrList& FeatureBase::getConstrainedByList() const
{
return constrained_by_list_;
diff --git a/include/core/feature/feature_motion.h b/include/core/feature/feature_motion.h
index ae588e29a..85aa2f86e 100644
--- a/include/core/feature/feature_motion.h
+++ b/include/core/feature/feature_motion.h
@@ -19,11 +19,18 @@ WOLF_PTR_TYPEDEFS(FeatureMotion);
class FeatureMotion : public FeatureBase
{
public:
- FeatureMotion(const std::string& _type,
- const VectorXd& _delta_preint,
- const MatrixXd _delta_preint_cov,
- const VectorXd& _calib_preint,
- const MatrixXd& _jacobian_calib);
+ FeatureMotion(const std::string& _type,
+ const VectorXd& _delta_preint,
+ const MatrixXd _delta_preint_cov,
+ const VectorXd& _calib_preint,
+ const MatrixXd& _jacobian_calib);
+ FeatureMotion(const std::string& _type,
+ const StateStructure& _structure,
+ const StateStructure& _structure_calib,
+ const VectorComposite& _delta_preint,
+ const MatrixComposite _delta_preint_cov,
+ const VectorComposite& _calib_preint,
+ const MatrixComposite& _jacobian_calib);
virtual ~FeatureMotion();
const Eigen::VectorXd& getDeltaPreint() const; ///< A new name for getMeasurement()
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index d3aa372b7..a6180bfb5 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -32,6 +32,19 @@ FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXd& _measu
// std::cout << "constructed +f" << id() << std::endl;
}
+FeatureBase::FeatureBase (const std::string& _type,
+ const StateStructure& _structure,
+ const VectorComposite& _measurement,
+ const MatrixComposite& _meas_uncertainty,
+ UncertaintyType _uncertainty_type)
+ : FeatureBase(_type,
+ _measurement.vector(_structure),
+ _meas_uncertainty.matrix(_structure, _structure),
+ _uncertainty_type)
+{
+ //
+}
+
FeatureBase::~FeatureBase()
{
// std::cout << "destructed -f" << id() << std::endl;
diff --git a/src/feature/feature_motion.cpp b/src/feature/feature_motion.cpp
index d0b988b79..1de38cde0 100644
--- a/src/feature/feature_motion.cpp
+++ b/src/feature/feature_motion.cpp
@@ -22,6 +22,23 @@ FeatureMotion::FeatureMotion(const std::string& _type,
//
}
+FeatureMotion::FeatureMotion(const std::string& _type,
+ const StateStructure& _structure,
+ const StateStructure& _structure_calib,
+ const VectorComposite& _delta_preint,
+ const MatrixComposite _delta_preint_cov,
+ const VectorComposite& _calib_preint,
+ const MatrixComposite& _jacobian_calib) :
+ FeatureMotion(_type,
+ _delta_preint.vector(_structure),
+ _delta_preint_cov.matrix(_structure, _structure),
+ _calib_preint.vector(_structure_calib),
+ _jacobian_calib.matrix(_structure, _structure_calib))
+{
+ //
+}
+
+
FeatureMotion::~FeatureMotion()
{
//
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 61f756905..dcc3cc6cd 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -159,10 +159,12 @@ FeatureBasePtr ProcessorDiffDrive::emplaceFeature(CaptureMotionPtr _capture_moti
{
auto key_feature_ptr = FeatureBase::emplace<FeatureMotion>(_capture_motion,
"ProcessorDiffDrive",
- _capture_motion->getBuffer().back().delta_integr_.vector("PO"),
- _capture_motion->getBuffer().back().delta_integr_cov_.matrix("PO","PO"),
- _capture_motion->getCalibrationPreint().vector("I"),
- _capture_motion->getBuffer().back().jacobian_calib_.matrix("PO","I"));
+ "PO",
+ "I",
+ _capture_motion->getBuffer().back().delta_integr_,
+ _capture_motion->getBuffer().back().delta_integr_cov_,
+ _capture_motion->getCalibrationPreint(),
+ _capture_motion->getBuffer().back().jacobian_calib_);
return key_feature_ptr;
}
--
GitLab