This repository has been archived by the owner on Apr 24, 2023. It is now read-only.
Factor Graph SLAM with GTSAM #834
Open
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
…into feat/slam-gtsam
…into feat/slam-gtsam
…into feat/slam-gtsam
…into feat/slam-gtsam
The wheel radius was accidentally set to the wheel diameter causing the robot to move slowly. This commit will change the param to its correct value.
… incorrect due to gazebo gps plugin bug
Initially the graph was initialized with a default orientation of 0 which really screwed stuff up with the mapping before the position estimate was able to converge. This commit uses the orientation from the imu as the initial heading and fixes the problems when the robot initially at a different heading.
Added initial yaw orientation for wheel odometer and cleaned up includes in cpp and h files
VAM7686
commented
Sep 2, 2021
| isam_.getFactorsUnsafe().print(); | ||
| #endif | ||
|
|
||
| result_ = isam_.calculateEstimate(); |
There was a problem hiding this comment.
Not sure if I should be using calculateBestEstimate() instead
VAM7686
commented
Sep 2, 2021
Comment on lines
+164
to
+165
| (gtsam::Vector(6) << g_xVar, g_yVar, g_zVar, g_thetaVariance * 2, g_thetaVariance * 2, g_thetaVariance) | ||
| .finished())); |
There was a problem hiding this comment.
Not sure if this is the proper way to initialize the noise matrix
Sign up for free
to subscribe to this conversation on GitHub.
Already have an account?
Sign in.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
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:
Other fixes:
Fixes #555 and #662
Testing
Test Case 1
roslaunch igvc_gazebo ramp_lane.launchroslaunch igvc_navigation 3D_nav_simulation.launchTest Case 2
roslaunch igvc_gazebo ramp_lane.launchroslaunch igvc_navigation 3D_nav_simulation.launchTest Case 3
roslaunch igvc_gazebo autonav.launchroslaunch igvc_navigation 3D_nav_simulation.launchExpectations for test cases:
Self Checklist
make format