#!/usr/bin/env python
# -*- coding:utf-8 -*-
#@Time : 2019/7/17 2:32
#@Author: 黄怀宇
#@File : codeRepetition.py
# from sklearn.metrics import accuracy_score #works
# print(accuracy_score([1, 1, 0], [1, 0, 1]))
from nuscenes.nuscenes import NuScenes
import numpy as np
nusc = NuScenes(version='v1.0-mini', dataroot='D:\\v1.0-mini', verbose=True)
print('all scene')
nusc.list_scenes() # 列出所有的场景
my_scene = nusc.scene[0]
print('\nmy_scene\n', my_scene) # 列出所有场景中的第一个场景对应的所有 token
first_sample_token = my_scene[
'first_sample_token'] # scene.json 中第一个元素组中的 first_sample_token
# sample.json 中第一帧的所有元素(token、timestamp、prev、next、scene_token) 和
# 汽车所有传感器对应的数据
my_sample = nusc.get('sample', first_sample_token)
print('my_sample\n', my_sample)
# 只取汽车所有传感器对应的数据的方法 print('my_sample\n', my_sample['data'])
# 只取CAM_FRONT 的方法
# sensor = 'CAM_FRONT'
# cam_front_data = nusc.get('sample_data', my_sample['data'][sensor])
# print(cam_front_data)
from nuscenes.nuscenes import NuScenes
from nuscenes.utils.data_classes import LidarPointCloud, RadarPointCloud, Box
from PIL import Image
from pyquaternion import Quaternion
import numpy as np
import os
import os.path as osp
from nuscenes.utils.geometry_utils import view_points, box_in_image, BoxVisibility, transform_matrix
nusc = NuScenes(version='v1.0-mini', dataroot='/share/nuscenes', verbose=True)
nusc_scenes = nusc.list_scenes()
current_scene = nusc.scene[0]
sensor = 'CAM_FRONT'
first_token = current_scene['first_sample_token']
first_token_meta = nusc.get('sample', first_token)
all_sensor_data = first_token_meta['data']
CAM_SAMPLE = nusc.get('sample_data', first_token_meta['data'][sensor])
CAM_EGO = CAM_SAMPLE['ego_pose_token']
EGO = nusc.get('ego_pose', token=CAM_EGO)
# map point cloud into image
def map_pointcloud_to_image(nusc,
pointsensor_token: str,
camera_token: str,
min_dist: float = 1.0,
render_intensity: bool = False,
show_lidarseg: bool = False):
"""
def main(args):
args.path = "/home/jo/training_data/nuscenes/nuscenes-v1.0"
args.resize = [640, 256, 0]
client = MongoClient(args.conn)
collection = client[args.db][args.collection]
nusc = NuScenes(version="v1.0-mini", dataroot=args.path, verbose=True)
nusc.list_scenes()
for scene in tqdm(nusc.scene):
next_sample_token = scene["first_sample_token"]
while True:
sample = nusc.get('sample', next_sample_token)
next_sample_token = sample["next"]
sample_data_token = sample["data"]["CAM_FRONT"]
sample_data = nusc.get_sample_data(sample_data_token)
cam_front_data = nusc.get('sample_data', sample_data_token)
# Create image data
img_path = sample_data[0]
if os.path.exists(img_path):
img = cv2.imread(img_path)
nusc.render_pointcloud_in_image
depth_map = map_pointcloud_to_image(nusc, sample['token'], pointsensor_channel='LIDAR_TOP', camera_channel='CAM_FRONT')
# Not sure about radar data, seems to not allign with lidar data very often, leaving it out for now
# depth_map = map_pointcloud_to_image(nusc, sample['token'], pointsensor_channel='RADAR_FRONT', camera_channel='CAM_FRONT', depth_map=depth_map)
roi = Roi()
if args.resize:
img, roi = resize_img(img, args.resize[0], args.resize[1], args.resize[2])
depth_map, _ = resize_img(depth_map, args.resize[0], args.resize[1], args.resize[2], cv2.INTER_NEAREST)
img_bytes = cv2.imencode('.jpeg', img)[1].tobytes()
depth_bytes = cv2.imencode(".png", depth_map)[1].tobytes()
content_type = "image/jpeg"
else:
print("WARNING: file not found: " + img_path + ", continue with next image")
continue
# Get sensor extrinsics, Not sure why roll and yaw seem to be PI/2 off compared to nuImage calibarted sensor
sensor = nusc.get('calibrated_sensor', cam_front_data['calibrated_sensor_token'])
q = MyQuaternion(sensor["rotation"][0], sensor["rotation"][1], sensor["rotation"][2], sensor["rotation"][3])
roll = math.atan2(2.0 * (q.z * q.y + q.w * q.x) , 1.0 - 2.0 * (q.x * q.x + q.y * q.y)) + math.pi * 0.5
pitch = math.asin(2.0 * (q.y * q.w - q.z * q.x))
yaw = math.atan2(2.0 * (q.z * q.w + q.x * q.y) , - 1.0 + 2.0 * (q.w * q.w + q.x * q.x)) + math.pi * 0.5
# print(sensor["translation"])
# print(f"Pitch: {pitch*57.2} Yaw: {yaw*57.2} Roll: {roll*57.2}")
# Sensor calibration is static, pose would be dynamic. TODO: Somehow also add some sort of cam to cam motion to be learned
# ego_pose = nusc.get("ego_pose", cam_front_data["ego_pose_token"])
# q = MyQuaternion(ego_pose["rotation"][0], ego_pose["rotation"][1], ego_pose["rotation"][2], ego_pose["rotation"][3])
# roll = math.atan2(2.0 * (q.z * q.y + q.w * q.x) , 1.0 - 2.0 * (q.x * q.x + q.y * q.y)) + math.pi * 0.5
# pitch = math.asin(2.0 * (q.y * q.w - q.z * q.x))
# yaw = math.atan2(2.0 * (q.z * q.w + q.x * q.y) , - 1.0 + 2.0 * (q.w * q.w + q.x * q.x)) + math.pi * 0.5
# print(ego_pose["translation"])
# print(f"Pitch: {pitch*57.2} Yaw: {yaw*57.2} Roll: {roll*57.2}")
entry = Entry(
img=img_bytes,
content_type=content_type,
depth=depth_bytes,
org_source="nuscenes",
org_id=img_path,
objects=[],
ignore=[],
has_3D_info=True,
has_track_info=True,
sensor_valid=True,
yaw=wrap_angle(yaw),
roll=wrap_angle(roll),
pitch=wrap_angle(pitch),
translation=sensor["translation"],
scene_token=sample["scene_token"],
timestamp=sample["timestamp"]
)
labels = sample_data[1]
idx_counter = 0
for box in labels:
if box.name in CLASS_MAP.keys():
box.translate(np.array([0, box.wlh[2] / 2, 0])) # translate center center to bottom center
pos_3d = np.array([*box.center, 1.0])
cam_mat = np.hstack((sample_data[2], np.zeros((3, 1))))
cam_mat[0][2] += roi.offset_left
cam_mat[1][2] += roi.offset_top
cam_mat *= roi.scale
cam_mat[2][2] = 1.0
# create rotation matrix, somehow using the rotation matrix directly from nuscenes does not work
# instead, we calc the rotation as it is in kitti and use the same code
v = np.dot(box.rotation_matrix, np.array([1, 0, 0]))
yaw = -np.arctan2(v[2], v[0])
rot_angle = wrap_angle(float(yaw) + math.pi * 0.5) # because parallel to optical view of camera = 90 deg
rot_mat = np.array([
[math.cos(rot_angle), 0.0, math.sin(rot_angle), 0.0],
[0.0, 1.0, 0.0, 0.0],
[-math.sin(rot_angle), 0.0, math.cos(rot_angle), 0.0],
[0.0, 0.0, 0.0, 1.0],
])
pos_2d = np.matmul(cam_mat, pos_3d)
pos_2d /= pos_2d[2]
box3d = calc_cuboid_from_3d(pos_3d, cam_mat, rot_mat, box.wlh[0], box.wlh[2], box.wlh[1])
box2d = bbox_from_cuboid(box3d)
annotation = nusc.get("sample_annotation", box.token)
instance_token = annotation["instance_token"]
entry.objects.append(Object(
obj_class=CLASS_MAP[box.name],
box2d=box2d,
box3d=box3d,
box3d_valid=True,
instance_token=instance_token,
truncated=None,
occluded=None,
width=box.wlh[0],
length=box.wlh[1],
height=box.wlh[2],
orientation=rot_angle,
# converted to autosar coordinate system
x=pos_3d[2],
y=-pos_3d[0],
z=-pos_3d[1]
))
# For debugging show the data in the image
box2d = list(map(int, box2d))
cv2.circle(img, (int(pos_2d[0]), int(pos_2d[1])), 3, (255, 0, 0))
cv2.rectangle(img, (box2d[0], box2d[1]), (box2d[0] + box2d[2], box2d[1] + box2d[3]), (255, 255, 0), 1)
cv2.putText(img,f"{idx_counter}", (box2d[0], box2d[1] + int(box2d[3])), 0, 0.4, (255, 255, 255))
idx_counter += 1
print(f"{idx_counter}: {math.degrees(rot_angle)}")
f, (ax1, ax2) = plt.subplots(2, 1)
ax1.imshow(cv2.cvtColor(img, cv2.COLOR_BGR2RGB))
max_val = np.amax(depth_map)
ax2.imshow(depth_map, cmap="gray", vmin=1, vmax=10000)
plt.show()
# collection.insert_one(entry.get_dict())
if next_sample_token == "":
break
import matplotlib.pyplot as plotter
from nuscenes.nuscenes import NuScenes
from nuscenes.utils.data_classes import RadarPointCloud
nusc = NuScenes(
version='v1.0-mini',
dataroot='C:/Users/chase/OneDrive - Clemson University/Perception/Q4',
verbose=True)
nusc.list_scenes()
my_scene = nusc.scene[0]
my_scene
first_sample_token = my_scene['first_sample_token']
# The rendering command below is commented out because it tends to crash in notebooks
# nusc.render_sample(first_sample_token)
my_sample = nusc.get('sample', first_sample_token)
my_sample
nusc.list_sample(my_sample['token'])
my_sample['data']
sensor = 'CAM_FRONT'
cam_front_data = nusc.get('sample_data', my_sample['data'][sensor])
cam_front_data
nusc.render_sample_data(cam_front_data['token'])
my_annotation_token = my_sample['anns'][18]
my_annotation_metadata = nusc.get('sample_annotation', my_annotation_token)
my_annotation_metadata
