diff --git a/include/bodydynamics/factor/factor_point_feet_zero_velocity.h b/include/bodydynamics/factor/factor_point_feet_zero_velocity.h new file mode 100644 index 0000000000000000000000000000000000000000..525a9350e6073d0574a312c92203229bb9b9e076 --- /dev/null +++ b/include/bodydynamics/factor/factor_point_feet_zero_velocity.h @@ -0,0 +1,113 @@ +/** + * \file factor_point_feet_nomove.h + * + * Created on: Feb 22, 2020 + * \author: mfourmy + */ + +#ifndef FACTOR_POINT_FEET_ZERO_VELOCITY_H_ +#define FACTOR_POINT_FEET_ZERO_VELOCITY_H_ + +#include "core/factor/factor_autodiff.h" + + + +namespace wolf +{ + +WOLF_PTR_TYPEDEFS(FactorPointFeetNomove); + +class FactorPointFeetNomove : public FactorAutodiff<FactorPointFeetNomove, + 3, + 3, + 4, + 3 + > +{ + public: + FactorPointFeetNomove(const FeatureBasePtr& _ftr_current_ptr, + const StateBlockPtr& _bias_imu, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + FactorStatus _status = FAC_ACTIVE); + + ~FactorPointFeetNomove() override { /* nothing */ } + + /* + Factor state blocks: + _pb: W_p_WB -> base position in world frame + _qb: W_q_B -> base orientation in world frame + _pbm: W_p_WB -> base position in world frame, previous KF + _qbm: W_q_B -> base orientation in world frame, previous KF + */ + template<typename T> + bool operator () ( + const T* const _vb, + const T* const _qb, + const T* const _bi, + T* _res) const; + + // Vector9d residual() const; + // double cost() const; + +}; + +} /* namespace wolf */ + + +namespace wolf { + +FactorPointFeetNomove::FactorPointFeetNomove( + const FeatureBasePtr& _ftr_current_ptr, + const StateBlockPtr& _bias_imu, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + FactorStatus _status) : + FactorAutodiff("FactorPointFeetNomove", + TOP_GEOM, + _ftr_current_ptr, + nullptr, // _frame_other_ptr + nullptr, // _capture_other_ptr + nullptr, // _feature_other_ptr + nullptr, // _landmark_other_ptr + _processor_ptr, + _apply_loss_function, + _status, + _ftr_current_ptr->getFrame()->getV(), + _ftr_current_ptr->getFrame()->getO(), + _bias_imu + ) +{ +} + +template<typename T> +bool FactorPointFeetNomove::operator () ( + const T* const _vb, + const T* const _qb, + const T* const _bi, + T* _res) const +{ + Map<Matrix<T,3,1> > res(_res); + + // State variables instanciation + Map<const Matrix<T,3,1> > vb(_vb); + Map<const Quaternion<T> > qb(_qb); + Map<const Matrix<T,3,1> > bw(_pbm+3); // gyro bias + Map<const Quaternion<T> > qbm(_qbm); + + // Measurements retrieval + Eigen::Matrix<T,6,1> meas = getMeasurement().cast<T>(); + Eigen::Matrix<T,3,1> b_v_bl = meas.head<3>(); + Eigen::Matrix<T,3,1> i_omg_i = meas.segment<3>(3); + Eigen::Matrix<T,3,1> b_p_bl = meas.tail<3>(); + + Matrix<T,3,1> err = qb.conjugate()*vb + (i_omg_i - bw).cross(b_p_bl) + b_v_bl; + + res = getMeasurementSquareRootInformationUpper() * err; + + return true; +} + +} // namespace wolf + +#endif /* FACTOR__POINT_FEET_ZERO_VELOCITY_H_ */