void downsampling(double scale)
{
CloudConstPtr pCloudConst = (CloudConstPtr)pCloud_input;
pcl::VoxelGrid<PointT> vg;
vg.setInputCloud(pCloudConst);
vg.setLeafSize(scale, scale, scale); // down sampling using a leaf size of 'scale'
vg.filter(*pCloud);
pCloud_input.reset(new Cloud);
pCloud_input = pCloud;
pCloud.reset(new Cloud);
}
/* ---[ */
int
main (int argc, char** argv)
{
// Check whether we want to enable debug mode
bool debug = false;
parse_argument (argc, argv, "-debug", debug);
if (debug)
setVerbosityLevel (L_DEBUG);
parse_argument (argc, argv, "-rejection", rejection);
parse_argument (argc, argv, "-visualization", visualize);
if (visualize)
vis.reset (new PCLVisualizer ("Registration example"));
// Parse the command line arguments for .pcd and .transform files
std::vector<int> p_pcd_file_indices, p_tr_file_indices;
p_pcd_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
if (p_pcd_file_indices.size () != 2)
{
print_error ("Need one input source PCD file and one input target PCD file to continue.\n");
print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]);
return (-1);
}
p_tr_file_indices = parse_file_extension_argument (argc, argv, ".transform");
if (p_tr_file_indices.size () != 1)
{
print_error ("Need one output transform file to continue.\n");
print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]);
return (-1);
}
// Load the files
print_info ("Loading %s as source and %s as target...\n", argv[p_pcd_file_indices[0]], argv[p_pcd_file_indices[1]]);
src.reset (new PointCloud<PointT>);
tgt.reset (new PointCloud<PointT>);
if (loadPCDFile (argv[p_pcd_file_indices[0]], *src) == -1 || loadPCDFile (argv[p_pcd_file_indices[1]], *tgt) == -1)
{
print_error ("Error reading the input files!\n");
return (-1);
}
// Compute the best transformtion
Eigen::Matrix4d transform;
icp (src, tgt, transform);
saveTransform (argv[p_tr_file_indices[0]], transform);
cerr.precision (15);
std::cerr << transform << std::endl;
}
void planeExtraction(int nPlane)
{
CloudConstPtr pCloudConst = (CloudConstPtr)pCloud_input;
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<PointT> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
// pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
// plane segmentation
for(int i=0;i<nPlane;i++)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud(pCloud_input);
seg.segment (*inliers, *coefficients); //*
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud (pCloudConst);
extract.setIndices (inliers);
extract.setNegative (false);
// Write the planar inliers to disk
CloudPtr cloud_plane;
cloud_plane.reset(new Cloud);
extract.filter (*cloud_plane); //*
// clouds_plane.push_back(cloud_plane);
// std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*pCloud); //*
}
pCloud_input.reset(new Cloud);
pCloud_input = pCloud;
pCloud.reset(new Cloud);
}
void ObjectDetector::processCloud(const CloudConstPtr& cloud)
{
cloud_pass_through_.reset(new Cloud);
// Computation goes here
pass_through_.setInputCloud(cloud);
pass.filter(*cloud_pass_through_);
// Estimate 3d convex hull
pcl::ConvexHull<PointType> hr;
hr.setInputCloud(cloud_pass_through_);
cloud_hull_.reset(new Cloud);
hr.reconstruct(*cloud_hull_, vertices_);
cloud_ = cloud;
new_cloud_ = true;
}
//OpenNI Grabber's cloud Callback function
void
cloud_cb (const CloudConstPtr &cloud)
{
boost::mutex::scoped_lock lock (mtx_);
cloud_pass_.reset (new Cloud);
cloud_pass_downsampled_.reset (new Cloud);
filterPassThrough (cloud, *cloud_pass_);
gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_);
if(counter < 10){
counter++;
}else{
//Track the object
tracker_->setInputCloud (cloud_pass_downsampled_);
tracker_->compute ();
new_cloud_ = true;
}
}
void workingspace()
{
double top = param_workspace_y + param_workspace_height/2;
double bottom = param_workspace_y - param_workspace_height/2;
double left = param_workspace_x - param_workspace_width/2;
double right = param_workspace_x + param_workspace_width/2;
double zbottom = param_workspace_z;
double ztop = param_workspace_z + param_workspace_zheight;
for(int i=0;i<pCloud_input->points.size();i++){
PointT temp_point = pCloud_input->points[i];
// cut off
if(temp_point.x<right && temp_point.x>left && temp_point.y<top && temp_point.y>bottom && temp_point.z>zbottom && temp_point.z<ztop)
pCloud->points.push_back(temp_point);
}
pCloud_input.reset(new Cloud);
pCloud_input = pCloud;
pCloud.reset(new Cloud);
workspace.header.frame_id = param_frame_id.data();
workspace.header.stamp = ros::Time::now();
pub_workspace.publish (workspace);
}
void
run ()
{
CloudPtr filecloud;
pcl::Grabber* interface;
if(filename_.empty()) {
interface = new pcl::OpenNIGrabber (device_id_);
boost::function<void (const CloudConstPtr&)> f = boost::bind (&PclPyramid::cloud_cb_, this, _1);
boost::signals2::connection c = interface->registerCallback (f);
interface->start ();
}
else
{
pcd_cloud.reset (new pcl::PCLPointCloud2);
if(pcd.read (filename_, *pcd_cloud, origin, orientation, version) < 0)
cout << "file read failed" << endl;
filecloud.reset (new Cloud);
pcl::fromPCLPointCloud2(*pcd_cloud, *filecloud);
cloud_ = filecloud;
}
#ifdef NOVIEWER
if(!filename_.empty())
get();
else
while(TRUE)
{
if (cloud_)
get();
boost::this_thread::sleep (boost::posix_time::microseconds (10000));
}
#else
while ( !viewer.wasStopped () )
{
if (cloud_)
{
//the call to get() sets the cloud_ to null;
viewer.showCloud (get ());
}
boost::this_thread::sleep (boost::posix_time::microseconds (10000));
}
#endif
if(filename_.empty()) {
interface->stop ();
}
}
/* ---[ */
int
main (int argc, char** argv)
{
if (argc < 3)
{
std::cerr << "No test file given. Please download `bun0.pcd` and `milk.pcd` pass its path to the test." << std::endl;
return (-1);
}
if (loadPCDFile<PointXYZ> (argv[1], cloud) < 0)
{
std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl;
return (-1);
}
CloudPtr milk_loaded(new PointCloud<PointXYZ>());
if (loadPCDFile<PointXYZ> (argv[2], *milk_loaded) < 0)
{
std::cerr << "Failed to read test file. Please download `milk.pcd` and pass its path to the test." << std::endl;
return (-1);
}
indices.resize (cloud.points.size ());
for (size_t i = 0; i < indices.size (); ++i)
{
indices[i] = static_cast<int>(i);
}
tree.reset (new search::KdTree<PointXYZ> (false));
tree->setInputCloud (cloud.makeShared ());
cloud_milk.reset(new PointCloud<PointXYZ>());
CloudPtr grid;
pcl::VoxelGrid < pcl::PointXYZ > grid_;
grid_.setInputCloud (milk_loaded);
grid_.setLeafSize (leaf_size_, leaf_size_, leaf_size_);
grid_.filter (*cloud_milk);
tree_milk.reset (new search::KdTree<PointXYZ> (false));
tree_milk->setInputCloud (cloud_milk);
testing::InitGoogleTest (&argc, argv);
return (RUN_ALL_TESTS ());
}
void cb_rgb(const pcl::PCLPointCloud2ConstPtr& input)
{
static int cnt = 0;
// input
pCloud_input.reset(new Cloud);
pCloud.reset(new Cloud);
pcl::fromPCLPointCloud2(*input, *pCloud_input);
if(isdebug) cout<<"I heard RGB, # of points: "<<pCloud_input->points.size()<<endl;
// downsampling
lastT = pcl::getTime ();
downsampling(cont_sampling);
if(isdebug) cout<<"downsampling computation time : "<<pcl::getTime()-lastT<<endl;
if(isdebug) cout<<"downsampling end, # of points: "<<pCloud_input->points.size()<<endl;
// transform to the given frame
lastT = pcl::getTime ();
transform(param_frame_id);
if(isdebug) cout<<"transform computation time : "<<pcl::getTime()-lastT<<endl;
// workspace
lastT = pcl::getTime ();
workingspace();
if(isdebug) cout<<"workingspace computation time : "<<pcl::getTime()-lastT<<endl;
if(isdebug) cout<<"workingspace end, # of points: "<<pCloud_input->points.size()<<endl;
// // planeExtraction
// lastT = pcl::getTime ();
// planeExtraction(1);
// if(isdebug) cout<<"planeExtraction computation time : "<<pcl::getTime()-lastT<<endl;
// if(isdebug) cout<<"planeExtraction end, # of points: "<<pCloud_input->points.size()<<endl;
// tracking
lastT = pcl::getTime ();
pctracking->run(pCloud_input);
nowT = pcl::getTime ();
if(isdebug) cout<<"total tracking computation time : "<<nowT-lastT<<endl;
// publish filtered pointcloud
pcl::PCLPointCloud2 output_filtered;
pcl::toPCLPointCloud2(*pctracking->getFilteredPC(), output_filtered);
output_filtered.header.frame_id=param_frame_id.data();
pub_filtered.publish (output_filtered);
// publish segmented pointcloud
pcl::PCLPointCloud2 output_segmented;
pcl::toPCLPointCloud2(*pctracking->getSegmentedPC(), output_segmented);
output_segmented.header.frame_id=param_frame_id.data();
pub_segmented.publish (output_segmented);
// publish model pointcloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pCloudOut_model;
pCloudOut_model.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
pctracking->object_tracks->toPointCloudXYZI_model(*pCloudOut_model);
pcl::PCLPointCloud2 output_model;
pcl::toPCLPointCloud2(*pCloudOut_model, output_model);
output_model.header.frame_id=param_frame_id.data();
pub_objectmodel.publish (output_model);
// output tracks pointcloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pCloudOut;
pCloudOut.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
pctracking->object_tracks->toPointCloudXYZI(*pCloudOut);
// ROS_INFO("# of track points: %d", pCloudOut->points.size());
// publish pointcloud(currentT) of tracks
pcl::PCLPointCloud2 output;
pcl::toPCLPointCloud2(*pCloudOut, output);
output.header.frame_id=param_frame_id.data();
pub_track.publish (output);
// // publish lastmodel
// pcl::PCLPointCloud2 output_lastmodel;
// pcl::toPCLPointCloud2(*pctracking->getLastModel(), output_lastmodel);
// output_lastmodel.header.frame_id=param_frame_id.data();
// pub_lastmodel.publish (output_lastmodel);
// // publish particles
// pcl::PCLPointCloud2 output_particles;
// pcl::toPCLPointCloud2(*pctracking->getParticles(), output_particles);
// output_particles.header.frame_id=param_frame_id.data();
// pub_particles.publish (output_particles);
// publish gaussians
pub_gaussians.publish(pctracking->object_tracks->oldGaussians());
pub_gaussians.publish(pctracking->object_tracks->toMarkerGaussians());
// publish gmms
// pub_gmms.publish(pctracking->object_tracks->toMarkerGMMs());
// pub_edges.publish(pctracking->object_tracks->toMarkerEdges());
// publish delete id marker of deleted tracks
pub_trackID.publish(pctracking->object_tracks->oldMarkerIDs());
pub_trackID.publish(pctracking->object_tracks->toMarkerIDs());
pCloudOut.reset();
cnt++;
}
void
segment (const PointT &picked_point,
int picked_idx,
PlanarRegion<PointT> ®ion,
PointIndices &indices,
CloudPtr &object)
{
// First frame is segmented using an organized multi plane segmentation approach from points and their normals
if (!first_frame_)
return;
// Estimate normals in the cloud
PointCloud<Normal>::Ptr normal_cloud (new PointCloud<Normal>);
ne_.setInputCloud (search_.getInputCloud ());
ne_.compute (*normal_cloud);
plane_comparator_->setDistanceMap (ne_.getDistanceMap ());
// Segment out all planes
mps_.setInputNormals (normal_cloud);
mps_.setInputCloud (search_.getInputCloud ());
// Use one of the overloaded segmentAndRefine calls to get all the information that we want out
vector<PlanarRegion<PointT> > regions;
vector<ModelCoefficients> model_coefficients;
vector<PointIndices> inlier_indices;
PointCloud<Label>::Ptr labels (new PointCloud<Label>);
vector<PointIndices> label_indices;
vector<PointIndices> boundary_indices;
mps_.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
PCL_DEBUG ("Number of planar regions detected: %zu for a cloud of %zu points and %zu normals.\n", regions.size (), search_.getInputCloud ()->points.size (), normal_cloud->points.size ());
double max_dist = numeric_limits<double>::max ();
// Compute the distances from all the planar regions to the picked point, and select the closest region
int idx = -1;
for (size_t i = 0; i < regions.size (); ++i)
{
double dist = pointToPlaneDistance (picked_point, regions[i].getCoefficients ());
if (dist < max_dist)
{
max_dist = dist;
idx = static_cast<int> (i);
}
}
PointIndices::Ptr plane_boundary_indices;
// Get the plane that holds the object of interest
if (idx != -1)
{
region = regions[idx];
plane_indices_.reset (new PointIndices (inlier_indices[idx]));
plane_boundary_indices.reset (new PointIndices (boundary_indices[idx]));
}
// Segment the object of interest
if (plane_boundary_indices && !plane_boundary_indices->indices.empty ())
{
object.reset (new Cloud);
segmentObject (picked_idx, search_.getInputCloud (), plane_indices_, plane_boundary_indices, *object);
// Save to disk
//pcl::io::saveTARPointCloud ("output.ltm", *object, "object.pcd");
}
first_frame_ = false;
}
void VisMotCoord::getPCScene(CloudPtr &pc_out)
{
pc_out.reset(new Cloud);
*pc_out = *scene.getPCScene();
}
int
main (int argc, char** argv)
{
if (argc < 3)
{
PCL_WARN("Please set device_id pcd_filename(e.g. $ %s '#1' sample.pcd)\n", argv[0]);
exit (1);
}
//read pcd file
target_cloud.reset(new Cloud());
if(pcl::io::loadPCDFile (argv[2], *target_cloud) == -1){
std::cout << "pcd file not found" << std::endl;
exit(-1);
}
std::string device_id = std::string (argv[1]);
counter = 0;
//Set parameters
new_cloud_ = false;
downsampling_grid_size_ = 0.002;
std::vector<double> default_step_covariance = std::vector<double> (6, 0.015 * 0.015);
default_step_covariance[3] *= 40.0;
default_step_covariance[4] *= 40.0;
default_step_covariance[5] *= 40.0;
std::vector<double> initial_noise_covariance = std::vector<double> (6, 0.00001);
std::vector<double> default_initial_mean = std::vector<double> (6, 0.0);
KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT>::Ptr tracker
(new KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT> (8));
ParticleT bin_size;
bin_size.x = 0.1f;
bin_size.y = 0.1f;
bin_size.z = 0.1f;
bin_size.roll = 0.1f;
bin_size.pitch = 0.1f;
bin_size.yaw = 0.1f;
//Set all parameters for KLDAdaptiveParticleFilterOMPTracker
tracker->setMaximumParticleNum (1000);
tracker->setDelta (0.99);
tracker->setEpsilon (0.2);
tracker->setBinSize (bin_size);
//Set all parameters for ParticleFilter
tracker_ = tracker;
tracker_->setTrans (Eigen::Affine3f::Identity ());
tracker_->setStepNoiseCovariance (default_step_covariance);
tracker_->setInitialNoiseCovariance (initial_noise_covariance);
tracker_->setInitialNoiseMean (default_initial_mean);
tracker_->setIterationNum (1);
tracker_->setParticleNum (600);
tracker_->setResampleLikelihoodThr(0.00);
tracker_->setUseNormal (false);
//Setup coherence object for tracking
ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr coherence
(new ApproxNearestPairPointCloudCoherence<RefPointType>);
DistanceCoherence<RefPointType>::Ptr distance_coherence
(new DistanceCoherence<RefPointType>);
coherence->addPointCoherence (distance_coherence);
pcl::search::Octree<RefPointType>::Ptr search (new pcl::search::Octree<RefPointType> (0.01));
coherence->setSearchMethod (search);
coherence->setMaximumDistance (0.01);
tracker_->setCloudCoherence (coherence);
//prepare the model of tracker's target
Eigen::Vector4f c;
Eigen::Affine3f trans = Eigen::Affine3f::Identity ();
CloudPtr transed_ref (new Cloud);
CloudPtr transed_ref_downsampled (new Cloud);
pcl::compute3DCentroid<RefPointType> (*target_cloud, c);
trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]);
pcl::transformPointCloud<RefPointType> (*target_cloud, *transed_ref, trans.inverse());
gridSampleApprox (transed_ref, *transed_ref_downsampled, downsampling_grid_size_);
//set reference model and trans
tracker_->setReferenceCloud (transed_ref_downsampled);
tracker_->setTrans (trans);
//Setup OpenNIGrabber and viewer
pcl::visualization::CloudViewer* viewer_ = new pcl::visualization::CloudViewer("PCL OpenNI Tracking Viewer");
pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id);
boost::function<void (const CloudConstPtr&)> f =
boost::bind (&cloud_cb, _1);
interface->registerCallback (f);
viewer_->runOnVisualizationThread (boost::bind(&viz_cb, _1), "viz_cb");
//Start viewer and object tracking
interface->start();
while (!viewer_->wasStopped ())
std::this_thread::sleep_for(1s);
interface->stop();
}