def _anno_to_2d_bbox(self, anno, pc_file, cam_front, lidar_top, ego_pose_cam, ego_pose_lidar, cam_intrinsic): # Make pixel indexes 0-based dists = [] nusc_box = self.nusc.get_box(anno['token']) # Move them to the ego-pose frame. nusc_box.translate(-np.array(ego_pose_cam['translation'])) nusc_box.rotate(Quaternion(ego_pose_cam['rotation']).inverse) # Move them to the calibrated sensor frame. nusc_box.translate(-np.array(cam_front['translation'])) nusc_box.rotate(Quaternion(cam_front['rotation']).inverse) dists.append(np.linalg.norm(nusc_box.center)) # Filter out the corners that are not in front of the calibrated sensor. #Corners is a 3x8 matrix, first four corners are the ones facing forward, last 4 are ons facing backward #(0,1) top, forward #(2,3) bottom, forward #(4,5) top, backward #(6,7) bottom, backward corners_3d = nusc_box.corners() #Getting first 4 values of Z dists.append(np.mean(corners_3d[2, :4])) # z is height of object for ego pose or lidar # y is height of object for camera frame #TODO: Discover why this is taking the Z axis in_front = np.argwhere(corners_3d[2, :] > 0).flatten() corners_3d = corners_3d[:, in_front] #print(corners_3d) #above = np.argwhere(corners_3d[2, :] > 0).flatten() #corners_3d = corners_3d[:, above] # Project 3d box to 2d. corner_coords = view_points(corners_3d, cam_intrinsic, True).T[:, :2].tolist() #print(corner_coords) # Keep only corners that fall within the image. polygon_from_2d_box = MultiPoint(corner_coords).convex_hull img_canvas = box(0, 0, self._imwidth - 1, self._imheight - 1) if polygon_from_2d_box.intersects(img_canvas): img_intersection = polygon_from_2d_box.intersection(img_canvas) intersection_coords = np.array( [coord for coord in img_intersection.exterior.coords]) min_x = min(intersection_coords[:, 0]) min_y = min(intersection_coords[:, 1]) max_x = max(intersection_coords[:, 0]) max_y = max(intersection_coords[:, 1]) #print('contained pts {}'.format(contained_points)) return [min_x, min_y, max_x, max_y], dists else: return None, dists
def get_relative_pose_np(r1, r2, t1, t2): r1, r2, t1, t2 = r1.astype('f4'), r2.astype('f4'), t1.astype('f4'), t2.astype('f4') pose = [] for i in range(t1.shape[0]): rr = (Quaternion(r2[i]) * Quaternion(r1[i]).inverse).elements R2 = Quaternion(r2[i]).rotation_matrix new_c2 = - np.dot(R2, t2[i]) rt = np.dot(R2, t1[i]) + new_c2 pose.append(np.concatenate((rt / np.linalg.norm(rt), rr))) return np.array(pose)
def test_x_yaw(self): omega_body = np.array([0.1, 0, 0]) roll, pitch, yaw = [0, 0, 90] omega_ned = np.array([0, 0.1, 0]) q = helper.degrees_to_quat_rotation(roll, pitch, yaw) T = helper.T(q) q_dot_ned = T @ omega_body py_quat = Quaternion(*q) py_quat_dot = Quaternion(*q_dot_ned) omega_ned_res = (2 * py_quat_dot * py_quat.conjugate).elements[1:] for a, b in zip(omega_ned, omega_ned_res): self.assertAlmostEqual(a, b)
def __init__(self): # State matrix # The first four are quaternions and the last three are gyro biases. self.x = np.zeros(shape=(State.Size, 1)) # Uncertainitiy covariance matrix self.P = np.eye(State.Size) # State transition matrix self.F = np.zeros(shape=(State.Size, State.Size)) # Measurement jacobian matrix self.H = np.zeros(shape=(Meas.Size, State.Size)) # Measurement covariance matrix self.R = np.eye(Meas.Size) # Process noise covariance matrix self.Q = np.eye(State.Size) self.initializeMatrices() # IMU rotation self.imuRotation = Quaternion(axis=[1., 0., 0.], degrees=180.0).rotation_matrix # Initialize node self.ekfNodeHandle = rospy.init_node('ekf_estimator') # Subscribers # We will be listensing on IMU topic self.imuSubscriber = rospy.Subscriber('imu/data_raw', Imu, self.omImuMessageReceived) # Sort of treated as a ground truth for comparison of accuracy. # You would need imu_tools package. self.magdwickFilterSub = rospy.Subscriber('imu/data', Imu, self.onMagdwickOutputReceived) # Publisher self.cubePublisher = rospy.Publisher('cubevis', Marker, queue_size=10) self.posePublisher = rospy.Publisher('pose', PoseStamped, queue_size=10) # Magdwick self.magdwickOutPublisher = rospy.Publisher('magdwick', PoseStamped, queue_size=10) self.cubeMagdwickPublisher = rospy.Publisher('cubeMadgwick', Marker, queue_size=10) self.timeStamp = None rospy.spin()
def draw_obj(obj_config, obj_cat, ax): for i in range(len(obj_config)): T3, T2, T1, D, W, H, R10, R11, R12, R13, R14, R15, dx, dy, dz, R20, R21, R22, R23, R24, R25 = obj_config[ i] R = rotation_matrix(R10, R11, R12, R13, R14, R15) R2 = rotation_matrix(R20, R21, R22, R23, R24, R25) R = Q._from_matrix(R2) * Q._from_matrix(R) verts = np.array([[T3, T2, T1], [T3, T2, H + T1], [T3, W + T2, T1], [T3, W + T2, H + T1], [D + T3, T2, T1], [D + T3, T2, H + T1], [D + T3, W + T2, T1], [D + T3, W + T2, H + T1]]) verts = np.matmul(R.rotation_matrix, verts.T).T verts += np.array([[dx, dy, dz]]) edges = [[0, 1, 3, 2, 0], [4, 5, 7, 6, 4], [0, 4], [1, 5], [2, 6], [3, 7]] for e in edges: ax.plot(verts[e, 0], verts[e, 1], verts[e, 2], color='red')
def get_frame_data( data ): """ ------------------------------------------------------------------------------------------------------------- get the necessary data for a given sample from the nuScenes database see https://www.nuscenes.org/data-format for the various dictionary keys used here data: [dir] nuScenes dictionary of a sweep data return: [tuple] <cam_trans> <cam_rot> <cam_model> <ego_trans> <ego_rot> ------------------------------------------------------------------------------------------------------------- """ cam_token = data[ 'calibrated_sensor_token' ] # camera data token cam_data = nu.get( 'calibrated_sensor', cam_token ) # data related with the camera in this frame cam_trans = np.array( cam_data[ 'translation' ] ) # camera 3D translation cam_rot = Quaternion( cam_data[ 'rotation' ] ) # camera 3D rotation cam_model = np.array( cam_data[ 'camera_intrinsic' ] ) # camera intrinsic model ego_token = data[ 'ego_pose_token' ] # ego pose token ego_pose = nu.get( 'ego_pose', ego_token ) # ego pose during this frame ego_trans = np.array( ego_pose[ 'translation' ] ) # ego 3D translation ego_rot = Quaternion( ego_pose[ 'rotation' ] ) # ego 3D rotation return cam_trans, cam_rot, cam_model, ego_trans, ego_rot
def publishVisualizationMarker(self): # Visualization marker marker = Marker() marker.header.frame_id = "odom" marker.header.stamp = rospy.Time() marker.type = Marker.CUBE marker.action = Marker.ADD # Position marker.pose.position.x = 1. marker.pose.position.y = 1. marker.pose.position.z = 1. # Orientation flip = Quaternion([0., 0., 0., 1.]) # markerOrient = flip * Quaternion(self.x[0:4, 0]) markerOrient = Quaternion(self.x[0:4, 0]) marker.pose.orientation.w = markerOrient[0] marker.pose.orientation.x = markerOrient[1] marker.pose.orientation.y = markerOrient[2] marker.pose.orientation.z = markerOrient[3] marker.scale.x = 0.7 marker.scale.y = 1. marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 # Publish the axes marker self.cubePublisher.publish(marker) # Pose poseMsg = PoseStamped() poseMsg.header.frame_id = "odom" poseMsg.pose.position.x = 1. poseMsg.pose.position.y = 1. poseMsg.pose.position.z = 1. # Orientation poseMsg.pose.orientation.w = markerOrient[0] poseMsg.pose.orientation.x = markerOrient[1] poseMsg.pose.orientation.y = markerOrient[2] poseMsg.pose.orientation.z = markerOrient[3] self.posePublisher.publish(poseMsg)
def predict(self, gyro, dt): # Using euler integration # If you want to use the more inaccurate euler integration instead of integrating # quaternion using the axis angle representation uncomment this line. Make sure that # the exponential integration is deactivated. # self.useEulerIntegration(gyro, dt) unbiasedGyro = np.subtract(gyro, self.x[State.BX:]) # Quaternion integration # Integrate quaternion using the axis angle representation rotationVec = unbiasedGyro * dt angle = np.linalg.norm(rotationVec) if not np.isclose(angle, 0): quat = Quaternion(axis=rotationVec, angle=angle) else: quat = Quaternion([1., 0., 0., 0.]) result = Quaternion(self.x[State.QW:State.BX, 0]) * quat self.x[State.QW:State.BX, 0] = result.normalised.elements if(self.x[0, 0] < 0): self.x[0, 0] = -self.x[0, 0] self.x[1, 0] = -self.x[1, 0] self.x[2, 0] = -self.x[2, 0] self.x[3, 0] = -self.x[3, 0] self.computeStateTransitionMatrix(gyro, dt)
def onMagdwickOutputReceived(self, imuData:Imu): """ Publish a cube and pose marker to visualize the magdwick algorithm result """ # Visualization marker marker = Marker() marker.header.frame_id = "odom" marker.header.stamp = rospy.Time() marker.type = Marker.CUBE marker.action = Marker.ADD # Position marker.pose.position.x = 0. marker.pose.position.y = 0. marker.pose.position.z = 0. # Orientation flip = Quaternion([0., 0., 0., 1.]) # markerOrient = flip * Quaternion(self.x[0:4, 0]) marker.pose.orientation.w = imuData.orientation.w marker.pose.orientation.x = imuData.orientation.x marker.pose.orientation.y = imuData.orientation.y marker.pose.orientation.z = imuData.orientation.z marker.scale.x = 0.7 marker.scale.y = 1. marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 # Publish the axes marker self.cubeMagdwickPublisher.publish(marker) # Pose poseMsg = PoseStamped() poseMsg.header.frame_id = "odom" poseMsg.pose.position.x = 0. poseMsg.pose.position.y = 0. poseMsg.pose.position.z = 0. # Orientation poseMsg.pose.orientation.w = imuData.orientation.w poseMsg.pose.orientation.x = imuData.orientation.x poseMsg.pose.orientation.y = imuData.orientation.y poseMsg.pose.orientation.z = imuData.orientation.z self.magdwickOutPublisher.publish(poseMsg)
def _rel_pose_after_rotation(self, each_pose): assert each_pose.shape[0] == 14 rotat0 = Quaternion(axis=[0, 0, 1], degrees=0) rotat1 = Quaternion(axis=[0, 0, 1], degrees=-90) rotat2 = Quaternion(axis=[0, 0, 1], degrees=-180) rotat3 = Quaternion(axis=[0, 0, 1], degrees=-270) rotats = [rotat0, rotat1, rotat2, rotat3] q1 = Quaternion(each_pose[3:7]) q2 = Quaternion(each_pose[10:14]) t1 = each_pose[:3] t2 = each_pose[7:10] relative_rotation, relative_translation = [], [] pose1 = [] pose2 = [] for i in range(4): new_q1 = rotats[i] * q1 pose1.append(np.concatenate((t1, new_q1.elements))) for j in range(4): new_q2 = rotats[j] * q2 if i == 0: pose2.append(np.concatenate((t2, new_q2.elements))) relative_rotation.append((new_q2 * new_q1.inverse).elements) relative_translation.append(self.get_relative_T_in_cam2_ref(new_q2.rotation_matrix, t1, t2)) relative_rotation = np.array(relative_rotation) relative_translation = np.array(relative_translation) pose1 = np.array(pose1) pose2 = np.array(pose2) assert pose1.shape[0] == pose2.shape[0] == 4 and pose2.shape[1] == pose1.shape[1] == 7 rel_pose = np.hstack((relative_translation, relative_rotation)).reshape((16, 7)) rel_pose = np.vstack((rel_pose, pose1, pose2, each_pose.reshape((2, 7)) )) return rel_pose.reshape((1, 26, 7))
def convert_bvh_to_quaternion(bvh_q) -> Quaternion: return Quaternion(w=bvh_q[0], x=bvh_q[1], y=bvh_q[2], z=bvh_q[3])
def get_relative_pose(idx): img1, img2 = annot[idx, 0], annot[idx, 8] f1 = txt['/'.join(img1.split('/')[-2:]).replace('.png', '.jpg')][0] f2 = txt['/'.join(img2.split('/')[-2:]).replace('.png', '.jpg')][0] im1 = cv2.imread(img1, 0) # @UndefinedVariable im2 = cv2.imread(img2, 0) # @UndefinedVariable assert im1.shape[0] < im1.shape[1] sift = cv2.xfeatures2d.SIFT_create() # @UndefinedVariable kp1, des1 = sift.detectAndCompute(im1, None) kp2, des2 = sift.detectAndCompute(im2, None) # create BFMatcher object bf = cv2.BFMatcher(cv2.NORM_L2, crossCheck=True) # @UndefinedVariable # Match descriptors. try: matches = bf.match(des1, des2) except cv2.error: # @UndefinedVariable return np.array([0.333, 0.333, 0.3334, 0.25, 0.25, 0.25, 0.25], 'f4') # Sort them in the order of their distance. matches = sorted(matches, key=lambda x: x.distance)[:100] # Draw first 10 matches. # draw_params = dict(#matchColor = (0,255,0), # draw matches in green color # singlePointColor = None, # flags = 2) # img3 = cv2.drawMatches(im1, kp1, im2, kp2, matches[:20], None, **draw_params) # plt.figure(figsize=(20,20)) # plt.imshow(img3) # plt.show() mat1 = get_cameraMatrix(f1, cx=1920, cy=1080) mat2 = get_cameraMatrix(f2, cx=1920, cy=1080) src = np.float32([kp1[m.queryIdx].pt for m in matches]).reshape(-1, 2) dst = np.float32([kp2[m.trainIdx].pt for m in matches]).reshape(-1, 2) src = normalize_point(src, mat1) dst = normalize_point(dst, mat2) src = src[:, :2] dst = dst[:, :2] tmp_results = [] for _threshold in [1e-2, 1e-3, 1e-4]: try: E, mask = cv2.findEssentialMat( np.copy(src), np.copy(dst), cameraMatrix=np.identity(3, 'f4'), threshold=_threshold) # @UndefinedVariable _, R, t, mask, _ = cv2.recoverPose( E, np.copy(src), np.copy(dst), mask=mask, cameraMatrix=np.identity(3, 'f4'), distanceThresh=50) # @UndefinedVariable except cv2.error: # @UndefinedVariable tmp_results.append( np.array([0.333, 0.333, 0.3334, 0.25, 0.25, 0.25, 0.25], 'f4')) continue tmp_results.append( np.concatenate((t.flatten(), np.array( Quaternion(matrix=R).elements).astype('f4').flatten()))) return np.array(tmp_results)
def get_2d_boxes(sample_data_token: str, visibilities: List[str]) -> List[OrderedDict]: """ Get the 2D annotation records for a given `sample_data_token`. :param sample_data_token: Sample data token belonging to a camera keyframe. :param visibilities: Visibility filter. :return: List of 2D annotation record that belongs to the input `sample_data_token` """ # Get the sample data and the sample corresponding to that sample data. sd_rec = nusc.get('sample_data', sample_data_token) assert sd_rec[ 'sensor_modality'] == 'camera', 'Error: get_2d_boxes only works for camera sample_data!' if not sd_rec['is_key_frame']: raise ValueError( 'The 2D re-projections are available only for keyframes.') s_rec = nusc.get('sample', sd_rec['sample_token']) # Get the calibrated sensor and ego pose record to get the transformation matrices. cs_rec = nusc.get('calibrated_sensor', sd_rec['calibrated_sensor_token']) pose_rec = nusc.get('ego_pose', sd_rec['ego_pose_token']) camera_intrinsic = np.array(cs_rec['camera_intrinsic']) # Get all the annotation with the specified visibilties. ann_recs = [ nusc.get('sample_annotation', token) for token in s_rec['anns'] ] ann_recs = [ ann_rec for ann_rec in ann_recs if (ann_rec['visibility_token'] in visibilities) ] repro_recs = [] for ann_rec in ann_recs: # Augment sample_annotation with token information. ann_rec['sample_annotation_token'] = ann_rec['token'] ann_rec['sample_data_token'] = sample_data_token # Get the box in global coordinates. box = nusc.get_box(ann_rec['token']) # Move them to the ego-pose frame. box.translate(-np.array(pose_rec['translation'])) box.rotate(Quaternion(pose_rec['rotation']).inverse) # Move them to the calibrated sensor frame. box.translate(-np.array(cs_rec['translation'])) box.rotate(Quaternion(cs_rec['rotation']).inverse) # Filter out the corners that are not in front of the calibrated sensor. corners_3d = box.corners() in_front = np.argwhere(corners_3d[2, :] > 0).flatten() corners_3d = corners_3d[:, in_front] # Project 3d box to 2d. corner_coords = view_points(corners_3d, camera_intrinsic, True).T[:, :2].tolist() # Keep only corners that fall within the image. final_coords = post_process_coords(corner_coords) # Skip if the convex hull of the re-projected corners does not intersect the image canvas. if final_coords is None: continue else: min_x, min_y, max_x, max_y = final_coords # Generate dictionary record to be included in the .json file. repro_rec = generate_record(ann_rec, min_x, min_y, max_x, max_y, sample_data_token, sd_rec['filename']) repro_recs.append(repro_rec) return repro_recs
def get_relative_pose(idx): img1, img2 = annot[idx, 0], annot[idx, 8] f1 = txt['/'.join(img1.split('/')[-2:]).replace('.png', '.jpg')][0] f2 = txt['/'.join(img2.split('/')[-2:]).replace('.png', '.jpg')][0] im1 = images[idx, 0] im2 = images[idx, 1] assert im1.shape[0] < im1.shape[1] if center_crop: im1 = im1[16:240, 116:340] im2 = im2[16:240, 116:340] sift = cv2.xfeatures2d.SIFT_create() # @UndefinedVariable kp1, des1 = sift.detectAndCompute(im1, None) kp2, des2 = sift.detectAndCompute(im2, None) # create BFMatcher object bf = cv2.BFMatcher(cv2.NORM_L2, crossCheck=True) # @UndefinedVariable # Match descriptors. try: matches = bf.match(des1, des2) except cv2.error: # @UndefinedVariable return np.array([0.333, 0.333, 0.3334, 0.25, 0.25, 0.25, 0.25], 'f4') # Sort them in the order of their distance. matches = sorted(matches, key = lambda x:x.distance)[:100] # Draw first 10 matches. # draw_params = dict(#matchColor = (0,255,0), # draw matches in green color # singlePointColor = None, # flags = 2) # img3 = cv2.drawMatches(im1, kp1, im2, kp2, matches[:20], None, **draw_params) # plt.figure(figsize=(20,20)) # plt.imshow(img3) # plt.show() if center_crop: mat1 = get_cameraMatrix(f1, cx = 224-0.5, cy = 224) mat2 = get_cameraMatrix(f2, cx = 224-0.5, cy = 224) else: mat1 = get_cameraMatrix(f1, cx = 455, cy = 256) mat2 = get_cameraMatrix(f2, cx = 455, cy = 256) src = np.float32([ kp1[m.queryIdx].pt for m in matches ]).reshape(-1,2) dst = np.float32([ kp2[m.trainIdx].pt for m in matches ]).reshape(-1,2) src = normalize_point(src, mat1) dst = normalize_point(dst, mat2) src = src[:, :2] dst = dst[:, :2] try: E, mask = cv2.findEssentialMat(src, dst, cameraMatrix = np.identity(3, 'f4'), threshold = _threshold) # @UndefinedVariable _, R, t, mask, _ = cv2.recoverPose(E, src, dst, mask = mask, cameraMatrix = np.identity(3, 'f4'), distanceThresh = 50) # @UndefinedVariable except cv2.error: # @UndefinedVariable return np.array([0.333, 0.333, 0.3334, 0.25, 0.25, 0.25, 0.25], 'f4') # matchesMask = mask.ravel().tolist() # draw_params = dict(#matchColor = (0,255,0), # draw matches in green color # singlePointColor = None, # matchesMask = matchesMask, # draw only inliers # flags = 2) # img3 = cv2.drawMatches(im1, kp1, im2, kp2, matches, None, **draw_params) # plt.figure(figsize=(20,20)) # plt.imshow(img3) # plt.show() return np.concatenate((t.flatten(), np.array(Quaternion(matrix=R).elements).astype('f4').flatten()))
def convert_xyz_euler_to_quaternion(euler) -> Quaternion: q = p.getQuaternionFromEuler(euler) return Quaternion(x=q[1], y=q[2], z=q[3], w=q[0])
LeftToeBase = 7 # type= JOINT_SPHERICAL Spine = 8 # type= JOINT_FIXED Spine1 = 9 # type= JOINT_SPHERICAL Spine2 = 10 # type= JOINT_SPHERICAL Neck = 11 # type= JOINT_SPHERICAL Head = 12 # type= JOINT_SPHERICAL RightShoulder = 13 # type= JOINT_SPHERICAL RightArm = 14 # type= JOINT_SPHERICAL RightForeArm = 15 # type= JOINT_SPHERICAL RightHand = 16 # type= JOINT_FIXED LeftShoulder = 17 # type= JOINT_SPHERICAL LeftArm = 18 # type= JOINT_SPHERICAL LeftForeArm = 19 # type= JOINT_SPHERICAL LeftHand = 20 # type= JOINT_FIXED LegOrientation = Quaternion(w=0, x=-0.707, y=0, z=0.707) # x -90 z 180 FootOrientation = Quaternion(w=0.422618, x=-0.906308, y=0, z=0) # x -130 ToeBaseOrientation = Quaternion(w=0.707, x=-0.707, y=0, z=0) # x -90 SpineOrientation = Quaternion(w=0.999048, x=0.043619, y=0, z=0) # x 5 Spine1Orientation = Quaternion(w=0.999048, x=0.043619, y=0, z=0) # x 5 Spine2Orientation = Quaternion(w=0.997564, x=0.069756, y=0, z=0) # x 8 NeckOrientation = Quaternion(w=0.996195, x=-0.087156, y=0, z=0) # x -10 HeadOrientation = Quaternion(w=0.92388, x=-0.382683, y=0, z=0) # x -45 # RightShoulderOrientation = Quaternion(w=-0.5, x=0.5, y=-0.5, z=0.5) # x -90 z 270 # RightArmOrientation = Quaternion(w=-0.5, x=0.5, y=-0.5, z=0.5) # RightForeArmOrientation = Quaternion(w=-0.5, x=0.5, y=-0.5, z=0.5) # RightHandOrientation = Quaternion(w=-0.5, x=0.5, y=-0.5, z=0.5) # LeftShoulderOrientation = Quaternion(w=0.5, x=-0.5, y=-0.5, z=0.5) # x -90 z 90 # LeftArmOrientation = Quaternion(w=0.5, x=-0.5, y=-0.5, z=0.5)
est_pose = [] for each_annot in annot: name1, name2 = '/'.join( each_annot[0].split('/')[-2:]), '/'.join( each_annot[8].split('/')[-2:]) pose1 = abspose[name1.replace('relative_cambridge', 'absolute_cambridge')] pose2 = abspose[name2.replace('relative_cambridge', 'absolute_cambridge')] q1, q2 = pose1[3:], pose2[3:] t1, t2 = pose1[:3], pose2[:3] rr = (Quaternion(q2) * Quaternion(q1).inverse).normalised R2 = Quaternion(q2).rotation_matrix new_c2 = -np.dot(R2, t2) rt = np.dot(R2, t1) + new_c2 est_pose.append(np.concatenate((rt, rr.elements))) est_pose = np.array(est_pose) tm = np.linalg.norm(est_pose[:, :3] - gt_pose[:, :3], axis=1) d = np.abs(np.sum(est_pose[:, 3:] * gt_pose[:, 3:], axis=1)) np.putmask(d, d > 1, 1) np.putmask(d, d < -1, -1) rd = 2 * np.arccos(d) * 180 / math.pi est_pose[:, :3] = est_pose[:, :3] / np.linalg.norm(
def get_2d_boxes(sample_data_token: str, visibilities: List[str], mode: str) -> List[OrderedDict]: """ Get the 2D annotation records for a given `sample_data_token`. :param sample_data_token: Sample data token belonging to a camera keyframe. :param visibilities: Visibility filter. :param mode: 'xywh' or 'xyxy' :return: List of 2D annotation record that belongs to the input `sample_data_token` """ # Get the sample data and the sample corresponding to that sample data. sd_rec = nusc.get('sample_data', sample_data_token) assert sd_rec['sensor_modality'] == 'camera', 'Error: get_2d_boxes only works for camera sample_data!' if not sd_rec['is_key_frame']: raise ValueError('The 2D re-projections are available only for keyframes.') s_rec = nusc.get('sample', sd_rec['sample_token']) # Get the calibrated sensor and ego pose record to get the transformation matrices. cs_rec = nusc.get('calibrated_sensor', sd_rec['calibrated_sensor_token']) pose_rec = nusc.get('ego_pose', sd_rec['ego_pose_token']) camera_intrinsic = np.array(cs_rec['camera_intrinsic']) # Get all the annotation with the specified visibilties. ann_recs = [nusc.get('sample_annotation', token) for token in s_rec['anns']] ann_recs = [ann_rec for ann_rec in ann_recs if (ann_rec['visibility_token'] in visibilities)] sd_annotations = [] for ann_rec in ann_recs: # Get the 3D box in global coordinates. box = nusc.get_box(ann_rec['token']) # Calculate distance from vehicle to box ego_translation = (box.center[0] - pose_rec['translation'][0], box.center[1] - pose_rec['translation'][1], box.center[2] - pose_rec['translation'][2]) ego_dist = np.sqrt(np.sum(np.array(ego_translation[:2]) ** 2)) dist = round(ego_dist,2) # Move them to the ego-pose frame. box.translate(-np.array(pose_rec['translation'])) box.rotate(Quaternion(pose_rec['rotation']).inverse) # Move them to the calibrated sensor frame. box.translate(-np.array(cs_rec['translation'])) box.rotate(Quaternion(cs_rec['rotation']).inverse) # Filter out the corners that are not in front of the calibrated sensor. corners_3d = box.corners() in_front = np.argwhere(corners_3d[2, :] > 0).flatten() corners_3d = corners_3d[:, in_front] # Project 3d box to 2d. corner_coords = view_points(corners_3d, camera_intrinsic, True).T[:, :2].tolist() # Keep only corners that fall within the image. final_coords = post_process_coords(corner_coords) # Skip if the convex hull of the re-projected corners does not intersect the image canvas. if final_coords is None: continue min_x, min_y, max_x, max_y = final_coords if mode == 'xywh': bbox = [min_x, min_y, max_x-min_x, max_y-min_y] else: bbox = [min_x, min_y, max_x, max_y] ## Generate 2D record to be included in the .json file. ann_2d = {} ann_2d['sample_data_token'] = sample_data_token ann_2d['bbox'] = np.around(bbox, 2).tolist() ann_2d['distance'] = dist ann_2d['category_name'] = ann_rec['category_name'] ann_2d['num_lidar_pts'] = ann_rec['num_lidar_pts'] ann_2d['num_radar_pts'] = ann_rec['num_radar_pts'] ann_2d['visibility_token'] = ann_rec['visibility_token'] sd_annotations.append(ann_2d) return sd_annotations
def change_of_basis(orientation: Quaternion, conventions: Quaternion) -> Quaternion: return conventions.__mul__(orientation).__mul__(conventions.inverse)
def get_2d_boxes(self, index: int, visibilities: List[str]) -> List[List]: """ Get the 2D annotation records for a given `sample_data_token`. :param sample_data_token: Sample data token belonging to a camera keyframe. :param visibilities: Visibility filter. :return: [catagory, min_x, min_y, max_x, max_y],format:xyxy,absolute_coor_value List of 2D annotation record that belongs to the input `sample_data_token` """ # Gettign data from nuscenes database sample_token = self.sample_tokens[index] sample = self.nusc.get('sample', sample_token) # Gettign sample_data_token from sensor['CAM_FRONT'] sample_data_token = sample['data'][self.camera_sensors[0]] # Get the sample data and the sample corresponding to that sample data. sd_rec = self.nusc.get('sample_data', sample_data_token) assert sd_rec[ 'sensor_modality'] == 'camera', 'Error: get_2d_boxes only works for camera sample_data!' if not sd_rec['is_key_frame']: raise ValueError( 'The 2D re-projections are available only for keyframes.') s_rec = self.nusc.get('sample', sd_rec['sample_token']) # Get the calibrated sensor and ego pose record to get the transformation matrices. cs_rec = self.nusc.get('calibrated_sensor', sd_rec['calibrated_sensor_token']) pose_rec = self.nusc.get('ego_pose', sd_rec['ego_pose_token']) camera_intrinsic = np.array(cs_rec['camera_intrinsic']) # Get all the annotation with the specified visibilties. ann_recs = [ self.nusc.get('sample_annotation', token) for token in s_rec['anns'] ] ann_recs = [ ann_rec for ann_rec in ann_recs if (ann_rec['visibility_token'] in visibilities) ] repro_recs = [] bboxes = [] for ann_rec in ann_recs: # Augment sample_annotation with token information. ann_rec['sample_annotation_token'] = ann_rec['token'] ann_rec['sample_data_token'] = sample_data_token # Get the box in global coordinates. box = self.nusc.get_box(ann_rec['token']) # Move them to the ego-pose frame. box.translate(-np.array(pose_rec['translation'])) box.rotate(Quaternion(pose_rec['rotation']).inverse) # Move them to the calibrated sensor frame. box.translate(-np.array(cs_rec['translation'])) box.rotate(Quaternion(cs_rec['rotation']).inverse) # Filter out the corners that are not in front of the calibrated sensor. corners_3d = box.corners() in_front = np.argwhere(corners_3d[2, :] > 0).flatten() corners_3d = corners_3d[:, in_front] # Project 3d box to 2d. corner_coords = view_points(corners_3d, camera_intrinsic, True).T[:, :2].tolist() # Keep only corners that fall within the image. final_coords = self.post_process_coords(corner_coords) # Skip if the convex hull of the re-projected corners does not intersect the image canvas. if final_coords is None: continue else: min_x, min_y, max_x, max_y = final_coords min_x = min_x / 1600 * self.image_max_side max_x = max_x / 1600 * self.image_max_side min_y = min_y / 900 * self.image_min_side max_y = max_y / 900 * self.image_min_side category = ann_rec['category_name'] #这里进行了类别的过滤! if category in list(self.classes.keys()): category_digit = self.classes[category] bbox = [category_digit, min_x, min_y, max_x, max_y] bboxes.append(bbox) else: continue if bboxes == []: bboxes.append([-1, 0, 0, 0, 0]) #添加-1为无anno,在可视化或者训练时要跳过处理 print(sample_token) print("有一帧没有gt_box!!!") return bboxes return bboxes
def normalizeQuaternion(self): """ Normalize the quaternion in the state vector """ self.x[State.QW:State.BX, 0] = Quaternion(self.x[State.QW:State.BX, 0]).normalised.elements
def get_2d_boxes(sample_data_token: str, visibilities: List[str]) -> List[OrderedDict]: """ Get the 2D annotation records for a given `sample_data_token`. :param sample_data_token: Sample data token belonging to a keyframe. :param visibilities: Visibility filter. :return: List of 2D annotation record that belongs to the input `sample_data_token` """ # Get the sample data and the sample corresponding to that sample data. sd_rec = nusc.get('sample_data', sample_data_token) if not sd_rec['is_key_frame']: raise ValueError( 'The 2D re-projections are available only for keyframes.') s_rec = nusc.get('sample', sd_rec['sample_token']) # Get the calibrated sensor and ego pose record to get the transformation matrices. cs_rec = nusc.get('calibrated_sensor', sd_rec['calibrated_sensor_token']) pose_rec = nusc.get('ego_pose', sd_rec['ego_pose_token']) camera_intrinsic = np.array(cs_rec['camera_intrinsic']) # Get all the annotation with the specified visibilties. ann_recs = [ nusc.get('sample_annotation', token) for token in s_rec['anns'] ] ann_recs = [ ann_rec for ann_rec in ann_recs if (ann_rec['visibility_token'] in visibilities) ] # Only for cars category ann_recs = [ ann_rec for ann_rec in ann_recs if (ann_rec['category_name'] == "vehicle.car") ] #Determine token for parked attribute for att in nusc.attribute: if att['name'] == 'vehicle.parked': parked_token = att['token'] if att['name'] == 'vehicle.stopped': stopped_token = att['token'] # Only get the parked && stopped atrribute ann_recs = [ ann_rec for ann_rec in ann_recs if (parked_token in ann_rec['attribute_tokens'] or stopped_token in ann_rec['attribute_tokens']) ] repro_recs = [sd_rec['filename']] for ann_rec in ann_recs: if parked_token in ann_rec['attribute_tokens']: vehicle_state = 1 elif stopped_token in ann_rec['attribute_tokens']: vehicle_state = 2 ann_rec['sample_annotation_token'] = ann_rec['token'] ann_rec['sample_data_token'] = sample_data_token box = nusc.get_box(ann_rec['token']) # Move them to the ego-pose frame. box.translate(-np.array(pose_rec['translation'])) box.rotate(Quaternion(pose_rec['rotation']).inverse) # Move them to the calibrated sensor frame. box.translate(-np.array(cs_rec['translation'])) box.rotate(Quaternion(cs_rec['rotation']).inverse) # Filter out the corners that are not in front of the calibrated sensor. corners_3d = box.corners() in_front = np.argwhere(corners_3d[2, :] > 0).flatten() corners_3d = corners_3d[:, in_front] # Project 3d box to 2d. corner_coords = view_points(corners_3d, camera_intrinsic, True).T[:, :2].tolist() # Keep only corners that fall within the image. final_coords = post_process_coords(corner_coords) # Skip if the convex hull of the re-projected corners does not intersect the image canvas. if final_coords is None: continue else: min_x, min_y, max_x, max_y = final_coords # Generate dictionary record to be included in the .json file. #repro_rec = generate_record(ann_rec, min_x, min_y, max_x, max_y, sample_data_token, sd_rec['filename']) repro_rec = [ vehicle_state, (max_x + min_x) / (2 * 1600), (max_y + min_y) / (2 * 900), (max_x - min_x) / (1600), (max_y - min_y) / (900) ] repro_recs.append(repro_rec) if (len(repro_recs) == 1): return [] return repro_recs
def get_2d_boxes(sample_data_token: str, visibilities: List[str], dataroot: str, box_image_dir: str) -> List[OrderedDict]: """ Get the 2D annotation records for a given `sample_data_token`. :param sample_data_token: Sample data token belonging to a camera keyframe. :param visibilities: Visibility filter. :return: List of 2D annotation record that belongs to the input `sample_data_token` """ # Get the sample data and the sample corresponding to that sample data. sd_rec = nusc.get('sample_data', sample_data_token) s_rec = nusc.get('sample', sd_rec['sample_token']) # Get the calibrated sensor and ego pose record to get the transformation matrices. cs_rec = nusc.get('calibrated_sensor', sd_rec['calibrated_sensor_token']) pose_rec = nusc.get('ego_pose', sd_rec['ego_pose_token']) camera_intrinsic = np.array(cs_rec['camera_intrinsic']) # Get all the annotation with the specified visibilties and in detection benchmark ann_recs = [ nusc.get('sample_annotation', token) for token in s_rec['anns'] ] ann_recs = [ann_rec for ann_rec in ann_recs if (ann_rec['visibility_token'] in visibilities and \ ann_rec['category_name'] in category2class.keys())] repro_recs = [] scene_image = Image.open(os.path.join(dataroot, sd_rec['filename'])) for i, ann_rec in enumerate(ann_recs): # Augment sample_annotation with token information. ann_rec['sample_annotation_token'] = ann_rec['token'] ann_rec['sample_data_token'] = sample_data_token # Get the box in global coordinates. box = nusc.get_box(ann_rec['token']) # Move them to the ego-pose frame. box.translate(-np.array(pose_rec['translation'])) box.rotate(Quaternion(pose_rec['rotation']).inverse) # Move them to the calibrated sensor frame. box.translate(-np.array(cs_rec['translation'])) box.rotate(Quaternion(cs_rec['rotation']).inverse) # Filter out the corners that are not in front of the calibrated sensor. corners_3d = box.corners() in_front = np.argwhere(corners_3d[2, :] > 0).flatten() corners_3d = corners_3d[:, in_front] # Project 3d box to 2d. corner_coords = view_points(corners_3d, camera_intrinsic, True).T[:, :2].tolist() # Keep only corners that fall within the image. final_coords = post_process_coords(corner_coords) # Skip if the convex hull of the re-projected corners does not intersect the image canvas. if final_coords is None: continue else: min_x, min_y, max_x, max_y = final_coords if max_x - min_x < 25.0 or max_y - min_y < 25.0: continue # Generate dictionary record to be included in the .json file. repro_rec = generate_record(ann_rec, min_x, min_y, max_x, max_y, sample_data_token, sd_rec['filename']) # add dimensions, location repro_rec['dimensions'] = box.wlh.tolist() repro_rec['location'] = box.center.tolist() # add theta_l (approximate) theta_ray = np.arctan2(repro_rec['location'][2], repro_rec['location'][0]) front = box.orientation.rotate(np.array([1.0, 0.0, 0.0])) ry = -np.arctan2(front[2], front[0]) theta_l = np.pi - theta_ray - ry if theta_l > np.pi: theta_l -= 2.0 * np.pi if theta_l < -np.pi: theta_l += 2.0 * np.pi repro_rec['theta_l'] = theta_l # save box image box_image = scene_image.resize((224, 224), box=final_coords) box_image_file = os.path.join( box_image_dir, 'camera__{}__{}.png'.format(sample_data_token, i)) box_image.save(box_image_file) repro_rec['box_image_file'] = os.path.join( *box_image_file.split('/')[-4:]) repro_recs.append(repro_rec) return repro_recs