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: "<
↧