Go to the documentation of this file.00001
00061 #ifdef __LINUX__
00062 #include "cob_people_detection/head_detector_node.h"
00063 #include "cob_vision_utils/GlobalDefines.h"
00064 #else
00065 #endif
00066
00067
00068 #include "opencv/cv.h"
00069
00070 #include <cv_bridge/cv_bridge.h>
00071 #include <sensor_msgs/image_encodings.h>
00072
00073
00074 #include <pcl/point_types.h>
00075 #include <pcl_ros/point_cloud.h>
00076
00077
00078 #include <cob_people_detection/timer.h>
00079
00080 using namespace ipa_PeopleDetector;
00081
00082 HeadDetectorNode::HeadDetectorNode(ros::NodeHandle nh) :
00083 node_handle_(nh)
00084 {
00085 data_directory_ = ros::package::getPath("cob_people_detection") + "/common/files/";
00086
00087
00088 double depth_increase_search_scale;
00089 int depth_drop_groups;
00090 int depth_min_search_scale_x;
00091 int depth_min_search_scale_y;
00092 std::cout << "\n--------------------------\nHead Detector Parameters:\n--------------------------\n";
00093 node_handle_.param("data_directory", data_directory_, data_directory_);
00094 std::cout << "data_directory = " << data_directory_ << "\n";
00095 node_handle_.param("fill_unassigned_depth_values", fill_unassigned_depth_values_, true);
00096 std::cout << "fill_unassigned_depth_values = " << fill_unassigned_depth_values_ << "\n";
00097 node_handle_.param("depth_increase_search_scale", depth_increase_search_scale, 1.1);
00098 std::cout << "depth_increase_search_scale = " << depth_increase_search_scale << "\n";
00099 node_handle_.param("depth_drop_groups", depth_drop_groups, 68);
00100 std::cout << "depth_drop_groups = " << depth_drop_groups << "\n";
00101 node_handle_.param("depth_min_search_scale_x", depth_min_search_scale_x, 20);
00102 std::cout << "depth_min_search_scale_x = " << depth_min_search_scale_x << "\n";
00103 node_handle_.param("depth_min_search_scale_y", depth_min_search_scale_y, 20);
00104 std::cout << "depth_min_search_scale_y = " << depth_min_search_scale_y << "\n";
00105 node_handle_.param("display_timing", display_timing_, false);
00106 std::cout << "display_timing = " << display_timing_ << "\n";
00107
00108
00109 head_detector_.init(data_directory_, depth_increase_search_scale, depth_drop_groups, depth_min_search_scale_x, depth_min_search_scale_y);
00110
00111
00112 head_position_publisher_ = node_handle_.advertise<cob_perception_msgs::ColorDepthImageArray>("head_positions", 1);
00113
00114
00115 pointcloud_sub_ = nh.subscribe("pointcloud_rgb", 1, &HeadDetectorNode::pointcloud_callback, this);
00116
00117 std::cout << "HeadDetectorNode initialized." << std::endl;
00118 }
00119
00120 HeadDetectorNode::~HeadDetectorNode(void)
00121 {
00122 }
00123
00124 void HeadDetectorNode::pointcloud_callback(const sensor_msgs::PointCloud2::ConstPtr& pointcloud)
00125 {
00126
00127
00128
00129
00130
00131 cv::Mat depth_image;
00132 cv::Mat color_image;
00133 convertPclMessageToMat(pointcloud, depth_image, color_image);
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149 std::vector < cv::Rect > head_bounding_boxes;
00150 head_detector_.detectRangeFace(depth_image, head_bounding_boxes, fill_unassigned_depth_values_);
00151
00152
00153 cob_perception_msgs::ColorDepthImageArray image_array;
00154 image_array.header = pointcloud->header;
00155 image_array.head_detections.resize(head_bounding_boxes.size());
00156 for (unsigned int i = 0; i < head_bounding_boxes.size(); i++)
00157 {
00158 cv_bridge::CvImage cv_ptr;
00159 image_array.head_detections[i].head_detection.x = head_bounding_boxes[i].x;
00160 image_array.head_detections[i].head_detection.y = head_bounding_boxes[i].y;
00161 image_array.head_detections[i].head_detection.width = head_bounding_boxes[i].width;
00162 image_array.head_detections[i].head_detection.height = head_bounding_boxes[i].height;
00163 cv::Mat depth_patch = depth_image(head_bounding_boxes[i]);
00164 cv_ptr.image = depth_patch;
00165 cv_ptr.encoding = sensor_msgs::image_encodings::TYPE_32FC3;
00166 image_array.head_detections[i].depth_image = *(cv_ptr.toImageMsg());
00167 cv::Mat color_patch = color_image(head_bounding_boxes[i]);
00168 cv_ptr.image = color_patch;
00169 cv_ptr.encoding = sensor_msgs::image_encodings::BGR8;
00170 image_array.head_detections[i].color_image = *(cv_ptr.toImageMsg());
00171 }
00172 head_position_publisher_.publish(image_array);
00173
00174 if (display_timing_ == true)
00175 ROS_INFO("%d HeadDetection: Time stamp of pointcloud message: %f. Delay: %f.", pointcloud->header.seq, pointcloud->header.stamp.toSec(),
00176 ros::Time::now().toSec() - pointcloud->header.stamp.toSec());
00177
00178 }
00179
00180 unsigned long HeadDetectorNode::convertPclMessageToMat(const sensor_msgs::PointCloud2::ConstPtr& pointcloud, cv::Mat& depth_image, cv::Mat& color_image)
00181 {
00182 pcl::PointCloud < pcl::PointXYZRGB > depth_cloud;
00183 pcl::fromROSMsg(*pointcloud, depth_cloud);
00184 depth_image.create(depth_cloud.height, depth_cloud.width, CV_32FC3);
00185 color_image.create(depth_cloud.height, depth_cloud.width, CV_8UC3);
00186 uchar* depth_image_ptr = (uchar*)depth_image.data;
00187 uchar* color_image_ptr = (uchar*)color_image.data;
00188 for (int v = 0; v < (int)depth_cloud.height; v++)
00189 {
00190 int depth_base_index = depth_image.step * v;
00191 int color_base_index = color_image.step * v;
00192 for (int u = 0; u < (int)depth_cloud.width; u++)
00193 {
00194 int depth_index = depth_base_index + 3 * u * sizeof(float);
00195 float* depth_data_ptr = (float*)(depth_image_ptr + depth_index);
00196 int color_index = color_base_index + 3 * u * sizeof(uchar);
00197 uchar* color_data_ptr = (uchar*)(color_image_ptr + color_index);
00198 pcl::PointXYZRGB point_xyz = depth_cloud(u, v);
00199 depth_data_ptr[0] = point_xyz.x;
00200 depth_data_ptr[1] = point_xyz.y;
00201 depth_data_ptr[2] = (isnan(point_xyz.z)) ? 0.f : point_xyz.z;
00202 color_data_ptr[0] = point_xyz.r;
00203 color_data_ptr[1] = point_xyz.g;
00204 color_data_ptr[2] = point_xyz.b;
00205
00206 }
00207 }
00208 return ipa_Utils::RET_OK;
00209 }
00210
00211
00212
00213 int main(int argc, char** argv)
00214 {
00215
00216 ros::init(argc, argv, "head_detector");
00217
00218
00219 ros::NodeHandle nh;
00220
00221
00222 HeadDetectorNode head_detector_node(nh);
00223
00224
00225
00226
00227
00228
00229 ros::spin();
00230
00231 return 0;
00232 }