点群を処理するC++のライブラリ Point Cloud Library には様々な特徴量を計算するクラスが用意されているが、独自のクラスを作りたいと思うのはプログラマの性だろう。さあ、やってみよう！

以下の公式リファレンスに従ってカスタムクラスを追加する。

検証した環境の情報

PCL 1.9.1
Windows10 x64

ここまでのお話

PCL の導入や、PointTで溢れる PCL のテンプレートのお話。

カスタムクラスの設計

計算する特徴量

pcl::Feature<PointT>クラスは点群中の各点の特徴量を計算するために用意された抽象クラスである。これを継承し、今回は次元特徴量を計算するカスタムクラスDimensionAnalysis<PointT>を作る。

次元特徴量とは？

点群の主成分分析（Principal Component Analysis，PCA）は，点群中の各点における法線や曲率の推定，周辺点群の分布状態推定，姿勢に依存しない局所座標系定義に用いられる．点群の点 $i$ の近傍点集合 $N(i)$ に対する主成分分析は，次の分散共分散行列（Variance-covariance matrix）${\rm C}_i$ の固有値解析により行われる．
{\rm C}_i = \frac{1}{|N(i)|} \sum_{j \ \in \ N(i)} ({\bf P}_j - \overline{\bf p})({\bf P}_j - \overline{\bf p})^T 
ここで，${\bf P}_j$ は点 $j$ の位置（列ベクトル），$\overline{\bf p}$ は $N(i)$ の重心，$|N(i)|$ は $N(i)$ の点数である．近傍点集合 $N(i)$ は，点 $i$ に近いほうから $k$ 個の点集合（$k$近傍）や，点 $i$ を中心とする半径 $r$ の球内の点集合が用いられる．行列 ${\rm C}_i$ の得られた固有値を $\lambda_1, \lambda_2, \lambda_3 \ ( \lambda_1 \ge \lambda_2 \ge \lambda_3 )$，対応する固有ベクトルを ${\bf e}_1, {\bf e}_2, {\bf e}_3$ とすると，${\bf e}_1$ は $N(i)$ の分散（ばらつき）が最大の方向となり，${\bf e}_2$ は，${\bf e}_1$ に直交するなかで分散が最大となる方向，${\bf e}_3$ は分散が最小の方向となる．各固有値は，対応する固有ベクトルの方向に沿った $N(i)$ の分散となる．次元特徴 $d_i$ は以下の通り求められる．
d_i = \mathop{\rm arg~max}\limits_{k = 1,2,3} \ \sigma_k
ここで，$\sigma_1 = \lambda_1 - \lambda_2, \sigma_2 = \lambda_2 - \lambda_3, \sigma_3 = \lambda_3$
次元特徴は，$N(i)$ の分布の次元を表す．

なにやら難しい文章だが要約すると、次元特徴 $d_i\ (=1,2,3)$ の値によって次のように表現できる。

1 : 周辺の点群は大体一直線上に分布している
2 : 周辺の点群は大体平面上に分布している
3 : 上記いずれでもない（周辺の点群の分布はバラバラ）

用意するファイル

PCL の各クラス・関数はテンプレートで書かれている。これは様々な点群の型PointTに対応するためだ。よく使うpcl::PointXYZだけ使えれば問題ない、という場合もあるかもしれないが、PCLに用意された汎用性を無駄にしないためにも今回はテンプレートを使ってみる。

カスタムクラスDimensionAnalysis<PointT>のため用意するファイル一覧

dim.h カスタムクラスのテンプレートを宣言
dim.hpp テンプレート実装の定義
dim.cpp テンプレートの実体化

dim.h

DimensionAnalysis<PointT>をテンプレートで宣言する。なお、計算された次元特徴を格納する点として新しい型PointXYZDも同時に用意した。次元数 $d_i$ を記録するだけならpcl::Labelでも十分だろうが、[PCL] カスタム型 PointT を追加するで折角学んだので試してみた。

dim.cpp

#pragma once
#define PCL_NO_PRECOMPILE
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>

// カスタム型 PointT の宣言
struct PointXYZD;

