Open3DをIntel RealSense D435で動かすためにTestRealSense.cppをlibrealsense2に対応するよう改造する

Open3Dは、2018年3月現在v0.1です。頻繁にアップデートがあると思われますので、この記事はすぐ風化する予定です。

Intel RealSense D435は、廃版になってしまったKinect1/2の代替として期待されている低価格のデプスセンサーです。同時に発売されたD415に対し、D435はグローバルシャッター方式であるという優位性があるため、移動ロボット界隈の人の人気を集めています。
D435もOpen3DもIntel製でとても親和性が高そうです。今回の記事では、RealSense D435をOpen3Dで使えるようにしたいと思います。

Intel製だから、そのまんま動くんじゃない？

Open3DのTestコードの中に、TestRealSense.cppというのがあり、最初はこれがすんなり動いてくれるものだとばかり思っていたのですが、２つ問題がありました。１つは、Linuxに対応していないこと。２つ目は、librealsense1にしか対応していないこと。
librealsense1というのは、Intel RealSense SR300より前の古いデプスセンサー用ドライバー/SDKです。関数インターフェースの互換性もありません。そのため、D415/D435は動いてくれません。
仕方ないので、また自分で写経してみることにしました。

librealsense2のインストール

librealsense2のソースコードはこちらです。便宜的にlibrealsense2と呼んでいますが、librealsenseが正式名称のようです。適切にインストールしてください。自分の環境は、Intel CPUを使ったUbuntu 16.04LTSのPCを使用し、librealsenseはソースからビルドしました。
GitHub - IntelRealSense/librealsense: Intel® RealSense™ SDK

TestRealSense2.cppのソースコード

Caution: まだバグがあります。
1. Optionの設定をしても、White Balanceが有効になっているように見えません。
2. Ctrl+Cでプログラムを止めると、プログラムを再実行したときにstreamからデータが拾えません。USBの抜き差しが必要になります。Viewerのウィンドウをxマークで落としてください。

TestRealSense2.cpp

// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

#include <iostream>
#include <thread>
#include <librealsense2/rs.hpp>
#include <Core/Core.h>
#include <Visualization/Visualization.h>

// Why IO.a, Core.a, Visualization.a don't have these objects?
#include <Core/Utility/FileSystem.cpp>
#include <Core/Camera/PinholeCameraTrajectory.cpp>
#include <Core/Geometry/TriangleMesh.cpp>
#include <Core/Geometry/TriangleMeshFactory.cpp>
#include <Core/Geometry/PointCloud.cpp>
#include <IO/ClassIO/PointCloudIO.cpp>
#include <IO/ClassIO/IJsonConvertible.cpp>
#include <IO/ClassIO/IJsonConvertibleIO.cpp>

using namespace three;

void print_option_range(const rs2::sensor& sensor, rs2_option option_type)
{
	// from api_how_to.h
	std::cout << "Supported range from option " << option_type << ":" << std::endl;

	rs2::option_range range = sensor.get_option_range(option_type);
	float default_value = range.def;
	float maximum_supported_value = range.max;
	float minimum_supported_value = range.min;
	float difference_to_next_value = range.step;
	std::cout << "  Min Value     : " << minimum_supported_value << std::endl;
	std::cout << "  Max Value     : " << maximum_supported_value << std::endl;
	std::cout << "  Default Value : " << default_value << std::endl;
	std::cout << "  Step          : " << difference_to_next_value << std::endl;
}

void change_option(const rs2::sensor& sensor, rs2_option option_type, float requested_value)
{
	try
	{
		sensor.set_option(option_type, requested_value);
	}
	catch(const rs2::error& e)
	{
		std::cerr << "Failed to set option " << option_type << ". (" << e.what() << ")" << std::endl;
	}
}

int main(int argc, char **args) try
{
	rs2::context ctx;
	rs2::device_list devices = ctx.query_devices();

	if(devices.size() == 1){
		PrintInfo("There is a connected RealSense device.\n");
	}
	else {
		PrintInfo("There are %d connected RealSense devices.\n",
			devices.size());
	}
	if(devices.size() == 0){
		return 0;
	}

	int dev_id = 0;
	rs2::device dev = devices[dev_id];

        std::string name = "Unknown Device";
        if(dev.supports(RS2_CAMERA_INFO_NAME)){ // include/h/rs_sensor.h
                name = dev.get_info(RS2_CAMERA_INFO_NAME);
        }
	PrintInfo("Using device %d, an %s\n", dev_id, name.c_str());

	std::string sn = "########";
	if(dev.supports(RS2_CAMERA_INFO_SERIAL_NUMBER)){
		sn = std::string("#") + dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
	}
	PrintInfo("    Serial number: %s\n", sn.c_str());

	std::string fn = "########";
	if(dev.supports(RS2_CAMERA_INFO_FIRMWARE_VERSION)){
		fn =dev.get_info(RS2_CAMERA_INFO_FIRMWARE_VERSION);
	}
	PrintInfo("    Firmware version: %s\n\n", fn.c_str());

	int index = 0;
	int rgb_camera = 0;
        int stereo_module = 0;
        std::vector<rs2::sensor> sensors = dev.query_sensors(); // api_how_to.h 
	for(rs2::sensor sensor : sensors){
		if(sensor.supports(RS2_CAMERA_INFO_NAME)){
			PrintInfo("%d: %s\n", index, sensor.get_info(RS2_CAMERA_INFO_NAME));
                        if(sensor.get_info(RS2_CAMERA_INFO_NAME) == std::string("Stereo Module")){
                                stereo_module = index;
                        }
			if(sensor.get_info(RS2_CAMERA_INFO_NAME) == std::string("RGB Camera")){
				rgb_camera = index;
			}
			index++;
		}
	}

	if(sensors[rgb_camera].supports(RS2_OPTION_ENABLE_AUTO_EXPOSURE)){ // rs_option.h
		print_option_range(sensors[rgb_camera], RS2_OPTION_ENABLE_AUTO_EXPOSURE);
		change_option(sensors[rgb_camera], RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0.0); // api_how_to.h
	}
	if(sensors[rgb_camera].supports(RS2_OPTION_EXPOSURE)){
		print_option_range(sensors[rgb_camera], RS2_OPTION_EXPOSURE);
		change_option(sensors[rgb_camera], RS2_OPTION_EXPOSURE, 625.0);
	}
        if(sensors[rgb_camera].supports(RS2_OPTION_GAIN)){
		print_option_range(sensors[rgb_camera], RS2_OPTION_GAIN);
		change_option(sensors[rgb_camera], RS2_OPTION_GAIN, 128.0);
	}
	if(sensors[rgb_camera].supports(RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE)){
		print_option_range(sensors[rgb_camera], RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE);
		change_option(sensors[rgb_camera], RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, 0.0);
	}
	// Above, it seems doesn't work...

	// Decrare Realsense pipeline, encapsulating the actual device and sensors
	rs2::pipeline pipe; // rs_pipeline.h

	rs2::config cfg; // examples/measure/rs-measure.cpp
	cfg.enable_stream(RS2_STREAM_DEPTH, RS2_FORMAT_Z16);
	cfg.enable_stream(RS2_STREAM_COLOR, RS2_FORMAT_RGB8);

	// Start streaming with recommended configuration
	auto profile = pipe.start(cfg);
	std::this_thread::sleep_for(std::chrono::milliseconds(50));
	if(sensors[stereo_module].supports(RS2_OPTION_WHITE_BALANCE)){
		change_option(sensors[stereo_module], RS2_OPTION_WHITE_BALANCE, 2100.0);
	}

	auto depth_stream = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
	auto depth_intr = depth_stream.get_intrinsics(); // Calibration data
	PrintInfo("%d %d %.6f %.6f %.6f %.6f\n",
			depth_intr.width, depth_intr.height,
			depth_intr.fx, depth_intr.fy,
			depth_intr.ppx, depth_intr.ppy);
	for(int i=0; i<5; i++){
		PrintInfo("%.6f ", depth_intr.coeffs[i]);
	}
	PrintInfo("\n\n");

	auto color_stream = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
	auto color_intr = color_stream.get_intrinsics();
	PrintInfo("%d %d %.6f %.6f %.6f %.6f\n",
                        color_intr.width, color_intr.height,
                        color_intr.fx, color_intr.fy,
                        color_intr.ppx, color_intr.ppy);
        for(int i=0; i<5; i++){
                PrintInfo("%.6f ", color_intr.coeffs[i]);
        }
        PrintInfo("\n\n");

	// get_extrinsics from api_how_to.h
	try
	{
		auto extrinsics = depth_stream.get_extrinsics_to(color_stream);
		for(int i=0; i<9; i++){
			PrintInfo("%.6f ", extrinsics.rotation[i]);
		}
		PrintInfo("\n");
		for(int i=0; i<3; i++){
			PrintInfo("%.6f ", extrinsics.translation[i]);
		}
		PrintInfo("\n");
	}
	catch(const std::exception& e)
	{
		std::cerr << "Failed to get extrinsics for the given streams. " << e.what() << std::endl;
	}

        auto depth_image_ptr = std::make_shared<Image>();
        depth_image_ptr->PrepareImage(depth_intr.width, depth_intr.height, 1, 2);
        auto color_image_ptr = std::make_shared<Image>();
        color_image_ptr->PrepareImage(color_intr.width, color_intr.height, 3, 1);
        FPSTimer timer("Realsense stream");

	Visualizer depth_vis, color_vis;
	if(depth_vis.CreateWindow("Depth", depth_intr.width, depth_intr.height, 15, 50) == false ||
		depth_vis.AddGeometry(depth_image_ptr) == false ||
	   color_vis.CreateWindow("Color", color_intr.width, color_intr.height, 675, 50) == false ||
		color_vis.AddGeometry(color_image_ptr) == false) {
		return 0;
	}
 
	while(depth_vis.PollEvents() && color_vis.PollEvents()){
		timer.Signal();

		rs2::frameset data = pipe.wait_for_frames();	// Wait for next set of frames from the camera
	
		rs2::frame depth = data.get_depth_frame();	// Find the depth data
		rs2::frame color = data.get_color_frame();	// Find the color data

		memcpy(depth_image_ptr->data_.data(),
			depth.get_data(), depth_intr.width * depth_intr.height * 2);
		memcpy(color_image_ptr->data_.data(),
			color.get_data(), color_intr.width * color_intr.height * 3);
	
		depth_vis.UpdateGeometry();
		color_vis.UpdateGeometry();

		if(sensors[rgb_camera].supports(RS2_OPTION_WHITE_BALANCE)){
			PrintInfo("\r %.2f\t", sensors[rgb_camera].get_option(RS2_OPTION_WHITE_BALANCE));
		}
	}

	return 1;
}
catch(const rs2::error & e)
{
	std::cerr << "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::cerr << e.what() << std::endl;
	return EXIT_FAILURE;
}

