//--------------------------------------------------------------------------------
int main(int argc, char** argv)
{
if(argc < 2)
{
PCL_ERROR("Syntax is: %s <source.ply>", argv[0]);
return EXIT_FAILURE;
}
// load pointcloud from file
std::string fname = std::string(argv[1]);
if (!checkFilename(fname))
{
PCL_ERROR("Given file is not a valid .ply file: ", argv[1]);
return EXIT_FAILURE;
}
std::cout << "loading " << fname << std::endl;
pcl::PointCloud<PointT>::Ptr cloud_in(new pcl::PointCloud<PointT>);
pcl::io::loadPLYFile(fname, *cloud_in);
// moving least squares
pcl::PointCloud<PointT>::Ptr cloud_mls = runMLS(cloud_in);
// save output
if (!cloud_mls->empty())
{
pcl::io::savePLYFileBinary("cloud_xyzrgb_mls.ply", *cloud_mls);
}
else
{
PCL_ERROR("MLS result pointcloud is empty.");
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
int
main (int argc, char** argv)
{
if (argc < 3){
std::cerr << "Usage: " << argv[0] << " [PCD 1] [PCD 2]" << std::endl;
return 1;
}
double maxCorrespondenceDistance = 0.05;
int maxIterations = 50;
double epsilon = 1e-8;
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::PCDReader reader;
reader.read(argv[1], *cloud_in);
reader.read(argv[2], *cloud_out);
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputCloud(cloud_in);
icp.setInputTarget(cloud_out);
icp.setMaxCorrespondenceDistance(maxCorrespondenceDistance);
icp.setMaximumIterations(maxIterations);
icp.setTransformationEpsilon(epsilon);
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)
{
if(argc < 2)
{
PCL_ERROR("Syntax is: %s <source.ply>", argv[0]);
return EXIT_FAILURE;
}
// load pointcloud from file
std::string fname = std::string(argv[1]);
if (!checkFilename(fname))
{
PCL_ERROR("Given file is not a valid .ply file: ", argv[1]);
return EXIT_FAILURE;
}
std::cout << "loading " << fname << std::endl;
pcl::PointCloud<PointNormalT>::Ptr cloud_in(new pcl::PointCloud<PointNormalT>);
pcl::io::loadPLYFile(fname, *cloud_in);
// statistical outlier removal
pcl::PointCloud<PointNormalT>::Ptr cloud_sor = filterSOR(cloud_in);
// save output
if (!cloud_sor->empty())
{
pcl::io::savePLYFileBinary("cloud_sor.ply", *cloud_sor);
}
else
{
PCL_ERROR("SOR filtered pointcloud is empty.");
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
int main(int argc,char** argv){
// Initialize ROS
ros::init (argc, argv, "test_unionfind");
ros::NodeHandle* nh = new ros::NodeHandle("~");
ros::Publisher pub = nh->advertise<sensor_msgs::PointCloud2> ("inliers", 1);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_in( new pcl::PointCloud<pcl::PointXYZRGBA>(10,10));
std::vector<int> inliers;
for(int col = 0 ; col < 10 ; col ++){
for (int row =0 ; row <10; row ++){
cloud_in->at(col,row).x = col;
cloud_in->at(col,row).y = row;
}
}
//first group
cloud_in->at(2,1).a = 1;
cloud_in->at(2,2).a = 1;
cloud_in->at(2,0).a = 1;
cloud_in->at(1,1).a = 1;
cloud_in->at(3,1).a = 1;
//second group
cloud_in->at(5,4).a = 1;
cloud_in->at(6,4).a = 1;
cloud_in->at(6,5).a = 1;
//third group
cloud_in->at(4,7).a = 1;
cloud_in->at(5,6).a = 1;
//cloud_in->at(5,7).a = 1;
cloud_in->at(6,6).a = 1;
union_find(cloud_in,inliers);
sensor_msgs::PointCloud2 msg_cloud_inliers;
pcl::PointCloud<pcl::PointXYZRGBA> cloud_inliers;
//for (size_t i = 0; i < inliers->size (); ++i)
// cloud_inliers.insert(cloud_inliers.end(),cloud->points[(*inliers)[i]]);
for (size_t i = 0; i < inliers.size (); ++i)
cloud_inliers.insert(cloud_inliers.end(),cloud_in->points[inliers[i]]);
// convert and Publish the data
//pcl::toROSMsg(cloud_inliers,msg_cloud_inliers);
pcl::toROSMsg(cloud_inliers,msg_cloud_inliers);
//pcl::toROSMsg(*line1_cloud,msg_cloud_inliers);
//modify the frame id
msg_cloud_inliers.header.frame_id="/world";
while(nh->ok()){
pub.publish (msg_cloud_inliers);
}
return 0;
}
//extract table and construct a table_detection table msg, store it in DB call this service and pass in a point cloud
bool extract2(table_detection::db_table::Request &req, table_detection::db_table::Response &res)
{
//copy cloud
pcl::PointCloud<Point>::Ptr cloud_in(new pcl::PointCloud<Point>);
pcl::fromROSMsg(req.cloud,*cloud_in);
//pass it to table extraction
bool rc = extract_table_msg(cloud_in, false, true, true);
return rc;
}
/** Saves the two point-clouds as .pcd files and the transform in a text-file.**/
bool saveData (pcd_service::PCDData::Request &req,
pcd_service::PCDData::Response &res) {
// Do ROS --> PCL conversions:
pcl::PCLPointCloud2 cloud_in_PCLP2;
RGBCloud::Ptr cloud_in(new RGBCloud);
pcl_conversions::toPCL(req.pc, cloud_in_PCLP2);
pcl::fromPCLPointCloud2(cloud_in_PCLP2, *cloud_in);
// write the point-clouds and transform data to files:
pcl::io::savePCDFileASCII (req.fname, *cloud_in);
return true;
}
/** 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);
}
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);
}
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);
}
/***********
* MAIN
**********/
int main(int argc, char **argv)
{
/////////////////////////////////////////////////////////
// mask definition for analysis of neighborhood of voxels UFO
mask = create_mask(MASKSIZE);
mask_ground = create_ground_mask_Z(MASKSIZE);
/////////////////////////////////////////////////////////
// 3D Viewing
if (debug & DEBUG_3D)
{
my_view_3D = new CView_3D();
my_view_3D->show_axis();
my_view_3D->initialize_ball_actors();
if (debug & DEBUG_MASK)
my_view_3D->initialize_mask_actors(mask,MASKSIZE,gridsize);
if (debug & DEBUG_TRAJECTORY)
my_view_3D->initialize_traj_actors(TRAJECTORY_SAMPLE);
}
my_trajectory = new CTrajectory();
///////////////////////////////////
// for pointcloud reading from disk
int image_number = 0;
//int first_image = 0;
int first_image = 0;
//int last_image = 126;
int last_image = 17;
char filename[200];
// debug
if (argc == 2)
{
debug = atoi(argv[1]);
}
if (ros_com==0)
{
// update fisrt and last image from args
if (argc == 3)
{
first_image = atoi(argv[1]);
last_image = atoi(argv[2]);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
for (image_number = first_image; image_number < last_image; image_number++)
{
//////////////////////
// Read point cloud
//sprintf(filename,"/home/paulo/rosbag/at_home_15032013/cloud/kinect_cloud_%d.pcd",image_number);
sprintf(filename,"/home/paulo/rosbag/ball_arm/cloud/kinect_cloud_%d.pcd",image_number);
//printf("\n reading %s",filename);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud_in) == -1) //* load the file
{
printf ("Couldn't read file %s\n",filename);
getchar();
return (-1);
}
Callback(cloud_in);
}
}
// for pointcloud reading from disk
///////////////////////////////////
//////////////////////////////////
// for ROS Operation
else
{
ros::init(argc, argv, "camera_depth_optical_frame");
listener = new (tf::TransformListener);
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/camera/depth/points", 1, Callback);
pub = n.advertise<geometry_msgs::PointStamped>("/ball3D", 10);
ros::spin();
}
//for ROS Operation
//////////////////////////////////
printf("\n---------------------------------");
printf("\n- General Report");
printf("\n Air ball detected: %d",air_detect);
printf("\n Ground ball detected: %d",ground_detect);
printf("\n Processed images: %d",im_num);
printf("\n---------------------------------\n");
if (debug & DEBUG_3D)
my_view_3D->start_interaction();
// HouseKeeping
delete(mask);
delete(mask_ground);
return 0;
}
int
main (int argc, char** argv)
{
if (argc < 2)
{
std::cerr << "Input argument 1: a .pcd file to compare against benchmark-target.pcd" << std::endl;
std::cerr << "Input argument 2 (optional): the octree resolution to use (default = 1.0)" << std::endl;
return 0;
}
float resolution;
if (argc > 2)
resolution = atof (argv[2]);
else
resolution = 1.0;
std::cerr << "Using octree resolution: " << resolution << std::endl;
std::cerr << std::endl << "Computing.";
CloudPtr cloud_target (new Cloud);
pcl::io::loadPCDFile ("theatre_benchmark_target.pcd", *cloud_target);
std::cerr << ".";
CloudPtr cloud_in (new Cloud);
pcl::io::loadPCDFile (argv[1], *cloud_in);
std::cerr << ".";
// Instantiate octree-based point cloud change detection classes
pcl::octree::OctreePointCloudChangeDetector<PointType> octree_forward (resolution);
octree_forward.setInputCloud (cloud_target);
octree_forward.addPointsFromInputCloud ();
octree_forward.switchBuffers ();
octree_forward.setInputCloud (cloud_in);
octree_forward.addPointsFromInputCloud ();
std::cerr << ".";
pcl::octree::OctreePointCloudChangeDetector<PointType> octree_backward (resolution);
octree_backward.setInputCloud (cloud_in);
octree_backward.addPointsFromInputCloud ();
octree_backward.switchBuffers ();
octree_backward.setInputCloud (cloud_target);
octree_backward.addPointsFromInputCloud ();
std::cerr << ".";
// Get vector of point indices from octree voxels that do exist in second one but do not exist in first one
std::vector<int> differences_forward;
octree_forward.getPointIndicesFromNewVoxels (differences_forward);
std::vector<int> differences_backward;
octree_backward.getPointIndicesFromNewVoxels (differences_backward);
std::cerr << ".";
int noise_present = 0, noise_added = 0;
for (size_t i = 0; i < differences_forward.size (); ++i)
{
if (checkPointInRange (cloud_in->points[differences_forward[i]], 500, 2000, 5000, 10000, -1825, 500))
noise_present++;
else if (checkPointInRange (cloud_in->points[differences_forward[i]], 0, 3000, 0, 1200, -1500, 2500))
noise_present++;
else if (checkPointInRange (cloud_in->points[differences_forward[i]], -3500, 3000, 8300, 10000, -1800, 6000))
noise_present++;
else if (checkPointInRange (cloud_in->points[differences_forward[i]], -3500, 3000, 0, 8300, 300, 6000))
noise_present++;
else
noise_added++;
}
if (noise_present > 424122)
{
noise_added += noise_present - 424122;
noise_present = 424122;
}
// Results
std::cerr << std::endl << std::endl << "Target cloud: " << cloud_target->width << " x " << cloud_target->height << std::endl;
std::cerr << "Input cloud: " << cloud_in->width << " x " << cloud_in->height << std::endl;
std::cerr << "Difference: " << abs (cloud_target->points.size () - cloud_in->points.size ()) << " points" << std::endl;
if (resolution <= 1.0)
{
std::cerr << std::endl << "Noise that was removed: " << 424122 - noise_present << " points" << std::endl;
std::cerr << "Noise that was not removed: " << noise_present << " points" << std::endl;
std::cerr << "Noise that was added: " << noise_added << " points" << std::endl;
std::cerr << "Non-noise that was removed: " << differences_backward.size () << " points" << std::endl;
std::cerr << std::endl << "Benchmark error result: " << noise_present / 424122 + 10 * noise_added / 4002050 + 10 * differences_backward.size () / 4002050;
std::cerr << " (0 is perfect, 1 is useless (can be higher than 1))" << std::endl;
}
return 0;
}
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);
}
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);
}
/*
* Receive callback for the /camera/depth_registered/points subscription
*/
std::vector<suturo_perception_msgs::PerceivedObject> SuturoPerceptionKnowledgeROSNode::receive_image_and_cloud(const sensor_msgs::ImageConstPtr& inputImage, const sensor_msgs::PointCloud2ConstPtr& inputCloud)
{
// process only one cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::fromROSMsg(*inputCloud,*cloud_in);
logger.logInfo((boost::format("Received a new point cloud: size = %s") % cloud_in->points.size()).str());
// Gazebo sends us unorganized pointclouds!
// Reorganize them to be able to compute the ROI of the objects
// This workaround is only tested for gazebo 1.9!
if(!cloud_in->isOrganized ())
{
logger.logInfo((boost::format("Received an unorganized PointCloud: %d x %d .Convert it to a organized one ...") % cloud_in->width % cloud_in->height ).str());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr org_cloud (new pcl::PointCloud<pcl::PointXYZRGB>());
org_cloud->width = 640;
org_cloud->height = 480;
org_cloud->is_dense = false;
org_cloud->points.resize(640 * 480);
for (int i = 0; i < cloud_in->points.size(); i++) {
pcl::PointXYZRGB result;
result.x = 0;
result.y = 0;
result.z = 0;
org_cloud->points[i]=cloud_in->points[i];
}
cloud_in = org_cloud;
}
cv_bridge::CvImagePtr cv_ptr;
cv_ptr = cv_bridge::toCvCopy(inputImage, enc::BGR8);
// Make a deep copy of the passed cv::Mat and set a new
// boost pointer to it.
boost::shared_ptr<cv::Mat> img(new cv::Mat(cv_ptr->image.clone()));
sp.setOriginalRGBImage(img);
logger.logInfo("processing...");
sp.setOriginalCloud(cloud_in);
sp.processCloudWithProjections(cloud_in);
logger.logInfo("Cloud processed. Lock buffer and return the results");
mutex.lock();
perceivedObjects = sp.getPerceivedObjects();
if(sp.getOriginalRGBImage()->cols != sp.getOriginalCloud()->width
&& sp.getOriginalRGBImage()->rows != sp.getOriginalCloud()->height)
{
// Adjust the ROI if the image is at 1280x1024 and the pointcloud is at 640x480
if(sp.getOriginalRGBImage()->cols == 1280 && sp.getOriginalRGBImage()->rows == 1024)
{
for (int i = 0; i < perceivedObjects.size(); i++) {
ROI roi = perceivedObjects.at(i).get_c_roi();
roi.origin.x*=2;
roi.origin.y*=2;
roi.width*=2;
roi.height*=2;
perceivedObjects.at(i).set_c_roi(roi);
}
}
else
{
logger.logError("UNSUPPORTED MIXTURE OF IMAGE AND POINTCLOUD DIMENSIONS");
}
}
// Execution pipeline
// Each capability provides an enrichment for the
// returned PerceivedObject
// initialize threadpool
boost::asio::io_service ioService;
boost::thread_group threadpool;
std::auto_ptr<boost::asio::io_service::work> work(new boost::asio::io_service::work(ioService));
// Add worker threads to threadpool
for(int i = 0; i < numThreads; ++i)
{
threadpool.create_thread(
boost::bind(&boost::asio::io_service::run, &ioService)
);
}
for (int i = 0; i < perceivedObjects.size(); i++)
{
// Initialize Capabilities
ColorAnalysis ca(perceivedObjects[i]);
ca.setLowerSThreshold(color_analysis_lower_s);
ca.setUpperSThreshold(color_analysis_upper_s);
ca.setLowerVThreshold(color_analysis_lower_v);
ca.setUpperVThreshold(color_analysis_upper_v);
suturo_perception_shape_detection::RandomSampleConsensus sd(perceivedObjects[i]);
//suturo_perception_vfh_estimation::VFHEstimation vfhe(perceivedObjects[i]);
// suturo_perception_3d_capabilities::CuboidMatcherAnnotator cma(perceivedObjects[i]);
// Init the cuboid matcher with the table coefficients
suturo_perception_3d_capabilities::CuboidMatcherAnnotator cma(perceivedObjects[i], sp.getTableCoefficients() );
// post work to threadpool
ioService.post(boost::bind(&ColorAnalysis::execute, ca));
ioService.post(boost::bind(&suturo_perception_shape_detection::RandomSampleConsensus::execute, sd));
//ioService.post(boost::bind(&suturo_perception_vfh_estimation::VFHEstimation::execute, vfhe));
ioService.post(boost::bind(&suturo_perception_3d_capabilities::CuboidMatcherAnnotator::execute, cma));
// Is 2d recognition enabled?
if(!recognitionDir.empty())
{
// perceivedObjects[i].c_recognition_label_2d="";
suturo_perception_2d_capabilities::LabelAnnotator2D la(perceivedObjects[i], sp.getOriginalRGBImage(), object_matcher_);
la.execute();
}
else
{
// Set an empty label
perceivedObjects[i].set_c_recognition_label_2d("");
}
}
//boost::this_thread::sleep(boost::posix_time::microseconds(1000));
// wait for thread completion.
// destroy the work object to wait for all queued tasks to finish
work.reset();
ioService.run();
threadpool.join_all();
std::vector<suturo_perception_msgs::PerceivedObject> perceivedObjs = *convertPerceivedObjects(&perceivedObjects); // TODO handle images in this method
mutex.unlock();
return perceivedObjs;
}
int
main (int argc,
char* argv[])
{
// The point clouds we will be using
PointCloudT::Ptr cloud_in (new PointCloudT); // Original point cloud
PointCloudT::Ptr cloud_tr (new PointCloudT); // Transformed point cloud
PointCloudT::Ptr cloud_icp (new PointCloudT); // ICP output point cloud
// Checking program arguments
if (argc < 2)
{
printf ("Usage :\n");
printf ("\t\t%s file.ply number_of_ICP_iterations\n", argv[0]);
PCL_ERROR ("Provide one ply file.\n");
return (-1);
}
int iterations = 1; // Default number of ICP iterations
if (argc > 2)
{
// If the user passed the number of iteration as an argument
iterations = atoi (argv[2]);
if (iterations < 1)
{
PCL_ERROR ("Number of initial iterations must be >= 1\n");
return (-1);
}
}
pcl::console::TicToc time;
time.tic ();
if (pcl::io::loadPLYFile (argv[1], *cloud_in) < 0)
{
PCL_ERROR ("Error loading cloud %s.\n", argv[1]);
return (-1);
}
std::cout << "\nLoaded file " << argv[1] << " (" << cloud_in->size () << " points) in " << time.toc () << " ms\n" << std::endl;
// Defining a rotation matrix and translation vector
Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity ();
// A rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
double theta = M_PI / 8; // The angle of rotation in radians
transformation_matrix (0, 0) = cos (theta);
transformation_matrix (0, 1) = -sin (theta);
transformation_matrix (1, 0) = sin (theta);
transformation_matrix (1, 1) = cos (theta);
// A translation on Z axis (0.4 meters)
transformation_matrix (2, 3) = 0.4;
// Display in terminal the transformation matrix
std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;
print4x4Matrix (transformation_matrix);
// Executing the transformation
pcl::transformPointCloud (*cloud_in, *cloud_icp, transformation_matrix);
*cloud_tr = *cloud_icp; // We backup cloud_icp into cloud_tr for later use
// The Iterative Closest Point algorithm
time.tic ();
pcl::IterativeClosestPoint<PointT, PointT> icp;
icp.setMaximumIterations (iterations);
icp.setInputSource (cloud_icp);
icp.setInputTarget (cloud_in);
icp.align (*cloud_icp);
icp.setMaximumIterations (1); // We set this variable to 1 for the next time we will call .align () function
std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc () << " ms" << std::endl;
if (icp.hasConverged ())
{
std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl;
std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;
transformation_matrix = icp.getFinalTransformation ().cast<double>();
print4x4Matrix (transformation_matrix);
}
else
{
PCL_ERROR ("\nICP has not converged.\n");
return (-1);
}
// Visualization
pcl::visualization::PCLVisualizer viewer ("ICP demo");
// Create two vertically separated viewports
int v1 (0);
int v2 (1);
viewer.createViewPort (0.0, 0.0, 0.5, 1.0, v1);
viewer.createViewPort (0.5, 0.0, 1.0, 1.0, v2);
// The color we will be using
float bckgr_gray_level = 0.0; // Black
float txt_gray_lvl = 1.0 - bckgr_gray_level;
// Original point cloud is white
pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h (cloud_in, (int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl,
(int) 255 * txt_gray_lvl);
viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v1", v1);
viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v2", v2);
// Transformed point cloud is green
pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h (cloud_tr, 20, 180, 20);
viewer.addPointCloud (cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);
// ICP aligned point cloud is red
pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h (cloud_icp, 180, 20, 20);
viewer.addPointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);
// Adding text descriptions in each viewport
viewer.addText ("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);
viewer.addText ("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);
std::stringstream ss;
ss << iterations;
std::string iterations_cnt = "ICP iterations = " + ss.str ();
viewer.addText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);
// Set background color
viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);
// Set camera position and orientation
viewer.setCameraPosition (-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
viewer.setSize (1280, 1024); // Visualiser window size
// Register keyboard callback :
viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL);
// Display the visualiser
while (!viewer.wasStopped ())
{
viewer.spinOnce ();
// The user pressed "space" :
if (next_iteration)
{
// The Iterative Closest Point algorithm
time.tic ();
icp.align (*cloud_icp);
std::cout << "Applied 1 ICP iteration in " << time.toc () << " ms" << std::endl;
if (icp.hasConverged ())
{
printf ("\033[11A"); // Go up 11 lines in terminal output.
printf ("\nICP has converged, score is %+.0e\n", icp.getFitnessScore ());
std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_in" << std::endl;
transformation_matrix *= icp.getFinalTransformation ().cast<double>(); // WARNING /!\ This is not accurate! For "educational" purpose only!
print4x4Matrix (transformation_matrix); // Print the transformation between original pose and current pose
ss.str ("");
ss << iterations;
std::string iterations_cnt = "ICP iterations = " + ss.str ();
viewer.updateText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt");
viewer.updatePointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2");
}
else
{
PCL_ERROR ("\nICP has not converged.\n");
return (-1);
}
}
next_iteration = false;
}
return (0);
}