// カスタム型の struct 定義
struct PointXYZD {
	PCL_ADD_POINT4D; // add float x,y,z
	float lambda[3];
	uint8_t dimension;

	inline PointXYZD() {
		x = y = z = 0;
		lambda[0] = lambda[1] = lambda[2] = 0;
		dimension = 0;
	};
	inline PointXYZD(const PointXYZD& p) {
		x = p.x;
		y = p.y;
		z = p.z;
		lambda[0] = p.lambda[0];
		lambda[1] = p.lambda[1];
		lambda[2] = p.lambda[2];
		dimension = p.dimension;
	};
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

// カスタム型の各フィールドを登録する
POINT_CLOUD_REGISTER_POINT_STRUCT(
	PointXYZD,
	(float, x, x)
	(float, y, y)
	(float, z, z)
	(float, lambda[0], lambda1)
	(float, lambda[1], lambda2)
	(float, lambda[2], lambda3)
	(uint8_t, dimension, dim)
)

// カスタム PCL クラスの宣言
#include <pcl/features/feature.h>
template<typename PointT>
class DimensionAnalysis : public pcl::Feature<PointT, PointXYZD> {

	public:
    // 参照する基底クラスの変数を明示的に宣言
	using pcl::Feature<PointT, PointXYZD>::feature_name_;
	using pcl::Feature<PointT, PointXYZD>::input_;
	using pcl::Feature<PointT, PointXYZD>::indices_;
	using pcl::Feature<PointT, PointXYZD>::search_parameter_;
    // 入出力する点群の型を宣言 
	typedef pcl::Feature<PointT, PointXYZD>::PointCloudConstPtr PointCloudConstPtr;
	typedef pcl::Feature<PointT, PointXYZD>::PointCloudOut PointCloutOut;

	DimensionAnalysis() {
		feature_name_ = "dimension";
	}

	virtual ~DimensionAnalysis() {};

	inline void setInputCloud(const PointCloudConstPtr& cloud) override {
		input_ = cloud;
	}

