Commit 05c465a4 authored by czb5793's avatar czb5793
Browse files

Plot the Hessian matrix of the graph

parent ce24bb59
......@@ -119,4 +119,17 @@ class GraphSlamPlotter(SlamPlotter):
alpha=0.8)
def plot_graph(self):
self.slam.graph.draw()
\ No newline at end of file
fig = plt.figure()
fig.add_subplot(121)
self.slam.graph.draw()
plt.title("Graph")
plt.legend()
plt.axis('square')
fig.add_subplot(122)
H, _ = self.slam.get_hessian()
H = np.abs(H)
plt.matshow(H, cmap="Greys", fignum=False)
plt.title("Hessian Matrix")
plt.show()
......@@ -61,6 +61,16 @@ class GraphBasedSLAM(Slam):
return [ (v.pose[0,0], v.pose[1,0]) \
for v in self.graph.get_estimated_pose_vertices()]
def get_hessian(self):
"""
Return the hessian matrix H and the information vector b
:return: H, b
H: the hessian matrix
b: the information vector
"""
H, b = self.graph.get_hessian()
return H.toarray(), b
def get_landmarks(self):
"""
Returns the estimated landmark positions
......
......@@ -98,7 +98,7 @@ class Graph:
preError = np.inf
for i in range(max_iter):
""" linearize the problem """
H, b = self.__linearize_constraints(self.vertices, self.edges, number_fix, damp_factor)
H, b = self.linearize_constraints(self.vertices, self.edges, number_fix, damp_factor)
""" solve sparse matrix """
dx = self.__solve_sparse(H, b)
""" update vertices """
......@@ -142,7 +142,7 @@ class Graph:
plt.axis('square')
plt.show()
def __linearize_constraints(self, vertices, edges, number_fix, damp_factor):
def linearize_constraints(self, vertices, edges, number_fix, damp_factor):
"""
Linearize the problem (global) i.e. compute the hessian matrix and the information vector
:return:
......
......@@ -238,6 +238,13 @@ class PoseLandmarkEdge(Edge):
return meas_rb, info
def calc_error_vector(self, x, lm, z):
"""
Calculate the error vector of the actual and expected measurements
:param x: Robot pose of the graph configuration. (expected)
:param lm: Landmark pose with respect to the graph configuration. (expected)
:param z: Actual measurement. A range-bearing vector, i.e. z == [distance, angle].T
:return:
"""
delta = lm - x[0:2]
distance = math.sqrt(delta[0,0]**2 + delta[1, 0]**2) # expected measurement
angle = math.atan2(delta[1, 0], delta[0, 0])
......@@ -248,10 +255,9 @@ class PoseLandmarkEdge(Edge):
def linearize_constraint(self, x, lm, z):
"""
Compute the error of a pose-landmark constraint.
:param x1: 3x1 vector (x,y,theta) of the robot pose.
:param lm: 2x1 vector (x,y) of the landmark.
:param z: 2x1 vector (x,y) of the measurement, the position of the landmark in.
the coordinate frame of the robot given by the vector x.
:param x1: 3x1 vector [x,y,theta].T of the robot pose.
:param lm: 2x1 vector [x, y].T of the landmark position.
:param z: 2x1 vector [distance, angle].T of the actual measurement
:return:
e 2x1 error of the constraint.
A 2x3 Jacobian wrt x.
......@@ -327,10 +333,12 @@ class LMGraph(Graph):
def normalize_angles(self, vertices):
for v in vertices:
if isinstance(v, PoseVertex):
# v.pose[2, 0] = atan2(sin(v.pose[2, 0]), cos(v.pose[2, 0]))
v.pose[2, 0] = normalize_angle(v.pose[2, 0])
def get_last_pose_vertex(self):
"""
Return the last vertex of poses
"""
v_pose = None
for v in reversed(self.vertices):
if isinstance(v, PoseVertex):
......@@ -339,6 +347,9 @@ class LMGraph(Graph):
return v_pose
def get_estimated_pose_vertices(self):
"""
Return a list of vertices that represent poses
"""
poses = []
for v in self.vertices:
if isinstance(v, PoseVertex):
......@@ -346,18 +357,29 @@ class LMGraph(Graph):
return poses
def get_estimated_landmark_vertices(self):
"""
Return a list of vertices that represent landmarks
"""
poses = []
for v in self.vertices:
if isinstance(v, LandmarkVertex):
poses.append(v)
return poses
def get_hessian(self):
"""
Return the hessian matrix H and the information vector b
:return: H, b
H: the hessian matrix
b: the information vector
"""
return self.linearize_constraints(self.vertices, self.edges, 0, 1)
def draw(self):
"""
Visualize the graph
"""
# draw vertices
plt.cla()
landmarks = []
vertices = []
for v in self.vertices:
......@@ -387,8 +409,3 @@ class LMGraph(Graph):
vertices = sample(vertices, k)
vx, vy = zip(*vertices)
plt.plot(vx, vy, '*r', label='Vertex ({0})'.format(num_vertices))
plt.title("Graph")
plt.legend()
plt.axis('square')
plt.show()
\ No newline at end of file
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