CMakeを書くのが面倒（というより、書き方をよく分かってない）なので、大した量でもないのでオプションを平で書いてコンパイルします。cvlはユーザー名でworkspaceはソースを置いているディレクトリです。適当に置き換えてください。openFrameworksと共存させたい目的で、一部のライブラリ(libjsoncpp.a)を自前でインストールした物を使っています。必要なければ外してください。

g++ -std=c++14 -I/home/cvl/workspace/librealsense/include/ -I/home/cvl/workspace/Open3D/src/ -I/usr/include/eigen3/ TestRealSense.cpp /home/cvl/workspace/Open3D/build/lib/*.a `pkg-config --static --libs gl` `pkg-config --static --libs glew` -lglfw -lgomp -ljpeg /usr/local/lib/libjsoncpp.a -L/usr/local/lib -lrealsense2

2018/03/26 追記

if(Open3D_BUILD_LIBREALSENSE2)
    find_package(realsense2 REQUIRED)　# native installed only 
    add_executable(TestRealSense2 TestRealSense2.cpp)
    target_link_libraries(TestRealSense2 Visualization realsense2)
    set_target_properties(TestRealSense2 PROPERTIES
                        FOLDER "Test"
                        RUNTIME_OUTPUT_DIRECTORY "${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/Test")
endif(Open3D_BUILD_LIBREALSENSE2)

実行すると、

./a.out
There is a connected RealSense device.
Using device 0, an Intel RealSense 435
Serial number: #801212070***
Firmware version: 05.09.02.00
0: Stereo Module
1: RGB Camera
Supported range from option Enable Auto Exposure:
Min Value : 0
Max Value : 1
Default Value : 1
Step : 1
Supported range from option Exposure:
Min Value : 41
Max Value : 10000
Default Value : 166
Step : 1
Supported range from option Gain:
Min Value : 0
Max Value : 128
Default Value : 64
Step : 1
Supported range from option Enable Auto White Balance:
Min Value : 0
Max Value : 1
Default Value : 1
Step : 1
1280 720 643.699280 643.699280 640.845093 349.351471
0.000000 0.000000 0.000000 0.000000 0.000000
1920 1080 1384.542603 1385.032715 961.749573 533.023682
0.000000 0.000000 0.000000 0.000000 0.000000
0.999969 0.004246 -0.006592 -0.004221 0.999984 0.003856 0.006608 -0.003828 0.999971
0.014762 0.000241 -0.000011
4600.00 Realsense stream at 11.50 fps.
4600.00 Realsense stream at 16.31 fps.
4600.00 Realsense stream at 16.24 fps.

f:id:cvl-robot:20180308205319p:plain
f:id:cvl-robot:20180308205323p:plain

一応、ちゃんと動いているように見えます。フィードバック歓迎しますので（というかお願いしますから）、何か問題を見つけたら直して教えてください。
ほぼ、TestRealSense.cppをそのまま書き換えることには成功しましたが、もともとこのコードは素のデータを画面表示するだけで殆ど何もしてくれてない！Depthの値とか、Rectifyとかどうなってるのか知りたいのに！RealSenseをもっと勉強しないと、駄目ですね。