Exemple #1
0
inline void
pcl::registration::getMatchIndices (const pcl::Correspondences& correspondences, std::vector<int>& indices)
{
  indices.resize (correspondences.size ());
  for (size_t i = 0; i < correspondences.size (); ++i)
    indices[i] = correspondences[i].index_match;
}
template <typename PointSource, typename PointTarget> inline void
TransformationEstimationJointOptimize<PointSource, PointTarget>::estimateRigidTransformation (
		const pcl::PointCloud<PointSource> &cloud_src,
		const pcl::PointCloud<PointTarget> &cloud_tgt,
		const pcl::Correspondences &correspondences,
		const pcl::Correspondences &correspondences_dfp,
		float alpha_arg,
		Eigen::Matrix4f &transformation_matrix)
{
	const int nr_correspondences = (int)correspondences.size();
	std::vector<int> indices_src(nr_correspondences);
	std::vector<int> indices_tgt(nr_correspondences);
	for (int i = 0; i < nr_correspondences; ++i)
	{
		indices_src[i] = correspondences[i].index_query;
		indices_tgt[i] = correspondences[i].index_match;
	}

	const int nr_correspondences_dfp = (int)correspondences_dfp.size();
	std::vector<int> indices_src_dfp(nr_correspondences_dfp);
	std::vector<int> indices_tgt_dfp(nr_correspondences_dfp);
	for (int i = 0; i < nr_correspondences_dfp; ++i)
	{
		indices_src_dfp[i] = correspondences_dfp[i].index_query;
		indices_tgt_dfp[i] = correspondences_dfp[i].index_match;
	}
	estimateRigidTransformation(cloud_src, indices_src, indices_src_dfp, cloud_tgt, indices_tgt, indices_tgt_dfp,
			alpha_arg, transformation_matrix);
}
void
pcl::registration::CorrespondenceRejectorDistance::getRemainingCorrespondences (
    const pcl::Correspondences& original_correspondences,
    pcl::Correspondences& remaining_correspondences)
{
  unsigned int number_valid_correspondences = 0;
  remaining_correspondences.resize (original_correspondences.size ());
  for (size_t i = 0; i < original_correspondences.size (); ++i)
  {
    if (data_container_)
    {
      if (data_container_->getCorrespondenceScore (original_correspondences[i]) < max_distance_)
      {
        remaining_correspondences[number_valid_correspondences] = original_correspondences[i];
        ++number_valid_correspondences;
      }
    }
    else
    {
      if (original_correspondences[i].distance < max_distance_)
      {
        remaining_correspondences[number_valid_correspondences] = original_correspondences[i];
        ++number_valid_correspondences;
      }
    }
  }
  remaining_correspondences.resize (number_valid_correspondences);
}
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorrespondences (
    pcl::Correspondences &correspondences, double max_distance)
{
  if (!initCompute ())
    return;

  double max_dist_sqr = max_distance * max_distance;

  typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget;
  correspondences.resize (indices_->size ());

  std::vector<int> index (1);
  std::vector<float> distance (1);
  pcl::Correspondence corr;
  unsigned int nr_valid_correspondences = 0;
  
  // Check if the template types are the same. If true, avoid a copy.
  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
  if (isSamePointType<PointSource, PointTarget> ())
  {
    // Iterate over the input set of source indices
    for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
    {
      tree_->nearestKSearch (input_->points[*idx], 1, index, distance);
      if (distance[0] > max_dist_sqr)
        continue;

      corr.index_query = *idx;
      corr.index_match = index[0];
      corr.distance = distance[0];
      correspondences[nr_valid_correspondences++] = corr;
    }
  }
  else
  {
    PointTarget pt;
    
    // Iterate over the input set of source indices
    for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
    {
      // Copy the source data to a target PointTarget format so we can search in the tree
      pcl::for_each_type <FieldListTarget> (pcl::NdConcatenateFunctor <PointSource, PointTarget> (
            input_->points[*idx], 
            pt));

      tree_->nearestKSearch (pt, 1, index, distance);
      if (distance[0] > max_dist_sqr)
        continue;

      corr.index_query = *idx;
      corr.index_match = index[0];
      corr.distance = distance[0];
      correspondences[nr_valid_correspondences++] = corr;
    }
  }
  correspondences.resize (nr_valid_correspondences);
  deinitCompute ();
}
Exemple #5
0
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);
}
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
                             const pcl::PointCloud<PointTarget> &cloud_tgt,
                             const pcl::Correspondences &correspondences,
                             Matrix4 &transformation_matrix) const
{
  ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
  ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
  std::vector<Scalar> weights (correspondences.size ());
  for (size_t i = 0; i < correspondences.size (); ++i)
    weights[i] = correspondences[i].weight;
  typename std::vector<Scalar>::const_iterator weights_it = weights.begin ();
  estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
}
Exemple #7
0
inline void
pcl::registration::getCorDistMeanStd (const pcl::Correspondences &correspondences, double &mean, double &stddev)
{
  if (correspondences.empty ())
    return;

  double sum = 0, sq_sum = 0;

  for (size_t i = 0; i < correspondences.size (); ++i)
  {
    sum += correspondences[i].distance;
    sq_sum += correspondences[i].distance * correspondences[i].distance;
  }
  mean = sum / static_cast<double> (correspondences.size ());
  double variance = (sq_sum - sum * sum / static_cast<double> (correspondences.size ())) / static_cast<double> (correspondences.size () - 1);
  stddev = sqrt (variance);
}
Exemple #8
0
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);
  }
}
Eigen::Matrix4f SHOTObjectGenerator::computeTransformationSAC(const pcl::PointCloud<PointXYZSHOT>::ConstPtr &cloud_src, const pcl::PointCloud<PointXYZSHOT>::ConstPtr &cloud_trg,
		const pcl::CorrespondencesConstPtr& correspondences, pcl::Correspondences& inliers)
{
	CLOG(LTRACE) << "Computing SAC" << std::endl ;

	pcl::registration::CorrespondenceRejectorSampleConsensus<PointXYZSHOT> sac ;
	sac.setInputSource(cloud_src) ;
	sac.setInputTarget(cloud_trg) ;
	sac.setInlierThreshold(0.001f) ;
	sac.setMaximumIterations(2000) ;
	sac.setInputCorrespondences(correspondences) ;
	sac.getCorrespondences(inliers) ;

	CLOG(LINFO) << "SAC inliers " << inliers.size();

	if ( ((float)inliers.size()/(float)correspondences->size()) >85)
		return Eigen::Matrix4f::Identity();
	return sac.getBestTransformation() ;
}
void
pcl::registration::CorrespondenceRejectorMedianDistance::getRemainingCorrespondences (
    const pcl::Correspondences& original_correspondences, 
    pcl::Correspondences& remaining_correspondences)
{
  std::vector <double> dists;
  dists.resize (original_correspondences.size ());

  for (size_t i = 0; i < original_correspondences.size (); ++i)
  {
    if (data_container_)
      dists[i] = data_container_->getCorrespondenceScore (original_correspondences[i]);
    else
      dists[i] = original_correspondences[i].distance;
  }

  std::vector <double> nth (dists);
  nth_element (nth.begin (), nth.begin () + (nth.size () / 2), nth.end ());
  median_distance_ = nth [nth.size () / 2];

  unsigned int number_valid_correspondences = 0;
  remaining_correspondences.resize (original_correspondences.size ());

  for (size_t i = 0; i < original_correspondences.size (); ++i)
    if (dists[i] <= median_distance_ * factor_)
      remaining_correspondences[number_valid_correspondences++] = original_correspondences[i];
  remaining_correspondences.resize (number_valid_correspondences);
}
void
pcl::getRejectedQueryIndices (const pcl::Correspondences &correspondences_before,
                              const pcl::Correspondences &correspondences_after,
                              std::vector<int>& indices,
                              bool presorting_required)
{
  indices.clear();

  const int nr_correspondences_before = static_cast<int> (correspondences_before.size ());
  const int nr_correspondences_after = static_cast<int> (correspondences_after.size ());

  if (nr_correspondences_before == 0)
    return;
  else if (nr_correspondences_after == 0)
  {
    indices.resize(nr_correspondences_before);
    for (int i = 0; i < nr_correspondences_before; ++i)
      indices[i] = correspondences_before[i].index_query;
    return;
  }

  std::vector<int> indices_before (nr_correspondences_before);
  for (int i = 0; i < nr_correspondences_before; ++i)
    indices_before[i] = correspondences_before[i].index_query;

  std::vector<int> indices_after (nr_correspondences_after);
  for (int i = 0; i < nr_correspondences_after; ++i)
    indices_after[i] = correspondences_after[i].index_query;

  if (presorting_required)
  {
    std::sort (indices_before.begin (), indices_before.end ());
    std::sort (indices_after.begin (), indices_after.end ());
  }

  set_difference (
      indices_before.begin (), indices_before.end (),
      indices_after.begin (),  indices_after.end (),
      inserter (indices, indices.begin ()));
}
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]);
  }
}
void
pcl::registration::CorrespondenceRejectorSurfaceNormal::getRemainingCorrespondences (
    const pcl::Correspondences& original_correspondences,
    pcl::Correspondences& remaining_correspondences)
{
  if (!data_container_)
  {
    PCL_ERROR ("[pcl::registratin::%s::getRemainingCorrespondences] DataContainer object is not initialized!\n", getClassName ().c_str ());
    return;
  }

  unsigned int number_valid_correspondences = 0;
  remaining_correspondences.resize (original_correspondences.size ());

  // Test each correspondence
  for (const auto &original_correspondence : original_correspondences)
  {
    if (data_container_->getCorrespondenceScoreFromNormals (original_correspondence) > threshold_)
      remaining_correspondences[number_valid_correspondences++] = original_correspondence;
  }
  remaining_correspondences.resize (number_valid_correspondences);
}
Exemple #14
0
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;
  }
}
void
pcl::registration::CorrespondenceRejectorSurfaceNormal::getRemainingCorrespondences (
    const pcl::Correspondences& original_correspondences,
    pcl::Correspondences& remaining_correspondences)
{
  if (!data_container_)
  {
    PCL_ERROR ("[pcl::registratin::%s::getRemainingCorrespondences] DataContainer object is not initialized!\n", getClassName ().c_str ());
    return;
  }

  unsigned int number_valid_correspondences = 0;
  remaining_correspondences.resize (original_correspondences.size ());

  // Test each correspondence
  for (size_t i = 0; i < original_correspondences.size (); ++i)
  {
    if (boost::static_pointer_cast<DataContainer<pcl::PointXYZ, pcl::PointNormal> >
        (data_container_)->getCorrespondenceScoreFromNormals (original_correspondences[i]) > threshold_)
      remaining_correspondences[number_valid_correspondences++] = original_correspondences[i];
  }
  remaining_correspondences.resize (number_valid_correspondences);
}
template <typename PointSource, typename PointTarget> inline void
TransformationEstimationJointOptimize<PointSource, PointTarget>::setCorrespondecesDFP ( pcl::Correspondences correspondences_dfp ) {
	const int nr_correspondences_dfp = (int)correspondences_dfp.size();
	indices_src_dfp_.resize(nr_correspondences_dfp);
	indices_tgt_dfp_.resize(nr_correspondences_dfp);
	for (int i = 0; i < nr_correspondences_dfp; ++i)
	{
		indices_src_dfp_[i] = correspondences_dfp[i].index_query;
		indices_tgt_dfp_[i] = correspondences_dfp[i].index_match;
	}
	// Update flags
	indices_src_dfp_set_ = true;
	indices_tgt_dfp_set_ = true;
}
template <typename PointSource, typename PointTarget> void
pcl::registration::CorrespondenceEstimation<PointSource, PointTarget>::determineCorrespondences (
    pcl::Correspondences &correspondences, float max_distance)
{
  typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget;

  if (!initCompute ())
    return;

  if (!target_)
  {
    PCL_WARN ("[pcl::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
    return;
  }

  float max_dist_sqr = max_distance * max_distance;

  correspondences.resize (indices_->size ());
  std::vector<int> index (1);
  std::vector<float> distance (1);
  pcl::Correspondence corr;
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Copy the source data to a target PointTarget format so we can search in the tree
    PointTarget pt;
    pcl::for_each_type <FieldListTarget> (pcl::NdConcatenateFunctor <PointSource, PointTarget> (
          input_->points[(*indices_)[i]], 
          pt));

    //if (tree_->nearestKSearch (input_->points[(*indices_)[i]], 1, index, distance))
    if (tree_->nearestKSearch (pt, 1, index, distance))
    {
      if (distance[0] <= max_dist_sqr)
      {
        corr.index_query = static_cast<int> (i);
        corr.index_match = index[0];
        corr.distance = distance[0];
        correspondences[i] = corr;
        continue;
      }
    }
//    correspondences[i] = pcl::Correspondence(i, -1, std::numeric_limits<float>::max());
  }
  deinitCompute ();
}
template <typename PointSource, typename PointTarget, typename MatScalar> inline void
pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::estimateRigidTransformation (
    const pcl::PointCloud<PointSource> &cloud_src,
    const pcl::PointCloud<PointTarget> &cloud_tgt,
    const pcl::Correspondences &correspondences,
    Matrix4 &transformation_matrix) const
{
  const int nr_correspondences = static_cast<const int> (correspondences.size ());
  std::vector<int> indices_src (nr_correspondences);
  std::vector<int> indices_tgt (nr_correspondences);
  for (int i = 0; i < nr_correspondences; ++i)
  {
    indices_src[i] = correspondences[i].index_query;
    indices_tgt[i] = correspondences[i].index_match;
  }

  estimateRigidTransformation (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
}
template <typename PointSource, typename PointTarget> inline void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
                             const pcl::PointCloud<PointTarget> &cloud_tgt,
                             const pcl::Correspondences &correspondences,
                             Eigen::Matrix4f &transformation_matrix)
{
  size_t nr_points = correspondences.size ();

  // Approximate as a linear least squares problem
  Eigen::MatrixXf A (nr_points, 6);
  Eigen::MatrixXf b (nr_points, 1);
  for (size_t i = 0; i < nr_points; ++i)
  {
    const int & src_idx = correspondences[i].index_query;
    const int & tgt_idx = correspondences[i].index_match;
    const float & sx = cloud_src.points[src_idx].x;
    const float & sy = cloud_src.points[src_idx].y;
    const float & sz = cloud_src.points[src_idx].z;
    const float & dx = cloud_tgt.points[tgt_idx].x;
    const float & dy = cloud_tgt.points[tgt_idx].y;
    const float & dz = cloud_tgt.points[tgt_idx].z;
    const float & nx = cloud_tgt.points[tgt_idx].normal[0];
    const float & ny = cloud_tgt.points[tgt_idx].normal[1];
    const float & nz = cloud_tgt.points[tgt_idx].normal[2];
    A (i, 0) = nz*sy - ny*sz;
    A (i, 1) = nx*sz - nz*sx; 
    A (i, 2) = ny*sx - nx*sy;
    A (i, 3) = nx;
    A (i, 4) = ny;
    A (i, 5) = nz;
    b (i, 0) = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
  }

  // Solve A*x = b
  Eigen::VectorXf x = A.colPivHouseholderQr ().solve (b);
  
  // Construct the transformation matrix from x
  constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
}
void
pcl::registration::CorrespondenceRejectorVarTrimmed::getRemainingCorrespondences (
    const pcl::Correspondences& original_correspondences, 
    pcl::Correspondences& remaining_correspondences)
{
  std::vector <double> dists;
  dists.resize (original_correspondences.size ());

  for (size_t i = 0; i < original_correspondences.size (); ++i)
  {
    if (data_container_)
    {
      dists[i] = data_container_->getCorrespondenceScore (original_correspondences[i]);
    }
    else
    {
      dists[i] = original_correspondences[i].distance;
    }
  }
  factor_ = optimizeInlierRatio (dists);
  nth_element (dists.begin (), dists.begin () + int (double (dists.size ()) * factor_), dists.end ());
  trimmed_distance_ = dists [int (double (dists.size ()) * factor_)];

  unsigned int number_valid_correspondences = 0;
  remaining_correspondences.resize (original_correspondences.size ());

  for (size_t i = 0; i < original_correspondences.size (); ++i)
  {
    if ( dists[i] < trimmed_distance_)
    {
      remaining_correspondences[number_valid_correspondences] = original_correspondences[i];
      ++number_valid_correspondences;
    }
  }
  remaining_correspondences.resize (number_valid_correspondences);
}
Exemple #21
0
template <typename PointT> bool
pcl::visualization::ImageViewer::showCorrespondences (
  const pcl::PointCloud<PointT> &source_img,
  const pcl::PointCloud<PointT> &target_img,
  const pcl::Correspondences &correspondences,
  int nth,
  const std::string &layer_id)
{
  if (correspondences.empty ())
  {
    PCL_DEBUG ("[pcl::visualization::ImageViewer::addCorrespondences] An empty set of correspondences given! Nothing to display.\n");
    return (false);
  }

  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
  if (am_it == layer_map_.end ())
  {
    PCL_DEBUG ("[pcl::visualization::ImageViewer::addCorrespondences] No layer with ID='%s' found. Creating new one...\n", layer_id.c_str ());
    am_it = createLayer (layer_id, source_img.width + target_img.width, std::max (source_img.height, target_img.height), 1.0, false);
  }
 
  int src_size = source_img.width * source_img.height * 3;
  int tgt_size = target_img.width * target_img.height * 3;

  // Set window size
  setSize (source_img.width + target_img.width , std::max (source_img.height, target_img.height));

  // Set data size
  if (data_size_ < (src_size + tgt_size))
  {
    data_size_ = src_size + tgt_size;
    data_.reset (new unsigned char[data_size_]);
  }

  // Copy data in VTK format
  int j = 0;
  for (size_t i = 0; i < std::max (source_img.height, target_img.height); ++i)
  {
    // Still need to copy the source?
    if (i < source_img.height)
    {
      for (size_t k = 0; k < source_img.width; ++k)
      {
        data_[j++] = source_img[i * source_img.width + k].r;
        data_[j++] = source_img[i * source_img.width + k].g;
        data_[j++] = source_img[i * source_img.width + k].b;
      }
    }
    else
    {
      memcpy (&data_[j], 0, source_img.width * 3);
      j += source_img.width * 3;
    }

    // Still need to copy the target?
    if (i < source_img.height)
    {
      for (size_t k = 0; k < target_img.width; ++k)
      {
        data_[j++] = target_img[i * source_img.width + k].r;
        data_[j++] = target_img[i * source_img.width + k].g;
        data_[j++] = target_img[i * source_img.width + k].b;
      }
    }
    else
    {
      memcpy (&data_[j], 0, target_img.width * 3);
      j += target_img.width * 3;
    }
  }

  void* data = const_cast<void*> (reinterpret_cast<const void*> (data_.get ()));
  
  vtkSmartPointer<vtkImageData> image = vtkSmartPointer<vtkImageData>::New ();
  image->SetDimensions (source_img.width + target_img.width, std::max (source_img.height, target_img.height), 1);
  image->SetScalarTypeToUnsignedChar ();
  image->SetNumberOfScalarComponents (3);
  image->AllocateScalars ();
  image->GetPointData ()->GetScalars ()->SetVoidArray (data, data_size_, 1);
  vtkSmartPointer<PCLContextImageItem> image_item = vtkSmartPointer<PCLContextImageItem>::New ();
#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION < 10))
  // Now create filter and set previously created transformation
  algo_->SetInput (image);
  algo_->Update ();
  image_item->set (0, 0, algo_->GetOutput ());
#else
  image_item->set (0, 0, image);
  interactor_style_->adjustCamera (image, ren_);
#endif
  am_it->actor->GetScene ()->AddItem (image_item);
  image_viewer_->SetSize (image->GetDimensions ()[0], image->GetDimensions ()[1]);

  // Draw lines between the best corresponding points
  for (size_t i = 0; i < correspondences.size (); i += nth)
  {
    double r, g, b;
    getRandomColors (r, g, b);
    unsigned char u_r = static_cast<unsigned char> (255.0 * r);
    unsigned char u_g = static_cast<unsigned char> (255.0 * g);
    unsigned char u_b = static_cast<unsigned char> (255.0 * b);
    vtkSmartPointer<context_items::Circle> query_circle = vtkSmartPointer<context_items::Circle>::New ();
    query_circle->setColors (u_r, u_g, u_b);
    vtkSmartPointer<context_items::Circle> match_circle = vtkSmartPointer<context_items::Circle>::New ();
    match_circle->setColors (u_r, u_g, u_b);
    vtkSmartPointer<context_items::Line> line = vtkSmartPointer<context_items::Line>::New ();
    line->setColors (u_r, u_g, u_b);

    float query_x = correspondences[i].index_query % source_img.width;
    float match_x = correspondences[i].index_match % target_img.width + source_img.width;
#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION >= 10))
    float query_y = correspondences[i].index_query / source_img.width;
    float match_y = correspondences[i].index_match / target_img.width;
#else
    float query_y = getSize ()[1] - correspondences[i].index_query / source_img.width;
    float match_y = getSize ()[1] - correspondences[i].index_match / target_img.width;
#endif

    query_circle->set (query_x, query_y, 3.0);
    match_circle->set (match_x, match_y, 3.0);
    line->set (query_x, query_y, match_x, match_y);

    am_it->actor->GetScene ()->AddItem (query_circle);
    am_it->actor->GetScene ()->AddItem (match_circle);
    am_it->actor->GetScene ()->AddItem (line);
  }
  
  return (true);
}
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineReciprocalCorrespondences (
    pcl::Correspondences &correspondences, double max_distance)
{
  if (!initCompute ())
    return;
  
  typedef typename pcl::traits::fieldList<PointSource>::type FieldListSource;
  typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget;
  typedef typename pcl::intersect<FieldListSource, FieldListTarget>::type FieldList;
  
  // setup tree for reciprocal search
  pcl::KdTreeFLANN<PointSource> tree_reciprocal;
  // Set the internal point representation of choice
  if (point_representation_)
    tree_reciprocal.setPointRepresentation (point_representation_);

  tree_reciprocal.setInputCloud (input_, indices_);

  double max_dist_sqr = max_distance * max_distance;

  correspondences.resize (indices_->size());
  std::vector<int> index (1);
  std::vector<float> distance (1);
  std::vector<int> index_reciprocal (1);
  std::vector<float> distance_reciprocal (1);
  pcl::Correspondence corr;
  unsigned int nr_valid_correspondences = 0;
  int target_idx = 0;

  // Check if the template types are the same. If true, avoid a copy.
  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
  if (isSamePointType<PointSource, PointTarget> ())
  {
    // Iterate over the input set of source indices
    for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
    {
      tree_->nearestKSearch (input_->points[*idx], 1, index, distance);
      if (distance[0] > max_dist_sqr)
        continue;

      target_idx = index[0];

      tree_reciprocal.nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
      if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0])
        continue;

      corr.index_query = *idx;
      corr.index_match = index[0];
      corr.distance = distance[0];
      correspondences[nr_valid_correspondences++] = corr;
    }
  }
  else
  {
    PointTarget pt_src;
    PointSource pt_tgt;
   
    // Iterate over the input set of source indices
    for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
    {
      // Copy the source data to a target PointTarget format so we can search in the tree
      pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointSource, PointTarget> (
            input_->points[*idx], 
            pt_src));

      tree_->nearestKSearch (pt_src, 1, index, distance);
      if (distance[0] > max_dist_sqr)
        continue;

      target_idx = index[0];

      // Copy the target data to a target PointSource format so we can search in the tree_reciprocal
      pcl::for_each_type<FieldList> (pcl::NdConcatenateFunctor <PointTarget, PointSource> (
            target_->points[target_idx],
            pt_tgt));

      tree_reciprocal.nearestKSearch (pt_tgt, 1, index_reciprocal, distance_reciprocal);
      if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0])
        continue;

      corr.index_query = *idx;
      corr.index_match = index[0];
      corr.distance = distance[0];
      correspondences[nr_valid_correspondences++] = corr;
    }
  }
  correspondences.resize (nr_valid_correspondences);
  deinitCompute ();
}
	/** \brief Determine the correspondences between input and target cloud.
	 * \param[out] correspondences the found correspondences (index of query point, index of target point, distance)
	 * \param[in] max_distance maximum allowed distance between correspondences
	 */
	virtual void determineCorrespondences(pcl::Correspondences &correspondences,
			double max_distance = std::numeric_limits<double>::max()) {

		if (!initCompute())
			return;
		double max_xyz_dist_sqr = max_distance * max_distance;
		correspondences.resize(indices_->size());
		std::vector<int> index(1);
		std::vector<float> distance(1);
		pcl::Correspondence corr;
		unsigned int nr_valid_correspondences = 0;
		int descriptor_size;
		// Check if the template types are the same. If true, avoid a copy.
		// Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
		if (isSamePointType<PointSource, PointTarget>()) {
			// Iterate over the input set of source indices
			for (std::vector<int>::const_iterator idx = indices_->begin();
					idx != indices_->end(); ++idx) {

				std::vector<int> neigh_xyz_indices(1);
				std::vector<float> neigh_xyz_sqr_dists(1);

				if (!pcl_isfinite (input_->points[*idx].descriptor[0])) //skipping NaNs
				{
					continue;
				}

				const float * descriptor = input_->points[*idx].descriptor;
				if (!input_->points[*idx].extended) {
					descriptor_size = 64;
				} else {
					descriptor_size = 128;
				}
				int max_neighs = 5;

				int found_xyz_neighs = tree_->nearestKSearch(
						input_->points[*idx], max_neighs, neigh_xyz_indices,
						neigh_xyz_sqr_dists);

				float min_distance = neigh_xyz_sqr_dists[max_neighs - 1];
				float min_descriptor_distance = 999999;
				int min_idx = max_neighs - 1;

				for (int i = 0; i < neigh_xyz_indices.size(); i++) {

					const float* descriptor_t =
							target_->points[neigh_xyz_indices[i]].descriptor;

					float desc_distance = 0.0;

					for (int j = 0; j < descriptor_size; j++) {
						float diff = descriptor[j] - descriptor_t[j];
						desc_distance += diff * diff;
					}

					desc_distance = sqrt(desc_distance);

					if (desc_distance < min_descriptor_distance) {
						min_descriptor_distance = desc_distance;
						min_idx = i;
						min_distance = neigh_xyz_sqr_dists[i];
					}
				}

				if (min_distance > max_xyz_dist_sqr)
					continue;

				corr.index_query = *idx;
				corr.index_match = neigh_xyz_indices[min_idx];
				corr.distance = min_distance;
				correspondences[nr_valid_correspondences++] = corr;

			}
		} else {
			PointTarget pt;
			// Iterate over the input set of source indices
			for (std::vector<int>::const_iterator idx = indices_->begin();
					idx != indices_->end(); ++idx) {
				// Copy the source data to a target PointTarget format so we can search in the tree
				pt = input_->points[*idx];

				tree_->nearestKSearch(pt, 1, index, distance);

				if (distance[0] > max_xyz_dist_sqr)
					continue;

				corr.index_query = *idx;
				corr.index_match = index[0];
				corr.distance = distance[0];
				correspondences[nr_valid_correspondences++] = corr;
			}
		}
		correspondences.resize(nr_valid_correspondences);

		deinitCompute();
	}
	/** \brief Determine the reciprocal correspondences between input and target cloud.
	 * A correspondence is considered reciprocal if both Src_i has Tgt_i as a
	 * correspondence, and Tgt_i has Src_i as one.
	 *
	 * \param[out] correspondences the found correspondences (index of query and target point, distance)
	 * \param[in] max_distance maximum allowed distance between correspondences
	 */
	virtual void determineReciprocalCorrespondences(
			pcl::Correspondences &correspondences, double max_distance =
					std::numeric_limits<double>::max()) {

		if (!initCompute())
			return;

		// setup tree for reciprocal search
		// Set the internal point representation of choice
		if (!initComputeReciprocal())
			return;
		double max_dist_sqr = max_distance * max_distance;

		correspondences.resize(indices_->size());
		std::vector<int> index(1);
		std::vector<float> distance(1);
		std::vector<int> index_reciprocal(1);
		std::vector<float> distance_reciprocal(1);
		pcl::Correspondence corr;
		unsigned int nr_valid_correspondences = 0;
		int target_idx = 0;

		// Check if the template types are the same. If true, avoid a copy.
		// Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
		if (isSamePointType<PointSource, PointTarget>()) {
			// Iterate over the input set of source indices
			for (std::vector<int>::const_iterator idx = indices_->begin();
					idx != indices_->end(); ++idx) {
				tree_->nearestKSearch(input_->points[*idx], 1, index, distance);

				if (distance[0] > max_dist_sqr)
					continue;

				target_idx = index[0];

				tree_reciprocal_->nearestKSearch(target_->points[target_idx], 1,
						index_reciprocal, distance_reciprocal);
				if (distance_reciprocal[0] > max_dist_sqr
						|| *idx != index_reciprocal[0])
					continue;

				corr.index_query = *idx;
				corr.index_match = index[0];
				corr.distance = distance[0];
				correspondences[nr_valid_correspondences++] = corr;
			}
		} else {
			PointTarget pt_src;
			PointSource pt_tgt;

			// Iterate over the input set of source indices
			for (std::vector<int>::const_iterator idx = indices_->begin();
					idx != indices_->end(); ++idx) {
				// Copy the source data to a target PointTarget format so we can search in the tree
				pt_src = input_->points[*idx];

				tree_->nearestKSearch(pt_src, 1, index, distance);
				//

				if (distance[0] > max_dist_sqr)
					continue;

				target_idx = index[0];

				// Copy the target data to a target PointSource format so we can search in the tree_reciprocal
				pt_tgt = target_->points[target_idx];

				tree_reciprocal_->nearestKSearch(pt_tgt, 1, index_reciprocal,
						distance_reciprocal);
				if (distance_reciprocal[0] > max_dist_sqr
						|| *idx != index_reciprocal[0])
					continue;

				corr.index_query = *idx;
				corr.index_match = index[0];
				corr.distance = distance[0];
				correspondences[nr_valid_correspondences++] = corr;
			}
		}
		correspondences.resize(nr_valid_correspondences);
		deinitCompute();
	}
	/** \brief Determine the correspondences between input and target cloud.
	 * \param[out] correspondences the found correspondences (index of query point, index of target point, distance)
	 * \param[in] max_distance maximum allowed distance between correspondences
	 */
	virtual void determineCorrespondences(pcl::Correspondences &correspondences,
			double max_distance = std::numeric_limits<double>::max()) {

		if (!initCompute())
			return;

		double max_dist_sqr = max_distance * max_distance;

		correspondences.resize(indices_->size());

		std::vector<int> index(1);
		std::vector<float> distance(1);
		std::vector<float> distanceRGB(1);

		//narazie na pale
		float minRGB = 2000;
		int minIndex = -2;

		pcl::Correspondence corr;
		unsigned int nr_valid_correspondences = 0;

		// Check if the template types are the same. If true, avoid a copy.
		// Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
		if (isSamePointType<PointSource, PointTarget>()) {
			// Iterate over the input set of source indices
			for (std::vector<int>::const_iterator idx = indices_->begin(); idx != indices_->end(); ++idx) {

				int r = input_->points[*idx].r;
				int g = input_->points[*idx].g;
				int b = input_->points[*idx].b;

				tree_->nearestKSearch (input_->points[*idx], 25, index, distance);
				float mindist = distance[24];
				float minrgbdist = 999999;
				int minind = 24;
				for (int i = 0; i < index.size(); i++) {

					int rr = target_->points[index[i]].r;
					int gg = target_->points[index[i]].g;
					int bb = target_->points[index[i]].b;

					float rd = r - rr;
					float gd = g - gg;
					float bd = b - bb;

					float distancergb = sqrt(rd*rd + gd*gd + bd*bd);

					if (distancergb < minrgbdist) {
						minrgbdist = distancergb;
						minind = i;
						mindist = distance[i];
					}
				}
				if (mindist > max_dist_sqr)
					continue;

				corr.index_query = *idx;
				corr.index_match = index[minind];
				corr.distance = mindist;
				correspondences[nr_valid_correspondences++] = corr;
			}
		} else {
			PointTarget pt;
			// Iterate over the input set of source indices
			for (std::vector<int>::const_iterator idx = indices_->begin();
					idx != indices_->end(); ++idx) {
				// Copy the source data to a target PointTarget format so we can search in the tree
				pt = input_->points[*idx];

				tree_->nearestKSearch(pt, 1, index, distance);
				if (distance[0] > max_dist_sqr)
					continue;

				corr.index_query = *idx;
				corr.index_match = index[0];
				corr.distance = distance[0];
				correspondences[nr_valid_correspondences++] = corr;
			}
		}
		correspondences.resize(nr_valid_correspondences);
		deinitCompute();
	}
