From a9f43ae12aa9ca938f11e148a23d97a0c6fa059c Mon Sep 17 00:00:00 2001 From: catevita Date: Mon, 20 May 2019 09:00:31 +0200 Subject: [PATCH 01/14] added node id iterator --- src/math.rs | 10 ++++ src/octree/batch_iterator.rs | 75 ----------------------------- src/octree/mod.rs | 55 +++++++++++++++++++++- src/octree/octree_iterator.rs | 39 +++++++++++++++ src/octree/octree_test.rs | 89 +++++++++++++++++++++++++++++++++++ 5 files changed, 191 insertions(+), 77 deletions(-) create mode 100644 src/octree/octree_test.rs diff --git a/src/math.rs b/src/math.rs index 7483ff2d..989d31bd 100644 --- a/src/math.rs +++ b/src/math.rs @@ -208,6 +208,16 @@ impl Obb { Self::new(isometry * &self.isometry_inv.inverse(), self.half_extent) } + /// returns the axis aligned box that holds the obb in the coordinate frame + pub fn get_encasing_aabb(&self) -> Aabb3 { + let a_box: Aabb3 = Aabb3::new(self.corners[0], self.corners[1]); + // grow the box + for i in 2..8 { + a_box.grow(self.corners[i]); + } + a_box + } + fn precompute_corners(isometry: &Isometry3, half_extent: &Vector3) -> [Point3; 8] { let corner_from = |x: S, y: S, z: S| { isometry.rotation.rotate_point(Point3::new(x, y, z)) + isometry.translation diff --git a/src/octree/batch_iterator.rs b/src/octree/batch_iterator.rs index 31d4115a..7f04f003 100644 --- a/src/octree/batch_iterator.rs +++ b/src/octree/batch_iterator.rs @@ -164,78 +164,3 @@ impl<'a> BatchIterator<'a> { point_stream.callback() } } - -#[cfg(test)] -mod tests { - use super::*; - use crate::color::Color; - use crate::generation::build_octree; - use cgmath::Point3; - use tempdir::TempDir; - - fn build_test_octree(batch_size: usize) -> Box { - let default_point = Point { - position: Vector3::new(-2_699_182.0, -4_294_938.0, 3_853_373.0), //ECEF parking lot porter dr - color: Color { - red: 255, - green: 0, - blue: 0, - alpha: 255, - }, - intensity: None, - }; - - let mut points = vec![default_point; 4 * batch_size + 1]; - points[3 * batch_size].position = Vector3::new(-2_702_846.0, -4_291_151.0, 3_855_012.0); // ECEF STANFORD - - let p = Point3::new(6_400_000.0, 6_400_000.0, 6_400_000.0); - let bounding_box = Aabb3::new(-1.0 * p, p); - - let pool = scoped_pool::Pool::new(10); - let tmp_dir = TempDir::new("octree").unwrap(); - - build_octree(&pool, &tmp_dir, 1.0, bounding_box, points.into_iter()); - crate::octree::on_disk::octree_from_directory(tmp_dir.into_path()).unwrap() - } - - #[test] - //#[ignore] - fn test_batch_iterator() { - let batch_size = NUM_POINTS_PER_BATCH / 10; - // define function - let mut point_count: usize = 0; - let mut print_count: usize = 1; - let num_points = 25 * batch_size / 10; - println!("batch_size= {} , num_points= {}", batch_size, num_points); - let callback_func = |point_data: PointData| -> Result<()> { - point_count += point_data.position.len(); - if point_count >= print_count * 2 * batch_size { - print_count += 1; - println!("Streamed {} points", point_count); - } - if point_count >= num_points { - return Err(std::io::Error::new( - std::io::ErrorKind::Interrupted, - format!("Maximum number of {} points reached.", num_points), - ) - .into()); - } - Ok(()) - }; - - // octree and iterator - let octree = build_test_octree(batch_size); - let location = PointLocation { - culling: PointCulling::Any(), - global_from_local: None, - }; - let mut batch_iterator = BatchIterator::new(&octree, &location, batch_size); - - let _err_stop = batch_iterator - .try_for_each_batch(callback_func) - .expect_err("Test error"); - - assert_eq!(3 * batch_size, point_count); - assert_eq!(2, print_count); - } -} diff --git a/src/octree/mod.rs b/src/octree/mod.rs index 38909336..ee12a262 100644 --- a/src/octree/mod.rs +++ b/src/octree/mod.rs @@ -13,7 +13,7 @@ // limitations under the License. use crate::errors::*; -use crate::math::{Cube, Obb, OrientedBeam}; +use crate::math::{Cube, Isometry3, Obb, OrientedBeam}; use crate::proto; use crate::Point; use cgmath::{EuclideanSpace, Matrix4, Point3}; @@ -43,12 +43,15 @@ pub use self::factory::OctreeFactory; mod octree_iterator; pub use self::octree_iterator::{ - contains, intersecting_node_ids, AllPointsIterator, FilteredPointsIterator, + contains, intersecting_node_ids, AllPointsIterator, FilteredPointsIterator, NodeIdIterator, }; mod batch_iterator; pub use self::batch_iterator::{BatchIterator, PointCulling, PointLocation, NUM_POINTS_PER_BATCH}; +#[cfg(test)] +mod octree_test; + // Version 9 -> 10: Change in NodeId proto from level (u8) and index (u64) to high (u64) and low // (u64). We are able to convert the proto on read, so the tools can still read version 9. // Version 10 -> 11: Change in AxisAlignedCuboid proto from Vector3f min/max to Vector3d min/max. @@ -338,9 +341,57 @@ impl Octree { AllPointsIterator::new(&self) } + /// return the bounding box saved in meta pub fn bounding_box(&self) -> &Aabb3 { &self.meta.bounding_box } + + /// return the bounding box from the node that contains all tree points + /// aligned with the coodinate system of choice + pub fn get_node_bounding_box(&self, coordinate_system: &Isometry3) -> Option> { + // traversal and return the first non-empty node or node with at least two existing children + // (to care for disjointed subtrees) + let first_node_func = Box::new(move |node_id: &NodeId, octree: &Octree| -> bool { + // if has point return + match octree.nodes.get(&node_id) { + Some(node) => { + if node.num_points > 0 { + return true; + } + //corner case: if it has at least two children + let mut num_children = 0; + for child_index in 0..8 { + let child_id = node_id.get_child_id(ChildIndex::from_u8(child_index)); + if octree.nodes.contains_key(&child_id) { + num_children += 1; + } + + if num_children >= 2 { + return true; + } + } + false + } + None => false, + } + }); + + let mut node_id_iterator = NodeIdIterator::new(self, first_node_func); + let id: NodeId = match node_id_iterator.next() { + Some(id) => id, + None => { + return None; + } + }; + + let o_box = Obb::from( + id.find_bounding_cube(&Cube::bounding(&self.meta.bounding_box)) + .to_aabb3(), + ); + // convert to coordinate system + let rotated_o_box = o_box.transform(&coordinate_system); + Some(rotated_o_box.get_encasing_aabb()) + } } struct OpenNode { diff --git a/src/octree/octree_iterator.rs b/src/octree/octree_iterator.rs index 4f6d3978..72576655 100644 --- a/src/octree/octree_iterator.rs +++ b/src/octree/octree_iterator.rs @@ -130,3 +130,42 @@ pub fn intersecting_node_ids( } node_ids } + +pub struct NodeIdIterator<'a> { + octree: &'a Octree, + filter_func: Box bool + 'a>, + node_ids: VecDeque, +} + +impl<'a> NodeIdIterator<'a> { + pub fn new(octree: &'a Octree, filter_func: Box bool + 'a>) -> Self { + NodeIdIterator { + octree, + node_ids: vec![NodeId::from_level_index(0, 0)].into(), + filter_func, + } + } +} + +impl<'a> Iterator for NodeIdIterator<'a> { + type Item = NodeId; + + fn next(&mut self) -> Option { + loop { + match self.node_ids.pop_front() { + Some(current) => { + for child_index in 0..8 { + let child_id = current.get_child_id(ChildIndex::from_u8(child_index)); + if self.octree.nodes.contains_key(&child_id) { + self.node_ids.push_back(child_id); + } + } + if (self.filter_func)(¤t, &self.octree) { + return Some(current); + } + } + None => return None, + } + } + } +} diff --git a/src/octree/octree_test.rs b/src/octree/octree_test.rs new file mode 100644 index 00000000..93f4bbae --- /dev/null +++ b/src/octree/octree_test.rs @@ -0,0 +1,89 @@ +#[cfg(test)] +mod tests { + use crate::color::Color; + use crate::errors::Result; + use crate::generation::build_octree; + use crate::math::Isometry3; + use crate::octree::{ + self, BatchIterator, NodeIdIterator, PointCulling, PointLocation, NUM_POINTS_PER_BATCH, + }; + use crate::{Point, PointData}; + use cgmath::{Point3, Matrix3, Quaternion, Vector3}; + use collision::Aabb3; + use tempdir::TempDir; + + fn build_test_octree(batch_size: usize) -> Box { + let default_point = Point { + position: Vector3::new(-2_699_182.0, -4_294_938.0, 3_853_373.0), //ECEF parking lot porter dr + color: Color { + red: 255, + green: 0, + blue: 0, + alpha: 255, + }, + intensity: None, + }; + + let mut points = vec![default_point; 4 * batch_size + 1]; + points[3 * batch_size].position = Vector3::new(-2_702_846.0, -4_291_151.0, 3_855_012.0); // ECEF STANFORD + + let p = Point3::new(6_400_000.0, 6_400_000.0, 6_400_000.0); + let bounding_box = Aabb3::new(-1.0 * p, p); + + let pool = scoped_pool::Pool::new(10); + let tmp_dir = TempDir::new("octree").unwrap(); + + build_octree(&pool, &tmp_dir, 1.0, bounding_box, points.into_iter()); + crate::octree::on_disk::octree_from_directory(tmp_dir.into_path()).unwrap() + } + + #[test] + //#[ignore] + fn test_batch_iterator() { + let batch_size = NUM_POINTS_PER_BATCH / 10; + // define function + let mut point_count: usize = 0; + let mut print_count: usize = 1; + let num_points = 25 * batch_size / 10; + println!("batch_size= {} , num_points= {}", batch_size, num_points); + let callback_func = |point_data: PointData| -> Result<()> { + point_count += point_data.position.len(); + if point_count >= print_count * 2 * batch_size { + print_count += 1; + println!("Streamed {} points", point_count); + } + if point_count >= num_points { + return Err(std::io::Error::new( + std::io::ErrorKind::Interrupted, + format!("Maximum number of {} points reached.", num_points), + ) + .into()); + } + Ok(()) + }; + + // octree and iterator + let octree = build_test_octree(batch_size); + let location = PointLocation { + culling: PointCulling::Any(), + global_from_local: None, + }; + let mut batch_iterator = BatchIterator::new(&octree, &location, batch_size); + + let _err_stop = batch_iterator + .try_for_each_batch(callback_func) + .expect_err("Test error"); + + assert_eq!(3 * batch_size, point_count); + assert_eq!(2, print_count); + } + + #[test] + fn test_bounding_box() { + let octree = build_test_octree(300_000); + let y_with_z = Isometry3{rotation: Quaternion::from_rotation(Matrix3::from_cols(Vector3::unit_x(), Vector3::unit_z() , Vector3::unit_y())), translation: Vector3::new(0.0,0.0,0.0)}; + let aabb = octree.get_node_bounding_box(&y_with_z); + let no_rot = Isometry3{rotation: Quaternion::one(), translation: Vector3::(0.0, 0.0, 0.0)}; + let aabb_no_change = octree.get_node_bounding_box(&no_rot); + } +} From 33d773b733ec473f16aceb80aa37132488a7cb52 Mon Sep 17 00:00:00 2001 From: catevita Date: Mon, 20 May 2019 09:22:57 +0200 Subject: [PATCH 02/14] compiling test --- src/octree/octree_test.rs | 30 +++++++++++++++++++++--------- 1 file changed, 21 insertions(+), 9 deletions(-) diff --git a/src/octree/octree_test.rs b/src/octree/octree_test.rs index 93f4bbae..5bf7bef8 100644 --- a/src/octree/octree_test.rs +++ b/src/octree/octree_test.rs @@ -4,11 +4,9 @@ mod tests { use crate::errors::Result; use crate::generation::build_octree; use crate::math::Isometry3; - use crate::octree::{ - self, BatchIterator, NodeIdIterator, PointCulling, PointLocation, NUM_POINTS_PER_BATCH, - }; + use crate::octree::{self, BatchIterator, PointCulling, PointLocation, NUM_POINTS_PER_BATCH}; use crate::{Point, PointData}; - use cgmath::{Point3, Matrix3, Quaternion, Vector3}; + use cgmath::{Matrix3, Point3, Quaternion, Vector3, Zero}; use collision::Aabb3; use tempdir::TempDir; @@ -38,7 +36,7 @@ mod tests { } #[test] - //#[ignore] + #[ignore] fn test_batch_iterator() { let batch_size = NUM_POINTS_PER_BATCH / 10; // define function @@ -79,11 +77,25 @@ mod tests { } #[test] + #[ignore] fn test_bounding_box() { let octree = build_test_octree(300_000); - let y_with_z = Isometry3{rotation: Quaternion::from_rotation(Matrix3::from_cols(Vector3::unit_x(), Vector3::unit_z() , Vector3::unit_y())), translation: Vector3::new(0.0,0.0,0.0)}; - let aabb = octree.get_node_bounding_box(&y_with_z); - let no_rot = Isometry3{rotation: Quaternion::one(), translation: Vector3::(0.0, 0.0, 0.0)}; - let aabb_no_change = octree.get_node_bounding_box(&no_rot); + let y_with_z = Isometry3 { + rotation: Quaternion::from(Matrix3::from_cols( + Vector3::unit_x(), + Vector3::unit_z(), + Vector3::unit_y(), + )), + translation: Vector3::new(0.0, 0.0, 0.0), + }; + let _aabb = octree.get_node_bounding_box(&y_with_z); + let no_rot = Isometry3 { + rotation: Quaternion::zero(), + translation: Vector3::new(0.0, 0.0, 0.0), + }; + println!("rotation y with z {:?}", _aabb); + let _aabb_no_change = octree.get_node_bounding_box(&no_rot); + + println!("no rotation {:?}", _aabb_no_change); } } From 2cc60719bba8ee304306f0fbabdc123a118e8c1f Mon Sep 17 00:00:00 2001 From: catevita Date: Mon, 20 May 2019 13:30:39 +0200 Subject: [PATCH 03/14] test --- src/octree/mod.rs | 9 ++++----- src/octree/octree_test.rs | 14 +++++++------- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/src/octree/mod.rs b/src/octree/mod.rs index ee12a262..3cd470a7 100644 --- a/src/octree/mod.rs +++ b/src/octree/mod.rs @@ -355,7 +355,8 @@ impl Octree { // if has point return match octree.nodes.get(&node_id) { Some(node) => { - if node.num_points > 0 { + // corner case: root node has one point + if !(node_id.index() == 0 && node.num_points == 1) && node.num_points > 0 { return true; } //corner case: if it has at least two children @@ -384,10 +385,8 @@ impl Octree { } }; - let o_box = Obb::from( - id.find_bounding_cube(&Cube::bounding(&self.meta.bounding_box)) - .to_aabb3(), - ); + let current = self.nodes.get(&id).unwrap(); + let o_box: Obb = Obb::from(current.bounding_cube.to_aabb3()); // convert to coordinate system let rotated_o_box = o_box.transform(&coordinate_system); Some(rotated_o_box.get_encasing_aabb()) diff --git a/src/octree/octree_test.rs b/src/octree/octree_test.rs index 5bf7bef8..38eb533b 100644 --- a/src/octree/octree_test.rs +++ b/src/octree/octree_test.rs @@ -22,7 +22,7 @@ mod tests { intensity: None, }; - let mut points = vec![default_point; 4 * batch_size + 1]; + let mut points = vec![default_point; 4 * batch_size]; points[3 * batch_size].position = Vector3::new(-2_702_846.0, -4_291_151.0, 3_855_012.0); // ECEF STANFORD let p = Point3::new(6_400_000.0, 6_400_000.0, 6_400_000.0); @@ -77,9 +77,9 @@ mod tests { } #[test] - #[ignore] + //#[ignore] fn test_bounding_box() { - let octree = build_test_octree(300_000); + let octree = build_test_octree(100_000); let y_with_z = Isometry3 { rotation: Quaternion::from(Matrix3::from_cols( Vector3::unit_x(), @@ -88,14 +88,14 @@ mod tests { )), translation: Vector3::new(0.0, 0.0, 0.0), }; - let _aabb = octree.get_node_bounding_box(&y_with_z); + let aabb = octree.get_node_bounding_box(&y_with_z); let no_rot = Isometry3 { rotation: Quaternion::zero(), translation: Vector3::new(0.0, 0.0, 0.0), }; - println!("rotation y with z {:?}", _aabb); - let _aabb_no_change = octree.get_node_bounding_box(&no_rot); + println!("rotation y with z {:?}", aabb); + let aabb_no_change = octree.get_node_bounding_box(&no_rot); - println!("no rotation {:?}", _aabb_no_change); + println!("no rotation {:?}", aabb_no_change); } } From 3560f86163b4dafc1fd8e3cddab92643c1b902da Mon Sep 17 00:00:00 2001 From: catevita Date: Tue, 21 May 2019 07:28:54 +0200 Subject: [PATCH 04/14] compiles --- src/octree/mod.rs | 34 +++++++++++++++++++++++++--------- src/octree/octree_iterator.rs | 2 +- 2 files changed, 26 insertions(+), 10 deletions(-) diff --git a/src/octree/mod.rs b/src/octree/mod.rs index 3cd470a7..d0e1b0bd 100644 --- a/src/octree/mod.rs +++ b/src/octree/mod.rs @@ -14,6 +14,7 @@ use crate::errors::*; use crate::math::{Cube, Isometry3, Obb, OrientedBeam}; +use crate::octree::octree_iterator::get_node_iterator; use crate::proto; use crate::Point; use cgmath::{EuclideanSpace, Matrix4, Point3}; @@ -355,8 +356,7 @@ impl Octree { // if has point return match octree.nodes.get(&node_id) { Some(node) => { - // corner case: root node has one point - if !(node_id.index() == 0 && node.num_points == 1) && node.num_points > 0 { + if node.num_points > 0 { return true; } //corner case: if it has at least two children @@ -378,15 +378,31 @@ impl Octree { }); let mut node_id_iterator = NodeIdIterator::new(self, first_node_func); - let id: NodeId = match node_id_iterator.next() { - Some(id) => id, - None => { - return None; + let mut aabb: Aabb3 = Aabb3::zero(); + 'outer: loop { + let id: NodeId = match node_id_iterator.next() { + Some(id) => id, + None => { + return None; + } + }; + + let current = self.nodes.get(&id).unwrap(); + // grow and continue if only one point + if current.num_points == 1 { + let mut node_iterator = get_node_iterator(self, &id); + let sparse_point = node_iterator.next().unwrap(); + aabb = aabb.grow(Point3::from_vec(sparse_point.position)); + } else { + let aabb_current_points = current.bounding_cube.to_aabb3().to_corners(); + for corner in 0..8 { + aabb = aabb.grow(aabb_current_points[corner]); + break 'outer; + } } - }; + } - let current = self.nodes.get(&id).unwrap(); - let o_box: Obb = Obb::from(current.bounding_cube.to_aabb3()); + let o_box: Obb = Obb::from(aabb); // convert to coordinate system let rotated_o_box = o_box.transform(&coordinate_system); Some(rotated_o_box.get_encasing_aabb()) diff --git a/src/octree/octree_iterator.rs b/src/octree/octree_iterator.rs index 72576655..ebae75b7 100644 --- a/src/octree/octree_iterator.rs +++ b/src/octree/octree_iterator.rs @@ -7,7 +7,7 @@ use collision::Aabb3; use std::collections::VecDeque; /// returns an Iterator over the points of the current node -fn get_node_iterator(octree: &Octree, node_id: &NodeId) -> NodeIterator { +pub fn get_node_iterator(octree: &Octree, node_id: &NodeId) -> NodeIterator { // TODO(sirver): This crashes on error. We should bubble up an error. NodeIterator::from_data_provider( &*octree.data_provider, From 81f9e164bde7f4b9db06c1e5579257ebca9ccf65 Mon Sep 17 00:00:00 2001 From: catevita Date: Tue, 21 May 2019 07:32:23 +0200 Subject: [PATCH 05/14] cargo clippy --- src/octree/mod.rs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/octree/mod.rs b/src/octree/mod.rs index d0e1b0bd..3d03344c 100644 --- a/src/octree/mod.rs +++ b/src/octree/mod.rs @@ -387,7 +387,7 @@ impl Octree { } }; - let current = self.nodes.get(&id).unwrap(); + let current = &self.nodes[&id]; // grow and continue if only one point if current.num_points == 1 { let mut node_iterator = get_node_iterator(self, &id); @@ -395,10 +395,10 @@ impl Octree { aabb = aabb.grow(Point3::from_vec(sparse_point.position)); } else { let aabb_current_points = current.bounding_cube.to_aabb3().to_corners(); - for corner in 0..8 { - aabb = aabb.grow(aabb_current_points[corner]); - break 'outer; + for corner in &aabb_current_points { + aabb = aabb.grow(*corner); } + break 'outer; } } From 6f3d7d2db4cbed031bb245c332bb6700332f2a72 Mon Sep 17 00:00:00 2001 From: catevita Date: Tue, 21 May 2019 07:58:37 +0200 Subject: [PATCH 06/14] fixed aabb with zero --- src/octree/mod.rs | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/src/octree/mod.rs b/src/octree/mod.rs index 3d03344c..62bef074 100644 --- a/src/octree/mod.rs +++ b/src/octree/mod.rs @@ -379,6 +379,7 @@ impl Octree { let mut node_id_iterator = NodeIdIterator::new(self, first_node_func); let mut aabb: Aabb3 = Aabb3::zero(); + let mut is_initialized = false; 'outer: loop { let id: NodeId = match node_id_iterator.next() { Some(id) => id, @@ -392,11 +393,21 @@ impl Octree { if current.num_points == 1 { let mut node_iterator = get_node_iterator(self, &id); let sparse_point = node_iterator.next().unwrap(); - aabb = aabb.grow(Point3::from_vec(sparse_point.position)); + let geo_point = Point3::from_vec(sparse_point.position); + if !is_initialized { + aabb = Aabb3::new(geo_point, geo_point); + is_initialized = true; + } else { + aabb = aabb.grow(Point3::from_vec(sparse_point.position)); + } } else { - let aabb_current_points = current.bounding_cube.to_aabb3().to_corners(); - for corner in &aabb_current_points { - aabb = aabb.grow(*corner); + let current_id_aabb = current.bounding_cube.to_aabb3(); + if !is_initialized { + aabb = current_id_aabb; + } else { + for corner in ¤t_id_aabb.to_corners() { + aabb = aabb.grow(*corner); + } } break 'outer; } From 2cb21a08a5012612f5058113c9af9eb3cd19e35a Mon Sep 17 00:00:00 2001 From: catevita Date: Tue, 21 May 2019 08:08:27 +0200 Subject: [PATCH 07/14] added ignore to test --- src/octree/octree_test.rs | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/octree/octree_test.rs b/src/octree/octree_test.rs index 38eb533b..c877b9bb 100644 --- a/src/octree/octree_test.rs +++ b/src/octree/octree_test.rs @@ -77,7 +77,7 @@ mod tests { } #[test] - //#[ignore] + #[ignore] fn test_bounding_box() { let octree = build_test_octree(100_000); let y_with_z = Isometry3 { @@ -89,11 +89,12 @@ mod tests { translation: Vector3::new(0.0, 0.0, 0.0), }; let aabb = octree.get_node_bounding_box(&y_with_z); + + println!("rotation y with z {:?}", aabb); let no_rot = Isometry3 { rotation: Quaternion::zero(), translation: Vector3::new(0.0, 0.0, 0.0), }; - println!("rotation y with z {:?}", aabb); let aabb_no_change = octree.get_node_bounding_box(&no_rot); println!("no rotation {:?}", aabb_no_change); From 36f5593d029ec92804b33f8f6d8cd2d775738bd3 Mon Sep 17 00:00:00 2001 From: catevita Date: Tue, 21 May 2019 08:19:42 +0200 Subject: [PATCH 08/14] nits --- src/octree/mod.rs | 2 +- src/octree/octree_test.rs | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/octree/mod.rs b/src/octree/mod.rs index 62bef074..efd2682c 100644 --- a/src/octree/mod.rs +++ b/src/octree/mod.rs @@ -398,7 +398,7 @@ impl Octree { aabb = Aabb3::new(geo_point, geo_point); is_initialized = true; } else { - aabb = aabb.grow(Point3::from_vec(sparse_point.position)); + aabb = aabb.grow(geo_point); } } else { let current_id_aabb = current.bounding_cube.to_aabb3(); diff --git a/src/octree/octree_test.rs b/src/octree/octree_test.rs index c877b9bb..a8e10de9 100644 --- a/src/octree/octree_test.rs +++ b/src/octree/octree_test.rs @@ -77,7 +77,7 @@ mod tests { } #[test] - #[ignore] + //#[ignore] fn test_bounding_box() { let octree = build_test_octree(100_000); let y_with_z = Isometry3 { @@ -93,7 +93,7 @@ mod tests { println!("rotation y with z {:?}", aabb); let no_rot = Isometry3 { rotation: Quaternion::zero(), - translation: Vector3::new(0.0, 0.0, 0.0), + translation: Vector3::new(100_000.0, 0.0, 0.0), }; let aabb_no_change = octree.get_node_bounding_box(&no_rot); From 273d0433f622a5f92811b322d006c258a14e6b16 Mon Sep 17 00:00:00 2001 From: catevita Date: Tue, 21 May 2019 08:57:29 +0200 Subject: [PATCH 09/14] removed aabb code, removed full tree traversal --- src/math.rs | 10 ----- src/octree/mod.rs | 76 +---------------------------------- src/octree/octree_iterator.rs | 12 +++--- src/octree/octree_test.rs | 23 ----------- 4 files changed, 7 insertions(+), 114 deletions(-) diff --git a/src/math.rs b/src/math.rs index 989d31bd..7483ff2d 100644 --- a/src/math.rs +++ b/src/math.rs @@ -208,16 +208,6 @@ impl Obb { Self::new(isometry * &self.isometry_inv.inverse(), self.half_extent) } - /// returns the axis aligned box that holds the obb in the coordinate frame - pub fn get_encasing_aabb(&self) -> Aabb3 { - let a_box: Aabb3 = Aabb3::new(self.corners[0], self.corners[1]); - // grow the box - for i in 2..8 { - a_box.grow(self.corners[i]); - } - a_box - } - fn precompute_corners(isometry: &Isometry3, half_extent: &Vector3) -> [Point3; 8] { let corner_from = |x: S, y: S, z: S| { isometry.rotation.rotate_point(Point3::new(x, y, z)) + isometry.translation diff --git a/src/octree/mod.rs b/src/octree/mod.rs index efd2682c..7311a2f5 100644 --- a/src/octree/mod.rs +++ b/src/octree/mod.rs @@ -13,8 +13,7 @@ // limitations under the License. use crate::errors::*; -use crate::math::{Cube, Isometry3, Obb, OrientedBeam}; -use crate::octree::octree_iterator::get_node_iterator; +use crate::math::{Cube, Obb, OrientedBeam}; use crate::proto; use crate::Point; use cgmath::{EuclideanSpace, Matrix4, Point3}; @@ -346,80 +345,7 @@ impl Octree { pub fn bounding_box(&self) -> &Aabb3 { &self.meta.bounding_box } - - /// return the bounding box from the node that contains all tree points - /// aligned with the coodinate system of choice - pub fn get_node_bounding_box(&self, coordinate_system: &Isometry3) -> Option> { - // traversal and return the first non-empty node or node with at least two existing children - // (to care for disjointed subtrees) - let first_node_func = Box::new(move |node_id: &NodeId, octree: &Octree| -> bool { - // if has point return - match octree.nodes.get(&node_id) { - Some(node) => { - if node.num_points > 0 { - return true; - } - //corner case: if it has at least two children - let mut num_children = 0; - for child_index in 0..8 { - let child_id = node_id.get_child_id(ChildIndex::from_u8(child_index)); - if octree.nodes.contains_key(&child_id) { - num_children += 1; - } - - if num_children >= 2 { - return true; - } - } - false - } - None => false, - } - }); - - let mut node_id_iterator = NodeIdIterator::new(self, first_node_func); - let mut aabb: Aabb3 = Aabb3::zero(); - let mut is_initialized = false; - 'outer: loop { - let id: NodeId = match node_id_iterator.next() { - Some(id) => id, - None => { - return None; - } - }; - - let current = &self.nodes[&id]; - // grow and continue if only one point - if current.num_points == 1 { - let mut node_iterator = get_node_iterator(self, &id); - let sparse_point = node_iterator.next().unwrap(); - let geo_point = Point3::from_vec(sparse_point.position); - if !is_initialized { - aabb = Aabb3::new(geo_point, geo_point); - is_initialized = true; - } else { - aabb = aabb.grow(geo_point); - } - } else { - let current_id_aabb = current.bounding_cube.to_aabb3(); - if !is_initialized { - aabb = current_id_aabb; - } else { - for corner in ¤t_id_aabb.to_corners() { - aabb = aabb.grow(*corner); - } - } - break 'outer; - } - } - - let o_box: Obb = Obb::from(aabb); - // convert to coordinate system - let rotated_o_box = o_box.transform(&coordinate_system); - Some(rotated_o_box.get_encasing_aabb()) - } } - struct OpenNode { node: Node, relation: Relation, diff --git a/src/octree/octree_iterator.rs b/src/octree/octree_iterator.rs index ebae75b7..509ae57f 100644 --- a/src/octree/octree_iterator.rs +++ b/src/octree/octree_iterator.rs @@ -154,13 +154,13 @@ impl<'a> Iterator for NodeIdIterator<'a> { loop { match self.node_ids.pop_front() { Some(current) => { - for child_index in 0..8 { - let child_id = current.get_child_id(ChildIndex::from_u8(child_index)); - if self.octree.nodes.contains_key(&child_id) { - self.node_ids.push_back(child_id); - } - } if (self.filter_func)(¤t, &self.octree) { + for child_index in 0..8 { + let child_id = current.get_child_id(ChildIndex::from_u8(child_index)); + if self.octree.nodes.contains_key(&child_id) { + self.node_ids.push_back(child_id); + } + } return Some(current); } } diff --git a/src/octree/octree_test.rs b/src/octree/octree_test.rs index a8e10de9..f52ed3c8 100644 --- a/src/octree/octree_test.rs +++ b/src/octree/octree_test.rs @@ -76,27 +76,4 @@ mod tests { assert_eq!(2, print_count); } - #[test] - //#[ignore] - fn test_bounding_box() { - let octree = build_test_octree(100_000); - let y_with_z = Isometry3 { - rotation: Quaternion::from(Matrix3::from_cols( - Vector3::unit_x(), - Vector3::unit_z(), - Vector3::unit_y(), - )), - translation: Vector3::new(0.0, 0.0, 0.0), - }; - let aabb = octree.get_node_bounding_box(&y_with_z); - - println!("rotation y with z {:?}", aabb); - let no_rot = Isometry3 { - rotation: Quaternion::zero(), - translation: Vector3::new(100_000.0, 0.0, 0.0), - }; - let aabb_no_change = octree.get_node_bounding_box(&no_rot); - - println!("no rotation {:?}", aabb_no_change); - } } From cff2a9878be580e883fe4e4a4246db6400be664e Mon Sep 17 00:00:00 2001 From: catevita Date: Fri, 31 May 2019 15:16:15 +0200 Subject: [PATCH 10/14] fixing for pr comments, cleanup --- src/octree/octree_iterator.rs | 2 +- src/octree/octree_test.rs | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/octree/octree_iterator.rs b/src/octree/octree_iterator.rs index 509ae57f..087e0536 100644 --- a/src/octree/octree_iterator.rs +++ b/src/octree/octree_iterator.rs @@ -7,7 +7,7 @@ use collision::Aabb3; use std::collections::VecDeque; /// returns an Iterator over the points of the current node -pub fn get_node_iterator(octree: &Octree, node_id: &NodeId) -> NodeIterator { +fn get_node_iterator(octree: &Octree, node_id: &NodeId) -> NodeIterator { // TODO(sirver): This crashes on error. We should bubble up an error. NodeIterator::from_data_provider( &*octree.data_provider, diff --git a/src/octree/octree_test.rs b/src/octree/octree_test.rs index f52ed3c8..1540e844 100644 --- a/src/octree/octree_test.rs +++ b/src/octree/octree_test.rs @@ -3,10 +3,9 @@ mod tests { use crate::color::Color; use crate::errors::Result; use crate::generation::build_octree; - use crate::math::Isometry3; use crate::octree::{self, BatchIterator, PointCulling, PointLocation, NUM_POINTS_PER_BATCH}; use crate::{Point, PointData}; - use cgmath::{Matrix3, Point3, Quaternion, Vector3, Zero}; + use cgmath::{Point3, Vector3}; use collision::Aabb3; use tempdir::TempDir; From 5211654e1974fe249b2130139f6a9e0fea1b5646 Mon Sep 17 00:00:00 2001 From: catevita Date: Wed, 5 Jun 2019 14:46:14 +0200 Subject: [PATCH 11/14] changes for PR review --- src/octree/mod.rs | 2 +- src/octree/octree_iterator.rs | 28 ++++++++++++---------------- src/octree/octree_test.rs | 2 +- 3 files changed, 14 insertions(+), 18 deletions(-) diff --git a/src/octree/mod.rs b/src/octree/mod.rs index 7311a2f5..13763793 100644 --- a/src/octree/mod.rs +++ b/src/octree/mod.rs @@ -43,7 +43,7 @@ pub use self::factory::OctreeFactory; mod octree_iterator; pub use self::octree_iterator::{ - contains, intersecting_node_ids, AllPointsIterator, FilteredPointsIterator, NodeIdIterator, + contains, intersecting_node_ids, AllPointsIterator, FilteredPointsIterator, NodeIdsIterator, }; mod batch_iterator; diff --git a/src/octree/octree_iterator.rs b/src/octree/octree_iterator.rs index 087e0536..d0aa9dd3 100644 --- a/src/octree/octree_iterator.rs +++ b/src/octree/octree_iterator.rs @@ -131,15 +131,15 @@ pub fn intersecting_node_ids( node_ids } -pub struct NodeIdIterator<'a> { +pub struct NodeIdsIterator<'a> { octree: &'a Octree, filter_func: Box bool + 'a>, node_ids: VecDeque, } -impl<'a> NodeIdIterator<'a> { +impl<'a> NodeIdsIterator<'a> { pub fn new(octree: &'a Octree, filter_func: Box bool + 'a>) -> Self { - NodeIdIterator { + NodeIdsIterator { octree, node_ids: vec![NodeId::from_level_index(0, 0)].into(), filter_func, @@ -147,25 +147,21 @@ impl<'a> NodeIdIterator<'a> { } } -impl<'a> Iterator for NodeIdIterator<'a> { +impl<'a> Iterator for NodeIdsIterator<'a> { type Item = NodeId; fn next(&mut self) -> Option { - loop { - match self.node_ids.pop_front() { - Some(current) => { - if (self.filter_func)(¤t, &self.octree) { - for child_index in 0..8 { - let child_id = current.get_child_id(ChildIndex::from_u8(child_index)); - if self.octree.nodes.contains_key(&child_id) { - self.node_ids.push_back(child_id); - } - } - return Some(current); + while let Some(current) = self.node_ids.pop_front() { + if (self.filter_func)(¤t, &self.octree) { + for child_index in 0..8 { + let child_id = current.get_child_id(ChildIndex::from_u8(child_index)); + if self.octree.nodes.contains_key(&child_id) { + self.node_ids.push_back(child_id); } } - None => return None, + return Some(current); } } + None } } diff --git a/src/octree/octree_test.rs b/src/octree/octree_test.rs index 1540e844..076a88e6 100644 --- a/src/octree/octree_test.rs +++ b/src/octree/octree_test.rs @@ -35,7 +35,7 @@ mod tests { } #[test] - #[ignore] + //#[ignore] fn test_batch_iterator() { let batch_size = NUM_POINTS_PER_BATCH / 10; // define function From a8f5bf910a05dfc83bbd3bb823c39a0d02d1db4f Mon Sep 17 00:00:00 2001 From: catevita Date: Wed, 5 Jun 2019 17:07:55 +0200 Subject: [PATCH 12/14] streamlined test fuctionalities --- src/octree/octree_test.rs | 109 +++++++++++++++++++++++++++++++------- 1 file changed, 89 insertions(+), 20 deletions(-) diff --git a/src/octree/octree_test.rs b/src/octree/octree_test.rs index 076a88e6..2243ce7e 100644 --- a/src/octree/octree_test.rs +++ b/src/octree/octree_test.rs @@ -3,13 +3,13 @@ mod tests { use crate::color::Color; use crate::errors::Result; use crate::generation::build_octree; - use crate::octree::{self, BatchIterator, PointCulling, PointLocation, NUM_POINTS_PER_BATCH}; + use crate::octree::{self, BatchIterator, PointCulling, PointLocation}; use crate::{Point, PointData}; - use cgmath::{Point3, Vector3}; - use collision::Aabb3; + use cgmath::{EuclideanSpace, Point3, Vector3}; + use collision::{Aabb, Aabb3}; use tempdir::TempDir; - fn build_test_octree(batch_size: usize) -> Box { + fn build_big_test_octree() -> Box { let default_point = Point { position: Vector3::new(-2_699_182.0, -4_294_938.0, 3_853_373.0), //ECEF parking lot porter dr color: Color { @@ -21,8 +21,8 @@ mod tests { intensity: None, }; - let mut points = vec![default_point; 4 * batch_size]; - points[3 * batch_size].position = Vector3::new(-2_702_846.0, -4_291_151.0, 3_855_012.0); // ECEF STANFORD + let mut points = vec![default_point; 200_000]; + points[3].position = Vector3::new(-2_702_846.0, -4_291_151.0, 3_855_012.0); // ECEF STANFORD let p = Point3::new(6_400_000.0, 6_400_000.0, 6_400_000.0); let bounding_box = Aabb3::new(-1.0 * p, p); @@ -34,25 +34,93 @@ mod tests { crate::octree::on_disk::octree_from_directory(tmp_dir.into_path()).unwrap() } + fn build_test_octree() -> Box { + let default_point = Point { + position: Vector3::new(0.0, 0.0, 0.0), + color: Color { + red: 255, + green: 0, + blue: 0, + alpha: 255, + }, + intensity: None, + }; + + let mut points = vec![default_point; 100_001]; + points[100_000].position = Vector3::new(-200., -40., 30.); + + let bounding_box = Aabb3::zero().grow(Point3::from_vec(points[100_000].position)); + + let pool = scoped_pool::Pool::new(10); + let tmp_dir = TempDir::new("octree").unwrap(); + + build_octree(&pool, &tmp_dir, 1.0, bounding_box, points.into_iter()); + crate::octree::on_disk::octree_from_directory(tmp_dir.into_path()).unwrap() + } + #[test] //#[ignore] fn test_batch_iterator() { - let batch_size = NUM_POINTS_PER_BATCH / 10; + let batch_size = 5000; // define function - let mut point_count: usize = 0; - let mut print_count: usize = 1; - let num_points = 25 * batch_size / 10; - println!("batch_size= {} , num_points= {}", batch_size, num_points); + let mut callback_count: usize = 0; + let max_num_points = 13_000; // 2*batch size + 3000 + let mut delivered_points = 0; + println!( + "batch_size= {} , num_points= {}", + batch_size, max_num_points + ); let callback_func = |point_data: PointData| -> Result<()> { - point_count += point_data.position.len(); - if point_count >= print_count * 2 * batch_size { - print_count += 1; - println!("Streamed {} points", point_count); + callback_count += 1; + delivered_points += point_data.position.len(); + + if delivered_points >= max_num_points { + return Err(std::io::Error::new( + std::io::ErrorKind::Interrupted, + format!("Maximum number of {} points reached.", max_num_points), + ) + .into()); } - if point_count >= num_points { + Ok(()) + }; + + // octree and iterator + let octree = build_test_octree(); + let location = PointLocation { + culling: PointCulling::Any(), + global_from_local: None, + }; + let mut batch_iterator = BatchIterator::new(&octree, &location, batch_size); + + let _err_stop = batch_iterator + .try_for_each_batch(callback_func) + .expect_err("Test error"); + + assert_eq!(3, callback_count); + assert_eq!(3 * batch_size, delivered_points); + assert!(delivered_points as i32 - max_num_points as i32 >= 0); + } + + #[test] + #[ignore] + fn test_batch_iterator_big_octree() { + let batch_size = 5000; + // define function + let mut callback_count: usize = 0; + let max_num_points = 13_000; // 2*batch size + 3000 + let mut delivered_points = 0; + println!( + "batch_size= {} , num_points= {}", + batch_size, max_num_points + ); + let callback_func = |point_data: PointData| -> Result<()> { + callback_count += 1; + delivered_points += point_data.position.len(); + + if delivered_points >= max_num_points { return Err(std::io::Error::new( std::io::ErrorKind::Interrupted, - format!("Maximum number of {} points reached.", num_points), + format!("Maximum number of {} points reached.", max_num_points), ) .into()); } @@ -60,7 +128,7 @@ mod tests { }; // octree and iterator - let octree = build_test_octree(batch_size); + let octree = build_big_test_octree(); let location = PointLocation { culling: PointCulling::Any(), global_from_local: None, @@ -71,8 +139,9 @@ mod tests { .try_for_each_batch(callback_func) .expect_err("Test error"); - assert_eq!(3 * batch_size, point_count); - assert_eq!(2, print_count); + assert_eq!(3, callback_count); + assert_eq!(3 * batch_size, delivered_points); + assert!(delivered_points as i32 - max_num_points as i32 >= 0); } } From 6e61f3cb7b29fd4f8d19a2662a658139ffa47ac4 Mon Sep 17 00:00:00 2001 From: catevita Date: Thu, 6 Jun 2019 10:42:38 +0200 Subject: [PATCH 13/14] removed commented line --- src/octree/octree_test.rs | 1 - 1 file changed, 1 deletion(-) diff --git a/src/octree/octree_test.rs b/src/octree/octree_test.rs index 2243ce7e..6e3d181f 100644 --- a/src/octree/octree_test.rs +++ b/src/octree/octree_test.rs @@ -59,7 +59,6 @@ mod tests { } #[test] - //#[ignore] fn test_batch_iterator() { let batch_size = 5000; // define function From e07bce0e36a1d66056dc3d7fe8dc51f8942ad920 Mon Sep 17 00:00:00 2001 From: catevita Date: Thu, 6 Jun 2019 10:57:56 +0200 Subject: [PATCH 14/14] embed the trait bound into the struct definition --- src/octree/octree_iterator.rs | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/src/octree/octree_iterator.rs b/src/octree/octree_iterator.rs index d0aa9dd3..0c6f3b6f 100644 --- a/src/octree/octree_iterator.rs +++ b/src/octree/octree_iterator.rs @@ -131,14 +131,20 @@ pub fn intersecting_node_ids( node_ids } -pub struct NodeIdsIterator<'a> { +pub struct NodeIdsIterator<'a, F> +where + F: Fn(&NodeId, &Octree) -> bool + 'a, +{ octree: &'a Octree, - filter_func: Box bool + 'a>, + filter_func: F, node_ids: VecDeque, } -impl<'a> NodeIdsIterator<'a> { - pub fn new(octree: &'a Octree, filter_func: Box bool + 'a>) -> Self { +impl<'a, F> NodeIdsIterator<'a, F> +where + F: Fn(&NodeId, &Octree) -> bool + 'a, +{ + pub fn new(octree: &'a Octree, filter_func: F) -> Self { NodeIdsIterator { octree, node_ids: vec![NodeId::from_level_index(0, 0)].into(), @@ -147,7 +153,10 @@ impl<'a> NodeIdsIterator<'a> { } } -impl<'a> Iterator for NodeIdsIterator<'a> { +impl<'a, F> Iterator for NodeIdsIterator<'a, F> +where + F: Fn(&NodeId, &Octree) -> bool + 'a, +{ type Item = NodeId; fn next(&mut self) -> Option {