summaryrefslogtreecommitdiffhomepage
path: root/tracker-points/point_tracker.h
diff options
context:
space:
mode:
Diffstat (limited to 'tracker-points/point_tracker.h')
-rw-r--r--tracker-points/point_tracker.h88
1 files changed, 88 insertions, 0 deletions
diff --git a/tracker-points/point_tracker.h b/tracker-points/point_tracker.h
new file mode 100644
index 00000000..70c7a9fc
--- /dev/null
+++ b/tracker-points/point_tracker.h
@@ -0,0 +1,88 @@
+/* Copyright (c) 2012 Patrick Ruoff
+ *
+ * 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 "compat/timer.hpp"
+#include "cv/affine.hpp"
+#include "cv/numeric.hpp"
+#include "pt-api.hpp"
+
+#include <cstddef>
+#include <memory>
+#include <vector>
+#include <array>
+
+#include <opencv2/core.hpp>
+
+#include <QObject>
+
+namespace pt_impl {
+
+// ----------------------------------------------------------------------------
+// Describes a 3-point model
+// nomenclature as in
+// [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"]
+
+using namespace numeric_types;
+
+struct PointModel final
+{
+ static constexpr unsigned N_POINTS = 3;
+
+ vec3 M01; // M01 in model frame
+ vec3 M02; // M02 in model frame
+
+ vec3 u; // unit vector perpendicular to M01,M02-plane
+
+ mat22 P;
+
+ enum Model { Clip, Cap, Custom };
+
+ explicit PointModel(const pt_settings& s);
+ void set_model(const pt_settings& s);
+
+ void get_d_order(const vec2* points, unsigned* d_order, const vec2& d) const;
+};
+
+// ----------------------------------------------------------------------------
+// Tracks a 3-point model
+// implementing the POSIT algorithm for coplanar points as presented in
+// [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"]
+class PointTracker final
+{
+public:
+ PointTracker();
+ // track the pose using the set of normalized point coordinates (x pos in range -0.5:0.5)
+ // f : (focal length)/(sensor width)
+ // dt : time since last call
+ void track(const std::vector<vec2>& projected_points, const PointModel& model, const pt_camera_info& info, int init_phase_timeout);
+ Affine pose() const { return X_CM; }
+ vec2 project(const vec3& v_M, f focal_length);
+ vec2 project(const vec3& v_M, f focal_length, const Affine& X_CM);
+ void reset_state();
+
+private:
+ // the points in model order
+ using PointOrder = std::array<vec2, 3>;
+
+ PointOrder find_correspondences(const vec2* projected_points, const PointModel &model);
+ PointOrder find_correspondences_previous(const vec2* points, const PointModel &model, const pt_camera_info& info);
+ // The POSIT algorithm, returns the number of iterations
+ int POSIT(const PointModel& point_model, const PointOrder& order, f focal_length);
+
+ Affine X_CM; // transform from model to camera
+ Affine X_CM_expected;
+ PointOrder prev_positions;
+ Timer t;
+ bool init_phase = true;
+};
+
+} // ns pt_impl
+
+using PointTracker = pt_impl::PointTracker;
+using PointModel = pt_impl::PointModel;