Esempio n. 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);
}
Esempio n. 2
0
pcl::IndicesPtr normalFiltering(
		const typename pcl::PointCloud<PointT>::Ptr & cloud,
		const pcl::IndicesPtr & indices,
		float angleMax,
		const Eigen::Vector4f & normal,
		float radiusSearch,
		const Eigen::Vector4f & viewpoint)
{
	typedef typename pcl::search::KdTree<PointT> KdTree;
	typedef typename KdTree::Ptr KdTreePtr;

	pcl::NormalEstimation<PointT, pcl::Normal> ne;
	ne.setInputCloud (cloud);
	if(indices->size())
	{
		ne.setIndices(indices);
	}

	KdTreePtr tree (new KdTree(false));

	if(indices->size())
	{
		tree->setInputCloud(cloud, indices);
	}
	else
	{
		tree->setInputCloud(cloud);
	}
	ne.setSearchMethod (tree);

	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

	ne.setRadiusSearch (radiusSearch);
	if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
	{
		ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
	}

	ne.compute (*cloud_normals);

	pcl::IndicesPtr output(new std::vector<int>(cloud_normals->size()));
	int oi = 0; // output iterator
	Eigen::Vector3f n(normal[0], normal[1], normal[2]);
	for(unsigned int i=0; i<cloud_normals->size(); ++i)
	{
		Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
		float angle = pcl::getAngle3D(normal, v);
		if(angle < angleMax)
		{
			output->at(oi++) = indices->size()!=0?indices->at(i):i;
		}
	}
	output->resize(oi);

	return output;
}
void updateIndices(pcl::IndicesPtr indices, pcl::PointIndices::Ptr inliers)
{
  int toRemove = inliers->indices.size();
  for(int i=0;i<toRemove;i++)
  {
    int origSize = indices->size();
    for(int j=0;j<origSize;j++)
    {
      if((*indices)[j]==inliers->indices[i])
      {
	indices->erase(indices->begin()+j);
	break;
      }
    }
  }
}
void FindFloorPlaneRANSAC()
{	
	double t = getTickCount();
	cout << "RANSAC...";
	/*
	 pcl::SampleConsensusModelPlane<pcl::PointXYZRGB>::Ptr model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZRGB> (cloud));
	 */
	pcl::SampleConsensusModelNormalPlane<pcl::PointXYZRGB,pcl::Normal>::Ptr model_p(
					new pcl::SampleConsensusModelNormalPlane<pcl::PointXYZRGB,pcl::Normal>(cloud));
	//	model_p->setInputCloud(cloud);
	model_p->setInputNormals(mls_normals);
	model_p->setNormalDistanceWeight(0.75);
	
	inliers.reset(new vector<int>);
	
	ransac.reset(new pcl::RandomSampleConsensus<pcl::PointXYZRGB>(model_p));
	ransac->setDistanceThreshold (.1);
	ransac->computeModel();	
	ransac->getInliers(*inliers);
	
	t = ((double)getTickCount() - t)/getTickFrequency();
	cout << "Done. (" << t <<"s)"<< endl;
	
	ransac->getModelCoefficients(coeffs[0]);
	model_p->optimizeModelCoefficients(*inliers,coeffs[0],coeffs[1]);
	
	floorcloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::copyPointCloud(*cloud,*inliers,*floorcloud);

}
void populateIndices(pcl::IndicesPtr indices, int size)
{
  for(int i=0;i<size;i++)
  {
    indices->push_back(i);
  }
}
Esempio n. 6
0
pcl::IndicesPtr radiusFiltering(
		const typename pcl::PointCloud<PointT>::Ptr & cloud,
		const pcl::IndicesPtr & indices,
		float radiusSearch,
		int minNeighborsInRadius)
{
	typedef typename pcl::search::KdTree<PointT> KdTree;
	typedef typename KdTree::Ptr KdTreePtr;
	KdTreePtr tree (new KdTree(false));

	if(indices->size())
	{
		pcl::IndicesPtr output(new std::vector<int>(indices->size()));
		int oi = 0; // output iterator
		tree->setInputCloud(cloud, indices);
		for(unsigned int i=0; i<indices->size(); ++i)
		{
			std::vector<int> kIndices;
			std::vector<float> kDistances;
			int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
			if(k > minNeighborsInRadius)
			{
				output->at(oi++) = indices->at(i);
			}
		}
		output->resize(oi);
		return output;
	}
	else
	{
		pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
		int oi = 0; // output iterator
		tree->setInputCloud(cloud);
		for(unsigned int i=0; i<cloud->size(); ++i)
		{
			std::vector<int> kIndices;
			std::vector<float> kDistances;
			int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
			if(k > minNeighborsInRadius)
			{
				output->at(oi++) = i;
			}
		}
		output->resize(oi);
		return output;
	}
}
Esempio n. 7
0
  bool checkChange()
  {
    cv::Mat mask = cv::Mat::zeros(color.rows, color.cols, CV_8U);
    cv::Mat img = cv::Mat::zeros(color.rows, color.cols, CV_32FC4);
    size_t changedPixels = 0;
    size_t size = indices->size();
    changes.clear();

    // Check pixel changes
    for(size_t i = 0; i < indices->size(); ++i)
    {
      const int idx = indices->at(i);

      cv::Vec4f v = invariantColor(color.at<cv::Vec3b>(idx), depth.at<uint16_t>(idx));
      img.at<cv::Vec4f>(idx) = v;
      mask.at<uint8_t>(idx) = 1;

      if(checkChange(v, idx))
      {
        changes.push_back(idx);
        ++changedPixels;
      }
    }
    // Check pixels that are masked out in current but not in last frame
    for(size_t i = 0; i < lastIndices.size(); ++i)
    {
      const int idx = lastIndices.at(i);
      if(!mask.at<uint8_t>(idx))
      {
        changes.push_back(idx);
        ++changedPixels;
        ++size;
      }
    }

    lastImg = img;
    lastMask = mask;
    indices->swap(lastIndices);

    const float diff = changedPixels / (float)size;
    outInfo(changedPixels << " from " << size << " pixels changed (" << diff * 100 << "%)");

    return diff > threshold;
  }
