def convert_to_open3d_pointcloud(dmap: Depthmap,
floor_altitude_in_meters: float):
"""Converts depthmap into Open3D pointcloud.
floor_altitude_in_meters is the floor altitude to align floor to Y=zero"""
points = []
normals = []
points_3d_arr = dmap.convert_2d_to_3d_oriented()
normal_3d_arr = dmap.calculate_normalmap_array(points_3d_arr)
for x in range(2, dmap.width - 2):
for y in range(2, dmap.height - 2):
depth = dmap.depthmap_arr[x, y]
if not depth:
continue
x_coord = points_3d_arr[0, x, y]
y_coord = points_3d_arr[1, x, y] - floor_altitude_in_meters
z_coord = points_3d_arr[2, x, y]
x_normal = normal_3d_arr[0, x, y]
y_normal = normal_3d_arr[1, x, y]
z_normal = normal_3d_arr[2, x, y]
points.append([x_coord, y_coord, z_coord])
normals.append([x_normal, y_normal, z_normal])
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points)
pcd.normals = o3d.utility.Vector3dVector(normals)
return pcd
def render_normal(dmap: Depthmap) -> np.ndarray:
"""Render normal vectors
How normal vector are visualized:
When a vector has (x,y,z)=(1,0,0), this will show in red color.
When a vector has (x,y,z)=(0,1,0), this will show in green color (e.g. floor).
When a vector has (x,y,z)=(0,0,1), this will show in blue color.
"""
points_3d_arr = dmap.convert_2d_to_3d_oriented(should_smooth=True)
normal = dmap.calculate_normalmap_array(points_3d_arr)
# We can't see negative values, so we take the absolute value
normal = abs(normal) # shape: (3, width, height)
return np.moveaxis(normal, 0, -1)