SLAMでよく使われるG2Oライブラリを触ってみる

　実行環境

windows 10
python 3.7
open3d 0.10.0
Visual Studio 2017

1．GICP

G2Oのサンプルソース通りにデータをセットしてみます。
Graph Optimizationでは、
ノードとして、2つの視点の回転行列・移動行列、いわゆる自己位置姿勢をセットし、
エッジとして、2つの点群の情報を与えます。

修正を行いたい情報をノード、制約条件の役割をもつ情報をエッジに与えることで、
ノードにセットされたデータの調整が行われます。

今回はノードである自己位置姿勢の修正を行います。
内部では、エッジである2つの点群の距離が最小になるよう修正が行われているものと思われます。

今回は、ウサギの点群を適当にずらして試してみます。
slamを想定するなら、各視点から見たときの点群でやるべき問題ですが、
手頃なデータがない・自己位置姿勢修正の使用感を試すだけなので、
同じ視点の点群の位置をずらすだけです。
前提として以下の通り。

・メインの点群の自己位置姿勢は固定し、対象の点群の自己位置姿勢の調整を行う。
・点の対応関係が既知の前提で行う。
・ライブラリの使用感を試す程度の検証なので、ノイズなどは追加してないが悪しからず。

検証機がwindowsの都合上、以下の流れで処理します。

1.pythonで点群の法線推定・点群情報をCSVで出力
2.C++でCSV読み込み・GICP実行。

pclよりもopen3dのほうが良いという方は、
g2oのpythonのラッパー版がubuntuにあるので、
全部pythonで統一した方が絶対よいと思います。

ソースコードは以下の通りです。

csvoutput.py

import numpy as np
import open3d as o3d
import csv

pcl1 = o3d.io.read_point_cloud("bun000_pcl.ply")
pcl2 = o3d.io.read_point_cloud("bun000_pcl.ply")

trans = np.array([[1,0,0,0.03]
                ,[0,np.cos(20),-1*np.sin(20),0.03],
                  [0,np.sin(20),np.cos(20),0.03],
                  [0,0,0,1]])
pcl2.transform(trans)

o3d.io.write_point_cloud("1bun000_pcl_trans.ply",pcl1)
o3d.io.write_point_cloud("2bun000_pcl_trans.ply",pcl2)

pcl1.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=5, max_nn=5))
pcl2.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=5, max_nn=5))

pcd_tree1 = o3d.geometry.KDTreeFlann(pcl1)
pcd_tree2 = o3d.geometry.KDTreeFlann(pcl2)

mainkey = 0
targetkey = 1
arsplit = str(mainkey) + "-" + str(targetkey)
fponint = open(str(arsplit) + "_Points.csv", 'w')
writer = csv.writer(fponint, lineterminator='\n')

for i in range(200):
    XYZ_W = pcl1.points[i]
    [k, idx, _] = pcd_tree1.search_knn_vector_3d(XYZ_W, 1)
    XYZ_nm = pcl1.normals[idx[0]]

    writer.writerow([mainkey, targetkey, mainkey
                        , XYZ_W.tolist()[0], XYZ_W.tolist()[1], XYZ_W.tolist()[2]
                        , XYZ_nm.tolist()[0], XYZ_nm.tolist()[1], XYZ_nm.tolist()[2]
                        , 1, 0, 0
                        , 0, 1, 0
                        , 0, 0, 1
                        , 0, 0, 0])

    XYZ_W = pcl2.points[i]
    [k, idx, _] = pcd_tree2.search_knn_vector_3d(XYZ_W, 1)
    XYZ_nm = pcl2.normals[idx[0]]

    writer.writerow([mainkey, targetkey, targetkey
                        , XYZ_W.tolist()[0], XYZ_W.tolist()[1], XYZ_W.tolist()[2]
                        , XYZ_nm.tolist()[0], XYZ_nm.tolist()[1], XYZ_nm.tolist()[2]
                                , trans[0][0], trans[0][1], trans[0][2]
                                , trans[1][0], trans[1][1], trans[1][2]
                                , trans[2][0], trans[2][1], trans[2][2]
                                , trans[0][3], trans[1][3], trans[2][3]])

