def move_forward(i, prev):
""" Forward 1 meter, turn around, Back 1 meter """
if i == 0:
return Pose(rotation(0,0,1,0), (0,0,0))
elif i < 10:
return prev * Pose(rotation(0,0,1,0), (0,0,.1))
elif i < 40:
return prev * Pose(rotation(math.pi / 30, 0, 1, 0), (0, 0, 0))
elif i < 50:
return prev * Pose(rotation(0,0,1,0), (0,0,.1))
def frompose(self, ipose, cam, color):
factor = 1e-1
xf = Pose(numpy.array([[1, 0, 0], [0, 0, 1], [0, -1, 0]]), [0, 0, 0])
pose = xf * ipose
#print ipose.xform(0,0,0), pose.xform(0,0,0)
for xi, xoff in [(0, 0), (100, 0.088)]:
x, y, z = pose.xform(xoff, 0, 0)
x /= factor
y /= factor
z /= factor
self.update(xi, x, y, z, .4, color)
x, y, z = pose.xform(xoff, 0, factor)
x /= factor
y /= factor
z /= factor
self.update(xi + 1, x, y, z, 0.1, color)
def mod2pos(x, y, z):
x, y, z = pose.xform(x + xoff, y, z)
x /= factor
y /= factor
z /= factor
return Position(x, y, z)
for i, (x, y) in enumerate([(0, 0), (640, 0), (0, 480),
(640, 480)]):
model = [(0, 0, 0), cam.pix2cam(x, y, 640)]
self.linestrip(xi + 2 + i, [mod2pos(*p) for p in model], color)
def getpose(transformer, parent_frame, frame, ttime):
ptf = transformer.getTransform(parent_frame, frame, ttime)
p = Pose(
transformations.rotation_matrix_from_quaternion(
(ptf.qx, ptf.qy, ptf.qz, ptf.qw))[:3, :3],
numpy.array((ptf.x, ptf.y, ptf.z)))
return p
def transf_fit_angle(transf, curve0, curve1):
# no translation
p = [0., 0., 0.]
# euler angles
e = transf[0:3]
pose = Pose(
transformations.rotation_matrix_from_euler(e[0], e[1], e[2],
'sxyz')[:3, :3], p)
curve01 = transf_curve(curve0, pose)
return ((curve01 - curve1)**2).sum()
def transf_fit_space(transf, curve0, curve1):
# translation vector
p = transf[0:3]
# euler angles
e = transf[3:6]
pose = Pose(
transformations.rotation_matrix_from_euler(e[0], e[1], e[2],
'sxyz')[:3, :3], p)
curve01 = transf_curve(curve0, pose)
return ((curve01 - curve1)**2).sum()
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 error_func(transf, curve0, curve1):
# translation vector
p = transf[0:3]
# euler angles
e = transf[3:6]
pose = Pose(
transformations.rotation_matrix_from_euler(e[0], e[1], e[2],
'sxyz')[:3, :3], p)
# pose = Pose(identity(3), p)
curve01 = transf_curve(curve0, pose)
return curve01 - curve1
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 transf_fit_angle_scale_time(transf, curve0, curve1):
# no translation
p = [0., 0., 0.]
# euler angles
e = transf[0:3]
# scale
s = transf[3]
# timestamp offset
t = transf[4]
pose = Pose(
transformations.rotation_matrix_from_euler(e[0], e[1], e[2],
'sxyz')[:3, :3], p)
curve01 = transf_curve(curve0, pose)
curve01 = scale_curve(curve01, s)
curve01 = curve01 + array([t, 0.0, 0.0, 0.0])
return ((curve01 - curve1)**2).sum()
def test_transf_fit0():
curve0 = random.random((100, 4))
p = [3., 4., 5.]
e = [0.1, 0.1, -0.2]
t = 0.001
pose = Pose(
transformations.rotation_matrix_from_euler(e[0], e[1], e[2],
'sxyz')[:3, :3], p)
print pose.M
# pose = Pose(identity(3), p)
curve1 = transf_curve(curve0, pose)
curve1 = scale_curve(curve1, 1.15)
curve1 = curve1 + array([t, 0., 0., 0.])
print 'diff of time', ((curve1[:, 0] - curve0[:, 0])**2).sum()
# fitting
transf_fit = transf_fit_space_scale_time
transf0 = [3.1, 4.1, 5.1, 0., 0., 0., 1.0, 0.00]
transf = fmin(transf_fit,
transf0,
args=(curve0, curve1),
maxiter=10000,
maxfun=10000)
print 'fmin, transf', transf
transf0 = [3.1, 4.1, 5.1, 0., 0., 0., 1.0, 0.00]
transf = fmin_powell(transf_fit, transf0, args=(curve0, curve1))
print 'fmin_powell, transf', transf
transf0 = [3.1, 4.1, 5.1, 0., 0., 0., 1.0, 0.00]
transf = fmin_cg(transf_fit, transf0, fprime=None, args=(curve0, curve1))
print 'fmin_cg, transf', transf
transf0 = [3.1, 4.1, 5.1, 0., 0., 0., 1.0, 0.00]
transf = fmin_bfgs(transf_fit, transf0, fprime=None, args=(curve0, curve1))
print 'fmin_bfgs, transf', transf
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)
def lookup_disparity(self, px, py):
x = self.D[0].getpixel((px, py))
y = self.D[1].getpixel((px, py))
z = self.D[2].getpixel((px, py))
(x, y, d) = self.cam.cam2pix(x, y, z)
assert abs(x - px) < 2
return d
use_magic_disparity = False
trajectory = [[] for i in vos]
for i in range(401):
print i
if 0:
desired_pose = Pose(rotation(0, 0, 1, 0), (0, 0, 0.01 * i))
cam = ray_camera(desired_pose, stereo_cam)
else:
theta = (i / 400.0) * math.pi * 2
r = 4.0
cam = ray_camera(
Pose(rotation(theta, 0, 1, 0),
(r - r * math.cos(theta), 0, r * math.sin(theta))),
stereo_cam)
if 0:
imL = Image.new("RGB", (640, 480))
imR = Image.new("RGB", (640, 480))
imL = imL.convert("L")
imR = imR.convert("L")
imD = [
1 + (1 - cos(angle)) * (x * x - 1),
-z * sin(angle) + (1 - cos(angle)) * x * y,
y * sin(angle) + (1 - cos(angle)) * x * z
],
[
z * sin(angle) + (1 - cos(angle)) * x * y,
1 + (1 - cos(angle)) * (y * y - 1),
-x * sin(angle) + (1 - cos(angle)) * y * z
],
[
-y * sin(angle) + (1 - cos(angle)) * x * z,
x * sin(angle) + (1 - cos(angle)) * y * z,
1 + (1 - cos(angle)) * (z * z - 1)
]])
p0 = Pose(rotation(0.0, 0, 0, 1), [0, 0, 0])
p1 = Pose(rotation(0.1, 0, 0, 1), [0, 0, 0])
print p0.angle(p1)
sys.exit(0)
cam = None
filename = "/u/prdata/videre-bags/loop2-color.bag"
filename = "/u/prdata/videre-bags/greenroom-2008-11-3-color.bag"
filename = "2008-11-04-09-55-12-topic.bag"
filename = "2008-11-04-14-44-56-topic.bag"
filename = "2008-11-05-14-35-11-topic.bag"
filename = "/u/prdata/videre-bags/vo1.bag"
filename = sys.argv[1]
framecounter = 0
oe_x = []
oe_y = []
def resolve(self, transformer):
if self.isVerbosity(1):
print "resolver"
print "getLatestCommonTime", transformer.getLatestCommonTime(
'map', 'stereo_link')
if len(self.observations) > 0:
print "obs earliest ", min(self.observations)[0]
print "obs latest ", max(self.observations)[0]
for (t, (cam, picture, iskey)) in self.observations:
# Add keys to the map if they are well-localized and TF can transform them
print iskey, transformer.canTransform('map', 'stereo_link', t)
if iskey and transformer.canTransform('map', 'stereo_link', t):
loc = self.localizationOK.query(t)
if loc == None:
if len(self.localizationOK.q) > 0:
print "No localizations for", t, "earliest", min(
self.localizationOK.q), "latest", max(
self.localizationOK.q)
if loc != None:
print "Have localization", loc
if loc == (True, True):
if self.isVerbosity(1):
print "PM adding frame"
picture.pose = getpose(transformer, 'map',
'stereo_link', t)
if not (cam in self.cameras):
self.cameras.append(cam)
ci = self.cameras.index(cam)
self.pics.append((ci, picture))
self.vt.add(None, picture.descriptors)
if 1 <= self.verbose:
print "Added image to picture map"
del self.observations[(t, (cam, picture, iskey))]
for (t, (cam, picture, iskey)) in self.observations:
# If localization is failing, look up the frame and return its pose
if not iskey and transformer.canTransform('stereo_link',
'base_link', t):
loc = self.localizationOK.query(t)
if loc != None:
r = None
if (loc[0] == False or loc[1] == False):
kp = picture.keypoints
descriptors = picture.descriptors
best = self.find(descriptors)
match_cam = self.cameras[self.pics[best][0]]
match_pic = self.pics[best][1]
pairs = self.ds.match0(match_pic.keypoints,
match_pic.descriptors, kp,
descriptors)
pairs = [(b, a) for (a, b, d) in pairs]
(inl, R,
S) = self.pe.estimateC(match_cam, match_pic.keypoints,
cam, kp, pairs)
if inl > 100:
r33 = numpy.mat(numpy.array(R).reshape(3, 3))
rel_pose = Pose(r33, numpy.array(S))
cam90 = Pose(
numpy.array([[0, 0, 1], [-1, 0, 0], [0, -1,
0]]),
numpy.array([0, 0, 0]))
rel_pose = cam90 * rel_pose * ~cam90
# Figure out camera's pose in map
m2s = match_pic.pose
cam_in_map = (m2s * rel_pose)
base_in_map = cam_in_map * getpose(
transformer, 'stereo_link', 'base_link', t)
r = (t, base_in_map)
del self.observations[(t, (cam, picture, iskey))]
return r
return None
def ground_truth(p, q): return Pose(transformations.rotation_matrix_from_quaternion([q.x, q.y, q.z, q.w])[:3,:3], [p.x, p.y, p.z])
(x, y, d) = self.cam.cam2pix(x, y, z)
#assert abs(x - px) < 2
return d
evaluate_disparity = False
use_magic_disparity = False
disparities = []
trajectory = [[] for i in vos]
ground_truth = []
for i in range(8):
print i
if 0:
desired_pose = Pose(rotation(0, 0, 1, 0), (0, 0, 0.01 * i))
else:
theta = (i / 400.0) * math.pi * 2
wobble = 0.25 * (sin(theta * 9.127) + cos(theta * 3.947))
r = 4.0 + wobble
desired_pose = Pose(
rotation(theta, 0, 1,
0), (4.0 - r * math.cos(theta), 0, r * math.sin(theta)))
ground_truth.append(desired_pose.xform(0, 0, 0))
cam = ray_camera(desired_pose, stereo_cam)
if 0: # set to 1 for the first time to generate cache
imL = Image.new("RGB", (640, 480))
imR = Image.new("RGB", (640, 480))
imD = [
Image.new("F", (640, 480)),
def workloop(self):
workframe = None
while not rospy.is_shutdown():
while self.frameq.empty() and not rospy.is_shutdown():
time.sleep(0.1)
while not self.frameq.empty() and not rospy.is_shutdown():
afmsg = self.frameq.get()
print "CORR: NEW WORK", "id", afmsg.id
p = Pose()
p.fromlist(afmsg.pose.v)
workframe = LibraryFrame(afmsg.id, p,
[(p.x, p.y, p.d)
for p in afmsg.keypoints],
[d.v for d in afmsg.descriptors])
assert len(workframe.descriptors) == len(workframe.kp)
for d in workframe.descriptors:
assert len(d) == 256
if workframe:
tosearch = list(self.library)
best = 100
best_frame = None
best_diff = None
for essay in tosearch:
prox, diffpose = self.vo.proximity(essay, workframe)
print "at", time.time(
), "essay.id", essay.id, "has prox", prox
if essay.id == workframe.id:
print len(essay.kp), len(workframe.kp)
if prox > best:
best = prox
best_frame = essay
best_diff = diffpose
if best_frame:
print "CORR: sending winner", best_frame.id, "at", time.time(
)
Top = workframe.pose
Tmk = best_frame.pose
Tkp = best_diff
Tmp = Tmk * Tkp
Tmo = Tmp * ~Top
for mat in ["Top", "Tmk", "Tkp", "Tmp", "Tmo", "Tmo*Top"]:
print mat
print eval(mat).M
print
print "corrector has pose Tmp:"
print(Tmo * Top).M
if 1:
a = Tmp.tolist()
b = (Tmo * Top).tolist()
for ai, bi in zip(a, b):
if abs(ai - bi) > 0.001:
print "FAIL"
for mat in ["Top", "Tmp", "Tmo", "Tmo*Top"]:
print mat
print eval(mat).M
assert 0
self.pub_tmo.publish(Pose44(Tmo.tolist()))
if 0:
Tpo = workframe.pose
Tpm = best_diff * best_frame.pose
Tp = ~Tpo
Tom = Top * Tpm
print "Tpo"
print Tpo.M
print "Tpm"
print Tpm.M
print "Computed Tom"
print Tom.M
print "Keyframe in map"
print(Tpo * Tom).M
self.pub_tom.publish(Pose44(Tom.tolist()))
#p = workframe.pose.invert().concatenate(best_frame.pose).concatenate(best_diff)
else:
print "CORR: did not find acceptable winner"
else:
time.sleep(0.1)
xs -= 4.5 * 1e-3
f = -0.06
xp = xs * cos(f) - ys * sin(f)
yp = ys * cos(f) + xs * sin(f)
pylab.plot(xp, yp, c=colors[i], label=vos[i].name())
pylab.quiver(xp, yp, vo_u[i], vo_v[i],
color=colors[i]) #, label = '_nolegend_')
#xk = [ x for j,x in enumerate(vo_x[i]) if j in vos[i].log_keyframes ]
#yk = [ y for j,y in enumerate(vo_y[i]) if j in vos[i].log_keyframes ]
#pylab.scatter(xk, yk, c = colors[i], label = '_nolegend_')
if vos[0].sba:
CX = []
CY = []
for sbap in vos[0].posechain:
p = Pose()
p.fromlist(sbap.M)
x, y, z = p.xform(0, 0, 0)
CX.append(x)
CY.append(z)
xs = numpy.array(CX)
ys = numpy.array(CY)
xs -= 4.5 * 1e-3
f = -0.06
xp = xs * cos(f) - ys * sin(f)
yp = ys * cos(f) + xs * sin(f)
pylab.plot(xp, yp, c='magenta', label='with SBA')
pylab.plot(oe_x, oe_y, c='green', label='wheel + IMU odometry')
xlim = pylab.xlim()
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 workloop(self):
workframe = None
while not rospy.is_shutdown():
while self.frameq.empty() and not rospy.is_shutdown():
time.sleep(0.1)
while not self.frameq.empty() and not rospy.is_shutdown():
afmsg = self.frameq.get()
print "CORR: NEW WORK", "id", afmsg.id
p = Pose()
p.fromlist(afmsg.pose.v)
workframe = LibraryFrame(afmsg.id, p, [ (p.x,p.y,p.d) for p in afmsg.keypoints], [d.v for d in afmsg.descriptors])
assert len(workframe.descriptors) == len(workframe.kp)
for d in workframe.descriptors:
assert len(d) == 256
if workframe:
tosearch = list(self.library)
best = 100
best_frame = None
best_diff = None
for essay in tosearch:
prox,diffpose = self.vo.proximity(essay, workframe)
print "at", time.time(), "essay.id", essay.id, "has prox", prox
if essay.id == workframe.id:
print len(essay.kp), len(workframe.kp)
if prox > best:
best = prox
best_frame = essay
best_diff = diffpose
if best_frame:
print "CORR: sending winner", best_frame.id, "at", time.time()
Top = workframe.pose
Tmk = best_frame.pose
Tkp = best_diff
Tmp = Tmk * Tkp
Tmo = Tmp * ~Top
for mat in [ "Top", "Tmk", "Tkp", "Tmp", "Tmo", "Tmo*Top" ]:
print mat
print eval(mat).M
print
print "corrector has pose Tmp:"
print (Tmo * Top).M
if 1:
a = Tmp.tolist()
b = (Tmo * Top).tolist()
for ai,bi in zip(a,b):
if abs(ai - bi) > 0.001:
print "FAIL"
for mat in [ "Top", "Tmp", "Tmo", "Tmo*Top" ]:
print mat
print eval(mat).M
assert 0
self.pub_tmo.publish(Pose44(Tmo.tolist()))
if 0:
Tpo = workframe.pose
Tpm = best_diff * best_frame.pose
Tp = ~Tpo
Tom = Top * Tpm
print "Tpo"
print Tpo.M
print "Tpm"
print Tpm.M
print "Computed Tom"
print Tom.M
print "Keyframe in map"
print (Tpo * Tom).M
self.pub_tom.publish(Pose44(Tom.tolist()))
#p = workframe.pose.invert().concatenate(best_frame.pose).concatenate(best_diff)
else:
print "CORR: did not find acceptable winner"
else:
time.sleep(0.1)
def move_combo(i):
R = rotation(i * 0.02, 0, 1, 0)
S = (i * -0.01, 0, 0)
return Pose(R, S)
oe_home = oe_pose
local = ~oe_home * oe_pose
(x,y,z) = local.xform(0,0,0) # ground truth x y z
# note that the phase space frame is x forward, y to left and z upward.
oe_x.append(-y)
oe_y.append(x)
# convert the time stamp to seconds in float
# secs = timeStamp.secs + timeStamp.nsecs*1.e-9
secs = msg.header.stamp.secs + msg.header.stamp.nsecs*1.e-9
stampedGroundTruth.append((secs, -y, -z, x))
print "There are", len(vo.tracks), "tracks"
print "There are", len([t for t in vo.tracks if t.alive]), "live tracks"
print "There are", len(set([t.p[-1] for t in vo.tracks if t.alive])), "unique endpoints on live tracks"
quality_pose = Pose()
if 1:
# Attempt to compute best possible end-to-end pose
vo = VisualOdometer(cam, feature_detector = FeatureDetector4x4(FeatureDetectorHarris), scavenge = True)
f0 = SparseStereoFrame(*first_pair)
f1 = SparseStereoFrame(imgL, imgR)
vo.handle_frame(f0)
vo.handle_frame(f1)
quality_pose = vo.pose
if 0:
for t in vos[2].all_tracks:
pylab.plot([x for (x,y,d) in t.p], [y for (x,y,d) in t.p])
pylab.xlim((0, 640))
pylab.ylim((0, 480))
pylab.savefig("foo.png", dpi=200)
strong = mk_covar(0.0001, 0.000002, 0.00002)
def lerp(t, a, b):
return a + (b - a) * t
def link_strength(t):
assert 0 <= t
assert t <= 1
return mk_covar(lerp(t, 0.01, 0.0001), lerp(t, 0.0002, 0.000002),
lerp(t, 0.002, 0.00002))
if 0:
pg_constraint(pg, 0, 1, Pose(numpy.identity(3), (1, 0, 0)),
link_strength(0))
pg_constraint(pg, 1, 2, Pose(numpy.identity(3), (1, 0, 0)),
link_strength(0))
pg_constraint(pg, 0, 2, Pose(numpy.identity(3), (1, 0, 0)),
link_strength(1))
pg.initializeOnlineIterations()
print "pg.error", pg.error()
for i in range(1000):
pg.iterate()
print "pg.error", pg.error()
print 0, newpose(0).xform(0, 0, 0)
print 1, newpose(1).xform(0, 0, 0)
print 2, newpose(2).xform(0, 0, 0)
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'
