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.

2 files changed, 52 insertions, 4 deletions

diff --git a/src/feature.cpp b/src/feature.cpp
index aa0490e..49a6262 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -82,9 +82,6 @@ Feature::findDepth ( const Quaterniond &qbw, double z, const Vector3d &xs,
     //Vector4d b;
     Matrix<double,6,1> b;
     b.segment<3>(0) = xs;
-    //b[3] = 1;
-    //b.segment<3>(4) = xr;
-    //b[7] = 1;
     b[3] = xr[0];
     b[4] = xr[1];
     b[5] = xr[2];
@@ -94,7 +91,6 @@ Feature::findDepth ( const Quaterniond &qbw, double z, const Vector3d &xs,
     //xb4 = P.inverse()*b;
     xb4 /= xb4[3];
     xb = xb4.segment<3>(0);
-    cout << xb.transpose() << endl;
 
     return xb ;
 }		/* -----  end of method Feature::findDepth  ----- */
@@ -460,3 +456,53 @@ Feature::P0 ( )
     return P;
 }		/* -----  end of method Feature::P0  ----- */
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Feature
+ *      Method:  Feature :: utm
+ * Description:  Return the feature position in world coordinates as a UTM
+ * struct.
+ *--------------------------------------------------------------------------------------
+ */
+UTM
+Feature::utm ( Vector3d &xbw, Quaterniond &q )
+{
+    UTM utm;
+    Vector3d xib,xiw;
+    xib = p2x(X);
+    xiw = xbw + q.toRotationMatrix()*xib;
+
+    utm.northing = xiw[0];
+    utm.easting = xiw[1];
+    utm.up = -xiw[2];
+    utm.zone_i = 16;
+    utm.zone_c = 'T';
+
+    return utm ;
+}		/* -----  end of method Feature::utm  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Feature
+ *      Method:  Feature :: sane
+ * Description:  Check if the feature position is sane.
+ *--------------------------------------------------------------------------------------
+ */
+bool
+Feature::sane ( )
+{
+    bool rv;
+    // Check that the depth is positive.
+    if (X[2]<0) {
+        rv = false;
+    } else if (1./X[2]<1.) { // Too close
+        rv = false;
+    } else if (1./X[2]>40.) { // Too far
+        rv = false;
+    } else {
+        rv = true;
+    }
+    
+    return rv ;
+}		/* -----  end of method Feature::sane  ----- */
+
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 );