From 50834b4ba0904a13242476cc515f85fda4f30844 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Wed, 22 Mar 2017 15:47:00 -0500
Subject: Add feature handling to state

---
 src/feature.h | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

(limited to 'src/feature.h')

diff --git a/src/feature.h b/src/feature.h
index fc8fe2f..eb3e5bd 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -18,9 +18,12 @@ class Feature
 {
     public:
         /* ====================  LIFECYCLE     ======================================= */
-        Feature (const Vector3d &pib, const Vector3d &xbw, const Quaterniond q );                             /* constructor */
+        Feature ();
+        Feature ( int id, const Vector3d &xib, const Vector3d &xpb, const Vector3d &xbw, const Quaterniond &q );                             /* constructor */
 
         /* ====================  ACCESSORS     ======================================= */
+        int id();
+        Matrix<double,3,3> P0();
 
         /* ====================  MUTATORS      ======================================= */
         void motionModel ( const Vector3d &ang, const Vector3d &vel, const double dt);
@@ -37,6 +40,8 @@ class Feature
         Matrix<double,6,6> R();
         Matrix<double,6,6> S ( const Matrix<double,9,9> &Pxx, const Matrix<double,9,3> &Pxy,
             const Matrix<double,3,3> &Pyy, const Vector3d &pos, const Quaterniond &q);
+        Vector3d p2x(const Vector3d &p);
+        Vector3d x2p(const Vector3d &x);
 
     protected:
         /* ====================  METHODS       ======================================= */
@@ -45,10 +50,9 @@ class Feature
 
     private:
         /* ====================  METHODS       ======================================= */
-        Vector3d p2x(const Vector3d &p);
-        Vector3d x2p(const Vector3d &x);
 
         /* ====================  DATA MEMBERS  ======================================= */
+        int _id;
         Vector3d X;
         Quaterniond q0;
         Vector3d xb0w;
-- 
cgit v1.1