Skip to content

Commit

Permalink
intro docs
Browse files Browse the repository at this point in the history
  • Loading branch information
@stelzo
stelzo committed May 9, 2024
1 parent 16e561d commit a09b2c3
Show file tree
Hide file tree
Showing 2 changed files with 135 additions and 35 deletions.
129 changes: 113 additions & 16 deletions benches/roundtrip.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@ use ros_pointcloud2::prelude::*;

use rand::Rng;

pub type PointXYZB = PointXYZINormal;

pub fn distance_to_origin(point: &PointXYZ) -> f32 {
((point.x.powi(2)) + (point.y.powi(2)) + (point.z.powi(2))).sqrt()
}
Expand Down Expand Up @@ -97,14 +99,15 @@ fn minus(point1: &PointXYZ, point2: &PointXYZ) -> PointXYZ {
}
}

pub fn generate_random_pointcloud(num_points: usize, min: f32, max: f32) -> Vec<PointXYZ> {
pub fn generate_random_pointcloud(num_points: usize, min: f32, max: f32) -> Vec<PointXYZB> {
let mut rng = rand::thread_rng();
let mut pointcloud = Vec::with_capacity(num_points);
for _ in 0..num_points {
let point = PointXYZ {
let point = PointXYZB {
x: rng.gen_range(min..max),
y: rng.gen_range(min..max),
z: rng.gen_range(min..max),
..Default::default()
};
pointcloud.push(point);
}
Expand Down Expand Up @@ -160,14 +163,14 @@ pub fn heavy_computing(point: &PointXYZ, iterations: u32) -> f32 {
}

#[cfg(feature = "derive")]
fn roundtrip_vec(cloud: Vec<PointXYZ>) -> bool {
fn roundtrip_vec(cloud: Vec<PointXYZB>) -> bool {
let orig_len = cloud.len();
let internal_msg = PointCloud2Msg::try_from_vec(cloud).unwrap();
let total: Vec<PointXYZ> = internal_msg.try_into_vec().unwrap();
orig_len == total.len()
}

fn roundtrip(cloud: Vec<PointXYZ>) -> bool {
fn roundtrip(cloud: Vec<PointXYZB>) -> bool {
let orig_len = cloud.len();
let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
let total = internal_msg
Expand All @@ -178,7 +181,7 @@ fn roundtrip(cloud: Vec<PointXYZ>) -> bool {
}

#[cfg(feature = "derive")]
fn roundtrip_filter_vec(cloud: Vec<PointXYZ>) -> bool {
fn roundtrip_filter_vec(cloud: Vec<PointXYZB>) -> bool {
let orig_len = cloud.len();
let internal_msg = PointCloud2Msg::try_from_vec(cloud).unwrap();
let total = internal_msg
Expand All @@ -193,7 +196,7 @@ fn roundtrip_filter_vec(cloud: Vec<PointXYZ>) -> bool {
orig_len == total.x as usize
}

fn roundtrip_filter(cloud: Vec<PointXYZ>) -> bool {
fn roundtrip_filter(cloud: Vec<PointXYZB>) -> bool {
let orig_len = cloud.len();
let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
let total = internal_msg
Expand All @@ -208,7 +211,7 @@ fn roundtrip_filter(cloud: Vec<PointXYZ>) -> bool {
orig_len == total.x as usize
}

fn roundtrip_computing(cloud: Vec<PointXYZ>) -> bool {
fn roundtrip_computing(cloud: Vec<PointXYZB>) -> bool {
let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
let total = internal_msg
.try_into_iter()
Expand All @@ -219,7 +222,7 @@ fn roundtrip_computing(cloud: Vec<PointXYZ>) -> bool {
}

#[cfg(feature = "rayon")]
fn roundtrip_computing_par(cloud: Vec<PointXYZ>) -> bool {
fn roundtrip_computing_par(cloud: Vec<PointXYZB>) -> bool {
let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
let total = internal_msg
.try_into_par_iter()
Expand All @@ -230,7 +233,7 @@ fn roundtrip_computing_par(cloud: Vec<PointXYZ>) -> bool {
}

#[cfg(feature = "derive")]
fn roundtrip_computing_par_par(cloud: Vec<PointXYZ>) -> bool {
fn roundtrip_computing_par_par(cloud: Vec<PointXYZB>) -> bool {
let internal_msg = PointCloud2Msg::try_from_par_iter(cloud.into_par_iter()).unwrap();
let total = internal_msg
.try_into_par_iter()
Expand All @@ -241,7 +244,7 @@ fn roundtrip_computing_par_par(cloud: Vec<PointXYZ>) -> bool {
}

#[cfg(feature = "derive")]
fn roundtrip_computing_vec(cloud: Vec<PointXYZ>) -> bool {
fn roundtrip_computing_vec(cloud: Vec<PointXYZB>) -> bool {
let internal_msg = PointCloud2Msg::try_from_vec(cloud).unwrap();
let total: f32 = internal_msg
.try_into_vec()
Expand All @@ -253,7 +256,7 @@ fn roundtrip_computing_vec(cloud: Vec<PointXYZ>) -> bool {
}

#[cfg(feature = "rayon")]
fn roundtrip_par(cloud: Vec<PointXYZ>) -> bool {
fn roundtrip_par(cloud: Vec<PointXYZB>) -> bool {
let orig_len = cloud.len();
let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
let total = internal_msg
Expand All @@ -264,7 +267,7 @@ fn roundtrip_par(cloud: Vec<PointXYZ>) -> bool {
}

#[cfg(feature = "rayon")]
fn roundtrip_par_par(cloud: Vec<PointXYZ>) -> bool {
fn roundtrip_par_par(cloud: Vec<PointXYZB>) -> bool {
let orig_len = cloud.len();
let internal_msg = PointCloud2Msg::try_from_par_iter(cloud.into_par_iter()).unwrap();
let total = internal_msg
Expand All @@ -275,7 +278,7 @@ fn roundtrip_par_par(cloud: Vec<PointXYZ>) -> bool {
}

#[cfg(feature = "rayon")]
fn roundtrip_filter_par(cloud: Vec<PointXYZ>) -> bool {
fn roundtrip_filter_par(cloud: Vec<PointXYZB>) -> bool {
let orig_len: usize = cloud.len();
let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
let total = internal_msg
Expand All @@ -291,7 +294,7 @@ fn roundtrip_filter_par(cloud: Vec<PointXYZ>) -> bool {
}

#[cfg(feature = "rayon")]
fn roundtrip_filter_par_par(cloud: Vec<PointXYZ>) -> bool {
fn roundtrip_filter_par_par(cloud: Vec<PointXYZB>) -> bool {
let orig_len: usize = cloud.len();
let internal_msg = PointCloud2Msg::try_from_par_iter(cloud.into_par_iter()).unwrap();
let total = internal_msg
Expand All @@ -307,12 +310,99 @@ fn roundtrip_filter_par_par(cloud: Vec<PointXYZ>) -> bool {
}

fn roundtrip_benchmark(c: &mut Criterion) {
let cloud_points_16k = generate_random_pointcloud(16_000, f32::MIN / 2.0, f32::MAX / 2.0);
let cloud_points_60k = generate_random_pointcloud(60_000, f32::MIN / 2.0, f32::MAX / 2.0);
let cloud_points_120k = generate_random_pointcloud(120_000, f32::MIN / 2.0, f32::MAX / 2.0);
let cloud_points_500k = generate_random_pointcloud(500_000, f32::MIN / 2.0, f32::MAX / 2.0);
let cloud_points_1_5m = generate_random_pointcloud(1_500_000, f32::MIN / 2.0, f32::MAX / 2.0);

// 60k points (Ouster OS with 64 beams)
// 16k points (Velodyne with 16 beams)

// Moving memory
c.bench_function("16k iter", |b| {
b.iter(|| {
black_box(roundtrip(cloud_points_16k.clone()));
})
});

#[cfg(feature = "rayon")]
c.bench_function("16k iter_par", |b| {
b.iter(|| {
black_box(roundtrip_par(cloud_points_16k.clone()));
})
});

#[cfg(feature = "rayon")]
c.bench_function("16k iter_par_par", |b| {
b.iter(|| {
black_box(roundtrip_par_par(cloud_points_16k.clone()));
})
});

#[cfg(feature = "derive")]
c.bench_function("16k vec", |b| {
b.iter(|| {
black_box(roundtrip_vec(cloud_points_16k.clone()));
})
});

// Simple distance filter
c.bench_function("16k iter_filter", |b| {
b.iter(|| {
roundtrip_filter(black_box(cloud_points_16k.clone()));
})
});

#[cfg(feature = "rayon")]
c.bench_function("16k filter_par", |b| {
b.iter(|| {
roundtrip_filter_par(black_box(cloud_points_16k.clone()));
})
});

#[cfg(feature = "rayon")]
c.bench_function("16k filter_par_par", |b| {
b.iter(|| {
black_box(roundtrip_filter_par_par(cloud_points_16k.clone()));
})
});

#[cfg(feature = "derive")]
c.bench_function("16k vec_filter", |b| {
b.iter(|| {
roundtrip_filter_vec(black_box(cloud_points_16k.clone()));
})
});

// Heavy computing
c.bench_function("16k iter_compute", |b| {
b.iter(|| {
roundtrip_computing(black_box(cloud_points_16k.clone()));
})
});

#[cfg(feature = "rayon")]
c.bench_function("16k iter_compute_par", |b| {
b.iter(|| {
roundtrip_computing_par(black_box(cloud_points_16k.clone()));
})
});

#[cfg(feature = "rayon")]
c.bench_function("16k iter_compute_par_par", |b| {
b.iter(|| {
roundtrip_computing_par_par(black_box(cloud_points_16k.clone()));
})
});

#[cfg(feature = "derive")]
c.bench_function("16k vec_compute", |b| {
b.iter(|| {
roundtrip_computing_vec(black_box(cloud_points_16k.clone()));
})
});

// 60k points (Ouster with 64 beams)

// Moving memory
c.bench_function("60k iter", |b| {
Expand All @@ -335,7 +425,14 @@ fn roundtrip_benchmark(c: &mut Criterion) {
})
});

// 120k points (Ouster OS with 128 beams)
#[cfg(feature = "derive")]
c.bench_function("60k vec", |b| {
b.iter(|| {
black_box(roundtrip_vec(cloud_points_60k.clone()));
})
});

// 120k points (Ouster with 128 beams)

// Moving memory
c.bench_function("120k iter", |b| {
Expand Down
41 changes: 22 additions & 19 deletions src/lib.rs
View file
Original file line number Diff line number Diff line change
@@ -1,36 +1,43 @@
//! A library to work with the PointCloud2 message type from ROS.
//!
//! ros_pointcloud2 mainly provides a [`ros_pointcloud2::PointCloud2Msg`] type that implements
//! functions for conversion to and from iterators.
//! Per default, ros_pointcloud2 provides a [`ros_pointcloud2::PointCloud2Msg`] type that implements conversions to and from iterators.
//! These are very flexible and faster than C++ PCL conversions for small to medium point clouds making them a good default for prototyping.
//! The meaning of small, medium and large depends on your device and the application.
//!
//! - [`ros_pointcloud2::PointCloud2Msg::try_from_iter`]
//! - [`ros_pointcloud2::PointCloud2Msg::try_into_iter`]
//!
//! For memory heavy applications, you can use the `derive` feature that allows you to copy
//! the data from and into a typed Vec. This increases compile times but is faster at runtime if you
//! only move the data around without any per-point processing.
//! For memory-heavy applications, you can use the `derive` feature that allows you to copy
//! the data from and into a typed Vec. When reading a Msg, your point struct must be an ordered subset of the type coming in at the Msg.
//! This info is not inside the source code, it is knowledge you have to provide to the library for gaining the speedup.
//! An example would be PointXYZINormal coming from some source as Msg and try_into_vec to Vec<PointXYZI>. They are the same up to the data you need.
//! Creating a Msg does not need to be ordered, since there is no conversion needed.
//! `derive` increases compile times but is faster for just moving data around without per-point processing.
//!
//! - [`ros_pointcloud2::PointCloud2Msg::try_from_vec`] (requires `derive` feature)
//! - [`ros_pointcloud2::PointCloud2Msg::try_into_vec`] (requires `derive` feature)
//!
//! For processing heavy applications, you can use the `rayon` feature to get a parallel iterator over the points.
//! For processing-heavy applications over point clouds, you may add the `rayon` feature.
//! It provides an implementation of Rayon's parallel iterator, which allows fully parallel processing of the point cloud.
//! Note that the `derive` feature is also required for creating a Msg from a parallel iterator.
//!
//! - [`ros_pointcloud2::PointCloud2Msg::try_into_par_iter`] (requires `rayon` feature)
//! - [`ros_pointcloud2::PointCloud2Msg::try_from_par_iter`] (requires `rayon` + `derive` feature)
//!
//! Note though, that writing in parallel to the same point cloud is not supported.
//! If you are not sure which feature makes sense for your application and device, you should try out the benchmark in the `benches` directory.
//!
//! For ROS interoperability, the message type generated by the respective crate must be converted to
//! the [`ros_pointcloud2::PointCloud2Msg`] using the `From` trait,
//! which mostly does ownership transfers without copying the point data.
//! the [`ros_pointcloud2::PointCloud2Msg`] using the `From` trait.
//! This mostly does ownership transfers and avoids copies of point data.
//!
//! There are implementations for the `r2r`, `rclrs` (ros2_rust) and `rosrust_msg` message types
//! There are implementations for the `r2r`, `rclrs` (ros2_rust) and `rosrust` message types
//! available with feature flags. If you miss a message type, please open an issue or a PR.
//! See the [`ros_pointcloud2::ros_types`] module for more information.
//!
//! Common point types like [`ros_pointcloud2::pcl_utils::PointXYZ`] or
//! [`ros_pointcloud2::pcl_utils::PointXYZI`] are provided in the
//! [`ros_pointcloud2::pcl_utils`] module. You can implement any custom point type
//! that can be described by the specification.
//! [`ros_pointcloud2::pcl_utils`] module. You can implement any additional custom point type.
//! See `examples/custom_enum_field_filter.rs` for an example.
//!
//! # Example PointXYZ
//! ```
Expand Down Expand Up @@ -258,11 +265,7 @@ impl PointCloud2Msg {
}
}

if offset == self.point_step {
Ok(true)
} else {
Err(MsgConversionError::DataLengthMismatch)
}
Ok(offset == self.point_step)
}

#[inline(always)]
Expand Down Expand Up @@ -347,7 +350,7 @@ impl PointCloud2Msg {
Ok(cloud)
}

#[cfg(all(feature = "rayon", not(feature = "derive")))]
/*#[cfg(all(feature = "rayon", not(feature = "derive")))]
pub fn try_from_par_iter<const N: usize, C>(
iterable: impl rayon::iter::ParallelIterator<Item = C>,
) -> Result<Self, MsgConversionError>
Expand All @@ -373,7 +376,7 @@ impl PointCloud2Msg {
cloud.row_step = cloud.width * cloud.point_step;
Ok(cloud)
}
}*/

#[cfg(all(feature = "rayon", feature = "derive"))]
pub fn try_from_par_iter<const N: usize, C>(
Expand Down

0 comments on commit a09b2c3

Please sign in to comment.