def read_pcd(filename): o3d.set_verbosity_level(VerbosityLevel.Error) pcd = o3d.read_point_cloud(filename) if pcd.colors: return np.concatenate([np.array(pcd.points), np.array(pcd.colors)], 1) else: colors = matplotlib.cm.cool(np.array(pcd.points)[:, 0]) return np.concatenate([np.array(pcd.points), colors[:, 0:3]], 1)
import open3d import tensorflow as tf import numpy as np import os import copy import time from utils.config import Config from datasets.common import Dataset from models.KPFCNN_model import KernelPointFCNN import matplotlib.pyplot as plt import matplotlib.cm as cm open3d.set_verbosity_level(open3d.VerbosityLevel.Error) class MiniDataset(Dataset): def __init__(self, files, voxel_size=0.03): Dataset.__init__(self, 'Mini') self.num_test = 0 self.anc_points = {"train": [], "test": []} self.ids_list = {"train": [], "test": []} for filename in files: pcd = open3d.read_point_cloud(filename) pcd = open3d.voxel_down_sample(pcd, voxel_size=voxel_size) points = np.array(pcd.points) self.anc_points['test'] += [points] self.ids_list['test'] += [filename] self.num_test += 1 def get_batch_gen(self, split, config): def random_balanced_gen():
def AngularDistance(T1, T2): R1 = T1[:3, :3] R2 = T2[:3, :3] R = R1.dot(R2.T) theta = np.arccos(np.clip((np.sum(np.diag(R)) - 1.0) / 2.0, -1, 1)) theta = theta / np.pi * 180.0 terr = np.linalg.norm(T1[:3, 3] - T2[:3, 3], 2) if (theta > 3) or (terr >= 0.3): return True else: return False period = 20 o3d.set_verbosity_level(o3d.VerbosityLevel.Debug) if True: import glob pose_files = glob.glob('stream/*.pose') num_frames = len(pose_files) frame_trace = [i for i in range(num_frames) if i % period == 0] #frame_trace += [i for i in range(450, 480, 2)] #frame_trace += [445] #compareA = args.compareA #对比的两个点云 #compareB = args.compareB frame_trace = sorted(frame_trace) pcd_partial = o3d.PointCloud() last_T = None