5 from gtsam
import Values
7 def circlePose3(numPoses: int = 8, radius: float = 1.0, symbolChar: str =
'\0') -> Values:
9 circlePose3 generates a set of poses in a circle. This function
10 returns those poses inside a gtsam.Values object, with sequential
11 keys starting from 0. An optional character may be provided, which
12 will be stored in the msb of each key (i.e. gtsam.Symbol).
14 We use aerospace/navlab convention, X forward, Y right, Z down
15 First pose will be at (R,0,0)
18 z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer)
19 Vehicle at p0 is looking towards y axis (X-axis points towards world y)
24 dtheta = 2 * np.pi / numPoses
34 for i
in range(numPoses):
36 gti =
gtsam.Point3(radius * np.cos(theta), radius * np.sin(theta), 0)
40 values.insert(key, gTi)
41 theta = theta + dtheta