Factor point feet altitude

include/bodydynamics/factor/factor_point_feet_altitude.h

+/**
+ * \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_ */