diff options
| -rw-r--r-- | tracker-pt/ftnoir_tracker_pt.cpp | 13 | ||||
| -rw-r--r-- | tracker-pt/point_tracker.cpp | 23 | ||||
| -rw-r--r-- | tracker-pt/point_tracker.h | 4 |
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 |