summaryrefslogtreecommitdiffhomepage
path: root/ftnoir_tracker_pt
diff options
context:
space:
mode:
Diffstat (limited to 'ftnoir_tracker_pt')
-rw-r--r--ftnoir_tracker_pt/ftnoir_tracker_pt.cpp72
-rw-r--r--ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h6
-rw-r--r--ftnoir_tracker_pt/point_extractor.cpp25
-rw-r--r--ftnoir_tracker_pt/point_tracker.cpp309
-rw-r--r--ftnoir_tracker_pt/point_tracker.h4
-rw-r--r--ftnoir_tracker_pt/pt_video_widget.cpp1
6 files changed, 226 insertions, 191 deletions
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index 7b70d4eb..9a5f11c4 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -1,4 +1,5 @@
/* Copyright (c) 2012 Patrick Ruoff
+ * Copyright (c) 2014-2015 Stanislaw Halik <sthalik@misaki.pl>
*
* Permission to use, copy, modify, and/or distribute this software for any
* purpose with or without fee is hereby granted, provided that the above
@@ -19,8 +20,8 @@
Tracker_PT::Tracker_PT()
: mutex(QMutex::Recursive),
commands(0),
- video_widget(NULL),
- video_frame(NULL),
+ video_widget(NULL),
+ video_frame(NULL),
ever_success(false)
{
connect(s.b.get(), SIGNAL(saving()), this, SLOT(apply_settings()));
@@ -28,8 +29,8 @@ Tracker_PT::Tracker_PT()
Tracker_PT::~Tracker_PT()
{
- set_command(ABORT);
- wait();
+ set_command(ABORT);
+ wait();
delete video_widget;
video_widget = NULL;
if (video_frame->layout()) delete video_frame->layout();
@@ -39,13 +40,13 @@ Tracker_PT::~Tracker_PT()
void Tracker_PT::set_command(Command command)
{
//QMutexLocker lock(&mutex);
- commands |= command;
+ commands |= command;
}
void Tracker_PT::reset_command(Command command)
{
//QMutexLocker lock(&mutex);
- commands &= ~command;
+ commands &= ~command;
}
bool Tracker_PT::get_focal_length(float& ret)
@@ -78,16 +79,21 @@ bool Tracker_PT::get_focal_length(float& ret)
return false;
}
+static inline bool nanp(double value)
+{
+ return std::isnan(value) || std::isinf(value);
+}
+
void Tracker_PT::run()
{
#ifdef PT_PERF_LOG
- QFile log_file(QCoreApplication::applicationDirPath() + "/PointTrackerPerformance.txt");
- if (!log_file.open(QIODevice::WriteOnly | QIODevice::Text)) return;
- QTextStream log_stream(&log_file);
+ QFile log_file(QCoreApplication::applicationDirPath() + "/PointTrackerPerformance.txt");
+ if (!log_file.open(QIODevice::WriteOnly | QIODevice::Text)) return;
+ QTextStream log_stream(&log_file);
#endif
apply_settings();
-
+
while((commands & ABORT) == 0)
{
const double dt = time.elapsed() * 1e-9;
@@ -109,22 +115,46 @@ void Tracker_PT::run()
// blobs are sorted in order of circularity
if (points.size() > PointModel::N_POINTS)
points.resize(PointModel::N_POINTS);
-
+
bool success = points.size() == PointModel::N_POINTS;
-
- ever_success |= success;
float fx;
if (!get_focal_length(fx))
continue;
+
+ Affine X_CM_ = pose();
if (success)
{
point_tracker.track(points, PointModel(s), fx, s.dynamic_pose, s.init_phase_timeout);
}
+ Affine X_CM = pose();
+
+ {
+ int j = 0;
+
+ for (int i = 0; i < 3; i++)
+ {
+ if (nanp(X_CM.t(i)))
+ goto nannan;
+ for (; j < 3; j++)
+ if (nanp(X_CM.R(i, j)))
+ {
+nannan: success = false;
+ X_CM = X_CM_;
+ {
+ QMutexLocker lock(&mutex);
+ point_tracker.reset(X_CM_);
+ }
+ goto nannannan;
+ }
+ }
+ }
+
+nannannan: ever_success |= success;
+
{
- Affine X_CM = pose();
Affine X_MH(cv::Matx33f::eye(), cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z)); // just copy pasted these lines from below
if (X_MH.t[0] == 0 && X_MH.t[1] == 0 && X_MH.t[2] == 0)
{
@@ -145,7 +175,7 @@ void Tracker_PT::run()
cv::Vec2f p_(p[0] / p[2] * fx, p[1] / p[2] * fx); // projected to screen
points.push_back(p_);
}
-
+
for (unsigned i = 0; i < points.size(); i++)
{
auto& p = points[i];
@@ -164,7 +194,7 @@ void Tracker_PT::run()
color,
4);
}
-
+
video_widget->update_image(frame);
}
#ifdef PT_PERF_LOG
@@ -233,26 +263,26 @@ void Tracker_PT::data(double *data)
if (ever_success)
{
Affine X_CM = pose();
-
+
Affine X_MH(cv::Matx33f::eye(), cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z));
Affine X_GH = X_CM * X_MH;
-
+
cv::Matx33f R = X_GH.R;
cv::Vec3f t = X_GH.t;
-
+
// translate rotation matrix from opengl (G) to roll-pitch-yaw (E) frame
// -z -> x, y -> z, x -> -y
cv::Matx33f R_EG(0, 0,-1,
-1, 0, 0,
0, 1, 0);
R = R_EG * R * R_EG.t();
-
+
// extract rotation angles
float alpha, beta, gamma;
beta = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );
alpha = atan2( R(1,0), R(0,0));
gamma = atan2( R(2,1), R(2,2));
-
+
// extract rotation angles
data[Yaw] = rad2deg * alpha;
data[Pitch] = -rad2deg * beta;
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h
index 0bfc05f7..78626468 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h
@@ -1,4 +1,5 @@
/* Copyright (c) 2012 Patrick Ruoff
+ * Copyright (c) 2014-2015 Stanislaw Halik <sthalik@misaki.pl>
*
* Permission to use, copy, modify, and/or distribute this software for any
* purpose with or without fee is hereby granted, provided that the above
@@ -13,9 +14,8 @@ using namespace options;
struct settings_pt : opts
{
- value<int> threshold,
- min_point_size,
- max_point_size;
+ value<int> threshold;
+ value<double> min_point_size, max_point_size;
value<int> t_MH_x, t_MH_y, t_MH_z;
value<int> fov, camera_mode;
diff --git a/ftnoir_tracker_pt/point_extractor.cpp b/ftnoir_tracker_pt/point_extractor.cpp
index 0ac2fc32..ec37dd00 100644
--- a/ftnoir_tracker_pt/point_extractor.cpp
+++ b/ftnoir_tracker_pt/point_extractor.cpp
@@ -1,4 +1,5 @@
/* Copyright (c) 2012 Patrick Ruoff
+ * Copyright (c) 2014-2015 Stanislaw Halik <sthalik@misaki.pl>
*
* Permission to use, copy, modify, and/or distribute this software for any
* purpose with or without fee is hereby granted, provided that the above
@@ -28,21 +29,21 @@ std::vector<cv::Vec2f> PointExtractor::extract_points(cv::Mat& frame)
cv::Mat frame_gray;
cv::cvtColor(frame, frame_gray, cv::COLOR_RGB2GRAY);
- const int region_size_min = s.min_point_size;
- const int region_size_max = s.max_point_size;
+ const double region_size_min = s.min_point_size;
+ const double region_size_max = s.max_point_size;
- struct simple_blob
+ struct blob
{
double radius;
cv::Vec2d pos;
double confid;
bool taken;
double area;
- simple_blob(double radius, const cv::Vec2d& pos, double confid, double area) : radius(radius), pos(pos), confid(confid), taken(false), area(area)
+ blob(double radius, const cv::Vec2d& pos, double confid, double area) : radius(radius), pos(pos), confid(confid), taken(false), area(area)
{
//qDebug() << "radius" << radius << "pos" << pos[0] << pos[1] << "confid" << confid;
}
- bool inside(const simple_blob& other)
+ bool inside(const blob& other)
{
cv::Vec2d tmp = pos - other.pos;
return sqrt(tmp.dot(tmp)) < radius;
@@ -52,7 +53,7 @@ std::vector<cv::Vec2f> PointExtractor::extract_points(cv::Mat& frame)
// mask for everything that passes the threshold (or: the upper threshold of the hysteresis)
cv::Mat frame_bin = cv::Mat::zeros(H, W, CV_8U);
- std::vector<simple_blob> blobs;
+ std::vector<blob> blobs;
std::vector<std::vector<cv::Point>> contours;
const int thres = s.threshold;
@@ -76,8 +77,8 @@ std::vector<cv::Vec2f> PointExtractor::extract_points(cv::Mat& frame)
const int sz = hist.rows*hist.cols;
int val = 0;
int cnt = 0;
- constexpr int min_pixels = 2000;
- const int pixels_to_include = std::max(0, static_cast<int>(min_pixels * (1. - s.threshold / 100.)));
+ constexpr int min_pixels = 250;
+ const auto pixels_to_include = std::max<int>(0, min_pixels * s.threshold/100.);
for (int i = sz-1; i >= 0; i--)
{
cnt += hist.at<float>(i);
@@ -87,8 +88,8 @@ std::vector<cv::Vec2f> PointExtractor::extract_points(cv::Mat& frame)
break;
}
}
- val *= .95;
- //qDebug() << "cnt" << cnt << "val" << val;
+ val *= 240./256.;
+ //qDebug() << "val" << val;
cv::Mat frame_bin_;
cv::threshold(frame_gray, frame_bin_, val, 255, CV_THRESH_BINARY);
@@ -148,13 +149,13 @@ std::vector<cv::Vec2f> PointExtractor::extract_points(cv::Mat& frame)
cv::putText(frame, buf, cv::Point(pos[0]+30, pos[1]+20), cv::FONT_HERSHEY_DUPLEX, 1, cv::Scalar(0, 0, 255), 1);
}
- blobs.push_back(simple_blob(radius, pos, confid, area));
+ blobs.push_back(blob(radius, pos, confid, area));
}
// clear old points
points.clear();
- using b = const simple_blob;
+ using b = const blob;
std::sort(blobs.begin(), blobs.end(), [](b& b1, b& b2) {return b1.confid > b2.confid;});
for (auto& b : blobs)
diff --git a/ftnoir_tracker_pt/point_tracker.cpp b/ftnoir_tracker_pt/point_tracker.cpp
index cedf1979..924b75de 100644
--- a/ftnoir_tracker_pt/point_tracker.cpp
+++ b/ftnoir_tracker_pt/point_tracker.cpp
@@ -15,19 +15,18 @@
const float PI = 3.14159265358979323846f;
-// ----------------------------------------------------------------------------
static void get_row(const cv::Matx33f& m, int i, cv::Vec3f& v)
{
- v[0] = m(i,0);
- v[1] = m(i,1);
- v[2] = m(i,2);
+ v[0] = m(i,0);
+ v[1] = m(i,1);
+ v[2] = m(i,2);
}
static void set_row(cv::Matx33f& m, int i, const cv::Vec3f& v)
{
- m(i,0) = v[0];
- m(i,1) = v[1];
- m(i,2) = v[2];
+ m(i,0) = v[0];
+ m(i,1) = v[1];
+ m(i,2) = v[2];
}
static bool d_vals_sort(const std::pair<float,int> a, const std::pair<float,int> b)
@@ -67,31 +66,31 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::
// set correspondences by minimum distance to projected model point
bool point_taken[PointModel::N_POINTS];
for (int i=0; i<PointModel::N_POINTS; ++i)
- point_taken[i] = false;
+ point_taken[i] = false;
for (int i=0; i<PointModel::N_POINTS; ++i)
{
- float min_sdist = 0;
- int min_idx = 0;
- // find closest point to projected model point i
- for (int j=0; j<PointModel::N_POINTS; ++j)
+ float min_sdist = 0;
+ int min_idx = 0;
+ // find closest point to projected model point i
+ for (int j=0; j<PointModel::N_POINTS; ++j)
+ {
+ cv::Vec2f d = p.points[i]-points[j];
+ float sdist = d.dot(d);
+ if (sdist < min_sdist || j==0)
{
- cv::Vec2f d = p.points[i]-points[j];
- float sdist = d.dot(d);
- if (sdist < min_sdist || j==0)
- {
- min_idx = j;
- min_sdist = sdist;
- }
+ min_idx = j;
+ min_sdist = sdist;
}
- // if one point is closest to more than one model point, fallback
- if (point_taken[min_idx])
- {
- init_phase = true;
- return find_correspondences(points, model);
- }
- point_taken[min_idx] = true;
- p.points[i] = points[min_idx];
+ }
+ // if one point is closest to more than one model point, fallback
+ if (point_taken[min_idx])
+ {
+ init_phase = true;
+ return find_correspondences(points, model);
+ }
+ point_taken[min_idx] = true;
+ p.points[i] = points[min_idx];
}
return p;
}
@@ -100,19 +99,19 @@ void PointTracker::track(const std::vector<cv::Vec2f>& points, const PointModel&
{
PointOrder order;
- if (t.elapsed_ms() > init_phase_timeout)
- {
- t.start();
- init_phase = true;
- }
+ if (t.elapsed_ms() > init_phase_timeout)
+ {
+ t.start();
+ init_phase = true;
+ }
if (!dynamic_pose || init_phase)
order = find_correspondences(points, model);
- else
- order = find_correspondences_previous(points, model, f);
-
+ else
+ order = find_correspondences_previous(points, model, f);
+
POSIT(model, order, f);
- init_phase = false;
+ init_phase = false;
t.start();
}
@@ -142,127 +141,127 @@ PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv
int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float focal_length)
{
- // 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
- cv::Matx33f R_expected = cv::Matx33f::eye();
-
- // initial pose = last (predicted) pose
- cv::Vec3f k;
- get_row(R_expected, 2, k);
- float Z0 = 1000.f;
-
- float old_epsilon_1 = 0;
- float old_epsilon_2 = 0;
- float epsilon_1 = 1;
- float epsilon_2 = 1;
-
- cv::Vec3f I0, J0;
- cv::Vec2f I0_coeff, J0_coeff;
-
- cv::Vec3f I_1, J_1, I_2, J_2;
- cv::Matx33f R_1, R_2;
- cv::Matx33f* R_current;
-
- const int MAX_ITER = 100;
- const float EPS_THRESHOLD = 1e-4;
-
- const cv::Vec2f* order = order_.points;
-
- int i=1;
- for (; i<MAX_ITER; ++i)
- {
- epsilon_1 = k.dot(model.M01)/Z0;
- epsilon_2 = k.dot(model.M02)/Z0;
-
- // vector of scalar products <I0, M0i> and <J0, M0i>
- cv::Vec2f I0_M0i(order[1][0]*(1.0 + epsilon_1) - order[0][0],
- order[2][0]*(1.0 + epsilon_2) - order[0][0]);
- cv::Vec2f J0_M0i(order[1][1]*(1.0 + epsilon_1) - order[0][1],
- order[2][1]*(1.0 + epsilon_2) - order[0][1]);
-
- // construct projection of I, J onto M0i plane: I0 and J0
- I0_coeff = model.P * I0_M0i;
- J0_coeff = model.P * J0_M0i;
- I0 = I0_coeff[0]*model.M01 + I0_coeff[1]*model.M02;
- J0 = J0_coeff[0]*model.M01 + J0_coeff[1]*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 = std::sqrt(std::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;
- }
-
- // construct the two solutions
- I_1 = I0 + rho*cos(theta)*model.u;
- I_2 = I0 - rho*cos(theta)*model.u;
-
- J_1 = J0 + rho*sin(theta)*model.u;
- J_2 = J0 - rho*sin(theta)*model.u;
-
- float norm_const = 1.0/cv::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;
-
- 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));
-
- // the single translation solution
- Z0 = norm_const * focal_length;
-
- // pick the rotation solution closer to the expected one
- // in simple metric d(A,B) = || I - A * B^T ||
- float R_1_deviation = cv::norm(cv::Matx33f::eye() - R_expected * R_1.t());
- float R_2_deviation = cv::norm(cv::Matx33f::eye() - R_expected * R_2.t());
-
- if (R_1_deviation < R_2_deviation)
- R_current = &R_1;
- else
- R_current = &R_2;
-
- get_row(*R_current, 2, k);
-
- // check for convergence condition
- if (std::abs(epsilon_1 - old_epsilon_1) + std::abs(epsilon_2 - old_epsilon_2) < EPS_THRESHOLD)
- break;
- old_epsilon_1 = epsilon_1;
- old_epsilon_2 = epsilon_2;
- }
-
- // apply results
- X_CM.R = *R_current;
- X_CM.t[0] = order[0][0] * Z0/focal_length;
- X_CM.t[1] = order[0][1] * Z0/focal_length;
- X_CM.t[2] = Z0;
-
- //qDebug() << "iter:" << i;
-
- return i;
+ // 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
+ cv::Matx33f R_expected = cv::Matx33f::eye();
+
+ // initial pose = last (predicted) pose
+ cv::Vec3f k;
+ get_row(R_expected, 2, k);
+ float Z0 = 1000.f;
+
+ float old_epsilon_1 = 0;
+ float old_epsilon_2 = 0;
+ float epsilon_1 = 1;
+ float epsilon_2 = 1;
+
+ cv::Vec3f I0, J0;
+ cv::Vec2f I0_coeff, J0_coeff;
+
+ cv::Vec3f I_1, J_1, I_2, J_2;
+ cv::Matx33f R_1, R_2;
+ cv::Matx33f* R_current;
+
+ const int MAX_ITER = 100;
+ const float EPS_THRESHOLD = 1e-4;
+
+ const cv::Vec2f* order = order_.points;
+
+ int i=1;
+ for (; i<MAX_ITER; ++i)
+ {
+ epsilon_1 = k.dot(model.M01)/Z0;
+ epsilon_2 = k.dot(model.M02)/Z0;
+
+ // vector of scalar products <I0, M0i> and <J0, M0i>
+ cv::Vec2f I0_M0i(order[1][0]*(1.0 + epsilon_1) - order[0][0],
+ order[2][0]*(1.0 + epsilon_2) - order[0][0]);
+ cv::Vec2f J0_M0i(order[1][1]*(1.0 + epsilon_1) - order[0][1],
+ order[2][1]*(1.0 + epsilon_2) - order[0][1]);
+
+ // construct projection of I, J onto M0i plane: I0 and J0
+ I0_coeff = model.P * I0_M0i;
+ J0_coeff = model.P * J0_M0i;
+ I0 = I0_coeff[0]*model.M01 + I0_coeff[1]*model.M02;
+ J0 = J0_coeff[0]*model.M01 + J0_coeff[1]*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 = std::sqrt(std::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;
+ }
+
+ // construct the two solutions
+ I_1 = I0 + rho*cos(theta)*model.u;
+ I_2 = I0 - rho*cos(theta)*model.u;
+
+ J_1 = J0 + rho*sin(theta)*model.u;
+ J_2 = J0 - rho*sin(theta)*model.u;
+
+ float norm_const = 1.0/cv::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;
+
+ 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));
+
+ // the single translation solution
+ Z0 = norm_const * focal_length;
+
+ // pick the rotation solution closer to the expected one
+ // in simple metric d(A,B) = || I - A * B^T ||
+ float R_1_deviation = cv::norm(cv::Matx33f::eye() - R_expected * R_1.t());
+ float R_2_deviation = cv::norm(cv::Matx33f::eye() - R_expected * R_2.t());
+
+ if (R_1_deviation < R_2_deviation)
+ R_current = &R_1;
+ else
+ R_current = &R_2;
+
+ get_row(*R_current, 2, k);
+
+ // check for convergence condition
+ if (std::abs(epsilon_1 - old_epsilon_1) + std::abs(epsilon_2 - old_epsilon_2) < EPS_THRESHOLD)
+ break;
+ old_epsilon_1 = epsilon_1;
+ old_epsilon_2 = epsilon_2;
+ }
+
+ // apply results
+ X_CM.R = *R_current;
+ X_CM.t[0] = order[0][0] * Z0/focal_length;
+ X_CM.t[1] = order[0][1] * Z0/focal_length;
+ X_CM.t[2] = Z0;
+
+ //qDebug() << "iter:" << i;
+
+ return i;
}
cv::Vec2f PointTracker::project(const cv::Vec3f& v_M, float f)
{
- 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]);
+ 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]);
}
diff --git a/ftnoir_tracker_pt/point_tracker.h b/ftnoir_tracker_pt/point_tracker.h
index df938237..f4268486 100644
--- a/ftnoir_tracker_pt/point_tracker.h
+++ b/ftnoir_tracker_pt/point_tracker.h
@@ -122,6 +122,10 @@ public:
void track(const std::vector<cv::Vec2f>& projected_points, const PointModel& model, float f, bool dynamic_pose, int init_phase_timeout);
Affine pose() const { return X_CM; }
cv::Vec2f project(const cv::Vec3f& v_M, float f);
+ void reset(const Affine& pose)
+ {
+ X_CM = pose;
+ }
private:
// the points in model order
struct PointOrder
diff --git a/ftnoir_tracker_pt/pt_video_widget.cpp b/ftnoir_tracker_pt/pt_video_widget.cpp
index 9f2b90f6..cbb7c268 100644
--- a/ftnoir_tracker_pt/pt_video_widget.cpp
+++ b/ftnoir_tracker_pt/pt_video_widget.cpp
@@ -1,4 +1,5 @@
/* Copyright (c) 2012 Patrick Ruoff
+ * Copyright (c) 2015 Stanislaw Halik <sthalik@misaki.pl>
*
* Permission to use, copy, modify, and/or distribute this software for any
* purpose with or without fee is hereby granted, provided that the above