はじめに
最近、建図に関する研究を行っている中で、GoogleのオープンソースSLAMフレームワークであるCartographerのソースコードを解析しました。その中には巧妙なアルゴリズム設計が多く、元論文である《Real-time Loop Closure in 2D LIDAR SLAM》の理論説明と合わせて、その核心的な目的であるリアルタイムなループクロージャーを実現する仕組みを理解しました。リアルタイム性を達成するためには、計算の高速化が不可欠で、Cartographerでは空間を犠牲にして時間の節約を図る戦略を採用しています。
確率マップ更新の目的
最新のレーザーポイントを収集し、マップとマッチングを行った後、レーザーポイントを現在のマップに挿入します。これは、グリッドマップの確率を更新する行為です。
確率マップ更新の方式
確率マップ更新の基本的な方法は、以下のような点が挙げられます：
- マップの各グリッドに確率更新を逐次的に適用します。通常はLog(Odds(x))の加減法を用いますが、直接の乗法計算を行う場合もあります。マップが大きい場合、大量の乗法計算が更新の効率を低下させるため、ほとんどのSLAM実装ではLog(Odds(x))の加減法を採用しています。
- Cartographerでは、空間を犠牲にして時間の節約を図る戦略を採用しています。具体的には、マップ更新のための査表を事前に作成し、更新プロセスを査表操作に置き換えます。これにより、加減法すら不要とし、計算時間を大幅に短縮します。
更新プロセスの詳細
更新対象となるマップの点は、レーザーがスキャンした範囲に限定されます。
上記の図を見ると、最初のグリッドの値が700の場合、査表を用いて素早く更新後の値892を取得し、直接最初のグリッドに書き込むことで、確率更新が完了します。この仕組みの核は、更新前の値をインデックスとした更新後の値を格納した表を作成することです。
注：上記の図の値はプロセスの説明を目的としたサンプルであり、実際の計算とは関連ありません。
査表の作成
査表の作成は、LaserFanInserterのコンストラクタ内で行われます：
// laser_fan_inserter.cc
LaserFanInserter::LaserFanInserter(
const proto::LaserFanInserterOptions& options)
: options_(options),
hit_table_(mapping::ComputeLookupTableToApplyOdds(
mapping::Odds(options.hit_probability()))),
miss_table_(mapping::ComputeLookupTableToApplyOdds(
mapping::Odds(options.miss_probability()))) {}
hit_table_とmiss_table_は、probability_values.ccのComputeLookupTableToApplyOdds関数を呼び出して作成されます。レーザーポイントのヒット確率(hit_probability)とミス確率(miss_probability)はLuaスクリプトを介して入力されます：
options.hit_probability() = 0.55; options.miss_probability() = 0.49;
probability_values.hとprobability_values.ccでは、確率値とOdds、整数値の間の変換を実現しています。確率の範囲は[0.1, 0.9]で、対応する整数値の範囲は[1, 32767]です。整数値が0の場合は、マップの状態がunknown（初期灰色）です。
Valueと確率の変換
probability_values.ccでは、Valueから確率への変換を効率的に実現するために、kValueToProbability表を作成します：
// probability_values.cc
float SlowValueToProbability(const uint16 value) {
CHECK_GE(value, 0);
CHECK_LE(value, 32767);
if (value == kUnknownProbabilityValue) {
return kMinProbability;
}
const float kScale = (kMaxProbability - kMinProbability) / 32766.f;
return value * kScale + (kMinProbability - kScale);
}
const std::vector<float>* PrecomputeValueToProbability() {
std::vector<float>* result = new std::vector<float>;
for (int repeat = 0; repeat != 2; ++repeat) {
for (int value = 0; value != 32768; ++value) {
result->push_back(SlowValueToProbability(value));
}
}
return result;
}
const std::vector<float>* const kValueToProbability = PrecomputeValueToProbability();
LookupTableの作成
ComputeLookupTableToApplyOdds関数では、以下のような手順でLookupTableを作成します：
std::vector<uint16> ComputeLookupTableToApplyOdds(const float odds) {
std::vector<uint16> result;
result.push_back(ProbabilityToValue(ProbabilityFromOdds(odds)) + kUpdateMarker);
for (int cell = 1; cell != 32768; ++cell) {
result.push_back(ProbabilityToValue(ProbabilityFromOdds(
odds * Odds((*kValueToProbability)[cell]))) +
kUpdateMarker);
}
return result;
}
注：
- LookupTableの最初の値は、未知領域（値0）の更新後のValueを直接格納します。
- 更新後のValueにはkUpdateMarkerが付加され、このマークは次回の更新前に削除されます。
マップ更新の核心
laser_fan_inserter.ccファイルでは、以下のような更新プロセスが定義されています：
void LaserFanInserter::Insert(const sensor::LaserFan& laser_fan,
ProbabilityGrid* const probability_grid) const {
CHECK_NOTNULL(probability_grid)->StartUpdate();
CastRays(laser_fan, probability_grid->limits(),
[this, &probability_grid](const Eigen::Array2i& hit) {
probability_grid->ApplyLookupTable(hit, hit_table_);
},
[this, &probability_grid](const Eigen::Array2i& miss) {
if (options_.insert_free_space()) {
probability_grid->ApplyLookupTable(miss, miss_table_);
}
});
}
注：
- CastRays関数はHitとMissの格子点を取得し、事前に作成したLookupTableを用いて更新を行います。
- HitとMissの更新は別個に行われ、Missの更新はオプションで有効化/無効化可能です。
LookupTableの適用
probability_grid.hファイルのApplyLookupTable関数は、LookupTableを用いて実際に格子点の値を更新します：
bool ApplyLookupTable(const Eigen::Array2i& xy_index,
const std::vector<uint16>& table) {
DCHECK_EQ(table.size(), mapping::kUpdateMarker);
const int cell_index = GetIndexOfCell(xy_index);
uint16& cell = cells_[cell_index];
if (cell >= mapping::kUpdateMarker) {
return false;
}
update_indices_.push_back(cell_index);
cell = table[cell];
DCHECK_GE(cell, mapping::kUpdateMarker);
UpdateBounds(xy_index);
return true;
}
この仕組みには、まだ詳細な点が含まれています。例えば：
- MapLimitの計算方法
- CastRaysのモデル
- 境界の更新
これらは別途説明が必要です。