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

GTSAMをVisual Studio Code上で使って、実際の動き、というよりデータ(数値)がどのように計算されているのかについて分析してみましたので、ご紹介します。GTSAMがどんなライブラリかというと、GitHubのページに下記のように紹介されています。

GTSAM is a C++ library that implements smoothing and mapping (SAM) in robotics and vision, using Factor Graphs and Bayes Networks as the underlying computing paradigm rather than sparse matrices.

意訳すると、因子グラフ(最適化手法の一種)とベイズネットワーク(有名な確率論ですね)を使って、ロボティクスとビジョンの領域に対してスムージングとマッピング(この頭文字を取ってSAM)の手法を提供するC++ライブラリです。IMUやGPSなど様々なセンサデータにはノイズなども含まれていて、そのまま計算してしまうと、とんでもないことになってしまうので、うまく確率論などを使ってスムーズなデータにしてくれるわけですね。SLAMとかには必要不可欠な処理で、様々なSLAMに使われています。

今回、このGTSAMの動作を観測するにあたって、いろいろと調べてみたところ、とても分かりやすく説明してくれている"GTSAM by Example"というページ(e-book)を見つけたので、その内容に沿って動作確認した結果をご紹介します。

今回は特にSLAMで使われているISAM2について説明します。上記ページで説明してくれているISAM2の例は、特定のランドマークに対する距離ノイズの多い計測結果を使って、2D SLAM問題を解く際に、ISAM2(Incremental Smoothing and Mapping)をどのように使うのかについて解説しています。

なお、Visual Studio CodeとPythonはすでに導入済みの前提で説明させていただきます。また、私の環境はMacのターミナルですが、Windows PowershellやLinuxなど他の環境でも、ほぼ同等のコマンドで実行可能と思われますが、環境ごとに異なる場合もありますので、その点はご了承ください。

まず初めに実行するためのフォルダに移動します。今回の説明では~/Documents/GTSAMフォルダを実行環境とします。

cd ~/Documents/GTSAMあ

実行するためのPython仮想環境を構築します。

python -m .venv
. .venv/bin/activate

上記ページ("GTSAM by Example")で用意されているデータやGTSAMも含めて下記でインストールします。

pip install matplotlib
pip install pandas
pip install plotly
pip install gtbook

~/Documents/GTSAMフォルダをプロジェクトフォルダとしてVisual Studio Codeを起動します。

code .

Visual Studio Code上で実行するPython環境を上記で設定した仮想環境'.venv'に変更します。Visual Studio Codeウィンドウ右下で私の環境では下図のように「'python38': conda」のようにPython環境を表示している箇所をマウスでクリックします。

下図のようにPython環境の候補が表示されるので、上記で設定した仮想環境'.venv'を選択します。

「ISAM2_Example.py」というPythonファイルを~/Documents/GTSAMフォルダに作成して、後記の全ソースコードを入力します。

まず試しに実行してみます。F5キーまたはメニューから[Run]-[Start Debuging]を実行します。下図のように表示された場合は「Python File」を選択してください。

下図のような絵が表示されたら実行成功です。

下記が"GTSAM by Example"で紹介されているISAM2サンプルの全ソースコードです。今回はここまでとして、次回からソースコードの実装について私なりの解釈で解説をしていきます。 

# 使用するライブラリをインポート
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.")

# 測距データの読み込み
#    書式: タイムスタンプ[秒]、送信側/ アンテナ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.")

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

# ノイズパラメータ初期化
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)

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)

# ループ用変数の初期化
i = 1  # ステップカウント
k = 0  # 測距カウント
initialized = False
lastPose = pose0
countK = 0

initializedLandmarks = set()

# オドメトリのループ
for t, relative_pose in odometry.items():
    # オドメトリ要素の追加
    newFactors.add(gtsam.BetweenFactorPose2(i - 1, i, relative_pose,
                                            odoNoise))

    # ポーズを予測して、初期推測値として追加
    predictedPose = lastPose.compose(relative_pose)
    lastPose = predictedPose
    initial.insert(i, predictedPose)

    # 追加すべき距離要素があるかの確認 
    # これらを追加する理由は、2つのオドメトリー観測タイムスタンプの間に受信した各測距値を、
    # 2つのオドメトリ観測タイムスタンプのうち後のオドメトリ観測タイムスタンプに観測されたポーズ変数に関連付けること
    while (k < K) and (triples[k][0] <= t):
        j = triples[k][1]
        landmark_key = gtsam.symbol('L', j)
        _range = triples[k][2]
        newFactors.add(gtsam.RangeFactor2D(
            i, landmark_key, _range, rangeNoise))
        # 受信IDが過去に観測されていない場合は新しいランドマークを初期化
        if landmark_key not in initializedLandmarks:
            p = rng.normal(loc=0, scale=100, size=(2,))
            initial.insert(landmark_key, p)
            print(f"Adding landmark L{j}")
            initializedLandmarks.add(landmark_key)
            # 観測が1件しかなく、ランドマークを完全に確定できないケースのために、
            # ランドマークに非常に緩い事前looseNoiseを加える
            newFactors.add(gtsam.PriorFactorPoint2(
                landmark_key, Point2(0, 0), looseNoise))
        k = k + 1
        countK = countK + 1

    # ISAM2を更新すべきかの確認
    if (k > minK) and (countK > incK):
        if not initialized:  # 最初のmionKはフルに最適化実施
            print(f"Initializing at time {k}")
            batchOptimizer = gtsam.LevenbergMarquardtOptimizer(
                newFactors, initial)
            initial = batchOptimizer.optimize()
            initialized = True

        isam.update(newFactors, initial)
        result = isam.calculateEstimate()
        lastPose = result.atPose2(i)
        newFactors = gtsam.NonlinearFactorGraph()
        initial = gtsam.Values()
        countK = 0

    i += 1

# 最終の最適化を実行して全ての測定値を取り込む(特に最後の値)
finalResult = isam.calculateEstimate()

# 最適化済みのランドマークの表示:
for j in [0,1,5,6]:
    landmark_key = gtsam.symbol('L', j)
    p = finalResult.atPoint2(landmark_key)
    print(f"{landmark_key}: {p}")

# 位置のプロット
poses = gtsam.utilities.allPose2s(finalResult)
landmarks = gtsam.utilities.extractPoint2(finalResult)
positions = np.array([poses.atPose2(key).translation()
                     for key in poses.keys()])
print(positions.shape)

fig = px.scatter(x=positions[:,0],y=positions[:,1])
fig.add_scatter(x=landmarks[:,0], y=landmarks[:,1], mode="markers", showlegend= False)
fig.update_layout(margin=dict(l=0, r=0, t=0, b=0))
fig.update_yaxes(scaleanchor = "x", scaleratio = 1)
fig.show()