Commit 3f271c47 authored by czb5793's avatar czb5793
Browse files

Small changes

parent 858512df
......@@ -110,6 +110,13 @@ class PoseGraph(Graph):
plt.title("Hessian Matrix (Binary)")
plt.show()
def find_vertex_by_id(id, vertices):
for v in vertices:
if id == v.id:
return v
return None
def read_data(filename):
"""
Read the g2o file
......@@ -140,7 +147,9 @@ def read_data(filename):
[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)
vi = find_vertex_by_id(i, vertices)
vj = find_vertex_by_id(j, vertices)
edge = PosePoseEdge(vi, vj, z, info)
edges.append(edge)
f.close()
......
......@@ -29,8 +29,8 @@ class EKFSlam(Slam):
self.distance_threshold = slam_cfg["ekf_slam"]["distance_threshold"]
self.robot_state_size = slam_cfg["robot_state_size"]
self.landmark_state_size = slam_cfg["landmark_state_size"]
self.sensor_noise = np.diag([slam_cfg["sensor_noise"]["detected_distance"],
np.deg2rad(slam_cfg["sensor_noise"]["detected_angle"])]) ** 2
self.sensor_noise = np.diag([slam_cfg["ekf_slam"]["sensor_noise"]["detected_distance"],
np.deg2rad(slam_cfg["ekf_slam"]["sensor_noise"]["detected_angle"])]) ** 2
self.motion_noise = np.diag([slam_cfg["ekf_slam"]["motion_noise"]["x"],
slam_cfg["ekf_slam"]["motion_noise"]["y"],
np.deg2rad(slam_cfg["ekf_slam"]["motion_noise"]["theta"])]) ** 2
......
......@@ -61,8 +61,8 @@ class FastSlam(Slam):
self.n_particles = slam_cfg["fast_slam"]["n_particles"]
self.robot_state_size = slam_cfg["robot_state_size"]
self.landmark_state_size = slam_cfg["landmark_state_size"]
self.sensor_noise = np.diag([slam_cfg["sensor_noise"]["detected_distance"],
np.deg2rad(slam_cfg["sensor_noise"]["detected_angle"])]) ** 2
self.sensor_noise = np.diag([slam_cfg["fast_slam"]["sensor_noise"]["detected_distance"],
np.deg2rad(slam_cfg["fast_slam"]["sensor_noise"]["detected_angle"])]) ** 2
self.motion_noise = np.diag([slam_cfg["fast_slam"]["motion_noise"]["translational_velocity"],
slam_cfg["fast_slam"]["motion_noise"]["rotational_velocity"]]) ** 2
self.landmark_correspondence_given = slam_cfg["feature_detector"]
......@@ -301,6 +301,13 @@ class FastSlam(Slam):
return particle
def get_particles(self):
"""
Get position and weight of particles.
:return:
"""
return [(p.x, p.y, p.w) for p in self.particles]
@staticmethod
def compute_importance_factor(innovation, Psi):
"""
......
......@@ -32,6 +32,8 @@ class GraphBasedSLAM(Slam):
self.frontend_pose_density = slam_cfg["graph_based_slam"]["frontend_pose_density"]
self.frontend_interval = slam_cfg["graph_based_slam"]['frontend_interval'] # the timestep interval of executing the frontend part.
self.num_fixed_vertices = slam_cfg["graph_based_slam"]["num_fixed_vertexes"]
# solver
self.solver = slam_cfg["graph_based_slam"]['solver'].lower()
# The estimated combined state vector, initially containing the robot pose at the origin and no landmarks
self.mu = np.zeros((self.robot_state_size, 1))
self.Sigma = np.zeros((self.robot_state_size, self.robot_state_size)) # The state covariance, initially set to absolute certainty of the initial robot pose
......@@ -44,6 +46,8 @@ class GraphBasedSLAM(Slam):
# add the first node to the graph
vertex1 = PoseVertex(self.mu, np.eye(3))
self.graph.add_vertex(vertex1)
self.counter = 0
def get_estimated_pose(self):
......@@ -88,6 +92,7 @@ class GraphBasedSLAM(Slam):
:param u: Motion command, A single item is a vector of [[translational_velocity], [angular_velocity]]
:param z: List of measurements. A single item is a tuple of (range, angle, landmark_id)
"""
self.counter += 1
start_time = time.time()
self.mu = self.motion_model(self.mu, u, self.dt) # Calculate next step
......@@ -100,7 +105,7 @@ class GraphBasedSLAM(Slam):
self.__front_end(z)
num_vertices = len(self.graph.vertices) # #vertices = #poses + #landmarks
if num_vertices > 0 and self.flg_optim == True:
print ("Graph-based SLAM starts the backend-optimization with " +
print (str(self.counter) + ": " + "Graph-based SLAM starts the backend-optimization with " +
str(num_vertices) + " vertices")
self.flg_optim = False
self.__back_end()
......@@ -163,7 +168,7 @@ class GraphBasedSLAM(Slam):
for i in range(self.num_fixed_vertices): # fix the first 20 vertices
fix_hessian += self.graph.vertices[i].dim
self.graph.graph_optimization(number_fix=fix_hessian, damp_factor=5, solver="cholesky")
self.graph.graph_optimization(number_fix=fix_hessian, damp_factor=5, solver=self.solver)
last_vertex = self.graph.get_last_pose_vertex()
self.mu = np.copy(last_vertex.pose) # update current state
self.Sigma = np.copy(last_vertex.sigma)
......
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