graphslam.pose.r2 module
Representation of a point in \(\mathbb{R}^2\).
- class graphslam.pose.r2.PoseR2(position)[source]
Bases:
BasePose
A representation of a 2-D point.
- Parameters:
position (np.ndarray, list) – The position in \(\mathbb{R}^2\)
- COMPACT_DIMENSIONALITY = 2
The compact dimensionality
- 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:
2 x 2
)- Return type:
np.ndarray
- jacobian_inverse()[source]
Compute the Jacobian of \(p^{-1}\).
- Returns:
The Jacobian of \(p^{-1}\) (shape:
2 x 2
)- 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:
2 x 2
)- 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:
2 x 2
)- 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:
2 x 2
)- 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:
2 x 2
)- 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:
2 x 2
)- 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:
2 x 2
)- 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:
2 x 2
)- 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:
2 x 2
)- 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 2
)- Return type:
np.ndarray
- property orientation
Return the pose’s orientation.
- Returns:
A
PoseR2
object has no orientation, so this will always return 0.- Return type:
float
- property position
Return the pose’s position.
- Returns:
The position portion of the pose
- Return type:
np.ndarray