diff options
Diffstat (limited to 'tracker-pt')
| -rw-r--r-- | tracker-pt/camera.cpp | 6 | ||||
| -rw-r--r-- | tracker-pt/ftnoir_tracker_pt.cpp | 3 | ||||
| -rwxr-xr-x | tracker-pt/point_tracker.cpp | 19 | ||||
| -rw-r--r-- | tracker-pt/point_tracker.h | 2 |
4 files changed, 18 insertions, 12 deletions
diff --git a/tracker-pt/camera.cpp b/tracker-pt/camera.cpp index 600ab26a..c995c11b 100644 --- a/tracker-pt/camera.cpp +++ b/tracker-pt/camera.cpp @@ -57,12 +57,12 @@ bool Camera::get_frame(float dt, cv::Mat* frame) { bool new_frame = _get_frame(frame); // measure fps of valid frames - const float dt_smoothing_const = 0.95; + constexpr float dt_smoothing_const = 0.95; dt_valid += dt; if (new_frame) { - dt_mean = dt_smoothing_const * dt_mean + (1.0 - dt_smoothing_const) * dt_valid; - cam_info.fps = dt_mean > 1e-3 ? 1.0 / dt_mean : 0; + dt_mean = dt_smoothing_const * dt_mean + (1 - dt_smoothing_const) * dt_valid; + cam_info.fps = dt_mean > 1e-3f ? 1 / dt_mean : 0; dt_valid = 0; } else diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp index c4ab5963..3415eecd 100644 --- a/tracker-pt/ftnoir_tracker_pt.cpp +++ b/tracker-pt/ftnoir_tracker_pt.cpp @@ -197,6 +197,9 @@ void Tracker_PT::data(double *data) 0, 1, 0); R = R_EG * R * R_EG.t(); + using std::atan2; + using std::sqrt; + // 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)) ); diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp index 493f311c..c7947c98 100755 --- a/tracker-pt/point_tracker.cpp +++ b/tracker-pt/point_tracker.cpp @@ -166,11 +166,14 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float cv::Matx33f R_1, R_2; cv::Matx33f* R_current; - const int MAX_ITER = 100; - const float EPS_THRESHOLD = 1e-4; + constexpr int MAX_ITER = 100; + const float EPS_THRESHOLD = 1e-4f; const cv::Vec2f* order = order_.points; + using std::sqrt; + using std::atan; + int i=1; for (; i<MAX_ITER; ++i) { @@ -178,10 +181,10 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float 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]); + cv::Vec2f I0_M0i(order[1][0]*(1 + epsilon_1) - order[0][0], + order[2][0]*(1 + epsilon_2) - order[0][0]); + cv::Vec2f J0_M0i(order[1][1]*(1 + epsilon_1) - order[0][1], + order[2][1]*(1 + epsilon_2) - order[0][1]); // construct projection of I, J onto M0i plane: I0 and J0 I0_coeff = model.P * I0_M0i; @@ -194,7 +197,7 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float float IJ0 = I0.dot(J0); float JJ0 = J0.dot(J0); float rho, theta; - // CAVEAT don't change to comparison with a small epsilon, e.g. 1e-4. -sh 20160423 + // CAVEAT don't change to comparison with an epsilon -sh 20160423 if (JJ0 == II0) { rho = std::sqrt(std::abs(2*IJ0)); theta = -PI/4; @@ -215,7 +218,7 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float 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 + float norm_const = 1/cv::norm(I_1); // all have the same norm // create rotation matrices I_1 *= norm_const; J_1 *= norm_const; diff --git a/tracker-pt/point_tracker.h b/tracker-pt/point_tracker.h index 2757f22c..e70059ff 100644 --- a/tracker-pt/point_tracker.h +++ b/tracker-pt/point_tracker.h @@ -78,7 +78,7 @@ public: float s11 = M01.dot(M01); float s12 = M01.dot(M02); float s22 = M02.dot(M02); - P = 1.0/(s11*s22-s12*s12) * cv::Matx22f(s22, -s12, -s12, s11); + P = 1/(s11*s22-s12*s12) * cv::Matx22f(s22, -s12, -s12, s11); } void set_model(settings_pt& s) |