def xtest_smoke(self): cam = camera.Camera((389.0, 389.0, 89.23, 323.42, 323.42, 274.95)) vo = VisualOdometer(cam) vo.reset_timers() dir = "/u/konolige/vslam/data/indoor1/" trail = [] prev_scale = None schedule = [(f + 1000) for f in (range(0, 100) + range(100, 0, -1) + [0] * 10)] schedule = range(1507) schedule = range(30) for f in schedule: lf = Image.open("%s/left-%04d.ppm" % (dir, f)) rf = Image.open("%s/right-%04d.ppm" % (dir, f)) lf.load() rf.load() af = SparseStereoFrame(lf, rf) vo.handle_frame(af) print f, vo.inl trail.append(numpy.array(vo.pose.M[0:3, 3].T)[0]) def d(a, b): d = a - b return sqrt(numpy.dot(d, d.conj())) print "covered ", sum([d(a, b) for (a, b) in zip(trail, trail[1:])]) print "from start", d(trail[0], trail[-1]), trail[0] - trail[-1] vo.summarize_timers() print vo.log_keyframes
def xtest_smoke(self): cam = camera.Camera((389.0, 389.0, 89.23, 323.42, 323.42, 274.95)) vo = VisualOdometer(cam) vo.reset_timers() dir = "/u/konolige/vslam/data/indoor1/" trail = [] prev_scale = None schedule = [(f+1000) for f in (range(0,100) + range(100,0,-1) + [0]*10)] schedule = range(1507) schedule = range(30) for f in schedule: lf = Image.open("%s/left-%04d.ppm" % (dir,f)) rf = Image.open("%s/right-%04d.ppm" % (dir,f)) lf.load() rf.load() af = SparseStereoFrame(lf, rf) vo.handle_frame(af) print f, vo.inl trail.append(numpy.array(vo.pose.M[0:3,3].T)[0]) def d(a,b): d = a - b return sqrt(numpy.dot(d,d.conj())) print "covered ", sum([d(a,b) for (a,b) in zip(trail, trail[1:])]) print "from start", d(trail[0], trail[-1]), trail[0] - trail[-1] vo.summarize_timers() print vo.log_keyframes
class VO: def __init__(self): rospy.Subscriber('/stereo/raw_stereo', stereo_msgs.msg.RawStereo, self.handle_raw_stereo, queue_size=2, buff_size=7000000) self.pub_vo = rospy.Publisher("/vo", deprecated_msgs.msg.VOPose) self.vo = None self.modulo = 0 self.fd = FeatureDetectorFast(300) self.ds = DescriptorSchemeCalonder() 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.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) pair = [imgAdapted(i, size) for i in [ msg.left_image, msg.right_image ]] af = SparseStereoFrame(pair[0], pair[1], feature_detector = self.fd, descriptor_scheme = self.ds) pose = self.vo.handle_frame(af) p = deprecated_msgs.msg.VOPose() p.inliers = self.vo.inl # XXX - remove after camera sets frame_id p.header = roslib.msg.Header(0, msg.header.stamp, "stereo_link") p.pose = geometry_msgs.msg.Pose(geometry_msgs.msg.Point(*pose.xform(0,0,0)), geometry_msgs.msg.Quaternion(*pose.quaternion())) self.pub_vo.publish(p)
def xtest_smoke_bag(self): import rosrecord import visualize class imgAdapted: def __init__(self, i): self.i = i self.size = (i.width, i.height) def tostring(self): return self.i.data cam = None filename = "/u/prdata/videre-bags/loop1-mono.bag" filename = "/u/prdata/videre-bags/vo1.bag" framecounter = 0 for topic, msg in rosrecord.logplayer(filename): print framecounter if rospy.is_shutdown(): break #print topic,msg if topic == "/videre/cal_params" and not cam: cam = camera.VidereCamera(msg.data) vo = VisualOdometer(cam) if cam and topic == "/videre/images": if -1 <= framecounter and framecounter < 360: imgR = imgAdapted(msg.images[0]) imgL = imgAdapted(msg.images[1]) af = SparseStereoFrame(imgL, imgR) pose = vo.handle_frame(af) visualize.viz(vo, af) framecounter += 1 print "distance from start:", vo.pose.distance() vo.summarize_timers()
class VODemo: vo = None frame = 0 def display_params(self, iar): if not self.vo: matrix = [] # Matrix will be in row,column order section = "" in_proj = 0 for l in iar.data.split('\n'): if len(l) > 0 and l[0] == '[': section = l.strip('[]') ws = l.split() if ws != []: if section == "right camera" and ws[0].isalpha(): in_proj = (ws[0] == 'proj') elif in_proj: matrix.append([float(s) for s in l.split()]) Fx = matrix[0][0] Fy = matrix[1][1] Cx = matrix[0][2] Cy = matrix[1][2] Tx = -matrix[0][3] / Fx self.params = (Fx, Fy, Tx, Cx, Cx, Cy) cam = camera.Camera(self.params) self.vo = VisualOdometer(cam) self.started = None def display_array(self, iar): diag = "" af = None if self.vo: if not self.started: self.started = time.time() if 0: time.sleep(0.028) else: imgR = imgAdapted(iar.images[0]) imgL = imgAdapted(iar.images[1]) af = SparseStereoFrame(imgL, imgR) if 1: pose = self.vo.handle_frame(af) diag = "%d inliers, moved %.1f" % (self.vo.inl, pose.distance()) if (self.frame % 1) == 0: took = time.time() - self.started print "%4d: %5.1f [%f fps]" % (self.frame, took, self.frame / took), diag self.frame += 1 #print "got message", len(iar.images) #print iar.images[0].width if SEE: right = ut.ros2cv(iar.images[0]) left = ut.ros2cv(iar.images[1]) hg.cvShowImage('channel L', left) hg.cvShowImage('channel R', right) hg.cvWaitKey(5)
class VODemo: vo = None frame = 0 def display_params(self, iar): if not self.vo: matrix = [] # Matrix will be in row,column order section = "" in_proj = 0 for l in iar.data.split("\n"): if len(l) > 0 and l[0] == "[": section = l.strip("[]") ws = l.split() if ws != []: if section == "right camera" and ws[0].isalpha(): in_proj = ws[0] == "proj" elif in_proj: matrix.append([float(s) for s in l.split()]) Fx = matrix[0][0] Fy = matrix[1][1] Cx = matrix[0][2] Cy = matrix[1][2] Tx = -matrix[0][3] / Fx self.params = (Fx, Fy, Tx, Cx, Cx, Cy) cam = camera.Camera(self.params) self.vo = VisualOdometer(cam) self.started = None def display_array(self, iar): diag = "" af = None if self.vo: if not self.started: self.started = time.time() if 0: time.sleep(0.028) else: imgR = imgAdapted(iar.images[0]) imgL = imgAdapted(iar.images[1]) af = SparseStereoFrame(imgL, imgR) if 1: pose = self.vo.handle_frame(af) diag = "%d inliers, moved %.1f" % (self.vo.inl, pose.distance()) if (self.frame % 1) == 0: took = time.time() - self.started print "%4d: %5.1f [%f fps]" % (self.frame, took, self.frame / took), diag self.frame += 1 # print "got message", len(iar.images) # print iar.images[0].width if SEE: right = ut.ros2cv(iar.images[0]) left = ut.ros2cv(iar.images[1]) hg.cvShowImage("channel L", left) hg.cvShowImage("channel R", right) hg.cvWaitKey(5)
class VO: def __init__(self, decimate): rospy.TopicSub('/videre/images', ImageArray, self.handle_array, queue_size=2, buff_size=7000000) rospy.TopicSub('/videre/cal_params', String, self.handle_params) self.pub_vo = rospy.Publisher("/vo", VOPose) self.vo = None self.decimate = decimate self.modulo = 0 def handle_params(self, iar): if not self.vo: cam = camera.VidereCamera(iar.data) self.vo = VisualOdometer(cam) self.intervals = [] self.took = [] def handle_array(self, iar): if self.vo: t0 = time.time() self.intervals.append(t0) if (self.modulo % self.decimate) == 0: imgR = imgAdapted(iar.images[0]) imgL = imgAdapted(iar.images[1]) af = SparseStereoFrame(imgL, imgR) if 1: pose = self.vo.handle_frame(af) else: pose = Pose() #print self.vo.num_frames, pose.xform(0,0,0), pose.quaternion() p = VOPose() p.inliers = self.vo.inl # XXX - remove after camera sets frame_id p.header = rostools.msg.Header(0, iar.header.stamp, "stereo_link") p.pose = stdmsg.Pose(stdmsg.Point(*pose.xform(0,0,0)), stdmsg.Quaternion(*pose.quaternion())) self.pub_vo.publish(p) self.modulo += 1 self.took.append(time.time() - t0) if (len(self.took) % 100) == 0: print len(self.took) def dump(self): iv = self.intervals took = self.took print "Incoming frames: %d in %f, so %fms" % (len(iv), iv[-1] - iv[0], (iv[-1] - iv[0]) / len(iv)) print "Time in callback: %fms" % (sum(took) / len(took))
vo_v[i].append(z1 - z) print framecounter framecounter += 1 if topic.endswith("odom_estimation"): oe_x.append(-msg.pose.position.y) oe_y.append(msg.pose.position.x) print "There are", len(vo.tracks), "tracks" print "There are", len([t for t in vo.tracks if t.alive]), "live tracks" print "There are", len(set([t.p[-1] for t in vo.tracks if t.alive])), "unique endpoints on live tracks" # Attempt to compute best possible end-to-end pose vo = VisualOdometer(cam, feature_detector = FeatureDetector4x4(FeatureDetectorHarris), scavenge = True) f0 = SparseStereoFrame(*first_pair) f1 = SparseStereoFrame(imgL, imgR) vo.handle_frame(f0) vo.handle_frame(f1) quality_pose = vo.pose colors = [ 'blue', 'red', 'black', 'magenta', 'cyan' ] for i in range(len(vos)): vos[i].planarity = planar(numpy.array([x for (x,y,z) in trajectory[i]]), numpy.array([y for (x,y,z) in trajectory[i]]), numpy.array([z for (x,y,z) in trajectory[i]])) xs = numpy.array(vo_x[i]) ys = numpy.array(vo_y[i]) xs -= 4.5 * 1e-3 f = -0.06 xp = xs * cos(f) - ys * sin(f) yp = ys * cos(f) + xs * sin(f) pylab.plot(xp, yp, c = colors[i], label = vos[i].name()) #pylab.quiver(xp, yp, vo_u[i], vo_v[i], color = colors[i]) #, label = '_nolegend_')
class PicmapNode: def __init__(self, args): rospy.init_node('picmap_server') stereo_cam = camera.Camera( (389.0, 389.0, 89.23 * 1e-3, 323.42, 323.42, 274.95)) self.vo = None self.transformer = pytf_swig.pyTransformer() self.transformer.setExtrapolationLimit(0.0) self.fd = FeatureDetectorFast(300) self.ds = DescriptorSchemeCalonder() self.pm = PictureMap(self.ds) #self.pub = rospy.Publisher("/picmap_pose", vslam.msg.Picmap) rospy.Subscriber('/stereo/raw_stereo', stereo_msgs.msg.RawStereo, self.handle_raw_stereo_queue, queue_size=2, buff_size=7000000) rospy.Subscriber('/amcl_pose', geometry_msgs.msg.PoseWithCovarianceStamped, self.handle_amcl_pose) rospy.Subscriber('/tf_message', tf.msg.tfMessage, self.handle_tf_queue) self.pmlock = threading.Lock() self.q = Queue.Queue(0) # 0 means maxsize is infinite self.tfq = Queue.Queue(0) t = threading.Thread(target=self.worker) t.start() def start(self): rospy.spin() pickle.dump(self.pm, open("/tmp/final_pm.pickle", "w")) def handle_tf_queue(self, msg): print " queued transform", min( [t.header.stamp.to_seconds() for t in msg.transforms]) self.tfq.put(msg) def handle_tf(self, msg): #print "incoming transform", min([ t.header.stamp.to_seconds() for t in msg.transforms ]) for transform in msg.transforms: ptf = py_transform_from_transform_stamped(transform) self.transformer.setTransform(ptf) #print self.transformer.allFramesAsString() #print "TF", "map->stereo_link getLatestCommonTime", self.transformer.getLatestCommonTime('map', 'stereo_link') def handle_raw_stereo_queue(self, msg): print " queued picture ", msg.header.stamp.to_seconds() self.q.put(msg) def worker(self): while True: # There are N messages in the queue. # Want to discard all but the most recent. while self.q.qsize() > 1: self.q.get() item = self.q.get() self.handle_raw_stereo(item) def handle_raw_stereo(self, msg): print "incoming picture ", msg.header.stamp.to_seconds() size = (msg.left_info.width, msg.left_info.height) cam = camera.StereoCamera(msg.right_info) if self.vo == None: 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.keys = set() self.v = Vis('raw stereo') #pair = [Image.fromstring("L", size, i.uint8_data.data) for i in [ msg.left_image, msg.right_image ]] pair = [dcamImage(i) for i in [msg.left_image, msg.right_image]] af = SparseStereoFrame(pair[0], pair[1], feature_detector=self.fd, descriptor_scheme=self.ds) af.t = msg.header.stamp.to_seconds() self.vo.handle_frame(af) self.v.show(msg.left_image.uint8_data.data, []) k = self.vo.keyframe if (not (k.id in self.keys)): self.keys.add(k.id) picture = Picture(k.features(), k.descriptors()) self.pm.newpic(k.t, cam, picture, True) if 0: picture = Picture(af.features(), af.descriptors()) self.pm.newpic(af.t, cam, picture, False) while True: if self.tfq.qsize() == 0: break tfmsg = self.tfq.get() if min([t.header.stamp.to_seconds() for t in tfmsg.transforms ]) > (msg.header.stamp.to_seconds() + 1.0): break self.handle_tf(tfmsg) self.pmlock.acquire() self.pm.resolve(self.transformer) self.pmlock.release() def handle_amcl_pose(self, msg): if self.vo: print "handle_amcl_pose(", msg, ")" happy = max([msg.pose.covariance[0], msg.pose.covariance[7] ]) < 0.003 print "picmap node got amcl", msg.header.stamp.to_seconds( ), msg.pose.covariance[0], msg.pose.covariance[7], "happy", happy self.pmlock.acquire() self.pm.newLocalization(msg.header.stamp.to_seconds(), happy) self.pmlock.release()
class VODemo: vo = None frame = 0 def __init__(self, mode, library): self.mode = mode self.library = library rospy.TopicSub('/videre/images', ImageArray, self.display_array) rospy.TopicSub('/videre/cal_params', String, self.display_params) rospy.TopicSub('/vo/tmo', Pose44, self.handle_corrections) self.viz_pub = rospy.Publisher("/overlay", Lines) self.vo_key_pub = rospy.Publisher("/vo/key", Frame) self.Tmo = Pose() self.mymarker1 = Marker(10) self.mymarkertrail = [Marker(11 + i) for i in range(10)] self.trail = [] self.vis = Vis() def display_params(self, iar): if not self.vo: cam = camera.VidereCamera(iar.data) print "cam.params", cam.params self.vo = VisualOdometer(cam) self.started = None if self.mode == 'learn': self.library = set() self.previous_keyframe = None self.know_state = 'lost' self.best_show_pose = None self.mymarker1.floor() def handle_corrections(self, corrmsg): print "GOT CORRECTION AT", time.time() Tmo_pose = Pose() Tmo_pose.fromlist(corrmsg.v) self.Tmo = Tmo_pose self.know_state = 'corrected' def display_array(self, iar): diag = "" af = None if self.vo: if not self.started: self.started = time.time() imgR = imgAdapted(iar.images[0]) imgL = imgAdapted(iar.images[1]) af = SparseStereoFrame(imgL, imgR) if 1: if self.mode == 'play': pose = self.vo.handle_frame(af) if self.mode == 'learn': pose = self.vo.handle_frame(af) if (af.id != 0) and (self.vo.inl < 80): print "*** LOST TRACK ***" #sys.exit(1) self.library.add(self.vo.keyframe) else: #diag = "best match %d from %d in library" % (max(probes)[0], len(self.library)) pass diag = "%d/%d inliers, moved %.1f library size %d" % ( self.vo.inl, len(af.kp), pose.distance(), len( self.library)) if self.mode == 'play': kf = self.vo.keyframe if kf != self.previous_keyframe: f = Frame() f.id = kf.id f.pose = Pose44(kf.pose.tolist()) f.keypoints = [ Keypoint(x, y, d) for (x, y, d) in kf.kp ] f.descriptors = [Descriptor(d) for d in kf.descriptors] print "ASKING FOR MATCH AT", time.time() self.vo_key_pub.publish(f) self.previous_keyframe = kf if kf.inl < 50 or self.vo.inl < 50: self.know_state = 'lost' else: self.know_state = { 'lost': 'lost', 'uncertain': 'uncertain', 'corrected': 'uncertain' }[self.know_state] result_pose = af.pose if self.mode == 'learn': self.mymarker1.frompose(af.pose, self.vo.cam, (255, 255, 255)) else: if self.best_show_pose and self.know_state == 'lost': inmap = self.best_show_pose else: Top = af.pose Tmo = self.Tmo inmap = Tmo * Top if self.know_state != 'lost': self.best_show_pose = inmap if self.know_state != 'lost' or not self.best_show_pose: color = { 'lost': (255, 0, 0), 'uncertain': (127, 127, 0), 'corrected': (0, 255, 0) }[self.know_state] self.mymarker1.frompose(inmap, self.vo.cam, color) if 0: self.trail.append(inmap) self.trail = self.trail[-10:] for i, p in enumerate(self.trail): self.mymarkertrail[i].frompose(p, color) #print af.diff_pose.xform(0,0,0), af.pose.xform(0,0,0) if self.frame > 5 and ((self.frame % 10) == 0): inliers = self.vo.pe.inliers() pts = [(1, int(x0), int(y0)) for ((x0, y0, d0), (x1, y1, d1)) in inliers] self.vis.show(iar.images[1].data, pts) if False and self.vo.pairs != []: ls = Lines() inliers = self.vo.pe.inliers() lr = "left_rectified" ls.lines = [ Line(lr, 0, 255, 0, x0, y0 - 2, x0, y0 + 2) for ((x0, y0, d0), (x1, y1, d1)) in inliers ] ls.lines += [ Line(lr, 0, 255, 0, x0 - 2, y0, x0 + 2, y0) for ((x0, y0, d0), (x1, y1, d1)) in inliers ] rr = "right_rectified" #ls.lines += [ Line(rr, 0,255,0,x0-d0,y0-2,x0-d0,y0+2) for ((x0,y0,d0), (x1,y1,d1)) in inliers] #ls.lines += [ Line(rr, 0,255,0,x0-2-d0,y0,x0+2-d0,y0) for ((x0,y0,d0), (x1,y1,d1)) in inliers] self.viz_pub.publish(ls) if (self.frame % 30) == 0: took = time.time() - self.started print "%4d: %5.1f [%f fps]" % (self.frame, took, self.frame / took), diag self.frame += 1 #print "got message", len(iar.images) #print iar.images[0].width if SEE: right = ut.ros2cv(iar.images[0]) left = ut.ros2cv(iar.images[1]) hg.cvShowImage('channel L', left) hg.cvShowImage('channel R', right) hg.cvWaitKey(5) def dump(self): print print self.vo.name() self.vo.summarize_timers() if self.mode == 'learn': print "DUMPING LIBRARY", len(self.library) f = open("library.pickle", "w") pickle.dump(self.vo.cam, f) db = [(af.id, af.pose, af.kp, af.descriptors) for af in self.library] pickle.dump(db, f) f.close() print "DONE"
def xtest_sim(self): # Test process with one 'ideal' camera, one real-world Videre camera_param_list = [ # (200.0, 200.0, 3.00, 320.0, 320.0, 240.0), (389.0, 389.0, 89.23, 323.42, 323.42, 274.95) ] def move_combo(i): R = rotation(i*0.02, 0, 1, 0) S = (i * -0.01,0,0) return Pose(R, S) def move_translate(i): R = rotation(0, 0, 1, 0) S = (0,0,0) return Pose(R, S) def move_Yrot(i): R = rotation(i*0.02, 0, 1, 0) S = (i * 0,0,0) return Pose(R, S) for movement in [ move_combo, move_Yrot ]: for cam_params in camera_param_list: cam = camera.Camera(cam_params) kps = [] model = [ (x*200,y*200,z*200) for x in range(-3,4) for y in range(-3,4) for z in range(-3,4) ] def rndimg(): b = "".join(random.sample([ chr(c) for c in range(256) ], 64)) return Image.fromstring("L", (8,8), b) def sprite(dst, x, y, src): try: dst.paste(src, (int(x)-4,int(y)-4)) except: print "paste failed", x, y random.seed(0) palette = [ rndimg() for i in model ] expected = [] afs = [] for i in range(100): P = movement(i) li = Image.new("L", (640, 480)) ri = Image.new("L", (640, 480)) q = 0 for (mx,my,mz) in model: pp = None pt_camera = (numpy.dot(P.M.I, numpy.array([mx,my,mz,1]).T)) (cx,cy,cz,cw) = numpy.array(pt_camera).ravel() if cz > 100: ((xl,yl),(xr,yr)) = cam.cam2pixLR(cx, cy, cz) if 0 <= xl and xl < 640 and 0 <= yl and yl < 480: sprite(li, xl, yl, palette[q]) sprite(ri, xr, yr, palette[q]) q += 1 #li.save("sim/left-%04d.png" % i) #ri.save("sim/right-%04d.png" % i) expected.append(P) afs.append(SparseStereoFrame(li, ri)) threshes = [0.0, 0.01, 0.02, 0.03, 0.04, 0.05, 0.06] threshes = [ 0.001 * i for i in range(100) ] threshes = range(100, 500, 10) error = [] for thresh in threshes: vo = VisualOdometer(cam, inlier_thresh = thresh) for (e,af) in zip(expected, afs)[::1]: vo.handle_frame(af)
class VODemo: vo = None frame = 0 def __init__(self, mode, library): self.mode = mode self.library = library rospy.TopicSub('/videre/images', ImageArray, self.display_array) rospy.TopicSub('/videre/cal_params', String, self.display_params) rospy.TopicSub('/vo/tmo', Pose44, self.handle_corrections) self.viz_pub = rospy.Publisher("/overlay", Lines) self.vo_key_pub = rospy.Publisher("/vo/key", Frame) self.Tmo = Pose() self.mymarker1 = Marker(10) self.mymarkertrail = [ Marker(11 + i) for i in range(10) ] self.trail = [] self.vis = Vis() def display_params(self, iar): if not self.vo: cam = camera.VidereCamera(iar.data) print "cam.params", cam.params self.vo = VisualOdometer(cam) self.started = None if self.mode == 'learn': self.library = set() self.previous_keyframe = None self.know_state = 'lost' self.best_show_pose = None self.mymarker1.floor() def handle_corrections(self, corrmsg): print "GOT CORRECTION AT", time.time() Tmo_pose = Pose() Tmo_pose.fromlist(corrmsg.v) self.Tmo = Tmo_pose self.know_state = 'corrected' def display_array(self, iar): diag = "" af = None if self.vo: if not self.started: self.started = time.time() imgR = imgAdapted(iar.images[0]) imgL = imgAdapted(iar.images[1]) af = SparseStereoFrame(imgL, imgR) if 1: if self.mode == 'play': pose = self.vo.handle_frame(af) if self.mode == 'learn': pose = self.vo.handle_frame(af) if (af.id != 0) and (self.vo.inl < 80): print "*** LOST TRACK ***" #sys.exit(1) self.library.add(self.vo.keyframe) else: #diag = "best match %d from %d in library" % (max(probes)[0], len(self.library)) pass diag = "%d/%d inliers, moved %.1f library size %d" % (self.vo.inl, len(af.kp), pose.distance(), len(self.library)) if self.mode == 'play': kf = self.vo.keyframe if kf != self.previous_keyframe: f = Frame() f.id = kf.id f.pose = Pose44(kf.pose.tolist()) f.keypoints = [ Keypoint(x,y,d) for (x,y,d) in kf.kp ] f.descriptors = [ Descriptor(d) for d in kf.descriptors ] print "ASKING FOR MATCH AT", time.time() self.vo_key_pub.publish(f) self.previous_keyframe = kf if kf.inl < 50 or self.vo.inl < 50: self.know_state = 'lost' else: self.know_state = { 'lost':'lost', 'uncertain':'uncertain', 'corrected':'uncertain' }[self.know_state] result_pose = af.pose if self.mode == 'learn': self.mymarker1.frompose(af.pose, self.vo.cam, (255,255,255)) else: if self.best_show_pose and self.know_state == 'lost': inmap = self.best_show_pose else: Top = af.pose Tmo = self.Tmo inmap = Tmo * Top if self.know_state != 'lost': self.best_show_pose = inmap if self.know_state != 'lost' or not self.best_show_pose: color = { 'lost' : (255,0,0), 'uncertain' : (127,127,0), 'corrected' : (0,255,0) }[self.know_state] self.mymarker1.frompose(inmap, self.vo.cam, color) if 0: self.trail.append(inmap) self.trail = self.trail[-10:] for i,p in enumerate(self.trail): self.mymarkertrail[i].frompose(p, color) #print af.diff_pose.xform(0,0,0), af.pose.xform(0,0,0) if self.frame > 5 and ((self.frame % 10) == 0): inliers = self.vo.pe.inliers() pts = [(1,int(x0),int(y0)) for ((x0,y0,d0), (x1,y1,d1)) in inliers] self.vis.show(iar.images[1].data, pts ) if False and self.vo.pairs != []: ls = Lines() inliers = self.vo.pe.inliers() lr = "left_rectified" ls.lines = [ Line(lr, 0,255,0,x0,y0-2,x0,y0+2) for ((x0,y0,d0), (x1,y1,d1)) in inliers] ls.lines += [ Line(lr, 0,255,0,x0-2,y0,x0+2,y0) for ((x0,y0,d0), (x1,y1,d1)) in inliers] rr = "right_rectified" #ls.lines += [ Line(rr, 0,255,0,x0-d0,y0-2,x0-d0,y0+2) for ((x0,y0,d0), (x1,y1,d1)) in inliers] #ls.lines += [ Line(rr, 0,255,0,x0-2-d0,y0,x0+2-d0,y0) for ((x0,y0,d0), (x1,y1,d1)) in inliers] self.viz_pub.publish(ls) if (self.frame % 30) == 0: took = time.time() - self.started print "%4d: %5.1f [%f fps]" % (self.frame, took, self.frame / took), diag self.frame += 1 #print "got message", len(iar.images) #print iar.images[0].width if SEE: right = ut.ros2cv(iar.images[0]) left = ut.ros2cv(iar.images[1]) hg.cvShowImage('channel L', left) hg.cvShowImage('channel R', right) hg.cvWaitKey(5) def dump(self): print print self.vo.name() self.vo.summarize_timers() if self.mode == 'learn': print "DUMPING LIBRARY", len(self.library) f = open("library.pickle", "w") pickle.dump(self.vo.cam, f) db = [(af.id, af.pose, af.kp, af.descriptors) for af in self.library] pickle.dump(db, f) f.close() print "DONE"
def xtest_sim(self): # Test process with one 'ideal' camera, one real-world Videre camera_param_list = [ # (200.0, 200.0, 3.00, 320.0, 320.0, 240.0), (389.0, 389.0, 89.23, 323.42, 323.42, 274.95) ] def move_combo(i): R = rotation(i * 0.02, 0, 1, 0) S = (i * -0.01, 0, 0) return Pose(R, S) def move_translate(i): R = rotation(0, 0, 1, 0) S = (0, 0, 0) return Pose(R, S) def move_Yrot(i): R = rotation(i * 0.02, 0, 1, 0) S = (i * 0, 0, 0) return Pose(R, S) for movement in [move_combo, move_Yrot]: for cam_params in camera_param_list: cam = camera.Camera(cam_params) kps = [] model = [(x * 200, y * 200, z * 200) for x in range(-3, 4) for y in range(-3, 4) for z in range(-3, 4)] def rndimg(): b = "".join(random.sample([chr(c) for c in range(256)], 64)) return Image.fromstring("L", (8, 8), b) def sprite(dst, x, y, src): try: dst.paste(src, (int(x) - 4, int(y) - 4)) except: print "paste failed", x, y random.seed(0) palette = [rndimg() for i in model] expected = [] afs = [] for i in range(100): P = movement(i) li = Image.new("L", (640, 480)) ri = Image.new("L", (640, 480)) q = 0 for (mx, my, mz) in model: pp = None pt_camera = (numpy.dot(P.M.I, numpy.array([mx, my, mz, 1]).T)) (cx, cy, cz, cw) = numpy.array(pt_camera).ravel() if cz > 100: ((xl, yl), (xr, yr)) = cam.cam2pixLR(cx, cy, cz) if 0 <= xl and xl < 640 and 0 <= yl and yl < 480: sprite(li, xl, yl, palette[q]) sprite(ri, xr, yr, palette[q]) q += 1 #li.save("sim/left-%04d.png" % i) #ri.save("sim/right-%04d.png" % i) expected.append(P) afs.append(SparseStereoFrame(li, ri)) threshes = [0.0, 0.01, 0.02, 0.03, 0.04, 0.05, 0.06] threshes = [0.001 * i for i in range(100)] threshes = range(100, 500, 10) error = [] for thresh in threshes: vo = VisualOdometer(cam, inlier_thresh=thresh) for (e, af) in zip(expected, afs)[::1]: vo.handle_frame(af)
def test_sim(self): # Test process with one 'ideal' camera, one real-world Videre camera_param_list = [ # (200.0, 200.0, 3.00, 320.0, 320.0, 240.0), (389.0, 389.0, 1e-3 * 89.23, 323.42, 323.42, 274.95) ] 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)) for movement in [ move_forward ]: # move_combo, move_Yrot ]: for cam_params in camera_param_list: cam = camera.Camera(cam_params) random.seed(0) def rr(): return 2 * random.random() - 1.0 model = [ (3 * rr(), 1 * rr(), 3 * rr()) for i in range(300) ] def rndimg(): b = "".join(random.sample([ chr(c) for c in range(256) ], 64)) return Image.fromstring("L", (8,8), b) def sprite(dst, x, y, src): try: dst.paste(src, (int(x)-4,int(y)-4)) except: print "paste failed", x, y palette = [ rndimg() for i in model ] expected = [] afs = [] P = None for i in range(50): P = movement(i, P) li = Image.new("L", (640, 480)) ri = Image.new("L", (640, 480)) q = 0 for (mx,my,mz) in model: pp = None pt_camera = (numpy.dot(P.M.I, numpy.array([mx,my,mz,1]).T)) (cx,cy,cz,cw) = numpy.array(pt_camera).ravel() if cz > .100: ((xl,yl),(xr,yr)) = cam.cam2pixLR(cx, cy, cz) if 0 <= xl and xl < 640 and 0 <= yl and yl < 480: sprite(li, xl, yl, palette[q]) sprite(ri, xr, yr, palette[q]) q += 1 expected.append(P) afs.append(SparseStereoFrame(imgStereo(li), imgStereo(ri))) vo = VisualOdometer(cam) for i,(af,ep) in enumerate(zip(afs, expected)): vo.handle_frame(af) if 0: print vo.pose.xform(0,0,0) print "expected", ep.M print "vo.pose", vo.pose.M print numpy.abs((ep.M - vo.pose.M)) self.assert_(numpy.alltrue(numpy.abs((ep.M - vo.pose.M)) < 0.2)) def run(vos): for af in afs: for vo in vos: vo.handle_frame(af) # Check that the pose estimators are truly independent v1 = VisualOdometer(cam, feature_detector = FeatureDetectorFast(), descriptor_scheme = DescriptorSchemeSAD(), inlier_error_threshold=1.0) v2 = VisualOdometer(cam, feature_detector = FeatureDetectorFast(), descriptor_scheme = DescriptorSchemeSAD(), inlier_error_threshold=2.0) v8 = VisualOdometer(cam, feature_detector = FeatureDetectorFast(), descriptor_scheme = DescriptorSchemeSAD(), inlier_error_threshold=8.0) v1a = VisualOdometer(cam, feature_detector = FeatureDetectorFast(), descriptor_scheme = DescriptorSchemeSAD(), inlier_error_threshold=1.0) run([v1]) run([v2,v8,v1a]) self.assert_(v1.pose.xform(0,0,0) == v1a.pose.xform(0,0,0)) for a,b in [ (v1,v2), (v2,v8), (v1, v8) ]: self.assert_(a.pose.xform(0,0,0) != b.pose.xform(0,0,0)) return