def test_icp_registration(self):
source = pp.PointCloud(np.random.rand(10, 3), pp.pointcloud.PointXYZField())
angle = np.deg2rad(10.0)
trans = np.identity(4)
trans[0, 0] = np.cos(angle)
trans[0, 1] = -np.sin(angle)
trans[1, 0] = np.sin(angle)
trans[1, 1] = np.cos(angle)
target = source.transform(trans)
res = pp.registration.icp_registration(source, target, 0.3)
np.testing.assert_almost_equal(res, trans)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_scale = depth_sensor.get_depth_scale()
# We will not display the background of objects more than
# clipping_distance_in_meters meters away
clipping_distance_in_meters = 3 # 3 meter
clipping_distance = clipping_distance_in_meters / depth_scale
# print(depth_scale)
# Create an align object
# rs.align allows us to perform alignment of depth frames to others frames
# The "align_to" is the stream type to which we plan to align depth frames.
align_to = rs.stream.color
align = rs.align(align_to)
pcd = pp.PointCloud()
flip_transform = np.array([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0],
[0, 0, 0, 1]])
client = pp.VizClient(host=args.host)
dist_fn = lambda c1, c2: pp.registration.compute_rmse(
c1.feature, c2.feature, 50)
assigner = pp.assignment.ClusterAssigner(dist_fn, 0.2)
try:
while True:
cur_params = client.get_parameters()
# Get frameset of color and depth
frames = pipeline.wait_for_frames()
# Align the depth frame to color frame
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_scale = depth_sensor.get_depth_scale()
# We will not display the background of objects more than
# clipping_distance_in_meters meters away
clipping_distance_in_meters = 3 # 3 meter
clipping_distance = clipping_distance_in_meters / depth_scale
# print(depth_scale)
# Create an align object
# rs.align allows us to perform alignment of depth frames to others frames
# The "align_to" is the stream type to which we plan to align depth frames.
align_to = rs.stream.color
align = rs.align(align_to)
pcd = pp.PointCloud()
flip_transform = np.array([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0],
[0, 0, 0, 1]])
client = pp.VizClient(host=args.host)
outfh = h5py.File("sensor_data.h5", "a")
if "pointclouds" in outfh:
grp = outfh["pointclouds"]
else:
grp = outfh.create_group("pointclouds")
try:
while True:
# Get frameset of color and depth
frames = pipeline.wait_for_frames()
# Align the depth frame to color frame
plane_pts = pc.points[mask, :]
not_plane_pts = pc.points[~mask, :]
# Cluster objects
db = DBSCAN(eps=0.1).fit(not_plane_pts)
labels = db.labels_
n_clusters = len(set(labels)) - (1 if -1 in labels else 0)
print("Number of class:", n_clusters)
mask_0 = labels == 0
mask_1 = labels == 1
# Draw results
client = pp.VizClient()
res1 = client.post_pointcloud(
pp.PointCloud(
np.c_[plane_pts,
np.tile([0.0, 0.0, 1.0], (len(plane_pts), 1))],
pp.pointcloud.PointXYZRGBField()), 'plane')
res2 = client.post_pointcloud(
pp.PointCloud(
np.c_[not_plane_pts[mask_0],
np.tile([0.0, 1.0, 0.0], (len(not_plane_pts[mask_0]), 1))],
pp.pointcloud.PointXYZRGBField()), 'obj1')
res3 = client.post_pointcloud(
pp.PointCloud(
np.c_[not_plane_pts[mask_1],
np.tile([1.0, 0.0, 1.0], (len(not_plane_pts[mask_1]), 1))],
pp.pointcloud.PointXYZRGBField()), 'obj2')
plane_area = calc_convexhull_area(plane_pts[:, :2])
obj0_area = calc_convexhull_area(not_plane_pts[mask_0, :2])
obj1_area = calc_convexhull_area(not_plane_pts[mask_1, :2])
client.add_log(f"Plane Area: {plane_area:.3f}")