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

c++版Open3DのVisualizationは、openFrameworks0.10.0で自前で作ると便利。

Open3Dは、3次元点群処理が簡単に出来て便利です。ですが、いくつか弱点があって
・Visualizationが便利なようで自由が利かず不便
・フォーマットが良く分からなくなることがある。ex. PointCloudとTriangleMeshの違いは？
・他のライブラリとのリンクが不便、だった（過去形）。

openFrameworks0.9.8以前では、Open3Dのリンクはとても面倒くさかったのですが、openFramworks0.10.0からは普通にリンクできるようになりました。
インタラクティブな3D表示はopenFrameworksの得意技なので、もうこっちに任せてしまいましょう。

編集中
ofxThreeGeometries.h

#pragma once

#include <iostream>
#include <memory>
#include <Eigen/Dense>

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

#include <ofNode.h>
#include <ofMesh.h>

class ofxThreeGeometries : public ofNode
{
public:
	ofVboMesh mesh;
        // ofMesh mesh;
	std::vector<ofMatrix4x4> mat;
	int mat_id;

	ofxThreeGeometries() { 
		mat.clear();
		mat_id = -1;
	}
	~ofxThreeGeometries() { 
		mat.clear();
		mesh.clear();
	}

	int set(const three::TriangleMesh &source) {
		switch (source.GetGeometryType()) {
		case three::Geometry::GeometryType::Unspecified:
			std::cout << "Unspecified" << std::endl;
			break;
		case three::Geometry::GeometryType::PointCloud:
			std::cout << "PointCloud" << std::endl;
			break;
		case three::Geometry::GeometryType::LineSet:
			std::cout << "LineSet" << std::endl;
			break;
		case three::Geometry::GeometryType::TriangleMesh:
			std::cout << "TriangleMesh" << std::endl;
			mesh.clear();
			if (source.HasVertices()) {
				for (int i = 0; i < source.vertices_.size(); i++) {
					ofVec3f v(source.vertices_.at(i).x(), source.vertices_.at(i).y(), source.vertices_.at(i).z());
					mesh.addVertex(v);
					if (source.HasVertexColors()) {
						ofColor c(source.vertex_colors_.at(i).x() * 255, source.vertex_colors_.at(i).y() * 255, source.vertex_colors_.at(i).z() * 255);
						mesh.addColor(c);
					}
					if (source.HasVertexNormals()) {
						ofVec3f n(source.vertex_normals_.at(i).x(), source.vertex_normals_.at(i).y(), source.vertex_normals_.at(i).z());
						mesh.addNormal(n);
					}
				}
			}
			if (source.HasTriangles()) {
				for (int i = 0; i < source.triangles_.size(); i++) {
					mesh.addTriangle(source.triangles_.at(i).x(), source.triangles_.at(i).y(), source.triangles_.at(i).z());
					if (source.HasTriangleNormals()) {
						// source.triangle_normals_.at(i).x();
					}
				}
			}
			{
				Eigen::Vector3d c = 0.5 * (source.GetMinBound() + source.GetMaxBound());
				this->setPosition(c.x(), c.y(), c.z());
				this->setOrientation(ofQuaternion());
			}
			break;
		case three::Geometry::GeometryType::Image:
			std::cout << "Image" << std::endl;
			break;
		default:
			break;
		}
		return 0;
	}

	int set(const three::PointCloud &source) {
		switch (source.GetGeometryType()) {
		case three::Geometry::GeometryType::Unspecified:
			std::cout << "Unspecified" << std::endl;
			break;
		case three::Geometry::GeometryType::PointCloud:
			//std::cout << "PointCloud" << std::endl;
			mesh.clear();
			if (source.HasPoints()) {
				for (int i = 0; i < source.points_.size(); i++) {
					ofVec3f v(source.points_.at(i).x(), source.points_.at(i).y(), source.points_.at(i).z());
					mesh.addVertex(v);

					if (source.HasColors()) {
						ofColor c(source.colors_.at(i).x() * 255, source.colors_.at(i).y() * 255, source.colors_.at(i).z() * 255);
						mesh.addColor(c);
					}
					if (source.HasNormals()) {
						ofVec3f n(source.normals_.at(i).x(), source.colors_.at(i).y(), source.colors_.at(i).z());
						mesh.addNormal(n);
					}
				}
			}
			{
				Eigen::Vector3d c = 0.5 * (source.GetMinBound() + source.GetMaxBound());
				this->setPosition(c.x(), c.y(), c.z());
			}
			break;
		case three::Geometry::GeometryType::LineSet:
			std::cout << "LineSet" << std::endl;
			break;
		case three::Geometry::GeometryType::TriangleMesh:
			std::cout << "TriangleMesh" << std::endl;
			break;
		case three::Geometry::GeometryType::Image:
			std::cout << "Image" << std::endl;
			break;
		default:
			break;
		}
	}

	void addTransformation(const Eigen::Matrix4d &Transformation) {
		// Transposed
		ofMatrix4x4 m(Transformation(0, 0), Transformation(1, 0), Transformation(2, 0), Transformation(3, 0),
				Transformation(0, 1), Transformation(1, 1), Transformation(2, 1), Transformation(3, 1),
				Transformation(0, 2), Transformation(1, 2), Transformation(2, 2), Transformation(3, 2),
				Transformation(0, 3), Transformation(1, 3), Transformation(2, 3), Transformation(3, 3));
		mat.push_back(m);
	}

	void setTransformationId(int i) {
		if (i >= 0 && i < mat.size()) {
			mat_id = i;
			this->setPosition(mat[mat_id].getTranslation());
			this->setOrientation(mat[mat_id].getRotate());
		}
	}

	int getTransformationId() {
		return mat_id;
	}

	void draw(ofPolyRenderMode renderMode) {
		ofPushMatrix();
		if (mat_id >= 0 && mat_id < mat.size()) {
			ofMultMatrix(mat[mat_id]);
		}
		mesh.draw(renderMode);
		
		ofPopMatrix();
	}

	void draw() {
		draw(ofPolyRenderMode::OF_MESH_POINTS);
	}
};

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

Optimal and Autonomous Control Using Reinforcement Learning: A Survey - IEEE Journals & Magazine
機械学習は、制御の自動化の方向へ進んでいくのは間違いない。

Dex-Net by BerkeleyAutomation
自動ビンピッキング

Kinematic and Dynamic Solvers | The Orocos Project
KDL IK。シンプルなC++で書かれたIKソルバー。

bitbucket.org
TRAC-IK。KDL-IKよりも性能が良いと謳うが、ROS必須。

