Commit 4b4d3ee9 authored by czb5793's avatar czb5793
Browse files

Analysis for slam data

parent cbf233de
This source diff could not be displayed because it is too large. You can view the blob instead.
This source diff could not be displayed because it is too large. You can view the blob instead.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
"""
Test the performance of the graph optimization
The used dataset_g2o can be found in https://lucacarlone.mit.edu/datasets/
"""
from supervisor.slam.graph.baseclass.Vertex import Vertex
from supervisor.slam.graph.baseclass.Edge import Edge
from supervisor.slam.graph.baseclass.Graph import Graph
from numpy.linalg import inv
from utils.math_util import normalize_angle
from supervisor.slam.graph.vetor2matrix import *
import matplotlib.pyplot as plt
class PoseVertex(Vertex):
def __init__(self, id, pose):
Vertex.__init__(self, pose, None, None)
assert pose.shape[0] == 3
self.id = id
def __str__(self):
x, y, theta = np.squeeze(self.pose)
return "Vertex[id = {0}, pose = {1}]".format(self.id, (x, y, theta))
class PosePoseEdge(Edge):
def __init__(self, id_vertex1, id_vertex2, z, information, list_vertices):
Edge.__init__(self, id_vertex1, id_vertex2, z, information, list_vertices)
def calc_error_vector(self, x1, x2, z):
"""
Calculate the error vector
:param x1: the previous vertex
:param x2: the current vertex
:param z: the actual measurement.
:return: an error vector
"""
Z = v2t(z)
X1 = v2t(x1)
X2 = v2t(x2)
err = t2v(inv(Z) @ inv(X1) @ X2)
return err
def linearize_constraint(self, x1, x2, z):
"""
Calculate the Jacobian matrices w.r.t. the current configuration.
"""
c = cos(x1[2, 0] + z[2, 0])
s = sin(x1[2, 0] + z[2, 0])
tx = x2[0, 0] - x1[0, 0]
ty = x2[1, 0] - x1[1, 0]
A = np.array([[-c, -s, -tx*s+ty*c],
[s, -c, -tx*c-ty*s],
[0, 0, -1]])
B = np.array([[c, s, 0],
[-s, c, 0],
[0, 0, 1]])
e = self.calc_error_vector(x1, x2, z)
return e, A, B
def __str__(self):
x, y, yaw = np.squeeze(self.z)
s = "PPE: id1:{0},id2: {1},z: [{2}, {3}, {4}]".format(self.id1, self.id2, x, y, yaw)
return s
class PoseGraph(Graph):
def __init__(self, vertices, edges):
Graph.__init__(self)
self.vertices = vertices
self.edges = edges
self.vertex_counter = len(vertices)
def normalize_angles(self, vertices):
for v in vertices:
if isinstance(v, PoseVertex):
v.pose[2, 0] = normalize_angle(v.pose[2, 0])
def draw(self):
"""
Visualize the graph
"""
# draw vertices
vertices = []
for v in self.vertices:
x, y = np.squeeze(v.pose[0:2, 0])
vertices.append((x, y))
# draw edges
for e in self.edges:
x1, y1 = np.squeeze(e.vertex1.pose[0:2, 0])
x2, y2 = np.squeeze(e.vertex2.pose[0:2, 0])
plt.plot([x1, x2], [y1, y2], 'k', linewidth=0.5)
num_vertices = len(vertices)
vx, vy = zip(*vertices)
plt.plot(vx, vy, 'ok', label='Vertex ({0})'.format(num_vertices), fillstyle='none')
plt.axis("equal")
plt.show()
def get_hessian(self):
return self.linearize_constraints(self.vertices, self.edges, 0, 1)
def plot_hessian(self):
H, _ = self.get_hessian()
H = np.abs(H.toarray())
H = H / np.max(H)
H[H > 0] = 1
plt.matshow(H, cmap="Greys", fignum=False)
plt.title("Hessian Matrix (Binary)")
plt.show()
def read_data(filename):
"""
Read the g2o file
:param filename: file name
:return: a PoseGraph object
"""
f = open(filename, "r")
vertices = list()
edges = list()
for line in f.readlines():
ele = line.strip().split(" ")
# A vertex
if ele[0] == "VERTEX_SE2":
# id, x, y, theta
i, x, y, yaw = int(ele[1]), float(ele[2]), float(ele[3]), float(ele[4])
pose = np.array([x, y, yaw], dtype=np.float32)[:, np.newaxis]
vertex = PoseVertex(i, pose)
vertices.append(vertex)
# An edge
elif ele[0] == "EDGE_SE2":
i, j = int(ele[1]), int(ele[2]) # identifier of two vertices
x, y, yaw = float(ele[3]), float(ele[4]), float(ele[5]) # actual measurement.
a, b, c, d, e, g = float(ele[6]), float(ele[7]), float(ele[8]), \
float(ele[9]), float(ele[10]), float(ele[11]) # value of information matrix
info = np.array([[a, b, c],
[b, d, e],
[c, e, g]], dtype=np.float32)
z = np.array([x, y, yaw], dtype=np.float32)[:, np.newaxis]
edge = PosePoseEdge(i, j, z, info, vertices)
edges.append(edge)
f.close()
vertices.sort(key = lambda v: v.id)
return PoseGraph(vertices, edges)
if __name__ == "__main__":
#posegraph = read_data("input_INTEL_g2o.g2o")
#posegraph = read_data("input_MITb_g2o.g2o")
posegraph = read_data("input_M3500_g2o.g2o")
#posegraph.graph_optimization(max_iter=30, damp_factor=5)
#posegraph = read_data("input_M3500a_g2o.g2o")
posegraph.graph_optimization(max_iter=16, damp_factor=1)
posegraph.draw()
import matplotlib.pyplot as plt
import numpy as np
import math
"""
This is a script to produce plots visualizing the difference between applying Gaussian noise
to the motion command before executing the motion (translational and rotational velocities)
or to the resulting robot pose after a noise free motion command was executed.
"""
# Fixing random state for reproducibility
np.random.seed(19680801)
N = 1000
vm = 1
wm = 1
alpha = math.pi/2
time = np.arange(0, 1, 0.01)
def pi_2_pi(angle):
return (angle + math.pi) % (2 * math.pi) - math.pi
def motion_model(vs, ws, dt):
x = vs / ws * (np.sin(alpha + dt * ws) - math.sin(alpha))
y = vs / ws * (-np.cos(alpha + dt * ws) + math.cos(alpha))
return x, y
def set_axes(plt):
axes = plt.gca()
axes.set_xlim([-1, 0.2])
axes.set_ylim([0, 1.2])
def noiseBeforeCommand():
set_axes(plt)
v = vm + np.random.normal(scale=.05, size=N)
w = wm + np.random.normal(scale=0.6, size=N)
u = np.vstack((v, w))
x, y = motion_model(v, w, 1)
plt.scatter(x, y, s=.2, c="black")
plt.plot(motion_model(vm, wm, time)[0], motion_model(vm, wm, time)[1])
plt.savefig('noise_before_command.png')
plt.close()
def noiseAfterCommand():
set_axes(plt)
x, y = motion_model(vm, wm, 1)
x = x + np.random.normal(scale=.075, size=N)
y = y + np.random.normal(scale=.075, size=N)
plt.scatter(x, y, s=.2, c="black")
plt.plot(motion_model(vm, wm, time)[0], motion_model(vm, wm, time)[1])
plt.savefig('noise_after_command.png')
plt.close()
noiseBeforeCommand()
noiseAfterCommand()
This diff is collapsed.
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