//
// 計算 pointcloud 的 VFH, 存成 vfh.pcd
//
void vfh_demo()
{
string filename;
cout << "Input filename: ";
cin >> filename;
// Create new point clouds to hold data
pcl::PointCloud<PointType>::Ptr points (new pcl::PointCloud<PointType>);
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs(new pcl::PointCloud<pcl::VFHSignature308>);
// Load point data
pcl::io::loadPCDFile(filename, *points);
// Compute normals
compute_surface_normals(points, normals);
// Create VFHEstimation
pcl::VFHEstimation<PointType, pcl::Normal, pcl::VFHSignature308> vfh;
vfh.setInputCloud(points);
vfh.setInputNormals(normals);
pcl::search::KdTree<PointType>::Ptr tree (new pcl::search::KdTree<PointType>());
vfh.setSearchMethod(tree);
vfh.compute(*vfhs);
// Save VFH
pcl::io::savePCDFileASCII("vfh.pcd", *vfhs);
}
void mpcl_extract_VFH(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
// http://virtuemarket-lab.blogspot.jp/2015/03/viewpoint-feature-histogram.html
// pcl::PointCloud<pcl::VFHSignature308>::Ptr Extract_VFH(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ());
pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh;
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs (new pcl::PointCloud<pcl::VFHSignature308> ());
// cloud_normals = surface_normals(cloud);
vfh.setInputCloud (cloud);
vfh.setInputNormals (cloud_normals);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
vfh.setSearchMethod (tree);
vfh.compute (*vfhs);
// return vfhs;
}
std::vector<float> compute_VFHS_features( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,pcl::PointCloud<pcl::Normal>::Ptr normals ){
pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh;
vfh.setInputCloud (cloud);
vfh.setInputNormals (normals);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
vfh.setSearchMethod (tree);
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs (new pcl::PointCloud<pcl::VFHSignature308> ());
vfh.compute (*vfhs);
pcl::VFHSignature308 value=vfhs->points[0];
std::cout << value;
std::vector<float> histogram;
for(int i=0;i< value.descriptorSize();i++){
histogram.push_back(value.histogram[i]);
}
return histogram;
}
/** \brief Loads an n-D histogram file as a VFH signature
* \param path the input file name
* \param vfh the resultant VFH model
*/
bool
loadHist (const boost::filesystem::path &path, vfh_model &vfh)
{
pcl::PointCloud<PointType>::Ptr cloud (new pcl::PointCloud<PointType>);
pcl::PointCloud<NormalType>::Ptr cloud_normals (new pcl::PointCloud<NormalType> ());
if (pcl::io::loadPCDFile<PointType> (path.string(), *cloud) == -1) {
PCL_ERROR ("Couldn't read file %s.pcd \n", path.string().c_str());
return false;
}
pcl::PointCloud<PointType>::Ptr cloud_filtered (new pcl::PointCloud<PointType>);
std::vector<int> filter_index;
pcl::removeNaNFromPointCloud (*cloud, *cloud_filtered, filter_index);
cloud = cloud_filtered;
pcl::NormalEstimationOMP<PointType, NormalType> norm_est;
norm_est.setKSearch (10);
norm_est.setInputCloud (cloud);
norm_est.compute (*cloud_normals);
pcl::VFHEstimation<PointType, NormalType, pcl::VFHSignature308> vfh_est;
vfh_est.setInputCloud (cloud);
vfh_est.setInputNormals (cloud_normals);
pcl::search::KdTree<PointType>::Ptr tree (new pcl::search::KdTree<PointType> ());
vfh_est.setSearchMethod (tree);
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs (new pcl::PointCloud<pcl::VFHSignature308> ());
vfh_est.compute (*vfhs);
vfh.second.resize (308);
for (size_t i = 0; i < pcl::VFHSignature308::descriptorSize(); ++i)
{
vfh.second[i] = vfhs->points[0].histogram[i];
}
vfh.first = path.string ();
return (true);
}
//Function that recursively reads all files and computes the VFH for them
void VFH::readFilesAndComputeVFH (const boost::filesystem::path &base_dir)
{
pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs(new pcl::PointCloud<pcl::VFHSignature308> ());
//Recursively read all files and compute VFH
for(boost::filesystem::directory_iterator it (base_dir); it!=boost::filesystem::directory_iterator (); ++it)
{
std::stringstream ss;
ss << it->path();
//if its a directory just call back the function
if (boost::filesystem::is_directory (it->status()))
{
pcl::console::print_highlight ("Entering directory %s.\n", ss.str().c_str());
//call rescursively our function
readFilesAndComputeVFH(it->path());
}
//if not, go ahead and read and process the file
if (boost::filesystem::is_regular_file (it->status()) && boost::filesystem::extension (it->path()) == VFH_FILES_EXTENSION )
{
if(pcl::io::loadPCDFile<PointT> (it->path().string(), *cloud) == -1)
PCL_ERROR ("Couldn't read the file %s.", it->path().string().c_str());
else
{
pcl::console::print_highlight ("Computing VFH for %s.\n", boost::filesystem::path(*it).string().c_str());
//Finally compute the vfh and save it
computeVFHistogram(cloud, vfhs);
//save them to file
pcl::PCDWriter writer;
std::stringstream ss;
ss <<boost::filesystem::path(*it).branch_path()<< "/" << boost::filesystem::basename(*it) << ".vfh";
pcl::console::print_highlight ("writing %s\n", ss.str().c_str());
writer.write<pcl::VFHSignature308> (ss.str(), *vfhs, false);
}
}
}
}
void VFH::doTheGuess(const pcl::PointCloud<PointT>::Ptr object, std::vector<std::string> &guesses)
{
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs(new pcl::PointCloud<pcl::VFHSignature308> ());
computeVFHistogram(object, vfhs);
vfh_model histogram;
pcl::PointCloud <pcl::VFHSignature308> point;
histogram.second.resize (308);
std::vector <pcl::PCLPointField> fields;
int vfh_idx = pcl::getFieldIndex (*vfhs, "vfh", fields);
for (size_t i = 0; i < fields[vfh_idx].count; ++i)
{
histogram.second[i] = vfhs->points[0].histogram[i];
std::cout<<histogram.second[i]<<std::endl;
}
histogram.first = "cloud_from_object.vfh";
//let look for the match
flann::Index<flann::ChiSquareDistance<float> > index (data, flann::SavedIndexParams ("kdtree.idx"));
index.buildIndex ();
nearestKSearch (index, histogram, 16, k_indices, k_distances);
pcl::console::print_highlight ("The closest 16 neighbors are:\n");
for (int i = 0; i < 16; ++i)
{
Eigen::Vector4f centroid;
pcl::compute3DCentroid (*object, centroid);
std::cerr<<centroid[0]<<centroid[1]<<centroid[2]<<centroid[3]<<std::endl;
pcl::console::print_info (" %d - %s (%d) with a distance of: %f\n",
i, models.at (k_indices[0][i]).first.c_str (), k_indices[0][i], k_distances[0][i]);
guesses.push_back(models.at (k_indices[0][i]).first);
}
}
bool cloud_cb(data_collection::process_cloud::Request &req,
data_collection::process_cloud::Response &res)
{
//Segment from cloud:
//May need to tweak segmentation parameters to get just the cluster I'm looking for.
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
nrg_object_recognition::segmentation seg_srv;
seg_srv.request.scene = req.in_cloud;
seg_srv.request.min_x = -.75, seg_srv.request.max_x = .4;
seg_srv.request.min_y = -5, seg_srv.request.max_y = .5;
seg_srv.request.min_z = 0.0, seg_srv.request.max_z = 1.15;
seg_client.call(seg_srv);
//SegmentCloud(req.in_cloud, clouds);
//For parameter tweaking, may be good to write all cluseters to file here for examination in pcd viewer.
pcl::PointCloud<pcl::PointXYZ>::Ptr cluster (new pcl::PointCloud<pcl::PointXYZ>);
//cluster = clouds.at(0);
pcl::fromROSMsg(seg_srv.response.clusters.at(0), *cluster);
//std::cout << "found " << clouds.size() << " clusters.\n";
std::cout << "cluster has " << cluster->height*cluster->width << " points.\n";
//Write raw pcd file (objecName_angle.pcd)
std::stringstream fileName_ss;
fileName_ss << "data/" << req.objectName << "_" << req.angle << ".pcd";
std::cout << "writing raw cloud to file...\n";
std::cout << fileName_ss.str() << std::endl;
pcl::io::savePCDFile(fileName_ss.str(), *cluster);
std::cout << "done.\n";
//Write vfh feature to file:
pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh;
vfh.setInputCloud (cluster);
//Estimate normals:
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cluster);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (0.03);
ne.compute (*cloud_normals);
vfh.setInputNormals (cloud_normals);
//Estimate vfh:
vfh.setSearchMethod (tree);
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs (new pcl::PointCloud<pcl::VFHSignature308> ());
// Compute the feature
vfh.compute (*vfhs);
//Write to file: (objectName_angle_vfh.pcd)
fileName_ss.str("");
std::cout << "writing vfh descriptor to file...\n";
fileName_ss << "data/" << req.objectName << "_" << req.angle << "_vfh.pcd";
pcl::io::savePCDFile(fileName_ss.str(), *vfhs);
std::cout << "done.\n";
//Extract cph
std::vector<float> feature;
CPHEstimation cph(5,72);
cph.setInputCloud(cluster);
cph.compute(feature);
//Write cph to file. (objectName_angle.csv)
std::ofstream outFile;
fileName_ss.str("");
fileName_ss << "data/" << req.objectName << "_" << req.angle << ".csv";
outFile.open(fileName_ss.str().c_str());
std::cout << "writing cph descriptor to file...\n";
for(unsigned int j=0; j<feature.size(); j++){
outFile << feature.at(j) << " ";
}
outFile.close();
fileName_ss.str("");
std::cout << "done.\n";
res.result = 1;
}
/** \brief Returns closest poses of of objects in training data to the query object
\param -q the path to the input point cloud
\param -k the number of nearest neighbors to return
*/
int main (int argc, char **argv)
{
//parse data directory
std::string queryCloudName;
pcl::console::parse_argument (argc, argv, "-q", queryCloudName);
boost::filesystem::path queryCloudPath(queryCloudName);
//parse number of nearest neighbors k
int k = 1;
pcl::console::parse_argument (argc, argv, "-k", k);
pcl::console::print_highlight ("using %d nearest neighbors.\n", k);
//read in point cloud
PointCloud::Ptr cloud (new PointCloud);
pcl::PCDReader reader;
//read ply file
pcl::PolygonMesh triangles;
if(queryCloudPath.extension().native().compare(".ply") == 0)
{
if( pcl::io::loadPolygonFilePLY(queryCloudPath.native(), triangles) == -1)
{
PCL_ERROR("Could not read .ply file\n");
return -1;
}
#if PCL17
pcl::fromPCLPointCloud2(triangles.cloud, *cloud);
#endif
#if PCL16
pcl::fromROSMsg(triangles.cloud, *cloud);
#endif
}
//read pcd file
else if(queryCloudPath.extension().native().compare(".pcd") == 0)
{
if( reader.read(queryCloudPath.native(), *cloud) == -1)
{
PCL_ERROR("Could not read .pcd file\n");
return -1;
}
}
else
{
PCL_ERROR("File must have extension .ply or .pcd\n");
return -1;
}
//Move point cloud so it is is centered at the origin
Eigen::Matrix<float,4,1> centroid;
pcl::compute3DCentroid(*cloud, centroid);
pcl::demeanPointCloud(*cloud, centroid, *cloud);
//Estimate normals
Normals::Ptr normals (new Normals);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normEst;
normEst.setInputCloud(cloud);
pcl::search::KdTree<pcl::PointXYZ>::Ptr normTree (new pcl::search::KdTree<pcl::PointXYZ>);
normEst.setSearchMethod(normTree);
normEst.setRadiusSearch(0.005);
normEst.compute(*normals);
//Create VFH estimation class
pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh;
vfh.setInputCloud(cloud);
vfh.setInputNormals(normals);
pcl::search::KdTree<pcl::PointXYZ>::Ptr vfhsTree (new pcl::search::KdTree<pcl::PointXYZ>);
vfh.setSearchMethod(vfhsTree);
//calculate VFHS features
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs (new pcl::PointCloud<pcl::VFHSignature308>);
vfh.setViewPoint(1, 0, 0);
vfh.compute(*vfhs);
//filenames
std::string featuresFileName = "training_features.h5";
std::string anglesFileName = "training_angles.list";
std::string kdtreeIdxFileName = "training_kdtree.idx";
//allocate flann matrices
std::vector<CloudInfo> cloudInfoList;
std::vector<PointCloud::Ptr> cloudList;
cloudList.resize(k);
flann::Matrix<int> k_indices;
flann::Matrix<float> k_distances;
flann::Matrix<float> data;
//load training data angles list
loadAngleData(cloudInfoList, anglesFileName);
flann::load_from_file (data, featuresFileName, "training_data");
flann::Index<flann::ChiSquareDistance<float> > index (data, flann::SavedIndexParams ("training_kdtree.idx"));
//perform knn search
index.buildIndex ();
nearestKSearch (index, vfhs, k, k_indices, k_distances);
// Output the results on screen
pcl::console::print_highlight ("The closest %d neighbors are:\n", k);
for (int i = 0; i < k; ++i)
{
//print nearest neighbor info to screen
pcl::console::print_info (" %d - theta = %f, phi = %f (%s) with a distance of: %f\n",
i,
cloudInfoList.at(k_indices[0][i]).theta*180.0/M_PI,
cloudInfoList.at(k_indices[0][i]).phi*180.0/M_PI,
cloudInfoList.at(k_indices[0][i]).filePath.c_str(),
k_distances[0][i]);
//retrieve pointcloud and store in list
PointCloud::Ptr cloudMatch (new PointCloud);
reader.read(cloudInfoList.at(k_indices[0][i]).filePath.native(), *cloudMatch);
//Move point cloud so it is is centered at the origin
pcl::compute3DCentroid(*cloudMatch, centroid);
pcl::demeanPointCloud(*cloudMatch, centroid, *cloudMatch);
cloudList[i] = cloudMatch;
}
//visualize histogram
/*
pcl::visualization::PCLHistogramVisualizer histvis;
histvis.addFeatureHistogram<pcl::VFHSignature308> (*vfhs, histLength);
*/
//Visualize point cloud and matches
//viewpoint cals
int y_s = (int)std::floor (sqrt ((double)k));
int x_s = y_s + (int)std::ceil ((k / (double)y_s) - y_s);
double x_step = (double)(1 / (double)x_s);
double y_step = (double)(1 / (double)y_s);
int viewport = 0, l = 0, m = 0;
std::string viewName = "match number ";
//setup visualizer and add query cloud
pcl::visualization::PCLVisualizer visu("KNN search");
visu.createViewPort (l * x_step, m * y_step, (l + 1) * x_step, (m + 1) * y_step, viewport);
visu.addPointCloud<pcl::PointXYZ> (cloud, ColorHandler(cloud, 0.0 , 255.0, 0.0), "Query Cloud Cloud", viewport);
visu.addText ("Query Cloud", 20, 30, 136.0/255.0, 58.0/255.0, 1, "Query Cloud", viewport);
visu.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_FONT_SIZE, 18, "Query Cloud", viewport);
visu.addCoordinateSystem (0.05, 0);
//add matches to plot
for(int i = 1; i < (k+1); ++i)
{
//shift viewpoint
++l;
if (l >= x_s)
{
l = 0;
m++;
}
//names and text labels
std::string textString = viewName;
std::string cloudname = viewName;
textString.append(boost::lexical_cast<std::string>(i));
cloudname.append(boost::lexical_cast<std::string>(i)).append("cloud");
//color proportional to distance
//add cloud
visu.createViewPort (l * x_step, m * y_step, (l + 1) * x_step, (m + 1) * y_step, viewport);
visu.addPointCloud<pcl::PointXYZ> (cloudList[i-1], ColorHandler(cloudList[i-1], 0.0 , 255.0, 0.0), cloudname, viewport);
visu.addText (textString, 20, 30, 136.0/255.0, 58.0/255.0, 1, textString, viewport);
visu.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_FONT_SIZE, 18, textString, viewport);
}
visu.spin();
return 0;
}
