Mobile Robotics-Project 1 Pose Graph Optimization Solved

In this project, we are going to use a non-linear weighted least squares optimization approach to solve the problem of getting a better estimate of our robot's trajectory. Least squares formulations are widely used for optimization, be it computer vision or robotics or machine learning. We will dive deep into it during this project and you will have complete clarity on optimization for vector-valued residual functions.

In this "Introduction" section, I am going to provide an introduction for SLAM problem for a robot operating in the 2D world. It is 2. section in this Project. The 1D SLAM problem (1.) is far much simpler to understand and will be described directly in the 1. section.

In a 2D world, a robot has 3 degrees of freedom, i.e. its pose in the world can be expressed by the state vector 𝐱 = (𝑥, 𝑦, 𝜃)T. For the scope of this project, we are interested only in the robot's trajectory through the 2D world, and NOT in distinct landmarks or the surronding map of the environment, i.e. we are only interested in "L"ocalization part of SLAM.

Therefore, we can represent it as a graph where the vertices represent robot poses 𝐱𝑖 and edges represent the spatial constraints between these poses. Such a map is generally called a pose graph.

Two different kinds of constraints are necessary for pose graph SLAM. The first are odometric constraints that connect two successive states 𝐱𝑖 and 𝐱𝑖+1 via a motion model. Furthermore, in order to perform loop closing, the robot has to recognize places it already visited before. This place recognition is also a part of the front-end and provides the second type of constraint, the loop closure constraints. These constraints connect two not necessarily successive poses 𝐱𝑖 and 𝐱𝑗.

 

 