[1712.07576] Learning to Act Properly: Predicting and Explaining Affordances from Images
アフォーダンス推定

Networked Virtual Environments: Design and Implementation (Siggraph Series)

[1603.00448] Guided Cost Learning: Deep Inverse Optimal Control via Policy Optimization
Guided Cost Learning

https://deepmind.com/blog/neural-scene-representation-and-rendering/
NN-SFM

https://sites.google.com/view/hierarchical-il-rl
階層的タスク解析

densepose.org
表現が不適切だけれど、FacebookのopenPose3D版

宮城県の市区町村を塗り分けろ - グラフ彩色問題 をD-Waveマシンで解く - T-Wave

pc.watch.impress.co.jp

CHOMP - Optimized Robotics
https://personalrobotics.ri.cmu.edu/files/courses/papers/Kalakrishnan11-stomp.pdf
ＲＲＴよりスマートな経路計画ライブラリ
ros.youtalk.jp — ROS Industrial MoveIt!事始め

http://www.mech.tohoku-gakuin.ac.jp/rde/contents/sendai/mechatro/archive/RMSeminar_No19.pdf

今日の漫画

数学を題材にした漫画。こんな天才数学少年も探せばいるのかな。

はじめアルゴリズム(1) (モーニング KC)

はじめアルゴリズム(2) (モーニング KC)

はじめアルゴリズム(3) (モーニング KC)

動力学シミュレータofxBulletを静的な衝突判定ライブラリとしてだけ使う方法 (その2)任意のメッシュ形状でも衝突判定したい

三角ポリゴンの集合で表現された任意形状をofxBulletで扱うためには， ofxBulletCustomShape()クラスを使います。
普通に動力学シミュレーションで使うだけなら、Exampleに倣って実行すれば問題ないかと思います。
github.com

ofxBullletCustomShapeクラスの改造

衝突判定をしたいときには、衝突グループと衝突マスクの指定を外から与えたいケースが多いのですが、標準ではそのインターフェースが用意されていませんので、ofxBulletCustomShapeクラスを魔改造してしまいましょう。

• add関数に衝突グループと衝突マスク指定の追加(他のプリミティブクラスのadd関数も同様に引数付きの呼び出しを追加すると良さそうですね。)
• addMesh関数に重心指定を追加（標準では頂点位置の平均位置になってしまう。)
• draw関数でmeshを描画、ただしメモリを無駄に食うので注意。

ofxBulletCustomShape.hのクラス定義内に追加

public:
	bool addMesh(ofMesh a_mesh, ofVec3f a_localScaling, bool a_bUseConvexHull, ofVec3f a_centroid);
        void addMaterial(ofMaterial a_material);
	void add(short group, short mask);

protected:
	std::vector<ofMesh>		meshes;
        std::vector<ofMaterial>            materials;

ofxBulletCustomShape.cppに追加及び変更

//--------------------------------------------------------------
bool ofxBulletCustomShape::addMesh(ofMesh a_mesh, ofVec3f a_localScaling, bool a_bUseConvexHull, ofVec3f a_centroid) {
	if (a_mesh.getMode() != OF_PRIMITIVE_TRIANGLES) {
		ofLog(OF_LOG_ERROR, "ofxBulletCustomShape :: addMesh : mesh must be set to OF_PRIMITIVE_TRIANGLES!! aborting");
		return false;
	}
	if (_bAdded == true) {
		ofLog(OF_LOG_ERROR, "ofxBulletCustomShape :: addMesh : can not call after calling add()");
		return false;
	}

	ofMesh scaled_mesh(a_mesh);
	for (size_t i(0); i < scaled_mesh.getNumVertices(); ++i) {
		scaled_mesh.setVertex(i, scaled_mesh.getVertex(i) * a_localScaling);
	}
	meshes.push_back(scaled_mesh);

	btVector3 localScaling(a_localScaling.x, a_localScaling.y, a_localScaling.z);
	auto indicies = a_mesh.getIndices();
	auto verticies = a_mesh.getVertices();

	btVector3 centroid = btVector3(a_centroid.x * a_localScaling.x,
			a_centroid.y * a_localScaling.y,
			a_centroid.z * a_localScaling.z);

	if (!a_bUseConvexHull) {
		vector<btVector3> newVerts;
		for (int i = 0; i < indicies.size(); i++) {
			btVector3 vertex(verticies[indicies[i]].x, verticies[indicies[i]].y, verticies[indicies[i]].z);
			vertex *= localScaling;
			vertex -= centroid;
			newVerts.push_back(vertex);
		}

		btConvexHullShape* convexShape = new btConvexHullShape(&(newVerts[0].getX()), newVerts.size());
		convexShape->setMargin(0.01f);
		shapes.push_back(convexShape);
		centroids.push_back( ofVec3f(centroid.getX(), centroid.getY(), centroid.getZ()) );
	}
	else {
		// HULL Building code from example ConvexDecompositionDemo.cpp //
		btTriangleMesh* trimesh = new btTriangleMesh();

		for (int i = 0; i < indicies.size() / 3; i++) {
			int index0 = indicies[i * 3];
			int index1 = indicies[i * 3 + 1];
			int index2 = indicies[i * 3 + 2];

			btVector3 vertex0(verticies[index0].x, verticies[index0].y, verticies[index0].z);
			btVector3 vertex1(verticies[index1].x, verticies[index1].y, verticies[index1].z);
			btVector3 vertex2(verticies[index2].x, verticies[index2].y, verticies[index2].z);

			vertex0 *= localScaling;
			vertex1 *= localScaling;
			vertex2 *= localScaling;

			trimesh->addTriangle(vertex0, vertex1, vertex2);
		}

		//cout << "ofxBulletCustomShape :: addMesh : input triangles = " << trimesh->getNumTriangles() << endl;
		//cout << "ofxBulletCustomShape :: addMesh : input indicies = " << indicies.size() << endl;
		//cout << "ofxBulletCustomShape :: addMesh : input verticies = " << verticies.size() << endl;

		btConvexShape* tmpConvexShape = new btConvexTriangleMeshShape(trimesh);

		//create a hull approximation
		btShapeHull* hull = new btShapeHull(tmpConvexShape);
		btScalar margin = tmpConvexShape->getMargin();
		hull->buildHull(margin);
		tmpConvexShape->setUserPointer(hull);

		centroid = btVector3(a_centroid.x * a_localScaling.x,
				a_centroid.y * a_localScaling.y,
				a_centroid.z * a_localScaling.z);

		//printf("ofxBulletCustomShape :: addMesh : new hull numTriangles = %d\n", hull->numTriangles());
		//printf("ofxBulletCustomShape :: addMesh : new hull numIndices = %d\n", hull->numIndices());
		//printf("ofxBulletCustomShape :: addMesh : new hull numVertices = %d\n", hull->numVertices());

		btConvexHullShape* convexShape = new btConvexHullShape();
		for (int i = 0; i<hull->numVertices(); i++) {
			convexShape->addPoint(hull->getVertexPointer()[i] - centroid);
		}

		delete tmpConvexShape;
		delete hull;

		shapes.push_back(convexShape);
		centroids.push_back(ofVec3f(centroid.getX(), centroid.getY(), centroid.getZ()));
	}
	return true;
}