fponint.close()

GICPbyG2o.cpp

# include <stdint.h>

# include <iostream>
# include <random>

# include "g2o/core/block_solver.h"
# include "g2o/core/optimization_algorithm_levenberg.h"
# include "g2o/core/solver.h"
# include "g2o/core/sparse_optimizer.h"
# include "g2o/solvers/dense/linear_solver_dense.h"
# include "g2o/stuff/sampler.h"
# include "g2o/types/icp/types_icp.h"
# include "picojson.h"
# include <sstream>
# include <vector>

// for convenience
using namespace Eigen;
using namespace std;
using namespace g2o;

vector<string> split(string& input, char delimiter)
{
	istringstream stream(input);
	string field;
	vector<string> result;
	while (getline(stream, field, delimiter)) {
		result.push_back(field);
	}
	return result;
}

enum CSV_PointDetail {
	MainKey,
	TargetKey,
	CurrentKey,
	XYZ_W1,
	XYZ_W2,
	XYZ_W3,
	XYZ_nm1,
	XYZ_nm2,
	XYZ_nm3,
	R00,
	R01,
	R02,
	R10,
	R11,
	R12,
	R20,
	R21,
	R22,
	T0,
	T1,
	T2,
};

int main()
{

	double data[21];
	std::ifstream ifs("C:\\git\\g2o\\g2o\\examples\\icp\\0-1_Points.csv");
	std::string str;
	bool flg = true;
	int cnt = 0;
	double Maindata[21];
	double Targetdata[21];

	double euc_noise = 0.01;       // noise in position, m
	  double outlier_ratio = 0.1;

	SparseOptimizer optimizer;
	optimizer.setVerbose(false);

	 //variable-size block solver
	g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
		g2o::make_unique<BlockSolverX>(g2o::make_unique<LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>>()));

	optimizer.setAlgorithm(solver);

	int M_nmIdx = 0;
	int T_nmIdx = 0;
	bool flgSetPose = true;
		while (getline(ifs, str))
		{
			cnt += 1;

			if (cnt == 3) {
				printf("%lf %lf %lf %lf %lf %lf\n",
					Maindata[XYZ_W1],
					Maindata[XYZ_W2],
					Maindata[XYZ_W3],
					Maindata[XYZ_nm1],
					Maindata[XYZ_nm2],
					Maindata[XYZ_nm3]);

				printf("%lf %lf %lf %lf %lf %lf\n",
					Targetdata[XYZ_W1],
					Targetdata[XYZ_W2],
					Targetdata[XYZ_W3],
					Targetdata[XYZ_nm1],
					Targetdata[XYZ_nm2],
					Targetdata[XYZ_nm3]);
				std::cout << " " << std::endl;

				//ノードとしてカメラ姿勢を追加
				if (flgSetPose) {
					int vertex_id = 0;
					 //回転行列・移動行列準備
					Vector3d tM(Maindata[T0], Maindata[T1], Maindata[T2]);
					Vector3d tT(Targetdata[T0], Targetdata[T1], Targetdata[T2]);
					Matrix3d rM;
					Matrix3d rT;

					rM << Maindata[R00], Maindata[R01], Maindata[R02], Maindata[R10], Maindata[R11], Maindata[R12], Maindata[R20], Maindata[R21], Maindata[R22];
					rT << Targetdata[R00], Targetdata[R01], Targetdata[R02], Targetdata[R10], Targetdata[R11], Targetdata[R12], Targetdata[R20], Targetdata[R21], Targetdata[R22];

					Eigen::Isometry3d camM; // camera pose
					Eigen::Isometry3d camT; // camera pose
					camM = rM;
					camM.translation() = tM;
					camT = rT;
					camT.translation() = tT;

					 //ノードの追加
					VertexSE3 *vcM = new VertexSE3();
					vcM->setEstimate(camM);
					vcM->setId(0);
					vcM->setFixed(true);
					optimizer.addVertex(vcM);

					VertexSE3 *vcT = new VertexSE3();
					vcT->setEstimate(camT);
					vcT->setId(1);
					//vcT->setFixed(true);
					optimizer.addVertex(vcT);

					flgSetPose = false;
				}

				 //点群のポイントの追加
				{
					 //get two poses
					VertexSE3* vpM =
						dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second);
					VertexSE3* vpT =
						dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);

					 //calculate the relative 3D position of the point
					Vector3d ptM(Maindata[XYZ_W1], Maindata[XYZ_W2], Maindata[XYZ_W3]);
					Vector3d ptT(Targetdata[XYZ_W1], Targetdata[XYZ_W2], Targetdata[XYZ_W3]);

					Vector3d nmM(Maindata[XYZ_nm1], Maindata[XYZ_nm2], Maindata[XYZ_nm3]);
					Vector3d nmT(Targetdata[XYZ_nm1], Targetdata[XYZ_nm2], Targetdata[XYZ_nm3]);
					nmM.normalize();
					nmT.normalize();


					Edge_V_V_GICP * e = new Edge_V_V_GICP();

					e->setVertex(0, vpM);      // first viewpoint
					e->setVertex(1, vpT);      // second viewpoint

					EdgeGICP meas;
					meas.pos0 = ptM;
					meas.pos1 = ptT;
					meas.normal0 = nmM;
					meas.normal1 = nmT;

					e->setMeasurement(meas);

					meas = e->measurement();
					e->information() = meas.prec0(0.01);

					optimizer.addEdge(e);
				}
				cnt = 1;
			}

			if (cnt == 1) {
				M_nmIdx += 1;
				sscanf(str.c_str(), "%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf",
					&Maindata[MainKey],
					&Maindata[TargetKey],
					&Maindata[CurrentKey],
					&Maindata[XYZ_W1],
					&Maindata[XYZ_W2],
					&Maindata[XYZ_W3],
					&Maindata[XYZ_nm1],
					&Maindata[XYZ_nm2],
					&Maindata[XYZ_nm3],
					&Maindata[R00],
					&Maindata[R01],
					&Maindata[R02],
					&Maindata[R10],
					&Maindata[R11],
					&Maindata[R12],
					&Maindata[R20],
					&Maindata[R21],
					&Maindata[R22],
					&Maindata[T0],
					&Maindata[T1],
					&Maindata[T2]);
			}

			if (cnt == 2) {
				T_nmIdx += 1;
				sscanf(str.c_str(), "%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf",
					&Targetdata[MainKey],
					&Targetdata[TargetKey],
					&Targetdata[CurrentKey],
					&Targetdata[XYZ_W1],
					&Targetdata[XYZ_W2],
					&Targetdata[XYZ_W3],
					&Targetdata[XYZ_nm1],
					&Targetdata[XYZ_nm2],
					&Targetdata[XYZ_nm3],
					&Targetdata[R00],
					&Targetdata[R01],
					&Targetdata[R02],
					&Targetdata[R10],
					&Targetdata[R11],
					&Targetdata[R12],
					&Targetdata[R20],
					&Targetdata[R21],
					&Targetdata[R22],
					&Targetdata[T0],
					&Targetdata[T1],
					&Targetdata[T2]);


			}
		}


		optimizer.initializeOptimization();
		optimizer.computeActiveErrors();
		cout << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl;

		optimizer.setVerbose(true);

		optimizer.optimize(10);

		cout << endl << "Second vertex should be near 0,0,1" << endl;
		cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second)
			->estimate().translation().transpose() << endl;
		cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second)
			->estimate().rotation() << endl;

		cout << "B" << endl;
		cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second)
			->estimate().translation().transpose() << endl;
		cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second)
			->estimate().rotation() << endl;
		cout << "B" << endl;

	return 0;

