Exemplo n.º 1
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);

    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;
}
Exemplo n.º 2
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;
}