diff --git a/python/gtsam/utils/circlePose3.py b/python/gtsam/utils/circlePose3.py index e1def9427..2c08c8749 100644 --- a/python/gtsam/utils/circlePose3.py +++ b/python/gtsam/utils/circlePose3.py @@ -1,10 +1,10 @@ -import gtsam -import math + import numpy as np -from math import pi +import gtsam +from gtsam import Values -def circlePose3(numPoses=8, radius=1.0, symbolChar='\0'): +def circlePose3(numPoses: int = 8, radius: float = 1.0, symbolChar='\0') -> Values: """ circlePose3 generates a set of poses in a circle. This function returns those poses inside a gtsam.Values object, with sequential @@ -21,14 +21,21 @@ def circlePose3(numPoses=8, radius=1.0, symbolChar='\0'): values = gtsam.Values() theta = 0.0 - dtheta = 2 * pi / numPoses + dtheta = 2 * np.pi / numPoses gRo = gtsam.Rot3( - np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F')) + np.array( + [ + [0., 1., 0.], + [1., 0., 0.], + [0., 0., -1.] + ], order='F' + ) + ) for i in range(numPoses): key = gtsam.symbol(symbolChar, i) - gti = gtsam.Point3(radius * math.cos(theta), radius * math.sin(theta), 0) - oRi = gtsam.Rot3.Yaw( - -theta) # negative yaw goes counterclockwise, with Z down ! + gti = gtsam.Point3(radius * np.cos(theta), radius * np.sin(theta), 0) + # negative yaw goes counterclockwise, with Z down ! + oRi = gtsam.Rot3.Yaw(-theta) gTi = gtsam.Pose3(gRo.compose(oRi), gti) values.insert(key, gTi) theta = theta + dtheta