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