Refactored feature classes with an intermediate FeatureApriltag class

CMakeLists.txt

@@ -104,6 +104,7 @@ SET(HDRS_FACTOR
 include/${PROJECT_NAME}/factor/factor_apriltag_proj.h
   )
 SET(HDRS_FEATURE
+include/${PROJECT_NAME}/feature/feature_apriltag.h
 include/${PROJECT_NAME}/feature/feature_apriltag_pose.h
 include/${PROJECT_NAME}/feature/feature_apriltag_proj.h
   )
@@ -117,8 +118,6 @@ include/${PROJECT_NAME}/processor/ippe.h
 
 # ============ SOURCES ============ 
 SET(SRCS_FEATURE
-src/feature/feature_apriltag_pose.cpp
-src/feature/feature_apriltag_proj.cpp
   )
 SET(SRCS_LANDMARK
 src/landmark/landmark_apriltag.cpp

src/feature/feature_apriltag_pose.cpp → include/apriltag/feature/feature_apriltag.h

@@ -19,24 +19,62 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-#include "apriltag/feature/feature_apriltag_pose.h"
+#ifndef FEATURE_APRILTAG_H_
+#define FEATURE_APRILTAG_H_
+
+//Wolf includes
+#include "core/feature/feature_base.h"
+
+// opencv
+#include <opencv2/features2d.hpp>
 
 namespace wolf {
+    
+WOLF_PTR_TYPEDEFS(FeatureApriltag);
+
+
+//class FeatureApriltag
+class FeatureApriltag : public FeatureBase
+{
+    public:
+
+        FeatureApriltag(
+            const std::string _type,
+            const Eigen::VectorXd & _measurement,
+            const Eigen::MatrixXd & _meas_covariance,
+            const int _tag_id,
+            const Vector8d & _corners_vec,
+            UncertaintyType _uncertainty_type = UNCERTAINTY_IS_INFO);
+        ~FeatureApriltag() override;
+        
+        /** \brief Return tag id
+         * 
+         **/
+        int getTagId() const; 
+
+        /** \brief Return vector of tag corners
+         * 
+         **/
+        const std::vector<cv::Point2d>& getTagCorners() const;
+
+
+    private:
+        int tag_id_;
+        std::vector<cv::Point2d> tag_corners_;
+        
+};
+
 
-FeatureApriltagPose::FeatureApriltagPose(const Eigen::Vector7d & _measurement,
-                                 const Eigen::Matrix6d & _meas_uncertainty,
-                                 const int _tag_id,
-                                 const Vector8d & _corners_vec,
-                                 double _rep_error1,
-                                 double _rep_error2,
-                                 bool _use_rotation,
-                                 UncertaintyType _uncertainty_type) :
-    FeatureBase("FeatureApriltagPose", _measurement, _meas_uncertainty, _uncertainty_type),
-    tag_id_     (_tag_id),
-    tag_corners_(4),
-    rep_error1_(_rep_error1),
-    rep_error2_(_rep_error2),
-    use_rotation_(_use_rotation)
+inline FeatureApriltag::FeatureApriltag(
+    const std::string _type,
+    const Eigen::VectorXd & _measurement,
+    const Eigen::MatrixXd & _meas_uncertainty,
+    const int _tag_id,
+    const Vector8d & _corners_vec,
+    UncertaintyType _uncertainty_type) :
+        FeatureBase(_type, _measurement, _meas_uncertainty, _uncertainty_type),
+        tag_id_(_tag_id),
+        tag_corners_(4)
 {
     setTrackId(_tag_id);  // assuming there is a single landmark with this id in the scene
 
@@ -47,34 +85,22 @@ FeatureApriltagPose::FeatureApriltagPose(const Eigen::Vector7d & _measurement,
     }
 }
 
-FeatureApriltagPose::~FeatureApriltagPose()
+inline FeatureApriltag::~FeatureApriltag()
 {
     //
 }
 
-double FeatureApriltagPose::getTagId() const
+inline int FeatureApriltag::getTagId() const
 {
     return tag_id_;
 }
 
-const std::vector<cv::Point2d>& FeatureApriltagPose::getTagCorners() const
+inline const std::vector<cv::Point2d>& FeatureApriltag::getTagCorners() const
 {
     return tag_corners_;
 }
 
-double FeatureApriltagPose::getRepError1() const
-{
-    return rep_error1_;
-}
-
-double FeatureApriltagPose::getRepError2() const
-{
-    return rep_error2_;
-}
-
-bool FeatureApriltagPose::getUserotation() const
-{
-    return use_rotation_;
-}
 
 } // namespace wolf
+
+#endif

include/apriltag/feature/feature_apriltag_pose.h

@@ -19,17 +19,18 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-#ifndef FEATURE_APRILTAG_H_
-#define FEATURE_APRILTAG_H_
+#ifndef FEATURE_APRILTAG_POSE_H_
+#define FEATURE_APRILTAG_POSE_H_
 
-//Wolf includes
-#include "core/feature/feature_base.h"
+// Wolf apriltag
+#include "apriltag/feature/feature_apriltag.h"
 
-//std includes
+// Wolf core
+#include <core/feature/feature_base.h>
 
-//external library incudes
-#include "apriltag/apriltag.h"
-#include "apriltag/common/zarray.h"
+// UMich apriltag library
+#include <apriltag/apriltag.h>
+#include <apriltag/common/zarray.h>
 
 // opencv
 #include <opencv2/features2d.hpp>
@@ -40,12 +41,13 @@ WOLF_PTR_TYPEDEFS(FeatureApriltagPose);
 
 
 //class FeatureApriltagPose
-class FeatureApriltagPose : public FeatureBase
+class FeatureApriltagPose : public FeatureApriltag
 {
     public:
 
-        FeatureApriltagPose(const Eigen::Vector7d & _measurement,
-                        const Eigen::Matrix6d & _meas_covariance,
+        FeatureApriltagPose(
+                        const Eigen::Vector7d & _measurement,
+                        const Eigen::Matrix6d & _meas_info,
                         const int _tag_id,
                         const Vector8d & _corners_vec,
                         double _rep_error1,
@@ -54,29 +56,50 @@ class FeatureApriltagPose : public FeatureBase
                         UncertaintyType _uncertainty_type = UNCERTAINTY_IS_INFO);
         ~FeatureApriltagPose() override;
         
-        /** \brief Returns tag id
-         * 
-         * Returns tag id
-         * 
-         **/
-        double getTagId() const; 
-
-        const std::vector<cv::Point2d>& getTagCorners() const;
-
         double getRepError1() const;
         double getRepError2() const;
         bool getUserotation() const;
 
-
     private:
-        int tag_id_;
-        std::vector<cv::Point2d> tag_corners_;
         double rep_error1_;
         double rep_error2_;
         bool use_rotation_;
-        
 };
 
+inline FeatureApriltagPose::FeatureApriltagPose(
+                                 const Eigen::Vector7d & _measurement,
+                                 const Eigen::Matrix6d & _meas_info,
+                                 const int _tag_id,
+                                 const Vector8d & _corners_vec,
+                                 double _rep_error1,
+                                 double _rep_error2,
+                                 bool _use_rotation,
+                                 UncertaintyType _uncertainty_type) :
+    FeatureApriltag("FeatureApriltagPose", _measurement, _meas_info, _tag_id, _corners_vec, _uncertainty_type),
+    rep_error1_(_rep_error1),
+    rep_error2_(_rep_error2),
+    use_rotation_(_use_rotation)
+{
+}
+
+inline FeatureApriltagPose::~FeatureApriltagPose()
+{
+    //
+}
+
+inline double FeatureApriltagPose::getRepError1() const
+{
+    return rep_error1_;
+}
+inline double FeatureApriltagPose::getRepError2() const
+{
+    return rep_error2_;
+}
+inline bool FeatureApriltagPose::getUserotation() const
+{
+    return use_rotation_;
+}
+
 } // namespace wolf
 
 #endif

include/apriltag/feature/feature_apriltag_proj.h

@@ -22,14 +22,15 @@
 #ifndef FEATURE_APRILTAG_PROJ_H_
 #define FEATURE_APRILTAG_PROJ_H_
 
-//Wolf includes
-#include "core/feature/feature_base.h"
+// Wolf apriltag
+#include "apriltag/feature/feature_apriltag.h"
 
-//std includes
+// Wolf core
+#include <core/feature/feature_base.h>
 
-//external library incudes
-#include "apriltag/apriltag.h"
-#include "apriltag/common/zarray.h"
+// UMich apriltag library
+#include <apriltag/apriltag.h>
+#include <apriltag/common/zarray.h>
 
 // opencv
 #include <opencv2/features2d.hpp>
@@ -39,37 +40,58 @@ namespace wolf {
 WOLF_PTR_TYPEDEFS(FeatureApriltagProj);
 
 //class FeatureApriltagProj
-class FeatureApriltagProj : public FeatureBase
+class FeatureApriltagProj : public FeatureApriltag
 {
     public:
-        FeatureApriltagProj(const Eigen::Vector8d & _measurement,
-                            const Eigen::Matrix8d & _meas_covariance,
-                            const int _tag_id,
-                            const double _tag_width,
-                            const Eigen::Vector7d& _pose_pnp,
-                            UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
+        FeatureApriltagProj(
+            const Eigen::Vector8d & _measurement,
+            const Eigen::Matrix8d & _meas_covariance,
+            const int _tag_id,
+            const double _tag_width,
+            const Eigen::Vector7d& _pose_pnp,
+            UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
 
         ~FeatureApriltagProj() override;
         
-        /** \brief Returns tag id
-         * 
-         * Returns tag id
-         * 
-         **/
-        int getTagId() const; 
-        double getTagWidth() const;
         const Eigen::Vector7d& getPosePnp() const;
-
-        const std::vector<cv::Point2d>& getTagCorners() const;
+        double getTagWidth() const;
 
     private:
-        int tag_id_;
-        double tag_width_;
-        std::vector<cv::Point2d> tag_corners_;
         Eigen::Vector7d pose_pnp_;
+        double tag_width_;
+
         
 };
 
+
+inline FeatureApriltagProj::FeatureApriltagProj(
+    const Eigen::Vector8d & _measurement,
+    const Eigen::Matrix8d & _meas_covariance,
+    const int _tag_id,
+    const double _tag_width,
+    const Eigen::Vector7d& _pose_pnp,
+    UncertaintyType _uncertainty_type) :
+        FeatureApriltag("FeatureApriltagProj", _measurement, _meas_covariance, _tag_id, _measurement, _uncertainty_type),
+        pose_pnp_(_pose_pnp),
+        tag_width_(_tag_width)
+{
+    
+}
+
+inline FeatureApriltagProj::~FeatureApriltagProj()
+{
+    //
+}
+
+inline const Eigen::Vector7d& FeatureApriltagProj::getPosePnp() const
+{
+    return pose_pnp_;
+}
+inline double FeatureApriltagProj::getTagWidth() const
+{
+    return tag_width_;
+}
+
 } // namespace wolf
 
 #endif

include/apriltag/processor/processor_tracker_landmark_apriltag.h

@@ -23,6 +23,8 @@
 #define _PROCESSOR_TRACKER_LANDMARK_APRILTAG_H_
 
 // Wolf apriltag includes
+#include "apriltag/feature/feature_apriltag.h"
+#include "apriltag/feature/feature_apriltag_proj.h"
 #include "apriltag/feature/feature_apriltag_pose.h"
 #include "apriltag/landmark/landmark_apriltag.h"
 #include "apriltag/factor/factor_apriltag_proj.h"

src/feature/feature_apriltag_proj.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#include "apriltag/feature/feature_apriltag_proj.h"
-
-namespace wolf {
-
-FeatureApriltagProj::FeatureApriltagProj(const Eigen::Vector8d & _measurement,
-                                         const Eigen::Matrix8d & _meas_uncertainty,
-                                         const int _tag_id,
-                                         const double _tag_width,
-                                         const Eigen::Vector7d& _pose_pnp,
-                                         UncertaintyType _uncertainty_type) :
-    FeatureBase("FeatureApriltagProj", _measurement, _meas_uncertainty, _uncertainty_type),
-    tag_id_     (_tag_id),
-    tag_width_  (_tag_width),
-    tag_corners_(4),
-    pose_pnp_(_pose_pnp)
-{
-    setTrackId(_tag_id);  // assuming there is a single landmark with this id in the scene
-
-    for (int i = 0; i < 4; i++)
-    {
-        tag_corners_[i].x = _measurement[2*i];
-        tag_corners_[i].y = _measurement[2*i+1];
-    }
-}
-
-FeatureApriltagProj::~FeatureApriltagProj()
-{
-    //
-}
-
-int FeatureApriltagProj::getTagId() const
-{
-    return tag_id_;
-}
-
-double FeatureApriltagProj::getTagWidth() const
-{
-    return tag_width_;
-}
-
-const std::vector<cv::Point2d>& FeatureApriltagProj::getTagCorners() const
-{
-    return tag_corners_;
-}
-
-const Eigen::Vector7d& FeatureApriltagProj::getPosePnp() const
-{
-    return pose_pnp_;
-}
-
-} // namespace wolf

src/processor/processor_tracker_landmark_apriltag.cpp

@@ -41,7 +41,6 @@ ProcessorTrackerLandmarkApriltag::ProcessorTrackerLandmarkApriltag( ParamsProces
         min_features_for_keyframe_(_params_tracker_landmark_apriltag->min_features_for_keyframe),
         nb_vote_for_every_first_(_params_tracker_landmark_apriltag->nb_vote_for_every_first_),
         nb_vote_(0)
-
 {
     // configure apriltag detector
     std::string famname(_params_tracker_landmark_apriltag->tag_family_);
@@ -179,20 +178,20 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
                 info.bottomRightCorner(3,3) = M_1_PI*M_1_PI * Eigen::Matrix3d::Identity();  // 180 degrees standar deviation
             }
 
-
-
             // add to detected features list
             detections_incoming_.push_back(std::make_shared<FeatureApriltagPose>(pose,
-                                                                             info,
-                                                                             tag_id,
-                                                                             corners_vec,
-                                                                             rep_error1,
-                                                                             rep_error2,
-                                                                             use_rotation,
-                                                                             FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO));
+                                                                                 info,
+                                                                                 tag_id,
+                                                                                 corners_vec,
+                                                                                 rep_error1,
+                                                                                 rep_error2,
+                                                                                 use_rotation,
+                                                                                 FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO));
         }
     }
 