(Source: Sunderhauf 2012 (https://core.ac.uk/download/pdf/89299995.pdf))

You will start from the inaccurate pose graph with odometry and loop closure information and by the end of this Project, you end up with an optimized pose graph (see above images) which should look close to ground truth trajectory. You can watch this video (https://youtu.be/saVZtgPyyJQ) to get an intuition for what we're about to do.

Okay, that's enough of theory. Let's get out hands dirty with the code!

In [ ]:

import matplotlib.pyplot as plt import math import os

 import jax.numpy as jnp #see supplementary notebook to see how to use this from jax import jacfwd

# If you're `importing numpy as np` for debugging purposes, 

# while submitting, please remove 'import numpy' and replace all np's with jnp's.(m

1. Pose Graph Optimization for 1D SLAM
A solved example for 1D SLAM which optimizes for pose variables using weighted least squares method

(Gauss Newton) has been explained in the class. It has been made available here

(https://www.notion.so/saishubodh/Solved-Example-1D-SLAM-weighted-LS-Illustrating-Sparsity-in-SLAMd8b45893843b4377b07b1d4aa1aab4de). Your first task is to code this from scratch. [[CP-M]]

For this section, you have to calculate Jacobian analytically yourself and use it. However, you can check how correct jax 's jacobian . Its usage is explained in the supplementary notebook.

In [ ]:

 ############################################################################## # TODO: Code for Section 1                                                   # pass

##############################################################################

#                             END OF YOUR CODE                               #

##############################################################################

2. Pose Graph Optimization for 2D SLAM
Things are about to get interesting!

2.1 Coding from scratch
Objective
A robot is travelling in a oval trajectory. It is equipped with wheel odometry for odometry information and RGBD sensors for loop closure information. Due to noise in wheel odometry it generates a noisy estimate of the trajectory. Our task is to use loop closure pairs to correct the drift.

We pose this problem as a graph optimization problem. In our graph, poses are the vertices and constraints are the edges.

Given:

In practical scenarios, we'd obtain the following from our sensors after some post-processing:

1.   Initial position

2.   Odometry Contraints/Edges: This "edge" information basically tells us relative transformation between two nodes. These two nodes are consecutive in the case of Odometry but not in the case of Loop Closure (next point).

3.   Loop Closure Contraints/Edges Remember that while optimizing, you have another kind of "anchor" edge as you've seen in 1. solved example.

You have been given a text file named edges.txt which has all the above 3 and it follows G2O's format (as explained in class, link here (https://www.notion.so/saishubodh/G2O-Edge-Descriptionfa07cc28967541dc8a71170de46c5da7) ).

Details:

1.   Using the following motion model, you have to first generate the "initialization" for all the poses/vertices using the "Given" information. Just like in the 1D case.

𝑥𝑘+1 = 𝑥𝑘 + Δ𝑥(𝑘,𝑘+1) cos(𝜃𝑘) − Δ𝑦(𝑘,𝑘+1) sin(𝜃𝑘)

𝑦𝑘+1 = 𝑦𝑘 + Δ𝑦(𝑘,𝑘+1) cos(𝜃𝑘) + Δ𝑥(𝑘,𝑘+1) sin(𝜃𝑘)

𝜃𝑘+1 = 𝜃𝑘 + Δ𝜃(𝑘,𝑘+1)
(3)
Even the loop closure nodes are related by the above model, except that it need not necessarily be consecutive notes k and k+1.

Save this initial trajectory as edges-poses.g2o .

If you plot the initialized poses using odometry information, you need to get as the right plot [[CP-M]] below (this is the "noisy trajectory"): (Left one is the ground truth)

 

(Use draw() helper function or g2o_viewer or EVO )

2.   Now calculate the residual and the Jacobian and update your parameters using LM.

Use LM algorithm. Regarding Jacobian calculation, you can use jax 's jacobian as part of your main code. However, you still have to separately calculate it analytically and verify if it matches with jax 's jacobian using [[CP-M]] frobenius norm frobNorm() ). Calculation and verification is compulsory, but it is your choice to use whichever as part of your optimization. Use whichever is faster.

3.   Regarding LM iterations, stopping criterion, information matrix values.

A.   [[CP-B]] As your iterations proceed, you have to print relevant information (iteration number and error value: 𝐹  𝐟 ⊤𝛀𝐟 (notion page link) (https://www.notion.so/saishubodh/From-linear-algebra-to-nonlinear-weighted-least-squares-optimization-

13cf17d318be4d45bb8577c4d3ea4a02#32832dee7d6c4ab49581463d9b784f21) at every step).

B.   [[CP-B]] You have to show the plots (ground truth, noisy & optimized: all 3 in a single plot) at every 10 steps or so.

C.   [[CP-M]] You could start with information values of 500 for odom edges, 700 for loop closure edges, 1000 for anchor edge (same for all dimensions). However, you have to heavily experiment with these values. (Given that you somehow know loop closure information is way more reliable than odometry.). At the end of your experimentation, your error 𝐹  𝐟 ⊤𝛀𝐟 should by < 40. Explain your experimentation in detail using tables/plots etc if necessary.

Do not worry if you're not getting a perfect trajectory. Our parametrization was oversimplified for the sake of this project. With that being said, it is possible to get the error down to < 40 and make it at least look like an oval shaped trajectory, even if it doesn't perfectly resemble the ground truth. However, using g2o (next section), you will be getting a close to ground truth trajectory.

In [ ]:

 ############################################################################## # TODO: Code for Section 2.1                                                 # pass

##############################################################################

#                             END OF YOUR CODE                               # ##############################################################################

2.1 Answer
Give a detailed answer addressing the above questions. When I run the above code, it should follow points described above (such as plots at every 10 steps) and (When I run the above code, it should) write the optimized poses to a file named opt.g2o . As a backup, save another file opt_backup.g2o in an offline manner beforehand.

That apart, save important plots and add them here so that it can supplement your answer, for example you could add plots at crucial stages of optimization. You have to add useful metrics/plots from EVO (refer to supplementary notebook). Using EVO, the bare minimum you have to report is mean absolute pose error (ape) and mean relative pose error (rpe) . However, you are encouraged to use tools like

evo_traj and more (https://github.com/MichaelGrupp/evo/#command-line-interface) and add more

plots/metrics. Marks will be awarded based on overall analysis & presentation which would reflect your understanding.

Note that EVO and g2o_viewer (below) could help you in debugging.

Add answer for 2.1 here:

2.2 Using g2o's optimization: g2o binary or g2o viewer
Installation setup is described in supplementary notebook. More details for 2.2.1 and 2.2.2 can be found in the supplementary notebook.

2.2.1 Optimizing edges.txt

First task is to optimize the poses of dataset you've been working with so far.

2.2.2 Optimizing intel and sphere datasets
You have been given two datasets in the data folder. You have to use g2o_viewer to optimize these both. You have to experiment with the options/parameters available in the GUI. More instructions in supplementary notebook. You have to experiment till you get the trajectories which look like the following:

 

2.2 Answer
Add images: take screenshot of the GUI of g2o_viewer after optimization for all 3 [[CP-M]] and add here. Briefly describe what you had to do (detailed answer is not expected). g2o could potentially give you close to ground truth trajectory for all 3, but if you are unable to get to close to ground truth, add the best you can get.

* Important Information regarding Questions 3 & 4
Note that it is mandatory to attempt EITHER 3 OR 4, only one of it. If you attempt both, the question which you score more will be considered and the other as bonus question.

It is encouraged for those into robotics/deep learning research to prefer 4 over 3.

[Bonus*] 3. Deriving Motion model geometrically
* - read information above under section "Important Information regarding Questions 3 & 4"

The current robot state is as follows: (𝑖 and 𝑘 are interchangably used below, sorry I am too lazy to edit now 😛)

 

Can you derive the below equation using geometry? (Read on)

𝑥𝑘+1 = 𝑥𝑘 + Δ𝑥(𝑘,𝑘+1) cos(𝜃𝑘) − Δ𝑦(𝑘,𝑘+1) sin(𝜃𝑘)

𝑦𝑘+1 = 𝑦𝑘 + Δ𝑦(𝑘,𝑘+1) cos(𝜃𝑘) + Δ𝑥(𝑘,𝑘+1) sin(𝜃𝑘)

𝜃𝑘+1 = 𝜃𝑘 + Δ𝜃(𝑘,𝑘+1)
(3)
In other words, we want to find 𝛿's in terms of Δ's

                                                                                𝛿𝑥 = Δ𝑥 cos(𝜃) − Δ𝑦 sin(𝜃)                        (2)

𝛿𝑦 = Δ𝑦 cos(𝜃) + Δ𝑥 sin(𝜃)

where 𝛿's are the updates in our motion model equation:

𝑥𝑘+1 = 𝑥𝑘 + 𝛿𝑥

𝑦𝑘+1 = 𝑦𝑘 + 𝛿𝑦

𝜃𝑘+1 = 𝜃𝑘 + 𝛿𝜃
(1)
Oh yes, 𝜃 is straightforward, i.e. 𝛿𝜃 = Δ𝜃 but why?

Using geometry, you could just draw and insert a self-explanatory image as the answer to this question.

If you can derive it without using geometry purely using transform matrices/algebra, that is fine too. Whatever you're comfortable.

3. Answer

Your answer here.

[Bonus*] 4. Research Paper Reading
* - read information above under section "Important Information regarding Questions 3 & 4"

(Do not get intimidated, you are not expected to do a thorough research analysis for this task. A high level understanding is sufficient.)

"Past, Present & Future of SLAM: Towards the Robust Perception Age" (https://arxiv.org/abs/1606.05830) is an exciting survey paper of 2016 which sums up, well, the "past, present & future" of SLAM. Your task is as follows:

1.   Go through the sections "IV. LONG-TERM AUTONOMY II: SCALABILITY" & "III. LONG-TERM

AUTONOMY I: ROBUSTNESS". Don't worry, you are not expected to have a deep understanding. Skip the parts which you don't understand at all. Go through it at a high level, and take a slightly closer look at "Open Problems" in these sections.

2.   Read up common applications of deep learning for computer vision/robotics through blogs online (for example, first 4 points of this (https://machinelearningmastery.com/applications-of-deep-learning-forcomputer-vision/). Again, you are only expected to understand it at a high level, for example, 'semantic segmentation is an application of deep learning for computer vision which is the task of assigning a category to each of the pixels in the image'.

Firstly, summarize your understanding of the above two points.

Now, from the understanding you've gathered so far, how would you approach solving those "Open Problems"? Can these algorithms help in dealing with some of the issues you might have faced during this project? Can the deep learing based high level understanding of the world help in SLAM? In the context of long term autonomy, imagine tomorrow's world with a buddy robot R2-D2 which follows you wherever you go... Now imagine how easily the trajectory can diverge, how big the map could soon become and how the computation could easily become intractable.

Answer the above questions in the context of this project and those 2 sections of the survey paper