コードの解説の続きです。
下記のコードでは、オドメトリとレンジ測定を処理するループが設定されています。オドメトリデータと測距データが組み合わせられ、newFactorsとinitialの値が更新されます。また、ISAM2のアップデートが行われ、推定結果が計算されます。
# ループ用変数の初期化
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()
上記のコードで最適化のためのメインループが実行されます。オドメトリ測定データをループでイテレートして、SLAM問題のグラフにオドメトリ制約条件を追加します。
ここで、制約条件(ファクタ)についてもう少し詳細にその意味を説明します。SLAM問題において、システムの状態や変数間の関係を表現するための数学的な制約が制約条件です。SLAM問題では、ロボットの軌跡(ポーズ)と周囲の環境に存在するランドマークの位置を同時に推定する必要があります。これを実現するために、制約条件は以下のような情報を提供します。
オドメトリ制約: ロボットの移動履歴、つまりオドメトリデータから得られる情報を表す制約です。これは、ロボットがどのように移動したかを示し、連続するポーズ間の関係を記述します。
レンジ制約: 測距データからランドマークとの距離を測定する情報を表す制約です。これにより、ロボットとランドマークの位置の関係が定義されます。
このループでは、2回分のISAMアップデート間で生成される新しいポーズpredictedPoseを推定し、その値でinitial値を更新します。レンジ制約としては、現在のポーズと過去の非関連測定値に関連するレンジ制約をグラフに追加します。さらに、過去に観測されたことのない新しい受信IDが最初に観測されたときには新しいランドマークとして初期化します。
最後に、新しい測距データ数countKが十分な数incKに達したときにISAMアップデートが実行されます。
ここまで解析した内容をVisualStudio Codeで追いかけてみます。
# ループ用変数の初期化
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))
relative_poseにはodometryの測定データがイテレートされるので、最初にここを実行する際にはodometryの最初の値が入っています。newFactorsは初期化時にpose0、つまりポーズの初期が設定されているので、i=0が初期値、i=1が下記の値としてオドメトリ制約条件BetweenFactorPose2としてnewFactorsに追加されることになります。
# ポーズを予測して、初期推測値として追加
predictedPose = lastPose.compose(relative_pose)
lastPoseには下記のようにpose0の値が設定されています。
上記コードを実行するとpredictedPoseは下記のように設定されています。lastPose(pose0)に対してrelative_poseの移動を反映した結果です。
lastPose = predictedPose
initial.insert(i, predictedPose)
initialに上記の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))
初回のjはtriplesの最初の受信IDなので1となっており、生成されたlandmark_keyは下記となっていました。
同様にtriplesの最初の受信ID=1に対する測距データである下記が_rangeに入ります。
# 受信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)
initializedLandmarksに過去に観測された受信IDが登録されているので、initializedLandmarksに登録されていない、つまり、過去に観測されていない受信IDの場合はinitialに追加します。初回に受信ID1のデータが登録された後のinitializedLandmarksは下記となっています。
新たに追加したランドマークに緩い事前ノイズを追加します。
# 観測が1件しかなく、ランドマークを完全に確定できないケースのために、
# ランドマークに非常に緩い事前looseNoiseを加える
newFactors.add(gtsam.PriorFactorPoint2(
landmark_key, Point2(0, 0), looseNoise))
k = k + 1
countK = countK + 1
解析全体で最低限minK(150)より多い個数で、前回更新からincK(25)より多い個数のランドマークが追加されていればISAM2を更新します。
# ISAM2を更新すべきかの確認
if (k > minK) and (countK > incK):
if not initialized: # 最初のminKはフルに最適化実施
print(f"Initializing at time {k}")
batchOptimizer = gtsam.LevenbergMarquardtOptimizer(
newFactors, initial)
initial = batchOptimizer.optimize()
initialized = True
最初にここに入って来た時のkは151でした。初回のみinitialを最適化して、最適化完了を示す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()
ここで要となるISAM2のupdate関数について見てみます。update関数の定義は下記の通りです。
void update(const NonlinearFactorGraph& newFactors, const Values& newTheta = Values());
この関数の主な引数は次のとおりです。
newFactors: 新しい制約や観測を含むNonlinearFactorGraphオブジェクトです。これはISAM2に新しい情報を提供するためのものであり、最適化の対象となります。
newTheta: （オプション）新しい情報の影響を制御するためのパラメータです。Valuesオブジェクトとして提供され、デフォルトでは空の値が使われます。これを適切に設定することで、新しい情報が既存の最適化結果に与える影響を調整できます。
新たな制約条件として引数newFactorsにnewFactorsを指定、引数newThetaにinitialを指定しています。更新の結果として最適化されたnewFactorsとinitialの値を取得します。
Adding landmark L1 Adding landmark L6 Adding landmark L0 Adding landmark L5 Initializing at time 151
実行結果は上記の通りです。ランドマークとしてはL1、L6、L0、L5の4つだけが追加されており、初期化はk=151のタイミングで実行されています。
# 最適化済みのランドマークの表示:
for j in [0,1,5,6]:
landmark_key = gtsam.symbol('L', j)
p = finalResult.atPoint2(landmark_key)
print(f"{landmark_key}: {p}")
上記コードを実行すると、ISAM2が推定したランドマークのIDとその位置が抽出され、結果が下記のように出力されます。上からL0, L1, L5, L6ですね。
5476377146882523136: [-35.97427664 26.3162765 ] 5476377146882523137: [-75.10179088 21.01401519] 5476377146882523141: [ -1.03850677 -12.13769491] 5476377146882523142: [-36.08617621 72.34978651]
続いて軌跡情報です。
# 位置のプロット
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)
解上記が実行されると、ISAM2で推定されたランドマークが landmarks、軌跡が positions という変数に抽出され、その positions の次元数が下記のように表示されます。
(4090, 2)
いよいよ、最後のソースです。
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()
上記コードを実行すると、下図のように全ての推定されたランドマークと軌跡が描画されます。
水色の線がSLAMが推測した軌跡、赤い点がランドマークです。どちらもマウスカーソルを上に持っていくと、その位置の座標が表示されます。赤い点の上にマウスカーソルを持ってきてランドマークL0, L1, L5, L6の座標と比較してみてください。一番上の赤い点がL6ですね。下記部分がランドマークを表示している部分です。
fig.add_scatter(x=landmarks[:,0], y=landmarks[:,1], mode="markers", showlegend= False)
以上でGTSAMをVisual Studio Codeで実行する例に関する解説は終わりとさせていただきます。
