/** \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; }