Draft
Conversation
Squashed commit of the following: commit b8db3e4 Author: newtype51 <connor.holmes@mail.utoronto.ca> Date: Fri Jul 18 13:17:33 2025 -0400 Added optimization test for WNOA Factor. Integrated WNOA into Lost in the woods example. commit b6570f4 Author: newtype51 <connor.holmes@mail.utoronto.ca> Date: Thu Jul 17 23:56:16 2025 -0400 WNOA optimization tests passing for SE2 and SE3. Moved all code into header file. commit 7f62e0e Author: newtype51 <connor.holmes@mail.utoronto.ca> Date: Thu Jul 17 20:18:18 2025 -0400 Converted WNOA factors to version that splits up the velocity and pose states into separate variables. Also implemented templated version. Tests passing except optimization test. commit 481d2d3 Author: newtype51 <connor.holmes@mail.utoronto.ca> Date: Mon Jul 14 19:57:39 2025 -0400 Implemented and tested Jacobians for WNOA model. commit 3db0d86 Author: newtype51 <connor.holmes@mail.utoronto.ca> Date: Sun Jul 13 20:13:36 2025 -0400 WNOA has testable trait. Added test cases for factor error eval. commit a7a0fc3 Author: newtype51 <connor.holmes@mail.utoronto.ca> Date: Sat Jul 5 19:09:59 2025 -0400 Started building WNOA motion model factor and unittests. commit 24af876 Author: newtype51 <connor.holmes@mail.utoronto.ca> Date: Thu Jun 19 20:36:28 2025 -0400 Assignment 2 now converging with bearing range measurements, even when init from odom. commit 7910855 Author: Connor Holmes <connor.holmes@mail.utoronto.ca> Date: Thu Jun 19 16:20:13 2025 -0400 Implementing bearing and range measurements. Expression working but optimization blows up. commit ffebeae Author: Connor Holmes <connor.holmes@mail.utoronto.ca> Date: Wed Jun 18 15:24:43 2025 -0400 assmt 2 with odometry-only working commit 6b48451 Author: Connor Holmes <connor.holmes@mail.utoronto.ca> Date: Tue Jun 17 20:19:01 2025 -0400 Import assigment 2 data and start making factor graph commit 3643a3c Author: newtype51 <connor.holmes@mail.utoronto.ca> Date: Mon Jun 16 17:25:05 2025 -0400 can store results and reproduce plots with python for 2d case, but something seems wrong with the covariance scaling commit 14a52ec Author: newtype51 <connor.holmes@mail.utoronto.ca> Date: Fri Jun 13 19:10:47 2025 -0400 set up dev container
- Changed Qc to a vector - Implemented new Point1 Manifold - Renamed WNOAFactors.h to WNAOFactor.h for consistency - Minor changes to plotting script (removed absolute path)
- Point1 Manifold - Giant-Glass-Of-Milk dataset
…ng (required yaml-cpp install). Added gt trajectory to plots.
…y and WNOA factors.
Created evaluable class for bearing-range 2d meas and wrote test functions for this class. Updated plotting so that it generates STEAM/GTSAM comparison plots.
…end to WNOV or other factors
resolved conflicts in WNOAFactor.h. Tested that it still works
…ses/velocities - used a new way to get the required blocks from JointMarginal
Refactored Interpolator. Covariance interpolation is working.
…o more computing Psi and Lambda twice)
Interpolator unit test
Allow for extrapolating in Interpolator
* Cleaned up WNOAGraph analysis * precomputation of inner -> outer key mappings * flattened Jacobian maps * Improved overall timing of wrapper factors * fixing seed for reproducability in WNOA analysis * more timing tests * Finalized timing tests on A2 * making sure new graph works with multi-threading * use steady clocks and introduce warm up phase * Small performance adjustments * generating results for paper * fixing implementation of Jacobian functions in WNOAFactor (before they were implemented using expressions in the book, which relied on left hand body frame pertubations) * added NEES to LostInTheWoods analysis. Fixed error in covariance heading plotting. Fixed error in RMSE calculation * tuning * Clean up * removed the need to indicate which factors are wrappers for the WNOAGraph and used dynamic casting instead - this should add minimal computational overhead but makes it a lot easier to set up the graph for users * Using precomputed Lambda and Psi values in WNOAGraph Added handing precomputed values that are shared between states with the same bordering states to the interpolator (these are not exploited yet) * working on precomputing shared values for states with the same bordering states * WNOAFactorGraph now precomputes xi_kp1 and xi_kp1_dot, as well as the Jacobians mapping from local to global poses at the borders. These values are shared between interpolated states with the same bordering states. Adds an additional small speed-up * Removed redundant code by breaking out computation of xi_kp1, xi_dot_kp1 and associated Jacobians into its own function in Interpolator class * modified plotting script for plots in paper * trying to debug SLAM performance
* Clean up Interpolator.h and add Doxygen style comments * Clean up StateData.h and add Doxygen style comments * Clean up WNOAFactor.h and add Doxygen style comments * Clean up WNOAInterpFactor.h and add Doxygen style comments * Clean up WNOAFactorGraph.h and add Doxygen style comments * Clean up WNOALMOPtimizer and add Doxygen style comments * Fix to timing plotting script
Member
|
Woohoo!!!. I asked for a review from copilot, which knows about our naming conventions, and also ran CI. After these two things have been satisfied by some more commits from y'all, I'll do a deep dive. |
Author
|
I set this up as a draft PR so that I could cleanly see all of the changes. It's definitely not ready for review yet. I will also split it into more digestible PRs, so please disregard for now! |
Fix damping parameter bug induced by merge.
|
|
||
| // add WNOA factor | ||
| graph.add(WNOAMotionFactor<Pose2>(P(0), V(0), P(1), V(1), timestep, Q_se2)); | ||
| // Set up initial geuss |
There was a problem hiding this comment.
The comment spelling "geuss" should be corrected to "guess".
|
|
||
| // add WNOA factor | ||
| graph.add(WNOAMotionFactor<Pose3>(P(0), V(0), P(1), V(1), timestep, Q_se3)); | ||
| // Set up initial geuss |
There was a problem hiding this comment.
The comment spelling "geuss" should be corrected to "guess".
|
|
||
| // add WNOA factor | ||
| graph.add(WNOAMotionFactor<Pose3>(P(0), V(0), P(1), V(1), timestep, Q_se3)); | ||
| // Set up initial geuss |
There was a problem hiding this comment.
The comment spelling "geuss" should be corrected to "guess".
…to make equations clearer.
…is is important for interpolation and Jacobian chain rules)
…ly added details on what data we precomputed and how we can save on compute by exploiting shared variables
Remove dead code from Interpolator test
Remove small_angle functions (missed) Move WNOA factor graph constructor into cpp file.
Code Clean-up: Adding comments and deleting dead code
This file contains hidden or 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
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
4 participants
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.
Draft PR for full Continuous-Time, Gaussian-Process Factors dev branch for GTSAM