def main():
argvs = sys.argv
model_num = str(argvs[1])
scene_num = str(argvs[2])
cloud_dir = "/mnt/container-data/remove_plane/"
model_path = cloud_dir + model_num + ".ply"
model_cloud = o3.read_point_cloud(model_path)
scene_path = cloud_dir + scene_num + ".ply"
scene_cloud = o3.read_point_cloud(scene_path)
rot_in = read_rotation(model_num)
model_cloud.transform(rot_in)
model_cloud = o3.voxel_down_sample(model_cloud, voxel_size=0.020)
scene_cloud = o3.voxel_down_sample(scene_cloud, voxel_size=0.020)
# 基準とするpointcloud, 回転させたいpointcloud の順番
cbs = [callbacks.Open3dVisualizerCallback(model_cloud, scene_cloud)]
objective_type = 'pt2pt'
# 基準とするpointcloud, 回転させたいpointcloud の順番
tf_param, _, _ = filterreg.registration_filterreg(
model_cloud,
scene_cloud,
scene_cloud.normals,
objective_type=objective_type,
sigma2=sig,
callbacks=cbs,
maxiter=ter,
tol=ol)
write_rotation(tf_param, scene_num)
def estimateTransform(self, source, target):
source = od.read_point_cloud(source)
target = od.read_point_cloud(target)
current_transformation = self.base_transform
for scale in range(len(self.voxel_radius)):
iterations = self.max_iter[scale]
radius = self.voxel_radius[scale]
source_down = od.voxel_down_sample(source, radius)
target_down = od.voxel_down_sample(target, radius)
od.estimate_normals(
source_down,
od.KDTreeSearchParamHybrid(radius=2 * radius,
max_nn=self.max_nn))
od.estimate_normals(
target_down,
od.KDTreeSearchParamHybrid(radius=2 * radius,
max_nn=self.max_nn))
result_icp = od.registration_colored_icp(
source_down, target_down, radius, current_transformation,
od.ICPConvergenceCriteria(
relative_fitness=self.relative_fitness,
relative_rmse=self.relative_rmse,
max_iteration=iterations))
current_transformation = result_icp.transformation
# self.draw_registration_result_original_color(
# source_down, target_down, current_transformation)
return current_transformation
def main():
depth_paths, T, pose_paths = getData(args.shapeid)
n = len(depth_paths)
print('found %d clean depth images...' % n)
intrinsic = np.array([[525.0,0,319.5],[0,525.0,239.5],[0,0,1]])
np.random.seed(816)
indices = np.random.permutation(n)
print(indices[:100])
#indices = sorted(indices)
make_dirs(PATH_MAT.format(args.shapeid, 0))
import open3d
pcd_combined = open3d.PointCloud()
for i, idx in enumerate(indices):
import ipdb; ipdb.set_trace()
print('%d / %d' % (i, len(indices)))
mesh = Mesh.read(depth_paths[idx], mode='depth', intrinsic = intrinsic)
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(mesh.vertex.T)
pcd.transform(inverse(T[idx]))
#pcd = open3d.voxel_down_sample(pcd, voxel_size=0.02)
pcd_combined += pcd
pcd_combined = open3d.voxel_down_sample(pcd_combined, voxel_size=0.02)
sio.savemat(PATH_MAT.format(args.shapeid, i), mdict={
'vertex': mesh.vertex,
'validIdx_rowmajor': mesh.validIdx,
'pose': T[idx],
'depth_path': depth_paths[idx],
'pose_path': pose_paths[idx]})
if i <= 50 and i >= 40:
pcd_combined_down = open3d.voxel_down_sample(pcd_combined, voxel_size=0.02)
open3d.draw_geometries([pcd_combined_down])
pcd_combined_down = open3d.voxel_down_sample(pcd_combined, voxel_size=0.02)
open3d.draw_geometries([pcd_combined_down])
def clusterComponents(pier, pierCap, voxel_size, plot, start, begining):
#attempt to cluster the component point clouds (pier used for testing)
start = clock_msg('Clustering Piers', start, begining)
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(pier)
pcd = open3d.voxel_down_sample(pcd, voxel_size=0.1)
xyz = np.asarray(pcd.points)
clusterPier, labelsPier = salan_dbscan.hdbscan_fun(xyz, plot)
start = clock_msg('Clustering PierCaps', start, begining)
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(pierCap)
pcd = open3d.voxel_down_sample(pcd, voxel_size=0.1)
xyz = np.asarray(pcd.points)
clusterPierCap, labelsPierCap = salan_dbscan.hdbscan_fun(xyz, plot)
#using the clustering data, find the bounding boxes for each
#cluster and extract those points from the FULL point set
scale = 0
bounds = np.zeros((len(clusterPier), 4))
for k in range(len(clusterPier)):
bounds[k] = xyBounds(clusterPier[k], scale)
cpFull, index = extract(pier, bounds)
#cluster the pierCap next
scale = 0
bounds = np.zeros((len(clusterPierCap), 4))
for k in range(len(clusterPierCap)):
bounds[k] = xyBounds(clusterPierCap[k], scale)
cpcFull, index = extract(pierCap, bounds)
return cpFull, cpcFull, start
def update_source(self, camera_point_cloud):
"""
Function to add new camera frame data to the source point cloud
Parameters: self.source_pcl
camera_point_cloud (open3d.PointCloud) : point cloud from the camera frame
"""
camera_pcl = copy.deepcopy(camera_point_cloud)
camera_pcl = pn.voxel_down_sample(camera_pcl, 1)
print len(np.asarray(camera_pcl.points))
if self.source_pcl.is_empty():
self.source_pcl = self.source_pcl + camera_pcl
return
else:
icp_result = icp_registration(self.source_pcl, camera_pcl, 3)
icp_result.transformation = icp_result.transformation
self.previous_transformations.append(icp_result.transformation)
self.source_pcl.transform(icp_result.transformation)
self.source_pcl = self.source_pcl + camera_pcl
self.source_pcl = pn.voxel_down_sample(self.source_pcl, 1)
# pn.draw_geometries([self.source_pcl])
if self.source_to_target_transformation.any():
self.source_to_target_transformation = \
self.source_to_target_transformation.dot(icp_result.transformation)
if len(self.previous_transformations) > 300:
self.previous_transformations.pop(0)
return
def registration_point_to_plane(scene1, scene2, voxel_size):
# scene1 and 2 are point cloud data
# voxel_size is grid size
#draw_registration_result(scene1,scene2,np.identity(4))
# voxel down sampling
scene1_down = open3d.voxel_down_sample(scene1, voxel_size)
scene2_down = open3d.voxel_down_sample(scene2, voxel_size)
#draw_registration_result(scene1_down,scene2_down,np.identity(4))
# estimate normal with search radius voxel_size*2.0
radius_normal = voxel_size * 2.0
open3d.estimate_normals(
scene1, open3d.KDTreeSearchParamHybrid(radius=radius_normal,
max_nn=30))
open3d.estimate_normals(
scene2, open3d.KDTreeSearchParamHybrid(radius=radius_normal,
max_nn=30))
open3d.estimate_normals(
scene1_down,
open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
open3d.estimate_normals(
scene2_down,
open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
# compute fpfh feature with search radius voxel_size/2.0
radius_feature = voxel_size * 2.0
scene1_fpfh = open3d.compute_fpfh_feature(
scene1_down,
search_param=open3d.KDTreeSearchParamHybrid(radius=radius_feature,
max_nn=100))
scene2_fpfh = open3d.compute_fpfh_feature(
scene2_down,
search_param=open3d.KDTreeSearchParamHybrid(radius=radius_feature,
max_nn=100))
# compute ransac registration
ransac_result = execute_global_registration(scene1_down, scene2_down,
scene1_fpfh, scene2_fpfh,
voxel_size)
#draw_registration_result(scene1,scene2,ransac_result.transformation)
# point to point ICP resigtration
distance_threshold = voxel_size * 10.0
result = open3d.registration_icp(
scene1, scene2, distance_threshold, ransac_result.transformation,
open3d.TransformationEstimationPointToPlane(),
open3d.ICPConvergenceCriteria(max_iteration=1000))
#draw_registration_result(scene1,scene2,result.transformation)
print(result)
return result
def _prepare(self, voxel_size):
source = open3d.PointCloud()
source.points = open3d.Vector3dVector(self._pcd_depth)
target = open3d.PointCloud()
target.points = open3d.Vector3dVector(self._pcd_cad)
source = open3d.voxel_down_sample(source, voxel_size=voxel_size)
target = open3d.voxel_down_sample(target, voxel_size=voxel_size)
return source, target
def test(tr_src, train_tgt, tst_src, gt, output, mean, std):
tensor_x = torch.Tensor(tst_src)
tensor_y = torch.stack([torch.Tensor(i) for i in gt])
# print(tensor_y.shape)
testset = utils.TensorDataset(tensor_x, tensor_y)
testloader = utils.DataLoader(testset)
loss = 0.0
test_size = 0
saved_data = np.zeros(gt.shape)
with torch.no_grad():
for data in testloader:
# print(saved_data)
inputs, targets = data[0].to(device), data[1].to(device)
outputs = posemlp(inputs)
loss += criterion(outputs, targets[0]).item()
saved_data[test_size] = outputs.cpu().numpy()
test_size += 1
saved_data = saved_data * std + mean
print(saved_data)
print('Finished testing with average loss %.6f.' % (loss/test_size))
gt = gt * std + mean
train_tgt = train_tgt * std + mean
print('Dumping data to ' + output)
np.save(output, saved_data)
pred_pcd = o3d.PointCloud()
pred_pcd.points = o3d.Vector3dVector(saved_data[..., :3])
pred_pcd = o3d.voxel_down_sample(pred_pcd, voxel_size=0.05)
pt_cnt = np.asarray(pred_pcd.points).size/3
pred_pcd.paint_uniform_color([0, 0, 1])
colors = np.zeros((pt_cnt, 3))
colors[..., 2] = np.ones(pt_cnt)
tr_src = np.around(tr_src, decimals=4)
tst_src = np.around(tst_src, decimals=4)
for i in range(pt_cnt):
if tst_src[i] in tr_src:
np.asarray(pred_pcd.colors)[i, 2] = 0
np.asarray(pred_pcd.colors)[i, 0] = 1
print(np.asarray(pred_pcd.colors))
gt_pcd = o3d.PointCloud()
gt_pcd.points = o3d.Vector3dVector(gt)
gt_pcd = o3d.voxel_down_sample(gt_pcd, voxel_size=0.05)
gt_pcd.paint_uniform_color([0, 1, 0])
tr_pcd = o3d.PointCloud()
tr_pcd.points = o3d.Vector3dVector(train_tgt)
tr_pcd = o3d.voxel_down_sample(tr_pcd, voxel_size=0.05)
tr_pcd.paint_uniform_color([1, 0, 0])
o3d.visualization.draw_geometries([pred_pcd, gt_pcd, tr_pcd])
def extract_points(points,
voxel_size=0.01,
x_range=(-5, 5),
y_range=(-2.2, 5.8),
z_range=(-5, -1.2),
i_range=(2, 8)):
#x_range= (-5, 5), y_range= (-2.2, 5.8),
x, y, z, i = points[:, 0], points[:, 1], points[:, 2], points[:, 3]
i = i * 255
mean = i.mean()
std = i.std()
i_range = (mean + i_range[0] * std, mean + i_range[1] * std)
in_range = box_in_range(x, y, z, i, x_range, y_range, z_range, i_range)
points = points[in_range]
pcd = open3d.geometry.PointCloud()
pcd.points = open3d.utility.Vector3dVector(points[:, :3])
pcd = open3d.voxel_down_sample(pcd, voxel_size)
pts_3d = np.asarray(pcd.points).astype(np.float32)
return pts_3d
def downsample_point_clouds():
cfg = parse_arguments()
vox_size = cfg.downsample_voxel_size
synth_set = cfg.synth_set
inp_dir = os.path.join(cfg.inp_dir, synth_set)
files = glob.glob('{}/*.mat'.format(inp_dir))
out_dir = cfg.out_dir
out_synthset = os.path.join(out_dir, cfg.synth_set)
mkdir_if_missing(out_synthset)
for k, model_file in enumerate(files):
print("{}/{}".format(k, len(files)))
file_name = os.path.basename(model_file)
sample_name, _ = os.path.splitext(file_name)
obj = scipy.io.loadmat(model_file)
out_filename = "{}/{}.mat".format(out_synthset, sample_name)
if os.path.isfile(out_filename):
print("already exists:", sample_name)
continue
Vgt = obj["points"]
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(Vgt)
downpcd = open3d.voxel_down_sample(pcd, voxel_size=vox_size)
down_xyz = np.asarray(downpcd.points)
scipy.io.savemat(out_filename, {"points": down_xyz})
def load_ply(self, data_dir, ind, downsample, aligned=True):
pcd = open3d.read_point_cloud(join(data_dir, f'{ind}.ply'))
pcd = open3d.voxel_down_sample(pcd, voxel_size=downsample)
if aligned is True:
matrix = np.load(join(data_dir, f'{ind}.pose.npy'))
pcd.transform(matrix)
return pcd
def plot_data(fgt, log_id):
gt = np.load('img_data/' + log_id + '/' + fgt)[..., :3]
gt_pcd = o3d.PointCloud()
gt_pcd.points = o3d.Vector3dVector(gt)
gt_pcd = o3d.voxel_down_sample(gt_pcd, voxel_size=0.05)
gt_pcd.paint_uniform_color([1, 0, 0])
# colors = np.zeros((gt.size/3, 3))
for i in range(gt.size / 300):
# if i in missed:
if True:
vis = o3d.visualization.Visualizer()
vis.create_window()
tmp = o3d.PointCloud()
tmp.points = o3d.Vector3dVector(gt[i * 100:(i + 1) * 100])
vis.add_geometry(tmp)
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
# image = vis.capture_screen_float_buffer()
# o3d.write_image('%.5d.png' % (i), image)
vis.capture_screen_image('%.7d.png' % (i), do_render=False)
vis.destroy_window()
# np.asarray(gt_pcd.colors)[i*100:(i+1)*100] = i*300.0/gt.size*np.ones(3)
np.asarray(gt_pcd.colors)[i * 100:(i + 1) * 100] = np.asarray(
[0, 0, 1])
# gt_pcd.colors = o3d.Vector3dVector(colors)
print(np.asarray(gt_pcd.colors))
def o3d_estimate_normals(source):
# downsample and estimate normals
source = o3d.voxel_down_sample(source, voxel_size = 2.5)
o3d.estimate_normals(source, search_param = o3d.KDTreeSearchParamHybrid(
radius = 5, max_nn = 30))
return source
def NonFirstCloud(self, cloud_index):
self.cloud1 = o3d.read_point_cloud(
r"C:\Users\newsmart\Desktop\NSTAlgo\3D\PCL\stitch\2\2-{}_Extract.ply".format(cloud_index - 1))
self.cloud2 = o3d.read_point_cloud(
r"C:\Users\newsmart\Desktop\NSTAlgo\3D\PCL\stitch\2\2-{}_Extract.ply".format(cloud_index))
# get local transformation between two lastest clouds
self.posLocalTrans = self.registerLocalCloud(self.cloud1, self.cloud2)
# if result is not good, drop it
if self.goodResultFlag:
# test for loop detection info
self.detectTransLoop = np.dot(self.posLocalTrans, self.detectTransLoop)
# print ("==== loop detect trans ====")
# print(self.detectTransLoop)
# print ("==== ==== ==== ==== ==== ====")
self.posWorldTrans = np.dot(self.posWorldTrans, self.posLocalTrans)
# update latest cloud
self.cloud1 = copy.deepcopy(self.cloud2)
self.cloud2.transform(self.posWorldTrans)
o3d.write_point_cloud(r"C:\Users\newsmart\Desktop\NSTAlgo\3D\PCL\stitch\2\test.ply",
self.cloud2, write_ascii=False)
self.cloud_base = self.cloud_base + self.cloud2
# Down Sampling
self.cloud_base = o3d.voxel_down_sample(self.cloud_base, 0.001)
self.registrationCount += 1
# save PCD file to local
o3d.write_point_cloud(r"C:\Users\newsmart\Desktop\NSTAlgo\3D\PCL\stitch\2\registerResult.ply",
self.cloud_base, write_ascii=False)
def setUp(self):
pcd = o3.read_point_cloud('data/horse.ply')
pcd = o3.voxel_down_sample(pcd, voxel_size=0.01)
self._source = np.asarray(pcd.points)
rot = trans.euler_matrix(*np.random.uniform(0.0, np.pi / 4, 3))
self._tf = tf.RigidTransformation(rot[:3, :3], np.zeros(3))
self._target = self._tf.transform(self._source)
def main():
script_dir = os.getcwd()
config = pyreg.functions.read_config()
bunny_raw = PointCloud()
try:
bunny_raw.read_pcd(config["files"]["source"])
except (SystemExit, KeyboardInterrupt):
raise
except Exception:
raise Exception("File not found!")
bunny_raw.center_around_origin()
bunny_raw_open3d = open3d.PointCloud()
bunny_raw_open3d.points = open3d.Vector3dVector(bunny_raw.points)
bunny_open3d = open3d.voxel_down_sample(
bunny_raw_open3d, voxel_size=config["downsample"]["voxel_size"])
bunny = PointCloud(
points=copy.deepcopy(numpy.asarray(bunny_open3d.points)))
mlab.figure()
bunny_pts = mlab.points3d(bunny.points[:, 0],
bunny.points[:, 1],
bunny.points[:, 2],
color=(0, 1, 0),
mode="point")
mlab.outline()
ipdb.set_trace()
bunny.write_pcd(file_format="txt",
file_path=config["files"]["output"] + "bunny_7000.xyz")
def sample_point_cloud_feature(point_cloud: op3.PointCloud,
voxel_size: float,
verbose=False):
"""
Down sample and sample the point cloud feature
:param point_cloud: an object of Open3D
:param voxel_size: a float value, that is how sparse you want the sample points is
:param verbose: a boolean value, display notification and visualization when True and no notification when False
:return: 2 objects of Open3D, that are down-sampled point cloud and point cloud feature fpfh
"""
if verbose: print(":: Downsample with a voxel size %.3f." % voxel_size)
point_cloud_down_sample = op3.voxel_down_sample(point_cloud, voxel_size)
radius_normal = voxel_size * 2
if verbose:
print(":: Estimate normal with search radius %.3f." % radius_normal)
op3.estimate_normals(
point_cloud_down_sample,
op3.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
radius_feature = voxel_size * 5
if verbose:
print(":: Compute FPFH feature with search radius %.3f." %
radius_feature)
point_cloud_fpfh = op3.compute_fpfh_feature(
point_cloud_down_sample,
op3.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
return point_cloud_down_sample, point_cloud_fpfh
def prepare_source_and_target_rigid_3d(source_filename,
noise_amp=0.001,
n_random=500,
orientation=np.deg2rad([0.0, 0.0,
30.0]),
normals=False):
source = o3.read_point_cloud(source_filename)
source = o3.voxel_down_sample(source, voxel_size=0.005)
print(source)
target = copy.deepcopy(source)
tp = np.asarray(target.points)
rg = 1.5 * (tp.max(axis=0) - tp.min(axis=0))
rands = (np.random.rand(n_random, 3) - 0.5) * rg + tp.mean(axis=0)
target.points = o3.Vector3dVector(
np.r_[tp + noise_amp * np.random.randn(*tp.shape), rands])
ans = trans.euler_matrix(*orientation)
target.transform(ans)
if normals:
o3.estimate_normals(source,
search_param=o3.geometry.KDTreeSearchParamHybrid(
radius=0.1, max_nn=50))
o3.orient_normals_to_align_with_direction(source)
o3.estimate_normals(target,
search_param=o3.geometry.KDTreeSearchParamHybrid(
radius=0.1, max_nn=50))
o3.orient_normals_to_align_with_direction(target)
return source, target
def ViewPLY(path):
# 読み込み
pointcloud = open3d.read_point_cloud(path)
# ダウンサンプリング
# 法線推定できなくなるので不採用
pointcloud = open3d.voxel_down_sample(pointcloud, 10)
# 法線推定
open3d.estimate_normals(pointcloud,
search_param=open3d.KDTreeSearchParamHybrid(
radius=20, max_nn=30))
# 法線の方向を視点ベースでそろえる
open3d.orient_normals_towards_camera_location(pointcloud,
camera_location=np.array(
[0., 10., 10.],
dtype="float64"))
# 可視化
open3d.draw_geometries([pointcloud])
# numpyに変換
points = np.asarray(pointcloud.points)
normals = np.asarray(pointcloud.normals)
print("points:{}".format(points.shape[0]))
X, Y, Z = Disassemble(points)
# OBB生成
_, _, length = buildOBB(points)
print("OBB_length: {}".format(length))
return points, X, Y, Z, normals, length
def create_graph_pyramid(args, cloud, pyramid_conf):
""" Builds a pyramid of graphs and pooling operations corresponding to progressively coarsened point cloud using voxelgrid.
Parameters:
pyramid_conf: list of tuples (grid resolution, neigborhood radius/k), defines the pyramid
"""
graphs = []
pooldata = []
prev_res = pyramid_conf[0][
0] # assuming the caller performed the initial sampling.
kd = None
for res, rad in pyramid_conf:
if prev_res != res:
cloud_new = open3d.voxel_down_sample(cloud, voxel_size=res)
poolmap, kd = create_pooling_map(cloud, cloud_new)
cloud = cloud_new
graph, kd = create_graph(cloud, args.pc_knn, rad, kd)
graphs.append(graph)
if prev_res != res:
pooldata.append((poolmap, graphs[-2], graphs[-1]))
prev_res = res
return graphs, pooldata
def filter_edges(pcl, radius_shearch=10, intensity=.5):
pcl = pn.voxel_down_sample(pcl, radius_shearch / 5)
pcl_points = np.asarray(pcl.points)
pcl_tree = pn.KDTreeFlann(pcl)
results = []
bar = tqdm.tqdm(total=len(pcl_points))
for i in range(len(pcl.points)):
[k, idx, _] = pcl_tree.search_radius_vector_3d(pcl.points[i],
radius_shearch)
nearest_points = pcl_points[idx]
tmp = nearest_points - pcl_points[i]
scalar_product = tmp.dot(tmp.T)
norm = np.sqrt(np.diag(scalar_product))
tmp = np.array([tmp[i] / norm[i] for i in range(len(tmp))])[1:]
scalar_product = abs(tmp.dot(tmp.T))
if len(scalar_product) > 0:
results.append(scalar_product.mean())
else:
results.append(0)
bar.update(1)
bar.close()
results = np.array(results)
id_edge = np.argwhere(
results < np.percentile(results, int(intensity * 100)))[:, 0]
resutls = pn.PointCloud()
resutls.points = pn.Vector3dVector(pcl_points[id_edge])
return resutls
def voxel_downsample(pts, voxel_size):
pc = o3d.PointCloud()
# print(pts.shape)
pc.points = o3d.Vector3dVector(pts.cpu().numpy())
v_pc = o3d.voxel_down_sample(pc, voxel_size=voxel_size)
v_pts = torch.Tensor(np.asarray(v_pc.points)).to(pts.device)
idx = square_distance(v_pts.unsqueeze(0), pts.unsqueeze(0))[0].topk(k=1, dim=1, largest=False)[1].view(-1)
return idx
def generate_map(self): map_cloud = open3d.PointCloud() for keyframe in self.keyframes: transformed = copy.deepcopy(keyframe.cloud) transformed.transform(keyframe.node.pose) map_cloud += transformed return open3d.voxel_down_sample(map_cloud, voxel_size=0.05)
def downsample(pcd, voxel_size):
pcd_open3d = open3d.PointCloud()
pcd_open3d.points = open3d.Vector3dVector(pcd.points)
pcd_open3d = open3d.voxel_down_sample(pcd_open3d, voxel_size=voxel_size)
pcd = PointCloud(points=copy.deepcopy(numpy.asarray(pcd_open3d.points)))
return pcd
def voxel(data):
mesh = Param["mesh"]
if mesh == 0: return data
if len(data) < 10: return data
d = data.astype(np.float32)
pc = o3d.PointCloud()
pc.points = o3d.Vector3dVector(d)
dwpc = o3d.voxel_down_sample(pc, voxel_size=mesh)
return np.reshape(np.asarray(dwpc.points), (-1, 3))
def downsample_pcd(pcd, voxel_size):
""" Use voxel downsampling from Open3D on PointCloud object """
pcd_open3d = open3d.PointCloud()
pcd_open3d.points = open3d.Vector3dVector(pcd.points)
pcd_open3d = open3d.voxel_down_sample(pcd_open3d, voxel_size=voxel_size)
pcd = PointCloud(points=numpy.asarray(pcd_open3d.points))
return pcd
def setUp(self):
pcd = o3.read_point_cloud('data/horse.ply')
pcd = o3.voxel_down_sample(pcd, voxel_size=0.01)
estimate_normals(pcd, o3.geometry.KDTreeSearchParamHybrid(radius=0.01, max_nn=10))
self._source = np.asarray(pcd.points)
rot = trans.euler_matrix(*np.random.uniform(0.0, np.pi / 4, 3))
self._tf = tf.RigidTransformation(rot[:3, :3], np.zeros(3))
self._target = self._tf.transform(self._source)
self._target_normals = np.asarray(np.dot(pcd.normals, self._tf.rot.T))
def uniform_downsample(self, voxel_size):
pcd_open3d = open3d.PointCloud()
pcd_open3d.points = open3d.Vector3dVector(copy.deepcopy(self.points))
pcd_open3d = open3d.voxel_down_sample(pcd_open3d,
voxel_size=voxel_size)
self.points = numpy.asarray(pcd_open3d.points)
return
def down_sampled_numpy(cloud):
"""
This method is used to get the down sampled point cloud
:param cloud: Point cloud which is to be down sampled [type: point cloud]
:return: down sampled point cloud [type: numpy asarry]
"""
down = o3d.voxel_down_sample(o3d.io.read_point_cloud(cloud),
voxel_size=0.009)
return np.asarray(down.points)
def update(vis=None):
global all_snapshots, packing_cost, packing_iterations_performed, min_packing_iterations, old_snapshot_downsampled, new_snapshot_downsampled
snapshot_index = check_for_new_images()
if snapshot_index == -1 or (len(all_snapshots) > 1 and packing_iterations_performed < min_packing_iterations):
# no new snapshots to combine, perform packing:
if len(all_snapshots) > 1:
transformation, packing_cost, new_snapshot_downsampled = packing_utils.iteration(old_snapshot_downsampled, new_snapshot_downsampled, packing_cost)
packing_iterations_performed += 1
if transformation is not None:
all_snapshots[-1].transform(transformation)
else:
print('.', end='')
return False
else:
print('Not enough snapshots for packing')
return False
else:
# process new snapshot
snapshot_pointcloud = process_new_snapshot(snapshot_index)
if snapshot_pointcloud is None:
print('Got empty snapshot.')
return False
print('\n\n New snapshot: {} \n'.format(len(all_snapshots) + 1))
packing_iterations_performed = 0
packing_cost = inf
# apply initial random translation
random_translation = transform_utils.get_random_translation(initial_offset)
snapshot_pointcloud.transform(random_translation)
all_snapshots.append(snapshot_pointcloud)
# downsampled pointclouds for packing behind the screen:
# merge previous new with the rest
if new_snapshot_downsampled is not None:
if old_snapshot_downsampled is not None:
old_snapshot_downsampled = np.vstack((old_snapshot_downsampled, new_snapshot_downsampled))
else:
old_snapshot_downsampled = new_snapshot_downsampled
# get new downsampled snapshot
new_snapshot_downsampled = np.asarray(open3d.voxel_down_sample(snapshot_pointcloud, voxel_size = 0.05).points)
if vis is not None:
vis.add_geometry(snapshot_pointcloud)
if vis is not None:
vis.update_geometry()
vis.update_renderer()
print(':', end='')
return False
