Skip to content

Commit

Permalink
Merge 80a5af9 into ea5a29f
Browse files Browse the repository at this point in the history
  • Loading branch information
@JeffLIrion
JeffLIrion committed Feb 22, 2020
2 parents ea5a29f + 80a5af9 commit 0af7b9b
Show file tree
Hide file tree
Showing 10 changed files with 65 additions and 62 deletions.
49 changes: 19 additions & 30 deletions README.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,21 +44,18 @@ SE(3) Dataset
>>> g.calc_chi2()
17425.89298299299
16720.020602489112
>>> g.optimize()
Iteration 1: chi2_prev = 17425.8930, self._chi2 = 2101.3908
Iteration 2: chi2_prev = 2101.3908, self._chi2 = 695.2287
Iteration 3: chi2_prev = 695.2287, self._chi2 = 685.6427
Iteration 4: chi2_prev = 685.6427, self._chi2 = 691.8391
Iteration 5: chi2_prev = 691.8391, self._chi2 = 691.4596
Iteration 6: chi2_prev = 691.4596, self._chi2 = 686.1112
Iteration 7: chi2_prev = 686.1112, self._chi2 = 685.2138
Iteration 8: chi2_prev = 685.2138, self._chi2 = 685.2582
Iteration 9: chi2_prev = 685.2582, self._chi2 = 685.3748
Iteration 10: chi2_prev = 685.3748, self._chi2 = 685.5076
Iteration 11: chi2_prev = 685.5076, self._chi2 = 685.5009
Iteration chi^2 rel. change
--------- ----- -----------
0 16720.0206
1 26.5495 -0.9984
2 1.2712 -0.9521
3 1.2402 -0.0244
4 1.2396 -0.0005
5 1.2395 -0.0001
SE(2) Dataset
Expand All @@ -72,27 +69,19 @@ SE(2) Dataset
>>> g.calc_chi2()
10140102.260977369
7191686.382493544
>>> g.optimize()
Iteration 1: chi2_prev = 10140102.2610, self._chi2 = 20788949397.2203
Iteration 2: chi2_prev = 20788949397.2203, self._chi2 = 16923475.8850
Iteration 3: chi2_prev = 16923475.8850, self._chi2 = 8294793755.7228
Iteration 4: chi2_prev = 8294793755.7228, self._chi2 = 220115513.6180
Iteration 5: chi2_prev = 220115513.6180, self._chi2 = 24117440.3125
Iteration 6: chi2_prev = 24117440.3125, self._chi2 = 1990004.8692
Iteration 7: chi2_prev = 1990004.8692, self._chi2 = 3445068.7836
Iteration 8: chi2_prev = 3445068.7836, self._chi2 = 788043.5452
Iteration 9: chi2_prev = 788043.5452, self._chi2 = 462337.4617
Iteration 10: chi2_prev = 462337.4617, self._chi2 = 183661.3263
Iteration 11: chi2_prev = 183661.3263, self._chi2 = 172777.5398
Iteration 12: chi2_prev = 172777.5398, self._chi2 = 157818.2026
Iteration 13: chi2_prev = 157818.2026, self._chi2 = 158420.4379
Iteration 14: chi2_prev = 158420.4379, self._chi2 = 157013.3727
Iteration 15: chi2_prev = 157013.3727, self._chi2 = 156995.5912
Iteration 16: chi2_prev = 156995.5912, self._chi2 = 156861.0154
Iteration 17: chi2_prev = 156861.0154, self._chi2 = 156857.2851
Iteration chi^2 rel. change
--------- ----- -----------
0 7191686.3825
1 319915276.1284 43.4840
2 124894535.1749 -0.6096
3 338185.8171 -0.9973
4 734.5142 -0.9978
5 215.8405 -0.7061
6 215.8405 -0.0000
References and Links
Expand Down
4 changes: 2 additions & 2 deletions graphslam/edge/base_edge.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,14 +21,14 @@ class BaseEdge:
The IDs of all vertices constrained by this edge
information : np.ndarray
The information matrix :math:`\Omega_j` associated with the edge
estimate : np.ndarray, float
estimate : BasePose, np.ndarray, float
The expected measurement :math:`\mathbf{z}_j`
vertices : list[graphslam.vertex.Vertex], None
A list of the vertices constrained by the edge
Attributes
----------
estimate : np.ndarray, float
estimate : BasePose, np.ndarray, float
The expected measurement :math:`\mathbf{z}_j`
information : np.ndarray
The information matrix :math:`\Omega_j` associated with the edge
Expand Down
10 changes: 5 additions & 5 deletions graphslam/edge/edge_odometry.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ class EdgeOdometry(BaseEdge):
A list of the vertices constrained by the edge
information : np.ndarray
The information matrix :math:`\Omega_j` associated with the edge
estimate : np.ndarray, float
estimate : BasePose
The expected measurement :math:`\mathbf{z}_j`
Attributes
Expand All @@ -28,7 +28,7 @@ class EdgeOdometry(BaseEdge):
A list of the vertices constrained by the edge
information : np.ndarray
The information matrix :math:`\Omega_j` associated with the edge
estimate : np.ndarray, float
estimate : BasePose
The expected measurement :math:`\mathbf{z}_j`
"""
Expand All @@ -47,7 +47,7 @@ def calc_error(self):
The error for the edge
"""
return self.estimate - (self.vertices[1].pose - self.vertices[0].pose).to_compact()
return (self.estimate - (self.vertices[1].pose - self.vertices[0].pose)).to_compact()

