Add topology API to factors

hello_wolf/factor_range_bearing.h

@@ -48,6 +48,11 @@ class FactorRangeBearing : public FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2,
             //
         }
 
+        virtual std::string getTopology() const override
+        {
+            return "LMK";
+        }
+
         template<typename T>
         bool operator ()(const T* const _p_w_r, // robot position
                          const T* const _o_w_r, // robot orientation

include/core/factor/factor_autodiff_distance_3D.h

@@ -38,6 +38,11 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D,
 
         virtual ~FactorAutodiffDistance3D() { /* nothing */ }
 
+        virtual std::string getTopology() const override
+        {
+            return std::string("GEOM");
+        }
+
         template<typename T>
         bool operator () (const T* const _pos1,
                           const T* const _pos2,

include/core/factor/factor_base.h

@@ -84,6 +84,21 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
 
         unsigned int id() const;
 
+        /** \brief get the main topological characteristic
+         * Possible outputs:
+         *   - "ABS" absolute factor
+         *   - "MOTION" motion factor, e.g. odometry, IMU
+         *   - "LOOP" loop closure factor
+         *   - "LMK" landmark observation factor
+         *   - "GEOM" some geometric relation, e.g. distance
+         *   - "UNDEFINED" undefined topology
+         *   - "OTHER" other topologies
+         *  You can add you own return strings if you wish. But the strings above may already have a function in WOLF.
+         *
+         *  This function needs to be implemented in all derived classes
+         */
+        virtual std::string getTopology() const = 0;
+
         /** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
         **/
         virtual bool evaluate(Scalar const* const* _parameters, Scalar* _residuals, Scalar** _jacobians) const = 0;

include/core/factor/factor_block_absolute.h

@@ -62,6 +62,11 @@ class FactorBlockAbsolute : public FactorAnalytic
 
         virtual ~FactorBlockAbsolute() = default;
 
+        virtual std::string getTopology() const override
+        {
+            return std::string("ABS");
+        }
+
         /** \brief Returns the residual evaluated in the states provided
          *
          * Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXs

include/core/factor/factor_diff_drive.h

@@ -80,6 +80,12 @@ class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive,
          **/
         virtual ~FactorDiffDrive() = default;
 
+        virtual std::string getTopology() const override
+        {
+            return std::string("MOTION");
+        }
+
+
         template<typename T>
         bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
                          const T* const _o2, const T* const _c, T* _residuals) const;

include/core/factor/factor_odom_2D.h

@@ -33,6 +33,12 @@ class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
 
         virtual ~FactorOdom2D() = default;
 
+        virtual std::string getTopology() const override
+        {
+            return std::string("MOTION");
+        }
+
+
         template<typename T>
         bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2,
                          T* _residuals) const;

include/core/factor/factor_odom_2D_analytic.h

@@ -26,6 +26,12 @@ class FactorOdom2DAnalytic : public FactorRelative2DAnalytic
 
         virtual ~FactorOdom2DAnalytic() = default;
 
+        virtual std::string getTopology() const override
+        {
+            return std::string("MOTION");
+        }
+
+
 //        /** \brief Returns the factor residual size
 //         *
 //         * Returns the factor residual size

include/core/factor/factor_odom_3D.h

@@ -28,6 +28,12 @@ class FactorOdom3D : public FactorAutodiff<FactorOdom3D,6,3,4,3,4>
 
         virtual ~FactorOdom3D() = default;
 
+        virtual std::string getTopology() const override
+        {
+            return std::string("MOTION");
+        }
+
+
         template<typename T>
                 bool operator ()(const T* const _p_current,
                                  const T* const _q_current,

include/core/factor/factor_pose_2D.h

@@ -25,6 +25,12 @@ class FactorPose2D: public FactorAutodiff<FactorPose2D,3,2,1>
 
         virtual ~FactorPose2D() = default;
 
+        virtual std::string getTopology() const override
+        {
+            return std::string("ABS");
+        }
+
+
         template<typename T>
         bool operator ()(const T* const _p, const T* const _o, T* _residuals) const;
 

include/core/factor/factor_pose_3D.h

@@ -24,6 +24,11 @@ class FactorPose3D: public FactorAutodiff<FactorPose3D,6,3,4>
 
         virtual ~FactorPose3D() = default;
 
+        virtual std::string getTopology() const override
+        {
+            return std::string("ABS");
+        }
+
         template<typename T>
         bool operator ()(const T* const _p, const T* const _o, T* _residuals) const;
 

include/core/factor/factor_quaternion_absolute.h

@@ -32,6 +32,11 @@ class FactorQuaternionAbsolute: public FactorAutodiff<FactorQuaternionAbsolute,3
 
         virtual ~FactorQuaternionAbsolute() = default;
 
+        virtual std::string getTopology() const override
+        {
+            return std::string("ABS");
+        }
+
         template<typename T>
         bool operator ()(const T* const _o, T* _residuals) const;
 

include/core/factor/factor_relative_2D_analytic.h

@@ -54,6 +54,11 @@ class FactorRelative2DAnalytic : public FactorAnalytic
             //
         }
 
+        virtual std::string getTopology() const override
+        {
+            return std::string("GEOM");
+        }
+
         virtual ~FactorRelative2DAnalytic() = default;
 
         /** \brief Returns the factor residual size

test/dummy/factor_feature_dummy.h

@@ -18,6 +18,11 @@ class FactorFeatureDummy : public FactorBase
 
         virtual ~FactorFeatureDummy() = default;
 
+        virtual std::string getTopology() const override
+        {
+            return "DUMMY";
+        }
+
         /** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
         **/
         virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const override {return true;};

test/dummy/factor_landmark_dummy.h

@@ -18,6 +18,11 @@ class FactorLandmarkDummy : public FactorBase
 
         virtual ~FactorLandmarkDummy() = default;
 
+        virtual std::string getTopology() const override
+        {
+            return "DUMMY";
+        }
+
         /** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
         **/
         virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const override {return true;};