void
v4r::MultiPlaneSegmentation<PointT>::segment(bool force_unorganized)
{
models_.clear();
if(input_->isOrganized() && !force_unorganized)
{
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
if(!normals_set_)
{
pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setMaxDepthChangeFactor (0.02f);
ne.setNormalSmoothingSize (20.0f);
ne.setBorderPolicy (pcl::IntegralImageNormalEstimation<PointT, pcl::Normal>::BORDER_POLICY_IGNORE);
ne.setInputCloud (input_);
ne.compute (*normal_cloud);
}
else
{
normal_cloud = normal_cloud_;
}
pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
mps.setMinInliers (min_plane_inliers_);
mps.setAngularThreshold (0.017453 * 2.f); // 2 degrees
mps.setDistanceThreshold (0.01); // 1cm
mps.setMaximumCurvature(0.002);
mps.setInputNormals (normal_cloud);
mps.setInputCloud (input_);
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
std::vector<pcl::ModelCoefficients> model_coefficients;
std::vector<pcl::PointIndices> inlier_indices;
pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
std::vector<pcl::PointIndices> label_indices;
std::vector<pcl::PointIndices> boundary_indices;
typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr ref_comp (
new pcl::PlaneRefinementComparator<PointT,
pcl::Normal, pcl::Label> ());
ref_comp->setDistanceThreshold (0.01f, false);
ref_comp->setAngularThreshold (0.017453 * 2.f);
mps.setRefinementComparator (ref_comp);
mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
//mps.segment (model_coefficients, inlier_indices);
//std::cout << model_coefficients.size() << std::endl;
if(merge_planes_)
{
//sort planes by size
//check if the first plane can be merged against the others, if yes, define a new plane combining both and add it to the cue
typedef boost::adjacency_matrix<boost::undirectedS, int> GraphPlane;
GraphPlane mergeable_planes (model_coefficients.size ());
for(size_t i=0; i < model_coefficients.size(); i++)
{
Eigen::Vector3f plane_i = Eigen::Vector3f (model_coefficients[i].values[0], model_coefficients[i].values[1],
model_coefficients[i].values[2]);
plane_i.normalize();
for(size_t j=(i+1); j < model_coefficients.size(); j++)
{
Eigen::Vector3f plane_j = Eigen::Vector3f (model_coefficients[j].values[0], model_coefficients[j].values[1],
model_coefficients[j].values[2]);
plane_j.normalize();
//std::cout << "dot product:" << plane_i.dot(plane_j) << " diff:" << std::abs(model_coefficients[i].values[3] - model_coefficients[j].values[3]) << std::endl;
if(plane_i.dot(plane_j) > 0.95)
{
if(std::abs(model_coefficients[i].values[3] - model_coefficients[j].values[3]) < 0.015)
{
boost::add_edge (static_cast<int>(i), static_cast<int>(j), mergeable_planes);
}
}
}
}
boost::vector_property_map<int> components (boost::num_vertices (mergeable_planes));
int n_cc = static_cast<int> (boost::connected_components (mergeable_planes, &components[0]));
std::vector<int> cc_sizes;
std::vector< std::vector<int> > cc_to_model_coeff;
cc_sizes.resize (n_cc, 0);
cc_to_model_coeff.resize(n_cc);
for (size_t i = 0; i < model_coefficients.size (); i++)
{
cc_sizes[components[i]]++;
cc_to_model_coeff[components[i]].push_back(i);
}
std::vector<pcl::ModelCoefficients> new_model_coefficients;
std::vector<pcl::PointIndices> new_inlier_indices;
for(size_t i=0; i < cc_sizes.size(); i++)
{
if(cc_sizes[i] < 2)
{
new_model_coefficients.push_back(model_coefficients[cc_to_model_coeff[i][0]]);
new_inlier_indices.push_back(inlier_indices[cc_to_model_coeff[i][0]]);
continue;
}
//std::cout << "going to merge CC:" << cc_sizes[i] << std::endl;
pcl::ModelCoefficients model_coeff;
model_coeff.values.resize(4);
for(size_t k=0; k < 4; k++)
model_coeff.values[k] = 0.f;
pcl::PointIndices merged_indices;
for(size_t j=0; j < cc_to_model_coeff[i].size(); j++)
{
for(size_t k=0; k < 4; k++)
model_coeff.values[k] += model_coefficients[cc_to_model_coeff[i][j]].values[k];
merged_indices.indices.insert(merged_indices.indices.end(), inlier_indices[cc_to_model_coeff[i][j]].indices.begin(),
inlier_indices[cc_to_model_coeff[i][j]].indices.end());
}
for(size_t k=0; k < 4; k++)
model_coeff.values[k] /= static_cast<float>(cc_to_model_coeff[i].size());
new_model_coefficients.push_back(model_coeff);
new_inlier_indices.push_back(merged_indices);
}
model_coefficients = new_model_coefficients;
inlier_indices = new_inlier_indices;
}
for(size_t i=0; i < model_coefficients.size(); i++)
{
PlaneModel<PointT> pm;
pm.coefficients_ = model_coefficients[i];
pm.cloud_ = input_;
pm.inliers_ = inlier_indices[i];
//recompute coefficients based on distance to camera and normal?
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*pm.cloud_, pm.inliers_, centroid);
Eigen::Vector3f c(centroid[0],centroid[1],centroid[2]);
Eigen::MatrixXf M_w(inlier_indices[i].indices.size(), 3);
float sum_w = 0.f;
for(size_t k=0; k < inlier_indices[i].indices.size(); k++)
{
const int &idx = inlier_indices[i].indices[k];
float d_c = (pm.cloud_->points[idx].getVector3fMap()).norm();
float w_k = std::max(1.f - std::abs(1.f - d_c), 0.f);
//w_k = 1.f;
M_w.row(k) = w_k * (pm.cloud_->points[idx].getVector3fMap() - c);
sum_w += w_k;
}
Eigen::Matrix3f scatter;
scatter.setZero ();
scatter = M_w.transpose() * M_w;
Eigen::JacobiSVD<Eigen::MatrixXf> svd(scatter, Eigen::ComputeFullV);
//std::cout << svd.matrixV() << std::endl;
Eigen::Vector3f n = svd.matrixV().col(2);
//flip normal if required
if(n.dot(c*-1) < 0)
n = n * -1.f;
float d = n.dot(c) * -1.f;
//std::cout << "normal:" << n << std::endl;
//std::cout << "d:" << d << std::endl;
pm.coefficients_.values[0] = n[0];
pm.coefficients_.values[1] = n[1];
pm.coefficients_.values[2] = n[2];
pm.coefficients_.values[3] = d;
pcl::PointIndices clean_inlier_indices;
float dist_threshold_ = 0.01f;
for(size_t k=0; k < inlier_indices[i].indices.size(); k++)
{
const int &idx = inlier_indices[i].indices[k];
Eigen::Vector3f p = pm.cloud_->points[idx].getVector3fMap();
float val = n.dot(p) + d;
if(std::abs(val) <= dist_threshold_)
clean_inlier_indices.indices.push_back( idx );
}
pm.inliers_ = clean_inlier_indices;
models_.push_back(pm);
}
}
else
{
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<PointT> vg;
PointTCloudPtr cloud_filtered (new PointTCloud);
vg.setInputCloud (input_);
float leaf_size_ = 0.005f;
float dist_threshold_ = 0.01f;
vg.setLeafSize (leaf_size_, leaf_size_, leaf_size_);
vg.filter (*cloud_filtered);
std::vector<bool> pixel_has_not_been_labelled( cloud_filtered->points.size(), true );
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<PointT> seg;
pcl::ModelCoefficients coefficients;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (dist_threshold_);
PointTCloudPtr cloud_filtered_leftover (new PointTCloud (*cloud_filtered));
while (1)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered_leftover);
pcl::PointIndices inliers_in_leftover;
seg.segment (inliers_in_leftover, coefficients);
std::cout << "inliers in left over: " << inliers_in_leftover.indices.size() << " of " << cloud_filtered_leftover->points.size() << std::endl;
if ( (int)inliers_in_leftover.indices.size () < min_plane_inliers_) // Could not estimate a(nother) planar model big enough for the given cloud.
break;
// make indices correspond to complete downsampled cloud
pcl::PointIndices indices_in_original_cloud;
size_t current_index_in_leftover = 0;
size_t px=0;
indices_in_original_cloud.indices.resize(inliers_in_leftover.indices.size());
for(size_t inl_id=0; inl_id < inliers_in_leftover.indices.size(); inl_id++) { // assumes indices are sorted in ascending order
bool found = false;
do {
if( pixel_has_not_been_labelled[px] ) {
if( current_index_in_leftover == inliers_in_leftover.indices [inl_id] ) {
indices_in_original_cloud.indices[ inl_id ] = px;
found = true;
}
current_index_in_leftover++;
}
px++;
} while( !found );
}
for(size_t i=0; i<indices_in_original_cloud.indices.size(); i++)
pixel_has_not_been_labelled[ indices_in_original_cloud.indices[i] ] = false;
//save coefficients
PlaneModel<PointT> pm;
pm.coefficients_ = coefficients;
pm.cloud_ = cloud_filtered;
pm.inliers_ = indices_in_original_cloud;
models_.push_back(pm);
pcl::copyPointCloud(*cloud_filtered, pixel_has_not_been_labelled, *cloud_filtered_leftover);
}
std::cout << "Number of planes found:" << models_.size() << "organized:" << static_cast<int>(input_->isOrganized() && !force_unorganized) << std::endl;
}
}
template <typename PointT> Eigen::VectorXf
open_ptrack::detection::GroundplaneEstimation<PointT>::compute ()
{
Eigen::VectorXf ground_coeffs;
ground_coeffs.resize(4);
// Manual mode:
if (ground_estimation_mode_ == 0)
{
std::cout << "Manual mode for ground plane estimation." << std::endl;
// Initialize viewer:
pcl::visualization::PCLVisualizer viewer("Pick 3 points");
// Create XYZ cloud:
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_xyzrgb(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointXYZRGB xyzrgb_point;
cloud_xyzrgb->points.resize(cloud_->width * cloud_->height, xyzrgb_point);
cloud_xyzrgb->width = cloud_->width;
cloud_xyzrgb->height = cloud_->height;
cloud_xyzrgb->is_dense = false;
for (int i=0; i<cloud_->height; i++)
{
for (int j=0; j<cloud_->width; j++)
{
cloud_xyzrgb->at(j,i).x = cloud_->at(j,i).x;
cloud_xyzrgb->at(j,i).y = cloud_->at(j,i).y;
cloud_xyzrgb->at(j,i).z = cloud_->at(j,i).z;
}
}
//#if (XYZRGB_CLOUDS)
// pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud_);
// viewer.addPointCloud<pcl::PointXYZRGB> (cloud_, rgb, "input_cloud");
//#else
// viewer.addPointCloud<pcl::PointXYZ> (cloud_, "input_cloud");
//#endif
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> rgb(cloud_xyzrgb, 255, 255, 255);
viewer.addPointCloud<pcl::PointXYZRGB> (cloud_xyzrgb, rgb, "input_cloud");
viewer.setCameraPosition(0,0,-2,0,-1,0,0);
// Add point picking callback to viewer:
struct callback_args_color cb_args;
//#if (XYZRGB_CLOUDS)
// PointCloudPtr clicked_points_3d (new PointCloud);
//#else
// pcl::PointCloud<pcl::PointXYZ>::Ptr clicked_points_3d (new pcl::PointCloud<pcl::PointXYZ>);
//#endif
pcl::PointCloud<pcl::PointXYZRGB>::Ptr clicked_points_3d (new pcl::PointCloud<pcl::PointXYZRGB>);
cb_args.clicked_points_3d = clicked_points_3d;
cb_args.viewerPtr = &viewer;
viewer.registerPointPickingCallback (GroundplaneEstimation::pp_callback, (void*)&cb_args);
// Spin until 'Q' is pressed:
viewer.spin();
viewer.setSize(1,1); // resize viewer in order to make it disappear
viewer.spinOnce();
viewer.close(); // close method does not work
std::cout << "done." << std::endl;
// Keep only the last three clicked points:
while(clicked_points_3d->points.size()>3)
{
clicked_points_3d->points.erase(clicked_points_3d->points.begin());
}
// Ground plane estimation:
std::vector<int> clicked_points_indices;
for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++)
clicked_points_indices.push_back(i);
// pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d);
pcl::SampleConsensusModelPlane<pcl::PointXYZRGB> model_plane(clicked_points_3d);
model_plane.computeModelCoefficients(clicked_points_indices,ground_coeffs);
std::cout << "Ground plane coefficients: " << ground_coeffs(0) << ", " << ground_coeffs(1) << ", " << ground_coeffs(2) <<
", " << ground_coeffs(3) << "." << std::endl;
}
// Semi-automatic mode:
if (ground_estimation_mode_ == 1)
{
std::cout << "Semi-automatic mode for ground plane estimation." << std::endl;
// Normals computation:
pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setMaxDepthChangeFactor (0.03f);
ne.setNormalSmoothingSize (20.0f);
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
ne.setInputCloud (cloud_);
ne.compute (*normal_cloud);
// Multi plane segmentation initialization:
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
mps.setMinInliers (500);
mps.setAngularThreshold (2.0 * M_PI / 180);
mps.setDistanceThreshold (0.2);
mps.setInputNormals (normal_cloud);
mps.setInputCloud (cloud_);
mps.segmentAndRefine (regions);
std::cout << "Found " << regions.size() << " planar regions." << std::endl;
// Color planar regions with different colors:
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
colored_cloud = colorRegions(regions);
if (regions.size()>0)
{
// Viewer initialization:
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(colored_cloud);
viewer.addPointCloud<pcl::PointXYZRGB> (colored_cloud, rgb, "input_cloud");
viewer.setCameraPosition(0,0,-2,0,-1,0,0);
// Add point picking callback to viewer:
struct callback_args_color cb_args;
typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr clicked_points_3d (new pcl::PointCloud<pcl::PointXYZRGB>);
cb_args.clicked_points_3d = clicked_points_3d;
cb_args.viewerPtr = &viewer;
viewer.registerPointPickingCallback (GroundplaneEstimation::pp_callback, (void*)&cb_args);
std::cout << "Shift+click on a floor point, then press 'Q'..." << std::endl;
// Spin until 'Q' is pressed:
viewer.spin();
viewer.setSize(1,1); // resize viewer in order to make it disappear
viewer.spinOnce();
viewer.close(); // close method does not work
std::cout << "done." << std::endl;
// Find plane closest to clicked point:
unsigned int index = 0;
float min_distance = FLT_MAX;
float distance;
float X = cb_args.clicked_points_3d->points[clicked_points_3d->points.size() - 1].x;
float Y = cb_args.clicked_points_3d->points[clicked_points_3d->points.size() - 1].y;
float Z = cb_args.clicked_points_3d->points[clicked_points_3d->points.size() - 1].z;
for(unsigned int i = 0; i < regions.size(); i++)
{
float a = regions[i].getCoefficients()[0];
float b = regions[i].getCoefficients()[1];
float c = regions[i].getCoefficients()[2];
float d = regions[i].getCoefficients()[3];
distance = (float) (fabs((a*X + b*Y + c*Z + d)))/(sqrtf(a*a+b*b+c*c));
if(distance < min_distance)
{
min_distance = distance;
index = i;
}
}
ground_coeffs[0] = regions[index].getCoefficients()[0];
ground_coeffs[1] = regions[index].getCoefficients()[1];
ground_coeffs[2] = regions[index].getCoefficients()[2];
ground_coeffs[3] = regions[index].getCoefficients()[3];
std::cout << "Ground plane coefficients: " << regions[index].getCoefficients()[0] << ", " << regions[index].getCoefficients()[1] << ", " <<
regions[index].getCoefficients()[2] << ", " << regions[index].getCoefficients()[3] << "." << std::endl;
}
}
// Automatic mode:
if ((ground_estimation_mode_ == 2) || (ground_estimation_mode_ == 3))
{
std::cout << "Automatic mode for ground plane estimation." << std::endl;
// Normals computation:
// pcl::NormalEstimation<PointT, pcl::Normal> ne;
//// ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
//// ne.setMaxDepthChangeFactor (0.03f);
//// ne.setNormalSmoothingSize (20.0f);
// pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
// pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
// ne.setSearchMethod (tree);
// ne.setRadiusSearch (0.2);
// ne.setInputCloud (cloud_);
// ne.compute (*normal_cloud);
pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setMaxDepthChangeFactor (0.03f);
ne.setNormalSmoothingSize (20.0f);
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
ne.setInputCloud (cloud_);
ne.compute (*normal_cloud);
// std::cout << "Normals estimated!" << std::endl;
//
// // Multi plane segmentation initialization:
// std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
// pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
// mps.setMinInliers (500);
// mps.setAngularThreshold (2.0 * M_PI / 180);
// mps.setDistanceThreshold (0.2);
// mps.setInputNormals (normal_cloud);
// mps.setInputCloud (cloud_);
// mps.segmentAndRefine (regions);
// Multi plane segmentation initialization:
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
mps.setMinInliers (500);
mps.setAngularThreshold (2.0 * M_PI / 180);
mps.setDistanceThreshold (0.2);
mps.setInputNormals (normal_cloud);
mps.setInputCloud (cloud_);
mps.segmentAndRefine (regions);
// std::cout << "Found " << regions.size() << " planar regions." << std::endl;
// Removing planes not compatible with camera roll ~= 0:
unsigned int i = 0;
while(i < regions.size())
{ // Check on the normal to the plane:
if(fabs(regions[i].getCoefficients()[1]) < 0.70)
{
regions.erase(regions.begin()+i);
}
else
++i;
}
// Order planar regions according to height (y coordinate):
std::sort(regions.begin(), regions.end(), GroundplaneEstimation::planeHeightComparator);
// Color selected planar region in red:
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
colored_cloud = colorRegions(regions, 0);
// If at least a valid plane remained:
if (regions.size()>0)
{
ground_coeffs[0] = regions[0].getCoefficients()[0];
ground_coeffs[1] = regions[0].getCoefficients()[1];
ground_coeffs[2] = regions[0].getCoefficients()[2];
ground_coeffs[3] = regions[0].getCoefficients()[3];
std::cout << "Ground plane coefficients: " << regions[0].getCoefficients()[0] << ", " << regions[0].getCoefficients()[1] << ", " <<
regions[0].getCoefficients()[2] << ", " << regions[0].getCoefficients()[3] << "." << std::endl;
// Result visualization:
if (ground_estimation_mode_ == 2)
{
// Viewer initialization:
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(colored_cloud);
viewer.addPointCloud<pcl::PointXYZRGB> (colored_cloud, rgb, "input_cloud");
viewer.setCameraPosition(0,0,-2,0,-1,0,0);
// Spin until 'Q' is pressed:
viewer.spin();
viewer.setSize(1,1); // resize viewer in order to make it disappear
viewer.spinOnce();
viewer.close(); // close method does not work
}
}
else
{
std::cout << "ERROR: no valid ground plane found!" << std::endl;
}
}
return ground_coeffs;
}
int
pcl::io::vtk2mesh (const vtkSmartPointer<vtkPolyData>& poly_data, pcl::PolygonMesh& mesh)
{
mesh.polygons.resize (0);
mesh.cloud.data.clear ();
mesh.cloud.width = mesh.cloud.height = 0;
mesh.cloud.is_dense = true;
vtkSmartPointer<vtkPoints> mesh_points = poly_data->GetPoints ();
vtkIdType nr_points = mesh_points->GetNumberOfPoints ();
vtkIdType nr_polygons = poly_data->GetNumberOfPolys ();
if (nr_points == 0)
return (0);
// First get the xyz information
pcl::PointCloud<pcl::PointXYZ>::Ptr xyz_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
xyz_cloud->points.resize (nr_points);
xyz_cloud->width = static_cast<uint32_t> (xyz_cloud->points.size ());
xyz_cloud->height = 1;
xyz_cloud->is_dense = true;
double point_xyz[3];
for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); i++)
{
mesh_points->GetPoint (i, &point_xyz[0]);
xyz_cloud->points[i].x = static_cast<float> (point_xyz[0]);
xyz_cloud->points[i].y = static_cast<float> (point_xyz[1]);
xyz_cloud->points[i].z = static_cast<float> (point_xyz[2]);
}
// And put it in the mesh cloud
pcl::toPCLPointCloud2 (*xyz_cloud, mesh.cloud);
// Then the color information, if any
vtkUnsignedCharArray* poly_colors = NULL;
if (poly_data->GetPointData() != NULL)
poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("Colors"));
// some applications do not save the name of scalars (including PCL's native vtk_io)
if (!poly_colors && poly_data->GetPointData () != NULL)
poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("scalars"));
if (!poly_colors && poly_data->GetPointData () != NULL)
poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("RGB"));
// TODO: currently only handles rgb values with 3 components
if (poly_colors && (poly_colors->GetNumberOfComponents () == 3))
{
pcl::PointCloud<pcl::RGB>::Ptr rgb_cloud (new pcl::PointCloud<pcl::RGB> ());
rgb_cloud->points.resize (nr_points);
rgb_cloud->width = static_cast<uint32_t> (rgb_cloud->points.size ());
rgb_cloud->height = 1;
rgb_cloud->is_dense = true;
unsigned char point_color[3];
for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); i++)
{
poly_colors->GetTupleValue (i, &point_color[0]);
rgb_cloud->points[i].r = point_color[0];
rgb_cloud->points[i].g = point_color[1];
rgb_cloud->points[i].b = point_color[2];
}
pcl::PCLPointCloud2 rgb_cloud2;
pcl::toPCLPointCloud2 (*rgb_cloud, rgb_cloud2);
pcl::PCLPointCloud2 aux;
pcl::concatenateFields (rgb_cloud2, mesh.cloud, aux);
mesh.cloud = aux;
}
// Then handle the normals, if any
vtkFloatArray* normals = NULL;
if (poly_data->GetPointData () != NULL)
normals = vtkFloatArray::SafeDownCast (poly_data->GetPointData ()->GetNormals ());
if (normals != NULL)
{
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal> ());
normal_cloud->resize (nr_points);
normal_cloud->width = static_cast<uint32_t> (xyz_cloud->points.size ());
normal_cloud->height = 1;
normal_cloud->is_dense = true;
for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); i++)
{
float normal[3];
normals->GetTupleValue (i, normal);
normal_cloud->points[i].normal_x = normal[0];
normal_cloud->points[i].normal_y = normal[1];
normal_cloud->points[i].normal_z = normal[2];
}
pcl::PCLPointCloud2 normal_cloud2;
pcl::toPCLPointCloud2 (*normal_cloud, normal_cloud2);
pcl::PCLPointCloud2 aux;
pcl::concatenateFields (normal_cloud2, mesh.cloud, aux);
mesh.cloud = aux;
}
// Now handle the polygons
mesh.polygons.resize (nr_polygons);
vtkIdType* cell_points;
vtkIdType nr_cell_points;
vtkCellArray * mesh_polygons = poly_data->GetPolys ();
mesh_polygons->InitTraversal ();
int id_poly = 0;
while (mesh_polygons->GetNextCell (nr_cell_points, cell_points))
{
mesh.polygons[id_poly].vertices.resize (nr_cell_points);
for (int i = 0; i < nr_cell_points; ++i)
mesh.polygons[id_poly].vertices[i] = static_cast<int> (cell_points[i]);
++id_poly;
}
return (static_cast<int> (nr_points));
}
int main(int argc,char**argv)
{
signal(SIGINT,sigint_handler);
srand(time(NULL));
int r_ = rand()%255;int g_ = rand()%255;int b_ = rand()%255;
int i=0;
kinect::Kinect2Grabber obj;
kinect::FrameData data;
//std::vector<int> compress;
//compress.push_back(CV_IMWRITE_JPEG_QUALITY );
//compress.push_back(100);
//compress.push_back(CV_IMWRITE_PNG_COMPRESSION);
//compress.push_back(2);
cv::namedWindow( "registered",CV_WINDOW_AUTOSIZE );
//cv::namedWindow( "Depth",CV_WINDOW_AUTOSIZE );
//cv::namedWindow( "Ir",CV_WINDOW_AUTOSIZE );
data = obj.getRegisteredImage();
//data = obj.getFrameData();
pcl::PointCloud<PointT>::Ptr cloud_(new pcl::PointCloud<PointT>());
pcl::PointCloud<PointT>::Ptr cloud_segment(new pcl::PointCloud<PointT>());
cloud_->width = data.depth_.cols;
cloud_->height = data.depth_.rows;
cloud_->points.resize(data.depth_.rows*data.depth_.cols);
pcl::visualization::PCLVisualizer* viewer(new pcl::visualization::PCLVisualizer ("Kinect Cloud"));
//pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud_);
viewer->addPointCloud<PointT>(cloud_,"Kinect Cloud");
//pcl::visualization::PointCloudColorHandlerCustom <PointT> color_handle(cloud_segment,r_,g_,b_);
//viewer->addPointCloud<PointT>(cloud_segment,"Kinect Cloud");
viewer->setBackgroundColor (0, 0, 0);
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3, "Kinect Cloud");
//viewer->addCoordinateSystem (1.0);
viewer->initCameraParameters ();
/*
//segment
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<PointT> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (cloud_);
*/
while(key!=99)
{
data = obj.getRegisteredImage();
//data = obj.getFrameData();
cv::imshow("registered",data.color_);
//cv::imshow("Depth",data.depth_);
//cv::imshow("Ir",data.IR_);
key = cv::waitKey(10);
libfreenect2::Freenect2Device::IrCameraParams params = obj.getIrParams();
std::cout<<"Camera centre : "<<params.cx<<" "<<params.cy<<std::endl;
std::cout<<"Focal parameters : "<<params.fx<<" "<<params.fy<<std::endl;
int size_r = data.color_.rows;
int size_c = data.color_.cols;
int index = 0;
#pragma omp parallel for
for(int i=0;i<size_r;i++)
{
for(int j=0;j<size_c;j++,index++)
{
float depth_pos = data.depth_.at<float>(i,j);
float pos_x = (float)((j-params.cx)*depth_pos)/params.fx;
float pos_y = (float)((i-params.cy)*depth_pos)/params.fy;
cloud_->points[index].x = pos_x;
cloud_->points[index].y = pos_y;
cloud_->points[index].z = depth_pos;
const cv::Vec3b tmp = data.color_.at<cv::Vec3b>(i,j);
cloud_->points[index].b = tmp.val[0];
cloud_->points[index].g = tmp.val[1];
cloud_->points[index].r = tmp.val[2];
}
}
/*
if(segment){
seg.segment (*inliers, *coefficients);
for(unsigned int i=0;i<inliers->indices.size();i++)
{
cloud_->points[inliers->indices[i]].r = 255;
cloud_->points[inliers->indices[i]].g = 0;
cloud_->points[inliers->indices[i]].b = 0;
}
}
*/
///////////////////////////////////////////////
//Multi Plane Segmentation
///////////////////////////////////////////////
pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
ne.setInputCloud (cloud_);
ne.compute (*normal_cloud);
float* distance_map = ne.getDistanceMap ();
boost::shared_ptr<pcl::EdgeAwarePlaneComparator<PointT,pcl::Normal> > eapc(new pcl::EdgeAwarePlaneComparator<PointT, pcl::Normal> ());
eapc->setDistanceMap (distance_map);
eapc->setDistanceThreshold (0.01f, false);
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
std::vector<pcl::ModelCoefficients> model_coefficients;
std::vector<pcl::PointIndices> inlier_indices;
pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
std::vector<pcl::PointIndices> label_indices;
std::vector<pcl::PointIndices> boundary_indices;
mps.setInputNormals (normal_cloud);
mps.setInputCloud (cloud_);
mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
//////////////////view contours of segmented planes////////////////////////////
if(contours_only)
{
std::vector<PointT,Eigen::aligned_allocator<PointT> > c_points;
int index_s=0;
unsigned int size_region = regions.size();
long int cloud_size;
for(unsigned int i=0;i<size_region;i++)
{
cloud_size+=regions[i].getContour().size();
}
cloud_segment->points.resize(cloud_size);
for(unsigned int i=0;i<regions.size();i++)
{
c_points = regions[i].getContour();
r_ = rand()%255;g_ = rand()%255;b_ = rand()%255;
for(unsigned int j=0;j<c_points.size();j++,index_s++)
{
cloud_segment->points[index_s].x = c_points[j].x;
cloud_segment->points[index_s].y = c_points[j].y;
cloud_segment->points[index_s].z = c_points[j].z;
cloud_segment->points[index_s].r = r_;
cloud_segment->points[index_s].g = g_;
cloud_segment->points[index_s].b = b_;
}
}
}
//////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//Using boundary indices
///////////////////////////////////////////////////////////////////////////////
else{
/*
for(unsigned int i=0;i<boundary_indices.size();i++)
{
r_ = rand()%255;g_ = rand()%255;b_ = rand()%255;
for(unsigned int j=0;j<boundary_indices[i].indices.size();j++)
{
cloud_->points[boundary_indices[i].indices[j]].r = r_;
cloud_->points[boundary_indices[i].indices[j]].g = g_;
cloud_->points[boundary_indices[i].indices[j]].b = b_;
}
}
*/
for(unsigned int i=0;i<label_indices.size();i++)
{
r_ = rand()%255;g_ = rand()%255;b_ = rand()%255;
for(unsigned int j=0;j<label_indices[i].indices.size();j++)
{
cloud_->points[label_indices[i].indices[j]].r = r_;
cloud_->points[label_indices[i].indices[j]].g = g_;
cloud_->points[label_indices[i].indices[j]].b = b_;
}
}
}
//viewer->updatePointCloud(cloud_segment,"Kinect Cloud");
viewer->updatePointCloud(cloud_,"Kinect Cloud");
viewer->spinOnce (100);
if(print&&(key==99)){
cloud_->height = 1;
cloud_->width = cloud_->points.size();
pcl::io::savePCDFileASCII ("test_new.pcd",*cloud_);
print = false;
}
}
return 0;
}
void
run ()
{
pcl::Grabber* interface = new pcl::OpenNIGrabber ();
boost::function<void(const pcl::PointCloud<PointT>::ConstPtr&)> f = boost::bind (&OpenNIOrganizedMultiPlaneSegmentation::cloud_cb_, this, _1);
//make a viewer
pcl::PointCloud<PointT>::Ptr init_cloud_ptr (new pcl::PointCloud<PointT>);
viewer = cloudViewer (init_cloud_ptr);
boost::signals2::connection c = interface->registerCallback (f);
interface->start ();
unsigned char red [6] = {255, 0, 0, 255, 255, 0};
unsigned char grn [6] = { 0, 255, 0, 255, 0, 255};
unsigned char blu [6] = { 0, 0, 255, 0, 255, 255};
pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setMaxDepthChangeFactor (0.03f);
ne.setNormalSmoothingSize (20.0f);
pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
mps.setMinInliers (10000);
mps.setAngularThreshold (0.017453 * 2.0); //3 degrees
mps.setDistanceThreshold (0.02); //2cm
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
pcl::PointCloud<PointT>::Ptr contour (new pcl::PointCloud<PointT>);
size_t prev_models_size = 0;
char name[1024];
while (!viewer->wasStopped ())
{
viewer->spinOnce (100);
if (prev_cloud && cloud_mutex.try_lock ())
{
regions.clear ();
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
double normal_start = pcl::getTime ();
ne.setInputCloud (prev_cloud);
ne.compute (*normal_cloud);
double normal_end = pcl::getTime ();
std::cout << "Normal Estimation took " << double (normal_end - normal_start) << std::endl;
double plane_extract_start = pcl::getTime ();
mps.setInputNormals (normal_cloud);
mps.setInputCloud (prev_cloud);
mps.segmentAndRefine (regions);
double plane_extract_end = pcl::getTime ();
std::cout << "Plane extraction took " << double (plane_extract_end - plane_extract_start) << std::endl;
std::cout << "Frame took " << double (plane_extract_end - normal_start) << std::endl;
pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>);
if (!viewer->updatePointCloud<PointT> (prev_cloud, "cloud"))
viewer->addPointCloud<PointT> (prev_cloud, "cloud");
removePreviousDataFromScreen (prev_models_size);
//Draw Visualization
for (size_t i = 0; i < regions.size (); i++)
{
Eigen::Vector3f centroid = regions[i].getCentroid ();
Eigen::Vector4f model = regions[i].getCoefficients ();
pcl::PointXYZ pt1 = pcl::PointXYZ (centroid[0], centroid[1], centroid[2]);
pcl::PointXYZ pt2 = pcl::PointXYZ (centroid[0] + (0.5f * model[0]),
centroid[1] + (0.5f * model[1]),
centroid[2] + (0.5f * model[2]));
sprintf (name, "normal_%lu", i);
viewer->addArrow (pt2, pt1, 1.0, 0, 0, false, name);
contour->points = regions[i].getContour ();
sprintf (name, "plane_%02zu", i);
pcl::visualization::PointCloudColorHandlerCustom <PointT> color (contour, red[i], grn[i], blu[i]);
viewer->addPointCloud (contour, color, name);
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name);
}
prev_models_size = regions.size ();
cloud_mutex.unlock ();
}
}
interface->stop ();
}
void
OrganizedSegmentationDemo::cloud_cb (const CloudConstPtr& cloud)
{
if (!capture_)
return;
QMutexLocker locker (&mtx_);
FPS_CALC ("computation");
// Estimate Normals
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
ne.setInputCloud (cloud);
ne.compute (*normal_cloud);
double* distance_map = ne.getDistanceMap ();
boost::shared_ptr<pcl::EdgeAwarePlaneComparator<PointT,pcl::Normal> > eapc = boost::dynamic_pointer_cast<pcl::EdgeAwarePlaneComparator<PointT,pcl::Normal> >(edge_aware_comparator_);
eapc->setDistanceMap (distance_map);
eapc->setDistanceThreshold (0.01, false);
// Segment Planes
double mps_start = pcl::getTime ();
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
std::vector<pcl::ModelCoefficients> model_coefficients;
std::vector<pcl::PointIndices> inlier_indices;
pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
std::vector<pcl::PointIndices> label_indices;
std::vector<pcl::PointIndices> boundary_indices;
mps.setInputNormals (normal_cloud);
mps.setInputCloud (cloud);
if (use_planar_refinement_)
{
mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
}
else
{
mps.segment (regions);//, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
}
double mps_end = pcl::getTime ();
std::cout << "MPS+Refine took: " << double(mps_end - mps_start) << std::endl;
//Segment Objects
pcl::PointCloud<PointT>::CloudVectorType clusters;
if (use_clustering_ && regions.size () > 0)
{
std::vector<bool> plane_labels;
plane_labels.resize (label_indices.size (), false);
for (size_t i = 0; i < label_indices.size (); i++)
{
if (label_indices[i].indices.size () > 10000)
{
plane_labels[i] = true;
}
}
euclidean_cluster_comparator_->setInputCloud (cloud);
euclidean_cluster_comparator_->setLabels (labels);
euclidean_cluster_comparator_->setExcludeLabels (plane_labels);
euclidean_cluster_comparator_->setDistanceThreshold (0.01, false);
pcl::PointCloud<pcl::Label> euclidean_labels;
std::vector<pcl::PointIndices> euclidean_label_indices;
pcl::OrganizedConnectedComponentSegmentation<PointT,pcl::Label> euclidean_segmentation (euclidean_cluster_comparator_);
euclidean_segmentation.setInputCloud (cloud);
euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
for (size_t i = 0; i < euclidean_label_indices.size (); i++)
{
if (euclidean_label_indices[i].indices.size () > 1000)
{
pcl::PointCloud<PointT> cluster;
pcl::copyPointCloud (*cloud,euclidean_label_indices[i].indices,cluster);
clusters.push_back (cluster);
}
}
PCL_INFO ("Got %d euclidean clusters!\n", clusters.size ());
}
{
QMutexLocker vis_locker (&vis_mtx_);
prev_cloud_ = *cloud;
prev_normals_ = *normal_cloud;
prev_regions_ = regions;
prev_distance_map_ = distance_map;
prev_clusters_ = clusters;
data_modified_ = true;
}
}
bool
segmentCloud (pointing_object_evaluation::ObjectSegmentationRequest::Request &req, pointing_object_evaluation::ObjectSegmentationRequest::Response &res)
{
CloudPtr cloud (new Cloud ());
{
//boost::lock_guard<boost::mutex> lock (cloud_mutex_);
*cloud = *cloud_;
}
unsigned char red [6] = {255, 0, 0, 255, 255, 0};
unsigned char grn [6] = { 0, 255, 0, 255, 0, 255};
unsigned char blu [6] = { 0, 0, 255, 0, 255, 255};
pcl::PointCloud<pcl::PointXYZRGB> aggregate_cloud;
// Estimate Normals
double ne_start = pcl::getTime ();
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
ne_.setInputCloud (cloud);
ne_.compute (*normal_cloud);
double ne_end = pcl::getTime ();
//std::cout << "NE took: " << double(ne_end - ne_start) << std::endl;
// Segment Planes
double mps_start = pcl::getTime ();
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
std::vector<pcl::ModelCoefficients> model_coefficients;
std::vector<pcl::PointIndices> inlier_indices;
pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
std::vector<pcl::PointIndices> label_indices;
std::vector<pcl::PointIndices> boundary_indices;
mps_.setInputNormals (normal_cloud);
mps_.setInputCloud (cloud);
//mps.segment (regions);
mps_.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
//Segment Objects
typename pcl::PointCloud<PointT>::CloudVectorType clusters;
if (regions.size () > 0)
{
//printf ("got some planes!\n");
std::vector<bool> plane_labels;
plane_labels.resize (label_indices.size (), false);
for (size_t i = 0; i < label_indices.size (); i++)
{
if (label_indices[i].indices.size () > 10000)
{
plane_labels[i] = true;
}
}
//printf ("made label vec\n");
typename pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>::Ptr euclidean_cluster_comparator_ (new typename pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>());
euclidean_cluster_comparator_->setInputCloud (cloud);
euclidean_cluster_comparator_->setLabels (labels);
euclidean_cluster_comparator_->setExcludeLabels (plane_labels);
euclidean_cluster_comparator_->setDistanceThreshold (0.01f, false);
pcl::PointCloud<pcl::Label> euclidean_labels;
std::vector<pcl::PointIndices> euclidean_label_indices;
pcl::OrganizedConnectedComponentSegmentation<PointT,pcl::Label> euclidean_segmentation (euclidean_cluster_comparator_);
euclidean_segmentation.setInputCloud (cloud);
euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
//printf ("done segmenting the clusters\n");
std::vector<Eigen::Vector4f> centroids;
for (size_t i = 0; i < euclidean_label_indices.size (); i++)
{
if (euclidean_label_indices[i].indices.size () > 1000)
{
pcl::PointCloud<PointT> cluster;
pcl::copyPointCloud (*cloud,euclidean_label_indices[i].indices,cluster);
clusters.push_back (cluster);
/*Eigen::Vector4f centroid;
pcl::compute3DCentroid (cluster, centroid);
centroids.push_back (centroid);*/
//pcl::PointXYZ centroid_pt (centroid[0], centroid[1], centroid[2]);
//double dist = sqrt (centroid[0]*centroid[0] + centroid[1]*centroid[1] + centroid[2]*centroid[2]);
//object_points_.push_back (centroid_pt);
// Send to RViz
pcl::PointCloud<pcl::PointXYZRGB> color_cluster;
pcl::copyPointCloud (cluster, color_cluster);
for (size_t j = 0; j < color_cluster.size (); j++)
{
color_cluster.points[j].r = (color_cluster.points[j].r + red[i%6]) / 2;
color_cluster.points[j].g = (color_cluster.points[j].g + grn[i%6]) / 2;
color_cluster.points[j].b = (color_cluster.points[j].b + blu[i%6]) / 2;
}
aggregate_cloud += color_cluster;
}
}
PCL_INFO ("Got %d euclidean clusters!\n", clusters.size ());
//PCL_INFO ("Got %d centroids!\n", centroids.size ());
aggregate_cloud.header = cloud->header;
sensor_msgs::PointCloud2 cloud_msg;
pcl::toROSMsg (aggregate_cloud, cloud_msg);
object_cloud_pub_.publish (cloud_msg);
// TODO: mutex
/*clusters_ = clusters;
centroids_= centroids;*/
}
}