def apply_transformation(self, M):
"""Apply the transformation matrix to all position vectors."""
super().apply_transformation(M)
translation, rotation, zoom, shear = affines.decompose(M)
transform = affines.compose(np.zeros(3), rotation, zoom, shear)[:3, :3]
self._patch_orient = np.dot(transform, self._patch_orient)
def move_first(self, displacement = [0,0,0]): currentPosition, currentRotation, currentZoom, currentShear = decompose(self.first.pos4d) position = np.array(currentPosition) + np.array(displacement) matrix = compose( position, currentRotation, [1,1,1], [0,0,0]) self.first.offset_apparatus(matrix)
def __init__(self, name, body_id, link_id, link_origin, mesh_path,
mesh_scale):
self.body_id = body_id
self.link_id = link_id
self.mesh_path = mesh_path
self.mesh_scale = mesh_scale
decomposed_origin = decompose(link_origin)
orn = mat2quat(decomposed_origin[1])
orn = [orn[1], orn[2], orn[3], orn[0]]
self.link_pose = [decomposed_origin[0], orn]
self.name = name
def offset_angle(self, angle, axis = [1,0,0]): self.angleOffset = angle axis = np.array(axis) Rotation = axangle2mat(axis, angle) currentPosition, currentRotation, currentZoom, currentShear = decompose(self.first.pos4d) matrix = compose( currentPosition ,Rotation,[1,1,1],[0,0,0]) self.first.offset_apparatus(matrix)
def test_parallel_calculated_rotations():
'''Regression test #164: An index mix-up in calculate_elempos introduced
unintended zoom and shear in the elements.
'''
t = np.array([[0,0,0, 1]])
obj = ParallelCalculated(pos_spec=t,
normal_spec=np.array([0, 0, 1, 1]),
parallel_spec=np.array([.3, .4, .5, 1]),
elem_class=CCD)
trans, rot, zoom, shear = decompose(obj.elem_pos[0])
assert np.allclose(t[0, :3], trans)
assert np.allclose(zoom, 1.)
assert np.allclose(shear, 0.)
def mat2pose(pose_mat, axes="sxyz"):
"""
Convert transformation matrix to vector of position and euler angles
:param pose_mat: 4x4 transformation matrix
:param axes: rotation axis, default to 'sxyz'
:return: pose vector : [x, y, z, q_w, q_x, q_y, q_z]
"""
# if not isinstance(type(pose_mat), np.mat):
# pose_mat = np.mat(pose_mat)
t, r, z, s = decompose(pose_mat)
pos = t
rot_euler = mat2euler(r, axes=axes)
rot_euler = np.array(rot_euler)
return pos, rot_euler
def vec_from_mat(pose_mat):
"""
Convert 4x4 transformation matrix in np.mat to 7D pose vector
:param pose_mat: 4x4 transformation matrix
:return: [x, y, z, q_w, q_x, q_y, q_z]
"""
# if not isinstance(type(pose_mat), np.mat):
# pose_mat = np.mat(pose_mat)
t, r, z, s = decompose(pose_mat)
pos = t
quat = mat2quat(r)
quat = np.array(quat)
pos = pos.tolist()
quat = quat.tolist()
pos.extend(quat)
return np.array(pos)
def bend_gratings(gratings, r_elem=1664):
'''Bend gratings in a gas to follow the Rowland cirle
Gratings are bend in one direction (the dispersion direction) only.
Assumes that the focal point is at the origin of the coordinate system!
Parameters
----------
gratings : list
List of gratings to be bend
'''
for e in gratings:
t, rot, z, s = decompose(e.geometry.pos4d)
d_phi = np.arctan(z[1] / r_elem)
c = Cylinder({
'position': t - r_elem * h2e(e.geometry['e_x']),
'orientation': rot,
'zoom': [r_elem, r_elem, z[2]],
'phi_lim': [-d_phi, d_phi]
})
c._geometry = e.geometry._geometry
e.geometry = c
e.display['shape'] = 'surface'
for e1 in e.elements:
# can't be the same geometry, because groove_angle is part of _geometry and that's different
# Maybe need to get that out again and make the geometry strictly the geometry
# But for now, make a new cylinder of each of them
# Even now, not sure that's needed, since intersect it run by FlatStack
c = Cylinder({
'position': t - r_elem * h2e(e.geometry['e_x']),
'orientation': rot,
'zoom': [r_elem, r_elem, z[2]],
'phi_lim': [-d_phi, d_phi]
})
c._geometry = e1.geometry._geometry
e1.geometry = c
e1.display['shape'] = 'surface'
def apply_translate(affine, offset):
T, R, Z, S = decompose(affine)
T = T + np.array(offset)
return compose(T, R, Z, S)
def apply_scale(affine, scale):
T, R, Z, S = decompose(affine)
Z = Z * np.array(scale)
return compose(T, R, Z, S)
def create_volume_prop(volume: np.ndarray,
affine: np.ndarray) -> vtk.vtkVolume:
"""
Convert a numpy 3D matrix to a vtkVolume object
:param volume:
:param affine:
:return:
"""
volume = volume / volume.max() * 255
volume = volume.astype('uint8')
dims = volume.shape
maxValue = volume.max()
minValue = volume.min()
dataImporter = vtk.vtkImageImport()
data_string = volume.tostring()
dataImporter.CopyImportVoidPointer(data_string, len(data_string))
dataImporter.SetDataScalarTypeToUnsignedChar()
dataImporter.SetNumberOfScalarComponents(1)
# todo ?????????????????
# set data spacing
_, _, spacing, _ = decompose(affine)
z = abs(spacing)
dataImporter.SetDataSpacing(z[2], z[0], z[1])
# dataImporter.SetDataSpacing(2.4, 0.7, 0.7)
dataImporter.SetDataExtent(0, dims[2] - 1, 0, dims[1] - 1, 0,
dims[0] - 1)
dataImporter.SetWholeExtent(0, dims[2] - 1, 0, dims[1] - 1, 0,
dims[0] - 1)
try:
# Compute by GPU
volume_mapper = vtk.vtkGPUVolumeRayCastMapper()
except Exception as e:
logging.warning("Failed to connect to GPU, render with CPU")
# Compute by CPU
volume_mapper = vtk.vtkFixedPointVolumeRayCastMapper()
out_port: vtk.vtkAlgorithmOutput = dataImporter.GetOutputPort()
volume_mapper.SetInputConnection(out_port)
# The color transfer function maps voxel intensities to colors.
# It is modality-specific, and often anatomy-specific as well.
# The goal is to one color for flesh (between 500 and 1000)
# and another color for bone (1150 and over).
colorLUT = np.arange(minValue, maxValue, maxValue / 5.0)
print(colorLUT)
volumeColor = vtk.vtkColorTransferFunction()
volumeColor.AddRGBPoint(colorLUT[0], 0.0, 0.0, 0.0)
volumeColor.AddRGBPoint(colorLUT[1], 1.0, 0.5, 0.3)
volumeColor.AddRGBPoint(colorLUT[2], 1.0, 0.5, 0.3)
volumeColor.AddRGBPoint(colorLUT[3], 1.0, 1.0, 0.9)
# The opacity transfer function is used to control the opacity
# of different tissue types.
volumeScalarOpacity = vtk.vtkPiecewiseFunction()
volumeScalarOpacity.AddPoint(colorLUT[0], 0.00)
volumeScalarOpacity.AddPoint(colorLUT[1], 0.15)
volumeScalarOpacity.AddPoint(colorLUT[2], 0.5)
volumeScalarOpacity.AddPoint(colorLUT[3], 0.85)
# The gradient opacity function is used to decrease the opacity
# in the "flat" regions of the volume while maintaining the opacity
# at the boundaries between tissue types. The gradient is measured
# as the amount by which the intensity changes over unit distance.
# For most medical data, the unit distance is 1mm.
volumeGradientOpacity = vtk.vtkPiecewiseFunction()
volumeGradientOpacity.AddPoint(0, 0.0)
volumeGradientOpacity.AddPoint(colorLUT[1] / 2, 0.5)
volumeGradientOpacity.AddPoint(colorLUT[2] / 2, 1.0)
volumeProperty = vtk.vtkVolumeProperty()
volumeProperty.SetColor(volumeColor)
volumeProperty.SetScalarOpacity(volumeScalarOpacity)
volumeProperty.SetGradientOpacity(volumeGradientOpacity)
volumeProperty.SetInterpolationTypeToLinear()
volumeProperty.ShadeOn()
volumeProperty.SetAmbient(0.4)
volumeProperty.SetDiffuse(0.6)
volumeProperty.SetSpecular(0.2)
# The vtkVolume is a vtkProp3D (like a vtkActor) and controls the position
# and orientation of the volume in world coordinates.
actorVolume = vtk.vtkVolume()
actorVolume.SetMapper(volume_mapper)
actorVolume.SetProperty(volumeProperty)
return actorVolume
def publish(self, dt):
if self.robot.active:
msg = TeamData()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "map"
msg.robot_id = self.robot.robot_id
msg.robot_position.pose = self.robot.pose
msg.robot_position.covariance = [self.robot.covariance, 0, 0, 0, 0, 0,
0, self.robot.covariance, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, self.robot.covariance]
if self.robot.ball.active:
try:
# beware, we use transforms3d here which has quaternion in different format than ros
robot_xyzw = self.robot.pose.orientation
robot_mat_in_world = quat2mat((robot_xyzw.w, robot_xyzw.x, robot_xyzw.y, robot_xyzw.z))
robot_trans_in_world = compose(
(self.robot.pose.position.x, self.robot.pose.position.y, self.robot.pose.position.z),
robot_mat_in_world, [1, 1, 1])
world_trans_in_robot = np.linalg.inv(robot_trans_in_world)
ball_xyzw = self.robot.ball.pose.orientation
mat_in_world = quat2mat((ball_xyzw.w, ball_xyzw.x, ball_xyzw.y, ball_xyzw.z))
trans_in_world = compose((self.robot.ball.pose.position.x, self.robot.ball.pose.position.y,
self.robot.ball.pose.position.z),
mat_in_world, [1, 1, 1])
trans_in_robot = np.matmul(world_trans_in_robot, trans_in_world)
pos_in_robot, mat_in_robot, _, _ = decompose(trans_in_robot)
ball_relative = PoseWithCovariance()
ball_relative.pose.position = Point(*pos_in_robot)
ball_absolute = PoseWithCovariance()
ball_absolute.pose.position = self.robot.ball.pose.position
ball_absolute.pose.orientation = Quaternion(0, 0, 0, 1)
ball_absolute.covariance = [self.robot.ball.covariance, 0, 0, 0, 0, 0,
0, self.robot.ball.covariance, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, self.robot.ball.covariance]
msg.ball_absolute = ball_absolute
cartesian_distance = math.sqrt(
ball_relative.pose.position.x ** 2 + ball_relative.pose.position.y ** 2)
msg.time_to_position_at_ball = cartesian_distance / ROBOT_SPEED
except tf2_ros.LookupException as ex:
rospy.logwarn_throttle(10.0, rospy.get_name() + ": " + str(ex))
return None
except tf2_ros.ExtrapolationException as ex:
rospy.logwarn_throttle(10.0, rospy.get_name() + ": " + str(ex))
return None
self.pub.publish(msg)
else:
# ball not seen
msg.ball_relative.covariance = 100.0
msg.time_to_position_at_ball = 0.0
self.pub.publish(msg)
from transforms3d.affines import compose, decompose, decompose44
# 平移矩阵
T = [20, 20, 30]
# 旋转矩阵
R = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
# 缩放矩阵
Z = [2.0, 3.0, 4.0]
A = compose(T, R, Z)
print("组成的变换矩阵是：")
print(A)
T1, R1, Z1, S1 = decompose(A)
print("组成的平移矩阵是：")
print(T1)
print("组成的旋转矩阵是：")
print(R1)
print("组成的缩放矩阵是：")
print(Z1)
print("组成的。。矩阵是：")
print(S1)
def test_decompose_shears():
# Check that zeros shears are also returned
T, R, Z, S = decompose(np.eye(4))
assert_array_equal(S, np.zeros(3))
