From 8535af062e26f583383174e7c78514bc107efdcb Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Sat, 25 Mar 2017 17:24:06 -0500
Subject: 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.
---
 src/feature.cpp | 25 +++++++++++++++++++++++++
 src/feature.h   |  1 +
 2 files changed, 26 insertions(+)

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 );
-- 
cgit v1.1