From d74b99391bbdfb25f9559834082ae7ee6d30720d Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Tue, 23 Sep 2014 02:12:20 +0200 Subject: decruft PT more, so it doesn't crash finally --- ftnoir_tracker_pt/point_tracker.h | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) (limited to 'ftnoir_tracker_pt/point_tracker.h') diff --git a/ftnoir_tracker_pt/point_tracker.h b/ftnoir_tracker_pt/point_tracker.h index a1f6f041..0339f392 100644 --- a/ftnoir_tracker_pt/point_tracker.h +++ b/ftnoir_tracker_pt/point_tracker.h @@ -14,7 +14,7 @@ #else # include #endif -#include +#include // ---------------------------------------------------------------------------- // Affine frame trafo @@ -60,9 +60,10 @@ public: static constexpr int N_POINTS = 3; PointModel(cv::Vec3f M01, cv::Vec3f M02); + PointModel(); - const cv::Vec3f& get_M01() const { return M01; } - const cv::Vec3f& get_M02() const { return M02; } + inline const cv::Vec3f& get_M01() const { return M01; } + inline const cv::Vec3f& get_M02() const { return M02; } private: cv::Vec3f M01; // M01 in model frame @@ -86,27 +87,27 @@ class PointTracker { 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& points, float f); - std::shared_ptr point_model; - + void track(const std::vector& projected_points, const PointModel& model); FrameTrafo get_pose() const { return X_CM; } void reset(); private: - inline cv::Vec2f project(const cv::Vec3f& v_M, float f) + // the points in model order + typedef struct { cv::Vec2f points[PointModel::N_POINTS]; } PointOrder; + static constexpr float focal_length = 1.0f; + + inline cv::Vec2f project(const cv::Vec3f& v_M) { 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]); + return cv::Vec2f(focal_length*v_C[0]/v_C[2], focal_length*v_C[1]/v_C[2]); } - void find_correspondences(const std::vector& points, float f); - int POSIT(float f); // The POSIT algorithm, returns the number of iterations + PointOrder find_correspondences(const std::vector& projected_points, const PointModel &model); + int POSIT(const PointModel& point_model, const PointOrder& order); // The POSIT algorithm, returns the number of iterations - cv::Vec2f p[PointModel::N_POINTS]; // the points in model order FrameTrafo X_CM; // trafo from model to camera }; -- cgit v1.2.3