def calc_jacobians(self):
r"""Calculate the Jacobian of the edge's error with respect to each constrained pose.
Expand All @@ -63,5 +63,5 @@ def calc_jacobians(self):
The Jacobian matrices for the edge with respect to each constrained pose
"""
return [-np.dot(self.vertices[1].pose.jacobian_self_ominus_other_wrt_other_compact(self.vertices[0].pose), self.vertices[0].pose.jacobian_boxplus()),
-np.dot(self.vertices[1].pose.jacobian_self_ominus_other_wrt_self_compact(self.vertices[0].pose), self.vertices[1].pose.jacobian_boxplus())]
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())]
11 changes: 9 additions & 2 deletions graphslam/graph.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -244,14 +244,21 @@ def optimize(self, tol=1e-4, max_iter=20, fix_first_pose=True):
# Previous iteration's chi^2 error
chi2_prev = -1.

# For displaying the optimization progress
print("Iteration chi^2 rel. change")
print("--------- ----- -----------")

for i in range(max_iter):
self._calc_chi2_gradient_hessian()

# Check for convergence (from the previous iteration); this avoids having to calculate chi^2 twice
if i > 0:
print("Iteration {:2d}: chi2_prev = {:.4f}, self._chi2 = {:.4f}".format(i, chi2_prev, self._chi2))
if self._chi2 < chi2_prev and (chi2_prev - self._chi2) / (chi2_prev + np.finfo(float).eps) < tol:
rel_diff = (chi2_prev - self._chi2) / (chi2_prev + np.finfo(float).eps)
print("{:9d} {:20.4f} {:18.4f}".format(i, self._chi2, -rel_diff))
if self._chi2 < chi2_prev and rel_diff < tol:
return
else:
print("{:9d} {:20.4f}".format(i, self._chi2))

# Update the previous iteration's chi^2 error
chi2_prev = self._chi2
Expand Down
13 changes: 6 additions & 7 deletions graphslam/load.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@
from .graph import Graph
from .pose.se2 import PoseSE2
from .pose.se3 import PoseSE3
from .util import solve_for_edge_dimensionality, upper_triangular_matrix_to_full_matrix
from .util import upper_triangular_matrix_to_full_matrix
from .vertex import Vertex


