graphslam.pose.se3 module
Representation of a pose in \(SE(3)\).
- class graphslam.pose.se3.PoseSE3(position, orientation)[source]
Bases:
BasePose
A representation of a pose in \(SE(3)\).
- Parameters:
position (np.ndarray, list) – The position in \(\mathbb{R}^3\)
orientation (np.ndarray, list) – The orientation of the pose as a unit quaternion: \([q_x, q_y, q_z, q_w]\)
- COMPACT_DIMENSIONALITY = 6
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:
7 x 6
)- Return type:
np.ndarray
- jacobian_inverse()[source]
Compute the Jacobian of \(p^{-1}\).
- Returns:
The Jacobian of \(p^{-1}\) (shape:
7 x 7
)- 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:
7 x 7
)- 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:
6 x 7
)- 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:
7 x 7
)- 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:
6 x 7
)- 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) – (Unused) The pose that is being added to
self
- Returns:
The Jacobian of \(p_1 \oplus p_2\) w.r.t. \(p_2\) (shape:
7 x 7
)- 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) – (Unused) The pose that is being added to
self
- Returns:
The Jacobian of \(p_1 \oplus p_2\) w.r.t. \(p_2\) (shape:
6 x 7
)- 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:
7 x 7
)- 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:
6 x 7
)- 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 (PoseR3) – The point 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_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 (PoseR3) – The point that is being added to
self
- Returns:
The Jacobian of \(p_1 \oplus p_2\) w.r.t. \(p_1\) (shape:
3 x 7
)- Return type:
np.ndarray
- property orientation
Return the pose’s orientation.
- Returns:
The pose’s quaternion
- 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