Skip to content

Commit

Permalink
Use abstract base classes for edges and poses (and don't subclass num…
Browse files Browse the repository at this point in the history 
…py array) (#39)

* Use base class for edges

* Use abstract base class for poses (and don't subclass numpy.ndarray)

* Use self._array instead of self.to_array()

* Rename _array to _data

* Rename EPSILON to _NUMERICAL_DIFFERENTIATION_EPSILON

* Rename _array -> _data
  • Loading branch information
@JeffLIrion
JeffLIrion committed Nov 4, 2023
1 parent cbce26c commit d2d62b1
Show file tree
Hide file tree
Showing 11 changed files with 97 additions and 303 deletions.
22 changes: 12 additions & 10 deletions graphslam/edge/base_edge.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,16 +5,14 @@
"""


from abc import ABC, abstractmethod

import numpy as np

from graphslam.pose.base_pose import BasePose


#: The difference that will be used for numerical differentiation
EPSILON = 1e-6


class BaseEdge:
class BaseEdge(ABC):
r"""A class for representing edges in Graph SLAM.
Parameters
Expand All @@ -40,12 +38,17 @@ class BaseEdge:
A list of the vertices constrained by the edge
"""

#: The difference that will be used for numerical differentiation
_NUMERICAL_DIFFERENTIATION_EPSILON = 1e-6

def __init__(self, vertex_ids, information, estimate, vertices=None):
self.vertex_ids = vertex_ids
self.information = information
self.estimate = estimate
self.vertices = vertices

@abstractmethod
def calc_error(self):
r"""Calculate the error for the edge: :math:`\mathbf{e}_j \in \mathbb{R}^\bullet`.
Expand All @@ -55,7 +58,6 @@ def calc_error(self):
The error for the edge
"""
raise NotImplementedError

def calc_chi2(self):
r"""Calculate the :math:`\chi^2` error for the edge.
Expand Down Expand Up @@ -141,17 +143,18 @@ def _calc_jacobian(self, err, dim, vertex_index):
for d in range(dim):
# update the pose
delta_pose = np.zeros(dim)
delta_pose[d] = EPSILON
delta_pose[d] = self._NUMERICAL_DIFFERENTIATION_EPSILON
self.vertices[vertex_index].pose += delta_pose

# compute the numerical derivative
jacobian[:, d] = (self.calc_error() - err) / EPSILON
jacobian[:, d] = (self.calc_error() - err) / self._NUMERICAL_DIFFERENTIATION_EPSILON

# restore the pose
self.vertices[vertex_index].pose = p0.copy()

return jacobian

@abstractmethod
def to_g2o(self):
"""Export the edge to the .g2o format.
Expand All @@ -161,8 +164,8 @@ def to_g2o(self):
The edge in .g2o format
"""
raise NotImplementedError

@abstractmethod
def plot(self, color=''):
"""Plot the edge.
Expand All @@ -172,7 +175,6 @@ def plot(self, color=''):
The color that will be used to plot the edge
"""
raise NotImplementedError

def approx_equal(self, other, tol=1e-6):
"""Check whether two edges are approximately equal.
Expand Down
58 changes: 38 additions & 20 deletions graphslam/pose/base_pose.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,15 +4,33 @@
"""

from abc import ABC, abstractmethod

import numpy as np


class BasePose(np.ndarray):
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`
"""

# pylint: disable=arguments-differ
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

@abstractmethod
def copy(self):
"""Return a copy of the pose.
Expand All @@ -22,8 +40,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 @@ -33,8 +51,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 @@ -44,7 +62,6 @@ 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 @@ -62,14 +79,16 @@ def approx_equal(self, other, tol=1e-6):
Whether the two poses are approximately equal
"""
return np.linalg.norm(self.to_array() - other.to_array()) / max(np.linalg.norm(self.to_array()), tol) < tol
# pylint: disable=protected-access
return np.linalg.norm(self._data - other._data) / max(np.linalg.norm(self._data), tol) < tol

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

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

@property
@abstractmethod
def inverse(self):
"""Return the pose's inverse.
Expand All @@ -103,13 +122,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 @@ -124,8 +143,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 @@ -140,7 +159,6 @@ 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 @@ -163,6 +181,7 @@ 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 @@ -177,8 +196,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 @@ -193,8 +212,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 @@ -209,8 +228,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 @@ -225,8 +244,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 @@ -241,8 +260,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 @@ -257,8 +276,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 @@ -273,8 +292,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 @@ -289,8 +308,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 @@ -300,4 +319,3 @@ 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
16 changes: 8 additions & 8 deletions graphslam/pose/r2.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,9 @@ class PoseR2(BasePose):
The position in :math:`\mathbb{R}^2`
"""
def __new__(cls, position):
obj = np.asarray(position, dtype=np.float64).view(cls)
return obj

def __init__(self, position):
super().__init__(position)

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

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

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

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

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

# ======================================================================= #
# #
Expand Down

0 comments on commit d2d62b1

Please sign in to comment.