cvl-robot's diary

研究ノート　メモメモ https://github.com/dotchang/

自分用読むべき論文メモ 2018年04月版

github.com
MotionCaptureデータのビューアやアノテーションなどに使えるopenFrameworksアドオン。VRM対応誰かやってくれないかな。

nico-opendata.jp
openpose.pyの出力を元に３次元位置推定．

ロボット-カメラキャリブレーション (robot camera calibration)

https://www.researchgate.net/publication/260691259_The_Open_Motion_Planning_Library
The Open Motion Planning Library
Robotics/Exercise/OMPLProgramming - NAIST::OnlineText

メモ： openFrameworksで2点を指定してofCylinderPrimitiveを描画する方法

忘れそうだったのでメモ。
端点を2点指定してその間にシリンダーを描画するサンプル。
ofCylinderPrimitiveは、デフォルトで(0,1,0)の方向に長軸が表示されるので、そのベクトルを2点を結ぶベクトルに沿うように回転させるだけ。
原理は簡単。忘れそうなのは、ofVec3fで外積を求める方法。
z.cross(v)だと、外積を返した上にzも同じ値になってしまう。外積を返すだけなら、z.getCrossed(v)｡
間に余計な処理を書いたせいで、無駄に時間を食ってしまった。

void ofApp::drawCylinder2Pt(ofVec3f p, ofVec3f q, float r)
{
                ofCylinderPrimitive cylinder;
		ofVec3f v = q - p;
		cylinder.setHeight(v.length());
		ofVec3f z(0.f, 1.f, 0.f);		// default cylinder direction
		ofQuaternion quat;
		quat.makeRotate(z, v);

		//float l = v.length();
		//l = (l == 0.f) ? FLT_EPSILON : l;
		//ofVec3f t = z.getCrossed(v);
		//float angle = 180.f / PI * acos(v.dot(z) / l);
		//quat.makeRotate(angle, t);

		cylinder.setRadius(r);
		cylinder.resetTransform();
		cylinder.rotate(quat);
		cylinder.setPosition((p + q)*0.5f);
		cylinder.draw();
}

今日の本文

明治時代に実在したイザベラバードという英国人女性が日本を旅行した旅行記をもとにした漫画。とても面白い。
原作の方も、バードおばさんが遠慮なく口が悪くて気分屋なのが面白い。

ふしぎの国のバード 1巻 (ビームコミックス)

ふしぎの国のバード 1巻 (ビームコミックス)

イザベラ・バードの日本紀行　合本版 (講談社学術文庫)

イザベラ・バードの日本紀行　合本版 (講談社学術文庫)

Open3DをIntel RealSense D435で動かすためにTestRealSense.cppをlibrealsense2に対応するよう改造する（その２）色付きPointCloudに変換

RealSense D435で取得したRGBColor画像とDepthをDepth解像度でlibrealsense側でalignして、Open3DのRGBD画像を作り、RGBD画像をさらにPointCloudに変換するサンプルです。
目視で確認したい場合は、DrawGeometriesのコメントアウトを外してください。ただし、１フレーム毎にループ処理が止まります。

