Go to Qiita Advent Calendar 2024 Top
LoginSignup
99
77

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 5 years have passed since last update.

@n_chiba_(Naoya Chiba)

SSII2018のTSを例題に，PCL (C++)とOpen3D (Python) の比較

Last updated at Posted at 2018-08-12

SSII2018のチュートリアルセッションで，秋月先生がOpen3Dを紹介されていた．
参考: https://confit.atlas.jp/guide/event/ssii2018/static/tutorial
参照スライド: https://www.slideshare.net/SSII_Slides/3d-101077557
ソース: https://github.com/sakizuki/SSII2018_Tutorial_Open3D

自分は普段点群処理をPCL (Point Cloud Library)で行っているが，コンパイルが遅いなど不満はありPythonで点群処理ができればだいぶうれしい．せっかくなのでOpen3Dのサンプルを写経すると同時に，普段使っているPCLでも実装してみて，書き心地の違いなどをまとめる．
環境はWindows，この記事のように構築．Open3Dもソースをコンパイルして導入．

比較対象

PCL (Point Cloud Library): http://pointclouds.org/
点群処理ライブラリ．C++で使える．
Open3D: http://www.open3d.org/
点群処理ライブラリ．C++とPythonで使える．今回はPythonから使用．

まずOpen3Dに慣れる

自分がOpen3Dに慣れていないので，写経してOpen3Dに慣れる．
ページ番号は参照スライドのページと対応．

読み込み，表示，保存（p.53）

import open3d

pointcloud = open3d.read_point_cloud("scene1.ply")
open3d.draw_geometries([pointcloud])
open3d.write_point_cloud("output.pcd", pointcloud)
ここら辺はやっぱりとても簡単にできる． `draw_geometries`の引数が点群じゃなく点群のリストなのが若干トラップ気味かもしれない． 表示がとても綺麗なのは気持ちが良い．

ダウンサンプリング，法線推定（p.53-54）

# ダウンサンプリング
pointcloud_ds = open3d.voxel_down_sample(pointcloud, voxel_size = 0.01)
open3d.draw_geometries([pointcloud_ds])

# 法線推定
open3d.estimate_normals(
    pointcloud,
    search_param = open3d.KDTreeSearchParamHybrid(
        radius = 0.1, max_nn = 30))
open3d.draw_geometries([pointcloud])

# ちなみに法線推定後は has_normals() が True になる
print(pointcloud.has_normals())

# 法線の方向を視点ベースでそろえる
open3d.orient_normals_towards_camera_location(
    pointcloud,
    camera_location = np.array([0., 10., 10.], dtype="float64"))
open3d.draw_geometries([pointcloud])

ダウンサンプリング

法線推定

法線の方向をそろえた

ダウンサンプリング，法線推定もとても楽々書ける．
estimate_normalsなどは戻り値ではなく引数の点群に対してNormalを与えることに注意．
estimate_normalsを叩いてからdraw_geometriesをすると，'n'キーで法線が可視化される．
PCLでのsetSearchSurface的なこと（全点に対して法線推定はしないけど，法線推定で近傍点を取り出すときにはすべての点を参照する）ができないようなので（ドキュメント調査不足？），その点は気になる．
毛だらけのうさぎを深夜にずっと見ているとなんとも言えない気持ちになることも分かった．

NumPy との連携（p.55）

import numpy as np

# ランダムな点を生成
data = np.random.rand(10, 3)

# Open3Dの点群に変換
pointcloud = open3d.PointCloud()
pointcloud.points = open3d.Vector3dVector(data)

# ランダムに打った点を可視化
open3d.draw_geometries([pointcloud])

# numpy.ndarrayに変換．元のデータともちろん一致する
xyz = np.asarray(pointcloud.points)
print(np.linalg.norm(data - xyz))

NumPyとの連携もだいたい思った通りに書ける．良い．

Open3DとPCLの比較

