Przejdź do treści

Robotic Programming Environments: L12

Pose graph optimization with g2o (0-10 points)

Description

Implement a solution for pose graph optimization using g2o to handle scenarios where vertices represent robot or landmark poses, and edges between these vertices represent measurement of robot pose change between different states and measurements of landmarks.

Let’s consider the below graph.

In this graph:

  • x_0, x_1, x_2\in SE(2) – vertices represent 2D poses of the robot,
  • f_1, f_2\in SE(2) – vertices represent poses of measured features.

We know that robot moves between x_0, x_1, and x_2 poses and we can measure the relative poses c_1, c_2 (called constraints) using odometry.

On the other hand, there are some landmarks f_1,f_2 in the environment that are observed by the robot (and measured by its sensor). We know that a robot measures f_1 when it is in x_0 and x_1. The measurements are denoted as c_{01} and c_{11}. Similarly, the f_2 is measured from x_1 and x_2.

By graph optimization you can understand estimation of all vertices poses x_0, x_1, x_2, f_1, f_2 based on the constraints c_1, c_2, c_{01}, c_{11}, c_{12}, c_{22}. The optimization process should minimize the error between the constraints and the estimated poses. The error is defined as the difference between the constraint and the relative pose between the vertices.

Task 1

Solve the pose graph optimization problem using g2o for the provided scenario.

Assume following constraints:

  • c_1=(2.1, 0, 0)
  • c_2=(1.9, 0, 0)
  • c_{01}=(0.5, 1.0, 0)
  • c_{11}=(-1.5, 1.0, 0)
  • c_{12}=(1.0, -1.0, 0)
  • c_{22}=(-1.0, -1.0, 0)
Steps

  1. Create optimizer: Set up a g2o optimizer with an appropriate solver and algorithm.

  2. Add pose vertices: Incorporate vertices representing poses using g2o.VertexSE2() for each pose vertex.

  3. Define constraints: Establish constraints (edges) between poses using the given g2o.EdgeSE2() function. These constraints should capture the relative poses between vertices.

  4. Optimize poses: Initialize the optimization problem and perform the optimization for a specified number of iterations.

  5. Retrieve and print results: After optimization, retrieve and print the optimized poses for each vertex.

  6. Calculate poses of the robot assuming that the initial pose is (0, 0, 0).

Extra task

Propose a new pose graph that contains at least 4 robot poses (x_1, x_2,\ldots, x_N \in SE(3)) and 3 measurements. Optimize the proposed graph for the new scenario where the robot is moving in a 3D space.

Remarks

  • g2o-python installation 
    pip install g2o-python
  • Assume that information matrix for each edge is equal to identity matrix.
  • Provide a well-documented code explaining your approach, variable definitions, and the optimization process.

Examples

Create optimizer

import g2o
optimizer = g2o.SparseOptimizer()
solver = g2o.BlockSolverSE2(g2o.LinearSolverEigenSE2())
optimizer.set_algorithm(g2o.OptimizationAlgorithmLevenberg(solver))

Add vertex

x0 = g2o.VertexSE2()
x0.set_id(0)
optimizer.add_vertex(x0)

Resources