実行結果は以下の通り。
出力結果をろくに調整してませんでした。

iteration= 0     chi2= 0.172924  time= 0.0169834         cumTime= 0.0169834      edges= 199      schur= 0        lambda= 0.000668        levenbergIter= 1
iteration= 1     chi2= 0.005286  time= 0.0093364         cumTime= 0.0263198      edges= 199      schur= 0        lambda= 0.000223        levenbergIter= 1
iteration= 2     chi2= 0.002247  time= 0.0101216         cumTime= 0.0364414      edges= 199      schur= 0        lambda= 0.000148        levenbergIter= 1
iteration= 3     chi2= 0.001901  time= 0.0106957         cumTime= 0.0471371      edges= 199      schur= 0        lambda= 0.000099        levenbergIter= 1
iteration= 4     chi2= 0.000204  time= 0.0119287         cumTime= 0.0590658      edges= 199      schur= 0        lambda= 0.000066        levenbergIter= 1
iteration= 5     chi2= 0.000000  time= 0.0115788         cumTime= 0.0706446      edges= 199      schur= 0        lambda= 0.000044        levenbergIter= 1
iteration= 6     chi2= 0.000000  time= 0.0117871         cumTime= 0.0824317      edges= 199      schur= 0        lambda= 0.000029        levenbergIter= 1
iteration= 7     chi2= 0.000000  time= 0.0093594         cumTime= 0.0917911      edges= 199      schur= 0        lambda= 0.000020        levenbergIter= 1
iteration= 8     chi2= 0.000000  time= 0.00829   cumTime= 0.100081       edges= 199      schur= 0        lambda= 0.000013        levenbergIter= 1
iteration= 9     chi2= 0.000000  time= 0.0082915         cumTime= 0.108373       edges= 199      schur= 0        lambda= 0.000009        levenbergIter= 1

