VLP16 — C++ Quick Example

こんにちは。PARTYのTechnical Director、梶原です。
この度エンジニアチームみんなでnoteにテック記事書いていきましょーということになりまして、今回は過去にMediumに投稿していた記事の移植からはじめようと思います（手抜き）

PARTYがどういう会社なのか、エンジニアチームがどういうスタイルで仕事をしているのかなどは、また別の記事でご紹介していこうと思います。

1. Introduction

さて、デジタル技術を何がしか使ったインスタレーションにおいて、空間にまつわるデータはインタラクティブで「魔法の様な」体験を作るのに必要な情報を大量にもたらしてくれます。
本記事で紹介するVLP16は、安価に手に入る3D LiDAR(Light Detection and Ranging)のひとつです。
ということで、このセンサーを使ってさくっとプロトタイプを作るためのステップを紹介します。
いざこれを使って何かしよう！というときに、結構情報が少なくてつまづくことが多かったので、なにがしかのヒントになれば幸いと思ってメモ書き程度ですがコードなども記載していきます。

＊執筆時の環境
- Win10
- VS2017

＊前提知識
- C++の基礎
- VisualStudioの基本的な使い方
- PCL(PointCloudLibrary) に関する基礎知識

＊レポジトリ
https://github.com/universax/VLP16_Test.git

2. VLP16について

VLP16はLiDARと呼ばれるセンサーです。半径100mの3D空間データをUDPを使ったストリームデータでリアルタイムに取得することが可能です。従って、このストリームのデータをデコードしてあげればいいことになります。
PCL(PointCloudLibrary)はこのVLP16のためのGrabber（データ取得のインターフェースクラス）を提供してくれているので、これを使っていきます。

3. PCL(PointCloudLibrary)について

PCLは、ポイントクラウドデータを高速に扱うための様々なライブラリの集合体です。

