graphslam.pose.se2 module
Representation of a pose in \(SE(2)\).
- class graphslam.pose.se2.PoseSE2(position, orientation)[source]
Bases:
BasePose
A representation of a pose in \(SE(2)\).
- Parameters:
position (np.ndarray, list) – The position in \(\mathbb{R}^2\)
orientation (float) – The angle of the pose (in radians)
- COMPACT_DIMENSIONALITY = 3
The compact dimensionality
- classmethod from_matrix(matrix)[source]
Return the pose as an \(SE(2)\) matrix.
- Parameters:
matrix (np.ndarray) – The \(SE(2)\) matrix that will be converted to a PoseSE2 instance
- Returns:
The matrix as a PoseSE2 object
- Return type:
- jacobian_boxplus()[source]
Compute the Jacobian of \(p_1 \boxplus \Delta \mathbf{x}\) w.r.t. \(\Delta \mathbf{x}\) evaluated at \(\Delta \mathbf{x} = \mathbf{0}\).
- Returns:
The Jacobian of \(p_1 \boxplus \Delta \mathbf{x}\) w.r.t. \(\Delta \mathbf{x}\) evaluated at \(\Delta \mathbf{x} = \mathbf{0}\) (shape:
3 x 3
)- Return type:
np.ndarray
- jacobian_inverse()[source]
Compute the Jacobian of \(p^{-1}\).
- Returns:
The Jacobian of \(p^{-1}\) (shape:
3 x 3
)- Return type:
np.ndarray
- jacobian_self_ominus_other_wrt_other(other)[source]
Compute the Jacobian of \(p_1 \ominus p_2\) w.r.t. \(p_2\).
- Parameters:
other (BasePose) – The pose that is being subtracted from
self
- Returns:
The Jacobian of \(p_1 \ominus p_2\) w.r.t. \(p_2\) (shape:
3 x 3
)- Return type:
np.ndarray
- jacobian_self_ominus_other_wrt_other_compact(other)[source]
Compute the Jacobian of \(p_1 \ominus p_2\) w.r.t. \(p_2\).
- Parameters:
other (BasePose) – The pose that is being subtracted from
self
- Returns:
The Jacobian of \(p_1 \ominus p_2\) w.r.t. \(p_2\) (shape:
3 x 3
)- Return type:
np.ndarray
- jacobian_self_ominus_other_wrt_self(other)[source]
Compute the Jacobian of \(p_1 \ominus p_2\) w.r.t. \(p_1\).
- Parameters:
other (BasePose) – The pose that is being subtracted from
self
- Returns:
The Jacobian of \(p_1 \ominus p_2\) w.r.t. \(p_1\) (shape:
3 x 3
)- Return type:
np.ndarray
- jacobian_self_ominus_other_wrt_self_compact(other)[source]
Compute the Jacobian of \(p_1 \ominus p_2\) w.r.t. \(p_1\).
- Parameters:
other (BasePose) – The pose that is being subtracted from
self
- Returns:
The Jacobian of \(p_1 \ominus p_2\) w.r.t. \(p_1\) (shape:
3 x 3
)- Return type:
np.ndarray
- jacobian_self_oplus_other_wrt_other(other)[source]
Compute the Jacobian of \(p_1 \oplus p_2\) w.r.t. \(p_2\).
- Parameters:
other (BasePose) – The pose that is being added to
self
- Returns:
The Jacobian of \(p_1 \oplus p_2\) w.r.t. \(p_2\) (shape:
3 x 3
)- Return type:
np.ndarray
- jacobian_self_oplus_other_wrt_other_compact(other)[source]
Compute the Jacobian of \(p_1 \oplus p_2\) w.r.t. \(p_2\).
- Parameters:
other (BasePose) – The pose that is being added to
self
- Returns:
The Jacobian of \(p_1 \oplus p_2\) w.r.t. \(p_2\) (shape:
3 x 3
)- Return type:
np.ndarray
- jacobian_self_oplus_other_wrt_self(other)[source]
Compute the Jacobian of \(p_1 \oplus p_2\) w.r.t. \(p_1\).
- Parameters:
other (BasePose) – The pose that is being added to
self
- Returns:
The Jacobian of \(p_1 \oplus p_2\) w.r.t. \(p_1\) (shape:
3 x 3
)- Return type:
np.ndarray
- jacobian_self_oplus_other_wrt_self_compact(other)[source]
Compute the Jacobian of \(p_1 \oplus p_2\) w.r.t. \(p_1\).
- Parameters:
other (BasePose) – The pose that is being added to
self
- Returns:
The Jacobian of \(p_1 \oplus p_2\) w.r.t. \(p_1\) (shape:
3 x 3
)- Return type:
np.ndarray
- jacobian_self_oplus_point_wrt_point(point)[source]
Compute the Jacobian of \(p_1 \oplus p_2\) w.r.t. \(p_2\), where \(p_2\) is a point.
- Parameters:
point (PoseR2) – The point that is being added to
self
- Returns:
The Jacobian of \(p_1 \oplus p_2\) w.r.t. \(p_2\) (shape:
2 x 2
)- Return type:
np.ndarray
- jacobian_self_oplus_point_wrt_self(point)[source]
Compute the Jacobian of \(p_1 \oplus p_2\) w.r.t. \(p_1\), where \(p_2\) is a point.
- Parameters:
point (PoseR2) – The point that is being added to
self
- Returns:
The Jacobian of \(p_1 \oplus p_2\) w.r.t. \(p_1\) (shape:
2 x 3
)- Return type:
np.ndarray
- property orientation
Return the pose’s orientation.
- Returns:
The angle of the pose
- Return type:
float
- property position
Return the pose’s position.
- Returns:
The position portion of the pose
- Return type:
np.ndarray
- to_array()[source]
Return the pose as a numpy array.
- Returns:
The pose as a numpy array
- Return type:
np.ndarray