#ifndef  feature_INC
#define  feature_INC
#include <Eigen/Dense>
#include "types.h"

using Eigen::Vector3d;
using Eigen::Matrix;
using Eigen::Quaterniond;

/*
 * =====================================================================================
 *        Class:  Feature
 *  Description:  
 * =====================================================================================
 */
class Feature
{
    public:
        /* ====================  LIFECYCLE     ======================================= */
        Feature ();                             /* constructor */

        /* ====================  ACCESSORS     ======================================= */

        /* ====================  MUTATORS      ======================================= */

        /* ====================  OPERATORS     ======================================= */
        Matrix<double,3,9> Fx();
        Matrix<double,3,3> Fy( const Vector3d &vel, const Vector3d &ang);
        Matrix<double,6,9> Hx( const Vector3d &pos, const Quaterniond &q);
        Matrix<double,6,3> Hy( const Vector3d &pos, const Quaterniond &q);
        Matrix<double,6,1> h( const Vector3d &x, const Quaterniond &q);
        /*
        void motionModel ( const Vector3d &acc, const Vector3d &ang, 
                const Quaterniond &q, const double dt);
        Matrix<double,9,9> Q(double dt);
        Matrix<double,1,1> R();
        Matrix<double,1,1> S(const Matrix<double,9,9> &P);
        void skewSymmetric(const Vector3d &x, Matrix<double,3,3> &y);
        void unicsv();
        */

    protected:
        /* ====================  METHODS       ======================================= */

        /* ====================  DATA MEMBERS  ======================================= */

    private:
        /* ====================  METHODS       ======================================= */
        Vector3d p2x(const Vector3d &p);
        Vector3d x2p(const Vector3d &x);

        /* ====================  DATA MEMBERS  ======================================= */
        Vector3d X;
        Quaterniond q0;
        Vector3d xb0w;

}; /* -----  end of class Feature  ----- */


#endif   /* ----- #ifndef feature_INC  ----- */