def saveVoxels(self, outputPath, save_binvox=False):
""" save voxel
Save the voxel into file.
Args:
outputPath: path to save numpy.
voxel: numpy.ndarray
"""
self._getVoxels()
startPoint = 0
if self._fileName.rfind("/") != -1:
startPoint = self._fileName.rfind("/") + 1
fileName = self._fileName[startPoint:self._fileName.rfind('.')] # cut the format end
# save npy
np.save(os.path.join(outputPath, fileName) + ".npy", self._voxels)
if save_binvox:
# save binvox
bool_voxel = self._voxels.astype(np.bool)
binvox = binvox_rw.Voxels(
data = bool_voxel,
dims = list(self._voxels.shape),
translate = [0.0, 0.0, 0.0],
scale = 1.0,
axis_order = 'xzy')
fp = open(os.path.join(outputPath, fileName) + ".binvox", 'wb+')
fp.truncate()
binvox.write(fp)
fp.close()
def demo():
'''
demo on how to use this class.
'''
import binvox_rw
# Constructor
sc = Shape_complete(verbose=True)
# Read demo binvox as (64*64*64) array
with open('demo/occupy.binvox', 'rb') as f:
occ = binvox_rw.read_as_3d_array(f).data
with open('demo/non_occupy.binvox', 'rb') as f:
non = binvox_rw.read_as_3d_array(f).data
# Complete shape
out = sc.complete(occ=occ, non=non, verbose=False)
# Thresholding. Threshold sets to be 0.5
th = 0.5
out[out >= th] = 1
out[out < th] = 0
# Save to file for demo
vox = binvox_rw.Voxels(out, [64, 64, 64], [0, 0, 0], 1, 'xyz')
with open('demo/output.binvox', 'wb') as f:
vox.write(f)
print('Output saved to demo/output.binvox.')
print(
'Please use ./viewvox demo/output.binvox to visualize the result.')
def pred_to_binvox(voxel_file):
voxel = np.load(voxel_file)[:, :, :, 0]
voxel = voxel > config['voxel_pred_threshold']
bvox = binvox.Voxels(voxel, voxel.shape, [0.0, 0.0, 0.0], 0.684696, 'xyz')
fname = voxel_file.replace('npy', 'binvox')
with open(fname, 'wb') as f:
bvox.write(f)
def voxelizePart(grid, scale, translate, dims, cloud, labels, partId,
outputPath):
bbmin = np.min(cloud, axis=0)
bbmax = np.max(cloud, axis=0)
center = 0.5 * (bbmax - bbmin)
w1s = np.where(grid == 1)
grid_xyz = [[x, y, z] for x, y, z in zip(w1s[0], w1s[1], w1s[2])]
grid_xyz = np.array(grid_xyz)
grid_xyz_sc = []
for p in grid_xyz:
trans_p = [0, 0, 0]
trans_p[0] = scale * ((1 / scale) * center[0] - 0.5 + float(
(p[0] + 0.5) / dims)) + translate[0]
trans_p[1] = scale * ((1 / scale) * center[1] - 0.5 + float(
(p[1] + 0.5) / dims)) + translate[1]
trans_p[2] = scale * ((1 / scale) * center[2] - 0.5 + float(
(p[2] + 0.5) / dims)) + translate[2]
grid_xyz_sc.append(trans_p)
grid_xyz_sc = np.array(grid_xyz_sc)
#grid_xyz_sc is now in the same coordinate frame as the point-cloud
clf = knc(n_neighbors=1)
clf.fit(cloud, labels)
voxelLabels = clf.predict(grid_xyz_sc)
partIndices = voxelLabels == partId
partVoxelIndices = grid_xyz[partIndices, :]
partvox = np.zeros((dims, dims, dims, 1))
partvox[partVoxelIndices[:, 0], partVoxelIndices[:, 2],
partVoxelIndices[:, 1], 0] = 1
partvox = partvox.astype('int')
partbinvox = binvox_rw.Voxels(partvox, (dims, dims, dims), [0, 0, 0], 1,
'xzy')
partname = 'model_' + str(partId) + '.binvox'
binvox_rw.write(partbinvox, open(os.path.join(outputPath, partname), 'wb'))
def pc_to_binvox_for_shape_completion(points, patch_size):
"""
This function creates a binvox object from a pointcloud. The voxel grid is slightly off center from the
pointcloud bbox center so that the back of the grid has more room for the completion.
:type points: numpy.ndarray
:param points: nx3 numpy array representing a pointcloud
:type patch_size: int
:param patch_size: how many voxels along a single dimension of the voxel grid.
Ex: patch_size=40 gives us a 40^3 voxel grid
:rtype: binvox_rw.Voxels
"""
if points.shape[1] != 3:
raise Exception(
"Invalid pointcloud size, should be nx3, but is {}".format(
points.shape))
# how much of the voxel grid do we want our pointcloud to fill.
# make this < 1 so that there is some padding on the edges
PERCENT_PATCH_SIZE = (4.0 / 5.0)
# Where should the center of the points be placed inside the voxel grid.
# normally make PERCENT_Z < 0.5 so that the points are placed towards the front of the grid
# this leaves more room for the shape completion to fill in the occluded back half of the occupancy grid.
PERCENT_X = 0.5
PERCENT_Y = 0.5
PERCENT_Z = 0.45
# get the center of the pointcloud in meters. Ex: center = np.array([0.2, 0.1, 2.0])
center = get_bbox_center(points)
# get the size of an individual voxel. Ex: voxel_resolution=0.01 meaning 1cm^3 voxel
# PERCENT_PATCH_SIZE determines how much extra padding to leave on the sides
voxel_resolution = get_voxel_resolution(points,
PERCENT_PATCH_SIZE * patch_size)
# this tuple is where we want to stick the center of the pointcloud in our voxel grid
# Ex: (20, 20, 18) leaving some extra room in the back half.
pc_center_in_voxel_grid = (patch_size * PERCENT_X, patch_size * PERCENT_Y,
patch_size * PERCENT_Z)
# create a voxel grid.
vox_np = voxelize_points(points=points[:, 0:3],
pc_bbox_center=center,
voxel_resolution=voxel_resolution,
num_voxels_per_dim=patch_size,
pc_center_in_voxel_grid=pc_center_in_voxel_grid)
# location in meters of the bottom corner of the voxel grid in world space
offset = np.array(
center) - np.array(pc_center_in_voxel_grid) * voxel_resolution
# create a voxel grid object to contain the grid, shape, offset in the world, and grid resolution
vox = binvox_rw.Voxels(vox_np, vox_np.shape, tuple(offset),
voxel_resolution * patch_size, "xyz")
return vox
def _save_grid(self, grids, path_and_name, verbose = False, resolution = RESOLUTION):
import binvox_rw
vox = binvox_rw.Voxels(grids, [RESOLUTION, RESOLUTION, RESOLUTION], [0, 0, 0], 1, 'xyz')
with open(path_and_name, 'wb') as f:
vox.write(f)
if verbose:
print('Output saved to ' + path_and_name + '.')
def save_voxel(grid, threshold, path):
data = np.zeros((32, 32, 32), dtype=bool)
data[np.where(grid >= threshold)] = True
vox = binvox_rw.Voxels(data, dims=[32,32,32], translate=[0,0,0], scale=1.0, axis_order='xyz')
with open(path + '.binvox', 'wb') as f:
vox.write(f)
sio.savemat(path + '.mat', {'voxel': data})
def createTestObject(self, dims):
translate = [0, 0, 0]
scale = [1, 1, 1]
axis_order = 'xyz'
data = np.zeros(dims)
voxels = binvox_rw.Voxels(data, dims, translate, scale, axis_order,
"test")
return voxels
def save_voxel(grid, epoch): data = np.zeros((32, 32, 32), dtype=bool) a,b,c = np.where(grid >= threshold) for x, y, z in zip(a, b, c): data[x, y, z] = True vox = binvox_rw.Voxels(data, dims=[32,32,32], translate=[0,0,0], scale=1.0, axis_order='xyz') with open(validviews_path + 'epoch' + str(epoch) + '.binvox', 'wb') as f: vox.write(f)
def meshToBinvox(url,
ext="_pre.ply",
dims=128,
doFilter=False,
normVals=None,
dimVals=None,
axis='xyz'):
shape = (dims, dims, dims)
data = np.zeros(shape, dtype=bool)
translate = (0, 0, 0)
scale = 1
axis_order = axis
bv = binvox_rw.Voxels(data, shape, translate, scale, axis_order)
print("Reading from: " + url)
mesh = trimesh.load(url)
if (normVals != None and dimVals != None):
for vert in mesh.vertices:
vert[0] = remap(vert[0], dimVals[0], dimVals[1],
dims * normVals[0], (dims * normVals[1]) - 1)
vert[1] = remap(vert[1], dimVals[2], dimVals[3],
dims * normVals[2], (dims * normVals[3]) - 1)
vert[2] = remap(vert[2], dimVals[4], dimVals[5],
dims * normVals[4], (dims * normVals[5]) - 1)
else:
mesh.vertices = scale_numpy_array(mesh.vertices, 0, dims - 1)
newMeshUrl = changeExtension(url, ext)
mesh.export(newMeshUrl)
for vert in mesh.vertices:
x = dims - 1 - int(vert[0])
y = int(vert[1])
z = int(vert[2])
data[x][y][z] = True
if (doFilter == True):
for i in range(0, 1): # 1
nd.binary_dilation(bv.data.copy(), output=bv.data)
for i in range(0, 3): # 3
nd.sobel(bv.data.copy(), output=bv.data)
nd.median_filter(bv.data.copy(), size=4, output=bv.data) # 4
for i in range(0, 2): # 2
nd.laplace(bv.data.copy(), output=bv.data)
for i in range(0, 0): # 0
nd.binary_erosion(bv.data.copy(), output=bv.data)
outputUrl = changeExtension(url, ".binvox")
print("Writing to: " + outputUrl)
with open(outputUrl, 'wb') as f:
bv.write(f)
def write_binvox(data, path):
data = np.rint(data).astype(np.uint8)
dims = (opt.img_width, opt.img_height, opt.img_depth) #data.shape
translate = [0, 0, 0]
scale = 1.0
axis_order = 'xzy'
v = binvox_rw.Voxels(data, dims, translate, scale, axis_order)
with open(path, 'bw') as f:
v.write(f)
def callback(msg, args):
file_prefix = args
arr = np.reshape(msg.data, tuple(d.size for d in msg.layout.dim))
occ = arr > 0
non = arr < 0
# Save to file for demo
timestr = datetime.utcnow().strftime("%Y-%m-%d-%H-%M-%S-%f")[:-3]
vox = binvox_rw.Voxels(occ, [64, 64, 64], [0, 0, 0], 1, 'xyz')
filename = file_prefix + '_occupied_' + timestr + '.binvox'
with open(filename, 'wb') as f:
vox.write(f)
print('Output saved to ' + filename + '.')
vox = binvox_rw.Voxels(non, [64, 64, 64], [0, 0, 0], 1, 'xyz')
filename = file_prefix + '_unoccupied_' + timestr + '.binvox'
with open(filename, 'wb') as f:
vox.write(f)
print('Output saved to ' + filename + '.')
def save_binvox(voxel, path):
assert (len(voxel.shape) == 3)
vox = binvox_rw.Voxels(
voxel,
dims=[voxel.shape[0], voxel.shape[1], voxel.shape[2]],
translate=[0, 0, 0],
scale=1.0,
axis_order='xyz')
with open(path, 'wb') as f:
vox.write(f)
def write_binvox(data, path, dims=128):
data = np.rint(data).astype(np.uint8)
shape = (dims, dims, dims) #data.shape
translate = [0, 0, 0]
scale = 1.0
axis_order = 'xzy'
v = binvox_rw.Voxels(data, shape, translate, scale, axis_order)
with open(path, 'bw') as f:
v.write(f)
def callback(msg):
arr = np.reshape(msg.data, tuple(d.size for d in msg.layout.dim))
print rospy.get_name(), "I heard %s" % str(arr)
occ = arr > 0
# Save to file for demo
vox = binvox_rw.Voxels(occ, [64, 64, 64], [0, 0, 0], 1, 'xyz')
with open('demo/output.binvox', 'wb') as f:
vox.write(f)
print('Output saved to demo/output.binvox.')
rospy.signal_shutdown("Got result.")
def generate_binvox_file(voxels, file_path, dimension):
"""
Generate a binvox file from voxels
"""
voxels = torch.round(voxels.view(dimension, dimension, dimension))
arr = voxels.numpy()
size = len(arr)
dims = [size, size, size]
scale = 1.0
translate = [0.0, 0.0, 0.0]
model = binvox_rw.Voxels(arr, dims, translate, scale, 'xyz')
model.write(f'{file_path}.binvox')
def save_binvox(cur_vol, name):
#print(model.dims)
#print(model.translate)
#print(model.scale)
#print(model.axis_order)
cur_model = binvox_rw.Voxels(data=cur_vol,
dims=list(cur_vol.shape),
translate=[0, 0, 0],
scale=1.0,
axis_order='xyz')
with open(name + '.binvox', 'wb') as f:
cur_model.write(f)
def saveVoxels(predicted_voxels, destinationPath, name, tar):
for voxel_array in predicted_voxels:
voxel_path = os.path.join(destinationPath, name + ".binvox")
voxel = binvox_rw.Voxels(voxel_array > 0.5, voxel_array.shape,
(0, 0, 0), 1, 'xyz')
with open(voxel_path, 'wb') as f:
voxel.write(f)
np_path = os.path.join(destinationPath, name + ".npy")
np.save(np_path, voxel_array)
tar.add(voxel_path, name + ".binvox")
tar.add(np_path, name + ".npy")
def pred_to_binvox(voxel_file):
s3 = boto3.resource('s3')
voxel = np.load(voxel_file)[:, :, :, 0]
voxel = voxel > config['voxel_pred_threshold']
bvox = binvox.Voxels(voxel, voxel.shape, [0.0, 0.0, 0.0], 0.684696, 'xyz')
fname = voxel_file.replace('npy', 'binvox')
with open(fname, 'wb') as f:
bvox.write(f)
print('Sending file to s3 : '+str(fname))
s3.meta.client.upload_file(fname, PROCESSED_IMAGE_BUCKET_NAME, ntpath.basename(fname))
print('Deleting file '+str(ntpath.basename(fname))+'raw input files')
os.remove('./demo/input/'+ntpath.basename(fname)[:-7])
def save_voxel(voxel_, filename, THRESHOLD=0.5):
S1 = voxel_.shape[2]
S2 = voxel_.shape[1]
S3 = voxel_.shape[0]
# st()
binvox_obj = binvox_rw.Voxels(np.transpose(voxel_, [2, 1, 0]) >= THRESHOLD,
dims=[S1, S2, S3],
translate=[0.0, 0.0, 0.0],
scale=1.0,
axis_order='xyz')
with open(filename, "wb") as f:
binvox_obj.write(f)
def write_binvox(data, path):
"""
write out voxel data
"""
data = np.rint(data).astype(np.uint8)
dims = data.shape
translate = [0., 0., 0.]
scale = 1.0
axis_order = 'xyz'
v = binvox_rw.Voxels(data, dims, translate, scale, axis_order)
with open(path, 'bw') as f:
v.write(f)
def save_voxel(voxel_, filename, THRESHOLD=0.5):
S1 = voxel_.shape[2]
S2 = voxel_.shape[1]
S3 = voxel_.shape[0]
binvox_obj = binvox_rw.Voxels(
voxel_ >= THRESHOLD, # I am already in xyz
dims=[S1, S2, S3],
translate=[0.0, 0.0, 0.0],
scale=1.0,
axis_order='xyz')
with open(filename, "wb") as f:
binvox_obj.write(f)
f.close()
def save_binvox_input_output(input_cloud, mean_voxel, binvox_folder):
import binvox_rw
import curvox
cnn_voxel = shape_completion_utils.round_voxel_grid_to_0_and_1(mean_voxel)
partial_vox = curvox.pc_vox_utils.pc_to_binvox_for_shape_completion(
points=input_cloud[:, 0:3], patch_size=40)
completed_vox = binvox_rw.Voxels(cnn_voxel, partial_vox.dims,
partial_vox.translate, partial_vox.scale,
partial_vox.axis_order)
binvox_rw.write(partial_vox,
open(binvox_folder + "network_input.binvox", 'w'))
binvox_rw.write(completed_vox,
open(binvox_folder + "network_output.binvox", 'w'))
def save_voxel(depths, silhouettes, path):
data = np.zeros((dim, dim, dim), dtype=bool)
for axis in range(6):
a, b = np.where(silhouettes[axis] == True)
for x, y in zip(a, b):
m, n, p = img2grid(x, y, depths[axis, x, y], dim, axis)
data[m, n, p] = True
# sio.savemat(path, {'voxels': data})
vox = binvox_rw.Voxels(data,
dims=[dim, dim, dim],
translate=[0, 0, 0],
scale=1.0,
axis_order='xyz')
with open(path, 'wb') as f:
vox.write(f)
def process_binvox(outfile, res, prediction, fn, idx):
# output voxel .binvox file
# computer center and scale
t = time.time()
MIN = np.min(prediction, 0)
MAX = np.max(prediction, 0)
translate = (MIN + MAX) * 0.5
translate = [float(x) for x in translate]
scale = np.max(MAX - MIN)
dims = [res, res, res]
order = 'xzy'
voxel_data = ((prediction - translate) / scale * res + (res - 1.0) / 2.0).T
with open(outfile, 'wb') as fout:
binvox_rw.write(
binvox_rw.Voxels(np.ascontiguousarray(voxel_data), dims, translate,
scale, order), fout)
print(fn, idx, voxel_data.shape, ' took %f[s]' % (time.time() - t))
def cnn_and_pc_to_mesh(observed_pc,
cnn_voxel,
filepath,
mesh_name,
model_pose,
log_pc=False,
pc_name=""):
cnn_voxel = round_voxel_grid_to_0_and_1(cnn_voxel)
temp_pcd_handle, temp_pcd_filepath = tempfile.mkstemp(suffix=".pcd")
os.close(temp_pcd_handle)
pcd = np_to_pcl(observed_pc)
pcl.save(pcd, temp_pcd_filepath)
partial_vox = curvox.pc_vox_utils.pc_to_binvox_for_shape_completion(
points=observed_pc[:, 0:3], patch_size=40)
completed_vox = binvox_rw.Voxels(cnn_voxel, partial_vox.dims,
partial_vox.translate, partial_vox.scale,
partial_vox.axis_order)
# Now we save the binvox file so that it can be passed to the
# post processing along with the partial.pcd
temp_handle, temp_binvox_filepath = tempfile.mkstemp(
suffix="output.binvox")
os.close(temp_handle)
binvox_rw.write(completed_vox, open(temp_binvox_filepath, 'w'))
# This is the file that the post-processed mesh will be saved it.
mesh_file = file_utils.create_file(filepath, mesh_name)
# mesh_reconstruction tmp/completion.binvox tmp/partial.pcd tmp/post_processed.ply
# This command will look something like
cmd_str = "mesh_reconstruction" + " " + temp_binvox_filepath + " " + temp_pcd_filepath \
+ " " + mesh_file + " --cuda"
subprocess.call(cmd_str.split(" "), stdout=FNULL, stderr=subprocess.STDOUT)
# subprocess.call(cmd_str.split(" "))
if log_pc:
pcd_file = file_utils.create_file(filepath, pc_name)
cmd_str = "pcl_pcd2ply -format 0" + " " + temp_pcd_filepath + " " + pcd_file
subprocess.call(cmd_str.split(" "),
stdout=FNULL,
stderr=subprocess.STDOUT)
map_object_to_gt(pcd_file, model_pose)
map_object_to_gt(mesh_file, model_pose)
def get_completion(partial_np_pc, transform):
goal = graspit_shape_completion.msg.CompleteMeshGoal()
assert partial_np_pc.shape[0] > 3
assert partial_np_pc.shape[1] == 3
for i in range(partial_np_pc.shape[0]):
point = partial_np_pc[i, :]
goal.partial_mesh.vertices.append(geometry_msgs.msg.Point(*point))
client = actionlib.SimpleActionClient('complete_mesh', graspit_shape_completion.msg.CompleteMeshAction)
client.wait_for_server()
client.send_goal(goal)
client.wait_for_result()
result = client.get_result()
data = np.ones((len(result.completed_mesh.vertices), 4))
for i, point in enumerate(result.completed_mesh.vertices):
x = point.x
y = point.y
z = point.z
data[i] = np.array((x, y, z, 1))
data_of = data[:]
trans_frame = PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, 0),
PyKDL.Vector(0, 0, -1))
trans_matrix = tf_conversions.posemath.toMatrix(trans_frame)
data = np.dot(trans_matrix, data.T).T
data_of = np.dot(transform, data_of.T).T
data = data[:, 0:3]
data_of = data_of[:, 0:3]
completed_pcd_cf = pcl.PointCloud(np.array(data, np.float32))
completed_pcd_of = pcl.PointCloud(np.array(data_of, np.float32))
vg = np.array(result.voxel_grid).reshape((PATCH_SIZE, PATCH_SIZE, PATCH_SIZE))
vox = binvox_rw.Voxels(vg,
(PATCH_SIZE, PATCH_SIZE, PATCH_SIZE),
(result.voxel_offset_x, result.voxel_offset_y, result.voxel_offset_z -1 ),
result.voxel_resolution * PATCH_SIZE,
"xyz")
return completed_pcd_cf, completed_pcd_of, vox
def generate_binvox_file(voxels):
"""
Given a 3D tensor, create a binvox file
"""
data = voxels.numpy()
size = len(data[0])
for x in range(size):
for y in range(size):
for z in range(size):
data[x][y][z] = 1.0 if data[x][y][z] > 0.5 else 0.0
size = len(voxels[0])
dims = [size, size, size]
scale = 1.0
translate = [0.0, 0.0, 0.0]
output = io.BytesIO()
model = binvox_rw.Voxels(data, dims, translate, scale, 'xyz')
model.write(output)
output.seek(0)
return output
def pcd2binvox(pcd_filepath, binvox_filepath, grid_dim):
'''
This function takes a pcd filepath, and converts it to
a binvox file. This does not voxeliz the point cloud,
it assumes each point can be represented as indices
ex cloud:
pts = [(0,1,0), (0,10,23), ...]
would be converted to a binvox with voxels
(0,1,0) and (0 , 10, 23) occupied
'''
pc = pcl.load(pcd_filepath)
pc_arr = pc.to_array().astype(int)
pc_mask = (pc_arr[:,0], pc_arr[:,1], pc_arr[:,2])
out = np.zeros((grid_dim, grid_dim, grid_dim))
out[pc_mask] = 1
out_int = out.astype(int)
bv = binvox_rw.Voxels(out_int, out_int.shape, (0,0,0),1, 'xyz')
binvox_rw.write(bv, open(binvox_filepath, 'w'))
def _saveVoxel(filename,
outputJsonPath, outputNumpyPath, outputBinvoxPath, voxel):
""" save voxel
Save the voxel into file.
Args:
filename: the filename of import.
outputJsonPath: path to save json.
outputNumpyPath: path to save numpy.
outputBinvoxPath: path to save binvox.
voxel: numpy.ndarray
"""
startPoint = 0
if filename.rfind("/") != -1:
startPoint = filename.rfind("/") + 1
filename = filename[startPoint:filename.rfind('.')] # cut the format end
# save npy
#voxel.tofile(outputNumpyPath + filename + ".numpy")
np.save(os.path.join( outputNumpyPath, filename ) + ".npy", voxel)
# save binvox
bool_voxel = voxel.astype(np.bool)
binvox = binvox_rw.Voxels(
data = bool_voxel,
dims = list(voxel.shape),
translate = [0.0, 0.0, 0.0],
scale = 1.0,
axis_order = 'xzy')
fp = open(os.path.join( outputBinvoxPath, filename ) + ".binvox", 'wb+')
fp.truncate()
binvox.write(fp)
fp.close()
# save json
array = voxel.reshape(-1,)
json_str = json.dumps(array.tolist())
json_file = open(os.path.join( outputJsonPath, filename ) + ".json", "w+")
json_file.truncate() # 清空当前文件的内容
json_file.write(json_str)
json_file.close()
