half gain logic done

1 files changed, 175 insertions, 0 deletions

diff --git a/facetracknoir/gain-control.hpp b/facetracknoir/gain-control.hpp
new file mode 100644
index 00000000..28887700
--- /dev/null
+++ b/facetracknoir/gain-control.hpp
@@ -0,0 +1,175 @@
+#pragma once
+
+#include <algorithm>
+#undef NDEBUG
+#include <cassert>
+#include <iterator>
+#include <tuple>
+#include <deque>
+#include <vector>
+
+#include "timer.hpp"
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include <QDebug>
+
+namespace detail {
+    template<typename t1, typename t2, typename t, typename m = t>
+    class zip_iterator : public std::iterator<std::forward_iterator_tag, t>
+    {
+    private:
+        using self = zip_iterator<t1, t2, t, m>;
+        t1 x1, z1;
+        t2 x2, z2;
+        void maybe_end() { if (x1 == z1 || x2 == z2) *this = end(); }
+    public:
+        zip_iterator(const t1& it1, const t1& end1, const t2& it2, const t2& end2)
+            : x1(it1), z1(end1), x2(it2), z2(end2) { maybe_end(); }
+        constexpr zip_iterator() {}
+        
+        static constexpr self end() { return self(); }
+        
+        self operator++() { x1++; x2++; self tmp = *this; maybe_end(); return tmp; }
+        self operator++(int) { self tmp(*this); x1++; x2++; maybe_end(); return tmp; }
+        bool operator==(const self& rhs) const { return x1 == rhs.x1 && x2 == rhs.x2; }
+        bool operator!=(const self& rhs) const { return !this->operator ==(rhs); }
+        t operator*() { return m(*x1, *x2); }
+    };
+}
+
+class Gain {
+private:
+    static constexpr bool use_box_filter = true;
+    static constexpr int box_size = 20 / 640.;
+    static constexpr double control_upper_bound = 1.0; // XXX FIXME implement for logitech crapola
+    static constexpr int GAIN_HISTORY_COUNT = 15, GAIN_HISTORY_EVERY_MS = 200;
+    
+    int control;
+    double step, eps;
+    
+    std::deque<double> means_history;
+    
+    Timer debug_timer, history_timer;
+    
+    typedef unsigned char px;
+    template<typename t1, typename t2, typename t, typename m = t>
+    using zip_iterator = detail::zip_iterator<t1, t2, t, m>;
+    
+    static double mean(const cv::Mat& frame)
+    {
+        // grayscale only
+        assert(frame.channels() == 1);
+        assert(frame.elemSize() == 1);
+        assert(!frame.empty());
+        
+        return std::accumulate(frame.begin<px>(), frame.end<px>(), 0.) / (frame.rows * frame.cols);
+    }
+    
+    static double get_variance(const cv::Mat& frame, double mean)
+    {
+        struct variance {
+        private:
+            double mu;
+        public:
+            variance(double mu) : mu(mu) {}
+            double operator()(double seed, px p)
+            {
+                double tmp = p - mu;
+                return seed + tmp * tmp;
+            }
+        } logic(mean);
+        
+        return std::accumulate(frame.begin<unsigned char>(), frame.end<unsigned char>(), 0., logic) / (frame.rows * frame.cols);
+    }
+    
+    static double get_covariance(const cv::Mat& frame, double mean, double prev_mean)
+    {
+        struct covariance {
+        public:
+            using pair = std::tuple<px, px>;
+        private:
+            double mu_0, mu_1;
+            
+            inline double Cov(double seed, const pair& t)
+            {
+                px p0 = std::get<0>(t);
+                px p1 = std::get<1>(t);
+                return seed + (p0 - mu_0) * (p1 - mu_1);
+            }
+        public:
+            covariance(double mu_0, double mu_1) : mu_0(mu_0), mu_1(mu_1) {}
+            
+            double operator()(double seed, const pair& t)
+            {
+                return Cov(seed, t);
+            }
+        } logic(mean, prev_mean);
+        
+        const double N = frame.rows * frame.cols;
+        
+        using zipper = zip_iterator<cv::MatConstIterator_<px>,
+        cv::MatConstIterator_<px>,
+        std::tuple<px, px>>;
+        
+        zipper zip(frame.begin<px>(),
+                                             frame.end<px>(),
+                                             frame.begin<px>(),
+                                             frame.end<px>());
+        std::vector<covariance::pair> values(zip, zipper::end());
+        
+        return std::accumulate(values.begin(), values.end(), 0., logic) / N;
+    }
+    
+#pragma GCC diagnostic ignored "-Wsign-compare"
+    
+public:
+    Gain(int control = CV_CAP_PROP_GAIN, double step = 0.3, double eps = 0.02) :
+        control(control), step(step), eps(eps)
+    {
+    }
+    
+    void tick(cv::VideoCapture&, const cv::Mat& frame_)
+    {
+        cv::Mat frame;
+        
+        if (use_box_filter)
+        {
+            cv::Mat tmp(frame_);
+            static constexpr int min_box = 3;
+            static constexpr int box = 2 * box_size;
+            cv::blur(frame_, tmp, cv::Size(min_box + box * frame_.cols, min_box + box * frame_.rows));
+            frame = tmp;
+        }
+        else
+            frame = frame_;
+
+        const double mu = mean(frame);
+        const double var = get_variance(frame, mu);
+        
+        if (debug_timer.elapsed_ms() > 500)
+        {
+            debug_timer.start();
+            qDebug() << "gain:" << "mean" << mu << "variance" << var;
+        }
+        
+        const int sz = means_history.size();
+        
+        for (int i = 0; i < sz; i++)
+        {
+            const double cov = get_covariance(frame, mu, means_history[i]);
+            
+            qDebug() << "cov" << i << cov;
+        }
+        
+        if (GAIN_HISTORY_COUNT > means_history.size() && history_timer.elapsed_ms() > GAIN_HISTORY_EVERY_MS)
+        {
+            means_history.push_front(mu);
+            
+            if (GAIN_HISTORY_COUNT == means_history.size())
+                means_history.pop_back();
+        }
+    }
+};