C++ Correspondences::size方法代碼示例

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

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

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

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

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

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

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