embeddedなブログ

組み込みシステムに関することや趣味に関することをダラダラと書いていきます(^^)

GTSAMをVisual Studio Code上で使ってみる(その2)

2023-11-05 21:59:10 | Windows Embedded Standard

GTSAMをVisual Studio Code上で使って、実際の動き、というよりデータ(数値)がどのように計算されているのかについてとても分かりやすく説明してくれている"GTSAM by Example"というページ(e-book)に沿って解説させていただきます。

前回は、ひとまずソースコードを入力して実行してみました。今回は上記ページ("GTSAM by Example")に従って、ソースコードに沿って私なりの解釈で解析させていただきます。

# 使用するライブラリをインポート
from gtsam import Point2, Pose2
import plotly.express as px
import numpy as np
import gtsam
import math

import matplotlib.pyplot as plt
from gtsam.utils import plot
from numpy.random import default_rng

rng = default_rng()

NM = gtsam.noiseModel # ガウス分布などの分布モデルがライブラリに含まれています

ガウス分布のノイズモデルを生成します。

# オドメトリデータの読み込み
# 推測航法(Dead-Reckoning)オドメトリ入力
#  書式: タイムスタンプ[秒]、移動距離差分[m]、方位変化差分[rad]

odometry = {}
data_file = gtsam.findExampleDataFile("Plaza2_DR.txt")

for row in np.loadtxt(data_file):
    t, distance_traveled, delta_heading = row
    odometry[t] = Pose2(distance_traveled, 0, delta_heading)
    
M = len(odometry)
print(f"Read {M} odometry entries.")

データセットを読み込みます。推測航法(Dead-Reckoning)オドメトリ入力のデータセットで、タイムスタンプ[秒]、移動距離差分[m]、方位変化差分[rad]が格納されています。読み込んだ各データは、タイムスタンプをキーとしてodometryというDictionaryタイプの変数に格納します。

今回引用したISAM2の例では、ウルトラワイドバンド(UWB)測距時のデータセットを使っています。このデータセットには、オドメトリデータ（移動距離と連続する各ステップ間の回転差）と、固定ランドマークまでの測距データの両方が含まれています。このデータセットはは無線ベースの測距システムを搭載した芝刈りロボットを用いて取得されたそうです。固定された無線ノードがランドマークとして使用され、それらの無線ノードは、芝刈りロボットに搭載された無線ノードに対して、測定距離と対応するユニークなIDを送信するので、データの関連付けが保証されているとのことです。

ここまでの実行で下記が出力されます。4096個のデータがodometryに格納されました。

Read 4090 odometry entries.

odometryに格納された値をVisual Studio Codeウォッチで確認すると下記のようになっています。

odometryの最初の要素を見てみると、キー、つまりタイムスタンプは　3152.099993944168[秒]、オドメトリでの移動距離は (0.000641521, 0)[m] 、回転差は -0.000673081[rad]ですね。タイムスタンプの間隔からodometryはおおよそ100ミリ秒間隔(10Hz)でサンプリングされていることが分かります。

# 測距データの読み込み
#  書式: タイムスタンプ[秒]、送信側/ アンテナID ; 受信側/ ノードID ; 距離[m]

triples = []
data_file = gtsam.findExampleDataFile("Plaza2_TD.txt")

for row in np.loadtxt(data_file):
    t, sender, receiver, _range = row
    triples.append((t, int(receiver), _range))
    
K = len(triples)
print(f"Read {K} range triples.")

次に測距データを読み込みます。測距データはオドメトリ入力と同時に読み込まれるのではなく、測距データは測距データだけで測定時のデータとタイムスタンプを記録しています。測距データのデータ数も当然オドメトリデータとは異なります。測距用のRFノードはロボットと固定のランドマークに設置されていて、ロボット側ノードが信号を送信して、測距値とそれぞれ応答したノード(ランドマーク)が自身のユニークなIDを返すので、ロボットがそれを受信します。

