Created
September 16, 2013 11:15
-
-
Save felix-kolbe/6579354 to your computer and use it in GitHub Desktop.
I think this worked, probably originated in some pcl tutorial..
http://answers.ros.org/question/41450/object-manipulation-or-tabletop-detection-with-non-pr2/?comment=78801#comment-78801
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include <boost/thread.hpp> | |
#include <ros/ros.h> | |
#include <tf/transform_broadcaster.h> | |
#include <tf/transform_listener.h> | |
#include <sensor_msgs/PointCloud2.h> | |
// PCL specific includes | |
#include <pcl_ros/point_cloud.h> | |
#include <pcl/ros/conversions.h> | |
#include <pcl_ros/transforms.h> | |
#include <pcl/point_cloud.h> | |
#include <pcl/point_types.h> | |
#include <pcl/io/pcd_io.h> | |
#include <pcl/sample_consensus/method_types.h> | |
#include <pcl/sample_consensus/model_types.h> | |
#include <pcl/segmentation/sac_segmentation.h> | |
#include <pcl/filters/extract_indices.h> | |
#include <pcl/filters/passthrough.h> | |
#include <pcl/features/normal_3d.h> | |
#define VIS | |
#ifdef VIS | |
#include <pcl/visualization/pcl_visualizer.h> | |
#include <pcl/visualization/point_cloud_handlers.h> | |
#endif | |
typedef pcl::PointXYZ PointT; | |
ros::Publisher pub_plane, pub_plane2, pub_cyl, pub_coeff; | |
#ifdef VIS | |
pcl::visualization::PCLVisualizer* viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); | |
//pcl::visualization::PCLVisualizer* rgbVis (pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud) | |
#endif | |
pcl::PassThrough<PointT> pass; | |
pcl::NormalEstimation<PointT, pcl::Normal> ne; | |
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> segn; | |
// pcl::SACSegmentation<PointT> segn; | |
pcl::ExtractIndices<pcl::Normal> extract_normals; | |
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ()); | |
boost::shared_ptr<pcl::PointCloud<PointT> > cloud_filtered (new pcl::PointCloud<PointT>); | |
boost::shared_ptr<pcl::PointCloud<pcl::Normal> > cloud_normals (new pcl::PointCloud<pcl::Normal>); | |
boost::shared_ptr<pcl::PointCloud<PointT> > cloud_without_plane (new pcl::PointCloud<PointT>); | |
boost::shared_ptr<pcl::PointCloud<pcl::Normal> > cloud_normals_wo_plane (new pcl::PointCloud<pcl::Normal>); | |
boost::shared_ptr<pcl::ModelCoefficients > coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients); | |
boost::shared_ptr<pcl::PointIndices > inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices); | |
boost::condition_variable cond; | |
boost::mutex mut; | |
bool data_ready; | |
//tf::TransformListener tf_listener; | |
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input) { | |
sensor_msgs::PointCloud2 transformed; | |
// tf_listener.transformPointCloud("base_link", *input, transformed); | |
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud | |
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); | |
pcl::fromROSMsg(*input, *cloud); | |
// pcl_ros:: | |
// // Read in the cloud data | |
// pcl::PCDReader reader; | |
// reader.read ("table_scene_mug_stereo_textured.pcd", *cloud); | |
// ROS_INFO_STREAM("PointCloud has: " << cloud->points.size () << " data points."); | |
//* | |
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); | |
pcl::PointIndices::Ptr inliers (new pcl::PointIndices); | |
// Create the segmentation object | |
pcl::SACSegmentation<pcl::PointXYZ> seg; | |
// pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; | |
// Optional | |
seg.setOptimizeCoefficients (true); | |
// Mandatory | |
seg.setModelType (pcl::SACMODEL_PLANE); | |
seg.setMethodType (pcl::SAC_RANSAC); | |
seg.setDistanceThreshold (0.01); | |
seg.setInputCloud(cloud); | |
seg.segment (*inliers, *coefficients); | |
if (inliers->indices.size () == 0) | |
{ | |
PCL_ERROR ("Could not estimate a planar model for the given dataset."); | |
// return (-1); | |
} | |
ROS_INFO_STREAM("Planar model:"); | |
ROS_INFO_STREAM("Model coefficients: " << coefficients->values[0] << " " | |
<< coefficients->values[1] << " " | |
<< coefficients->values[2] << " " | |
<< coefficients->values[3]); | |
// the estimated plane parameters (in ax + by + cz + d = 0 form) | |
// static tf::TransformBroadcaster tf_br; | |
// tf::Transform transform; | |
// transform.setOrigin(tf::Vector3(coefficients->values[0], coefficients->values[1], coefficients->values[2])); | |
//// transform.setRotation(tf::Quaternion(msg->theta, 0, 0) ); | |
// tf_br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/kinect1_rgb_optical_frame", "planar")); | |
ROS_INFO_STREAM("Model inliers: " << inliers->indices.size ()); | |
// for (size_t i = 0; i < inliers->indices.size (); ++i) | |
// ROS_INFO_STREAM(inliers->indices[i] << " " << cloud->points[inliers->indices[i]].x << " " | |
// << cloud->points[inliers->indices[i]].y << " " | |
// << cloud->points[inliers->indices[i]].z); | |
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>); | |
//*/ | |
// Create the filtering object | |
pcl::ExtractIndices<pcl::PointXYZ> extract; | |
//* | |
// Extract the inliers | |
extract.setInputCloud (cloud); | |
extract.setIndices (inliers); | |
extract.setNegative (false); | |
extract.filter (*cloud_out); | |
cloud_out->header.stamp = ros::Time::now (); | |
cloud_out->header.frame_id = "/kinect1_rgb_optical_frame"; | |
// cloud_out->height = cloud_out->width = 1; | |
// for (size_t i = 0; i < inliers->indices.size (); ++i) | |
// cloud_out->push_back(pcl::PointXYZ(cloud.points[inliers->indices[i]])); | |
// sensor_msgs::PointCloud2 output; | |
// Publish the data | |
pub_plane.publish (cloud_out); | |
// pub_coeff.publish(coefficients); | |
//*/ | |
//----- | |
ROS_INFO_STREAM("next models:"); | |
// pcl::PassThrough<PointT> pass; | |
// pcl::NormalEstimation<PointT, pcl::Normal> ne; | |
// pcl::SACSegmentationFromNormals<PointT, pcl::Normal> segn; | |
//// pcl::SACSegmentation<PointT> segn; | |
// pcl::ExtractIndices<pcl::Normal> extract_normals; | |
// pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ()); | |
// | |
// boost::shared_ptr<pcl::PointCloud<PointT> > cloud_filtered (new pcl::PointCloud<PointT>); | |
// boost::shared_ptr<pcl::PointCloud<pcl::Normal> > cloud_normals (new pcl::PointCloud<pcl::Normal>); | |
// boost::shared_ptr<pcl::PointCloud<PointT> > cloud_without_plane (new pcl::PointCloud<PointT>); | |
// boost::shared_ptr<pcl::PointCloud<pcl::Normal> > cloud_normals_wo_plane (new pcl::PointCloud<pcl::Normal>); | |
// boost::shared_ptr<pcl::ModelCoefficients > coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients); | |
// boost::shared_ptr<pcl::PointIndices > inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices); | |
ROS_INFO_STREAM("PointCloud has: " << cloud->points.size () << " data points."); | |
ROS_INFO_STREAM("Filtering..."); | |
// Build a passthrough filter to remove spurious NaNs | |
pass.setInputCloud (cloud); | |
pass.setFilterFieldName ("z"); | |
pass.setFilterLimits (0, 1.5); | |
pass.filter (*cloud_filtered); | |
ROS_INFO_STREAM("PointCloud after filtering has: " << cloud_filtered->points.size () << " data points."); | |
// cloud_filtered = cloud; | |
ROS_INFO_STREAM("Estimating point normals..."); | |
// Estimate point normals | |
ne.setSearchMethod (tree); | |
ne.setInputCloud (cloud_filtered); | |
ne.setKSearch (50); | |
ne.compute (*cloud_normals); | |
// Create the segmentation object for the planar model and set all the parameters | |
segn.setOptimizeCoefficients (true); | |
segn.setModelType (pcl::SACMODEL_NORMAL_PLANE); | |
// segn.setModelType (pcl::SACMODEL_PLANE); | |
segn.setNormalDistanceWeight (0.1); // 0.1 | |
segn.setMethodType (pcl::SAC_RANSAC); | |
segn.setMaxIterations (100); | |
segn.setDistanceThreshold (0.05); // 0.02 | |
segn.setInputCloud (cloud_filtered); | |
segn.setInputNormals (cloud_normals); | |
ROS_INFO_STREAM("Obtaining plane inliers..."); | |
// Obtain the plane inliers and coefficients | |
segn.segment (*inliers_plane, *coefficients_plane); | |
ROS_INFO_STREAM("Plane coefficients: " << *coefficients_plane); | |
ROS_INFO_STREAM("Extracting plane inliers..."); | |
// Extract the planar inliers from the input cloud | |
extract.setInputCloud (cloud_filtered); | |
extract.setIndices (inliers_plane); | |
extract.setNegative (false); | |
// Write the planar inliers to disk | |
pcl::PointCloud<PointT>::Ptr cloud_plane (new pcl::PointCloud<PointT> ()); | |
extract.filter (*cloud_plane); | |
ROS_INFO_STREAM("PointCloud representing the planar component: " << cloud_plane->points.size () << " data points."); | |
// writer.write ("table_scene_mug_stereo_textured_plane.pcd", *cloud_plane, false); | |
pub_plane2.publish (cloud_plane); | |
ROS_INFO_STREAM("Removing plane inliers..."); | |
// Remove the planar inliers, extract the rest | |
extract.setNegative (true); | |
extract.filter (*cloud_without_plane); | |
// pub_plane.publish (cloud_without_plane); | |
ROS_INFO_STREAM("Extracting normals..."); | |
extract_normals.setNegative (true); | |
extract_normals.setInputCloud (cloud_normals); | |
extract_normals.setIndices (inliers_plane); | |
extract_normals.filter (*cloud_normals_wo_plane); | |
// Create the segmentation object for cylinder segmentation and set all the parameters | |
segn.setOptimizeCoefficients (true); | |
segn.setModelType (pcl::SACMODEL_CYLINDER); | |
segn.setMethodType (pcl::SAC_RRANSAC); | |
segn.setNormalDistanceWeight (0.01); | |
// segn.setMaxIterations (10000); | |
segn.setMaxIterations (500); | |
segn.setDistanceThreshold (0.02); | |
segn.setRadiusLimits (0.02, 0.06); | |
segn.setInputCloud (cloud_without_plane); | |
segn.setInputNormals (cloud_normals_wo_plane); | |
ROS_INFO_STREAM("Obtaining cylinder inliers..."); | |
// Obtain the cylinder inliers and coefficients | |
segn.segment (*inliers_cylinder, *coefficients_cylinder); | |
ROS_INFO_STREAM("Cylinder coefficients: " << *coefficients_cylinder); | |
// // Write the cylinder inliers to disk | |
extract.setInputCloud (cloud_without_plane); | |
extract.setIndices (inliers_cylinder); | |
extract.setNegative (false); | |
ROS_INFO_STREAM("Extracting cylinder inliers..."); | |
pcl::PointCloud<PointT>::Ptr cloud_cylinder (new pcl::PointCloud<PointT> ()); | |
extract.filter (*cloud_cylinder); | |
#ifdef VIS | |
ROS_INFO("Notifying viewer thread.."); | |
{ | |
boost::lock_guard<boost::mutex> lock(mut); | |
data_ready=true; | |
} | |
cond.notify_one(); | |
// viewer->removeAllPointClouds(); | |
// viewer->addPointCloud<pcl::PointXYZ> (cloud_without_plane, "cloud wo plane"); | |
// viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_without_plane, cloud_normals_wo_plane, 1, 0.001, "normals"); | |
// pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color (cloud_cylinder, 0, 255, 0); | |
// viewer->addPointCloud<pcl::PointXYZ> (cloud_cylinder, single_color, "cylinder cloud"); | |
#endif | |
if (cloud_cylinder->points.empty ()) | |
ROS_WARN_STREAM("Can't find the cylindrical component."); | |
else { | |
ROS_INFO_STREAM("PointCloud representing the cylindrical component: " << cloud_cylinder->points.size () << " data points."); | |
// writer.write ("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false); | |
pub_cyl.publish (cloud_cylinder); | |
} | |
// return (0); | |
// exit(42); | |
} | |
void ros_spin() { | |
ros::spin(); | |
} | |
int main (int argc, char** argv) { | |
// Initialize ROS | |
ros::init (argc, argv, "planar_segmentation"); | |
ros::NodeHandle nh; | |
// Create a ROS subscriber for the input point cloud | |
ros::Subscriber sub = nh.subscribe ("/kinect1/depth_registered/points", 1, cloud_cb); | |
// Create a ROS publisher for the output point cloud | |
pub_plane = nh.advertise<pcl::PointCloud<pcl::PointXYZ> > ("segmented_plane", 1); | |
pub_plane2 = nh.advertise<pcl::PointCloud<pcl::PointXYZ> > ("segmented_plane2", 1); | |
pub_cyl = nh.advertise<pcl::PointCloud<pcl::PointXYZ> > ("cylinder", 1); | |
// Create a ROS publisher for the output model coefficients | |
pub_coeff = nh.advertise<pcl::ModelCoefficients> ("seg_plane_model_coeff", 1); | |
#ifdef VIS | |
viewer->setBackgroundColor (0, 0, 0); | |
viewer->addCoordinateSystem (0.5); | |
viewer->initCameraParameters (); | |
viewer->addPointCloud<pcl::PointXYZ> (cloud_without_plane, "cloud wo plane"); | |
viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_without_plane, cloud_normals_wo_plane, 1, 0.001, "normals"); | |
#endif | |
// Spin | |
#ifdef VIS | |
boost::thread ros_spinner(ros_spin); | |
// ros::Duration sleeptime(0.010); | |
while (!viewer->wasStopped ()) | |
{ | |
// ros::spinOnce(); | |
viewer->spinOnce (100); | |
// sleeptime.sleep(); | |
boost::this_thread::sleep (boost::posix_time::milliseconds (100)); | |
boost::unique_lock<boost::mutex> lock(mut); | |
// while(!data_ready) { | |
{ | |
cond.timed_wait(lock, boost::posix_time::milliseconds(10)); | |
if(data_ready) { | |
data_ready = false; | |
ROS_INFO("Updating Viewer.."); | |
// viewer->removeAllPointClouds(); | |
viewer->updatePointCloud<pcl::PointXYZ> (cloud_without_plane, "cloud wo plane"); | |
viewer->removePointCloud("normals"); | |
viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_without_plane, cloud_normals_wo_plane, 1, 0.001, "normals"); | |
} | |
} | |
} | |
#else | |
ros::spin (); | |
#endif | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
I tried this code but it didn't work...
The error message is like that,
'''
In file included from /usr/include/vtk-6.2/vtkObjectBase.h:44:0,
from /usr/include/vtk-6.2/vtkSmartPointerBase.h:27,
from /usr/include/vtk-6.2/vtkSmartPointer.h:23,
from /usr/include/pcl-1.7/pcl/visualization/point_cloud_geometry_handlers.h:48,
from /usr/include/pcl-1.7/pcl/visualization/point_cloud_handlers.h:41,
from /usr/include/pcl-1.7/pcl/visualization/common/actor_map.h:40,
from /usr/include/pcl-1.7/pcl/visualization/pcl_visualizer.h:48,
from /home/kyungpyo/git/ACE_DANGUN/src/app/brain_perception/src/lidar_tracker/src/./tracker/lidar_track.hpp:48,
from /home/kyungpyo/git/ACE_DANGUN/src/app/brain_perception/src/lidar_tracker/src/./tracker/lidar_tracker.hpp:55,
from /home/kyungpyo/git/ACE_DANGUN/src/app/brain_perception/src/lidar_tracker/src/ros_lidar_tracker.hpp:81,
from /home/kyungpyo/git/ACE_DANGUN/src/app/brain_perception/src/lidar_tracker/src/ros_lidar_tracker.cpp:2:
/usr/include/vtk-6.2/vtkAtomicInt.h:307:28: error: reference to ‘detail’ is ambiguous
class vtkAtomicInt: public detail::vtkAtomicIntImpl
^
/usr/include/vtk-6.2/vtkAtomicInt.h:70:1: note: candidates are: namespace detail { }
{
'''
If you can solve this problem, please comment.
Thanks!