diff --git a/cloudini_lib/CMakeLists.txt b/cloudini_lib/CMakeLists.txt index d9e6de8..a52fc82 100644 --- a/cloudini_lib/CMakeLists.txt +++ b/cloudini_lib/CMakeLists.txt @@ -134,6 +134,9 @@ target_compile_features(cloudini_lib PUBLIC cxx_std_20) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") target_compile_options(cloudini_lib PRIVATE -Wall -Wextra) + # The codec runs much faster with -O3 than with the -O2 of RelWithDebInfo + # (the default of ROS/colcon builds) or of distribution packages: + target_compile_options(cloudini_lib PRIVATE $<$>:-O3>) endif() target_include_directories(cloudini_lib diff --git a/cloudini_lib/include/cloudini_lib/field_decoder.hpp b/cloudini_lib/include/cloudini_lib/field_decoder.hpp index a91b8a9..34a8f9d 100644 --- a/cloudini_lib/include/cloudini_lib/field_decoder.hpp +++ b/cloudini_lib/include/cloudini_lib/field_decoder.hpp @@ -99,6 +99,41 @@ class FieldDecoderCopy : public FieldDecoder { input.trim_front(field_size_); } + // Same result as decode() once per point, as one copy when the values are contiguous (a single-field + // cloud), or a strided one with a compile-time size otherwise. + void decodePoints(ConstBufferView& input, uint8_t* output, size_t point_step, size_t count) override { + if (input.size() / field_size_ < count) { + throw std::runtime_error("Truncated encoded data: not enough bytes for a complete point"); + } + if (offset_ != kDecodeButSkipStore) { + const uint8_t* src = input.data(); + uint8_t* dst = output + offset_; + if (point_step == field_size_) { + memcpy(dst, src, count * field_size_); + } else { + switch (field_size_) { + case 1: + copyStrided<1>(src, dst, point_step, count); + break; + case 2: + copyStrided<2>(src, dst, point_step, count); + break; + case 4: + copyStrided<4>(src, dst, point_step, count); + break; + case 8: + copyStrided<8>(src, dst, point_step, count); + break; + default: + for (size_t i = 0; i < count; ++i) { + memcpy(dst + i * point_step, src + i * field_size_, field_size_); + } + } + } + } + input.trim_front(count * field_size_); + } + void reset() override {} size_t maxInputBytes() const override { @@ -113,6 +148,13 @@ class FieldDecoderCopy : public FieldDecoder { } private: + template + static void copyStrided(const uint8_t* src, uint8_t* dst, size_t point_step, size_t count) { + for (size_t i = 0; i < count; ++i) { + memcpy(dst + i * point_step, src + i * N, N); + } + } + size_t offset_ = 0; size_t field_size_ = 0; }; diff --git a/cloudini_lib/include/cloudini_lib/field_encoder.hpp b/cloudini_lib/include/cloudini_lib/field_encoder.hpp index 49c9094..54f4a38 100644 --- a/cloudini_lib/include/cloudini_lib/field_encoder.hpp +++ b/cloudini_lib/include/cloudini_lib/field_encoder.hpp @@ -39,6 +39,21 @@ class FieldEncoder { */ virtual size_t encode(const ConstBufferView& point_view, BufferView& output) = 0; + /** + * @brief Same as calling encode() for `count` consecutive points, which encoders can do faster. + * + * @param points Pointer to the first point. + * @param point_step Bytes from one point to the next. + * @return The number of bytes written to the output buffer. + */ + virtual size_t encodePoints(const uint8_t* points, size_t point_step, size_t count, BufferView& output) { + size_t written = 0; + for (size_t i = 0; i < count; ++i) { + written += encode(ConstBufferView(points + i * point_step, point_step), output); + } + return written; + } + virtual void reset() = 0; // Flush any remaining buffered data. Default implementation does nothing. @@ -59,9 +74,47 @@ class FieldEncoderCopy : public FieldEncoder { return field_size_; } + // One copy when the values are contiguous (a single-field cloud), a strided one with a compile-time + // size otherwise. + size_t encodePoints(const uint8_t* points, size_t point_step, size_t count, BufferView& output) override { + const uint8_t* src = points + offset_; + uint8_t* dst = output.data(); + if (point_step == field_size_) { + memcpy(dst, src, count * field_size_); + } else { + switch (field_size_) { + case 1: + copyStrided<1>(src, dst, point_step, count); + break; + case 2: + copyStrided<2>(src, dst, point_step, count); + break; + case 4: + copyStrided<4>(src, dst, point_step, count); + break; + case 8: + copyStrided<8>(src, dst, point_step, count); + break; + default: + for (size_t i = 0; i < count; ++i) { + memcpy(dst + i * field_size_, src + i * point_step, field_size_); + } + } + } + output.trim_front(count * field_size_); + return count * field_size_; + } + void reset() override {} private: + template + static void copyStrided(const uint8_t* src, uint8_t* dst, size_t point_step, size_t count) { + for (size_t i = 0; i < count; ++i) { + memcpy(dst + i * N, src + i * point_step, N); + } + } + size_t offset_; size_t field_size_; }; @@ -107,6 +160,15 @@ class FieldEncoderFloat_Lossy : public FieldEncoder { size_t encode(const ConstBufferView& point_view, BufferView& output) override; + // Same as encode() once per point, without a virtual call per point. + size_t encodePoints(const uint8_t* points, size_t point_step, size_t count, BufferView& output) override { + size_t written = 0; + for (size_t i = 0; i < count; ++i) { + written += FieldEncoderFloat_Lossy::encode(ConstBufferView(points + i * point_step, point_step), output); + } + return written; + } + void reset() override { prev_value_ = 0.0; } diff --git a/cloudini_lib/src/v4_codec.cpp b/cloudini_lib/src/v4_codec.cpp index 43d6803..174cf1a 100644 --- a/cloudini_lib/src/v4_codec.cpp +++ b/cloudini_lib/src/v4_codec.cpp @@ -69,6 +69,13 @@ size_t EncodeV4Stage1Chunk( size_t points_in_current_chunk = 0; size_t serialized_size = 0; + if (encoders.size() == 1 && info.point_step > 0) { + // A single field: encode the whole chunk at once. + const size_t count = std::min(points_per_chunk, cloud_data.size() / info.point_step); + serialized_size += encoders.front()->encodePoints(cloud_data.data(), info.point_step, count, output); + cloud_data.trim_front(count * info.point_step); + points_in_current_chunk = count; + } while (!cloud_data.empty() && points_in_current_chunk < points_per_chunk) { for (auto& encoder : encoders) { serialized_size += encoder->encode(cloud_data, output); diff --git a/cloudini_lib/test/test_field_encoders.cpp b/cloudini_lib/test/test_field_encoders.cpp index 5b62287..0dc86a7 100644 --- a/cloudini_lib/test/test_field_encoders.cpp +++ b/cloudini_lib/test/test_field_encoders.cpp @@ -1523,3 +1523,76 @@ TEST(FieldEncoders, RefineResolutionsToDataKeepsErrorBoundForLargeValues) { } EXPECT_LE(max_error_odometer, 0.0005 + 1e-6); } + +// Single-field clouds take the bulk encodePoints()/decodePoints() paths: they must produce the same stream +// as encoding point by point, for contiguous (point_step == field size) and padded points. +TEST(FieldEncoders, SingleFieldBulkMatchesPerPoint) { + using namespace Cloudini; + std::mt19937 rng(3); + std::uniform_real_distribution dist(-50.0f, 50.0f); + const size_t kNumPoints = 70000; // more than one chunk + + for (const size_t point_step : {size_t{4}, size_t{12}}) { + for (const bool lossless : {true, false}) { + std::vector points(kNumPoints * point_step, 0); + for (size_t i = 0; i < kNumPoints; ++i) { + const float v = dist(rng); + memcpy(points.data() + i * point_step, &v, sizeof(v)); + } + const std::optional resolution = lossless ? std::nullopt : std::optional(1e-3f); + + // Reference: one FieldEncoder::encode() call per point. + std::unique_ptr ref_encoder; + if (lossless) { + ref_encoder = std::make_unique(0, FieldType::FLOAT32); + } else { + ref_encoder = std::make_unique>(0, *resolution); + } + std::vector ref(kNumPoints * 16); + BufferView ref_view(ref.data(), ref.size()); + size_t ref_size = 0; + for (size_t i = 0; i < kNumPoints; ++i) { + ref_size += ref_encoder->encode(ConstBufferView(points.data() + i * point_step, point_step), ref_view); + } + + std::unique_ptr bulk_encoder; + if (lossless) { + bulk_encoder = std::make_unique(0, FieldType::FLOAT32); + } else { + bulk_encoder = std::make_unique>(0, *resolution); + } + std::vector bulk(kNumPoints * 16); + BufferView bulk_view(bulk.data(), bulk.size()); + const size_t bulk_size = bulk_encoder->encodePoints(points.data(), point_step, kNumPoints, bulk_view); + ASSERT_EQ(ref_size, bulk_size); + ASSERT_EQ(0, memcmp(ref.data(), bulk.data(), ref_size)); + + // Whole-cloud round trip: + EncodingInfo info; + info.fields = {{"v", 0, FieldType::FLOAT32, resolution}}; + info.width = kNumPoints; + info.height = 1; + info.point_step = static_cast(point_step); + info.encoding_opt = EncodingOptions::LOSSY; + info.compression_opt = CompressionOption::ZSTD; + std::vector encoded; + PointcloudEncoder(info).encode(ConstBufferView(points.data(), points.size()), encoded); + + std::vector decoded(points.size(), 0); + ConstBufferView input(encoded.data(), encoded.size()); + const EncodingInfo header = DecodeHeader(input); + PointcloudDecoder().decode(header, input, BufferView(decoded.data(), decoded.size())); + for (size_t i = 0; i < kNumPoints; ++i) { + float a = 0; + float b = 0; + memcpy(&a, points.data() + i * point_step, sizeof(a)); + memcpy(&b, decoded.data() + i * point_step, sizeof(b)); + if (lossless) { + ASSERT_EQ(a, b) << "point " << i; + } else { + ASSERT_NEAR(a, b, 0.5e-3f + 1e-5f) << "point " << i; + } + } + } + } +}