diff --git a/src/feature_base.cpp b/src/feature_base.cpp index 3cc0b6ca3d64b7299641de401fb67068c0af1b08..ef70986b1525e3e0d3877da8522cc2f420937d4c 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_covariance) : +FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_uncertainty, UncertaintyType _uncertainty_is_info) : NodeBase("FEATURE", _type), capture_ptr_(), is_removing_(false), @@ -15,8 +15,21 @@ FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXs& _measu landmark_id_(0), measurement_(_measurement) { - setMeasurementCovariance(_meas_covariance); -// std::cout << "constructed +f" << id() << std::endl; + switch (_uncertainty_is_info ) + { + case UNCERTAINTY_IS_INFO : + setMeasurementInformation(_meas_uncertainty); + break; + case UNCERTAINTY_IS_COVARIANCE : + setMeasurementCovariance(_meas_uncertainty); + break; + case UNCERTAINTY_IS_STDDEV : + WOLF_ERROR("STDEV case Not implemented yet"); + break; + default : + break; + } + // std::cout << "constructed +f" << id() << std::endl; } FeatureBase::~FeatureBase() diff --git a/src/feature_base.h b/src/feature_base.h index 3bd60033a40929c7e6a18705acbd46a2a56a94e3..1ea3f475dd14935d39ace39b42e6f69473dd9c25 100644 --- a/src/feature_base.h +++ b/src/feature_base.h @@ -38,12 +38,19 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature public: + typedef enum + { + UNCERTAINTY_IS_COVARIANCE, + UNCERTAINTY_IS_INFO, + UNCERTAINTY_IS_STDDEV + } UncertaintyType; + /** \brief Constructor from capture pointer and measure * \param _tp type of feature -- see wolf.h * \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_covariance); + FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_uncertainty, UncertaintyType _uncertainty_is_info = UNCERTAINTY_IS_COVARIANCE); virtual ~FeatureBase(); void remove();