diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3d5f3117a78320322fa060da9d1fb63aa0d72219..7c1cf5c8443a510097caf0682105612c6aa11bec 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -118,7 +118,7 @@ include/${PROJECT_NAME}/factor/factor_point_feet_nomove.h
include/${PROJECT_NAME}/factor/factor_point_feet_altitude.h
)
SET(HDRS_FEATURE
-include/${PROJECT_NAME}/feature/feature_force_torque_preint.h
+include/${PROJECT_NAME}/feature/feature_force_torque.h
include/${PROJECT_NAME}/feature/feature_inertial_kinematics.h
)
SET(HDRS_PROCESSOR
@@ -144,7 +144,7 @@ src/capture/capture_leg_odom.cpp
src/capture/capture_point_feet_nomove.cpp
)
SET(SRCS_FEATURE
-src/feature/feature_force_torque_preint.cpp
+src/feature/feature_force_torque.cpp
src/feature/feature_inertial_kinematics.cpp
)
SET(SRCS_PROCESSOR
diff --git a/include/bodydynamics/factor/factor_force_torque.h b/include/bodydynamics/factor/factor_force_torque.h
index c9917622723d929d7d4c5a1dd246a400b880fa6d..9f4db7c21fd32f833b47aa1ca4ebaf32181e454b 100644
--- a/include/bodydynamics/factor/factor_force_torque.h
+++ b/include/bodydynamics/factor/factor_force_torque.h
@@ -39,7 +39,7 @@ WOLF_PTR_TYPEDEFS(FactorForceTorque);
class FactorForceTorque : public FactorAutodiff<FactorForceTorque, 12, 3,3,3,4,3,6, 3,3,3,4>
{
public:
- FactorForceTorque(const FeatureForceTorquePreintPtr& _ftr_ptr,
+ FactorForceTorque(const FeatureForceTorquePtr& _ftr_ptr,
const CaptureForceTorquePtr& _cap_origin_ptr, // gets to bp1, bw1
const ProcessorBasePtr& _processor_ptr,
const CaptureBasePtr& _cap_ikin_other,
@@ -129,7 +129,7 @@ class FactorForceTorque : public FactorAutodiff<FactorForceTorque, 12, 3,3,3,4,3
///////////////////// IMPLEMENTATION ////////////////////////////
inline FactorForceTorque::FactorForceTorque(
- const FeatureForceTorquePreintPtr& _ftr_ptr,
+ const FeatureForceTorquePtr& _ftr_ptr,
const CaptureForceTorquePtr& _cap_origin_ptr,
const ProcessorBasePtr& _processor_ptr,
const CaptureBasePtr& _cap_ikin_other,
@@ -162,8 +162,8 @@ inline FactorForceTorque::FactorForceTorque(
dcd_preint_(_ftr_ptr->getMeasurement().segment<3>(3)),
dLc_preint_(_ftr_ptr->getMeasurement().segment<3>(6)),
dq_preint_(_ftr_ptr->getMeasurement().data()+9),
- pbc_bias_preint_( std::static_pointer_cast<FeatureForceTorquePreint>(_ftr_ptr)->getPbcBiasPreint()),
- gyro_bias_preint_(std::static_pointer_cast<FeatureForceTorquePreint>(_ftr_ptr)->getGyroBiasPreint()),
+ pbc_bias_preint_( std::static_pointer_cast<FeatureForceTorque>(_ftr_ptr)->getPbcBiasPreint()),
+ gyro_bias_preint_(std::static_pointer_cast<FeatureForceTorque>(_ftr_ptr)->getGyroBiasPreint()),
J_dLc_pb_(_ftr_ptr->getJacobianBias().block<3,3>(6,0)), // Jac dLc wrt pbc bias
J_dc_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(0,3)), // Jac dc wrt gyro bias
J_dcd_wb_(_ftr_ptr->getJacobianBias().block<3,3>(3,3)), // Jac dc wrt gyro bias
diff --git a/include/bodydynamics/feature/feature_force_torque_preint.h b/include/bodydynamics/feature/feature_force_torque.h
similarity index 79%
rename from include/bodydynamics/feature/feature_force_torque_preint.h
rename to include/bodydynamics/feature/feature_force_torque.h
index fc04fa4731027bd0a48e5473ca996742d0df91cd..cf2018cbd68e06f8043f9f86fbeb6ac01ad2545a 100644
--- a/include/bodydynamics/feature/feature_force_torque_preint.h
+++ b/include/bodydynamics/feature/feature_force_torque.h
@@ -19,8 +19,8 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#ifndef FEATURE_FORCE_TORQUE_PREINT_H_
-#define FEATURE_FORCE_TORQUE_PREINT_H_
+#ifndef FEATURE_FORCE_TORQUE_H_
+#define FEATURE_FORCE_TORQUE_H_
//Wolf includes
#include <core/feature/feature_base.h>
@@ -31,9 +31,9 @@
namespace wolf {
//WOLF_PTR_TYPEDEFS(CaptureImu);
-WOLF_PTR_TYPEDEFS(FeatureForceTorquePreint);
+WOLF_PTR_TYPEDEFS(FeatureForceTorque);
-class FeatureForceTorquePreint : public FeatureBase
+class FeatureForceTorque : public FeatureBase
{
public:
@@ -46,7 +46,7 @@ class FeatureForceTorquePreint : public FeatureBase
* \param _gyro_bias gyroscope bias of origin frame time
* \param _cap_ftpreint_ptr pointer to parent CaptureMotion (CaptureForceTorque)
*/
- FeatureForceTorquePreint(const Eigen::VectorXd& _delta_preintegrated,
+ FeatureForceTorque(const Eigen::VectorXd& _delta_preintegrated,
const Eigen::MatrixXd& _delta_preintegrated_covariance,
const Eigen::Vector6d& _biases_preint,
const Eigen::Matrix<double,12,6>& _J_delta_biases);
@@ -55,9 +55,9 @@ class FeatureForceTorquePreint : public FeatureBase
// *
// * \param _cap_imu_ptr pointer to parent CaptureMotion
// */
-// FeatureForceTorquePreint(CaptureMotionPtr _cap_imu_ptr);
+// FeatureForceTorque(CaptureMotionPtr _cap_imu_ptr);
- ~FeatureForceTorquePreint() override = default;
+ ~FeatureForceTorque() override = default;
const Eigen::Vector3d& getPbcBiasPreint() const;
const Eigen::Vector3d& getGyroBiasPreint() const;
@@ -75,21 +75,21 @@ class FeatureForceTorquePreint : public FeatureBase
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};
-inline const Eigen::Vector3d& FeatureForceTorquePreint::getPbcBiasPreint() const
+inline const Eigen::Vector3d& FeatureForceTorque::getPbcBiasPreint() const
{
return pbc_bias_preint_;
}
-inline const Eigen::Vector3d& FeatureForceTorquePreint::getGyroBiasPreint() const
+inline const Eigen::Vector3d& FeatureForceTorque::getGyroBiasPreint() const
{
return gyro_bias_preint_;
}
-inline const Eigen::Matrix<double, 12, 6>& FeatureForceTorquePreint::getJacobianBias() const
+inline const Eigen::Matrix<double, 12, 6>& FeatureForceTorque::getJacobianBias() const
{
return J_delta_bias_;
}
} // namespace wolf
-#endif // FEATURE_FORCE_TORQUE_PREINT_H_
+#endif // FEATURE_FORCE_TORQUE_H_
diff --git a/src/feature/feature_force_torque_preint.cpp b/src/feature/feature_force_torque.cpp
similarity index 87%
rename from src/feature/feature_force_torque_preint.cpp
rename to src/feature/feature_force_torque.cpp
index 1a21081d827843645a62c75508b3ceda10794c71..44d1be590b582d7e07fe0fe85ef538d38419c63c 100644
--- a/src/feature/feature_force_torque_preint.cpp
+++ b/src/feature/feature_force_torque.cpp
@@ -22,11 +22,11 @@
#include "bodydynamics/feature/feature_force_torque_preint.h"
namespace wolf {
-FeatureForceTorquePreint::FeatureForceTorquePreint(const Eigen::VectorXd& _delta_preintegrated,
+FeatureForceTorque::FeatureForceTorque(const Eigen::VectorXd& _delta_preintegrated,
const Eigen::MatrixXd& _delta_preintegrated_covariance,
const Eigen::Vector6d& _biases_preint,
const Eigen::Matrix<double,12,6>& _J_delta_biases) :
- FeatureBase("FeatureForceTorquePreint", _delta_preintegrated, _delta_preintegrated_covariance),
+ FeatureBase("FeatureForceTorque", _delta_preintegrated, _delta_preintegrated_covariance),
pbc_bias_preint_(_biases_preint.head<3>()),
gyro_bias_preint_(_biases_preint.tail<3>()),
J_delta_bias_(_J_delta_biases)
diff --git a/src/processor/processor_force_torque_preint.cpp b/src/processor/processor_force_torque_preint.cpp
index 57f2e7fd3376b98069e65e8f3c641cb67414b57b..33bb2ef1df8191b72fb3f04c1574847c3fc7e318 100644
--- a/src/processor/processor_force_torque_preint.cpp
+++ b/src/processor/processor_force_torque_preint.cpp
@@ -122,7 +122,7 @@ CaptureMotionPtr ProcessorForceTorquePreint::emplaceCapture(const FrameBasePtr&
FeatureBasePtr ProcessorForceTorquePreint::emplaceFeature(CaptureMotionPtr _capture_motion)
{
// Retrieving the origin capture and getting the bias from here should be done here?
- auto feature = FeatureBase::emplace<FeatureForceTorquePreint>(_capture_motion,
+ auto feature = FeatureBase::emplace<FeatureForceTorque>(_capture_motion,
_capture_motion->getBuffer().back().delta_integr_,
_capture_motion->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
_capture_motion->getCalibrationPreint(),
@@ -179,7 +179,7 @@ void ProcessorForceTorquePreint::setCalibration (const CaptureBasePtr _capture,
FactorBasePtr ProcessorForceTorquePreint::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
{
CaptureForceTorquePtr cap_ftpreint_origin = std::static_pointer_cast<CaptureForceTorque>(_capture_origin);
- FeatureForceTorquePreintPtr ftr_ftpreint = std::static_pointer_cast<FeatureForceTorquePreint>(_feature_motion);
+ FeatureForceTorquePtr ftr_ftpreint = std::static_pointer_cast<FeatureForceTorque>(_feature_motion);
CaptureForceTorquePtr cap_ftpreint_new = std::static_pointer_cast<CaptureForceTorque>(ftr_ftpreint->getCapture());
auto fac_ftpreint = FactorBase::emplace<FactorForceTorque>(