Merge branch 'Apriltag' of https://gitlab.iri.upc.edu/mobile_robotics/wolf into Apriltag

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()

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();