Expand Down Expand Up @@ -50,10 +50,9 @@ def load_g2o_se2(infile):
if line.startswith("EDGE_SE2"):
numbers = line[9:].split()
arr = np.array([float(number) for number in numbers[2:]], dtype=np.float64)
n = solve_for_edge_dimensionality(len(arr) - 2)
vertex_ids = [int(numbers[0]), int(numbers[1])]
estimate = arr[:n]
information = upper_triangular_matrix_to_full_matrix(arr[n:], n)
estimate = PoseSE2(arr[:2], arr[2])
information = upper_triangular_matrix_to_full_matrix(arr[3:], 3)
e = EdgeOdometry(vertex_ids, information, estimate)
edges.append(e)
continue
Expand Down Expand Up @@ -94,10 +93,10 @@ def load_g2o_se3(infile):
if line.startswith("EDGE_SE3:QUAT"):
numbers = line[14:].split()
arr = np.array([float(number) for number in numbers[2:]], dtype=np.float64)
n = 6
vertex_ids = [int(numbers[0]), int(numbers[1])]
estimate = arr[:n]
information = upper_triangular_matrix_to_full_matrix(arr[-21:], n)
estimate = PoseSE3(arr[:3], arr[3:7])
estimate.normalize()
information = upper_triangular_matrix_to_full_matrix(arr[7:], 6)
e = EdgeOdometry(vertex_ids, information, estimate)
edges.append(e)
continue
Expand Down
8 changes: 5 additions & 3 deletions graphslam/pose/se2.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,8 @@

from .base_pose import BasePose

from ..util import neg_pi_to_pi


class PoseSE2(BasePose):
r"""A representation of a pose in :math:`SE(2)`.
Expand All @@ -23,7 +25,7 @@ class PoseSE2(BasePose):
"""
def __new__(cls, position, orientation):
obj = np.array([position[0], position[1], orientation], dtype=np.float64).view(cls)
obj = np.array([position[0], position[1], neg_pi_to_pi(orientation)], dtype=np.float64).view(cls)
return obj

def copy(self):
Expand Down Expand Up @@ -147,7 +149,7 @@ 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])], self[2] + other[2])
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]))

def __sub__(self, other):
r"""Subtract poses (i.e., inverse pose composition): :math:`p_1 \ominus p_2`.
Expand All @@ -163,7 +165,7 @@ 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])], self[2] - other[2])
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]))

# ======================================================================= #
# #
Expand Down
3 changes: 2 additions & 1 deletion graphslam/pose/se3.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,8 @@ def normalize(self):
"""Normalize the quaternion portion of the pose.
"""
self[3:] /= np.linalg.norm(self[3:])
sgn = 1. if self[6] >= 0. else -1.
self[3:] /= (sgn * np.linalg.norm(self[3:]))

def copy(self):
"""Return a copy of the pose.
Expand Down
6 changes: 4 additions & 2 deletions tests/test_base_edge.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,11 +68,12 @@ def test_calc_jacobians(self):
"""
p1 = PoseR2([1, 2])
p2 = PoseR2([3, 4])
estimate = PoseR2([0, 0])

v1 = Vertex(1, p1)
v2 = Vertex(2, p2)

e = EdgeOdometry([1, 2], np.eye(2), 0, [v1, v2])
e = EdgeOdometry([1, 2], np.eye(2), estimate, [v1, v2])

jacobians = e.calc_jacobians()

Expand All @@ -85,11 +86,12 @@ def test_calc_chi2_gradient_hessian(self):
"""
p1 = PoseR2([1, 3])
p2 = PoseR2([2, 4])
estimate = PoseR2([0, 0])

v1 = Vertex(0, p1, 0)
v2 = Vertex(1, p2, 1)

e = EdgeOdometry([0, 1], np.eye(2), 0, [v1, v2])
e = EdgeOdometry([0, 1], np.eye(2), estimate, [v1, v2])

chi2, gradient, hessian = e.calc_chi2_gradient_hessian()

Expand Down
20 changes: 12 additions & 8 deletions tests/test_graph.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,13 +32,14 @@ def setUp(self):
p1 = PoseR2(np.random.random_sample(2))
p2 = PoseR2(np.random.random_sample(2))
p3 = PoseR2(np.random.random_sample(2))
estimate = PoseR2([0, 0])

v1 = Vertex(1, p1)
v2 = Vertex(2, p2)
v3 = Vertex(3, p3)

e1 = EdgeOdometry([1, 2], np.eye(2), np.zeros(2), [v1, v2])
e2 = EdgeOdometry([3, 2], 2 * np.eye(2), np.zeros(2), [v3, v2])
e1 = EdgeOdometry([1, 2], np.eye(2), estimate, [v1, v2])
e2 = EdgeOdometry([3, 2], 2 * np.eye(2), estimate, [v3, v2])

