void buildOctoMap(const pcl::PointCloud<PointT> &cloud, OcTreeROS & tree)
{
pcl::PointCloud<PointT>::Ptr cloud_ptr(new pcl::PointCloud<PointT > (cloud));
pcl::PointCloud<PointT>::Ptr cloud_cam(new pcl::PointCloud<PointT > ());
int cnt =0;
// find all the camera indices
map<int,int> camera_indices;
for (size_t i = 0; i < cloud.points.size(); ++i) {
camera_indices[(int) cloud.points[i].cameraIndex] = 1;
}
// for every camera index .. apply filter and get the point cloud
for (map<int,int>::iterator it = camera_indices.begin(); it != camera_indices.end();it++)
{
int ci = (*it).first;
apply_camera_filter(*cloud_ptr,*cloud_cam,ci);
// convert to pointXYZ format
sensor_msgs::PointCloud2 cloud_blob;
pcl::toROSMsg(*cloud_cam,cloud_blob);
pcl::PointCloud<pcl::PointXYZ> xyzcloud;
pcl::fromROSMsg(cloud_blob, xyzcloud);
// find the camera co-ordinate
VectorG cam_coordinates = originalFrames[ci]->getCameraTrans().getOrigin();
pcl::PointXYZ origin (cam_coordinates.v[0], cam_coordinates.v[1], cam_coordinates.v[2]);
// insert to the tree
tree.insertScan(xyzcloud,origin,-1,true);
}
}
int
main (int, char **argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ());
pcl::PCDWriter writer;
if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], *cloud_ptr) == -1)
{
std::cout<<"Couldn't read the file "<<argv[1]<<std::endl;
return (-1);
}
std::cout << "Loaded pcd file " << argv[1] << " with " << cloud_ptr->points.size () << std::endl;
// Normal estimation
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud_ptr);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n (new pcl::search::KdTree<pcl::PointXYZ>());
ne.setSearchMethod (tree_n);
ne.setRadiusSearch (0.03);
ne.compute (*cloud_normals);
std::cout << "Estimated the normals" << std::endl;
// Creating the kdtree object for the search method of the extraction
boost::shared_ptr<pcl::KdTree<pcl::PointXYZ> > tree_ec (new pcl::KdTreeFLANN<pcl::PointXYZ> ());
tree_ec->setInputCloud (cloud_ptr);
// Extracting Euclidean clusters using cloud and its normals
std::vector<int> indices;
std::vector<pcl::PointIndices> cluster_indices;
const float tolerance = 0.5f; // 50cm tolerance in (x, y, z) coordinate system
const double eps_angle = 5 * (M_PI / 180.0); // 5degree tolerance in normals
const unsigned int min_cluster_size = 50;
pcl::extractEuclideanClusters (*cloud_ptr, *cloud_normals, tolerance, tree_ec, cluster_indices, eps_angle, min_cluster_size);
std::cout << "No of clusters formed are " << cluster_indices.size () << std::endl;
// Saving the clusters in seperate pcd files
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud_ptr->points[*pit]);
cloud_cluster->width = static_cast<uint32_t> (cloud_cluster->points.size ());
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster using xyzn: " << cloud_cluster->points.size () << " data points." << std::endl;
std::stringstream ss;
ss << "./cloud_cluster_" << j << ".pcd";
writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false);
j++;
}
return (0);
}
int
main (int, char** av)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_no_nans (new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_segmented (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PCDWriter writer;
if (pcl::io::loadPCDFile(av[1], *cloud_ptr)==-1)
{
std::cout << "Couldn't find the file " << av[1] << std::endl;
return -1;
}
std::cout << "Loaded cloud " << av[1] << " of size " << cloud_ptr->points.size() << std::endl;
// Remove the nans
cloud_ptr->is_dense = false;
cloud_no_nans->is_dense = false;
std::vector<int> indices;
pcl::removeNaNFromPointCloud (*cloud_ptr, *cloud_no_nans, indices);
std::cout << "Removed nans from " << cloud_ptr->points.size () << " to " << cloud_no_nans->points.size () << std::endl;
// Estimate the normals
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud_no_nans);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n (new pcl::search::KdTree<pcl::PointXYZ>());
ne.setSearchMethod (tree_n);
ne.setRadiusSearch (0.03);
ne.compute (*cloud_normals);
std::cout << "Normals are computed and size is " << cloud_normals->points.size () << std::endl;
// Region growing
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> rg;
rg.setSmoothModeFlag (false); // Depends on the cloud being processed
rg.setInputCloud (cloud_no_nans);
rg.setInputNormals (cloud_normals);
std::vector <pcl::PointIndices> clusters;
rg.extract (clusters);
cloud_segmented = rg.getColoredCloud ();
// Writing the resulting cloud into a pcd file
std::cout << "No of segments done is " << clusters.size () << std::endl;
writer.write<pcl::PointXYZRGB> ("segment_result.pcd", *cloud_segmented, false);
return (0);
}
al::perception::Entity
fromEntityMsg (const al_msgs::Entity &msg_entity)
{
al::perception::Entity entity;
if (msg_entity.cloud_object.data.size ())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg (msg_entity.cloud_object, *cloud_ptr);
entity.setCloudData (cloud_ptr);
}
entity.setFeatures (msg_entity.feature_vector);
entity.setCollisionObject (msg_entity.collision_object);
return entity;
}
int
main (int argc, char** argv)
{
sensor_msgs::PointCloud2 cloud_blob;
pcl::PointCloud<PointT> cloud;
if (pcl::io::loadPCDFile (argv[1], cloud_blob) == -1)
{
ROS_ERROR ("Couldn't read file test_pcd.pcd");
return (-1);
}
ROS_INFO ("Loaded %d data points from test_pcd.pcd with the following fields: %s", (int)(cloud_blob.width * cloud_blob.height), pcl::getFieldsList (cloud_blob).c_str ());
// Convert to the templated message type
pcl::fromROSMsg (cloud_blob, cloud);
pcl::PointCloud<PointT>::Ptr cloud_ptr (new pcl::PointCloud<PointT> (cloud));
pcl::PassThrough<PointT> pass;
pass.setInputCloud (cloud_ptr);
pass.filter (cloud);
ROS_INFO ("Size after removing NANs : %d", (int)cloud.points.size());
ColorRGB color;
vector<pcl::PointPLY> points (cloud.points.size());
for(size_t i = 0; i < cloud.points.size(); i++)
{
color.assignColor(cloud.points.at(i).rgb);
points.at(i).x = cloud.points.at(i).x;
points.at(i).y = cloud.points.at(i).y;
points.at(i).z = cloud.points.at(i).z;
points.at(i).r = color.getR();
points.at(i).g = color.getG();
points.at(i).b = color.getB();
}
std::string fn (argv[1]);
fn = fn.substr(0,fn.find('.'));
fn = fn+ ".ply";
// pcl::io::savePCDFileASCII (fn, cloud);
writePLY(fn,points);
ROS_INFO ("Saved %d data points to ply file.", (int)cloud.points.size ());
return (0);
}
int main(int argc, char** argv) {
int scene_num = atoi(argv[2]);
sensor_msgs::PointCloud2 cloud_blob;
pcl::PointCloud<PointT> cloud;
std::ofstream labelfile, nfeatfile, efeatfile;
if (pcl::io::loadPCDFile(argv[1], cloud_blob) == -1) {
ROS_ERROR("Couldn't read file test_pcd.pcd");
return (-1);
}
ROS_INFO("Loaded %d data points from test_pcd.pcd with the following fields: %s", (int) (cloud_blob.width * cloud_blob.height), pcl::getFieldsList(cloud_blob).c_str());
// convert to templated message type
pcl::fromROSMsg(cloud_blob, cloud);
pcl::PointCloud<PointT>::Ptr cloud_ptr(new pcl::PointCloud<PointT > (cloud));
pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT > ());
pcl::PointCloud<PointT>::Ptr cloud_seg(new pcl::PointCloud<PointT > ());
//pcl::PointCloud<PointXYZI>::Ptr cloud_seg (new pcl::PointCloud<PointXYZI> ());
std::vector<pcl::PointCloud<PointT> > segment_clouds;
std::map<int,int> segment_num_index_map;
pcl::PointIndices::Ptr segment_indices(new pcl::PointIndices());
// get segments
std::set<int> myset;
// find the max segment number
int max_segment_num = 0;
for (size_t i = 0; i < cloud.points.size(); ++i) {
if ( cloud.points[i].label>0) {
myset.insert(cloud.points[i].segment);
}
}
set<int>::iterator it;
SpectralProfile temp;
//vector<SpectralProfile> spectralProfiles;
for (it=myset.begin(); it!=myset.end(); it++) {
apply_segment_filter_and_compute_HOG (*cloud_ptr,*cloud_seg,*it,temp);
cerr<<*it<<"\t"<<cloud_seg->points[1].label<<"\t"<<cloud_seg->size()<<endl;
//if (label!=0) cout << "segment: "<< seg << " label: " << label << " size: " << cloud_seg->points.size() << endl;
}
}
void getSegmentDistanceToBoundary( const pcl::PointCloud<PointT> &cloud , map<int,float> &segment_boundary_distance){
pcl::PointCloud<PointT>::Ptr cloud_rest(new pcl::PointCloud<PointT > ());
pcl::PointCloud<PointT>::Ptr cloud_cam(new pcl::PointCloud<PointT > ());
pcl::PointCloud<PointT>::Ptr cloud_seg(new pcl::PointCloud<PointT > ());
pcl::PointCloud<PointT>::Ptr cloud_ptr(new pcl::PointCloud<PointT > (cloud));
int cnt =0;
// find all the camera indices// find the max segment number
map<int,int> camera_indices;
for (size_t i = 0; i < cloud.points.size(); ++i) {
camera_indices[(int) cloud.points[i].cameraIndex] = 1;
}
// for every camera index .. apply filter anf get the point cloud
for (map<int,int>::iterator it = camera_indices.begin(); it != camera_indices.end();it++)
{
int ci = (*it).first;
apply_camera_filter(*cloud_ptr,*cloud_cam,ci);
// find the segment list
map<int,int> segments;
for (size_t i = 0; i < cloud_cam->points.size(); ++i) {
if( cloud_cam->points[i].label != 0)
segments[(int) cloud_cam->points[i].segment] = 1;
}
for (map<int,int>::iterator it2 = segments.begin(); it2 != segments.end();it2++){
cnt++;
int segnum = (*it2).first;
apply_segment_filter(*cloud_cam,*cloud_seg,segnum);
apply_notsegment_filter(*cloud_cam,*cloud_rest,segnum);
float bdist = getDistanceToBoundary(*cloud_seg,*cloud_rest);
map<int , float>::iterator segit = segment_boundary_distance.find(segnum);
if(segit== segment_boundary_distance.end() || bdist> segment_boundary_distance[segnum] )
segment_boundary_distance[segnum] = bdist;
// if(cnt == 1) outcloud = *cloud_seg;
// else outcloud += *cloud_seg;
}
}
//for(map<int,float>::iterator it = )
}
int LoadPCD::compute()
{
//for each selected filename
for (int k = 0; k < m_filenames.size(); ++k)
{
QString filename = m_filenames[k];
sensor_msgs::PointCloud2 * pcd_sensor_cloud_in = loadSensorMessage(filename);
boost::shared_ptr<sensor_msgs::PointCloud2> cloud_ptr_in = boost::make_shared<sensor_msgs::PointCloud2>(*pcd_sensor_cloud_in);
sensor_msgs::PointCloud2::Ptr cloud_ptr (new sensor_msgs::PointCloud2);
if (pcd_sensor_cloud_in->is_dense == false) //data may contain nans. Remove them
{
//now we need to remove nans
pcl::PassThrough<sensor_msgs::PointCloud2> passFilter;
passFilter.setInputCloud(cloud_ptr_in);
passFilter.filter(*cloud_ptr);
}
else
cloud_ptr = cloud_ptr_in;
ccPointCloud* out_cloud = sm2ccConverter(cloud_ptr).getCCloud();
if (!out_cloud)
return -31;
QString cloud_name = QFileInfo(filename).baseName();
out_cloud->setName(cloud_name);
QFileInfo fi(filename);
QString containerName = QString("%1 (%2)").arg(fi.fileName()).arg(fi.absolutePath());
ccHObject* cloudContainer = new ccHObject(containerName);
assert(out_cloud);
cloudContainer->addChild(out_cloud);
emit newEntity(cloudContainer);
}
return 1;
}
TEST(NearestScanCostFunction, minDistanceToPointCloud)
{
wall_following::NearestScanCostFunction nearest_scan_cost_function;
nearest_scan_cost_function.setMaxDistanceToScan(1.0);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointXYZ point_1;
point_1.x = 0.0;
point_1.y = 0.0;
point_1.z = 0.0;
pcl::PointXYZ point_2;
point_2.x = 1.0;
point_2.y = 1.0;
point_2.z = 0.0;
pcl::PointXYZ point_3;
point_3.x = -10.0;
point_3.y = 0.0;
point_3.z = 0.0;
cloud_ptr->points.push_back(point_1);
cloud_ptr->points.push_back(point_2);
nearest_scan_cost_function.initKdTree(cloud_ptr);
double dist_1 = nearest_scan_cost_function.minDistanceToPointCloud(point_1.x, point_1.y);
EXPECT_FLOAT_EQ(dist_1, 0.0);
double dist_2 = nearest_scan_cost_function.minDistanceToPointCloud(point_2.x, point_2.y);
EXPECT_FLOAT_EQ(dist_2, 0.0);
double dist_3 = nearest_scan_cost_function.minDistanceToPointCloud(point_3.x, point_3.y);
EXPECT_FLOAT_EQ(dist_3, 10.0);
double dist_4 = nearest_scan_cost_function.minDistanceToPointCloud(-0.5, 0.0);
EXPECT_FLOAT_EQ(dist_4, 0.5);
}
int main()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr1(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr segmented_cloud(new pcl::PointCloud<pcl::PointXYZ>);
//load point cloud
loadcloud(cloud_ptr);
pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);
//calculate the normal of the point cloud.
NormalCalculation(cloud_ptr,normals);
//display point cloud.
displaycloud(cloud_ptr);
//display the normal of the point cloud.
displayNormal(cloud_ptr, normals);
//segmentation by region growing method.
segmented_cloud=RegionGrowingSegmentation(cloud_ptr,normals);
//display the segmented the cloud.
displaycloud(segmented_cloud);
return 0;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RGBDFrame::getPCLcloud(){
unsigned char * rgbdata = (unsigned char *)rgb.data;
unsigned short * depthdata = (unsigned short *)depth.data;
const unsigned int width = camera->width; const unsigned int height = camera->height;
const double idepth = camera->idepth_scale;
const double cx = camera->cx; const double cy = camera->cy;
const double ifx = 1.0/camera->fx; const double ify = 1.0/camera->fy;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
cloud->width = width;
cloud->height = height;
cloud->points.resize(width*height);
for(unsigned int w = 0; w < width; w++){
for(unsigned int h = 0; h < height;h++){
int ind = h*width+w;
double z = idepth*double(depthdata[ind]);
if(z > 0){
pcl::PointXYZRGB p;
p.x = (double(w) - cx) * z * ifx;
p.y = (double(h) - cy) * z * ify;
p.z = z;
p.b = rgbdata[3*ind+0];
p.g = rgbdata[3*ind+1];
p.r = rgbdata[3*ind+2];
cloud->points[ind] = p;
}
}
}
pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
ne.setMaxDepthChangeFactor(0.02f);
ne.setNormalSmoothingSize(10.0f);
ne.setInputCloud(cloud);
ne.compute(*normals);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
cloud_ptr->width = width;
cloud_ptr->height = height;
cloud_ptr->points.resize(width*height);
for(unsigned int w = 0; w < width; w++){
for(unsigned int h = 0; h < height;h++){
int ind = h*width+w;
pcl::PointXYZRGBNormal & p0 = cloud_ptr->points[ind];
pcl::PointXYZRGB p1 = cloud->points[ind];
pcl::Normal p2 = normals->points[ind];
p0.x = p1.x;
p0.y = p1.y;
p0.z = p1.z;
p0.r = p1.r;
p0.g = p1.g;
p0.b = p1.b;
p0.normal_x = p2.normal_x;
p0.normal_y = p2.normal_y;
p0.normal_z = p2.normal_z;
}
}
return cloud_ptr;
}
int
main (int argc, char **argv)
{
std::cout << "main function" << std::endl;
pcl::Grabber* interface;
ros::NodeHandle *nh;
ros::Subscriber sub;
ros::Publisher clusterPub, normalPub, planePub,msgPub;
bool spawnObject = true;
K2G *k2g;
processor freenectprocessor = OPENGL;
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB>> cloud;
std::cout << "ros node initialized" << std::endl;
ros::init(argc, argv, "hlpr_single_plane_segmentation",ros::init_options::NoSigintHandler);
nh = new ros::NodeHandle("~");
// nh->getParam("segmentation/viz", viz_);
// std::cout<<"viz is set to " << viz_ << endl;
parsedArguments pA;
if(parseArguments(argc, argv, pA, *nh) < 0)
return 0;
viz_ = pA.viz;
ridiculous_global_variables::ignore_low_sat = pA.saturation_hack;
ridiculous_global_variables::saturation_threshold = pA.saturation_hack_value;
ridiculous_global_variables::saturation_mapped_value = pA.saturation_mapped_value;
RansacSinglePlaneSegmentation multi_plane_app;
pcl::PointCloud<PointT>::Ptr cloud_ptr (new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr ncloud_ptr (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::Label>::Ptr label_ptr (new pcl::PointCloud<pcl::Label>);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
multi_plane_app.initSegmentation(pA.seg_color_ind, pA.ecc_dist_thresh, pA.ecc_color_thresh);
if(viz_) {
viewer = cloudViewer(cloud_ptr);
multi_plane_app.setViewer(viewer);
}
float workSpace[] = {-0.55,0.5,-0.2,0.45,0.3,2.0};
multi_plane_app.setWorkingVolumeThresholds(workSpace);
msgPub = nh->advertise<hlpr_perception_msgs::SegClusters>(segOutRostopic,5);
switch (pA.pc_source)
{
case 0:
{
std::cout << "Using ros topic as input" << std::endl;
sub = nh->subscribe<sensor_msgs::PointCloud2>(pA.ros_topic, 1, cloud_cb_ros_);
break;
}
case 1:
{
std::cout << "Using the openni device" << std::endl;
interface = new pcl::OpenNIGrabber ();
boost::function<void(const pcl::PointCloud<PointT>::ConstPtr&)> f = boost::bind (&cloud_cb_direct_,_1);
boost::signals2::connection c = interface->registerCallback (f);
interface->start ();
break;
}
case 2:
default:
{
std::cout << "Using kinect v2" << std::endl;
freenectprocessor = static_cast<processor>(pA.freenectProcessor);
k2g = new K2G(freenectprocessor, true);
cloud = k2g->getCloud();
prev_cloud = cloud;
gotFirst = true;
break;
}
}
signal(SIGINT, interruptFn);
while (!interrupt && (!viz_ || !viewer->wasStopped ()))
{
if(pA.ros_node)
{
ros::spinOnce();
}
if(viz_)
viewer->spinOnce(20);
if(!gotFirst)
continue;
int selected_cluster_index = -1;
if(cloud_mutex.try_lock ())
{
if(pA.pc_source == 2)
{
cloud = k2g->getCloud();
fake_cloud_cb_kinectv2_(cloud);
}
if(viz_ && pA.justViewPointCloud)
{
pcl::PointCloud<PointT>::Ptr filtered_prev_cloud(new pcl::PointCloud<PointT>(*prev_cloud));
multi_plane_app.preProcPointCloud(filtered_prev_cloud);
if (!viewer->updatePointCloud<PointT> (filtered_prev_cloud, "cloud"))
{
viewer->addPointCloud<PointT> (filtered_prev_cloud, "cloud");
}
selected_cluster_index = -1;
}
else
{
pcl::PointCloud<PointT>::CloudVectorType clusters;
std::vector<pcl::PointCloud<pcl::Normal> > clusterNormals;
Eigen::Vector4f plane;
std::vector<size_t> clusterIndices;
std::vector<std::vector<int>> clusterIndicesStore;
selected_cluster_index = multi_plane_app.processOnce(prev_cloud,clusters,clusterNormals,plane,clusterIndicesStore,
pA.pre_proc,
pA.merge_clusters, viz_, pA.filterNoise); //true is for the viewer
hlpr_perception_msgs::SegClusters msg;
//msg.header.stamp = ros::Time::now();
// Pull out the cluster indices and put in msg
for (int ti = 0; ti < clusterIndicesStore.size(); ti++){
hlpr_perception_msgs::ClusterIndex cluster_idx_msg;
for (int j = 0; j < clusterIndicesStore[ti].size(); j++){
std_msgs::Int32 temp_msg;
temp_msg.data = clusterIndicesStore[ti][j];
cluster_idx_msg.indices.push_back(temp_msg);
}
msg.cluster_ids.push_back(cluster_idx_msg);
//clusterIndices.insert(clusterIndices.end(),clusterIndicesStore[ti].begin(), clusterIndicesStore[ti].end()); // For removing ALL cluster points
}
for(int i = 0; i < clusters.size(); i++) {
sensor_msgs::PointCloud2 out;
pcl::PCLPointCloud2 tmp;
pcl::toPCLPointCloud2(clusters[i], tmp);
pcl_conversions::fromPCL(tmp, out);
msg.clusters.push_back(out);
}
for(int i = 0; i < clusterNormals.size(); i++) {
sensor_msgs::PointCloud2 out;
pcl::PCLPointCloud2 tmp;
pcl::toPCLPointCloud2(clusterNormals[i], tmp);
pcl_conversions::fromPCL(tmp, out);
msg.normals.push_back(out);
}
std_msgs::Float32MultiArray planeMsg;
planeMsg.data.clear();
for(int i = 0; i < 4; i++)
planeMsg.data.push_back(plane[i]);
//planePub.publish(planeMsg);
msg.plane = planeMsg;
msgPub.publish(msg);
//clusterPub.publish(clusters[0]);
//normalPub.publish(clusterNormals[0]);
}
cloud_mutex.unlock();
/*the z_thresh may result in wrong cluster to be selected. it might be a good idea to do
* some sort of mean shift tracking, or selecting the biggest amongst the candidates (vs choosing the most similar color)
* or sending all the plausible ones to c6 and decide there
*/
}
if(selected_cluster_index < 0)
continue;
}
if(pA.pc_source == 1)
{
interface->stop ();
delete interface;
}
delete nh;
ros::shutdown();
return 0;
}
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_no_nans (new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_segmented (new pcl::PointCloud<pcl::PointXYZRGB>());
struct timeval tpstart,tpend;
float timeuse;
pcl::PCDWriter writer;
if (pcl::io::loadPCDFile(argv[1], *cloud_ptr)==-1)
{
std::cout << "Couldn't find the file " << argv[1] << std::endl;
return -1;
}
std::cout << "Loaded cloud " << argv[1] << " of size " << cloud_ptr->points.size() << std::endl;
// Remove the nans
cloud_ptr->is_dense = false;
cloud_no_nans->is_dense = false;
std::vector<int> indices;
pcl::removeNaNFromPointCloud (*cloud_ptr, *cloud_no_nans, indices);
std::cout << "Removed nans from " << cloud_ptr->points.size () << " to " << cloud_no_nans->points.size () << std::endl;
gettimeofday(&tpstart,NULL);
// Estimate the normals
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud_no_nans);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n (new pcl::search::KdTree<pcl::PointXYZ>());
ne.setSearchMethod (tree_n);
ne.setKSearch(30);
//ne.setRadiusSearch (0.03);
ne.compute (*cloud_normals);
std::cout << "Normals are computed and size is " << cloud_normals->points.size () << std::endl;
// Region growing
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> rg;
rg.setMinClusterSize (50);
rg.setSmoothModeFlag (true); // Depends on the cloud being processed
rg.setSmoothnessThreshold (10*M_PI/180);
rg.setResidualTestFlag (true);
rg.setResidualThreshold (0.005);
rg.setCurvatureTestFlag (true);
rg.setCurvatureThreshold (0.008);
rg.setNumberOfNeighbours (30);
rg.setInputCloud (cloud_no_nans);
rg.setInputNormals (cloud_normals);
std::vector <pcl::PointIndices> clusters;
rg.extract (clusters);
cloud_segmented = rg.getColoredCloud ();
cloud_segmented->width = cloud_segmented->points.size();
cloud_segmented->height = 1;
gettimeofday(&tpend,NULL);
timeuse=1000000*(tpend.tv_sec-tpstart.tv_sec) + tpend.tv_usec-tpstart.tv_usec;
timeuse/=1000000;
std::cout << "Segmentation time: " << timeuse << std::endl;
std::cout << "Number of segments done is " << clusters.size () << std::endl;
writer.write<pcl::PointXYZRGB> ("segment_result.pcd", *cloud_segmented, false);
std::cout << "Number of points in the segments: " << cloud_segmented->size() << std::endl;
pcl::visualization::PCLVisualizer viewer ("Detected planes with Pseudo-color");
viewer.setBackgroundColor (1.0, 1.0, 1.0);
viewer.addPointCloud (cloud_segmented, "cloud segmented", 0);
viewer.addCoordinateSystem ();
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud segmented");
viewer.spin();
// Writing the resulting cloud into a pcd file
return (0);
}
