void FaceDetector::imageCallback(const sensor_msgs::Image::ConstPtr& image_ptr)
{
private_nh_.param("/qbo_face_tracking/send_to_recognizer", send_to_face_recognizer_, false);
/*
* Cretate a CvImage pointer to get cv::Mat representation of image
*/
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(image_ptr, sensor_msgs::image_encodings::RGB8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::Mat image_received = (cv_ptr->image).clone();
image_size_ = cv_ptr->image.size();
/*
* Creating structure to publish nose color
*/
qbo_arduqbo::Nose nose;
nose.header.stamp = ros::Time::now();
nose.color=1;
string detection_type = "";
if(face_detected_bool_) //If face was detected in previous iteration - use CAM shift
{
if(detectFaceCamShift(image_received) != 0)
{
face_detected_bool_ = false;
//Reset cam_shift_detected
cam_shift_detected_count_ = 0;
cam_shift_undetected_count_++;
}
else
{
detection_type = "CAMSHIFT";
//Update cam_shift counter
cam_shift_detected_count_++;
cam_shift_undetected_count_ = 0;
}
}
if(!face_detected_bool_) //If face was not detected - use Haar Cascade
{
vector<cv::Rect> faces_roi;
detectFacesHaar(image_received, faces_roi);
if(faces_roi.size() > 0) //If Haar cascade classifier found a face
{
face_detected_bool_ = true;
track_object_ = false;
//Changed
detected_face_roi_ = faces_roi[0];
detected_face_ = cv_ptr->image(detected_face_roi_);
//Adjust face ratio because Haar Cascade always returns a square
face_ratio_ = 1.3*detected_face_roi_.height/detected_face_roi_.width;
detection_type = "HAAR";
}
}
if(!face_detected_bool_ && exist_alternative_) //If face was not detected - use Haar Cascade
{
vector<cv::Rect> faces_roi;
detectFacesAltHaar(image_received, faces_roi);
if(faces_roi.size() > 0) //If Haar cascade classifier found a face
{
face_detected_bool_ = true;
track_object_ = false;
//Changed
detected_face_roi_ = faces_roi[0];
detected_face_ = cv_ptr->image(detected_face_roi_);
//Adjust face ratio because Haar Cascade always returns a square
face_ratio_ = 1.3*detected_face_roi_.height/detected_face_roi_.width;
detection_type = "HAAR";
}
}
/*
* Kalman filter for Face pos estimation
*/
kalman_filter_.predict();
if(face_detected_bool_) //IF face detected, use measured position to update kalman filter
{
if(undetected_count_>=undetected_threshold_)
{
cv::Mat new_state(4,1, CV_32FC1);
new_state.at<float>(0,0) = float(detected_face_roi_.x+detected_face_roi_.width/2.);
new_state.at<float>(1,0) = float(detected_face_roi_.y+detected_face_roi_.height/2.);
new_state.at<float>(2,0) = 0.;
new_state.at<float>(3,0) = 0.;
new_state.copyTo(kalman_filter_.statePre);
new_state.copyTo(kalman_filter_.statePost);
}
else
{
float u_vel = float(detected_face_roi_.x+detected_face_roi_.width/2.)-kalman_filter_.statePost.at<float>(0,0);
float v_vel = float(detected_face_roi_.y+detected_face_roi_.height/2.)-kalman_filter_.statePost.at<float>(1,0);
cv::Mat measures(4,1, CV_32FC1);
measures.at<float>(0,0) = float(detected_face_roi_.x+detected_face_roi_.width/2.);
measures.at<float>(1,0) = float(detected_face_roi_.y+detected_face_roi_.height/2.);
measures.at<float>(2,0) = u_vel;
measures.at<float>(3,0) = v_vel;
kalman_filter_.correct(measures);
}
}
else //If face haven't been found, use only Kalman prediction
{
kalman_filter_.statePre.copyTo(kalman_filter_.statePost);
kalman_filter_.errorCovPre.copyTo(kalman_filter_.errorCovPost);
}
/*
* Compute head distance
*/
float head_distance;
if(!face_detected_bool_)
{
if(head_distances_.size()!=0)
head_distance=head_distances_[0];//100;
else
head_distance = 5;
}
else
{
head_distance=calcDistanceToHead(detected_face_, kalman_filter_);
// ROS_ERROR("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxhead_distance:%f", head_distance);
}
if(head_distances_.size()==0)
{
for(int i=0;i<10;i++)
head_distances_.push_back(head_distance);
}
else
{
head_distances_.pop_back();
head_distances_.insert(head_distances_.begin(),head_distance);
}
head_distance=0.0; //Reuse variable to compute mean head distance
//Use mean distance of last measured head distances
for(unsigned int i=0;i<head_distances_.size();i++)
{
//ROS_ERROR("xxxxxxxxxx0xxxxxhead_distance:%f, s_:%f\n", head_distance, head_distances_[i]);
head_distance+=head_distances_[i];
}
head_distance=head_distance/head_distances_.size();
//ROS_ERROR("xxxxxxxxxx2xxxxxhead_distance:%f", head_distance);
//Update undetected count
if(!face_detected_bool_)
{ undetected_count_++;
}
else
{
undetected_count_ = 0;
}
//Create Face Pos and Size message
qbo_face_msgs::FacePosAndDist message;
message.image_width=cv_ptr->image.cols;
message.image_height=cv_ptr->image.rows;
message.type_of_tracking = detection_type;
if(undetected_count_<undetected_threshold_) //If head have been recently detected, use Kalman filter prediction
{
message.face_detected = true;
message.u = kalman_filter_.statePost.at<float>(0,0) - cv_ptr->image.cols/2.;
message.v = kalman_filter_.statePost.at<float>(1,0) - cv_ptr->image.rows/2.;
message.distance_to_head = float(head_distance);
}
else //If head has not been recently detected, face detection have failed
{
message.u = default_pos_.x;
message.v = default_pos_.y;
message.face_detected = false;
}
if(face_detected_bool_)
{
//Publish head to topic
cv_ptr->image = detected_face_;
face_pub_.publish(cv_ptr->toImageMsg());
if(send_to_face_recognizer_)
sendToRecognizer();
//Change nose color
nose.color=4; //If face detected - Blue
if(head_distance<distance_threshold_)
{
nose.color=2; //If face is near - Green
}
}
//Publish nose color
nose_color_pub_.publish(nose);
/*
* Draws for the Viewer
* Velocity vector, face rectangle and face center
*/
int pred_x = int(message.u) + int(message.image_width)/2;
int pred_y = int(message.v) + int(message.image_height)/2;
cv::Point pred_kal = cv::Point(pred_x, pred_y);
cv::Point tip_u_vel = cv::Point(pred_kal.x + (kalman_filter_.statePost.at<float>(2,0))/1., pred_kal.y);
cv::Point tip_v_vel = cv::Point(pred_kal.x, pred_kal.y + (kalman_filter_.statePost.at<float>(3,0))/1.);
//Draw face center and rectangle
cv::circle(image_received, pred_kal,3,cv::Scalar(0,0,255), 3);
cv::rectangle(image_received, detected_face_roi_, cv::Scalar(255,0,0), 2);
if(tip_u_vel.x >= 0 && tip_u_vel.y >= 0 && tip_u_vel.x < image_received.cols && tip_u_vel.y < image_received.rows)
{
cv::line(image_received, pred_kal, tip_u_vel, cv::Scalar(0,255,0), 2);
}
if(tip_v_vel.x >= 0 && tip_v_vel.y >= 0 && tip_v_vel.x < image_received.cols && tip_v_vel.y < image_received.rows)
{
cv::line(image_received, pred_kal, tip_v_vel, cv::Scalar(255,0,0), 2);
}
//Draw name of person in image
if(print_recognized_face_)
cv::putText(image_received, name_detected_, cv::Point(40,40), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0,0,255), 3);
/*
* Publish Image viewer
*/
cv_bridge::CvImagePtr cv_ptr2;
try
{
cv_ptr2 = cv_bridge::toCvCopy(image_ptr, sensor_msgs::image_encodings::RGB8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv_ptr2->image = image_received;
viewer_image_pub_.publish(cv_ptr2->toImageMsg());
/*
* Publish face position and size
*/
face_position_and_size_pub_.publish(message);
imshow("1111",image_received);
cv::waitKey(10);
/*
* Update Haar cascade use frequency for Dynamic Haar Cascade
*/
if(dynamic_check_haar_)
{
if(undetected_count_ > 10)
{
check_Haar_ = 2;
cam_shift_detected_count_ = 0;
cam_shift_undetected_count_ = 0;
}
else if(cam_shift_undetected_count_>3)
{
check_Haar_/=2;
if(check_Haar_<2)
check_Haar_ = 2;
}
if(cam_shift_detected_count_>10)
{
check_Haar_+=5;
if(check_Haar_>100)
check_Haar_ = 100;
}
track_object_= check_Haar_ ;
}
/*
* ROS_INFO
*/
if(face_detected_bool_)
{
if(dynamic_check_haar_)
ROS_INFO("FACE DETECTED -> Head Pos :(%d, %d), Head Distance: %lg, Dynamic check Haar: %u, Det type: %s, Alt: %s", (int)message.u, (int)message.v, head_distance, check_Haar_, detection_type.c_str(), (exist_alternative_)?"true":"false");
else
ROS_INFO("FACE DETECTED -> Head Pos :(%d, %d), Head Distance: %lg, Check Haar: %u, Detection type: %s, Alt: %s",
(int)message.u, (int)message.v, head_distance, check_Haar_, detection_type.c_str(), (exist_alternative_)?"true":"false");
}
else
{
if(dynamic_check_haar_)
ROS_INFO("NO FACE DETECTED. Using Haar Cascade Classifier to find one. Dynamic Check Haar: %u, Alt: %s", check_Haar_, (exist_alternative_)?"true":"false");
else
ROS_INFO("NO FACE DETECTED. Using Haar Cascade Classifier to find one. Check Haar: %u, Alt: %s", check_Haar_, (exist_alternative_)?"true":"false");
}
/*
* Refresh face_detected bool to use Haar Cascade once in a while
*/
if(loop_counter_%check_Haar_==0)
face_detected_bool_ = false;
if(loop_counter_%check_track_obj_==0)
track_object_ = false;
loop_counter_++;
}
void FaceDetector::imageCallback(const sensor_msgs::Image::ConstPtr& image_ptr)
{
#ifdef marco_debug
ROS_INFO("imagecallback called");
#endif
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(image_ptr, sensor_msgs::image_encodings::RGB8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::Mat image_received = (cv_ptr->image).clone();
image_size_ = cv_ptr->image.size();
vector<cv::Rect> faces_roi;
cv::Mat grayscale;
cv::cvtColor(image_received,grayscale,CV_RGB2GRAY); // marco: converting to grayscale before haar detection
detectFacesHaar(image_received, faces_roi,face_classifier_);
if(faces_roi.size() > 0) //If Haar cascade classifier found a face
{
#ifdef marco_debug
ROS_INFO("haar cascade found a face");
#endif
vector<cv::Rect> eyes_roi;
detectFacesHaar(grayscale(faces_roi[0]), eyes_roi,alternative_face_classifier_);
if (eyes_roi.size()>0)
{
//ROS_INFO("HAAR CLASSIFIER FOUND A FACE");
face_detected_bool_ = true;
track_object_ = false;
//Changed
detected_face_roi_ = faces_roi[0];
detected_face_ = cv_ptr->image(detected_face_roi_);
}
else face_detected_bool_=false;
}
else
{
face_detected_bool_=false;
}
// >>>>>>>> KALMAN
double precTick = ticks_;
ticks_ = (double) cv::getTickCount();
double dT = (ticks_ - precTick) / cv::getTickFrequency(); //seconds
if (found_)
{
// >>>> Matrix A
kalman_filter_.transitionMatrix.at<float>(2) = dT;
kalman_filter_.transitionMatrix.at<float>(9) = dT;
// <<<< Matrix A
state_ = kalman_filter_.predict();
//cout << "State post:" << endl << state_ << endl;
cv::Rect predRect;
predRect.width = state_.at<float>(4);
predRect.height = state_.at<float>(5);
predRect.x = state_.at<float>(0) - predRect.width / 2;
predRect.y = state_.at<float>(1) - predRect.height / 2;
cv::Point center;
center.x = state_.at<float>(0);
center.y = state_.at<float>(1);
cv::circle(image_received, center, 2, CV_RGB(255,0,0), -1);
cv::rectangle(image_received, predRect, CV_RGB(255,0,0), 2);
}
if (!face_detected_bool_)
{
notFoundCount_++;
//cout << "notFoundCount:" << notFoundCount_ << endl;
if( notFoundCount_ >= 10 )
{
found_ = false;
}
else
{
kalman_filter_.statePost = state_;
}
}
else
{
notFoundCount_ = 0;
meas_.at<float>(0) = (float)(detected_face_roi_.x + detected_face_roi_.width/2);
meas_.at<float>(1) = (float)(detected_face_roi_.y + detected_face_roi_.height/2);
meas_.at<float>(2) = (float)detected_face_roi_.width;
meas_.at<float>(3) = (float)detected_face_roi_.height;
if (!found_) // First detection!
{
// >>>> Initialization
kalman_filter_.errorCovPre.at<float>(0) = 1; // px
kalman_filter_.errorCovPre.at<float>(7) = 1; // px
kalman_filter_.errorCovPre.at<float>(14) = 1;
kalman_filter_.errorCovPre.at<float>(21) = 1;
kalman_filter_.errorCovPre.at<float>(28) = 1; // px
kalman_filter_.errorCovPre.at<float>(35) = 1; // px
state_.at<float>(0) = meas_.at<float>(0);
state_.at<float>(1) = meas_.at<float>(1);
state_.at<float>(2) = 0;
state_.at<float>(3) = 0;
state_.at<float>(4) = meas_.at<float>(2);
state_.at<float>(5) = meas_.at<float>(3);
// <<<< Initialization
found_ = true;
}
else
kalman_filter_.correct(meas_); // Kalman Correction
// cout << "Measure matrix:" << endl << meas_ << endl;
}
float head_distance;
// ROS_INFO("Computing head distance");
if(!face_detected_bool_)
{
if(head_distances_.size()!=0)
{
head_distance=head_distances_[0];//100;
}
else
head_distance = 3.0;
}
else
{
head_distance=calcDistanceToHead(detected_face_, kalman_filter_);
#ifdef marco_debug
ROS_ERROR ("COMPUTED HEAD DISTANCE: %f",head_distance);
#endif
}
if(head_distances_.size()==0)
{
for(int i=0;i<10;i++)
head_distances_.push_back(head_distance);
}
else
{
head_distances_.pop_back();
head_distances_.insert(head_distances_.begin(),head_distance);
}
#ifdef marco_debug
ROS_INFO("head distance computed");
#endif
head_distance=0; //Reuse variable to compute mean head distance
//Use mean distance of last measured head distances
for(unsigned int i=0;i<head_distances_.size();i++)
head_distance+=head_distances_[i];
head_distance=head_distance/head_distances_.size();
//ROS_ERROR ("HEAD DISTANCE AFTER MEAN COMPUTING: %f",head_distance);
#ifdef marco_debug
ROS_INFO("updated undetected count");
#endif
if(head_distance != head_distance) //not a number
head_distance=3.0;
//Create HeadDistance message
std_msgs::Float32 head_distance_msgs;
head_distance_msgs.data=head_distance;
face_distance_pub_.publish(head_distance_msgs);
//Create Face Pos and Size message
sensor_msgs::JointState message;
int servos_count=2;
message.name.resize(servos_count);
message.position.resize(servos_count);
message.velocity.resize(servos_count);
message.name[0]="head_pan_joint"; //izquierda-derecha
message.name[1]="head_tilt_joint"; //arriba-abajo
message.header.stamp = ros::Time::now();
#ifdef marco_debug
ROS_INFO("message created");
#endif
if( notFoundCount_ >= 10 )
{
float rand_tilt = search_min_tilt_+((search_max_tilt_-search_min_tilt_) * double(rand()%20)/20.);
float rand_pan = search_min_pan_+((search_max_pan_-search_min_pan_) * double(rand()%20)/20.);
message.position[0]=rand_pan;
message.position[1]=rand_tilt;
message.velocity[0]=search_pan_vel_;
message.velocity[1]=search_tilt_vel_;
//ROS_ERROR("RANDOM MOVE: tilt: %f pan: %f",rand_tilt,rand_pan);
random_=true;
}
else
{
if(face_detected_bool_)
{
float pan_pos=meas_.at<float>(0); // it was meas_
float tilt_pos=meas_.at<float>(1);
pan_pos=scalePan(pan_pos);
tilt_pos=scaleTilt(tilt_pos);
message.position[0]=yaw_from_joint_ + pan_pos;
message.position[1]=pitch_from_joint_ + tilt_pos;
message.velocity[0]=abs(pan_pos*1.3) ;
message.velocity[1]=abs(tilt_pos*1.5);
}
else
{
float pan_pos=state_.at<float>(0); // it was meas_
float tilt_pos=state_.at<float>(1);
pan_pos=scalePan(pan_pos);
tilt_pos=scaleTilt(tilt_pos);
message.position[0]=yaw_from_joint_ + pan_pos;
message.position[1]=pitch_from_joint_ + tilt_pos;
message.velocity[0]=abs(pan_pos*1.3);
message.velocity[1]=abs(tilt_pos*1.5);
}
//ROS_ERROR("KALMAN MOVE: pan: %f --- tilt: %f",message.position[0], message.position[1]);
random_=false;
}
cv::imshow("Kalman predictor", image_received);
cvWaitKey(1);
/*
* Publish joint state
*/
#ifdef marco_debug
ROS_INFO("publishing: pan_pos: %lg --- tilt_pos: %lg",message.position[0],message.position[1]);
#endif
joint_pub_.publish(message);
//ROS_ERROR("JOint state yaw_from_joint_: %f",yaw_from_joint_);
sendBaseVelocity(head_distance);
}