//--------------------------------------------------------------
void ofxBulletCustomShape::add(short group, short mask) {
	_bAdded = true;
	btTransform trans;
	trans.setIdentity();

	for (int i = 0; i < centroids.size(); i++) {
		_centroid += centroids[i];
	}
	if (centroids.size() > 0)
		_centroid /= (float)centroids.size();
	btVector3 shiftCentroid;
	for (int i = 0; i < shapes.size(); i++) {
		shiftCentroid = btVector3(centroids[i].x, centroids[i].y, centroids[i].z);
		shiftCentroid -= btVector3(_centroid.x, _centroid.y, _centroid.z);
		trans.setOrigin((shiftCentroid));
		((btCompoundShape*)_shape)->addChildShape(trans, shapes[i]);
	}
	_rigidBody = ofGetBtRigidBodyFromCollisionShape(_shape, _startTrans, _mass);
	setCreated(_rigidBody);
	createInternalUserData();
	_world->addRigidBody(_rigidBody, group, mask); ///// changed
	setProperties(.4, .75);
	setDamping(.25);
}

//--------------------------------------------------------------
void ofxBulletCustomShape::addMaterial(ofMaterial a_material) {
	materials.push_back(a_material);
}

//--------------------------------------------------------------
void ofxBulletCustomShape::draw() {
	if (meshes.size()) {
		ofPushMatrix();
		transformGL();
		for (size_t i(0); i < meshes.size(); ++i) {
			if (i < materials.size()) {
				materials[i].begin();
				meshes[i].draw();
				materials[i].end();
			}
			else
				meshes[i].draw();
		}
		restoreTransformGL();
		ofPopMatrix();
	}
}

呼び出し方

呼び出しのサンプルは、前回のページとの差分を部分的に抽出すると
モデル作成

	shapes.push_back(new ofxBulletCustomShape());
	for (int i = 0; i < model.getNumMeshes(); i++) {
		((ofxBulletCustomShape*)shapes.back())->addMesh(model.getMesh(i), ofVec3f(1.f, 1.f, 1.f), false, ofVec3f(0.f, 0.f, 0.f));

		ofxAssimpMeshHelper & meshHelper = assimpModel.getMeshHelper(i);
		logos[i]->addMaterial(meshHelper.material);
	}
	((ofxBulletCustomShape*)shapes.back())->create(world.world, ofVec3f(), 0.1);
	shapes.back()->add(1, 7);

位置の更新

	btTransform trans = ofGetBtTransform( model.getModelMatrix().getTranslation() + model_mat.getTranslation(),  model.getModelMatrix().getRotate() * model_mat.getRotate());
	shapes[id]->getRigidBody()->setWorldTransform(trans);
	shapes[id]->getRigidBody()->getMotionState()->setWorldTransform(trans);

	world.update(0.f, 1);

Assimpの座標への変換がややこしい。addModelの際に頂点座標を変換して織り込んでしまったほうが良いかもしれない。
描画の更新

	ofEnableLighting();
	ofPushStyle();
	//world.drawDebug();
	for (size_t i(0); i < shapes.size(); i++) {
		if (bColliding[i]) {
			ofSetColor(ofColor::yellow, 50);
		}
		else {
			ofSetColor(ofColor::white, 50);
		}
		shapes[i]->draw();
	}
	ofPopStyle();

*1->addMesh(model.getMesh(i), ofVec3f(1.f, 1.f, 1.f), false, ofVec3f(0.f, 0.f, 0.f));の3番目の引数のfalseが重要。
falseだと簡易化した多面体として衝突テストを行い、trueだと生のポリゴンで衝突テストを行う。実用的な速さを求めるなら、false。精度が欲しいならtrue。

今日の本文

レイリが面白い。

レイリ 第4巻 (少年チャンピオン・コミックスエクストラ)

