Why my Feature Matching code run so slow with bad result?

classic Classic list List threaded Threaded
2 messages Options
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Why my Feature Matching code run so slow with bad result?

qianertongre
Hello all, I am new to PCL. My professor need me to change parameters of existing codes to match a model in a scene. But when I try to modify these parameters, the codes take a long time to run with very bad result(find a wrong place or cannot find anything). I found a post http://www.pcl-users.org/program-hangs-when-computing-normals-td4030929.html with the similar problem that codes take a long time to run and the solution for this problem is to delete all the vn 1,0,0 v 0,0,0. But even after I changed my point cloud as this post said, the time I need to wait is still too long. Here is an example image to show my result. Lego My professor need me finish this parameters experiment quickly. I need your talent! Any ideas for changing parameters is appreciated. If the problem comes from the point cloud I get, then please tell me how to get the point cloud so that I can do this feature matching easily. Thank you so much for all the people reading this post carefully. The model and scene I use are in the attachment below called onlychair.pcd and chair.pcd: data.zip The code I am using is as below: #include <pcl/features/ppf.h> #include <pcl/io/pcd_io.h> #include <pcl/filters/voxel_grid.h> #include <pcl/features/normal_3d.h> #include <pcl/registration/ppf_registration.h> #include <pcl/visualization/pcl_visualizer.h> #include <pcl/segmentation/sac_segmentation.h> #include <pcl/filters/extract_indices.h> #include <pcl/sample_consensus/method_types.h> #include <pcl/sample_consensus/model_types.h> using namespace pcl; using namespace std; const Eigen::Vector4f subsampling_leaf_size(0.02f, 0.02f, 0.02f, 0.0f);//0.02 const float normal_estimation_search_radius = 0.1f;//0.05 string model_file = "D:/FeatureMatching/data/onlychair.pcd";//4x8sTrackingModified//onlychair//flashhood_new_small string scene_file = "D:/FeatureMatching/data/chair.pcd";//LegoModified//chair//testmodified PointCloud::Ptr subsampleAndCalculateNormals(PointCloud::Ptr cloud, Eigen::Vector4f a) { PointCloud::Ptr cloud_subsampled(new PointCloud()); VoxelGrid subsampling_filter; subsampling_filter.setInputCloud(cloud); subsampling_filter.setLeafSize(a); subsampling_filter.filter(*cloud_subsampled); PointCloud::Ptr cloud_subsampled_normals(new PointCloud()); NormalEstimation<PointXYZ, Normal> normal_estimation_filter; normal_estimation_filter.setInputCloud(cloud_subsampled); search::KdTree::Ptr search_tree(new search::KdTree); normal_estimation_filter.setSearchMethod(search_tree); normal_estimation_filter.setRadiusSearch(normal_estimation_search_radius); normal_estimation_filter.compute(*cloud_subsampled_normals); PointCloud::Ptr cloud_subsampled_with_normals(new PointCloud()); concatenateFields(*cloud_subsampled, *cloud_subsampled_normals, *cloud_subsampled_with_normals); PCL_INFO("Cloud dimensions before / after subsampling: %u / %u\n", cloud->points.size(), cloud_subsampled->points.size()); return cloud_subsampled_with_normals; } int main(int argc, char** argv) { /// read point clouds from HDD PCL_INFO("Reading scene ...\n"); PointCloud::Ptr cloud_scene(new PointCloud()); PCDReader reader; reader.read(scene_file, *cloud_scene); PCL_INFO("Scene read: %s\n", scene_file); PCL_INFO("Reading models ...\n"); vector<PointCloud<PointXYZ>::Ptr > cloud_models; PointCloud::Ptr cloud(new PointCloud()); reader.read(model_file, *cloud); cloud_models.push_back(cloud); PCL_INFO("Model read: %s\n", model_file); pcl::SACSegmentation seg; pcl::ExtractIndices extract; seg.setOptimizeCoefficients(true); seg.setModelType(pcl::SACMODEL_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setMaxIterations(100);//1000 seg.setDistanceThreshold(0.05);//0.05 extract.setNegative(true); pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients()); pcl::PointIndices::Ptr inliers(new pcl::PointIndices()); unsigned nr_points = unsigned(cloud_scene->points.size()); while (cloud_scene->points.size() > 0.2 * nr_points)////////////////////////////////////////////////0.2 { seg.setInputCloud(cloud_scene); seg.segment(*inliers, *coefficients); PCL_INFO("Plane inliers: %u\n", inliers->indices.size()); if (inliers->indices.size() < 50000) break;//50000 extract.setInputCloud(cloud_scene); extract.setIndices(inliers); extract.filter(*cloud_scene); } Eigen::Vector4f subsampling_leaf_size_scene(0.025f, 0.025f, 0.025f, 0.0f);//downsample Eigen::Vector4f subsampling_leaf_size_model(0.03f, 0.03f, 0.03f, 0.0f); PointCloud::Ptr cloud_scene_input = subsampleAndCalculateNormals(cloud_scene, subsampling_leaf_size_scene); vector<PointCloud<PointNormal>::Ptr > cloud_models_with_normals; PCL_INFO("Training models ...\n"); vector hashmap_search_vector; for (size_t model_i = 0; model_i < cloud_models.size(); ++model_i) { PointCloud::Ptr cloud_model_input = subsampleAndCalculateNormals(cloud_models[model_i], subsampling_leaf_size_model); cloud_models_with_normals.push_back(cloud_model_input); PointCloud::Ptr cloud_model_ppf(new PointCloud()); PPFEstimation<PointNormal, PointNormal, PPFSignature> ppf_estimator; ppf_estimator.setInputCloud(cloud_model_input); ppf_estimator.setInputNormals(cloud_model_input); ppf_estimator.compute(*cloud_model_ppf); PPFHashMapSearch::Ptr hashmap_search(new PPFHashMapSearch(12.0f / 180.0f * float(M_PI),0.3f));///////////////////////////////////////12.0,0.5 hashmap_search->setInputFeatureCloud(cloud_model_ppf); hashmap_search_vector.push_back(hashmap_search); } visualization::PCLVisualizer viewer("PPF Object Recognition - Results"); viewer.setBackgroundColor(0, 0, 0); viewer.addPointCloud(cloud_scene); viewer.spinOnce(10); PCL_INFO("Registering models to scene ...\n"); for (size_t model_i = 0; model_i < cloud_models.size(); ++model_i) { //PCL_INFO("%u\n", model_i); PPFRegistration<PointNormal, PointNormal> ppf_registration; // set parameters for the PPF registration procedure ppf_registration.setSceneReferencePointSamplingRate(10);/////////////10 ppf_registration.setPositionClusteringThreshold(0.2f);//////////////////0.2 ppf_registration.setRotationClusteringThreshold(12.0f / 180.0f * float(M_PI));///////////////12 ppf_registration.setSearchMethod(hashmap_search_vector[model_i]); ppf_registration.setInputSource(cloud_models_with_normals[model_i]); ppf_registration.setInputTarget(cloud_scene_input); PointCloud cloud_output_subsampled; ppf_registration.align(cloud_output_subsampled); PointCloud::Ptr cloud_output_subsampled_xyz(new PointCloud()); for (size_t i = 0; i < cloud_output_subsampled.points.size(); ++i) cloud_output_subsampled_xyz->points.push_back(PointXYZ(cloud_output_subsampled.points[i].x, cloud_output_subsampled.points[i].y, cloud_output_subsampled.points[i].z)); Eigen::Matrix4f mat = ppf_registration.getFinalTransformation(); Eigen::Affine3f final_transformation(mat); // io::savePCDFileASCII ("output_subsampled_registered.pcd", cloud_output_subsampled); PointCloud::Ptr cloud_output(new PointCloud()); pcl::transformPointCloud(*cloud_models[model_i], *cloud_output, final_transformation); stringstream ss; ss << "model_" << model_i; visualization::PointCloudColorHandlerRandom random_color(cloud_output->makeShared()); viewer.addPointCloud(cloud_output, random_color, ss.str()); // io::savePCDFileASCII (ss.str ().c_str (), *cloud_output); PCL_INFO("Showing model %s\n", ss.str().c_str()); } PCL_INFO("All models have been registered!\n"); while (!viewer.wasStopped()) { viewer.spinOnce(100); boost::this_thread::sleep(boost::posix_time::microseconds(100000)); } return 0; } Thank you again for reading all the codes. Shenyang
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Why my Feature Matching code run so slow with bad result?

