Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Wrapping Updates #1146

Merged
merged 3 commits into from
Mar 25, 2022
Merged

Wrapping Updates #1146

Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
96c541b
new method and wrapping for getNewFactorsIndices
varunagrawal Mar 25, 2022
1739192
wrap measured and add tests
varunagrawal Mar 25, 2022
1f494fd
update test for ISAM
varunagrawal Mar 25, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
update test for ISAM
  • Loading branch information
@varunagrawal
varunagrawal committed Mar 25, 2022
commit 1f494fd1bd7382dea45f82ff58b93717f8a20c1d
17 changes: 9 additions & 8 deletions python/gtsam/tests/test_VisualISAMExample.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,18 +10,16 @@
"""
import unittest

import numpy as np

import gtsam
import gtsam.utils.visual_data_generator as generator
import gtsam.utils.visual_isam as visual_isam
from gtsam import symbol
from gtsam.utils.test_case import GtsamTestCase


class TestVisualISAMExample(GtsamTestCase):

"""Test class for ISAM2 with visual landmarks."""
def test_VisualISAMExample(self):
"""Test to see if ISAM works as expected for a simple visual SLAM example."""
# Data Options
options = generator.Options()
options.triangle = False
Expand All @@ -39,19 +37,22 @@ def test_VisualISAMExample(self):
data, truth = generator.generate_data(options)

# Initialize iSAM with the first pose and points
isam, result, nextPose = visual_isam.initialize(data, truth, isamOptions)
isam, result, nextPose = visual_isam.initialize(
data, truth, isamOptions)

# Main loop for iSAM: stepping through all poses
for currentPose in range(nextPose, options.nrCameras):
isam, result = visual_isam.step(data, isam, result, truth, currentPose)
isam, result = visual_isam.step(data, isam, result, truth,
currentPose)

for i in range(len(truth.cameras)):
for i, _ in enumerate(truth.cameras):
pose_i = result.atPose3(symbol('x', i))
self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)

for j in range(len(truth.points)):
for j, _ in enumerate(truth.points):
point_j = result.atPoint3(symbol('l', j))
self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)


if __name__ == "__main__":
unittest.main()