Add Feature methods.

Added a method to test if a feature is sane. It looks to see if the
feature has negative depth or if it is too close or too far away.

Added a method to output the feature location as a UTM struct.

1 files changed, 2 insertions, 0 deletions

diff --git a/src/feature.h b/src/feature.h
index fe2afea..3e99bab 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -29,11 +29,13 @@ class Feature
         /* ====================  ACCESSORS     ======================================= */
         int id();
         Matrix<double,3,3> P0();
+        UTM utm(Vector3d &xbw, Quaterniond &q); 
 
         /* ====================  MUTATORS      ======================================= */
         void motionModel ( const Vector3d &ang, const Vector3d &vel, const double dt);
 
         /* ====================  OPERATORS     ======================================= */
+        bool sane();
         Vector3d findDepth( const Quaterniond &q, double z, const Vector3d &xs,
                 const Vector3d &xr);
         Matrix<double,3,9> Fx( double dt );