def plot_output(self, tensor_out, demo_name, count):
img = tensor_out['img_ph'][0, :]
mask = tensor_out['mask_pred'][0, :]
util.create_dir(self.output_dir + '/' + demo_name)
color = [0, 0, 1]
mask_img = image.multichannel_to_image(mask)
masked_img = image.apply_mask(img, mask_img, alpha=0.5)
plt.subplot(221)
plt.imshow(img)
plt.subplot(222)
plt.imshow(mask_img)
plt.subplot(223)
plt.imshow(masked_img)
plt.savefig(self.output_dir + '/' + demo_name + '/' + '%04d' %
(count - 1) + '.png')
plt.close()
np.save(
self.output_dir + '/' + demo_name + '/' + 'mask_%04d' %
(count - 1) + '.npy', mask)
def read(config):
task_name = config['task_name']
nb_object = config['object']['nb_object']
fps = config['animation']['fps']
se3_path = './output/' + task_name + '/pose/se3_pose.npy'
demo_list = os.listdir('./data/' + task_name)
output_dir = './output/' + task_name + '/read_pose'
se3_dict = np.load(se3_path, allow_pickle=True).item()
obj = vision.SE3object(np.zeros(6), angle_type='axis')
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
color = ['g', 'r']
# draw trajectory w.r.t. camera
for demo_name, se3_traj in se3_dict.items():
depth_dir = './data/' + task_name + '/' + demo_name + '/depth'
#mask_dir = './output/'+task_name+'/segment/'+demo_name
mask_dir = './data/' + task_name + '/' + demo_name + '/mask'
depth_files = sorted(glob.glob(depth_dir + '/*.npy'))
mask_files = sorted(glob.glob(mask_dir + '/*.npy'))
output_path = output_dir + '/' + demo_name
util.create_dir(output_path, clear=True)
depth = np.load(depth_files[0])
mask = np.load(mask_files[0])
se30 = se3_traj[0, 0, :]
SE30 = se3_to_SE3(se30)
T0 = SE30[0:3, 3]
T_traj = np.expand_dims(T0, 0)
# modify for multiple object
for t in range(len(se3_traj)):
ax.clear()
for obj_idx in range(nb_object):
se3 = se3_traj[t, obj_idx, :]
SE3 = se3_to_SE3(se3)
T = SE3[0:3, 3]
T_traj = np.concatenate([T_traj, np.expand_dims(T, 0)], 0)
ax.plot(T_traj[:, 0],
T_traj[:, 1],
T_traj[:, 2],
color[obj_idx],
alpha=0.5,
linewidth=4)
obj.apply_pose(se3)
obj.plot(ax, scale=0.05, linewidth=3)
util.set_axes_equal(ax)
#ax.axis('scaled')
ax.set_xlabel('x(m)')
ax.set_ylabel('y(m)')
ax.set_zlabel('z(m)')
ax.view_init(elev=-60, azim=-90)
fig.savefig(output_path + '/%06d.png' % t)
np.save(output_path + '/pose_traj.npy', se3_traj)
video_path = output_path + '/object_trajectory.avi'
util.frame_to_video(output_path, video_path, 'png', fps=fps)()
def train(self, continuous=False):
sess = self.sess
saver = self.saver
placeholders = self.placeholders
tensors = self.tensors
operations = self.operations
task_name = self.task_name
batch_size = self.batch_size
segnet_stream = Segnet_stream(self.config)
batch_iter = segnet_stream.iterator(batch_size=batch_size)
util.create_dir(self.figure_dir, clear=True)
util.create_dir(self.weight_dir, clear=False)
if continuous:
saver.restore(sess, self.weight_dir + '/u_net.ckpt')
writer = Writer(task_name,
self.network_name + '_train',
append=True)
else:
sess.run(self.init)
writer = Writer(task_name,
self.network_name + '_train',
append=False)
global_step = 0
one_cycle_loss = 0
while True:
try:
try:
img_batch, depth_batch, mask_batch, demo_name = next(
batch_iter)
except StopIteration:
one_cycle_train = segnet_stream.count
log_string = str(global_step) + '\t' + str(
one_cycle_loss / one_cycle_train) + '\n'
writer.write(log_string)
batch_iter = segnet_stream.iterator(batch_size=batch_size)
one_cycle_loss = 0
saver.save(sess, self.weight_dir + '/u_net.ckpt')
continue
feed_dict = {
placeholders['img_ph']: img_batch,
placeholders['depth_ph']: depth_batch,
placeholders['mask_ph']: mask_batch
}
tensor_out, _ = sess.run([tensors, operations],
feed_dict=feed_dict)
global_step += 1
one_cycle_loss += tensor_out['loss']
print(tensor_out['loss'])
except:
traceback.print_exc()
IPython.embed()
break
def __init__(self, data_name, txt_name, append=False):
log_dir = './log/' + data_name
util.create_dir(log_dir)
self.log_file = log_dir + '/' + txt_name + ".txt"
## write header of the file
if append:
with open(self.log_file, 'a') as f:
f.write(str(datetime.datetime.now()) + '\n')
else:
with open(self.log_file, 'w') as f:
f.write(str(datetime.datetime.now()) + '\n')
def test(self):
sess = self.sess
saver = self.saver
placeholders = self.placeholders
tensors = self.tensors
operations = self.operations
task_name = self.task_name
batch_size = self.batch_size
fps = self.fps
saver.restore(sess, self.weight_dir + '/u_net.ckpt')
test_tensors = {
'img_ph': tensors['img_ph'],
'mask_pred': tensors['mask_pred']
}
segnet_stream = Segnet_stream(self.config, mode='test')
batch_iter = segnet_stream.iterator(batch_size=batch_size)
writer = Writer(task_name, self.network_name + '_test')
util.create_dir(self.output_dir, clear=True)
while True:
try:
img_batch, depth_batch, _, demo_name = next(batch_iter)
count = segnet_stream.count
except StopIteration:
break
feed_dict = {
placeholders['img_ph']: img_batch,
placeholders['depth_ph']: depth_batch
}
tensor_out = sess.run(test_tensors, feed_dict=feed_dict)
self.plot_output(tensor_out, demo_name, count)
## save image animation
demo_list = os.listdir('./data/' + self.task_name)
for demo_name in demo_list:
img_path = self.output_dir + '/' + demo_name
video_path = self.output_dir + '/' + demo_name + '/mask_video.avi'
util.frame_to_video(img_path, video_path, 'png', fps=fps)()
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+' ')
start_record = input(colored('Start record? [y/n]','green'))
util.create_dir(save_dir, clear = True)
if start_record == 'y':
print(colored('Press ctrl+c will finish record','green'))
os.system('rosbag record -O %s/raw.bag %s'%(save_dir,merged_topic))
print('terminate program')
os.killpg(os.getpgid(p_ros.pid), signal.SIGTERM)
sys.exit()
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()
args = parser.parse_args()
task_name = args.task_name
demo_name = args.demo_name
except:
traceback.print_exc()
print(
colored(
'type like: python3 ./make_segment_label.py [task_name] [demo_name]'
), 'r')
print(colored('ex: python3 ./make_segment_label.py task1 demo0'))
rgb_dir = './data/' + task_name + '/' + demo_name + '/rgb'
rgb_imgs = sorted(glob.glob(rgb_dir + '/*.png'))
class_file = './configure/' + task_name + '_objects.txt'
label_dir = './output/' + task_name + '/label/' + demo_name + '/labeled_img'
util.create_dir(label_dir, clear=True)
json_dir = './output/' + task_name + '/label/' + demo_name + '/annotations'
util.create_dir(json_dir, clear=True)
skip = input(
colored(
'Pleas enter the number of skipping frames for eveery one label [current demo has %d images]'
% len(rgb_imgs), 'green'))
skip = int(skip)
for i, rgb_img in enumerate(rgb_imgs):
if i % skip == 0:
rgb_file_name = os.path.basename(rgb_img)
source_file = rgb_dir + '/' + rgb_file_name
target_file = label_dir + '/' + rgb_file_name
os.system('cp %s %s' % (source_file, target_file))
def train(self, continuous = False):
sess = self.sess
saver = self.saver
placeholders = self.placeholders
tensors = self.tensors
operators = self.operators
task_name = self.task_name
batch_size = self.batch_size
scale = self.scale
pose_stream = Pose_stream(task_name, batch_size, camera = self.camera, scale = scale)
batch_iter = pose_stream.iterator(batch_size = batch_size)
util.create_dir(self.weight_dir)
if continuous:
saver.restore(sess, self.weight_dir+'/u_net.ckpt')
writer = Writer(task_name, self.network_name+'_train', append = True)
util.create_dir(self.figure_dir+'/depth', clear = False)
util.create_dir(self.figure_dir+'/warped_img', clear = False)
else:
sess.run(self.init)
writer = Writer(task_name, self.network_name+'_train', append = False)
util.create_dir(self.figure_dir+'/depth', clear = True)
util.create_dir(self.figure_dir+'/warped_img', clear = True)
global_step = 0
one_cycle_loss = 0
one_cycle_photo = 0
one_cycle_pc = 0
#one_cycle_depth = 0
#one_cycle_rgb = 0
one_cycle_volume = 0
one_cycle_time = time.time()
num_data = pose_stream.num_data
print('number of data to train:%06d'%num_data)
minimum_loss = 1e10
while True:
try:
frame0_batch = []
frame1_batch = []
depth0_batch = []
depth1_batch = []
mask0_batch = []
mask1_batch = []
nan0_batch = []
nan1_batch = []
demo_batch = []
bbox0_batch = []
bbox1_batch = []
g_vr0_batch = []
g_vr1_batch = []
try:
for i in range(self.batch_size):
frame0_sample, frame1_sample, \
depth0_sample, depth1_sample, \
mask0_sample, mask1_sample, \
nan0_sample, nan1_sample, \
g_vr0_sample, g_vr1_sample, \
bbox0_sample, bbox1_sample, demo_name = next(batch_iter)
frame0_batch.append(frame0_sample)
frame1_batch.append(frame1_sample)
depth0_batch.append(depth0_sample)
depth1_batch.append(depth1_sample)
mask0_batch.append(mask0_sample)
mask1_batch.append(mask1_sample)
nan0_batch.append(nan0_sample)
nan1_batch.append(nan1_sample)
g_vr0_batch.append(g_vr0_sample)
g_vr1_batch.append(g_vr1_sample)
bbox0_batch.append(bbox0_sample)
bbox1_batch.append(bbox1_sample)
frame0_batch = np.concatenate(frame0_batch, axis = 0)
frame1_batch = np.concatenate(frame1_batch, axis = 0)
depth0_batch = np.concatenate(depth0_batch, axis = 0)
depth1_batch = np.concatenate(depth1_batch, axis = 0)
mask0_batch = np.concatenate(mask0_batch, axis = 0)
mask1_batch = np.concatenate(mask1_batch, axis = 0)
nan0_batch = np.concatenate(nan0_batch, axis = 0)
nan1_batch = np.concatenate(nan1_batch, axis = 0)
g_vr0_batch = np.concatenate(g_vr0_batch, axis = 0 )
g_vr1_batch = np.concatenate(g_vr1_batch, axis = 0 )
bbox0_batch = np.concatenate(bbox0_batch, axis = 0)
bbox1_batch = np.concatenate(bbox1_batch, axis = 0)
except StopIteration:
##
one_cycle_train = pose_stream._count
duration = time.time()-one_cycle_time
log_string = '%d \t %.2f \t l: %.10f \t p: %.10f \t pc: %.10f \t window: %.10f \n'\
%(global_step, duration,
one_cycle_loss/one_cycle_train, one_cycle_photo/one_cycle_train,
one_cycle_pc/one_cycle_train,
#one_cycle_depth/one_cycle_train,
#one_cycle_rgb/one_cycle_train,
one_cycle_volume/one_cycle_train)
#log_string = str(global_step)+ '\t' + str(one_cycle_loss/one_cycle_train) + '\n'
writer.write(log_string)
current_loss = one_cycle_loss/one_cycle_train
if minimum_loss > current_loss:
minimum_loss = current_loss
print(colored(log_string,'red'))
else:
print(log_string)
if True: #global_step % (int(10000/num_data)+1) == 0 and global_step != 0:
saver.save(sess, self.weight_dir+'/u_net.ckpt')
if True: #global_step % (int(1000/num_data)+1) == 0:
self.subplot_depth(tensor_out, self.figure_dir+'/depth/%s.png'%str(global_step).zfill(10))
self.subplot_warpedImg(tensor_out, self.figure_dir+'/warped_img/%s.png'%str(global_step).zfill(10))
batch_iter = pose_stream.iterator(batch_size = 32)
one_cycle_loss = 0
one_cycle_photo = 0
one_cycle_pc = 0
one_cycle_depth = 0
one_cycle_rgb = 0
one_cycle_volume = 0
one_cycle_time = time.time()
global_step +=1
continue
feed_dict = {
placeholders['frame0_ph'] : frame0_batch,
placeholders['frame1_ph'] : frame1_batch,
placeholders['depth0_ph'] : depth0_batch,
placeholders['depth1_ph'] : depth1_batch,
placeholders['mask0_ph'] : mask0_batch,
placeholders['mask1_ph'] : mask1_batch,
placeholders['nan0_ph'] : nan0_batch,
placeholders['nan1_ph'] : nan1_batch,
placeholders['g_vr0_ph'] : g_vr0_batch,
placeholders['g_vr1_ph'] : g_vr1_batch,
placeholders['bbox0_ph'] : bbox0_batch,
placeholders['bbox1_ph'] : bbox1_batch
}
tensor_out, _ = sess.run([tensors, operators], feed_dict = feed_dict)
one_cycle_loss += tensor_out['loss']
one_cycle_photo += self._p*tensor_out['photometric_loss']
one_cycle_pc += self._pc*tensor_out['pc_loss']
#one_cycle_depth += self._d*tensor_out['depth_recover_loss']
#one_cycle_rgb += self._recon*tensor_out['rgb_recover_loss']
one_cycle_volume += self._v*tensor_out['volume_loss']
except:
traceback.print_exc()
print('current task:'+self.task_name)
print('saving weight...')
IPython.embed()
time.sleep(3)
saver.save(sess, self.weight_dir+'/u_net.ckpt')
break
def test(self, saveFigure = False):
sess = self.sess
saver = self.saver
placeholders = self.placeholders
tensors = self.tensors
task_name = self.task_name
batch_size = self.batch_size
scale = self.scale
saver.restore(sess, self.weight_dir+'/u_net.ckpt')
pose_stream = Pose_stream(task_name, batch_size, camera = self.camera, scale = scale, mode = 'test')
demo_list = pose_stream.demo_list
batch_iter = pose_stream.iterator(batch_size = batch_size)
writer = Writer(task_name, self.network_name+'_test')
util.create_dir(self.output_dir, clear = True)
for demo_name in demo_list:
util.create_dir(self.output_dir+'/'+demo_name+'/depth')
util.create_dir(self.output_dir+'/'+demo_name+'/warped_img')
self._error = 0
global_step = 0
one_cycle_loss = 0
one_cycle_photo = 0
one_cycle_pc = 0
one_cycle_depth = 0
one_cycle_rgb = 0
one_cycle_volume = 0
se3_dict = {}
while True:
try:
frame0_batch, frame1_batch, \
depth0_batch, depth1_batch, \
mask0_batch, mask1_batch, \
nan0_batch, nan1_batch, \
g_vr0_batch, g_vr1_batch, \
bbox0_batch, bbox1_batch, \
demo_name = next(batch_iter)
print(demo_name)
count = pose_stream._count
except StopIteration:
break
feed_dict = {
placeholders['frame0_ph'] : frame0_batch,
placeholders['frame1_ph'] : frame1_batch,
placeholders['depth0_ph'] : depth0_batch,
placeholders['depth1_ph'] : depth1_batch,
placeholders['mask0_ph'] : mask0_batch,
placeholders['mask1_ph'] : mask1_batch,
placeholders['nan0_ph'] : nan0_batch,
placeholders['nan1_ph'] : nan1_batch,
placeholders['bbox0_ph'] : bbox0_batch,
placeholders['bbox1_ph'] : bbox1_batch,
placeholders['g_vr0_ph'] : g_vr0_batch,
placeholders['g_vr1_ph'] : g_vr1_batch
}
tensor_out = sess.run(tensors, feed_dict = feed_dict)
one_cycle_loss += tensor_out['loss']
one_cycle_photo += tensor_out['photometric_loss']
one_cycle_pc += tensor_out['pc_loss']
#one_cycle_depth += tensor_out['depth_recover_loss']
#one_cycle_rgb += tensor_out['rgb_recover_loss']
one_cycle_volume += tensor_out['volume_loss']
if saveFigure:
self.subplot_depth(tensor_out, self.output_dir+'/'+demo_name+'/depth/%s.png'%str(count-1).zfill(10))
self.subplot_warpedImg(tensor_out, self.output_dir+'/'+demo_name+'/warped_img/%s.png'%str(count-1).zfill(10))
writer.write(str(self._error)+'\n')
print('============')
print(tensor_out['loss'])
print(tensor_out['se3_0'])
se3_0 = tensor_out['se3_0']
se3_1 = tensor_out['se3_1']
if demo_name not in se3_dict:
se3_dict[demo_name] = []
se3_dict[demo_name].append(se3_0)
se3_dict[demo_name].append(se3_1)
one_cycle_train = pose_stream._count
log_string = '%d \t l: %.10f \t p: %.10f \t pc: %.10f \t d: %.10f rgb: %.10f volume: %.10f \n'\
%(global_step,
one_cycle_loss/one_cycle_train, one_cycle_photo/one_cycle_train,
one_cycle_pc/one_cycle_train,
one_cycle_depth/one_cycle_train, one_cycle_rgb/one_cycle_train, one_cycle_volume/one_cycle_train)
writer.write(log_string)
for demo_name, se3 in se3_dict.items():
se3_dict[demo_name] = np.concatenate(se3, axis = 0)
np.save(self.output_dir+'/se3_pose.npy', se3_dict)
def __init__(self, task_name, batch_size, camera = 'zed_mini', scale = 1, mode = 'train'):
self.batch_size = batch_size
self.mode = mode
self.camera = camera
self.img_dir = './data/'+task_name
#self.mask_dir = './output/'+task_name+'/segment'
self.mask_dir = './data/'+task_name
self.preprocess_dir = './.preprocess/'+task_name
self.scale = scale
self.num_data = 0
self._count = 0
self.demo_list = os.listdir(self.img_dir)
self.img_dict = {}
self.depth_dict = {}
self.mask_dict = {}
self.g_vr_dict = {}
self.img_preprocess_dict = {}
self.depth_preprocess_dict = {}
self.nan_mask_preprocess_dict = {}
self.mask_preprocess_dict = {}
self.bbox_preprocess_dict = {}
self.g_vr_preprocess_dict = {}
for demo in self.demo_list:
img_demo_dir = self.img_dir+'/'+demo
mask_demo_dir = self.mask_dir +'/'+demo
self.img_dict[demo] = sorted(glob.glob(img_demo_dir+'/rgb/*.npy'))
self.depth_dict[demo] = sorted(glob.glob(img_demo_dir+'/depth/*.npy'))
self.mask_dict[demo] = sorted(glob.glob(mask_demo_dir+'/mask/*.npy'))
self.g_vr_dict[demo] = sorted(glob.glob(img_demo_dir+'/vicon/k_zed/*.npy'))
for demo in self.demo_list:
img_list = self.img_dict[demo]
depth_list = self.depth_dict[demo]
mask_list = self.mask_dict[demo]
g_vr_list = self.g_vr_dict[demo]
preprocess_dir = self.preprocess_dir+'/'+demo
preprocess_img_dir = preprocess_dir +'/image'
preprocess_depth_dir = preprocess_dir +'/depth'
preprocess_nan_mask_dir = preprocess_dir +'/nan_mask'
preprocess_mask_dir = preprocess_dir +'/mask'
preprocess_mask_img_dir = preprocess_dir +'/mask_img'
preprocess_bbox_dir = preprocess_dir +'/bbox'
preprocess_g_vr_dir = preprocess_dir + '/g_vr'
if mode == 'train':
util.create_dir(preprocess_img_dir, clear = True)
util.create_dir(preprocess_depth_dir, clear = True)
util.create_dir(preprocess_nan_mask_dir, clear = True)
util.create_dir(preprocess_mask_dir, clear = True)
util.create_dir(preprocess_mask_img_dir, clear = True)
util.create_dir(preprocess_bbox_dir, clear = True)
util.create_dir(preprocess_g_vr_dir, clear = True)
index = 0
total_len = len(img_list)
for img_path, depth_path, mask_path, g_vr_path in zip(img_list, depth_list, mask_list, g_vr_list):
img_load = np.load(img_path)
depth_load = np.load(depth_path)
mask_load = np.load(mask_path)
xi_vr_load = np.load(g_vr_path)
img = preprocess_img(img_load, scale=scale)
depth, nan_mask= preprocess_depth(depth_load, scale=scale)
mask = preprocess_mask(mask_load, scale=1)
pc = vision.np_cloud_transformer(depth, self.camera, scale = scale)
bbox = preprocess_bbox(pc, mask_load, scale = scale)
g_vr = se3_to_SE3(xi_vr_load)
img = np.expand_dims(img, axis = 0)
depth = np.expand_dims(depth, axis = 0)
mask = np.expand_dims(mask, axis = 0)
nan_mask = np.expand_dims(nan_mask, axis = 0)
bbox = np.expand_dims(bbox, axis = 0)
g_vr = np.expand_dims(g_vr,0)
np.save(preprocess_img_dir+'/%04d.npy'%index, img)
np.save(preprocess_depth_dir+'/%04d.npy'%index, depth)
np.save(preprocess_nan_mask_dir+'/%04d.npy'%index, nan_mask)
np.save(preprocess_mask_dir+'/%04d.npy'%index, mask)
np.save(preprocess_bbox_dir+'/%04d.npy'%index, bbox)
np.save(preprocess_g_vr_dir+'/%04d.npy'%index, g_vr)
mask_img = multichannel_to_image(mask[0,:])
np.save(preprocess_mask_img_dir+'/%s.png'%str(index).zfill(10), mask_img)
index+=1
self.img_preprocess_dict[demo] = sorted(glob.glob(preprocess_img_dir+'/*.npy'))
self.depth_preprocess_dict[demo] = sorted(glob.glob(preprocess_depth_dir+'/*.npy'))
self.nan_mask_preprocess_dict[demo] = sorted(glob.glob(preprocess_nan_mask_dir+'/*.npy'))
self.mask_preprocess_dict[demo] = sorted(glob.glob(preprocess_mask_dir+'/*.npy'))
self.bbox_preprocess_dict[demo] = sorted(glob.glob(preprocess_bbox_dir+'/*.npy'))
self.g_vr_preprocess_dict[demo] = sorted(glob.glob(preprocess_g_vr_dir+'/*.npy'))
self.num_data += len(self.img_dict[demo])
args = parser.parse_args()
task_name = args.task_name
CLEAR = args.clear
except:
traceback.print_exc()
print(colored('type like: python3 ./read_bag.py [task_name] [option]'),'r')
print(colored('ex: python3 ./read_bag.py task1 --clear'))
task_dir = './data/'+task_name
demo_dirs = sorted(os.listdir(task_dir))
for demo_name in demo_dirs:
## 1. reindex rosbag file
reindex_dir = task_dir + '/' + demo_name +'/reindex'
bag_path = task_dir + '/' + demo_name +'/raw.bag.active'
util.create_dir(reindex_dir, clear = True)
print(colored('reindex bag file','blue'))
reindex_cmd = "rosbag reindex --output-dir=" + reindex_dir +" "+bag_path
os.system(reindex_cmd)
## 2. extract infomation from reindexed bag
print(colored('extract bag file','blue'))
demo_dir = task_dir + '/' + demo_name
rgb_dir = demo_dir + '/rgb_raw'
depth_dir = demo_dir + '/depth_raw'
vicon_dir = demo_dir + '/vicon_raw'
object_names = util.load_txt('./configure/'+task_name+'_objects.txt')
object_names += ['k_giant_ur3', 'k_zed']
vicon_topics = ['/vicon/'+name+'/'+name for name in object_names ]
def read(config):
task_name = config['task_name']
camera = config['camera']['camera']
nb_object = config['object']['nb_object']
scale = config['pose']['scale']
fps = config['animation']['fps']
pose_path = './output/'+task_name+'/pose'
data_dir = './data/'+task_name
output_dir = './output/'+task_name+'/read_pose2'
se3_dict = np.load(pose_path+'/se3_pose.npy', allow_pickle = True).item()
obj = vision.SE3object(np.zeros(6), angle_type = 'axis')
if camera == 'zed':
intrinsic = vision.Zed_intrinsic(scale = scale)
elif camera == 'zed_mini':
intrinsic = vision.Zed_mini_intrinsic(scale = scale)
fig = plt.figure()
ax = fig.add_subplot(111)
for demo_name, se3_traj in se3_dict.items():
output_path = output_dir+'/'+demo_name
util.create_dir(output_path, clear = True)
img_list = sorted(glob.glob(data_dir+'/'+demo_name+'/rgb/*.npy'))
depth_dir = './data/'+task_name+'/'+demo_name+'/depth'
mask_dir = './output/'+task_name+'/segment/'+demo_name
depth_files = sorted(glob.glob(depth_dir+'/*.npy'))
mask_files = sorted(glob.glob(mask_dir+'/*.npy'))
obj_idx = 0
ref_frame = 0
se30 = se3_traj[ref_frame,obj_idx,:]
mask0 = np.load(mask_files[ref_frame])
depth0 = np.load(depth_files[ref_frame])
depth0 = cv2.resize(depth0, None, fx=scale, fy=scale)
pc0 = vision.np_cloud_transformer(depth0, camera, scale = scale)
init_R = se3_to_SE3(se30)[0:3,0:3]
com = vision.get_com(mask0, pc0, obj_idx, init_R = init_R)
g_c_com = se3_to_SE3(com)
g_co = se3_to_SE3(se30)
g_oc = inv_SE3(g_co)
g_o_com = np.matmul(g_oc, g_c_com)
for t in range(len(se3_traj)):
ax.clear()
img = np.load(img_list[t])
img = cv2.resize(img, None, fx = scale, fy = scale)
ax.imshow(img/255.)
for obj_idx in range(nb_object):
xi_co = se3_traj[t,obj_idx,:]
g_co = se3_to_SE3(xi_co)
g_c_com = np.matmul(g_co, g_o_com)
SE3 = np.copy(g_c_com)
T = SE3[0:3,3]
u,v = projection(T, intrinsic)
ax.scatter(u,v, c = 'k')
se3 = SE3_to_se3(SE3)
obj.apply_pose(se3)
s = 0.1 *10
scaled_xbasis = 0.1*(obj.xbasis-obj.orientation)+obj.orientation
scaled_ybasis = 0.1*(obj.ybasis-obj.orientation)+obj.orientation
scaled_zbasis = 0.1*(obj.zbasis-obj.orientation)+obj.orientation
x_u, x_v = projection(scaled_xbasis, intrinsic)
y_u, y_v = projection(scaled_ybasis, intrinsic)
z_u, z_v = projection(scaled_zbasis, intrinsic)
x_u_u = s*(x_u-u) + u
x_v_v = s*(x_v-v) + v
y_u_u = s*(y_u-u) + u
y_v_v = s*(y_v-v) + v
z_u_u = s*(z_u-u) + u
z_v_v = s*(z_v-v) + v
ax.plot([u, x_u_u], [v, x_v_v], c = 'r', linewidth = 3)
ax.plot([u, y_u_u], [v, y_v_v], c = 'g', linewidth = 3)
ax.plot([u, z_u_u], [v, z_v_v], c = 'b', linewidth = 3)
########################################
ax.set_xlim([0, intrinsic.w])
ax.set_ylim([intrinsic.h,0])
fig.savefig(output_path+'/%s.png'%str(t).zfill(10))
video_path = output_path+'/se3_on_image.avi'
util.frame_to_video(output_path, video_path, 'png', fps=fps)()
object_names += ['k_giant_ur3', 'k_zed']
for demo_name in demo_dirs:
se3_dict = {}
for obj in object_names:
vicon_dir = './data/' + task_name + '/' + demo_name + '/vicon/' + obj
vicon_files = sorted(glob.glob(vicon_dir + '/*.npy'))
se3_dict[obj] = []
for f in vicon_files:
se3 = np.load(f)
se3_dict[obj].append(np.expand_dims(se3, 0))
se3_dict[obj] = np.concatenate(se3_dict[obj], 0)
data_len = se3_dict[obj].shape[0]
output_dir = './data/' + task_name + '/' + demo_name + '/vicon_traj'
util.create_dir(output_dir)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
colors = ['r', 'g', 'b']
se3_objects = {}
se3_traj = {}
obj_color = {}
for i, obj in enumerate(object_names):
se3_objects[obj] = vision.SE3object(angle_type='axis')
se3_traj[obj] = np.zeros([0, 3])
obj_color[obj] = colors[i]
for t in range(data_len):
ax.clear()
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
def compare(config):
'''
vicon data is required!
execute 'read_bag' first
'''
task_name = config['data_name']
nb_object = config['object']['nb_object']
supervision = config['pose']['supervision']
fps = config['animation']['fps']
data_dir = './data/' + task_name
pose_dir = './output/read_pose/' + task_name
vicon_dir = './output/vicon/' + task_name
output_dir = './output/compare_traj/' + task_name
util.create_dir(output_dir, clear=True)
demo_list = os.listdir('./data/' + task_name)
for demo in demo_list:
data_demo_dir = data_dir + '/' + demo
vicon_demo_dir = vicon_dir + '/' + demo
cam_pos0 = np.load(data_demo_dir + '/camera_position0.npy',
allow_pickle=True)
cam_pos1 = np.load(data_demo_dir + '/camera_position1.npy',
allow_pickle=True)
robot_pos0 = np.load(data_demo_dir + '/pioneer_position0.npy',
allow_pickle=True)
robot_pos1 = np.load(data_demo_dir + '/pioneer_position1.npy',
allow_pickle=True)
cut_point = np.loadtxt(data_demo_dir + '/cut.txt')
util.create_dir(output_dir + '/' + demo + '/plot', clear=True)
util.create_dir(output_dir + '/' + demo + '/pickle', clear=True)
## open vicon trajectory
vision_traj_list = np.load(pose_dir + '/' + demo + '/pose_traj.npy')
obj_files = sorted(glob.glob(vicon_demo_dir + '/*_pose.npy'))
assert len(obj_files) == 1
vicon_traj_list = []
for obj_file in obj_files:
vicon_traj_loaded = np.load(obj_file, allow_pickle=True)
#for i in range(1,7):
# vicon_traj_loaded[:,i] = medfilt(vicon_traj_loaded[:,i],11)
vicon_traj_list.append(vicon_traj_loaded)
## open cam time
cam_start_time = cam_pos0[0]
cam_finish_time = cam_pos1[0]
## cut point
total_img = cut_point[0]
beginning_cut = cut_point[1]
endding_cut = cut_point[2]
## modify cam start time
play_time = cam_finish_time - cam_start_time
cam_start_time = cam_start_time + play_time * (beginning_cut /
(total_img - 1))
cam_finish_time = cam_finish_time - play_time * (endding_cut /
(total_img - 1))
for i in range(nb_object):
vicon_traj = vicon_traj_list[i]
vision_traj = vision_traj_list[:, i, :]
vicon_time = vicon_traj[:, 0]
vicon_start_idx = 0
vicon_finish_idx = len(vicon_time)
for i in range(len(vicon_time)):
if vicon_time[i] < cam_start_time:
vicon_start_idx = i
if vicon_time[i] < cam_finish_time:
vicon_finish_idx = i
vicon_start_idx += 1
clipped_vicon_time = vicon_time[vicon_start_idx:vicon_finish_idx]
clipped_vicon = vicon_traj[vicon_start_idx:vicon_finish_idx, 1:7]
### align time
len_vision = len(vision_traj)
vision_time = np.arange(
clipped_vicon_time[0], clipped_vicon_time[-1],
(clipped_vicon_time[-1] - clipped_vicon_time[0]) / len_vision)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
vicon_plot = np.zeros((0, 3))
vicon_se3 = np.zeros((0, 6))
vision_plot = np.zeros((0, 3))
vision_se3 = np.zeros((0, 6))
obj_vicon = vision.SE3object(np.zeros(6), angle_type='axis')
obj_vision = vision.SE3object(np.zeros(6), angle_type='axis')
v_t = 1
for t in range(len(vision_traj)): # ah
for tt in range(v_t - 1, len(clipped_vicon_time)):
if clipped_vicon_time[tt] > vision_time[t]:
break
v_t = tt
#print('vision_time:%04.04f'%vision_time[t])
#print('vicon_time:%04.04f'%clipped_vicon_time[v_t])
#print('\n')
## time
vicon_T = se3_to_SE3(clipped_vicon[v_t, :])
vision_T = se3_to_SE3(vision_traj[t, :])
vicon_plot = np.concatenate(
[vicon_plot,
np.expand_dims(vicon_T[0:3, 3], 0)], 0)
vision_plot = np.concatenate(
[vision_plot,
np.expand_dims(vision_T[0:3, 3], 0)], 0)
vicon_se3_element = SE3_to_se3(
se3_to_SE3(clipped_vicon[v_t, :]))
vicon_se3 = np.concatenate(
[vicon_se3,
np.expand_dims(vicon_se3_element, 0)], 0)
vision_se3 = np.concatenate(
[vision_se3,
np.expand_dims(SE3_to_se3(vision_T), 0)], 0)
plt.close('all')
## align translation
x0 = np.random.rand(12)
optimize_len = int(len(vision_plot))
########################## for task3 (occlusion)
#if task_name == 'task3':
# optimize_len = 100
def objective_fn(x0):
SE30 = se3_to_SE3(x0[0:6])
SE31 = se3_to_SE3(x0[6:12])
loss = 0
for t in range(optimize_len):
transformed = np.matmul(
np.matmul(SE30, se3_to_SE3(vision_se3[t, :])), SE31)
transformed_se3 = SE3_to_se3(transformed)
loss += np.sum(
np.square(transformed_se3[:] - vicon_se3[t, :]))
#transformed = un_homo( np.matmul(np.matmul(SE30, to_homo(vision_plot[t,:])),SE31))
#loss += np.sum(np.square(transformed-vicon_plot[t,:]))
return loss
print(demo)
print('initial_loss:' + str(objective_fn(x0)))
if False:
result = np.load(output_dir + '/' + demo +
'/optimization.npy').item()
else:
#'''
result = minimize(objective_fn,
x0,
method='BFGS',
tol=1e-7,
options={
'gtol': 1e-6,
'disp': True
})
#'''
'''
result = minimize(objective_fn,
x0,
method='Nelder-Mead',
options={'maxiter':1})
'''
np.save(output_dir + '/' + demo + '/optimization.npy', result)
print('optimized_loss:' + str(objective_fn(result.x)))
print(result.x)
SE30 = se3_to_SE3(result.x[0:6])
SE31 = se3_to_SE3(result.x[6:12])
### align orientation
#'''
vision_SE30 = np.matmul(
np.matmul(SE30, se3_to_SE3(vision_se3[0, :])), SE31)
vicon_SE30 = se3_to_SE3(vicon_se3[0, :])
R_target = vicon_SE30[0:3, 0:3]
R_ori = vision_SE30[0:3, 0:3]
T_ori = vision_SE30[0:3, 3]
SO3_align = np.matmul(R_target, np.transpose(R_ori))
#####################################################
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
transformed_vision_se3 = np.zeros((0, 6))
transformed_vision_SE3 = np.zeros((0, 4, 4))
transformed_vision_plot = np.zeros((0, 3))
loss = 0
position_error = []
rotation_error = []
total_position = []
total_rotation = []
for t in range(len(vicon_se3)):
## time
vicon_se3_t = vicon_se3[t, :]
vision_se3_t = vision_se3[t, :]
vicon_T_t = se3_to_SE3(vicon_se3[t, :])
vision_T_t = se3_to_SE3(vision_se3[t, :])
############################### not alignment!!!!!!!!!
#if supervision == 'never':
# se3_rc = np.asarray([0.333653152, -0.19545771, -0.41434446, -0.16426212, 0.4854613, -0.28981152],dtype = np.float32)
# g_rc = se3_to_SE3(se3_rc)
# _, g_vr = util.load_vicon(data_demo_dir+'/camera_position0.npy')
# SE3 = np.matmul(g_vr, g_rc)
vision_T_t = np.matmul(np.matmul(SE30, vision_T_t), SE31)
#vision_T_t[0:3,0:3] = np.matmul(SO3_align, vision_T_t[0:3,0:3])
vision_se3_t = SE3_to_se3(vision_T_t)
#IPython.embed()
transformed_vision_se3 = np.concatenate([
transformed_vision_se3,
np.expand_dims(SE3_to_se3(vision_T_t), 0)
], 0)
transformed_vision_plot = np.concatenate([
transformed_vision_plot,
np.expand_dims(vision_T_t[0:3, 3], 0)
], 0)
transformed_vision_SE3 = np.concatenate(
[transformed_vision_SE3,
np.expand_dims(vision_T_t, 0)], 0)
loss += np.sqrt(
np.sum(
np.square(
SE3_to_se3(vicon_T_t) - SE3_to_se3(vision_T_t))))
position_error.append(
np.expand_dims(
np.sqrt(
np.sum(
np.square(vicon_T_t[0:3, 3] -
vision_T_t[0:3, 3]))), 0))
rotation_error.append(
np.expand_dims(
np.sqrt(
np.sum(
np.square(vicon_se3_t[3:6] -
vision_se3_t[3:6]))), 0))
total_position.append(np.expand_dims(vicon_T_t[0:3, 3], 0))
total_rotation.append(np.expand_dims(vicon_se3_t[3:6], 0))
ax.clear()
obj_vicon.apply_pose(vicon_se3_t)
obj_vision.apply_pose(vision_se3_t)
obj_vicon.plot(ax, scale=0.015, linewidth=3)
obj_vision.plot(ax, scale=0.015, linewidth=3)
##
ax.plot(vicon_plot[:t, 0],
vicon_plot[:t, 1],
vicon_plot[:t, 2],
'--',
color='r',
alpha=0.5,
linewidth=4)
ax.plot(transformed_vision_plot[:, 0],
transformed_vision_plot[:, 1],
transformed_vision_plot[:, 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_dir + '/' + demo + '/%05d.png' % t)
plt.close()
loss = loss / len(vision_traj)
position_error = np.sum(position_error) / len(
vision_traj) #np.concatenate(position_error,0)
rotation_error = np.sum(rotation_error) / len(
vision_traj) #np.concatenate(rotation_error,0)
total_position = np.concatenate(total_position, 0)
total_rotation = np.concatenate(total_rotation, 0)
np.savetxt(output_dir + '/' + demo + '/loss.txt', [loss])
np.savetxt(output_dir + '/' + demo + '/position_error.txt',
[position_error])
np.savetxt(output_dir + '/' + demo + '/rotation_error.txt',
[rotation_error])
np.savetxt(output_dir + '/' + demo + '/total_position.txt',
total_position)
np.savetxt(output_dir + '/' + demo + '/total_rotation.txt',
total_rotation)
#################### save data for jigang
pickle_dict = {
'time': np.transpose(vision_time),
'se3': transformed_vision_se3,
'SE3': transformed_vision_SE3
}
io.savemat(output_dir + '/' + demo + '/pickle/trajectories.mat',
pickle_dict)
#####################################################
if len(vision_time) > len(transformed_vision_se3):
vision_time = vision_time[:len(transformed_vision_se3)]
fig = plt.figure()
ax1 = fig.add_subplot(311)
ax1.plot(clipped_vicon_time, clipped_vicon[:, 0], '--')
ax1.plot(vision_time, transformed_vision_se3[:, 0])
ax1.set_title('se3[0:3]')
ax2 = fig.add_subplot(312)
ax2.plot(clipped_vicon_time, clipped_vicon[:, 1], '--')
ax2.plot(vision_time, transformed_vision_se3[:, 1])
ax3 = fig.add_subplot(313)
ax3.plot(clipped_vicon_time, clipped_vicon[:, 2], '--')
ax3.plot(vision_time, transformed_vision_se3[:, 2])
fig.savefig(output_dir + '/' + demo + '/plot/xyz.png')
#####################################################
fig = plt.figure()
ax1 = fig.add_subplot(311)
ax1.plot(clipped_vicon_time, clipped_vicon[:, 3], '--')
ax1.plot(vision_time, transformed_vision_se3[:, 3])
ax1.set_title('se3[3:6]')
ax2 = fig.add_subplot(312)
ax2.plot(clipped_vicon_time, clipped_vicon[:, 4], '--')
ax2.plot(vision_time, transformed_vision_se3[:, 4])
ax3 = fig.add_subplot(313)
ax3.plot(clipped_vicon_time, clipped_vicon[:, 5], '--')
ax3.plot(vision_time, transformed_vision_se3[:, 5])
fig.savefig(output_dir + '/' + demo + '/plot/wxwywz.png')
plt.close()
#####################################################
diff_vicon = (
clipped_vicon[1:, :] - clipped_vicon[:-1, :]) / np.expand_dims(
clipped_vicon_time[1:] - clipped_vicon_time[:-1], 1)
diff_vision = (transformed_vision_se3[1:, :] -
transformed_vision_se3[:-1, :]) / np.expand_dims(
vision_time[1:] - vision_time[:-1], 1)
fig = plt.figure()
ax1 = fig.add_subplot(311)
ax1.plot(clipped_vicon_time[:-1], diff_vicon[:, 0], '--')
ax1.plot(vision_time[:-1], diff_vision[:, 0])
ax1.set_title('diff_se3[0:3]')
ax1.set_ylim([-0.5, 0.5])
ax2 = fig.add_subplot(312)
ax2.plot(clipped_vicon_time[:-1], diff_vicon[:, 1], '--')
ax2.plot(vision_time[:-1], diff_vision[:, 1])
ax2.set_ylim([-0.5, 0.5])
ax3 = fig.add_subplot(313)
ax3.plot(clipped_vicon_time[:-1], diff_vicon[:, 2], '--')
ax3.plot(vision_time[:-1], diff_vision[:, 2])
ax3.set_ylim([-0.5, 0.5])
fig.savefig(output_dir + '/' + demo + '/plot/diff_xyz.png')
plt.close()
#####################################################
fig = plt.figure()
ax1 = fig.add_subplot(311)
ax1.plot(clipped_vicon_time[:-1], diff_vicon[:, 3], '--')
ax1.plot(vision_time[:-1], diff_vision[:, 3])
ax1.set_title('diff_se3[0:3]')
ax1.set_ylim([-0.5, 0.5])
ax2 = fig.add_subplot(312)
ax2.plot(clipped_vicon_time[:-1], diff_vicon[:, 4], '--')
ax2.plot(vision_time[:-1], diff_vision[:, 4])
ax2.set_ylim([-0.5, 0.5])
ax3 = fig.add_subplot(313)
ax3.plot(clipped_vicon_time[:-1], diff_vicon[:, 5], '--')
ax3.plot(vision_time[:-1], diff_vision[:, 5])
ax3.set_ylim([-0.5, 0.5])
fig.savefig(output_dir + '/' + demo + '/plot/diff_wxwywz.png')
plt.close()
#####################################################
video_path = output_dir + '/' + demo + '/se3_compare.avi'
util.frame_to_video(output_dir + '/' + demo,
video_path,
'png',
fps=fps)()