Add Feature method to test if in FOV

Added a boolean function that returns if the feature is in the FOV. This
method does not use the camera calibration, it just tests if the
feature's angle w.r.t. the body.

2 files changed, 26 insertions, 0 deletions

diff --git a/src/feature.cpp b/src/feature.cpp
index 136f820..23a7fb7 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -513,3 +513,28 @@ Feature::dx ( const Vector3d &del )
     return ;
 }		/* -----  end of method Feature::dx  ----- */
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Feature
+ *      Method:  Feature :: inFOV
+ * Description:  Returns true if the feature is in the field of view. This is a
+ * somewhat loose interpretation of that because we don't know the actual camera
+ * parameters.
+ *--------------------------------------------------------------------------------------
+ */
+bool
+Feature::inFOV ( )
+{
+    bool rv;
+    double angle;
+    angle = atan2(1./X[2],X[0]/X[2]);
+    if (X[2]>1) {
+        rv = false;
+    } else if (angle<M_PI_4 || angle>(3*M_PI_4)) {
+        rv = false;
+    } else {
+        rv = true; 
+    }
+    return rv;
+}		/* -----  end of method Feature::inFOV  ----- */
+
diff --git a/src/feature.h b/src/feature.h
index b921d8b..d7015f2 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -37,6 +37,7 @@ class Feature
 
         /* ====================  OPERATORS     ======================================= */
         bool sane();
+        bool inFOV();
         Vector3d findDepth( const Quaterniond &q, double z, const Vector3d &xs,
                 const Vector3d &xr);
         Matrix<double,3,9> Fx( double dt );