Let's start with the header file. The ROS header files we are using in the code are here. We have to include ros/ros.h in every ROS C++ node; otherwise, the source code will not compile. The remaining three headers are image-transport headers, which have functions to publish and subscribe to image messages at a low bandwidth. The cv_bridge header has functions to convert between OpenCV ROS data types. The image_encoding.h header has the image-encoding format used during ROS-OpenCV conversions:
#include <ros/ros.h> #include <image_transport/image_transport.h> #include <cv_bridge/cv_bridge.h> #include <sensor_msgs/image_encodings.h>
The next set of headers is for OpenCV. The imgproc header consists of image-processing functions, highgui has GUI-related functions, and objdetect.hpp has APIs for object detection, such as the Haar classifier:
#include <opencv2/imgproc/imgproc.hpp> #include <opencv2/highgui/highgui.hpp> #include "opencv2/objdetect.hpp"
The last header file is for accessing a custom message called centroid. The centroid message definition has two fields, int32 x and int32 y. These can hold the centroid of the file. You can check this message definition from the face_tracker_pkg/msg/centroid.msg folder:
#include <face_tracker_pkg/centroid.h>
The following lines of code give a name to the raw image window and face-detection window:
static const std::string OPENCV_WINDOW = "raw_image_window"; static const std::string OPENCV_WINDOW_1 = "face_detector";
The following lines of code create a C++ class for our face detector. The code snippet creates handles of NodeHandle, which is a mandatory handle for an ROS node; image_transport, which helps to send ROS Image messages across the ROS computing graph; and a publisher for the face centroid, which can publish the centroid values using the centroid.msg file defined by us. The remaining definitions are for handling parameter values from the parameter file, track.yaml:
class Face_Detector
{
ros::NodeHandle nh_;
image_transport::ImageTransport it_;
image_transport::Subscriber image_sub_;
image_transport::Publisher image_pub_;
ros::Publisher face_centroid_pub;
face_tracker_pkg::centroid face_centroid;
string input_image_topic, output_image_topic, haar_file_face;
int face_tracking, display_original_image, display_tracking_image,
center_offset, screenmaxx;
The following is the code for retrieving ROS parameters inside the track.yaml file. The advantage of using ROS parameters is that we can avoid hardcoding these values inside the program and modify the values without recompiling the code:
try{
nh_.getParam("image_input_topic", input_image_topic);
nh_.getParam("face_detected_image_topic", output_image_topic);
nh_.getParam("haar_file_face", haar_file_face);
nh_.getParam("face_tracking", face_tracking);
nh_.getParam("display_original_image", display_original_image);
nh_.getParam("display_tracking_image", display_tracking_image);
nh_.getParam("center_offset", center_offset);
nh_.getParam("screenmaxx", screenmaxx);
ROS_INFO("Successfully Loaded tracking parameters");
}
The following code creates a subscriber for the input image topic and publisher for the face-detected image. Whenever an image arrives on the input image topic, it will call a function called imageCb. The names of the topics are retrieved from ROS parameters. We create another publisher for publishing the centroid value, which is the last line of the code snippet:
image_sub_ = it_.subscribe(input_image_topic, 1,
&Face_Detector::imageCb, this);
image_pub_ = it_.advertise(output_image_topic, 1);
face_centroid_pub = nh_.advertise<face_tracker_pkg::centroid>
("/face_centroid",10);
The next bit of code is the definition of imageCb, which is a callback for input_image_topic. What it basically does is convert the sensor_msgs/Image data into the cv::Mat OpenCV data type. The cv_bridge::CvImagePtr cv_ptr buffer is allocated for storing the OpenCV image after performing the ROS-OpenCV conversion using the cv_bridge::toCvCopy function:
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
namespace enc = sensor_msgs::image_encodings;
try
{
cv_ptr = cv_bridge::toCvCopy(msg,
sensor_msgs::image_encodings::BGR8);
}
We have already discussed the Haar classifier; here is the code to load the Haar classifier file:
string cascadeName = haar_file_face;
CascadeClassifier cascade;
if( !cascade.load( cascadeName ) )
{
cerr << "ERROR: Could not load classifier cascade" << endl;
}
We are now moving to the core part of the program, which is the detection of the face performed on the converted OpenCV image data type from the ROS Image message. The following is the function call of detectAndDraw(), which performs the face detection, and in the last line, you can see the output image topic being published. Using cv_ptr->image, we can retrieve the cv::Mat data type, and in the next line, cv_ptr->toImageMsg() can convert this into an ROS Image message. The arguments of the detectAndDraw() function are the OpenCV image and cascade variables:
detectAndDraw( cv_ptr->image, cascade );
image_pub_.publish(cv_ptr->toImageMsg());
Let's understand the detectAndDraw() function, which is adapted from the OpenCV sample code for face detection: the function arguments are the input image and cascade object. The next bit of code will convert the image into grayscale first and equalize the histogram using OpenCV APIs. This is a kind of preprocessing before detecting the face from the image. The cascade.detectMultiScale() function is used for this purpose (http://docs.opencv.org/2.4/modules/objdetect/doc/cascade_classification.html):
Mat gray, smallImg;
cvtColor( img, gray, COLOR_BGR2GRAY );
double fx = 1 / scale ;
resize( gray, smallImg, Size(), fx, fx, INTER_LINEAR );
equalizeHist( smallImg, smallImg );
t = (double)cvGetTickCount();
cascade.detectMultiScale( smallImg, faces,
1.1, 15, 0
|CASCADE_SCALE_IMAGE,
Size(30, 30) );
The following loop will iterate on each face that is detected using the detectMultiScale() function. For each face, it finds the centroid and publishes to the /face_centroid topic:
for ( size_t i = 0; i < faces.size(); i++ )
{
Rect r = faces[i];
Mat smallImgROI;
vector<Rect> nestedObjects;
Point center;
Scalar color = colors[i%8];
int radius;
double aspect_ratio = (double)r.width/r.height;
if( 0.75 < aspect_ratio && aspect_ratio < 1.3 )
{
center.x = cvRound((r.x + r.width*0.5)*scale);
center.y = cvRound((r.y + r.height*0.5)*scale);
radius = cvRound((r.width + r.height)*0.25*scale);
circle( img, center, radius, color, 3, 8, 0 );
face_centroid.x = center.x;
face_centroid.y = center.y;
//Publishing centroid of detected face
face_centroid_pub.publish(face_centroid);
}
To make the output image window more interactive, there are text and lines to alert about the user's face on the left or right or at the center. This last section of code is mainly for that purpose. It uses OpenCV APIs to do this job. Here is the code to display text such as Left, Right, and Center on the screen:
putText(img, "Left", cvPoint(50,240),
FONT_HERSHEY_SIMPLEX, 1,
cvScalar(255,0,0), 2, CV_AA);
putText(img, "Center", cvPoint(280,240),
FONT_HERSHEY_SIMPLEX,
1, cvScalar(0,0,255), 2, CV_AA);
putText(img, "Right", cvPoint(480,240),
FONT_HERSHEY_SIMPLEX,
1, cvScalar(255,0,0), 2, CV_AA);
Excellent! We're done with the tracker code; let's see how to build it and make it executable.