qianertongre
Sorry about the format of codes.

#include <pcl/features/ppf.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/registration/ppf_registration.h>

#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>


using namespace pcl;
using namespace std;



const Eigen::Vector4f subsampling_leaf_size(0.02f, 0.02f, 0.02f, 0.0f);//0.02
const float normal_estimation_search_radius = 0.1f;//0.05



string model_file = "D:/FeatureMatching/data/onlychair.pcd";//4x8sTrackingModified//onlychair//flashhood_new_small
string scene_file = "D:/FeatureMatching/data/chair.pcd";//LegoModified//chair//testmodified



PointCloud<PointNormal>::Ptr
subsampleAndCalculateNormals(PointCloud<PointXYZ>::Ptr cloud, Eigen::Vector4f a)

{

        PointCloud<PointXYZ>::Ptr cloud_subsampled(new PointCloud<PointXYZ>());

        VoxelGrid<PointXYZ> subsampling_filter;

        subsampling_filter.setInputCloud(cloud);

        subsampling_filter.setLeafSize(a);

        subsampling_filter.filter(*cloud_subsampled);



        PointCloud<Normal>::Ptr cloud_subsampled_normals(new PointCloud<Normal>());

        NormalEstimation<PointXYZ, Normal> normal_estimation_filter;

        normal_estimation_filter.setInputCloud(cloud_subsampled);

        search::KdTree<PointXYZ>::Ptr search_tree(new search::KdTree<PointXYZ>);

        normal_estimation_filter.setSearchMethod(search_tree);

        normal_estimation_filter.setRadiusSearch(normal_estimation_search_radius);

        normal_estimation_filter.compute(*cloud_subsampled_normals);



        PointCloud<PointNormal>::Ptr cloud_subsampled_with_normals(new PointCloud<PointNormal>());

        concatenateFields(*cloud_subsampled, *cloud_subsampled_normals, *cloud_subsampled_with_normals);



        PCL_INFO("Cloud dimensions before / after subsampling: %u / %u\n", cloud->points.size(), cloud_subsampled->points.size());

        return cloud_subsampled_with_normals;

}



