/*
* 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();
}
void SegmentPlane::detectPlaneCallback(const sensor_msgs::PointCloud2::ConstPtr& source_pc)
{
pcl::PointCloud<pcl::PointXYZ> pcl_source;
pcl::fromROSMsg(*source_pc, pcl_source);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_source_ptr(new pcl::PointCloud<pcl::PointXYZ>(pcl_source));
pcl::PointCloud<pcl::PointXYZ>::Ptr resized_pc_ptr (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
approximate_voxel_filter.setLeafSize (leaf_size_x_, leaf_size_y_, leaf_size_z_);
approximate_voxel_filter.setInputCloud (pcl_source_ptr);
approximate_voxel_filter.filter (*resized_pc_ptr);
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(0.1);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZ> extract;
seg.setInputCloud(resized_pc_ptr);
seg.segment(*inliers, *coefficients);
if(inliers->indices.size() == 0)
{
ROS_WARN("Could not estimate a planar model for the given dataset");
return;
}
extract.setInputCloud(resized_pc_ptr);
extract.setIndices(inliers);
extract.setNegative(false);
extract.filter(*cloud_p);
sensor_msgs::PointCloud2 inliers_pc;
pcl::toROSMsg(*cloud_p, inliers_pc);
inliers_pc.header.stamp = ros::Time::now();
inliers_pc.header.frame_id = frame_id_;
inliers_pc_pub_.publish(inliers_pc);
}
int main (int argc, char** argv)
{
sensor_msgs::PointCloud2::Ptr cloud_blob (new sensor_msgs::PointCloud2), cloud_filtered_blob (new sensor_msgs::PointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the cloud data
pcl::PCDReader reader;
reader.read ("table_scene_lms400.pcd", *cloud_blob);
std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->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 (cloud_blob);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered_blob);
// Convert to the templated PointCloud
pcl::fromROSMsg (*cloud_filtered_blob, *cloud_filtered);
std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
// Write the downsampled version to disk
pcl::PCDWriter writer;
writer.write<pcl::PointXYZ> ("table_scene_lms400_downsampled.pcd", *cloud_filtered, false);
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.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZ> extract;
int i = 0, nr_points = (int) cloud_filtered->points.size ();
// While 30% of the original cloud is still there
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::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the inliers
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_p);
std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_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);
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud_filtered);
i++;
}
return (0);
}
void FilterPlanes::filterPlanes(vector<BasicPlane> &plane_stack){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>), cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>);
if (verbose_text>0){
cout << "PointCloud before filtering: " << cloud->width * cloud->height << " data points." << std::endl;
}
#if DO_TIMING_PROFILE
vector<int64_t> tic_toc;
tic_toc.push_back(bot_timestamp_now());
#endif
/* Ptcoll_cfg ptcoll_cfg;
ptcoll_cfg.point_lists_id =pose_element_id; //bot_timestamp_now();
ptcoll_cfg.collection = pose_coll_id;
ptcoll_cfg.element_id = pose_element_id; */
//1. Downsample the dataset using a leaf size of 1cm
// this is 99% of the cpu time
pcl::VoxelGrid<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud);
// for table dataset: sor.setLeafSize (0.01, 0.01, 0.01);
sor.setLeafSize (0.05, 0.05, 0.05);
// sor.setLeafSize (0.1, 0.1, 0.1);
sor.filter (*cloud_filtered);
if (verbose_text>0){
cout << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
}
#if DO_TIMING_PROFILE
tic_toc.push_back(bot_timestamp_now());
#endif
if (verbose_lcm>2){
/* ptcoll_cfg.id = 200;
ptcoll_cfg.reset=true;
ptcoll_cfg.name ="cloud_downsampled";
ptcoll_cfg.npoints = cloud_filtered->points.size();
float colorm_temp0[] ={-1.0,-1.0,-1.0,-1.0};
ptcoll_cfg.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float));
ptcoll_cfg.type =1;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg, *cloud_filtered);*/
}
// 2. Set up the Ransac Plane Fitting Object:
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100); // was 4000
seg.setDistanceThreshold (distance_threshold_); // 0.01 for table data set
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
#if DO_TIMING_PROFILE
tic_toc.push_back(bot_timestamp_now());
#endif
int n_major = 0, nr_points = cloud_filtered->points.size ();
//vector<BasicPlaneX> plane_stack;
BasicPlane one_plane;
// Extract the primary planes:
// major planes are the coarse planes extracted via ransac. they might contain disconnected points
// these are broken up into minor planes which are portions of major planes
if(verbose_lcm > 2){
for (int i=0;i<7;i++){
char n_major_char[10];
sprintf(n_major_char,"%d",i);
/* ptcoll_cfg.id =210+ i+3;
ptcoll_cfg.reset=true;
ptcoll_cfg.name =(char*) "cloud_p ";
ptcoll_cfg.name.append(n_major_char);
ptcoll_cfg.npoints = 0;
float colorm_temp0[] ={-1.0,-1.0,-1.0,-1.0};
ptcoll_cfg.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float));
ptcoll_cfg.type=1;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg, *cloud_filtered);
*/
}
for (int i=0;i<7;i++){
/*
Ptcoll_cfg ptcoll_cfg2;
// the i below accounts for super planes in the same utime
ptcoll_cfg2.point_lists_id =pose_element_id; //bot_timestamp_now();
ptcoll_cfg2.collection = pose_coll_id;
ptcoll_cfg2.element_id = pose_element_id;
if (i==0){
ptcoll_cfg2.reset=true;
}else{
ptcoll_cfg2.reset=false;
}
ptcoll_cfg2.id =500;
ptcoll_cfg2.name.assign("Grown Stack Final");
ptcoll_cfg2.npoints = 0;
ptcoll_cfg2.type =1;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg2, *cloud_filtered);
*/
}
// TODO: this will rest this object. need to add publish of reset
// at start of this block and set rese ttofal
for (int i=0;i<7;i++){
/*
Ptcoll_cfg ptcoll_cfg2;
// the i below accounts for super planes in the same utime
ptcoll_cfg2.point_lists_id =pose_element_id; //bot_timestamp_now();
ptcoll_cfg2.collection = pose_coll_id;
ptcoll_cfg2.element_id = pose_element_id;
if (i==0){
ptcoll_cfg2.reset=true;
}else{
ptcoll_cfg2.reset=false;
}
ptcoll_cfg2.id =501;
ptcoll_cfg2.name.assign("Grown Stack Final Hull");
ptcoll_cfg2.npoints = 0;
ptcoll_cfg2.type =3;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg2, *cloud_filtered);
*/
}
}
#if DO_TIMING_PROFILE
tic_toc.push_back(bot_timestamp_now());
#endif
while (cloud_filtered->points.size () > stop_proportion_ * nr_points) {
// While XX% of the original cloud is still there
char n_major_char[10];
sprintf(n_major_char,"%d",n_major);
if (n_major >6) {
if (verbose_text >0){
std::cout << n_major << " is the max number of planes to find, quitting" << std::endl;
}
break;
}
//a 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;
}
if (inliers->indices.size () < stop_cloud_size_) // stop when the plane is only a few points
{
//std::cerr << "No remaining planes in this set" << std::endl;
break;
}
//b Extract the inliers
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_p);
if (verbose_text>0){
std::cout << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;
}
// Create the filtering object
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud_p);
sor.setMeanK (30);
sor.setStddevMulThresh (0.5); //was 1.0
sor.filter (*cloud_p);
if(verbose_lcm > 2){
/*
ptcoll_cfg.id =210+ n_major+3;
ptcoll_cfg.reset=true;
ptcoll_cfg.name ="cloud_p ";
ptcoll_cfg.name.append(n_major_char);
ptcoll_cfg.npoints = cloud_p->points.size();
float colorm_temp0[] ={-1.0,-1.0,-1.0,-1.0};
ptcoll_cfg.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float));
ptcoll_cfg.type=1;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg, *cloud_p);
*/
}
vector<BasicPlane> grown_stack;
vector<BasicPlane> * grown_stack_ptr;
grown_stack_ptr = &grown_stack;
GrowCloud grow;
grow.setInputCloud(cloud_p);
grow.setMinCloudSize(stop_cloud_size_); // useing stop cloud size here too
grow.setLCM(publish_lcm);
grow.doGrowCloud(*grown_stack_ptr);
if (verbose_text>0){
cout << "grow_cloud new found " << grown_stack.size() << " seperate clouds\n";
}
// Spit raw clouds out to LCM:
if(verbose_lcm > 2){
for (int i=0;i<grown_stack.size();i++){
/*
Ptcoll_cfg ptcoll_cfg2;
ptcoll_cfg2.reset=false;
// the i below accounts for super planes in the same utime
ptcoll_cfg2.point_lists_id =10*n_major + i; //filterplanes->pose_element_id;
ptcoll_cfg2.collection = pose_coll_id;
ptcoll_cfg2.element_id = pose_element_id;
ptcoll_cfg2.id =500;
ptcoll_cfg2.name.assign("Grown Stack Final");
ptcoll_cfg2.npoints = grown_stack[i].cloud.points.size ();
ptcoll_cfg2.type =1;
int id = plane_stack.size() + i; // 10*n_major + i;
float colorm_temp0[] ={colors[3*(id%num_colors)],colors[3*(id%num_colors)+1],colors[3*(id%num_colors)+2] ,0.0};
ptcoll_cfg2.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float));
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg2, grown_stack[i].cloud);
*/
}
}
BasicPlane one_plane;
int n_minor=0;
BOOST_FOREACH( BasicPlane one_plane, grown_stack ){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZRGB>);
// c Project the model inliers (seems to be necessary to fitting convex hull
pcl::ProjectInliers<pcl::PointXYZRGB> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp (new pcl::PointCloud<pcl::PointXYZRGB> ());
*temp = (one_plane.cloud);
proj.setInputCloud (temp);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud_projected);
std::vector <pcl::Vertices> vertices;
if (1==1){ // convex:
pcl::ConvexHull<pcl::PointXYZRGB> chull;
chull.setInputCloud (cloud_projected);
chull.setDimension(2);
chull.reconstruct (*cloud_hull,vertices);
}else { // concave
pcl::ConcaveHull<pcl::PointXYZRGB> chull;
chull.setInputCloud (cloud_projected);
chull.setKeepInformation(true);
chull.setAlpha(0.5);
// for arch way:
// 1.1 too few
// 0.7 a little to few but much better
chull.reconstruct (*cloud_hull,vertices);
}
//std::cout << "Hull has: " << cloud_hull->points.size () << " vertices." << std::endl;
if (cloud_hull->points.size () ==0){
cout <<"ERROR: CONVEX HULL HAS NO POINTS! - NEED TO RESOLVE THIS\n";
}
// d.2 Find the mean colour of the hull:
int rgba[]={0,0,0,0};
for(int i=0;i<temp->points.size();i++){
int rgba_one = *reinterpret_cast<int*>(&temp->points[i].rgba);
rgba[3] += ((rgba_one >> 24) & 0xff);
rgba[2] += ((rgba_one >> 16) & 0xff);
rgba[1] += ((rgba_one >> 8) & 0xff);
rgba[0] += (rgba_one & 0xff);
}
double scale = ((double) temp->points.size());
rgba[3] =(int) round(((double) rgba[3]) / scale);
rgba[2] =(int) round(((double) rgba[2]) / scale);
rgba[1] =(int) round(((double) rgba[1]) / scale);
rgba[0] =(int) round(((double) rgba[0]) / scale);
// Write the plane to file for experimentation:
//stringstream oss;
//oss << ptcoll_cfg.element_id <<"_" << ptcoll_cfg.name << ".pcd";
//writer.write (oss.str(), *this_hull, false);
for(int i=0;i<cloud_hull->points.size();i++){
unsigned char* rgba_ptr = (unsigned char*)&cloud_hull->points[i].rgba;
(*rgba_ptr) = rgba[0] ;
(*(rgba_ptr+1)) = rgba[1];
(*(rgba_ptr+2)) = rgba[2];
(*(rgba_ptr+3)) = rgba[3];
}
(one_plane.coeffs) = *coefficients;
one_plane.cloud = *cloud_hull;
one_plane.utime = pose_element_id;
one_plane.major = n_major;
one_plane.minor = n_minor;
one_plane.n_source_points = cloud_projected->points.size();
plane_stack.push_back(one_plane);
n_minor++;
// int stop;
// cout << "int to cont: ";
// cin >> stop;
}
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud_filtered);
n_major++;
}
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// Container for original & filtered data
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
// pcl::PCLPointCloud2 cloud_filtered;
pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
// Convert to PCL data type
pcl_conversions::toPCL(*input, *cloud_blob);
// Perform the actual filtering
pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2);
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloud_blob);
sor.setLeafSize (0.05, 0.05, 0.05);
sor.setFilterFieldName("z");
sor.setFilterLimits(0.01, 0.3);
sor.filter (*cloud_filtered_blob);
// // Remove outlier X
// pcl::PCLPointCloud2::Ptr cloud_filtered_blobx (new pcl::PCLPointCloud2);
// pcl::VoxelGrid<pcl::PCLPointCloud2> sorx;
// sorx.setInputCloud(cloud_filtered_blobz);
// sorx.setFilterFieldName("x");
// sorx.setFilterLimits(-1, 1);
// sorx.filter(*cloud_filtered_blobx);
// // Remove outlier Y
// pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2);
// pcl::VoxelGrid<pcl::PCLPointCloud2> sory;
// sory.setInputCloud(cloud_filtered_blobx);
// sory.setFilterFieldName("y");
// sory.setFilterLimits(-1, 1);
// sory.filter(*cloud_filtered_blob);
// Convert to the templated PointCloud
pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);
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);
Eigen::Vector3f upVector(0, 0, 1);
seg.setAxis(upVector);
seg.setEpsAngle(1.5708);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.05);
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
return;
}
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZ> extract;
// Extract the inliers
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_p);
// Publish inliers
// sensor_msgs::PointCloud2 inlierpc;
// pcl_conversions::fromPCL(cloud_p, inlierpc);
pub.publish (*cloud_p);
// Publish the model coefficients
pcl_msgs::ModelCoefficients ros_coefficients;
pcl_conversions::fromPCL(*coefficients, ros_coefficients);
pubCoef.publish (ros_coefficients);
// ==========================================
// // Convert to ROS data type
// sensor_msgs::PointCloud2 downpc;
// pcl_conversions::fromPCL(cloud_filtered, downpc);
// // Publish the data
// pub.publish (downpc);
// pcl::ModelCoefficients coefficients;
// pcl::PointIndices inliers;
// //.makeShared()
// // Create the segmentation object
// pcl::SACSegmentation<pcl::PointXYZ> seg;
// seg.setInputCloud (&cloudPtr);
// seg.setOptimizeCoefficients (true); // Optional
// seg.setModelType (pcl::SACMODEL_PLANE); // Mandatory
// seg.setMethodType (pcl::SAC_RANSAC);
// seg.setDistanceThreshold (0.1);
// seg.segment (inliers, coefficients);
// if (inliers.indices.size () == 0)
// {
// PCL_ERROR ("Could not estimate a planar model for the given dataset.");
// return;
// }
// std::cerr << "Model coefficients: " << coefficients.values[0] << " "
// << coefficients.values[1] << " "
// << coefficients.values[2] << " "
// << coefficients.values[3] << std::endl;
// // Publish the model coefficients
// pcl_msgs::ModelCoefficients ros_coefficients;
// pcl_conversions::fromPCL(coefficients, ros_coefficients);
// pubCoef.publish (ros_coefficients);
}
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2), cloud_filtered_blob (new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
pcl_conversions::toPCL(*input, *cloud_blob);
// Fill in the cloud data
//pcl::PCDReader reader;
//reader.read ("table_scene_lms400.pcd", *cloud_blob);
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloud_blob);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered_blob);
// Convert to the templated PointCloud
pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);
//std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
// Write the downsampled version to disk
//pcl::PCDWriter writer;
//writer.write<pcl::PointXYZ> ("table_scene_lms400_downsampled.pcd", *cloud_filtered, false);
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.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZ> extract;
// int i = 0, nr_points = (int) cloud_filtered->points.size ();
// While 30% of the original cloud is still there
//while (cloud_filtered->points.size () > 0.3 * nr_points)
for (int i = 0; i < 2; i++)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
ROS_ERROR("Could not estimate a planar model for the given dataset.");
break;
}
// Extract the inliers
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_p);
// std::stringstream ss;
// ss << "table_scene_lms400_plane_" << i << ".pcd";
// writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud_f);
cloud_filtered.swap (cloud_f);
//i++;
}
//pcl::PCLPointCloud2 pre_output;
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(*cloud_p, output);
//pcl_conversions::fromPCL(pre_output, output);
pub.publish(output);
}
int
main (int argc, char** argv)
{
if (argc < 2){
std::cerr << "Usage:" << argv[0] << " PCD_filename [-smoothness 7.0] [-distance 0.1]"
<< std::endl;
return 0;
}
const char *filename = argv[1];
double smoothness=7.0, distance=0.01;
for (int i=2; i<argc; i++){
if (strcmp("-smoothness",argv[i])==0){
smoothness=atof(argv[++i]);
}else if(strcmp("-distance", argv[i])==0){
distance=atof(argv[++i]);
}
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PCDReader reader;
reader.read (filename, *cloud);
int npoint = cloud->points.size();
std::cout << "npoint = " << npoint << std::endl;
pcl::search::Search<pcl::PointXYZRGB>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZRGB> > (new pcl::search::KdTree<pcl::PointXYZRGB>);
pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> normal_estimator;
normal_estimator.setSearchMethod (tree);
normal_estimator.setInputCloud (cloud);
normal_estimator.setKSearch (50);
normal_estimator.compute (*normals);
pcl::RegionGrowing<pcl::PointXYZRGB, pcl::Normal> reg;
reg.setMinClusterSize (100);
reg.setMaxClusterSize (40000);
reg.setSearchMethod (tree);
reg.setNumberOfNeighbours (30);
reg.setInputCloud (cloud);
reg.setInputNormals (normals);
reg.setSmoothnessThreshold (smoothness / 180.0 * M_PI);
reg.setCurvatureThreshold (1.0);
std::vector <pcl::PointIndices> clusters;
reg.extract (clusters);
std::cout << "Number of clusters is equal to " << clusters.size () << std::endl;
pcl::visualization::PCLVisualizer viewer("Cloud Viewer");
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (distance);
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_p(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
int count = 1;
for (unsigned int i=0; i<clusters.size(); i++){
std::cout << "cluster" << i << std::endl;
extract.setInputCloud( cloud );
*inliers = clusters[i];
extract.setIndices( inliers );
extract.setNegative( false );
extract.filter( *cluster );
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = cluster;
while(cloud->points.size() > (int)(cluster->points.size())*0.1){
std::cout << "count = " << count << 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.");
return (-1);
}
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
std::cerr << "roll:" << -asin(coefficients->values[1])
<< "[rad](" << -asin(coefficients->values[1])*180/M_PI
<< "[deg]), pitch:" << asin(coefficients->values[0])
<< "[rad](" << asin(coefficients->values[0])*180/M_PI
<< "[deg])" << std::endl;
extract.setInputCloud( cloud );
extract.setIndices( inliers );
extract.setNegative( false );
extract.filter( *cloud_p );
for (unsigned int i=0; i<cloud_p->points.size(); i++){
cloud_p->points[i].r = count&1 ? 255 : 0;
cloud_p->points[i].g = count&2 ? 255 : 0;
cloud_p->points[i].b = count&4 ? 255 : 0;
}
std::stringstream ss;
ss << "cloud_" << count;
viewer.addPointCloud( cloud_p, ss.str() );
// 平面として選ばれなかった側の点に対する処理
extract.setNegative( true );
extract.filter( *cloud_f );
cloud = cloud_f;
// カウントアップ
count++;
}
}
while (!viewer.wasStopped ()){
viewer.spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return (0);
}
void PclExtractor::cloudCallback(const Cloud2Msg::ConstPtr& cloud2Msg_input)
{
boost::mutex::scoped_lock(mutex_);
sensor_msgs::PointCloud2 output;
sensor_msgs::PointCloud2 convex_hull;
sensor_msgs::PointCloud2 object_msg;
sensor_msgs::PointCloud2 nonObject_msg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg (*cloud2Msg_input, *cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// *** Normal estimation
// Create the normal estimation class and pass the input dataset to it
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);
// Creating the kdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
ne.setSearchMethod(tree);
// output dataset
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
// use all neighbors in a sphere of radius 3cm
ne.setRadiusSearch(0.3);
// compute the features
ne.compute(*cloud_normals);
// *** End of normal estimation
// *** Plane Estimation From Normals Start
// Create the segmentation object
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
// Optional
seg.setOptimizeCoefficients(true);
// Mandatory
// seg.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
seg.setModelType(pcl::SACMODEL_PLANE);
// seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
//y가 z
// const Eigen::Vector3f z_axis(0,-1.0,0);
// seg.setAxis(z_axis);
// seg.setEpsAngle(seg_setEpsAngle_);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations (seg_setMaxIterations_);
seg.setDistanceThreshold(seg_setDistanceThreshold_);
seg.setNormalDistanceWeight(seg_setNormalDistanceWeight_);
// seg.setProbability(seg_probability_);
seg.setInputCloud((*cloud).makeShared());
seg.setInputNormals(cloud_normals);
seg.segment(*inliers, *coefficients);
std::cerr <<"input: "<<cloud->width*cloud->height<<"Model inliers: " << inliers->indices.size () << std::endl;
if (inliers->indices.size () == 0)
{
ROS_ERROR ("Could not estimate a planar model for the given dataset.");
}
// *** End of Plane Estimation
// *** Plane Estimation Start
// Create the segmentation object
/* pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
//seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
// seg.setModelType(pcl::SACMODEL_PLANE);
// seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
//y가 z
const Eigen::Vector3f z_axis(0,-1.0,0);
seg.setAxis(z_axis);
seg.setEpsAngle(seg_setEpsAngle_);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations (seg_setMaxIterations_);
seg.setDistanceThreshold(seg_setDistanceThreshold_);
seg.setInputCloud((*cloud).makeShared());
seg.segment(*inliers, *coefficients);
std::cerr <<"input: "<<cloud->width*cloud->height<<"Model inliers: " << inliers->indices.size () << std::endl;
if (inliers->indices.size () == 0)
{
ROS_ERROR ("Could not estimate a planar model for the given dataset.");
}
// *** End of Plane Estimation
*/
pcl::ExtractIndices<pcl::PointXYZ> extract;
// Extrat the inliers
extract.setInputCloud(cloud);
extract.setIndices(inliers);
pcl::PointCloud<pcl::PointXYZ>::Ptr ground_hull(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);
if ((int)inliers->indices.size() > minPoints_)
{
extract.setNegative(false);
extract.filter(*cloud_p);
pcl::toROSMsg(*cloud_p, output);
std::cerr << "PointCloud representing the planar component: "
<< cloud_p->width * cloud_p->height << " data points." << std::endl;
// Step 3c. Project the ground inliers
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType(pcl::SACMODEL_PLANE);
proj.setInputCloud(cloud_p);
proj.setModelCoefficients(coefficients);
proj.filter(*cloud_projected);
// Step 3d. Create a Convex Hull representation of the projected inliers
pcl::ConvexHull<pcl::PointXYZ> chull;
//chull.setInputCloud(cloud_p);
chull.setInputCloud(cloud_projected);
chull.setDimension(chull_setDimension_);//2D
chull.reconstruct(*ground_hull);
pcl::toROSMsg(*ground_hull, convex_hull);
//pcl::toROSMsg(*ground_hull, convex_hull);
ROS_INFO ("Convex hull has: %d data points.", (int) ground_hull->points.size ());
// Publish the data
plane_pub_.publish (output);
hull_pub_.publish(convex_hull);
}
// Create the filtering object
extract.setNegative(true);
extract.filter(*cloud_f);
ROS_INFO ("Cloud_f has: %d data points.", (int) cloud_f->points.size ());
// cloud.swap(cloud_f);
pcl::PointCloud<pcl::PointXYZ>::Ptr object(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr nonObject(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointIndices::Ptr object_indices(new pcl::PointIndices);
// cloud statictical filter
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudStatisticalFiltered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud_f);
sor.setMeanK(sor_setMeanK_);
sor.setStddevMulThresh(sor_setStddevMulThresh_);
sor.filter(*cloudStatisticalFiltered);
pcl::ExtractIndices<pcl::PointXYZ> exObjectIndices;
//exObjectIndices.setInputCloud(cloud_f);
exObjectIndices.setInputCloud(cloudStatisticalFiltered);
exObjectIndices.setIndices(object_indices);
exObjectIndices.filter(*object);
exObjectIndices.setNegative(true);
exObjectIndices.filter(*nonObject);
ROS_INFO ("Object has: %d data points.", (int) object->points.size ());
pcl::toROSMsg(*object, object_msg);
object_pub_.publish(object_msg);
//pcl::toROSMsg(*nonObject, nonObject_msg);
//pcl::toROSMsg(*cloud_f, nonObject_msg);
pcl::toROSMsg(*cloudStatisticalFiltered, nonObject_msg);
nonObject_pub_.publish(nonObject_msg);
}
void rgb_pcl::cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input){
// Container for original & filtered data
#if PR2
if(target_frame.find(base_frame) == std::string::npos){
getTransformCloud(input, *input);
}
sensor_msgs::PointCloud2 in = *input;
sensor_msgs::PointCloud2 out;
pcl_ros::transformPointCloud(target_frame, net_transform, in, out);
#endif
// ROS_INFO("Cloud acquired...");
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>),
cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>),
cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);
Mat displayImage = cv::Mat(Size(640, 480), CV_8UC3);
displayImage = Scalar(120);
// Convert to PCL data type
#if PR2
pcl_conversions::toPCL(out, *cloud);
#endif
#if !PR2
pcl_conversions::toPCL(*input, *cloud);
#endif
// ROS_INFO("\t=>Cloud rotated...");
// Perform the actual filtering
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloudPtr);
sor.setLeafSize (0.005, 0.005, 0.005);
sor.filter (*cloud_filtered_blob);
pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);
ModelCoefficientsPtr coefficients (new pcl::ModelCoefficients);
PointCloudPtr plane_points(new PointCloud), point_points_2d_hull(new PointCloud);
std::vector<PointCloudPtr> object_clouds;
pcl::PointCloud<pcl::PointXYZRGB> combinedCloud;
#if PR2
make_crop_box_marker(marker_publisher, base_frame, 0, 0.2, -1, 0.2, 1.3, 2, 1.3);
// Define your cube with two points in space:
Eigen::Vector4f minPoint;
minPoint[0]=0.2; // define minimum point x
minPoint[1]=-1; // define minimum point y
minPoint[2]=0.2; // define minimum point z
Eigen::Vector4f maxPoint;
maxPoint[0]=1.5; // define max point x
maxPoint[1]=1; // define max point y
maxPoint[2]=1.5; // define max point z
pcl::CropBox<pcl::PointXYZRGB> cropFilter;
cropFilter.setInputCloud (cloud_filtered);
cropFilter.setMin(minPoint);
cropFilter.setMax(maxPoint);
cropFilter.filter (*cloud_filtered);
#endif
#if !PR2
//Rotate the point cloud
Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
// Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
float theta = M_PI; // The angle of rotation in radians
// Define a translation of 0 meters on the x axis
transform_1.translation() << 0.0, 0.0, 1.0;
// The same rotation matrix as before; tetha radians arround X axis
transform_1.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
// Executing the transformation
pcl::transformPointCloud (*cloud_filtered, *cloud_filtered, transform_1);
#endif
interpretTableScene(cloud_filtered, coefficients, plane_points, point_points_2d_hull, object_clouds);
int c = 0;
#if PUBLISH_CLOUDS
int ID_object = -1;
#endif
for(auto cloudCluster: object_clouds){
// get_cloud_matching(cloudCluster); //histogram matching
#if PUBLISH_CLOUDS
ID_object = c;
#endif
combinedCloud += *cloudCluster;
combinedCloud.header = cloud_filtered->header;
c++;
}
#if DISPLAY
drawPointCloud(combinedCloud, displayImage);
#endif
getTracker(object_clouds, displayImage);
stateDetection();
// ROS_INFO("\t=>Cloud analysed...");
#if PUBLISH_CLOUDS
sensor_msgs::PointCloud2 output;
if(object_clouds.size() >= ID_object && ID_object >= 0){
pcl::toROSMsg(combinedCloud, output);
// Publish the data
pub.publish (output);
}
#endif
end = ros::Time::now();
std::stringstream ss;
ss <<(end-begin);
string s_FPS = ss.str();
#if DISPLAY
cv::putText(displayImage, "FPS: "+to_string((int)1/(stof(s_FPS))) + " Desired: "+to_string(DESIRED_FPS), cv::Point(10, 10), CV_FONT_HERSHEY_COMPLEX, 0.4, Scalar(0,0,0));
imshow("RGB", displayImage);
#endif
waitKey(1);
begin = ros::Time::now();
}
void cloud_cb (const pcl::PCLPointCloud2ConstPtr& cloud_blob)
{
std::cout<<"Received Kinect message\n";
pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>), cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>), cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloud_blob);
boost::this_thread::sleep (boost::posix_time::microseconds (100));
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered_blob);
// Convert to the templated PointCloud
pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (cloud_filtered);
int i = 0, nr_points = (int) cloud_filtered->points.size ();
pcl::IndicesPtr remaining (new std::vector<int>);
remaining->resize (nr_points);
for (size_t i = 0; i < remaining->size (); ++i) { (*remaining)[i] = static_cast<int>(i); }
std::cout << "here again" << std::endl;
// While 30% of the original cloud is still there
while (remaining->size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setIndices (remaining);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0) break;
// Extract the inliers
std::vector<int>::iterator it = remaining->begin();
for (size_t i = 0; i < inliers->indices.size (); ++i)
{
int curr = inliers->indices[i];
// Remove it from further consideration.
while (it != remaining->end() && *it < curr) { ++it; }
if (it == remaining->end()) break;
if (*it == curr) it = remaining->erase(it);
}
i++;
}
std::cout << "Found " << i << " planes." << std::endl;
// Color all the non-planar things.
for (std::vector<int>::iterator it = remaining->begin(); it != remaining->end(); ++it)
{
uint8_t r = 0, g = 255, b = 0;
uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
cloud_filtered->at(*it).rgb = *reinterpret_cast<float*>(&rgb);
}
pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
viewer.showCloud (cloud_filtered);
while (!viewer.wasStopped ())
{
}
// Publish the planes we found.
pcl::PCLPointCloud2 outcloud;
pcl::toPCLPointCloud2 (*cloud_filtered, outcloud);
pub.publish (outcloud);
}
void point_cb(const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 cloud_filtered;
pcl_conversions::toPCL(*cloud_msg, *cloud);
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud(cloudPtr);
float leaf = 0.005;
sor.setLeafSize(leaf, leaf, leaf);
sor.filter(cloud_filtered);
sensor_msgs::PointCloud2 sensor_pcl;
pcl_conversions::moveFromPCL(cloud_filtered, sensor_pcl);
//publish pcl data
pub_voxel.publish(sensor_pcl);
global_counter++;
if((theta == 0.0 && y_offset == 0.0) || global_counter < 800 ){
// part for planar segmentation starts here ..
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>), cloud_p(new pcl::PointCloud<pcl::PointXYZ>), cloud_seg(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p_rotated(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p_transformed(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>);
sensor_msgs::PointCloud2 plane_sensor, plane_transf_sensor;
//convert sen
pcl::fromROSMsg(*cloud_msg, *cloud1);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.01);
seg.setInputCloud(cloud1);
seg.segment (*inliers, *coefficients);
Eigen::Matrix<float, 1, 3> surface_normal;
Eigen::Matrix<float, 1, 3> floor_normal;
surface_normal[0] = coefficients->values[0];
surface_normal[1] = coefficients->values[1];
surface_normal[2] = coefficients->values[2];
std::cout << surface_normal[0] << "\n" << surface_normal[1] << "\n" << surface_normal[2];
floor_normal[0] = 0.0;
floor_normal[1] = 1.0;
floor_normal[2] = 0.0;
theta = acos(surface_normal.dot(floor_normal));
//extract the inliers - copied from tutorials...
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud1);
extract.setIndices (inliers);
extract.setNegative(true);
extract.filter(*cloud_p);
ROS_INFO("print cloud info %d", cloud_p->height);
pcl::toROSMsg(*cloud_p, plane_sensor);
pub_plane_simple.publish(plane_sensor);
// anti rotate the point cloud by first finding the angle of rotation
Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
transform_1.translation() << 0.0, 0.0, 0.0;
transform_1.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
pcl::transformPointCloud(*cloud_p, *cloud_p_rotated, transform_1);
double y_sum = 0;
// int num_points =
for (int i = 0; i < 20; i++){
y_sum = cloud_p_rotated->points[i].y;
}
y_offset = y_sum / 20;
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
transform_2.translation() << 0.0, -y_offset, 0.0;
transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
pcl::transformPointCloud(*cloud_p, *cloud_p_transformed, transform_2);
pcl::transformPointCloud(*cloud1, *cloud_transformed, transform_2);
//now remove the y offset because of the camera rotation
pcl::toROSMsg(*cloud_p_transformed, plane_transf_sensor);
// pcl::toROSMsg(*cloud_transformed, plane_transf_sensor);
// pcl::toROSMsg(*cloud1, plane_transf_sensor);
pub_plane_transf.publish(plane_transf_sensor);
}
ras_msgs::Cam_transform cft;
cft.theta = theta;
cft.y_offset = y_offset;
pub_ctf.publish(cft);
// pub_tf.publish();
}
