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)()