スライドp.56-62のモデルベースマッチングをPCLとOpen3Dで実装してみる．以降，関数単位で比較．
どちらか一方ではできないと書いている部分について，もし調査不足であればぜひ教えていただけるとありがたい．
テストデータとして，Stanford Bunnyの点群から昔作ったステレオで計測した点群のシミュレーションプログラムで片面点群にしたものを使用して仮想カメラ位置を移動して得た2視点分の点群を作っておく．

Scene 1 Scene 2 Scene 1+2

インポート・インクルード

Open3D
import open3d
import numpy as np
import copy
PCL
#include <string>
#include <tuple>
#include <Eigen/Core>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>

PCLは巨大なためヘッダが分割されており，使いたい機能がどのヘッダにあったか毎回調べる必要がある（自分だけ？）．

法線推定と特徴量計算

Open3D
def preprocess_point_cloud(pointcloud, voxel_size):
    # Keypoint を Voxel Down Sample で生成
    keypoints = open3d.voxel_down_sample(pointcloud, voxel_size)

    # 法線推定
    radius_normal = voxel_size * 2
    view_point = np.array([0., 10., 10.], dtype="float64")
    open3d.estimate_normals(
        keypoints,
        search_param = open3d.KDTreeSearchParamHybrid(radius = radius_normal, max_nn = 30))
    open3d.orient_normals_towards_camera_location(keypoints, camera_location = view_point)

    #　FPFH特徴量計算
    radius_feature = voxel_size * 5
    fpfh = open3d.compute_fpfh_feature(
        keypoints,
        search_param = open3d.KDTreeSearchParamHybrid(radius = radius_feature, max_nn = 100))

    return keypoints, fpfh
PCL
// 法線推定と特徴量計算
auto preprocess_point_cloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointcloud, const float voxel_size)
{
	//Keypoint を Voxel Down Sample で生成
	const pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints(new pcl::PointCloud<pcl::PointXYZ>);
	const boost::shared_ptr<pcl::VoxelGrid<pcl::PointXYZ>> sor(new pcl::VoxelGrid<pcl::PointXYZ>); // なぜかPtrがprotectedなので
	sor->setInputCloud(pointcloud);
	sor->setLeafSize(voxel_size, voxel_size, voxel_size);
	sor->filter(*keypoints);

	// 法線推定
	const float radius_normal = voxel_size * 2.0;
	const auto view_point = pcl::PointXYZ(0.0, 10.0, 10.0);

	const pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	const pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal>::Ptr ne(new pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal>);
	const pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
	ne->setInputCloud(pointcloud);
	ne->setRadiusSearch(radius_normal);
	ne->setSearchMethod(kdtree);
	ne->setViewPoint(view_point.x, view_point.y, view_point.z);
	ne->compute(*normals);

	// FPFH特徴量計算
	const float radius_feature = voxel_size * 5.0;

	const pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh(new pcl::PointCloud<pcl::FPFHSignature33>);
	const pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33>::Ptr fpfhe(new pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33>);
	fpfhe->setInputCloud(keypoints);
	fpfhe->setSearchSurface(pointcloud); // Open3Dだとこれができない（？）
	fpfhe->setInputNormals(normals);
	fpfhe->setSearchMethod(kdtree);
	fpfhe->setRadiusSearch(radius_feature);
	fpfhe->compute(*fpfh);

	return std::make_pair(keypoints, fpfh);
}

当然ながらPCLのほうが長いため読みづらい．
型がごちゃごちゃしていることを別にしても，Pythonのキーワード引数はこのような場合に相性が良い．
PCLもパラメータがそれぞれ何なのか分かるように努力しているが，Open3D（というかPython）実装のほうがすっきりする．
ライブラリの機能としてはsetSearchSurfaceがつかえる点でPCLが優秀．

RANSAC による Global Registration

