実行環境
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
ということでやってみ・・・るのは次回