Improve pinhole, io, and augment db.

rust/src/bin/export_accumulated_sweeps.rs

@@ -34,7 +34,7 @@ static SPLIT_NAMES: Lazy<Vec<&str>> = 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<String> =
     Lazy::new(|| format!("{DATASET_NAME}_{NUM_ACCUMULATED_SWEEPS}_sweep"));

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<String> =
     Lazy::new(|| format!("{DATASET_NAME}_{NUM_ACCUMULATED_SWEEPS}_database"));
 static SRC_PREFIX: Lazy<PathBuf> = 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

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<f32, Ix2>,
-        points_camera: &ArrayView<f32, Ix2>,
+        z: &ArrayView<f32, Ix1>,
     ) -> Array<bool, Ix2> {
         let num_points = uv.shape()[0];
         let mut is_within_frustum = Array::<bool, Ix1>::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<f32, Ix2>,
-    ) -> (Array<f32, Ix2>, Array<f32, Ix2>, Array<bool, Ix2>) {
+    ) -> (Array<f32, Ix2>, Array<bool, Ix2>) {
         // 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::OwnedRepr<f32>, 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<f32, Ix2>,
         city_se3_ego_camera_t: SE3,
         city_se3_ego_lidar_t: SE3,
-    ) -> (Array<f32, Ix2>, Array<f32, Ix2>, Array<bool, Ix2>) {
+    ) -> (Array<f32, Ix2>, Array<bool, Ix2>) {
         // Convert cartesian to homogeneous coordinates.
         let ego_cam_t_se3_ego_lidar_t = city_se3_ego_camera_t
             .inverse()

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()
@@ -137,14 +143,15 @@ pub fn read_accumulate_lidar(
 
                 let pose_i = poses
                     .clone()
+                    .unwrap()
                     .lazy()
                     .filter(col("timestamp_ns").eq(lit(timestamp_ns_i)))
                     .select(&[cols(POSE_COLUMNS)])
                     .collect()
                     .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());