Esempio n. 8
0
    void pcdCloud::extract(CloudPtr output,  pcl::IndicesPtr inliers, pcl::IndicesPtr outliers, bool invert=false)
    {
        extract(output, inliers, invert);
        outliers->assign(_extractor.getRemovedIndices()->begin(), _extractor.getRemovedIndices()->end());
//         std::cout << "# removedIndices = " << outliers->size() << std::endl;
        // if invert=true, meaning extracting the outliers, then removedIndices which is outliers = inliers 
        if(invert)
        {
            std::cout << "warning: param inliers, outliers are the same\n";
        }
    }
Esempio n. 9
0
std::vector<pcl::IndicesPtr> extractClusters(
		const typename pcl::PointCloud<PointT>::Ptr & cloud,
		const pcl::IndicesPtr & indices,
		float clusterTolerance,
		int minClusterSize,
		int maxClusterSize,
		int * biggestClusterIndex)
{
	typedef typename pcl::search::KdTree<PointT> KdTree;
	typedef typename KdTree::Ptr KdTreePtr;

	KdTreePtr tree(new KdTree);
	pcl::EuclideanClusterExtraction<PointT> ec;
	ec.setClusterTolerance (clusterTolerance);
	ec.setMinClusterSize (minClusterSize);
	ec.setMaxClusterSize (maxClusterSize);
	ec.setInputCloud (cloud);

	if(indices->size())
	{
		ec.setIndices(indices);
		tree->setInputCloud(cloud, indices);
	}
	else
	{
		tree->setInputCloud(cloud);
	}
	ec.setSearchMethod (tree);

	std::vector<pcl::PointIndices> cluster_indices;
	ec.extract (cluster_indices);

	int maxIndex=-1;
	unsigned int maxSize = 0;
	std::vector<pcl::IndicesPtr> output(cluster_indices.size());
	for(unsigned int i=0; i<cluster_indices.size(); ++i)
	{
		output[i] = pcl::IndicesPtr(new std::vector<int>(cluster_indices[i].indices));

		if(maxSize < cluster_indices[i].indices.size())
		{
			maxSize = cluster_indices[i].indices.size();
			maxIndex = i;
		}
	}
	if(biggestClusterIndex)
	{
		*biggestClusterIndex = maxIndex;
	}

	return output;
}
Esempio n. 10
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);
      }
    }
  }