*1:ofxBulletCustomShape*)shapes.back(

動力学シミュレータofxBulletを静的な衝突判定ライブラリとしてだけ使う方法

OMPLが自由に使えるようになって経路計画がお手軽で楽しいのですが、衝突判定を自分で書くのが億劫です。プリミティブな形状同士の衝突判定は一つ一つであればたいして難しくもありませんが、効率的に、さまざまな形状の、複数物体同士を、衝突するものとしないもののグループを考慮して、衝突判定する、なんていうことを考え始めると、途端に面倒くさくなります。

衝突判定ライブラリ探し

衝突判定ライブラリはすでに様々な提案があるのですが、openFrameworksで簡単に使えるということを考えるとC++がよく、なおかつ誰かがすでにaddonを作ってくれていることが望ましいということになります。
文献1に素晴らしいまとめがあります。どの程度最新のものまでをカバーしているのかわかりませんが、少なくともこの2年ぐらいのメジャーなライブラリが表にまとめられています。この中で、該当しそうなものを探すとFCLかBullet, ODE, ReactPhysics3dが良さそうだとわかります。FCLのofAddonを探してみましたが見つかりませんでした。またFCLはまだ開発途上だと本家のページに注意書きがあります。ofxODEもよさそうですが、以前使った感触として分かりにくかった記憶があるので今回は避けます。ofxBulletは動力学シミュレーションを使った派手な演出を行うために、結構有名です。動力学計算の機能は不要ですが、今回はofxBulletの衝突判定機能だけを取り出して使ってみたいと思います。

衝突判定のサンプル

物体が動かない時間が止まった状態で、こちらで与えた物体の位置姿勢で、物体同士が衝突を起こしているかどうかを判別したいと思います。サンプルとして、球、箱、円柱の3種類のプリミティブに適当な位置姿勢を与えて判定することにします。ただし、球と箱は仲がよいもの同士と仮定して、球と箱は衝突状態でも衝突を判定しないことにします。

物体の位置姿勢を強制的に更新する方法は[4]で示されています。

衝突フィルタリングの方法は、[5][6]で示されています。
worldへの登録は、shapes->add関数ではなく､フラグを付けつつ自前でやります。world.update()は静的に判定できれば良いので、時間を0、更新回数を1回にします。

大雑把なフローチャートはこんな感じで、簡単。衝突判定結果は、コールバック関数の中でフラグの更新をして返されます。

ofApp.h

#pragma once

#include "ofMain.h"
#include "ofxBullet.h"

class ofApp : public ofBaseApp{

	public:
		void setup();
		void update();
		void draw();

		void keyPressed(int key);
		
		ofxBulletWorldRigid	world;
		std::vector<ofxBulletRigidBody*> shapes;
		ofEasyCam cam;
		ofLight light;
		std::vector<bool> bColliding;

		void onCollision(ofxBulletCollisionData& cdata);
};

ofApp.cpp

#include "ofApp.h"

//--------------------------------------------------------------
void ofApp::onCollision(ofxBulletCollisionData& cdata) {
	for (int i = 0; i < shapes.size(); i++) {
		if(*shapes[i] == cdata) {
			bColliding[i] = true;
		}
	}
}

//--------------------------------------------------------------
void ofApp::setup(){
	ofSetFrameRate(60);
	ofSetVerticalSync(true);
	ofSetBackgroundColor(ofColor::black);

	world.setup();
	world.enableCollisionEvents();
	ofAddListener(world.COLLISION_EVENT, this, &ofApp::onCollision);
	world.setCamera(&cam);
	world.setGravity(ofVec3f(0, 0, -9.8f));

	// Collision Settings
#define BIT(x) (1<<(x))
	enum collisiontypes {
		COL_NOTHING = 0,
		COL_SPHERE = BIT(0),
		COL_BOX = BIT(1),
		COL_CAPSULE = BIT(2),
		COL_CONE = BIT(3),
		COL_CYLINDER = BIT(4)
	};

	// Objects
	shapes.push_back(new ofxBulletSphere());
	((ofxBulletSphere*)shapes.back())->init(ofBtGetSphereCollisionShape(0.8f));
	((ofxBulletSphere*)shapes.back())->create(world.world);
	shapes.back()->setActivationState(DISABLE_DEACTIVATION);
	world.world->addCollisionObject(shapes.back()->getCollisionObject(), COL_SPHERE, COL_CAPSULE | COL_CONE | COL_CYLINDER);

	shapes.push_back(new ofxBulletBox());
	((ofxBulletBox*)shapes.back())->init(ofBtGetBoxCollisionShape(1.2f));
	((ofxBulletBox*)shapes.back())->create(world.world);
	shapes.back()->setActivationState(DISABLE_DEACTIVATION);
	world.world->addCollisionObject(shapes.back()->getCollisionObject(), COL_BOX, COL_CAPSULE | COL_CONE | COL_CYLINDER);

#if 0
	shapes.push_back(new ofxBulletCapsule());
	((ofxBulletCapsule*)shapes.back())->init(ofBtGetCapsuleCollisionShape(0.4f, 2.2f));
	((ofxBulletCapsule*)shapes.back())->create(world.world, ofVec3f(0.f, 0.f, 0.f), 0.2f);
	shapes.back()->setActivationState(DISABLE_DEACTIVATION);
	world.world->addCollisionObject(shapes.back()->getCollisionObject(), COL_CAPSULE, COL_SPHERE | COL_BOX | COL_CONE | COL_CYLINDER);

	shapes.push_back(new ofxBulletCone());
	((ofxBulletCone*)shapes.back())->init(ofBtGetConeCollisionShape(0.9f, 0.6f));
	((ofxBulletCone*)shapes.back())->create(world.world);
	shapes.back()->setActivationState(DISABLE_DEACTIVATION);
	world.world->addCollisionObject(shapes.back()->getCollisionObject(), COL_CONE, COL_SPHERE | COL_BOX | COL_CAPSULE);
#endif

	shapes.push_back(new ofxBulletCylinder());
	((ofxBulletCylinder*)shapes.back())->init(ofBtGetCylinderCollisionShape(0.7f, 0.7f));
	((ofxBulletCylinder*)shapes.back())->create(world.world, ofVec3f(0.f, 0.f, 0.f), 0.4f);
	shapes.back()->setActivationState(DISABLE_DEACTIVATION);
	world.world->addCollisionObject(shapes.back()->getCollisionObject(), COL_CYLINDER, COL_SPHERE | COL_BOX | COL_CAPSULE);

	for (size_t i(0); i < shapes.size(); ++i) {
		bColliding.push_back(false);
	}

	// Camera and Light settings
	light.setParent(cam, true);
	cam.setFarClip(16.f);
	cam.setNearClip(0.001f);
	cam.setDistance(8.f);
	cam.setPosition(0.f, -8.f, 0.f);
	cam.lookAt(ofVec3f(0.f, 0.f, 0.f), ofVec3f(0.f, 0.f, 1.f));
}

//--------------------------------------------------------------
void ofApp::update(){
	for (size_t i(0); i < bColliding.size(); i++) {
		bColliding[i] = false;
	}
	world.update(0.f, 1);
}

//--------------------------------------------------------------
void ofApp::draw(){
	ofEnableDepthTest();
	ofEnableLighting();
	light.enable();

	cam.begin();
	ofSetColor(ofColor::white);
	for (size_t i(0); i < shapes.size(); i++) {
		ofPushStyle();
		if (bColliding[i]) ofSetColor(ofColor::yellow);
		shapes[i]->draw();
		ofPopStyle();
	}

	// draw GroundPlane
	ofDisableLighting();
	ofPushMatrix();
	ofTranslate(0.f, 0.f, 0.f);
	ofRotate(90.f, 0.f, 1.f, 0.f);
	ofPushStyle();
	ofSetColor(ofColor::blue);
	ofDrawGridPlane(0.1f, 10, false);
	ofPopStyle();
	ofPopMatrix();

	cam.end();
}

//--------------------------------------------------------------
void ofApp::keyPressed(int key){
	// Updates for Shapes Position
	for (size_t i(0); i < shapes.size(); i++) {
		ofVec3f pos = ofVec3f(ofRandom(-2.f, 2.f), ofRandom(-2.f, 2.f), ofRandom(-2.f, 2.f));
		ofQuaternion quat;
		quat.makeRotate(ofVec3f(0.f, 1.f, 0.f), ofVec3f(ofRandom(-1.f, 1.f), ofRandom(-1.f, 1.f), ofRandom(-1.f, 1.f)).normalized());
		btTransform trans = ofGetBtTransform(pos, quat);
		shapes[i]->getRigidBody()->setWorldTransform(trans);
		shapes[i]->getRigidBody()->getMotionState()->setWorldTransform(trans);
	}
}

実行結果はこんな感じ。
何かキーを押すと、位置姿勢がランダムに更新されます。衝突していれば黄色、していなければ白色でプリミティブが表示されます。

コメントアウトしてあるCapsuleやConeは、ofxBulletUtils.hにわずかな変更を加えると使えるようになります。

static btCapsuleShape* ofBtGetCapsuleCollisionShape(float a_radius, float a_height) {
	return new btCapsuleShape(a_radius, a_height);
}

static btConeShape* ofBtGetConeCollisionShape(float a_radius, float a_height) {
	return new btConeShape(a_radius, a_height);
}

また、ofxBulletのCylinderやCapsuleのcreate関数の姿勢の指定にはバグがあります。Quaternionと指定しているのに内部でAxis-Angleとして渡しています。トラブル防止のために、Create関数では姿勢を与えないほうが無難です。

[1] Awesome Collision Detection
github.com

[2] ofxBullet
github.com

[3] Bulletについて
Bullet - Game Science Wiki

[4] Bulletで強制的に位置姿勢を更新する方法
stackoverflow.com

[5] 衝突テストを行う物同士のラベル付けの方法
Bullet Physics Japanese Manual - 06

[6] Collision Filtering
Collision Filtering - Physics Simulation Wiki

[]
2018年5月15日 – プログラミング de 落書き

今日のマンガ

星崎真紀先生の、魔法のリノベ全3巻がリアルで面白い。発行部数が少ないのか書店で見かけることはまずありませんので、ネットで注文。

魔法のリノベ ： 1 (ジュールコミックス)

魔法のリノベ(2) (ジュールコミックス)

魔法のリノベ(3) (ジュールコミックス)

openFrameworksでOMPLを動かしたい（その5）

また[1]を参考に、修正を加えてみます。
１．障害物を追加
２．cboundsの値を調整
３．アニメーションを追加

#pragma once

#include "ofxOMPL.h"
#include "ofMain.h"

class Scheduler : public ofThread {
public:
	unsigned long counter = 0;
	std::vector<double> durations;

	Scheduler() {
		counter = 0;
	}

	void start() {
		counter = 0;
		timer.setPeriodicEvent(durations[0] * 1000000000);
		baseElapsedTimeMills = ofGetElapsedTimeMillis();
		startThread();
	}

	void setDurations(std::vector<double>& d) {
		durations = d;
	}

	uint64_t getPeriod()
	{
		return ofGetElapsedTimeMillis() - baseElapsedTimeMills;
	}

private:
	ofTimer timer;
	void threadedFunction() {
		while (isThreadRunning()) {
			timer.waitNext();

			baseElapsedTimeMills = ofGetElapsedTimeMillis();
			counter++;
			if (counter >= durations.size()) {
				counter = 0;
			}
			timer.setPeriodicEvent(durations[counter] * 1000000000);
		}
	}
	uint64_t baseElapsedTimeMills;
};

class ofApp : public ofBaseApp{

	public:
		void setup();
		void update();
		void draw();

		void keyPressed(int key);
		void keyReleased(int key);
		void mouseMoved(int x, int y );
		void mouseDragged(int x, int y, int button);
		void mousePressed(int x, int y, int button);
		void mouseReleased(int x, int y, int button);
		void mouseEntered(int x, int y);
		void mouseExited(int x, int y);
		void windowResized(int w, int h);
		void dragEvent(ofDragInfo dragInfo);
		void gotMessage(ofMessage msg);
		
		void planWithSimpleSetup();
		bool isStateValid(const ompl::control::SpaceInformation *si, const ompl::base::State *state);

		std::shared_ptr<ompl::base::RealVectorBounds> bounds;
		std::shared_ptr<ompl::base::RealVectorBounds> cbounds;
		//std::shared_ptr<ompl::geometric::SimpleSetup> ss;
		void propagate(const ompl::base::State *start, const ompl::control::Control *control, const double duration, ompl::base::State *result);
		ompl::base::PlannerStatus solved;

protected:
		ofEasyCam cam;
		ofBoxPrimitive boundsbox;
		ofBoxPrimitive obstacle;
		ofPolyline polyline;
		std::vector<ofMatrix4x4> stateAxis;
		std::vector<ofVec3f> stateAxisRaw;
		std::vector<ofVec3f> controlAxis;
		Scheduler mysch;
};

*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#include "ofApp.h"

#include <iostream>

namespace ob = ompl::base;
namespace og = ompl::geometric;
namespace oc = ompl::control;

template<typename T>
inline T FMod(const T &x, const T &y)
{
	if (y == 0)  return x;
	return x - y*std::floor(x / y);
}
// Convert radian to [-pi,pi)
double RadToNPiPPi(const double &x)
{
	return FMod(x + PI, PI*2.0) - PI;
}

// a decomposition is only needed for SyclopRRT and SyclopEST
class MyDecomposition : public oc::GridDecomposition
{
public:
	MyDecomposition(const int length, const ob::RealVectorBounds& bounds)
		: GridDecomposition(length, 2, bounds)
	{
	}
	void project(const ob::State* s, std::vector<double>& coord) const override
	{
		coord.resize(2);
		coord[0] = s->as<ob::SE2StateSpace::StateType>()->getX();
		coord[1] = s->as<ob::SE2StateSpace::StateType>()->getY();
	}

	void sampleFullState(const ob::StateSamplerPtr& sampler, const std::vector<double>& coord, ob::State* s) const override
	{
		sampler->sampleUniform(s);
		s->as<ob::SE2StateSpace::StateType>()->setXY(coord[0], coord[1]);
	}
};

ofApp* app;

bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
{
	return app->isStateValid(si, state);
}

bool ofApp::isStateValid(const oc::SpaceInformation *si, const ob::State *state)
{
	//    ob::ScopedState<ob::SE2StateSpace>
	// cast the abstract state type to the type we expect
	const auto *se2state = state->as<ob::SE2StateSpace::StateType>();

	// extract the first component of the state and cast it to what we expect
	const auto *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);

	// extract the second component of the state and cast it to what we expect
	const auto *rot = se2state->as<ob::SO2StateSpace::StateType>(1);

	// check validity of state defined by pos & rot
	ofVec3f low = -0.5f * obstacle.getSize() + obstacle.getPosition();
	ofVec3f high = 0.5f * obstacle.getSize() + obstacle.getPosition();
	if ((low.x <= pos->values[0] && pos->values[0] < high.x) &&
		(low.y <= pos->values[1] && pos->values[1] < high.y)) {
		return false;
	}

	// return a value that is always true but uses the two variables we define, so we avoid compiler warnings
	return si->satisfiesBounds(state) && (const void*)rot != (const void*)pos;
}

