def read_tf_frame(self, frame):
"""
This method read a /tf frame and create the corresponding node in the world given in constructor
@param frame:
@return :
"""
try:
msg = self.tfBuffer.lookup_transform(self.reference_frame, frame,
rospy.Time(0))
trans = [
msg.transform.translation.x, msg.transform.translation.y,
msg.transform.translation.z
]
rot = [
msg.transform.rotation.x, msg.transform.rotation.y,
msg.transform.rotation.z, msg.transform.rotation.w
]
translation_mat = transformations.translation_matrix(trans)
rotation_mat = transformations.quaternion_matrix(rot)
transform = numpy.dot(translation_mat, rotation_mat)
node = self.create_object(frame)
node.transformation = transform
if frame in self.already_created_frame_node_ids:
node.id = self.already_created_frame_node_ids[frame]
else:
self.already_created_frame_node_ids[frame] = node.id
if node.id in self.nodes_transform:
if math.sqrt(trans[0] * trans[0] + trans[1] * trans[1] +
trans[2] * trans[2]) < MIN_DIST_DETECTION:
return None
if not numpy.allclose(self.nodes_transform[node.id],
node.transformation,
rtol=0,
atol=EPSILON):
self.nodes_transform[node.id] = transform
return node
else:
self.nodes_transform[node.id] = transform
return node
except (tf2_ros.TransformException, tf2_ros.LookupException,
tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
pass
def transformation_matrix(user, joint_code):
joint = user.skeleton.joints[joint_code]
#confidence = head.positionConfidence
translation = (joint.position.x / 1000., joint.position.y / 1000.,
joint.position.z / 1000.)
translation_mat = transformations.translation_matrix(translation)
orientation = (joint.orientation.x, joint.orientation.y,
joint.orientation.z, joint.orientation.w)
rotation_mat = transformations.quaternion_matrix(orientation)
# mirror back the skeleton along the X axis, as the freenect driver mirrors the
# RGBD image by default
mirror_mat = transformations.reflection_matrix((0, 0, 0), (1, 0, 0))
return numpy.dot(mirror_mat, numpy.dot(translation_mat, rotation_mat))
def transformation_matrix(user, joint_code):
joint = user.skeleton.joints[joint_code]
#confidence = head.positionConfidence
translation = (joint.position.x/1000.,
joint.position.y/1000.,
joint.position.z/1000.)
translation_mat = transformations.translation_matrix(translation)
orientation = (joint.orientation.x,
joint.orientation.y,
joint.orientation.z,
joint.orientation.w)
rotation_mat = transformations.quaternion_matrix(orientation)
# mirror back the skeleton along the X axis, as the freenect driver mirrors the
# RGBD image by default
mirror_mat = transformations.reflection_matrix((0,0,0),(1,0,0))
return numpy.dot(mirror_mat, numpy.dot(translation_mat, rotation_mat))
"the error messages in the console. Model data "
"(s.dat, h.dat...) might be missing.")
sys.exit(-1)
logger.info("User tracker loaded.")
logger.info("Now waiting for humans...")
#############
with underworlds.Context("Human tracker") as ctx:
world = ctx.worlds[args.world]
nodes = world.scene.nodes
camera = Node("kinect", ENTITY)
translation_cam = transformations.translation_matrix((1, 0, 0.5))
# According to http://www.openni.ru/wp-content/uploads/2013/02/NITE-Algorithms.pdf
# the sensor is oriented as follow:
# " +X points to the right of the, +Y points up, and +Z
# points in the direction of increasing depth."
rotation_cam = transformations.euler_matrix(math.pi / 2, 0,
math.pi / 2)
camera.transformation = numpy.dot(translation_cam, rotation_cam)
camera.parent = world.scene.rootnode.id
nodes.append(camera)
# Load the mannequin mesh into underworlds and get back the list of
# underworlds nodes
bodypartslist = ModelLoader(args.world).load(
sys.exit(-1)
logger.info("User tracker loaded.")
logger.info("Now waiting for humans...")
#############
with underworlds.Context("Human tracker") as ctx:
world = ctx.worlds[args.world]
nodes = world.scene.nodes
camera = Node("kinect", ENTITY)
translation_cam=transformations.translation_matrix((1,0,0.5))
# According to http://www.openni.ru/wp-content/uploads/2013/02/NITE-Algorithms.pdf
# the sensor is oriented as follow:
# " +X points to the right of the, +Y points up, and +Z
# points in the direction of increasing depth."
rotation_cam=transformations.euler_matrix(math.pi/2,0,math.pi/2)
camera.transformation = numpy.dot(translation_cam, rotation_cam)
camera.parent = world.scene.rootnode.id
nodes.append(camera)
# Load the mannequin mesh into underworlds and get back the list of
# underworlds nodes
bodypartslist = ModelLoader(args.world).load("../share/mannequin.blend",
ctx=ctx,
def transformation_matrix(t, q):
translation_mat = translation_matrix(t)
rotation_mat = quaternion_matrix(q)
return numpy.dot(translation_mat, rotation_mat)
