summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--tracker-pt/ftnoir_tracker_pt.cpp13
-rw-r--r--tracker-pt/point_tracker.cpp23
-rw-r--r--tracker-pt/point_tracker.h4
3 files changed, 34 insertions, 6 deletions
diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp
index 374120f4..49159ac8 100644
--- a/tracker-pt/ftnoir_tracker_pt.cpp
+++ b/tracker-pt/ftnoir_tracker_pt.cpp
@@ -103,6 +103,11 @@ void Tracker_PT::run()
if (new_frame && !frame_.empty())
{
+ CamInfo cam_info;
+
+ if (!camera.get_info(cam_info))
+ continue;
+
point_extractor.extract_points(frame_, points);
point_count = points.size();
@@ -115,7 +120,13 @@ void Tracker_PT::run()
if (success)
{
- point_tracker.track(points, PointModel(s), fx, s.dynamic_pose, s.init_phase_timeout);
+ point_tracker.track(points,
+ PointModel(s),
+ fx,
+ s.dynamic_pose,
+ s.init_phase_timeout,
+ cam_info.res_x,
+ cam_info.res_y);
ever_success = true;
}
diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp
index 35a78777..d5948c63 100644
--- a/tracker-pt/point_tracker.cpp
+++ b/tracker-pt/point_tracker.cpp
@@ -92,13 +92,21 @@ PointTracker::PointTracker() : init_phase(true)
{
}
-PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::vector<vec2>& points, const PointModel& model, f focal_length)
+PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::vector<vec2>& points,
+ const PointModel& model,
+ f focal_length,
+ int w,
+ int h)
{
PointTracker::PointOrder p;
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);
+ const int diagonal = int(std::sqrt(w*w + h*h));
+ constexpr int div = 50;
+ const int max_dist = diagonal / div; // 16 pixels for 640x480
+
// set correspondences by minimum distance to projected model point
bool point_taken[PointModel::N_POINTS];
for (unsigned i=0; i<PointModel::N_POINTS; ++i)
@@ -119,6 +127,9 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::
min_sdist = sdist;
}
}
+ if (min_sdist > max_dist)
+ return find_correspondences(points, model);
+
// if one point is closest to more than one model point, fallback
if (point_taken[min_idx])
{
@@ -132,7 +143,13 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::
return p;
}
-void PointTracker::track(const std::vector<vec2>& points, const PointModel& model, f focal_length, 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,
+ int w,
+ int h)
{
PointOrder order;
@@ -146,7 +163,7 @@ void PointTracker::track(const std::vector<vec2>& points, const PointModel& mode
order = find_correspondences(points, model);
else
{
- order = find_correspondences_previous(points, model, focal_length);
+ order = find_correspondences_previous(points, model, focal_length, w, h);
}
POSIT(model, order, focal_length);
diff --git a/tracker-pt/point_tracker.h b/tracker-pt/point_tracker.h
index fab0e9ac..00e371f5 100644
--- a/tracker-pt/point_tracker.h
+++ b/tracker-pt/point_tracker.h
@@ -80,7 +80,7 @@ public:
// 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);
+ 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, PointTracker::f focal_length);
@@ -97,7 +97,7 @@ private:
};
PointOrder find_correspondences(const std::vector<vec2>& projected_points, const PointModel &model);
- PointOrder find_correspondences_previous(const std::vector<vec2>& points, const PointModel &model, f focal_length);
+ PointOrder find_correspondences_previous(const std::vector<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