keep the size and return status

ros-perception · Feb 19, 2025 · cb239e9 · cb239e9
1 parent 2a5c296
commit cb239e9
Showing 1 changed file with 12 additions and 14 deletions.
diff --git a/src/nodelet/lk_flow_nodelet.cpp b/src/nodelet/lk_flow_nodelet.cpp
@@ -195,8 +195,8 @@ class LKFlowNodelet : public opencv_apps::Nodelet
         if (prevGray.empty())
           gray.copyTo(prevGray);
         cv::calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err, win_size, 3, termcrit, 0, 0.001);
-        size_t i, k;
-        for (i = k = 0; i < points[1].size(); i++)
+        size_t i;
+        for (i = 0; i < points[1].size(); i++)
         {
           if (addRemovePt)
           {
@@ -207,25 +207,23 @@ class LKFlowNodelet : public opencv_apps::Nodelet
             }
           }
 
-          if (!status[i])
-            continue;
-
-          points[1][k++] = points[1][i];
-          cv::circle(image, points[1][i], 3, cv::Scalar(0, 255, 0), -1, 8);
-          cv::line(image, points[1][i], points[0][i], cv::Scalar(0, 255, 0), 1, 8, 0);
-
           opencv_apps::Flow flow_msg;
           opencv_apps::Point2D point_msg;
           opencv_apps::Point2D velocity_msg;
-          point_msg.x = points[1][i].x;
-          point_msg.y = points[1][i].y;
-          velocity_msg.x = points[1][i].x - points[0][i].x;
-          velocity_msg.y = points[1][i].y - points[0][i].y;
+          if (status[i])
+          {
+            cv::circle(image, points[1][i], 3, cv::Scalar(0, 255, 0), -1, 8);
+            cv::line(image, points[1][i], points[0][i], cv::Scalar(0, 255, 0), 1, 8, 0);
+            point_msg.x = points[1][i].x;
+            point_msg.y = points[1][i].y;
+            velocity_msg.x = points[1][i].x - points[0][i].x;
+            velocity_msg.y = points[1][i].y - points[0][i].y;
+          }
           flow_msg.point = point_msg;
           flow_msg.velocity = velocity_msg;
+          flows_msg.status.push_back(status[i]);
           flows_msg.flow.push_back(flow_msg);
         }
-        points[1].resize(k);
       }
 
       if (addRemovePt && points[1].size() < (size_t)MAX_COUNT)