def __init__ (self, *args, **margs):
if len(args) == 4:
self.q = na.array(args, dtype=na.float64)
elif len(args) == 1 and len(args[0]) == 4:
self.q = na.array(args[0][:], dtype=na.float64)
else:
raise ValueError("invalid initializer: " + str(args))
if margs.get("check_norm", True):
assert_sorta_eq(na.linalg.norm(self.q), 1.0)
def findIntersect(self,
here_point,
select_point,
obj=None,
grounding_filter=lambda x: True):
if obj == None:
obj = self.findClosestGrounding(select_point, grounding_filter)
p = obj.prism
x, y, z = sf.math3d_intersect_line_plane(
tp([select_point, here_point]),
tp([(x, y, p.zEnd) for x, y in p.points_pts]))
assert_sorta_eq(z, p.zEnd)
z = p.zEnd
return (x, y, z), obj
def test_features(self):
from spatial_feature_expected_results import results
for result in results:
fig_xyth = result["args"]["fig_xyth"]
gnd_xy = result["args"]["gnd_xy"]
result_dict = result["result"]
feats_C = sfe_extract_f_path_l_polygon(fig_xyth,
gnd_xy,
normalize=True)
names_C = sfe_f_path_l_polygon_names()
fdict = compute_fdict(names_C, feats_C)
for key, value in fdict.iteritems():
expected_value = result_dict[key]
assert_sorta_eq(value, expected_value)
def __init__(self, timestamps, points_xyzquat):
"""
Timestamps are in microseconds.
"""
assert_sorta_eq(na.array(points_xyzquat).shape[0], 7)
self.type = self.__class__.__name__
try:
timestamps, points_xyzquat = compress(timestamps, points_xyzquat)
except:
print timestamps
print points_xyzquat
raise
self.timestamps = [long(x) for x in timestamps]
self.points_xyzquat = na.array(points_xyzquat)
assert_sorta_eq(self.points_xyzquat.shape[0], 7)
self.points_xyztheta = points_xyzquat_to_xyztheta(self.points_xyzquat)
self.updateRep()