diff --git a/hello_wolf/factor_range_bearing.h b/hello_wolf/factor_range_bearing.h
index ffe896f75932fb269068ad1210ad37350e62ce30..0ca3ec0a4f31a403424f928b38cc98015ea728d7 100644
--- a/hello_wolf/factor_range_bearing.h
+++ b/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
diff --git a/include/core/factor/factor_autodiff_distance_3D.h b/include/core/factor/factor_autodiff_distance_3D.h
index e26c412812fd7c7e869a79c8b95085eac43ed633..7c5c2d3767cef907138b8d60fdbc9090a3a71158 100644
--- a/include/core/factor/factor_autodiff_distance_3D.h
+++ b/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,
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 58ad77ae08d78efe9a0b5454551a3d69dfd21d50..a79e0640337809e2c9a22e3c6525d2b65b6386b9 100644
--- a/include/core/factor/factor_base.h
+++ b/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(double const* const* _parameters, double* _residuals, double** _jacobians) const = 0;
diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h
index 86df0571601490f0845c79aeac7411e2492c45e3..a5cfac6600adffbbb8dcad6fe0fa8c77d1808605 100644
--- a/include/core/factor/factor_block_absolute.h
+++ b/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::VectorXd
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index 7065a709c1570017956b0e818e95e4f5794181a6..a57ca27393cb1a05f87923755d7e6695e5f76fcf 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/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;
diff --git a/include/core/factor/factor_odom_2D.h b/include/core/factor/factor_odom_2D.h
index 4089238d8382223ab0260de7fbbbd23e68e8fcb9..23be00e288af137aafbac87025fc066de6fd51a3 100644
--- a/include/core/factor/factor_odom_2D.h
+++ b/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;
diff --git a/include/core/factor/factor_odom_2D_analytic.h b/include/core/factor/factor_odom_2D_analytic.h
index 793e071e1fc79ca43fa368f50c021bb5cdba9cd6..f705a76edb9dd87ad8aa5c7cb0a253fcb4859754 100644
--- a/include/core/factor/factor_odom_2D_analytic.h
+++ b/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
diff --git a/include/core/factor/factor_odom_3D.h b/include/core/factor/factor_odom_3D.h
index fbae8852ddc80001994d0bf18b4e5aa9b4d8392c..347bf50685cc05d93ccb55f0a80f58417b31ae6b 100644
--- a/include/core/factor/factor_odom_3D.h
+++ b/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,
diff --git a/include/core/factor/factor_pose_2D.h b/include/core/factor/factor_pose_2D.h
index ccbce591c01aa4038fe2bf56b9b0b4618678197d..5c47ee4bb272531fa17fa65caed367e8f3eadf49 100644
--- a/include/core/factor/factor_pose_2D.h
+++ b/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;
diff --git a/include/core/factor/factor_pose_3D.h b/include/core/factor/factor_pose_3D.h
index 50045c32dd1b2bdabce9265172cb3f43360e87a2..f8032f3b3bbfdaa6dcc490385f6880650fa63e12 100644
--- a/include/core/factor/factor_pose_3D.h
+++ b/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;
diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h
index 52d6809a6036e407f130766d3076de0fb6c18d41..1d9793c7253f9ceb700d6320544d85c16d1110fc 100644
--- a/include/core/factor/factor_quaternion_absolute.h
+++ b/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;
diff --git a/include/core/factor/factor_relative_2D_analytic.h b/include/core/factor/factor_relative_2D_analytic.h
index bd0b3ff66706627c58b58f64fcb81a275f3e1163..79da205abd9a3b703c0daba65424da7725d5aefe 100644
--- a/include/core/factor/factor_relative_2D_analytic.h
+++ b/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
diff --git a/test/dummy/factor_feature_dummy.h b/test/dummy/factor_feature_dummy.h
index 96a2dee46c5075e2b249499ab57d8a0bf2459237..6b31ffa63a398dc166a4e51fc552352f05a28390 100644
--- a/test/dummy/factor_feature_dummy.h
+++ b/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(double const* const* parameters, double* residuals, double** jacobians) const override {return true;};
diff --git a/test/dummy/factor_landmark_dummy.h b/test/dummy/factor_landmark_dummy.h
index f23c4f044778d4197a615e272f56ebd77ad70d7d..b4db44b7890f73289d54cb2708c85edb8a1b8b46 100644
--- a/test/dummy/factor_landmark_dummy.h
+++ b/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(double const* const* parameters, double* residuals, double** jacobians) const override {return true;};