diff --git a/include/bodydynamics/factor/factor_force_torque_preint.h b/include/bodydynamics/factor/factor_force_torque.h similarity index 94% rename from include/bodydynamics/factor/factor_force_torque_preint.h rename to include/bodydynamics/factor/factor_force_torque.h index 5e2397e9b2c58837e225bd74a2a18fcfc625fca0..c9917622723d929d7d4c5a1dd246a400b880fa6d 100644 --- a/include/bodydynamics/factor/factor_force_torque_preint.h +++ b/include/bodydynamics/factor/factor_force_torque.h @@ -19,11 +19,11 @@ // along with this program. If not, see <http://www.gnu.org/licenses/>. // //--------LICENSE_END-------- -#ifndef FACTOR_FORCE_TORQUE_PREINT_THETA_H_ -#define FACTOR_FORCE_TORQUE_PREINT_THETA_H_ +#ifndef FACTOR_FORCE_TORQUE_H_ +#define FACTOR_FORCE_TORQUE_H_ //Wolf includes -#include "bodydynamics/capture/capture_force_torque_preint.h" +#include "bodydynamics/capture/capture_force_torque.h" #include "bodydynamics/feature/feature_force_torque_preint.h" #include "bodydynamics/sensor/sensor_force_torque.h" #include "core/factor/factor_autodiff.h" @@ -33,13 +33,13 @@ namespace wolf { -WOLF_PTR_TYPEDEFS(FactorForceTorquePreint); +WOLF_PTR_TYPEDEFS(FactorForceTorque); //class -class FactorForceTorquePreint : public FactorAutodiff<FactorForceTorquePreint, 12, 3,3,3,4,3,6, 3,3,3,4> +class FactorForceTorque : public FactorAutodiff<FactorForceTorque, 12, 3,3,3,4,3,6, 3,3,3,4> { public: - FactorForceTorquePreint(const FeatureForceTorquePreintPtr& _ftr_ptr, + FactorForceTorque(const FeatureForceTorquePreintPtr& _ftr_ptr, const CaptureForceTorquePtr& _cap_origin_ptr, // gets to bp1, bw1 const ProcessorBasePtr& _processor_ptr, const CaptureBasePtr& _cap_ikin_other, @@ -47,7 +47,7 @@ class FactorForceTorquePreint : public FactorAutodiff<FactorForceTorquePreint, 1 bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE); - ~FactorForceTorquePreint() override = default; + ~FactorForceTorque() override = default; /** \brief : compute the residual from the state blocks being iterated by the solver. -> computes the expected measurement @@ -128,7 +128,7 @@ class FactorForceTorquePreint : public FactorAutodiff<FactorForceTorquePreint, 1 ///////////////////// IMPLEMENTATION //////////////////////////// -inline FactorForceTorquePreint::FactorForceTorquePreint( +inline FactorForceTorque::FactorForceTorque( const FeatureForceTorquePreintPtr& _ftr_ptr, const CaptureForceTorquePtr& _cap_origin_ptr, const ProcessorBasePtr& _processor_ptr, @@ -136,8 +136,8 @@ inline FactorForceTorquePreint::FactorForceTorquePreint( const CaptureBasePtr& _cap_gyro_other, bool _apply_loss_function, FactorStatus _status) : - FactorAutodiff<FactorForceTorquePreint, 12, 3,3,3,4,3,6, 3,3,3,4>( - "FactorForceTorquePreint", + FactorAutodiff<FactorForceTorque, 12, 3,3,3,4,3,6, 3,3,3,4>( + "FactorForceTorque", TOP_MOTION, _ftr_ptr, _cap_origin_ptr->getFrame(), @@ -176,7 +176,7 @@ inline FactorForceTorquePreint::FactorForceTorquePreint( } template<typename T> -inline bool FactorForceTorquePreint::operator ()(const T* const _c1, +inline bool FactorForceTorque::operator ()(const T* const _c1, const T* const _cd1, const T* const _Lc1, const T* const _q1, @@ -206,7 +206,7 @@ inline bool FactorForceTorquePreint::operator ()(const T* const _c1, } template<typename D1, typename D2, typename D3, typename D4> -inline bool FactorForceTorquePreint::residual(const MatrixBase<D1> & _c1, +inline bool FactorForceTorque::residual(const MatrixBase<D1> & _c1, const MatrixBase<D1> & _cd1, const MatrixBase<D1> & _Lc1, const QuaternionBase<D2> & _q1, @@ -305,7 +305,7 @@ inline bool FactorForceTorquePreint::residual(const MatrixBase<D1> & _c1, return true; } -// Matrix<double,12,1> FactorForceTorquePreint::residual() const +// Matrix<double,12,1> FactorForceTorque::residual() const // { // Matrix<double,12,1> res; @@ -333,7 +333,7 @@ inline bool FactorForceTorquePreint::residual(const MatrixBase<D1> & _c1, // return res; // } -// double FactorForceTorquePreint::cost() const +// double FactorForceTorque::cost() const // { // Matrix<double,12,1> toto = residual(); // } diff --git a/src/processor/processor_force_torque_preint.cpp b/src/processor/processor_force_torque_preint.cpp index 09348f2c3ce419d367f4877381f13cdc91215f18..57f2e7fd3376b98069e65e8f3c641cb67414b57b 100644 --- a/src/processor/processor_force_torque_preint.cpp +++ b/src/processor/processor_force_torque_preint.cpp @@ -21,10 +21,10 @@ //--------LICENSE_END-------- // wolf #include "bodydynamics/math/force_torque_delta_tools.h" -#include "bodydynamics/capture/capture_force_torque_preint.h" +#include "bodydynamics/capture/capture_force_torque.h" #include "bodydynamics/feature/feature_force_torque_preint.h" #include "bodydynamics/processor/processor_force_torque_preint.h" -#include "bodydynamics/factor/factor_force_torque_preint.h" +#include "bodydynamics/factor/factor_force_torque.h" namespace wolf { @@ -182,7 +182,7 @@ FactorBasePtr ProcessorForceTorquePreint::emplaceFactor(FeatureBasePtr _feature_ FeatureForceTorquePreintPtr ftr_ftpreint = std::static_pointer_cast<FeatureForceTorquePreint>(_feature_motion); CaptureForceTorquePtr cap_ftpreint_new = std::static_pointer_cast<CaptureForceTorque>(ftr_ftpreint->getCapture()); - auto fac_ftpreint = FactorBase::emplace<FactorForceTorquePreint>( + auto fac_ftpreint = FactorBase::emplace<FactorForceTorque>( ftr_ftpreint, ftr_ftpreint, cap_ftpreint_origin,