Skip to content

Commit

Permalink
Make poses subclass numpy.ndarray (#42)
Browse files Browse the repository at this point in the history 
* Make poses subclass numpy.ndarray

* Formatting
  • Loading branch information
@JeffLIrion
JeffLIrion committed Nov 4, 2023
1 parent 4b24828 commit 0cec941
Show file tree
Hide file tree
Showing 6 changed files with 208 additions and 62 deletions.
62 changes: 21 additions & 41 deletions graphslam/pose/base_pose.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,33 +4,13 @@
"""

from abc import ABC, abstractmethod

import numpy as np


class BasePose(ABC):
"""A base class for poses.
Parameters
----------
arr : np.ndarray, list
The array that will be stored as `self._data`
dtype : type
The type for the numpy array `self._data`
"""

def __init__(self, arr, dtype=np.float64):
self._data = np.array(arr, dtype=dtype)

def __getitem__(self, key):
return self._data[key]

def __setitem__(self, key, value):
self._data[key] = value
class BasePose(np.ndarray):
"""A base class for poses."""

@abstractmethod
# pylint: disable=arguments-differ
def copy(self):
"""Return a copy of the pose.
Expand All @@ -40,8 +20,8 @@ def copy(self):
A copy of the pose
"""
raise NotImplementedError

@abstractmethod
def to_array(self):
"""Return the pose as a numpy array.
Expand All @@ -51,8 +31,8 @@ def to_array(self):
The pose as a numpy array
"""
raise NotImplementedError

@abstractmethod
def to_compact(self):
"""Return the pose as a compact numpy array.
Expand All @@ -62,6 +42,7 @@ def to_compact(self):
The pose as a compact numpy array
"""
raise NotImplementedError

def approx_equal(self, other, tol=1e-6):
"""Check whether two poses are approximately equal.
Expand All @@ -79,16 +60,14 @@ def approx_equal(self, other, tol=1e-6):
Whether the two poses are approximately equal
"""
# pylint: disable=protected-access
return np.linalg.norm(self._data - other._data) / max(np.linalg.norm(self._data), tol) < tol
return np.linalg.norm(self.to_array() - other.to_array()) / max(np.linalg.norm(self.to_array()), tol) < tol

# ======================================================================= #
# #
# Properties #
# #
# ======================================================================= #
@property
@abstractmethod
def position(self):
"""Return the pose's position.
Expand All @@ -98,9 +77,9 @@ def position(self):
The pose's position
"""
raise NotImplementedError

@property
@abstractmethod
def orientation(self):
"""Return the pose's orientation.
Expand All @@ -110,9 +89,9 @@ def orientation(self):
The pose's orientation
"""
raise NotImplementedError

@property
@abstractmethod
def inverse(self):
"""Return the pose's inverse.
Expand All @@ -122,13 +101,13 @@ def inverse(self):
The pose's inverse
"""
raise NotImplementedError

# ======================================================================= #
# #
# Magic Methods #
# #
# ======================================================================= #
@abstractmethod
def __add__(self, other):
"""Add poses (i.e., pose composition).
Expand All @@ -143,8 +122,8 @@ def __add__(self, other):
The result of pose composition
"""
raise NotImplementedError

@abstractmethod
def __sub__(self, other):
"""Subtract poses (i.e., inverse pose composition).
Expand All @@ -159,6 +138,7 @@ def __sub__(self, other):
The result of inverse pose composition
"""
raise NotImplementedError

def __iadd__(self, other):
"""Add poses in-place (i.e., pose composition).
Expand All @@ -181,7 +161,6 @@ def __iadd__(self, other):
# Jacobians #
# #
# ======================================================================= #
@abstractmethod
def jacobian_self_oplus_other_wrt_self(self, other):
r"""Compute the Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_1`.
Expand All @@ -196,8 +175,8 @@ def jacobian_self_oplus_other_wrt_self(self, other):
The Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_1`.
"""
raise NotImplementedError

@abstractmethod
def jacobian_self_oplus_other_wrt_self_compact(self, other):
r"""Compute the Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_1`.
Expand All @@ -212,8 +191,8 @@ def jacobian_self_oplus_other_wrt_self_compact(self, other):
The Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_1`.
"""
raise NotImplementedError

@abstractmethod
def jacobian_self_oplus_other_wrt_other(self, other):
r"""Compute the Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_2`.
Expand All @@ -228,8 +207,8 @@ def jacobian_self_oplus_other_wrt_other(self, other):
The Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_2`.
"""
raise NotImplementedError

@abstractmethod
def jacobian_self_oplus_other_wrt_other_compact(self, other):
r"""Compute the Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_2`.
Expand All @@ -244,8 +223,8 @@ def jacobian_self_oplus_other_wrt_other_compact(self, other):
The Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_2`.
"""
raise NotImplementedError

@abstractmethod
def jacobian_self_ominus_other_wrt_self(self, other):
r"""Compute the Jacobian of :math:`p_1 \ominus p_2` w.r.t. :math:`p_1`.
Expand All @@ -260,8 +239,8 @@ def jacobian_self_ominus_other_wrt_self(self, other):
The Jacobian of :math:`p_1 \ominus p_2` w.r.t. :math:`p_1`.
"""
raise NotImplementedError

@abstractmethod
def jacobian_self_ominus_other_wrt_self_compact(self, other):
r"""Compute the Jacobian of :math:`p_1 \ominus p_2` w.r.t. :math:`p_1`.
Expand All @@ -276,8 +255,8 @@ def jacobian_self_ominus_other_wrt_self_compact(self, other):
The Jacobian of :math:`p_1 \ominus p_2` w.r.t. :math:`p_1`.
"""
raise NotImplementedError

@abstractmethod
def jacobian_self_ominus_other_wrt_other(self, other):
r"""Compute the Jacobian of :math:`p_1 \ominus p_2` w.r.t. :math:`p_2`.
Expand All @@ -292,8 +271,8 @@ def jacobian_self_ominus_other_wrt_other(self, other):
The Jacobian of :math:`p_1 \ominus p_2` w.r.t. :math:`p_2`.
"""
raise NotImplementedError

@abstractmethod
def jacobian_self_ominus_other_wrt_other_compact(self, other):
r"""Compute the Jacobian of :math:`p_1 \ominus p_2` w.r.t. :math:`p_2`.
Expand All @@ -308,8 +287,8 @@ def jacobian_self_ominus_other_wrt_other_compact(self, other):
The Jacobian of :math:`p_1 \ominus p_2` w.r.t. :math:`p_2`.
"""
raise NotImplementedError

@abstractmethod
def jacobian_boxplus(self):
r"""Compute the Jacobian of :math:`p_1 \boxplus \Delta \mathbf{x}` w.r.t. :math:`\Delta \mathbf{x}` evaluated at :math:`\Delta \mathbf{x} = \mathbf{0}`.
Expand All @@ -319,3 +298,4 @@ def jacobian_boxplus(self):
The Jacobian of :math:`p_1 \boxplus \Delta \mathbf{x}` w.r.t. :math:`\Delta \mathbf{x}` evaluated at :math:`\Delta \mathbf{x} = \mathbf{0}`
"""
raise NotImplementedError
15 changes: 8 additions & 7 deletions graphslam/pose/r2.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,9 @@ class PoseR2(BasePose):
#: The compact dimensionality
COMPACT_DIMENSIONALITY = 2

def __init__(self, position):
super().__init__(position)
def __new__(cls, position):
obj = np.asarray(position, dtype=np.float64).view(cls)
return obj

def copy(self):
"""Return a copy of the pose.
Expand All @@ -45,7 +46,7 @@ def to_array(self):
The pose as a numpy array
"""
return np.array(self._data)
return np.array(self)

def to_compact(self):
"""Return the pose as a compact numpy array.
Expand All @@ -56,7 +57,7 @@ def to_compact(self):
The pose as a compact numpy array
"""
return np.array(self._data)
return np.array(self)

# ======================================================================= #
# #
Expand All @@ -73,7 +74,7 @@ def position(self):
The position portion of the pose
"""
return np.array(self._data)
return np.array(self)

@property
def orientation(self):
Expand Down Expand Up @@ -118,7 +119,7 @@ def __add__(self, other):
The result of pose composition
"""
return PoseR2([self[0] + other[0], self[1] + other[1]])
return PoseR2(np.add(self, other))

def __sub__(self, other):
"""Subtract poses (i.e., inverse pose composition).
Expand All @@ -134,7 +135,7 @@ def __sub__(self, other):
The result of inverse pose composition
"""
return PoseR2([self[0] - other[0], self[1] - other[1]])
return PoseR2(np.subtract(self, other))

# ======================================================================= #
# #
Expand Down
15 changes: 8 additions & 7 deletions graphslam/pose/r3.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,9 @@ class PoseR3(BasePose):
#: The compact dimensionality
COMPACT_DIMENSIONALITY = 3

def __init__(self, position):
super().__init__(position)
def __new__(cls, position):
obj = np.asarray(position, dtype=np.float64).view(cls)
return obj

def copy(self):
"""Return a copy of the pose.
Expand All @@ -45,7 +46,7 @@ def to_array(self):
The pose as a numpy array
"""
return np.array(self._data)
return np.array(self)

def to_compact(self):
"""Return the pose as a compact numpy array.
Expand All @@ -56,7 +57,7 @@ def to_compact(self):
The pose as a compact numpy array
"""
return np.array(self._data)
return np.array(self)

# ======================================================================= #
# #
Expand All @@ -73,7 +74,7 @@ def position(self):
The position portion of the pose
"""
return np.array(self._data)
return np.array(self)

@property
def orientation(self):
Expand Down Expand Up @@ -118,7 +119,7 @@ def __add__(self, other):
The result of pose composition
"""
return PoseR3([self[0] + other[0], self[1] + other[1], self[2] + other[2]])
return PoseR3(np.add(self, other))

def __sub__(self, other):
"""Subtract poses (i.e., inverse pose composition).
Expand All @@ -134,7 +135,7 @@ def __sub__(self, other):
The result of inverse pose composition
"""
return PoseR3([self[0] - other[0], self[1] - other[1], self[2] - other[2]])
return PoseR3(np.subtract(self, other))

# ======================================================================= #
# #
Expand Down
9 changes: 5 additions & 4 deletions graphslam/pose/se2.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,9 @@ class PoseSE2(BasePose):
#: The compact dimensionality
COMPACT_DIMENSIONALITY = 3

def __init__(self, position, orientation):
super().__init__([position[0], position[1], neg_pi_to_pi(orientation)])
def __new__(cls, position, orientation):
obj = np.array([position[0], position[1], neg_pi_to_pi(orientation)], dtype=np.float64).view(cls)
return obj

def copy(self):
"""Return a copy of the pose.
Expand All @@ -51,7 +52,7 @@ def to_array(self):
The pose as a numpy array
"""
return np.array(self._data)
return np.array(self)

def to_compact(self):
"""Return the pose as a compact numpy array.
Expand All @@ -62,7 +63,7 @@ def to_compact(self):
The pose as a compact numpy array
"""
return np.array(self._data)
return np.array(self)

def to_matrix(self):
"""Return the pose as an :math:`SE(2)` matrix.
Expand Down
7 changes: 4 additions & 3 deletions graphslam/pose/se3.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,10 +24,11 @@ class PoseSE3(BasePose):
#: The compact dimensionality
COMPACT_DIMENSIONALITY = 6

def __init__(self, position, orientation):
def __new__(cls, position, orientation):
# fmt: off
super().__init__([position[0], position[1], position[2], orientation[0], orientation[1], orientation[2], orientation[3]])
obj = np.array([position[0], position[1], position[2], orientation[0], orientation[1], orientation[2], orientation[3]], dtype=np.float64).view(cls)
# fmt: on
return obj

def normalize(self):
"""Normalize the quaternion portion of the pose."""
Expand All @@ -54,7 +55,7 @@ def to_array(self):
The pose as a numpy array
"""
return np.array(self._data)
return np.array(self)

def to_compact(self):
"""Return the pose as a compact numpy array.
Expand Down

0 comments on commit 0cec941

Please sign in to comment.