def get_corners(self): ''' corners.T => (8, 3) (0:4) -> top (4:8) -> bottom axis 2: x axis 0: y axis 1: z (top,bottom) | V (1,5)----(2,6) | | | | (0,4)----(3,7) ''' if self.__corners is None: # Compute corners self.__corners = np.array( [[ -self.l / 2, self.l / 2, self.l / 2, -self.l / 2, -self.l / 2, self.l / 2, self.l / 2, -self.l / 2 ], [0, 0, 0, 0, -self.h, -self.h, -self.h, -self.h], [ -self.w / 2, -self.w / 2, self.w / 2, self.w / 2, -self.w / 2, -self.w / 2, self.w / 2, self.w / 2 ]], dtype=np.float32) H = np.dot(translation_matrix(self.x, self.y, self.z), rot_y_matrix(self.yaw)) self.__corners = transform(H, self.__corners) return self.__corners
def get_arrow_pts(self): if self.__arrow_pts is None: # Compute arrow from center pointing forward self.__arrow_pts = np.array([[0, (self.l / 2) + 1], [-self.h / 2, -self.h / 2], [0, 0]]) H = np.dot(translation_matrix(self.x, self.y, self.z), rot_y_matrix(self.yaw)) self.__arrow_pts = transform(H, self.__arrow_pts) return self.__arrow_pts
def get_corners_3D(box): corners = np.array([[ -box.l / 2, box.l / 2, box.l / 2, -box.l / 2, -box.l / 2, box.l / 2, box.l / 2, -box.l / 2 ], [0, 0, 0, 0, -box.h, -box.h, -box.h, -box.h], [ -box.w / 2, -box.w / 2, box.w / 2, box.w / 2, -box.w / 2, -box.w / 2, box.w / 2, box.w / 2 ]], dtype=np.float32) H = np.dot(translation_matrix(box.x, box.y, box.z), rot_y_matrix(box.yaw)) return transform(H, corners)
def get_velo(path, calib_path, workspace_lim=((-40, 40), (-1, 2.5), (0, 70)), use_fov_filter=True): velo = np.fromfile(path, dtype=np.float32).reshape((-1, 4)).T pts = velo[0:3] reflectance = velo[3:] # Transform points from velo coordinates to rectified camera coordinates V2C, R0, P2 = get_calib(calib_path) pts = transform(np.dot(R0, V2C), pts) # Remove points out of workspace pts, reflectance = box_filter(pts, workspace_lim, decorations=reflectance) # Remove points not projecting onto the image plane if use_fov_filter: pts, reflectance = fov_filter(pts, P=P2, img_size=(IMG_HEIGHT, IMG_WIDTH), decorations=reflectance) return pts, reflectance