Open3DのReconstruction systemをC++に移植（その1-1 make_fragment)

Open3DのReconstruction Systetmは，距離画像とカラー画像の動画シーケンスから立体モデルを復元するためのPythonチュートリアルです。
詳細はこちらにあります。
Reconstruction system — Open3D 0.1 dev documentation
テストデータは、上記で紹介されているこれを使います。（約500MB）
016.zip - Google ドライブ

これをc++(openFrameworks)に移植していきます。開発環境はWindows10とopenFrameworks0.10.0です。
あらかじめ断っておくと、Open3Dはもともとc++で実装されていてpythonはインターフェースしているだけなので、Reconstruction systemをc++で書いたところでたいして速くなりません。
また、まだ全部のコンポーネントの移植が済んでいないのでデバッグが完ぺきではありません。
Reconstruction systemは、Make fragments、Register fragments、Integrate sceneの3つのコンポーネントにより構成されています。

make_fragments

make_framgentsは，ひと続きの動画シーケンスから時間的に連続するフレームとループクロージャーの検出をしてPoseGraphを作って、そこから小さめのメッシュの復元をします。
詳細はこちら。
Make fragments — Open3D 0.1 dev documentation
pythonソースは、次の５個から構成されています。
make_fragments.py
common.py
opencv.py
opencv_pose_estimation.py
optimize_posegraph.py

make_fragments.h

#pragma once

// Open3D: www.open3d.org
// The MIT License (MIT)
// See license file or visit www.open3d.org for details

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


std::tuple<bool, Eigen::Matrix4d, Eigen::Matrix6d> register_one_rgbd_pair(int s, int t, std::vector<std::string>& color_files, std::vector<std::string>& depth_files,
	three::PinholeCameraIntrinsic intrinsic, bool with_opencv);

void make_posegraph_for_fragment(std::string path_dataset, int sid, int eid, std::vector<std::string> color_files, std::vector<std::string> depth_files,
	int fragment_id, int n_fragments, three::PinholeCameraIntrinsic intrinsic, bool with_opencv);

std::shared_ptr<three::TriangleMesh> integrate_rgb_frames_for_fragment(std::vector<std::string>& color_files, std::vector<std::string>& depth_files,
	int fragment_id, int n_fragments, std::string pose_graph_name, three::PinholeCameraIntrinsic intrinsic);

void make_mesh_for_fragment(std::string path_dataset, std::vector<std::string>& color_files, std::vector<std::string>& depth_files,
	int fragment_id, int n_fragments, three::PinholeCameraIntrinsic intrinsic);

void process_fragments(std::string path_dataset, std::string path_intrinsic, bool with_opencv);

int make_fragments_main(std::string path_dataset_, std::string path_intrisic_);

make_fragments.cpp

#include "make_fragments.h"

#include "common_py.h"
#include "opencv_py.h"
#include "opencv_pose_estimation.h"
#include "optimize_posegraph.h"

#include <Core/Registration/PoseGraph.h>

#include <Eigen/Dense>

#include <iostream>
#include <thread>

/////////////////////////
// some global parameters for the global registration
/////////////////////////

extern int n_frames_per_fragment;
extern int n_keyframes_per_n_frame;


//////////////////////////
// file related
//////////////////////////

extern std::string folder_fragment;
extern std::string template_fragment_posegraph;
extern std::string template_fragment_posegraph_optimized;
extern std::string template_fragment_mesh;
extern std::string folder_scene;
extern std::string template_global_posegraph;
extern std::string template_global_posegraph_optimized;
extern std::string template_global_mesh;


