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.cpp106
1 files changed, 55 insertions, 51 deletions
diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp
index 599ce2d3..bc5bf3cd 100644
--- a/tracker-pt/point_tracker.cpp
+++ b/tracker-pt/point_tracker.cpp
@@ -15,32 +15,32 @@
const float PI = 3.14159265358979323846f;
-static void get_row(const cv::Matx33f& m, int i, cv::Vec3f& v)
+static void get_row(const cv::Matx33d& m, int i, cv::Vec3d& 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(cv::Matx33d& m, int i, const cv::Vec3d& 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<double,int> a, const std::pair<double,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
+void PointModel::get_d_order(const std::vector<cv::Vec2d>& points, int* d_order, const cv::Vec2d& d) const
{
// fit line to orthographically projected points
- std::vector<std::pair<float,int>> d_vals;
+ std::vector<std::pair<double,int>> d_vals;
// get sort indices with respect to d scalar product
for (unsigned i = 0; i<points.size(); ++i)
- d_vals.push_back(std::pair<float, int>(d.dot(points[i]), i));
+ d_vals.push_back(std::pair<double, int>(d.dot(points[i]), i));
std::sort(d_vals.begin(),
d_vals.end(),
@@ -70,13 +70,13 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::
for (int i=0; i<PointModel::N_POINTS; ++i)
{
- float min_sdist = 0;
- int min_idx = 0;
+ double min_sdist = 0;
+ unsigned min_idx = 0;
// find closest point to projected model point i
for (int j=0; j<PointModel::N_POINTS; ++j)
{
- cv::Vec2f d = p.points[i]-points[j];
- float sdist = d.dot(d);
+ cv::Vec2d d = p.points[i]-points[j];
+ double sdist = d.dot(d);
if (sdist < min_sdist || j==0)
{
min_idx = j;
@@ -115,19 +115,19 @@ void PointTracker::track(const std::vector<cv::Vec2f>& points, const PointModel&
t.start();
}
-PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv::Vec2f>& points, const PointModel& model)
+PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv::Vec2d>& 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]);
+ cv::Vec2d 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<cv::Vec2d> {
+ cv::Vec2d{0,0},
+ cv::Vec2d(model.M01[0], model.M01[1]),
+ cv::Vec2d(model.M02[0], model.M02[1])
},
model_d_order,
d);
@@ -140,6 +140,7 @@ PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv
}
int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float focal_length)
+bool PointTracker::POSIT(const PointModel& model, const PointOrder& order_, double 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"]
@@ -147,29 +148,29 @@ 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();
+ cv::Matx33d R_expected = cv::Matx33d::eye();
// initial pose = last (predicted) pose
- cv::Vec3f k;
+ cv::Vec3d k;
get_row(R_expected, 2, k);
- float Z0 = 1000.f;
- float old_epsilon_1 = 0;
- float old_epsilon_2 = 0;
- float epsilon_1 = 1;
- float epsilon_2 = 1;
+ double Z0 = 1000;
+ double old_epsilon_1 = 0;
+ double old_epsilon_2 = 0;
+ double epsilon_1 = 1;
+ double epsilon_2 = 1;
- cv::Vec3f I0, J0;
- cv::Vec2f I0_coeff, J0_coeff;
+ cv::Vec3d I0, J0;
+ cv::Vec2d I0_coeff, J0_coeff;
- cv::Vec3f I_1, J_1, I_2, J_2;
- cv::Matx33f R_1, R_2;
- cv::Matx33f* R_current;
+ cv::Vec3d I_1, J_1, I_2, J_2;
+ cv::Matx33d R_1, R_2;
+ cv::Matx33d& R_current = R_1;
- const int MAX_ITER = 100;
- const float EPS_THRESHOLD = 1e-4;
+ const int MAX_ITER = 500;
+ static constexpr double eps = 1e-6;
- const cv::Vec2f* order = order_.points;
+ const cv::Vec2d* order = order_.points;
int i=1;
for (; i<MAX_ITER; ++i)
@@ -178,9 +179,9 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
epsilon_2 = k.dot(model.M02)/Z0;
// vector of scalar products <I0, M0i> and <J0, M0i>
- cv::Vec2f I0_M0i(order[1][0]*(1.0 + epsilon_1) - order[0][0],
+ cv::Vec2d I0_M0i(order[1][0]*(1.0 + epsilon_1) - order[0][0],
order[2][0]*(1.0 + epsilon_2) - order[0][0]);
- cv::Vec2f J0_M0i(order[1][1]*(1.0 + epsilon_1) - order[0][1],
+ cv::Vec2d J0_M0i(order[1][1]*(1.0 + epsilon_1) - order[0][1],
order[2][1]*(1.0 + epsilon_2) - order[0][1]);
// construct projection of I, J onto M0i plane: I0 and J0
@@ -190,20 +191,20 @@ 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;
+ double II0 = I0.dot(I0);
+ double IJ0 = I0.dot(J0);
+ double JJ0 = J0.dot(J0);
+ double rho, theta;
if (JJ0 == II0) {
- rho = std::sqrt(std::abs(2*IJ0));
- theta = -PI/4;
+ rho = std::sqrt(std::fabs(2*IJ0));
+ theta = -M_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 += (JJ0 - II0 < 0) * M_PI;
theta /= 2;
}
@@ -214,7 +215,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.0/cv::norm(I_1); // all have the same norm
+ double norm_const = 1/cv::norm(I_1); // all have the same norm
// create rotation matrices
I_1 *= norm_const; J_1 *= norm_const;
@@ -233,26 +234,29 @@ 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());
+ double R_1_deviation = cv::norm(cv::Matx33d::eye() - R_expected * R_1.t());
+ double R_2_deviation = cv::norm(cv::Matx33d::eye() - R_expected * R_2.t());
if (R_1_deviation < R_2_deviation)
- R_current = &R_1;
+ R_current = R_1;
else
- R_current = &R_2;
+ R_current = R_2;
- get_row(*R_current, 2, k);
+ 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 double 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;
}
QMutexLocker l(&mtx);
// apply results
- X_CM.R = *R_current;
+ X_CM.R = R_current;
X_CM.t[0] = order[0][0] * Z0/focal_length;
X_CM.t[1] = order[0][1] * Z0/focal_length;
X_CM.t[2] = Z0;
@@ -262,8 +266,8 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
return i;
}
-cv::Vec2f PointTracker::project(const cv::Vec3f& v_M, float f)
+cv::Vec2d PointTracker::project(const cv::Vec3d& v_M, double f)
{
- 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]);
+ cv::Vec3d v_C = X_CM * v_M;
+ return cv::Vec2d(f*v_C[0]/v_C[2], f*v_C[1]/v_C[2]);
}
generated by cgit v1.2.3 (git 2.39.1) at 2025-01-31 07:15:27 +0000