読み込んだデータは、タイムスタンプ[秒]、送信ノードID(ロボット)、受信ノードID(ランドマーク)、測距値[m]で、タイムスタンプと受信ノードID、測距値をtriplesというDictionaryタイプの変数に格納します。格納時にキーは設定せず順番にアペンドしています。

ここまでの実行で下記が出力されます。1816個のデータがtriplesに格納されました。

Read 1816 range triples.

triplesに格納された値をVisual Studio Codeウォッチで確認すると下記のようになっています。

オドメトリのタイムスタンプ間隔はおよそ100ミリ秒、それに対して測距データはおよそ200ミリ秒(結構ばらつきがありますがおおよそ5Hz)なので、オドメトリの方がサンプル周波数が2倍ほど速いようです。データ数もオドメトリが測距データの2倍少しなので、そんな感じですね。

# パラメータ
minK = 150  # 最初に処理をするための測距データ最小値
incK = 25  # 1回のISAM更新を処理するための新しい測距データ最小値
robust = True

SLAMを実施するためのパラメータを初期化します。ISAM更新には変数の再順序付けと再リニア化の2つの重要なステップがあります。これらの処理は計算負荷が高いため、毎回ISAM更新を実行せず、新しい測定データ数が十分な数に達したときに(上記で宣言されているincKという変数で管理)ISAM更新を実行します。

# ノイズパラメータ初期化
priorSigmas = gtsam.Point3(1, 1, math.pi) # ポーズ用[σ_x, σ_y, σ_角度]
odoSigmas = gtsam.Point3(0.05, 0.01, 0.1) # オドメトリ用[σ_x, σ_y, σ_角度]
sigmaR = 100        # 標準偏差の範囲

priorNoise = NM.Diagonal.Sigmas(priorSigmas)  # 標準偏差σを指定して対角ノイズモデルを生成
looseNoise = NM.Isotropic.Sigma(2, 1000)     # 標準偏差σを指定して等方性ノイズモデルを生成
odoNoise = NM.Diagonal.Sigmas(odoSigmas)     # オドメトリ
gaussian = NM.Isotropic.Sigma(1, sigmaR)     # 非ロバスト
tukey = NM.Robust.Create(NM.mEstimator.Tukey.Create(15), gaussian)  # ロバスト
rangeNoise = tukey if robust else gaussian

ノイズ関連のパラメータを初期化します。

# ISAMの初期化
isam = gtsam.ISAM2()
print(isam)

ISAMのインスタンスを生成します。

: cliques: 0, variables: 0

ここまでの実行でISAMのインスタンスの状態が表示されます。

pose0 = Pose2(-34.2086489999201, 45.3007639991120, math.pi - 2.021089)
newFactors = gtsam.NonlinearFactorGraph()
newFactors.addPriorPose2(0, pose0, priorNoise)
initial = gtsam.Values()
initial.insert(0, pose0)
print(newFactors, initial)

今回のISAMの実装例では、ISAMのアップデート間に、NonlinearFactorGraphオブジェクトnewFactors変数で新しいファクターグラフ(制約条件)を追跡します。新しいファクターには測距データまたはオドメトリデータの測定値と関連付けられます。ポーズの初期値(初期位置)はpose0として位置は-34.2086489999201, 45.3007639991120で向きは1.1205[rad](だいたい64°)を設定します。initialという変数は、GTSAMの値を格納するためのオブジェクトです。具体的には、推定の初期値や結果を格納し、後段の処理で使います。このinitialは最初にポーズ初期値であるpose0を設定します。ポーズのinitial値は初期の推定を行うために利用され、後でISAM2アルゴリズムによって更新されます。

NonlinearFactorGraph: size: 1

Factor 0: PriorFactor on 0
  prior mean:  (-34.2086, 45.3008, 1.1205)
  noise model: diagonal sigmas [1; 1; 3.14159265];

 Values with 1 values:
Value 0: (gtsam::Pose2)
(-34.208649, 45.300764, 1.12050365)

ここまでの実行で新しいファクターnewFactorsとinitialの初期値が表示されます。

今回の解説はここまでとさせていただきます。次回で続くソースコードについて解説をさせていただきます。

 


最新の画像［もっと見る］