int main(int argc, char** argv)
{
if (argc != 3)
{
printUsage(argv);
return -1;
}
std::string filename_in = argv[1];
std::string filename_out = argv[2];
// read in
printf("Reading cloud\n");
PointCloudT::Ptr cloud;
cloud.reset(new rgbdtools::PointCloudT());
pcl::PCDReader reader;
reader.read(filename_in, *cloud);
alignGlobalCloud(cloud);
return 0;
}
int main (int argc, char** argv)
{
PointCloudT::Ptr cloud (new PointCloudT);
PointCloudT::Ptr new_cloud (new PointCloudT);
bool new_cloud_available_flag = false;
//pcl::Grabber* grab = new pcl::OpenNIGrabber ();
PointCloudT::Ptr ddd;
boost::function<void (const PointCloudT::ConstPtr&)> f =
boost::bind(&grabberCallback, _1, cloud, &new_cloud_available_flag);
grab->registerCallback (f);
viewer->registerKeyboardCallback(keyboardEventOccurred);
grab->start ();
bool first_time = true;
FILE* objects;
objects = fopen ("objects.txt","a");
while(!new_cloud_available_flag)
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
new_cloud_available_flag=false;
////////////////////
// invert correction
////////////////////
Eigen::Matrix4f transMat = Eigen::Matrix4f::Identity();
transMat (1,1) = -1;
////////////////////
// pass filter
////////////////////
PointCloudT::Ptr passed_cloud;
pcl::PassThrough<PointT> pass;
passed_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
////////////////////
// voxel grid
////////////////////
PointCloudT::Ptr voxelized_cloud;
voxelized_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
pcl::VoxelGrid<PointT> vg;
vg.setLeafSize (0.001, 0.001, 0.001);
////////////////////
// sac segmentation
////////////////////
PointCloudT::Ptr cloud_f;
PointCloudT::Ptr cloud_plane;
PointCloudT::Ptr cloud_filtered;
cloud_f = boost::shared_ptr<PointCloudT>(new PointCloudT);
cloud_plane = boost::shared_ptr<PointCloudT> (new PointCloudT);
cloud_filtered = boost::shared_ptr<PointCloudT> (new PointCloudT);
pcl::SACSegmentation<PointT> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
////////////////////
// euclidean clustering
////////////////////
std::vector<pcl::PointIndices> cluster_indices;
std::vector<PointCloudT::Ptr> extracted_clusters;
pcl::search::KdTree<PointT>::Ptr eucl_tree (new pcl::search::KdTree<PointT>);
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (0.04);
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (eucl_tree);
PointCloudT::Ptr cloud_cluster;
////////////////////
// vfh estimate
////////////////////
pcl::NormalEstimation<PointT, pcl::Normal> ne;
pcl::search::KdTree<PointT>::Ptr vfh_tree1 (new pcl::search::KdTree<PointT> ());
pcl::VFHEstimation<PointT, pcl::Normal, pcl::VFHSignature308> vfh;
pcl::search::KdTree<PointT>::Ptr vfh_tree2 (new pcl::search::KdTree<PointT> ());
std::vector<pcl::PointCloud<pcl::VFHSignature308>::Ptr> computed_vfhs;
ne.setSearchMethod (vfh_tree1);
ne.setRadiusSearch (0.05);
vfh.setSearchMethod (vfh_tree2);
vfh.setRadiusSearch (0.05);
pcl::PointCloud<pcl::Normal>::Ptr normals;
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs;
////////////////////
// nearest neighbour
////////////////////
int k = 6;
std::string kdtree_idx_file_name = "kdtree.idx";
std::string training_data_h5_file_name = "training_data.h5";
std::string training_data_list_file_name = "training_data.list";
// std::vector<vfh_model> models;
// flann::Matrix<float> data;
// loadFileList (models, training_data_list_file_name);
// flann::load_from_file (data,
// training_data_h5_file_name,
// "training_data");
//
// flann::Index<flann::ChiSquareDistance<float> > index (data,
// flann::SavedIndexParams
// ("kdtree.idx"));
////////////////////
// process nearest neighbour
////////////////////
std::vector<hypothesis> final_hypothesis;
final_hypothesis.clear();
double last = pcl::getTime();
while (! viewer->wasStopped())
{
if (new_cloud_available_flag)
{
new_cloud_available_flag = false;
double now = pcl::getTime();
////////////////////
// pass filter
////////////////////
//passed_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
////////////////////
// voxel grid
////////////////////
//voxelized_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
////////////////////
// sac segmentation
////////////////////
//cloud_f = boost::shared_ptr<PointCloudT>(new PointCloudT);
//cloud_plane = boost::shared_ptr<PointCloudT> (new PointCloudT);
//cloud_filtered = boost::shared_ptr<PointCloudT> (new PointCloudT);
////////////////////
// euclidean clustering
////////////////////
extracted_clusters.clear();
cluster_indices.clear();
////////////////////
// vfh estimate
////////////////////
computed_vfhs.clear();
////////////////////
// nearest neighbour
////////////////////
cloud_mutex.lock();
//displayCloud(viewer,cloud);
boost::thread displayCloud_(displayCloud,viewer,cloud);
if(now-last > 13 || first_time)
{
first_time = false;
last=now;
////////////////////
// invert correction
////////////////////
pcl::transformPointCloud(*cloud,*new_cloud,transMat);
////////////////////
// pass filter
////////////////////
pass.setInputCloud (new_cloud);
pass.setFilterFieldName ("x");
pass.setFilterLimits (-0.5, 0.5);
//pass.setFilterLimitsNegative (true);
pass.filter (*passed_cloud);
////////////////////
// voxel grid
////////////////////
vg.setInputCloud (passed_cloud);
vg.filter (*voxelized_cloud);
////////////////////
// sac segmentation
////////////////////
*cloud_filtered = *voxelized_cloud;
int i=0, nr_points = (int) voxelized_cloud->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Couldnt estimate a planar model for the dataset.\n";
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
}
////////////////////
// euclidean clustering
////////////////////
// Creating the KdTree object for the search method of the extraction
eucl_tree->setInputCloud (cloud_filtered);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
//PointCloudT::Ptr cloud_cluster (new PointCloudT);
cloud_cluster = boost::shared_ptr<PointCloudT>(new PointCloudT);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud_filtered->points [*pit]);
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
extracted_clusters.push_back(cloud_cluster);
}
cloud_cluster.reset();
////////////////////
// vfh estimate
////////////////////
for (int z=0; z<extracted_clusters.size(); ++z)
{
vfhs = boost::shared_ptr<pcl::PointCloud<pcl::VFHSignature308> > (new pcl::PointCloud<pcl::VFHSignature308>);
normals = boost::shared_ptr<pcl::PointCloud<pcl::Normal> > (new pcl::PointCloud<pcl::Normal>);
ne.setInputCloud (extracted_clusters.at(z));
ne.compute (*normals);
vfh.setInputCloud (extracted_clusters.at(z));
vfh.setInputNormals (normals);
vfh.compute (*vfhs);
computed_vfhs.push_back(vfhs);
}
vfhs.reset();
normals.reset();
////////////////////
// nearest neighbour
////////////////////
std::vector<vfh_model> models;
flann::Matrix<int> k_indices;
flann::Matrix<float> k_distances;
flann::Matrix<float> data;
loadFileList (models, training_data_list_file_name);
flann::load_from_file (data,
training_data_h5_file_name,
"training_data");
flann::Index<flann::ChiSquareDistance<float> > index
(data,
flann::SavedIndexParams
("kdtree.idx"));
for(int z=0; z<computed_vfhs.size(); ++z)
{
vfh_model histogram;
histogram.second.resize(308);
for (size_t i = 0; i < 308; ++i)
{
histogram.second[i] = computed_vfhs.at(z)->points[0].histogram[i];
}
index.buildIndex ();
nearestKSearch (index, histogram, k, k_indices, k_distances);
hypothesis x;
x.distance = k_distances[0][0];
size_t index = models.at(k_indices[0][0]).first.find("_v",5);
x.object_name = models.at(k_indices[0][0]).first.substr(5,index-5);
ddd = boost::shared_ptr<PointCloudT>(new PointCloudT);
pcl::transformPointCloud(*extracted_clusters.at(z),*ddd,transMat);
x.cluster = ddd;
ddd.reset();
std::string filename ="";
filename += "inputcloud_" + boost::lexical_cast<std::string>(j+1);
filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd";
x.cluster_name = filename.c_str();
final_hypothesis.push_back(x);
x.cluster.reset();
//delete x;
// std::string filename ="";
// filename += "inputcloud_" + boost::lexical_cast<std::string>(j+1);
// filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd";
// const char* filen = filename.c_str();
// fprintf(objects,"%s",filen);
// fprintf(objects,"::");
// fprintf(objects,models.at (k_indices[0][0]).first.c_str());
// fprintf(objects,"::");
// fprintf(objects,"%f",k_distances[0][0]);
// fprintf(objects,"\n");
}
delete[] k_indices.ptr ();
delete[] k_distances.ptr ();
delete[] data.ptr ();
std::cout << final_hypothesis.size() << std::endl;
viewer->removeAllShapes();
for(int z=0; z<final_hypothesis.size();++z)
{
if(final_hypothesis.at(z).distance < 100)
{
fprintf(objects,"%s",final_hypothesis.at(z).cluster_name.c_str());
fprintf(objects,"::");
fprintf(objects,"%s",final_hypothesis.at(z).object_name.c_str());
fprintf(objects,"::");
fprintf(objects,"%f",final_hypothesis.at(z).distance);
fprintf(objects,"\n");
std::stringstream ddd;
ddd << final_hypothesis.at(z).object_name;
ddd << "\n" << "(";
ddd << final_hypothesis.at(z).distance;
ddd << ")";
viewer->addText3D(ddd.str().c_str(),
final_hypothesis.at(z).cluster->points[0],
0.02,1,1,1,
boost::lexical_cast<std::string>(z));
drawBoundingBox(final_hypothesis.at(z).cluster,viewer,z);
}
}
//boost::thread allBoxes_(allBoxes,viewer,final_hypothesis);
//allBoxes_.join();
viewer->spinOnce();
final_hypothesis.clear();
j++;
}
// for(int z=0; z<extracted_clusters.size(); ++z)
// {
// //viewer->addPointCloud<PointT>(extracted_clusters.at(z),
// // boost::lexical_cast<std::string>(z));
//
// std::string filename ="";
// filename += "inputcloud_" + boost::lexical_cast<std::string>(j);
// filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd";
// pcl::io::savePCDFile(filename,*extracted_clusters.at(z),false);
// }
// for(int z=0; z<computed_vfhs.size(); ++z)
// {
// //viewer->addPointCloud<PointT>(extracted_clusters.at(z),
// // boost::lexical_cast<std::string>(z));
//
// std::string filename ="";
// filename += "inputcloud_" + boost::lexical_cast<std::string>(j);
// filename += "_" + boost::lexical_cast<std::string>(z) + "_vfhs.pcd";
// pcl::io::savePCDFileASCII<pcl::VFHSignature308> (filename, *computed_vfhs.at(z));
// }
//viewer->removeAllShapes();
// viewer->removeAllPointClouds();
// viewer->setCameraPosition(0, 0, 0, 0, 0, 1, 0, -1, 0);
// pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
// viewer->addPointCloud<PointT>(cloud,rgb,"input_cloud");
// viewer->spinOnce();
//std::cout << final_hypothesis.at(0).cluster->points[0];
//boost::this_thread::sleep(boost::posix_time::milliseconds(10));
displayCloud_.join();
cloud_mutex.unlock();
}
}
grab->stop();
return 0;
}
