1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
|
/* 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 "ftnoir_tracker_pt_settings.h"
using namespace pt_types;
#include <opencv2/core.hpp>
#include <cstddef>
#include <memory>
#include <vector>
#include <array>
#include <QObject>
class Affine final
{
public:
Affine() : R(mat33::eye()), t(0,0,0) {}
Affine(const mat33& R, const vec3& t) : R(R),t(t) {}
mat33 R;
vec3 t;
};
inline Affine operator*(const Affine& X, const Affine& Y)
{
return Affine(X.R*Y.R, X.R*Y.t + X.t);
}
inline Affine operator*(const mat33& X, const Affine& Y)
{
return Affine(X*Y.R, X*Y.t);
}
inline Affine operator*(const Affine& X, const mat33& Y)
{
return Affine(X.R*Y, X.t);
}
inline vec3 operator*(const Affine& X, const vec3& v)
{
return X.R*v + X.t;
}
// ----------------------------------------------------------------------------
// Describes a 3-point model
// nomenclature as in
// [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"]
class PointModel final
{
friend class PointTracker;
public:
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 };
PointModel(settings_pt& s);
void set_model(settings_pt& 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, f focal_length, bool dynamic_pose, int init_phase_timeout, int w, int h);
Affine pose() { 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);
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, f focal_length, int w, int h);
int POSIT(const PointModel& point_model, const PointOrder& order, f focal_length); // The POSIT algorithm, returns the number of iterations
Affine X_CM; // trafo from model to camera
Timer t;
bool init_phase;
};
|
