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 move_forward(i, prev): """ Forward 1 meter, turn around, Back 1 meter """ if i == 0: return Pose(rotation(0, 0, 1, 0), (0, 0, 0)) elif i < 10: return prev * Pose(rotation(0, 0, 1, 0), (0, 0, .1)) elif i < 40: return prev * Pose(rotation(math.pi / 30, 0, 1, 0), (0, 0, 0)) elif i < 50: return prev * Pose(rotation(0, 0, 1, 0), (0, 0, .1))
def PE(self, af0, af1): pairs = [ (a,b) for (a,b,_) in self.ds.match0(self.node_kp[af1], self.node_descriptors[af1], self.node_kp[af0], self.node_descriptors[af0],True) ] inl,R,T = self.pe.estimateC(self.cam, self.node_kp[af0], self.cam, self.node_kp[af1], pairs) if inl == 0: return (0, None) else: r33 = numpy.mat(numpy.array(R).reshape(3,3)) return (inl, Pose(r33, numpy.array(T)))
def newpose(self, id): try: self.pg.recomputeAllTransformations() xyz,euler = self.pg.vertex(id) p = from_xyz_euler(xyz, euler) except: print "Failed to get vertex", id p = Pose() return p
def getImage(i): theta = (i / 400.0) * math.pi * 2 wobble = 0.05 * (sin(theta * 9.5) + sin(theta * 3.5)) r = 4.0 + wobble desired_pose = Pose(rotation(theta, 0, 1, 0), (4.0 - r * math.cos(theta), 0, r * math.sin(theta))) if not os.access("/tmp/out%06d.png" % i, os.R_OK): cam = ray_camera(desired_pose, stereo_cam) imL = Image.new("RGB", (640, 480)) imR = Image.new("RGB", (640, 480)) imD = [ Image.new("F", (640, 480)), Image.new("F", (640, 480)), Image.new("F", (640, 480)) ] def is_visible(p, pt): camx, camy, camz = (~p).xform(pt.x, pt.y, pt.z) if camz <= 0: return False if abs(camx / camz) > 2.0: return False if abs(camy / camz) > 2.0: return False return True scene = [ object(sphere(center, 0.3), shadeLitCloud, {'scale': 10}) for center in blobs if is_visible(desired_pose, center) ] scene += [ object(isphere(vec3(0, 0, 0), 1000), shadeLitCloud, {'scale': 0.001}) ] render_stereo_scene(imL, imR, imD, cam, scene, i * 0.033) imL = imL.convert("L") imR = imR.convert("L") im = Image.merge("RGB", (imL, imR, imR)) im.save("/tmp/out%06d.png" % i) imD[0].save("/tmp/out%06d-x.tiff" % i) imD[1].save("/tmp/out%06d-y.tiff" % i) imD[2].save("/tmp/out%06d-z.tiff" % i) else: im = Image.open("/tmp/out%06d.png" % i) imL, imR, _ = im.split() return (desired_pose, imL, imR)
def test_pe(self): random.seed(0) cloud = [(4 * (random.random() - 0.5), 4 * (random.random() - 0.5), 4 * (random.random() - 0.5)) for i in range(20)] vo = VisualOdometer( camera.Camera( (389.0, 389.0, 89.23 * 1e-3, 323.42, 323.42, 274.95))) stereo_cam = {} af = {} fd = FeatureDetectorStar(300) ds = DescriptorSchemeSAD() for i in range(5): stereo_cam[i] = camera.Camera( (389.0, 389.0, .080 + .010 * i, 323.42, 323.42, 274.95)) desired_pose = Pose() cam = ray_camera(desired_pose, stereo_cam[i]) imL = Image.new("RGB", (640, 480)) imR = Image.new("RGB", (640, 480)) scene = [] scene += [ object(isphere(vec3(0, 0, 0), 1000), shadeLitCloud, {'scale': 0.001}) ] scene += [ object(isphere(vec3(x, y, z + 6), 0.3), shadeLitCloud, {'scale': 3}) for (x, y, z) in cloud ] imL, imR = [im.convert("L") for im in [imL, imR]] render_stereo_scene(imL, imR, None, cam, scene, 0) af[i] = SparseStereoFrame(imL, imR, feature_detector=fd, descriptor_scheme=ds) vo.process_frame(af[i]) pe = PoseEstimator() for i1 in range(5): for i0 in range(5): pairs = vo.temporal_match(af[i1], af[i0]) res = pe.estimateC(stereo_cam[i0], af[i0].features(), stereo_cam[i1], af[i1].features(), pairs, False) x, y, z = res[2] self.assertAlmostEqual(x, 0, 0) self.assertAlmostEqual(y, 0, 0) self.assertAlmostEqual(z, 0, 0)
def handleFrame(self): r = False (w,h,li,ri) = self.source.getImage() self.f = SparseStereoFrame(li, ri, disparity_range = 96, feature_detector = self.fd, descriptor_scheme = self.ds) self.vo.handle_frame(self.f) print self.vo.inl self.inlier_history = self.inlier_history[-20:] + [self.vo.inl] # If the VO inlier count falls below 20, we're not well-connected for this link if self.vo.inl < 20: self.connected = False self.f.pose = Pose() self.snail_trail = [] # Add a frame to graph if: # Frame zero, or # There have been N successful preceding frames, and either # Not connected, # or if connected, the distance from the youngest graph frame to this frame is > thresh add_to_graph = None if self.vo.keyframe.id == 0 and len(self.skel.nodes) == 0: add_to_graph = "Zero keyframe" elif min(self.inlier_history) > 20: if not self.connected: add_to_graph = "Unconnected - but now recovering good poses" # This is a new graph, so generate a new label self.label += 1 else: relpose = ~self.last_added_pose * self.f.pose if relpose.distance() > self.skel_dist_thresh: add_to_graph = "Connected and distance thresh passed" if add_to_graph: self.skel.setlabel(self.label) self.skel.add(self.vo.keyframe, self.connected) print "ADD %d %s" % (self.f.id, add_to_graph) self.connected = True self.last_added_pose = self.f.pose self.snail_trail = [] # self.ioptimize(10) self.snail_trail.append(self.f.pose) # Render the camera view using OpenCV, then load it into an OpenGL texture if True: if not self.cvim: self.cvim = cv.CreateImage((w,h/2), cv.IPL_DEPTH_8U, 3) im = cv.CreateImage((w,h), cv.IPL_DEPTH_8U, 1) im2 = cv.CreateImage((w/2,h/2), cv.IPL_DEPTH_8U, 1) for i,src in enumerate([li, ri]): cv.SetData(im, str(src.tostring()), w) cv.Resize(im, im2) cv.SetImageROI(self.cvim, (i * w / 2, 0, w / 2, h / 2)) cv.CvtColor(im2, self.cvim, cv.CV_GRAY2BGR) cv.ResetImageROI(self.cvim) def annotate(cvim): green = cv.RGB(0,255,0) red = cv.RGB(0,0,255) def half(p): return (p[0] / 2, p[1] / 2) for (x,y,d) in self.f.features(): cv.Circle(cvim, half((x, y)), 2, green) d = int(d) cv.Line(cvim, ((w+x-d)/2, y/2 - 2), ((w+x-d)/2, y/2 + 2), green) a_features = self.vo.keyframe.features() b_features = self.f.features() inliers = set([ (b,a) for (a,b) in self.vo.pe.inl]) outliers = set(self.vo.pairs) - inliers for (a,b) in inliers: cv.Line(cvim, half(b_features[b]), half(a_features[a]), green) for (a,b) in outliers: cv.Line(cvim, half(b_features[b]), half(a_features[a]), red) annotate(self.cvim) glBindTexture(GL_TEXTURE_2D, demo.textures[2]) glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB8, 640, 240, 0, GL_RGB, GL_UNSIGNED_BYTE, self.cvim.tostring()) if self.camera_preview: cv.ShowImage("Camera", self.cvim) if cv.WaitKey(10) == 27: r = True return r
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)
def add(self, this, connected = 1): """ add frame *this* to skeleton, *connected* 1 means good link to previous, 0 means no link to previous - use fill data, -1 means no link to previous force weak link. """ if len(self.nodes) == 0: self.nodes.add(this.id) self.node_labels[this.id] = self.label r = True elif not(this.id in self.nodes): previd = max(self.nodes) # Ignore the node if there are less than node_vist frames since the previous node # Treat fill data like regular links if (connected != -1) and (this.id - previd) < self.node_vdist: return False if (connected == 1) and self.prev_pose: print "Strong link from %d to %d" % (previd, this.id) relpose = ~self.prev_pose * this.pose inf = strong print "LINK: Strong from obs", relpose.xform(0,0,0) self.every_edge.append((previd, this.id, relpose, 1.0)) else: if self.fills and (connected == 0): print "Strong link from %d to %d" % (previd, this.id) relpose = self.gt(self.prev_id, this.id) self.every_edge.append((previd, this.id, relpose, 1.0)) inf = strong print "LINK: Strong from fill", relpose.xform(0,0,0) else: self.weak_edges.append((previd, this.id)) print "Weak link from %d to %d" % (previd, this.id) relpose = Pose(numpy.identity(3), [ 0, 0, 0 ]) self.every_edge.append((previd, this.id, relpose, 0.0)) inf = weak print "LINK: Weak" self.nodes.add(this.id) self.node_labels[this.id] = self.label self.edges.add( (previd, this.id) ) self.timer['toro add'].start() vtop = self.pg.addIncrementalEdge(previd, this.id, relpose.xform(0,0,0), relpose.euler(), inf) self.vset.add(previd) self.vset.add(this.id) # print self.vset print "ADDED VO CONSTRAINT", previd, this.id, inf self.timer['toro add'].stop() #print "added node at", this.pose.xform(0,0,0), "in graph as", self.newpose(this.id).xform(0,0,0) self.memoize_node_kp_d(this) far = [ id for id in self.place_find(this.descriptors()) if (not id in [this.id, previd])] self.place_ids.append(this.id) self.add_links(this.id, far) r = True else: r = False if r: self.prev_pose = this.pose self.prev_id = this.id #self.prev_frame = this return r
def gt(self, i0, i1): assert i0 <= i1 r = ~self.fills[i0] * self.fills[i1] return r p = Pose() p.M[0,0] = r.M[0,0] p.M[0,1] = r.M[0,2] p.M[0,2] = -r.M[0,1] p.M[0,3] = r.M[0,3] p.M[1,0] = -r.M[2,1] p.M[1,1] = r.M[2,2] p.M[1,2] = -r.M[2,0] p.M[1,3] = -r.M[2,3] p.M[2,0] = -r.M[1,0] p.M[2,1] = r.M[1,2] p.M[2,2] = r.M[1,1] p.M[2,3] = r.M[1,3] return p cam90 = Pose(numpy.array([[ 1, 0, 0 ], [ 0, 0, -1 ], [ 0, 1, 0 ]]), numpy.array([0, 0, 0])) return cam90 * r * ~cam90
def fill(self, filename): startframe = None if len(self.nodes) != 0: my_start = max(self.nodes) + 1 else: my_start = 0 from tf import transformations self.fills = {} for l in open(filename): f = l.split() if len(f) == 11: if not startframe: startframe = int(f[1]) assert f[4] == 'pose:' X = float(f[5]) Y = float(f[6]) Z = float(f[7]) if 0: roll = math.pi * float(f[8]) / 180.0 pitch = math.pi * float(f[9]) / 180.0 yaw = math.pi * float(f[10]) / 180.0 sa = math.sin(yaw); sb = math.sin(pitch); sg = math.sin(roll); ca = math.cos(yaw); cb = math.cos(pitch); cg = math.cos(roll); P = numpy.array( [ [ ca*cb , -sa*cg + ca*sb*sg , sa*sg + ca*sb*cg ], [ sa*cb , ca*cg + sa*sb*sg , -ca*sg + sa*sb*cg ], [ -sb , cb*sg , cb*cg ]]).T else: euler = [ float(c) * math.pi / 180 for c in f[8:11] ] sa = math.sin(euler[2]); sb = math.sin(euler[1]); sg = math.sin(euler[0]); ca = math.cos(euler[2]); cb = math.cos(euler[1]); cg = math.cos(euler[0]); P = numpy.zeros((3,3)) P[0,0] = ca*cb; P[0,1] = -sa*cg + ca*sb*sg; P[0,2] = sa*sg + ca*sb*cg; P[1,0] = sa*cb; P[1,1] = ca*cg + sa*sb*sg; P[1,2] = -ca*sg + sa*sb*cg; P[2,0] = -sb; P[2,1] = cb*sg; P[2,2] = cb*cg; cam90 = Pose(numpy.array([[ 1, 0, 0 ], [ 0, 0, -1 ], [ 0, 1, 0 ]]), numpy.array([0, 0, 0])) veh_t = Pose(P, numpy.array([X, Y, Z])) Tc2v = Pose( numpy.array([ [0.010546 , -0.006048, 0.999926 ], [-0.999650 , 0.024210 , 0.010689 ], [-0.024273 ,-0.999689 ,-0.005791 ]]), numpy.array( [ 1.783353 , 0.265480 , -0.041039 ])) self.fills[my_start + int(f[1])-startframe] = (~Tc2v * veh_t) * Tc2v if 0: import pylab, sys po = [ self.fills[x].xform(0,0,0) for x in sorted(self.fills.keys()) ] pylab.scatter([x for (x,y,z) in po], [y for (x,y,z) in po], c = 'r') print self.fills[0].M po = [ self.gt(0, x).xform(0,0,0) for x in sorted(self.fills.keys()) ] pylab.scatter([x for (x,y,z) in po], [z for (x,y,z) in po], c = 'g') pylab.show() sys.exit(1)
def ground_truth(p, q): return Pose( transformations.rotation_matrix_from_quaternion( [q.x, q.y, q.z, q.w])[:3, :3], [p.x, p.y, p.z])