def processData(data):
if isinstance(data, Quaternion):
return data
elif isinstance(data, list) and len(data) == 4:
return Quaternion(data)
else:
return Quaternion()
async def nut(self):
my_qq = np.array([1, 0, 0, 0], dtype=np.float64)
my_off = Quaternion()
my_off2 = Quaternion.from_x_rotation(np.pi/2)*Quaternion.from_y_rotation(np.pi/2)
mig = ahrs.filters.Madgwick(frequency=100.0)
gg = []
while self.coro_keep_alive['nut'].is_alive:
try:
if self.last_read:
gg = u.get_numbers_from_text(self.last_read.decode().strip('\r'), ',')
self.last_read = b''
if len(gg) == 9 and len(self.poses) > 3:
# my_qq = mig.updateIMU(my_qq, gg[3:6], gg[6:])
my_qq = mig.updateMARG(my_qq, gg[3:6], gg[6:], gg[:3])
# print (my_qq)
temp = Quaternion(my_off*Quaternion([*my_qq[1:], my_qq[0]])*my_off2).normalised
self.poses[3].r_w = temp[3]
self.poses[3].r_y = temp[0]
self.poses[3].r_z = temp[1]
self.poses[3].r_x = temp[2]
if self._serialResetYaw:
my_off = (Quaternion([*my_qq[1:], my_qq[0]])*my_off2).inverse.normalised
mig = ahrs.filters.Madgwick(frequency=100.0)
except Exception as e:
print (f'nut: dead, reason: {e}')
# break
await asyncio.sleep(self.coro_keep_alive['nut'].sleep_delay)
async def serial_listener():
try:
irl_rot_off = Quaternion.from_x_rotation(
0
) # supply your own imu offsets here, has to be a Quaternion object
with serial.Serial(SERIAL_PORT, SERIAL_BAUD, timeout=1 / 4) as ser:
with serial.threaded.ReaderThread(
ser, u.SerialReaderFactory) as protocol:
protocol.write_line("nut")
await asyncio.sleep(5)
print('serial_listener: starting tracking thread')
while poser.coro_keep_alive["serial_listener"].is_alive:
gg = u.get_numbers_from_text(protocol.last_read, ",")
if len(gg) >= 4:
w, x, y, z, *_ = gg
my_q = Quaternion([-y, z, -x, w])
my_q = Quaternion(my_q * irl_rot_off).normalised
poser.poses[0].r_w = round(my_q[3], 5)
poser.poses[0].r_x = round(my_q[0], 5)
poser.poses[0].r_y = round(my_q[1], 5)
poser.poses[0].r_z = round(my_q[2], 5)
await asyncio.sleep(
poser.coro_keep_alive["serial_listener"].sleep_delay)
except Exception as e:
print(f'serial_listener: failed {e}')
poser.coro_keep_alive["serial_listener"].is_alive = False
def test_euler_to_quaternion_1(self):
v = np.radians(Vector3([0, 0, 0]))
self.assertQuaternionAreEqual(euler_angles_to_quaternion(v), (Quaternion([0, 0, 0, 1])))
v = np.radians([0, 0, 180])
self.assertQuaternionAreEqual(euler_angles_to_quaternion(v), (Quaternion([0, 0, 1, 0])))
v = np.radians([0, 180, 0])
self.assertQuaternionAreEqual(euler_angles_to_quaternion(v), (Quaternion([0, 1, 0, 0])))
v = np.radians([180, 0, 0])
self.assertQuaternionAreEqual(euler_angles_to_quaternion(v), (Quaternion([1, 0, 0, 0])))
def from_to_quaternion(v1, v2, is_unit=True, epsilon=1e-5):
# https://stackoverflow.com/a/11741520/782170
if not is_unit:
v1 = unit_vector(v1)
v2 = unit_vector(v2)
k_cos_theta = np.dot(v1, v2)
k = np.sqrt(np.dot(v1, v1) * np.dot(v2, v2))
if abs(k_cos_theta / k + 1) < epsilon:
# 180 degree rotation around any orthogonal vector
return Quaternion(0, unit_vector(perpendicular_vector(v1)))
q = Quaternion(k_cos_theta + k, *np.cross(v1, v2))
return q.normalize
def test_oo_examples(self):
from pyrr import Quaternion, Matrix44, Vector3
import numpy as np
point = Vector3([1., 2., 3.])
orientation = Quaternion()
translation = Vector3()
scale = Vector3([1., 1., 1.])
# translate along X by 1
translation += [1.0, 0.0, 0.0]
# rotate about Y by pi/2
rotation = Quaternion.from_y_rotation(np.pi / 2.0)
orientation = rotation * orientation
# create a matrix
# start our matrix off using the scale
matrix = Matrix44.from_scale(scale)
# apply our orientation
# we can multiply matricies and quaternions directly!
matrix = matrix * orientation
# apply our translation
translation = Matrix44.from_translation(translation)
matrix = matrix * translation
# transform our point by the matrix
# vectors are transformable by matrices and quaternions directly
point = matrix * point
def euler_angles_to_quaternion(eulers, dtype=None):
"""Creates a quaternion from a set of Euler angles.
Rotation order for euler angles is YZX.
See: http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
"""
dtype = dtype or eulers.dtype
heading_half = eulers[1] / 2.
attitude_half = eulers[2] / 2.
bank_half = eulers[0] / 2.
c1 = np.math.cos(heading_half)
s1 = np.math.sin(heading_half)
c2 = np.math.cos(attitude_half)
s2 = np.math.sin(attitude_half)
c3 = np.math.cos(bank_half)
s3 = np.math.sin(bank_half)
c1c2 = c1 * c2
s1s2 = s1 * s2
return Quaternion(
[
c1c2 * s3 + s1s2 * c3,
s1 * c2 * c3 + c1 * s2 * s3,
c1 * s2 * c3 - s1 * c2 * s3,
c1c2 * c3 - s1s2 * s3,
],
dtype=dtype
)
async def serial_listener3(self):
"""Get orientation data from serial."""
irl_rot_off = Quaternion.from_x_rotation(
np.pi /
3) # imu on this controller is rotated 90 degrees irl for me
my_off = Quaternion()
if not check_serial_dict(self.serialPaths, 'cont_r'):
print(
'failed to init serial_listener3: bad serial dict for key \'cont_r\''
)
self.coro_keep_alive["serial_listener3"][0] = False
return
with serial.Serial(self.serialPaths["cont_r"], 115200,
timeout=1 / 5) as ser2:
with serial.threaded.ReaderThread(
ser2, u.SerialReaderFactory) as protocol:
protocol.write_line("nut")
await asyncio.sleep(5)
while self.coro_keep_alive["serial_listener3"][0]:
try:
gg = u.get_numbers_from_text(protocol.last_read, ',')
if len(gg) > 0:
w, x, y, z = gg
my_q = Quaternion([-y, z, -x, w])
if self._serialResetYaw:
my_off = Quaternion([0, z, 0,
w]).inverse.normalised
my_q = my_off * my_q * irl_rot_off
self.pose_controller_r.r_w = round(my_q[3], 5)
self.pose_controller_r.r_x = round(my_q[0], 5)
self.pose_controller_r.r_y = round(my_q[1], 5)
self.pose_controller_r.r_z = round(my_q[2], 5)
await asyncio.sleep(
self.coro_keep_alive["serial_listener3"][1])
except Exception as e:
print(f"{self.serial_listener.__name__}: {e}")
break
self.coro_keep_alive["serial_listener3"][0] = False
def __compute_bbox_coords(self, mesh, view):
upright = Quaternion.from_y_rotation(np.pi / 2)
rotation = Quaternion(mesh['rotation'])
center = mesh['translation']
world_corners = {
'top_left':
Vector3([center[0], center[1], center[2]]) +
(rotation * upright *
Vector3([0, mesh['width'] / 2, mesh['height'] / 2])),
'top_right':
Vector3([center[0], center[1], center[2]]) +
(rotation * upright *
Vector3([0, -mesh['width'] / 2, mesh['height'] / 2])),
'bottom_right':
Vector3([center[0], center[1], center[2]]) +
(rotation * upright *
Vector3([0, -mesh['width'] / 2, -mesh['height'] / 2])),
'bottom_left':
Vector3([center[0], center[1], center[2]]) +
(rotation * upright *
Vector3([0, mesh['width'] / 2, -mesh['height'] / 2]))
}
hidden_corners = 0
left = right = top = bottom = None
for key, value in world_corners.items():
img_coords = self.__project_to_img_frame(value, view)
if (img_coords[0] < 10 or img_coords[0] > (self.width - 10)
or img_coords[1] < 10 or img_coords[1] >
(self.height - 10)):
hidden_corners += 1
if left is None or (img_coords[0] < left['x']):
left = {'x': img_coords[0], 'y': img_coords[1]}
if top is None or (img_coords[1] < top['y']):
top = {'x': img_coords[0], 'y': img_coords[1]}
if bottom is None or (img_coords[1] > bottom['y']):
bottom = {'x': img_coords[0], 'y': img_coords[1]}
if right is None or (img_coords[0] > right['x']):
right = {'x': img_coords[0], 'y': img_coords[1]}
image_corners = {
'min': [int(left['x']), int(top['y'])],
'max': [int(right['x']), int(bottom['y'])]
}
if hidden_corners > 3:
return {}
elif hidden_corners > 0:
for key, img_coords in image_corners.items():
for i in range(0, 2):
if img_coords[i] < 0:
img_coords[i] = 0
elif img_coords[i] > (self.width
if i == 0 else self.height):
img_coords[i] = self.width if i == 0 else self.height
image_corners[key] = img_coords
return image_corners
def buildBoneMatrices(tree, parentmat):
trans = tree['transform']
translate = Matrix44.from_translation(trans['position'])
rotate = Quaternion(trans['rotation']).matrix44
mat = mats[tree['bone']] = rotate * translate * parentmat
for x in tree['children']:
buildBoneMatrices(x, mat)
def test_conversions(self):
from pyrr import Quaternion, Matrix33, Matrix44, Vector3, Vector4
v3 = Vector3([1., 0., 0.])
v4 = Vector4.from_vector3(v3, w=1.0)
v3, w = Vector3.from_vector4(v4)
m44 = Matrix44()
q = Quaternion(m44)
m33 = Matrix33(q)
m33 = Matrix44().matrix33
m44 = Matrix33().matrix44
q = Matrix44().quaternion
q = Matrix33().quaternion
m33 = Quaternion().matrix33
m44 = Quaternion().matrix44
def __init__(self):
self.location = Vector3()
self.scale = Vector3([1, 1, 1])
self.rotation = Rotator([0.0, 0.0, 0.0])
self.quaternion = Quaternion([0, 0, 0, 1])
self.initial_matrix = Matrix44.identity()
self.transform_matrix = Matrix44.identity()
# Flag indicate whether the transformation is modified or not
self.is_changed = False
def setData(self, data):
if isinstance(data, Quaternion):
self._data = data
elif isinstance(data, list) and len(data) == 4:
# here serialized data will be handled
# when node desirializes itself, it creates all pins
# and then sets data to them. Here, data will be set fo the first time after deserialization
self._data = Quaternion(data)
else:
self._data = self.defaultValue()
PinWidgetBase.setData(self, self._data)
def test_quaternion_matrix_conversion(self):
# https://au.mathworks.com/help/robotics/ref/quat2rotm.html?requestedDomain=www.mathworks.com
q = Quaternion([0.7071, 0., 0., 0.7071])
m33 = q.matrix33
expected = np.array([
[1., 0., 0.],
[0., -0., -1.],
[0., 1., -0.],
])
self.assertTrue(np.allclose(m33, expected))
# issue #42
q = Quaternion([0.80087974, 0.03166748, 0.59114721, -0.09018753])
m33 = q.matrix33
q2 = Quaternion.from_matrix(m33)
print(q, q2)
self.assertTrue(np.allclose(q, q2))
q3 = Quaternion.from_matrix(m33.T)
self.assertFalse(np.allclose(q2, q3))
def __init__(self, name: str = "New Node"):
""" Node element of scene graph (tree structure).
Args:
name: Name for string representation.
"""
self.children = list()
self._parent = None
self.name = name
self._local_position = Vector3()
self._world_position = Vector3()
self._scale = Vector3([1., 1., 1.])
self._local_quaternion = Quaternion()
self._world_quaternion = Quaternion()
self._world_matrix = Matrix44.identity()
self._local_matrix = Matrix44.identity()
self.__matrix_needs_update = True
def parse_annotations(self, path: str):
if not os.path.isfile(path):
raise Exception("Annotations file not found")
annotations = dict()
with open(path) as file:
file.readline() # Discard the header
for line in file:
items = line.split(',')
annotations[items[0].strip()] = BackgroundAnnotations(
Vector3([float(x) for x in items[1:4]]),
Quaternion([float(x) for x in items[4:8]]))
return annotations
def bounce(self, dist, norm):
if dist > self.r:
return False
norm = numpy.array(norm)
rot = self.rotation
rot_mat = rot.matrix33
low_pt = None
for xp in -1, 1:
for yp in -1, 1:
for zp in -1, 1:
point = rot_mat * Vector3([xp, yp, zp])
if low_pt is None or point[2] < low_pt[2]:
low_pt = point
if low_pt[2] >= dist:
return False
#play_sound('die-hit')
self.locked = False
rot_imp = Vector3(norm).cross(Vector3(
(*low_pt.xy, 0.0))) * (.5 + hypot(*self.speed) / 10)
riq = Quaternion.from_axis(rot_imp)
ump = -Vector3(self.speed).cross(norm)
umpq = Quaternion.from_axis(ump)
if np.isnan(umpq).any():
umpq = Quaternion()
self.rotation_speed = Quaternion().lerp(
umpq * riq * self.rotation_speed, 0.7)
self.speed *= DAMPING_BOUNCE
# Reflect speed
if linalg_norm(norm) >= 2:
1 / 0
s = self.speed
self.speed = s - 2 * s.dot(norm) * norm
self.pos += (self.r - dist) * norm
return True
def test_operators(self):
from pyrr import Quaternion, Matrix44, Matrix33, Vector3, Vector4
import numpy as np
# matrix multiplication
m = Matrix44() * Matrix33()
m = Matrix44() * Quaternion()
m = Matrix33() * Quaternion()
# matrix inverse
m = ~Matrix44.from_x_rotation(np.pi)
# quaternion multiplication
q = Quaternion() * Quaternion()
q = Quaternion() * Matrix44()
q = Quaternion() * Matrix33()
# quaternion inverse (conjugate)
q = ~Quaternion()
# quaternion dot product
d = Quaternion() | Quaternion()
# vector oprations
v = Vector3() + Vector3()
v = Vector4() - Vector4()
# vector transform
v = Quaternion() * Vector3()
v = Matrix44() * Vector3()
v = Matrix44() * Vector4()
v = Matrix33() * Vector3()
# dot and cross products
dot = Vector3() | Vector3()
cross = Vector3() ^ Vector3()
def update_transform_matrix(self):
scale_matrix = Matrix44.from_scale(self.scale)
translation_matrix = Matrix44.from_translation(self.location)
# TODO: For some reason the qx, qy, qz part of the quaternion must be flipped
# Need to understand why and fix it
# The change need to be made together with the coordinate conversion in NDDS
# rotation_matrix = Matrix44.from_quaternion(self.quaternion)
qx, qy, qz, qw = self.quaternion
test_quaternion = Quaternion([-qx, -qy, -qz, qw])
rotation_matrix = Matrix44.from_quaternion(test_quaternion)
# print('update_transform_matrix: rotation_matrix = {}'.format(rotation_matrix))
relative_matrix = (translation_matrix * scale_matrix * rotation_matrix)
# self.transform_matrix = relative_matrix * self.initial_matrix
self.transform_matrix = relative_matrix
def __generate(self, index):
baseImage = self.base_dataset.get()
annotations = []
for gate in self.gates_config['gates']:
view = self.__compute_view_matrix(baseImage.annotations)
coords = self.__compute_bbox_coords(gate, view)
if coords != {}:
distance = self.__compute_camera_proximity(
gate, baseImage.annotations)
camera_yaw = self.__euler_yaw(
baseImage.annotations.orientation)
gate_yaw = self.__euler_yaw(Quaternion(gate['rotation']))
rotation = camera_yaw - gate_yaw
bbox = {
'class_id': 1,
'min': [coords['min'][0], coords['min'][1]],
'max': [coords['max'][0], coords['max'][1]],
'distance': distance,
'rotation': rotation
}
annotations.append(bbox)
self.annotated_dataset.put(
AnnotatedImage(baseImage.image_path(), index, annotations))
def convert_tf_to_transform(self, transform_stamped):
position_vector = Vector4()
position_vector.x = transform_stamped.transform.translation.x
position_vector.y = transform_stamped.transform.translation.y
position_vector.z = transform_stamped.transform.translation.z
position_vector.w = 0.
rotation_quaterniond = Quaternion()
rotation_quaterniond.w = transform_stamped.transform.rotation.w
rotation_quaterniond.x = transform_stamped.transform.rotation.x
rotation_quaterniond.y = transform_stamped.transform.rotation.y
rotation_quaterniond.z = transform_stamped.transform.rotation.z
scale = Vector4([1., 1., 1., 1.])
affine3d = Affine3d(position_vector, rotation_quaterniond, scale)
transform = Transform(affine3d,
transform_stamped.header.frame_id,
transform_stamped.child_frame_id,
transform_stamped.header.stamp.to_time(),
authority="")
return transform
def test_rotate_z_1(self):
self.root.rotate_z(180)
self.assertQuaternionAreEqual(self.root.local_quaternion,
Quaternion([0, 0, 1, 0]))
def get_orientation(self, convergence_acc: float, overshoot_acc: float, convergence_mag: float,
overshoot_mag: float):
# https://www.sciencedirect.com/science/article/pii/S2405896317321201
if self.prev_up is not None:
t2 = time.time()
k_acc, k_bias_acc = get_ki_kbias(convergence_acc, overshoot_acc, t2 - self.prev_orientation_time)
k_mag, k_bias_mag = get_ki_kbias(convergence_mag, overshoot_mag, t2 - self.prev_orientation_time)
grav = np.asarray(unit_vector(self.get_grav()))
mag = unit_vector(np.asarray(self.get_mag()))
east = unit_vector(np.cross(mag, grav))
west = -east
north = -unit_vector(np.cross(east, grav))
up = -unit_vector(np.cross(north, east))
gyro = -self.get_gyro()
if self.prev_bias is not None:
if np.isnan(self.prev_bias).any():
self.prev_bias = None
else:
gyro += self.prev_bias
gy = Quaternion.from_eulers(gyro * (t2 - self.prev_orientation_time))
pred_west = (~gy * Quaternion(np.pad(self.prev_west, (0,1), 'constant', constant_values=0)) * gy)
pred_up = (~gy * Quaternion(np.pad(self.prev_up, (0,1), 'constant', constant_values=0)) * gy)
pred_north = (~gy * Quaternion(np.pad(self.prev_north, (0,1), 'constant', constant_values=0)) * gy)
pred_north_v = np.asarray(pred_north.xyz)
pred_west_v = np.asarray(pred_west.xyz)
pred_up_v = np.asarray(pred_up.xyz)
fix_north = Quaternion.from_axis(np.cross(north, pred_north_v) * k_mag)
fix_west = Quaternion.from_axis(np.cross(west, pred_west_v) * k_mag)
fix_up = Quaternion.from_axis(np.cross(up, pred_west_v) * k_acc)
x_acc = np.cross(up, pred_up_v)
x_mag = np.cross(north, pred_north_v)
pb0 = k_bias_acc*x_acc + k_bias_mag*x_mag
if self.prev_bias is not None:
self.prev_bias = self.prev_bias * (t2 - self.prev_orientation_time) + pb0
else:
self.prev_bias = pb0
true_north = unit_vector((pred_north * fix_north).xyz)
true_up = unit_vector((pred_up * fix_up).xyz)
true_west = unit_vector((pred_west * fix_west).xyz)
rot = np.asarray([true_north, true_west, true_up]).transpose()
q = Quaternion.from_matrix(rot)
if not np.isnan(north).any():
self.prev_north = north
if not np.isnan(west).any():
self.prev_west = west
if not np.isnan(up).any():
self.prev_up = up
self.prev_orientation_time = t2
return q
else:
grav = np.asarray(unit_vector(self.get_grav()))
mag = unit_vector(np.asarray(self.get_mag()))
east = unit_vector(np.cross(mag, grav))
north = -unit_vector(np.cross(east, grav))
up = -unit_vector(np.cross(north, east))
rot = np.asarray([north, -east, up]).transpose()
q = Quaternion.from_matrix(rot)
if (not np.isnan(grav).any()) and (not np.isnan(mag).any()):
self.prev_west = -east
self.prev_north = north
self.prev_up = up
self.prev_orientation_time = time.time()
return q
def reset_transform(self):
self.set_location(Vector3())
# self.set_rotation(euler.create(0.0, 0.0, 0.0))
self.set_quaternion(Quaternion([0, 0, 0, 1]))
self.mark_changed()
def test_euler_1(self):
self.root.local_euler_angles = Vector3([180, 0, 0])
self.assertQuaternionAreEqual(self.root.local_quaternion,
Quaternion([1, 0, 0, 0]))
def test_rotate_z_4(self):
self.root.rotate_z(180, local_space=False)
self.assertQuaternionAreEqual(self.root.local_quaternion,
Quaternion([0, 0, 1, 0]))
self.assertQuaternionAreEqual(self.root.world_quaternion,
Quaternion([0, 0, 1, 0]))
def test_rotate_z_3(self):
self.root.rotate_z(720)
self.assertQuaternionAreEqual(self.root.local_quaternion,
Quaternion([0, 0, 0, 1]))
def test_rotate_y_2(self):
self.root.rotate_y(360)
self.assertQuaternionAreEqual(self.root.local_quaternion,
Quaternion([0, 0, 0, 1]))
def test_quaternion_4(self):
self.root.world_quaternion = [1, 0, 0, 0]
self.child_2.local_quaternion = [1, 0, 0, 0]
self.assertQuaternionAreEqual(self.child_2.world_quaternion,
Quaternion([0, 0, 0, 1]))
def predict_eval_and_save_external(steps, hparams, sync_replicas, input_fn, external_options):
object_to_test = external_options["obj_name"]
out_folder = external_options["out_folder"]
ckpt_path = external_options["ckpt_path"]
dataset_folder = external_options["dataset_folder"]
mesh_folder = external_options["mesh_folder"]
run_config = tf.estimator.RunConfig(
keep_checkpoint_every_n_hours=0.5,
save_checkpoints_secs=180,
save_summary_steps=50)
estimator = tf.estimator.Estimator(
model_fn=estimator_model_fn,
params=hparams, config=run_config)
print("loading model: ", ckpt_path)
input_fn = input_fn(dataset=object_to_test, folder_tfrecord=dataset_folder, mesh_folder=mesh_folder, batch_size=1)
predictions = estimator.predict(input_fn=input_fn, checkpoint_path=ckpt_path)
pose_pred = []
pose_gt = []
if not os.path.exists(out_folder):
os.mkdir(out_folder)
out_folder_object = os.path.join(out_folder, object_to_test)
if not os.path.exists(out_folder_object):
os.mkdir(out_folder_object)
it_img = 0
for item in predictions:
pos_gt = item["pos_gt"]
quat_gt = item["quat_gt"]
pos_init = item["pos_init"]
quat_init = item["quat_init"]
xy_max_ind = item["xy_out"]
z_max_att = item["z_out"]
xy_image_delta = xy_max_ind[:2]
xy_image = item["im_center"] + xy_image_delta / item["image_scaling"]
xy_image = np.squeeze(xy_image)
z_model = pos_init[2]
cam_mat = item["cam_mat"]
x_model_cam_frame = (xy_image[0] - cam_mat[0, 2]) / cam_mat[0, 0]
x_model_cam_frame = x_model_cam_frame * z_model
y_model_cam_frame = (xy_image[1] - cam_mat[1, 2]) / cam_mat[1, 1]
y_model_cam_frame = y_model_cam_frame * z_model
pose_model_lateral = np.array([x_model_cam_frame, y_model_cam_frame, z_model])
pos_max_att = np.exp(z_max_att) * pose_model_lateral
quat_init_q = Quaternion(quat_init)
R_init = np.array(quat_init_q.matrix33)
quat_out_q = Quaternion(item["quat_out"])
R_out = np.array(quat_out_q.matrix33)
pose_pred_it = np.zeros((3, 4))
pose_pred_it[:, 3] = pos_max_att
R_pred = np.matmul(R_out, R_init)
pose_pred_it[:3, :3] = R_pred
pose_pred.append(pose_pred_it)
pose_gt_it = np.zeros((3, 4))
pose_gt_it[:, 3] = pos_gt
quat_gt = Quaternion(quat_gt)
R_gt = np.array(quat_gt.matrix33)
pose_gt_it[:3, :3] = R_gt
pose_gt.append(pose_gt_it)
if it_img % 100 == 0:
print(it_img)
object_to_test = "%06d" % it_img
out_file = os.path.join(out_folder_object, object_to_test + "_predict.npy")
out_dict = {
'pose_pred': pose_pred_it,
'pose_gt': pose_gt_it,
}
np.save(out_file, out_dict)
it_img += 1
