circlePose3.py
Go to the documentation of this file.
1 
2 import numpy as np
3 
4 import gtsam
5 from gtsam import Values
6 
7 def circlePose3(numPoses: int = 8, radius: float = 1.0, symbolChar: str = '\0') -> Values:
8  """
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).
13 
14  We use aerospace/navlab convention, X forward, Y right, Z down
15  First pose will be at (R,0,0)
16  ^y ^ X
17  | |
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)
20  """
21 
22  values = gtsam.Values()
23  theta = 0.0
24  dtheta = 2 * np.pi / numPoses
25  gRo = gtsam.Rot3(
26  np.array(
27  [
28  [0., 1., 0.],
29  [1., 0., 0.],
30  [0., 0., -1.]
31  ], order='F'
32  )
33  )
34  for i in range(numPoses):
35  key = gtsam.symbol(symbolChar, i)
36  gti = gtsam.Point3(radius * np.cos(theta), radius * np.sin(theta), 0)
37  # negative yaw goes counterclockwise, with Z down !
38  oRi = gtsam.Rot3.Yaw(-theta)
39  gTi = gtsam.Pose3(gRo.compose(oRi), gti)
40  values.insert(key, gTi)
41  theta = theta + dtheta
42  return values
gtsam::Rot3::Yaw
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition: Rot3.h:174
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
gtsam::symbol
Key symbol(unsigned char c, std::uint64_t j)
Definition: inference/Symbol.h:139
gtsam::utils.circlePose3.circlePose3
Values circlePose3(int numPoses=8, float radius=1.0, str symbolChar='\0')
Definition: circlePose3.py:7
gtsam::Values
Definition: Values.h:65
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:06