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(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):
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_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 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:
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 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
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 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 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 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
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)
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)),
]
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))
def handle_params(self, iar):
if not self.vo:
cam = camera.VidereCamera(iar.data)
self.vo = VisualOdometer(cam)
self.intervals = []
self.took = []
#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(),
scavenge=True,
sba=(3, 10, 10)),
# VisualOdometer(cam, feature_detector = FeatureDetectorFast(), descriptor_scheme = DescriptorSchemeSAD(), scavenge = True),
]
start, end = 0, 100
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])
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)
(389.0, 389.0, 89.23 * 1e-3, 323.42, 323.42, 274.95))
#(f0,f1) = [ ComputedDenseStereoFrame(Image.open("../vslam/kk2/%06dL.png" % i), Image.open("../vslam/kk2/%06dR.png" % i)) for i in [670, 671] ]
(f0, f1) = [
ComputedDenseStereoFrame(Image.open("f%d-left.png" % i),
Image.open("f%d-right.png" % i)) for i in [0, 1]
]
d = "/u/jamesb/ros/ros-pkg/vision/vslam/trial"
#(f0,f1) = [ ComputedDenseStereoFrame(Image.open("%s/%06dL.png" % (d,i)), Image.open("%s/%06dR.png" % (d,i))) for i in [276, 278] ]
#(f0,f1) = [ ComputedDenseStereoFrame(Image.open("../vslam/trial/%06dL.png" % i), Image.open("../vslam/trial/%06dR.png" % i)) for i in [277, 278] ]
#(f0,f1) = [ ComputedDenseStereoFrame(Image.open("../vslam/trial/%06dL.png" % i), Image.open("../vslam/trial/%06dR.png" % i)) for i in [0, 1] ]
chipsize = (640, 480)
factor = 640 / chipsize[0]
vo = VisualOdometer(stereo_cam)
vo.process_frame(f0)
vo.process_frame(f1)
pairs = vo.temporal_match(f0, f1)
pairs = [(b, a) for (a, b) in pairs]
def xform(M, x, y, z):
nx = vop.mad(M[0], x, vop.mad(M[1], y, vop.mad(M[2], z, M[3])))
ny = vop.mad(M[4], x, vop.mad(M[5], y, vop.mad(M[6], z, M[7])))
nz = vop.mad(M[8], x, vop.mad(M[9], y, vop.mad(M[10], z, M[11])))
return (nx, ny, nz)
def img_err(f0, f1, RT):
# limits of file images
start, end = 941, 1000
start, end = 0, 27000
f1start, f1end = 0, 3000
f2start, f2end = 20000, 23000
for topic, msg, t in rosrecord.logplayer(filename):
if rospy.is_shutdown():
break
if topic == "/dcam/raw_stereo":
if not cam:
cam = camera.StereoCamera(msg.right_info)
vo = VisualOdometer(cam, scavenge = False, feature_detector = FeatureDetectorFast(), \
inlier_error_threshold = 3.0, sba = None, \
inlier_thresh = 100, \
position_keypoint_thresh = 0.2, angle_keypoint_thresh = 0.15)
vo_x = []
vo_y = []
vo_u = []
vo_v = []
trajectory = []
frames1 = []
frames2 = []
if framecounter == end:
break
has_moved = False
# set high for getting different views
angle_thresh = 0.15
dist_thresh = 0.2
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 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)),
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 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),
f.seek(skipto)
#print f.tell(), msg
if rospy.is_shutdown():
break
if topic.endswith("stereo/raw_stereo") or topic.endswith("dcam/raw_stereo"):
if not cam:
cam = camera.StereoCamera(msg.right_info)
vos = [
VisualOdometer(cam,
# scavenge = True,
scavenge = False,
feature_detector = FeatureDetectorFast(),
# feature_detector = FeatureDetectorHarris(),
inlier_error_threshold = 3.0,
# sba = (1,300,10),
# sba = (1,1,10),
# sba = (300,10,10),
sba = None,
inlier_thresh = 100,
ransac_iters = 100,
position_keypoint_thresh = -0.1, angle_keypoint_thresh = 0.15)
]
vo_x = [ [] for i in vos]
vo_y = [ [] for i in vos]
vo_u = [ [] for i in vos]
vo_v = [ [] for i in vos]
trajectory = [ [] for i in vos]
stampedTrajectory = [ [] for i in vos]
# jdc turning off skeletoning
skel = Skeleton(cam)
first_pair = None
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()),
VisualOdometer(cam,
feature_detector=FeatureDetectorFast(),
descriptor_scheme=DescriptorSchemeSAD(),
scavenge=True),
#VisualOdometer(cam, feature_detector = FeatureDetectorHarris(), descriptor_scheme = DescriptorSchemeSAD()),
#VisualOdometer(cam, feature_detector = FeatureDetectorFast(), descriptor_scheme = DescriptorSchemeSAD(), inlier_error_threshold=1.0),
#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]
trajectory = [[] for i in vos]
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]
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.5),
#VisualOdometer(stereo_cam, feature_detector = FeatureDetectorHarris(), sba=(1,20,10), inlier_error_threshold = 1.5),
#VisualOdometer(stereo_cam, feature_detector = FeatureDetectorHarris(), sba=(1,49,50), 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)),
#VisualOdometer(stereo_cam, scavenge = True, sba = (1,1,5)),
]
