def test_quaternion_renormalized():
"""Make sure that a quaternion is correctly re-normalized.
Normalized and unnormalized quaternion variants should generate the same 3d rotation matrix.
"""
q1 = np.array([0.0, 0.0, 0.0, 1.0])
R1 = quat2rotmat(q1)
q2 = np.array([0.0, 0.0, 0.0, 2.0])
R2 = quat2rotmat(q2)
assert np.allclose(R1, R2)
def label_to_bbox(label: _LabelType) -> Tuple[np.ndarray, float]:
"""Convert a label into a parameterized bounding box that lives in the ego-vehicle
coordinate frame.
Args:
label: _LabelType
Returns:
bbox: Numpmy array for bounding box itself
orientation: angle in radians, representing bounding box yaw
"""
length = label["length"]
width = label["width"]
height = label["height"]
p0 = np.array([-length / 2, -width / 2, -height / 2])[:, np.newaxis]
p1 = np.array([+length / 2, -width / 2, -height / 2])[:, np.newaxis]
p2 = np.array([-length / 2, +width / 2, -height / 2])[:, np.newaxis]
bbox = np.array([p0, p1, p2, height])
R = quat2rotmat((label["rotation"]["w"], label["rotation"]["x"],
label["rotation"]["y"], label["rotation"]["z"]))
t = np.array(
[label["center"]["x"], label["center"]["y"],
label["center"]["z"]])[:, np.newaxis]
v = np.array([1, 0, 0])[:, np.newaxis]
orientation = np.matmul(R, v)
orientation = np.arctan2(orientation[1, 0], orientation[0, 0])
return transform_bounding_box_3d(bbox, R, t), orientation
def test_quat2rotmat_1() -> None:
"""Test receiving a quaternion in (w, x, y, z) from a camera extrinsic matrix."""
q = np.array([1.0, 0.0, 0.0, 0.0])
R = quat2rotmat(q)
assert np.allclose(R, quat2rotmat_numpy(q))
R_gt = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
assert np.allclose(R_gt, R)
def label_to_bbox(label: _LabelType) -> Tuple[np.ndarray, np.ndarray]:
"""Convert a label into a bounding box.
Args:
label: _LabelType
Returns:
bbox: nupmy array for bounding box itself
orientation: numpy array for bounding box orientation
"""
length = label["length"]
width = label["width"]
height = label["height"]
p0 = np.array([-length / 2, -width / 2, -height / 2])[:, np.newaxis]
p1 = np.array([+length / 2, -width / 2, -height / 2])[:, np.newaxis]
p2 = np.array([-length / 2, +width / 2, -height / 2])[:, np.newaxis]
bbox = np.array([p0, p1, p2, height])
R = quat2rotmat((label["rotation"]["w"], label["rotation"]["x"],
label["rotation"]["y"], label["rotation"]["z"]))
t = np.array(
[label["center"]["x"], label["center"]["y"],
label["center"]["z"]])[:, np.newaxis]
v = np.array([1, 0, 0])[:, np.newaxis]
orientation = np.matmul(R, v)
orientation = np.arctan2(orientation[1, 0], orientation[0, 0])
return transform_bounding_box_3d(bbox, R, t), orientation
def as_3d_bbox(self) -> np.ndarray:
r"""Calculate the 8 bounding box corners.
Args:
None
Returns:
Numpy array of shape (8,3)
Corner numbering::
5------4
|\\ |\\
| \\ | \\
6--\\--7 \\
\\ \\ \\ \\
l \\ 1-------0 h
e \\ || \\ || e
n \\|| \\|| i
g \\2------3 g
t width. h
h. t.
First four corners are the ones facing forward.
The last four are the ones facing backwards.
"""
# 3D bounding box corners. (Convention: x points forward, y to the left, z up.)
x_corners = self.length / 2 * np.array([1, 1, 1, 1, -1, -1, -1, -1])
y_corners = self.width / 2 * np.array([1, -1, -1, 1, 1, -1, -1, 1])
z_corners = self.height / 2 * np.array([1, 1, -1, -1, 1, 1, -1, -1])
corners_object_frame = np.vstack((x_corners, y_corners, z_corners)).T
egovehicle_SE3_object = SE3(rotation=quat2rotmat(self.quaternion), translation=self.translation)
corners_egovehicle_frame = egovehicle_SE3_object.transform_point_cloud(corners_object_frame)
return corners_egovehicle_frame
def visualize_ground_lidar_pts(log_id: str, dataset_dir: str,
experiment_prefix: str):
"""Process a log by drawing the LiDAR returns that are classified as belonging
to the ground surface in a red to green colormap in the image.
Args:
log_id: The ID of a log
dataset_dir: Where the dataset is stored
experiment_prefix: Output prefix
"""
sdb = SynchronizationDB(dataset_dir, collect_single_log_id=log_id)
city_info_fpath = f"{dataset_dir}/{log_id}/city_info.json"
city_info = read_json_file(city_info_fpath)
city_name = city_info["city_name"]
avm = ArgoverseMap()
ply_fpaths = sorted(glob.glob(f"{dataset_dir}/{log_id}/lidar/PC_*.ply"))
for i, ply_fpath in enumerate(ply_fpaths):
if i % 500 == 0:
print(f"\tOn file {i} of {log_id}")
lidar_timestamp_ns = ply_fpath.split("/")[-1].split(".")[0].split(
"_")[-1]
pose_fpath = f"{dataset_dir}/{log_id}/poses/city_SE3_egovehicle_{lidar_timestamp_ns}.json"
if not Path(pose_fpath).exists():
continue
pose_data = read_json_file(pose_fpath)
rotation = np.array(pose_data["rotation"])
translation = np.array(pose_data["translation"])
city_to_egovehicle_se3 = SE3(rotation=quat2rotmat(rotation),
translation=translation)
lidar_pts = load_ply(ply_fpath)
lidar_timestamp_ns = int(lidar_timestamp_ns)
draw_ground_pts_in_image(
sdb,
lidar_pts,
city_to_egovehicle_se3,
avm,
log_id,
lidar_timestamp_ns,
city_name,
dataset_dir,
experiment_prefix,
)
for camera_name in CAMERA_LIST:
if "stereo" in camera_name:
fps = 5
else:
fps = 10
cmd = f"ffmpeg -r {fps} -f image2 -i '{experiment_prefix}_ground_viz/{log_id}/{camera_name}/%*.jpg' {experiment_prefix}_ground_viz/{experiment_prefix}_{log_id}_{camera_name}_{fps}fps.mp4"
print(cmd)
run_command(cmd)
def test_leave_only_roi_region():
"""
test leave_only_roi_region function
(lidar_pts,egovehicle_to_city_se3,ground_removal_method, city_name='MIA')
"""
pc = ply_loader.load_ply(TEST_DATA_LOC / "1/lidar/PC_0.ply")
pose_data = read_json_file(TEST_DATA_LOC / "1/poses/city_SE3_egovehicle_0.json")
rotation = np.array(pose_data["rotation"])
translation = np.array(pose_data["translation"])
ego_R = quat2rotmat(rotation)
ego_t = translation
egovehicle_to_city_se3 = SE3(rotation=ego_R, translation=ego_t)
pts = eval_utils.leave_only_roi_region(pc, egovehicle_to_city_se3, ground_removal_method="map")
def test_quat2rotmat_4() -> None:
"""Test receiving a quaternion in (w, x, y, z) from an object trajectory."""
q = np.array([
0.0036672729619914197, -1.3748614058859026e-05,
-0.00023389080405946338, 0.9999932480847505
])
R = quat2rotmat(q)
assert np.allclose(R, quat2rotmat_numpy(q))
R_gt = np.array([
[-9.99973102e-01, -7.33448997e-03, -2.92125253e-05],
[7.33450283e-03, -9.99972993e-01, -4.67677610e-04],
[-2.57815596e-05, -4.67879290e-04, 9.99999890e-01],
])
assert np.allclose(R_gt, R)
def test_quat2rotmat_3() -> None:
"""Test receiving a quaternion in (w, x, y, z) from a camera extrinsic matrix."""
q = np.array([
0.6115111374269877, -0.6173269265351116, -0.3480540121107544,
0.3518806604959585
])
R = quat2rotmat(q)
assert np.allclose(R, quat2rotmat_numpy(q))
R_gt = np.array([
[5.10076811e-01, -6.31658748e-04, -8.60128623e-01],
[8.60084113e-01, -9.82506691e-03, 5.10057631e-01],
[-8.77300364e-03, -9.99951533e-01, -4.46825914e-03],
])
assert np.allclose(R_gt, R)
def test_quat2rotmat_2() -> None:
"""Test receiving a quaternion in (w, x, y, z) from a camera extrinsic matrix."""
q = np.array([
0.4962730586309743, -0.503110985154011, 0.4964713836540661,
-0.5040918101963521
])
R = quat2rotmat(q)
assert np.allclose(R, quat2rotmat_numpy(q))
R_gt = np.array([
[-1.18477579e-03, 7.73955092e-04, 9.99998999e-01],
[-9.99894783e-01, -1.44584330e-02, -1.17346213e-03],
[1.44575103e-02, -9.99895172e-01, 7.91003660e-04],
])
assert np.allclose(R_gt, R)
def test_quat2rotmat_5() -> None:
"""Test receiving a quaternion in (w, x, y, z) from an object trajectory."""
q = np.array([
0.9998886199825181, -0.002544078377693514, -0.0028621717588219564,
-0.01442509159370476
])
R = quat2rotmat(q)
assert np.allclose(R, quat2rotmat_numpy(q))
R_gt = np.array([
[0.99956745, 0.02886153, -0.00565031],
[-0.02883241, 0.99957089, 0.00517016],
[0.0057971, -0.00500502, 0.99997067],
])
assert np.allclose(R_gt, R)
def test_rotmat2quat() -> None:
"""Ensure `rotmat2quat()` correctly converts rotation matrices to scalar-first quaternions."""
num_trials = 1000
for trial in range(num_trials):
# generate random rotation matrices by sampling quaternion elements from normal distribution
# https://en.wikipedia.org/wiki/Rotation_matrix#Uniform_random_rotation_matrices
q = np.random.randn(4)
q /= np.linalg.norm(q)
R = quat2rotmat(q)
q_ = rotmat2quat(R)
# Note: A unit quaternion multiplied by 1 or -1 represents the same 3d rotation
# https://math.stackexchange.com/questions/2016282/negative-quaternion
# https://math.stackexchange.com/questions/1790521/unit-quaternion-multiplied-by-1
assert np.allclose(q, q_) or np.allclose(-q, q_)
def get_city_to_egovehicle_se3(self, log_id: str,
timestamp: int) -> Optional[SE3]:
"""
Args:
log_id: str, unique ID of vehicle log
timestamp: int, timestamp of sensor observation, in nanoseconds
Returns:
city_to_egovehicle_se3: SE3 transformation to bring egovehicle frame point into city frame.
"""
pose_fpath = f"{self.data_dir}/{log_id}/poses/city_SE3_egovehicle_{timestamp}.json"
if not Path(pose_fpath).exists():
return None
pose_data = read_json_file(pose_fpath)
rotation = np.array(pose_data["rotation"])
translation = np.array(pose_data["translation"])
city_to_egovehicle_se3 = SE3(rotation=quat2rotmat(rotation),
translation=translation)
return city_to_egovehicle_se3
def get_city_SE3_egovehicle_at_sensor_t(sensor_timestamp: int, dataset_dir: str, log_id: str) -> Optional[SE3]:
"""Get transformation from ego-vehicle to city coordinates at a given timestamp.
Args:
sensor_timestamp: integer representing timestamp when sensor measurement captured, in nanoseconds
dataset_dir:
log_id: string representing unique log identifier
Returns:
SE3 for translating ego-vehicle coordinates to city coordinates if found, else None.
"""
pose_fpath = f"{dataset_dir}/{log_id}/poses/city_SE3_egovehicle_{sensor_timestamp}.json"
if not Path(pose_fpath).exists():
logger.error(f"missing pose {sensor_timestamp}")
return None
pose_city_to_ego = read_json_file(pose_fpath)
rotation = np.array(pose_city_to_ego["rotation"])
translation = np.array(pose_city_to_ego["translation"])
city_to_egovehicle_se3 = SE3(rotation=quat2rotmat(rotation), translation=translation)
return city_to_egovehicle_se3
def get_camera_extrinsic_matrix(config: Dict[str, Any]) -> np.ndarray:
"""Load camera calibration rotation and translation.
Note that the camera calibration file contains the SE3 for sensor frame to the vehicle frame, i.e.
pt_egovehicle = egovehicle_SE3_sensor * pt_sensor
Then build extrinsic matrix from rotation matrix and translation, a member
of SE3. Then we return the inverse of the SE3 transformation.
Args:
config: Calibration config in json, or calibration file path.
Returns:
Camera rotation and translation matrix.
"""
vehicle_SE3_sensor = config["vehicle_SE3_camera_"]
egovehicle_t_camera = np.array(vehicle_SE3_sensor["translation"])
egovehicle_q_camera = vehicle_SE3_sensor["rotation"]["coefficients"]
egovehicle_R_camera = quat2rotmat(egovehicle_q_camera)
egovehicle_T_camera = SE3(rotation=egovehicle_R_camera, translation=egovehicle_t_camera)
return egovehicle_T_camera.inverse().transform_matrix
def as_2d_bbox(self) -> np.ndarray:
"""Construct a 2D bounding box from this label.
Length is x, width is y, and z is height
Alternatively could write code like::
x_corners = l / 2 * np.array([1, 1, 1, 1, -1, -1, -1, -1])
y_corners = w / 2 * np.array([1, -1, -1, 1, 1, -1, -1, 1])
z_corners = h / 2 * np.array([1, 1, -1, -1, 1, 1, -1, -1])
corners = np.vstack((x_corners, y_corners, z_corners))
"""
bbox_object_frame = np.array([
[self.length / 2.0, self.width / 2.0, self.height / 2.0],
[self.length / 2.0, -self.width / 2.0, self.height / 2.0],
[-self.length / 2.0, self.width / 2.0, self.height / 2.0],
[-self.length / 2.0, -self.width / 2.0, self.height / 2.0],
])
egovehicle_SE3_object = SE3(rotation=quat2rotmat(self.quaternion),
translation=self.translation)
bbox_in_egovehicle_frame = egovehicle_SE3_object.transform_point_cloud(
bbox_object_frame)
return bbox_in_egovehicle_frame
def main(args: argparse.Namespace) -> None:
OUTPUT_ROOT = args.argo_dir
NUSCENES_ROOT = args.nuscenes_dir
NUSCENES_VERSION = args.nuscenes_version
if not os.path.exists(OUTPUT_ROOT):
os.makedirs(OUTPUT_ROOT)
nusc = NuScenes(version=NUSCENES_VERSION,
dataroot=NUSCENES_ROOT,
verbose=True)
for scene in nusc.scene:
scene_token = scene["token"]
sample_token = scene["first_sample_token"]
scene_path = os.path.join(OUTPUT_ROOT, scene_token)
if not os.path.exists(scene_path):
os.makedirs(scene_path)
log_token = scene["log_token"]
nusc_log = nusc.get("log", log_token)
nusc_city = nusc_log["location"]
with open(os.path.join(scene_path, f"city_info.json"), "w") as f:
json.dump({"city_name": CITY_TO_ID[nusc_city]}, f)
# Calibration info for all the sensors
calibration_info = get_calibration_info(nusc, scene)
calib_path = os.path.join(scene_path, f"vehicle_calibration_info.json")
with open(calib_path, "w") as f:
json.dump(calibration_info, f)
while sample_token != "":
sample = nusc.get("sample", sample_token)
timestamp = round_to_micros(sample["timestamp"])
tracked_labels = []
# city_SE3_vehicle pose
ego_pose = None
# Copy nuscenes sensor data into argoverse format and get the pose of the vehicle in the city frame
for sensor, sensor_token in sample["data"].items():
if sensor in SENSOR_NAMES:
argo_sensor = SENSOR_NAMES[sensor]
output_sensor_path = os.path.join(scene_path, argo_sensor)
if not os.path.exists(output_sensor_path):
os.makedirs(output_sensor_path)
sensor_data = nusc.get("sample_data", sensor_token)
file_path = os.path.join(NUSCENES_ROOT,
sensor_data["filename"])
if sensor == "LIDAR_TOP":
# nuscenes lidar data is stored as (x, y, z, intensity, ring index)
scan = np.fromfile(file_path, dtype=np.float32)
points = scan.reshape((-1, 5))
# Transform lidar points from point sensor frame to egovehicle frame
calibration = nusc.get(
"calibrated_sensor",
sensor_data["calibrated_sensor_token"])
egovehicle_R_lidar = quat2rotmat(
calibration["rotation"])
egovehicle_t_lidar = np.array(
calibration["translation"])
egovehicle_SE3_lidar = SE3(
rotation=egovehicle_R_lidar,
translation=egovehicle_t_lidar)
points_egovehicle = egovehicle_SE3_lidar.transform_point_cloud(
points[:, :3])
extract_pc(points_egovehicle, points,
output_sensor_path, timestamp)
else:
shutil.copy(
file_path,
os.path.join(output_sensor_path,
f"{argo_sensor}_{timestamp}.jpg"),
)
if ego_pose is None:
ego_pose = nusc.get("ego_pose",
sensor_data["ego_pose_token"])
# Save ego pose to json file
poses_path = os.path.join(scene_path, f"poses")
if not os.path.exists(poses_path):
os.makedirs(poses_path)
ego_pose_dict = {
"rotation": ego_pose["rotation"],
"translation": ego_pose["translation"],
}
with open(
os.path.join(poses_path,
f"city_SE3_egovehicle_{timestamp}.json"),
"w") as f:
json.dump(ego_pose_dict, f)
# Object annotations
labels_path = os.path.join(scene_path,
f"per_sweep_annotations_amodal")
if not os.path.exists(labels_path):
os.makedirs(labels_path)
for ann_token in sample["anns"]:
annotation = nusc.get("sample_annotation", ann_token)
city_SE3_object = SE3(
quat2rotmat(annotation["rotation"]),
np.array(annotation["translation"]),
)
city_SE3_egovehicle = SE3(quat2rotmat(ego_pose["rotation"]),
np.array(ego_pose["translation"]))
egovehicle_SE3_city = city_SE3_egovehicle.inverse()
egovehicle_SE3_object = egovehicle_SE3_city.right_multiply_with_se3(
city_SE3_object)
x, y, z = egovehicle_SE3_object.translation
qw, qx, qy, qz = Quaternion(
matrix=egovehicle_SE3_object.rotation)
width, length, height = annotation["size"]
label_class = annotation["category_name"]
tracked_labels.append({
"center": {
"x": x,
"y": y,
"z": z
},
"rotation": {
"x": qx,
"y": qy,
"z": qz,
"w": qw
},
"length":
length,
"width":
width,
"height":
height,
"track_label_uuid":
annotation["instance_token"],
"timestamp":
timestamp,
"label_class":
get_argo_label(label_class),
})
json_fpath = os.path.join(
labels_path, f"tracked_object_labels_{timestamp}.json")
with open(json_fpath, "w") as f:
json.dump(tracked_labels, f)
sample_token = sample["next"]
def eval_tracks(
path_tracker_output: str,
path_dataset: _PathLike,
d_min: float,
d_max: float,
out_file: TextIO,
centroid_method: str,
) -> None:
"""Evaluate tracking output.
Args:
path_tracker_output: path to tracker output
path_dataset: path to dataset
d_min: minimum distance range
d_max: maximum distance range
out_file: output file object
centroid_method: method for ground truth centroid estimation
"""
acc = mm.MOTAccumulator(auto_id=True)
path_track_data = sorted(glob.glob(os.fspath(path_tracker_output) + "/*"))
log_id = pathlib.Path(path_dataset).name
logger.info("log_id = %s", log_id)
city_info_fpath = f"{path_dataset}/city_info.json"
city_info = read_json_file(city_info_fpath)
city_name = city_info["city_name"]
logger.info("city name = %s", city_name)
ID_gt_all: List[str] = []
for ind_frame in range(len(path_track_data)):
if ind_frame % 50 == 0:
logger.info("%d/%d" % (ind_frame, len(path_track_data)))
timestamp_lidar = int(path_track_data[ind_frame].split("/")[-1].split("_")[-1].split(".")[0])
path_gt = os.path.join(
path_dataset, "per_sweep_annotations_amodal", f"tracked_object_labels_{timestamp_lidar}.json"
)
if not os.path.exists(path_gt):
logger.warning("Missing ", path_gt)
continue
gt_data = read_json_file(path_gt)
pose_data = read_json_file(f"{path_dataset}/poses/city_SE3_egovehicle_{timestamp_lidar}.json")
rotation = np.array(pose_data["rotation"])
translation = np.array(pose_data["translation"])
ego_R = quat2rotmat(rotation)
ego_t = translation
egovehicle_to_city_se3 = SE3(rotation=ego_R, translation=ego_t)
pc_raw0 = load_ply(os.path.join(path_dataset, f"lidar/PC_{timestamp_lidar}.ply"))
pc_raw_roi = leave_only_roi_region(
pc_raw0, egovehicle_to_city_se3, ground_removal_method="no", city_name=city_name
)
gt: Dict[str, Dict[str, Any]] = {}
id_gts = []
for i in range(len(gt_data)):
if gt_data[i]["label_class"] != "VEHICLE":
continue
bbox, orientation = label_to_bbox(gt_data[i])
pc_segment = get_pc_inside_bbox(pc_raw_roi, bbox)
center = np.array([gt_data[i]["center"]["x"], gt_data[i]["center"]["y"], gt_data[i]["center"]["z"]])
if (
len(pc_segment) >= min_point_num
and bbox[3] > 0
and in_distance_range_pose(np.zeros(3), center, d_min, d_max)
):
track_label_uuid = gt_data[i]["track_label_uuid"]
gt[track_label_uuid] = {}
if centroid_method == "average":
gt[track_label_uuid]["centroid"] = pc_segment.sum(axis=0) / len(pc_segment)
elif centroid_method == "label_center":
gt[track_label_uuid]["centroid"] = center
else:
logger.warning("Not implemented")
gt[track_label_uuid]["bbox"] = bbox
gt[track_label_uuid]["orientation"] = orientation
if track_label_uuid not in ID_gt_all:
ID_gt_all.append(track_label_uuid)
id_gts.append(track_label_uuid)
tracks: Dict[str, Dict[str, Any]] = {}
id_tracks: List[str] = []
track_data = read_json_file(path_track_data[ind_frame])
for track in track_data:
key = track["track_label_uuid"]
if track["label_class"] != "VEHICLE" or track["height"] == 0:
continue
center = np.array([track["center"]["x"], track["center"]["y"], track["center"]["z"]])
if in_distance_range_pose(np.zeros(3), center, d_min, d_max):
tracks[key] = {}
tracks[key]["centroid"] = center
id_tracks.append(key)
dists: List[List[float]] = []
for gt_key, gt_value in gt.items():
gt_track_data: List[float] = []
dists.append(gt_track_data)
for track_key, track_value in tracks.items():
gt_track_data.append(get_distance(gt_value, track_value, "centroid"))
acc.update(id_gts, id_tracks, dists)
mh = mm.metrics.create()
summary = mh.compute(
acc,
metrics=[
"num_frames",
"mota",
"motp",
"idf1",
"mostly_tracked",
"mostly_lost",
"num_false_positives",
"num_misses",
"num_switches",
"num_fragmentations",
],
name="acc",
)
logger.info("summary = %s", summary)
num_tracks = len(ID_gt_all)
fn = os.path.basename(path_tracker_output)
num_frames = summary["num_frames"][0]
mota = summary["mota"][0] * 100
motp = summary["motp"][0]
idf1 = summary["idf1"][0]
most_track = summary["mostly_tracked"][0] / num_tracks
most_lost = summary["mostly_lost"][0] / num_tracks
num_fp = summary["num_false_positives"][0]
num_miss = summary["num_misses"][0]
num_switch = summary["num_switches"][0]
num_flag = summary["num_fragmentations"][0]
out_string = (
f"{fn} {num_frames} {mota:.2f} {motp:.2f} {idf1:.2f} {most_track:.2f} "
f"{most_lost:.2f} {num_fp} {num_miss} {num_switch} {num_flag} \n"
)
out_file.write(out_string)
def main(nusc: NuScenes, args: argparse.Namespace, start_index: int,
end_index: int) -> None:
"""
Convert sweeps and samples into (unannotated) Argoverse format. Overview of algorithm:
1) Iterate over all scenes in the NuScenes dataset. For each scene, obtain first sample in the scene.
2) Get the sample_data corresponding to each of the channels from the sample, and convert it to argo format.
3) While the sample_data is not corresponding to a key_frame, get the next sample_data, and repeat step 2.
4) Go to the next sample while we are in the same scene.
"""
OUTPUT_ROOT = args.argo_dir
NUSCENES_ROOT = args.nuscenes_dir
NUSCENES_VERSION = args.nuscenes_version
if not os.path.exists(OUTPUT_ROOT):
os.makedirs(OUTPUT_ROOT)
tot_scenes = len(nusc.scene)
for scene in nusc.scene[start_index:min(end_index, tot_scenes)]:
scene_token = scene["token"]
sample_token = scene["first_sample_token"]
scene_path = os.path.join(OUTPUT_ROOT, scene_token)
if not os.path.exists(scene_path):
os.makedirs(scene_path)
log_token = scene["log_token"]
nusc_log = nusc.get("log", log_token)
nusc_city = nusc_log["location"]
save_json_dict(os.path.join(scene_path, f"city_info.json"),
{"city_name": CITY_TO_ID[nusc_city]})
# Calibration info for all the sensors
calibration_info = get_calibration_info(nusc, scene)
calib_path = os.path.join(scene_path, f"vehicle_calibration_info.json")
save_json_dict(calib_path, calibration_info)
while sample_token != "":
sample = nusc.get("sample", sample_token)
timestamp = round_to_micros(sample["timestamp"])
tracked_labels = []
# city_SE3_vehicle pose
ego_pose = None
nsweeps_lidar = 10
nsweeps_cam = 6
# Save ego pose to json file
poses_path = os.path.join(scene_path, f"poses")
if not os.path.exists(poses_path):
os.makedirs(poses_path)
# Copy nuscenes sensor data into argoverse format and get the pose of the vehicle in the city frame
for sensor, sensor_token in sample["data"].items():
if sensor in SENSOR_NAMES:
argo_sensor = SENSOR_NAMES[sensor]
output_sensor_path = os.path.join(scene_path, argo_sensor)
if not os.path.exists(output_sensor_path):
os.makedirs(output_sensor_path)
sensor_data = nusc.get("sample_data", sensor_token)
file_path = os.path.join(NUSCENES_ROOT,
sensor_data["filename"])
i = 0
if sensor == "LIDAR_TOP":
# nuscenes lidar data is stored as (x, y, z, intensity, ring index)
while i < nsweeps_lidar and sensor_token != "":
sensor_data = nusc.get("sample_data", sensor_token)
file_path = os.path.join(NUSCENES_ROOT,
sensor_data["filename"])
timestamp = round_to_micros(
sensor_data["timestamp"])
# Not always exactly 10
if (sensor_data["is_key_frame"]
and i != 0) or sample_token == "":
break
scan = np.fromfile(file_path, dtype=np.float32)
points = scan.reshape((-1, 5))
# Transform lidar points from point sensor frame to egovehicle frame
calibration = nusc.get(
"calibrated_sensor",
sensor_data["calibrated_sensor_token"],
)
egovehicle_R_lidar = quat2rotmat(
calibration["rotation"])
egovehicle_t_lidar = np.array(
calibration["translation"])
egovehicle_SE3_lidar = SE3(
rotation=egovehicle_R_lidar,
translation=egovehicle_t_lidar,
)
points_egovehicle = egovehicle_SE3_lidar.transform_point_cloud(
points[:, :3])
write_ply(points_egovehicle, points,
output_sensor_path, timestamp)
if not os.path.isfile(
os.path.join(
poses_path,
f"city_SE3_egovehicle_{timestamp}.json"
)):
ego_pose = nusc.get(
"ego_pose", sensor_data["ego_pose_token"])
ego_pose_dict = {
"rotation": ego_pose["rotation"],
"translation": ego_pose["translation"],
}
save_json_dict(
os.path.join(
poses_path,
f"city_SE3_egovehicle_{timestamp}.json"
), ego_pose_dict)
sensor_token = sensor_data["next"]
else:
while i < nsweeps_cam and sensor_token != "":
sensor_data = nusc.get("sample_data", sensor_token)
file_path = os.path.join(NUSCENES_ROOT,
sensor_data["filename"])
timestamp = round_to_micros(
sensor_data["timestamp"])
# Not always exactly 6
if sensor_data["is_key_frame"] and i != 0:
break
shutil.copy(
file_path,
os.path.join(output_sensor_path,
f"{argo_sensor}_{timestamp}.jpg"),
)
sensor_token = sensor_data["next"]
if not os.path.isfile(
os.path.join(
poses_path,
f"city_SE3_egovehicle_{timestamp}.json"
)):
ego_pose = nusc.get(
"ego_pose", sensor_data["ego_pose_token"])
ego_pose_dict = {
"rotation": ego_pose["rotation"],
"translation": ego_pose["translation"],
}
save_json_dict(
os.path.join(
poses_path,
f"city_SE3_egovehicle_{timestamp}.json"
), ego_pose_dict)
sample_token = sample["next"]
def test_invalid_quaternion_zero_norm():
"""Ensure that passing a zero-norm quaternion raises an error, as normalization would divide by 0."""
q = np.array([0.0, 0.0, 0.0, 0.0])
with pytest.raises(ZeroDivisionError) as e_info:
R = quat2rotmat(q)
