summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
Diffstat
-rw-r--r--tracker-easy/kalman-filter-pose.cpp117
-rw-r--r--tracker-easy/kalman-filter-pose.h32
-rw-r--r--tracker-easy/tracker-easy.cpp29
-rw-r--r--tracker-easy/tracker-easy.h11
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
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-05 19:01:34 +0000