template <typename PointT> pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
pcl::SupervoxelClustering<PointT>::getColoredCloud () const
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr colored_cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::copyPointCloud (*input_,*colored_cloud);
pcl::PointCloud <pcl::PointXYZRGBA>::iterator i_colored;
typename pcl::PointCloud <PointT>::const_iterator i_input = input_->begin ();
std::vector <int> indices;
std::vector <float> sqr_distances;
for (i_colored = colored_cloud->begin (); i_colored != colored_cloud->end (); ++i_colored,++i_input)
{
if ( !pcl::isFinite<PointT> (*i_input))
i_colored->rgb = 0;
else
{
i_colored->rgb = 0;
LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input);
VoxelData& voxel_data = leaf->getData ();
if (voxel_data.owner_)
i_colored->rgba = label_colors_[voxel_data.owner_->getLabel ()];
}
}
return (colored_cloud);
}
template <typename PointT, typename NormalT> pcl::PointCloud<pcl::PointXYZRGB>::Ptr
pcl::RegionGrowingHSV<PointT, NormalT>::getShadowCloud ()
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
//colored_cloud = (new pcl::PointCloud<pcl::PointXYZRGB>)->makeShared ();
//colored_cloud->width = input_->width;
//colored_cloud->height = input_->height;
//colored_cloud->is_dense = input_->is_dense;
std::vector< pcl::PointIndices >::iterator i_segment;
int i=0;
for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++)
{
std::vector<int>::iterator i_point;
for (i_point = i_segment->indices.begin (); i_point != i_segment->indices.end (); i_point++)
{
int index;
index = *i_point;
//colored_cloud->points[index].r = segment_shadow_labels_.at(i) == 1 ? 255:0;
//colored_cloud->points[index].g = segment_shadow_labels_.at(i) == 1 ? 0:255;
//colored_cloud->points[index].b = 0;
pcl::PointXYZRGB point;
point.x = *(input_->points[index].data);
point.y = *(input_->points[index].data + 1);
point.z = *(input_->points[index].data + 2);
point.r = 100;
point.g = 100;
point.b = 100;
if (segment_shadow_labels_.at(i)==0)//shadow
{
point.r = 255;
}
if (segment_shadow_labels_.at(i)==1)//not shadow
{
point.g = 255;
}
if (segment_shadow_labels_.at(i)==2)//object
{
point.b = 255;
}
colored_cloud->points.push_back (point);
}
i++;
}
colored_cloud->height = colored_cloud->points.size();
colored_cloud->width = 1;
return (colored_cloud);
}
template <typename PointT> pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
pcl::SupervoxelClustering<PointT>::getColoredVoxelCloud () const
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr colored_cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
{
typename PointCloudT::Ptr voxels;
sv_itr->getVoxels (voxels);
pcl::PointCloud<pcl::PointXYZRGBA> rgb_copy;
copyPointCloud (*voxels, rgb_copy);
pcl::PointCloud<pcl::PointXYZRGBA>::iterator rgb_copy_itr = rgb_copy.begin ();
for ( ; rgb_copy_itr != rgb_copy.end (); ++rgb_copy_itr)
rgb_copy_itr->rgba = label_colors_ [sv_itr->getLabel ()];
*colored_cloud += rgb_copy;
}
return colored_cloud;
}
template <typename PointT> pcl::PointCloud<pcl::PointXYZRGB>::Ptr
open_ptrack::detection::GroundplaneEstimation<PointT>::colorRegions (std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions, int index)
{
// Color different planes with different colors:
float voxel_size = 0.06;
// Initialize colored point cloud with points from input point cloud:
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointXYZRGB white_point;
white_point.r = 255;
white_point.g = 255;
white_point.b = 255;
colored_cloud->points.resize(cloud_->width * cloud_->height, white_point);
colored_cloud->width = cloud_->width;
colored_cloud->height = cloud_->height;
colored_cloud->is_dense = false;
for (int i=0; i<cloud_->height; i++)
{
for (int j=0; j<cloud_->width; j++)
{
colored_cloud->at(j,i).x = cloud_->at(j,i).x;
colored_cloud->at(j,i).y = cloud_->at(j,i).y;
colored_cloud->at(j,i).z = cloud_->at(j,i).z;
}
}
for (size_t i = 0; i < regions.size (); i++)
{
Eigen::Vector3f centroid = regions[i].getCentroid ();
Eigen::Vector4f model = regions[i].getCoefficients ();
pcl::IndicesPtr inliers(new std::vector<int>);
boost::shared_ptr<pcl::SampleConsensusModelPlane<PointT> > ground_model(new pcl::SampleConsensusModelPlane<PointT>(cloud_));
ground_model->selectWithinDistance(model, voxel_size, *inliers);
int r = (rand() % 256);
int g = (rand() % 256);
int b = (rand() % 256);
for(unsigned int j = 0; j < inliers->size(); j++)
{
colored_cloud->points.at(inliers->at(j)).r = r;
colored_cloud->points.at(inliers->at(j)).g = g;
colored_cloud->points.at(inliers->at(j)).b = b;
}
}
// If index is passed, color index-th region in red:
if (index >= 0 && !regions.empty())
{
Eigen::Vector3f centroid = regions[index].getCentroid ();
Eigen::Vector4f model = regions[index].getCoefficients ();
pcl::IndicesPtr inliers(new std::vector<int>);
boost::shared_ptr<pcl::SampleConsensusModelPlane<PointT> > ground_model(new pcl::SampleConsensusModelPlane<PointT>(cloud_));
ground_model->selectWithinDistance(model, voxel_size, *inliers);
for(unsigned int j = 0; j < inliers->size(); j++)
{
colored_cloud->points.at(inliers->at(j)).r = 255;
colored_cloud->points.at(inliers->at(j)).g = 0;
colored_cloud->points.at(inliers->at(j)).b = 0;
}
}
return colored_cloud;
}
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;
}
