Fix pts corresponding

HeadPoseDetector.cpp

@@ -201,7 +201,13 @@ std::pair<CvPts, CvPts> calc_optical_flow(cv::Mat & prev_img, cv::Mat & cur_img,
     TermCriteria criteria = TermCriteria((TermCriteria::COUNT) + (TermCriteria::EPS), 30, 0.01);
     CvPts cur_pts = predict_pts;
     CvPts pts_velocity;
-    calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, Size(10,10), 2, criteria, cv::OPTFLOW_USE_INITIAL_FLOW);
+
+    if (prev_pts.size() == predict_pts.size()) {
+        calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, Size(10,10), 2, criteria, cv::OPTFLOW_USE_INITIAL_FLOW);
+    } else {
+        calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, Size(10,10), 2, criteria);
+
+    }
 
     reduceVector(prev_pts, status);
     reduceVector(cur_pts, status);
@@ -314,8 +320,9 @@ std::pair<bool, Pose> HeadPoseDetector::detect_head_pose(cv::Mat & frame, double
 
     TicToc tic1;
     CvPts landmarks = lmd->detect(frame, roi);
-    auto ret = this->solve_face_pose(landmarks, frame);
 
+    double distance_sum = 0;
+
     if (last_landmark_pts.size() > 0) {
         // calculate optical flow
         TicToc tic;
@@ -326,14 +333,34 @@ std::pair<bool, Pose> HeadPoseDetector::detect_head_pose(cv::Mat & frame, double
         for (auto _id : last_ids) {
             pts3d.push_back(model_points_68[_id]);
         }
+
         ekf.update_by_feature_pts(t, ret, pts3d);
+
         if (settings->enable_preview) {
             for (int i = 0; i < tracked_pts.size(); i++) {
-                cv::arrowedLine(frame, last_landmark_pts[i], tracked_pts[i], cv::Scalar(0, 0, 255), 1, 8, 0, 0.2);
+                cv::arrowedLine(frame, last_landmark_pts[i], tracked_pts[i], cv::Scalar(255, 0, 0), 1, 8, 0, 0.2);
+                cv::circle(frame, tracked_pts[i], 1, cv::Scalar(0, 0, 0), -1);
             }
         }
+        /*
+        for (int i = 0; i < last_ids.size(); i++) {
+            int _id = last_ids[i];
+            double dis = cv::norm(tracked_pts[i] - landmarks[_id]);
+            if (settings->enable_preview) {
+                if (dis < 3.0) {
+                    cv::arrowedLine(frame, landmarks[_id], tracked_pts[i], cv::Scalar(0, 255, 255), 1, 8, 0, 0.2);
+                } else {
+                    cv::arrowedLine(frame, landmarks[_id], tracked_pts[i], cv::Scalar(0, 0, 255), 1, 8, 0, 0.2);
+                }
+            }
+            distance_sum += dis;
+        }*/
     }
 
+
+    auto ret = this->solve_face_pose(landmarks, frame);
+
+
     if (ret.first) {
         auto pose = ret.second;
         char rot[100] = {0};
@@ -348,14 +375,14 @@ std::pair<bool, Pose> HeadPoseDetector::detect_head_pose(cv::Mat & frame, double
             }
         }
 
-        last_landmark_pts = landmarks;
-
         last_clean_frame = frame_clean;
-
+        last_landmark_pts = landmarks;
+        last_ids.clear();
         for (int i = 0; i < 68; i++) {
             last_ids.push_back(i);
         }
 
+
         return make_pair(true, make_pair(R, T));
     }
     return make_pair(false, make_pair(R, T));