Movement Detection with Sequential Image comparison

edit

Dear all, i'm trying to detect object movement using sequential image comparison in ROS. But there is some bug and I'm unable to find. here is my code snippet in detail... I want to know that is this the correct way to detect movement using sequential image comparison?

void callback(const ImageConstPtr& image_rgb, const ImageConstPtr& image_depth_source) { pcl::StopWatch watch;

//some boolean variables for added functionality
bool objectDetected = false;
//these two can be toggled by pressing 'd' or 't'
bool debugMode = false;
bool trackingEnabled = false;
//pause and resume code
bool pause = false;
//set up the matrices that we will need
//the two frames we will be comparing
Mat frame1,frame2;
//their grayscale images (needed for absdiff() function)
Mat grayImage1,grayImage2;
//resulting difference image
Mat differenceImage;
//thresholded difference image (for use in findContours() function)
Mat thresholdImage;

// Solve all of perception here... (ROS Format converted to OpenCV format here)
cv::Mat image_color = cv_bridge::toCvCopy(image_rgb)->image;
cv::Mat image_depth = cv_bridge::toCvCopy(image_depth_source)->image;

// publisher definition
std_msgs::Float32MultiArray dataArray;
// data clear 
dataArray.data.clear();

int count;

if (count<10)
{
    image_color.copyTo(frame1);
    count++;
    return;
}
else
{
    image_color.copyTo(frame2);
}

//convert frame1 to gray scale for frame differencing
cv::cvtColor(frame1,grayImage1,COLOR_BGR2GRAY);

//convert frame2 to gray scale for frame differencing
cv::cvtColor(frame2,grayImage2,COLOR_BGR2GRAY);

//perform frame differencing with the sequential images. 
cv::absdiff(grayImage1,grayImage2,differenceImage);

//threshold intensity image at a given sensitivity value
cv::threshold(differenceImage,thresholdImage,SENSITIVITY_VALUE,255,THRESH_BINARY);

//show the difference image and threshold image
cv::imshow("Difference Image",differenceImage);
cv::imshow("Threshold Image", thresholdImage);

imshow("Frame1",frame1);

frame2.copyTo(frame1);


cout<<count<<endl;
// for image refreshing 
cv::waitKey(1);

}

int main(int argc, char** argv) { ros::init(argc, argv, "vision_node");

ros::NodeHandle nh;

// topic subscription
message_filters::Subscriber<Image> RGB_sub(nh, "/camera/rgb/image_color", 1);
message_filters::Subscriber<Image> DEPTH_sub(nh, "/camera/depth/image_raw", 1);

// synchronization policy
typedef sync_policies::ApproximateTime<Image,Image> MySyncPolicy;

// ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), RGB_sub, DEPTH_sub);
sync.registerCallback(boost::bind(&callback, _1, _2));

// data publish for further processing 
pubData = nh.advertise<std_msgs::Float32MultiArray>("pubData", 10);

ros::spin();
return 0;

}

I'll be a great help if anyone can guide me in solving this problem.

Regards, Prasanna