import open3d as o3d
import numpy as np

pcd = o3d.io.read_point_cloud("../laser1.ply")
# ply_point_cloud = o3d.data.PLYPointCloud()
# pcd = o3d.io.read_point_cloud(ply_point_cloud.path)

corners = np.array([
  [ 10, -3.21384387,  0.30217625,],
  [ 10, -1.13804348,  0.29917539,],
  [ 7.69983939, -1.16651864,  0.30329364,],
  [ 7.67473496, -3.24231903,  0.3062945, ],
  [ 10, -3.21276837,  1.03551451,],
  [ 10, -1.13696798,  1.03251366,],
  [ 7.69856999, -1.16544314,  1.03663191,],
  [ 7.67346556, -3.24124353,  1.03963277,],
])

# Convert the corners array to have type float64
bounding_polygon = corners.astype("float64")

# Create a SelectionPolygonVolume
vol = o3d.visualization.SelectionPolygonVolume()

# You need to specify what axis to orient the polygon to.
# I choose the "Y" axis. I made the max value the maximum Y of
# the polygon vertices and the min value the minimum Y of the
# polygon vertices.
vol.orthogonal_axis = "Y"
vol.axis_max = np.max(bounding_polygon[:, 1])
vol.axis_min = np.min(bounding_polygon[:, 1])

# Set all the Y values to 0 (they aren't needed since we specified what they
# should be using just vol.axis_max and vol.axis_min).
bounding_polygon[:, 1] = 0

# Convert the np.array to a Vector3dVector
vol.bounding_polygon = o3d.utility.Vector3dVector(bounding_polygon)

# Crop the point cloud using the Vector3dVector
cropped_pcd = vol.crop_point_cloud(pcd)

# Get a nice looking bounding box to display around the newly cropped point cloud
# (This part is optional and just for display purposes)
bounding_box = cropped_pcd.get_axis_aligned_bounding_box()
bounding_box.color = (1, 0, 0)

# Draw the newly cropped PCD and bounding box
# o3d.visualization.draw_geometries([cropped_pcd, bounding_box],
#                                   zoom=2,
#                                   front=[5, -2, 0.5],
#                                   lookat=[7.67473496, -3.24231903,  0.3062945],
#                                   up=[1.0, 0.0, 0.0])
# o3d.visualization.draw_geometries([pcd, bounding_box])
o3d.visualization.draw([pcd, bounding_box],)