def dialog_cb(self, idx):
# TODO animace: zesednout, srovnat, pustit timer a pak obratit
# self.set_enabled(False)
ax = self.get_yaw_axis()
angle = 0
# clear yaw & rotate
if ax == PlaceItem.Z:
sres = conversions.qv_mult(self.quaternion, (1, 0, 0))
angle = math.atan2(sres[1], sres[0])
self.position[
2] = self.object_type.bbox.dimensions[0] / 2 # TODO fix this
elif ax == PlaceItem.X or ax == PlaceItem.Y:
sres = conversions.qv_mult(self.quaternion, (0, 0, 1))
angle = math.atan2(sres[1], sres[0])
self.position[
2] = self.object_type.bbox.dimensions[2] / 2 # TODO fix this
else:
print "error"
return
angle = round(angle / (math.pi / 2)) * math.pi / 2
axis = [0, 0, 0]
axis[ax] = 1.0
q = tf.transformations.quaternion_about_axis(angle, axis)
self.set_orientation(q)
# final rotation
rot = (0, 0, 0, 1)
if idx == 0: # "Rotate |"
rot = (0.707, 0, 0, 0.707)
elif idx == 1: # "Rotate --"
rot = (0, 0.707, 0, 0.707)
fin_q = tf.transformations.quaternion_multiply(self.quaternion, rot)
self.set_orientation(fin_q)
if self.point:
self.point.setPos(self.boundingRect().topLeft())
self._update_desc_pos()
def set_orientation(self, q):
self.quaternion = q
ax = self.get_yaw_axis()
if ax == ObjectItem.Z:
self.lx = self.m2pix(self.inflate +
self.object_type.bbox.dimensions[0])
self.ly = self.m2pix(self.inflate +
self.object_type.bbox.dimensions[1])
sres = conversions.qv_mult(self.quaternion, (1, 0, 0))
angle = math.atan2(sres[1], sres[0])
self.on_table = self.position[
2] < self.object_type.bbox.dimensions[2] + 0.05
elif ax in [ObjectItem.X, ObjectItem.Y]:
res = conversions.qv_mult(self.quaternion, (0, 0, 1))
self.lx = self.m2pix(self.inflate +
self.object_type.bbox.dimensions[2])
# TODO use correct dimension (x/y) - now let's assume that x and y dimensions are same
self.ly = self.m2pix(self.inflate +
self.object_type.bbox.dimensions[1])
angle = math.atan2(res[1], res[0])
self.on_table = self.position[
2] < self.object_type.bbox.dimensions[0] + 0.05
else:
self.set_enabled(False, True)
return
self.setRotation(-angle / (math.pi * 2) * 360)
# TODO if not on table - display somewhere list of detected objects or what?
self.set_enabled(self.on_table, True)
self.update()
def get_yaw_axis(self):
ax = ((1, 0, 0), (0, 1, 0), (0, 0, 1))
for idx in range(0, len(ax)):
res = conversions.qv_mult(self.quaternion, ax[idx])
# TODO euclid dist
if conversions.is_close(res[0], 0, abs_tol=0.1) and conversions.is_close(res[1], 0, abs_tol=0.1):
return idx
return -1
def get_yaw_axis(self):
ax = ((1, 0, 0), (0, 1, 0), (0, 0, 1))
c_idx = None
c_dist = None
for idx in range(len(ax)):
res = conversions.qv_mult(self.quaternion, ax[idx])
dist = math.sqrt(res[0]**2 + res[1]**2)
if c_dist is None or c_dist > dist:
c_dist = dist
c_idx = idx
# TODO disable object (return -1) if dist is too high?
return c_idx