def __init__(self, sdf_data, origin, resolution, use_abs=True, T_sdf_world=RigidTransform(from_frame='sdf', to_frame='world')):
self.data_ = sdf_data
self.origin_ = origin
self.resolution_ = resolution
self.dims_ = self.data_.shape
# set up surface params
self.surface_thresh_ = self.resolution_ * np.sqrt(2) / 2 # resolution is max dist from surface when surf is orthogonal to diagonal grid cells
spts, _ = self.surface_points()
self.center_ = 0.5 * (np.min(spts, axis=0) + np.max(spts, axis=0))
self.points_buf_ = np.zeros([Sdf3D.num_interpolants, 3], dtype=np.int)
self.coords_buf_ = np.zeros([3,])
self.pts_ = None
# tranform sdf basis to grid (X and Z axes are flipped!)
t_world_grid = self.resolution_ * self.center_
s_world_grid = 1.0 / self.resolution_
t_grid_sdf = self.origin
self.T_grid_sdf_ = SimilarityTransform(translation=t_grid_sdf,
scale=self.resolution,
from_frame='grid',
to_frame='sdf')
self.T_sdf_world_ = T_sdf_world
self.T_grid_world_ = self.T_sdf_world_ * self.T_grid_sdf_
self.T_sdf_grid_ = self.T_grid_sdf_.inverse()
self.T_world_grid_ = self.T_grid_world_.inverse()
self.T_world_sdf_ = self.T_sdf_world_.inverse()
# optionally use only the absolute values (useful for non-closed meshes in 3D)
self.use_abs_ = use_abs
if use_abs:
self.data_ = np.abs(self.data_)
self._compute_gradients()
def test_similarity_transformation(self):
R_a_b = RigidTransform.random_rotation()
t_a_b = RigidTransform.random_translation()
s_a_b = 2 * np.random.rand()
R_b_c = RigidTransform.random_rotation()
t_b_c = RigidTransform.random_translation()
s_b_c = 2 * np.random.rand()
T_a_b = SimilarityTransform(R_a_b, t_a_b, s_a_b, "a", "b")
T_b_c = SimilarityTransform(R_b_c, t_b_c, s_b_c, "b", "c")
T_b_a = T_a_b.inverse()
x_a = np.random.rand(3)
p_a = Point(x_a, "a")
p_a2 = T_b_a * T_a_b * p_a
self.assertTrue(np.allclose(p_a.data, p_a2.data))
p_b = T_a_b * p_a
p_b2 = s_a_b * (R_a_b.dot(p_a.data)) + t_a_b
self.assertTrue(np.allclose(p_b.data, p_b2))
p_c = T_b_c * T_a_b * p_a
p_c2 = s_b_c * (R_b_c.dot(p_b2)) + t_b_c
self.assertTrue(np.allclose(p_c.data, p_c2))
v_a = np.random.rand(3)
v_a = v_a / np.linalg.norm(v_a)
v_a = Direction(v_a, "a")
v_b = T_a_b * v_a
v_b2 = R_a_b.dot(v_a.data)
self.assertTrue(np.allclose(v_b.data, v_b2))
def __init__(self,
sdf_data,
origin,
resolution,
use_abs=False,
T_sdf_world=RigidTransform(from_frame='sdf',
to_frame='world')):
self.data_ = sdf_data
self.origin_ = origin
self.resolution_ = resolution
self.dims_ = self.data_.shape
# set up surface params
self.surface_thresh_ = self.resolution_ * np.sqrt(2) / 2
self.surface_points_ = None
self.surface_points_w_ = None
self.surface_vals_ = None
self._compute_surface_points()
# resolution is max dist from surface when surf is orthogonal to diagonal grid cells
spts, _ = self.surface_points()
self.center_ = 0.5 * (np.min(spts, axis=0) + np.max(spts, axis=0))
self.points_buf_ = np.zeros([Sdf3D.num_interpolants, 3], dtype=np.int)
self.coords_buf_ = np.zeros([
3,
])
self.pts_ = None
# tranform sdf basis to grid (X and Z axes are flipped!)
t_world_grid = self.resolution_ * self.center_
s_world_grid = 1.0 / self.resolution_
# FIXME: Since in autolab_core==0.0.4, it applies (un)scale transformation before translation in SimilarityTransform
# here we shoule use unscaled origin to get the correct world coordinates
# PS: in world coordinate, the origin here is the left-bottom-down corner of the padded bounding squre box
t_grid_sdf = self.origin
self.T_grid_sdf_ = SimilarityTransform(translation=t_grid_sdf,
scale=self.resolution,
from_frame='grid',
to_frame='sdf')
self.T_sdf_world_ = T_sdf_world
self.T_grid_world_ = self.T_sdf_world_ * self.T_grid_sdf_
self.T_sdf_grid_ = self.T_grid_sdf_.inverse()
self.T_world_grid_ = self.T_grid_world_.inverse()
self.T_world_sdf_ = self.T_sdf_world_.inverse()
# optionally use only the absolute values (useful for non-closed meshes in 3D)
self.use_abs_ = use_abs
if use_abs:
self.data_ = np.abs(self.data_)
self._compute_gradients()
self.surface_points_w_ = self.transform_pt_grid_to_obj(
self.surface_points_.T).T
surface, _ = self.surface_points(grid_basis=True)
self.surface_for_signed_val = surface[np.random.choice(
len(surface), 1000)] # FIXME: for speed
def rescale(self, scale):
""" Rescales uniformly by a given factor.
Parameters
----------
scale : float
the amount to scale the object
Returns
-------
:obj:`GraspableObject3D`
the graspable object rescaled by the given factor
"""
stf = SimilarityTransform(scale=scale)
sdf_rescaled = self.sdf_.rescale(scale)
mesh_rescaled = self.mesh_.transform(stf)
convex_pieces_rescaled = None
if self.convex_pieces_ is not None:
convex_pieces_rescaled = []
for convex_piece in self.convex_pieces_:
convex_piece_rescaled = convex_piece.transform(stf)
convex_pieces_rescaled.append(convex_piece_rescaled)
return GraspableObject3D(sdf_rescaled,
mesh_rescaled,
key=self.key,
model_name=self.model_name,
mass=self.mass,
convex_pieces=convex_pieces_rescaled)
def sample(self, size=1):
""" Sample random variables from the model.
Parameters
----------
size : int
number of sample to take
Returns
-------
:obj:`list` of :obj:`GraspableObject3D`
sampled graspable objects from the pose random variable
"""
samples = []
for i in range(size):
num_consecutive_failures = 0
prev_len = len(samples)
while len(samples) == prev_len:
try:
# sample random pose
obj_copy = copy.deepcopy(self.obj_)
xi = self.r_xi_rv_.rvs(size=1)
S_xi = skew(xi)
R = self.R_sample_sigma_.dot(
scipy.linalg.expm(S_xi).dot(
self.R_sample_sigma_.T.dot(
self.mean_T_obj_world_.rotation)))
s = max(self.s_rv_.rvs(size=1)[0], 0)
t = self.R_sample_sigma_.dot(self.t_rv_.rvs(size=1).T).T
z = self.R_sample_sigma_.dot(self.com_rv_.rvs(size=1))
sample_tf = SimilarityTransform(rotation=R.T,
translation=t,
scale=s)
z_tf = sample_tf * Point(z, frame=sample_tf.from_frame)
z_tf = z_tf.data
# transform object by pose
obj_sample = obj_copy.transform(sample_tf)
obj_sample.mesh.center_of_mass = z_tf
samples.append(obj_sample)
except Exception as e:
num_consecutive_failures += 1
if num_consecutive_failures > 3:
raise
# not a list if only 1 sample
if size == 1 and len(samples) > 0:
return samples[0]
return samples
def main():
# initialize logging
logging.getLogger().setLevel(31)
# parse args
parser = argparse.ArgumentParser(
description='Annotate Thingiverse Dataset Models',
epilog='Written by Matthew Matl (mmatl)')
parser.add_argument('--config',
help='config filename',
default='cfg/tools/rescaler.yaml')
args = parser.parse_args()
# read config
config_filename = args.config
config = YamlConfig(config_filename)
# get gripper mesh
gripper_filename = config['gripper_filename']
gripper_mesh = trimesh.load_mesh(gripper_filename)
# get metadata information
identifier_key = config['identifier_key']
identifier_value = config['identifier_value']
scale_key = config['scale_key']
default_scale = config['default_scale']
override = config['override']
ds = ThingiverseDataset(config['dataset_dir'])
for i, thing_id in enumerate(ds.keys):
thing = None
thing_metadata = ds.metadata(thing_id)
changed_model_keys = []
for model_id in thing_metadata['models']:
model_data = thing_metadata['models'][model_id]
# If the identifier isn't in the model's metadata, skip it
if identifier_key not in model_data['metadata'] or model_data[
'metadata'][identifier_key] != identifier_value:
continue
# If we're overriding or the scale key hasn't been set, modify the model
if override or scale_key not in model_data['metadata']:
# Load the model
if thing is None:
thing = ds[thing_id]
model = thing[model_id]
logging.log(
31, u"{} ({}): {} ({})".format(thing.name, thing.id,
model.name,
model.id).encode('utf-8'))
changed_model_keys.append(model.id)
# Rescale back to original dimensions if overriding
if scale_key in model.metadata:
model.mesh.apply_scale(1.0 / model.metadata[scale_key])
model.metadata[scale_key] = default_scale
# Visualize the model, registering the grow/shrink callbacks
stf = SimilarityTransform(from_frame='world', to_frame='world')
rot = RigidTransform(from_frame='world', to_frame='world')
registered_keys = {
'j': (rescale_callback, ['model', rot, stf, 0.1]),
'k': (rescale_callback, ['model', rot, stf, -0.1]),
'u': (rescale_callback, ['model', rot, stf, 1.0]),
'i': (rescale_callback, ['model', rot, stf, -1.0]),
'h': (rotate_callback, ['model', rot, stf])
}
vis.figure()
vis.mesh(gripper_mesh,
T_mesh_world=RigidTransform(translation=(0, 0, -0.08),
from_frame='obj',
to_frame='world'),
style='surface',
color=(0.3, 0.3, 0.3),
name='gripper')
vis.mesh(model.mesh, style='surface', name='model')
vis.show(animate=True, registered_keys=registered_keys)
# Transform the model and update its metadata
model.mesh.apply_transform(stf.matrix)
model.metadata[scale_key] = stf.scale
if thing:
ds.save(thing, only_metadata=False, model_keys=changed_model_keys)
logging.log(31, '{}/{} things...'.format(i, len(ds.keys)))
def main():
# initialize logging
logging.getLogger().setLevel(31)
parser = argparse.ArgumentParser(
description='Annotate Thingiverse Dataset Models',
epilog='Written by Matthew Matl (mmatl)')
parser.add_argument('--config',
help='config filename',
default='cfg/tools/fixer.yaml')
args = parser.parse_args()
config_filename = args.config
config = YamlConfig(config_filename)
ds = ThingiverseDataset(config['dataset_dir'])
scale_key = config['scale_key']
score_key = config['scale_key']
cache_dir = config['cache_dir']
thing_ids = [
'562693', '2338550', '90830', '2587480', '2158012', '1484054',
'1363175', '708889', '1906394'
]
#for i, thing_id in enumerate(ds.keys):
for i, thing_id in enumerate(thing_ids):
logging.log(31, '{}/{} things...'.format(i, len(ds.keys)))
# Load a thing
thing = ds[thing_id]
# Check each of its models for watertightness
needs_redownload = False
for model in thing.models:
if not model.mesh.is_watertight:
needs_redownload = True
break
if not needs_redownload:
logging.log(31, 'Thing {} ok'.format(thing.id))
continue
logging.log(31, 'Redownloading thing {}'.format(thing.id))
# Re-download the thing if needed
new_thing = Thing.retrieve(thing.id, cache_dir)
if new_thing is None:
print "THING {} WAS NONE, SHOULD DELETE!!!".format(thing.id)
continue
# Create mapping from new model ids to old model ids
mid_map = {}
basename_to_cc_map = {}
for model in new_thing.models:
baseid = re.search('(.*)_cc_[0-9]*$', model.id)
if baseid is None:
if model.id in thing.model_keys:
mid_map[model.id] = model.id
else:
baseid = baseid.group(1)
# Retrieve map from vertex counts to cc's
if baseid not in basename_to_cc_map:
basename_to_cc_map[baseid] = {}
for old_model in thing.models:
if re.search('.*_cc_[0-9]*', old_model.id):
basename_to_cc_map[baseid][len(
old_model.mesh.vertices)] = old_model.id
# Find closest connected component to remap
n_verts = len(model.mesh.vertices)
if n_verts in basename_to_cc_map[baseid]:
mid_map[model.id] = basename_to_cc_map[baseid][n_verts]
else:
print "HI"
# For each model, update metadata and rescale if needed
for model in new_thing.models:
if not model.id in mid_map:
model.metadata[score_key] = 0
else:
old_id = mid_map[model.id]
old_metadata = thing[old_id].metadata
model.metadata.update(old_metadata)
if scale_key in old_metadata:
stf = SimilarityTransform(scale=old_metadata[scale_key])
model.mesh.apply_transform(stf.matrix)
ds.save(new_thing)
class Sdf3D(Sdf):
# static indexing vars
num_interpolants = 8
min_coords_x = [0, 2, 3, 5]
max_coords_x = [1, 4, 6, 7]
min_coords_y = [0, 1, 3, 6]
max_coords_y = [2, 4, 5, 7]
min_coords_z = [0, 1, 2, 4]
max_coords_z = [3, 5, 6, 7]
def __init__(self, sdf_data, origin, resolution, use_abs=True, T_sdf_world=RigidTransform(from_frame='sdf', to_frame='world')):
self.data_ = sdf_data
self.origin_ = origin
self.resolution_ = resolution
self.dims_ = self.data_.shape
# set up surface params
self.surface_thresh_ = self.resolution_ * np.sqrt(2) / 2 # resolution is max dist from surface when surf is orthogonal to diagonal grid cells
spts, _ = self.surface_points()
self.center_ = 0.5 * (np.min(spts, axis=0) + np.max(spts, axis=0))
self.points_buf_ = np.zeros([Sdf3D.num_interpolants, 3], dtype=np.int)
self.coords_buf_ = np.zeros([3,])
self.pts_ = None
# tranform sdf basis to grid (X and Z axes are flipped!)
t_world_grid = self.resolution_ * self.center_
s_world_grid = 1.0 / self.resolution_
t_grid_sdf = self.origin
self.T_grid_sdf_ = SimilarityTransform(translation=t_grid_sdf,
scale=self.resolution,
from_frame='grid',
to_frame='sdf')
self.T_sdf_world_ = T_sdf_world
self.T_grid_world_ = self.T_sdf_world_ * self.T_grid_sdf_
self.T_sdf_grid_ = self.T_grid_sdf_.inverse()
self.T_world_grid_ = self.T_grid_world_.inverse()
self.T_world_sdf_ = self.T_sdf_world_.inverse()
# optionally use only the absolute values (useful for non-closed meshes in 3D)
self.use_abs_ = use_abs
if use_abs:
self.data_ = np.abs(self.data_)
self._compute_gradients()
def transform(self, delta_T):
""" Creates a new SDF with a given pose with respect to world coordinates.
Parameters
----------
delta_T : :obj:`autolab_core.RigidTransform`
transform from cur sdf to transformed sdf coords
"""
new_T_sdf_world = self.T_sdf_world_ * delta_T.inverse().as_frames('sdf', 'sdf')
return Sdf3D(self.data_, self.origin_, self.resolution_, use_abs=self.use_abs_,
T_sdf_world=new_T_sdf_world)
def _signed_distance(self, coords):
"""Returns the signed distance at the given coordinates, interpolating
if necessary.
Parameters
----------
coords : :obj:`numpy.ndarray` of int
A 3-dimensional ndarray that indicates the desired
coordinates in the grid.
Returns
-------
float
The signed distance at the given coords (interpolated).
Raises
------
IndexError
If the coords vector does not have three entries.
"""
pass
if len(coords) != 3:
raise IndexError('Indexing must be 3 dimensional')
if self.is_out_of_bounds(coords):
logging.debug('Out of bounds access. Snapping to SDF dims')
# snap to grid dims
self.coords_buf_[0] = max(0, min(coords[0], self.dims_[0] - 1))
self.coords_buf_[1] = max(0, min(coords[1], self.dims_[1] - 1))
self.coords_buf_[2] = max(0, min(coords[2], self.dims_[2] - 1))
# regular indexing if integers
if np.issubdtype(type(coords[0]), np.integer) and \
np.issubdtype(type(coords[1]), np.integer) and \
np.issubdtype(type(coords[2]), np.integer):
return self.data_[int(self.coords_buf_[0]), int(self.coords_buf_[1]), int(self.coords_buf_[2])]
# otherwise interpolate
min_coords = np.floor(self.coords_buf_)
max_coords = min_coords + 1 # assumed to be on grid
self.points_buf_[Sdf3D.min_coords_x, 0] = min_coords[0]
self.points_buf_[Sdf3D.max_coords_x, 0] = max_coords[0]
self.points_buf_[Sdf3D.min_coords_y, 1] = min_coords[1]
self.points_buf_[Sdf3D.max_coords_y, 1] = max_coords[1]
self.points_buf_[Sdf3D.min_coords_z, 2] = min_coords[2]
self.points_buf_[Sdf3D.max_coords_z, 2] = max_coords[2]
# bilinearly interpolate points
sd = 0.0
for i in range(Sdf3D.num_interpolants):
p = self.points_buf_[i,:]
if self.is_out_of_bounds(p):
v = 0.0
else:
v = self.data_[p[0], p[1], p[2]]
w = np.prod(-np.abs(p - self.coords_buf_) + 1)
sd = sd + w * v
return sd
def __getitem__(self, coords):
"""Returns the signed distance at the given coordinates.
Parameters
----------
coords : :obj:`numpy.ndarray` of int
A or 3-dimensional ndarray that indicates the desired
coordinates in the grid.
Returns
-------
float
The signed distance at the given coords (interpolated).
Raises
------
IndexError
If the coords vector does not have three entries.
"""
return self._signed_distance(coords)
def gradient(self, coords):
"""Returns the SDF gradient at the given coordinates, interpolating if necessary
Parameters
----------
coords : :obj:`numpy.ndarray` of int
A 3-dimensional ndarray that indicates the desired
coordinates in the grid.
Returns
-------
float
The gradient at the given coords (interpolated).
Raises
------
IndexError
If the coords vector does not have three entries.
"""
if len(coords) != 3:
raise IndexError('Indexing must be 3 dimensional')
# log warning if out of bounds access
if self.is_out_of_bounds(coords):
logging.debug('Out of bounds access. Snapping to SDF dims')
# snap to grid dims
self.coords_buf_[0] = max(0, min(coords[0], self.dims_[0] - 1))
self.coords_buf_[1] = max(0, min(coords[1], self.dims_[1] - 1))
self.coords_buf_[2] = max(0, min(coords[2], self.dims_[2] - 1))
# regular indexing if integers
if type(coords[0]) is int and type(coords[1]) is int and type(coords[2]) is int:
self.coords_buf_ = self.coords_buf_.astype(np.int)
return self.data_[self.coords_buf_[0], self.coords_buf_[1], self.coords_buf_[2]]
# otherwise interpolate
min_coords = np.floor(self.coords_buf_)
max_coords = min_coords + 1
self.points_buf_[Sdf3D.min_coords_x, 0] = min_coords[0]
self.points_buf_[Sdf3D.max_coords_x, 0] = min_coords[0]
self.points_buf_[Sdf3D.min_coords_y, 1] = min_coords[1]
self.points_buf_[Sdf3D.max_coords_y, 1] = max_coords[1]
self.points_buf_[Sdf3D.min_coords_z, 2] = min_coords[2]
self.points_buf_[Sdf3D.max_coords_z, 2] = max_coords[2]
# bilinear interpolation
g = np.zeros(3)
gp = np.zeros(3)
w_sum = 0.0
for i in range(Sdf3D.num_interpolants):
p = self.points_buf_[i,:]
if self.is_out_of_bounds(p):
gp[0] = 0.0
gp[1] = 0.0
gp[2] = 0.0
else:
gp[0] = self.gradients_[0][p[0], p[1], p[2]]
gp[1] = self.gradients_[1][p[0], p[1], p[2]]
gp[2] = self.gradients_[2][p[0], p[1], p[2]]
w = np.prod(-np.abs(p - self.coords_buf_) + 1)
g = g + w * gp
return g
def curvature(self, coords, delta=0.001):
"""
Returns an approximation to the local SDF curvature (Hessian) at the
given coordinate in grid basis.
Parameters
---------
coords : numpy 3-vector
the grid coordinates at which to get the curvature
Returns
-------
curvature : 3x3 ndarray of the curvature at the surface points
"""
# perturb local coords
coords_x_up = coords + np.array([delta, 0, 0])
coords_x_down = coords + np.array([-delta, 0, 0])
coords_y_up = coords + np.array([0, delta, 0])
coords_y_down = coords + np.array([0, -delta, 0])
coords_z_up = coords + np.array([0, 0, delta])
coords_z_down = coords + np.array([0, 0, -delta])
# get gradient
grad_x_up = self.gradient(coords_x_up)
grad_x_down = self.gradient(coords_x_down)
grad_y_up = self.gradient(coords_y_up)
grad_y_down = self.gradient(coords_y_down)
grad_z_up = self.gradient(coords_z_up)
grad_z_down = self.gradient(coords_z_down)
# finite differences
curvature_x = (grad_x_up - grad_x_down) / (4 * delta)
curvature_y = (grad_y_up - grad_y_down) / (4 * delta)
curvature_z = (grad_z_up - grad_z_down) / (4 * delta)
curvature = np.c_[curvature_x, np.c_[curvature_y, curvature_z]]
curvature = curvature + curvature.T
return curvature
def surface_normal(self, coords, delta=1.5):
"""Returns the sdf surface normal at the given coordinates by
computing the tangent plane using SDF interpolation.
Parameters
----------
coords : :obj:`numpy.ndarray` of int
A 3-dimensional ndarray that indicates the desired
coordinates in the grid.
delta : float
A radius for collecting surface points near the target coords
for calculating the surface normal.
Returns
-------
:obj:`numpy.ndarray` of float
The 3-dimensional ndarray that represents the surface normal.
Raises
------
IndexError
If the coords vector does not have three entries.
"""
if len(coords) != 3:
raise IndexError('Indexing must be 3 dimensional')
# log warning if out of bounds access
if self.is_out_of_bounds(coords):
logging.debug('Out of bounds access. Snapping to SDF dims')
# snap to grid dims
coords[0] = max(0, min(coords[0], self.dims_[0] - 1))
coords[1] = max(0, min(coords[1], self.dims_[1] - 1))
coords[2] = max(0, min(coords[2], self.dims_[2] - 1))
index_coords = np.zeros(3)
# check points on surface
sdf_val = self[coords]
if np.abs(sdf_val) >= self.surface_thresh_:
logging.warning('Cannot compute normal. Point must be on surface')
return None
# collect all surface points within the delta sphere
X = []
d = np.zeros(3)
dx = -delta
while dx <= delta:
dy = -delta
while dy <= delta:
dz = -delta
while dz <= delta:
d = np.array([dx, dy, dz])
if dx != 0 or dy != 0 or dz != 0:
d = delta * d / np.linalg.norm(d)
index_coords[0] = coords[0] + d[0]
index_coords[1] = coords[1] + d[1]
index_coords[2] = coords[2] + d[2]
sdf_val = self[index_coords]
if np.abs(sdf_val) < self.surface_thresh_:
X.append([index_coords[0], index_coords[1], index_coords[2], sdf_val])
dz += delta
dy += delta
dx += delta
# fit a plane to the surface points
X.sort(key = lambda x: x[3])
X = np.array(X)[:,:3]
A = X - np.mean(X, axis=0)
try:
U, S, V = np.linalg.svd(A.T)
n = U[:,2]
except:
logging.warning('Tangent plane does not exist. Returning None.')
return None
return n
def surface_points(self, grid_basis=True):
"""Returns the points on the surface.
Parameters
----------
grid_basis : bool
If False, the surface points are transformed to the world frame.
If True (default), the surface points are left in grid coordinates.
Returns
-------
:obj:`tuple` of :obj:`numpy.ndarray` of int, :obj:`numpy.ndarray` of float
The points on the surface and the signed distances at those points.
"""
surface_points = np.where(np.abs(self.data_) < self.surface_thresh_)
x = surface_points[0]
y = surface_points[1]
z = surface_points[2]
surface_points = np.c_[x, np.c_[y, z]]
surface_vals = self.data_[surface_points[:,0], surface_points[:,1], surface_points[:,2]]
if not grid_basis:
surface_points = self.transform_pt_grid_to_obj(surface_points.T)
surface_points = surface_points.T
return surface_points, surface_vals
def rescale(self, scale):
""" Rescale an SDF by a given scale factor.
Parameters
----------
scale : float
the amount to scale the SDF
Returns
-------
:obj:`Sdf3D`
new sdf with given scale
"""
resolution_tf = scale * self.resolution_
return Sdf3D(self.data_, self.origin_, resolution_tf, use_abs=self.use_abs_,
T_sdf_world=self.T_sdf_world_)
def transform_dense(self, delta_T, detailed = False):
""" Transform the grid by pose T and scale with canonical reference
frame at the SDF center with axis alignment.
Parameters
----------
delta_T : SimilarityTransform
the transformation from the current frame of reference to the new frame of reference
detailed : bool
whether or not to use interpolation
Returns
-------
:obj:`Sdf3D`
new sdf with grid warped by T
"""
# map all surface points to their new location
start_t = time.clock()
# form points array
if self.pts_ is None:
[x_ind, y_ind, z_ind] = np.indices(self.dims_)
self.pts_ = np.c_[x_ind.flatten().T, np.c_[y_ind.flatten().T, z_ind.flatten().T]].astype(np.float32)
# transform points
num_pts = self.pts_.shape[0]
pts_sdf = self.T_grid_sdf_ * PointCloud(self.pts_.T, frame='grid')
pts_sdf_tf = delta_T.as_frames('sdf', 'sdf') * pts_sdf
pts_grid_tf = self.T_sdf_grid_ * pts_sdf_tf
pts_tf = pts_grid_tf.data.T
all_points_t = time.clock()
# transform the center
origin_sdf = self.T_grid_sdf_ * Point(self.origin_, frame='grid')
origin_sdf_tf = delta_T.as_frames('sdf', 'sdf') * origin_sdf
origin_tf = self.T_sdf_grid_ * origin_sdf_tf
origin_tf = origin_tf.data
# use same resolution (since indices will be rescaled)
resolution_tf = self.resolution_
origin_res_t = time.clock()
# add each point to the new pose
if detailed:
sdf_data_tf = np.zeros([num_pts, 1])
for i in range(num_pts):
sdf_data_tf[i] = self[pts_tf[i,0], pts_tf[i,1], pts_tf[i,2]]
else:
pts_tf_round = np.round(pts_tf).astype(np.int64)
# snap to closest boundary
pts_tf_round[:,0] = np.max(np.c_[np.zeros([num_pts, 1]), pts_tf_round[:,0]], axis=1)
pts_tf_round[:,0] = np.min(np.c_[(self.dims_[0] - 1) * np.ones([num_pts, 1]), pts_tf_round[:,0]], axis=1)
pts_tf_round[:,1] = np.max(np.c_[np.zeros([num_pts, 1]), pts_tf_round[:,1]], axis=1)
pts_tf_round[:,1] = np.min(np.c_[(self.dims_[1] - 1) * np.ones([num_pts, 1]), pts_tf_round[:,1]], axis=1)
pts_tf_round[:,2] = np.max(np.c_[np.zeros([num_pts, 1]), pts_tf_round[:,2]], axis=1)
pts_tf_round[:,2] = np.min(np.c_[(self.dims_[2] - 1) * np.ones([num_pts, 1]), pts_tf_round[:,2]], axis=1)
sdf_data_tf = self.data_[pts_tf_round[:,0], pts_tf_round[:,1], pts_tf_round[:,2]]
sdf_data_tf_grid = sdf_data_tf.reshape(self.dims_)
tf_t = time.clock()
logging.debug('Sdf3D: Time to transform coords: %f' %(all_points_t - start_t))
logging.debug('Sdf3D: Time to transform origin: %f' %(origin_res_t - all_points_t))
logging.debug('Sdf3D: Time to transfer sd: %f' %(tf_t - origin_res_t))
return Sdf3D(sdf_data_tf_grid, origin_tf, resolution_tf, use_abs=self._use_abs_, T_sdf_world=self.T_sdf_world_)
def transform_pt_obj_to_grid(self, x_sdf, direction = False):
""" Converts a point in sdf coords to the grid basis. If direction then don't translate.
Parameters
----------
x_sdf : numpy 3xN ndarray or numeric scalar
points to transform from sdf basis in meters to grid basis
Returns
-------
x_grid : numpy 3xN ndarray or scalar
points in grid basis
"""
if isinstance(x_sdf, Number):
return self.T_world_grid_.scale * x_sdf
if direction:
points_sdf = NormalCloud(x_sdf.astype(np.float32), frame='world')
else:
points_sdf = PointCloud(x_sdf.astype(np.float32), frame='world')
x_grid = self.T_world_grid_ * points_sdf
return x_grid.data
def transform_pt_grid_to_obj(self, x_grid, direction = False):
""" Converts a point in grid coords to the world basis. If direction then don't translate.
Parameters
----------
x_grid : numpy 3xN ndarray or numeric scalar
points to transform from grid basis to sdf basis in meters
Returns
-------
x_sdf : numpy 3xN ndarray
points in sdf basis (meters)
"""
if isinstance(x_grid, Number):
return self.T_grid_world_.scale * x_grid
if direction:
points_grid = NormalCloud(x_grid.astype(np.float32), frame='grid')
else:
points_grid = PointCloud(x_grid.astype(np.float32), frame='grid')
x_sdf = self.T_grid_world_ * points_grid
return x_sdf.data
@staticmethod
def find_zero_crossing_linear(x1, y1, x2, y2):
""" Find zero crossing using linear approximation"""
# NOTE: use sparingly, approximations can be shoddy
d = x2 - x1
t1 = 0
t2 = np.linalg.norm(d)
v = d / t2
m = (y2 - y1) / (t2 - t1)
b = y1
t_zc = -b / m
x_zc = x1 + t_zc * v
return x_zc
@staticmethod
def find_zero_crossing_quadratic(x1, y1, x2, y2, x3, y3, eps = 1.0):
""" Find zero crossing using quadratic approximation along 1d line"""
# compute coords along 1d line
v = x2 - x1
v = v / np.linalg.norm(v)
if v[v!=0].shape[0] == 0:
logging.error('Difference is 0. Probably a bug')
t1 = 0
t2 = (x2 - x1)[v!=0] / v[v!=0]
t2 = t2[0]
t3 = (x3 - x1)[v!=0] / v[v!=0]
t3 = t3[0]
# solve for quad approx
x1_row = np.array([t1**2, t1, 1])
x2_row = np.array([t2**2, t2, 1])
x3_row = np.array([t3**2, t3, 1])
X = np.array([x1_row, x2_row, x3_row])
y_vec = np.array([y1, y2, y3])
try:
w = np.linalg.solve(X, y_vec)
except np.linalg.LinAlgError:
logging.error('Singular matrix. Probably a bug')
return None
# get positive roots
possible_t = np.roots(w)
t_zc = None
for i in range(possible_t.shape[0]):
if possible_t[i] >= 0 and possible_t[i] <= 10 and not np.iscomplex(possible_t[i]):
t_zc = possible_t[i]
# if no positive roots find min
if np.abs(w[0]) < 1e-10:
return None
if t_zc is None:
t_zc = -w[1] / (2 * w[0])
if t_zc < -eps or t_zc > eps:
return None
x_zc = x1 + t_zc * v
return x_zc
