IntelRealSenseD435を使ってみる

2020年6月7日

こんにちは。画像処理を始めた理由のひとつがロボットに積んであーだこーだしたかったというのがあります。今回は、IntelのRealSenseD435を使ってみました。まだ、SDKのプログラムをしっかりと読み進めていない状態ですが、Windows10の環境構築の忘備録として書いていきたいと思います。最終的にはROSで使用することになるのかな？とか考えています。

環境構築の流れに関しては、RealSenseのGithubのリポジトリに書いてあるので、詳しくはGithubのリポジトリをみていただければと思います。URLはこちらです。

 




環境を整える

GithubからSDKをダウンロード

さきほど紹介したGithubのリポジトリのreleasesからSDKをインストールしましょう。ubuntuやwindows、mac等様々なOSに対応しているようなので各自の環境にあったものをインストールすればOKだと思います。

Visual Studio Communityのプロジェクトのプロジェクトの設定を行う

OpenCVの環境構築のときにやったときと同じように、プロジェクトのプロパティから設定を行っていきます。

私の環境では次のように設定しました。

opencvと一緒に設定
include に追加
;C:/opencv349/install/include;C:/opencv349/install/include;C:/Program Files (x86)/Intel RealSense SDK 2.0/include;

linkerに追加
C:/Program Files (x86)/Intel RealSense SDK 2.0/lib/x64;C:/opencv349/install/x64/vc16/lib

入力に追加
;opencv_world349.lib;realsense2.lib;

また、PATHにbinファイルを追加しておきました。

C:Program Files (x86)Intel RealSense SDK 2.0binx64

ちゃんとビルドが通るかチェックしてみる

main関数を作成して、以下のプログラムでビルドが通るかどうかの確認を行いました。

#include <librealsense2/rs.hpp>
#include <opencv2/opencv.hpp>

#include <iostream>

using namespace std;
using namespace cv;
using namespace rs2;

int main()
{
    pipeline pipe;
    Mat data;
    cout << "hello world" << endl;
    return 0;
}

openCVやRealSenseのSDKのクラスを作成できるかどうかのチェックをしているだけです。設定が間違っている場合はエラーがでてしまうと思います。

OpenCVとRealSenseのSDKを利用して画像を表示してみる

Wrapperクラスを作成する

OpenCVとRealSenseを組み合わせて使用したいということで、RealSenseのSDKから取得したデータをOpenCVのデータ形式に変換して使えるようにする必要があります。毎回、プログラムを書いたりコピペするのは面倒だと考えられるのでwrapperクラスを作成して、そちらから使用するという方法をとりました。

ここで紹介するwrapperクラスは、RGBカメラからの画像を取得するプログラムとDepthカメラを距離に対して色がついた状態にした画像を取得するということができるようにしました。プロトタイプのプログラムを下記に載せておきます。mainでtry-catchによる例外処理をしています。例えば、realsenseが繋がっていないときに例外処理で止まるようになっています。

#include <librealsense2/rs.hpp>
#include <opencv2/opencv.hpp>

#include <iostream>

#include "realSensseWrapper.h"

using namespace std;
using namespace cv;
using namespace rs2;

int main() try
{
	realSensseWrapper sense;

	sense.init();

	while (true) {
		sense.setFrame();

		Mat color = sense.getColorImage();
		Mat depth = sense.getDepthImage();
		Mat distance = sense.getDepthDistance();

		imshow("color frame", color);
		imshow("depth frame", depth);
		imshow("depth distance", distance);

		// q key exit
		int key = waitKey(1);
		if (key == 'q') break;
	}

	destroyAllWindows();

	return 0;

}
catch (const rs2::error & e)
{
	std::cout << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):n    " << e.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const std::exception & e)
{
	std::cout << e.what() << std::endl;
	return EXIT_FAILURE;
}
#pragma once

#include <librealsense2/rs.hpp>
#include <opencv2/opencv.hpp>

class realSensseWrapper
{
private:
	rs2::pipeline* pipe = nullptr;
	cv::Size color_size = cv::Size(1280, 720);
	cv::Size depth_size = cv::Size(1280, 720);
	int color_fps = 30;
	int depth_fps = 30;

	rs2::colorizer color_map;
	
	rs2::frame color_frame;
	rs2::frame depth_color_frame;
	rs2::frame depth_distance_frame;

	bool is_data = false;

public:
	enum {
		COLOR_CAMERA,
		DEPTH_CAMERA,
	};


public:
	// コンストラクタ
	realSensseWrapper();
	// デストラクタ
	~realSensseWrapper();

	// 画像サイズの設定
	void setCameraSize(int select, cv::Size ksize);

