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 gtsam
from gtsam import 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)
from gtsam import Point3, Pose2, PriorFactorPose2, Values
def run(args: Namespace) -> None:
@ -41,7 +36,7 @@ def run(args: Namespace) -> None:
graph, initial = gtsam.readG2o(g2oFile)
# 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))
print(graph)