Пример #1
0
TEST (PCL, CorrespondenceRejectorSurfaceNormal)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr source (new pcl::PointCloud<pcl::PointXYZ>(cloud_source));
  pcl::PointCloud<pcl::PointXYZ>::Ptr target (new pcl::PointCloud<pcl::PointXYZ>(cloud_target));

  // re-do correspondence estimation
  boost::shared_ptr<pcl::Correspondences> correspondences (new pcl::Correspondences);
  pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ> corr_est;
  corr_est.setInputCloud (source);
  corr_est.setInputTarget (target);
  corr_est.determineCorrespondences (*correspondences);


  pcl::PointCloud<pcl::PointNormal>::Ptr source_normals(new pcl::PointCloud<pcl::PointNormal>);
  pcl::copyPointCloud(*source, *source_normals);
  pcl::PointCloud<pcl::PointNormal>::Ptr target_normals(new pcl::PointCloud<pcl::PointNormal>);
  pcl::copyPointCloud(*target, *target_normals);

  pcl::NormalEstimation<pcl::PointNormal, pcl::PointNormal> norm_est_src;
  norm_est_src.setSearchMethod (pcl::search::KdTree<pcl::PointNormal>::Ptr (new pcl::search::KdTree<pcl::PointNormal>));
  norm_est_src.setKSearch (10);
  norm_est_src.setInputCloud (source_normals);
  norm_est_src.compute (*source_normals);

  pcl::NormalEstimation<pcl::PointNormal, pcl::PointNormal> norm_est_tgt;
  norm_est_tgt.setSearchMethod (pcl::search::KdTree<pcl::PointNormal>::Ptr (new pcl::search::KdTree<pcl::PointNormal>));
  norm_est_tgt.setKSearch (10);
  norm_est_tgt.setInputCloud (target_normals);
  norm_est_tgt.compute (*target_normals);

  pcl::registration::CorrespondenceRejectorSurfaceNormal  corr_rej_surf_norm;
  corr_rej_surf_norm.initializeDataContainer <pcl::PointXYZ, pcl::PointNormal> ();
  corr_rej_surf_norm.setInputCloud <pcl::PointXYZ> (source);
  corr_rej_surf_norm.setInputTarget <pcl::PointXYZ> (target);
  corr_rej_surf_norm.setInputNormals <pcl::PointXYZ, pcl::PointNormal> (source_normals);
  corr_rej_surf_norm.setTargetNormals <pcl::PointXYZ, pcl::PointNormal> (target_normals);

  boost::shared_ptr<pcl::Correspondences>  correspondences_result_rej_surf_norm (new pcl::Correspondences);
  corr_rej_surf_norm.setInputCorrespondences (correspondences);
  corr_rej_surf_norm.setThreshold (0.5);

  corr_rej_surf_norm.getCorrespondences (*correspondences_result_rej_surf_norm);

  // check for correct matches
  if (int (correspondences_result_rej_surf_norm->size ()) == nr_correspondences_result_rej_dist)
    for (int i = 0; i < nr_correspondences_result_rej_dist; ++i)
      EXPECT_EQ ((*correspondences_result_rej_surf_norm)[i].index_match, correspondences_dist[i][1]);
}
Пример #2
0
    typename PointCloud<PointT>::Ptr transform(
            const typename PointCloud<PointT>::Ptr source,
            const typename PointCloud<PointT>::Ptr target,boost::shared_ptr<Configuration> configuration)
    {
        source_cloud = *source;
        target_cloud = *target;

        cout << "[PFHTransformStrategy::transform] Setting source cloud: "
            << source->size() << " points" << endl;
        cout << "[PFHTransformStrategy::transform] Setting target cloud: "
            << target->size() << " points" << endl;

        typename PointCloud<PointT>::Ptr transformed(new PointCloud<PointT>);

        typename PointCloud<PointXYZRGB>::Ptr source_points(source);
        typename PointCloud<PointXYZRGB>::Ptr source_filtered(
                new PointCloud<PointT>);
        PointCloud<Normal>::Ptr source_normals(new PointCloud<Normal>);
        PointCloud<PointWithScale>::Ptr source_keypoints(
                new PointCloud<PointWithScale>);
        PointCloud<PFHSignature125>::Ptr source_descriptors(
                new PointCloud<PFHSignature125>);

        typename PointCloud<PointT>::Ptr target_points(target);
        typename PointCloud<PointT>::Ptr target_filtered(
                new PointCloud<PointT>);
        PointCloud<Normal>::Ptr target_normals(
                new PointCloud<Normal>);
        PointCloud<PointWithScale>::Ptr target_keypoints(
                new PointCloud<PointWithScale>);
        PointCloud<PFHSignature125>::Ptr target_descriptors(
                new PointCloud<PFHSignature125>);

        cout << "[PFHTransformStrategy::transform] Downsampling source and "
            << "target clouds..." << endl;

        //filter(source_points, source_filtered, 0.01f);
        //filter(target_points, target_filtered, 0.01f);
        source_filtered=source_points;
        target_filtered=target_points;
        cout << "[PFHTransformStrategy::transform] Creating normals for "
            << "source and target cloud..." << endl;

        create_normals<Normal>(source_filtered, source_normals);
        create_normals<Normal>(target_filtered, target_normals);

        cout << "[PFHTransformStrategy::transform] Finding keypoints in "
            << "source and target cloud..." << endl;

        detect_keypoints(source_filtered, source_keypoints);
        detect_keypoints(target_filtered, target_keypoints);

        for(PointWithScale p: source_keypoints->points){
        	cout<<"keypoint "<<p;
        }

        vector<int> source_indices(source_keypoints->points.size());
        vector<int> target_indices(target_keypoints->points.size());

        cout << "[PFHTransformStrategy::transform] Computing PFH features "
            << "for source and target cloud..." << endl;

        compute_PFH_features_at_keypoints(source_filtered, source_normals,source_keypoints,
                source_descriptors, target_indices);
        compute_PFH_features_at_keypoints(target_filtered, target_normals,target_keypoints,
                target_descriptors, target_indices);

        vector<int> correspondences;
        vector<float> correspondence_scores;

        find_feature_correspondence(source_descriptors, target_descriptors,
                correspondences, correspondence_scores);

        cout << "correspondences: " << correspondences.size() << endl;
        cout << "c. scores: " << correspondence_scores.size() << endl;

        cout << "First cloud: Found " << source_keypoints->size() << " keypoints "
            << "out of " << source_filtered->size() << " total points." << endl;
        cout << "Second cloud: Found " << target_keypoints->size() << " keypoints"
            << " out of " << target_filtered->size() << " total points." << endl;

        // Start with the actual transformation. Yeay :)
        TransformationFromCorrespondences tfc;
        tfc.reset();

        vector<int> sorted_scores;
        cv::sortIdx(correspondence_scores, sorted_scores, CV_SORT_EVERY_ROW);

        vector<float> tmp(correspondence_scores);
        sort(tmp.begin(), tmp.end());

        float median_score = tmp[tmp.size() / 2];
        vector<int> fidx;
        vector<int> fidxt;

        Eigen::Vector3f source_position(0, 0, 0);
        Eigen::Vector3f target_position(0, 0, 0);
        for (size_t i = 0; i < correspondence_scores.size(); i++) {
            int index = sorted_scores[i];
            if (median_score >= correspondence_scores[index]) {
                source_position[0] = source_keypoints->points[index].x;
                source_position[1] = source_keypoints->points[index].y;
                source_position[2] = source_keypoints->points[index].z;

                target_position[0] = target_keypoints->points[index].x;
                target_position[1] = target_keypoints->points[index].y;
                target_position[2] = target_keypoints->points[index].z;

                if (abs(source_position[1] - target_position[1]) > 0.2) {
                    continue;
                }
               // cout<< "abs position difference:"<<abs(source_position[1] - target_position[1])<<"C-Score"<<correspondence_scores[index]<<endl;
               // cout<<" Source Position " <<source_position<<endl;
               // cout<<" target position "<<target_position<<endl;
                tfc.add(source_position, target_position,
                        correspondence_scores[index]);
                fidx.push_back(source_indices[index]);
                fidxt.push_back(target_indices[correspondences[index]]);
            }
        }
        cout << "TFC samples: "<<tfc.getNoOfSamples()<<" AccumulatedWeight "<<tfc.getAccumulatedWeight()<<endl;
        Eigen::Affine3f tr;
        tr = tfc.getTransformation();
        cout << "TFC transformation: " << endl;
        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                cout << tr.rotation()(i, i) << "\t";
            }
            cout << endl;
        }

        transformPointCloud(*source_filtered, *transformed,
                tfc.getTransformation());
        cout << "transformation finished";
        return transformed;
    };