// ----------------------------------------------------------------------------
// -                        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>
#include <Core/Geometry/LineSet.cpp>
#include <Core/Geometry/DownSample.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 fro 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 = -1;
	int stereo_module = -1;
	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

        int depth_width = 1280;	// depth_intr.width;
        int depth_height = 720; // depth_intr.height;
	int depth_fps = 30;	// 30 or 60 due to resolution
        int color_width = 1280;	// color_intr.width
        int color_height = 720; // color_intr.height
	int color_fps = 30;	// 30 or 60

	rs2::config cfg; // examples/measure/rs-measure.cpp
	cfg.enable_stream(RS2_STREAM_DEPTH, depth_width, depth_height, RS2_FORMAT_Z16, depth_fps);
	cfg.enable_stream(RS2_STREAM_COLOR, color_width, color_height, RS2_FORMAT_RGB8, color_fps);

	// 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_width, depth_height, 1, 2);
        auto color_image_ptr = std::make_shared<Image>();
        color_image_ptr->PrepareImage(color_width, color_height, 3, 1);
        FPSTimer timer("Realsense stream");

	Visualizer depth_vis, color_vis;
	if(depth_vis.CreateWindow("Depth", depth_width, depth_height, 15, 50) == false ||
		depth_vis.AddGeometry(depth_image_ptr) == false ||
	   color_vis.CreateWindow("Color", color_width, color_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

		// Retrieve Aligned Frame
		rs2::align align( rs2_stream::RS2_STREAM_COLOR );
		rs2::frameset aligned_frameset = align.process( data );
		if( !aligned_frameset.size() ){
        		return -1;
		}

		rs2::frame depth = aligned_frameset.get_depth_frame();	// Find the depth data
		rs2::frame color = aligned_frameset.get_color_frame();	// Find the color data

		memcpy(depth_image_ptr->data_.data(),
			depth.get_data(), depth_width * depth_height * 2);
		memcpy(color_image_ptr->data_.data(),
			color.get_data(), color_width * color_height * 3);

		depth_vis.UpdateGeometry();
		color_vis.UpdateGeometry();

                double depth_scale = 1000;
                double depth_trunc = 4.0;
                bool convert_rgb_to_intensity = false;
                auto rgbd = CreateRGBDImageFromColorAndDepth(*color_image_ptr, *depth_image_ptr,
				depth_scale, depth_trunc, convert_rgb_to_intensity);
		auto pcd = CreatePointCloudFromRGBDImage(*rgbd,
				PinholeCameraIntrinsic(color_intr.width, color_intr.height,
                        			color_intr.fx, color_intr.fy,
                        			color_intr.ppx, color_intr.ppy));
		
		// DrawGeometries({pcd}); // uncomment, if you want to check the result
		
		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;
	// pipe.stop();  // how can I get the pointer of the pipe?
	return EXIT_FAILURE;
}

f:id:cvl-robot:20180315233755p:plain
f:id:cvl-robot:20180315235337p:plain

updated 2018/03/19

		auto pcd = CreatePointCloudFromRGBDImage(*rgbd,
				PinholeCameraIntrinsic(color_intr.width, color_intr.height,
                        			color_intr.fx, color_intr.fy,
                        			color_intr.ppx, color_intr.ppy));

I'm sorry, I missed. not depth_intr.

f:id:cvl-robot:20180319153101p:plain

Open3Dに寄稿する方法とマナーを調べる

（編集中）
Open3Dのソースコードは、githubで管理されています。普段、仕事ではほとんど必要ないので、githubはgit cloneして便利に人のソースコードを使わせてもらうか、メモ代わりにGistにコードを張り付けるだけで、あまりちゃんと使ったことがありません。c++のTutorialをCotributeしたら？と声をかけてもらったので、ちゃんとその方法とマナーを調べたいと思います。

Open3DのContributeの仕方マニュアル

Contributing to Open3D — Open3D 0.1 dev documentation
まずはこれにざっと目を通します。

issues and pull request system

masterブランチは安定板のOpen3Dのためだけに使われていて、全てのソースコードの変更はissuesとpull requestの仕組みを通して管理されていて、次の4つの手順に寄るのだそうです。よく分かってないので、殆ど全訳になりますが読んでいきましょう。

1. Issueを開いて、リクエストやバグの報告をする。
Issues · IntelVCL/Open3D · GitHub
2. 新しいブランチやリポジトリのフォークを用意して、変更を加えて、pull requestを送信します。
Pull Requests · IntelVCL/Open3D · GitHub
3. Pull Requestでコードの変更が検査され、議論されます。変更は、Issueで提起され、議論を通して育てられます。
4. 管理者の一人がpull requestをmasterブランチにマージします。

pull requestってよく聴くけど使わないのでよく分かってない。あとで良く調べることにして、とりあえず続けましょう。
ざっくりわかることは、プロジェクトに参加したい人はまず1と2をやれば良いということですね。ソースコードの管理は、Adminsがちゃんと面倒みているので、いきなり何か変なことしてしまって壊したりする心配とかは不要で、レベルの低いソースコードを書いても、ちゃんとブラッシュアップしてもらえる仕組みがあって安心だということですね。

Maintain sanity of the project

buildを止めてしまうことが無いように、Windows, Mac, Linuxの3つの環境でテストしなさい、とあります。困ったな、Macの開発環境が無いぞ？

  • c++環境では、c++11を使うようにして、c++14, C++17はまだ一部のコンパイラで安定しないから使わないこと。
  • Pythonは2.7系と3.x系の両方で動作を確認すること

Coding style

コードの可読性を保つために、コーディングスタイルを保つことは重要な要素です。
1. コード自体がドキュメントと思え
2. 既存のコードとの一貫性を保て
3. 常識に従え

基本的には、ちょっとアレンジしたGoogle Coding Styleに従う。
Google C++ Style Guide

  • インデントにはtabを使う。空白4つ分。強制改行にはインデント2つ分。（一行80文字の制限)
  • ヘッダーガードには#pragma onceを使う
  • 全てのOpen3Dメンバーはthreeネームスペースの中に入れること。
  • 裸のポインタを使わずに、std::shared_ptrやstd::unique_ptrを使うこと。
  • c++11の機能を推奨

