dkf.h

// clang: MatousFormat
#ifndef DKF_H
#define DKF_H
 
#include <mrs_lib/lkf.h>
#include <mrs_lib/geometry/misc.h>
#include <iostream>
 
namespace mrs_lib
{
 
  /* class DKF //{ */
  template <int n_states, int n_inputs>
  class DKF : public LKF<n_states, n_inputs, -1>
  {
  public:
    /* DKF definitions (typedefs, constants etc) //{ */
    using Base_class = LKF<n_states, n_inputs, -1>;            
    static constexpr int n = Base_class::n;              
    static constexpr int m = Base_class::m;              
    static constexpr int p = Base_class::p;              
    using x_t = typename Base_class::x_t;                
    using u_t = typename Base_class::u_t;                
    using z_t = typename Base_class::z_t;                
    using P_t = typename Base_class::P_t;                
    using R_t = typename Base_class::R_t;                
    using Q_t = typename Base_class::Q_t;                
    using statecov_t = typename Base_class::statecov_t;  
    typedef Eigen::Matrix<double, n, n> A_t;  
    typedef Eigen::Matrix<double, n, m> B_t;  
    typedef Eigen::Matrix<double, p, n> H_t;  
    typedef Eigen::Matrix<double, n, p> K_t;  
    using mat2_t = Eigen::Matrix<double, 2, 2>;
    using mat3_t = Eigen::Matrix<double, 3, 3>;
    using pt3_t = mrs_lib::geometry::vec3_t;
    using pt2_t = mrs_lib::geometry::vec2_t;
    using vec3_t = mrs_lib::geometry::vec3_t;
    //}
 
  public:
    DKF(){};
 
    DKF(const A_t& A, const B_t& B) : Base_class(A, B, {}) {};
 
    /* correctLine() method //{ */
    virtual std::enable_if_t<((n > 3) || (n == Eigen::Dynamic)), statecov_t> correctLine(const statecov_t& sc, const pt3_t& line_origin, const vec3_t& line_direction, const double line_variance) const
    {
      assert(line_direction.norm() > 0.0);
 
      using M_t = Eigen::Matrix<double, 3, n>;
      using W_t = Eigen::Matrix<double, 3, 1>;
      using N_t = Eigen::Matrix<double, 3, 2>;
      using o_t = Eigen::Matrix<double, 3, 1>;
      using R_t = Eigen::Matrix<double, 2, 2>;
 
      const M_t M = M_t::Identity(3, sc.P.cols());
      const W_t W = line_direction;
      const o_t o = line_origin;
 
      // doesn't work - the kernel is always zero for some reason
      /* const Eigen::FullPivLU<W_t> lu(W); */
      /* const N_t N = lu.kernel(); */
      // works for a line measurement
      const mat3_t rot = mrs_lib::geometry::rotationBetween(W_t::UnitX(), W);
      // the first column should have the same direction as W - we don't care about it,
      // take the second and third column vectors, those are the null space of W
      const N_t N = rot.block<3, 2>(0, 1);
      const z_t z = N.transpose() * o;
      const H_t H = N.transpose() * M;
      const R_t R = line_variance * N.transpose() * N;
 
      return this->correction_impl(sc, z, R, H);
    };
    //}
    
    /* correctPlane() method //{ */
    virtual std::enable_if_t<((n > 3) || (n == Eigen::Dynamic)), statecov_t> correctPlane(const statecov_t& sc, const pt3_t& plane_origin, const vec3_t& plane_normal, const double plane_variance) const
    {
      assert(plane_normal.norm() > 0.0);
 
      // we don't need W, since the plane is minimally defined by its origin and normal, where the normal is a basis for its null space
      using M_t = Eigen::Matrix<double, 3, n>;
      using N_t = Eigen::Matrix<double, 3, 1>;
      using o_t = Eigen::Matrix<double, 3, 1>;
      using R_t = Eigen::Matrix<double, 1, 1>;
 
      const M_t M = M_t::Identity(3, sc.P.cols());
      const o_t o = plane_origin;
 
      const N_t N = plane_normal.normalized(); //works for plane
      const z_t z = N.transpose() * o;
      const H_t H = N.transpose() * M;
      const R_t R = (R_t() << plane_variance).finished(); //R is a scalar here
 
      return this->correction_impl(sc, z, R, H);
    };
    //}
  };
  //}
 
}  // namespace mrs_lib
 
#endif // DKF_H