dynamic thresholding

1 files changed, 13 insertions, 1 deletions

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();