def CalculatePoseResults(self, pose, pose_gt):
errors = np.zeros(3)
matrix = np.array(t3fA2mat(pose)).reshape((4, 4))
rotation = matrix[:3, :3]
translation = matrix[:3, 3]
matrix_gt = np.array(t3fA2mat(pose_gt)).reshape((4, 4))
rotation_gt = matrix_gt[:3, :3]
translation_gt = matrix_gt[:3, 3]
translation_error = np.linalg.norm(translation - translation_gt)
rotation_error = math.acos(
np.clip((np.matmul(rotation.T, rotation_gt).trace() - 1) / 2, -1,
1))
tracking_loss = int(translation_error > 0.05
or rotation_error > 5 * math.pi / 180)
errors[0] = translation_error
errors[1] = rotation_error
errors[2] = tracking_loss
return errors
def run(self):
if self.evaluate:
self.poses_first_error = []
self.poses_second_error = []
self.ResetBody(0)
i_frame = 0
for region_modality in self.tracker.region_modalities:
region_modality.StartModality()
# Iterate over all frames
while True:
if not self.ExecuteMeasuredTrackingCycle(i_frame):
return
if self.evaluate:
for key, body in self.bodies.items():
if key == "squirrel_small":
self.poses_second_error.append(
self.CalculatePoseResults(
body.body2world_pose(),
self.poses_second[i_frame + 1]))
else:
self.poses_first_error.append(
self.CalculatePoseResults(
body.body2world_pose(),
self.poses_first[i_frame + 1]))
self.ResetBody(i_frame + 1)
if i_frame >= 999:
result = np.mean(self.poses_first_error, axis=0)
print({
"translation_loss": result[0],
"rotation_loss": result[1],
"success_rate": 1 - result[2]
})
break
for key, body in self.bodies.items():
body2world_pose = np.array(t3fA2mat(
body.body2world_pose())).reshape((4, 4))
self.output_lst.append({
"name": key,
"translation": body2world_pose[:3, 3],
"rotation": body2world_pose[:3, :3]
})
for region_modality in self.tracker.region_modalities:
region_modality.StartModality()
i_frame += 1