	protected:
	void computeFeature(PointCloudOut& output);
	bool initCompute();
	bool deinitCompute();

};

ポイントは以下の2点。

pcl::Featureを継承する
pcl::Feature::computeFeatureを宣言する

ユーザ側が使用時に pcl::Feature::compute を呼び出すと、

initCompute 前処理
computeFeature 実際の処理
deinitCompute 後処理

の順に基底クラスpcl::Featureから呼び出される。initCompute,deinitComputeは独自に定義しなくても問題ないようだ。

dim.hpp

ここにはテンプレート実装を定義する。

dim.hpp

#pragma once

#include "dim.h"
#include <pcl/common/pca.h>
#include <iostream>

inline int get_dimension_feature(const float* eigen_value) {
	float s1 = eigen_value[0] - eigen_value[1];
	float s2 = eigen_value[1] - eigen_value[2];
	float s3 = eigen_value[2];
	if ( s1 >= s2 ) {
		return s1 >= s3 ? 1 : 3;
	} else {
		return s2 >= s3 ? 2 : 3;
	}
}

template<typename PointT>
void DimensionAnalysis<PointT>::computeFeature(PointCloudOut& output) {
	pcl::PCA<PointT> pca;
	pca.setInputCloud(input_);
	std::vector<float> dist;
	pcl::PointIndices::Ptr pca_indices(new pcl::PointIndices());
	const int size = indices_->size();
	// boost::shared_ptr <std::vector<int> > indices_ : 指定された点群input_のうち特徴量を計算する対象のインデックス
	for ( int i = 0; i < size; i++ ) {
		int index = (*indices_)[i];
		dist.clear();
		pca_indices->indices.clear();
		// 親クラスのメンバ関数 searchForNeighbors で近傍点を探す
		this->searchForNeighbors(index, search_parameter_, pca_indices->indices, dist);
		// 見つかった近傍点のインデックスを指定して主成分分析
		pca.setIndices(pca_indices);
		Eigen::Vector3f& eigen_values = pca.getEigenValues();
		PointXYZD& dst = output.points[i];
		dst.lambda[0] = eigen_values(0);
		dst.lambda[1] = eigen_values(1);
		dst.lambda[2] = eigen_values(2);
		// 固有値＝各主成分方向の分散　を比較して次元特徴を計算する
		dst.dimension = (uint8_t)get_dimension_feature(dst.lambda);
		const PointT& p = input_->points[index];
		dst.x = p.x;
		dst.y = p.y;
		dst.z = p.z;
	}
	output.is_dense = true;
	output.height = 1;
	output.width = output.points.size();
}

template<typename PointT>
bool DimensionAnalysis<PointT>::initCompute() {
	std::cout << "Dimension calc...";
	return pcl::Feature<PointT, PointXYZD>::initCompute();
}

template<typename PointT>
bool DimensionAnalysis<PointT>::deinitCompute() {
	std::cout << "done." << std::endl;
	return pcl::Feature<PointT, PointXYZD>::deinitCompute();
}

// 型パラメータTに対し、クラステンプレートから明示的に実体化する方法を定義する
#define PCL_INSTANTIATE_DimensionAnalysis(T) template class PCL_EXPORTS DimensionAnalysis<T>;

基本的に特徴量の計算処理を computeFeature で具体的に定義すればよい。
処理する対象の点群は次の基底クラスのメンバを参照する。

input_ : pcl::PointCloud<PointT>::ConstPtr 入力として指定された点群
indices_ : boost::shared_ptr <std::vector<int>> input_のうち対象の点のインデックス

setInputCloud と同時に setIndices で点のインデックスが指定される場合があるため、処理の対象はinput_ の点群全体ではない。

また、点群中のある点 $i$ の特徴量を計算するには、どの点が近くに存在するか知る必要がある（次元特徴量の導入で触れた近傍点集合 $N(i)$ のこと）。通常は、点 $i$ からのユークリッド距離が近い順に $k$個、または半径 $r$ 以内の点を近傍点の集合とする。この近傍を探すには関数Feature::searchForNeighborsを使う。

FeatureではsetSearchMethodでユーザが指定したkd木を利用して近傍探索を行う。このときsetKSearchとsetRadiusSearchで「近い順に $k$個」または「半径 $r$ 以内」と探索方法が選択できるが、searchForNeighborsを使う派生クラス側はその差を気にする必要はない。

並列化の検討
computeFeatureで点群の数だけfor文内を繰り返しているが、

kd木で近傍探索
近傍の点集合に対しpcl::PCAで主成分分析
結果を書き込む

の過程において、kd木の探索はデータ構造の更新を伴わないため、pcl::PCAオブジェクトをスレッドごとに用意すれば並列処理できる。複数スレッドが同じ点を処理しないよう慎重に同期させれば問題ない。

dim.cpp

dim.hpp でクラステンプレートの実装を定義したので、ここでは具体的なテンプレート引数PointTで実体化する。

PCL では主に明示的な実体化が用いられる。例えば、pcl::PointXYZ に対してこのクラステンプレートDimensionAnalysis<PointT> を明示的に実体化するには、次のように記述する。

#include "dim.h" // クラステンプレートの宣言
#include "dim.hpp" //クラステンプレートの実装
#include <pcl/point_types.h>
template class DimensionAnalysis<pcl::PointXYZ>;

しかし、PCL には様々な点の型PointTが存在する。これらすべての型に明示的実体化で対応するには、上記の構文をPointTの数だけ列挙する必要があり大変だ。そこで PCL で用意されたマクロ PCL_INSTANTIATE を利用する。構文は次の通り。

PCL_INSTANTIATE("クラステンプレート名",("PointT_1")[("PointT_2")...]);

実体化したい型パラメータ名を第二引数に列挙すると、各PointTに対してプリプロセッサが展開してくれる。

PCL_INSTANTIATE_"クラステンプレート名" "PointT";

すると、dim.hpp で定義したマクロ PCL_INSTANTIATE_DimensionAnalysis(T) を呼び出すので、最終的に展開された形は、

template class PCL_EXPORTS DimensionAnalysis<PointT_1>;
template class PCL_EXPORTS DimensionAnalysis<PointT_2>;
....
template class PCL_EXPORTS DimensionAnalysis<PointT_N>;

となる。さらに、PCL ではx,y,z座標を持つ点の型の集合としてPCL_XYZ_POINT_TYPESが用意されているので次のように簡潔に記述できる。

dim.cpp

#include <pcl/point_types.h>
#include <pcl/impl/instantiate.hpp>

#include "dim.h"
#include "dim.hpp"

//想定されるすべての型PointTに対してクラステンプレートを明示的に実体化する
PCL_INSTANTIATE(DimensionAnalysis, PCL_XYZ_POINT_TYPES);

DimensionAnalysisが計算する次元特徴量は、点群中の各点の空間座標に対し主成分分析を行うことで求められるので、x,y,z座標を持つすべて型PointTについて実体化しておけば十分と考えPCL_XYZ_POINT_TYPESを選択した。ほかにも多数のプリセットが用意されているので point_types.hpp を見てほしい。

なぜこの３ファイルか？

なぜクラスひとつ追加するのにファイル３つも用意するのか？テンプレートに馴染みがないと疑問に思うかもしれないが、この謎はコンパイラーとリンカーの気持ちになれば理解できる。

ソースコードがコンパイルされると中間ファイルが生成され、最後にリンカーによって実行ファイルが得られる。あるユーザがmain.cppからDimensionAnalysisを利用する場面を考えると、図のように二つの翻訳単位が生じる。ユーザ側はdim.hで宣言されたクラスを利用して何かしらの処理を行うが、main.oはユーザの数と修正の度にコンパイルが発生する。一方で、ライブラリ側のdim.oにはdim.hで宣言されたテンプレートの実装と実体化が含まれ、実行ファイル生成時にリンクされる。dim.oは１回コンパイルすれば変更する必要がない。つまり、ライブラリ側と使う側とでクラスの宣言と実装が分離され、コンパイルが少なく済むように設計されている。

動作確認

実際に点群データを入力して動かしてみる。

テストデータ

data1 建物
PCL のチュートリアルで紹介されていた点群データStatues_4.pcdを利用する。点群ファイルはこのサイトから取得して解凍しておく。CloudCompare で視覚化すると下図のような様子。

何かしらの建物を正面から見たようだ。図の色はintensityフィールドの値に応じて着色している。intensityは反射強度だろうから、この点群はレーザスキャナで測定したデータと思われる。図の真ん中に丸い点群の空白がある場所にスキャナを設置して測定したかもしれない。

data2 樹木
ふたつ目は建物とは対照的な樹木の点群を使ってみた。図は高さ方向に応じて着色。

main.cpp

用意したカスタムクラスDimensionAnalysisを利用する。必要なのはクラスの宣言だけなので、dim.hだけincludeする。

main.cpp

#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/impl/point_cloud_geometry_handlers.hpp>
#include "dim.h"

int main(int argc, char** argv) {
	if ( argc < 3 ) return 1;
	// 点群の準備
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
	if ( pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) < 0 ) {
		return 1;
	}
	// 近傍探索時のk
	int k = atoi(argv[2]);

