def set_plane(self, ground_plane, xz_extents):
"""
Calculates 3 points for plane visualization
based on the provided ground plane
:param ground_plane: Plane equation coefficients (a, b, c, d)
:param xz_extents: Extents along the xz plane for visualization
"""
min_x = xz_extents[0][0]
max_x = xz_extents[0][1]
min_z = xz_extents[1][0]
max_z = xz_extents[1][1]
plane_point0 = geometry_utils.calculate_plane_point(
ground_plane, (min_x, None, min_z))
plane_point1 = geometry_utils.calculate_plane_point(
ground_plane, (max_x, None, min_z))
plane_point2 = geometry_utils.calculate_plane_point(
ground_plane, (min_x, None, max_z))
self.vtk_plane_source.SetOrigin(*plane_point0)
self.vtk_plane_source.SetPoint1(*plane_point1)
self.vtk_plane_source.SetPoint2(*plane_point2)
self.vtk_plane_source.Update()
vtk_plane_poly_data = self.vtk_plane_source.GetOutput()
self.vtk_plane_mapper.SetInputData(vtk_plane_poly_data)
self.vtk_actor.SetMapper(self.vtk_plane_mapper)
def create_point_corners(self, box_4c, ground_plane):
centroid_x = np.sum(box_4c[0:4]) / 4.0
centroid_z = np.sum(box_4c[4:8]) / 4.0
x1 = box_4c[0]
x2 = box_4c[1]
x3 = box_4c[2]
x4 = box_4c[3]
z1 = box_4c[4]
z2 = box_4c[5]
z3 = box_4c[6]
z4 = box_4c[7]
h1 = box_4c[8]
h2 = box_4c[9]
_, ground_y, _ = geometry_utils.calculate_plane_point(
ground_plane, [centroid_x, None, centroid_z])
y_lo = ground_y - h1
y_hi = ground_y - h2
# Draw each line of the cube
p1 = [x1, y_lo, z1]
p2 = [x2, y_lo, z2]
p3 = [x3, y_lo, z3]
p4 = [x4, y_lo, z4]
p5 = [x1, y_hi, z1]
p6 = [x2, y_hi, z2]
p7 = [x3, y_hi, z3]
p8 = [x4, y_hi, z4]
self.vtk_points.InsertNextPoint(p1)
self.vtk_points.InsertNextPoint(p2)
self.vtk_points.InsertNextPoint(p3)
self.vtk_points.InsertNextPoint(p4)
self.vtk_points.InsertNextPoint(p5)
self.vtk_points.InsertNextPoint(p6)
self.vtk_points.InsertNextPoint(p7)
self.vtk_points.InsertNextPoint(p8)
# Set text labels
p_names = ['P1', 'P2', 'P3', 'P4', 'P5', 'P6', 'P7', 'P8']
self.vtk_text_labels.set_text_labels([p1, p2, p3, p4, p5, p6, p7, p8],
p_names)
def np_box_3d_to_box_4c(box_3d, ground_plane):
"""Converts a single box_3d to box_4c
Args:
box_3d: box_3d (6,)
ground_plane: ground plane coefficients (4,)
Returns:
box_4c (10,)
"""
format_checker.check_box_3d_format(box_3d)
anchor = box_3d_encoder.box_3d_to_anchor(box_3d, ortho_rotate=True)[0]
centroid_x = anchor[0]
centroid_y = anchor[1]
centroid_z = anchor[2]
dim_x = anchor[3]
dim_y = anchor[4]
dim_z = anchor[5]
# Create temporary box at (0, 0) for rotation
half_dim_x = dim_x / 2
half_dim_z = dim_z / 2
# Box corners
x_corners = np.asarray([half_dim_x, half_dim_x, -half_dim_x, -half_dim_x])
z_corners = np.array([half_dim_z, -half_dim_z, -half_dim_z, half_dim_z])
ry = box_3d[6]
# Find nearest 90 degree
half_pi = np.pi / 2
ortho_ry = np.round(ry / half_pi) * half_pi
# Find rotation to make the box ortho aligned
ry_diff = ry - ortho_ry
# Create transformation matrix, including rotation and translation
tr_mat = np.array([[np.cos(ry_diff),
np.sin(ry_diff), centroid_x],
[-np.sin(ry_diff),
np.cos(ry_diff), centroid_z], [0, 0, 1]])
# Create a ones row
ones_row = np.ones(x_corners.shape)
# Append the column of ones to be able to multiply
points_stacked = np.vstack([x_corners, z_corners, ones_row])
corners = np.matmul(tr_mat, points_stacked)
# Discard the last row (ones)
corners = corners[0:2]
# Calculate height off ground plane
ground_y = geometry_utils.calculate_plane_point(
ground_plane, [centroid_x, None, centroid_z])[1]
h1 = ground_y - centroid_y
h2 = h1 + dim_y
# Stack into (10,) ndarray
box_4c = np.hstack([corners.flatten(), h1, h2])
return box_4c