void
DoSampleRun ()
{
cob_3d_mapping_filters::SpeckleFilter<PointXYZ> filter;
pcl::PointCloud<PointXYZ>::Ptr cloud (new pcl::PointCloud<PointXYZ> ());
pcl::PointCloud<PointXYZ>::Ptr cloud_out (new pcl::PointCloud<PointXYZ> ());
pcl::io::loadPCDFile ("/home/goa/Ubuntu One/diss/images/raw/filter_sequence_amplitude2.pcd", *cloud);
filter.setInputCloud (cloud);
filter.filter (*cloud_out);
pcl::io::savePCDFileASCII ("/home/goa/Ubuntu One/diss/images/raw/filter_sequence_speckle2.pcd", *cloud_out);
}
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 */
}
/** Creates point cloud with random points,
transforms created point cloud with known translate matrix,
adds noise to transformed point cloud,
estimates rotation and translation matrix. output matrix to ANDROID LOG */
void simplePclRegistration()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the CloudIn data
cloud_in->width = 5;
cloud_in->height = 1;
cloud_in->is_dense = false;
cloud_in->points.resize (cloud_in->width * cloud_in->height);
for (size_t i = 0; i < cloud_in->points.size (); ++i)
{
cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
}
/* std::cout << "Saved " << cloud_in->points.size () << " data points to input:" << std::endl;
for (size_t i = 0; i < cloud_in->points.size (); ++i) std::cout << " " <<
cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<
cloud_in->points[i].z << std::endl;
*/
*cloud_out = *cloud_in;
// std::cout << "size:" << cloud_out->points.size() << std::endl;
for (size_t i = 0; i < cloud_in->points.size (); ++i)
{ cloud_out->points[i].x = cloud_in->points[i].x + 0.7f+ 0.01f*rand()/(RAND_MAX + 1.0f);
cloud_out->points[i].y = cloud_in->points[i].y + 0.2f;
}
// std::cout << "Transformed " << cloud_in->points.size () << " data points:" << std::endl;
/* for (size_t i = 0; i < cloud_out->points.size (); ++i)
std::cout << " " << cloud_out->points[i].x << " " << cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;
*/
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputCloud(cloud_in);
icp.setInputTarget(cloud_out);
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);
// std::cout << "has converged:" << icp.hasConverged() << " score: " <<
// icp.getFitnessScore() << std::endl;
// std::cout << icp.getFinalTransformation() << std::endl;
string outputstr;
ostringstream out_message;
out_message << icp.getFinalTransformation();
outputstr=out_message.str();
LOGI("%s", outputstr.c_str());
}
int
main (int argc, char** argv)
{
srand(time(0));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the CloudIn data
cloud_in->width = 5;
cloud_in->height = 1;
cloud_in->is_dense = false;
cloud_in->points.resize (cloud_in->width * cloud_in->height);
for (size_t i = 0; i < cloud_in->points.size (); ++i)
{
cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
}
std::cout << "Saved " << cloud_in->points.size () << " data points to input:"
<< std::endl;
for (size_t i = 0; i < cloud_in->points.size (); ++i) std::cout << " " <<
cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<
cloud_in->points[i].z << std::endl;
*cloud_out = *cloud_in;
std::cout << "size:" << cloud_out->points.size() << std::endl;
for (size_t i = 0; i < cloud_in->points.size (); ++i)
cloud_out->points[i].x = cloud_in->points[i].x + 70.0f;
std::cout << "Transformed " << cloud_in->points.size () << " data points:"
<< std::endl;
for (size_t i = 0; i < cloud_out->points.size (); ++i)
std::cout << " " << cloud_out->points[i].x << " " <<
cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;
// ICP
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputCloud(cloud_in);
icp.setInputTarget(cloud_out);
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
return (0);
}
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile ("/home/omari/Datasets/Static_Scenes/scene_0014/pc_cluster_1.pcd", *cloud_in);
pcl::io::loadPCDFile ("/home/omari/Datasets/Static_Scenes/scene_0014/pc_cluster_2.pcd", *cloud_out);
// Fill in the CloudIn data
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud_out);
icp.setInputTarget(cloud_in);
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
return (0);
}
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 */
}
int
main (int argc, char** argv)
{
// Get input object and scene
if (argc < 2)
{
pcl::console::print_error ("Syntax is: %s cloud1.pcd (cloud2.pcd)\n", argv[0]);
return (1);
}
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZRGBA>);
// Load object and scene
pcl::console::print_highlight ("Loading point clouds...\n");
if(argc<3)
{
if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[1], *cloud_in) < 0)
pcl::console::print_error ("Error loading first file!\n");
*cloud_out = *cloud_in;
//transform cloud
Eigen::Affine3f transformation;
transformation.setIdentity();
transformation.translate(Eigen::Vector3f(0.3,0.02,-0.1));
float roll, pitch, yaw;
roll = 0.02; pitch = 1.2; yaw = 0;
Eigen::AngleAxisf rollAngle(roll, Eigen::Vector3f::UnitX());
Eigen::AngleAxisf pitchAngle(pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf yawAngle(yaw, Eigen::Vector3f::UnitZ());
Eigen::Quaternion<float> q = rollAngle*pitchAngle*yawAngle;
transformation.rotate(q);
pcl::transformPointCloud<pcl::PointXYZRGBA>(*cloud_in, *cloud_out, transformation);
std::cout << "Transformed " << cloud_in->points.size () << " data points:"
<< std::endl;
}else{
if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[1], *cloud_in) < 0 ||
pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[2], *cloud_out) < 0)
{
pcl::console::print_error ("Error loading files!\n");
return (1);
}
}
// Fill in the CloudIn data
// cloud_in->width = 100;
// cloud_in->height = 1;
// cloud_in->is_dense = false;
// cloud_in->points.resize (cloud_in->width * cloud_in->height);
// for (size_t i = 0; i < cloud_in->points.size (); ++i)
// {
// cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
// cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
// cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
// }
std::cout << "size:" << cloud_out->points.size() << std::endl;
{
pcl::ScopeTime("icp proces");
pcl::IterativeClosestPoint<pcl::PointXYZRGBA, pcl::PointXYZRGBA> icp;
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
pcl::PointCloud<pcl::PointXYZRGBA> Final;
icp.setMaximumIterations(1000000);
icp.setRANSACOutlierRejectionThreshold(0.01);
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
//translation, rotation
Eigen::Matrix4f icp_transformation=icp.getFinalTransformation();
Eigen::Matrix3f icp_rotation = icp_transformation.block<3,3>(0,0);
Eigen::Vector3f euler = icp_rotation.eulerAngles(0,1,2);
std::cout << "rotation: " << euler.transpose() << std::endl;
std::cout << "translation:" << icp_transformation.block<3,1>(0,3).transpose() << std::endl;
}
return (0);
}
//extract table clouds, convex hull, and convert to msg stored in DB
bool extract_table_msg(pcl_cloud::Ptr cloud_in, bool is_whole, bool store_cloud=false, bool store_convex=false)
{
pcl_cloud::Ptr cloud1(new pcl_cloud());
pcl_cloud::Ptr cloud_out(new pcl_cloud());
//filter out range that may not be a table plane
pcl::PassThrough<Point> pass;
pass.setFilterFieldName("z");
pass.setFilterLimits(0.45,1.2);
pass.setInputCloud(cloud_in);
pass.filter(*cloud1);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::SACSegmentation<Point> seg;
seg.setOptimizeCoefficients (true);
//plane model
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (cloud1);
seg.segment (*inliers, *coefficients);
if(inliers->indices.size () == 0){
ROS_INFO("No plane detected!");
return false;
}
//form a new cloud from indices
pcl::ExtractIndices<Point> extract;
extract.setInputCloud (cloud1);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_out);
//filter out noise
pcl_cloud::Ptr cloud_nonoise(new pcl_cloud());
outlier_filter_radius(cloud_out,cloud_nonoise);
/*
//normal estimation and filter table plane
pcl::NormalEstimationOMP<Point, pcl::Normal> ne;
ne.setInputCloud (cloud_nonoise);
pcl::search::KdTree<Point>::Ptr tree (new pcl::search::KdTree<Point> ());
ne.setSearchMethod (tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (0.03);
ne.compute (*cloud_normals);
pcl::Normal nor;
float size = 0.0;
for(int i=0;i<cloud_normals->height*cloud_normals->width;i++){
if(isnan(cloud_normals->at(i).normal_x)){
//std::cout<<cloud_normals->at(i).normal_x<<std::endl;
}
else{
nor.normal_x += cloud_normals->at(i).normal_x;
nor.normal_y += cloud_normals->at(i).normal_y;
nor.normal_z += cloud_normals->at(i).normal_z;
size++;
}
}
//normalize instead of average
float normal_length = sqrt(nor.normal_x*nor.normal_x+nor.normal_y*nor.normal_y+nor.normal_z*nor.normal_z);
nor.normal_x = nor.normal_x/normal_length;
nor.normal_y = nor.normal_y/normal_length;
nor.normal_z = nor.normal_z/normal_length;
*/
//instead of computer normal again, using it from plane model
pcl::Normal nor;
nor.normal_x = coefficients->values[0];
nor.normal_y = coefficients->values[1];
nor.normal_z = coefficients->values[2];
if(Debug){
std::cout<<nor.normal_x<<std::endl;
std::cout<<nor.normal_y<<std::endl;
std::cout<<nor.normal_z<<std::endl;
}
//the default viewpoint is (0,0,0),thus using global cloud some plane and viewpoint are in a same plane,
//which may not helpful to flip normal direction
pcl::Normal standard_normal;
standard_normal.normal_x = 0.0;
standard_normal.normal_y = 0.0;
standard_normal.normal_z = 1.0;
float cosin_angle = (standard_normal.normal_z*nor.normal_z);
float tmp_angle = acos(cosin_angle) * (180.0/PI);
//if(tmp_angle>90.0?(tmp_angle-90.0)<normal_angle:tmp_angle<normal_angle){
if(tmp_angle-90.0>0.0){
if((180.0 - tmp_angle)<normal_angle){
ROS_INFO("Normal direction meet requirement %f, angle calculated is: 180.0-%f=%f. ", normal_angle, tmp_angle, 180.0-tmp_angle);
}
else{
ROS_INFO("Normal direction exceed threshold %f, angle calculated is: 180.0-%f=%f. skip.", normal_angle, tmp_angle, 180.0-tmp_angle);
return false;
}
}
else{
if(tmp_angle<normal_angle) {
ROS_INFO("Normal direction meet requirement angle %f, angle calculated is %f", normal_angle, tmp_angle);
}
else{
ROS_INFO("Normal direction exceed threshold %f, angle calculated is: %f. skip.", normal_angle, tmp_angle);
return false;
}
}
pcl_cloud::Ptr cloud_hull(new pcl_cloud());
pcl_cloud::Ptr cloud_cave(new pcl_cloud());
extract_convex(cloud1,inliers,coefficients,cloud_hull,cloud_cave);
//reject some size convex
//store the plane that pass the table filter
if(store_cloud) {
ROS_INFO("Storing table cloud (noise & filted)...");
if (is_whole) {
mongodb_store::MessageStoreProxy table_whole_cloud_noise(*nh, "whole_table_clouds_noise");
mongodb_store::MessageStoreProxy table_whole_cloud(*nh, "whole_table_clouds");
//conversion
sensor_msgs::PointCloud2 whole_table_cloud_noise;
pcl::toROSMsg(*cloud_out, whole_table_cloud_noise);
table_whole_cloud_noise.insert(whole_table_cloud_noise);
sensor_msgs::PointCloud2 whole_table_cloud;
pcl::toROSMsg(*cloud_nonoise, whole_table_cloud);
table_whole_cloud.insert(whole_table_cloud);
}
else {
mongodb_store::MessageStoreProxy dbtable_cloud_noise(*nh, "table_clouds_noise");
mongodb_store::MessageStoreProxy dbtable_cloud(*nh, "table_clouds");
//conversion
sensor_msgs::PointCloud2 table_cloud_noise;
pcl::toROSMsg(*cloud_out, table_cloud_noise);
dbtable_cloud_noise.insert(table_cloud_noise);
sensor_msgs::PointCloud2 table_cloud;
pcl::toROSMsg(*cloud_nonoise, table_cloud);
dbtable_cloud.insert(table_cloud);
//store table centre for each scan
ROS_INFO("Storing table centre...");
Table<Point> tb(nh);
tb.table_cloud_centre(table_cloud);
}
}
ROS_INFO("Convex cloud has been extracted contains %lu points", (*cloud_hull).points.size());
if(store_convex){
ROS_INFO("Storing convex cloud...");
if(is_whole){
mongodb_store::MessageStoreProxy dbtable_whole_convex_cloud(*nh, "whole_table_convex");
mongodb_store::MessageStoreProxy dbtable_whole_concave_cloud(*nh, "whole_table_concave");
//conversion
sensor_msgs::PointCloud2 whole_table_convex;
pcl::toROSMsg(*cloud_hull, whole_table_convex);
dbtable_whole_convex_cloud.insert(whole_table_convex);
sensor_msgs::PointCloud2 whole_table_concave;
pcl::toROSMsg(*cloud_cave, whole_table_concave);
dbtable_whole_concave_cloud.insert(whole_table_concave);
}
else{
mongodb_store::MessageStoreProxy dbtable_convex_cloud(*nh, "table_convex");
mongodb_store::MessageStoreProxy dbtable_concave_cloud(*nh, "table_concave");
//conversion
sensor_msgs::PointCloud2 table_convex;
pcl::toROSMsg(*cloud_hull, table_convex);
dbtable_convex_cloud.insert(table_convex);
sensor_msgs::PointCloud2 table_concave;
pcl::toROSMsg(*cloud_cave, table_concave);
dbtable_concave_cloud.insert(table_concave);
}
}
//construct a table msg
table_detection::Table table_msg;
table_msg.pose.pose.orientation.w = 1.0;
table_msg.header.frame_id = "/map";
table_msg.header.stamp = ros::Time();
for(int i=0;i<(*cloud_hull).points.size();i++){
geometry_msgs::Point32 pp;
pp.x = (*cloud_hull).at(i).x;
pp.y = (*cloud_hull).at(i).y;
pp.z = (*cloud_hull).at(i).z;
table_msg.tabletop.points.push_back(pp);
}
//insert to mongodb
if(is_whole){
mongodb_store::MessageStoreProxy whole_table_shape(*nh, "whole_table_shapes");
whole_table_shape.insert(table_msg);
ROS_INFO("Insert whole table shape to collection.");
}
else{
mongodb_store::MessageStoreProxy table_shape(*nh, "table_shapes");
table_shape.insert(table_msg);
ROS_INFO("Insert table shape to collection.");
}
return true;
}
void
DoSampleRun2 ()
{
cob_3d_mapping_filters::SpeckleFilter<PointXYZ> filter;
pcl::PointCloud<PointXYZ>::Ptr cloud (new pcl::PointCloud<PointXYZ> ());
pcl::PointCloud<PointXYZ>::Ptr cloud_out (new pcl::PointCloud<PointXYZ> ());
cloud->width = 640;
cloud->height = 480;
double x = 0, y = 0;
for (unsigned int i = 0; i < cloud->width; i++, y += 0.001)
{
x = 0;
for (unsigned int j = 0; j < cloud->height; j++, x += 0.001)
{
PointXYZ pt;
pt.x = x;
pt.y = y;
pt.z = 1;
cloud->points.push_back (pt);
}
}
boost::mt19937 rng; // I don't seed it on purpouse (it's not relevant)
boost::normal_distribution<> nd (0.0, 0.05);
boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
for (unsigned int i = 0; i < 3000; i++)
cloud->points[i * 100].z += var_nor ();
//pcl::io::savePCDFileASCII("/tmp/spk_cloud.pcd", *cloud);
filter.setInputCloud (cloud);
for (unsigned int s = 10; s <= 100; s += 10)
{
std::stringstream ss;
ss << "/tmp/spk_acc_" << s << ".txt";
std::ofstream file;
file.open (ss.str ().c_str ());
file << "thr\ttp\tfn\tfp\n";
for (double c = 0.01; c <= 0.1; c += 0.01)
{
filter.setFilterParam (s, c);
filter.filter (*cloud_out);
pcl::PointIndices::Ptr ind = filter.getRemovedIndices ();
std::cout << "Cloud size " << cloud_out->size () << ", ind: " << ind->indices.size () << std::endl;
int fn_ctr = 0, tp_ctr = 0;
for (unsigned int i = 0; i < 3000; i++)
{
bool found = false;
for (unsigned int j = 0; j < ind->indices.size (); j++)
{
if (ind->indices[j] == i * 100)
{
tp_ctr++;
found = true;
break;
}
}
if (!found)
fn_ctr++;
}
int fp_ctr = ind->indices.size () - tp_ctr;
double fn_ratio = (double)fn_ctr / 3000;
double fp_ratio = (double)fp_ctr / 3000;
double tp_ratio = (double)tp_ctr / 3000;
file << c << "\t" << tp_ratio << "\t" << fn_ratio << "\t" << fp_ratio << "\n";
std::cout << "c: " << c << " fn: " << fn_ratio << ", tp: " << tp_ratio << " fp: " << fp_ratio << std::endl;
}
file.close ();
}
}
void CloudAssembler::process(const sensor_msgs::PointCloud2::ConstPtr &cloud)
{
ROS_INFO_STREAM_ONCE("CloudAssembler::process(): Point cloud received");
geometry_msgs::PoseStamped p;
if (!getRobotPose(cloud->header.stamp, p)) return;
bool update = false;
double dist = sqrt(pow(robot_pose_.pose.position.x - p.pose.position.x, 2) + pow(robot_pose_.pose.position.y - p.pose.position.y, 2));
if (dist > dist_th_)
{
robot_pose_ = p;
if (dist > max_dist_th_)
{
cloud_buff_->clear();
}
else update = true;
}
VPointCloud vpcl;
TPointCloudPtr tpcl(new TPointCloud());
// Retrieve the input point cloud
pcl::fromROSMsg(*cloud, vpcl);
pcl::copyPointCloud(vpcl, *tpcl);
pcl::PassThrough< TPoint > pass;
pass.setInputCloud(tpcl);
pass.setFilterFieldName("x");
pass.setFilterLimits(min_x_, max_x_);
pass.filter(*tpcl);
pass.setInputCloud(tpcl);
pass.setFilterFieldName("y");
pass.setFilterLimits(min_y_, max_y_);
pass.filter(*tpcl);
pass.setInputCloud(tpcl);
pass.setFilterFieldName("z");
pass.setFilterLimits(min_z_, max_z_);
pass.filter(*tpcl);
pcl::ApproximateVoxelGrid<TPoint> psor;
psor.setInputCloud(tpcl);
psor.setDownsampleAllData(false);
psor.setLeafSize(filter_cloud_res_, filter_cloud_res_, filter_cloud_res_);
psor.filter(*tpcl);
pcl::StatisticalOutlierRemoval< TPoint > foutl;
foutl.setInputCloud(tpcl);
foutl.setMeanK(filter_cloud_k_);
foutl.setStddevMulThresh(filter_cloud_th_);
foutl.filter(*tpcl);
pcl_ros::transformPointCloud("odom", *tpcl, *tpcl, listener_);
// get accumulated cloud
TPointCloudPtr pcl_out(new TPointCloud());
for (unsigned int i = 0; i < cloud_buff_->size(); i++)
{
*pcl_out += cloud_buff_->at(i);
}
// registration
if (cloud_buff_->size() > 0)
{
pcl::IterativeClosestPoint< TPoint, TPoint> icp;
icp.setInputCloud(tpcl);
icp.setInputTarget(pcl_out);
pcl::PointCloud<TPoint> aligned;
icp.align(aligned);
if (icp.hasConverged())
{
*tpcl = aligned;
std::cout << "ICP score: " << icp.getFitnessScore() << std::endl;
}
}
if (update) cloud_buff_->push_back(*tpcl);
if (points_pub_.getNumSubscribers() == 0)
{
return;
}
*pcl_out += *tpcl;
pcl::ApproximateVoxelGrid<TPoint> sor;
sor.setInputCloud(pcl_out);
sor.setDownsampleAllData(false);
sor.setLeafSize(filter_cloud_res_, filter_cloud_res_, filter_cloud_res_);
TPointCloudPtr pcl_filt(new TPointCloud());
sor.filter(*pcl_filt);
sensor_msgs::PointCloud2::Ptr cloud_out(new sensor_msgs::PointCloud2());
pcl::toROSMsg(*pcl_filt, *cloud_out);
//std::cout << "points: " << pcl_out->points.size() << std::endl;
cloud_out->header.stamp = cloud->header.stamp;
cloud_out->header.frame_id = fixed_frame_;
points_pub_.publish(cloud_out);
}
int PCLWrapper::test_registration(){
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(
new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(
new pcl::PointCloud<pcl::PointXYZ>);
char buf[1024];
sprintf(buf, "Testing Registration\n");
__android_log_write(ANDROID_LOG_INFO, "PCL FILTER TESTING:", buf);
// Fill in the CloudIn data
cloud_in->width = 5;
cloud_in->height = 1;
cloud_in->is_dense = false;
cloud_in->points.resize(cloud_in->width * cloud_in->height);
for (size_t i = 0; i < cloud_in->points.size(); ++i) {
cloud_in->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
cloud_in->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
cloud_in->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
}
std::cout << "Saved " << cloud_in->points.size() << " data points to input:"
<< std::endl;
for (size_t i = 0; i < cloud_in->points.size(); ++i)
std::cout << " " << cloud_in->points[i].x << " "
<< cloud_in->points[i].y << " " << cloud_in->points[i].z
<< std::endl;
*cloud_out = *cloud_in;
std::cout << "size:" << cloud_out->points.size() << std::endl;
for (size_t i = 0; i < cloud_in->points.size(); ++i)
cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;
std::cout << "Transformed " << cloud_in->points.size() << " data points:"
<< std::endl;
sprintf(buf, "Transformed %d\n", cloud_in->points.size());
__android_log_write(ANDROID_LOG_INFO, "PCL FILTER TESTING:", buf);
for (size_t i = 0; i < cloud_out->points.size(); ++i)
std::cout << " " << cloud_out->points[i].x << " "
<< cloud_out->points[i].y << " " << cloud_out->points[i].z
<< std::endl;
pcl::IterativeClosestPoint < pcl::PointXYZ, pcl::PointXYZ > icp;
icp.setInputCloud(cloud_in);
icp.setInputTarget(cloud_out);
pcl::PointCloud < pcl::PointXYZ > Final;
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: "
<< icp.getFitnessScore() << std::endl;
sprintf(buf, "has converged: %d, score: %lf\n ", icp.hasConverged(), icp.getFitnessScore());
__android_log_write(ANDROID_LOG_INFO, "PCL FILTER TESTING:", buf);
std::cout << icp.getFinalTransformation() << std::endl;
// sprintf(buf, "Final Transformation: %s\n ", icp.getFinalTransformation());
// __android_log_write(ANDROID_LOG_INFO, "PCL FILTER TESTING:", buf);
}
int
main (int argc, char** argv)
{
// Data containers used
pcl::PointCloud<PointTypeIO>::Ptr cloud_in (new pcl::PointCloud<PointTypeIO>), cloud_out (new pcl::PointCloud<PointTypeIO>);
pcl::PointCloud<PointTypeFull>::Ptr cloud_with_normals (new pcl::PointCloud<PointTypeFull>);
pcl::IndicesClustersPtr clusters (new pcl::IndicesClusters), small_clusters (new pcl::IndicesClusters), large_clusters (new pcl::IndicesClusters);
pcl::search::KdTree<PointTypeIO>::Ptr search_tree (new pcl::search::KdTree<PointTypeIO>);
pcl::console::TicToc tt;
// Load the input point cloud
std::cerr << "Loading...\n", tt.tic ();
pcl::io::loadPCDFile (argv[1], *cloud_in);
std::cerr << ">> Done: " << tt.toc () << " ms, " << cloud_in->points.size () << " points\n";
// Downsample the cloud using a Voxel Grid class
std::cerr << "Downsampling...\n", tt.tic ();
pcl::VoxelGrid<PointTypeIO> vg;
vg.setInputCloud (cloud_in);
vg.setLeafSize (80.0, 80.0, 80.0);
vg.setDownsampleAllData (true);
vg.filter (*cloud_out);
std::cerr << ">> Done: " << tt.toc () << " ms, " << cloud_out->points.size () << " points\n";
// Set up a Normal Estimation class and merge data in cloud_with_normals
std::cerr << "Computing normals...\n", tt.tic ();
pcl::copyPointCloud (*cloud_out, *cloud_with_normals);
pcl::NormalEstimation<PointTypeIO, PointTypeFull> ne;
ne.setInputCloud (cloud_out);
ne.setSearchMethod (search_tree);
ne.setRadiusSearch (300.0);
ne.compute (*cloud_with_normals);
std::cerr << ">> Done: " << tt.toc () << " ms\n";
// Set up a Conditional Euclidean Clustering class
std::cerr << "Segmenting to clusters...\n", tt.tic ();
pcl::ConditionalEuclideanClustering<PointTypeFull> cec (true);
cec.setInputCloud (cloud_with_normals);
cec.setConditionFunction (&customRegionGrowing);
cec.setClusterTolerance (500.0);
cec.setMinClusterSize (cloud_with_normals->points.size () / 1000);
cec.setMaxClusterSize (cloud_with_normals->points.size () / 5);
cec.segment (*clusters);
cec.getRemovedClusters (small_clusters, large_clusters);
std::cerr << ">> Done: " << tt.toc () << " ms\n";
// Using the intensity channel for lazy visualization of the output
for (int i = 0; i < small_clusters->size (); ++i)
for (int j = 0; j < (*small_clusters)[i].indices.size (); ++j)
cloud_out->points[(*small_clusters)[i].indices[j]].intensity = -2.0;
for (int i = 0; i < large_clusters->size (); ++i)
for (int j = 0; j < (*large_clusters)[i].indices.size (); ++j)
cloud_out->points[(*large_clusters)[i].indices[j]].intensity = +10.0;
for (int i = 0; i < clusters->size (); ++i)
{
int label = rand () % 8;
for (int j = 0; j < (*clusters)[i].indices.size (); ++j)
cloud_out->points[(*clusters)[i].indices[j]].intensity = label;
}
// Save the output point cloud
std::cerr << "Saving...\n", tt.tic ();
pcl::io::savePCDFile ("output.pcd", *cloud_out);
std::cerr << ">> Done: " << tt.toc () << " ms\n";
return (0);
}
void PlaneFinder::pcCallback(const sensor_msgs::PointCloud2::ConstPtr & pc)
{
pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2);
pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered1 (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered2 (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered3 (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ());
sensor_msgs::PointCloud2 pc2;
double height = -0.5;
// Transformation into PCL type PointCloud2
pcl_conversions::toPCL(*(pc), *(pcl_pc));
//////////////////
// Voxel filter //
//////////////////
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (pcl_pc);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered);
// Transformation into ROS type
//pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out));
//pcl_conversions::moveFromPCL(*(cloud_filtered), pc2);
//debug2_pub.publish(pc2);
// Transformation into PCL type PointCloud<pcl::PointXYZRGB>
pcl::fromPCLPointCloud2(*(cloud_filtered), *(cloud_filtered1));
if(pcl_ros::transformPointCloud("map", *(cloud_filtered1), *(cloud_filtered2), tf_))
{
////////////////////////
// PassThrough filter //
////////////////////////
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud (cloud_filtered2);
pass.setFilterFieldName ("x");
pass.setFilterLimits (-0.003, 0.9);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered2);
pass.setInputCloud (cloud_filtered2);
pass.setFilterFieldName ("y");
pass.setFilterLimits (-0.5, 0.5);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered2);
/////////////////////////
// Planar segmentation //
/////////////////////////
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
//seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
int i = 0, nr_points = (int) cloud_filtered2->points.size ();
// While 50% of the original cloud is still there
while (cloud_filtered2->points.size () > 0.5 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered2);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
if( (fabs(coefficients->values[0]) < 0.02) &&
(fabs(coefficients->values[1]) < 0.02) &&
(fabs(coefficients->values[2]) > 0.9) )
{
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
height = coefficients->values[3];
// Extract the inliers
extract.setInputCloud (cloud_filtered2);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_filtered3);
// Transformation into ROS type
//pcl::toPCLPointCloud2(*(cloud_filtered3), *(cloud_out));
//pcl_conversions::moveFromPCL(*(cloud_out), pc2);
//debug_pub.publish(pc2);
}
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud_f);
cloud_filtered2.swap (cloud_f);
i++;
}
/*
pass.setInputCloud (cloud_filtered2);
pass.setFilterFieldName ("z");
pass.setFilterLimits (height, 1.5);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered2);
*/
// Transformation into ROS type
//pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out));
//pcl_conversions::moveFromPCL(*(cloud_out), pc2);
//debug_pub.publish(pc2);
/////////////////////////////////
// Statistical Outlier Removal //
/////////////////////////////////
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud_filtered2);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*cloud_filtered2);
//////////////////////////////////
// Euclidian Cluster Extraction //
//////////////////////////////////
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud (cloud_filtered2);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (50);
ec.setMaxClusterSize (5000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered2);
ec.extract (cluster_indices);
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster1 (new pcl::PointCloud<pcl::PointXYZRGB>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster1->points.push_back (cloud_filtered2->points[*pit]);
cloud_cluster1->width = cloud_cluster1->points.size ();
cloud_cluster1->height = 1;
cloud_cluster1->is_dense = true;
cloud_cluster1->header.frame_id = "/map";
if(j == 0) {
// Transformation into ROS type
pcl::toPCLPointCloud2(*(cloud_cluster1), *(cloud_out));
pcl_conversions::moveFromPCL(*(cloud_out), pc2);
debug_pub.publish(pc2);
}
else {
// Transformation into ROS type
pcl::toPCLPointCloud2(*(cloud_cluster1), *(cloud_out));
pcl_conversions::moveFromPCL(*(cloud_out), pc2);
debug2_pub.publish(pc2);
}
j++;
}
// Transformation into ROS type
//pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out));
//pcl_conversions::moveFromPCL(*(cloud_out), pc2);
//debug2_pub.publish(pc2);
//debug2_pub.publish(*pc_map);
}
}
int
main (int argc, char** argv)
{
CloudPtr cloud_in (new Cloud);
CloudPtr cloud_out (new Cloud);
pcl::ScopeTime time("performance");
float endTime;
pcl::io::loadPCDFile (argv[1], *cloud_in);
std::cout << "Input size: " << cloud_in->width << " by " << cloud_in->height << std::endl;
///////////////////////////////////////////////////////////////////////////////////////////
//
pcl::PassThrough<PointType> pass;
pass.setInputCloud (cloud_in);
pass.setFilterLimitsNegative(1);
//pass.setKeepOrganized(true);
pass.setFilterFieldName ("x");
pass.setFilterLimits (1300, 2200.0);
pass.filter (*cloud_out);
pass.setInputCloud(cloud_out);
pass.setFilterFieldName ("y");
pass.setFilterLimits (8000, 10000);
pass.filter (*cloud_out);
//
pass.setFilterFieldName ("z");
pass.setFilterLimits (-2000, -200);
pass.filter (*cloud_out);
pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZI> octree (10.0);
// Add points from cloudA to octree
octree.setInputCloud (cloud_out);
octree.addPointsFromInputCloud ();
// Switch octree buffers: This resets octree but keeps previous tree structure in memory.
octree.switchBuffers ();
// Add points from cloudB to octree
octree.setInputCloud (cloud_in);
octree.addPointsFromInputCloud ();
std::vector<int> newPointIdxVector;
// Get vector of point indices from octree voxels which did not exist in previous buffer
octree.getPointIndicesFromNewVoxels (newPointIdxVector);
//
///////////////////////////////////////////////////////////////////////////////////////////
//
// std::vector<int> unused;
// pcl::removeNaNFromPointCloud (*cloud_in, *cloud_in, unused);
//
///////////////////////////////////////////////////////////////////////////////////////////
//
// pcl::search::Search<PointType>::Ptr searcher (new pcl::search::KdTree<PointType> (false));
// searcher->setInputCloud (cloud_in);
// PointType point;
// point.x = -10000;
// point.y = 0;
// point.z = 0;
// std::vector<int> k_indices;
// std::vector<float> unused2;
// //searcher->radiusSearch (point, 100, k_indices, unused2);
// searcher->nearestKSearch (point, 500000, k_indices, unused2);
// cloud_out->width = k_indices.size ();
// cloud_out->height = 1;
// cloud_out->is_dense = false;
// cloud_out->points.resize (cloud_out->width);
// for (size_t i = 0; i < cloud_out->points.size (); ++i)
// {
// cloud_out->points[i] = cloud_in->points[k_indices[i]];
// }
//
///////////////////////////////////////////////////////////////////////////////////////////
//
// cloud_out->width = cloud_in->width / 10;
// cloud_out->height = 1;
// cloud_out->is_dense = false;
// cloud_out->points.resize (cloud_out->width);
// for (size_t i = 0; i < cloud_out->points.size (); ++i)
// {
// cloud_out->points[i] = cloud_in->points[i * 10];
// }
//
///////////////////////////////////////////////////////////////////////////////////////////
// pcl::BilateralFilter<PointType> filter;
// filter.setInputCloud (cloud_in);
// pcl::octree::OctreeLeafDataTVector<int> leafT;
// pcl::search::Search<PointType>::Ptr searcher (new pcl::search::Octree
// < PointType,
// pcl::octree::OctreeLeafDataTVector<int> ,
// pcl::octree::OctreeBase<int, pcl::octree::OctreeLeafDataTVector<int> >
// > (500) );
// //pcl::search::Octree <PointType> ocTreeSearch(1);
// filter.setSearchMethod (searcher);
// double sigma_s, sigma_r;
//
// filter.setHalfSize (500);
// time.reset();
// filter.filter (*cloud_out);
// time.getTime();
// std::cout << "Benchmark Bilateral filer using: kdtree searching, sigma_s = 50, sigma_r = default" << std::endl;
// std::cout << "Results: clocks = " << end - start << ", clockspersec = " << CLOCKS_PER_SEC << std::endl;
// std::ofstream filestream;
// filestream.open ("timer_results.txt");
// char filename[50];
// for (sigma_s = 1000; sigma_s <= 1000; sigma_s *= 10)
// for (sigma_r = 0.001; sigma_r <= 1000; sigma_r *= 10)
// {
// std::cout << "sigma_s = " << sigma_s << ", sigma_r = " << sigma_r << " clockspersec = " << CLOCKS_PER_SEC
// << std::endl;
// filter.setHalfSize (sigma_s);
// filter.setStdDev (sigma_r);
// start = clock ();
// filter.filter (*cloud_out);
// end = clock ();
// sprintf (filename, "bruteforce-s%g-r%g.pcd", sigma_s, sigma_r);
// pcl::io::savePCDFileBinary (filename, *cloud_out);
// filestream << "sigma_s = " << sigma_s << ", sigma_r = " << sigma_r << " time = " << end - start
// << " clockspersec = " << CLOCKS_PER_SEC << std::endl;
// }
// filestream.close ();
// std::cout << "Output size: " << cloud_out->width << " by " << cloud_out->height << std::endl;
CloudPtr cloud_n (new Cloud);
CloudPtr cloud_buff (new Cloud);
pcl::io::loadPCDFile ("only_leaves.pcd", *cloud_n);
pcl::copyPointCloud(*cloud_in, newPointIdxVector, *cloud_buff);
// pcl::io::savePCDFileBinary<pcl::PointXYZI>("delt.pcd",*cloud_buff);
cloud_buff->operator+=(*cloud_n);
pcl::io::savePCDFileBinary<pcl::PointXYZI>("cropped_cloud.pcd",*cloud_buff);
std::cout << std::endl << "Goodbye World" << std::endl << std::endl;
return (0);
}
