Easy Tracker: built-in Kalman filter.

It's hard to tell if it brings much though.

4 files changed, 179 insertions, 10 deletions

diff --git a/tracker-easy/kalman-filter-pose.cpp b/tracker-easy/kalman-filter-pose.cpp
new file mode 100644
index 00000000..0f6bdbdb
--- /dev/null
+++ b/tracker-easy/kalman-filter-pose.cpp
@@ -0,0 +1,117 @@
+/* Copyright (c) 2019, Stephane Lenclud <github@lenclud.com>
+
+ * Permission to use, copy, modify, and/or distribute this
+ * software for any purpose with or without fee is hereby granted,
+ * provided that the above copyright notice and this permission
+ * notice appear in all copies.
+ */
+
+#include "kalman-filter-pose.h"
+
+namespace EasyTracker
+{
+
+    KalmanFilterPose::KalmanFilterPose()
+    {
+
+    }
+
+
+    void KalmanFilterPose::Init(int nStates, int nMeasurements, int nInputs, double dt)
+    {
+        iMeasurements = cv::Mat(nMeasurements, 1, CV_64FC1);
+
+        init(nStates, nMeasurements, nInputs, CV_64F);                 // init Kalman Filter
+
+        // TODO: Use parameters instead of magic numbers
+        setIdentity(processNoiseCov, cv::Scalar::all(1)); //1e-5      // set process noise
+        setIdentity(measurementNoiseCov, cv::Scalar::all(1)); //1e-2   // set measurement noise
+        setIdentity(errorCovPost, cv::Scalar::all(1));             // error covariance
+
+        /** DYNAMIC MODEL **/
+
+        //  [1 0 0 dt  0  0 dt2   0   0 0 0 0  0  0  0   0   0   0]
+        //  [0 1 0  0 dt  0   0 dt2   0 0 0 0  0  0  0   0   0   0]
+        //  [0 0 1  0  0 dt   0   0 dt2 0 0 0  0  0  0   0   0   0]
+        //  [0 0 0  1  0  0  dt   0   0 0 0 0  0  0  0   0   0   0]
+        //  [0 0 0  0  1  0   0  dt   0 0 0 0  0  0  0   0   0   0]
+        //  [0 0 0  0  0  1   0   0  dt 0 0 0  0  0  0   0   0   0]
+        //  [0 0 0  0  0  0   1   0   0 0 0 0  0  0  0   0   0   0]
+        //  [0 0 0  0  0  0   0   1   0 0 0 0  0  0  0   0   0   0]
+        //  [0 0 0  0  0  0   0   0   1 0 0 0  0  0  0   0   0   0]
+        //  [0 0 0  0  0  0   0   0   0 1 0 0 dt  0  0 dt2   0   0]
+        //  [0 0 0  0  0  0   0   0   0 0 1 0  0 dt  0   0 dt2   0]
+        //  [0 0 0  0  0  0   0   0   0 0 0 1  0  0 dt   0   0 dt2]
+        //  [0 0 0  0  0  0   0   0   0 0 0 0  1  0  0  dt   0   0]
+        //  [0 0 0  0  0  0   0   0   0 0 0 0  0  1  0   0  dt   0]
+        //  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  1   0   0  dt]
+        //  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  0   1   0   0]
+        //  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  0   0   1   0]
+        //  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  0   0   0   1]
+
+        // position
+        transitionMatrix.at<double>(0, 3) = dt;
+        transitionMatrix.at<double>(1, 4) = dt;
+        transitionMatrix.at<double>(2, 5) = dt;
+        transitionMatrix.at<double>(3, 6) = dt;
+        transitionMatrix.at<double>(4, 7) = dt;
+        transitionMatrix.at<double>(5, 8) = dt;
+        transitionMatrix.at<double>(0, 6) = 0.5*pow(dt, 2);
+        transitionMatrix.at<double>(1, 7) = 0.5*pow(dt, 2);
+        transitionMatrix.at<double>(2, 8) = 0.5*pow(dt, 2);
+
+        // orientation
+        transitionMatrix.at<double>(9, 12) = dt;
+        transitionMatrix.at<double>(10, 13) = dt;
+        transitionMatrix.at<double>(11, 14) = dt;
+        transitionMatrix.at<double>(12, 15) = dt;
+        transitionMatrix.at<double>(13, 16) = dt;
+        transitionMatrix.at<double>(14, 17) = dt;
+        transitionMatrix.at<double>(9, 15) = 0.5*pow(dt, 2);
+        transitionMatrix.at<double>(10, 16) = 0.5*pow(dt, 2);
+        transitionMatrix.at<double>(11, 17) = 0.5*pow(dt, 2);
+
+
+        /** MEASUREMENT MODEL **/
+
+        //  [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
+        //  [0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
+        //  [0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
+        //  [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0]
+        //  [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0]
+        //  [0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0]
+
+        measurementMatrix.at<double>(0, 0) = 1;  // x
+        measurementMatrix.at<double>(1, 1) = 1;  // y
+        measurementMatrix.at<double>(2, 2) = 1;  // z
+        measurementMatrix.at<double>(3, 9) = 1;  // roll
+        measurementMatrix.at<double>(4, 10) = 1; // pitch
+        measurementMatrix.at<double>(5, 11) = 1; // yaw
+    }
+
+    void KalmanFilterPose::Update(double& aX, double& aY, double& aZ, double& aRoll, double& aPitch, double& aYaw)
+    {
+        // Set measurement to predict
+        iMeasurements.at<double>(0) = aX; // x
+        iMeasurements.at<double>(1) = aY; // y
+        iMeasurements.at<double>(2) = aZ; // z
+        iMeasurements.at<double>(3) = aRoll;      // roll
+        iMeasurements.at<double>(4) = aPitch;      // pitch
+        iMeasurements.at<double>(5) = aYaw;      // yaw
+
+        // First predict, to update the internal statePre variable
+        cv::Mat prediction = predict();
+        // The "correct" phase that is going to use the predicted value and our measurement
+        cv::Mat estimated = correct(iMeasurements);
+        // Estimated translation
+        aX = estimated.at<double>(0);
+        aY = estimated.at<double>(1);
+        aZ = estimated.at<double>(2);
+        // Estimated euler angles        
+        aRoll = estimated.at<double>(9);
+        aPitch = estimated.at<double>(10);
+        aYaw = estimated.at<double>(11);
+    }
+
+}
+
diff --git a/tracker-easy/kalman-filter-pose.h b/tracker-easy/kalman-filter-pose.h
new file mode 100644
index 00000000..c454a27c
--- /dev/null
+++ b/tracker-easy/kalman-filter-pose.h
@@ -0,0 +1,32 @@
+/* Copyright (c) 2019, Stephane Lenclud <github@lenclud.com>
+
+ * Permission to use, copy, modify, and/or distribute this
+ * software for any purpose with or without fee is hereby granted,
+ * provided that the above copyright notice and this permission
+ * notice appear in all copies.
+ */
+
+#pragma once
+
+#include <opencv2/core.hpp>
+#include <opencv2/video/tracking.hpp>
+
+
+namespace EasyTracker
+{
+
+    ///
+    /// TODO: do not use a constant time difference
+    ///
+    class KalmanFilterPose: public cv::KalmanFilter
+    {
+    public:
+        KalmanFilterPose();
+        void Init(int aStateCount, int aMeasurementCount, int aInputCount, double aDt);
+        void Update(double& aX, double& aY, double& aZ, double& aRoll, double& aPitch, double& aYaw);
+
+
+        cv::Mat iMeasurements;
+    };
+
+}
diff --git a/tracker-easy/tracker-easy.cpp b/tracker-easy/tracker-easy.cpp
index b15db44e..e432b5da 100644
--- a/tracker-easy/tracker-easy.cpp
+++ b/tracker-easy/tracker-easy.cpp
@@ -1,6 +1,4 @@
-/* Copyright (c) 2012 Patrick Ruoff
- * Copyright (c) 2014-2016 Stanislaw Halik <sthalik@misaki.pl>
- * Copyright (c) 2019 Stephane Lenclud
+/* Copyright (c) 2019 Stephane Lenclud
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -49,6 +47,14 @@ namespace EasyTracker
         set_fov(iSettings.fov);
 
         CreateModelFromSettings();
+
+        
+        //int nStates = 18;            // the number of states
+        //int nMeasurements = 6;       // the number of measured states
+        //int nInputs = 0;             // the number of control actions
+        //double dt = 0.125;           // time between measurements (1/FPS)
+
+        iKf.Init(18,6,0,0033);
     }
 
     Tracker::~Tracker()
@@ -243,12 +249,21 @@ namespace EasyTracker
                 }
             }
 
-            // Send solution data back to main thread
-            QMutexLocker l2(&data_lock);
+                        
             if (iBestSolutionIndex != -1)
             {
-                iBestAngles = iAngles[iBestSolutionIndex];
-                iBestTranslation = iTranslations[iBestSolutionIndex];
+                // Best translation
+                cv::Vec3d translation = iTranslations[iBestSolutionIndex];
+                // Best angles
+                cv::Vec3d angles = iAngles[iBestSolutionIndex];
+
+                // Pass solution through our kalman filter
+                iKf.Update(translation[0], translation[1], translation[2], angles[2], angles[0], angles[1]);
+
+                // Send solution data back to main thread
+                QMutexLocker l2(&data_lock);
+                iBestAngles = angles;
+                iBestTranslation = translation;
             }
 
         }
diff --git a/tracker-easy/tracker-easy.h b/tracker-easy/tracker-easy.h
index a8c8e6be..a84d725e 100644
--- a/tracker-easy/tracker-easy.h
+++ b/tracker-easy/tracker-easy.h
@@ -1,5 +1,4 @@
-/* Copyright (c) 2012 Patrick Ruoff
- * Copyright (c) 2014-2016 Stanislaw Halik <sthalik@misaki.pl>
+/* Copyright (c) 2019 Stephane Lenclud
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -14,15 +13,18 @@
 #include "video/camera.hpp"
 #include "compat/timer.hpp"
 
+
 #include "preview.h"
 #include "settings.h"
 #include "point-extractor.h"
+#include "kalman-filter-pose.h"
 
 #include <atomic>
 #include <memory>
 #include <vector>
 
 #include <opencv2/core.hpp>
+#include <opencv2/video/tracking.hpp>
 
 #include <QThread>
 #include <QMutex>
@@ -87,7 +89,7 @@ namespace EasyTracker
         std::atomic<bool> ever_success = false;
         mutable QMutex center_lock, data_lock;
 
-        //
+        // Statistics
         Timer iTimer;
         Timer iFpsTimer;
         int iFrameCount = 0;
@@ -95,6 +97,9 @@ namespace EasyTracker
         int iFps = 0;
         int iSkippedFps = 0;
 
+        //
+        KalmanFilterPose iKf;
+
         // Vertices defining the model we are tracking
         std::vector<cv::Point3f> iModel;
         // Bitmap points corresponding to model vertices