def optimizeSuperpixels(self, x):
x = x.reshape(6, ).copy()
base_frame = self.instance_frame
# x = np.multiply(x, self.gains)
frame = KDLFromArray(x, fmt=LineOptimizer.DEFAULT_FORMAT)
frame = base_frame * frame
matrix = KLDtoNumpyMatrix(frame)
R = matrix[:3, :3]
t = matrix[:3, 3] * 1000.0
ren_rgb = renderer.render(self.model,
self.im_size,
self.K,
R,
t,
mode='rgb',
surf_color=[0, 1.0, 0])
nonzeroindices = np.argwhere(ren_rgb > 0)
labels = set(ariadne.graph.labels[nonzeroindices[:, 0],
nonzeroindices[:, 1]])
# print("LABELS", )
# for index in nonzeroindices:
# label = ariadne.graph.labels[index[0], index[1]]
# labels[label] = True
zero = np.zeros(self.image.shape[:2])
output_image = self.output_image.copy()
for l in labels:
label_indices = np.argwhere(ariadne.graph.labels == l)
output_image[label_indices[:, 0],
label_indices[:,
1]] = np.array([255, 255,
255]).astype(np.uint8)
zero[label_indices[:, 0],
label_indices[:, 1]] = np.array([255]).astype(np.uint8)
output_image[nonzeroindices[:, 0],
nonzeroindices[:, 1]] = np.array([255, 0,
255]).astype(np.uint8)
zero[nonzeroindices[:, 0], nonzeroindices[:, 1]] = 0
# cv2.imshow("model", ren_rgb)
# cv2.imshow("image", output_image)
# cv2.imshow("zero", zero)
# c = cv2.waitKey(1)
count = np.count_nonzero(zero.ravel())
if count == 0:
count = np.inf
# print("CURENT X", x, count)
return count
def optimizeRGBDifference(self, x):
x = x.reshape(6,).copy()
base_frame = self.instance_frame
x = np.multiply(x, self.gains)
if np.linalg.norm(x[:3]) > 0.2:
return np.inf
# x[3:] = x[3:]*180/math.pi
frame = KDLFromArray(x, fmt=LineOptimizer.DEFAULT_FORMAT)
frame = base_frame*frame
matrix = KLDtoNumpyMatrix(frame)
R = matrix[:3, :3]
t = matrix[:3, 3]*1000.0
ren_rgb = renderer.render(
self.model,
self.img_size,
self.K,
R, t, mode='rgb') # , surf_color=(76.0/255.0, 72.0/255.0, 82.0/255.0))
gray = self.gray
rgb_lines = lsd.detect(gray)[0]
model_lines = lsd.detect(cv2.cvtColor(ren_rgb, cv2.COLOR_BGR2GRAY))[0]
zeros = np.zeros(self.gray.shape)
rgb_lines_img = self.drawSegments(zeros, rgb_lines)
model_lines_img = self.drawSegments(zeros, model_lines)
ren_rgb = cv2.cvtColor(ren_rgb, cv2.COLOR_BGR2GRAY)
rendered_image = gray.copy()
rendered_image[ren_rgb != 0] = ren_rgb[ren_rgb != 0]
grayf = gray.astype(float)
rendered_imagef = rendered_image.astype(float)
diff = np.abs(grayf - rendered_imagef)/255.0
# rgb_lines_img = lsd.drawSegments(zeros, rgb_lines)
# model_lines_img = lsd.drawSegments(zeros, model_lines)
count = np.sum(diff)
print("MAXMIN", np.min(diff), np.max(diff))
cv2.imshow("model", rendered_image)
cv2.imshow("diff", diff)
cv2.waitKey(10)
return count
def optimize(self, x):
x = x.reshape(6, ).copy()
base_frame = self.instance_frame
x = np.multiply(x, self.gains)
frame = KDLFromArray(x, fmt=LineOptimizer.DEFAULT_FORMAT)
frame = base_frame * frame
matrix = KLDtoNumpyMatrix(frame)
R = matrix[:3, :3]
t = matrix[:3, 3] * 1000.0
ren_rgb = renderer.render(self.model,
self.img_size,
self.K,
R,
t,
mode='rgb',
surf_color=(1, 1, 1),
ambient_weight=0.0)
rgb_lines = lsd.detect(self.gray)[0]
model_lines = lsd.detect(cv2.cvtColor(ren_rgb, cv2.COLOR_BGR2GRAY))[0]
zeros = np.zeros(self.gray.shape)
rgb_lines_img = self.drawSegments(zeros, rgb_lines)
model_lines_img = self.drawSegments(zeros, model_lines)
rendered_image = cv2.addWeighted(self.image, 1, ren_rgb, 0.85, 0)
# rgb_lines_img = lsd.drawSegments(zeros, rgb_lines)
# model_lines_img = lsd.drawSegments(zeros, model_lines)
# diff = rgb_lines_img - model_lines_img
diff = cv2.bitwise_and(rgb_lines_img, model_lines_img)
# diff = np.abs(diff.astype(np.uint8))
count = np.count_nonzero(diff.ravel())
print("MAXMIN", np.min(diff), np.max(diff), count)
cv2.imshow("model", rendered_image)
cv2.imshow("opt_rgb", rgb_lines_img)
cv2.imshow("opt_model", model_lines_img)
cv2.imshow("opt_diff", diff)
cv2.waitKey(10)
return -count
def render(self, rgb, model, R, t):
K = self.get_camera_intrinsic()
surf_color = (1, 0, 0)
im_size = (rgb.shape[1], rgb.shape[0])
ren_rgb = renderer.render(model,
im_size,
K,
R,
t,
surf_color=surf_color,
mode='rgb')
ren_gray = cv2.cvtColor(ren_rgb, cv2.COLOR_RGB2GRAY)
mask = np.zeros((ren_gray.shape[0], ren_gray.shape[1]))
mask[ren_gray != 0] = 255
# cv2.imwrite("/home/david/test.png",mask)
vis_rgb = 0.4 * rgb.astype(np.float) + 0.6 * ren_rgb.astype(np.float)
vis_rgb = vis_rgb.astype(np.uint8)
return vis_rgb, mask
def optimizeGradientOrientation(self, x):
x = x.reshape(7, ).copy()
base_frame = self.instance_frame
frame = KDLFromArray(x, fmt=LineOptimizer.DEFAULT_FORMAT)
frame = base_frame * frame
matrix = KLDtoNumpyMatrix(frame)
R = matrix[:3, :3]
t = matrix[:3, 3] * 1000.0
ren_rgb = renderer.render(self.model,
self.im_size,
self.K,
R,
t,
mode='rgb',
surf_color=[100, 100, 100])
image_angle = self.getGradientOrientation(self.image)
model_angle = self.getGradientOrientation(ren_rgb)
mask = np.zeros(image_angle.shape)
mask[model_angle > 0] = image_angle[model_angle > 0]
diff = np.abs(mask - model_angle)
e = np.sum(diff.ravel())
output = image.copy()
output[ren_rgb > 0] = ren_rgb[ren_rgb > 0]
cv2.imshow("image", image_angle)
cv2.imshow("model", model_angle)
cv2.imshow("mask", diff)
cv2.imshow("output", output)
cv2.waitKey(1)
print("Error", e)
return e
instance_frame = instances_poses[0]*transform
instance_frame = poses[index].Inverse() * instance_frame
if optimized_correction is not None:
print("CORRECTING WIGH", optimized_correction)
instance_frame = instance_frame*optimized_correction
instance_frame_matrix = KLDtoNumpyMatrix(instance_frame)
R = instance_frame_matrix[:3, :3]
t = instance_frame_matrix[:3, 3]*1000.0
im_size = (640, 480)
ren_rgb = renderer.render(model, im_size, K, R, t,
mode='rgb', surf_color=[0, 1.0, 0])
# rgb_lines = lsd.detect(gray)[0]
# model_lines = lsd.detect(cv2.cvtColor(ren_rgb, cv2.COLOR_BGR2GRAY))[0]
# zeros = np.zeros(gray.shape)
# rgb_lines_img = lsd.drawSegments(zeros, rgb_lines)
# model_lines_img = lsd.drawSegments(zeros, model_lines)
if optimized_correction is None:
lopt = LineOptimizer(K, image, model, instance_frame)
eye = PyKDL.Frame()
cv2.imshow("model", image)
cv2.waitKey(0)
# Visualization #1
#-----------------------------------------------------------------------
# Load RGB image
rgb_path = rgb_path_mask.format(device, obj_id, im_id, rgb_ext[device])
rgb = imageio.imread(rgb_path)
# Render RGB image of the object model at the pose associated with
# the training image into a
# surf_color = obj_colors[obj_id]
surf_color = (0, 1, 0)
im_size = (rgb.shape[1], rgb.shape[0])
ren_rgb = renderer.render(model,
im_size,
K,
R,
t,
surf_color=surf_color,
mode='rgb')
vis_rgb = 0.5 * rgb.astype(np.float) + 0.5 * ren_rgb.astype(np.float)
vis_rgb = vis_rgb.astype(np.uint8)
# Draw the bounding box of the object
vis_rgb = misc.draw_rect(vis_rgb, im_gt[0]['obj_bb'])
# Save the visualization
vis_rgb[vis_rgb > 255] = 255
vis_rgb_path = vis_rgb_path_mask.format(obj_id, device, model_type,
im_id)
imageio.imwrite(vis_rgb_path, vis_rgb.astype(np.uint8))
rgb_path = rgb_path_mask.format(device, scene_id, im_id,
rgb_ext[device])
rgb = imageio.imread(rgb_path)
im_size = (rgb.shape[1], rgb.shape[0])
vis_rgb = np.zeros(rgb.shape, np.float)
for gt in scene_gt[im_id]:
model = models[gt['obj_id']]
R = gt['cam_R_m2c']
t = gt['cam_t_m2c']
surf_color = obj_colors[gt['obj_id'] - 1]
ren_rgb = renderer.render(model,
im_size,
K,
R,
t,
surf_color=surf_color,
mode='rgb')
# Draw the bounding box of the object
ren_rgb = misc.draw_rect(ren_rgb, gt['obj_bb'])
vis_rgb += 0.7 * ren_rgb.astype(np.float)
# cv2.imshow('step vis', ren_rgb)
# cv2.waitKey()
# Save the visualization
vis_rgb = 0.6 * vis_rgb + 0.4 * rgb
vis_rgb[vis_rgb > 255] = 255
vis_rgb_path = vis_rgb_path_mask.format(scene_id, device, model_type,
def test(ctx):
config = ctx.obj['config']
scene_ids = range(1, 21)
device = config['device']
model_type = config['model_type']
im_step = int(config['image_step'])
data_path = config['dataset_path']
output_dir = config['output_path']
# Paths to the elements of the T-LESS dataset
model_path_mask = os.path.join(data_path, 'models_' + model_type,
'obj_{:02d}.ply')
scene_info_path_mask = os.path.join(data_path, 'test_{}', '{:02d}',
'info.yml')
scene_gt_path_mask = os.path.join(data_path, 'test_{}', '{:02d}', 'gt.yml')
rgb_path_mask = os.path.join(data_path, 'test_{}', '{:02d}', 'rgb',
'{:04d}.{}')
depth_path_mask = os.path.join(data_path, 'test_{}', '{:02d}', 'depth',
'{:04d}.png')
rgb_ext = {'primesense': 'png', 'kinect': 'png', 'canon': 'jpg'}
obj_colors_path = os.path.join('data', 'obj_rgb.txt')
vis_rgb_path_mask = os.path.join(output_dir, '{:02d}_{}_{}_{:04d}_rgb.png')
vis_depth_path_mask = os.path.join(output_dir,
'{:02d}_{}_{}_{:04d}_depth_diff.png')
misc.ensure_dir(output_dir)
obj_colors = inout.load_colors(obj_colors_path)
plt.ioff() # Turn interactive plotting off
for scene_id in scene_ids:
# Load info about the test images (including camera parameters etc.)
scene_info_path = scene_info_path_mask.format(device, scene_id)
scene_info = inout.load_scene_info(scene_info_path)
scene_gt_path = scene_gt_path_mask.format(device, scene_id)
gts = inout.load_scene_gt(scene_gt_path)
# Load models of objects present in the scene
scene_obj_ids = set()
for gt in gts[0]:
scene_obj_ids.add(gt['obj_id'])
models = {}
for scene_obj_id in scene_obj_ids:
model_path = model_path_mask.format(scene_obj_id)
models[scene_obj_id] = inout.load_ply(model_path)
for im_id, im_info in scene_info.items():
if im_id % im_step != 0:
continue
print('scene: ' + str(scene_id) + ', device: ' + device +
', im_id: ' + str(im_id))
# Get intrinsic camera parameters
K = im_info['cam_K']
# Visualization #1
# -----------------------------------------------------------------------
# Load RGB image
rgb_path = rgb_path_mask.format(device, scene_id, im_id,
rgb_ext[device])
rgb = scipy.misc.imread(rgb_path)
im_size = (rgb.shape[1], rgb.shape[0])
vis_rgb = np.zeros(rgb.shape, np.float)
for gt in gts[im_id]:
model = models[gt['obj_id']]
R = gt['cam_R_m2c']
t = gt['cam_t_m2c']
surf_color = obj_colors[gt['obj_id'] - 1]
ren_rgb = renderer.render(model,
im_size,
K,
R,
t,
surf_color=surf_color,
mode='rgb')
import cv2
cv2.imshow("test", ren_rgb)
cv2.waitKey()
# Draw the bounding box of the object
ren_rgb = misc.draw_rect(ren_rgb, gt['obj_bb'])
vis_rgb += 0.7 * ren_rgb.astype(np.float)
# Save the visualization
vis_rgb = 0.6 * vis_rgb + 0.4 * rgb
vis_rgb[vis_rgb > 255] = 255
vis_rgb_path = vis_rgb_path_mask.format(scene_id, device,
model_type, im_id)
scipy.misc.imsave(vis_rgb_path, vis_rgb.astype(np.uint8))
# Visualization #2
# -----------------------------------------------------------------------
if device != 'canon':
# Load depth image
depth_path = depth_path_mask.format(device, scene_id, im_id,
rgb_ext[device])
depth = scipy.misc.imread(depth_path) # Unit: 0.1 mm
depth = depth.astype(np.float) * 0.1 # Convert to mm
# Render the objects at the ground truth poses
im_size = (depth.shape[1], depth.shape[0])
ren_depth = np.zeros(depth.shape, np.float)
for gt in gts[im_id]:
model = models[gt['obj_id']]
R = gt['cam_R_m2c']
t = gt['cam_t_m2c']
# Render the current object
ren_depth_obj = renderer.render(model,
im_size,
K,
R,
t,
mode='depth')
# Add to the final depth map only the parts of the surface that
# are closer than the surfaces rendered before
visible_mask = np.logical_or(ren_depth == 0,
ren_depth_obj < ren_depth)
mask = np.logical_and(ren_depth_obj != 0, visible_mask)
ren_depth[mask] = ren_depth_obj[mask].astype(np.float)
# Calculate the depth difference at pixels where both depth maps
# are valid
valid_mask = (depth > 0) * (ren_depth > 0)
depth_diff = valid_mask * (depth - ren_depth.astype(np.float))
# Save the visualization
vis_depth_path = vis_depth_path_mask.format(
scene_id, device, model_type, im_id)
plt.matshow(depth_diff)
plt.title('captured - rendered depth [mm]')
plt.colorbar()
plt.savefig(vis_depth_path, pad=0)
plt.close()
def optimizeSuperpixelsReduced(self, x):
x = x.reshape(7, ).copy()
base_frame = self.instance_frame
#x = np.multiply(x, self.gains)
frame = KDLFromArray(x, fmt=LineOptimizer.DEFAULT_FORMAT)
frame = base_frame * frame
matrix = KLDtoNumpyMatrix(frame)
R = matrix[:3, :3]
t = matrix[:3, 3] * 1000.0
ren_rgb = renderer.render(self.model,
self.im_size,
self.K,
R,
t,
mode='rgb',
surf_color=[0, 1.0, 0])
nonzeroindices = np.argwhere(ren_rgb > 0)
labels_raw = ariadne.graph.labels[nonzeroindices[:, 0],
nonzeroindices[:, 1]].ravel()
labels = np.unique(labels_raw)
y = np.bincount(labels_raw)
ii = np.nonzero(y)[0]
labels_count = dict(zip(ii, y[ii]))
labels_variance = np.var(y[ii])
#print("LABELS_COUNT", labels_count, dict(labels_count))
# And then:
# print("LABELS", labels)
# for index in nonzeroindices:
# label = ariadne.graph.labels[index[0], index[1]]
# labels[label] = True
zero = np.zeros(self.image.shape[:2])
if self.debug:
output_image = self.output_image.copy()
counter = 0
occupied = 0.0
total = 0.0
occupied_map = {}
for l in labels:
label_indices = np.argwhere(ariadne.graph.labels == l)
counter += label_indices.shape[0]
occupied_map[l] = float(labels_count[l]) / \
float(label_indices.shape[0])
if occupied_map[l] > 0.1:
occupied += labels_count[l]
total += label_indices.shape[0]
if self.debug:
output_image[label_indices[:, 0],
label_indices[:,
1]] = np.array([255, 255, 255
]).astype(np.uint8)
zero[label_indices[:, 0],
label_indices[:, 1]] = np.array([255]).astype(np.uint8)
if self.debug:
output_image[nonzeroindices[:, 0],
nonzeroindices[:,
1]] = np.array([255, 0,
255]).astype(np.uint8)
zero[nonzeroindices[:, 0], nonzeroindices[:, 1]] = 0
counter -= nonzeroindices.shape[0]
void_space_single = 0.0
for k, v in occupied_map.items():
void_space_single += v
void_space_single /= float(labels.shape[0])
void_space = (float(total) - float(occupied)) / float(total)
#print("VOID_SPACE", void_space, void_space_single)
if self.debug:
cv2.imshow("model", ren_rgb)
cv2.imshow("image", output_image)
cv2.imshow("zero", zero)
c = cv2.waitKey(1)
#count = counter - labels_variance*0.01
e = void_space
print("COUNTER DIFFERENCE", void_space, void_space_single)
# if count == 0:
# count = np.inf
# print("CURENT X", x, count)
return e
# Visualization #1
#-----------------------------------------------------------------------
# Load RGB image
rgb_path = rgb_path_mask.format(device, scene_id, im_id, rgb_ext[device])
rgb = scipy.misc.imread(rgb_path)
im_size = (rgb.shape[1], rgb.shape[0])
vis_rgb = np.zeros(rgb.shape, np.float)
for gt in scene_gt[im_id]:
model = models[gt['obj_id']]
R = gt['cam_R_m2c']
t = gt['cam_t_m2c']
surf_color = obj_colors[gt['obj_id'] - 1]
ren_rgb = renderer.render(model, im_size, K, R, t,
surf_color=surf_color, mode='rgb')
# Draw the bounding box of the object
ren_rgb = misc.draw_rect(ren_rgb, gt['obj_bb'])
vis_rgb += 0.7 * ren_rgb.astype(np.float)
# Save the visualization
vis_rgb = 0.6 * vis_rgb + 0.4 * rgb
vis_rgb[vis_rgb > 255] = 255
vis_rgb_path = vis_rgb_path_mask.format(scene_id, device, model_type, im_id)
scipy.misc.imsave(vis_rgb_path, vis_rgb.astype(np.uint8))
# Visualization #2
#-----------------------------------------------------------------------
if device != 'canon':
