From c284792ce372f78026b2d78583e565db55f7151a Mon Sep 17 00:00:00 2001 From: Pat Bradley Date: Wed, 23 Mar 2022 17:15:56 -0700 Subject: [PATCH 01/14] adding some files --- nucleus/transform.py | 275 +++++++++++++++++++++++++++++++++++++++++++ nucleus/utils.py | 16 +++ 2 files changed, 291 insertions(+) create mode 100644 nucleus/transform.py diff --git a/nucleus/transform.py b/nucleus/transform.py new file mode 100644 index 00000000..21a5a8bd --- /dev/null +++ b/nucleus/transform.py @@ -0,0 +1,275 @@ +import numpy as np +import transforms3d as t3d +from pyquaternion import Quaternion + + +class Transform: + """Transform object represent a rigid transformation matrix (rotation and translation). + + Transform is a 4x4 matrix, although it could be instance using (16,1), (3,4), (3,3) or (3,1) matrixes. + **Note**: not all the methods from Transform will work using scaled/small matrixes + + .. highlight:: python + .. code-block:: python + + [ + [ r00, r01, r02, t0], + [ r10, r11, r12, t1], + [ r20, r21, r22, t2], + [ 0, 0, 0, 1] + ] + + """ + + def __init__(self, value=None): + self.matrix = np.eye(4) + + if isinstance(value, Transform): + self.matrix = np.array(value) + + elif isinstance(value, Quaternion): + self.rotation = value.rotation_matrix + + elif value is not None: + value = np.array(value) + if value.shape == (4, 4): + self.matrix = value + + if value.shape == (16,): + self.matrix = np.array(value).reshape((4, 4)) + + elif value.shape == (3, 4): + self.matrix[:3, :4] = value + + elif value.shape == (3, 3): + self.rotation = value + + elif value.shape == (3,): + self.translation = value + + @staticmethod + def from_Rt(R, t): + """Create a transform based on a rotation and a translation components. + + :param R: Rotation matrix or quaternion. + :type R: Quaternion, list + :param t: Translation component + :type t: list + :returns: Transform created based on the components + :rtype: Transform + + """ + if isinstance(R, Quaternion): + R = R.rotation_matrix + return Transform(np.block([[R, np.mat(t).T], [np.zeros(3), 1]])) + + @staticmethod + def from_euler(angles, axes="sxyz", degrees=False): + """Create a transform from euler angles + + :param angles: Values of the rotation per axis + :type angles: list + :param axes: Order of the axis (default ``sxyz``) + :type axes: str + :param degrees: Use degrees or radians values (default ``False`` = radians) + :type degrees: boolean + :returns: Transform created from euler angles + :rtype: Transform + + """ + if degrees: + angles = np.deg2rad(angles) + return Transform(t3d.euler.euler2mat(*angles, axes=axes)) + + @staticmethod + def from_transformed_points(A, B): + """Create a transform from two points + + :param A: Point A (x,y,z) + :type A: list + :param B: Point B (x,y,z) + :type B: list + :returns: Transform created from the angles + :rtype: Transform + + """ + assert A.shape == B.shape + + mean_A = np.mean(A, axis=0) + mean_B = np.mean(B, axis=0) + centroid_A = A - mean_A + centroid_B = B - mean_B + + C = centroid_A.T @ centroid_B + V, S, W = np.linalg.svd(C) + + if (np.linalg.det(V) * np.linalg.det(W)) < 0.0: + S[-1] = -S[-1] + V[:, -1] = -V[:, -1] + + R = V @ W + t = mean_B - mean_A @ R + + return Transform.from_Rt(R.T, t) + + @staticmethod + def random(): + """Create a transform from random rotation and translation + + :returns: Transform created based on the angles + :rtype: Transform + + """ + return Transform.from_Rt(Quaternion.random(), np.random.rand(3)) + + @property + def rotation(self): + """Transform rotation + + :getter: Return transform's rotation + :setter: Set transform rotation, could use a 3x3 matrix or a Quaternion + :type: 3x3 matrix + """ + return self.matrix[:3, :3] + + @property + def quaternion(self): + """Transform rotation as quaternion + + :getter: Return transform's rotation as quaternion + :type: Quaternion + """ + return Quaternion(t3d.quaternions.mat2quat(self.matrix[:3, :3])) + + @rotation.setter + def rotation(self, rotation): + if isinstance(rotation, Quaternion): + rotation = rotation.rotation_matrix + self.matrix[:3, :3] = rotation + + @property + def position(self): + """Transform position/translation + + :getter: Return transform's position + :setter: Set transform's position list(3x1) + :type: list + """ + return self.matrix[:3, 3].flatten() + + @position.setter + def position(self, position): + self.matrix[:3, 3] = np.array(position).reshape(3) + + @property + def translation(self): + """Transform position/translation + + :getter: Return transform's position + :setter: Set transform's position list(3x1) + :type: list + """ + return self.matrix[:3, 3].flatten() + + @translation.setter + def translation(self, translation): + self.matrix[:3, 3] = np.array(translation).reshape(3) + + @property + def euler_angles(self, axes="sxyz"): + """Transform rotation in euler angles + + :getter: Return transform's rotaiton in euler angles + :type: list + """ + return t3d.euler.mat2euler(self.matrix, axes=axes) + + @property + def euler_degrees(self, axes="sxyz"): + """Transform rotation in euler degrees + + :getter: Return transform's rotaiton in euler degrees + :type: list + """ + return np.rad2deg(t3d.euler.mat2euler(self.matrix, axes=axes)) + + @property + def T(self): + """Transpose of the transform + + :returns: Transpose of the transform + :rtype: Transform + + """ + try: + return Transform(self.matrix.T) + except ValueError: + print("Can not transpose the Transform matrix") + + @property + def inverse(self): + """Inverse of the transform + + :returns: Inverse of the transform + :rtype: Transform + + """ + try: + return Transform.from_Rt( + self.rotation.T, np.dot(-self.rotation.T, self.translation) + ) + except ValueError: + print("Can not inverse the Transform matrix") + + def apply(self, points): + """Apply transform to a list of points + + :param points: List of points (N,3) or (N,4) + :returns: List of points witht the transform applied + :rtype: list + + """ + points_4d = np.hstack([points[:, :3], np.ones((points.shape[0], 1))]) + transformed_4d = points_4d.dot(self.matrix.T) + return np.hstack([transformed_4d[:, :3], points[:, 3:]]) + + def interpolate(self, other, factor): + """Interpotation of the transform + + :param other: Transform to interpolate with + :type other: Transform + :param factor: Factor of interpolation + :type factor: float between 0 and 1 + :returns: Transform resulted from the interpolation + :rtype: Transform + + """ + assert 0 <= factor <= 1.0 + other = Transform(other) + return self.from_Rt( + Quaternion.slerp(self.quaternion, other.quaternion, factor), + self.position + factor * (other.position - self.position), + ) + + def __array__(self): + return self.matrix + + def __getitem__(self, values): + return self.matrix.__getitem__(values) + + def __add__(self, other): + return Transform(other) @ self + + def __matmul__(self, other): + if isinstance(other, np.ndarray): + return self.apply(other) + return Transform(self.matrix @ Transform(other).matrix) + + def __eq__(self, other): + return np.allclose(self.matrix, other.matrix) + + def __repr__(self): + return "R=%s t=%s" % ( + np.array_str(self.euler_degrees, precision=3, suppress_small=True), + np.array_str(self.position, precision=3, suppress_small=True), + ) diff --git a/nucleus/utils.py b/nucleus/utils.py index 3be51e77..544955f3 100644 --- a/nucleus/utils.py +++ b/nucleus/utils.py @@ -8,6 +8,7 @@ import requests from requests.models import HTTPError +import numpy as np from nucleus.annotation import ( Annotation, @@ -283,3 +284,18 @@ def replace_double_slashes(s: str) -> str: for key, val in STRING_REPLACEMENTS.items(): s = s.replace(key, val) return s + + +def bin_to_numpy(bin_file_path: str = None): + ''' + Takes bin file and returns the Point Cloud + Args: + param filepath: bin filepath + type filepath: str + + Returns: + np_pcd: (x,4) numpy array representation of Point Cloud + type frame_to_s3_URI: np.array + ''' + np_pcd = np.fromfile(bin_file_path, dtype=np.float32).reshape(-1, 4) + return np_pcd From 9b31d955a2e435f89b0201a4c370888e698753d7 Mon Sep 17 00:00:00 2001 From: Pat Bradley Date: Mon, 4 Apr 2022 19:58:05 -0700 Subject: [PATCH 02/14] Nucleus 3D ingest base level transforms --- .../ObjectApproachDev(NoClassDef).py | 202 ++++++++++++++++++ nucleus/sensorfusion.py | 147 +++++++++++++ nucleus/utils.py | 6 + 3 files changed, 355 insertions(+) create mode 100644 IngestDevelopmentScripts/ObjectApproachDev(NoClassDef).py create mode 100644 nucleus/sensorfusion.py diff --git a/IngestDevelopmentScripts/ObjectApproachDev(NoClassDef).py b/IngestDevelopmentScripts/ObjectApproachDev(NoClassDef).py new file mode 100644 index 00000000..1d66fef8 --- /dev/null +++ b/IngestDevelopmentScripts/ObjectApproachDev(NoClassDef).py @@ -0,0 +1,202 @@ +from nucleus.transform import Transform +from nucleus.__init__ import NucleusClient +import open3d as o3d +import numpy as np +from pyquaternion import Quaternion +import glob +import json + +def read_pcd(fp): + pcd = o3d.io.read_point_cloud(fp) + points = np.asarray(pcd.points) + return points + +class RawPointCloud: + def __init__(self, points: np.array = None, transform: Transform = None): + self.points = points + if transform is not None: + self.points = transform.apply(points) + + def load_pcd(self, filepath: str, transform: Transform = None): + points = read_pcd(filepath) + self.points = points + if transform is not None: + self.points = transform.apply(points) + +class CamCalibration: + @property + def extrinsic_matrix(self): + """Camera extrinsic + + :getter: Return camera's extrinsic matrix (pose.inverse[:3, :4]) + :setter: pose = Transform(matrix).inverse + :type: 3x4 matrix + """ + return self.pose.inverse[:3, :4] + + @extrinsic_matrix.setter + def extrinsic_matrix(self, matrix): + self.pose = Transform(matrix).inverse + + def __init__(self, extrinsic_matrix = None): + self.extrinsic_matrix = extrinsic_matrix + +class RawFrame: + def __init__(self, dev_pose: Transform = None, **kwargs): + self.dev_pose = pose + self.items = {} + for key, value in kwargs.items(): + self.items[key] = value + + def get_world_points(self): + """Return the list of points with the frame transformation applied + + :returns: List of points in world coordinates + :rtype: np.array + + """ + return np.hstack( + [ + self.pose @ self.points[:, :3], + self.points[:, 3:4], + self.points[:, 4:5], + ] + ) + +class RawScene: + def __init__(self, frames: [RawFrame] = None): + if frames is None: + self.frames = [] + else: + self.frames = frames + + def add_frame(self, frame: RawFrame = None): + self.frames.append(frame) + + def make_transforms_relative(self): + """Make all the frame transform relative to the first transform/frame. This will set the first transform to position (0,0,0) and heading (1,0,0,0)""" + offset = self.frames[0].dev_pose.inverse + for frame in self.frames: + frame.dev_pose = offset @ frame.dev_pose + + def apply_transforms(self, relative: bool = False): + if relative: + RawScene.make_transforms_relative() + for frame in self.frames: + for item in frame.items: + if isinstance(frame.items[item],RawPointCloud): + frame.items[item].points = np.hstack( + [ + frame.dev_pose @ frame.items[item].points[:, :3], + frame.items[item].points[:, 3:4], + frame.items[item].points[:, 4:5], + ] + ) + else: + wct = frame.dev_pose @ frame.items[item].pose + frame.items[item].pose = wct + + +pcd_files = sorted(glob.glob("/Users/patrickbradley/Desktop/Pilots/Velodyne/Data/Velodyne 10 Frame Sequence/Pointclouds/*")) +poses = sorted(glob.glob("/Users/patrickbradley/Desktop/Pilots/Velodyne/Data/Velodyne 10 Frame Sequence/Poses/*")) + +cam1_rotation = Quaternion(0.5020734498049307,0.5020734498049306,-0.4979179159268883,0.49791791592688817).rotation_matrix +cam1_translation = [-0.047619047619047894, -0.023809523809523725, -0.015873015873015817] + +cam1_transform = Transform.from_Rt(R=cam1_rotation, t=cam1_translation) +cam_1_calib = CamCalibration(extrinsic_matrix=cam1_transform.matrix) + +RawScene = RawScene() +for idx,pcd_fp in enumerate(pcd_files): + pointcloud = RawPointCloud() + pointcloud.load_pcd(pcd_fp) + + with open(poses[idx], "rb") as f: + pose = json.load(f) + pose = Transform.from_Rt(R=np.matrix(pose['rotation']), t=pose['translation']) + + raw_frame = RawFrame(lidar=pointcloud, cam1=cam_1_calib, dev_pose=pose) + + RawScene.add_frame(raw_frame) +RawScene.apply_transforms() + + + + + + + + + + + +# + +# +# + + + + + +# +# RawPointCloud = RawPointCloud( +# points = points, +# transform = transform. +# ) + +#Local Image +# PrecursorImage1 = nucleus.PrecursorImage( +# image_filepath="~/Desktop/ImageFile.png", +# reference_id="scene-1-cam1-image0", +# cam_params=cam1_cam_params +# ) +# +# FramePose = nucleus.Transform.from_Rt(R=poses[idx][‘rotation’], t = poses[idx][‘translation’]) +# PrecursorFrame = nucleus.PrecursorFrame(lidar=PrecursorPointCloud, +# cam1 = PrecursorImage1, +# cam2 = PrecursorImage2, +# pose = FramePose) +# +# cam1_cam_transform = Transform.from_Rt(R=cam1_cam_extrinsics['rotation'], cam1_cam_extrinsics['translation']) +# +# #New Object - Intrinsics and Extrinsics (extrinsics are lidar to cam). Calibration is VERY similar to CameraParams but with added functionality +# cam1_cam_params = nucleus.Calibration(fx=Cam1_cam_intrinsics["fx"], fy=Cam1_cam_intrinsics['fy'], extrinsic_matrix=cam1_cam_transform.matrix) +# +# PrecursorScene = LidarSceneTransformer() +# +# local_pcds = sorted(glob.glob("~/localpcdfiles")) +# for idx, pcd_fp in enumerate(local_pcds): +# points = read_pcd(fp) #Points in Ego Coordinates +# +# PrecursorPointCloud = nucleus.PrecursorPointCloud( +# points = points, +# reference_id="scene-1-pointcloud-0", +# metadata={"is_raining": False} +# ) +# +# #Local Image +# PrecursorImage1 = nucleus.PrecursorImage( +# image_filepath="~/Desktop/ImageFile.png", +# reference_id="scene-1-cam1-image0", +# cam_params=cam1_cam_params +# ) +# +# FramePose = nucleus.Transform.from_Rt(R=poses[idx][‘rotation’], t = poses[idx][‘translation’]) +# PrecursorFrame = nucleus.PrecursorFrame(lidar=PrecursorPointCloud, +# cam1 = PrecursorImage1, +# cam2 = PrecursorImage2, +# pose = FramePose) +# +# PrecursorScene.add_frame(PrecursorFrame) +# +# final_scene = ingest_scene.transform_and_upload(s3_client = s3_client, +# bucket = "bucket", +# prefix = "prefix", +# reference_id = "scene-0") +# +# job = dataset.append( +# items=[final_scene], +# update=True, +# asynchronous=True # required for 3D uploads +# ) \ No newline at end of file diff --git a/nucleus/sensorfusion.py b/nucleus/sensorfusion.py new file mode 100644 index 00000000..7b275a98 --- /dev/null +++ b/nucleus/sensorfusion.py @@ -0,0 +1,147 @@ +import numpy as np +from nucleus.transform import Transform +from nucleus.utils import read_pcd +import copy + +class RawPointCloud: + ''' + RawPointClouds are containers for raw point cloud data. This structure contains Point Clouds as (N,3) or (N,4) numpy arrays + + Point cloud data is assumed to be in ego coordinates. + If the point cloud is in World Coordinates, one can provide the inverse pose as the "transform" argument. If this argument + is present, the point cloud will be transformed back into ego coordinates + + Args: + points (np.array): Point cloud data represented as (N,3) or (N,4) numpy arrays + transform (:class:`Transform`): If in World Coordinate, the transformation used to transform lidar points into ego coordinates + ''' + + def __init__(self, points: np.array = None, transform: Transform = None): + self.points = points + if transform is not None: + self.points = transform.apply(points) + + def load_pcd(self, filepath: str, transform: Transform = None): + ''' + Takes raw pcd file and reads in as numpy array. + + Args: + filepath (str): Local filepath to pcd file + transform (:class:`Transform`): If in world, the transformation used to transform lidar points into ego coordinates + ''' + points = read_pcd(filepath) + self.points = points + if transform is not None: + self.points = transform.apply(points) + + +class CamCalibration: + ''' + CamCalibration solely holds the pose of the camera + This CamCalibration will inevitably be transformed by the device_pose + + Args: + extrinsic_matrix (np.array): (4,4) extrinsic transformation matrix representing device_to_lidar + ''' + + def __init__(self, extrinsic_matrix=None): + self.extrinsic_matrix = extrinsic_matrix + + @property + def extrinsic_matrix(self): + """Camera extrinsic + + :getter: Return camera's extrinsic matrix (pose.inverse[:3, :4]) + :setter: pose = Transform(matrix).inverse + :type: 3x4 matrix + """ + return self.pose.inverse[:3, :4] + + @extrinsic_matrix.setter + def extrinsic_matrix(self, matrix): + ''' + Sets pose as inverse of extrinsic matrix + ''' + self.pose = Transform(matrix).inverse + + +class RawFrame: + ''' + RawFrames are containers for point clouds, image extrinsics, and device pose. + These objects most notably are leveraged to transform point clouds and image extrinsic matrices to the world coordinate frame. + + Args: + dev_pose (:class:`Transform`): World Coordinate transformation for frame + **kwargs (Dict[str, :class:`RawPointCloud, :class: CameraCalibration`]): Mappings from sensor name + to pointcloud and camera calibrations. Each frame of a lidar scene must contain exactly one + pointcloud and any number of camera calibrations + ''' + def __init__(self, dev_pose: Transform = None, **kwargs): + self.dev_pose = dev_pose + self.items = {} + for key, value in kwargs.items(): + if isinstance(value, CamCalibration): + self.items[key] = copy.copy(value) + else: + self.items[key] = value + + def get_world_points(self): + """Return the list of points with the frame transformation applied + + :returns: List of points in world coordinates + :rtype: np.array + + """ + for item in self.items: + if isinstance(self.items[item], RawPointCloud): + return np.hstack( + [ + self.dev_pose @ self.items[item][:, :3], + self.items[item][:, 3:4], + self.items[item][:, 4:5] + ] + ) + + +class RawScene: + ''' + RawsScenes are containers for frames + These objects most notably are leveraged to transform point clouds and image extrinsic matrices to the world coordinate frame. + + Args: + dev_pose (:class:`Transform`): World Coordinate transformation for frame + **kwargs (Dict[str, :class:`RawPointCloud, :class: CameraCalibration`]): Mappings from sensor name + to pointcloud and camera calibrations. Each frame of a lidar scene must contain exactly one + pointcloud and any number of camera calibrations + ''' + def __init__(self, frames: [RawFrame] = None): + if frames is None: + self.frames = [] + else: + self.frames = frames + + def add_frame(self, frame: RawFrame = None): + self.frames.append(frame) + + def make_transforms_relative(self): + """Make all the frame transform relative to the first transform/frame. This will set the first transform to position (0,0,0) and heading (1,0,0,0)""" + offset = self.frames[0].dev_pose.inverse + for frame in self.frames: + frame.dev_pose = offset @ frame.dev_pose + + def apply_transforms(self, relative: bool = False): + if relative: + self.make_transforms_relative() + for frame in self.frames: + for item in frame.items: + if isinstance(frame.items[item], RawPointCloud): + frame.items[item].points = np.hstack( + [ + frame.dev_pose @ frame.items[item].points[:, :3], + frame.items[item].points[:, 3:4], + frame.items[item].points[:, 4:5], + ] + ) + else: + wct = frame.dev_pose @ frame.items[item].pose + frame.items[item].pose = wct \ No newline at end of file diff --git a/nucleus/utils.py b/nucleus/utils.py index 544955f3..8e148c25 100644 --- a/nucleus/utils.py +++ b/nucleus/utils.py @@ -5,6 +5,7 @@ import uuid from collections import defaultdict from typing import IO, Dict, List, Sequence, Type, Union +import open3d as o3d import requests from requests.models import HTTPError @@ -299,3 +300,8 @@ def bin_to_numpy(bin_file_path: str = None): ''' np_pcd = np.fromfile(bin_file_path, dtype=np.float32).reshape(-1, 4) return np_pcd + +def read_pcd(fp): + pcd = o3d.io.read_point_cloud(fp) + points = np.asarray(pcd.points) + return points From 5f155ecca175ca58103488702964d99d258e9e27 Mon Sep 17 00:00:00 2001 From: Pat Bradley Date: Mon, 4 Apr 2022 20:11:37 -0700 Subject: [PATCH 03/14] updating testing file --- .../ObjectApproachDev(NoClassDef).py | 235 +++--------------- 1 file changed, 41 insertions(+), 194 deletions(-) diff --git a/IngestDevelopmentScripts/ObjectApproachDev(NoClassDef).py b/IngestDevelopmentScripts/ObjectApproachDev(NoClassDef).py index 1d66fef8..73b612df 100644 --- a/IngestDevelopmentScripts/ObjectApproachDev(NoClassDef).py +++ b/IngestDevelopmentScripts/ObjectApproachDev(NoClassDef).py @@ -1,202 +1,49 @@ -from nucleus.transform import Transform -from nucleus.__init__ import NucleusClient -import open3d as o3d +from nucleus.sensorfusion import * import numpy as np -from pyquaternion import Quaternion import glob -import json - -def read_pcd(fp): - pcd = o3d.io.read_point_cloud(fp) - points = np.asarray(pcd.points) - return points - -class RawPointCloud: - def __init__(self, points: np.array = None, transform: Transform = None): - self.points = points - if transform is not None: - self.points = transform.apply(points) - - def load_pcd(self, filepath: str, transform: Transform = None): - points = read_pcd(filepath) - self.points = points - if transform is not None: - self.points = transform.apply(points) - -class CamCalibration: - @property - def extrinsic_matrix(self): - """Camera extrinsic - - :getter: Return camera's extrinsic matrix (pose.inverse[:3, :4]) - :setter: pose = Transform(matrix).inverse - :type: 3x4 matrix - """ - return self.pose.inverse[:3, :4] - - @extrinsic_matrix.setter - def extrinsic_matrix(self, matrix): - self.pose = Transform(matrix).inverse - - def __init__(self, extrinsic_matrix = None): - self.extrinsic_matrix = extrinsic_matrix - -class RawFrame: - def __init__(self, dev_pose: Transform = None, **kwargs): - self.dev_pose = pose - self.items = {} - for key, value in kwargs.items(): - self.items[key] = value - - def get_world_points(self): - """Return the list of points with the frame transformation applied - - :returns: List of points in world coordinates - :rtype: np.array - - """ - return np.hstack( - [ - self.pose @ self.points[:, :3], - self.points[:, 3:4], - self.points[:, 4:5], - ] - ) - -class RawScene: - def __init__(self, frames: [RawFrame] = None): - if frames is None: - self.frames = [] - else: - self.frames = frames - - def add_frame(self, frame: RawFrame = None): - self.frames.append(frame) - - def make_transforms_relative(self): - """Make all the frame transform relative to the first transform/frame. This will set the first transform to position (0,0,0) and heading (1,0,0,0)""" - offset = self.frames[0].dev_pose.inverse - for frame in self.frames: - frame.dev_pose = offset @ frame.dev_pose - - def apply_transforms(self, relative: bool = False): - if relative: - RawScene.make_transforms_relative() - for frame in self.frames: - for item in frame.items: - if isinstance(frame.items[item],RawPointCloud): - frame.items[item].points = np.hstack( - [ - frame.dev_pose @ frame.items[item].points[:, :3], - frame.items[item].points[:, 3:4], - frame.items[item].points[:, 4:5], - ] - ) - else: - wct = frame.dev_pose @ frame.items[item].pose - frame.items[item].pose = wct - - -pcd_files = sorted(glob.glob("/Users/patrickbradley/Desktop/Pilots/Velodyne/Data/Velodyne 10 Frame Sequence/Pointclouds/*")) -poses = sorted(glob.glob("/Users/patrickbradley/Desktop/Pilots/Velodyne/Data/Velodyne 10 Frame Sequence/Poses/*")) - -cam1_rotation = Quaternion(0.5020734498049307,0.5020734498049306,-0.4979179159268883,0.49791791592688817).rotation_matrix -cam1_translation = [-0.047619047619047894, -0.023809523809523725, -0.015873015873015817] - -cam1_transform = Transform.from_Rt(R=cam1_rotation, t=cam1_translation) -cam_1_calib = CamCalibration(extrinsic_matrix=cam1_transform.matrix) + +def format_pointcloud(lidar_np): + mask = lidar_np[:, 4] == 0.0 + pc_1 = lidar_np[mask, :] + pc_1 = np.delete(pc_1, (4), 1) + return pc_1 + +npz_files = sorted(glob.glob("sdk-sample-data/*")) +npz_files.remove("sdk-sample-data/extrinsics_seq000-003_11042021.npz") + +updated_extrinsics = np.load("sdk-sample-data/extrinsics_seq000-003_11042021.npz", allow_pickle = True) +wnsl_extrinsics = updated_extrinsics['camera_WindshieldNarrowStereoLeft_lidar_extrinsic'] +print(f"Camera Extrinsics: \n{wnsl_extrinsics}") + +cam_1_calib = CamCalibration(extrinsic_matrix=wnsl_extrinsics) +print(f"Original Camera Pose:{cam_1_calib.pose}") RawScene = RawScene() -for idx,pcd_fp in enumerate(pcd_files): - pointcloud = RawPointCloud() - pointcloud.load_pcd(pcd_fp) +for idx,npz_fp in enumerate(npz_files): + print(f"Frame Index: {idx}") - with open(poses[idx], "rb") as f: - pose = json.load(f) - pose = Transform.from_Rt(R=np.matrix(pose['rotation']), t=pose['translation']) + frame_npz = np.load(npz_fp, allow_pickle=True) - raw_frame = RawFrame(lidar=pointcloud, cam1=cam_1_calib, dev_pose=pose) + pointcloud_np= format_pointcloud(frame_npz['points']) + print(f"PointCloud Shape: {pointcloud_np.shape}") + pointcloud = RawPointCloud(points=pointcloud_np) + + print(f"World Coordinate Transformation:\n{frame_npz['vehicle_local_tf']}") + frame_pose = Transform(frame_npz['vehicle_local_tf']) + print(f"Frame Pose: {frame_pose}") + + raw_frame = RawFrame(lidar=pointcloud, cam1=cam_1_calib, dev_pose=frame_pose) RawScene.add_frame(raw_frame) -RawScene.apply_transforms() - - - - - - - - - - - -# - -# -# - - - - - -# -# RawPointCloud = RawPointCloud( -# points = points, -# transform = transform. -# ) - -#Local Image -# PrecursorImage1 = nucleus.PrecursorImage( -# image_filepath="~/Desktop/ImageFile.png", -# reference_id="scene-1-cam1-image0", -# cam_params=cam1_cam_params -# ) -# -# FramePose = nucleus.Transform.from_Rt(R=poses[idx][‘rotation’], t = poses[idx][‘translation’]) -# PrecursorFrame = nucleus.PrecursorFrame(lidar=PrecursorPointCloud, -# cam1 = PrecursorImage1, -# cam2 = PrecursorImage2, -# pose = FramePose) -# -# cam1_cam_transform = Transform.from_Rt(R=cam1_cam_extrinsics['rotation'], cam1_cam_extrinsics['translation']) -# -# #New Object - Intrinsics and Extrinsics (extrinsics are lidar to cam). Calibration is VERY similar to CameraParams but with added functionality -# cam1_cam_params = nucleus.Calibration(fx=Cam1_cam_intrinsics["fx"], fy=Cam1_cam_intrinsics['fy'], extrinsic_matrix=cam1_cam_transform.matrix) -# -# PrecursorScene = LidarSceneTransformer() -# -# local_pcds = sorted(glob.glob("~/localpcdfiles")) -# for idx, pcd_fp in enumerate(local_pcds): -# points = read_pcd(fp) #Points in Ego Coordinates -# -# PrecursorPointCloud = nucleus.PrecursorPointCloud( -# points = points, -# reference_id="scene-1-pointcloud-0", -# metadata={"is_raining": False} -# ) -# -# #Local Image -# PrecursorImage1 = nucleus.PrecursorImage( -# image_filepath="~/Desktop/ImageFile.png", -# reference_id="scene-1-cam1-image0", -# cam_params=cam1_cam_params -# ) -# -# FramePose = nucleus.Transform.from_Rt(R=poses[idx][‘rotation’], t = poses[idx][‘translation’]) -# PrecursorFrame = nucleus.PrecursorFrame(lidar=PrecursorPointCloud, -# cam1 = PrecursorImage1, -# cam2 = PrecursorImage2, -# pose = FramePose) -# -# PrecursorScene.add_frame(PrecursorFrame) -# -# final_scene = ingest_scene.transform_and_upload(s3_client = s3_client, -# bucket = "bucket", -# prefix = "prefix", -# reference_id = "scene-0") -# -# job = dataset.append( -# items=[final_scene], -# update=True, -# asynchronous=True # required for 3D uploads -# ) \ No newline at end of file + +print(f"Frame 5, Point1 PreTransform: {RawScene.frames[4].items['lidar'].points[0]}") +print(f"Frame 5, Camera Extrinsics PreTransform: {RawScene.frames[4].items['cam1'].pose}") +RawScene.apply_transforms(relative=True) +print(f"Frame 5, Point1 in World: {RawScene.frames[4].items['lidar'].points[0]}") +print(f"Frame 5, Camera Extrinsics PostTransform: {RawScene.frames[4].items['cam1'].pose}") + + + + + + From 19c57c6a58a1707246fc55afaaf072ec690888a8 Mon Sep 17 00:00:00 2001 From: Pat Bradley Date: Mon, 4 Apr 2022 20:53:26 -0700 Subject: [PATCH 04/14] cleaning --- .../ObjectApproachDev(NoClassDef).py | 49 ------ nucleus/sensorfusion.py | 147 ------------------ 2 files changed, 196 deletions(-) delete mode 100644 IngestDevelopmentScripts/ObjectApproachDev(NoClassDef).py delete mode 100644 nucleus/sensorfusion.py diff --git a/IngestDevelopmentScripts/ObjectApproachDev(NoClassDef).py b/IngestDevelopmentScripts/ObjectApproachDev(NoClassDef).py deleted file mode 100644 index 73b612df..00000000 --- a/IngestDevelopmentScripts/ObjectApproachDev(NoClassDef).py +++ /dev/null @@ -1,49 +0,0 @@ -from nucleus.sensorfusion import * -import numpy as np -import glob - -def format_pointcloud(lidar_np): - mask = lidar_np[:, 4] == 0.0 - pc_1 = lidar_np[mask, :] - pc_1 = np.delete(pc_1, (4), 1) - return pc_1 - -npz_files = sorted(glob.glob("sdk-sample-data/*")) -npz_files.remove("sdk-sample-data/extrinsics_seq000-003_11042021.npz") - -updated_extrinsics = np.load("sdk-sample-data/extrinsics_seq000-003_11042021.npz", allow_pickle = True) -wnsl_extrinsics = updated_extrinsics['camera_WindshieldNarrowStereoLeft_lidar_extrinsic'] -print(f"Camera Extrinsics: \n{wnsl_extrinsics}") - -cam_1_calib = CamCalibration(extrinsic_matrix=wnsl_extrinsics) -print(f"Original Camera Pose:{cam_1_calib.pose}") - -RawScene = RawScene() -for idx,npz_fp in enumerate(npz_files): - print(f"Frame Index: {idx}") - - frame_npz = np.load(npz_fp, allow_pickle=True) - - pointcloud_np= format_pointcloud(frame_npz['points']) - print(f"PointCloud Shape: {pointcloud_np.shape}") - - pointcloud = RawPointCloud(points=pointcloud_np) - - print(f"World Coordinate Transformation:\n{frame_npz['vehicle_local_tf']}") - frame_pose = Transform(frame_npz['vehicle_local_tf']) - print(f"Frame Pose: {frame_pose}") - - raw_frame = RawFrame(lidar=pointcloud, cam1=cam_1_calib, dev_pose=frame_pose) - RawScene.add_frame(raw_frame) - -print(f"Frame 5, Point1 PreTransform: {RawScene.frames[4].items['lidar'].points[0]}") -print(f"Frame 5, Camera Extrinsics PreTransform: {RawScene.frames[4].items['cam1'].pose}") -RawScene.apply_transforms(relative=True) -print(f"Frame 5, Point1 in World: {RawScene.frames[4].items['lidar'].points[0]}") -print(f"Frame 5, Camera Extrinsics PostTransform: {RawScene.frames[4].items['cam1'].pose}") - - - - - - diff --git a/nucleus/sensorfusion.py b/nucleus/sensorfusion.py deleted file mode 100644 index 7b275a98..00000000 --- a/nucleus/sensorfusion.py +++ /dev/null @@ -1,147 +0,0 @@ -import numpy as np -from nucleus.transform import Transform -from nucleus.utils import read_pcd -import copy - -class RawPointCloud: - ''' - RawPointClouds are containers for raw point cloud data. This structure contains Point Clouds as (N,3) or (N,4) numpy arrays - - Point cloud data is assumed to be in ego coordinates. - If the point cloud is in World Coordinates, one can provide the inverse pose as the "transform" argument. If this argument - is present, the point cloud will be transformed back into ego coordinates - - Args: - points (np.array): Point cloud data represented as (N,3) or (N,4) numpy arrays - transform (:class:`Transform`): If in World Coordinate, the transformation used to transform lidar points into ego coordinates - ''' - - def __init__(self, points: np.array = None, transform: Transform = None): - self.points = points - if transform is not None: - self.points = transform.apply(points) - - def load_pcd(self, filepath: str, transform: Transform = None): - ''' - Takes raw pcd file and reads in as numpy array. - - Args: - filepath (str): Local filepath to pcd file - transform (:class:`Transform`): If in world, the transformation used to transform lidar points into ego coordinates - ''' - points = read_pcd(filepath) - self.points = points - if transform is not None: - self.points = transform.apply(points) - - -class CamCalibration: - ''' - CamCalibration solely holds the pose of the camera - This CamCalibration will inevitably be transformed by the device_pose - - Args: - extrinsic_matrix (np.array): (4,4) extrinsic transformation matrix representing device_to_lidar - ''' - - def __init__(self, extrinsic_matrix=None): - self.extrinsic_matrix = extrinsic_matrix - - @property - def extrinsic_matrix(self): - """Camera extrinsic - - :getter: Return camera's extrinsic matrix (pose.inverse[:3, :4]) - :setter: pose = Transform(matrix).inverse - :type: 3x4 matrix - """ - return self.pose.inverse[:3, :4] - - @extrinsic_matrix.setter - def extrinsic_matrix(self, matrix): - ''' - Sets pose as inverse of extrinsic matrix - ''' - self.pose = Transform(matrix).inverse - - -class RawFrame: - ''' - RawFrames are containers for point clouds, image extrinsics, and device pose. - These objects most notably are leveraged to transform point clouds and image extrinsic matrices to the world coordinate frame. - - Args: - dev_pose (:class:`Transform`): World Coordinate transformation for frame - **kwargs (Dict[str, :class:`RawPointCloud, :class: CameraCalibration`]): Mappings from sensor name - to pointcloud and camera calibrations. Each frame of a lidar scene must contain exactly one - pointcloud and any number of camera calibrations - ''' - def __init__(self, dev_pose: Transform = None, **kwargs): - self.dev_pose = dev_pose - self.items = {} - for key, value in kwargs.items(): - if isinstance(value, CamCalibration): - self.items[key] = copy.copy(value) - else: - self.items[key] = value - - def get_world_points(self): - """Return the list of points with the frame transformation applied - - :returns: List of points in world coordinates - :rtype: np.array - - """ - for item in self.items: - if isinstance(self.items[item], RawPointCloud): - return np.hstack( - [ - self.dev_pose @ self.items[item][:, :3], - self.items[item][:, 3:4], - self.items[item][:, 4:5] - ] - ) - - -class RawScene: - ''' - RawsScenes are containers for frames - These objects most notably are leveraged to transform point clouds and image extrinsic matrices to the world coordinate frame. - - Args: - dev_pose (:class:`Transform`): World Coordinate transformation for frame - **kwargs (Dict[str, :class:`RawPointCloud, :class: CameraCalibration`]): Mappings from sensor name - to pointcloud and camera calibrations. Each frame of a lidar scene must contain exactly one - pointcloud and any number of camera calibrations - ''' - def __init__(self, frames: [RawFrame] = None): - if frames is None: - self.frames = [] - else: - self.frames = frames - - def add_frame(self, frame: RawFrame = None): - self.frames.append(frame) - - def make_transforms_relative(self): - """Make all the frame transform relative to the first transform/frame. This will set the first transform to position (0,0,0) and heading (1,0,0,0)""" - offset = self.frames[0].dev_pose.inverse - for frame in self.frames: - frame.dev_pose = offset @ frame.dev_pose - - def apply_transforms(self, relative: bool = False): - if relative: - self.make_transforms_relative() - for frame in self.frames: - for item in frame.items: - if isinstance(frame.items[item], RawPointCloud): - frame.items[item].points = np.hstack( - [ - frame.dev_pose @ frame.items[item].points[:, :3], - frame.items[item].points[:, 3:4], - frame.items[item].points[:, 4:5], - ] - ) - else: - wct = frame.dev_pose @ frame.items[item].pose - frame.items[item].pose = wct \ No newline at end of file From 479c34204f46384bcfc2b5b5e376aa1f507f216c Mon Sep 17 00:00:00 2001 From: Pat Bradley Date: Mon, 4 Apr 2022 21:06:01 -0700 Subject: [PATCH 05/14] Sensor Fusion base transforms --- IngestDevelopmentScripts/TestScript.py | 43 ++++++++ nucleus/sensorfusion.py | 138 +++++++++++++++++++++++++ 2 files changed, 181 insertions(+) create mode 100644 IngestDevelopmentScripts/TestScript.py create mode 100644 nucleus/sensorfusion.py diff --git a/IngestDevelopmentScripts/TestScript.py b/IngestDevelopmentScripts/TestScript.py new file mode 100644 index 00000000..163b611e --- /dev/null +++ b/IngestDevelopmentScripts/TestScript.py @@ -0,0 +1,43 @@ +from nucleus.sensorfusion import * +import numpy as np +import glob + +def format_pointcloud(lidar_np): + mask = lidar_np[:, 4] == 0.0 + pc_1 = lidar_np[mask, :] + pc_1 = np.delete(pc_1, (4), 1) + return pc_1 + +npz_files = sorted(glob.glob("sdk-sample-data/*")) +npz_files.remove("sdk-sample-data/extrinsics_seq000-003_11042021.npz") + +updated_extrinsics = np.load("sdk-sample-data/extrinsics_seq000-003_11042021.npz", allow_pickle = True) +wnsl_extrinsics = updated_extrinsics['camera_WindshieldNarrowStereoLeft_lidar_extrinsic'] +print(f"Camera Extrinsics: \n{wnsl_extrinsics}") + +cam_1_calib = CamCalibration(extrinsic_matrix=wnsl_extrinsics) +print(f"Original Camera Pose:{cam_1_calib.pose}") + +RawScene = RawScene() +for idx,npz_fp in enumerate(npz_files): + print(f"Frame Index: {idx}") + + frame_npz = np.load(npz_fp, allow_pickle=True) + + pointcloud_np= format_pointcloud(frame_npz['points']) + print(f"PointCloud Shape: {pointcloud_np.shape}") + + pointcloud = RawPointCloud(points=pointcloud_np) + + print(f"World Coordinate Transformation:\n{frame_npz['vehicle_local_tf']}") + frame_pose = Transform(frame_npz['vehicle_local_tf']) + print(f"Frame Pose: {frame_pose}") + + raw_frame = RawFrame(lidar=pointcloud, cam1=cam_1_calib, dev_pose=frame_pose) + RawScene.add_frame(raw_frame) + +print(f"Frame 5, Point1 PreTransform: {RawScene.frames[4].items['lidar'].points[0]}") +print(f"Frame 5, Camera Extrinsics PreTransform: {RawScene.frames[4].items['cam1'].pose}") +RawScene.apply_transforms(relative=True) +print(f"Frame 5, Point1 in World: {RawScene.frames[4].items['lidar'].points[0]}") +print(f"Frame 5, Camera Extrinsics PostTransform: {RawScene.frames[4].items['cam1'].pose}") \ No newline at end of file diff --git a/nucleus/sensorfusion.py b/nucleus/sensorfusion.py new file mode 100644 index 00000000..cc3d872b --- /dev/null +++ b/nucleus/sensorfusion.py @@ -0,0 +1,138 @@ +import numpy as np +from nucleus.transform import Transform +from nucleus.utils import read_pcd +import copy + +class RawPointCloud: + ''' + RawPointClouds are containers for raw point cloud data. This structure contains Point Clouds as (N,3) or (N,4) numpy arrays + Point cloud data is assumed to be in ego coordinates. + If the point cloud is in World Coordinates, one can provide the inverse pose as the "transform" argument. If this argument + is present, the point cloud will be transformed back into ego coordinates + Args: + points (np.array): Point cloud data represented as (N,3) or (N,4) numpy arrays + transform (:class:`Transform`): If in World Coordinate, the transformation used to transform lidar points into ego coordinates + ''' + + def __init__(self, points: np.array = None, transform: Transform = None): + self.points = points + if transform is not None: + self.points = transform.apply(points) + + def load_pcd(self, filepath: str, transform: Transform = None): + ''' + Takes raw pcd file and reads in as numpy array. + Args: + filepath (str): Local filepath to pcd file + transform (:class:`Transform`): If in world, the transformation used to transform lidar points into ego coordinates + ''' + points = read_pcd(filepath) + self.points = points + if transform is not None: + self.points = transform.apply(points) + + +class CamCalibration: + ''' + CamCalibration solely holds the pose of the camera + This CamCalibration will inevitably be transformed by the device_pose + Args: + extrinsic_matrix (np.array): (4,4) extrinsic transformation matrix representing device_to_lidar + ''' + + def __init__(self, extrinsic_matrix=None): + self.extrinsic_matrix = extrinsic_matrix + + @property + def extrinsic_matrix(self): + """Camera extrinsic + :getter: Return camera's extrinsic matrix (pose.inverse[:3, :4]) + :setter: pose = Transform(matrix).inverse + :type: 3x4 matrix + """ + return self.pose.inverse[:3, :4] + + @extrinsic_matrix.setter + def extrinsic_matrix(self, matrix): + ''' + Sets pose as inverse of extrinsic matrix + ''' + self.pose = Transform(matrix).inverse + + +class RawFrame: + ''' + RawFrames are containers for point clouds, image extrinsics, and device pose. + These objects most notably are leveraged to transform point clouds and image extrinsic matrices to the world coordinate frame. + Args: + dev_pose (:class:`Transform`): World Coordinate transformation for frame + **kwargs (Dict[str, :class:`RawPointCloud, :class: CameraCalibration`]): Mappings from sensor name + to pointcloud and camera calibrations. Each frame of a lidar scene must contain exactly one + pointcloud and any number of camera calibrations + ''' + def __init__(self, dev_pose: Transform = None, **kwargs): + self.dev_pose = dev_pose + self.items = {} + for key, value in kwargs.items(): + if isinstance(value, CamCalibration): + self.items[key] = copy.copy(value) + else: + self.items[key] = value + + def get_world_points(self): + """Return the list of points with the frame transformation applied + :returns: List of points in world coordinates + :rtype: np.array + """ + for item in self.items: + if isinstance(self.items[item], RawPointCloud): + return np.hstack( + [ + self.dev_pose @ self.items[item][:, :3], + self.items[item][:, 3:4], + self.items[item][:, 4:5] + ] + ) + + +class RawScene: + ''' + RawsScenes are containers for frames + These objects most notably are leveraged to transform point clouds and image extrinsic matrices to the world coordinate frame. + Args: + dev_pose (:class:`Transform`): World Coordinate transformation for frame + **kwargs (Dict[str, :class:`RawPointCloud, :class: CameraCalibration`]): Mappings from sensor name + to pointcloud and camera calibrations. Each frame of a lidar scene must contain exactly one + pointcloud and any number of camera calibrations + ''' + def __init__(self, frames: [RawFrame] = None): + if frames is None: + self.frames = [] + else: + self.frames = frames + + def add_frame(self, frame: RawFrame = None): + self.frames.append(frame) + + def make_transforms_relative(self): + """Make all the frame transform relative to the first transform/frame. This will set the first transform to position (0,0,0) and heading (1,0,0,0)""" + offset = self.frames[0].dev_pose.inverse + for frame in self.frames: + frame.dev_pose = offset @ frame.dev_pose + + def apply_transforms(self, relative: bool = False): + if relative: + self.make_transforms_relative() + for frame in self.frames: + for item in frame.items: + if isinstance(frame.items[item], RawPointCloud): + frame.items[item].points = np.hstack( + [ + frame.dev_pose @ frame.items[item].points[:, :3], + frame.items[item].points[:, 3:4], + frame.items[item].points[:, 4:5], + ] + ) + else: + wct = frame.dev_pose @ frame.items[item].pose + frame.items[item].pose = wct \ No newline at end of file From 96cacb9214cd38868d11c6bc753db0f1d63af957 Mon Sep 17 00:00:00 2001 From: Pat Bradley Date: Mon, 4 Apr 2022 21:12:03 -0700 Subject: [PATCH 06/14] Sensor Fusion Basic Objects --- IngestDevelopmentScripts/TestScript.py | 43 ++++++++ nucleus/sensorfusion.py | 138 +++++++++++++++++++++++++ nucleus/utils.py | 67 ------------ 3 files changed, 181 insertions(+), 67 deletions(-) create mode 100644 IngestDevelopmentScripts/TestScript.py create mode 100644 nucleus/sensorfusion.py diff --git a/IngestDevelopmentScripts/TestScript.py b/IngestDevelopmentScripts/TestScript.py new file mode 100644 index 00000000..163b611e --- /dev/null +++ b/IngestDevelopmentScripts/TestScript.py @@ -0,0 +1,43 @@ +from nucleus.sensorfusion import * +import numpy as np +import glob + +def format_pointcloud(lidar_np): + mask = lidar_np[:, 4] == 0.0 + pc_1 = lidar_np[mask, :] + pc_1 = np.delete(pc_1, (4), 1) + return pc_1 + +npz_files = sorted(glob.glob("sdk-sample-data/*")) +npz_files.remove("sdk-sample-data/extrinsics_seq000-003_11042021.npz") + +updated_extrinsics = np.load("sdk-sample-data/extrinsics_seq000-003_11042021.npz", allow_pickle = True) +wnsl_extrinsics = updated_extrinsics['camera_WindshieldNarrowStereoLeft_lidar_extrinsic'] +print(f"Camera Extrinsics: \n{wnsl_extrinsics}") + +cam_1_calib = CamCalibration(extrinsic_matrix=wnsl_extrinsics) +print(f"Original Camera Pose:{cam_1_calib.pose}") + +RawScene = RawScene() +for idx,npz_fp in enumerate(npz_files): + print(f"Frame Index: {idx}") + + frame_npz = np.load(npz_fp, allow_pickle=True) + + pointcloud_np= format_pointcloud(frame_npz['points']) + print(f"PointCloud Shape: {pointcloud_np.shape}") + + pointcloud = RawPointCloud(points=pointcloud_np) + + print(f"World Coordinate Transformation:\n{frame_npz['vehicle_local_tf']}") + frame_pose = Transform(frame_npz['vehicle_local_tf']) + print(f"Frame Pose: {frame_pose}") + + raw_frame = RawFrame(lidar=pointcloud, cam1=cam_1_calib, dev_pose=frame_pose) + RawScene.add_frame(raw_frame) + +print(f"Frame 5, Point1 PreTransform: {RawScene.frames[4].items['lidar'].points[0]}") +print(f"Frame 5, Camera Extrinsics PreTransform: {RawScene.frames[4].items['cam1'].pose}") +RawScene.apply_transforms(relative=True) +print(f"Frame 5, Point1 in World: {RawScene.frames[4].items['lidar'].points[0]}") +print(f"Frame 5, Camera Extrinsics PostTransform: {RawScene.frames[4].items['cam1'].pose}") \ No newline at end of file diff --git a/nucleus/sensorfusion.py b/nucleus/sensorfusion.py new file mode 100644 index 00000000..cc3d872b --- /dev/null +++ b/nucleus/sensorfusion.py @@ -0,0 +1,138 @@ +import numpy as np +from nucleus.transform import Transform +from nucleus.utils import read_pcd +import copy + +class RawPointCloud: + ''' + RawPointClouds are containers for raw point cloud data. This structure contains Point Clouds as (N,3) or (N,4) numpy arrays + Point cloud data is assumed to be in ego coordinates. + If the point cloud is in World Coordinates, one can provide the inverse pose as the "transform" argument. If this argument + is present, the point cloud will be transformed back into ego coordinates + Args: + points (np.array): Point cloud data represented as (N,3) or (N,4) numpy arrays + transform (:class:`Transform`): If in World Coordinate, the transformation used to transform lidar points into ego coordinates + ''' + + def __init__(self, points: np.array = None, transform: Transform = None): + self.points = points + if transform is not None: + self.points = transform.apply(points) + + def load_pcd(self, filepath: str, transform: Transform = None): + ''' + Takes raw pcd file and reads in as numpy array. + Args: + filepath (str): Local filepath to pcd file + transform (:class:`Transform`): If in world, the transformation used to transform lidar points into ego coordinates + ''' + points = read_pcd(filepath) + self.points = points + if transform is not None: + self.points = transform.apply(points) + + +class CamCalibration: + ''' + CamCalibration solely holds the pose of the camera + This CamCalibration will inevitably be transformed by the device_pose + Args: + extrinsic_matrix (np.array): (4,4) extrinsic transformation matrix representing device_to_lidar + ''' + + def __init__(self, extrinsic_matrix=None): + self.extrinsic_matrix = extrinsic_matrix + + @property + def extrinsic_matrix(self): + """Camera extrinsic + :getter: Return camera's extrinsic matrix (pose.inverse[:3, :4]) + :setter: pose = Transform(matrix).inverse + :type: 3x4 matrix + """ + return self.pose.inverse[:3, :4] + + @extrinsic_matrix.setter + def extrinsic_matrix(self, matrix): + ''' + Sets pose as inverse of extrinsic matrix + ''' + self.pose = Transform(matrix).inverse + + +class RawFrame: + ''' + RawFrames are containers for point clouds, image extrinsics, and device pose. + These objects most notably are leveraged to transform point clouds and image extrinsic matrices to the world coordinate frame. + Args: + dev_pose (:class:`Transform`): World Coordinate transformation for frame + **kwargs (Dict[str, :class:`RawPointCloud, :class: CameraCalibration`]): Mappings from sensor name + to pointcloud and camera calibrations. Each frame of a lidar scene must contain exactly one + pointcloud and any number of camera calibrations + ''' + def __init__(self, dev_pose: Transform = None, **kwargs): + self.dev_pose = dev_pose + self.items = {} + for key, value in kwargs.items(): + if isinstance(value, CamCalibration): + self.items[key] = copy.copy(value) + else: + self.items[key] = value + + def get_world_points(self): + """Return the list of points with the frame transformation applied + :returns: List of points in world coordinates + :rtype: np.array + """ + for item in self.items: + if isinstance(self.items[item], RawPointCloud): + return np.hstack( + [ + self.dev_pose @ self.items[item][:, :3], + self.items[item][:, 3:4], + self.items[item][:, 4:5] + ] + ) + + +class RawScene: + ''' + RawsScenes are containers for frames + These objects most notably are leveraged to transform point clouds and image extrinsic matrices to the world coordinate frame. + Args: + dev_pose (:class:`Transform`): World Coordinate transformation for frame + **kwargs (Dict[str, :class:`RawPointCloud, :class: CameraCalibration`]): Mappings from sensor name + to pointcloud and camera calibrations. Each frame of a lidar scene must contain exactly one + pointcloud and any number of camera calibrations + ''' + def __init__(self, frames: [RawFrame] = None): + if frames is None: + self.frames = [] + else: + self.frames = frames + + def add_frame(self, frame: RawFrame = None): + self.frames.append(frame) + + def make_transforms_relative(self): + """Make all the frame transform relative to the first transform/frame. This will set the first transform to position (0,0,0) and heading (1,0,0,0)""" + offset = self.frames[0].dev_pose.inverse + for frame in self.frames: + frame.dev_pose = offset @ frame.dev_pose + + def apply_transforms(self, relative: bool = False): + if relative: + self.make_transforms_relative() + for frame in self.frames: + for item in frame.items: + if isinstance(frame.items[item], RawPointCloud): + frame.items[item].points = np.hstack( + [ + frame.dev_pose @ frame.items[item].points[:, :3], + frame.items[item].points[:, 3:4], + frame.items[item].points[:, 4:5], + ] + ) + else: + wct = frame.dev_pose @ frame.items[item].pose + frame.items[item].pose = wct \ No newline at end of file diff --git a/nucleus/utils.py b/nucleus/utils.py index 84c935a3..3be51e77 100644 --- a/nucleus/utils.py +++ b/nucleus/utils.py @@ -4,28 +4,20 @@ import json import uuid from collections import defaultdict -<<<<<<< HEAD from typing import IO, Dict, List, Sequence, Type, Union -import open3d as o3d -======= -from typing import IO, TYPE_CHECKING, Dict, List, Sequence, Type, Union ->>>>>>> origin/master import requests from requests.models import HTTPError -import numpy as np from nucleus.annotation import ( Annotation, BoxAnnotation, CategoryAnnotation, CuboidAnnotation, - LineAnnotation, MultiCategoryAnnotation, PolygonAnnotation, SegmentationAnnotation, ) -from nucleus.errors import NucleusAPIError from .constants import ( ANNOTATION_TYPES, @@ -34,11 +26,7 @@ CATEGORY_TYPE, CUBOID_TYPE, ITEM_KEY, - LAST_PAGE, - LINE_TYPE, MULTICATEGORY_TYPE, - PAGE_SIZE, - PAGE_TOKEN, POLYGON_TYPE, REFERENCE_ID_KEY, SEGMENTATION_TYPE, @@ -48,7 +36,6 @@ BoxPrediction, CategoryPrediction, CuboidPrediction, - LinePrediction, PolygonPrediction, SegmentationPrediction, ) @@ -60,9 +47,6 @@ '\\\\"': '"', } -if TYPE_CHECKING: - from . import NucleusClient - class KeyErrorDict(dict): """Wrapper for response dicts with deprecated keys. @@ -106,7 +90,6 @@ def format_prediction_response( Union[ BoxPrediction, PolygonPrediction, - LinePrediction, CuboidPrediction, CategoryPrediction, SegmentationPrediction, @@ -131,14 +114,12 @@ def format_prediction_response( Union[ Type[BoxPrediction], Type[PolygonPrediction], - Type[LinePrediction], Type[CuboidPrediction], Type[CategoryPrediction], Type[SegmentationPrediction], ], ] = { BOX_TYPE: BoxPrediction, - LINE_TYPE: LinePrediction, POLYGON_TYPE: PolygonPrediction, CUBOID_TYPE: CuboidPrediction, CATEGORY_TYPE: CategoryPrediction, @@ -212,9 +193,6 @@ def convert_export_payload(api_payload): annotations[POLYGON_TYPE].append( PolygonAnnotation.from_json(polygon) ) - for line in row[LINE_TYPE]: - line[REFERENCE_ID_KEY] = row[ITEM_KEY][REFERENCE_ID_KEY] - annotations[LINE_TYPE].append(LineAnnotation.from_json(line)) for box in row[BOX_TYPE]: box[REFERENCE_ID_KEY] = row[ITEM_KEY][REFERENCE_ID_KEY] annotations[BOX_TYPE].append(BoxAnnotation.from_json(box)) @@ -305,48 +283,3 @@ def replace_double_slashes(s: str) -> str: for key, val in STRING_REPLACEMENTS.items(): s = s.replace(key, val) return s - - -<<<<<<< HEAD -def bin_to_numpy(bin_file_path: str = None): - ''' - Takes bin file and returns the Point Cloud - Args: - param filepath: bin filepath - type filepath: str - - Returns: - np_pcd: (x,4) numpy array representation of Point Cloud - type frame_to_s3_URI: np.array - ''' - np_pcd = np.fromfile(bin_file_path, dtype=np.float32).reshape(-1, 4) - return np_pcd - -def read_pcd(fp): - pcd = o3d.io.read_point_cloud(fp) - points = np.asarray(pcd.points) - return points -======= -def paginate_generator( - client: "NucleusClient", - endpoint: str, - result_key: str, - page_size: int = 100000, -): - last_page = False - page_token = None - while not last_page: - try: - response = client.make_request( - {PAGE_TOKEN: page_token, PAGE_SIZE: page_size}, - endpoint, - requests.post, - ) - except NucleusAPIError as e: - if e.status_code == 503: - e.message += f"/n Your request timed out while trying to get a page size of {page_size}. Try lowering the page_size." - raise e - page_token, last_page = response[PAGE_TOKEN], response[LAST_PAGE] - for json_value in response[result_key]: - yield json_value ->>>>>>> origin/master From 3cea6896d33753919478172a065212cc80060145 Mon Sep 17 00:00:00 2001 From: Pat Bradley Date: Mon, 4 Apr 2022 21:30:50 -0700 Subject: [PATCH 07/14] cleaning2 --- nucleus/utils.py | 285 ----------------------------------------------- 1 file changed, 285 deletions(-) delete mode 100644 nucleus/utils.py diff --git a/nucleus/utils.py b/nucleus/utils.py deleted file mode 100644 index 3be51e77..00000000 --- a/nucleus/utils.py +++ /dev/null @@ -1,285 +0,0 @@ -"""Shared stateless utility function library""" - -import io -import json -import uuid -from collections import defaultdict -from typing import IO, Dict, List, Sequence, Type, Union - -import requests -from requests.models import HTTPError - -from nucleus.annotation import ( - Annotation, - BoxAnnotation, - CategoryAnnotation, - CuboidAnnotation, - MultiCategoryAnnotation, - PolygonAnnotation, - SegmentationAnnotation, -) - -from .constants import ( - ANNOTATION_TYPES, - ANNOTATIONS_KEY, - BOX_TYPE, - CATEGORY_TYPE, - CUBOID_TYPE, - ITEM_KEY, - MULTICATEGORY_TYPE, - POLYGON_TYPE, - REFERENCE_ID_KEY, - SEGMENTATION_TYPE, -) -from .dataset_item import DatasetItem -from .prediction import ( - BoxPrediction, - CategoryPrediction, - CuboidPrediction, - PolygonPrediction, - SegmentationPrediction, -) -from .scene import LidarScene, VideoScene - -STRING_REPLACEMENTS = { - "\\\\n": "\n", - "\\\\t": "\t", - '\\\\"': '"', -} - - -class KeyErrorDict(dict): - """Wrapper for response dicts with deprecated keys. - - Parameters: - **kwargs: Mapping from the deprecated key to a warning message. - """ - - def __init__(self, **kwargs): - self._deprecated = {} - - for key, msg in kwargs.items(): - if not isinstance(key, str): - raise TypeError( - f"All keys must be strings! Received non-string '{key}'" - ) - if not isinstance(msg, str): - raise TypeError( - f"All warning messages must be strings! Received non-string '{msg}'" - ) - - self._deprecated[key] = msg - - super().__init__() - - def __missing__(self, key): - """Raises KeyError for deprecated keys, otherwise uses base dict logic.""" - if key in self._deprecated: - raise KeyError(self._deprecated[key]) - try: - super().__missing__(key) - except AttributeError as e: - raise KeyError(key) from e - - -def format_prediction_response( - response: dict, -) -> Union[ - dict, - List[ - Union[ - BoxPrediction, - PolygonPrediction, - CuboidPrediction, - CategoryPrediction, - SegmentationPrediction, - ] - ], -]: - """Helper function to convert JSON response from endpoints to python objects - - Args: - response: JSON dictionary response from REST endpoint. - Returns: - annotation_response: Dictionary containing a list of annotations for each type, - keyed by the type name. - """ - annotation_payload = response.get(ANNOTATIONS_KEY, None) - if not annotation_payload: - # An error occurred - return response - annotation_response = {} - type_key_to_class: Dict[ - str, - Union[ - Type[BoxPrediction], - Type[PolygonPrediction], - Type[CuboidPrediction], - Type[CategoryPrediction], - Type[SegmentationPrediction], - ], - ] = { - BOX_TYPE: BoxPrediction, - POLYGON_TYPE: PolygonPrediction, - CUBOID_TYPE: CuboidPrediction, - CATEGORY_TYPE: CategoryPrediction, - SEGMENTATION_TYPE: SegmentationPrediction, - } - for type_key in annotation_payload: - type_class = type_key_to_class[type_key] - annotation_response[type_key] = [ - type_class.from_json(annotation) - for annotation in annotation_payload[type_key] - ] - return annotation_response - - -def format_dataset_item_response(response: dict) -> dict: - """Format the raw client response into api objects. - - Args: - response: JSON dictionary response from REST endpoint - Returns: - item_dict: A dictionary with two entries, one for the dataset item, and annother - for all of the associated annotations. - """ - if ANNOTATIONS_KEY not in response: - raise ValueError( - f"Server response was missing the annotation key: {response}" - ) - if ITEM_KEY not in response: - raise ValueError( - f"Server response was missing the item key: {response}" - ) - item = response[ITEM_KEY] - annotation_payload = response[ANNOTATIONS_KEY] - - annotation_response = {} - for annotation_type in ANNOTATION_TYPES: - if annotation_type in annotation_payload: - annotation_response[annotation_type] = [ - Annotation.from_json(ann) - for ann in annotation_payload[annotation_type] - ] - return { - ITEM_KEY: DatasetItem.from_json(item), - ANNOTATIONS_KEY: annotation_response, - } - - -def convert_export_payload(api_payload): - """Helper function to convert raw JSON to API objects - - Args: - api_payload: JSON dictionary response from REST endpoint - Returns: - return_payload: A list of dictionaries for each dataset item. Each dictionary - is in the same format as format_dataset_item_response: one key for the - dataset item, another for the annotations. - """ - return_payload = [] - for row in api_payload: - return_payload_row = {} - return_payload_row[ITEM_KEY] = DatasetItem.from_json(row[ITEM_KEY]) - annotations = defaultdict(list) - if row.get(SEGMENTATION_TYPE) is not None: - segmentation = row[SEGMENTATION_TYPE] - segmentation[REFERENCE_ID_KEY] = row[ITEM_KEY][REFERENCE_ID_KEY] - annotations[SEGMENTATION_TYPE] = SegmentationAnnotation.from_json( - segmentation - ) - for polygon in row[POLYGON_TYPE]: - polygon[REFERENCE_ID_KEY] = row[ITEM_KEY][REFERENCE_ID_KEY] - annotations[POLYGON_TYPE].append( - PolygonAnnotation.from_json(polygon) - ) - for box in row[BOX_TYPE]: - box[REFERENCE_ID_KEY] = row[ITEM_KEY][REFERENCE_ID_KEY] - annotations[BOX_TYPE].append(BoxAnnotation.from_json(box)) - for cuboid in row[CUBOID_TYPE]: - cuboid[REFERENCE_ID_KEY] = row[ITEM_KEY][REFERENCE_ID_KEY] - annotations[CUBOID_TYPE].append(CuboidAnnotation.from_json(cuboid)) - for category in row[CATEGORY_TYPE]: - category[REFERENCE_ID_KEY] = row[ITEM_KEY][REFERENCE_ID_KEY] - annotations[CATEGORY_TYPE].append( - CategoryAnnotation.from_json(category) - ) - for multicategory in row[MULTICATEGORY_TYPE]: - multicategory[REFERENCE_ID_KEY] = row[ITEM_KEY][REFERENCE_ID_KEY] - annotations[MULTICATEGORY_TYPE].append( - MultiCategoryAnnotation.from_json(multicategory) - ) - return_payload_row[ANNOTATIONS_KEY] = annotations - return_payload.append(return_payload_row) - return return_payload - - -def serialize_and_write( - upload_units: Sequence[ - Union[DatasetItem, Annotation, LidarScene, VideoScene] - ], - file_pointer, -): - if len(upload_units) == 0: - raise ValueError( - "Expecting at least one object when serializing objects to upload, but got zero. Please try again." - ) - for unit in upload_units: - try: - if isinstance( - unit, (DatasetItem, Annotation, LidarScene, VideoScene) - ): - file_pointer.write(unit.to_json() + "\n") - else: - file_pointer.write(json.dumps(unit) + "\n") - except TypeError as e: - type_name = type(unit).__name__ - message = ( - f"The following {type_name} could not be serialized: {unit}\n" - ) - message += ( - "This is usally an issue with a custom python object being " - "present in the metadata. Please inspect this error and adjust the " - "metadata so it is json-serializable: only python primitives such as " - "strings, ints, floats, lists, and dicts. For example, you must " - "convert numpy arrays into list or lists of lists.\n" - ) - message += f"The specific error was {e}" - raise ValueError(message) from e - - -def upload_to_presigned_url(presigned_url: str, file_pointer: IO): - # TODO optimize this further to deal with truly huge files and flaky internet connection. - upload_response = requests.put(presigned_url, file_pointer) - if not upload_response.ok: - raise HTTPError( - f"Tried to put a file to url, but failed with status {upload_response.status_code}. The detailed error was: {upload_response.text}" - ) - - -def serialize_and_write_to_presigned_url( - upload_units: Sequence[ - Union[DatasetItem, Annotation, LidarScene, VideoScene] - ], - dataset_id: str, - client, -): - """This helper function can be used to serialize a list of API objects to NDJSON.""" - request_id = uuid.uuid4().hex - response = client.make_request( - payload={}, - route=f"dataset/{dataset_id}/signedUrl/{request_id}", - requests_command=requests.get, - ) - - strio = io.StringIO() - serialize_and_write(upload_units, strio) - strio.seek(0) - upload_to_presigned_url(response["signed_url"], strio) - return request_id - - -def replace_double_slashes(s: str) -> str: - for key, val in STRING_REPLACEMENTS.items(): - s = s.replace(key, val) - return s From 26c357803e5181530b73ccd1af7792f600f3f7d5 Mon Sep 17 00:00:00 2001 From: Pat Bradley Date: Tue, 5 Apr 2022 14:00:27 -0700 Subject: [PATCH 08/14] adding utils --- nucleus/utils.py | 325 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 325 insertions(+) create mode 100644 nucleus/utils.py diff --git a/nucleus/utils.py b/nucleus/utils.py new file mode 100644 index 00000000..0c37f42a --- /dev/null +++ b/nucleus/utils.py @@ -0,0 +1,325 @@ +"""Shared stateless utility function library""" + +import io +import json +import uuid +from collections import defaultdict +from typing import IO, TYPE_CHECKING, Dict, List, Sequence, Type, Union + +import requests +from requests.models import HTTPError + +from nucleus.annotation import ( + Annotation, + BoxAnnotation, + CategoryAnnotation, + CuboidAnnotation, + LineAnnotation, + MultiCategoryAnnotation, + PolygonAnnotation, + SegmentationAnnotation, +) +from nucleus.errors import NucleusAPIError + +from .constants import ( + ANNOTATION_TYPES, + ANNOTATIONS_KEY, + BOX_TYPE, + CATEGORY_TYPE, + CUBOID_TYPE, + ITEM_KEY, + LAST_PAGE, + LINE_TYPE, + MULTICATEGORY_TYPE, + PAGE_SIZE, + PAGE_TOKEN, + POLYGON_TYPE, + REFERENCE_ID_KEY, + SEGMENTATION_TYPE, +) +from .dataset_item import DatasetItem +from .prediction import ( + BoxPrediction, + CategoryPrediction, + CuboidPrediction, + LinePrediction, + PolygonPrediction, + SegmentationPrediction, +) +from .scene import LidarScene, VideoScene + +STRING_REPLACEMENTS = { + "\\\\n": "\n", + "\\\\t": "\t", + '\\\\"': '"', +} + +if TYPE_CHECKING: + from . import NucleusClient + + +class KeyErrorDict(dict): + """Wrapper for response dicts with deprecated keys. + + Parameters: + **kwargs: Mapping from the deprecated key to a warning message. + """ + + def __init__(self, **kwargs): + self._deprecated = {} + + for key, msg in kwargs.items(): + if not isinstance(key, str): + raise TypeError( + f"All keys must be strings! Received non-string '{key}'" + ) + if not isinstance(msg, str): + raise TypeError( + f"All warning messages must be strings! Received non-string '{msg}'" + ) + + self._deprecated[key] = msg + + super().__init__() + + def __missing__(self, key): + """Raises KeyError for deprecated keys, otherwise uses base dict logic.""" + if key in self._deprecated: + raise KeyError(self._deprecated[key]) + try: + super().__missing__(key) + except AttributeError as e: + raise KeyError(key) from e + + +def format_prediction_response( + response: dict, +) -> Union[ + dict, + List[ + Union[ + BoxPrediction, + PolygonPrediction, + LinePrediction, + CuboidPrediction, + CategoryPrediction, + SegmentationPrediction, + ] + ], +]: + """Helper function to convert JSON response from endpoints to python objects + + Args: + response: JSON dictionary response from REST endpoint. + Returns: + annotation_response: Dictionary containing a list of annotations for each type, + keyed by the type name. + """ + annotation_payload = response.get(ANNOTATIONS_KEY, None) + if not annotation_payload: + # An error occurred + return response + annotation_response = {} + type_key_to_class: Dict[ + str, + Union[ + Type[BoxPrediction], + Type[PolygonPrediction], + Type[LinePrediction], + Type[CuboidPrediction], + Type[CategoryPrediction], + Type[SegmentationPrediction], + ], + ] = { + BOX_TYPE: BoxPrediction, + LINE_TYPE: LinePrediction, + POLYGON_TYPE: PolygonPrediction, + CUBOID_TYPE: CuboidPrediction, + CATEGORY_TYPE: CategoryPrediction, + SEGMENTATION_TYPE: SegmentationPrediction, + } + for type_key in annotation_payload: + type_class = type_key_to_class[type_key] + annotation_response[type_key] = [ + type_class.from_json(annotation) + for annotation in annotation_payload[type_key] + ] + return annotation_response + + +def format_dataset_item_response(response: dict) -> dict: + """Format the raw client response into api objects. + + Args: + response: JSON dictionary response from REST endpoint + Returns: + item_dict: A dictionary with two entries, one for the dataset item, and annother + for all of the associated annotations. + """ + if ANNOTATIONS_KEY not in response: + raise ValueError( + f"Server response was missing the annotation key: {response}" + ) + if ITEM_KEY not in response: + raise ValueError( + f"Server response was missing the item key: {response}" + ) + item = response[ITEM_KEY] + annotation_payload = response[ANNOTATIONS_KEY] + + annotation_response = {} + for annotation_type in ANNOTATION_TYPES: + if annotation_type in annotation_payload: + annotation_response[annotation_type] = [ + Annotation.from_json(ann) + for ann in annotation_payload[annotation_type] + ] + return { + ITEM_KEY: DatasetItem.from_json(item), + ANNOTATIONS_KEY: annotation_response, + } + + +def convert_export_payload(api_payload): + """Helper function to convert raw JSON to API objects + + Args: + api_payload: JSON dictionary response from REST endpoint + Returns: + return_payload: A list of dictionaries for each dataset item. Each dictionary + is in the same format as format_dataset_item_response: one key for the + dataset item, another for the annotations. + """ + return_payload = [] + for row in api_payload: + return_payload_row = {} + return_payload_row[ITEM_KEY] = DatasetItem.from_json(row[ITEM_KEY]) + annotations = defaultdict(list) + if row.get(SEGMENTATION_TYPE) is not None: + segmentation = row[SEGMENTATION_TYPE] + segmentation[REFERENCE_ID_KEY] = row[ITEM_KEY][REFERENCE_ID_KEY] + annotations[SEGMENTATION_TYPE] = SegmentationAnnotation.from_json( + segmentation + ) + for polygon in row[POLYGON_TYPE]: + polygon[REFERENCE_ID_KEY] = row[ITEM_KEY][REFERENCE_ID_KEY] + annotations[POLYGON_TYPE].append( + PolygonAnnotation.from_json(polygon) + ) + for line in row[LINE_TYPE]: + line[REFERENCE_ID_KEY] = row[ITEM_KEY][REFERENCE_ID_KEY] + annotations[LINE_TYPE].append(LineAnnotation.from_json(line)) + for box in row[BOX_TYPE]: + box[REFERENCE_ID_KEY] = row[ITEM_KEY][REFERENCE_ID_KEY] + annotations[BOX_TYPE].append(BoxAnnotation.from_json(box)) + for cuboid in row[CUBOID_TYPE]: + cuboid[REFERENCE_ID_KEY] = row[ITEM_KEY][REFERENCE_ID_KEY] + annotations[CUBOID_TYPE].append(CuboidAnnotation.from_json(cuboid)) + for category in row[CATEGORY_TYPE]: + category[REFERENCE_ID_KEY] = row[ITEM_KEY][REFERENCE_ID_KEY] + annotations[CATEGORY_TYPE].append( + CategoryAnnotation.from_json(category) + ) + for multicategory in row[MULTICATEGORY_TYPE]: + multicategory[REFERENCE_ID_KEY] = row[ITEM_KEY][REFERENCE_ID_KEY] + annotations[MULTICATEGORY_TYPE].append( + MultiCategoryAnnotation.from_json(multicategory) + ) + return_payload_row[ANNOTATIONS_KEY] = annotations + return_payload.append(return_payload_row) + return return_payload + + +def serialize_and_write( + upload_units: Sequence[ + Union[DatasetItem, Annotation, LidarScene, VideoScene] + ], + file_pointer, +): + if len(upload_units) == 0: + raise ValueError( + "Expecting at least one object when serializing objects to upload, but got zero. Please try again." + ) + for unit in upload_units: + try: + if isinstance( + unit, (DatasetItem, Annotation, LidarScene, VideoScene) + ): + file_pointer.write(unit.to_json() + "\n") + else: + file_pointer.write(json.dumps(unit) + "\n") + except TypeError as e: + type_name = type(unit).__name__ + message = ( + f"The following {type_name} could not be serialized: {unit}\n" + ) + message += ( + "This is usally an issue with a custom python object being " + "present in the metadata. Please inspect this error and adjust the " + "metadata so it is json-serializable: only python primitives such as " + "strings, ints, floats, lists, and dicts. For example, you must " + "convert numpy arrays into list or lists of lists.\n" + ) + message += f"The specific error was {e}" + raise ValueError(message) from e + + +def upload_to_presigned_url(presigned_url: str, file_pointer: IO): + # TODO optimize this further to deal with truly huge files and flaky internet connection. + upload_response = requests.put(presigned_url, file_pointer) + if not upload_response.ok: + raise HTTPError( + f"Tried to put a file to url, but failed with status {upload_response.status_code}. The detailed error was: {upload_response.text}" + ) + + +def serialize_and_write_to_presigned_url( + upload_units: Sequence[ + Union[DatasetItem, Annotation, LidarScene, VideoScene] + ], + dataset_id: str, + client, +): + """This helper function can be used to serialize a list of API objects to NDJSON.""" + request_id = uuid.uuid4().hex + response = client.make_request( + payload={}, + route=f"dataset/{dataset_id}/signedUrl/{request_id}", + requests_command=requests.get, + ) + + strio = io.StringIO() + serialize_and_write(upload_units, strio) + strio.seek(0) + upload_to_presigned_url(response["signed_url"], strio) + return request_id + + +def replace_double_slashes(s: str) -> str: + for key, val in STRING_REPLACEMENTS.items(): + s = s.replace(key, val) + return s + + +def paginate_generator( + client: "NucleusClient", + endpoint: str, + result_key: str, + page_size: int = 100000, +): + last_page = False + page_token = None + while not last_page: + try: + response = client.make_request( + {PAGE_TOKEN: page_token, PAGE_SIZE: page_size}, + endpoint, + requests.post, + ) + except NucleusAPIError as e: + if e.status_code == 503: + e.message += f"/n Your request timed out while trying to get a page size of {page_size}. Try lowering the page_size." + raise e + page_token, last_page = response[PAGE_TOKEN], response[LAST_PAGE] + for json_value in response[result_key]: + yield json_value From f3c2fa7c2be7ba09af33351954f77f547c5ca4b7 Mon Sep 17 00:00:00 2001 From: patrick-scale <84043557+patrick-scale@users.noreply.github.com> Date: Fri, 8 Apr 2022 11:25:01 -0700 Subject: [PATCH 09/14] Update nucleus/sensorfusion.py Co-authored-by: Sasha Harrison <70984140+sasha-scale@users.noreply.github.com> --- nucleus/sensorfusion.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nucleus/sensorfusion.py b/nucleus/sensorfusion.py index cc3d872b..d025dd7e 100644 --- a/nucleus/sensorfusion.py +++ b/nucleus/sensorfusion.py @@ -32,7 +32,7 @@ def load_pcd(self, filepath: str, transform: Transform = None): self.points = transform.apply(points) -class CamCalibration: +class CameraCalibration: ''' CamCalibration solely holds the pose of the camera This CamCalibration will inevitably be transformed by the device_pose From 9d9e32dd353d5b6b3fcb59139c6b450caf4d0deb Mon Sep 17 00:00:00 2001 From: patrick-scale <84043557+patrick-scale@users.noreply.github.com> Date: Fri, 8 Apr 2022 11:26:40 -0700 Subject: [PATCH 10/14] Update nucleus/sensorfusion.py Co-authored-by: Sasha Harrison <70984140+sasha-scale@users.noreply.github.com> --- nucleus/sensorfusion.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nucleus/sensorfusion.py b/nucleus/sensorfusion.py index d025dd7e..f03bb85c 100644 --- a/nucleus/sensorfusion.py +++ b/nucleus/sensorfusion.py @@ -70,7 +70,7 @@ class RawFrame: to pointcloud and camera calibrations. Each frame of a lidar scene must contain exactly one pointcloud and any number of camera calibrations ''' - def __init__(self, dev_pose: Transform = None, **kwargs): + def __init__(self, device_pose: Transform = None, **kwargs): self.dev_pose = dev_pose self.items = {} for key, value in kwargs.items(): From 5021ce6bbe8274d2faf6ef7f6a3e5098d060f628 Mon Sep 17 00:00:00 2001 From: patrick-scale <84043557+patrick-scale@users.noreply.github.com> Date: Fri, 8 Apr 2022 11:27:05 -0700 Subject: [PATCH 11/14] Update nucleus/sensorfusion.py Co-authored-by: Sasha Harrison <70984140+sasha-scale@users.noreply.github.com> --- nucleus/sensorfusion.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nucleus/sensorfusion.py b/nucleus/sensorfusion.py index f03bb85c..69c043d8 100644 --- a/nucleus/sensorfusion.py +++ b/nucleus/sensorfusion.py @@ -105,7 +105,8 @@ class RawScene: to pointcloud and camera calibrations. Each frame of a lidar scene must contain exactly one pointcloud and any number of camera calibrations ''' - def __init__(self, frames: [RawFrame] = None): +from typing import List + def __init__(self, frames: List[RawFrame] = []): if frames is None: self.frames = [] else: From 4cca2e6e68c22cfd97067cb39b4d88990f53a6f7 Mon Sep 17 00:00:00 2001 From: patrick-scale <84043557+patrick-scale@users.noreply.github.com> Date: Fri, 8 Apr 2022 11:27:46 -0700 Subject: [PATCH 12/14] Update nucleus/transform.py Co-authored-by: Sasha Harrison <70984140+sasha-scale@users.noreply.github.com> --- nucleus/transform.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nucleus/transform.py b/nucleus/transform.py index 21a5a8bd..a14e4675 100644 --- a/nucleus/transform.py +++ b/nucleus/transform.py @@ -6,7 +6,7 @@ class Transform: """Transform object represent a rigid transformation matrix (rotation and translation). - Transform is a 4x4 matrix, although it could be instance using (16,1), (3,4), (3,3) or (3,1) matrixes. + Transform is a 4x4 matrix, although it could be instance using (16,1), (3,4), (3,3) or (3,1) matrices. **Note**: not all the methods from Transform will work using scaled/small matrixes .. highlight:: python From f7c0d39fb0fb6ad7794a97d6642ca599b4f87f18 Mon Sep 17 00:00:00 2001 From: Pat Bradley Date: Fri, 8 Apr 2022 16:25:56 -0700 Subject: [PATCH 13/14] feedback updates --- IngestDevelopmentScripts/TestScript.py | 4 +- nucleus/sensorfusion.py | 56 +++++++++++++++++--------- nucleus/sensorfusion_utils.py | 19 +++++++++ 3 files changed, 57 insertions(+), 22 deletions(-) create mode 100644 nucleus/sensorfusion_utils.py diff --git a/IngestDevelopmentScripts/TestScript.py b/IngestDevelopmentScripts/TestScript.py index 163b611e..902ce0c6 100644 --- a/IngestDevelopmentScripts/TestScript.py +++ b/IngestDevelopmentScripts/TestScript.py @@ -15,7 +15,7 @@ def format_pointcloud(lidar_np): wnsl_extrinsics = updated_extrinsics['camera_WindshieldNarrowStereoLeft_lidar_extrinsic'] print(f"Camera Extrinsics: \n{wnsl_extrinsics}") -cam_1_calib = CamCalibration(extrinsic_matrix=wnsl_extrinsics) +cam_1_calib = CameraCalibration(extrinsic_matrix=wnsl_extrinsics) print(f"Original Camera Pose:{cam_1_calib.pose}") RawScene = RawScene() @@ -33,7 +33,7 @@ def format_pointcloud(lidar_np): frame_pose = Transform(frame_npz['vehicle_local_tf']) print(f"Frame Pose: {frame_pose}") - raw_frame = RawFrame(lidar=pointcloud, cam1=cam_1_calib, dev_pose=frame_pose) + raw_frame = RawFrame(lidar=pointcloud, cam1=cam_1_calib, device_pose=frame_pose) RawScene.add_frame(raw_frame) print(f"Frame 5, Point1 PreTransform: {RawScene.frames[4].items['lidar'].points[0]}") diff --git a/nucleus/sensorfusion.py b/nucleus/sensorfusion.py index 69c043d8..2722b722 100644 --- a/nucleus/sensorfusion.py +++ b/nucleus/sensorfusion.py @@ -1,21 +1,41 @@ import numpy as np from nucleus.transform import Transform -from nucleus.utils import read_pcd import copy +from typing import List +from dataclasses import dataclass + +from sensorfusion_utils import read_pcd +@dataclass class RawPointCloud: ''' RawPointClouds are containers for raw point cloud data. This structure contains Point Clouds as (N,3) or (N,4) numpy arrays Point cloud data is assumed to be in ego coordinates. - If the point cloud is in World Coordinates, one can provide the inverse pose as the "transform" argument. If this argument - is present, the point cloud will be transformed back into ego coordinates + + If the point cloud is in world coordinates, one can provide the inverse pose as the "transform" argument. If this argument + is present, the point cloud will be transformed back into ego coordinates by: + + transform.apply(points) + + or in the extended implementation: + + points_4d = np.hstack([points[:, :3], np.ones((points.shape[0], 1))]) + transformed_4d = points_4d.dot(self.matrix.T) + return np.hstack([transformed_4d[:, :3], points[:, 3:]]) + + Args: points (np.array): Point cloud data represented as (N,3) or (N,4) numpy arrays transform (:class:`Transform`): If in World Coordinate, the transformation used to transform lidar points into ego coordinates ''' + points: np.array def __init__(self, points: np.array = None, transform: Transform = None): self.points = points + + if points is not None and (len(self.points.shape) != 2 or self.points.shape[1] not in [3, 4]): + raise Exception(f'numpy array has unexpected shape{self.points.shape}. Please convert to (N,3) or (N,4) numpy array where each row is [x,y,z] or [x,y,z,i]') + if transform is not None: self.points = transform.apply(points) @@ -34,8 +54,8 @@ def load_pcd(self, filepath: str, transform: Transform = None): class CameraCalibration: ''' - CamCalibration solely holds the pose of the camera - This CamCalibration will inevitably be transformed by the device_pose + CameraCalibration solely holds the pose of the camera + This CameraCalibration will inevitably be transformed by the device_pose Args: extrinsic_matrix (np.array): (4,4) extrinsic transformation matrix representing device_to_lidar ''' @@ -65,16 +85,16 @@ class RawFrame: RawFrames are containers for point clouds, image extrinsics, and device pose. These objects most notably are leveraged to transform point clouds and image extrinsic matrices to the world coordinate frame. Args: - dev_pose (:class:`Transform`): World Coordinate transformation for frame + device_pose (:class:`Transform`): World Coordinate transformation for frame **kwargs (Dict[str, :class:`RawPointCloud, :class: CameraCalibration`]): Mappings from sensor name to pointcloud and camera calibrations. Each frame of a lidar scene must contain exactly one pointcloud and any number of camera calibrations ''' def __init__(self, device_pose: Transform = None, **kwargs): - self.dev_pose = dev_pose + self.device_pose = device_pose self.items = {} for key, value in kwargs.items(): - if isinstance(value, CamCalibration): + if isinstance(value, CameraCalibration): self.items[key] = copy.copy(value) else: self.items[key] = value @@ -88,7 +108,7 @@ def get_world_points(self): if isinstance(self.items[item], RawPointCloud): return np.hstack( [ - self.dev_pose @ self.items[item][:, :3], + self.device_pose @ self.items[item][:, :3], self.items[item][:, 3:4], self.items[item][:, 4:5] ] @@ -98,28 +118,24 @@ def get_world_points(self): class RawScene: ''' RawsScenes are containers for frames - These objects most notably are leveraged to transform point clouds and image extrinsic matrices to the world coordinate frame. Args: - dev_pose (:class:`Transform`): World Coordinate transformation for frame - **kwargs (Dict[str, :class:`RawPointCloud, :class: CameraCalibration`]): Mappings from sensor name - to pointcloud and camera calibrations. Each frame of a lidar scene must contain exactly one - pointcloud and any number of camera calibrations + frames (:class:`RawFrame`): Indexed sequential frame objects composing the scene ''' -from typing import List + def __init__(self, frames: List[RawFrame] = []): if frames is None: self.frames = [] else: self.frames = frames - def add_frame(self, frame: RawFrame = None): + def add_frame(self, frame: RawFrame): self.frames.append(frame) def make_transforms_relative(self): """Make all the frame transform relative to the first transform/frame. This will set the first transform to position (0,0,0) and heading (1,0,0,0)""" - offset = self.frames[0].dev_pose.inverse + offset = self.frames[0].device_pose.inverse for frame in self.frames: - frame.dev_pose = offset @ frame.dev_pose + frame.device_pose = offset @ frame.device_pose def apply_transforms(self, relative: bool = False): if relative: @@ -129,11 +145,11 @@ def apply_transforms(self, relative: bool = False): if isinstance(frame.items[item], RawPointCloud): frame.items[item].points = np.hstack( [ - frame.dev_pose @ frame.items[item].points[:, :3], + frame.device_pose @ frame.items[item].points[:, :3], frame.items[item].points[:, 3:4], frame.items[item].points[:, 4:5], ] ) else: - wct = frame.dev_pose @ frame.items[item].pose + wct = frame.device_pose @ frame.items[item].pose frame.items[item].pose = wct \ No newline at end of file diff --git a/nucleus/sensorfusion_utils.py b/nucleus/sensorfusion_utils.py new file mode 100644 index 00000000..25e40e76 --- /dev/null +++ b/nucleus/sensorfusion_utils.py @@ -0,0 +1,19 @@ +import open3d as o3d +import numpy as np + +def read_pcd(pcd_filepath): + ''' + Loads in pcd file and returns (N,3) or (N,4) numpy array + + Args: + param pcd_filepath : filepath to local .pcd file + type pcd_filepath: str + + Returns: + point_numpy: (N,4) or (N,3) numpy array of points + type point_numpy: np.array + ''' + point_cloud = o3d.io.read_point_cloud(pcd_filepath) + + point_numpy = np.asarray(point_cloud.points)[:, :4] + return point_numpy \ No newline at end of file From 203ec131803dd0d8e14f3a6a5f86c9d3d9021ef4 Mon Sep 17 00:00:00 2001 From: Pat Bradley Date: Fri, 8 Apr 2022 16:49:03 -0700 Subject: [PATCH 14/14] exception updates and name changes --- .../testing_script.py | 0 nucleus/sensorfusion.py | 9 ++++++++- 2 files changed, 8 insertions(+), 1 deletion(-) rename IngestDevelopmentScripts/TestScript.py => ingest_development_scripts/testing_script.py (100%) diff --git a/IngestDevelopmentScripts/TestScript.py b/ingest_development_scripts/testing_script.py similarity index 100% rename from IngestDevelopmentScripts/TestScript.py rename to ingest_development_scripts/testing_script.py diff --git a/nucleus/sensorfusion.py b/nucleus/sensorfusion.py index 2722b722..05708480 100644 --- a/nucleus/sensorfusion.py +++ b/nucleus/sensorfusion.py @@ -128,6 +128,8 @@ def __init__(self, frames: List[RawFrame] = []): else: self.frames = frames + self.transformed = False + def add_frame(self, frame: RawFrame): self.frames.append(frame) @@ -138,8 +140,12 @@ def make_transforms_relative(self): frame.device_pose = offset @ frame.device_pose def apply_transforms(self, relative: bool = False): + if self.transformed: + raise Exception("apply_transforms was called more than once. Raw Scene has already been transformed!") + if relative: self.make_transforms_relative() + for frame in self.frames: for item in frame.items: if isinstance(frame.items[item], RawPointCloud): @@ -152,4 +158,5 @@ def apply_transforms(self, relative: bool = False): ) else: wct = frame.device_pose @ frame.items[item].pose - frame.items[item].pose = wct \ No newline at end of file + frame.items[item].pose = wct + self.transformed = True \ No newline at end of file