diff options
| -rw-r--r-- | ftnoir_tracker_pt/ftnoir_tracker_pt.cpp | 20 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/point_extractor.cpp | 2 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/point_extractor.h | 2 |
3 files changed, 19 insertions, 5 deletions
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp index 61216000..cfeefe38 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp @@ -74,7 +74,22 @@ void Tracker::run() { QMutexLocker lock(&mutex); - const std::vector<cv::Vec2f>& points = point_extractor.extract_points(frame); + std::vector<cv::Vec2f> points = point_extractor.extract_points(frame); + + if (points.size() == PointModel::N_POINTS) + point_tracker.track(points, PointModel(s), get_focal_length()); + + { + Affine CM; + pose(&CM); + cv::Vec3f MH(s.t_MH_x, s.t_MH_y, s.t_MH_z); + cv::Vec3f p = CM.t - MH; + float fx = get_focal_length(); + cv::Vec2f p_(p[0] / p[2] * fx, p[1] / p[2] * fx); + + points.push_back(p_); + } + for (auto p : points) { auto p2 = cv::Point(p[0] * frame.cols + frame.cols/2, -p[1] * frame.cols + frame.rows/2); @@ -90,8 +105,7 @@ void Tracker::run() color, 4); } - if (points.size() == PointModel::N_POINTS) - point_tracker.track(points, PointModel(s), get_focal_length()); + video_widget->update_image(frame); } #ifdef PT_PERF_LOG diff --git a/ftnoir_tracker_pt/point_extractor.cpp b/ftnoir_tracker_pt/point_extractor.cpp index f4eefb0f..949fd58f 100644 --- a/ftnoir_tracker_pt/point_extractor.cpp +++ b/ftnoir_tracker_pt/point_extractor.cpp @@ -20,7 +20,7 @@ PointExtractor::PointExtractor(){ //freopen("CON", "w", stderr); } // ---------------------------------------------------------------------------- -const vector<Vec2f>& PointExtractor::extract_points(Mat& frame) +std::vector<Vec2f> PointExtractor::extract_points(Mat& frame) { const int W = frame.cols; const int H = frame.rows; diff --git a/ftnoir_tracker_pt/point_extractor.h b/ftnoir_tracker_pt/point_extractor.h index 2cac29a9..06d148d6 100644 --- a/ftnoir_tracker_pt/point_extractor.h +++ b/ftnoir_tracker_pt/point_extractor.h @@ -21,7 +21,7 @@ public: // extracts points from frame and draws some processing info into frame, if draw_output is set // dt: time since last call in seconds // WARNING: returned reference is valid as long as object - const std::vector<cv::Vec2f>& extract_points(cv::Mat &frame); + std::vector<cv::Vec2f> extract_points(cv::Mat &frame); const std::vector<cv::Vec2f>& get_points() { return points; } PointExtractor(); |