	// 次元特徴量の計算
	DimensionAnalysis<pcl::PointXYZ> da;
	da.setInputCloud(cloud);
	// 近傍の定義を指定
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kd_tree(new pcl::search::KdTree<pcl::PointXYZ>());
	da.setSearchMethod(kd_tree);
	da.setKSearch(k);
    // 計算結果を格納するカスタムデータ型 PointXYZD の点群を渡す
	pcl::PointCloud<PointXYZD>::Ptr result(new pcl::PointCloud<PointXYZD>());
	da.compute(*result);

	// 計算された次元特徴に応じて点群を分割
	pcl::PointCloud<PointXYZD>::Ptr segments[3];
	for ( int i = 0; i < 3; i++ ) {
		segments[i] = pcl::PointCloud<PointXYZD>::Ptr(new pcl::PointCloud<PointXYZD>());
	}
	for ( auto& p : result->points ) {
		segments[p.dimension - 1]->points.push_back(p);
	}
	for ( int i = 0; i < 3; i++ ) {
		segments[i]->is_dense = true;
		segments[i]->height = 1;
		segments[i]->width = segments[i]->points.size();
	}

	// 色分けして表示
	pcl::visualization::PCLVisualizer viewer("dimension");
	pcl::visualization::PointCloudColorHandlerCustom<PointXYZD> handler1(segments[0], 255, 0, 0);
	pcl::visualization::PointCloudColorHandlerCustom<PointXYZD> handler2(segments[1], 0, 255, 0);
	pcl::visualization::PointCloudColorHandlerCustom<PointXYZD> handler3(segments[2], 0, 0, 255);
	viewer.addPointCloud(segments[0], handler1, "d1", 0);
	viewer.addPointCloud(segments[1], handler2, "d2", 0);
	viewer.addPointCloud(segments[2], handler3, "d3", 0);
	viewer.setSize(800, 600);
	viewer.setBackgroundColor(1, 1, 1, 0);
	while ( !viewer.wasStopped() ) {
		viewer.spinOnce();
	}
	return 0;
}