template <typename PointSource, typename PointTarget> void
pcl::registration::CorrespondenceEstimation<PointSource, PointTarget>::determineReciprocalCorrespondences (
    pcl::Correspondences &correspondences)
{
  typedef typename pcl::traits::fieldList<PointSource>::type FieldListSource;
  typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget;
  typedef typename pcl::intersect<FieldListSource, FieldListTarget>::type FieldList;
  
  if (!initCompute ())
    return;

  if (!target_)
  {
    PCL_WARN ("[pcl::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
    return;
  }

  // setup tree for reciprocal search
  pcl::KdTreeFLANN<PointSource> tree_reciprocal;
  tree_reciprocal.setInputCloud (input_, indices_);

  correspondences.resize (indices_->size());
  std::vector<int> index (1);
  std::vector<float> distance (1);
  std::vector<int> index_reciprocal (1);
  std::vector<float> distance_reciprocal (1);
  pcl::Correspondence corr;
  unsigned int nr_valid_correspondences = 0;

  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Copy the source data to a target PointTarget format so we can search in the tree
    PointTarget pt_src;
    pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointSource, PointTarget> (
          input_->points[(*indices_)[i]], 
          pt_src));

    //tree_->nearestKSearch (input_->points[(*indices_)[i]], 1, index, distance);
    tree_->nearestKSearch (pt_src, 1, index, distance);

    // Copy the target data to a target PointSource format so we can search in the tree_reciprocal
    PointSource pt_tgt;
    pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTarget, PointSource> (
          target_->points[index[0]],
          pt_tgt));
    //tree_reciprocal.nearestKSearch (target_->points[index[0]], 1, index_reciprocal, distance_reciprocal);
    tree_reciprocal.nearestKSearch (pt_tgt, 1, index_reciprocal, distance_reciprocal);

    if ((*indices_)[i] == index_reciprocal[0])
    {
      corr.index_query = (*indices_)[i];
      corr.index_match = index[0];
      corr.distance = distance[0];
      correspondences[nr_valid_correspondences] = corr;
      ++nr_valid_correspondences;
    }
  }
  correspondences.resize (nr_valid_correspondences);

  deinitCompute ();
}
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
pcl::registration::CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::determineCorrespondences (
    pcl::Correspondences &correspondences, double max_distance)
{
  if (!initCompute ())
    return;

  typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget;
  correspondences.resize (indices_->size ());

  std::vector<int> nn_indices (k_);
  std::vector<float> nn_dists (k_);

  double min_dist = std::numeric_limits<double>::max ();
  int min_index = 0;
  
  pcl::Correspondence corr;
  unsigned int nr_valid_correspondences = 0;

  // Check if the template types are the same. If true, avoid a copy.
  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
  if (isSamePointType<PointSource, PointTarget> ())
  {
    PointTarget pt;
    // Iterate over the input set of source indices
    for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
    {
      tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);

      // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
      min_dist = std::numeric_limits<double>::max ();
      
      // Find the best correspondence
      for (size_t j = 0; j < nn_indices.size (); j++)
      {
        // computing the distance between a point and a line in 3d. 
        // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
        pt.x = target_->points[nn_indices[j]].x - input_->points[*idx_i].x;
        pt.y = target_->points[nn_indices[j]].y - input_->points[*idx_i].y;
        pt.z = target_->points[nn_indices[j]].z - input_->points[*idx_i].z;

        const NormalT &normal = source_normals_->points[*idx_i];
        Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
        Eigen::Vector3d V (pt.x, pt.y, pt.z);
        Eigen::Vector3d C = N.cross (V);
        
        // Check if we have a better correspondence
        double dist = C.dot (C);
        if (dist < min_dist)
        {
          min_dist = dist;
          min_index = static_cast<int> (j);
        }
      }
      if (min_dist > max_distance)
        continue;

      corr.index_query = *idx_i;
      corr.index_match = nn_indices[min_index];
      corr.distance = nn_dists[min_index];//min_dist;
      correspondences[nr_valid_correspondences++] = corr;
    }
  }
  else
  {
    PointTarget pt;
    
    // Iterate over the input set of source indices
    for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
    {
      tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
 
      // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
      min_dist = std::numeric_limits<double>::max ();
      
      // Find the best correspondence
      for (size_t j = 0; j < nn_indices.size (); j++)
      {
        PointSource pt_src;
        // Copy the source data to a target PointTarget format so we can search in the tree
        pcl::for_each_type <FieldListTarget> (pcl::NdConcatenateFunctor <PointSource, PointTarget> (
            input_->points[*idx_i], 
            pt_src));

        // computing the distance between a point and a line in 3d. 
        // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
        pt.x = target_->points[nn_indices[j]].x - pt_src.x;
        pt.y = target_->points[nn_indices[j]].y - pt_src.y;
        pt.z = target_->points[nn_indices[j]].z - pt_src.z;
        
        const NormalT &normal = source_normals_->points[*idx_i];
        Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
        Eigen::Vector3d V (pt.x, pt.y, pt.z);
        Eigen::Vector3d C = N.cross (V);
        
        // Check if we have a better correspondence
        double dist = C.dot (C);
        if (dist < min_dist)
        {
          min_dist = dist;
          min_index = static_cast<int> (j);
        }
      }
      if (min_dist > max_distance)
        continue;
      
      corr.index_query = *idx_i;
      corr.index_match = nn_indices[min_index];
      corr.distance = nn_dists[min_index];//min_dist;
      correspondences[nr_valid_correspondences++] = corr;
    }
  }
  correspondences.resize (nr_valid_correspondences);
  deinitCompute ();
}
Exemple #28
0
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::determineBaseMatches (
  const std::vector <int> &base_indices,
  std::vector <std::vector <int> > &matches,
  const pcl::Correspondences &pairs_a,
  const pcl::Correspondences &pairs_b,
  const float (&ratio)[2])
{
  // calculate edge lengths of base
  float dist_base[4];
  dist_base[0] = pcl::euclideanDistance (target_->points[base_indices[0]], target_->points[base_indices[2]]);
  dist_base[1] = pcl::euclideanDistance (target_->points[base_indices[0]], target_->points[base_indices[3]]);
  dist_base[2] = pcl::euclideanDistance (target_->points[base_indices[1]], target_->points[base_indices[2]]);
  dist_base[3] = pcl::euclideanDistance (target_->points[base_indices[1]], target_->points[base_indices[3]]);

  // loop over first point pair correspondences and store intermediate points 'e' in new point cloud
  PointCloudSourcePtr cloud_e (new PointCloudSource);
  cloud_e->resize (pairs_a.size () * 2);
  PointCloudSourceIterator it_pt = cloud_e->begin ();
  for (pcl::Correspondences::const_iterator it_pair = pairs_a.begin (), it_pair_e = pairs_a.end () ; it_pair != it_pair_e; it_pair++)
  {
    const PointSource *pt1 = &(input_->points[it_pair->index_match]);
    const PointSource *pt2 = &(input_->points[it_pair->index_query]);

    // calculate intermediate points using both ratios from base (r1,r2)
    for (int i = 0; i < 2; i++, it_pt++)
    {
      it_pt->x = pt1->x + ratio[i] * (pt2->x - pt1->x);
      it_pt->y = pt1->y + ratio[i] * (pt2->y - pt1->y);
      it_pt->z = pt1->z + ratio[i] * (pt2->z - pt1->z);
    }
  }

  // initialize new kd tree of intermediate points from first point pair correspondences
  KdTreeReciprocalPtr tree_e (new KdTreeReciprocal);
  tree_e->setInputCloud (cloud_e);

  std::vector <int> ids;
  std::vector <float> dists_sqr;

  // loop over second point pair correspondences
  for (pcl::Correspondences::const_iterator it_pair = pairs_b.begin (), it_pair_e = pairs_b.end () ; it_pair != it_pair_e; it_pair++)
  {
    const PointTarget *pt1 = &(input_->points[it_pair->index_match]);
    const PointTarget *pt2 = &(input_->points[it_pair->index_query]);

    // calculate intermediate points using both ratios from base (r1,r2)
    for (int i = 0; i < 2; i++)
    {
      PointTarget pt_e;
      pt_e.x = pt1->x + ratio[i] * (pt2->x - pt1->x);
      pt_e.y = pt1->y + ratio[i] * (pt2->y - pt1->y);
      pt_e.z = pt1->z + ratio[i] * (pt2->z - pt1->z);

      // search for corresponding intermediate points
      tree_e->radiusSearch (pt_e, coincidation_limit_, ids, dists_sqr);
      for (std::vector <int>::iterator it = ids.begin (), it_e = ids.end (); it != it_e; it++)
      {
        std::vector <int> match_indices (4);

        match_indices[0] = pairs_a[static_cast <int> (std::floor ((float)(*it/2.f)))].index_match;
        match_indices[1] = pairs_a[static_cast <int> (std::floor ((float)(*it/2.f)))].index_query;
        match_indices[2] = it_pair->index_match;
        match_indices[3] = it_pair->index_query;

        // EDITED: added coarse check of match based on edge length (due to rigid-body )
        if (checkBaseMatch (match_indices, dist_base) < 0)
          continue;

        matches.push_back (match_indices);
      }
    }
  }

  // return unsuccessfull if no match was found
  return (matches.size () > 0 ? 0 : -1);
}
template <typename PointT> void 
pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::getRemainingCorrespondences (
    const pcl::Correspondences& original_correspondences, 
    pcl::Correspondences& remaining_correspondences)
{
  if (!input_)
  {
    PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input cloud dataset was given!\n", getClassName ().c_str ());
    return;
  }

  if (!target_)
  {
    PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input target dataset was given!\n", getClassName ().c_str ());
    return;
  }

  if (save_inliers_)
     inlier_indices_.clear ();

  int nr_correspondences = static_cast<int> (original_correspondences.size ());
  std::vector<int> source_indices (nr_correspondences);
  std::vector<int> target_indices (nr_correspondences);

  // Copy the query-match indices
  for (size_t i = 0; i < original_correspondences.size (); ++i)
  {
    source_indices[i] = original_correspondences[i].index_query;
    target_indices[i] = original_correspondences[i].index_match;
  }

   // from pcl/registration/icp.hpp:
   std::vector<int> source_indices_good;
   std::vector<int> target_indices_good;
   {
     // From the set of correspondences found, attempt to remove outliers
     // Create the registration model
     typedef typename pcl::SampleConsensusModelRegistration<PointT>::Ptr SampleConsensusModelRegistrationPtr;
     SampleConsensusModelRegistrationPtr model;
     model.reset (new pcl::SampleConsensusModelRegistration<PointT> (input_, source_indices));
     // Pass the target_indices
     model->setInputTarget (target_, target_indices);
     // Create a RANSAC model
     pcl::RandomSampleConsensus<PointT> sac (model, inlier_threshold_);
     sac.setMaxIterations (max_iterations_);

     // Compute the set of inliers
     if (!sac.computeModel ())
     {
       remaining_correspondences = original_correspondences;
       best_transformation_.setIdentity ();
       return;
     }
     else
     {
       if (refine_ && !sac.refineModel ())
       {
         PCL_ERROR ("[pcl::registration::CorrespondenceRejectorSampleConsensus::getRemainingCorrespondences] Could not refine the model! Returning an empty solution.\n");
         return;
       }
       
       std::vector<int> inliers;
       sac.getInliers (inliers);

       if (inliers.size () < 3)
       {
         remaining_correspondences = original_correspondences;
         best_transformation_.setIdentity ();
         return;
       }
       boost::unordered_map<int, int> index_to_correspondence;
       for (int i = 0; i < nr_correspondences; ++i)
         index_to_correspondence[original_correspondences[i].index_query] = i;

       remaining_correspondences.resize (inliers.size ());
       for (size_t i = 0; i < inliers.size (); ++i)
         remaining_correspondences[i] = original_correspondences[index_to_correspondence[inliers[i]]];

       if (save_inliers_)
       {
         inlier_indices_.reserve (inliers.size ());
         for (const int &inlier : inliers)
           inlier_indices_.push_back (index_to_correspondence[inlier]);
       }

       // get best transformation
       Eigen::VectorXf model_coefficients;
       sac.getModelCoefficients (model_coefficients);
       best_transformation_.row (0) = model_coefficients.segment<4>(0);
       best_transformation_.row (1) = model_coefficients.segment<4>(4);
       best_transformation_.row (2) = model_coefficients.segment<4>(8);
       best_transformation_.row (3) = model_coefficients.segment<4>(12);
     }
   }
}
template <typename PointSource, typename PointTarget, typename NormalT> void
pcl::registration::CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT>::determineCorrespondences (
    pcl::Correspondences &correspondences, double max_distance)
{
  if (!initCompute ())
    return;

  typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget;
  correspondences.resize (indices_->size ());

  std::vector<int> nn_indices (k_);
  std::vector<float> nn_dists (k_);

  float min_dist = std::numeric_limits<float>::max ();
  int min_index = 0;
  
  pcl::Correspondence corr;
  unsigned int nr_valid_correspondences = 0;

  // Check if the template types are the same. If true, avoid a copy.
  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
  if (isSamePointType<PointSource, PointTarget> ())
  {
    PointTarget pt;
    // Iterate over the input set of source indices
    for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
    {
      tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);

      // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
      min_dist = std::numeric_limits<float>::max ();
      
      // Find the best correspondence
      for (size_t j = 0; j < nn_indices.size (); j++)
      {
        float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
                          source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
                          source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
        float dist = nn_dists[min_index] * (2.0f - cos_angle * cos_angle);
        
        if (dist < min_dist)
        {
          min_dist = dist;
          min_index = j;
        }
      }
      if (min_dist > max_distance)
        continue;

      corr.index_query = *idx_i;
      corr.index_match = nn_indices[min_index];
      corr.distance = nn_dists[min_index];//min_dist;
      correspondences[nr_valid_correspondences++] = corr;
    }
  }
  else
  {
    PointTarget pt;
    
    // Iterate over the input set of source indices
    for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
    {
      tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
 
      // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
      min_dist = std::numeric_limits<float>::max ();
      
      // Find the best correspondence
      for (size_t j = 0; j < nn_indices.size (); j++)
      {
        PointSource pt_src;
        // Copy the source data to a target PointTarget format so we can search in the tree
        pcl::for_each_type <FieldListTarget> (pcl::NdConcatenateFunctor <PointSource, PointTarget> (
            input_->points[*idx_i], 
            pt_src));

        float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
                          source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
                          source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
        float dist = nn_dists[min_index] * (2.0f - cos_angle * cos_angle);
        
        if (dist < min_dist)
        {
          min_dist = dist;
          min_index = j;
        }
      }
      if (min_dist > max_distance)
        continue;
      
      corr.index_query = *idx_i;
      corr.index_match = nn_indices[min_index];
      corr.distance = nn_dists[min_index];//min_dist;
      correspondences[nr_valid_correspondences++] = corr;
    }
  }
  correspondences.resize (nr_valid_correspondences);
  deinitCompute ();
}