def test_it_should_rotate_from_other_transform(self):
transform = Transform.from_parameters(0, 0, 0, 1, 0.707, 3.1)
rotation = Transform.from_parameters(0, 0, 0, -1, -0.707, -3.1)
transform.rotate(transform=rotation)
truth = np.array([[0.39, 0.5479, 0.74, 0.],
[0.9196, -0.2729, -0.2826, 0.],
[0.0471, 0.7908, -0.6103, 0.],
[0., 0., 0., 1.]])
np.testing.assert_almost_equal(transform.matrix, truth, 4)
def test_it_should_translate_from_other_transform(self):
transform = Transform.from_parameters(10, 1, 2.2, 1, 0.707, 3.1)
translation = Transform.from_parameters(10, -1, 2.2, 0, 0, 0)
transform.translate(transform=translation)
truth = np.array([[-0.7597, -0.0316, 0.6496, 20],
[-0.5236, -0.5626, -0.6398, 0],
[0.3856, -0.8262, 0.4108, 4.4],
[0, 0, 0, 1]])
np.testing.assert_almost_equal(transform.matrix, truth, 4)
def estimate_current_pose(self, previous_pose, current_rgb, current_depth, debug=False):
render_rgb, render_depth = self.renderer.render(previous_pose.transpose())
# todo implement this part on gpu...
rgbA, depthA = normalize_scale(render_rgb, render_depth, previous_pose.inverse(), self.camera, self.image_size,
self.object_width)
rgbB, depthB = normalize_scale(current_rgb, current_depth, previous_pose.inverse(), self.camera, self.image_size,
self.object_width)
depthA = normalize_depth(depthA, previous_pose)
depthB = normalize_depth(depthB, previous_pose)
if debug:
show_frames(rgbA, depthA, rgbB, depthB)
rgbA, depthA = normalize_channels(rgbA, depthA, self.mean[:4], self.std[:4])
rgbB, depthB = normalize_channels(rgbB, depthB, self.mean[4:], self.std[4:])
self.input_buffer[0, 0:3, :, :] = rgbA
self.input_buffer[0, 3, :, :] = depthA
self.input_buffer[0, 4:7, :, :] = rgbB
self.input_buffer[0, 7, :, :] = depthB
self.prior_buffer[0] = np.array(previous_pose.to_parameters(isQuaternion=True))
prediction = self.tracker_model.test([self.input_buffer, self.prior_buffer]).asNumpyTensor()
prediction = unnormalize_label(prediction, self.translation_range, self.rotation_range)
if debug:
print("Prediction : {}".format(prediction))
prediction = Transform.from_parameters(*prediction[0], is_degree=True)
current_pose = combine_view_transform(previous_pose, prediction)
self.debug_rgb = render_rgb
return current_pose
def test_it_should_init_from_parameters(self):
transform = Transform.from_parameters(10, 1, 2.2, 1, 0.707, 3.1)
truth = np.array([[-0.7597, -0.0316, 0.6496, 10],
[-0.5236, -0.5626, -0.6398, 1],
[0.3856, -0.8262, 0.4108, 2.2],
[0, 0, 0, 1]])
np.testing.assert_almost_equal(transform.matrix, truth, 4)
def __next__(self):
rgb, depth = self.sensor.get_frame()
rgb = cv2.resize(rgb,
(self.sensor.camera.width, self.sensor.camera.height))
depth = cv2.resize(
depth, (self.sensor.camera.width, self.sensor.camera.height))
frame = Frame(rgb, depth, self.count)
self.count += 1
pose = None
if self.do_compute:
pose = self.detector.detect(rgb.copy())
if pose is None:
pose = Transform.from_parameters(0, 0, -1, 0, 0, 0)
return frame, pose
def load(self):
"""
Load a viewpoints.json to dataset's structure
Todo: datastructure should be more similar to json structure...
:return: return false if the dataset is empty.
"""
# Load viewpoints file and camera file
try:
with open(os.path.join(self.path, "viewpoints.json")) as data_file:
data = json.load(data_file)
self.camera = Camera.load_from_json(self.path)
except FileNotFoundError:
return False
self.metadata = data["metaData"]
self.set_save_type(self.metadata["save_type"])
count = 0
# todo this is not clean!
while True:
try:
id = str(count)
pose = Transform.from_parameters(
*[float(data[id]["vector"][str(x)]) for x in range(6)])
self.add_pose(None, None, pose)
if "pairs" in data[id]:
for i in range(int(data[id]["pairs"])):
pair_id = "{}n{}".format(id, i)
pair_pose = Transform.from_parameters(*[
float(data[pair_id]["vector"][str(x)])
for x in range(6)
])
self.add_pair(None, None, pair_pose, count)
count += 1
except KeyError:
break
return True
def detect(self, img):
self.likelihood = self.detector.detect_mat(img)
detection = None
if self.likelihood > 0.1:
# get board and draw it
board = self.detector.getDetectedBoard()
rvec = board.Rvec.copy()
tvec = board.Tvec.copy()
matrix = cv2.Rodrigues(rvec)[0]
rodrigues = mat2euler(matrix)
detection = Transform.from_parameters(tvec[0], -tvec[1], -tvec[2],
rodrigues[0], -rodrigues[1],
-rodrigues[2])
return detection
metadata["min_radius"] = str(SPHERE_MIN_RADIUS)
metadata["max_radius"] = str(SPHERE_MAX_RADIUS)
for i in range(real_dataset.size()):
frame, pose = real_dataset.data_pose[i]
rgb_render, depth_render = vpRender.render(pose.transpose())
rgb, depth = frame.get_rgb_depth(real_dataset.path)
masked_rgb, masked_depth = mask_real_image(rgb, depth, depth_render)
for j in range(SAMPLE_QUANTITY):
rotated_rgb, rotated_depth, rotated_pose = random_z_rotation(
masked_rgb, masked_depth, pose, real_dataset.camera)
random_transform = Transform.random(
(-TRANSLATION_RANGE, TRANSLATION_RANGE),
(-ROTATION_RANGE, ROTATION_RANGE))
inverted_random_transform = Transform.from_parameters(
*(-random_transform.to_parameters()))
previous_pose = rotated_pose.copy()
previous_pose = combine_view_transform(previous_pose,
inverted_random_transform)
rgbA, depthA = vpRender.render(previous_pose.transpose())
bb = compute_2Dboundingbox(previous_pose,
real_dataset.camera,
OBJECT_WIDTH,
scale=(1000, -1000, -1000))
rgbA, depthA = normalize_scale(rgbA, depthA, bb,
real_dataset.camera, IMAGE_SIZE)
rgbB, depthB = normalize_scale(rotated_rgb, rotated_depth, bb,
real_dataset.camera, IMAGE_SIZE)
def test_two_similar_transform_should_be_equal(self):
transform1 = Transform.from_parameters(1.1, 1.2, 1.3, 1.1, 1.2, 1.3)
transform2 = Transform.from_parameters(1.10001, 1.2, 1.30001, 1.1, 1.200001, 1.3)
self.assertEqual(transform1, transform2)
def test_two_identity_should_be_equal(self):
transform1 = Transform()
transform2 = Transform()
self.assertEqual(transform1, transform2)
transform3 = Transform.from_parameters(1, 0, 0, 0, 0, 0)
self.assertNotEqual(transform1, transform3)
def estimate_current_pose(self,
previous_pose,
current_rgb,
current_depth,
debug=False,
debug_time=False):
if debug_time:
start_time = time.time()
bb = compute_2Dboundingbox(previous_pose,
self.camera,
self.object_width,
scale=(1000, 1000, -1000))
bb2 = compute_2Dboundingbox(previous_pose,
self.camera,
self.object_width,
scale=(1000, -1000, -1000))
if debug_time:
print("Compute BB : {}".format(time.time() - start_time))
start_time = time.time()
rgbA, depthA = self.compute_render(previous_pose, bb)
if debug_time:
print("Render : {}".format(time.time() - start_time))
start_time = time.time()
rgbB, depthB = normalize_scale(current_rgb, current_depth, bb2,
self.camera, self.image_size)
debug_info = (rgbA, bb2, np.hstack((rgbA, rgbB)))
rgbA = rgbA.astype(np.float)
rgbB = rgbB.astype(np.float)
depthA = depthA.astype(np.float)
depthB = depthB.astype(np.float)
depthA = normalize_depth(depthA, previous_pose)
depthB = normalize_depth(depthB, previous_pose)
if debug:
show_frames(rgbA, depthA, rgbB, depthB)
rgbA, depthA = normalize_channels(rgbA, depthA, self.mean[:4],
self.std[:4])
rgbB, depthB = normalize_channels(rgbB, depthB, self.mean[4:],
self.std[4:])
self.input_buffer[0, 0:3, :, :] = rgbA
self.input_buffer[0, 3, :, :] = depthA
self.input_buffer[0, 4:7, :, :] = rgbB
self.input_buffer[0, 7, :, :] = depthB
self.prior_buffer[0] = np.array(
previous_pose.to_parameters(isQuaternion=True))
if debug_time:
print("Normalize : {}".format(time.time() - start_time))
start_time = time.time()
prediction = self.tracker_model.test(
[self.input_buffer, self.prior_buffer]).asNumpyTensor()
if debug_time:
print("Network time : {}".format(time.time() - start_time))
prediction = unnormalize_label(prediction, self.translation_range,
self.rotation_range)
if debug:
print("Prediction : {}".format(prediction))
prediction = Transform.from_parameters(*prediction[0], is_degree=True)
current_pose = combine_view_transform(previous_pose, prediction)
return current_pose, debug_info