def pose_callback(
self, msg
): # parses /pose messages into pose vertices and edges between them
# store x,y,theta of new pose; use them to construct g2o SE2 for the pose measurement
x = msg.pose.position.x
y = msg.pose.position.y
theta = msg.pose.orientation.z
se2 = g2o.SE2(x, y, theta) # pose measurement
# se2 = g2o.SE2(0, 0, 0) # pose measurement with zeros; uncomment to test optimization functionality
prev_vertex_id = self.optimizer.get_last_pose_vertex_id(
) # store most recent prior pose vertex id, if any, before adding new vertex (as this will override last vertex id)
v_se2, new_vertex_id = self.optimizer.add_pose_vertex(
se2) # register new pose vertex with optimizer
if prev_vertex_id != -1: # this is not the first pose vertex, so a pose-pose edge can be calculated and added
# calculate delta x,y,theta between prev and new poses; use them to construct g2o SE2 for the edge measurement
delta_x = x - self.last_pose_x
delta_y = y - self.last_pose_y
delta_theta = msg.pose.orientation.z - self.last_pose_theta
delta_se2 = g2o.SE2(delta_x, delta_y,
delta_theta) # pose-pose edge measurement
self.optimizer.add_pose_edge(
[prev_vertex_id, new_vertex_id],
delta_se2) # register the new edge with the optimizer
# update the x,y,theta of the most recent pose vertex, to be used to calculate the next pose-pose edge measurement
self.last_pose_x = x
self.last_pose_y = y
self.last_pose_theta = theta
return
def add_vertex(self, id, x_estimate, fixed=False):
v_se2 = g2o.VertexSE2()
v_se2.set_id(id)
se2 = g2o.SE2(x_estimate, 0, 0)
v_se2.set_estimate(se2)
v_se2.set_fixed(fixed)
self.optimizer.add_vertex(v_se2)
def graph_make(self):
graph = PoseGraphOptimization()
information_matrix_pose = np.identity(3)
information_matrix_scan = np.identity(3)
information_matrix_pose[0][0] = 1 / Q
information_matrix_scan[0][0] = 1 / R
for i in range(len(self.X_list)):
graph.add_vertex(i, g2o.SE2(self.X_list[i], 0, 0))
for j in range(len(self.transition_list)):
graph.add_edge([j, j + 1], g2o.SE2(self.transition_list[j], 0, 0),
information_matrix_pose)
for k in range(len(self.scan_diff_list)):
graph.add_edge([k, k + 1], g2o.SE2(self.scan_diff_list[k], 0, 0),
information_matrix_scan)
return graph
def save_pose_edge(self, id1, id2, dx, dy, info_mtx):
if id1 < 0 or id2 < 0:
return
dtheta = 0
self.slam_file.write("EDGE_SE2 " + str(id1) + " " + str(id2) + " " + str(dx) + " " + str(dy) + " " + str(dtheta))
for i in range(6):
self.slam_file.write(" " + str(info_mtx.item(i)))
self.slam_file.write("\n")
new_info_mtx = np.identity(3)
self.pgo.add_edge([id1, id2], g2o.SE2(dx, dy, dtheta), new_info_mtx)
def pose_callback(self, msg):
if not self.get_initial_pose:
self.get_initial_pose = True
self.initial_pose_x = msg.pose.position.x
self.initial_pose_y = msg.pose.position.y
rotation = (msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(rotation)
self.initial_pose_theta = euler[2]
self.init_time = rospy.get_time()
self.t.append(rospy.get_time() - self.init_time)
x = msg.pose.position.x - self.initial_pose_x
y = msg.pose.position.y = self.initial_pose_y
rotation = (msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(rotation)
theta = euler[2] - self.initial_pose_theta
self.x.append(x)
self.y.append(y)
self.theta.append(theta)
# add pose vertex
pose = g2o.SE2(x, y, theta)
self.graph_optimizer.add_vertex(self.id, pose, 1)
# add edge between poses
if self.id != 1:
relative_pose = [
x - self.last_pose[0], y - self.last_pose[1],
theta - self.last_pose[2]
]
measurement = g2o.SE2(relative_pose[0], relative_pose[1],
relative_pose[2])
self.graph_optimizer.add_edge([self.id, self.id - 1], measurement,
1)
# add edge between pose and landmark
measurement = [self.distance, 0]
self.graph_optimizer.add_edge([self.id, 0], measurement, 2)
self.last_pose = [x, y, theta]
self.id += 1
def add_edge(self, fromVertex, toVertex, dx, information=np.identity(3)):
edge = g2o.EdgeSE2()
for i, v in enumerate([fromVertex, toVertex]):
if isinstance(v, int):
v = self.optimizer.vertex(v)
edge.set_vertex(i, v)
edge.set_measurement(g2o.SE2(dx, 0, 0)) # relative pose
edge.set_information(information)
self.optimizer.add_edge(edge)
def pose_callback(self, msg):
x = msg.pose.position.x
y = msg.pose.position.y
rotation = (msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w)
euler_angle = tf.transformations.euler_from_quaternion(rotation)
th = euler_angle[2]
if ((x != self.last_pose[0]) and (y != self.last_pose[1])):
self.id += 1
pose_ = g2o.SE2(x, y, th)
self.graph_opt.add_vertex(self.id, pose_, False)
ms = [(x - self.last_pose[0]), (y - self.last_pose[1]),
(th - self.last_pose[2])]
measure = g2o.SE2(ms[0], ms[1], ms[2])
if (id > 1):
self.graph_opt.add_edge([self.id, self.id - 1], measure, True)
self.graph_opt.add_edge([self.id, 0], [self.dist_, 0], False)
self.last_pose = [x, y, th]
self.t += 0.1
self.t_lst.append(self.t)
self.x_lst.append(x)
self.y_lst.append(y)
self.th_lst.append(th)
def save_pose_vtx(self, id, x, y, theta):
self.slam_file.write("VERTEX_SE2 " + str(id) + " " + str(x) + " " + str(y) + " " + str(theta) + "\n")
g2o_pose = g2o.SE2(x,y,theta)
self.pgo.add_vertex(id, g2o_pose)
angles = np.radians(index)
converted_scans = np.array(
[np.cos(angles), np.sin(angles)]).T * scans[:, np.newaxis]
lasers.append(np.array(converted_scans))
x = float(tokens[2 + num_readings])
y = float(tokens[3 + num_readings])
theta = float(tokens[4 + num_readings])
odoms.append([x, y, theta])
odoms = np.array(odoms)
lasers = np.array(lasers)
# Starting point
optimizer = pose_graph.PoseGraphOptimization()
pose = np.eye(3)
optimizer.add_vertex(0, g2o.SE2(g2o.Isometry2d(pose)), True)
init_pose = np.eye(3)
vertex_idx = 1
registered_lasers = []
max_x = -float('inf')
max_y = -float('inf')
min_x = float('inf')
min_y = float('inf')
for odom_idx, odom in enumerate(odoms):
# Initialize
if odom_idx == 0:
prev_odom = odom.copy()
prev_idx = 0