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

use Point3 instead of artificial vector3

release/4.3a0
John Lambert 2021-10-22 15:05:05 -04:00 committed by GitHub
parent 97fee355ee
commit d48b7371bb
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 2 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -13,12 +13,7 @@ import unittest
import numpy as np import numpy as np
import gtsam import gtsam
from gtsam import Pose2, PriorFactorPose2, Values from gtsam import Point3, Pose2, PriorFactorPose2, Values
def vector3(x: float, y: float, z: float) -> np.ndarray:
"""Create 3d double numpy array."""
return np.array([x, y, z], dtype=float)
class TestLago(unittest.TestCase): class TestLago(unittest.TestCase):
@ -32,7 +27,7 @@ class TestLago(unittest.TestCase):
graph, initial = gtsam.readG2o(g2oFile) graph, initial = gtsam.readG2o(g2oFile)
# Add prior on the pose having index (key) = 0 # Add prior on the pose having index (key) = 0
priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) priorModel = gtsam.noiseModel.Diagonal.Variances(Point3(1e-6, 1e-6, 1e-8))
graph.add(PriorFactorPose2(0, Pose2(), priorModel)) graph.add(PriorFactorPose2(0, Pose2(), priorModel))
estimateLago: Values = gtsam.lago.initialize(graph) estimateLago: Values = gtsam.lago.initialize(graph)