hrl_head_tracking: head_training.cpp Source File

Go to the documentation of this file.
00001 
00002 #include <hrl_head_tracking/head_tracking.h>
00003 
00004 void expandPC(const PCRGB::Ptr& pc_label, const PCRGB::Ptr& pc_base, 
00005               PCRGB::Ptr& pc_expand, double radius)
00006 {
00007     KDTree::Ptr kd_tree(new pcl::KdTreeFLANN<PRGB> ());
00008     kd_tree->setInputCloud(pc_base);
00009     vector<int> inds;
00010     vector<float> dists;
00011     for(uint32_t i=0;i<pc_label->size();i++) {
00012         inds.clear(); dists.clear();
00013         kd_tree->radiusSearch(*pc_label, i, radius, inds, dists);
00014         for(uint32_t j=0;j<inds.size();j++)
00015             COPY_PT_INTO_CLOUD(pc_label, pc_expand, inds[j]);
00016     }
00017 }
00018 
00019 /*
00020 void extractHeadCluster(const PCRGB::Ptr& pc_label, PCRGB::Ptr& pc_out, PRGB& seed)
00021 {
00022     double pc_clust_dist;
00023     int pc_clust_min_size;
00024     ros::param::param<double>("~pc_clust_dist", pc_clust_dist, 0.02);
00025     ros::param::param<int>("~pc_clust_min_size", pc_clust_min_size, 20);
00026 
00027     // Extracts Euclidean clusters
00028     pcl::PointIndices::Ptr inds_ptr (new pcl::PointIndices());
00029     for(uint32_t i=0;i<pc_label->size();i++) 
00030         inds_ptr->indices.push_back(i);
00031     pcl::EuclideanClusterExtraction<PRGB> pc_clust;
00032     pcl::KdTree<PRGB>::Ptr clust_tree (new pcl::KdTreeFLANN<PRGB> ());
00033     pc_clust.setClusterTolerance(pc_clust_dist);
00034     pc_clust.setMinClusterSize(pc_clust_min_size);
00035     pc_clust.setIndices(inds_ptr);
00036     pc_clust.setInputCloud(pc_label);
00037     pc_clust.setSearchMethod(clust_tree);
00038     std::vector<pcl::PointIndices> pc_clust_list;
00039     pc_clust.extract(pc_clust_list);
00040 
00041     // find the cluster closest our seed
00042     uint32_t closest_clust = 0;
00043     for(uint32_t i=0;i<pc_clust_list.size();i++) {
00044         PCRGB::Ptr pc_clust(new PCRGB());
00045         for(uint32_t j=0;j<pc_clust_list[i].indices.size();j++)
00046             COPY_PT_INTO_CLOUD(pc_label, pc_clust, j);
00047         KDTree::Ptr kd_tree(new pcl::KdTreeFLANN<PRGB> ());
00048         kd_tree->setInputCloud(pc_clust);
00049         // TODO NN TO COMPUTE DIST
00050     }
00051 
00052     // extract cluster into output
00053     for(uint32_t i=0;i<pc_clust_list[closest_clust].indices.size();i++) 
00054         COPY_PT_INTO_CLOUD(pc_label, pc_out, pc_clust_list[closest_clust].indices[i]);
00055 }
00056 */
00057 
00058 int main(int argc, char **argv)
00059 {
00060     ros::init(argc, argv, "head_training");
00061     ros::NodeHandle nh;
00062     PCRGB::Ptr input_pc;
00063     readPCBag(argv[1], input_pc);
00064     PCRGB::Ptr skin_pc(new PCRGB());
00065     PCRGB::Ptr expanded_pc(new PCRGB());
00066     PCRGB::Ptr trimmed_pc(new PCRGB());
00067 
00068     uint32_t closest_ind = findClosestPoint(input_pc, atoi(argv[3]), atoi(argv[4]));
00069     sphereTrim(input_pc, trimmed_pc, closest_ind, atof(argv[5]));
00070     extractSkinPC(trimmed_pc, skin_pc, atof(argv[2]));
00071 
00072     skin_pc->header.frame_id = "/base_link";
00073     savePCBag("skin_pc.bag", skin_pc);
00074     pubLoop(skin_pc, "test", 5);
00075     return 0;
00076 }