- Boost (http://www.boost.org/)
-Eigen (http://eigen.tuxfamily.org/)
-FLANN (http://www.cs.ubc.ca/research/flann/)
-Visualization ToolKit (VTK) (http://www.vtk.org/)
-etc...

たくさんありますね。PCLが便利なのは、これらを"PCL::"という名前空間の中で機能をまとめたクラスを提供してくれている点です。

＊インストールの仕方
ありがたいことに"all in one package"が提供されています。
(http://unanancyowen.com/en/pcl181/).

＊今回はPCLの基本的なところはかっ飛ばしてます。

4. データの受信

それでは実際にコードをみていきましょう。
簡略化してあるので、細かいことはGithubのレポジトリをみてください。

// VLP16.h
// Grabber Instance
boost::shared_ptr<pcl::VLPGrabber> mVlpGrabber;
// Connection instance for open and close
boost::signals2::connection mConnection;
// Save point cloud data into this pointer
pcl::PointCloud<PointType>::ConstPtr mCloud;
// Mutex
boost::mutex mVLPMutex;

// VLP16.cpp
void VLP16::Start(string ipAddress, unsigned short port){
   // New grabber
   mVlpGrabber = boost::shared_ptr<pcl::VLPGrabber>(new pcl::VLPGrabber(boost::asio::ip::address::from_string(ipAddress), boost::lexical_cast<unsigned short>(port)));
   // Set callback and connection
   boost::function<void(const pcl::PointCloud<PointType>::ConstPtr&)> cb = boost::bind(&VLP16::vlpCallback, this, _1);
   mConnection = mVlpGrabber->registerCallback(cb);
   // Start :)
   mVlpGrabber->start();
}

//Callback
void VLP16::vlpCallback(const pcl::PointCloud<PointType>::ConstPtr& cloudPtr)
{
   // Lock and Copy
   boost::mutex::scoped_lock lock(mVLPMutex);
   mCloud = cloudPtr;
}

void VLP16::Close()
{
   mVlpGrabber->stop();
   mConnection.disconnect();
}

やっていることは簡単で、Grabberのインスタンスを作り、コールバック先を指定、connectionを生成します。
あとは簡単で、単にstart()を呼んでやればdisconnect()でセンサーを閉じるまでデータが流れ続けてきます。
この生データをコピーし、好きに調理していきます。

5. Filter

取得したポイントクラウドデータをフィルターしたり、セグメンテーションしたり、物体表面の法線を取得したり、、、PCLは本当にたくさんの機能を提供してくれます。
ここでは、簡単なフィルタリングの処理を紹介しておきます。

void PCLManager::edgeRmoveFilter(pcl::PointCloud<PointType>::Ptr cloud) {
	pcl::PointCloud<PointNormalType>::Ptr normal = createNormals(cloud);

	vector<int> removeIndex;
	for (int i = 0; i < cloud->points.size(); i++)
	{
		if (abs(normal->points[i].normal_x / normal->points[i].normal_z) > 0.1)
		{
			removeIndex.push_back(i);
		}
	}

	for (int i = 0; i < removeIndex.size(); i++)
	{
		cloud->erase(cloud->points.begin() + removeIndex[i] - i);
	}
}


void PCLManager::statisticalOutlierFilter(pcl::PointCloud<PointType>::Ptr cloud)
{
	pcl::StatisticalOutlierRemoval<PointType> sor;
	sor.setInputCloud(cloud);
	sor.setMeanK(10);
	sor.setStddevMulThresh(1.0);

	pcl::PointCloud<PointType>::Ptr cloud_filtered(new pcl::PointCloud<PointType>);
	sor.filter(*cloud_filtered);

	pcl::copyPointCloud(*cloud_filtered, *cloud);
	cloud_filtered.reset();
}

void PCLManager::voxelGridFilter(float leaf, pcl::PointCloud<PointType>::Ptr cloud)
{
	//VoxelGrid
	pcl::VoxelGrid<PointType> grid;
	grid.setLeafSize(leaf, leaf, leaf);
	grid.setInputCloud(cloud);
	pcl::PointCloud<PointType>::Ptr cloud_filtered(new pcl::PointCloud<PointType>());
	grid.filter(*cloud_filtered);
	pcl::copyPointCloud(*cloud_filtered, *cloud);
	cloud_filtered.reset();
}

void PCLManager::extractIndices(pcl::PointCloud<PointType>::Ptr cloud, pcl::PointIndices::Ptr inliners)
{
	pcl::PointCloud<PointType>::Ptr tmp(new pcl::PointCloud<PointType>());
	pcl::copyPointCloud(*cloud, *tmp);

	pcl::ExtractIndices<PointType> extract;
	extract.setInputCloud(tmp);
	extract.setIndices(inliners);

	extract.setNegative(true);
	extract.filter(*cloud);
}

void PCLManager::radiusOutlinerFilter(pcl::PointCloud<PointType>::Ptr cloud) {
	pcl::RadiusOutlierRemoval<PointType> ror;
	ror.setInputCloud(cloud);
	ror.setRadiusSearch(0.05);
	ror.setMinNeighborsInRadius(2);
	pcl::PointCloud<PointType>::Ptr filterdCloud(new pcl::PointCloud<PointType>());
	ror.filter(*filterdCloud);
	*cloud = *filterdCloud;
	filterdCloud.reset();
}

void PCLManager::passThroughFilter(pcl::PointCloud<PointType>::Ptr inputCloud, const string &fieldName, float min, float max) {
	pcl::PassThrough<PointType> pass;
	pass.setInputCloud(inputCloud);
	pass.setFilterFieldName(fieldName);
	pass.setFilterLimits(min, max);

	pcl::PointCloud<PointType>::Ptr filterdCloud(new pcl::PointCloud<PointType>());
	pass.filter(*filterdCloud);
	*inputCloud = *filterdCloud;
}

void PCLManager::nanRemovalFilter(pcl::PointCloud<PointType>::Ptr cloud) {
	std::vector<int> indices;
	pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);
}

これらを使用するコードは、こんな感じになります。

// Copy for compute
pcl::PointCloud<PointType>::Ptr calcPoints(new pcl::PointCloud<PointType>());
*calcPoints = *mCloud;

// Filter
float voxelVal = 0.01f; // Set Voxel size to 0.01m
voxelGridFilter(voxelVal, calcPoints);
statisticalOutlierFilter(calcPoints);
passThroughFilter(calcPoints, "x", -2.0, 2.0);  //-2.0m to 2.0m
passThroughFilter(calcPoints, "y", 0.0, 2.0);   //0.0m to 2.0m
passThroughFilter(calcPoints, "z", 0.0, 3.0);   //0.0m to 3.0m

その他のPCLのよく使いそうな機能は、Githubレポジトリの"PCLManager"クラスにまとめてあるので参考にしてみてください。

6. Visualize

PCLにはデータ表示の機能も提供されています。
通常のvisualiserであれば、下記の様に簡単に使うことができます。

// Global or Class member
pcl::visualization::CloudViewer mVlpViewer("VLP");

// at loop routine called every frame.
vlpViewer.showCloud(calcPoints);

表示する内容をカスタマイズしたい場合も、比較的簡単にカスタマイズが可能です。
以下の例では、Top / Side / Front / Perspectiveの4つの視点でポイントクラウドを描画し、キーボードの入力を使える様にしています。

#pragma once


enum Edit_Mode {
	Edit_Mode_None = 0,
	Edit_Mode_X,
	Edit_Mode_Y,
	Edit_Mode_Z,
	Edit_Mode_Pitch,
	Edit_Mode_Yaw,
	Edit_Mode_Roll,
	Edit_Mode_Save,
	Edit_Mode_Load
};

class VisualizerManager
{
public:
	VisualizerManager();
	~VisualizerManager() {}

	void updateVisualizer(pcl::PointCloud<PointType>::Ptr inputCloud);

	

private:
	//Visualizer
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
	pcl::PointCloud<PointType>::Ptr showCloud;
	void setupVisualizer(pcl::PointCloud<PointType>::Ptr inputCloud);
	void setDefaultViewPoints();
	void updateDebugInfo();
	Edit_Mode editMode;

	//Event
	void keyboardEventOccurred(const pcl::visualization::KeyboardEvent & event, void * viewer_void);
	void mouseEventOccurred(const pcl::visualization::KeyboardEvent & event, void * viewer_void);

	boost::mutex mutex_lock;
};

実装の中身はこんな感じです。

#include "stdafx.h"
#include "Sensor.h"
#include "VisualizerManager.h"

using namespace std;

VisualizerManager::VisualizerManager()
{
	showCloud.reset(new pcl::PointCloud<PointType>());
	viewer.reset(new pcl::visualization::PCLVisualizer("VLP16 Viewer"));
	setupVisualizer(showCloud);
	editMode = Edit_Mode_None;
	updateDebugInfo();
}

void VisualizerManager::updateVisualizer(pcl::PointCloud<PointType>::Ptr inputCloud)
{
	mutex_lock.lock();
	*showCloud = *inputCloud;
	mutex_lock.unlock();

	for (int i = 0; i < 4; i++)
	{
		string idStr = "Sensor v" + to_string(i + 1);
		viewer->updatePointCloud(showCloud, idStr);
	}
	viewer->spinOnce();
}

void VisualizerManager::setupVisualizer(pcl::PointCloud<PointType>::Ptr inputCloud) {
	//-----V1 bottom-left
	int v1(0);
	viewer->createViewPort(0, 0, 0.5, 0.5, v1);
	viewer->addPointCloud(inputCloud, "Sensor v1", v1);
	viewer->createViewPortCamera(v1);

	//-----V2 bottom-right
	int v2(0);
	viewer->createViewPort(0.5, 0, 1.0, 0.5, v2);
	viewer->addText("Top", 10, 10, "v2", v2);
	viewer->addPointCloud(inputCloud, "Sensor v2", v2);
	viewer->createViewPortCamera(v2);

	//-----V3 top-left
	int v3(0);
	viewer->createViewPort(0, 0.5, 0.5, 1.0, v3);
	viewer->addText("Front", 10, 10, "v3", v3);
	viewer->addPointCloud(inputCloud, "Sensor v3", v3);
	viewer->createViewPortCamera(v3);

	//-----V4 top-right
	int v4(0);
	viewer->createViewPort(0.5, 0.5, 1.0, 1.0, v4);
	viewer->addText("Side", 10, 10, "v4", v4);
	viewer->addPointCloud(inputCloud, "Sensor v4", v4);
	viewer->createViewPortCamera(v4);

	//View Points
	setDefaultViewPoints();

	//-----Common
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 0.5);
	viewer->addCoordinateSystem(0.5);
	viewer->registerKeyboardCallback(&VisualizerManager::keyboardEventOccurred, *this, (void*)&viewer);
}

void VisualizerManager::setDefaultViewPoints() {
	//View Points
	viewer->initCameraParameters();
	viewer->setCameraPosition(3.0, 3.0, -3.0, 0, 0, 0, 1);	//3D
	viewer->setCameraPosition(0, 0.0, 6.0, 0, 1, 0, 2);	//Top
	viewer->setCameraPosition(0, -3.0, 0.0, 0, 0, 1, 3);	//Front
	viewer->setCameraPosition(-4.0, 0.0, 0.0, -1, 0, 0, 4);	//Side
}

void VisualizerManager::updateDebugInfo() {
	//-----Edit Mode
	string editModeStr;
	switch (editMode)
	{
	case Edit_Mode_None:
		editModeStr = " Edit mode -> 1: X, 2: Y, 3: Z, 4: Pitch, 5: Yaw, 6: Roll\n Command -> s: save, l: load, 0: reset view";
		break;
	case Edit_Mode_X:
		editModeStr = "Edit Mode ----- X /// (Up key: increase, Down key: decrease)";
		break;
	case Edit_Mode_Y:
		editModeStr = "Edit Mode ----- Y /// (Up key: increase, Down key: decrease)";
		break;
	case Edit_Mode_Z:
		editModeStr = "Edit Mode ----- Z /// (Up key: increase, Down key: decrease)";
		break;
	case Edit_Mode_Pitch:
		editModeStr = "Edit Mode ----- Pitch /// (Up key: increase, Down key: decrease)";
		break;
	case Edit_Mode_Yaw:
		editModeStr = "Edit Mode ----- Yaw /// (Up key: increase, Down key: decrease)";
		break;
	case Edit_Mode_Roll:
		editModeStr = "Edit Mode ----- Roll /// (Up key: increase, Down key: decrease)";
		break;
	case Edit_Mode_Save:
		editModeStr = "Edit Mode ----- Saved data to Kinect_Settings.xml";
		break;
	case Edit_Mode_Load:
		editModeStr = "Edit Mode ----- Loaded data from Kinect_Settings.xml";
		break;
	default:
		editModeStr = "";
		break;
	}

	viewer->removeText3D("v1", 1);
	viewer->addText(editModeStr, 10, 10, "v1", 1);
}


void VisualizerManager::keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void* viewer_void)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = *static_cast<boost::shared_ptr<pcl::visualization::PCLVisualizer> *> (viewer_void);
	cout << event.getKeySym() << endl;
	string inputKey = event.getKeySym();

	Sensor &sensor = Sensor::GetInstance();

	//---------- Reset View
	if (inputKey == "0")
	{
		setDefaultViewPoints();
	}
	//-----Save Settings
	if (inputKey == "s" || inputKey == "S")
	{
		sensor.saveSensorPostureData();
		editMode = Edit_Mode_Save;
	}
	//-----Load Settings
	if (inputKey == "l" || inputKey == "L")
	{
		sensor.loadSensorPostureData();
		editMode = Edit_Mode_Load;
	}
	//-----Change Edit Mode
	if (inputKey == "1")
	{
		editMode = Edit_Mode_X;
	}
	else if (inputKey == "2")
	{
		editMode = Edit_Mode_Y;
	}
	else if (inputKey == "3")
	{
		editMode = Edit_Mode_Z;
	}
	else if (inputKey == "4")
	{
		editMode = Edit_Mode_Pitch;
	}
	else if (inputKey == "5")
	{
		editMode = Edit_Mode_Yaw;
	}
	else if (inputKey == "6")
	{
		editMode = Edit_Mode_Roll;
	}
	else if (inputKey == "Down" || inputKey == "Up") {
		//do nothing
	}
	else {
		editMode = Edit_Mode_None;
	}


	switch (editMode)
	{
	case Edit_Mode_X:
		if (inputKey == "Down")
		{
			sensor.setX(sensor.getX() - 0.01);
		}
		if (inputKey == "Up")
		{
			sensor.setX(sensor.getX() + 0.01);
		}
		break;
	case Edit_Mode_Y:
		if (inputKey == "Down")
		{
			sensor.setY(sensor.getY() - 0.01);
		}
		if (inputKey == "Up")
		{
			sensor.setY(sensor.getY() + 0.01);
		}
		break;
	case Edit_Mode_Z:
		if (inputKey == "Down")
		{
			sensor.setZ(sensor.getZ() - 0.01);
		}
		if (inputKey == "Up")
		{
			sensor.setZ(sensor.getZ() + 0.01);
		}
		break;
	case Edit_Mode_Pitch:
		if (inputKey == "Down")
		{
			sensor.setPitch(sensor.getPitch() - 0.1);
		}
		if (inputKey == "Up")
		{
			sensor.setPitch(sensor.getPitch() + 0.1);
		}
		break;
	case Edit_Mode_Yaw:
		if (inputKey == "Down")
		{
			sensor.setYaw(sensor.getYaw() - 0.1);
		}
		if (inputKey == "Up")
		{
			sensor.setYaw(sensor.getYaw() + 0.1);
		}
		break;
	case Edit_Mode_Roll:
		if (inputKey == "Down")
		{
			sensor.setRoll(sensor.getRoll() - 0.1);
		}
		if (inputKey == "Up")
		{
			sensor.setRoll(sensor.getRoll() + 0.1);
		}
		break;
	default:
		break;
	}

	updateDebugInfo();
	cout << inputKey << endl;
}

void VisualizerManager::mouseEventOccurred(const pcl::visualization::KeyboardEvent & event, void * viewer_void)
{
}

使い方はこんな感じ。

#include "VisualizerManager.h"

// Instance
VisualizerManager visualizer;

// Within looping routine
visualizer.updateVisualizer(calcPoints);

7. 最後に

ということで、VLP16は比較的入手もしやすく、様々な用途に使えるセンサーですので、みなさま是非ご自身のプロジェクトで使ってみてください。