その他、現代的なC++コーディングスタイルは次の資料を参考にすること。
CppCoreGuidelines/CppCoreGuidelines.md at master · isocpp/CppCoreGuidelines · GitHub

で、分からないところの整理。

そんなに難しくないので、概ね分かったような気になっていますが、根本的にgitの使い方が分かりません。結局pull requestをするまでどう準備してどう操作すればいいものやら?また明日調べましょう。

How to use github

0. Sign up for GIthub and log-in.
1. Go to https://github.com/IntelVCL/Open3D, and push [fork] button.
2. git clone https://github.com/IntelVCL/Open3D
3. git config user.name "Your name"
4. git config user.email "your@email.com"
5. Edit your source code and put it on your cloned directory.
6. git add "your source code"
7. git commit
8. git push

Open3Dをライブラリとして使い、自分の独立したプロジェクトとしてプログラムを書きたいときのCMakeサンプル

CMakeの正しい書き方がわからない・・・
今、公式にmake installやfind cmakeを準備中だそうですので、急がないのであればそれを待つことをお勧めします。
とりあえず、Ubuntu環境でひとつのチュートリアルをビルドするためのCMakeファイルの例。わからないことだらけ。

  • どうすれば、Open３Dのルートを見つけられる？
  • libjsoncppの謎の挙動。nativeインストールしたせい？
  • libpng12じゃなくてlibpng16を自動で見つける方法
  • 様々な環境でのテストの方法
cmake_minimum_required(VERSION 3.0.0)

# Please set a path to Open3D root
set(Open3D_DIR "/home/cvl/workspace/Open3D")
set(Open3D_SOURCE_DIR "${Open3D_DIR}/src")
set(Open3D_LIBRARY_DIR "${Open3D_DIR}/build/lib/")

# Please set a project name
set(PROJECT_NAME "ColoredPointRegistration")
set(EXECUTABLE_OUTPUT_PATH "${Open3D_DIR}/build/bin/Tutorial")

project(${PROJECT_NAME})
add_executable(${PROJECT_NAME} "${PROJECT_NAME}.cpp")
target_compile_features(${PROJECT_NAME} PRIVATE cxx_range_for)
find_package(PkgConfig REQUIRED)

# set include directories
include_directories(${Open3D_SOURCE_DIR})
pkg_check_modules(EIGEN3 eigen3>=3.2.7)
include_directories(${EIGEN3_INCLUDE_DIRS})

