def test_sparse_stereo(self):
left = Image.new("L", (640, 480))
circle(left, 320, 200, 4, 255)
for disparity in range(20):
right = Image.new("L", (640, 480))
circle(right, 320 - disparity, 200, 4, 255)
sf = SparseStereoFrame(left, right)
self.assertAlmostEqual(sf.lookup_disparity(320, 200), disparity, 0)
def test_sparse_stereo(self):
left = Image.new("L", (640,480))
circle(left, 320, 200, 4, 255)
for disparity in range(20):
right = Image.new("L", (640,480))
circle(right, 320 - disparity, 200, 4, 255)
sf = SparseStereoFrame(left, right)
self.assertAlmostEqual(sf.lookup_disparity(320,200), disparity, 0)
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 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 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 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_smoke(self):
cam = camera.Camera((389.0, 389.0, 89.23, 323.42, 323.42, 274.95))
vo = VisualOdometer(cam)
vo.reset_timers()
dir = "/u/konolige/vslam/data/indoor1/"
trail = []
prev_scale = None
schedule = [(f + 1000)
for f in (range(0, 100) + range(100, 0, -1) + [0] * 10)]
schedule = range(1507)
schedule = range(30)
for f in schedule:
lf = Image.open("%s/left-%04d.ppm" % (dir, f))
rf = Image.open("%s/right-%04d.ppm" % (dir, f))
lf.load()
rf.load()
af = SparseStereoFrame(lf, rf)
vo.handle_frame(af)
print f, vo.inl
trail.append(numpy.array(vo.pose.M[0:3, 3].T)[0])
def d(a, b):
d = a - b
return sqrt(numpy.dot(d, d.conj()))
print "covered ", sum([d(a, b) for (a, b) in zip(trail, trail[1:])])
print "from start", d(trail[0], trail[-1]), trail[0] - trail[-1]
vo.summarize_timers()
print vo.log_keyframes
def xtest_smoke_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 load_from_bag(filename, selected_frames):
cam = None
framecounter = 0
afs = {}
for topic, msg in rosrecord.logplayer(filename):
if rospy.is_shutdown():
break
if topic == "/videre/cal_params" and not cam:
cam = camera.VidereCamera(msg.data)
if cam and topic == "/videre/images":
print "frame", framecounter
if framecounter in selected_frames:
def imgAdapted(msg_img):
return Image.fromstring("L",
(msg_img.width, msg_img.height),
msg_img.data)
imgR = imgAdapted(msg.images[0])
imgL = imgAdapted(msg.images[1])
afs[framecounter] = SparseStereoFrame(imgL, imgR)
if framecounter == max(selected_frames):
break
framecounter += 1
return (cam, afs)
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 frame(self, imarray):
# No calibration params yet.
if not self.vo:
return
if self.seq > 10000:
sys.exit()
if DEBUG:
print ""
print ""
print "Frame ", self.seq
print ""
print ""
im = imarray.images[1]
im_r = imarray.images[0]
if im.colorspace == "mono8":
im_py = Image.fromstring("L", (im.width, im.height), im.data)
im_r_py = Image.fromstring("L", (im_r.width, im_r.height),
im_r.data)
elif im.colorspace == "rgb24":
use_color = True
im_col_py = Image.fromstring("RGB", (im.width, im.height), im.data)
im_py = im_col_py.convert("L")
im_r_py = Image.fromstring("RGB", (im_r.width, im_r.height),
im_r.data)
im_r_py = im_r_py.convert("L")
else:
print "Unknown colorspace"
return
# Detect faces on the first frame
if not self.current_keyframes:
self.faces = self.p.detectAllFaces(im_py.tostring(), im.width,
im.height, self.cascade_file,
1.0, None, None, True)
if DEBUG:
print "Faces ", self.faces
sparse_pred_list = []
sparse_pred_list_2d = []
old_rect = [0, 0, 0, 0]
ia = SparseStereoFrame(im_py, im_r_py)
ia.matches = []
ia.desc_diffs = []
ia.good_matches = []
# Track each face
iface = -1
for face in self.faces:
iface += 1
(x, y, w, h) = copy.copy(self.faces[iface])
if DEBUG:
print "A face ", (x, y, w, h)
(old_center, old_diff) = self.rect_to_center_diff((x, y, w, h))
if self.face_centers_3d and iface < len(self.face_centers_3d):
censize3d = list(copy.copy(self.face_centers_3d[iface]))
censize3d.append(2.0 *
self.real_face_sizes_3d[iface]) ###ZMULT
self.get_features(ia, self.num_feats, (x, y, w, h), censize3d)
else:
self.get_features(ia, self.num_feats, (x, y, w, h),
(0.0, 0.0, 0.0, 1000000.0))
if not ia.kp2d:
continue
# First frame:
if len(self.current_keyframes) < iface + 1:
(cen, diff) = self.rect_to_center_diff((x, y, w, h))
cen3d = self.cam.pix2cam(cen[0], cen[1], ia.avgd)
cen3d = list(cen3d)
ltf = self.cam.pix2cam(x, y, ia.avgd)
rbf = self.cam.pix2cam(x + w, y + h, ia.avgd)
fs3d = ((rbf[0] - ltf[0]) + (rbf[1] - ltf[1])) / 4.0
# This assumes that we're tracking the face plane center, not the center of the head sphere.
# If you want to track the center of the sphere instead, do: cen3d[2] += fs3d
# Check that the face is a reasonable size. If not, skip this face.
if 2 * fs3d < self.min_real_face_size or 2 * fs3d > self.max_real_face_size or iface > 1: #HACK: ONLY ALLOW ONE FACE
self.faces.pop(iface)
iface -= 1
continue
if DESCRIPTOR == 'CALONDER':
self.vo.collect_descriptors(ia)
elif DESCRIPTOR == 'SAD':
self.vo.collect_descriptors_sad(ia)
else:
pass
self.current_keyframes.append(0)
self.keyframes.append(copy.copy(ia))
self.feats_to_centers.append(
self.make_face_model(cen, diff, ia.kp2d))
self.real_face_sizes_3d.append(copy.deepcopy(fs3d))
self.feats_to_centers_3d.append(
self.make_face_model(cen3d, (fs3d, fs3d, fs3d), ia.kp3d))
self.face_centers_3d.append(copy.deepcopy(cen3d))
self.recent_good_frames.append(copy.copy(ia))
self.recent_good_rects.append(copy.deepcopy([x, y, w, h]))
self.recent_good_centers_3d.append(copy.deepcopy(cen3d))
self.recent_good_motion.append([0.0] * 3) #dx,dy,dimfacesize
self.recent_good_motion_3d.append([0.0] * 3)
self.same_key_rgfs.append(True)
if DEBUG:
print "cen2d", cen
print "cen3d", self.face_centers_3d[iface]
# End first frame
# Later frames
else:
if DESCRIPTOR == 'CALONDER':
self.vo.collect_descriptors(ia)
elif DESCRIPTOR == 'SAD':
self.vo.collect_descriptors_sad(ia)
else:
pass
done_matching = False
bad_frame = False
while not done_matching:
# Try matching to the keyframe
keyframe = self.keyframes[self.current_keyframes[iface]]
temp_match = self.vo.temporal_match(ia,
keyframe,
want_distances=True)
ia.matches = [(m2, m1) for (m1, m2, m3) in temp_match]
ia.desc_diffs = [m3 for (m1, m2, m3) in temp_match]
print "temp matches", temp_match
ia.good_matches = [
s < self.desc_diff_thresh for s in ia.desc_diffs
]
n_good_matches = len([
m for m in ia.desc_diffs if m < self.desc_diff_thresh
])
if DEBUG:
if len(keyframe.kp) < 2:
print "Keyframe has less than 2 kps"
if n_good_matches < len(keyframe.kp) / 2.0:
print "ngoodmatches, len key.kp, len key.kp/2", n_good_matches, len(
keyframe.kp), len(keyframe.kp) / 2.0
# Not enough matches, get a new keyframe
if len(keyframe.kp) < 2 or n_good_matches < len(
keyframe.kp) / 2.0:
if DEBUG:
print "New keyframe"
# Make a new face model, either from a recent good frame, or from the current image
if not self.same_key_rgfs[iface]:
if DEBUG:
print "centers at beginning of new keyframe"
print "cen2d", [
self.faces[iface][0] +
self.faces[iface][2] / 2.0,
self.faces[iface][1] +
self.faces[iface][3] / 2.0
]
print "cen3d", self.face_centers_3d[iface]
matched_z_list = [
kp3d[2] for (kp3d, is_good) in zip(
self.recent_good_frames[iface].kp3d, self.
recent_good_frames[iface].good_matches)
if is_good
]
if len(matched_z_list) == 0:
matched_z_list = [
kp3d[2] for kp3d in
self.recent_good_frames[iface].kp3d
]
avgz_goodmatches = sum(matched_z_list) / len(
matched_z_list)
tokeep = [
math.fabs(
self.recent_good_frames[iface].kp3d[i][2] -
avgz_goodmatches) <
2.0 * self.real_face_sizes_3d[iface]
for i in range(
len(self.recent_good_frames[iface].kp3d))
]
kp3d_for_model = [
kp3d for (kp3d, tk) in zip(
self.recent_good_frames[iface].kp3d,
tokeep) if tk
]
kp_for_model = [
kp for (kp, tk) in zip(
self.recent_good_frames[iface].kp, tokeep)
if tk
]
# If you're not left with enough points, just take all of them and don't worry about the depth constraints.
if len(kp3d_for_model) < 2:
kp3d_for_model = copy.deepcopy(
self.recent_good_frames[iface].kp3d)
kp_for_model = copy.deepcopy(
self.recent_good_frames[iface].kp)
(cen, diff) = self.rect_to_center_diff(
self.recent_good_rects[iface])
self.feats_to_centers[
iface] = self.make_face_model(
cen, diff,
[(kp0, kp1)
for (kp0, kp1, kp2) in kp_for_model])
cen3d = self.recent_good_centers_3d[iface]
self.feats_to_centers_3d[
iface] = self.make_face_model(
cen3d,
[self.real_face_sizes_3d[iface]] * 3,
kp3d_for_model)
self.keyframes[
self.current_keyframes[iface]] = copy.copy(
self.recent_good_frames[iface])
self.keyframes[self.current_keyframes[
iface]].kp = kp_for_model
self.keyframes[
self.current_keyframes[iface]].kp2d = [
(k0, k1) for (k0, k1, k2) in kp_for_model
]
self.keyframes[self.current_keyframes[
iface]].kp3d = kp3d_for_model
self.keyframes[
self.current_keyframes[iface]].matches = [
(i, i) for i in range(len(kp_for_model))
]
self.keyframes[
self.current_keyframes[iface]].good_matches = [
True
] * len(kp_for_model)
self.keyframes[self.current_keyframes[
iface]].desc_diffs = [0] * len(kp_for_model)
if DESCRIPTOR == 'CALONDER':
self.vo.collect_descriptors(self.keyframes[
self.current_keyframes[iface]])
elif DESCRIPTOR == 'SAD':
self.vo.collect_descriptors_sad(self.keyframes[
self.current_keyframes[iface]])
else:
pass
self.face_centers_3d[iface] = copy.deepcopy(cen3d)
# Not changing the face size
self.current_keyframes[
iface] = 0 #### HACK: ONLY ONE KEYFRAME!!!
self.same_key_rgfs[iface] = True
# Don't need to change the recent good frame yet.
if DEBUG:
print "centers at end of new keyframe"
print "cen2d", [
self.faces[iface][0] +
self.faces[iface][2] / 2.0,
self.faces[iface][1] +
self.faces[iface][3] / 2.0
]
print "cen3d", self.face_centers_3d[iface]
else:
# Making a new model off of the current frame but with the predicted new position.
# HACK: The displacement computation assumes that the robot/head is still, fix this.
bad_frame = True
#done_matching = True
if DEBUG:
print "Bad frame ", self.seq, " for face ", iface
(cen, diff) = self.rect_to_center_diff(
self.faces[iface])
if DEBUG:
print "Motion for bad frame ", self.recent_good_motion[
iface], self.recent_good_motion_3d[iface]
new_cen = [
cen[0] + self.recent_good_motion[iface][0],
cen[1] + self.recent_good_motion[iface][1]
]
diff = [
diff[0] + self.recent_good_motion[iface][2],
diff[1] + self.recent_good_motion[iface][2]
]
self.faces[iface] = (new_cen[0] - diff[0],
new_cen[1] - diff[1],
2.0 * diff[0], 2.0 * diff[1])
(x, y, w, h) = copy.deepcopy(self.faces[iface])
pred_cen_3d = [
o + n for (o, n) in zip(
self.face_centers_3d[iface],
self.recent_good_motion_3d[iface])
]
pred_cen_3d.append(
2.0 *
self.real_face_sizes_3d[iface]) #### ZMULT
self.get_features(ia, self.num_feats, (x, y, w, h),
pred_cen_3d)
if not ia.kp2d:
break
if DESCRIPTOR == 'CALONDER':
self.vo.collect_descriptors(ia)
elif DESCRIPTOR == 'SAD':
self.vo.collect_descriptors_sad(ia)
else:
pass
self.keyframes[
self.current_keyframes[iface]] = copy.copy(ia)
self.current_keyframes[iface] = 0
(cen, diff) = self.rect_to_center_diff(
self.faces[iface])
self.feats_to_centers[
iface] = self.make_face_model(
cen, diff, ia.kp2d)
self.feats_to_centers_3d[
iface] = self.make_face_model([
pred_cen_3d[0], pred_cen_3d[1],
pred_cen_3d[2]
], [self.real_face_sizes_3d[iface]] * 3,
ia.kp3d)
self.face_centers_3d[iface] = copy.deepcopy(
pred_cen_3d)
self.same_key_rgfs[iface] = True
# Good matches, mark this frame as good
else:
done_matching = True
# END MATCHING
# If we got enough matches for this frame, track.
if ia.kp and ia.kp2d:
# Track
sparse_pred_list = []
sparse_pred_list_2d = []
probs = []
bandwidths = []
size_mult = 0.05 #1.0
for ((match1, match2),
score) in zip(ia.matches, ia.desc_diffs):
if score < self.desc_diff_thresh:
sparse_pred_list.append([
ia.kp3d[match2][i] +
self.feats_to_centers_3d[iface][match1][i]
for i in range(3)
])
sparse_pred_list_2d.append([
ia.kp2d[match2][i] +
self.feats_to_centers[iface][match1][i]
for i in range(2)
])
#probs.append(score)
probs = [1.0] * len(
sparse_pred_list_2d
) # Ignore actual match scores. Uncomment line above to use the match scores.
bandwidths = [size_mult * self.real_face_sizes_3d[iface]
] * len(sparse_pred_list_2d)
(old_center,
old_diff) = self.rect_to_center_diff(self.faces[iface])
if DEBUG:
print "Old center 3d ", self.face_centers_3d[iface]
print "Old center 2d ", old_center
old_rect = self.faces[iface] # For display only
new_center = self.mean_shift_sparse(
self.face_centers_3d[iface][0:3], sparse_pred_list,
probs, bandwidths, 10, 1.0)
new_center_2d = self.cam.cam2pix(new_center[0],
new_center[1],
new_center[2])
# The above line assumes that we're tracking the face plane center, not the center of the head sphere.
# If you want to track the center of the sphere instead, subtract self.real_face_sizes[iface] from the z-coord.
ltf = self.cam.cam2pix(
new_center[0] - self.real_face_sizes_3d[iface],
new_center[1] - self.real_face_sizes_3d[iface],
new_center[2])
rbf = self.cam.cam2pix(
new_center[0] + self.real_face_sizes_3d[iface],
new_center[1] + self.real_face_sizes_3d[iface],
new_center[2])
w = rbf[0] - ltf[0]
h = rbf[1] - ltf[1]
if DEBUG:
print "new center 3d ", new_center
print "new_center 2d ", new_center_2d
(nx, ny, nw, nh) = (new_center_2d[0] - (w - 1) / 2.0,
new_center_2d[1] - (h - 1) / 2.0, w, h)
# Force the window back into the image.
nx += max(0, 0 - nx) + min(0, im.width - nx + nw)
ny += max(0, 0 - ny) + min(0, im.height - ny + nh)
self.faces[iface] = [nx, ny, nw, nh]
self.recent_good_rects[iface] = [nx, ny, nw, nh]
self.recent_good_centers_3d[iface] = copy.deepcopy(
new_center)
if bad_frame:
self.recent_good_motion[
iface] = self.recent_good_motion[iface]
self.recent_good_motion_3d[
iface] = self.recent_good_motion_3d[iface]
else:
self.recent_good_motion[iface] = [
new_center_2d[0] - old_center[0],
new_center_2d[1] - old_center[1],
((nw - 1.0) / 2.0) - old_diff[0]
]
self.recent_good_motion_3d[iface] = [
new_center[i] - self.face_centers_3d[iface][i]
for i in range(len(new_center))
]
self.face_centers_3d[iface] = copy.deepcopy(new_center)
self.recent_good_frames[iface] = copy.copy(ia)
self.same_key_rgfs[iface] = False
if DEBUG:
print "motion ", self.recent_good_motion[
iface], self.recent_good_motion_3d[iface]
print "face 2d ", self.faces[iface]
print "face center 3d ", self.face_centers_3d[iface]
# Output the location of this face center in the 3D camera frame (of the left camera), and rotate
# the coordinates to match the robot's idea of the 3D camera frame.
center_uvd = (nx + (nw - 1) / 2.0, ny + (nh - 1) / 2.0,
(numpy.average(ia.kp, 0))[2])
center_camXYZ = self.cam.pix2cam(center_uvd[0],
center_uvd[1],
center_uvd[2])
center_robXYZ = (center_camXYZ[2], -center_camXYZ[0],
-center_camXYZ[1])
########### PUBLISH the face center for the head controller to track. ########
if not self.usebag:
#stamped_point = PointStamped()
#(stamped_point.point.x, stamped_point.point.y, stamped_point.point.z) = center_robXYZ
#stamped_point.header.frame_id = "stereo"
#stamped_point.header.stamp = imarray.header.stamp
#self.pub.publish(stamped_point)
pm = PositionMeasurement()
pm.header.stamp = imarray.header.stamp
pm.name = "stereo_face_feature_tracker"
pm.object_id = -1
(pm.pos.x, pm.pos.y, pm.pos.z) = center_robXYZ
pm.header.frame_id = "stereo_link"
pm.reliability = 0.5
pm.initialization = 0
#pm.covariance
self.pub.publish(pm)
# End later frames
############ DRAWING ################
if SAVE_PICS:
if not self.keyframes or len(self.keyframes) <= iface:
bigim_py = im_py
draw = ImageDraw.Draw(bigim_py)
else:
key_im = self.keyframes[self.current_keyframes[iface]]
keyim_py = Image.fromstring("L", key_im.size,
key_im.rawdata)
bigim_py = Image.new(
"RGB", (im_py.size[0] + key_im.size[0], im_py.size[1]))
bigim_py.paste(keyim_py.convert("RGB"), (0, 0))
bigim_py.paste(im_py, (key_im.size[0] + 1, 0))
draw = ImageDraw.Draw(bigim_py)
(x, y, w, h) = self.faces[iface]
draw.rectangle((x, y, x + w, y + h), outline=(0, 255, 0))
draw.rectangle(
(x + key_im.size[0], y, x + w + key_im.size[0], y + h),
outline=(0, 255, 0))
(x, y, w, h) = old_rect
draw.rectangle((x, y, x + w, y + h),
outline=(255, 255, 255))
draw.rectangle(
(x + key_im.size[0], y, x + w + key_im.size[0], y + h),
outline=(255, 255, 255))
mstart = old_center
mend = (old_center[0] + self.recent_good_motion[iface][0],
old_center[1] + self.recent_good_motion[iface][1])
draw.rectangle((mstart[0] - 1, mstart[1] - 1,
mstart[0] + 1, mstart[1] + 1),
outline=(255, 255, 255))
draw.rectangle(
(mend[0] - 1, mend[1] - 1, mend[0] + 1, mend[1] + 1),
outline=(0, 255, 0))
draw.line(mstart + mend, fill=(255, 255, 255))
for (x, y) in key_im.kp2d:
draw_x(draw, (x, y), (1, 1), (255, 0, 0))
for (x, y) in ia.kp2d:
draw_x(draw, (x + key_im.size[0], y), (1, 1),
(255, 0, 0))
if self.seq > 0:
if ia.matches:
for ((m1, m2),
score) in zip(ia.matches, ia.desc_diffs):
if score > self.desc_diff_thresh:
color = (255, 0, 0)
else:
color = (0, 255, 0)
draw.line(
(key_im.kp2d[m1][0], key_im.kp2d[m1][1],
ia.kp2d[m2][0] + key_im.size[0],
ia.kp2d[m2][1]),
fill=color)
for (i, (u, v)) in enumerate(sparse_pred_list_2d):
bscale = min(1, probs[i] / 0.01)
draw_x(draw, (u, v), (1, 1),
(128.0 + 128.0 * bscale,
128.0 + 128.0 * bscale,
(1.0 - bscale) * 255.0))
draw_x(draw, (u + key_im.size[0], v), (1, 1),
(128.0 + 128.0 * bscale,
128.0 + 128.0 * bscale,
(1.0 - bscale) * 255.0))
####### PUBLISH 3d visualization point cloud ###################
if self.usebag and self.visualize:
cloud = PointCloud()
cloud.header.frame_id = "stereo"
cloud.header.stamp = imarray.header.stamp
cloud.pts = []
cloud.pts.append(Point())
(cloud.pts[0].x, cloud.pts[0].y,
cloud.pts[0].z) = self.face_centers_3d[iface][:3]
for (i, kp3d) in enumerate(ia.kp3d):
cloud.pts.append(Point())
(cloud.pts[i].x, cloud.pts[i].y,
cloud.pts[i].z) = kp3d
lp = len(cloud.pts)
if self.seq > 0:
for (i, (u, v)) in enumerate(sparse_pred_list):
cloud.pts[lp + i].append(Point())
(cloud.pts[lp + i].x, cloud.pts[lp + i].y,
cloud.pts[lp + i].z) = sparse_pred_list[i][:3]
self.pub.publish(cloud)
bigim_py.save("/tmp/tiff/feats%06d_%03d.tiff" %
(self.seq, iface))
#END DRAWING
# END FACE LOOP
self.seq += 1
imL, imR, _ = im.split()
if use_magic_disparity:
imD = [
Image.open("/tmp/out%06d-x.tiff" % fn),
Image.open("/tmp/out%06d-y.tiff" % fn),
Image.open("/tmp/out%06d-z.tiff" % fn)
]
if 0:
imL, imR = imR, imL
imL = imL.transpose(Image.FLIP_LEFT_RIGHT)
imR = imR.transpose(Image.FLIP_LEFT_RIGHT)
for i in range(3):
imD[i] = imD[i].transpose(Image.FLIP_LEFT_RIGHT)
if evaluate_disparity:
f0 = MagicStereoFrame(imgStereo(imL), imgStereo(imR), imD, stereo_cam)
f1 = SparseStereoFrame(imgStereo(imL), imgStereo(imR))
vos[0].find_keypoints(f0)
pairs = [(f0.lookup_disparity(x, y), f1.lookup_disparity(x, y))
for (x, y) in f0.kp2d]
disparities += [(a, b) for (a, b) in pairs if b != None]
else:
for j, vo in enumerate(vos):
if use_magic_disparity:
f = MagicStereoFrame(imgStereo(imL), imgStereo(imR), imD,
stereo_cam)
else:
f = SparseStereoFrame(imgStereo(imL), imgStereo(imR))
#f = PhonyFrame(~desired_pose, stereo_cam)
#vo.lock_handle_frame(f)
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:
pairs = vo.temporal_match(prev_frame, frame)
solution = vo.solve(prev_frame.kp, frame.kp, pairs, True)
(inl, rot, shift) = solution
sos += numpy.array(shift)
print sos
prev_frame = frame
framecounter += 1
def frame(self, imarray):
# No calibration params yet.
if not self.vo:
return
if self.seq > 10000:
sys.exit()
if DEBUG:
print ""
print ""
print "Frame ", self.seq
print ""
print ""
im = imarray.images[1]
im_r = imarray.images[0]
if im.colorspace == "mono8":
im_py = Image.fromstring("L", (im.width, im.height), im.data)
im_r_py = Image.fromstring("L", (im_r.width, im_r.height), im_r.data)
elif im.colorspace == "rgb24":
use_color = True
im_col_py = Image.fromstring("RGB", (im.width, im.height), im.data)
im_py = im_col_py.convert("L")
im_r_py = Image.fromstring("RGB", (im_r.width, im_r.height), im_r.data)
im_r_py = im_r_py.convert("L")
else :
print "Unknown colorspace"
return
# Detect faces on the first frame
if not self.current_keyframes :
self.faces = self.p.detectAllFaces(im_py.tostring(), im.width, im.height, self.cascade_file, 1.0, None, None, True)
if DEBUG:
print "Faces ", self.faces
sparse_pred_list = []
sparse_pred_list_2d = []
old_rect = [0,0,0,0]
ia = SparseStereoFrame(im_py,im_r_py)
ia.matches = []
ia.desc_diffs = []
ia.good_matches = []
# Track each face
iface = -1
for face in self.faces:
iface += 1
(x,y,w,h) = copy.copy(self.faces[iface])
if DEBUG:
print "A face ", (x,y,w,h)
(old_center, old_diff) = self.rect_to_center_diff((x,y,w,h))
if self.face_centers_3d and iface<len(self.face_centers_3d):
censize3d = list(copy.copy(self.face_centers_3d[iface]))
censize3d.append(2.0*self.real_face_sizes_3d[iface]) ###ZMULT
self.get_features(ia, self.num_feats, (x,y,w,h), censize3d)
else:
self.get_features(ia, self.num_feats, (x, y, w, h), (0.0,0.0,0.0,1000000.0))
if not ia.kp2d:
continue
# First frame:
if len(self.current_keyframes) < iface+1:
(cen,diff) = self.rect_to_center_diff((x,y,w,h))
cen3d = self.cam.pix2cam(cen[0],cen[1],ia.avgd)
cen3d = list(cen3d)
ltf = self.cam.pix2cam(x,y,ia.avgd)
rbf = self.cam.pix2cam(x+w,y+h,ia.avgd)
fs3d = ( (rbf[0]-ltf[0]) + (rbf[1]-ltf[1]) )/4.0
# This assumes that we're tracking the face plane center, not the center of the head sphere.
# If you want to track the center of the sphere instead, do: cen3d[2] += fs3d
# Check that the face is a reasonable size. If not, skip this face.
if 2*fs3d < self.min_real_face_size or 2*fs3d > self.max_real_face_size or iface > 1: #HACK: ONLY ALLOW ONE FACE
self.faces.pop(iface)
iface -= 1
continue
if DESCRIPTOR=='CALONDER':
self.vo.collect_descriptors(ia)
elif DESCRIPTOR=='SAD':
self.vo.collect_descriptors_sad(ia)
else:
pass
self.current_keyframes.append(0)
self.keyframes.append(copy.copy(ia))
self.feats_to_centers.append(self.make_face_model( cen, diff, ia.kp2d ))
self.real_face_sizes_3d.append( copy.deepcopy(fs3d) )
self.feats_to_centers_3d.append( self.make_face_model( cen3d, (fs3d,fs3d,fs3d), ia.kp3d) )
self.face_centers_3d.append( copy.deepcopy(cen3d) )
self.recent_good_frames.append(copy.copy(ia))
self.recent_good_rects.append(copy.deepcopy([x,y,w,h]))
self.recent_good_centers_3d.append(copy.deepcopy(cen3d))
self.recent_good_motion.append([0.0]*3) #dx,dy,dimfacesize
self.recent_good_motion_3d.append([0.0]*3)
self.same_key_rgfs.append(True)
if DEBUG:
print "cen2d", cen
print "cen3d", self.face_centers_3d[iface]
# End first frame
# Later frames
else :
if DESCRIPTOR=='CALONDER':
self.vo.collect_descriptors(ia)
elif DESCRIPTOR=='SAD':
self.vo.collect_descriptors_sad(ia)
else:
pass
done_matching = False
bad_frame = False
while not done_matching:
# Try matching to the keyframe
keyframe = self.keyframes[self.current_keyframes[iface]]
temp_match = self.vo.temporal_match(ia,keyframe,want_distances=True)
ia.matches = [(m2,m1) for (m1,m2,m3) in temp_match]
ia.desc_diffs = [m3 for (m1,m2,m3) in temp_match]
print "temp matches", temp_match
ia.good_matches = [s < self.desc_diff_thresh for s in ia.desc_diffs]
n_good_matches = len([m for m in ia.desc_diffs if m < self.desc_diff_thresh])
if DEBUG:
if len(keyframe.kp)<2:
print "Keyframe has less than 2 kps"
if n_good_matches < len(keyframe.kp)/2.0:
print "ngoodmatches, len key.kp, len key.kp/2", n_good_matches, len(keyframe.kp), len(keyframe.kp)/2.0
# Not enough matches, get a new keyframe
if len(keyframe.kp)<2 or n_good_matches < len(keyframe.kp)/2.0 :
if DEBUG:
print "New keyframe"
# Make a new face model, either from a recent good frame, or from the current image
if not self.same_key_rgfs[iface] :
if DEBUG:
print "centers at beginning of new keyframe"
print "cen2d", [self.faces[iface][0]+self.faces[iface][2]/2.0, self.faces[iface][1]+self.faces[iface][3]/2.0]
print "cen3d", self.face_centers_3d[iface]
matched_z_list = [kp3d[2] for (kp3d,is_good) in zip(self.recent_good_frames[iface].kp3d,self.recent_good_frames[iface].good_matches) if is_good]
if len(matched_z_list) == 0:
matched_z_list = [kp3d[2] for kp3d in self.recent_good_frames[iface].kp3d]
avgz_goodmatches = sum(matched_z_list)/ len(matched_z_list)
tokeep = [math.fabs(self.recent_good_frames[iface].kp3d[i][2]-avgz_goodmatches) < 2.0*self.real_face_sizes_3d[iface]
for i in range(len(self.recent_good_frames[iface].kp3d))]
kp3d_for_model = [kp3d for (kp3d,tk) in zip(self.recent_good_frames[iface].kp3d,tokeep) if tk]
kp_for_model = [kp for (kp,tk) in zip(self.recent_good_frames[iface].kp,tokeep) if tk]
# If you're not left with enough points, just take all of them and don't worry about the depth constraints.
if len(kp3d_for_model) < 2:
kp3d_for_model = copy.deepcopy(self.recent_good_frames[iface].kp3d)
kp_for_model = copy.deepcopy(self.recent_good_frames[iface].kp)
(cen, diff) = self.rect_to_center_diff(self.recent_good_rects[iface])
self.feats_to_centers[iface] = self.make_face_model( cen, diff, [(kp0,kp1) for (kp0,kp1,kp2) in kp_for_model])
cen3d = self.recent_good_centers_3d[iface]
self.feats_to_centers_3d[iface] = self.make_face_model( cen3d, [self.real_face_sizes_3d[iface]]*3, kp3d_for_model)
self.keyframes[self.current_keyframes[iface]] = copy.copy(self.recent_good_frames[iface])
self.keyframes[self.current_keyframes[iface]].kp = kp_for_model
self.keyframes[self.current_keyframes[iface]].kp2d = [(k0,k1) for (k0,k1,k2) in kp_for_model]
self.keyframes[self.current_keyframes[iface]].kp3d = kp3d_for_model
self.keyframes[self.current_keyframes[iface]].matches = [(i,i) for i in range(len(kp_for_model))]
self.keyframes[self.current_keyframes[iface]].good_matches = [True]*len(kp_for_model)
self.keyframes[self.current_keyframes[iface]].desc_diffs = [0]*len(kp_for_model)
if DESCRIPTOR=='CALONDER':
self.vo.collect_descriptors(self.keyframes[self.current_keyframes[iface]])
elif DESCRIPTOR=='SAD':
self.vo.collect_descriptors_sad(self.keyframes[self.current_keyframes[iface]])
else:
pass
self.face_centers_3d[iface] = copy.deepcopy(cen3d)
# Not changing the face size
self.current_keyframes[iface] = 0 #### HACK: ONLY ONE KEYFRAME!!!
self.same_key_rgfs[iface] = True
# Don't need to change the recent good frame yet.
if DEBUG:
print "centers at end of new keyframe"
print "cen2d", [self.faces[iface][0]+self.faces[iface][2]/2.0, self.faces[iface][1]+self.faces[iface][3]/2.0]
print "cen3d", self.face_centers_3d[iface]
else :
# Making a new model off of the current frame but with the predicted new position.
# HACK: The displacement computation assumes that the robot/head is still, fix this.
bad_frame = True
#done_matching = True
if DEBUG:
print "Bad frame ", self.seq, " for face ", iface
(cen,diff) = self.rect_to_center_diff(self.faces[iface])
if DEBUG:
print "Motion for bad frame ", self.recent_good_motion[iface], self.recent_good_motion_3d[iface]
new_cen = [cen[0]+self.recent_good_motion[iface][0], cen[1]+self.recent_good_motion[iface][1]]
diff = [diff[0]+self.recent_good_motion[iface][2], diff[1]+self.recent_good_motion[iface][2]]
self.faces[iface] = (new_cen[0]-diff[0], new_cen[1]-diff[1], 2.0*diff[0], 2.0*diff[1])
(x,y,w,h) = copy.deepcopy(self.faces[iface])
pred_cen_3d = [o+n for (o,n) in zip(self.face_centers_3d[iface],self.recent_good_motion_3d[iface])]
pred_cen_3d.append(2.0*self.real_face_sizes_3d[iface]) #### ZMULT
self.get_features(ia, self.num_feats, (x,y,w,h), pred_cen_3d)
if not ia.kp2d:
break
if DESCRIPTOR=='CALONDER':
self.vo.collect_descriptors(ia)
elif DESCRIPTOR=='SAD':
self.vo.collect_descriptors_sad(ia)
else:
pass
self.keyframes[self.current_keyframes[iface]] = copy.copy(ia)
self.current_keyframes[iface] = 0
(cen,diff) = self.rect_to_center_diff(self.faces[iface])
self.feats_to_centers[iface] = self.make_face_model( cen, diff, ia.kp2d )
self.feats_to_centers_3d[iface] = self.make_face_model( [pred_cen_3d[0],pred_cen_3d[1],pred_cen_3d[2]], [self.real_face_sizes_3d[iface]]*3, ia.kp3d)
self.face_centers_3d[iface] = copy.deepcopy(pred_cen_3d)
self.same_key_rgfs[iface] = True
# Good matches, mark this frame as good
else:
done_matching = True
# END MATCHING
# If we got enough matches for this frame, track.
if ia.kp and ia.kp2d:
# Track
sparse_pred_list = []
sparse_pred_list_2d = []
probs = []
bandwidths = []
size_mult = 0.05 #1.0
for ((match1, match2), score) in zip(ia.matches, ia.desc_diffs):
if score < self.desc_diff_thresh:
sparse_pred_list.append( [ia.kp3d[match2][i]+self.feats_to_centers_3d[iface][match1][i] for i in range(3)] )
sparse_pred_list_2d.append( [ia.kp2d[match2][i]+self.feats_to_centers[iface][match1][i] for i in range(2)] )
#probs.append(score)
probs = [1.0] * len(sparse_pred_list_2d) # Ignore actual match scores. Uncomment line above to use the match scores.
bandwidths = [size_mult*self.real_face_sizes_3d[iface]] * len(sparse_pred_list_2d)
(old_center, old_diff) = self.rect_to_center_diff(self.faces[iface])
if DEBUG:
print "Old center 3d ", self.face_centers_3d[iface]
print "Old center 2d ", old_center
old_rect = self.faces[iface] # For display only
new_center = self.mean_shift_sparse( self.face_centers_3d[iface][0:3], sparse_pred_list, probs, bandwidths, 10, 1.0 )
new_center_2d = self.cam.cam2pix(new_center[0], new_center[1], new_center[2])
# The above line assumes that we're tracking the face plane center, not the center of the head sphere.
# If you want to track the center of the sphere instead, subtract self.real_face_sizes[iface] from the z-coord.
ltf = self.cam.cam2pix( new_center[0]-self.real_face_sizes_3d[iface], new_center[1]-self.real_face_sizes_3d[iface], new_center[2])
rbf = self.cam.cam2pix( new_center[0]+self.real_face_sizes_3d[iface], new_center[1]+self.real_face_sizes_3d[iface], new_center[2])
w = rbf[0]-ltf[0]
h = rbf[1]-ltf[1]
if DEBUG:
print "new center 3d ", new_center
print "new_center 2d ", new_center_2d
(nx,ny,nw,nh) = (new_center_2d[0]-(w-1)/2.0, new_center_2d[1]-(h-1)/2.0, w, h)
# Force the window back into the image.
nx += max(0,0-nx) + min(0, im.width - nx+nw)
ny += max(0,0-ny) + min(0, im.height - ny+nh)
self.faces[iface] = [nx, ny, nw, nh]
self.recent_good_rects[iface] = [nx,ny,nw,nh]
self.recent_good_centers_3d[iface] = copy.deepcopy(new_center)
if bad_frame:
self.recent_good_motion[iface] = self.recent_good_motion[iface]
self.recent_good_motion_3d[iface] = self.recent_good_motion_3d[iface]
else:
self.recent_good_motion[iface] = [new_center_2d[0]-old_center[0], new_center_2d[1]-old_center[1], ((nw-1.0)/2.0)-old_diff[0]]
self.recent_good_motion_3d[iface] = [ new_center[i]-self.face_centers_3d[iface][i] for i in range(len(new_center))]
self.face_centers_3d[iface] = copy.deepcopy(new_center)
self.recent_good_frames[iface] = copy.copy(ia)
self.same_key_rgfs[iface] = False
if DEBUG:
print "motion ", self.recent_good_motion[iface], self.recent_good_motion_3d[iface]
print "face 2d ", self.faces[iface]
print "face center 3d ", self.face_centers_3d[iface]
# Output the location of this face center in the 3D camera frame (of the left camera), and rotate
# the coordinates to match the robot's idea of the 3D camera frame.
center_uvd = (nx + (nw-1)/2.0, ny + (nh-1)/2.0, (numpy.average(ia.kp,0))[2] )
center_camXYZ = self.cam.pix2cam(center_uvd[0], center_uvd[1], center_uvd[2])
center_robXYZ = (center_camXYZ[2], -center_camXYZ[0], -center_camXYZ[1])
########### PUBLISH the face center for the head controller to track. ########
if not self.usebag:
#stamped_point = PointStamped()
#(stamped_point.point.x, stamped_point.point.y, stamped_point.point.z) = center_robXYZ
#stamped_point.header.frame_id = "stereo"
#stamped_point.header.stamp = imarray.header.stamp
#self.pub.publish(stamped_point)
pm = PositionMeasurement()
pm.header.stamp = imarray.header.stamp
pm.name = "stereo_face_feature_tracker"
pm.object_id = -1
(pm.pos.x,pm.pos.y, pm.pos.z) = center_robXYZ
pm.header.frame_id = "stereo_link"
pm.reliability = 0.5;
pm.initialization = 0;
#pm.covariance
self.pub.publish(pm)
# End later frames
############ DRAWING ################
if SAVE_PICS:
if not self.keyframes or len(self.keyframes) <= iface :
bigim_py = im_py
draw = ImageDraw.Draw(bigim_py)
else :
key_im = self.keyframes[self.current_keyframes[iface]]
keyim_py = Image.fromstring("L", key_im.size, key_im.rawdata)
bigim_py = Image.new("RGB",(im_py.size[0]+key_im.size[0], im_py.size[1]))
bigim_py.paste(keyim_py.convert("RGB"),(0,0))
bigim_py.paste(im_py,(key_im.size[0]+1,0))
draw = ImageDraw.Draw(bigim_py)
(x,y,w,h) = self.faces[iface]
draw.rectangle((x,y,x+w,y+h),outline=(0,255,0))
draw.rectangle((x+key_im.size[0],y,x+w+key_im.size[0],y+h),outline=(0,255,0))
(x,y,w,h) = old_rect
draw.rectangle((x,y,x+w,y+h),outline=(255,255,255))
draw.rectangle((x+key_im.size[0],y,x+w+key_im.size[0],y+h),outline=(255,255,255))
mstart = old_center
mend = (old_center[0]+self.recent_good_motion[iface][0], old_center[1]+self.recent_good_motion[iface][1])
draw.rectangle((mstart[0]-1,mstart[1]-1,mstart[0]+1,mstart[1]+1), outline=(255,255,255))
draw.rectangle((mend[0]-1,mend[1]-1,mend[0]+1,mend[1]+1), outline=(0,255,0))
draw.line(mstart+mend, fill=(255,255,255))
for (x,y) in key_im.kp2d :
draw_x(draw, (x,y), (1,1), (255,0,0))
for (x,y) in ia.kp2d:
draw_x(draw, (x+key_im.size[0],y), (1,1), (255,0,0))
if self.seq > 0 :
if ia.matches:
for ((m1,m2), score) in zip(ia.matches,ia.desc_diffs) :
if score > self.desc_diff_thresh :
color = (255,0,0)
else :
color = (0,255,0)
draw.line((key_im.kp2d[m1][0], key_im.kp2d[m1][1], ia.kp2d[m2][0]+key_im.size[0], ia.kp2d[m2][1]), fill=color)
for (i, (u,v)) in enumerate(sparse_pred_list_2d) :
bscale = min(1,probs[i]/0.01)
draw_x(draw, (u,v), (1,1), (128.0+128.0*bscale,128.0+128.0*bscale,(1.0-bscale)*255.0))
draw_x(draw, (u+key_im.size[0],v), (1,1),(128.0+128.0*bscale,128.0+128.0*bscale,(1.0-bscale)*255.0))
####### PUBLISH 3d visualization point cloud ###################
if self.usebag and self.visualize:
cloud = PointCloud()
cloud.header.frame_id = "stereo"
cloud.header.stamp = imarray.header.stamp
cloud.pts = []
cloud.pts.append(Point())
(cloud.pts[0].x, cloud.pts[0].y, cloud.pts[0].z) = self.face_centers_3d[iface][:3]
for (i,kp3d) in enumerate(ia.kp3d):
cloud.pts.append(Point())
(cloud.pts[i].x,cloud.pts[i].y,cloud.pts[i].z) = kp3d
lp = len(cloud.pts)
if self.seq > 0:
for (i, (u,v)) in enumerate(sparse_pred_list):
cloud.pts[lp+i].append(Point())
(cloud.pts[lp+i].x,cloud.pts[lp+i].y,cloud.pts[lp+i].z) = sparse_pred_list[i][:3]
self.pub.publish(cloud)
bigim_py.save("/tmp/tiff/feats%06d_%03d.tiff" % (self.seq, iface))
#END DRAWING
# END FACE LOOP
self.seq += 1
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)
pylab.scatter([x for (x, y, d) in f0.kp], [y for (x, y, d) in f0.kp],
label='f0 kp',
c='red')
pylab.scatter([x for (x, y, d) in f1.kp], [y for (x, y, d) in f1.kp],
label='f1 kp',
if prev_wheel_pose and wheel_pose:
angles.append(prev_wheel_pose.angle(wheel_pose))
qangles.append(prev_wheel_pose.qangle(wheel_pose))
if not wheel_pose:
has_moved = True # be conservative until wheel odom starts up
elif not prev_wheel_pose or \
((not flag_15Hz or framecounter%2 == 0) and \
prev_wheel_pose.further_than(wheel_pose, dist_thresh, angle_thresh)):
prev_wheel_pose = wheel_pose
prev_wheel_o = wheel_o
has_moved = True
if has_moved and start <= framecounter:
af = SparseStereoFrame(dcamImage(msg.left_image),
dcamImage(msg.right_image))
# save frames, hope there's enough storage
if framecounter > f1start and framecounter < f1end:
frames1.append(af)
vo.setup_frame(af)
if framecounter > f2start and framecounter < f2end:
frames2.append(af)
vo.setup_frame(af)
for fr in frames1:
vo.check_inliers(af, fr)
print "Inliers: ", vo.inl
# vo.handle_frame(af)
x, y, z = vo.pose.xform(0, 0, 0)
trajectory.append((x, y, z))
vo_x.append(x)
vo_y.append(z)
def xtest_sim(self):
# Test process with one 'ideal' camera, one real-world Videre
camera_param_list = [
# (200.0, 200.0, 3.00, 320.0, 320.0, 240.0),
(389.0, 389.0, 89.23, 323.42, 323.42, 274.95)
]
def move_combo(i):
R = rotation(i * 0.02, 0, 1, 0)
S = (i * -0.01, 0, 0)
return Pose(R, S)
def move_translate(i):
R = rotation(0, 0, 1, 0)
S = (0, 0, 0)
return Pose(R, S)
def move_Yrot(i):
R = rotation(i * 0.02, 0, 1, 0)
S = (i * 0, 0, 0)
return Pose(R, S)
for movement in [move_combo, move_Yrot]:
for cam_params in camera_param_list:
cam = camera.Camera(cam_params)
kps = []
model = [(x * 200, y * 200, z * 200) for x in range(-3, 4)
for y in range(-3, 4) for z in range(-3, 4)]
def rndimg():
b = "".join(random.sample([chr(c) for c in range(256)],
64))
return Image.fromstring("L", (8, 8), b)
def sprite(dst, x, y, src):
try:
dst.paste(src, (int(x) - 4, int(y) - 4))
except:
print "paste failed", x, y
random.seed(0)
palette = [rndimg() for i in model]
expected = []
afs = []
for i in range(100):
P = movement(i)
li = Image.new("L", (640, 480))
ri = Image.new("L", (640, 480))
q = 0
for (mx, my, mz) in model:
pp = None
pt_camera = (numpy.dot(P.M.I,
numpy.array([mx, my, mz, 1]).T))
(cx, cy, cz, cw) = numpy.array(pt_camera).ravel()
if cz > 100:
((xl, yl), (xr, yr)) = cam.cam2pixLR(cx, cy, cz)
if 0 <= xl and xl < 640 and 0 <= yl and yl < 480:
sprite(li, xl, yl, palette[q])
sprite(ri, xr, yr, palette[q])
q += 1
#li.save("sim/left-%04d.png" % i)
#ri.save("sim/right-%04d.png" % i)
expected.append(P)
afs.append(SparseStereoFrame(li, ri))
threshes = [0.0, 0.01, 0.02, 0.03, 0.04, 0.05, 0.06]
threshes = [0.001 * i for i in range(100)]
threshes = range(100, 500, 10)
error = []
for thresh in threshes:
vo = VisualOdometer(cam, inlier_thresh=thresh)
for (e, af) in zip(expected, afs)[::1]:
vo.handle_frame(af)
def frame(self, imarray):
# No calibration params yet.
if not self.vo:
return
if self.seq > 10000:
sys.exit()
if DEBUG:
print ""
print ""
print "Frame ", self.seq
print ""
print ""
im = imarray.images[1]
im_r = imarray.images[0]
if im.colorspace == "mono8":
im_py = Image.fromstring("L", (im.width, im.height), im.data)
im_r_py = Image.fromstring("L", (im_r.width, im_r.height), im_r.data)
elif im.colorspace == "rgb24":
use_color = True
im_col_py = Image.fromstring("RGB", (im.width, im.height), im.data)
im_py = im_col_py.convert("L")
im_r_py = Image.fromstring("RGB", (im_r.width, im_r.height), im_r.data)
im_r_py = im_r_py.convert("L")
else :
print "Unknown colorspace"
return
# Detect faces on the first frame
if not self.current_keyframes :
self.faces = self.p.detectAllFaces(im_py.tostring(), im.width, im.height, self.cascade_file, 1.0, None, None, True)
if DEBUG:
print "Faces ", self.faces
sparse_pred_list = []
sparse_pred_list_2d = []
old_rect = [0,0,0,0]
ia = SparseStereoFrame(im_py,im_r_py)
ia.matches = []
ia.desc_diffs = []
ia.good_matches = []
# Track each face
iface = -1
for face in self.faces:
iface += 1
(x,y,w,h) = copy.copy(self.faces[iface])
if DEBUG:
print "A face ", (x,y,w,h)
(old_center, old_diff) = self.rect_to_center_diff((x,y,w,h))
if self.face_centers_3d and iface<len(self.face_centers_3d):
censize3d = list(copy.copy(self.face_centers_3d[iface]))
censize3d.append(1.0*self.real_face_sizes_3d[iface]) ###ZMULT
self.get_features(ia, self.num_feats, (x,y,w,h), censize3d)
else:
self.get_features(ia, self.num_feats, (x, y, w, h), (0.0,0.0,0.0,1000000.0))
if not ia.kp2d:
continue
# First frame:
if len(self.current_keyframes) < iface+1:
(cen,diff) = self.rect_to_center_diff((x,y,w,h))
cen3d = self.cam.pix2cam(cen[0],cen[1],ia.avgd)
ltf = self.cam.pix2cam(x,y,ia.avgd)
rbf = self.cam.pix2cam(x+w,y+h,ia.avgd)
fs3d = ( (rbf[0]-ltf[0]) + (rbf[1]-ltf[1]) )/4.0
# Check that the face is a reasonable size. If not, skip this face.
if 2*fs3d < self.min_real_face_size or 2*fs3d > self.max_real_face_size or iface > 1: #HACK: ONLY ALLOW ONE FACE
self.faces.pop(iface)
iface -= 1
continue
if DESCRIPTOR=='CALONDER':
self.vo.collect_descriptors(ia)
elif DESCRIPTOR=='SAD':
self.vo.collect_descriptors_sad(ia)
else:
pass
self.current_keyframes.append(0)
self.keyframes.append(copy.copy(ia))
self.feats_to_centers.append(self.make_face_model( cen, diff, ia.kp2d ))
self.real_face_sizes_3d.append( copy.deepcopy(fs3d) )
self.feats_to_centers_3d.append( self.make_face_model( cen3d, (fs3d,fs3d,fs3d), ia.kp3d) )
self.face_centers_3d.append( copy.deepcopy(cen3d) )
self.recent_good_frames.append(copy.copy(ia))
self.recent_good_rects.append(copy.deepcopy([x,y,w,h]))
self.recent_good_motion.append([0.0]*3) #dx,dy,dimfacesize
self.recent_good_motion_3d.append([0.0]*3)
self.same_key_rgfs.append(True)
# End first frame
# Later frames
else :
if DESCRIPTOR=='CALONDER':
self.vo.collect_descriptors(ia)
elif DESCRIPTOR=='SAD':
self.vo.collect_descriptors_sad(ia)
else:
pass
done_matching = False
bad_frame = False
while not done_matching:
# Try matching to the keyframe
keyframe = self.keyframes[self.current_keyframes[iface]]
temp_match = self.vo.temporal_match(ia,keyframe,want_distances=True)
ia.matches = [(m2,m1) for (m1,m2,m3) in temp_match]
ia.desc_diffs = [m3 for (m1,m2,m3) in temp_match]
print "Scores", ia.desc_diffs
#ia.matches = self.vo.temporal_match(keyframe,ia,want_distances=True)
#ia.desc_diffs = [(VO.sad(keyframe.descriptors[a], ia.descriptors[b])) for (a,b) in ia.matches]
ia.good_matches = [s < self.desc_diff_thresh for s in ia.desc_diffs]
n_good_matches = len([m for m in ia.desc_diffs if m < self.desc_diff_thresh])
# Not enough matches, get a new keyframe
if len(keyframe.kp)<2 or n_good_matches < len(keyframe.kp)/2.0 :
if DEBUG:
print "New keyframe"
# Make a new face model, either from a recent good frame, or from the current image
if not self.same_key_rgfs[iface] :
matched_z_list = [tz for ((tx,ty,tz),is_good) in zip(self.recent_good_frames[iface].kp,self.recent_good_frames[iface].good_matches) if is_good]
if len(matched_z_list) == 0:
matched_z_list = [tz for (tx,ty,tz) in self.recent_good_frames[iface].kp]
avgd_goodmatches = sum(matched_z_list)/ len(matched_z_list)
avg3d_goodmatches = self.cam.pix2cam(0.0,0.0,avgd_goodmatches)
kp3d = [self.cam.pix2cam(kp[0],kp[1],kp[2]) for kp in self.recent_good_frames[iface].kp]
print "kp ", self.recent_good_frames[iface].kp
print "kp3d ",kp3d
print avg3d_goodmatches
kp3d_for_model = [this_kp3d for this_kp3d in kp3d if math.fabs(this_kp3d[2]-avg3d_goodmatches[2]) < 2.0*self.real_face_sizes_3d[iface] ]
kp_for_model = [this_kp
for (this_kp, this_kp3d) in zip(self.recent_good_frames[iface].kp, kp3d)
if math.fabs(this_kp3d[2]-avg3d_goodmatches[2]) < 2.0*self.real_face_sizes_3d[iface] ]
# If you're not left with enough points, just take all of them and don't worry about the depth constraints.
if len(kp3d_for_model) < 2:
kp3d_for_model = kp3d
kp_for_model = copy.deepcopy(self.recent_good_frames[iface].kp)
(cen, diff) = self.rect_to_center_diff(self.recent_good_rects[iface])
self.feats_to_centers[iface] = self.make_face_model( cen, diff, [(kp0,kp1) for (kp0,kp1,kp2) in kp_for_model])
avgd = sum([kp2 for (kp0,kp1,kp2) in kp_for_model])/len(kp_for_model)
cen3d = self.cam.pix2cam(cen[0],cen[1],avgd)
self.feats_to_centers_3d[iface] = self.make_face_model( cen3d, [self.real_face_sizes_3d[iface]]*3, kp3d_for_model)
self.keyframes[self.current_keyframes[iface]] = copy.copy(self.recent_good_frames[iface])
self.keyframes[self.current_keyframes[iface]].kp = kp_for_model
self.keyframes[self.current_keyframes[iface]].kp2d = [(k0,k1) for (k0,k1,k2) in kp_for_model]
self.keyframes[self.current_keyframes[iface]].kp3d = kp3d_for_model
self.keyframes[self.current_keyframes[iface]].matches = [(i,i) for i in range(len(kp_for_model))]
self.keyframes[self.current_keyframes[iface]].good_matches = [True]*len(kp_for_model)
self.keyframes[self.current_keyframes[iface]].desc_diffs = [0]*len(kp_for_model)
self.face_centers_3d[iface] = copy.deepcopy(cen3d)
# Not changing the face size
self.current_keyframes[iface] = 0 #### HACK: ONLY ONE KEYFRAME!!!
self.same_key_rgfs[iface] = True
# Don't need to change the recent good frame yet.
else :
# Making a new model off of the current frame but with the predicted new position.
# HACK: The displacement computation assumes that the robot/head is still, fix this.
bad_frame = True
#done_matching = True
if DEBUG:
print "Bad frame ", self.seq, " for face ", iface
(cen,diff) = self.rect_to_center_diff(self.faces[iface])
if DEBUG:
print "Motion for bad frame ", self.recent_good_motion[iface], self.recent_good_motion_3d[iface]
new_cen = [cen[0]+self.recent_good_motion[iface][0], cen[1]+self.recent_good_motion[iface][1]]
diff = [diff[0]+self.recent_good_motion[iface][2], diff[1]+self.recent_good_motion[iface][2]]
self.faces[iface] = (new_cen[0]-diff[0], new_cen[1]-diff[1], 2.0*diff[0], 2.0*diff[1])
(x,y,w,h) = copy.deepcopy(self.faces[iface])
pred_cen_3d = [o+n for (o,n) in zip(self.face_centers_3d[iface],self.recent_good_motion_3d[iface])]
pred_cen_3d.append(1.0*self.real_face_sizes_3d[iface]) #### ZMULT
self.get_features(ia, self.num_feats, (x,y,w,h), pred_cen_3d)
if not ia.kp2d:
break
if DESCRIPTOR=='CALONDER':
self.vo.collect_descriptors(ia)
elif DESCRIPTOR=='SAD':
self.vo.collect_descriptors_sad(ia)
else:
pass
self.keyframes[self.current_keyframes[iface]] = copy.copy(ia)
self.current_keyframes[iface] = 0
(cen,diff) = self.rect_to_center_diff(self.faces[iface])
self.feats_to_centers[iface] = self.make_face_model( cen, diff, ia.kp2d )
cen3d = self.cam.pix2cam(cen[0],cen[1],ia.avgd)
self.feats_to_centers_3d[iface] = self.make_face_model( cen3d, [self.real_face_sizes_3d[iface]]*3, ia.kp3d)
self.face_centers_3d[iface] = copy.deepcopy(cen3d)
self.same_key_rgfs[iface] = True
# Good matches, mark this frame as good
else:
done_matching = True
# END MATCHING
# If we got enough matches for this frame, track.
if ia.kp and ia.kp2d:
# Track
sparse_pred_list = []
sparse_pred_list_2d = []
probs = []
bandwidths = []
size_mult = 1.0
for ((match1, match2), score) in zip(ia.matches, ia.desc_diffs):
if score < self.desc_diff_thresh:
kp3d = self.cam.pix2cam(ia.kp[match2][0],ia.kp[match2][1],ia.kp[match2][2])
sparse_pred_list.append( (kp3d[0]+self.feats_to_centers_3d[iface][match1][0],
kp3d[1]+self.feats_to_centers_3d[iface][match1][1],
kp3d[2]+self.feats_to_centers_3d[iface][match1][2]) )
sparse_pred_list_2d.append( (ia.kp2d[match2][0]+self.feats_to_centers[iface][match1][0], ia.kp2d[match2][1]+self.feats_to_centers[iface][match1][1]) )
probs = [1.0] * len(sparse_pred_list_2d)
bandwidths = [size_mult*self.real_face_sizes_3d[iface]] * len(sparse_pred_list_2d)
if DEBUG:
print "Old center 3d ", self.face_centers_3d[iface]
print "Old center 2d ",(x+(w-1)/2.0, y+(h-1)/2.0)
old_rect = self.faces[iface]
(old_center, old_diff) = self.rect_to_center_diff(old_rect)
new_center = self.mean_shift_sparse( self.face_centers_3d[iface], sparse_pred_list, probs, bandwidths, 10, 5.0 )
new_center_2d = self.cam.cam2pix(new_center[0], new_center[1], new_center[2])
ltf = self.cam.cam2pix( new_center[0]-self.real_face_sizes_3d[iface], new_center[1]-self.real_face_sizes_3d[iface], new_center[2])
rbf = self.cam.cam2pix( new_center[0]+self.real_face_sizes_3d[iface], new_center[1]+self.real_face_sizes_3d[iface], new_center[2])
w = rbf[0]-ltf[0]
h = rbf[1]-ltf[1]
if DEBUG:
print "new center 3d ", new_center
print "new_center 2d ", new_center_2d
(nx,ny,nw,nh) = (new_center_2d[0]-(w-1)/2.0, new_center_2d[1]-(h-1)/2.0, w, h)
# Force the window back into the image.
dx = max(0,0-nx) + min(0, im.width - nx+nw)
dy = max(0,0-ny) + min(0, im.height - ny+nh)
nx += dx
ny += dy
self.faces[iface] = [nx, ny, nw, nh]
self.recent_good_rects[iface] = [nx,ny,nw,nh]
if bad_frame:
self.recent_good_motion[iface] = self.recent_good_motion[iface]
self.recent_good_motion_3d[iface] = self.recent_good_motion_3d[iface]
else:
self.recent_good_motion[iface] = [new_center_2d[0]-old_center[0], new_center_2d[1]-old_center[1], ((nw-1.0)/2.0)-old_diff[0]]
self.recent_good_motion_3d[iface] = [ new_center[i]-self.face_centers_3d[iface][i] for i in range(len(new_center))]
self.face_centers_3d[iface] = copy.deepcopy(new_center)
self.recent_good_frames[iface] = copy.copy(ia)
self.same_key_rgfs[iface] = False
if DEBUG:
print "motion ", self.recent_good_motion[iface]
print "face 2d ", self.faces[iface]
print "face center 3d ", self.face_centers_3d[iface]
# Output the location of this face center in the 3D camera frame (of the left camera), and rotate
# the coordinates to match the robot's idea of the 3D camera frame.
center_uvd = (nx + (nw-1)/2.0, ny + (nh-1)/2.0, (numpy.average(ia.kp,0))[2] )
center_camXYZ = self.cam.pix2cam(center_uvd[0], center_uvd[1], center_uvd[2])
center_robXYZ = (center_camXYZ[2], -center_camXYZ[0], -center_camXYZ[1])
########### PUBLISH the face center for the head controller to track. ########
if not self.usebag:
stamped_point = PointStamped()
(stamped_point.point.x, stamped_point.point.y, stamped_point.point.z) = center_robXYZ
stamped_point.header.frame_id = "stereo"
stamped_point.header.stamp = imarray.header.stamp
self.pub.publish(stamped_point)
# End later frames
############ DRAWING ################
if SAVE_PICS:
if not self.keyframes or len(self.keyframes) <= iface :
bigim_py = im_py
draw = ImageDraw.Draw(bigim_py)
else :
key_im = self.keyframes[self.current_keyframes[iface]]
keyim_py = Image.fromstring("L", key_im.size, key_im.rawdata)
bigim_py = Image.new("RGB",(im_py.size[0]+key_im.size[0], im_py.size[1]))
bigim_py.paste(keyim_py.convert("RGB"),(0,0))
bigim_py.paste(im_py,(key_im.size[0]+1,0))
draw = ImageDraw.Draw(bigim_py)
(x,y,w,h) = self.faces[iface]
draw.rectangle((x,y,x+w,y+h),outline=(0,255,0))
draw.rectangle((x+key_im.size[0],y,x+w+key_im.size[0],y+h),outline=(0,255,0))
(x,y,w,h) = old_rect
draw.rectangle((x,y,x+w,y+h),outline=(255,255,255))
draw.rectangle((x+key_im.size[0],y,x+w+key_im.size[0],y+h),outline=(255,255,255))
mstart = old_center
mend = (old_center[0]+self.recent_good_motion[iface][0], old_center[1]+self.recent_good_motion[iface][1])
draw.rectangle((mstart[0]-1,mstart[1]-1,mstart[0]+1,mstart[1]+1), outline=(255,255,255))
draw.rectangle((mend[0]-1,mend[1]-1,mend[0]+1,mend[1]+1), outline=(0,255,0))
draw.line(mstart+mend, fill=(255,255,255))
for (x,y) in key_im.kp2d :
draw_x(draw, (x,y), (1,1), (255,0,0))
for (x,y) in ia.kp2d:
draw_x(draw, (x+key_im.size[0],y), (1,1), (255,0,0))
if self.seq > 0 :
for (x,y) in sparse_pred_list_2d :
draw_x(draw, (x,y), (1,1), (0,0,255))
draw_x(draw, (x+key_im.size[0],y), (1,1), (0,0,255))
if ia.matches:
for ((m1,m2), score) in zip(ia.matches,ia.desc_diffs) :
if score > self.desc_diff_thresh :
color = (255,0,0)
else :
color = (0,255,0)
draw.line((key_im.kp2d[m1][0], key_im.kp2d[m1][1], ia.kp2d[m2][0]+key_im.size[0], ia.kp2d[m2][1]), fill=color)
bigim_py.save("/tmp/tiff/feats%06d_%03d.tiff" % (self.seq, iface))
#END DRAWING
# END FACE LOOP
self.seq += 1
def load_from_dir(dir, selected_frames):
afs = dict([(f,
SparseStereoFrame(Image.open("%s/left-%04d.ppm" % (dir, f)),
Image.open("%s/right-%04d.ppm" % (dir, f))))
for f in selected_frames])
return afs
def __init__(self, L, R, D, cam):
SparseStereoFrame.__init__(self, L, R)
self.D = D
self.cam = cam
else:
fn = i
im = Image.open("out%06d.png" % fn)
imL, imR, _ = im.split()
if use_magic_disparity:
imD = [
Image.open("out%06d-x.tiff" % fn),
Image.open("out%06d-y.tiff" % fn),
Image.open("out%06d-z.tiff" % fn)
]
imL, imR = imR, imL
imL = imL.transpose(Image.FLIP_LEFT_RIGHT)
imR = imR.transpose(Image.FLIP_LEFT_RIGHT)
if 1:
for j, vo in enumerate(vos):
f = SparseStereoFrame(imgStereo(imL), imgStereo(imR))
#f = MagicStereoFrame(imgStereo(imL), imgStereo(imR), imD, stereo_cam)
#f = PhonyFrame(~desired_pose, stereo_cam)
#vo.lock_handle_frame(f)
vo.handle_frame(f)
#visualize.viz(vo, f)
(x, y, z) = vo.pose.xform(0, 0, 0)
if 0:
print vo.name(), (x, y, z)
(ex, ey, ez) = desired_pose.xform(0, 0, 0)
Ys[j].append(z - ez)
else:
trajectory[j].append((x, y, z))
from skeleton import Skeleton
if 0:
skel = Skeleton(cam)
skel.node_vdist = 0
skel.adaptive = False
else:
skel = None
gt = []
f = []
for i in range(2100): # range(1,146):
dir = "/u/jamesb/ros/ros-pkg/vision/vslam/trial"
L = Image.open("%s/%06dL.png" % (dir, i))
R = Image.open("%s/%06dR.png" % (dir, i))
nf = SparseStereoFrame(L, R)
vo.setup_frame(nf)
print i, "kp=", len(nf.kp)
nf.id = len(f)
if vt:
vt.add(nf.lf, nf.descriptors)
if skel:
vo.handle_frame_0(nf)
skel.add(vo.keyframe)
vo.correct(skel.correct_frame_pose, nf)
print len(gt), vo.inl
gt.append(vo.pose.xform(0, 0, 0))
else:
pf = open("trial/%06d.pose.pickle" % i, "r")
gt.append(pickle.load(pf))
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
comp = Image.new("RGB", (w * 2, h * 2))
if 0:
for i, vo in enumerate(vos):
af = SparseStereoFrame(imgL, imgR)
vo.handle_frame(af)
if 1:
leftframe = visualize.render_source_with_keypoints(
vo, af)
comp.paste(leftframe, (w * i, 0))
leftframe = visualize.render_text(vo, af)
comp.paste(leftframe, (w * i, h))
else:
if af.id == 0:
master_frame = af
else:
print "id", af.id, "inliers:", vo.inl, "proximity:", vo.proximity(
master_frame, af)
comp = visualize.render_source_with_keypoints_stats(
vo, af)
]
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]
skel = Skeleton(cam)
if skel_load_filename:
skel.load(skel_load_filename)
vos[0].num_frames = max(skel.nodes) + 1
framecounter = max(skel.nodes) + 1
oe_x = []
oe_y = []
oe_home = None
for i,vo in enumerate(vos):
af = SparseStereoFrame(l_image, r_image)
vo.handle_frame(af)
inl_history.append(vo.inl)
inl_history = inl_history[-2:]
af.connected = vo.inl > 7
# Log keyframes into "pool_loop"
if False and not vo.keyframe.id in keys:
k = vo.keyframe
Image.fromstring("L", (640,480), k.lf.tostring()).save("dump/%06dL.png" % len(keys))
Image.fromstring("L", (640,480), k.rf.tostring()).save("dump/%06dR.png" % len(keys))
print "saving frame", "id", k.id, "as key", len(keys), "inliers:", k.inl, "keypoints:", len(k.kp2d), len(k.kp)
#vo.report_frame(k)
keys.add(k.id)
if max(inl_history) > 7:
skel.setlabel(label)
def frame(self, imarray):
# No calibration params yet.
if not self.vo:
return
if self.seq > 10000:
sys.exit()
if DEBUG:
print ""
print ""
print "Frame ", self.seq
print ""
print ""
im = imarray.images[1]
im_r = imarray.images[0]
if im.colorspace == "mono8":
im_py = Image.fromstring("L", (im.width, im.height), im.data)
im_r_py = Image.fromstring("L", (im_r.width, im_r.height),
im_r.data)
elif im.colorspace == "rgb24":
use_color = True
im_col_py = Image.fromstring("RGB", (im.width, im.height), im.data)
im_py = im_col_py.convert("L")
im_r_py = Image.fromstring("RGB", (im_r.width, im_r.height),
im_r.data)
im_r_py = im_r_py.convert("L")
else:
print "Unknown colorspace"
return
# Detect faces on the first frame
if not self.current_keyframes:
self.faces = self.p.detectAllFaces(im_py.tostring(), im.width,
im.height, self.cascade_file,
1.0, None, None, True)
if DEBUG:
print "Faces ", self.faces
sparse_pred_list = []
sparse_pred_list_2d = []
old_rect = [0, 0, 0, 0]
ia = SparseStereoFrame(im_py, im_r_py)
ia.matches = []
ia.desc_diffs = []
ia.good_matches = []
# Track each face
iface = -1
for face in self.faces:
iface += 1
(x, y, w, h) = copy.copy(self.faces[iface])
if DEBUG:
print "A face ", (x, y, w, h)
(old_center, old_diff) = self.rect_to_center_diff((x, y, w, h))
if self.face_centers_3d and iface < len(self.face_centers_3d):
censize3d = list(copy.copy(self.face_centers_3d[iface]))
censize3d.append(1.0 *
self.real_face_sizes_3d[iface]) ###ZMULT
self.get_features(ia, self.num_feats, (x, y, w, h), censize3d)
else:
self.get_features(ia, self.num_feats, (x, y, w, h),
(0.0, 0.0, 0.0, 1000000.0))
if not ia.kp2d:
continue
# First frame:
if len(self.current_keyframes) < iface + 1:
(cen, diff) = self.rect_to_center_diff((x, y, w, h))
cen3d = self.cam.pix2cam(cen[0], cen[1], ia.avgd)
ltf = self.cam.pix2cam(x, y, ia.avgd)
rbf = self.cam.pix2cam(x + w, y + h, ia.avgd)
fs3d = ((rbf[0] - ltf[0]) + (rbf[1] - ltf[1])) / 4.0
# Check that the face is a reasonable size. If not, skip this face.
if 2 * fs3d < self.min_real_face_size or 2 * fs3d > self.max_real_face_size or iface > 1: #HACK: ONLY ALLOW ONE FACE
self.faces.pop(iface)
iface -= 1
continue
if DESCRIPTOR == 'CALONDER':
self.vo.collect_descriptors(ia)
elif DESCRIPTOR == 'SAD':
self.vo.collect_descriptors_sad(ia)
else:
pass
self.current_keyframes.append(0)
self.keyframes.append(copy.copy(ia))
self.feats_to_centers.append(
self.make_face_model(cen, diff, ia.kp2d))
self.real_face_sizes_3d.append(copy.deepcopy(fs3d))
self.feats_to_centers_3d.append(
self.make_face_model(cen3d, (fs3d, fs3d, fs3d), ia.kp3d))
self.face_centers_3d.append(copy.deepcopy(cen3d))
self.recent_good_frames.append(copy.copy(ia))
self.recent_good_rects.append(copy.deepcopy([x, y, w, h]))
self.recent_good_motion.append([0.0] * 3) #dx,dy,dimfacesize
self.recent_good_motion_3d.append([0.0] * 3)
self.same_key_rgfs.append(True)
# End first frame
# Later frames
else:
if DESCRIPTOR == 'CALONDER':
self.vo.collect_descriptors(ia)
elif DESCRIPTOR == 'SAD':
self.vo.collect_descriptors_sad(ia)
else:
pass
done_matching = False
bad_frame = False
while not done_matching:
# Try matching to the keyframe
keyframe = self.keyframes[self.current_keyframes[iface]]
temp_match = self.vo.temporal_match(ia,
keyframe,
want_distances=True)
ia.matches = [(m2, m1) for (m1, m2, m3) in temp_match]
ia.desc_diffs = [m3 for (m1, m2, m3) in temp_match]
print "Scores", ia.desc_diffs
#ia.matches = self.vo.temporal_match(keyframe,ia,want_distances=True)
#ia.desc_diffs = [(VO.sad(keyframe.descriptors[a], ia.descriptors[b])) for (a,b) in ia.matches]
ia.good_matches = [
s < self.desc_diff_thresh for s in ia.desc_diffs
]
n_good_matches = len([
m for m in ia.desc_diffs if m < self.desc_diff_thresh
])
# Not enough matches, get a new keyframe
if len(keyframe.kp) < 2 or n_good_matches < len(
keyframe.kp) / 2.0:
if DEBUG:
print "New keyframe"
# Make a new face model, either from a recent good frame, or from the current image
if not self.same_key_rgfs[iface]:
matched_z_list = [
tz for ((tx, ty, tz), is_good) in zip(
self.recent_good_frames[iface].kp, self.
recent_good_frames[iface].good_matches)
if is_good
]
if len(matched_z_list) == 0:
matched_z_list = [
tz
for (tx, ty, tz
) in self.recent_good_frames[iface].kp
]
avgd_goodmatches = sum(matched_z_list) / len(
matched_z_list)
avg3d_goodmatches = self.cam.pix2cam(
0.0, 0.0, avgd_goodmatches)
kp3d = [
self.cam.pix2cam(kp[0], kp[1], kp[2])
for kp in self.recent_good_frames[iface].kp
]
print "kp ", self.recent_good_frames[iface].kp
print "kp3d ", kp3d
print avg3d_goodmatches
kp3d_for_model = [
this_kp3d for this_kp3d in kp3d
if math.fabs(this_kp3d[2] -
avg3d_goodmatches[2]) < 2.0 *
self.real_face_sizes_3d[iface]
]
kp_for_model = [
this_kp for (this_kp, this_kp3d) in zip(
self.recent_good_frames[iface].kp, kp3d)
if math.fabs(this_kp3d[2] -
avg3d_goodmatches[2]) < 2.0 *
self.real_face_sizes_3d[iface]
]
# If you're not left with enough points, just take all of them and don't worry about the depth constraints.
if len(kp3d_for_model) < 2:
kp3d_for_model = kp3d
kp_for_model = copy.deepcopy(
self.recent_good_frames[iface].kp)
(cen, diff) = self.rect_to_center_diff(
self.recent_good_rects[iface])
self.feats_to_centers[
iface] = self.make_face_model(
cen, diff,
[(kp0, kp1)
for (kp0, kp1, kp2) in kp_for_model])
avgd = sum([
kp2 for (kp0, kp1, kp2) in kp_for_model
]) / len(kp_for_model)
cen3d = self.cam.pix2cam(cen[0], cen[1], avgd)
self.feats_to_centers_3d[
iface] = self.make_face_model(
cen3d,
[self.real_face_sizes_3d[iface]] * 3,
kp3d_for_model)
self.keyframes[
self.current_keyframes[iface]] = copy.copy(
self.recent_good_frames[iface])
self.keyframes[self.current_keyframes[
iface]].kp = kp_for_model
self.keyframes[
self.current_keyframes[iface]].kp2d = [
(k0, k1) for (k0, k1, k2) in kp_for_model
]
self.keyframes[self.current_keyframes[
iface]].kp3d = kp3d_for_model
self.keyframes[
self.current_keyframes[iface]].matches = [
(i, i) for i in range(len(kp_for_model))
]
self.keyframes[
self.current_keyframes[iface]].good_matches = [
True
] * len(kp_for_model)
self.keyframes[self.current_keyframes[
iface]].desc_diffs = [0] * len(kp_for_model)
self.face_centers_3d[iface] = copy.deepcopy(cen3d)
# Not changing the face size
self.current_keyframes[
iface] = 0 #### HACK: ONLY ONE KEYFRAME!!!
self.same_key_rgfs[iface] = True
# Don't need to change the recent good frame yet.
else:
# Making a new model off of the current frame but with the predicted new position.
# HACK: The displacement computation assumes that the robot/head is still, fix this.
bad_frame = True
#done_matching = True
if DEBUG:
print "Bad frame ", self.seq, " for face ", iface
(cen, diff) = self.rect_to_center_diff(
self.faces[iface])
if DEBUG:
print "Motion for bad frame ", self.recent_good_motion[
iface], self.recent_good_motion_3d[iface]
new_cen = [
cen[0] + self.recent_good_motion[iface][0],
cen[1] + self.recent_good_motion[iface][1]
]
diff = [
diff[0] + self.recent_good_motion[iface][2],
diff[1] + self.recent_good_motion[iface][2]
]
self.faces[iface] = (new_cen[0] - diff[0],
new_cen[1] - diff[1],
2.0 * diff[0], 2.0 * diff[1])
(x, y, w, h) = copy.deepcopy(self.faces[iface])
pred_cen_3d = [
o + n for (o, n) in zip(
self.face_centers_3d[iface],
self.recent_good_motion_3d[iface])
]
pred_cen_3d.append(
1.0 *
self.real_face_sizes_3d[iface]) #### ZMULT
self.get_features(ia, self.num_feats, (x, y, w, h),
pred_cen_3d)
if not ia.kp2d:
break
if DESCRIPTOR == 'CALONDER':
self.vo.collect_descriptors(ia)
elif DESCRIPTOR == 'SAD':
self.vo.collect_descriptors_sad(ia)
else:
pass
self.keyframes[
self.current_keyframes[iface]] = copy.copy(ia)
self.current_keyframes[iface] = 0
(cen, diff) = self.rect_to_center_diff(
self.faces[iface])
self.feats_to_centers[
iface] = self.make_face_model(
cen, diff, ia.kp2d)
cen3d = self.cam.pix2cam(cen[0], cen[1], ia.avgd)
self.feats_to_centers_3d[
iface] = self.make_face_model(
cen3d,
[self.real_face_sizes_3d[iface]] * 3,
ia.kp3d)
self.face_centers_3d[iface] = copy.deepcopy(cen3d)
self.same_key_rgfs[iface] = True
# Good matches, mark this frame as good
else:
done_matching = True
# END MATCHING
# If we got enough matches for this frame, track.
if ia.kp and ia.kp2d:
# Track
sparse_pred_list = []
sparse_pred_list_2d = []
probs = []
bandwidths = []
size_mult = 1.0
for ((match1, match2),
score) in zip(ia.matches, ia.desc_diffs):
if score < self.desc_diff_thresh:
kp3d = self.cam.pix2cam(ia.kp[match2][0],
ia.kp[match2][1],
ia.kp[match2][2])
sparse_pred_list.append(
(kp3d[0] +
self.feats_to_centers_3d[iface][match1][0],
kp3d[1] +
self.feats_to_centers_3d[iface][match1][1],
kp3d[2] +
self.feats_to_centers_3d[iface][match1][2]))
sparse_pred_list_2d.append(
(ia.kp2d[match2][0] +
self.feats_to_centers[iface][match1][0],
ia.kp2d[match2][1] +
self.feats_to_centers[iface][match1][1]))
probs = [1.0] * len(sparse_pred_list_2d)
bandwidths = [size_mult * self.real_face_sizes_3d[iface]
] * len(sparse_pred_list_2d)
if DEBUG:
print "Old center 3d ", self.face_centers_3d[iface]
print "Old center 2d ", (x + (w - 1) / 2.0,
y + (h - 1) / 2.0)
old_rect = self.faces[iface]
(old_center, old_diff) = self.rect_to_center_diff(old_rect)
new_center = self.mean_shift_sparse(
self.face_centers_3d[iface], sparse_pred_list, probs,
bandwidths, 10, 5.0)
new_center_2d = self.cam.cam2pix(new_center[0],
new_center[1],
new_center[2])
ltf = self.cam.cam2pix(
new_center[0] - self.real_face_sizes_3d[iface],
new_center[1] - self.real_face_sizes_3d[iface],
new_center[2])
rbf = self.cam.cam2pix(
new_center[0] + self.real_face_sizes_3d[iface],
new_center[1] + self.real_face_sizes_3d[iface],
new_center[2])
w = rbf[0] - ltf[0]
h = rbf[1] - ltf[1]
if DEBUG:
print "new center 3d ", new_center
print "new_center 2d ", new_center_2d
(nx, ny, nw, nh) = (new_center_2d[0] - (w - 1) / 2.0,
new_center_2d[1] - (h - 1) / 2.0, w, h)
# Force the window back into the image.
dx = max(0, 0 - nx) + min(0, im.width - nx + nw)
dy = max(0, 0 - ny) + min(0, im.height - ny + nh)
nx += dx
ny += dy
self.faces[iface] = [nx, ny, nw, nh]
self.recent_good_rects[iface] = [nx, ny, nw, nh]
if bad_frame:
self.recent_good_motion[
iface] = self.recent_good_motion[iface]
self.recent_good_motion_3d[
iface] = self.recent_good_motion_3d[iface]
else:
self.recent_good_motion[iface] = [
new_center_2d[0] - old_center[0],
new_center_2d[1] - old_center[1],
((nw - 1.0) / 2.0) - old_diff[0]
]
self.recent_good_motion_3d[iface] = [
new_center[i] - self.face_centers_3d[iface][i]
for i in range(len(new_center))
]
self.face_centers_3d[iface] = copy.deepcopy(new_center)
self.recent_good_frames[iface] = copy.copy(ia)
self.same_key_rgfs[iface] = False
if DEBUG:
print "motion ", self.recent_good_motion[iface]
print "face 2d ", self.faces[iface]
print "face center 3d ", self.face_centers_3d[iface]
# Output the location of this face center in the 3D camera frame (of the left camera), and rotate
# the coordinates to match the robot's idea of the 3D camera frame.
center_uvd = (nx + (nw - 1) / 2.0, ny + (nh - 1) / 2.0,
(numpy.average(ia.kp, 0))[2])
center_camXYZ = self.cam.pix2cam(center_uvd[0],
center_uvd[1],
center_uvd[2])
center_robXYZ = (center_camXYZ[2], -center_camXYZ[0],
-center_camXYZ[1])
########### PUBLISH the face center for the head controller to track. ########
if not self.usebag:
stamped_point = PointStamped()
(stamped_point.point.x, stamped_point.point.y,
stamped_point.point.z) = center_robXYZ
stamped_point.header.frame_id = "stereo"
stamped_point.header.stamp = imarray.header.stamp
self.pub.publish(stamped_point)
# End later frames
############ DRAWING ################
if SAVE_PICS:
if not self.keyframes or len(self.keyframes) <= iface:
bigim_py = im_py
draw = ImageDraw.Draw(bigim_py)
else:
key_im = self.keyframes[self.current_keyframes[iface]]
keyim_py = Image.fromstring("L", key_im.size,
key_im.rawdata)
bigim_py = Image.new(
"RGB", (im_py.size[0] + key_im.size[0], im_py.size[1]))
bigim_py.paste(keyim_py.convert("RGB"), (0, 0))
bigim_py.paste(im_py, (key_im.size[0] + 1, 0))
draw = ImageDraw.Draw(bigim_py)
(x, y, w, h) = self.faces[iface]
draw.rectangle((x, y, x + w, y + h), outline=(0, 255, 0))
draw.rectangle(
(x + key_im.size[0], y, x + w + key_im.size[0], y + h),
outline=(0, 255, 0))
(x, y, w, h) = old_rect
draw.rectangle((x, y, x + w, y + h),
outline=(255, 255, 255))
draw.rectangle(
(x + key_im.size[0], y, x + w + key_im.size[0], y + h),
outline=(255, 255, 255))
mstart = old_center
mend = (old_center[0] + self.recent_good_motion[iface][0],
old_center[1] + self.recent_good_motion[iface][1])
draw.rectangle((mstart[0] - 1, mstart[1] - 1,
mstart[0] + 1, mstart[1] + 1),
outline=(255, 255, 255))
draw.rectangle(
(mend[0] - 1, mend[1] - 1, mend[0] + 1, mend[1] + 1),
outline=(0, 255, 0))
draw.line(mstart + mend, fill=(255, 255, 255))
for (x, y) in key_im.kp2d:
draw_x(draw, (x, y), (1, 1), (255, 0, 0))
for (x, y) in ia.kp2d:
draw_x(draw, (x + key_im.size[0], y), (1, 1),
(255, 0, 0))
if self.seq > 0:
for (x, y) in sparse_pred_list_2d:
draw_x(draw, (x, y), (1, 1), (0, 0, 255))
draw_x(draw, (x + key_im.size[0], y), (1, 1),
(0, 0, 255))
if ia.matches:
for ((m1, m2),
score) in zip(ia.matches, ia.desc_diffs):
if score > self.desc_diff_thresh:
color = (255, 0, 0)
else:
color = (0, 255, 0)
draw.line(
(key_im.kp2d[m1][0], key_im.kp2d[m1][1],
ia.kp2d[m2][0] + key_im.size[0],
ia.kp2d[m2][1]),
fill=color)
bigim_py.save("/tmp/tiff/feats%06d_%03d.tiff" %
(self.seq, iface))
#END DRAWING
# END FACE LOOP
self.seq += 1
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)
trajectory = [ [] for i in vos]
start,end = 941,1000
start,end = 0,2000
if cam and topic.endswith("videre/images"):
if framecounter == end:
break
if start <= framecounter and (framecounter % 1) == 0:
imgR = imgAdapted(msg.images[0])
imgL = imgAdapted(msg.images[1])
if not first_pair:
first_pair = (imgL, imgR)
for i,vo in enumerate(vos):
af = SparseStereoFrame(imgL, imgR)
vo.handle_frame(af)
x,y,z = vo.pose.xform(0,0,0)
assert abs(x) < 20.0
trajectory[i].append((x,y,z))
vo_x[i].append(x)
vo_y[i].append(z)
x1,y1,z1 = vo.pose.xform(0,0,1)
vo_u[i].append(x1 - x)
vo_v[i].append(z1 - z)
print framecounter
framecounter += 1
if topic.endswith("odom_estimation"):
oe_x.append(-msg.pose.position.y)
oe_y.append(msg.pose.position.x)
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
