Skip to content
Snippets Groups Projects
Commit 01f84aaf authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Add composite API to FeatureBase and FeatureMotion

parent b5e6009c
No related branches found
No related tags found
1 merge request!358WIP: Resolve "Complete state vector new data structure?"
Pipeline #5314 failed
......@@ -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_;
......
......@@ -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()
......
......@@ -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;
......
......@@ -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()
{
//
......
......@@ -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;
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment