void ppfmap::CudaPPFMatch<PointT, NormalT>::getCorrespondences(
const PointCloudPtr cloud, const NormalsPtr normals,
pcl::Correspondences& correspondences) {
std::vector<Pose> poses;
detect(cloud, normals, poses);
for (const auto& pose : poses) {
correspondences.push_back(pose.c);
}
}
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::linkMatchWithBase (
const std::vector <int> &base_indices,
std::vector <int> &match_indices,
pcl::Correspondences &correspondences)
{
// calculate centroid of base and target
Eigen::Vector4f centre_base, centre_match;
pcl::compute3DCentroid (*target_, base_indices, centre_base);
pcl::compute3DCentroid (*input_, match_indices, centre_match);
PointTarget centre_pt_base;
centre_pt_base.x = centre_base[0];
centre_pt_base.y = centre_base[1];
centre_pt_base.z = centre_base[2];
PointSource centre_pt_match;
centre_pt_match.x = centre_match[0];
centre_pt_match.y = centre_match[1];
centre_pt_match.z = centre_match[2];
// find corresponding points according to their distance to the centroid
std::vector <int> copy = match_indices;
std::vector <int>::const_iterator it_base = base_indices.begin (), it_base_e = base_indices.end ();
std::vector <int>::iterator it_match, it_match_e = copy.end ();
std::vector <int>::iterator it_match_orig = match_indices.begin ();
for (; it_base != it_base_e; it_base++, it_match_orig++)
{
float dist_sqr_1 = pcl::squaredEuclideanDistance (target_->points[*it_base], centre_pt_base);
float best_diff_sqr = FLT_MAX;
int best_index = -1;
for (it_match = copy.begin (); it_match != it_match_e; it_match++)
{
// calculate difference of distances to centre point
float dist_sqr_2 = pcl::squaredEuclideanDistance (input_->points[*it_match], centre_pt_match);
float diff_sqr = std::abs(dist_sqr_1 - dist_sqr_2);
if (diff_sqr < best_diff_sqr)
{
best_diff_sqr = diff_sqr;
best_index = *it_match;
}
}
// assign new correspondence and update indices of matched targets
correspondences.push_back (pcl::Correspondence (best_index, *it_base, best_diff_sqr));
*it_match_orig = best_index;
}
}
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::bruteForceCorrespondences (
int idx1,
int idx2,
pcl::Correspondences &pairs)
{
const float max_norm_diff = 0.5f * max_norm_diff_ * M_PI / 180.f;
// calculate reference segment distance and normal angle
float ref_dist = pcl::euclideanDistance (target_->points[idx1], target_->points[idx2]);
float ref_norm_angle = (use_normals_ ? (target_normals_->points[idx1].getNormalVector3fMap () -
target_normals_->points[idx2].getNormalVector3fMap ()).norm () : 0.f);
// loop over all pairs of points in source point cloud
std::vector <int>::iterator it_out = source_indices_->begin (), it_out_e = source_indices_->end () - 1;
std::vector <int>::iterator it_in, it_in_e = source_indices_->end ();
for ( ; it_out != it_out_e; it_out++)
{
it_in = it_out + 1;
const PointSource *pt1 = &(*input_)[*it_out];
for ( ; it_in != it_in_e; it_in++)
{
const PointSource *pt2 = &(*input_)[*it_in];
// check point distance compared to reference dist (from base)
float dist = pcl::euclideanDistance (*pt1, *pt2);
if (std::abs(dist - ref_dist) < max_pair_diff_)
{
// add here normal evaluation if normals are given
if (use_normals_)
{
const NormalT *pt1_n = &(source_normals_->points[*it_out]);
const NormalT *pt2_n = &(source_normals_->points[*it_in]);
float norm_angle_1 = (pt1_n->getNormalVector3fMap () - pt2_n->getNormalVector3fMap ()).norm ();
float norm_angle_2 = (pt1_n->getNormalVector3fMap () + pt2_n->getNormalVector3fMap ()).norm ();
float norm_diff = std::min <float> (std::abs (norm_angle_1 - ref_norm_angle), std::abs (norm_angle_2 - ref_norm_angle));
if (norm_diff > max_norm_diff)
continue;
}
pairs.push_back (pcl::Correspondence (*it_in, *it_out, dist));
pairs.push_back (pcl::Correspondence (*it_out, *it_in, dist));
}
}
}
// return success if at least one correspondence was found
return (pairs.size () == 0 ? -1 : 0);
}
void
pcl::registration::CorrespondenceRejectionOrganizedBoundary::getRemainingCorrespondences (const pcl::Correspondences& original_correspondences,
pcl::Correspondences& remaining_correspondences)
{
pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud = boost::static_pointer_cast<pcl::registration::DataContainer<pcl::PointXYZ, pcl::PointNormal> >(data_container_)->getInputTarget ();
if (!cloud->isOrganized ())
{
PCL_ERROR ("[pcl::registration::CorrespondenceRejectionOrganizedBoundary::getRemainingCorrespondences] The target cloud is not organized.\n");
remaining_correspondences.clear ();
return;
}
remaining_correspondences.reserve (original_correspondences.size ());
for (size_t c_i = 0; c_i < original_correspondences.size (); ++c_i)
{
/// Count how many NaNs bound the target point
int x = original_correspondences[c_i].index_match % cloud->width;
int y = original_correspondences[c_i].index_match / cloud->width;
int nan_count_tgt = 0;
for (int x_d = -window_size_/2; x_d <= window_size_/2; ++x_d)
for (int y_d = -window_size_/2; y_d <= window_size_/2; ++y_d)
if (x + x_d >= 0 && x + x_d < cloud->width &&
y + y_d >= 0 && y + y_d < cloud->height)
{
if (!pcl_isfinite ((*cloud)(x + x_d, y + y_d).z) ||
fabs ((*cloud)(x, y).z - (*cloud)(x + x_d, y + y_d).z) > depth_step_threshold_)
nan_count_tgt ++;
}
if (nan_count_tgt >= boundary_nans_threshold_)
continue;
/// The correspondence passes both tests, add it to the filtered set of correspondences
remaining_correspondences.push_back (original_correspondences[c_i]);
}
}