std::tuple<bool, Eigen::Matrix4d, Eigen::Matrix6d> register_one_rgbd_pair(int s, int t, std::vector<std::string>& color_files, std::vector<std::string>& depth_files,
	three::PinholeCameraIntrinsic intrinsic, bool with_opencv)
{
	// read_images
	auto color_s = std::make_shared<three::Image>();
	auto depth_s = std::make_shared<three::Image>();
	auto color_t = std::make_shared<three::Image>();
	auto depth_t = std::make_shared<three::Image>();
	three::ReadImage(color_files[s], *color_s);
	three::ReadImage(depth_files[s], *depth_s);
	three::ReadImage(color_files[t], *color_t);
	three::ReadImage(depth_files[t], *depth_t);
	auto source_rgbd_image = std::make_shared<three::RGBDImage>();
	auto target_rgbd_image = std::make_shared<three::RGBDImage>();
	source_rgbd_image = three::CreateRGBDImageFromColorAndDepth(*color_s, *depth_s,
		1000.0, 3.0, true);
	target_rgbd_image = three::CreateRGBDImageFromColorAndDepth(*color_t, *depth_t,
		1000.0, 3.0, true);

	if (abs(s - t) != 1) {
		if (with_opencv) {
			auto ret = pose_estimation(
				*source_rgbd_image, *target_rgbd_image, intrinsic, true/*false*/);
			bool success_5pt = std::get<0>(ret);
			Eigen::Matrix4d odo_init = std::get<1>(ret);
			if (success_5pt) {
				auto ret = three::ComputeRGBDOdometry(
					*source_rgbd_image, *target_rgbd_image, intrinsic,
					odo_init, three::RGBDOdometryJacobianFromHybridTerm(),
					three::OdometryOption());
				return ret;
			}
		}
		return std::tuple<bool, Eigen::Matrix4d, Eigen::Matrix6d>(false, Eigen::Matrix4d::Identity(), Eigen::Matrix6d::Identity());
	}
	else {
		Eigen::Matrix4d odo_init = Eigen::Matrix4d::Identity();
		return three::ComputeRGBDOdometry(
			*source_rgbd_image, *target_rgbd_image, intrinsic, odo_init,
			three::RGBDOdometryJacobianFromHybridTerm(), three::OdometryOption());
	}

	return std::tuple<bool, Eigen::Matrix4d, Eigen::Matrix6d>(false, Eigen::Matrix4d::Identity(), Eigen::Matrix6d::Identity()); // dummy
}

void make_posegraph_for_fragment(std::string path_dataset, int sid, int eid, std::vector<std::string> color_files, std::vector<std::string> depth_files,
	int fragment_id, int n_fragments, three::PinholeCameraIntrinsic intrinsic, bool with_opencv)
{
	three::SetVerbosityLevel(three::VerbosityLevel::VerboseError);
	three::PoseGraph pose_graph;
	Eigen::Matrix4d trans_odometry = Eigen::Matrix4d::Identity();
	pose_graph.nodes_.push_back(three::PoseGraphNode(trans_odometry));
	for (int s = sid; s < eid; s++) {
		for (int t = s + 1; t < eid; t++) {
			// odometry
			if (t == s + 1) {
				std::cout << "Fragment " << fragment_id << " / " << n_fragments - 1 << " :: RGBD matching between frame : " << s << " and " << t << std::endl;
				auto ret = register_one_rgbd_pair(
					s, t, color_files, depth_files, intrinsic, with_opencv);
				bool& success = std::get<0>(ret);
				Eigen::Matrix4d& trans = std::get<1>(ret);
				Eigen::Matrix6d& info = std::get<2>(ret);
				trans_odometry = trans * trans_odometry;
				Eigen::Matrix4d trans_odometry_inv = trans_odometry.inverse();
				pose_graph.nodes_.push_back(three::PoseGraphNode(trans_odometry_inv));
				pose_graph.edges_.push_back(
					three::PoseGraphEdge(s - sid, t - sid, trans, info, false));
			}

			// keyframe loop closure
			if ((s % n_keyframes_per_n_frame == 0) && (t % n_keyframes_per_n_frame == 0)) {
				std::cout << "Fragment " << fragment_id << " / " << n_fragments - 1 << " :: RGBD matching between frame : " << s << " and " << t << std::endl;
				auto ret = register_one_rgbd_pair(
					s, t, color_files, depth_files, intrinsic, with_opencv);
				bool success = std::get<0>(ret);
				Eigen::Matrix4d trans = std::get<1>(ret);
				Eigen::Matrix6d info = std::get<2>(ret);
				if (success) {
					pose_graph.edges_.push_back(
						three::PoseGraphEdge(s - sid, t - sid, trans, info, true));
				}
			}
		}
	}
	std::string filename(path_dataset + template_fragment_posegraph);
	sprintf((char*)filename.data(), (path_dataset + template_fragment_posegraph).c_str(), fragment_id);
	WriteIJsonConvertibleToJSON(filename, pose_graph);
	//int ret = three::WritePoseGraph(filename, pose_graph);
}

