diff options
Diffstat (limited to 'FTNoIR_Tracker_PT')
| -rw-r--r-- | FTNoIR_Tracker_PT/point_tracker.cpp | 19 | ||||
| -rw-r--r-- | FTNoIR_Tracker_PT/point_tracker.h | 8 |
2 files changed, 8 insertions, 19 deletions
diff --git a/FTNoIR_Tracker_PT/point_tracker.cpp b/FTNoIR_Tracker_PT/point_tracker.cpp index cce327e9..a881da62 100644 --- a/FTNoIR_Tracker_PT/point_tracker.cpp +++ b/FTNoIR_Tracker_PT/point_tracker.cpp @@ -91,10 +91,7 @@ PointTracker::PointTracker() dt_reset(1),
v_t(0,0,0),
v_r(0,0,0),
- dynamic_pose_resolution(true),
- fov(0),
- _w(0),
- _h(0)
+ dynamic_pose_resolution(true)
{
}
@@ -144,7 +141,7 @@ bool PointTracker::track(const vector<Vec2f>& points, float fov, float dt, int w predict(dt_valid);
// if there is a point correspondence problem something has gone wrong, do a reset
- if (!find_correspondences(points))
+ if (!find_correspondences(points, fov, w, h))
{
//qDebug()<<"Error in finding point correspondences!";
X_CM = X_CM_old; // undo prediction
@@ -181,7 +178,7 @@ void PointTracker::update_velocities(float dt) v_t = (X_CM.t - X_CM_old.t)/dt;
}
-bool PointTracker::find_correspondences(const vector<Vec2f>& points)
+bool PointTracker::find_correspondences(const vector<Vec2f>& points, float fov, int w, int h)
{
if (init_phase) {
// We do a simple freetrack-like sorting in the init phase...
@@ -198,9 +195,9 @@ bool PointTracker::find_correspondences(const vector<Vec2f>& points) else {
// ... otherwise we look at the distance to the projection of the expected model points
// project model points under current pose
- p_exp[0] = project(Vec3f(0,0,0));
- p_exp[1] = project(point_model->M01);
- p_exp[2] = project(point_model->M02);
+ p_exp[0] = project(Vec3f(0,0,0), fov, w, h);
+ p_exp[1] = project(point_model->M01, fov, w, h);
+ p_exp[2] = project(point_model->M02, fov, w, h);
// set correspondences by minimum distance to projected model point
bool point_taken[PointModel::N_POINTS];
@@ -236,10 +233,6 @@ bool PointTracker::find_correspondences(const vector<Vec2f>& points) void PointTracker::POSIT(float fov, int w, int h)
{
- // XXX hack
- this->fov = fov;
- _w = w;
- _h = h;
std::vector<cv::Point3f> obj_points;
std::vector<cv::Point2f> img_points;
diff --git a/FTNoIR_Tracker_PT/point_tracker.h b/FTNoIR_Tracker_PT/point_tracker.h index ac43489e..741d5af4 100644 --- a/FTNoIR_Tracker_PT/point_tracker.h +++ b/FTNoIR_Tracker_PT/point_tracker.h @@ -98,16 +98,12 @@ public: FrameTrafo get_pose() const { return X_CM; }
void reset();
- float fov;
- int _w, _h;
-
protected:
- cv::Vec2f project(const cv::Vec3f& v_M)
+ cv::Vec2f project(const cv::Vec3f& v_M, float fov, int w, int h)
{
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);
@@ -126,7 +122,7 @@ protected: return cv::Vec2f();
}
- bool find_correspondences(const std::vector<cv::Vec2f>& points);
+ bool find_correspondences(const std::vector<cv::Vec2f>& points, float fov, int w, int h);
cv::Vec2f p[PointModel::N_POINTS]; // the points in model order
cv::Vec2f p_exp[PointModel::N_POINTS]; // the expected point positions
|