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
