def __init__(self, args):
rospy.init_node('roadmap_server')
self.optimization_distance = rospy.get_param('~optimization_distance', 10);
self.tf = TransformListener()
stereo_cam = camera.Camera((389.0, 389.0, 89.23 * 1e-3, 323.42, 323.42, 274.95))
self.skel = Skeleton(stereo_cam)
# self.skel.node_vdist = 1
if False:
self.skel.load(args[1])
self.skel.optimize()
self.startframe = 100000
else:
self.startframe = 0
self.frame_timestamps = {}
self.vo = None
self.pub = rospy.Publisher("roadmap", vslam.msg.Roadmap)
time.sleep(1)
#self.send_map(rospy.time(0))
self.wheel_odom_edges = set()
rospy.Subscriber('/wide_stereo/raw_stereo', stereo_msgs.msg.RawStereo, self.handle_raw_stereo, queue_size=2, buff_size=7000000)
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 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)
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
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 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_pe(self):
random.seed(0)
cloud = [(4 * (random.random() - 0.5), 4 * (random.random() - 0.5),
4 * (random.random() - 0.5)) for i in range(20)]
vo = VisualOdometer(
camera.Camera(
(389.0, 389.0, 89.23 * 1e-3, 323.42, 323.42, 274.95)))
stereo_cam = {}
af = {}
fd = FeatureDetectorStar(300)
ds = DescriptorSchemeSAD()
for i in range(5):
stereo_cam[i] = camera.Camera(
(389.0, 389.0, .080 + .010 * i, 323.42, 323.42, 274.95))
desired_pose = Pose()
cam = ray_camera(desired_pose, stereo_cam[i])
imL = Image.new("RGB", (640, 480))
imR = Image.new("RGB", (640, 480))
scene = []
scene += [
object(isphere(vec3(0, 0, 0), 1000), shadeLitCloud,
{'scale': 0.001})
]
scene += [
object(isphere(vec3(x, y, z + 6), 0.3), shadeLitCloud,
{'scale': 3}) for (x, y, z) in cloud
]
imL, imR = [im.convert("L") for im in [imL, imR]]
render_stereo_scene(imL, imR, None, cam, scene, 0)
af[i] = SparseStereoFrame(imL,
imR,
feature_detector=fd,
descriptor_scheme=ds)
vo.process_frame(af[i])
pe = PoseEstimator()
for i1 in range(5):
for i0 in range(5):
pairs = vo.temporal_match(af[i1], af[i0])
res = pe.estimateC(stereo_cam[i0], af[i0].features(),
stereo_cam[i1], af[i1].features(), pairs,
False)
x, y, z = res[2]
self.assertAlmostEqual(x, 0, 0)
self.assertAlmostEqual(y, 0, 0)
self.assertAlmostEqual(z, 0, 0)
def __init__(self, *camparams):
self.iet = 3.0
self.rnd = random.Random()
self.rnd.seed(7)
self.setNumRansacIterations(100)
if camparams != ():
self.default_camera = camera.Camera(camparams)
else:
self.default_camera = None
self.inl = []
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, 2)
self.assertAlmostEqual(shift[1], 0.0, 2)
self.assertAlmostEqual(shift[2], 0.0, 2)
for (et, at) in zip(rot, expected_rot):
self.assertAlmostEqual(et, at, 2)
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, 2)
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)
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)
import math
import copy
import pickle
from stereo import DenseStereoFrame, SparseStereoFrame
from visualodometer import VisualOdometer, Pose, DescriptorSchemeCalonder, DescriptorSchemeSAD, FeatureDetectorFast, FeatureDetector4x4, FeatureDetectorStar, FeatureDetectorHarris, from_xyz_euler
from skeleton import Skeleton
from reader import reader
from math import *
from stereo_utils import camera
import pylab, numpy
from matplotlib.patches import Ellipse
stereo_cam = camera.Camera((389.0, 389.0, 89.23 * 1e-3, 323.42, 323.42, 274.95))
bad_vertices = set([ 35437, 35455, 37380, 40122, 40126, 40207, 40229 ])
if 1:
skel = Skeleton(stereo_cam)
filename = "/wg/wgdata1/vol1/iros2009/mkplot_snap"
filename = "ABCDEFG"
skel.load(filename)
print "skel.nodes:", len(skel.nodes)
for e in skel.edges:
if e[0] in bad_vertices or e[1] in bad_vertices:
print "bad edge", e
skel.nodes = [ id for id in skel.nodes if not id in bad_vertices ]
print "skel.nodes:", len(skel.nodes)
print set(skel.labelization())
skel.optimize()
from stereo_utils import camera
from stereo_utils.stereo import SparseStereoFrame
from stereo_utils.descriptor_schemes import DescriptorSchemeCalonder, DescriptorSchemeSAD
from stereo_utils.feature_detectors import FeatureDetectorFast, FeatureDetector4x4, FeatureDetectorStar, FeatureDetectorHarris
from visual_odometry.pe import PoseEstimator
Fx = 389.0
Fy = 389.0
Tx = 0.08923
Clx = 323.42
Crx = 323.42
Cy = 274.95
# Camera
cam = camera.Camera((Fx, Fy, Tx, Clx, Crx, Cy))
# Feature Detector
fd = FeatureDetectorFast(300)
# Descriptor Scheme
ds = DescriptorSchemeCalonder()
# f0 is a stereo pair
f0 = SparseStereoFrame(Image.open("f0-left.png"), Image.open("f0-right.png"), feature_detector = fd, descriptor_scheme = ds)
# f1 is also a stereo pair
f1 = SparseStereoFrame(Image.open("f1-left.png"), Image.open("f1-right.png"), feature_detector = fd, descriptor_scheme = ds)
# Create a pose estimator, and set it to do 500 RANSAC iterations
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, 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):
print "render", i
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))
return
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
