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:37 -04:00 committed by GitHub
parent d48b7371bb
commit a93c58abd6
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/examples/Pose2SLAMExample_lago.py
View File

@ -25,12 +25,7 @@ from argparse import Namespace
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)
def run(args: Namespace) -> None: def run(args: Namespace) -> None:
@ -41,7 +36,7 @@ def run(args: Namespace) -> None:
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))
print(graph) print(graph)