Commit 142c2a3a authored by czb5793's avatar czb5793
Browse files

small changes

parent 133d1a76
......@@ -24,7 +24,7 @@ from gi.repository import Gtk as gtk
from gui.Frame import Frame
from gui.Painter import Painter
from matplotlib import pyplot as plt
# user response codes for file chooser dialog buttons
LS_DIALOG_RESPONSE_CANCEL = 1
LS_DIALOG_RESPONSE_ACCEPT = 2
......@@ -389,6 +389,7 @@ class Viewer:
Callback function that handles a click on the "Slam evaluation" button
:param widget: The corresponding widget
"""
if self.simulator.ekfslam_evaluation is not None:
self.simulator.ekfslam_evaluation.plot()
if self.simulator.fastslam_evaluation is not None:
......@@ -396,6 +397,7 @@ class Viewer:
if self.simulator.graphbasedslam_evaluation is not None:
self.simulator.graphbasedslam_evaluation.plot()
def on_plot_covariances(self, widget):
"""
Callback function that handles a click on the "Plot covariances" button
......
......@@ -236,12 +236,13 @@ class Supervisor:
if self.graphbasedslam is not None:
self.graphbasedslam.update(motion_command, zip(measured_distances, sensor_angles, landmark_ids))
# update mappings
if self.ekfslam_mapping is not None:
self.ekfslam_mapping.update(zip(measured_distances, sensor_angles))
if self.fastslam_mapping is not None:
self.fastslam_mapping.update(zip(measured_distances, sensor_angles))
if self.graphbasedslam_mapping is not None:
self.graphbasedslam_mapping.update(zip(measured_distances, sensor_angles))
if self.time > 500:
if self.ekfslam_mapping is not None:
self.ekfslam_mapping.update(zip(measured_distances, sensor_angles))
if self.fastslam_mapping is not None:
self.fastslam_mapping.update(zip(measured_distances, sensor_angles))
if self.graphbasedslam_mapping is not None:
self.graphbasedslam_mapping.update(zip(measured_distances, sensor_angles))
def _send_robot_commands(self):
"""
......
......@@ -151,22 +151,22 @@ class Graph:
"""
indices, hess_size = self.get_block_index_(vertices) # calculate indices for each block of the hessian matrix
b = np.zeros((hess_size, 1), dtype=np.float32) # information vector
Hessian_data = [] # store data of hessian matrix
row_indices = [] # row indices of the corresponding values in H matrix
col_indices = [] # column indices of the corresponding values in H matrix
Hessian_data = list() # a list of block matrices
row_indices = list() # row indices of the corresponding values in H matrix
col_indices = list() # column indices of the corresponding values in H matrix
for edge in edges:
err, A, B = edge.linearize() # calculate error and jacobian matrix
omega = edge.information
""" Compute the block matrices and vectors """
b1 = (err.T @ omega @ A).T
b2 = (err.T @ omega @ B).T
Hii = A.T @ omega @ A
Hjj = B.T @ omega @ B
Hij = A.T @ omega @ B
Hji = B.T @ omega @ A # Hij.T
Hji = B.T @ omega @ A
# Update hessian matrix
""" Compute the hessian matrix and vector """
i1, i2 = indices[edge.id1]
j1, j2 = indices[edge.id2]
b[i1:i2, :] += b1
......@@ -178,7 +178,7 @@ class Graph:
H[i1:i2, j1:j2] += Hij
H[j1:j2, i1:i2] += Hji
"""
# calculate indices for block Hessian
""" Calculate the indices of the block in the Hessian """
inxii_1, inxii_2 = self.cartesian_product(i1, i2, i1, i2)
inxjj_1, inxjj_2 = self.cartesian_product(j1, j2, j1, j2)
inxij_1, inxij_2 = self.cartesian_product(i1, i2, j1, j2)
......@@ -188,13 +188,13 @@ class Graph:
Hessian_data += (Hii.flatten().tolist() + Hjj.flatten().tolist()
+ Hij.flatten().tolist() + Hji.flatten().tolist())
""" add dampling factor """
""" Add damping factor """
I = [damp_factor]*number_fix
ii = [i for i in range(number_fix)]
Hessian_data += I
row_indices += ii
col_indices += ii
""" Create a sparse hessian matrix in a efficient way. """
""" Create a sparse hessian matrix and store it by a memory efficient representation. """
H = coo_matrix((Hessian_data, (row_indices, col_indices)), shape=(hess_size, hess_size), dtype=np.float32)
return H, b
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment