PCL - ICP代碼研讀（十一 ） - Correspondence實現

前言

接續PCL - ICP代碼研讀（十 ） - Correspondence架構，本篇繼續來看Correspondence各函數的實現，本片對應到cppcorrespondence.cpp這個文件。

getRejectedQueryIndices

getRejectedQueryIndices函數用於比較correspondences_before和correspondences_after，目的是從中找出是哪些點對被拒絕了。

presorting_required用於決定在找差集之前是否要先排序，預設是要。

#include <pcl/types.h>
#include <pcl/correspondence.h>
#include <algorithm>
#include <iterator>

//
void
pcl::getRejectedQueryIndices (const pcl::Correspondences &correspondences_before,
                              const pcl::Correspondences &correspondences_after,
                              Indices& indices,
                              bool presorting_required)
{

indices用於儲存函數的輸出，在使用前先清空：

  // indices:被拒絕掉的配對的source index
  indices.clear();

先前的配對數和之後的配對數：

  const auto nr_correspondences_before = correspondences_before.size ();
  const auto nr_correspondences_after = correspondences_after.size ();

極端情況1：先前的配對數為0，那麼兩者的差集一定是空集合。

  if (nr_correspondences_before == 0)
    return;

極端情況2：之後的配對數為0，那麼兩者的差集一定是先前的配對。

  if (nr_correspondences_after == 0)
  {
    // 如果所有配對都被拒絕,那麼indices就設成所有配對的source index
    indices.resize(nr_correspondences_before);
    for (std::size_t i = 0; i < nr_correspondences_before; ++i)
      indices[i] = correspondences_before[i].index_query;
    return;
  }

從兩組配對中取出索引（注意取的是配對在source點雲中的索引index_query）：

  Indices indices_before (nr_correspondences_before);
  for (std::size_t i = 0; i < nr_correspondences_before; ++i)
    indices_before[i] = correspondences_before[i].index_query;

  Indices indices_after (nr_correspondences_after);
  for (std::size_t i = 0; i < nr_correspondences_after; ++i)
    indices_after[i] = correspondences_after[i].index_query;

如果presorting_required為true，則預先對indices_before和indices_after做排序。

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

使用std::set_difference找出indices_before與indices_after的差集，並將結果填入indices。

  // indices:indices_before與indices_after的差集
  set_difference (
      indices_before.begin (), indices_before.end (),
      indices_after.begin (),  indices_after.end (),
      inserter (indices, indices.begin ()));
}

注：參考std::set_difference，在使用std::set_difference之前，必須事先將兩個容器做排序。

<< 運算子

重載<<運算子，之後便可以直接使用<< c輸出Corresondence物件c的配對點索引和距離。

namespace pcl
{
  std::ostream&
  operator << (std::ostream& os, const Correspondence& c)
  {
    os << c.index_query << " " << c.index_match << " " << c.distance;
    return (os);
  }
}