NDTスキャンマッチング第1回3D勉強会@PFN 2018年5月27日

2018.5.27 第1回 3D勉強会 @PFN
「NDTスキャンマッチング」
株式会社マップフォー代表取締役
名古屋大学情報科学研究科
橘川雄樹
yuki.kitsukawa@tier4.jp
Twitter: @YukiKitsukawa

自己紹介
橘川雄樹（きつかわゆうき）
u 名古屋大学大学院情報科学研究科博士後期課程３年
u 株式会社マップフォー代表取締役
研究内容
自動運転システムにおける位置推定、高精度３次元地図
1991年生まれ
2009.3 神奈川県立湘南高等学校卒業
2014.3 名古屋大学工学部電気電子・情報工学科卒業
2016.3 名古屋大学大学院情報科学研究科情報システム学専攻博士前期課程修了
2016.9 株式会社マップフォー設立
現在名古屋大学大学院情報科学研究科博士後期課程３年

NDTスキャンマッチングのアルゴリズム

NDTとICPの比較
ICP
(Iterative Closest Points)
NDT
(Normal Distributions Transform)
計算量
M: 地図
N: スキャン
O(MN)
(O(N log M) ‒ KD-treeを用いた場合)
地図とスキャンのポイント数に依存
O(N)
地図のポイント数には依存しない
アルゴリズム最近傍点間の2乗和を最小化地図空間を正規分布で近似し、入力スキャンの
対応要素を探索
First proposed A Method for Registration of 3-D Shapes
Paul J. Besl, Neil D, McKay. 1992
The Normal Distributions Transform: A New
Approach to Laser Scan Matching
Peter Biber. 2003

NDTスキャンマッチング
5
NDT スキャンマッチングのアルゴリズム
1. モデルを一定の大きさのセルに分割
2. 各セルの平均・分散を計算
平均
分散
! =
1
$
%
&'(
)
*+
, =
1
$ − 1
%
&'(
)
*+ − ! (*+ − !)0
確率密度関数
(PDF*)
1 * =
1
2
exp −
* − ! 0,6( * − !
2
* Probability Density Function
セル（NDボクセル）
$ : セル内に含まれるポイント数
*&'(,…) : セル内に含まれるポイント
NDT ‒ Normal Distributions transform

6
（tx,ty,tz,α,β,γ）
3. 入力スキャンの各点に対応する要素を求める
4. 評価値を計算
5. ニュートン法により、入力スキャンの座標変換
値を更新
6. 3-5 を収束するまで繰り返し
評価関数 ! " = − %
&'(
)
* +(p, /&)
1 : セル内に含まれるポイント数
2 : 位置・姿勢
/&'(,…) : セル内に含まれるポイント
+(p, /&) : 座標変換後のポイント

7
（tx,ty,tz,α,β,γ）
計算量：スキャンデータに依存（地図データに依存しない）
Takeuchi Eijiro, and Takashi Tsubouchi.
"A 3-D scan matching using improved 3-D normal distributions transform for mobile robotic mapping." Intelligent
Robots and Systems, 2006 IEEE/RSJ International Conference on. IEEE, 2006.
3. 入力スキャンの各点に対応する
要素を求める
4. 評価値を計算
5. ニュートン法により、入力スキャンの座標変換
値を更新
6. 3-5を収束するまで繰り返し
評価関数 ! " = − %
&'(
)
* +(", .&)

NDTスキャンマッチング実装 on Autoware

位置推定 ndt_matching
公式サイト - https://autoware.ai/
ソースコード - https://github.com/CPFL/Autoware
AutowareのNDT
PCL(Point Cloud Library)のNormalDistributionsTransformクラスを用いて実装
最新版 - Ver. 1.7

プログラムの流れ
NDTに変換
初期位置の設定
入力スキャンのダウンサンプリング
地図と入力スキャンのマッチング
推定位置を更新
地図データ読み込み
入力スキャン読み込み
地図データ (PCD) リアルタイムデータ (Lidar)
NDT
Scan Matching

NDTに変換
setInputTarget() [ndt.h]
init() [ndt.h] ‒ ボクセルの初期化
setLeafSize() [voxel_grid.h]
‒ ボクセルサイズの設定
setInputCloud() [pcl_base.hpp]
‒ 地図点群のセット
filter() [voxel_grid_covariance.h]
‒ 各ボクセルの平均・分散を計算
static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
static void map_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
// Convert the data type(from sensor_msgs to pcl).
pcl::fromROSMsg(*input, map);
pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(map));
// Setting point cloud to be aligned to.
// ボクセルサイズの設定
ndt.setResolution(ndt_res);
// 地図登録
ndt.setInputTarget(map_ptr);
// 最大のイテレーション回数の設定
ndt.setMaximumIterations(max_iter);
// 反復計算のステップ幅の設定
ndt.setStepSize(step_size);
// 収束条件の設定
ndt.setTransformationEpsilon(trans_eps);
map_loaded = 1;
}
※ コードは分かりやすいように修正してあります