Open3D
def execute_global_registration(kp1, kp2, fpfh1, fpfh2, voxel_size):
    distance_threshold = voxel_size * 2.5
    result = open3d.registration_ransac_based_on_feature_matching(
        kp1, kp2, fpfh1, fpfh2, distance_threshold,
        open3d.TransformationEstimationPointToPoint(False), 4,
        [open3d.CorrespondenceCheckerBasedOnEdgeLength(0.9),
         open3d.CorrespondenceCheckerBasedOnDistance(distance_threshold)],
        open3d.RANSACConvergenceCriteria(500000, 1000))
    return result
PCL
auto execute_global_registration(
	const std::pair<const pcl::PointCloud<pcl::PointXYZ>::Ptr, const pcl::PointCloud<pcl::FPFHSignature33>::Ptr>& scene1,
	const std::pair<const pcl::PointCloud<pcl::PointXYZ>::Ptr, const pcl::PointCloud<pcl::FPFHSignature33>::Ptr>& scene2,
	const float voxel_size)
{
	const auto& kp1 = scene1.first;
	const auto& kp2 = scene2.first;
	const auto& fpfh1 = scene1.second;
	const auto& fpfh2 = scene2.second;

	const float distance_threshold = voxel_size * 2.5;

	const pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
	const pcl::SampleConsensusPrerejective<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33>::Ptr align(new pcl::SampleConsensusPrerejective<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33>);
	align->setInputSource(kp1);
	align->setSourceFeatures(fpfh1);
	align->setInputTarget(kp2);
	align->setTargetFeatures(fpfh2);
	align->setMaximumIterations(500000);
	align->setNumberOfSamples(4);
	align->setCorrespondenceRandomness(2);
	align->setSimilarityThreshold(0.9f);
	align->setMaxCorrespondenceDistance(distance_threshold);
	align->setInlierFraction(0.25f);
	align->align(*output);

	return align->getFinalTransformation();
}

setInlierFraction，setCorrespondenceRandomnessの機能でPCLが優れる．
Open3Dのほうが実行結果を綺麗に返しているような印象．
型名をいちいち書く関係でC++は読みにくく，これもPythonのほうがすっきり読める．

ICP によるRegistration

Open3D
def refine_registration(scene1, scene2, trans, voxel_size):
    distance_threshold = voxel_size * 0.4
    result = open3d.registration_icp(
        scene1, scene2, distance_threshold, trans,
        open3d.TransformationEstimationPointToPoint())
    return result
PCL
auto refine_registration(
	const pcl::PointCloud<pcl::PointXYZ>::Ptr& scene1,
	const pcl::PointCloud<pcl::PointXYZ>::Ptr& scene2,
	const Eigen::Matrix4f& trans,
	const float voxel_size)
{
	const auto scene1_temp = scene1->makeShared();
	pcl::transformPointCloud(*scene1_temp, *scene1_temp, trans);

	const float distance_threshold = voxel_size * 0.4;

	const pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
	const pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>::Ptr icp(new pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>);
	icp->setInputSource(scene1_temp);
	icp->setInputTarget(scene2);
	icp->setMaxCorrespondenceDistance(distance_threshold);
	icp->align(*output);

	return icp->getFinalTransformation();
}

剛体変換行列の初期値を入力できる点でOpen3Dが優れる．
Pythonのほうが読みやすい点は他と同様．

点群の読み込み

Open3D
# 注）一行なので関数として分けていない
open3d.read_point_cloud("scene1.ply")
PCL
auto read_point_cloud(const std::string& filename)
{
	const pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>);
	const int retval = pcl::io::loadPLYFile(filename, *pointcloud);
	if (retval == -1 || pointcloud->size() <= 0)
	{
		PCL_ERROR("File load error.");
		exit(-1);
	}

	return pointcloud;
}

