def compare(config, LOAD=False):
'''
vicon data is required!
execute 'read_bag' first
'''
task_name = config['task_name']
fps = config['animation']['fps']
object_list = util.load_txt('./configure/%s_objects.txt' % task_name)
nb_object = len(object_list)
data_dir = './data/' + task_name
pose_dir = './output/' + task_name + '/read_pose'
output_dir = './output/' + task_name + '/compare_traj'
util.create_dir(output_dir, clear=False)
## load data
demo_list = os.listdir('./data/' + task_name)
for demo in demo_list:
rgb_demo_dir = data_dir + '/' + demo + '/rgb'
cam_demo_dir = data_dir + '/' + demo + '/vicon/k_zed'
output_demo_dir = output_dir + '/' + demo
util.create_dir(output_demo_dir + '/traj', clear=True)
util.create_dir(output_demo_dir + '/plot', clear=True)
# g_vc1 : vicon to camera(vicon_pose) = cam pose by vicon
# g_c1c2 : camera(vicon_pose) to camera center(center of image plane)
# g_c2o1 : camera center to object(vision coordinates) = object pose by vision
# g_o1o2 : object(vision coordinates) to object(vicon coordinates)
# g_vo2_gt : vicon to object(vicon coordinates) = object pose by vicon
assert len(object_list) == 1 ## to do : multiple object
obj_demo_dir = data_dir + '/' + demo + '/vicon/' + object_list[
0] ## to do : multiple object
se3_c2o1 = np.load(pose_dir + '/' + demo +
'/pose_traj.npy')[:,
0, :] ## to do : multiple object
se3_vc1 = load_vicon(cam_demo_dir)
se3_vo2_gt = load_vicon(obj_demo_dir)
# make vicon orientation = [0,0,0]
demo_len = 50 #len(se3_c2o1) #################################
g_vo2_gt_0 = se3_to_SE3(se3_vo2_gt[0, :])
R0 = g_vo2_gt_0[0:3, 0:3]
T0 = np.zeros(3)
g0_inv = RT_to_SE3(np.transpose(R0), T0)
for t in range(demo_len):
g_vo2_gt_t = se3_to_SE3(se3_vo2_gt[t, :])
new_g_vo2_gt_t = np.matmul(g_vo2_gt_t, g0_inv)
new_se3_vo2_gt_t = SE3_to_se3(new_g_vo2_gt_t)
se3_vo2_gt[t, :] = new_se3_vo2_gt_t
x0 = np.random.rand(12)
#x0 = np.random.rand(6)
optimize_len = int(len(se3_c2o1))
def objective_fn(x0):
# g_vc1 : vicon to camera(vicon_pose) = se3_cam
# g_c1c2 : camera(vicon_pose) to camera center(center of image plane)
# g_c2o1 : camera center to object(vision coordinates) = se3_vision
# g_o1o2 : object(vision coordinates) to object(vicon coordinates)
g_c1c2 = se3_to_SE3(x0[0:6])
g_o1o2 = se3_to_SE3(x0[6:12])
#g_c1c2 = se3_to_SE3(np.zeros(6))
#g_o1o2 = se3_to_SE3(x0)
loss = 0
for t in range(optimize_len):
g_c2o1_t = se3_to_SE3(se3_c2o1[t, :])
g_vc1_t = se3_to_SE3(se3_vc1[t, :])
g_vo2 = np.matmul(
g_vc1_t, np.matmul(g_c1c2, np.matmul(g_c2o1_t, g_o1o2)))
g_vo2 = np.matmul(g_vc1_t, np.matmul(g_c2o1_t, g_o1o2))
g_vo2_gt = se3_to_SE3(se3_vo2_gt[t, :])
se3_vo2 = SE3_to_se3(g_vo2)
#loss += np.sum(np.square(se3_vo2[:]-se3_vo2_gt[t,:])) #----------------
loss += np.sum(np.square(g_vo2[0:3, 3] - g_vo2_gt[0:3, 3]))
#loss += np.sum(np.square(g_vo2-g_vo2_gt)) #----------------------
return loss
print(colored('initial_loss:' + str(objective_fn(x0)), 'blue'))
if LOAD:
result = np.load(output_demo_dir + '/optimization.npy',
allow_pickle=True).item()
cmd = input('Optimization more? [y/n]')
if cmd == 'y':
x0 = result.x
result = minimize(objective_fn,
x0,
method='BFGS',
options={'disp': True})
else:
pass
else:
#'''
result = minimize(objective_fn,
x0,
method='BFGS',
options={'disp': True})
#'''
'''
result = minimize(objective_fn,
x0,
method='BFGS',
tol=1e-8,
options={'gtol': 1e-8, 'disp': True})
'''
np.save(output_demo_dir + '/optimization.npy', result)
print(colored('optimized_loss:' + str(objective_fn(result.x)), 'blue'))
g_c1c2 = se3_to_SE3(result.x[0:6])
g_o1o2 = se3_to_SE3(result.x[6:12])
#g_c1c2 = se3_to_SE3(np.zeros(6))
#g_o1o2 = se3_to_SE3(result.x)
obj_vicon = vision.SE3object(np.zeros(6), angle_type='axis')
obj_vision = vision.SE3object(np.zeros(6), angle_type='axis')
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
loss = 0
position_error = []
rotation_error = []
vicon_traj = []
vision_traj = []
vicon_euler = []
vision_euler = []
T_vo2 = np.zeros((0, 3))
T_vo2_gt = np.zeros((0, 3))
g_c2o1_0 = se3_to_SE3(se3_c2o1[0, :])
g_vc1_0 = se3_to_SE3(se3_vc1[0, :])
g_vo2_0 = np.matmul(g_vc1_0,
np.matmul(g_c1c2, np.matmul(g_c2o1_0, g_o1o2)))
g_vo2_0_gt = se3_to_SE3(se3_vo2_gt[0, :])
R_target = g_vo2_0_gt[0:3, 0:3]
R_ori = g_vo2_0[0:3, 0:3]
T_ori = g_vo2_0[0:3, 3]
SO3_align = np.matmul(R_target, np.transpose(R_ori))
SE3_align = np.matmul(inv_SE3(g_vo2_0), g_vo2_0_gt)
for t in range(demo_len):
g_c2o1_t = se3_to_SE3(se3_c2o1[t, :])
g_vc1_t = se3_to_SE3(se3_vc1[t, :])
g_vo2_t = np.matmul(g_vc1_t,
np.matmul(g_c1c2, np.matmul(g_c2o1_t, g_o1o2)))
#g_vo2_t[0:3,0:3] = np.matmul(SO3_align, g_vo2_t[0:3,0:3])
#g_vo2_t = np.matmul(g_vo2_t, SE3_align)
se3_vo2_t_gt = SE3_to_se3(se3_to_SE3(se3_vo2_gt[t, :]))
g_vo2_t_gt = se3_to_SE3(se3_vo2_t_gt)
se3_vo2_t = SE3_to_se3(g_vo2_t)
T_vo2 = np.concatenate(
[T_vo2, np.expand_dims(g_vo2_t[0:3, 3], 0)], 0)
T_vo2_gt = np.concatenate(
[T_vo2_gt, np.expand_dims(g_vo2_t_gt[0:3, 3], 0)], 0)
ax.clear()
obj_vicon.apply_pose(se3_vo2_t)
obj_vision.apply_pose(se3_vo2_t_gt)
obj_vicon.plot(ax, scale=0.015, linewidth=3)
obj_vision.plot(ax, scale=0.015, linewidth=3)
ax.plot(T_vo2_gt[:t, 0],
T_vo2_gt[:t, 1],
T_vo2_gt[:t, 2],
'--',
color='r',
alpha=0.5,
linewidth=4)
ax.plot(T_vo2[:, 0],
T_vo2[:, 1],
T_vo2[:, 2],
color='g',
alpha=0.5,
linewidth=3)
util.set_axes_equal(ax)
ax.set_xlabel('x(m)')
ax.set_ylabel('y(m)')
ax.set_zlabel('z(m)')
fig.savefig(output_demo_dir + '/traj/%05d.png' % t)
## rescale (||w|| = 1)
#se3_vo2_t[3:6] = (1./np.sqrt(np.sum(np.square(se3_vo2_t[3:6]))))*se3_vo2_t[3:6]
#se3_vo2_t_gt[3:6] = (1./np.sqrt(np.sum(np.square(se3_vo2_t_gt[3:6]))))*se3_vo2_t_gt[3:6]
loss += np.sqrt(np.sum(np.square(se3_vo2_t_gt - se3_vo2_t)))
position_error.append(
np.expand_dims(
np.sqrt(
np.sum(np.square(g_vo2_t_gt[0:3, 3] -
g_vo2_t[0:3, 3]))), 0))
rotation_error.append(
np.expand_dims(
np.sqrt(
np.sum(np.square(se3_vo2_t_gt[3:6] - se3_vo2_t[3:6]))),
0))
vicon_traj.append(np.expand_dims(se3_vo2_t_gt, 0))
vision_traj.append(np.expand_dims(se3_vo2_t, 0))
vicon_euler.append(
np.expand_dims(R_to_euler(g_vo2_t_gt[0:3, 0:3]), 0))
vision_euler.append(
np.expand_dims(R_to_euler(g_vo2_t[0:3, 0:3]), 0))
if t == 0:
total_translation = 0
total_rotation = 0
prev_g_vo = g_vo2_t_gt
prev_se3_vo = se3_vo2_t_gt
else:
total_translation += np.sqrt(
np.sum(np.square(g_vo2_t_gt[0:3, 3] - prev_g_vo[0:3, 3])))
total_rotation += np.sqrt(
np.sum(np.square(se3_vo2_t_gt[3:6] - prev_se3_vo[3:6])))
prev_g_vo = g_vo2_t_gt
prev_se3_vo = se3_vo2_t_gt
plt.close()
## save loss
loss = loss / demo_len
position_error = np.sum(
position_error) / demo_len #np.concatenate(position_error,0)
rotation_error = np.sum(
rotation_error) / demo_len #np.concatenate(rotation_error,0)
vicon_traj = np.concatenate(vicon_traj, 0)
vision_traj = np.concatenate(vision_traj, 0)
vicon_euler = np.concatenate(vicon_euler, 0)
vision_euler = np.concatenate(vision_euler, 0)
np.savetxt(output_demo_dir + '/loss.txt', [loss])
np.savetxt(output_demo_dir + '/position_error.txt', [position_error])
np.savetxt(output_demo_dir + '/rotation_error.txt', [rotation_error])
np.savetxt(output_demo_dir + '/total_translation.txt',
[total_translation])
np.savetxt(output_demo_dir + '/total_rotation.txt', [total_rotation])
np.savetxt(output_demo_dir + '/vicon_traj.txt', vicon_traj)
np.savetxt(output_demo_dir + '/vision_traj.txt', vision_traj)
## save plot
fig = plt.figure()
ymins = []
ymaxs = []
axes = []
scales = []
for i in range(3):
ax = plt.subplot(3, 1, i + 1)
ax.plot(np.arange(demo_len),
vicon_traj[:, i],
'--',
color='r',
alpha=0.5,
linewidth=4)
ax.plot(np.arange(demo_len),
vision_traj[:, i],
color='g',
alpha=0.5,
linewidth=3)
ymin, ymax = ax.get_ylim()
ymins.append(ymin)
ymaxs.append(ymax)
axes.append(ax)
scales.append(ymax - ymin)
ymin = min(ymins)
ymax = max(ymaxs)
scale_max = max(scales)
for ax, ymin, ymax in zip(axes, ymins, ymaxs):
center = (ymin + ymax) / 2
ax.set_ylim([center - scale_max / 2, center + scale_max / 2])
fig.savefig(output_demo_dir + '/v_component.png')
plt.close()
fig = plt.figure()
ymins = []
ymaxs = []
axes = []
scales = []
for i in range(3):
ax = plt.subplot(3, 1, i + 1)
ax.plot(np.arange(demo_len),
vicon_traj[:, i + 3],
'--',
color='r',
alpha=0.5,
linewidth=4)
ax.plot(np.arange(demo_len),
vision_traj[:, i + 3],
color='g',
alpha=0.5,
linewidth=3)
ymin, ymax = ax.get_ylim()
ymins.append(ymin)
ymaxs.append(ymax)
axes.append(ax)
scales.append(ymax - ymin)
ymin = min(ymins)
ymax = max(ymaxs)
scale_max = max(scales)
for ax, ymin, ymax in zip(axes, ymins, ymaxs):
center = (ymin + ymax) / 2
ax.set_ylim([center - scale_max / 2, center + scale_max / 2])
fig.savefig(output_demo_dir + '/w_component.png')
plt.close()
## translation and rotation
fig = plt.figure()
ymins = []
ymaxs = []
axes = []
scales = []
for i in range(3):
ax = plt.subplot(3, 1, i + 1)
ax.plot(np.arange(demo_len),
T_vo2_gt[:, i],
'--',
color='r',
alpha=0.5,
linewidth=4)
ax.plot(np.arange(demo_len),
T_vo2[:, i],
color='g',
alpha=0.5,
linewidth=3)
ymin, ymax = ax.get_ylim()
ymins.append(ymin)
ymaxs.append(ymax)
axes.append(ax)
scales.append(ymax - ymin)
ymin = min(ymins)
ymax = max(ymaxs)
scale_max = max(scales)
for ax, ymin, ymax in zip(axes, ymins, ymaxs):
center = (ymin + ymax) / 2
ax.set_ylim([center - scale_max / 2, center + scale_max / 2])
fig.savefig(output_demo_dir + '/translation_component.png')
plt.close()
fig = plt.figure()
ymins = []
ymaxs = []
axes = []
scales = []
for i in range(3):
ax = plt.subplot(3, 1, i + 1)
ax.plot(np.arange(demo_len),
vicon_euler[:, i],
'--',
color='r',
alpha=0.5,
linewidth=4)
ax.plot(np.arange(demo_len),
vision_euler[:, i],
color='g',
alpha=0.5,
linewidth=3)
ymin, ymax = ax.get_ylim()
ymins.append(ymin)
ymaxs.append(ymax)
axes.append(ax)
scales.append(ymax - ymin)
ymin = min(ymins)
ymax = max(ymaxs)
scale_max = max(scales)
for ax, ymin, ymax in zip(axes, ymins, ymaxs):
center = (ymin + ymax) / 2
ax.set_ylim([center - scale_max / 2, center + scale_max / 2])
fig.savefig(output_demo_dir + '/rotation_component.png')
plt.close()
WATCH = args.watch
VICON = args.vicon
except:
traceback.print_exc()
print(colored('type like: python3 ./collect_data.py [task_name] [demo_name] [option]'),'r')
print(colored('ex: python3 ./collect_data.py task1 demo0 --vicon'))
## Just watch camera output
if WATCH:
p_rviz = subprocess.Popen('roslaunch babymind image_visualize.launch', stdout = subprocess.PIPE, shell = True)
input(colored('stop watch? [y/n]', 'green'))
os.killpg(os.getpgid(p_rviz.pid), signal.SIGTERM)
sys.exit()
## Log vision (+vicon) data
object_names = util.load_txt('./configure/'+task_name+'_objects.txt')
vicon_topics = ['/vicon/k_giant_ur3/k_giant_ur3', '/vicon/k_zed/k_zed']
vicon_topics += ['/vicon/'+name+'/'+name for name in object_names ]
camera_topics = ['/zed/zed_node/rgb/image_rect_color', '/zed/zed_node/depth/depth_registered']
save_dir = './data/'+task_name+'/'+demo_name
if VICON:
p_ros = subprocess.Popen('roslaunch babymind data_collect.launch', stdout = subprocess.PIPE, shell = True)
topics = camera_topics + vicon_topics
else:
p_ros = subprocess.Popen('roslaunch babymind data_collect_no_vicon.launch', stdout = subprocess.PIPE, shell = True)
topics = camera_topics
merged_topic = ''
for topic in topics:
merged_topic += (topic+' ')
def preprocess(config):
fps = config['animation']['fps']
## 1. resize image
## 2. generate mask from json
task_name = config['task_name']
object_file = './configure/'+task_name+'_objects.txt'
class_names = util.load_txt(object_file)
using_class = class_names
demos = sorted(os.listdir('./output/'+task_name+'/label'))
for demo_name in demos:
json_path = './output/'+task_name+'/label/'+demo_name+'/annotations/segment_label.json'
img_source_dir = './output/'+task_name+'/label/'+demo_name+'/labeled_img'
if not os.path.exists(json_path):
print('skip:' +str(demo_name))
continue
## load json files from
dataset = coco.CocoDataset()
dataset.load_coco(json_path, img_source_dir)
dataset.prepare()
## print dataset configuration
print("Image Count: {}".format(len(dataset.image_ids)))
print("Class Count: {}".format(dataset.num_classes))
preprocess_dir = './output/'+task_name+'/preprocess/'+demo_name
img_dir = preprocess_dir+'/labeled_image'
mask_dir = preprocess_dir+'/mask'
util.create_dir(img_dir,clear = True)
util.create_dir(mask_dir,clear = True)
count = 0
for image_id in dataset.image_ids:
image = dataset.load_image(image_id)
mask, class_ids = dataset.load_mask(image_id)
img_h, img_w, img_ch = image.shape
'''
## Fit different size image into the same shaped
image, window, scale, padding, _ = utils.resize_image(
image,
min_dim=IMAGE_MIN_DIM,
max_dim=IMAGE_MAX_DIM,
mode="square")
mask = utils.resize_mask(mask, scale, padding)
## Resize image
image = transform.resize(image, RESIZE_SHAPE, anti_aliasing = True)
mask = transform.resize(mask, RESIZE_SHAPE, anti_aliasing = True)
'''
## fit data for u-net
label = mask_to_label(mask, class_ids, class_names, using_class)
if label is None:
pass
else:
label_img = multichannel_to_image(label)
io.imsave(img_dir+'/%04d.png'%image_id, image)
io.imsave(mask_dir+'/%04d.png'%image_id, label_img)
np.save(img_dir+'/%04d.npy'%image_id, image)
np.save(mask_dir+'/%04d.npy'%image_id, label)
count += 1