Add initial view to Feature.

2 files changed, 10 insertions, 0 deletions

diff --git a/src/feature.cpp b/src/feature.cpp
index 23a7fb7..fe62bbf 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -32,10 +32,17 @@ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xr,
     Vector3d xib;
     xib = findDepth(q,z,xs,xr);
     X = x2p(xib);
+    X0 = X;
     xb0w = xbw;
     q0 = q;
 }  /* -----  end of method Feature::Feature  (constructor)  ----- */
 
+Vector2d
+Feature::initialView (  )
+{
+    return X0.segment<2>(0);
+}		/* -----  end of method Feature::initialView  ----- */
+
 
 /*
  *--------------------------------------------------------------------------------------
diff --git a/src/feature.h b/src/feature.h
index d7015f2..a405e8f 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -4,6 +4,7 @@
 #include <iostream>
 #include "types.h"
 
+using Eigen::Vector2d;
 using Eigen::Vector3d;
 using Eigen::Matrix;
 using Eigen::MatrixXd;
@@ -28,6 +29,7 @@ class Feature
 
         /* ====================  ACCESSORS     ======================================= */
         int id();
+        Vector2d initialView();
         Matrix<double,3,3> P0();
         UTM utm(Vector3d &xbw, Quaterniond &q); 
 
@@ -65,6 +67,7 @@ class Feature
         /* ====================  DATA MEMBERS  ======================================= */
         int _id;
         Vector3d X;
+        Vector3d X0;
         Quaterniond q0;
         Vector3d xb0w;