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

register_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>

void register_fragments_main(std::string path_dataset_);

register_fragments.cpp

#include "register_fragments.h"
#include "common_py.h"
#include "optimize_posegraph.h"

#include <Core/Registration/PoseGraph.h>
#include <Core/Registration/FastGlobalRegistration.h> 
#include <Core/Registration/ColoredICP.h> 

#include <Eigen/Dense>

/////////////////////////
// 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<std::shared_ptr<three::PointCloud>, std::shared_ptr<three::Feature>> preprocess_point_cloud(three::PointCloud& pcd)
{
	std::shared_ptr<three::PointCloud> pcd_down = std::make_shared<three::PointCloud>();
	pcd_down = three::VoxelDownSample(pcd, 0.05);
	double radius = 0.1;
	int max_nn = 30;
	three::EstimateNormals(*pcd_down,
		three::KDTreeSearchParamHybrid(radius, max_nn));
	radius = 0.25;
	max_nn = 100;
	std::shared_ptr<three::Feature> pcd_fpfh = std::make_shared<three::Feature>();
	pcd_fpfh = three::ComputeFPFHFeature(*pcd_down,
		three::KDTreeSearchParamHybrid(radius, max_nn));
	return std::tuple<std::shared_ptr<three::PointCloud>, std::shared_ptr<three::Feature>>(pcd_down, pcd_fpfh);
}

std::tuple<bool, three::RegistrationResult> register_point_cloud_fpfh(three::PointCloud& source, three::PointCloud& target,
	three::Feature& source_fpfh, three::Feature& target_fpfh)
{
	double maximum_correspondence_distance = 0.07;
	three::RegistrationResult result = three::FastGlobalRegistration(
		source, target, source_fpfh, target_fpfh,
		three::FastGlobalRegistrationOption(1.399999999999911, false, true, maximum_correspondence_distance));
	if (result.transformation_.trace() == 4.0) {
		return std::tuple<bool, three::RegistrationResult>(false, Eigen::Matrix4d::Identity());
	}
	return std::tuple<bool, three::RegistrationResult>(true, result);
}

void draw_registration_result(three::PointCloud& source, three::PointCloud& target, Eigen::Matrix4d transformation)
{
	std::shared_ptr<three::PointCloud> source_temp = std::make_shared<three::PointCloud>(source);
	std::shared_ptr<three::PointCloud> target_temp = std::make_shared<three::PointCloud>(target);
	source_temp->PaintUniformColor(Eigen::Vector3d(1., 0.706, 0.));
	target_temp->PaintUniformColor(Eigen::Vector3d(0., 0.651, 0.929));
	source_temp->Transform(transformation);
	three::DrawGeometries({ source_temp, target_temp });
}

std::tuple<bool, Eigen::Matrix4d> compute_initial_registartion(int s, int t, three::PointCloud& source_down, three::PointCloud& target_down,
	three::Feature& source_fpfh, three::Feature& target_fpfh, std::string path_dataset, bool draw_result = false)
{
	Eigen::Matrix4d transformation;
	if (t == s + 1) { // odomety case
		std::cout << "Using RGBD odometry" << std::endl;
		std::string filename(path_dataset + template_fragment_posegraph_optimized);
		sprintf((char*)filename.data(), (path_dataset + template_fragment_posegraph_optimized).c_str(), s);
		three::PoseGraph pose_graph_frag;
		three::ReadIJsonConvertibleFromJSON(filename, pose_graph_frag);
		int n_nodes = static_cast<int>(pose_graph_frag.nodes_.size());
		transformation = pose_graph_frag.nodes_[n_nodes - 1].pose_.inverse();
		std::cout << pose_graph_frag.nodes_[0].pose_ << std::endl;
		std::cout << transformation << std::endl;
	}
	else { // loop closure case
		std::cout << "register_point_cloud_fpfh" << std::endl;
		auto ret = register_point_cloud_fpfh(
			source_down, target_down,
			source_fpfh, target_fpfh);
		bool success_ransac = std::get<0>(ret);
		three::RegistrationResult result_ransac = std::get<1>(ret);
		if (!success_ransac) {
			std::cout << "No reasonable solution. Skip this pair" << std::endl;
			return std::tuple<bool, Eigen::Matrix4d>(false, Eigen::Matrix4d::Identity());
		}
		else {
			transformation = result_ransac.transformation_;
		}
		std::cout << transformation << std::endl;
	}

	if (draw_result) {
		draw_registration_result(source_down, target_down,
			transformation);
	}
	return std::tuple<bool, Eigen::Matrix4d>(true, transformation);
}

