void topicCallback_input(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
/* protected region user implementation of subscribe callback for input on begin */
pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2 ());
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ());
// Transformation into PCL type PointCloud2
pcl_conversions::toPCL((*msg), *(pcl_pc));
// Transformation into PCL type PointCloud<pcl::PointXYZRGB>
pcl::fromPCLPointCloud2(*(pcl_pc), *(pcl_cloud));
////////////////////////
// PassThrough filter //
////////////////////////
pcl::ConditionOr<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionOr<pcl::PointXYZ> ());
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::GT, robot_pose_x+localconfig.inhib_size)));
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::LT, robot_pose_x-localconfig.inhib_size)));
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::GT, robot_pose_y+localconfig.inhib_size)));
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::LT, robot_pose_y-localconfig.inhib_size)));
// build the filter
pcl::ConditionalRemoval<pcl::PointXYZ> condrem (range_cond);
condrem.setInputCloud (pcl_cloud);
condrem.setKeepOrganized(true);
// apply filter
condrem.filter (*pcl_cloud);
// Transformation into ROS type
pcl::toPCLPointCloud2(*(pcl_cloud), *(cloud_out));
pcl_conversions::moveFromPCL(*(cloud_out), output_pcloud2);
output_ready = true;
/* protected region user implementation of subscribe callback for input end */
}
void My_Filter::pclCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud){
pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2 ());
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr final2 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ());
sensor_msgs::PointCloud2 pc2;
std::vector<int> inliers;
// Transformation into PCL type PointCloud2
pcl_conversions::toPCL((*cloud), *(pcl_pc));
// Transformation into PCL type PointCloud<pcl::PointXYZRGB>
pcl::fromPCLPointCloud2(*(pcl_pc), *(pcl_cloud));
std::list<geometry_msgs::PoseStamped> tmp_list;
int robot_counter = 0;
for (std::list<geometry_msgs::PoseStamped>::iterator it = robots.begin() ; it != robots.end(); ++it)
{
if(it->pose.orientation.x > 0.001)
{ // Robot currently not seen
////////////////////////
// PassThrough filter //
////////////////////////
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (pcl_cloud);
pass.setFilterFieldName ("x");
pass.setFilterLimits (it->pose.position.x-DETECTION_DISTANCE-it->pose.orientation.x*0.002,
it->pose.position.x+DETECTION_DISTANCE+it->pose.orientation.x*0.002);
//pass.setFilterLimitsNegative (true);
pass.filter (*final2);
pass.setInputCloud (final2);
pass.setFilterFieldName ("y");
pass.setFilterLimits (it->pose.position.y-DETECTION_DISTANCE-it->pose.orientation.x*0.002,
it->pose.position.y+DETECTION_DISTANCE+it->pose.orientation.x*0.002);
//pass.setFilterLimitsNegative (true);
pass.filter (*final2);
}
else
{ // Robot seen
////////////////////////
// PassThrough filter //
////////////////////////
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (pcl_cloud);
pass.setFilterFieldName ("x");
pass.setFilterLimits (it->pose.position.x-DETECTION_DISTANCE, it->pose.position.x+DETECTION_DISTANCE);
//pass.setFilterLimitsNegative (true);
pass.filter (*final2);
pass.setInputCloud (final2);
pass.setFilterFieldName ("y");
pass.setFilterLimits (it->pose.position.y-DETECTION_DISTANCE, it->pose.position.y+DETECTION_DISTANCE);
//pass.setFilterLimitsNegative (true);
pass.filter (*final2);
pcl::ConditionOr<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionOr<pcl::PointXYZ> ());
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::GT, it->pose.position.x+DETECTION_DISTANCE)));
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::LT, it->pose.position.x-DETECTION_DISTANCE)));
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::GT, it->pose.position.y+DETECTION_DISTANCE)));
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::LT, it->pose.position.y-DETECTION_DISTANCE)));
// build the filter
pcl::ConditionalRemoval<pcl::PointXYZ> condrem (range_cond);
condrem.setInputCloud (pcl_cloud);
condrem.setKeepOrganized(true);
// apply filter
condrem.filter (*pcl_cloud);
}
if(final2->size() > 2)
{ // Something in the box
double meanX = 0.0;
double meanY = 0.0;
for(int i = 0; i < final2->size(); i++)
{
meanX += final2->at(i).x;
meanY += final2->at(i).y;
}
meanX = meanX / final2->size();
meanY = meanY / final2->size();
//robots.at(robot_counter).pose.position.x = meanX;
//robots.at(robot_counter).pose.position.y = meanY;
it->pose.position.x = meanX;
it->pose.position.y = meanY;
char numstr[50];
sprintf(numstr, "/robot_%s_link", it->header.frame_id.c_str());
t = tf::StampedTransform(tf::Transform(tf::createQuaternionFromYaw(0.0), tf::Vector3(meanX, meanY, 0.0)),
ros::Time::now(), "/world", numstr);
t.stamp_ = ros::Time::now();
broadcaster.sendTransform(t);
it->pose.orientation.x = 0.0;
tmp_list.push_front(*it);
//std::cout << "size : " << final2->size() << std::endl;
}
else
{ // Nothing found in the box
it->pose.orientation.x += 1.0;
if(it->pose.orientation.x > 100.0)
{
it->pose.orientation.x = 100.0;
}
//std::cout << "coef : " << it->pose.orientation.x << std::endl;
geometry_msgs::PoseStamped tmp = *it;
//robots.push_back(*it);
//robots.erase(it);
//robots.push_back(tmp);
tmp_list.push_back(*it);
}
robot_counter++;
}
robots = tmp_list;
/////////////////////////////////
// Statistical Outlier Removal //
/////////////////////////////////
/* pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud (pcl_cloud);
sor.setMeanK (200);
sor.setStddevMulThresh (1.0);
sor.filter (*final);
*/
//////////////////////////////////
// Euclidian Cluster Extraction //
//////////////////////////////////
// Creating the KdTree object for the search method of the extraction
/* pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (pcl_cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.05); // 2cm
ec.setMinClusterSize (5);
ec.setMaxClusterSize (50);
ec.setSearchMethod (tree);
ec.setInputCloud (pcl_cloud);
ec.extract (cluster_indices);
std::cout << cluster_indices.size() << std::endl;
double x = 0.0;
double y = 0.0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
x = 0.0;
y = 0.0;
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
{
x +=pcl_cloud->points[*pit].x;
y +=pcl_cloud->points[*pit].y;
}
x = x / it->indices.size();
y = y / it->indices.size();
std::cout << "x : " << x << " y : " << y << " size : " << it->indices.size() << std::endl;
}
*/
// Transformation into ROS type
pcl::toPCLPointCloud2(*(pcl_cloud), *(cloud_out));
pcl_conversions::moveFromPCL(*(cloud_out), pc2);
point_cloud_publisher_.publish(pc2);
}
void topicCallback_input_cloud(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
/* protected region user implementation of subscribe callback for input_cloud on begin */
pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2 ());
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr final2 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ());
sensor_msgs::PointCloud2 pc2;
std::vector<int> inliers;
// Transformation into PCL type PointCloud2
pcl_conversions::toPCL((*msg), *(pcl_pc));
// Transformation into PCL type PointCloud<pcl::PointXYZRGB>
pcl::fromPCLPointCloud2(*(pcl_pc), *(pcl_cloud));
std::list<geometry_msgs::PoseStamped> tmp_list;
int robot_counter = 0;
for (std::list<geometry_msgs::PoseStamped>::iterator it = robots.begin() ; it != robots.end(); ++it)
{
if(it->pose.orientation.x > 0.001)
{ // Robot currently not seen
////////////////////////
// PassThrough filter //
////////////////////////
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (pcl_cloud);
pass.setFilterFieldName ("x");
pass.setFilterLimits (it->pose.position.x-localconfig.detection_distance-it->pose.orientation.x*0.002,
it->pose.position.x+localconfig.detection_distance+it->pose.orientation.x*0.002);
//pass.setFilterLimitsNegative (true);
pass.filter (*final2);
pass.setInputCloud (final2);
pass.setFilterFieldName ("y");
pass.setFilterLimits (it->pose.position.y-localconfig.detection_distance-it->pose.orientation.x*0.002,
it->pose.position.y+localconfig.detection_distance+it->pose.orientation.x*0.002);
//pass.setFilterLimitsNegative (true);
pass.filter (*final2);
}
else
{ // Robot seen
////////////////////////
// PassThrough filter //
////////////////////////
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (pcl_cloud);
pass.setFilterFieldName ("x");
pass.setFilterLimits (it->pose.position.x-localconfig.detection_distance, it->pose.position.x+localconfig.detection_distance);
//pass.setFilterLimitsNegative (true);
pass.filter (*final2);
pass.setInputCloud (final2);
pass.setFilterFieldName ("y");
pass.setFilterLimits (it->pose.position.y-localconfig.detection_distance, it->pose.position.y+localconfig.detection_distance);
//pass.setFilterLimitsNegative (true);
pass.filter (*final2);
pcl::ConditionOr<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionOr<pcl::PointXYZ> ());
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::GT, it->pose.position.x+localconfig.detection_distance)));
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::LT, it->pose.position.x-localconfig.detection_distance)));
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::GT, it->pose.position.y+localconfig.detection_distance)));
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::LT, it->pose.position.y-localconfig.detection_distance)));
// build the filter
pcl::ConditionalRemoval<pcl::PointXYZ> condrem (range_cond);
condrem.setInputCloud (pcl_cloud);
condrem.setKeepOrganized(true);
// apply filter
condrem.filter (*pcl_cloud);
}
if(final2->size() > 2)
{ // Something in the box
double meanX = 0.0;
double meanY = 0.0;
for(int i = 0; i < final2->size(); i++)
{
meanX += final2->at(i).x;
meanY += final2->at(i).y;
}
meanX = meanX / final2->size();
meanY = meanY / final2->size();
//robots.at(robot_counter).pose.position.x = meanX;
//robots.at(robot_counter).pose.position.y = meanY;
it->pose.position.x = (meanX + it->pose.position.x) / 2.0;
it->pose.position.y = (meanY + it->pose.position.y) / 2.0;
char numstr[50];
sprintf(numstr, "/robot_%s_link", it->header.frame_id.c_str());
t = tf::StampedTransform(tf::Transform(tf::createQuaternionFromYaw(0.0), tf::Vector3(meanX, meanY, 0.0)),
ros::Time::now(), "/world", numstr);
t.stamp_ = ros::Time::now();
broadcaster.sendTransform(t);
it->pose.orientation.x = 0.0;
tmp_list.push_front(*it);
if(it->header.frame_id == "0")
{
robot1_output_ready = true;
}
else
{
robot2_output_ready = true;
}
//std::cout << "size : " << final2->size() << std::endl;
}
else
{ // Nothing found in the box
it->pose.orientation.x += 1.0;
if(it->pose.orientation.x > 100.0)
{
it->pose.orientation.x = 100.0;
}
//std::cout << "coef : " << it->pose.orientation.x << std::endl;
geometry_msgs::PoseStamped tmp = *it;
//robots.push_back(*it);
//robots.erase(it);
//robots.push_back(tmp);
tmp_list.push_back(*it);
}
robot_counter++;
}
robots = tmp_list;
/* protected region user implementation of subscribe callback for input_cloud end */
}
void callback(const PCMsg::ConstPtr& kinect_pc_msg){
// Conversion from sensor_msgs::PointCloud2 to pcl::PointCloud
pcl::fromROSMsg(*kinect_pc_msg, *kinects_pc_);
// Remove NaNs
vector<int> indices;
pcl::removeNaNFromPointCloud<PointType>(*kinects_pc_, *kinects_pc_, indices);
// Downsampling
pc_downsampling<PointType>(kinects_pc_, 0.005,cluster_cloud_);
// Get transform between pc frame and base_link
if(first_frame_){
// get transform between the pc frame and the output frame
get_transform(kinects_pc_->header.frame_id, "base_link", pc_transform_);
first_frame_ = false;
}
// Transform the pc to base_link
pcl::transformPointCloud(*cluster_cloud_, *cluster_cloud_, pc_transform_.translation, pc_transform_.rotation);
kinects_pc_->header.frame_id = "base_link";
// Clip the pointcloud
pcl::ConditionAnd<PointType>::Ptr height_cond (new pcl::ConditionAnd<PointType> ());
pcl::ConditionalRemoval<PointType> condrem (height_cond);
height_cond->addComparison ( pcl::FieldComparison<PointType>::ConstPtr (new pcl::FieldComparison<PointType> ("z", pcl::ComparisonOps::GT, 0.15)));
height_cond->addComparison ( pcl::FieldComparison<PointType>::ConstPtr (new pcl::FieldComparison<PointType> ("y", pcl::ComparisonOps::GT, -1.0)));
height_cond->addComparison ( pcl::FieldComparison<PointType>::ConstPtr (new pcl::FieldComparison<PointType> ("y", pcl::ComparisonOps::LT, 1.0)));
height_cond->addComparison ( pcl::FieldComparison<PointType>::ConstPtr (new pcl::FieldComparison<PointType> ("x", pcl::ComparisonOps::LT, 1.5)));
height_cond->addComparison ( pcl::FieldComparison<PointType>::ConstPtr (new pcl::FieldComparison<PointType> ("x", pcl::ComparisonOps::GT, -0.5)));
condrem.setInputCloud (cluster_cloud_);
condrem.setKeepOrganized(true);
condrem.filter (*cluster_cloud_);
// Estimate normals
norm_est_.setKSearch(20);
norm_est_.setInputCloud (cluster_cloud_);
norm_est_.compute (*normals_);
// PCL Viewer
if(first_frame_)
viewer_.addPointCloudNormals<PointType, NormalType>(cluster_cloud_, normals_, 1, 0.05, "normals");
else{
viewer_.removePointCloud("normals");
viewer_.addPointCloudNormals<PointType, NormalType>(cluster_cloud_, normals_, 1, 0.05, "normals");
}
viewer_.updatePointCloud(cluster_cloud_,"sample_cloud");
viewer_.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "normals");
viewer_.spinOnce();
bool use_hough_ (true), show_correspondences_(true), show_keypoints_(true);
float model_ss_ (0.005f);
float scene_ss_ (0.005f);
float descr_rad_ (0.036f);
//float rf_rad_ (0.1f);
float rf_rad_(rf_rad);
float cg_size_ (0.2f);
float cg_thresh_ (3.0f);
int knn = 1;
pcl::PointCloud<int> model_keypoints_indices, scene_keypoints_indices;
pcl::PointCloud<PointType>::Ptr model_keypoints, scene_keypoints;
model_keypoints = boost::shared_ptr<pcl::PointCloud<PointType> >(new pcl::PointCloud<PointType> );
scene_keypoints = boost::shared_ptr<pcl::PointCloud<PointType> >(new pcl::PointCloud<PointType> );
pcl::UniformSampling<PointType> uniform_sampling;
uniform_sampling.setInputCloud (robot_pc_);
uniform_sampling.setRadiusSearch (model_ss_);
uniform_sampling.compute(model_keypoints_indices);
std::cout << "Model total points: " << robot_pc_->size () << "; Selected Keypoints: " << model_keypoints_indices.size () << std::endl;
uniform_sampling.setInputCloud (cluster_cloud_);
uniform_sampling.setRadiusSearch (scene_ss_);;
uniform_sampling.compute(scene_keypoints_indices);
std::cout << "Scene total points: " << cluster_cloud_->size () << "; Selected Keypoints: " << scene_keypoints_indices.size () << std::endl;
// Use Keypoint indices to make a PointCloud
model_keypoints->points.resize(model_keypoints_indices.points.size ());
for (size_t i=0; i<model_keypoints_indices.points.size (); ++i)
model_keypoints->points[i].getVector3fMap () = robot_pc_->points[model_keypoints_indices.points[i]].getVector3fMap();
scene_keypoints->points.resize (scene_keypoints_indices.points.size ());
for (size_t i=0; i<scene_keypoints_indices.points.size (); ++i)
scene_keypoints->points[i].getVector3fMap () = cluster_cloud_->points[scene_keypoints_indices.points[i]].getVector3fMap();
std::cout<<"keypoints converted"<<std::endl;
//
// Compute Descriptor for keypoints
//
pcl::PointCloud<DescriptorType>::Ptr model_descriptors (new pcl::PointCloud<DescriptorType> ());
pcl::PointCloud<DescriptorType>::Ptr scene_descriptors (new pcl::PointCloud<DescriptorType> ());
pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est;
//descr_est.setKSearch(10);
descr_est.setRadiusSearch (descr_rad_);
descr_est.setInputCloud (model_keypoints);
descr_est.setInputNormals (robot_normals_);
descr_est.setSearchSurface (robot_pc_);
descr_est.compute (*model_descriptors);
std::cout<<"SHOTEstimationOMP done for model"<<std::endl;
pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est2;
// descr_est2.setKSearch(10);
descr_est2.setRadiusSearch (descr_rad_);
descr_est2.setInputCloud (scene_keypoints);
descr_est2.setInputNormals (normals_);
descr_est2.setSearchSurface (cluster_cloud_);
descr_est2.compute (*scene_descriptors);
std::cout<<"SHOTEstimationOMP done for scene"<<std::endl;
//
// Find Model-Scene Correspondences with KdTree
//
pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
pcl::KdTreeFLANN<DescriptorType> match_search;
match_search.setInputCloud (model_descriptors);
std::cout<<"KdTreeFLANN searched"<<std::endl;
// For each scene keypoint descriptor, find nearest neighbor into the model keypoints descriptor cloud and add it to the correspondences vector.
// for (size_t i = 0; i < scene_descriptors->size (); ++i)
// {
// std::vector<int> neigh_indices (1);
// std::vector<float> neigh_sqr_dists (1);
// if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs
// {
// continue;
// }
// int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
// // if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
// if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
// {
// pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
// model_scene_corrs->push_back (corr);
// }
// }
for (size_t i = 0; i < scene_descriptors->size (); ++i)
{
std::vector<int> neigh_indices (knn);
std::vector<float> neigh_sqr_dists (knn);
if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs
{
continue;
}
int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), knn, neigh_indices, neigh_sqr_dists);
for(int k = 0; k < found_neighs; k++)
{
if(found_neighs == 1 && neigh_sqr_dists[k] < 0.1f) // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
{
pcl::Correspondence corr (neigh_indices[k], static_cast<int> (i), neigh_sqr_dists[k]);
model_scene_corrs->push_back (corr);
}
}
}
std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl;
//
// Actual Clustering
//
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
std::vector<pcl::Correspondences> clustered_corrs;
std::cout<<"Actual clustering done"<<std::endl;
// Using Hough3D
if (use_hough_)
{
//
// Compute (Keypoints) Reference Frames only for Hough
//
pcl::PointCloud<RFType>::Ptr model_rf (new pcl::PointCloud<RFType> ());
pcl::PointCloud<RFType>::Ptr scene_rf (new pcl::PointCloud<RFType> ());
pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est;
rf_est.setFindHoles (true);
rf_est.setRadiusSearch (rf_rad_);
rf_est.setInputCloud (model_keypoints);
rf_est.setInputNormals (robot_normals_);
rf_est.setSearchSurface (robot_pc_);
rf_est.compute (*model_rf);
rf_est.setInputCloud (scene_keypoints);
rf_est.setInputNormals (normals_);
rf_est.setSearchSurface (cluster_cloud_);
rf_est.compute (*scene_rf);
// Clustering
pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
clusterer.setHoughBinSize (cg_size_);
clusterer.setHoughThreshold (cg_thresh_);
clusterer.setUseInterpolation (true);
clusterer.setUseDistanceWeight (false);
clusterer.setInputCloud (model_keypoints);
clusterer.setInputRf (model_rf);
clusterer.setSceneCloud (scene_keypoints);
clusterer.setSceneRf (scene_rf);
clusterer.setModelSceneCorrespondences (model_scene_corrs);
//clusterer.cluster (clustered_corrs);
clusterer.recognize (rototranslations, clustered_corrs);
}
else // Using GeometricConsistency
{
pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
gc_clusterer.setGCSize (cg_size_);
gc_clusterer.setGCThreshold (cg_thresh_);
gc_clusterer.setInputCloud (model_keypoints);
gc_clusterer.setSceneCloud (scene_keypoints);
gc_clusterer.setModelSceneCorrespondences (model_scene_corrs);
//gc_clusterer.cluster (clustered_corrs);
gc_clusterer.recognize (rototranslations, clustered_corrs);
}
//
// Output results
//
std::cout << "Model instances found: " << rototranslations.size () << std::endl;
for (size_t i = 0; i < rototranslations.size (); ++i)
{
std::cout << "\n Instance " << i + 1 << ":" << std::endl;
std::cout << " Correspondences belonging to this instance: " << clustered_corrs[i].size () << std::endl;
// Print the rotation matrix and translation vector
Eigen::Matrix3f rotation = rototranslations[i].block<3,3>(0, 0);
Eigen::Vector3f translation = rototranslations[i].block<3,1>(0, 3);
printf ("\n");
printf (" | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
printf (" R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
printf (" | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
printf ("\n");
printf (" t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));
}
// Visualization
// White cloud: scene
// Yellow cloud: off_scene_MODEL
// Blue cloud: scene keypoints and off_scene_MODEL keypoints
// Red cloud: rotated model
// Green lines: correspondences between
pcl::visualization::PCLVisualizer viewer ("Correspondence Grouping");
viewer.addPointCloud (cluster_cloud_, "scene_cloud");
pcl::PointCloud<PointType>::Ptr off_scene_model (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr off_scene_model_keypoints (new pcl::PointCloud<PointType> ());
if (show_correspondences_ || show_keypoints_)
{
// We are translating the model so that it doesn't end in the middle of the scene representation
pcl::transformPointCloud (*robot_pc_, *off_scene_model, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0));
pcl::transformPointCloud (*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0));
pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler (off_scene_model, 255, 255, 128);
viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model");
}
if (show_keypoints_)
{
pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_keypoints_color_handler (scene_keypoints, 0, 0, 255);
viewer.addPointCloud (scene_keypoints, scene_keypoints_color_handler, "scene_keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "scene_keypoints");
pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_keypoints_color_handler (off_scene_model_keypoints, 0, 0, 255);
viewer.addPointCloud (off_scene_model_keypoints, off_scene_model_keypoints_color_handler, "off_scene_model_keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "off_scene_model_keypoints");
}
for (size_t i = 0; i < rototranslations.size (); ++i)
{
pcl::PointCloud<PointType>::Ptr rotated_model (new pcl::PointCloud<PointType> ());
pcl::transformPointCloud (*robot_pc_, *rotated_model, rototranslations[i]);
std::stringstream ss_cloud;
ss_cloud << "instance" << i;
pcl::visualization::PointCloudColorHandlerCustom<PointType> rotated_model_color_handler (rotated_model, 255, 0, 0);
viewer.addPointCloud (rotated_model, rotated_model_color_handler, ss_cloud.str ());
if (show_correspondences_)
{
for (size_t j = 0; j < clustered_corrs[i].size (); ++j)
{
std::stringstream ss_line;
ss_line << "correspondence_line" << i << "_" << j;
PointType& model_point = off_scene_model_keypoints->at (clustered_corrs[i][j].index_query);
PointType& scene_point = scene_keypoints->at (clustered_corrs[i][j].index_match);
// We are drawing a line for each pair of clustered correspondences found between the model and the scene
viewer.addLine<PointType, PointType> (model_point, scene_point, 0, 255, 0, ss_line.str ());
}
}
}
while (!viewer.wasStopped ())
{
viewer.spinOnce ();
}
exit(0);
}
bool filterIt(int argc, char **argv,pcl::PointCloud<PointType>::Ptr cloud, pcl::PointCloud<PointType>::Ptr cloud_filtered) {
pcl::ScopeTime time("performance");
time.reset();
if (strcmp(argv[2], "-r") == 0) {
if (argc != 5) {
std::cout << "All the parameters are not correctly set for the radius removal filter" << std::endl;
return 0;
}
pcl::RadiusOutlierRemoval<PointType> outrem;
// build the filter
outrem.setInputCloud(cloud);
outrem.setRadiusSearch(atof(argv[3]));
outrem.setMinNeighborsInRadius (atof(argv[4]));
// apply filter
outrem.filter (*cloud_filtered);
}
else if (strcmp(argv[2], "-c") == 0) {
if (argc != 6) {
std::cout << "All the parameters are not correctly set for the conditional filter" << std::endl;
return 0;
}
// build the condition
pcl::ConditionAnd<PointType>::Ptr range_cond (new pcl::ConditionAnd<PointType> ());
range_cond->addComparison (pcl::FieldComparison<PointType>::ConstPtr (new
pcl::FieldComparison<PointType> (argv[5], pcl::ComparisonOps::GT, atof ( argv[3] ))));
range_cond->addComparison (pcl::FieldComparison<PointType>::ConstPtr (new
pcl::FieldComparison<PointType> (argv[5], pcl::ComparisonOps::LT, atof(argv[4]) )));
// build the filter
pcl::ConditionalRemoval<PointType> condrem (range_cond);
condrem.setInputCloud (cloud);
condrem.setKeepOrganized(false);
// apply filter
condrem.filter (*cloud_filtered);
}
else if (strcmp(argv[2], "-s") == 0) {
if (argc != 5) {
std::cout << "All the parameters are not correctly set for the statistical filter" << std::endl;
return 0;
}
// Create the filtering object
pcl::StatisticalOutlierRemoval<PointType> sor;
sor.setInputCloud (cloud);
sor.setMeanK (atof(argv[3]));
sor.setStddevMulThresh (atof(argv[4]));
sor.filter (*cloud_filtered);
}
else if (strcmp(argv[2], "-v") == 0) {
// Create the filtering object
pcl::VoxelGrid<PointType> sor;
sor.setDownsampleAllData(true);
sor.setInputCloud (cloud);
sor.setLeafSize (atof(argv[3]), atof(argv[3]), atof(argv[3]));
sor.filter (*cloud_filtered);
}
else if (strcmp(argv[2], "-av") == 0) {
// Create the filtering object
pcl::ApproximateVoxelGrid<PointType> sor;
sor.setDownsampleAllData(true);
sor.setInputCloud (cloud);
sor.setLeafSize (atof(argv[3]), atof(argv[3]), atof(argv[3]));
sor.filter (*cloud_filtered);
}
else if (strcmp(argv[2], "-rg") == 0) {
// Create the filtering object
std::vector<pcl::IndicesPtr> clusteredIndices;
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::RegionGrowing<PointType> sor;
//sor.setDownsampleAllData(true);
sor.setInputCloud (cloud);
sor.setGrowingDistance(atof(argv[3]));
//sor.setLeafSize (atof(argv[3]), atof(argv[3]), atof(argv[3]));
if (atof(argv[4]) > 0){
sor.setEpsAngle(atof(argv[4]));
pcl::NormalEstimation<PointType, pcl::Normal> ne;
ne.setInputCloud (cloud);
pcl::search::KdTree<PointType>::Ptr tree (new pcl::search::KdTree<PointType> ());
ne.setSearchMethod (tree);
ne.setRadiusSearch (atof(argv[3]));
ne.compute (*cloud_normals);
sor.setNormals(cloud_normals);
cout << "normals estimed" << endl;
}
sor.cluster (&clusteredIndices);
cout << "clusters? " <<clusteredIndices.size()<<endl;
classification<PointType> classif(cloud, clusteredIndices);
// pcl::search::KdTree<PointType>::Ptr tree (new pcl::search::KdTree<PointType>);
// tree->setInputCloud (cloud);
//extractEuclideanClusters (
// const PointCloud<PointT> &cloud, const boost::shared_ptr<search::Search<PointT> > &tree,
// float tolerance, std::vector<PointIndices> &clusters,
// unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
// boost::shared_ptr<pcl::search::KdTree<PointType> > tree (new pcl::search::KdTree<PointType>);
// tree->setInputCloud (cloud);
// std::vector<pcl::PointIndices> clusters;
// pcl::extractEuclideanClusters<PointType>(cloud, tree, 10, clusters, 100, 200);
// pcl::EuclideanClusterExtraction<PointType> ec;
// ec.setClusterTolerance ( atof(argv[3]) ); // 2cm
// ec.setSearchMethod (tree);
// ec.setInputCloud (cloud);
// ec.extract (clusteredIndices);
// cout << "clusters? " <<clusteredIndices.size()<<endl;
pcl::PointCloud<PointType>::Ptr buff_cloud (new pcl::PointCloud<PointType>);
for (int i=0; i<clusteredIndices.size(); i++) {
//cout << "ok 2" << endl;
if (clusteredIndices.operator[](i)->size() < 2300 || clusteredIndices.operator[](i)->size() > 2500) {
// cout << "ok 3 " << i << endl;
//cout << clusteredIndices[i]->size() << endl;
pcl::copyPointCloud(*cloud, clusteredIndices[i].operator*(), *buff_cloud);
cloud_filtered->operator+=(*buff_cloud);
buff_cloud->clear();
}
}
if(system("mkdir clusters"))
cout << "creating directory \"clusters\""<< endl;
saveclusters(clusteredIndices, cloud, "clusters/");
std::stringstream command;
command << "pcd_viewer ";
for (int i =0 ; i< clusteredIndices.size(); i++)
// if (clusteredIndices[i].indices.size() > 2300 && clusteredIndices[i].indices.size() < 2500)
command << "clusters/" << i << ".pcd ";
if(system (command.str().data()))
cout << "visualizing clusters" << endl;
FILE *fp;
fp = fopen ( "command.sh", "wt" ) ;
fprintf ( fp, " %s\n", command.str().data() );
fclose(fp);
}
else {
std::cerr << "please specify the point cloud and the command line arg '-r' or '-c' or '-s' or '-v' or '-av' or 'rg' + param" << std::endl;
exit(0);
}
time.getTime();
return 1;
}
void
pcl::kinfuLS::WorldModel<PointT>::getExistingData(const double previous_origin_x, const double previous_origin_y, const double previous_origin_z, const double offset_x, const double offset_y, const double offset_z, const double volume_x, const double volume_y, const double volume_z, pcl::PointCloud<PointT> &existing_slice)
{
double newOriginX = previous_origin_x + offset_x;
double newOriginY = previous_origin_y + offset_y;
double newOriginZ = previous_origin_z + offset_z;
double newLimitX = newOriginX + volume_x;
double newLimitY = newOriginY + volume_y;
double newLimitZ = newOriginZ + volume_z;
// filter points in the space of the new cube
PointCloudPtr newCube (new pcl::PointCloud<PointT>);
// condition
ConditionAndPtr range_condAND (new pcl::ConditionAnd<PointT> ());
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, newOriginX)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, newLimitX)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, newOriginY)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, newLimitY)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, newOriginZ)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, newLimitZ)));
// build the filter
pcl::ConditionalRemoval<PointT> condremAND (true);
condremAND.setCondition (range_condAND);
condremAND.setInputCloud (world_);
condremAND.setKeepOrganized (false);
// apply filter
condremAND.filter (*newCube);
// filter points that belong to the new slice
ConditionOrPtr range_condOR (new pcl::ConditionOr<PointT> ());
if(offset_x >= 0)
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, previous_origin_x + volume_x - 1.0 )));
else
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, previous_origin_x )));
if(offset_y >= 0)
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, previous_origin_y + volume_y - 1.0 )));
else
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, previous_origin_y )));
if(offset_z >= 0)
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, previous_origin_z + volume_z - 1.0 )));
else
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, previous_origin_z )));
// build the filter
pcl::ConditionalRemoval<PointT> condrem (true);
condrem.setCondition (range_condOR);
condrem.setInputCloud (newCube);
condrem.setKeepOrganized (false);
// apply filter
condrem.filter (existing_slice);
if(existing_slice.points.size () != 0)
{
//transform the slice in new cube coordinates
Eigen::Affine3f transformation;
transformation.translation ()[0] = newOriginX;
transformation.translation ()[1] = newOriginY;
transformation.translation ()[2] = newOriginZ;
transformation.linear ().setIdentity ();
transformPointCloud (existing_slice, existing_slice, transformation.inverse ());
}
}
inline void
normal_diff_filter(
const pcl::PointCloud<PointT> &in,
pcl::PointCloud<PointT> &out,
double scale1 = 0.40, ///The smallest scale to use in the DoN filter.
double scale2 = 0.60, ///The largest scale to use in the DoN filter.
double threshold = 0.6 ///The minimum DoN magnitude to threshold by
)
{
boost::shared_ptr<pcl::PointCloud<PointT> > in_ptr(in.makeShared());
boost::shared_ptr<pcl::search::KdTree<PointT> > tree_n;
tree_n.reset( new pcl::search::KdTree<PointT>(false) );
tree_n->setInputCloud(in_ptr);
tree_n->setEpsilon(0.5);
if (scale1 >= scale2)
{
printf("Error: Large scale must be > small scale!");
return;
}
// Compute normals using both small and large scales at each point
pcl::NormalEstimationOMP<PointT, pcl::PointNormal> ne;
ne.setInputCloud (in_ptr);
ne.setSearchMethod (tree_n);
/**
* NOTE: setting viewpoint is very important, so that we can ensure
* normals are all pointed in the same direction!
*/
ne.setViewPoint (std::numeric_limits<float>::max (), std::numeric_limits<float>::max (), std::numeric_limits<float>::max ());
// calculate normals with the small scale
pcl::PointCloud<pcl::PointNormal>::Ptr normals_small_scale (new pcl::PointCloud<pcl::PointNormal>);
ne.setRadiusSearch (scale1);
ne.compute (*normals_small_scale);
// calculate normals with the large scale
pcl::PointCloud<pcl::PointNormal>::Ptr normals_large_scale (new pcl::PointCloud<pcl::PointNormal>);
ne.setRadiusSearch (scale2);
ne.compute (*normals_large_scale);
// Create output cloud for DoN results
pcl::PointCloud<pcl::PointNormal>::Ptr doncloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::copyPointCloud<PointT, pcl::PointNormal>(in, *doncloud);
// Create DoN operator
pcl::DifferenceOfNormalsEstimation<PointT, pcl::PointNormal, pcl::PointNormal> don;
don.setInputCloud(in_ptr);
don.setNormalScaleLarge(normals_large_scale);
don.setNormalScaleSmall(normals_small_scale);
if (!don.initCompute ())
{
std::cerr << "Error: Could not intialize DoN feature operator" << std::endl;
return;
}
// Compute DoN
don.computeFeature (*doncloud);
// Build the condition for filtering
pcl::ConditionOr<pcl::PointNormal>::Ptr range_cond (
new pcl::ConditionOr<pcl::PointNormal> ()
);
range_cond->addComparison (pcl::FieldComparison<pcl::PointNormal>::ConstPtr (
new pcl::FieldComparison<pcl::PointNormal> ("curvature", pcl::ComparisonOps::GT, threshold))
);
// Build the filter
pcl::ConditionalRemoval<pcl::PointNormal> condrem (range_cond, true);
condrem.setInputCloud(doncloud);
pcl::PointCloud<pcl::PointNormal>::Ptr doncloud_filtered (new pcl::PointCloud<pcl::PointNormal>);
// Apply filter
condrem.filter(*doncloud_filtered);
boost::shared_ptr<pcl::PointIndices> indices(new pcl::PointIndices);
condrem.getRemovedIndices(*indices);
//std::cout << "Indices: " << indices->indices.size() << " Filtered: " << doncloud_filtered->size () << " data points." << std::endl;
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud(in_ptr);
extract.setIndices(indices);
extract.setNegative (true);
extract.filter(out);
// Save filtered output
//std::cout << "In: " << in.size() << " Filtered: " << out.size () << " data points." << std::endl;
}
int
main (int argc, char** argv)
{
if (argc != 2)
{
std::cerr << "please specify command line arg '-r' or '-c'" << std::endl;
exit(0);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the cloud data
cloud->width = 5;
cloud->height = 1;
cloud->points.resize (cloud->width * cloud->height);
for (size_t i = 0; i < cloud->points.size (); ++i)
{
cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
}
if (strcmp(argv[1], "-r") == 0){
pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
// build the filter
outrem.setInputCloud(cloud);
outrem.setRadiusSearch(0.8);
outrem.setMinNeighborsInRadius (2);
// apply filter
outrem.filter (*cloud_filtered);
}
else if (strcmp(argv[1], "-c") == 0){
// build the condition
pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond (new
pcl::ConditionAnd<pcl::PointXYZ> ());
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("z", pcl::ComparisonOps::GT, 0.0)));
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("z", pcl::ComparisonOps::LT, 0.8)));
// build the filter
pcl::ConditionalRemoval<pcl::PointXYZ> condrem (range_cond);
condrem.setInputCloud (cloud);
condrem.setKeepOrganized(true);
// apply filter
condrem.filter (*cloud_filtered);
}
else{
std::cerr << "please specify command line arg '-r' or '-c'" << std::endl;
exit(0);
}
std::cerr << "Cloud before filtering: " << std::endl;
for (size_t i = 0; i < cloud->points.size (); ++i)
std::cerr << " " << cloud->points[i].x << " "
<< cloud->points[i].y << " "
<< cloud->points[i].z << std::endl;
// display pointcloud after filtering
std::cerr << "Cloud after filtering: " << std::endl;
for (size_t i = 0; i < cloud_filtered->points.size (); ++i)
std::cerr << " " << cloud_filtered->points[i].x << " "
<< cloud_filtered->points[i].y << " "
<< cloud_filtered->points[i].z << std::endl;
return (0);
}
void
pcl::WorldModel<PointT>::getWorldAsCubes (const double size, std::vector<typename pcl::WorldModel<PointT>::PointCloudPtr> &cubes, std::vector<Eigen::Vector3f> &transforms, double overlap)
{
if(world_->points.size () == 0)
{
PCL_INFO("The world is empty, returning nothing\n");
return;
}
PCL_INFO ("Getting world as cubes. World contains %d points.\n", world_->points.size ());
// remove nans from world cloud
world_->is_dense = false;
std::vector<int> indices;
pcl::removeNaNFromPointCloud ( *world_, *world_, indices);
PCL_INFO ("World contains %d points after nan removal.\n", world_->points.size ());
// check cube size value
double cubeSide = size;
if (cubeSide <= 0.0f)
{
PCL_ERROR ("Size of the cube must be positive and non null (%f given). Setting it to 3.0 meters.\n", cubeSide);
cubeSide = 512.0f;
}
std::cout << "cube size is set to " << cubeSide << std::endl;
// check overlap value
double step_increment = 1.0f - overlap;
if (overlap < 0.0)
{
PCL_ERROR ("Overlap ratio must be positive or null (%f given). Setting it to 0.0 procent.\n", overlap);
step_increment = 1.0f;
}
if (overlap > 1.0)
{
PCL_ERROR ("Overlap ratio must be less or equal to 1.0 (%f given). Setting it to 10 procent.\n", overlap);
step_increment = 0.1f;
}
// get world's bounding values on XYZ
PointT min, max;
pcl::getMinMax3D(*world_, min, max);
PCL_INFO ("Bounding box for the world: \n\t [%f - %f] \n\t [%f - %f] \n\t [%f - %f] \n", min.x, max.x, min.y, max.y, min.z, max.z);
PointT origin = min;
// clear returned vectors
cubes.clear();
transforms.clear();
// iterate with box filter
while (origin.x < max.x)
{
origin.y = min.y;
while (origin.y < max.y)
{
origin.z = min.z;
while (origin.z < max.z)
{
// extract cube here
PCL_INFO ("Extracting cube at: [%f, %f, %f].\n", origin.x, origin.y, origin.z);
// pointcloud for current cube.
PointCloudPtr box (new pcl::PointCloud<PointT>);
// set conditional filter
ConditionAndPtr range_cond (new pcl::ConditionAnd<PointT> ());
range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, origin.x)));
range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, origin.x + cubeSide)));
range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, origin.y)));
range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, origin.y + cubeSide)));
range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, origin.z)));
range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, origin.z + cubeSide)));
// build the filter
pcl::ConditionalRemoval<PointT> condrem (range_cond);
condrem.setInputCloud (world_);
condrem.setKeepOrganized(false);
// apply filter
condrem.filter (*box);
// also push transform along with points.
if(box->points.size() > 0)
{
Eigen::Vector3f transform;
transform[0] = origin.x, transform[1] = origin.y, transform[2] = origin.z;
transforms.push_back(transform);
cubes.push_back(box);
}
else
{
PCL_INFO ("Extracted cube was empty, skiping this one.\n");
}
origin.z += cubeSide * step_increment;
}
origin.y += cubeSide * step_increment;
}
origin.x += cubeSide * step_increment;
}
/* for(int c = 0 ; c < cubes.size() ; ++c)
{
std::stringstream name;
name << "cloud" << c+1 << ".pcd";
pcl::io::savePCDFileASCII(name.str(), *(cubes[c]));
}*/
std::cout << "returning " << cubes.size() << " cubes" << std::endl;
}
