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 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