void main1(int argc, char* argv[]){
f1 = path_txt + argv[2] + ".txt";
fply1 = path_ply + argv[2] + ".ply";
fply2 = path_ply + argv[2] + "_new.ply";
PCloud cloud;
PCloud cloud_f(new Cloud);
LoadCloudFromTXT(f1.data(), cloud);
start = ros::Time::now();
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.0, 1.0);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_f);
end = ros::Time::now();
ROS_INFO("Noise Removal: %f", (end-start).toSec());
PrintCloudToPLY(fply1.data(), cloud);
PrintCloudToPLY(fply2.data(), cloud_f);
std::cout <<"Number of points in cloud " << cloud->size() << std::endl;
std::cout <<"Number of points in cloud_f " << cloud_f->size() << std::endl;
}
void main0(int argc, char* argv[]){
f1 = path_txt + argv[2] + ".txt";
fply1 = path_ply + argv[2] + ".ply";
fply2 = path_ply + argv[2] + "_new.ply";
PCloud cloud;
PCloud cloud_f(new Cloud);
LoadCloudFromTXT(f1.data(), cloud);
start = ros::Time::now();
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*cloud_f);
end = ros::Time::now();
ROS_INFO("Noise Removal: %f", (end-start).toSec());
PrintCloudToPLY(fply1.data(), cloud);
PrintCloudToPLY(fply2.data(), cloud_f);
std::cout <<"Number of points in cloud " << cloud->size() << std::endl;
std::cout <<"Number of points in cloud_f " << cloud_f->size() << std::endl;
}
pcl::PointCloud<PointT>::Ptr cropCloud(const pcl::PointCloud<PointT>::Ptr cloud, const pcl::ModelCoefficients::Ptr planeCoef, float elev)
{
pcl::PointCloud<PointT>::Ptr cloud_f(new pcl::PointCloud<PointT>());
pcl::copyPointCloud(*cloud, *cloud_f);
pcl::ProjectInliers<PointT> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setInputCloud (cloud_f);
proj.setModelCoefficients (planeCoef);
pcl::PointCloud<PointT>::Ptr cloud_projected(new pcl::PointCloud<PointT>());
proj.filter (*cloud_projected);
for(size_t i = 0 ; i < cloud_f->size() ; i++ )
{
if( pcl_isfinite(cloud_f->at(i).z) == true )
{
float diffx = cloud_f->at(i).x-cloud_projected->at(i).x;
float diffy = cloud_f->at(i).y-cloud_projected->at(i).y;
float diffz = cloud_f->at(i).z-cloud_projected->at(i).z;
float dist = sqrt( diffx*diffx + diffy*diffy + diffz*diffz);
if ( dist <= elev )
{
cloud_f->at(i).x = NAN;
cloud_f->at(i).y = NAN;
cloud_f->at(i).z = NAN;
}
}
}
cloud_f->is_dense = false;
return cloud_f;
}
// Callback Function for the subscribed ROS topic
void cloud_cb (const pcl::PCLPointCloud2ConstPtr& input) {
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromPCLPointCloud2(*input,*cloud);
// Create the filtering object: downsample the dataset using a leaf size of 0.5cm
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (input);
sor.setLeafSize (0.005f, 0.005f, 0.005f);
pcl::PCLPointCloud2 cloud_filtered;
sor.filter (cloud_filtered);
// Create the segmentation object
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered2(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromPCLPointCloud2(cloud_filtered,*cloud_filtered2);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);
seg.setInputCloud (cloud_filtered2);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
return;
}
// Extract the inliers
extract.setInputCloud (cloud_filtered2);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_p);
std::cout << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;
// Publish the model coefficients
pcl::PCLPointCloud2 segmented_pcl;
pcl::toPCLPointCloud2(*cloud_p,segmented_pcl);
sensor_msgs::PointCloud2 segmented, not_segmented;
pcl_conversions::fromPCL(cloud_filtered,segmented);
pcl_conversions::fromPCL(*input,not_segmented);
pub.publish (segmented);
pub2.publish(not_segmented);
//Update PCL Viewer
printToPCLViewer();
}
void Planar_filter::Segment_cloud()
{
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
PointCloudT::Ptr cloud_plane (new PointCloudT ());
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
std::vector<pcl::PointIndices> cluster_indices;
seg.setInputCloud (_cloud);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud (_cloud);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
PointCloudT::Ptr cloud_f(new PointCloudT);
extract.filter (*cloud_plane);
//std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
extract.setNegative (true);
extract.filter (*cloud_f);
*_cloud = *cloud_f;
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
tree->setInputCloud (_cloud);
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (_cloud);
ec.extract (cluster_indices);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
PointCloudT::Ptr cloud_cluster (new PointCloudT);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
cloud_cluster->points.push_back (_cloud->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
_Segmented_clouds.push_back(cloud_cluster);
}
}
void cloud_cb (sensor_msgs::PointCloud2ConstPtr input) {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromROSMsg (*input, *cloud_filtered);
//std::cout << "Input pointCloud has: " << cloud_filtered->points.size () << " data points." << std::endl; //*
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGB> ());
seg.setOptimizeCoefficients (true);
seg.setModelType (ModelType);//(pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (MaxIterations);//(100);
seg.setDistanceThreshold (DistanceThreshold);//(0.02);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);
int i=0, nr_points = (int) cloud_filtered->points.size ();
while (cloud_filtered->points.size () > RatioLimit * nr_points)//0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
// extract.setNegative (false);
// // Write the planar inliers to disk
// extract.filter (*cloud_plane);
// std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative (ExtractNegative);
extract.filter (*cloud_f);
cloud_filtered = cloud_f;
}
sensor_msgs::PointCloud2 output;
pcl::toROSMsg (*cloud_filtered , output);
output.header.stamp = ros::Time::now();
output.header.frame_id = "/camera_rgb_optical_frame";
// Publish the data
cloud_pub.publish (output);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr remove_ground_plane(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
//see also pcl cluster extraction tutorial
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.05);
int i=0, nr_points = (int) cloud->points.size ();
while (cloud->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud = *cloud_f;
}
return cloud;
}
int
ClusterExtraction::getPlanes (const pcl::PointCloud<PoinT>::ConstPtr& _cloud)
{
pcl::SACSegmentation<PoinT> seg;
pcl::search::KdTree<PoinT>::Ptr tree (new pcl::search::KdTree<PoinT> ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
pcl::ExtractIndices<PoinT> extract;
pcl::PointCloud<PoinT>::Ptr cloud (new pcl::PointCloud<PoinT>);
*cloud = *_cloud;
pcl::PointCloud<PoinT>::Ptr cloud_f (new pcl::PointCloud<PoinT>);
int nr_points = (int) cloud->points.size();
int i;
for(i = 0;cloud->points.size () > 0.4 * nr_points; i++)
{
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
//std::cout << "Could not estimate a model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
extract.setInputCloud (cloud);
extract.setIndices (inliers);
extract.setNegative (true);
extract.filter (*cloud_f);
//ROS_INFO("%d points -> pass %d\n", cloud->points.size(), i);
*cloud = *cloud_f;
//ROS_INFO("%d points -> pass %d after filters...\n\n", cloud->points.size(), i);
}
return i;
}
void main2(int argc, char* argv[]){
double m_voxel_leah_size = 0.005;
f1 = path_txt + argv[2] + ".txt";
fply1 = path_ply + argv[2] + ".ply";
fply2 = path_ply + argv[2] + "_new.ply";
fply3 = path_ply + argv[2] + "_filter_new.ply";
PCloud cloud;
PCloud cloud_f(new Cloud);
PCloud cloud_filter(new Cloud);
LoadCloudFromTXT(f1.data(), cloud);
start = ros::Time::now();
pcl::VoxelGrid<pcl::PointXYZRGB> grid;
grid.setLeafSize(m_voxel_leah_size,m_voxel_leah_size,m_voxel_leah_size);
grid.setInputCloud(cloud);
grid.filter(*cloud_f);
end = ros::Time::now();
double t1=(end-start).toSec();
ROS_INFO("Downsampling: %f",t1);
start = ros::Time::now();
pcl::PassThrough<pcl::PointXYZRGB> filter;
grid.setInputCloud(cloud_f);
grid.filter(*cloud_filter);
end = ros::Time::now();
double t2=(end-start).toSec();
ROS_INFO("filter: %f", t2);
ROS_INFO("Down+filter: %f", t1+t2);
PrintCloudToPLY(fply1.data(), cloud);
PrintCloudToPLY(fply2.data(), cloud_f);
PrintCloudToPLY(fply3.data(), cloud_filter);
std::cout <<"Number of points in cloud " << cloud->size() << std::endl;
std::cout <<"Number of points in cloud_f " << cloud_f->size() << std::endl;
std::cout <<"Number of points in cloud_filter " << cloud_filter->size() << std::endl;
}
void cloud_cb2(const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
//clear
acc.clear();
//input
pcl::PointCloud<pointtype>::Ptr wallcloud_in(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg (*cloud_msg, *wallcloud_in);
//output-segment
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::PointCloud<pointtype>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
//segmentation
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_LINE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.11);
seg.setMaxIterations(1000);
//filtering object
pcl::ExtractIndices<pcl::PointXYZ> extract;
//remove planes and fit the parameters
while (wallcloud_in->points.size () > remainingspoints)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (wallcloud_in);
seg.segment (*inliers, *coefficients);
//utilize coefficients
if (coefficients.values[0]==coefficients.values[0]){
acc.push_back(std::vector<float>(coefficients.values.data(),coefficients.values.size()));
std::cout << "." <<std::flush;
}
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud_f);
wallcloud_in.swap (cloud_f);
}
}
PointViewSet VoxelGridFilter::run(PointViewPtr input)
{
PointViewPtr output = input->makeNew();
PointViewSet viewSet;
viewSet.insert(output);
bool logOutput = log()->getLevel() > LogLevel::Debug1;
if (logOutput)
log()->floatPrecision(8);
log()->get(LogLevel::Debug2) << "Process VoxelGridFilter..." << std::endl;
BOX3D buffer_bounds;
input->calculateBounds(buffer_bounds);
// convert PointView to PointNormal
typedef pcl::PointCloud<pcl::PointXYZ> Cloud;
Cloud::Ptr cloud(new Cloud);
pclsupport::PDALtoPCD(input, *cloud, buffer_bounds);
pclsupport::setLogLevel(log()->getLevel());
// initial setup
pcl::VoxelGrid<pcl::PointXYZ> vg;
vg.setInputCloud(cloud);
vg.setLeafSize(m_leaf_x, m_leaf_y, m_leaf_z);
// create PointCloud for results
Cloud::Ptr cloud_f(new Cloud);
vg.filter(*cloud_f);
if (cloud_f->points.empty())
{
log()->get(LogLevel::Debug2) << "Filtered cloud has no points!" << std::endl;
return viewSet;
}
pclsupport::PCDtoPDAL(*cloud_f, output, buffer_bounds);
log()->get(LogLevel::Debug2) << cloud->points.size() << " before, " <<
cloud_f->points.size() << " after" << std::endl;
log()->get(LogLevel::Debug2) << output->size() << std::endl;
return viewSet;
}
/* ========================================
* Fill Code: PLANE SEGEMENTATION
* ========================================*/
pcl::PointCloud<pcl::PointXYZ>::Ptr planeSegmentation(const pcl::PointCloud<pcl::PointXYZ>::Ptr (&input_cloud))
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_cloud (new pcl::PointCloud<pcl::PointXYZ>(*input_cloud));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (200);
seg.setDistanceThreshold (0.01); //keep as 1cm
// Segment the largest planar component from the cropped cloud
seg.setInputCloud (cropped_cloud);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
ROS_WARN_STREAM ("Could not estimate a planar model for the given dataset.") ;
//break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cropped_cloud);
extract.setIndices(inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
ROS_INFO_STREAM("PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." );
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
return cloud_f;
}
pcl::PointCloud<PointT>::Ptr FilterBoundary(const pcl::PointCloud<PointT>::Ptr cloud)
{
float threshold = 0.04;
int BLen = 5;
int w = cloud->width;
int h = cloud->height;
int num = cloud->size();
cv::Mat depthmap = cv::Mat::ones(h+2*BLen, w+2*BLen, CV_32FC1)*-1000;
for(int i = 0 ; i < num; i++ )
{
if( pcl_isfinite(cloud->at(i).z) == true )
{
int r_idx = i / w + BLen;
int c_idx = i % w + BLen;
depthmap.at<float>(r_idx, c_idx) = cloud->at(i).z;
}
}
pcl::PointCloud<PointT>::Ptr cloud_f(new pcl::PointCloud<PointT>());
for(int i=0 ; i<num; i++ ){
int row = i / w + BLen;
int col = i % w + BLen;
if( pcl_isfinite(cloud->at(i).z) == true )
{
float zCur = depthmap.at<float>(row, col);
if( fabs(depthmap.at<float>(row-BLen, col)-zCur) > threshold || fabs(depthmap.at<float>(row+BLen, col)-zCur) > threshold
|| fabs(zCur-depthmap.at<float>(row, col-BLen)) > threshold || fabs(zCur-depthmap.at<float>(row, col+BLen)) > threshold)
;//Boundary->push_back(cloud_rgb->points[i]);
else
cloud_f->push_back(cloud->at(i));
}
}
return cloud_f;
}
PointCloud<PointXYZ>::Ptr PCLTools::segmentPlanar(PointCloud<PointXYZ>::Ptr cloud, bool negative) {
// Create the segmentation object for the planar model and set all the parameters
PointCloud<PointXYZ>::Ptr cloud_f(new PointCloud<PointXYZ>);
SACSegmentation<PointXYZ> seg;
PointIndices::Ptr inliers(new PointIndices);
ModelCoefficients::Ptr coefficients(new ModelCoefficients);
PointCloud<PointXYZ>::Ptr cloud_plane(new PointCloud<PointXYZ> ());
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(100);
seg.setDistanceThreshold(0.02);
// Segment the largest planar component from the remaining cloud
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size () == 0) {
cerr << "Could not estimate a planar model for the given dataset." << endl;
return cloud_f;
}
// Extract the planar inliers from the input cloud
ExtractIndices<PointXYZ> extract;
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.setNegative(false);
// Get the points associated with the planar surface
extract.filter(*cloud_plane);
// Remove the planar inliers, extract the rest
extract.setNegative(negative);
extract.filter(*cloud_f);
return 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
*/
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);
}
}
RTC::ReturnCode_t PlaneRemover::onExecute(RTC::UniqueId ec_id)
{
//std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
if (m_originalIn.isNew()){
m_originalIn.read();
// CORBA -> PCL
pcl::PointCloud<pcl::PointXYZ>::Ptr original (new pcl::PointCloud<pcl::PointXYZ>);
original->points.resize(m_original.width*m_original.height);
float *src = (float *)m_original.data.get_buffer();
for (int i=0; i<original->points.size(); i++){
original->points[i].x = src[0];
original->points[i].y = src[1];
original->points[i].z = src[2];
src += 4;
}
// PROCESSING
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (m_distThd);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = original;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ExtractIndices<pcl::PointXYZ> extract;
while(1){
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () < m_pointNumThd) break;
extract.setInputCloud( cloud );
extract.setIndices( inliers );
extract.setNegative( true );
extract.filter( *cloud_f );
cloud = cloud_f;
}
//std::cout << "PLaneRemover: original = " << original->points.size() << ", filtered = " << cloud->points.size() << ", thd=" << m_distThd << std::endl;
// PCL -> CORBA
m_filtered.width = cloud->points.size();
m_filtered.row_step = m_filtered.point_step*m_filtered.width;
m_filtered.data.length(m_filtered.height*m_filtered.row_step);
float *dst = (float *)m_filtered.data.get_buffer();
for (int i=0; i<cloud->points.size(); i++){
dst[0] = cloud->points[i].x;
dst[1] = cloud->points[i].y;
dst[2] = cloud->points[i].z;
dst += 4;
}
m_filteredOut.write();
}
return RTC::RTC_OK;
}
PointViewSet PoissonFilter::run(PointViewPtr input)
{
PointViewPtr output = input->makeNew();
PointViewSet viewSet;
viewSet.insert(output);
bool logOutput = log()->getLevel() > LogLevel::Debug1;
if (logOutput)
log()->floatPrecision(8);
log()->get(LogLevel::Debug2) << "Process PoissonFilter..." << std::endl;
BOX3D buffer_bounds;
input->calculateBounds(buffer_bounds);
// convert PointView to PointNormal
typedef pcl::PointCloud<pcl::PointXYZ> Cloud;
Cloud::Ptr cloud(new Cloud);
pclsupport::PDALtoPCD(input, *cloud, buffer_bounds);
pclsupport::setLogLevel(log()->getLevel());
// pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree;
pcl::search::KdTree<pcl::PointNormal>::Ptr tree2;
// Create search tree
tree.reset(new pcl::search::KdTree<pcl::PointXYZ> (false));
tree->setInputCloud(cloud);
// Normal estimation
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal> ());
n.setInputCloud(cloud);
n.setSearchMethod(tree);
n.setKSearch(20);
n.compute(*normals);
// Concatenate XYZ and normal information
pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
// Create search tree
tree2.reset(new pcl::search::KdTree<pcl::PointNormal>);
tree2->setInputCloud(cloud_with_normals);
// initial setup
pcl::Poisson<pcl::PointNormal> p;
p.setInputCloud(cloud_with_normals);
p.setSearchMethod(tree2);
p.setDepth(m_depth);
p.setPointWeight(m_point_weight);
// create PointCloud for results
pcl::PolygonMesh grid;
p.reconstruct(grid);
Cloud::Ptr cloud_f(new Cloud);
pcl::fromPCLPointCloud2(grid.cloud, *cloud_f);
if (cloud_f->points.empty())
{
log()->get(LogLevel::Debug2) << "Filtered cloud has no points!" << std::endl;
return viewSet;
}
pclsupport::PCDtoPDAL(*cloud_f, output, buffer_bounds);
log()->get(LogLevel::Debug2) << cloud->points.size() << " before, " <<
cloud_f->points.size() << " after" << std::endl;
log()->get(LogLevel::Debug2) << output->size() << std::endl;
return viewSet;
}
/*
* 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();
}
int
main (int argc, char** argv)
{
// Read in the cloud data
pcl::PCDReader reader;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
reader.read ("table_scene_lms400.pcd", *cloud);
std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud (cloud);
vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg.filter (*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //*
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
int i=0, nr_points = (int) cloud_filtered->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
// Visualization removed noise
printf("\ncloud_cluster\n");
pcl::visualization::PCLVisualizer viewer1 ("cloud_cluster");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_cluster_handler (cloud_cluster, 0, 0, 0); // Where 255,255,255 are R,G,B colors
viewer1.addPointCloud (cloud_cluster, cloud_cluster_handler, "cloud_cluster"); // We add the point cloud to the viewer and pass the color handler
//viewer.addCoordinateSystem (1.0, "cloud", 0);
viewer1.setBackgroundColor(0.95, 0.95, 0.95, 0); // Setting background to a dark grey
viewer1.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud_cluster");
//viewer.setPosition(800, 400); // Setting visualiser window position
while (!viewer1.wasStopped ()) { // Display the visualiser untill 'q' key is pressed
viewer1.spinOnce ();
}
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
std::stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
j++;
}
return (0);
}
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// sensor_msgs::PointCloud2 cloud_filtered;
pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled_XYZ (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr output_p (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
sensor_msgs::PointCloud2::Ptr downsampled (new sensor_msgs::PointCloud2);
// sensor_msgs::PointCloud2::Ptr output (new sensor_msgs::PointCloud2);
// std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height
// << " data points." << std::endl;
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
sor.setInputCloud (input);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*downsampled);
// std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
// Change from type sensor_msgs::PointCloud2 to pcl::PointXYZ
pcl::fromROSMsg (*downsampled, *downsampled_XYZ);
//Create the SACSegmentation object and set the model and method type
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);//For more info: wikipedia.org/wiki/RANSAC
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);//How close a point must be to the model to considered an inlier
int i = 0, nr_points = (int) downsampled_XYZ->points.size ();
//Contains the plane point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
// While 30% of the original cloud is still there
while (downsampled_XYZ->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (downsampled_XYZ);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (downsampled_XYZ);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
// std::cerr << "PointCloud representing the planar component: "
// << output_p->width * output_p->height << " data points." << std::endl;
// std::stringstream ss;
// ss << "table_scene_lms400_plane_" << i << ".pcd";
// writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
downsampled_XYZ.swap(cloud_f);
i++;
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (downsampled_XYZ);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (downsampled_XYZ);
ec.extract (cluster_indices);
ros::NodeHandle nh;
//Create a publisher for each cluster
for (int i = 0; i < cluster_indices.size(); ++i)
{
std::string topicName = "/pcl_tut/cluster" + boost::lexical_cast<std::string>(i);
ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2> (topicName, 1);
pub_vec.push_back(pub);
}
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (downsampled_XYZ->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
// std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
//Convert the pointcloud to be used in ROS
sensor_msgs::PointCloud2::Ptr output (new sensor_msgs::PointCloud2);
pcl::toROSMsg (*cloud_cluster, *output);
output->header.frame_id = input->header.frame_id;
// Publish the data
pub_vec[j].publish (output);
++j;
}
}
void ClusterExtraction::extract() {
/*
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = in_pcl.read();
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (clusterTolerance); // 2cm
ec.setMinClusterSize (minClusterSize);
ec.setMaxClusterSize (maxClusterSize);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud);
ec.extract (cluster_indices);
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters;
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
clusters.push_back(cloud_cluster);
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
std::stringstream ss;
// if(j==0)
// {
// ss << "cloud_cluster_0" << ".pcd";
// writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false);
// j++;
// }
// if(j==0)
//{
std::cin.ignore();
out_the_biggest_cluster.write(cloud_cluster);
j++;
std::cout<<"Zapis!!!\n";
// }
}
out_indices.write(cluster_indices);
out_clusters.write(clusters);
//std::cout<<"j=="<<j<<endl;
//std::cout<<clusters.size()<<endl;
*/
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
// Read in the cloud data
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = in_pcl.read();
std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud (cloud);
vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg.filter (*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //*
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
//pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
int i=0, nr_points = (int) cloud_filtered->points.size ();
/*
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
}
*/
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
//std::stringstream ss;
//ss << "cloud_cluster_" << j << ".pcd";
//writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
//std::cin.ignore();
// check if cluster contain interesting us plane, if so, return cluster, next remove plane and we have object :)
//if (include_plane){
out_the_biggest_cluster.write(cloud_cluster);
break;
//}
j++;
}
}
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg (*input, *cloud);
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud(cloud);
vg.setLeafSize(0.01f, 0.01f, 0.01f);
vg.filter(*cloud_filtered);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ>() );
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(100);
seg.setDistanceThreshold(0.02);
int i = 0, nr_points = (int) cloud_filtered -> points.size();
while(cloud_filtered -> points.size() > 0.3 * nr_points)
{
seg.setInputCloud(cloud_filtered);
seg.segment(*inliers, *coefficients);
if(inliers -> indices.size() == 0)
break;
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
extract.filter(*cloud_plane);
extract.setNegative(true);
extract.filter(*cloud_f);
*cloud_filtered = *cloud_f;
}
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree -> setInputCloud(cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.02);
ec.setMinClusterSize(100);
ec.setMaxClusterSize(25000);
ec.setSearchMethod(tree);
ec.setInputCloud(cloud_filtered);
ec.extract(cluster_indices);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
cloud_cluster->points.push_back (cloud_filtered->points[*pit]);
}
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(*cloud_cluster, output);
pub.publish (output);
}
void ClusterExtraction::processCloud(float plane_tolerance)
{
ros::Time stamp = ros::Time::now();
if(!pcl_data)
{
ROS_INFO("No xtion_camera data.");
sleep(1);
return;
}
//pcl::PointCloud<PoinT>::Ptr _cloud;// (new pcl::PointCloud<PoinT>);
sensor_msgs::PointCloud2ConstPtr _temp_cloud_ = pcl_data;
pcl::PointCloud<PoinT>::Ptr _cloud (new pcl::PointCloud<PoinT>);
pcl::fromROSMsg(*_temp_cloud_, *_cloud);
pcl::VoxelGrid<PoinT> vg_filter;
pcl::PointCloud<PoinT>::Ptr cloud_filtered (new pcl::PointCloud<PoinT>);
vg_filter.setInputCloud (_cloud);
vg_filter.setLeafSize (0.01f, 0.01f, 0.01f);
vg_filter.filter (*cloud_filtered);
_cloud = cloud_filtered;
/**********************************************
* NEW BULL
*********************************************/
//////////////////////////////////////////////////////////////////////
// Cluster Extraction
//////////////////////////////////////////////////////////////////////
// Findout the points that are more than 1.25 m away.
pcl::PointIndices::Ptr out_of_range_points (new pcl::PointIndices);
unsigned int i = 0;
BOOST_FOREACH(const PoinT& pt, _cloud->points)
{
if(sqrt( (pt.x*pt.x) + (pt.y*pt.y) + (pt.z*pt.z) ) > 1.5 || isnan (pt.x) || isnan (pt.y) || isnan (pt.z) ||
isinf (pt.x) || isinf (pt.y) || isinf (pt.z) )
out_of_range_points->indices.push_back(i);
i++;
}
pcl::ExtractIndices<PoinT> extract;
pcl::PointCloud<PoinT>::Ptr cloud (new pcl::PointCloud<PoinT>);
// Perform the extraction of these points (indices).
extract.setInputCloud(_cloud);
extract.setIndices(out_of_range_points);
extract.setNegative (true);
extract.filter (*cloud);
// Prepare plane segmentation.
pcl::SACSegmentation<PoinT> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<PoinT>::Ptr cloud_plane; // This door is opened elsewhere.
pcl::PointCloud<PoinT>::Ptr cloud_f (new pcl::PointCloud<PoinT> ());
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.02);
// Remove the planes.
i = 0;
int nr_points = (int) cloud->points.size();
tf::StampedTransform base_link_to_openni;
try
{
tf_listener->waitForTransform("base_link", cloud->header.frame_id, ros::Time(0), ros::Duration(1));
//tf_listener->transformPoint("base_link", plane_normal, _plane_normal);
tf_listener->lookupTransform("base_link", cloud->header.frame_id, ros::Time(0), base_link_to_openni);
}
catch(tf::TransformException& ex)
{
ROS_INFO("COCKED UP POINT INFO! Why: %s", ex.what());
}
// We do this here so that we can put in an empty cluster, if we see no table in the first place.
doro_msgs::ClusterArray __clusters;
while (cloud->points.size () > 0.5 * nr_points)
{
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
//std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
extract.setInputCloud (cloud);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_f);
// Is this a parallel to ground plane? If yes, save it.
tf::Vector3 plane_normal (coefficients->values[0], coefficients->values[1], coefficients->values[2]);
tf::Vector3 _plane_normal = base_link_to_openni*plane_normal;
// What's the angle between this vector and the actual z axis? cos_inverse ( j component )...
tf::Vector3 normal (_plane_normal.x(), _plane_normal.y(), _plane_normal.z());
normal = normal.normalized();
//std::cout<<"x: "<<normal.x()<<"\t";
//std::cout<<"y: "<<normal.y()<<"\t";
//std::cout<<"z: "<<normal.z()<<"\t";
if(acos (normal.z()) < plane_tolerance)
{
cloud_plane = pcl::PointCloud<PoinT>::Ptr(new pcl::PointCloud<PoinT>);
*cloud_plane = *cloud_f;
}
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud = *cloud_f;
}
if(!cloud_plane)
{
ROS_INFO("No table or table-like object could be seen. Can't extract...");
//clusters_pub.publish(__clusters);
//sleep(1);
return;
}
//ROS_INFO("Table seen.");
//table_cloud_pub.publish(cloud_plane);
//////////////////////////////////////////////////////////////////////
/**
* COMPUTE THE CENTROID OF THE PLANE AND PUBLISH IT.
*/
//////////////////////////////////////////////////////////////////////
Eigen::Vector4f plane_cen;
// REMOVE COMMENTS WITH REAL ROBOT!!!
pcl::compute3DCentroid(*cloud_plane, plane_cen);
//std::cout<< plane_cen;
tf::Vector3 plane_centroid (plane_cen[0], plane_cen[1], plane_cen[2]);
tf::Vector3 _plane_centroid = base_link_to_openni*plane_centroid;
geometry_msgs::PointStamped _plane_centroid_ROSMsg;
_plane_centroid_ROSMsg.header.frame_id = "base_link";
_plane_centroid_ROSMsg.header.stamp = stamp;
_plane_centroid_ROSMsg.point.x = _plane_centroid.x();
_plane_centroid_ROSMsg.point.y = _plane_centroid.y();
_plane_centroid_ROSMsg.point.z = _plane_centroid.z();
// Publish the centroid.
table_position_pub.publish(_plane_centroid_ROSMsg);
pcl::search::KdTree<PoinT>::Ptr tree (new pcl::search::KdTree<PoinT>);
if(cloud->points.size() == 0)
{
clusters_pub.publish(__clusters);
return;
}
tree->setInputCloud (cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<PoinT> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (20);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud);
ec.extract (cluster_indices);
//ROS_INFO("WIDTH: %d, HEIGHT: %d\n", _cloud->width, _cloud->height);
//ROS_INFO("GOOD TILL HERE!");
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<PoinT>::Ptr cloud_cluster (new pcl::PointCloud<PoinT>);
cloud_cluster->header.frame_id = cloud->header.frame_id;
long int color_r, color_g, color_b;
uint8_t mean_r, mean_g, mean_b;
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
{
cloud_cluster->points.push_back (cloud->points[*pit]);
/* ***************** */
/* COLOR COMPUTATION */
/* ***************** */
color_r += cloud->points[*pit].r;
color_g += cloud->points[*pit].g;
color_b += cloud->points[*pit].b;
}
geometry_msgs::Point a, b;
std::vector <double> cluster_dims = getClusterDimensions(cloud_cluster, a, b);
mean_r = (uint8_t) (color_r / cloud_cluster->points.size ());
mean_g = (uint8_t) (color_g / cloud_cluster->points.size ());
mean_b = (uint8_t) (color_b / cloud_cluster->points.size ());
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
cloud_cluster->header.frame_id = cloud->header.frame_id;
Eigen::Vector4f cluster_cen;
pcl::compute3DCentroid(*cloud_cluster, cluster_cen);
tf::Vector3 cluster_centroid (cluster_cen[0], cluster_cen[1], cluster_cen[2]);
tf::Vector3 _cluster_centroid = base_link_to_openni*cluster_centroid;
geometry_msgs::PointStamped _cluster_centroid_ROSMsg;
_cluster_centroid_ROSMsg.header.frame_id = "base_link";
_cluster_centroid_ROSMsg.header.stamp = stamp;
_cluster_centroid_ROSMsg.point.x = _cluster_centroid.x();
_cluster_centroid_ROSMsg.point.y = _cluster_centroid.y();
_cluster_centroid_ROSMsg.point.z = _cluster_centroid.z();
if(DIST(cluster_centroid,plane_centroid) < 0.6 && _cluster_centroid.z() > _plane_centroid.z())
{
doro_msgs::Cluster __cluster;
__cluster.centroid = _cluster_centroid_ROSMsg;
// Push cluster dimentions. Viewed width, breadth and height
__cluster.cluster_size = cluster_dims;
__cluster.a = a;
__cluster.b = b;
// Push colors
__cluster.color.push_back(mean_r);
__cluster.color.push_back(mean_g);
__cluster.color.push_back(mean_b);
__clusters.clusters.push_back (__cluster);
j++;
}
}
clusters_pub.publish(__clusters);
/////////////// RUBBISH ENDS ////////////////
//if(pcl_data)
// pcl_data.reset();
}
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr read_pcd() {
// Read in the cloud data
pcl::PCDReader reader;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>), cloud_f (new pcl::PointCloud<pcl::PointXYZRGBA>);
reader.read("data/tripod_3/global.pcd", *cloud);
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);
return cloud;
}
int
ClusterExtraction::getCylinders (const pcl::PointCloud<PoinT>::ConstPtr& _cloud)
{
pcl::NormalEstimation<PoinT, pcl::Normal> ne;
pcl::SACSegmentationFromNormals<PoinT, pcl::Normal> seg;
pcl::search::KdTree<PoinT>::Ptr tree (new pcl::search::KdTree<PoinT> ());
pcl::PointCloud<pcl::Normal>::Ptr cloud_norm (new pcl::PointCloud<pcl::Normal>);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
ne.setSearchMethod (tree);
ne.setInputCloud (_cloud);
ne.setKSearch (50);
ne.compute (*cloud_norm);
//pcl::PointCloud<PoinT>::Ptr cloud_plane; // This door is opened elsewhere.
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_CYLINDER);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setRadiusLimits (0.01, 0.04);
seg.setMaxIterations (10000);
seg.setDistanceThreshold (0.03);
seg.setNormalDistanceWeight (0.02);
pcl::ExtractIndices<PoinT> extract;
pcl::ExtractIndices<pcl::Normal> extract_norm;
pcl::PointCloud<PoinT>::Ptr cloud (new pcl::PointCloud<PoinT>);
*cloud = *_cloud;
pcl::PointCloud<PoinT>::Ptr cloud_f (new pcl::PointCloud<PoinT>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_norm_f (new pcl::PointCloud<pcl::Normal>);
int nr_points = (int) cloud->points.size();
int i;
for(i = 0;cloud->points.size () > 0.4 * nr_points; i++)
{
seg.setInputNormals (cloud_norm);
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
//std::cout << "Could not estimate a model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
extract.setInputCloud (cloud);
extract.setIndices (inliers);
extract.setNegative (true);
extract.filter (*cloud_f);
//ROS_INFO("%d points -> pass %d\n", cloud->points.size(), i);
*cloud = *cloud_f;
//ROS_INFO("%d points -> pass %d after filters...\n\n", cloud->points.size(), i);
// Same for normals
extract_norm.setInputCloud (cloud_norm);
extract_norm.setIndices (inliers);
extract_norm.setNegative (true);
extract_norm.filter (*cloud_norm_f);
*cloud_norm = *cloud_norm_f;
if (inliers->indices.size () < 80)
{
i--;
//printf("SACMODELS: %f\n", coefficients->values[6]);
}
}
return i;
}
int main (int argc, char** argv)
{
//---------------------------------------------------------------------------------------------------
//-- Initialization stuff
//---------------------------------------------------------------------------------------------------
//-- Command-line arguments
float ransac_threshold = 0.02;
float hsv_s_threshold = 0.30;
float hsv_v_threshold = 0.35;
//-- Show usage
if (pcl::console::find_switch(argc, argv, "-h") || pcl::console::find_switch(argc, argv, "--help"))
{
show_usage(argv[0]);
return 0;
}
if (pcl::console::find_switch(argc, argv, "--ransac-threshold"))
pcl::console::parse_argument(argc, argv, "--ransac-threshold", ransac_threshold);
else
{
std::cerr << "RANSAC theshold not specified, using default value..." << std::endl;
}
if (pcl::console::find_switch(argc, argv, "--hsv-s-threshold"))
pcl::console::parse_argument(argc, argv, "--hsv-s-threshold", hsv_s_threshold);
else
{
std::cerr << "Saturation theshold not specified, using default value..." << std::endl;
}
if (pcl::console::find_switch(argc, argv, "--hsv-v-threshold"))
pcl::console::parse_argument(argc, argv, "--hsv-v-threshold", hsv_v_threshold);
else
{
std::cerr << "Value theshold not specified, using default value..." << std::endl;
}
//-- Get point cloud file from arguments
std::vector<int> filenames;
bool file_is_pcd = false;
filenames = pcl::console::parse_file_extension_argument(argc, argv, ".ply");
if (filenames.size() != 1)
{
filenames = pcl::console::parse_file_extension_argument(argc, argv, ".pcd");
if (filenames.size() != 1)
{
show_usage(argv[0]);
return -1;
}
file_is_pcd = true;
}
//-- Load point cloud data
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (file_is_pcd)
{
if (pcl::io::loadPCDFile(argv[filenames[0]], *source_cloud) < 0)
{
std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
show_usage(argv[0]);
return -1;
}
}
else
{
if (pcl::io::loadPLYFile(argv[filenames[0]], *source_cloud) < 0)
{
std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
show_usage(argv[0]);
return -1;
}
}
//-- Load point cloud data (with color)
pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_cloud_color(new pcl::PointCloud<pcl::PointXYZRGB>);
if (file_is_pcd)
{
if (pcl::io::loadPCDFile(argv[filenames[0]], *source_cloud_color) < 0)
{
std::cout << "Error loading colored point cloud " << argv[filenames[0]] << std::endl << std::endl;
show_usage(argv[0]);
return -1;
}
}
else
{
if (pcl::io::loadPLYFile(argv[filenames[0]], *source_cloud_color) < 0)
{
std::cout << "Error loading colored point cloud " << argv[filenames[0]] << std::endl << std::endl;
show_usage(argv[0]);
return -1;
}
}
//-- Print arguments to user
std::cout << "Selected arguments: " << std::endl
<< "\tRANSAC threshold: " << ransac_threshold << std::endl
<< "\tColor point threshold: " << hsv_s_threshold << std::endl
<< "\tColor region threshold: " << hsv_v_threshold << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
//--------------------------------------------------------------------------------------------------------
//-- Program does actual work from here
//--------------------------------------------------------------------------------------------------------
Debug debug;
debug.setAutoShow(false);
debug.setEnabled(false);
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(source_cloud_color, Debug::COLOR_ORIGINAL);
debug.show("Original with color");
//-- Downsample the dataset prior to plane detection (using a leaf size of 1cm)
//-----------------------------------------------------------------------------------
pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
voxel_grid.setInputCloud(source_cloud);
voxel_grid.setLeafSize(0.01f, 0.01f, 0.01f);
voxel_grid.filter(*cloud_filtered);
std::cout << "Initially PointCloud has: " << source_cloud->points.size () << " data points." << std::endl;
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;
//-- Detect all possible planes
//-----------------------------------------------------------------------------------
std::vector<pcl::ModelCoefficientsPtr> all_planes;
pcl::SACSegmentation<pcl::PointXYZ> ransac_segmentation;
ransac_segmentation.setOptimizeCoefficients(true);
ransac_segmentation.setModelType(pcl::SACMODEL_PLANE);
ransac_segmentation.setMethodType(pcl::SAC_RANSAC);
ransac_segmentation.setDistanceThreshold(ransac_threshold);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr current_plane(new pcl::ModelCoefficients);
int i=0, nr_points = (int) cloud_filtered->points.size();
while (cloud_filtered->points.size() > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
ransac_segmentation.setInputCloud(cloud_filtered);
ransac_segmentation.segment(*inliers, *current_plane);
if (inliers->indices.size() == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
// Remove the planar inliers, extract the rest
extract.setNegative(true);
extract.filter(*cloud_f);
*cloud_filtered = *cloud_f;
//-- Save plane
pcl::ModelCoefficients::Ptr copy_current_plane(new pcl::ModelCoefficients);
*copy_current_plane = *current_plane;
all_planes.push_back(copy_current_plane);
//-- Debug stuff
debug.setEnabled(false);
debug.plotPlane(*current_plane, Debug::COLOR_BLUE);
debug.plotPointCloud<pcl::PointXYZ>(cloud_filtered, Debug::COLOR_RED);
debug.show("Plane segmentation");
}
//-- Filter planes to obtain garment plane
//-----------------------------------------------------------------------------------
pcl::ModelCoefficients::Ptr garment_plane(new pcl::ModelCoefficients);
float min_height = FLT_MAX;
pcl::PointXYZ garment_projected_center;
for(int i = 0; i < all_planes.size(); i++)
{
//-- Check orientation
Eigen::Vector3f normal_vector(all_planes[i]->values[0],
all_planes[i]->values[1],
all_planes[i]->values[2]);
normal_vector.normalize();
Eigen::Vector3f good_orientation(0, -1, -1);
good_orientation.normalize();
std::cout << "Checking vector with dot product: " << std::abs(normal_vector.dot(good_orientation)) << std::endl;
if (std::abs(normal_vector.dot(good_orientation)) >= 0.9)
{
//-- Check "height" (height is defined in the local frame of reference in the yz direction)
//-- With this frame, it is approximately equal to the norm of the vector OO' (being O the
//-- center of the old frame and O' the projection of that center onto the plane).
//-- Project center point onto given plane:
pcl::PointCloud<pcl::PointXYZ>::Ptr center_to_be_projected_cloud(new pcl::PointCloud<pcl::PointXYZ>);
center_to_be_projected_cloud->points.push_back(pcl::PointXYZ(0,0,0));
pcl::PointCloud<pcl::PointXYZ>::Ptr center_projected_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ProjectInliers<pcl::PointXYZ> project_inliners;
project_inliners.setModelType(pcl::SACMODEL_PLANE);
project_inliners.setInputCloud(center_to_be_projected_cloud);
project_inliners.setModelCoefficients(all_planes[i]);
project_inliners.filter(*center_projected_cloud);
pcl::PointXYZ projected_center = center_projected_cloud->points[0];
Eigen::Vector3f projected_center_vector(projected_center.x, projected_center.y, projected_center.z);
float height = projected_center_vector.norm();
if (height < min_height)
{
min_height = height;
*garment_plane = *all_planes[i];
garment_projected_center = projected_center;
}
}
}
if (!(min_height < FLT_MAX))
{
std::cerr << "Garment plane not found!" << std::endl;
return -3;
}
else
{
std::cout << "Found closest plane with h=" << min_height << std::endl;
//-- Debug stuff
debug.setEnabled(true);
debug.plotPlane(*garment_plane, Debug::COLOR_BLUE);
debug.plotPointCloud<pcl::PointXYZ>(source_cloud, Debug::COLOR_RED);
debug.show("Garment plane");
}
//-- Reorient cloud to origin (with color point cloud)
//-----------------------------------------------------------------------------------
//-- Translating to center
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr centered_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Affine3f translation_transform = Eigen::Affine3f::Identity();
translation_transform.translation() << -garment_projected_center.x, -garment_projected_center.y, -garment_projected_center.z;
//pcl::transformPointCloud(*source_cloud_color, *centered_cloud, translation_transform);
//-- Orient using the plane normal
pcl::PointCloud<pcl::PointXYZRGB>::Ptr oriented_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Vector3f normal_vector(garment_plane->values[0], garment_plane->values[1], garment_plane->values[2]);
//-- Check normal vector orientation
if (normal_vector.dot(Eigen::Vector3f::UnitZ()) >= 0 && normal_vector.dot(Eigen::Vector3f::UnitY()) >= 0)
normal_vector = -normal_vector;
Eigen::Quaternionf rotation_quaternion = Eigen::Quaternionf().setFromTwoVectors(normal_vector, Eigen::Vector3f::UnitZ());
//pcl::transformPointCloud(*centered_cloud, *oriented_cloud, Eigen::Vector3f(0,0,0), rotation_quaternion);
Eigen::Transform<float, 3, Eigen::Affine> t(rotation_quaternion * translation_transform);
pcl::transformPointCloud(*source_cloud_color, *oriented_cloud, t);
//-- Save to file
record_transformation(argv[filenames[0]]+std::string("-transform1.txt"), translation_transform, rotation_quaternion);
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(oriented_cloud, Debug::COLOR_GREEN);
debug.show("Oriented");
//-- Filter points under the garment table
//-----------------------------------------------------------------------------------
pcl::PointCloud<pcl::PointXYZRGB>::Ptr garment_table_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PassThrough<pcl::PointXYZRGB> passthrough_filter;
passthrough_filter.setInputCloud(oriented_cloud);
passthrough_filter.setFilterFieldName("z");
passthrough_filter.setFilterLimits(-ransac_threshold/2.0f, FLT_MAX);
passthrough_filter.setFilterLimitsNegative(false);
passthrough_filter.filter(*garment_table_cloud);
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(garment_table_cloud, Debug::COLOR_GREEN);
debug.show("Table cloud (filtered)");
//-- Color segmentation of the garment
//-----------------------------------------------------------------------------------
//-- HSV thresholding
pcl::PointCloud<pcl::PointXYZHSV>::Ptr hsv_cloud(new pcl::PointCloud<pcl::PointXYZHSV>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered_garment_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloudXYZRGBtoXYZHSV(*garment_table_cloud, *hsv_cloud);
for (int i = 0; i < hsv_cloud->points.size(); i++)
{
if (isnan(hsv_cloud->points[i].x) || isnan(hsv_cloud->points[i].y || isnan(hsv_cloud->points[i].z)))
continue;
if (hsv_cloud->points[i].s > hsv_s_threshold && hsv_cloud->points[i].v > hsv_v_threshold)
filtered_garment_cloud->push_back(garment_table_cloud->points[i]);
}
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(filtered_garment_cloud, Debug::COLOR_GREEN);
debug.show("Garment cloud");
//-- Euclidean Clustering of the resultant cloud
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud(filtered_garment_cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> euclidean_custering;
euclidean_custering.setClusterTolerance(0.005);
euclidean_custering.setMinClusterSize(100);
euclidean_custering.setSearchMethod(tree);
euclidean_custering.setInputCloud(filtered_garment_cloud);
euclidean_custering.extract(cluster_indices);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr largest_color_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
int largest_cluster_size = 0;
for (auto it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
for (auto pit = it->indices.begin (); pit != it->indices.end (); ++pit)
cloud_cluster->points.push_back(filtered_garment_cloud->points[*pit]);
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "Found cluster of " << cloud_cluster->points.size() << " points." << std::endl;
if (cloud_cluster->points.size() > largest_cluster_size)
{
largest_cluster_size = cloud_cluster->points.size();
*largest_color_cluster = *cloud_cluster;
}
}
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(largest_color_cluster, Debug::COLOR_GREEN);
debug.show("Filtered garment cloud");
//-- Centering the point cloud before saving it
//-----------------------------------------------------------------------------------
//-- Find bounding box
pcl::MomentOfInertiaEstimation<pcl::PointXYZRGB> feature_extractor;
pcl::PointXYZRGB min_point_AABB, max_point_AABB;
pcl::PointXYZRGB min_point_OBB, max_point_OBB;
pcl::PointXYZRGB position_OBB;
Eigen::Matrix3f rotational_matrix_OBB;
feature_extractor.setInputCloud(largest_color_cluster);
feature_extractor.compute();
feature_extractor.getAABB(min_point_AABB, max_point_AABB);
feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
//-- Translating to center
pcl::PointCloud<pcl::PointXYZRGB>::Ptr centered_garment_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Affine3f garment_translation_transform = Eigen::Affine3f::Identity();
garment_translation_transform.translation() << -position_OBB.x, -position_OBB.y, -position_OBB.z;
pcl::transformPointCloud(*largest_color_cluster, *centered_garment_cloud, garment_translation_transform);
//-- Orient using the principal axes of the bounding box
pcl::PointCloud<pcl::PointXYZRGB>::Ptr oriented_garment_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Vector3f principal_axis_x(max_point_OBB.x - min_point_OBB.x, 0, 0);
Eigen::Quaternionf garment_rotation_quaternion = Eigen::Quaternionf().setFromTwoVectors(principal_axis_x, Eigen::Vector3f::UnitX()); //-- This transformation is wrong (I guess)
Eigen::Transform<float, 3, Eigen::Affine> t2 = Eigen::Transform<float, 3, Eigen::Affine>::Identity();
t2.rotate(rotational_matrix_OBB.inverse());
//pcl::transformPointCloud(*centered_garment_cloud, *oriented_garment_cloud, Eigen::Vector3f(0,0,0), garment_rotation_quaternion);
pcl::transformPointCloud(*centered_garment_cloud, *oriented_garment_cloud, t2);
//-- Save to file
record_transformation(argv[filenames[0]]+std::string("-transform2.txt"), garment_translation_transform, Eigen::Quaternionf(t2.rotation()));
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(oriented_garment_cloud, Debug::COLOR_GREEN);
debug.plotBoundingBox(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB, Debug::COLOR_YELLOW);
debug.show("Oriented garment patch");
//-- Save point cloud in file to process it in Python
pcl::io::savePCDFileBinary(argv[filenames[0]]+std::string("-output.pcd"), *oriented_garment_cloud);
return 0;
}
void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud) {
if (!viewer.wasStopped()) {
// viewer.showCloud (cloud);
// writer.write<pcl::PointXYZ> ("kinect_cloud.pcd", *cloud, false); //*
// std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl;
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud (cloud);
vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg.filter (*cloud_filtered);
// std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //*
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
int i=0, nr_points = (int) cloud_filtered->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Write the planar inliers to disk
extract.filter (*cloud_plane);
// std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
cloud_filtered = cloud_f;
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
int j = 0;
std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin ();
// for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
// std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
// std::stringstream ss;
// ss << "cloud_cluster_" << j << ".pcd";
// writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
// j++;
viewer.showCloud (cloud_cluster);
}
}
}
int main(int argc, char* argv[]) {
//Load Matrix Q
cv::FileStorage fs("/home/bodereau/Bureau/OpenCVReprojectImageToPointCloud/Q.xml", cv::FileStorage::READ);
cv::Mat Q;
pcl::visualization::CloudViewer viewer ("3D Viewer");
fs["Q"] >> Q;
//If size of Q is not 4x4 exit
if (Q.cols != 4 || Q.rows != 4)
{
std::cerr << "ERROR: Could not read matrix Q (doesn't exist or size is not 4x4)" << std::endl;
return 1;
}
//Get the interesting parameters from Q
double Q03, Q13, Q23, Q32, Q33;
Q03 = Q.at<double>(0,3);
Q13 = Q.at<double>(1,3);
Q23 = Q.at<double>(2,3);
Q32 = Q.at<double>(3,2);
Q33 = Q.at<double>(3,3);
std::cout << "Q(0,3) = "<< Q03 <<"; Q(1,3) = "<< Q13 <<"; Q(2,3) = "<< Q23 <<"; Q(3,2) = "<< Q32 <<"; Q(3,3) = "<< Q33 <<";" << std::endl;
cv::Size size(752, 480);
vision::StereoRectifier rectifier(size);
rectifier.load_intrinsic_params("/home/bodereau/Bureau/intrinsics.yml");
rectifier.load_extrinsic_params("/home/bodereau/Bureau/extrinsics.yml");
rectifier.stereo_rectify();
rectifier.dump_rectification_info();
int idx = 220;
//time
clock_t start,other_clock,clock2,clock4;
float time,time2,timtotal,time3,time4;
timtotal = 0.0;
start = clock();
//~time
cv::namedWindow("vigne");
cv::namedWindow("bord");
while(idx++ < 500) {
//printf("boucle : %d \n",idx);
std::string ref = std::to_string(idx);
std::string leftImageFilename = "/home/bodereau/Bureau/tiff2/" + ref + "_l.tiff";
std::string rightImageFilename = "/home/bodereau/Bureau/tiff2/" + ref + "_r.tiff";
cv::Mat g1, g2;
clock2 = clock();
cv::Size size_rectified = rectifier.get_recified_size();
cv::Mat3b mat_r = cv::imread(rightImageFilename);
cv::Mat3b mat_l = cv::imread(leftImageFilename);
cv::Mat3b rectified_l = cv::Mat3b::zeros(size_rectified);
cv::Mat3b rectified_r = cv::Mat3b::zeros(size_rectified);
rectifier.generate_rectified_mat(mat_l, mat_r, rectified_l, rectified_r);
DTSStereo dtsStereo;
/*int test = dtsStereo.computeLab(rectified_l.data, size_rectified.area(), size_rectified.width);
int test2 = dtsStereo.compute(rectified_l.data, size_rectified.area(), size_rectified.width);*/
int height_without_sky = dtsStereo.computeHisto(rectified_l, size_rectified.area(), size_rectified.width);
cv::namedWindow("img test1", CV_WINDOW_AUTOSIZE);
printf("height without sky %d \n", height_without_sky);
clock2 = clock() - clock2;
time3 = ((float) (clock2)) / CLOCKS_PER_SEC;
printf("time for rectifie : %f \n", time3);
other_clock = clock();
cv::cvtColor(rectified_l, g1, CV_BGR2GRAY);
cv::cvtColor(rectified_r, g2, CV_BGR2GRAY);
cv::Mat left_image_rectified_crop, right_image_rectified_crop;
/*cv::Rect win(1, 339-test, 560, test);
cv::Rect win2(1,339-test2,560,test2);*/
cv::Rect win3(1, 339 - height_without_sky, 560, height_without_sky);
rectified_l(win3).copyTo(left_image_rectified_crop);
rectified_r(win3).copyTo(right_image_rectified_crop);
//printf("convert to png::image left done \n");
//png::image<png::rgb_pixel> rightImage("right.png");
other_clock = clock() - other_clock;
time2 = ((float) (other_clock)) / CLOCKS_PER_SEC;
timtotal = timtotal + time2;
printf("time lost in png make : %f \n", time2);
//~time*/
/*idxVect = 0;
rightImage.write("rgb2.png");*/
//printf("convert to png::image right done \n");
SPSStereo sps;
sps.setIterationTotal(outerIterationTotal, innerIterationTotal);
sps.setWeightParameter(lambda_pos, lambda_depth, lambda_bou, lambda_smo);
sps.setInlierThreshold(lambda_d);
sps.setPenaltyParameter(lambda_hinge, lambda_occ, lambda_pen);
cv::Mat segmentImage(600, 600, CV_16UC1);
std::vector <std::vector<double>> disparityPlaneParameters;
std::vector <std::vector<int>> boundaryLabels;
//printf("go to compute \n");
other_clock = clock();
cv::Mat disparity(600, 600, CV_16UC1);
sps.compute(superpixelTotal, left_image_rectified_crop, right_image_rectified_crop, segmentImage, disparity, disparityPlaneParameters, boundaryLabels);
/*cv::StereoSGBM sgbm;
sgbm.SADWindowSize = 5;
sgbm.numberOfDisparities = 256;
sgbm.preFilterCap = 0;
sgbm.minDisparity = 0;
sgbm.uniquenessRatio = 1;
sgbm.speckleWindowSize = 150;
sgbm.speckleRange = 2;
sgbm.disp12MaxDiff = 10;
sgbm.fullDP = false;
sgbm.P1 = 1000;
sgbm.P2 = 2400;
cv::Mat disper;
sgbm(dst, dst2, disper);
normalize(disper, disparity, 0, 255, CV_MINMAX, CV_8U);*/
other_clock = clock() - other_clock;
time2 = ((float) (other_clock)) / CLOCKS_PER_SEC;
printf("time to compute : %f \n", time2);
//printf("compute done \n");
other_clock = clock();
/*png::image<png::rgb_pixel> segmentBoundaryImage;
makeSegmentBoundaryImage(dst, segmentImage, boundaryLabels, segmentBoundaryImage);
segmentBoundaryImage.write(ref + "__bound.png");*/
//disparityImage.write(ref + "__disparity.png");
other_clock = clock() - other_clock;
cv::imshow("vigne", disparity);
/*cv::Mat copy;
disparity.copyTo(copy);
STVFlow stvflow;
stvflow.compute(disparity,dst);*/
//copy.convertTo(copy, CV_8U,1/255.0,1/255.0);
/*cv::Mat dst455;
cv::threshold(disparity,dst455,50, 255, cv::THRESH_BINARY);*/
/*cv::imshow("bord",disparity);
cv::threshold(copy,copy,100, 255, cv::THRESH_BINARY);
cv::imshow("img test1",dst);*/
cv::waitKey(100);
cv::Mat img_disparity, img_rgb;
img_disparity = disparity;
img_rgb = left_image_rectified_crop;
img_disparity.convertTo(img_disparity, CV_8U, 1 / 255.0, 1 / 255.0); //A REMETRE EN SPS
//Create matrix that will contain 3D corrdinates of each pixel
cv::Mat recons3D(img_disparity.size(), CV_32FC3);
//Reproject image to 3D
std::cout << "Reprojecting image to 3D..." << std::endl;
cv::reprojectImageTo3D(img_disparity, recons3D, Q, false, CV_32F);
std::cout << "Creating Point Cloud..." << std::endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_all_the_image_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
double px, py, pz;
uchar pr, pg, pb;
clock4 = clock();
for (int i = 0; i < img_rgb.rows; i++) {
uchar *rgb_ptr = img_rgb.ptr<uchar>(i);
uchar *disp_ptr = img_disparity.ptr<uchar>(i);
//double* recons_ptr = recons3D.ptr<double>(i);
for (int j = 0; j < img_rgb.cols; j++) {
//Get 3D coordinates
uchar d = disp_ptr[j];
if (d == 0) continue; //Discard bad pixels
double pw = -1.0 * static_cast<double>(d) * Q32 + Q33;
px = static_cast<double>(j) + Q03;
py = static_cast<double>(i) + Q13;
pz = Q23;
px = px / pw;
py = py / pw;
pz = pz / pw;
//Get RGB info
pb = rgb_ptr[3 * j];
pg = rgb_ptr[3 * j + 1];
pr = rgb_ptr[3 * j + 2];
//Insert info into point cloud structure
pcl::PointXYZRGB point;
point.x = px;
point.y = py;
point.z =-1* pz;
uint32_t rgb = (static_cast<uint32_t>(pr) << 16 |
static_cast<uint32_t>(pg) << 8 | static_cast<uint32_t>(pb));
point.rgb = *reinterpret_cast<float *>(&rgb);
//if(-1*pz < 90){
point_cloud_all_the_image_rgb->points.push_back(point);//}
pcl::PointXYZ pointx;
pointx.x = px;
pointx.y = py;
pointx.z = -1*pz;
if(-1*pz <90)
cloud->points.push_back(pointx);
}
}
point_cloud_all_the_image_rgb->width = (int) point_cloud_all_the_image_rgb->points.size();
point_cloud_all_the_image_rgb->height = 1;
cloud->width = (int) cloud->points.size();
cloud->height = 1;
clock4 = clock() - clock4;
time4 = ((float) (clock4)) / CLOCKS_PER_SEC;
printf("temps a faire les pointcloud : %f \n", time4);
clock4 = clock();
// Read in the cloud data
pcl::PCDReader reader;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
//cloud = cloud_ptr;
std::cout << "PointCloud before filtering has: " << cloud->points.size() << " data points." << std::endl; //*
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ground(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgbseg(new pcl::PointCloud<pcl::PointXYZRGB>);
ExtractTheGround extractTheGround;
extractTheGround.compute(cloud, cloud_filtered, cloud_ground);
clock4 = clock() - clock4;
time4 = ((float) (clock4)) / CLOCKS_PER_SEC;
printf("temps a sortir le sol : %f \n", time4);
clock4 = clock();
std::cout << "ytyt" << std::endl;
std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clusters;
EuclidianClusters euclidianClusters;
euclidianClusters.compute(cloud_filtered, clusters);
clock4 = clock() - clock4;
time4 = ((float) (clock4)) / CLOCKS_PER_SEC;
printf("temps a faire la seg euclid : %f \n", time4);
clock4 = clock();
/*pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgbseg_less(new pcl::PointCloud<pcl::PointXYZRGB>);
for(int i=0;i < cloud_filtered->points.size();i++){
for(int j=0;j < point_cloud_ptr->points.size();j++){
if(cloud_filtered->points[i].x == point_cloud_ptr->points[j].x && cloud_filtered->points[i].y == point_cloud_ptr->points[j].y &&
cloud_filtered->points[i].z == point_cloud_ptr->points[j].z){
cloud_rgbseg_less->points.push_back(point_cloud_ptr->points[j]);
break;
}
}
}
RGBSegmentation rgbseg;
rgbseg.compute(cloud_rgbseg_less, cloud_rgbseg);*/
//viewer.removeVisualizationCallable("jhjh");
viewer.removeVisualizationCallable("jhjha");
viewer.showCloud(point_cloud_all_the_image_rgb,"jhjha");
clock4 = clock() - clock4;
time4 = ((float) (clock4)) / CLOCKS_PER_SEC;
printf("temps afficher image: %f \n", time4);
clock4 = clock();
//viewer.showCloud(cloud_rgbseg,"jhjh");
/*ClustersFilter clustersFilter;
clustersFilter.compute(clusters);*/
pcl::PointCloud<pcl::PointXYZ>::Ptr cube(new pcl::PointCloud<pcl::PointXYZ>);
for(int i=0; i< clusters.size();i++) {
float minX, minY, minZ,maxZ,maxX,maxY,memY,memZ;
int nbrY,nbrZ;
bool dontgo = false;
bool zminbool = false;
for(int j=0; j < clusters.at(i)->points.size();j++) {
//pour chaque points du cluster en question, eh ouais !
float xpoint = clusters.at(i)->points[j].x;
float ypoint = clusters.at(i)->points[j].y;
float zpoint = clusters.at(i)->points[j].z;
if(j==0){
minX = xpoint; minY = ypoint; minZ = zpoint;
maxX = xpoint; maxY = ypoint; maxZ= zpoint;
memY = ypoint; memZ = zpoint;
}
if(memZ == zpoint ){
nbrZ++;
}else {
if (nbrZ >= 50) {
maxZ = zpoint;
if (!zminbool) {
zminbool = true;
minZ = zpoint;
}
}
nbrZ = 1;
}
if(memY != ypoint)
memY = ypoint;
if(memZ != zpoint) {
memZ = zpoint;
}
/*if(zpoint > maxZ)
maxZ = zpoint;*/
/*if(zpoint < minZ)
minZ = zpoint;*/
if(xpoint < minX)
minX= xpoint;
if(xpoint > maxX)
maxX = xpoint;
if( ypoint < minY)
minY = ypoint;
if( ypoint > maxY)
maxY = ypoint;
if(maxZ > (minZ +15))
break;
}
if (maxX - minX > 3 * (maxY - minY))
dontgo = true;
if(maxZ - minZ < 3)
dontgo = true;
if(dontgo)
continue;
float xdebut = minX, ydebut = minY, zdebut = minZ, xfin = maxX, yfin = maxY, zfin = maxZ;
//float xdebut = -10.20, ydebut = 0, zdebut = 12.20, xfin = -50.40, yfin = 15.24, zfin = -13.56;
if (ydebut > yfin) {
float tmp = ydebut;
ydebut = yfin;
yfin = tmp;
}
if (xdebut > xfin) {
float tmp = xdebut;
xdebut = xfin;
xfin = tmp;
}
if (zdebut > zfin) {
float tmp = zdebut;
zdebut = zfin;
zfin = tmp;
}
for (float i = xdebut; i <= xfin; i+=0.3) {
for (float j = ydebut; j <= yfin; j+=0.3) {
pcl::PointXYZ p;
pcl::PointXYZ p2;
p.x = i;
if (i == xdebut || (i >= xfin - 0.3 && i <= xfin + 0.3)) {
p.y = j;
}
else {
p.y = ydebut;
p2.y = yfin;
}
p.z = zfin;
p2.x = p.x;
p2.z = p.z;
cube->points.push_back(p);
cube->points.push_back(p2);
p.z = zdebut;
p2.z = zdebut;
cube->points.push_back(p);
cube->points.push_back(p2);
}
}
for(float j=ydebut; j <=yfin;j+=0.3){
for(float z =zdebut;z<=zfin;z+=0.3){
pcl::PointXYZ p;
pcl::PointXYZ p2;
p.x=xdebut;
p2.x=xfin;
p.y=j;
p.z=z;
p2.y=p.y;
p2.z=p.z;
if (j == ydebut || (j>= yfin - 0.3 && j <= yfin + 0.3)) {
cube->points.push_back(p);
cube->points.push_back(p2);
}
}
}
}
viewer.showCloud (cube,"oh");
/*pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb_memory2 (new pcl::PointCloud<pcl::PointXYZRGB>);
int r2=15,b2=255,g3r=125;
for(int i = 0; i < clusters.size();i++){
uint32_t rgb = (static_cast<uint32_t>(r2) << 16 |
static_cast<uint32_t>(g3r) << 8 | static_cast<uint32_t>(b2));
for(int j=0;j< clusters.at(i)->points.size();j++){
clusters.at(i)->points[j].rgb = *reinterpret_cast<float*>(&rgb);
}
r2+=20;
b2+=100;
g3r+=5;
if(r2 >252)
r2=0;
if(g3r>252)
g3r=0;
if(b2>253)
b2=0;
*cloud_rgb_memory2 += *clusters.at(i);
}
clock4 = clock() - clock4;
time4 = ((float) (clock4)) / CLOCKS_PER_SEC;
printf("temps a faire les clusters avant : %f \n", time4);
clock4 = clock();
std::cout<<"clusters size :"<<clusters.size() <<std::endl;
//viewer.showCloud (cloud_rgb_memory2,"eh eh eh");
clock4 = clock() - clock4;
time4 = ((float) (clock4)) / CLOCKS_PER_SEC;
printf("temps a faire les clusters aprés : %f \n", time4);
clock4 = clock();*/
}
//time
start = clock() - start;
time = ((float) ( start))/ CLOCKS_PER_SEC;
printf("time for all : %f and time without loss :%f \n",time,time - timtotal);
}
void cloud_callback(const sensor_msgs::PointCloud2ConstPtr& input) {
ROS_DEBUG("Got new pointcloud.");
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
std_msgs::Header header = input->header;
pcl::fromROSMsg(*input, *cloud);
//all the objects needed
pcl::PCDReader reader;
pcl::PassThrough<PointT> pass;
pcl::NormalEstimation<PointT, pcl::Normal> ne;
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
pcl::PCDWriter writer;
pcl::ExtractIndices<PointT> extract;
pcl::ExtractIndices<pcl::Normal> extract_normals;
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
//data sets
pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_sphere (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_sphere (new pcl::PointIndices);
ROS_DEBUG("PointCloud before filtering has: %i data points.", (int)cloud->points.size());
//filter our NaNs
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 2.0);
pass.filter (*cloud_filtered);
ROS_DEBUG("PointCloud after filtering has: %i data points." , (int)cloud_filtered->points.size());
*cloud_filtered = *cloud;//remove the pass through filter basically
//estimate normal points
ne.setSearchMethod (tree);
ne.setInputCloud (cloud_filtered);
//ne.setKSearch(10);
ne.setRadiusSearch(0.025);
ne.compute (*cloud_normals);
// Create the segmentation object for the planar model and set all the parameters
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
seg.setNormalDistanceWeight (0.05);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (5.0);
seg.setInputCloud (cloud_filtered);
seg.setInputNormals (cloud_normals);
// Obtain the plane inliers and coefficients
seg.segment (*inliers_plane, *coefficients_plane);
std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
// Extract the planar inliers from the input cloud
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers_plane);
extract.setNegative (false);
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_filtered2);
extract_normals.setNegative (true);
extract_normals.setInputCloud (cloud_normals);
extract_normals.setIndices (inliers_plane);
extract_normals.filter (*cloud_normals2);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_SPHERE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setNormalDistanceWeight (0.1);
seg.setMaxIterations (100);
seg.setDistanceThreshold (5.0);
seg.setRadiusLimits (0, 1.0);
seg.setInputCloud (cloud_filtered2);
seg.setInputNormals (cloud_normals2);
extract.setInputCloud (cloud_filtered2);
extract.setIndices (inliers_sphere);
extract.setNegative (false);
pcl::PointCloud<PointT>::Ptr sphere_cloud (new pcl::PointCloud<PointT> ());
extract.filter(*sphere_cloud);
pcl::PointCloud<pcl::PointXYZ> sphere_cloud_in = *sphere_cloud;
sensor_msgs::PointCloud2 sphere_cloud_out;
pcl::toROSMsg(sphere_cloud_in, sphere_cloud_out);
sphere_cloud_out.header = header;
buoy_cloud_pub.publish(sphere_cloud_out);
}
