diff options
| -rw-r--r-- | opentrack/tracker.cpp | 13 |
1 files changed, 11 insertions, 2 deletions
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp index 24482bfc..7e6919a3 100644 --- a/opentrack/tracker.cpp +++ b/opentrack/tracker.cpp @@ -103,8 +103,6 @@ void Tracker::t_compensate(const cv::Matx33d& rmat, const double* xyz, double* o void Tracker::logic() { - libs.pTracker->data(newpose); - if (enabledp) for (int i = 0; i < 6; i++) final_raw(i) = newpose[i]; @@ -196,6 +194,7 @@ void Tracker::run() { { t.start(); + libs.pTracker->data(newpose); logic(); double q = sleep_ms * 1000L; @@ -204,6 +203,16 @@ void Tracker::run() { usleep((long)q); } + { + // do one last pass with origin pose + for (int i = 0; i < 6; i++) + newpose[i] = 0; + logic(); + // filter may inhibit exact origin + Pose p; + libs.pProtocol->pose(p); + } + #if defined(_WIN32) (void) timeEndPeriod(1); #endif |