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; }
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; } }
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; }
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; }
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 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); } }