// colored pointcloud registration
// This is implementation of following paper
// J. Park, Q.-Y. Zhou, V. Koltun,
// Colored Point Cloud Registration Revisited, ICCV 2017
std::tuple<Eigen::Matrix4d, Eigen::Matrix6d> register_colored_point_cloud_icp(three::PointCloud& source, three::PointCloud& target,
	Eigen::Matrix4d init_transformation = Eigen::Matrix4d::Identity(),
	std::vector<double> voxel_radius = std::vector<double>({ 0.05, 0.025, 0.0125 }), std::vector<int> max_iter = std::vector<int>({ 50, 30, 14 }),
	bool draw_result = false)
{
	Eigen::Matrix4d current_transformation = init_transformation;
	three::RegistrationResult result_icp;

	for (int scale = 0; scale < max_iter.size(); scale++) { // multi-scale approach
		int iter = max_iter[scale];
		double radius = voxel_radius[scale];
		std::cout << "radius " << radius << std::endl;
		std::shared_ptr<three::PointCloud> source_down = three::VoxelDownSample(source, radius);
		std::shared_ptr<three::PointCloud> target_down = three::VoxelDownSample(target, radius);
		double radius_n = radius * 2;
		int max_nn = 30;
		three::EstimateNormals(*source_down, three::KDTreeSearchParamHybrid(radius_n, max_nn));
		if (0) { // print(np.assarray(source_down.normals))
			for (int i = 0; i < source_down->normals_.size(); i++) {
				std::cout << source_down->normals_[i] << " ";
			}
			std::cout << std::endl;
		}
		three::EstimateNormals(*target_down, three::KDTreeSearchParamHybrid(radius_n, max_nn));
		double relative_fitness = 1e-6;
		double relative_rmse = 1e-6;
		int max_iteration = iter;
		result_icp = three::RegistrationColoredICP(*source_down, *target_down,
			radius, current_transformation,
			three::ICPConvergenceCriteria(relative_fitness, relative_rmse, max_iteration));
		current_transformation = result_icp.transformation_;
	}
	Eigen::Matrix6d information_matrix = three::GetInformationMatrixFromPointClouds(
		source, target, 0.07, result_icp.transformation_);
	if (draw_result) {
		draw_registration_result_original_color(source, target,
			result_icp.transformation_);
	}
	return std::tuple<Eigen::Matrix4d, Eigen::Matrix6d>(result_icp.transformation_, information_matrix);
}

std::tuple<bool, Eigen::Matrix4d, Eigen::Matrix6d> local_refinement(int s, int t, three::PointCloud& source, three::PointCloud& target,
	Eigen::Matrix4d transformation_init, bool draw_result = false)
{
	Eigen::Matrix4d transformation;
	Eigen::Matrix6d information;
	if (t == s + 1) { // odometry case
		std::cout << "register_point_cloud_icp" << std::endl;
		std::vector<double> voxel_radius({ 0.0125 });
		std::vector<int> max_iter({ 30 });
		auto ret = register_colored_point_cloud_icp(
			source, target, transformation_init,
			voxel_radius, max_iter);
		transformation = std::get<0>(ret);
		information = std::get<1>(ret);
	}
	else { // loop closure case
		std::cout << "register_colored_point_cloud" << std::endl;
		auto ret = register_colored_point_cloud_icp(
			source, target, transformation_init);
		transformation = std::get<0>(ret);
		information = std::get<1>(ret);
	}

	bool success_local = false;
	if ((information(5, 5) / std::min(source.points_.size(), target.points_.size())) > 0.3) {
		success_local = true;
	}
	if (draw_result) {
		draw_registration_result_original_color(
			source, target, transformation);
	}
	return std::tuple<bool, Eigen::Matrix4d, Eigen::Matrix6d>(success_local, transformation, information);
}

