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);
}
}
void filterRegion(const Region ®ion)
{
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);
}
}
}