Mais conteúdo relacionado Semelhante a NDTスキャンマッチング 第1回3D勉強会@PFN 2018年5月27日 (20) NDTスキャンマッチング 第1回3D勉強会@PFN 2018年5月27日1. 2018.5.27 第1回 3D勉強会 @PFN
「NDTスキャンマッチング」
株式会社マップフォー 代表取締役
名古屋大学 情報科学研究科
橘川 雄樹
yuki.kitsukawa@tier4.jp
Twitter: @YukiKitsukawa
2. 自己紹介
橘川 雄樹(きつかわ ゆうき)
u 名古屋大学大学院 情報科学研究科 博士後期課程3年
u 株式会社マップフォー 代表取締役
研究内容
自動運転システムにおける位置推定、高精度3次元地図
1991年生まれ
2009.3 神奈川県立湘南高等学校 卒業
2014.3 名古屋大学 工学部 電気電子・情報工学科 卒業
2016.3 名古屋大学大学院 情報科学研究科 情報システム学専攻博士前期課程 修了
2016.9 株式会社マップフォー 設立
現在 名古屋大学大学院 情報科学研究科 博士後期課程3年
4. 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
5. 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
7. NDTスキャンマッチング
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を収束するまで繰り返し
評価関数 ! " = − %
&'(
)
* +(", .&)
12. 位置推定 ndt_matching
公式サイト - https://autoware.ai/
ソースコード - https://github.com/CPFL/Autoware
AutowareのNDT
PCL(Point Cloud Library)のNormalDistributionsTransformクラスを用いて実装
最新版 - Ver. 1.7
14. 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;
}
※ コードは分かりやすいように修正してあります
15. プログラムの流れ
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);
16. プログラムの流れ
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;
18. 会社情報
設立 2016年9月
所在地 名古屋大学オープンイノベーション拠点 (OICX)
東京大学アントレプレナープラザ
メンバー 13名(名大、東大、筑波大)
事業内容
1. 走行データ計測、計測システム構築
2. 3次元地図作成業務
3. 3次元データ計測デバイス販売
4. 地図作成サービス運営
https://www.map4.jp/
yuki.kitsukawa@tier4.jp
https://www.facebook.com/map4.jp/
Join Us !!!
自動運転
Autoware
ROS
3次元地図
移動ロボット
SLAM
Gazebo
Unity
センサシミュレーション