PCL(Point Cloud Library)のデータ構造メモ(平面推定)

## はじめに
PCL(Point Cloud Library)を使うことになったが、点群とか扱うのは初めてでほとんどよくわかっていない。
よって、Webにあるサンプルコードの読解を通して、少しずつPCLを学んでいければ良いと考え、記録に残す。

なお、PCLのバージョンはV1.7.1とする。

## 今回読むサンプルコード
[このページ](http://ros-robot.blogspot.jp/2011/08/pclapi-point-cloud-library-pcl-pcl-api.html)にある平面推定のためのサンプルコードを使わせていただく。
OTLさん、ありがとうございます。

### PointCloud
どうやら点群空間(?と言っていいのか?)を管理するためのデータ。
クラスそのものの定義よりも、説明コメントのほうが重要。
特に点データを新たに追加する場合は、push_back()メソッドを使うということがわかればまずは良さそう。

```cpp:common/include/pcl/point_cloud.h
  /** \brief PointCloud represents the base class in PCL for storing collections of 3D points.
    *
    * The class is templated, which means you need to specify the type of data
    * that it should contain. For example, to create a point cloud that holds 4
    * random XYZ data points, use:
    *
    * \code
    * pcl::PointCloud<pcl::PointXYZ> cloud;
    * cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ())); 
    * cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ())); 
    * cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ())); 
    * cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ())); 
    * \endcode
    *
    * The PointCloud class contains the following elements:
    *   - \b width - specifies the width of the point cloud dataset in the number of points. WIDTH has two meanings:
    *     - it can specify the total number of points in the cloud (equal with POINTS see below) for unorganized datasets;
    *     - it can specify the width (total number of points in a row) of an organized point cloud dataset.
    *   \a Mandatory.
    *   - \b height - specifies the height of the point cloud dataset in the number of points. HEIGHT has two meanings:
    *     - it can specify the height (total number of rows) of an organized point cloud dataset;
    *     - it is set to 1 for unorganized datasets (thus used to check whether a dataset is organized or not).
    *   \a Mandatory.
    *   - \b points - the data array where all points of type <b>PointT</b> are stored. \a Mandatory.
    *
    *   - \b is_dense - specifies if all the data in <b>points</b> is finite (true), or whether it might contain Inf/NaN values
    * (false). \a Mandatory.
    *
    *   - \b sensor_origin_ - specifies the sensor acquisition pose (origin/translation). \a Optional.
    *   - \b sensor_orientation_ - specifies the sensor acquisition pose (rotation). \a Optional.
    *
    * \author Patrick Mihelich, Radu B. Rusu
    */

template <typename PointT>
class PCL_EXPORTS PointCloud
{
```

```cpp:サンプルコードより抜粋
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);  

```

### ModelCoefficients

これは、「ヘッダ」とfloat型のvectorから構成されるデータ構造。
「ヘッダ」はPCLHeader。PCLHeaderは文字通りヘッダ的なもので、シーケンス番号、タイムスタンプ、フレームIDで構成される。
※フレームIDの具体的な用途はよくわからないが、とりあえず「そういうものがある」と割り切る。

```cpp:common/include/pcl/PCLHeader.h
namespace pcl 
{
  struct PCLHeader
  {
// 略
    /** \brief Sequence number */
    pcl::uint32_t seq;
    /** \brief A timestamp associated with the time when the data was acquired
      *
      * The value represents microseconds since 1970-01-01 00:00:00 (the UNIX epoch).
      */
    pcl::uint64_t stamp;
    /** \brief Coordinate frame ID */
    std::string frame_id;

    typedef boost::shared_ptr<PCLHeader> Ptr;
    typedef boost::shared_ptr<PCLHeader const> ConstPtr;
```

### PointIndices

```cpp:サンプルコードより抜粋
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
```

PointIndicesは、ModelCoefficientsと構造が似ている。
以下のとおりで、ModelCoefficientsと比べると、vectorが<float>でなく<int>である点が異なる。型の名前から何かのインデックスを管理するためのデータ構造と思われる。

```cpp:common/include/pcl/PointIndices.h
  struct PointIndices
  {
    PointIndices () : header (), indices ()
    {}  

    ::pcl::PCLHeader header;

    std::vector<int> indices;

    public:
      typedef boost::shared_ptr< ::pcl::PointIndices> Ptr;
      typedef boost::shared_ptr< ::pcl::PointIndices const> ConstPtr;
  }; // struct PointIndices
```

### SACSegmentation

```cpp:サンプルコードより抜粋
  // Create the segmentation object  
  pcl::SACSegmentation<pcl::PointXYZRGB> seg;
```

SAC、segmentationというのがよくわからない。クラスの型をみることにする。

```cpp:segmentation/include/pcl/segmentation/sac_segmentation.h
  /** \brief @b SACSegmentation represents the Nodelet segmentation class for
    * Sample Consensus methods and models, in the sense that it just creates a
    * Nodelet wrapper for generic-purpose SAC-based segmentation.
    * \author Radu Bogdan Rusu
    * \ingroup segmentation
    */
  template <typename PointT>
  class SACSegmentation : public PCLBase<PointT>
  {
```

SACについては、[この論文](http://www.cc.kyoto-su.ac.jp/~kano/pdf/study/student/NaokiPaper.pdf)が参考になりそう。
細かなところは理解できていないが、特徴点を使ってざっくりと位置合わせを行う手法らしい。複数の画像から構成した複数の点群を統合する。当然それら点群は「ずれて」いることも考えられる。

```text:論文より抜粋
立体物の完全な3次元データを取得するためには、複数の視点から取得した3次元点群データを一つに統合する必要がある。
複数の点群データを、お互いに独立して取得した場合、それらの位置関係を事前に知ることができない。このとき、まず、複数の点群データを粗く位置合わせし、その後、ICPアルゴリズムで高精度に位置合わせを行う。ICPアルゴリズムは、繰り返し的な処理で位置合わせを高精度化する方法である。そのため、初期状態において2つの3次元点群データの位置が大体あっていないと有効に働かない。初期位置合わせを行うために、SAC-IA(Sample Consensus Initial Alignment)関数を用いる。
初期位置合わせを行うために、SAC-IA(Sample Consensus Initial Alignment)関数を用いる。
SAC-IAは初期位置合わせのためにRANSAC（Randum  Sample Consensus）を用いる方法である。位置合わせを行う2つの3次元点群データから、それぞれ複数の特徴点を抽出する。ここで、点群データにおける特徴点とは３次元形状の特徴（コーナーなど）を表すことができる点のことである。2つの点群データの一方をターゲット、他方をインプットとして、特徴点間でランダムに対応付けをする。対応付けた特徴点間が最も近づくように座標変換を行う。そのときに生じる位置の差を、特徴点の誤差とする。この対応付けを一定回数行い、最も2つの特徴点の誤差が低くなった場合を座標変換として採用する。そしてターゲットに合わせてインプットを回転、拡大縮小、平行移動をおこない、再配置することで、大まかな位置を合わせることができる。
```

上記論文では、RANSACについて「2つの点群データの一方をターゲット、他方をインプットとして、特徴点間でランダムに対応付けをする。対応付けた特徴点間が最も近づくように座標変換を行う。そのときに生じる位置の差を、特徴点の誤差とする。」と書いている。しかし、この点が理解しきれていない。

調べてみると、[この文書](http://playwithopencv.blogspot.jp/2010/07/ransac.html)を見つけた。
このページの記事を読む限り、RANSACというのは、以下のようなアルゴリズムであると理解した。

1. 集めたデータからN個ランダムに選んで、それに対して最小二乗法などを用いてパラメータを求めて式を作る。
2. 1で作った式を残りのパラメータに適用して、誤差の容認範囲内になっているパラメータ数を求める。(リンク先文書で「投票」と言っているのはそれ)
3. 1,2を繰り返して、容認範囲内データの数が最も多いパラメータを採用する。
4. 容認範囲外データを取り除いた上で、残りのデータを用いてパラメータを求める。これが最終的なパラメータになる。

ここまでのことを念頭に置いて、サンプルコードを先に読み進める。

```cpp:サンプルコードより抜粋
  // Optional  
  seg.setOptimizeCoefficients (true);  
  // Mandatory  
  seg.setModelType (pcl::SACMODEL_PLANE);  
  seg.setMethodType (pcl::SAC_RANSAC);  
  seg.setDistanceThreshold (0.1);  
  
  seg.setInputCloud (cloud.makeShared ());  
  seg.segment (*inliers, *coefficients);
```

おそらく、setDistanceThreshold()は「容認する誤差範囲」を指定するのだろう。
あと、segment()について、ライブラリのソースコードには以下のような記載がある。
まさに「パラメータを求めて、範囲外の不適切な点を取り除いた点の集合を取得する」のであろう。

```cpp:segmentation/include/pcl/segmentation/sac_segmentation.h
      /** \brief Base method for segmentation of a model in a PointCloud given by <setInputCloud (), setIndices ()>
        * \param[in] inliers the resultant point indices that support the model found (inliers)        * \param[out] model_coefficients the resultant model coefficients
        */
      virtual void
      segment (PointIndices &inliers, ModelCoefficients &model_coefficients);
```

最後に、抽出したセグメント内の点を赤くして、それをVisualizerで表示することになる。

```cpp:サンプルコードからの抜粋
  for (size_t i = 0; i < inliers->indices.size (); ++i) {  
// 略
    cloud.points[inliers->indices[i]].r = 255;  
    cloud.points[inliers->indices[i]].g = 0;  
    cloud.points[inliers->indices[i]].b = 0;  
  }  
  pcl::visualization::CloudViewer viewer("Cloud Viewer");  
```

以上