Quantcast
Channel: ROS Answers: Open Source Q&A Forum - RSS feed
Viewing all articles
Browse latest Browse all 18

calcOpticalFlowPyrLK and goodFeaturesToTrack doesn't work properly

$
0
0
I trying to port some code i written in C++ non ROS into a Ros environment, which causing some problems.. The purpose of this code is to track the movement of a persons face, a bit like the [pi_face_tracker](http://wiki.ros.org/pi_face_tracker) The problem though, is that the opticalFlow isn't working properly.. The imshow screen doesn't say where the new points are, but just keep the old points. Which makes me suspect if it even tracking the movements of the point or not?.. here is the code: #include "image_converter.h" #include int getch() { static struct termios oldt, newt; tcgetattr( STDIN_FILENO, &oldt); // save old settings newt = oldt; newt.c_cc[VMIN] = 0; newt.c_cc[VTIME] = 0; newt.c_lflag &= ~(ICANON); // disable buffering tcsetattr( STDIN_FILENO, TCSANOW, &newt); // apply new settings int c = getchar(); // read character (non-blocking) tcsetattr( STDIN_FILENO, TCSANOW, &oldt); // restore old settings return c; } ImageConverter::ImageConverter() : it_(nh_),pos(2), vel(2) { // Subscrive to input video feed and publish output video feed image_sub_ = it_.subscribe("/image/left/image_rect_color", 1, &ImageConverter::imageDetect, this); image_pub_ = it_.advertise("/image_converter/output_video", 1); state = false; reinit = false; redetect = false; frames = 0; cout << "start over" << endl; } ImageConverter::~ImageConverter() { //cv::destroyAllWindows(); } void ImageConverter::imageDetect(const sensor_msgs::ImageConstPtr& msg) { facedetect_pub = nh_.advertise("/ptu/cmd", 1); sub = nh_.subscribe("/joint_states",1,&ImageConverter::chatterCallback,this); cv_bridge::CvImagePtr cv_ptr; sensor_msgs::JointState ms; try { cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); return; } if(!(state)) { vector faces(1); Point center_of_frame(cv_ptr->image.size().width/2,cv_ptr->image.size().height/2); pair corners; pair displacement; double displacement_pixel_x; double displacement_pixel_y; pair response; if (cv_ptr->image.type()!= 8) { cvtColor(cv_ptr->image, cv_ptr->image, CV_8U); } //-- 1. Load the cascades if( !face_cascade.load( face_cascade_XML ) ){ cout << "Cascade Error" << endl; }; circle(cv_ptr->image, center_of_frame, 1, CV_RGB(0,255,255),8,8,0); //-- Detect faces face_cascade.detectMultiScale( cv_ptr->image, faces, 1.1, 2, 0, Size(100, 100) ); if(faces.empty()) { if( !face_cascade.load( face_cascade_XML1 ) ) { cout << "Cascade Error" << endl; }; face_cascade.detectMultiScale( cv_ptr->image, faces, 1.1, 2, 0, Size(100, 100) ); if( !face_cascade.load( nose_xml ) ) { cout << "Cascade Error" << endl; }; face_cascade.detectMultiScale( cv_ptr->image, faces, 1.1, 2, 0, Size(100, 100) ); } for( int i = 0; iimage, faces[i], CV_RGB(0,255,0),4,8,0); circle(cv_ptr->image, center_position_of_face, 8, CV_RGB(128,128,128),8,8,0); circle(cv_ptr->image, corner_1, 1, CV_RGB(128,128,128),8,8,0); circle(cv_ptr->image, corner_2, 1, CV_RGB(128,128,128),8,8,0); circle(cv_ptr->image, corner_3, 1, CV_RGB(128,128,128),8,8,0); circle(cv_ptr->image, corner_4, 1, CV_RGB(128,128,128),8,8,0); line(cv_ptr->image, center_position_of_face, center_of_frame, CV_RGB(0,200,0),1,8); } flip(cv_ptr->image, cv_ptr->image, 1); imshow("Facedetection", cv_ptr->image); if(faces.back().area()>200*200) { face = faces.back(); } cout << "face - info: " << face.area() << endl; waitKey(1); int cc = getch(); if(cc == 'y' || redetect == true) { cout << "keypress registered" << endl; state = true; redetect = false; //destroyWindow("Facedetection"); } } else { Mat gray, prevGray, image,test; image = cv_ptr->image; cvtColor(image,gray,COLOR_BGR2GRAY); equalizeHist(gray,gray); Mat mask(image.size(),CV_8UC1,Scalar(0)); mask(face).setTo(Scalar(255)); static uint64_t c; if (c++ == 0 || reinit == true) { cout << "automatic initialization" << endl; goodFeaturesToTrack(gray, points[1], MAX_COUNT, 0.01, 10, mask, 3, 0, 0.04); cornerSubPix(gray, points[1], subPixWinSize, Size(-1,-1), termcrit); cout << "Recalc" << endl; reinit = false; } if( !points[0].empty() ) { // cout << "i am here" << endl; vector status; vector err; if(prevGray.empty()) { gray.copyTo(prevGray); } calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err, winSize,3, termcrit, 0, qualitylevel); size_t i, k; //outlier_point(); for( i = k = 0; i < points[1].size(); i++ ) { if( !status[i] ) { cout << "removed " << endl; circle( image, points[1][i],11, Scalar(255,0,0), -1, 8); } if(abs(points[1][i].x - (face.br().x+face.tl().x)/2) > window+200 || abs(points[1][i].y-(face.tl().y+face.br().y)/2) > window+200 ) { circle( image, points[1][i],11, Scalar(0,0,255), -1, 8); } else { points[1][k++] = points[1][i]; circle( image, points[1][i], 3, Scalar(0,255,0), -1, 8); } } cout << "k: " << k << endl; points[1].resize(k); RotatedRect ka = fitEllipse(points[1]); ellipse(image, ka, CV_RGB(0, 0, 255)); circle(image,ka.center,5,CV_RGB(120,120,120)); cout << "Center-position!!!!!!!!!!!!!!!!!!!!!!!!!!: " << floor(ka.center.x) << "," << floor(ka.center.y) << endl; } if( points[1].size() < (size_t)MAX_COUNT ) { vector tmp; tmp.push_back(point); cornerSubPix( gray, tmp, winSize, cvSize(-1,-1), termcrit); points[1].push_back(tmp[0]); } flip(image, image, 1); imshow("LK Demo", image); waitKey(1); std::swap(points[1], points[0]); cv::swap(prevGray, gray); cout << "new - point: "<

Viewing all articles
Browse latest Browse all 18

Trending Articles



<script src="https://jsc.adskeeper.com/r/s/rssing.com.1596347.js" async> </script>