/** \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);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCDReader reader;
if(reader.read(queryCloudPath.native(), *cloud) == -1)
return -1;
VFHPoseEstimator poseEstimator;
float roll, pitch, yaw;
poseEstimator.getPose(cloud, roll, pitch, yaw, true);
std::cout << roll << " " << pitch << " " << yaw << std::endl;
return 0;
}
/** \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;
}