Skip to content

Commit

Permalink
opti with byte similar
Browse files Browse the repository at this point in the history
  • Loading branch information
@stelzo
stelzo committed May 8, 2024
1 parent 3948d68 commit 14eb60f
Show file tree
Hide file tree
Showing 5 changed files with 116 additions and 60 deletions.
46 changes: 31 additions & 15 deletions benches/roundtrip.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -186,9 +186,7 @@ fn roundtrip_filter_vec(cloud: Vec<PointXYZ>) -> bool {
let total = internal_msg
.try_into_iter()
.unwrap()
.filter(|point: &PointXYZ| {
(point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
})
.filter(|point: &PointXYZ| distance_to_origin(point) < 69.9)
.fold(PointXYZ::default(), |acc, point| PointXYZ {
x: acc.x + point.x,
y: acc.y + point.y,
Expand All @@ -203,9 +201,7 @@ fn roundtrip_filter(cloud: Vec<PointXYZ>) -> bool {
let total = internal_msg
.try_into_iter()
.unwrap()
.filter(|point: &PointXYZ| {
(point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
})
.filter(|point: &PointXYZ| distance_to_origin(point) < 69.9)
.fold(PointXYZ::default(), |acc, point| PointXYZ {
x: acc.x + point.x,
y: acc.y + point.y,
Expand Down Expand Up @@ -249,13 +245,13 @@ fn roundtrip_computing_par_par(cloud: Vec<PointXYZ>) -> bool {
#[cfg(feature = "derive")]
fn roundtrip_computing_vec(cloud: Vec<PointXYZ>) -> bool {
let internal_msg = PointCloud2Msg::try_from_vec(cloud).unwrap();
let total: Vec<f32> = internal_msg
let total: f32 = internal_msg
.try_into_vec()
.unwrap()
.into_iter()
.map(|point: PointXYZ| heavy_computing(&point, 100))
.collect();
total.iter().sum::<f32>() > 0.0
.sum();
total > 0.0
}

#[cfg(feature = "rayon")]
Expand Down Expand Up @@ -287,9 +283,7 @@ fn roundtrip_filter_par(cloud: Vec<PointXYZ>) -> bool {
let total = internal_msg
.try_into_par_iter()
.unwrap()
.filter(|point: &PointXYZ| {
(point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
})
.filter(|point: &PointXYZ| distance_to_origin(point) < 69.9)
.reduce(PointXYZ::default, |acc, point| PointXYZ {
x: acc.x + point.x,
y: acc.y + point.y,
Expand All @@ -305,9 +299,7 @@ fn roundtrip_filter_par_par(cloud: Vec<PointXYZ>) -> bool {
let total = internal_msg
.try_into_par_iter()
.unwrap()
.filter(|point: &PointXYZ| {
(point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
})
.filter(|point: &PointXYZ| distance_to_origin(point) < 69.9)
.reduce(PointXYZ::default, |acc, point| PointXYZ {
x: acc.x + point.x,
y: acc.y + point.y,
Expand All @@ -317,10 +309,34 @@ fn roundtrip_filter_par_par(cloud: Vec<PointXYZ>) -> bool {
}

fn roundtrip_benchmark(c: &mut Criterion) {
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)

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

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

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

// 120k points (Ouster OS with 128 beams)

// Moving memory
Expand Down
1 change: 0 additions & 1 deletion examples/custom_enum_field_filter.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,6 @@ impl From<u8> for Label {
}
}

#[cfg(not(feature = "derive"))]
impl CustomPoint {
fn new(x: f32, y: f32, z: f32, intensity: f32, my_custom_label: Label) -> Self {
Self {
Expand Down
87 changes: 57 additions & 30 deletions rpcl2_derive/src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ fn get_allowed_types() -> HashMap<&'static str, usize> {
}

/// Derive macro for the `Fields` trait.
///
///
/// Given the ordering from the source code of your struct, this macro will generate an array of field names.
#[proc_macro_derive(RosFields)]
pub fn ros_point_fields_derive(input: TokenStream) -> TokenStream {
Expand All @@ -29,28 +29,39 @@ pub fn ros_point_fields_derive(input: TokenStream) -> TokenStream {

let fields = match input.data {
syn::Data::Struct(ref data) => data.fields.clone(),
_ => return syn::Error::new_spanned(input, "Only structs are supported").to_compile_error().into(),
_ => {
return syn::Error::new_spanned(input, "Only structs are supported")
.to_compile_error()
.into()
}
};

let allowed_datatypes = get_allowed_types();

if fields.is_empty() {
return syn::Error::new_spanned(input, "No fields found").to_compile_error().into();
return syn::Error::new_spanned(input, "No fields found")
.to_compile_error()
.into();
}

for field in fields.iter() {
let ty = field.ty.to_token_stream().to_string();
if !allowed_datatypes.contains_key(&ty.as_str()) {
return syn::Error::new_spanned(field, "Field type not allowed").to_compile_error().into();
return syn::Error::new_spanned(field, "Field type not allowed")
.to_compile_error()
.into();
}
}

let field_len_token: usize = fields.len();

let field_names = fields.iter().map(|field| {
let field_name = field.ident.as_ref().unwrap();
quote! { stringify!(#field_name) }
}).collect::<Vec<_>>();

let field_names = fields
.iter()
.map(|field| {
let field_name = field.ident.as_ref().unwrap();
quote! { stringify!(#field_name) }
})
.collect::<Vec<_>>();

let field_impl = quote! {
impl Fields<#field_len_token> for #name {
Expand All @@ -77,28 +88,39 @@ pub fn ros_point_derive(input: TokenStream) -> TokenStream {

let fields = match input.data {
syn::Data::Struct(ref data) => data.fields.clone(),
_ => return syn::Error::new_spanned(input, "Only structs are supported").to_compile_error().into(),
_ => {
return syn::Error::new_spanned(input, "Only structs are supported")
.to_compile_error()
.into()
}
};

let allowed_datatypes = get_allowed_types();

if fields.is_empty() {
return syn::Error::new_spanned(input, "No fields found").to_compile_error().into();
return syn::Error::new_spanned(input, "No fields found")
.to_compile_error()
.into();
}

for field in fields.iter() {
let ty = field.ty.to_token_stream().to_string();
if !allowed_datatypes.contains_key(&ty.as_str()) {
return syn::Error::new_spanned(field, "Field type not allowed").to_compile_error().into();
return syn::Error::new_spanned(field, "Field type not allowed")
.to_compile_error()
.into();
}
}

let field_len_token: usize = fields.len();

let field_names = fields.iter().map(|field| {
let field_name = field.ident.as_ref().unwrap();
quote! { stringify!(#field_name) }
}).collect::<Vec<_>>();

let field_names = fields
.iter()
.map(|field| {
let field_name = field.ident.as_ref().unwrap();
quote! { stringify!(#field_name) }
})
.collect::<Vec<_>>();

let field_impl = quote! {
impl ros_pointcloud2::Fields<#field_len_token> for #name {
Expand All @@ -110,12 +132,16 @@ pub fn ros_point_derive(input: TokenStream) -> TokenStream {
}
};

let field_names_get = fields.iter().enumerate().map(|(idx, f)| {
let field_name = f.ident.as_ref().unwrap();
quote! { #field_name: point.fields[#idx].get() }
}).collect::<Vec<_>>();
let field_names_get = fields
.iter()
.enumerate()
.map(|(idx, f)| {
let field_name = f.ident.as_ref().unwrap();
quote! { #field_name: point[#idx].get() }
})
.collect::<Vec<_>>();

let from_my_point = quote! {
let from_my_point = quote! {
impl From<ros_pointcloud2::RPCL2Point<#field_len_token>> for #name {
fn from(point: ros_pointcloud2::RPCL2Point<#field_len_token>) -> Self {
Self {
Expand All @@ -125,17 +151,18 @@ pub fn ros_point_derive(input: TokenStream) -> TokenStream {
}
};

let field_names_into = fields.iter().map(|f| {
let field_name = f.ident.as_ref().unwrap();
quote! { point.#field_name.into() }
}).collect::<Vec<_>>();
let field_names_into = fields
.iter()
.map(|f| {
let field_name = f.ident.as_ref().unwrap();
quote! { point.#field_name.into() }
})
.collect::<Vec<_>>();

let from_custom_point = quote! {
impl From<#name> for ros_pointcloud2::RPCL2Point<#field_len_token> {
fn from(point: #name) -> Self {
ros_pointcloud2::RPCL2Point {
fields: [ #(#field_names_into,)* ]
}
[ #(#field_names_into,)* ].into()
}
}
};
Expand All @@ -152,4 +179,4 @@ pub fn ros_point_derive(input: TokenStream) -> TokenStream {
});

TokenStream::from(out)
}
}
36 changes: 25 additions & 11 deletions src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -227,26 +227,26 @@ impl PointCloud2Msg {

let mut offset: u32 = 0;
let layout = TypeLayoutInfo::try_from(C::type_layout())?;
for (f, msg_f) in layout.fields.into_iter().zip(self.fields.iter()) {
for (f, msg_f) in layout.fields.iter().zip(self.fields.iter()) {
match f {
PointField::Field {
name,
datatype,
size,
} => {
if msg_f.name != name {
if (*msg_f).name != *name {
return Err(MsgConversionError::FieldNotFound(vec![name.clone()]));
}

if msg_f.datatype != datatype {
if (*msg_f).datatype != *datatype {
return Err(MsgConversionError::InvalidFieldFormat);
}

if msg_f.offset != offset {
if (*msg_f).offset != offset {
return Err(MsgConversionError::DataLengthMismatch);
}

if msg_f.count != 1 {
if (*msg_f).count != 1 {
return Err(MsgConversionError::DataLengthMismatch);
}

Expand All @@ -258,7 +258,11 @@ impl PointCloud2Msg {
}
}

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

#[inline(always)]
Expand Down Expand Up @@ -415,17 +419,27 @@ impl PointCloud2Msg {
where
C: PointConvertible<N>,
{
self.assert_byte_similarity::<N, C>()?;
let same_size = self.assert_byte_similarity::<N, C>()?;

match system_endian() {
Endianness::Big => Ok(self.try_into_iter()?.collect()),
Endianness::Little => {
let mut vec = Vec::with_capacity(self.width as usize);
let raw_data: *const C = self.data.as_ptr() as *const C;
unsafe {
for i in 0..self.width {
let point = raw_data.add(i as usize).read();
vec.push(point);
if same_size {
unsafe {
std::ptr::copy_nonoverlapping(
raw_data as *const u8,
vec.as_mut_ptr() as *mut u8,
self.data.len(),
);
}
} else {
unsafe {
for i in 0..self.width {
let point = raw_data.add(i as usize).read();
vec.push(point);
}
}
}

Expand Down
6 changes: 3 additions & 3 deletions tests/e2e_test.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -392,9 +392,9 @@ fn write_less_than_available() {
impl From<RPCL2Point<3>> for CustomPoint {
fn from(point: RPCL2Point<3>) -> Self {
Self {
x: point.fields[0].get(),
y: point.fields[1].get(),
z: point.fields[2].get(),
x: point[0].get(),
y: point[1].get(),
z: point[2].get(),
dummy: 0.0,
}
}
Expand Down

0 comments on commit 14eb60f

Please sign in to comment.