def render(filename, color, theta, dest): skel = Skeleton(stereo_cam) skel.load(filename, load_PR = False) skel.optimize() skel.optimize() skel.plot(color, False, theta) xlim = pylab.xlim() ylim = pylab.ylim() xrange = xlim[1] - xlim[0] yrange = ylim[1] - ylim[0] r = max(xrange, yrange) * 0.5 # 0.32 mid = sum(xlim) / 2 pylab.xlim(mid - r, mid + r) mid = sum(ylim) / 2 pylab.ylim(mid - r, mid + r)
def train_test_split_observations(ratio, classifier):
data_dir = Path('..') / 'z'
clips = []
# Store Path objects pointing to clipped skeletons
for root, dirs, files in os.walk(str(data_dir)):
clips += [ Path(root) / f for f in fnmatch.filter(files, '*clipped*tsv') ]
# Random split into training and test data
np.random.shuffle(clips)
split_pt = int(ratio * len(clips))
clips_train = clips[:split_pt]
clips_test = clips[split_pt:]
O = []
X = []
y = []
for c in clips_train:
s = Skeleton(str(c))
s.load(skipheader=False, delimiter='\t', extracols=(232,))
s.angularize()
X += [ s.data[:,:-1].tolist() ]
y += [ s.data[:,-1].tolist() ]
classifier.fit([frame for clip in X for frame in clip ], [ label for clip in y for label in clip ])
for clip in X:
O += [ classifier.predict(clip).tolist() ]
O_test = []
X_test = []
y_test = []
for c in clips_test:
s = Skeleton(str(c))
s.load(skipheader=False, delimiter='\t', extracols=(232,))
s.angularize()
X_test += [ s.data[:, :-1].tolist() ]
y_test += [ s.data[:, -1].tolist() ]
for clip in X_test:
O_test += [ classifier.predict(clip).tolist() ]
return (X, y, O, X_test, y_test, O_test)
def dataset_summary():
dataset_folder = io_utils.get_dataset_folder()
skinning_path = os.path.join(dataset_folder, 'skinning.npy')
skeleton_path = os.path.join(dataset_folder, 'skeleton.txt')
print('-- ANIMATION CLIPS --')
clips = fnmatch.filter(os.listdir(dataset_folder), '*.npz')
print(clips)
print('num clips : ' + str(len(clips)))
print('-- SKINNING --')
print('skinning file : ' + str(os.path.exists(skinning_path)))
print('-- SKELETON --')
print('skeleton file : ' + str(os.path.exists(skeleton_path)))
skeleton = Skeleton()
skeleton.load(skeleton_path)
skeleton.print_root()
def train_test_split_clips(ratio):
data_dir = Path('..') / 'z'
clips = []
# Store Path objects pointing to clipped skeletons
for root, dirs, files in os.walk(str(data_dir)):
clips += [
Path(root) / f for f in fnmatch.filter(files, '*clipped*tsv')
]
# Random split into training and test data
np.random.shuffle(clips)
split_pt = int(ratio * len(clips))
clips_train = clips[:split_pt]
clips_test = clips[split_pt:]
X = []
y = []
for c in clips_train:
s = Skeleton(str(c))
s.load(skipheader=False, delimiter='\t', extracols=(232, ))
#s.normalize('spine-base')
X += s.data[:, :-1].tolist()
y += s.data[:, -1].tolist()
X_test = []
y_test = []
for c in clips_test:
s = Skeleton(str(c))
s.load(skipheader=False, delimiter='\t', extracols=(232, ))
X_test += s.data[:, :-1].tolist()
y_test += s.data[:, -1].tolist()
return (X, y, X_test, y_test)
def gen_sim_matrix(x_files, y_files):
# Get a combined list of the skeleton files chosen earlier
combined_files = x_files + y_files
# Initializing the similarity matrix to be used for final comparison
s_matrix = np.zeros(len(combined_files)**2).reshape(
len(combined_files), len(combined_files))
# Populating the similarity matrix with distance outputs from DTW
for i, file_i in enumerate(combined_files):
sk_i = Skeleton(file_i)
sk_i.load()
sk_i.normalize('spine-base')
for j, file_j in enumerate(combined_files):
sk_j = Skeleton(file_j)
sk_j.load()
sk_j.normalize('spine-base')
dist, cost, acc, path = dtw(
sk_i.data, sk_j.data, dist=lambda a, b: np.linalg.norm(a - b))
s_matrix[i, j] = dist
return s_matrix
from reader import reader
from math import *
from stereo_utils import camera
import pylab, numpy
from matplotlib.patches import Ellipse
stereo_cam = camera.Camera((389.0, 389.0, 89.23 * 1e-3, 323.42, 323.42, 274.95))
bad_vertices = set([ 35437, 35455, 37380, 40122, 40126, 40207, 40229 ])
if 1:
skel = Skeleton(stereo_cam)
filename = "/wg/wgdata1/vol1/iros2009/mkplot_snap"
filename = "ABCDEFG"
skel.load(filename)
print "skel.nodes:", len(skel.nodes)
for e in skel.edges:
if e[0] in bad_vertices or e[1] in bad_vertices:
print "bad edge", e
skel.nodes = [ id for id in skel.nodes if not id in bad_vertices ]
print "skel.nodes:", len(skel.nodes)
print set(skel.labelization())
skel.optimize()
skel.optimize()
skel.save("iros2009/ABCDEF2")
pos,edges,_ = skel.localization()
f = open("deletion2.pickle", "w")
pickle.dump(pos, f)
pickle.dump(edges, f)
pickle.dump(skel.labelization(), f)
def train_test_split_observations(ratio, classifier):
data_dir = Path('..') / 'z'
clips = []
# Store Path objects pointing to clipped skeletons
for root, dirs, files in os.walk(str(data_dir)):
clips += [
Path(root) / f for f in fnmatch.filter(files, '*clipped*tsv')
]
# Random split into training and test data
np.random.shuffle(clips)
split_pt = int(ratio * len(clips))
clips_train = clips[:split_pt]
clips_test = clips[split_pt:]
O = []
X = []
y = []
for c in clips_train:
s = Skeleton(str(c))
s.load(skipheader=False, delimiter='\t', extracols=(232, ))
#s.normalize('spine-base')
y += [s.data[1:, -1].tolist()]
# Keep s.data numpy 2d array for now
s.data = s.data[:, :-1]
last_frame = None
temp_clip = []
for frame in s.data:
# Note that frame is numpy array
if last_frame != None:
temp_clip += [(frame - last_frame).tolist()]
last_frame = frame
X += [temp_clip]
classifier.fit([frame for clip in X for frame in clip],
[label for clip in y for label in clip])
for clip in X:
O += [classifier.predict(clip).tolist()]
O_test = []
X_test = []
y_test = []
for c in clips_test:
s = Skeleton(str(c))
s.load(skipheader=False, delimiter='\t', extracols=(232, ))
#s.normalize('spine-base')
y_test += [s.data[1:, -1].tolist()]
# Keep s.data numpy 2d array for now
s.data = s.data[:, :-1]
last_frame = None
temp_clip = []
for frame in s.data:
# Note that frame is numpy array
if last_frame != None:
temp_clip += [(frame - last_frame).tolist()]
last_frame = frame
X_test += [temp_clip]
for clip in X_test:
O_test += [classifier.predict(clip).tolist()]
return (X, y, O, X_test, y_test, O_test)
class RoadmapServer:
def __init__(self, args):
rospy.init_node('roadmap_server')
self.optimization_distance = rospy.get_param('~optimization_distance', 10);
self.tf = TransformListener()
stereo_cam = camera.Camera((389.0, 389.0, 89.23 * 1e-3, 323.42, 323.42, 274.95))
self.skel = Skeleton(stereo_cam)
# self.skel.node_vdist = 1
if False:
self.skel.load(args[1])
self.skel.optimize()
self.startframe = 100000
else:
self.startframe = 0
self.frame_timestamps = {}
self.vo = None
self.pub = rospy.Publisher("roadmap", vslam.msg.Roadmap)
time.sleep(1)
#self.send_map(rospy.time(0))
self.wheel_odom_edges = set()
rospy.Subscriber('/wide_stereo/raw_stereo', stereo_msgs.msg.RawStereo, self.handle_raw_stereo, queue_size=2, buff_size=7000000)
def send_map(self, stamp):
if len(self.skel.nodes) < 4:
return
TG = nx.MultiDiGraph()
for i in self.skel.nodes:
TG.add_node(i)
for (a,b,p,c) in self.skel.every_edge + list(self.wheel_odom_edges):
TG.add_edge(a, b, (p, c))
here = max(self.skel.nodes)
# TG is the total graph, G is the local subgraph
uTG = TG.to_undirected()
print "uTG", uTG.nodes(), uTG.edges()
close = set([here])
for i in range(self.optimization_distance):
close |= set(nx.node_boundary(uTG, close))
print "close", close
G = TG.subgraph(close)
uG = uTG.subgraph(close)
pg = TreeOptimizer3()
def mk_covar(xyz, rp, yaw):
return (1.0 / math.sqrt(xyz),1.0 / math.sqrt(xyz), 1.0 / math.sqrt(xyz), 1.0 / math.sqrt(rp), 1.0 / math.sqrt(rp), 1.0 / math.sqrt(yaw))
weak = mk_covar(9e10,3,3)
strong = mk_covar(0.0001, 0.000002, 0.00002)
pg.initializeOnlineOptimization()
def pgadd(p, n, data):
relpose, strength = data
if strength == 0.0:
cov = weak
else:
cov = strong
pg.addIncrementalEdge(p, n, relpose.xform(0,0,0), relpose.euler(), cov)
Gedges = G.edges(data = True)
revmap = {}
map = {}
for n in nx.dfs_preorder(uG, here):
revmap[len(map)] = n
map[n] = len(map)
priors = [p for p in uG.neighbors(n) if p in map]
if priors == []:
print "START NODE", n, "as", map[n]
else:
print "NEXT NODE", n
p = priors[0]
if not G.has_edge(p, n):
n,p = p,n
data = G.get_edge_data(p, n)[0]
Gedges.remove((p, n, data))
pgadd(map[p], map[n], data)
for (p,n,d) in Gedges:
pgadd(map[p], map[n], d)
pg.initializeOnlineIterations()
start_error = pg.error()
for i in range(10):
pg.iterate()
print "Error from", start_error, "to", pg.error()
pg.recomputeAllTransformations()
print self.tf.getFrameStrings()
target_frame = "base_link"
t = self.tf.getLatestCommonTime("wide_stereo_optical_frame", target_frame)
trans,rot = self.tf.lookupTransform(target_frame, "wide_stereo_optical_frame", t)
xp = Pose()
xp.fromlist(self.tf.fromTranslationRotation(trans,rot))
roadmap_nodes = dict([ (n, (30,0,0)) for n in TG.nodes() ])
nl = sorted(roadmap_nodes.keys())
print "nl", nl
for i in sorted(map.values()):
xyz,euler = pg.vertex(i)
p = from_xyz_euler(xyz, euler)
pose_in_target = xp * p
x,y,z = pose_in_target.xform(0,0,0)
euler = pose_in_target.euler()
print x,y,z, euler
roadmap_nodes[revmap[i]] = (x, y, euler[2])
roadmap_edges = []
for (p,n) in set(TG.edges()):
print p,n
best_length = max([ (confidence, pose.distance()) for (pose, confidence) in TG.get_edge_data(p, n).values()])[1]
roadmap_edges.append((p, n, best_length))
msg = vslam.msg.Roadmap()
msg.header.stamp = stamp
msg.header.frame_id = "base_link"
msg.nodes = [vslam.msg.Node(*roadmap_nodes[n]) for n in nl]
print "(p,n)", [ (p,n) for (p,n,l) in roadmap_edges ]
msg.edges = [vslam.msg.Edge(nl.index(p), nl.index(n), l) for (p, n, l) in roadmap_edges]
msg.localization = nl.index(here)
self.pub.publish(msg)
if 1:
import matplotlib.pyplot as plt
fig = plt.figure(figsize = (12, 6))
plt.subplot(121)
pos = nx.spring_layout(TG, iterations=1000)
nx.draw(TG, pos,node_color='#A0CBE2')
nx.draw_networkx_edges(G, pos, with_labels=False, edge_color='r', alpha=0.5, width=25.0, arrows = False)
plt.subplot(122)
nx.draw(G, pos = dict([(n, roadmap_nodes[n][:2]) for n in G.nodes()]))
#plt.show()
plt.savefig("/tmp/map_%d.png" % here)
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.fd = FeatureDetectorFast(300)
self.ds = DescriptorSchemeCalonder()
self.vo = VisualOdometer(cam, scavenge = False,
inlier_error_threshold = 3.0, sba = None,
inlier_thresh = 100,
position_keypoint_thresh = 0.2, angle_keypoint_thresh = 0.15)
self.vo.num_frames = self.startframe
pair = [Image.fromstring("L", size, i.uint8_data.data) for i in [ msg.left_image, msg.right_image ]]
af = SparseStereoFrame(pair[0], pair[1], feature_detector = self.fd, descriptor_scheme = self.ds)
self.vo.handle_frame(af)
self.frame_timestamps[af.id] = msg.header.stamp
if self.skel.add(self.vo.keyframe):
print "====>", self.vo.keyframe.id, self.frame_timestamps[self.vo.keyframe.id]
#print self.tf.getFrameStrings()
#assert self.tf.frameExists("wide_stereo_optical_frame")
#assert self.tf.frameExists("odom_combined")
N = sorted(self.skel.nodes)
for a,b in zip(N, N[1:]):
if (a,b) in self.skel.edges:
t0 = self.frame_timestamps[a]
t1 = self.frame_timestamps[b]
if self.tf.canTransformFull("wide_stereo_optical_frame", t0, "wide_stereo_optical_frame", t1, "odom_combined"):
t,r = self.tf.lookupTransformFull("wide_stereo_optical_frame", t0, "wide_stereo_optical_frame", t1, "odom_combined")
relpose = Pose()
relpose.fromlist(self.tf.fromTranslationRotation(t,r))
self.wheel_odom_edges.add((a, b, relpose, 1.0))
self.send_map(msg.header.stamp)
if vos == None:
vos = [
VisualOdometer(cam, scavenge = False, feature_detector = FeatureDetectorFast(),
descriptor_scheme = DescriptorSchemeCalonder(),
inlier_error_threshold = 3.0, sba = None,
inlier_thresh = 100,
position_keypoint_thresh = 0.2, angle_keypoint_thresh = 0.15)
]
vo_x = [ [] for i in vos]
vo_y = [ [] for i in vos]
vo_u = [ [] for i in vos]
vo_v = [ [] for i in vos]
trajectory = [ [] for i in vos]
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))
