from util import savePLY, saveVoxels
tfrecord_filename = sys.argv[1]
voxel_scale = float(sys.argv[2]) #0.047 or 0.094 or 0.188
it = tf_record_iterator(tfrecord_filename)
if sys.argv[1].startswith('vis'):
foldername = os.path.dirname(tfrecord_filename)
else:
foldername = '.'
n = next(it)
for k in ['input_sdf', 'target_df', 'prediction_df']:
if k in n:
A = n[k]
s = n[k + '/dim']
sdf = A.reshape(s).copy()
# sdf = skimage.draw.ellipsoid(6,10,16,levelset=True)
print('Loaded sdf', sdf.shape, sdf.min(), sdf.max())
vertices, faces, normals, values = skimage.measure.marching_cubes_lewiner(
sdf, 1)
V = numpy.ones((len(vertices), 6)) * 255
V[:, :3] = vertices * voxel_scale
savePLY(
'%s/%d_%s.ply' % (foldername, int(round(voxel_scale, 2) * 100), k),
V, faces)
saveVoxels('%s/voxel_%d_%s.ply' %
(foldername, int(round(voxel_scale, 2) * 100), k),
sdf,
voxel_scale,
cross_section=True)
pcd_with_labels[:, 6] = instance_labels
pcd_with_labels[:, 7] = class_labels
pcd_with_labels = point_cloud2.create_cloud(
msg.header, fields, pcd_with_labels)
bag.write('laser_cloud_surround', pcd_with_labels, t=t)
dt = (msg.header.stamp.to_sec() - init_time)
print("t:%.2f" % dt, pcd.shape, count_msg, len(instance_set),
'instances', numpy.sum(instance_labels > 0), 'labels')
rospy.init_node('bag_converter')
listener = tf.TransformListener()
subscribed_topic = '/full_cloud_projected'
global_cloud_sub = rospy.Subscriber(subscribed_topic, PointCloud2,
process_cloud)
print('Listening to %s ...' % subscribed_topic)
rospy.spin()
if velodyne_to_faro is None:
global_cloud = numpy.array(global_cloud)
print(global_cloud.shape)
global_cloud = numpy.hstack(
(global_cloud, numpy.zeros((len(global_cloud), 3))))
print(global_cloud.shape)
global_cloud = downsample(global_cloud, 0.05)
print(global_cloud.shape)
savePLY('viz/global_cloud.ply', global_cloud)
else:
bag.close()
points = points[:, :6]
print(len(points[0, :]))
print('Loaded', len(points), 'points')
points_XYZ = points[:, [0, 2, 1]] #swap Y&Z coordinates
synthetic_points_XYZ = fill_holes(points_XYZ,
max_circumradius=0.7,
max_ratio_radius_area=0.02,
distance=0.01)
print('Created', len(synthetic_points_XYZ), 'synthetic points')
synthetic_points_XYZ = synthetic_points_XYZ[:, [0, 2,
1]] #swap Y&Z coordinates
synthetic_points = numpy.zeros((len(synthetic_points_XYZ), 6))
synthetic_points_color = points[:, 3:6].mean(axis=0)
print("Apply color", synthetic_points_color)
synthetic_points[:, :3] = synthetic_points_XYZ
synthetic_points[:, 3:6] = synthetic_points_color
result_points = numpy.vstack((synthetic_points, points))
savePLY('./Data/02_pettit_input_hole_filled.ply', result_points)
tuple(p) for p in numpy.round((cloud[:, :3] / resolution)).astype(int)
]
voxel_set = set()
downsampled_cloud = []
for i in range(len(cloud)):
if not voxel_coordinates[i] in voxel_set:
voxel_set.add(voxel_coordinates[i])
downsampled_cloud.append(cloud[i])
return numpy.array(downsampled_cloud)
dirname = sys.argv[1] #directory containing separate raw scans
building_name = sys.argv[2]
output_dir = dirname + "/.."
scans = []
filelist = []
filelist.extend(glob.glob("%s/*.ply" % dirname))
filelist = sorted(filelist)
for s in filelist:
pcd, _ = loadPLY(s)
print(s, pcd.shape)
scans.append(pcd)
for L in range(1, len(scans)):
for l in itertools.combinations(range(len(scans)), L):
stacked = numpy.vstack([scans[i] for i in l])
stacked = downsample(stacked, 0.01)
output_file = '%s/%s_real%s.ply' % (output_dir, building_name, ''.join(
[str(i) for i in l]))
savePLY(output_file, stacked)
import numpy
from util import savePLY
numpy.random.seed(0)
#create point cloud from wall (XZ plane)
N = 10000
pcd = numpy.zeros((N, 6))
pcd[:,0] = numpy.around(numpy.random.random(N), decimals=3) #random X coordinate
pcd[:,2] = numpy.around(numpy.random.random(N), decimals=3) #random Z coordinate
pcd[:,3:6] = 255 #white color
savePLY('wall.ply', pcd)
#create artificial hole
hole_position = [0.75, 0, 0.75]
hole_radius = 0.2
r = numpy.sqrt((pcd[:,0]-hole_position[0])**2 + (pcd[:,2]-hole_position[2])**2)
savePLY('wall_with_hole.ply', pcd[r > hole_radius, :])
#fill in hole with color
pcd_color = pcd.copy()
pcd_color[r <= hole_radius, 3:6] = [255, 0, 0]
savePLY('wall_with_color.ply', pcd_color)
#add noise
pcd_noise = pcd.copy()
pcd_noise[:,:3] += numpy.random.randn(N,3)*0.01
savePLY('wall_with_noise.ply', pcd_noise)
def fit_plane_LSE_RANSAC(points, iters=100, inlier_thresh=0.05):
# points: Nx4 homogeneous 3d points
# return:
# plane: 1d array of four elements [a, b, c, d] of ax+by+cz+d = 0
# inlier_list: 1d array of size N of inlier points
# make into homogeneous coordinates
color = points[:, 3:]
p = np.hstack((points[:, :3], np.ones((points.shape[0], 1))))
fitted = np.zeros((0, points.shape[1]))
for _ in range(100):
if points.shape[0] < 500:
break
max_inlier_num = -1
max_inlier_list = None
N = points.shape[0]
assert N >= 3
for i in range(iters):
chose_id = np.random.choice(N, 3, replace=False)
chose_points = p[chose_id, :]
tmp_plane = fit_plane_LSE(chose_points)
dists = get_point_dist(p, tmp_plane)
tmp_inlier_list = np.where(dists < inlier_thresh)[0]
tmp_inliers = p[tmp_inlier_list, :]
num_inliers = tmp_inliers.shape[0]
if num_inliers > max_inlier_num:
max_inlier_num = num_inliers
max_inlier_list = tmp_inlier_list
final_points = p[max_inlier_list, :]
plane = fit_plane_LSE(final_points)
# assign random color to segmented plane
#c = np.ones((final_points.shape[0],3)) * np.random.randint(256, size=(1,3))
c = points[max_inlier_list, 3:]
sav = np.hstack((final_points[:, :3], c))
# set color as mean of plane color
color = np.mean(c, 0)
# points to be filled
pts = sav[:, [0, 2, 1]]
hole = fill_holes(pts,
distance=0.05,
max_circumradius=0.2,
max_ratio_radius_area=0.1)
hole = hole[:, [0, 2, 1]]
# add color to hole points
hole = np.hstack((hole, np.ones(hole.shape) * color))
# add to already fitted point cloud
fitted = np.concatenate((fitted, sav, hole))
fit_variance = np.var(get_point_dist(final_points, plane))
#print('RANSAC fit variance: %f' % fit_variance)
dists = get_point_dist(p, plane)
select_thresh = inlier_thresh * 1
inlier_list = np.where(dists < select_thresh)[0]
# remove fitted points from original point cloud
points = np.delete(points, max_inlier_list, 0)
p = np.delete(p, max_inlier_list, 0)
util.savePLY('plane_fitted.ply', fitted)
return points
output_pc[i, 2] = (v_f[i] / density / bb1.half_xyz[2] -
1.0) * bb1.half_xyz[2]
#RGB color
output_pc[i, 3:6] = filled_image[v_f[i], u_f[i], :]
pstack = np.array(pstack)
q, _ = flann.nn(x.astype(np.int32),
pstack.astype(np.int32),
1,
algorithm='kdtree_simple')
for i in range(len(idx)):
min_v, min_u = x[q[i]]
output_pc[idx[i], 1] = depth_image[min_v, min_u]
output_pc[:, :3] += center
#flip z axis
output_pc[:, 2] = -output_pc[:, 2]
original_pc, _ = loadPLY('../input/%s_input.ply' % test_filename)
stacked_pc = list(original_pc)
voxel_resolution = 0.05
original_voxels = set([
tuple(k) for k in np.round((original_pc[:, :3] /
voxel_resolution)).astype(int)
])
added_voxels = [
tuple(k) for k in np.round((output_pc[:, :3] /
voxel_resolution)).astype(int)
]
for i in range(len(added_voxels)):
if not added_voxels[i] in original_voxels:
stacked_pc.append(output_pc[i])
savePLY('%s_output.ply' % test_filename, stacked_pc)
tf.train.Feature(int64_list=tf.train.Int64List(value=voxels.shape)),
'target_df':
tf.train.Feature(float_list=tf.train.FloatList(
value=voxels.flatten().tolist()))
}
example = tf.train.Example(features=tf.train.Features(feature=out_feature))
writer.write(example.SerializeToString())
print('Saved to %s' % sys.argv[2])
saveVoxels('/home/jd/Desktop/voxels.ply', voxels, voxel_scale, offset)
vertices, faces, normals, values = skimage.measure.marching_cubes_lewiner(
voxels, 1)
V = numpy.zeros((len(vertices), 6)) * 255
V[:, :3] = vertices * voxel_scale
V[:, :3] += offset
savePLY('/home/jd/Desktop/marching_cubes.ply', V, faces)
sys.exit(1)
import mesh_to_sdf
import trimesh
import matplotlib.pyplot as plt
voxels = mesh_to_sdf.mesh_to_voxels(mesh, 100, pad=False)
print('voxels', voxels.shape, voxels.min(), voxels.max())
vertices, faces, normals, values = skimage.measure.marching_cubes_lewiner(
voxels, 0)
V = np.zeros((len(vertices), 6)) * 255
V[:, :3] = vertices * voxel_scale
savePLY('/home/jd/Desktop/voxels.ply', V, faces)
import numpy
import sys
from util import loadPLY, savePLY
import os
gt_filename = sys.argv[1] #ground truth combined point cloud
building_name = sys.argv[2] #mason_east
pcd, _ = loadPLY(gt_filename)
print(gt_filename, pcd.shape)
minDims = pcd[:, :3].min(axis=0)
maxDims = pcd[:, :3].max(axis=0)
N = 20
for i in range(N):
#create an interval between 10% and 90% of extent
r1, r2 = numpy.random.random(2) * 0.8 + 0.1
x1 = minDims[0] + (maxDims[0] - minDims[0]) * min(r1, r2)
x2 = minDims[0] + (maxDims[0] - minDims[0]) * max(r1, r2)
r1, r2 = numpy.random.random(2) * 0.8 + 0.1
z1 = minDims[2] + (maxDims[2] - minDims[2]) * min(r1, r2)
z2 = minDims[2] + (maxDims[2] - minDims[2]) * max(r1, r2)
xmask = numpy.logical_or(pcd[:, 0] < x1, pcd[:, 0] > x2)
zmask = numpy.logical_or(pcd[:, 2] < z1, pcd[:, 2] > z2)
output = pcd[numpy.logical_or(xmask, zmask)]
output_file = '%s/%s_synthetic%02d.ply' % (os.path.dirname(gt_filename),
building_name, i)
savePLY(output_file, output)