self.g = Graph([e1, e2], [v1, v2, v3])

Expand Down Expand Up @@ -78,13 +79,14 @@ def setUp(self):
p1 = PoseR3(np.random.random_sample(3))
p2 = PoseR3(np.random.random_sample(3))
p3 = PoseR3(np.random.random_sample(3))
estimate = PoseR3([0, 0, 0])

v1 = Vertex(1, p1)
v2 = Vertex(2, p2)
v3 = Vertex(3, p3)

e1 = EdgeOdometry([1, 2], np.eye(3), np.zeros(3), [v1, v2])
e2 = EdgeOdometry([3, 2], 2 * np.eye(3), np.zeros(3), [v3, v2])
e1 = EdgeOdometry([1, 2], np.eye(3), estimate, [v1, v2])
e2 = EdgeOdometry([3, 2], 2 * np.eye(3), estimate, [v3, v2])

self.g = Graph([e1, e2], [v1, v2, v3])

Expand All @@ -103,13 +105,14 @@ def setUp(self):
p1 = PoseSE2(np.random.random_sample(2), np.random.random_sample())
p2 = PoseSE2(np.random.random_sample(2), np.random.random_sample())
p3 = PoseSE2(np.random.random_sample(2), np.random.random_sample())
estimate = PoseSE2([0, 0], 0.)

v1 = Vertex(1, p1)
v2 = Vertex(2, p2)
v3 = Vertex(3, p3)

e1 = EdgeOdometry([1, 2], np.eye(3), np.zeros(3), [v1, v2])
e2 = EdgeOdometry([3, 2], 2 * np.eye(3), np.zeros(3), [v3, v2])
e1 = EdgeOdometry([1, 2], np.eye(3), estimate, [v1, v2])
e2 = EdgeOdometry([3, 2], 2 * np.eye(3), estimate, [v3, v2])

self.g = Graph([e1, e2], [v1, v2, v3])

Expand All @@ -128,6 +131,7 @@ def setUp(self):
p1 = PoseSE3(np.random.random_sample(3), np.random.random_sample(4))
p2 = PoseSE3(np.random.random_sample(3), np.random.random_sample(4))
p3 = PoseSE3(np.random.random_sample(3), np.random.random_sample(4))
estimate = PoseSE3([0, 0, 0], [0, 0, 0, 1])

p1.normalize()
p2.normalize()
Expand All @@ -137,8 +141,8 @@ def setUp(self):
v2 = Vertex(2, p2)
v3 = Vertex(3, p3)

e1 = EdgeOdometry([1, 2], np.eye(6), np.zeros(6), [v1, v2])
e2 = EdgeOdometry([3, 2], 2 * np.eye(6), np.zeros(6), [v3, v2])
e1 = EdgeOdometry([1, 2], np.eye(6), estimate, [v1, v2])
e2 = EdgeOdometry([3, 2], 2 * np.eye(6), estimate, [v3, v2])

self.g = Graph([e1, e2], [v1, v2, v3])

Expand Down
3 changes: 1 addition & 2 deletions tests/test_se3.g2o
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,4 @@
VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
VERTEX_SE3:QUAT 1 0.000000 1.000000 2.000000 0.500000 0.500000 0.500000 0.500000
sldfkjEDGE_SE3:QUAT 0 1 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1.00000 1.00000 1.00000 1.00000 1.00000 1.00000
EDGE_SE3:QUAT 0 1 4.15448 -0.0665288 0.000389663 -0.0107791 0.00867285 -0.00190021 0.999902 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 4.00073 -0.000375887 0.0691425 3.9997 -8.5017e-05 4.00118
EDGE_SE3:QUAT 0 1 3.000000 4.000000 5.000000 0.500000 0.500000 0.500000 0.500000 1.000000 2.000000 3.000000 4.000000 5.000000 6.000000 7.000000 8.000000 9.000000 10.000000 11.000000 12.000000 13.000000 14.000000 15.000000 16.000000 17.000000 18.000000 19.000000 20.000000 21.000000
This line is not supported

0 comments on commit 0af7b9b

Please sign in to comment.