NDTに変換
static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
pcl::PointCloud<pcl::PointXYZI> scan;
pcl::fromROSMsg(*input, scan);
if(measurement_range != MAX_MEASUREMENT_RANGE){
scan = removePointsByRange(scan, 0, measurement_range);
}
pcl::PointCloud<pcl::PointXYZI>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZI>(scan));
pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>());
sensor_msgs::PointCloud2 filtered_msg;
// if voxel_leaf_size < 0.1 voxel_grid_filter cannot down sample (It is specification in PCL)
if (voxel_leaf_size >= 0.1)
{
// Downsampling the velodyne scan using VoxelGrid filter
pcl::VoxelGrid<pcl::PointXYZI> voxel_grid_filter;
voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
voxel_grid_filter.setInputCloud(scan_ptr);
voxel_grid_filter.filter(*filtered_scan_ptr);
pcl::toROSMsg(*filtered_scan_ptr, filtered_msg);
}
else
{
pcl::toROSMsg(*scan_ptr, filtered_msg);
}
filtered_msg.header = input->header;
filtered_points_pub.publish(filtered_msg);
}
// 入力スキャンの登録
ndt.setInputSource(filtered_scan_ptr);

NDTに変換
predict_pose.x = previous_pose.x + offset_x;
predict_pose.y = previous_pose.y + offset_y;
predict_pose.z = previous_pose.z + offset_z;
predict_pose.roll = previous_pose.roll;
predict_pose.pitch = previous_pose.pitch;
predict_pose.yaw = previous_pose.yaw + offset_yaw;
// Lidarの1スキャン前との差分をオドメトリ/IMUから補正
if (_use_imu == true && _use_odom == true)
imu_odom_calc(current_scan_time);
if (_use_imu == true && _use_odom == false)
imu_calc(current_scan_time);
if (_use_imu == false && _use_odom == true)
odom_calc(current_scan_time);
pose predict_pose_for_ndt;
if (_use_imu == true && _use_odom == true)
predict_pose_for_ndt = predict_pose_imu_odom;
else if (_use_imu == true && _use_odom == false)
predict_pose_for_ndt = predict_pose_imu;
else if (_use_imu == false && _use_odom == true)
predict_pose_for_ndt = predict_pose_odom;
else
predict_pose_for_ndt = predict_pose;
Eigen::Translation3f init_translation(predict_pose_for_ndt.x, predict_pose_for_ndt.y,
predict_pose_for_ndt.z);
Eigen::AngleAxisf init_rotation_x(predict_pose_for_ndt.roll, Eigen::Vector3f::UnitX());
Eigen::AngleAxisf init_rotation_y(predict_pose_for_ndt.pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf init_rotation_z(predict_pose_for_ndt.yaw, Eigen::Vector3f::UnitZ());
// マッチングの初期値
Eigen::Matrix4f init_guess = (init_translation * init_rotation_z * init_rotation_y *
init_rotation_x) * tf_btol;

NDTに変換
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
// マッチング
ndt.align(*output_cloud, init_guess);
// マッチングが収束したかどうか
has_converged = ndt.hasConverged();
// 座標変換（回転＋並進）行列
t = ndt.getFinalTransformation();
// 収束までのイテレーション回数
iteration = ndt.getFinalNumIteration();
// 入力スキャン各ポイントの地図との最近傍点との距離の平均値
fitness_score = ndt.getFitnessScore();
// マッチングの確かさ（評価値）
trans_probability = ndt.getTransformationProbability();
align() [registration.hpp]
computeTransformation() [registration.h] → [ndt.hpp]
while(!converged) solve() [JacobiSVD.h(Eigen)]

会社情報
設立２０１６年９月
所在地名古屋大学オープンイノベーション拠点 (OICX)
東京大学アントレプレナープラザ
メンバー１３名（名大、東大、筑波大）
事業内容
1. 走行データ計測、計測システム構築
2. ３次元地図作成業務
3. ３次元データ計測デバイス販売
4. 地図作成サービス運営
https://www.map4.jp/
yuki.kitsukawa@tier4.jp
https://www.facebook.com/map4.jp/
Join Us !!!
自動運転
Autoware
ROS
３次元地図
移動ロボット
SLAM
Gazebo
Unity
センサシミュレーション

NDTスキャンマッチング第1回3D勉強会@PFN 2018年5月27日

Recommended

Recommended

More Related Content

What's hot

What's hot (20)

Similar to NDTスキャンマッチング第1回3D勉強会@PFN 2018年5月27日

Similar to NDTスキャンマッチング第1回3D勉強会@PFN 2018年5月27日 (20)