PCLだけエラー処理を書いているのでフェアな比較ではないが，PCLにおいてpointcloud->size()のチェックは必須なので（チェックしないておくと思わぬタイミングでエラー吐いてビビる），まあこんな感じ．
PCLの入出力は他にも色々な書き方ができるため，他人のコードが読みにくくなりがちな印象．

位置姿勢推定結果の表示

Open3D
def draw_registration_result(source, target, transformation):
    source_temp = copy.deepcopy(source)
    target_temp = copy.deepcopy(target)
    source_temp.paint_uniform_color([1, 0.706, 0])
    target_temp.paint_uniform_color([0, 0.651, 0.929])
    source_temp.transform(transformation)
    open3d.draw_geometries([source_temp, target_temp])
PCL
auto draw_registration_result(
	const std::string& window_name,
	const pcl::PointCloud<pcl::PointXYZ>::Ptr& scene1,
	const pcl::PointCloud<pcl::PointXYZ>::Ptr& scene2,
	const Eigen::Matrix4f& trans)
{
	const pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer(window_name));
	viewer->setBackgroundColor(255.0, 255.0, 255.0);

	const auto scene1_temp = scene1->makeShared();
	const auto scene2_temp = scene2->makeShared();

	pcl::transformPointCloud(*scene1_temp, *scene1_temp, trans);

	const pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>::Ptr color1(new pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(scene1_temp, 1.0*255, 0.706 * 255, 0.0 * 255));
	const pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>::Ptr color2(new pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(scene2_temp, 0.0 * 255, 0.651 * 255, 0.929 * 255));
	viewer->addPointCloud<pcl::PointXYZ>(scene1_temp, *color1, "scene1");
	viewer->addPointCloud<pcl::PointXYZ>(scene2_temp, *color2, "scene2");

	return viewer;
}

言うまでもなくOpen3Dの方がシンプルで書きやすい．個人的にPCLの一番使いにくいと思っている点がこのVisualization周りで，とてもごちゃごちゃする．
普段は自分用にラッパーを書いて使っており，今回久しぶりに直接叩いたらやっぱり使いにくかった．
特に色の指定，doubleなのに[0, 1]ではなく[0, 255]なのはとても違和感がある．
一応PCLVisualizerが高機能なのは事実で，addText3Dとかは便利だし楽しい．加えて直接VTKを触れるようになっている点は良い．

メイン

Open3D
# 読み込み
scene1 = open3d.read_point_cloud("scene1.ply")
scene2 = open3d.read_point_cloud("scene2.ply")

# scene2 を適当に回転・並進
transform_matrix = np.asarray([
    [1., 0., 0., -0.1],
    [0., 0., -1., 0.1],
    [0., 1., 0., -0.1],
    [0., 0., 0., 1.]], dtype="float64")
scene2.transform(transform_matrix)

# 位置合わせ前の点群の表示
draw_registration_result(scene1, scene2, np.eye(4))

voxel_size = 0.01

# RANSAC による Global Registration
scene1_kp, scene1_fpfh = preprocess_point_cloud(scene1, voxel_size)
scene2_kp, scene2_fpfh = preprocess_point_cloud(scene2, voxel_size)
result_ransac = execute_global_registration(scene1_kp, scene2_kp, scene1_fpfh, scene2_fpfh, voxel_size)
draw_registration_result(scene1, scene2, result_ransac.transformation)

