perf(cloudini_lib): faster single-field and lossless encoding/decoding

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 $<$<NOT:$<CONFIG:Debug>>:-O3>)
 endif()
 
 target_include_directories(cloudini_lib

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 <size_t N>
+  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;
 };

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 <size_t N>
+  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;
   }

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);

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<float> 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<uint8_t> 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<float> resolution = lossless ? std::nullopt : std::optional<float>(1e-3f);
+
+      // Reference: one FieldEncoder::encode() call per point.
+      std::unique_ptr<FieldEncoder> ref_encoder;
+      if (lossless) {
+        ref_encoder = std::make_unique<FieldEncoderCopy>(0, FieldType::FLOAT32);
+      } else {
+        ref_encoder = std::make_unique<FieldEncoderFloat_Lossy<float>>(0, *resolution);
+      }
+      std::vector<uint8_t> 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<FieldEncoder> bulk_encoder;
+      if (lossless) {
+        bulk_encoder = std::make_unique<FieldEncoderCopy>(0, FieldType::FLOAT32);
+      } else {
+        bulk_encoder = std::make_unique<FieldEncoderFloat_Lossy<float>>(0, *resolution);
+      }
+      std::vector<uint8_t> 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<uint32_t>(point_step);
+      info.encoding_opt = EncodingOptions::LOSSY;
+      info.compression_opt = CompressionOption::ZSTD;
+      std::vector<uint8_t> encoded;
+      PointcloudEncoder(info).encode(ConstBufferView(points.data(), points.size()), encoded);
+
+      std::vector<uint8_t> 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;
+        }
+      }
+    }
+  }
+}