二次元レーザー SLAM をゼロから構築 --- g2o ベースのバックエンド最適化に基づくコード実装

1 g2o の紹介

g2o (General Graphic Optimization) は、グラフ最適化に基づくライブラリです。

グラフ最適化は、最適化問題をグラフとして表現する方法です。グラフは、いくつかの頂点 (Vertex) と、これらの頂点を接続するエッジ (Edge) で構成されます。最適化変数は頂点で表され、誤差項はエッジで表されます。

インターネット上にはもっと具体的な紹介がたくさんあるので、ここでは詳しく説明しません。

g2oについての解説記事をいくつか紹介しますが、とても良いです。

SLAM をゼロから一緒に学ぶ | グラフの最適化を理解し、g2o コードを段階的に理解できるようにします
https://blog.csdn.net/electech6/article/details/86534426
SLAM をゼロから一緒に学ぶ | g2o 頂点プログラミング ルーチンをマスターする
https: / /blog.csdn.net/electech6/article/details/88018481
SLAMをゼロから一緒に学ぶ | g2oのコードルーチンをマスター
https://zhuanlan.zhihu.com/p/58521241
SLAM-G2Oの本質を分析
https:/ / www.guyuehome.com/34679

2 g2oに基づくバックエンド最適化のコード解説

kartoでのポーズグラフのノードとエッジの説明は前回の記事で紹介しましたので、興味のある方はそちらもご覧ください。

ゼロから 2 次元レーザー SLAM を構築 — Karto バックエンドのポーズ グラフに関連するデータ構造分析
https://blog.csdn.net/tiancailx/article/details/121317039

2.1 ヘッダーファイル

まず、最適化インターフェース ScanSolver を継承した G2oSolver クラスを karto で宣言します。

class G2oSolver : public karto::ScanSolver
{
    
    
public:
  G2oSolver();
  virtual ~G2oSolver();

public:
  virtual void Clear();
  virtual void Compute();
  virtual const karto::ScanSolver::IdPoseVector &GetCorrections() const;

  virtual void AddNode(karto::Vertex<karto::LocalizedRangeScan> *pVertex);
  virtual void AddConstraint(karto::Edge<karto::LocalizedRangeScan> *pEdge);

  void publishGraphVisualization(visualization_msgs::MarkerArray &marray);

private:
  karto::ScanSolver::IdPoseVector mCorrections;
  g2o::SparseOptimizer mOptimizer;
};

2.2 オプティマイザの初期化

g2o の初期化は基本的にこの 4 つのステップで構成されています.上記の最初の記事ですでに非常に明確に説明されているので、これ以上は説明しません.オプティマイザの初期化をコンストラクタに入れます, LinearSolverCSparse 線形ソルバーを使用します, LM 降下アルゴリズムを使用し
ます.

typedef g2o::BlockSolver<g2o::BlockSolverTraits<-1, -1>> SlamBlockSolver;
typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;

G2oSolver::G2oSolver()
{
    
    
  // 第1步：创建一个线性求解器LinearSolver
  SlamLinearSolver *linearSolver = new SlamLinearSolver();
  linearSolver->setBlockOrdering(false);
  // 第2步：创建BlockSolver。并用上面定义的线性求解器初始化
  SlamBlockSolver *blockSolver = new SlamBlockSolver(linearSolver);
  // 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
  g2o::OptimizationAlgorithmLevenberg *solver =
      new g2o::OptimizationAlgorithmLevenberg(blockSolver);
  // 第4步：创建稀疏优化器（SparseOptimizer）
  mOptimizer.setAlgorithm(solver);
}

2.3 ポーズグラフへのノードの追加

karto の頂点のデータ構造は karto::Vertexkarto::LocalizedRangeScan であり、g2o で定義されたデータ構造 VertexSE2 に変換する必要があります。

ソースコードによると、
VertexSE2 : public BaseVertex<3, SE2> //2D pose Vertex, (x,y,theta)

VertexSE2 は、ポーズの頂点 (x、y、シータ) を 2D で表します。

ここのコードは非常に明確です。つまり、ノード マッチング後のポーズが取得され、g2o::SE2 形式に変換され、poseVertex->setEstimate() 関数に渡されます。

そして、このノードの ID を設定します。

ノードを設定したら、このノードをオプティマイザーに入れます。

void G2oSolver::AddNode(karto::Vertex<karto::LocalizedRangeScan> *pVertex)
{
    
    
  karto::Pose2 odom = pVertex->GetObject()->GetCorrectedPose();
  g2o::VertexSE2 *poseVertex = new g2o::VertexSE2;
  poseVertex->setEstimate(g2o::SE2(odom.GetX(), odom.GetY(), odom.GetHeading()));
  poseVertex->setId(pVertex->GetObject()->GetUniqueId());
  mOptimizer.addVertex(poseVertex);
  ROS_DEBUG("[g2o] Adding node %d.", pVertex->GetObject()->GetUniqueId());
}

2.4 ポーズグラフへのエッジ (制約) の追加

最初に、EdgeSE2 形式でエッジを宣言してから、このエッジにデータを追加します。

最初に、2 つのノードに対応する ID 番号をエッジに追加します。

次に、これら 2 つのポーズ間のポーズ変換が観測として g2o::SE2 測定() 関数に追加されます。

