summaryrefslogtreecommitdiffhomepage
path: root/tracker-pt/point_tracker.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'tracker-pt/point_tracker.cpp')
-rw-r--r--tracker-pt/point_tracker.cpp152
1 files changed, 96 insertions, 56 deletions
diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp
index 51f10470..4c1e177f 100644
--- a/tracker-pt/point_tracker.cpp
+++ b/tracker-pt/point_tracker.cpp
@@ -13,32 +13,69 @@
#include <QDebug>
-static void get_row(const cv::Matx33f& m, int i, cv::Vec3f& v)
+using mat33 = pt_types::mat33;
+using vec3 = pt_types::vec3;
+using f = pt_types::f;
+
+static void get_row(const mat33& m, int i, vec3& v)
{
v[0] = m(i,0);
v[1] = m(i,1);
v[2] = m(i,2);
}
-static void set_row(cv::Matx33f& m, int i, const cv::Vec3f& v)
+static void set_row(mat33& m, int i, const vec3& v)
{
m(i,0) = v[0];
m(i,1) = v[1];
m(i,2) = v[2];
}
-static bool d_vals_sort(const std::pair<float,int> a, const std::pair<float,int> b)
+static bool d_vals_sort(const std::pair<f,int> a, const std::pair<f,int> b)
{
return a.first < b.first;
}
-void PointModel::get_d_order(const std::vector<cv::Vec2f>& points, int d_order[], cv::Vec2f d) const
+PointModel::PointModel(settings_pt& s)
+{
+ set_model(s);
+ // calculate u
+ u = M01.cross(M02);
+ u /= norm(u);
+
+ // calculate projection matrix on M01,M02 plane
+ f s11 = M01.dot(M01);
+ f s12 = M01.dot(M02);
+ f s22 = M02.dot(M02);
+ P = 1/(s11*s22-s12*s12) * mat22(s22, -s12, -s12, s11);
+}
+
+void PointModel::set_model(settings_pt& s)
+{
+ switch (s.active_model_panel)
+ {
+ case Clip:
+ M01 = vec3(0, static_cast<f>(s.clip_ty), -static_cast<f>(s.clip_tz));
+ M02 = vec3(0, -static_cast<f>(s.clip_by), -static_cast<f>(s.clip_bz));
+ break;
+ case Cap:
+ M01 = vec3(-static_cast<f>(s.cap_x), -static_cast<f>(s.cap_y), -static_cast<f>(s.cap_z));
+ M02 = vec3(static_cast<f>(s.cap_x), -static_cast<f>(s.cap_y), -static_cast<f>(s.cap_z));
+ break;
+ case Custom:
+ M01 = vec3(s.m01_x, s.m01_y, s.m01_z);
+ M02 = vec3(s.m02_x, s.m02_y, s.m02_z);
+ break;
+ }
+}
+
+void PointModel::get_d_order(const std::vector<vec2>& points, int* d_order, vec2 d) const
{
// fit line to orthographically projected points
- std::vector<std::pair<float,int>> d_vals;
+ std::vector<std::pair<f,int>> d_vals;
// get sort indices with respect to d scalar product
for (unsigned i = 0; i < PointModel::N_POINTS; ++i)
- d_vals.push_back(std::pair<float, int>(d.dot(points[i]), i));
+ d_vals.push_back(std::pair<f, int>(d.dot(points[i]), i));
std::sort(d_vals.begin(),
d_vals.end(),
@@ -54,12 +91,12 @@ PointTracker::PointTracker() : init_phase(true)
{
}
-PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::vector<cv::Vec2f>& points, const PointModel& model, float f)
+PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::vector<vec2>& points, const PointModel& model, f focal_length)
{
PointTracker::PointOrder p;
- p.points[0] = project(cv::Vec3f(0,0,0), f);
- p.points[1] = project(model.M01, f);
- p.points[2] = project(model.M02, f);
+ p.points[0] = project(vec3(0,0,0), focal_length);
+ p.points[1] = project(model.M01, focal_length);
+ p.points[2] = project(model.M02, focal_length);
// set correspondences by minimum distance to projected model point
bool point_taken[PointModel::N_POINTS];
@@ -68,13 +105,13 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::
for (unsigned i=0; i<PointModel::N_POINTS; ++i)
{
- float min_sdist = 0;
+ f min_sdist = 0;
unsigned min_idx = 0;
// find closest point to projected model point i
for (unsigned j=0; j<PointModel::N_POINTS; ++j)
{
- cv::Vec2f d = p.points[i]-points[j];
- float sdist = d.dot(d);
+ vec2 d = p.points[i]-points[j];
+ f sdist = d.dot(d);
if (sdist < min_sdist || j==0)
{
min_idx = j;
@@ -93,7 +130,7 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::
return p;
}
-void PointTracker::track(const std::vector<cv::Vec2f>& points, const PointModel& model, float f, bool dynamic_pose, int init_phase_timeout)
+void PointTracker::track(const std::vector<vec2>& points, const PointModel& model, f focal_length, bool dynamic_pose, int init_phase_timeout)
{
PointOrder order;
@@ -106,26 +143,26 @@ void PointTracker::track(const std::vector<cv::Vec2f>& points, const PointModel&
if (!dynamic_pose || init_phase)
order = find_correspondences(points, model);
else
- order = find_correspondences_previous(points, model, f);
+ order = find_correspondences_previous(points, model, focal_length);
- POSIT(model, order, f);
+ POSIT(model, order, focal_length);
init_phase = false;
t.start();
}
-PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv::Vec2f>& points, const PointModel& model)
+PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<vec2>& points, const PointModel& model)
{
// We do a simple freetrack-like sorting in the init phase...
// sort points
int point_d_order[PointModel::N_POINTS];
int model_d_order[PointModel::N_POINTS];
- cv::Vec2f d(model.M01[0]-model.M02[0], model.M01[1]-model.M02[1]);
+ vec2 d(model.M01[0]-model.M02[0], model.M01[1]-model.M02[1]);
model.get_d_order(points, point_d_order, d);
// calculate d and d_order for simple freetrack-like point correspondence
- model.get_d_order(std::vector<cv::Vec2f> {
- cv::Vec2f{0,0},
- cv::Vec2f(model.M01[0], model.M01[1]),
- cv::Vec2f(model.M02[0], model.M02[1])
+ model.get_d_order(std::vector<vec2> {
+ vec2{0,0},
+ vec2(model.M01[0], model.M01[1]),
+ vec2(model.M02[0], model.M02[1])
},
model_d_order,
d);
@@ -137,7 +174,7 @@ PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv
return p;
}
-int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float focal_length)
+int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, f focal_length)
{
// POSIT algorithm for coplanar points as presented in
// [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"]
@@ -145,45 +182,45 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
// The expected rotation used for resolving the ambiguity in POSIT:
// In every iteration step the rotation closer to R_expected is taken
- cv::Matx33f R_expected = cv::Matx33f::eye();
+ mat33 R_expected = mat33::eye();
// initial pose = last (predicted) pose
- cv::Vec3f k;
+ vec3 k;
get_row(R_expected, 2, k);
- float Z0 = 1000.f;
+ f Z0 = f(1000);
- float old_epsilon_1 = 0;
- float old_epsilon_2 = 0;
- float epsilon_1 = 1;
- float epsilon_2 = 1;
+ f old_epsilon_1 = 0;
+ f old_epsilon_2 = 0;
+ f epsilon_1 = 1;
+ f epsilon_2 = 1;
- cv::Vec3f I0, J0;
- cv::Vec2f I0_coeff, J0_coeff;
+ vec3 I0, J0;
+ vec2 I0_coeff, J0_coeff;
- cv::Vec3f I_1, J_1, I_2, J_2;
- cv::Matx33f R_1, R_2;
- cv::Matx33f* R_current;
+ vec3 I_1, J_1, I_2, J_2;
+ mat33 R_1, R_2;
+ mat33* R_current = &R_1;
- constexpr int MAX_ITER = 100;
- const float EPS_THRESHOLD = 1e-4f;
+ static constexpr int max_iter = 100;
- const cv::Vec2f* order = order_.points;
+ const vec2* order = order_.points;
using std::sqrt;
using std::atan;
using std::cos;
using std::sin;
+ using std::fabs;
int i=1;
- for (; i<MAX_ITER; ++i)
+ for (; i<max_iter; ++i)
{
epsilon_1 = k.dot(model.M01)/Z0;
epsilon_2 = k.dot(model.M02)/Z0;
// vector of scalar products <I0, M0i> and <J0, M0i>
- cv::Vec2f I0_M0i(order[1][0]*(1 + epsilon_1) - order[0][0],
+ vec2 I0_M0i(order[1][0]*(1 + epsilon_1) - order[0][0],
order[2][0]*(1 + epsilon_2) - order[0][0]);
- cv::Vec2f J0_M0i(order[1][1]*(1 + epsilon_1) - order[0][1],
+ vec2 J0_M0i(order[1][1]*(1 + epsilon_1) - order[0][1],
order[2][1]*(1 + epsilon_2) - order[0][1]);
// construct projection of I, J onto M0i plane: I0 and J0
@@ -193,22 +230,22 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
J0 = J0_coeff[0]*model.M01 + J0_coeff[1]*model.M02;
// calculate u component of I, J
- float II0 = I0.dot(I0);
- float IJ0 = I0.dot(J0);
- float JJ0 = J0.dot(J0);
- float rho, theta;
+ f II0 = I0.dot(I0);
+ f IJ0 = I0.dot(J0);
+ f JJ0 = J0.dot(J0);
+ f rho, theta;
// CAVEAT don't change to comparison with an epsilon -sh 20160423
if (JJ0 == II0) {
- rho = std::sqrt(std::abs(2*IJ0));
- theta = -PI/4;
+ rho = sqrt(fabs(2*IJ0));
+ theta = -pi/4;
if (IJ0<0) theta *= -1;
}
else {
rho = sqrt(sqrt( (JJ0-II0)*(JJ0-II0) + 4*IJ0*IJ0 ));
theta = atan( -2*IJ0 / (JJ0-II0) );
// avoid branch misprediction
- theta += (JJ0 - II0 < 0) * PI;
- theta /= 2;
+ theta += (JJ0 - II0 < 0) * pi;
+ theta *= f(.5);
}
// construct the two solutions
@@ -218,7 +255,7 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
J_1 = J0 + rho*sin(theta)*model.u;
J_2 = J0 - rho*sin(theta)*model.u;
- float norm_const = 1/cv::norm(I_1); // all have the same norm
+ f norm_const = 1/cv::norm(I_1); // all have the same norm
// create rotation matrices
I_1 *= norm_const; J_1 *= norm_const;
@@ -237,8 +274,8 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
// pick the rotation solution closer to the expected one
// in simple metric d(A,B) = || I - A * B^T ||
- float R_1_deviation = cv::norm(cv::Matx33f::eye() - R_expected * R_1.t());
- float R_2_deviation = cv::norm(cv::Matx33f::eye() - R_expected * R_2.t());
+ f R_1_deviation = cv::norm(mat33::eye() - R_expected * R_1.t());
+ f R_2_deviation = cv::norm(mat33::eye() - R_expected * R_2.t());
if (R_1_deviation < R_2_deviation)
R_current = &R_1;
@@ -248,8 +285,11 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
get_row(*R_current, 2, k);
// check for convergence condition
- if (std::abs(epsilon_1 - old_epsilon_1) + std::abs(epsilon_2 - old_epsilon_2) < EPS_THRESHOLD)
+ const f delta = fabs(epsilon_1 - old_epsilon_1) + fabs(epsilon_2 - old_epsilon_2);
+
+ if (!(delta > eps))
break;
+
old_epsilon_1 = epsilon_1;
old_epsilon_2 = epsilon_2;
}
@@ -266,8 +306,8 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
return i;
}
-cv::Vec2f PointTracker::project(const cv::Vec3f& v_M, float f)
+pt_types::vec2 PointTracker::project(const vec3& v_M, f focal_length)
{
- cv::Vec3f v_C = X_CM * v_M;
- return cv::Vec2f(f*v_C[0]/v_C[2], f*v_C[1]/v_C[2]);
+ vec3 v_C = X_CM * v_M;
+ return vec2(focal_length*v_C[0]/v_C[2], focal_length*v_C[1]/v_C[2]);
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-21 07:42:12 +0000