Add feature handling to state

1 files changed, 32 insertions, 2 deletions

diff --git a/src/feature.cpp b/src/feature.cpp
index 2afd5a0..fd4a85f 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -25,13 +25,18 @@
  * Description:  constructor
  *--------------------------------------------------------------------------------------
  */
-Feature::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 */
 {
-    X = pib;
+    _id = id;
+    X = x2p(xib);
+    X[2] = 1./20.;
     xb0w = xbw;
     q0 = q;
 }  /* -----  end of method Feature::Feature  (constructor)  ----- */
 
+Feature::Feature () {;}
+
 
 /*
  *--------------------------------------------------------------------------------------
@@ -368,3 +373,28 @@ Feature::Hy (  const Vector3d &pos, const Quaterniond &q )
     return H ;
 }		/* -----  end of method Feature::Hy  ----- */
 
+int
+Feature::id ( )
+{
+    return _id;
+}		/* -----  end of method Feature::id  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Feature
+ *      Method:  Feature :: P0
+ * Description:  Returns the initial P matrix.
+ *--------------------------------------------------------------------------------------
+ */
+Matrix<double,3,3>
+Feature::P0 ( )
+{
+    Matrix<double,3,3> P;
+    double p0 = 1e-4;
+    double p1 = 1e-2;
+    P << p0, 0., 0., 
+         0., p0, 0.,
+         0., 0., p1;
+    return P;
+}		/* -----  end of method Feature::P0  ----- */
+