# 　メインの点群(vcM->setFixed(true)により修正は行われない)
translation : 
0 0 0
rotation : 
1 0 0
0 1 0
0 0 1

# 　位置合わせ対象の点群
translation :
-0.03    -0.0396308     0.0151459
rotation :
           1  -3.04486e-12  -3.02239e-13
 1.51848e-12      0.408082      0.912945
-2.65645e-12     -0.912945      0.408082

出力された回転行列・移動行列を、
open3dのtransform関数で適用させてみます。

位置が合ってますね。
ノードとエッジのセットの仕方がなんとなくわかった気がします。
法線推定の精度も関わってきそうなので、なかなか大変そうです。

2．Bundle Adjust バンドル調整

バンドル調整もやってみます。

ある画像Ａより推定された三次元座標を画像平面にプロジェクションしたものと、
同じ三次元座標が投影されている画像Ｂの特徴点の座標。
これら2つは本来同じものであるはずですが、
特徴点の誤差、外部・内部パラメータの誤差により、
微妙に違った結果になります。

このときの二乗誤差が最小となるように、パラメータの調整を行うのがバンドル調整。
上記画像の例では、パラメータとして、射影行列と三次元座標の調整を行う想定です。

graph optimizationでのバンドル調整では、
ノードとして、三次元座標と各始点の回転行列・移動行列、
制約条件として射影した画像平面座標を与えて、調整を行います。

以下、ビルドした時のメモ。

[環境]
WIndows10
cmake 3.18.0-r2
Visual Studio 2017

[Eigen 3.3.4] 
http://bitbucket.org/eigen/eigen/get/3.3.4.tar.gz
ダウンロードするだけ。

[suitesparse 5.4.0]
https://github.com/jlblancoc/suitesparse-metis-for-windows
手順に従って必要なファイルを配置。cmakeでビルド。
Hunterなどの設定はとくにせず、lapackの要求も無視。

[g2o]
https://github.com/RainerKuemmerle/g2o
cmakeでビルド。ライブラリのパスを設定。私の環境では下記パスを指定する。
うまくいかないときは、cmake_module配下のFindxxxx.cmakeの中身を調べてパスを正しくセット。

Eigen3_DIR　         -  D:/git/eigen-334/Eigen
EIGEN3_INCLUDE_DIR   -  D:/git/eigen-334
CHOLMOD_INCLUDE_DIR　-  D:/git/suitesparse-metis-for-windows/SuiteSparse/CHOLMOD/Include
CHOLMOD_LIBRARY　    -  D:/git/suitesparse-metis-for-windows/build/lib/Release/libcholmod.lib

ということでやってみ・・・るのは次回