diff --git a/include/bodydynamics/factor/factor_force_torque.h b/include/bodydynamics/factor/factor_force_torque.h
index c04ecaa5b08a38b6a2306cc1a15ee159b475002d..39ccacc20bc6086214f4963cb1fa1e4572f2da60 100644
--- a/include/bodydynamics/factor/factor_force_torque.h
+++ b/include/bodydynamics/factor/factor_force_torque.h
@@ -36,11 +36,6 @@ class FactorForceTorque : public FactorAutodiff<FactorForceTorque, 9, 3,3,3, 3,3
~FactorForceTorque() override { /* nothing */ }
- std::string getTopology() const override
- {
- return std::string("GEOM");
- }
-
/*
_ck : COM position in world frame, time k
_cdk : COM velocity in world frame, time k
@@ -97,6 +92,7 @@ FactorForceTorque::FactorForceTorque(
bool _apply_loss_function,
FactorStatus _status) :
FactorAutodiff("FactorForceTorque",
+ TOP_GEOM,
_feat,
_frame_other, // _frame_other_ptr
nullptr, // _capture_other_ptr
diff --git a/include/bodydynamics/factor/factor_force_torque_preint.h b/include/bodydynamics/factor/factor_force_torque_preint.h
index 3e8cb94b105315c86d4d1e9adcc0a9055b7c0f8f..05b2e571fa5f59a9f328adcfd53c82d7501662de 100644
--- a/include/bodydynamics/factor/factor_force_torque_preint.h
+++ b/include/bodydynamics/factor/factor_force_torque_preint.h
@@ -28,11 +28,6 @@ class FactorForceTorquePreint : public FactorAutodiff<FactorForceTorquePreint, 1
~FactorForceTorquePreint() override = default;
- std::string getTopology() const override
- {
- return std::string("MOTION");
- }
-
/** \brief : compute the residual from the state blocks being iterated by the solver.
-> computes the expected measurement
-> corrects actual measurement with new bias
@@ -122,6 +117,7 @@ inline FactorForceTorquePreint::FactorForceTorquePreint(
FactorStatus _status) :
FactorAutodiff<FactorForceTorquePreint, 12, 3,3,3,4,3,6, 3,3,3,4>(
"FactorForceTorquePreint",
+ TOP_MOTION,
_ftr_ptr,
_cap_origin_ptr->getFrame(),
_cap_origin_ptr,
diff --git a/include/bodydynamics/factor/factor_inertial_kinematics.h b/include/bodydynamics/factor/factor_inertial_kinematics.h
index 734309b3034beaf5f76bcb0bf8cf4e742c28c740..0352a13bf2b4e5ae27b1c309d1a7b8fdb560c97e 100644
--- a/include/bodydynamics/factor/factor_inertial_kinematics.h
+++ b/include/bodydynamics/factor/factor_inertial_kinematics.h
@@ -27,11 +27,6 @@ class FactorInertialKinematics : public FactorAutodiff<FactorInertialKinematics,
~FactorInertialKinematics() override { /* nothing */ }
- std::string getTopology() const override
- {
- return std::string("GEOM");
- }
-
// to keep track of the imu bias, not the standard way to proceed
StateBlockPtr sb_bias_imu_;
@@ -75,6 +70,7 @@ FactorInertialKinematics::FactorInertialKinematics(
bool _apply_loss_function,
FactorStatus _status) :
FactorAutodiff("FactorInertialKinematics",
+ TOP_GEOM,
_feat,
nullptr, // _frame_other_ptr
nullptr, // _capture_other_ptr
diff --git a/include/bodydynamics/factor/factor_point_feet_nomove.h b/include/bodydynamics/factor/factor_point_feet_nomove.h
index 7e4d3ff1dd1285c52faf6d62db96daa0274d398c..a9897ec348bedce98ba502895253cc068630e530 100644
--- a/include/bodydynamics/factor/factor_point_feet_nomove.h
+++ b/include/bodydynamics/factor/factor_point_feet_nomove.h
@@ -42,11 +42,6 @@ class FactorPointFeetNomove : public FactorAutodiff<FactorPointFeetNomove,
~FactorPointFeetNomove() override { /* nothing */ }
- std::string getTopology() const override
- {
- return std::string("GEOM");
- }
-
/*
Factor state blocks:
_pb: W_p_WB -> base position in world frame
@@ -79,6 +74,7 @@ FactorPointFeetNomove::FactorPointFeetNomove(
bool _apply_loss_function,
FactorStatus _status) :
FactorAutodiff("FactorPointFeetNomove",
+ TOP_GEOM,
_ftr_current_ptr,
_frame_past_ptr, // _frame_other_ptr
nullptr, // _capture_other_ptr
diff --git a/include/bodydynamics/processor/processor_point_feet_nomove.h b/include/bodydynamics/processor/processor_point_feet_nomove.h
index 06ec45e8b7c0f543edb6bcb8c89b54d0561753b7..b1be5f547e0721c3783a26b765cf8c66a43b44d3 100644
--- a/include/bodydynamics/processor/processor_point_feet_nomove.h
+++ b/include/bodydynamics/processor/processor_point_feet_nomove.h
@@ -20,7 +20,7 @@ struct ParamsProcessorPointFeetNomove : public ParamsProcessorBase
{
}
~ParamsProcessorPointFeetNomove() override = default;
- std::string print() const
+ std::string print() const override
{
return "\n" + ParamsProcessorBase::print() + "\n";
}
diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp
index 2a8ef4179695edba64fe2447f3179330637dc8de..12de186211a52c91ccbb85b087311556b9ebf359 100644
--- a/test/gtest_factor_force_torque.cpp
+++ b/test/gtest_factor_force_torque.cpp
@@ -56,7 +56,7 @@ FactorInertialKinematics_2KF_1v_bfix,ZeroMvt:
// ODOM3d
#include "core/capture/capture_pose.h"
#include "core/feature/feature_pose.h"
-#include "core/factor/factor_odom_3d.h"
+#include "core/factor/factor_relative_pose_3d.h"
#include <Eigen/Eigenvalues>
@@ -516,7 +516,7 @@ class FactorInertialKinematics_2KF_ForceX_Odom3d : public FactorInertialKinemati
CaptureBasePtr cap_pose_base = CaptureBase::emplace<CapturePose>(KFB_, tB_, nullptr, pose_A_B, rel_pose_cov);
FeatureBasePtr ftr_pose_base = FeatureBase::emplace<FeaturePose>(cap_pose_base, pose_A_B, rel_pose_cov);
- FactorBase::emplace<FactorOdom3d>(ftr_pose_base, ftr_pose_base, KFA_, nullptr, false);
+ FactorBase::emplace<FactorRelativePose3d>(ftr_pose_base, ftr_pose_base, KFA_, nullptr, false, TOP_MOTION);
// With Odom3d the bias on relative base COM position is observable, we unfix the KFB state block
CIKB_->getStateBlock('I')->unfix();
diff --git a/test/gtest_processor_force_torque_preint.cpp b/test/gtest_processor_force_torque_preint.cpp
index 287b9525096e5a83f680611acb351b0648874c87..4c8d6ebedc692c824ba17bfa41c6a852e1831415 100644
--- a/test/gtest_processor_force_torque_preint.cpp
+++ b/test/gtest_processor_force_torque_preint.cpp
@@ -24,7 +24,7 @@
#include <core/capture/capture_pose.h>
#include <core/feature/feature_base.h>
#include <core/factor/factor_block_absolute.h>
-#include <core/factor/factor_odom_3d.h>
+#include <core/factor/factor_relative_pose_3d.h>
// IMU
#include "imu/sensor/sensor_imu.h"
@@ -273,7 +273,7 @@ public:
KF2_ = problem_->getTrajectory()->getLastFrame();
CaptureBasePtr cap_pose_base = CaptureBase::emplace<CapturePose>(KF2_, KF2_->getTimeStamp(), nullptr, prev_pose_curr_, rel_pose_cov);
FeatureBasePtr ftr_odom3d_base = FeatureBase::emplace<FeaturePose>(cap_pose_base, prev_pose_curr_, rel_pose_cov);
- FactorBase::emplace<FactorOdom3d>(ftr_odom3d_base, ftr_odom3d_base, KF1_, nullptr, false);
+ FactorBase::emplace<FactorRelativePose3d>(ftr_odom3d_base, ftr_odom3d_base, KF1_, nullptr, false, TOP_MOTION);
////////////////////////////////////////////
// Add absolute factor on Imu biases to simulate a fix()