void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result)
{
	app->propagate(start, control, duration, result);
}

void ofApp::propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result)
{
	const auto *se2state = start->as<ob::SE2StateSpace::StateType>();
	const double* pos = se2state->as<ob::RealVectorStateSpace::StateType>(0)->values;
	const double rot = se2state->as<ob::SO2StateSpace::StateType>(1)->value;
	const double* ctrl = control->as<oc::RealVectorControlSpace::ControlType>()->values;

	result->as<ob::SE2StateSpace::StateType>()->setXY(
		pos[0] + ctrl[0] * duration * cos(rot),
		pos[1] + ctrl[0] * duration * sin(rot));
	result->as<ob::SE2StateSpace::StateType>()->setYaw(
		RadToNPiPPi(rot + ctrl[1] * duration));
}

void ofApp::planWithSimpleSetup()
{
	// construct the state space we are planning in
	auto space(std::make_shared<ob::SE2StateSpace>());

	// set the bounds for the R^2 part of SE(2)
	bounds = std::make_shared<ob::RealVectorBounds>(2);
	bounds->setLow(-1);
	bounds->setHigh(1);

	space->setBounds(*bounds);

	// create a control space
	auto cspace(std::make_shared<oc::RealVectorControlSpace>(space, 2));

	// set the bounds for the control space
	cbounds = std::make_shared<ob::RealVectorBounds>(2);
	cbounds->setLow(0.f, 0.f);
	cbounds->setHigh(0.f, 0.5f);
	cbounds->setLow(1.f, -2.f);
	cbounds->setHigh(1.f, 2.f);

	cspace->setBounds(*cbounds);

	// define a simple setup class
	oc::SimpleSetup ss(cspace);

	// set the state propagation routine
	ss.setStatePropagator(::propagate);

	// set state validity checking for this space
	ss.setStateValidityChecker(
		[&ss](const ob::State *state) { return ::isStateValid(ss.getSpaceInformation().get(), state); });

	// create a start state
	ob::ScopedState<ob::SE2StateSpace> start(space);
	start->setX(-0.5);
	start->setY(0.0);
	start->setYaw(0.0);

	// create a  goal state; use the hard way to set the elements
	ob::ScopedState<ob::SE2StateSpace> goal(space);
	(*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
	(*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.5;
	(*goal)[1]->as<ob::SO2StateSpace::StateType>()->value = 0.0;


	// set the start and goal states
	ss.setStartAndGoalStates(start, goal, 0.05);

	//ss.setPlanner(std::make_shared<oc::PDST>(ss.getSpaceInformation()));
	//ss.getSpaceInformation()->setMinMaxControlDuration(1,100);
	// attempt to solve the problem within one second of planning time
	solved = ss.solve(10.0);

	if (solved)
	{
		std::cout << "Found solution:" << std::endl;
		// print the path to screen

		ss.getSolutionPath().printAsMatrix(std::cout);
	}
	else
		std::cout << "No solution found" << std::endl;

	// setups for drawings
	// bound
	std::vector<double> h = bounds->high;
	std::vector<double> l = bounds->low;
	ofVec3f high, low, vol;
	for (int i = 0; i < h.size(); i++) {
		high[i] = h[i];
		low[i] = l[i];
	}
	vol = high - low;
	boundsbox.set(vol[0], vol[1], vol[2], 1, 1, 1);
	boundsbox.setPosition((high + low) / 2.f);

	if (solved)
	{
		stateAxis.clear();
		polyline.clear();
		controlAxis.clear();
		for (int i = 0; i < ss.getSolutionPath().getStateCount(); i++) {
			std::vector<double> reals;
			ss.getStateSpace()->copyToReals(reals, ss.getSolutionPath().getState(i));
			ofMatrix4x4 mat;
			mat.setTranslation(reals[0], reals[1], 0.f);
			ofQuaternion quat;
			quat.makeRotate(ofRadToDeg(reals[2]), ofVec3f(0.f, 0.f, 1.f));
			mat.setRotate(quat);
			stateAxis.push_back(mat);
			polyline.addVertex(mat.getTranslation());

			stateAxisRaw.push_back(ofVec3f(reals[0], reals[1], reals[2]));
		}

		for (int i = 0; i < ss.getSolutionPath().getControlCount(); i++) {
			const double* c = ss.getSolutionPath().getControl(i)->as<oc::RealVectorControlSpace::ControlType>()->values;
			double duration = ss.getSolutionPath().getControlDuration(i);
			controlAxis.push_back(ofVec3f(c[0], c[1], duration));
		}
		mysch.setDurations(ss.getSolutionPath().getControlDurations());
	}
}


//--------------------------------------------------------------
void ofApp::setup() {
	app = this;
	std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
	obstacle.setWidth(0.2f);
	obstacle.setHeight(0.8f);
	obstacle.setDepth(0.2f);
	obstacle.setPosition(-0.2f, 0.2f, 0.f);
	planWithSimpleSetup();
	mysch.start();

	cam.setNearClip(0.001f);
	cam.setTarget(boundsbox);
	cam.setDistance(boundsbox.getSize().length() * 2.f);
}

//--------------------------------------------------------------
void ofApp::update() {

}

//--------------------------------------------------------------
void ofApp::draw() {
	ofSetBackgroundColor(ofColor::black);

	cam.begin();
	ofPushStyle();
	ofSetColor(ofColor::blue);
	ofDrawGridPlane(0.01f);
	ofSetColor(ofColor::grey, 66);
	boundsbox.drawWireframe();
	ofSetColor(ofColor::darkBlue, 66);
	obstacle.draw();
	ofPopStyle();
	if (solved)
	{
		ofPushStyle();
		ofSetColor(ofColor::yellow);
		polyline.draw();
		ofPopStyle();
		for (unsigned int i = 0; i <= mysch.counter; i++) {
			ofPushMatrix();
			ofMultMatrix(stateAxis[i]);
			ofDrawAxis(boundsbox.getSize().length() / 100.f);
			ofPopMatrix();
		}

		float t = (float)mysch.getPeriod() / 1000.f;
		ofVec3f pos = stateAxisRaw[mysch.counter];
		ofVec3f ctr = controlAxis[mysch.counter];
		ofMatrix4x4 mat;
		float rad = RadToNPiPPi(pos[2] + t * ctr[1]);
		mat.setTranslation(pos[0] + ctr[0] * t * cos(pos[2]),
				pos[1] + ctr[0] * t * sin(pos[2]),
				0.f);
		ofQuaternion quat;
		quat.makeRotate(ofRadToDeg(rad), ofVec3f(0.f, 0.f, 1.f));
		mat.setRotate(quat);
		ofPushMatrix();
		ofMultMatrix(mat);
		ofDrawAxis(boundsbox.getSize().length() / 100.f);
		ofPopMatrix();
	}
	cam.end();
}

//--------------------------------------------------------------
void ofApp::keyPressed(int key) {

}

//--------------------------------------------------------------
void ofApp::keyReleased(int key) {

}

//--------------------------------------------------------------
void ofApp::mouseMoved(int x, int y) {

}

//--------------------------------------------------------------
void ofApp::mouseDragged(int x, int y, int button) {

}

//--------------------------------------------------------------
void ofApp::mousePressed(int x, int y, int button) {

}

//--------------------------------------------------------------
void ofApp::mouseReleased(int x, int y, int button) {

}

//--------------------------------------------------------------
void ofApp::mouseEntered(int x, int y) {

}

//--------------------------------------------------------------
void ofApp::mouseExited(int x, int y) {

}

//--------------------------------------------------------------
void ofApp::windowResized(int w, int h) {

}

//--------------------------------------------------------------
void ofApp::gotMessage(ofMessage msg) {

}

//--------------------------------------------------------------
void ofApp::dragEvent(ofDragInfo dragInfo) {

}

実行結果はこんな感じ。障害物を避けてスタートとゴールを結んでいます。
アニメーションは、よく見ると時々はみ出している・・・何かを間違えているっぽい気がする。。。

[1] Robotics/OMPL - NAIST::OnlineText 制御指令値を含むプラニング

openFrameworksでOMPLを動かしたい（その4）

omplのdemo_RigidBodyPlanningWithControlsをopenFrameworksに移植していきます。

ofxOMPL.hの更新

必要なヘッダーファイルを足していきます。
ofxOMPL.h

#pragma once

// Plaease add these settings manually
// #include path example:	D:\workspace\ompl\src
//					D:\workspace\eigen-334
//					D:\workspace\boost_1_58_0
// #library path example:	D:\workspace\ompl\build\release\lib\Release
//					(D:\workspace\ompl\build\release\lib\Debug)
//					D:\workspace\boost_1_58_0\stage\x64\lib

#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>
#include <ompl/geometric/SimpleSetup.h>

#include <ompl/config.h>

#include <ompl/base/goals/GoalState.h>
#include <ompl/base/spaces/SE2StateSpace.h>
#include <ompl/control/spaces/RealVectorControlSpace.h>
#include <ompl/control/planners/kpiece/KPIECE1.h>
#include <ompl/control/planners/rrt/RRT.h>
#include <ompl/control/planners/est/EST.h>
#include <ompl/control/planners/syclop/SyclopRRT.h>
#include <ompl/control/planners/syclop/SyclopEST.h>
#include <ompl/control/planners/pdst/PDST.h>
#include <ompl/control/planners/syclop/GridDecomposition.h>
#include <ompl/control/SimpleSetup.h>

#include <ompl/geometric/planners/prm/PRM.h>

#if _DEBUG
#pragma comment(lib, "ompl.lib")
#pragma comment(lib, "boost_serialization-vc140-mt-1_58.lib")
#else
#pragma comment(lib, "ompl.lib")
#pragma comment(lib, "boost_serialization-vc140-mt-1_58.lib")
#endif

myRigidBodyPlanningWithControlsプロジェクト

main.cppはデフォルトのままなので省略。
ofApp.h

#pragma once

#include "ofxOMPL.h"
#include "ofMain.h"

class ofApp : public ofBaseApp{

	public:
		void setup();
		void update();
		void draw();

		void keyPressed(int key);
		void keyReleased(int key);
		void mouseMoved(int x, int y );
		void mouseDragged(int x, int y, int button);
		void mousePressed(int x, int y, int button);
		void mouseReleased(int x, int y, int button);
		void mouseEntered(int x, int y);
		void mouseExited(int x, int y);
		void windowResized(int w, int h);
		void dragEvent(ofDragInfo dragInfo);
		void gotMessage(ofMessage msg);
		
		void planWithSimpleSetup();
		bool isStateValid(const ompl::control::SpaceInformation *si, const ompl::base::State *state);

		std::shared_ptr<ompl::base::RealVectorBounds> bounds;
		std::shared_ptr<ompl::base::RealVectorBounds> cbounds;
		//std::shared_ptr<ompl::geometric::SimpleSetup> ss;
		ompl::base::PlannerStatus solved;
protected:
		ofEasyCam cam;
		ofBoxPrimitive boundsbox;

		ofPolyline polyline;
		std::vector<ofMatrix4x4> stateAxis;
};

ofApp.cpp

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#include "ofApp.h"

#include <iostream>

namespace ob = ompl::base;
namespace og = ompl::geometric;
namespace oc = ompl::control;

// a decomposition is only needed for SyclopRRT and SyclopEST
class MyDecomposition : public oc::GridDecomposition
{
public:
	MyDecomposition(const int length, const ob::RealVectorBounds& bounds)
		: GridDecomposition(length, 2, bounds)
	{
	}
	void project(const ob::State* s, std::vector<double>& coord) const override
	{
		coord.resize(2);
		coord[0] = s->as<ob::SE2StateSpace::StateType>()->getX();
		coord[1] = s->as<ob::SE2StateSpace::StateType>()->getY();
	}

	void sampleFullState(const ob::StateSamplerPtr& sampler, const std::vector<double>& coord, ob::State* s) const override
	{
		sampler->sampleUniform(s);
		s->as<ob::SE2StateSpace::StateType>()->setXY(coord[0], coord[1]);
	}
};

ofApp* app;

bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
{
	return app->isStateValid(si, state);
}

bool ofApp::isStateValid(const oc::SpaceInformation *si, const ob::State *state)
{
	//    ob::ScopedState<ob::SE2StateSpace>
	// cast the abstract state type to the type we expect
	const auto *se2state = state->as<ob::SE2StateSpace::StateType>();

	// extract the first component of the state and cast it to what we expect
	const auto *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);

	// extract the second component of the state and cast it to what we expect
	const auto *rot = se2state->as<ob::SO2StateSpace::StateType>(1);

	// check validity of state defined by pos & rot


	// return a value that is always true but uses the two variables we define, so we avoid compiler warnings
	return si->satisfiesBounds(state) && (const void*)rot != (const void*)pos;
}

void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result)
{
	const auto *se2state = start->as<ob::SE2StateSpace::StateType>();
	const double* pos = se2state->as<ob::RealVectorStateSpace::StateType>(0)->values;
	const double rot = se2state->as<ob::SO2StateSpace::StateType>(1)->value;
	const double* ctrl = control->as<oc::RealVectorControlSpace::ControlType>()->values;

	result->as<ob::SE2StateSpace::StateType>()->setXY(
		pos[0] + ctrl[0] * duration * cos(rot),
		pos[1] + ctrl[0] * duration * sin(rot));
	result->as<ob::SE2StateSpace::StateType>()->setYaw(
		rot + ctrl[1] * duration);
}

