예제 #1
0
void findOverlaps(const PCRGB::Ptr& target_pc, const PCRGB::Ptr& source_pc,
                  pcl::IndicesPtr& target_inds_list, pcl::IndicesPtr& source_inds_list, 
                  double icp_radius=0.03, bool use_rgb=false, double color_weight=0.001) 
                  {
    KDTree::Ptr kd_tree(new pcl::KdTreeFLANN<PRGB> ());
    kd_tree->setInputCloud(target_pc);
    if(use_rgb){
        boost::shared_ptr<pcl::DefaultPointRepresentation<PRGB> const> 
            pt_rep(new pcl::DefaultPointRepresentation<PRGB>(color_weight, color_weight, color_weight));
        kd_tree->setPointRepresentation(pt_rep);
    }
    vector<bool> target_inds(target_pc->points.size(), false);
    vector<bool> source_inds(source_pc->points.size(), false);
    for(uint32_t i=0;i<source_pc->points.size();i++) {
        vector<int> inds;
        vector<float> dists;
        kd_tree->radiusSearch(*source_pc, i, icp_radius, inds, dists);
        for(uint32_t j=0;j<inds.size();j++) 
            target_inds[inds[j]] = true;
        if(inds.size() > 0)
            source_inds[i] = true;
    }
    for(uint32_t i=0;i<target_pc->points.size();i++) 
        if(target_inds[i])
            target_inds_list->push_back(i);
    for(uint32_t i=0;i<source_pc->points.size();i++) 
        if(source_inds[i])
            source_inds_list->push_back(i);
}
void populateIndices(pcl::IndicesPtr indices, int size)
{
  for(int i=0;i<size;i++)
  {
    indices->push_back(i);
  }
}
예제 #3
0
  void filterRegion(const Region &region)
  {
    const float minX = -(region.width / 2) + border;
    const float maxX = (region.width / 2) - border;
    float minY = -(region.height / 2) + border;
    const float maxY = (region.height / 2) - border;
    const float minZ = -(region.depth / 2);
    float maxZ = 0.5;
    if (region.name == "drawer_sinkblock_upper_open")
    {
        maxZ = 0.08;
    }
    //needed because of crappy sem map
    if(region.name == "kitchen_sink_block_counter_top")
    {
      minY += 1;//don't get points for the sink
    }
    else if(region.name == "kitchen_island_counter_top")
    {
      minY += 0.6; //same for the hot plate
    }

    tf::Transform transform;
    transform = region.transform.inverse() * camToWorld;

    Eigen::Affine3d eigenTransform;
    tf::transformTFToEigen(transform, eigenTransform);

    pcl::PointCloud<PointT>::Ptr transformed(new pcl::PointCloud<PointT>());

    pcl::transformPointCloud<PointT>(*cloud, *transformed, eigenTransform);

    for(size_t i = 0; i < transformed->points.size(); ++i)
    {
      const PointT &p = transformed->points[i];
      if(p.x > minX && p.x < maxX && p.y > minY && p.y < maxY && p.z > minZ && p.z < maxZ)
      {
        indices->push_back(i);
      }
    }
  }