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()
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()
def xtest_image_pan(self):
cam = camera.Camera((1.0, 1.0, 89.23, 320., 320., 240.0))
vo = VisualOdometer(cam)
prev_af = None
pose = None
im = Image.open("img1.pgm")
for x in [0,
5]: # range(0,100,10) + list(reversed(range(0, 100, 10))):
lf = im.crop((x, 0, x + 640, 480))
rf = im.crop((x, 0, x + 640, 480))
af = SparseStereoFrame(lf, rf)
vo.find_keypoints(af)
vo.find_disparities(af)
vo.collect_descriptors(af)
if prev_af:
pairs = vo.temporal_match(prev_af, af)
pose = vo.solve(prev_af.kp, af.kp, pairs)
for i in range(10):
old = prev_af.kp[pairs[i][0]]
new = af.kp[pairs[i][1]]
print old, new, new[0] - old[0]
prev_af = af
print "frame", x, "has", len(af.kp), "keypoints", pose
def xtest_image_pan(self):
cam = camera.Camera((1.0, 1.0, 89.23, 320., 320., 240.0))
vo = VisualOdometer(cam)
prev_af = None
pose = None
im = Image.open("img1.pgm")
for x in [0,5]: # range(0,100,10) + list(reversed(range(0, 100, 10))):
lf = im.crop((x, 0, x + 640, 480))
rf = im.crop((x, 0, x + 640, 480))
af = SparseStereoFrame(lf, rf)
vo.find_keypoints(af)
vo.find_disparities(af)
vo.collect_descriptors(af)
if prev_af:
pairs = vo.temporal_match(prev_af, af)
pose = vo.solve(prev_af.kp, af.kp, pairs)
for i in range(10):
old = prev_af.kp[pairs[i][0]]
new = af.kp[pairs[i][1]]
print old, new, new[0] - old[0]
prev_af = af
print "frame", x, "has", len(af.kp), "keypoints", pose
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 test_solve_spin(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),
]
for cam_params in camera_param_list:
cam = camera.Camera(cam_params)
vo = VisualOdometer(cam)
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)]
for angle in range(80):
im = Image.new("L", (640, 480))
theta = (angle / 80.) * (pi * 2)
R = rotation(theta, 0, 1, 0)
kp = []
for (mx, my, mz) in model:
pp = None
pt_camera = (numpy.dot(numpy.array([mx, my, mz]), R))
(cx, cy, cz) = numpy.array(pt_camera).ravel()
if cz > 100:
(x, y, d) = cam.cam2pix(cx, cy, cz)
if 0 <= x and x < 640 and 0 <= y and y < 480:
pp = (x, y, d)
circle(im, x, y, 2, 255)
kp.append(pp)
kps.append(kp)
expected_rot = numpy.array(
numpy.mat(rotation(2 * pi / 80, 0, 1, 0))).ravel()
for i in range(100):
i0 = i % 80
i1 = (i + 1) % 80
pairs = [(i, i) for i in range(len(model))
if (kps[i0][i] and kps[i1][i])]
def sanify(L, sub):
return [i or sub for i in L]
(inliers, rot, shift) = vo.solve(sanify(kps[i0], (0, 0, 0)),
sanify(kps[i1], (0, 0, 0)),
pairs)
self.assert_(inliers != 0)
self.assertAlmostEqual(shift[0], 0.0, 3)
self.assertAlmostEqual(shift[1], 0.0, 3)
self.assertAlmostEqual(shift[2], 0.0, 3)
for (et, at) in zip(rot, expected_rot):
self.assertAlmostEqual(et, at, 3)
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 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()
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)
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)
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 test_solve_spin(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),
]
for cam_params in camera_param_list:
cam = camera.Camera(cam_params)
vo = VisualOdometer(cam)
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) ]
for angle in range(80):
im = Image.new("L", (640, 480))
theta = (angle / 80.) * (pi * 2)
R = rotation(theta, 0, 1, 0)
kp = []
for (mx,my,mz) in model:
pp = None
pt_camera = (numpy.dot(numpy.array([mx,my,mz]), R))
(cx,cy,cz) = numpy.array(pt_camera).ravel()
if cz > 100:
(x,y,d) = cam.cam2pix(cx, cy, cz)
if 0 <= x and x < 640 and 0 <= y and y < 480:
pp = (x,y,d)
circle(im, x, y, 2, 255)
kp.append(pp)
kps.append(kp)
expected_rot = numpy.array(numpy.mat(rotation(2 * pi / 80, 0, 1, 0))).ravel()
for i in range(100):
i0 = i % 80
i1 = (i + 1) % 80
pairs = [ (i,i) for i in range(len(model)) if (kps[i0][i] and kps[i1][i]) ]
def sanify(L, sub):
return [i or sub for i in L]
(inliers, rot, shift) = vo.solve(sanify(kps[i0],(0,0,0)), sanify(kps[i1],(0,0,0)), pairs)
self.assert_(inliers != 0)
self.assertAlmostEqual(shift[0], 0.0, 3)
self.assertAlmostEqual(shift[1], 0.0, 3)
self.assertAlmostEqual(shift[2], 0.0, 3)
for (et, at) in zip(rot, expected_rot):
self.assertAlmostEqual(et, at, 3)
def test_solve_rotation(self):
cam = camera.Camera((389.0, 389.0, 89.23, 323.42, 323.42, 274.95))
vo = VisualOdometer(cam)
model = []
radius = 1200.0
kps = []
for angle in range(80):
im = Image.new("L", (640, 480))
theta = (angle / 80.) * (pi * 2)
R = rotation(theta, 0, 1, 0)
kp = []
for s in range(7):
for t in range(7):
y = -400
pt_model = numpy.array([110 * (s - 3), y,
110 * (t - 3)]).transpose()
pt_camera = (numpy.dot(pt_model, R) +
numpy.array([0, 0, radius])).transpose()
(cx, cy, cz) = [float(i) for i in pt_camera]
(x, y, d) = cam.cam2pix(cx, cy, cz)
reversed = cam.pix2cam(x, y, d)
self.assertAlmostEqual(cx, reversed[0], 3)
self.assertAlmostEqual(cy, reversed[1], 3)
self.assertAlmostEqual(cz, reversed[2], 3)
kp.append((x, y, d))
circle(im, x, y, 2, 255)
kps.append(kp)
expected_shift = 2 * radius * sin(pi / 80)
for i in range(100):
i0 = i % 80
i1 = (i + 1) % 80
pairs = zip(range(len(kps[i0])), range(len(kps[i1])))
(inliers, rod, shift) = vo.solve(kps[i0], kps[i1], pairs)
actual_shift = sqrt(shift[0] * shift[0] + shift[1] * shift[1] +
shift[2] * shift[2])
# Should be able to estimate camera shift to nearest thousandth of mm
self.assertAlmostEqual(actual_shift, expected_shift, 3)
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)
def test_solve_rotation(self):
cam = camera.Camera((389.0, 389.0, 89.23, 323.42, 323.42, 274.95))
vo = VisualOdometer(cam)
model = []
radius = 1200.0
kps = []
for angle in range(80):
im = Image.new("L", (640, 480))
theta = (angle / 80.) * (pi * 2)
R = rotation(theta, 0, 1, 0)
kp = []
for s in range(7):
for t in range(7):
y = -400
pt_model = numpy.array([110 * (s - 3), y, 110 * (t - 3)]).transpose()
pt_camera = (numpy.dot(pt_model, R) + numpy.array([0, 0, radius])).transpose()
(cx, cy, cz) = [ float(i) for i in pt_camera ]
(x,y,d) = cam.cam2pix(cx, cy, cz)
reversed = cam.pix2cam(x, y, d)
self.assertAlmostEqual(cx, reversed[0], 3)
self.assertAlmostEqual(cy, reversed[1], 3)
self.assertAlmostEqual(cz, reversed[2], 3)
kp.append((x,y,d))
circle(im, x, y, 2, 255)
kps.append(kp)
expected_shift = 2 * radius * sin(pi / 80)
for i in range(100):
i0 = i % 80
i1 = (i + 1) % 80
pairs = zip(range(len(kps[i0])), range(len(kps[i1])))
(inliers, rod, shift) = vo.solve(kps[i0], kps[i1], pairs)
actual_shift = sqrt(shift[0]*shift[0] + shift[1]*shift[1] + shift[2]*shift[2])
# Should be able to estimate camera shift to nearest thousandth of mm
self.assertAlmostEqual(actual_shift, expected_shift, 3)
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 test_stereo(self):
cam = camera.VidereCamera(open("wallcal.ini").read())
#lf = Image.open("wallcal-L.bmp").convert("L")
#rf = Image.open("wallcal-R.bmp").convert("L")
for offset in [ 1, 10, 10.25, 10.5, 10.75, 11, 63]:
lf = Image.open("snap.png").convert("L")
rf = Image.open("snap.png").convert("L")
rf = rf.resize((16 * 640, 480))
rf = ImageChops.offset(rf, -int(offset * 16), 0)
rf = rf.resize((640,480), Image.ANTIALIAS)
for gradient in [ False, True ]:
af = SparseStereoFrame(lf, rf, gradient)
vo = VisualOdometer(cam)
vo.find_keypoints(af)
vo.find_disparities(af)
error = offset - sum([d for (x,y,d) in af.kp]) / len(af.kp)
self.assert_(abs(error) < 0.25)
if 0:
scribble = Image.merge("RGB", (lf,rf,Image.new("L", lf.size))).resize((1280,960))
#scribble = Image.merge("RGB", (Image.fromstring("L", lf.size, af0.lgrad),Image.fromstring("L", lf.size, af0.rgrad),Image.new("L", lf.size))).resize((1280,960))
draw = ImageDraw.Draw(scribble)
for (x,y,d) in af0.kp:
draw.line([ (2*x,2*y), (2*x-2*d,2*y) ], fill = (255,255,255))
for (x,y,d) in af1.kp:
draw.line([ (2*x,2*y+1), (2*x-2*d,2*y+1) ], fill = (0,0,255))
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 params(self, pmsg):
if not self.vo:
self.cam = camera.VidereCamera(pmsg.data)
if DESCRIPTOR == 'CALONDER':
self.vo = VisualOdometer(
self.cam, descriptor_scheme=DescriptorSchemeCalonder())
self.desc_diff_thresh = 0.001
elif DESCRIPTOR == 'SAD':
self.vo = Tracker(self.cam)
self.desc_diff_thresh = 2000.0
else:
print "Unknown descriptor"
return
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))
def test_sad(self):
cam = camera.Camera((389.0, 389.0, 89.23, 323.42, 323.42, 274.95))
vo = VisualOdometer(cam)
class adapter:
def __init__(self, im):
self.rawdata = im.tostring()
self.size = im.size
im = adapter(Image.open("img1.pgm"))
vo.feature_detector.thresh *= 15
vo.find_keypoints(im)
im.kp = im.kp2d
vo.collect_descriptors(im)
print len(im.kp)
matches = vo.temporal_match(im, im)
for (a, b) in matches:
self.assert_(a == b)
def main(args):
f = open("pruned.pickle", "r")
cam = pickle.load(f)
db = pickle.load(f)
f.close()
print cam.params
vo = VisualOdometer(cam)
library = set()
for (id, pose, kp, desc) in db:
lf = LibraryFrame(id, pose, kp, desc)
library.add(lf)
corr = Corrector(vo, library)
rospy.ready('corrector')
try:
corr.workloop()
except KeyboardInterrupt:
print "shutting down"
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 test_sad(self):
cam = camera.Camera((389.0, 389.0, 89.23, 323.42, 323.42, 274.95))
vo = VisualOdometer(cam)
class adapter:
def __init__(self, im):
self.rawdata = im.tostring()
self.size = im.size
im = adapter(Image.open("img1.pgm"))
vo.feature_detector.thresh *= 15
vo.find_keypoints(im)
im.kp = im.kp2d
vo.collect_descriptors(im)
print len(im.kp)
matches = vo.temporal_match(im, im)
for (a,b) in matches:
self.assert_(a == b)
import rostest
import rospy
import Image
from stereo_utils import camera
import pylab, numpy
from stereo import ComputedDenseStereoFrame, SparseStereoFrame
from visualodometer import VisualOdometer, Pose, DescriptorSchemeCalonder, DescriptorSchemeSAD, FeatureDetectorFast, FeatureDetector4x4, FeatureDetectorStar, FeatureDetectorHarris, from_xyz_euler
stereo_cam = camera.Camera(
(389.0, 389.0, 89.23 * 1e-3, 323.42, 323.42, 274.95))
vo = VisualOdometer(stereo_cam,
feature_detector=FeatureDetectorStar(),
descriptor_scheme=DescriptorSchemeCalonder())
(f0, f1) = [
SparseStereoFrame(Image.open("f%d-left.png" % i),
Image.open("f%d-right.png" % i)) for i in [0, 1]
]
vo.setup_frame(f0)
vo.setup_frame(f1)
pairs = vo.temporal_match(f0, f1)
for (a, b) in pairs:
pylab.plot([f0.kp[a][0], f1.kp[b][0]], [f0.kp[a][1], f1.kp[b][1]])
pylab.imshow(numpy.fromstring(f0.lf.tostring(), numpy.uint8).reshape(480, 640),
cmap=pylab.cm.gray)
1221, 1225, 1227, 1233, 1237, 1244, 1250, 1258, 1263, 1269, 1274, 1280,
1285, 1291, 1298, 1305, 1308, 1313, 1317, 1320, 1323, 1325, 1328, 1329,
1332, 1334, 1335, 1337, 1339, 1340, 1341, 1342, 1343, 1344, 1345, 1346,
1347, 1348, 1349, 1350, 1352, 1354, 1355, 1356, 1357, 1358, 1359, 1360,
1361, 1362, 1363, 1367, 1372, 1377, 1379, 1381, 1382, 1383, 1384, 1385,
1386, 1388, 1391, 1394, 1395, 1399, 1404, 1411, 1418, 1421, 1423, 1429,
1432, 1436, 1438, 1440, 1444, 1447, 1455
][::100])
# Example 2: load frames 560,570... 940 from loop1-mono.bag
# (cam, afs) = load_from_bag( "/u/prdata/videre-bags/loop1-mono.bag", range(560, 941, 100))
vos = [
VisualOdometer(cam,
feature_detector=FeatureDetectorStar(),
descriptor_scheme=DescriptorSchemeCalonder()),
VisualOdometer(cam,
feature_detector=FeatureDetectorFast(),
descriptor_scheme=DescriptorSchemeCalonder()),
VisualOdometer(cam,
feature_detector=FeatureDetectorHarris(),
descriptor_scheme=DescriptorSchemeCalonder())
]
def metric(af0, af1):
""" Given a pair of frames, return the distance metric using inliers from the solved pose """
pairs = vo.temporal_match(af0, af1)
if len(pairs) > 10:
(inl, rot, shift) = vo.solve(af0.kp, af1.kp, pairs)
for filename in sys.argv[1:]:
cam = None
prev_frame = None
framecounter = 0
all_ds = []
sos = numpy.array([.0, .0, .0])
for topic, msg in rosrecord.logplayer(filename):
if rospy.is_shutdown():
break
if topic.endswith("videre/cal_params") and not cam:
cam = camera.VidereCamera(msg.data)
vo = VisualOdometer(cam, feature_detector = FeatureDetectorFast(), descriptor_scheme = DescriptorSchemeSAD())
if cam and topic.endswith("videre/images"):
imgR = imgAdapted(msg.images[0])
imgL = imgAdapted(msg.images[1])
assert msg.images[0].label == "right_rectified"
assert msg.images[1].label == "left_rectified"
frame = SparseStereoFrame(imgL, imgR)
vo.find_keypoints(frame)
vo.find_disparities(frame)
#frame.kp = [ (x,y,d) for (x,y,d) in frame.kp if d > 8]
all_ds += [ d for (x,y,d) in frame.kp ]
vo.collect_descriptors(frame)
if prev_frame:
import cPickle as pickle
import math
import random
import time
import sys
random.seed(0)
cam = camera.Camera((432.0, 432.0, 0.088981018518518529, 313.78210000000001,
313.78210000000001, 220.40700000000001))
vo = VisualOdometer(cam,
scavenge=False,
feature_detector=FeatureDetectorFast(),
descriptor_scheme=DescriptorSchemeCalonder(),
inlier_error_threshold=3.0,
sba=None,
inlier_thresh=99999,
position_keypoint_thresh=0.2,
angle_keypoint_thresh=0.15)
dir = "dump"
f = []
for i in [int(a) for a in sys.argv[1:]]:
L = Image.open("%s/%06dL.png" % (dir, i))
R = Image.open("%s/%06dR.png" % (dir, i))
nf = SparseStereoFrame(L, R)
vo.setup_frame(nf)
nf.id = len(f)
f.append(nf)
class LibraryFrame:
def __init__(self, pose, kp, desc):
self.pose = pose
self.kp = kp
self.descriptors = desc
afs = {}
for (id, pose, kp, desc) in db:
lf = LibraryFrame(pose, kp, desc)
afs[id] = lf
vo = VisualOdometer(cam,
feature_detector=FeatureDetectorFast(),
descriptor_scheme=DescriptorSchemeSAD())
print "%d frames in library" % len(afs)
most_overlap = 99999
while most_overlap > 180:
ov = []
for i in afs.keys():
for j in [k for k in afs.keys() if k > i]:
(overlap, _) = vo.proximity(afs[i], afs[j])
ov.append((overlap, i))
print max(ov)
most_overlap = max(ov)[0]
del afs[max(ov)[1]]
print "%d frames in library" % len(afs)
import pylab, numpy
import cPickle as pickle
import math
import random
import time
random.seed(0)
pg = TreeOptimizer3()
pg.initializeOnlineOptimization()
cam = camera.Camera((432.0, 432.0, 0.088981018518518529, 313.78210000000001,
313.78210000000001, 220.40700000000001))
#vo = VisualOdometer(cam)
vo = VisualOdometer(cam,
scavenge=False,
feature_detector=FeatureDetectorFast(),
descriptor_scheme=DescriptorSchemeCalonder())
def pg_constraint(pg, a, b, pose, inf):
pg.addIncrementalEdge(a, b, pose.xform(0, 0, 0), pose.euler(), inf)
def newpose(id):
xyz, euler = pg.vertex(id)
return from_xyz_euler(xyz, euler)
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))
for filename in sys.argv[1:]:
cam = None
prev_frame = None
framecounter = 0
all_ds = []
sos = numpy.array([.0, .0, .0])
for topic, msg in rosrecord.logplayer(filename):
if rospy.is_shutdown():
break
if topic.endswith("videre/cal_params") and not cam:
cam = camera.VidereCamera(msg.data)
vo = VisualOdometer(cam,
feature_detector=FeatureDetectorFast(),
descriptor_scheme=DescriptorSchemeSAD())
if cam and topic.endswith("videre/images"):
imgR = imgAdapted(msg.images[0])
imgL = imgAdapted(msg.images[1])
assert msg.images[0].label == "right_rectified"
assert msg.images[1].label == "left_rectified"
frame = SparseStereoFrame(imgL, imgR)
vo.find_keypoints(frame)
vo.find_disparities(frame)
#frame.kp = [ (x,y,d) for (x,y,d) in frame.kp if d > 8]
all_ds += [d for (x, y, d) in frame.kp]
vo.collect_descriptors(frame)
#vo = VisualOdometer(cam, inlier_thresh = 999999, descriptor_scheme = DescriptorSchemeCalonder())
#vo2 = VisualOdometer(cam, inlier_thresh = 999999, descriptor_scheme = DescriptorSchemeSAD())
#vo1 = VisualOdometer(cam, feature_detector = FeatureDetectorHarris(), descriptor_scheme = DescriptorSchemeCalonder())
#vo2 = VisualOdometer(cam, feature_detector = FeatureDetectorHarris(), descriptor_scheme = DescriptorSchemeSAD())
#vo = VisualOdometer(cam, descriptor_scheme = DescriptorSchemeCalonder())
#vo2 = VisualOdometer(cam, descriptor_scheme = DescriptorSchemeSAD())
#vo = VisualOdometer(cam, feature_detector = FeatureDetectorFast())
#vo2 = VisualOdometer(cam, feature_detector = FeatureDetector4x4(FeatureDetectorFast))
#vos = [vo1,vo2]
vos = [
VisualOdometer(cam,
feature_detector=FeatureDetectorFast(),
descriptor_scheme=DescriptorSchemeSAD()),
# VisualOdometer(cam, feature_detector = FeatureDetectorFast(), descriptor_scheme = DescriptorSchemeSAD(), scavenge = True),
]
start, end = 0, 10
if cam and topic.endswith("videre/images"):
print framecounter
if framecounter == end:
break
if start <= framecounter and (framecounter % 1) == 0:
imgR = imgAdapted(msg.images[0])
imgL = imgAdapted(msg.images[1])
w, h = imgL.size
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"
cam = pickle.load(f)
db = pickle.load(f)
f.close()
class LibraryFrame:
def __init__(self, pose, kp, desc):
self.pose = pose
self.kp = kp
self.descriptors = desc
afs = {}
for (id, pose, kp, desc) in db:
lf = LibraryFrame(pose, kp, desc)
afs[id] = lf
vo = VisualOdometer(cam, feature_detector = FeatureDetectorFast(), descriptor_scheme = DescriptorSchemeSAD())
print "%d frames in library" % len(afs)
most_overlap = 99999
while most_overlap > 180:
ov = []
for i in afs.keys():
for j in [k for k in afs.keys() if k > i]:
(overlap, _) = vo.proximity(afs[i], afs[j])
ov.append((overlap, i))
print max(ov)
most_overlap = max(ov)[0]
del afs[max(ov)[1]]
print "%d frames in library" % len(afs)
f = open("pruned.pickle", "w")
class PhonyFrame:
def __init__(self, pose, cam):
p3d = [pose.xform(*p) for p in points]
self.kp = [cam.cam2pix(*p) for p in p3d]
def r():
return -0.5 + random.random() * 1
self.kp = [(x + r(), y + r(), d) for (x, y, d) in self.kp]
vos = [
VisualOdometer(stereo_cam,
scavenge=False,
sba=(1, 1, 10),
inlier_error_threshold=3.0),
VisualOdometer(stereo_cam,
scavenge=False,
sba=(1, 1, 10),
inlier_error_threshold=3.0),
#VisualOdometer(stereo_cam, sba=(1,1,1)),
#VisualOdometer(stereo_cam, scavenge = True, sba=None, inlier_error_threshold = 3.0),
#VisualOdometer(stereo_cam, sba=(5,5,10), inlier_error_threshold = 1.5),
#VisualOdometer(stereo_cam, sba=(5,5,10), inlier_error_threshold = 2.0),
#VisualOdometer(stereo_cam, sba=(5,5,10), inlier_error_threshold = 2.5),
#VisualOdometer(stereo_cam, sba=(5,5,10), inlier_error_threshold = 3.0),
#VisualOdometer(stereo_cam, sba=(5,5,10), inlier_error_threshold = 3.5),
#VisualOdometer(stereo_cam, feature_detector = FeatureDetector4x4(FeatureDetectorHarris), scavenge = True, sba = None),
class PhonyFrame:
def __init__(self, pose, cam):
p3d = [pose.xform(*p) for p in points]
self.kp = [cam.cam2pix(*p) for p in p3d]
def r():
return -0.5 + random.random() * 1
self.kp = [(x + r(), y + r(), d) for (x, y, d) in self.kp]
vos = [
#VisualOdometer(stereo_cam, sba=(1,1,1)),
VisualOdometer(stereo_cam, sba=None, inlier_error_threshold=1.0),
VisualOdometer(stereo_cam,
feature_detector=FeatureDetectorHarris(),
sba=(3, 10, 10),
inlier_error_threshold=1.0),
#VisualOdometer(stereo_cam, sba=(5,5,10), inlier_error_threshold = 1.5),
#VisualOdometer(stereo_cam, sba=(5,5,10), inlier_error_threshold = 2.0),
#VisualOdometer(stereo_cam, sba=(5,5,10), inlier_error_threshold = 2.5),
#VisualOdometer(stereo_cam, sba=(5,5,10), inlier_error_threshold = 3.0),
#VisualOdometer(stereo_cam, sba=(5,5,10), inlier_error_threshold = 3.5),
#VisualOdometer(stereo_cam, feature_detector = FeatureDetector4x4(FeatureDetectorHarris), scavenge = True, sba = None),
#PhonyVisualOdometer(stereo_cam),
#PhonyVisualOdometer(stereo_cam, sba = (5,5,5))
#VisualOdometer(stereo_cam, feature_detector = FeatureDetector4x4(FeatureDetectorFast)),
def handle_params(self, iar):
if not self.vo:
cam = camera.VidereCamera(iar.data)
self.vo = VisualOdometer(cam)
self.intervals = []
self.took = []
from visualodometer import VisualOdometer, Pose, DescriptorSchemeCalonder, DescriptorSchemeSAD, FeatureDetectorFast, FeatureDetector4x4, FeatureDetectorStar, FeatureDetectorHarris, from_xyz_euler
from stereo import SparseStereoFrame
from timer import Timer
import pylab, numpy
import cPickle as pickle
import math
import random
import time
random.seed(0)
cam = camera.Camera((432.0, 432.0, 0.088981018518518529, 313.78210000000001,
313.78210000000001, 220.40700000000001))
vo = VisualOdometer(cam,
scavenge=True,
feature_detector=FeatureDetectorFast(),
descriptor_scheme=DescriptorSchemeCalonder())
vt = place_recognition.load("/u/mihelich/images/holidays/holidays.tree")
f = []
for i in range(180): # range(1,146):
print i
L = Image.open("pool_loop/%06dL.png" % i)
R = Image.open("pool_loop/%06dR.png" % i)
nf = SparseStereoFrame(L, R)
vo.setup_frame(nf)
nf.id = len(f)
if vt:
vt.add(nf.lf, nf.descriptors)
f.append(nf)
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"
class PhonyFrame:
def __init__(self, pose, cam):
p3d = [pose.xform(*p) for p in points]
self.kp = [cam.cam2pix(*p) for p in p3d]
def r():
return -0.5 + random.random() * 1
self.kp = [(x + r(), y + r(), d) for (x, y, d) in self.kp]
vos = [
#VisualOdometer(stereo_cam, sba=(1,1,1)),
VisualOdometer(stereo_cam, sba=None, inlier_error_threshold=3.0),
# VisualOdometer(stereo_cam, sba=(3,10,10), inlier_error_threshold = 1.0),
VisualOdometer(stereo_cam, sba=(3, 10, 10), inlier_error_threshold=3.0),
#VisualOdometer(stereo_cam, sba=(5,5,10), inlier_error_threshold = 1.5),
#VisualOdometer(stereo_cam, sba=(5,5,10), inlier_error_threshold = 2.0),
#VisualOdometer(stereo_cam, sba=(5,5,10), inlier_error_threshold = 2.5),
#VisualOdometer(stereo_cam, sba=(5,5,10), inlier_error_threshold = 3.0),
#VisualOdometer(stereo_cam, sba=(5,5,10), inlier_error_threshold = 3.5),
#VisualOdometer(stereo_cam, feature_detector = FeatureDetector4x4(FeatureDetectorHarris), scavenge = True, sba = None),
#PhonyVisualOdometer(stereo_cam),
#PhonyVisualOdometer(stereo_cam, sba = (5,5,5))
#VisualOdometer(stereo_cam, feature_detector = FeatureDetector4x4(FeatureDetectorFast)),
#VisualOdometer(stereo_cam, scavenge = True, sba = (1,1,5)),
]
min_distance = 10.0
return VO.harris(frame.rawdata, frame.size[0], frame.size[1], 1000, quality_level, min_distance)
for topic, msg, t in rosrecord.logplayer(filename):
if rospy.is_shutdown():
break
if topic.endswith("videre/cal_params") and not cam:
print msg.data
cam = camera.VidereCamera(msg.data)
(Fx, Fy, Tx, Clx, Crx, Cy) = cam.params
Tx /= (7.30 / 7.12)
cam = camera.Camera((Fx, Fy, Tx, Clx, Crx, Cy))
vos = [
VisualOdometer(cam, feature_detector = FeatureDetectorFast(), descriptor_scheme = DescriptorSchemeSAD(), sba = None),
VisualOdometer(cam, feature_detector = FeatureDetectorMine(), inlier_error_threshold = 1.0, descriptor_scheme = DescriptorSchemeSAD(), sba = (3,10,10)),
#VisualOdometer(cam, feature_detector = FeatureDetectorFast(), descriptor_scheme = DescriptorSchemeSAD(), sba = (3,8,10)),
#VisualOdometer(cam, feature_detector = FeatureDetectorFast(), descriptor_scheme = DescriptorSchemeSAD()),
#VisualOdometer(cam, feature_detector = FeatureDetectorFast(), descriptor_scheme = DescriptorSchemeSAD(), scavenge = True),
#VisualOdometer(cam, feature_detector = FeatureDetectorFast(), descriptor_scheme = DescriptorSchemeSAD(), scavenge = True, inlier_thresh = 100),
#VisualOdometer(cam, feature_detector = FeatureDetectorHarris(), descriptor_scheme = DescriptorSchemeSAD()),
#VisualOdometer(cam, feature_detector = FeatureDetectorFast(), descriptor_scheme = DescriptorSchemeSAD()),
#VisualOdometer(cam, feature_detector = FeatureDetector4x4(FeatureDetectorFast), descriptor_scheme = DescriptorSchemeSAD()),
]
vo_x = [ [] for i in vos]
vo_y = [ [] for i in vos]
vo_u = [ [] for i in vos]
vo_v = [ [] for i in vos]
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