	// fpsを設定
	void setFps(int _color_fps = 30, int _depth_fps = 30);

	// 初期設定用関数
	int init();

	// 画像サイズを取得
	cv::Size getSize(int select);

	// 画像を取得する前に呼ぶ必要がある
	void setFrame();

	// RGBカメラのデータをMatクラスの形式で返す
	cv::Mat getColorImage();

	// Depthカメラのデータを色付きにしてMatクラスの形式で返す
	cv::Mat getDepthImage();

	// Depthカメラの距離のデータをMatクラスの形式で返す
	cv::Mat getDepthDistance();
};


#include "realSensseWrapper.h"

#include <iostream>

using namespace std;
using namespace cv;
using namespace rs2;

realSensseWrapper::realSensseWrapper()
{
	// デバイスを抽象化するパイプラインを構築
	if (pipe == nullptr) {
		pipe = new pipeline();
	}
}

realSensseWrapper::~realSensseWrapper()
{
	delete pipe;
}

void realSensseWrapper::setCameraSize(int select, Size ksize)
{
	if(select == COLOR_CAMERA){
		if (ksize.width > 1920) ksize.width = 1920;
		else if (ksize.width < 1) ksize.width = 1;

		if (ksize.height > 1080) ksize.height = 1080;
		else if (ksize.height < 1) ksize.height = 1;

		color_size = ksize;
	}
	else if (select == DEPTH_CAMERA) {
		if (ksize.width > 1280) ksize.width = 1280;
		else if (ksize.width < 1) ksize.width = 1;

		if (ksize.height > 720) ksize.height = 720;
		else if (ksize.height < 1) ksize.height = 1;
		depth_size = ksize;
	}
	else {
		cout << "selection is missing" << endl;
	}
}

void realSensseWrapper::setFps(int _color_fps, int _depth_fps)
{
	if (_color_fps < 0) _color_fps = 1;
	else if (_color_fps > 90) _color_fps = 90;

	if (_depth_fps < 0) _depth_fps = 1;
	else if (_depth_fps > 90) _depth_fps = 90;

	color_fps = _color_fps;
	depth_fps = _depth_fps;
}

int realSensseWrapper::init()
{

	config cfg;
	// 明示的にストリームを有効化する
	// カラーストリーム画像をBGR8フォーマット
	cfg.enable_stream(RS2_STREAM_COLOR, color_size.width, color_size.height, RS2_FORMAT_BGR8, color_fps);
	// デプスカメラのz16フォーマット
	cfg.enable_stream(RS2_STREAM_DEPTH, depth_size.width, depth_size.height, RS2_FORMAT_Z16, depth_fps);

	pipe->start(cfg);

	// 初期化に成功したら1を返す
	return 1;
}

Size realSensseWrapper::getSize(int select)
{
	if (select == DEPTH_CAMERA) {
		return depth_size;
	}
	else {
		return color_size;
	}
}

void realSensseWrapper::setFrame()
{
	// ストリームがフレームセットを取得するまで待機
	frameset frames = pipe->wait_for_frames();

	// フレームセットからカラーフレームを取得
	color_frame = frames.get_color_frame();
	// フレームセットからデプスフレームを取得(BGRの色つきのマップに変更)
	depth_color_frame = frames.get_depth_frame().apply_filter(color_map);
	// フレームセットからデプスフレームを取得(距離用)
	depth_distance_frame = frames.get_depth_frame();

	is_data = true;
}

Mat realSensseWrapper::getColorImage()
{
	if (!is_data) {
		// 初回のみ、データを取得する
		setFrame();
	}
	
	Mat data(color_size, CV_8UC3, (void*)color_frame.get_data(), cv::Mat::AUTO_STEP);
	return data;
	

}

Mat realSensseWrapper::getDepthImage()
{
	if (!is_data) {
		// 初回のみ、データを取得する
		setFrame();
	}
	
	Mat data(depth_size, CV_8UC3, (void*)depth_color_frame.get_data(), cv::Mat::AUTO_STEP);
	cvtColor(data, data, COLOR_RGB2BGR);
	return data;
}

Mat realSensseWrapper::getDepthDistance()
{
	Mat dis(depth_size, CV_16UC1, (void*)depth_distance_frame.get_data(), cv::Mat::AUTO_STEP);
	dis.convertTo(dis, CV_64F);
	auto depth_scale = pipe->get_active_profile().get_device().first<depth_sensor>().get_depth_scale();
	dis = dis * depth_scale;
	return dis;
}

さいごに

RealSenseをWindows環境でOpenCVと一緒に使用ができるようになりました。参考になれば幸いです。

プログラムに問題等あれば指摘をしていただけると助かります。