Processor apriltag adapted to feature taking information matrix instead of covariance

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

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

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)

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

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