diff options
Diffstat (limited to 'FTNoIR_Tracker_PT/point_tracker.h')
| -rw-r--r-- | FTNoIR_Tracker_PT/point_tracker.h | 33 |
1 files changed, 27 insertions, 6 deletions
diff --git a/FTNoIR_Tracker_PT/point_tracker.h b/FTNoIR_Tracker_PT/point_tracker.h index 11034100..e05e8f98 100644 --- a/FTNoIR_Tracker_PT/point_tracker.h +++ b/FTNoIR_Tracker_PT/point_tracker.h @@ -90,7 +90,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
- bool track(const std::vector<cv::Vec2f>& points, float f, float dt);
+ bool track(const std::vector<cv::Vec2f>& points, float fov, float dt, int w, int h, const cv::Vec3f &headpos);
boost::shared_ptr<PointModel> point_model;
bool dynamic_pose_resolution;
@@ -98,15 +98,35 @@ public: FrameTrafo get_pose() const { return X_CM; }
void reset();
+ float fov;
+ int _w, _h;
protected:
- inline cv::Vec2f project(const cv::Vec3f& v_M, float f)
+ 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]);
+ if (!rvec.empty() && !tvec.empty() && fov > 0)
+ {
+ const float HT_PI = 3.1415926535;
+ const int w = _w, h = _h;
+ const float focal_length_w = 0.5 * w / tan(fov * HT_PI / 180);
+ const float focal_length_h = 0.5 * h / tan(fov * h / w * HT_PI / 180.0);
+
+ cv::Mat intrinsics = cv::Mat::eye(3, 3, CV_32FC1);
+ intrinsics.at<float> (0, 0) = focal_length_w;
+ intrinsics.at<float> (1, 1) = focal_length_h;
+ intrinsics.at<float> (0, 2) = w/2;
+ intrinsics.at<float> (1, 2) = h/2;
+ std::vector<cv::Point3f> xs;
+ xs.push_back(v_M);
+ cv::Mat dist_coeffs = cv::Mat::zeros(5, 1, CV_32FC1);
+ std::vector<cv::Point2f> rets(1);
+ cv::projectPoints(xs, rvec, tvec, intrinsics, dist_coeffs, rets);
+ return rets[0];
+ }
+ return cv::Vec2f();
}
- bool find_correspondences(const std::vector<cv::Vec2f>& points, float f);
+ bool find_correspondences(const std::vector<cv::Vec2f>& points);
cv::Vec2f p[PointModel::N_POINTS]; // the points in model order
cv::Vec2f p_exp[PointModel::N_POINTS]; // the expected point positions
@@ -116,7 +136,7 @@ protected: void reset_velocities();
- int POSIT(float f); // The POSIT algorithm, returns the number of iterations
+ void POSIT(float fov, int w, int h, const cv::Vec3f &headpos); // The POSIT algorithm, returns the number of iterations
bool init_phase;
float dt_valid; // time since last valid tracking result
@@ -124,6 +144,7 @@ protected: cv::Vec3f v_r;
FrameTrafo X_CM; // trafo from model to camera
FrameTrafo X_CM_old;
+ cv::Mat rvec, tvec;
};
#endif //POINTTRACKER_H
|