def test_png_export():
"""Test if the graph can be exported to PNG."""
random_state = np.random.RandomState(0)
ee2robot = transform_from_pq(
np.hstack((np.array([0.4, -0.3,
0.5]), random_quaternion(random_state))))
cam2robot = transform_from_pq(np.hstack((np.array([0.0, 0.0, 0.8]), q_id)))
object2cam = transform_from(
active_matrix_from_intrinsic_euler_xyz(np.array([0.0, 0.0, 0.5])),
np.array([0.5, 0.1, 0.1]))
tm = TransformManager()
tm.add_transform("end-effector", "robot", ee2robot)
tm.add_transform("camera", "robot", cam2robot)
tm.add_transform("object", "camera", object2cam)
_, filename = tempfile.mkstemp(".png")
try:
tm.write_png(filename)
assert_true(os.path.exists(filename))
except ImportError:
raise SkipTest("pydot is required for this test")
finally:
if os.path.exists(filename):
try:
os.remove(filename)
except WindowsError:
pass # workaround for permission problem on Windows
def test_transforms_from_pqs_0dims():
random_state = np.random.RandomState(0)
pq = np.empty(7)
pq[:3] = random_state.randn(3)
pq[3:] = random_quaternion(random_state)
A2B = transforms_from_pqs(pq, False)
assert_array_almost_equal(A2B, transform_from_pq(pq))
def compute(self):
for key in self.cameras_dict:
frames = self.cameras_dict[key][2].wait_for_frames()
f = frames.first_or_default(rs.stream.pose)
data = f.as_pose_frame().get_pose_data()
#self.cameras_dict[key][3] = data
self.cameras_dict[key][1].add_transform("world", "slam_sensor", pytr.transform_from_pq([data.translation.x * 1000.0,
-data.translation.z * 1000.0,
data.translation.y * 1000.0,
data.rotation.w,
data.rotation.x,
data.rotation.y,
data.rotation.z]))
t = self.cameras_dict[key][1].get_transform("world", "origin")
self.cameras_dict[key][3] = pytr.transform(t,[0,0,0,1])[0:3]
self.cameras_dict[key][4] = self.quaternion_to_euler_angle(data.rotation.w, data.rotation.x, data.rotation.y, data.rotation.z)
self.cameras_dict[key][5] = data.mapper_confidence
self.cameras_dict[key][6] = data.tracker_confidence
# print
if self.print:
ret = RoboCompFullPoseEstimation.FullPoseEuler()
sigma = 0
#CALCULATE ADDITION BOTH DATA'S CAMERA
for key in self.cameras_dict:
ret.x = ret.x + self.cameras_dict[key][3][0] * self.cameras_dict[key][6]
ret.y = ret.y + self.cameras_dict[key][3][1] * self.cameras_dict[key][6]
ret.z = ret.z + self.cameras_dict[key][3][2] * self.cameras_dict[key][6]
ret.rx = ret.rx + self.cameras_dict[key][4][0] * self.cameras_dict[key][6]
ret.ry = ret.ry + self.cameras_dict[key][4][1] * self.cameras_dict[key][6]
ret.rz = ret.rz + self.cameras_dict[key][4][2] * self.cameras_dict[key][6]
sigma = sigma + self.cameras_dict[key][6]
#CALCULATE AVERAGE OF POSITION
ret.x = ret.x / sigma
ret.y = ret.y / sigma
ret.z = ret.z / sigma
#CALCULATE AVERAGE OF ANGLES
ret.rx = ret.rx / sigma
ret.ry = ret.ry / sigma
ret.rz = ret.rz / sigma
print(sigma, ret.x, ret.y, ret.z, ret.rx, ret.ry, ret.rz)
def transform_point_cloud_trans_quat(cloud_msg, translation, rotation,
target_frame):
"""
transform cloud using transforms3d to calculate transformation matrix
:type cloud_msg: PointCloud2
:param translation: translation vector
:type translation: list
:param rotation: rotation quaternion, i.e. [w, x, y, z]
:param target_frame: name of new frame to fill in cloud_msg.header.frame_id
:return: transformed cloud
:rtype: PointCloud2
"""
from pytransform3d.transformations import transform_from_pq
transform_matrix = transform_from_pq(np.append(translation, rotation))
return transform_point_cloud(cloud_msg, transform_matrix, target_frame)
def test_transform_from_pq():
"""Test conversion from position and quaternion to homogeneous matrix."""
pq = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0])
A2B = transform_from_pq(pq)
assert_array_almost_equal(A2B, np.eye(4))
In this example, we will use the transform manager to infer a transformation
automatically.
"""
print(__doc__)
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d.rotations import (random_quaternion, matrix_from_euler_xyz,
q_id)
from pytransform3d.transformations import transform_from_pq, transform_from
from pytransform3d.transform_manager import TransformManager
random_state = np.random.RandomState(0)
ee2robot = transform_from_pq(
np.hstack((np.array([0.4, -0.3, 0.5]), random_quaternion(random_state))))
cam2robot = transform_from_pq(np.hstack((np.array([0.0, 0.0, 0.8]), q_id)))
object2cam = transform_from(matrix_from_euler_xyz(np.array([0.0, 0.0, 0.5])),
np.array([0.5, 0.1, 0.1]))
tm = TransformManager()
tm.add_transform("end-effector", "robot", ee2robot)
tm.add_transform("camera", "robot", cam2robot)
tm.add_transform("object", "camera", object2cam)
ee2object = tm.get_transform("end-effector", "object")
ax = tm.plot_frames_in("robot", s=0.1)
ax.set_xlim((-0.25, 0.75))
ax.set_ylim((-0.5, 0.5))
ax.set_zlim((0.0, 1.0))
"""
===========================
Camera Representation in 3D
===========================
This visualization is inspired by Blender's camera visualization. It will
show the camera center, a virtual image plane at a desired distance to the
camera center, and the top direction of the virtual image plane.
"""
print(__doc__)
import numpy as np
import pytransform3d.transformations as pt
import pytransform3d.visualizer as pv
cam2world = pt.transform_from_pq([0, 0, 0, np.sqrt(0.5), -np.sqrt(0.5), 0, 0])
# default parameters of a camera in Blender
sensor_size = np.array([0.036, 0.024])
intrinsic_matrix = np.array([[0.05, 0, sensor_size[0] / 2.0],
[0, 0.05, sensor_size[1] / 2.0], [0, 0, 1]])
virtual_image_distance = 1
fig = pv.Figure()
fig.plot_transform(A2B=cam2world, s=0.2)
fig.plot_camera(cam2world=cam2world,
M=intrinsic_matrix,
sensor_size=sensor_size,
virtual_image_distance=virtual_image_distance)
if "__file__" in globals():
fig.show()
else:
"slam_sensor", "robot",
pytr.transform_from(
pyrot.active_matrix_from_intrinsic_euler_xyz([0.0, 0.0, 0]),
[0.0, 0.0, 0.0]))
while True:
frames = pipeline.wait_for_frames()
f = frames.first_or_default(rs.stream.pose)
data = f.as_pose_frame().get_pose_data()
#angles = quaternion_to_euler_angle([data.rotation.w, data.rotation.x, data.rotation.y, data.rotation.z])
tm.add_transform(
"world", "slam_sensor",
pytr.transform_from_pq([
data.translation.x * 1000.0, -data.translation.z * 1000.0,
data.translation.y * 1000.0, data.rotation.w, data.rotation.x,
data.rotation.y, data.rotation.z
]))
t = tm.get_transform("world", "origin")
q = pyrot.quaternion_from_matrix(t[0:3, 0:3])
print(
pytr.transform(t, [0, 0, 0, 1])[0:3], quaternion_to_euler_angle(q),
data.mapper_confidence, data.tracker_confidence)
##print("\r Device Position: ", t[0][3], t[1][3], t[2][3], angles, end="\r")
# now = time.time()
# with open("back_side.txt", "w") as text_file:
# while (time.time() - now) < 5:
# frames = pipeline.wait_for_frames()