void readerCallback (const sensor_msgs::PointCloud2ConstPtr& input)
{
const pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
const pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
pcl::fromROSMsg(*input, *cloud);
std::cerr << "PointCloud has: " << cloud->points.size () << " data points." << std::endl;
pcl::RegionGrowingRGB<PointT> reg_seg;
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> sac_seg;
sac_seg.setOptimizeCoefficients (true);
sac_seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
sac_seg.setNormalDistanceWeight (0.1);
sac_seg.setMethodType (pcl::SAC_RANSAC);
sac_seg.setMaxIterations (200);
sac_seg.setDistanceThreshold (0.03);
filterCloud(cloud, cloud, normals);
removeModel(cloud, normals, sac_seg, true);
sac_seg.setDistanceThreshold (0.01);
removeModel(cloud, normals, sac_seg, true);
//sac_seg.setModelType (pcl::SACMODEL_LINE);
//sac_seg.setDistanceThreshold (0.15);
//removeModel(cloud, normals, sac_seg, false);
std::cerr << "PointCloud representing the extracted component: " << cloud->points.size () << " data points." << std::endl;
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(*cloud, output);
pc_pub.publish(output);
}
void ObjectDetector::cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*input, *input_cloud);
// if theta and y_translation are not yet set, then run segmentation
if(theta == 0.0 && y_translation == 0.0){
performSegmentation(input_cloud);
}
else{
// downsample the point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled_cloud = downsampleCloud(input_cloud);
// perform transformation
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud = transformCloud(downsampled_cloud);
// filter floor, walls and noise so only objects remain
pcl::PointCloud<pcl::PointXYZ>::Ptr object_cloud = filterCloud(transformed_cloud);
// cluster objects & publish cluster
clusterCloud(object_cloud);
}
}
Object_Recognition::Object_Recognition(ros::NodeHandle& nh,boost::property_tree::ptree& propertyTree)
// Initialization
:
modelCloud(new PointCloudColored), siExtractor(propertyTree) , filterCloud(propertyTree)
{
// Correlation point cloud blob size parameters
maxBlobSize = propertyTree.get<std::size_t>("CorrelationParam.maxBlobSize");
maxModelBlobSize = propertyTree.get<std::size_t>("CorrelationParam.maxModelBlobSize");
// Correlation accuracy requirement parameters
minAccuracy = propertyTree.get<float>("CorrelationParam.minAccuracy");
minCount = propertyTree.get<std::size_t>("CorrelationParam.minCount");
// Visualization parameters
camera_frame_name = propertyTree.get<std::string>("Visualization.camera_frame_name");
// Initialize the library to load from
Load_From_PCD loadedModel(propertyTree);
// Initialize the publisher for the recognized object and its bounding box
filteredCloudPub = nh.advertise<PointCloudColored>("filteredCloud",1,true);
clusterPub = nh.advertise<PointCloudColored>("clusterCloud",1,true);
// Load library of references
cloudLibrary = loadedModel.getCloudLibrary();
// Reserve space for the library's spin images
librarySpinImages.reserve(cloudLibrary.size());
for (size_t cloud = 0; cloud < cloudLibrary.size() ; cloud++)
{
// Initialize a histogram object for the library of histograms and push the allocation to the vector of the library
pcl::PointCloud<pcl::Histogram<histLength>>::Ptr hist (new pcl::PointCloud<pcl::Histogram<histLength>>);
librarySpinImages.push_back(hist);
// Get the corresponding spin image point cloud of histograms
siExtractor.getSpinImage(cloudLibrary[cloud].myCloud, librarySpinImages[cloud]);
// Free pointer space
hist.reset();
}
// Initialize the bounding box publisher
boxPub = nh.advertise<visualization_msgs::Marker>(propertyTree.get<std::string>("Namespace.boundingBoxTopicName"),1);
// A point cloud filterer object
Cloud_Filter filterCloud(propertyTree);
// Marker types to indicate the type of the output
marker.type = visualization_msgs::Marker::CUBE;
}
void CloudAssimilator::processClouds()
{
pcl::PointCloud<pcl::PointXYZ> combined_cloud = m_left_cloud;
if(m_left_cloud.points.size() == 0 || m_right_cloud.points.size() == 0)
{
ROS_WARN("Clouds empty!");
m_have_new_left_cloud = false;
m_have_new_right_cloud = false;
return;
}
if(m_sync_clouds)
{
pcl::PointCloud<pcl::PointXYZ> synced_right_cloud;
if(!timeSyncCloud(pcl_conversions::fromPCL(m_left_cloud.header), m_right_cloud, synced_right_cloud))
{
m_have_new_left_cloud = false;
m_have_new_right_cloud = false;
return;
}
combined_cloud += synced_right_cloud;
}
else
{
combined_cloud += m_right_cloud;
}
if(combined_cloud.points.size() == 0)
{
ROS_WARN("Input clouds had no points!");
return;
}
pcl::PointCloud<pcl::PointXYZ> filtered_cloud;
filterCloud(combined_cloud, filtered_cloud);
// if(m_double_filter && filtered_cloud.points.size() != 0)
// {
// pcl::PointCloud<pcl::PointXYZ> double_filtered_cloud;
// filterCloud(filtered_cloud, double_filtered_cloud);
// filtered_cloud = double_filtered_cloud;
// }
if((((double) filtered_cloud.size()) / ((double) combined_cloud.size())) < 0.5)
{
ROS_WARN_THROTTLE(1.0, "%d/%d (%g%%) cloud points remaining after filtering", (int) filtered_cloud.size(), (int) combined_cloud.size(), 100.0 * filtered_cloud.size() / combined_cloud.size());
}
sensor_msgs::PointCloud2 filtered_cloud_msg2;
pcl::toROSMsg(filtered_cloud, filtered_cloud_msg2);
filtered_cloud_msg2.header = pcl_conversions::fromPCL(m_left_cloud.header);
sensor_msgs::PointCloud filtered_cloud_msg;
convertPointCloud2ToPointCloud(filtered_cloud_msg2, filtered_cloud_msg);
filtered_cloud_msg.header = filtered_cloud_msg2.header;
m_cloud_pub.publish(filtered_cloud_msg);
if(m_output_laser_scan)
{
sensor_msgs::LaserScan scan;
pointCloudToLaserScan(filtered_cloud_msg, scan);
m_scan_pub.publish(scan);
}
m_have_new_left_cloud = false;
m_have_new_right_cloud = false;
}