From b46de7fa26abafaba422479466b3811b4a54027b Mon Sep 17 00:00:00 2001 From: Benjamin Wilson Date: Thu, 25 May 2023 21:33:45 -0700 Subject: [PATCH 1/5] Update export tool. --- rust/src/bin/export_augmentation_database.rs | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/rust/src/bin/export_augmentation_database.rs b/rust/src/bin/export_augmentation_database.rs index ef763e9d..154a8af6 100644 --- a/rust/src/bin/export_augmentation_database.rs +++ b/rust/src/bin/export_augmentation_database.rs @@ -47,6 +47,9 @@ const NUM_ACCUMULATED_SWEEPS: usize = 1; /// Memory maps the sweeps for fast pre-processing. Requires .feather files to be uncompressed. const MEMORY_MAPPED: bool = false; +/// Minimum number of points required to export an object. +const MIN_NUM_PTS: usize = 5; + static DST_DATASET_NAME: Lazy = Lazy::new(|| format!("{DATASET_NAME}_{NUM_ACCUMULATED_SWEEPS}_database")); static SRC_PREFIX: Lazy = Lazy::new(|| ROOT_DIR.join(DATASET_NAME).join(DATASET_TYPE)); @@ -113,6 +116,9 @@ pub fn main() { .collect_vec(); let points_i = lidar_ndarray.select(Axis(0), &indices); + if points_i.shape()[0] < MIN_NUM_PTS { + continue; + } let data_frame_i = _build_data_frame(points_i, EXPORTED_COLUMN_NAMES.clone()); category_counter From 82537da4ab77c2d0507e704f437310e00860c9c9 Mon Sep 17 00:00:00 2001 From: Benjamin Wilson Date: Thu, 25 May 2023 21:39:00 -0700 Subject: [PATCH 2/5] Prevent reading poses if accumulated sweeps is 1. --- rust/src/io.rs | 38 +++++++++++++++++++++++--------------- 1 file changed, 23 insertions(+), 15 deletions(-) diff --git a/rust/src/io.rs b/rust/src/io.rs index 37588000..ed739e42 100644 --- a/rust/src/io.rs +++ b/rust/src/io.rs @@ -86,23 +86,29 @@ pub fn read_accumulate_lidar( let start_idx = i64::max(idx as i64 - num_accumulated_sweeps as i64 + 1, 0) as usize; let log_ids = file_index["log_id"].utf8().unwrap(); let timestamps = file_index["timestamp_ns"].u64().unwrap(); - let poses_path = log_dir.join("city_SE3_egovehicle.feather"); - let poses = read_feather_eager(&poses_path, memory_mapped); - let pose_ref = ndarray_filtered_from_frame( - &poses, - cols(POSE_COLUMNS), - col("timestamp_ns").eq(timestamp_ns), - ); + let (poses, ego_se3_city) = if num_accumulated_sweeps > 1 { + let poses_path = log_dir.join("city_SE3_egovehicle.feather"); + let poses = read_feather_eager(&poses_path, memory_mapped); - let translation = pose_ref.slice(s![0, ..3]).as_standard_layout().to_owned(); - let quat_wxyz = pose_ref.slice(s![0, 3..]).as_standard_layout().to_owned(); - let rotation = _quat_to_mat3(&quat_wxyz.view()); - let city_se3_ego = SE3 { - rotation, - translation, + let pose_ref = ndarray_filtered_from_frame( + &poses, + cols(POSE_COLUMNS), + col("timestamp_ns").eq(timestamp_ns), + ); + + let translation = pose_ref.slice(s![0, ..3]).as_standard_layout().to_owned(); + let quat_wxyz = pose_ref.slice(s![0, 3..]).as_standard_layout().to_owned(); + let rotation = _quat_to_mat3(&quat_wxyz.view()); + let city_se3_ego = SE3 { + rotation, + translation, + }; + let ego_se3_city = city_se3_ego.inverse(); + (Some(poses), Some(ego_se3_city)) + } else { + (None, None) }; - let ego_se3_city = city_se3_ego.inverse(); let indices: Vec<_> = (start_idx..=idx).collect(); let mut lidar_list = indices .par_iter() @@ -136,6 +142,8 @@ pub fn read_accumulate_lidar( .unwrap(); let pose_i = poses + .clone() + .unwrap() .clone() .lazy() .filter(col("timestamp_ns").eq(lit(timestamp_ns_i))) @@ -144,7 +152,7 @@ pub fn read_accumulate_lidar( .unwrap(); let city_se3_ego_i = data_frame_to_se3(pose_i); - let ego_ref_se3_ego_i = ego_se3_city.compose(&city_se3_ego_i); + let ego_ref_se3_ego_i = ego_se3_city.clone().unwrap().compose(&city_se3_ego_i); let xyz_ref = ego_ref_se3_ego_i.transform_from(&xyz.view()); let x_ref = Series::new("x", xyz_ref.slice(s![.., 0]).to_owned().into_raw_vec()); let y_ref = Series::new("y", xyz_ref.slice(s![.., 1]).to_owned().into_raw_vec()); From de4fa571b1cf1e05368cb16f2a763f917e372dc0 Mon Sep 17 00:00:00 2001 From: Benjamin Wilson Date: Fri, 26 May 2023 00:41:27 -0400 Subject: [PATCH 3/5] Default to memory mapping. --- rust/src/bin/export_accumulated_sweeps.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rust/src/bin/export_accumulated_sweeps.rs b/rust/src/bin/export_accumulated_sweeps.rs index 88c8e760..6a5d966b 100644 --- a/rust/src/bin/export_accumulated_sweeps.rs +++ b/rust/src/bin/export_accumulated_sweeps.rs @@ -34,7 +34,7 @@ static SPLIT_NAMES: Lazy> = Lazy::new(|| vec!["train", "val", "test"]) const NUM_ACCUMULATED_SWEEPS: usize = 5; /// Memory maps the sweeps for fast pre-processing. Requires .feather files to be uncompressed. -const MEMORY_MAPPED: bool = false; +const MEMORY_MAPPED: bool = true; static DST_DATASET_NAME: Lazy = Lazy::new(|| format!("{DATASET_NAME}_{NUM_ACCUMULATED_SWEEPS}_sweep")); From 5c85667a996b0fc3de3df25554792029dde96296 Mon Sep 17 00:00:00 2001 From: Benjamin Wilson Date: Thu, 25 May 2023 22:04:49 -0700 Subject: [PATCH 4/5] Remove clone. --- rust/src/io.rs | 1 - 1 file changed, 1 deletion(-) diff --git a/rust/src/io.rs b/rust/src/io.rs index ed739e42..fd4b0ca4 100644 --- a/rust/src/io.rs +++ b/rust/src/io.rs @@ -144,7 +144,6 @@ pub fn read_accumulate_lidar( let pose_i = poses .clone() .unwrap() - .clone() .lazy() .filter(col("timestamp_ns").eq(lit(timestamp_ns_i))) .select(&[cols(POSE_COLUMNS)]) From 3b3b618bb04ad89429d865e3383c44e7ad7741f1 Mon Sep 17 00:00:00 2001 From: Benjamin Wilson Date: Thu, 25 May 2023 22:43:45 -0700 Subject: [PATCH 5/5] Improve pinhole. --- rust/src/geometry/camera/pinhole_camera.rs | 31 ++++++++++------------ 1 file changed, 14 insertions(+), 17 deletions(-) diff --git a/rust/src/geometry/camera/pinhole_camera.rs b/rust/src/geometry/camera/pinhole_camera.rs index b47b3ed8..5a3ddd7c 100644 --- a/rust/src/geometry/camera/pinhole_camera.rs +++ b/rust/src/geometry/camera/pinhole_camera.rs @@ -1,6 +1,6 @@ use std::{ops::DivAssign, path::Path}; -use ndarray::{par_azip, s, Array, ArrayView, Ix1, Ix2}; +use ndarray::{par_azip, s, Array, ArrayView, Axis, Ix1, Ix2}; use polars::{ lazy::dsl::{col, lit}, prelude::{DataFrame, IntoLazy}, @@ -156,16 +156,16 @@ impl PinholeCamera { pub fn cull_to_view_frustum( self, uv: &ArrayView, - points_camera: &ArrayView, + z: &ArrayView, ) -> Array { let num_points = uv.shape()[0]; let mut is_within_frustum = Array::::from_vec(vec![false; num_points]) .into_shape([num_points, 1]) .unwrap(); - par_azip!((mut is_within_frustum_i in is_within_frustum.outer_iter_mut(), uv_row in uv.outer_iter(), point_cam in points_camera.outer_iter()) { + par_azip!((mut is_within_frustum_i in is_within_frustum.outer_iter_mut(), uv_row in uv.outer_iter(), z_i in z) { let is_within_frustum_x = (uv_row[0] >= 0.) && (uv_row[0] < self.width_px() as f32); let is_within_frustum_y = (uv_row[1] >= 0.) && (uv_row[1] < self.height_px() as f32); - let is_within_frustum_z = point_cam[2] > 0.; + let is_within_frustum_z = z_i > &0.; is_within_frustum_i[0] = is_within_frustum_x & is_within_frustum_y & is_within_frustum_z; }); is_within_frustum @@ -175,20 +175,17 @@ impl PinholeCamera { pub fn project_ego_to_image( &self, points_ego: Array, - ) -> (Array, Array, Array) { + ) -> (Array, Array) { // Convert cartesian to homogeneous coordinates. let points_hom_ego = cart_to_hom(points_ego); - let points_hom_cam = points_hom_ego.dot(&self.extrinsics().t()); - - let mut uvz = points_hom_cam - .slice(s![.., ..3]) - .dot(&self.intrinsics.k().t()); - let z = uvz.slice(s![.., 2..3]).clone().to_owned(); - uvz.slice_mut(s![.., ..2]).div_assign(&z); - let is_valid_points = self - .clone() - .cull_to_view_frustum(&uvz.view(), &points_hom_cam.view()); - (uvz.to_owned(), points_hom_cam.to_owned(), is_valid_points) + let pvm = self.p().dot(&self.extrinsics()); + let mut uvz = points_hom_ego.dot(&pvm.t()).slice(s![.., ..3]).to_owned(); + let z: ndarray::ArrayBase, ndarray::Dim<[usize; 1]>> = + uvz.slice(s![.., 2]).clone().to_owned(); + uvz.slice_mut(s![.., ..2]) + .div_assign(&z.clone().insert_axis(Axis(1))); + let is_valid_points = self.clone().cull_to_view_frustum(&uvz.view(), &z.view()); + (uvz.to_owned(), is_valid_points) } /// Project a collection of 3D points (provided in the egovehicle frame) to the image plane. @@ -197,7 +194,7 @@ impl PinholeCamera { points_ego: Array, city_se3_ego_camera_t: SE3, city_se3_ego_lidar_t: SE3, - ) -> (Array, Array, Array) { + ) -> (Array, Array) { // Convert cartesian to homogeneous coordinates. let ego_cam_t_se3_ego_lidar_t = city_se3_ego_camera_t .inverse()