+    WOLF_INFO("\ndetections_incoming_: ", detections_incoming_.size())
+
     apriltag_detections_destroy(detections);
 }
 
@@ -409,7 +408,7 @@ unsigned int ProcessorTrackerLandmarkApriltag::findLandmarks(const LandmarkBaseP
 {   
     for (auto feature_in_image : detections_incoming_)
     {
-        int tag_id(std::static_pointer_cast<FeatureApriltagPose>(feature_in_image)->getTagId());
+        int tag_id(std::static_pointer_cast<FeatureApriltag>(feature_in_image)->getTagId());
 
         for (auto landmark_in_ptr : _landmarks_in)
         {

test/CMakeLists.txt

@@ -8,9 +8,7 @@ add_subdirectory(gtest)
 #                                                         #
 ###########################################################
 
-wolf_add_gtest(gtest_feature_apriltag gtest_feature_apriltag_pose.cpp)
-
-wolf_add_gtest(gtest_feature_apriltag_proj gtest_feature_apriltag_proj.cpp)
+wolf_add_gtest(gtest_features_apriltag gtest_features_apriltag.cpp)
 
 wolf_add_gtest(gtest_landmark_apriltag gtest_landmark_apriltag.cpp)
 

test/gtest_feature_apriltag_proj.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * \file gtest_feature_apriltag_proj.cpp
- *
- *  Created on: May 30, 2022
- *      \author: mfourmy
- */
-
-
-#include <core/utils/utils_gtest.h>
-
-#include "apriltag/feature/feature_apriltag_proj.h"
-
-using namespace wolf;
-
-class FeatureApriltagProj_test : public testing::Test
-{
-    public:
-        Eigen::Vector8d meas_;
-        Eigen::Matrix8d cov_;
-        int tag_id_;
-        double tag_width_;
-        apriltag_detection_t det_;
-        Eigen::Vector7d pose_pnp_;
-
-        void SetUp() override
-        {
-
-            tag_id_ = 1;
-            tag_width_ = 0.2;
-
-            meas_ << 1.0, -1.0,
-                     1.0,  1.0,
-                    -1.0,  1.0,
-                    -1.0, -1.0;
-            cov_.setIdentity();
-            pose_pnp_ << 0,0,0, 0,0,0,1;
-        }
-};
-
-TEST_F(FeatureApriltagProj_test, type)
-{
-    auto f = std::make_shared<FeatureApriltagProj>(meas_, cov_, tag_id_, tag_width_, pose_pnp_);
-
-    ASSERT_EQ(f->getType(), "FeatureApriltagProj");
-}
-
-TEST_F(FeatureApriltagProj_test, getters)
-{
-    auto f = std::make_shared<FeatureApriltagProj>(meas_, cov_, tag_id_, tag_width_, pose_pnp_);
-
-    ASSERT_EQ(f->getTagId(), tag_id_);
-    ASSERT_EQ(f->getTagWidth(), tag_width_);
-    ASSERT_MATRIX_APPROX(f->getPosePnp(), pose_pnp_, 1e-6);
-}
-
-
-TEST_F(FeatureApriltagProj_test, getCorners)
-{
-    auto f = std::make_shared<FeatureApriltagProj>(meas_, cov_, tag_id_, tag_width_, pose_pnp_);
-
-    ASSERT_EQ(f->getTagCorners().size(), 4);
-
-    ASSERT_EQ(f->getTagCorners()[0].x,  1.0);
-    ASSERT_EQ(f->getTagCorners()[0].y, -1.0);
-    ASSERT_EQ(f->getTagCorners()[1].x,  1.0);
-    ASSERT_EQ(f->getTagCorners()[1].y,  1.0);
-    ASSERT_EQ(f->getTagCorners()[2].x, -1.0);
-    ASSERT_EQ(f->getTagCorners()[2].y,  1.0);
-    ASSERT_EQ(f->getTagCorners()[3].x, -1.0);
-    ASSERT_EQ(f->getTagCorners()[3].y, -1.0);
-}
-
-int main(int argc, char **argv)
-{
-  testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
-}
-

test/gtest_feature_apriltag_pose.cpp → test/gtest_features_apriltag.cpp

@@ -19,27 +19,24 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-/**
- * \file gtest_feature_apriltag_pose.cpp
- *
- *  Created on: Dec 22, 2018
- *      \author: jsola
- */
-
 
 #include <core/utils/utils_gtest.h>
 
+#include "apriltag/feature/feature_apriltag.h"
 #include "apriltag/feature/feature_apriltag_pose.h"
+#include "apriltag/feature/feature_apriltag_proj.h"
 
 using namespace wolf;
 
-class FeatureApriltag_test : public testing::Test
+class FeatureApriltag_fixture : public testing::Test
 {
     public:
         Eigen::Vector7d pose;
-        Eigen::Matrix6d cov;
+        Eigen::Matrix6d info_pose;
         Eigen::Vector8d corners_vec;