void CloudPlaneFiltrationPerp(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,
pcl::PointCloud<pcl::PointXYZ>::Ptr &planed_cloud,
float distancethreshold,
float zepsangle,
bool negative){
pcl::PointCloud<pcl::PointXYZ>::Ptr plane_cloud_tmp(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ExtractIndices<pcl::PointXYZ> extract;
//Finding plane segments and filtering positive and negative delta
pcl::SACSegmentation<pcl::PointXYZ> plane_seg;
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
plane_seg.setOptimizeCoefficients(true);
//plane_seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);// (pcl::SACMODEL_PLANE);
plane_seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
plane_seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
plane_seg.setMethodType(pcl::SAC_RANSAC);
plane_seg.setMaxIterations(1000);
plane_seg.setDistanceThreshold(distancethreshold);
plane_seg.setAxis(Eigen::Vector3f(0, 0, 1));
plane_seg.setEpsAngle(zepsangle);
plane_seg.setInputCloud(cloud);
plane_seg.segment(*inliers, *coefficients);
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.setNegative(negative);
extract.filter(*plane_cloud_tmp);
planed_cloud.swap(plane_cloud_tmp);
}
void pointCloudCallback(const sensor_msgs::PointCloud2& scan_msg) {
ROS_INFO("Received a new PointCloud2 message");
DynamicJoinPclConfig dynjoinpcl_config = dynjoinpcl.getConfig();
std::vector<Quad> quads;
pcl::PointCloud<pcl::PointXYZ> scan_pcl;
pcl::PointCloud<pcl::PointXYZRGBNormal> map_new_pcl;
pcl::PointXYZ laser_center;
map_new_pcl.header = map_pcl.header;
convpcl.transform(scan_msg, scan_pcl);
convpcl.getFrameOrigin(dynjoinpcl_config.laser_frame, laser_center);
dynjoinpcl.joinPCL(scan_pcl, map_pcl, map_new_pcl, laser_center, quads);
map_pcl.swap(map_new_pcl);
sensor_msgs::PointCloud2 map_msg_out;
pcl::toROSMsg(map_pcl, map_msg_out);
pcl_pub.publish(map_msg_out);
visualization_msgs::MarkerArray quads_marker_array;
makeQuadsMarkerArray(quads, quads_marker_array);
quads_marker_array_pub.publish(quads_marker_array);
}
void VoxelGridFiltration(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &voxeled_cloud, float voxeldensity){
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::VoxelGrid<pcl::PointXYZ> vg;
vg.setInputCloud(cloud);
vg.setLeafSize(voxeldensity, voxeldensity, voxeldensity); //FIXME. Need to create function with flexeble values for leaf cloud size (Cloud, Count point per dimentions -> BoundingBox -> dimentions -> setLeafSize -> LightCloud)
vg.filter(*filtered_cloud);
voxeled_cloud.swap(filtered_cloud);
}
void CloudPlaneFiltration(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,
pcl::PointCloud<pcl::PointXYZ>::Ptr &planed_cloud,
float clastertollerance, int minclastersize, int maxclastersize, float cloud_zstep){
pcl::PointCloud<pcl::PointXYZ>::Ptr result_cloud_tmp(new pcl::PointCloud<pcl::PointXYZ>);
float minz = FLT_MAX;
float maxz = -FLT_MAX;
float cloud_height = 0;
float cloud_step = cloud_zstep;
//Get min z coordinate
//Get max z coordinate
for (int i = 0; i < cloud->size(); i++){
if (minz > cloud->points[i].z)
minz = cloud->points[i].z;
if (maxz < cloud->points[i].z)
maxz = cloud->points[i].z;
}
//Get height of cloud with oZ axiz
cloud_height = std::abs(maxz - minz);
for (float istep = minz; istep < maxz; istep += cloud_step){
vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> step_clasters;
pcl::PointCloud<pcl::PointXYZ>::Ptr step_cloud_tmp(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointIndices::Ptr step_inliers(new pcl::PointIndices);
pcl::ExtractIndices<pcl::PointXYZ> extract;
//std::cout << istep << std::endl; //DEBUG
for (int i = 0; i < cloud->size(); i++){
if (cloud->points[i].z >= istep && cloud->points[i].z < istep + cloud_step){
step_inliers->indices.push_back(i);
//plane_cloud_tmp->push_back(cloud->points[i].z);
}
}
extract.setInputCloud(cloud);
extract.setIndices(step_inliers);
extract.setNegative(false);
extract.filter(*step_cloud_tmp);
//Find clasters in step cloud
if (step_cloud_tmp->points.size() >= 0){
FindClasters(step_cloud_tmp, step_clasters, clastertollerance, minclastersize, maxclastersize);
if (step_clasters.size() > 0){
for (int k = 0; k < step_clasters.size(); k++){
for (int p = 0; p < step_clasters[k]->points.size(); p++){
result_cloud_tmp->points.push_back(step_clasters[k]->points[p]);
}
}
}
}
}
planed_cloud.swap(result_cloud_tmp);
}
bool LaserSensorProcessor::cleanPointCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud)
{
pcl::PointCloud<pcl::PointXYZRGB> tempPointCloud;
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*pointCloud, tempPointCloud, indices);
tempPointCloud.is_dense = true;
pointCloud->swap(tempPointCloud);
ROS_DEBUG("ElevationMap: cleanPointCloud() reduced point cloud to %i points.", static_cast<int>(pointCloud->size()));
return true;
}
void CloudNoizeFiltration(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &filtered_cloud){
// Create the filtering object
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud_tmp(new pcl::PointCloud<pcl::PointXYZ>);
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud);
sor.setMeanK(10);
sor.setStddevMulThresh(1.0);
sor.setNegative(false);
sor.filter(*filtered_cloud_tmp);
filtered_cloud.swap(filtered_cloud_tmp);
}
bool StereoSensorProcessor::cleanPointCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud)
{
pcl::PointCloud<pcl::PointXYZRGB> tempPointCloud;
originalWidth_ = pointCloud->width;
pcl::removeNaNFromPointCloud(*pointCloud, tempPointCloud, indices_);
tempPointCloud.is_dense = true;
pointCloud->swap(tempPointCloud);
ROS_DEBUG("ElevationMap: cleanPointCloud() reduced point cloud to %i points.", static_cast<int>(pointCloud->size()));
return true;
}
pcl::PointCloud<PointT>::Ptr removeWallsCloud(pcl::PointCloud<PointT>::Ptr cloud_seg)
{
pcl::PointCloud<PointT>::Ptr cloud_plane (new pcl::PointCloud<PointT>);
pcl::ModelCoefficients::Ptr coeff (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
pcl::NormalEstimation<PointT, pcl::Normal> ne;
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
pcl::ExtractIndices<PointT> extract;
// Estimate point normals
ne.setSearchMethod (tree);
ne.setKSearch (50);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (seg_distance);
seg.setNormalDistanceWeight (normal_distance_weight);
seg.setMaxIterations (1000);
int i = 0, nr_points = (int) cloud_seg->points.size ();
// While 20% of the original cloud is still there
while (cloud_seg->points.size () > seg_percentage * nr_points && i < 10 && cloud_seg->points.size() > 0)
{
//seg.setInputCloud (cloud);
ne.setInputCloud (cloud_seg);
ne.compute (*cloud_normals);
//seg.setInputCloud (cloud);
seg.setInputCloud (cloud_seg);
seg.setInputNormals (cloud_normals);
seg.segment (*inliers, *coeff);
if (inliers->indices.size () == 0)
{
break;
}
if(inliers->indices.size() < nr_points/20 || inliers->indices.size() < 10){
i++;
continue;
}
// Extract the planar inliers from the input cloud
extract.setInputCloud (cloud_seg);
extract.setIndices (inliers);
extract.setNegative (true);
extract.filter (*cloud_plane);
cloud_seg.swap (cloud_plane);
i++;
}
return cloud_seg;
}
bool KinectSensorProcessor::cleanPointCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud)
{
pcl::PassThrough<pcl::PointXYZRGB> passThroughFilter;
pcl::PointCloud<pcl::PointXYZRGB> tempPointCloud;
passThroughFilter.setInputCloud(pointCloud);
passThroughFilter.setFilterFieldName("z");
passThroughFilter.setFilterLimits(sensorParameters_.at("cutoff_min_depth"), sensorParameters_.at("cutoff_max_depth"));
// This makes the point cloud also dense (no NaN points).
passThroughFilter.filter(tempPointCloud);
tempPointCloud.is_dense = true;
pointCloud->swap(tempPointCloud);
ROS_DEBUG("cleanPointCloud() reduced point cloud to %i points.", static_cast<int>(pointCloud->size()));
return true;
}
void pointCloudCallback(const sensor_msgs::PointCloud2& scan_msg)
{
ROS_INFO("Received a new PointCloud2 message");
DynamicJoinPclConfig dynjoinpcl_config = dynjoinpcl.getConfig();
NormalEstimationPclConfig normal_config = normal.getConfig();
// downsample
pcl::PointCloud<pcl::PointXYZ> scan_pcl;
pcl::PointCloud<pcl::PointXYZRGBNormal> scan_norm_pcl;
convpcl.transform(scan_msg, scan_pcl);
scan_norm_pcl.header = scan_pcl.header;
pcl::VoxelGrid<pcl::PointXYZ> sor;
sor.setInputCloud(scan_pcl.makeShared());
sor.setLeafSize(dynjoinpcl_config.leaf_size, dynjoinpcl_config.leaf_size, dynjoinpcl_config.leaf_size);
sor.filter(scan_pcl);
pcl::copyPointCloud(scan_pcl, scan_norm_pcl);
// normal estimation
pcl::KdTreeFLANN<pcl::PointXYZRGBNormal> scan_norm_kdtree;
scan_norm_kdtree.setInputCloud(scan_norm_pcl.makeShared());
tf::Transform t;
pcl::PointXYZ laser_center;
convpcl.getLastTransform(t);
convpcl.getFrameOrigin(dynjoinpcl_config.laser_frame, laser_center);
normal.computeNormals(scan_norm_pcl, scan_norm_kdtree, laser_center);
// dynamic join
pcl::PointCloud<pcl::PointXYZRGBNormal> map_new_pcl;
map_new_pcl.header = map_pcl.header;
dynjoinpcl.joinPCL(scan_norm_pcl, map_pcl, map_new_pcl, laser_center);
map_pcl.swap(map_new_pcl);
//colorNormalsPCL(map_pcl);
visualizeNormals(map_pcl);
sensor_msgs::PointCloud2 map_msg_out;
pcl::toROSMsg(map_pcl, map_msg_out);
pcl_pub.publish(map_msg_out);
}
/*
* Find the ground in the environment.
* Params[in/out]: the cloud, the coeff of the planes, the indices, the ground cloud, the hull cloud
*/
void Segmentation::removeWall(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud)
{
pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr indices(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE); // We search a plane perpendicular to the y
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.023); //0.25
seg.setAxis(Eigen::Vector3f(0,-std::sqrt(2)/2,std::sqrt(2)/2)); // Axis Y 0, -1, 0
seg.setEpsAngle(30.0f * (M_PI/180.0f)); // 50 degrees of error
pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
seg.setInputCloud (cloud);
seg.segment (*indices, *coeff);
if (indices->indices.size () == 0)
{
PCL_ERROR ("Could not estimate a planar model (Ground).");
}
else
{
extract.setInputCloud(cloud);
extract.setIndices(indices);
extract.setNegative(true);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGBA>);
extract.filter(*cloud_f);
cloud.swap(cloud_f);
}
}
/*
* Find the ground in the environment.
* Params[in/out]: the cloud, the coeff of the planes, the indices, the ground cloud, the hull cloud
*/
bool Segmentation::findGround(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud, MainPlane &mp)
{
pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr indices(new pcl::PointIndices);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr ground(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr hullGround(new pcl::PointCloud<pcl::PointXYZRGBA>);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE); // We search a plane perpendicular to the y
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.030); //0.25 / 35
seg.setAxis(_axis); // Axis Y 0, -1, 0
seg.setEpsAngle(30.0f * (M_PI/180.0f)); // 50 degrees of error
pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
seg.setInputCloud (cloud);
seg.segment (*indices, *coeff);
mp.setCoefficients(coeff);
//mp.setIndices(indices);
if (indices->indices.size () == 0)
{
PCL_ERROR ("Could not estimate a planar model (Ground).");
return false;
}
else
{
extract.setInputCloud(cloud);
extract.setIndices(indices);
extract.setNegative(false);
extract.filter(*ground);
mp.setPlaneCloud(ground);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr concaveHull(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr plane(new pcl::PointCloud<pcl::PointXYZRGBA>);
// Copy the points of the plane to a new cloud.
pcl::ExtractIndices<pcl::PointXYZRGBA> extractHull;
extractHull.setInputCloud(cloud);
extractHull.setIndices(indices);
extractHull.filter(*plane);
pcl::ConcaveHull<pcl::PointXYZRGBA> chull;
chull.setInputCloud (plane);
chull.setAlpha (0.1);
chull.reconstruct (*concaveHull);
mp.setHull(concaveHull);
//vectorCloudinliers.push_back(convexHull);
extract.setNegative(true);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGBA>);
extract.filter(*cloud_f);
cloud.swap(cloud_f);
return true;
}
}
/*
* Find the other planes in the environment.
* Params[in/out]: the cloud, the normal of the ground, the coeff of the planes, the planes, the hull
* return the number of inliers
*/
int Segmentation::findOtherPlanesRansac(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud,Eigen::Vector3f axis, std::vector <pcl::ModelCoefficients> &coeffPlanes, std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > &vectorCloudInliers, std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > &vectorHull )
{
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
// Optional
seg.setOptimizeCoefficients (true);
vectorCloudInliers.clear();
coeffPlanes.clear();
vectorHull.clear();
// Mandatory
seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
const int nb_points = cloud->points.size();
pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
seg.setMaxIterations(5);
seg.setDistanceThreshold (0.020); //0.15
seg.setAxis(axis);
//std::cout<< "axis are a : " << axis[0] << " b : " << axis[1] << " c ; " << axis[2] << std::endl;
seg.setEpsAngle(20.0f * (M_PI/180.0f));
while (cloud->points.size() > _coeffRansac * nb_points)
{
// std::cout << "the number is " << cloud->points.size() << std::endl;
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
//PCL_ERROR ("Could not estimate a planar model for the given dataset.");
break;
}
else
{
coeffPlanes.push_back(*coefficients);
//vectorInliers.push_back(*inliers);
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.setNegative(false);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_p (new pcl::PointCloud<pcl::PointXYZRGBA>);
extract.filter(*cloud_p);
//Eigen::Vector4f centroid;
//pcl::compute3DCentroid(*cloud_p,*inliers,centroid);
// passthroughFilter(centroid[0] - 2, centroid[0] + 2, centroid[1] - 2, centroid[1] + 2, centroid[2] - 2, centroid[2] + 2, cloud_p, cloud_p);
//statisticalRemovalOutliers(cloud_p);
//statisticalRemovalOutliers(cloud_p); -0.00485527 b : -0.895779 c ; -0.444474
//if((std::abs(coefficients->values[0]) < (0.1 )) && ( std::abs(coefficients->values[1]) > (0.89)) && (std::abs(coefficients->values[2]) > (0.40)))
//{
// std::cout<< "coeff are a : " << coefficients->values[0] << " b : " << coefficients->values[1] << " c ; " << coefficients->values[2] << std::endl;
vectorCloudInliers.push_back(cloud_p);
//}
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr concaveHull(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr plane(new pcl::PointCloud<pcl::PointXYZRGBA>);
// Copy the points of the plane to a new cloud.
pcl::ExtractIndices<pcl::PointXYZRGBA> extractHull;
extractHull.setInputCloud(cloud);
extractHull.setIndices(inliers);
extractHull.filter(*plane);
// Object for retrieving the convex hull.
// pcl::ConvexHull<pcl::PointXYZRGBA> hull;
// hull.setInputCloud(plane);
// hull.reconstruct(*convexHull);
pcl::ConcaveHull<pcl::PointXYZRGBA> chull;
// chull.setInputCloud (plane);
// chull.setAlpha (0.1);
// chull.reconstruct (*concaveHull);
// vectorHull.push_back(concaveHull);
//vectorCloudinliers.push_back(convexHull);
extract.setNegative(true);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGBA>);
extract.filter(*cloud_f);
cloud.swap(cloud_f);
// std::cout << "the number is " << cloud->points.size() << std::endl;
}
}
return vectorCloudInliers.size();
}