void
pp_callback (const pcl::visualization::PointPickingEvent& event, void* cookie)
{
int idx = event.getPointIndex ();
if (idx == -1)
return;
if (!cloud)
{
cloud = *reinterpret_cast<pcl::PCLPointCloud2::Ptr*> (cookie);
xyzcloud.reset (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2 (*cloud, *xyzcloud);
search.setInputCloud (xyzcloud);
}
// Return the correct index in the cloud instead of the index on the screen
std::vector<int> indices (1);
std::vector<float> distances (1);
// Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we must search for the real point
pcl::PointXYZ picked_pt;
event.getPoint (picked_pt.x, picked_pt.y, picked_pt.z);
//TODO: Look into this.
search.nearestKSearch (picked_pt, 1, indices, distances);
PCL_INFO ("Point index picked: %d (real: %d) - [%f, %f, %f]\n", idx, indices[0], picked_pt.x, picked_pt.y, picked_pt.z);
idx = indices[0];
// If two points were selected, draw an arrow between them
pcl::PointXYZ p1, p2;
if (event.getPoints (p1.x, p1.y, p1.z, p2.x, p2.y, p2.z) && p)
{
std::stringstream ss;
ss << p1 << p2;
p->addArrow<pcl::PointXYZ, pcl::PointXYZ> (p1, p2, 1.0, 1.0, 1.0, ss.str ());
return;
}
// Else, if a single point has been selected
std::stringstream ss;
ss << idx;
// Get the cloud's fields
for (size_t i = 0; i < cloud->fields.size (); ++i)
{
if (!isMultiDimensionalFeatureField (cloud->fields[i]))
continue;
PCL_INFO ("Multidimensional field found: %s\n", cloud->fields[i].name.c_str ());
#if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
ph_global.addFeatureHistogram (*cloud, cloud->fields[i].name, idx, ss.str ());
ph_global.renderOnce ();
#endif
}
if (p)
{
pcl::PointXYZ pos;
event.getPoint (pos.x, pos.y, pos.z);
p->addText3D<pcl::PointXYZ> (ss.str (), pos, 0.0005, 1.0, 1.0, 1.0, ss.str ());
}
}
void FindPickedPoint(const pcl::visualization::PointPickingEvent& event) {
int idx = event.getPointIndex ();
if (idx == -1)
{
std::cout << "Invalid pick!\n;";
return;
}
search.setInputCloud(build_cloud_accurate_plane);
// Return the correct index in the cloud instead of the index on the screen
std::vector<int> indices (1);
std::vector<float> distances (1);
// Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we must search for the real point
pcl::PointXYZ picked_pt;
event.getPoint (picked_pt.x, picked_pt.y, picked_pt.z);
search.nearestKSearch (picked_pt, 1, indices, distances);
picked_points.push_back(picked_pt);
}
int ICP::getNormal(const CloudPtr &cloud_in, NormalCloudPtr &cloud_out, pcl::search::KdTree<Point>::Ptr &tree)
{
tree->setInputCloud(cloud_in);
pcl::NormalEstimation<Point, NormalPoint> norm_est;
norm_est.setSearchMethod(tree);
norm_est.setKSearch(_GetNormalKSearch);
norm_est.setInputCloud(cloud_in);
norm_est.compute(*cloud_out);
Utils::combineField(cloud_in, cloud_out);
return 0;
}
void depthPointsCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {
// Instantiate various clouds
pcl::PCLPointCloud2* cloud_intermediate = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud_intermediate);
pcl::PointCloud<pcl::PointXYZ> cloud;
// Convert to PCL data type
pcl_conversions::toPCL(*cloud_msg, *cloud_intermediate);
// Apply Voxel Filter on PCLPointCloud2
vox.setInputCloud (cloudPtr);
vox.setInputCloud (cloudPtr);
vox.filter (*cloud_intermediate);
// Convert PCL::PointCloud2 to PCL::PointCloud<PointXYZ>
pcl::fromPCLPointCloud2(*cloud_intermediate, cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p = cloud.makeShared();
// Apply Passthrough Filter
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.3, 1);
pass.setInputCloud (cloud_p);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_p);
// Apply Passthrough Filter
pass.setFilterFieldName ("x");
pass.setFilterLimits (-0.2, 0.2);
pass.setInputCloud (cloud_p);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.0, 3.0);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_p);
// Apply Statistical Noise Filter
sor.setInputCloud (cloud_p);
sor.filter (*cloud_p);
// Planar segmentation: Remove large planes? Or extract floor?
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
int nr_points = (int) cloud_p->points.size ();
Eigen::Vector3f lol (0, 1, 0);
seg.setEpsAngle( 30.0f * (3.14f/180.0f) );
seg.setAxis(lol);
//while(cloud_p->points.size () > 0.2 * nr_points) {
sor.setInputCloud (cloud_p);
sor.filter (*cloud_p);
// 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.01);
seg.setInputCloud (cloud_p);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
//break;
}
else {
/*std::cout << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << "\n";*/
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_p);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(*cloud_p);
}
//}
Eigen::Vector3f lol_p (0.5f, 0, 0.5f);
seg.setAxis(lol_p);
while(cloud_p->points.size () > 0.1 * nr_points) {
seg.setInputCloud (cloud_p);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
break;
}
else {
/*std::cout << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << "\n";*/
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_p);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(*cloud_p);
}
}
// Apply Statistical Noise Filter
sor.setInputCloud (cloud_p);
sor.filter (*cloud_p);
if(cloud_p->points.size() > 0) {
std::vector<pcl::PointIndices> cluster_indices;
tree->setInputCloud (cloud_p);
ec.setInputCloud (cloud_p);
ec.extract (cluster_indices);
std::cout << "Clusters detected: " << cluster_indices.size() << "\n";
// Convert to ROS data type
}
// Convert to ROS data type
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(*cloud_p, output);
// Publish the data
downsample_pub.publish(output);
}
/* ---[ */
int
main (int argc, char** argv)
{
if (argc < 2)
{
std::cout << "need path to table_scene_mug_stereo_textured.pcd file\n";
return (-1);
}
pcl::io::loadPCDFile (argv [1], *organized_sparse_cloud);
// create unorganized cloud
unorganized_dense_cloud->resize (unorganized_point_count);
unorganized_dense_cloud->height = 1;
unorganized_dense_cloud->width = unorganized_point_count;
unorganized_dense_cloud->is_dense = true;
unorganized_sparse_cloud->resize (unorganized_point_count);
unorganized_sparse_cloud->height = 1;
unorganized_sparse_cloud->width = unorganized_point_count;
unorganized_sparse_cloud->is_dense = false;
PointXYZ point;
for (unsigned pIdx = 0; pIdx < unorganized_point_count; ++pIdx)
{
point.x = rand_float ();
point.y = rand_float ();
point.z = rand_float ();
unorganized_dense_cloud->points [pIdx] = point;
if (rand_uint () == 0)
unorganized_sparse_cloud->points [pIdx].x = unorganized_sparse_cloud->points [pIdx].y = unorganized_sparse_cloud->points [pIdx].z = std::numeric_limits<float>::quiet_NaN ();
else
unorganized_sparse_cloud->points [pIdx] = point;
}
unorganized_grid_cloud->reserve (1000);
unorganized_grid_cloud->height = 1;
unorganized_grid_cloud->width = 1000;
unorganized_grid_cloud->is_dense = true;
// values between 0 and 1
for (unsigned xIdx = 0; xIdx < 10; ++xIdx)
{
for (unsigned yIdx = 0; yIdx < 10; ++yIdx)
{
for (unsigned zIdx = 0; zIdx < 10; ++zIdx)
{
point.x = 0.1f * static_cast<float>(xIdx);
point.y = 0.1f * static_cast<float>(yIdx);
point.z = 0.1f * static_cast<float>(zIdx);
unorganized_grid_cloud->push_back (point);
}
}
}
createIndices (organized_input_indices, static_cast<unsigned> (organized_sparse_cloud->size () - 1));
createIndices (unorganized_input_indices, unorganized_point_count - 1);
brute_force.setSortedResults (true);
KDTree.setSortedResults (true);
octree_search.setSortedResults (true);
organized.setSortedResults (true);
unorganized_search_methods.push_back (&brute_force);
unorganized_search_methods.push_back (&KDTree);
unorganized_search_methods.push_back (&octree_search);
organized_search_methods.push_back (&brute_force);
organized_search_methods.push_back (&KDTree);
organized_search_methods.push_back (&octree_search);
organized_search_methods.push_back (&organized);
createQueryIndices (unorganized_dense_cloud_query_indices, unorganized_dense_cloud, query_count);
createQueryIndices (unorganized_sparse_cloud_query_indices, unorganized_sparse_cloud, query_count);
createQueryIndices (organized_sparse_query_indices, organized_sparse_cloud, query_count);
testing::InitGoogleTest (&argc, argv);
return (RUN_ALL_TESTS ());
}
void extractEuclideanClusters (
PointCloud<PointXYZRGB >::Ptr cloud, pcl::PointCloud<pcl::Normal >::Ptr normals,
pcl::search::KdTree<PointXYZRGB >::Ptr tree,
float tolerance, std::vector<pcl::PointIndices > &clusters, double eps_angle,
unsigned int min_pts_per_cluster = 1,
unsigned int max_pts_per_cluster = (std::numeric_limits<int >::max) ())
{
// \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns
//and indices[i]
float adjTolerance = 0;
// Create a bool vector of processed point indices, and initialize it to false
std::vector<bool > processed(cloud->points.size(), false);
std::vector<int> nn_indices;
std::vector<float> nn_distances;
// Process all points in the indices vector
std::cout << "Point size is " << cloud->points.size () << std::endl;
for (size_t i = 0; i < cloud->points.size (); ++i)
{
if(processed[i])
continue;
std::vector<int > seed_queue;
int sq_idx = 0;
seed_queue.push_back(i);
processed[i] = true;
int cnt = 0;
while (sq_idx < (int)seed_queue.size())
{
cnt++;
// Search for sq_idx
// adjTolerance = cloud->points[seed_queue[sq_idx]].distance * tolerance;
adjTolerance = tolerance;
if (!tree->radiusSearch(seed_queue[sq_idx], adjTolerance, nn_indices, nn_distances))
{
sq_idx++;
continue;
}
for(size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
{
if (processed[nn_indices[j]]) // Has this point been processed before ?
continue;
processed[nn_indices[j]] = true;
// [-1;1]
double dot_p =
normals->points[i].normal[0] * normals->points[nn_indices[j]].normal[0] +
normals->points[i].normal[1] * normals->points[nn_indices[j]].normal[1] +
normals->points[i].normal[2] * normals->points[nn_indices[j]].normal[2];
if ( fabs (acos (dot_p)) < eps_angle )
{
processed[nn_indices[j]] = true;
seed_queue.push_back (nn_indices[j]);
}
}
sq_idx++;
}
// If this queue is satisfactory, add to the clusters
if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
{
pcl::PointIndices r;
r.indices.resize (seed_queue.size ());
for (size_t j = 0; j < seed_queue.size (); ++j)
r.indices[j] = seed_queue[j];
sort (r.indices.begin (), r.indices.end ());
r.indices.erase (unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
r.header = cloud->header;
//ROS_INFO ("cluster of size %d data point\n ",r.indices.size());
clusters.push_back(r);
}
}
}
