summaryrefslogtreecommitdiff
path: root/src/feature.cpp
diff options
context:
space:
mode:
authorMartin Miller2017-03-25 17:24:06 -0500
committerMartin Miller2017-03-25 17:24:06 -0500
commit8535af062e26f583383174e7c78514bc107efdcb (patch)
treed70ff608d12c183bc5703f575e10b2f836b5c227 /src/feature.cpp
parentf409d322fd400b412c632b74f76c6c9794fbf6c0 (diff)
downloadrefslam-8535af062e26f583383174e7c78514bc107efdcb.zip
refslam-8535af062e26f583383174e7c78514bc107efdcb.tar.gz
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.
Diffstat (limited to 'src/feature.cpp')
-rw-r--r--src/feature.cpp25
1 files changed, 25 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 ----- */
+