def __init__(self, name): self.name = name # Params inference self.fps = 1 self.period = 1 / self.fps self.batch_size = 1 self.points_sub = 256 # 128 256 512 self.block_sub = 0.2 self.stride_sub = 0.2 self.gpu_index = 0 self.desired_points = 0 self.model_path = "/home/miguel/Desktop/test_po3d/256_22_2/model.ckpt" self.path_cls = "/home/miguel/Desktop/test_po3d/1.txt" self.classes, self.labels, self.label2color = indoor3d_util.get_info_classes( self.path_cls) # TODO importar modelos valvulas, convertirlos a o3d pointclouds, calcular fpfh y meterlos en una lista self.init = False self.new_pc = False # Set image subscriber pc_sub = message_filters.Subscriber( '/stereo_down/scaled_x2/points2_filtered', PointCloud2) #pc_sub = message_filters.Subscriber('/stereo_down/scaled_x2/points2', PointCloud2) info_sub = message_filters.Subscriber('/stereo_down/left/camera_info', CameraInfo) ts_image = message_filters.TimeSynchronizer([pc_sub, info_sub], 10) ts_image.registerCallback(self.cb_pc) # Set class image publisher self.pub_pc_base = rospy.Publisher( "/stereo_down/scaled_x2/points2_base", PointCloud2, queue_size=4) self.pub_pc_seg = rospy.Publisher("/stereo_down/scaled_x2/points2_seg", PointCloud2, queue_size=4) # Set classification timer rospy.Timer(rospy.Duration(self.period), self.run) # CvBridge for image conversion self.bridge = CvBridge()
def eval_one_epoch(sess, ops, room_path, out_data_label_filename, out_gt_label_filename): error_cnt = 0 is_training = False total_correct = 0 total_seen = 0 loss_sum = 0 total_seen_class = [0 for _ in range(NUM_CLASSES)] total_correct_class = [0 for _ in range(NUM_CLASSES)] if parsed_args.visu: fout = open( os.path.join(DUMP_DIR, os.path.basename(room_path)[:-4] + '_pred.obj'), 'w') fout_gt = open( os.path.join(DUMP_DIR, os.path.basename(room_path)[:-4] + '_gt.obj'), 'w') fout_data_label = open(out_data_label_filename, 'w') fout_gt_label = open(out_gt_label_filename, 'w') current_data, current_label = indoor3d_util.room2blocks_wrapper_normalized( room_path, NUM_POINT, block_size=0.1, stride=0.1) current_data = current_data[:, 0:NUM_POINT, :] current_label = np.squeeze(current_label) # Get room dimension.. data_label = np.load(room_path) data = data_label[:, 0:6] max_room_x = max(data[:, 0]) max_room_y = max(data[:, 1]) max_room_z = max(data[:, 2]) file_size = current_data.shape[0] print("--------------- FILE SIZE: " + str(file_size)) num_batches = file_size // BATCH_SIZE print(file_size) for batch_idx in range(num_batches): start_idx = batch_idx * BATCH_SIZE end_idx = (batch_idx + 1) * BATCH_SIZE cur_batch_size = end_idx - start_idx feed_dict = { ops['pointclouds_pl']: current_data[start_idx:end_idx, :, :], ops['labels_pl']: current_label[start_idx:end_idx], ops['is_training_pl']: is_training } loss_val, pred_val = sess.run([ops['loss'], ops['pred_softmax']], feed_dict=feed_dict) if parsed_args.no_clutter: pred_label = np.argmax(pred_val[:, :, 0:12], 2) # BxN else: pred_label = np.argmax(pred_val, 2) # BxN # Save prediction labels to OBJ file for b in range(BATCH_SIZE): pts = current_data[start_idx + b, :, :] l = current_label[start_idx + b, :] pts[:, 6] *= max_room_x pts[:, 7] *= max_room_y pts[:, 8] *= max_room_z pts[:, 3:6] *= 255.0 pred = pred_label[b, :] for i in range(NUM_POINT): g_classes, g_class2label, g_label2color = indoor3d_util.get_info_classes( path_cls) color = g_label2color[pred[i]] color_gt = g_label2color[current_label[start_idx + b, i]] if parsed_args.visu: fout.write('v %f %f %f %d %d %d\n' % (pts[i, 6], pts[i, 7], pts[i, 8], color[0], color[1], color[2])) fout_gt.write('v %f %f %f %d %d %d\n' % (pts[i, 6], pts[i, 7], pts[i, 8], color_gt[0], color_gt[1], color_gt[2])) fout_data_label.write( '%f %f %f %d %d %d %f %d\n' % (pts[i, 6], pts[i, 7], pts[i, 8], pts[i, 3], pts[i, 4], pts[i, 5], pred_val[b, i, pred[i]], pred[i])) fout_gt_label.write('%d\n' % (l[i])) correct = np.sum(pred_label == current_label[start_idx:end_idx, :]) total_correct += correct total_seen += (cur_batch_size * NUM_POINT) loss_sum += (loss_val * BATCH_SIZE) for i in range(start_idx, end_idx): for j in range(NUM_POINT): l = current_label[i, j] total_seen_class[l] += 1 total_correct_class[l] += (pred_label[i - start_idx, j] == l) log_string('eval mean loss: %f' % (loss_sum / float(total_seen / NUM_POINT))) if total_seen > 0: log_string('eval accuracy: %f' % (total_correct / float(total_seen))) fout_data_label.close() fout_gt_label.close() if parsed_args.visu: fout.close() fout_gt.close() return total_correct, total_seen
type=int, default=256, help='Point number sub [default: 4096]') parser.add_argument('--test_name', help='name of the test') parsed_args = parser.parse_args() path_data = parsed_args.path_data path_cls = parsed_args.path_cls model_path = os.path.join(parsed_args.model_path, "model.ckpt") points_sub = parsed_args.points_sub test_name = parsed_args.test_name dump_path = os.path.join(parsed_args.model_path, "dump_" + test_name) if not os.path.exists(dump_path): os.mkdir(dump_path) classes, labels, label2color = indoor3d_util.get_info_classes(path_cls) num_classes = len(classes) batch_size = 1 gpu_index = 0 block_sub = 0.2 stride_sub = 0.2 def evaluate(data, label, xyz_max, sess, ops): is_training = False label = np.squeeze(label) num_batches = data.shape[0] // batch_size
def __init__(self, name): self.name = name # Params inference self.fps = 2 # target fps //PARAM self.period = 1/self.fps # target period //PARAM self.batch_size = 1 # //PARAM self.points_sub = 128 # //PARAM self.block_sub = 0.1 # //PARAM self.stride_sub = 0.1 # //PARAM self.gpu_index = 0 # //PARAM self.desired_points = int(6000/(128/self.points_sub)) # n of points to wich the received pc will be downsampled //PARAM # get valve matching targets self.targets_path = "/home/miguel/Desktop/PIPES2/dgcnn/valve_targets" # //PARAM self.targets_list = list() for file_name in natsorted(os.listdir(self.targets_path)): target_path = os.path.join(self.targets_path, file_name) target = get_info.read_ply(target_path, "model") xyz_central = np.mean(target, axis=0)[0:3] target[:, 0:3] -= xyz_central target[:, 2] *= -1 # flip Z axis target_o3d = o3d.geometry.PointCloud() target_o3d.points = o3d.utility.Vector3dVector(target[:,0:3]) target_o3d.colors = o3d.utility.Vector3dVector(target[:,3:6]) target_o3d.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.01, max_nn=15)) # //PARAM target_o3d.orient_normals_to_align_with_direction(orientation_reference=([0, 0, 1])) # //PARAM self.targets_list.append(target_o3d) # Params get_instance self.col_inst = { 0: [255, 255, 0], 1: [255, 0, 255], 2: [0, 255, 255], 3: [0, 128, 0], 4: [0, 0, 128], 5: [128, 0, 0], 6: [0, 255, 0], 7: [0, 0, 255], 8: [255, 0, 0], 9: [0, 100, 0], 10: [0, 0, 100], 11: [100, 0, 0], 12: [100, 0, 255], 13: [0, 255, 100], 13: [255, 100, 0] } self.rad_p = 0.04 # max distance for pipe growing //PARAM self.rad_v = 0.04 # max distance for valve growing //PARAM self.dim_p = 3 # compute 2D (2) or 3D (3) distance for pipe growing //PARAM self.dim_v = 2 # compute 2D (2) or 3D (3) distance for valve growing //PARAM self.min_p_p = 60 # minimum number of points to consider a blob as a pipe //PARAM self.min_p_v = 30 # 40 80 140 # minimum number of points to consider a blob as a valve //PARAM self.model_path = "/home/miguel/Desktop/PIPES2/dgcnn/sem_seg/RUNS/sparus_xiroi/test/128_11_1/model.ckpt" # path to model //PARAM self.path_cls = "/home/miguel/Desktop/PIPES2/dgcnn/sem_seg/RUNS/sparus_xiroi/test/128_11_1/cls.txt" # path to clases info //PARAM self.classes, self.labels, self.label2color = indoor3d_util.get_info_classes(self.path_cls) # get classes info self.init = False self.new_pc = False # set subscribers pc_sub = message_filters.Subscriber('/stereo_down/scaled_x2/points2_filtered', PointCloud2) # //PARAM #pc_sub = message_filters.Subscriber('/stereo_down/scaled_x2/points2', PointCloud2) # //PARAM pc_sub.registerCallback(self.cb_pc) # Set class image publishers self.pub_pc_base = rospy.Publisher("/stereo_down/scaled_x2/points2_base", PointCloud2, queue_size=4) self.pub_pc_seg = rospy.Publisher("/stereo_down/scaled_x2/points2_seg", PointCloud2, queue_size=4) self.pub_pc_inst = rospy.Publisher("/stereo_down/scaled_x2/points2_inst", PointCloud2, queue_size=4) self.pub_pc_info = rospy.Publisher("/stereo_down/scaled_x2/points2_info", PointCloud2, queue_size=4) # Set segmentation timer rospy.Timer(rospy.Duration(self.period), self.run)