std::tuple<Eigen::Matrix4d, three::PoseGraph> update_odometry_posegraph(int s, int t, Eigen::Matrix4d transformation, Eigen::Matrix6d information,
	Eigen::Matrix4d odometry, three::PoseGraph& pose_graph)
{
	std::cout << "Update PoseGraph" << std::endl;
	if (t == s + 1) { // odometry case
		odometry = transformation * odometry;
		Eigen::Matrix4d odometry_inv = odometry.inverse();
		pose_graph.nodes_.push_back(three::PoseGraphNode(odometry_inv));
		bool uncertain = false;
		pose_graph.edges_.push_back(
			three::PoseGraphEdge(s, t, transformation,
				information, uncertain));
	}
	else { // loop closure case
		bool uncertain = true;
		pose_graph.edges_.push_back(
			three::PoseGraphEdge(s, t, transformation,
				information, uncertain));
	}
	return std::tuple<Eigen::Matrix4d, three::PoseGraph>(odometry, pose_graph);
}

void register_point_cloud(std::string path_dataset, std::vector<std::string>& ply_file_names, bool draw_result = false)
{
	three::PoseGraph pose_graph = three::PoseGraph();
	Eigen::Matrix4d odometry = Eigen::Matrix4d::Identity();
	pose_graph.nodes_.push_back(three::PoseGraphNode(odometry));
	Eigen::Matrix6d info = Eigen::Matrix6d::Identity();

	int n_files = static_cast<int>(ply_file_names.size());
	for (int s = 0; s < n_files; s++) {
		for (int t = s + 1; t < n_files; t++) {
			std::shared_ptr<three::PointCloud> source = std::make_shared<three::PointCloud>();
			std::shared_ptr<three::PointCloud> target = std::make_shared<three::PointCloud>();
			std::cout << "reading " << ply_file_names[s] << " ...";
			bool ret = three::ReadPointCloud(ply_file_names[s], *source);
			std::cout << "reading " << ply_file_names[s] << " ...";
			ret = three::ReadPointCloud(ply_file_names[t], *target);
			auto ret_s = preprocess_point_cloud(*source);
			std::shared_ptr<three::PointCloud> source_down = std::get<0>(ret_s);
			std::shared_ptr<three::Feature> source_fpfh = std::get<1>(ret_s);
			auto ret_t = preprocess_point_cloud(*target);
			std::shared_ptr<three::PointCloud> target_down = std::get<0>(ret_t);
			std::shared_ptr<three::Feature> target_fpfh = std::get<1>(ret_t);
			auto ret_c = compute_initial_registartion(
				s, t, *source_down, *target_down,
				*source_fpfh, *target_fpfh, path_dataset, draw_result);
			bool success_global = std::get<0>(ret_c);
			Eigen::Matrix4d transformation_init = std::get<1>(ret_c);
			if (!success_global) {
				continue;
			}
			//std::cout << "success global" << s << " , " << t << std::endl;
			auto ret_l = local_refinement(s, t, *source, *target,
				transformation_init, draw_result);
			bool success_local = std::get<0>(ret_l);
			Eigen::Matrix4d transformation_icp = std::get<1>(ret_l);
			Eigen::Matrix6d information_icp = std::get<2>(ret_l);
			if (!success_local) {
				continue;
			}
			//std::cout << "success local" << s << " , " << t << std::endl;
			auto ret_u = update_odometry_posegraph(s, t,
				transformation_icp, information_icp,
				odometry, pose_graph);
			odometry = std::get<0>(ret_u);
			// pose_graph = std::get<1>(ret_u);
			// std::cout << pose_graph << std::endl;
		}
	}

	// three::WritePoseGraph(path_dataset + template_global_posegraph, pose_graph);
	three::WriteIJsonConvertibleToJSON(path_dataset + template_global_posegraph, pose_graph);
}

void register_fragments_main(std::string path_dataset) {
	three::SetVerbosityLevel(three::VerbosityLevel::VerboseDebug);

	std::shared_ptr<std::vector<std::string>> ply_file_names = get_file_list(path_dataset + folder_fragment, ".ply");
	make_folder(path_dataset + folder_scene);
	register_point_cloud(path_dataset, *ply_file_names);
	optimize_posegraph_for_scene(path_dataset);
}