OpenCV Optical Flow Core Dumped - c++

Question

I am using the OpenCV LK Optical Flow example, for some robot vision application with ROS.
It seems to be working reasonably well, however, I am having the error below:
OpenCV Error: Assertion failed ((npoints = prevPtsMat.checkVector(2, CV_32F, true)) >= 0) in calc, file /build/opencv-L2vuMj/opencv-3.2.0+dfsg/modules/video/src/lkpyramid.cpp, line 1231
terminate called after throwing an instance of 'cv::Exception'
what(): /build/opencv-L2vuMj/opencv-3.2.0+dfsg/modules/video/src/lkpyramid.cpp:1231: error: (-215) (npoints = prevPtsMat.checkVector(2, CV_32F, true)) >= 0 in function calc
Aborted (core dumped)
As far I could understand by reading the error message, and by the system behavior, it is happening when all of the initial tracking points are gone in the current frame.
There would be a way to restart this process (defining new points) instead of aborting?
My code by the way:
#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/video.hpp>
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <vector>
using namespace cv;
using namespace std;
static const std::string OPENCV_WINDOW = "Image_window";
class ImageConverter
{
ros::NodeHandle nh_;
image_transport::ImageTransport it_;
image_transport::Subscriber image_sub_;
image_transport::Publisher image_pub_;
public:
cv::Mat current_frame;
public:
ImageConverter()
: it_(nh_)
{
image_sub_ = it_.subscribe("/realsense/color/image_raw", 1,
&ImageConverter::imageCallback, this);
cv::namedWindow(OPENCV_WINDOW);
}
~ImageConverter()
{
cv::destroyWindow(OPENCV_WINDOW);
}
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_image;
try
{
cv_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
current_frame = cv_image->image;
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "OpenCV_Node");
ImageConverter MyNode;
ros::Rate r(30);
r.sleep();
ros::spinOnce();
r.sleep();
// Tentative of Using Open CV Tools with a ROS Image
// Lucas-Kanade Optical Flow from: https://docs.opencv.org/master/d4/dee/tutorial_optical_flow.html
// Create some random colors
vector<Scalar> colors;
RNG rng;
for(int i = 0; i < 100; i++)
{
int r = rng.uniform(0, 256);
int g = rng.uniform(0, 256);
int b = rng.uniform(0, 256);
colors.push_back(Scalar(r,g,b));
}
Mat old_frame, old_gray;
vector<Point2f> p0, p1;
// Take first frame and find corners in it
old_frame = MyNode.current_frame;
cvtColor(old_frame, old_gray, COLOR_BGR2GRAY);
goodFeaturesToTrack(old_gray, p0, 100, 0.3, 7, Mat(), 7, false, 0.04);
// Create a mask image for drawing purposes
Mat mask = Mat::zeros(old_frame.size(), old_frame.type());
//while(true){
while(ros::ok()){
r.sleep();
Mat frame, frame_gray;
frame = MyNode.current_frame;
if (frame.empty())
break;
cvtColor(frame, frame_gray, COLOR_BGR2GRAY);
// calculate optical flow
vector<uchar> status;
vector<float> err;
TermCriteria criteria = TermCriteria((TermCriteria::COUNT) + (TermCriteria::EPS), 10, 0.03);
calcOpticalFlowPyrLK(old_gray, frame_gray, p0, p1, status, err, Size(15,15), 2, criteria);
vector<Point2f> good_new;
for(uint i = 0; i < p0.size(); i++)
{
// Select good points
if(status[i] == 1) {
good_new.push_back(p1[i]);
// draw the tracks
line(mask,p1[i], p0[i], colors[i], 2);
circle(frame, p1[i], 5, colors[i], -1);
}
}
Mat img;
add(frame, mask, img);
imshow(OPENCV_WINDOW, img);
int keyboard = waitKey(30);
if (keyboard == 'q' || keyboard == 27)
break;
// Now update the previous frame and previous points
old_gray = frame_gray.clone();
p0 = good_new;
ros::spinOnce();
}
return 0;
}