diff options
Diffstat (limited to 'ftnoir_tracker_pt/point_tracker.h')
| -rw-r--r-- | ftnoir_tracker_pt/point_tracker.h | 25 |
1 files changed, 13 insertions, 12 deletions
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 <memory> #endif -#include <list> +#include <vector> // ---------------------------------------------------------------------------- // 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<cv::Vec2f>& points, float f); - std::shared_ptr<PointModel> point_model; - + void track(const std::vector<cv::Vec2f>& 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<cv::Vec2f>& points, float f); - int POSIT(float f); // The POSIT algorithm, returns the number of iterations + PointOrder find_correspondences(const std::vector<cv::Vec2f>& 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 }; |