int main(int argc, char** argv)

{





        /// read point clouds from HDD

        PCL_INFO("Reading scene ...\n");
        PointCloud<PointXYZ>::Ptr cloud_scene(new PointCloud<PointXYZ>());
        PCDReader reader;
        reader.read(scene_file, *cloud_scene);

        PCL_INFO("Scene read: %s\n", scene_file);
        PCL_INFO("Reading models ...\n");

        vector<PointCloud<PointXYZ>::Ptr > cloud_models;



        PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>());
        reader.read(model_file, *cloud);
        cloud_models.push_back(cloud);
        PCL_INFO("Model read: %s\n", model_file);




        pcl::SACSegmentation<pcl::PointXYZ> seg;
        pcl::ExtractIndices<pcl::PointXYZ> extract;

        seg.setOptimizeCoefficients(true);
        seg.setModelType(pcl::SACMODEL_PLANE);
        seg.setMethodType(pcl::SAC_RANSAC);
        seg.setMaxIterations(100);//1000
        seg.setDistanceThreshold(0.05);//0.05
        extract.setNegative(true);

        pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
        pcl::PointIndices::Ptr inliers(new pcl::PointIndices());

        unsigned nr_points = unsigned(cloud_scene->points.size());
        while (cloud_scene->points.size() > 0.2 * nr_points)////////////////////////////////////////////////0.2
        {

                seg.setInputCloud(cloud_scene);
                seg.segment(*inliers, *coefficients);
                PCL_INFO("Plane inliers: %u\n", inliers->indices.size());

                if (inliers->indices.size() < 50000) break;//50000
                extract.setInputCloud(cloud_scene);
                extract.setIndices(inliers);
                extract.filter(*cloud_scene);

        }


        Eigen::Vector4f subsampling_leaf_size_scene(0.025f, 0.025f, 0.025f, 0.0f);//downsample
        Eigen::Vector4f subsampling_leaf_size_model(0.03f, 0.03f, 0.03f, 0.0f);
        PointCloud<PointNormal>::Ptr cloud_scene_input = subsampleAndCalculateNormals(cloud_scene, subsampling_leaf_size_scene);
        vector<PointCloud<PointNormal>::Ptr > cloud_models_with_normals;
        PCL_INFO("Training models ...\n");

        vector<PPFHashMapSearch::Ptr> hashmap_search_vector;
        for (size_t model_i = 0; model_i < cloud_models.size(); ++model_i)
        {

                PointCloud<PointNormal>::Ptr cloud_model_input = subsampleAndCalculateNormals(cloud_models[model_i], subsampling_leaf_size_model);
                cloud_models_with_normals.push_back(cloud_model_input);


                PointCloud<PPFSignature>::Ptr cloud_model_ppf(new PointCloud<PPFSignature>());
                PPFEstimation<PointNormal, PointNormal, PPFSignature> ppf_estimator;
                ppf_estimator.setInputCloud(cloud_model_input);
                ppf_estimator.setInputNormals(cloud_model_input);
                ppf_estimator.compute(*cloud_model_ppf);



                PPFHashMapSearch::Ptr hashmap_search(new PPFHashMapSearch(12.0f / 180.0f * float(M_PI),0.3f));///////////////////////////////////////12.0,0.5

                hashmap_search->setInputFeatureCloud(cloud_model_ppf);
                hashmap_search_vector.push_back(hashmap_search);

        }





        visualization::PCLVisualizer viewer("PPF Object Recognition - Results");
        viewer.setBackgroundColor(0, 0, 0);
        viewer.addPointCloud(cloud_scene);
        viewer.spinOnce(10);
        PCL_INFO("Registering models to scene ...\n");
        for (size_t model_i = 0; model_i < cloud_models.size(); ++model_i)
        {
                //PCL_INFO("%u\n", model_i);


                PPFRegistration<PointNormal, PointNormal> ppf_registration;

                // set parameters for the PPF registration procedure

                ppf_registration.setSceneReferencePointSamplingRate(10);/////////////10
                ppf_registration.setPositionClusteringThreshold(0.2f);//////////////////0.2
                ppf_registration.setRotationClusteringThreshold(12.0f / 180.0f * float(M_PI));///////////////12
                ppf_registration.setSearchMethod(hashmap_search_vector[model_i]);
                ppf_registration.setInputSource(cloud_models_with_normals[model_i]);
                ppf_registration.setInputTarget(cloud_scene_input);



                PointCloud<PointNormal> cloud_output_subsampled;
                ppf_registration.align(cloud_output_subsampled);

                PointCloud<PointXYZ>::Ptr cloud_output_subsampled_xyz(new PointCloud<PointXYZ>());

                for (size_t i = 0; i < cloud_output_subsampled.points.size(); ++i)
                        cloud_output_subsampled_xyz->points.push_back(PointXYZ(cloud_output_subsampled.points[i].x, cloud_output_subsampled.points[i].y, cloud_output_subsampled.points[i].z));


                Eigen::Matrix4f mat = ppf_registration.getFinalTransformation();
                Eigen::Affine3f final_transformation(mat);

                //  io::savePCDFileASCII ("output_subsampled_registered.pcd", cloud_output_subsampled);
                PointCloud<PointXYZ>::Ptr cloud_output(new PointCloud<PointXYZ>());
                pcl::transformPointCloud(*cloud_models[model_i], *cloud_output, final_transformation);


                stringstream ss; ss << "model_" << model_i;
                visualization::PointCloudColorHandlerRandom<PointXYZ> random_color(cloud_output->makeShared());
                viewer.addPointCloud(cloud_output, random_color, ss.str());

                //    io::savePCDFileASCII (ss.str ().c_str (), *cloud_output);
                PCL_INFO("Showing model %s\n", ss.str().c_str());

        }



        PCL_INFO("All models have been registered!\n");





        while (!viewer.wasStopped())
        {

                viewer.spinOnce(100);

                boost::this_thread::sleep(boost::posix_time::microseconds(100000));

        }



        return 0;

}
Loading...