void ofApp::planWithSimpleSetup()
{
	// construct the state space we are planning in
	auto space(std::make_shared<ob::SE2StateSpace>());

	// set the bounds for the R^2 part of SE(2)
	bounds = std::make_shared<ob::RealVectorBounds>(2);
	bounds->setLow(-1);
	bounds->setHigh(1);

	space->setBounds(*bounds);

	// create a control space
	auto cspace(std::make_shared<oc::RealVectorControlSpace>(space, 2));

	// set the bounds for the control space
	cbounds = std::make_shared<ob::RealVectorBounds>(2);
	cbounds->setLow(-0.3);
	cbounds->setHigh(0.3);

	cspace->setBounds(*cbounds);

	// define a simple setup class
	oc::SimpleSetup ss(cspace);

	// set the state propagation routine
	ss.setStatePropagator(propagate);

	// set state validity checking for this space
	ss.setStateValidityChecker(
		[&ss](const ob::State *state) { return ::isStateValid(ss.getSpaceInformation().get(), state); });

	// create a start state
	ob::ScopedState<ob::SE2StateSpace> start(space);
	start->setX(-0.5);
	start->setY(0.0);
	start->setYaw(0.0);

	// create a  goal state; use the hard way to set the elements
	ob::ScopedState<ob::SE2StateSpace> goal(space);
	(*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
	(*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.5;
	(*goal)[1]->as<ob::SO2StateSpace::StateType>()->value = 0.0;


	// set the start and goal states
	ss.setStartAndGoalStates(start, goal, 0.05);

	//ss.setPlanner(std::make_shared<oc::PDST>(ss.getSpaceInformation()));
	//ss.getSpaceInformation()->setMinMaxControlDuration(1,100);
	// attempt to solve the problem within one second of planning time
	solved = ss.solve(10.0);

	if (solved)
	{
		std::cout << "Found solution:" << std::endl;
		// print the path to screen

		ss.getSolutionPath().printAsMatrix(std::cout);
	}
	else
		std::cout << "No solution found" << std::endl;

	// setups for drawings
	// bound
	std::vector<double> h = bounds->high;
	std::vector<double> l = bounds->low;
	ofVec3f high, low, vol;
	for (int i = 0; i < h.size(); i++) {
		high[i] = h[i];
		low[i] = l[i];
	}
	vol = high - low;
	boundsbox.set(vol[0], vol[1], vol[2], 1, 1, 1);
	boundsbox.setPosition((high + low) / 2.f);

	if (solved)
	{
		stateAxis.clear();
		polyline.clear();
		for (int i = 0; i < ss.getSolutionPath().getStateCount(); i++) {
			std::vector<double> reals;
			ss.getStateSpace()->copyToReals(reals, ss.getSolutionPath().getState(i));
			/*
			std::cout << reals[0] << " " << reals[1] << " " << reals[2] << " ";
			if (i >= 1 && i <= ss.getSolutionPath().getControlCount()) {
				const double* c = ss.getSolutionPath().getControl(i-1)->as<oc::RealVectorControlSpace::ControlType>()->values;
				double duration = ss.getSolutionPath().getControlDuration(i-1);
				std::cout << c[0] << " " << c[1] << " " << duration;
			}
			else {
				std::cout << 0 << " " << 0 << " " << 0;
			}
			std::cout << std::endl;
			*/
			ofMatrix4x4 mat;
			mat.setTranslation(reals[0], reals[1], 0.f);
			ofQuaternion quat;
			quat.makeRotate(ofRadToDeg(reals[2]), ofVec3f(0.f, 0.f, 1.f));
			mat.setRotate(quat);
			stateAxis.push_back(mat);
			polyline.addVertex(mat.getTranslation());
		}
	}
}


//--------------------------------------------------------------
void ofApp::setup(){
	std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
	planWithSimpleSetup();

	cam.setNearClip(0.001f);
	cam.setTarget(boundsbox);
	cam.setDistance(boundsbox.getSize().length() * 2.f);
}

//--------------------------------------------------------------
void ofApp::update(){

}

//--------------------------------------------------------------
void ofApp::draw(){
	ofSetBackgroundColor(ofColor::black);

	cam.begin();
	ofPushStyle();
	ofSetColor(ofColor::blue);
	ofDrawGridPlane(0.01f);
	ofSetColor(ofColor::grey, 66);
	boundsbox.drawWireframe();
	ofPopStyle();
	if (solved)
	{
		ofPushStyle();
		ofSetColor(ofColor::yellow);
		polyline.draw();
		ofPopStyle();
		for (unsigned int i = 0; i < stateAxis.size(); i++) {
			ofPushMatrix();
			ofMultMatrix(stateAxis[i]);
			ofDrawAxis(boundsbox.getSize().length() / 100.f);
			ofPopMatrix();
		}
	}
	cam.end();
}

//--------------------------------------------------------------
void ofApp::keyPressed(int key){

}

//--------------------------------------------------------------
void ofApp::keyReleased(int key){

}

//--------------------------------------------------------------
void ofApp::mouseMoved(int x, int y ){

}

//--------------------------------------------------------------
void ofApp::mouseDragged(int x, int y, int button){

}

//--------------------------------------------------------------
void ofApp::mousePressed(int x, int y, int button){

}

//--------------------------------------------------------------
void ofApp::mouseReleased(int x, int y, int button){

}

//--------------------------------------------------------------
void ofApp::mouseEntered(int x, int y){

}

//--------------------------------------------------------------
void ofApp::mouseExited(int x, int y){

}

//--------------------------------------------------------------
void ofApp::windowResized(int w, int h){

}

//--------------------------------------------------------------
void ofApp::gotMessage(ofMessage msg){

}

//--------------------------------------------------------------
void ofApp::dragEvent(ofDragInfo dragInfo){ 

}

2次元空間で、Stateは(x, y, yaw)です。回転の単位はラジアン。制御のパラメータは、(前進速度、回転速度、期間)です。
よくわからないまま動かしてみると、こんな結果になります。

なんかよくわからないふらふらした動きしてますね。