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 ee6ecda364be67b6cfbe07589d7df7a24ad8d069..70fc00a3f3831aead1054544f954854e372c58c6 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(Scalar const* const* _parameters, Scalar* _residuals, Scalar** _jacobians) const = 0; diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h index 2eed31556681df9d1c6f07150627c8689870c144..fc22f101da958a59c00ae99ba44a8ad7f805aa9c 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::VectorXs diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h index 687a24728a081025a0f80f9780cb0df925109dd6..92e8b6af30a767e393c67735668f77e5d8a5b63d 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 ff322cf51790789313a289a3e49f5379fac6add0..1b1e75246571f8f7d0cfd7e0f70ac2a31a40bd73 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 38eaa7fae16f4031ca4cab3dc2209d26440cc132..06458ea70e84b80d700c5d8c579dd42383d7db29 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 25d751ca849a6b1ec7650bd01646846570a04680..0a32f8304cb245c44d9b56db1f9ca3e2a1f88977 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 520e771471fcc2c7e8f309e8f539530ad6c30d77..52cae6ff5a63c437af9ca6128038ad22afd84b2c 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 9f28b946136f75875a630388bb4674ebbf485651..f355d89b5fc7f69947d91ee432769f3649af267a 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 477db6368bb4217b41e154095cbca3bd2c00cf6f..ce1e4c5b540a86a6ac69f33f7db1cfcd9fe53f76 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 c5636621c0413e229c4603891ba52ea35cf0d297..f17a6326f4f94953b68382dda4d0d63b27862cb0 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 c95634eb09ffbea46dae309720c02f889458539d..4d0de747a0b50a1d5610ad5fb05640f8d3814df3 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(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const override {return true;}; diff --git a/test/dummy/factor_landmark_dummy.h b/test/dummy/factor_landmark_dummy.h index d46f6c645cd3d3bc626fc648a2107308022da96a..c016d77132801bf59631ed8f96da8488d0540e26 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(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const override {return true;};