Esempio n. 11
0
void segmentObstaclesFromGround(
		const typename pcl::PointCloud<PointT>::Ptr & cloud,
		pcl::IndicesPtr & ground,
		pcl::IndicesPtr & obstacles,
		float normalRadiusSearch,
		float groundNormalAngle,
		int minClusterSize)
{
	ground.reset(new std::vector<int>);
	obstacles.reset(new std::vector<int>);

	// Find the ground
	pcl::IndicesPtr flatSurfaces = util3d::normalFiltering<PointT>(
			cloud,
			groundNormalAngle,
			Eigen::Vector4f(0,0,1,0),
			normalRadiusSearch*2.0f,
			Eigen::Vector4f(0,0,100,0));

	int biggestFlatSurfaceIndex;
	std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = util3d::extractClusters<PointT>(
			cloud,
			flatSurfaces,
			normalRadiusSearch*2.0f,
			minClusterSize,
			std::numeric_limits<int>::max(),
			&biggestFlatSurfaceIndex);


	// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
	ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
	Eigen::Vector4f min,max;
	pcl::getMinMax3D<PointT>(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);

	for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
	{
		if((int)i!=biggestFlatSurfaceIndex)
		{
			Eigen::Vector4f centroid;
			pcl::compute3DCentroid<PointT>(*cloud, *clusteredFlatSurfaces.at(i), centroid);
			if(centroid[2] >= min[2] && centroid[2] <= max[2])
			{
				ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
			}
		}
	}

	if(ground->size() != cloud->size())
	{
		// Remove ground
		pcl::IndicesPtr otherStuffIndices = util3d::extractNegativeIndices<PointT>(cloud, ground);

		//Cluster remaining stuff (obstacles)
		std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters<PointT>(
				cloud,
				otherStuffIndices,
				normalRadiusSearch*2.0f,
				minClusterSize);

		// merge indices
		obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
	}
}
Esempio n. 12
0
  TyErrorId processWithLock(CAS &tcas, ResultSpecification const &res_spec)
  {
    MEASURE_TIME;
    outInfo("process begins");
    rs::SceneCas cas(tcas);
    rs::Scene scene = cas.getScene();

    cas.get(VIEW_CLOUD, *cloud);
    cas.get(VIEW_COLOR_IMAGE, color);
    cas.get(VIEW_DEPTH_IMAGE, depth);
    cas.get(VIEW_CAMERA_INFO, cameraInfo);

    indices->clear();
    indices->reserve(cloud->points.size());

    camToWorld.setIdentity();
    if(scene.viewPoint.has())
    {
      rs::conversion::from(scene.viewPoint.get(), camToWorld);
    }
    else
    {
      outWarn("No camera to world transformation, no further processing!");
      throw rs::FrameFilterException();
    }
    worldToCam = tf::StampedTransform(camToWorld.inverse(), camToWorld.stamp_, camToWorld.child_frame_id_, camToWorld.frame_id_);
    computeFrustum();

    //default place to look for objects is counter tops except if we got queried for some different place
    //message comes from desigantor and is not the same as the entries from the semantic map so we need
    //to transform them
    rs::Query qs = rs::create<rs::Query>(tcas);
    std::vector<std::string> regionsToLookAt;
    regionsToLookAt.assign(defaultRegions.begin(),defaultRegions.end());
    regions.clear();
    if(cas.getFS("QUERY", qs))
    {
      outWarn("loaction set in query: " << qs.location());
      if(std::find(defaultRegions.begin(), defaultRegions.end(), qs.location()) == std::end(defaultRegions) && qs.location()!="")
      {
        regionsToLookAt.clear();
        regionsToLookAt.push_back(qs.location());
      }
    }

    if(regions.empty())
    {
      std::vector<rs::SemanticMapObject> semanticRegions;
      getSemanticMapEntries(cas, regionsToLookAt, semanticRegions);

      regions.resize(semanticRegions.size());
      for(size_t i = 0; i < semanticRegions.size(); ++i)
      {

        Region &region = regions[i];
        region.width = semanticRegions[i].width();
        region.depth = semanticRegions[i].depth();
        region.height = semanticRegions[i].height();
        region.name = semanticRegions[i].name();
        rs::conversion::from(semanticRegions[i].transform(), region.transform);
      }
    }

    for(size_t i = 0; i < regions.size(); ++i)
    {
      if(frustumCulling(regions[i]) || !frustumCulling_)
      {
        outInfo("region inside frustum: " << regions[i].name);
        filterRegion(regions[i]);
      }
      else
      {
        outInfo("region outside frustum: " << regions[i].name);
      }
    }

    pcl::ExtractIndices<PointT> ei;
    ei.setKeepOrganized(true);
    ei.setIndices(indices);
    ei.filterDirectly(cloud);

    cas.set(VIEW_CLOUD, *cloud);

    if(changeDetection && !indices->empty())
    {
      ++frames;
      if(lastImg.empty())
      {
        lastMask = cv::Mat::ones(color.rows, color.cols, CV_8U);
        lastImg = cv::Mat::zeros(color.rows, color.cols, CV_32FC4);
      }

      uint32_t secondsPassed = camToWorld.stamp_.sec - lastTime.sec;
      bool change = checkChange() || cas.has("QUERY") || secondsPassed > timeout;

      if(!change)
      {
        ++filtered;
      }
      else
      {
        lastTime = camToWorld.stamp_;
      }
      outInfo("filtered frames: " << filtered << " / " << frames << "(" << (filtered / (float)frames) * 100 << "%)");

      if(!change)
      {
        outWarn("no changes in frame detected, no further processing!");
        throw rs::FrameFilterException();
      }
    }

    return UIMA_ERR_NONE;
  }