def handle_raw_stereo(self, msg): size = (msg.left_info.width, msg.left_info.height) if self.vo == None: cam = camera.StereoCamera(msg.right_info) self.fd = FeatureDetectorFast(300) self.ds = DescriptorSchemeCalonder() self.vo = VisualOdometer(cam, scavenge = False, inlier_error_threshold = 3.0, sba = None, inlier_thresh = 100, position_keypoint_thresh = 0.2, angle_keypoint_thresh = 0.15) self.vo.num_frames = self.startframe pair = [Image.fromstring("L", size, i.uint8_data.data) for i in [ msg.left_image, msg.right_image ]] af = SparseStereoFrame(pair[0], pair[1], feature_detector = self.fd, descriptor_scheme = self.ds) self.vo.handle_frame(af) self.frame_timestamps[af.id] = msg.header.stamp if self.skel.add(self.vo.keyframe): print "====>", self.vo.keyframe.id, self.frame_timestamps[self.vo.keyframe.id] #print self.tf.getFrameStrings() #assert self.tf.frameExists("wide_stereo_optical_frame") #assert self.tf.frameExists("odom_combined") N = sorted(self.skel.nodes) for a,b in zip(N, N[1:]): if (a,b) in self.skel.edges: t0 = self.frame_timestamps[a] t1 = self.frame_timestamps[b] if self.tf.canTransformFull("wide_stereo_optical_frame", t0, "wide_stereo_optical_frame", t1, "odom_combined"): t,r = self.tf.lookupTransformFull("wide_stereo_optical_frame", t0, "wide_stereo_optical_frame", t1, "odom_combined") relpose = Pose() relpose.fromlist(self.tf.fromTranslationRotation(t,r)) self.wheel_odom_edges.add((a, b, relpose, 1.0)) self.send_map(msg.header.stamp)
def send_map(self, stamp): if len(self.skel.nodes) < 4: return TG = nx.MultiDiGraph() for i in self.skel.nodes: TG.add_node(i) for (a,b,p,c) in self.skel.every_edge + list(self.wheel_odom_edges): TG.add_edge(a, b, (p, c)) here = max(self.skel.nodes) # TG is the total graph, G is the local subgraph uTG = TG.to_undirected() print "uTG", uTG.nodes(), uTG.edges() close = set([here]) for i in range(self.optimization_distance): close |= set(nx.node_boundary(uTG, close)) print "close", close G = TG.subgraph(close) uG = uTG.subgraph(close) pg = TreeOptimizer3() def mk_covar(xyz, rp, yaw): return (1.0 / math.sqrt(xyz),1.0 / math.sqrt(xyz), 1.0 / math.sqrt(xyz), 1.0 / math.sqrt(rp), 1.0 / math.sqrt(rp), 1.0 / math.sqrt(yaw)) weak = mk_covar(9e10,3,3) strong = mk_covar(0.0001, 0.000002, 0.00002) pg.initializeOnlineOptimization() def pgadd(p, n, data): relpose, strength = data if strength == 0.0: cov = weak else: cov = strong pg.addIncrementalEdge(p, n, relpose.xform(0,0,0), relpose.euler(), cov) Gedges = G.edges(data = True) revmap = {} map = {} for n in nx.dfs_preorder(uG, here): revmap[len(map)] = n map[n] = len(map) priors = [p for p in uG.neighbors(n) if p in map] if priors == []: print "START NODE", n, "as", map[n] else: print "NEXT NODE", n p = priors[0] if not G.has_edge(p, n): n,p = p,n data = G.get_edge_data(p, n)[0] Gedges.remove((p, n, data)) pgadd(map[p], map[n], data) for (p,n,d) in Gedges: pgadd(map[p], map[n], d) pg.initializeOnlineIterations() start_error = pg.error() for i in range(10): pg.iterate() print "Error from", start_error, "to", pg.error() pg.recomputeAllTransformations() print self.tf.getFrameStrings() target_frame = "base_link" t = self.tf.getLatestCommonTime("wide_stereo_optical_frame", target_frame) trans,rot = self.tf.lookupTransform(target_frame, "wide_stereo_optical_frame", t) xp = Pose() xp.fromlist(self.tf.fromTranslationRotation(trans,rot)) roadmap_nodes = dict([ (n, (30,0,0)) for n in TG.nodes() ]) nl = sorted(roadmap_nodes.keys()) print "nl", nl for i in sorted(map.values()): xyz,euler = pg.vertex(i) p = from_xyz_euler(xyz, euler) pose_in_target = xp * p x,y,z = pose_in_target.xform(0,0,0) euler = pose_in_target.euler() print x,y,z, euler roadmap_nodes[revmap[i]] = (x, y, euler[2]) roadmap_edges = [] for (p,n) in set(TG.edges()): print p,n best_length = max([ (confidence, pose.distance()) for (pose, confidence) in TG.get_edge_data(p, n).values()])[1] roadmap_edges.append((p, n, best_length)) msg = vslam.msg.Roadmap() msg.header.stamp = stamp msg.header.frame_id = "base_link" msg.nodes = [vslam.msg.Node(*roadmap_nodes[n]) for n in nl] print "(p,n)", [ (p,n) for (p,n,l) in roadmap_edges ] msg.edges = [vslam.msg.Edge(nl.index(p), nl.index(n), l) for (p, n, l) in roadmap_edges] msg.localization = nl.index(here) self.pub.publish(msg) if 1: import matplotlib.pyplot as plt fig = plt.figure(figsize = (12, 6)) plt.subplot(121) pos = nx.spring_layout(TG, iterations=1000) nx.draw(TG, pos,node_color='#A0CBE2') nx.draw_networkx_edges(G, pos, with_labels=False, edge_color='r', alpha=0.5, width=25.0, arrows = False) plt.subplot(122) nx.draw(G, pos = dict([(n, roadmap_nodes[n][:2]) for n in G.nodes()])) #plt.show() plt.savefig("/tmp/map_%d.png" % here)