def rotate_pose(point, quat, theta, axis="x"):
# set quaternion to transforms3d format, i.e. w,x,y,z
quat_t3d = [quat[3], quat[0], quat[1], quat[2]]
rot_mat = quaternions.quat2mat(quat_t3d)
H1 = np.eye(4)
H1[:3,:3] = rot_mat
H1[:3,3] = point
H2 = np.eye(4)
if axis == "x":
rot_axis = np.array(eulerangles.x_rotation(theta))
elif axis == "y":
rot_axis = np.array(eulerangles.y_rotation(theta))
elif axis == "z":
rot_axis = np.array(eulerangles.z_rotation(theta))
print rot_axis H2[:3,:3] = rot_axis
H3 = np.dot(H1, H2)
point_out = H3[:3,3]
quat_t3dout = quaternions.mat2quat(H3[:3,:3])
# Turn to ROS format, i.e. x, y, z, w
quat_out = [quat_t3dout[1], quat_t3dout[2], quat_t3dout[3], quat_t3do
ut[0]]
rospy.loginfo("Rotated pose: ")
print point_out, quat_out
return point_out, quat_out
def directional_normalization(df):
rotation_vector = df[['Milliseconds', 'CompassBearing']]
# pd.options.mode.chained_assignment = None
sensors = [
'AccelerometerLinear', 'Accelerometer', 'Gravity', 'Compass',
'Gyroscope'
]
headers = df.columns
df_sensors = []
for sensor in sensors:
if headers.contains(sensor + 'X'):
df_sensors.append(sensor)
for row in range(0, len(df)):
compassbearing = rotation_vector.loc[row, 'CompassBearing']
axis = [0, 0, 1]
# rotation_matrix = transforms3d.axangles.axangle2mat(axis , np.radians(compassbearing))
rotation_matrix = np.array(eular.z_rotation(
np.radians(compassbearing))).astype(np.float64)
for sensor in df_sensors:
sensor_data = df.ix[row][[
sensor + 'X', sensor + 'Y', sensor + 'Z'
]]
normalized_row = np.matmul(rotation_matrix,
np.asarray(sensor_data))
df.ix[row,
[sensor + 'X', sensor + 'Y', sensor + 'Z']] = normalized_row
return df
def test_euler_rotations(self):
"""Compare rotations from fluxer and transforms3d"""
# Derive the rotation matrix applied in euler_rotate
# The transpose of a product of matrices is equal to the product of
# their transposes in reverse order... so reverse the order of
# multiplication, which obfuscates the meaning... The alternative
# is to have the rotation matrix pre-multiply column vectors, which
# is inconvenient. Either way at least one transpose is required
euler_mat0 = (euler_deriv.x_rotation(self.phi).T *
euler_deriv.y_rotation(self.theta).T *
euler_deriv.z_rotation(self.psi).T)
subs = dict(zip([self.phi, self.theta, self.psi],
*np.radians(self.euler_degs)))
M_zyx = np.array(euler_mat0.subs(subs)).astype(np.float)
npt.assert_array_almost_equal(np.dot(self.uvw, M_zyx),
self.uvw_rot)
xrot_active = flux.rotation_matrix(self.euler_degs[0, 0], 0, True)
yrot_active = flux.rotation_matrix(self.euler_degs[0, 1], 1, True)
zrot_active = flux.rotation_matrix(self.euler_degs[0, 2], 2, True)
euler_mat1 = np.dot(xrot_active, np.dot(yrot_active, zrot_active))
npt.assert_array_almost_equal(np.dot(self.uvw, euler_mat1),
self.uvw_rot)
# Try with passive rotation and transpose the resulting rotation
# matrix
xrot_passive = flux.rotation_matrix(self.euler_degs[0, 0], 0)
yrot_passive = flux.rotation_matrix(self.euler_degs[0, 1], 1)
zrot_passive = flux.rotation_matrix(self.euler_degs[0, 2], 2)
euler_mat2 = np.dot(zrot_passive,
np.dot(yrot_passive, xrot_passive))
npt.assert_array_almost_equal(np.dot(self.uvw, euler_mat2.T),
self.uvw_rot)
# Now looping through rows, mimicking the real case of changing
# euler angles. First show derivation:
uvw_rots = np.empty_like(self.uvw)
for i, v in enumerate(self.degs):
euler_mati = self.euler_mat_fun(v)
uvw_roti = np.dot(self.uvw[i], euler_mati)
uvw_rots[i] = uvw_roti
npt.assert_array_almost_equal(uvw_rots, self.uvw_rot)
def __init__(self,
Rvec=None,
Tvec=None,
esqns=None,
marker_id=None,
z_up=False):
super(ARMarker, self).__init__()
self.Rvec = Rvec
self.Tvec = Tvec[0]
self.esqn = esqns
self.esqns = esqns[0]
self.marker_id = marker_id
self.z_up = z_up
rot, _ = cv2.Rodrigues(self.Rvec)
self.M = np.array( # Is rotation of the frame, pertenece a Frame (self)
[[rot[0, 0], rot[0, 1], rot[0, 2]],
[rot[1, 0], rot[1, 1], rot[1, 2]],
[rot[2, 0], rot[2, 1], rot[2, 2]]])
self.p = np.array( # Traslation of the frame
[self.Tvec[[0]], self.Tvec[[1]], self.Tvec[[2]]])
if self.z_up:
self.M = self.M * tfeuler.x_rotation(-np.pi / 2.0)
self.M = self.M * tfeuler.z_rotation(-np.pi)
self.corners = []
for p in self.esqns:
self.corners.append(p)
self.radius = int(0.5 *
np.linalg.norm(self.corners[0] - self.corners[2]))
self.center = np.array([0.0, 0.0])
for p in self.corners:
self.center += p
self.center = self.center / 4
self.side_in_pixel = 0
for i in range(0, len(self.corners)):
i_next = (i + 1) % len(self.corners)
p1 = np.array([self.corners[i]])
p2 = np.array([self.corners[i_next]])
self.side_in_pixel += np.linalg.norm(p1 - p2)
self.side_in_pixel = self.side_in_pixel / float(len(self.corners))
def __init__(self, it_trainer, num_orientations=8):
self.ORIENTATIONS = num_orientations
self.it_trainer = it_trainer
orientations = [
x * (2 * math.pi / self.ORIENTATIONS)
for x in range(0, self.ORIENTATIONS)
]
agglomerated_pv_points = []
agglomerated_pv_vectors = []
agglomerated_pv_vdata = []
agglomerated_normals = []
self.trainer = it_trainer
self.sample_size = it_trainer.pv_points.shape[0]
pv_vdata = np.zeros((self.sample_size, 3), np.float64)
pv_vdata[:, 0:2] = np.hstack(
(it_trainer.pv_norms.reshape(-1, 1),
it_trainer.pv_mapped_norms.reshape(-1, 1)))
for angle in orientations:
rotation = z_rotation(angle)
agglomerated_pv_points.append(
np.dot(it_trainer.pv_points, rotation.T))
agglomerated_pv_vectors.append(
np.dot(it_trainer.pv_vectors, rotation.T))
agglomerated_pv_vdata.append(pv_vdata)
agglomerated_normals.append(
np.dot(it_trainer.normal_env, rotation.T))
self.agglomerated_pv_points = np.asarray(
agglomerated_pv_points).reshape(-1, 3)
self.agglomerated_pv_vectors = np.asarray(
agglomerated_pv_vectors).reshape(-1, 3)
self.agglomerated_pv_vdata = np.asarray(agglomerated_pv_vdata).reshape(
-1, 3)
self.agglomerated_normals = np.asarray(agglomerated_normals).reshape(
-1, 3)
tri_mesh_object_file = tester.objs_filenames[0]
influence_radius = tester.objs_influence_radios[0]
# visualizing
tri_mesh_obj = trimesh.load_mesh(tri_mesh_object_file, process=False)
idx_from = orientation * tester.num_pv
idx_to = idx_from + tester.num_pv
pv_begin = tester.compiled_pv_begin[idx_from:idx_to]
pv_direction = tester.compiled_pv_direction[idx_from:idx_to]
provenance_vectors = trimesh.load_path(
np.hstack((pv_begin, pv_begin + pv_direction)).reshape(-1, 2, 3))
pv_intersections = analyzer.calculated_pvs_intersection(0, orientation)
R = z_rotation(angle) # rotation matrix
Z = np.ones(3) # zooms
T = testing_point
A = compose(T, R, Z)
tri_mesh_obj.apply_transform(A)
tri_mesh_env.visual.face_colors = [100, 100, 100, 100]
tri_mesh_obj.visual.face_colors = [0, 255, 0, 100]
intersections = trimesh.points.PointCloud(pv_intersections,
color=[0, 255, 255, 255])
sphere = trimesh.primitives.Sphere(radius=influence_radius,
center=testing_point,
subdivisions=4)
sphere.visual.face_colors = [100, 0, 0, 20]
if image is not None:
original_image_copy = image.copy()
# detect the markers on the image
detections = marker.detect(image=image, markers_map=markers_map)
if marker.isDetected():
print(marker.center())
# ⬢⬢⬢⬢⬢➤ Image Area of Interest
for id in detections.keys():
image_area = ImageAreaOfInterest(
) # BUG TRANSFORMATION & COORDINATES
relative_tf_2d = tf.affines.compose([50, 50, 0],
tfeuler.z_rotation(
-np.pi / 2.0),
np.ones(3))
image_area.update(image, marker.marker_tf_2d[id],
relative_tf_2d)
image_area.setRectArea(height=300, width=300)
masked_image = image_area.getMaskedImage(
show_it=args.debug)
# cropped_image = image_area.getCroppedArea(show_it=args.debug) # BUG size.width<0 or size.height<0
# ⬢⬢⬢⬢⬢➤ Image Processing
# image_proc = cropped_image
# image_proc = ImageProcessing.grayScale(image_proc, filter_it=True, show_it=args.debug)
# image_proc = ImageProcessing.adaptiveThreshold(image_proc, show_it=args.debug)
# image_proc = ImageProcessing.saltAndPepper(image_proc, kerner_size=3, show_it=args.debug)
# image_proc = ImageProcessing.fixedThreshold(image_proc, th=200, show_it=args.debug)
agg = Agglomerator(trainer)
angle = (2 * math.pi / agg.ORIENTATIONS)
for ori in range(agg.ORIENTATIONS):
idx_from = ori * agg.sample_size
idx_to = idx_from + agg.sample_size
pv_origin = agg.agglomerated_pv_points[idx_from:idx_to]
pv_final = agg.agglomerated_pv_points[
idx_from:idx_to] + agg.agglomerated_pv_vectors[idx_from:idx_to]
reference_ori = np.array([[0, 0, 0]])
reference_fin = np.array([[0.5, 0, 0]])
R = z_rotation(angle * ori) # accumulated rotation in each iteration
Z = np.ones(3) # zooms
T = [0, 0, 0] # translation
A = compose(T, R, Z)
tri_mesh_obj.apply_transform(A)
tri_mesh_env.apply_transform(A)
reference_ori = np.dot(reference_ori,
R.T) # np.mat(reference_ori)*np.mat(R)
reference_fin = np.dot(reference_fin,
R.T) # np.mat(reference_ori)*np.mat(R)
# VISUALIZATION
tri_pv = trimesh.load_path(
np.hstack((pv_origin, pv_final)).reshape(-1, 2, 3))
tri_pv_origin = trimesh.points.PointCloud(pv_origin,