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

clean up test_Pose3SLAMExample.py

release/4.3a0
John Lambert 2021-08-12 07:41:45 -04:00 committed by GitHub
parent c9bcb1430c
commit 85e58a78bb
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
python/gtsam/tests/test_Pose3SLAMExample.py
View File

@ -15,14 +15,14 @@ import numpy as np
import gtsam
from gtsam.utils.test_case import GtsamTestCase
from gtsam.utils.circlePose3 import *
from gtsam.utils.circlePose3 import circlePose3
class TestPose3SLAMExample(GtsamTestCase):
def test_Pose3SLAMExample(self):
def test_Pose3SLAMExample(self) -> None:
# Create a hexagon of poses
hexagon = circlePose3(6, 1.0)
hexagon = circlePose3(numPoses=6, radius=1.0)
p0 = hexagon.atPose3(0)
p1 = hexagon.atPose3(1)
@ -31,7 +31,7 @@ class TestPose3SLAMExample(GtsamTestCase):
fg.add(gtsam.NonlinearEqualityPose3(0, p0))
delta = p0.between(p1)
covariance = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.05, 0.05, 0.05, 5. * pi / 180, 5. * pi / 180, 5. * pi / 180]))
np.array([0.05, 0.05, 0.05, np.deg2rad(5.), np.deg2rad(5.), np.deg2rad(5.)]))
fg.add(gtsam.BetweenFactorPose3(0, 1, delta, covariance))
fg.add(gtsam.BetweenFactorPose3(1, 2, delta, covariance))
fg.add(gtsam.BetweenFactorPose3(2, 3, delta, covariance))