def from_PointCloud2(ros_pc):
"""From a ROS PointCloud2 to a Klampt PointCloud"""
from klampt import PointCloud
from sensor_msgs.msg import PointField
from sensor_msgs import point_cloud2
pc = PointCloud()
if ros_pc.header.seq > 0:
pc.setSetting("id", str(ros_pc.header.seq))
if ros_pc.fields[0].name != 'x' or ros_pc.fields[
1].name != 'y' or ros_pc.fields[2].name != 'z':
raise ValueError("PointCloud2 message doesn't have x,y,z fields?")
for i in range(3, len(ros_pc.fields)):
pc.addProperty(ros_pc.fields[i].name)
N = ros_pc.height * ros_pc.width
plist = []
proplist = []
if ros_pc.height > 1:
structured = True
pc.settings.set("width", str(pc.width))
pc.settings.set("height", str(pc.height))
for p in point_cloud2.read_points(ros_pc):
plist += [p[0], p[1], p[2]]
proplist += [float(v) for v in p[3:]]
pc.setPoints(len(plist) // 3, plist)
pc.setProperties(proplist)
return pc
def from_open3d(obj):
"""Converts open3d geometry to a Klamp't geometry.
"""
if isinstance(obj,open3d.geometry.PointCloud):
pc = PointCloud()
for p in obj.points:
pc.vertices.append(p[0])
pc.vertices.append(p[1])
pc.vertices.append(p[2])
if obj.has_colors():
geometry.point_cloud_set_colors(pc,obj.colors,('r','g','b'),'rgb')
#TODO: other properties
return pc
elif isinstance(obj,open3d.geometry.TriangleMesh):
m = TriangleMesh()
for p in obj.vertices:
m.vertices.append(p[0])
m.vertices.append(p[1])
m.vertices.append(p[2])
for i in obj.triangles:
m.indices.append(int(i[0]))
m.indices.append(int(i[1]))
m.indices.append(int(i[2]))
return m
elif isinstance(obj,open3d.geometry.VoxelGrid):
grid = VolumeGrid()
import numpy as np
occupied = np.array(obj.voxels,dtype=np.int32)
imin = np.min(occupied,axis=0)
imax = np.max(occupied,axis=0)
assert imin.shape == (3,)
assert imax.shape == (3,)
bmin = obj.origin + (imin-1)*obj.voxel_size
bmax = obj.origin + (imax+2)*obj.voxel_size
grid.bbox.append(bmin[0])
grid.bbox.append(bmin[1])
grid.bbox.append(bmin[2])
grid.bbox.append(bmax[0])
grid.bbox.append(bmax[1])
grid.bbox.append(bmax[2])
grid.dims.append(imax[0]-imin[0]+3)
grid.dims.append(imax[1]-imin[1]+3)
grid.dims.append(imax[2]-imin[2]+3)
grid.values.resize(grid.dims[0]*grid.dims[1]*grid.dims[2],-1.0)
for cell in occupied:
grid.set(int(cell[0]-imin[0]+1),int(cell[1]-imin[1]+1),int(cell[2]-imin[2]+1),1.0)
return grid
raise TypeError("Invalid type")
def from_numpy(obj, type='auto', template=None):
"""Converts a numpy array or multiple numpy arrays to a Klamp't object.
Supports:
* lists and tuples
* RigidTransform: accepts a 4x4 homogeneous coordinate transform
* Matrix3, Rotation: accepts a 3x3 matrix.
* Configs
* Trajectory: accepts a pair (times,milestones)
* TriangleMesh: accepts a pair (verts,indices)
* PointCloud: accepts a n x (3+k) array, where k is the # of properties
* VolumeGrid: accepts a triple (bmin,bmax,array)
* Geometry3D: accepts a pair (T,geomdata)
"""
global supportedTypes
if type == 'auto' and template is not None:
otype = types.objectToTypes(template)
if isinstance(otype, (list, tuple)):
for t in otype:
if t in supportedTypes:
type = t
break
if type == 'auto':
raise ValueError('obj is not a supported type: ' +
', '.join(otype))
else:
type = otype
if type == 'auto':
if isinstance(obj, (tuple, list)):
if all(isinstance(v, np.ndarray) for v in obj):
if len(obj) == 2:
if len(obj[0].shape) == 1 and len(obj[1].shape) == 2:
type = 'Trajectory'
elif len(obj[0].shape) == 2 and len(
obj[1].shape
) == 2 and obj[0].shape[1] == 3 and obj[1].shape[1] == 3:
type = 'TriangleMesh'
if len(obj) == 3:
if obj[0].shape == (3, ) and obj[1].shape == (3, ):
type = 'VolumeGrid'
if type == 'auto':
raise ValueError(
"Can't auto-detect type of list of shapes" +
', '.join(str(v.shape) for v in obj))
else:
if isinstance(obj[0], np.ndarray) and obj[0].shape == (4, 4):
type = 'Geometry3D'
else:
raise ValueError(
"Can't auto-detect type of irregular list")
else:
assert isinstance(
obj, np.ndarray
), "Can only convert lists, tuples, and arrays from numpy"
if obj.shape == (3, 3):
type = 'Matrix3'
elif obj.shape == (4, 4):
type = 'RigidTransform'
elif len(obj.shape) == 1:
type = 'Config'
else:
raise ValueError("Can't auto-detect type of matrix of shape " +
str(obj.shape))
if type not in supportedTypes:
raise ValueError(type + ' is not a supported type')
if type == 'RigidTransform':
return se3.from_homogeneous(obj)
elif type == 'Rotation' or type == 'Matrix3':
return so3.from_matrix(obj)
elif type == 'Trajectory':
assert len(obj) == 2, "Trajectory format is (times,milestones)"
times = obj[0].tolist()
milestones = obj[1].tolist()
if template is not None:
return template.constructor()(times, milestones)
from klampt.model.trajectory import Trajectory
return Trajectory(times, milestones)
elif type == 'TriangleMesh':
from klampt import TriangleMesh
res = TriangleMesh()
vflat = obj[0].flatten()
res.vertices.resize(len(vflat))
for i, v in enumerate(vflat):
res.vertices[i] = float(v)
iflat = obj[1].flatten()
res.indices.resize(len(iflat))
for i, v in enumerate(iflat):
res.indices[i] = int(v)
return res
elif type == 'PointCloud':
from klampt import PointCloud
assert len(obj.shape) == 2, "PointCloud array must be a 2D array"
assert obj.shape[1] >= 3, "PointCloud array must have at least 3 values"
points = obj[:, :3]
properties = obj[:, 3:]
res = PointCloud()
res.setPoints(points.shape[0], points.flatten())
if template is not None:
if len(template.propertyNames) != properties.shape[1]:
raise ValueError(
"Template object doesn't have the same properties as the numpy object"
)
for i in range(len(template.propertyNames)):
res.propertyNames[i] = template.propertyNames[i]
else:
for i in range(properties.shape[1]):
res.propertyNames.append('property %d' % (i + 1))
if len(res.propertyNames) > 0:
res.properties.resize(len(res.propertyNames) * points.shape[0])
if obj.shape[1] >= 3:
res.setProperties(properties.flatten())
return res
elif type == 'VolumeGrid':
from klampt import VolumeGrid
assert len(obj) == 3, "VolumeGrid format is (bmin,bmax,values)"
assert len(obj[2].shape) == 3, "VolumeGrid values must be a 3D array"
bmin = obj[0]
bmax = obj[1]
values = obj[2]
res = VolumeGrid()
res.bbox.append(bmin[0])
res.bbox.append(bmin[1])
res.bbox.append(bmin[2])
res.bbox.append(bmax[0])
res.bbox.append(bmax[1])
res.bbox.append(bmax[2])
res.dims.append(values.shape[0])
res.dims.append(values.shape[1])
res.dims.append(values.shape[2])
vflat = values.flatten()
res.values.resize(len(vflat))
for i, v in enumerate(vflat):
res.values[i] = v
return res
elif type == 'Group':
from klampt import Geometry3D
res = Geometry3D()
assert isinstance(
obj,
(list, tuple)), "Group format is a list or tuple of Geometry3D's"
for i in range(len(obj)):
res.setElement(i, from_numpy(obj[i], 'Geometry3D'))
return res
elif type == 'Geometry3D':
from klampt import Geometry3D
if not isinstance(obj, (list, tuple)) or len(obj) != 2:
raise ValueError("Geometry3D must be a (transform,geometry) tuple")
T = from_numpy(obj[0], 'RigidTransform')
geomdata = obj[1]
subtype = None
if template is not None:
subtype = template.type()
if subtype == 'PointCloud':
g = Geometry3D(
from_numpy(geomdata, subtype, template.getPointCloud()))
else:
g = Geometry3D(from_numpy(geomdata, subtype))
g.setCurrentTransform(*T)
return g
subtype = 'Group'
if all(isinstance(v, np.ndarray) for v in geomdata):
if len(geomdata) == 2:
if len(geomdata[0].shape) == 1 and len(geomdata[1].shape) == 2:
subtype = 'Trajectory'
elif len(geomdata[0].shape) == 2 and len(
geomdata[1].shape) == 2 and geomdata[0].shape[
1] == 3 and geomdata[1].shape[1] == 3:
subtype = 'TriangleMesh'
if len(geomdata) == 3:
if geomdata[0].shape == (3, ) and geomdata[1].shape == (3, ):
subtype = 'VolumeGrid'
g = Geometry3D(from_numpy(obj, subtype))
g.setCurrentTransform(*T)
return g
else:
return obj.flatten()
open3d.io.write_point_cloud("testPcd.pcd", open3dPcd)
else:
for ele in pcd_cut:
xyz.append(ele[0:3])
rgb.append(ele[3:6])
open3dPcd.points = open3d.utility.Vector3dVector(
np.asarray(xyz, dtype=np.float32))
open3dPcd.colors = open3d.utility.Vector3dVector(
np.asarray(rgb, dtype=np.float32))
open3d.io.write_point_cloud("testPcd.pcd", open3dPcd)
pcdK = loader.loadGeometry3D("testPcd.pcd")
klampt_pcd = pcdK.getPointCloud()
properties = np.array(klampt_pcd.properties).reshape(
(klampt_pcd.numPoints(), klampt_pcd.numProperties()))
pcd_new = PointCloud()
rs = []
gs = []
bs = []
for a in properties:
position = a[0:3]
pcd_new.addPoint(position)
rgb = a[3]
(r, g, b) = rgb_int2tuple(rgb)
rs.append(r)
gs.append(g)
bs.append(b)
pcd_new.addProperty('r', rs)
pcd_new.addProperty('g', gs)
pcd_new.addProperty('b', bs)
for idz in range(grid.dims[2]):
d=[(idx-grid.dims[0]/2)/float(grid.dims[0]/2), \
(idy-grid.dims[1]/2)/float(grid.dims[1]/2), \
(idz-grid.dims[2]/2)/float(grid.dims[2]/2)]
freq = 2.
amp = .1
thres = .5 * (1. + math.sin(d[0] * math.pi * freq) * math.sin(
d[1] * math.pi * freq) * math.sin(d[2] * math.pi * freq) * amp)
grid.set(idx, idy, idz, op.norm(d) / op.norm([1., 1., 1.]) - thres)
sdf.geometry().setVolumeGrid(grid)
sdf.appearance().setColor(random.uniform(0., 1.), random.uniform(0., 1.),
random.uniform(0., 1.))
#example PointCloud
points = world.makeTerrain("PointCloud")
cloud = PointCloud()
for i in range(1000):
cloud.addPoint([
random.uniform(0., 0.1),
random.uniform(0., 0.1),
random.uniform(0., 0.1)
])
points.geometry().setPointCloud(cloud)
points.geometry().transform([1, 0, 0, 0, 1, 0, 0, 0, 1], [0., 1.2, 0.])
points.appearance().setColor(random.uniform(0., 1.), random.uniform(0., 1.),
random.uniform(0., 1.))
#robot
robot = world.loadRobot("../../data/robots/tx90robotiq.rob")
for l in range(robot.numLinks()):
lk = robot.link(l)
def get_point_cloud(self,
camera=None,
colors=True,
normals=True,
structured=False,
near_filter=None,
far_filter=None,
format='numpy'):
"""Converts an RGBD scan to a point cloud in camera coordinates.
Points whose Z value is less than near_filter or greater than far_filter
will be excluded.
if format=='numpy':
If colors=True and normals=True, the result is an N x 9 Numpy array, giving
colors and normals. I.e, the matrix has columns X Y Z R G B Nx Ny Nz.
R, G, B are in the range [0,1]
If colors=True and normals=False, only the first 6 columns are returned.
If colors=False and normals=True, only the first 3 and last 3 columns are returned.
If colors=False and normals=False, only the X,Y,Z columns are returned.
if format == 'PointCloud':
A Klampt PointCloud is returned.
If structured=True, all points are returned, even ones with missing depth
and those outside of the near/far filter. Their rows will be zero
"""
if camera is None:
camera = self.camera
if camera is None or camera.depth_intrinsics is None:
raise ValueError("Need a camera's intrinsics to be defined")
if self.depth is None:
raise ValueError("Depth is not defined for this scan")
if colors and self.rgb is None:
print(
"Warning, requested colors but rgb information is not defined for this scan"
)
colors = False
if colors and normals:
channels = range(9)
elif colors:
channels = range(6)
elif normals:
channels = list(range(3)) + list(range(6, 9))
else:
channels = range(3)
if near_filter is None:
near_filter = camera.depth_minimum
else:
near_filter = max(near_filter, camera.depth_minimum)
if far_filter is None:
far_filter = camera.depth_maximum
else:
far_filter = min(far_filter, camera.depth_maximum)
fx, fy = camera.depth_intrinsics['fx'], camera.depth_intrinsics['fy']
cx, cy = camera.depth_intrinsics['cx'], camera.depth_intrinsics['cy']
depth_scale = camera.depth_image_scale
h, w = self.depth.shape[0], self.depth.shape[1]
Z = self.depth * (1.0 / depth_scale)
X, Y = np.meshgrid(np.array(range(w)) - cx, np.array(range(h)) - cy)
X *= Z * (1.0 / fx)
Y *= Z * (1.0 / fy)
points = np.stack((X, Y, Z), -1).reshape(w * h, 3)
#compute colors
if colors:
if camera.T_color_to_depth is not None:
raise NotImplementedError(
"TODO: map from color to depth frames")
colors = (self.rgb / 255.0).reshape(w * h, 3)
have_colors = True
else:
colors = np.empty((w * h, 3))
have_colors = False
if normals:
#compute normals from image
#What's all this fuss? getting good normals in the presence of missing depth readings
gxc = (Z[:, 2:] - Z[:, :-2]) / (X[:, 2:] - X[:, :-2])
gxn = (Z[:, 1:] - Z[:, :-1]) / (X[:, 1:] - X[:, :-1])
gyc = (Z[2:, :] - Z[:-2, :]) / (Y[2:, :] - Y[:-2, :])
gyn = (Z[1:, :] - Z[:-1, :]) / (Y[1:, :] - Y[:-1, :])
nancol = np.full((h, 1), np.nan)
gxc = np.hstack((nancol, gxc, nancol))
gxp = np.hstack((nancol, gxn))
gxn = np.hstack((gxn, nancol))
gx = np.where(
np.isnan(gxc),
np.where(np.isnan(gxn), np.where(np.isnan(gxp), 0, gxp), gxn),
gxc)
nanrow = np.full((1, w), np.nan)
gyc = np.vstack((nanrow, gyc, nanrow))
gyp = np.vstack((nanrow, gyn))
gyn = np.vstack((gyn, nanrow))
gy = np.where(
np.isnan(gyc),
np.where(np.isnan(gyn), np.where(np.isnan(gyp), 0, gyp), gyn),
gyc)
normals = np.stack((gx, gy, -np.ones((h, w))),
-1).reshape(w * h, 3)
np.nan_to_num(normals, copy=False)
normals = normals / np.linalg.norm(normals, axis=1)[:, np.newaxis]
np.nan_to_num(normals, copy=False)
have_normals = True
else:
normals = np.empty((w * h, 3))
have_normals = False
#join them all up
points = np.hstack((points, colors, normals))
#depth filtering
indices = np.logical_and(points[:, 2] >= near_filter,
points[:, 2] <= far_filter)
#print(np.sum(indices),"Points pass the depth test")
#print("Depth range",points[:,2].min(),points[:,2].max())
if structured:
res = points[:, channels]
res[~indices, :] = 0
else:
res = points[indices.nonzero()[0]][:, channels]
if format == 'PointCloud':
from klampt import PointCloud
pc = PointCloud()
pc.setPoints(res.shape[0], res[:, :3].flatten())
pstart = 0
if have_colors:
pc.addProperty('r')
pc.addProperty('g')
pc.addProperty('b')
pc.setProperties(0, res[:, 3])
pc.setProperties(1, res[:, 4])
pc.setProperties(2, res[:, 5])
pstart = 3
if have_normals:
pc.addProperty('normal_x')
pc.addProperty('normal_y')
pc.addProperty('normal_z')
pc.setProperties(0 + pstart, res[:, 6])
pc.setProperties(1 + pstart, res[:, 7])
pc.setProperties(2 + pstart, res[:, 8])
return pc
return res