std::shared_ptr<three::TriangleMesh> integrate_rgb_frames_for_fragment(std::vector<std::string>& color_files, std::vector<std::string>& depth_files,
	int fragment_id, int n_fragments, std::string pose_graph_name, three::PinholeCameraIntrinsic intrinsic)
{
	three::PoseGraph pose_graph;
	three::ReadIJsonConvertibleFromJSON(pose_graph_name, pose_graph);
	three::ScalableTSDFVolume volume = three::ScalableTSDFVolume(3.0 / 512.0,
		0.04, true);

	for (int i = 0; i < pose_graph.nodes_.size(); i++) {
		int i_abs = fragment_id * n_frames_per_fragment + i;
		std::cout << "Fragment " << fragment_id << " / " << n_fragments - 1 << " : integrate rgbd frame "
			<< i_abs << " (" << i + 1 << " of " << pose_graph.nodes_.size() << ")." << std::endl;
		std::shared_ptr<three::Image> color = std::make_shared<three::Image>();
		std::shared_ptr<three::Image> depth = std::make_shared<three::Image>();
		three::ReadImage(color_files[i_abs], *color);
		three::ReadImage(depth_files[i_abs], *depth);
		std::shared_ptr<three::RGBDImage> rgbd = three::CreateRGBDImageFromColorAndDepth(*color, *depth,
			1000.0, 3.0, false);
		Eigen::Matrix4d pose = pose_graph.nodes_[i].pose_;
		volume.Integrate(*rgbd, intrinsic, pose.inverse());
	}

	std::shared_ptr<three::TriangleMesh> mesh = volume.ExtractTriangleMesh();
	mesh->ComputeVertexNormals();
	return mesh;
}

void make_mesh_for_fragment(std::string path_dataset, std::vector<std::string>& color_files, std::vector<std::string>& depth_files,
	int fragment_id, int n_fragments, three::PinholeCameraIntrinsic intrinsic)
{
	std::string filename(path_dataset + template_fragment_posegraph_optimized);
	sprintf((char*)filename.data(), (path_dataset + template_fragment_posegraph_optimized).c_str(), fragment_id);
	std::shared_ptr<three::TriangleMesh> mesh = integrate_rgb_frames_for_fragment(
		color_files, depth_files, fragment_id, n_fragments,
		filename,
		intrinsic
	);
	std::string mesh_name(path_dataset + template_fragment_mesh);
	sprintf((char*)mesh_name.data(), (path_dataset + template_fragment_mesh).c_str(), fragment_id);
	//three::WriteTriangleMesh(mesh_name, *mesh, false, true);
	three::WriteTriangleMeshToPLY(mesh_name, *mesh, false, true);
}

three::PinholeCameraIntrinsic read_pinhole_camera_intrinsic(const std::string &filename)
{
	three::PinholeCameraIntrinsic intrinsic;

	ReadIJsonConvertible(filename, intrinsic);

	return intrinsic;
}

