summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
Diffstat
-rw-r--r--FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui13
-rw-r--r--FTNoIR_Tracker_PT/camera.cpp8
-rw-r--r--FTNoIR_Tracker_PT/camera.h6
-rw-r--r--FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp4
-rw-r--r--FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp2
-rw-r--r--FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.h4
-rw-r--r--FTNoIR_Tracker_PT/point_tracker.cpp191
-rw-r--r--FTNoIR_Tracker_PT/point_tracker.h33
8 files changed, 92 insertions, 169 deletions
diff --git a/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui b/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui
index bdbed955..461253cf 100644
--- a/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui
+++ b/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui
@@ -515,23 +515,26 @@
<item row="1" column="0">
<widget class="QLabel" name="label_34">
<property name="text">
- <string>F/W</string>
+ <string>FOV</string>
</property>
<property name="buddy">
- <cstring>f_dspin</cstring>
+ <cstring>fov_dspin</cstring>
</property>
</widget>
</item>
<item row="1" column="1">
- <widget class="QDoubleSpinBox" name="f_dspin">
+ <widget class="QDoubleSpinBox" name="fov_dspin">
<property name="toolTip">
<string>The camera's focal length devided by its sensor width</string>
</property>
<property name="decimals">
<number>2</number>
</property>
+ <property name="minimum">
+ <double>1.000000000000000</double>
+ </property>
<property name="singleStep">
- <double>0.100000000000000</double>
+ <double>1.000000000000000</double>
</property>
</widget>
</item>
@@ -1733,7 +1736,7 @@
<tabstop>res_x_spin</tabstop>
<tabstop>res_y_spin</tabstop>
<tabstop>fps_spin</tabstop>
- <tabstop>f_dspin</tabstop>
+ <tabstop>fov_dspin</tabstop>
<tabstop>camroll_combo</tabstop>
<tabstop>campitch_spin</tabstop>
<tabstop>camyaw_spin</tabstop>
diff --git a/FTNoIR_Tracker_PT/camera.cpp b/FTNoIR_Tracker_PT/camera.cpp
index 754533c5..8986be60 100644
--- a/FTNoIR_Tracker_PT/camera.cpp
+++ b/FTNoIR_Tracker_PT/camera.cpp
@@ -108,11 +108,11 @@ void Camera::set_device_index(int index)
}
}
-void Camera::set_f(float f)
+void Camera::set_fov(float f)
{
- if (cam_desired.f != f)
+ if (cam_desired.fov != f)
{
- cam_desired.f = f;
+ cam_desired.fov = f;
_set_f();
}
}
@@ -208,7 +208,7 @@ void CVCamera::_set_index()
void CVCamera::_set_f()
{
- cam_info.f = cam_desired.f;
+ cam_info.fov = cam_desired.fov;
}
void CVCamera::_set_fps()
diff --git a/FTNoIR_Tracker_PT/camera.h b/FTNoIR_Tracker_PT/camera.h
index ea68c387..6768e419 100644
--- a/FTNoIR_Tracker_PT/camera.h
+++ b/FTNoIR_Tracker_PT/camera.h
@@ -25,12 +25,12 @@ void get_camera_device_names(std::vector<std::string>& device_names);
// ----------------------------------------------------------------------------
struct CamInfo
{
- CamInfo() : res_x(0), res_y(0), fps(0), f(1) {}
+ CamInfo() : res_x(0), res_y(0), fps(0), fov(56) {}
int res_x;
int res_y;
int fps;
- float f; // (focal length) / (sensor width)
+ float fov;
};
// ----------------------------------------------------------------------------
@@ -48,7 +48,7 @@ public:
// calls corresponding template methods and reinitializes frame rate calculation
void set_device_index(int index);
- void set_f(float f);
+ void set_fov(float f);
void set_fps(int fps);
void set_res(int x_res, int y_res);
diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp
index 6bcad861..a3e8919b 100644
--- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp
+++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp
@@ -84,7 +84,7 @@ void Tracker::run()
{
frame = frame_rotation.rotate_frame(frame);
const std::vector<cv::Vec2f>& points = point_extractor.extract_points(frame, dt, false);
- tracking_valid = point_tracker.track(points, camera.get_info().f, dt);
+ tracking_valid = point_tracker.track(points, camera.get_info().fov, dt, frame.cols, frame.rows, t_MH);
video_widget->update_image(frame);
}
#ifdef PT_PERF_LOG
@@ -104,7 +104,7 @@ void Tracker::apply(settings& s)
camera.set_device_index(s.cam_index);
camera.set_res(s.cam_res_x, s.cam_res_y);
camera.set_fps(s.cam_fps);
- camera.set_f(s.cam_f);
+ camera.set_fov(s.cam_fov);
frame_rotation.rotation = static_cast<RotationType>(static_cast<int>(s.cam_roll));
point_extractor.threshold_val = s.threshold;
point_extractor.threshold_secondary_val = s.threshold_secondary;
diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp
index c103b78c..4ae20f48 100644
--- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp
+++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp
@@ -46,7 +46,7 @@ TrackerDialog::TrackerDialog()
tie_setting(s.reset_time, ui.reset_spin);
tie_setting(s.cam_index, ui.camdevice_combo);
- tie_setting(s.cam_f, ui.f_dspin);
+ tie_setting(s.cam_fov, ui.fov_dspin);
tie_setting(s.cam_res_x, ui.res_x_spin);
tie_setting(s.cam_res_y, ui.res_y_spin);
tie_setting(s.cam_fps, ui.fps_spin);
diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.h b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.h
index 564e1264..e0dfa2e6 100644
--- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.h
+++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.h
@@ -28,7 +28,7 @@ struct settings
threshold_secondary,
min_point_size,
max_point_size;
- value<double> cam_f;
+ value<double> cam_fov;
value<int> m01_x, m01_y, m01_z;
value<int> m02_x, m02_y, m02_z;
@@ -57,7 +57,7 @@ struct settings
threshold_secondary(b, "threshold-secondary", 128),
min_point_size(b, "min-point-size", 10),
max_point_size(b, "max-point-size", 50),
- cam_f(b, "camera-focal-length", 1),
+ cam_fov(b, "camera-fov", 56),
m01_x(b, "m_01-x", 0),
m01_y(b, "m_01-y", 0),
m01_z(b, "m_01-z", 0),
diff --git a/FTNoIR_Tracker_PT/point_tracker.cpp b/FTNoIR_Tracker_PT/point_tracker.cpp
index dfefdaf8..1df70b17 100644
--- a/FTNoIR_Tracker_PT/point_tracker.cpp
+++ b/FTNoIR_Tracker_PT/point_tracker.cpp
@@ -17,23 +17,6 @@ using namespace cv;
using namespace boost;
using namespace std;
-const float PI = 3.14159265358979323846f;
-
-// ----------------------------------------------------------------------------
-static void get_row(const Matx33f& m, int i, Vec3f& v)
-{
- v[0] = m(i,0);
- v[1] = m(i,1);
- v[2] = m(i,2);
-}
-
-static void set_row(Matx33f& m, int i, const Vec3f& v)
-{
- m(i,0) = v[0];
- m(i,1) = v[1];
- m(i,2) = v[2];
-}
-
// ----------------------------------------------------------------------------
PointModel::PointModel(Vec3f M01, Vec3f M02)
: M01(M01),
@@ -108,9 +91,11 @@ PointTracker::PointTracker()
dt_reset(1),
v_t(0,0,0),
v_r(0,0,0),
- dynamic_pose_resolution(true)
+ dynamic_pose_resolution(true),
+ fov(0),
+ _w(0),
+ _h(0)
{
- X_CM.t[2] = 1000; // default position: 1 m away from cam;
}
void PointTracker::reset()
@@ -128,7 +113,7 @@ void PointTracker::reset_velocities()
}
-bool PointTracker::track(const vector<Vec2f>& points, float f, float dt)
+bool PointTracker::track(const vector<Vec2f>& points, float fov, float dt, int w, int h, const cv::Vec3f& headpos)
{
if (!dynamic_pose_resolution) init_phase = true;
@@ -140,12 +125,7 @@ bool PointTracker::track(const vector<Vec2f>& points, float f, float dt)
reset();
}
- bool no_model =
-#ifdef OPENTRACK_API
- point_model.get() == NULL;
-#else
- !point_model;
-#endif
+ bool no_model = !point_model;
// if there is a pointtracking problem, reset the velocities
if (no_model || points.size() != PointModel::N_POINTS)
@@ -161,7 +141,7 @@ bool PointTracker::track(const vector<Vec2f>& points, float f, float dt)
predict(dt_valid);
// if there is a point correspondence problem something has gone wrong, do a reset
- if (!find_correspondences(points, f))
+ if (!find_correspondences(points))
{
//qDebug()<<"Error in finding point correspondences!";
X_CM = X_CM_old; // undo prediction
@@ -169,7 +149,8 @@ bool PointTracker::track(const vector<Vec2f>& points, float f, float dt)
return false;
}
- int n_iter = POSIT(f);
+ // XXX TODO fov
+ POSIT(fov, w, h, headpos);
//qDebug()<<"Number of POSIT iterations: "<<n_iter;
if (!init_phase)
@@ -198,7 +179,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, float f)
+bool PointTracker::find_correspondences(const vector<Vec2f>& points)
{
if (init_phase) {
// We do a simple freetrack-like sorting in the init phase...
@@ -215,9 +196,9 @@ bool PointTracker::find_correspondences(const vector<Vec2f>& points, float f)
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), f);
- p_exp[1] = project(point_model->M01, f);
- p_exp[2] = project(point_model->M02, f);
+ p_exp[0] = project(Vec3f(0,0,0));
+ p_exp[1] = project(point_model->M01);
+ p_exp[2] = project(point_model->M02);
// set correspondences by minimum distance to projected model point
bool point_taken[PointModel::N_POINTS];
@@ -251,130 +232,48 @@ bool PointTracker::find_correspondences(const vector<Vec2f>& points, float f)
-int PointTracker::POSIT(float f)
+void PointTracker::POSIT(float fov, int w, int h, const cv::Vec3f& headpos)
{
- // POSIT algorithm for coplanar points as presented in
- // [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"]
- // we use the same notation as in the paper here
-
- // The expected rotation used for resolving the ambiguity in POSIT:
- // In every iteration step the rotation closer to R_expected is taken
- Matx33f R_expected;
- if (init_phase)
- R_expected = Matx33f::eye(); // in the init phase, we want to be close to the default pose = no rotation
- else
- R_expected = X_CM.R; // later we want to be close to the last (predicted) rotation
-
- // initial pose = last (predicted) pose
- Vec3f k;
- get_row(R_expected, 2, k);
- float Z0 = init_phase ? 1000 : X_CM.t[2];
-
- float old_epsilon_1 = 0;
- float old_epsilon_2 = 0;
- float epsilon_1 = 1;
- float epsilon_2 = 1;
-
- Vec3f I0, J0;
- Vec2f I0_coeff, J0_coeff;
-
- Vec3f I_1, J_1, I_2, J_2;
- Matx33f R_1, R_2;
- Matx33f* R_current;
-
- const int MAX_ITER = 100;
- const float EPS_THRESHOLD = 1e-4;
-
- int i=1;
- for (; i<MAX_ITER; ++i)
- {
- epsilon_1 = k.dot(point_model->M01)/Z0;
- epsilon_2 = k.dot(point_model->M02)/Z0;
-
- // vector of scalar products <I0, M0i> and <J0, M0i>
- Vec2f I0_M0i(p[1][0]*(1.0 + epsilon_1) - p[0][0],
- p[2][0]*(1.0 + epsilon_2) - p[0][0]);
- Vec2f J0_M0i(p[1][1]*(1.0 + epsilon_1) - p[0][1],
- p[2][1]*(1.0 + epsilon_2) - p[0][1]);
-
- // construct projection of I, J onto M0i plane: I0 and J0
- I0_coeff = point_model->P * I0_M0i;
- J0_coeff = point_model->P * J0_M0i;
- I0 = I0_coeff[0]*point_model->M01 + I0_coeff[1]*point_model->M02;
- J0 = J0_coeff[0]*point_model->M01 + J0_coeff[1]*point_model->M02;
-
- // calculate u component of I, J
- float II0 = I0.dot(I0);
- float IJ0 = I0.dot(J0);
- float JJ0 = J0.dot(J0);
- float rho, theta;
- if (JJ0 == II0) {
- rho = sqrt(abs(2*IJ0));
- theta = -PI/4;
- if (IJ0<0) theta *= -1;
- }
- else {
- rho = sqrt(sqrt( (JJ0-II0)*(JJ0-II0) + 4*IJ0*IJ0 ));
- theta = atan( -2*IJ0 / (JJ0-II0) );
- if (JJ0 - II0 < 0) theta += PI;
- theta /= 2;
- }
+ // XXX hack
+ this->fov = fov;
+ _w = w;
+ _h = h;
+ std::vector<cv::Point3f> obj_points;
+ std::vector<cv::Point2f> img_points;
- // construct the two solutions
- I_1 = I0 + rho*cos(theta)*point_model->u;
- I_2 = I0 - rho*cos(theta)*point_model->u;
+ obj_points.push_back(headpos);
+ obj_points.push_back(point_model->M01 + headpos);
+ obj_points.push_back(point_model->M02 + headpos);
- J_1 = J0 + rho*sin(theta)*point_model->u;
- J_2 = J0 - rho*sin(theta)*point_model->u;
+ img_points.push_back(p[0]);
+ img_points.push_back(p[1]);
+ img_points.push_back(p[2]);
- float norm_const = 1.0/norm(I_1); // all have the same norm
-
- // create rotation matrices
- I_1 *= norm_const; J_1 *= norm_const;
- I_2 *= norm_const; J_2 *= norm_const;
+ const float HT_PI = 3.1415926535;
- set_row(R_1, 0, I_1);
- set_row(R_1, 1, J_1);
- set_row(R_1, 2, I_1.cross(J_1));
-
- set_row(R_2, 0, I_2);
- set_row(R_2, 1, J_2);
- set_row(R_2, 2, I_2.cross(J_2));
+ 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);
- // the single translation solution
- Z0 = norm_const * f;
+ 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;
- // pick the rotation solution closer to the expected one
- // in simple metric d(A,B) = || I - A * B^T ||
- float R_1_deviation = norm(Matx33f::eye() - R_expected * R_1.t());
- float R_2_deviation = norm(Matx33f::eye() - R_expected * R_2.t());
+ cv::Mat dist_coeffs = cv::Mat::zeros(5, 1, CV_32FC1);
- if (R_1_deviation < R_2_deviation)
- R_current = &R_1;
- else
- R_current = &R_2;
+ bool lastp = !rvec.empty() && !tvec.empty() && !init_phase;
- get_row(*R_current, 2, k);
+ cv::solvePnP(obj_points, img_points, intrinsics, dist_coeffs, rvec, tvec, lastp, cv::ITERATIVE);
- // check for convergence condition
- if (abs(epsilon_1 - old_epsilon_1) + abs(epsilon_2 - old_epsilon_2) < EPS_THRESHOLD)
- break;
- old_epsilon_1 = epsilon_1;
- old_epsilon_2 = epsilon_2;
- }
+ cv::Mat rmat;
+ cv::Rodrigues(rvec, rmat);
// apply results
- X_CM.R = *R_current;
- X_CM.t[0] = p[0][0] * Z0/f;
- X_CM.t[1] = p[0][1] * Z0/f;
- X_CM.t[2] = Z0;
-
- return i;
-
- //Rodrigues(X_CM.R, r);
- //qDebug()<<"iter: "<<i;
- //qDebug()<<"t: "<<X_CM.t[0]<<' '<<X_CM.t[1]<<' '<<X_CM.t[2];
- //Vec3f r;
- //
- //qDebug()<<"r: "<<r[0]<<' '<<r[1]<<' '<<r[2]<<'\n';
+ for (int i = 0; i < 3; i++)
+ {
+ X_CM.t[i] = tvec.at<double>(i);
+ for (int j = 0; j < 3; j++)
+ X_CM.R(i, j) = rmat.at<double>(i, j);
+ }
}
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
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-17 20:40:18 +0000