Skip to content

Commit

Permalink
WIP
Browse files Browse the repository at this point in the history
  • Loading branch information
@JeffLIrion
JeffLIrion committed Nov 4, 2023
1 parent dee98e7 commit 3c87051
Show file tree
Hide file tree
Showing 7 changed files with 99 additions and 24 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
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
26 changes: 26 additions & 0 deletions graphslam/pose/se2.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,9 @@ def to_matrix(self):
The pose as an :math:`SE(2)` matrix
"""
# 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 +133,9 @@ def inverse(self):
The pose's inverse
"""
# 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 +156,9 @@ def __add__(self, other):
The result of pose composition
"""
# 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 +174,9 @@ def __sub__(self, other):
The result of inverse pose composition
"""
# 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 +197,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 +217,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 +237,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 +257,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 +277,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 +297,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 +317,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 +337,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 +352,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 3c87051

Please sign in to comment.