void process_fragments(std::string path_dataset, std::string path_intrinsic, bool with_opencv)
{
	three::PinholeCameraIntrinsic intrinsic;
	if (!path_intrinsic.empty()) {
		intrinsic = read_pinhole_camera_intrinsic(path_intrinsic);
	}
	else {
		intrinsic = three::PinholeCameraIntrinsic(
			three::PinholeCameraIntrinsicParameters::PrimeSenseDefault);
	}

	make_folder(path_dataset + folder_fragment);
	std::shared_ptr<std::vector<std::string>> color_files;
	std::shared_ptr<std::vector<std::string>> depth_files;
	get_rgbd_file_lists(color_files, depth_files, path_dataset);
	int n_files = static_cast<int>(color_files->size());
	int n_fragments = static_cast<int>(ceil(float(n_files) / n_frames_per_fragment));

	for (int fragment_id = 0; fragment_id < n_fragments; fragment_id++) {
		int sid = fragment_id * n_frames_per_fragment;
		int eid = std::min(sid + n_frames_per_fragment, n_files);
		make_posegraph_for_fragment(path_dataset, sid, eid, *color_files, *depth_files,
			fragment_id, n_fragments, intrinsic, with_opencv);
		optimize_posegraph_for_fragment(path_dataset, fragment_id);
		make_mesh_for_fragment(path_dataset, *color_files, *depth_files,
			fragment_id, n_fragments, intrinsic);
	}
}

int make_fragments_main(std::string path_dataset_, std::string path_intrinsic_)
{
	std::cout << "making fragments from RGBD sequence." << std::endl;
	std::string path_dataset = path_dataset_;
	std::cout << "path to the dataset " << path_dataset << std::endl;
	std::string path_intrinsic = path_intrinsic_;
	std::cout << "path to the RGBD camera intrinsic " << path_intrinsic << std::endl;

	// check opencv package
	bool with_opencv = initialize_opencv(true);
	
	process_fragments(path_dataset, path_intrinsic, with_opencv);
	return 0;
}

common_py.h

#pragma once

// Open3D: www.open3d.org
// The MIT License (MIT)
// See license file or visit www.open3d.org for details

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


std::shared_ptr<std::vector<std::string>> get_file_list(std::string path, std::string extension);

void get_rgbd_file_lists(std::shared_ptr<std::vector<std::string>>& color_files,
	std::shared_ptr<std::vector<std::string>>& depth_files,
	std::string path_dataset);

void make_folder(std::string path_folder);


////////////////////////
// visualization related
////////////////////////

void draw_pcd(std::shared_ptr<three::PointCloud>& pcd);

void draw_registration(three::PointCloud& source,
		three::PointCloud& target, Eigen::Matrix4d& transformation);

void draw_registration_result_original_color(three::PointCloud& source,
		three::PointCloud& target, Eigen::Matrix4d transformation);

common_py.cpp

#include "common_py.h"

#include <ofFileUtils.h>

using namespace three;

/////////////////////////
// some global parameters for the global registration
/////////////////////////

int n_frames_per_fragment = 100;
int n_keyframes_per_n_frame = 5;

//////////////////////////
// file related
//////////////////////////

std::string folder_fragment = "/fragments/";
std::string template_fragment_posegraph = folder_fragment + "fragment_%03d.json";
std::string template_fragment_posegraph_optimized = folder_fragment +
"fragment_optimized_%03d.json";
std::string template_fragment_mesh = folder_fragment + "fragment_%03d.ply";
std::string folder_scene = "/scene/";
std::string template_global_posegraph = folder_scene + "global_registration.json";
std::string template_global_posegraph_optimized = folder_scene +
"global_registration_optimized.json";
std::string template_global_mesh = folder_scene + "integrated.ply";


std::shared_ptr<std::vector<std::string>> get_file_list(std::string path, std::string extension)
{
	auto file_list = std::make_shared<std::vector<std::string>>();

	//std::cout << path << " " << extension << std::endl;
	if (extension == "None") {
		ofDirectory dir(path);
		for (int i = 0; i < dir.getFiles().size(); i++) {
			std::string f = dir.getFile(i).getFileName();
			ofFile path_f;
			path_f.open(path + f, ofFile::Mode::ReadOnly);
			if (path_f.exists()) {
				if (path_f.isFile()) {
					file_list->push_back(path_f.getAbsolutePath());
				}
			}
		}
	}
	else {
		ofDirectory dir(path);
		for (int i = 0; i < dir.getFiles().size(); i++) {
			std::string f = dir.getFile(i).getFileName();
			ofFile path_f;
			path_f.open(path + f, ofFile::Mode::ReadOnly);
			if (path_f.exists()) {
				if (path_f.isFile()) {
					if ('.' + path_f.getExtension() == extension) {
						file_list->push_back(path_f.getAbsolutePath());
					}
				}
			}
		}
	}
	std::sort(file_list->begin(), file_list->end());

	return file_list;
}

