[lk_flow] Keep matching information for sparse optical flow

msg/FlowArray.msg

@@ -1 +1,2 @@
 Flow[] flow
+bool[] status

msg/FlowArrayStamped.msg

@@ -1,2 +1,3 @@
 Header header
 Flow[] flow
+bool[] status

src/nodelet/fback_flow_nodelet.cpp

@@ -165,6 +165,7 @@ class FBackFlowNodelet : public opencv_apps::Nodelet
             velocity_msg.y = fxy.y;
             flow_msg.point = point_msg;
             flow_msg.velocity = velocity_msg;
+            flows_msg.status.push_back(true);
             flows_msg.flow.push_back(flow_msg);
           }
       }

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)

src/nodelet/simple_flow_nodelet.cpp

@@ -195,6 +195,7 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet
           velocity_msg.y = scale_row * flow_at_point[1];
           flow_msg.point = point_msg;
           flow_msg.velocity = velocity_msg;
+          flows_msg.status.push_back(true);
           flows_msg.flow.push_back(flow_msg);
         }
       }