 | 1 | initial version |
Hi I am trying to overrule a openCV exception which occurs when the Mask which is applied on a image, lies outside of the dimension image... But i cannot make it do something else than catch that exeption when it happens... How do i make it overrule this problem?
| | 2 | No.2 Revision |
Hi I am trying to overrule a openCV exception which occurs when the Mask which is applied on a image, lies outside of the dimension image... But i cannot make it do something else than catch that exeption when it happens... How do i make it overrule this problem?
This is the exception I want overruled:
OpenCV Error: Assertion failed (0 <= roi.x && 0 <= roi.width && roi.x + roi.width <= m.cols && 0 <= roi.y && 0 <= roi.height && roi.y + roi.height <= m.rows) in Mat, file /build/buildd/opencv-2.4.8+dfsg1/modules/core/src/matrix.cpp, line 323 terminate called after throwing an instance of 'cv::Exception' what(): /build/buildd/opencv-2.4.8+dfsg1/modules/core/src/matrix.cpp:323: error: (-215) 0 <= roi.x && 0 <= roi.width && roi.x + roi.width <= m.cols && 0 <= roi.y && 0 <= roi.height && roi.y + roi.height <= m.rows in function Mat
| | 3 | No.3 Revision |
Hi I am trying to overrule a openCV exception which occurs when the Mask which is applied on a image, lies outside of the dimension image... But i cannot make it do something else than catch that exeption when it happens... How do i make it overrule this problem?
This is the exception I want overruled:
OpenCV Error: Assertion failed (0 <= roi.x && 0 <= roi.width && roi.x + roi.width <= m.cols && 0 <= roi.y && 0 <= roi.height && roi.y + roi.height <= m.rows) in Mat, file /build/buildd/opencv-2.4.8+dfsg1/modules/core/src/matrix.cpp, line 323 terminate called after throwing an instance of 'cv::Exception' what(): /build/buildd/opencv-2.4.8+dfsg1/modules/core/src/matrix.cpp:323: error: (-215) 0 <= roi.x && 0 <= roi.width && roi.x + roi.width <= m.cols && 0 <= roi.y && 0 <= roi.height && roi.y + roi.height <= m.rows in function Mat
code:
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;
}
double ImageConverter::euclideanDistance(Point2f point1)
{
double match=0;
match += sqrt((point1.x - 0) * (point1.x - 0)+ (point1.y - 0) * (point1.y - 0));
return match;
}
Point2f ImageConverter::Mean(vector<Point2f> points)
{
Mat mean_;
reduce(points,mean_,CV_REDUCE_AVG,1);
Point2f mean(mean_.at<float>(0,0),mean_.at<float>(0,1));
//cout << "Mean: " << mean_ << endl;
return mean;
}
bool ImageConverter::std_dev(vector<Point2f> points)
{
double Std_dev;
Point2f mean = Mean(points);
for(int i = 0; i < points.size(); i++)
{
Std_dev += pow(euclideanDistance(points[i]-mean),2);
}
Std_dev = Std_dev/points.size();
Std_dev = sqrt(Std_dev);
cout << "STD_DEV: " << Std_dev << endl;
if(Std_dev <= face.width)
{
cout << "Still reasonable fine " << endl;
return true; //Threshold has to be added to know when it should re init the goodFeaturesToTrack Function.
}
else
{
cout <<"you are fucked - too much drifting " << endl;
return false;
}
}
double ImageConverter::std_dev_determine(vector<Point2f> points)
{
double Std_dev;
Point2f mean = Mean(points);
for(int i = 0; i < points.size(); i++)
{
Std_dev += pow(euclideanDistance(points[i]-mean),2);
}
Std_dev = Std_dev/points.size();
Std_dev = sqrt(Std_dev);
return Std_dev;
}
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;
igen = false;
sendCommands = false;
faceWidth = 0;
faceHeight = 0;
frames = 0;
face.x = 0;
face.y = 0;
face.width = 250;
face.height = 250;
posSendMsgCounter = 0;
goodFeatures.reserve(500);
String frontalface_alt2_XML = "/usr/share/opencv/haarcascades/haarcascade_frontalface_alt2.xml";
String frontal_alt_XML = "/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml";
String frontal_alt_tree_XML = "/usr/share/opencv/haarcascades/haarcascade_frontalface_alt_tree.xml";
String frontal_face_xml = "/usr/share/opencv/haarcascades/haarcascade_frontalface_default.xml";
if( !frontalface_alt2.load( frontalface_alt2_XML ) )
{
cout << "face_cascade Cascade Error" << endl;
};
if( !frontal_alt.load( frontal_alt_XML ) )
{
cout << "profileface Cascade Error" << endl;
};
if(!frontal_alt_tree.load( frontal_alt_tree_XML ))
{
cout << "nose Cascade Error" << endl;
}
if(!frontal_face.load( frontal_face_xml ))
{
cout << "nose Cascade Error" << 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))
{
image = cv_ptr->image;
//ROI_IMAGE = cv_ptr->image;
smallIMG = image;
cvtColor(smallIMG,smallIMG,COLOR_BGR2GRAY);
cv::equalizeHist(smallIMG, smallIMG);
ROI_IMAGE = smallIMG;
Size size(200,200);
resize(smallIMG,smallIMG,size);
vector<Rect> faces(1);
Rect face_big;
center_of_frame.x = image.size().width/2;
center_of_frame.y = image.size().height/2;
center_of_frame_small.x = smallIMG.size().width/2;
center_of_frame_small.y = smallIMG.size().height/2;
pair<Point, Point> corners;
pair<double,double> displacement;
double displacement_pixel_x;
double displacement_pixel_y;
pair<Rect, bool> response;
//if (image.type()!= 8)
//{
//cvtColor(image,image, CV_8U);
//}
//-- 1. Load the cascades
circle(image, center_of_frame, 1, CV_RGB(0,255,255),8,8,0);
circle(smallIMG, center_of_frame_small, 1, CV_RGB(0,255,255),3,3,0);
//-- Detect faces
frontalface_alt2.detectMultiScale(smallIMG, faces, 1.1, 2, 0, Size(10, 10) );
//frontal_alt.detectMultiScale( smallIMG, faces, 1.1, 2, 0, Size(0, 0) );
//frontal_alt_tree.detectMultiScale( smallIMG, faces, 1.1, 2, 0, Size(0, 0) );
//frontal_face.detectMultiScale( smallIMG, faces, 1.1, 2, 0, Size(0, 0) );
int i,k;
for( i = k = 0; i<faces.size() ; i++)
{
Point center_position_of_face_small((faces[i].br().x+faces[i].tl().x)/2,(faces[i].tl().y+faces[i].br().y)/2);
Point corner_1_small(faces[i].tl().x,faces[i].br().y);
Point corner_2_small = faces[i].tl();
Point corner_3_small = faces[i].br();
Point corner_4_small(faces[i].br().x,faces[i].tl().y);
Point center_position_of_face((faces[i].br().x*float(image.cols)/float(smallIMG.cols)+faces[i].tl().x*float(image.cols)/float(smallIMG.cols))/2,(faces[i].tl().y*float(image.rows)/float(smallIMG.rows)+faces[i].br().y*float(image.rows)/float(smallIMG.rows))/2);
Point corner_1(faces[i].br().x*float(image.cols)/float(smallIMG.cols),faces[i].tl().y*float(image.rows)/float(smallIMG.rows));
Point corner_2(faces[i].tl().x*float(image.cols)/float(smallIMG.cols),faces[i].tl().y*float(image.rows)/float(smallIMG.rows));
Point corner_3(faces[i].br().x*float(image.cols)/float(smallIMG.cols),faces[i].br().y*float(image.rows)/float(smallIMG.rows));
Point corner_4(faces[i].tl().x*float(image.cols)/float(smallIMG.cols),faces[i].br().y*float(image.rows)/float(smallIMG.rows));
face_big.br() = corner_2;
face_big.tl() = corner_3;
face_big.width = corner_1.x-corner_2.x;
face_big.height = corner_1.y-corner_4.y;
Rect test(corner_2,corner_3);
face_big = test;
face_big += Size(15,70);
rectangle(smallIMG, faces[i], CV_RGB(0,255,0),4,8,0);
rectangle(image,test , CV_RGB(45,255,102),8,8,0);
circle(smallIMG, center_position_of_face_small, 8, CV_RGB(128,128,128),8,8,0);
circle(smallIMG, corner_1_small, 1, CV_RGB(128,128,128),8,8,0);
circle(smallIMG, corner_2_small, 1, CV_RGB(128,128,128),8,8,0);
circle(smallIMG, corner_3_small, 1, CV_RGB(128,128,128),8,8,0);
circle(smallIMG, corner_4_small, 1, CV_RGB(128,128,128),8,8,0);
circle(image, center_position_of_face, 8, CV_RGB(128,128,128),8,8,0);
circle(image, corner_1, 1, CV_RGB(128,128,128),8,8,0);
circle(image, corner_2, 1, CV_RGB(128,128,128),8,8,0);
circle(image, corner_3, 1, CV_RGB(128,128,128),8,8,0);
circle(image, corner_4, 1, CV_RGB(128,128,128),8,8,0);
line(smallIMG, center_position_of_face_small, center_of_frame_small, CV_RGB(0,200,0),1,8);
line(image, center_position_of_face, center_of_frame, CV_RGB(0,200,0),1,8);
faces[k++] = faces[i];
}
faces.resize(k);
flip(image, image, 1);
flip(smallIMG,smallIMG,1);
flip(ROI_IMAGE,ROI_IMAGE,1);
imshow("Facedetection", image);
imshow("FACEdetection - small", smallIMG);
moveWindow("Facedetection",0,0);
if(face_big.area() != 0 && (0 <= face_big.x && 0 <= face_big.width && face_big.x + face_big.width <= image.cols && 0 <= face_big.y && 0 <= face_big.height && face_big.y + face_big.height <= image.rows) )
{
ROI = ROI_IMAGE(face_big);
imwrite("/home/kratnarajah/Desktop/template_matching_images/ROI.jpg",ROI);
faceWidth = face_big.width;
faceHeight = face_big.height;
face = face_big;
}
cout << "face - info: " << face_big.area() << endl;
waitKey(1);
int cc = getch();
if(cc == 'y' || redetect == true)
{
if(cc == 'y')
{
cout << "keypress registered" << endl;
}
else if(redetect == true)
{
cout << "redetect applied" << endl;
}
state = true;
redetect = false;
destroyAllWindows();
}
int key = getch();
if(key == 'r')
{
cout << "keypress registered" << endl;
cout << "RESET STATE" << endl;
pos[0] = 0;
pos[1] = 0;
vel[0] = 0.1;
vel[1] = 0.1;
ms.position = pos;
ms.velocity = vel;
facedetect_pub.publish(ms);
}
}
else
{
double start,duration;
start = ros::Time::now().toSec();
if(posSendMsgCounter > posisit)
{
posSendMsgCounter = 0;
cout << "posSendMsgCounter reset" << endl;
}
posSendMsgCounter++;
image = cv_ptr->image;
flip(image, image, 1);
Mat mask(image.size(),CV_8UC1,Scalar(0));
mask(face).setTo(Scalar(255));
flip(mask,mask,1);
cvtColor(image,gray,COLOR_BGR2GRAY);
equalizeHist(gray,gray);
Rect bounds(0,0,image.rows,image.cols);
face = face & bounds;
if(!(1 <= face.x && face.x + face.width <= image.cols-1 && 1 <= face.y && face.y + face.height <= image.rows-1)) // Try to react when it comes outside => it has to leave the mask where it last was last found within the image.
{
cout << "I am out" << endl;
backup_center = Mean(goodFeatures);
Rect NewBOx;
NewBOx.x = backup_center.x-faceWidth/2;
NewBOx.y = backup_center.y-faceHeight/2;
NewBOx.width = face.width;
NewBOx.height = face.height;
face = NewBOx;
circle(image,backup_center,10,Scalar(255,0,255),-1,12,0);
}
cout << "face.x: " << face.x << " should be bigger than 0 "<<endl;
cout << "face.x + face.width: " << face.x + face.width << " <= " << image.cols << endl;
cout << "face.y: " << face.y << " should be bigger than 0 "<<endl;
cout << "face.y + face.height: " << face.y + face.height << " <= " << image.rows << endl;
imshow("Mask", mask);
circle(cv_ptr->image, center_of_frame, 1, CV_RGB(0,255,255),8,8,0);
static uint64_t c;
if (c++ == 0)
{
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 << "points[1].size(): "<< points[1].size()<< endl;
reinit = false;
cout << "i am here" << endl;
if(points[1].size() < 50)
{
cout << "Is below threshold for amount of points" << endl;
c = 0;
}
}
if( !points[0].empty() )
{
vector<uchar> status;
vector<float> err;
vector<uchar> status2;
vector<float> err2;
if(prevGray.empty())
{
gray.copyTo(prevGray);
}
//Lukas kanade optilcal flow + Forward - backward error
calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err, winSize, 3, termcrit, OPTFLOW_LK_GET_MIN_EIGENVALS, qualitylevel);
calcOpticalFlowPyrLK(gray, prevGray, points[1], point_final, status2, err2, winSize, 3, termcrit, OPTFLOW_LK_GET_MIN_EIGENVALS, qualitylevel);
Point2f center = Mean(point_final);
Point2f center2 = Mean(points[1]);
if(std_dev(point_final) && std_dev(points[1])) //tracking mode
{
if(igen == true)
{
cout << "Kill yo self..... !!!!!!!!! arghr" << endl;
igen = false;
}
cout << "Alowable deviation" << endl;
goodFeatures.push_back(center); //Forward - backward error
goodFeatures.push_back(center2); // Forward tracking mean
sendCommands = true;
ROI,mask = gray(face);
imwrite("/home/kratnarajah/Desktop/template_matching_images/ROI.jpg",mask);
if(goodFeatures.empty())
{
backup_center = center;
backup_center.y =+ 40;
}
else
{
backup_center = Mean(goodFeatures);
Rect NewBOx;
NewBOx.x = backup_center.x-faceWidth/2;
NewBOx.y = backup_center.y-faceHeight/2;
NewBOx.width = face.width;
NewBOx.height = face.height;
face = NewBOx;
}
cout << "face.tl(): " << face.tl() << endl;
cout << "backup_center: " <<backup_center << endl;
if(std_dev_determine(point_final)>120 ||std_dev_determine(point_final)>120)
{
frames++;
cout << "Issues may occur (frame++) => taking precaution by removing outliers" << endl;
if((0 <= face.x && 0 <= face.width && face.x + face.width <= gray.cols-300 && 0 <= face.y && 0 <= face.height && face.y + face.height <= gray.rows-300) )
{
cout << "increasin rect" << endl;
face+= Size(1,1);
}
else
{
cout << "size limit reached" << endl;
}
}
//rectangle(image,face,Scalar(255,255,0));
}
else // performing reinit
{
cout << "Reinit performed" << endl;
//cout << std_dev_determine(point_final) <<" , "<< std_dev_determine(points[1]) <<" , "<< std_dev_determine(goodFeatures)<< endl;
Rect redetected;
redetected = box(gray,backup_center);
cout <<redetected.width <<" , " <<redetected.height << endl;
if(!(0 <= redetected.x && 100 <= redetected.width && redetected.x + redetected.width <= gray.cols && 0 <= redetected.y && 100 <= redetected.height && redetected.y + redetected.height <= gray.rows))
{
cout << "Not able to redetect - use last detected center!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!: " << endl;
Rect NewBOx;
NewBOx.x = backup_center.x-face.width/2;
NewBOx.y = backup_center.y-face.height/2;
NewBOx.width = face.width;
NewBOx.height = face.height;
face = NewBOx;
}
else
{
face = redetected;
}
points[0].clear();
points[1].clear();
point_final.clear();
goodFeatures.clear();
status.clear();
status2.clear();
rectangle(image,face,Scalar(255,255,0));
mask(face).setTo(Scalar(255));
vector<Point2f> tmp;
goodFeaturesToTrack(gray, tmp, MAX_COUNT, 0.01, 10, mask, 3, 0, 0.04);
if((tmp.size() > 0))
{
cout << "Too few goodFeaturesToTrack so Corner Subpix not initiated: :(" << endl;
cornerSubPix( gray, tmp, winSize, cvSize(-1,-1), termcrit);
}
points[1].insert(points[1].end(),tmp.begin(),tmp.end());
cout <<"points size: " <<points[1].size() << endl;
//calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err, winSize, 3, termcrit, OPTFLOW_LK_GET_MIN_EIGENVALS, qualitylevel);
calcOpticalFlowPyrLK(gray, prevGray, points[1], point_final, status2, err2, winSize, 3, termcrit, OPTFLOW_LK_GET_MIN_EIGENVALS, qualitylevel);
igen = true;
sendCommands = false;
cout << "STD_dev of new pointCloud: " << std_dev_determine(points[1]) << endl;
cout << "Not performing FORWARD AND BACKWARDS ERROR ESTIMATION - simple lukas kanade" << endl;
}
for( int i = 0; i < points[1].size(); i++ )
{
if (igen == true)
{
if(status2[i])
{
goodFeatures.push_back(point_final[i]);
}
}
if( status[i] && status2[i] && igen == false ) // Forward - backward Error estimation => Good values are only taken.
{
if(std_dev_determine(point_final) < 80)//std_dev_determine(points[1]))
{
if(euclideanDistance(Mean(point_final) - point_final[i]) < face.width || euclideanDistance(Mean(point_final) - point_final[i]) <face.height)//std_dev_determine(point_final))
{
goodFeatures.push_back(point_final[i]);
}
else
{
point_final.erase(point_final.begin()+i);
}
}
else
{
if(euclideanDistance(Mean(points[1]) - points[1][i]) < face.width ||euclideanDistance(Mean(points[1]) - points[1][i]) < face.height)//std_dev_determine(points[1])*(2*frames))
{
goodFeatures.push_back(points[1][i]);
}
else
{
points[1].erase(points[1].begin()+i);
}
}
}
}
if(goodFeatures.size() > 40 && igen == true) //Setting Backup center;
{
backup_center = center;
}
cout << "GoodFeatures.size(): "<< goodFeatures.size() << endl;
for(int j = 0; j<500; j++)
{
if(igen == false)
{
circle(image,goodFeatures[j],4, Scalar(255,0,0),-1,8,0);
}
if(igen == true)
{
circle(image,goodFeatures[j],4, Scalar(0,255,0),-1,8,0);
circle(image,Mean(goodFeatures),4, Scalar(0,255,0),-1,8,0);
}
}
//circle(image,goodFeatures[0],9, Scalar(0,0,255),-1,12,0);
//circle(image,goodFeatures[1],4, Scalar(0,255,0),-1,8,0);
//circle(image,Mean(goodFeatures),10,Scalar(255,0,255),-1,12,0);
circle(image,backup_center,10,Scalar(0,255,255),-1,12,0);
displacement = make_pair(Mean(goodFeatures).x-center_of_frame.x,Mean(goodFeatures).y-center_of_frame.y);
line(image, backup_center , center_of_frame, CV_RGB(0,200,0),1,8);
}
if( goodFeatures.size() < 10 )
{
cout << "Adding extra because goodFeature count getting low - Lagging" << endl;
vector<Point2f> tmp;
tmp.push_back(point);
Rect templ = face;
if(templ.width < image.cols && templ.height < image.rows )
{
templ+= Size(10,10);
}
Mat maskINC(image.size(),CV_8UC1,Scalar(0));
maskINC(face).setTo(Scalar(255));
goodFeaturesToTrack(gray, tmp, MAX_COUNT, 0.01, 10, maskINC, 3, 0, 0.04);
cout << "size tmp: " << tmp.size() << endl;
if((tmp.size() > 0))
{
cornerSubPix( gray, tmp, winSize, cvSize(-1,-1), termcrit);
}
goodFeatures.insert(goodFeatures.end(),tmp.begin(),tmp.end());
tmp.clear();
}
std::swap(points[1], points[0]);
cv::swap(prevGray, gray);
goodFeatures.clear();
duration = ros::Time::now().toSec() - start;
| | 4 | No.4 Revision |
Hi I am trying to overrule a openCV exception which occurs when the Mask which is applied on a image, lies outside of the dimension image... But i cannot make it do something else than catch that exeption when it happens... How do i make it overrule this problem?
This is the exception I want overruled:
OpenCV Error: Assertion failed (0 <= roi.x && 0 <= roi.width && roi.x + roi.width <= m.cols && 0 <= roi.y && 0 <= roi.height && roi.y + roi.height <= m.rows) in Mat, file /build/buildd/opencv-2.4.8+dfsg1/modules/core/src/matrix.cpp, line 323 terminate called after throwing an instance of 'cv::Exception' what(): /build/buildd/opencv-2.4.8+dfsg1/modules/core/src/matrix.cpp:323: error: (-215) 0 <= roi.x && 0 <= roi.width && roi.x + roi.width <= m.cols && 0 <= roi.y && 0 <= roi.height && roi.y + roi.height <= m.rows in function Mat
code:
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;
}
double ImageConverter::euclideanDistance(Point2f point1)
{
double match=0;
match += sqrt((point1.x - 0) * (point1.x - 0)+ (point1.y - 0) * (point1.y - 0));
return match;
}
Point2f ImageConverter::Mean(vector<Point2f> points)
{
Mat mean_;
reduce(points,mean_,CV_REDUCE_AVG,1);
Point2f mean(mean_.at<float>(0,0),mean_.at<float>(0,1));
//cout << "Mean: " << mean_ << endl;
return mean;
}
bool ImageConverter::std_dev(vector<Point2f> points)
{
double Std_dev;
Point2f mean = Mean(points);
for(int i = 0; i < points.size(); i++)
{
Std_dev += pow(euclideanDistance(points[i]-mean),2);
}
Std_dev = Std_dev/points.size();
Std_dev = sqrt(Std_dev);
cout << "STD_DEV: " << Std_dev << endl;
if(Std_dev <= face.width)
{
cout << "Still reasonable fine " << endl;
return true; //Threshold has to be added to know when it should re init the goodFeaturesToTrack Function.
}
else
{
cout <<"you are fucked - too much drifting " << endl;
return false;
}
}
double ImageConverter::std_dev_determine(vector<Point2f> points)
{
double Std_dev;
Point2f mean = Mean(points);
for(int i = 0; i < points.size(); i++)
{
Std_dev += pow(euclideanDistance(points[i]-mean),2);
}
Std_dev = Std_dev/points.size();
Std_dev = sqrt(Std_dev);
return Std_dev;
}
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;
igen = false;
sendCommands = false;
faceWidth = 0;
faceHeight = 0;
frames = 0;
face.x = 0;
face.y = 0;
face.width = 250;
face.height = 250;
posSendMsgCounter = 0;
goodFeatures.reserve(500);
String frontalface_alt2_XML = "/usr/share/opencv/haarcascades/haarcascade_frontalface_alt2.xml";
String frontal_alt_XML = "/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml";
String frontal_alt_tree_XML = "/usr/share/opencv/haarcascades/haarcascade_frontalface_alt_tree.xml";
String frontal_face_xml = "/usr/share/opencv/haarcascades/haarcascade_frontalface_default.xml";
if( !frontalface_alt2.load( frontalface_alt2_XML ) )
{
cout << "face_cascade Cascade Error" << endl;
};
if( !frontal_alt.load( frontal_alt_XML ) )
{
cout << "profileface Cascade Error" << endl;
};
if(!frontal_alt_tree.load( frontal_alt_tree_XML ))
{
cout << "nose Cascade Error" << endl;
}
if(!frontal_face.load( frontal_face_xml ))
{
cout << "nose Cascade Error" << 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))
{
image = cv_ptr->image;
//ROI_IMAGE = cv_ptr->image;
smallIMG = image;
cvtColor(smallIMG,smallIMG,COLOR_BGR2GRAY);
cv::equalizeHist(smallIMG, smallIMG);
ROI_IMAGE = smallIMG;
Size size(200,200);
resize(smallIMG,smallIMG,size);
vector<Rect> faces(1);
Rect face_big;
center_of_frame.x = image.size().width/2;
center_of_frame.y = image.size().height/2;
center_of_frame_small.x = smallIMG.size().width/2;
center_of_frame_small.y = smallIMG.size().height/2;
pair<Point, Point> corners;
pair<double,double> displacement;
double displacement_pixel_x;
double displacement_pixel_y;
pair<Rect, bool> response;
//if (image.type()!= 8)
//{
//cvtColor(image,image, CV_8U);
//}
//-- 1. Load the cascades
circle(image, center_of_frame, 1, CV_RGB(0,255,255),8,8,0);
circle(smallIMG, center_of_frame_small, 1, CV_RGB(0,255,255),3,3,0);
//-- Detect faces
frontalface_alt2.detectMultiScale(smallIMG, faces, 1.1, 2, 0, Size(10, 10) );
//frontal_alt.detectMultiScale( smallIMG, faces, 1.1, 2, 0, Size(0, 0) );
//frontal_alt_tree.detectMultiScale( smallIMG, faces, 1.1, 2, 0, Size(0, 0) );
//frontal_face.detectMultiScale( smallIMG, faces, 1.1, 2, 0, Size(0, 0) );
int i,k;
for( i = k = 0; i<faces.size() ; i++)
{
Point center_position_of_face_small((faces[i].br().x+faces[i].tl().x)/2,(faces[i].tl().y+faces[i].br().y)/2);
Point corner_1_small(faces[i].tl().x,faces[i].br().y);
Point corner_2_small = faces[i].tl();
Point corner_3_small = faces[i].br();
Point corner_4_small(faces[i].br().x,faces[i].tl().y);
Point center_position_of_face((faces[i].br().x*float(image.cols)/float(smallIMG.cols)+faces[i].tl().x*float(image.cols)/float(smallIMG.cols))/2,(faces[i].tl().y*float(image.rows)/float(smallIMG.rows)+faces[i].br().y*float(image.rows)/float(smallIMG.rows))/2);
Point corner_1(faces[i].br().x*float(image.cols)/float(smallIMG.cols),faces[i].tl().y*float(image.rows)/float(smallIMG.rows));
Point corner_2(faces[i].tl().x*float(image.cols)/float(smallIMG.cols),faces[i].tl().y*float(image.rows)/float(smallIMG.rows));
Point corner_3(faces[i].br().x*float(image.cols)/float(smallIMG.cols),faces[i].br().y*float(image.rows)/float(smallIMG.rows));
Point corner_4(faces[i].tl().x*float(image.cols)/float(smallIMG.cols),faces[i].br().y*float(image.rows)/float(smallIMG.rows));
face_big.br() = corner_2;
face_big.tl() = corner_3;
face_big.width = corner_1.x-corner_2.x;
face_big.height = corner_1.y-corner_4.y;
Rect test(corner_2,corner_3);
face_big = test;
face_big += Size(15,70);
rectangle(smallIMG, faces[i], CV_RGB(0,255,0),4,8,0);
rectangle(image,test , CV_RGB(45,255,102),8,8,0);
circle(smallIMG, center_position_of_face_small, 8, CV_RGB(128,128,128),8,8,0);
circle(smallIMG, corner_1_small, 1, CV_RGB(128,128,128),8,8,0);
circle(smallIMG, corner_2_small, 1, CV_RGB(128,128,128),8,8,0);
circle(smallIMG, corner_3_small, 1, CV_RGB(128,128,128),8,8,0);
circle(smallIMG, corner_4_small, 1, CV_RGB(128,128,128),8,8,0);
circle(image, center_position_of_face, 8, CV_RGB(128,128,128),8,8,0);
circle(image, corner_1, 1, CV_RGB(128,128,128),8,8,0);
circle(image, corner_2, 1, CV_RGB(128,128,128),8,8,0);
circle(image, corner_3, 1, CV_RGB(128,128,128),8,8,0);
circle(image, corner_4, 1, CV_RGB(128,128,128),8,8,0);
line(smallIMG, center_position_of_face_small, center_of_frame_small, CV_RGB(0,200,0),1,8);
line(image, center_position_of_face, center_of_frame, CV_RGB(0,200,0),1,8);
faces[k++] = faces[i];
}
faces.resize(k);
flip(image, image, 1);
flip(smallIMG,smallIMG,1);
flip(ROI_IMAGE,ROI_IMAGE,1);
imshow("Facedetection", image);
imshow("FACEdetection - small", smallIMG);
moveWindow("Facedetection",0,0);
if(face_big.area() != 0 && (0 <= face_big.x && 0 <= face_big.width && face_big.x + face_big.width <= image.cols && 0 <= face_big.y && 0 <= face_big.height && face_big.y + face_big.height <= image.rows) )
{
ROI = ROI_IMAGE(face_big);
imwrite("/home/kratnarajah/Desktop/template_matching_images/ROI.jpg",ROI);
faceWidth = face_big.width;
faceHeight = face_big.height;
face = face_big;
}
cout << "face - info: " << face_big.area() << endl;
waitKey(1);
int cc = getch();
if(cc == 'y' || redetect == true)
{
if(cc == 'y')
{
cout << "keypress registered" << endl;
}
else if(redetect == true)
{
cout << "redetect applied" << endl;
}
state = true;
redetect = false;
destroyAllWindows();
}
int key = getch();
if(key == 'r')
{
cout << "keypress registered" << endl;
cout << "RESET STATE" << endl;
pos[0] = 0;
pos[1] = 0;
vel[0] = 0.1;
vel[1] = 0.1;
ms.position = pos;
ms.velocity = vel;
facedetect_pub.publish(ms);
}
}
else
{
double start,duration;
start = ros::Time::now().toSec();
if(posSendMsgCounter > posisit)
{
posSendMsgCounter = 0;
cout << "posSendMsgCounter reset" << endl;
}
posSendMsgCounter++;
image = cv_ptr->image;
flip(image, image, 1);
Mat mask(image.size(),CV_8UC1,Scalar(0));
mask(face).setTo(Scalar(255));
flip(mask,mask,1);
cvtColor(image,gray,COLOR_BGR2GRAY);
equalizeHist(gray,gray);
Rect bounds(0,0,image.rows,image.cols);
face = face & bounds;
if(!(1 <= face.x && face.x + face.width <= image.cols-1 && 1 <= face.y && face.y + face.height <= image.rows-1)) // Try to react when it comes outside => it has to leave the mask where it last was last found within the image.
{
cout << "I am out" << endl;
backup_center = Mean(goodFeatures);
Rect NewBOx;
NewBOx.x = backup_center.x-faceWidth/2;
NewBOx.y = backup_center.y-faceHeight/2;
NewBOx.width = face.width;
NewBOx.height = face.height;
face = NewBOx;
circle(image,backup_center,10,Scalar(255,0,255),-1,12,0);
}
cout << "face.x: " << face.x << " should be bigger than 0 "<<endl;
cout << "face.x + face.width: " << face.x + face.width << " <= " << image.cols << endl;
cout << "face.y: " << face.y << " should be bigger than 0 "<<endl;
cout << "face.y + face.height: " << face.y + face.height << " <= " << image.rows << endl;
imshow("Mask", mask);
circle(cv_ptr->image, center_of_frame, 1, CV_RGB(0,255,255),8,8,0);
static uint64_t c;
if (c++ == 0)
{
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 << "points[1].size(): "<< points[1].size()<< endl;
reinit = false;
cout << "i am here" << endl;
if(points[1].size() < 50)
{
cout << "Is below threshold for amount of points" << endl;
c = 0;
}
}
if( !points[0].empty() )
{
vector<uchar> status;
vector<float> err;
vector<uchar> status2;
vector<float> err2;
if(prevGray.empty())
{
gray.copyTo(prevGray);
}
//Lukas kanade optilcal flow + Forward - backward error
calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err, winSize, 3, termcrit, OPTFLOW_LK_GET_MIN_EIGENVALS, qualitylevel);
calcOpticalFlowPyrLK(gray, prevGray, points[1], point_final, status2, err2, winSize, 3, termcrit, OPTFLOW_LK_GET_MIN_EIGENVALS, qualitylevel);
Point2f center = Mean(point_final);
Point2f center2 = Mean(points[1]);
if(std_dev(point_final) && std_dev(points[1])) //tracking mode
{
if(igen == true)
{
cout << "Kill yo self..... !!!!!!!!! arghr" << endl;
igen = false;
}
cout << "Alowable deviation" << endl;
goodFeatures.push_back(center); //Forward - backward error
goodFeatures.push_back(center2); // Forward tracking mean
sendCommands = true;
ROI,mask = gray(face);
imwrite("/home/kratnarajah/Desktop/template_matching_images/ROI.jpg",mask);
if(goodFeatures.empty())
{
backup_center = center;
backup_center.y =+ 40;
}
else
{
backup_center = Mean(goodFeatures);
Rect NewBOx;
NewBOx.x = backup_center.x-faceWidth/2;
NewBOx.y = backup_center.y-faceHeight/2;
NewBOx.width = face.width;
NewBOx.height = face.height;
face = NewBOx;
}
cout << "face.tl(): " << face.tl() << endl;
cout << "backup_center: " <<backup_center << endl;
if(std_dev_determine(point_final)>120 ||std_dev_determine(point_final)>120)
{
frames++;
cout << "Issues may occur (frame++) => taking precaution by removing outliers" << endl;
if((0 <= face.x && 0 <= face.width && face.x + face.width <= gray.cols-300 && 0 <= face.y && 0 <= face.height && face.y + face.height <= gray.rows-300) )
{
cout << "increasin rect" << endl;
face+= Size(1,1);
}
else
{
cout << "size limit reached" << endl;
}
}
//rectangle(image,face,Scalar(255,255,0));
}
else // performing reinit
{
cout << "Reinit performed" << endl;
//cout << std_dev_determine(point_final) <<" , "<< std_dev_determine(points[1]) <<" , "<< std_dev_determine(goodFeatures)<< endl;
Rect redetected;
redetected = box(gray,backup_center);
cout <<redetected.width <<" , " <<redetected.height << endl;
if(!(0 <= redetected.x && 100 <= redetected.width && redetected.x + redetected.width <= gray.cols && 0 <= redetected.y && 100 <= redetected.height && redetected.y + redetected.height <= gray.rows))
{
cout << "Not able to redetect - use last detected center!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!: " << endl;
Rect NewBOx;
NewBOx.x = backup_center.x-face.width/2;
NewBOx.y = backup_center.y-face.height/2;
NewBOx.width = face.width;
NewBOx.height = face.height;
face = NewBOx;
}
else
{
face = redetected;
}
points[0].clear();
points[1].clear();
point_final.clear();
goodFeatures.clear();
status.clear();
status2.clear();
rectangle(image,face,Scalar(255,255,0));
mask(face).setTo(Scalar(255));
vector<Point2f> tmp;
goodFeaturesToTrack(gray, tmp, MAX_COUNT, 0.01, 10, mask, 3, 0, 0.04);
if((tmp.size() > 0))
{
cout << "Too few goodFeaturesToTrack so Corner Subpix not initiated: :(" << endl;
cornerSubPix( gray, tmp, winSize, cvSize(-1,-1), termcrit);
}
points[1].insert(points[1].end(),tmp.begin(),tmp.end());
cout <<"points size: " <<points[1].size() << endl;
//calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err, winSize, 3, termcrit, OPTFLOW_LK_GET_MIN_EIGENVALS, qualitylevel);
calcOpticalFlowPyrLK(gray, prevGray, points[1], point_final, status2, err2, winSize, 3, termcrit, OPTFLOW_LK_GET_MIN_EIGENVALS, qualitylevel);
igen = true;
sendCommands = false;
cout << "STD_dev of new pointCloud: " << std_dev_determine(points[1]) << endl;
cout << "Not performing FORWARD AND BACKWARDS ERROR ESTIMATION - simple lukas kanade" << endl;
}
for( int i = 0; i < points[1].size(); i++ )
{
if (igen == true)
{
if(status2[i])
{
goodFeatures.push_back(point_final[i]);
}
}
if( status[i] && status2[i] && igen == false ) // Forward - backward Error estimation => Good values are only taken.
{
if(std_dev_determine(point_final) < 80)//std_dev_determine(points[1]))
{
if(euclideanDistance(Mean(point_final) - point_final[i]) < face.width || euclideanDistance(Mean(point_final) - point_final[i]) <face.height)//std_dev_determine(point_final))
{
goodFeatures.push_back(point_final[i]);
}
else
{
point_final.erase(point_final.begin()+i);
}
}
else
{
if(euclideanDistance(Mean(points[1]) - points[1][i]) < face.width ||euclideanDistance(Mean(points[1]) - points[1][i]) < face.height)//std_dev_determine(points[1])*(2*frames))
{
goodFeatures.push_back(points[1][i]);
}
else
{
points[1].erase(points[1].begin()+i);
}
}
}
}
if(goodFeatures.size() > 40 && igen == true) //Setting Backup center;
{
backup_center = center;
}
cout << "GoodFeatures.size(): "<< goodFeatures.size() << endl;
for(int j = 0; j<500; j++)
{
if(igen == false)
{
circle(image,goodFeatures[j],4, Scalar(255,0,0),-1,8,0);
}
if(igen == true)
{
circle(image,goodFeatures[j],4, Scalar(0,255,0),-1,8,0);
circle(image,Mean(goodFeatures),4, Scalar(0,255,0),-1,8,0);
}
}
//circle(image,goodFeatures[0],9, Scalar(0,0,255),-1,12,0);
//circle(image,goodFeatures[1],4, Scalar(0,255,0),-1,8,0);
//circle(image,Mean(goodFeatures),10,Scalar(255,0,255),-1,12,0);
circle(image,backup_center,10,Scalar(0,255,255),-1,12,0);
displacement = make_pair(Mean(goodFeatures).x-center_of_frame.x,Mean(goodFeatures).y-center_of_frame.y);
line(image, backup_center , center_of_frame, CV_RGB(0,200,0),1,8);
}
if( goodFeatures.size() < 10 )
{
cout << "Adding extra because goodFeature count getting low - Lagging" << endl;
vector<Point2f> tmp;
tmp.push_back(point);
Rect templ = face;
if(templ.width < image.cols && templ.height < image.rows )
{
templ+= Size(10,10);
}
Mat maskINC(image.size(),CV_8UC1,Scalar(0));
maskINC(face).setTo(Scalar(255));
goodFeaturesToTrack(gray, tmp, MAX_COUNT, 0.01, 10, maskINC, 3, 0, 0.04);
cout << "size tmp: " << tmp.size() << endl;
if((tmp.size() > 0))
{
cornerSubPix( gray, tmp, winSize, cvSize(-1,-1), termcrit);
}
goodFeatures.insert(goodFeatures.end(),tmp.begin(),tmp.end());
tmp.clear();
}
std::swap(points[1], points[0]);
cv::swap(prevGray, gray);
goodFeatures.clear();
duration = ros::Time::now().toSec() - start;