12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix LAGO docs, add test

release/4.3a0
Frank Dellaert 2025-04-26 17:52:46 -04:00
parent 8fbe3c9c8c
commit 3e13c44802
3 changed files with 131 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

183
gtsam/slam/doc/lago.ipynb
View File

File diff suppressed because one or more lines are too long

1
gtsam/slam/slam.i
View File

@ -467,6 +467,7 @@ typedef gtsam::TriangulationFactor<gtsam::PinholePose<gtsam::Cal3Unified>>
namespace lago {
gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true);
gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess);
gtsam::VectorValues initializeOrientations(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true);
}
} // namespace gtsam

28
python/gtsam/tests/test_lago.py
View File

@ -13,7 +13,7 @@ import unittest
import numpy as np
import gtsam
from gtsam import Point3, Pose2, PriorFactorPose2, Values
from gtsam import BetweenFactorPose2, Point3, Pose2, PriorFactorPose2, Values
class TestLago(unittest.TestCase):
@ -33,6 +33,32 @@ class TestLago(unittest.TestCase):
estimateLago: Values = gtsam.lago.initialize(graph)
assert isinstance(estimateLago, Values)
def test_initialize2(self) -> None:
"""Smokescreen to ensure LAGO can be imported and run on toy data stored in a g2o file."""
# 1. Create a NonlinearFactorGraph with Pose2 factors
graph = gtsam.NonlinearFactorGraph()
# Add a prior on the first pose
prior_mean = Pose2(0.0, 0.0, 0.0)
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.05]))
graph.add(PriorFactorPose2(0, prior_mean, prior_noise))
# Add odometry factors (simulating moving in a square)
odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
graph.add(BetweenFactorPose2(0, 1, Pose2(2.0, 0.0, 0.0), odometry_noise))
graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, np.pi / 2), odometry_noise))
graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, np.pi / 2), odometry_noise))
graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, np.pi / 2), odometry_noise))
# Add a loop closure factor
loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.25, 0.25, 0.15]))
# Ideal loop closure would be Pose2(2.0, 0.0, np.pi/2)
measured_loop = Pose2(2.1, 0.1, np.pi / 2 + 0.05)
graph.add(BetweenFactorPose2(4, 0, measured_loop, loop_noise))
estimateLago: Values = gtsam.lago.initialize(graph)
assert isinstance(estimateLago, Values)
if __name__ == "__main__":
unittest.main()