# link dynamic libraries
pkg_check_modules(JSONCPP jsoncpp)
find_package(OpenGL)
find_package(GLEW REQUIRED)
find_package(glfw3)
include(FindJPEG)
target_link_libraries(${PROJECT_NAME} ${JSONCPP_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${OPENGL_gl_LIBRARY})
target_link_libraries(${PROJECT_NAME} ${JSONCPP_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${GLEW_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${GLFW3_LIBRARY})
target_link_libraries(${PROJECT_NAME} ${JPEG_LIBRARY})

# link static libraries
find_library(Open3D_CORE NAMES libCore.a PATHS ${Open3D_LIBRARY_DIR})
find_library(Open3D_IO NAMES libIO.a PATHS ${Open3D_LIBRARY_DIR})
find_library(Open3D_VISUALIZATION libVisualization.a PATHS ${Open3D_LIBRARY_DIR})
find_library(LIBPNG NAMES libpng.a PATHS ${Open3D_LIBRARY_DIR})
find_library(LIBZ NAMES libzlib.a PATHS ${Open3D_LIBRARY_DIR})
find_library(LIBTINYFILEDIALOGS NAMES libtinyfiledialogs.a PATHS ${Open3D_LIBRARY_DIR})
find_library(LIBJSONCPP NAMES libjsoncpp.a PATHS ${Open3D_LIBRARY_DIR})
target_link_libraries(${PROJECT_NAME} ${Open3D_CORE})
target_link_libraries(${PROJECT_NAME} ${Open3D_IO})
target_link_libraries(${PROJECT_NAME} ${Open3D_VISUALIZATION})
target_link_libraries(${PROJECT_NAME} "${Open3D_LIBRARY_DIR}/libpng.a") # Force libpng16
target_link_libraries(${PROJECT_NAME} ${LIBZ})
target_link_libraries(${PROJECT_NAME} ${LIBTINYFILEDIALOGS})
target_link_libraries(${PROJECT_NAME} ${LIBJSONCPP})

# OpenMP settings
find_package(OpenMP)
if (OPENMP_FOUND) 
    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
endif()

Intel RealSense D435のキャリブレーション

Intel RealSense D435のデプスキャリブレーションの方法を調べていきます。

資料

まず目を通すべき重要な資料はこれです。
https://www.intel.com/content/dam/support/us/en/documents/emerging-technologies/intel-realsense-technology/RealSense_D400_Dyn_Calib_Quick_Start.pdf
Intelの技術資料は、まとまっているようでまとまっていません。似たような文書が散乱していて欲しい情報を得るのに、一瞬戸惑ってしまいます。
もう少し細かい情報を調べたいときに見る資料はこちらです。
https://www.intel.com/content/dam/support/us/en/documents/emerging-technologies/intel-realsense-technology/RealSense_D400_Dyn_Calib_User_Guide.pdf

Dynamic Target Tool

Dynamic Calibratorでは、外部パラメータのみをキャリブレーションします。

  • RotationLeftRight, TranslationLeftRight
  • RotationLeftRGB, TranslationLeftRGB

キャリブレーションパターンを紙に印刷して板に張り付けて、キャリブレーションボードを作る従来のやり方もできますが、スマホを使ってやや現代風の方法をとることが出来ます。（この記事では、これを試してみたかっただけ、とも。通常は工場出荷時にキャリブレーションされていますので、外部パラメータのキャリブレーションを試す必要はありません。）
まず、iPhoneやAndroidのアプリとして配布されているIntel RealSense Dynamic Target Toolを、スマホにダウンロードしてインストールしてください。デバイスモデルが正しく認識されれば、適切に調整されたキャリブパターンがスマホ画面に表示されます。
f:id:cvl-robot:20180314150744p:plain
https://itunes.apple.com/jp/app/dynamic-target-tool/id1291448596?mt=8
Dynamic Target Tool - Google Play の Android アプリ

f:id:cvl-robot:20180314150528p:plainf:id:cvl-robot:20180314150534p:plain
Dynamic Target Toolのキャリブレーションパターン

DynamicCalibrator

キャリブレーションツールを起動します。ターゲット有と無の２つのキャリブレーション方法を選べるようです。

> Intel.RealSense.DynamicCalbrator

ターゲット有の場合

  • Use Targetにチェックを入れて、Start Calibrationを押します。

RealSenseは三脚等に固定し、カメラの前1mぐらいの空間を物理的に空けます。

  • rectification phase

少し準備の間があって画面中央に青枠が表示されます。カメラからおよそ70㎝の距離(50-100cmの間)で、なおかつ、青枠の中に映る位置にスマホを持っていきます。上手くいくと、青枠が外れて透明になりますので、全部の青枠が無くなるように青枠全体をなめるようにスマホを動かしていきます。全部、透明になると次のフェーズに勝手に移行します。

  • Scale calibration phase

画面中に認識されたパターンの枠が表示されますので、奥行50-100cm、画面の右から左まで、万遍なくマーカー枠が記録されるように動かしていきます。必要なデータがそろうと、勝手に終了します。

ターゲット無の場合

画面の指示に従って、センサーを適当に動かすと終了します。

キャリブレーション結果の確認

専用ツールを起動して目視でチェックします。項目が多くてよく分かりません。平面が平らに計測できているかどうかチェックします。

>rs-depth-quality

キャリブレーションデータの取得

> Intel.Realsense.CustomRW -r calib.xml

で、センサーの中に書かれたキャリブデータをファイルに落とします。
うちの子だと、カメラパラメータは以下の通りでした。RGBカメラのキャリブレーションをしなければいけませんね。

> Intel.Realsense.CustomRW -r calib.xml
CustomRW for Intel RealSense D400, Version: 2.5.2.0

Calibration parameters from the device:
resolutionLeftRight: 1280 800

FocalLengthLeft: 640.671387 640.275391
PrincipalPointLeft: 636.945984 387.554199
DistortionLeft: -0.058403 0.066275 0.000052 -0.000805 -0.021272

FocalLengthRight: 641.304993 640.990051
PrincipalPointRight: 637.570923 388.892883
DistortionRight: -0.058802 0.067591 0.000368 -0.000935 -0.021977

RotationLeftRight: 0.999986 -0.003006 0.004452
0.002983 0.999983 0.005066
-0.004467 -0.005052 0.999977
TranslationLeftRight: -49.963303 -0.013997 0.335144

HasRGB: 1

resolutionRGB: 1920 1080

FocalLengthColor: 1384.542603 1385.032715
PrincipalPointColor: 961.749573 533.023682
DistortionColor: 0.000000 0.000000 0.000000 0.000000 0.000000
RotationLeftColor: 0.999967 -0.006930 0.004343
0.006935 0.999975 -0.001158
-0.004335 0.001188 0.999990
TranslationLeftColor: 14.763059 0.139163 0.053030

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をもっと勉強しないと、駄目ですね。

realsense2のc++コードの参考はここが良い

unanancyowen.com

今日の本

東ロボの失敗？から副次的に洗い出された日本の教育の大問題の提起。
英語教育の前に日本語教育をちゃんとしないとだめですね！

AI vs. 教科書が読めない子どもたち

AI vs. 教科書が読めない子どもたち

blog.tinect.jp

Open3DのPythonチュートリアルをC++に移植(その6) intaractive_visualization.py

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

interactive_visualizationは、マウス操作によるクロップ（領域きりだし）と、ポイント選択による初期位置指定をつかった位置合わせを提供します。これも、c++のTestコードが用意されていますので、ここのソースは見る必要がありません。

python
Open3D/interactive_visualization.py at master · IntelVCL/Open3D · GitHub

c++
Open3D/ManuallyCropPointCloud.cpp at master · IntelVCL/Open3D · GitHub
Open3D/src/Tools/ManuallyAlignPointCloud at master · IntelVCL/Open3D · GitHub

// ----------------------------------------------------------------------------
// -                        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 <memory>
#include <thread>

#include <Core/Core.h>
#include <IO/IO.h>
#include <Visualization/Visualization.h>

#include <assert.h>

using namespace three;

void VisualizeRegistration(const three::PointCloud &source,
		const three::PointCloud &target, const Eigen::Matrix4d &Transformation)
{
	std::shared_ptr<PointCloud> source_transformed_ptr(new PointCloud);
	std::shared_ptr<PointCloud> target_ptr(new PointCloud);
	*source_transformed_ptr = source;
	*target_ptr = target;
	source_transformed_ptr->Transform(Transformation);
	DrawGeometries({ source_transformed_ptr, target_ptr }, "Registration result");
}

void DemoCropGeometry()
{
	std::cout << "Demo for manual geometry cropping" << std::endl;
	std::cout << "1) Press 'Y' twice to align geometry with negative direction of y-axis" << std::endl;
	std::cout << "2) Press 'K' to lock screen and to switch to selection mode" << std::endl;
	std::cout << "3) Drag for rectangle selection," << std::endl;
	std::cout << "   or use ctrl + left click for polygon selection" << std::endl;
	std::cout << "4) Press 'C' to get a selected geometry and to save it" << std::endl;
	std::cout << "5) Press 'F' to switch to freeview mode" << std::endl;
        // std::shared_ptr<PointCloud>
	auto pcd = CreatePointCloudFromFile("../../lib/TestData/ICP/cloud_bin_0.pcd");
	DrawGeometriesWithEditing({pcd});
}

std::vector<size_t>& PickPoints(std::shared_ptr<const Geometry> geom_ptr)
{
	std::cout << std::endl;
	std::cout << "1) Please pick at least three correspondences using [shift + left click]" << std::endl;
	std::cout << "   Press [shift + right click] to undo point picking" << std::endl;
	std::cout << "2) After picking points, press q for close the window" << std::endl;
	VisualizerWithEditing vis;
	vis.CreateWindow();
	vis.AddGeometry(geom_ptr);
	vis.Run(); // user picks points
	vis.DestroyWindow();
	std::cout << std::endl;
	return vis.GetPickedPoints();
}

void DemoManualRegistration()
{
	std::cout << "Demo for manual ICP" << std::endl;
        // std::shared_ptr<PointCloud>
	auto source = CreatePointCloudFromFile("../../lib/TestData/ICP/cloud_bin_0.pcd");
        // std::shared_ptr<PointCloud>
	auto target = CreatePointCloudFromFile("../../lib/TestData/ICP/cloud_bin_2.pcd");
	std::cout << "Visualization of two point clouds before manual alignment" << std::endl;
	//DrawRegistrationResult(source, target, Eigen::Matrix4d::Identity());
	VisualizeRegistration(*source, *target, Eigen::Matrix4d::Identity());

	// pick points from two point clouds and builds correspondences
        // std::vector<size_t>&
	auto picked_id_source = PickPoints(source);
        // std::vector<size_t>&
	auto picked_id_target = PickPoints(target);
	assert((picked_id_source.size() >=3) && (picked_id_target.size() >= 3));
	assert(picked_id_source.size() == picked_id_target.size());
	CorrespondenceSet corr;
	for(size_t i = 0; i < picked_id_source.size(); i++){
		corr.push_back(Eigen::Vector2i(picked_id_source[i], picked_id_target[i]));
	}

	// estimate rough transformation using correspondences
	std::cout << "Compute a rough transformation using the correspondences given by user" << std::endl;
	TransformationEstimationPointToPoint p2p;
        // Eigen::Matrix4d
	auto trans_init = p2p.ComputeTransformation(*source, *target, corr);

	// point-to-point ICP for refinement
	std::cout << "Perform point-to-point ICP refinement" << std::endl;
	double threshold = 0.03; // 3cm distance threshold
        // RegistrationResult
	auto reg_p2p = RegistrationICP(*source, *target, threshold, trans_init,
				TransformationEstimationPointToPoint());

	VisualizeRegistration(*source, *target, reg_p2p.transformation_);
	std::cout << std::endl;
}

int main(int argc, char *argv[])
{
	SetVerbosityLevel(VerbosityLevel::VerboseAlways);

	DemoCropGeometry();
	DemoManualRegistration();

	return 1;
}

f:id:cvl-robot:20180306143728p:plainf:id:cvl-robot:20180306143743p:plain
Crop
f:id:cvl-robot:20180306143834p:plainf:id:cvl-robot:20180306143841p:plain
Selection
f:id:cvl-robot:20180306143848p:plain
f:id:cvl-robot:20180306143951p:plainf:id:cvl-robot:20180306144002p:plain
Manual Registration
f:id:cvl-robot:20180306144127p:plainf:id:cvl-robot:20180306144131p:plain
Manual Registration-Missed
f:id:cvl-robot:20180306144135p:plain
f:id:cvl-robot:20180306144224p:plainf:id:cvl-robot:20180306144228p:plain
Manual Registration-Succes?
f:id:cvl-robot:20180306144231p:plain

なかなかうまく合いません。Point-to-Point ICPが厳しいのだとは思いますが、ColoredICPかFPFHに任せてしまった方がよさそうですね。

このチュートリアル和訳が良い

http://lang.sist.chukyo-u.ac.jp/classes/Open3D/