MediumDensity = 5
def get_intrinsic_matrix(frame):
intrinsics = frame.profile.as_video_stream_profile().intrinsics
out = np.identity(3)
out[0, 0] = intrinsics.fx
out[1, 1] = intrinsics.fy
out[0, 2] = intrinsics.ppx
out[1, 2] = intrinsics.ppy
return out
if __name__ == "__main__":
pp.vizspawn(host=args.host)
# Create a pipeline
pipeline = rs.pipeline()
#Create a config and configure the pipeline to stream
# different resolutions of color and depth streams
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 30)
# Start streaming
profile = pipeline.start(config)
depth_sensor = profile.get_device().first_depth_sensor()
out = np.identity(3)
out[0, 0] = intrinsics.fx
out[1, 1] = intrinsics.fy
out[0, 2] = intrinsics.ppx
out[1, 2] = intrinsics.ppy
return out
if __name__ == "__main__":
params = {
"dist_thresh": (0.03, 0.0, 0.1),
"voxel_size": (0.01, 0.0, 0.1),
"point_size_thresh": (100, 0, 1000)
}
pp.vizspawn(host=args.host, params=params)
# Create a pipeline
pipeline = rs.pipeline()
#Create a config and configure the pipeline to stream
# different resolutions of color and depth streams
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 30)
# Start streaming
profile = pipeline.start(config)
depth_sensor = profile.get_device().first_depth_sensor()
(p0[1] - p1[1]))
def calc_convexhull_area(points):
hull = ConvexHull(points[:, :2])
hull_points = points[hull.vertices, :2]
hull_points = np.r_[hull_points, [hull_points[0]]]
hull_mean = hull_points.mean(axis=0)
area = 0
for i in range(len(hull_points) - 1):
area += calc_triangle_area(hull_mean, hull_points[i],
hull_points[i + 1])
return area
pp.vizspawn()
# Segment plane
pc = pp.io.load_pcd('data/test0.pcd')
pc = pp.filter.remove_invalid_points(pc)
pc = pp.filter.voxel_grid_filter(pc, 0.01)
print(pc.points)
plane, mask = pp.segmentation.segmentation_plane(pc, dist_thresh=0.01)
print(plane, sum(mask))
# Rotate to plane coordinate
axis = np.array([-plane[1], plane[0], 0.0])
axis /= np.linalg.norm(axis)
angle = np.arccos(plane[2] / np.linalg.norm(plane[:3]))
trans = np.identity(4)
trans[:3, :3] = t3d.axangles.axangle2mat(axis, angle)