通常、2 つのノードの姿勢は観測値と同じであり、最適化には影響しませんが、ループが検出された後、ノードの姿勢が変化し、2 つの姿勢によって計算された位置が変化します。ノード 姿勢変換はこの観測とは異なり、最適化に影響します。

次に、kartoz の共分散行列を補助情報行列に追加します。

最後に、設定されたエッジがポーズ グラフに追加されます。

void G2oSolver::AddConstraint(karto::Edge<karto::LocalizedRangeScan> *pEdge)
{
    
    
  // Create a new edge
  g2o::EdgeSE2 *odometry = new g2o::EdgeSE2;

  // Set source and target
  int sourceID = pEdge->GetSource()->GetObject()->GetUniqueId();
  int targetID = pEdge->GetTarget()->GetObject()->GetUniqueId();
  odometry->vertices()[0] = mOptimizer.vertex(sourceID);
  odometry->vertices()[1] = mOptimizer.vertex(targetID);

  // Set the measurement (odometry distance between vertices)
  karto::LinkInfo *pLinkInfo = (karto::LinkInfo *)(pEdge->GetLabel());
  karto::Pose2 diff = pLinkInfo->GetPoseDifference();
  g2o::SE2 measurement(diff.GetX(), diff.GetY(), diff.GetHeading());
  odometry->setMeasurement(measurement);

  // Set the covariance of the measurement
  karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();
  Eigen::Matrix<double, 3, 3> info;
  info(0, 0) = precisionMatrix(0, 0);
  info(0, 1) = info(1, 0) = precisionMatrix(0, 1);
  info(0, 2) = info(2, 0) = precisionMatrix(0, 2);
  info(1, 1) = precisionMatrix(1, 1);
  info(1, 2) = info(2, 1) = precisionMatrix(1, 2);
  info(2, 2) = precisionMatrix(2, 2);
  odometry->setInformation(info);

  // Add the constraint to the optimizer
  ROS_DEBUG("[g2o] Adding Edge from node %d to node %d.", sourceID, targetID);
  mOptimizer.addEdge(odometry);
}

2.5 解く

最適化されたポーズを解決して保存します。

void G2oSolver::Compute()
{
    
    
  mCorrections.clear();

  // Fix the first node in the graph to hold the map in place
  g2o::OptimizableGraph::Vertex *first = mOptimizer.vertex(0);
  if (!first)
  {
    
    
    ROS_ERROR("[g2o] No Node with ID 0 found!");
    return;
  }
  first->setFixed(true);

  // Do the graph optimization
  mOptimizer.initializeOptimization();
  int iter = mOptimizer.optimize(40);

  // Write the result so it can be used by the mapper
  g2o::SparseOptimizer::VertexContainer nodes = mOptimizer.activeVertices();
  for (g2o::SparseOptimizer::VertexContainer::const_iterator n = nodes.begin(); n != nodes.end(); n++)
  {
    
    
    double estimate[3];
    if ((*n)->getEstimateData(estimate))
    {
    
    
      karto::Pose2 pose(estimate[0], estimate[1], estimate[2]);
      mCorrections.push_back(std::make_pair((*n)->id(), pose));
    }
    else
    {
    
    
      ROS_ERROR("[g2o] Could not get estimated pose from Optimizer!");
    }
  }
}

3ラン

3.1 依存関係

この記事のコードは、g2o のキネティック バージョンに依存する必要があります。これは、次の手順に従ってインストールする必要があります。
sudo apt-get install ros-kinetic-libg2o

ubuntu1804のg2oは試してないのでコンパイルできるかわかりません。

3.2ラン

この記事に対応するデータパッケージは公式アカウントのlesson6に返信して入手し、launchのbag_filenameを実際のディレクトリ名に変更してください。

以前に使用したデータ パッケージのリンクをTencentドキュメントに配置しました。Tencent ドキュメントのアドレスは次のとおりです。

次のコマンドを使用して、この記事に対応するプログラムを実行します
roslaunch lesson6 karto_slam_outdoor.launch solver_type:=g2o_solver

3.3 結果の分析

起動後、使用されているオプティマイザの特定のタイプが表示されます。

[ INFO] [1636946951.395714883]: ----> Karto SLAM started.
[ INFO] [1636946951.445798371]: Use back end.
[ INFO] [1636946951.445875510]: solver type is G2OSolver.

運用の初期段階では、ループバックが見つからなかったため、最適化されていません。

最適化すると、[g2o] のログがターミナルに出力されます。

[ INFO] [1636947150.294108945, 1606808847.990224492]: [g2o] Optimization finished after 31 iterations.
[ INFO] [1636947150.332113885, 1606808848.030497104]: [g2o] Clearing Optimizer..

以前、この記事でKarto のバックエンド最適化とループバック検出機能の比較テストと分析を分析しましたが、ループバックと最適化機能が追加されていない場合、非常に明白なオーバーレイが最後に生成されます。

最適化前

最適化

最終マップ

最適化がまだ機能していることがわかります。

4 まとめ

この記事では、g2o を使用して karto の最適化を計算する方法を簡単に説明します. g2o 最適化が実際に効果をもたらしたのは定性的分析にすぎません. 一部の学生が独自の定量的分析を行いたい場合は、コードを変更して、分析。

次の記事では、ceres を使用してバックエンドの最適化を計算します。