# ICP による refine
result = refine_registration(scene1, scene2, result_ransac.transformation, voxel_size)
draw_registration_result(scene1, scene2, result.transformation)
PCL
int main()
{
	// 読み込み
	const auto scene1 = read_point_cloud("scene1.ply");
	const auto scene2 = read_point_cloud("scene2.ply");

	// scene2 を適当に回転・並進
	Eigen::Matrix4f transform_matrix;
	transform_matrix <<
		1.0, 0.0, 0.0, -0.1,
		0.0, 0.0, -1.0, 0.1,
		0.0, 1.0, 0.0, -0.1,
		0.0, 0.0, 0.0, 1;
	pcl::transformPointCloud(*scene2, *scene2, transform_matrix);

	// 位置合わせ前の点群の表示
	const auto viewer1 = draw_registration_result("Initial", scene1, scene2, Eigen::Matrix4f::Identity());

	const float voxel_size = 0.01;

	//RANSAC による Global Registration
	const auto scene1_kp_fpfh = preprocess_point_cloud(scene1, voxel_size);
	const auto scene_kp_fpfh = preprocess_point_cloud(scene2, voxel_size);
	const auto result_ransac = execute_global_registration(scene1_kp_fpfh, scene_kp_fpfh, voxel_size);
	const auto viewer2 = draw_registration_result("Global", scene1, scene2, result_ransac);

	// ICP による refine
	const auto result = refine_registration(scene1, scene2, result_ransac, voxel_size);
	const auto viewer3 = draw_registration_result("Refined", scene1, scene2, result * result_ransac);
	
	while (!viewer1->wasStopped() && !viewer2->wasStopped() && !viewer3->wasStopped())
	{
		viewer1->spinOnce();
		viewer2->spinOnce();
		viewer3->spinOnce();
	}
}

メインパートは大差ない（というかメインパートが大差なくなるように書いた）．
Open3DでもNon-blockingなvisualizationはできるらしいが，ちょっと面倒そうなのでやめた．
このあたりはアプリケーションによるだろうけど，個人的にはPCLの方が好み．

今回はバラしていないので関係ないが，普通C++でpair（とかtupleとか）を展開するにはstd::tieを使う．
しかし自分のようにconstじゃなくなるのが気持ち悪い人は，多分const autoで受け取ってからstd::getを使うことになってごちゃごちゃする．このあたりはPythonが書きやすい．

ちなみに総行数はOpen3Dが88行，PCLが182行．

実行結果

適当に配置 Global Registration ICP
Open3D
PCL
当然どちらでもできる．
（FPFHを計算している点群が違う＆RANSACに乱数が含まれるため，完全には一致しない）

その他感想

ライブラリとしては，この程度のことをするのであればどっちでも良い，という印象．
込み入ったことをやろうとするとPCLの方が色々実装されてたり小回りも効いたり良いのかも．
Open3Dはもっともっさりするのかと思っていたが，ほぼC++で実装されている部分らしくこの程度であれば実行時間も気にならない．

C++はいちいち型を書くのがやはり苦痛．今回のようにautoを使えばだいぶ書きやすくなるのでぜひ使って欲しいが，それでもPythonよりは書きにくい．
PCLはテンプレートを多用しているため，コンパイルが遅い上Visual StudioのIntelliSenseが結構反応が遅くなる．
今回はOpen3DはJupyterLab環境のPythonから利用したが，よほどこっちのほうがサクサク実装できるように感じた．
CMakeLists.txtを書いてCMakeでプロジェクト作って，という作業も（CMake以前よりは相当楽になったとはいえ）ちょっと面倒．環境構築できていればいきなり使える点でも，Python+Open3Dが良い．

PCLをPythonで使うという手もあり（例えば https://github.com/strawlab/python-pcl ），自分は触っていないけど多分便利な気がする．
ただし，Open3Dのキーワード引数の有効活用を前提としたライブラリ設計と比較すると，PCLのPythonバインディングよりはOpen3Dに期待したいような感触がある．

まとめ

まだPCLほど高機能ではないが，Pythonで点群処理ができるのは素晴らしい．実行速度をあまり気にしないときはOpen3Dを使うのが良さげ．
各点ごとに面倒な処理をするとか，PCLにしか実装されていないとか，実行速度が重要なときとかはPCL．

ソースコード

PCL: https://gist.github.com/naoya-chiba/9cdeb04439ee7998687a41c03ebd21d0
Open3D: https://gist.github.com/naoya-chiba/b77c6a88a7a40fad005668de6bce0d0d

99
77
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
99
77

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?