不具合対応 "Invalid (NaN, Inf) point"

Statues_4.pcdを入力として実行したところ、

kdtree_flann.hpp:136: int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const 
PointT&, int, std::vector<int>&, std::vector<float>&) const [with PointT = pcl::PointXYZ; 
Dist = flann::L2_Simple<float>]: Assertion `point_representation_->isValid (point) 
&& "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"' failed.

と怒られてしまった。どうやら不正な値NaN, Infを座標値にもつ点が紛れているらしい。面倒だがデータを読み込んだ後にNaN, Infを取り除く処理を以下のように追加して解決した。

main.cpp

#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/impl/point_cloud_geometry_handlers.hpp>
#include <pcl/point_representation.h>
#include <pcl/filters/extract_indices.h>
#include "dim.h"

int main(int argc, char** argv) {

	if ( argc < 3 ) return 1;

	// 点群の準備
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
	if ( pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) < 0 ) {
		return 1;
	}
	// 近傍探索時のk
	int k = atoi(argv[2]);

	// NaN Infinite を取り除く
	pcl::DefaultPointRepresentation<pcl::PointXYZ> representation;
	pcl::PointCloud<pcl::PointXYZ>::Ptr clean(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::PointIndices::Ptr indices(new pcl::PointIndices());
	for ( int i = 0; i < cloud->points.size(); i++ ) {
		if ( representation.isValid(cloud->points[i]) ) indices->indices.push_back(i);
	}
	pcl::ExtractIndices<pcl::PointXYZ> extract;
	extract.setInputCloud(cloud);
	extract.setIndices(indices);
	extract.setNegative(false);
	extract.filter(*clean);

	cloud = clean;

	// 中略
	return 0;
}

結果

近傍探索のパラメータを $k=100$ として計算した次元特徴量 $d_i \ (=1,2,3)$ に応じて色分けして示すと、
data1 建物

建物は平面的な構造が多いので、やはり $d_i = 2$ が圧倒的に多い。あとは建物の縁や格子状の構造など直線的な部分は $d_i = 1$ となっている。$d_i = 3$ がほとんど見られないのは建造物として性質というより、測定方法が原因かもしれない。おそらく中央にある丸い空白の位置からレーザスキャナなどで１回だけ測定したために、奥行きのない平面的な点群データとなっていると想像できる。

data2 樹木

樹木の枝先や縁などは $d_i = 1$ が多く見られるが、内側のモヤっとしている箇所は $d_i = 2,3$ が混じっている様子がわかる。

まとめ

今回は"Writing a new PCL class" に従って次元特徴量を計算するカスタムクラスDimensionAnalysisを追加し、実際の点群データでテストした。テンプレートを利用してカスタムクラスを設計したので、少々面倒な過程もあったが、PCL の汎用性を生かしつつ様々な型PointTに対応できる。