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]);
}
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;
};
