Project for course "Artificial Intelligence in Robotics"
- Robot localization with GTSAM
Goal of the project was to prepare a state estimator using graph optimization for sensor fusion. To accomplish that the GTSAM library [1] was chosen.
For the project we chose to simulate a TurtleBot3 Waffle in Gazebo simulator. The 3 sensors used to create a factor graph were:
- Odometry,
- IMU,
- LiDAR.
From odometry the position and orientation was available straight away, from IMU it was enough to integrate the linear acceleration twice to get the position and to integrate angular velocity once to receive the orientation. LiDAR returns distance to obstacles around the robot, so those values were used to create a point cloud. ICP (Iterative closest point) algorithm [2] uses two consecutive point clouds and finds the rotation matrix and translation vector between them (we chose the Open3D [3] implementation of the algorithm).
The factor graph, made with all sensors from previous subsection, is shown in the picture below.
mkdir ~/turtlebot_localization_ws && cd ~/turtlebot_localization_ws
git clone https://github.com/mmcza/Robot-localization-with-GTSAM
sudo apt-get install -y ros-humble-turtlebot3-gazebo && sudo apt-get install -y ros-humble-nav2-bringup && sudo apt-get install -y ros-humble-navigation2 && sudo apt-get install -y ros-humble-gtsam && sudo apt-get install -y python3-pip
pip install open3d && pip install gtsam && pip install numpy==1.24.4
cd Robot-localization-with-GTSAM && docker build -t robot_localization .
bash start_container.sh
docker exec -ti robot_localization bash
Note
The robot_localization_ws directory is shared between the host and container. In the result files inside of it might require sudo privileges to save any changes.
Note
Dockerfile and script for running container are based on Rafał Staszak's repository
To make the project work we need to prepare the Gazebo simulator properly
source /opt/ros/humble/setup.bash
export TURTLEBOT3_MODEL=waffle
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/humble/share/turtlebot3_gazebo/models
source /usr/share/gazebo/setup.bash
Inside of your workspace (NOT inside your src)
colcon build --symlink-install
source install/setup.bash
In first terminal start the estimator
ros2 run robot_localization estimator
In second terminal start the simulator
ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False
After launching the estimator and simulator you have to estimate the initial position (by clicking specific button and placing arrow inside RVIZ) and add a goal (so the robot start to move). You can also visualize the estimated state of the robot.
Position and orientation of the robot (from estimator, ground truth from gazebo and odometry) is also saved to trajectory.csv file, so it's possible to display it and check the results after the experiment has ended.
Due to the fact that the Odometry was returning basically same results as real position of the robot, there was added an additional noise with normal distribution (
A comparision of Odometry error (with noise) and error from our estimator can be seen on the plot below
Table with parameters for both errors (additionally after removing first measurement as it was completely different for the estimator) - calculated using Pandas describe() method.
| Parameter | Estimator | Odometry | Estimator >0s | Odometry >0s |
|---|---|---|---|---|
| count | 736 | 736 | 735 | 735 |
| mean | 0.099232 | 0.104481 | 0.096562 | 0.104488 |
| std | 0.088873 | 0.056087 | 0.051536 | 0.056125 |
| min | 0.004427 | 0.002932 | 0.004427 | 0.002932 |
| 25% | 0.059396 | 0.062254 | 0.059389 | 0.062188 |
| 50% | 0.086959 | 0.095967 | 0.086887 | 0.095914 |
| 75% | 0.122585 | 0.135575 | 0.122386 | 0.135611 |
| max | 2.061525 | 0.304993 | 0.314617 | 0.304993 |
The estimator without the intial position has lowest values for all parameters apart from it's outliers - min and max (what is interesting because both top 25% and top 75% are the lowest). The mean value of odometry's error is ~8% bigger than the mean error of estimator.
Based on the data from the table, it is noticeable that the estimator does its job and reduces the noise.
This plot illustrates the comparison of odometry, estimated, and real trajectories collected from the dataset.
Due to some issues with sampling of the real position of the robot there might be made some improvements. One way could be to add constant and equal interval between adding factors to the graph. Another possible solution would be to optimize the graph less frequently (for example once every 5 or 10 poses are added as initial state to the graph.)
[1] Dellaert, F. and GTSAM Contributors borglab/gtsam v.4.2a8. Georgia Tech Borg Lab (2022). https://github.com/borglab/gtsam, https://doi.org/10.5281/zenodo.5794541
[2] Zhang, Z. Iterative point matching for registration of free-form curves and surfaces. Int J Comput Vision 13, 119–152 (1994). https://doi.org/10.1007/BF01427149
[3] Zhou, Q. and Park, J. and Koltun, V. Open3D: A Modern Library for 3D Data Processing. arXiv (2018). https://doi.org/10.48550/arXiv.1801.09847