diff --git a/include/bodydynamics/factor/factor_point_feet_altitude.h b/include/bodydynamics/factor/factor_point_feet_altitude.h new file mode 100644 index 0000000000000000000000000000000000000000..e74313b9747dad237d63d78659d1149d5622420e --- /dev/null +++ b/include/bodydynamics/factor/factor_point_feet_altitude.h @@ -0,0 +1,124 @@ +/** + * \file factor_point_feet_altitude.h + * + * Created on: Nov 5, 2020 + * \author: mfourmy + */ + +#ifndef FACTOR_POINT_FEET_ALTITUDE_H_ +#define FACTOR_POINT_FEET_ALTITUDE_H_ + +#include "core/factor/factor_autodiff.h" + +namespace wolf +{ + +WOLF_PTR_TYPEDEFS(FactorPointFeetAltitude); + +class FactorPointFeetAltitude : public FactorAutodiff<FactorPointFeetAltitude, + 1, + 3, + 4 + > +{ + public: + FactorPointFeetAltitude(const FeatureBasePtr& _ftr_current_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + FactorStatus _status = FAC_ACTIVE); + + ~FactorPointFeetAltitude() override { /* nothing */ } + + /* + Factor state blocks: + _pb: W_p_WB -> base position in world frame + _qb: W_q_B -> base orientation in world frame + */ + template<typename T> + bool operator () ( + const T* const _pb, + const T* const _qb, + T* _res) const; + + Vector1d error() const; + Vector1d res() const; + double cost() const; + +}; + +} /* namespace wolf */ + + +namespace wolf { + +FactorPointFeetAltitude::FactorPointFeetAltitude( + const FeatureBasePtr& _ftr_current_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + FactorStatus _status) : + FactorAutodiff("FactorPointFeetAltitude", + 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()->getP(), + _ftr_current_ptr->getFrame()->getO() + ) +{ +} + +Vector1d FactorPointFeetAltitude::error() const{ + + Vector3d pb = getFrame()->getP()->getState(); + Vector4d qb_vec = getFrame()->getO()->getState(); + Quaterniond qb(qb_vec); + + // Measurements retrieval + Eigen::Matrix<double,4,1> meas = getMeasurement(); + Vector3d b_p_bl = meas.topRows(3); + Vector1d z = meas.bottomRows(1); + + return (pb + qb*b_p_bl).block(2,0,1,1) - z; +} + +Vector1d FactorPointFeetAltitude::res() const{ + return getMeasurementSquareRootInformationUpper() * error(); +} + + +double FactorPointFeetAltitude::cost() const{ + return res().squaredNorm(); +} + +template<typename T> +bool FactorPointFeetAltitude::operator () ( + const T* const _pb, + const T* const _qb, + T* _res) const +{ + Map<Matrix<T,1,1> > res(_res); + + // State variables instanciation + Map<const Matrix<T,3,1> > pb(_pb); + Map<const Quaternion<T> > qb(_qb); + + // Measurements retrieval + Eigen::Matrix<T,4,1> meas = getMeasurement().cast<T>(); + Matrix<T,3,1> b_p_bl = meas.topRows(3); // Bpast_p_BpastL + Matrix<T,1,1> z = meas.bottomRows(1); // altitude + + Matrix<T,1,1> err = (pb + qb*b_p_bl).block(2,0,1,1) - z; + + res = getMeasurementSquareRootInformationUpper() * err; + + return true; +} + +} // namespace wolf + +#endif /* FACTOR__POINT_FEET_ALTITUDE_H_ */