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 (why not use the existing => my uses haar cascades when the point.size() drops below a threshold) 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 <termios.h>
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("/bumblebeePTU/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<sensor_msgs::JointState>("/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<Rect> faces(1);
Point center_of_frame(cv_ptr->image.size().width/2,cv_ptr->image.size().height/2);
pair<Point, Point> corners;
pair<double,double> displacement;
double displacement_pixel_x;
double displacement_pixel_y;
pair<Rect, bool> 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; i<faces.size() ; i++)
{
Point center_position_of_face((faces[i].br().x+faces[i].tl().x)/2,(faces[i].tl().y+faces[i].br().y)/2);
Point corner_1(faces[i].br().x,faces[i].tl().y);
Point corner_2 = faces[i].tl();
Point corner_3 = faces[i].br();
Point corner_4(faces[i].tl().x,faces[i].br().y);
rectangle(cv_ptr->image, 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<uchar> status;
vector<float> 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<Point2f> 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: "<<points[1].size() << endl;
cout << "old - point: " << points[0].size() << endl;
frames++;
cout << "frames: "<< frames << endl;
if(frames==1 || points[1].size() < 40)
{
//destroyWindow("LK Demo");
state = false;
reinit = true;
redetect = true;
frames = 0;
}
}
}