Go to the documentation of this file.00001
00061 #ifdef __LINUX__
00062 #include "cob_people_detection/face_normalizer_node.h"
00063 #include "cob_vision_utils/GlobalDefines.h"
00064 #else
00065 #endif
00066
00067
00068 #include "opencv/cv.h"
00069 #include "opencv/highgui.h"
00070 #include <cv_bridge/cv_bridge.h>
00071 #include <sensor_msgs/image_encodings.h>
00072
00073
00074 #include <boost/shared_ptr.hpp>
00075 #include "cob_people_detection/face_normalizer_node.h"
00076 using namespace ipa_PeopleDetector;
00077
00078
00079 void voidDeleter(const sensor_msgs::Image* const )
00080 {
00081 }
00082
00083 FaceNormalizerNode::FaceNormalizerNode(ros::NodeHandle nh) :
00084 node_handle_(nh)
00085 {
00086
00087
00088
00089
00090
00091 norm_face_publisher_ = node_handle_.advertise<cob_perception_msgs::ColorDepthImageCropArray>("norm_faces", 1);
00092
00093
00094 face_position_subscriber_ = nh.subscribe("face_positions", 1, &FaceNormalizerNode::facePositionsCallback, this);
00095 }
00096
00097 FaceNormalizerNode::~FaceNormalizerNode(void)
00098 {
00099 }
00100
00101 void FaceNormalizerNode::facePositionsCallback(const cob_perception_msgs::ColorDepthImageArray::ConstPtr& face_positions)
00102 {
00103
00104
00105
00106 cv_bridge::CvImageConstPtr cv_ptr;
00107 std::vector < cv::Mat > heads_color_images;
00108 heads_color_images.resize(face_positions->head_detections.size());
00109 std::vector < cv::Mat > heads_depth_images;
00110 heads_depth_images.resize(face_positions->head_detections.size());
00111 std::vector < std::vector<cv::Rect>> face_bounding_boxes;
00112 face_bounding_boxes.resize(face_positions->head_detections.size());
00113 std::vector < cv::Rect > head_bounding_boxes;
00114 head_bounding_boxes.resize(face_positions->head_detections.size());
00115 for (unsigned int i = 0; i < face_positions->head_detections.size(); i++)
00116 {
00117
00118 {
00119 sensor_msgs::ImageConstPtr msgPtr = boost::shared_ptr<sensor_msgs::Image const>(&(face_positions->head_detections[i].color_image), voidDeleter);
00120 try
00121 {
00122 cv_ptr = cv_bridge::toCvShare(msgPtr, sensor_msgs::image_encodings::BGR8);
00123 } catch (cv_bridge::Exception& e)
00124 {
00125 ROS_ERROR("cv_bridge exception: %s", e.what());
00126 return;
00127 }
00128 heads_color_images[i] = cv_ptr->image;
00129 }
00130
00131
00132 sensor_msgs::ImageConstPtr msgPtr = boost::shared_ptr<sensor_msgs::Image const>(&(face_positions->head_detections[i].depth_image), voidDeleter);
00133 try
00134 {
00135 cv_ptr = cv_bridge::toCvShare(msgPtr, sensor_msgs::image_encodings::TYPE_32FC3);
00136 } catch (cv_bridge::Exception& e)
00137 {
00138 ROS_ERROR("cv_bridge exception: %s", e.what());
00139 return;
00140 }
00141 heads_depth_images[i] = cv_ptr->image;
00142
00143
00144 face_bounding_boxes[i].resize(face_positions->head_detections[i].face_detections.size());
00145 for (uint j = 0; j < face_bounding_boxes[i].size(); j++)
00146 {
00147 const cob_perception_msgs::Rect& source_rect = face_positions->head_detections[i].face_detections[j];
00148 cv::Rect rect(source_rect.x, source_rect.y, source_rect.width, source_rect.height);
00149 face_bounding_boxes[i][j] = rect;
00150 }
00151
00152
00153 const cob_perception_msgs::Rect& source_rect = face_positions->head_detections[i].head_detection;
00154 cv::Rect rect(source_rect.x, source_rect.y, source_rect.width, source_rect.height);
00155 head_bounding_boxes[i] = rect;
00156 }
00157
00158 std::vector < cv::Mat > crops;
00159 for (size_t i = 0; i < heads_depth_images.size(); i++)
00160 {
00161
00162 for (size_t j = 0; j < face_bounding_boxes[i].size(); j++)
00163 {
00164 int dim = 160;
00165
00166
00167 cv::Mat bgr_crop, xyz_crop;
00168 heads_color_images[i](face_bounding_boxes[i][j]).copyTo(bgr_crop);
00169 heads_depth_images[i](face_bounding_boxes[i][j]).copyTo(xyz_crop);
00170 cv::Vec2f offset;
00171 offset[0] = head_bounding_boxes[i].x + face_bounding_boxes[i][j].x;
00172 offset[1] = head_bounding_boxes[i].y + face_bounding_boxes[i][j].y;
00173 cv::Size norm_size = cv::Size(160, 160);
00174
00175 face_normalizer_.normalizeFace(bgr_crop, xyz_crop, norm_size, offset);
00176 crops.push_back(bgr_crop);
00177
00178 }
00179 }
00180
00181 cob_perception_msgs::ColorDepthImageCropArray image_array;
00182 image_array.cdia = *face_positions;
00183 image_array.crops.resize(crops.size());
00184 for (unsigned int i = 0; i < crops.size(); i++)
00185 {
00186 cv_bridge::CvImage cv_ptr;
00187 cv_ptr.image = crops[i];
00188 cv_ptr.encoding = sensor_msgs::image_encodings::BGR8;
00189 image_array.crops[i] = *(cv_ptr.toImageMsg());
00190 }
00191
00192 norm_face_publisher_.publish(image_array);
00193 }
00194
00195
00196
00197 int main(int argc, char** argv)
00198 {
00199
00200 ros::init(argc, argv, "face_normalizer");
00201
00202
00203 ros::NodeHandle nh;
00204
00205
00206 FaceNormalizerNode face_recognizer_node(nh);
00207
00208
00209
00210
00211
00212
00213 ros::spin();
00214
00215 return 0;
00216 }