Factor Graph SLAM with GTSAM

[VAM7686]

Description

Uses the GTSAM library to implement 3D localization using a factor graph. GPS coordinates, IMU measurements, magnetometer measurements, and wheel odometry measurements are continually integrated into the factor graph which helps localizes the robot over time. With 3D localization we are able to go over uneven terrain such as ramps.

This PR does the following: Main focus:

• Implements 3D localization using a factor graph.

Other fixes:

• Fixes GPS plugin configuration issues
• Fixes GPS waypoints for simulations
• Added magnetometer integration in robot_localization ekf_localization_node configuration

Fixes #555 and #662

• [x] #555
• [ ] #662

Testing

Test Case 1

1. Launch ramp_lane.launch
   • roslaunch igvc_gazebo ramp_lane.launch
2. Launch the 3D localization and navigation using the 3D_nav_simulation.launch
   • roslaunch igvc_navigation 3D_nav_simulation.launch
3. Launch rviz and display ground truth from the topic /ground_truth and display the slam estimate from the topic /odometry/filtered

Test Case 2

1. Set yaw angle in ramp.launch file to a non zero number
2. Launch ramp_lane.launch
   • roslaunch igvc_gazebo ramp_lane.launch
3. Launch the 3D localization and navigation using the 3D_nav_simulation.launch
   • roslaunch igvc_navigation 3D_nav_simulation.launch
4. Launch rviz and display ground truth from the topic /ground_truth and display the slam estimate from the topic /odometry/filtered

Test Case 3

1. Launch autonav.launch
   • roslaunch igvc_gazebo autonav.launch
2. Launch the 3D localization and navigation using the 3D_nav_simulation.launch
   • roslaunch igvc_navigation 3D_nav_simulation.launch
3. Launch rviz and display ground truth from the topic /ground_truth and display the slam estimate from the topic /odometry/filtered

Expectations for test cases:

1. Slam estimate matches the ground truth.
2. Robot is able to go over ramps.
3. No noise on the costmap. Should not be like this.

Self Checklist

• [x] I have formatted my code using make format
• [x] I have tested that the new behavior works