Go to the documentation of this file.00001
00002 void extractSkinPC(PCRGB::Ptr& pc)
00003 {
00004 uint8_t r, g, b;
00005 double skin_like, h, s, l;
00006 for(uint32_t i=0;i<pc->size();i++) {
00007 if(pc->points[i].x == pc->points[i].x &&
00008 pc->points[i].y == pc->points[i].y &&
00009 pc->points[i].z == pc->points[i].z) {
00010 extractRGB(pc->points[i].rgb, r, g, b);
00011 skin_like = skin_likelihood(r, g, b);
00012 extractHSL(pc->points[i].rgb, h, s, l);
00013 writeHSL(0, min(skin_like*30, 100.0), l, pc->points[i].rgb);
00014 }
00015 }
00016 }
00017
00018 #if 0
00019 ros::Publisher pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/pose", 100);
00020 geometry_msgs::PoseStamped ps;
00021 ps.pose.position.x = input_pc->points[closest_ind].x;
00022 ps.pose.position.y = input_pc->points[closest_ind].y;
00023 ps.pose.position.z = input_pc->points[closest_ind].z;
00024 ps.pose.orientation.w = 1.0;
00025 ps.header.frame_id = "/base_link";
00026 while(ros::ok()) {
00027 pose_pub.publish(ps);
00028 ros::Duration(0.1).sleep();
00029 }
00030 #endif