voxelmap clearing

include/gtsam_points/ann/impl/incremental_voxelmap_impl.hpp

@@ -20,6 +20,13 @@ IncrementalVoxelMap<VoxelContents>::IncrementalVoxelMap(double leaf_size)
 template <typename VoxelContents>
 IncrementalVoxelMap<VoxelContents>::~IncrementalVoxelMap() {}
 
+template <typename VoxelContents>
+void IncrementalVoxelMap<VoxelContents>::clear() {
+  lru_counter = 0;
+  flat_voxels.clear();
+  voxels.clear();
+}
+
 template <typename VoxelContents>
 void IncrementalVoxelMap<VoxelContents>::insert(const PointCloud& points) {
   // Insert points to the voxelmap
@@ -184,4 +191,40 @@ std::vector<double> IncrementalVoxelMap<VoxelContents>::voxel_intensities() cons
   return intensities;
 }
 
+template <typename VoxelContents>
+PointCloudCPU::Ptr IncrementalVoxelMap<VoxelContents>::voxel_data() const {
+  auto frame = std::make_shared<PointCloudCPU>();
+  frame->points_storage.reserve(flat_voxels.size() * 10);
+  if (has_normals()) {
+    frame->normals_storage.reserve(flat_voxels.size() * 10);
+  }
+  if (has_covs()) {
+    frame->covs_storage.reserve(flat_voxels.size() * 10);
+  }
+  if (has_intensities()) {
+    frame->intensities_storage.reserve(flat_voxels.size() * 10);
+  }
+
+  visit_points([&](const auto& voxel, const int i) {
+    frame->points_storage.emplace_back(frame::point(voxel, i));
+    if (frame::has_normals(voxel)) {
+      frame->normals_storage.emplace_back(frame::normal(voxel, i));
+    }
+    if (frame::has_covs(voxel)) {
+      frame->covs_storage.emplace_back(frame::cov(voxel, i));
+    }
+    if (frame::has_intensities(voxel)) {
+      frame->intensities_storage.emplace_back(frame::intensity(voxel, i));
+    }
+  });
+
+  frame->num_points = frame->points_storage.size();
+  frame->points = frame->points_storage.data();
+  frame->normals = frame->normals_storage.empty() ? nullptr : frame->normals_storage.data();
+  frame->covs = frame->covs_storage.empty() ? nullptr : frame->covs_storage.data();
+  frame->intensities = frame->intensities_storage.empty() ? nullptr : frame->intensities_storage.data();
+
+  return frame;
+}
+
 }  // namespace gtsam_points

include/gtsam_points/ann/incremental_covariance_voxelmap.hpp

@@ -32,6 +32,9 @@ struct IncrementalCovarianceVoxelMap : public IncrementalVoxelMap<IncrementalCov
   /// @brief Set the number of threads for normal estimation.
   void set_num_threads(int num_threads);
 
+  /// @brief Clear the voxelmap.
+  virtual void clear() override;
+
   /// @brief Insert point into the voxelmap.
   virtual void insert(const PointCloud& points) override;
 

include/gtsam_points/ann/incremental_voxelmap.hpp

@@ -4,7 +4,7 @@
 
 #include <Eigen/Core>
 #include <gtsam_points/util/vector3i_hash.hpp>
-#include <gtsam_points/types/point_cloud.hpp>
+#include <gtsam_points/types/point_cloud_cpu.hpp>
 #include <gtsam_points/ann/nearest_neighbor_search.hpp>
 
 namespace gtsam_points {
@@ -59,6 +59,9 @@ struct IncrementalVoxelMap : public NearestNeighborSearch {
   /// @brief Number of voxels in the voxelmap.
   size_t num_voxels() const { return flat_voxels.size(); }
 
+  /// @brief Clear the voxelmap.
+  virtual void clear();
+
   /// @brief Insert points to the voxelmap.
   /// @param points Point cloud
   /// @param T      Transformation matrix
@@ -92,6 +95,8 @@ struct IncrementalVoxelMap : public NearestNeighborSearch {
   virtual std::vector<Eigen::Matrix4d> voxel_covs() const;
   virtual std::vector<double> voxel_intensities() const;
 
+  virtual PointCloudCPU::Ptr voxel_data() const;
+
 protected:
   std::vector<Eigen::Vector3i> neighbor_offsets(const int neighbor_voxel_mode) const;
 

src/gtsam_points/ann/incremental_covariance_voxelmap.cpp

@@ -60,6 +60,12 @@ void IncrementalCovarianceVoxelMap::set_num_threads(int num_threads) {
   this->num_threads = num_threads;
 }
 
+void IncrementalCovarianceVoxelMap::clear() {
+  IncrementalVoxelMap<IncrementalCovarianceContainer>::clear();
+  eig_stats.clear();
+  eig_stats.resize(10);
+}
+
 void IncrementalCovarianceVoxelMap::insert(const PointCloud& points) {
   EasyProfiler prof("IncrementalCovarianceVoxelMap::insert");