diff options
Diffstat (limited to 'FTNoIR_Tracker_PT/point_tracker.cpp')
| -rw-r--r-- | FTNoIR_Tracker_PT/point_tracker.cpp | 191 |
1 files changed, 146 insertions, 45 deletions
diff --git a/FTNoIR_Tracker_PT/point_tracker.cpp b/FTNoIR_Tracker_PT/point_tracker.cpp index 1df70b17..dfefdaf8 100644 --- a/FTNoIR_Tracker_PT/point_tracker.cpp +++ b/FTNoIR_Tracker_PT/point_tracker.cpp @@ -17,6 +17,23 @@ 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),
@@ -91,11 +108,9 @@ 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)
{
+ X_CM.t[2] = 1000; // default position: 1 m away from cam;
}
void PointTracker::reset()
@@ -113,7 +128,7 @@ void PointTracker::reset_velocities() }
-bool PointTracker::track(const vector<Vec2f>& points, float fov, float dt, int w, int h, const cv::Vec3f& headpos)
+bool PointTracker::track(const vector<Vec2f>& points, float f, float dt)
{
if (!dynamic_pose_resolution) init_phase = true;
@@ -125,7 +140,12 @@ bool PointTracker::track(const vector<Vec2f>& points, float fov, float dt, int w reset();
}
- bool no_model = !point_model;
+ bool no_model =
+#ifdef OPENTRACK_API
+ point_model.get() == NULL;
+#else
+ !point_model;
+#endif
// if there is a pointtracking problem, reset the velocities
if (no_model || points.size() != PointModel::N_POINTS)
@@ -141,7 +161,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, f))
{
//qDebug()<<"Error in finding point correspondences!";
X_CM = X_CM_old; // undo prediction
@@ -149,8 +169,7 @@ bool PointTracker::track(const vector<Vec2f>& points, float fov, float dt, int w return false;
}
- // XXX TODO fov
- POSIT(fov, w, h, headpos);
+ int n_iter = POSIT(f);
//qDebug()<<"Number of POSIT iterations: "<<n_iter;
if (!init_phase)
@@ -179,7 +198,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 f)
{
if (init_phase) {
// We do a simple freetrack-like sorting in the init phase...
@@ -196,9 +215,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), f);
+ p_exp[1] = project(point_model->M01, f);
+ p_exp[2] = project(point_model->M02, f);
// set correspondences by minimum distance to projected model point
bool point_taken[PointModel::N_POINTS];
@@ -232,48 +251,130 @@ bool PointTracker::find_correspondences(const vector<Vec2f>& points) -void PointTracker::POSIT(float fov, int w, int h, const cv::Vec3f& headpos)
+int PointTracker::POSIT(float f)
{
- // XXX hack
- this->fov = fov;
- _w = w;
- _h = h;
- std::vector<cv::Point3f> obj_points;
- std::vector<cv::Point2f> img_points;
+ // 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;
+ }
- obj_points.push_back(headpos);
- obj_points.push_back(point_model->M01 + headpos);
- obj_points.push_back(point_model->M02 + headpos);
+ // construct the two solutions
+ I_1 = I0 + rho*cos(theta)*point_model->u;
+ I_2 = I0 - rho*cos(theta)*point_model->u;
- img_points.push_back(p[0]);
- img_points.push_back(p[1]);
- img_points.push_back(p[2]);
+ J_1 = J0 + rho*sin(theta)*point_model->u;
+ J_2 = J0 - rho*sin(theta)*point_model->u;
- const float HT_PI = 3.1415926535;
+ 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 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);
+ 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));
- 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;
+ // the single translation solution
+ Z0 = norm_const * f;
- cv::Mat dist_coeffs = cv::Mat::zeros(5, 1, CV_32FC1);
+ // 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());
- bool lastp = !rvec.empty() && !tvec.empty() && !init_phase;
+ if (R_1_deviation < R_2_deviation)
+ R_current = &R_1;
+ else
+ R_current = &R_2;
- cv::solvePnP(obj_points, img_points, intrinsics, dist_coeffs, rvec, tvec, lastp, cv::ITERATIVE);
+ get_row(*R_current, 2, k);
- cv::Mat rmat;
- cv::Rodrigues(rvec, rmat);
+ // 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;
+ }
// apply results
- 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);
- }
+ 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';
}
|