From 7b8821a18a2105df4c0b1eedfbd36f1f7984428c Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Wed, 30 Aug 2017 14:50:03 -0500
Subject: dynamic thresholding

---
 src/state.cpp | 14 +++++++++++++-
 1 file changed, 13 insertions(+), 1 deletion(-)

diff --git a/src/state.cpp b/src/state.cpp
index b7ac67a..c44aafe 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -1230,7 +1230,19 @@ State::doMeasurements ( vector<measurement_t> &z, Vision &viz,
 #else
     featuresAsMeasurements(updatedFeatures, cam, q);
 #endif
-    cerr << z.size() << endl;
+    int numboth = 0;
+    for (auto i=z.begin(); i<z.end(); ++i) {
+        if (i->z_type==BOTH) ++numboth;
+    }
+    cerr << z.size() << " " << numboth << endl;
+    if (numboth<10) {
+        xcorrsrc *= 0.9;
+        xcorrref *= 0.9;
+    } else if (numboth>12) {
+        xcorrsrc *= 1.1;
+        xcorrref *= 1.1;
+    }
+    //cerr << z.size() << endl;
     viz.drawMeasurements(z, cam, cv::Scalar(0,0,200), cv::Scalar(0,0,200), false);
     viz.drawMeasurements(updatedFeatures, cam, cv::Scalar(0,255,0), cv::Scalar(0,200,0), true);
     viz.show();
-- 
cgit v1.1