From 4128892f1cd87f4f0a587254efe95d6488e6d8f1 Mon Sep 17 00:00:00 2001 From: HaoLi-China Date: Sat, 10 Jan 2015 01:40:02 +0800 Subject: [PATCH 1/2] fixed the empty point bug in over-segmentation. --- .../Algorithm/ObjPreSegment/scene_seg.cpp | 88 ++++++++++++------- .../Algorithm/ObjPreSegment/scene_seg.h | 8 +- .../ObjPreSegment/supervoxel_clustering.hpp | 13 +-- .../Point Cloud/UI/dlg_camera_para.cpp | 20 +++-- 4 files changed, 83 insertions(+), 46 deletions(-) diff --git a/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/scene_seg.cpp b/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/scene_seg.cpp index e826ea1..25b0aed 100644 --- a/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/scene_seg.cpp +++ b/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/scene_seg.cpp @@ -847,7 +847,7 @@ void detect_separation_plane(PointCloudPtr_RGB_NORMAL cloud, vector extract;// Create the filtering object + PointCloudPtr_RGB_NORMAL cloud_tem(new PointCloud_RGB_NORMAL); + PointCloudPtr_RGB_NORMAL cloud_p(new PointCloud_RGB_NORMAL); - pcl::PointIndices::Ptr inliers (new pcl::PointIndices); - // Create the segmentation object - pcl::SACSegmentation seg; - // Optional - seg.setOptimizeCoefficients (true); - // Mandatory - seg.setModelType (pcl::SACMODEL_PLANE); - seg.setMethodType (pcl::SAC_RANSAC); - seg.setMaxIterations (1000); - seg.setDistanceThreshold (0.015); + pcl::copyPointCloud(*sourceCloud, *cloud_tem); + while(1){ + pcl::PointIndices::Ptr inliers (new pcl::PointIndices); + // Create the segmentation object + pcl::SACSegmentation seg; + // Optional + seg.setOptimizeCoefficients (true); + // Mandatory + seg.setModelType (pcl::SACMODEL_PLANE); + seg.setMethodType (pcl::SAC_RANSAC); + seg.setMaxIterations (1000); + seg.setDistanceThreshold (0.015); - // Segment the largest planar component from the remaining cloud - seg.setInputCloud (sourceCloud); - seg.segment (*inliers, plane_coefficients); - if (inliers->indices.size () == 0) - { - std::cerr << "Could not estimate a planar model for the given dataset." << std::endl; - return; - } + // Segment the largest planar component from the remaining cloud + seg.setInputCloud (cloud_tem); + seg.segment (*inliers, plane_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 (sourceCloud); - extract.setIndices (inliers); - extract.setNegative (false); - extract.filter (*planeCloud); - std::cerr << "PointCloud representing the planar component: " << planeCloud->width * planeCloud->height << " data points." << std::endl; + //showPointCloud(cloud_tem, "cloud_tem"); - getRectForPlaneCloud(planeCloud, boost::make_shared(plane_coefficients), rect_cloud); + // Extract the inliers + extract.setInputCloud (cloud_tem); + extract.setIndices (inliers); + extract.setNegative (false); + extract.filter (*planeCloud); + std::cerr << "PointCloud representing the planar component: " << planeCloud->width * planeCloud->height << " data points." << std::endl; - // Create the filtering object - extract.setNegative (true); - extract.filter (*remainingCloud); + //showPointCloud(planeCloud, "planeCloud"); + + getRectForPlaneCloud(planeCloud, boost::make_shared(plane_coefficients), rect_cloud); + + // Create the filtering object + extract.setNegative (true); + extract.filter (*cloud_p); + //showPointCloud(cloud_p, "cloud_p"); + + pcl::copyPointCloud(*cloud_p, *cloud_tem); + + Eigen::Vector3d normal; + normal << plane_coefficients.values[0], plane_coefficients.values[1], plane_coefficients.values[2]; + normal.normalize(); + + if(std::abs(normal.dot(Eigen::Vector3d(0,0,1)))>0.7){ + break; + } + } + + pcl::copyPointCloud(*cloud_tem, *remainingCloud); } //get transform matrix between plane and x_y plane @@ -1171,7 +1193,7 @@ void getCloudOnTable(PointCloudPtr_RGB_NORMAL cloud, PointCloudPtr rect_cloud, E //} //Euclidean Cluster Extraction -void object_seg_ECE(PointCloudPtr_RGB_NORMAL cloud, std::vector &cluster_points){ +void object_seg_ECE(PointCloudPtr_RGB_NORMAL cloud, std::vector &cluster_points){ // Creating the KdTree object for the search method of the extraction pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); tree->setInputCloud (cloud); @@ -1192,7 +1214,10 @@ void object_seg_ECE(PointCloudPtr_RGB_NORMAL cloud, std::vector::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++){ cloud_cluster->points.push_back (cloud->points[*pit]); //* } - cluster_points.push_back(cloud_cluster); + + MyPointCloud_RGB_NORMAL mpc; + PointCloud_RGB_NORMAL2MyPointCloud_RGB_NORMAL(cloud_cluster, mpc); + cluster_points.push_back(mpc); j++; } @@ -2332,6 +2357,7 @@ void object_seg_Plane(PointCloudPtr_RGB_NORMAL sourceCloud, std::vector& patch_clouds, PointCloudT::Ptr colored_cloud, PointNCloudT::Ptr normal_cloud){ + PointCloudT::Ptr ct(new PointCloudT); NormalCloudTPtr normals(new NormalCloudT); diff --git a/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/scene_seg.h b/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/scene_seg.h index 0467b10..7d2c54a 100644 --- a/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/scene_seg.h +++ b/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/scene_seg.h @@ -1,9 +1,9 @@ #ifndef SCENE_SEG_H #define SCENE_SEG_H -#include "Algorithm/Common/common_func.h" -#include "Algorithm/Common/common_type.h" -#include "Algorithm/Common/visualizer.h" +#include "../Common/common_func.h" +#include "../Common/common_type.h" +#include "../Common/visualizer.h" #include @@ -57,7 +57,7 @@ void getCloudOnTable(PointCloudPtr_RGB_NORMAL cloud, PointCloudPtr rect_cloud, E //detect table plane void detect_table_plane(PointCloudPtr_RGB_NORMAL sourceCloud, PointCloudPtr_RGB_NORMAL planeCloud, PointCloudPtr_RGB_NORMAL remainCloud); //Euclidean Cluster Extraction -void object_seg_ECE(PointCloudPtr_RGB_NORMAL clound, vector &cluster_points); +void object_seg_ECE(PointCloudPtr_RGB_NORMAL cloud, std::vector &cluster_points); //find a minimum bounding rect void find_min_rect(PointCloudPtr_RGB_NORMAL cloud, cv::Point2f &p0,cv::Point2f &p1,cv::Point2f &p2,cv::Point2f &p3); //VCCS over-segmentation diff --git a/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/supervoxel_clustering.hpp b/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/supervoxel_clustering.hpp index 5c17689..1261a90 100644 --- a/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/supervoxel_clustering.hpp +++ b/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/supervoxel_clustering.hpp @@ -333,16 +333,19 @@ template void for (std::vector::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++){ cloud_cluster->points.push_back (voxels_tem->points[*pit]); //* } - + if(cloud_cluster->size()<10){ for(int i=0;isize();i++){ LeafContainerT* leaf_arg = adjacency_octree_->getLeafContainerAtPoint(cloud_cluster->at(i)); VoxelData& data_tem = leaf_arg->getData(); - data_tem.owner_ = 0; - data_tem.distance_ = std::numeric_limits::max (); - sv_itr->removeLeaf(leaf_arg); - } + if(leaf_arg){ + data_tem.owner_ = 0; + data_tem.distance_ = std::numeric_limits::max (); + + sv_itr->removeLeaf(leaf_arg); + } + } } } } diff --git a/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/UI/dlg_camera_para.cpp b/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/UI/dlg_camera_para.cpp index f90a749..0b880f1 100644 --- a/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/UI/dlg_camera_para.cpp +++ b/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/UI/dlg_camera_para.cpp @@ -1609,19 +1609,23 @@ int CameraParaDlg::getUpdateData() float normal_importance = 1.0f; /******************Euclidean Cluster Extraction************************/ - std::vector cluster_points; + std::vector cluster_points; object_seg_ECE(tabletopCloud, cluster_points); for(int i=0;isize()<200){ + if(cluster_points.at(i).mypoints.size()<200){ continue; } PointCloudT::Ptr colored_cloud(new PointCloudT); vector patch_clouds; PointNCloudT::Ptr normal_cloud(new PointNCloudT); - VCCS_over_segmentation(cluster_points.at(i),voxel_resolution,seed_resolution,color_importance,spatial_importance,normal_importance,patch_clouds,colored_cloud,normal_cloud); + + PointCloudPtr_RGB_NORMAL ct(new PointCloud_RGB_NORMAL); + MyPointCloud_RGB_NORMAL2PointCloud_RGB_NORMAL(cluster_points.at(i), ct); + + VCCS_over_segmentation(ct,voxel_resolution,seed_resolution,color_importance,spatial_importance,normal_importance,patch_clouds,colored_cloud,normal_cloud); std::stringstream str; str<<"colored_voxel_cloud"< cluster_points; + std::vector cluster_points; object_seg_ECE(tabletopCloud, cluster_points); for(int i=0;isize()<200){ + if(cluster_points.at(i).mypoints.size()<200){ continue; } PointCloudT::Ptr colored_cloud(new PointCloudT); vector patch_clouds; PointNCloudT::Ptr normal_cloud(new PointNCloudT); - VCCS_over_segmentation(cluster_points.at(i),voxel_resolution,seed_resolution,color_importance,spatial_importance,normal_importance,patch_clouds,colored_cloud,normal_cloud); + + PointCloudPtr_RGB_NORMAL ct(new PointCloud_RGB_NORMAL); + MyPointCloud_RGB_NORMAL2PointCloud_RGB_NORMAL(cluster_points.at(i), ct); + + VCCS_over_segmentation(ct,voxel_resolution,seed_resolution,color_importance,spatial_importance,normal_importance,patch_clouds,colored_cloud,normal_cloud); std::stringstream str; str<<"colored_voxel_cloud"< Date: Sat, 10 Jan 2015 02:32:46 +0800 Subject: [PATCH 2/2] fixed some bugs. --- .../Point Cloud/Algorithm/ObjPreSegment/scene_seg.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/scene_seg.cpp b/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/scene_seg.cpp index 25b0aed..1f5ae41 100644 --- a/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/scene_seg.cpp +++ b/Currently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/scene_seg.cpp @@ -971,9 +971,12 @@ void detect_table(PointCloudPtr_RGB_NORMAL sourceCloud, pcl::ModelCoefficients& if(std::abs(normal.dot(Eigen::Vector3d(0,0,1)))>0.7){ break; } + else{ + appendCloud_RGB_NORMAL(planeCloud, remainingCloud); + } } - pcl::copyPointCloud(*cloud_tem, *remainingCloud); + appendCloud_RGB_NORMAL(cloud_tem, remainingCloud); } //get transform matrix between plane and x_y plane @@ -1019,7 +1022,8 @@ void getCloudOnTable(PointCloudPtr_RGB_NORMAL cloud, PointCloudPtr rect_cloud, E p.normal_y = cloud_tem->at(k).normal_y; p.normal_z = cloud_tem->at(k).normal_z; - if(cloud_tem->at(k).z < rect_cloud_tem->at(0).z){ + //=========modify the height of tabletop things + if(cloud_tem->at(k).z < rect_cloud_tem->at(0).z + 0.015){ continue; }