def __init__(self):
rospy.init_node("graphoptimizer")
self.posegraph = PoseGraph()
self.rate = rospy.Rate(hz=RATE_HZ)
self.pose_sub = rospy.Subscriber("pose",
PoseStamped,
self.on_pose,
queue_size=1)
self.scan_sub = rospy.Subscriber("scan",
LaserScan,
self.on_scan,
queue_size=1)
self.factorize_landmark_edges = rospy.get_param(
"~factorize_landmark_edges", default=True)
self.vertex_seq = WALL_VERTEX_ID # landmark has id 1. The pose vertices will start with id of 2
self.prev_pose = None
self.last_msg_time = rospy.Time.now()
self.has_wall_vertex = False
self.prev_wallconstraint_vertexid = None
self.landmark_key_vertices = []
self.landmark_vertex_measurements = []
def __init__(self, raw_data, gui):
self.gui = gui
self.costmap = CostMap()
x = base_x_
y = base_y_
yaw = base_yaw_
self.scan_base = np.array(
[[np.cos(base_yaw_), -np.sin(base_yaw_), base_x_],
[np.sin(base_yaw_), np.cos(base_yaw_), base_y_], [0., 0., 1.]])
self.pg = PoseGraph() # LS-SLAM pose graph
self.node_scan = self.readdata(raw_data)
print("Generating graph...")
self.creategraph(self.node_scan)
print("Graph OK.")
# Show graph before optimization
self.show_graph()
self.show_pointcloud()
import posegraph
import time
from utils import *
reload(posegraph)
from posegraph import PoseGraph
import matplotlib.pyplot as plt
# Specify files to load
vfile = 'data/killian-v.dat'
efile = 'data/killian-e.dat'
pg = PoseGraph()
pg.readGraph(vfile, efile)
"""
pg.readGraph('data/goalpoints_toro_result.graph')
cols = [(np.random.random(), np.random.random(), np.random.random()) for i in range(1000)]
coords = pg.nodes[:, :2]
uncertainties, path = get_uncertainties_and_path(coords)
for j in range(len(path)):
plt.scatter(pg.nodes[j, 0], pg.nodes[j, 1], s=1+np.min((200, 1000*(uncertainties[j][2][2])/ephi**2)))
for e in [[20,8], [36,20], [98,36], [102,138], [112,128], [127,113]]:
plt.scatter(pg.nodes[e[0], 0], pg.nodes[e[0], 1], s=80, c=cols[e[0]])
plt.text(pg.nodes[e[0], 0]+(np.random.random()*1-.5), pg.nodes[e[0], 1]+(np.random.random()*1-.5), str(e[0]))
plt.scatter(pg.nodes[e[1], 0], pg.nodes[e[1], 1], s=80, c=cols[e[0]])
plt.text(pg.nodes[e[1], 0]+(np.random.random()*1-.5), pg.nodes[e[1], 1]+(np.random.random()*1-.5), str(e[1]))
plt.show()
"""
plt.ion()
plt.figure()
class PA3Optimizer:
def __init__(self):
rospy.init_node("graphoptimizer")
self.posegraph = PoseGraph()
self.rate = rospy.Rate(hz=RATE_HZ)
self.pose_sub = rospy.Subscriber("pose",
PoseStamped,
self.on_pose,
queue_size=1)
self.scan_sub = rospy.Subscriber("scan",
LaserScan,
self.on_scan,
queue_size=1)
self.factorize_landmark_edges = rospy.get_param(
"~factorize_landmark_edges", default=True)
self.vertex_seq = WALL_VERTEX_ID # landmark has id 1. The pose vertices will start with id of 2
self.prev_pose = None
self.last_msg_time = rospy.Time.now()
self.has_wall_vertex = False
self.prev_wallconstraint_vertexid = None
self.landmark_key_vertices = []
self.landmark_vertex_measurements = []
def on_pose(self, posemsg):
"""Handles a pose message by adding a vertex (& odometry edge) to the graph optimizer
Arguments:
posemsg {PoseStamped} -- A newly recorded pose in the system
"""
self.vertex_seq += 1
self.posegraph.add_vertex(self.vertex_seq, posemsg.pose.position.x)
rospy.loginfo("POSE #{0: 2d} @ {1: .3f}".format(
self.vertex_seq, posemsg.pose.position.x))
if self.prev_pose is not None:
dx = posemsg.pose.position.x - self.prev_pose.position.x
info_matrix = np.identity(3)
info_matrix[0][0] = WHEEL_ODOMETRY_INFORMATION
# Add an edge from the previous pose to the current one)
self.posegraph.add_edge(fromVertex=self.vertex_seq - 1,
toVertex=self.vertex_seq,
dx=dx,
information=info_matrix)
rospy.loginfo(" => dx: {0:0.3f}".format(dx))
# TODO: Add to the output file
self.prev_pose = posemsg.pose
self.last_msg_time = rospy.Time.now()
def on_scan(self, scanmsg):
"""Handles a scan message by adding a landmark edge to the graph optimizer
Arguments:
scanmsg {LaserScan} -- A new scan reading
"""
if self.prev_pose is None:
rospy.logwarn("Ignoring scan b/c of lack of prev pose")
# Can't provide an estimate to the wall position until we get our first pose measurement.
return
if self.prev_wallconstraint_vertexid is self.vertex_seq:
rospy.logwarn(
"Ignoring scan b/c we already encoded a constraint to the current vertex"
)
return
# Note: for the robot, the first element in the laser scan message faces forward
x_dist = scanmsg.ranges[0]
if np.isinf(x_dist):
rospy.logwarn("Ignoring scan b/c of infinite measurement")
return
x_std_dev = x_dist * SCAN_STDDEV_PERCENTAGE
x_information = 1.0 / x_std_dev
x_pos_estimate = self.prev_pose.position.x + x_dist
if self.factorize_landmark_edges:
self.addscan_factored(x_pos_estimate, x_information, x_dist)
else:
self.addscan_nonfactored(x_pos_estimate, x_information, x_dist)
def addscan_nonfactored(self, x_pos_estimate, x_information, x_dist):
if not self.has_wall_vertex:
self.posegraph.add_landmark(WALL_VERTEX_ID, x_pos_estimate)
self.has_wall_vertex = True
rospy.loginfo(
"Created landmark node at estimated position {0: .4f}".format(
x_pos_estimate))
info_matrix = np.identity(2)
info_matrix[0][0] = x_information
self.posegraph.add_landmark_edge( \
vertex_id=self.vertex_seq, \
landmark_id=WALL_VERTEX_ID, \
dx=x_dist, \
information=info_matrix)
self.prev_wallconstraint_vertexid = self.vertex_seq
self.last_msg_time = rospy.Time.now()
rospy.loginfo(
" {0} to WALL: range {1: .4f} est pos:{2: .4f}".format(
self.vertex_seq, x_dist, x_pos_estimate))
def addscan_factored(self, x_pos_estimate, x_information, x_dist):
if self.prev_wallconstraint_vertexid is not None and \
self.vertex_seq - self.prev_wallconstraint_vertexid < LANDMARK_FACTORIZATION_QUOTIENT:
# This isn't a key vertex (one that will have landmark constraints added to it)
return
# Add an edge between current vertex and all existing key vertices
for v, v_x_dist in zip(self.landmark_key_vertices,
self.landmark_vertex_measurements):
dx = v_x_dist - x_dist
info_matrix = np.identity(3)
info_matrix[0][
0] = x_information # TODO: this should perhaps be an average of information value of current and previous scan
# Add an edge from the previous pose to the current one)
self.posegraph.add_edge(fromVertex=v,
toVertex=self.vertex_seq,
dx=dx,
information=info_matrix)
rospy.loginfo(
"LANDMARK-FACTORIZED: #{0: 2d} landmark_x_dist:{1: .4f}".format(
self.vertex_seq, x_dist))
self.landmark_vertex_measurements.append(x_dist)
self.landmark_key_vertices.append(self.vertex_seq)
self.prev_wallconstraint_vertexid = self.vertex_seq
def spin(self):
# Ensure we have at least 1 vertex (e.g. that the rosbag has started playing),
# and then wait a bit after the last message before starting the optimization routine
while not rospy.is_shutdown() and \
(self.vertex_seq == WALL_VERTEX_ID or rospy.Time.now() - self.last_msg_time < WAIT_TIME_BEFORE_OPTIMIZE):
self.rate.sleep()
rospy.loginfo("No new messages in {0}. Moving on".format(
WAIT_TIME_BEFORE_OPTIMIZE))
self.optimize()
def optimize(self):
self.pose_sub.unregister()
self.scan_sub.unregister()
preop_file = rospy.get_param("~preoptimization_output")
rospy.loginfo(
"Graph construction complete. Printing graph to {0}".format(
preop_file))
self.posegraph.save(preop_file)
self.posegraph.optimize()
postop_file = rospy.get_param("~postoptimization_output")
rospy.loginfo(
"Optimization complete. Printing graph to {0}".format(postop_file))
self.posegraph.save(postop_file)
class graphSLAM():
def __init__(self, raw_data, gui):
self.gui = gui
self.costmap = CostMap()
x = base_x_
y = base_y_
yaw = base_yaw_
self.scan_base = np.array(
[[np.cos(base_yaw_), -np.sin(base_yaw_), base_x_],
[np.sin(base_yaw_), np.cos(base_yaw_), base_y_], [0., 0., 1.]])
self.pg = PoseGraph() # LS-SLAM pose graph
self.node_scan = self.readdata(raw_data)
print("Generating graph...")
self.creategraph(self.node_scan)
print("Graph OK.")
# Show graph before optimization
self.show_graph()
self.show_pointcloud()
#print("waiting create map ...")
#self.show_map()
#self.gui.setpcd(pos, adj)
def show_graph(self):
adj = np.array([[edge.id_from, edge.id_to] for edge in self.pg.edges])
pos = self.pg.nodes[:, 0:2]
self.gui.setgraph(pos, adj, self.loop_candidates)
def show_pointcloud(self):
pcd = np.array([[0, 0]])
for scan, pose in self.node_scan:
scan_trans = transpose_by_pose(pose, scan)
pcd = np.vstack((pcd, scan_trans))
self.gui.setpcd(pcd)
def show_map(self, use_gaussian=False):
self.costmap.map_data.fill(0)
pcd = np.array([[0, 0]])
for scan, pose in self.node_scan:
scan_trans = transpose_by_pose(pose, scan)
pcd = np.vstack((pcd, scan_trans))
idx = self.costmap.world_map(pcd)
for i in range(idx.shape[0]):
p = idx[i, :].astype(int)
if use_gaussian:
self.costmap.updateCostMap(p, 30, True)
else:
self.costmap.updateCostMap(p, 10, False)
self.gui.setmap(self.costmap.map_data)
def update(self):
pcd = np.array([[0, 0]])
#self.node_scan.clear()
node_scan = []
#del self.node_scan[:]
for i in range(len(self.node_scan)):
scan, pose = self.node_scan[i]
old_pose = self.pg.nodes[i, :]
node_scan.append((scan, self.pg.nodes[i, :]))
self.node_scan = node_scan
def checkupdate(self, pre_pose, cur_pose):
dx = pre_pose[0] - cur_pose[0]
dy = pre_pose[1] - cur_pose[1]
da = angle_diff(pre_pose[2], cur_pose[2])
trans = sqrt(dx**2 + dy**2)
update = (trans > d_thresh_) or (fabs(da) > a_thresh_)
return update
def optimize(self):
print('Waiting optimization...')
self.pg.optimize(opt_iter_)
print('Optimization OK.')
self.update()
self.show_graph()
self.show_pointcloud()
def readdata(self, raw_data):
data = []
for scan, odom in raw_data:
rot, trans = getRtFromM(self.scan_base)
scan = transpose(rot, trans, scan)
pose = t2v(odom).ravel()
if len(data) == 0:
data.append((scan, pose))
if not self.checkupdate(data[-1][1], pose):
continue
data.append((scan, pose))
#print pose
return data
def creategraph(self, data):
nodes = []
edges = []
# Generate basic nodes and edges for our pose graph
for i in range(len(data)):
scan, pose = data[i]
nodes.append(pose)
self.pg.nodes.append(pose)
if i == 0:
continue
infm = np.array([[weight_trans_, 0, 0], [0, weight_trans_, 0],
[0, 0, weight_rot_]])
edge = [i - 1, i, get_motion_vector(nodes[i], nodes[i - 1]), infm]
edges.append(edge)
self.pg.edges.append(PoseEdge(*edge))
self.pg.nodes = np.array(self.pg.nodes)
# Detect the loop-closing
loop_count = 0
self.loop_candidates = []
for i in range(len(data)):
if i + min_loop_dist_ >= len(data):
continue
scan_i, pose_i = data[i]
if not check_loop_candidate(scan_i, hough_weight_):
continue
self.loop_candidates.append(i)
for j in range(i + min_loop_dist_, len(data)):
scan_j, pose_j = data[j]
dst = distance.euclidean(pose_i[0:2], pose_j[0:2])
if dst > max_dist_:
continue
#if not check_loop_candidate(scan_j, hough_weight_):
# continue
loop_count += 1
delta_odom_init = getDeltaM(v2t(pose_i), v2t(pose_j))
delta_rot_init, delta_trans_init = getRtFromM(delta_odom_init)
#start_time = time.time()
icp = ICP(scan_i, scan_j)
delta_rot, delta_trans, cost = icp.calculate(
200, delta_rot_init, delta_trans_init, max_dist_)
if (cost > first_cost_):
#print 'old cost:',cost
delta_rot, delta_trans, cost = icp.calculate(
200, delta_rot, delta_trans, 0.5)
#print 'new cost:',cost
#print '-------------'
if (use_cost_threshold_ and (cost > second_cost_)):
continue
#cost = cost/5
if icp_debug_:
new_scan_j = transpose(delta_rot, delta_trans.ravel(),
scan_j)
scan_j = transpose(delta_rot_init,
delta_trans_init.ravel(), scan_j)
show_icp_matching(
scan_i, new_scan_j, scan_j,
str(i) + '-->' + str(j) + 'cost:' + str(cost))
#plt.show()
infm = np.array([[weight_trans_ / cost**2, 0, 0],
[0, weight_trans_ / cost**2, 0],
[0, 0, weight_rot_ / cost**2]])
edge = [i, j, t2v(getMFromRt(delta_rot, delta_trans)), infm]
edges.append(edge)
self.pg.edges.append(PoseEdge(*edge))
print('{0} loop have been detected!'.format(loop_count))
import posegraph
import time
from utils import *
reload(posegraph)
from posegraph import PoseGraph
import matplotlib.pyplot as plt
# Specify files to load
vfile = 'data/killian-v.dat'
efile = 'data/killian-e.dat'
pg = PoseGraph()
pg.readGraph(vfile, efile)
"""
pg.readGraph('data/goalpoints_toro_result.graph')
cols = [(np.random.random(), np.random.random(), np.random.random()) for i in range(1000)]
coords = pg.nodes[:, :2]
uncertainties, path = get_uncertainties_and_path(coords)
for j in range(len(path)):
plt.scatter(pg.nodes[j, 0], pg.nodes[j, 1], s=1+np.min((200, 1000*(uncertainties[j][2][2])/ephi**2)))
for e in [[20,8], [36,20], [98,36], [102,138], [112,128], [127,113]]:
plt.scatter(pg.nodes[e[0], 0], pg.nodes[e[0], 1], s=80, c=cols[e[0]])
plt.text(pg.nodes[e[0], 0]+(np.random.random()*1-.5), pg.nodes[e[0], 1]+(np.random.random()*1-.5), str(e[0]))
plt.scatter(pg.nodes[e[1], 0], pg.nodes[e[1], 1], s=80, c=cols[e[0]])
plt.text(pg.nodes[e[1], 0]+(np.random.random()*1-.5), pg.nodes[e[1], 1]+(np.random.random()*1-.5), str(e[1]))
plt.show()
"""
plt.ion()
def runslam(coords, loopedges):
uncertainties, path = get_uncertainties_and_path(coords, linear_uncertainty=ex, angular_uncertainty=ephi)
pg = PoseGraph() # LS-SLAM pose graph
edges = []
# populate with data - path
for p in range(len(path)):
pg.nodes.append(path[p])
if p > 0:
covm = uncertainties[p]#+uncertainties[p-1]
try:
infm=np.linalg.inv(covm)
except:
infm=np.linalg.inv(covm)
edge = [p-1, p, get_motion_vector(path[p], path[p-1]), infm]
edges.append(edge)
pg.edges.append(PoseEdge(*edge))
pg.nodes = np.array(pg.nodes)
# populate with data - loop closures
for idpair in loopedges:
sx = minex
sy = miney
st = minephi+ephi*np.sum([np.abs(path[p][2]-path[p-1][2]) for p in range(idpair[1], idpair[0]+1)])
cov_prec = [
[sx**2,0,0],
[0,sy**2,0],
[0,0,st**2]
]
d = get_motion_vector([0,0]+[pg.nodes[idpair[0]][2]], [0,0]+[pg.nodes[idpair[1]][2]])
print "inserting loop closure vector btw. ",idpair,": ", d, np.rad2deg(d[2]), "\t theta uncertainty:", cov_prec[2][2]
try:
edge = [idpair[0], idpair[1], d, np.linalg.inv(cov_prec)]
except:
edge = [idpair[0], idpair[1], d, np.linalg.inv(cov_prec)]
edges.append(edge)
pg.edges.append(PoseEdge(*edge))
writePoseGraph(pg.nodes, edges, 'data/goalpoints.graph') # write graph in g2o format for TORO
# run SLAM
plt.clf()
#plt.subplot(1,3,1)
plt.title('path (before SLAM)')
for j in range(len(path)):
plt.scatter(pg.nodes[j, 0], pg.nodes[j, 1], s=1+np.min((200, 1000*(uncertainties[j][2][2])/ephi**2)))
#plt.text(pg.nodes[j, 0]+30*float(j)/len(path)+np.random.random()*2-1, pg.nodes[j, 1]+np.random.random()*2-1, str(j))
for e in loopedges:
plt.scatter(pg.nodes[e[0], 0], pg.nodes[e[0], 1], s=80, c=cols[e[0]])
plt.text(pg.nodes[e[0], 0]+(np.random.random()*1-.5), pg.nodes[e[0], 1]+(np.random.random()*1-.5), str(e[0]))
plt.scatter(pg.nodes[e[1], 0], pg.nodes[e[1], 1], s=80, c=cols[e[0]])
plt.text(pg.nodes[e[1], 0]+(np.random.random()*1-.5), pg.nodes[e[1], 1]+(np.random.random()*1-.5), str(e[1]))
plt.draw()
time.sleep(1)
print "optimization step (1)"
pg.optimize(1)
for k in range(2):
plt.subplot(1,2,k+1)
plt.title('path (after '+str(k+1)+' SLAM steps)')
plt.scatter(path[:, 0], path[:, 1], s=1)
plt.hold(True)
for j in range(len(path)):
plt.scatter(pg.nodes[j, 0], pg.nodes[j, 1], s=1+np.min((200, 1000*(uncertainties[j][2][2])/ephi**2)))
#plt.text(pg.nodes[j, 0], pg.nodes[j, 1], str(j))
plt.draw()
print "optimization step (5)"
pg.optimize(5)
plt.show(block=True)
base_x_ = 0.
base_y_ = 0.
base_yaw_ = 0.03
#base_yaw_ = 0.03
#base_yaw_ = -3.12
#base_x_ = -0.11
#base_y_ = 0.
#base_yaw_ = -1.54
##########################
d_thresh_ = .5
a_thresh_ = np.pi/6
##########################
pg = PoseGraph()
data = np.array([[0.,0,0],\
[1,0,-0.57],\
[1,-1,-1.14],\
[0,-1,1.57],\
[0,-0.2,0],\
])
loopedges = np.array([[0,4]])
def creategraph(data):
nodes = []
edges = []
for i in range(len(data)):
pose = data[i]
nodes.append(pose)