void get_rgbd_file_lists(std::shared_ptr<std::vector<std::string>>& color_files,
	std::shared_ptr<std::vector<std::string>>& depth_files,
	std::string path_dataset)
{
	std::string path_color = path_dataset + "/image/";
	std::string path_depth = path_dataset + "/depth/";
	color_files = get_file_list(path_color, ".jpg");
	auto color_files_png = get_file_list(path_color, ".png");
	std::copy(color_files_png->begin(), color_files_png->end(), std::back_inserter(*color_files));
	depth_files = get_file_list(path_depth, ".png");
}

void make_folder(std::string path_folder)
{
	ofDirectory dir(path_folder);
	if (!dir.exists()) {
		dir.create();
	}
}


////////////////////////
// visualization related
////////////////////////

void draw_pcd(std::shared_ptr<three::PointCloud>& pcd)
{
	Eigen::Matrix4d flip_transform;
	flip_transform << 1., 0., 0., 0.,
		0., -1., 0., 0.,
		0., 0., -1., 0.,
		0., 0., 0., 1.;

	auto pcd_temp = std::make_shared<three::PointCloud>(*pcd);
	pcd_temp->Transform(flip_transform);
	three::DrawGeometries({ pcd_temp });
}

void draw_registration(three::PointCloud& source, three::PointCloud& target, Eigen::Matrix4d& transformation)
{
	Eigen::Matrix4d flip_transform;
	flip_transform << 1., 0., 0., 0.,
		0., -1., 0., 0.,
		0., 0., -1., 0.,
		0., 0., 0., 1.;

	auto source_temp = std::make_shared<three::PointCloud>(source);
	auto target_temp = std::make_shared<three::PointCloud>(target);
	source_temp->PaintUniformColor(Eigen::Vector3d(1.f, 0.706f, 0.f));
	target_temp->PaintUniformColor(Eigen::Vector3d(0.f, 0.651f, 0.929f));
	source_temp->Transform(transformation);
	source_temp->Transform(flip_transform);
	target_temp->Transform(flip_transform);
	three::DrawGeometries({ source_temp, target_temp });
}

void draw_registration_result_original_color(three::PointCloud& source, three::PointCloud& target, Eigen::Matrix4d transformation)
{
	Eigen::Matrix4d flip_transform;
	flip_transform << 1., 0., 0., 0.,
		0., -1., 0., 0.,
		0., 0., -1., 0.,
		0., 0., 0., 1.;

	auto source_temp = std::make_shared<three::PointCloud>(source);
	auto target_temp = std::make_shared<three::PointCloud>(target);
	source_temp->Transform(transformation);
	source_temp->Transform(flip_transform);
	target_temp->Transform(flip_transform);
	three::DrawGeometries({ source_temp, target_temp });
}

opencv_py.h

#pragma once

// Open3D: www.open3d.org
// The MIT License (MIT)
// See license file or visit www.open3d.org for details

bool initialize_opencv(bool use);

opencv_py.cpp

#include "opencv_py.h"

#include <iostream>

// opencv
bool initialize_opencv(bool use = true)
{
	bool opencv_installed = use;
	if (!opencv_installed) {
		std::cout << "OpenCV is not detected. Using Identity as an initial" << std::endl;
	}
	else {
		std::cout << "OpenCV is detected. using ORB + 5pt algorithm" << std::endl;
	}
	return opencv_installed;
}

長くなったので続きは次のページ。

今日のそうめん

この業務用そうめんが安くてとても美味しい。
store.shopping.yahoo.co.jp