EurekaMoments

This is my studying logs about Autonomous driving, Machine learning technologies and etc.

書籍「SLAM入門」の読書録~SLAM起動部分編~

SLAM入門: ロボットの自己位置推定と地図構築の技術

SLAM入門: ロボットの自己位置推定と地図構築の技術

  • 作者:友納 正裕
  • 発売日: 2018/03/03
  • メディア: 単行本(ソフトカバー)

f:id:sy4310:20200627213415p:plain

目次

目的

  • 書籍「SLAM入門」を参考に、SLAMシステムを起動するソフトウェア全体の設計について理解する。

SLAM実行を統括するクラス: SlamLauncher

この書籍のプログラムでは、入力データの読み込みから各種処理の実行、結果の描画を全てこのSlamLauncherクラスに集約させている。

クラス設計

f:id:sy4310:20200627213335p:plain

SLAM全体を複数の処理に分割して、下記のクラスに割り振っている。

  • SensorDataReader
  • PointCloudMap
  • SlamFrontEnd
  • MapDrawer
  • FrameworkCustomizer

また、ロボットの位置・姿勢、スキャンデータ、良く使う関数などを下記の構造体で定義している。これらは、各クラス間で共通で使いまわされる。

  • Pose2D
  • Scan2D
  • LPoint2D
  • MyUtil

MyUtilクラス

角度の正規化や逆行列計算、行列の固有値・固有ベクトル計算の関数をまとめたもの。

f:id:sy4310:20200628115045p:plain

Pose2Dクラス

ロボットの位置・姿勢・回転行列に関する処理をまとめたもの。この位置と姿勢に基づいたスキャン点の座標変換も担う。

f:id:sy4310:20200628115516p:plain

LPoint2Dクラス

スキャン点一つ当たりの位置座標や法線ベクトルに関する処理をまとめたもの。

f:id:sy4310:20200628115938p:plain

Scan2Dクラス

スキャンデータ点群と、それらをスキャンした時のロボット位置・姿勢角を持つ。

f:id:sy4310:20200628121252p:plain

SensorDataReaderクラス

データファイルの読み込み、スキャンデータのロードなどの処理を担う。

f:id:sy4310:20200628145612p:plain

PointCloudMapクラス

スキャン点群を用いた地図構築、構築した地図の座標変換、一括修正などを担う。

f:id:sy4310:20200628150257p:plain

SlamFrontEndクラス

フロントエンドに該当する処理を担う。バックエンドのクラスまでが集約されているに少し違和感を感じが、こういうものなのだろうか。

f:id:sy4310:20200628153329p:plain

MapDrawerクラス

構築した地図、スキャンデータ、ロボットの移動軌跡などを描画する処理を担う。

f:id:sy4310:20200628155203p:plain

FrameworkCustomizerクラス

このプログラムでは、フレームワークという、継承と仮想関数を利用して効率よくプログラムを開発する仕組みが採用されている。このクラスは、システムの目的や条件に応じてフレームワークを事前にカスタマイズする処理を担っている。

f:id:sy4310:20200628155954p:plain

メソッド設計

コンストラクタ・デストラクタ

C++ではメンバ変数をこんなふうに初期化出来るらしい。便利。

f:id:sy4310:20200628160843p:plain

setStartN

何番目のデータから読み込み始めるかを指定する。データ読み込みに関するメソッドだから、SensorDataReaderクラスに持たせて、それを呼び出す方が責務の分担が分かりやすい気がする。

f:id:sy4310:20200628162707p:plain

setOdometryOnly

オドメトリ情報のみで地図構築するかどうかを指定する。

f:id:sy4310:20200628163015p:plain

run

SLAMを実行する。データファイルを読み込み、ファイルの終わりに到達するまでSLAM実行、結果の描画などを行う。

f:id:sy4310:20200628164018p:plain

SLAM実行部分は、フロントエンドクラスからメソッドを呼び出す形で処理される。

f:id:sy4310:20200628164219p:plain

showScans

スキャンデータのみを描画する時に呼び出される。

f:id:sy4310:20200628164922p:plain

mapByOdometry

オドメトリに基づいて地図構築を行う。入力引数にはスキャンデータがポインタ渡しされる。参照渡しとの違いが良く分かっていなかったが、参照渡しでは無効値を表す記法がないらしい。つまり無効値かどうかのチェックが出来ない。スキャンデータは常に得られる訳ではないので、nullチェックが出来るポインタ渡しの方が適していると考えられる。

f:id:sy4310:20200628171454p:plain

setFilename

SensorDataReaderクラスのメソッドを呼び出して、データファイルを読み込む。

f:id:sy4310:20200628171700p:plain

skipData

指定したところまでスキャンデータを読み飛ばす。

f:id:sy4310:20200628172002p:plain

customizeFramework

FrameworkCustomizerクラスのメソッドを呼び出して、フレームワークのカスタマイズを行う。

f:id:sy4310:20200628172529p:plain

参考記事

qiita.com

cocodrips.hateblo.jp

GitHub

記載されている各UMLダイアグラムは、全てこちらのGitHubで公開済み。

github.com