pt: visualize model center as another point

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();