Skip to content

Commit

Permalink
Apply black formatting (#41)
Browse files Browse the repository at this point in the history 
* Apply black formatting

* WIP

* WIP

* Formatting

* Formatting

* Formatting
  • Loading branch information
@JeffLIrion
JeffLIrion committed Nov 4, 2023
1 parent aa749e9 commit 4b24828
Show file tree
Hide file tree
Showing 21 changed files with 344 additions and 412 deletions.
4 changes: 2 additions & 2 deletions .github/workflows/python-package.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,9 +30,9 @@ jobs:
run: |
python -m pip install --upgrade pip
make venv
- name: Linting checks with pylint, flake8, and (soon) black
- name: Linting checks with pylint, flake8, and black
run: |
make lint-flake8 lint-pylint
make lint
- name: Test with pytest
env:
GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
Expand Down
14 changes: 12 additions & 2 deletions graphslam/edge/base_edge.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,7 +96,13 @@ def calc_chi2_gradient_hessian(self):

jacobians = self.calc_jacobians()

return chi2, {v.gradient_index: np.dot(np.dot(np.transpose(err), self.information), jacobian) for v, jacobian in zip(self.vertices, jacobians)}, {(self.vertices[i].gradient_index, self.vertices[j].gradient_index): np.dot(np.dot(np.transpose(jacobians[i]), self.information), jacobians[j]) for i in range(len(jacobians)) for j in range(i, len(jacobians))}
# fmt: off
return (
chi2,
{v.gradient_index: np.dot(np.dot(np.transpose(err), self.information), jacobian) for v, jacobian in zip(self.vertices, jacobians)},
{(self.vertices[i].gradient_index, self.vertices[j].gradient_index): np.dot(np.dot(np.transpose(jacobians[i]), self.information), jacobians[j]) for i in range(len(jacobians)) for j in range(i, len(jacobians))},
)
# fmt: on

def calc_jacobians(self):
r"""Calculate the Jacobian of the edge's error with respect to each constrained pose.
Expand Down Expand Up @@ -166,7 +172,7 @@ def to_g2o(self):
"""

@abstractmethod
def plot(self, color=''):
def plot(self, color=""):
"""Plot the edge.
Parameters
Expand Down Expand Up @@ -198,10 +204,14 @@ def approx_equal(self, other, tol=1e-6):
if any(v_id1 != v_id2 for v_id1, v_id2 in zip(self.vertex_ids, other.vertex_ids)):
return False

# fmt: off
if self.information.shape != other.information.shape or np.linalg.norm(self.information - other.information) / max(np.linalg.norm(self.information), tol) >= tol:
return False
# fmt: on

if isinstance(self.estimate, BasePose):
return isinstance(other.estimate, BasePose) and self.estimate.approx_equal(other.estimate, tol)

# fmt: off
return not isinstance(other.estimate, BasePose) and np.linalg.norm(self.estimate - other.estimate) / max(np.linalg.norm(self.estimate), tol) < tol
# fmt: onn
6 changes: 5 additions & 1 deletion graphslam/edge/edge_odometry.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,8 +73,10 @@ def calc_jacobians(self):
The Jacobian matrices for the edge with respect to each constrained pose
"""
# fmt: off
return [np.dot(np.dot(self.estimate.jacobian_self_ominus_other_wrt_other_compact(self.vertices[1].pose - self.vertices[0].pose), self.vertices[1].pose.jacobian_self_ominus_other_wrt_other(self.vertices[0].pose)), self.vertices[0].pose.jacobian_boxplus()),
np.dot(np.dot(self.estimate.jacobian_self_ominus_other_wrt_other_compact(self.vertices[1].pose - self.vertices[0].pose), self.vertices[1].pose.jacobian_self_ominus_other_wrt_self(self.vertices[0].pose)), self.vertices[1].pose.jacobian_boxplus())]
# fmt: on

def to_g2o(self):
"""Export the edge to the .g2o format.
Expand All @@ -85,15 +87,17 @@ def to_g2o(self):
The edge in .g2o format
"""
# fmt: off
if isinstance(self.vertices[0].pose, PoseSE2):
return "EDGE_SE2 {} {} {} {} {} ".format(self.vertex_ids[0], self.vertex_ids[1], self.estimate[0], self.estimate[1], self.estimate[2]) + " ".join([str(x) for x in self.information[np.triu_indices(3, 0)]]) + "\n"

if isinstance(self.vertices[0].pose, PoseSE3):
return "EDGE_SE3:QUAT {} {} {} {} {} {} {} {} {} ".format(self.vertex_ids[0], self.vertex_ids[1], self.estimate[0], self.estimate[1], self.estimate[2], self.estimate[3], self.estimate[4], self.estimate[5], self.estimate[6]) + " ".join([str(x) for x in self.information[np.triu_indices(6, 0)]]) + "\n"
# fmt: on

raise NotImplementedError

def plot(self, color='b'):
def plot(self, color="b"):
"""Plot the edge.
Parameters
Expand Down
33 changes: 22 additions & 11 deletions graphslam/graph.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -129,7 +129,7 @@ def __iadd__(self, other):
return other

def __init__(self):
self.chi2 = 0.
self.chi2 = 0.0
self.gradient = defaultdict(_Chi2GradientHessian.DefaultArray)
self.hessian = defaultdict(_Chi2GradientHessian.DefaultArray)

Expand Down Expand Up @@ -187,6 +187,7 @@ class Graph(object):
A list of the vertices in the graph
"""

def __init__(self, edges, vertices):
# The vertices and edges lists
self._edges = edges
Expand All @@ -203,9 +204,7 @@ def __init__(self, edges, vertices):
self._initialize()

def _initialize(self):
"""Fill in the ``vertices`` attributes for the graph's edges, and other necessary preparations.
"""
"""Fill in the ``vertices`` attributes for the graph's edges, and other necessary preparations."""
# Fill in the vertices' `gradient_index` attribute
gradient_index = 0
for v in self._vertices:
Expand Down Expand Up @@ -235,10 +234,10 @@ def calc_chi2(self):
return self._chi2

def _calc_chi2_gradient_hessian(self):
r"""Calculate the :math:`\chi^2` error, the gradient :math:`\mathbf{b}`, and the Hessian :math:`H`.
"""
r"""Calculate the :math:`\chi^2` error, the gradient :math:`\mathbf{b}`, and the Hessian :math:`H`."""
# fmt: off
chi2_gradient_hessian = reduce(_Chi2GradientHessian.update, (e.calc_chi2_gradient_hessian() for e in self._edges), _Chi2GradientHessian())
# fmt: on

self._chi2 = chi2_gradient_hessian.chi2

Expand All @@ -247,7 +246,9 @@ def _calc_chi2_gradient_hessian(self):
for gradient_idx, contrib in chi2_gradient_hessian.gradient.items():
# If a vertex is fixed, its block in the gradient vector is zero and so there is nothing to do
if gradient_idx not in self._fixed_gradient_indices:
# fmt: off
self._gradient[gradient_idx: gradient_idx + len(contrib)] += contrib
# fmt: on

# Fill in the Hessian matrix
self._hessian = lil_matrix((self._len_gradient, self._len_gradient), dtype=np.float64)
Expand All @@ -256,13 +257,19 @@ def _calc_chi2_gradient_hessian(self):
if hessian_row_idx in self._fixed_gradient_indices or hessian_col_idx in self._fixed_gradient_indices:
# For fixed vertices, the diagonal block is the identity matrix and the off-diagonal blocks are zero
if hessian_row_idx == hessian_col_idx:
# fmt: off
self._hessian[hessian_row_idx: hessian_row_idx + dim, hessian_col_idx: hessian_col_idx + dim] = np.eye(dim)
# fmt: on
continue

# fmt: off
self._hessian[hessian_row_idx: hessian_row_idx + dim, hessian_col_idx: hessian_col_idx + dim] = contrib
# fmt: on

if hessian_row_idx != hessian_col_idx:
# fmt: off
self._hessian[hessian_col_idx: hessian_col_idx + dim, hessian_row_idx: hessian_row_idx + dim] = np.transpose(contrib)
# fmt: on

def optimize(self, tol=1e-4, max_iter=20, fix_first_pose=True):
r"""Optimize the :math:`\chi^2` error for the ``Graph``.
Expand All @@ -284,7 +291,7 @@ def optimize(self, tol=1e-4, max_iter=20, fix_first_pose=True):
self._fixed_gradient_indices = {v.gradient_index for v in self._vertices if v.fixed}

# Previous iteration's chi^2 error
chi2_prev = -1.
chi2_prev = -1.0

# For displaying the optimization progress
print("\nIteration chi^2 rel. change")
Expand All @@ -310,7 +317,9 @@ def optimize(self, tol=1e-4, max_iter=20, fix_first_pose=True):

# Apply the updates
for v in self._vertices:
# fmt: off
v.pose += dx[v.gradient_index: v.gradient_index + v.pose.COMPACT_DIMENSIONALITY]
# fmt: on

# If we reached the maximum number of iterations, print out the final iteration's results
self.calc_chi2()
Expand All @@ -326,14 +335,14 @@ def to_g2o(self, outfile):
The path where the graph will be saved
"""
with open(outfile, 'w') as f:
with open(outfile, "w") as f:
for v in self._vertices:
f.write(v.to_g2o())

for e in self._edges:
f.write(e.to_g2o())

def plot(self, vertex_color='r', vertex_marker='o', vertex_markersize=3, edge_color='b', title=None):
def plot(self, vertex_color="r", vertex_marker="o", vertex_markersize=3, edge_color="b", title=None):
"""Plot the graph.
Parameters
Expand All @@ -355,7 +364,7 @@ def plot(self, vertex_color='r', vertex_marker='o', vertex_markersize=3, edge_co

fig = plt.figure()
if len(self._vertices[0].pose.position) == 3:
fig.add_subplot(111, projection='3d')
fig.add_subplot(111, projection="3d")

for e in self._edges:
e.plot(edge_color)
Expand Down Expand Up @@ -388,4 +397,6 @@ def approx_equal(self, other, tol=1e-6):
if len(self._edges) != len(other._edges) or len(self._vertices) != len(other._vertices):
return False

# fmt: off
return all(e1.approx_equal(e2, tol) for e1, e2 in zip(self._edges, other._edges)) and all(v1.pose.approx_equal(v2.pose, tol) for v1, v2 in zip(self._vertices, other._vertices))
# fmt: on
2 changes: 1 addition & 1 deletion graphslam/pose/r2.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,7 @@ def orientation(self):
A ``PoseR2`` object has no orientation, so this will always return 0.
"""
return 0.
return 0.0

@property
def inverse(self):
Expand Down
2 changes: 1 addition & 1 deletion graphslam/pose/r3.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,7 @@ def orientation(self):
A ``PoseR3`` object has no orientation, so this will always return 0.
"""
return 0.
return 0.0

@property
def inverse(self):
Expand Down
43 changes: 39 additions & 4 deletions graphslam/pose/se2.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,12 @@ def to_matrix(self):
The pose as an :math:`SE(2)` matrix
"""
return np.array([[np.cos(self[2]), -np.sin(self[2]), self[0]], [np.sin(self[2]), np.cos(self[2]), self[1]], [0., 0., 1.]], dtype=np.float64)
# fmt: off
return np.array([[np.cos(self[2]), -np.sin(self[2]), self[0]],
[np.sin(self[2]), np.cos(self[2]), self[1]],
[0., 0., 1.]],
dtype=np.float64)
# fmt: on

@classmethod
def from_matrix(cls, matrix):
Expand Down Expand Up @@ -131,7 +136,11 @@ def inverse(self):
The pose's inverse
"""
return PoseSE2([-self[0] * np.cos(self[2]) - self[1] * np.sin(self[2]), self[0] * np.sin(self[2]) - self[1] * np.cos(self[2])], -self[2])
# fmt: off
return PoseSE2([-self[0] * np.cos(self[2]) - self[1] * np.sin(self[2]),
self[0] * np.sin(self[2]) - self[1] * np.cos(self[2])],
-self[2])
# fmt: on

# ======================================================================= #
# #
Expand All @@ -152,7 +161,11 @@ def __add__(self, other):
The result of pose composition
"""
return PoseSE2([self[0] + other[0] * np.cos(self[2]) - other[1] * np.sin(self[2]), self[1] + other[0] * np.sin(self[2]) + other[1] * np.cos(self[2])], neg_pi_to_pi(self[2] + other[2]))
# fmt: off
return PoseSE2([self[0] + other[0] * np.cos(self[2]) - other[1] * np.sin(self[2]),
self[1] + other[0] * np.sin(self[2]) + other[1] * np.cos(self[2])],
neg_pi_to_pi(self[2] + other[2]))
# fmt: on

def __sub__(self, other):
r"""Subtract poses (i.e., inverse pose composition): :math:`p_1 \ominus p_2`.
Expand All @@ -168,7 +181,11 @@ def __sub__(self, other):
The result of inverse pose composition
"""
return PoseSE2([(self[0] - other[0]) * np.cos(other[2]) + (self[1] - other[1]) * np.sin(other[2]), (other[0] - self[0]) * np.sin(other[2]) + (self[1] - other[1]) * np.cos(other[2])], neg_pi_to_pi(self[2] - other[2]))
# fmt: off
return PoseSE2([(self[0] - other[0]) * np.cos(other[2]) + (self[1] - other[1]) * np.sin(other[2]),
(other[0] - self[0]) * np.sin(other[2]) + (self[1] - other[1]) * np.cos(other[2])],
neg_pi_to_pi(self[2] - other[2]))
# fmt: on

# ======================================================================= #
# #
Expand All @@ -189,9 +206,11 @@ def jacobian_self_oplus_other_wrt_self(self, other):
The Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_1`.
"""
# fmt: off
return np.array([[1., 0., -other[0] * np.sin(self[2]) - other[1] * np.cos(self[2])],
[0., 1., other[0] * np.cos(self[2]) - other[1] * np.sin(self[2])],
[0., 0., 1.]])
# fmt: on

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 @@ -207,9 +226,11 @@ 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`.
"""
# fmt: off
return np.array([[1., 0., -other[0] * np.sin(self[2]) - other[1] * np.cos(self[2])],
[0., 1., other[0] * np.cos(self[2]) - other[1] * np.sin(self[2])],
[0., 0., 1.]])
# fmt: on

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 @@ -225,9 +246,11 @@ def jacobian_self_oplus_other_wrt_other(self, other):
The Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_2`.
"""
# fmt: off
return np.array([[np.cos(self[2]), -np.sin(self[2]), 0.],
[np.sin(self[2]), np.cos(self[2]), 0.],
[0., 0., 1.]])
# fmt: on

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 @@ -243,9 +266,11 @@ 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`.
"""
# fmt: off
return np.array([[np.cos(self[2]), -np.sin(self[2]), 0.],
[np.sin(self[2]), np.cos(self[2]), 0.],
[0., 0., 1.]])
# fmt: on

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 @@ -261,9 +286,11 @@ def jacobian_self_ominus_other_wrt_self(self, other):
The Jacobian of :math:`p_1 \ominus p_2` w.r.t. :math:`p_1`.
"""
# fmt: off
return np.array([[np.cos(other[2]), np.sin(other[2]), 0.],
[-np.sin(other[2]), np.cos(other[2]), 0.],
[0., 0., 1.]])
# fmt: on

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 @@ -279,9 +306,11 @@ 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`.
"""
# fmt: off
return np.array([[np.cos(other[2]), np.sin(other[2]), 0.],
[-np.sin(other[2]), np.cos(other[2]), 0.],
[0., 0., 1.]])
# fmt: on

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 @@ -297,9 +326,11 @@ def jacobian_self_ominus_other_wrt_other(self, other):
The Jacobian of :math:`p_1 \ominus p_2` w.r.t. :math:`p_2`.
"""
# fmt: off
return np.array([[-np.cos(other[2]), -np.sin(other[2]), (other[0] - self[0]) * np.sin(other[2]) + (self[1] - other[1]) * np.cos(other[2])],
[np.sin(other[2]), -np.cos(other[2]), (other[0] - self[0]) * np.cos(other[2]) + (other[1] - self[1]) * np.sin(other[2])],
[0., 0., -1.]])
# fmt: on

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 @@ -315,9 +346,11 @@ 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`.
"""
# fmt: off
return np.array([[-np.cos(other[2]), -np.sin(other[2]), (other[0] - self[0]) * np.sin(other[2]) + (self[1] - other[1]) * np.cos(other[2])],
[np.sin(other[2]), -np.cos(other[2]), (other[0] - self[0]) * np.cos(other[2]) + (other[1] - self[1]) * np.sin(other[2])],
[0., 0., -1.]])
# fmt: on

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 @@ -328,6 +361,8 @@ 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}`
"""
# fmt: off
return np.array([[np.cos(self[2]), -np.sin(self[2]), 0.],
[np.sin(self[2]), np.cos(self[2]), 0.],
[0., 0., 1.]])
# fmt: on
Loading

0 comments on commit 4b24828

Please sign in to comment.