From 9f1c630c158b3a5a0ffec0d56d1bc1d9cf674d2e Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 13 Oct 2023 21:25:03 +0200 Subject: [PATCH 001/159] Implement Quasi Euclidean SDF generator --- libraries/wavemap/CMakeLists.txt | 2 + .../data_structure/volumetric/hashed_blocks.h | 55 ++++-- .../volumetric/impl/hashed_blocks_inl.h | 23 ++- .../include/wavemap/utils/sdf/bucket_queue.h | 49 +++++ .../wavemap/utils/sdf/impl/bucket_queue_inl.h | 76 ++++++++ .../include/wavemap/utils/sdf/sdf_generator.h | 59 ++++++ .../volumetric/hashed_blocks.cc | 17 +- .../wavemap/src/utils/sdf/sdf_generator.cc | 178 ++++++++++++++++++ .../test/src/utils/test_sdf_generation.cc | 96 ++++++++++ 9 files changed, 515 insertions(+), 40 deletions(-) create mode 100644 libraries/wavemap/include/wavemap/utils/sdf/bucket_queue.h create mode 100644 libraries/wavemap/include/wavemap/utils/sdf/impl/bucket_queue_inl.h create mode 100644 libraries/wavemap/include/wavemap/utils/sdf/sdf_generator.h create mode 100644 libraries/wavemap/src/utils/sdf/sdf_generator.cc create mode 100644 libraries/wavemap/test/src/utils/test_sdf_generation.cc diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index 8e0314070..6006a0971 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -66,6 +66,7 @@ add_library(${PROJECT_NAME} src/integrator/ray_tracing/ray_tracing_integrator.cc src/integrator/integrator_base.cc src/integrator/integrator_factory.cc + src/utils/sdf/sdf_generator.cc src/utils/stopwatch.cc src/utils/thread_pool.cc) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) @@ -115,6 +116,7 @@ if (CATKIN_ENABLE_TESTING) test/src/utils/test_int_math.cc test/src/utils/test_map_interpolator.cpp test/src/utils/test_query_accelerator.cc + test/src/utils/test_sdf_generation.cc test/src/utils/test_thread_pool.cc test/src/utils/test_tree_math.cc) target_include_directories(test_${PROJECT_NAME} PRIVATE test/include) diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_blocks.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_blocks.h index d87200903..3e7b98a91 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_blocks.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_blocks.h @@ -14,13 +14,25 @@ namespace wavemap { class HashedBlocks : public VolumetricDataStructureBase { public: + static constexpr IndexElement kCellsPerSideLog2 = 4; + static constexpr IndexElement kCellsPerSide = + int_math::exp2(kCellsPerSideLog2); + static constexpr IndexElement kCellsPerBlock = + int_math::exp2(kDim * kCellsPerSideLog2); + using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; using Config = VolumetricDataStructureConfig; static constexpr bool kRequiresExplicitThresholding = false; - // Use the base class' constructor - using VolumetricDataStructureBase::VolumetricDataStructureBase; + using Block = std::array; + using BlockIndex = Index3D; + using CellIndex = Index3D; + + explicit HashedBlocks(const VolumetricDataStructureConfig& config, + FloatingPoint default_value = 0.f) + : VolumetricDataStructureBase(config.checkValid()), + default_value_(default_value) {} bool empty() const override { return blocks_.empty(); } size_t size() const override { return kCellsPerBlock * blocks_.size(); } @@ -37,37 +49,44 @@ class HashedBlocks : public VolumetricDataStructureBase { Index3D getMinIndex() const override; Index3D getMaxIndex() const override; IndexElement getTreeHeight() const override { return 0; } + const VolumetricDataStructureConfig& getConfig() { return config_; } FloatingPoint getCellValue(const Index3D& index) const override; void setCellValue(const Index3D& index, FloatingPoint new_value) override; void addToCellValue(const Index3D& index, FloatingPoint update) override; + FloatingPoint getDefaultCellValue() const { return default_value_; } - void forEachLeaf( - typename VolumetricDataStructureBase::IndexedLeafVisitorFunction - visitor_fn) const override; - - private: - static constexpr IndexElement kCellsPerSideLog2 = 4; - static constexpr IndexElement kCellsPerSide = - int_math::exp2(kCellsPerSideLog2); - static constexpr IndexElement kCellsPerBlock = - int_math::exp2(kDim * kCellsPerSideLog2); - - using Block = std::array; - using BlockIndex = Index3D; - using CellIndex = Index3D; - - std::unordered_map> blocks_; + bool hasBlock(const Index3D& block_index) const { + return blocks_.count(block_index); + } + Block& getOrAllocateBlock(const Index3D& block_index); + Block& getBlock(const Index3D& block_index) { + return blocks_.at(block_index); + } + const Block& getBlock(const Index3D& block_index) const { + return blocks_.at(block_index); + } + auto& getBlocks() { return blocks_; } + const auto& getBlocks() const { return blocks_; } FloatingPoint* accessCellData(const Index3D& index, bool auto_allocate = false); const FloatingPoint* accessCellData(const Index3D& index) const; + void forEachLeaf( + typename VolumetricDataStructureBase::IndexedLeafVisitorFunction + visitor_fn) const override; + static BlockIndex computeBlockIndexFromIndex(const Index3D& index); static CellIndex computeCellIndexFromBlockIndexAndIndex( const BlockIndex& block_index, const Index3D& index); static Index3D computeIndexFromBlockIndexAndCellIndex( const BlockIndex& block_index, const CellIndex& cell_index); + + private: + const FloatingPoint default_value_; + + std::unordered_map> blocks_; }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_blocks_inl.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_blocks_inl.h index 179569230..f16954b85 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_blocks_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_blocks_inl.h @@ -14,7 +14,7 @@ inline FloatingPoint HashedBlocks::getCellValue(const Index3D& index) const { if (cell_data) { return static_cast(*cell_data); } - return 0.f; + return default_value_; } inline void HashedBlocks::setCellValue(const Index3D& index, @@ -40,20 +40,25 @@ inline void HashedBlocks::addToCellValue(const Index3D& index, } } +inline HashedBlocks::Block& HashedBlocks::getOrAllocateBlock( + const Index3D& block_index) { + if (!hasBlock(block_index)) { + auto it = blocks_.try_emplace(block_index).first; + it->second.fill(default_value_); + } + return blocks_.at(block_index); +} + inline FloatingPoint* HashedBlocks::accessCellData(const Index3D& index, bool auto_allocate) { BlockIndex block_index = computeBlockIndexFromIndex(index); - auto it = blocks_.find(block_index); - if (it == blocks_.end()) { - if (auto_allocate) { - it = blocks_.emplace(block_index, Block{}).first; - } else { - return nullptr; - } + if (!auto_allocate && !hasBlock(block_index)) { + return nullptr; } + Block& block = getOrAllocateBlock(block_index); CellIndex cell_index = computeCellIndexFromBlockIndexAndIndex(block_index, index); - return &it->second[convert::indexToLinearIndex(cell_index)]; + return &block[convert::indexToLinearIndex(cell_index)]; } inline const FloatingPoint* HashedBlocks::accessCellData( diff --git a/libraries/wavemap/include/wavemap/utils/sdf/bucket_queue.h b/libraries/wavemap/include/wavemap/utils/sdf/bucket_queue.h new file mode 100644 index 000000000..0d5d0176c --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/sdf/bucket_queue.h @@ -0,0 +1,49 @@ +#ifndef WAVEMAP_UTILS_SDF_BUCKET_QUEUE_H_ +#define WAVEMAP_UTILS_SDF_BUCKET_QUEUE_H_ + +#include +#include +#include + +#include + +namespace wavemap { +/** + * Bucketed priority queue, mostly following L. Yatziv et al in + * O(N) Implementation of the Fast Marching Algorithm, though skipping the + * circular aspect (don't care about a bit more memory used for this). + * Based on voxblox's bucket queue, see: + * https://github.com/ethz-asl/voxblox/blob/master/voxblox/include/voxblox/utils/bucket_queue.h + */ +template +class BucketQueue { + public: + explicit BucketQueue(int num_buckets, FloatingPoint max_key); + + /// WARNING: Calling this method clears the queue. + void setNumBuckets(int num_buckets, FloatingPoint max_key); + + size_t size() const { return num_elements_; } + bool empty() { return num_elements_ == 0; } + void clear(); + + void push(FloatingPoint key, const ValueT& value); + void pop(); + ValueT front(); + + private: + int num_buckets_; + FloatingPoint max_key_; + std::vector> buckets_; + + /// Used to speed up retrievals + int last_bucket_index_; + + /// Used to speed up empty checks + size_t num_elements_; +}; +} // namespace wavemap + +#include "wavemap/utils/sdf/impl/bucket_queue_inl.h" + +#endif // WAVEMAP_UTILS_SDF_BUCKET_QUEUE_H_ diff --git a/libraries/wavemap/include/wavemap/utils/sdf/impl/bucket_queue_inl.h b/libraries/wavemap/include/wavemap/utils/sdf/impl/bucket_queue_inl.h new file mode 100644 index 000000000..f7fd4175c --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/sdf/impl/bucket_queue_inl.h @@ -0,0 +1,76 @@ +#ifndef WAVEMAP_UTILS_SDF_IMPL_BUCKET_QUEUE_INL_H_ +#define WAVEMAP_UTILS_SDF_IMPL_BUCKET_QUEUE_INL_H_ + +namespace wavemap { +template +BucketQueue::BucketQueue(int num_buckets, FloatingPoint max_key) + : num_buckets_(num_buckets), + max_key_(max_key), + last_bucket_index_(0), + num_elements_(0) { + buckets_.resize(num_buckets_); +} + +template +void BucketQueue::setNumBuckets(int num_buckets, + FloatingPoint max_key) { + max_key_ = max_key; + num_buckets_ = num_buckets; + buckets_.clear(); + buckets_.resize(num_buckets_); + num_elements_ = 0; +} + +template +void BucketQueue::clear() { + buckets_.clear(); + buckets_.resize(num_buckets_); + last_bucket_index_ = 0; + num_elements_ = 0; +} + +template +void BucketQueue::push(FloatingPoint key, const ValueT& value) { + CHECK_NE(num_buckets_, 0); + if (key > max_key_) { + key = max_key_; + } + int bucket_index = std::floor(std::abs(key) / max_key_ * (num_buckets_ - 1)); + if (bucket_index >= num_buckets_) { + bucket_index = num_buckets_ - 1; + } + if (bucket_index < last_bucket_index_) { + last_bucket_index_ = bucket_index; + } + buckets_[bucket_index].push(value); + num_elements_++; +} + +template +void BucketQueue::pop() { + if (empty()) { + return; + } + while (last_bucket_index_ < num_buckets_ && + buckets_[last_bucket_index_].empty()) { + last_bucket_index_++; + } + if (last_bucket_index_ < num_buckets_) { + buckets_[last_bucket_index_].pop(); + num_elements_--; + } +} + +template +ValueT BucketQueue::front() { + CHECK_NE(num_buckets_, 0); + CHECK(!empty()); + while (last_bucket_index_ < num_buckets_ && + buckets_[last_bucket_index_].empty()) { + last_bucket_index_++; + } + return buckets_[last_bucket_index_].front(); +} +} // namespace wavemap + +#endif // WAVEMAP_UTILS_SDF_IMPL_BUCKET_QUEUE_INL_H_ diff --git a/libraries/wavemap/include/wavemap/utils/sdf/sdf_generator.h b/libraries/wavemap/include/wavemap/utils/sdf/sdf_generator.h new file mode 100644 index 000000000..bb171ad03 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/sdf/sdf_generator.h @@ -0,0 +1,59 @@ +#ifndef WAVEMAP_UTILS_SDF_SDF_GENERATOR_H_ +#define WAVEMAP_UTILS_SDF_SDF_GENERATOR_H_ + +#include "wavemap/data_structure/volumetric/hashed_blocks.h" +#include "wavemap/data_structure/volumetric/hashed_wavelet_octree.h" +#include "wavemap/utils/iterate/grid_iterator.h" +#include "wavemap/utils/query/query_accelerator.h" +#include "wavemap/utils/sdf/bucket_queue.h" + +namespace wavemap { +namespace neighborhood { +std::array generateNeighborIndexOffsets(); +std::array generateNeighborDistanceOffsets( + FloatingPoint cell_width); +} // namespace neighborhood + +class QuasiEuclideanSDFGenerator { + public: + static constexpr FloatingPoint kMaxMultiplicativeError = 0.125f + 1e-3f; + + explicit QuasiEuclideanSDFGenerator(FloatingPoint max_distance = 2.f, + FloatingPoint occupancy_threshold = 0.f) + : occupancy_threshold_(occupancy_threshold), + max_distance_(max_distance) {} + + HashedBlocks generate(const HashedWaveletOctree& occupancy_map); + + FloatingPoint getOccupancyThreshold() const { return occupancy_threshold_; } + FloatingPoint getMaxDistance() const { return max_distance_; } + + static bool isUnobserved(FloatingPoint occupancy_value) { + return std::abs(occupancy_value) < 1e-3f; + } + static bool isObserved(FloatingPoint occupancy_value) { + return !isUnobserved(occupancy_value); + } + bool isFree(FloatingPoint occupancy_value) const { + return occupancy_value < occupancy_threshold_; + } + bool isOccupied(FloatingPoint occupancy_value) const { + return occupancy_threshold_ < occupancy_value; + } + + private: + static constexpr FloatingPoint kTolerance = 1e-2f; + inline static const auto kNeighborIndexOffsets = + neighborhood::generateNeighborIndexOffsets(); + + const FloatingPoint occupancy_threshold_; + const FloatingPoint max_distance_; + + void seed(const HashedWaveletOctree& occupancy_map, HashedBlocks& sdf, + BucketQueue& open_queue) const; + void propagate(const HashedWaveletOctree& occupancy_map, HashedBlocks& sdf, + BucketQueue& open_queue) const; +}; +} // namespace wavemap + +#endif // WAVEMAP_UTILS_SDF_SDF_GENERATOR_H_ diff --git a/libraries/wavemap/src/data_structure/volumetric/hashed_blocks.cc b/libraries/wavemap/src/data_structure/volumetric/hashed_blocks.cc index 91ab7209c..fd34028c3 100644 --- a/libraries/wavemap/src/data_structure/volumetric/hashed_blocks.cc +++ b/libraries/wavemap/src/data_structure/volumetric/hashed_blocks.cc @@ -2,21 +2,12 @@ namespace wavemap { void HashedBlocks::prune() { - const Index3D min_local_cell_index = Index3D::Zero(); - const Index3D max_local_cell_index = Index3D::Constant(kCellsPerSide - 1); - // TODO(victorr): Iterate directly over linear index instead of grid - const Grid local_grid(min_local_cell_index, max_local_cell_index); - std::unordered_set> blocks_to_delete; for (const auto& [block_index, block_data] : blocks_) { - if (!std::any_of( - local_grid.begin(), local_grid.end(), - [&block_data = block_data](const auto& cell_index) { - const FloatingPoint cell_value = - block_data[convert::indexToLinearIndex( - cell_index)]; - return cell_value != FloatingPoint{}; - })) { + if (std::all_of(block_data.begin(), block_data.end(), + [default_value = default_value_](const auto& cell_value) { + return cell_value == default_value; + })) { blocks_to_delete.emplace(block_index); } } diff --git a/libraries/wavemap/src/utils/sdf/sdf_generator.cc b/libraries/wavemap/src/utils/sdf/sdf_generator.cc new file mode 100644 index 000000000..0bc768795 --- /dev/null +++ b/libraries/wavemap/src/utils/sdf/sdf_generator.cc @@ -0,0 +1,178 @@ +#include "wavemap/utils/sdf/sdf_generator.h" + +#include + +namespace wavemap { +std::array neighborhood::generateNeighborIndexOffsets() { + std::array neighbor_offsets{}; + size_t array_idx = 0u; + for (const Index3D& index : Grid<3>(-Index3D::Ones(), Index3D::Ones())) { + if (index != Index3D::Zero()) { + neighbor_offsets[array_idx] = index; + ++array_idx; + } + } + return neighbor_offsets; +} + +std::array neighborhood::generateNeighborDistanceOffsets( + FloatingPoint cell_width) { + std::array distance_offsets{}; + size_t array_idx = 0u; + for (const Index3D& index_offset : generateNeighborIndexOffsets()) { + distance_offsets[array_idx] = + cell_width * index_offset.cast().norm(); + ++array_idx; + } + return distance_offsets; +} + +HashedBlocks QuasiEuclideanSDFGenerator::generate( + const HashedWaveletOctree& occupancy_map) { + ZoneScoped; + // Initialize the SDF data structure + const FloatingPoint min_cell_width = occupancy_map.getMinCellWidth(); + const VolumetricDataStructureConfig config{min_cell_width, 0.f, + max_distance_}; + HashedBlocks sdf(config, max_distance_); + + // Initialize the bucketed priority queue + const int num_bins = + static_cast(std::ceil(max_distance_ / min_cell_width)); + BucketQueue open{num_bins, max_distance_}; + + // Seed and propagate the SDF + seed(occupancy_map, sdf, open); + propagate(occupancy_map, sdf, open); + + return sdf; +} + +void QuasiEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, + HashedBlocks& sdf, + BucketQueue& open_queue) const { + ZoneScoped; + // Create an occupancy query accelerator + QueryAccelerator occupancy_query_accelerator{occupancy_map}; + + // For all free cells that border an obstacle: + // - initialize their SDF value, and + // - add them to the open queue + occupancy_map.forEachLeaf([this, &occupancy_map, &sdf, &open_queue, + min_cell_width = occupancy_map.getMinCellWidth()]( + const OctreeIndex& node_index, + FloatingPoint node_occupancy) { + // Only process obstacles + if (isUnobserved(node_occupancy) || isFree(node_occupancy)) { + return; + } + + // Span a grid at the highest resolution (=SDF resolution) that pads the + // multi-resolution obstacle cell with 1 voxel in all directions + const Index3D min_corner = convert::nodeIndexToMinCornerIndex(node_index); + const Index3D max_corner = convert::nodeIndexToMaxCornerIndex(node_index); + const Grid<3> grid{ + Grid<3>(min_corner - Index3D::Ones(), max_corner + Index3D::Ones())}; + + // Iterate over the grid + for (const Index3D& index : grid) { + // Skip cells that are inside the occupied node (obstacle) + // NOTE: Occupied cells (negative distances) are handled in the + // propagation stage. + const Index3D nearest_inner_index = + index.cwiseMax(min_corner).cwiseMin(max_corner); + const bool voxel_is_inside = (index == nearest_inner_index); + if (voxel_is_inside) { + continue; + } + + // Skip the cell if it is not free + const FloatingPoint occupancy = occupancy_map.getCellValue(index); + if (isUnobserved(occupancy) || isOccupied(occupancy)) { + continue; + } + + // Get the voxel's SDF value + FloatingPoint& sdf_value = *sdf.accessCellData(index, true); + const bool sdf_uninitialized = + sdf.getDefaultCellValue() - kTolerance < sdf_value; + + // Update the voxel's SDF value + const FloatingPoint distance_to_surface = + 0.5f * min_cell_width * + (index - nearest_inner_index).cast().norm(); + sdf_value = std::min(sdf_value, distance_to_surface); + + // If the voxel is not yet in the open queue, add it + if (sdf_uninitialized) { + open_queue.push(distance_to_surface, index); + } + } + }); +} + +void QuasiEuclideanSDFGenerator::propagate( + const HashedWaveletOctree& occupancy_map, HashedBlocks& sdf, + BucketQueue& open_queue) const { + ZoneScoped; + // Create an occupancy query accelerator + QueryAccelerator occupancy_query_accelerator{occupancy_map}; + + // Precompute the neighbor distance offsets + const auto neighbor_distance_offsets = + neighborhood::generateNeighborDistanceOffsets( + occupancy_map.getMinCellWidth()); + CHECK_EQ(kNeighborIndexOffsets.size(), neighbor_distance_offsets.size()); + + // Propagate the distance + while (!open_queue.empty()) { + TracyPlot("QueueLength", static_cast(open_queue.size())); + const Index3D index = open_queue.front(); + const FloatingPoint sdf_value = sdf.getCellValue(index); + const FloatingPoint df_value = std::abs(sdf_value); + TracyPlot("Distance", df_value); + open_queue.pop(); + + for (size_t neighbor_idx = 0; neighbor_idx < kNeighborIndexOffsets.size(); + ++neighbor_idx) { + // Compute the neighbor's distance if reached from the current voxel + const FloatingPoint neighbor_distance = + df_value + neighbor_distance_offsets[neighbor_idx]; + if (max_distance_ <= neighbor_distance) { + continue; + } + + // Get the neighbor's SDF value + const Index3D& neighbor_index = + index + kNeighborIndexOffsets[neighbor_idx]; + FloatingPoint& neighbor_sdf = *sdf.accessCellData(neighbor_index, true); + + // If the neighbor is uninitialized, get its sign from the occupancy map + const bool neighbor_sdf_uninitialized = + sdf.getDefaultCellValue() - kTolerance < neighbor_sdf; + if (neighbor_sdf_uninitialized) { + const FloatingPoint neighbor_occupancy = + occupancy_query_accelerator.getCellValue(neighbor_index); + // Never initialize or update unknown cells + if (!isObserved(neighbor_occupancy)) { + continue; + } + // Set the sign + if (isOccupied(neighbor_occupancy)) { + neighbor_sdf = -sdf.getDefaultCellValue(); + } + } + + // Update the neighbor's SDF value + const FloatingPoint neighbor_df = + std::min(std::abs(neighbor_sdf), neighbor_distance); + neighbor_sdf = std::copysign(neighbor_df, neighbor_sdf); + + // If the neighbor is not yet in the open queue, add it + if (neighbor_sdf_uninitialized) { + open_queue.push(neighbor_distance, neighbor_index); + } + } + } +} +} // namespace wavemap diff --git a/libraries/wavemap/test/src/utils/test_sdf_generation.cc b/libraries/wavemap/test/src/utils/test_sdf_generation.cc new file mode 100644 index 000000000..cc1eba1a2 --- /dev/null +++ b/libraries/wavemap/test/src/utils/test_sdf_generation.cc @@ -0,0 +1,96 @@ +#include + +#include "wavemap/common.h" +#include "wavemap/data_structure/volumetric/hashed_blocks.h" +#include "wavemap/data_structure/volumetric/hashed_wavelet_octree.h" +#include "wavemap/test/config_generator.h" +#include "wavemap/test/fixture_base.h" +#include "wavemap/test/geometry_generator.h" +#include "wavemap/utils/sdf/sdf_generator.h" + +namespace wavemap { +class SdfGenerationTest : public FixtureBase, + public GeometryGenerator, + public ConfigGenerator {}; + +TEST_F(SdfGenerationTest, QuasiEuclideanSDFGenerator) { + // Params + const Index3D min_index = Index3D::Constant(-10); + const Index3D max_index = Index3D::Constant(100); + constexpr FloatingPoint kMaxSdfDistance = 2.f; + + // Create a map and sample random obstacles + const auto config = + ConfigGenerator::getRandomConfig(); + HashedWaveletOctree map{config}; + const auto obstacle_cells = + GeometryGenerator::getRandomIndexVector<3>(10, 20, min_index, max_index); + const FloatingPoint min_cell_width = map.getMinCellWidth(); + + // Set default occupancy to free + const IndexElement padding = std::ceil(kMaxSdfDistance / min_cell_width); + const OctreeIndex min_block_index = convert::indexAndHeightToNodeIndex<3>( + min_index.array() - padding, map.getTreeHeight()); + const OctreeIndex max_block_index = convert::indexAndHeightToNodeIndex<3>( + max_index.array() + padding, map.getTreeHeight()); + for (const auto& block_index : + Grid(min_block_index.position, max_block_index.position)) { + map.getOrAllocateBlock(block_index).getRootScale() = config.min_log_odds; + } + + // Set obstacles to occupied + for (const Index3D& index : obstacle_cells) { + map.setCellValue(index, config.max_log_odds); + } + + // Generate the SDF + QuasiEuclideanSDFGenerator sdf_generator{kMaxSdfDistance}; + const auto sdf = sdf_generator.generate(map); + + // Compare the SDF distances to the brute force min distance + sdf.forEachLeaf([&map, &sdf_generator, &sdf, &obstacle_cells, min_cell_width]( + const OctreeIndex& node_index, FloatingPoint sdf_value) { + // In unobserved space, the SDF should be uninitialized + const FloatingPoint occupancy_value = map.getCellValue(node_index); + if (QuasiEuclideanSDFGenerator::isUnobserved(occupancy_value)) { + // In unknown space the SDF should be uninitialized + EXPECT_NEAR(sdf_value, sdf.getDefaultCellValue(), kEpsilon); + return; + } + + // In occupied space, the SDF should be negative and vice versa + if (sdf_generator.isOccupied(occupancy_value)) { + // In occupied space, the SDF should be negative + EXPECT_LE(sdf_value, 0.f); + return; + } + if (sdf_value < 0.f) { + EXPECT_GT(occupancy_value, 0.f); + return; + } + + // We're in free space + // Compute the distance to the closest obstacle using brute force + FloatingPoint min_distance_brute_force = sdf.getDefaultCellValue(); + const Point3D node_center = + convert::nodeIndexToCenterPoint(node_index, min_cell_width); + for (const auto& obstacle_cell : obstacle_cells) { + const auto obstacle_aabb = convert::nodeIndexToAABB( + OctreeIndex{0, obstacle_cell}, min_cell_width); + min_distance_brute_force = std::min( + obstacle_aabb.minDistanceTo(node_center), min_distance_brute_force); + } + + // Check that the SDF accurately approximates the min obstacle distance + constexpr FloatingPoint kMaxMultiplicativeError = + QuasiEuclideanSDFGenerator::kMaxMultiplicativeError; + if (min_distance_brute_force <= sdf.getDefaultCellValue()) { + EXPECT_NEAR(sdf_value, min_distance_brute_force, + min_distance_brute_force * kMaxMultiplicativeError); + } else { + EXPECT_NEAR(sdf_value, sdf.getDefaultCellValue(), + sdf.getDefaultCellValue() * kMaxMultiplicativeError); + } + }); +} +} // namespace wavemap From 3d0f0f41de998025824e6b853124af50bb510816 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 13 Oct 2023 22:37:41 +0200 Subject: [PATCH 002/159] Correctly handle surface crossings and negative distances --- .../wavemap/src/utils/sdf/sdf_generator.cc | 34 +++++-- .../test/src/utils/test_sdf_generation.cc | 88 +++++++++++++------ 2 files changed, 85 insertions(+), 37 deletions(-) diff --git a/libraries/wavemap/src/utils/sdf/sdf_generator.cc b/libraries/wavemap/src/utils/sdf/sdf_generator.cc index 0bc768795..8e8ef379f 100644 --- a/libraries/wavemap/src/utils/sdf/sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/sdf_generator.cc @@ -58,7 +58,8 @@ void QuasiEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, // For all free cells that border an obstacle: // - initialize their SDF value, and // - add them to the open queue - occupancy_map.forEachLeaf([this, &occupancy_map, &sdf, &open_queue, + occupancy_map.forEachLeaf([this, &occupancy_query_accelerator, &sdf, + &open_queue, min_cell_width = occupancy_map.getMinCellWidth()]( const OctreeIndex& node_index, FloatingPoint node_occupancy) { @@ -87,7 +88,8 @@ void QuasiEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, } // Skip the cell if it is not free - const FloatingPoint occupancy = occupancy_map.getCellValue(index); + const FloatingPoint occupancy = + occupancy_query_accelerator.getCellValue(index); if (isUnobserved(occupancy) || isOccupied(occupancy)) { continue; } @@ -119,10 +121,12 @@ void QuasiEuclideanSDFGenerator::propagate( QueryAccelerator occupancy_query_accelerator{occupancy_map}; // Precompute the neighbor distance offsets + const FloatingPoint min_cell_width = occupancy_map.getMinCellWidth(); const auto neighbor_distance_offsets = - neighborhood::generateNeighborDistanceOffsets( - occupancy_map.getMinCellWidth()); + neighborhood::generateNeighborDistanceOffsets(min_cell_width); CHECK_EQ(kNeighborIndexOffsets.size(), neighbor_distance_offsets.size()); + const FloatingPoint half_max_neighbor_distance_offset = + std::sqrt(3.f) * min_cell_width / 2; // Propagate the distance while (!open_queue.empty()) { @@ -136,9 +140,9 @@ void QuasiEuclideanSDFGenerator::propagate( for (size_t neighbor_idx = 0; neighbor_idx < kNeighborIndexOffsets.size(); ++neighbor_idx) { // Compute the neighbor's distance if reached from the current voxel - const FloatingPoint neighbor_distance = + FloatingPoint neighbor_df_candidate = df_value + neighbor_distance_offsets[neighbor_idx]; - if (max_distance_ <= neighbor_distance) { + if (max_distance_ <= neighbor_df_candidate) { continue; } @@ -163,14 +167,26 @@ void QuasiEuclideanSDFGenerator::propagate( } } + // Handle sign changes when propagating across the surface + if (0.f < sdf_value && neighbor_sdf < 0.f) { + // NOTE: When the opened cell and the neighbor cell have the same sign, + // the distance field value and offset are summed to obtain the + // unsigned neighbor distance. Whereas when moving across the + // surface, the df_value and offset have opposite signs and reduce + // each other instead. + DCHECK_LE(df_value, half_max_neighbor_distance_offset); + neighbor_df_candidate = + neighbor_distance_offsets[neighbor_idx] - df_value; + } + // Update the neighbor's SDF value - const FloatingPoint neighbor_df = - std::min(std::abs(neighbor_sdf), neighbor_distance); + FloatingPoint neighbor_df = std::abs(neighbor_sdf); + neighbor_df = std::min(neighbor_df, neighbor_df_candidate); neighbor_sdf = std::copysign(neighbor_df, neighbor_sdf); // If the neighbor is not yet in the open queue, add it if (neighbor_sdf_uninitialized) { - open_queue.push(neighbor_distance, neighbor_index); + open_queue.push(neighbor_df_candidate, neighbor_index); } } } diff --git a/libraries/wavemap/test/src/utils/test_sdf_generation.cc b/libraries/wavemap/test/src/utils/test_sdf_generation.cc index cc1eba1a2..6a2520558 100644 --- a/libraries/wavemap/test/src/utils/test_sdf_generation.cc +++ b/libraries/wavemap/test/src/utils/test_sdf_generation.cc @@ -19,16 +19,24 @@ TEST_F(SdfGenerationTest, QuasiEuclideanSDFGenerator) { const Index3D max_index = Index3D::Constant(100); constexpr FloatingPoint kMaxSdfDistance = 2.f; - // Create a map and sample random obstacles + // Create the map const auto config = ConfigGenerator::getRandomConfig(); HashedWaveletOctree map{config}; - const auto obstacle_cells = + + // Generate random obstacles + auto obstacle_cells = GeometryGenerator::getRandomIndexVector<3>(10, 20, min_index, max_index); + // Add a cube const FloatingPoint min_cell_width = map.getMinCellWidth(); + const IndexElement padding = std::ceil(kMaxSdfDistance / min_cell_width); + const Index3D cube_center{2, 6, 4}; + for (const Index3D& index : + Grid<3>(cube_center.array() - padding, cube_center.array() + padding)) { + obstacle_cells.emplace_back(index); + } // Set default occupancy to free - const IndexElement padding = std::ceil(kMaxSdfDistance / min_cell_width); const OctreeIndex min_block_index = convert::indexAndHeightToNodeIndex<3>( min_index.array() - padding, map.getTreeHeight()); const OctreeIndex max_block_index = convert::indexAndHeightToNodeIndex<3>( @@ -48,8 +56,9 @@ TEST_F(SdfGenerationTest, QuasiEuclideanSDFGenerator) { const auto sdf = sdf_generator.generate(map); // Compare the SDF distances to the brute force min distance - sdf.forEachLeaf([&map, &sdf_generator, &sdf, &obstacle_cells, min_cell_width]( - const OctreeIndex& node_index, FloatingPoint sdf_value) { + sdf.forEachLeaf([&map, &sdf_generator, &sdf, &obstacle_cells, min_cell_width, + padding](const OctreeIndex& node_index, + FloatingPoint sdf_value) { // In unobserved space, the SDF should be uninitialized const FloatingPoint occupancy_value = map.getCellValue(node_index); if (QuasiEuclideanSDFGenerator::isUnobserved(occupancy_value)) { @@ -58,38 +67,61 @@ TEST_F(SdfGenerationTest, QuasiEuclideanSDFGenerator) { return; } - // In occupied space, the SDF should be negative and vice versa - if (sdf_generator.isOccupied(occupancy_value)) { - // In occupied space, the SDF should be negative - EXPECT_LE(sdf_value, 0.f); - return; - } - if (sdf_value < 0.f) { - EXPECT_GT(occupancy_value, 0.f); - return; - } - - // We're in free space - // Compute the distance to the closest obstacle using brute force - FloatingPoint min_distance_brute_force = sdf.getDefaultCellValue(); const Point3D node_center = convert::nodeIndexToCenterPoint(node_index, min_cell_width); - for (const auto& obstacle_cell : obstacle_cells) { - const auto obstacle_aabb = convert::nodeIndexToAABB( - OctreeIndex{0, obstacle_cell}, min_cell_width); - min_distance_brute_force = std::min( - obstacle_aabb.minDistanceTo(node_center), min_distance_brute_force); + + // Find the closest surface using brute force + FloatingPoint sdf_brute_force = sdf.getDefaultCellValue(); + if (sdf_generator.isFree(occupancy_value)) { + // In free space, the SDF should always be positive + EXPECT_GT(sdf_value, 0.f); + + // Find the distance to the closest obstacle + for (const auto& obstacle_cell : obstacle_cells) { + const auto obstacle_aabb = convert::nodeIndexToAABB( + OctreeIndex{0, obstacle_cell}, min_cell_width); + sdf_brute_force = + std::min(obstacle_aabb.minDistanceTo(node_center), sdf_brute_force); + } + } else { + // Find the distance to the closest free cell + for (const Index3D& neighbor_index : + Grid<3>(node_index.position.array() - padding, + node_index.position.array() + padding)) { + const FloatingPoint neighbor_occupancy_value = + map.getCellValue(neighbor_index); + if (sdf_generator.isFree(neighbor_occupancy_value) && + QuasiEuclideanSDFGenerator::isObserved(neighbor_occupancy_value)) { + const auto free_cell_aabb = convert::nodeIndexToAABB( + OctreeIndex{0, neighbor_index}, min_cell_width); + sdf_brute_force = std::min(free_cell_aabb.minDistanceTo(node_center), + sdf_brute_force); + } + } + // Adjust the sign to reflect we're inside the obstacle + sdf_brute_force = -sdf_brute_force; + + // In occupied space, the SDF should be + if (std::abs(sdf_brute_force) < sdf.getDefaultCellValue()) { + // Negative + EXPECT_LT(sdf_value, 0.f); + } else { + // Or uninitialized + EXPECT_NEAR(sdf_value, sdf.getDefaultCellValue(), kEpsilon); + } } // Check that the SDF accurately approximates the min obstacle distance constexpr FloatingPoint kMaxMultiplicativeError = QuasiEuclideanSDFGenerator::kMaxMultiplicativeError; - if (min_distance_brute_force <= sdf.getDefaultCellValue()) { - EXPECT_NEAR(sdf_value, min_distance_brute_force, - min_distance_brute_force * kMaxMultiplicativeError); + if (std::abs(sdf_brute_force) < sdf.getDefaultCellValue()) { + EXPECT_NEAR(sdf_value, sdf_brute_force, + std::abs(sdf_brute_force) * kMaxMultiplicativeError) + << "At index " << print::eigen::oneLine(node_index.position); } else { EXPECT_NEAR(sdf_value, sdf.getDefaultCellValue(), - sdf.getDefaultCellValue() * kMaxMultiplicativeError); + sdf.getDefaultCellValue() * kMaxMultiplicativeError) + << "At index " << print::eigen::oneLine(node_index.position); } }); } From eb8035f72987d46ad8357711fed0a2681aee8f16 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 16 Oct 2023 14:38:03 +0200 Subject: [PATCH 003/159] Minor refactoring --- .../volumetric/cell_types/occupancy_state.h | 46 ------------------- .../hashed_chunked_wavelet_octree_block_inl.h | 4 +- .../impl/hashed_wavelet_octree_block_inl.h | 4 +- .../volumetric/impl/volumetric_octree_inl.h | 2 - .../volumetric/impl/wavelet_octree_inl.h | 4 +- .../utils/query/occupancy_classifier.h | 29 ++++++++++++ ...ator.h => quasi_euclidean_sdf_generator.h} | 26 +++-------- .../volumetric/volumetric_octree.cc | 6 ++- .../volumetric/wavelet_octree.cc | 6 ++- .../wavemap/src/utils/sdf/sdf_generator.cc | 14 +++--- .../test/src/utils/test_sdf_generation.cc | 19 ++++---- 11 files changed, 67 insertions(+), 93 deletions(-) delete mode 100644 libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/occupancy_state.h create mode 100644 libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h rename libraries/wavemap/include/wavemap/utils/sdf/{sdf_generator.h => quasi_euclidean_sdf_generator.h} (64%) diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/occupancy_state.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/occupancy_state.h deleted file mode 100644 index e9c256187..000000000 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/occupancy_state.h +++ /dev/null @@ -1,46 +0,0 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_CELL_TYPES_OCCUPANCY_STATE_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_CELL_TYPES_OCCUPANCY_STATE_H_ - -#include "wavemap/common.h" - -namespace wavemap { -class OccupancyState { - public: - static OccupancyState Unknown() { return {false, false}; } - static OccupancyState Free() { return {true, false}; } - static OccupancyState Occupied() { return {true, true}; } - - OccupancyState() : OccupancyState(Unknown()) {} - - void setUnknown() { *this = Unknown(); } - void setFree() { *this = Free(); } - void setOccupied() { *this = Occupied(); } - - bool isUnknown() const { return !observed_; } - bool isObserved() const { return observed_; } - bool isFree() const { return observed_ && !occupied_; } - bool isOccupied() const { return observed_ && occupied_; } - - static bool isObserved(FloatingPoint cell_value) { - return 1e-3f < std::abs(cell_value); - } - static OccupancyState fromValue(FloatingPoint cell_value) { - if (!isObserved(cell_value)) { - return Unknown(); - } else if (cell_value < 0.f) { - return Free(); - } else { - return Occupied(); - } - } - - private: - OccupancyState(bool observed, bool occupied) - : observed_(observed), occupied_(occupied) {} - - bool observed_; - bool occupied_; -}; -} // namespace wavemap - -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_CELL_TYPES_OCCUPANCY_STATE_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_chunked_wavelet_octree_block_inl.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_chunked_wavelet_octree_block_inl.h index b194cfa8f..bc7559e6a 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_chunked_wavelet_octree_block_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_chunked_wavelet_octree_block_inl.h @@ -1,7 +1,7 @@ #ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ #define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ -#include "wavemap/data_structure/volumetric/cell_types/occupancy_state.h" +#include "wavemap/utils/query/occupancy_classifier.h" namespace wavemap { inline bool HashedChunkedWaveletOctreeBlock::empty() const { @@ -10,7 +10,7 @@ inline bool HashedChunkedWaveletOctreeBlock::empty() const { // coefficients, we also need to check whether its scale coefficient // (average value over the whole block) is zero. return chunked_ndtree_.empty() && - !OccupancyState::isObserved(root_scale_coefficient_); + !OccupancyClassifier::isObserved(root_scale_coefficient_); } inline FloatingPoint HashedChunkedWaveletOctreeBlock::getTimeSinceLastUpdated() diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_wavelet_octree_block_inl.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_wavelet_octree_block_inl.h index 03fe0a012..4cf975de0 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_wavelet_octree_block_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_wavelet_octree_block_inl.h @@ -1,7 +1,7 @@ #ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ #define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ -#include "wavemap/data_structure/volumetric/cell_types/occupancy_state.h" +#include "wavemap/utils/query/occupancy_classifier.h" namespace wavemap { inline bool HashedWaveletOctreeBlock::empty() const { @@ -10,7 +10,7 @@ inline bool HashedWaveletOctreeBlock::empty() const { // coefficients, we also need to check whether its scale coefficient // (average value over the whole block) is zero. return ndtree_.empty() && - !OccupancyState::isObserved(root_scale_coefficient_); + !OccupancyClassifier::isObserved(root_scale_coefficient_); } inline FloatingPoint HashedWaveletOctreeBlock::getTimeSinceLastUpdated() const { diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/volumetric_octree_inl.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/volumetric_octree_inl.h index ebed6b854..97a184a31 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/volumetric_octree_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/volumetric_octree_inl.h @@ -6,8 +6,6 @@ #include #include -#include "wavemap/data_structure/volumetric/cell_types/occupancy_state.h" - namespace wavemap { inline OctreeIndex::ChildArray VolumetricOctree::getFirstChildIndices() const { OctreeIndex::ChildArray first_child_indices = diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/wavelet_octree_inl.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/wavelet_octree_inl.h index 687a83bc8..0c58e9cd1 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/wavelet_octree_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/wavelet_octree_inl.h @@ -6,7 +6,7 @@ #include #include -#include "wavemap/data_structure/volumetric/cell_types/occupancy_state.h" +#include "wavemap/utils/query/occupancy_classifier.h" namespace wavemap { inline bool WaveletOctree::empty() const { @@ -15,7 +15,7 @@ inline bool WaveletOctree::empty() const { // coefficients, we also need to check whether its scale coefficient // (average value over the whole map) is zero. return ndtree_.empty() && - !OccupancyState::isObserved(root_scale_coefficient_); + !OccupancyClassifier::isObserved(root_scale_coefficient_); } inline void WaveletOctree::clear() { diff --git a/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h b/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h new file mode 100644 index 000000000..95bee52f3 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h @@ -0,0 +1,29 @@ +#ifndef WAVEMAP_UTILS_QUERY_OCCUPANCY_CLASSIFIER_H_ +#define WAVEMAP_UTILS_QUERY_OCCUPANCY_CLASSIFIER_H_ + +namespace wavemap { +class OccupancyClassifier { + public: + explicit OccupancyClassifier(FloatingPoint log_odds_occupancy_threshold = 0.f) + : occupancy_threshold_(log_odds_occupancy_threshold) {} + + static bool isUnobserved(FloatingPoint log_odds_occupancy_value) { + return std::abs(log_odds_occupancy_value) < 1e-3f; + } + static bool isObserved(FloatingPoint log_odds_occupancy_value) { + return !isUnobserved(log_odds_occupancy_value); + } + + bool isFree(FloatingPoint log_odds_occupancy_value) const { + return log_odds_occupancy_value < occupancy_threshold_; + } + bool isOccupied(FloatingPoint log_odds_occupancy_value) const { + return occupancy_threshold_ < log_odds_occupancy_value; + } + + private: + const FloatingPoint occupancy_threshold_; +}; +} // namespace wavemap + +#endif // WAVEMAP_UTILS_QUERY_OCCUPANCY_CLASSIFIER_H_ diff --git a/libraries/wavemap/include/wavemap/utils/sdf/sdf_generator.h b/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h similarity index 64% rename from libraries/wavemap/include/wavemap/utils/sdf/sdf_generator.h rename to libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h index bb171ad03..691de1987 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/sdf_generator.h +++ b/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h @@ -1,9 +1,10 @@ -#ifndef WAVEMAP_UTILS_SDF_SDF_GENERATOR_H_ -#define WAVEMAP_UTILS_SDF_SDF_GENERATOR_H_ +#ifndef WAVEMAP_UTILS_SDF_QUASI_EUCLIDEAN_SDF_GENERATOR_H_ +#define WAVEMAP_UTILS_SDF_QUASI_EUCLIDEAN_SDF_GENERATOR_H_ #include "wavemap/data_structure/volumetric/hashed_blocks.h" #include "wavemap/data_structure/volumetric/hashed_wavelet_octree.h" #include "wavemap/utils/iterate/grid_iterator.h" +#include "wavemap/utils/query/occupancy_classifier.h" #include "wavemap/utils/query/query_accelerator.h" #include "wavemap/utils/sdf/bucket_queue.h" @@ -20,34 +21,19 @@ class QuasiEuclideanSDFGenerator { explicit QuasiEuclideanSDFGenerator(FloatingPoint max_distance = 2.f, FloatingPoint occupancy_threshold = 0.f) - : occupancy_threshold_(occupancy_threshold), - max_distance_(max_distance) {} + : max_distance_(max_distance), classifier_(occupancy_threshold) {} HashedBlocks generate(const HashedWaveletOctree& occupancy_map); - FloatingPoint getOccupancyThreshold() const { return occupancy_threshold_; } FloatingPoint getMaxDistance() const { return max_distance_; } - static bool isUnobserved(FloatingPoint occupancy_value) { - return std::abs(occupancy_value) < 1e-3f; - } - static bool isObserved(FloatingPoint occupancy_value) { - return !isUnobserved(occupancy_value); - } - bool isFree(FloatingPoint occupancy_value) const { - return occupancy_value < occupancy_threshold_; - } - bool isOccupied(FloatingPoint occupancy_value) const { - return occupancy_threshold_ < occupancy_value; - } - private: static constexpr FloatingPoint kTolerance = 1e-2f; inline static const auto kNeighborIndexOffsets = neighborhood::generateNeighborIndexOffsets(); - const FloatingPoint occupancy_threshold_; const FloatingPoint max_distance_; + const OccupancyClassifier classifier_; void seed(const HashedWaveletOctree& occupancy_map, HashedBlocks& sdf, BucketQueue& open_queue) const; @@ -56,4 +42,4 @@ class QuasiEuclideanSDFGenerator { }; } // namespace wavemap -#endif // WAVEMAP_UTILS_SDF_SDF_GENERATOR_H_ +#endif // WAVEMAP_UTILS_SDF_QUASI_EUCLIDEAN_SDF_GENERATOR_H_ diff --git a/libraries/wavemap/src/data_structure/volumetric/volumetric_octree.cc b/libraries/wavemap/src/data_structure/volumetric/volumetric_octree.cc index 4d7257bb6..4af6e24bc 100644 --- a/libraries/wavemap/src/data_structure/volumetric/volumetric_octree.cc +++ b/libraries/wavemap/src/data_structure/volumetric/volumetric_octree.cc @@ -1,5 +1,7 @@ #include "wavemap/data_structure/volumetric/volumetric_octree.h" +#include "wavemap/utils/query/occupancy_classifier.h" + namespace wavemap { DECLARE_CONFIG_MEMBERS(VolumetricOctreeConfig, (min_cell_width) @@ -85,7 +87,7 @@ Index3D VolumetricOctree::getMinIndex() const { Index3D min_index = getMaxPossibleIndex(); forEachLeaf([&min_index](const OctreeIndex& node_index, FloatingPoint value) { - if (OccupancyState::isObserved(value)) { + if (OccupancyClassifier::isObserved(value)) { const Index3D index = convert::nodeIndexToMinCornerIndex(node_index); min_index = min_index.cwiseMin(index); } @@ -100,7 +102,7 @@ Index3D VolumetricOctree::getMaxIndex() const { Index3D max_index = getMinPossibleIndex(); forEachLeaf([&max_index](const OctreeIndex& node_index, FloatingPoint value) { - if (OccupancyState::isObserved(value)) { + if (OccupancyClassifier::isObserved(value)) { const Index3D index = convert::nodeIndexToMaxCornerIndex(node_index); max_index = max_index.cwiseMax(index); } diff --git a/libraries/wavemap/src/data_structure/volumetric/wavelet_octree.cc b/libraries/wavemap/src/data_structure/volumetric/wavelet_octree.cc index 353f2e167..f9ac98c09 100644 --- a/libraries/wavemap/src/data_structure/volumetric/wavelet_octree.cc +++ b/libraries/wavemap/src/data_structure/volumetric/wavelet_octree.cc @@ -1,5 +1,7 @@ #include "wavemap/data_structure/volumetric/wavelet_octree.h" +#include "wavemap/utils/query/occupancy_classifier.h" + namespace wavemap { DECLARE_CONFIG_MEMBERS(WaveletOctreeConfig, (min_cell_width) @@ -34,7 +36,7 @@ Index3D WaveletOctree::getMinIndex() const { Index3D min_index = getMaxPossibleIndex(); forEachLeaf([&min_index](const OctreeIndex& node_index, FloatingPoint value) { - if (OccupancyState::isObserved(value)) { + if (OccupancyClassifier::isObserved(value)) { const Index3D index = convert::nodeIndexToMinCornerIndex(node_index); min_index = min_index.cwiseMin(index); } @@ -49,7 +51,7 @@ Index3D WaveletOctree::getMaxIndex() const { Index3D max_index = getMinPossibleIndex(); forEachLeaf([&max_index](const OctreeIndex& node_index, FloatingPoint value) { - if (OccupancyState::isObserved(value)) { + if (OccupancyClassifier::isObserved(value)) { const Index3D index = convert::nodeIndexToMaxCornerIndex(node_index); max_index = max_index.cwiseMax(index); } diff --git a/libraries/wavemap/src/utils/sdf/sdf_generator.cc b/libraries/wavemap/src/utils/sdf/sdf_generator.cc index 8e8ef379f..d42d92fbe 100644 --- a/libraries/wavemap/src/utils/sdf/sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/sdf_generator.cc @@ -1,7 +1,7 @@ -#include "wavemap/utils/sdf/sdf_generator.h" - #include +#include "wavemap/utils/sdf/quasi_euclidean_sdf_generator.h" + namespace wavemap { std::array neighborhood::generateNeighborIndexOffsets() { std::array neighbor_offsets{}; @@ -64,7 +64,8 @@ void QuasiEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, const OctreeIndex& node_index, FloatingPoint node_occupancy) { // Only process obstacles - if (isUnobserved(node_occupancy) || isFree(node_occupancy)) { + if (OccupancyClassifier::isUnobserved(node_occupancy) || + classifier_.isFree(node_occupancy)) { return; } @@ -90,7 +91,8 @@ void QuasiEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, // Skip the cell if it is not free const FloatingPoint occupancy = occupancy_query_accelerator.getCellValue(index); - if (isUnobserved(occupancy) || isOccupied(occupancy)) { + if (OccupancyClassifier::isUnobserved(occupancy) || + classifier_.isOccupied(occupancy)) { continue; } @@ -158,11 +160,11 @@ void QuasiEuclideanSDFGenerator::propagate( const FloatingPoint neighbor_occupancy = occupancy_query_accelerator.getCellValue(neighbor_index); // Never initialize or update unknown cells - if (!isObserved(neighbor_occupancy)) { + if (!OccupancyClassifier::isObserved(neighbor_occupancy)) { continue; } // Set the sign - if (isOccupied(neighbor_occupancy)) { + if (classifier_.isOccupied(neighbor_occupancy)) { neighbor_sdf = -sdf.getDefaultCellValue(); } } diff --git a/libraries/wavemap/test/src/utils/test_sdf_generation.cc b/libraries/wavemap/test/src/utils/test_sdf_generation.cc index 6a2520558..218e727cd 100644 --- a/libraries/wavemap/test/src/utils/test_sdf_generation.cc +++ b/libraries/wavemap/test/src/utils/test_sdf_generation.cc @@ -6,7 +6,7 @@ #include "wavemap/test/config_generator.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" -#include "wavemap/utils/sdf/sdf_generator.h" +#include "wavemap/utils/sdf/quasi_euclidean_sdf_generator.h" namespace wavemap { class SdfGenerationTest : public FixtureBase, @@ -19,10 +19,11 @@ TEST_F(SdfGenerationTest, QuasiEuclideanSDFGenerator) { const Index3D max_index = Index3D::Constant(100); constexpr FloatingPoint kMaxSdfDistance = 2.f; - // Create the map + // Create the map and occupancy classification util const auto config = ConfigGenerator::getRandomConfig(); HashedWaveletOctree map{config}; + const OccupancyClassifier classifier{}; // Generate random obstacles auto obstacle_cells = @@ -56,12 +57,12 @@ TEST_F(SdfGenerationTest, QuasiEuclideanSDFGenerator) { const auto sdf = sdf_generator.generate(map); // Compare the SDF distances to the brute force min distance - sdf.forEachLeaf([&map, &sdf_generator, &sdf, &obstacle_cells, min_cell_width, - padding](const OctreeIndex& node_index, - FloatingPoint sdf_value) { + sdf.forEachLeaf([&map, &classifier, &sdf_generator, &sdf, &obstacle_cells, + min_cell_width, padding](const OctreeIndex& node_index, + FloatingPoint sdf_value) { // In unobserved space, the SDF should be uninitialized const FloatingPoint occupancy_value = map.getCellValue(node_index); - if (QuasiEuclideanSDFGenerator::isUnobserved(occupancy_value)) { + if (OccupancyClassifier::isUnobserved(occupancy_value)) { // In unknown space the SDF should be uninitialized EXPECT_NEAR(sdf_value, sdf.getDefaultCellValue(), kEpsilon); return; @@ -72,7 +73,7 @@ TEST_F(SdfGenerationTest, QuasiEuclideanSDFGenerator) { // Find the closest surface using brute force FloatingPoint sdf_brute_force = sdf.getDefaultCellValue(); - if (sdf_generator.isFree(occupancy_value)) { + if (classifier.isFree(occupancy_value)) { // In free space, the SDF should always be positive EXPECT_GT(sdf_value, 0.f); @@ -90,8 +91,8 @@ TEST_F(SdfGenerationTest, QuasiEuclideanSDFGenerator) { node_index.position.array() + padding)) { const FloatingPoint neighbor_occupancy_value = map.getCellValue(neighbor_index); - if (sdf_generator.isFree(neighbor_occupancy_value) && - QuasiEuclideanSDFGenerator::isObserved(neighbor_occupancy_value)) { + if (classifier.isFree(neighbor_occupancy_value) && + OccupancyClassifier::isObserved(neighbor_occupancy_value)) { const auto free_cell_aabb = convert::nodeIndexToAABB( OctreeIndex{0, neighbor_index}, min_cell_width); sdf_brute_force = std::min(free_cell_aabb.minDistanceTo(node_center), From 393a3dba378566957873ea43e7d2cbfc58207f6a Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 20 Oct 2023 11:01:43 +0200 Subject: [PATCH 004/159] Major refactoring to make the spatial data structures reusable --- examples/src/queries/accelerated_queries.cc | 2 +- examples/src/queries/fixed_resolution.cc | 2 +- examples/src/queries/interpolation.cc | 2 +- examples/src/queries/multi_resolution.cc | 2 +- libraries/wavemap/CMakeLists.txt | 21 +-- .../benchmark/benchmark_haar_transforms.cc | 4 +- .../wavemap/config/impl/config_base_inl.h | 8 +- .../sdf => data_structure}/bucket_queue.h | 10 +- .../wavemap/data_structure/dense_grid.h | 62 ++++++++ .../impl/bucket_queue_inl.h | 8 +- .../data_structure/impl/spatial_hash_inl.h | 148 ++++++++++++++++++ .../wavemap/data_structure/spatial_hash.h | 66 ++++++++ .../volumetric/impl/hashed_blocks_inl.h | 102 ------------ .../wavemap/indexing/index_conversions.h | 18 ++- .../include/wavemap/indexing/ndtree_index.h | 3 +- .../wavemap/integrator/integrator_factory.h | 2 +- .../coarse_to_fine_integrator.h | 2 +- .../hashed_chunked_wavelet_integrator.h | 4 +- .../hashed_wavelet_integrator.h | 4 +- .../hierarchical_range_bounds.h | 7 +- .../hashed_chunked_wavelet_integrator_inl.h | 6 +- .../impl/hashed_wavelet_integrator_inl.h | 6 +- .../coarse_to_fine/wavelet_integrator.h | 4 +- .../projective/projective_integrator.h | 4 +- .../ray_tracing/ray_tracing_integrator.h | 2 +- .../cell_types/haar_coefficients.h | 6 +- .../cell_types/haar_transform.h | 10 +- .../cell_types/impl/haar_transform_inl.h | 55 +++---- .../volumetric => map}/hashed_blocks.h | 72 +++++---- .../hashed_chunked_wavelet_octree.h | 50 +++--- .../hashed_chunked_wavelet_octree_block.h | 14 +- .../hashed_wavelet_octree.h | 48 ++++-- .../hashed_wavelet_octree_block.h | 14 +- .../wavemap/map/impl/hashed_blocks_inl.h | 69 ++++++++ .../hashed_chunked_wavelet_octree_block_inl.h | 6 +- .../impl/hashed_chunked_wavelet_octree_inl.h | 63 ++++---- .../impl/hashed_wavelet_octree_block_inl.h | 6 +- .../impl/hashed_wavelet_octree_inl.h | 63 ++++---- .../impl/volumetric_octree_inl.h | 6 +- .../impl/wavelet_octree_inl.h | 6 +- .../volumetric_data_structure_base.h | 6 +- .../volumetric_data_structure_factory.h | 8 +- .../volumetric => map}/volumetric_octree.h | 10 +- .../volumetric => map}/wavelet_octree.h | 12 +- .../wavemap/utils/bits/bit_operations.h | 16 +- .../include/wavemap/utils/data/comparisons.h | 27 ++++ .../include/wavemap/utils/data/eigen_checks.h | 94 +++++++++++ .../include/wavemap/utils/math/int_math.h | 11 +- .../wavemap/utils/query/map_interpolator.h | 2 +- .../wavemap/utils/query/query_accelerator.h | 18 +-- .../wavemap/utils/sdf/cell_neighborhoods.h | 12 ++ .../utils/sdf/quasi_euclidean_sdf_generator.h | 15 +- .../volumetric/hashed_blocks.cc | 65 -------- .../hashed_chunked_wavelet_octree.cc | 99 ------------ .../volumetric/hashed_wavelet_octree.cc | 98 ------------ .../hashed_chunked_wavelet_integrator.cc | 5 +- .../hashed_wavelet_integrator.cc | 5 +- libraries/wavemap/src/map/hashed_blocks.cc | 42 +++++ .../src/map/hashed_chunked_wavelet_octree.cc | 83 ++++++++++ .../hashed_chunked_wavelet_octree_block.cc | 2 +- .../wavemap/src/map/hashed_wavelet_octree.cc | 82 ++++++++++ .../hashed_wavelet_octree_block.cc | 2 +- .../volumetric_data_structure_base.cc | 2 +- .../volumetric_data_structure_factory.cc | 12 +- .../volumetric => map}/volumetric_octree.cc | 2 +- .../volumetric => map}/wavelet_octree.cc | 2 +- .../src/utils/sdf/cell_neighborhoods.cc | 29 ++++ ...or.cc => quasi_euclidean_sdf_generator.cc} | 33 +--- .../include/wavemap/test/config_generator.h | 10 +- .../test/include/wavemap/test/eigen_utils.h | 55 ++++--- .../test/src/data_structure/test_haar_cell.cc | 2 +- .../src/data_structure/test_hashed_blocks.cc | 2 +- .../test_volumetric_data_structure.cc | 27 ++-- .../data_structure/test_volumetric_octree.cc | 2 +- .../src/indexing/test_index_conversions.cc | 12 +- .../test/src/indexing/test_ndtree_index.cc | 2 +- .../test_hierarchical_range_image.cc | 5 +- .../integrator/test_pointcloud_integrators.cc | 10 +- .../test_range_image_intersector.cc | 5 +- .../wavemap/test/src/utils/test_int_math.cc | 18 ++- .../test/src/utils/test_map_interpolator.cpp | 2 +- .../test/src/utils/test_query_accelerator.cc | 4 +- .../test/src/utils/test_sdf_generation.cc | 4 +- .../include/wavemap_io/file_conversions.h | 2 +- .../include/wavemap_io/stream_conversions.h | 8 +- .../wavemap_io/src/stream_conversions.cc | 15 +- .../test/src/test_file_conversions.cc | 8 +- .../wavemap_ros/impl/wavemap_server_inl.h | 23 +-- .../wavemap_ros/input_handler/input_handler.h | 2 +- .../include/wavemap_ros/wavemap_server.h | 2 +- .../input_handler/pointcloud_input_handler.cc | 5 +- ros/wavemap_ros/src/rosbag_processor.cc | 3 +- ros/wavemap_ros/src/wavemap_server.cc | 2 +- .../map_msg_conversions.h | 8 +- .../src/map_msg_conversions.cc | 81 +++++----- .../test/src/test_map_msg_conversions.cc | 8 +- .../include/wavemap_rviz_plugin/common.h | 2 +- .../visuals/cell_selector.h | 4 +- .../visuals/slice_visual.h | 2 +- .../visuals/voxel_visual.h | 8 +- .../src/visuals/slice_visual.cc | 2 +- .../src/visuals/voxel_visual.cc | 139 ++++++++-------- 102 files changed, 1331 insertions(+), 934 deletions(-) rename libraries/wavemap/include/wavemap/{utils/sdf => data_structure}/bucket_queue.h (82%) create mode 100644 libraries/wavemap/include/wavemap/data_structure/dense_grid.h rename libraries/wavemap/include/wavemap/{utils/sdf => data_structure}/impl/bucket_queue_inl.h (90%) create mode 100644 libraries/wavemap/include/wavemap/data_structure/impl/spatial_hash_inl.h create mode 100644 libraries/wavemap/include/wavemap/data_structure/spatial_hash.h delete mode 100644 libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_blocks_inl.h rename libraries/wavemap/include/wavemap/{data_structure/volumetric => map}/cell_types/haar_coefficients.h (95%) rename libraries/wavemap/include/wavemap/{data_structure/volumetric => map}/cell_types/haar_transform.h (93%) rename libraries/wavemap/include/wavemap/{data_structure/volumetric => map}/cell_types/impl/haar_transform_inl.h (74%) rename libraries/wavemap/include/wavemap/{data_structure/volumetric => map}/hashed_blocks.h (51%) rename libraries/wavemap/include/wavemap/{data_structure/volumetric => map}/hashed_chunked_wavelet_octree.h (76%) rename libraries/wavemap/include/wavemap/{data_structure/volumetric => map}/hashed_chunked_wavelet_octree_block.h (85%) rename libraries/wavemap/include/wavemap/{data_structure/volumetric => map}/hashed_wavelet_octree.h (76%) rename libraries/wavemap/include/wavemap/{data_structure/volumetric => map}/hashed_wavelet_octree_block.h (84%) create mode 100644 libraries/wavemap/include/wavemap/map/impl/hashed_blocks_inl.h rename libraries/wavemap/include/wavemap/{data_structure/volumetric => map}/impl/hashed_chunked_wavelet_octree_block_inl.h (90%) rename libraries/wavemap/include/wavemap/{data_structure/volumetric => map}/impl/hashed_chunked_wavelet_octree_inl.h (60%) rename libraries/wavemap/include/wavemap/{data_structure/volumetric => map}/impl/hashed_wavelet_octree_block_inl.h (84%) rename libraries/wavemap/include/wavemap/{data_structure/volumetric => map}/impl/hashed_wavelet_octree_inl.h (59%) rename libraries/wavemap/include/wavemap/{data_structure/volumetric => map}/impl/volumetric_octree_inl.h (93%) rename libraries/wavemap/include/wavemap/{data_structure/volumetric => map}/impl/wavelet_octree_inl.h (96%) rename libraries/wavemap/include/wavemap/{data_structure/volumetric => map}/volumetric_data_structure_base.h (93%) rename libraries/wavemap/include/wavemap/{data_structure/volumetric => map}/volumetric_data_structure_factory.h (58%) rename libraries/wavemap/include/wavemap/{data_structure/volumetric => map}/volumetric_octree.h (93%) rename libraries/wavemap/include/wavemap/{data_structure/volumetric => map}/wavelet_octree.h (93%) create mode 100644 libraries/wavemap/include/wavemap/utils/data/eigen_checks.h create mode 100644 libraries/wavemap/include/wavemap/utils/sdf/cell_neighborhoods.h delete mode 100644 libraries/wavemap/src/data_structure/volumetric/hashed_blocks.cc delete mode 100644 libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree.cc delete mode 100644 libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree.cc create mode 100644 libraries/wavemap/src/map/hashed_blocks.cc create mode 100644 libraries/wavemap/src/map/hashed_chunked_wavelet_octree.cc rename libraries/wavemap/src/{data_structure/volumetric => map}/hashed_chunked_wavelet_octree_block.cc (99%) create mode 100644 libraries/wavemap/src/map/hashed_wavelet_octree.cc rename libraries/wavemap/src/{data_structure/volumetric => map}/hashed_wavelet_octree_block.cc (98%) rename libraries/wavemap/src/{data_structure/volumetric => map}/volumetric_data_structure_base.cc (86%) rename libraries/wavemap/src/{data_structure/volumetric => map}/volumetric_data_structure_factory.cc (87%) rename libraries/wavemap/src/{data_structure/volumetric => map}/volumetric_octree.cc (98%) rename libraries/wavemap/src/{data_structure/volumetric => map}/wavelet_octree.cc (98%) create mode 100644 libraries/wavemap/src/utils/sdf/cell_neighborhoods.cc rename libraries/wavemap/src/utils/sdf/{sdf_generator.cc => quasi_euclidean_sdf_generator.cc} (88%) diff --git a/examples/src/queries/accelerated_queries.cc b/examples/src/queries/accelerated_queries.cc index 9683ec3c7..aa8d54b90 100644 --- a/examples/src/queries/accelerated_queries.cc +++ b/examples/src/queries/accelerated_queries.cc @@ -1,4 +1,4 @@ -#include +#include #include #include diff --git a/examples/src/queries/fixed_resolution.cc b/examples/src/queries/fixed_resolution.cc index 8281570b5..0b3717fa1 100644 --- a/examples/src/queries/fixed_resolution.cc +++ b/examples/src/queries/fixed_resolution.cc @@ -1,4 +1,4 @@ -#include +#include #include "wavemap_examples/common.h" diff --git a/examples/src/queries/interpolation.cc b/examples/src/queries/interpolation.cc index 8fcd20e82..eedb57190 100644 --- a/examples/src/queries/interpolation.cc +++ b/examples/src/queries/interpolation.cc @@ -1,4 +1,4 @@ -#include +#include #include #include "wavemap_examples/common.h" diff --git a/examples/src/queries/multi_resolution.cc b/examples/src/queries/multi_resolution.cc index 341dd05a7..6aa0abe21 100644 --- a/examples/src/queries/multi_resolution.cc +++ b/examples/src/queries/multi_resolution.cc @@ -1,5 +1,5 @@ -#include #include +#include #include "wavemap_examples/common.h" diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index 6006a0971..6f3e40da2 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -39,15 +39,6 @@ include_directories(include ${catkin_INCLUDE_DIRS}) # cmake-lint: disable=C0301 add_library(${PROJECT_NAME} src/config/value_with_unit.cc - src/data_structure/volumetric/hashed_blocks.cc - src/data_structure/volumetric/hashed_chunked_wavelet_octree.cc - src/data_structure/volumetric/hashed_chunked_wavelet_octree_block.cc - src/data_structure/volumetric/hashed_wavelet_octree.cc - src/data_structure/volumetric/hashed_wavelet_octree_block.cc - src/data_structure/volumetric/volumetric_octree.cc - src/data_structure/volumetric/wavelet_octree.cc - src/data_structure/volumetric/volumetric_data_structure_base.cc - src/data_structure/volumetric/volumetric_data_structure_factory.cc src/integrator/measurement_model/continuous_beam.cc src/integrator/measurement_model/constant_ray.cc src/integrator/measurement_model/continuous_ray.cc @@ -66,7 +57,17 @@ add_library(${PROJECT_NAME} src/integrator/ray_tracing/ray_tracing_integrator.cc src/integrator/integrator_base.cc src/integrator/integrator_factory.cc - src/utils/sdf/sdf_generator.cc + src/map/hashed_blocks.cc + src/map/hashed_chunked_wavelet_octree.cc + src/map/hashed_chunked_wavelet_octree_block.cc + src/map/hashed_wavelet_octree.cc + src/map/hashed_wavelet_octree_block.cc + src/map/volumetric_octree.cc + src/map/wavelet_octree.cc + src/map/volumetric_data_structure_base.cc + src/map/volumetric_data_structure_factory.cc + src/utils/sdf/cell_neighborhoods.cc + src/utils/sdf/quasi_euclidean_sdf_generator.cc src/utils/stopwatch.cc src/utils/thread_pool.cc) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/libraries/wavemap/benchmark/benchmark_haar_transforms.cc b/libraries/wavemap/benchmark/benchmark_haar_transforms.cc index 830aec4ff..39e367bf9 100644 --- a/libraries/wavemap/benchmark/benchmark_haar_transforms.cc +++ b/libraries/wavemap/benchmark/benchmark_haar_transforms.cc @@ -1,7 +1,7 @@ #include -#include "wavemap/data_structure/volumetric/cell_types/haar_coefficients.h" -#include "wavemap/data_structure/volumetric/cell_types/haar_transform.h" +#include "wavemap/map/cell_types/haar_coefficients.h" +#include "wavemap/map/cell_types/haar_transform.h" #include "wavemap/utils/random_number_generator.h" namespace wavemap { diff --git a/libraries/wavemap/include/wavemap/config/impl/config_base_inl.h b/libraries/wavemap/include/wavemap/config/impl/config_base_inl.h index dc37a71cf..228ac366e 100644 --- a/libraries/wavemap/include/wavemap/config/impl/config_base_inl.h +++ b/libraries/wavemap/include/wavemap/config/impl/config_base_inl.h @@ -146,11 +146,15 @@ ConfigBase::from( // See if the current param name matches one of the config's members const auto& member_it = std::find_if( member_map.begin(), member_map.end(), - [&](const auto& member) { return member.name == param_name; }); + [¶m_name = std::as_const(param_name)](const auto& member) { + return member.name == param_name; + }); // If so, load it if (member_it != member_map.end()) { - auto param_loader = [&](auto&& ptr) { + auto param_loader = [¶m_name = std::as_const(param_name), + ¶m_value = std::as_const(param_value), + &config](auto&& ptr) { detail::loadParam(param_name, param_value, config, ptr); }; std::visit(param_loader, member_it->ptr); diff --git a/libraries/wavemap/include/wavemap/utils/sdf/bucket_queue.h b/libraries/wavemap/include/wavemap/data_structure/bucket_queue.h similarity index 82% rename from libraries/wavemap/include/wavemap/utils/sdf/bucket_queue.h rename to libraries/wavemap/include/wavemap/data_structure/bucket_queue.h index 0d5d0176c..209184a68 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/bucket_queue.h +++ b/libraries/wavemap/include/wavemap/data_structure/bucket_queue.h @@ -1,11 +1,11 @@ -#ifndef WAVEMAP_UTILS_SDF_BUCKET_QUEUE_H_ -#define WAVEMAP_UTILS_SDF_BUCKET_QUEUE_H_ +#ifndef WAVEMAP_DATA_STRUCTURE_BUCKET_QUEUE_H_ +#define WAVEMAP_DATA_STRUCTURE_BUCKET_QUEUE_H_ #include #include #include -#include +#include namespace wavemap { /** @@ -44,6 +44,6 @@ class BucketQueue { }; } // namespace wavemap -#include "wavemap/utils/sdf/impl/bucket_queue_inl.h" +#include "wavemap/data_structure/impl/bucket_queue_inl.h" -#endif // WAVEMAP_UTILS_SDF_BUCKET_QUEUE_H_ +#endif // WAVEMAP_DATA_STRUCTURE_BUCKET_QUEUE_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/dense_grid.h b/libraries/wavemap/include/wavemap/data_structure/dense_grid.h new file mode 100644 index 000000000..4719b1c50 --- /dev/null +++ b/libraries/wavemap/include/wavemap/data_structure/dense_grid.h @@ -0,0 +1,62 @@ +#ifndef WAVEMAP_DATA_STRUCTURE_DENSE_GRID_H_ +#define WAVEMAP_DATA_STRUCTURE_DENSE_GRID_H_ + +#include "wavemap/common.h" +#include "wavemap/indexing/index_conversions.h" +#include "wavemap/utils/data/eigen_checks.h" +#include "wavemap/utils/math/int_math.h" + +namespace wavemap { +template +class DenseGrid { + private: + static_assert(int_math::is_power_of_two(cells_per_side), + "Cells per side must be an exact power of 2."); + static constexpr IndexElement kCellsPerSideLog2 = + int_math::log2_floor(cells_per_side); + + public: + static constexpr IndexElement kDim = dim; + static constexpr IndexElement kCellsPerSide = cells_per_side; + static constexpr IndexElement kCellsPerBlock = + int_math::exp2(dim * kCellsPerSideLog2); + + using DataArrayType = std::array; + + explicit DenseGrid(const CellDataT& default_value) { + if (default_value != CellDataT{}) { + data_.fill(default_value); + } + } + + CellDataT& operator[](size_t linear_index) { + DCHECK_GE(linear_index, 0); + DCHECK_LT(linear_index, kCellsPerBlock); + return data_[linear_index]; + } + const CellDataT& operator[](size_t linear_index) const { + DCHECK_GE(linear_index, 0); + DCHECK_LT(linear_index, kCellsPerBlock); + return data_[linear_index]; + } + + CellDataT& at(const Index& index) { + DCHECK_EIGEN_GE(index, Index::Zero()); + DCHECK_EIGEN_LT(index, Index::Constant(kCellsPerSide)); + return data_[convert::indexToLinearIndex(index)]; + } + const CellDataT& at(const Index& index) const { + DCHECK_EIGEN_GE(index, Index::Zero()); + DCHECK_EIGEN_LT(index, Index::Constant(kCellsPerSide)); + return data_[convert::indexToLinearIndex(index)]; + } + + DataArrayType& data() { return data_; } + const DataArrayType& data() const { return data_; } + + private: + DataArrayType data_{}; +}; +} // namespace wavemap + +#endif // WAVEMAP_DATA_STRUCTURE_DENSE_GRID_H_ diff --git a/libraries/wavemap/include/wavemap/utils/sdf/impl/bucket_queue_inl.h b/libraries/wavemap/include/wavemap/data_structure/impl/bucket_queue_inl.h similarity index 90% rename from libraries/wavemap/include/wavemap/utils/sdf/impl/bucket_queue_inl.h rename to libraries/wavemap/include/wavemap/data_structure/impl/bucket_queue_inl.h index f7fd4175c..98d9a7a72 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/impl/bucket_queue_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/impl/bucket_queue_inl.h @@ -1,5 +1,7 @@ -#ifndef WAVEMAP_UTILS_SDF_IMPL_BUCKET_QUEUE_INL_H_ -#define WAVEMAP_UTILS_SDF_IMPL_BUCKET_QUEUE_INL_H_ +#ifndef WAVEMAP_DATA_STRUCTURE_IMPL_BUCKET_QUEUE_INL_H_ +#define WAVEMAP_DATA_STRUCTURE_IMPL_BUCKET_QUEUE_INL_H_ + +#include namespace wavemap { template @@ -73,4 +75,4 @@ ValueT BucketQueue::front() { } } // namespace wavemap -#endif // WAVEMAP_UTILS_SDF_IMPL_BUCKET_QUEUE_INL_H_ +#endif // WAVEMAP_DATA_STRUCTURE_IMPL_BUCKET_QUEUE_INL_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/impl/spatial_hash_inl.h b/libraries/wavemap/include/wavemap/data_structure/impl/spatial_hash_inl.h new file mode 100644 index 000000000..055418568 --- /dev/null +++ b/libraries/wavemap/include/wavemap/data_structure/impl/spatial_hash_inl.h @@ -0,0 +1,148 @@ +#ifndef WAVEMAP_DATA_STRUCTURE_IMPL_SPATIAL_HASH_INL_H_ +#define WAVEMAP_DATA_STRUCTURE_IMPL_SPATIAL_HASH_INL_H_ + +#include +#include + +namespace wavemap { +namespace convert { +template +Index indexToBlockIndex(const Index& index) { + static_assert(int_math::is_power_of_two(cells_per_side), + "Cells per side must be an exact power of 2."); + static constexpr IndexElement kCellsPerSideLog2 = + int_math::log2_floor(cells_per_side); + return int_math::div_exp2_floor(index, kCellsPerSideLog2); +} + +template +Index indexToCellIndex(const Index& index) { + static_assert(int_math::is_power_of_two(cells_per_side), + "Cells per side must be an exact power of 2."); + static constexpr IndexElement kCellsPerSideLog2 = + int_math::log2_floor(cells_per_side); + constexpr IndexElement mask = (1 << kCellsPerSideLog2) - 1; + Index cell_index{}; + for (int dim_idx = 0; dim_idx < dim; ++dim_idx) { + cell_index[dim_idx] = index[dim_idx] & mask; + } + return cell_index; +} + +template +Index cellAndBlockIndexToIndex(const Index& block_index, + const Index& cell_index) { + return cells_per_side * block_index + cell_index; +} +} // namespace convert + +template +Index SpatialHash::getMinBlockIndex() const { + if (empty()) { + return Index::Zero(); + } + + BlockIndex min_block_index = + BlockIndex::Constant(std::numeric_limits::max()); + forEachBlock([&min_block_index](const BlockIndex& block_index, + const BlockData& /*block*/) { + min_block_index = min_block_index.cwiseMin(block_index); + }); + return min_block_index; +} + +template +Index SpatialHash::getMaxBlockIndex() const { + if (empty()) { + return Index::Zero(); + } + + BlockIndex max_block_index = + BlockIndex::Constant(std::numeric_limits::lowest()); + forEachBlock([&max_block_index](const BlockIndex& block_index, + const BlockData& /*block*/) { + max_block_index = max_block_index.cwiseMax(block_index); + }); + return max_block_index; +} + +template +bool SpatialHash::hasBlock( + const SpatialHash::BlockIndex& block_index) const { + return block_map_.count(block_index); +} + +template +typename SpatialHash::BlockData* +SpatialHash::getBlock( + const SpatialHash::BlockIndex& block_index) { + const auto& it = block_map_.find(block_index); + if (it != block_map_.end()) { + return &it->second; + } else { + return nullptr; + } +} + +template +const typename SpatialHash::BlockData* +SpatialHash::getBlock( + const SpatialHash::BlockIndex& block_index) const { + const auto& it = block_map_.find(block_index); + if (it != block_map_.end()) { + return &it->second; + } else { + return nullptr; + } +} + +template +template +typename SpatialHash::BlockData& +SpatialHash::getOrAllocateBlock( + const SpatialHash::BlockIndex& block_index, Args... args) { + return block_map_.try_emplace(block_index, std::forward(args)...) + .first->second; +} + +template +void SpatialHash::eraseBlock( + const SpatialHash::BlockIndex& block_index) { + block_map_.erase(block_index); +} + +template +template +void SpatialHash::eraseBlockIf( + IndexedBlockVisitor indicator_fn) { + for (auto it = block_map_.begin(); it != block_map_.end();) { + const BlockIndex& block_index = it->first; + BlockData& block = it->second; + if (std::invoke(indicator_fn, block_index, block)) { + it = block_map_.erase(it); + } else { + ++it; + } + } +} + +template +template +void SpatialHash::forEachBlock( + IndexedBlockVisitor visitor_fn) { + for (auto& [block_index, block_data] : block_map_) { + std::invoke(visitor_fn, block_index, block_data); + } +} + +template +template +void SpatialHash::forEachBlock( + IndexedBlockVisitor visitor_fn) const { + for (const auto& [block_index, block_data] : block_map_) { + std::invoke(visitor_fn, block_index, block_data); + } +} +} // namespace wavemap + +#endif // WAVEMAP_DATA_STRUCTURE_IMPL_SPATIAL_HASH_INL_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/spatial_hash.h b/libraries/wavemap/include/wavemap/data_structure/spatial_hash.h new file mode 100644 index 000000000..7a2ad9583 --- /dev/null +++ b/libraries/wavemap/include/wavemap/data_structure/spatial_hash.h @@ -0,0 +1,66 @@ +#ifndef WAVEMAP_DATA_STRUCTURE_SPATIAL_HASH_H_ +#define WAVEMAP_DATA_STRUCTURE_SPATIAL_HASH_H_ + +#include + +#include "wavemap/common.h" +#include "wavemap/indexing/index_conversions.h" +#include "wavemap/indexing/index_hashes.h" +#include "wavemap/utils/math/int_math.h" + +namespace wavemap { +namespace convert { +template +Index indexToBlockIndex(const Index& index); + +template +Index indexToCellIndex(const Index& index); + +template +Index cellAndBlockIndexToIndex(const Index& block_index, + const Index& cell_index); +} // namespace convert + +template +class SpatialHash { + public: + static constexpr IndexElement kDim = dim; + + using BlockIndex = Index; + using CellIndex = Index; + using BlockData = BlockDataT; + + bool empty() const { return block_map_.empty(); } + size_t size() const { return block_map_.size(); } + void clear() { block_map_.clear(); } + + Index getMinBlockIndex() const; + Index getMaxBlockIndex() const; + + bool hasBlock(const BlockIndex& block_index) const; + + BlockData* getBlock(const BlockIndex& block_index); + const BlockData* getBlock(const BlockIndex& block_index) const; + template + BlockData& getOrAllocateBlock(const BlockIndex& block_index, Args... args); + + void eraseBlock(const BlockIndex& block_index); + template + void eraseBlockIf(IndexedBlockVisitor indicator_fn); + + auto& getHashMap() { return block_map_; } + const auto& getHashMap() const { return block_map_; } + + template + void forEachBlock(IndexedBlockVisitor visitor_fn); + template + void forEachBlock(IndexedBlockVisitor visitor_fn) const; + + private: + std::unordered_map, BlockDataT, IndexHash> block_map_; +}; +} // namespace wavemap + +#include "wavemap/data_structure/impl/spatial_hash_inl.h" + +#endif // WAVEMAP_DATA_STRUCTURE_SPATIAL_HASH_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_blocks_inl.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_blocks_inl.h deleted file mode 100644 index f16954b85..000000000 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_blocks_inl.h +++ /dev/null @@ -1,102 +0,0 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_HASHED_BLOCKS_INL_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_HASHED_BLOCKS_INL_H_ - -#include -#include -#include - -#include "wavemap/indexing/index_conversions.h" -#include "wavemap/utils/iterate/grid_iterator.h" - -namespace wavemap { -inline FloatingPoint HashedBlocks::getCellValue(const Index3D& index) const { - const FloatingPoint* cell_data = accessCellData(index); - if (cell_data) { - return static_cast(*cell_data); - } - return default_value_; -} - -inline void HashedBlocks::setCellValue(const Index3D& index, - FloatingPoint new_value) { - constexpr bool kAutoAllocate = true; - FloatingPoint* cell_data = accessCellData(index, kAutoAllocate); - if (cell_data) { - // TODO(victorr): Decide whether truncation should be applied here as well - *cell_data = new_value; - } else { - LOG(ERROR) << "Failed to allocate cell at index: " << index; - } -} - -inline void HashedBlocks::addToCellValue(const Index3D& index, - FloatingPoint update) { - constexpr bool kAutoAllocate = true; - FloatingPoint* cell_data = accessCellData(index, kAutoAllocate); - if (cell_data) { - *cell_data = clampedAdd(*cell_data, update); - } else { - LOG(ERROR) << "Failed to allocate cell at index: " << index; - } -} - -inline HashedBlocks::Block& HashedBlocks::getOrAllocateBlock( - const Index3D& block_index) { - if (!hasBlock(block_index)) { - auto it = blocks_.try_emplace(block_index).first; - it->second.fill(default_value_); - } - return blocks_.at(block_index); -} - -inline FloatingPoint* HashedBlocks::accessCellData(const Index3D& index, - bool auto_allocate) { - BlockIndex block_index = computeBlockIndexFromIndex(index); - if (!auto_allocate && !hasBlock(block_index)) { - return nullptr; - } - Block& block = getOrAllocateBlock(block_index); - CellIndex cell_index = - computeCellIndexFromBlockIndexAndIndex(block_index, index); - return &block[convert::indexToLinearIndex(cell_index)]; -} - -inline const FloatingPoint* HashedBlocks::accessCellData( - const Index3D& index) const { - BlockIndex block_index = computeBlockIndexFromIndex(index); - const auto& it = blocks_.find(block_index); - if (it != blocks_.end()) { - CellIndex cell_index = - computeCellIndexFromBlockIndexAndIndex(block_index, index); - return &it->second[convert::indexToLinearIndex(cell_index)]; - } - return nullptr; -} - -inline HashedBlocks::BlockIndex HashedBlocks::computeBlockIndexFromIndex( - const Index3D& index) { - return int_math::div_exp2_floor(index, kCellsPerSideLog2); -} - -inline HashedBlocks::CellIndex -HashedBlocks::computeCellIndexFromBlockIndexAndIndex( - const HashedBlocks::BlockIndex& block_index, const Index3D& index) { - const Index3D origin = kCellsPerSide * block_index; - Index3D cell_index = index - origin; - - DCHECK((0 <= cell_index.array() && cell_index.array() < kCellsPerSide).all()) - << "(Local) cell indices should always be within range [0, " - "kCellsPerSide[, but computed index equals " - << cell_index << " instead."; - - return cell_index; -} - -inline Index3D HashedBlocks::computeIndexFromBlockIndexAndCellIndex( - const HashedBlocks::BlockIndex& block_index, - const HashedBlocks::CellIndex& cell_index) { - return kCellsPerSide * block_index + cell_index; -} -} // namespace wavemap - -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_HASHED_BLOCKS_INL_H_ diff --git a/libraries/wavemap/include/wavemap/indexing/index_conversions.h b/libraries/wavemap/include/wavemap/indexing/index_conversions.h index a19e31216..9b62fa749 100644 --- a/libraries/wavemap/include/wavemap/indexing/index_conversions.h +++ b/libraries/wavemap/include/wavemap/indexing/index_conversions.h @@ -9,6 +9,7 @@ #include "wavemap/data_structure/aabb.h" #include "wavemap/indexing/ndtree_index.h" #include "wavemap/utils/bits/morton_encoding.h" +#include "wavemap/utils/data/eigen_checks.h" #include "wavemap/utils/math/int_math.h" namespace wavemap::convert { @@ -76,18 +77,19 @@ inline Index indexToNewResolution(const Index& src_index, } inline FloatingPoint heightToCellWidth(FloatingPoint min_cell_width, - NdtreeIndexElement height) { + IndexElement height) { return min_cell_width * static_cast(int_math::exp2(height)); } -inline NdtreeIndexElement cellWidthToHeight(FloatingPoint cell_width, - FloatingPoint min_cell_width_inv) { +inline IndexElement cellWidthToHeight(FloatingPoint cell_width, + FloatingPoint min_cell_width_inv) { return std::ceil(std::log2(cell_width * min_cell_width_inv)); } template inline LinearIndex indexToLinearIndex(const Index& index) { - DCHECK((0 <= index.array() && index.array() < cells_per_side).all()); + DCHECK_EIGEN_GE(index, Index::Zero()); + DCHECK_EIGEN_LT(index, Index::Constant(cells_per_side)); constexpr auto pow_sequence = int_math::pow_sequence(); return std::transform_reduce(pow_sequence.cbegin(), pow_sequence.cend(), @@ -96,6 +98,7 @@ inline LinearIndex indexToLinearIndex(const Index& index) { template inline Index linearIndexToIndex(LinearIndex linear_index) { + DCHECK_GE(linear_index, 0); DCHECK_LT(linear_index, std::pow(cells_per_side, dim)); constexpr auto pow_sequence = int_math::pow_sequence(); @@ -109,7 +112,7 @@ inline Index linearIndexToIndex(LinearIndex linear_index) { template inline NdtreeIndex pointToNodeIndex(const Point& point, FloatingPoint min_cell_width, - NdtreeIndexElement height) { + IndexElement height) { const FloatingPoint node_width = heightToCellWidth(min_cell_width, height); const Index position_index = pointToNearestIndex(point, 1.f / node_width); @@ -154,7 +157,7 @@ inline AABB> nodeIndexToAABB(const NdtreeIndex& node_index, template inline NdtreeIndex indexAndHeightToNodeIndex(const Index& index, - NdtreeIndexElement height) { + IndexElement height) { DCHECK_GE(height, 0); NdtreeIndex node_index{height, index}; node_index.position = int_math::div_exp2_floor(node_index.position, height); @@ -174,8 +177,7 @@ template inline Index nodeIndexToMaxCornerIndex( const NdtreeIndex& node_index) { DCHECK_GE(node_index.height, 0); - const NdtreeIndexElement max_child_offset = - int_math::exp2(node_index.height) - 1; + const IndexElement max_child_offset = int_math::exp2(node_index.height) - 1; Index index = nodeIndexToMinCornerIndex(node_index).array() + max_child_offset; return index; diff --git a/libraries/wavemap/include/wavemap/indexing/ndtree_index.h b/libraries/wavemap/include/wavemap/indexing/ndtree_index.h index 7f619af27..c199a5dca 100644 --- a/libraries/wavemap/include/wavemap/indexing/ndtree_index.h +++ b/libraries/wavemap/include/wavemap/indexing/ndtree_index.h @@ -9,7 +9,6 @@ #include "wavemap/utils/math/int_math.h" namespace wavemap { -using NdtreeIndexElement = int; using NdtreeIndexRelativeChild = uint8_t; // TODO(victorr): Consider renaming NdtreeIndex to something like @@ -18,7 +17,7 @@ using NdtreeIndexRelativeChild = uint8_t; // structure that's being indexed. template struct NdtreeIndex { - using Element = NdtreeIndexElement; + using Element = IndexElement; using RelativeChild = NdtreeIndexRelativeChild; using Position = Eigen::Matrix; diff --git a/libraries/wavemap/include/wavemap/integrator/integrator_factory.h b/libraries/wavemap/include/wavemap/integrator/integrator_factory.h index 245961829..f20fd1f48 100644 --- a/libraries/wavemap/include/wavemap/integrator/integrator_factory.h +++ b/libraries/wavemap/include/wavemap/integrator/integrator_factory.h @@ -4,8 +4,8 @@ #include #include -#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" #include "wavemap/integrator/integrator_base.h" +#include "wavemap/map/volumetric_data_structure_base.h" #include "wavemap/utils/thread_pool.h" namespace wavemap { diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h index efd8fc4c0..c0331635b 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h @@ -4,10 +4,10 @@ #include #include -#include "wavemap/data_structure/volumetric/volumetric_octree.h" #include "wavemap/integrator/projective/coarse_to_fine/range_image_intersector.h" #include "wavemap/integrator/projective/projective_integrator.h" #include "wavemap/integrator/projective/update_type.h" +#include "wavemap/map/volumetric_octree.h" namespace wavemap { class CoarseToFineIntegrator : public ProjectiveIntegrator { diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h index ed5e92044..d698eaf24 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h @@ -5,10 +5,10 @@ #include #include -#include "wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree.h" -#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" #include "wavemap/integrator/projective/coarse_to_fine/range_image_intersector.h" #include "wavemap/integrator/projective/projective_integrator.h" +#include "wavemap/map/hashed_chunked_wavelet_octree.h" +#include "wavemap/map/volumetric_data_structure_base.h" #include "wavemap/utils/thread_pool.h" namespace wavemap { diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h index 597e8c091..091cd1592 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h @@ -5,10 +5,10 @@ #include #include -#include "wavemap/data_structure/volumetric/hashed_wavelet_octree.h" -#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" #include "wavemap/integrator/projective/coarse_to_fine/range_image_intersector.h" #include "wavemap/integrator/projective/projective_integrator.h" +#include "wavemap/map/hashed_wavelet_octree.h" +#include "wavemap/map/volumetric_data_structure_base.h" #include "wavemap/utils/thread_pool.h" namespace wavemap { diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h index a94161103..676567251 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h @@ -10,6 +10,7 @@ #include #include "wavemap/data_structure/image.h" +#include "wavemap/indexing/ndtree_index.h" #include "wavemap/integrator/projection_model/projector_base.h" #include "wavemap/integrator/projective/update_type.h" @@ -34,7 +35,7 @@ class HierarchicalRangeBounds { DCHECK_EQ(upper_bound_levels_.size(), max_height_); } - NdtreeIndexElement getMaxHeight() const { return max_height_; } + IndexElement getMaxHeight() const { return max_height_; } FloatingPoint getMinRange() const { return min_range_; } static FloatingPoint getUnknownValueLowerBound() { return kUnknownValueLowerBound; @@ -109,8 +110,8 @@ class HierarchicalRangeBounds { range_image_->transform([=](auto val) { return isUnobserved(val); }), [](auto a, auto b) { return a || b; }, true); - const NdtreeIndexElement max_height_ = - static_cast(lower_bound_levels_.size()); + const IndexElement max_height_ = + static_cast(lower_bound_levels_.size()); template std::vector> computeReducedPyramid(const Image& range_image, diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h index 439528ed4..f3073904a 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h @@ -22,9 +22,9 @@ inline void HashedChunkedWaveletIntegrator::recursiveTester( // NOLINT update_job_list.emplace_back(node_index.position); return; } - if (occupancy_map_->hasBlock(node_index.position)) { - const auto& block = occupancy_map_->getBlock(node_index.position); - if (min_log_odds_shrunk_ <= block.getRootScale()) { + if (const auto* block = occupancy_map_->getBlock(node_index.position); + block) { + if (min_log_odds_shrunk_ <= block->getRootScale()) { // Add the block to the job list update_job_list.emplace_back(node_index.position); } diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h index df4868c8d..9569980c3 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h @@ -22,9 +22,9 @@ inline void HashedWaveletIntegrator::recursiveTester( // NOLINT update_job_list.emplace_back(node_index); return; } - if (occupancy_map_->hasBlock(node_index.position)) { - const auto& block = occupancy_map_->getBlock(node_index.position); - if (min_log_odds_ + kNoiseThreshold / 10.f <= block.getRootScale()) { + if (const auto* block = occupancy_map_->getBlock(node_index.position); + block) { + if (min_log_odds_ + kNoiseThreshold / 10.f <= block->getRootScale()) { // Add the block to the job list update_job_list.emplace_back(node_index); } diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/wavelet_integrator.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/wavelet_integrator.h index 4a1264529..d609e1556 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/wavelet_integrator.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/wavelet_integrator.h @@ -4,10 +4,10 @@ #include #include -#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" -#include "wavemap/data_structure/volumetric/wavelet_octree.h" #include "wavemap/integrator/projective/coarse_to_fine/range_image_intersector.h" #include "wavemap/integrator/projective/projective_integrator.h" +#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/wavelet_octree.h" namespace wavemap { class WaveletIntegrator : public ProjectiveIntegrator { diff --git a/libraries/wavemap/include/wavemap/integrator/projective/projective_integrator.h b/libraries/wavemap/include/wavemap/integrator/projective/projective_integrator.h index 4e8460ec5..583aeee83 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/projective_integrator.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/projective_integrator.h @@ -6,10 +6,10 @@ #include "wavemap/config/type_selector.h" #include "wavemap/data_structure/image.h" -#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" #include "wavemap/integrator/integrator_base.h" #include "wavemap/integrator/measurement_model/measurement_model_base.h" #include "wavemap/integrator/projection_model/projector_base.h" +#include "wavemap/map/volumetric_data_structure_base.h" namespace wavemap { /** @@ -29,7 +29,7 @@ struct ProjectiveIntegratorConfig : ConfigBase { //! res). Can be set to 1 for 1/2 of the max resolution, to 2 for 1/4 of the //! max resolution, etc.This can be used to fuse multiple inputs with //! different maximum resolutions into a single map. - NdtreeIndexElement termination_height = 0; + IndexElement termination_height = 0; //! The update error threshold at which the coarse-to-fine measurement //! integrator is allowed to terminate, in log-odds. For more information, //! please refer to: https://www.roboticsproceedings.org/rss19/p065.pdf. diff --git a/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h b/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h index 19a768cd4..567b5fcf7 100644 --- a/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h +++ b/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h @@ -3,9 +3,9 @@ #include -#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" #include "wavemap/integrator/integrator_base.h" #include "wavemap/integrator/measurement_model/constant_ray.h" +#include "wavemap/map/volumetric_data_structure_base.h" #include "wavemap/utils/iterate/ray_iterator.h" namespace wavemap { diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/haar_coefficients.h b/libraries/wavemap/include/wavemap/map/cell_types/haar_coefficients.h similarity index 95% rename from libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/haar_coefficients.h rename to libraries/wavemap/include/wavemap/map/cell_types/haar_coefficients.h index 102d0cd64..cffac8918 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/haar_coefficients.h +++ b/libraries/wavemap/include/wavemap/map/cell_types/haar_coefficients.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_CELL_TYPES_HAAR_COEFFICIENTS_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_CELL_TYPES_HAAR_COEFFICIENTS_H_ +#ifndef WAVEMAP_MAP_CELL_TYPES_HAAR_COEFFICIENTS_H_ +#define WAVEMAP_MAP_CELL_TYPES_HAAR_COEFFICIENTS_H_ #include #include @@ -138,4 +138,4 @@ struct HaarCoefficients { }; } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_CELL_TYPES_HAAR_COEFFICIENTS_H_ +#endif // WAVEMAP_MAP_CELL_TYPES_HAAR_COEFFICIENTS_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/haar_transform.h b/libraries/wavemap/include/wavemap/map/cell_types/haar_transform.h similarity index 93% rename from libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/haar_transform.h rename to libraries/wavemap/include/wavemap/map/cell_types/haar_transform.h index f90ca5311..680acaeac 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/haar_transform.h +++ b/libraries/wavemap/include/wavemap/map/cell_types/haar_transform.h @@ -1,11 +1,11 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_CELL_TYPES_HAAR_TRANSFORM_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_CELL_TYPES_HAAR_TRANSFORM_H_ +#ifndef WAVEMAP_MAP_CELL_TYPES_HAAR_TRANSFORM_H_ +#define WAVEMAP_MAP_CELL_TYPES_HAAR_TRANSFORM_H_ #include #include "wavemap/common.h" -#include "wavemap/data_structure/volumetric/cell_types/haar_coefficients.h" #include "wavemap/indexing/ndtree_index.h" +#include "wavemap/map/cell_types/haar_coefficients.h" namespace wavemap { // Transform 2^d scale space coefficients into 1 coarse scale and 2^d - 1 @@ -135,6 +135,6 @@ struct HaarTransform { }; } // namespace wavemap -#include "wavemap/data_structure/volumetric/cell_types/impl/haar_transform_inl.h" +#include "wavemap/map/cell_types/impl/haar_transform_inl.h" -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_CELL_TYPES_HAAR_TRANSFORM_H_ +#endif // WAVEMAP_MAP_CELL_TYPES_HAAR_TRANSFORM_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/impl/haar_transform_inl.h b/libraries/wavemap/include/wavemap/map/cell_types/impl/haar_transform_inl.h similarity index 74% rename from libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/impl/haar_transform_inl.h rename to libraries/wavemap/include/wavemap/map/cell_types/impl/haar_transform_inl.h index e2fb260bd..e14434f57 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/cell_types/impl/haar_transform_inl.h +++ b/libraries/wavemap/include/wavemap/map/cell_types/impl/haar_transform_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_CELL_TYPES_IMPL_HAAR_TRANSFORM_INL_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_CELL_TYPES_IMPL_HAAR_TRANSFORM_INL_H_ +#ifndef WAVEMAP_MAP_CELL_TYPES_IMPL_HAAR_TRANSFORM_INL_H_ +#define WAVEMAP_MAP_CELL_TYPES_IMPL_HAAR_TRANSFORM_INL_H_ #include "wavemap/utils/bits/bit_operations.h" #include "wavemap/utils/math/int_math.h" @@ -12,13 +12,11 @@ typename HaarCoefficients::Parent ForwardLifted( typename HaarCoefficients::CoefficientsArray parent_coefficients; // Apply the forward transform one beam (row/col) at a time using lifting - constexpr NdtreeIndexElement kNumBeams = int_math::exp2(dim - 1); + constexpr IndexElement kNumBeams = int_math::exp2(dim - 1); // Perform the transform along axis 0 while moving the data into the array - for (NdtreeIndexElement beam_idx = 0; beam_idx < kNumBeams; ++beam_idx) { - const NdtreeIndexElement scale_idx = - bit_ops::squeeze_in(beam_idx, false, 0); - const NdtreeIndexElement detail_idx = - bit_ops::squeeze_in(beam_idx, true, 0); + for (IndexElement beam_idx = 0; beam_idx < kNumBeams; ++beam_idx) { + const IndexElement scale_idx = bit_ops::squeeze_in(beam_idx, false, 0); + const IndexElement detail_idx = bit_ops::squeeze_in(beam_idx, true, 0); parent_coefficients[detail_idx] = child_scales[detail_idx] - child_scales[scale_idx]; parent_coefficients[scale_idx] = @@ -26,11 +24,11 @@ typename HaarCoefficients::Parent ForwardLifted( static_cast(0.5) * parent_coefficients[detail_idx]; } // Perform the transform along the remaining axes in-place - for (NdtreeIndexElement dim_idx = 1; dim_idx < dim; ++dim_idx) { - for (NdtreeIndexElement beam_idx = 0; beam_idx < kNumBeams; ++beam_idx) { - const NdtreeIndexElement scale_idx = + for (IndexElement dim_idx = 1; dim_idx < dim; ++dim_idx) { + for (IndexElement beam_idx = 0; beam_idx < kNumBeams; ++beam_idx) { + const IndexElement scale_idx = bit_ops::squeeze_in(beam_idx, false, dim_idx); - const NdtreeIndexElement detail_idx = + const IndexElement detail_idx = bit_ops::squeeze_in(beam_idx, true, dim_idx); parent_coefficients[detail_idx] -= parent_coefficients[scale_idx]; parent_coefficients[scale_idx] += @@ -52,9 +50,9 @@ typename HaarCoefficients::Parent ForwardParallel( // manages to vectorize the loops nicely constexpr auto kNumCoefficients = HaarCoefficients::kNumCoefficients; - for (NdtreeIndexElement parent_idx = 0; parent_idx < kNumCoefficients; + for (IndexElement parent_idx = 0; parent_idx < kNumCoefficients; ++parent_idx) { - for (NdtreeIndexElement child_idx = 0; child_idx < kNumCoefficients; + for (IndexElement child_idx = 0; child_idx < kNumCoefficients; ++child_idx) { parent_coefficients[parent_idx] += bit_ops::parity(parent_idx & ~child_idx) ? -child_scales[child_idx] @@ -75,7 +73,7 @@ typename HaarCoefficients::Parent ForwardSingleChild( constexpr auto kNumCoefficients = HaarCoefficients::kNumCoefficients; - for (NdtreeIndexElement parent_idx = 0; parent_idx < kNumCoefficients; + for (IndexElement parent_idx = 0; parent_idx < kNumCoefficients; ++parent_idx) { parent_coefficients[parent_idx] = bit_ops::parity(parent_idx & ~child_idx) ? -child_scale : +child_scale; @@ -92,23 +90,21 @@ typename HaarCoefficients::CoefficientsArray BackwardLifted( typename HaarCoefficients::CoefficientsArray child_scales; // Apply the inverse transform one beam (row/col) at a time using lifting - constexpr NdtreeIndexElement kNumBeams = int_math::exp2(dim - 1); + constexpr IndexElement kNumBeams = int_math::exp2(dim - 1); // Perform the transform along axis 0 while moving the data into the array - for (NdtreeIndexElement beam_idx = 0; beam_idx < kNumBeams; ++beam_idx) { - const NdtreeIndexElement scale_idx = - bit_ops::squeeze_in(beam_idx, false, 0); - const NdtreeIndexElement detail_idx = - bit_ops::squeeze_in(beam_idx, true, 0); + for (IndexElement beam_idx = 0; beam_idx < kNumBeams; ++beam_idx) { + const IndexElement scale_idx = bit_ops::squeeze_in(beam_idx, false, 0); + const IndexElement detail_idx = bit_ops::squeeze_in(beam_idx, true, 0); child_scales[scale_idx] = parent[scale_idx] - static_cast(0.5) * parent[detail_idx]; child_scales[detail_idx] = parent[detail_idx] + child_scales[scale_idx]; } // Perform the transform along the remaining axes in-place - for (NdtreeIndexElement dim_idx = 1; dim_idx < dim; ++dim_idx) { - for (NdtreeIndexElement beam_idx = 0; beam_idx < kNumBeams; ++beam_idx) { - const NdtreeIndexElement scale_idx = + for (IndexElement dim_idx = 1; dim_idx < dim; ++dim_idx) { + for (IndexElement beam_idx = 0; beam_idx < kNumBeams; ++beam_idx) { + const IndexElement scale_idx = bit_ops::squeeze_in(beam_idx, false, dim_idx); - const NdtreeIndexElement detail_idx = + const IndexElement detail_idx = bit_ops::squeeze_in(beam_idx, true, dim_idx); child_scales[scale_idx] -= static_cast(0.5) * child_scales[detail_idx]; @@ -126,10 +122,9 @@ typename HaarCoefficients::CoefficientsArray BackwardParallel( constexpr auto kNumCoefficients = HaarCoefficients::kNumCoefficients; - for (NdtreeIndexElement child_idx = 0; child_idx < kNumCoefficients; - ++child_idx) { + for (IndexElement child_idx = 0; child_idx < kNumCoefficients; ++child_idx) { child_scales[child_idx] += parent.scale; - for (NdtreeIndexElement parent_idx = 1; parent_idx < kNumCoefficients; + for (IndexElement parent_idx = 1; parent_idx < kNumCoefficients; ++parent_idx) { const ValueT contribution = parent.details[parent_idx - 1] / @@ -149,7 +144,7 @@ typename HaarCoefficients::Scale BackwardSingleChild( NdtreeIndexRelativeChild child_idx) { ValueT scale = parent.scale; - for (NdtreeIndexElement parent_idx = 1; + for (IndexElement parent_idx = 1; parent_idx < HaarCoefficients::kNumCoefficients; ++parent_idx) { const ValueT contribution = @@ -163,4 +158,4 @@ typename HaarCoefficients::Scale BackwardSingleChild( } } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_CELL_TYPES_IMPL_HAAR_TRANSFORM_INL_H_ +#endif // WAVEMAP_MAP_CELL_TYPES_IMPL_HAAR_TRANSFORM_INL_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_blocks.h b/libraries/wavemap/include/wavemap/map/hashed_blocks.h similarity index 51% rename from libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_blocks.h rename to libraries/wavemap/include/wavemap/map/hashed_blocks.h index 3e7b98a91..8abc28c31 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_blocks.h +++ b/libraries/wavemap/include/wavemap/map/hashed_blocks.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_HASHED_BLOCKS_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_HASHED_BLOCKS_H_ +#ifndef WAVEMAP_MAP_HASHED_BLOCKS_H_ +#define WAVEMAP_MAP_HASHED_BLOCKS_H_ #include #include @@ -7,40 +7,42 @@ #include "wavemap/common.h" #include "wavemap/config/config_base.h" -#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" +#include "wavemap/data_structure/dense_grid.h" +#include "wavemap/data_structure/spatial_hash.h" #include "wavemap/indexing/index_hashes.h" +#include "wavemap/map/volumetric_data_structure_base.h" #include "wavemap/utils/math/int_math.h" namespace wavemap { class HashedBlocks : public VolumetricDataStructureBase { - public: - static constexpr IndexElement kCellsPerSideLog2 = 4; - static constexpr IndexElement kCellsPerSide = - int_math::exp2(kCellsPerSideLog2); - static constexpr IndexElement kCellsPerBlock = - int_math::exp2(kDim * kCellsPerSideLog2); + private: + static constexpr IndexElement kCellsPerSide = int_math::exp2(4); + public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; using Config = VolumetricDataStructureConfig; static constexpr bool kRequiresExplicitThresholding = false; - using Block = std::array; using BlockIndex = Index3D; using CellIndex = Index3D; + using Block = DenseGrid; + using BlockHashMap = SpatialHash; explicit HashedBlocks(const VolumetricDataStructureConfig& config, FloatingPoint default_value = 0.f) : VolumetricDataStructureBase(config.checkValid()), default_value_(default_value) {} - bool empty() const override { return blocks_.empty(); } - size_t size() const override { return kCellsPerBlock * blocks_.size(); } + bool empty() const override { return block_map_.empty(); } + size_t size() const override { + return Block::kCellsPerBlock * block_map_.size(); + } void threshold() override { // Not needed. Data is already tresholded when added. } void prune() override; - void clear() override { blocks_.clear(); } + void clear() override { block_map_.clear(); } size_t getMemoryUsage() const override { return size() * sizeof(FloatingPoint); @@ -48,48 +50,44 @@ class HashedBlocks : public VolumetricDataStructureBase { Index3D getMinIndex() const override; Index3D getMaxIndex() const override; + Index3D getMinBlockIndex() const { return block_map_.getMinBlockIndex(); } + Index3D getMaxBlockIndex() const { return block_map_.getMaxBlockIndex(); } IndexElement getTreeHeight() const override { return 0; } const VolumetricDataStructureConfig& getConfig() { return config_; } FloatingPoint getCellValue(const Index3D& index) const override; + FloatingPoint& getCellValueRef(const Index3D& index); + FloatingPoint getDefaultCellValue() const { return default_value_; } void setCellValue(const Index3D& index, FloatingPoint new_value) override; void addToCellValue(const Index3D& index, FloatingPoint update) override; - FloatingPoint getDefaultCellValue() const { return default_value_; } - bool hasBlock(const Index3D& block_index) const { - return blocks_.count(block_index); - } + bool hasBlock(const Index3D& block_index) const; + Block* getBlock(const Index3D& block_index); + const Block* getBlock(const Index3D& block_index) const; Block& getOrAllocateBlock(const Index3D& block_index); - Block& getBlock(const Index3D& block_index) { - return blocks_.at(block_index); + + auto& getHashMap() { return block_map_; } + const auto& getHashMap() const { return block_map_; } + + template + void forEachBlock(IndexedBlockVisitor visitor_fn) { + block_map_.forEachBlock(visitor_fn); } - const Block& getBlock(const Index3D& block_index) const { - return blocks_.at(block_index); + template + void forEachBlock(IndexedBlockVisitor visitor_fn) const { + block_map_.forEachBlock(visitor_fn); } - auto& getBlocks() { return blocks_; } - const auto& getBlocks() const { return blocks_; } - - FloatingPoint* accessCellData(const Index3D& index, - bool auto_allocate = false); - const FloatingPoint* accessCellData(const Index3D& index) const; void forEachLeaf( typename VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn) const override; - static BlockIndex computeBlockIndexFromIndex(const Index3D& index); - static CellIndex computeCellIndexFromBlockIndexAndIndex( - const BlockIndex& block_index, const Index3D& index); - static Index3D computeIndexFromBlockIndexAndCellIndex( - const BlockIndex& block_index, const CellIndex& cell_index); - private: const FloatingPoint default_value_; - - std::unordered_map> blocks_; + BlockHashMap block_map_; }; } // namespace wavemap -#include "wavemap/data_structure/volumetric/impl/hashed_blocks_inl.h" +#include "wavemap/map/impl/hashed_blocks_inl.h" -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_HASHED_BLOCKS_H_ +#endif // WAVEMAP_MAP_HASHED_BLOCKS_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree.h b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree.h similarity index 76% rename from libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree.h rename to libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree.h index b0e309087..f894064be 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree.h +++ b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree.h @@ -1,14 +1,15 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_HASHED_CHUNKED_WAVELET_OCTREE_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_HASHED_CHUNKED_WAVELET_OCTREE_H_ +#ifndef WAVEMAP_MAP_HASHED_CHUNKED_WAVELET_OCTREE_H_ +#define WAVEMAP_MAP_HASHED_CHUNKED_WAVELET_OCTREE_H_ #include #include #include "wavemap/common.h" #include "wavemap/config/config_base.h" -#include "wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree_block.h" -#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" +#include "wavemap/data_structure/spatial_hash.h" #include "wavemap/indexing/index_hashes.h" +#include "wavemap/map/hashed_chunked_wavelet_octree_block.h" +#include "wavemap/map/volumetric_data_structure_base.h" #include "wavemap/utils/math/int_math.h" namespace wavemap { @@ -66,25 +67,28 @@ class HashedChunkedWaveletOctree : public VolumetricDataStructureBase { using Config = HashedChunkedWaveletOctreeConfig; static constexpr bool kRequiresExplicitThresholding = true; - using Block = HashedChunkedWaveletOctreeBlock; - using BlockIndex = Block::BlockIndex; + using BlockIndex = Index3D; using CellIndex = OctreeIndex; + using Block = HashedChunkedWaveletOctreeBlock; + using BlockHashMap = SpatialHash; explicit HashedChunkedWaveletOctree( const HashedChunkedWaveletOctreeConfig& config) : VolumetricDataStructureBase(config), config_(config.checkValid()) {} - bool empty() const override { return blocks_.empty(); } + bool empty() const override { return block_map_.empty(); } size_t size() const override; void threshold() override; void prune() override; void pruneSmart() override; - void clear() override { blocks_.clear(); } + void clear() override { block_map_.clear(); } size_t getMemoryUsage() const override; Index3D getMinIndex() const override; Index3D getMaxIndex() const override; + Index3D getMinBlockIndex() const { return block_map_.getMinBlockIndex(); } + Index3D getMaxBlockIndex() const { return block_map_.getMaxBlockIndex(); } IndexElement getTreeHeight() const override { return config_.tree_height; } IndexElement getChunkHeight() const { return Block::kChunkHeight; } Index3D getBlockSize() const { @@ -98,11 +102,21 @@ class HashedChunkedWaveletOctree : public VolumetricDataStructureBase { void addToCellValue(const Index3D& index, FloatingPoint update) override; bool hasBlock(const Index3D& block_index) const; + Block* getBlock(const Index3D& block_index); + const Block* getBlock(const Index3D& block_index) const; Block& getOrAllocateBlock(const Index3D& block_index); - Block& getBlock(const Index3D& block_index); - const Block& getBlock(const Index3D& block_index) const; - auto& getBlocks() { return blocks_; } - const auto& getBlocks() const { return blocks_; } + + auto& getHashMap() { return block_map_; } + const auto& getHashMap() const { return block_map_; } + + template + void forEachBlock(IndexedBlockVisitor visitor_fn) { + block_map_.forEachBlock(visitor_fn); + } + template + void forEachBlock(IndexedBlockVisitor visitor_fn) const { + block_map_.forEachBlock(visitor_fn); + } void forEachLeaf( typename VolumetricDataStructureBase::IndexedLeafVisitorFunction @@ -110,26 +124,26 @@ class HashedChunkedWaveletOctree : public VolumetricDataStructureBase { private: const HashedChunkedWaveletOctreeConfig config_; - const IndexElement cells_per_block_side_ = int_math::exp2(config_.tree_height); - std::unordered_map> blocks_; + BlockHashMap block_map_; + // TODO(victorr): Remove BlockIndex computeBlockIndexFromIndex(const Index3D& index) const { return int_math::div_exp2_floor(index, config_.tree_height); } + // TODO(victorr): Remove BlockIndex computeBlockIndexFromIndex(const OctreeIndex& node_index) const { - // TODO(victorr): Divide by height difference directly instead of round trip - // through height 0 const Index3D index = convert::nodeIndexToMinCornerIndex(node_index); return int_math::div_exp2_floor(index, config_.tree_height); } + // TODO(victorr): Remove CellIndex computeCellIndexFromBlockIndexAndIndex( const BlockIndex& block_index, OctreeIndex index) const; }; } // namespace wavemap -#include "wavemap/data_structure/volumetric/impl/hashed_chunked_wavelet_octree_inl.h" +#include "wavemap/map/impl/hashed_chunked_wavelet_octree_inl.h" -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_HASHED_CHUNKED_WAVELET_OCTREE_H_ +#endif // WAVEMAP_MAP_HASHED_CHUNKED_WAVELET_OCTREE_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree_block.h b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h similarity index 85% rename from libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree_block.h rename to libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h index dacda5bd8..f644d65b2 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree_block.h +++ b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h @@ -1,11 +1,11 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_H_ +#ifndef WAVEMAP_MAP_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_H_ +#define WAVEMAP_MAP_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_H_ #include "wavemap/common.h" #include "wavemap/data_structure/chunked_ndtree/chunked_ndtree.h" -#include "wavemap/data_structure/volumetric/cell_types/haar_coefficients.h" -#include "wavemap/data_structure/volumetric/cell_types/haar_transform.h" -#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" +#include "wavemap/map/cell_types/haar_coefficients.h" +#include "wavemap/map/cell_types/haar_transform.h" +#include "wavemap/map/volumetric_data_structure_base.h" #include "wavemap/utils/time/time.h" namespace wavemap { @@ -101,6 +101,6 @@ class HashedChunkedWaveletOctreeBlock { }; } // namespace wavemap -#include "wavemap/data_structure/volumetric/impl/hashed_chunked_wavelet_octree_block_inl.h" +#include "wavemap/map/impl/hashed_chunked_wavelet_octree_block_inl.h" -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_H_ +#endif // WAVEMAP_MAP_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_wavelet_octree.h b/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h similarity index 76% rename from libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_wavelet_octree.h rename to libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h index 4a09985cd..95131f0ad 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_wavelet_octree.h +++ b/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h @@ -1,14 +1,15 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_HASHED_WAVELET_OCTREE_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_HASHED_WAVELET_OCTREE_H_ +#ifndef WAVEMAP_MAP_HASHED_WAVELET_OCTREE_H_ +#define WAVEMAP_MAP_HASHED_WAVELET_OCTREE_H_ #include #include #include "wavemap/common.h" #include "wavemap/config/config_base.h" -#include "wavemap/data_structure/volumetric/hashed_wavelet_octree_block.h" -#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" +#include "wavemap/data_structure/spatial_hash.h" #include "wavemap/indexing/index_hashes.h" +#include "wavemap/map/hashed_wavelet_octree_block.h" +#include "wavemap/map/volumetric_data_structure_base.h" #include "wavemap/utils/math/int_math.h" namespace wavemap { @@ -62,24 +63,27 @@ class HashedWaveletOctree : public VolumetricDataStructureBase { using Config = HashedWaveletOctreeConfig; static constexpr bool kRequiresExplicitThresholding = true; - using Block = HashedWaveletOctreeBlock; - using BlockIndex = Block::BlockIndex; + using BlockIndex = Index3D; using CellIndex = OctreeIndex; + using Block = HashedWaveletOctreeBlock; + using BlockHashMap = SpatialHash; explicit HashedWaveletOctree(const HashedWaveletOctreeConfig& config) : VolumetricDataStructureBase(config), config_(config.checkValid()) {} - bool empty() const override { return blocks_.empty(); } + bool empty() const override { return block_map_.empty(); } size_t size() const override; void threshold() override; void prune() override; void pruneSmart() override; - void clear() override { blocks_.clear(); } + void clear() override { block_map_.clear(); } size_t getMemoryUsage() const override; Index3D getMinIndex() const override; Index3D getMaxIndex() const override; + Index3D getMinBlockIndex() const { return block_map_.getMinBlockIndex(); } + Index3D getMaxBlockIndex() const { return block_map_.getMaxBlockIndex(); } IndexElement getTreeHeight() const override { return config_.tree_height; } Index3D getBlockSize() const { return Index3D::Constant(cells_per_block_side_); @@ -92,11 +96,21 @@ class HashedWaveletOctree : public VolumetricDataStructureBase { void addToCellValue(const Index3D& index, FloatingPoint update) override; bool hasBlock(const Index3D& block_index) const; + Block* getBlock(const Index3D& block_index); + const Block* getBlock(const Index3D& block_index) const; Block& getOrAllocateBlock(const Index3D& block_index); - Block& getBlock(const Index3D& block_index); - const Block& getBlock(const Index3D& block_index) const; - auto& getBlocks() { return blocks_; } - const auto& getBlocks() const { return blocks_; } + + auto& getHashMap() { return block_map_; } + const auto& getHashMap() const { return block_map_; } + + template + void forEachBlock(IndexedBlockVisitor visitor_fn) { + block_map_.forEachBlock(visitor_fn); + } + template + void forEachBlock(IndexedBlockVisitor visitor_fn) const { + block_map_.forEachBlock(visitor_fn); + } void forEachLeaf( typename VolumetricDataStructureBase::IndexedLeafVisitorFunction @@ -104,24 +118,26 @@ class HashedWaveletOctree : public VolumetricDataStructureBase { private: const HashedWaveletOctreeConfig config_; - const IndexElement cells_per_block_side_ = int_math::exp2(config_.tree_height); - std::unordered_map> blocks_; + BlockHashMap block_map_; + // TODO(victorr): Remove BlockIndex computeBlockIndexFromIndex(const Index3D& index) const { return int_math::div_exp2_floor(index, config_.tree_height); } + // TODO(victorr): Remove BlockIndex computeBlockIndexFromIndex(const OctreeIndex& node_index) const { const Index3D index = convert::nodeIndexToMinCornerIndex(node_index); return int_math::div_exp2_floor(index, config_.tree_height); } + // TODO(victorr): Remove CellIndex computeCellIndexFromBlockIndexAndIndex( const BlockIndex& block_index, OctreeIndex index) const; }; } // namespace wavemap -#include "wavemap/data_structure/volumetric/impl/hashed_wavelet_octree_inl.h" +#include "wavemap/map/impl/hashed_wavelet_octree_inl.h" -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_HASHED_WAVELET_OCTREE_H_ +#endif // WAVEMAP_MAP_HASHED_WAVELET_OCTREE_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_wavelet_octree_block.h b/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree_block.h similarity index 84% rename from libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_wavelet_octree_block.h rename to libraries/wavemap/include/wavemap/map/hashed_wavelet_octree_block.h index c891703c7..5ae706c4b 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_wavelet_octree_block.h +++ b/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree_block.h @@ -1,11 +1,11 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_HASHED_WAVELET_OCTREE_BLOCK_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_HASHED_WAVELET_OCTREE_BLOCK_H_ +#ifndef WAVEMAP_MAP_HASHED_WAVELET_OCTREE_BLOCK_H_ +#define WAVEMAP_MAP_HASHED_WAVELET_OCTREE_BLOCK_H_ #include "wavemap/common.h" #include "wavemap/data_structure/ndtree/ndtree.h" -#include "wavemap/data_structure/volumetric/cell_types/haar_coefficients.h" -#include "wavemap/data_structure/volumetric/cell_types/haar_transform.h" -#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" +#include "wavemap/map/cell_types/haar_coefficients.h" +#include "wavemap/map/cell_types/haar_transform.h" +#include "wavemap/map/volumetric_data_structure_base.h" #include "wavemap/utils/time/time.h" namespace wavemap { @@ -86,6 +86,6 @@ class HashedWaveletOctreeBlock { }; } // namespace wavemap -#include "wavemap/data_structure/volumetric/impl/hashed_wavelet_octree_block_inl.h" +#include "wavemap/map/impl/hashed_wavelet_octree_block_inl.h" -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_HASHED_WAVELET_OCTREE_BLOCK_H_ +#endif // WAVEMAP_MAP_HASHED_WAVELET_OCTREE_BLOCK_H_ diff --git a/libraries/wavemap/include/wavemap/map/impl/hashed_blocks_inl.h b/libraries/wavemap/include/wavemap/map/impl/hashed_blocks_inl.h new file mode 100644 index 000000000..5e2f88d34 --- /dev/null +++ b/libraries/wavemap/include/wavemap/map/impl/hashed_blocks_inl.h @@ -0,0 +1,69 @@ +#ifndef WAVEMAP_MAP_IMPL_HASHED_BLOCKS_INL_H_ +#define WAVEMAP_MAP_IMPL_HASHED_BLOCKS_INL_H_ + +#include +#include +#include + +#include "wavemap/indexing/index_conversions.h" +#include "wavemap/utils/iterate/grid_iterator.h" + +namespace wavemap { +inline FloatingPoint HashedBlocks::getCellValue(const Index3D& index) const { + const BlockIndex block_index = + convert::indexToBlockIndex(index); + const auto* block = getBlock(block_index); + if (!block) { + return default_value_; + } + const CellIndex cell_index = convert::indexToCellIndex(index); + return block->at(cell_index); +} + +inline FloatingPoint& HashedBlocks::getCellValueRef(const Index3D& index) { + const BlockIndex block_index = + convert::indexToBlockIndex(index); + auto& block = getOrAllocateBlock(block_index); + const CellIndex cell_index = convert::indexToCellIndex(index); + return block.at(cell_index); +} + +inline void HashedBlocks::setCellValue(const Index3D& index, + FloatingPoint new_value) { + const BlockIndex block_index = + convert::indexToBlockIndex(index); + Block& block = getOrAllocateBlock(block_index); + const CellIndex cell_index = convert::indexToCellIndex(index); + block.at(cell_index) = new_value; +} + +inline void HashedBlocks::addToCellValue(const Index3D& index, + FloatingPoint update) { + const BlockIndex block_index = + convert::indexToBlockIndex(index); + Block& block = getOrAllocateBlock(block_index); + const CellIndex cell_index = convert::indexToCellIndex(index); + FloatingPoint& cell_data = block.at(cell_index); + cell_data = clampedAdd(cell_data, update); +} + +inline bool HashedBlocks::hasBlock(const Index3D& block_index) const { + return block_map_.hasBlock(block_index); +} + +inline HashedBlocks::Block* HashedBlocks::getBlock(const Index3D& block_index) { + return block_map_.getBlock(block_index); +} + +inline const HashedBlocks::Block* HashedBlocks::getBlock( + const Index3D& block_index) const { + return block_map_.getBlock(block_index); +} + +inline HashedBlocks::Block& HashedBlocks::getOrAllocateBlock( + const Index3D& block_index) { + return block_map_.getOrAllocateBlock(block_index, default_value_); +} +} // namespace wavemap + +#endif // WAVEMAP_MAP_IMPL_HASHED_BLOCKS_INL_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_chunked_wavelet_octree_block_inl.h b/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_block_inl.h similarity index 90% rename from libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_chunked_wavelet_octree_block_inl.h rename to libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_block_inl.h index bc7559e6a..1954a69ab 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_chunked_wavelet_octree_block_inl.h +++ b/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_block_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ +#ifndef WAVEMAP_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ +#define WAVEMAP_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ #include "wavemap/utils/query/occupancy_classifier.h" @@ -61,4 +61,4 @@ inline FloatingPoint HashedChunkedWaveletOctreeBlock::getCellValue( } } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ +#endif // WAVEMAP_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_chunked_wavelet_octree_inl.h b/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_inl.h similarity index 60% rename from libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_chunked_wavelet_octree_inl.h rename to libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_inl.h index 9e1300de9..a634a4dff 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_chunked_wavelet_octree_inl.h +++ b/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_inl.h @@ -1,39 +1,39 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_INL_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_INL_H_ +#ifndef WAVEMAP_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_INL_H_ +#define WAVEMAP_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_INL_H_ #include "wavemap/indexing/index_conversions.h" namespace wavemap { inline size_t HashedChunkedWaveletOctree::size() const { size_t size = 0u; - for (const auto& [block_index, block] : blocks_) { + forEachBlock([&size](const BlockIndex& /*block_index*/, const Block& block) { size += block.size(); - } + }); return size; } inline FloatingPoint HashedChunkedWaveletOctree::getCellValue( const Index3D& index) const { const BlockIndex block_index = computeBlockIndexFromIndex(index); - if (hasBlock(block_index)) { - const auto& block = getBlock(block_index); - const CellIndex cell_index = - computeCellIndexFromBlockIndexAndIndex(block_index, {0, index}); - return block.getCellValue(cell_index); + const Block* block = getBlock(block_index); + if (!block) { + return 0.f; } - return 0.f; + const CellIndex cell_index = + computeCellIndexFromBlockIndexAndIndex(block_index, {0, index}); + return block->getCellValue(cell_index); } inline FloatingPoint HashedChunkedWaveletOctree::getCellValue( const OctreeIndex& index) const { const BlockIndex block_index = computeBlockIndexFromIndex(index); - if (hasBlock(block_index)) { - const auto& block = getBlock(block_index); - const CellIndex cell_index = - computeCellIndexFromBlockIndexAndIndex(block_index, index); - return block.getCellValue(cell_index); + const Block* block = getBlock(block_index); + if (!block) { + return 0.f; } - return 0.f; + const CellIndex cell_index = + computeCellIndexFromBlockIndexAndIndex(block_index, index); + return block->getCellValue(cell_index); } inline void HashedChunkedWaveletOctree::setCellValue(const Index3D& index, @@ -56,33 +56,24 @@ inline void HashedChunkedWaveletOctree::addToCellValue(const Index3D& index, inline bool HashedChunkedWaveletOctree::hasBlock( const Index3D& block_index) const { - return blocks_.count(block_index); -} - -inline HashedChunkedWaveletOctree::Block& -HashedChunkedWaveletOctree::getOrAllocateBlock(const Index3D& block_index) { - if (!hasBlock(block_index)) { - blocks_.try_emplace(block_index, config_.tree_height, config_.min_log_odds, - config_.max_log_odds); - } - return blocks_.at(block_index); + return block_map_.hasBlock(block_index); } -inline HashedChunkedWaveletOctree::Block& HashedChunkedWaveletOctree::getBlock( +inline HashedChunkedWaveletOctree::Block* HashedChunkedWaveletOctree::getBlock( const Index3D& block_index) { - return blocks_.at(block_index); + return block_map_.getBlock(block_index); } -inline const HashedChunkedWaveletOctree::Block& +inline const HashedChunkedWaveletOctree::Block* HashedChunkedWaveletOctree::getBlock(const Index3D& block_index) const { - return blocks_.at(block_index); + return block_map_.getBlock(block_index); } -inline void HashedChunkedWaveletOctree::forEachLeaf( - VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn) const { - for (const auto& [block_index, block] : blocks_) { - block.forEachLeaf(block_index, visitor_fn); - } +inline HashedChunkedWaveletOctree::Block& +HashedChunkedWaveletOctree::getOrAllocateBlock(const Index3D& block_index) { + return block_map_.getOrAllocateBlock(block_index, config_.tree_height, + config_.min_log_odds, + config_.max_log_odds); } inline HashedChunkedWaveletOctree::CellIndex @@ -97,4 +88,4 @@ HashedChunkedWaveletOctree::computeCellIndexFromBlockIndexAndIndex( } } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_INL_H_ +#endif // WAVEMAP_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_INL_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_wavelet_octree_block_inl.h b/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_block_inl.h similarity index 84% rename from libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_wavelet_octree_block_inl.h rename to libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_block_inl.h index 4cf975de0..f3ed57e8a 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_wavelet_octree_block_inl.h +++ b/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_block_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ +#ifndef WAVEMAP_MAP_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ +#define WAVEMAP_MAP_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ #include "wavemap/utils/query/occupancy_classifier.h" @@ -36,4 +36,4 @@ inline FloatingPoint HashedWaveletOctreeBlock::getCellValue( } } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ +#endif // WAVEMAP_MAP_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_wavelet_octree_inl.h b/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_inl.h similarity index 59% rename from libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_wavelet_octree_inl.h rename to libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_inl.h index 7f1896149..7e49c8ebf 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/hashed_wavelet_octree_inl.h +++ b/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_inl.h @@ -1,39 +1,39 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_HASHED_WAVELET_OCTREE_INL_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_HASHED_WAVELET_OCTREE_INL_H_ +#ifndef WAVEMAP_MAP_IMPL_HASHED_WAVELET_OCTREE_INL_H_ +#define WAVEMAP_MAP_IMPL_HASHED_WAVELET_OCTREE_INL_H_ #include "wavemap/indexing/index_conversions.h" namespace wavemap { inline size_t HashedWaveletOctree::size() const { size_t size = 0u; - for (const auto& [block_index, block] : blocks_) { + forEachBlock([&size](const BlockIndex& /*block_index*/, const Block& block) { size += block.size(); - } + }); return size; } inline FloatingPoint HashedWaveletOctree::getCellValue( const Index3D& index) const { const BlockIndex block_index = computeBlockIndexFromIndex(index); - if (hasBlock(block_index)) { - const auto& block = getBlock(block_index); - const CellIndex cell_index = - computeCellIndexFromBlockIndexAndIndex(block_index, {0, index}); - return block.getCellValue(cell_index); + const Block* block = getBlock(block_index); + if (!block) { + return 0.f; } - return 0.f; + const CellIndex cell_index = + computeCellIndexFromBlockIndexAndIndex(block_index, {0, index}); + return block->getCellValue(cell_index); } inline FloatingPoint HashedWaveletOctree::getCellValue( const OctreeIndex& index) const { const BlockIndex block_index = computeBlockIndexFromIndex(index); - if (hasBlock(block_index)) { - const auto& block = getBlock(block_index); - const CellIndex cell_index = - computeCellIndexFromBlockIndexAndIndex(block_index, index); - return block.getCellValue(cell_index); + const Block* block = getBlock(block_index); + if (!block) { + return 0.f; } - return 0.f; + const CellIndex cell_index = + computeCellIndexFromBlockIndexAndIndex(block_index, index); + return block->getCellValue(cell_index); } inline void HashedWaveletOctree::setCellValue(const Index3D& index, @@ -55,33 +55,24 @@ inline void HashedWaveletOctree::addToCellValue(const Index3D& index, } inline bool HashedWaveletOctree::hasBlock(const Index3D& block_index) const { - return blocks_.count(block_index); -} - -inline HashedWaveletOctree::Block& HashedWaveletOctree::getOrAllocateBlock( - const Index3D& block_index) { - if (!hasBlock(block_index)) { - blocks_.try_emplace(block_index, config_.tree_height, config_.min_log_odds, - config_.max_log_odds); - } - return blocks_.at(block_index); + return block_map_.hasBlock(block_index); } -inline HashedWaveletOctree::Block& HashedWaveletOctree::getBlock( +inline HashedWaveletOctree::Block* HashedWaveletOctree::getBlock( const Index3D& block_index) { - return blocks_.at(block_index); + return block_map_.getBlock(block_index); } -inline const HashedWaveletOctree::Block& HashedWaveletOctree::getBlock( +inline const HashedWaveletOctree::Block* HashedWaveletOctree::getBlock( const Index3D& block_index) const { - return blocks_.at(block_index); + return block_map_.getBlock(block_index); } -inline void HashedWaveletOctree::forEachLeaf( - VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn) const { - for (const auto& [block_index, block] : blocks_) { - block.forEachLeaf(block_index, visitor_fn); - } +inline HashedWaveletOctree::Block& HashedWaveletOctree::getOrAllocateBlock( + const Index3D& block_index) { + return block_map_.getOrAllocateBlock(block_index, config_.tree_height, + config_.min_log_odds, + config_.max_log_odds); } inline HashedWaveletOctree::CellIndex @@ -96,4 +87,4 @@ HashedWaveletOctree::computeCellIndexFromBlockIndexAndIndex( } } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_HASHED_WAVELET_OCTREE_INL_H_ +#endif // WAVEMAP_MAP_IMPL_HASHED_WAVELET_OCTREE_INL_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/volumetric_octree_inl.h b/libraries/wavemap/include/wavemap/map/impl/volumetric_octree_inl.h similarity index 93% rename from libraries/wavemap/include/wavemap/data_structure/volumetric/impl/volumetric_octree_inl.h rename to libraries/wavemap/include/wavemap/map/impl/volumetric_octree_inl.h index 97a184a31..646e5a133 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/volumetric_octree_inl.h +++ b/libraries/wavemap/include/wavemap/map/impl/volumetric_octree_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_VOLUMETRIC_OCTREE_INL_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_VOLUMETRIC_OCTREE_INL_H_ +#ifndef WAVEMAP_MAP_IMPL_VOLUMETRIC_OCTREE_INL_H_ +#define WAVEMAP_MAP_IMPL_VOLUMETRIC_OCTREE_INL_H_ #include #include @@ -88,4 +88,4 @@ VolumetricOctree::getDeepestNodeAtIndex(const Index3D& index) const { } } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_VOLUMETRIC_OCTREE_INL_H_ +#endif // WAVEMAP_MAP_IMPL_VOLUMETRIC_OCTREE_INL_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/wavelet_octree_inl.h b/libraries/wavemap/include/wavemap/map/impl/wavelet_octree_inl.h similarity index 96% rename from libraries/wavemap/include/wavemap/data_structure/volumetric/impl/wavelet_octree_inl.h rename to libraries/wavemap/include/wavemap/map/impl/wavelet_octree_inl.h index 0c58e9cd1..7e316fbc7 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/impl/wavelet_octree_inl.h +++ b/libraries/wavemap/include/wavemap/map/impl/wavelet_octree_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_WAVELET_OCTREE_INL_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_WAVELET_OCTREE_INL_H_ +#ifndef WAVEMAP_MAP_IMPL_WAVELET_OCTREE_INL_H_ +#define WAVEMAP_MAP_IMPL_WAVELET_OCTREE_INL_H_ #include #include @@ -152,4 +152,4 @@ inline void WaveletOctree::addToCellValue(const OctreeIndex& index, } } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_IMPL_WAVELET_OCTREE_INL_H_ +#endif // WAVEMAP_MAP_IMPL_WAVELET_OCTREE_INL_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_data_structure_base.h b/libraries/wavemap/include/wavemap/map/volumetric_data_structure_base.h similarity index 93% rename from libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_data_structure_base.h rename to libraries/wavemap/include/wavemap/map/volumetric_data_structure_base.h index 69a247a7a..116eb5479 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_data_structure_base.h +++ b/libraries/wavemap/include/wavemap/map/volumetric_data_structure_base.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_VOLUMETRIC_DATA_STRUCTURE_BASE_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_VOLUMETRIC_DATA_STRUCTURE_BASE_H_ +#ifndef WAVEMAP_MAP_VOLUMETRIC_DATA_STRUCTURE_BASE_H_ +#define WAVEMAP_MAP_VOLUMETRIC_DATA_STRUCTURE_BASE_H_ #include #include @@ -106,4 +106,4 @@ class VolumetricDataStructureBase { }; } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_VOLUMETRIC_DATA_STRUCTURE_BASE_H_ +#endif // WAVEMAP_MAP_VOLUMETRIC_DATA_STRUCTURE_BASE_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_data_structure_factory.h b/libraries/wavemap/include/wavemap/map/volumetric_data_structure_factory.h similarity index 58% rename from libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_data_structure_factory.h rename to libraries/wavemap/include/wavemap/map/volumetric_data_structure_factory.h index a13b7f1fe..8c9c1dbe5 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_data_structure_factory.h +++ b/libraries/wavemap/include/wavemap/map/volumetric_data_structure_factory.h @@ -1,9 +1,9 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_VOLUMETRIC_DATA_STRUCTURE_FACTORY_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_VOLUMETRIC_DATA_STRUCTURE_FACTORY_H_ +#ifndef WAVEMAP_MAP_VOLUMETRIC_DATA_STRUCTURE_FACTORY_H_ +#define WAVEMAP_MAP_VOLUMETRIC_DATA_STRUCTURE_FACTORY_H_ #include -#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" +#include "wavemap/map/volumetric_data_structure_base.h" namespace wavemap { class VolumetricDataStructureFactory { @@ -19,4 +19,4 @@ class VolumetricDataStructureFactory { }; } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_VOLUMETRIC_DATA_STRUCTURE_FACTORY_H_ +#endif // WAVEMAP_MAP_VOLUMETRIC_DATA_STRUCTURE_FACTORY_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_octree.h b/libraries/wavemap/include/wavemap/map/volumetric_octree.h similarity index 93% rename from libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_octree.h rename to libraries/wavemap/include/wavemap/map/volumetric_octree.h index c636f7ce7..d4a3b20bc 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_octree.h +++ b/libraries/wavemap/include/wavemap/map/volumetric_octree.h @@ -1,14 +1,14 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_VOLUMETRIC_OCTREE_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_VOLUMETRIC_OCTREE_H_ +#ifndef WAVEMAP_MAP_VOLUMETRIC_OCTREE_H_ +#define WAVEMAP_MAP_VOLUMETRIC_OCTREE_H_ #include #include #include "wavemap/config/config_base.h" #include "wavemap/data_structure/ndtree/ndtree.h" -#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" #include "wavemap/indexing/index_conversions.h" #include "wavemap/indexing/ndtree_index.h" +#include "wavemap/map/volumetric_data_structure_base.h" namespace wavemap { /** @@ -130,6 +130,6 @@ class VolumetricOctree : public VolumetricDataStructureBase { }; } // namespace wavemap -#include "wavemap/data_structure/volumetric/impl/volumetric_octree_inl.h" +#include "wavemap/map/impl/volumetric_octree_inl.h" -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_VOLUMETRIC_OCTREE_H_ +#endif // WAVEMAP_MAP_VOLUMETRIC_OCTREE_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/wavelet_octree.h b/libraries/wavemap/include/wavemap/map/wavelet_octree.h similarity index 93% rename from libraries/wavemap/include/wavemap/data_structure/volumetric/wavelet_octree.h rename to libraries/wavemap/include/wavemap/map/wavelet_octree.h index 3bf1f860e..e8b4f72fc 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/wavelet_octree.h +++ b/libraries/wavemap/include/wavemap/map/wavelet_octree.h @@ -1,15 +1,15 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_WAVELET_OCTREE_H_ -#define WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_WAVELET_OCTREE_H_ +#ifndef WAVEMAP_MAP_WAVELET_OCTREE_H_ +#define WAVEMAP_MAP_WAVELET_OCTREE_H_ #include #include #include "wavemap/config/config_base.h" #include "wavemap/data_structure/ndtree/ndtree.h" -#include "wavemap/data_structure/volumetric/cell_types/haar_transform.h" -#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" #include "wavemap/indexing/index_conversions.h" #include "wavemap/indexing/ndtree_index.h" +#include "wavemap/map/cell_types/haar_transform.h" +#include "wavemap/map/volumetric_data_structure_base.h" namespace wavemap { /** @@ -146,6 +146,6 @@ class WaveletOctree : public VolumetricDataStructureBase { }; } // namespace wavemap -#include "wavemap/data_structure/volumetric/impl/wavelet_octree_inl.h" +#include "wavemap/map/impl/wavelet_octree_inl.h" -#endif // WAVEMAP_DATA_STRUCTURE_VOLUMETRIC_WAVELET_OCTREE_H_ +#endif // WAVEMAP_MAP_WAVELET_OCTREE_H_ diff --git a/libraries/wavemap/include/wavemap/utils/bits/bit_operations.h b/libraries/wavemap/include/wavemap/utils/bits/bit_operations.h index 5912b5b73..9bdb910ad 100644 --- a/libraries/wavemap/include/wavemap/utils/bits/bit_operations.h +++ b/libraries/wavemap/include/wavemap/utils/bits/bit_operations.h @@ -88,13 +88,21 @@ R bit_cast(T bitstring) { } template -std::make_unsigned_t bit_cast_unsigned(T bitstring) { - return bit_cast>(bitstring); +constexpr std::make_unsigned_t bit_cast_unsigned(T bitstring) { + if constexpr (std::is_unsigned_v) { + return bitstring; + } else { + return bit_cast>(bitstring); + } } template -std::make_signed_t bit_cast_signed(T bitstring) { - return bit_cast>(bitstring); +constexpr std::make_signed_t bit_cast_signed(T bitstring) { + if constexpr (std::is_signed_v) { + return bitstring; + } else { + return bit_cast>(bitstring); + } } template diff --git a/libraries/wavemap/include/wavemap/utils/data/comparisons.h b/libraries/wavemap/include/wavemap/utils/data/comparisons.h index 938fcfbea..ffad33312 100644 --- a/libraries/wavemap/include/wavemap/utils/data/comparisons.h +++ b/libraries/wavemap/include/wavemap/utils/data/comparisons.h @@ -5,6 +5,8 @@ #include #include +#include "wavemap/common.h" + namespace wavemap::data { // Test strict equality to zero template @@ -27,6 +29,31 @@ bool is_nonzero(const DataT& data, ThresholdT threshold) { return threshold < std::abs(value); }); } + +template +bool EigenCwise(const FirstType& matrix_a, const SecondType& matrix_b) { + static_assert(static_cast(FirstType::RowsAtCompileTime) == + static_cast(SecondType::RowsAtCompileTime)); + static_assert(static_cast(FirstType::ColsAtCompileTime) == + static_cast(SecondType::ColsAtCompileTime)); + if (ComparisonOp{}(matrix_a.array(), matrix_b.array()).all()) { + return true; + } else { + return false; + } +} + +template +bool EigenCwiseNear(const FirstType& matrix_a, const SecondType& matrix_b, + FloatingPoint precision = kEpsilon) { + static_assert(FirstType::RowsAtCompileTime == SecondType::RowsAtCompileTime); + static_assert(FirstType::ColsAtCompileTime == SecondType::ColsAtCompileTime); + if (((matrix_a - matrix_b).array().abs() < precision).all()) { + return true; + } else { + return false; + } +} } // namespace wavemap::data #endif // WAVEMAP_UTILS_DATA_COMPARISONS_H_ diff --git a/libraries/wavemap/include/wavemap/utils/data/eigen_checks.h b/libraries/wavemap/include/wavemap/utils/data/eigen_checks.h new file mode 100644 index 000000000..d67bc2a6b --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/data/eigen_checks.h @@ -0,0 +1,94 @@ +#ifndef WAVEMAP_UTILS_DATA_EIGEN_CHECKS_H_ +#define WAVEMAP_UTILS_DATA_EIGEN_CHECKS_H_ + +#include + +#include + +#include "wavemap/common.h" +#include "wavemap/utils/data/comparisons.h" +#include "wavemap/utils/print/eigen.h" + +// Define regular checks +#define CHECK_EIGEN_EQ(MatrixA, MatrixB) \ + CHECK(wavemap::data::EigenCwise>(MatrixA, MatrixB)) \ + << "Expected" << print::eigen::oneLine(MatrixA) \ + << " ==" << print::eigen::oneLine(MatrixB) + +#define CHECK_EIGEN_NE(MatrixA, MatrixB) \ + CHECK(wavemap::data::EigenCwise>(MatrixA, MatrixB)) \ + << "Expected" << print::eigen::oneLine(MatrixA) \ + << " !=" << print::eigen::oneLine(MatrixB) + +#define CHECK_EIGEN_LE(MatrixA, MatrixB) \ + CHECK(wavemap::data::EigenCwise>(MatrixA, MatrixB)) \ + << "Expected" << print::eigen::oneLine(MatrixA) \ + << " <=" << print::eigen::oneLine(MatrixB) + +#define CHECK_EIGEN_LT(MatrixA, MatrixB) \ + CHECK(wavemap::data::EigenCwise>(MatrixA, MatrixB)) \ + << "Expected" << print::eigen::oneLine(MatrixA) << " <" \ + << print::eigen::oneLine(MatrixB) + +#define CHECK_EIGEN_GE(MatrixA, MatrixB) \ + CHECK(wavemap::data::EigenCwise>(MatrixA, MatrixB)) \ + << "Expected" << print::eigen::oneLine(MatrixA) \ + << " >=" << print::eigen::oneLine(MatrixB) + +#define CHECK_EIGEN_GT(MatrixA, MatrixB) \ + CHECK(wavemap::data::EigenCwise>(MatrixA, MatrixB)) \ + << "Expected" << print::eigen::oneLine(MatrixA) << " >" \ + << print::eigen::oneLine(MatrixB) + +#define CHECK_EIGEN_NEAR(MatrixA, MatrixB, Tolerance) \ + CHECK(wavemap::data::EigenCwiseNear(MatrixA, MatrixB, Tolerance)) \ + << "Expected" << print::eigen::oneLine(MatrixA) << " near" \ + << print::eigen::oneLine(MatrixB) << " with tolerance " << Tolerance + +// Define debug checks +#if DCHECK_IS_ON() + +#define DCHECK_EIGEN_EQ(MatrixA, MatrixB) CHECK_EIGEN_EQ(MatrixA, MatrixB) +#define DCHECK_EIGEN_NE(MatrixA, MatrixB) CHECK_EIGEN_NE(MatrixA, MatrixB) +#define DCHECK_EIGEN_LE(MatrixA, MatrixB) CHECK_EIGEN_LE(MatrixA, MatrixB) +#define DCHECK_EIGEN_LT(MatrixA, MatrixB) CHECK_EIGEN_LT(MatrixA, MatrixB) +#define DCHECK_EIGEN_GE(MatrixA, MatrixB) CHECK_EIGEN_GE(MatrixA, MatrixB) +#define DCHECK_EIGEN_GT(MatrixA, MatrixB) CHECK_EIGEN_GT(MatrixA, MatrixB) +#define DCHECK_EIGEN_NEAR(MatrixA, MatrixB, Tolerance) \ + CHECK_EIGEN_NEAR(MatrixA, MatrixB, Tolerance) + +// When debug checking is disabled, replace the DCHECKs with empty macros +#else + +#define DCHECK_EIGEN_EQ(MatrixA, MatrixB) \ + GLOG_MSVC_PUSH_DISABLE_WARNING(4127) \ + while (false) GLOG_MSVC_POP_WARNING() CHECK_EIGEN_EQ(MatrixA, MatrixB) + +#define DCHECK_EIGEN_NE(MatrixA, MatrixB) \ + GLOG_MSVC_PUSH_DISABLE_WARNING(4127) \ + while (false) GLOG_MSVC_POP_WARNING() CHECK_EIGEN_NE(MatrixA, MatrixB) + +#define DCHECK_EIGEN_LE(MatrixA, MatrixB) \ + GLOG_MSVC_PUSH_DISABLE_WARNING(4127) \ + while (false) GLOG_MSVC_POP_WARNING() CHECK_EIGEN_LE(MatrixA, MatrixB) + +#define DCHECK_EIGEN_LT(MatrixA, MatrixB) \ + GLOG_MSVC_PUSH_DISABLE_WARNING(4127) \ + while (false) GLOG_MSVC_POP_WARNING() CHECK_EIGEN_LT(MatrixA, MatrixB) + +#define DCHECK_EIGEN_GE(MatrixA, MatrixB) \ + GLOG_MSVC_PUSH_DISABLE_WARNING(4127) \ + while (false) GLOG_MSVC_POP_WARNING() CHECK_EIGEN_GE(MatrixA, MatrixB) + +#define DCHECK_EIGEN_GT(MatrixA, MatrixB) \ + GLOG_MSVC_PUSH_DISABLE_WARNING(4127) \ + while (false) GLOG_MSVC_POP_WARNING() CHECK_EIGEN_GT(MatrixA, MatrixB) + +#define DCHECK_EIGEN_NEAR(MatrixA, MatrixB, Tolerance) \ + GLOG_MSVC_PUSH_DISABLE_WARNING(4127) \ + while (false) \ + GLOG_MSVC_POP_WARNING() CHECK_EIGEN_NEAR(MatrixA, MatrixB, Tolerance) + +#endif + +#endif // WAVEMAP_UTILS_DATA_EIGEN_CHECKS_H_ diff --git a/libraries/wavemap/include/wavemap/utils/math/int_math.h b/libraries/wavemap/include/wavemap/utils/math/int_math.h index 4e982aec9..eff1d25c2 100644 --- a/libraries/wavemap/include/wavemap/utils/math/int_math.h +++ b/libraries/wavemap/include/wavemap/utils/math/int_math.h @@ -17,7 +17,11 @@ template constexpr T log2_floor(T value) { static_assert(std::is_integral_v); DCHECK(value != static_cast(0)); - return std::numeric_limits::digits - bit_ops::clz(value); + if (std::is_signed_v) { + return std::numeric_limits::digits - bit_ops::clz(value); + } else { + return std::numeric_limits::digits - bit_ops::clz(value) - 1; + } } template @@ -31,6 +35,11 @@ constexpr T log2_ceil(T value) { } } +template +constexpr bool is_power_of_two(T x) { + return x && ((x & (x - 1)) == 0); +} + template constexpr T mult_exp2(T value, int exp) { return value * exp2(exp); diff --git a/libraries/wavemap/include/wavemap/utils/query/map_interpolator.h b/libraries/wavemap/include/wavemap/utils/query/map_interpolator.h index 1103b283b..5307cce8d 100644 --- a/libraries/wavemap/include/wavemap/utils/query/map_interpolator.h +++ b/libraries/wavemap/include/wavemap/utils/query/map_interpolator.h @@ -2,8 +2,8 @@ #define WAVEMAP_UTILS_QUERY_MAP_INTERPOLATOR_H_ #include "wavemap/common.h" -#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" #include "wavemap/indexing/index_conversions.h" +#include "wavemap/map/volumetric_data_structure_base.h" namespace wavemap::interpolate { FloatingPoint trilinear(const wavemap::VolumetricDataStructureBase& map, diff --git a/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h b/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h index 74c7b1b32..f6cb2a073 100644 --- a/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h +++ b/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h @@ -2,10 +2,10 @@ #define WAVEMAP_UTILS_QUERY_QUERY_ACCELERATOR_H_ #include -#include -#include "wavemap/data_structure/volumetric/hashed_wavelet_octree.h" +#include "wavemap/data_structure/spatial_hash.h" #include "wavemap/indexing/index_hashes.h" +#include "wavemap/map/hashed_wavelet_octree.h" namespace wavemap { class QueryAccelerator { @@ -13,7 +13,7 @@ class QueryAccelerator { static constexpr int kDim = 3; explicit QueryAccelerator(const HashedWaveletOctree& map) - : block_map_(map.getBlocks()), tree_height_(map.getTreeHeight()) {} + : block_map_(map.getHashMap()), tree_height_(map.getTreeHeight()) {} FloatingPoint getCellValue(const Index3D& index) { return getCellValue(OctreeIndex{0, index}); @@ -36,11 +36,11 @@ class QueryAccelerator { DCHECK_LE(height_, tree_height_); } else { // Test if the queried block exists - if (block_map_.count(block_index_)) { + const auto* current_block = block_map_.getBlock(block_index_); + if (current_block) { // If yes, load it - const auto& current_block = block_map_.at(block_index_); - node_stack_[tree_height_] = ¤t_block.getRootNode(); - value_stack_[tree_height_] = current_block.getRootScale(); + node_stack_[tree_height_] = ¤t_block->getRootNode(); + value_stack_[tree_height_] = current_block->getRootScale(); height_ = tree_height_; } else { // Otherwise return ignore this query and return 'unknown' @@ -90,11 +90,9 @@ class QueryAccelerator { using Coefficients = HaarCoefficients; using Transform = HaarTransform; using BlockIndex = Index3D; - using BlockMap = - std::unordered_map>; using NodeType = NdtreeNode; - const BlockMap& block_map_; + const HashedWaveletOctree::BlockHashMap& block_map_; const IndexElement tree_height_; std::array> node_stack_{}; diff --git a/libraries/wavemap/include/wavemap/utils/sdf/cell_neighborhoods.h b/libraries/wavemap/include/wavemap/utils/sdf/cell_neighborhoods.h new file mode 100644 index 000000000..d958edb15 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/sdf/cell_neighborhoods.h @@ -0,0 +1,12 @@ +#ifndef WAVEMAP_UTILS_SDF_CELL_NEIGHBORHOODS_H_ +#define WAVEMAP_UTILS_SDF_CELL_NEIGHBORHOODS_H_ + +#include "wavemap/common.h" + +namespace wavemap::neighborhood { +std::array generateNeighborIndexOffsets(); +std::array generateNeighborDistanceOffsets( + FloatingPoint cell_width); +} // namespace wavemap::neighborhood + +#endif // WAVEMAP_UTILS_SDF_CELL_NEIGHBORHOODS_H_ diff --git a/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h b/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h index 691de1987..95a4ffb3c 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h +++ b/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h @@ -1,20 +1,13 @@ #ifndef WAVEMAP_UTILS_SDF_QUASI_EUCLIDEAN_SDF_GENERATOR_H_ #define WAVEMAP_UTILS_SDF_QUASI_EUCLIDEAN_SDF_GENERATOR_H_ -#include "wavemap/data_structure/volumetric/hashed_blocks.h" -#include "wavemap/data_structure/volumetric/hashed_wavelet_octree.h" -#include "wavemap/utils/iterate/grid_iterator.h" +#include "wavemap/data_structure/bucket_queue.h" +#include "wavemap/map/hashed_blocks.h" +#include "wavemap/map/hashed_wavelet_octree.h" #include "wavemap/utils/query/occupancy_classifier.h" -#include "wavemap/utils/query/query_accelerator.h" -#include "wavemap/utils/sdf/bucket_queue.h" +#include "wavemap/utils/sdf/cell_neighborhoods.h" namespace wavemap { -namespace neighborhood { -std::array generateNeighborIndexOffsets(); -std::array generateNeighborDistanceOffsets( - FloatingPoint cell_width); -} // namespace neighborhood - class QuasiEuclideanSDFGenerator { public: static constexpr FloatingPoint kMaxMultiplicativeError = 0.125f + 1e-3f; diff --git a/libraries/wavemap/src/data_structure/volumetric/hashed_blocks.cc b/libraries/wavemap/src/data_structure/volumetric/hashed_blocks.cc deleted file mode 100644 index fd34028c3..000000000 --- a/libraries/wavemap/src/data_structure/volumetric/hashed_blocks.cc +++ /dev/null @@ -1,65 +0,0 @@ -#include "wavemap/data_structure/volumetric/hashed_blocks.h" - -namespace wavemap { -void HashedBlocks::prune() { - std::unordered_set> blocks_to_delete; - for (const auto& [block_index, block_data] : blocks_) { - if (std::all_of(block_data.begin(), block_data.end(), - [default_value = default_value_](const auto& cell_value) { - return cell_value == default_value; - })) { - blocks_to_delete.emplace(block_index); - } - } - std::for_each(blocks_to_delete.cbegin(), blocks_to_delete.cend(), - [&blocks = blocks_](const auto& block_index) { - blocks.erase(block_index); - }); -} - -Index3D HashedBlocks::getMinIndex() const { - if (empty()) { - return Index3D::Zero(); - } - - Index3D min_block_index = - Index3D::Constant(std::numeric_limits::max()); - for (const auto& [block_index, block] : blocks_) { - min_block_index = min_block_index.cwiseMin(block_index); - } - return kCellsPerSide * min_block_index; -} - -Index3D HashedBlocks::getMaxIndex() const { - if (empty()) { - return Index3D::Zero(); - } - - Index3D max_block_index = - Index3D::Constant(std::numeric_limits::lowest()); - for (const auto& [block_index, block] : blocks_) { - max_block_index = max_block_index.cwiseMax(block_index); - } - return kCellsPerSide * (max_block_index + Index3D::Ones()); -} - -void HashedBlocks::forEachLeaf( - VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn) const { - const Index3D min_local_cell_index = Index3D::Zero(); - const Index3D max_local_cell_index = Index3D::Constant(kCellsPerSide - 1); - - for (const auto& [block_index, block_data] : blocks_) { - // TODO(victorr): Iterate directly over linear index instead of grid - for (const Index3D& cell_index : - Grid(min_local_cell_index, max_local_cell_index)) { - const FloatingPoint cell_data = - block_data[convert::indexToLinearIndex(cell_index)]; - const Index3D index = - computeIndexFromBlockIndexAndCellIndex(block_index, cell_index); - const OctreeIndex hierarchical_cell_index = - convert::indexAndHeightToNodeIndex(index, 0); - visitor_fn(hierarchical_cell_index, cell_data); - } - } -} -} // namespace wavemap diff --git a/libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree.cc b/libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree.cc deleted file mode 100644 index a612a3fb8..000000000 --- a/libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree.cc +++ /dev/null @@ -1,99 +0,0 @@ -#include "wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree.h" - -#include - -#include - -namespace wavemap { -DECLARE_CONFIG_MEMBERS(HashedChunkedWaveletOctreeConfig, - (min_cell_width) - (min_log_odds) - (max_log_odds) - (tree_height) - (only_prune_blocks_if_unused_for)); - -bool HashedChunkedWaveletOctreeConfig::isValid(bool verbose) const { - bool is_valid = true; - - is_valid &= IS_PARAM_GT(min_cell_width, 0.f, verbose); - is_valid &= IS_PARAM_LT(min_log_odds, max_log_odds, verbose); - is_valid &= IS_PARAM_GT(tree_height, 0, verbose); - is_valid &= IS_PARAM_LE(tree_height, kMaxSupportedTreeHeight, verbose); - - return is_valid; -} - -void HashedChunkedWaveletOctree::threshold() { - ZoneScoped; - for (auto& [block_index, block] : blocks_) { - block.threshold(); - } -} - -void HashedChunkedWaveletOctree::prune() { - ZoneScoped; - std::unordered_set> blocks_to_remove; - for (auto& [block_index, block] : blocks_) { - block.prune(); - if (block.empty()) { - blocks_to_remove.emplace(block_index); - } - } - for (const auto& index : blocks_to_remove) { - blocks_.erase(index); - } -} - -void HashedChunkedWaveletOctree::pruneSmart() { - ZoneScoped; - std::unordered_set> blocks_to_remove; - for (auto& [block_index, block] : blocks_) { - if (config_.only_prune_blocks_if_unused_for < - block.getTimeSinceLastUpdated()) { - block.prune(); - } - if (block.empty()) { - blocks_to_remove.emplace(block_index); - } - } - for (const auto& index : blocks_to_remove) { - blocks_.erase(index); - } -} - -size_t HashedChunkedWaveletOctree::getMemoryUsage() const { - ZoneScoped; - // TODO(victorr): Also include the memory usage of the unordered map itself - size_t memory_usage = 0u; - for (const auto& [block_index, block] : blocks_) { - memory_usage += block.getMemoryUsage(); - } - return memory_usage; -} - -Index3D HashedChunkedWaveletOctree::getMinIndex() const { - if (empty()) { - return Index3D::Zero(); - } - - Index3D min_block_index = - Index3D::Constant(std::numeric_limits::max()); - for (const auto& [block_index, block] : blocks_) { - min_block_index = min_block_index.cwiseMin(block_index); - } - return cells_per_block_side_ * min_block_index; -} - -Index3D HashedChunkedWaveletOctree::getMaxIndex() const { - if (empty()) { - return Index3D::Zero(); - } - - Index3D max_block_index = - Index3D::Constant(std::numeric_limits::lowest()); - for (const auto& [block_index, block] : blocks_) { - max_block_index = max_block_index.cwiseMax(block_index); - } - return cells_per_block_side_ * (max_block_index + Index3D::Ones()); -} -} // namespace wavemap diff --git a/libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree.cc b/libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree.cc deleted file mode 100644 index e605a4c36..000000000 --- a/libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree.cc +++ /dev/null @@ -1,98 +0,0 @@ -#include "wavemap/data_structure/volumetric/hashed_wavelet_octree.h" - -#include - -#include - -namespace wavemap { -DECLARE_CONFIG_MEMBERS(HashedWaveletOctreeConfig, - (min_cell_width) - (min_log_odds) - (max_log_odds) - (tree_height) - (only_prune_blocks_if_unused_for)); - -bool HashedWaveletOctreeConfig::isValid(bool verbose) const { - bool is_valid = true; - - is_valid &= IS_PARAM_GT(min_cell_width, 0.f, verbose); - is_valid &= IS_PARAM_LT(min_log_odds, max_log_odds, verbose); - is_valid &= IS_PARAM_GT(tree_height, 0, verbose); - - return is_valid; -} - -void HashedWaveletOctree::threshold() { - ZoneScoped; - for (auto& [block_index, block] : blocks_) { - block.threshold(); - } -} - -void HashedWaveletOctree::prune() { - ZoneScoped; - std::unordered_set> blocks_to_remove; - for (auto& [block_index, block] : blocks_) { - block.prune(); - if (block.empty()) { - blocks_to_remove.emplace(block_index); - } - } - for (const auto& index : blocks_to_remove) { - blocks_.erase(index); - } -} - -void HashedWaveletOctree::pruneSmart() { - ZoneScoped; - std::unordered_set> blocks_to_remove; - for (auto& [block_index, block] : blocks_) { - if (config_.only_prune_blocks_if_unused_for < - block.getTimeSinceLastUpdated()) { - block.prune(); - } - if (block.empty()) { - blocks_to_remove.emplace(block_index); - } - } - for (const auto& index : blocks_to_remove) { - blocks_.erase(index); - } -} - -size_t HashedWaveletOctree::getMemoryUsage() const { - ZoneScoped; - // TODO(victorr): Also include the memory usage of the unordered map itself - size_t memory_usage = 0u; - for (const auto& [block_index, block] : blocks_) { - memory_usage += block.getMemoryUsage(); - } - return memory_usage; -} - -Index3D HashedWaveletOctree::getMinIndex() const { - if (empty()) { - return Index3D::Zero(); - } - - Index3D min_block_index = - Index3D::Constant(std::numeric_limits::max()); - for (const auto& [block_index, block] : blocks_) { - min_block_index = min_block_index.cwiseMin(block_index); - } - return cells_per_block_side_ * min_block_index; -} - -Index3D HashedWaveletOctree::getMaxIndex() const { - if (empty()) { - return Index3D::Zero(); - } - - Index3D max_block_index = - Index3D::Constant(std::numeric_limits::lowest()); - for (const auto& [block_index, block] : blocks_) { - max_block_index = max_block_index.cwiseMax(block_index); - } - return cells_per_block_side_ * (max_block_index + Index3D::Ones()); -} -} // namespace wavemap diff --git a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc index dabb04baa..fb96c859e 100644 --- a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc +++ b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc @@ -36,8 +36,9 @@ void HashedChunkedWaveletIntegrator::updateMap() { // Update it with the threadpool for (const auto& block_index : blocks_to_update) { thread_pool_->add_task([this, block_index]() { - auto& block = occupancy_map_->getBlock(block_index); - updateBlock(block, block_index); + if (auto* block = occupancy_map_->getBlock(block_index); block) { + updateBlock(*block, block_index); + } }); } thread_pool_->wait_all(); diff --git a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc index cfc560448..e9bb0e6f5 100644 --- a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc +++ b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc @@ -36,8 +36,9 @@ void HashedWaveletIntegrator::updateMap() { // Update it with the threadpool for (const auto& block_index : blocks_to_update) { thread_pool_->add_task([this, block_index]() { - auto& block = occupancy_map_->getBlock(block_index.position); - updateBlock(block, block_index); + if (auto* block = occupancy_map_->getBlock(block_index.position)) { + updateBlock(*block, block_index); + } }); } thread_pool_->wait_all(); diff --git a/libraries/wavemap/src/map/hashed_blocks.cc b/libraries/wavemap/src/map/hashed_blocks.cc new file mode 100644 index 000000000..8deef9011 --- /dev/null +++ b/libraries/wavemap/src/map/hashed_blocks.cc @@ -0,0 +1,42 @@ +#include "wavemap/map/hashed_blocks.h" + +namespace wavemap { +Index3D HashedBlocks::getMinIndex() const { + return kCellsPerSide * getMinBlockIndex(); +} + +Index3D HashedBlocks::getMaxIndex() const { + if (empty()) { + return Index3D::Zero(); + } + return kCellsPerSide * (getMaxBlockIndex().array() + 1) - 1; +} + +void HashedBlocks::prune() { + block_map_.eraseBlockIf( + [default_value = default_value_](const BlockIndex& /*block_index*/, + const Block& block) { + return std::all_of(block.data().cbegin(), block.data().cend(), + [default_value](const auto& cell_value) { + return cell_value == default_value; + }); + }); +} + +void HashedBlocks::forEachLeaf( + VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn) const { + forEachBlock([&visitor_fn](const BlockIndex& block_index, + const Block& block) { + for (LinearIndex cell_idx = 0u; cell_idx < Block::kCellsPerBlock; + ++cell_idx) { + const Index3D cell_index = + convert::linearIndexToIndex(cell_idx); + const Index3D index = convert::cellAndBlockIndexToIndex( + block_index, cell_index); + const FloatingPoint cell_data = block[cell_idx]; + const OctreeIndex hierarchical_cell_index = OctreeIndex{0, index}; + visitor_fn(hierarchical_cell_index, cell_data); + } + }); +} +} // namespace wavemap diff --git a/libraries/wavemap/src/map/hashed_chunked_wavelet_octree.cc b/libraries/wavemap/src/map/hashed_chunked_wavelet_octree.cc new file mode 100644 index 000000000..36568a08d --- /dev/null +++ b/libraries/wavemap/src/map/hashed_chunked_wavelet_octree.cc @@ -0,0 +1,83 @@ +#include "wavemap/map/hashed_chunked_wavelet_octree.h" + +#include + +#include + +namespace wavemap { +DECLARE_CONFIG_MEMBERS(HashedChunkedWaveletOctreeConfig, + (min_cell_width) + (min_log_odds) + (max_log_odds) + (tree_height) + (only_prune_blocks_if_unused_for)); + +bool HashedChunkedWaveletOctreeConfig::isValid(bool verbose) const { + bool is_valid = true; + + is_valid &= IS_PARAM_GT(min_cell_width, 0.f, verbose); + is_valid &= IS_PARAM_LT(min_log_odds, max_log_odds, verbose); + is_valid &= IS_PARAM_GT(tree_height, 0, verbose); + is_valid &= IS_PARAM_LE(tree_height, kMaxSupportedTreeHeight, verbose); + + return is_valid; +} + +void HashedChunkedWaveletOctree::threshold() { + ZoneScoped; + forEachBlock([](const BlockIndex& /*block_index*/, Block& block) { + block.threshold(); + }); +} + +void HashedChunkedWaveletOctree::prune() { + ZoneScoped; + block_map_.eraseBlockIf([](const BlockIndex& /*block_index*/, Block& block) { + block.prune(); + return block.empty(); + }); +} + +void HashedChunkedWaveletOctree::pruneSmart() { + ZoneScoped; + block_map_.eraseBlockIf( + [&config = config_](const BlockIndex& /*block_index*/, Block& block) { + if (config.only_prune_blocks_if_unused_for < + block.getTimeSinceLastUpdated()) { + block.prune(); + } + return block.empty(); + }); +} + +size_t HashedChunkedWaveletOctree::getMemoryUsage() const { + ZoneScoped; + // TODO(victorr): Also include the memory usage of the unordered map itself + size_t memory_usage = 0u; + forEachBlock( + [&memory_usage](const BlockIndex& /*block_index*/, const Block& block) { + memory_usage += block.getMemoryUsage(); + }); + return memory_usage; +} + +Index3D HashedChunkedWaveletOctree::getMinIndex() const { + return cells_per_block_side_ * getMinBlockIndex(); +} + +Index3D HashedChunkedWaveletOctree::getMaxIndex() const { + if (empty()) { + return Index3D::Zero(); + } + return cells_per_block_side_ * (getMaxBlockIndex().array() + 1) - 1; +} + +void HashedChunkedWaveletOctree::forEachLeaf( + VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn) const { + forEachBlock( + [&visitor_fn](const BlockIndex& block_index, const Block& block) { + block.forEachLeaf(block_index, visitor_fn); + }); +} + +} // namespace wavemap diff --git a/libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree_block.cc b/libraries/wavemap/src/map/hashed_chunked_wavelet_octree_block.cc similarity index 99% rename from libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree_block.cc rename to libraries/wavemap/src/map/hashed_chunked_wavelet_octree_block.cc index 8752516f2..b3214a8af 100644 --- a/libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree_block.cc +++ b/libraries/wavemap/src/map/hashed_chunked_wavelet_octree_block.cc @@ -1,4 +1,4 @@ -#include "wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree_block.h" +#include "wavemap/map/hashed_chunked_wavelet_octree_block.h" #include diff --git a/libraries/wavemap/src/map/hashed_wavelet_octree.cc b/libraries/wavemap/src/map/hashed_wavelet_octree.cc new file mode 100644 index 000000000..0168501ad --- /dev/null +++ b/libraries/wavemap/src/map/hashed_wavelet_octree.cc @@ -0,0 +1,82 @@ +#include "wavemap/map/hashed_wavelet_octree.h" + +#include + +#include + +namespace wavemap { +DECLARE_CONFIG_MEMBERS(HashedWaveletOctreeConfig, + (min_cell_width) + (min_log_odds) + (max_log_odds) + (tree_height) + (only_prune_blocks_if_unused_for)); + +bool HashedWaveletOctreeConfig::isValid(bool verbose) const { + bool is_valid = true; + + is_valid &= IS_PARAM_GT(min_cell_width, 0.f, verbose); + is_valid &= IS_PARAM_LT(min_log_odds, max_log_odds, verbose); + is_valid &= IS_PARAM_GT(tree_height, 0, verbose); + + return is_valid; +} + +void HashedWaveletOctree::threshold() { + ZoneScoped; + forEachBlock([](const BlockIndex& /*block_index*/, Block& block) { + block.threshold(); + }); +} + +void HashedWaveletOctree::prune() { + ZoneScoped; + block_map_.eraseBlockIf([](const BlockIndex& /*block_index*/, Block& block) { + block.prune(); + return block.empty(); + }); +} + +void HashedWaveletOctree::pruneSmart() { + ZoneScoped; + block_map_.eraseBlockIf( + [&config = config_](const BlockIndex& /*block_index*/, Block& block) { + if (config.only_prune_blocks_if_unused_for < + block.getTimeSinceLastUpdated()) { + block.prune(); + } + return block.empty(); + }); +} + +size_t HashedWaveletOctree::getMemoryUsage() const { + ZoneScoped; + // TODO(victorr): Also include the memory usage of the unordered map itself + size_t memory_usage = 0u; + forEachBlock( + [&memory_usage](const BlockIndex& /*block_index*/, const Block& block) { + memory_usage += block.getMemoryUsage(); + }); + return memory_usage; +} + +Index3D HashedWaveletOctree::getMinIndex() const { + return cells_per_block_side_ * getMinBlockIndex(); +} + +Index3D HashedWaveletOctree::getMaxIndex() const { + if (empty()) { + return Index3D::Zero(); + } + return cells_per_block_side_ * (getMaxBlockIndex().array() + 1) - 1; +} + +void HashedWaveletOctree::forEachLeaf( + VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn) const { + forEachBlock( + [&visitor_fn](const BlockIndex& block_index, const Block& block) { + block.forEachLeaf(block_index, visitor_fn); + }); +} + +} // namespace wavemap diff --git a/libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree_block.cc b/libraries/wavemap/src/map/hashed_wavelet_octree_block.cc similarity index 98% rename from libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree_block.cc rename to libraries/wavemap/src/map/hashed_wavelet_octree_block.cc index eb7cebfea..8b21355a8 100644 --- a/libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree_block.cc +++ b/libraries/wavemap/src/map/hashed_wavelet_octree_block.cc @@ -1,4 +1,4 @@ -#include "wavemap/data_structure/volumetric/hashed_wavelet_octree_block.h" +#include "wavemap/map/hashed_wavelet_octree_block.h" #include diff --git a/libraries/wavemap/src/data_structure/volumetric/volumetric_data_structure_base.cc b/libraries/wavemap/src/map/volumetric_data_structure_base.cc similarity index 86% rename from libraries/wavemap/src/data_structure/volumetric/volumetric_data_structure_base.cc rename to libraries/wavemap/src/map/volumetric_data_structure_base.cc index 34c15e230..4d04652cb 100644 --- a/libraries/wavemap/src/data_structure/volumetric/volumetric_data_structure_base.cc +++ b/libraries/wavemap/src/map/volumetric_data_structure_base.cc @@ -1,4 +1,4 @@ -#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" +#include "wavemap/map/volumetric_data_structure_base.h" #include "wavemap/utils/meta/nameof.h" diff --git a/libraries/wavemap/src/data_structure/volumetric/volumetric_data_structure_factory.cc b/libraries/wavemap/src/map/volumetric_data_structure_factory.cc similarity index 87% rename from libraries/wavemap/src/data_structure/volumetric/volumetric_data_structure_factory.cc rename to libraries/wavemap/src/map/volumetric_data_structure_factory.cc index db64dee4c..447f077ae 100644 --- a/libraries/wavemap/src/data_structure/volumetric/volumetric_data_structure_factory.cc +++ b/libraries/wavemap/src/map/volumetric_data_structure_factory.cc @@ -1,10 +1,10 @@ -#include "wavemap/data_structure/volumetric/volumetric_data_structure_factory.h" +#include "wavemap/map/volumetric_data_structure_factory.h" -#include "wavemap/data_structure/volumetric/hashed_blocks.h" -#include "wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree.h" -#include "wavemap/data_structure/volumetric/hashed_wavelet_octree.h" -#include "wavemap/data_structure/volumetric/volumetric_octree.h" -#include "wavemap/data_structure/volumetric/wavelet_octree.h" +#include "wavemap/map/hashed_blocks.h" +#include "wavemap/map/hashed_chunked_wavelet_octree.h" +#include "wavemap/map/hashed_wavelet_octree.h" +#include "wavemap/map/volumetric_octree.h" +#include "wavemap/map/wavelet_octree.h" namespace wavemap { VolumetricDataStructureBase::Ptr VolumetricDataStructureFactory::create( diff --git a/libraries/wavemap/src/data_structure/volumetric/volumetric_octree.cc b/libraries/wavemap/src/map/volumetric_octree.cc similarity index 98% rename from libraries/wavemap/src/data_structure/volumetric/volumetric_octree.cc rename to libraries/wavemap/src/map/volumetric_octree.cc index 4af6e24bc..bb06fb127 100644 --- a/libraries/wavemap/src/data_structure/volumetric/volumetric_octree.cc +++ b/libraries/wavemap/src/map/volumetric_octree.cc @@ -1,4 +1,4 @@ -#include "wavemap/data_structure/volumetric/volumetric_octree.h" +#include "wavemap/map/volumetric_octree.h" #include "wavemap/utils/query/occupancy_classifier.h" diff --git a/libraries/wavemap/src/data_structure/volumetric/wavelet_octree.cc b/libraries/wavemap/src/map/wavelet_octree.cc similarity index 98% rename from libraries/wavemap/src/data_structure/volumetric/wavelet_octree.cc rename to libraries/wavemap/src/map/wavelet_octree.cc index f9ac98c09..87fde1c62 100644 --- a/libraries/wavemap/src/data_structure/volumetric/wavelet_octree.cc +++ b/libraries/wavemap/src/map/wavelet_octree.cc @@ -1,4 +1,4 @@ -#include "wavemap/data_structure/volumetric/wavelet_octree.h" +#include "wavemap/map/wavelet_octree.h" #include "wavemap/utils/query/occupancy_classifier.h" diff --git a/libraries/wavemap/src/utils/sdf/cell_neighborhoods.cc b/libraries/wavemap/src/utils/sdf/cell_neighborhoods.cc new file mode 100644 index 000000000..9a36b141a --- /dev/null +++ b/libraries/wavemap/src/utils/sdf/cell_neighborhoods.cc @@ -0,0 +1,29 @@ +#include "wavemap/utils/sdf/cell_neighborhoods.h" + +#include "wavemap/utils/iterate/grid_iterator.h" + +namespace wavemap::neighborhood { +std::array generateNeighborIndexOffsets() { + std::array neighbor_offsets{}; + size_t array_idx = 0u; + for (const Index3D& index : Grid<3>(-Index3D::Ones(), Index3D::Ones())) { + if (index != Index3D::Zero()) { + neighbor_offsets[array_idx] = index; + ++array_idx; + } + } + return neighbor_offsets; +} + +std::array generateNeighborDistanceOffsets( + FloatingPoint cell_width) { + std::array distance_offsets{}; + size_t array_idx = 0u; + for (const Index3D& index_offset : generateNeighborIndexOffsets()) { + distance_offsets[array_idx] = + cell_width * index_offset.cast().norm(); + ++array_idx; + } + return distance_offsets; +} +} // namespace wavemap::neighborhood diff --git a/libraries/wavemap/src/utils/sdf/sdf_generator.cc b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc similarity index 88% rename from libraries/wavemap/src/utils/sdf/sdf_generator.cc rename to libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc index d42d92fbe..ab91a6ecc 100644 --- a/libraries/wavemap/src/utils/sdf/sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc @@ -1,32 +1,11 @@ -#include - #include "wavemap/utils/sdf/quasi_euclidean_sdf_generator.h" -namespace wavemap { -std::array neighborhood::generateNeighborIndexOffsets() { - std::array neighbor_offsets{}; - size_t array_idx = 0u; - for (const Index3D& index : Grid<3>(-Index3D::Ones(), Index3D::Ones())) { - if (index != Index3D::Zero()) { - neighbor_offsets[array_idx] = index; - ++array_idx; - } - } - return neighbor_offsets; -} +#include -std::array neighborhood::generateNeighborDistanceOffsets( - FloatingPoint cell_width) { - std::array distance_offsets{}; - size_t array_idx = 0u; - for (const Index3D& index_offset : generateNeighborIndexOffsets()) { - distance_offsets[array_idx] = - cell_width * index_offset.cast().norm(); - ++array_idx; - } - return distance_offsets; -} +#include "wavemap/utils/iterate/grid_iterator.h" +#include "wavemap/utils/query/query_accelerator.h" +namespace wavemap { HashedBlocks QuasiEuclideanSDFGenerator::generate( const HashedWaveletOctree& occupancy_map) { ZoneScoped; @@ -97,7 +76,7 @@ void QuasiEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, } // Get the voxel's SDF value - FloatingPoint& sdf_value = *sdf.accessCellData(index, true); + FloatingPoint& sdf_value = sdf.getCellValueRef(index); const bool sdf_uninitialized = sdf.getDefaultCellValue() - kTolerance < sdf_value; @@ -151,7 +130,7 @@ void QuasiEuclideanSDFGenerator::propagate( // Get the neighbor's SDF value const Index3D& neighbor_index = index + kNeighborIndexOffsets[neighbor_idx]; - FloatingPoint& neighbor_sdf = *sdf.accessCellData(neighbor_index, true); + FloatingPoint& neighbor_sdf = sdf.getCellValueRef(neighbor_index); // If the neighbor is uninitialized, get its sign from the occupancy map const bool neighbor_sdf_uninitialized = diff --git a/libraries/wavemap/test/include/wavemap/test/config_generator.h b/libraries/wavemap/test/include/wavemap/test/config_generator.h index bf3418a07..c899d5070 100644 --- a/libraries/wavemap/test/include/wavemap/test/config_generator.h +++ b/libraries/wavemap/test/include/wavemap/test/config_generator.h @@ -4,15 +4,15 @@ #include #include "wavemap/common.h" -#include "wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree.h" -#include "wavemap/data_structure/volumetric/hashed_wavelet_octree.h" -#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" -#include "wavemap/data_structure/volumetric/volumetric_octree.h" -#include "wavemap/data_structure/volumetric/wavelet_octree.h" #include "wavemap/integrator/measurement_model/continuous_beam.h" #include "wavemap/integrator/projection_model/spherical_projector.h" #include "wavemap/integrator/projective/projective_integrator.h" #include "wavemap/integrator/ray_tracing/ray_tracing_integrator.h" +#include "wavemap/map/hashed_chunked_wavelet_octree.h" +#include "wavemap/map/hashed_wavelet_octree.h" +#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/volumetric_octree.h" +#include "wavemap/map/wavelet_octree.h" #include "wavemap/utils/random_number_generator.h" namespace wavemap { diff --git a/libraries/wavemap/test/include/wavemap/test/eigen_utils.h b/libraries/wavemap/test/include/wavemap/test/eigen_utils.h index af9dc836a..7e400e587 100644 --- a/libraries/wavemap/test/include/wavemap/test/eigen_utils.h +++ b/libraries/wavemap/test/include/wavemap/test/eigen_utils.h @@ -2,6 +2,7 @@ #define WAVEMAP_TEST_EIGEN_UTILS_H_ #include +#include #include @@ -9,54 +10,66 @@ namespace wavemap { template -::testing::AssertionResult EigenCwise(const EigenA& matrix_a, - const EigenB& matrix_b) { +::testing::AssertionResult EigenCwise(const std::string& A_str, + const EigenA& A_matrix, + const std::string& op_str, + const std::string& B_str, + const EigenB& B_matrix) { static_assert(static_cast(EigenA::RowsAtCompileTime) == static_cast(EigenB::RowsAtCompileTime)); static_assert(static_cast(EigenA::ColsAtCompileTime) == static_cast(EigenB::ColsAtCompileTime)); - if (ComparisonOp{}(matrix_a.array(), matrix_b.array()).all()) { + if (ComparisonOp{}(A_matrix.array(), B_matrix.array()).all()) { return ::testing::AssertionSuccess(); } else { return ::testing::AssertionFailure() - << "for matrix A" << print::eigen::oneLine(matrix_a) - << " and matrix B" << print::eigen::oneLine(matrix_b); + << "Expected " << A_str << print::eigen::oneLine(A_matrix) << " " + << op_str << " " << B_str << print::eigen::oneLine(B_matrix); } } template -::testing::AssertionResult EigenCwiseNear(const EigenA& matrix_a, - const EigenB& matrix_b, - FloatingPoint precision = kEpsilon) { +::testing::AssertionResult EigenCwiseNear(const std::string& A_str, + const EigenA& A_matrix, + const std::string& B_str, + const EigenB& B_matrix, + const std::string& tolerance_str, + FloatingPoint tolerance = kEpsilon) { static_assert(EigenA::RowsAtCompileTime == EigenB::RowsAtCompileTime); static_assert(EigenA::ColsAtCompileTime == EigenB::ColsAtCompileTime); - if (((matrix_a - matrix_b).array().abs() < precision).all()) { + if (((A_matrix - B_matrix).array().abs() < tolerance).all()) { return ::testing::AssertionSuccess(); } else { return ::testing::AssertionFailure() - << "for matrix A" << print::eigen::oneLine(matrix_a) - << " and matrix B" << print::eigen::oneLine(matrix_b) - << " with precision " << precision; + << "Expected " << A_str << print::eigen::oneLine(A_matrix) + << " near " << B_str << print::eigen::oneLine(B_matrix) + << " with tolerance (" << tolerance_str << ") " << tolerance; } } #define EXPECT_EIGEN_EQ(MatrixA, MatrixB) \ - EXPECT_TRUE(EigenCwise>(MatrixA, MatrixB)) + EXPECT_TRUE( \ + EigenCwise>(#MatrixA, MatrixA, "==", #MatrixB, MatrixB)) #define EXPECT_EIGEN_LT(MatrixA, MatrixB) \ - EXPECT_TRUE(EigenCwise>(MatrixA, MatrixB)) + EXPECT_TRUE( \ + EigenCwise>(#MatrixA, MatrixA, "<", #MatrixB, MatrixB)) -#define EXPECT_EIGEN_LE(MatrixA, MatrixB) \ - EXPECT_TRUE(EigenCwise>(MatrixA, MatrixB)) +#define EXPECT_EIGEN_LE(MatrixA, MatrixB) \ + EXPECT_TRUE(EigenCwise>(#MatrixA, MatrixA, "<=", #MatrixB, \ + MatrixB)) #define EXPECT_EIGEN_GT(MatrixA, MatrixB) \ - EXPECT_TRUE(EigenCwise>(MatrixA, MatrixB)) + EXPECT_TRUE( \ + EigenCwise>(#MatrixA, MatrixA, ">", #MatrixB, MatrixB)) -#define EXPECT_EIGEN_GE(MatrixA, MatrixB) \ - EXPECT_TRUE(EigenCwise>(MatrixA, MatrixB)) +#define EXPECT_EIGEN_GE(MatrixA, MatrixB) \ + EXPECT_TRUE(EigenCwise>(#MatrixA, MatrixA, \ + ">=", #MatrixB, MatrixB)) -#define EXPECT_EIGEN_NEAR(MatrixA, MatrixB, Precision) \ - EXPECT_TRUE(EigenCwiseNear(MatrixA, MatrixB, Precision)) +#define EXPECT_EIGEN_NEAR(MatrixA, MatrixB, Tolerance) \ + EXPECT_TRUE(EigenCwiseNear(#MatrixA, MatrixA, #MatrixB, MatrixB, #Tolerance, \ + Tolerance)) } // namespace wavemap #endif // WAVEMAP_TEST_EIGEN_UTILS_H_ diff --git a/libraries/wavemap/test/src/data_structure/test_haar_cell.cc b/libraries/wavemap/test/src/data_structure/test_haar_cell.cc index 2585e41db..c9c0b9964 100644 --- a/libraries/wavemap/test/src/data_structure/test_haar_cell.cc +++ b/libraries/wavemap/test/src/data_structure/test_haar_cell.cc @@ -1,7 +1,7 @@ #include #include "wavemap/common.h" -#include "wavemap/data_structure/volumetric/cell_types/haar_transform.h" +#include "wavemap/map/cell_types/haar_transform.h" #include "wavemap/test/fixture_base.h" #include "wavemap/utils/print/container.h" diff --git a/libraries/wavemap/test/src/data_structure/test_hashed_blocks.cc b/libraries/wavemap/test/src/data_structure/test_hashed_blocks.cc index 7860b29aa..b2a48c086 100644 --- a/libraries/wavemap/test/src/data_structure/test_hashed_blocks.cc +++ b/libraries/wavemap/test/src/data_structure/test_hashed_blocks.cc @@ -1,7 +1,7 @@ #include #include "wavemap/common.h" -#include "wavemap/data_structure/volumetric/hashed_blocks.h" +#include "wavemap/map/hashed_blocks.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" diff --git a/libraries/wavemap/test/src/data_structure/test_volumetric_data_structure.cc b/libraries/wavemap/test/src/data_structure/test_volumetric_data_structure.cc index 8b9ea00fd..7ccfdc7d9 100644 --- a/libraries/wavemap/test/src/data_structure/test_volumetric_data_structure.cc +++ b/libraries/wavemap/test/src/data_structure/test_volumetric_data_structure.cc @@ -1,14 +1,15 @@ #include #include "wavemap/common.h" -#include "wavemap/data_structure/volumetric/hashed_blocks.h" -#include "wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree.h" -#include "wavemap/data_structure/volumetric/hashed_wavelet_octree.h" -#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" -#include "wavemap/data_structure/volumetric/volumetric_octree.h" -#include "wavemap/data_structure/volumetric/wavelet_octree.h" #include "wavemap/indexing/index_conversions.h" +#include "wavemap/map/hashed_blocks.h" +#include "wavemap/map/hashed_chunked_wavelet_octree.h" +#include "wavemap/map/hashed_wavelet_octree.h" +#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/volumetric_octree.h" +#include "wavemap/map/wavelet_octree.h" #include "wavemap/test/config_generator.h" +#include "wavemap/test/eigen_utils.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" @@ -102,10 +103,8 @@ TYPED_TEST(VolumetricDataStructureTest, MinMaxIndexGetters) { { const Index3D map_min_index = map_base_ptr->getMinIndex(); const Index3D map_max_index = map_base_ptr->getMaxIndex(); - for (OctreeIndex::Element dim = 0; dim < OctreeIndex::kDim; ++dim) { - EXPECT_EQ(map_min_index[dim], 0) << " along dimension " << dim; - EXPECT_EQ(map_max_index[dim], 0) << " along dimension " << dim; - } + EXPECT_EIGEN_EQ(map_min_index, Index3D::Zero()); + EXPECT_EIGEN_EQ(map_max_index, Index3D::Zero()); } const std::vector random_indices = GeometryGenerator::getRandomIndexVector<3>(); @@ -121,12 +120,8 @@ TYPED_TEST(VolumetricDataStructureTest, MinMaxIndexGetters) { { const Index3D map_min_index = map_base_ptr->getMinIndex(); const Index3D map_max_index = map_base_ptr->getMaxIndex(); - for (OctreeIndex::Element dim = 0; dim < OctreeIndex::kDim; ++dim) { - EXPECT_LE(map_min_index[dim], reference_min_index[dim]) - << " along dimension " << dim; - EXPECT_GE(map_max_index[dim], reference_max_index[dim]) - << " along dimension " << dim; - } + EXPECT_EIGEN_LE(map_min_index, reference_min_index); + EXPECT_EIGEN_GE(map_max_index, reference_max_index); } } } diff --git a/libraries/wavemap/test/src/data_structure/test_volumetric_octree.cc b/libraries/wavemap/test/src/data_structure/test_volumetric_octree.cc index 8f5e8c7aa..e0221964b 100644 --- a/libraries/wavemap/test/src/data_structure/test_volumetric_octree.cc +++ b/libraries/wavemap/test/src/data_structure/test_volumetric_octree.cc @@ -1,7 +1,7 @@ #include #include "wavemap/common.h" -#include "wavemap/data_structure/volumetric/volumetric_octree.h" +#include "wavemap/map/volumetric_octree.h" #include "wavemap/test/config_generator.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" diff --git a/libraries/wavemap/test/src/indexing/test_index_conversions.cc b/libraries/wavemap/test/src/indexing/test_index_conversions.cc index 93bf16cdc..2b714d5bb 100644 --- a/libraries/wavemap/test/src/indexing/test_index_conversions.cc +++ b/libraries/wavemap/test/src/indexing/test_index_conversions.cc @@ -16,7 +16,7 @@ namespace wavemap { template class IndexConversionsTest : public FixtureBase, public GeometryGenerator { protected: - static constexpr NdtreeIndexElement kMaxHeight = 14; + static constexpr IndexElement kMaxHeight = 14; const typename NdtreeIndex::Position kMinNdtreePositionIndex = NdtreeIndex::Position::Zero(); const typename NdtreeIndex::Position @@ -60,10 +60,10 @@ TYPED_TEST(IndexConversionsTest, NodeIndexConversions) { TestFixture::kMinNdtreePositionIndex, TestFixture::kMaxNdtreePositionIndex, 1, TestFixture::kMaxHeight); random_indices.emplace_back(NdtreeIndex{0, {0, 0}}); - for (NdtreeIndexElement index_height = 0; - index_height < TestFixture::kMaxHeight; ++index_height) { - for (NdtreeIndexElement index_x = -1; index_x <= 1; ++index_x) { - for (NdtreeIndexElement index_y = -1; index_y <= 1; ++index_y) { + for (IndexElement index_height = 0; index_height < TestFixture::kMaxHeight; + ++index_height) { + for (IndexElement index_x = -1; index_x <= 1; ++index_x) { + for (IndexElement index_y = -1; index_y <= 1; ++index_y) { random_indices.emplace_back( NdtreeIndex{index_height, {index_x, index_y}}); } @@ -160,7 +160,7 @@ TYPED_TEST(IndexConversionsTest, NodeIndexConversions) { const Point aabb_center = 0.5f * (node_aabb.min + node_aabb.max); const FloatingPoint aabb_width = node_aabb.template width<0>(); - const NdtreeIndexElement aabb_height = + const IndexElement aabb_height = convert::cellWidthToHeight(aabb_width, 1.f / random_min_cell_width); const NdtreeIndex roundtrip_node_index = convert::pointToNodeIndex(aabb_center, random_min_cell_width, diff --git a/libraries/wavemap/test/src/indexing/test_ndtree_index.cc b/libraries/wavemap/test/src/indexing/test_ndtree_index.cc index ae6c1dd6c..b5af233c7 100644 --- a/libraries/wavemap/test/src/indexing/test_ndtree_index.cc +++ b/libraries/wavemap/test/src/indexing/test_ndtree_index.cc @@ -14,7 +14,7 @@ namespace wavemap { template class NdtreeIndexTest : public FixtureBase, public GeometryGenerator { protected: - static constexpr NdtreeIndexElement kMaxHeight = 14; + static constexpr IndexElement kMaxHeight = 14; const typename NdtreeIndexT::Position kMinNdtreePositionIndex = NdtreeIndexT::Position::Zero(); const typename NdtreeIndexT::Position kMaxNdtreePositionIndex = diff --git a/libraries/wavemap/test/src/integrator/test_hierarchical_range_image.cc b/libraries/wavemap/test/src/integrator/test_hierarchical_range_image.cc index 19306d658..1202840e1 100644 --- a/libraries/wavemap/test/src/integrator/test_hierarchical_range_image.cc +++ b/libraries/wavemap/test/src/integrator/test_hierarchical_range_image.cc @@ -39,9 +39,8 @@ TEST_F(HierarchicalRangeImage2DTest, PyramidConstruction) { hierarchical_range_image.getImageToPyramidScaleFactor()); // Test all the bounds from top to bottom - const NdtreeIndexElement max_height = - hierarchical_range_image.getMaxHeight(); - for (NdtreeIndexElement height = 0; height <= max_height; ++height) { + const IndexElement max_height = hierarchical_range_image.getMaxHeight(); + for (IndexElement height = 0; height <= max_height; ++height) { const Index2D current_level_dims = int_math::div_exp2_ceil(range_image_dims_scaled, height); for (const Index2D& position : diff --git a/libraries/wavemap/test/src/integrator/test_pointcloud_integrators.cc b/libraries/wavemap/test/src/integrator/test_pointcloud_integrators.cc index 0f136f928..ca9cc81a1 100644 --- a/libraries/wavemap/test/src/integrator/test_pointcloud_integrators.cc +++ b/libraries/wavemap/test/src/integrator/test_pointcloud_integrators.cc @@ -3,11 +3,6 @@ #include #include "wavemap/common.h" -#include "wavemap/data_structure/volumetric/hashed_blocks.h" -#include "wavemap/data_structure/volumetric/hashed_wavelet_octree.h" -#include "wavemap/data_structure/volumetric/volumetric_data_structure_base.h" -#include "wavemap/data_structure/volumetric/volumetric_octree.h" -#include "wavemap/data_structure/volumetric/wavelet_octree.h" #include "wavemap/indexing/index_conversions.h" #include "wavemap/indexing/index_hashes.h" #include "wavemap/integrator/integrator_base.h" @@ -18,6 +13,11 @@ #include "wavemap/integrator/projective/coarse_to_fine/wavelet_integrator.h" #include "wavemap/integrator/projective/fixed_resolution/fixed_resolution_integrator.h" #include "wavemap/integrator/ray_tracing/ray_tracing_integrator.h" +#include "wavemap/map/hashed_blocks.h" +#include "wavemap/map/hashed_wavelet_octree.h" +#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/volumetric_octree.h" +#include "wavemap/map/wavelet_octree.h" #include "wavemap/test/config_generator.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" diff --git a/libraries/wavemap/test/src/integrator/test_range_image_intersector.cc b/libraries/wavemap/test/src/integrator/test_range_image_intersector.cc index b82ef2b51..acc6cae5b 100644 --- a/libraries/wavemap/test/src/integrator/test_range_image_intersector.cc +++ b/libraries/wavemap/test/src/integrator/test_range_image_intersector.cc @@ -92,7 +92,7 @@ TEST_F(RangeImage2DIntersectorTest, RangeImageUpdateType) { integrator_config.min_range, integrator_config.max_range); const FloatingPoint min_cell_width_inv = 1.f / min_cell_width; - constexpr NdtreeIndexElement kMaxHeight = 6; + constexpr IndexElement kMaxHeight = 6; const Index3D min_index = convert::pointToCeilIndex<3>( posed_range_image->getOrigin() - Vector3D::Constant(kMaxRange), min_cell_width_inv); @@ -101,8 +101,7 @@ TEST_F(RangeImage2DIntersectorTest, RangeImageUpdateType) { min_cell_width_inv); for (const Index3D& index : getRandomIndexVector(min_index, max_index, 50, 100)) { - const NdtreeIndexElement height = - getRandomNdtreeIndexHeight(2, kMaxHeight); + const IndexElement height = getRandomNdtreeIndexHeight(2, kMaxHeight); const OctreeIndex query_index = convert::indexAndHeightToNodeIndex(index, height); const Index3D min_reference_index = diff --git a/libraries/wavemap/test/src/utils/test_int_math.cc b/libraries/wavemap/test/src/utils/test_int_math.cc index 3a43bf4d0..ad094bba4 100644 --- a/libraries/wavemap/test/src/utils/test_int_math.cc +++ b/libraries/wavemap/test/src/utils/test_int_math.cc @@ -8,7 +8,11 @@ namespace wavemap { TEST(IntMathTest, Exp2) { constexpr int kMaxExponent = 31; for (int exponent = 0; exponent < kMaxExponent; ++exponent) { - EXPECT_EQ(int_math::exp2(exponent), std::exp2(exponent)) + EXPECT_EQ(int_math::exp2(static_cast(exponent)), + std::exp2(static_cast(exponent))) + << "For exponent " << exponent; + EXPECT_EQ(int_math::exp2(static_cast(exponent)), + std::exp2(static_cast(exponent))) << "For exponent " << exponent; } } @@ -25,9 +29,17 @@ TEST(IntMathTest, Log2) { for (int base_value = 2; 0 < base_value; base_value <<= 1) { for (int offset : {-1, 0, 1}) { const int value = base_value + offset; - EXPECT_EQ(int_math::log2_floor(value), std::floor(std::log2(value))) + EXPECT_EQ(int_math::log2_floor(static_cast(value)), + std::floor(std::log2(static_cast(value)))) + << "For value " << value; + EXPECT_EQ(int_math::log2_ceil(static_cast(value)), + std::ceil(std::log2(static_cast(value)))) + << "For value " << value; + EXPECT_EQ(int_math::log2_floor(static_cast(value)), + std::floor(std::log2(static_cast(value)))) << "For value " << value; - EXPECT_EQ(int_math::log2_ceil(value), std::ceil(std::log2(value))) + EXPECT_EQ(int_math::log2_ceil(static_cast(value)), + std::ceil(std::log2(static_cast(value)))) << "For value " << value; } } diff --git a/libraries/wavemap/test/src/utils/test_map_interpolator.cpp b/libraries/wavemap/test/src/utils/test_map_interpolator.cpp index 4578dbdf9..e2f561c6b 100644 --- a/libraries/wavemap/test/src/utils/test_map_interpolator.cpp +++ b/libraries/wavemap/test/src/utils/test_map_interpolator.cpp @@ -1,7 +1,7 @@ #include -#include "wavemap/data_structure/volumetric/hashed_blocks.h" #include "wavemap/indexing/index_conversions.h" +#include "wavemap/map/hashed_blocks.h" #include "wavemap/utils/iterate/grid_iterator.h" #include "wavemap/utils/query/map_interpolator.h" diff --git a/libraries/wavemap/test/src/utils/test_query_accelerator.cc b/libraries/wavemap/test/src/utils/test_query_accelerator.cc index 5a2516fff..3821fb4ef 100644 --- a/libraries/wavemap/test/src/utils/test_query_accelerator.cc +++ b/libraries/wavemap/test/src/utils/test_query_accelerator.cc @@ -1,7 +1,7 @@ #include #include -#include -#include +#include +#include #include #include #include diff --git a/libraries/wavemap/test/src/utils/test_sdf_generation.cc b/libraries/wavemap/test/src/utils/test_sdf_generation.cc index 218e727cd..c1733016d 100644 --- a/libraries/wavemap/test/src/utils/test_sdf_generation.cc +++ b/libraries/wavemap/test/src/utils/test_sdf_generation.cc @@ -1,8 +1,8 @@ #include #include "wavemap/common.h" -#include "wavemap/data_structure/volumetric/hashed_blocks.h" -#include "wavemap/data_structure/volumetric/hashed_wavelet_octree.h" +#include "wavemap/map/hashed_blocks.h" +#include "wavemap/map/hashed_wavelet_octree.h" #include "wavemap/test/config_generator.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" diff --git a/libraries/wavemap_io/include/wavemap_io/file_conversions.h b/libraries/wavemap_io/include/wavemap_io/file_conversions.h index 2c2fcaca9..2c2331583 100644 --- a/libraries/wavemap_io/include/wavemap_io/file_conversions.h +++ b/libraries/wavemap_io/include/wavemap_io/file_conversions.h @@ -3,7 +3,7 @@ #include -#include +#include #include "wavemap_io/stream_conversions.h" diff --git a/libraries/wavemap_io/include/wavemap_io/stream_conversions.h b/libraries/wavemap_io/include/wavemap_io/stream_conversions.h index 81ad5e383..c91e2b3cf 100644 --- a/libraries/wavemap_io/include/wavemap_io/stream_conversions.h +++ b/libraries/wavemap_io/include/wavemap_io/stream_conversions.h @@ -5,10 +5,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include "wavemap_io/streamable_types.h" diff --git a/libraries/wavemap_io/src/stream_conversions.cc b/libraries/wavemap_io/src/stream_conversions.cc index 751214a09..283408ced 100644 --- a/libraries/wavemap_io/src/stream_conversions.cc +++ b/libraries/wavemap_io/src/stream_conversions.cc @@ -160,11 +160,12 @@ void mapToStream(const HashedWaveletOctree& map, std::ostream& ostream) { hashed_wavelet_octree_header.min_log_odds = map.getMinLogOdds(); hashed_wavelet_octree_header.max_log_odds = map.getMaxLogOdds(); hashed_wavelet_octree_header.tree_height = map.getTreeHeight(); - hashed_wavelet_octree_header.num_blocks = map.getBlocks().size(); + hashed_wavelet_octree_header.num_blocks = map.getHashMap().size(); hashed_wavelet_octree_header.write(ostream); // Iterate over all the map's blocks - for (const auto& [block_index, block] : map.getBlocks()) { + map.forEachBlock([&ostream, min_log_odds, max_log_odds]( + const Index3D& block_index, const auto& block) { // Serialize the block's metadata streamable::HashedWaveletOctreeBlockHeader block_header; block_header.root_node_offset = {block_index.x(), block_index.y(), @@ -208,7 +209,7 @@ void mapToStream(const HashedWaveletOctree& map, std::ostream& ostream) { } streamable_node.write(ostream); } - } + }); } bool streamToMap(std::istream& istream, HashedWaveletOctree::Ptr& map) { @@ -294,11 +295,13 @@ void mapToStream(const HashedChunkedWaveletOctree& map, std::ostream& ostream) { hashed_wavelet_octree_header.min_log_odds = map.getMinLogOdds(); hashed_wavelet_octree_header.max_log_odds = map.getMaxLogOdds(); hashed_wavelet_octree_header.tree_height = map.getTreeHeight(); - hashed_wavelet_octree_header.num_blocks = map.getBlocks().size(); + hashed_wavelet_octree_header.num_blocks = map.getHashMap().size(); hashed_wavelet_octree_header.write(ostream); // Iterate over all the map's blocks - for (const auto& [block_index, block] : map.getBlocks()) { + map.forEachBlock([&ostream, tree_height, chunk_height, min_log_odds, + max_log_odds](const Index3D& block_index, + const auto& block) { // Serialize the block's metadata streamable::HashedWaveletOctreeBlockHeader block_header; block_header.root_node_offset = {block_index.x(), block_index.y(), @@ -377,6 +380,6 @@ void mapToStream(const HashedChunkedWaveletOctree& map, std::ostream& ostream) { } streamable_node.write(ostream); } - } + }); } } // namespace wavemap::io diff --git a/libraries/wavemap_io/test/src/test_file_conversions.cc b/libraries/wavemap_io/test/src/test_file_conversions.cc index c1b8fe988..247119d65 100644 --- a/libraries/wavemap_io/test/src/test_file_conversions.cc +++ b/libraries/wavemap_io/test/src/test_file_conversions.cc @@ -1,9 +1,9 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include diff --git a/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h b/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h index 87c280e43..0dc57f2fd 100644 --- a/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h +++ b/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h @@ -17,12 +17,13 @@ void WavemapServer::publishHashedMap(HashedMapT* hashed_map, // Find the blocks that changed since the last publication time const Timestamp start_time = Time::now(); std::unordered_set changed_blocks; - for (const auto& [block_idx, block] : hashed_map->getBlocks()) { - if (republish_whole_map || - last_map_pub_time_ < block.getLastUpdatedStamp()) { - changed_blocks.emplace(block_idx); + hashed_map->forEachBlock([&changed_blocks, republish_whole_map, + last_pub_time = last_map_pub_time_]( + const Index3D& block_index, const auto& block) { + if (republish_whole_map || last_pub_time < block.getLastUpdatedStamp()) { + changed_blocks.emplace(block_index); } - } + }); last_map_pub_time_ = start_time; // Publish the changed blocks, 'max_num_blocks_per_msg' at a time @@ -30,11 +31,13 @@ void WavemapServer::publishHashedMap(HashedMapT* hashed_map, // Prepare the blocks to publish in the current iteration std::unordered_set blocks_to_publish; int block_cnt = 0; - for (const auto& block_idx : changed_blocks) { - hashed_map->getBlock(block_idx).threshold(); - blocks_to_publish.insert(block_idx); - if (config_.max_num_blocks_per_msg <= ++block_cnt) { - break; + for (const auto& block_index : changed_blocks) { + if (auto* block = hashed_map->getBlock(block_index); block) { + block->threshold(); + blocks_to_publish.insert(block_index); + if (config_.max_num_blocks_per_msg <= ++block_cnt) { + break; + } } } diff --git a/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler.h b/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler.h index 0519a0a51..c06e0c656 100644 --- a/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler.h +++ b/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler.h @@ -10,8 +10,8 @@ #include #include #include -#include #include +#include #include #include diff --git a/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h b/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h index aecb1a948..0a2dbb819 100644 --- a/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h +++ b/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h @@ -12,9 +12,9 @@ #include #include #include -#include #include #include +#include #include #include #include diff --git a/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc b/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc index def282c92..e691fae52 100644 --- a/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc +++ b/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc @@ -44,7 +44,7 @@ PointcloudInputHandler::PointcloudInputHandler( transformer, config_.num_undistortion_interpolation_intervals_per_cloud) { // Subscribe to the pointcloud input - registerCallback(config_.topic_type, [&](auto callback_ptr) { + registerCallback(config_.topic_type, [this, &nh](auto callback_ptr) { pointcloud_sub_ = nh.subscribe( config_.topic_name, config_.topic_queue_length, callback_ptr, this); }); @@ -271,7 +271,8 @@ void PointcloudInputHandler::processQueue() { bool PointcloudInputHandler::hasField(const sensor_msgs::PointCloud2& msg, const std::string& field_name) { return std::any_of(msg.fields.cbegin(), msg.fields.cend(), - [&](const sensor_msgs::PointField& field) { + [&field_name = std::as_const(field_name)]( + const sensor_msgs::PointField& field) { return field.name == field_name; }); } diff --git a/ros/wavemap_ros/src/rosbag_processor.cc b/ros/wavemap_ros/src/rosbag_processor.cc index 35b8a7f4c..9e213fd84 100644 --- a/ros/wavemap_ros/src/rosbag_processor.cc +++ b/ros/wavemap_ros/src/rosbag_processor.cc @@ -30,7 +30,8 @@ bool RosbagProcessor::addRosbags(std::istringstream& rosbag_paths) { bool RosbagProcessor::bagsContainTopic(const std::string& topic_name) { const auto connections = bag_view_.getConnections(); return std::any_of(connections.cbegin(), connections.cend(), - [&](const rosbag::ConnectionInfo* info) { + [&topic_name = std::as_const(topic_name)]( + const rosbag::ConnectionInfo* info) { return info && info->topic == topic_name; }); } diff --git a/ros/wavemap_ros/src/wavemap_server.cc b/ros/wavemap_ros/src/wavemap_server.cc index a353f59c8..1fc557297 100644 --- a/ros/wavemap_ros/src/wavemap_server.cc +++ b/ros/wavemap_ros/src/wavemap_server.cc @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h b/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h index 5b338a63a..4cee0c6e2 100644 --- a/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h +++ b/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h @@ -9,10 +9,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc index e67082040..f739ca643 100644 --- a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc +++ b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc @@ -145,13 +145,13 @@ void mapToRosMsg( msg.max_log_odds = map.getMaxLogOdds(); msg.tree_height = map.getTreeHeight(); - msg.allocated_block_indices.reserve(map.getBlocks().size()); - for (const auto& [block_index, _] : map.getBlocks()) { + msg.allocated_block_indices.reserve(map.getHashMap().size()); + map.forEachBlock([&msg](const Index3D& block_index, const auto& /*block*/) { auto& block_index_msg = msg.allocated_block_indices.emplace_back(); block_index_msg.x = block_index.x(); block_index_msg.y = block_index.y(); block_index_msg.z = block_index.z(); - } + }); // If blocks to include were specified, check that they exist // and remove the ones that don't @@ -166,25 +166,29 @@ void mapToRosMsg( } } else { // Otherwise, include all blocks include_blocks.emplace(); - for (const auto& [block_index, block] : map.getBlocks()) { - include_blocks->emplace(block_index); - } + map.forEachBlock( + [&include_blocks](const Index3D& block_index, const auto& /*block*/) { + include_blocks->emplace(block_index); + }); } // Serialize the specified blocks int block_idx = 0; msg.blocks.resize(include_blocks->size()); for (const auto& block_index : include_blocks.value()) { - const auto& block = map.getBlock(block_index); - auto& block_msg = msg.blocks[block_idx++]; - // If a thread pool was provided, use it - if (thread_pool) { - thread_pool->add_task([&]() { - blockToRosMsg(block_index, block, min_log_odds, max_log_odds, + if (const auto* block = map.getBlock(block_index); block) { + auto& block_msg = msg.blocks[block_idx++]; + // If a thread pool was provided, use it + if (thread_pool) { + thread_pool->add_task( + [block_index, block, min_log_odds, max_log_odds, &block_msg]() { + blockToRosMsg(block_index, *block, min_log_odds, max_log_odds, + block_msg); + }); + } else { // Otherwise, use the current thread + blockToRosMsg(block_index, *block, min_log_odds, max_log_odds, block_msg); - }); - } else { // Otherwise, use the current thread - blockToRosMsg(block_index, block, min_log_odds, max_log_odds, block_msg); + } } } @@ -259,14 +263,10 @@ void rosMsgToMap(const wavemap_msgs::HashedWaveletOctree& msg, allocated_blocks.emplace(block_index.x, block_index.y, block_index.z); } // Remove local blocks that should no longer exist according to the map msg - for (auto it = map->getBlocks().begin(); it != map->getBlocks().end();) { - const auto block_index = it->first; - if (!allocated_blocks.count(block_index)) { - it = map->getBlocks().erase(it); - } else { - ++it; - } - } + map->getHashMap().eraseBlockIf( + [&allocated_blocks](const Index3D& block_index, const auto& /*block*/) { + return !allocated_blocks.count(block_index); + }); } for (const auto& block_msg : msg.blocks) { @@ -319,13 +319,13 @@ void mapToRosMsg( msg.max_log_odds = map.getMaxLogOdds(); msg.tree_height = map.getTreeHeight(); - msg.allocated_block_indices.reserve(map.getBlocks().size()); - for (const auto& [block_index, _] : map.getBlocks()) { + msg.allocated_block_indices.reserve(map.getHashMap().size()); + map.forEachBlock([&msg](const Index3D& block_index, const auto& /*block*/) { auto& block_index_msg = msg.allocated_block_indices.emplace_back(); block_index_msg.x = block_index.x(); block_index_msg.y = block_index.y(); block_index_msg.z = block_index.z(); - } + }); // If blocks to include were specified, check that they exist // and remove the ones that don't @@ -340,26 +340,29 @@ void mapToRosMsg( } } else { // Otherwise, include all blocks include_blocks.emplace(); - for (const auto& [block_index, block] : map.getBlocks()) { - include_blocks->emplace(block_index); - } + map.forEachBlock( + [&include_blocks](const Index3D& block_index, const auto& /*block*/) { + include_blocks->emplace(block_index); + }); } // Serialize the specified blocks int block_idx = 0; msg.blocks.resize(include_blocks->size()); for (const auto& block_index : include_blocks.value()) { - const auto& block = map.getBlock(block_index); - auto& block_msg = msg.blocks[block_idx++]; - // If a thread pool was provided, use it - if (thread_pool) { - thread_pool->add_task([&]() { - blockToRosMsg(block_index, block, min_log_odds, max_log_odds, + if (const auto* block = map.getBlock(block_index); block) { + auto& block_msg = msg.blocks[block_idx++]; + // If a thread pool was provided, use it + if (thread_pool) { + thread_pool->add_task([block_index, block, min_log_odds, max_log_odds, + tree_height, &block_msg]() { + blockToRosMsg(block_index, *block, min_log_odds, max_log_odds, + tree_height, block_msg); + }); + } else { // Otherwise, use the current thread + blockToRosMsg(block_index, *block, min_log_odds, max_log_odds, tree_height, block_msg); - }); - } else { // Otherwise, use the current thread - blockToRosMsg(block_index, block, min_log_odds, max_log_odds, tree_height, - block_msg); + } } } diff --git a/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc b/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc index 4dac3ae6a..425086d83 100644 --- a/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc +++ b/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc @@ -1,9 +1,9 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h index bb4a815e1..dcb6248ea 100644 --- a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h +++ b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h @@ -3,7 +3,7 @@ #include -#include +#include namespace wavemap::rviz_plugin { struct MapAndMutex { diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h index 7e0043c74..c999ce765 100644 --- a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h +++ b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h @@ -1,12 +1,14 @@ #ifndef WAVEMAP_RVIZ_PLUGIN_VISUALS_CELL_SELECTOR_H_ #define WAVEMAP_RVIZ_PLUGIN_VISUALS_CELL_SELECTOR_H_ +#ifndef Q_MOC_RUN #include #include #include #include -#include +#include #include +#endif namespace wavemap::rviz_plugin { struct CellSelectionMode : public TypeSelector { diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h index c96a8824f..c5a5f0bc9 100644 --- a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h +++ b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h @@ -14,7 +14,7 @@ #include #include #include -#include +#include #include "wavemap_rviz_plugin/common.h" #include "wavemap_rviz_plugin/visuals/cell_layer.h" diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h index 19b3c82ae..d5034f1e0 100644 --- a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h +++ b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h @@ -18,8 +18,8 @@ #include #include #include -#include #include +#include #include #include "wavemap_rviz_plugin/common.h" @@ -115,10 +115,10 @@ class VoxelVisual : public QObject { Ogre::Vector3 camera_position_at_last_lod_update_{}; bool force_lod_update_ = true; void updateLOD(const Point3D& camera_position); - static NdtreeIndexElement computeRecommendedBlockLodHeight( + static IndexElement computeRecommendedBlockLodHeight( FloatingPoint distance_to_cam, FloatingPoint min_cell_width, - NdtreeIndexElement min_height, NdtreeIndexElement max_height); - std::optional getCurrentBlockLodHeight( + IndexElement min_height, IndexElement max_height); + std::optional getCurrentBlockLodHeight( IndexElement map_tree_height, const Index3D& block_idx); // Drawing related methods diff --git a/ros/wavemap_rviz_plugin/src/visuals/slice_visual.cc b/ros/wavemap_rviz_plugin/src/visuals/slice_visual.cc index 60bcd4af5..c45f142cb 100644 --- a/ros/wavemap_rviz_plugin/src/visuals/slice_visual.cc +++ b/ros/wavemap_rviz_plugin/src/visuals/slice_visual.cc @@ -73,7 +73,7 @@ void SliceVisual::update() { const int max_height = map->getTreeHeight(); // Cache the intersecting node z-indices in function of node height - const NdtreeIndexElement num_levels = max_height + 1; + const IndexElement num_levels = max_height + 1; std::vector intersecting_indices(num_levels); std::generate(intersecting_indices.begin(), intersecting_indices.end(), [=, height = 0]() mutable { diff --git a/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc b/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc index 97994bcbd..bc7f4c624 100644 --- a/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc +++ b/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc @@ -3,8 +3,8 @@ #include #include #include -#include #include +#include namespace wavemap::rviz_plugin { VoxelVisual::VoxelVisual(Ogre::SceneManager* scene_manager, @@ -129,7 +129,7 @@ void VoxelVisual::updateMap(bool redraw_all) { for (auto it = block_voxel_layers_map_.begin(); it != block_voxel_layers_map_.end();) { const auto block_idx = it->first; - if (!hashed_map->getBlocks().count(block_idx)) { + if (!hashed_map->hasBlock(block_idx)) { it = block_voxel_layers_map_.erase(it); } else { ++it; @@ -139,7 +139,7 @@ void VoxelVisual::updateMap(bool redraw_all) { for (auto it = block_update_queue_.begin(); it != block_update_queue_.end();) { const auto block_idx = it->first; - if (!hashed_map->getBlocks().count(block_idx)) { + if (!hashed_map->hasBlock(block_idx)) { it = block_update_queue_.erase(it); } else { ++it; @@ -149,20 +149,24 @@ void VoxelVisual::updateMap(bool redraw_all) { // Add all blocks that changed since the last publication time // to the drawing queue - const auto min_termination_height = termination_height_property_.getInt(); - for (const auto& [block_idx, block] : hashed_map->getBlocks()) { - // NOTE: Since the queue is stored as a set, there are no duplicates. - if (redraw_all || last_update_time_ < block.getLastUpdatedStamp()) { - block_update_queue_[block_idx] = min_termination_height; - // Force the LODs to be updated, s.t. the new blocks directly get - // drawn at the right max resolution - force_lod_update_ = true; - } - } + hashed_map->forEachBlock( + [&update_queue = block_update_queue_, + &force_lod_update = force_lod_update_, + last_update_time = last_update_time_, redraw_all, + min_termination_height = termination_height_property_.getInt()]( + const Index3D& block_index, const auto& block) { + if (redraw_all || last_update_time < block.getLastUpdatedStamp()) { + update_queue[block_index] = min_termination_height; + // Force the LODs to be updated, s.t. the new blocks directly get + // drawn at the right max resolution + force_lod_update = true; + } + }); } else { // Otherwise, draw the whole octree at once (legacy support) const IndexElement num_levels = tree_height + 1; VoxelsPerLevel voxels_per_level(num_levels); - map->forEachLeaf([&](const auto& cell_index, auto cell_log_odds) { + map->forEachLeaf([&voxels_per_level, this, tree_height, min_cell_width]( + const auto& cell_index, auto cell_log_odds) { appendLeafCenterAndColor(tree_height, min_cell_width, cell_index, cell_log_odds, voxels_per_level); }); @@ -199,40 +203,47 @@ void VoxelVisual::updateLOD(const Point3D& camera_position) { dynamic_cast(map.get()); hashed_map) { const auto min_termination_height = termination_height_property_.getInt(); - for (const auto& [block_idx, block] : hashed_map->getBlocks()) { - // Compute the recommended LOD level height - const OctreeIndex block_node_idx{tree_height, block_idx}; - const AABB block_aabb = - convert::nodeIndexToAABB(block_node_idx, map->getMinCellWidth()); - const FloatingPoint distance_to_cam = - block_aabb.minDistanceTo(camera_position); - const auto term_height_recommended = computeRecommendedBlockLodHeight( - distance_to_cam, hashed_map->getMinCellWidth(), - min_termination_height, tree_height - 1); - - // If the block is already queued to be updated, set the recommended level - if (block_update_queue_.count(block_idx)) { - block_update_queue_[block_idx] = term_height_recommended; - } else { - // Otherwise, only add the block to the update queue if the recommended - // level is higher than what's currently drawn or significantly lower - const auto term_height_current = - getCurrentBlockLodHeight(tree_height, block_idx); - if (term_height_current) { - if (term_height_current.value() < min_termination_height || - term_height_current.value() < term_height_recommended - 1 || - term_height_recommended < term_height_current.value()) { - block_update_queue_[block_idx] = term_height_recommended; + const auto min_cell_width = hashed_map->getMinCellWidth(); + hashed_map->forEachBlock( + [&block_update_queue = block_update_queue_, + &camera_position = std::as_const(camera_position), this, tree_height, + min_termination_height, + min_cell_width](const Index3D& block_index, const auto& /*block*/) { + // Compute the recommended LOD level height + const OctreeIndex block_node_idx{tree_height, block_index}; + const AABB block_aabb = + convert::nodeIndexToAABB(block_node_idx, min_cell_width); + const FloatingPoint distance_to_cam = + block_aabb.minDistanceTo(camera_position); + const auto term_height_recommended = computeRecommendedBlockLodHeight( + distance_to_cam, min_cell_width, min_termination_height, + tree_height - 1); + + // If the block is already queued to be updated, set the recommended + // level + if (block_update_queue.count(block_index)) { + block_update_queue[block_index] = term_height_recommended; + } else { + // Otherwise, only add the block to the update queue if the + // recommended level is higher than what's currently drawn or + // significantly lower + const auto term_height_current = + getCurrentBlockLodHeight(tree_height, block_index); + if (term_height_current) { + if (term_height_current.value() < min_termination_height || + term_height_current.value() < term_height_recommended - 1 || + term_height_recommended < term_height_current.value()) { + block_update_queue_[block_index] = term_height_recommended; + } + } } - } - } - } + }); } } -NdtreeIndexElement VoxelVisual::computeRecommendedBlockLodHeight( +IndexElement VoxelVisual::computeRecommendedBlockLodHeight( FloatingPoint distance_to_cam, FloatingPoint min_cell_width, - NdtreeIndexElement min_height, NdtreeIndexElement max_height) { + IndexElement min_height, IndexElement max_height) { ZoneScoped; // Compute the recommended level based on the size of the cells projected into // the image plane @@ -242,7 +253,7 @@ NdtreeIndexElement VoxelVisual::computeRecommendedBlockLodHeight( min_height, max_height); } -std::optional VoxelVisual::getCurrentBlockLodHeight( +std::optional VoxelVisual::getCurrentBlockLodHeight( IndexElement map_tree_height, const Index3D& block_idx) { ZoneScoped; if (block_voxel_layers_map_.count(block_idx)) { @@ -449,25 +460,27 @@ void VoxelVisual::processBlockUpdateQueue(const Point3D& camera_position) { std::chrono::milliseconds(max_ms_per_frame_property_.getInt()); const auto max_end_time = start_time + max_time_per_frame; for (const auto& [block_idx, _1, _2] : changed_blocks_sorted) { - const auto& block = hashed_map->getBlock(block_idx); - const IndexElement term_height = block_update_queue_[block_idx]; - const int num_levels = tree_height + 1 - term_height; - VoxelsPerLevel voxels_per_level(num_levels); - block.forEachLeaf( - block_idx, - [&](const auto& cell_index, auto cell_log_odds) { - appendLeafCenterAndColor(tree_height, min_cell_width, cell_index, - cell_log_odds, voxels_per_level); - }, - term_height); - drawMultiResolutionVoxels(tree_height, min_cell_width, block_idx, alpha, - voxels_per_level, - block_voxel_layers_map_[block_idx]); - block_update_queue_.erase(block_idx); - - const auto current_time = Time::now(); - if (max_end_time < current_time) { - break; + if (const auto* block = hashed_map->getBlock(block_idx); block) { + const IndexElement term_height = block_update_queue_[block_idx]; + const int num_levels = tree_height + 1 - term_height; + VoxelsPerLevel voxels_per_level(num_levels); + block->forEachLeaf( + block_idx, + [&voxels_per_level, this, tree_height, min_cell_width]( + const auto& cell_index, auto cell_log_odds) { + appendLeafCenterAndColor(tree_height, min_cell_width, cell_index, + cell_log_odds, voxels_per_level); + }, + term_height); + drawMultiResolutionVoxels(tree_height, min_cell_width, block_idx, alpha, + voxels_per_level, + block_voxel_layers_map_[block_idx]); + block_update_queue_.erase(block_idx); + + const auto current_time = Time::now(); + if (max_end_time < current_time) { + break; + } } } } From 373b87c459d7e419474bf07486bb5e9b8507f831 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 20 Oct 2023 13:06:35 +0200 Subject: [PATCH 005/159] Clean up block and cell index conversions for hashed data structures --- .../data_structure/impl/spatial_hash_inl.h | 31 ++------------- .../wavemap/data_structure/spatial_hash.h | 12 ++---- .../include/wavemap/map/hashed_blocks.h | 9 ++++- .../map/hashed_chunked_wavelet_octree.h | 14 +------ .../wavemap/map/hashed_wavelet_octree.h | 14 +------ .../wavemap/map/impl/hashed_blocks_inl.h | 33 ++++++++++------ .../impl/hashed_chunked_wavelet_octree_inl.h | 38 ++++++++++--------- .../map/impl/hashed_wavelet_octree_inl.h | 37 +++++++++--------- .../include/wavemap/utils/math/int_math.h | 15 ++++++++ libraries/wavemap/src/map/hashed_blocks.cc | 3 +- .../wavemap/test/src/utils/test_int_math.cc | 14 +++++++ 11 files changed, 111 insertions(+), 109 deletions(-) diff --git a/libraries/wavemap/include/wavemap/data_structure/impl/spatial_hash_inl.h b/libraries/wavemap/include/wavemap/data_structure/impl/spatial_hash_inl.h index 055418568..6f9cc7223 100644 --- a/libraries/wavemap/include/wavemap/data_structure/impl/spatial_hash_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/impl/spatial_hash_inl.h @@ -6,33 +6,10 @@ namespace wavemap { namespace convert { -template -Index indexToBlockIndex(const Index& index) { - static_assert(int_math::is_power_of_two(cells_per_side), - "Cells per side must be an exact power of 2."); - static constexpr IndexElement kCellsPerSideLog2 = - int_math::log2_floor(cells_per_side); - return int_math::div_exp2_floor(index, kCellsPerSideLog2); -} - -template -Index indexToCellIndex(const Index& index) { - static_assert(int_math::is_power_of_two(cells_per_side), - "Cells per side must be an exact power of 2."); - static constexpr IndexElement kCellsPerSideLog2 = - int_math::log2_floor(cells_per_side); - constexpr IndexElement mask = (1 << kCellsPerSideLog2) - 1; - Index cell_index{}; - for (int dim_idx = 0; dim_idx < dim; ++dim_idx) { - cell_index[dim_idx] = index[dim_idx] & mask; - } - return cell_index; -} - -template -Index cellAndBlockIndexToIndex(const Index& block_index, - const Index& cell_index) { - return cells_per_side * block_index + cell_index; +template +Index indexToBlockIndex(const Index& index, + IndexElement cells_per_block_side_log_2) { + return int_math::div_exp2_floor(index, cells_per_block_side_log_2); } } // namespace convert diff --git a/libraries/wavemap/include/wavemap/data_structure/spatial_hash.h b/libraries/wavemap/include/wavemap/data_structure/spatial_hash.h index 7a2ad9583..0e6999369 100644 --- a/libraries/wavemap/include/wavemap/data_structure/spatial_hash.h +++ b/libraries/wavemap/include/wavemap/data_structure/spatial_hash.h @@ -10,15 +10,9 @@ namespace wavemap { namespace convert { -template -Index indexToBlockIndex(const Index& index); - -template -Index indexToCellIndex(const Index& index); - -template -Index cellAndBlockIndexToIndex(const Index& block_index, - const Index& cell_index); +template +Index indexToBlockIndex(const Index& index, + IndexElement cells_per_block_side_log_2); } // namespace convert template diff --git a/libraries/wavemap/include/wavemap/map/hashed_blocks.h b/libraries/wavemap/include/wavemap/map/hashed_blocks.h index 8abc28c31..a963a4b15 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_blocks.h +++ b/libraries/wavemap/include/wavemap/map/hashed_blocks.h @@ -16,7 +16,9 @@ namespace wavemap { class HashedBlocks : public VolumetricDataStructureBase { private: - static constexpr IndexElement kCellsPerSide = int_math::exp2(4); + static constexpr IndexElement kCellsPerSideLog2 = 4; + static constexpr IndexElement kCellsPerSide = + int_math::exp2(kCellsPerSideLog2); public: using Ptr = std::shared_ptr; @@ -85,6 +87,11 @@ class HashedBlocks : public VolumetricDataStructureBase { private: const FloatingPoint default_value_; BlockHashMap block_map_; + + static Index3D indexToBlockIndex(const Index3D& index); + static Index3D indexToCellIndex(const Index3D& index); + static Index3D cellAndBlockIndexToIndex(const Index3D& block_index, + const Index3D& cell_index); }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree.h b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree.h index f894064be..2eeb70aab 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree.h +++ b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree.h @@ -129,18 +129,8 @@ class HashedChunkedWaveletOctree : public VolumetricDataStructureBase { BlockHashMap block_map_; - // TODO(victorr): Remove - BlockIndex computeBlockIndexFromIndex(const Index3D& index) const { - return int_math::div_exp2_floor(index, config_.tree_height); - } - // TODO(victorr): Remove - BlockIndex computeBlockIndexFromIndex(const OctreeIndex& node_index) const { - const Index3D index = convert::nodeIndexToMinCornerIndex(node_index); - return int_math::div_exp2_floor(index, config_.tree_height); - } - // TODO(victorr): Remove - CellIndex computeCellIndexFromBlockIndexAndIndex( - const BlockIndex& block_index, OctreeIndex index) const; + BlockIndex indexToBlockIndex(const OctreeIndex& node_index) const; + CellIndex indexToCellIndex(OctreeIndex index) const; }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h b/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h index 95131f0ad..b46bcc516 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h +++ b/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h @@ -123,18 +123,8 @@ class HashedWaveletOctree : public VolumetricDataStructureBase { BlockHashMap block_map_; - // TODO(victorr): Remove - BlockIndex computeBlockIndexFromIndex(const Index3D& index) const { - return int_math::div_exp2_floor(index, config_.tree_height); - } - // TODO(victorr): Remove - BlockIndex computeBlockIndexFromIndex(const OctreeIndex& node_index) const { - const Index3D index = convert::nodeIndexToMinCornerIndex(node_index); - return int_math::div_exp2_floor(index, config_.tree_height); - } - // TODO(victorr): Remove - CellIndex computeCellIndexFromBlockIndexAndIndex( - const BlockIndex& block_index, OctreeIndex index) const; + BlockIndex indexToBlockIndex(const OctreeIndex& node_index) const; + CellIndex indexToCellIndex(OctreeIndex index) const; }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/map/impl/hashed_blocks_inl.h b/libraries/wavemap/include/wavemap/map/impl/hashed_blocks_inl.h index 5e2f88d34..9a865096d 100644 --- a/libraries/wavemap/include/wavemap/map/impl/hashed_blocks_inl.h +++ b/libraries/wavemap/include/wavemap/map/impl/hashed_blocks_inl.h @@ -10,39 +10,35 @@ namespace wavemap { inline FloatingPoint HashedBlocks::getCellValue(const Index3D& index) const { - const BlockIndex block_index = - convert::indexToBlockIndex(index); + const BlockIndex block_index = indexToBlockIndex(index); const auto* block = getBlock(block_index); if (!block) { return default_value_; } - const CellIndex cell_index = convert::indexToCellIndex(index); + const CellIndex cell_index = indexToCellIndex(index); return block->at(cell_index); } inline FloatingPoint& HashedBlocks::getCellValueRef(const Index3D& index) { - const BlockIndex block_index = - convert::indexToBlockIndex(index); + const BlockIndex block_index = indexToBlockIndex(index); auto& block = getOrAllocateBlock(block_index); - const CellIndex cell_index = convert::indexToCellIndex(index); + const CellIndex cell_index = indexToCellIndex(index); return block.at(cell_index); } inline void HashedBlocks::setCellValue(const Index3D& index, FloatingPoint new_value) { - const BlockIndex block_index = - convert::indexToBlockIndex(index); + const BlockIndex block_index = indexToBlockIndex(index); Block& block = getOrAllocateBlock(block_index); - const CellIndex cell_index = convert::indexToCellIndex(index); + const CellIndex cell_index = indexToCellIndex(index); block.at(cell_index) = new_value; } inline void HashedBlocks::addToCellValue(const Index3D& index, FloatingPoint update) { - const BlockIndex block_index = - convert::indexToBlockIndex(index); + const BlockIndex block_index = indexToBlockIndex(index); Block& block = getOrAllocateBlock(block_index); - const CellIndex cell_index = convert::indexToCellIndex(index); + const CellIndex cell_index = indexToCellIndex(index); FloatingPoint& cell_data = block.at(cell_index); cell_data = clampedAdd(cell_data, update); } @@ -64,6 +60,19 @@ inline HashedBlocks::Block& HashedBlocks::getOrAllocateBlock( const Index3D& block_index) { return block_map_.getOrAllocateBlock(block_index, default_value_); } + +inline Index3D HashedBlocks::indexToBlockIndex(const Index3D& index) { + return convert::indexToBlockIndex(index, kCellsPerSideLog2); +} + +inline Index3D HashedBlocks::indexToCellIndex(const Index3D& index) { + return int_math::div_exp2_floor_remainder(index, kCellsPerSideLog2); +} + +inline Index3D HashedBlocks::cellAndBlockIndexToIndex( + const Index3D& block_index, const Index3D& cell_index) { + return kCellsPerSide * block_index + cell_index; +} } // namespace wavemap #endif // WAVEMAP_MAP_IMPL_HASHED_BLOCKS_INL_H_ diff --git a/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_inl.h b/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_inl.h index a634a4dff..89824ff6c 100644 --- a/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_inl.h +++ b/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_inl.h @@ -14,43 +14,42 @@ inline size_t HashedChunkedWaveletOctree::size() const { inline FloatingPoint HashedChunkedWaveletOctree::getCellValue( const Index3D& index) const { - const BlockIndex block_index = computeBlockIndexFromIndex(index); + const BlockIndex block_index = + convert::indexToBlockIndex(index, config_.tree_height); const Block* block = getBlock(block_index); if (!block) { return 0.f; } - const CellIndex cell_index = - computeCellIndexFromBlockIndexAndIndex(block_index, {0, index}); + const CellIndex cell_index = indexToCellIndex({0, index}); return block->getCellValue(cell_index); } inline FloatingPoint HashedChunkedWaveletOctree::getCellValue( const OctreeIndex& index) const { - const BlockIndex block_index = computeBlockIndexFromIndex(index); + const BlockIndex block_index = indexToBlockIndex(index); const Block* block = getBlock(block_index); if (!block) { return 0.f; } - const CellIndex cell_index = - computeCellIndexFromBlockIndexAndIndex(block_index, index); + const CellIndex cell_index = indexToCellIndex(index); return block->getCellValue(cell_index); } inline void HashedChunkedWaveletOctree::setCellValue(const Index3D& index, FloatingPoint new_value) { - const BlockIndex block_index = computeBlockIndexFromIndex(index); + const BlockIndex block_index = + convert::indexToBlockIndex(index, config_.tree_height); auto& block = getOrAllocateBlock(block_index); - const CellIndex cell_index = - computeCellIndexFromBlockIndexAndIndex(block_index, {0, index}); + const CellIndex cell_index = indexToCellIndex({0, index}); block.setCellValue(cell_index, new_value); } inline void HashedChunkedWaveletOctree::addToCellValue(const Index3D& index, FloatingPoint update) { - const BlockIndex block_index = computeBlockIndexFromIndex(index); + const BlockIndex block_index = + convert::indexToBlockIndex(index, config_.tree_height); auto& block = getOrAllocateBlock(block_index); - const CellIndex cell_index = - computeCellIndexFromBlockIndexAndIndex(block_index, {0, index}); + const CellIndex cell_index = indexToCellIndex({0, index}); block.addToCellValue(cell_index, update); } @@ -76,14 +75,19 @@ HashedChunkedWaveletOctree::getOrAllocateBlock(const Index3D& block_index) { config_.max_log_odds); } +inline HashedChunkedWaveletOctree::BlockIndex +HashedChunkedWaveletOctree::indexToBlockIndex( + const OctreeIndex& node_index) const { + const Index3D index = convert::nodeIndexToMinCornerIndex(node_index); + return convert::indexToBlockIndex(index, config_.tree_height); +} + inline HashedChunkedWaveletOctree::CellIndex -HashedChunkedWaveletOctree::computeCellIndexFromBlockIndexAndIndex( - const HashedChunkedWaveletOctree::BlockIndex& block_index, - OctreeIndex index) const { +HashedChunkedWaveletOctree::indexToCellIndex(OctreeIndex index) const { DCHECK_LE(index.height, config_.tree_height); const IndexElement height_difference = config_.tree_height - index.height; - index.position -= int_math::mult_exp2(block_index, height_difference); - DCHECK((0 <= index.position.array()).all()); + index.position = + int_math::div_exp2_floor_remainder(index.position, height_difference); return index; } } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_inl.h b/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_inl.h index 7e49c8ebf..137ddad6d 100644 --- a/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_inl.h +++ b/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_inl.h @@ -14,43 +14,42 @@ inline size_t HashedWaveletOctree::size() const { inline FloatingPoint HashedWaveletOctree::getCellValue( const Index3D& index) const { - const BlockIndex block_index = computeBlockIndexFromIndex(index); + const BlockIndex block_index = + convert::indexToBlockIndex(index, config_.tree_height); const Block* block = getBlock(block_index); if (!block) { return 0.f; } - const CellIndex cell_index = - computeCellIndexFromBlockIndexAndIndex(block_index, {0, index}); + const CellIndex cell_index = indexToCellIndex({0, index}); return block->getCellValue(cell_index); } inline FloatingPoint HashedWaveletOctree::getCellValue( const OctreeIndex& index) const { - const BlockIndex block_index = computeBlockIndexFromIndex(index); + const BlockIndex block_index = indexToBlockIndex(index); const Block* block = getBlock(block_index); if (!block) { return 0.f; } - const CellIndex cell_index = - computeCellIndexFromBlockIndexAndIndex(block_index, index); + const CellIndex cell_index = indexToCellIndex(index); return block->getCellValue(cell_index); } inline void HashedWaveletOctree::setCellValue(const Index3D& index, FloatingPoint new_value) { - const BlockIndex block_index = computeBlockIndexFromIndex(index); + const BlockIndex block_index = + convert::indexToBlockIndex(index, config_.tree_height); auto& block = getOrAllocateBlock(block_index); - const CellIndex cell_index = - computeCellIndexFromBlockIndexAndIndex(block_index, {0, index}); + const CellIndex cell_index = indexToCellIndex({0, index}); block.setCellValue(cell_index, new_value); } inline void HashedWaveletOctree::addToCellValue(const Index3D& index, FloatingPoint update) { - const BlockIndex block_index = computeBlockIndexFromIndex(index); + const BlockIndex block_index = + convert::indexToBlockIndex(index, config_.tree_height); auto& block = getOrAllocateBlock(block_index); - const CellIndex cell_index = - computeCellIndexFromBlockIndexAndIndex(block_index, {0, index}); + const CellIndex cell_index = indexToCellIndex({0, index}); block.addToCellValue(cell_index, update); } @@ -75,14 +74,18 @@ inline HashedWaveletOctree::Block& HashedWaveletOctree::getOrAllocateBlock( config_.max_log_odds); } -inline HashedWaveletOctree::CellIndex -HashedWaveletOctree::computeCellIndexFromBlockIndexAndIndex( - const HashedWaveletOctree::BlockIndex& block_index, +inline HashedWaveletOctree::BlockIndex HashedWaveletOctree::indexToBlockIndex( + const OctreeIndex& node_index) const { + const Index3D index = convert::nodeIndexToMinCornerIndex(node_index); + return convert::indexToBlockIndex(index, config_.tree_height); +} + +inline HashedWaveletOctree::CellIndex HashedWaveletOctree::indexToCellIndex( OctreeIndex index) const { DCHECK_LE(index.height, config_.tree_height); const IndexElement height_difference = config_.tree_height - index.height; - index.position -= int_math::mult_exp2(block_index, height_difference); - DCHECK((0 <= index.position.array()).all()); + index.position = + int_math::div_exp2_floor_remainder(index.position, height_difference); return index; } } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/utils/math/int_math.h b/libraries/wavemap/include/wavemap/utils/math/int_math.h index eff1d25c2..09d28bce4 100644 --- a/libraries/wavemap/include/wavemap/utils/math/int_math.h +++ b/libraries/wavemap/include/wavemap/utils/math/int_math.h @@ -51,6 +51,11 @@ constexpr int div_exp2_ceil(int value, int exp) { return (value + exp2(exp) - 1) >> exp; } +constexpr int div_exp2_floor_remainder(int value, int exp) { + const IndexElement mask = (1 << exp) - 1; + return value & mask; +} + template Eigen::Matrix div_exp2_floor(Eigen::Matrix vector, int exp) { @@ -61,6 +66,16 @@ Eigen::Matrix div_exp2_floor(Eigen::Matrix vector, return vector; } +template +Eigen::Matrix div_exp2_floor_remainder( + Eigen::Matrix vector, int exp) { + DCHECK_GE(exp, 0); + for (int dim_idx = 0; dim_idx < dim; ++dim_idx) { + vector[dim_idx] = div_exp2_floor_remainder(vector[dim_idx], exp); + } + return vector; +} + template Eigen::Matrix div_exp2_ceil(Eigen::Matrix vector, int exp) { diff --git a/libraries/wavemap/src/map/hashed_blocks.cc b/libraries/wavemap/src/map/hashed_blocks.cc index 8deef9011..1b9280ea0 100644 --- a/libraries/wavemap/src/map/hashed_blocks.cc +++ b/libraries/wavemap/src/map/hashed_blocks.cc @@ -31,8 +31,7 @@ void HashedBlocks::forEachLeaf( ++cell_idx) { const Index3D cell_index = convert::linearIndexToIndex(cell_idx); - const Index3D index = convert::cellAndBlockIndexToIndex( - block_index, cell_index); + const Index3D index = cellAndBlockIndexToIndex(block_index, cell_index); const FloatingPoint cell_data = block[cell_idx]; const OctreeIndex hierarchical_cell_index = OctreeIndex{0, index}; visitor_fn(hierarchical_cell_index, cell_data); diff --git a/libraries/wavemap/test/src/utils/test_int_math.cc b/libraries/wavemap/test/src/utils/test_int_math.cc index ad094bba4..9f6a48f33 100644 --- a/libraries/wavemap/test/src/utils/test_int_math.cc +++ b/libraries/wavemap/test/src/utils/test_int_math.cc @@ -59,6 +59,20 @@ TEST(IntMathTest, DivExp2) { } } +TEST(IntMathTest, DivExp2Remainder) { + constexpr int kMaxExp = 10; + constexpr int kMaxValue = 2000; + constexpr int kMinValue = -kMaxValue; + for (int exp = 0; exp < kMaxExp; ++exp) { + for (int value = kMinValue; value < kMaxValue; ++value) { + EXPECT_EQ( + int_math::div_exp2_floor_remainder(value, exp), + value - int_math::exp2(exp) * int_math::div_exp2_floor(value, exp)) + << "For value " << value << " and exp " << exp; + } + } +} + TEST(IntMathTest, PowSequence) { // Test trivial (length 1) sequences constexpr std::array expected_pow_sequence_length_1 = {1}; From 31d60cba7319ac4920841d595705eceab8a1f3b8 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 26 Oct 2023 13:36:48 +0200 Subject: [PATCH 006/159] Draft full Euclidean SDF generator --- libraries/wavemap/CMakeLists.txt | 2 + .../wavemap/data_structure/dense_grid.h | 2 +- .../utils/sdf/full_euclidean_sdf_generator.h | 44 ++++ .../sdf/impl/vector_distance_field_inl.h | 69 ++++++ .../utils/sdf/quasi_euclidean_sdf_generator.h | 4 +- .../wavemap/utils/sdf/vector_distance_field.h | 67 ++++++ .../utils/sdf/full_euclidean_sdf_generator.cc | 197 ++++++++++++++++++ .../sdf/quasi_euclidean_sdf_generator.cc | 31 +-- .../src/utils/sdf/vector_distance_field.cc | 3 + .../test/src/utils/test_sdf_generation.cc | 64 ++++-- 10 files changed, 453 insertions(+), 30 deletions(-) create mode 100644 libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h create mode 100644 libraries/wavemap/include/wavemap/utils/sdf/impl/vector_distance_field_inl.h create mode 100644 libraries/wavemap/include/wavemap/utils/sdf/vector_distance_field.h create mode 100644 libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc create mode 100644 libraries/wavemap/src/utils/sdf/vector_distance_field.cc diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index 79c8345ce..2337dac1c 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -67,7 +67,9 @@ add_library(${PROJECT_NAME} src/map/volumetric_data_structure_base.cc src/map/volumetric_data_structure_factory.cc src/utils/sdf/cell_neighborhoods.cc + src/utils/sdf/full_euclidean_sdf_generator.cc src/utils/sdf/quasi_euclidean_sdf_generator.cc + src/utils/sdf/vector_distance_field.cc src/utils/stopwatch.cc src/utils/thread_pool.cc) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/libraries/wavemap/include/wavemap/data_structure/dense_grid.h b/libraries/wavemap/include/wavemap/data_structure/dense_grid.h index 4719b1c50..40b8848c6 100644 --- a/libraries/wavemap/include/wavemap/data_structure/dense_grid.h +++ b/libraries/wavemap/include/wavemap/data_structure/dense_grid.h @@ -21,7 +21,7 @@ class DenseGrid { static constexpr IndexElement kCellsPerBlock = int_math::exp2(dim * kCellsPerSideLog2); - using DataArrayType = std::array; + using DataArrayType = std::array; explicit DenseGrid(const CellDataT& default_value) { if (default_value != CellDataT{}) { diff --git a/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h b/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h new file mode 100644 index 000000000..052d55f8e --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h @@ -0,0 +1,44 @@ +#ifndef WAVEMAP_UTILS_SDF_FULL_EUCLIDEAN_SDF_GENERATOR_H_ +#define WAVEMAP_UTILS_SDF_FULL_EUCLIDEAN_SDF_GENERATOR_H_ + +#include "wavemap/common.h" +#include "wavemap/data_structure/bucket_queue.h" +#include "wavemap/map/hashed_blocks.h" +#include "wavemap/map/hashed_wavelet_octree.h" +#include "wavemap/utils/sdf/cell_neighborhoods.h" +#include "wavemap/utils/sdf/vector_distance_field.h" + +namespace wavemap { +class FullEuclideanSDFGenerator { + public: + static constexpr FloatingPoint kMaxRelativeUnderEstimate = 1e-3f; + static constexpr FloatingPoint kMaxRelativeOverEstimate = 1e-3f; + + explicit FullEuclideanSDFGenerator(FloatingPoint max_distance = 2.f, + FloatingPoint occupancy_threshold = 0.f) + : max_distance_(max_distance), classifier_(occupancy_threshold) {} + + HashedBlocks generate(const HashedWaveletOctree& occupancy_map); + + FloatingPoint getMaxDistance() const { return max_distance_; } + + private: + inline static const auto kNeighborIndexOffsets = + neighborhood::generateNeighborIndexOffsets(); + + const FloatingPoint max_distance_; + const OccupancyClassifier classifier_; + + static FloatingPoint minDistanceTo(const Index3D& child, + const Index3D& parent, + FloatingPoint min_cell_width); + + void seed(const HashedWaveletOctree& occupancy_map, VectorDistanceField& sdf, + BucketQueue& open_queue) const; + void propagate(const HashedWaveletOctree& occupancy_map, + VectorDistanceField& sdf, + BucketQueue& open_queue) const; +}; +} // namespace wavemap + +#endif // WAVEMAP_UTILS_SDF_FULL_EUCLIDEAN_SDF_GENERATOR_H_ diff --git a/libraries/wavemap/include/wavemap/utils/sdf/impl/vector_distance_field_inl.h b/libraries/wavemap/include/wavemap/utils/sdf/impl/vector_distance_field_inl.h new file mode 100644 index 000000000..3817a07b5 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/sdf/impl/vector_distance_field_inl.h @@ -0,0 +1,69 @@ +#ifndef WAVEMAP_UTILS_SDF_IMPL_VECTOR_DISTANCE_FIELD_INL_H_ +#define WAVEMAP_UTILS_SDF_IMPL_VECTOR_DISTANCE_FIELD_INL_H_ + +namespace wavemap { +inline const VectorDistance& VectorDistanceField::getCellValue( + const wavemap::Index3D& index) const { + const BlockIndex block_index = indexToBlockIndex(index); + const auto* block = getBlock(block_index); + if (!block) { + return default_value_; + } + const CellIndex cell_index = indexToCellIndex(index); + return block->at(cell_index); +} + +inline VectorDistance& VectorDistanceField::getCellValueRef( + const Index3D& index) { + const BlockIndex block_index = indexToBlockIndex(index); + auto& block = getOrAllocateBlock(block_index); + const CellIndex cell_index = indexToCellIndex(index); + return block.at(cell_index); +} + +inline VectorDistanceField::Block* VectorDistanceField::getBlock( + const Index3D& block_index) { + return block_map_.getBlock(block_index); +} + +inline const VectorDistanceField::Block* VectorDistanceField::getBlock( + const Index3D& block_index) const { + return block_map_.getBlock(block_index); +} + +inline VectorDistanceField::Block& VectorDistanceField::getOrAllocateBlock( + const Index3D& block_index) { + return block_map_.getOrAllocateBlock(block_index, default_value_); +} + +template +void VectorDistanceField::forEachLeaf( + IndexedLeafVisitorFunction visitor_fn) const { + block_map_.forEachBlock([&visitor_fn](const BlockIndex& block_index, + const Block& block) { + for (LinearIndex cell_idx = 0u; cell_idx < Block::kCellsPerBlock; + ++cell_idx) { + const Index3D cell_index = + convert::linearIndexToIndex(cell_idx); + const Index3D index = cellAndBlockIndexToIndex(block_index, cell_index); + const auto& cell_data = block[cell_idx]; + std::invoke(visitor_fn, index, cell_data); + } + }); +} + +inline Index3D VectorDistanceField::indexToBlockIndex(const Index3D& index) { + return convert::indexToBlockIndex(index, kCellsPerSideLog2); +} + +inline Index3D VectorDistanceField::indexToCellIndex(const Index3D& index) { + return int_math::div_exp2_floor_remainder(index, kCellsPerSideLog2); +} + +inline Index3D VectorDistanceField::cellAndBlockIndexToIndex( + const Index3D& block_index, const Index3D& cell_index) { + return kCellsPerSide * block_index + cell_index; +} +} // namespace wavemap + +#endif // WAVEMAP_UTILS_SDF_IMPL_VECTOR_DISTANCE_FIELD_INL_H_ diff --git a/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h b/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h index 95a4ffb3c..e52e34e1d 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h +++ b/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h @@ -10,7 +10,8 @@ namespace wavemap { class QuasiEuclideanSDFGenerator { public: - static constexpr FloatingPoint kMaxMultiplicativeError = 0.125f + 1e-3f; + static constexpr FloatingPoint kMaxRelativeUnderEstimate = kEpsilon; + static constexpr FloatingPoint kMaxRelativeOverEstimate = 0.125f + 1e-3f; explicit QuasiEuclideanSDFGenerator(FloatingPoint max_distance = 2.f, FloatingPoint occupancy_threshold = 0.f) @@ -21,7 +22,6 @@ class QuasiEuclideanSDFGenerator { FloatingPoint getMaxDistance() const { return max_distance_; } private: - static constexpr FloatingPoint kTolerance = 1e-2f; inline static const auto kNeighborIndexOffsets = neighborhood::generateNeighborIndexOffsets(); diff --git a/libraries/wavemap/include/wavemap/utils/sdf/vector_distance_field.h b/libraries/wavemap/include/wavemap/utils/sdf/vector_distance_field.h new file mode 100644 index 000000000..1112057ef --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/sdf/vector_distance_field.h @@ -0,0 +1,67 @@ +#ifndef WAVEMAP_UTILS_SDF_VECTOR_DISTANCE_FIELD_H_ +#define WAVEMAP_UTILS_SDF_VECTOR_DISTANCE_FIELD_H_ + +#include + +#include "wavemap/common.h" +#include "wavemap/data_structure/dense_grid.h" +#include "wavemap/data_structure/spatial_hash.h" +#include "wavemap/utils/math/int_math.h" + +namespace wavemap { +struct VectorDistance { + Index3D parent = Index3D::Constant(std::numeric_limits::max()); + FloatingPoint distance = 0.f; + + friend bool operator==(const VectorDistance& lhs, const VectorDistance& rhs) { + return lhs.parent == rhs.parent && lhs.distance == rhs.distance; + } + friend bool operator!=(const VectorDistance& lhs, const VectorDistance& rhs) { + return !(lhs == rhs); + } +}; + +class VectorDistanceField { + private: + static constexpr IndexElement kCellsPerSideLog2 = 4; + static constexpr IndexElement kCellsPerSide = + int_math::exp2(kCellsPerSideLog2); + static constexpr IndexElement kDim = 3; + + public: + using BlockIndex = Index3D; + using CellIndex = Index3D; + using Block = DenseGrid; + using BlockHashMap = SpatialHash; + + explicit VectorDistanceField(FloatingPoint min_cell_width, + FloatingPoint default_value = 0.f) + : min_cell_width_(min_cell_width), + default_value_({VectorDistance{}.parent, default_value}) {} + + const VectorDistance& getCellValue(const Index3D& index) const; + VectorDistance& getCellValueRef(const Index3D& index); + const VectorDistance& getDefaultCellValue() const { return default_value_; } + + Block* getBlock(const Index3D& block_index); + const Block* getBlock(const Index3D& block_index) const; + Block& getOrAllocateBlock(const Index3D& block_index); + + template + void forEachLeaf(IndexedLeafVisitorFunction visitor_fn) const; + + private: + const FloatingPoint min_cell_width_; + const VectorDistance default_value_; + BlockHashMap block_map_; + + static Index3D indexToBlockIndex(const Index3D& index); + static Index3D indexToCellIndex(const Index3D& index); + static Index3D cellAndBlockIndexToIndex(const Index3D& block_index, + const Index3D& cell_index); +}; +} // namespace wavemap + +#include "wavemap/utils/sdf/impl/vector_distance_field_inl.h" + +#endif // WAVEMAP_UTILS_SDF_VECTOR_DISTANCE_FIELD_H_ diff --git a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc new file mode 100644 index 000000000..599f110e2 --- /dev/null +++ b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc @@ -0,0 +1,197 @@ +#include "wavemap/utils/sdf/full_euclidean_sdf_generator.h" + +#include + +#include "wavemap/utils/iterate/grid_iterator.h" +#include "wavemap/utils/query/query_accelerator.h" + +namespace wavemap { +HashedBlocks FullEuclideanSDFGenerator::generate( + const HashedWaveletOctree& occupancy_map) { + ZoneScoped; + // Initialize the SDF data structure + const FloatingPoint min_cell_width = occupancy_map.getMinCellWidth(); + VectorDistanceField full_sdf(min_cell_width, max_distance_); + + // Initialize the bucketed priority queue + const int num_bins = + static_cast(std::ceil(max_distance_ / min_cell_width)); + BucketQueue open{num_bins, max_distance_}; + + // Seed and propagate the SDF + seed(occupancy_map, full_sdf, open); + propagate(occupancy_map, full_sdf, open); + + // Copy into regular data structure + const VolumetricDataStructureConfig config{min_cell_width, 0.f, + max_distance_}; + HashedBlocks sdf(config, max_distance_); + full_sdf.forEachLeaf( + [&sdf](const Index3D& cell_index, const VectorDistance& cell_value) { + sdf.setCellValue(cell_index, cell_value.distance); + }); + + return sdf; +} + +void FullEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, + VectorDistanceField& sdf, + BucketQueue& open_queue) const { + ZoneScoped; + // Create an occupancy query accelerator + QueryAccelerator occupancy_query_accelerator{occupancy_map}; + + // For all free cells that border an obstacle: + // - initialize their SDF value, and + // - add them to the open queue + occupancy_map.forEachLeaf([this, &occupancy_query_accelerator, &sdf, + &open_queue, + min_cell_width = occupancy_map.getMinCellWidth()]( + const OctreeIndex& node_index, + FloatingPoint node_occupancy) { + // Only process obstacles + if (OccupancyClassifier::isUnobserved(node_occupancy) || + classifier_.isFree(node_occupancy)) { + return; + } + + // Span a grid at the highest resolution (=SDF resolution) that pads the + // multi-resolution obstacle cell with 1 voxel in all directions + const Index3D min_corner = convert::nodeIndexToMinCornerIndex(node_index); + const Index3D max_corner = convert::nodeIndexToMaxCornerIndex(node_index); + const Grid<3> grid{ + Grid<3>(min_corner - Index3D::Ones(), max_corner + Index3D::Ones())}; + + // Iterate over the grid + for (const Index3D& index : grid) { + // Skip cells that are inside the occupied node (obstacle) + // NOTE: Occupied cells (negative distances) are handled in the + // propagation stage. + const Index3D nearest_inner_index = + index.cwiseMax(min_corner).cwiseMin(max_corner); + const bool voxel_is_inside = (index == nearest_inner_index); + if (voxel_is_inside) { + continue; + } + + // Skip the cell if it is not free + const FloatingPoint occupancy = + occupancy_query_accelerator.getCellValue(index); + if (OccupancyClassifier::isUnobserved(occupancy) || + classifier_.isOccupied(occupancy)) { + continue; + } + + // Get the voxel's SDF value + auto& [sdf_parent, sdf_value] = sdf.getCellValueRef(index); + const bool sdf_uninitialized = + sdf_parent == sdf.getDefaultCellValue().parent; + + // Update the voxel's SDF value + const FloatingPoint distance_to_surface = + minDistanceTo(index, nearest_inner_index, min_cell_width); + if (distance_to_surface < sdf_value) { + sdf_value = distance_to_surface; + sdf_parent = nearest_inner_index; + } + + // If the voxel is not yet in the open queue, add it + if (sdf_uninitialized) { + open_queue.push(distance_to_surface, index); + } + } + }); +} + +void FullEuclideanSDFGenerator::propagate( + const HashedWaveletOctree& occupancy_map, VectorDistanceField& sdf, + BucketQueue& open_queue) const { + ZoneScoped; + // Create an occupancy query accelerator + QueryAccelerator occupancy_query_accelerator{occupancy_map}; + + // Precompute the neighbor distance offsets + const FloatingPoint min_cell_width = occupancy_map.getMinCellWidth(); + const FloatingPoint half_max_neighbor_distance_offset = + 0.5f * std::sqrt(3.f) * min_cell_width + 1e-3f; + + // Propagate the distance + while (!open_queue.empty()) { + TracyPlot("QueueLength", static_cast(open_queue.size())); + const Index3D index = open_queue.front(); + const auto& [sdf_parent, sdf_value] = sdf.getCellValue(index); + const FloatingPoint df_value = std::abs(sdf_value); + TracyPlot("Distance", df_value); + open_queue.pop(); + + for (const Index3D& index_offset : kNeighborIndexOffsets) { + // Compute the neighbor's distance if reached from the current voxel + const Index3D neighbor_index = index + index_offset; + FloatingPoint neighbor_df_candidate = + minDistanceTo(neighbor_index, sdf_parent, min_cell_width); + if (max_distance_ <= neighbor_df_candidate) { + continue; + } + + // Get the neighbor's SDF value + auto& [neighbor_sdf_parent, neighbor_sdf_value] = + sdf.getCellValueRef(neighbor_index); + + // If the neighbor is uninitialized, get its sign from the occupancy map + const bool neighbor_sdf_uninitialized = + neighbor_sdf_parent == sdf.getDefaultCellValue().parent; + if (neighbor_sdf_uninitialized) { + const FloatingPoint neighbor_occupancy = + occupancy_query_accelerator.getCellValue(neighbor_index); + // Never initialize or update unknown cells + if (!OccupancyClassifier::isObserved(neighbor_occupancy)) { + continue; + } + // Set the sign + if (classifier_.isOccupied(neighbor_occupancy)) { + neighbor_sdf_value = -sdf.getDefaultCellValue().distance; + } + } + + // Handle sign changes when propagating across the surface + const bool crossed_surface = + std::signbit(neighbor_sdf_value) != std::signbit(sdf_value); + if (crossed_surface) { + if (neighbor_sdf_value < 0.f) { + DCHECK_LE(df_value, half_max_neighbor_distance_offset); + neighbor_df_candidate = + minDistanceTo(neighbor_index, index, min_cell_width); + } else { + continue; + } + } + + // Update the neighbor's SDF value + const FloatingPoint neighbor_df = std::abs(neighbor_sdf_value); + if (neighbor_df_candidate < neighbor_df) { + neighbor_sdf_value = + std::copysign(neighbor_df_candidate, neighbor_sdf_value); + if (crossed_surface) { + neighbor_sdf_parent = index; + } else { + neighbor_sdf_parent = sdf_parent; + } + } + + // If the neighbor is not yet in the open queue, add it + if (neighbor_sdf_uninitialized) { + open_queue.push(neighbor_df_candidate, neighbor_index); + } + } + } +} + +FloatingPoint FullEuclideanSDFGenerator::minDistanceTo( + const Index3D& child, const Index3D& parent, FloatingPoint min_cell_width) { + const auto parent_aabb = + convert::nodeIndexToAABB(OctreeIndex{0, parent}, min_cell_width); + const auto child_center = convert::indexToCenterPoint(child, min_cell_width); + const FloatingPoint min_dist = parent_aabb.minDistanceTo(child_center); + return min_dist; +} +} // namespace wavemap diff --git a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc index ab91a6ecc..54f18d993 100644 --- a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc @@ -77,8 +77,7 @@ void QuasiEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, // Get the voxel's SDF value FloatingPoint& sdf_value = sdf.getCellValueRef(index); - const bool sdf_uninitialized = - sdf.getDefaultCellValue() - kTolerance < sdf_value; + const bool sdf_uninitialized = sdf.getDefaultCellValue() == sdf_value; // Update the voxel's SDF value const FloatingPoint distance_to_surface = @@ -107,7 +106,7 @@ void QuasiEuclideanSDFGenerator::propagate( neighborhood::generateNeighborDistanceOffsets(min_cell_width); CHECK_EQ(kNeighborIndexOffsets.size(), neighbor_distance_offsets.size()); const FloatingPoint half_max_neighbor_distance_offset = - std::sqrt(3.f) * min_cell_width / 2; + 0.5f * std::sqrt(3.f) * min_cell_width + 1e-3f; // Propagate the distance while (!open_queue.empty()) { @@ -128,13 +127,13 @@ void QuasiEuclideanSDFGenerator::propagate( } // Get the neighbor's SDF value - const Index3D& neighbor_index = + const Index3D neighbor_index = index + kNeighborIndexOffsets[neighbor_idx]; FloatingPoint& neighbor_sdf = sdf.getCellValueRef(neighbor_index); // If the neighbor is uninitialized, get its sign from the occupancy map const bool neighbor_sdf_uninitialized = - sdf.getDefaultCellValue() - kTolerance < neighbor_sdf; + sdf.getDefaultCellValue() == neighbor_sdf; if (neighbor_sdf_uninitialized) { const FloatingPoint neighbor_occupancy = occupancy_query_accelerator.getCellValue(neighbor_index); @@ -149,15 +148,19 @@ void QuasiEuclideanSDFGenerator::propagate( } // Handle sign changes when propagating across the surface - if (0.f < sdf_value && neighbor_sdf < 0.f) { - // NOTE: When the opened cell and the neighbor cell have the same sign, - // the distance field value and offset are summed to obtain the - // unsigned neighbor distance. Whereas when moving across the - // surface, the df_value and offset have opposite signs and reduce - // each other instead. - DCHECK_LE(df_value, half_max_neighbor_distance_offset); - neighbor_df_candidate = - neighbor_distance_offsets[neighbor_idx] - df_value; + if (std::signbit(neighbor_sdf) != std::signbit(sdf_value)) { + if (neighbor_sdf < 0.f) { + // NOTE: When the opened cell and the neighbor cell have the same + // sign, the distance field value and offset are summed to + // obtain the unsigned neighbor distance. Whereas when moving + // across the surface, the df_value and offset have opposite + // signs and reduce each other instead. + DCHECK_LE(df_value, half_max_neighbor_distance_offset); + neighbor_df_candidate = + neighbor_distance_offsets[neighbor_idx] - df_value; + } else { + continue; + } } // Update the neighbor's SDF value diff --git a/libraries/wavemap/src/utils/sdf/vector_distance_field.cc b/libraries/wavemap/src/utils/sdf/vector_distance_field.cc new file mode 100644 index 000000000..1f8a21d3b --- /dev/null +++ b/libraries/wavemap/src/utils/sdf/vector_distance_field.cc @@ -0,0 +1,3 @@ +#include "wavemap/utils/sdf/vector_distance_field.h" + +namespace wavemap {} diff --git a/libraries/wavemap/test/src/utils/test_sdf_generation.cc b/libraries/wavemap/test/src/utils/test_sdf_generation.cc index c1733016d..890935064 100644 --- a/libraries/wavemap/test/src/utils/test_sdf_generation.cc +++ b/libraries/wavemap/test/src/utils/test_sdf_generation.cc @@ -6,14 +6,20 @@ #include "wavemap/test/config_generator.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" +#include "wavemap/utils/sdf/full_euclidean_sdf_generator.h" #include "wavemap/utils/sdf/quasi_euclidean_sdf_generator.h" namespace wavemap { +template class SdfGenerationTest : public FixtureBase, public GeometryGenerator, public ConfigGenerator {}; -TEST_F(SdfGenerationTest, QuasiEuclideanSDFGenerator) { +using SdfGeneratorTypes = + ::testing::Types; +TYPED_TEST_SUITE(SdfGenerationTest, SdfGeneratorTypes, ); + +TYPED_TEST(SdfGenerationTest, BruteForceEquivalence) { // Params const Index3D min_index = Index3D::Constant(-10); const Index3D max_index = Index3D::Constant(100); @@ -53,7 +59,7 @@ TEST_F(SdfGenerationTest, QuasiEuclideanSDFGenerator) { } // Generate the SDF - QuasiEuclideanSDFGenerator sdf_generator{kMaxSdfDistance}; + TypeParam sdf_generator{kMaxSdfDistance}; const auto sdf = sdf_generator.generate(map); // Compare the SDF distances to the brute force min distance @@ -73,6 +79,8 @@ TEST_F(SdfGenerationTest, QuasiEuclideanSDFGenerator) { // Find the closest surface using brute force FloatingPoint sdf_brute_force = sdf.getDefaultCellValue(); + Index3D parent_brute_force = + Index3D::Constant(std::numeric_limits::max()); if (classifier.isFree(occupancy_value)) { // In free space, the SDF should always be positive EXPECT_GT(sdf_value, 0.f); @@ -81,8 +89,11 @@ TEST_F(SdfGenerationTest, QuasiEuclideanSDFGenerator) { for (const auto& obstacle_cell : obstacle_cells) { const auto obstacle_aabb = convert::nodeIndexToAABB( OctreeIndex{0, obstacle_cell}, min_cell_width); - sdf_brute_force = - std::min(obstacle_aabb.minDistanceTo(node_center), sdf_brute_force); + const FloatingPoint min_dist = obstacle_aabb.minDistanceTo(node_center); + if (min_dist < sdf_brute_force) { + sdf_brute_force = min_dist; + parent_brute_force = obstacle_cell; + } } } else { // Find the distance to the closest free cell @@ -95,8 +106,12 @@ TEST_F(SdfGenerationTest, QuasiEuclideanSDFGenerator) { OccupancyClassifier::isObserved(neighbor_occupancy_value)) { const auto free_cell_aabb = convert::nodeIndexToAABB( OctreeIndex{0, neighbor_index}, min_cell_width); - sdf_brute_force = std::min(free_cell_aabb.minDistanceTo(node_center), - sdf_brute_force); + const FloatingPoint min_dist = + free_cell_aabb.minDistanceTo(node_center); + if (min_dist < sdf_brute_force) { + sdf_brute_force = min_dist; + parent_brute_force = neighbor_index; + } } } // Adjust the sign to reflect we're inside the obstacle @@ -113,15 +128,38 @@ TEST_F(SdfGenerationTest, QuasiEuclideanSDFGenerator) { } // Check that the SDF accurately approximates the min obstacle distance - constexpr FloatingPoint kMaxMultiplicativeError = - QuasiEuclideanSDFGenerator::kMaxMultiplicativeError; + constexpr FloatingPoint kMaxRelativeUnderEstimate = + TypeParam::kMaxRelativeUnderEstimate; + constexpr FloatingPoint kMaxRelativeOverEstimate = + TypeParam::kMaxRelativeOverEstimate; if (std::abs(sdf_brute_force) < sdf.getDefaultCellValue()) { - EXPECT_NEAR(sdf_value, sdf_brute_force, - std::abs(sdf_brute_force) * kMaxMultiplicativeError) - << "At index " << print::eigen::oneLine(node_index.position); + if (0.f < sdf_brute_force) { + EXPECT_LT(sdf_value, sdf_brute_force * (1.f + kMaxRelativeOverEstimate)) + << "At index " << print::eigen::oneLine(node_index.position) + << " with nearest obstacle " + << print::eigen::oneLine(parent_brute_force); + EXPECT_GT(sdf_value, + sdf_brute_force * (1.f - kMaxRelativeUnderEstimate)) + << "At index " << print::eigen::oneLine(node_index.position) + << " with nearest obstacle " + << print::eigen::oneLine(parent_brute_force); + } else { + EXPECT_GT(sdf_value, sdf_brute_force * (1.f + kMaxRelativeOverEstimate)) + << "At index " << print::eigen::oneLine(node_index.position) + << " with nearest free cell " + << print::eigen::oneLine(parent_brute_force); + EXPECT_LT(sdf_value, + sdf_brute_force * (1.f - kMaxRelativeUnderEstimate)) + << "At index " << print::eigen::oneLine(node_index.position) + << " with nearest free cell " + << print::eigen::oneLine(parent_brute_force); + } } else { - EXPECT_NEAR(sdf_value, sdf.getDefaultCellValue(), - sdf.getDefaultCellValue() * kMaxMultiplicativeError) + EXPECT_LT(sdf_value, + sdf.getDefaultCellValue() * (1.f + kMaxRelativeOverEstimate)) + << "At index " << print::eigen::oneLine(node_index.position); + EXPECT_GT(sdf_value, + sdf.getDefaultCellValue() * (1.f - kMaxRelativeUnderEstimate)) << "At index " << print::eigen::oneLine(node_index.position); } }); From 8902c4ef4e7528d8161825bdaf38479420f262cf Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 26 Oct 2023 13:53:12 +0200 Subject: [PATCH 007/159] Clean up min distance computation --- .../utils/sdf/full_euclidean_sdf_generator.cc | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc index 599f110e2..a7cfbfb38 100644 --- a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc @@ -188,10 +188,17 @@ void FullEuclideanSDFGenerator::propagate( FloatingPoint FullEuclideanSDFGenerator::minDistanceTo( const Index3D& child, const Index3D& parent, FloatingPoint min_cell_width) { - const auto parent_aabb = - convert::nodeIndexToAABB(OctreeIndex{0, parent}, min_cell_width); - const auto child_center = convert::indexToCenterPoint(child, min_cell_width); - const FloatingPoint min_dist = parent_aabb.minDistanceTo(child_center); - return min_dist; + const Point3D child_center = + convert::indexToCenterPoint(child, min_cell_width); + + const Point3D parent_min_corner = + convert::indexToMinCorner(parent, min_cell_width); + const Point3D parent_max_corner = + parent_min_corner + Vector3D::Constant(min_cell_width); + const Point3D closest_point = + child_center.cwiseMax(parent_min_corner).cwiseMin(parent_max_corner); + + const FloatingPoint min_distance = (child_center - closest_point).norm(); + return min_distance; } } // namespace wavemap From 2b72e6d84eb463d099f6757184f83f5971ad05ad Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 27 Oct 2023 15:55:56 +0200 Subject: [PATCH 008/159] Implement (de)serialization of HashedBlock maps to files --- .../wavemap_io/impl/streamable_types_impl.h | 39 ++++++++- .../include/wavemap_io/stream_conversions.h | 4 + .../include/wavemap_io/streamable_types.h | 29 +++++-- .../wavemap_io/src/stream_conversions.cc | 84 +++++++++++++++++++ .../test/src/test_file_conversions.cc | 36 ++++++-- 5 files changed, 173 insertions(+), 19 deletions(-) diff --git a/libraries/wavemap_io/include/wavemap_io/impl/streamable_types_impl.h b/libraries/wavemap_io/include/wavemap_io/impl/streamable_types_impl.h index 18cb6d34b..249437a04 100644 --- a/libraries/wavemap_io/include/wavemap_io/impl/streamable_types_impl.h +++ b/libraries/wavemap_io/include/wavemap_io/impl/streamable_types_impl.h @@ -16,6 +16,39 @@ Index3D Index3D::read(std::istream& istream) { return instance; } +void HashedBlockHeader::write(std::ostream& ostream) const { + block_offset.write(ostream); +} + +HashedBlockHeader HashedBlockHeader::read(std::istream& istream) { + HashedBlockHeader instance; + instance.block_offset = Index3D::read(istream); + return instance; +} + +void HashedBlocksHeader::write(std::ostream& ostream) const { + ostream.write(reinterpret_cast(&min_cell_width), + sizeof(min_cell_width)); + ostream.write(reinterpret_cast(&min_log_odds), + sizeof(min_log_odds)); + ostream.write(reinterpret_cast(&max_log_odds), + sizeof(max_log_odds)); + ostream.write(reinterpret_cast(&num_blocks), sizeof(num_blocks)); +} + +HashedBlocksHeader HashedBlocksHeader::read(std::istream& istream) { + HashedBlocksHeader instance; + istream.read(reinterpret_cast(&instance.min_cell_width), + sizeof(min_cell_width)); + istream.read(reinterpret_cast(&instance.min_log_odds), + sizeof(min_log_odds)); + istream.read(reinterpret_cast(&instance.max_log_odds), + sizeof(max_log_odds)); + istream.read(reinterpret_cast(&instance.num_blocks), + sizeof(num_blocks)); + return instance; +} + void WaveletOctreeNode::write(std::ostream& ostream) const { for (Float coefficient : detail_coefficients) { ostream.write(reinterpret_cast(&coefficient), @@ -64,8 +97,7 @@ WaveletOctreeHeader WaveletOctreeHeader::read(std::istream& istream) { } void HashedWaveletOctreeBlockHeader::write(std::ostream& ostream) const { - ostream.write(reinterpret_cast(&root_node_offset), - sizeof(root_node_offset)); + root_node_offset.write(ostream); ostream.write(reinterpret_cast(&root_node_scale_coefficient), sizeof(root_node_scale_coefficient)); } @@ -73,8 +105,7 @@ void HashedWaveletOctreeBlockHeader::write(std::ostream& ostream) const { HashedWaveletOctreeBlockHeader HashedWaveletOctreeBlockHeader::read( std::istream& istream) { HashedWaveletOctreeBlockHeader instance; - istream.read(reinterpret_cast(&instance.root_node_offset), - sizeof(root_node_offset)); + instance.root_node_offset = Index3D::read(istream); istream.read(reinterpret_cast(&instance.root_node_scale_coefficient), sizeof(root_node_scale_coefficient)); return instance; diff --git a/libraries/wavemap_io/include/wavemap_io/stream_conversions.h b/libraries/wavemap_io/include/wavemap_io/stream_conversions.h index c91e2b3cf..ca91a74d4 100644 --- a/libraries/wavemap_io/include/wavemap_io/stream_conversions.h +++ b/libraries/wavemap_io/include/wavemap_io/stream_conversions.h @@ -6,6 +6,7 @@ #include #include +#include #include #include #include @@ -16,6 +17,9 @@ namespace wavemap::io { bool mapToStream(const VolumetricDataStructureBase& map, std::ostream& ostream); bool streamToMap(std::istream& istream, VolumetricDataStructureBase::Ptr& map); +void mapToStream(const HashedBlocks& map, std::ostream& ostream); +bool streamToMap(std::istream& istream, HashedBlocks::Ptr& map); + void mapToStream(const WaveletOctree& map, std::ostream& ostream); bool streamToMap(std::istream& istream, WaveletOctree::Ptr& map); diff --git a/libraries/wavemap_io/include/wavemap_io/streamable_types.h b/libraries/wavemap_io/include/wavemap_io/streamable_types.h index ba76e6d2c..d542b5296 100644 --- a/libraries/wavemap_io/include/wavemap_io/streamable_types.h +++ b/libraries/wavemap_io/include/wavemap_io/streamable_types.h @@ -8,7 +8,7 @@ namespace wavemap::io::streamable { // NOTE: This file defines the serialization format for all types that might be // used in wavemap map files. The idea is that these are used as common // building blocks (e.g. 3D indices are always (de)serialized using -// serializable::Index3D) and modified as little as possible to avoid +// streamable::Index3D) and modified as little as possible to avoid // breaking backward compatibility with previously saved maps. using UInt8 = uint8_t; @@ -25,6 +25,24 @@ struct Index3D { inline static Index3D read(std::istream& istream); }; +struct HashedBlockHeader { + Index3D block_offset{}; + + inline void write(std::ostream& ostream) const; + inline static HashedBlockHeader read(std::istream& istream); +}; + +struct HashedBlocksHeader { + Float min_cell_width{}; + Float min_log_odds{}; + Float max_log_odds{}; + + UInt64 num_blocks{}; + + inline void write(std::ostream& ostream) const; + inline static HashedBlocksHeader read(std::istream& istream); +}; + struct WaveletOctreeNode { std::array detail_coefficients{}; UInt8 allocated_children_bitset{}; @@ -68,13 +86,10 @@ struct HashedWaveletOctreeHeader { struct StorageFormat : TypeSelector { using TypeSelector::TypeSelector; - enum Id : TypeId { - kWaveletOctree, - kHashedWaveletOctree, - }; + enum Id : TypeId { kWaveletOctree, kHashedWaveletOctree, kHashedBlocks }; - static constexpr std::array names = {"wavelet_octree", - "hashed_wavelet_octree"}; + static constexpr std::array names = { + "wavelet_octree", "hashed_wavelet_octree", "hashed_blocks"}; inline void write(std::ostream& ostream) const; inline static StorageFormat read(std::istream& istream); diff --git a/libraries/wavemap_io/src/stream_conversions.cc b/libraries/wavemap_io/src/stream_conversions.cc index dbb9ce03c..d922d9d82 100644 --- a/libraries/wavemap_io/src/stream_conversions.cc +++ b/libraries/wavemap_io/src/stream_conversions.cc @@ -4,6 +4,11 @@ namespace wavemap::io { bool mapToStream(const VolumetricDataStructureBase& map, std::ostream& ostream) { // Call the appropriate mapToStream converter based on the map's derived type + if (const auto* hashed_blocks = dynamic_cast(&map); + hashed_blocks) { + io::mapToStream(*hashed_blocks, ostream); + return true; + } if (const auto* wavelet_octree = dynamic_cast(&map); wavelet_octree) { io::mapToStream(*wavelet_octree, ostream); @@ -31,6 +36,14 @@ bool streamToMap(std::istream& istream, VolumetricDataStructureBase::Ptr& map) { // Call the appropriate streamToMap converter based on the received map's type const auto storage_format = streamable::StorageFormat::peek(istream); switch (storage_format.toTypeId()) { + case streamable::StorageFormat::kHashedBlocks: { + auto hashed_blocks = std::dynamic_pointer_cast(map); + if (!streamToMap(istream, hashed_blocks)) { + return false; + } + map = hashed_blocks; + return true; + } case streamable::StorageFormat::kWaveletOctree: { auto wavelet_octree = std::dynamic_pointer_cast(map); if (!streamToMap(istream, wavelet_octree)) { @@ -56,6 +69,77 @@ bool streamToMap(std::istream& istream, VolumetricDataStructureBase::Ptr& map) { } } +void mapToStream(const HashedBlocks& map, std::ostream& ostream) { + // Serialize the map's data structure type + streamable::StorageFormat storage_format = + streamable::StorageFormat::kHashedBlocks; + storage_format.write(ostream); + + // Serialize the map and data structure's metadata + streamable::HashedBlocksHeader hashed_blocks_header; + hashed_blocks_header.min_cell_width = map.getMinCellWidth(); + hashed_blocks_header.min_log_odds = map.getMinLogOdds(); + hashed_blocks_header.max_log_odds = map.getMaxLogOdds(); + hashed_blocks_header.num_blocks = map.getHashMap().size(); + hashed_blocks_header.write(ostream); + + // Iterate over all the map's blocks + map.getHashMap().forEachBlock( + [&ostream](const Index3D& block_index, const HashedBlocks::Block& block) { + // Serialize the block's metadata + streamable::HashedBlockHeader block_header; + block_header.block_offset = {block_index.x(), block_index.y(), + block_index.z()}; + block_header.write(ostream); + + // Serialize the block's data (its cells) + for (FloatingPoint value : block.data()) { + streamable::Float value_serialized = value; + ostream.write(reinterpret_cast(&value_serialized), + sizeof(value_serialized)); + } + }); +} + +bool streamToMap(std::istream& istream, HashedBlocks::Ptr& map) { + // Make sure the map in the input stream is of the correct type + if (streamable::StorageFormat::read(istream) != + streamable::StorageFormat::kHashedBlocks) { + return false; + } + + // Deserialize the map's config and initialize the data structure + const auto hashed_blocks_header = + streamable::HashedBlocksHeader::read(istream); + VolumetricDataStructureConfig config; + config.min_cell_width = hashed_blocks_header.min_cell_width; + config.min_log_odds = hashed_blocks_header.min_log_odds; + config.max_log_odds = hashed_blocks_header.max_log_odds; + map = std::make_shared(config); + + // Deserialize all the blocks + for (size_t block_idx = 0; block_idx < hashed_blocks_header.num_blocks; + ++block_idx) { + // Deserialize the block header, containing its position + const auto block_header = streamable::HashedBlockHeader::read(istream); + const Index3D block_index{block_header.block_offset.x, + block_header.block_offset.y, + block_header.block_offset.z}; + auto& block = map->getOrAllocateBlock(block_index); + + // Deserialize the block's remaining data (its cells) + for (LinearIndex linear_index = 0; + linear_index < HashedBlocks::Block::kCellsPerBlock; ++linear_index) { + streamable::Float value_deserialized{}; + istream.read(reinterpret_cast(&value_deserialized), + sizeof(value_deserialized)); + block[linear_index] = value_deserialized; + } + } + + return true; +} + void mapToStream(const WaveletOctree& map, std::ostream& ostream) { // Serialize the map's data structure type streamable::StorageFormat storage_format = diff --git a/libraries/wavemap_io/test/src/test_file_conversions.cc b/libraries/wavemap_io/test/src/test_file_conversions.cc index 247119d65..988febdfe 100644 --- a/libraries/wavemap_io/test/src/test_file_conversions.cc +++ b/libraries/wavemap_io/test/src/test_file_conversions.cc @@ -21,7 +21,7 @@ class FileConversionsTest : public FixtureBase, }; using VolumetricDataStructureTypes = - ::testing::Types; TYPED_TEST_SUITE(FileConversionsTest, VolumetricDataStructureTypes, ); @@ -34,7 +34,9 @@ TYPED_TEST(FileConversionsTest, MetadataPreservation) { ASSERT_EQ(map->getMinCellWidth(), config.min_cell_width); ASSERT_EQ(map->getMinLogOdds(), config.min_log_odds); ASSERT_EQ(map->getMaxLogOdds(), config.max_log_odds); - ASSERT_EQ(map->getTreeHeight(), config.tree_height); + if constexpr (!std::is_same_v) { + ASSERT_EQ(map->getTreeHeight(), config.tree_height); + } // Convert to base pointer VolumetricDataStructureBase::ConstPtr map_base = map; @@ -61,7 +63,9 @@ TYPED_TEST(FileConversionsTest, MetadataPreservation) { EXPECT_EQ(map_round_trip->getMinCellWidth(), config.min_cell_width); EXPECT_EQ(map_round_trip->getMinLogOdds(), config.min_log_odds); EXPECT_EQ(map_round_trip->getMaxLogOdds(), config.max_log_odds); - EXPECT_EQ(map_round_trip->getTreeHeight(), config.tree_height); + if constexpr (!std::is_same_v) { + EXPECT_EQ(map_round_trip->getTreeHeight(), config.tree_height); + } } else { typename TypeParam::ConstPtr map_round_trip = std::dynamic_pointer_cast(map_base_round_trip); @@ -71,7 +75,9 @@ TYPED_TEST(FileConversionsTest, MetadataPreservation) { EXPECT_EQ(map_round_trip->getMinCellWidth(), config.min_cell_width); EXPECT_EQ(map_round_trip->getMinLogOdds(), config.min_log_odds); EXPECT_EQ(map_round_trip->getMaxLogOdds(), config.max_log_odds); - EXPECT_EQ(map_round_trip->getTreeHeight(), config.tree_height); + if constexpr (!std::is_same_v) { + EXPECT_EQ(map_round_trip->getTreeHeight(), config.tree_height); + } } } @@ -102,8 +108,15 @@ TYPED_TEST(FileConversionsTest, InsertionAndLeafVisitor) { map_base_round_trip->forEachLeaf( [&map_original](const OctreeIndex& node_index, FloatingPoint round_trip_value) { - EXPECT_NEAR(round_trip_value, map_original.getCellValue(node_index), - TestFixture::kAcceptableReconstructionError); + if constexpr (std::is_same_v) { + EXPECT_EQ(node_index.height, 0); + EXPECT_NEAR(round_trip_value, + map_original.getCellValue(node_index.position), + TestFixture::kAcceptableReconstructionError); + } else { + EXPECT_NEAR(round_trip_value, map_original.getCellValue(node_index), + TestFixture::kAcceptableReconstructionError); + } }); // TODO(victorr): Remove this special case once deserializing directly @@ -125,8 +138,15 @@ TYPED_TEST(FileConversionsTest, InsertionAndLeafVisitor) { map_original.forEachLeaf([&map_round_trip](const OctreeIndex& node_index, FloatingPoint original_value) { - EXPECT_NEAR(original_value, map_round_trip->getCellValue(node_index), - TestFixture::kAcceptableReconstructionError); + if constexpr (std::is_same_v) { + EXPECT_EQ(node_index.height, 0); + EXPECT_NEAR(original_value, + map_round_trip->getCellValue(node_index.position), + TestFixture::kAcceptableReconstructionError); + } else { + EXPECT_NEAR(original_value, map_round_trip->getCellValue(node_index), + TestFixture::kAcceptableReconstructionError); + } }); } } From 6fcd67f3dab1191a9dd88032e9786f05d7058109 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 27 Oct 2023 16:25:52 +0200 Subject: [PATCH 009/159] Implement (de)serialization of HashedBlocks to ROS msgs --- .../wavemap_io/src/stream_conversions.cc | 2 +- .../test/src/test_file_conversions.cc | 6 +- ros/wavemap_msgs/msg/HashedBlock.msg | 2 + ros/wavemap_msgs/msg/HashedBlocks.msg | 7 ++ ros/wavemap_msgs/msg/Map.msg | 1 + .../map_msg_conversions.h | 4 + .../src/map_msg_conversions.cc | 114 +++++++++++++++++- .../test/src/test_map_msg_conversions.cc | 34 ++++-- 8 files changed, 152 insertions(+), 18 deletions(-) create mode 100644 ros/wavemap_msgs/msg/HashedBlock.msg create mode 100644 ros/wavemap_msgs/msg/HashedBlocks.msg diff --git a/libraries/wavemap_io/src/stream_conversions.cc b/libraries/wavemap_io/src/stream_conversions.cc index d922d9d82..42eec0e4d 100644 --- a/libraries/wavemap_io/src/stream_conversions.cc +++ b/libraries/wavemap_io/src/stream_conversions.cc @@ -84,7 +84,7 @@ void mapToStream(const HashedBlocks& map, std::ostream& ostream) { hashed_blocks_header.write(ostream); // Iterate over all the map's blocks - map.getHashMap().forEachBlock( + map.forEachBlock( [&ostream](const Index3D& block_index, const HashedBlocks::Block& block) { // Serialize the block's metadata streamable::HashedBlockHeader block_header; diff --git a/libraries/wavemap_io/test/src/test_file_conversions.cc b/libraries/wavemap_io/test/src/test_file_conversions.cc index 988febdfe..d67183656 100644 --- a/libraries/wavemap_io/test/src/test_file_conversions.cc +++ b/libraries/wavemap_io/test/src/test_file_conversions.cc @@ -54,7 +54,7 @@ TYPED_TEST(FileConversionsTest, MetadataPreservation) { // TODO(victorr): Add option to deserialize into hashed chunked wavelet // octrees, instead of implicitly converting them to regular // hashed wavelet octrees. - if (std::is_same_v) { + if constexpr (std::is_same_v) { HashedWaveletOctree::ConstPtr map_round_trip = std::dynamic_pointer_cast(map_base_round_trip); ASSERT_TRUE(map_round_trip); @@ -63,9 +63,7 @@ TYPED_TEST(FileConversionsTest, MetadataPreservation) { EXPECT_EQ(map_round_trip->getMinCellWidth(), config.min_cell_width); EXPECT_EQ(map_round_trip->getMinLogOdds(), config.min_log_odds); EXPECT_EQ(map_round_trip->getMaxLogOdds(), config.max_log_odds); - if constexpr (!std::is_same_v) { - EXPECT_EQ(map_round_trip->getTreeHeight(), config.tree_height); - } + EXPECT_EQ(map_round_trip->getTreeHeight(), config.tree_height); } else { typename TypeParam::ConstPtr map_round_trip = std::dynamic_pointer_cast(map_base_round_trip); diff --git a/ros/wavemap_msgs/msg/HashedBlock.msg b/ros/wavemap_msgs/msg/HashedBlock.msg new file mode 100644 index 000000000..e7877a9d3 --- /dev/null +++ b/ros/wavemap_msgs/msg/HashedBlock.msg @@ -0,0 +1,2 @@ +int32[3] block_offset +float32[] values diff --git a/ros/wavemap_msgs/msg/HashedBlocks.msg b/ros/wavemap_msgs/msg/HashedBlocks.msg new file mode 100644 index 000000000..ed6a07298 --- /dev/null +++ b/ros/wavemap_msgs/msg/HashedBlocks.msg @@ -0,0 +1,7 @@ +float32 min_cell_width +float32 min_log_odds +float32 max_log_odds + +Index3D[] allocated_block_indices + +HashedBlock[] blocks diff --git a/ros/wavemap_msgs/msg/Map.msg b/ros/wavemap_msgs/msg/Map.msg index ec5245d3e..41db3abe1 100644 --- a/ros/wavemap_msgs/msg/Map.msg +++ b/ros/wavemap_msgs/msg/Map.msg @@ -1,4 +1,5 @@ Header header +HashedBlocks[] hashed_blocks WaveletOctree[] wavelet_octree HashedWaveletOctree[] hashed_wavelet_octree diff --git a/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h b/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h index 4cee0c6e2..7d406d1b1 100644 --- a/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h +++ b/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -23,6 +24,9 @@ bool mapToRosMsg(const VolumetricDataStructureBase& map, bool rosMsgToMap(const wavemap_msgs::Map& msg, VolumetricDataStructureBase::Ptr& map); +void mapToRosMsg(const HashedBlocks& map, wavemap_msgs::HashedBlocks& msg); +void rosMsgToMap(const wavemap_msgs::HashedBlocks& msg, HashedBlocks::Ptr& map); + void mapToRosMsg(const WaveletOctree& map, wavemap_msgs::WaveletOctree& msg); void rosMsgToMap(const wavemap_msgs::WaveletOctree& msg, WaveletOctree::Ptr& map); diff --git a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc index 6b181ea5b..1dbf787a8 100644 --- a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc +++ b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc @@ -12,6 +12,11 @@ bool mapToRosMsg(const VolumetricDataStructureBase& map, msg.header.frame_id = frame_id; // Write the map data + if (const auto* hashed_blocks = dynamic_cast(&map); + hashed_blocks) { + convert::mapToRosMsg(*hashed_blocks, msg.hashed_blocks.emplace_back()); + return true; + } if (const auto* wavelet_octree = dynamic_cast(&map); wavelet_octree) { convert::mapToRosMsg(*wavelet_octree, msg.wavelet_octree.emplace_back()); @@ -42,16 +47,40 @@ bool rosMsgToMap(const wavemap_msgs::Map& msg, VolumetricDataStructureBase::Ptr& map) { ZoneScoped; // Check validity - if ((msg.wavelet_octree.size() == 1) != - (msg.hashed_wavelet_octree.size() != 1)) { - ROS_WARN( - "Maps must be serialized either as wavelet octrees or hashed " - "wavelet octrees. Encountered message contains both. Ignoring."); + bool is_valid = true; + std::string error_msg = + "Maps must be serialized as either one hashed block data structure, " + "wavelet octree, or hashed wavelet octree. "; + if (1 < msg.hashed_blocks.size()) { + error_msg += "Message contains multiple hashed block data structures. "; + is_valid = false; + } + if (1 < msg.wavelet_octree.size()) { + error_msg += "Message contains multiple wavelet octrees. "; + is_valid = false; + } + if (1 < msg.hashed_wavelet_octree.size()) { + error_msg += "Message contains multiple hashed wavelet octrees. "; + is_valid = false; + } + if (msg.hashed_blocks.empty() && msg.wavelet_octree.empty() && + msg.hashed_wavelet_octree.empty()) { + error_msg += "Message contains neither. "; + is_valid = false; + } + if (!is_valid) { + ROS_WARN_STREAM(error_msg + "Ignoring."); map = nullptr; return false; } // Read the data + if (!msg.hashed_blocks.empty()) { + auto hashed_blocks = std::dynamic_pointer_cast(map); + rosMsgToMap(msg.hashed_blocks.front(), hashed_blocks); + map = hashed_blocks; + return true; + } if (!msg.wavelet_octree.empty()) { auto wavelet_octree = std::dynamic_pointer_cast(map); rosMsgToMap(msg.wavelet_octree.front(), wavelet_octree); @@ -73,6 +102,81 @@ bool rosMsgToMap(const wavemap_msgs::Map& msg, return false; } +void mapToRosMsg(const HashedBlocks& map, wavemap_msgs::HashedBlocks& msg) { + ZoneScoped; + // Serialize the map and data structure's metadata + msg.min_cell_width = map.getMinCellWidth(); + msg.min_log_odds = map.getMinLogOdds(); + msg.max_log_odds = map.getMaxLogOdds(); + + // Indicate which blocks are allocated in the map + // NOTE: This is done such that subscribers know when blocks should be removed + // during incremental map transmission. + msg.allocated_block_indices.reserve(map.getHashMap().size()); + map.forEachBlock([&msg](const Index3D& block_index, const auto& /*block*/) { + auto& block_index_msg = msg.allocated_block_indices.emplace_back(); + block_index_msg.x = block_index.x(); + block_index_msg.y = block_index.y(); + block_index_msg.z = block_index.z(); + }); + + // Serialize all blocks + map.forEachBlock( + [&msg](const Index3D& block_index, const HashedBlocks::Block& block) { + auto& block_msg = msg.blocks.emplace_back(); + // Serialize the block's metadata + block_msg.block_offset = {block_index.x(), block_index.y(), + block_index.z()}; + // Serialize the block's data (dense grid) + block_msg.values.reserve(HashedBlocks::Block::kCellsPerBlock); + for (FloatingPoint value : block.data()) { + block_msg.values.emplace_back(value); + } + }); +} + +void rosMsgToMap(const wavemap_msgs::HashedBlocks& msg, + HashedBlocks::Ptr& map) { + ZoneScoped; + // Deserialize the map's config + VolumetricDataStructureConfig config; + config.min_cell_width = msg.min_cell_width; + config.min_log_odds = msg.min_log_odds; + config.max_log_odds = msg.max_log_odds; + + // Check if the map already exists and has compatible settings + if (map && map->getConfig() == config) { + // Load allocated block list into a hash table for quick membership lookups + std::unordered_set allocated_blocks; + for (const auto& block_index : msg.allocated_block_indices) { + allocated_blocks.emplace(block_index.x, block_index.y, block_index.z); + } + // Remove local blocks that should no longer exist according to the map msg + map->getHashMap().eraseBlockIf( + [&allocated_blocks](const Index3D& block_index, const auto& /*block*/) { + return !allocated_blocks.count(block_index); + }); + } else { + // Otherwise create a new map + map = std::make_shared(config); + } + + // Deserialize all blocks + for (const auto& block_msg : msg.blocks) { + // Get the block + const Index3D block_index{block_msg.block_offset[0], + block_msg.block_offset[1], + block_msg.block_offset[2]}; + auto& block = map->getOrAllocateBlock(block_index); + + // Deserialize the block's data (dense grid) + for (LinearIndex linear_index = 0; linear_index < block_msg.values.size(); + ++linear_index) { + block[linear_index] = block_msg.values[linear_index]; + } + } +} + void mapToRosMsg(const WaveletOctree& map, wavemap_msgs::WaveletOctree& msg) { ZoneScoped; // Serialize the map and data structure's metadata diff --git a/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc b/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc index 8c584add6..130afa62b 100644 --- a/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc +++ b/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc @@ -28,7 +28,7 @@ class MapMsgConversionsTest : public FixtureBase, }; using VolumetricDataStructureTypes = - ::testing::Types; TYPED_TEST_SUITE(MapMsgConversionsTest, VolumetricDataStructureTypes, ); @@ -41,7 +41,9 @@ TYPED_TEST(MapMsgConversionsTest, MetadataPreservation) { ASSERT_EQ(map->getMinCellWidth(), config.min_cell_width); ASSERT_EQ(map->getMinLogOdds(), config.min_log_odds); ASSERT_EQ(map->getMaxLogOdds(), config.max_log_odds); - ASSERT_EQ(map->getTreeHeight(), config.tree_height); + if constexpr (!std::is_same_v) { + ASSERT_EQ(map->getTreeHeight(), config.tree_height); + } // Convert to base pointer VolumetricDataStructureBase::ConstPtr map_base = map; @@ -64,7 +66,7 @@ TYPED_TEST(MapMsgConversionsTest, MetadataPreservation) { // TODO(victorr): Add option to deserialize into hashed chunked wavelet // octrees, instead of implicitly converting them to regular // hashed wavelet octrees. - if (std::is_same_v) { + if constexpr (std::is_same_v) { HashedWaveletOctree::ConstPtr map_round_trip = std::dynamic_pointer_cast(map_base_round_trip); ASSERT_TRUE(map_round_trip); @@ -83,7 +85,9 @@ TYPED_TEST(MapMsgConversionsTest, MetadataPreservation) { EXPECT_EQ(map_round_trip->getMinCellWidth(), config.min_cell_width); EXPECT_EQ(map_round_trip->getMinLogOdds(), config.min_log_odds); EXPECT_EQ(map_round_trip->getMaxLogOdds(), config.max_log_odds); - EXPECT_EQ(map_round_trip->getTreeHeight(), config.tree_height); + if constexpr (!std::is_same_v) { + EXPECT_EQ(map_round_trip->getTreeHeight(), config.tree_height); + } } } @@ -115,8 +119,15 @@ TYPED_TEST(MapMsgConversionsTest, InsertionAndLeafVisitor) { map_base_round_trip->forEachLeaf( [&map_original](const OctreeIndex& node_index, FloatingPoint round_trip_value) { - EXPECT_NEAR(round_trip_value, map_original.getCellValue(node_index), - TestFixture::kAcceptableReconstructionError); + if constexpr (std::is_same_v) { + EXPECT_EQ(node_index.height, 0); + EXPECT_NEAR(round_trip_value, + map_original.getCellValue(node_index.position), + TestFixture::kAcceptableReconstructionError); + } else { + EXPECT_NEAR(round_trip_value, map_original.getCellValue(node_index), + TestFixture::kAcceptableReconstructionError); + } }); // TODO(victorr): Remove this special case once deserializing directly @@ -138,8 +149,15 @@ TYPED_TEST(MapMsgConversionsTest, InsertionAndLeafVisitor) { map_original.forEachLeaf([&map_round_trip](const OctreeIndex& node_index, FloatingPoint original_value) { - EXPECT_NEAR(original_value, map_round_trip->getCellValue(node_index), - TestFixture::kAcceptableReconstructionError); + if constexpr (std::is_same_v) { + EXPECT_EQ(node_index.height, 0); + EXPECT_NEAR(original_value, + map_round_trip->getCellValue(node_index.position), + TestFixture::kAcceptableReconstructionError); + } else { + EXPECT_NEAR(original_value, map_round_trip->getCellValue(node_index), + TestFixture::kAcceptableReconstructionError); + } }); } } From ee8d212d906808382e83af478a51f4ab516e9329 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 27 Oct 2023 16:43:55 +0200 Subject: [PATCH 010/159] Extend Rviz plugin to support ESDFs --- .../utils/color_conversions.h | 16 +++++++ .../visuals/slice_visual.h | 13 ++++++ .../visuals/voxel_visual.h | 6 +-- .../src/visuals/slice_visual.cc | 43 +++++++++++++++---- .../src/visuals/voxel_visual.cc | 16 +++---- 5 files changed, 75 insertions(+), 19 deletions(-) diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/color_conversions.h b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/color_conversions.h index abda8e27a..bdbc1f748 100644 --- a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/color_conversions.h +++ b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/color_conversions.h @@ -21,6 +21,22 @@ inline Ogre::ColourValue logOddsToColor(FloatingPoint log_odds) { // Map a voxel's position to a color (HSV color map) Ogre::ColourValue positionToColor(const Point3D& center_point); + +// Map a voxel's raw value to a color after normalizing it (grey value) +inline Ogre::ColourValue scalarToColor(FloatingPoint value, + FloatingPoint min_value, + FloatingPoint max_value) { + Ogre::ColourValue color; + color.a = 1.f; + + const FloatingPoint rescaled_odds = + (value - min_value) / (max_value - min_value); + color.a = 1.f; + color.r = rescaled_odds; + color.g = rescaled_odds; + color.b = rescaled_odds; + return color; +} } // namespace wavemap::rviz_plugin #endif // WAVEMAP_RVIZ_PLUGIN_UTILS_COLOR_CONVERSIONS_H_ diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h index c5a5f0bc9..6b3e555ad 100644 --- a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h +++ b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -21,6 +22,14 @@ #endif namespace wavemap::rviz_plugin { +struct SliceColorMode : public TypeSelector { + using TypeSelector::TypeSelector; + + enum Id : TypeId { kProbability, kRaw }; + + static constexpr std::array names = {"Probability", "Raw"}; +}; + // Each instance of MultiResolutionGridVisual represents the visualization of a // map's leaves as squares whose sizes match their height in the tree. class SliceVisual : public QObject { @@ -47,8 +56,11 @@ class SliceVisual : public QObject { // user-editable properties void generalUpdateCallback() { update(); } void opacityUpdateCallback(); + void colorModeUpdateCallback(); private: + SliceColorMode slice_color_mode_ = SliceColorMode::kProbability; + // Shared pointer to the map, owned by WavemapMapDisplay const std::shared_ptr map_and_mutex_; @@ -73,6 +85,7 @@ class SliceVisual : public QObject { rviz::FloatProperty max_occupancy_threshold_property_; rviz::FloatProperty slice_height_property_; rviz::FloatProperty opacity_property_; + rviz::EnumProperty color_mode_property_; }; } // namespace wavemap::rviz_plugin diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h index d5034f1e0..add3b93f5 100644 --- a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h +++ b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h @@ -30,8 +30,8 @@ #endif namespace wavemap::rviz_plugin { -struct ColorMode : public TypeSelector { - using TypeSelector::TypeSelector; +struct VoxelColorMode : public TypeSelector { + using TypeSelector::TypeSelector; enum Id : TypeId { kHeight, kProbability, kFlat }; @@ -71,7 +71,7 @@ class VoxelVisual : public QObject { void flatColorUpdateCallback(); private: - ColorMode voxel_color_mode_ = ColorMode::kHeight; + VoxelColorMode voxel_color_mode_ = VoxelColorMode::kHeight; Ogre::ColourValue voxel_flat_color_ = Ogre::ColourValue::Blue; // Shared pointer to the map, owned by WavemapMapDisplay diff --git a/ros/wavemap_rviz_plugin/src/visuals/slice_visual.cc b/ros/wavemap_rviz_plugin/src/visuals/slice_visual.cc index c45f142cb..e90eb5780 100644 --- a/ros/wavemap_rviz_plugin/src/visuals/slice_visual.cc +++ b/ros/wavemap_rviz_plugin/src/visuals/slice_visual.cc @@ -3,6 +3,8 @@ #include #include +#include "wavemap_rviz_plugin/utils/color_conversions.h" + namespace wavemap::rviz_plugin { SliceVisual::SliceVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, @@ -27,7 +29,17 @@ SliceVisual::SliceVisual(Ogre::SceneManager* scene_manager, submenu_root_property, SLOT(generalUpdateCallback()), this), opacity_property_("Alpha", 1.0, "Opacity of the displayed visuals.", submenu_root_property, SLOT(opacityUpdateCallback()), - this) { + this), + color_mode_property_( + "Color mode", "", "Mode determining the cell colors.", + submenu_root_property, SLOT(colorModeUpdateCallback()), this) { + // Initialize the property menu + color_mode_property_.clearOptions(); + for (const auto& name : SliceColorMode::names) { + color_mode_property_.addOption(name); + } + color_mode_property_.setStringStd(slice_color_mode_.toStr()); + // Initialize the slice cell material // NOTE: Certain properties, such as alpha transparency, are set on a // per-material basis. We therefore need to create one unique material @@ -110,13 +122,17 @@ void SliceVisual::update() { cell.center.z = slice_height; // Set the cube's color - const FloatingPoint cell_odds = std::exp(cell_log_odds); - const FloatingPoint cell_prob = cell_odds / (1.f + cell_odds); - const FloatingPoint cell_free_prob = 1.f - cell_prob; - cell.color.a = 1.f; - cell.color.r = cell_free_prob; - cell.color.g = cell_free_prob; - cell.color.b = cell_free_prob; + switch (slice_color_mode_.toTypeId()) { + case SliceColorMode::kRaw: { + cell.color = scalarToColor(cell_log_odds, min_occupancy_log_odds, + max_occupancy_log_odds); + break; + } + case SliceColorMode::kProbability: + default: + cell.color = logOddsToColor(cell_log_odds); + break; + } }); // Add a grid layer for each scale level @@ -178,4 +194,15 @@ void SliceVisual::setAlpha(FloatingPoint alpha) { grid_level->setAlpha(alpha); } } + +void SliceVisual::colorModeUpdateCallback() { + // Update the cached color mode value + const SliceColorMode old_color_mode = slice_color_mode_; + slice_color_mode_ = SliceColorMode(color_mode_property_.getStdString()); + + // Update the map if the color mode changed + if (slice_color_mode_ != old_color_mode) { + update(); + } +} } // namespace wavemap::rviz_plugin diff --git a/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc b/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc index 125aafa7c..372bfae57 100644 --- a/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc +++ b/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc @@ -55,11 +55,11 @@ VoxelVisual::VoxelVisual(Ogre::SceneManager* scene_manager, max_ms_per_frame_property_.setMin(0); // Color mode color_mode_property_.clearOptions(); - for (const auto& name : ColorMode::names) { + for (const auto& name : VoxelColorMode::names) { color_mode_property_.addOption(name); } color_mode_property_.setStringStd(voxel_color_mode_.toStr()); - flat_color_property_.setHidden(voxel_color_mode_ != ColorMode::kFlat); + flat_color_property_.setHidden(voxel_color_mode_ != VoxelColorMode::kFlat); // Initialize the camera tracker used to update the LOD levels for each block prerender_listener_ = std::make_unique( @@ -291,11 +291,11 @@ void VoxelVisual::opacityUpdateCallback() { void VoxelVisual::colorModeUpdateCallback() { ZoneScoped; // Update the cached color mode value - const ColorMode old_color_mode = voxel_color_mode_; - voxel_color_mode_ = ColorMode(color_mode_property_.getStdString()); + const VoxelColorMode old_color_mode = voxel_color_mode_; + voxel_color_mode_ = VoxelColorMode(color_mode_property_.getStdString()); // Show/hide the flat color picker depending on the chosen mode - flat_color_property_.setHidden(voxel_color_mode_ != ColorMode::kFlat); + flat_color_property_.setHidden(voxel_color_mode_ != VoxelColorMode::kFlat); // Update the map if the color mode changed if (voxel_color_mode_ != old_color_mode) { @@ -340,13 +340,13 @@ void VoxelVisual::appendLeafCenterAndColor(int tree_height, // Set the cube's color switch (voxel_color_mode_.toTypeId()) { - case ColorMode::kFlat: + case VoxelColorMode::kFlat: point.color = voxel_flat_color_; break; - case ColorMode::kProbability: + case VoxelColorMode::kProbability: point.color = logOddsToColor(cell_log_odds); break; - case ColorMode::kHeight: + case VoxelColorMode::kHeight: default: point.color = positionToColor(cell_center); break; From e5164b16fcc3cf635d7b78e713f19833ec47b988 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 27 Oct 2023 17:28:08 +0200 Subject: [PATCH 011/159] Add a small occupancy to ESDF conversion script --- .../utils/sdf/full_euclidean_sdf_generator.h | 2 +- .../utils/sdf/quasi_euclidean_sdf_generator.h | 2 +- .../utils/sdf/full_euclidean_sdf_generator.cc | 2 +- .../sdf/quasi_euclidean_sdf_generator.cc | 2 +- ros/wavemap_ros/CMakeLists.txt | 3 + ros/wavemap_ros/app/occupancy_to_esdf.cc | 57 +++++++++++++++++++ 6 files changed, 64 insertions(+), 4 deletions(-) create mode 100644 ros/wavemap_ros/app/occupancy_to_esdf.cc diff --git a/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h b/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h index 052d55f8e..a44c0c6a4 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h +++ b/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h @@ -18,7 +18,7 @@ class FullEuclideanSDFGenerator { FloatingPoint occupancy_threshold = 0.f) : max_distance_(max_distance), classifier_(occupancy_threshold) {} - HashedBlocks generate(const HashedWaveletOctree& occupancy_map); + HashedBlocks generate(const HashedWaveletOctree& occupancy_map) const; FloatingPoint getMaxDistance() const { return max_distance_; } diff --git a/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h b/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h index e52e34e1d..7d4d4a0db 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h +++ b/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h @@ -17,7 +17,7 @@ class QuasiEuclideanSDFGenerator { FloatingPoint occupancy_threshold = 0.f) : max_distance_(max_distance), classifier_(occupancy_threshold) {} - HashedBlocks generate(const HashedWaveletOctree& occupancy_map); + HashedBlocks generate(const HashedWaveletOctree& occupancy_map) const; FloatingPoint getMaxDistance() const { return max_distance_; } diff --git a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc index a7cfbfb38..fc8768457 100644 --- a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc @@ -7,7 +7,7 @@ namespace wavemap { HashedBlocks FullEuclideanSDFGenerator::generate( - const HashedWaveletOctree& occupancy_map) { + const HashedWaveletOctree& occupancy_map) const { ZoneScoped; // Initialize the SDF data structure const FloatingPoint min_cell_width = occupancy_map.getMinCellWidth(); diff --git a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc index 54f18d993..d993e25a9 100644 --- a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc @@ -7,7 +7,7 @@ namespace wavemap { HashedBlocks QuasiEuclideanSDFGenerator::generate( - const HashedWaveletOctree& occupancy_map) { + const HashedWaveletOctree& occupancy_map) const { ZoneScoped; // Initialize the SDF data structure const FloatingPoint min_cell_width = occupancy_map.getMinCellWidth(); diff --git a/ros/wavemap_ros/CMakeLists.txt b/ros/wavemap_ros/CMakeLists.txt index 2002ae718..4ce6d5256 100644 --- a/ros/wavemap_ros/CMakeLists.txt +++ b/ros/wavemap_ros/CMakeLists.txt @@ -37,6 +37,9 @@ target_link_libraries(wavemap_server_node ${PROJECT_NAME}) cs_add_executable(wavemap_rosbag_processor app/rosbag_processor.cc) target_link_libraries(wavemap_rosbag_processor ${PROJECT_NAME}) +add_executable(occupancy_to_esdf app/occupancy_to_esdf.cc) +target_link_libraries(occupancy_to_esdf ${PROJECT_NAME}) + # Export cs_install() cs_export() diff --git a/ros/wavemap_ros/app/occupancy_to_esdf.cc b/ros/wavemap_ros/app/occupancy_to_esdf.cc new file mode 100644 index 000000000..c20b225a0 --- /dev/null +++ b/ros/wavemap_ros/app/occupancy_to_esdf.cc @@ -0,0 +1,57 @@ +#include +#include + +#include +#include +#include +#include +#include +#include + +using namespace wavemap; // NOLINT +int main(int argc, char** argv) { + // Initialize GLOG + google::InitGoogleLogging(argv[0]); + google::ParseCommandLineFlags(&argc, &argv, false); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + // Read params + if (argc != 2) { + LOG(ERROR) + << "Please supply a path to an occupancy map as the first argument."; + return EXIT_FAILURE; + } + const std::filesystem::path map_file_path = argv[1]; + + // Load the occupancy map + VolumetricDataStructureBase::Ptr occupancy_map; + io::fileToMap(map_file_path, occupancy_map); + CHECK_NOTNULL(occupancy_map); + + // Currently, only hashed wavelet octree maps are supported as input + const auto hashed_map = + std::dynamic_pointer_cast(occupancy_map); + if (!hashed_map) { + LOG(ERROR) + << "Only hashed wavelet octree occupancy maps are currently supported."; + } + + // Generate the ESDF + const std::filesystem::path esdf_file_path = + std::filesystem::path(map_file_path).replace_extension(".sdf.wvmp"); + LOG(INFO) << "Generating ESDF"; + constexpr FloatingPoint kOccupancyThreshold = 0.f; + constexpr FloatingPoint kMaxDistance = 2.f; + const QuasiEuclideanSDFGenerator sdf_generator{kMaxDistance, + kOccupancyThreshold}; + const auto esdf = sdf_generator.generate(*hashed_map); + + // Save the ESDF + LOG(INFO) << "Saving ESDF to path: " << esdf_file_path; + if (!io::mapToFile(esdf, esdf_file_path)) { + LOG(ERROR) << "Could not save ESDF"; + return EXIT_FAILURE; + } +} From a059ba5b2f2c8740b3a8140bf216f1668016d731 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 15 Nov 2023 12:57:12 +0100 Subject: [PATCH 012/159] Reduce code duplication between hashed dense block data structures --- libraries/wavemap/CMakeLists.txt | 1 - .../wavemap/data_structure/dense_block_hash.h | 69 ++++++++++++ .../impl/dense_block_hash_inl.h | 105 ++++++++++++++++++ .../wavemap/data_structure/spatial_hash.h | 1 - .../include/wavemap/map/hashed_blocks.h | 55 ++------- .../wavemap/map/impl/hashed_blocks_inl.h | 56 +--------- .../utils/sdf/full_euclidean_sdf_generator.h | 16 ++- .../sdf/impl/vector_distance_field_inl.h | 69 ------------ .../wavemap/utils/sdf/vector_distance_field.h | 67 ----------- libraries/wavemap/src/map/hashed_blocks.cc | 17 +-- .../utils/sdf/full_euclidean_sdf_generator.cc | 9 +- .../sdf/quasi_euclidean_sdf_generator.cc | 4 +- .../src/utils/sdf/vector_distance_field.cc | 3 - 13 files changed, 215 insertions(+), 257 deletions(-) create mode 100644 libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h create mode 100644 libraries/wavemap/include/wavemap/data_structure/impl/dense_block_hash_inl.h delete mode 100644 libraries/wavemap/include/wavemap/utils/sdf/impl/vector_distance_field_inl.h delete mode 100644 libraries/wavemap/include/wavemap/utils/sdf/vector_distance_field.h delete mode 100644 libraries/wavemap/src/utils/sdf/vector_distance_field.cc diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index 2337dac1c..df07d0e3f 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -69,7 +69,6 @@ add_library(${PROJECT_NAME} src/utils/sdf/cell_neighborhoods.cc src/utils/sdf/full_euclidean_sdf_generator.cc src/utils/sdf/quasi_euclidean_sdf_generator.cc - src/utils/sdf/vector_distance_field.cc src/utils/stopwatch.cc src/utils/thread_pool.cc) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h b/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h new file mode 100644 index 000000000..fdbea9efc --- /dev/null +++ b/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h @@ -0,0 +1,69 @@ +#ifndef WAVEMAP_DATA_STRUCTURE_DENSE_BLOCK_HASH_H_ +#define WAVEMAP_DATA_STRUCTURE_DENSE_BLOCK_HASH_H_ + +#include "wavemap/data_structure/dense_grid.h" +#include "wavemap/data_structure/spatial_hash.h" +#include "wavemap/utils/math/int_math.h" + +namespace wavemap { +template +class DenseBlockHash { + public: + static constexpr IndexElement kCellsPerSide = cells_per_side; + static constexpr IndexElement kCellsPerSideLog2 = + int_math::log2_floor(cells_per_side); + static constexpr IndexElement kDim = dim; + + using BlockIndex = Index3D; + using CellIndex = Index3D; + using Block = DenseGrid; + using BlockHashMap = SpatialHash; + + explicit DenseBlockHash(FloatingPoint min_cell_width, + CellDataT default_value = 0.f) + : min_cell_width_(min_cell_width), default_value_(default_value) {} + + bool empty() const { return block_map_.empty(); } + size_t size() const { return Block::kCellsPerBlock * block_map_.size(); } + void clear() { block_map_.clear(); } + + bool hasBlock(const Index3D& block_index) const { + return block_map_.hasBlock(block_index); + } + Block* getBlock(const Index3D& block_index); + const Block* getBlock(const Index3D& block_index) const; + Block& getOrAllocateBlock(const Index3D& block_index); + + const CellDataT& getCellValue(const Index3D& index) const; + CellDataT& getOrAllocateCellValue(const Index3D& index); + const CellDataT& getDefaultCellValue() const { return default_value_; } + + template + void forEachBlock(IndexedBlockVisitor visitor_fn) { + block_map_.forEachBlock(visitor_fn); + } + template + void forEachBlock(IndexedBlockVisitor visitor_fn) const { + block_map_.forEachBlock(visitor_fn); + } + + template + void forEachLeaf(IndexedLeafVisitorFunction visitor_fn); + template + void forEachLeaf(IndexedLeafVisitorFunction visitor_fn) const; + + protected: + const FloatingPoint min_cell_width_; + const CellDataT default_value_; + BlockHashMap block_map_; + + static Index3D indexToBlockIndex(const Index3D& index); + static Index3D indexToCellIndex(const Index3D& index); + static Index3D cellAndBlockIndexToIndex(const Index3D& block_index, + const Index3D& cell_index); +}; +} // namespace wavemap + +#include "wavemap/data_structure/impl/dense_block_hash_inl.h" + +#endif // WAVEMAP_DATA_STRUCTURE_DENSE_BLOCK_HASH_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/impl/dense_block_hash_inl.h b/libraries/wavemap/include/wavemap/data_structure/impl/dense_block_hash_inl.h new file mode 100644 index 000000000..ee22957e2 --- /dev/null +++ b/libraries/wavemap/include/wavemap/data_structure/impl/dense_block_hash_inl.h @@ -0,0 +1,105 @@ +#ifndef WAVEMAP_DATA_STRUCTURE_IMPL_DENSE_BLOCK_HASH_INL_H_ +#define WAVEMAP_DATA_STRUCTURE_IMPL_DENSE_BLOCK_HASH_INL_H_ + +namespace wavemap { +template +inline DenseGrid* +DenseBlockHash::getBlock( + const Index3D& block_index) { + return block_map_.getBlock(block_index); +} + +template +inline const DenseGrid* +DenseBlockHash::getBlock( + const Index3D& block_index) const { + return block_map_.getBlock(block_index); +} + +template +inline DenseGrid& +DenseBlockHash::getOrAllocateBlock( + const Index3D& block_index) { + return block_map_.getOrAllocateBlock(block_index, default_value_); +} + +template +inline const CellDataT& +DenseBlockHash::getCellValue( + const wavemap::Index3D& index) const { + const BlockIndex block_index = indexToBlockIndex(index); + const auto* block = getBlock(block_index); + if (!block) { + return default_value_; + } + const CellIndex cell_index = indexToCellIndex(index); + return block->at(cell_index); +} + +template +inline CellDataT& +DenseBlockHash::getOrAllocateCellValue( + const Index3D& index) { + const BlockIndex block_index = indexToBlockIndex(index); + auto& block = getOrAllocateBlock(block_index); + const CellIndex cell_index = indexToCellIndex(index); + return block.at(cell_index); +} + +template +template +void DenseBlockHash::forEachLeaf( + IndexedLeafVisitorFunction visitor_fn) { + block_map_.forEachBlock([&visitor_fn](const BlockIndex& block_index, + const Block& block) { + for (LinearIndex cell_idx = 0u; cell_idx < Block::kCellsPerBlock; + ++cell_idx) { + const Index3D cell_index = + convert::linearIndexToIndex(cell_idx); + const Index3D index = cellAndBlockIndexToIndex(block_index, cell_index); + auto& cell_data = block[cell_idx]; + std::invoke(visitor_fn, index, cell_data); + } + }); +} + +template +template +void DenseBlockHash::forEachLeaf( + IndexedLeafVisitorFunction visitor_fn) const { + block_map_.forEachBlock([&visitor_fn](const BlockIndex& block_index, + const Block& block) { + for (LinearIndex cell_idx = 0u; cell_idx < Block::kCellsPerBlock; + ++cell_idx) { + const Index3D cell_index = + convert::linearIndexToIndex(cell_idx); + const Index3D index = cellAndBlockIndexToIndex(block_index, cell_index); + const auto& cell_data = block[cell_idx]; + std::invoke(visitor_fn, index, cell_data); + } + }); +} + +template +inline Index3D +DenseBlockHash::indexToBlockIndex( + const Index3D& index) { + return convert::indexToBlockIndex(index, kCellsPerSideLog2); +} + +template +inline Index3D DenseBlockHash::indexToCellIndex( + const Index3D& index) { + return int_math::div_exp2_floor_remainder(index, kCellsPerSideLog2); +} + +template +inline Index3D +DenseBlockHash::cellAndBlockIndexToIndex( + const Index3D& block_index, const Index3D& cell_index) { + return kCellsPerSide * block_index + cell_index; +} + +} // namespace wavemap + +#endif // WAVEMAP_DATA_STRUCTURE_IMPL_DENSE_BLOCK_HASH_INL_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/spatial_hash.h b/libraries/wavemap/include/wavemap/data_structure/spatial_hash.h index 0e6999369..54790135a 100644 --- a/libraries/wavemap/include/wavemap/data_structure/spatial_hash.h +++ b/libraries/wavemap/include/wavemap/data_structure/spatial_hash.h @@ -21,7 +21,6 @@ class SpatialHash { static constexpr IndexElement kDim = dim; using BlockIndex = Index; - using CellIndex = Index; using BlockData = BlockDataT; bool empty() const { return block_map_.empty(); } diff --git a/libraries/wavemap/include/wavemap/map/hashed_blocks.h b/libraries/wavemap/include/wavemap/map/hashed_blocks.h index a963a4b15..7dc17dc58 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_blocks.h +++ b/libraries/wavemap/include/wavemap/map/hashed_blocks.h @@ -7,44 +7,34 @@ #include "wavemap/common.h" #include "wavemap/config/config_base.h" -#include "wavemap/data_structure/dense_grid.h" -#include "wavemap/data_structure/spatial_hash.h" -#include "wavemap/indexing/index_hashes.h" +#include "wavemap/data_structure/dense_block_hash.h" #include "wavemap/map/volumetric_data_structure_base.h" -#include "wavemap/utils/math/int_math.h" namespace wavemap { -class HashedBlocks : public VolumetricDataStructureBase { - private: - static constexpr IndexElement kCellsPerSideLog2 = 4; - static constexpr IndexElement kCellsPerSide = - int_math::exp2(kCellsPerSideLog2); - +class HashedBlocks + : public VolumetricDataStructureBase, + public DenseBlockHash { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; using Config = VolumetricDataStructureConfig; static constexpr bool kRequiresExplicitThresholding = false; - using BlockIndex = Index3D; - using CellIndex = Index3D; - using Block = DenseGrid; - using BlockHashMap = SpatialHash; + using VolumetricDataStructureBase::kDim; explicit HashedBlocks(const VolumetricDataStructureConfig& config, FloatingPoint default_value = 0.f) : VolumetricDataStructureBase(config.checkValid()), - default_value_(default_value) {} + DenseBlockHash(config.min_cell_width, default_value) {} - bool empty() const override { return block_map_.empty(); } - size_t size() const override { - return Block::kCellsPerBlock * block_map_.size(); - } + bool empty() const override { return DenseBlockHash::empty(); } + size_t size() const override { return DenseBlockHash::size(); } void threshold() override { // Not needed. Data is already tresholded when added. } void prune() override; - void clear() override { block_map_.clear(); } + void clear() override { DenseBlockHash::clear(); } size_t getMemoryUsage() const override { return size() * sizeof(FloatingPoint); @@ -58,40 +48,15 @@ class HashedBlocks : public VolumetricDataStructureBase { const VolumetricDataStructureConfig& getConfig() { return config_; } FloatingPoint getCellValue(const Index3D& index) const override; - FloatingPoint& getCellValueRef(const Index3D& index); - FloatingPoint getDefaultCellValue() const { return default_value_; } void setCellValue(const Index3D& index, FloatingPoint new_value) override; void addToCellValue(const Index3D& index, FloatingPoint update) override; - bool hasBlock(const Index3D& block_index) const; - Block* getBlock(const Index3D& block_index); - const Block* getBlock(const Index3D& block_index) const; - Block& getOrAllocateBlock(const Index3D& block_index); - auto& getHashMap() { return block_map_; } const auto& getHashMap() const { return block_map_; } - template - void forEachBlock(IndexedBlockVisitor visitor_fn) { - block_map_.forEachBlock(visitor_fn); - } - template - void forEachBlock(IndexedBlockVisitor visitor_fn) const { - block_map_.forEachBlock(visitor_fn); - } - void forEachLeaf( typename VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn) const override; - - private: - const FloatingPoint default_value_; - BlockHashMap block_map_; - - static Index3D indexToBlockIndex(const Index3D& index); - static Index3D indexToCellIndex(const Index3D& index); - static Index3D cellAndBlockIndexToIndex(const Index3D& block_index, - const Index3D& cell_index); }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/map/impl/hashed_blocks_inl.h b/libraries/wavemap/include/wavemap/map/impl/hashed_blocks_inl.h index 9a865096d..47ab8a7f6 100644 --- a/libraries/wavemap/include/wavemap/map/impl/hashed_blocks_inl.h +++ b/libraries/wavemap/include/wavemap/map/impl/hashed_blocks_inl.h @@ -10,69 +10,19 @@ namespace wavemap { inline FloatingPoint HashedBlocks::getCellValue(const Index3D& index) const { - const BlockIndex block_index = indexToBlockIndex(index); - const auto* block = getBlock(block_index); - if (!block) { - return default_value_; - } - const CellIndex cell_index = indexToCellIndex(index); - return block->at(cell_index); -} - -inline FloatingPoint& HashedBlocks::getCellValueRef(const Index3D& index) { - const BlockIndex block_index = indexToBlockIndex(index); - auto& block = getOrAllocateBlock(block_index); - const CellIndex cell_index = indexToCellIndex(index); - return block.at(cell_index); + return DenseBlockHash::getCellValue(index); } inline void HashedBlocks::setCellValue(const Index3D& index, FloatingPoint new_value) { - const BlockIndex block_index = indexToBlockIndex(index); - Block& block = getOrAllocateBlock(block_index); - const CellIndex cell_index = indexToCellIndex(index); - block.at(cell_index) = new_value; + getOrAllocateCellValue(index) = new_value; } inline void HashedBlocks::addToCellValue(const Index3D& index, FloatingPoint update) { - const BlockIndex block_index = indexToBlockIndex(index); - Block& block = getOrAllocateBlock(block_index); - const CellIndex cell_index = indexToCellIndex(index); - FloatingPoint& cell_data = block.at(cell_index); + FloatingPoint& cell_data = getOrAllocateCellValue(index); cell_data = clampedAdd(cell_data, update); } - -inline bool HashedBlocks::hasBlock(const Index3D& block_index) const { - return block_map_.hasBlock(block_index); -} - -inline HashedBlocks::Block* HashedBlocks::getBlock(const Index3D& block_index) { - return block_map_.getBlock(block_index); -} - -inline const HashedBlocks::Block* HashedBlocks::getBlock( - const Index3D& block_index) const { - return block_map_.getBlock(block_index); -} - -inline HashedBlocks::Block& HashedBlocks::getOrAllocateBlock( - const Index3D& block_index) { - return block_map_.getOrAllocateBlock(block_index, default_value_); -} - -inline Index3D HashedBlocks::indexToBlockIndex(const Index3D& index) { - return convert::indexToBlockIndex(index, kCellsPerSideLog2); -} - -inline Index3D HashedBlocks::indexToCellIndex(const Index3D& index) { - return int_math::div_exp2_floor_remainder(index, kCellsPerSideLog2); -} - -inline Index3D HashedBlocks::cellAndBlockIndexToIndex( - const Index3D& block_index, const Index3D& cell_index) { - return kCellsPerSide * block_index + cell_index; -} } // namespace wavemap #endif // WAVEMAP_MAP_IMPL_HASHED_BLOCKS_INL_H_ diff --git a/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h b/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h index a44c0c6a4..b002fb188 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h +++ b/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h @@ -6,9 +6,23 @@ #include "wavemap/map/hashed_blocks.h" #include "wavemap/map/hashed_wavelet_octree.h" #include "wavemap/utils/sdf/cell_neighborhoods.h" -#include "wavemap/utils/sdf/vector_distance_field.h" namespace wavemap { +struct VectorDistance { + Index3D parent; + FloatingPoint distance; + + friend bool operator==(const VectorDistance& lhs, const VectorDistance& rhs) { + return lhs.parent == rhs.parent && lhs.distance == rhs.distance; + } + friend bool operator!=(const VectorDistance& lhs, const VectorDistance& rhs) { + return !(lhs == rhs); + } +}; + +using VectorDistanceField = + DenseBlockHash; + class FullEuclideanSDFGenerator { public: static constexpr FloatingPoint kMaxRelativeUnderEstimate = 1e-3f; diff --git a/libraries/wavemap/include/wavemap/utils/sdf/impl/vector_distance_field_inl.h b/libraries/wavemap/include/wavemap/utils/sdf/impl/vector_distance_field_inl.h deleted file mode 100644 index 3817a07b5..000000000 --- a/libraries/wavemap/include/wavemap/utils/sdf/impl/vector_distance_field_inl.h +++ /dev/null @@ -1,69 +0,0 @@ -#ifndef WAVEMAP_UTILS_SDF_IMPL_VECTOR_DISTANCE_FIELD_INL_H_ -#define WAVEMAP_UTILS_SDF_IMPL_VECTOR_DISTANCE_FIELD_INL_H_ - -namespace wavemap { -inline const VectorDistance& VectorDistanceField::getCellValue( - const wavemap::Index3D& index) const { - const BlockIndex block_index = indexToBlockIndex(index); - const auto* block = getBlock(block_index); - if (!block) { - return default_value_; - } - const CellIndex cell_index = indexToCellIndex(index); - return block->at(cell_index); -} - -inline VectorDistance& VectorDistanceField::getCellValueRef( - const Index3D& index) { - const BlockIndex block_index = indexToBlockIndex(index); - auto& block = getOrAllocateBlock(block_index); - const CellIndex cell_index = indexToCellIndex(index); - return block.at(cell_index); -} - -inline VectorDistanceField::Block* VectorDistanceField::getBlock( - const Index3D& block_index) { - return block_map_.getBlock(block_index); -} - -inline const VectorDistanceField::Block* VectorDistanceField::getBlock( - const Index3D& block_index) const { - return block_map_.getBlock(block_index); -} - -inline VectorDistanceField::Block& VectorDistanceField::getOrAllocateBlock( - const Index3D& block_index) { - return block_map_.getOrAllocateBlock(block_index, default_value_); -} - -template -void VectorDistanceField::forEachLeaf( - IndexedLeafVisitorFunction visitor_fn) const { - block_map_.forEachBlock([&visitor_fn](const BlockIndex& block_index, - const Block& block) { - for (LinearIndex cell_idx = 0u; cell_idx < Block::kCellsPerBlock; - ++cell_idx) { - const Index3D cell_index = - convert::linearIndexToIndex(cell_idx); - const Index3D index = cellAndBlockIndexToIndex(block_index, cell_index); - const auto& cell_data = block[cell_idx]; - std::invoke(visitor_fn, index, cell_data); - } - }); -} - -inline Index3D VectorDistanceField::indexToBlockIndex(const Index3D& index) { - return convert::indexToBlockIndex(index, kCellsPerSideLog2); -} - -inline Index3D VectorDistanceField::indexToCellIndex(const Index3D& index) { - return int_math::div_exp2_floor_remainder(index, kCellsPerSideLog2); -} - -inline Index3D VectorDistanceField::cellAndBlockIndexToIndex( - const Index3D& block_index, const Index3D& cell_index) { - return kCellsPerSide * block_index + cell_index; -} -} // namespace wavemap - -#endif // WAVEMAP_UTILS_SDF_IMPL_VECTOR_DISTANCE_FIELD_INL_H_ diff --git a/libraries/wavemap/include/wavemap/utils/sdf/vector_distance_field.h b/libraries/wavemap/include/wavemap/utils/sdf/vector_distance_field.h deleted file mode 100644 index 1112057ef..000000000 --- a/libraries/wavemap/include/wavemap/utils/sdf/vector_distance_field.h +++ /dev/null @@ -1,67 +0,0 @@ -#ifndef WAVEMAP_UTILS_SDF_VECTOR_DISTANCE_FIELD_H_ -#define WAVEMAP_UTILS_SDF_VECTOR_DISTANCE_FIELD_H_ - -#include - -#include "wavemap/common.h" -#include "wavemap/data_structure/dense_grid.h" -#include "wavemap/data_structure/spatial_hash.h" -#include "wavemap/utils/math/int_math.h" - -namespace wavemap { -struct VectorDistance { - Index3D parent = Index3D::Constant(std::numeric_limits::max()); - FloatingPoint distance = 0.f; - - friend bool operator==(const VectorDistance& lhs, const VectorDistance& rhs) { - return lhs.parent == rhs.parent && lhs.distance == rhs.distance; - } - friend bool operator!=(const VectorDistance& lhs, const VectorDistance& rhs) { - return !(lhs == rhs); - } -}; - -class VectorDistanceField { - private: - static constexpr IndexElement kCellsPerSideLog2 = 4; - static constexpr IndexElement kCellsPerSide = - int_math::exp2(kCellsPerSideLog2); - static constexpr IndexElement kDim = 3; - - public: - using BlockIndex = Index3D; - using CellIndex = Index3D; - using Block = DenseGrid; - using BlockHashMap = SpatialHash; - - explicit VectorDistanceField(FloatingPoint min_cell_width, - FloatingPoint default_value = 0.f) - : min_cell_width_(min_cell_width), - default_value_({VectorDistance{}.parent, default_value}) {} - - const VectorDistance& getCellValue(const Index3D& index) const; - VectorDistance& getCellValueRef(const Index3D& index); - const VectorDistance& getDefaultCellValue() const { return default_value_; } - - Block* getBlock(const Index3D& block_index); - const Block* getBlock(const Index3D& block_index) const; - Block& getOrAllocateBlock(const Index3D& block_index); - - template - void forEachLeaf(IndexedLeafVisitorFunction visitor_fn) const; - - private: - const FloatingPoint min_cell_width_; - const VectorDistance default_value_; - BlockHashMap block_map_; - - static Index3D indexToBlockIndex(const Index3D& index); - static Index3D indexToCellIndex(const Index3D& index); - static Index3D cellAndBlockIndexToIndex(const Index3D& block_index, - const Index3D& cell_index); -}; -} // namespace wavemap - -#include "wavemap/utils/sdf/impl/vector_distance_field_inl.h" - -#endif // WAVEMAP_UTILS_SDF_VECTOR_DISTANCE_FIELD_H_ diff --git a/libraries/wavemap/src/map/hashed_blocks.cc b/libraries/wavemap/src/map/hashed_blocks.cc index 1b9280ea0..6cc0cd7f0 100644 --- a/libraries/wavemap/src/map/hashed_blocks.cc +++ b/libraries/wavemap/src/map/hashed_blocks.cc @@ -25,17 +25,10 @@ void HashedBlocks::prune() { void HashedBlocks::forEachLeaf( VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn) const { - forEachBlock([&visitor_fn](const BlockIndex& block_index, - const Block& block) { - for (LinearIndex cell_idx = 0u; cell_idx < Block::kCellsPerBlock; - ++cell_idx) { - const Index3D cell_index = - convert::linearIndexToIndex(cell_idx); - const Index3D index = cellAndBlockIndexToIndex(block_index, cell_index); - const FloatingPoint cell_data = block[cell_idx]; - const OctreeIndex hierarchical_cell_index = OctreeIndex{0, index}; - visitor_fn(hierarchical_cell_index, cell_data); - } - }); + DenseBlockHash::forEachLeaf( + [&visitor_fn](const Index3D& index, FloatingPoint cell_data) { + const OctreeIndex hierarchical_cell_index = OctreeIndex{0, index}; + visitor_fn(hierarchical_cell_index, cell_data); + }); } } // namespace wavemap diff --git a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc index fc8768457..32db6f6f2 100644 --- a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc @@ -11,7 +11,10 @@ HashedBlocks FullEuclideanSDFGenerator::generate( ZoneScoped; // Initialize the SDF data structure const FloatingPoint min_cell_width = occupancy_map.getMinCellWidth(); - VectorDistanceField full_sdf(min_cell_width, max_distance_); + const Index3D uninitialized_parent = + Index3D::Constant(std::numeric_limits::max()); + VectorDistanceField full_sdf( + min_cell_width, VectorDistance{uninitialized_parent, max_distance_}); // Initialize the bucketed priority queue const int num_bins = @@ -83,7 +86,7 @@ void FullEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, } // Get the voxel's SDF value - auto& [sdf_parent, sdf_value] = sdf.getCellValueRef(index); + auto& [sdf_parent, sdf_value] = sdf.getOrAllocateCellValue(index); const bool sdf_uninitialized = sdf_parent == sdf.getDefaultCellValue().parent; @@ -135,7 +138,7 @@ void FullEuclideanSDFGenerator::propagate( // Get the neighbor's SDF value auto& [neighbor_sdf_parent, neighbor_sdf_value] = - sdf.getCellValueRef(neighbor_index); + sdf.getOrAllocateCellValue(neighbor_index); // If the neighbor is uninitialized, get its sign from the occupancy map const bool neighbor_sdf_uninitialized = diff --git a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc index d993e25a9..0e391d1cb 100644 --- a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc @@ -76,7 +76,7 @@ void QuasiEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, } // Get the voxel's SDF value - FloatingPoint& sdf_value = sdf.getCellValueRef(index); + FloatingPoint& sdf_value = sdf.getOrAllocateCellValue(index); const bool sdf_uninitialized = sdf.getDefaultCellValue() == sdf_value; // Update the voxel's SDF value @@ -129,7 +129,7 @@ void QuasiEuclideanSDFGenerator::propagate( // Get the neighbor's SDF value const Index3D neighbor_index = index + kNeighborIndexOffsets[neighbor_idx]; - FloatingPoint& neighbor_sdf = sdf.getCellValueRef(neighbor_index); + FloatingPoint& neighbor_sdf = sdf.getOrAllocateCellValue(neighbor_index); // If the neighbor is uninitialized, get its sign from the occupancy map const bool neighbor_sdf_uninitialized = diff --git a/libraries/wavemap/src/utils/sdf/vector_distance_field.cc b/libraries/wavemap/src/utils/sdf/vector_distance_field.cc deleted file mode 100644 index 1f8a21d3b..000000000 --- a/libraries/wavemap/src/utils/sdf/vector_distance_field.cc +++ /dev/null @@ -1,3 +0,0 @@ -#include "wavemap/utils/sdf/vector_distance_field.h" - -namespace wavemap {} From 8964b6ba80653ef6ac7bbe9280c7fa44f60bc00b Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 15 Nov 2023 14:08:26 +0100 Subject: [PATCH 013/159] Move ESDF generation script to examples --- examples/CMakeLists.txt | 2 ++ .../app => examples/src/planning}/occupancy_to_esdf.cc | 0 ros/wavemap_ros/CMakeLists.txt | 3 --- 3 files changed, 2 insertions(+), 3 deletions(-) rename {ros/wavemap_ros/app => examples/src/planning}/occupancy_to_esdf.cc (100%) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 3e2c83a98..a18cc6813 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -35,3 +35,5 @@ cs_add_executable(trilinear_interpolation cs_add_executable(classification src/queries/classification.cc) + +cs_add_executable(occupancy_to_esdf src/planning/occupancy_to_esdf.cc) diff --git a/ros/wavemap_ros/app/occupancy_to_esdf.cc b/examples/src/planning/occupancy_to_esdf.cc similarity index 100% rename from ros/wavemap_ros/app/occupancy_to_esdf.cc rename to examples/src/planning/occupancy_to_esdf.cc diff --git a/ros/wavemap_ros/CMakeLists.txt b/ros/wavemap_ros/CMakeLists.txt index 4ce6d5256..2002ae718 100644 --- a/ros/wavemap_ros/CMakeLists.txt +++ b/ros/wavemap_ros/CMakeLists.txt @@ -37,9 +37,6 @@ target_link_libraries(wavemap_server_node ${PROJECT_NAME}) cs_add_executable(wavemap_rosbag_processor app/rosbag_processor.cc) target_link_libraries(wavemap_rosbag_processor ${PROJECT_NAME}) -add_executable(occupancy_to_esdf app/occupancy_to_esdf.cc) -target_link_libraries(occupancy_to_esdf ${PROJECT_NAME}) - # Export cs_install() cs_export() From f3fbbe66439b80832631ad9133041dde7b458a04 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 15 Nov 2023 14:23:53 +0100 Subject: [PATCH 014/159] Improve SDF tests --- .../utils/sdf/quasi_euclidean_sdf_generator.h | 2 +- .../test/src/utils/test_sdf_generation.cc | 253 +++++++++--------- 2 files changed, 131 insertions(+), 124 deletions(-) diff --git a/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h b/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h index 7d4d4a0db..788278e9e 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h +++ b/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h @@ -10,7 +10,7 @@ namespace wavemap { class QuasiEuclideanSDFGenerator { public: - static constexpr FloatingPoint kMaxRelativeUnderEstimate = kEpsilon; + static constexpr FloatingPoint kMaxRelativeUnderEstimate = 1e-3f; static constexpr FloatingPoint kMaxRelativeOverEstimate = 0.125f + 1e-3f; explicit QuasiEuclideanSDFGenerator(FloatingPoint max_distance = 2.f, diff --git a/libraries/wavemap/test/src/utils/test_sdf_generation.cc b/libraries/wavemap/test/src/utils/test_sdf_generation.cc index 890935064..a5403d339 100644 --- a/libraries/wavemap/test/src/utils/test_sdf_generation.cc +++ b/libraries/wavemap/test/src/utils/test_sdf_generation.cc @@ -20,148 +20,155 @@ using SdfGeneratorTypes = TYPED_TEST_SUITE(SdfGenerationTest, SdfGeneratorTypes, ); TYPED_TEST(SdfGenerationTest, BruteForceEquivalence) { - // Params - const Index3D min_index = Index3D::Constant(-10); - const Index3D max_index = Index3D::Constant(100); - constexpr FloatingPoint kMaxSdfDistance = 2.f; + constexpr int kNumIterations = 2; + for (int iteration = 0; iteration < kNumIterations; ++iteration) { + // Params + const Index3D min_index = Index3D::Constant(-10); + const Index3D max_index = Index3D::Constant(100); + const FloatingPoint kMaxSdfDistance = + FixtureBase::getRandomFloat(0.2f, 4.f); - // Create the map and occupancy classification util - const auto config = - ConfigGenerator::getRandomConfig(); - HashedWaveletOctree map{config}; - const OccupancyClassifier classifier{}; + // Create the map and occupancy classification util + const auto config = + ConfigGenerator::getRandomConfig(); + HashedWaveletOctree map{config}; + const OccupancyClassifier classifier{}; - // Generate random obstacles - auto obstacle_cells = - GeometryGenerator::getRandomIndexVector<3>(10, 20, min_index, max_index); - // Add a cube - const FloatingPoint min_cell_width = map.getMinCellWidth(); - const IndexElement padding = std::ceil(kMaxSdfDistance / min_cell_width); - const Index3D cube_center{2, 6, 4}; - for (const Index3D& index : - Grid<3>(cube_center.array() - padding, cube_center.array() + padding)) { - obstacle_cells.emplace_back(index); - } + // Generate random obstacles + auto obstacle_cells = GeometryGenerator::getRandomIndexVector<3>( + 10, 20, min_index, max_index); + // Add a cube + const FloatingPoint min_cell_width = map.getMinCellWidth(); + const IndexElement padding = std::ceil(kMaxSdfDistance / min_cell_width); + const Index3D cube_center{2, 6, 4}; + for (const Index3D& index : Grid<3>(cube_center.array() - padding, + cube_center.array() + padding)) { + obstacle_cells.emplace_back(index); + } - // Set default occupancy to free - const OctreeIndex min_block_index = convert::indexAndHeightToNodeIndex<3>( - min_index.array() - padding, map.getTreeHeight()); - const OctreeIndex max_block_index = convert::indexAndHeightToNodeIndex<3>( - max_index.array() + padding, map.getTreeHeight()); - for (const auto& block_index : - Grid(min_block_index.position, max_block_index.position)) { - map.getOrAllocateBlock(block_index).getRootScale() = config.min_log_odds; - } + // Set default occupancy to free + const OctreeIndex min_block_index = convert::indexAndHeightToNodeIndex<3>( + min_index.array() - padding, map.getTreeHeight()); + const OctreeIndex max_block_index = convert::indexAndHeightToNodeIndex<3>( + max_index.array() + padding, map.getTreeHeight()); + for (const auto& block_index : + Grid(min_block_index.position, max_block_index.position)) { + map.getOrAllocateBlock(block_index).getRootScale() = config.min_log_odds; + } - // Set obstacles to occupied - for (const Index3D& index : obstacle_cells) { - map.setCellValue(index, config.max_log_odds); - } + // Set obstacles to occupied + for (const Index3D& index : obstacle_cells) { + map.setCellValue(index, config.max_log_odds); + } - // Generate the SDF - TypeParam sdf_generator{kMaxSdfDistance}; - const auto sdf = sdf_generator.generate(map); + // Generate the SDF + TypeParam sdf_generator{kMaxSdfDistance}; + const auto sdf = sdf_generator.generate(map); - // Compare the SDF distances to the brute force min distance - sdf.forEachLeaf([&map, &classifier, &sdf_generator, &sdf, &obstacle_cells, - min_cell_width, padding](const OctreeIndex& node_index, - FloatingPoint sdf_value) { - // In unobserved space, the SDF should be uninitialized - const FloatingPoint occupancy_value = map.getCellValue(node_index); - if (OccupancyClassifier::isUnobserved(occupancy_value)) { - // In unknown space the SDF should be uninitialized - EXPECT_NEAR(sdf_value, sdf.getDefaultCellValue(), kEpsilon); - return; - } + // Compare the SDF distances to the brute force min distance + sdf.forEachLeaf([&map, &classifier, &sdf_generator, &sdf, &obstacle_cells, + min_cell_width, padding](const OctreeIndex& node_index, + FloatingPoint sdf_value) { + // In unobserved space, the SDF should be uninitialized + const FloatingPoint occupancy_value = map.getCellValue(node_index); + if (OccupancyClassifier::isUnobserved(occupancy_value)) { + // In unknown space the SDF should be uninitialized + EXPECT_NEAR(sdf_value, sdf.getDefaultCellValue(), kEpsilon); + return; + } - const Point3D node_center = - convert::nodeIndexToCenterPoint(node_index, min_cell_width); + const Point3D node_center = + convert::nodeIndexToCenterPoint(node_index, min_cell_width); - // Find the closest surface using brute force - FloatingPoint sdf_brute_force = sdf.getDefaultCellValue(); - Index3D parent_brute_force = - Index3D::Constant(std::numeric_limits::max()); - if (classifier.isFree(occupancy_value)) { - // In free space, the SDF should always be positive - EXPECT_GT(sdf_value, 0.f); + // Find the closest surface using brute force + FloatingPoint sdf_brute_force = sdf.getDefaultCellValue(); + Index3D parent_brute_force = + Index3D::Constant(std::numeric_limits::max()); + if (classifier.isFree(occupancy_value)) { + // In free space, the SDF should always be positive + EXPECT_GT(sdf_value, 0.f); - // Find the distance to the closest obstacle - for (const auto& obstacle_cell : obstacle_cells) { - const auto obstacle_aabb = convert::nodeIndexToAABB( - OctreeIndex{0, obstacle_cell}, min_cell_width); - const FloatingPoint min_dist = obstacle_aabb.minDistanceTo(node_center); - if (min_dist < sdf_brute_force) { - sdf_brute_force = min_dist; - parent_brute_force = obstacle_cell; - } - } - } else { - // Find the distance to the closest free cell - for (const Index3D& neighbor_index : - Grid<3>(node_index.position.array() - padding, - node_index.position.array() + padding)) { - const FloatingPoint neighbor_occupancy_value = - map.getCellValue(neighbor_index); - if (classifier.isFree(neighbor_occupancy_value) && - OccupancyClassifier::isObserved(neighbor_occupancy_value)) { - const auto free_cell_aabb = convert::nodeIndexToAABB( - OctreeIndex{0, neighbor_index}, min_cell_width); + // Find the distance to the closest obstacle + for (const auto& obstacle_cell : obstacle_cells) { + const auto obstacle_aabb = convert::nodeIndexToAABB( + OctreeIndex{0, obstacle_cell}, min_cell_width); const FloatingPoint min_dist = - free_cell_aabb.minDistanceTo(node_center); + obstacle_aabb.minDistanceTo(node_center); if (min_dist < sdf_brute_force) { sdf_brute_force = min_dist; - parent_brute_force = neighbor_index; + parent_brute_force = obstacle_cell; } } - } - // Adjust the sign to reflect we're inside the obstacle - sdf_brute_force = -sdf_brute_force; - - // In occupied space, the SDF should be - if (std::abs(sdf_brute_force) < sdf.getDefaultCellValue()) { - // Negative - EXPECT_LT(sdf_value, 0.f); } else { - // Or uninitialized - EXPECT_NEAR(sdf_value, sdf.getDefaultCellValue(), kEpsilon); + // Find the distance to the closest free cell + for (const Index3D& neighbor_index : + Grid<3>(node_index.position.array() - padding, + node_index.position.array() + padding)) { + const FloatingPoint neighbor_occupancy_value = + map.getCellValue(neighbor_index); + if (classifier.isFree(neighbor_occupancy_value) && + OccupancyClassifier::isObserved(neighbor_occupancy_value)) { + const auto free_cell_aabb = convert::nodeIndexToAABB( + OctreeIndex{0, neighbor_index}, min_cell_width); + const FloatingPoint min_dist = + free_cell_aabb.minDistanceTo(node_center); + if (min_dist < sdf_brute_force) { + sdf_brute_force = min_dist; + parent_brute_force = neighbor_index; + } + } + } + // Adjust the sign to reflect we're inside the obstacle + sdf_brute_force = -sdf_brute_force; + + // In occupied space, the SDF should be + if (std::abs(sdf_brute_force) < sdf.getDefaultCellValue()) { + // Negative + EXPECT_LT(sdf_value, 0.f); + } else { + // Or uninitialized + EXPECT_NEAR(sdf_value, sdf.getDefaultCellValue(), kEpsilon); + } } - } - // Check that the SDF accurately approximates the min obstacle distance - constexpr FloatingPoint kMaxRelativeUnderEstimate = - TypeParam::kMaxRelativeUnderEstimate; - constexpr FloatingPoint kMaxRelativeOverEstimate = - TypeParam::kMaxRelativeOverEstimate; - if (std::abs(sdf_brute_force) < sdf.getDefaultCellValue()) { - if (0.f < sdf_brute_force) { - EXPECT_LT(sdf_value, sdf_brute_force * (1.f + kMaxRelativeOverEstimate)) - << "At index " << print::eigen::oneLine(node_index.position) - << " with nearest obstacle " - << print::eigen::oneLine(parent_brute_force); - EXPECT_GT(sdf_value, - sdf_brute_force * (1.f - kMaxRelativeUnderEstimate)) - << "At index " << print::eigen::oneLine(node_index.position) - << " with nearest obstacle " - << print::eigen::oneLine(parent_brute_force); + // Check that the SDF accurately approximates the min obstacle distance + constexpr FloatingPoint kMaxRelativeUnderEstimate = + TypeParam::kMaxRelativeUnderEstimate; + constexpr FloatingPoint kMaxRelativeOverEstimate = + TypeParam::kMaxRelativeOverEstimate; + if (std::abs(sdf_brute_force) < sdf.getDefaultCellValue()) { + if (0.f < sdf_brute_force) { + EXPECT_LT(sdf_value, + sdf_brute_force * (1.f + kMaxRelativeOverEstimate)) + << "At index " << print::eigen::oneLine(node_index.position) + << " with nearest obstacle " + << print::eigen::oneLine(parent_brute_force); + EXPECT_GT(sdf_value, + sdf_brute_force * (1.f - kMaxRelativeUnderEstimate)) + << "At index " << print::eigen::oneLine(node_index.position) + << " with nearest obstacle " + << print::eigen::oneLine(parent_brute_force); + } else { + EXPECT_GT(sdf_value, + sdf_brute_force * (1.f + kMaxRelativeOverEstimate)) + << "At index " << print::eigen::oneLine(node_index.position) + << " with nearest free cell " + << print::eigen::oneLine(parent_brute_force); + EXPECT_LT(sdf_value, + sdf_brute_force * (1.f - kMaxRelativeUnderEstimate)) + << "At index " << print::eigen::oneLine(node_index.position) + << " with nearest free cell " + << print::eigen::oneLine(parent_brute_force); + } } else { - EXPECT_GT(sdf_value, sdf_brute_force * (1.f + kMaxRelativeOverEstimate)) - << "At index " << print::eigen::oneLine(node_index.position) - << " with nearest free cell " - << print::eigen::oneLine(parent_brute_force); EXPECT_LT(sdf_value, - sdf_brute_force * (1.f - kMaxRelativeUnderEstimate)) - << "At index " << print::eigen::oneLine(node_index.position) - << " with nearest free cell " - << print::eigen::oneLine(parent_brute_force); + sdf.getDefaultCellValue() * (1.f + kMaxRelativeOverEstimate)) + << "At index " << print::eigen::oneLine(node_index.position); + EXPECT_GT(sdf_value, + sdf.getDefaultCellValue() * (1.f - kMaxRelativeUnderEstimate)) + << "At index " << print::eigen::oneLine(node_index.position); } - } else { - EXPECT_LT(sdf_value, - sdf.getDefaultCellValue() * (1.f + kMaxRelativeOverEstimate)) - << "At index " << print::eigen::oneLine(node_index.position); - EXPECT_GT(sdf_value, - sdf.getDefaultCellValue() * (1.f - kMaxRelativeUnderEstimate)) - << "At index " << print::eigen::oneLine(node_index.position); - } - }); + }); + } } } // namespace wavemap From 585d32e55c2ee49bc77e49c635b109c81d44f75f Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 16 Nov 2023 10:21:37 +0100 Subject: [PATCH 015/159] Minor cleanup --- examples/CMakeLists.txt | 3 ++- examples/src/planning/occupancy_to_esdf.cc | 1 - .../wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc | 6 +++--- .../wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc | 6 +++--- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index a18cc6813..e54cf7eea 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -36,4 +36,5 @@ cs_add_executable(trilinear_interpolation cs_add_executable(classification src/queries/classification.cc) -cs_add_executable(occupancy_to_esdf src/planning/occupancy_to_esdf.cc) +cs_add_executable(occupancy_to_esdf + src/planning/occupancy_to_esdf.cc) diff --git a/examples/src/planning/occupancy_to_esdf.cc b/examples/src/planning/occupancy_to_esdf.cc index c20b225a0..93f57c95e 100644 --- a/examples/src/planning/occupancy_to_esdf.cc +++ b/examples/src/planning/occupancy_to_esdf.cc @@ -1,5 +1,4 @@ #include -#include #include #include diff --git a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc index 32db6f6f2..a30996991 100644 --- a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc @@ -141,9 +141,9 @@ void FullEuclideanSDFGenerator::propagate( sdf.getOrAllocateCellValue(neighbor_index); // If the neighbor is uninitialized, get its sign from the occupancy map - const bool neighbor_sdf_uninitialized = + const bool neighbor_uninitialized = neighbor_sdf_parent == sdf.getDefaultCellValue().parent; - if (neighbor_sdf_uninitialized) { + if (neighbor_uninitialized) { const FloatingPoint neighbor_occupancy = occupancy_query_accelerator.getCellValue(neighbor_index); // Never initialize or update unknown cells @@ -182,7 +182,7 @@ void FullEuclideanSDFGenerator::propagate( } // If the neighbor is not yet in the open queue, add it - if (neighbor_sdf_uninitialized) { + if (neighbor_uninitialized) { open_queue.push(neighbor_df_candidate, neighbor_index); } } diff --git a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc index 0e391d1cb..b687a1de8 100644 --- a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc @@ -132,9 +132,9 @@ void QuasiEuclideanSDFGenerator::propagate( FloatingPoint& neighbor_sdf = sdf.getOrAllocateCellValue(neighbor_index); // If the neighbor is uninitialized, get its sign from the occupancy map - const bool neighbor_sdf_uninitialized = + const bool neighbor_uninitialized = sdf.getDefaultCellValue() == neighbor_sdf; - if (neighbor_sdf_uninitialized) { + if (neighbor_uninitialized) { const FloatingPoint neighbor_occupancy = occupancy_query_accelerator.getCellValue(neighbor_index); // Never initialize or update unknown cells @@ -169,7 +169,7 @@ void QuasiEuclideanSDFGenerator::propagate( neighbor_sdf = std::copysign(neighbor_df, neighbor_sdf); // If the neighbor is not yet in the open queue, add it - if (neighbor_sdf_uninitialized) { + if (neighbor_uninitialized) { open_queue.push(neighbor_df_candidate, neighbor_index); } } From 587fa3023711f64c889387f8e8ed8d9665e453b0 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 16 Nov 2023 10:15:49 +0100 Subject: [PATCH 016/159] Clean up and extend ray iterator --- .../utils/iterate/impl/ray_iterator_inl.h | 81 +++++++++++++++++++ .../wavemap/utils/iterate/ray_iterator.h | 54 ++----------- .../ray_tracing/ray_tracing_integrator.cc | 2 +- 3 files changed, 90 insertions(+), 47 deletions(-) create mode 100644 libraries/wavemap/include/wavemap/utils/iterate/impl/ray_iterator_inl.h diff --git a/libraries/wavemap/include/wavemap/utils/iterate/impl/ray_iterator_inl.h b/libraries/wavemap/include/wavemap/utils/iterate/impl/ray_iterator_inl.h new file mode 100644 index 000000000..b4e402c6c --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/iterate/impl/ray_iterator_inl.h @@ -0,0 +1,81 @@ +#ifndef WAVEMAP_UTILS_ITERATE_IMPL_RAY_ITERATOR_INL_H_ +#define WAVEMAP_UTILS_ITERATE_IMPL_RAY_ITERATOR_INL_H_ + +namespace wavemap { +template +Ray::Ray(const Index& start_index, const Index& end_index) + : start_index_(start_index), + end_index_(end_index), + ray_length_in_steps_((end_index_ - start_index_).cwiseAbs().sum() + 1u) { + if (ray_length_in_steps_ == 0u) { + return; + } + + // Compute the direction of steps along the ray + const Index ray_discrete = end_index_ - start_index_; + ray_step_signs_ = ray_discrete.cwiseSign(); + + // Compute distance to cross one grid cell along the ray in t + const Vector ray = ray_discrete.template cast(); + t_step_size_ = + (ray_discrete.array() == 0) + .select(2.f, + ray_step_signs_.template cast().cwiseQuotient( + ray)); + + // Compute the initial value for the traversal in t + const Index step_rectified = ray_step_signs_.cwiseMax(0); + const Vector distance_to_boundaries = + step_rectified.template cast().array() - 0.5f; + t_to_next_boundary_init_ = + (ray_discrete.array() == 0) + .select(2.f, distance_to_boundaries.cwiseQuotient(ray)); +} + +template +Ray::Ray(const Point& start_point, const Point& end_point, + FloatingPoint cell_width) + : start_index_(convert::pointToNearestIndex(start_point, 1.f / cell_width)), + end_index_(convert::pointToNearestIndex(end_point, 1.f / cell_width)), + ray_length_in_steps_((end_index_ - start_index_).cwiseAbs().sum() + 1u) { + CHECK(!start_point.hasNaN()); + CHECK(!end_point.hasNaN()); + if (ray_length_in_steps_ == 0u) { + return; + } + + // Compute the direction of steps along the ray + const Vector ray = end_point - start_point; + ray_step_signs_ = ray.cwiseSign().template cast(); + + // Compute distance to cross one grid cell along the ray in t + t_step_size_ = + (ray.array() == 0.f) + .select(2.f, convert::indexToMinCorner(ray_step_signs_, cell_width) + .cwiseQuotient(ray)); + + // Compute the initial value for the traversal in t + const Index step_rectified = ray_step_signs_.cwiseMax(0); + const Point start_relative = + start_point - convert::indexToMinCorner(start_index_, cell_width); + const Vector distance_to_boundaries = + convert::indexToMinCorner(step_rectified, cell_width) - start_relative; + t_to_next_boundary_init_ = + (ray.array() == 0.f) + .select(2.f, distance_to_boundaries.cwiseQuotient(ray)); +} + +template +typename Ray::Iterator& Ray::Iterator::operator++() { + int t_min_idx; + t_to_next_boundary_.minCoeff(&t_min_idx); + + current_index_[t_min_idx] += ray_.ray_step_signs_[t_min_idx]; + t_to_next_boundary_[t_min_idx] += ray_.t_step_size_[t_min_idx]; + ++current_step_; + + return *this; +} +} // namespace wavemap + +#endif // WAVEMAP_UTILS_ITERATE_IMPL_RAY_ITERATOR_INL_H_ diff --git a/libraries/wavemap/include/wavemap/utils/iterate/ray_iterator.h b/libraries/wavemap/include/wavemap/utils/iterate/ray_iterator.h index f9373cad9..801c4eb44 100644 --- a/libraries/wavemap/include/wavemap/utils/iterate/ray_iterator.h +++ b/libraries/wavemap/include/wavemap/utils/iterate/ray_iterator.h @@ -11,40 +11,9 @@ namespace wavemap { template class Ray { public: + Ray(const Index& start_index, const Index& end_index); Ray(const Point& start_point, const Point& end_point, - FloatingPoint min_cell_width) { - const Point start_point_scaled = start_point / min_cell_width; - const Point end_point_scaled = end_point / min_cell_width; - if (start_point_scaled.hasNaN() || end_point_scaled.hasNaN()) { - ray_length_in_steps_ = 0u; - return; - } - - start_index_ = convert::scaledPointToNearestIndex(start_point_scaled); - end_index_ = convert::scaledPointToNearestIndex(end_point_scaled); - const Index diff_index = end_index_ - start_index_; - ray_length_in_steps_ = diff_index.cwiseAbs().sum() + 1u; - - const Vector ray_scaled = end_point_scaled - start_point_scaled; - ray_step_signs_ = ray_scaled.cwiseSign().template cast(); - - const Index corrected_step = ray_step_signs_.cwiseMax(0); - const Point start_scaled_shifted = - start_point_scaled - start_index_.template cast(); - const Vector distance_to_boundaries = - corrected_step.template cast() - start_scaled_shifted; - - t_to_next_boundary_init_ = - (ray_scaled.array().abs() <= 0.f) - .select(2.f, distance_to_boundaries.array() / ray_scaled.array()); - - // Distance to cross one grid cell along the ray in t - t_step_size_ = - (ray_scaled.array().abs() <= 0.f) - .select(2.f, - ray_step_signs_.template cast().array() / - ray_scaled.array()); - } + FloatingPoint cell_width); class Iterator { public: @@ -60,16 +29,7 @@ class Ray { } Index operator*() const { return current_index_; } - Iterator& operator++() { // prefix ++ - int t_min_idx; - t_to_next_boundary_.minCoeff(&t_min_idx); - - current_index_[t_min_idx] += ray_.ray_step_signs_[t_min_idx]; - t_to_next_boundary_[t_min_idx] += ray_.t_step_size_[t_min_idx]; - ++current_step_; - - return *this; - } + Iterator& operator++(); // prefix ++ Iterator operator++(int) { // postfix ++ Iterator retval = *this; ++(*this); // call the above prefix incrementer @@ -94,9 +54,9 @@ class Ray { Iterator end() const { return Iterator{*this, /*end*/ true}; } private: - Index start_index_; - Index end_index_; - unsigned int ray_length_in_steps_; + const Index start_index_; + const Index end_index_; + const size_t ray_length_in_steps_; Index ray_step_signs_; Vector t_step_size_; @@ -104,4 +64,6 @@ class Ray { }; } // namespace wavemap +#include "wavemap/utils/iterate/impl/ray_iterator_inl.h" + #endif // WAVEMAP_UTILS_ITERATE_RAY_ITERATOR_H_ diff --git a/libraries/wavemap/src/integrator/ray_tracing/ray_tracing_integrator.cc b/libraries/wavemap/src/integrator/ray_tracing/ray_tracing_integrator.cc index 540687a87..492bb1ac3 100644 --- a/libraries/wavemap/src/integrator/ray_tracing/ray_tracing_integrator.cc +++ b/libraries/wavemap/src/integrator/ray_tracing/ray_tracing_integrator.cc @@ -38,7 +38,7 @@ void RayTracingIntegrator::integratePointcloud( (W_start_point - W_end_point).norm(); const Point3D W_end_point_truncated = getEndPointOrMaxRange( W_start_point, W_end_point, measured_distance, config_.max_range); - const Ray ray(W_start_point, W_end_point_truncated, measured_distance); + const Ray ray(W_start_point, W_end_point_truncated, min_cell_width); for (const auto& index : ray) { const FloatingPoint update = measurement_model.computeUpdate(index); occupancy_map_->addToCellValue(index, update); From 70a126fe2174ffa3a15d63e90e3bcf40a34cd48b Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 17 Nov 2023 12:53:39 +0100 Subject: [PATCH 017/159] Handle default arguments for non-float types --- .../wavemap/include/wavemap/data_structure/dense_block_hash.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h b/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h index fdbea9efc..9842d33ff 100644 --- a/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h +++ b/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h @@ -20,7 +20,7 @@ class DenseBlockHash { using BlockHashMap = SpatialHash; explicit DenseBlockHash(FloatingPoint min_cell_width, - CellDataT default_value = 0.f) + CellDataT default_value = CellDataT{}) : min_cell_width_(min_cell_width), default_value_(default_value) {} bool empty() const { return block_map_.empty(); } From 9a67f5953a479fc3cad7fe634cae3917b18a7ee8 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 20 Nov 2023 22:39:56 +0100 Subject: [PATCH 018/159] Draft plugin system --- docs/pages/configuration.rst | 4 +- ros/wavemap_ros/CMakeLists.txt | 18 ++- ros/wavemap_ros/app/rosbag_processor.cc | 22 +-- .../input_handler/input_handler_factory.h | 31 ----- .../depth_image_input.h} | 36 +++-- .../impl/pointcloud_input_impl.h} | 20 +-- .../input_handler.h => inputs/input_base.h} | 65 +++++---- .../wavemap_ros/inputs/input_factory.h | 32 +++++ .../pointcloud_input.h} | 43 +++--- .../impl/publish_map_operation_inl.h} | 25 ++-- .../wavemap_ros/operations/operation_base.h | 28 ++++ .../operations/operation_factory.h | 29 ++++ .../operations/prune_map_operation.h | 51 +++++++ .../operations/publish_map_operation.h | 93 +++++++++++++ .../operations/threshold_map_operation.h | 54 ++++++++ .../wavemap_ros/{ => utils}/logging_level.h | 6 +- .../pointcloud_undistorter.h | 12 +- .../stamped_pointcloud.h} | 30 ++--- .../{ => utils}/rosbag_processor.h | 6 +- .../wavemap_ros/{ => utils}/tf_transformer.h | 6 +- .../include/wavemap_ros/wavemap_server.h | 82 ++++-------- .../input_handler/input_handler_factory.cc | 71 ---------- .../depth_image_input.cc} | 38 +++--- .../input_handler.cc => inputs/input_base.cc} | 39 ++++-- ros/wavemap_ros/src/inputs/input_factory.cc | 68 ++++++++++ .../pointcloud_input.cc} | 44 +++--- .../src/operations/operation_factory.cc | 65 +++++++++ .../src/operations/prune_map_operation.cc | 10 ++ .../src/operations/publish_map_operation.cc | 68 ++++++++++ .../src/operations/threshold_map_operation.cc | 10 ++ .../pointcloud_undistorter.cc | 4 +- .../src/{ => utils}/rosbag_processor.cc | 2 +- .../src/{ => utils}/tf_transformer.cc | 2 +- ros/wavemap_ros/src/wavemap_server.cc | 126 ++++++------------ 34 files changed, 787 insertions(+), 453 deletions(-) delete mode 100644 ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler_factory.h rename ros/wavemap_ros/include/wavemap_ros/{input_handler/depth_image_input_handler.h => inputs/depth_image_input.h} (74%) rename ros/wavemap_ros/include/wavemap_ros/{input_handler/impl/pointcloud_input_handler_impl.h => inputs/impl/pointcloud_input_impl.h} (57%) rename ros/wavemap_ros/include/wavemap_ros/{input_handler/input_handler.h => inputs/input_base.h} (56%) create mode 100644 ros/wavemap_ros/include/wavemap_ros/inputs/input_factory.h rename ros/wavemap_ros/include/wavemap_ros/{input_handler/pointcloud_input_handler.h => inputs/pointcloud_input.h} (74%) rename ros/wavemap_ros/include/wavemap_ros/{impl/wavemap_server_inl.h => operations/impl/publish_map_operation_inl.h} (68%) create mode 100644 ros/wavemap_ros/include/wavemap_ros/operations/operation_base.h create mode 100644 ros/wavemap_ros/include/wavemap_ros/operations/operation_factory.h create mode 100644 ros/wavemap_ros/include/wavemap_ros/operations/prune_map_operation.h create mode 100644 ros/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h create mode 100644 ros/wavemap_ros/include/wavemap_ros/operations/threshold_map_operation.h rename ros/wavemap_ros/include/wavemap_ros/{ => utils}/logging_level.h (82%) rename ros/wavemap_ros/include/wavemap_ros/{input_handler => utils/pointcloud_undistortion}/pointcloud_undistorter.h (69%) rename ros/wavemap_ros/include/wavemap_ros/{input_handler/generic_stamped_pointcloud.h => utils/pointcloud_undistortion/stamped_pointcloud.h} (67%) rename ros/wavemap_ros/include/wavemap_ros/{ => utils}/rosbag_processor.h (92%) rename ros/wavemap_ros/include/wavemap_ros/{ => utils}/tf_transformer.h (94%) delete mode 100644 ros/wavemap_ros/src/input_handler/input_handler_factory.cc rename ros/wavemap_ros/src/{input_handler/depth_image_input_handler.cc => inputs/depth_image_input.cc} (83%) rename ros/wavemap_ros/src/{input_handler/input_handler.cc => inputs/input_base.cc} (73%) create mode 100644 ros/wavemap_ros/src/inputs/input_factory.cc rename ros/wavemap_ros/src/{input_handler/pointcloud_input_handler.cc => inputs/pointcloud_input.cc} (87%) create mode 100644 ros/wavemap_ros/src/operations/operation_factory.cc create mode 100644 ros/wavemap_ros/src/operations/prune_map_operation.cc create mode 100644 ros/wavemap_ros/src/operations/publish_map_operation.cc create mode 100644 ros/wavemap_ros/src/operations/threshold_map_operation.cc rename ros/wavemap_ros/src/{input_handler => utils/pointcloud_undistortion}/pointcloud_undistorter.cc (97%) rename ros/wavemap_ros/src/{ => utils}/rosbag_processor.cc (98%) rename ros/wavemap_ros/src/{ => utils}/tf_transformer.cc (98%) diff --git a/docs/pages/configuration.rst b/docs/pages/configuration.rst index a4892dce1..625fcbfb5 100644 --- a/docs/pages/configuration.rst +++ b/docs/pages/configuration.rst @@ -83,7 +83,7 @@ Selected by setting ``inputs[i]/type: "pointcloud"``. The settings are available under ``inputs[i]/general`` are: -.. doxygenstruct:: wavemap::PointcloudInputHandlerConfig +.. doxygenstruct:: wavemap::PointcloudInputConfig :project: wavemap :members: @@ -93,7 +93,7 @@ Selected by setting ``inputs[i]/type: "depth_image"``. The settings are available under ``inputs[i]/general`` are: -.. doxygenstruct:: wavemap::DepthImageInputHandlerConfig +.. doxygenstruct:: wavemap::DepthImageInputConfig :project: wavemap :members: diff --git a/ros/wavemap_ros/CMakeLists.txt b/ros/wavemap_ros/CMakeLists.txt index 2002ae718..28d2db463 100644 --- a/ros/wavemap_ros/CMakeLists.txt +++ b/ros/wavemap_ros/CMakeLists.txt @@ -19,13 +19,17 @@ add_wavemap_compile_definitions_and_options() # Libraries cs_add_library(${PROJECT_NAME} - src/rosbag_processor.cc - src/tf_transformer.cc - src/input_handler/depth_image_input_handler.cc - src/input_handler/input_handler.cc - src/input_handler/input_handler_factory.cc - src/input_handler/pointcloud_input_handler.cc - src/input_handler/pointcloud_undistorter.cc + src/inputs/depth_image_input.cc + src/inputs/input_base.cc + src/inputs/input_factory.cc + src/inputs/pointcloud_input.cc + src/operations/operation_factory.cc + src/operations/prune_map_operation.cc + src/operations/publish_map_operation.cc + src/operations/threshold_map_operation.cc + src/utils/pointcloud_undistortion/pointcloud_undistorter.cc + src/utils/rosbag_processor.cc + src/utils/tf_transformer.cc src/wavemap_server.cc) # Link OpenCV explicitly to avoid issues on Jetson target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) diff --git a/ros/wavemap_ros/app/rosbag_processor.cc b/ros/wavemap_ros/app/rosbag_processor.cc index 7f1b7b5f8..561ba7e71 100644 --- a/ros/wavemap_ros/app/rosbag_processor.cc +++ b/ros/wavemap_ros/app/rosbag_processor.cc @@ -1,11 +1,11 @@ -#include "wavemap_ros/rosbag_processor.h" +#include "wavemap_ros/utils/rosbag_processor.h" #include #include #include -#include "wavemap_ros/input_handler/depth_image_input_handler.h" -#include "wavemap_ros/input_handler/pointcloud_input_handler.h" +#include "wavemap_ros/inputs/depth_image_input.h" +#include "wavemap_ros/inputs/pointcloud_input.h" #include "wavemap_ros/wavemap_server.h" using namespace wavemap; // NOLINT @@ -39,24 +39,24 @@ int main(int argc, char** argv) { const param::Array integrator_params_array = param::convert::toParamArray(nh_private, "inputs"); for (const auto& integrator_params : integrator_params_array) { - InputHandler* input_handler = + InputBase* input_handler = wavemap_server.addInput(integrator_params, nh, nh_private); if (input_handler) { switch (input_handler->getType().toTypeId()) { - case InputHandlerType::kPointcloud: { + case InputType::kPointcloud: { auto pointcloud_handler = - dynamic_cast(input_handler); - PointcloudInputHandler::registerCallback( + dynamic_cast(input_handler); + PointcloudInput::registerCallback( pointcloud_handler->getTopicType(), [&](auto callback_ptr) { rosbag_processor.addCallback(input_handler->getTopicName(), callback_ptr, pointcloud_handler); }); } continue; - case InputHandlerType::kDepthImage: + case InputType::kDepthImage: rosbag_processor.addCallback( - input_handler->getTopicName(), &DepthImageInputHandler::callback, - dynamic_cast(input_handler)); + input_handler->getTopicName(), &DepthImageInput::callback, + dynamic_cast(input_handler)); continue; } } @@ -79,7 +79,7 @@ int main(int argc, char** argv) { } wavemap_server.getMap()->prune(); - wavemap_server.publishMap(); + wavemap_server.runOperations(/*force_run_all*/ true); if (nh_private.param("keep_alive", false)) { ros::spin(); diff --git a/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler_factory.h b/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler_factory.h deleted file mode 100644 index 9089c1f0c..000000000 --- a/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler_factory.h +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef WAVEMAP_ROS_INPUT_HANDLER_INPUT_HANDLER_FACTORY_H_ -#define WAVEMAP_ROS_INPUT_HANDLER_INPUT_HANDLER_FACTORY_H_ - -#include -#include - -#include "wavemap/utils/thread_pool.h" -#include "wavemap_ros/input_handler/input_handler.h" - -namespace wavemap { -class InputHandlerFactory { - public: - static std::unique_ptr create( - const param::Value& params, std::string world_frame, - VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh, - ros::NodeHandle nh_private, - std::optional default_input_handler_type = - std::nullopt); - - static std::unique_ptr create( - InputHandlerType input_handler_type, const param::Value& params, - std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh, - ros::NodeHandle nh_private); -}; -} // namespace wavemap - -#endif // WAVEMAP_ROS_INPUT_HANDLER_INPUT_HANDLER_FACTORY_H_ diff --git a/ros/wavemap_ros/include/wavemap_ros/input_handler/depth_image_input_handler.h b/ros/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h similarity index 74% rename from ros/wavemap_ros/include/wavemap_ros/input_handler/depth_image_input_handler.h rename to ros/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h index 696e8bdb0..4056a98a1 100644 --- a/ros/wavemap_ros/include/wavemap_ros/input_handler/depth_image_input_handler.h +++ b/ros/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ROS_INPUT_HANDLER_DEPTH_IMAGE_INPUT_HANDLER_H_ -#define WAVEMAP_ROS_INPUT_HANDLER_DEPTH_IMAGE_INPUT_HANDLER_H_ +#ifndef WAVEMAP_ROS_INPUTS_DEPTH_IMAGE_INPUT_H_ +#define WAVEMAP_ROS_INPUTS_DEPTH_IMAGE_INPUT_H_ #include #include @@ -11,14 +11,13 @@ #include #include -#include "wavemap_ros/input_handler/input_handler.h" +#include "wavemap_ros/inputs/input_base.h" namespace wavemap { /** * Config struct for the depth image input handler. */ -struct DepthImageInputHandlerConfig - : public ConfigBase { +struct DepthImageInputConfig : public ConfigBase { //! Name of the ROS topic to subscribe to. std::string topic_name = "scan"; //! Queue length to use when subscribing to the ROS topic. @@ -57,7 +56,7 @@ struct DepthImageInputHandlerConfig static MemberMap memberMap; // Conversion to InputHandler base config - operator InputHandlerConfig() const { // NOLINT + operator InputBaseConfig() const { // NOLINT return {topic_name, topic_queue_length, processing_retry_period, reprojected_pointcloud_topic_name, projected_range_image_topic_name}; @@ -66,18 +65,17 @@ struct DepthImageInputHandlerConfig bool isValid(bool verbose) const override; }; -class DepthImageInputHandler : public InputHandler { +class DepthImageInput : public InputBase { public: - DepthImageInputHandler(const DepthImageInputHandlerConfig& config, - const param::Value& params, std::string world_frame, - VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, - ros::NodeHandle nh, ros::NodeHandle nh_private); - - InputHandlerType getType() const override { - return InputHandlerType::kDepthImage; - } + DepthImageInput(const DepthImageInputConfig& config, + const param::Value& params, std::string world_frame, + VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr transformer, + std::shared_ptr thread_pool, ros::NodeHandle nh, + ros::NodeHandle nh_private, + std::function map_update_callback = {}); + + InputType getType() const override { return InputType::kDepthImage; } void callback(const sensor_msgs::ImageConstPtr& depth_image_msg) { callback(*depth_image_msg); @@ -87,7 +85,7 @@ class DepthImageInputHandler : public InputHandler { } private: - const DepthImageInputHandlerConfig config_; + const DepthImageInputConfig config_; std::vector scanwise_integrators_; image_transport::Subscriber depth_image_sub_; @@ -98,4 +96,4 @@ class DepthImageInputHandler : public InputHandler { }; } // namespace wavemap -#endif // WAVEMAP_ROS_INPUT_HANDLER_DEPTH_IMAGE_INPUT_HANDLER_H_ +#endif // WAVEMAP_ROS_INPUTS_DEPTH_IMAGE_INPUT_H_ diff --git a/ros/wavemap_ros/include/wavemap_ros/input_handler/impl/pointcloud_input_handler_impl.h b/ros/wavemap_ros/include/wavemap_ros/inputs/impl/pointcloud_input_impl.h similarity index 57% rename from ros/wavemap_ros/include/wavemap_ros/input_handler/impl/pointcloud_input_handler_impl.h rename to ros/wavemap_ros/include/wavemap_ros/inputs/impl/pointcloud_input_impl.h index 589475bb9..c0a815a20 100644 --- a/ros/wavemap_ros/include/wavemap_ros/input_handler/impl/pointcloud_input_handler_impl.h +++ b/ros/wavemap_ros/include/wavemap_ros/inputs/impl/pointcloud_input_impl.h @@ -1,23 +1,17 @@ -#ifndef WAVEMAP_ROS_INPUT_HANDLER_IMPL_POINTCLOUD_INPUT_HANDLER_IMPL_H_ -#define WAVEMAP_ROS_INPUT_HANDLER_IMPL_POINTCLOUD_INPUT_HANDLER_IMPL_H_ +#ifndef WAVEMAP_ROS_INPUTS_IMPL_POINTCLOUD_INPUT_IMPL_H_ +#define WAVEMAP_ROS_INPUTS_IMPL_POINTCLOUD_INPUT_IMPL_H_ namespace wavemap { template -bool PointcloudInputHandler::registerCallback(PointcloudTopicType type, - RegistrarT registrar) { +bool PointcloudInput::registerCallback(PointcloudTopicType type, + RegistrarT registrar) { switch (type.toTypeId()) { case PointcloudTopicType::kPointCloud2: - // clang-format off - registrar(static_cast( - &PointcloudInputHandler::callback)); - // clang-format on - return true; case PointcloudTopicType::kOuster: // clang-format off - registrar(static_cast( - &PointcloudInputHandler::callback)); + &PointcloudInput::callback)); // clang-format on return true; case PointcloudTopicType::kLivox: @@ -43,4 +37,4 @@ bool PointcloudInputHandler::registerCallback(PointcloudTopicType type, } } // namespace wavemap -#endif // WAVEMAP_ROS_INPUT_HANDLER_IMPL_POINTCLOUD_INPUT_HANDLER_IMPL_H_ +#endif // WAVEMAP_ROS_INPUTS_IMPL_POINTCLOUD_INPUT_IMPL_H_ diff --git a/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler.h b/ros/wavemap_ros/include/wavemap_ros/inputs/input_base.h similarity index 56% rename from ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler.h rename to ros/wavemap_ros/include/wavemap_ros/inputs/input_base.h index 0519a0a51..b5973189b 100644 --- a/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler.h +++ b/ros/wavemap_ros/include/wavemap_ros/inputs/input_base.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ROS_INPUT_HANDLER_INPUT_HANDLER_H_ -#define WAVEMAP_ROS_INPUT_HANDLER_INPUT_HANDLER_H_ +#ifndef WAVEMAP_ROS_INPUTS_INPUT_BASE_H_ +#define WAVEMAP_ROS_INPUTS_INPUT_BASE_H_ #include #include @@ -15,18 +15,18 @@ #include #include -#include "wavemap_ros/tf_transformer.h" +#include "wavemap_ros/utils/tf_transformer.h" namespace wavemap { -struct InputHandlerType : public TypeSelector { - using TypeSelector::TypeSelector; +struct InputType : public TypeSelector { + using TypeSelector::TypeSelector; enum Id : TypeId { kPointcloud, kDepthImage }; static constexpr std::array names = {"pointcloud", "depth_image"}; }; -struct InputHandlerConfig : public ConfigBase { +struct InputBaseConfig : public ConfigBase { std::string topic_name = "scan"; int topic_queue_length = 10; @@ -38,11 +38,11 @@ struct InputHandlerConfig : public ConfigBase { static MemberMap memberMap; // Constructors - InputHandlerConfig() = default; - InputHandlerConfig(std::string topic_name, int topic_queue_length, - FloatingPoint processing_retry_period, - std::string reprojected_pointcloud_topic_name, - std::string projected_range_image_topic_name) + InputBaseConfig() = default; + InputBaseConfig(std::string topic_name, int topic_queue_length, + FloatingPoint processing_retry_period, + std::string reprojected_pointcloud_topic_name, + std::string projected_range_image_topic_name) : topic_name(std::move(topic_name)), topic_queue_length(topic_queue_length), processing_retry_period(processing_retry_period), @@ -54,49 +54,44 @@ struct InputHandlerConfig : public ConfigBase { bool isValid(bool verbose) const override; }; -class InputHandler { +class InputBase { public: - InputHandler(const InputHandlerConfig& config, const param::Value& params, - std::string world_frame, - VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, - const ros::NodeHandle& nh, ros::NodeHandle nh_private); - virtual ~InputHandler() = default; - - virtual InputHandlerType getType() const = 0; - + InputBase(const InputBaseConfig& config, const param::Value& params, + std::string world_frame, + VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr transformer, + std::shared_ptr thread_pool, const ros::NodeHandle& nh, + ros::NodeHandle nh_private, + std::function map_update_callback = {}); + virtual ~InputBase() = default; + + virtual InputType getType() const = 0; const std::string& getTopicName() { return config_.topic_name; } - bool shouldPublishReprojectedPointcloud() const { - return !config_.reprojected_pointcloud_topic_name.empty() && - 0 < reprojected_pointcloud_pub_.getNumSubscribers(); - } - bool shouldPublishProjectedRangeImage() const { - return !config_.projected_range_image_topic_name.empty() && - 0 < projected_range_image_pub_.getNumSubscribers(); - } - protected: - const InputHandlerConfig config_; + const InputBaseConfig config_; const std::string world_frame_; + std::shared_ptr transformer_; + std::vector integrators_; Stopwatch integration_timer_; - std::shared_ptr transformer_; - virtual void processQueue() = 0; ros::Timer queue_processing_retry_timer_; + std::function map_update_callback_; + + bool shouldPublishReprojectedPointcloud() const; void publishReprojectedPointcloud(const ros::Time& stamp, const PosedPointcloud<>& posed_pointcloud); ros::Publisher reprojected_pointcloud_pub_; + bool shouldPublishProjectedRangeImage() const; void publishProjectedRangeImage(const ros::Time& stamp, const Image<>& range_image); image_transport::Publisher projected_range_image_pub_; }; } // namespace wavemap -#endif // WAVEMAP_ROS_INPUT_HANDLER_INPUT_HANDLER_H_ +#endif // WAVEMAP_ROS_INPUTS_INPUT_BASE_H_ diff --git a/ros/wavemap_ros/include/wavemap_ros/inputs/input_factory.h b/ros/wavemap_ros/include/wavemap_ros/inputs/input_factory.h new file mode 100644 index 000000000..ef75ae94b --- /dev/null +++ b/ros/wavemap_ros/include/wavemap_ros/inputs/input_factory.h @@ -0,0 +1,32 @@ +#ifndef WAVEMAP_ROS_INPUTS_INPUT_FACTORY_H_ +#define WAVEMAP_ROS_INPUTS_INPUT_FACTORY_H_ + +#include +#include + +#include "wavemap/utils/thread_pool.h" +#include "wavemap_ros/inputs/input_base.h" + +namespace wavemap { +class InputFactory { + public: + static std::unique_ptr create( + const param::Value& params, std::string world_frame, + VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr transformer, + std::shared_ptr thread_pool, ros::NodeHandle nh, + ros::NodeHandle nh_private, + std::optional default_input_type = std::nullopt, + std::function map_update_callback = {}); + + static std::unique_ptr create( + InputType input_type, const param::Value& params, std::string world_frame, + VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr transformer, + std::shared_ptr thread_pool, ros::NodeHandle nh, + ros::NodeHandle nh_private, + std::function map_update_callback = {}); +}; +} // namespace wavemap + +#endif // WAVEMAP_ROS_INPUTS_INPUT_FACTORY_H_ diff --git a/ros/wavemap_ros/include/wavemap_ros/input_handler/pointcloud_input_handler.h b/ros/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h similarity index 74% rename from ros/wavemap_ros/include/wavemap_ros/input_handler/pointcloud_input_handler.h rename to ros/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h index a8009b9ce..0296b2111 100644 --- a/ros/wavemap_ros/include/wavemap_ros/input_handler/pointcloud_input_handler.h +++ b/ros/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ROS_INPUT_HANDLER_POINTCLOUD_INPUT_HANDLER_H_ -#define WAVEMAP_ROS_INPUT_HANDLER_POINTCLOUD_INPUT_HANDLER_H_ +#ifndef WAVEMAP_ROS_INPUTS_POINTCLOUD_INPUT_H_ +#define WAVEMAP_ROS_INPUTS_POINTCLOUD_INPUT_H_ #include #include @@ -7,8 +7,8 @@ #include -#include "wavemap_ros/input_handler/input_handler.h" -#include "wavemap_ros/input_handler/pointcloud_undistorter.h" +#include "wavemap_ros/inputs/input_base.h" +#include "wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h" #ifdef LIVOX_AVAILABLE #include @@ -26,8 +26,8 @@ struct PointcloudTopicType : public TypeSelector { /** * Config struct for the pointcloud input handler. */ -struct PointcloudInputHandlerConfig - : public ConfigBase { +struct PointcloudInputConfig + : public ConfigBase { //! Name of the ROS topic to subscribe to. std::string topic_name = "scan"; //! Message type of the ROS topic to subscribe to. @@ -71,7 +71,7 @@ struct PointcloudInputHandlerConfig static MemberMap memberMap; // Conversion to InputHandler base config - operator InputHandlerConfig() const { // NOLINT + operator InputBaseConfig() const { // NOLINT return {topic_name, topic_queue_length, processing_retry_period, reprojected_pointcloud_topic_name, projected_range_image_topic_name}; @@ -80,18 +80,17 @@ struct PointcloudInputHandlerConfig bool isValid(bool verbose) const override; }; -class PointcloudInputHandler : public InputHandler { +class PointcloudInput : public InputBase { public: - PointcloudInputHandler(const PointcloudInputHandlerConfig& config, - const param::Value& params, std::string world_frame, - VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, - ros::NodeHandle nh, ros::NodeHandle nh_private); - - InputHandlerType getType() const override { - return InputHandlerType::kPointcloud; - } + PointcloudInput(const PointcloudInputConfig& config, + const param::Value& params, std::string world_frame, + VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr transformer, + std::shared_ptr thread_pool, ros::NodeHandle nh, + ros::NodeHandle nh_private, + std::function map_update_callback = {}); + + InputType getType() const override { return InputType::kPointcloud; } PointcloudTopicType getTopicType() const { return config_.topic_type; } void callback(const sensor_msgs::PointCloud2& pointcloud_msg); @@ -103,11 +102,11 @@ class PointcloudInputHandler : public InputHandler { static bool registerCallback(PointcloudTopicType type, RegistrarT registrar); private: - const PointcloudInputHandlerConfig config_; + const PointcloudInputConfig config_; PointcloudUndistorter pointcloud_undistorter_; ros::Subscriber pointcloud_sub_; - std::queue pointcloud_queue_; + std::queue pointcloud_queue_; void processQueue() override; static bool hasField(const sensor_msgs::PointCloud2& msg, @@ -115,6 +114,6 @@ class PointcloudInputHandler : public InputHandler { }; } // namespace wavemap -#include "wavemap_ros/input_handler/impl/pointcloud_input_handler_impl.h" +#include "wavemap_ros/inputs/impl/pointcloud_input_impl.h" -#endif // WAVEMAP_ROS_INPUT_HANDLER_POINTCLOUD_INPUT_HANDLER_H_ +#endif // WAVEMAP_ROS_INPUTS_POINTCLOUD_INPUT_H_ diff --git a/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h b/ros/wavemap_ros/include/wavemap_ros/operations/impl/publish_map_operation_inl.h similarity index 68% rename from ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h rename to ros/wavemap_ros/include/wavemap_ros/operations/impl/publish_map_operation_inl.h index 4af65e6d6..626965a12 100644 --- a/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h +++ b/ros/wavemap_ros/include/wavemap_ros/operations/impl/publish_map_operation_inl.h @@ -1,29 +1,28 @@ -#ifndef WAVEMAP_ROS_IMPL_WAVEMAP_SERVER_INL_H_ -#define WAVEMAP_ROS_IMPL_WAVEMAP_SERVER_INL_H_ +#ifndef WAVEMAP_ROS_OPERATIONS_IMPL_PUBLISH_MAP_OPERATION_INL_H_ +#define WAVEMAP_ROS_OPERATIONS_IMPL_PUBLISH_MAP_OPERATION_INL_H_ -#include #include -#include -#include #include +#include #include #include namespace wavemap { template -void WavemapServer::publishHashedMap(HashedMapT* hashed_map, - bool republish_whole_map) { +void PublishMapOperation::publishHashedMap(const ros::Time& current_time, + HashedMapT* hashed_map, + bool republish_whole_map) { // Find the blocks that changed since the last publication time - const Timestamp start_time = Time::now(); + const Timestamp start_time_internal = Time::now(); std::unordered_set changed_blocks; for (const auto& [block_idx, block] : hashed_map->getBlocks()) { if (republish_whole_map || - last_map_pub_time_ < block.getLastUpdatedStamp()) { + last_run_timestamp_internal_ < block.getLastUpdatedStamp()) { changed_blocks.emplace(block_idx); } } - last_map_pub_time_ = start_time; + last_run_timestamp_internal_ = start_time_internal; // Publish the changed blocks, 'max_num_blocks_per_msg' at a time while (!changed_blocks.empty()) { @@ -40,8 +39,8 @@ void WavemapServer::publishHashedMap(HashedMapT* hashed_map, // Serialize and publish the selected blocks wavemap_msgs::Map map_msg; - map_msg.header.frame_id = config_.world_frame; - map_msg.header.stamp = ros::Time::now(); + map_msg.header.frame_id = world_frame_; + map_msg.header.stamp = current_time; convert::mapToRosMsg(*hashed_map, map_msg.hashed_wavelet_octree.emplace_back(), blocks_to_publish, thread_pool_); @@ -58,4 +57,4 @@ void WavemapServer::publishHashedMap(HashedMapT* hashed_map, } } // namespace wavemap -#endif // WAVEMAP_ROS_IMPL_WAVEMAP_SERVER_INL_H_ +#endif // WAVEMAP_ROS_OPERATIONS_IMPL_PUBLISH_MAP_OPERATION_INL_H_ diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/operation_base.h b/ros/wavemap_ros/include/wavemap_ros/operations/operation_base.h new file mode 100644 index 000000000..0709c43a0 --- /dev/null +++ b/ros/wavemap_ros/include/wavemap_ros/operations/operation_base.h @@ -0,0 +1,28 @@ +#ifndef WAVEMAP_ROS_OPERATIONS_OPERATION_BASE_H_ +#define WAVEMAP_ROS_OPERATIONS_OPERATION_BASE_H_ + +#include +#include + +namespace wavemap { +struct OperationType : public TypeSelector { + using TypeSelector::TypeSelector; + + enum Id : TypeId { kThresholdMap, kPruneMap, kPublishMap }; + + static constexpr std::array names = {"threshold_map", "prune_map", + "publish_map"}; +}; + +class OperationBase { + public: + OperationBase() = default; + virtual ~OperationBase() = default; + + virtual OperationType getType() const = 0; + + virtual void run(const ros::Time& current_time, bool force_run) = 0; +}; +} // namespace wavemap + +#endif // WAVEMAP_ROS_OPERATIONS_OPERATION_BASE_H_ diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/operation_factory.h b/ros/wavemap_ros/include/wavemap_ros/operations/operation_factory.h new file mode 100644 index 000000000..ceacbbe59 --- /dev/null +++ b/ros/wavemap_ros/include/wavemap_ros/operations/operation_factory.h @@ -0,0 +1,29 @@ +#ifndef WAVEMAP_ROS_OPERATIONS_OPERATION_FACTORY_H_ +#define WAVEMAP_ROS_OPERATIONS_OPERATION_FACTORY_H_ + +#include +#include + +#include +#include +#include + +#include "wavemap_ros/operations/operation_base.h" + +namespace wavemap { +class OperationFactory { + public: + static std::unique_ptr create( + const param::Value& params, std::string world_frame, + VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr thread_pool, ros::NodeHandle nh_private, + std::optional default_operation_type = std::nullopt); + + static std::unique_ptr create( + OperationType operation_type, const param::Value& params, + std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr thread_pool, ros::NodeHandle nh_private); +}; +} // namespace wavemap + +#endif // WAVEMAP_ROS_OPERATIONS_OPERATION_FACTORY_H_ diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/prune_map_operation.h b/ros/wavemap_ros/include/wavemap_ros/operations/prune_map_operation.h new file mode 100644 index 000000000..a35955af5 --- /dev/null +++ b/ros/wavemap_ros/include/wavemap_ros/operations/prune_map_operation.h @@ -0,0 +1,51 @@ +#ifndef WAVEMAP_ROS_OPERATIONS_PRUNE_MAP_OPERATION_H_ +#define WAVEMAP_ROS_OPERATIONS_PRUNE_MAP_OPERATION_H_ + +#include + +#include +#include + +#include "wavemap_ros/operations/operation_base.h" + +namespace wavemap { +/** + * Config struct for map pruning operations. + */ +struct PruneMapOperationConfig : public ConfigBase { + //! Time period controlling how often the map is pruned. + Seconds once_every = 10.f; + + static MemberMap memberMap; + + bool isValid(bool verbose) const override; +}; + +class PruneMapOperation : public OperationBase { + public: + PruneMapOperation(const PruneMapOperationConfig& config, + VolumetricDataStructureBase::Ptr occupancy_map) + : config_(config.checkValid()), + occupancy_map_(std::move(occupancy_map)) {} + + OperationType getType() const override { return OperationType::kPruneMap; } + + bool shouldRun(const ros::Time& current_time) { + return config_.once_every < (current_time - last_run_timestamp_).toSec(); + } + + void run(const ros::Time& current_time, bool force_run) override { + if (force_run || shouldRun(current_time)) { + occupancy_map_->pruneSmart(); + last_run_timestamp_ = current_time; + } + } + + private: + const PruneMapOperationConfig config_; + const VolumetricDataStructureBase::Ptr occupancy_map_; + ros::Time last_run_timestamp_; +}; +} // namespace wavemap + +#endif // WAVEMAP_ROS_OPERATIONS_PRUNE_MAP_OPERATION_H_ diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h b/ros/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h new file mode 100644 index 000000000..2bb2a6ce5 --- /dev/null +++ b/ros/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h @@ -0,0 +1,93 @@ +#ifndef WAVEMAP_ROS_OPERATIONS_PUBLISH_MAP_OPERATION_H_ +#define WAVEMAP_ROS_OPERATIONS_PUBLISH_MAP_OPERATION_H_ + +#include +#include + +#include +#include +#include +#include +#include + +#include "wavemap_ros/operations/operation_base.h" + +namespace wavemap { +/** + * Config struct for map pruning operations. + */ +struct PublishMapOperationConfig + : public ConfigBase { + //! Time period controlling how often the map is published. + Seconds once_every = 2.f; + + //! Maximum number of blocks to transmit per wavemap map message. + //! Used to control the maximum message size. Only works in combination with + //! hash-based map data structures. + int max_num_blocks_per_msg = 1000; + + //! Name of the topic the map will be published on. + //! Note that the name of the service to request full map retransmissions will + //! be the name of this topic suffixed with "_request_full". + std::string topic = "map"; + + static MemberMap memberMap; + + bool isValid(bool verbose) const override; +}; + +class PublishMapOperation : public OperationBase { + public: + PublishMapOperation(const PublishMapOperationConfig& config, + std::string world_frame, + VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr thread_pool, + ros::NodeHandle nh_private); + + OperationType getType() const override { return OperationType::kPublishMap; } + + bool shouldRun(const ros::Time& current_time) { + return config_.once_every < (current_time - last_run_timestamp_).toSec(); + } + + void run(const ros::Time& current_time, bool force_run) override { + if (force_run || shouldRun(current_time)) { + publishMap(current_time, false); + last_run_timestamp_ = current_time; + } + } + + private: + const PublishMapOperationConfig config_; + const std::string world_frame_; + const VolumetricDataStructureBase::Ptr occupancy_map_; + const std::shared_ptr thread_pool_; + ros::Time last_run_timestamp_; + + // Map publishing + ros::Publisher map_pub_; + Timestamp last_run_timestamp_internal_; + void publishMap(const ros::Time& current_time, bool republish_whole_map); + + // NOTE: For hashed map types, such as HashedWaveletOctree and + // HashedChunkedWaveletOctree, we support incremental map transmissions + // which only include the blocks that changed since the last + // transmission, unless republish_whole_map is set to true. + // In case the number of blocks that changed exceeds + // config_.max_num_blocks_per_msg, the map update is transferred using + // multiple messages. This can be useful when transmitting the maps over + // unreliable networks, where smaller packets can perform better in + // terms of packet loss, or when the change is so large that + // transmitting it as a single message would exceed the maximum ROS + // message size (1GB). + template + void publishHashedMap(const ros::Time& current_time, HashedMapT* hashed_map, + bool republish_whole_map = false); + + ros::ServiceServer republish_whole_map_srv_; +}; +} // namespace wavemap + +#include "wavemap_ros/operations/impl/publish_map_operation_inl.h" + +#endif // WAVEMAP_ROS_OPERATIONS_PUBLISH_MAP_OPERATION_H_ diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/threshold_map_operation.h b/ros/wavemap_ros/include/wavemap_ros/operations/threshold_map_operation.h new file mode 100644 index 000000000..dc9e35ec6 --- /dev/null +++ b/ros/wavemap_ros/include/wavemap_ros/operations/threshold_map_operation.h @@ -0,0 +1,54 @@ +#ifndef WAVEMAP_ROS_OPERATIONS_THRESHOLD_MAP_OPERATION_H_ +#define WAVEMAP_ROS_OPERATIONS_THRESHOLD_MAP_OPERATION_H_ + +#include + +#include +#include + +#include "wavemap_ros/operations/operation_base.h" + +namespace wavemap { +/** + * Config struct for map thresholding operations. + */ +struct ThresholdMapOperationConfig + : public ConfigBase { + //! Time period controlling how often the map is thresholded. + Seconds once_every = 2.f; + + static MemberMap memberMap; + + bool isValid(bool verbose) const override; +}; + +class ThresholdMapOperation : public OperationBase { + public: + ThresholdMapOperation(const ThresholdMapOperationConfig& config, + VolumetricDataStructureBase::Ptr occupancy_map) + : config_(config.checkValid()), + occupancy_map_(std::move(occupancy_map)) {} + + OperationType getType() const override { + return OperationType::kThresholdMap; + } + + bool shouldRun(const ros::Time& current_time) { + return config_.once_every < (current_time - last_run_timestamp_).toSec(); + } + + void run(const ros::Time& current_time, bool force_run) override { + if (force_run || shouldRun(current_time)) { + occupancy_map_->threshold(); + last_run_timestamp_ = current_time; + } + } + + private: + const ThresholdMapOperationConfig config_; + const VolumetricDataStructureBase::Ptr occupancy_map_; + ros::Time last_run_timestamp_; +}; +} // namespace wavemap + +#endif // WAVEMAP_ROS_OPERATIONS_THRESHOLD_MAP_OPERATION_H_ diff --git a/ros/wavemap_ros/include/wavemap_ros/logging_level.h b/ros/wavemap_ros/include/wavemap_ros/utils/logging_level.h similarity index 82% rename from ros/wavemap_ros/include/wavemap_ros/logging_level.h rename to ros/wavemap_ros/include/wavemap_ros/utils/logging_level.h index 6cafd8650..10f6f318f 100644 --- a/ros/wavemap_ros/include/wavemap_ros/logging_level.h +++ b/ros/wavemap_ros/include/wavemap_ros/utils/logging_level.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ROS_LOGGING_LEVEL_H_ -#define WAVEMAP_ROS_LOGGING_LEVEL_H_ +#ifndef WAVEMAP_ROS_UTILS_LOGGING_LEVEL_H_ +#define WAVEMAP_ROS_UTILS_LOGGING_LEVEL_H_ #include #include @@ -19,4 +19,4 @@ struct LoggingLevel : public TypeSelector { }; } // namespace wavemap -#endif // WAVEMAP_ROS_LOGGING_LEVEL_H_ +#endif // WAVEMAP_ROS_UTILS_LOGGING_LEVEL_H_ diff --git a/ros/wavemap_ros/include/wavemap_ros/input_handler/pointcloud_undistorter.h b/ros/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h similarity index 69% rename from ros/wavemap_ros/include/wavemap_ros/input_handler/pointcloud_undistorter.h rename to ros/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h index 15685edc7..e06999b6c 100644 --- a/ros/wavemap_ros/include/wavemap_ros/input_handler/pointcloud_undistorter.h +++ b/ros/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ROS_INPUT_HANDLER_POINTCLOUD_UNDISTORTER_H_ -#define WAVEMAP_ROS_INPUT_HANDLER_POINTCLOUD_UNDISTORTER_H_ +#ifndef WAVEMAP_ROS_UTILS_POINTCLOUD_UNDISTORTION_POINTCLOUD_UNDISTORTER_H_ +#define WAVEMAP_ROS_UTILS_POINTCLOUD_UNDISTORTION_POINTCLOUD_UNDISTORTER_H_ #include #include @@ -10,8 +10,8 @@ #include #include -#include "wavemap_ros/input_handler/generic_stamped_pointcloud.h" -#include "wavemap_ros/tf_transformer.h" +#include "wavemap_ros/utils/pointcloud_undistortion/stamped_pointcloud.h" +#include "wavemap_ros/utils/tf_transformer.h" namespace wavemap { class PointcloudUndistorter { @@ -29,7 +29,7 @@ class PointcloudUndistorter { num_interpolation_intervals_per_cloud_( num_interpolation_intervals_per_cloud) {} - Result undistortPointcloud(GenericStampedPointcloud& stamped_pointcloud, + Result undistortPointcloud(StampedPointcloud& stamped_pointcloud, PosedPointcloud<>& undistorted_pointcloud, const std::string& fixed_frame); @@ -39,4 +39,4 @@ class PointcloudUndistorter { }; } // namespace wavemap -#endif // WAVEMAP_ROS_INPUT_HANDLER_POINTCLOUD_UNDISTORTER_H_ +#endif // WAVEMAP_ROS_UTILS_POINTCLOUD_UNDISTORTION_POINTCLOUD_UNDISTORTER_H_ diff --git a/ros/wavemap_ros/include/wavemap_ros/input_handler/generic_stamped_pointcloud.h b/ros/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/stamped_pointcloud.h similarity index 67% rename from ros/wavemap_ros/include/wavemap_ros/input_handler/generic_stamped_pointcloud.h rename to ros/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/stamped_pointcloud.h index a9290f32d..469f3ce30 100644 --- a/ros/wavemap_ros/include/wavemap_ros/input_handler/generic_stamped_pointcloud.h +++ b/ros/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/stamped_pointcloud.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ROS_INPUT_HANDLER_GENERIC_STAMPED_POINTCLOUD_H_ -#define WAVEMAP_ROS_INPUT_HANDLER_GENERIC_STAMPED_POINTCLOUD_H_ +#ifndef WAVEMAP_ROS_UTILS_POINTCLOUD_UNDISTORTION_STAMPED_POINTCLOUD_H_ +#define WAVEMAP_ROS_UTILS_POINTCLOUD_UNDISTORTION_STAMPED_POINTCLOUD_H_ #include #include @@ -9,12 +9,12 @@ #include namespace wavemap { -struct GenericStampedPoint { +struct StampedPoint { Point3D position; uint32_t time_offset; - GenericStampedPoint(FloatingPoint x, FloatingPoint y, FloatingPoint z, - uint64_t time_offset) + StampedPoint(FloatingPoint x, FloatingPoint y, FloatingPoint z, + uint64_t time_offset) : position(x, y, z), time_offset(time_offset) {} std::string toStr() const { @@ -24,10 +24,10 @@ struct GenericStampedPoint { } }; -class GenericStampedPointcloud { +class StampedPointcloud { public: - GenericStampedPointcloud(uint64_t time_base, std::string sensor_frame, - size_t expected_num_points = 0) + StampedPointcloud(uint64_t time_base, std::string sensor_frame, + size_t expected_num_points = 0) : sensor_frame_(std::move(sensor_frame)), time_base_(time_base) { points_.reserve(expected_num_points); } @@ -61,13 +61,11 @@ class GenericStampedPointcloud { } // Point related getters - std::vector& getPoints() { return points_; } - const std::vector& getPoints() const { return points_; } + std::vector& getPoints() { return points_; } + const std::vector& getPoints() const { return points_; } - GenericStampedPoint& operator[](size_t point_idx) { - return points_[point_idx]; - } - const GenericStampedPoint& operator[](size_t point_idx) const { + StampedPoint& operator[](size_t point_idx) { return points_[point_idx]; } + const StampedPoint& operator[](size_t point_idx) const { return points_[point_idx]; } @@ -85,10 +83,10 @@ class GenericStampedPointcloud { private: std::string sensor_frame_; uint64_t time_base_; - std::vector points_; + std::vector points_; bool is_sorted = false; }; } // namespace wavemap -#endif // WAVEMAP_ROS_INPUT_HANDLER_GENERIC_STAMPED_POINTCLOUD_H_ +#endif // WAVEMAP_ROS_UTILS_POINTCLOUD_UNDISTORTION_STAMPED_POINTCLOUD_H_ diff --git a/ros/wavemap_ros/include/wavemap_ros/rosbag_processor.h b/ros/wavemap_ros/include/wavemap_ros/utils/rosbag_processor.h similarity index 92% rename from ros/wavemap_ros/include/wavemap_ros/rosbag_processor.h rename to ros/wavemap_ros/include/wavemap_ros/utils/rosbag_processor.h index 7e807876b..d412c1ae8 100644 --- a/ros/wavemap_ros/include/wavemap_ros/rosbag_processor.h +++ b/ros/wavemap_ros/include/wavemap_ros/utils/rosbag_processor.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ROS_ROSBAG_PROCESSOR_H_ -#define WAVEMAP_ROS_ROSBAG_PROCESSOR_H_ +#ifndef WAVEMAP_ROS_UTILS_ROSBAG_PROCESSOR_H_ +#define WAVEMAP_ROS_UTILS_ROSBAG_PROCESSOR_H_ #include #include @@ -57,4 +57,4 @@ class RosbagProcessor { #include "wavemap_ros/impl/rosbag_processor_inl.h" -#endif // WAVEMAP_ROS_ROSBAG_PROCESSOR_H_ +#endif // WAVEMAP_ROS_UTILS_ROSBAG_PROCESSOR_H_ diff --git a/ros/wavemap_ros/include/wavemap_ros/tf_transformer.h b/ros/wavemap_ros/include/wavemap_ros/utils/tf_transformer.h similarity index 94% rename from ros/wavemap_ros/include/wavemap_ros/tf_transformer.h rename to ros/wavemap_ros/include/wavemap_ros/utils/tf_transformer.h index 80eb01a60..9ec596f38 100644 --- a/ros/wavemap_ros/include/wavemap_ros/tf_transformer.h +++ b/ros/wavemap_ros/include/wavemap_ros/utils/tf_transformer.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ROS_TF_TRANSFORMER_H_ -#define WAVEMAP_ROS_TF_TRANSFORMER_H_ +#ifndef WAVEMAP_ROS_UTILS_TF_TRANSFORMER_H_ +#define WAVEMAP_ROS_UTILS_TF_TRANSFORMER_H_ #include #include @@ -57,4 +57,4 @@ class TfTransformer { }; } // namespace wavemap -#endif // WAVEMAP_ROS_TF_TRANSFORMER_H_ +#endif // WAVEMAP_ROS_UTILS_TF_TRANSFORMER_H_ diff --git a/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h b/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h index aecb1a948..41801d367 100644 --- a/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h +++ b/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h @@ -5,7 +5,6 @@ #include #include #include -#include #include #include @@ -13,43 +12,27 @@ #include #include #include -#include -#include #include -#include -#include -#include -#include "wavemap_ros/input_handler/input_handler.h" -#include "wavemap_ros/tf_transformer.h" +#include "wavemap_ros/inputs/input_base.h" +#include "wavemap_ros/operations/operation_base.h" +#include "wavemap_ros/utils/logging_level.h" +#include "wavemap_ros/utils/tf_transformer.h" namespace wavemap { /** * Config struct for wavemap's ROS server. */ -struct WavemapServerConfig : ConfigBase { +struct WavemapServerConfig : ConfigBase { //! Name of the coordinate frame in which to store the map. //! Will be used as the frame_id for ROS TF lookups. std::string world_frame = "odom"; - //! Time period controlling how often the map is thresholded. - //! To disable thresholding, set it to a negative number [not recommended]. - Seconds thresholding_period = 1.f; - //! Time period controlling how often the map is pruned. - //! To disable pruning, set it to a negative number. - Seconds pruning_period = 10.f; - //! Time period controlling how often the map is published. - //! To disable map publishing, set it to a negative number. - Seconds publication_period = 10.f; - //! Maximum number of blocks to transmit per wavemap map message. - //! Used to control the maximum message size. Only works in combination with - //! hash-based map data structures. - int max_num_blocks_per_msg = 1000; + //! Minimum severity level for ROS logging messages to be logged. + LoggingLevel logging_level = LoggingLevel::kInfo; //! Maximum number of threads to use. //! Defaults to the number of threads supported by the CPU. int num_threads = std::max(1, static_cast(std::thread::hardware_concurrency())); - //! Minimum severity level for ROS logging messages to be logged. - LoggingLevel logging_level = LoggingLevel::kInfo; //! Whether or not to allow resetting the map through the reset_map service. bool allow_reset_map_service = false; @@ -64,62 +47,43 @@ class WavemapServer { WavemapServer(ros::NodeHandle nh, ros::NodeHandle nh_private, const WavemapServerConfig& config); - void publishMap(bool republish_whole_map = false); - bool saveMap(const std::filesystem::path& file_path) const; - bool loadMap(const std::filesystem::path& file_path); + InputBase* addInput(const param::Value& integrator_params, + const ros::NodeHandle& nh, ros::NodeHandle nh_private); + OperationBase* addOperation(const param::Value& operation_params, + ros::NodeHandle nh_private); - InputHandler* addInput(const param::Value& integrator_params, - const ros::NodeHandle& nh, ros::NodeHandle nh_private); + void runOperations(bool force_run_all = false); VolumetricDataStructureBase::Ptr getMap() { return occupancy_map_; } VolumetricDataStructureBase::ConstPtr getMap() const { return occupancy_map_; } + bool saveMap(const std::filesystem::path& file_path) const; + bool loadMap(const std::filesystem::path& file_path); + private: const WavemapServerConfig config_; + // Map data structure VolumetricDataStructureBase::Ptr occupancy_map_; - std::shared_ptr transformer_; + // Threadpool shared among all input handlers and operations std::shared_ptr thread_pool_; - std::vector> input_handlers_; - - void subscribeToTimers(const ros::NodeHandle& nh); - ros::Timer map_pruning_timer_; - ros::Timer map_thresholding_timer_; - ros::Timer map_publication_timer_; - void subscribeToTopics(ros::NodeHandle& nh); + // Transform and depth inputs + std::shared_ptr transformer_; + std::vector> input_handlers_; - void advertiseTopics(ros::NodeHandle& nh_private); - ros::Publisher map_pub_; + // Operations to perform after map updates + std::vector> operations_; + // ROS services void advertiseServices(ros::NodeHandle& nh_private); - ros::ServiceServer republish_whole_map_srv_; ros::ServiceServer reset_map_srv_; ros::ServiceServer save_map_srv_; ros::ServiceServer load_map_srv_; - - // Map block publishing - // NOTE: For hashed map types, such as HashedWaveletOctree and - // HashedChunkedWaveletOctree, we support incremental map transmissions - // which only include the blocks that changed since the last - // transmission, unless republish_whole_map is set to true. - // In case the number of blocks that changed exceeds - // config_.max_num_blocks_per_msg, the map update is transferred using - // multiple messages. This can be useful when transmitting the maps over - // unreliable networks, where smaller packets can perform better in - // terms of packet loss, or when the change is so large that - // transmitting it as a single message would exceed the maximum ROS - // message size (1GB). - template - void publishHashedMap(HashedMapT* hashed_map, - bool republish_whole_map = false); - Timestamp last_map_pub_time_; }; } // namespace wavemap -#include "wavemap_ros/impl/wavemap_server_inl.h" - #endif // WAVEMAP_ROS_WAVEMAP_SERVER_H_ diff --git a/ros/wavemap_ros/src/input_handler/input_handler_factory.cc b/ros/wavemap_ros/src/input_handler/input_handler_factory.cc deleted file mode 100644 index 934167607..000000000 --- a/ros/wavemap_ros/src/input_handler/input_handler_factory.cc +++ /dev/null @@ -1,71 +0,0 @@ -#include "wavemap_ros/input_handler/input_handler_factory.h" - -#include "wavemap_ros/input_handler/depth_image_input_handler.h" -#include "wavemap_ros/input_handler/pointcloud_input_handler.h" - -namespace wavemap { -std::unique_ptr InputHandlerFactory::create( - const param::Value& params, std::string world_frame, - VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh, - ros::NodeHandle nh_private, - std::optional default_input_handler_type) { - if (const auto type = InputHandlerType::from(params); type) { - return create(type.value(), params, world_frame, occupancy_map, - std::move(transformer), std::move(thread_pool), nh, - nh_private); - } - - if (default_input_handler_type.has_value()) { - ROS_WARN_STREAM("Default type \"" - << default_input_handler_type.value().toStr() - << "\" will be created instead."); - return create(default_input_handler_type.value(), params, - std::move(world_frame), std::move(occupancy_map), - std::move(transformer), std::move(thread_pool), nh, - nh_private); - } - - ROS_ERROR("No default was set. Returning nullptr."); - return nullptr; -} - -std::unique_ptr InputHandlerFactory::create( - InputHandlerType input_handler_type, const param::Value& params, - std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh, - ros::NodeHandle nh_private) { - // Create the input handler - switch (input_handler_type.toTypeId()) { - case InputHandlerType::kPointcloud: { - if (const auto config = - PointcloudInputHandlerConfig::from(params, "general"); - config) { - return std::make_unique( - config.value(), params, std::move(world_frame), - std::move(occupancy_map), std::move(transformer), - std::move(thread_pool), nh, nh_private); - } else { - ROS_ERROR("Pointcloud input handler config could not be loaded."); - return nullptr; - } - } - case InputHandlerType::kDepthImage: { - if (const auto config = - DepthImageInputHandlerConfig::from(params, "general"); - config) { - return std::make_unique( - config.value(), params, std::move(world_frame), - std::move(occupancy_map), std::move(transformer), - std::move(thread_pool), nh, nh_private); - } else { - ROS_ERROR("Depth image input handler config could not be loaded."); - return nullptr; - } - } - } - return nullptr; -} -} // namespace wavemap diff --git a/ros/wavemap_ros/src/input_handler/depth_image_input_handler.cc b/ros/wavemap_ros/src/inputs/depth_image_input.cc similarity index 83% rename from ros/wavemap_ros/src/input_handler/depth_image_input_handler.cc rename to ros/wavemap_ros/src/inputs/depth_image_input.cc index cd88e33dc..7c3644589 100644 --- a/ros/wavemap_ros/src/input_handler/depth_image_input_handler.cc +++ b/ros/wavemap_ros/src/inputs/depth_image_input.cc @@ -1,4 +1,4 @@ -#include "wavemap_ros/input_handler/depth_image_input_handler.h" +#include "wavemap_ros/inputs/depth_image_input.h" #include #include @@ -7,7 +7,7 @@ #include namespace wavemap { -DECLARE_CONFIG_MEMBERS(DepthImageInputHandlerConfig, +DECLARE_CONFIG_MEMBERS(DepthImageInputConfig, (topic_name) (topic_queue_length) (processing_retry_period) @@ -19,7 +19,7 @@ DECLARE_CONFIG_MEMBERS(DepthImageInputHandlerConfig, (reprojected_pointcloud_topic_name) (projected_range_image_topic_name)); -bool DepthImageInputHandlerConfig::isValid(bool verbose) const { +bool DepthImageInputConfig::isValid(bool verbose) const { bool all_valid = true; all_valid &= IS_PARAM_NE(topic_name, std::string(""), verbose); @@ -30,15 +30,18 @@ bool DepthImageInputHandlerConfig::isValid(bool verbose) const { return all_valid; } -DepthImageInputHandler::DepthImageInputHandler( - const DepthImageInputHandlerConfig& config, const param::Value& params, - std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh, - ros::NodeHandle nh_private) - : InputHandler(config, params, std::move(world_frame), - std::move(occupancy_map), std::move(transformer), - std::move(thread_pool), nh, nh_private), +DepthImageInput::DepthImageInput(const DepthImageInputConfig& config, + const param::Value& params, + std::string world_frame, + VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr transformer, + std::shared_ptr thread_pool, + ros::NodeHandle nh, ros::NodeHandle nh_private, + std::function map_update_callback) + : InputBase(config, params, std::move(world_frame), + std::move(occupancy_map), std::move(transformer), + std::move(thread_pool), nh, nh_private, + std::move(map_update_callback)), config_(config.checkValid()) { // Get pointers to the underlying scanwise integrators for (const auto& integrator : integrators_) { @@ -54,11 +57,11 @@ DepthImageInputHandler::DepthImageInputHandler( image_transport::ImageTransport it(nh); depth_image_sub_ = it.subscribe( config_.topic_name, config_.topic_queue_length, - &DepthImageInputHandler::callback, this, + &DepthImageInput::callback, this, image_transport::TransportHints(config_.image_transport_hints)); } -void DepthImageInputHandler::processQueue() { +void DepthImageInput::processQueue() { ZoneScoped; while (!depth_image_queue_.empty()) { const sensor_msgs::Image& oldest_msg = depth_image_queue_.front(); @@ -118,6 +121,11 @@ void DepthImageInputHandler::processQueue() { << "s. Total integration time: " << integration_timer_.getTotalDuration() << "s."); + // Notify subscribers that the map was updated + if (map_update_callback_) { + std::invoke(map_update_callback_); + } + // Publish debugging visualizations if (shouldPublishReprojectedPointcloud()) { const auto posed_pointcloud = reproject(posed_range_image); @@ -140,7 +148,7 @@ void DepthImageInputHandler::processQueue() { } } -PosedPointcloud<> DepthImageInputHandler::reproject( +PosedPointcloud<> DepthImageInput::reproject( const PosedImage<>& posed_range_image) { ZoneScoped; auto projective_integrator = diff --git a/ros/wavemap_ros/src/input_handler/input_handler.cc b/ros/wavemap_ros/src/inputs/input_base.cc similarity index 73% rename from ros/wavemap_ros/src/input_handler/input_handler.cc rename to ros/wavemap_ros/src/inputs/input_base.cc index 53f1f9d8b..746289649 100644 --- a/ros/wavemap_ros/src/input_handler/input_handler.cc +++ b/ros/wavemap_ros/src/inputs/input_base.cc @@ -1,4 +1,4 @@ -#include "wavemap_ros/input_handler/input_handler.h" +#include "wavemap_ros/inputs/input_base.h" #include #include @@ -10,14 +10,14 @@ #include namespace wavemap { -DECLARE_CONFIG_MEMBERS(InputHandlerConfig, +DECLARE_CONFIG_MEMBERS(InputBaseConfig, (topic_name) (topic_queue_length) (processing_retry_period) (reprojected_pointcloud_topic_name) (projected_range_image_topic_name)); -bool InputHandlerConfig::isValid(bool verbose) const { +bool InputBaseConfig::isValid(bool verbose) const { bool all_valid = true; all_valid &= IS_PARAM_NE(topic_name, std::string(""), verbose); @@ -27,16 +27,17 @@ bool InputHandlerConfig::isValid(bool verbose) const { return all_valid; } -InputHandler::InputHandler(const InputHandlerConfig& config, - const param::Value& params, std::string world_frame, - VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, - const ros::NodeHandle& nh, - ros::NodeHandle nh_private) +InputBase::InputBase(const InputBaseConfig& config, const param::Value& params, + std::string world_frame, + VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr transformer, + std::shared_ptr thread_pool, + const ros::NodeHandle& nh, ros::NodeHandle nh_private, + std::function map_update_callback) : config_(config.checkValid()), world_frame_(std::move(world_frame)), - transformer_(std::move(transformer)) { + transformer_(std::move(transformer)), + map_update_callback_(std::move(map_update_callback)) { // Create the integrators const auto integrators_param = params.getChild("integrators"); if (!integrators_param) { @@ -79,7 +80,12 @@ InputHandler::InputHandler(const InputHandlerConfig& config, } } -void InputHandler::publishReprojectedPointcloud( +bool InputBase::shouldPublishReprojectedPointcloud() const { + return !config_.reprojected_pointcloud_topic_name.empty() && + 0 < reprojected_pointcloud_pub_.getNumSubscribers(); +} + +void InputBase::publishReprojectedPointcloud( const ros::Time& stamp, const PosedPointcloud<>& posed_pointcloud) { ZoneScoped; sensor_msgs::PointCloud pointcloud_msg; @@ -99,8 +105,13 @@ void InputHandler::publishReprojectedPointcloud( reprojected_pointcloud_pub_.publish(pointcloud2_msg); } -void InputHandler::publishProjectedRangeImage(const ros::Time& stamp, - const Image<>& range_image) { +bool InputBase::shouldPublishProjectedRangeImage() const { + return !config_.projected_range_image_topic_name.empty() && + 0 < projected_range_image_pub_.getNumSubscribers(); +} + +void InputBase::publishProjectedRangeImage(const ros::Time& stamp, + const Image<>& range_image) { ZoneScoped; cv_bridge::CvImage cv_image; cv_image.header.stamp = stamp; diff --git a/ros/wavemap_ros/src/inputs/input_factory.cc b/ros/wavemap_ros/src/inputs/input_factory.cc new file mode 100644 index 000000000..723c0f108 --- /dev/null +++ b/ros/wavemap_ros/src/inputs/input_factory.cc @@ -0,0 +1,68 @@ +#include "wavemap_ros/inputs/input_factory.h" + +#include "wavemap_ros/inputs/depth_image_input.h" +#include "wavemap_ros/inputs/pointcloud_input.h" + +namespace wavemap { +std::unique_ptr InputFactory::create( + const param::Value& params, std::string world_frame, + VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr transformer, + std::shared_ptr thread_pool, ros::NodeHandle nh, + ros::NodeHandle nh_private, std::optional default_input_type, + std::function map_update_callback) { + if (const auto type = InputType::from(params); type) { + return create(type.value(), params, world_frame, occupancy_map, + std::move(transformer), std::move(thread_pool), nh, + nh_private, std::move(map_update_callback)); + } + + if (default_input_type.has_value()) { + ROS_WARN_STREAM("Default type \"" << default_input_type.value().toStr() + << "\" will be created instead."); + return create(default_input_type.value(), params, std::move(world_frame), + std::move(occupancy_map), std::move(transformer), + std::move(thread_pool), nh, nh_private, + std::move(map_update_callback)); + } + + ROS_ERROR("No default was set. Returning nullptr."); + return nullptr; +} + +std::unique_ptr InputFactory::create( + InputType input_type, const param::Value& params, std::string world_frame, + VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr transformer, + std::shared_ptr thread_pool, ros::NodeHandle nh, + ros::NodeHandle nh_private, std::function map_update_callback) { + // Create the input handler + switch (input_type.toTypeId()) { + case InputType::kPointcloud: + if (const auto config = PointcloudInputConfig::from(params, "general"); + config) { + return std::make_unique( + config.value(), params, std::move(world_frame), + std::move(occupancy_map), std::move(transformer), + std::move(thread_pool), nh, nh_private, + std::move(map_update_callback)); + } else { + ROS_ERROR("Pointcloud input handler config could not be loaded."); + return nullptr; + } + case InputType::kDepthImage: + if (const auto config = DepthImageInputConfig::from(params, "general"); + config) { + return std::make_unique( + config.value(), params, std::move(world_frame), + std::move(occupancy_map), std::move(transformer), + std::move(thread_pool), nh, nh_private, + std::move(map_update_callback)); + } else { + ROS_ERROR("Depth image input handler config could not be loaded."); + return nullptr; + } + } + return nullptr; +} +} // namespace wavemap diff --git a/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc b/ros/wavemap_ros/src/inputs/pointcloud_input.cc similarity index 87% rename from ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc rename to ros/wavemap_ros/src/inputs/pointcloud_input.cc index def282c92..13f4f8f52 100644 --- a/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc +++ b/ros/wavemap_ros/src/inputs/pointcloud_input.cc @@ -1,4 +1,4 @@ -#include "wavemap_ros/input_handler/pointcloud_input_handler.h" +#include "wavemap_ros/inputs/pointcloud_input.h" #include #include @@ -6,7 +6,7 @@ #include namespace wavemap { -DECLARE_CONFIG_MEMBERS(PointcloudInputHandlerConfig, +DECLARE_CONFIG_MEMBERS(PointcloudInputConfig, (topic_name) (topic_type) (topic_queue_length) @@ -19,7 +19,7 @@ DECLARE_CONFIG_MEMBERS(PointcloudInputHandlerConfig, (reprojected_pointcloud_topic_name) (projected_range_image_topic_name)); -bool PointcloudInputHandlerConfig::isValid(bool verbose) const { +bool PointcloudInputConfig::isValid(bool verbose) const { bool all_valid = true; all_valid &= IS_PARAM_NE(topic_name, std::string(""), verbose); @@ -30,15 +30,17 @@ bool PointcloudInputHandlerConfig::isValid(bool verbose) const { return all_valid; } -PointcloudInputHandler::PointcloudInputHandler( - const PointcloudInputHandlerConfig& config, const param::Value& params, - std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh, - ros::NodeHandle nh_private) - : InputHandler(config, params, std::move(world_frame), - std::move(occupancy_map), transformer, - std::move(thread_pool), nh, nh_private), +PointcloudInput::PointcloudInput(const PointcloudInputConfig& config, + const param::Value& params, + std::string world_frame, + VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr transformer, + std::shared_ptr thread_pool, + ros::NodeHandle nh, ros::NodeHandle nh_private, + std::function map_update_callback) + : InputBase(config, params, std::move(world_frame), + std::move(occupancy_map), transformer, std::move(thread_pool), + nh, nh_private, std::move(map_update_callback)), config_(config.checkValid()), pointcloud_undistorter_( transformer, @@ -50,8 +52,7 @@ PointcloudInputHandler::PointcloudInputHandler( }); } -void PointcloudInputHandler::callback( - const sensor_msgs::PointCloud2& pointcloud_msg) { +void PointcloudInput::callback(const sensor_msgs::PointCloud2& pointcloud_msg) { ZoneScoped; // Skip empty clouds const size_t num_points = pointcloud_msg.height * pointcloud_msg.width; @@ -82,8 +83,8 @@ void PointcloudInputHandler::callback( std::string sensor_frame_id = config_.sensor_frame_id.empty() ? pointcloud_msg.header.frame_id : config_.sensor_frame_id; - GenericStampedPointcloud stamped_pointcloud{ - stamp_nsec, std::move(sensor_frame_id), num_points}; + StampedPointcloud stamped_pointcloud{stamp_nsec, std::move(sensor_frame_id), + num_points}; // Load the points with time information if undistortion is enabled bool loaded = false; @@ -154,7 +155,7 @@ void PointcloudInputHandler::callback( } #endif -void PointcloudInputHandler::processQueue() { +void PointcloudInput::processQueue() { ZoneScoped; while (!pointcloud_queue_.empty()) { auto& oldest_msg = pointcloud_queue_.front(); @@ -243,6 +244,11 @@ void PointcloudInputHandler::processQueue() { << "s. Total integration time: " << integration_timer_.getTotalDuration() << "s."); + // Notify subscribers that the map was updated + if (map_update_callback_) { + std::invoke(map_update_callback_); + } + // Publish debugging visualizations if (shouldPublishReprojectedPointcloud()) { publishReprojectedPointcloud( @@ -268,8 +274,8 @@ void PointcloudInputHandler::processQueue() { } } -bool PointcloudInputHandler::hasField(const sensor_msgs::PointCloud2& msg, - const std::string& field_name) { +bool PointcloudInput::hasField(const sensor_msgs::PointCloud2& msg, + const std::string& field_name) { return std::any_of(msg.fields.cbegin(), msg.fields.cend(), [&](const sensor_msgs::PointField& field) { return field.name == field_name; diff --git a/ros/wavemap_ros/src/operations/operation_factory.cc b/ros/wavemap_ros/src/operations/operation_factory.cc new file mode 100644 index 000000000..26ce62f04 --- /dev/null +++ b/ros/wavemap_ros/src/operations/operation_factory.cc @@ -0,0 +1,65 @@ +#include "wavemap_ros/operations/operation_factory.h" + +#include "wavemap_ros/operations/prune_map_operation.h" +#include "wavemap_ros/operations/publish_map_operation.h" +#include "wavemap_ros/operations/threshold_map_operation.h" + +namespace wavemap { +std::unique_ptr OperationFactory::create( + const param::Value& params, std::string world_frame, + VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr thread_pool, ros::NodeHandle nh_private, + std::optional default_operation_type) { + if (const auto type = OperationType::from(params); type) { + return create(type.value(), params, std::move(world_frame), + std::move(occupancy_map), std::move(thread_pool), nh_private); + } + + if (default_operation_type.has_value()) { + ROS_WARN_STREAM("Default type \"" << default_operation_type.value().toStr() + << "\" will be created instead."); + return create(default_operation_type.value(), params, + std::move(world_frame), std::move(occupancy_map), + std::move(thread_pool), nh_private); + } + + ROS_ERROR("No default was set. Returning nullptr."); + return nullptr; +} + +std::unique_ptr OperationFactory::create( + OperationType operation_type, const param::Value& params, + std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr thread_pool, ros::NodeHandle nh_private) { + // Create the operation handler + switch (operation_type.toTypeId()) { + case OperationType::kThresholdMap: + if (const auto config = ThresholdMapOperationConfig::from(params); + config) { + return std::make_unique( + config.value(), std::move(occupancy_map)); + } else { + ROS_ERROR("Threshold map operation config could not be loaded."); + return nullptr; + } + case OperationType::kPruneMap: + if (const auto config = PruneMapOperationConfig::from(params); config) { + return std::make_unique(config.value(), + std::move(occupancy_map)); + } else { + ROS_ERROR("Prune map operation config could not be loaded."); + return nullptr; + } + case OperationType::kPublishMap: + if (const auto config = PublishMapOperationConfig::from(params); config) { + return std::make_unique( + config.value(), std::move(world_frame), std::move(occupancy_map), + std::move(thread_pool), nh_private); + } else { + ROS_ERROR("Publish map operation config could not be loaded."); + return nullptr; + } + } + return nullptr; +} +} // namespace wavemap diff --git a/ros/wavemap_ros/src/operations/prune_map_operation.cc b/ros/wavemap_ros/src/operations/prune_map_operation.cc new file mode 100644 index 000000000..57e8156b6 --- /dev/null +++ b/ros/wavemap_ros/src/operations/prune_map_operation.cc @@ -0,0 +1,10 @@ +#include "wavemap_ros/operations/prune_map_operation.h" + +namespace wavemap { +DECLARE_CONFIG_MEMBERS(PruneMapOperationConfig, + (once_every)); + +bool PruneMapOperationConfig::isValid(bool verbose) const { + return IS_PARAM_GT(once_every, 0.f, verbose); +} +} // namespace wavemap diff --git a/ros/wavemap_ros/src/operations/publish_map_operation.cc b/ros/wavemap_ros/src/operations/publish_map_operation.cc new file mode 100644 index 000000000..91fd97c9b --- /dev/null +++ b/ros/wavemap_ros/src/operations/publish_map_operation.cc @@ -0,0 +1,68 @@ +#include "wavemap_ros/operations/publish_map_operation.h" + +#include +#include +#include +#include + +namespace wavemap { +DECLARE_CONFIG_MEMBERS(PublishMapOperationConfig, + (once_every) + (max_num_blocks_per_msg) + (topic)); + +bool PublishMapOperationConfig::isValid(bool verbose) const { + bool all_valid = true; + + all_valid &= IS_PARAM_GT(once_every, 0.f, verbose); + all_valid &= IS_PARAM_GT(max_num_blocks_per_msg, 0, verbose); + all_valid &= IS_PARAM_NE(topic, std::string(), verbose); + + return all_valid; +} + +PublishMapOperation::PublishMapOperation( + const PublishMapOperationConfig& config, std::string world_frame, + VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr thread_pool, ros::NodeHandle nh_private) + : config_(config.checkValid()), + world_frame_(std::move(world_frame)), + occupancy_map_(std::move(occupancy_map)), + thread_pool_(std::move(thread_pool)) { + map_pub_ = nh_private.advertise(config_.topic, 10); + republish_whole_map_srv_ = + nh_private.advertiseService( + config_.topic + "_request_full", + [this](auto& /*request*/, auto& /*response*/) { + publishMap(ros::Time::now(), true); + return true; + }); +} + +void PublishMapOperation::publishMap(const ros::Time& current_time, + bool republish_whole_map) { + ZoneScoped; + if (!occupancy_map_->empty()) { + if (auto* hashed_wavelet_octree = + dynamic_cast(occupancy_map_.get()); + hashed_wavelet_octree) { + publishHashedMap(current_time, hashed_wavelet_octree, + republish_whole_map); + } else if (auto* hashed_chunked_wavelet_octree = + dynamic_cast( + occupancy_map_.get()); + hashed_chunked_wavelet_octree) { + publishHashedMap(current_time, hashed_chunked_wavelet_octree, + republish_whole_map); + } else { + occupancy_map_->threshold(); + wavemap_msgs::Map map_msg; + if (convert::mapToRosMsg(*occupancy_map_, world_frame_, current_time, + map_msg)) { + map_pub_.publish(map_msg); + } + } + } +} +} // namespace wavemap diff --git a/ros/wavemap_ros/src/operations/threshold_map_operation.cc b/ros/wavemap_ros/src/operations/threshold_map_operation.cc new file mode 100644 index 000000000..6bdb9aef0 --- /dev/null +++ b/ros/wavemap_ros/src/operations/threshold_map_operation.cc @@ -0,0 +1,10 @@ +#include "wavemap_ros/operations/threshold_map_operation.h" + +namespace wavemap { +DECLARE_CONFIG_MEMBERS(ThresholdMapOperationConfig, + (once_every)); + +bool ThresholdMapOperationConfig::isValid(bool verbose) const { + return IS_PARAM_GT(once_every, 0.f, verbose); +} +} // namespace wavemap diff --git a/ros/wavemap_ros/src/input_handler/pointcloud_undistorter.cc b/ros/wavemap_ros/src/utils/pointcloud_undistortion/pointcloud_undistorter.cc similarity index 97% rename from ros/wavemap_ros/src/input_handler/pointcloud_undistorter.cc rename to ros/wavemap_ros/src/utils/pointcloud_undistortion/pointcloud_undistorter.cc index 8e1557985..979f6b899 100644 --- a/ros/wavemap_ros/src/input_handler/pointcloud_undistorter.cc +++ b/ros/wavemap_ros/src/utils/pointcloud_undistortion/pointcloud_undistorter.cc @@ -1,11 +1,11 @@ -#include "wavemap_ros/input_handler/pointcloud_undistorter.h" +#include "wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h" #include #include namespace wavemap { PointcloudUndistorter::Result PointcloudUndistorter::undistortPointcloud( - GenericStampedPointcloud& stamped_pointcloud, + StampedPointcloud& stamped_pointcloud, PosedPointcloud<>& undistorted_pointcloud, const std::string& fixed_frame) { ZoneScoped; // Get the time interval diff --git a/ros/wavemap_ros/src/rosbag_processor.cc b/ros/wavemap_ros/src/utils/rosbag_processor.cc similarity index 98% rename from ros/wavemap_ros/src/rosbag_processor.cc rename to ros/wavemap_ros/src/utils/rosbag_processor.cc index 35b8a7f4c..83e495bac 100644 --- a/ros/wavemap_ros/src/rosbag_processor.cc +++ b/ros/wavemap_ros/src/utils/rosbag_processor.cc @@ -1,4 +1,4 @@ -#include "wavemap_ros/rosbag_processor.h" +#include "wavemap_ros/utils/rosbag_processor.h" #include diff --git a/ros/wavemap_ros/src/tf_transformer.cc b/ros/wavemap_ros/src/utils/tf_transformer.cc similarity index 98% rename from ros/wavemap_ros/src/tf_transformer.cc rename to ros/wavemap_ros/src/utils/tf_transformer.cc index ba7d7e13d..af21e789e 100644 --- a/ros/wavemap_ros/src/tf_transformer.cc +++ b/ros/wavemap_ros/src/utils/tf_transformer.cc @@ -1,4 +1,4 @@ -#include "wavemap_ros/tf_transformer.h" +#include "wavemap_ros/utils/tf_transformer.h" #include diff --git a/ros/wavemap_ros/src/wavemap_server.cc b/ros/wavemap_ros/src/wavemap_server.cc index a353f59c8..31e685163 100644 --- a/ros/wavemap_ros/src/wavemap_server.cc +++ b/ros/wavemap_ros/src/wavemap_server.cc @@ -1,24 +1,17 @@ #include "wavemap_ros/wavemap_server.h" -#include #include -#include #include #include #include -#include #include -#include -#include "wavemap_ros/input_handler/input_handler_factory.h" +#include "wavemap_ros/inputs/input_factory.h" +#include "wavemap_ros/operations/operation_factory.h" namespace wavemap { DECLARE_CONFIG_MEMBERS(WavemapServerConfig, (world_frame) - (thresholding_period) - (pruning_period) - (publication_period) - (max_num_blocks_per_msg) (num_threads) (logging_level) (allow_reset_map_service)); @@ -27,7 +20,6 @@ bool WavemapServerConfig::isValid(bool verbose) const { bool all_valid = true; all_valid &= IS_PARAM_NE(world_frame, std::string(""), verbose); - all_valid &= IS_PARAM_GT(max_num_blocks_per_msg, 0, verbose); all_valid &= IS_PARAM_GT(num_threads, 0, verbose); return all_valid; @@ -38,7 +30,7 @@ bool WavemapServerConfig::isValid(bool verbose) const { WavemapServer::WavemapServer(ros::NodeHandle nh, ros::NodeHandle nh_private) : WavemapServer(nh, nh_private, WavemapServerConfig::from( - param::convert::toParamValue(nh_private, "map/general")) + param::convert::toParamValue(nh_private, "general")) .value()) {} WavemapServer::WavemapServer(ros::NodeHandle nh, ros::NodeHandle nh_private, @@ -54,7 +46,7 @@ WavemapServer::WavemapServer(ros::NodeHandle nh, ros::NodeHandle nh_private, // Setup data structure const auto data_structure_params = - param::convert::toParamValue(nh_private, "map/data_structure"); + param::convert::toParamValue(nh_private, "map"); occupancy_map_ = VolumetricDataStructureFactory::create( data_structure_params, VolumetricDataStructureType::kHashedBlocks); CHECK_NOTNULL(occupancy_map_); @@ -72,33 +64,45 @@ WavemapServer::WavemapServer(ros::NodeHandle nh, ros::NodeHandle nh_private, addInput(integrator_params, nh, nh_private); } + // Setup operation handlers + const param::Array operation_params_array = + param::convert::toParamArray(nh_private, "operations"); + for (const auto& operation_params : operation_params_array) { + addOperation(operation_params, nh_private); + } + // Connect to ROS - subscribeToTimers(nh); - subscribeToTopics(nh); - advertiseTopics(nh_private); advertiseServices(nh_private); } -void WavemapServer::publishMap(bool republish_whole_map) { - ZoneScoped; - if (occupancy_map_ && !occupancy_map_->empty()) { - if (auto* hashed_wavelet_octree = - dynamic_cast(occupancy_map_.get()); - hashed_wavelet_octree) { - publishHashedMap(hashed_wavelet_octree, republish_whole_map); - } else if (auto* hashed_chunked_wavelet_octree = - dynamic_cast( - occupancy_map_.get()); - hashed_chunked_wavelet_octree) { - publishHashedMap(hashed_chunked_wavelet_octree, republish_whole_map); - } else { - occupancy_map_->threshold(); - wavemap_msgs::Map map_msg; - if (convert::mapToRosMsg(*occupancy_map_, config_.world_frame, - ros::Time::now(), map_msg)) { - map_pub_.publish(map_msg); - } - } +InputBase* WavemapServer::addInput(const param::Value& integrator_params, + const ros::NodeHandle& nh, + ros::NodeHandle nh_private) { + auto input_handler = InputFactory::create( + integrator_params, config_.world_frame, occupancy_map_, transformer_, + thread_pool_, nh, nh_private, std::nullopt, + [this]() { runOperations(); }); + if (input_handler) { + return input_handlers_.emplace_back(std::move(input_handler)).get(); + } + return nullptr; +} + +OperationBase* WavemapServer::addOperation(const param::Value& operation_params, + ros::NodeHandle nh_private) { + auto operation_handler = + OperationFactory::create(operation_params, config_.world_frame, + occupancy_map_, thread_pool_, nh_private); + if (operation_handler) { + return operations_.emplace_back(std::move(operation_handler)).get(); + } + return nullptr; +} + +void WavemapServer::runOperations(bool force_run_all) { + const ros::Time current_time = ros::Time::now(); + for (auto& operation : operations_) { + operation->run(current_time, force_run_all); } } @@ -116,59 +120,7 @@ bool WavemapServer::loadMap(const std::filesystem::path& file_path) { return io::fileToMap(file_path, occupancy_map_); } -InputHandler* WavemapServer::addInput(const param::Value& integrator_params, - const ros::NodeHandle& nh, - ros::NodeHandle nh_private) { - auto input_handler = InputHandlerFactory::create( - integrator_params, config_.world_frame, occupancy_map_, transformer_, - thread_pool_, nh, nh_private); - if (input_handler) { - return input_handlers_.emplace_back(std::move(input_handler)).get(); - } - return nullptr; -} - -void WavemapServer::subscribeToTimers(const ros::NodeHandle& nh) { - if (0.f < config_.thresholding_period) { - ROS_INFO_STREAM("Registering map thresholding timer with period " - << config_.thresholding_period << "s"); - map_thresholding_timer_ = nh.createTimer( - ros::Duration(config_.thresholding_period), - [this](const auto& /*event*/) { occupancy_map_->threshold(); }); - } - - if (0.f < config_.pruning_period) { - ROS_INFO_STREAM("Registering map pruning timer with period " - << config_.pruning_period << "s"); - map_pruning_timer_ = nh.createTimer( - ros::Duration(config_.pruning_period), - [this](const auto& /*event*/) { occupancy_map_->pruneSmart(); }); - } - - if (0.f < config_.publication_period) { - ROS_INFO_STREAM("Registering map publishing timer with period " - << config_.publication_period << "s"); - map_publication_timer_ = - nh.createTimer(ros::Duration(config_.publication_period), - [this](const auto& /*event*/) { publishMap(); }); - } -} - -void WavemapServer::subscribeToTopics(ros::NodeHandle& /*nh*/) {} - -void WavemapServer::advertiseTopics(ros::NodeHandle& nh_private) { - map_pub_ = nh_private.advertise("map", 10); -} - void WavemapServer::advertiseServices(ros::NodeHandle& nh_private) { - republish_whole_map_srv_ = - nh_private.advertiseService( - "republish_whole_map", [this](auto& /*request*/, auto& /*response*/) { - publishMap(true); - return true; - }); - reset_map_srv_ = nh_private.advertiseService( "reset_map", [this](auto& /*request*/, auto& response) { From 5abdf6f710ca0076d5b5b32be31b43c35cf76d02 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 20 Nov 2023 23:09:15 +0100 Subject: [PATCH 019/159] Update wavemap config schema and configs --- .../config/wavemap_livox_mid360.yaml | 25 +++++---- .../wavemap_livox_mid360_azure_kinect.yaml | 25 +++++---- .../wavemap_livox_mid360_pico_flexx.yaml | 27 ++++++---- .../wavemap_livox_mid360_pico_monstar.yaml | 27 ++++++---- .../config/wavemap_ouster_os0.yaml | 21 +++++--- .../wavemap_ouster_os0_pico_monstar.yaml | 27 ++++++---- .../config/wavemap_ouster_os1.yaml | 24 +++++---- .../config/wavemap_panoptic_mapping_rgbd.yaml | 24 +++++---- tooling/schemas/wavemap/general.json | 36 +++++++++++++ .../depth_image_input.json | 0 .../input.json} | 0 .../integration_method.json | 0 .../measurement_integrator.json | 0 .../measurement_model.json | 0 .../projection_model.json | 0 .../pointcloud_input.json | 0 .../wavemap/{data_structure.json => map.json} | 0 .../schemas/wavemap/operations/operation.json | 30 +++++++++++ .../operations/prune_map_operation.json | 15 ++++++ .../operations/publish_map_operation.json | 27 ++++++++++ .../operations/threshold_map_operation.json | 15 ++++++ tooling/schemas/wavemap/wavemap_config.json | 25 ++++----- tooling/schemas/wavemap/wavemap_server.json | 53 ------------------- 23 files changed, 257 insertions(+), 144 deletions(-) create mode 100644 tooling/schemas/wavemap/general.json rename tooling/schemas/wavemap/{measurement_input => inputs}/depth_image_input.json (100%) rename tooling/schemas/wavemap/{measurement_input/measurement_input.json => inputs/input.json} (100%) rename tooling/schemas/wavemap/{measurement_input => inputs}/measurement_integrator/integration_method.json (100%) rename tooling/schemas/wavemap/{measurement_input => inputs}/measurement_integrator/measurement_integrator.json (100%) rename tooling/schemas/wavemap/{measurement_input => inputs}/measurement_integrator/measurement_model.json (100%) rename tooling/schemas/wavemap/{measurement_input => inputs}/measurement_integrator/projection_model.json (100%) rename tooling/schemas/wavemap/{measurement_input => inputs}/pointcloud_input.json (100%) rename tooling/schemas/wavemap/{data_structure.json => map.json} (100%) create mode 100644 tooling/schemas/wavemap/operations/operation.json create mode 100644 tooling/schemas/wavemap/operations/prune_map_operation.json create mode 100644 tooling/schemas/wavemap/operations/publish_map_operation.json create mode 100644 tooling/schemas/wavemap/operations/threshold_map_operation.json delete mode 100644 tooling/schemas/wavemap/wavemap_server.json diff --git a/ros/wavemap_ros/config/wavemap_livox_mid360.yaml b/ros/wavemap_ros/config/wavemap_livox_mid360.yaml index e9e40033a..f03c9da59 100644 --- a/ros/wavemap_ros/config/wavemap_livox_mid360.yaml +++ b/ros/wavemap_ros/config/wavemap_livox_mid360.yaml @@ -1,14 +1,19 @@ +general: + world_frame: "odom" + logging_level: debug + allow_reset_map_service: true + map: - general: - world_frame: "odom" - thresholding_period: { seconds: 2.0 } - pruning_period: { seconds: 10.0 } - publication_period: { seconds: 3.0 } - logging_level: debug - allow_reset_map_service: true - data_structure: - type: hashed_chunked_wavelet_octree - min_cell_width: { meters: 0.1 } + type: hashed_chunked_wavelet_octree + min_cell_width: { meters: 0.1 } + +operations: + - type: threshold_map + once_every: { seconds: 2.0 } + - type: prune_map + once_every: { seconds: 10.0 } + - type: publish_map + once_every: { seconds: 2.0 } inputs: - type: pointcloud diff --git a/ros/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml b/ros/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml index 2378addab..ea724e4c5 100644 --- a/ros/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml +++ b/ros/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml @@ -1,14 +1,19 @@ +general: + world_frame: "odom" + logging_level: debug + allow_reset_map_service: true + map: - general: - world_frame: "odom" - thresholding_period: { seconds: 2.0 } - pruning_period: { seconds: 10.0 } - publication_period: { seconds: 2.0 } - logging_level: debug - allow_reset_map_service: true - data_structure: - type: hashed_chunked_wavelet_octree - min_cell_width: { meters: 0.01 } + type: hashed_chunked_wavelet_octree + min_cell_width: { meters: 0.01 } + +operations: + - type: threshold_map + once_every: { seconds: 2.0 } + - type: prune_map + once_every: { seconds: 10.0 } + - type: publish_map + once_every: { seconds: 2.0 } inputs: - type: depth_image diff --git a/ros/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml b/ros/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml index e51b97ae1..d54cd2681 100644 --- a/ros/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml +++ b/ros/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml @@ -1,15 +1,20 @@ +general: + world_frame: "odom" + logging_level: debug + allow_reset_map_service: true + map: - general: - world_frame: "odom" - thresholding_period: { seconds: 2.0 } - pruning_period: { seconds: 10.0 } - publication_period: { seconds: 2.0 } - logging_level: debug - allow_reset_map_service: true - data_structure: - type: hashed_chunked_wavelet_octree - min_cell_width: { meters: 0.01 } - tree_height: 9 + type: hashed_chunked_wavelet_octree + min_cell_width: { meters: 0.01 } + tree_height: 9 + +operations: + - type: threshold_map + once_every: { seconds: 2.0 } + - type: prune_map + once_every: { seconds: 10.0 } + - type: publish_map + once_every: { seconds: 2.0 } inputs: - type: depth_image diff --git a/ros/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml b/ros/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml index 05ce9092b..497996d82 100644 --- a/ros/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml +++ b/ros/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml @@ -1,15 +1,20 @@ +general: + world_frame: "odom" + logging_level: debug + allow_reset_map_service: true + map: - general: - world_frame: "odom" - thresholding_period: { seconds: 2.0 } - pruning_period: { seconds: 10.0 } - publication_period: { seconds: 2.0 } - logging_level: debug - allow_reset_map_service: true - data_structure: - type: hashed_chunked_wavelet_octree - min_cell_width: { meters: 0.01 } - tree_height: 9 + type: hashed_chunked_wavelet_octree + min_cell_width: { meters: 0.01 } + tree_height: 9 + +operations: + - type: threshold_map + once_every: { seconds: 2.0 } + - type: prune_map + once_every: { seconds: 10.0 } + - type: publish_map + once_every: { seconds: 2.0 } inputs: - type: depth_image diff --git a/ros/wavemap_ros/config/wavemap_ouster_os0.yaml b/ros/wavemap_ros/config/wavemap_ouster_os0.yaml index 9b43ce7e3..53fe1bace 100644 --- a/ros/wavemap_ros/config/wavemap_ouster_os0.yaml +++ b/ros/wavemap_ros/config/wavemap_ouster_os0.yaml @@ -1,15 +1,20 @@ +general: + world_frame: "odom" + logging_level: debug + allow_reset_map_service: true + map: - general: - world_frame: "odom" - thresholding_period: { seconds: 2.0 } - pruning_period: { seconds: 10.0 } - publication_period: { seconds: 2.0 } - logging_level: debug - allow_reset_map_service: true - data_structure: type: hashed_chunked_wavelet_octree min_cell_width: { meters: 0.1 } +operations: + - type: threshold_map + once_every: { seconds: 2.0 } + - type: prune_map + once_every: { seconds: 10.0 } + - type: publish_map + once_every: { seconds: 2.0 } + inputs: - type: pointcloud general: diff --git a/ros/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml b/ros/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml index 98b4d3f2a..93c378da3 100644 --- a/ros/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml +++ b/ros/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml @@ -1,15 +1,20 @@ +general: + world_frame: "gravity_init" + logging_level: debug + allow_reset_map_service: true + map: - general: - world_frame: "gravity_init" - thresholding_period: { seconds: 2.0 } - pruning_period: { seconds: 10.0 } - publication_period: { seconds: 2.0 } - logging_level: debug - allow_reset_map_service: true - data_structure: - type: hashed_chunked_wavelet_octree - min_cell_width: { meters: 0.01 } - tree_height: 9 + type: hashed_chunked_wavelet_octree + min_cell_width: { meters: 0.01 } + tree_height: 9 + +operations: + - type: threshold_map + once_every: { seconds: 2.0 } + - type: prune_map + once_every: { seconds: 10.0 } + - type: publish_map + once_every: { seconds: 2.0 } inputs: - type: depth_image diff --git a/ros/wavemap_ros/config/wavemap_ouster_os1.yaml b/ros/wavemap_ros/config/wavemap_ouster_os1.yaml index 321bded70..edf4b8e1e 100644 --- a/ros/wavemap_ros/config/wavemap_ouster_os1.yaml +++ b/ros/wavemap_ros/config/wavemap_ouster_os1.yaml @@ -1,13 +1,19 @@ +general: + world_frame: "odom" + logging_level: debug + allow_reset_map_service: true + map: - general: - world_frame: "odom" - pruning_period: { seconds: 10.0 } - publication_period: { seconds: 10.0 } - logging_level: debug - allow_reset_map_service: true - data_structure: - type: wavelet_octree - min_cell_width: { meters: 0.2 } + type: wavelet_octree + min_cell_width: { meters: 0.2 } + +operations: + - type: threshold_map + once_every: { seconds: 2.0 } + - type: prune_map + once_every: { seconds: 10.0 } + - type: publish_map + once_every: { seconds: 10.0 } inputs: - type: pointcloud diff --git a/ros/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml b/ros/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml index 06a562e57..9c7536ad3 100644 --- a/ros/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml +++ b/ros/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml @@ -1,13 +1,19 @@ +general: + world_frame: "odom" + logging_level: debug + allow_reset_map_service: true + map: - general: - world_frame: "odom" - pruning_period: { seconds: 10.0 } - publication_period: { seconds: 2.0 } - logging_level: debug - allow_reset_map_service: true - data_structure: - type: hashed_chunked_wavelet_octree - min_cell_width: { meters: 0.02 } + type: hashed_chunked_wavelet_octree + min_cell_width: { meters: 0.02 } + +operations: + - type: threshold_map + once_every: { seconds: 2.0 } + - type: prune_map + once_every: { seconds: 10.0 } + - type: publish_map + once_every: { seconds: 2.0 } inputs: - type: depth_image diff --git a/tooling/schemas/wavemap/general.json b/tooling/schemas/wavemap/general.json new file mode 100644 index 000000000..57df38460 --- /dev/null +++ b/tooling/schemas/wavemap/general.json @@ -0,0 +1,36 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "description": "General properties of the wavemap server.", + "type": "object", + "additionalProperties": false, + "properties": { + "world_frame": { + "description": "Name of the coordinate frame in which to store the map. Will be used as the frame_id for ROS TF lookups.", + "examples": [ + "world", + "odom" + ], + "type": "string" + }, + "num_threads": { + "description": "Maximum number of threads to use. Defaults to the number of threads supported by the CPU.", + "type": "integer", + "exclusiveMinimum": 0 + }, + "logging_level": { + "description": "Minimum severity level for ROS logging messages to be logged. Defaults to \"info\".", + "type": "string", + "enum": [ + "debug", + "info", + "warning", + "error", + "fatal" + ] + }, + "allow_reset_map_service": { + "description": "Whether or not to allow resetting the map through the reset_map service.", + "type": "boolean" + } + } +} diff --git a/tooling/schemas/wavemap/measurement_input/depth_image_input.json b/tooling/schemas/wavemap/inputs/depth_image_input.json similarity index 100% rename from tooling/schemas/wavemap/measurement_input/depth_image_input.json rename to tooling/schemas/wavemap/inputs/depth_image_input.json diff --git a/tooling/schemas/wavemap/measurement_input/measurement_input.json b/tooling/schemas/wavemap/inputs/input.json similarity index 100% rename from tooling/schemas/wavemap/measurement_input/measurement_input.json rename to tooling/schemas/wavemap/inputs/input.json diff --git a/tooling/schemas/wavemap/measurement_input/measurement_integrator/integration_method.json b/tooling/schemas/wavemap/inputs/measurement_integrator/integration_method.json similarity index 100% rename from tooling/schemas/wavemap/measurement_input/measurement_integrator/integration_method.json rename to tooling/schemas/wavemap/inputs/measurement_integrator/integration_method.json diff --git a/tooling/schemas/wavemap/measurement_input/measurement_integrator/measurement_integrator.json b/tooling/schemas/wavemap/inputs/measurement_integrator/measurement_integrator.json similarity index 100% rename from tooling/schemas/wavemap/measurement_input/measurement_integrator/measurement_integrator.json rename to tooling/schemas/wavemap/inputs/measurement_integrator/measurement_integrator.json diff --git a/tooling/schemas/wavemap/measurement_input/measurement_integrator/measurement_model.json b/tooling/schemas/wavemap/inputs/measurement_integrator/measurement_model.json similarity index 100% rename from tooling/schemas/wavemap/measurement_input/measurement_integrator/measurement_model.json rename to tooling/schemas/wavemap/inputs/measurement_integrator/measurement_model.json diff --git a/tooling/schemas/wavemap/measurement_input/measurement_integrator/projection_model.json b/tooling/schemas/wavemap/inputs/measurement_integrator/projection_model.json similarity index 100% rename from tooling/schemas/wavemap/measurement_input/measurement_integrator/projection_model.json rename to tooling/schemas/wavemap/inputs/measurement_integrator/projection_model.json diff --git a/tooling/schemas/wavemap/measurement_input/pointcloud_input.json b/tooling/schemas/wavemap/inputs/pointcloud_input.json similarity index 100% rename from tooling/schemas/wavemap/measurement_input/pointcloud_input.json rename to tooling/schemas/wavemap/inputs/pointcloud_input.json diff --git a/tooling/schemas/wavemap/data_structure.json b/tooling/schemas/wavemap/map.json similarity index 100% rename from tooling/schemas/wavemap/data_structure.json rename to tooling/schemas/wavemap/map.json diff --git a/tooling/schemas/wavemap/operations/operation.json b/tooling/schemas/wavemap/operations/operation.json new file mode 100644 index 000000000..a0600b49e --- /dev/null +++ b/tooling/schemas/wavemap/operations/operation.json @@ -0,0 +1,30 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "description": "Properties of a single operation to perform after map updates.", + "type": "object", + "required": [ + "type" + ], + "properties": { + "type": { + "description": "Type of the operation that should be used.", + "type": "string", + "enum": [ + "threshold_map", + "prune_map", + "publish_map" + ] + } + }, + "oneOf": [ + { + "$ref": "threshold_map_operation.json" + }, + { + "$ref": "prune_map_operation.json" + }, + { + "$ref": "publish_map_operation.json" + } + ] +} diff --git a/tooling/schemas/wavemap/operations/prune_map_operation.json b/tooling/schemas/wavemap/operations/prune_map_operation.json new file mode 100644 index 000000000..97b863c8f --- /dev/null +++ b/tooling/schemas/wavemap/operations/prune_map_operation.json @@ -0,0 +1,15 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "description": "Properties of a single map pruning operation.", + "type": "object", + "additionalProperties": false, + "properties": { + "type": { + "const": "prune_map" + }, + "once_every": { + "description": "Time period controlling how often the map is pruned.", + "$ref": "../value_with_unit/convertible_to_seconds.json" + } + } +} diff --git a/tooling/schemas/wavemap/operations/publish_map_operation.json b/tooling/schemas/wavemap/operations/publish_map_operation.json new file mode 100644 index 000000000..25fc73c88 --- /dev/null +++ b/tooling/schemas/wavemap/operations/publish_map_operation.json @@ -0,0 +1,27 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "description": "Properties of a single map publishing operation.", + "type": "object", + "additionalProperties": false, + "properties": { + "type": { + "const": "publish_map" + }, + "once_every": { + "description": "Time period controlling how often the map is published.", + "$ref": "../value_with_unit/convertible_to_seconds.json" + }, + "max_num_blocks_per_msg": { + "description": "Maximum number of blocks to transmit per wavemap map message. Used to control the maximum message size. Only works in combination with hash-based map data structures.", + "type": "integer", + "exclusiveMinimum": 0 + }, + "topic": { + "description": "Name of the topic the map will be published on. Note that the name of the service to request full map retransmissions will be the name of this topic suffixed with \"_request_full\".", + "type": "string", + "examples": [ + "map" + ] + } + } +} diff --git a/tooling/schemas/wavemap/operations/threshold_map_operation.json b/tooling/schemas/wavemap/operations/threshold_map_operation.json new file mode 100644 index 000000000..bc93bc11c --- /dev/null +++ b/tooling/schemas/wavemap/operations/threshold_map_operation.json @@ -0,0 +1,15 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "description": "Properties of a single map thresholding operation.", + "type": "object", + "additionalProperties": false, + "properties": { + "type": { + "const": "threshold_map" + }, + "once_every": { + "description": "Time period controlling how often the map is thresholded.", + "$ref": "../value_with_unit/convertible_to_seconds.json" + } + } +} diff --git a/tooling/schemas/wavemap/wavemap_config.json b/tooling/schemas/wavemap/wavemap_config.json index 35d4efcca..7ce0761b9 100644 --- a/tooling/schemas/wavemap/wavemap_config.json +++ b/tooling/schemas/wavemap/wavemap_config.json @@ -4,28 +4,29 @@ "description": "Schema to provide code completion and validation for wavemap configuration files.", "type": "object", "properties": { + "general": { + "$ref": "general.json" + }, "map": { - "description": "The properties of the server's map instance.", - "type": "object", - "additionalProperties": false, - "properties": { - "general": { - "$ref": "wavemap_server.json" - }, - "data_structure": { - "$ref": "data_structure.json" - } - } + "$ref": "map.json" }, "inputs": { "description": "A list of measurement inputs.", "type": "array", "items": { - "$ref": "measurement_input/measurement_input.json" + "$ref": "inputs/input.json" + } + }, + "operations": { + "description": "A list of operations to perform after map updates.", + "type": "array", + "items": { + "$ref": "operations/operation.json" } } }, "required": [ + "general", "map", "inputs" ] diff --git a/tooling/schemas/wavemap/wavemap_server.json b/tooling/schemas/wavemap/wavemap_server.json deleted file mode 100644 index c3ab630e8..000000000 --- a/tooling/schemas/wavemap/wavemap_server.json +++ /dev/null @@ -1,53 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "description": "General properties of the map.", - "type": "object", - "additionalProperties": false, - "properties": { - "world_frame": { - "description": "Name of the coordinate frame in which to store the map. Will be used as the frame_id for ROS TF lookups.", - "examples": [ - "world", - "odom" - ], - "type": "string" - }, - "thresholding_period": { - "description": "Time period controlling how often the map is thresholded. To disable thresholding, set it to a negative number [not recommended].", - "$ref": "value_with_unit/convertible_to_seconds.json" - }, - "pruning_period": { - "description": "Time period controlling how often the map is pruned. To disable pruning, set it to a negative number.", - "$ref": "value_with_unit/convertible_to_seconds.json" - }, - "publication_period": { - "description": "Time period controlling how often the map is published. To disable map publishing, set it to a negative number.", - "$ref": "value_with_unit/convertible_to_seconds.json" - }, - "max_num_blocks_per_msg": { - "description": "Maximum number of blocks to transmit per wavemap map message. Used to control the maximum message size. Only works in combination with hash-based map data structures.", - "type": "integer", - "exclusiveMinimum": 0 - }, - "num_threads": { - "description": "Maximum number of threads to use. Defaults to the number of threads supported by the CPU.", - "type": "integer", - "exclusiveMinimum": 0 - }, - "logging_level": { - "description": "Minimum severity level for ROS logging messages to be logged. Defaults to \"info\".", - "type": "string", - "enum": [ - "debug", - "info", - "warning", - "error", - "fatal" - ] - }, - "allow_reset_map_service": { - "description": "Whether or not to allow resetting the map through the reset_map service.", - "type": "boolean" - } - } -} From 68110fd6e03fbe2804c2e200dc72ff28285dc87e Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 21 Nov 2023 10:32:14 +0100 Subject: [PATCH 020/159] Update Livox-only code --- .../include/wavemap_ros/inputs/impl/pointcloud_input_impl.h | 4 ++-- ros/wavemap_ros/src/inputs/pointcloud_input.cc | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ros/wavemap_ros/include/wavemap_ros/inputs/impl/pointcloud_input_impl.h b/ros/wavemap_ros/include/wavemap_ros/inputs/impl/pointcloud_input_impl.h index c0a815a20..5b899986a 100644 --- a/ros/wavemap_ros/include/wavemap_ros/inputs/impl/pointcloud_input_impl.h +++ b/ros/wavemap_ros/include/wavemap_ros/inputs/impl/pointcloud_input_impl.h @@ -17,9 +17,9 @@ bool PointcloudInput::registerCallback(PointcloudTopicType type, case PointcloudTopicType::kLivox: #ifdef LIVOX_AVAILABLE // clang-format off - registrar(static_cast( - &PointcloudInputHandler::callback)); + &PointcloudInput::callback)); // clang-format on return true; #else diff --git a/ros/wavemap_ros/src/inputs/pointcloud_input.cc b/ros/wavemap_ros/src/inputs/pointcloud_input.cc index 13f4f8f52..8237d3a7d 100644 --- a/ros/wavemap_ros/src/inputs/pointcloud_input.cc +++ b/ros/wavemap_ros/src/inputs/pointcloud_input.cc @@ -127,7 +127,7 @@ void PointcloudInput::callback(const sensor_msgs::PointCloud2& pointcloud_msg) { } #ifdef LIVOX_AVAILABLE -void PointcloudInputHandler::callback( +void PointcloudInput::callback( const livox_ros_driver2::CustomMsg& pointcloud_msg) { ZoneScoped; // Skip empty clouds @@ -144,8 +144,8 @@ void PointcloudInputHandler::callback( std::string sensor_frame_id = config_.sensor_frame_id.empty() ? pointcloud_msg.header.frame_id : config_.sensor_frame_id; - GenericStampedPointcloud stamped_pointcloud{ - stamp_nsec, std::move(sensor_frame_id), pointcloud_msg.points.size()}; + StampedPointcloud stamped_pointcloud{stamp_nsec, std::move(sensor_frame_id), + pointcloud_msg.points.size()}; for (const auto& point : pointcloud_msg.points) { stamped_pointcloud.emplace(point.x, point.y, point.z, point.offset_time); } From 47c3b0acaf69f94eb3b64c1560aa0793079c70cb Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 21 Nov 2023 10:56:20 +0100 Subject: [PATCH 021/159] Implement map_crop operation for local mapping --- ros/wavemap_ros/CMakeLists.txt | 1 + .../operations/crop_map_operation.h | 76 +++++++++++++++++++ .../wavemap_ros/operations/operation_base.h | 4 +- .../operations/operation_factory.h | 3 + ros/wavemap_ros/src/inputs/input_factory.cc | 8 ++ .../src/operations/crop_map_operation.cc | 75 ++++++++++++++++++ .../src/operations/operation_factory.cc | 25 +++++- ros/wavemap_ros/src/wavemap_server.cc | 6 +- 8 files changed, 191 insertions(+), 7 deletions(-) create mode 100644 ros/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h create mode 100644 ros/wavemap_ros/src/operations/crop_map_operation.cc diff --git a/ros/wavemap_ros/CMakeLists.txt b/ros/wavemap_ros/CMakeLists.txt index 28d2db463..43ce55063 100644 --- a/ros/wavemap_ros/CMakeLists.txt +++ b/ros/wavemap_ros/CMakeLists.txt @@ -23,6 +23,7 @@ cs_add_library(${PROJECT_NAME} src/inputs/input_base.cc src/inputs/input_factory.cc src/inputs/pointcloud_input.cc + src/operations/crop_map_operation.cc src/operations/operation_factory.cc src/operations/prune_map_operation.cc src/operations/publish_map_operation.cc diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h b/ros/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h new file mode 100644 index 000000000..e7787fbd1 --- /dev/null +++ b/ros/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h @@ -0,0 +1,76 @@ +#ifndef WAVEMAP_ROS_OPERATIONS_CROP_MAP_OPERATION_H_ +#define WAVEMAP_ROS_OPERATIONS_CROP_MAP_OPERATION_H_ + +#include +#include +#include + +#include +#include + +#include "wavemap_ros/operations/operation_base.h" +#include "wavemap_ros/utils/tf_transformer.h" + +namespace wavemap { +/** + * Config struct for map pruning operations. + */ +struct CropMapOperationConfig : public ConfigBase { + //! Time period controlling how often the map is cropped. + Seconds once_every = 10.f; + + //! Name of the TF frame to treat as the center point. Usually the robot's + //! body frame. When the cropper runs, all blocks that are further than + //! remove_blocks_beyond_distance from this point are deleted. + std::string body_frame = "body"; + + //! Distance beyond which blocks are deleted. + Meters remove_blocks_beyond_distance; + + static MemberMap memberMap; + + bool isValid(bool verbose) const override; +}; + +class CropMapOperation : public OperationBase { + public: + CropMapOperation(const CropMapOperationConfig& config, + std::string world_frame, + std::shared_ptr transformer, + VolumetricDataStructureBase::Ptr occupancy_map) + : config_(config.checkValid()), + world_frame_(std::move(world_frame)), + transformer_(std::move(transformer)), + occupancy_map_(std::move(occupancy_map)) {} + + OperationType getType() const override { return OperationType::kCropMap; } + + bool shouldRun(const ros::Time& current_time) { + return config_.once_every < (current_time - last_run_timestamp_).toSec(); + } + + void run(const ros::Time& current_time, bool force_run) override; + + private: + const CropMapOperationConfig config_; + const std::string world_frame_; + const std::shared_ptr transformer_; + const VolumetricDataStructureBase::Ptr occupancy_map_; + ros::Time last_run_timestamp_; + + template + void eraseBlockIf(MapT* map, IndicatorFunctionT indicator_fn) { + auto& block_map = map->getBlocks(); + for (auto block_it = block_map.begin(); block_it != block_map.end();) { + const auto& block_index = block_it->first; + if (std::invoke(indicator_fn, block_index)) { + block_it = block_map.erase(block_it); + } else { + ++block_it; + } + } + } +}; +} // namespace wavemap + +#endif // WAVEMAP_ROS_OPERATIONS_CROP_MAP_OPERATION_H_ diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/operation_base.h b/ros/wavemap_ros/include/wavemap_ros/operations/operation_base.h index 0709c43a0..4ed51b0dd 100644 --- a/ros/wavemap_ros/include/wavemap_ros/operations/operation_base.h +++ b/ros/wavemap_ros/include/wavemap_ros/operations/operation_base.h @@ -8,10 +8,10 @@ namespace wavemap { struct OperationType : public TypeSelector { using TypeSelector::TypeSelector; - enum Id : TypeId { kThresholdMap, kPruneMap, kPublishMap }; + enum Id : TypeId { kThresholdMap, kPruneMap, kPublishMap, kCropMap }; static constexpr std::array names = {"threshold_map", "prune_map", - "publish_map"}; + "publish_map", "crop_map"}; }; class OperationBase { diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/operation_factory.h b/ros/wavemap_ros/include/wavemap_ros/operations/operation_factory.h index ceacbbe59..f23c3c1fa 100644 --- a/ros/wavemap_ros/include/wavemap_ros/operations/operation_factory.h +++ b/ros/wavemap_ros/include/wavemap_ros/operations/operation_factory.h @@ -9,6 +9,7 @@ #include #include "wavemap_ros/operations/operation_base.h" +#include "wavemap_ros/utils/tf_transformer.h" namespace wavemap { class OperationFactory { @@ -16,12 +17,14 @@ class OperationFactory { static std::unique_ptr create( const param::Value& params, std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr transformer, std::shared_ptr thread_pool, ros::NodeHandle nh_private, std::optional default_operation_type = std::nullopt); static std::unique_ptr create( OperationType operation_type, const param::Value& params, std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr transformer, std::shared_ptr thread_pool, ros::NodeHandle nh_private); }; } // namespace wavemap diff --git a/ros/wavemap_ros/src/inputs/input_factory.cc b/ros/wavemap_ros/src/inputs/input_factory.cc index 723c0f108..03c16e4f0 100644 --- a/ros/wavemap_ros/src/inputs/input_factory.cc +++ b/ros/wavemap_ros/src/inputs/input_factory.cc @@ -36,6 +36,11 @@ std::unique_ptr InputFactory::create( std::shared_ptr transformer, std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private, std::function map_update_callback) { + if (!input_type.isValid()) { + ROS_ERROR("Received request to create input handler with invalid type."); + return nullptr; + } + // Create the input handler switch (input_type.toTypeId()) { case InputType::kPointcloud: @@ -63,6 +68,9 @@ std::unique_ptr InputFactory::create( return nullptr; } } + + ROS_ERROR_STREAM("Factory does not support creation of input type " + << input_type.toStr() << "."); return nullptr; } } // namespace wavemap diff --git a/ros/wavemap_ros/src/operations/crop_map_operation.cc b/ros/wavemap_ros/src/operations/crop_map_operation.cc new file mode 100644 index 000000000..c551c54ec --- /dev/null +++ b/ros/wavemap_ros/src/operations/crop_map_operation.cc @@ -0,0 +1,75 @@ +#include "wavemap_ros/operations/crop_map_operation.h" + +#include +#include +#include + +namespace wavemap { +DECLARE_CONFIG_MEMBERS(CropMapOperationConfig, + (once_every) + (body_frame) + (remove_blocks_beyond_distance)); + +bool CropMapOperationConfig::isValid(bool verbose) const { + bool all_valid = true; + + all_valid &= IS_PARAM_GT(once_every, 0.f, verbose); + all_valid &= IS_PARAM_NE(body_frame, std::string(), verbose); + all_valid &= IS_PARAM_GT(remove_blocks_beyond_distance, 0.f, verbose); + + return all_valid; +} + +void CropMapOperation::run(const ros::Time& current_time, bool force_run) { + if (!force_run && !shouldRun(current_time)) { + return; + } + last_run_timestamp_ = current_time; + + if (occupancy_map_->empty()) { + return; + } + + Transformation3D T_W_B; + if (!transformer_->lookupTransform(world_frame_, config_.body_frame, + current_time, T_W_B)) { + ROS_WARN_STREAM( + "Could not look up center point for map cropping. TF lookup of " + "body_frame \"" + << config_.body_frame << "\" w.r.t. world_frame \"" << world_frame_ + << "\" at time " << current_time << " failed."); + return; + } + + const IndexElement tree_height = occupancy_map_->getTreeHeight(); + const FloatingPoint min_cell_width = occupancy_map_->getMinCellWidth(); + const Point3D t_W_B = T_W_B.getPosition(); + + auto indicator_fn = [tree_height, min_cell_width, &config = config_, + &t_W_B](const Index3D& block_index) { + const auto block_node_index = OctreeIndex{tree_height, block_index}; + const auto block_aabb = + convert::nodeIndexToAABB(block_node_index, min_cell_width); + const FloatingPoint d_B_block = block_aabb.minDistanceTo(t_W_B); + return config.remove_blocks_beyond_distance < d_B_block; + }; + + if (auto* hashed_blocks = + dynamic_cast(occupancy_map_.get()); + hashed_blocks) { + eraseBlockIf(hashed_blocks, indicator_fn); + } else if (auto* hashed_wavelet_octree = + dynamic_cast(occupancy_map_.get()); + hashed_wavelet_octree) { + eraseBlockIf(hashed_wavelet_octree, indicator_fn); + } else if (auto* hashed_chunked_wavelet_octree = + dynamic_cast( + occupancy_map_.get()); + hashed_chunked_wavelet_octree) { + eraseBlockIf(hashed_chunked_wavelet_octree, indicator_fn); + } else { + ROS_WARN( + "Map cropping is only supported for hash-based map data structures."); + } +} +} // namespace wavemap diff --git a/ros/wavemap_ros/src/operations/operation_factory.cc b/ros/wavemap_ros/src/operations/operation_factory.cc index 26ce62f04..2450fc220 100644 --- a/ros/wavemap_ros/src/operations/operation_factory.cc +++ b/ros/wavemap_ros/src/operations/operation_factory.cc @@ -1,5 +1,6 @@ #include "wavemap_ros/operations/operation_factory.h" +#include "wavemap_ros/operations/crop_map_operation.h" #include "wavemap_ros/operations/prune_map_operation.h" #include "wavemap_ros/operations/publish_map_operation.h" #include "wavemap_ros/operations/threshold_map_operation.h" @@ -8,11 +9,13 @@ namespace wavemap { std::unique_ptr OperationFactory::create( const param::Value& params, std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr transformer, std::shared_ptr thread_pool, ros::NodeHandle nh_private, std::optional default_operation_type) { if (const auto type = OperationType::from(params); type) { return create(type.value(), params, std::move(world_frame), - std::move(occupancy_map), std::move(thread_pool), nh_private); + std::move(occupancy_map), std::move(transformer), + std::move(thread_pool), nh_private); } if (default_operation_type.has_value()) { @@ -20,7 +23,7 @@ std::unique_ptr OperationFactory::create( << "\" will be created instead."); return create(default_operation_type.value(), params, std::move(world_frame), std::move(occupancy_map), - std::move(thread_pool), nh_private); + std::move(transformer), std::move(thread_pool), nh_private); } ROS_ERROR("No default was set. Returning nullptr."); @@ -30,7 +33,13 @@ std::unique_ptr OperationFactory::create( std::unique_ptr OperationFactory::create( OperationType operation_type, const param::Value& params, std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr transformer, std::shared_ptr thread_pool, ros::NodeHandle nh_private) { + if (!operation_type.isValid()) { + ROS_ERROR("Received request to create operation with invalid type."); + return nullptr; + } + // Create the operation handler switch (operation_type.toTypeId()) { case OperationType::kThresholdMap: @@ -59,7 +68,19 @@ std::unique_ptr OperationFactory::create( ROS_ERROR("Publish map operation config could not be loaded."); return nullptr; } + case OperationType::kCropMap: + if (const auto config = CropMapOperationConfig::from(params); config) { + return std::make_unique( + config.value(), std::move(world_frame), std::move(transformer), + std::move(occupancy_map)); + } else { + ROS_ERROR("Crop map operation config could not be loaded."); + return nullptr; + } } + + ROS_ERROR_STREAM("Factory does not support creation of operation type " + << operation_type.toStr() << "."); return nullptr; } } // namespace wavemap diff --git a/ros/wavemap_ros/src/wavemap_server.cc b/ros/wavemap_ros/src/wavemap_server.cc index 31e685163..5e53b544c 100644 --- a/ros/wavemap_ros/src/wavemap_server.cc +++ b/ros/wavemap_ros/src/wavemap_server.cc @@ -90,9 +90,9 @@ InputBase* WavemapServer::addInput(const param::Value& integrator_params, OperationBase* WavemapServer::addOperation(const param::Value& operation_params, ros::NodeHandle nh_private) { - auto operation_handler = - OperationFactory::create(operation_params, config_.world_frame, - occupancy_map_, thread_pool_, nh_private); + auto operation_handler = OperationFactory::create( + operation_params, config_.world_frame, occupancy_map_, transformer_, + thread_pool_, nh_private); if (operation_handler) { return operations_.emplace_back(std::move(operation_handler)).get(); } From 5b7d3a0ae1f6b0de0f54a01328908fce962f3473 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 21 Nov 2023 11:05:55 +0100 Subject: [PATCH 022/159] Drive operations with input message time instead of ros::Time::now() --- ros/wavemap_ros/app/rosbag_processor.cc | 2 +- .../wavemap_ros/inputs/depth_image_input.h | 14 +++++++------- .../include/wavemap_ros/inputs/input_base.h | 6 +++--- .../include/wavemap_ros/inputs/input_factory.h | 4 ++-- .../wavemap_ros/inputs/pointcloud_input.h | 14 +++++++------- .../include/wavemap_ros/wavemap_server.h | 2 +- .../src/inputs/depth_image_input.cc | 17 ++++++++--------- ros/wavemap_ros/src/inputs/input_base.cc | 2 +- ros/wavemap_ros/src/inputs/input_factory.cc | 5 +++-- ros/wavemap_ros/src/inputs/pointcloud_input.cc | 18 +++++++++--------- ros/wavemap_ros/src/wavemap_server.cc | 6 +++--- 11 files changed, 45 insertions(+), 45 deletions(-) diff --git a/ros/wavemap_ros/app/rosbag_processor.cc b/ros/wavemap_ros/app/rosbag_processor.cc index 561ba7e71..595274f91 100644 --- a/ros/wavemap_ros/app/rosbag_processor.cc +++ b/ros/wavemap_ros/app/rosbag_processor.cc @@ -79,7 +79,7 @@ int main(int argc, char** argv) { } wavemap_server.getMap()->prune(); - wavemap_server.runOperations(/*force_run_all*/ true); + wavemap_server.runOperations(ros::Time::now(), /*force_run_all*/ true); if (nh_private.param("keep_alive", false)) { ros::spin(); diff --git a/ros/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h b/ros/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h index 4056a98a1..4660f868a 100644 --- a/ros/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h +++ b/ros/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h @@ -67,13 +67,13 @@ struct DepthImageInputConfig : public ConfigBase { class DepthImageInput : public InputBase { public: - DepthImageInput(const DepthImageInputConfig& config, - const param::Value& params, std::string world_frame, - VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh, - ros::NodeHandle nh_private, - std::function map_update_callback = {}); + DepthImageInput( + const DepthImageInputConfig& config, const param::Value& params, + std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr transformer, + std::shared_ptr thread_pool, ros::NodeHandle nh, + ros::NodeHandle nh_private, + std::function map_update_callback = {}); InputType getType() const override { return InputType::kDepthImage; } diff --git a/ros/wavemap_ros/include/wavemap_ros/inputs/input_base.h b/ros/wavemap_ros/include/wavemap_ros/inputs/input_base.h index b5973189b..44b28a00d 100644 --- a/ros/wavemap_ros/include/wavemap_ros/inputs/input_base.h +++ b/ros/wavemap_ros/include/wavemap_ros/inputs/input_base.h @@ -62,7 +62,7 @@ class InputBase { std::shared_ptr transformer, std::shared_ptr thread_pool, const ros::NodeHandle& nh, ros::NodeHandle nh_private, - std::function map_update_callback = {}); + std::function map_update_callback = {}); virtual ~InputBase() = default; virtual InputType getType() const = 0; @@ -72,7 +72,7 @@ class InputBase { const InputBaseConfig config_; const std::string world_frame_; - std::shared_ptr transformer_; + const std::shared_ptr transformer_; std::vector integrators_; Stopwatch integration_timer_; @@ -80,7 +80,7 @@ class InputBase { virtual void processQueue() = 0; ros::Timer queue_processing_retry_timer_; - std::function map_update_callback_; + std::function map_update_callback_; bool shouldPublishReprojectedPointcloud() const; void publishReprojectedPointcloud(const ros::Time& stamp, diff --git a/ros/wavemap_ros/include/wavemap_ros/inputs/input_factory.h b/ros/wavemap_ros/include/wavemap_ros/inputs/input_factory.h index ef75ae94b..7c7da5ed6 100644 --- a/ros/wavemap_ros/include/wavemap_ros/inputs/input_factory.h +++ b/ros/wavemap_ros/include/wavemap_ros/inputs/input_factory.h @@ -17,7 +17,7 @@ class InputFactory { std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private, std::optional default_input_type = std::nullopt, - std::function map_update_callback = {}); + std::function map_update_callback = {}); static std::unique_ptr create( InputType input_type, const param::Value& params, std::string world_frame, @@ -25,7 +25,7 @@ class InputFactory { std::shared_ptr transformer, std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private, - std::function map_update_callback = {}); + std::function map_update_callback = {}); }; } // namespace wavemap diff --git a/ros/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h b/ros/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h index 0296b2111..cf5c8b88f 100644 --- a/ros/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h +++ b/ros/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h @@ -82,13 +82,13 @@ struct PointcloudInputConfig class PointcloudInput : public InputBase { public: - PointcloudInput(const PointcloudInputConfig& config, - const param::Value& params, std::string world_frame, - VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh, - ros::NodeHandle nh_private, - std::function map_update_callback = {}); + PointcloudInput( + const PointcloudInputConfig& config, const param::Value& params, + std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr transformer, + std::shared_ptr thread_pool, ros::NodeHandle nh, + ros::NodeHandle nh_private, + std::function map_update_callback = {}); InputType getType() const override { return InputType::kPointcloud; } PointcloudTopicType getTopicType() const { return config_.topic_type; } diff --git a/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h b/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h index 41801d367..c797d907f 100644 --- a/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h +++ b/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h @@ -52,7 +52,7 @@ class WavemapServer { OperationBase* addOperation(const param::Value& operation_params, ros::NodeHandle nh_private); - void runOperations(bool force_run_all = false); + void runOperations(const ros::Time& current_time, bool force_run_all = false); VolumetricDataStructureBase::Ptr getMap() { return occupancy_map_; } VolumetricDataStructureBase::ConstPtr getMap() const { diff --git a/ros/wavemap_ros/src/inputs/depth_image_input.cc b/ros/wavemap_ros/src/inputs/depth_image_input.cc index 7c3644589..a2966f802 100644 --- a/ros/wavemap_ros/src/inputs/depth_image_input.cc +++ b/ros/wavemap_ros/src/inputs/depth_image_input.cc @@ -30,14 +30,13 @@ bool DepthImageInputConfig::isValid(bool verbose) const { return all_valid; } -DepthImageInput::DepthImageInput(const DepthImageInputConfig& config, - const param::Value& params, - std::string world_frame, - VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, - ros::NodeHandle nh, ros::NodeHandle nh_private, - std::function map_update_callback) +DepthImageInput::DepthImageInput( + const DepthImageInputConfig& config, const param::Value& params, + std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr transformer, + std::shared_ptr thread_pool, ros::NodeHandle nh, + ros::NodeHandle nh_private, + std::function map_update_callback) : InputBase(config, params, std::move(world_frame), std::move(occupancy_map), std::move(transformer), std::move(thread_pool), nh, nh_private, @@ -123,7 +122,7 @@ void DepthImageInput::processQueue() { // Notify subscribers that the map was updated if (map_update_callback_) { - std::invoke(map_update_callback_); + std::invoke(map_update_callback_, stamp); } // Publish debugging visualizations diff --git a/ros/wavemap_ros/src/inputs/input_base.cc b/ros/wavemap_ros/src/inputs/input_base.cc index 746289649..edc08cf46 100644 --- a/ros/wavemap_ros/src/inputs/input_base.cc +++ b/ros/wavemap_ros/src/inputs/input_base.cc @@ -33,7 +33,7 @@ InputBase::InputBase(const InputBaseConfig& config, const param::Value& params, std::shared_ptr transformer, std::shared_ptr thread_pool, const ros::NodeHandle& nh, ros::NodeHandle nh_private, - std::function map_update_callback) + std::function map_update_callback) : config_(config.checkValid()), world_frame_(std::move(world_frame)), transformer_(std::move(transformer)), diff --git a/ros/wavemap_ros/src/inputs/input_factory.cc b/ros/wavemap_ros/src/inputs/input_factory.cc index 03c16e4f0..8768fedcb 100644 --- a/ros/wavemap_ros/src/inputs/input_factory.cc +++ b/ros/wavemap_ros/src/inputs/input_factory.cc @@ -10,7 +10,7 @@ std::unique_ptr InputFactory::create( std::shared_ptr transformer, std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private, std::optional default_input_type, - std::function map_update_callback) { + std::function map_update_callback) { if (const auto type = InputType::from(params); type) { return create(type.value(), params, world_frame, occupancy_map, std::move(transformer), std::move(thread_pool), nh, @@ -35,7 +35,8 @@ std::unique_ptr InputFactory::create( VolumetricDataStructureBase::Ptr occupancy_map, std::shared_ptr transformer, std::shared_ptr thread_pool, ros::NodeHandle nh, - ros::NodeHandle nh_private, std::function map_update_callback) { + ros::NodeHandle nh_private, + std::function map_update_callback) { if (!input_type.isValid()) { ROS_ERROR("Received request to create input handler with invalid type."); return nullptr; diff --git a/ros/wavemap_ros/src/inputs/pointcloud_input.cc b/ros/wavemap_ros/src/inputs/pointcloud_input.cc index 8237d3a7d..d0fb8bfe8 100644 --- a/ros/wavemap_ros/src/inputs/pointcloud_input.cc +++ b/ros/wavemap_ros/src/inputs/pointcloud_input.cc @@ -30,14 +30,13 @@ bool PointcloudInputConfig::isValid(bool verbose) const { return all_valid; } -PointcloudInput::PointcloudInput(const PointcloudInputConfig& config, - const param::Value& params, - std::string world_frame, - VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, - ros::NodeHandle nh, ros::NodeHandle nh_private, - std::function map_update_callback) +PointcloudInput::PointcloudInput( + const PointcloudInputConfig& config, const param::Value& params, + std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, + std::shared_ptr transformer, + std::shared_ptr thread_pool, ros::NodeHandle nh, + ros::NodeHandle nh_private, + std::function map_update_callback) : InputBase(config, params, std::move(world_frame), std::move(occupancy_map), transformer, std::move(thread_pool), nh, nh_private, std::move(map_update_callback)), @@ -246,7 +245,8 @@ void PointcloudInput::processQueue() { // Notify subscribers that the map was updated if (map_update_callback_) { - std::invoke(map_update_callback_); + std::invoke(map_update_callback_, + convert::nanoSecondsToRosTime(oldest_msg.getEndTime())); } // Publish debugging visualizations diff --git a/ros/wavemap_ros/src/wavemap_server.cc b/ros/wavemap_ros/src/wavemap_server.cc index 5e53b544c..e7997534d 100644 --- a/ros/wavemap_ros/src/wavemap_server.cc +++ b/ros/wavemap_ros/src/wavemap_server.cc @@ -81,7 +81,7 @@ InputBase* WavemapServer::addInput(const param::Value& integrator_params, auto input_handler = InputFactory::create( integrator_params, config_.world_frame, occupancy_map_, transformer_, thread_pool_, nh, nh_private, std::nullopt, - [this]() { runOperations(); }); + [this](const ros::Time& current_time) { runOperations(current_time); }); if (input_handler) { return input_handlers_.emplace_back(std::move(input_handler)).get(); } @@ -99,8 +99,8 @@ OperationBase* WavemapServer::addOperation(const param::Value& operation_params, return nullptr; } -void WavemapServer::runOperations(bool force_run_all) { - const ros::Time current_time = ros::Time::now(); +void WavemapServer::runOperations(const ros::Time& current_time, + bool force_run_all) { for (auto& operation : operations_) { operation->run(current_time, force_run_all); } From a1b85781bc38f120abef28877c0fd8e92e19a016 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 21 Nov 2023 11:18:42 +0100 Subject: [PATCH 023/159] Add schema for map crop operation config validation --- .../operations/crop_map_operation.h | 2 +- .../operations/crop_map_operation.json | 26 +++++++++++++++++++ .../schemas/wavemap/operations/operation.json | 6 ++++- 3 files changed, 32 insertions(+), 2 deletions(-) create mode 100644 tooling/schemas/wavemap/operations/crop_map_operation.json diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h b/ros/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h index e7787fbd1..a77d5ed53 100644 --- a/ros/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h +++ b/ros/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h @@ -24,7 +24,7 @@ struct CropMapOperationConfig : public ConfigBase { //! remove_blocks_beyond_distance from this point are deleted. std::string body_frame = "body"; - //! Distance beyond which blocks are deleted. + //! Distance beyond which blocks are deleted when the cropper is executed. Meters remove_blocks_beyond_distance; static MemberMap memberMap; diff --git a/tooling/schemas/wavemap/operations/crop_map_operation.json b/tooling/schemas/wavemap/operations/crop_map_operation.json new file mode 100644 index 000000000..427259030 --- /dev/null +++ b/tooling/schemas/wavemap/operations/crop_map_operation.json @@ -0,0 +1,26 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "description": "Properties of a single map cropping operation.", + "type": "object", + "additionalProperties": false, + "properties": { + "type": { + "const": "crop_map" + }, + "once_every": { + "description": "Time period controlling how often the map is cropped.", + "$ref": "../value_with_unit/convertible_to_seconds.json" + }, + "body_frame": { + "description": "Name of the TF frame to treat as the center point. Usually the robot's body frame. When the cropper runs, all blocks that are further than remove_blocks_beyond_distance from this point are deleted.", + "type": "string", + "examples": [ + "body" + ] + }, + "remove_blocks_beyond_distance": { + "description": "Distance beyond which blocks are deleted when the cropper is executed.", + "$ref": "../value_with_unit/convertible_to_meters.json" + } + } +} diff --git a/tooling/schemas/wavemap/operations/operation.json b/tooling/schemas/wavemap/operations/operation.json index a0600b49e..090d2bd24 100644 --- a/tooling/schemas/wavemap/operations/operation.json +++ b/tooling/schemas/wavemap/operations/operation.json @@ -12,7 +12,8 @@ "enum": [ "threshold_map", "prune_map", - "publish_map" + "publish_map", + "crop_map" ] } }, @@ -25,6 +26,9 @@ }, { "$ref": "publish_map_operation.json" + }, + { + "$ref": "crop_map_operation.json" } ] } From d3fda74a7384a1d1f8419566159d5a0b1fcd4b31 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 23 Nov 2023 10:39:59 +0100 Subject: [PATCH 024/159] Add convenience aliases --- .../wavemap/data_structure/chunked_ndtree/chunked_ndtree.h | 7 +++++++ .../wavemap/include/wavemap/data_structure/ndtree/ndtree.h | 7 +++++++ .../wavemap/map/hashed_chunked_wavelet_octree_block.h | 7 +++---- .../include/wavemap/map/hashed_wavelet_octree_block.h | 5 +++-- libraries/wavemap/include/wavemap/map/volumetric_octree.h | 5 +++-- 5 files changed, 23 insertions(+), 8 deletions(-) diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h index f3bbc6097..69ceeb076 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h @@ -56,6 +56,13 @@ class ChunkedNdtree { std::pair getChunkAndRelativeIndex( const IndexType& index) const; }; + +template +using ChunkedBinaryTree = ChunkedNdtree; +template +using ChunkedQuadtree = ChunkedNdtree; +template +using ChunkedOctree = ChunkedNdtree; } // namespace wavemap #include "wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h" diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree.h b/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree.h index bc2e05413..b659c19af 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree.h +++ b/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree.h @@ -57,6 +57,13 @@ class Ndtree { NodeType* getNode(const IndexType& index, bool auto_allocate); const NodeType* getNode(const IndexType& index) const; }; + +template +using BinaryTree = Ndtree; +template +using Quadtree = Ndtree; +template +using Octree = Ndtree; } // namespace wavemap #include "wavemap/data_structure/ndtree/impl/ndtree_inl.h" diff --git a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h index 2efb78e63..7d6ea01b4 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h +++ b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h @@ -17,8 +17,8 @@ class HashedChunkedWaveletOctreeBlock { using BlockIndex = Index3D; using Coefficients = HaarCoefficients; using Transform = HaarTransform; - using NodeChunkType = - NdtreeNodeChunk; + using ChunkedOctreeType = ChunkedOctree; + using NodeChunkType = ChunkedOctreeType::NodeChunkType; explicit HashedChunkedWaveletOctreeBlock(IndexElement tree_height, FloatingPoint min_log_odds, @@ -82,8 +82,7 @@ class HashedChunkedWaveletOctreeBlock { const FloatingPoint min_log_odds_; const FloatingPoint max_log_odds_; - ChunkedNdtree chunked_ndtree_{ - tree_height_ - 1}; + ChunkedOctreeType chunked_ndtree_{tree_height_ - 1}; Coefficients::Scale root_scale_coefficient_{}; bool needs_thresholding_ = false; diff --git a/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree_block.h b/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree_block.h index 5ae706c4b..c3365dcd2 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree_block.h +++ b/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree_block.h @@ -15,7 +15,8 @@ class HashedWaveletOctreeBlock { using BlockIndex = Index3D; using Coefficients = HaarCoefficients; using Transform = HaarTransform; - using NodeType = NdtreeNode; + using OctreeType = Octree; + using NodeType = OctreeType::NodeType; explicit HashedWaveletOctreeBlock(IndexElement tree_height, FloatingPoint min_log_odds, @@ -73,7 +74,7 @@ class HashedWaveletOctreeBlock { const FloatingPoint min_log_odds_; const FloatingPoint max_log_odds_; - Ndtree ndtree_{tree_height_ - 1}; + OctreeType ndtree_{tree_height_ - 1}; Coefficients::Scale root_scale_coefficient_{}; bool needs_thresholding_ = false; diff --git a/libraries/wavemap/include/wavemap/map/volumetric_octree.h b/libraries/wavemap/include/wavemap/map/volumetric_octree.h index d4a3b20bc..a2d2d7e6f 100644 --- a/libraries/wavemap/include/wavemap/map/volumetric_octree.h +++ b/libraries/wavemap/include/wavemap/map/volumetric_octree.h @@ -47,7 +47,8 @@ class VolumetricOctree : public VolumetricDataStructureBase { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; using Config = VolumetricOctreeConfig; - using NodeType = NdtreeNode; + using OctreeType = Octree; + using NodeType = OctreeType::NodeType; static constexpr bool kRequiresExplicitThresholding = true; @@ -94,7 +95,7 @@ class VolumetricOctree : public VolumetricDataStructureBase { private: const VolumetricOctreeConfig config_; - Ndtree ndtree_{config_.tree_height}; + OctreeType ndtree_{config_.tree_height}; OctreeIndex getInternalRootNodeIndex() const { return OctreeIndex{config_.tree_height, OctreeIndex::Position::Zero()}; From 82ca28ee3adacb4782b6fcafc5219755ce91ad5d Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 24 Nov 2023 13:45:15 +0100 Subject: [PATCH 025/159] Rename operations param to operations_pipeline as suggested by @jk-ethz This more clearly expresses the concept and also directly implies that they're run sequentially - i.e. users can change the order of the operations by simply reordering them in the config's yaml array. --- ros/wavemap_ros/config/wavemap_livox_mid360.yaml | 2 +- .../config/wavemap_livox_mid360_azure_kinect.yaml | 2 +- ros/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml | 2 +- .../config/wavemap_livox_mid360_pico_monstar.yaml | 2 +- ros/wavemap_ros/config/wavemap_ouster_os0.yaml | 6 +++--- ros/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml | 2 +- ros/wavemap_ros/config/wavemap_ouster_os1.yaml | 2 +- ros/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml | 2 +- ros/wavemap_ros/src/wavemap_server.cc | 2 +- .../operations/{operation.json => operations_pipeline.json} | 0 tooling/schemas/wavemap/wavemap_config.json | 4 ++-- 11 files changed, 13 insertions(+), 13 deletions(-) rename tooling/schemas/wavemap/operations/{operation.json => operations_pipeline.json} (100%) diff --git a/ros/wavemap_ros/config/wavemap_livox_mid360.yaml b/ros/wavemap_ros/config/wavemap_livox_mid360.yaml index f03c9da59..823432fa0 100644 --- a/ros/wavemap_ros/config/wavemap_livox_mid360.yaml +++ b/ros/wavemap_ros/config/wavemap_livox_mid360.yaml @@ -7,7 +7,7 @@ map: type: hashed_chunked_wavelet_octree min_cell_width: { meters: 0.1 } -operations: +operations_pipeline: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map diff --git a/ros/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml b/ros/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml index ea724e4c5..05b109a4e 100644 --- a/ros/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml +++ b/ros/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml @@ -7,7 +7,7 @@ map: type: hashed_chunked_wavelet_octree min_cell_width: { meters: 0.01 } -operations: +operations_pipeline: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map diff --git a/ros/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml b/ros/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml index d54cd2681..0b5a02b98 100644 --- a/ros/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml +++ b/ros/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml @@ -8,7 +8,7 @@ map: min_cell_width: { meters: 0.01 } tree_height: 9 -operations: +operations_pipeline: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map diff --git a/ros/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml b/ros/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml index 497996d82..0652712af 100644 --- a/ros/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml +++ b/ros/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml @@ -8,7 +8,7 @@ map: min_cell_width: { meters: 0.01 } tree_height: 9 -operations: +operations_pipeline: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map diff --git a/ros/wavemap_ros/config/wavemap_ouster_os0.yaml b/ros/wavemap_ros/config/wavemap_ouster_os0.yaml index 53fe1bace..becc40df4 100644 --- a/ros/wavemap_ros/config/wavemap_ouster_os0.yaml +++ b/ros/wavemap_ros/config/wavemap_ouster_os0.yaml @@ -4,10 +4,10 @@ general: allow_reset_map_service: true map: - type: hashed_chunked_wavelet_octree - min_cell_width: { meters: 0.1 } + type: hashed_chunked_wavelet_octree + min_cell_width: { meters: 0.1 } -operations: +operations_pipeline: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map diff --git a/ros/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml b/ros/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml index 93c378da3..20bfdd7d8 100644 --- a/ros/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml +++ b/ros/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml @@ -8,7 +8,7 @@ map: min_cell_width: { meters: 0.01 } tree_height: 9 -operations: +operations_pipeline: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map diff --git a/ros/wavemap_ros/config/wavemap_ouster_os1.yaml b/ros/wavemap_ros/config/wavemap_ouster_os1.yaml index edf4b8e1e..a1a788059 100644 --- a/ros/wavemap_ros/config/wavemap_ouster_os1.yaml +++ b/ros/wavemap_ros/config/wavemap_ouster_os1.yaml @@ -7,7 +7,7 @@ map: type: wavelet_octree min_cell_width: { meters: 0.2 } -operations: +operations_pipeline: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map diff --git a/ros/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml b/ros/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml index 9c7536ad3..a206873d7 100644 --- a/ros/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml +++ b/ros/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml @@ -7,7 +7,7 @@ map: type: hashed_chunked_wavelet_octree min_cell_width: { meters: 0.02 } -operations: +operations_pipeline: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map diff --git a/ros/wavemap_ros/src/wavemap_server.cc b/ros/wavemap_ros/src/wavemap_server.cc index e7997534d..74b302069 100644 --- a/ros/wavemap_ros/src/wavemap_server.cc +++ b/ros/wavemap_ros/src/wavemap_server.cc @@ -66,7 +66,7 @@ WavemapServer::WavemapServer(ros::NodeHandle nh, ros::NodeHandle nh_private, // Setup operation handlers const param::Array operation_params_array = - param::convert::toParamArray(nh_private, "operations"); + param::convert::toParamArray(nh_private, "operations_pipeline"); for (const auto& operation_params : operation_params_array) { addOperation(operation_params, nh_private); } diff --git a/tooling/schemas/wavemap/operations/operation.json b/tooling/schemas/wavemap/operations/operations_pipeline.json similarity index 100% rename from tooling/schemas/wavemap/operations/operation.json rename to tooling/schemas/wavemap/operations/operations_pipeline.json diff --git a/tooling/schemas/wavemap/wavemap_config.json b/tooling/schemas/wavemap/wavemap_config.json index 7ce0761b9..97055ba0a 100644 --- a/tooling/schemas/wavemap/wavemap_config.json +++ b/tooling/schemas/wavemap/wavemap_config.json @@ -17,11 +17,11 @@ "$ref": "inputs/input.json" } }, - "operations": { + "operations_pipeline": { "description": "A list of operations to perform after map updates.", "type": "array", "items": { - "$ref": "operations/operation.json" + "$ref": "operations/operations_pipeline.json" } } }, From d67c840f84d0c00173e5b9981d83fe2b519e27fa Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 24 Nov 2023 16:47:28 +0100 Subject: [PATCH 026/159] Redesign tree data structure interfaces --- .../chunked_ndtree/chunked_ndtree.h | 16 +- .../chunked_ndtree/impl/chunked_ndtree_inl.h | 52 +++---- .../wavemap/data_structure/dense_block_hash.h | 46 +++--- .../impl/dense_block_hash_inl.h | 147 ++++++++++++------ .../data_structure/impl/spatial_hash_inl.h | 48 +++--- .../data_structure/ndtree/impl/ndtree_inl.h | 132 ++++++++-------- .../ndtree/impl/ndtree_node_inl.h | 56 ++++--- .../wavemap/data_structure/ndtree/ndtree.h | 40 +++-- .../data_structure/ndtree/ndtree_node.h | 26 ++-- .../wavemap/data_structure/spatial_hash.h | 12 +- .../include/wavemap/indexing/ndtree_index.h | 4 - .../impl/wavelet_integrator_inl.h | 2 +- .../wavemap/map/impl/hashed_blocks_inl.h | 6 +- .../wavemap/map/impl/volumetric_octree_inl.h | 16 +- .../wavemap/map/impl/wavelet_octree_inl.h | 12 +- .../hashed_wavelet_integrator.cc | 3 +- libraries/wavemap/src/map/hashed_blocks.cc | 7 +- .../src/map/hashed_wavelet_octree_block.cc | 14 +- .../wavemap/src/map/volumetric_octree.cc | 7 +- libraries/wavemap/src/map/wavelet_octree.cc | 2 +- .../utils/sdf/full_euclidean_sdf_generator.cc | 13 +- .../sdf/quasi_euclidean_sdf_generator.cc | 11 +- .../test/src/data_structure/test_ndtree.cc | 33 +++- .../src/iterator/test_subtree_iterator.cc | 28 ++-- .../test/src/utils/test_sdf_generation.cc | 14 +- .../wavemap_io/src/stream_conversions.cc | 4 +- .../test/src/test_file_conversions.cc | 10 +- .../src/map_msg_conversions.cc | 4 +- .../test/src/test_map_msg_conversions.cc | 10 +- 29 files changed, 431 insertions(+), 344 deletions(-) diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h index 69ceeb076..fef22b4a1 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h @@ -15,11 +15,12 @@ template class ChunkedNdtree { public: using IndexType = NdtreeIndex; + using HeightType = IndexElement; using NodeChunkType = NdtreeNodeChunk; using NodeDataType = NodeDataT; - static constexpr int kChunkHeight = chunk_height; + static constexpr HeightType kChunkHeight = chunk_height; - explicit ChunkedNdtree(int max_height); + explicit ChunkedNdtree(HeightType max_height); ~ChunkedNdtree() = default; bool empty() const { return root_chunk_.empty(); } @@ -27,12 +28,15 @@ class ChunkedNdtree { void clear() { root_chunk_.clear(); } void prune(); + HeightType getMaxHeight() const { return max_height_; } + size_t getMemoryUsage() const; + bool hasNode(const IndexType& index) const; - void allocateNode(const IndexType& index); + NodeDataT* getNodeData(const IndexType& index, bool auto_allocate = true); const NodeDataT* getNodeData(const IndexType& index) const; + void getOrAllocateNode(const IndexType& index); - int getMaxHeight() const { return max_height_; } NodeChunkType& getRootChunk() { return root_chunk_; } const NodeChunkType& getRootChunk() const { return root_chunk_; } @@ -45,11 +49,9 @@ class ChunkedNdtree { return Subtree(&root_chunk_); } - size_t getMemoryUsage() const; - private: NodeChunkType root_chunk_; - const int max_height_; + const HeightType max_height_; std::pair getChunkAndRelativeIndex( const IndexType& index, bool auto_allocate); diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h index 29b7231b9..70f7e1748 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h @@ -50,6 +50,30 @@ void ChunkedNdtree::prune() { } } +template +size_t ChunkedNdtree::getMemoryUsage() const { + size_t memory_usage = 0u; + + std::stack stack; + stack.emplace(&root_chunk_); + while (!stack.empty()) { + const NodeChunkType* chunk = stack.top(); + stack.pop(); + memory_usage += chunk->getMemoryUsage(); + + if (chunk->hasChildrenArray()) { + for (LinearIndex child_idx = 0; child_idx < NodeChunkType::kNumChildren; + ++child_idx) { + if (chunk->hasChild(child_idx)) { + stack.emplace(chunk->getChild(child_idx)); + } + } + } + } + + return memory_usage; +} + template bool ChunkedNdtree::hasNode( const ChunkedNdtree::IndexType& index) const { @@ -57,8 +81,8 @@ bool ChunkedNdtree::hasNode( } template -void ChunkedNdtree::allocateNode( - const ChunkedNdtree::IndexType& index) { +void ChunkedNdtree::getOrAllocateNode( + const IndexType& index) { getChunkAndRelativeIndex(index, /*auto_allocate*/ true); } @@ -82,30 +106,6 @@ const NodeDataT* ChunkedNdtree::getNodeData( return nullptr; } -template -size_t ChunkedNdtree::getMemoryUsage() const { - size_t memory_usage = 0u; - - std::stack stack; - stack.emplace(&root_chunk_); - while (!stack.empty()) { - const NodeChunkType* chunk = stack.top(); - stack.pop(); - memory_usage += chunk->getMemoryUsage(); - - if (chunk->hasChildrenArray()) { - for (LinearIndex child_idx = 0; child_idx < NodeChunkType::kNumChildren; - ++child_idx) { - if (chunk->hasChild(child_idx)) { - stack.emplace(chunk->getChild(child_idx)); - } - } - } - } - - return memory_usage; -} - template std::pair::NodeChunkType*, LinearIndex> diff --git a/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h b/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h index 9842d33ff..94b018871 100644 --- a/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h +++ b/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h @@ -14,8 +14,6 @@ class DenseBlockHash { int_math::log2_floor(cells_per_side); static constexpr IndexElement kDim = dim; - using BlockIndex = Index3D; - using CellIndex = Index3D; using Block = DenseGrid; using BlockHashMap = SpatialHash; @@ -27,25 +25,31 @@ class DenseBlockHash { size_t size() const { return Block::kCellsPerBlock * block_map_.size(); } void clear() { block_map_.clear(); } - bool hasBlock(const Index3D& block_index) const { - return block_map_.hasBlock(block_index); - } - Block* getBlock(const Index3D& block_index); - const Block* getBlock(const Index3D& block_index) const; - Block& getOrAllocateBlock(const Index3D& block_index); + bool hasBlock(const Index& block_index) const; + bool eraseBlock(const Index& block_index); + Block* getBlock(const Index& block_index); + const Block* getBlock(const Index& block_index) const; + Block& getOrAllocateBlock(const Index& block_index); - const CellDataT& getCellValue(const Index3D& index) const; - CellDataT& getOrAllocateCellValue(const Index3D& index); - const CellDataT& getDefaultCellValue() const { return default_value_; } + // NOTE: The values are stored in blocks. Therefore, allocating a value in a + // block that does not yet exist also allocates all its siblings. These + // will be initialized to default_value_. + // If you want to make sure a value has been explicitly set by you, + // check that it differs from the default_value_. For example, using the + // equalsDefaultValue(value) method below. + bool hasValue(const Index& index) const; + CellDataT* getValue(const Index& index); + const CellDataT* getValue(const Index& index) const; + CellDataT& getOrAllocateValue(const Index& index); + + const CellDataT& getValueOrDefault(const Index& index) const; + const CellDataT& getDefaultValue() const { return default_value_; } + bool equalsDefaultValue(const CellDataT& value) const; template - void forEachBlock(IndexedBlockVisitor visitor_fn) { - block_map_.forEachBlock(visitor_fn); - } + void forEachBlock(IndexedBlockVisitor visitor_fn); template - void forEachBlock(IndexedBlockVisitor visitor_fn) const { - block_map_.forEachBlock(visitor_fn); - } + void forEachBlock(IndexedBlockVisitor visitor_fn) const; template void forEachLeaf(IndexedLeafVisitorFunction visitor_fn); @@ -57,10 +61,10 @@ class DenseBlockHash { const CellDataT default_value_; BlockHashMap block_map_; - static Index3D indexToBlockIndex(const Index3D& index); - static Index3D indexToCellIndex(const Index3D& index); - static Index3D cellAndBlockIndexToIndex(const Index3D& block_index, - const Index3D& cell_index); + static Index indexToBlockIndex(const Index& index); + static Index indexToCellIndex(const Index& index); + static Index cellAndBlockIndexToIndex(const Index& block_index, + const Index& cell_index); }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/data_structure/impl/dense_block_hash_inl.h b/libraries/wavemap/include/wavemap/data_structure/impl/dense_block_hash_inl.h index ee22957e2..a7653eeed 100644 --- a/libraries/wavemap/include/wavemap/data_structure/impl/dense_block_hash_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/impl/dense_block_hash_inl.h @@ -2,101 +2,156 @@ #define WAVEMAP_DATA_STRUCTURE_IMPL_DENSE_BLOCK_HASH_INL_H_ namespace wavemap { +template +bool DenseBlockHash::hasBlock( + const Index& block_index) const { + return block_map_.hasBlock(block_index); +} + +template +bool DenseBlockHash::eraseBlock( + const Index& block_index) { + return block_map_.eraseBlock(block_index); +} + template inline DenseGrid* DenseBlockHash::getBlock( - const Index3D& block_index) { + const Index& block_index) { return block_map_.getBlock(block_index); } template inline const DenseGrid* DenseBlockHash::getBlock( - const Index3D& block_index) const { + const Index& block_index) const { return block_map_.getBlock(block_index); } template inline DenseGrid& DenseBlockHash::getOrAllocateBlock( - const Index3D& block_index) { + const Index& block_index) { return block_map_.getOrAllocateBlock(block_index, default_value_); } -template -inline const CellDataT& -DenseBlockHash::getCellValue( - const wavemap::Index3D& index) const { - const BlockIndex block_index = indexToBlockIndex(index); - const auto* block = getBlock(block_index); - if (!block) { - return default_value_; +template +bool DenseBlockHash::hasValue( + const Index& index) const { + return getValue(index); +} + +template +CellDataT* DenseBlockHash::getValue( + const Index& index) { + return const_cast(std::as_const(*this).getValue(index)); +} + +template +const CellDataT* DenseBlockHash::getValue( + const Index& index) const { + const Index block_index = indexToBlockIndex(index); + if (const Block* block = getBlock(block_index); block) { + const Index cell_index = indexToCellIndex(index); + return &block->at(cell_index); } - const CellIndex cell_index = indexToCellIndex(index); - return block->at(cell_index); + return nullptr; } template inline CellDataT& -DenseBlockHash::getOrAllocateCellValue( - const Index3D& index) { - const BlockIndex block_index = indexToBlockIndex(index); - auto& block = getOrAllocateBlock(block_index); - const CellIndex cell_index = indexToCellIndex(index); +DenseBlockHash::getOrAllocateValue( + const Index& index) { + const Index block_index = indexToBlockIndex(index); + Block& block = getOrAllocateBlock(block_index); + const Index cell_index = indexToCellIndex(index); return block.at(cell_index); } +template +const CellDataT& +DenseBlockHash::getValueOrDefault( + const Index& index) const { + if (const CellDataT* value = getValue(index); value) { + return *value; + } + return default_value_; +} + +template +bool DenseBlockHash::equalsDefaultValue( + const CellDataT& value) const { + return value == default_value_; +} + +template +template +void DenseBlockHash::forEachBlock( + IndexedBlockVisitor visitor_fn) { + block_map_.forEachBlock(visitor_fn); +} + +template +template +void DenseBlockHash::forEachBlock( + IndexedBlockVisitor visitor_fn) const { + block_map_.forEachBlock(visitor_fn); +} + template template void DenseBlockHash::forEachLeaf( IndexedLeafVisitorFunction visitor_fn) { - block_map_.forEachBlock([&visitor_fn](const BlockIndex& block_index, - const Block& block) { - for (LinearIndex cell_idx = 0u; cell_idx < Block::kCellsPerBlock; - ++cell_idx) { - const Index3D cell_index = - convert::linearIndexToIndex(cell_idx); - const Index3D index = cellAndBlockIndexToIndex(block_index, cell_index); - auto& cell_data = block[cell_idx]; - std::invoke(visitor_fn, index, cell_data); - } - }); + block_map_.forEachBlock( + [&visitor_fn](const Index& block_index, Block& block) { + for (LinearIndex cell_idx = 0u; cell_idx < Block::kCellsPerBlock; + ++cell_idx) { + const Index cell_index = + convert::linearIndexToIndex(cell_idx); + const Index index = + cellAndBlockIndexToIndex(block_index, cell_index); + CellDataT& cell_data = block[cell_idx]; + std::invoke(visitor_fn, index, cell_data); + } + }); } template template void DenseBlockHash::forEachLeaf( IndexedLeafVisitorFunction visitor_fn) const { - block_map_.forEachBlock([&visitor_fn](const BlockIndex& block_index, - const Block& block) { - for (LinearIndex cell_idx = 0u; cell_idx < Block::kCellsPerBlock; - ++cell_idx) { - const Index3D cell_index = - convert::linearIndexToIndex(cell_idx); - const Index3D index = cellAndBlockIndexToIndex(block_index, cell_index); - const auto& cell_data = block[cell_idx]; - std::invoke(visitor_fn, index, cell_data); - } - }); + block_map_.forEachBlock( + [&visitor_fn](const Index& block_index, const Block& block) { + for (LinearIndex cell_idx = 0u; cell_idx < Block::kCellsPerBlock; + ++cell_idx) { + const Index cell_index = + convert::linearIndexToIndex(cell_idx); + const Index index = + cellAndBlockIndexToIndex(block_index, cell_index); + const CellDataT& cell_data = block[cell_idx]; + std::invoke(visitor_fn, index, cell_data); + } + }); } template -inline Index3D +inline Index DenseBlockHash::indexToBlockIndex( - const Index3D& index) { + const Index& index) { return convert::indexToBlockIndex(index, kCellsPerSideLog2); } template -inline Index3D DenseBlockHash::indexToCellIndex( - const Index3D& index) { +inline Index +DenseBlockHash::indexToCellIndex( + const Index& index) { return int_math::div_exp2_floor_remainder(index, kCellsPerSideLog2); } template -inline Index3D +inline Index DenseBlockHash::cellAndBlockIndexToIndex( - const Index3D& block_index, const Index3D& cell_index) { + const Index& block_index, const Index& cell_index) { return kCellsPerSide * block_index + cell_index; } diff --git a/libraries/wavemap/include/wavemap/data_structure/impl/spatial_hash_inl.h b/libraries/wavemap/include/wavemap/data_structure/impl/spatial_hash_inl.h index 6f9cc7223..b5bc57ef7 100644 --- a/libraries/wavemap/include/wavemap/data_structure/impl/spatial_hash_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/impl/spatial_hash_inl.h @@ -49,6 +49,27 @@ bool SpatialHash::hasBlock( return block_map_.count(block_index); } +template +bool SpatialHash::eraseBlock( + const SpatialHash::BlockIndex& block_index) { + return block_map_.erase(block_index); +} + +template +template +void SpatialHash::eraseBlockIf( + IndexedBlockVisitor indicator_fn) { + for (auto it = block_map_.begin(); it != block_map_.end();) { + const BlockIndex& block_index = it->first; + BlockData& block = it->second; + if (std::invoke(indicator_fn, block_index, block)) { + it = block_map_.erase(it); + } else { + ++it; + } + } +} + template typename SpatialHash::BlockData* SpatialHash::getBlock( @@ -74,35 +95,14 @@ SpatialHash::getBlock( } template -template +template typename SpatialHash::BlockData& SpatialHash::getOrAllocateBlock( - const SpatialHash::BlockIndex& block_index, Args... args) { - return block_map_.try_emplace(block_index, std::forward(args)...) + const SpatialHash::BlockIndex& block_index, DefaultArgs&&... args) { + return block_map_.try_emplace(block_index, std::forward(args)...) .first->second; } -template -void SpatialHash::eraseBlock( - const SpatialHash::BlockIndex& block_index) { - block_map_.erase(block_index); -} - -template -template -void SpatialHash::eraseBlockIf( - IndexedBlockVisitor indicator_fn) { - for (auto it = block_map_.begin(); it != block_map_.end();) { - const BlockIndex& block_index = it->first; - BlockData& block = it->second; - if (std::invoke(indicator_fn, block_index, block)) { - it = block_map_.erase(it); - } else { - ++it; - } - } -} - template template void SpatialHash::forEachBlock( diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_inl.h b/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_inl.h index c5f078bfc..cd593f32d 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_inl.h @@ -2,6 +2,7 @@ #define WAVEMAP_DATA_STRUCTURE_NDTREE_IMPL_NDTREE_INL_H_ #include +#include #include #include "wavemap/data_structure/pointcloud.h" @@ -9,7 +10,9 @@ namespace wavemap { template -Ndtree::Ndtree(int max_height) : max_height_(max_height) { +template +Ndtree::Ndtree(int max_height, RootNodeArgs&&... args) + : max_height_(max_height), root_node_(std::forward(args)...) { CHECK_LE(max_height_, morton::kMaxTreeHeight); } @@ -29,13 +32,12 @@ void Ndtree::prune() { NodeType* child_ptr = node.getChild(child_idx); if (child_ptr) { if (child_ptr->empty()) { - node.deleteChild(child_idx); + node.eraseChild(child_idx); } else { has_non_empty_child = true; } } } - // Free up the children array if it only contains null pointers if (!has_non_empty_child) { node.deleteChildrenArray(); @@ -47,97 +49,103 @@ void Ndtree::prune() { template size_t Ndtree::getMemoryUsage() const { size_t memory_usage = 0u; - - std::stack stack; - stack.emplace(&root_node_); - while (!stack.empty()) { - const NodeType* node = stack.top(); - stack.pop(); - memory_usage += node->getMemoryUsage(); - - if (node->hasChildrenArray()) { - for (NdtreeIndexRelativeChild child_idx = 0; - child_idx < NodeType::kNumChildren; ++child_idx) { - if (node->hasChild(child_idx)) { - stack.emplace(node->getChild(child_idx)); - } - } - } + for (const NodeType& node : + getIterator()) { + memory_usage += node.getMemoryUsage(); } - return memory_usage; } template -bool Ndtree::deleteNode(const IndexType& index) { +bool Ndtree::eraseNode(const IndexType& index) { IndexType parent_index = index.computeParentIndex(); - NodeType* parent_node = getNode(parent_index, /*auto_allocate*/ false); + NodeType* parent_node = getNode(parent_index); if (parent_node) { - return parent_node->deleteChild(index.computeRelativeChildIndex()); + return parent_node->eraseChild(index.computeRelativeChildIndex()); } return false; } template -NodeDataT* Ndtree::getNodeData(const Ndtree::IndexType& index, - bool auto_allocate) { - if (NodeType* node = getNode(index, auto_allocate); node) { - return &node->data(); - } - return nullptr; +typename Ndtree::NodeType* Ndtree::getNode( + const IndexType& index) { + return const_cast(std::as_const(*this).getNode(index)); } template -const NodeDataT* Ndtree::getNodeData( - const Ndtree::IndexType& index) const { - if (const NodeType* node = getNode(index); node) { - return &node->data(); +const typename Ndtree::NodeType* +Ndtree::getNode(const IndexType& index) const { + const NodeType* node = &root_node_; + const MortonIndex morton_code = convert::nodeIndexToMorton(index); + for (int node_height = max_height_; index.height < node_height; + --node_height) { + const NdtreeIndexRelativeChild child_index = + NdtreeIndex::computeRelativeChildIndex(morton_code, node_height); + // Check if the child is allocated + const NodeType* child = node->getChild(child_index); + if (!child) { + return nullptr; + } + node = child; } - return nullptr; + return node; } template -typename Ndtree::NodeType* Ndtree::getNode( - const IndexType& index, bool auto_allocate) { - NodeType* current_parent = &root_node_; +template +typename Ndtree::NodeType& +Ndtree::getOrAllocateNode(const Ndtree::IndexType& index, + DefaultArgs&&... args) { + NodeType* node = &root_node_; const MortonIndex morton_code = convert::nodeIndexToMorton(index); - for (int parent_height = max_height_; index.height < parent_height; - --parent_height) { + for (int node_height = max_height_; index.height < node_height; + --node_height) { const NdtreeIndexRelativeChild child_index = - NdtreeIndex::computeRelativeChildIndex(morton_code, parent_height); - // Check if the child is allocated - if (!current_parent->hasChild(child_index)) { - if (auto_allocate) { - current_parent->allocateChild(child_index); - } else { - return nullptr; - } - } - - current_parent = current_parent->getChild(child_index); + NdtreeIndex::computeRelativeChildIndex(morton_code, node_height); + // Get the child, allocating if needed + node = &node->getOrAllocateChild(child_index, + std::forward(args)...); } + return *node; +} - return current_parent; +template +std::pair::NodeType*, IndexElement> +Ndtree::getNodeOrAncestor(const Ndtree::IndexType& index) { + auto rv = std::as_const(*this).getNodeOrAncestor(index); + return {const_cast(rv.first), rv.second}; } template -const typename Ndtree::NodeType* -Ndtree::getNode(const IndexType& index) const { - const NodeType* current_parent = &root_node_; +std::pair::NodeType*, IndexElement> +Ndtree::getNodeOrAncestor( + const Ndtree::IndexType& index) const { + const NodeType* node = &root_node_; const MortonIndex morton_code = convert::nodeIndexToMorton(index); - for (int parent_height = max_height_; index.height < parent_height; - --parent_height) { + for (int node_height = max_height_; index.height < node_height; + --node_height) { const NdtreeIndexRelativeChild child_index = - NdtreeIndex::computeRelativeChildIndex(morton_code, parent_height); + NdtreeIndex::computeRelativeChildIndex(morton_code, node_height); // Check if the child is allocated - if (!current_parent->hasChild(child_index)) { - return nullptr; + const NodeType* child = node->getChild(child_index); + if (!child) { + return {node, node_height}; } - - current_parent = current_parent->getChild(child_index); + node = child; } + return {node, index.height}; +} - return current_parent; +template +template +auto Ndtree::getIterator() { + return Subtree(&root_node_); +} + +template +template +auto Ndtree::getIterator() const { + return Subtree(&root_node_); } } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_node_inl.h b/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_node_inl.h index 3e8e11b08..5501cc959 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_node_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_node_inl.h @@ -18,6 +18,15 @@ void NdtreeNode::clear() { data() = DataT{}; } +template +size_t NdtreeNode::getMemoryUsage() const { + size_t memory_usage = sizeof(NdtreeNode); + if (hasChildrenArray()) { + memory_usage += sizeof(ChildrenArray); + } + return memory_usage; +} + template bool NdtreeNode::hasNonzeroData() const { return data::is_nonzero(data_); @@ -28,12 +37,6 @@ bool NdtreeNode::hasNonzeroData(FloatingPoint threshold) const { return data::is_nonzero(data_, threshold); } -template -bool NdtreeNode::hasChild( - NdtreeIndexRelativeChild child_index) const { - return getChild(child_index); -} - template bool NdtreeNode::hasAtLeastOneChild() const { if (hasChildrenArray()) { @@ -45,22 +48,16 @@ bool NdtreeNode::hasAtLeastOneChild() const { } template -template -NdtreeNode* NdtreeNode::allocateChild( - NdtreeIndexRelativeChild child_index, NodeConstructorArgs&&... args) { - CHECK_GE(child_index, 0u); - CHECK_LT(child_index, kNumChildren); - if (!hasChildrenArray()) { - children_ = std::make_unique(); - } - children_->operator[](child_index) = - std::make_unique(std::forward(args)...); - return children_->operator[](child_index).get(); +bool NdtreeNode::hasChild( + NdtreeIndexRelativeChild child_index) const { + return getChild(child_index); } template -bool NdtreeNode::deleteChild(NdtreeIndexRelativeChild child_index) { - if (hasChild(child_index)) { +bool NdtreeNode::eraseChild(NdtreeIndexRelativeChild child_index) { + CHECK_GE(child_index, 0u); + CHECK_LT(child_index, kNumChildren); + if (hasChildrenArray()) { children_->operator[](child_index).reset(); return true; } @@ -88,12 +85,23 @@ const NdtreeNode* NdtreeNode::getChild( } template -size_t NdtreeNode::getMemoryUsage() const { - size_t memory_usage = sizeof(NdtreeNode); - if (hasChildrenArray()) { - memory_usage += sizeof(ChildrenArray); +template +NdtreeNode& NdtreeNode::getOrAllocateChild( + NdtreeIndexRelativeChild child_index, DefaultArgs&&... args) { + CHECK_GE(child_index, 0u); + CHECK_LT(child_index, kNumChildren); + // Make sure the children array is allocated + if (!hasChildrenArray()) { + children_ = std::make_unique(); } - return memory_usage; + // Get the child, allocating it if needed + ChildPtr& child_smart_ptr = children_->operator[](child_index); + if (!child_smart_ptr) { + child_smart_ptr = + std::make_unique(std::forward(args)...); + } + // Return a reference to the child + return *child_smart_ptr; } } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree.h b/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree.h index b659c19af..25edd46f6 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree.h +++ b/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree.h @@ -15,11 +15,13 @@ template class Ndtree { public: using IndexType = NdtreeIndex; + using HeightType = IndexElement; using NodeType = NdtreeNode; using NodeDataType = NodeDataT; - static constexpr int kChunkHeight = 1; + static constexpr HeightType kChunkHeight = 1; - explicit Ndtree(int max_height); + template + explicit Ndtree(HeightType max_height, RootNodeArgs&&... args); ~Ndtree() = default; bool empty() const { return root_node_.empty(); } @@ -27,35 +29,31 @@ class Ndtree { void clear() { root_node_.clear(); } void prune(); + HeightType getMaxHeight() const { return max_height_; } + size_t getMemoryUsage() const; + bool hasNode(const IndexType& index) const { return getNode(index); } - void allocateNode(const IndexType& index) { - getNode(index, /*auto_allocate*/ true); - } - bool deleteNode(const IndexType& index); - NodeDataT* getNodeData(const IndexType& index, bool auto_allocate = true); - const NodeDataT* getNodeData(const IndexType& index) const; + bool eraseNode(const IndexType& index); + NodeType* getNode(const IndexType& index); + const NodeType* getNode(const IndexType& index) const; + template + NodeType& getOrAllocateNode(const IndexType& index, DefaultArgs&&... args); + + std::pair getNodeOrAncestor(const IndexType& index); + std::pair getNodeOrAncestor( + const IndexType& index) const; - int getMaxHeight() const { return max_height_; } NodeType& getRootNode() { return root_node_; } const NodeType& getRootNode() const { return root_node_; } template - auto getIterator() { - return Subtree(&root_node_); - } + auto getIterator(); template - auto getIterator() const { - return Subtree(&root_node_); - } - - size_t getMemoryUsage() const; + auto getIterator() const; private: + const HeightType max_height_; NodeType root_node_; - const int max_height_; - - NodeType* getNode(const IndexType& index, bool auto_allocate); - const NodeType* getNode(const IndexType& index) const; }; template diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree_node.h b/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree_node.h index aeac1c1eb..b82b8da71 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree_node.h +++ b/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree_node.h @@ -3,6 +3,7 @@ #include #include +#include #include "wavemap/common.h" #include "wavemap/indexing/ndtree_index.h" @@ -15,15 +16,14 @@ class NdtreeNode { static constexpr int kNumChildren = NdtreeIndex::kNumChildren; NdtreeNode() = default; - explicit NdtreeNode(DataT data) : data_(data) {} + template + explicit NdtreeNode(Args&&... args) : data_(std::forward(args)...) {} ~NdtreeNode() = default; bool empty() const; void clear(); - friend bool operator==(const NdtreeNode& lhs, const NdtreeNode& rhs) { - return &rhs == &lhs; - } + size_t getMemoryUsage() const; bool hasNonzeroData() const; bool hasNonzeroData(FloatingPoint threshold) const; @@ -31,21 +31,25 @@ class NdtreeNode { const DataT& data() const { return data_; } bool hasChildrenArray() const { return static_cast(children_); } + bool hasAtLeastOneChild() const; void deleteChildrenArray() { children_.reset(); } bool hasChild(NdtreeIndexRelativeChild child_index) const; - bool hasAtLeastOneChild() const; - template - NdtreeNode* allocateChild(NdtreeIndexRelativeChild child_index, - NodeConstructorArgs&&... args); - bool deleteChild(NdtreeIndexRelativeChild child_index); + bool eraseChild(NdtreeIndexRelativeChild child_index); + NdtreeNode* getChild(NdtreeIndexRelativeChild child_index); const NdtreeNode* getChild(NdtreeIndexRelativeChild child_index) const; + template + NdtreeNode& getOrAllocateChild(NdtreeIndexRelativeChild child_index, + DefaultArgs&&... args); - size_t getMemoryUsage() const; + friend bool operator==(const NdtreeNode& lhs, const NdtreeNode& rhs) { + return &rhs == &lhs; + } private: - using ChildrenArray = std::array, kNumChildren>; + using ChildPtr = std::unique_ptr; + using ChildrenArray = std::array; DataT data_{}; std::unique_ptr children_; diff --git a/libraries/wavemap/include/wavemap/data_structure/spatial_hash.h b/libraries/wavemap/include/wavemap/data_structure/spatial_hash.h index 54790135a..2499a55b1 100644 --- a/libraries/wavemap/include/wavemap/data_structure/spatial_hash.h +++ b/libraries/wavemap/include/wavemap/data_structure/spatial_hash.h @@ -31,15 +31,15 @@ class SpatialHash { Index getMaxBlockIndex() const; bool hasBlock(const BlockIndex& block_index) const; + bool eraseBlock(const BlockIndex& block_index); + template + void eraseBlockIf(IndexedBlockVisitor indicator_fn); BlockData* getBlock(const BlockIndex& block_index); const BlockData* getBlock(const BlockIndex& block_index) const; - template - BlockData& getOrAllocateBlock(const BlockIndex& block_index, Args... args); - - void eraseBlock(const BlockIndex& block_index); - template - void eraseBlockIf(IndexedBlockVisitor indicator_fn); + template + BlockData& getOrAllocateBlock(const BlockIndex& block_index, + DefaultArgs&&... args); auto& getHashMap() { return block_map_; } const auto& getHashMap() const { return block_map_; } diff --git a/libraries/wavemap/include/wavemap/indexing/ndtree_index.h b/libraries/wavemap/include/wavemap/indexing/ndtree_index.h index c199a5dca..b1eb40505 100644 --- a/libraries/wavemap/include/wavemap/indexing/ndtree_index.h +++ b/libraries/wavemap/include/wavemap/indexing/ndtree_index.h @@ -11,10 +11,6 @@ namespace wavemap { using NdtreeIndexRelativeChild = uint8_t; -// TODO(victorr): Consider renaming NdtreeIndex to something like -// HierarchicalNdIndex, since it can represent hierarchical -// quadtrant/octant subvolumes regardless of the exact data -// structure that's being indexed. template struct NdtreeIndex { using Element = IndexElement; diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h index 592e3e664..3a867cc97 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h @@ -73,7 +73,7 @@ inline FloatingPoint WaveletIntegrator::recursiveSamplerCompressor( // NOLINT // Since the approximation error would still be too big, refine if (!node) { // Allocate the current node if it has not yet been allocated - node = parent_node.allocateChild(relative_child_index); + node = &parent_node.getOrAllocateChild(relative_child_index); } const WaveletOctree::Coefficients::CoefficientsArray child_scale_coefficients = diff --git a/libraries/wavemap/include/wavemap/map/impl/hashed_blocks_inl.h b/libraries/wavemap/include/wavemap/map/impl/hashed_blocks_inl.h index 47ab8a7f6..f7f318133 100644 --- a/libraries/wavemap/include/wavemap/map/impl/hashed_blocks_inl.h +++ b/libraries/wavemap/include/wavemap/map/impl/hashed_blocks_inl.h @@ -10,17 +10,17 @@ namespace wavemap { inline FloatingPoint HashedBlocks::getCellValue(const Index3D& index) const { - return DenseBlockHash::getCellValue(index); + return getValueOrDefault(index); } inline void HashedBlocks::setCellValue(const Index3D& index, FloatingPoint new_value) { - getOrAllocateCellValue(index) = new_value; + getOrAllocateValue(index) = new_value; } inline void HashedBlocks::addToCellValue(const Index3D& index, FloatingPoint update) { - FloatingPoint& cell_data = getOrAllocateCellValue(index); + FloatingPoint& cell_data = getOrAllocateValue(index); cell_data = clampedAdd(cell_data, update); } } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/map/impl/volumetric_octree_inl.h b/libraries/wavemap/include/wavemap/map/impl/volumetric_octree_inl.h index 646e5a133..0e033a44f 100644 --- a/libraries/wavemap/include/wavemap/map/impl/volumetric_octree_inl.h +++ b/libraries/wavemap/include/wavemap/map/impl/volumetric_octree_inl.h @@ -43,13 +43,9 @@ inline void VolumetricOctree::setCellValue(const Index3D& index, inline void VolumetricOctree::setCellValue(const OctreeIndex& node_index, FloatingPoint new_value) { - constexpr bool kAutoAllocate = true; const OctreeIndex internal_index = toInternal(node_index); - if (auto* data = ndtree_.getNodeData(internal_index, kAutoAllocate); data) { - *data = new_value; - } else { - LOG(ERROR) << "Failed to allocate cell at index: " << node_index.toString(); - } + auto& data = ndtree_.getOrAllocateNode(internal_index).data(); + data = new_value; } inline void VolumetricOctree::addToCellValue(const Index3D& index, @@ -60,13 +56,9 @@ inline void VolumetricOctree::addToCellValue(const Index3D& index, inline void VolumetricOctree::addToCellValue(const OctreeIndex& node_index, FloatingPoint update) { - constexpr bool kAutoAllocate = true; const OctreeIndex internal_index = toInternal(node_index); - if (auto* data = ndtree_.getNodeData(internal_index, kAutoAllocate); data) { - *data += update; - } else { - LOG(ERROR) << "Failed to allocate cell at index: " << node_index.toString(); - } + auto& data = ndtree_.getOrAllocateNode(internal_index).data(); + data += update; } inline const VolumetricOctree::NodeType* diff --git a/libraries/wavemap/include/wavemap/map/impl/wavelet_octree_inl.h b/libraries/wavemap/include/wavemap/map/impl/wavelet_octree_inl.h index 7e316fbc7..69e373a9b 100644 --- a/libraries/wavemap/include/wavemap/map/impl/wavelet_octree_inl.h +++ b/libraries/wavemap/include/wavemap/map/impl/wavelet_octree_inl.h @@ -88,10 +88,8 @@ inline void WaveletOctree::setCellValue(const OctreeIndex& index, NodeType* current_parent = node_ptrs.back(); current_value = Transform::backwardSingleChild( {current_value, current_parent->data()}, child_index); - if (!current_parent->hasChild(child_index)) { - current_parent->allocateChild(child_index); - } - node_ptrs.emplace_back(current_parent->getChild(child_index)); + NodeType& child = current_parent->getOrAllocateChild(child_index); + node_ptrs.emplace_back(&child); } DCHECK_EQ(node_ptrs.size(), height_difference); @@ -130,10 +128,8 @@ inline void WaveletOctree::addToCellValue(const OctreeIndex& index, const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); NodeType* current_parent = node_ptrs.back(); - if (!current_parent->hasChild(child_index)) { - current_parent->allocateChild(child_index); - } - node_ptrs.emplace_back(current_parent->getChild(child_index)); + NodeType& child = current_parent->getOrAllocateChild(child_index); + node_ptrs.emplace_back(&child); } DCHECK_EQ(node_ptrs.size(), height_difference); diff --git a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc index e9bb0e6f5..ac140a35a 100644 --- a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc +++ b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc @@ -186,7 +186,8 @@ void HashedWaveletIntegrator::updateBlock(HashedWaveletOctree::Block& block, // Since the approximation error would still be too big, refine if (!node) { // Allocate the current node if it has not yet been allocated - node = parent_node.allocateChild(node_index.computeRelativeChildIndex()); + node = &parent_node.getOrAllocateChild( + node_index.computeRelativeChildIndex()); } stack.emplace(StackElement{*node, node_index, 0, HashedWaveletOctreeBlock::Transform::backward( diff --git a/libraries/wavemap/src/map/hashed_blocks.cc b/libraries/wavemap/src/map/hashed_blocks.cc index 6cc0cd7f0..101469418 100644 --- a/libraries/wavemap/src/map/hashed_blocks.cc +++ b/libraries/wavemap/src/map/hashed_blocks.cc @@ -14,11 +14,10 @@ Index3D HashedBlocks::getMaxIndex() const { void HashedBlocks::prune() { block_map_.eraseBlockIf( - [default_value = default_value_](const BlockIndex& /*block_index*/, - const Block& block) { + [this](const Index3D& /*block_index*/, const Block& block) { return std::all_of(block.data().cbegin(), block.data().cend(), - [default_value](const auto& cell_value) { - return cell_value == default_value; + [this](const auto& cell_value) { + return equalsDefaultValue(cell_value); }); }); } diff --git a/libraries/wavemap/src/map/hashed_wavelet_octree_block.cc b/libraries/wavemap/src/map/hashed_wavelet_octree_block.cc index 8b21355a8..c34f5a1a0 100644 --- a/libraries/wavemap/src/map/hashed_wavelet_octree_block.cc +++ b/libraries/wavemap/src/map/hashed_wavelet_octree_block.cc @@ -46,10 +46,8 @@ void HashedWaveletOctreeBlock::setCellValue(const OctreeIndex& index, NodeType* current_parent = node_ptrs.back(); current_value = Transform::backwardSingleChild( {current_value, current_parent->data()}, child_index); - if (!current_parent->hasChild(child_index)) { - current_parent->allocateChild(child_index); - } - node_ptrs.emplace_back(current_parent->getChild(child_index)); + NodeType& child = current_parent->getOrAllocateChild(child_index); + node_ptrs.emplace_back(&child); } DCHECK_EQ(node_ptrs.size(), height_difference); @@ -84,10 +82,8 @@ void HashedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index, const NdtreeIndexRelativeChild child_index = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); NodeType* current_parent = node_ptrs.back(); - if (!current_parent->hasChild(child_index)) { - current_parent->allocateChild(child_index); - } - node_ptrs.emplace_back(current_parent->getChild(child_index)); + NodeType& child = current_parent->getOrAllocateChild(child_index); + node_ptrs.emplace_back(&child); } DCHECK_EQ(node_ptrs.size(), height_difference); @@ -182,7 +178,7 @@ void HashedWaveletOctreeBlock::recursivePrune( // NOLINT NodeType& child_node = *node.getChild(child_idx); recursivePrune(child_node); if (!child_node.hasChildrenArray() && !child_node.hasNonzeroData(1e-3f)) { - node.deleteChild(child_idx); + node.eraseChild(child_idx); } else { has_at_least_one_child = true; } diff --git a/libraries/wavemap/src/map/volumetric_octree.cc b/libraries/wavemap/src/map/volumetric_octree.cc index bb06fb127..c76daa34c 100644 --- a/libraries/wavemap/src/map/volumetric_octree.cc +++ b/libraries/wavemap/src/map/volumetric_octree.cc @@ -33,11 +33,12 @@ void VolumetricOctree::prune() { if (node.hasChildrenArray()) { for (NdtreeIndexRelativeChild child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { - if (node.hasChild(child_idx)) { - recursive_fn(node_value, *node.getChild(child_idx)); + NodeType* child = node.getChild(child_idx); + if (child) { + recursive_fn(node_value, *child); } else if (kEpsilon < std::abs(node_value)) { // Always propagate non-zero internal node value down to leaves - recursive_fn(node_value, *node.allocateChild(child_idx)); + recursive_fn(node_value, node.getOrAllocateChild(child_idx)); } } } diff --git a/libraries/wavemap/src/map/wavelet_octree.cc b/libraries/wavemap/src/map/wavelet_octree.cc index 87fde1c62..1783f2810 100644 --- a/libraries/wavemap/src/map/wavelet_octree.cc +++ b/libraries/wavemap/src/map/wavelet_octree.cc @@ -138,7 +138,7 @@ WaveletOctree::Coefficients::Scale WaveletOctree::recursivePrune( // NOLINT child_scale_coefficients[child_idx] = recursivePrune(child_node, child_scale_coefficients[child_idx]); if (!child_node.hasChildrenArray() && !child_node.hasNonzeroData(1e-3f)) { - node.deleteChild(child_idx); + node.eraseChild(child_idx); } else { has_at_least_one_child = true; } diff --git a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc index a30996991..7832ee001 100644 --- a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc @@ -86,9 +86,8 @@ void FullEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, } // Get the voxel's SDF value - auto& [sdf_parent, sdf_value] = sdf.getOrAllocateCellValue(index); - const bool sdf_uninitialized = - sdf_parent == sdf.getDefaultCellValue().parent; + auto& [sdf_parent, sdf_value] = sdf.getOrAllocateValue(index); + const bool sdf_uninitialized = sdf_parent == sdf.getDefaultValue().parent; // Update the voxel's SDF value const FloatingPoint distance_to_surface = @@ -122,7 +121,7 @@ void FullEuclideanSDFGenerator::propagate( while (!open_queue.empty()) { TracyPlot("QueueLength", static_cast(open_queue.size())); const Index3D index = open_queue.front(); - const auto& [sdf_parent, sdf_value] = sdf.getCellValue(index); + const auto& [sdf_parent, sdf_value] = sdf.getValueOrDefault(index); const FloatingPoint df_value = std::abs(sdf_value); TracyPlot("Distance", df_value); open_queue.pop(); @@ -138,11 +137,11 @@ void FullEuclideanSDFGenerator::propagate( // Get the neighbor's SDF value auto& [neighbor_sdf_parent, neighbor_sdf_value] = - sdf.getOrAllocateCellValue(neighbor_index); + sdf.getOrAllocateValue(neighbor_index); // If the neighbor is uninitialized, get its sign from the occupancy map const bool neighbor_uninitialized = - neighbor_sdf_parent == sdf.getDefaultCellValue().parent; + neighbor_sdf_parent == sdf.getDefaultValue().parent; if (neighbor_uninitialized) { const FloatingPoint neighbor_occupancy = occupancy_query_accelerator.getCellValue(neighbor_index); @@ -152,7 +151,7 @@ void FullEuclideanSDFGenerator::propagate( } // Set the sign if (classifier_.isOccupied(neighbor_occupancy)) { - neighbor_sdf_value = -sdf.getDefaultCellValue().distance; + neighbor_sdf_value = -sdf.getDefaultValue().distance; } } diff --git a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc index b687a1de8..f81579be9 100644 --- a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc @@ -76,8 +76,8 @@ void QuasiEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, } // Get the voxel's SDF value - FloatingPoint& sdf_value = sdf.getOrAllocateCellValue(index); - const bool sdf_uninitialized = sdf.getDefaultCellValue() == sdf_value; + FloatingPoint& sdf_value = sdf.getOrAllocateValue(index); + const bool sdf_uninitialized = sdf.getDefaultValue() == sdf_value; // Update the voxel's SDF value const FloatingPoint distance_to_surface = @@ -129,11 +129,10 @@ void QuasiEuclideanSDFGenerator::propagate( // Get the neighbor's SDF value const Index3D neighbor_index = index + kNeighborIndexOffsets[neighbor_idx]; - FloatingPoint& neighbor_sdf = sdf.getOrAllocateCellValue(neighbor_index); + FloatingPoint& neighbor_sdf = sdf.getOrAllocateValue(neighbor_index); // If the neighbor is uninitialized, get its sign from the occupancy map - const bool neighbor_uninitialized = - sdf.getDefaultCellValue() == neighbor_sdf; + const bool neighbor_uninitialized = sdf.getDefaultValue() == neighbor_sdf; if (neighbor_uninitialized) { const FloatingPoint neighbor_occupancy = occupancy_query_accelerator.getCellValue(neighbor_index); @@ -143,7 +142,7 @@ void QuasiEuclideanSDFGenerator::propagate( } // Set the sign if (classifier_.isOccupied(neighbor_occupancy)) { - neighbor_sdf = -sdf.getDefaultCellValue(); + neighbor_sdf = -sdf.getDefaultValue(); } } diff --git a/libraries/wavemap/test/src/data_structure/test_ndtree.cc b/libraries/wavemap/test/src/data_structure/test_ndtree.cc index 96b677c95..55706539f 100644 --- a/libraries/wavemap/test/src/data_structure/test_ndtree.cc +++ b/libraries/wavemap/test/src/data_structure/test_ndtree.cc @@ -42,7 +42,7 @@ TYPED_TEST(NdtreeTest, AllocatingAndClearing) { EXPECT_EQ(ndtree.hasNode(random_index), index_is_inside_root_chunk_node) << random_index.toString() << " and tree height " << tree_height; - ndtree.allocateNode(random_index); + ndtree.getOrAllocateNode(random_index); EXPECT_TRUE(ndtree.hasNode(random_index)) << random_index.toString() << " and tree height " << tree_height; EXPECT_EQ(ndtree.empty(), index_is_inside_root_chunk_node); @@ -54,6 +54,31 @@ TYPED_TEST(NdtreeTest, AllocatingAndClearing) { } } +// TODO(victorr): Remove this workaround after improving the interfaces of the +// ChunkedNdtree. +template +auto* getOrAllocateNodeData(const IndexT& index, TreeT& ndtree) { + using TreeType = std::decay_t; + if constexpr (std::is_same_v> || + std::is_same_v> || + std::is_same_v>) { + return ndtree.getNodeData(index); + } else { + return &ndtree.getOrAllocateNode(index).data(); + } +} +template +auto* getNodeData(const IndexT& index, TreeT& ndtree) { + using TreeType = std::decay_t; + if constexpr (std::is_same_v> || + std::is_same_v> || + std::is_same_v>) { + return ndtree.getNodeData(index); + } else { + return &CHECK_NOTNULL(ndtree.getNode(index))->data(); + } +} + TYPED_TEST(NdtreeTest, GettingAndSetting) { using IndexType = typename TypeParam::IndexType; using PositionType = typename IndexType::Position; @@ -89,7 +114,7 @@ TYPED_TEST(NdtreeTest, GettingAndSetting) { inserted_values.emplace(random_index, random_value); // Insert - auto* data = ndtree.getNodeData(random_index); + auto* data = getOrAllocateNodeData(random_index, ndtree); ASSERT_NE(data, nullptr) << "At index " << random_index.toString(); *data = random_value; } @@ -97,7 +122,7 @@ TYPED_TEST(NdtreeTest, GettingAndSetting) { // Test regular getter for (const auto& [index, value] : inserted_values) { EXPECT_TRUE(ndtree.hasNode(index)) << "At index " << index.toString(); - auto* data = ndtree.getNodeData(index); + auto* data = getNodeData(index, ndtree); ASSERT_NE(data, nullptr) << "At index " << index.toString(); EXPECT_EQ(*data, value) << "At index " << index.toString(); } @@ -107,7 +132,7 @@ TYPED_TEST(NdtreeTest, GettingAndSetting) { for (const auto& [index, value] : inserted_values) { EXPECT_TRUE(ndtree_cref.hasNode(index)) << "At index " << index.toString(); - auto* data = ndtree_cref.getNodeData(index); + auto* data = getNodeData(index, ndtree_cref); ASSERT_NE(data, nullptr) << "At index " << index.toString(); EXPECT_EQ(*data, value) << "At index " << index.toString(); } diff --git a/libraries/wavemap/test/src/iterator/test_subtree_iterator.cc b/libraries/wavemap/test/src/iterator/test_subtree_iterator.cc index 05b7e29bf..c8af0067a 100644 --- a/libraries/wavemap/test/src/iterator/test_subtree_iterator.cc +++ b/libraries/wavemap/test/src/iterator/test_subtree_iterator.cc @@ -12,22 +12,22 @@ class SubtreeIteratorTest : public FixtureBase { static std::unique_ptr demoTree() { auto root_node = std::make_unique(1); - auto child_0 = root_node->allocateChild(0, 2); - auto child_1 = root_node->allocateChild(1, 3); + auto& child_0 = root_node->getOrAllocateChild(0, 2); + auto& child_1 = root_node->getOrAllocateChild(1, 3); - child_0->allocateChild(0, 4); - auto child_01 = child_0->allocateChild(1, 5); - child_0->allocateChild(2, 6); - auto child_11 = child_1->allocateChild(1, 7); - auto child_13 = child_1->allocateChild(3, 8); + child_0.getOrAllocateChild(0, 4); + auto& child_01 = child_0.getOrAllocateChild(1, 5); + child_0.getOrAllocateChild(2, 6); + auto& child_11 = child_1.getOrAllocateChild(1, 7); + auto& child_13 = child_1.getOrAllocateChild(3, 8); - child_01->allocateChild(2, 9); - child_01->allocateChild(3, 10); - child_11->allocateChild(0, 11); - child_11->allocateChild(1, 12); - child_11->allocateChild(2, 13); - child_11->allocateChild(3, 14); - child_13->allocateChild(2, 15); + child_01.getOrAllocateChild(2, 9); + child_01.getOrAllocateChild(3, 10); + child_11.getOrAllocateChild(0, 11); + child_11.getOrAllocateChild(1, 12); + child_11.getOrAllocateChild(2, 13); + child_11.getOrAllocateChild(3, 14); + child_13.getOrAllocateChild(2, 15); return root_node; } diff --git a/libraries/wavemap/test/src/utils/test_sdf_generation.cc b/libraries/wavemap/test/src/utils/test_sdf_generation.cc index a5403d339..86aff0e1a 100644 --- a/libraries/wavemap/test/src/utils/test_sdf_generation.cc +++ b/libraries/wavemap/test/src/utils/test_sdf_generation.cc @@ -73,7 +73,7 @@ TYPED_TEST(SdfGenerationTest, BruteForceEquivalence) { const FloatingPoint occupancy_value = map.getCellValue(node_index); if (OccupancyClassifier::isUnobserved(occupancy_value)) { // In unknown space the SDF should be uninitialized - EXPECT_NEAR(sdf_value, sdf.getDefaultCellValue(), kEpsilon); + EXPECT_NEAR(sdf_value, sdf.getDefaultValue(), kEpsilon); return; } @@ -81,7 +81,7 @@ TYPED_TEST(SdfGenerationTest, BruteForceEquivalence) { convert::nodeIndexToCenterPoint(node_index, min_cell_width); // Find the closest surface using brute force - FloatingPoint sdf_brute_force = sdf.getDefaultCellValue(); + FloatingPoint sdf_brute_force = sdf.getDefaultValue(); Index3D parent_brute_force = Index3D::Constant(std::numeric_limits::max()); if (classifier.isFree(occupancy_value)) { @@ -122,12 +122,12 @@ TYPED_TEST(SdfGenerationTest, BruteForceEquivalence) { sdf_brute_force = -sdf_brute_force; // In occupied space, the SDF should be - if (std::abs(sdf_brute_force) < sdf.getDefaultCellValue()) { + if (std::abs(sdf_brute_force) < sdf.getDefaultValue()) { // Negative EXPECT_LT(sdf_value, 0.f); } else { // Or uninitialized - EXPECT_NEAR(sdf_value, sdf.getDefaultCellValue(), kEpsilon); + EXPECT_NEAR(sdf_value, sdf.getDefaultValue(), kEpsilon); } } @@ -136,7 +136,7 @@ TYPED_TEST(SdfGenerationTest, BruteForceEquivalence) { TypeParam::kMaxRelativeUnderEstimate; constexpr FloatingPoint kMaxRelativeOverEstimate = TypeParam::kMaxRelativeOverEstimate; - if (std::abs(sdf_brute_force) < sdf.getDefaultCellValue()) { + if (std::abs(sdf_brute_force) < sdf.getDefaultValue()) { if (0.f < sdf_brute_force) { EXPECT_LT(sdf_value, sdf_brute_force * (1.f + kMaxRelativeOverEstimate)) @@ -162,10 +162,10 @@ TYPED_TEST(SdfGenerationTest, BruteForceEquivalence) { } } else { EXPECT_LT(sdf_value, - sdf.getDefaultCellValue() * (1.f + kMaxRelativeOverEstimate)) + sdf.getDefaultValue() * (1.f + kMaxRelativeOverEstimate)) << "At index " << print::eigen::oneLine(node_index.position); EXPECT_GT(sdf_value, - sdf.getDefaultCellValue() * (1.f - kMaxRelativeUnderEstimate)) + sdf.getDefaultValue() * (1.f - kMaxRelativeUnderEstimate)) << "At index " << print::eigen::oneLine(node_index.position); } }); diff --git a/libraries/wavemap_io/src/stream_conversions.cc b/libraries/wavemap_io/src/stream_conversions.cc index 42eec0e4d..5f5ffc1c5 100644 --- a/libraries/wavemap_io/src/stream_conversions.cc +++ b/libraries/wavemap_io/src/stream_conversions.cc @@ -216,7 +216,7 @@ bool streamToMap(std::istream& istream, WaveletOctree::Ptr& map) { const bool child_exists = read_node.allocated_children_bitset & (1 << relative_child_idx); if (child_exists) { - stack.emplace(node->allocateChild(relative_child_idx)); + stack.emplace(&node->getOrAllocateChild(relative_child_idx)); } } } @@ -348,7 +348,7 @@ bool streamToMap(std::istream& istream, HashedWaveletOctree::Ptr& map) { const bool child_exists = read_node.allocated_children_bitset & (1 << relative_child_idx); if (child_exists) { - stack.emplace(node->allocateChild(relative_child_idx)); + stack.emplace(&node->getOrAllocateChild(relative_child_idx)); } } } diff --git a/libraries/wavemap_io/test/src/test_file_conversions.cc b/libraries/wavemap_io/test/src/test_file_conversions.cc index d67183656..2c1412f0f 100644 --- a/libraries/wavemap_io/test/src/test_file_conversions.cc +++ b/libraries/wavemap_io/test/src/test_file_conversions.cc @@ -109,10 +109,11 @@ TYPED_TEST(FileConversionsTest, InsertionAndLeafVisitor) { if constexpr (std::is_same_v) { EXPECT_EQ(node_index.height, 0); EXPECT_NEAR(round_trip_value, - map_original.getCellValue(node_index.position), + map_original.getValueOrDefault(node_index.position), TestFixture::kAcceptableReconstructionError); } else { - EXPECT_NEAR(round_trip_value, map_original.getCellValue(node_index), + EXPECT_NEAR(round_trip_value, + map_original.getValueOrDefault(node_index), TestFixture::kAcceptableReconstructionError); } }); @@ -139,10 +140,11 @@ TYPED_TEST(FileConversionsTest, InsertionAndLeafVisitor) { if constexpr (std::is_same_v) { EXPECT_EQ(node_index.height, 0); EXPECT_NEAR(original_value, - map_round_trip->getCellValue(node_index.position), + map_round_trip->getValueOrDefault(node_index.position), TestFixture::kAcceptableReconstructionError); } else { - EXPECT_NEAR(original_value, map_round_trip->getCellValue(node_index), + EXPECT_NEAR(original_value, + map_round_trip->getValueOrDefault(node_index), TestFixture::kAcceptableReconstructionError); } }); diff --git a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc index 1dbf787a8..7973e5fb2 100644 --- a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc +++ b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc @@ -245,7 +245,7 @@ void rosMsgToMap(const wavemap_msgs::WaveletOctree& msg, const bool child_exists = node_msg.allocated_children_bitset & (1 << relative_child_idx); if (child_exists) { - stack.emplace(node->allocateChild(relative_child_idx)); + stack.emplace(&node->getOrAllocateChild(relative_child_idx)); } } } @@ -441,7 +441,7 @@ void rosMsgToMap(const wavemap_msgs::HashedWaveletOctree& msg, const bool child_exists = node_msg.allocated_children_bitset & (1 << relative_child_idx); if (child_exists) { - stack.emplace(node->allocateChild(relative_child_idx)); + stack.emplace(&node->getOrAllocateChild(relative_child_idx)); } } } diff --git a/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc b/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc index 130afa62b..9d8ea6661 100644 --- a/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc +++ b/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc @@ -122,10 +122,11 @@ TYPED_TEST(MapMsgConversionsTest, InsertionAndLeafVisitor) { if constexpr (std::is_same_v) { EXPECT_EQ(node_index.height, 0); EXPECT_NEAR(round_trip_value, - map_original.getCellValue(node_index.position), + map_original.getValueOrDefault(node_index.position), TestFixture::kAcceptableReconstructionError); } else { - EXPECT_NEAR(round_trip_value, map_original.getCellValue(node_index), + EXPECT_NEAR(round_trip_value, + map_original.getValueOrDefault(node_index), TestFixture::kAcceptableReconstructionError); } }); @@ -152,10 +153,11 @@ TYPED_TEST(MapMsgConversionsTest, InsertionAndLeafVisitor) { if constexpr (std::is_same_v) { EXPECT_EQ(node_index.height, 0); EXPECT_NEAR(original_value, - map_round_trip->getCellValue(node_index.position), + map_round_trip->getValueOrDefault(node_index.position), TestFixture::kAcceptableReconstructionError); } else { - EXPECT_NEAR(original_value, map_round_trip->getCellValue(node_index), + EXPECT_NEAR(original_value, + map_round_trip->getValueOrDefault(node_index), TestFixture::kAcceptableReconstructionError); } }); From eeb48f7754050e1cfe0ee85685108f7296004832 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 29 Nov 2023 21:15:19 +0100 Subject: [PATCH 027/159] Add generic hashed octree data structure --- .../impl/ndtree_block_hash_inl.h | 284 ++++++++++++++++++ .../data_structure/ndtree_block_hash.h | 100 ++++++ .../test/src/test_file_conversions.cc | 6 +- .../test/src/test_map_msg_conversions.cc | 6 +- 4 files changed, 388 insertions(+), 8 deletions(-) create mode 100644 libraries/wavemap/include/wavemap/data_structure/impl/ndtree_block_hash_inl.h create mode 100644 libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h diff --git a/libraries/wavemap/include/wavemap/data_structure/impl/ndtree_block_hash_inl.h b/libraries/wavemap/include/wavemap/data_structure/impl/ndtree_block_hash_inl.h new file mode 100644 index 000000000..9c8b571dc --- /dev/null +++ b/libraries/wavemap/include/wavemap/data_structure/impl/ndtree_block_hash_inl.h @@ -0,0 +1,284 @@ +#ifndef WAVEMAP_DATA_STRUCTURE_IMPL_NDTREE_BLOCK_HASH_INL_H_ +#define WAVEMAP_DATA_STRUCTURE_IMPL_NDTREE_BLOCK_HASH_INL_H_ + +#include +#include + +namespace wavemap { +template +inline size_t NdtreeBlockHash::size() const { + size_t size = 0u; + forEachBlock([&size](const Index& /*block_index*/, const Block& block) { + size += block.size(); + }); + return size; +} + +template +bool NdtreeBlockHash::hasBlock( + const Index& block_index) const { + return block_map_.hasBlock(block_index); +} + +template +bool NdtreeBlockHash::eraseBlock( + const Index& block_index) { + return block_map_.eraseBlock(block_index); +} + +template +inline typename NdtreeBlockHash::Block* +NdtreeBlockHash::getBlock(const Index& block_index) { + return block_map_.getBlock(block_index); +} + +template +inline const typename NdtreeBlockHash::Block* +NdtreeBlockHash::getBlock(const Index& block_index) const { + return block_map_.getBlock(block_index); +} + +template +inline typename NdtreeBlockHash::Block& +NdtreeBlockHash::getOrAllocateBlock( + const Index& block_index) { + return block_map_.getOrAllocateBlock(block_index, tree_height_, + default_value_); +} + +template +bool NdtreeBlockHash::hasNode( + const NdtreeIndex& index) const { + return getNode(index); +} + +template +bool NdtreeBlockHash::eraseNode(const NdtreeIndex& index) { + const Index block_index = indexToBlockIndex(index); + if (const Block* block = getBlock(block_index); block) { + const NdtreeIndex cell_index = indexToCellIndex(index); + return block->eraseNode(cell_index); + } + return false; +} + +template +typename NdtreeBlockHash::Node* +NdtreeBlockHash::getNode(const NdtreeIndex& index) { + return const_cast(std::as_const(*this).getNode(index)); +} + +template +const typename NdtreeBlockHash::Node* +NdtreeBlockHash::getNode(const NdtreeIndex& index) const { + const Index block_index = indexToBlockIndex(index); + if (const Block* block = getBlock(block_index); block) { + const NdtreeIndex cell_index = indexToCellIndex(index); + return block->getNode(cell_index); + } + return nullptr; +} + +template +typename NdtreeBlockHash::Node& +NdtreeBlockHash::getOrAllocateNode( + const NdtreeIndex& index) { + const Index block_index = indexToBlockIndex(index); + Block& block = getOrAllocateBlock(block_index); + const NdtreeIndex cell_index = indexToCellIndex(index); + return block.getOrAllocateNode(cell_index, default_value_); +} + +template +std::pair::Node*, IndexElement> +NdtreeBlockHash::getNodeOrAncestor( + const NdtreeIndex& index) { + return const_cast(std::as_const(*this).getNode(index)); +} + +template +std::pair::Node*, IndexElement> +NdtreeBlockHash::getNodeOrAncestor( + const NdtreeIndex& index) const { + const Index block_index = indexToBlockIndex(index); + if (const Block* block = getBlock(block_index); block) { + const NdtreeIndex cell_index = indexToCellIndex(index); + return block->getNodeOrAncestor(cell_index); + } + return {nullptr, tree_height_}; +} + +template +bool NdtreeBlockHash::hasValue( + const NdtreeIndex& index) const { + return hasNode(index); +} + +template +CellDataT* NdtreeBlockHash::getValue( + const NdtreeIndex& index) { + return const_cast(std::as_const(*this).getValue(index)); +} + +template +const CellDataT* NdtreeBlockHash::getValue( + const NdtreeIndex& index) const { + if (const Node* node = getNode(index); node) { + return &node->data(); + } + return nullptr; +} + +template +CellDataT& NdtreeBlockHash::getOrAllocateValue( + const NdtreeIndex& index) { + return getOrAllocateNode(index).data(); +} + +template +std::pair +NdtreeBlockHash::getValueOrAncestor( + const NdtreeIndex& index) { + auto rv = getNodeOrAncestor(index); + return {rv.first ? &rv.first->data() : nullptr, rv.second}; +} + +template +std::pair +NdtreeBlockHash::getValueOrAncestor( + const NdtreeIndex& index) const { + auto rv = getNodeOrAncestor(index); + return {rv.first ? &rv.first->data() : nullptr, rv.second}; +} + +template +const CellDataT& NdtreeBlockHash::getValueOrDefault( + const NdtreeIndex& index) const { + if (const CellDataT* value = getValue(index); value) { + return *value; + } + return default_value_; +} + +template +bool NdtreeBlockHash::equalsDefaultValue( + const CellDataT& value) const { + return value == default_value_; +} + +template +template +void NdtreeBlockHash::forEachBlock( + IndexedBlockVisitor visitor_fn) { + block_map_.forEachBlock(visitor_fn); +} + +template +template +void NdtreeBlockHash::forEachBlock( + IndexedBlockVisitor visitor_fn) const { + block_map_.forEachBlock(visitor_fn); +} + +template +template +void NdtreeBlockHash::forEachLeaf( + IndexedLeafVisitorFunction visitor_fn) { + struct StackElement { + const OctreeIndex node_index; + Node& node; + }; + + block_map_.forEachBlock([&visitor_fn, tree_height = tree_height_]( + const Index& block_index, Block& block) { + std::stack stack; + stack.emplace(StackElement{OctreeIndex{tree_height, block_index}, + block.getRootNode()}); + while (!stack.empty()) { + const OctreeIndex node_index = stack.top().node_index; + Node& node = stack.top().node; + stack.pop(); + + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + const OctreeIndex child_node_index = + node_index.computeChildIndex(child_idx); + if (node.hasChild(child_idx)) { + Node& child_node = *node.getChild(child_idx); + stack.emplace(StackElement{child_node_index, child_node}); + } else { + visitor_fn(child_node_index, node.data()); + } + } + } + }); +} + +template +template +void NdtreeBlockHash::forEachLeaf( + IndexedLeafVisitorFunction visitor_fn) const { + struct StackElement { + const OctreeIndex node_index; + const Node& node; + }; + + block_map_.forEachBlock( + [&visitor_fn, tree_height = tree_height_](const Index& block_index, + const Block& block) { + std::stack stack; + stack.emplace(StackElement{OctreeIndex{tree_height, block_index}, + block.getRootNode()}); + while (!stack.empty()) { + const OctreeIndex node_index = stack.top().node_index; + const Node& node = stack.top().node; + stack.pop(); + + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + const OctreeIndex child_node_index = + node_index.computeChildIndex(child_idx); + if (node.hasChild(child_idx)) { + const Node& child_node = *node.getChild(child_idx); + stack.emplace(StackElement{child_node_index, child_node}); + } else { + visitor_fn(child_node_index, node.data()); + } + } + } + }); +} + +template +inline Index NdtreeBlockHash::indexToBlockIndex( + const NdtreeIndex& index) const { + DCHECK_GE(index.height, 0); + DCHECK_LE(index.height, tree_height_); + const IndexElement depth = tree_height_ - index.height; + return int_math::div_exp2_floor(index.position, depth); +} + +template +inline NdtreeIndex NdtreeBlockHash::indexToCellIndex( + const NdtreeIndex& index) const { + DCHECK_GE(index.height, 0); + DCHECK_LE(index.height, tree_height_); + const IndexElement depth = tree_height_ - index.height; + return {index.height, + int_math::div_exp2_floor_remainder(index.position, depth)}; +} + +template +inline NdtreeIndex +NdtreeBlockHash::cellAndBlockIndexToIndex( + const Index& block_index, const NdtreeIndex& cell_index) const { + DCHECK_GE(cell_index.height, 0); + DCHECK_LE(cell_index.height, tree_height_); + const IndexElement depth = tree_height_ - cell_index.height; + const IndexElement cells_per_side_at_height = int_math::exp2(depth); + return {cell_index.height, + cell_index.position + cells_per_side_at_height * block_index}; +} +} // namespace wavemap + +#endif // WAVEMAP_DATA_STRUCTURE_IMPL_NDTREE_BLOCK_HASH_INL_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h b/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h new file mode 100644 index 000000000..59fc06457 --- /dev/null +++ b/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h @@ -0,0 +1,100 @@ +#ifndef WAVEMAP_DATA_STRUCTURE_NDTREE_BLOCK_HASH_H_ +#define WAVEMAP_DATA_STRUCTURE_NDTREE_BLOCK_HASH_H_ + +#include + +#include "wavemap/data_structure/ndtree/ndtree.h" +#include "wavemap/data_structure/spatial_hash.h" +#include "wavemap/utils/math/int_math.h" + +namespace wavemap { +template +class NdtreeBlockHash { + public: + static constexpr IndexElement kDim = dim; + + using Block = Ndtree; + using Node = typename Block::NodeType; + using BlockHashMap = SpatialHash; + + using IndexType = NdtreeIndex; + using HeightType = IndexElement; + static constexpr HeightType kChunkHeight = 1; + + explicit NdtreeBlockHash(HeightType max_tree_height, + CellDataT default_value = CellDataT{}) + : tree_height_(max_tree_height), default_value_(default_value) {} + + bool empty() const { return block_map_.empty(); } + size_t size() const; + void clear() { block_map_.clear(); } + + HeightType getMaxHeight() const { return tree_height_; } + + bool hasBlock(const Index& block_index) const; + bool eraseBlock(const Index& block_index); + Block* getBlock(const Index& block_index); + const Block* getBlock(const Index& block_index) const; + Block& getOrAllocateBlock(const Index& block_index); + + bool hasNode(const IndexType& index) const; + bool eraseNode(const IndexType& index); + Node* getNode(const IndexType& index); + const Node* getNode(const IndexType& index) const; + Node& getOrAllocateNode(const IndexType& index); + + std::pair getNodeOrAncestor(const IndexType& index); + std::pair getNodeOrAncestor( + const IndexType& index) const; + + // NOTE: The values are stored in trees. Therefore, allocating a new node also + // allocates all the parents of the node that do not yet exist. Their + // data fields will be initialized to default_value_. + // If you want to make sure a value has been explicitly set by you, + // check that it differs from the default_value_. For example, using the + // equalsDefaultValue(value) method below. + bool hasValue(const IndexType& index) const; + CellDataT* getValue(const IndexType& index); + const CellDataT* getValue(const IndexType& index) const; + CellDataT& getOrAllocateValue(const IndexType& index); + + std::pair getValueOrAncestor(const IndexType& index); + std::pair getValueOrAncestor( + const IndexType& index) const; + + const CellDataT& getValueOrDefault(const IndexType& index) const; + const CellDataT& getDefaultCellValue() const { return default_value_; } + bool equalsDefaultValue(const CellDataT& value) const; + + template + void forEachBlock(IndexedBlockVisitor visitor_fn); + template + void forEachBlock(IndexedBlockVisitor visitor_fn) const; + + template + void forEachLeaf(IndexedLeafVisitorFunction visitor_fn); + template + void forEachLeaf(IndexedLeafVisitorFunction visitor_fn) const; + + protected: + const HeightType tree_height_; + const CellDataT default_value_; + BlockHashMap block_map_; + + Index indexToBlockIndex(const IndexType& index) const; + IndexType indexToCellIndex(const IndexType& index) const; + IndexType cellAndBlockIndexToIndex(const Index& block_index, + const IndexType& cell_index) const; +}; + +template +using BinaryTreeBlockHash = NdtreeBlockHash; +template +using QuadtreeBlockHash = NdtreeBlockHash; +template +using OctreeBlockHash = NdtreeBlockHash; +} // namespace wavemap + +#include "wavemap/data_structure/impl/ndtree_block_hash_inl.h" + +#endif // WAVEMAP_DATA_STRUCTURE_NDTREE_BLOCK_HASH_H_ diff --git a/libraries/wavemap_io/test/src/test_file_conversions.cc b/libraries/wavemap_io/test/src/test_file_conversions.cc index 2c1412f0f..f04410d44 100644 --- a/libraries/wavemap_io/test/src/test_file_conversions.cc +++ b/libraries/wavemap_io/test/src/test_file_conversions.cc @@ -112,8 +112,7 @@ TYPED_TEST(FileConversionsTest, InsertionAndLeafVisitor) { map_original.getValueOrDefault(node_index.position), TestFixture::kAcceptableReconstructionError); } else { - EXPECT_NEAR(round_trip_value, - map_original.getValueOrDefault(node_index), + EXPECT_NEAR(round_trip_value, map_original.getCellValue(node_index), TestFixture::kAcceptableReconstructionError); } }); @@ -143,8 +142,7 @@ TYPED_TEST(FileConversionsTest, InsertionAndLeafVisitor) { map_round_trip->getValueOrDefault(node_index.position), TestFixture::kAcceptableReconstructionError); } else { - EXPECT_NEAR(original_value, - map_round_trip->getValueOrDefault(node_index), + EXPECT_NEAR(original_value, map_round_trip->getCellValue(node_index), TestFixture::kAcceptableReconstructionError); } }); diff --git a/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc b/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc index 9d8ea6661..61f64beea 100644 --- a/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc +++ b/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc @@ -125,8 +125,7 @@ TYPED_TEST(MapMsgConversionsTest, InsertionAndLeafVisitor) { map_original.getValueOrDefault(node_index.position), TestFixture::kAcceptableReconstructionError); } else { - EXPECT_NEAR(round_trip_value, - map_original.getValueOrDefault(node_index), + EXPECT_NEAR(round_trip_value, map_original.getCellValue(node_index), TestFixture::kAcceptableReconstructionError); } }); @@ -156,8 +155,7 @@ TYPED_TEST(MapMsgConversionsTest, InsertionAndLeafVisitor) { map_round_trip->getValueOrDefault(node_index.position), TestFixture::kAcceptableReconstructionError); } else { - EXPECT_NEAR(original_value, - map_round_trip->getValueOrDefault(node_index), + EXPECT_NEAR(original_value, map_round_trip->getCellValue(node_index), TestFixture::kAcceptableReconstructionError); } }); From be8a0728c792829455ab75c830a72e623df19c9d Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 30 Nov 2023 16:49:15 +0100 Subject: [PATCH 028/159] Refactor grid neighborhood utils --- libraries/wavemap/CMakeLists.txt | 1 - .../include/wavemap/utils/math/int_math.h | 9 +++++ .../utils/neighbors/grid_neighborhood.h | 20 +++++++++++ .../neighbors/impl/grid_neighborhood_inl.h | 36 +++++++++++++++++++ .../wavemap/utils/sdf/cell_neighborhoods.h | 12 ------- .../utils/sdf/full_euclidean_sdf_generator.h | 4 +-- .../utils/sdf/quasi_euclidean_sdf_generator.h | 4 +-- .../src/utils/sdf/cell_neighborhoods.cc | 29 --------------- .../sdf/quasi_euclidean_sdf_generator.cc | 2 +- .../wavemap/test/src/utils/test_int_math.cc | 15 ++++++++ .../src/visuals/cell_selector.cc | 19 ++-------- 11 files changed, 88 insertions(+), 63 deletions(-) create mode 100644 libraries/wavemap/include/wavemap/utils/neighbors/grid_neighborhood.h create mode 100644 libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_neighborhood_inl.h delete mode 100644 libraries/wavemap/include/wavemap/utils/sdf/cell_neighborhoods.h delete mode 100644 libraries/wavemap/src/utils/sdf/cell_neighborhoods.cc diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index df07d0e3f..e1ef8530a 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -66,7 +66,6 @@ add_library(${PROJECT_NAME} src/map/wavelet_octree.cc src/map/volumetric_data_structure_base.cc src/map/volumetric_data_structure_factory.cc - src/utils/sdf/cell_neighborhoods.cc src/utils/sdf/full_euclidean_sdf_generator.cc src/utils/sdf/quasi_euclidean_sdf_generator.cc src/utils/stopwatch.cc diff --git a/libraries/wavemap/include/wavemap/utils/math/int_math.h b/libraries/wavemap/include/wavemap/utils/math/int_math.h index 09d28bce4..db740d4e2 100644 --- a/libraries/wavemap/include/wavemap/utils/math/int_math.h +++ b/libraries/wavemap/include/wavemap/utils/math/int_math.h @@ -86,6 +86,15 @@ Eigen::Matrix div_exp2_ceil(Eigen::Matrix vector, return vector; } +template +constexpr T pow(T base, int exponent) { + T pow = 1; + for (int iteration = 0; iteration < exponent; ++iteration) { + pow *= base; + } + return pow; +} + template constexpr std::array pow_sequence() { static_assert(0 < dim); diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/grid_neighborhood.h b/libraries/wavemap/include/wavemap/utils/neighbors/grid_neighborhood.h new file mode 100644 index 000000000..58f4bee30 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/neighbors/grid_neighborhood.h @@ -0,0 +1,20 @@ +#ifndef WAVEMAP_UTILS_NEIGHBORS_GRID_NEIGHBORHOOD_H_ +#define WAVEMAP_UTILS_NEIGHBORS_GRID_NEIGHBORHOOD_H_ + +#include "wavemap/common.h" +#include "wavemap/utils/math/int_math.h" + +namespace wavemap { +template +struct grid_neighborhood { + static constexpr int kNumNeighbors = int_math::pow(3, dim) - 1; + + static std::array, kNumNeighbors> generateIndexOffsets(); + static std::array generateDistanceOffsets( + FloatingPoint cell_width); +}; +} // namespace wavemap + +#include "wavemap/utils/neighbors/impl/grid_neighborhood_inl.h" + +#endif // WAVEMAP_UTILS_NEIGHBORS_GRID_NEIGHBORHOOD_H_ diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_neighborhood_inl.h b/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_neighborhood_inl.h new file mode 100644 index 000000000..ad3829de7 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_neighborhood_inl.h @@ -0,0 +1,36 @@ +#ifndef WAVEMAP_UTILS_NEIGHBORS_IMPL_GRID_NEIGHBORHOOD_INL_H_ +#define WAVEMAP_UTILS_NEIGHBORS_IMPL_GRID_NEIGHBORHOOD_INL_H_ + +#include "wavemap/utils/iterate/grid_iterator.h" + +namespace wavemap { +template +std::array, grid_neighborhood::kNumNeighbors> +grid_neighborhood::generateIndexOffsets() { + std::array, kNumNeighbors> neighbor_offsets{}; + size_t array_idx = 0u; + for (const Index& index : + Grid(-Index::Ones(), Index::Ones())) { + if (index != Index::Zero()) { + neighbor_offsets[array_idx] = index; + ++array_idx; + } + } + return neighbor_offsets; +} + +template +std::array::kNumNeighbors> +grid_neighborhood::generateDistanceOffsets(FloatingPoint cell_width) { + std::array distance_offsets{}; + size_t array_idx = 0u; + for (const Index& index_offset : generateIndexOffsets()) { + distance_offsets[array_idx] = + cell_width * index_offset.template cast().norm(); + ++array_idx; + } + return distance_offsets; +} +} // namespace wavemap + +#endif // WAVEMAP_UTILS_NEIGHBORS_IMPL_GRID_NEIGHBORHOOD_INL_H_ diff --git a/libraries/wavemap/include/wavemap/utils/sdf/cell_neighborhoods.h b/libraries/wavemap/include/wavemap/utils/sdf/cell_neighborhoods.h deleted file mode 100644 index d958edb15..000000000 --- a/libraries/wavemap/include/wavemap/utils/sdf/cell_neighborhoods.h +++ /dev/null @@ -1,12 +0,0 @@ -#ifndef WAVEMAP_UTILS_SDF_CELL_NEIGHBORHOODS_H_ -#define WAVEMAP_UTILS_SDF_CELL_NEIGHBORHOODS_H_ - -#include "wavemap/common.h" - -namespace wavemap::neighborhood { -std::array generateNeighborIndexOffsets(); -std::array generateNeighborDistanceOffsets( - FloatingPoint cell_width); -} // namespace wavemap::neighborhood - -#endif // WAVEMAP_UTILS_SDF_CELL_NEIGHBORHOODS_H_ diff --git a/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h b/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h index b002fb188..aae88be43 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h +++ b/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h @@ -5,7 +5,7 @@ #include "wavemap/data_structure/bucket_queue.h" #include "wavemap/map/hashed_blocks.h" #include "wavemap/map/hashed_wavelet_octree.h" -#include "wavemap/utils/sdf/cell_neighborhoods.h" +#include "wavemap/utils/neighbors/grid_neighborhood.h" namespace wavemap { struct VectorDistance { @@ -38,7 +38,7 @@ class FullEuclideanSDFGenerator { private: inline static const auto kNeighborIndexOffsets = - neighborhood::generateNeighborIndexOffsets(); + grid_neighborhood<3>::generateIndexOffsets(); const FloatingPoint max_distance_; const OccupancyClassifier classifier_; diff --git a/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h b/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h index 788278e9e..2c358f2f3 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h +++ b/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h @@ -4,8 +4,8 @@ #include "wavemap/data_structure/bucket_queue.h" #include "wavemap/map/hashed_blocks.h" #include "wavemap/map/hashed_wavelet_octree.h" +#include "wavemap/utils/neighbors/grid_neighborhood.h" #include "wavemap/utils/query/occupancy_classifier.h" -#include "wavemap/utils/sdf/cell_neighborhoods.h" namespace wavemap { class QuasiEuclideanSDFGenerator { @@ -23,7 +23,7 @@ class QuasiEuclideanSDFGenerator { private: inline static const auto kNeighborIndexOffsets = - neighborhood::generateNeighborIndexOffsets(); + grid_neighborhood<3>::generateIndexOffsets(); const FloatingPoint max_distance_; const OccupancyClassifier classifier_; diff --git a/libraries/wavemap/src/utils/sdf/cell_neighborhoods.cc b/libraries/wavemap/src/utils/sdf/cell_neighborhoods.cc deleted file mode 100644 index 9a36b141a..000000000 --- a/libraries/wavemap/src/utils/sdf/cell_neighborhoods.cc +++ /dev/null @@ -1,29 +0,0 @@ -#include "wavemap/utils/sdf/cell_neighborhoods.h" - -#include "wavemap/utils/iterate/grid_iterator.h" - -namespace wavemap::neighborhood { -std::array generateNeighborIndexOffsets() { - std::array neighbor_offsets{}; - size_t array_idx = 0u; - for (const Index3D& index : Grid<3>(-Index3D::Ones(), Index3D::Ones())) { - if (index != Index3D::Zero()) { - neighbor_offsets[array_idx] = index; - ++array_idx; - } - } - return neighbor_offsets; -} - -std::array generateNeighborDistanceOffsets( - FloatingPoint cell_width) { - std::array distance_offsets{}; - size_t array_idx = 0u; - for (const Index3D& index_offset : generateNeighborIndexOffsets()) { - distance_offsets[array_idx] = - cell_width * index_offset.cast().norm(); - ++array_idx; - } - return distance_offsets; -} -} // namespace wavemap::neighborhood diff --git a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc index f81579be9..03d1c6359 100644 --- a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc @@ -103,7 +103,7 @@ void QuasiEuclideanSDFGenerator::propagate( // Precompute the neighbor distance offsets const FloatingPoint min_cell_width = occupancy_map.getMinCellWidth(); const auto neighbor_distance_offsets = - neighborhood::generateNeighborDistanceOffsets(min_cell_width); + grid_neighborhood<3>::generateDistanceOffsets(min_cell_width); CHECK_EQ(kNeighborIndexOffsets.size(), neighbor_distance_offsets.size()); const FloatingPoint half_max_neighbor_distance_offset = 0.5f * std::sqrt(3.f) * min_cell_width + 1e-3f; diff --git a/libraries/wavemap/test/src/utils/test_int_math.cc b/libraries/wavemap/test/src/utils/test_int_math.cc index 9f6a48f33..162ecc83c 100644 --- a/libraries/wavemap/test/src/utils/test_int_math.cc +++ b/libraries/wavemap/test/src/utils/test_int_math.cc @@ -3,6 +3,7 @@ #include #include "wavemap/utils/math/int_math.h" +#include "wavemap/utils/random_number_generator.h" namespace wavemap { TEST(IntMathTest, Exp2) { @@ -73,6 +74,20 @@ TEST(IntMathTest, DivExp2Remainder) { } } +TEST(IntMathTest, Pow) { + constexpr int kNumIterations = 10; + constexpr int kMinBase = -50; + constexpr int kMaxBase = 50; + constexpr int kMaxExponent = 5; + RandomNumberGenerator rng; + for (int iteration = 0; iteration < kNumIterations; ++iteration) { + const int base = rng.getRandomInteger(kMinBase, kMaxBase); + const int exponent = rng.getRandomInteger(0, kMaxExponent); + EXPECT_EQ(int_math::pow(base, exponent), std::pow(base, exponent)) + << "For base " << base << " and exponent " << exponent; + } +} + TEST(IntMathTest, PowSequence) { // Test trivial (length 1) sequences constexpr std::array expected_pow_sequence_length_1 = {1}; diff --git a/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc b/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc index 71fc2e614..b60d02116 100644 --- a/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc +++ b/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc @@ -1,23 +1,10 @@ #include "wavemap_rviz_plugin/visuals/cell_selector.h" -#include +#include namespace wavemap::rviz_plugin { -std::array generateNeighborIndexOffsets() { - std::array neighbor_offsets{}; - size_t array_idx = 0u; - for (const Index3D& index : Grid<3>(-Index3D::Ones(), Index3D::Ones())) { - if (index != Index3D::Zero()) { - neighbor_offsets[array_idx] = index; - ++array_idx; - } - } - std::sort( - neighbor_offsets.begin(), neighbor_offsets.end(), - [](const auto& lhs, const auto& rhs) { return lhs.norm() < rhs.norm(); }); - return neighbor_offsets; -} -static const auto kNeighborOffsets = generateNeighborIndexOffsets(); +static const auto kNeighborOffsets = + grid_neighborhood<3>::generateIndexOffsets(); CellSelector::CellSelector(rviz::Property* submenu_root_property, std::function redraw_map) From bb49e605c9dd0a9a357c216253c5ff68263b67e6 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 1 Dec 2023 11:54:55 +0100 Subject: [PATCH 029/159] Refactor and extend ROS msg conversions --- .../operations/crop_map_operation.h | 2 +- .../operations/publish_map_operation.h | 2 +- .../src/operations/crop_map_operation.cc | 1 + .../src/operations/publish_map_operation.cc | 40 ++++++++------- ros/wavemap_ros/src/utils/tf_transformer.cc | 2 +- .../geometry_msg_conversions.h | 51 +++++++++++++++---- 6 files changed, 66 insertions(+), 32 deletions(-) diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h b/ros/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h index a77d5ed53..7fe62e8ef 100644 --- a/ros/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h +++ b/ros/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h @@ -13,7 +13,7 @@ namespace wavemap { /** - * Config struct for map pruning operations. + * Config struct for map cropping operations. */ struct CropMapOperationConfig : public ConfigBase { //! Time period controlling how often the map is cropped. diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h b/ros/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h index 2bb2a6ce5..b50169003 100644 --- a/ros/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h +++ b/ros/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h @@ -14,7 +14,7 @@ namespace wavemap { /** - * Config struct for map pruning operations. + * Config struct for map publishing operations. */ struct PublishMapOperationConfig : public ConfigBase { diff --git a/ros/wavemap_ros/src/operations/crop_map_operation.cc b/ros/wavemap_ros/src/operations/crop_map_operation.cc index c551c54ec..7abe1184c 100644 --- a/ros/wavemap_ros/src/operations/crop_map_operation.cc +++ b/ros/wavemap_ros/src/operations/crop_map_operation.cc @@ -26,6 +26,7 @@ void CropMapOperation::run(const ros::Time& current_time, bool force_run) { } last_run_timestamp_ = current_time; + // If the map is empty, there's no work to do if (occupancy_map_->empty()) { return; } diff --git a/ros/wavemap_ros/src/operations/publish_map_operation.cc b/ros/wavemap_ros/src/operations/publish_map_operation.cc index 91fd97c9b..1ae8fd5fb 100644 --- a/ros/wavemap_ros/src/operations/publish_map_operation.cc +++ b/ros/wavemap_ros/src/operations/publish_map_operation.cc @@ -43,25 +43,27 @@ PublishMapOperation::PublishMapOperation( void PublishMapOperation::publishMap(const ros::Time& current_time, bool republish_whole_map) { ZoneScoped; - if (!occupancy_map_->empty()) { - if (auto* hashed_wavelet_octree = - dynamic_cast(occupancy_map_.get()); - hashed_wavelet_octree) { - publishHashedMap(current_time, hashed_wavelet_octree, - republish_whole_map); - } else if (auto* hashed_chunked_wavelet_octree = - dynamic_cast( - occupancy_map_.get()); - hashed_chunked_wavelet_octree) { - publishHashedMap(current_time, hashed_chunked_wavelet_octree, - republish_whole_map); - } else { - occupancy_map_->threshold(); - wavemap_msgs::Map map_msg; - if (convert::mapToRosMsg(*occupancy_map_, world_frame_, current_time, - map_msg)) { - map_pub_.publish(map_msg); - } + // If the map is empty, there's no work to do + if (occupancy_map_->empty()) { + return; + } + + if (auto* hashed_wavelet_octree = + dynamic_cast(occupancy_map_.get()); + hashed_wavelet_octree) { + publishHashedMap(current_time, hashed_wavelet_octree, republish_whole_map); + } else if (auto* hashed_chunked_wavelet_octree = + dynamic_cast( + occupancy_map_.get()); + hashed_chunked_wavelet_octree) { + publishHashedMap(current_time, hashed_chunked_wavelet_octree, + republish_whole_map); + } else { + occupancy_map_->threshold(); + wavemap_msgs::Map map_msg; + if (convert::mapToRosMsg(*occupancy_map_, world_frame_, current_time, + map_msg)) { + map_pub_.publish(map_msg); } } } diff --git a/ros/wavemap_ros/src/utils/tf_transformer.cc b/ros/wavemap_ros/src/utils/tf_transformer.cc index af21e789e..2b212927a 100644 --- a/ros/wavemap_ros/src/utils/tf_transformer.cc +++ b/ros/wavemap_ros/src/utils/tf_transformer.cc @@ -73,7 +73,7 @@ bool TfTransformer::lookupTransformImpl(const std::string& to_frame_id, } geometry_msgs::TransformStamped transform_msg = tf_buffer_.lookupTransform(to_frame_id, from_frame_id, frame_timestamp); - convert::rosMsgToTransformation3D(transform_msg.transform, transform); + transform = convert::transformMsgToTransformation3D(transform_msg.transform); return true; } } // namespace wavemap diff --git a/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/geometry_msg_conversions.h b/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/geometry_msg_conversions.h index e0cb64f9f..b03b4927a 100644 --- a/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/geometry_msg_conversions.h +++ b/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/geometry_msg_conversions.h @@ -2,25 +2,56 @@ #define WAVEMAP_ROS_CONVERSIONS_GEOMETRY_MSG_CONVERSIONS_H_ #include +#include namespace wavemap::convert { -inline void rosMsgToPoint3D(const geometry_msgs::Vector3& msg, Point3D& point) { +inline Point3D pointMsgToPoint3D(const geometry_msgs::Point& msg) { Eigen::Vector3d point_double; - tf::vectorMsgToEigen(msg, point_double); - point = point_double.cast(); + tf::pointMsgToEigen(msg, point_double); + return point_double.cast(); } -inline void rosMsgToRotation3D(const geometry_msgs::Quaternion& msg, - Rotation3D& rotation) { +inline geometry_msgs::Point point3DToPointMsg(const Point3D& point) { + geometry_msgs::Point msg; + tf::pointEigenToMsg(point.cast(), msg); + return msg; +} + +inline Point3D point32MsgToPoint3D(const geometry_msgs::Point32& msg) { + return {msg.x, msg.y, msg.z}; +} + +inline geometry_msgs::Point32 point3DToPoint32Msg(const Point3D& point) { + geometry_msgs::Point32 msg; + msg.x = point.x(); + msg.y = point.y(); + msg.z = point.z(); + return msg; +} + +inline Vector3D vector3MsgToVector3D(const geometry_msgs::Vector3& msg) { + Eigen::Vector3d vector_double; + tf::vectorMsgToEigen(msg, vector_double); + return vector_double.cast(); +} + +inline geometry_msgs::Vector3 vector3DToVector3Msg(const Vector3D& vector) { + geometry_msgs::Vector3 msg; + tf::vectorEigenToMsg(vector.cast(), msg); + return msg; +} + +inline Rotation3D quaternionMsgToRotation3D( + const geometry_msgs::Quaternion& msg) { Eigen::Quaterniond rotation_double; tf::quaternionMsgToEigen(msg, rotation_double); - rotation = Rotation3D{rotation_double.cast()}; + return Rotation3D{rotation_double.cast()}; } -inline void rosMsgToTransformation3D(const geometry_msgs::Transform& msg, - Transformation3D& transform) { - rosMsgToPoint3D(msg.translation, transform.getPosition()); - rosMsgToRotation3D(msg.rotation, transform.getRotation()); +inline Transformation3D transformMsgToTransformation3D( + const geometry_msgs::Transform& msg) { + return Transformation3D{quaternionMsgToRotation3D(msg.rotation), + vector3MsgToVector3D(msg.translation)}; } } // namespace wavemap::convert From 39f95104b33a5ef8f6ccf56ecfa00dd5aa56bd56 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 1 Dec 2023 12:08:39 +0100 Subject: [PATCH 030/159] Add plugin to publish obstacles as fixed-resolution pointcloud --- ros/wavemap_ros/CMakeLists.txt | 1 + .../wavemap_ros/operations/operation_base.h | 11 ++- .../operations/publish_pointcloud_operation.h | 69 +++++++++++++++++ .../src/operations/operation_factory.cc | 13 +++- .../publish_pointcloud_operation.cc | 76 +++++++++++++++++++ 5 files changed, 167 insertions(+), 3 deletions(-) create mode 100644 ros/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h create mode 100644 ros/wavemap_ros/src/operations/publish_pointcloud_operation.cc diff --git a/ros/wavemap_ros/CMakeLists.txt b/ros/wavemap_ros/CMakeLists.txt index 43ce55063..09e870159 100644 --- a/ros/wavemap_ros/CMakeLists.txt +++ b/ros/wavemap_ros/CMakeLists.txt @@ -27,6 +27,7 @@ cs_add_library(${PROJECT_NAME} src/operations/operation_factory.cc src/operations/prune_map_operation.cc src/operations/publish_map_operation.cc + src/operations/publish_pointcloud_operation.cc src/operations/threshold_map_operation.cc src/utils/pointcloud_undistortion/pointcloud_undistorter.cc src/utils/rosbag_processor.cc diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/operation_base.h b/ros/wavemap_ros/include/wavemap_ros/operations/operation_base.h index 4ed51b0dd..2d1be30de 100644 --- a/ros/wavemap_ros/include/wavemap_ros/operations/operation_base.h +++ b/ros/wavemap_ros/include/wavemap_ros/operations/operation_base.h @@ -8,10 +8,17 @@ namespace wavemap { struct OperationType : public TypeSelector { using TypeSelector::TypeSelector; - enum Id : TypeId { kThresholdMap, kPruneMap, kPublishMap, kCropMap }; + enum Id : TypeId { + kThresholdMap, + kPruneMap, + kPublishMap, + kPublishPointcloud, + kCropMap + }; static constexpr std::array names = {"threshold_map", "prune_map", - "publish_map", "crop_map"}; + "publish_map", "publish_pointcloud", + "crop_map"}; }; class OperationBase { diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h b/ros/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h new file mode 100644 index 000000000..184744f49 --- /dev/null +++ b/ros/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h @@ -0,0 +1,69 @@ +#ifndef WAVEMAP_ROS_OPERATIONS_PUBLISH_POINTCLOUD_OPERATION_H_ +#define WAVEMAP_ROS_OPERATIONS_PUBLISH_POINTCLOUD_OPERATION_H_ + +#include +#include + +#include +#include +#include +#include + +#include "wavemap_ros/operations/operation_base.h" + +namespace wavemap { +/** + * Config struct for obstacle pointcloud publishing operations. + */ +struct PublishPointcloudOperationConfig + : public ConfigBase { + //! Time period controlling how often the pointcloud is published. + Seconds once_every = 1.f; + + //! Threshold in log odds above which cells are classified as occupied. + FloatingPoint occupancy_threshold_log_odds = 1e-3f; + + //! Name of the topic the pointcloud will be published on. + std::string topic = "obstacle_pointcloud"; + + static MemberMap memberMap; + + bool isValid(bool verbose) const override; +}; + +class PublishPointcloudOperation : public OperationBase { + public: + PublishPointcloudOperation(const PublishPointcloudOperationConfig& config, + std::string world_frame, + VolumetricDataStructureBase::Ptr occupancy_map, + ros::NodeHandle nh_private); + + OperationType getType() const override { + return OperationType::kPublishPointcloud; + } + + bool shouldRun(const ros::Time& current_time) { + return config_.once_every < (current_time - last_run_timestamp_).toSec(); + } + + void run(const ros::Time& current_time, bool force_run) override { + if (force_run || shouldRun(current_time)) { + publishPointcloud(current_time); + last_run_timestamp_ = current_time; + } + } + + private: + const PublishPointcloudOperationConfig config_; + const std::string world_frame_; + const VolumetricDataStructureBase::Ptr occupancy_map_; + ros::Time last_run_timestamp_; + + // Pointcloud publishing + ros::Publisher pointcloud_pub_; + Timestamp last_run_timestamp_internal_; + void publishPointcloud(const ros::Time& cell_index); +}; +} // namespace wavemap + +#endif // WAVEMAP_ROS_OPERATIONS_PUBLISH_POINTCLOUD_OPERATION_H_ diff --git a/ros/wavemap_ros/src/operations/operation_factory.cc b/ros/wavemap_ros/src/operations/operation_factory.cc index 2450fc220..01ca5bd58 100644 --- a/ros/wavemap_ros/src/operations/operation_factory.cc +++ b/ros/wavemap_ros/src/operations/operation_factory.cc @@ -3,6 +3,7 @@ #include "wavemap_ros/operations/crop_map_operation.h" #include "wavemap_ros/operations/prune_map_operation.h" #include "wavemap_ros/operations/publish_map_operation.h" +#include "wavemap_ros/operations/publish_pointcloud_operation.h" #include "wavemap_ros/operations/threshold_map_operation.h" namespace wavemap { @@ -68,6 +69,16 @@ std::unique_ptr OperationFactory::create( ROS_ERROR("Publish map operation config could not be loaded."); return nullptr; } + case OperationType::kPublishPointcloud: + if (const auto config = PublishPointcloudOperationConfig::from(params); + config) { + return std::make_unique( + config.value(), std::move(world_frame), std::move(occupancy_map), + nh_private); + } else { + ROS_ERROR("Publish pointcloud operation config could not be loaded."); + return nullptr; + } case OperationType::kCropMap: if (const auto config = CropMapOperationConfig::from(params); config) { return std::make_unique( @@ -79,7 +90,7 @@ std::unique_ptr OperationFactory::create( } } - ROS_ERROR_STREAM("Factory does not support creation of operation type " + ROS_ERROR_STREAM("Factory does not (yet) support creation of operation type " << operation_type.toStr() << "."); return nullptr; } diff --git a/ros/wavemap_ros/src/operations/publish_pointcloud_operation.cc b/ros/wavemap_ros/src/operations/publish_pointcloud_operation.cc new file mode 100644 index 000000000..ef0cef228 --- /dev/null +++ b/ros/wavemap_ros/src/operations/publish_pointcloud_operation.cc @@ -0,0 +1,76 @@ +#include "wavemap_ros/operations/publish_pointcloud_operation.h" + +#include +#include +#include +#include +#include + +namespace wavemap { +DECLARE_CONFIG_MEMBERS(PublishPointcloudOperationConfig, + (once_every) + (occupancy_threshold_log_odds) + (topic)); + +bool PublishPointcloudOperationConfig::isValid(bool verbose) const { + bool all_valid = true; + + all_valid &= IS_PARAM_GT(once_every, 0.f, verbose); + all_valid &= IS_PARAM_NE(topic, std::string(), verbose); + + return all_valid; +} + +PublishPointcloudOperation::PublishPointcloudOperation( + const PublishPointcloudOperationConfig& config, std::string world_frame, + VolumetricDataStructureBase::Ptr occupancy_map, ros::NodeHandle nh_private) + : config_(config.checkValid()), + world_frame_(std::move(world_frame)), + occupancy_map_(std::move(occupancy_map)) { + pointcloud_pub_ = + nh_private.advertise(config_.topic, 10); +} + +void PublishPointcloudOperation::publishPointcloud( + const ros::Time& current_time) { + ZoneScoped; + // If the map is empty, there's no work to do + if (occupancy_map_->empty()) { + return; + } + + // Create the message and set the header + sensor_msgs::PointCloud obstacle_pointcloud_msg; + obstacle_pointcloud_msg.header.frame_id = world_frame_; + obstacle_pointcloud_msg.header.stamp = current_time; + + // Add the points + occupancy_map_->forEachLeaf([&point_msg_vector = + obstacle_pointcloud_msg.points, + cell_width = occupancy_map_->getMinCellWidth(), + occupancy_threshold = + config_.occupancy_threshold_log_odds]( + const OctreeIndex& node_index, + FloatingPoint node_log_odds) { + if (occupancy_threshold < node_log_odds) { + if (node_index.height == 0) { + const Point3D center = + convert::indexToCenterPoint(node_index.position, cell_width); + point_msg_vector.emplace_back(convert::point3DToPoint32Msg(center)); + } else { + const Index3D node_min_corner = + convert::nodeIndexToMinCornerIndex(node_index); + const Index3D node_max_corner = + convert::nodeIndexToMaxCornerIndex(node_index); + for (const Index3D& index : Grid(node_min_corner, node_max_corner)) { + const Point3D center = convert::indexToCenterPoint(index, cell_width); + point_msg_vector.emplace_back(convert::point3DToPoint32Msg(center)); + } + } + } + }); + + // Publish the msg + pointcloud_pub_.publish(obstacle_pointcloud_msg); +} +} // namespace wavemap From 70c57a8ee4cf3e7e51f78b22eb3fd18a57eb6593 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 1 Dec 2023 12:18:09 +0100 Subject: [PATCH 031/159] Update config validation schema to support new operation --- .../operations/operations_pipeline.json | 4 +++ .../publish_pointcloud_operation.json | 26 +++++++++++++++++++ 2 files changed, 30 insertions(+) create mode 100644 tooling/schemas/wavemap/operations/publish_pointcloud_operation.json diff --git a/tooling/schemas/wavemap/operations/operations_pipeline.json b/tooling/schemas/wavemap/operations/operations_pipeline.json index 090d2bd24..ca8a6ef20 100644 --- a/tooling/schemas/wavemap/operations/operations_pipeline.json +++ b/tooling/schemas/wavemap/operations/operations_pipeline.json @@ -13,6 +13,7 @@ "threshold_map", "prune_map", "publish_map", + "publish_pointcloud", "crop_map" ] } @@ -27,6 +28,9 @@ { "$ref": "publish_map_operation.json" }, + { + "$ref": "publish_pointcloud_operation.json" + }, { "$ref": "crop_map_operation.json" } diff --git a/tooling/schemas/wavemap/operations/publish_pointcloud_operation.json b/tooling/schemas/wavemap/operations/publish_pointcloud_operation.json new file mode 100644 index 000000000..8acad43ad --- /dev/null +++ b/tooling/schemas/wavemap/operations/publish_pointcloud_operation.json @@ -0,0 +1,26 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "description": "Properties of a single obstacle pointcloud publishing operation.", + "type": "object", + "additionalProperties": false, + "properties": { + "type": { + "const": "publish_pointcloud" + }, + "once_every": { + "description": "Time period controlling how often the pointcloud is published.", + "$ref": "../value_with_unit/convertible_to_seconds.json" + }, + "occupancy_threshold_log_odds": { + "description": "Threshold in log odds above which cells are classified as occupied.", + "type": "number" + }, + "topic": { + "description": "Name of the topic the pointcloud will be published on.", + "type": "string", + "examples": [ + "obstacle_pointcloud" + ] + } + } +} From 07e67a2a975513b41d959544ebb319c36fd1a146 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 1 Dec 2023 14:36:34 +0100 Subject: [PATCH 032/159] Switch to sensor_msgs::PointCloud2 msg type --- .../src/operations/publish_pointcloud_operation.cc | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/ros/wavemap_ros/src/operations/publish_pointcloud_operation.cc b/ros/wavemap_ros/src/operations/publish_pointcloud_operation.cc index ef0cef228..6201cf37f 100644 --- a/ros/wavemap_ros/src/operations/publish_pointcloud_operation.cc +++ b/ros/wavemap_ros/src/operations/publish_pointcloud_operation.cc @@ -1,6 +1,8 @@ #include "wavemap_ros/operations/publish_pointcloud_operation.h" #include +#include +#include #include #include #include @@ -28,7 +30,7 @@ PublishPointcloudOperation::PublishPointcloudOperation( world_frame_(std::move(world_frame)), occupancy_map_(std::move(occupancy_map)) { pointcloud_pub_ = - nh_private.advertise(config_.topic, 10); + nh_private.advertise(config_.topic, 10); } void PublishPointcloudOperation::publishPointcloud( @@ -71,6 +73,9 @@ void PublishPointcloudOperation::publishPointcloud( }); // Publish the msg - pointcloud_pub_.publish(obstacle_pointcloud_msg); + sensor_msgs::PointCloud2 obstacle_pointcloud2_msg; + sensor_msgs::convertPointCloudToPointCloud2(obstacle_pointcloud_msg, + obstacle_pointcloud2_msg); + pointcloud_pub_.publish(obstacle_pointcloud2_msg); } } // namespace wavemap From f99a57915580893cc89cea4ffe4d2b7502a0adce Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Sat, 2 Dec 2023 15:53:31 +0100 Subject: [PATCH 033/159] Extend and test adjacency utils --- libraries/wavemap/CMakeLists.txt | 4 + .../wavemap/utils/bits/bit_operations.h | 5 ++ .../include/wavemap/utils/math/int_math.h | 20 +++++ .../wavemap/utils/neighbors/adjacency.h | 20 +++++ .../wavemap/utils/neighbors/grid_adjacency.h | 27 +++++++ .../utils/neighbors/grid_neighborhood.h | 26 +++++-- .../utils/neighbors/impl/grid_adjacency_inl.h | 42 ++++++++++ .../neighbors/impl/grid_neighborhood_inl.h | 58 ++++++++++---- .../neighbors/impl/ndtree_adjacency_inl.h | 6 ++ .../utils/neighbors/ndtree_adjacency.h | 33 ++++++++ .../utils/sdf/full_euclidean_sdf_generator.h | 2 +- .../utils/sdf/quasi_euclidean_sdf_generator.h | 2 +- .../sdf/quasi_euclidean_sdf_generator.cc | 4 +- .../wavemap/test/src/utils/test_adjacency.cc | 64 +++++++++++++++ .../test/src/utils/test_bit_manipulation.cc | 10 +++ .../test/src/utils/test_grid_adjacency.cc | 1 + .../test/src/utils/test_grid_neighborhood.cc | 78 +++++++++++++++++++ .../wavemap/test/src/utils/test_int_math.cc | 42 ++++++++++ .../test/src/utils/test_ndtree_adjacency.cc | 1 + .../src/visuals/cell_selector.cc | 4 +- 20 files changed, 422 insertions(+), 27 deletions(-) create mode 100644 libraries/wavemap/include/wavemap/utils/neighbors/adjacency.h create mode 100644 libraries/wavemap/include/wavemap/utils/neighbors/grid_adjacency.h create mode 100644 libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_adjacency_inl.h create mode 100644 libraries/wavemap/include/wavemap/utils/neighbors/impl/ndtree_adjacency_inl.h create mode 100644 libraries/wavemap/include/wavemap/utils/neighbors/ndtree_adjacency.h create mode 100644 libraries/wavemap/test/src/utils/test_adjacency.cc create mode 100644 libraries/wavemap/test/src/utils/test_grid_adjacency.cc create mode 100644 libraries/wavemap/test/src/utils/test_grid_neighborhood.cc create mode 100644 libraries/wavemap/test/src/utils/test_ndtree_adjacency.cc diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index e1ef8530a..626f3e535 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -110,10 +110,14 @@ if (CATKIN_ENABLE_TESTING) test/src/iterator/test_grid_iterator.cc test/src/iterator/test_ray_iterator.cc test/src/iterator/test_subtree_iterator.cc + test/src/utils/test_adjacency.cc test/src/utils/test_approximate_trigonometry.cc test/src/utils/test_bit_manipulation.cc test/src/utils/test_data_utils.cc test/src/utils/test_fill_utils.cc + test/src/utils/test_grid_neighborhood.cc + test/src/utils/test_grid_adjacency.cc + test/src/utils/test_ndtree_adjacency.cc test/src/utils/test_int_math.cc test/src/utils/test_log_odds_converter.cc test/src/utils/test_map_interpolator.cpp diff --git a/libraries/wavemap/include/wavemap/utils/bits/bit_operations.h b/libraries/wavemap/include/wavemap/utils/bits/bit_operations.h index 9bdb910ad..bc476b742 100644 --- a/libraries/wavemap/include/wavemap/utils/bits/bit_operations.h +++ b/libraries/wavemap/include/wavemap/utils/bits/bit_operations.h @@ -105,6 +105,11 @@ constexpr std::make_signed_t bit_cast_signed(T bitstring) { } } +template +constexpr bool is_bit_set(T bitstring, int bit_index) { + return (bitstring >> bit_index) & 1; +} + template constexpr T rotate_left(T bitstring, T shift) { static_assert(width <= 8 * sizeof(T)); diff --git a/libraries/wavemap/include/wavemap/utils/math/int_math.h b/libraries/wavemap/include/wavemap/utils/math/int_math.h index db740d4e2..598bb2f99 100644 --- a/libraries/wavemap/include/wavemap/utils/math/int_math.h +++ b/libraries/wavemap/include/wavemap/utils/math/int_math.h @@ -110,6 +110,26 @@ template constexpr T div_round_up(T numerator, T denominator) { return numerator / denominator + (numerator % denominator != 0); } + +template +constexpr T factorial(T x) { + T result = 1; + for (; 1 < x; --x) { + result *= x; + } + return result; +} + +template +constexpr T binomial(T n, T m) { + if (n < m) { + return 0; + } else if (n == m) { + return 1; + } else { + return factorial(n) / (factorial(m) * factorial(n - m)); + } +} } // namespace wavemap::int_math #endif // WAVEMAP_UTILS_MATH_INT_MATH_H_ diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/adjacency.h b/libraries/wavemap/include/wavemap/utils/neighbors/adjacency.h new file mode 100644 index 000000000..e1c626856 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/neighbors/adjacency.h @@ -0,0 +1,20 @@ +#ifndef WAVEMAP_UTILS_NEIGHBORS_ADJACENCY_H_ +#define WAVEMAP_UTILS_NEIGHBORS_ADJACENCY_H_ + +#include "wavemap/common.h" + +namespace wavemap { +enum class AdjacencyType : int { kVertex = 0, kEdge, kFace, kCube }; + +using AdjacencyMask = uint8_t; + +constexpr AdjacencyMask kAdjacencyNone = AdjacencyMask{0}; + +template +constexpr AdjacencyMask kAdjacencyAnyDisjoint = + static_cast((1 << dim) - 1); + +constexpr AdjacencyMask kAdjacencyAny = static_cast(-1); +} // namespace wavemap + +#endif // WAVEMAP_UTILS_NEIGHBORS_ADJACENCY_H_ diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/grid_adjacency.h b/libraries/wavemap/include/wavemap/utils/neighbors/grid_adjacency.h new file mode 100644 index 000000000..7da366949 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/neighbors/grid_adjacency.h @@ -0,0 +1,27 @@ +#ifndef WAVEMAP_UTILS_NEIGHBORS_GRID_ADJACENCY_H_ +#define WAVEMAP_UTILS_NEIGHBORS_GRID_ADJACENCY_H_ + +#include "wavemap/common.h" +#include "wavemap/utils/neighbors/adjacency.h" + +namespace wavemap { +template +constexpr std::optional computeAdjacencyType( + const Index& index_1, const Index& index_2); + +template +constexpr std::optional computeAdjacencyType( + const Index& index_offset); + +template +constexpr bool areAdjacent(const Index& index_1, const Index& index_2, + AdjacencyMask adjacency_mask); + +template +constexpr bool isAdjacent(const Index& index_offset, + AdjacencyMask adjacency_mask); +} // namespace wavemap + +#include "wavemap/utils/neighbors/impl/grid_adjacency_inl.h" + +#endif // WAVEMAP_UTILS_NEIGHBORS_GRID_ADJACENCY_H_ diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/grid_neighborhood.h b/libraries/wavemap/include/wavemap/utils/neighbors/grid_neighborhood.h index 58f4bee30..db5fcbca9 100644 --- a/libraries/wavemap/include/wavemap/utils/neighbors/grid_neighborhood.h +++ b/libraries/wavemap/include/wavemap/utils/neighbors/grid_neighborhood.h @@ -2,16 +2,32 @@ #define WAVEMAP_UTILS_NEIGHBORS_GRID_NEIGHBORHOOD_H_ #include "wavemap/common.h" +#include "wavemap/utils/iterate/grid_iterator.h" #include "wavemap/utils/math/int_math.h" +#include "wavemap/utils/neighbors/adjacency.h" +#include "wavemap/utils/neighbors/grid_adjacency.h" namespace wavemap { template -struct grid_neighborhood { - static constexpr int kNumNeighbors = int_math::pow(3, dim) - 1; +struct GridNeighborhood { + static constexpr int numNeighbors(AdjacencyType adjacency_type); + static constexpr int numNeighbors(AdjacencyMask adjacency_mask); - static std::array, kNumNeighbors> generateIndexOffsets(); - static std::array generateDistanceOffsets( - FloatingPoint cell_width); + template + static std::array, + GridNeighborhood::numNeighbors(adjacency_mask)> + generateIndexOffsets(); + + static auto generateIndexOffsetsAllDisjointAdjacent() { + return generateIndexOffsets>(); + } + static auto generateIndexOffsetsAllAdjacent() { + return generateIndexOffsets(); + } + + template + static constexpr std::array computeOffsetLengths( + const std::array& offsets, FloatingPoint scale); }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_adjacency_inl.h b/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_adjacency_inl.h new file mode 100644 index 000000000..733f9547d --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_adjacency_inl.h @@ -0,0 +1,42 @@ +#ifndef WAVEMAP_UTILS_NEIGHBORS_IMPL_GRID_ADJACENCY_INL_H_ +#define WAVEMAP_UTILS_NEIGHBORS_IMPL_GRID_ADJACENCY_INL_H_ + +#include "wavemap/utils/bits/bit_operations.h" + +namespace wavemap { +template +constexpr std::optional computeAdjacencyType( + const Index& index_1, const Index& index_2) { + return computeAdjacencyType(index_1 - index_2); +} + +template +constexpr std::optional computeAdjacencyType( + const Index& index_offset) { + static_assert(dim <= 3); + const auto offset_abs = index_offset.cwiseAbs(); + if ((1 < offset_abs.array()).any()) { + return std::nullopt; + } + const int num_offset_axes = offset_abs.sum(); + return static_cast(dim - num_offset_axes); +} + +template +constexpr bool areAdjacent(const Index& index_1, const Index& index_2, + AdjacencyMask adjacency_mask) { + return isAdjacent(index_1 - index_2, adjacency_mask); +} + +template +constexpr bool isAdjacent(const Index& index_offset, + AdjacencyMask adjacency_mask) { + if (const auto adjacency = computeAdjacencyType(index_offset); adjacency) { + return bit_ops::is_bit_set(adjacency_mask, + static_cast(adjacency.value())); + } + return false; +} +} // namespace wavemap + +#endif // WAVEMAP_UTILS_NEIGHBORS_IMPL_GRID_ADJACENCY_INL_H_ diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_neighborhood_inl.h b/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_neighborhood_inl.h index ad3829de7..1c7286987 100644 --- a/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_neighborhood_inl.h +++ b/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_neighborhood_inl.h @@ -1,34 +1,60 @@ #ifndef WAVEMAP_UTILS_NEIGHBORS_IMPL_GRID_NEIGHBORHOOD_INL_H_ #define WAVEMAP_UTILS_NEIGHBORS_IMPL_GRID_NEIGHBORHOOD_INL_H_ -#include "wavemap/utils/iterate/grid_iterator.h" +#include namespace wavemap { template -std::array, grid_neighborhood::kNumNeighbors> -grid_neighborhood::generateIndexOffsets() { - std::array, kNumNeighbors> neighbor_offsets{}; +constexpr int GridNeighborhood::numNeighbors( + AdjacencyType adjacency_type) { + return int_math::exp2(dim - static_cast(adjacency_type)) * + int_math::binomial(dim, static_cast(adjacency_type)); +} + +template +constexpr int GridNeighborhood::numNeighbors( + AdjacencyMask adjacency_mask) { + int num_neighbors = 0; + for (int adjacency_type = 0; adjacency_type <= dim; ++adjacency_type) { + if (bit_ops::is_bit_set(adjacency_mask, adjacency_type)) { + num_neighbors += numNeighbors(static_cast(adjacency_type)); + } + } + return num_neighbors; +} + +template +template +std::array, GridNeighborhood::numNeighbors(adjacency_mask)> +GridNeighborhood::generateIndexOffsets() { + static_assert(dim <= 3); + // Initialize the array + constexpr int num_neighbors = numNeighbors(adjacency_mask); + std::array, num_neighbors> neighbor_offsets{}; + // Iterate over all possible neighbor offsets, storing the ones that match size_t array_idx = 0u; - for (const Index& index : + for (const Index& offset : Grid(-Index::Ones(), Index::Ones())) { - if (index != Index::Zero()) { - neighbor_offsets[array_idx] = index; + // Add the offset if its adjacency type matches the adjacency mask + if (adjacency_mask == kAdjacencyAny || isAdjacent(offset, adjacency_mask)) { + neighbor_offsets[array_idx] = offset; ++array_idx; } } + // Return return neighbor_offsets; } template -std::array::kNumNeighbors> -grid_neighborhood::generateDistanceOffsets(FloatingPoint cell_width) { - std::array distance_offsets{}; - size_t array_idx = 0u; - for (const Index& index_offset : generateIndexOffsets()) { - distance_offsets[array_idx] = - cell_width * index_offset.template cast().norm(); - ++array_idx; - } +template +constexpr std::array +GridNeighborhood::computeOffsetLengths( + const std::array& offsets, FloatingPoint scale) { + std::array distance_offsets{}; + std::transform(offsets.begin(), offsets.end(), distance_offsets.begin(), + [scale](const VectorT& offset) { + return scale * offset.template cast().norm(); + }); return distance_offsets; } } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/impl/ndtree_adjacency_inl.h b/libraries/wavemap/include/wavemap/utils/neighbors/impl/ndtree_adjacency_inl.h new file mode 100644 index 000000000..d92274702 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/neighbors/impl/ndtree_adjacency_inl.h @@ -0,0 +1,6 @@ +#ifndef WAVEMAP_UTILS_NEIGHBORS_IMPL_NDTREE_ADJACENCY_INL_H_ +#define WAVEMAP_UTILS_NEIGHBORS_IMPL_NDTREE_ADJACENCY_INL_H_ + +namespace wavemap {} + +#endif // WAVEMAP_UTILS_NEIGHBORS_IMPL_NDTREE_ADJACENCY_INL_H_ diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/ndtree_adjacency.h b/libraries/wavemap/include/wavemap/utils/neighbors/ndtree_adjacency.h new file mode 100644 index 000000000..ea59d0f7f --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/neighbors/ndtree_adjacency.h @@ -0,0 +1,33 @@ +#ifndef WAVEMAP_UTILS_NEIGHBORS_NDTREE_ADJACENCY_H_ +#define WAVEMAP_UTILS_NEIGHBORS_NDTREE_ADJACENCY_H_ + +#include "wavemap/common.h" +#include "wavemap/indexing/ndtree_index.h" + +namespace wavemap { +// Return true if node_1 and node_2 touch or overlap. +template +static bool areAdjacent(const NdtreeIndex& index_1, + const NdtreeIndex& index_2) { + const IndexElement height_diff = std::abs(index_1.height - index_2.height); + const IndexElement width = int_math::exp2(height_diff); + + const NdtreeIndex& smallest_node = + index_1.height < index_2.height ? index_1 : index_2; + const NdtreeIndex& biggest_node = + index_1.height < index_2.height ? index_2 : index_1; + + const Index3D biggest_node_min_corner = + int_math::mult_exp2(biggest_node.position, height_diff); + const auto min_neighbor = biggest_node_min_corner.array() - 1; + const auto max_neighbor = biggest_node_min_corner.array() + width; + + return (min_neighbor <= smallest_node.position.array() || + smallest_node.position.array() <= max_neighbor) + .all(); +} +} // namespace wavemap + +#include "wavemap/utils/neighbors/impl/ndtree_adjacency_inl.h" + +#endif // WAVEMAP_UTILS_NEIGHBORS_NDTREE_ADJACENCY_H_ diff --git a/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h b/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h index aae88be43..4b9d30461 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h +++ b/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h @@ -38,7 +38,7 @@ class FullEuclideanSDFGenerator { private: inline static const auto kNeighborIndexOffsets = - grid_neighborhood<3>::generateIndexOffsets(); + GridNeighborhood<3>::generateIndexOffsetsAllDisjointAdjacent(); const FloatingPoint max_distance_; const OccupancyClassifier classifier_; diff --git a/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h b/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h index 2c358f2f3..f65dd1d76 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h +++ b/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h @@ -23,7 +23,7 @@ class QuasiEuclideanSDFGenerator { private: inline static const auto kNeighborIndexOffsets = - grid_neighborhood<3>::generateIndexOffsets(); + GridNeighborhood<3>::generateIndexOffsetsAllDisjointAdjacent(); const FloatingPoint max_distance_; const OccupancyClassifier classifier_; diff --git a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc index 03d1c6359..eb48e0a58 100644 --- a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc @@ -103,8 +103,8 @@ void QuasiEuclideanSDFGenerator::propagate( // Precompute the neighbor distance offsets const FloatingPoint min_cell_width = occupancy_map.getMinCellWidth(); const auto neighbor_distance_offsets = - grid_neighborhood<3>::generateDistanceOffsets(min_cell_width); - CHECK_EQ(kNeighborIndexOffsets.size(), neighbor_distance_offsets.size()); + GridNeighborhood<3>::computeOffsetLengths(kNeighborIndexOffsets, + min_cell_width); const FloatingPoint half_max_neighbor_distance_offset = 0.5f * std::sqrt(3.f) * min_cell_width + 1e-3f; diff --git a/libraries/wavemap/test/src/utils/test_adjacency.cc b/libraries/wavemap/test/src/utils/test_adjacency.cc new file mode 100644 index 000000000..1f0d02038 --- /dev/null +++ b/libraries/wavemap/test/src/utils/test_adjacency.cc @@ -0,0 +1,64 @@ +#include + +#include "wavemap/utils/bits/bit_operations.h" +#include "wavemap/utils/neighbors/adjacency.h" + +namespace wavemap::neighbors { +TEST(AdjacencyTest, AdjacencyMask) { + // Check that the "no adjacency" mask tests negative for any adjacency type + EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyNone, + static_cast(AdjacencyType::kVertex)), + false); + EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyNone, + static_cast(AdjacencyType::kEdge)), + false); + EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyNone, + static_cast(AdjacencyType::kFace)), + false); + EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyNone, + static_cast(AdjacencyType::kCube)), + false); + + // Check that the "any adjacency" mask tests positive for any adjacency type + EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAny, + static_cast(AdjacencyType::kVertex)), + true); + EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAny, + static_cast(AdjacencyType::kEdge)), + true); + EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAny, + static_cast(AdjacencyType::kFace)), + true); + EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAny, + static_cast(AdjacencyType::kCube)), + true); + + // Check the "any disjoint adjacency" mask for 2D grids + EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAnyDisjoint<2>, + static_cast(AdjacencyType::kVertex)), + true); + EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAnyDisjoint<2>, + static_cast(AdjacencyType::kEdge)), + true); + EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAnyDisjoint<2>, + static_cast(AdjacencyType::kFace)), + false); + EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAnyDisjoint<2>, + static_cast(AdjacencyType::kCube)), + false); + + // Check the "any disjoint adjacency" mask for 3D grids + EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAnyDisjoint<3>, + static_cast(AdjacencyType::kVertex)), + true); + EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAnyDisjoint<3>, + static_cast(AdjacencyType::kEdge)), + true); + EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAnyDisjoint<3>, + static_cast(AdjacencyType::kFace)), + true); + EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAnyDisjoint<3>, + static_cast(AdjacencyType::kCube)), + false); +} +} // namespace wavemap::neighbors diff --git a/libraries/wavemap/test/src/utils/test_bit_manipulation.cc b/libraries/wavemap/test/src/utils/test_bit_manipulation.cc index ab230d69c..55a9bc731 100644 --- a/libraries/wavemap/test/src/utils/test_bit_manipulation.cc +++ b/libraries/wavemap/test/src/utils/test_bit_manipulation.cc @@ -8,6 +8,16 @@ namespace wavemap { using BitManipulationTest = FixtureBase; +TEST_F(BitManipulationTest, IsBitSet) { + for (int iteration = 0; iteration < 1000; ++iteration) { + const auto bitstring = + getRandomInteger(0, std::numeric_limits::max()); + const auto bit_index = getRandomInteger(0, 31); + EXPECT_EQ(bit_ops::is_bit_set(bitstring, bit_index), + static_cast((bitstring >> bit_index) & 1)); + } +} + TEST_F(BitManipulationTest, RotateLeft) { // Test default template argument deduction EXPECT_EQ(bit_ops::rotate_left(0b1011, 3), 0b1011000); diff --git a/libraries/wavemap/test/src/utils/test_grid_adjacency.cc b/libraries/wavemap/test/src/utils/test_grid_adjacency.cc new file mode 100644 index 000000000..bf2f61208 --- /dev/null +++ b/libraries/wavemap/test/src/utils/test_grid_adjacency.cc @@ -0,0 +1 @@ +#include "wavemap/utils/neighbors/grid_adjacency.h" diff --git a/libraries/wavemap/test/src/utils/test_grid_neighborhood.cc b/libraries/wavemap/test/src/utils/test_grid_neighborhood.cc new file mode 100644 index 000000000..f2b739da9 --- /dev/null +++ b/libraries/wavemap/test/src/utils/test_grid_neighborhood.cc @@ -0,0 +1,78 @@ +#include + +#include "wavemap/test/fixture_base.h" +#include "wavemap/utils/neighbors/grid_neighborhood.h" +#include "wavemap/utils/print/eigen.h" + +namespace wavemap::neighbors { +template +class GridNeighborhoodTest : public FixtureBase {}; + +using Dimensions = ::testing::Types, + std::integral_constant, + std::integral_constant>; +TYPED_TEST_SUITE(GridNeighborhoodTest, Dimensions, ); + +TYPED_TEST(GridNeighborhoodTest, NumNeighborsForAdjacencyType) { + constexpr int kDim = TypeParam::value; + constexpr int kDimIdx = kDim - 1; + const std::array num_vertices = {2, 4, 8}; + const std::array num_edges = {1, 4, 12}; + const std::array num_faces = {0, 1, 6}; + const std::array num_cubes = {0, 0, 1}; + EXPECT_EQ(GridNeighborhood::numNeighbors(AdjacencyType::kVertex), + num_vertices[kDimIdx]); + EXPECT_EQ(GridNeighborhood::numNeighbors(AdjacencyType::kEdge), + num_edges[kDimIdx]); + EXPECT_EQ(GridNeighborhood::numNeighbors(AdjacencyType::kFace), + num_faces[kDimIdx]); + EXPECT_EQ(GridNeighborhood::numNeighbors(AdjacencyType::kCube), + num_cubes[kDimIdx]); +} + +TYPED_TEST(GridNeighborhoodTest, NumNeighborsForAdjacencyMask) { + constexpr int kDim = TypeParam::value; + constexpr int kDimIdx = kDim - 1; + const std::array num_no_adjacency_neighbors = {0, 0, 0}; + const std::array num_any_disjoint_adjacency_neighbors = {2, 8, 26}; + const std::array num_any_adjacency_neighbors = {3, 9, 27}; + EXPECT_EQ(GridNeighborhood::numNeighbors(kAdjacencyNone), + num_no_adjacency_neighbors[kDimIdx]); + EXPECT_EQ(GridNeighborhood::numNeighbors(kAdjacencyAnyDisjoint), + num_any_disjoint_adjacency_neighbors[kDimIdx]); + EXPECT_EQ(GridNeighborhood::numNeighbors(kAdjacencyAny), + num_any_adjacency_neighbors[kDimIdx]); +} + +TYPED_TEST(GridNeighborhoodTest, IndexOffsets) { + constexpr int kDim = TypeParam::value; + + // All neighbors with disjoint adjacency + const auto all_disjoint_adjacent_offsets = GridNeighborhood< + kDim>::template generateIndexOffsets>(); + EXPECT_EQ(all_disjoint_adjacent_offsets.size(), + GridNeighborhood::numNeighbors(kAdjacencyAnyDisjoint)); + for (const auto& offset : + Grid(Index::Constant(-1), Index::Constant(1))) { + if (!offset.isZero()) { + EXPECT_EQ(std::count(all_disjoint_adjacent_offsets.begin(), + all_disjoint_adjacent_offsets.end(), offset), + 1) + << "For offset " << print::eigen::oneLine(offset); + } + } + + // All neighbors, including the node itself (i.e. no offset) + const auto all_adjacent_offsets = + GridNeighborhood::template generateIndexOffsets(); + EXPECT_EQ(all_adjacent_offsets.size(), + GridNeighborhood::numNeighbors(kAdjacencyAny)); + for (const auto& offset : + Grid(Index::Constant(-1), Index::Constant(1))) { + EXPECT_EQ(std::count(all_adjacent_offsets.begin(), + all_adjacent_offsets.end(), offset), + 1) + << "For offset " << print::eigen::oneLine(offset); + } +} +} // namespace wavemap::neighbors diff --git a/libraries/wavemap/test/src/utils/test_int_math.cc b/libraries/wavemap/test/src/utils/test_int_math.cc index 162ecc83c..ea6106425 100644 --- a/libraries/wavemap/test/src/utils/test_int_math.cc +++ b/libraries/wavemap/test/src/utils/test_int_math.cc @@ -127,4 +127,46 @@ TEST(IntMathTest, PowSequence) { EXPECT_EQ(pow_sequence_base_min_2_length_4, expected_pow_sequence_base_min_2_length_4); } + +TEST(IntMathTest, Factorial) { + EXPECT_EQ(int_math::factorial(0), 1); + EXPECT_EQ(int_math::factorial(1), 1); + EXPECT_EQ(int_math::factorial(2), 2); + EXPECT_EQ(int_math::factorial(3), 6); + EXPECT_EQ(int_math::factorial(4), 24); + EXPECT_EQ(int_math::factorial(5), 120); + EXPECT_EQ(int_math::factorial(6), 720); + EXPECT_EQ(int_math::factorial(7), 5040); + EXPECT_EQ(int_math::factorial(8), 40320); + EXPECT_EQ(int_math::factorial(9), 362880); + EXPECT_EQ(int_math::factorial(10), 3628800); +} + +TEST(IntMathTest, Binomial) { + EXPECT_EQ(int_math::binomial(0, 0), 1); + EXPECT_EQ(int_math::binomial(0, 1), 0); + EXPECT_EQ(int_math::binomial(0, 2), 0); + EXPECT_EQ(int_math::binomial(0, 3), 0); + EXPECT_EQ(int_math::binomial(0, 4), 0); + EXPECT_EQ(int_math::binomial(1, 0), 1); + EXPECT_EQ(int_math::binomial(1, 1), 1); + EXPECT_EQ(int_math::binomial(1, 2), 0); + EXPECT_EQ(int_math::binomial(1, 3), 0); + EXPECT_EQ(int_math::binomial(1, 4), 0); + EXPECT_EQ(int_math::binomial(2, 0), 1); + EXPECT_EQ(int_math::binomial(2, 1), 2); + EXPECT_EQ(int_math::binomial(2, 2), 1); + EXPECT_EQ(int_math::binomial(2, 3), 0); + EXPECT_EQ(int_math::binomial(2, 4), 0); + EXPECT_EQ(int_math::binomial(3, 0), 1); + EXPECT_EQ(int_math::binomial(3, 1), 3); + EXPECT_EQ(int_math::binomial(3, 2), 3); + EXPECT_EQ(int_math::binomial(3, 3), 1); + EXPECT_EQ(int_math::binomial(3, 4), 0); + EXPECT_EQ(int_math::binomial(4, 0), 1); + EXPECT_EQ(int_math::binomial(4, 1), 4); + EXPECT_EQ(int_math::binomial(4, 2), 6); + EXPECT_EQ(int_math::binomial(4, 3), 4); + EXPECT_EQ(int_math::binomial(4, 4), 1); +} } // namespace wavemap diff --git a/libraries/wavemap/test/src/utils/test_ndtree_adjacency.cc b/libraries/wavemap/test/src/utils/test_ndtree_adjacency.cc new file mode 100644 index 000000000..170afbc8a --- /dev/null +++ b/libraries/wavemap/test/src/utils/test_ndtree_adjacency.cc @@ -0,0 +1 @@ +#include "wavemap/utils/neighbors/ndtree_adjacency.h" diff --git a/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc b/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc index b60d02116..8c59d53e4 100644 --- a/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc +++ b/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc @@ -1,10 +1,10 @@ #include "wavemap_rviz_plugin/visuals/cell_selector.h" -#include +#include namespace wavemap::rviz_plugin { static const auto kNeighborOffsets = - grid_neighborhood<3>::generateIndexOffsets(); + neighbors::generateIndexOffsetLut<3, kAnyDisjointAdjacency<3>>(); CellSelector::CellSelector(rviz::Property* submenu_root_property, std::function redraw_map) From d5acc16796d55c812e2c5d5720b6af410e39366b Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Sat, 2 Dec 2023 20:46:05 +0100 Subject: [PATCH 034/159] Make Adjacency types and masks more intuitive to use --- .../include/wavemap/config/type_selector.h | 5 +- .../wavemap/utils/neighbors/adjacency.h | 31 +++- .../wavemap/utils/neighbors/grid_adjacency.h | 29 +++- .../utils/neighbors/grid_neighborhood.h | 15 +- .../utils/neighbors/impl/adjacency_inl.h | 28 ++++ .../utils/neighbors/impl/grid_adjacency_inl.h | 38 +++-- .../neighbors/impl/grid_neighborhood_inl.h | 30 ++-- .../utils/sdf/full_euclidean_sdf_generator.h | 2 +- .../utils/sdf/quasi_euclidean_sdf_generator.h | 2 +- .../wavemap/test/src/utils/test_adjacency.cc | 93 ++++++------ .../test/src/utils/test_grid_adjacency.cc | 141 ++++++++++++++++++ .../test/src/utils/test_grid_neighborhood.cc | 26 ++-- .../src/visuals/cell_selector.cc | 4 +- 13 files changed, 321 insertions(+), 123 deletions(-) create mode 100644 libraries/wavemap/include/wavemap/utils/neighbors/impl/adjacency_inl.h diff --git a/libraries/wavemap/include/wavemap/config/type_selector.h b/libraries/wavemap/include/wavemap/config/type_selector.h index 77ff254ad..aa66bcce2 100644 --- a/libraries/wavemap/include/wavemap/config/type_selector.h +++ b/libraries/wavemap/include/wavemap/config/type_selector.h @@ -46,7 +46,7 @@ struct TypeSelector { explicit operator TypeName() const { return typeIdToStr(id_); } explicit operator TypeId() const { return id_; } TypeName toStr() const { return typeIdToStr(id_); } - TypeId toTypeId() const { return id_; } + constexpr TypeId toTypeId() const { return id_; } // Comparison operators friend bool operator==(const DerivedTypeSelectorT& lhs, @@ -62,6 +62,7 @@ struct TypeSelector { bool isValid() const { return isValidTypeId(id_); } // Static methods for convenience + // TODO(victorr): Simplify name to "toStr()" static TypeName typeIdToStr(TypeId type_id) { if (isValidTypeId(type_id)) { return DerivedTypeSelectorT::names[static_cast(type_id)]; @@ -69,7 +70,9 @@ struct TypeSelector { return "Invalid"; } } + // TODO(victorr): Simplify name to "toTypeId()" static TypeId strToTypeId(const std::string& name); + // TODO(victorr): Simplify name to "isValid()" static bool isValidTypeId(TypeId type_id) { return 0 <= type_id && static_cast(type_id) < DerivedTypeSelectorT::names.size(); diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/adjacency.h b/libraries/wavemap/include/wavemap/utils/neighbors/adjacency.h index e1c626856..3dcc4334b 100644 --- a/libraries/wavemap/include/wavemap/utils/neighbors/adjacency.h +++ b/libraries/wavemap/include/wavemap/utils/neighbors/adjacency.h @@ -2,19 +2,34 @@ #define WAVEMAP_UTILS_NEIGHBORS_ADJACENCY_H_ #include "wavemap/common.h" +#include "wavemap/config/type_selector.h" namespace wavemap { -enum class AdjacencyType : int { kVertex = 0, kEdge, kFace, kCube }; +struct Adjacency : TypeSelector { + using TypeSelector::TypeSelector; + using Mask = uint8_t; -using AdjacencyMask = uint8_t; + enum Id : TypeId { + kSharedVertex, + kSharedEdge, + kSharedFace, + kSharedCube, + kAnyDisjoint, + kAny, + kNone + }; + static constexpr std::array names = { + "shared_vertex", "shared_edge", "shared_face", "shared_cube", + "any_disjoint", "any", "none"}; -constexpr AdjacencyMask kAdjacencyNone = AdjacencyMask{0}; + template + static constexpr Mask toMask(Id type_id); -template -constexpr AdjacencyMask kAdjacencyAnyDisjoint = - static_cast((1 << dim) - 1); - -constexpr AdjacencyMask kAdjacencyAny = static_cast(-1); + template + constexpr Mask toMask() const; +}; } // namespace wavemap +#include "wavemap/utils/neighbors/impl/adjacency_inl.h" + #endif // WAVEMAP_UTILS_NEIGHBORS_ADJACENCY_H_ diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/grid_adjacency.h b/libraries/wavemap/include/wavemap/utils/neighbors/grid_adjacency.h index 7da366949..6c20d8cd4 100644 --- a/libraries/wavemap/include/wavemap/utils/neighbors/grid_adjacency.h +++ b/libraries/wavemap/include/wavemap/utils/neighbors/grid_adjacency.h @@ -6,20 +6,37 @@ namespace wavemap { template -constexpr std::optional computeAdjacencyType( - const Index& index_1, const Index& index_2); +constexpr Adjacency::Id computeAdjacency(const Index& index_1, + const Index& index_2); template -constexpr std::optional computeAdjacencyType( - const Index& index_offset); +constexpr Adjacency::Id computeAdjacency(const Index& index_offset); template constexpr bool areAdjacent(const Index& index_1, const Index& index_2, - AdjacencyMask adjacency_mask); + Adjacency::Id adjacency); template constexpr bool isAdjacent(const Index& index_offset, - AdjacencyMask adjacency_mask); + Adjacency::Id adjacency); + +template +constexpr bool areAdjacent(const Index& index_1, const Index& index_2, + Adjacency::Mask adjacency_mask); + +template +constexpr bool isAdjacent(const Index& index_offset, + Adjacency::Mask adjacency_mask); + +// Forbid implicit conversions +template +constexpr bool areAdjacent(const Index& index_1, const Index& index_2, + T adjacency_mask) = delete; + +// Forbid implicit conversions +template +constexpr bool isAdjacent(const Index& index_offset, + T adjacency_mask) = delete; } // namespace wavemap #include "wavemap/utils/neighbors/impl/grid_adjacency_inl.h" diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/grid_neighborhood.h b/libraries/wavemap/include/wavemap/utils/neighbors/grid_neighborhood.h index db5fcbca9..e9cf3d3f1 100644 --- a/libraries/wavemap/include/wavemap/utils/neighbors/grid_neighborhood.h +++ b/libraries/wavemap/include/wavemap/utils/neighbors/grid_neighborhood.h @@ -10,21 +10,12 @@ namespace wavemap { template struct GridNeighborhood { - static constexpr int numNeighbors(AdjacencyType adjacency_type); - static constexpr int numNeighbors(AdjacencyMask adjacency_mask); + static constexpr int numNeighbors(Adjacency::Id adjacency); - template - static std::array, - GridNeighborhood::numNeighbors(adjacency_mask)> + template + static std::array, GridNeighborhood::numNeighbors(adjacency)> generateIndexOffsets(); - static auto generateIndexOffsetsAllDisjointAdjacent() { - return generateIndexOffsets>(); - } - static auto generateIndexOffsetsAllAdjacent() { - return generateIndexOffsets(); - } - template static constexpr std::array computeOffsetLengths( const std::array& offsets, FloatingPoint scale); diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/impl/adjacency_inl.h b/libraries/wavemap/include/wavemap/utils/neighbors/impl/adjacency_inl.h new file mode 100644 index 000000000..fffd68eaf --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/neighbors/impl/adjacency_inl.h @@ -0,0 +1,28 @@ +#ifndef WAVEMAP_UTILS_NEIGHBORS_IMPL_ADJACENCY_INL_H_ +#define WAVEMAP_UTILS_NEIGHBORS_IMPL_ADJACENCY_INL_H_ + +namespace wavemap { +template +constexpr Adjacency::Mask Adjacency::toMask(Adjacency::Id type_id) { + static_assert(dim <= 3); + if (0 <= type_id && type_id <= 3) { + return 1 << type_id; + } + switch (type_id) { + case kAnyDisjoint: + return (1 << dim) - 1; + case kAny: + return static_cast(-1); + case kNone: + default: + return 0; + } +} + +template +constexpr Adjacency::Mask Adjacency::toMask() const { + return toMask(id_); +} +} // namespace wavemap + +#endif // WAVEMAP_UTILS_NEIGHBORS_IMPL_ADJACENCY_INL_H_ diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_adjacency_inl.h b/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_adjacency_inl.h index 733f9547d..663d15fa8 100644 --- a/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_adjacency_inl.h +++ b/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_adjacency_inl.h @@ -5,37 +5,45 @@ namespace wavemap { template -constexpr std::optional computeAdjacencyType( - const Index& index_1, const Index& index_2) { - return computeAdjacencyType(index_1 - index_2); +constexpr Adjacency::Id computeAdjacency(const Index& index_1, + const Index& index_2) { + return computeAdjacency(index_1 - index_2); } template -constexpr std::optional computeAdjacencyType( - const Index& index_offset) { +constexpr Adjacency::Id computeAdjacency(const Index& index_offset) { static_assert(dim <= 3); const auto offset_abs = index_offset.cwiseAbs(); if ((1 < offset_abs.array()).any()) { - return std::nullopt; + return Adjacency::kNone; } const int num_offset_axes = offset_abs.sum(); - return static_cast(dim - num_offset_axes); + return Adjacency::Id{dim - num_offset_axes}; } template constexpr bool areAdjacent(const Index& index_1, const Index& index_2, - AdjacencyMask adjacency_mask) { - return isAdjacent(index_1 - index_2, adjacency_mask); + Adjacency::Id adjacency) { + return areAdjacent(index_1, index_2, Adjacency::toMask(adjacency)); } template constexpr bool isAdjacent(const Index& index_offset, - AdjacencyMask adjacency_mask) { - if (const auto adjacency = computeAdjacencyType(index_offset); adjacency) { - return bit_ops::is_bit_set(adjacency_mask, - static_cast(adjacency.value())); - } - return false; + Adjacency::Id adjacency) { + return isAdjacent(index_offset, Adjacency::toMask(adjacency)); +} + +template +constexpr bool areAdjacent(const Index& index_1, const Index& index_2, + Adjacency::Mask adjacency_mask) { + return isAdjacent(index_1 - index_2, adjacency_mask); +} + +template +constexpr bool isAdjacent(const Index& index_offset, + Adjacency::Mask adjacency_mask) { + const auto adjacency = computeAdjacency(index_offset); + return Adjacency::toMask(adjacency) & adjacency_mask; } } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_neighborhood_inl.h b/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_neighborhood_inl.h index 1c7286987..78c0b6ad8 100644 --- a/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_neighborhood_inl.h +++ b/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_neighborhood_inl.h @@ -5,38 +5,32 @@ namespace wavemap { template -constexpr int GridNeighborhood::numNeighbors( - AdjacencyType adjacency_type) { - return int_math::exp2(dim - static_cast(adjacency_type)) * - int_math::binomial(dim, static_cast(adjacency_type)); -} - -template -constexpr int GridNeighborhood::numNeighbors( - AdjacencyMask adjacency_mask) { +constexpr int GridNeighborhood::numNeighbors(Adjacency::Id adjacency) { + const Adjacency::Mask adjacency_mask = Adjacency::toMask(adjacency); int num_neighbors = 0; - for (int adjacency_type = 0; adjacency_type <= dim; ++adjacency_type) { - if (bit_ops::is_bit_set(adjacency_mask, adjacency_type)) { - num_neighbors += numNeighbors(static_cast(adjacency_type)); + for (int polygon_dim = 0; polygon_dim <= dim; ++polygon_dim) { + if (bit_ops::is_bit_set(adjacency_mask, polygon_dim)) { + const int num_polygons_in_boundary = + int_math::exp2(dim - static_cast(polygon_dim)) * + int_math::binomial(dim, static_cast(polygon_dim)); + num_neighbors += num_polygons_in_boundary; } } return num_neighbors; } template -template -std::array, GridNeighborhood::numNeighbors(adjacency_mask)> +template +std::array, GridNeighborhood::numNeighbors(adjacency)> GridNeighborhood::generateIndexOffsets() { - static_assert(dim <= 3); // Initialize the array - constexpr int num_neighbors = numNeighbors(adjacency_mask); - std::array, num_neighbors> neighbor_offsets{}; + std::array, numNeighbors(adjacency)> neighbor_offsets{}; // Iterate over all possible neighbor offsets, storing the ones that match size_t array_idx = 0u; for (const Index& offset : Grid(-Index::Ones(), Index::Ones())) { // Add the offset if its adjacency type matches the adjacency mask - if (adjacency_mask == kAdjacencyAny || isAdjacent(offset, adjacency_mask)) { + if (adjacency == Adjacency::kAny || isAdjacent(offset, adjacency)) { neighbor_offsets[array_idx] = offset; ++array_idx; } diff --git a/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h b/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h index 4b9d30461..07101e92a 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h +++ b/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h @@ -38,7 +38,7 @@ class FullEuclideanSDFGenerator { private: inline static const auto kNeighborIndexOffsets = - GridNeighborhood<3>::generateIndexOffsetsAllDisjointAdjacent(); + GridNeighborhood<3>::generateIndexOffsets(); const FloatingPoint max_distance_; const OccupancyClassifier classifier_; diff --git a/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h b/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h index f65dd1d76..e960a31a2 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h +++ b/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h @@ -23,7 +23,7 @@ class QuasiEuclideanSDFGenerator { private: inline static const auto kNeighborIndexOffsets = - GridNeighborhood<3>::generateIndexOffsetsAllDisjointAdjacent(); + GridNeighborhood<3>::generateIndexOffsets(); const FloatingPoint max_distance_; const OccupancyClassifier classifier_; diff --git a/libraries/wavemap/test/src/utils/test_adjacency.cc b/libraries/wavemap/test/src/utils/test_adjacency.cc index 1f0d02038..6adad8ba4 100644 --- a/libraries/wavemap/test/src/utils/test_adjacency.cc +++ b/libraries/wavemap/test/src/utils/test_adjacency.cc @@ -1,64 +1,65 @@ #include +#include "wavemap/test/fixture_base.h" #include "wavemap/utils/bits/bit_operations.h" #include "wavemap/utils/neighbors/adjacency.h" -namespace wavemap::neighbors { -TEST(AdjacencyTest, AdjacencyMask) { +namespace wavemap { +template +class AdjacencyTest : public FixtureBase {}; + +using Dimensions = ::testing::Types, + std::integral_constant, + std::integral_constant>; +TYPED_TEST_SUITE(AdjacencyTest, Dimensions, ); + +TYPED_TEST(AdjacencyTest, AdjacencyMask) { + constexpr int kDim = TypeParam::value; + // Check that the "no adjacency" mask tests negative for any adjacency type - EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyNone, - static_cast(AdjacencyType::kVertex)), + EXPECT_EQ(bit_ops::is_bit_set(Adjacency::toMask(Adjacency::kNone), + static_cast(Adjacency::kSharedVertex)), false); - EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyNone, - static_cast(AdjacencyType::kEdge)), + EXPECT_EQ(bit_ops::is_bit_set(Adjacency::toMask(Adjacency::kNone), + static_cast(Adjacency::kSharedEdge)), false); - EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyNone, - static_cast(AdjacencyType::kFace)), + EXPECT_EQ(bit_ops::is_bit_set(Adjacency::toMask(Adjacency::kNone), + static_cast(Adjacency::kSharedFace)), false); - EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyNone, - static_cast(AdjacencyType::kCube)), + EXPECT_EQ(bit_ops::is_bit_set(Adjacency::toMask(Adjacency::kNone), + static_cast(Adjacency::kSharedCube)), false); // Check that the "any adjacency" mask tests positive for any adjacency type - EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAny, - static_cast(AdjacencyType::kVertex)), + EXPECT_EQ(bit_ops::is_bit_set(Adjacency::toMask(Adjacency::kAny), + static_cast(Adjacency::kSharedVertex)), true); - EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAny, - static_cast(AdjacencyType::kEdge)), + EXPECT_EQ(bit_ops::is_bit_set(Adjacency::toMask(Adjacency::kAny), + static_cast(Adjacency::kSharedEdge)), true); - EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAny, - static_cast(AdjacencyType::kFace)), - true); - EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAny, - static_cast(AdjacencyType::kCube)), - true); - - // Check the "any disjoint adjacency" mask for 2D grids - EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAnyDisjoint<2>, - static_cast(AdjacencyType::kVertex)), + EXPECT_EQ(bit_ops::is_bit_set(Adjacency::toMask(Adjacency::kAny), + static_cast(Adjacency::kSharedFace)), true); - EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAnyDisjoint<2>, - static_cast(AdjacencyType::kEdge)), + EXPECT_EQ(bit_ops::is_bit_set(Adjacency::toMask(Adjacency::kAny), + static_cast(Adjacency::kSharedCube)), true); - EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAnyDisjoint<2>, - static_cast(AdjacencyType::kFace)), - false); - EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAnyDisjoint<2>, - static_cast(AdjacencyType::kCube)), - false); - // Check the "any disjoint adjacency" mask for 3D grids - EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAnyDisjoint<3>, - static_cast(AdjacencyType::kVertex)), - true); - EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAnyDisjoint<3>, - static_cast(AdjacencyType::kEdge)), - true); - EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAnyDisjoint<3>, - static_cast(AdjacencyType::kFace)), - true); - EXPECT_EQ(bit_ops::is_bit_set(kAdjacencyAnyDisjoint<3>, - static_cast(AdjacencyType::kCube)), - false); + // Check the "any disjoint adjacency" mask + EXPECT_EQ( + bit_ops::is_bit_set(Adjacency::toMask(Adjacency::kAnyDisjoint), + static_cast(Adjacency::kSharedVertex)), + true); + EXPECT_EQ( + bit_ops::is_bit_set(Adjacency::toMask(Adjacency::kAnyDisjoint), + static_cast(Adjacency::kSharedEdge)), + 1 < kDim); + EXPECT_EQ( + bit_ops::is_bit_set(Adjacency::toMask(Adjacency::kAnyDisjoint), + static_cast(Adjacency::kSharedFace)), + 2 < kDim); + EXPECT_EQ( + bit_ops::is_bit_set(Adjacency::toMask(Adjacency::kAnyDisjoint), + static_cast(Adjacency::kSharedCube)), + 3 < kDim); } -} // namespace wavemap::neighbors +} // namespace wavemap diff --git a/libraries/wavemap/test/src/utils/test_grid_adjacency.cc b/libraries/wavemap/test/src/utils/test_grid_adjacency.cc index bf2f61208..17181b9de 100644 --- a/libraries/wavemap/test/src/utils/test_grid_adjacency.cc +++ b/libraries/wavemap/test/src/utils/test_grid_adjacency.cc @@ -1 +1,142 @@ +#include + +#include "wavemap/test/fixture_base.h" +#include "wavemap/test/geometry_generator.h" #include "wavemap/utils/neighbors/grid_adjacency.h" +#include "wavemap/utils/neighbors/grid_neighborhood.h" + +namespace wavemap { +template +class GridAdjacencyTest : public FixtureBase, public GeometryGenerator {}; + +using Dimensions = ::testing::Types, + std::integral_constant, + std::integral_constant>; +TYPED_TEST_SUITE(GridAdjacencyTest, Dimensions, ); + +TYPED_TEST(GridAdjacencyTest, SharedVertexAdjacency) { + constexpr int kDim = TypeParam::value; + using IndexType = Index; + + const auto vertex_adjacency_offsets = GridNeighborhood< + kDim>::template generateIndexOffsets(); + for (const IndexType& index : + TestFixture::template getRandomIndexVector(100, 100)) { + for (const IndexType& offset : vertex_adjacency_offsets) { + const IndexType neighbor = index + offset; + const auto adjacency_type = Adjacency{computeAdjacency(index, neighbor)}; + EXPECT_TRUE(adjacency_type.isValid()); + EXPECT_EQ(adjacency_type.toTypeId(), Adjacency::kSharedVertex); + EXPECT_TRUE(areAdjacent(index, neighbor, Adjacency::kSharedVertex)); + } + } +} + +TYPED_TEST(GridAdjacencyTest, SharedEdgeAdjacency) { + constexpr int kDim = TypeParam::value; + using IndexType = Index; + + const auto edge_adjacency_offsets = GridNeighborhood< + kDim>::template generateIndexOffsets(); + for (const IndexType& index : + TestFixture::template getRandomIndexVector(100, 100)) { + for (const IndexType& offset : edge_adjacency_offsets) { + const IndexType neighbor = index + offset; + const auto adjacency_type = Adjacency{computeAdjacency(index, neighbor)}; + EXPECT_TRUE(adjacency_type.isValid()); + EXPECT_EQ(adjacency_type.toTypeId(), Adjacency::kSharedEdge); + EXPECT_TRUE(areAdjacent(index, neighbor, Adjacency::kSharedEdge)); + } + } +} + +TYPED_TEST(GridAdjacencyTest, SharedFaceAdjacency) { + constexpr int kDim = TypeParam::value; + using IndexType = Index; + + const auto face_adjacency_offsets = GridNeighborhood< + kDim>::template generateIndexOffsets(); + for (const IndexType& index : + TestFixture::template getRandomIndexVector(100, 100)) { + for (const IndexType& offset : face_adjacency_offsets) { + const IndexType neighbor = index + offset; + const auto adjacency_type = Adjacency{computeAdjacency(index, neighbor)}; + EXPECT_TRUE(adjacency_type.isValid()); + EXPECT_EQ(adjacency_type.toTypeId(), Adjacency::kSharedFace); + EXPECT_TRUE(areAdjacent(index, neighbor, Adjacency::kSharedFace)); + } + } +} + +TYPED_TEST(GridAdjacencyTest, SharedCubeAdjacency) { + constexpr int kDim = TypeParam::value; + using IndexType = Index; + + const auto cube_adjacency_offsets = GridNeighborhood< + kDim>::template generateIndexOffsets(); + for (const IndexType& index : + TestFixture::template getRandomIndexVector(100, 100)) { + for (const IndexType& offset : cube_adjacency_offsets) { + const IndexType neighbor = index + offset; + const auto adjacency_type = Adjacency{computeAdjacency(index, neighbor)}; + EXPECT_TRUE(adjacency_type.isValid()); + EXPECT_EQ(adjacency_type.toTypeId(), Adjacency::kSharedCube); + EXPECT_TRUE(areAdjacent(index, neighbor, Adjacency::kSharedCube)); + } + } +} + +TYPED_TEST(GridAdjacencyTest, AnyDisjointAdjacency) { + constexpr int kDim = TypeParam::value; + using IndexType = Index; + + const auto disjoint_adjacency_offsets = GridNeighborhood< + kDim>::template generateIndexOffsets(); + for (const IndexType& index : + TestFixture::template getRandomIndexVector(100, 100)) { + for (const IndexType& offset : disjoint_adjacency_offsets) { + const IndexType neighbor = index + offset; + const auto adjacency_type = Adjacency{computeAdjacency(index, neighbor)}; + EXPECT_TRUE(adjacency_type.isValid()); + EXPECT_TRUE(areAdjacent(index, neighbor, Adjacency::kAnyDisjoint)); + } + } +} + +TYPED_TEST(GridAdjacencyTest, NoAdjacency) { + constexpr int kDim = TypeParam::value; + using IndexType = Index; + + const auto disjoint_adjacency_offsets = GridNeighborhood< + kDim>::template generateIndexOffsets(); + for (const IndexType& index : + TestFixture::template getRandomIndexVector(100, 100)) { + for (const IndexType& offset : disjoint_adjacency_offsets) { + const IndexType non_adjacent_index = index + 2 * offset; + const auto adjacency_type = + Adjacency{computeAdjacency(index, non_adjacent_index)}; + EXPECT_TRUE(adjacency_type.isValid()); + EXPECT_EQ(adjacency_type, Adjacency::kNone); + EXPECT_FALSE(areAdjacent(index, non_adjacent_index, Adjacency::kAny)); + } + } +} + +TYPED_TEST(GridAdjacencyTest, AnyAdjacency) { + constexpr int kDim = TypeParam::value; + using IndexType = Index; + + const auto any_adjacency_offsets = + GridNeighborhood::template generateIndexOffsets(); + + for (const IndexType& index : + TestFixture::template getRandomIndexVector(100, 100)) { + for (const IndexType& offset : any_adjacency_offsets) { + const IndexType neighbor = index + offset; + const auto adjacency_type = Adjacency{computeAdjacency(index, neighbor)}; + EXPECT_TRUE(adjacency_type.isValid()); + EXPECT_TRUE(areAdjacent(index, neighbor, Adjacency::kAny)); + } + } +} +} // namespace wavemap diff --git a/libraries/wavemap/test/src/utils/test_grid_neighborhood.cc b/libraries/wavemap/test/src/utils/test_grid_neighborhood.cc index f2b739da9..d668ee617 100644 --- a/libraries/wavemap/test/src/utils/test_grid_neighborhood.cc +++ b/libraries/wavemap/test/src/utils/test_grid_neighborhood.cc @@ -4,7 +4,7 @@ #include "wavemap/utils/neighbors/grid_neighborhood.h" #include "wavemap/utils/print/eigen.h" -namespace wavemap::neighbors { +namespace wavemap { template class GridNeighborhoodTest : public FixtureBase {}; @@ -20,13 +20,13 @@ TYPED_TEST(GridNeighborhoodTest, NumNeighborsForAdjacencyType) { const std::array num_edges = {1, 4, 12}; const std::array num_faces = {0, 1, 6}; const std::array num_cubes = {0, 0, 1}; - EXPECT_EQ(GridNeighborhood::numNeighbors(AdjacencyType::kVertex), + EXPECT_EQ(GridNeighborhood::numNeighbors(Adjacency::kSharedVertex), num_vertices[kDimIdx]); - EXPECT_EQ(GridNeighborhood::numNeighbors(AdjacencyType::kEdge), + EXPECT_EQ(GridNeighborhood::numNeighbors(Adjacency::kSharedEdge), num_edges[kDimIdx]); - EXPECT_EQ(GridNeighborhood::numNeighbors(AdjacencyType::kFace), + EXPECT_EQ(GridNeighborhood::numNeighbors(Adjacency::kSharedFace), num_faces[kDimIdx]); - EXPECT_EQ(GridNeighborhood::numNeighbors(AdjacencyType::kCube), + EXPECT_EQ(GridNeighborhood::numNeighbors(Adjacency::kSharedCube), num_cubes[kDimIdx]); } @@ -36,11 +36,11 @@ TYPED_TEST(GridNeighborhoodTest, NumNeighborsForAdjacencyMask) { const std::array num_no_adjacency_neighbors = {0, 0, 0}; const std::array num_any_disjoint_adjacency_neighbors = {2, 8, 26}; const std::array num_any_adjacency_neighbors = {3, 9, 27}; - EXPECT_EQ(GridNeighborhood::numNeighbors(kAdjacencyNone), + EXPECT_EQ(GridNeighborhood::numNeighbors(Adjacency::kNone), num_no_adjacency_neighbors[kDimIdx]); - EXPECT_EQ(GridNeighborhood::numNeighbors(kAdjacencyAnyDisjoint), + EXPECT_EQ(GridNeighborhood::numNeighbors(Adjacency::kAnyDisjoint), num_any_disjoint_adjacency_neighbors[kDimIdx]); - EXPECT_EQ(GridNeighborhood::numNeighbors(kAdjacencyAny), + EXPECT_EQ(GridNeighborhood::numNeighbors(Adjacency::kAny), num_any_adjacency_neighbors[kDimIdx]); } @@ -49,9 +49,9 @@ TYPED_TEST(GridNeighborhoodTest, IndexOffsets) { // All neighbors with disjoint adjacency const auto all_disjoint_adjacent_offsets = GridNeighborhood< - kDim>::template generateIndexOffsets>(); + kDim>::template generateIndexOffsets(); EXPECT_EQ(all_disjoint_adjacent_offsets.size(), - GridNeighborhood::numNeighbors(kAdjacencyAnyDisjoint)); + GridNeighborhood::numNeighbors(Adjacency::kAnyDisjoint)); for (const auto& offset : Grid(Index::Constant(-1), Index::Constant(1))) { if (!offset.isZero()) { @@ -64,9 +64,9 @@ TYPED_TEST(GridNeighborhoodTest, IndexOffsets) { // All neighbors, including the node itself (i.e. no offset) const auto all_adjacent_offsets = - GridNeighborhood::template generateIndexOffsets(); + GridNeighborhood::template generateIndexOffsets(); EXPECT_EQ(all_adjacent_offsets.size(), - GridNeighborhood::numNeighbors(kAdjacencyAny)); + GridNeighborhood::numNeighbors(Adjacency::kAny)); for (const auto& offset : Grid(Index::Constant(-1), Index::Constant(1))) { EXPECT_EQ(std::count(all_adjacent_offsets.begin(), @@ -75,4 +75,4 @@ TYPED_TEST(GridNeighborhoodTest, IndexOffsets) { << "For offset " << print::eigen::oneLine(offset); } } -} // namespace wavemap::neighbors +} // namespace wavemap diff --git a/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc b/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc index 8c59d53e4..071ab8ff9 100644 --- a/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc +++ b/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc @@ -1,10 +1,10 @@ #include "wavemap_rviz_plugin/visuals/cell_selector.h" -#include +#include namespace wavemap::rviz_plugin { static const auto kNeighborOffsets = - neighbors::generateIndexOffsetLut<3, kAnyDisjointAdjacency<3>>(); + GridNeighborhood<3>::generateIndexOffsets(); CellSelector::CellSelector(rviz::Property* submenu_root_property, std::function redraw_map) From 1f9f6f586895b679032fe8f4465272fb533b1aa3 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Sat, 2 Dec 2023 21:25:26 +0100 Subject: [PATCH 035/159] Refactor the TypeSelector class to support nicer syntax --- .../wavemap/config/impl/type_selector_inl.h | 51 +++++++++++++- .../wavemap/config/impl/value_with_unit_inl.h | 12 ++-- .../include/wavemap/config/type_selector.h | 69 ++++--------------- .../impl/continuous_beam_inl.h | 2 +- .../impl/continuous_ray_inl.h | 2 +- .../src/integrator/integrator_factory.cc | 10 +-- .../measurement_model_factory.cc | 2 +- .../projection_model/projector_factory.cc | 2 +- .../map/volumetric_data_structure_factory.cc | 2 +- .../test/src/utils/test_grid_adjacency.cc | 8 +-- .../wavemap_io/src/stream_conversions.cc | 2 +- ros/wavemap_ros/app/rosbag_processor.cc | 2 +- .../impl/pointcloud_input_handler_impl.h | 2 +- .../input_handler/input_handler_factory.cc | 2 +- .../input_handler/pointcloud_input_handler.cc | 2 +- .../src/visuals/cell_selector.cc | 2 +- .../src/visuals/slice_visual.cc | 2 +- .../src/visuals/voxel_visual.cc | 2 +- 18 files changed, 92 insertions(+), 84 deletions(-) diff --git a/libraries/wavemap/include/wavemap/config/impl/type_selector_inl.h b/libraries/wavemap/include/wavemap/config/impl/type_selector_inl.h index 926013bfe..ea1d4568e 100644 --- a/libraries/wavemap/include/wavemap/config/impl/type_selector_inl.h +++ b/libraries/wavemap/include/wavemap/config/impl/type_selector_inl.h @@ -6,9 +6,58 @@ #include namespace wavemap { +template +TypeSelector::TypeSelector( + const TypeSelector::TypeName& type_name) { + id_ = toTypeId(type_name); +} + +template +TypeSelector::TypeSelector(TypeId type_id) { + if (isValid(type_id)) { + id_ = type_id; + } +} + +template +TypeSelector::~TypeSelector() { + // Force the derived config classes to specify: + // - an enum with Ids, using type "int" as the enum's underlying type + // - an array of names + // NOTE: These static asserts have to be placed in a method that's + // guaranteed to be evaluated by the compiler, making the dtor is a + // good option. + static_assert( + std::is_same_v, + int>); + static_assert(std::is_enum_v, + "Derived TypeSelector type must define an enum called Id."); + static_assert( + std::is_convertible_v, + "Derived TypeSelector type must define an array called names whose " + "members are convertible to std::strings."); +} + +template +bool TypeSelector::isValid(TypeId type_id) { + return 0 <= type_id && + static_cast(type_id) < DerivedTypeSelectorT::names.size(); +} + +template +typename TypeSelector::TypeName +TypeSelector::toStr(TypeId type_id) { + if (isValid(type_id)) { + return DerivedTypeSelectorT::names[static_cast(type_id)]; + } else { + return "Invalid"; + } +} + template typename TypeSelector::TypeId -TypeSelector::strToTypeId(const std::string& name) { +TypeSelector::toTypeId(const std::string& name) { for (size_t type_idx = 0; type_idx < DerivedNamedTypeSetT::names.size(); ++type_idx) { if (name == DerivedNamedTypeSetT::names[type_idx]) { diff --git a/libraries/wavemap/include/wavemap/config/impl/value_with_unit_inl.h b/libraries/wavemap/include/wavemap/config/impl/value_with_unit_inl.h index 26d7a1101..f918c456f 100644 --- a/libraries/wavemap/include/wavemap/config/impl/value_with_unit_inl.h +++ b/libraries/wavemap/include/wavemap/config/impl/value_with_unit_inl.h @@ -6,7 +6,7 @@ namespace wavemap { template std::string ValueWithUnit::toStr() const { - return std::to_string(value) + " [" + SiUnit::typeIdToStr(unit) + "]"; + return std::to_string(value) + " [" + SiUnit::toStr(unit) + "]"; } template @@ -17,14 +17,14 @@ std::optional> ValueWithUnit::from( LOG(WARNING) << "Tried to load a value with annotated units from a param " "that is not of type Map (dictionary). Cannot perform " "conversion to SI unit \"" - << SiUnit::typeIdToStr(unit) << "\". Will be ignored."; + << SiUnit::toStr(unit) << "\". Will be ignored."; return std::nullopt; } if (param_map->size() != 1) { LOG(WARNING) << "Value with unit should have exactly one key and value. " "Cannot perform conversion to SI unit \"" - << SiUnit::typeIdToStr(unit) << "\". Will be ignored."; + << SiUnit::toStr(unit) << "\". Will be ignored."; return std::nullopt; } @@ -34,7 +34,7 @@ std::optional> ValueWithUnit::from( if (!si_unit_and_conversion) { LOG(WARNING) << "Value has unknown unit \"" << param_unit << "\". Cannot perform conversion to SI unit \"" - << SiUnit::typeIdToStr(unit) << "\". Will be ignored."; + << SiUnit::toStr(unit) << "\". Will be ignored."; return std::nullopt; } @@ -42,7 +42,7 @@ std::optional> ValueWithUnit::from( if (si_unit != unit) { LOG(WARNING) << "Value has unit \"" << param_unit << "\" which cannot be converted to SI unit \"" - << SiUnit::typeIdToStr(unit) << "\". Will be ignored."; + << SiUnit::toStr(unit) << "\". Will be ignored."; return std::nullopt; } @@ -56,7 +56,7 @@ std::optional> ValueWithUnit::from( LOG(WARNING) << "Value must be of type float or int. Cannot perform " "conversion to SI unit \"" - << SiUnit::typeIdToStr(unit) << "\". Will be ignored."; + << SiUnit::toStr(unit) << "\". Will be ignored."; return std::nullopt; } } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/config/type_selector.h b/libraries/wavemap/include/wavemap/config/type_selector.h index aa66bcce2..80fa75b9c 100644 --- a/libraries/wavemap/include/wavemap/config/type_selector.h +++ b/libraries/wavemap/include/wavemap/config/type_selector.h @@ -17,66 +17,25 @@ struct TypeSelector { static constexpr TypeId kInvalidTypeId = -1; // Construct from a given type name or ID - explicit TypeSelector(const TypeName& type_name) { - id_ = strToTypeId(type_name); - } - TypeSelector(TypeId type_id) { // NOLINT - if (isValidTypeId(type_id)) { - id_ = type_id; - } - } + TypeSelector(const TypeName& type_name); // NOLINT + TypeSelector(TypeId type_id); // NOLINT - ~TypeSelector() { - // Force the derived config classes to specify: - // - an enum with Ids - // - an array of names - // NOTE: These static asserts have to be placed in a method that's - // guaranteed to be evaluated by the compiler, making the dtor is a - // good option. - static_assert(std::is_enum_v, - "Derived TypeSelector type must define an enum called Id."); - static_assert( - std::is_convertible_v, - "Derived TypeSelector type must define an array called names whose " - "members are convertible to std::strings."); - } + // Destructor + ~TypeSelector(); - // Conversions to the underlying type's name or ID - explicit operator TypeName() const { return typeIdToStr(id_); } - explicit operator TypeId() const { return id_; } - TypeName toStr() const { return typeIdToStr(id_); } - constexpr TypeId toTypeId() const { return id_; } - - // Comparison operators - friend bool operator==(const DerivedTypeSelectorT& lhs, - const DerivedTypeSelectorT& rhs) { - return lhs.id_ == rhs.id_; - } - friend bool operator!=(const DerivedTypeSelectorT& lhs, - const DerivedTypeSelectorT& rhs) { - return !(lhs == rhs); - } + // Allow implicit conversions to the underlying type's name or ID + operator TypeName() const { return toStr(id_); } + operator TypeId() const { return id_; } // Method to check if the type ID is currently valid - bool isValid() const { return isValidTypeId(id_); } + bool isValid() const { return isValid(id_); } + static bool isValid(TypeId type_id); - // Static methods for convenience - // TODO(victorr): Simplify name to "toStr()" - static TypeName typeIdToStr(TypeId type_id) { - if (isValidTypeId(type_id)) { - return DerivedTypeSelectorT::names[static_cast(type_id)]; - } else { - return "Invalid"; - } - } - // TODO(victorr): Simplify name to "toTypeId()" - static TypeId strToTypeId(const std::string& name); - // TODO(victorr): Simplify name to "isValid()" - static bool isValidTypeId(TypeId type_id) { - return 0 <= type_id && - static_cast(type_id) < DerivedTypeSelectorT::names.size(); - } + // Explicit conversions to the underlying type's name or ID + TypeName toStr() const { return toStr(id_); } + static TypeName toStr(TypeId type_id); + constexpr TypeId toTypeId() const { return id_; } + static TypeId toTypeId(const std::string& name); // Convenience method to read the type from params static std::optional from(const param::Value& params); diff --git a/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_beam_inl.h b/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_beam_inl.h index b3f1a677f..a3bc6534b 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_beam_inl.h +++ b/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_beam_inl.h @@ -32,7 +32,7 @@ inline FloatingPoint ContinuousBeam::computeWorstCaseApproximationError( inline FloatingPoint ContinuousBeam::computeUpdate( const SensorCoordinates& sensor_coordinates) const { - switch (config_.beam_selector_type.toTypeId()) { + switch (config_.beam_selector_type) { case BeamSelectorType::kNearestNeighbor: return computeBeamUpdateNearestNeighbor( *range_image_, *beam_offset_image_, *projection_model_, diff --git a/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_ray_inl.h b/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_ray_inl.h index 06467b69e..56d6ed701 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_ray_inl.h +++ b/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_ray_inl.h @@ -26,7 +26,7 @@ inline FloatingPoint ContinuousRay::computeWorstCaseApproximationError( inline FloatingPoint ContinuousRay::computeUpdate( const SensorCoordinates& sensor_coordinates) const { - switch (config_.beam_selector_type.toTypeId()) { + switch (config_.beam_selector_type) { case BeamSelectorType::kNearestNeighbor: { const auto image_index = projection_model_->imageToNearestIndex(sensor_coordinates.image); diff --git a/libraries/wavemap/src/integrator/integrator_factory.cc b/libraries/wavemap/src/integrator/integrator_factory.cc index b2d21702b..bae265353 100644 --- a/libraries/wavemap/src/integrator/integrator_factory.cc +++ b/libraries/wavemap/src/integrator/integrator_factory.cc @@ -82,7 +82,7 @@ IntegratorBase::Ptr IntegratorFactory::create( } // Assemble the projective integrator - switch (integrator_type.toTypeId()) { + switch (integrator_type) { case IntegratorType::kFixedResolutionIntegrator: { return std::make_shared( integrator_config.value(), projection_model, posed_range_image, @@ -98,7 +98,7 @@ IntegratorBase::Ptr IntegratorFactory::create( } else { LOG(ERROR) << "Integrator of type " << integrator_type.toStr() << " only supports data structures of type " - << VolumetricDataStructureType::typeIdToStr( + << VolumetricDataStructureType::toStr( VolumetricDataStructureType::kOctree) << ". Returning nullptr."; } @@ -114,7 +114,7 @@ IntegratorBase::Ptr IntegratorFactory::create( } else { LOG(ERROR) << "Integrator of type " << integrator_type.toStr() << " only supports data structures of type " - << VolumetricDataStructureType::typeIdToStr( + << VolumetricDataStructureType::toStr( VolumetricDataStructureType::kWaveletOctree) << ". Returning nullptr."; } @@ -131,7 +131,7 @@ IntegratorBase::Ptr IntegratorFactory::create( } else { LOG(ERROR) << "Integrator of type " << integrator_type.toStr() << " only supports data structures of type " - << VolumetricDataStructureType::typeIdToStr( + << VolumetricDataStructureType::toStr( VolumetricDataStructureType::kHashedWaveletOctree) << ". Returning nullptr."; } @@ -149,7 +149,7 @@ IntegratorBase::Ptr IntegratorFactory::create( LOG(ERROR) << "Integrator of type " << integrator_type.toStr() << " only supports data structures of type " - << VolumetricDataStructureType::typeIdToStr( + << VolumetricDataStructureType::toStr( VolumetricDataStructureType::kHashedChunkedWaveletOctree) << ". Returning nullptr."; } diff --git a/libraries/wavemap/src/integrator/measurement_model/measurement_model_factory.cc b/libraries/wavemap/src/integrator/measurement_model/measurement_model_factory.cc index 23fae91b4..e755650dc 100644 --- a/libraries/wavemap/src/integrator/measurement_model/measurement_model_factory.cc +++ b/libraries/wavemap/src/integrator/measurement_model/measurement_model_factory.cc @@ -31,7 +31,7 @@ MeasurementModelBase::Ptr wavemap::MeasurementModelFactory::create( MeasurementModelType measurement_model_type, ProjectorBase::ConstPtr projection_model, Image<>::ConstPtr range_image, Image::ConstPtr beam_offset_image, const param::Value& params) { - switch (measurement_model_type.toTypeId()) { + switch (measurement_model_type) { case MeasurementModelType::kContinuousRay: { if (const auto config = ContinuousRayConfig::from(params, "measurement_model"); diff --git a/libraries/wavemap/src/integrator/projection_model/projector_factory.cc b/libraries/wavemap/src/integrator/projection_model/projector_factory.cc index c4f753864..b0916a148 100644 --- a/libraries/wavemap/src/integrator/projection_model/projector_factory.cc +++ b/libraries/wavemap/src/integrator/projection_model/projector_factory.cc @@ -24,7 +24,7 @@ ProjectorBase::Ptr wavemap::ProjectorFactory::create( ProjectorBase::Ptr wavemap::ProjectorFactory::create( ProjectorType projector_type, const param::Value& params) { - switch (projector_type.toTypeId()) { + switch (projector_type) { case ProjectorType::kSphericalProjector: { if (const auto config = SphericalProjectorConfig::from(params, "projection_model"); diff --git a/libraries/wavemap/src/map/volumetric_data_structure_factory.cc b/libraries/wavemap/src/map/volumetric_data_structure_factory.cc index 447f077ae..e016b01a4 100644 --- a/libraries/wavemap/src/map/volumetric_data_structure_factory.cc +++ b/libraries/wavemap/src/map/volumetric_data_structure_factory.cc @@ -28,7 +28,7 @@ VolumetricDataStructureBase::Ptr VolumetricDataStructureFactory::create( VolumetricDataStructureBase::Ptr VolumetricDataStructureFactory::create( VolumetricDataStructureType data_structure_type, const param::Value& params) { - switch (data_structure_type.toTypeId()) { + switch (data_structure_type) { case VolumetricDataStructureType::kHashedBlocks: { if (const auto config = VolumetricDataStructureConfig::from(params); config) { diff --git a/libraries/wavemap/test/src/utils/test_grid_adjacency.cc b/libraries/wavemap/test/src/utils/test_grid_adjacency.cc index 17181b9de..0782f8045 100644 --- a/libraries/wavemap/test/src/utils/test_grid_adjacency.cc +++ b/libraries/wavemap/test/src/utils/test_grid_adjacency.cc @@ -26,7 +26,7 @@ TYPED_TEST(GridAdjacencyTest, SharedVertexAdjacency) { const IndexType neighbor = index + offset; const auto adjacency_type = Adjacency{computeAdjacency(index, neighbor)}; EXPECT_TRUE(adjacency_type.isValid()); - EXPECT_EQ(adjacency_type.toTypeId(), Adjacency::kSharedVertex); + EXPECT_EQ(adjacency_type, Adjacency::kSharedVertex); EXPECT_TRUE(areAdjacent(index, neighbor, Adjacency::kSharedVertex)); } } @@ -44,7 +44,7 @@ TYPED_TEST(GridAdjacencyTest, SharedEdgeAdjacency) { const IndexType neighbor = index + offset; const auto adjacency_type = Adjacency{computeAdjacency(index, neighbor)}; EXPECT_TRUE(adjacency_type.isValid()); - EXPECT_EQ(adjacency_type.toTypeId(), Adjacency::kSharedEdge); + EXPECT_EQ(adjacency_type, Adjacency::kSharedEdge); EXPECT_TRUE(areAdjacent(index, neighbor, Adjacency::kSharedEdge)); } } @@ -62,7 +62,7 @@ TYPED_TEST(GridAdjacencyTest, SharedFaceAdjacency) { const IndexType neighbor = index + offset; const auto adjacency_type = Adjacency{computeAdjacency(index, neighbor)}; EXPECT_TRUE(adjacency_type.isValid()); - EXPECT_EQ(adjacency_type.toTypeId(), Adjacency::kSharedFace); + EXPECT_EQ(adjacency_type, Adjacency::kSharedFace); EXPECT_TRUE(areAdjacent(index, neighbor, Adjacency::kSharedFace)); } } @@ -80,7 +80,7 @@ TYPED_TEST(GridAdjacencyTest, SharedCubeAdjacency) { const IndexType neighbor = index + offset; const auto adjacency_type = Adjacency{computeAdjacency(index, neighbor)}; EXPECT_TRUE(adjacency_type.isValid()); - EXPECT_EQ(adjacency_type.toTypeId(), Adjacency::kSharedCube); + EXPECT_EQ(adjacency_type, Adjacency::kSharedCube); EXPECT_TRUE(areAdjacent(index, neighbor, Adjacency::kSharedCube)); } } diff --git a/libraries/wavemap_io/src/stream_conversions.cc b/libraries/wavemap_io/src/stream_conversions.cc index 5f5ffc1c5..44aa7409c 100644 --- a/libraries/wavemap_io/src/stream_conversions.cc +++ b/libraries/wavemap_io/src/stream_conversions.cc @@ -35,7 +35,7 @@ bool mapToStream(const VolumetricDataStructureBase& map, bool streamToMap(std::istream& istream, VolumetricDataStructureBase::Ptr& map) { // Call the appropriate streamToMap converter based on the received map's type const auto storage_format = streamable::StorageFormat::peek(istream); - switch (storage_format.toTypeId()) { + switch (storage_format) { case streamable::StorageFormat::kHashedBlocks: { auto hashed_blocks = std::dynamic_pointer_cast(map); if (!streamToMap(istream, hashed_blocks)) { diff --git a/ros/wavemap_ros/app/rosbag_processor.cc b/ros/wavemap_ros/app/rosbag_processor.cc index 7f1b7b5f8..02c0059af 100644 --- a/ros/wavemap_ros/app/rosbag_processor.cc +++ b/ros/wavemap_ros/app/rosbag_processor.cc @@ -42,7 +42,7 @@ int main(int argc, char** argv) { InputHandler* input_handler = wavemap_server.addInput(integrator_params, nh, nh_private); if (input_handler) { - switch (input_handler->getType().toTypeId()) { + switch (input_handler->getType()) { case InputHandlerType::kPointcloud: { auto pointcloud_handler = dynamic_cast(input_handler); diff --git a/ros/wavemap_ros/include/wavemap_ros/input_handler/impl/pointcloud_input_handler_impl.h b/ros/wavemap_ros/include/wavemap_ros/input_handler/impl/pointcloud_input_handler_impl.h index 589475bb9..92e2b9700 100644 --- a/ros/wavemap_ros/include/wavemap_ros/input_handler/impl/pointcloud_input_handler_impl.h +++ b/ros/wavemap_ros/include/wavemap_ros/input_handler/impl/pointcloud_input_handler_impl.h @@ -5,7 +5,7 @@ namespace wavemap { template bool PointcloudInputHandler::registerCallback(PointcloudTopicType type, RegistrarT registrar) { - switch (type.toTypeId()) { + switch (type) { case PointcloudTopicType::kPointCloud2: // clang-format off registrar(static_cast InputHandlerFactory::create( std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private) { // Create the input handler - switch (input_handler_type.toTypeId()) { + switch (input_handler_type) { case InputHandlerType::kPointcloud: { if (const auto config = PointcloudInputHandlerConfig::from(params, "general"); diff --git a/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc b/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc index e691fae52..7dfe42c1b 100644 --- a/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc +++ b/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc @@ -90,7 +90,7 @@ void PointcloudInputHandler::callback( sensor_msgs::PointCloud2ConstIterator pos_it(pointcloud_msg, "x"); if (config_.undistort_motion) { // NOTE: Livox pointclouds are not handled here but in their own callback. - switch (config_.topic_type.toTypeId()) { + switch (config_.topic_type) { case PointcloudTopicType::kOuster: if (hasField(pointcloud_msg, "t")) { sensor_msgs::PointCloud2ConstIterator t_it(pointcloud_msg, diff --git a/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc b/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc index 071ab8ff9..fc452f286 100644 --- a/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc +++ b/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc @@ -68,7 +68,7 @@ void CellSelector::setMap(const VolumetricDataStructureBase::ConstPtr& map) { bool CellSelector::shouldBeDrawn(const OctreeIndex& cell_index, FloatingPoint cell_log_odds) const { - switch (cell_selection_mode_.toTypeId()) { + switch (cell_selection_mode_) { case CellSelectionMode::kSurface: // Skip free cells if (cell_log_odds < surface_occupancy_threshold_ || diff --git a/ros/wavemap_rviz_plugin/src/visuals/slice_visual.cc b/ros/wavemap_rviz_plugin/src/visuals/slice_visual.cc index e90eb5780..f3bc29ef0 100644 --- a/ros/wavemap_rviz_plugin/src/visuals/slice_visual.cc +++ b/ros/wavemap_rviz_plugin/src/visuals/slice_visual.cc @@ -122,7 +122,7 @@ void SliceVisual::update() { cell.center.z = slice_height; // Set the cube's color - switch (slice_color_mode_.toTypeId()) { + switch (slice_color_mode_) { case SliceColorMode::kRaw: { cell.color = scalarToColor(cell_log_odds, min_occupancy_log_odds, max_occupancy_log_odds); diff --git a/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc b/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc index 372bfae57..5275e6f0e 100644 --- a/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc +++ b/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc @@ -339,7 +339,7 @@ void VoxelVisual::appendLeafCenterAndColor(int tree_height, point.center.z = cell_center[2]; // Set the cube's color - switch (voxel_color_mode_.toTypeId()) { + switch (voxel_color_mode_) { case VoxelColorMode::kFlat: point.color = voxel_flat_color_; break; From f1d214d1fd8eb2f4a4b2dc8ba38dfe75e797ca6e Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 4 Dec 2023 11:07:42 +0100 Subject: [PATCH 036/159] Use available methods directly --- .../wavemap/map/impl/hashed_chunked_wavelet_octree_block_inl.h | 2 +- .../include/wavemap/map/impl/hashed_wavelet_octree_block_inl.h | 2 +- libraries/wavemap/include/wavemap/map/impl/wavelet_octree_inl.h | 2 +- libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc | 2 +- .../wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_block_inl.h b/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_block_inl.h index 1954a69ab..70212dfab 100644 --- a/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_block_inl.h +++ b/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_block_inl.h @@ -10,7 +10,7 @@ inline bool HashedChunkedWaveletOctreeBlock::empty() const { // coefficients, we also need to check whether its scale coefficient // (average value over the whole block) is zero. return chunked_ndtree_.empty() && - !OccupancyClassifier::isObserved(root_scale_coefficient_); + OccupancyClassifier::isUnobserved(root_scale_coefficient_); } inline FloatingPoint HashedChunkedWaveletOctreeBlock::getTimeSinceLastUpdated() diff --git a/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_block_inl.h b/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_block_inl.h index f3ed57e8a..85cce0ae9 100644 --- a/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_block_inl.h +++ b/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_block_inl.h @@ -10,7 +10,7 @@ inline bool HashedWaveletOctreeBlock::empty() const { // coefficients, we also need to check whether its scale coefficient // (average value over the whole block) is zero. return ndtree_.empty() && - !OccupancyClassifier::isObserved(root_scale_coefficient_); + OccupancyClassifier::isUnobserved(root_scale_coefficient_); } inline FloatingPoint HashedWaveletOctreeBlock::getTimeSinceLastUpdated() const { diff --git a/libraries/wavemap/include/wavemap/map/impl/wavelet_octree_inl.h b/libraries/wavemap/include/wavemap/map/impl/wavelet_octree_inl.h index 69e373a9b..0dabc6a77 100644 --- a/libraries/wavemap/include/wavemap/map/impl/wavelet_octree_inl.h +++ b/libraries/wavemap/include/wavemap/map/impl/wavelet_octree_inl.h @@ -15,7 +15,7 @@ inline bool WaveletOctree::empty() const { // coefficients, we also need to check whether its scale coefficient // (average value over the whole map) is zero. return ndtree_.empty() && - !OccupancyClassifier::isObserved(root_scale_coefficient_); + OccupancyClassifier::isUnobserved(root_scale_coefficient_); } inline void WaveletOctree::clear() { diff --git a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc index 7832ee001..c0adb5046 100644 --- a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc @@ -146,7 +146,7 @@ void FullEuclideanSDFGenerator::propagate( const FloatingPoint neighbor_occupancy = occupancy_query_accelerator.getCellValue(neighbor_index); // Never initialize or update unknown cells - if (!OccupancyClassifier::isObserved(neighbor_occupancy)) { + if (OccupancyClassifier::isUnobserved(neighbor_occupancy)) { continue; } // Set the sign diff --git a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc index eb48e0a58..305103f44 100644 --- a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc @@ -137,7 +137,7 @@ void QuasiEuclideanSDFGenerator::propagate( const FloatingPoint neighbor_occupancy = occupancy_query_accelerator.getCellValue(neighbor_index); // Never initialize or update unknown cells - if (!OccupancyClassifier::isObserved(neighbor_occupancy)) { + if (OccupancyClassifier::isUnobserved(neighbor_occupancy)) { continue; } // Set the sign From 1e528af366a4075e4ac355353ef3d0fb43f25331 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 5 Dec 2023 10:46:19 +0100 Subject: [PATCH 037/159] Avoid unnecessary square root computation in point validity check --- .../integrator/impl/integrator_base_inl.h | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/libraries/wavemap/include/wavemap/integrator/impl/integrator_base_inl.h b/libraries/wavemap/include/wavemap/integrator/impl/integrator_base_inl.h index 6352e20cb..9905186f0 100644 --- a/libraries/wavemap/include/wavemap/integrator/impl/integrator_base_inl.h +++ b/libraries/wavemap/include/wavemap/integrator/impl/integrator_base_inl.h @@ -3,18 +3,24 @@ namespace wavemap { inline bool IntegratorBase::isMeasurementValid(const Point3D& C_end_point) { + // Reject points with NaN coordinates if (C_end_point.hasNaN()) { return false; } - const FloatingPoint measured_distance = C_end_point.norm(); - if (measured_distance < 1e-3f) { + + // Reject points with suspicious range values + constexpr FloatingPoint kValidRangeLowerThresholdSquared = 1e-3f * 1e-3f; + constexpr FloatingPoint kValidRangeUpperThresholdSquared = 1e3f * 1e3f; + const FloatingPoint measured_distance_squared = C_end_point.squaredNorm(); + if (measured_distance_squared < kValidRangeLowerThresholdSquared) { return false; } - if (1e3 < measured_distance) { - LOG(INFO) << "Skipping measurement with suspicious length: " - << measured_distance; + if (kValidRangeUpperThresholdSquared < measured_distance_squared) { + LOG(INFO) << "Skipping point with suspicious range value: " + << std::sqrt(measured_distance_squared); return false; } + return true; } From 6a15148ff0d8233524ffa400a87d17dd93f3ae40 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 5 Dec 2023 10:46:19 +0100 Subject: [PATCH 038/159] Avoid unnecessary square root computation in point validity check --- .../integrator/impl/integrator_base_inl.h | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/libraries/wavemap/include/wavemap/integrator/impl/integrator_base_inl.h b/libraries/wavemap/include/wavemap/integrator/impl/integrator_base_inl.h index 6352e20cb..9905186f0 100644 --- a/libraries/wavemap/include/wavemap/integrator/impl/integrator_base_inl.h +++ b/libraries/wavemap/include/wavemap/integrator/impl/integrator_base_inl.h @@ -3,18 +3,24 @@ namespace wavemap { inline bool IntegratorBase::isMeasurementValid(const Point3D& C_end_point) { + // Reject points with NaN coordinates if (C_end_point.hasNaN()) { return false; } - const FloatingPoint measured_distance = C_end_point.norm(); - if (measured_distance < 1e-3f) { + + // Reject points with suspicious range values + constexpr FloatingPoint kValidRangeLowerThresholdSquared = 1e-3f * 1e-3f; + constexpr FloatingPoint kValidRangeUpperThresholdSquared = 1e3f * 1e3f; + const FloatingPoint measured_distance_squared = C_end_point.squaredNorm(); + if (measured_distance_squared < kValidRangeLowerThresholdSquared) { return false; } - if (1e3 < measured_distance) { - LOG(INFO) << "Skipping measurement with suspicious length: " - << measured_distance; + if (kValidRangeUpperThresholdSquared < measured_distance_squared) { + LOG(INFO) << "Skipping point with suspicious range value: " + << std::sqrt(measured_distance_squared); return false; } + return true; } From b204a33b06198d2768438ea081a2bb5986e31987 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 5 Dec 2023 13:31:39 +0100 Subject: [PATCH 039/159] Extend AABBs to handle AABB-AABB min and max distance queries --- .../include/wavemap/data_structure/aabb.h | 51 ++++++++++++++----- .../impl/hierarchical_range_bounds_inl.h | 2 +- 2 files changed, 40 insertions(+), 13 deletions(-) diff --git a/libraries/wavemap/include/wavemap/data_structure/aabb.h b/libraries/wavemap/include/wavemap/data_structure/aabb.h index c0ddf89a4..89fe437ad 100644 --- a/libraries/wavemap/include/wavemap/data_structure/aabb.h +++ b/libraries/wavemap/include/wavemap/data_structure/aabb.h @@ -32,37 +32,64 @@ struct AABB { } PointType closestPointTo(const PointType& point) const { - PointType closest_point = point.cwiseMax(min); - closest_point = closest_point.cwiseMin(max); + PointType closest_point = point.cwiseMax(min).cwiseMin(max); return closest_point; } PointType furthestPointFrom(const PointType& point) const { const PointType aabb_center = (min + max) / static_cast(2); - const PointType furthest_point = + PointType furthest_point = (aabb_center.array() < point.array()).select(min, max); return furthest_point; } - FloatingPoint minSquaredDistanceTo(const PointType& point) const { - return (point - closestPointTo(point)).squaredNorm(); + PointType minOffsetTo(const PointType& point) const { + return point - closestPointTo(point); } - FloatingPoint maxSquaredDistanceTo(const PointType& point) const { - return (point - furthestPointFrom(point)).squaredNorm(); + PointType maxOffsetTo(const PointType& point) const { + return point - furthestPointFrom(point); } - FloatingPoint minDistanceTo(const PointType& point) const { - return (point - closestPointTo(point)).norm(); + // TODO(victorr): Check correctness with unit tests + PointType minOffsetTo(const AABB& aabb) const { + const PointType greatest_min = min.cwiseMax(aabb.min); + const PointType smallest_max = max.cwiseMin(aabb.max); + return (greatest_min - smallest_max).cwiseMax(0); } - FloatingPoint maxDistanceTo(const PointType& point) const { - return (point - furthestPointFrom(point)).norm(); + // TODO(victorr): Check correctness with unit tests. Pay particular + // attention to whether the offset signs are correct. + PointType maxOffsetTo(const AABB& aabb) const { + const PointType diff_1 = min - aabb.max; + const PointType diff_2 = max - aabb.min; + PointType offset = + (diff_2.array().abs() < diff_1.array().abs()).select(diff_1, diff_2); + return offset; + } + + template + ScalarType minSquaredDistanceTo(const GeometricEntityT& entity) const { + return minOffsetTo(entity).squaredNorm(); + } + template + ScalarType maxSquaredDistanceTo(const GeometricEntityT& entity) const { + return maxOffsetTo(entity).squaredNorm(); + } + + template + ScalarType minDistanceTo(const GeometricEntityT& entity) const { + return minOffsetTo(entity).norm(); + } + template + ScalarType maxDistanceTo(const GeometricEntityT& entity) const { + return maxOffsetTo(entity).norm(); } template ScalarType width() const { return max[dim] - min[dim]; } + PointType widths() const { return max - min; } Corners corner_matrix() const { - Eigen::Matrix corners; + Eigen::Matrix corners; for (int corner_idx = 0; corner_idx < kNumCorners; ++corner_idx) { for (int dim_idx = 0; dim_idx < kDim; ++dim_idx) { corners(dim_idx, corner_idx) = corner_coordinate(dim_idx, corner_idx); diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h index bb2c31f6a..62a56072a 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h @@ -257,7 +257,7 @@ inline bool HierarchicalRangeBounds::hasUnobserved( inline UpdateType HierarchicalRangeBounds::getUpdateType( const Index2D& min_image_idx, const Index2D& max_image_idx, FloatingPoint range_min, FloatingPoint range_max) const { - const Index2D min_image_idx_clamped = min_image_idx.cwiseMax(Index2D::Zero()); + const Index2D min_image_idx_clamped = min_image_idx.cwiseMax(0); const Index2D max_image_idx_clamped = max_image_idx.cwiseMin(range_image_->getDimensions() - Index2D::Ones()); From 1689a67a7bfa306831466f02c8ad6e2f33cd0a98 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 5 Dec 2023 17:24:25 +0100 Subject: [PATCH 040/159] Use bit_ops::is_bit_set to make code easier to read --- libraries/wavemap/include/wavemap/data_structure/aabb.h | 2 +- .../include/wavemap/map/cell_types/haar_coefficients.h | 2 +- .../src/integrator/projection_model/ouster_projector.cc | 2 +- .../projection_model/pinhole_camera_projector.cc | 2 +- .../integrator/projection_model/spherical_projector.cc | 2 +- libraries/wavemap_io/src/stream_conversions.cc | 8 ++++---- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/libraries/wavemap/include/wavemap/data_structure/aabb.h b/libraries/wavemap/include/wavemap/data_structure/aabb.h index 89fe437ad..334b86960 100644 --- a/libraries/wavemap/include/wavemap/data_structure/aabb.h +++ b/libraries/wavemap/include/wavemap/data_structure/aabb.h @@ -107,7 +107,7 @@ struct AABB { } ScalarType corner_coordinate(int dim_idx, int corner_idx) const { - if (corner_idx & (0b1 << dim_idx)) { + if (bit_ops::is_bit_set(corner_idx, dim_idx)) { return max[dim_idx]; } else { return min[dim_idx]; diff --git a/libraries/wavemap/include/wavemap/map/cell_types/haar_coefficients.h b/libraries/wavemap/include/wavemap/map/cell_types/haar_coefficients.h index cffac8918..7c879f906 100644 --- a/libraries/wavemap/include/wavemap/map/cell_types/haar_coefficients.h +++ b/libraries/wavemap/include/wavemap/map/cell_types/haar_coefficients.h @@ -63,7 +63,7 @@ struct HaarCoefficients { for (int coeff_idx = 1; coeff_idx <= kNumDetailCoefficients; ++coeff_idx) { for (int dim_idx = 0; dim_idx < kDim; ++dim_idx) { - ss << ((coeff_idx & (0b1 << dim_idx)) ? "H" : "L"); + ss << (bit_ops::is_bit_set(coeff_idx, dim_idx) ? "H" : "L"); } ss << " = " << this->operator[](coeff_idx) << ", "; } diff --git a/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc b/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc index 4eae4dcab..0657c6f79 100644 --- a/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc +++ b/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc @@ -50,7 +50,7 @@ AABB OusterProjector::cartesianToSensorAABB( ++corner_idx) { C_aabb_corners[corner_idx] = C_aabb_min; for (int dim_idx = 0; dim_idx < 3; ++dim_idx) { - if ((corner_idx >> dim_idx) & 1) { + if (bit_ops::is_bit_set(corner_idx, dim_idx)) { C_aabb_corners[corner_idx] += C_aabb_edges.col(dim_idx); } } diff --git a/libraries/wavemap/src/integrator/projection_model/pinhole_camera_projector.cc b/libraries/wavemap/src/integrator/projection_model/pinhole_camera_projector.cc index 660907468..43f7d54d0 100644 --- a/libraries/wavemap/src/integrator/projection_model/pinhole_camera_projector.cc +++ b/libraries/wavemap/src/integrator/projection_model/pinhole_camera_projector.cc @@ -22,7 +22,7 @@ AABB PinholeCameraProjector::cartesianToSensorAABB( ++corner_idx) { C_aabb_corners[corner_idx] = C_aabb_min; for (int dim_idx = 0; dim_idx < 3; ++dim_idx) { - if ((corner_idx >> dim_idx) & 1) { + if (bit_ops::is_bit_set(corner_idx, dim_idx)) { C_aabb_corners[corner_idx] += C_aabb_edges.col(dim_idx); } } diff --git a/libraries/wavemap/src/integrator/projection_model/spherical_projector.cc b/libraries/wavemap/src/integrator/projection_model/spherical_projector.cc index 9e7b39679..ff8b9906c 100644 --- a/libraries/wavemap/src/integrator/projection_model/spherical_projector.cc +++ b/libraries/wavemap/src/integrator/projection_model/spherical_projector.cc @@ -50,7 +50,7 @@ AABB SphericalProjector::cartesianToSensorAABB( ++corner_idx) { C_aabb_corners[corner_idx] = C_aabb_min; for (int dim_idx = 0; dim_idx < 3; ++dim_idx) { - if ((corner_idx >> dim_idx) & 1) { + if (bit_ops::is_bit_set(corner_idx, dim_idx)) { C_aabb_corners[corner_idx] += C_aabb_edges.col(dim_idx); } } diff --git a/libraries/wavemap_io/src/stream_conversions.cc b/libraries/wavemap_io/src/stream_conversions.cc index 44aa7409c..de661c003 100644 --- a/libraries/wavemap_io/src/stream_conversions.cc +++ b/libraries/wavemap_io/src/stream_conversions.cc @@ -213,8 +213,8 @@ bool streamToMap(std::istream& istream, WaveletOctree::Ptr& map) { // the nodes are popped from the stack in increasing order. for (int relative_child_idx = wavemap::OctreeIndex::kNumChildren - 1; 0 <= relative_child_idx; --relative_child_idx) { - const bool child_exists = - read_node.allocated_children_bitset & (1 << relative_child_idx); + const bool child_exists = bit_ops::is_bit_set( + read_node.allocated_children_bitset, relative_child_idx); if (child_exists) { stack.emplace(&node->getOrAllocateChild(relative_child_idx)); } @@ -345,8 +345,8 @@ bool streamToMap(std::istream& istream, HashedWaveletOctree::Ptr& map) { // the nodes are popped from the stack in increasing order. for (int relative_child_idx = wavemap::OctreeIndex::kNumChildren - 1; 0 <= relative_child_idx; --relative_child_idx) { - const bool child_exists = - read_node.allocated_children_bitset & (1 << relative_child_idx); + const bool child_exists = bit_ops::is_bit_set( + read_node.allocated_children_bitset, relative_child_idx); if (child_exists) { stack.emplace(&node->getOrAllocateChild(relative_child_idx)); } From 7278b6e225d34a70346dec4c04945e39835c0756 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 5 Dec 2023 19:44:21 +0100 Subject: [PATCH 041/159] Additional convenience methods --- .../include/wavemap/utils/query/occupancy_classifier.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h b/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h index 95bee52f3..96cedbc0b 100644 --- a/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h +++ b/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h @@ -21,6 +21,15 @@ class OccupancyClassifier { return occupancy_threshold_ < log_odds_occupancy_value; } + bool isObservedAndFree(FloatingPoint log_odds_occupancy_value) const { + return isObserved(log_odds_occupancy_value) && + isFree(log_odds_occupancy_value); + } + bool isObservedAndOccupied(FloatingPoint log_odds_occupancy_value) const { + return isObserved(log_odds_occupancy_value) && + isOccupied(log_odds_occupancy_value); + } + private: const FloatingPoint occupancy_threshold_; }; From 3c4fa22dc46340e0b6fdd5bc4ab21bacd039cf52 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 8 Dec 2023 18:25:53 +0100 Subject: [PATCH 042/159] Test NdtreeIndex adjacency checks --- .../utils/neighbors/ndtree_adjacency.h | 4 +- .../test/src/utils/test_ndtree_adjacency.cc | 52 +++++++++++++++++++ 2 files changed, 54 insertions(+), 2 deletions(-) diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/ndtree_adjacency.h b/libraries/wavemap/include/wavemap/utils/neighbors/ndtree_adjacency.h index ea59d0f7f..e252abcc3 100644 --- a/libraries/wavemap/include/wavemap/utils/neighbors/ndtree_adjacency.h +++ b/libraries/wavemap/include/wavemap/utils/neighbors/ndtree_adjacency.h @@ -17,12 +17,12 @@ static bool areAdjacent(const NdtreeIndex& index_1, const NdtreeIndex& biggest_node = index_1.height < index_2.height ? index_2 : index_1; - const Index3D biggest_node_min_corner = + const Index biggest_node_min_corner = int_math::mult_exp2(biggest_node.position, height_diff); const auto min_neighbor = biggest_node_min_corner.array() - 1; const auto max_neighbor = biggest_node_min_corner.array() + width; - return (min_neighbor <= smallest_node.position.array() || + return (min_neighbor <= smallest_node.position.array() && smallest_node.position.array() <= max_neighbor) .all(); } diff --git a/libraries/wavemap/test/src/utils/test_ndtree_adjacency.cc b/libraries/wavemap/test/src/utils/test_ndtree_adjacency.cc index 170afbc8a..c527edcd0 100644 --- a/libraries/wavemap/test/src/utils/test_ndtree_adjacency.cc +++ b/libraries/wavemap/test/src/utils/test_ndtree_adjacency.cc @@ -1 +1,53 @@ +#include "wavemap/test/fixture_base.h" +#include "wavemap/test/geometry_generator.h" +#include "wavemap/utils/neighbors/grid_adjacency.h" +#include "wavemap/utils/neighbors/grid_neighborhood.h" #include "wavemap/utils/neighbors/ndtree_adjacency.h" + +namespace wavemap { +template +class NdtreeAdjacencyTest : public FixtureBase, public GeometryGenerator {}; + +using Dimensions = ::testing::Types, + std::integral_constant, + std::integral_constant>; +TYPED_TEST_SUITE(NdtreeAdjacencyTest, Dimensions, ); + +TYPED_TEST(NdtreeAdjacencyTest, AnyAdjacency) { + constexpr int kDim = TypeParam::value; + using IndexType = Index; + using NdtreeIndexType = NdtreeIndex; + + const auto vertex_adjacency_offsets = + GridNeighborhood::template generateIndexOffsets(); + for (const auto& index : + TestFixture::template getRandomNdtreeIndexVector( + IndexType::Constant(-100), IndexType::Constant(100), 0, 6, 100, + 100)) { + for (const IndexType& offset : vertex_adjacency_offsets) { + const NdtreeIndexType neighbor{index.height, index.position + offset}; + EXPECT_TRUE(areAdjacent(index, neighbor)); + EXPECT_TRUE( + areAdjacent(index.computeParentIndex(index.height + 1), neighbor)); + } + } +} + +TYPED_TEST(NdtreeAdjacencyTest, NoAdjacency) { + constexpr int kDim = TypeParam::value; + using IndexType = Index; + using NdtreeIndexType = NdtreeIndex; + + const auto vertex_adjacency_offsets = GridNeighborhood< + kDim>::template generateIndexOffsets(); + for (const auto& index : + TestFixture::template getRandomNdtreeIndexVector( + IndexType::Constant(-100), IndexType::Constant(100), 0, 6, 100, + 100)) { + for (const IndexType& offset : vertex_adjacency_offsets) { + const NdtreeIndexType neighbor{index.height, index.position + 2 * offset}; + EXPECT_FALSE(areAdjacent(index, neighbor)); + } + } +} +} // namespace wavemap From 4ddf81a6fbd84288702aa983dd5849a46e8a532c Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 11 Dec 2023 11:17:38 +0100 Subject: [PATCH 043/159] Hint GCC that the loop has a small, fixed size --- .../wavemap/data_structure/ndtree/impl/ndtree_node_inl.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_node_inl.h b/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_node_inl.h index 5501cc959..4cccb5ce9 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_node_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_node_inl.h @@ -40,9 +40,12 @@ bool NdtreeNode::hasNonzeroData(FloatingPoint threshold) const { template bool NdtreeNode::hasAtLeastOneChild() const { if (hasChildrenArray()) { - return std::any_of( - children_->cbegin(), children_->cend(), - [](const auto& child_ptr) { return static_cast(child_ptr); }); + for (NdtreeIndexRelativeChild child_idx = 0; child_idx < kNumChildren; + ++child_idx) { + if (children_->operator[](child_idx)) { + return true; + } + } } return false; } From b6e1336f094af4155bde4827953f5c83702cdca2 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 12 Dec 2023 14:58:27 +0100 Subject: [PATCH 044/159] Fix LoD level selection when using Rviz's TopDownOrtho ViewController --- .../impl/spherical_projector_inl.h | 2 +- .../visuals/voxel_visual.h | 7 ++-- .../src/visuals/voxel_visual.cc | 33 +++++++++++++------ 3 files changed, 28 insertions(+), 14 deletions(-) diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/spherical_projector_inl.h b/libraries/wavemap/include/wavemap/integrator/projection_model/impl/spherical_projector_inl.h index 74d67bb44..eb6cf3eaa 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/spherical_projector_inl.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/impl/spherical_projector_inl.h @@ -9,7 +9,7 @@ namespace wavemap { inline SensorCoordinates SphericalProjector::cartesianToSensor( const Point3D& C_point) const { ImageCoordinates image_coordinates = cartesianToImage(C_point); - const FloatingPoint range = C_point.norm(); + const FloatingPoint range = cartesianToSensorZ(C_point); return {std::move(image_coordinates), range}; } diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h index 19b3c82ae..b8e835e0b 100644 --- a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h +++ b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h @@ -114,10 +114,11 @@ class VoxelVisual : public QObject { float lod_update_distance_threshold_ = 0.1f; Ogre::Vector3 camera_position_at_last_lod_update_{}; bool force_lod_update_ = true; - void updateLOD(const Point3D& camera_position); + void updateLOD(const Ogre::Camera& active_camera); static NdtreeIndexElement computeRecommendedBlockLodHeight( - FloatingPoint distance_to_cam, FloatingPoint min_cell_width, - NdtreeIndexElement min_height, NdtreeIndexElement max_height); + const Ogre::Camera& active_camera, const OctreeIndex& block_index, + FloatingPoint min_cell_width, NdtreeIndexElement min_height, + NdtreeIndexElement max_height); std::optional getCurrentBlockLodHeight( IndexElement map_tree_height, const Index3D& block_idx); diff --git a/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc b/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc index 2c631f00d..b929534e5 100644 --- a/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc +++ b/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc @@ -177,7 +177,7 @@ void VoxelVisual::updateMap(bool redraw_all) { } } -void VoxelVisual::updateLOD(const Point3D& camera_position) { +void VoxelVisual::updateLOD(const Ogre::Camera& active_camera) { ZoneScoped; if (!visibility_property_.getBool()) { return; @@ -200,13 +200,9 @@ void VoxelVisual::updateLOD(const Point3D& camera_position) { const auto min_termination_height = termination_height_property_.getInt(); for (const auto& [block_idx, block] : hashed_map->getBlocks()) { // Compute the recommended LOD level height - const OctreeIndex block_node_idx{tree_height, block_idx}; - const AABB block_aabb = - convert::nodeIndexToAABB(block_node_idx, map->getMinCellWidth()); - const FloatingPoint distance_to_cam = - block_aabb.minDistanceTo(camera_position); + const OctreeIndex block_node_index{tree_height, block_idx}; const auto term_height_recommended = computeRecommendedBlockLodHeight( - distance_to_cam, hashed_map->getMinCellWidth(), + active_camera, block_node_index, hashed_map->getMinCellWidth(), min_termination_height, tree_height - 1); // If the block is already queued to be updated, set the recommended level @@ -230,11 +226,27 @@ void VoxelVisual::updateLOD(const Point3D& camera_position) { } NdtreeIndexElement VoxelVisual::computeRecommendedBlockLodHeight( - FloatingPoint distance_to_cam, FloatingPoint min_cell_width, - NdtreeIndexElement min_height, NdtreeIndexElement max_height) { + const Ogre::Camera& active_camera, const OctreeIndex& block_index, + FloatingPoint min_cell_width, NdtreeIndexElement min_height, + NdtreeIndexElement max_height) { ZoneScoped; + // TODO(victorr): Compute the LoD level using the camera's projection matrix + // and the screen's pixel density, to better generalize across + // displays (high/low DPI) and alternative projection modes + // If the projection type is orthographic, e.g. when + // using Rviz's TopDownOrtho ViewController, always use the highest resolution + if (active_camera.getProjectionType() == Ogre::PT_ORTHOGRAPHIC) { + return min_height; + } + // Compute the recommended level based on the size of the cells projected into // the image plane + const AABB block_aabb = convert::nodeIndexToAABB(block_index, min_cell_width); + const Point3D camera_position{active_camera.getPosition().x, + active_camera.getPosition().y, + active_camera.getPosition().z}; + const FloatingPoint distance_to_cam = + block_aabb.minDistanceTo(camera_position); constexpr FloatingPoint kFactor = 0.002f; return std::clamp(static_cast(std::floor(std::log2( 1.f + kFactor * distance_to_cam / min_cell_width))), @@ -502,6 +514,7 @@ void VoxelVisual::setAlpha(FloatingPoint alpha) { void VoxelVisual::prerenderCallback(Ogre::Camera* active_camera) { ZoneScoped; + CHECK_NOTNULL(active_camera); // Recompute the desired LOD level for each block in the map if // the camera moved significantly or an update was requested explicitly const bool camera_moved = @@ -511,7 +524,7 @@ void VoxelVisual::prerenderCallback(Ogre::Camera* active_camera) { active_camera->getDerivedPosition().y, active_camera->getDerivedPosition().z}; if (force_lod_update_ || camera_moved) { - updateLOD(camera_position); + updateLOD(*active_camera); camera_position_at_last_lod_update_ = active_camera->getPosition(); force_lod_update_ = false; } From 9b66f7ce1907c3c71b636fff60964c3491955473 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 14 Dec 2023 12:48:26 +0100 Subject: [PATCH 045/159] Only hallucinate filler leaves when needed --- .../impl/ndtree_block_hash_inl.h | 47 ++++++++++--------- 1 file changed, 25 insertions(+), 22 deletions(-) diff --git a/libraries/wavemap/include/wavemap/data_structure/impl/ndtree_block_hash_inl.h b/libraries/wavemap/include/wavemap/data_structure/impl/ndtree_block_hash_inl.h index 9c8b571dc..291397bd4 100644 --- a/libraries/wavemap/include/wavemap/data_structure/impl/ndtree_block_hash_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/impl/ndtree_block_hash_inl.h @@ -223,30 +223,33 @@ void NdtreeBlockHash::forEachLeaf( const Node& node; }; - block_map_.forEachBlock( - [&visitor_fn, tree_height = tree_height_](const Index& block_index, - const Block& block) { - std::stack stack; - stack.emplace(StackElement{OctreeIndex{tree_height, block_index}, - block.getRootNode()}); - while (!stack.empty()) { - const OctreeIndex node_index = stack.top().node_index; - const Node& node = stack.top().node; - stack.pop(); - - for (NdtreeIndexRelativeChild child_idx = 0; - child_idx < OctreeIndex::kNumChildren; ++child_idx) { - const OctreeIndex child_node_index = - node_index.computeChildIndex(child_idx); - if (node.hasChild(child_idx)) { - const Node& child_node = *node.getChild(child_idx); - stack.emplace(StackElement{child_node_index, child_node}); - } else { - visitor_fn(child_node_index, node.data()); - } + block_map_.forEachBlock([&visitor_fn, tree_height = tree_height_]( + const Index& block_index, + const Block& block) { + std::stack stack; + stack.emplace(StackElement{OctreeIndex{tree_height, block_index}, + block.getRootNode()}); + while (!stack.empty()) { + const OctreeIndex node_index = stack.top().node_index; + const Node& node = stack.top().node; + stack.pop(); + + if (node.hasAtLeastOneChild()) { + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + const OctreeIndex child_node_index = + node_index.computeChildIndex(child_idx); + if (const Node* child_node = node.getChild(child_idx); child_node) { + stack.emplace(StackElement{child_node_index, *child_node}); + } else { + visitor_fn(child_node_index, node.data()); } } - }); + } else { + visitor_fn(node_index, node.data()); + } + } + }); } template From 1d0c51cdc716de0c3bf4db5136457efaff2e6f9a Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Sat, 16 Dec 2023 22:18:55 +0100 Subject: [PATCH 046/159] Add query accelerator for Spatial and NdtreeBlock hash data structures --- libraries/wavemap/CMakeLists.txt | 1 + .../utils/query/impl/query_accelerator_inl.h | 55 +++++++ .../wavemap/utils/query/query_accelerator.h | 139 +++++++++--------- .../src/utils/query/query_accelerator.cc | 71 +++++++++ .../visuals/cell_selector.h | 3 +- 5 files changed, 196 insertions(+), 73 deletions(-) create mode 100644 libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h create mode 100644 libraries/wavemap/src/utils/query/query_accelerator.cc diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index 626f3e535..72c192efb 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -66,6 +66,7 @@ add_library(${PROJECT_NAME} src/map/wavelet_octree.cc src/map/volumetric_data_structure_base.cc src/map/volumetric_data_structure_factory.cc + src/utils/query/query_accelerator.cc src/utils/sdf/full_euclidean_sdf_generator.cc src/utils/sdf/quasi_euclidean_sdf_generator.cc src/utils/stopwatch.cc diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h new file mode 100644 index 000000000..fd2b5cff4 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h @@ -0,0 +1,55 @@ +#ifndef WAVEMAP_UTILS_QUERY_IMPL_QUERY_ACCELERATOR_INL_H_ +#define WAVEMAP_UTILS_QUERY_IMPL_QUERY_ACCELERATOR_INL_H_ + +#include + +namespace wavemap { +template +BlockDataT* QueryAccelerator>::getBlock( + const Index& block_index) { + if (block_index != last_block_index_) { + last_block_index_ = block_index; + last_block_ = spatial_hash_.getBlock(block_index); + } + return last_block_; +} + +template +template +BlockDataT& QueryAccelerator>::getOrAllocateBlock( + const Index& block_index, DefaultArgs&&... args) { + if (block_index != last_block_index_ || !last_block_) { + last_block_index_ = block_index; + last_block_ = &spatial_hash_.getOrAllocateBlock( + block_index, std::forward(args)...); + } + return *last_block_; +} + +template +typename QueryAccelerator>::BlockType* +QueryAccelerator>::getBlock( + const Index& block_index) { + if (block_index != last_block_index_) { + last_block_index_ = block_index; + last_block_ = ndtree_block_hash_.getBlock(block_index); + } + return last_block_; +} + +template +template +typename QueryAccelerator>::BlockType& +QueryAccelerator>::getOrAllocateBlock( + const Index& block_index, DefaultArgs&&... args) { + if (block_index != last_block_index_ || !last_block_) { + last_block_index_ = block_index; + last_block_ = &ndtree_block_hash_.getOrAllocateBlock( + block_index, std::forward(args)...); + } + return *last_block_; +} + +} // namespace wavemap + +#endif // WAVEMAP_UTILS_QUERY_IMPL_QUERY_ACCELERATOR_INL_H_ diff --git a/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h b/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h index f6cb2a073..da5de15df 100644 --- a/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h +++ b/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h @@ -3,14 +3,72 @@ #include +#include "wavemap/data_structure/ndtree_block_hash.h" #include "wavemap/data_structure/spatial_hash.h" #include "wavemap/indexing/index_hashes.h" #include "wavemap/map/hashed_wavelet_octree.h" namespace wavemap { -class QueryAccelerator { +// Base template +template +class QueryAccelerator {}; + +// Template deduction guide +template +QueryAccelerator(T type) -> QueryAccelerator; + +// Query accelerator for vanilla spatial hashes +template +class QueryAccelerator> { + public: + static constexpr int kDim = dim; + + explicit QueryAccelerator(SpatialHash& spatial_hash) + : spatial_hash_(spatial_hash) {} + + BlockDataT* getBlock(const Index& block_index); + template + BlockDataT& getOrAllocateBlock(const Index& block_index, + DefaultArgs&&... args); + + private: + SpatialHash& spatial_hash_; + + Index last_block_index_ = + Index3D::Constant(std::numeric_limits::max()); + BlockDataT* last_block_ = nullptr; +}; + +// Query accelerator for ndtree block hashes +template +class QueryAccelerator> { + public: + static constexpr int kDim = dim; + using BlockType = typename NdtreeBlockHash::Block; + + explicit QueryAccelerator(NdtreeBlockHash& ndtree_block_hash) + : ndtree_block_hash_(ndtree_block_hash) {} + + BlockType* getBlock(const Index& block_index); + template + BlockType& getOrAllocateBlock(const Index& block_index, + DefaultArgs&&... args); + + // TODO(victorr): Implement accelerated cell accessors + + private: + NdtreeBlockHash& ndtree_block_hash_; + + Index last_block_index_ = + Index3D::Constant(std::numeric_limits::max()); + BlockType* last_block_ = nullptr; +}; + +// Query accelerator for hashed wavelet octrees +template <> +class QueryAccelerator { public: - static constexpr int kDim = 3; + static constexpr int kDim = HashedWaveletOctree::kDim; explicit QueryAccelerator(const HashedWaveletOctree& map) : block_map_(map.getHashMap()), tree_height_(map.getTreeHeight()) {} @@ -19,77 +77,12 @@ class QueryAccelerator { return getCellValue(OctreeIndex{0, index}); } - FloatingPoint getCellValue(const OctreeIndex& index) { - // Remember previous query indices and compute new ones - const BlockIndex previous_block_index = block_index_; - const MortonIndex previous_morton_code = morton_code_; - const IndexElement previous_height = height_; - block_index_ = computeBlockIndexFromIndex(index); - morton_code_ = convert::nodeIndexToMorton(index); - - // Check whether we're in the same block as last time - if (block_index_ == previous_block_index) { - // Compute the last ancestor the current and previous query had in common - auto last_common_ancestor = OctreeIndex::computeLastCommonAncestorHeight( - morton_code_, index.height, previous_morton_code, previous_height); - height_ = last_common_ancestor; - DCHECK_LE(height_, tree_height_); - } else { - // Test if the queried block exists - const auto* current_block = block_map_.getBlock(block_index_); - if (current_block) { - // If yes, load it - node_stack_[tree_height_] = ¤t_block->getRootNode(); - value_stack_[tree_height_] = current_block->getRootScale(); - height_ = tree_height_; - } else { - // Otherwise return ignore this query and return 'unknown' - block_index_ = previous_block_index; - morton_code_ = previous_morton_code; - return 0.f; - } - } - - // If the requested value was already decompressed in the last query, return - if (height_ == index.height) { - return value_stack_[height_]; - } - - // Load the node at height_ if it was not yet loaded last time - if (previous_height != tree_height_ && height_ == previous_height) { - const HashedWaveletOctree::Block::NodeType* parent_node = - node_stack_[height_ + 1]; - const NdtreeIndexRelativeChild child_index = - OctreeIndex::computeRelativeChildIndex(morton_code_, height_ + 1); - if (!parent_node->hasChild(child_index)) { - return value_stack_[height_]; - } - node_stack_[height_] = parent_node->getChild(child_index); - } - - // Walk down the tree from height_ to index.height - while (true) { - const HashedWaveletOctree::Block::NodeType* parent_node = - node_stack_[height_]; - const FloatingPoint parent_value = value_stack_[height_]; - const NdtreeIndexRelativeChild child_idx = - OctreeIndex::computeRelativeChildIndex(morton_code_, height_); - --height_; - value_stack_[height_] = Transform::backwardSingleChild( - {parent_value, parent_node->data()}, child_idx); - if (height_ == index.height || !parent_node->hasChild(child_idx)) { - break; - } - node_stack_[height_] = parent_node->getChild(child_idx); - } - - return value_stack_[height_]; - } + FloatingPoint getCellValue(const OctreeIndex& index); private: using Coefficients = HaarCoefficients; using Transform = HaarTransform; - using BlockIndex = Index3D; + using BlockIndex = typename HashedWaveletOctree::BlockIndex; using NodeType = NdtreeNode; const HashedWaveletOctree::BlockHashMap& block_map_; @@ -98,13 +91,13 @@ class QueryAccelerator { std::array> node_stack_{}; std::array> value_stack_{}; - Index3D block_index_ = - Index3D::Constant(std::numeric_limits::max()); + BlockIndex block_index_ = + BlockIndex ::Constant(std::numeric_limits::max()); MortonIndex morton_code_ = std::numeric_limits::max(); IndexElement height_ = tree_height_; BlockIndex computeBlockIndexFromIndex(const OctreeIndex& node_index) const { - const Index3D index = convert::nodeIndexToMinCornerIndex(node_index); + const BlockIndex index = convert::nodeIndexToMinCornerIndex(node_index); return int_math::div_exp2_floor(index, tree_height_); } @@ -112,4 +105,6 @@ class QueryAccelerator { }; } // namespace wavemap +#include "wavemap/utils/query/impl/query_accelerator_inl.h" + #endif // WAVEMAP_UTILS_QUERY_QUERY_ACCELERATOR_H_ diff --git a/libraries/wavemap/src/utils/query/query_accelerator.cc b/libraries/wavemap/src/utils/query/query_accelerator.cc new file mode 100644 index 000000000..fd59ecbd2 --- /dev/null +++ b/libraries/wavemap/src/utils/query/query_accelerator.cc @@ -0,0 +1,71 @@ +#include "wavemap/utils/query/query_accelerator.h" + +namespace wavemap { +FloatingPoint QueryAccelerator::getCellValue( + const OctreeIndex& index) { + // Remember previous query indices and compute new ones + const BlockIndex previous_block_index = block_index_; + const MortonIndex previous_morton_code = morton_code_; + const IndexElement previous_height = height_; + block_index_ = computeBlockIndexFromIndex(index); + morton_code_ = convert::nodeIndexToMorton(index); + + // Check whether we're in the same block as last time + if (block_index_ == previous_block_index) { + // Compute the last ancestor the current and previous query had in common + auto last_common_ancestor = OctreeIndex::computeLastCommonAncestorHeight( + morton_code_, index.height, previous_morton_code, previous_height); + height_ = last_common_ancestor; + DCHECK_LE(height_, tree_height_); + } else { + // Test if the queried block exists + const auto* current_block = block_map_.getBlock(block_index_); + if (current_block) { + // If yes, load it + node_stack_[tree_height_] = ¤t_block->getRootNode(); + value_stack_[tree_height_] = current_block->getRootScale(); + height_ = tree_height_; + } else { + // Otherwise return ignore this query and return 'unknown' + block_index_ = previous_block_index; + morton_code_ = previous_morton_code; + return 0.f; + } + } + + // If the requested value was already decompressed in the last query, return + if (height_ == index.height) { + return value_stack_[height_]; + } + + // Load the node at height_ if it was not yet loaded last time + if (previous_height != tree_height_ && height_ == previous_height) { + const HashedWaveletOctree::Block::NodeType* parent_node = + node_stack_[height_ + 1]; + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code_, height_ + 1); + if (!parent_node->hasChild(child_index)) { + return value_stack_[height_]; + } + node_stack_[height_] = parent_node->getChild(child_index); + } + + // Walk down the tree from height_ to index.height + while (true) { + const HashedWaveletOctree::Block::NodeType* parent_node = + node_stack_[height_]; + const FloatingPoint parent_value = value_stack_[height_]; + const NdtreeIndexRelativeChild child_idx = + OctreeIndex::computeRelativeChildIndex(morton_code_, height_); + --height_; + value_stack_[height_] = Transform::backwardSingleChild( + {parent_value, parent_node->data()}, child_idx); + if (height_ == index.height || !parent_node->hasChild(child_idx)) { + break; + } + node_stack_[height_] = parent_node->getChild(child_idx); + } + + return value_stack_[height_]; +} +} // namespace wavemap diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h index c999ce765..abd68545d 100644 --- a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h +++ b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h @@ -44,7 +44,8 @@ class CellSelector : public QObject { private: std::function redraw_map_; - mutable std::optional query_accelerator_; + mutable std::optional> + query_accelerator_; // Selection mode and thresholds CellSelectionMode cell_selection_mode_ = CellSelectionMode::kSurface; From c704ba47e273131711f411ab5bcba52e55be3c8e Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Sun, 17 Dec 2023 00:10:14 +0100 Subject: [PATCH 047/159] Replace checks with dchecks --- .../include/wavemap/data_structure/impl/bucket_queue_inl.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/wavemap/include/wavemap/data_structure/impl/bucket_queue_inl.h b/libraries/wavemap/include/wavemap/data_structure/impl/bucket_queue_inl.h index 98d9a7a72..97ad085ba 100644 --- a/libraries/wavemap/include/wavemap/data_structure/impl/bucket_queue_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/impl/bucket_queue_inl.h @@ -33,7 +33,7 @@ void BucketQueue::clear() { template void BucketQueue::push(FloatingPoint key, const ValueT& value) { - CHECK_NE(num_buckets_, 0); + DCHECK_NE(num_buckets_, 0); if (key > max_key_) { key = max_key_; } @@ -65,8 +65,8 @@ void BucketQueue::pop() { template ValueT BucketQueue::front() { - CHECK_NE(num_buckets_, 0); - CHECK(!empty()); + DCHECK_NE(num_buckets_, 0); + DCHECK(!empty()); while (last_bucket_index_ < num_buckets_ && buckets_[last_bucket_index_].empty()) { last_bucket_index_++; From 1e3076e28ef3c15be0a33c4934af1ceb13956d93 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 27 Dec 2023 19:05:44 +0100 Subject: [PATCH 048/159] Simplify traversing ChunkedNdtrees by emulating single node ptrs --- .../chunked_ndtree/chunked_ndtree.h | 47 +++-- ...ee_node_chunk.h => chunked_ndtree_chunk.h} | 43 +++-- .../chunked_ndtree/chunked_ndtree_node_ptr.h | 61 ++++++ ...chunk_inl.h => chunked_ndtree_chunk_inl.h} | 91 ++++----- .../chunked_ndtree/impl/chunked_ndtree_inl.h | 175 ++++++++++-------- .../impl/chunked_ndtree_node_ptr_inl.h | 106 +++++++++++ .../map/hashed_chunked_wavelet_octree_block.h | 9 +- .../include/wavemap/utils/math/tree_math.h | 3 +- .../hashed_chunked_wavelet_integrator.cc | 3 +- .../hashed_chunked_wavelet_octree_block.cc | 63 ++----- .../test/src/data_structure/test_ndtree.cc | 58 +++--- 11 files changed, 414 insertions(+), 245 deletions(-) rename libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/{ndtree_node_chunk.h => chunked_ndtree_chunk.h} (62%) create mode 100644 libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_ptr.h rename libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/{ndtree_node_chunk_inl.h => chunked_ndtree_chunk_inl.h} (65%) create mode 100644 libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_ptr_inl.h diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h index fef22b4a1..8aea02b4c 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h @@ -6,7 +6,8 @@ #include #include "wavemap/common.h" -#include "wavemap/data_structure/chunked_ndtree/ndtree_node_chunk.h" +#include "wavemap/data_structure/chunked_ndtree/chunked_ndtree_chunk.h" +#include "wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_ptr.h" #include "wavemap/indexing/ndtree_index.h" #include "wavemap/utils/iterate/subtree_iterator.h" @@ -16,7 +17,9 @@ class ChunkedNdtree { public: using IndexType = NdtreeIndex; using HeightType = IndexElement; - using NodeChunkType = NdtreeNodeChunk; + using ChunkType = ChunkedNdtreeChunk; + using NodePtrType = ChunkedNdtreeNodePtr; + using NodeConstPtrType = ChunkedNdtreeNodePtr; using NodeDataType = NodeDataT; static constexpr HeightType kChunkHeight = chunk_height; @@ -31,32 +34,36 @@ class ChunkedNdtree { HeightType getMaxHeight() const { return max_height_; } size_t getMemoryUsage() const; - bool hasNode(const IndexType& index) const; + // TODO(victorr): Add methods to directly query and operate on chunks, + // once a proper index type for chunks has been defined. + // The ChunkIndex type would be similar to NdtreeIndex, but has + // to account for the chunks having a branching factor that + // differs from 2 (probably 2^(dim * chunk_height)). - NodeDataT* getNodeData(const IndexType& index, bool auto_allocate = true); - const NodeDataT* getNodeData(const IndexType& index) const; - void getOrAllocateNode(const IndexType& index); + bool hasNode(const IndexType& index) const { return getNode(index); } + NodePtrType getNode(const IndexType& index); + NodeConstPtrType getNode(const IndexType& index) const; + template + NodePtrType getOrAllocateNode(const IndexType& index, DefaultArgs&&... args); - NodeChunkType& getRootChunk() { return root_chunk_; } - const NodeChunkType& getRootChunk() const { return root_chunk_; } + std::pair getNodeOrAncestor(const IndexType& index); + std::pair getNodeOrAncestor( + const IndexType& index) const; + + ChunkType& getRootChunk() { return root_chunk_; } + const ChunkType& getRootChunk() const { return root_chunk_; } + + NodePtrType getRootNode() { return {root_chunk_, 0, 0}; } + NodeConstPtrType getRootNode() const { return {root_chunk_, 0, 0}; } template - auto getIterator() { - return Subtree(&root_chunk_); - } + auto getChunkIterator(); template - auto getIterator() const { - return Subtree(&root_chunk_); - } + auto getChunkIterator() const; private: - NodeChunkType root_chunk_; + ChunkType root_chunk_; const HeightType max_height_; - - std::pair getChunkAndRelativeIndex( - const IndexType& index, bool auto_allocate); - std::pair getChunkAndRelativeIndex( - const IndexType& index) const; }; template diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/ndtree_node_chunk.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_chunk.h similarity index 62% rename from libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/ndtree_node_chunk.h rename to libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_chunk.h index c604042b1..f06dd40cb 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/ndtree_node_chunk.h +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_chunk.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_NDTREE_NODE_CHUNK_H_ -#define WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_NDTREE_NODE_CHUNK_H_ +#ifndef WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_CHUNK_H_ +#define WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_CHUNK_H_ #include #include @@ -12,8 +12,10 @@ namespace wavemap { template -class NdtreeNodeChunk { +class ChunkedNdtreeChunk { public: + static constexpr int kDim = dim; + static constexpr int kHeight = height; static constexpr int kNumInnerNodes = tree_math::perfect_tree::num_total_nodes(height); static constexpr int kNumChildren = @@ -22,32 +24,30 @@ class NdtreeNodeChunk { using DataType = DataT; using BitRef = typename std::bitset::reference; - NdtreeNodeChunk() = default; - ~NdtreeNodeChunk() = default; + ChunkedNdtreeChunk() = default; + ~ChunkedNdtreeChunk() = default; bool empty() const; void clear(); - friend bool operator==(const NdtreeNodeChunk& lhs, - const NdtreeNodeChunk& rhs) { - return &rhs == &lhs; - } + size_t getMemoryUsage() const; // Methods to operate at the chunk level bool hasNonzeroData() const; bool hasNonzeroData(FloatingPoint threshold) const; bool hasChildrenArray() const { return static_cast(child_chunks_); } + bool hasAtLeastOneChild() const; void deleteChildrenArray() { child_chunks_.reset(); } bool hasChild(LinearIndex relative_child_index) const; - bool hasAtLeastOneChild() const; - NdtreeNodeChunk* allocateChild(LinearIndex relative_child_index); - bool deleteChild(LinearIndex relative_child_index); - NdtreeNodeChunk* getChild(LinearIndex relative_child_index); - const NdtreeNodeChunk* getChild(LinearIndex relative_child_index) const; + bool eraseChild(LinearIndex relative_child_index); - size_t getMemoryUsage() const; + ChunkedNdtreeChunk* getChild(LinearIndex relative_child_index); + const ChunkedNdtreeChunk* getChild(LinearIndex relative_child_index) const; + template + ChunkedNdtreeChunk& getOrAllocateChild(LinearIndex relative_child_index, + DefaultArgs&&... args); // Methods to operate on individual nodes inside the chunk bool nodeHasNonzeroData(LinearIndex relative_node_index) const; @@ -60,11 +60,16 @@ class NdtreeNodeChunk { BitRef nodeHasAtLeastOneChild(LinearIndex relative_node_index); bool nodeHasAtLeastOneChild(LinearIndex relative_node_index) const; + friend bool operator==(const ChunkedNdtreeChunk& lhs, + const ChunkedNdtreeChunk& rhs) { + return &rhs == &lhs; + } + private: using NodeDataArray = std::array; using NodeChildBitset = std::bitset; - using ChildChunkArray = - std::array, kNumChildren>; + using ChunkPtr = std::unique_ptr; + using ChildChunkArray = std::array; NodeDataArray node_data_{}; NodeChildBitset node_has_at_least_one_child_{}; @@ -72,6 +77,6 @@ class NdtreeNodeChunk { }; } // namespace wavemap -#include "wavemap/data_structure/chunked_ndtree/impl/ndtree_node_chunk_inl.h" +#include "wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h" -#endif // WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_NDTREE_NODE_CHUNK_H_ +#endif // WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_CHUNK_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_ptr.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_ptr.h new file mode 100644 index 000000000..48d8b195e --- /dev/null +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_ptr.h @@ -0,0 +1,61 @@ +#ifndef WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_NODE_PTR_H_ +#define WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_NODE_PTR_H_ + +#include "wavemap/common.h" +#include "wavemap/data_structure/chunked_ndtree/chunked_ndtree_chunk.h" +#include "wavemap/utils/data/comparisons.h" + +namespace wavemap { +template +class ChunkedNdtreeNodePtr { + public: + static constexpr int kDim = ChunkType::kDim; + static constexpr int kNumChildren = NdtreeIndex::kNumChildren; + + ChunkedNdtreeNodePtr() = default; + ChunkedNdtreeNodePtr(ChunkType& chunk, IndexElement relative_node_depth, + LinearIndex level_traversal_distance) + : ChunkedNdtreeNodePtr(&chunk, relative_node_depth, + level_traversal_distance) {} + ChunkedNdtreeNodePtr(ChunkType* chunk, IndexElement relative_node_depth, + LinearIndex level_traversal_distance) + : chunk_(chunk), + relative_node_depth_(relative_node_depth), + level_traversal_distance_(level_traversal_distance) {} + + operator bool() const { return chunk_; } // NOLINT + + bool empty() const { return !hasAtLeastOneChild() && !hasNonzeroData(); } + + bool hasNonzeroData() const; + bool hasNonzeroData(FloatingPoint threshold) const; + auto& data(); + const auto& data() const; + + typename ChunkType::BitRef hasAtLeastOneChild(); + bool hasAtLeastOneChild() const; + + bool hasChild(NdtreeIndexRelativeChild child_index) const; + + ChunkedNdtreeNodePtr getChild( + NdtreeIndexRelativeChild child_index); + ChunkedNdtreeNodePtr getChild( + NdtreeIndexRelativeChild child_index) const; + template + ChunkedNdtreeNodePtr getOrAllocateChild(NdtreeIndexRelativeChild child_index, + DefaultArgs&&... args) const; + + private: + ChunkType* chunk_ = nullptr; + IndexElement relative_node_depth_ = 0; + LinearIndex level_traversal_distance_ = 0u; + + LinearIndex computeRelativeNodeIndex() const; + LinearIndex computeChildLevelTraversalDistance( + NdtreeIndexRelativeChild child_index) const; +}; +} // namespace wavemap + +#include "wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_ptr_inl.h" + +#endif // WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_NODE_PTR_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/ndtree_node_chunk_inl.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h similarity index 65% rename from libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/ndtree_node_chunk_inl.h rename to libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h index 62476c621..33d45db67 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/ndtree_node_chunk_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_NDTREE_NODE_CHUNK_INL_H_ -#define WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_NDTREE_NODE_CHUNK_INL_H_ +#ifndef WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_CHUNK_INL_H_ +#define WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_CHUNK_INL_H_ #include #include @@ -8,25 +8,34 @@ namespace wavemap { template -bool NdtreeNodeChunk::empty() const { +bool ChunkedNdtreeChunk::empty() const { return !hasChildrenArray() && !hasNonzeroData(); } template -void NdtreeNodeChunk::clear() { +void ChunkedNdtreeChunk::clear() { deleteChildrenArray(); std::fill(node_data_.begin(), node_data_.end(), DataT{}); } template -bool NdtreeNodeChunk::hasNonzeroData() const { +size_t ChunkedNdtreeChunk::getMemoryUsage() const { + size_t memory_usage = sizeof(ChunkedNdtreeChunk); + if (hasChildrenArray()) { + memory_usage += sizeof(ChildChunkArray); + } + return memory_usage; +} + +template +bool ChunkedNdtreeChunk::hasNonzeroData() const { return std::any_of( node_data_.cbegin(), node_data_.cend(), [](const auto& node_data) { return data::is_nonzero(node_data); }); } template -bool NdtreeNodeChunk::hasNonzeroData( +bool ChunkedNdtreeChunk::hasNonzeroData( FloatingPoint threshold) const { return std::any_of(node_data_.cbegin(), node_data_.cend(), [threshold](const auto& node_data) { @@ -35,7 +44,7 @@ bool NdtreeNodeChunk::hasNonzeroData( } template -DataT& NdtreeNodeChunk::nodeData( +DataT& ChunkedNdtreeChunk::nodeData( LinearIndex relative_node_index) { CHECK_GE(relative_node_index, 0u); CHECK_LT(relative_node_index, kNumInnerNodes); @@ -43,7 +52,7 @@ DataT& NdtreeNodeChunk::nodeData( } template -const DataT& NdtreeNodeChunk::nodeData( +const DataT& ChunkedNdtreeChunk::nodeData( LinearIndex relative_node_index) const { CHECK_GE(relative_node_index, 0u); CHECK_LT(relative_node_index, kNumInnerNodes); @@ -51,13 +60,7 @@ const DataT& NdtreeNodeChunk::nodeData( } template -bool NdtreeNodeChunk::hasChild( - LinearIndex relative_child_index) const { - return getChild(relative_child_index); -} - -template -bool NdtreeNodeChunk::hasAtLeastOneChild() const { +bool ChunkedNdtreeChunk::hasAtLeastOneChild() const { if (hasChildrenArray()) { return std::any_of( child_chunks_->cbegin(), child_chunks_->cend(), @@ -67,21 +70,13 @@ bool NdtreeNodeChunk::hasAtLeastOneChild() const { } template -NdtreeNodeChunk* -NdtreeNodeChunk::allocateChild( - LinearIndex relative_child_index) { - CHECK_GE(relative_child_index, 0u); - CHECK_LT(relative_child_index, kNumChildren); - if (!hasChildrenArray()) { - child_chunks_ = std::make_unique(); - } - child_chunks_->operator[](relative_child_index) = - std::make_unique(); - return child_chunks_->operator[](relative_child_index).get(); +bool ChunkedNdtreeChunk::hasChild( + LinearIndex relative_child_index) const { + return getChild(relative_child_index); } template -bool NdtreeNodeChunk::deleteChild( +bool ChunkedNdtreeChunk::eraseChild( LinearIndex relative_child_index) { if (hasChild(relative_child_index)) { child_chunks_->operator[](relative_child_index).reset(); @@ -91,8 +86,8 @@ bool NdtreeNodeChunk::deleteChild( } template -NdtreeNodeChunk* -NdtreeNodeChunk::getChild( +ChunkedNdtreeChunk* +ChunkedNdtreeChunk::getChild( LinearIndex relative_child_index) { CHECK_GE(relative_child_index, 0u); CHECK_LT(relative_child_index, kNumChildren); @@ -103,8 +98,8 @@ NdtreeNodeChunk::getChild( } template -const NdtreeNodeChunk* -NdtreeNodeChunk::getChild( +const ChunkedNdtreeChunk* +ChunkedNdtreeChunk::getChild( LinearIndex relative_child_index) const { CHECK_GE(relative_child_index, 0u); CHECK_LT(relative_child_index, kNumChildren); @@ -115,42 +110,54 @@ NdtreeNodeChunk::getChild( } template -size_t NdtreeNodeChunk::getMemoryUsage() const { - size_t memory_usage = sizeof(NdtreeNodeChunk); - if (hasChildrenArray()) { - memory_usage += sizeof(ChildChunkArray); +template +ChunkedNdtreeChunk& +ChunkedNdtreeChunk::getOrAllocateChild( + LinearIndex relative_child_index, DefaultArgs&&... args) { + CHECK_GE(relative_child_index, 0u); + CHECK_LT(relative_child_index, kNumChildren); + // Make sure the children array is allocated + if (!hasChildrenArray()) { + child_chunks_ = std::make_unique(); } - return memory_usage; + // Get the child, allocating it if needed + ChunkPtr& child_smart_ptr = child_chunks_->operator[](relative_child_index); + if (!child_smart_ptr) { + child_smart_ptr = std::make_unique( + std::forward(args)...); + } + // Return a reference to the child + return *child_smart_ptr; } template -bool NdtreeNodeChunk::nodeHasNonzeroData( +bool ChunkedNdtreeChunk::nodeHasNonzeroData( LinearIndex relative_node_index) const { DCHECK_LT(relative_node_index, kNumInnerNodes); return data::is_nonzero(node_data_[relative_node_index]); } template -bool NdtreeNodeChunk::nodeHasNonzeroData( +bool ChunkedNdtreeChunk::nodeHasNonzeroData( LinearIndex relative_node_index, FloatingPoint threshold) const { DCHECK_LT(relative_node_index, kNumInnerNodes); return data::is_nonzero(node_data_[relative_node_index], threshold); } template -typename NdtreeNodeChunk::BitRef -NdtreeNodeChunk::nodeHasAtLeastOneChild( +typename ChunkedNdtreeChunk::BitRef +ChunkedNdtreeChunk::nodeHasAtLeastOneChild( LinearIndex relative_node_index) { DCHECK_LT(relative_node_index, kNumInnerNodes); return node_has_at_least_one_child_[relative_node_index]; } template -bool NdtreeNodeChunk::nodeHasAtLeastOneChild( +bool ChunkedNdtreeChunk::nodeHasAtLeastOneChild( LinearIndex relative_node_index) const { DCHECK_LT(relative_node_index, kNumInnerNodes); return node_has_at_least_one_child_[relative_node_index]; } } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_NDTREE_NODE_CHUNK_INL_H_ +#endif // WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_CHUNK_INL_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h index 70f7e1748..1f0903a4d 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h @@ -18,24 +18,25 @@ ChunkedNdtree::ChunkedNdtree(int max_height) template size_t ChunkedNdtree::size() const { - auto subtree_iterator = getIterator(); + auto subtree_iterator = + getChunkIterator(); const size_t num_chunks = std::distance(subtree_iterator.begin(), subtree_iterator.end()); - return num_chunks * NodeChunkType::kNumInnerNodes; + return num_chunks * ChunkType::kNumInnerNodes; } template void ChunkedNdtree::prune() { - for (NodeChunkType& chunk : - getIterator()) { + for (ChunkType& chunk : + getChunkIterator()) { if (chunk.hasChildrenArray()) { bool has_non_empty_child = false; - for (LinearIndex child_idx = 0; child_idx < NodeChunkType::kNumChildren; + for (LinearIndex child_idx = 0; child_idx < ChunkType::kNumChildren; ++child_idx) { - NodeChunkType* child_ptr = chunk.getChild(child_idx); + ChunkType* child_ptr = chunk.getChild(child_idx); if (child_ptr) { if (child_ptr->empty()) { - chunk.deleteChild(child_idx); + chunk.eraseChild(child_idx); } else { has_non_empty_child = true; } @@ -54,15 +55,15 @@ template size_t ChunkedNdtree::getMemoryUsage() const { size_t memory_usage = 0u; - std::stack stack; + std::stack stack; stack.emplace(&root_chunk_); while (!stack.empty()) { - const NodeChunkType* chunk = stack.top(); + const ChunkType* chunk = stack.top(); stack.pop(); memory_usage += chunk->getMemoryUsage(); if (chunk->hasChildrenArray()) { - for (LinearIndex child_idx = 0; child_idx < NodeChunkType::kNumChildren; + for (LinearIndex child_idx = 0; child_idx < ChunkType::kNumChildren; ++child_idx) { if (chunk->hasChild(child_idx)) { stack.emplace(chunk->getChild(child_idx)); @@ -75,100 +76,116 @@ size_t ChunkedNdtree::getMemoryUsage() const { } template -bool ChunkedNdtree::hasNode( - const ChunkedNdtree::IndexType& index) const { - return getChunkAndRelativeIndex(index).first; -} - -template -void ChunkedNdtree::getOrAllocateNode( - const IndexType& index) { - getChunkAndRelativeIndex(index, /*auto_allocate*/ true); +typename ChunkedNdtree::NodePtrType +ChunkedNdtree::getNode( + const ChunkedNdtree::IndexType& index) { + NodePtrType node = getRootNode(); + const MortonIndex morton_code = convert::nodeIndexToMorton(index); + for (int node_height = max_height_; index.height < node_height; + --node_height) { + const NdtreeIndexRelativeChild child_index = + NdtreeIndex::computeRelativeChildIndex(morton_code, node_height); + // Check if the child is allocated + NodePtrType child = node.getChild(child_index); + if (!child) { + return {}; + } + node = child; + } + return node; } template -NodeDataT* ChunkedNdtree::getNodeData( - const ChunkedNdtree::IndexType& index, bool auto_allocate) { - auto [chunk, relative_index] = getChunkAndRelativeIndex(index, auto_allocate); - if (chunk) { - return &chunk->nodeData(relative_index); +typename ChunkedNdtree::NodeConstPtrType +ChunkedNdtree::getNode( + const ChunkedNdtree::IndexType& index) const { + NodeConstPtrType node = getRootNode(); + const MortonIndex morton_code = convert::nodeIndexToMorton(index); + for (int node_height = max_height_; index.height < node_height; + --node_height) { + const NdtreeIndexRelativeChild child_index = + NdtreeIndex::computeRelativeChildIndex(morton_code, node_height); + // Check if the child is allocated + NodeConstPtrType child = node.getChild(child_index); + if (!child) { + return {}; + } + node = child; } - return nullptr; + return node; } template -const NodeDataT* ChunkedNdtree::getNodeData( - const ChunkedNdtree::IndexType& index) const { - auto [chunk, relative_index] = getChunkAndRelativeIndex(index); - if (chunk) { - return &chunk->nodeData(relative_index); +template +typename ChunkedNdtree::NodePtrType +ChunkedNdtree::getOrAllocateNode( + const ChunkedNdtree::IndexType& index, DefaultArgs&&... args) { + NodePtrType node = getRootNode(); + const MortonIndex morton_code = convert::nodeIndexToMorton(index); + for (int node_height = max_height_; index.height < node_height; + --node_height) { + const NdtreeIndexRelativeChild child_index = + NdtreeIndex::computeRelativeChildIndex(morton_code, node_height); + // Get the child, allocating if needed + node = node.getOrAllocateChild(child_index, + std::forward(args)...); } - return nullptr; + return node; } template -std::pair::NodeChunkType*, - LinearIndex> -ChunkedNdtree::getChunkAndRelativeIndex( - const ChunkedNdtree::IndexType& index, bool auto_allocate) { +std::pair::NodePtrType, + typename ChunkedNdtree::HeightType> +ChunkedNdtree::getNodeOrAncestor( + const ChunkedNdtree::IndexType& index) { + NodePtrType node = getRootNode(); const MortonIndex morton_code = convert::nodeIndexToMorton(index); - const int chunk_top_height = - chunk_height * int_math::div_round_up(index.height, chunk_height); - - NodeChunkType* current_parent = &root_chunk_; - for (int parent_height = max_height_; chunk_top_height < parent_height; - parent_height -= chunk_height) { - const int child_height = parent_height - chunk_height; - const LinearIndex child_index = - NdtreeIndex::computeLevelTraversalDistance( - morton_code, parent_height, child_height); + for (int node_height = max_height_; index.height < node_height; + --node_height) { + const NdtreeIndexRelativeChild child_index = + NdtreeIndex::computeRelativeChildIndex(morton_code, node_height); // Check if the child is allocated - if (!current_parent->hasChild(child_index)) { - if (auto_allocate) { - current_parent->allocateChild(child_index); - } else { - return {nullptr, 0u}; - } + NodePtrType child = node.getChild(child_index); + if (!child) { + return {node, node_height}; } - current_parent = current_parent->getChild(child_index); + node = child; } - - const LinearIndex relative_index = - NdtreeIndex::computeTreeTraversalDistance( - morton_code, chunk_top_height, index.height); - - return {current_parent, relative_index}; + return {node, index.height}; } template std::pair< - const typename ChunkedNdtree::NodeChunkType*, - LinearIndex> -ChunkedNdtree::getChunkAndRelativeIndex( + typename ChunkedNdtree::NodeConstPtrType, + typename ChunkedNdtree::HeightType> +ChunkedNdtree::getNodeOrAncestor( const ChunkedNdtree::IndexType& index) const { + NodeConstPtrType node = getRootNode(); const MortonIndex morton_code = convert::nodeIndexToMorton(index); - const int chunk_top_height = - chunk_height * int_math::div_round_up(index.height, chunk_height); - - const NodeChunkType* current_parent = &root_chunk_; - for (int parent_height = max_height_; chunk_top_height < parent_height; - parent_height -= chunk_height) { - const int child_height = parent_height - chunk_height; - const LinearIndex child_index = - NdtreeIndex::computeLevelTraversalDistance( - morton_code, parent_height, child_height); - // Return if the child is not allocated - if (!current_parent->hasChild(child_index)) { - return {nullptr, 0u}; + for (int node_height = max_height_; index.height < node_height; + --node_height) { + const NdtreeIndexRelativeChild child_index = + NdtreeIndex::computeRelativeChildIndex(morton_code, node_height); + // Check if the child is allocated + NodeConstPtrType child = node.getChild(child_index); + if (!child) { + return {node, node_height}; } - current_parent = current_parent->getChild(child_index); + node = child; } + return {node, index.height}; +} - const LinearIndex relative_index = - NdtreeIndex::computeTreeTraversalDistance( - morton_code, chunk_top_height, index.height); +template +template +auto ChunkedNdtree::getChunkIterator() { + return Subtree(&root_chunk_); +} - return {current_parent, relative_index}; +template +template +auto ChunkedNdtree::getChunkIterator() const { + return Subtree(&root_chunk_); } } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_ptr_inl.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_ptr_inl.h new file mode 100644 index 000000000..246979529 --- /dev/null +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_ptr_inl.h @@ -0,0 +1,106 @@ +#ifndef WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_NODE_PTR_INL_H_ +#define WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_NODE_PTR_INL_H_ + +#include + +namespace wavemap { +template +bool ChunkedNdtreeNodePtr::hasNonzeroData() const { + return chunk_->nodeHasNonzeroData(computeRelativeNodeIndex()); +} + +template +bool ChunkedNdtreeNodePtr::hasNonzeroData( + FloatingPoint threshold) const { + return chunk_->nodeHasNonzeroData(computeRelativeNodeIndex(), threshold); +} + +template +auto& ChunkedNdtreeNodePtr::data() { + return chunk_->nodeData(computeRelativeNodeIndex()); +} + +template +const auto& ChunkedNdtreeNodePtr::data() const { + return chunk_->nodeData(computeRelativeNodeIndex()); +} + +template +typename ChunkType::BitRef +ChunkedNdtreeNodePtr::hasAtLeastOneChild() { + return chunk_->nodeHasAtLeastOneChild(computeRelativeNodeIndex()); +} + +template +bool ChunkedNdtreeNodePtr::hasAtLeastOneChild() const { + return chunk_->nodeHasAtLeastOneChild(computeRelativeNodeIndex()); +} + +template +bool ChunkedNdtreeNodePtr::hasChild( + NdtreeIndexRelativeChild child_index) const { + return getChild(child_index); +} + +template +ChunkedNdtreeNodePtr ChunkedNdtreeNodePtr::getChild( + NdtreeIndexRelativeChild child_index) { + const IndexElement child_depth = relative_node_depth_ + 1; + const LinearIndex child_level_traversal_distance = + computeChildLevelTraversalDistance(child_index); + if (child_depth % ChunkType::kHeight == 0) { + auto* child_chunk = chunk_->getChild(child_level_traversal_distance); + return {child_chunk, 0, 0u}; + } else { + return {chunk_, child_depth, child_level_traversal_distance}; + } +} + +template +ChunkedNdtreeNodePtr ChunkedNdtreeNodePtr::getChild( + NdtreeIndexRelativeChild child_index) const { + const IndexElement child_depth = relative_node_depth_ + 1; + const LinearIndex child_level_traversal_distance = + computeChildLevelTraversalDistance(child_index); + if (child_depth % ChunkType::kHeight == 0) { + const auto* child_chunk = chunk_->getChild(child_level_traversal_distance); + return {child_chunk, 0, 0u}; + } else { + return {chunk_, child_depth, child_level_traversal_distance}; + } +} + +template +template +ChunkedNdtreeNodePtr +ChunkedNdtreeNodePtr::getOrAllocateChild( + NdtreeIndexRelativeChild child_index, DefaultArgs&&... args) const { + const IndexElement child_depth = relative_node_depth_ + 1; + const LinearIndex child_level_traversal_distance = + computeChildLevelTraversalDistance(child_index); + if (child_depth % ChunkType::kHeight == 0) { + auto& child_chunk = chunk_->getOrAllocateChild( + child_level_traversal_distance, std::forward(args)...); + return {child_chunk, 0, 0}; + } else { + return {chunk_, child_depth, child_level_traversal_distance}; + } +} + +template +LinearIndex ChunkedNdtreeNodePtr::computeRelativeNodeIndex() const { + const LinearIndex parent_to_first_child_distance = + tree_math::perfect_tree::num_total_nodes_fast(relative_node_depth_); + return parent_to_first_child_distance + level_traversal_distance_; +} + +template +LinearIndex ChunkedNdtreeNodePtr::computeChildLevelTraversalDistance( + NdtreeIndexRelativeChild child_index) const { + DCHECK_GE(child_index, 0); + DCHECK_LT(child_index, 1 << kDim); + return (level_traversal_distance_ << kDim) | child_index; +} +} // namespace wavemap + +#endif // WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_NODE_PTR_INL_H_ diff --git a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h index 7d6ea01b4..d88951fe6 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h +++ b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h @@ -18,7 +18,7 @@ class HashedChunkedWaveletOctreeBlock { using Coefficients = HaarCoefficients; using Transform = HaarTransform; using ChunkedOctreeType = ChunkedOctree; - using NodeChunkType = ChunkedOctreeType::NodeChunkType; + using NodeChunkType = ChunkedOctreeType::ChunkType; explicit HashedChunkedWaveletOctreeBlock(IndexElement tree_height, FloatingPoint min_log_odds, @@ -39,7 +39,8 @@ class HashedChunkedWaveletOctreeBlock { void forEachLeaf( const BlockIndex& block_index, typename VolumetricDataStructureBase::IndexedLeafVisitorFunction - visitor_fn) const; + visitor_fn, + IndexElement termination_height = 0) const; Coefficients::Scale& getRootScale() { return root_scale_coefficient_; } const Coefficients::Scale& getRootScale() const { @@ -65,11 +66,11 @@ class HashedChunkedWaveletOctreeBlock { template auto getChunkIterator() { - return chunked_ndtree_.getIterator(); + return chunked_ndtree_.getChunkIterator(); } template auto getChunkIterator() const { - return chunked_ndtree_.getIterator(); + return chunked_ndtree_.getChunkIterator(); } size_t getMemoryUsage() const { return chunked_ndtree_.getMemoryUsage(); } diff --git a/libraries/wavemap/include/wavemap/utils/math/tree_math.h b/libraries/wavemap/include/wavemap/utils/math/tree_math.h index 0f78819fb..9dce143bd 100644 --- a/libraries/wavemap/include/wavemap/utils/math/tree_math.h +++ b/libraries/wavemap/include/wavemap/utils/math/tree_math.h @@ -15,7 +15,8 @@ constexpr size_t num_total_nodes_fast(size_t tree_height) { constexpr size_t kMaxHeight = kBitWidth / dim - 1; constexpr size_t kMaxNumNodes = num_total_nodes(kMaxHeight); DCHECK(tree_height <= kMaxHeight); - return kMaxNumNodes >> ((kMaxHeight - tree_height) * dim); + const size_t height_difference = kMaxHeight - tree_height; + return kMaxNumNodes >> (height_difference * dim); } template diff --git a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc index 45fadcb5d..8979d746e 100644 --- a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc +++ b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc @@ -158,7 +158,8 @@ void HashedChunkedWaveletIntegrator::updateNodeRecursive( // NOLINT morton_code, parent_chunk_top_height, child_height); chunk_containing_child = parent_chunk.getChild(linear_child_index); if (!chunk_containing_child) { - chunk_containing_child = parent_chunk.allocateChild(linear_child_index); + chunk_containing_child = + &parent_chunk.getOrAllocateChild(linear_child_index); } child_node_in_chunk_index = 0u; } diff --git a/libraries/wavemap/src/map/hashed_chunked_wavelet_octree_block.cc b/libraries/wavemap/src/map/hashed_chunked_wavelet_octree_block.cc index b3214a8af..79917c619 100644 --- a/libraries/wavemap/src/map/hashed_chunked_wavelet_octree_block.cc +++ b/libraries/wavemap/src/map/hashed_chunked_wavelet_octree_block.cc @@ -67,7 +67,7 @@ void HashedChunkedWaveletOctreeBlock::setCellValue(const OctreeIndex& index, chunk_ptrs[chunk_depth + 1] = current_chunk->getChild(linear_child_index); } else { chunk_ptrs[chunk_depth + 1] = - current_chunk->allocateChild(linear_child_index); + ¤t_chunk->getOrAllocateChild(linear_child_index); } } @@ -116,7 +116,7 @@ void HashedChunkedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index, chunk_ptrs[chunk_depth] = current_chunk->getChild(linear_child_index); } else { chunk_ptrs[chunk_depth] = - current_chunk->allocateChild(linear_child_index); + ¤t_chunk->getOrAllocateChild(linear_child_index); } } @@ -145,7 +145,8 @@ void HashedChunkedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index, void HashedChunkedWaveletOctreeBlock::forEachLeaf( const BlockIndex& block_index, - VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn) const { + VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn, + IndexElement termination_height) const { ZoneScoped; if (empty()) { return; @@ -153,60 +154,32 @@ void HashedChunkedWaveletOctreeBlock::forEachLeaf( struct StackElement { const OctreeIndex node_index; - const NodeChunkType& chunk; + ChunkedNdtreeNodePtr node; const Coefficients::Scale scale_coefficient{}; }; std::stack stack; stack.emplace(StackElement{{tree_height_, block_index}, - chunked_ndtree_.getRootChunk(), + chunked_ndtree_.getRootNode(), root_scale_coefficient_}); while (!stack.empty()) { const OctreeIndex index = stack.top().node_index; - const NodeChunkType& chunk = stack.top().chunk; + const auto node = stack.top().node; const FloatingPoint scale_coefficient = stack.top().scale_coefficient; stack.pop(); - const MortonIndex morton_code = convert::nodeIndexToMorton(index); - const int chunk_top_height = - kChunkHeight * int_math::div_round_up(index.height, kChunkHeight); - const LinearIndex relative_node_index = - OctreeIndex::computeTreeTraversalDistance(morton_code, chunk_top_height, - index.height); const Coefficients::CoefficientsArray child_scale_coefficients = - Transform::backward( - {scale_coefficient, chunk.nodeData(relative_node_index)}); - const bool parent_has_nonzero_children = - chunk.nodeHasAtLeastOneChild(relative_node_index); - - for (NdtreeIndexRelativeChild relative_child_idx = 0; - relative_child_idx < OctreeIndex::kNumChildren; ++relative_child_idx) { - const OctreeIndex child_index = - index.computeChildIndex(relative_child_idx); + Transform::backward({scale_coefficient, {node.data()}}); + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + const OctreeIndex child_node_index = index.computeChildIndex(child_idx); const FloatingPoint child_scale_coefficient = - child_scale_coefficients[relative_child_idx]; - - const bool child_in_same_chunk = child_index.height % kChunkHeight != 0; - if (child_in_same_chunk) { - if (parent_has_nonzero_children) { - stack.emplace( - StackElement{child_index, chunk, child_scale_coefficient}); - } else { - visitor_fn(child_index, child_scale_coefficient); - } + child_scale_coefficients[child_idx]; + auto child_node = node.getChild(child_idx); + if (child_node && termination_height < child_node_index.height) { + stack.emplace(StackElement{child_node_index, child_node, + child_scale_coefficient}); } else { - const MortonIndex child_morton = - convert::nodeIndexToMorton(child_index); - const LinearIndex relative_child_chunk_index = - OctreeIndex::computeLevelTraversalDistance( - child_morton, chunk_top_height, child_index.height); - if (chunk.hasChild(relative_child_chunk_index)) { - const NodeChunkType& child_chunk = - *chunk.getChild(relative_child_chunk_index); - stack.emplace( - StackElement{child_index, child_chunk, child_scale_coefficient}); - } else { - visitor_fn(child_index, child_scale_coefficient); - } + visitor_fn(child_node_index, child_scale_coefficient); } } } @@ -305,7 +278,7 @@ void HashedChunkedWaveletOctreeBlock::recursivePrune( // NOLINT recursivePrune(child_chunk); if (!child_chunk.hasChildrenArray() && !child_chunk.hasNonzeroData(kNonzeroCoefficientThreshold)) { - chunk.deleteChild(linear_child_idx); + chunk.eraseChild(linear_child_idx); } else { has_at_least_one_child = true; } diff --git a/libraries/wavemap/test/src/data_structure/test_ndtree.cc b/libraries/wavemap/test/src/data_structure/test_ndtree.cc index 55706539f..3668c82f5 100644 --- a/libraries/wavemap/test/src/data_structure/test_ndtree.cc +++ b/libraries/wavemap/test/src/data_structure/test_ndtree.cc @@ -54,31 +54,6 @@ TYPED_TEST(NdtreeTest, AllocatingAndClearing) { } } -// TODO(victorr): Remove this workaround after improving the interfaces of the -// ChunkedNdtree. -template -auto* getOrAllocateNodeData(const IndexT& index, TreeT& ndtree) { - using TreeType = std::decay_t; - if constexpr (std::is_same_v> || - std::is_same_v> || - std::is_same_v>) { - return ndtree.getNodeData(index); - } else { - return &ndtree.getOrAllocateNode(index).data(); - } -} -template -auto* getNodeData(const IndexT& index, TreeT& ndtree) { - using TreeType = std::decay_t; - if constexpr (std::is_same_v> || - std::is_same_v> || - std::is_same_v>) { - return ndtree.getNodeData(index); - } else { - return &CHECK_NOTNULL(ndtree.getNode(index))->data(); - } -} - TYPED_TEST(NdtreeTest, GettingAndSetting) { using IndexType = typename TypeParam::IndexType; using PositionType = typename IndexType::Position; @@ -114,17 +89,24 @@ TYPED_TEST(NdtreeTest, GettingAndSetting) { inserted_values.emplace(random_index, random_value); // Insert - auto* data = getOrAllocateNodeData(random_index, ndtree); - ASSERT_NE(data, nullptr) << "At index " << random_index.toString(); - *data = random_value; + auto& data = ndtree.getOrAllocateNode(random_index).data(); + data = random_value; } // Test regular getter for (const auto& [index, value] : inserted_values) { EXPECT_TRUE(ndtree.hasNode(index)) << "At index " << index.toString(); - auto* data = getNodeData(index, ndtree); - ASSERT_NE(data, nullptr) << "At index " << index.toString(); - EXPECT_EQ(*data, value) << "At index " << index.toString(); + auto node = ndtree.getNode(index); + ASSERT_TRUE(node) << "At index " << index.toString(); + if (node) { + typename TypeParam::NodeDataType data; + if constexpr (TypeParam::kChunkHeight == 1) { + data = node->data(); + } else { + data = node.data(); + } + EXPECT_EQ(data, value) << "At index " << index.toString(); + } } // Test const getter @@ -132,9 +114,17 @@ TYPED_TEST(NdtreeTest, GettingAndSetting) { for (const auto& [index, value] : inserted_values) { EXPECT_TRUE(ndtree_cref.hasNode(index)) << "At index " << index.toString(); - auto* data = getNodeData(index, ndtree_cref); - ASSERT_NE(data, nullptr) << "At index " << index.toString(); - EXPECT_EQ(*data, value) << "At index " << index.toString(); + auto node = ndtree_cref.getNode(index); + ASSERT_TRUE(node) << "At index " << index.toString(); + if (node) { + typename TypeParam::NodeDataType data; + if constexpr (TypeParam::kChunkHeight == 1) { + data = node->data(); + } else { + data = node.data(); + } + EXPECT_EQ(data, value) << "At index " << index.toString(); + } } } } From dbb608695584145b7bf2bbd14e0a1ca352ba9264 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 27 Dec 2023 19:29:39 +0100 Subject: [PATCH 049/159] Update serialization methods to use the new simulated ptrs --- .../map/hashed_chunked_wavelet_octree_block.h | 4 ++ .../wavemap_io/src/stream_conversions.cc | 71 +++++-------------- .../map_msg_conversions.h | 1 - .../src/map_msg_conversions.cc | 68 +++++------------- 4 files changed, 36 insertions(+), 108 deletions(-) diff --git a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h index d88951fe6..2be7724df 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h +++ b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h @@ -19,6 +19,8 @@ class HashedChunkedWaveletOctreeBlock { using Transform = HaarTransform; using ChunkedOctreeType = ChunkedOctree; using NodeChunkType = ChunkedOctreeType::ChunkType; + using NodePtrType = ChunkedOctreeType::NodePtrType; + using NodeConstPtrType = ChunkedOctreeType::NodeConstPtrType; explicit HashedChunkedWaveletOctreeBlock(IndexElement tree_height, FloatingPoint min_log_odds, @@ -46,6 +48,8 @@ class HashedChunkedWaveletOctreeBlock { const Coefficients::Scale& getRootScale() const { return root_scale_coefficient_; } + NodePtrType getRootNode() { return chunked_ndtree_.getRootNode(); } + NodeConstPtrType getRootNode() const { return chunked_ndtree_.getRootNode(); } NodeChunkType& getRootChunk() { return chunked_ndtree_.getRootChunk(); } const NodeChunkType& getRootChunk() const { return chunked_ndtree_.getRootChunk(); diff --git a/libraries/wavemap_io/src/stream_conversions.cc b/libraries/wavemap_io/src/stream_conversions.cc index de661c003..e6344f4ac 100644 --- a/libraries/wavemap_io/src/stream_conversions.cc +++ b/libraries/wavemap_io/src/stream_conversions.cc @@ -360,15 +360,12 @@ bool streamToMap(std::istream& istream, HashedWaveletOctree::Ptr& map) { void mapToStream(const HashedChunkedWaveletOctree& map, std::ostream& ostream) { // Define convenience types and constants struct StackElement { - const OctreeIndex node_index; - const HashedChunkedWaveletOctreeBlock::NodeChunkType& chunk; - const FloatingPoint scale_coefficient; + const FloatingPoint scale; + HashedChunkedWaveletOctreeBlock::NodeConstPtrType node; }; constexpr FloatingPoint kNumericalNoise = 1e-3f; const auto min_log_odds = map.getMinLogOdds() + kNumericalNoise; const auto max_log_odds = map.getMaxLogOdds() - kNumericalNoise; - const auto tree_height = map.getTreeHeight(); - const auto chunk_height = map.getChunkHeight(); // Indicate the map's data structure type streamable::StorageFormat storage_format = @@ -385,9 +382,8 @@ void mapToStream(const HashedChunkedWaveletOctree& map, std::ostream& ostream) { hashed_wavelet_octree_header.write(ostream); // Iterate over all the map's blocks - map.forEachBlock([&ostream, tree_height, chunk_height, min_log_odds, - max_log_odds](const Index3D& block_index, - const auto& block) { + map.forEachBlock([&ostream, min_log_odds, max_log_odds]( + const Index3D& block_index, const auto& block) { // Serialize the block's metadata streamable::HashedWaveletOctreeBlockHeader block_header; block_header.root_node_offset = {block_index.x(), block_index.y(), @@ -398,69 +394,34 @@ void mapToStream(const HashedChunkedWaveletOctree& map, std::ostream& ostream) { // Serialize the block's data (all nodes of its octree) std::stack stack; - stack.emplace(StackElement{{tree_height, block_index}, - block.getRootChunk(), - block.getRootScale()}); + stack.emplace(StackElement{block.getRootScale(), block.getRootNode()}); while (!stack.empty()) { - const OctreeIndex index = stack.top().node_index; - const FloatingPoint scale = stack.top().scale_coefficient; - const auto& chunk = stack.top().chunk; + const FloatingPoint scale = stack.top().scale; + auto node = stack.top().node; stack.pop(); - // Compute the node's index w.r.t. the data chunk that contains it - const MortonIndex morton_code = convert::nodeIndexToMorton(index); - const int chunk_top_height = - chunk_height * int_math::div_round_up(index.height, chunk_height); - const LinearIndex relative_node_index = - OctreeIndex::computeTreeTraversalDistance( - morton_code, chunk_top_height, index.height); - // Serialize the node's data streamable::WaveletOctreeNode streamable_node; - const auto& node_data = chunk.nodeData(relative_node_index); - std::copy(node_data.begin(), node_data.end(), + std::copy(node.data().begin(), node.data().end(), streamable_node.detail_coefficients.begin()); - // If the node has no children, continue - if (!chunk.nodeHasAtLeastOneChild(relative_node_index)) { - streamable_node.write(ostream); - continue; - } - - // Otherwise, evaluate which of its children should be serialized + // Evaluate which of its children should be serialized const auto child_scales = - HashedWaveletOctreeBlock::Transform::backward({scale, node_data}); + HashedWaveletOctreeBlock::Transform::backward({scale, node.data()}); // NOTE: We iterate and add nodes to the stack in decreasing order s.t. // the nodes are popped from the stack in increasing order. for (int relative_child_idx = OctreeIndex::kNumChildren - 1; 0 <= relative_child_idx; --relative_child_idx) { // If the child is saturated, we don't need to store its descendants - const FloatingPoint child_scale = child_scales[relative_child_idx]; + const auto child_scale = child_scales[relative_child_idx]; if (child_scale < min_log_odds || max_log_odds < child_scale) { continue; } - - // Check if the child is no longer in the current chunk - const OctreeIndex child_index = - index.computeChildIndex(relative_child_idx); - if (child_index.height % chunk_height == 0) { - // If so, check if the chunk exists - const MortonIndex child_morton = - convert::nodeIndexToMorton(child_index); - const LinearIndex linear_child_index = - OctreeIndex::computeLevelTraversalDistance( - child_morton, chunk_top_height, child_index.height); - if (chunk.hasChild(linear_child_index)) { - const auto& child_chunk = *chunk.getChild(linear_child_index); - // Indicate that the child will be serialized - // and add it to the stack - stack.emplace(StackElement{child_index, child_chunk, child_scale}); - streamable_node.allocated_children_bitset += - (1 << relative_child_idx); - } - } else { - // Indicate that the child will be serialized and add it to the stack - stack.emplace(StackElement{child_index, chunk, child_scale}); + // Otherwise, indicate that the child will be serialized + // and add it to the stack + auto child = node.getChild(relative_child_idx); + if (child) { + stack.emplace(StackElement{child_scale, child}); streamable_node.allocated_children_bitset += (1 << relative_child_idx); } diff --git a/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h b/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h index 7d406d1b1..99849b4a2 100644 --- a/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h +++ b/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h @@ -51,7 +51,6 @@ void mapToRosMsg(const HashedChunkedWaveletOctree& map, void blockToRosMsg(const HashedChunkedWaveletOctree::BlockIndex& block_index, const HashedChunkedWaveletOctree::Block& block, FloatingPoint min_log_odds, FloatingPoint max_log_odds, - IndexElement tree_height, wavemap_msgs::HashedWaveletOctreeBlock& msg); } // namespace wavemap::convert diff --git a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc index 7973e5fb2..ffe9d4d15 100644 --- a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc +++ b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc @@ -507,11 +507,11 @@ void mapToRosMsg( thread_pool->add_task([block_index, block, min_log_odds, max_log_odds, tree_height, &block_msg]() { blockToRosMsg(block_index, *block, min_log_odds, max_log_odds, - tree_height, block_msg); + block_msg); }); } else { // Otherwise, use the current thread blockToRosMsg(block_index, *block, min_log_odds, max_log_odds, - tree_height, block_msg); + block_msg); } } } @@ -525,17 +525,13 @@ void mapToRosMsg( void blockToRosMsg(const HashedChunkedWaveletOctree::BlockIndex& block_index, const HashedChunkedWaveletOctree::Block& block, FloatingPoint min_log_odds, FloatingPoint max_log_odds, - IndexElement tree_height, wavemap_msgs::HashedWaveletOctreeBlock& msg) { ZoneScoped; // Define convenience types and constants struct StackElement { - const OctreeIndex node_index; - const HashedChunkedWaveletOctreeBlock::NodeChunkType& chunk; - const FloatingPoint scale_coefficient; + const FloatingPoint scale; + HashedChunkedWaveletOctreeBlock::NodeConstPtrType node; }; - constexpr IndexElement chunk_height = - HashedChunkedWaveletOctreeBlock::kChunkHeight; // Serialize the block's metadata msg.root_node_offset.x = block_index.x(); @@ -546,67 +542,35 @@ void blockToRosMsg(const HashedChunkedWaveletOctree::BlockIndex& block_index, // Serialize the block's data (all nodes of its octree) std::stack stack; - stack.emplace(StackElement{ - {tree_height, block_index}, block.getRootChunk(), block.getRootScale()}); + stack.emplace(StackElement{block.getRootScale(), block.getRootNode()}); while (!stack.empty()) { - const OctreeIndex index = stack.top().node_index; - const FloatingPoint scale = stack.top().scale_coefficient; - const auto& chunk = stack.top().chunk; + const FloatingPoint scale = stack.top().scale; + auto node = stack.top().node; stack.pop(); - // Compute the node's index w.r.t. the data chunk that contains it - const MortonIndex morton_code = convert::nodeIndexToMorton(index); - const int chunk_top_height = - chunk_height * int_math::div_round_up(index.height, chunk_height); - const LinearIndex relative_node_index = - OctreeIndex::computeTreeTraversalDistance(morton_code, chunk_top_height, - index.height); - // Serialize the node's data auto& node_msg = msg.nodes.emplace_back(); - const auto& node_data = chunk.nodeData(relative_node_index); - std::copy(node_data.cbegin(), node_data.cend(), + std::copy(node.data().cbegin(), node.data().cend(), node_msg.detail_coefficients.begin()); node_msg.allocated_children_bitset = 0; - // If the node has no children, continue - if (!chunk.nodeHasAtLeastOneChild(relative_node_index)) { - continue; - } - - // Otherwise, evaluate which of its children should be serialized + // Evaluate which of its children should be serialized const auto child_scales = - HashedWaveletOctreeBlock::Transform::backward({scale, node_data}); + HashedWaveletOctreeBlock::Transform::backward({scale, node.data()}); // NOTE: We iterate and add nodes to the stack in decreasing order s.t. // the nodes are popped from the stack in increasing order. for (int relative_child_idx = OctreeIndex::kNumChildren - 1; 0 <= relative_child_idx; --relative_child_idx) { // If the child is saturated, we don't need to store its descendants - const FloatingPoint child_scale = child_scales[relative_child_idx]; + const auto child_scale = child_scales[relative_child_idx]; if (child_scale < min_log_odds || max_log_odds < child_scale) { continue; } - - // Check if the child is no longer in the current chunk - const OctreeIndex child_index = - index.computeChildIndex(relative_child_idx); - if (child_index.height % chunk_height == 0) { - // If so, check if the chunk exists - const MortonIndex child_morton = - convert::nodeIndexToMorton(child_index); - const LinearIndex linear_child_index = - OctreeIndex::computeLevelTraversalDistance( - child_morton, chunk_top_height, child_index.height); - if (chunk.hasChild(linear_child_index)) { - const auto& child_chunk = *chunk.getChild(linear_child_index); - // Indicate that the child will be serialized - // and add it to the stack - stack.emplace(StackElement{child_index, child_chunk, child_scale}); - node_msg.allocated_children_bitset += (1 << relative_child_idx); - } - } else { - // Indicate that the child will be serialized and add it to the stack - stack.emplace(StackElement{child_index, chunk, child_scale}); + // Otherwise, indicate that the child will be serialized + // and add it to the stack + auto child = node.getChild(relative_child_idx); + if (child) { + stack.emplace(StackElement{child_scale, child}); node_msg.allocated_children_bitset += (1 << relative_child_idx); } } From b4349e16972eeed2641dfeb028e33d4ebdb633ef Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 28 Dec 2023 16:02:34 +0100 Subject: [PATCH 050/159] Improve simulated pointer/reference semantics --- .../chunked_ndtree/chunked_ndtree.h | 8 +- .../chunked_ndtree/chunked_ndtree_node_ptr.h | 91 +++++++++--- .../chunked_ndtree/impl/chunked_ndtree_inl.h | 28 ++-- .../impl/chunked_ndtree_node_ptr_inl.h | 135 ++++++++++++++---- .../hashed_chunked_wavelet_integrator.h | 9 +- .../map/hashed_chunked_wavelet_octree_block.h | 22 +-- .../hashed_chunked_wavelet_octree_block_inl.h | 3 +- .../hashed_chunked_wavelet_integrator.cc | 8 +- .../hashed_chunked_wavelet_octree_block.cc | 36 ++--- .../test/src/data_structure/test_ndtree.cc | 14 +- .../wavemap_io/src/stream_conversions.cc | 7 +- .../src/map_msg_conversions.cc | 7 +- 12 files changed, 253 insertions(+), 115 deletions(-) diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h index 8aea02b4c..106792a9f 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h @@ -18,6 +18,8 @@ class ChunkedNdtree { using IndexType = NdtreeIndex; using HeightType = IndexElement; using ChunkType = ChunkedNdtreeChunk; + using NodeRefType = ChunkedNdtreeNodeRef; + using NodeConstRefType = ChunkedNdtreeNodeRef; using NodePtrType = ChunkedNdtreeNodePtr; using NodeConstPtrType = ChunkedNdtreeNodePtr; using NodeDataType = NodeDataT; @@ -44,7 +46,7 @@ class ChunkedNdtree { NodePtrType getNode(const IndexType& index); NodeConstPtrType getNode(const IndexType& index) const; template - NodePtrType getOrAllocateNode(const IndexType& index, DefaultArgs&&... args); + NodeRefType getOrAllocateNode(const IndexType& index, DefaultArgs&&... args); std::pair getNodeOrAncestor(const IndexType& index); std::pair getNodeOrAncestor( @@ -53,8 +55,8 @@ class ChunkedNdtree { ChunkType& getRootChunk() { return root_chunk_; } const ChunkType& getRootChunk() const { return root_chunk_; } - NodePtrType getRootNode() { return {root_chunk_, 0, 0}; } - NodeConstPtrType getRootNode() const { return {root_chunk_, 0, 0}; } + NodeRefType getRootNode() { return {root_chunk_, 0, 0}; } + NodeConstRefType getRootNode() const { return {root_chunk_, 0, 0}; } template auto getChunkIterator(); diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_ptr.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_ptr.h index 48d8b195e..f4e503539 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_ptr.h +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_ptr.h @@ -1,31 +1,79 @@ #ifndef WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_NODE_PTR_H_ #define WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_NODE_PTR_H_ +#include + #include "wavemap/common.h" #include "wavemap/data_structure/chunked_ndtree/chunked_ndtree_chunk.h" #include "wavemap/utils/data/comparisons.h" namespace wavemap { +template +class ChunkedNdtreeNodeRef; + template class ChunkedNdtreeNodePtr { public: - static constexpr int kDim = ChunkType::kDim; - static constexpr int kNumChildren = NdtreeIndex::kNumChildren; + using NodeRef = ChunkedNdtreeNodeRef; + using NodeConstRef = ChunkedNdtreeNodeRef; + // Constructors ChunkedNdtreeNodePtr() = default; - ChunkedNdtreeNodePtr(ChunkType& chunk, IndexElement relative_node_depth, - LinearIndex level_traversal_distance) - : ChunkedNdtreeNodePtr(&chunk, relative_node_depth, - level_traversal_distance) {} ChunkedNdtreeNodePtr(ChunkType* chunk, IndexElement relative_node_depth, - LinearIndex level_traversal_distance) - : chunk_(chunk), - relative_node_depth_(relative_node_depth), - level_traversal_distance_(level_traversal_distance) {} + LinearIndex level_traversal_distance); + + // Copy/move constructors + ChunkedNdtreeNodePtr(const ChunkedNdtreeNodePtr& other); + ChunkedNdtreeNodePtr(ChunkedNdtreeNodePtr&& other) noexcept; - operator bool() const { return chunk_; } // NOLINT + // Copy/move assignment operators + ChunkedNdtreeNodePtr& operator=(const ChunkedNdtreeNodePtr& other); + ChunkedNdtreeNodePtr& operator=(ChunkedNdtreeNodePtr&& other) noexcept; - bool empty() const { return !hasAtLeastOneChild() && !hasNonzeroData(); } + // Emulate pointer semantics + void reset() { node_.reset(); } + operator bool() const { return node_.has_value(); } // NOLINT + NodeRef operator*() { return node_.value(); } + NodeConstRef operator*() const { return node_.value(); } + NodeRef* operator->() { return node_.operator->(); } + const NodeConstRef* operator->() const { return node_.operator->(); } + + private: + std::optional node_; +}; + +template +class ChunkedNdtreeNodeRef { + public: + using NodeRef = ChunkedNdtreeNodeRef; + using NodeConstRef = ChunkedNdtreeNodeRef; + using NodePtr = ChunkedNdtreeNodePtr; + using NodeConstPtr = ChunkedNdtreeNodePtr; + + static constexpr int kDim = ChunkType::kDim; + static constexpr int kNumChildren = NdtreeIndex::kNumChildren; + using NodeDataType = typename ChunkType::DataType; + + ChunkedNdtreeNodeRef(ChunkType& chunk, IndexElement relative_node_depth, + LinearIndex level_traversal_distance); + + // Copy/move constructor + ChunkedNdtreeNodeRef(const ChunkedNdtreeNodeRef& other); + ChunkedNdtreeNodeRef(ChunkedNdtreeNodeRef&& other) noexcept; + + // Copy/move assignment operators are deleted (to behave like a ref) + ChunkedNdtreeNodeRef& operator=(const ChunkedNdtreeNodeRef&) = delete; + ChunkedNdtreeNodeRef& operator=(ChunkedNdtreeNodeRef&&) = delete; + + // Conversion to const ref + operator NodeConstRef(); + + // Conversion to pointer + NodePtr operator&(); // NOLINT + NodeConstPtr operator&() const; // NOLINT + + // Regular node methods + bool empty() const; bool hasNonzeroData() const; bool hasNonzeroData(FloatingPoint threshold) const; @@ -37,22 +85,23 @@ class ChunkedNdtreeNodePtr { bool hasChild(NdtreeIndexRelativeChild child_index) const; - ChunkedNdtreeNodePtr getChild( - NdtreeIndexRelativeChild child_index); - ChunkedNdtreeNodePtr getChild( - NdtreeIndexRelativeChild child_index) const; + NodePtr getChild(NdtreeIndexRelativeChild child_index); + NodeConstPtr getChild(NdtreeIndexRelativeChild child_index) const; template - ChunkedNdtreeNodePtr getOrAllocateChild(NdtreeIndexRelativeChild child_index, - DefaultArgs&&... args) const; + NodeRef getOrAllocateChild(NdtreeIndexRelativeChild child_index, + DefaultArgs&&... args) const; private: - ChunkType* chunk_ = nullptr; - IndexElement relative_node_depth_ = 0; - LinearIndex level_traversal_distance_ = 0u; + ChunkType& chunk_; + const IndexElement relative_node_depth_ = 0; + const LinearIndex level_traversal_distance_ = 0u; LinearIndex computeRelativeNodeIndex() const; LinearIndex computeChildLevelTraversalDistance( NdtreeIndexRelativeChild child_index) const; + + template + friend class ChunkedNdtreeNodePtr; }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h index 1f0903a4d..8fba55d86 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h @@ -33,7 +33,7 @@ void ChunkedNdtree::prune() { bool has_non_empty_child = false; for (LinearIndex child_idx = 0; child_idx < ChunkType::kNumChildren; ++child_idx) { - ChunkType* child_ptr = chunk.getChild(child_idx); + ChunkType* child_ptr = chunk->getChild(child_idx); if (child_ptr) { if (child_ptr->empty()) { chunk.eraseChild(child_idx); @@ -79,14 +79,14 @@ template typename ChunkedNdtree::NodePtrType ChunkedNdtree::getNode( const ChunkedNdtree::IndexType& index) { - NodePtrType node = getRootNode(); + NodePtrType node = &getRootNode(); const MortonIndex morton_code = convert::nodeIndexToMorton(index); for (int node_height = max_height_; index.height < node_height; --node_height) { const NdtreeIndexRelativeChild child_index = NdtreeIndex::computeRelativeChildIndex(morton_code, node_height); // Check if the child is allocated - NodePtrType child = node.getChild(child_index); + NodePtrType child = node->getChild(child_index); if (!child) { return {}; } @@ -99,14 +99,14 @@ template typename ChunkedNdtree::NodeConstPtrType ChunkedNdtree::getNode( const ChunkedNdtree::IndexType& index) const { - NodeConstPtrType node = getRootNode(); + NodeConstPtrType node = &getRootNode(); const MortonIndex morton_code = convert::nodeIndexToMorton(index); for (int node_height = max_height_; index.height < node_height; --node_height) { const NdtreeIndexRelativeChild child_index = NdtreeIndex::computeRelativeChildIndex(morton_code, node_height); // Check if the child is allocated - NodeConstPtrType child = node.getChild(child_index); + NodeConstPtrType child = node->getChild(child_index); if (!child) { return {}; } @@ -117,20 +117,20 @@ ChunkedNdtree::getNode( template template -typename ChunkedNdtree::NodePtrType +typename ChunkedNdtree::NodeRefType ChunkedNdtree::getOrAllocateNode( const ChunkedNdtree::IndexType& index, DefaultArgs&&... args) { - NodePtrType node = getRootNode(); + NodePtrType node = &getRootNode(); const MortonIndex morton_code = convert::nodeIndexToMorton(index); for (int node_height = max_height_; index.height < node_height; --node_height) { const NdtreeIndexRelativeChild child_index = NdtreeIndex::computeRelativeChildIndex(morton_code, node_height); // Get the child, allocating if needed - node = node.getOrAllocateChild(child_index, - std::forward(args)...); + node = &node->getOrAllocateChild(child_index, + std::forward(args)...); } - return node; + return *node; } template @@ -138,14 +138,14 @@ std::pair::NodePtrType, typename ChunkedNdtree::HeightType> ChunkedNdtree::getNodeOrAncestor( const ChunkedNdtree::IndexType& index) { - NodePtrType node = getRootNode(); + NodePtrType node = &getRootNode(); const MortonIndex morton_code = convert::nodeIndexToMorton(index); for (int node_height = max_height_; index.height < node_height; --node_height) { const NdtreeIndexRelativeChild child_index = NdtreeIndex::computeRelativeChildIndex(morton_code, node_height); // Check if the child is allocated - NodePtrType child = node.getChild(child_index); + NodePtrType child = node->getChild(child_index); if (!child) { return {node, node_height}; } @@ -160,14 +160,14 @@ std::pair< typename ChunkedNdtree::HeightType> ChunkedNdtree::getNodeOrAncestor( const ChunkedNdtree::IndexType& index) const { - NodeConstPtrType node = getRootNode(); + NodeConstPtrType node = &getRootNode(); const MortonIndex morton_code = convert::nodeIndexToMorton(index); for (int node_height = max_height_; index.height < node_height; --node_height) { const NdtreeIndexRelativeChild child_index = NdtreeIndex::computeRelativeChildIndex(morton_code, node_height); // Check if the child is allocated - NodeConstPtrType child = node.getChild(child_index); + NodeConstPtrType child = node->getChild(child_index); if (!child) { return {node, node_height}; } diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_ptr_inl.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_ptr_inl.h index 246979529..b830754bb 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_ptr_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_ptr_inl.h @@ -5,81 +5,168 @@ namespace wavemap { template -bool ChunkedNdtreeNodePtr::hasNonzeroData() const { - return chunk_->nodeHasNonzeroData(computeRelativeNodeIndex()); +ChunkedNdtreeNodePtr::ChunkedNdtreeNodePtr( + ChunkType* chunk, IndexElement relative_node_depth, + LinearIndex level_traversal_distance) + : node_(chunk ? std::make_optional(*chunk, relative_node_depth, + level_traversal_distance) + : std::nullopt) {} + +template +ChunkedNdtreeNodePtr::ChunkedNdtreeNodePtr( + const ChunkedNdtreeNodePtr& other) + : node_(other ? std::make_optional(other.node_.value()) + : std::nullopt) {} + +template +ChunkedNdtreeNodePtr::ChunkedNdtreeNodePtr( + ChunkedNdtreeNodePtr&& other) noexcept + : node_(other ? std::make_optional(std::move(other.node_.value())) + : std::nullopt) {} + +template +ChunkedNdtreeNodePtr& ChunkedNdtreeNodePtr::operator=( + const ChunkedNdtreeNodePtr& other) { + if (other) { + node_.emplace(other.node_.value()); + } else { + reset(); + } + return *this; +} + +template +ChunkedNdtreeNodePtr& ChunkedNdtreeNodePtr::operator=( + ChunkedNdtreeNodePtr&& other) noexcept { + if (other) { + node_.emplace(std::move(other.node_.value())); + } else { + reset(); + } + return *this; +} + +template +ChunkedNdtreeNodeRef::ChunkedNdtreeNodeRef( + ChunkType& chunk, IndexElement relative_node_depth, + LinearIndex level_traversal_distance) + : chunk_(chunk), + relative_node_depth_(relative_node_depth), + level_traversal_distance_(level_traversal_distance) {} + +template +ChunkedNdtreeNodeRef::ChunkedNdtreeNodeRef( + const ChunkedNdtreeNodeRef& other) + : ChunkedNdtreeNodeRef(other.chunk_, other.relative_node_depth_, + other.level_traversal_distance_) {} + +template +ChunkedNdtreeNodeRef::ChunkedNdtreeNodeRef( + ChunkedNdtreeNodeRef&& other) noexcept + : chunk_(other.chunk_), + relative_node_depth_(other.relative_node_depth_), + level_traversal_distance_(other.level_traversal_distance_) {} + +template +ChunkedNdtreeNodeRef::operator NodeConstRef() { + return {chunk_, relative_node_depth_, level_traversal_distance_}; +} + +template +typename ChunkedNdtreeNodeRef::NodePtr +ChunkedNdtreeNodeRef::operator&() { // NOLINT + return {&chunk_, relative_node_depth_, level_traversal_distance_}; +} + +template +typename ChunkedNdtreeNodeRef::NodeConstPtr +ChunkedNdtreeNodeRef::operator&() const { // NOLINT + return {&chunk_, relative_node_depth_, level_traversal_distance_}; } template -bool ChunkedNdtreeNodePtr::hasNonzeroData( +bool ChunkedNdtreeNodeRef::empty() const { + return !hasAtLeastOneChild() && !hasNonzeroData(); +} + +template +bool ChunkedNdtreeNodeRef::hasNonzeroData() const { + return chunk_.nodeHasNonzeroData(computeRelativeNodeIndex()); +} + +template +bool ChunkedNdtreeNodeRef::hasNonzeroData( FloatingPoint threshold) const { - return chunk_->nodeHasNonzeroData(computeRelativeNodeIndex(), threshold); + return chunk_.nodeHasNonzeroData(computeRelativeNodeIndex(), threshold); } template -auto& ChunkedNdtreeNodePtr::data() { - return chunk_->nodeData(computeRelativeNodeIndex()); +auto& ChunkedNdtreeNodeRef::data() { + return chunk_.nodeData(computeRelativeNodeIndex()); } template -const auto& ChunkedNdtreeNodePtr::data() const { - return chunk_->nodeData(computeRelativeNodeIndex()); +const auto& ChunkedNdtreeNodeRef::data() const { + return chunk_.nodeData(computeRelativeNodeIndex()); } template typename ChunkType::BitRef -ChunkedNdtreeNodePtr::hasAtLeastOneChild() { - return chunk_->nodeHasAtLeastOneChild(computeRelativeNodeIndex()); +ChunkedNdtreeNodeRef::hasAtLeastOneChild() { + return chunk_.nodeHasAtLeastOneChild(computeRelativeNodeIndex()); } template -bool ChunkedNdtreeNodePtr::hasAtLeastOneChild() const { - return chunk_->nodeHasAtLeastOneChild(computeRelativeNodeIndex()); +bool ChunkedNdtreeNodeRef::hasAtLeastOneChild() const { + return chunk_.nodeHasAtLeastOneChild(computeRelativeNodeIndex()); } template -bool ChunkedNdtreeNodePtr::hasChild( +bool ChunkedNdtreeNodeRef::hasChild( NdtreeIndexRelativeChild child_index) const { return getChild(child_index); } template -ChunkedNdtreeNodePtr ChunkedNdtreeNodePtr::getChild( +typename ChunkedNdtreeNodeRef::NodePtr +ChunkedNdtreeNodeRef::getChild( NdtreeIndexRelativeChild child_index) { const IndexElement child_depth = relative_node_depth_ + 1; const LinearIndex child_level_traversal_distance = computeChildLevelTraversalDistance(child_index); if (child_depth % ChunkType::kHeight == 0) { - auto* child_chunk = chunk_->getChild(child_level_traversal_distance); + auto* child_chunk = chunk_.getChild(child_level_traversal_distance); return {child_chunk, 0, 0u}; } else { - return {chunk_, child_depth, child_level_traversal_distance}; + return {&chunk_, child_depth, child_level_traversal_distance}; } } template -ChunkedNdtreeNodePtr ChunkedNdtreeNodePtr::getChild( +typename ChunkedNdtreeNodeRef::NodeConstPtr +ChunkedNdtreeNodeRef::getChild( NdtreeIndexRelativeChild child_index) const { const IndexElement child_depth = relative_node_depth_ + 1; const LinearIndex child_level_traversal_distance = computeChildLevelTraversalDistance(child_index); if (child_depth % ChunkType::kHeight == 0) { - const auto* child_chunk = chunk_->getChild(child_level_traversal_distance); + const auto* child_chunk = chunk_.getChild(child_level_traversal_distance); return {child_chunk, 0, 0u}; } else { - return {chunk_, child_depth, child_level_traversal_distance}; + return {&chunk_, child_depth, child_level_traversal_distance}; } } template template -ChunkedNdtreeNodePtr -ChunkedNdtreeNodePtr::getOrAllocateChild( +ChunkedNdtreeNodeRef +ChunkedNdtreeNodeRef::getOrAllocateChild( NdtreeIndexRelativeChild child_index, DefaultArgs&&... args) const { const IndexElement child_depth = relative_node_depth_ + 1; const LinearIndex child_level_traversal_distance = computeChildLevelTraversalDistance(child_index); if (child_depth % ChunkType::kHeight == 0) { - auto& child_chunk = chunk_->getOrAllocateChild( + auto& child_chunk = chunk_.getOrAllocateChild( child_level_traversal_distance, std::forward(args)...); return {child_chunk, 0, 0}; } else { @@ -88,14 +175,14 @@ ChunkedNdtreeNodePtr::getOrAllocateChild( } template -LinearIndex ChunkedNdtreeNodePtr::computeRelativeNodeIndex() const { +LinearIndex ChunkedNdtreeNodeRef::computeRelativeNodeIndex() const { const LinearIndex parent_to_first_child_distance = tree_math::perfect_tree::num_total_nodes_fast(relative_node_depth_); return parent_to_first_child_distance + level_traversal_distance_; } template -LinearIndex ChunkedNdtreeNodePtr::computeChildLevelTraversalDistance( +LinearIndex ChunkedNdtreeNodeRef::computeChildLevelTraversalDistance( NdtreeIndexRelativeChild child_index) const { DCHECK_GE(child_index, 0); DCHECK_LT(child_index, 1 << kDim); diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h index d698eaf24..2f0d595de 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h @@ -60,14 +60,17 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { const HashedChunkedWaveletOctree::BlockIndex& block_index); void updateNodeRecursive( - HashedChunkedWaveletOctreeBlock::NodeChunkType& parent_chunk, + HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::ChunkType& + parent_chunk, const OctreeIndex& parent_node_index, LinearIndex parent_in_chunk_index, FloatingPoint& parent_value, - HashedChunkedWaveletOctreeBlock::NodeChunkType::BitRef parent_has_child, + HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::ChunkType::BitRef + parent_has_child, bool& block_needs_thresholding); void updateLeavesBatch( const OctreeIndex& parent_index, FloatingPoint& parent_value, - HashedChunkedWaveletOctreeBlock::NodeChunkType::DataType& parent_details); + HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::ChunkType::DataType& + parent_details); }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h index 2be7724df..22a8aa645 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h +++ b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h @@ -18,9 +18,6 @@ class HashedChunkedWaveletOctreeBlock { using Coefficients = HaarCoefficients; using Transform = HaarTransform; using ChunkedOctreeType = ChunkedOctree; - using NodeChunkType = ChunkedOctreeType::ChunkType; - using NodePtrType = ChunkedOctreeType::NodePtrType; - using NodeConstPtrType = ChunkedOctreeType::NodeConstPtrType; explicit HashedChunkedWaveletOctreeBlock(IndexElement tree_height, FloatingPoint min_log_odds, @@ -48,10 +45,16 @@ class HashedChunkedWaveletOctreeBlock { const Coefficients::Scale& getRootScale() const { return root_scale_coefficient_; } - NodePtrType getRootNode() { return chunked_ndtree_.getRootNode(); } - NodeConstPtrType getRootNode() const { return chunked_ndtree_.getRootNode(); } - NodeChunkType& getRootChunk() { return chunked_ndtree_.getRootChunk(); } - const NodeChunkType& getRootChunk() const { + ChunkedOctreeType::NodeRefType getRootNode() { + return chunked_ndtree_.getRootNode(); + } + ChunkedOctreeType::NodeConstRefType getRootNode() const { + return chunked_ndtree_.getRootNode(); + } + ChunkedOctreeType::ChunkType& getRootChunk() { + return chunked_ndtree_.getRootChunk(); + } + const ChunkedOctreeType::ChunkType& getRootChunk() const { return chunked_ndtree_.getRootChunk(); } @@ -99,8 +102,9 @@ class HashedChunkedWaveletOctreeBlock { bool is_nonzero_child; }; RecursiveThresholdReturnValue recursiveThreshold( - NodeChunkType& chunk, Coefficients::Scale scale_coefficient); - void recursivePrune(NodeChunkType& chunk); + ChunkedOctreeType::ChunkType& chunk, + Coefficients::Scale scale_coefficient); + void recursivePrune(ChunkedOctreeType::ChunkType& chunk); }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_block_inl.h b/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_block_inl.h index 70212dfab..14b654650 100644 --- a/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_block_inl.h +++ b/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_block_inl.h @@ -22,7 +22,8 @@ inline FloatingPoint HashedChunkedWaveletOctreeBlock::getCellValue( const OctreeIndex& index) const { // Descend the tree chunk by chunk const MortonIndex morton_code = convert::nodeIndexToMorton(index); - const NodeChunkType* current_chunk = &chunked_ndtree_.getRootChunk(); + const ChunkedOctreeType::ChunkType* current_chunk = + &chunked_ndtree_.getRootChunk(); FloatingPoint value = root_scale_coefficient_; for (int chunk_top_height = tree_height_; index.height < chunk_top_height; chunk_top_height -= kChunkHeight) { diff --git a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc index 8979d746e..c661a1436 100644 --- a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc +++ b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc @@ -82,10 +82,11 @@ void HashedChunkedWaveletIntegrator::updateBlock( } void HashedChunkedWaveletIntegrator::updateNodeRecursive( // NOLINT - HashedChunkedWaveletOctreeBlock::NodeChunkType& parent_chunk, + HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::ChunkType& parent_chunk, const OctreeIndex& parent_node_index, LinearIndex parent_in_chunk_index, FloatingPoint& parent_value, - HashedChunkedWaveletOctreeBlock::NodeChunkType::BitRef parent_has_child, + HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::ChunkType::BitRef + parent_has_child, bool& block_needs_thresholding) { auto& parent_details = parent_chunk.nodeData(parent_in_chunk_index); auto child_values = HashedChunkedWaveletOctreeBlock::Transform::backward( @@ -146,7 +147,8 @@ void HashedChunkedWaveletIntegrator::updateNodeRecursive( // NOLINT const int parent_chunk_top_height = chunk_height_ * int_math::div_round_up(parent_height, chunk_height_); - HashedChunkedWaveletOctreeBlock::NodeChunkType* chunk_containing_child; + HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::ChunkType* + chunk_containing_child; LinearIndex child_node_in_chunk_index; if (child_height % chunk_height_ != 0) { chunk_containing_child = &parent_chunk; diff --git a/libraries/wavemap/src/map/hashed_chunked_wavelet_octree_block.cc b/libraries/wavemap/src/map/hashed_chunked_wavelet_octree_block.cc index 79917c619..38b8af84f 100644 --- a/libraries/wavemap/src/map/hashed_chunked_wavelet_octree_block.cc +++ b/libraries/wavemap/src/map/hashed_chunked_wavelet_octree_block.cc @@ -30,14 +30,14 @@ void HashedChunkedWaveletOctreeBlock::setCellValue(const OctreeIndex& index, // Descend the tree chunk by chunk while decompressing, and caching chunk ptrs const MortonIndex morton_code = convert::nodeIndexToMorton(index); - std::array chunk_ptrs{}; + std::array chunk_ptrs{}; chunk_ptrs[0] = &chunked_ndtree_.getRootChunk(); FloatingPoint current_value = root_scale_coefficient_; for (int chunk_top_height = tree_height_; index.height < chunk_top_height; chunk_top_height -= kChunkHeight) { // Get the current chunk const int chunk_depth = (tree_height_ - chunk_top_height) / kChunkHeight; - NodeChunkType* const current_chunk = chunk_ptrs[chunk_depth]; + ChunkedOctreeType::ChunkType* const current_chunk = chunk_ptrs[chunk_depth]; // Decompress level by level for (int parent_height = chunk_top_height; chunk_top_height - kChunkHeight < parent_height; --parent_height) { @@ -76,7 +76,7 @@ void HashedChunkedWaveletOctreeBlock::setCellValue(const OctreeIndex& index, ++parent_height) { // Get the current chunk const int chunk_depth = (tree_height_ - parent_height) / kChunkHeight; - NodeChunkType* current_chunk = chunk_ptrs[chunk_depth]; + ChunkedOctreeType::ChunkType* current_chunk = chunk_ptrs[chunk_depth]; // Get the index of the data w.r.t. the chunk const int chunk_top_height = tree_height_ - chunk_depth * kChunkHeight; const LinearIndex relative_node_index = @@ -101,7 +101,7 @@ void HashedChunkedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index, setLastUpdatedStamp(); const MortonIndex morton_code = convert::nodeIndexToMorton(index); - std::array chunk_ptrs{}; + std::array chunk_ptrs{}; chunk_ptrs[0] = &chunked_ndtree_.getRootChunk(); const int last_chunk_depth = (tree_height_ - index.height - 1) / kChunkHeight; for (int chunk_depth = 1; chunk_depth <= last_chunk_depth; ++chunk_depth) { @@ -111,7 +111,7 @@ void HashedChunkedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index, const LinearIndex linear_child_index = OctreeIndex::computeLevelTraversalDistance( morton_code, parent_chunk_top_height, chunk_top_height); - NodeChunkType* current_chunk = chunk_ptrs[chunk_depth - 1]; + ChunkedOctreeType::ChunkType* current_chunk = chunk_ptrs[chunk_depth - 1]; if (current_chunk->hasChild(linear_child_index)) { chunk_ptrs[chunk_depth] = current_chunk->getChild(linear_child_index); } else { @@ -125,7 +125,7 @@ void HashedChunkedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index, ++parent_height) { // Get the current chunk const int chunk_depth = (tree_height_ - parent_height) / kChunkHeight; - NodeChunkType* current_chunk = chunk_ptrs[chunk_depth]; + ChunkedOctreeType::ChunkType* current_chunk = chunk_ptrs[chunk_depth]; // Get the index of the data w.r.t. the chunk const int chunk_top_height = tree_height_ - chunk_depth * kChunkHeight; const LinearIndex relative_node_index = @@ -154,7 +154,7 @@ void HashedChunkedWaveletOctreeBlock::forEachLeaf( struct StackElement { const OctreeIndex node_index; - ChunkedNdtreeNodePtr node; + ChunkedOctreeType::NodeConstRefType node; const Coefficients::Scale scale_coefficient{}; }; std::stack stack; @@ -174,9 +174,9 @@ void HashedChunkedWaveletOctreeBlock::forEachLeaf( const OctreeIndex child_node_index = index.computeChildIndex(child_idx); const FloatingPoint child_scale_coefficient = child_scale_coefficients[child_idx]; - auto child_node = node.getChild(child_idx); - if (child_node && termination_height < child_node_index.height) { - stack.emplace(StackElement{child_node_index, child_node, + if (auto child_node = node.getChild(child_idx); + child_node && termination_height < child_node_index.height) { + stack.emplace(StackElement{child_node_index, *child_node, child_scale_coefficient}); } else { visitor_fn(child_node_index, child_scale_coefficient); @@ -187,7 +187,7 @@ void HashedChunkedWaveletOctreeBlock::forEachLeaf( HashedChunkedWaveletOctreeBlock::RecursiveThresholdReturnValue HashedChunkedWaveletOctreeBlock::recursiveThreshold( // NOLINT - HashedChunkedWaveletOctreeBlock::NodeChunkType& chunk, + HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::ChunkType& chunk, FloatingPoint scale_coefficient) { constexpr auto tree_size = [](auto tree_height) { return static_cast( @@ -221,11 +221,11 @@ HashedChunkedWaveletOctreeBlock::recursiveThreshold( // NOLINT // Threshold const int first_leaf_idx = tree_size(kChunkHeight); - for (LinearIndex child_idx = 0; child_idx < NodeChunkType::kNumChildren; - ++child_idx) { + for (LinearIndex child_idx = 0; + child_idx < ChunkedOctreeType::ChunkType::kNumChildren; ++child_idx) { const LinearIndex array_idx = first_leaf_idx + child_idx; if (chunk.hasChild(child_idx)) { - NodeChunkType& child_chunk = *chunk.getChild(child_idx); + ChunkedOctreeType::ChunkType& child_chunk = *chunk.getChild(child_idx); const auto rv = recursiveThreshold(child_chunk, chunk_scale_coefficients[array_idx]); chunk_scale_coefficients[array_idx] = rv.scale; @@ -268,13 +268,15 @@ HashedChunkedWaveletOctreeBlock::recursiveThreshold( // NOLINT } void HashedChunkedWaveletOctreeBlock::recursivePrune( // NOLINT - HashedChunkedWaveletOctreeBlock::NodeChunkType& chunk) { + HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::ChunkType& chunk) { constexpr FloatingPoint kNonzeroCoefficientThreshold = 1e-3f; bool has_at_least_one_child = false; for (LinearIndex linear_child_idx = 0; - linear_child_idx < NodeChunkType::kNumChildren; ++linear_child_idx) { + linear_child_idx < ChunkedOctreeType::ChunkType::kNumChildren; + ++linear_child_idx) { if (chunk.hasChild(linear_child_idx)) { - NodeChunkType& child_chunk = *chunk.getChild(linear_child_idx); + ChunkedOctreeType::ChunkType& child_chunk = + *chunk.getChild(linear_child_idx); recursivePrune(child_chunk); if (!child_chunk.hasChildrenArray() && !child_chunk.hasNonzeroData(kNonzeroCoefficientThreshold)) { diff --git a/libraries/wavemap/test/src/data_structure/test_ndtree.cc b/libraries/wavemap/test/src/data_structure/test_ndtree.cc index 3668c82f5..adaecaecf 100644 --- a/libraries/wavemap/test/src/data_structure/test_ndtree.cc +++ b/libraries/wavemap/test/src/data_structure/test_ndtree.cc @@ -99,12 +99,7 @@ TYPED_TEST(NdtreeTest, GettingAndSetting) { auto node = ndtree.getNode(index); ASSERT_TRUE(node) << "At index " << index.toString(); if (node) { - typename TypeParam::NodeDataType data; - if constexpr (TypeParam::kChunkHeight == 1) { - data = node->data(); - } else { - data = node.data(); - } + auto data = node->data(); EXPECT_EQ(data, value) << "At index " << index.toString(); } } @@ -117,12 +112,7 @@ TYPED_TEST(NdtreeTest, GettingAndSetting) { auto node = ndtree_cref.getNode(index); ASSERT_TRUE(node) << "At index " << index.toString(); if (node) { - typename TypeParam::NodeDataType data; - if constexpr (TypeParam::kChunkHeight == 1) { - data = node->data(); - } else { - data = node.data(); - } + auto data = node->data(); EXPECT_EQ(data, value) << "At index " << index.toString(); } } diff --git a/libraries/wavemap_io/src/stream_conversions.cc b/libraries/wavemap_io/src/stream_conversions.cc index e6344f4ac..c1509705b 100644 --- a/libraries/wavemap_io/src/stream_conversions.cc +++ b/libraries/wavemap_io/src/stream_conversions.cc @@ -361,7 +361,7 @@ void mapToStream(const HashedChunkedWaveletOctree& map, std::ostream& ostream) { // Define convenience types and constants struct StackElement { const FloatingPoint scale; - HashedChunkedWaveletOctreeBlock::NodeConstPtrType node; + HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::NodeConstRefType node; }; constexpr FloatingPoint kNumericalNoise = 1e-3f; const auto min_log_odds = map.getMinLogOdds() + kNumericalNoise; @@ -419,9 +419,8 @@ void mapToStream(const HashedChunkedWaveletOctree& map, std::ostream& ostream) { } // Otherwise, indicate that the child will be serialized // and add it to the stack - auto child = node.getChild(relative_child_idx); - if (child) { - stack.emplace(StackElement{child_scale, child}); + if (auto child = node.getChild(relative_child_idx); child) { + stack.emplace(StackElement{child_scale, *child}); streamable_node.allocated_children_bitset += (1 << relative_child_idx); } diff --git a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc index ffe9d4d15..c9ed9f74c 100644 --- a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc +++ b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc @@ -530,7 +530,7 @@ void blockToRosMsg(const HashedChunkedWaveletOctree::BlockIndex& block_index, // Define convenience types and constants struct StackElement { const FloatingPoint scale; - HashedChunkedWaveletOctreeBlock::NodeConstPtrType node; + HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::NodeConstRefType node; }; // Serialize the block's metadata @@ -568,9 +568,8 @@ void blockToRosMsg(const HashedChunkedWaveletOctree::BlockIndex& block_index, } // Otherwise, indicate that the child will be serialized // and add it to the stack - auto child = node.getChild(relative_child_idx); - if (child) { - stack.emplace(StackElement{child_scale, child}); + if (auto child = node.getChild(relative_child_idx); child) { + stack.emplace(StackElement{child_scale, *child}); node_msg.allocated_children_bitset += (1 << relative_child_idx); } } From 3dbec0362b13941d5e786ffe11e1b70dc83e8b64 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 29 Dec 2023 12:26:50 +0100 Subject: [PATCH 051/159] Rename VolumetricDataStructure to Map --- examples/src/io/load_map_from_file.cc | 2 +- examples/src/io/receive_map_over_ros.cc | 2 +- examples/src/planning/occupancy_to_esdf.cc | 4 +- examples/src/queries/fixed_resolution.cc | 4 +- .../queries/nearest_neighbor_interpolation.cc | 4 +- .../src/queries/trilinear_interpolation.cc | 4 +- libraries/wavemap/CMakeLists.txt | 4 +- .../chunked_ndtree/chunked_ndtree.h | 2 +- ...de_ptr.h => chunked_ndtree_node_address.h} | 8 ++-- ...nl.h => chunked_ndtree_node_address_inl.h} | 6 +-- .../wavemap/integrator/integrator_factory.h | 7 ++-- .../hashed_chunked_wavelet_integrator.h | 2 +- .../hashed_wavelet_integrator.h | 2 +- .../coarse_to_fine/wavelet_integrator.h | 2 +- .../fixed_resolution_integrator.h | 4 +- .../projective/projective_integrator.h | 2 +- .../ray_tracing/ray_tracing_integrator.h | 2 +- .../include/wavemap/map/hashed_blocks.h | 21 +++++------ .../map/hashed_chunked_wavelet_octree.h | 11 +++--- .../map/hashed_chunked_wavelet_octree_block.h | 10 ++--- .../wavemap/map/hashed_wavelet_octree.h | 11 +++--- .../wavemap/map/hashed_wavelet_octree_block.h | 10 ++--- ...etric_data_structure_base.h => map_base.h} | 35 ++++++++---------- .../wavemap/include/wavemap/map/map_factory.h | 19 ++++++++++ .../map/volumetric_data_structure_factory.h | 22 ----------- .../include/wavemap/map/volumetric_octree.h | 11 +++--- .../include/wavemap/map/wavelet_octree.h | 11 +++--- .../wavemap/utils/query/map_interpolator.h | 4 +- .../utils/sdf/full_euclidean_sdf_generator.h | 3 +- .../src/integrator/integrator_factory.cc | 24 +++++------- libraries/wavemap/src/map/hashed_blocks.cc | 2 +- .../src/map/hashed_chunked_wavelet_octree.cc | 2 +- .../hashed_chunked_wavelet_octree_block.cc | 2 +- .../wavemap/src/map/hashed_wavelet_octree.cc | 2 +- .../src/map/hashed_wavelet_octree_block.cc | 2 +- ...ric_data_structure_base.cc => map_base.cc} | 4 +- ...ta_structure_factory.cc => map_factory.cc} | 37 ++++++++----------- .../wavemap/src/map/volumetric_octree.cc | 2 +- libraries/wavemap/src/map/wavelet_octree.cc | 2 +- .../utils/sdf/full_euclidean_sdf_generator.cc | 3 +- .../sdf/quasi_euclidean_sdf_generator.cc | 3 +- .../include/wavemap/test/config_generator.h | 8 ++-- .../src/data_structure/test_hashed_blocks.cc | 3 +- .../test_volumetric_data_structure.cc | 14 +++---- .../integrator/test_pointcloud_integrators.cc | 9 ++--- .../test/src/utils/test_map_interpolator.cpp | 2 +- .../include/wavemap_io/file_conversions.h | 8 ++-- .../include/wavemap_io/stream_conversions.h | 4 +- libraries/wavemap_io/src/file_conversions.cc | 6 +-- .../wavemap_io/src/stream_conversions.cc | 7 ++-- .../test/src/test_file_conversions.cc | 8 ++-- .../input_handler/depth_image_input_handler.h | 2 +- .../wavemap_ros/input_handler/input_handler.h | 2 +- .../input_handler/input_handler_factory.h | 5 +-- .../input_handler/pointcloud_input_handler.h | 2 +- .../include/wavemap_ros/wavemap_server.h | 10 ++--- .../depth_image_input_handler.cc | 2 +- .../src/input_handler/input_handler.cc | 2 +- .../input_handler/input_handler_factory.cc | 5 +-- .../input_handler/pointcloud_input_handler.cc | 2 +- ros/wavemap_ros/src/wavemap_server.cc | 6 +-- .../map_msg_conversions.h | 8 ++-- .../src/map_msg_conversions.cc | 10 ++--- .../test/src/test_map_msg_conversions.cc | 8 ++-- .../include/wavemap_rviz_plugin/common.h | 4 +- .../visuals/cell_selector.h | 2 +- .../visuals/slice_visual.h | 2 +- .../visuals/voxel_visual.h | 2 +- .../src/visuals/cell_selector.cc | 2 +- .../src/visuals/slice_visual.cc | 2 +- .../src/visuals/voxel_visual.cc | 6 +-- 71 files changed, 208 insertions(+), 259 deletions(-) rename libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/{chunked_ndtree_node_ptr.h => chunked_ndtree_node_address.h} (98%) rename libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/{chunked_ndtree_node_ptr_inl.h => chunked_ndtree_node_address_inl.h} (98%) rename libraries/wavemap/include/wavemap/map/{volumetric_data_structure_base.h => map_base.h} (72%) create mode 100644 libraries/wavemap/include/wavemap/map/map_factory.h delete mode 100644 libraries/wavemap/include/wavemap/map/volumetric_data_structure_factory.h rename libraries/wavemap/src/map/{volumetric_data_structure_base.cc => map_base.cc} (77%) rename libraries/wavemap/src/map/{volumetric_data_structure_factory.cc => map_factory.cc} (63%) diff --git a/examples/src/io/load_map_from_file.cc b/examples/src/io/load_map_from_file.cc index dfb9389b7..c3c8ca06f 100644 --- a/examples/src/io/load_map_from_file.cc +++ b/examples/src/io/load_map_from_file.cc @@ -2,7 +2,7 @@ int main(int, char**) { // Create a smart pointer that will own the loaded map - wavemap::VolumetricDataStructureBase::Ptr loaded_map; + wavemap::MapBase::Ptr loaded_map; // Load the map wavemap::io::fileToMap("/some/path/to/your/map.wvmp", loaded_map); diff --git a/examples/src/io/receive_map_over_ros.cc b/examples/src/io/receive_map_over_ros.cc index a5ea7729e..4fa65420f 100644 --- a/examples/src/io/receive_map_over_ros.cc +++ b/examples/src/io/receive_map_over_ros.cc @@ -2,7 +2,7 @@ #include // Create a smart pointer that will own the received map -wavemap::VolumetricDataStructureBase::Ptr loaded_map; +wavemap::MapBase::Ptr loaded_map; // Define the map callback void mapCallback(const wavemap_msgs::Map& msg) { diff --git a/examples/src/planning/occupancy_to_esdf.cc b/examples/src/planning/occupancy_to_esdf.cc index 93f57c95e..502fa8bd0 100644 --- a/examples/src/planning/occupancy_to_esdf.cc +++ b/examples/src/planning/occupancy_to_esdf.cc @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include #include @@ -25,7 +25,7 @@ int main(int argc, char** argv) { const std::filesystem::path map_file_path = argv[1]; // Load the occupancy map - VolumetricDataStructureBase::Ptr occupancy_map; + MapBase::Ptr occupancy_map; io::fileToMap(map_file_path, occupancy_map); CHECK_NOTNULL(occupancy_map); diff --git a/examples/src/queries/fixed_resolution.cc b/examples/src/queries/fixed_resolution.cc index 7d55444e3..0e1aa6297 100644 --- a/examples/src/queries/fixed_resolution.cc +++ b/examples/src/queries/fixed_resolution.cc @@ -1,4 +1,4 @@ -#include +#include #include "wavemap_examples/common.h" @@ -7,7 +7,7 @@ int main(int, char**) { // Declare a map pointer for illustration purposes // NOTE: See the other tutorials on how to load maps from files or ROS topics, // such as the map topic published by the wavemap ROS server. - VolumetricDataStructureBase::Ptr map; + MapBase::Ptr map; // Declare the index to query // NOTE: See wavemap/indexing/index_conversions.h for helper methods diff --git a/examples/src/queries/nearest_neighbor_interpolation.cc b/examples/src/queries/nearest_neighbor_interpolation.cc index defcc7c56..471db57f4 100644 --- a/examples/src/queries/nearest_neighbor_interpolation.cc +++ b/examples/src/queries/nearest_neighbor_interpolation.cc @@ -1,4 +1,4 @@ -#include +#include #include #include "wavemap_examples/common.h" @@ -8,7 +8,7 @@ int main(int, char**) { // Create an empty map for illustration purposes // NOTE: See the other tutorials on how to load maps from files or ROS topics, // such as the map topic published by the wavemap ROS server. - VolumetricDataStructureBase::Ptr map; + MapBase::Ptr map; // Declare the point to query [in map frame] const Point3D query_point = Point3D::Zero(); diff --git a/examples/src/queries/trilinear_interpolation.cc b/examples/src/queries/trilinear_interpolation.cc index 78061d4ad..d52f22aeb 100644 --- a/examples/src/queries/trilinear_interpolation.cc +++ b/examples/src/queries/trilinear_interpolation.cc @@ -1,4 +1,4 @@ -#include +#include #include #include "wavemap_examples/common.h" @@ -8,7 +8,7 @@ int main(int, char**) { // Create an empty map for illustration purposes // NOTE: See the other tutorials on how to load maps from files or ROS topics, // such as the map topic published by the wavemap ROS server. - VolumetricDataStructureBase::Ptr map; + MapBase::Ptr map; // Declare the point to query [in map frame] const Point3D query_point = Point3D::Zero(); diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index 72c192efb..c03892b9b 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -64,8 +64,8 @@ add_library(${PROJECT_NAME} src/map/hashed_wavelet_octree_block.cc src/map/volumetric_octree.cc src/map/wavelet_octree.cc - src/map/volumetric_data_structure_base.cc - src/map/volumetric_data_structure_factory.cc + src/map/map_base.cc + src/map/map_factory.cc src/utils/query/query_accelerator.cc src/utils/sdf/full_euclidean_sdf_generator.cc src/utils/sdf/quasi_euclidean_sdf_generator.cc diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h index 106792a9f..243288b41 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h @@ -7,7 +7,7 @@ #include "wavemap/common.h" #include "wavemap/data_structure/chunked_ndtree/chunked_ndtree_chunk.h" -#include "wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_ptr.h" +#include "wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_address.h" #include "wavemap/indexing/ndtree_index.h" #include "wavemap/utils/iterate/subtree_iterator.h" diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_ptr.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_address.h similarity index 98% rename from libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_ptr.h rename to libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_address.h index f4e503539..b281149d4 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_ptr.h +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_address.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_NODE_PTR_H_ -#define WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_NODE_PTR_H_ +#ifndef WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_NODE_ADDRESS_H_ +#define WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_NODE_ADDRESS_H_ #include @@ -105,6 +105,6 @@ class ChunkedNdtreeNodeRef { }; } // namespace wavemap -#include "wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_ptr_inl.h" +#include "wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h" -#endif // WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_NODE_PTR_H_ +#endif // WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_NODE_ADDRESS_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_ptr_inl.h b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h similarity index 98% rename from libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_ptr_inl.h rename to libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h index b830754bb..d14efeb72 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_ptr_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_NODE_PTR_INL_H_ -#define WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_NODE_PTR_INL_H_ +#ifndef WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_NODE_ADDRESS_INL_H_ +#define WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_NODE_ADDRESS_INL_H_ #include @@ -190,4 +190,4 @@ LinearIndex ChunkedNdtreeNodeRef::computeChildLevelTraversalDistance( } } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_NODE_PTR_INL_H_ +#endif // WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_NODE_ADDRESS_INL_H_ diff --git a/libraries/wavemap/include/wavemap/integrator/integrator_factory.h b/libraries/wavemap/include/wavemap/integrator/integrator_factory.h index f20fd1f48..884560dd5 100644 --- a/libraries/wavemap/include/wavemap/integrator/integrator_factory.h +++ b/libraries/wavemap/include/wavemap/integrator/integrator_factory.h @@ -5,21 +5,20 @@ #include #include "wavemap/integrator/integrator_base.h" -#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/map_base.h" #include "wavemap/utils/thread_pool.h" namespace wavemap { class IntegratorFactory { public: static IntegratorBase::Ptr create( - const param::Value& params, - VolumetricDataStructureBase::Ptr occupancy_map, + const param::Value& params, MapBase::Ptr occupancy_map, std::shared_ptr thread_pool = nullptr, std::optional default_integrator_type = std::nullopt); static IntegratorBase::Ptr create( IntegratorType integrator_type, const param::Value& params, - VolumetricDataStructureBase::Ptr occupancy_map, + MapBase::Ptr occupancy_map, std::shared_ptr thread_pool = nullptr); }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h index 2f0d595de..320875151 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h @@ -8,7 +8,7 @@ #include "wavemap/integrator/projective/coarse_to_fine/range_image_intersector.h" #include "wavemap/integrator/projective/projective_integrator.h" #include "wavemap/map/hashed_chunked_wavelet_octree.h" -#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/map_base.h" #include "wavemap/utils/thread_pool.h" namespace wavemap { diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h index 091cd1592..e5f39d16c 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h @@ -8,7 +8,7 @@ #include "wavemap/integrator/projective/coarse_to_fine/range_image_intersector.h" #include "wavemap/integrator/projective/projective_integrator.h" #include "wavemap/map/hashed_wavelet_octree.h" -#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/map_base.h" #include "wavemap/utils/thread_pool.h" namespace wavemap { diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/wavelet_integrator.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/wavelet_integrator.h index d609e1556..e1f32ef4c 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/wavelet_integrator.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/wavelet_integrator.h @@ -6,7 +6,7 @@ #include "wavemap/integrator/projective/coarse_to_fine/range_image_intersector.h" #include "wavemap/integrator/projective/projective_integrator.h" -#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/map_base.h" #include "wavemap/map/wavelet_octree.h" namespace wavemap { diff --git a/libraries/wavemap/include/wavemap/integrator/projective/fixed_resolution/fixed_resolution_integrator.h b/libraries/wavemap/include/wavemap/integrator/projective/fixed_resolution/fixed_resolution_integrator.h index f0555c839..6f329244d 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/fixed_resolution/fixed_resolution_integrator.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/fixed_resolution/fixed_resolution_integrator.h @@ -16,14 +16,14 @@ class FixedResolutionIntegrator : public ProjectiveIntegrator { PosedImage<>::Ptr posed_range_image, Image::Ptr beam_offset_image, MeasurementModelBase::ConstPtr measurement_model, - VolumetricDataStructureBase::Ptr occupancy_map) + MapBase::Ptr occupancy_map) : ProjectiveIntegrator( config, std::move(projection_model), std::move(posed_range_image), std::move(beam_offset_image), std::move(measurement_model)), occupancy_map_(std::move(CHECK_NOTNULL(occupancy_map))) {} private: - const VolumetricDataStructureBase::Ptr occupancy_map_; + const MapBase::Ptr occupancy_map_; AABB aabb_; void importPointcloud(const PosedPointcloud<>& pointcloud) override; diff --git a/libraries/wavemap/include/wavemap/integrator/projective/projective_integrator.h b/libraries/wavemap/include/wavemap/integrator/projective/projective_integrator.h index 583aeee83..5c50b926f 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/projective_integrator.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/projective_integrator.h @@ -9,7 +9,7 @@ #include "wavemap/integrator/integrator_base.h" #include "wavemap/integrator/measurement_model/measurement_model_base.h" #include "wavemap/integrator/projection_model/projector_base.h" -#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/map_base.h" namespace wavemap { /** diff --git a/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h b/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h index 567b5fcf7..b41468cd0 100644 --- a/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h +++ b/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h @@ -5,7 +5,7 @@ #include "wavemap/integrator/integrator_base.h" #include "wavemap/integrator/measurement_model/constant_ray.h" -#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/map_base.h" #include "wavemap/utils/iterate/ray_iterator.h" namespace wavemap { diff --git a/libraries/wavemap/include/wavemap/map/hashed_blocks.h b/libraries/wavemap/include/wavemap/map/hashed_blocks.h index 7dc17dc58..dc4fbb03d 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_blocks.h +++ b/libraries/wavemap/include/wavemap/map/hashed_blocks.h @@ -8,24 +8,22 @@ #include "wavemap/common.h" #include "wavemap/config/config_base.h" #include "wavemap/data_structure/dense_block_hash.h" -#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/map_base.h" namespace wavemap { -class HashedBlocks - : public VolumetricDataStructureBase, - public DenseBlockHash { +class HashedBlocks : public MapBase, + public DenseBlockHash { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - using Config = VolumetricDataStructureConfig; + using Config = MapBaseConfig; static constexpr bool kRequiresExplicitThresholding = false; - using VolumetricDataStructureBase::kDim; + using MapBase::kDim; - explicit HashedBlocks(const VolumetricDataStructureConfig& config, + explicit HashedBlocks(const MapBaseConfig& config, FloatingPoint default_value = 0.f) - : VolumetricDataStructureBase(config.checkValid()), + : MapBase(config.checkValid()), DenseBlockHash(config.min_cell_width, default_value) {} bool empty() const override { return DenseBlockHash::empty(); } @@ -45,7 +43,7 @@ class HashedBlocks Index3D getMinBlockIndex() const { return block_map_.getMinBlockIndex(); } Index3D getMaxBlockIndex() const { return block_map_.getMaxBlockIndex(); } IndexElement getTreeHeight() const override { return 0; } - const VolumetricDataStructureConfig& getConfig() { return config_; } + const MapBaseConfig& getConfig() { return config_; } FloatingPoint getCellValue(const Index3D& index) const override; void setCellValue(const Index3D& index, FloatingPoint new_value) override; @@ -55,8 +53,7 @@ class HashedBlocks const auto& getHashMap() const { return block_map_; } void forEachLeaf( - typename VolumetricDataStructureBase::IndexedLeafVisitorFunction - visitor_fn) const override; + typename MapBase::IndexedLeafVisitorFunction visitor_fn) const override; }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree.h b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree.h index 2eeb70aab..590be6bbb 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree.h +++ b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree.h @@ -9,7 +9,7 @@ #include "wavemap/data_structure/spatial_hash.h" #include "wavemap/indexing/index_hashes.h" #include "wavemap/map/hashed_chunked_wavelet_octree_block.h" -#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/map_base.h" #include "wavemap/utils/math/int_math.h" namespace wavemap { @@ -53,14 +53,14 @@ struct HashedChunkedWaveletOctreeConfig only_prune_blocks_if_unused_for(only_prune_blocks_if_unused_for) {} // Conversion to DataStructureBase config - operator VolumetricDataStructureConfig() const { // NOLINT + operator MapBaseConfig() const { // NOLINT return {min_cell_width, min_log_odds, max_log_odds}; } bool isValid(bool verbose) const override; }; -class HashedChunkedWaveletOctree : public VolumetricDataStructureBase { +class HashedChunkedWaveletOctree : public MapBase { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; @@ -74,7 +74,7 @@ class HashedChunkedWaveletOctree : public VolumetricDataStructureBase { explicit HashedChunkedWaveletOctree( const HashedChunkedWaveletOctreeConfig& config) - : VolumetricDataStructureBase(config), config_(config.checkValid()) {} + : MapBase(config), config_(config.checkValid()) {} bool empty() const override { return block_map_.empty(); } size_t size() const override; @@ -119,8 +119,7 @@ class HashedChunkedWaveletOctree : public VolumetricDataStructureBase { } void forEachLeaf( - typename VolumetricDataStructureBase::IndexedLeafVisitorFunction - visitor_fn) const override; + typename MapBase::IndexedLeafVisitorFunction visitor_fn) const override; private: const HashedChunkedWaveletOctreeConfig config_; diff --git a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h index 22a8aa645..fe8fd3b49 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h +++ b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h @@ -5,7 +5,7 @@ #include "wavemap/data_structure/chunked_ndtree/chunked_ndtree.h" #include "wavemap/map/cell_types/haar_coefficients.h" #include "wavemap/map/cell_types/haar_transform.h" -#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/map_base.h" #include "wavemap/utils/time/time.h" namespace wavemap { @@ -35,11 +35,9 @@ class HashedChunkedWaveletOctreeBlock { void setCellValue(const OctreeIndex& index, FloatingPoint new_value); void addToCellValue(const OctreeIndex& index, FloatingPoint update); - void forEachLeaf( - const BlockIndex& block_index, - typename VolumetricDataStructureBase::IndexedLeafVisitorFunction - visitor_fn, - IndexElement termination_height = 0) const; + void forEachLeaf(const BlockIndex& block_index, + typename MapBase::IndexedLeafVisitorFunction visitor_fn, + IndexElement termination_height = 0) const; Coefficients::Scale& getRootScale() { return root_scale_coefficient_; } const Coefficients::Scale& getRootScale() const { diff --git a/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h b/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h index b46bcc516..722958d06 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h +++ b/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h @@ -9,7 +9,7 @@ #include "wavemap/data_structure/spatial_hash.h" #include "wavemap/indexing/index_hashes.h" #include "wavemap/map/hashed_wavelet_octree_block.h" -#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/map_base.h" #include "wavemap/utils/math/int_math.h" namespace wavemap { @@ -49,14 +49,14 @@ struct HashedWaveletOctreeConfig : ConfigBase { only_prune_blocks_if_unused_for(only_prune_blocks_if_unused_for) {} // Conversion to DataStructureBase config - operator VolumetricDataStructureConfig() const { // NOLINT + operator MapBaseConfig() const { // NOLINT return {min_cell_width, min_log_odds, max_log_odds}; } bool isValid(bool verbose) const override; }; -class HashedWaveletOctree : public VolumetricDataStructureBase { +class HashedWaveletOctree : public MapBase { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; @@ -69,7 +69,7 @@ class HashedWaveletOctree : public VolumetricDataStructureBase { using BlockHashMap = SpatialHash; explicit HashedWaveletOctree(const HashedWaveletOctreeConfig& config) - : VolumetricDataStructureBase(config), config_(config.checkValid()) {} + : MapBase(config), config_(config.checkValid()) {} bool empty() const override { return block_map_.empty(); } size_t size() const override; @@ -113,8 +113,7 @@ class HashedWaveletOctree : public VolumetricDataStructureBase { } void forEachLeaf( - typename VolumetricDataStructureBase::IndexedLeafVisitorFunction - visitor_fn) const override; + typename MapBase::IndexedLeafVisitorFunction visitor_fn) const override; private: const HashedWaveletOctreeConfig config_; diff --git a/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree_block.h b/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree_block.h index c3365dcd2..0f1a86f91 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree_block.h +++ b/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree_block.h @@ -5,7 +5,7 @@ #include "wavemap/data_structure/ndtree/ndtree.h" #include "wavemap/map/cell_types/haar_coefficients.h" #include "wavemap/map/cell_types/haar_transform.h" -#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/map_base.h" #include "wavemap/utils/time/time.h" namespace wavemap { @@ -35,11 +35,9 @@ class HashedWaveletOctreeBlock { void setCellValue(const OctreeIndex& index, FloatingPoint new_value); void addToCellValue(const OctreeIndex& index, FloatingPoint update); - void forEachLeaf( - const BlockIndex& block_index, - typename VolumetricDataStructureBase::IndexedLeafVisitorFunction - visitor_fn, - IndexElement termination_height = 0) const; + void forEachLeaf(const BlockIndex& block_index, + typename MapBase::IndexedLeafVisitorFunction visitor_fn, + IndexElement termination_height = 0) const; Coefficients::Scale& getRootScale() { return root_scale_coefficient_; } const Coefficients::Scale& getRootScale() const { diff --git a/libraries/wavemap/include/wavemap/map/volumetric_data_structure_base.h b/libraries/wavemap/include/wavemap/map/map_base.h similarity index 72% rename from libraries/wavemap/include/wavemap/map/volumetric_data_structure_base.h rename to libraries/wavemap/include/wavemap/map/map_base.h index 116eb5479..c8ff80adf 100644 --- a/libraries/wavemap/include/wavemap/map/volumetric_data_structure_base.h +++ b/libraries/wavemap/include/wavemap/map/map_base.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_VOLUMETRIC_DATA_STRUCTURE_BASE_H_ -#define WAVEMAP_MAP_VOLUMETRIC_DATA_STRUCTURE_BASE_H_ +#ifndef WAVEMAP_MAP_MAP_BASE_H_ +#define WAVEMAP_MAP_MAP_BASE_H_ #include #include @@ -11,8 +11,8 @@ #include "wavemap/indexing/ndtree_index.h" namespace wavemap { -struct VolumetricDataStructureType : TypeSelector { - using TypeSelector::TypeSelector; +struct MapType : TypeSelector { + using TypeSelector::TypeSelector; enum Id : TypeId { kHashedBlocks, @@ -28,10 +28,9 @@ struct VolumetricDataStructureType : TypeSelector { }; /** - * Base config struct for volumetric data structures. + * Base config struct for maps. */ -struct VolumetricDataStructureConfig - : ConfigBase { +struct MapBaseConfig : ConfigBase { //! Maximum resolution of the map, set as the width of the smallest cell that //! it can represent. Meters min_cell_width = 0.1f; @@ -44,10 +43,9 @@ struct VolumetricDataStructureConfig static MemberMap memberMap; // Constructors - VolumetricDataStructureConfig() = default; - VolumetricDataStructureConfig(FloatingPoint min_cell_width, - FloatingPoint min_log_odds, - FloatingPoint max_log_odds) + MapBaseConfig() = default; + MapBaseConfig(FloatingPoint min_cell_width, FloatingPoint min_log_odds, + FloatingPoint max_log_odds) : min_cell_width(min_cell_width), min_log_odds(min_log_odds), max_log_odds(max_log_odds) {} @@ -55,16 +53,15 @@ struct VolumetricDataStructureConfig bool isValid(bool verbose) const override; }; -class VolumetricDataStructureBase { +class MapBase { public: static constexpr int kDim = 3; - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; - explicit VolumetricDataStructureBase( - const VolumetricDataStructureConfig& config) + explicit MapBase(const MapBaseConfig& config) : config_(config.checkValid()) {} - virtual ~VolumetricDataStructureBase() = default; + virtual ~MapBase() = default; virtual bool empty() const = 0; virtual size_t size() const = 0; @@ -95,7 +92,7 @@ class VolumetricDataStructureBase { virtual void forEachLeaf(IndexedLeafVisitorFunction visitor_fn) const = 0; protected: - const VolumetricDataStructureConfig config_; + const MapBaseConfig config_; FloatingPoint clamp(FloatingPoint value) const { return std::clamp(value, config_.min_log_odds, config_.max_log_odds); @@ -106,4 +103,4 @@ class VolumetricDataStructureBase { }; } // namespace wavemap -#endif // WAVEMAP_MAP_VOLUMETRIC_DATA_STRUCTURE_BASE_H_ +#endif // WAVEMAP_MAP_MAP_BASE_H_ diff --git a/libraries/wavemap/include/wavemap/map/map_factory.h b/libraries/wavemap/include/wavemap/map/map_factory.h new file mode 100644 index 000000000..5e63638c0 --- /dev/null +++ b/libraries/wavemap/include/wavemap/map/map_factory.h @@ -0,0 +1,19 @@ +#ifndef WAVEMAP_MAP_MAP_FACTORY_H_ +#define WAVEMAP_MAP_MAP_FACTORY_H_ + +#include + +#include "wavemap/map/map_base.h" + +namespace wavemap { +class MapFactory { + public: + static MapBase::Ptr create( + const param::Value& params, + std::optional default_map_type = std::nullopt); + + static MapBase::Ptr create(MapType map_type, const param::Value& params); +}; +} // namespace wavemap + +#endif // WAVEMAP_MAP_MAP_FACTORY_H_ diff --git a/libraries/wavemap/include/wavemap/map/volumetric_data_structure_factory.h b/libraries/wavemap/include/wavemap/map/volumetric_data_structure_factory.h deleted file mode 100644 index 8c9c1dbe5..000000000 --- a/libraries/wavemap/include/wavemap/map/volumetric_data_structure_factory.h +++ /dev/null @@ -1,22 +0,0 @@ -#ifndef WAVEMAP_MAP_VOLUMETRIC_DATA_STRUCTURE_FACTORY_H_ -#define WAVEMAP_MAP_VOLUMETRIC_DATA_STRUCTURE_FACTORY_H_ - -#include - -#include "wavemap/map/volumetric_data_structure_base.h" - -namespace wavemap { -class VolumetricDataStructureFactory { - public: - static VolumetricDataStructureBase::Ptr create( - const param::Value& params, - std::optional default_data_structure_type = - std::nullopt); - - static VolumetricDataStructureBase::Ptr create( - VolumetricDataStructureType data_structure_type, - const param::Value& params); -}; -} // namespace wavemap - -#endif // WAVEMAP_MAP_VOLUMETRIC_DATA_STRUCTURE_FACTORY_H_ diff --git a/libraries/wavemap/include/wavemap/map/volumetric_octree.h b/libraries/wavemap/include/wavemap/map/volumetric_octree.h index a2d2d7e6f..24e69e1f4 100644 --- a/libraries/wavemap/include/wavemap/map/volumetric_octree.h +++ b/libraries/wavemap/include/wavemap/map/volumetric_octree.h @@ -8,7 +8,7 @@ #include "wavemap/data_structure/ndtree/ndtree.h" #include "wavemap/indexing/index_conversions.h" #include "wavemap/indexing/ndtree_index.h" -#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/map_base.h" namespace wavemap { /** @@ -35,14 +35,14 @@ struct VolumetricOctreeConfig : ConfigBase { tree_height(tree_height) {} // Conversion to DataStructureBase config - operator VolumetricDataStructureConfig() const { // NOLINT + operator MapBaseConfig() const { // NOLINT return {min_cell_width, min_log_odds, max_log_odds}; } bool isValid(bool verbose) const override; }; -class VolumetricOctree : public VolumetricDataStructureBase { +class VolumetricOctree : public MapBase { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; @@ -53,7 +53,7 @@ class VolumetricOctree : public VolumetricDataStructureBase { static constexpr bool kRequiresExplicitThresholding = true; explicit VolumetricOctree(const VolumetricOctreeConfig& config) - : VolumetricDataStructureBase(config), config_(config.checkValid()) {} + : MapBase(config), config_(config.checkValid()) {} bool empty() const override { return ndtree_.empty(); } size_t size() const override { return ndtree_.size(); } @@ -76,8 +76,7 @@ class VolumetricOctree : public VolumetricDataStructureBase { void addToCellValue(const OctreeIndex& index, FloatingPoint update); void forEachLeaf( - typename VolumetricDataStructureBase::IndexedLeafVisitorFunction - visitor_fn) const override; + typename MapBase::IndexedLeafVisitorFunction visitor_fn) const override; NodeType& getRootNode() { return ndtree_.getRootNode(); } const NodeType& getRootNode() const { return ndtree_.getRootNode(); } diff --git a/libraries/wavemap/include/wavemap/map/wavelet_octree.h b/libraries/wavemap/include/wavemap/map/wavelet_octree.h index e8b4f72fc..61e979d14 100644 --- a/libraries/wavemap/include/wavemap/map/wavelet_octree.h +++ b/libraries/wavemap/include/wavemap/map/wavelet_octree.h @@ -9,7 +9,7 @@ #include "wavemap/indexing/index_conversions.h" #include "wavemap/indexing/ndtree_index.h" #include "wavemap/map/cell_types/haar_transform.h" -#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/map_base.h" namespace wavemap { /** @@ -40,14 +40,14 @@ struct WaveletOctreeConfig : ConfigBase { tree_height(tree_height) {} // Conversion to DataStructureBase config - operator VolumetricDataStructureConfig() const { // NOLINT + operator MapBaseConfig() const { // NOLINT return {min_cell_width, min_log_odds, max_log_odds}; } bool isValid(bool verbose) const override; }; -class WaveletOctree : public VolumetricDataStructureBase { +class WaveletOctree : public MapBase { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; @@ -59,7 +59,7 @@ class WaveletOctree : public VolumetricDataStructureBase { static constexpr bool kRequiresExplicitThresholding = true; explicit WaveletOctree(const WaveletOctreeConfig& config) - : VolumetricDataStructureBase(config), config_(config.checkValid()) {} + : MapBase(config), config_(config.checkValid()) {} bool empty() const override; size_t size() const override { return ndtree_.size(); } @@ -84,8 +84,7 @@ class WaveletOctree : public VolumetricDataStructureBase { void addToCellValue(const OctreeIndex& index, FloatingPoint update); void forEachLeaf( - typename VolumetricDataStructureBase::IndexedLeafVisitorFunction - visitor_fn) const override; + typename MapBase::IndexedLeafVisitorFunction visitor_fn) const override; Coefficients::Scale& getRootScale() { return root_scale_coefficient_; } const Coefficients::Scale& getRootScale() const { diff --git a/libraries/wavemap/include/wavemap/utils/query/map_interpolator.h b/libraries/wavemap/include/wavemap/utils/query/map_interpolator.h index b9b0dbb91..3acb3cfd0 100644 --- a/libraries/wavemap/include/wavemap/utils/query/map_interpolator.h +++ b/libraries/wavemap/include/wavemap/utils/query/map_interpolator.h @@ -3,11 +3,11 @@ #include "wavemap/common.h" #include "wavemap/indexing/index_conversions.h" -#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/map_base.h" #include "wavemap/utils/data/eigen_checks.h" namespace wavemap::interpolate { -FloatingPoint trilinear(const wavemap::VolumetricDataStructureBase& map, +FloatingPoint trilinear(const wavemap::MapBase& map, const wavemap::Point3D& position) { const FloatingPoint cell_width = map.getMinCellWidth(); const FloatingPoint cell_width_inv = 1.f / cell_width; diff --git a/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h b/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h index 07101e92a..2f8dc2ef4 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h +++ b/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h @@ -20,8 +20,7 @@ struct VectorDistance { } }; -using VectorDistanceField = - DenseBlockHash; +using VectorDistanceField = DenseBlockHash; class FullEuclideanSDFGenerator { public: diff --git a/libraries/wavemap/src/integrator/integrator_factory.cc b/libraries/wavemap/src/integrator/integrator_factory.cc index bae265353..5b32fc52e 100644 --- a/libraries/wavemap/src/integrator/integrator_factory.cc +++ b/libraries/wavemap/src/integrator/integrator_factory.cc @@ -12,7 +12,7 @@ namespace wavemap { IntegratorBase::Ptr IntegratorFactory::create( - const param::Value& params, VolumetricDataStructureBase::Ptr occupancy_map, + const param::Value& params, MapBase::Ptr occupancy_map, std::shared_ptr thread_pool, std::optional default_integrator_type) { if (const auto type = IntegratorType::from(params, "integration_method"); @@ -34,8 +34,7 @@ IntegratorBase::Ptr IntegratorFactory::create( IntegratorBase::Ptr IntegratorFactory::create( IntegratorType integrator_type, const param::Value& params, - VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr thread_pool) { + MapBase::Ptr occupancy_map, std::shared_ptr thread_pool) { // If we're using a ray tracing based integrator, we can build it directly if (integrator_type == IntegratorType::kRayTracingIntegrator) { if (const auto config = @@ -98,8 +97,7 @@ IntegratorBase::Ptr IntegratorFactory::create( } else { LOG(ERROR) << "Integrator of type " << integrator_type.toStr() << " only supports data structures of type " - << VolumetricDataStructureType::toStr( - VolumetricDataStructureType::kOctree) + << MapType::toStr(MapType::kOctree) << ". Returning nullptr."; } break; @@ -114,8 +112,7 @@ IntegratorBase::Ptr IntegratorFactory::create( } else { LOG(ERROR) << "Integrator of type " << integrator_type.toStr() << " only supports data structures of type " - << VolumetricDataStructureType::toStr( - VolumetricDataStructureType::kWaveletOctree) + << MapType::toStr(MapType::kWaveletOctree) << ". Returning nullptr."; } break; @@ -131,8 +128,7 @@ IntegratorBase::Ptr IntegratorFactory::create( } else { LOG(ERROR) << "Integrator of type " << integrator_type.toStr() << " only supports data structures of type " - << VolumetricDataStructureType::toStr( - VolumetricDataStructureType::kHashedWaveletOctree) + << MapType::toStr(MapType::kHashedWaveletOctree) << ". Returning nullptr."; } break; @@ -146,12 +142,10 @@ IntegratorBase::Ptr IntegratorFactory::create( beam_offset_image, measurement_model, std::move(hashed_chunked_wavelet_map), std::move(thread_pool)); } else { - LOG(ERROR) - << "Integrator of type " << integrator_type.toStr() - << " only supports data structures of type " - << VolumetricDataStructureType::toStr( - VolumetricDataStructureType::kHashedChunkedWaveletOctree) - << ". Returning nullptr."; + LOG(ERROR) << "Integrator of type " << integrator_type.toStr() + << " only supports data structures of type " + << MapType::toStr(MapType::kHashedChunkedWaveletOctree) + << ". Returning nullptr."; } break; } diff --git a/libraries/wavemap/src/map/hashed_blocks.cc b/libraries/wavemap/src/map/hashed_blocks.cc index 101469418..39dc32d5a 100644 --- a/libraries/wavemap/src/map/hashed_blocks.cc +++ b/libraries/wavemap/src/map/hashed_blocks.cc @@ -23,7 +23,7 @@ void HashedBlocks::prune() { } void HashedBlocks::forEachLeaf( - VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn) const { + MapBase::IndexedLeafVisitorFunction visitor_fn) const { DenseBlockHash::forEachLeaf( [&visitor_fn](const Index3D& index, FloatingPoint cell_data) { const OctreeIndex hierarchical_cell_index = OctreeIndex{0, index}; diff --git a/libraries/wavemap/src/map/hashed_chunked_wavelet_octree.cc b/libraries/wavemap/src/map/hashed_chunked_wavelet_octree.cc index 36568a08d..c6f99d268 100644 --- a/libraries/wavemap/src/map/hashed_chunked_wavelet_octree.cc +++ b/libraries/wavemap/src/map/hashed_chunked_wavelet_octree.cc @@ -73,7 +73,7 @@ Index3D HashedChunkedWaveletOctree::getMaxIndex() const { } void HashedChunkedWaveletOctree::forEachLeaf( - VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn) const { + MapBase::IndexedLeafVisitorFunction visitor_fn) const { forEachBlock( [&visitor_fn](const BlockIndex& block_index, const Block& block) { block.forEachLeaf(block_index, visitor_fn); diff --git a/libraries/wavemap/src/map/hashed_chunked_wavelet_octree_block.cc b/libraries/wavemap/src/map/hashed_chunked_wavelet_octree_block.cc index 38b8af84f..86399634b 100644 --- a/libraries/wavemap/src/map/hashed_chunked_wavelet_octree_block.cc +++ b/libraries/wavemap/src/map/hashed_chunked_wavelet_octree_block.cc @@ -145,7 +145,7 @@ void HashedChunkedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index, void HashedChunkedWaveletOctreeBlock::forEachLeaf( const BlockIndex& block_index, - VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn, + MapBase::IndexedLeafVisitorFunction visitor_fn, IndexElement termination_height) const { ZoneScoped; if (empty()) { diff --git a/libraries/wavemap/src/map/hashed_wavelet_octree.cc b/libraries/wavemap/src/map/hashed_wavelet_octree.cc index 0168501ad..bf1404693 100644 --- a/libraries/wavemap/src/map/hashed_wavelet_octree.cc +++ b/libraries/wavemap/src/map/hashed_wavelet_octree.cc @@ -72,7 +72,7 @@ Index3D HashedWaveletOctree::getMaxIndex() const { } void HashedWaveletOctree::forEachLeaf( - VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn) const { + MapBase::IndexedLeafVisitorFunction visitor_fn) const { forEachBlock( [&visitor_fn](const BlockIndex& block_index, const Block& block) { block.forEachLeaf(block_index, visitor_fn); diff --git a/libraries/wavemap/src/map/hashed_wavelet_octree_block.cc b/libraries/wavemap/src/map/hashed_wavelet_octree_block.cc index c34f5a1a0..69c371083 100644 --- a/libraries/wavemap/src/map/hashed_wavelet_octree_block.cc +++ b/libraries/wavemap/src/map/hashed_wavelet_octree_block.cc @@ -103,7 +103,7 @@ void HashedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index, void HashedWaveletOctreeBlock::forEachLeaf( const BlockIndex& block_index, - VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn, + MapBase::IndexedLeafVisitorFunction visitor_fn, IndexElement termination_height) const { ZoneScoped; if (empty()) { diff --git a/libraries/wavemap/src/map/volumetric_data_structure_base.cc b/libraries/wavemap/src/map/map_base.cc similarity index 77% rename from libraries/wavemap/src/map/volumetric_data_structure_base.cc rename to libraries/wavemap/src/map/map_base.cc index 4d04652cb..71b714f97 100644 --- a/libraries/wavemap/src/map/volumetric_data_structure_base.cc +++ b/libraries/wavemap/src/map/map_base.cc @@ -1,4 +1,4 @@ -#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/map_base.h" #include "wavemap/utils/meta/nameof.h" @@ -8,7 +8,7 @@ DECLARE_CONFIG_MEMBERS(VolumetricDataStructureConfig, (min_log_odds) (max_log_odds)); -bool VolumetricDataStructureConfig::isValid(bool verbose) const { +bool MapBaseConfig::isValid(bool verbose) const { bool is_valid = true; is_valid &= IS_PARAM_GT(min_cell_width, 0.f, verbose); diff --git a/libraries/wavemap/src/map/volumetric_data_structure_factory.cc b/libraries/wavemap/src/map/map_factory.cc similarity index 63% rename from libraries/wavemap/src/map/volumetric_data_structure_factory.cc rename to libraries/wavemap/src/map/map_factory.cc index e016b01a4..01abe5d7c 100644 --- a/libraries/wavemap/src/map/volumetric_data_structure_factory.cc +++ b/libraries/wavemap/src/map/map_factory.cc @@ -1,4 +1,4 @@ -#include "wavemap/map/volumetric_data_structure_factory.h" +#include "wavemap/map/map_factory.h" #include "wavemap/map/hashed_blocks.h" #include "wavemap/map/hashed_chunked_wavelet_octree.h" @@ -7,31 +7,26 @@ #include "wavemap/map/wavelet_octree.h" namespace wavemap { -VolumetricDataStructureBase::Ptr VolumetricDataStructureFactory::create( - const param::Value& params, - std::optional default_data_structure_type) { - if (const auto type = VolumetricDataStructureType::from(params); type) { +MapBase::Ptr MapFactory::create(const param::Value& params, + std::optional default_map_type) { + if (const auto type = MapType::from(params); type) { return create(type.value(), params); } - if (default_data_structure_type.has_value()) { - LOG(WARNING) << "Default type \"" - << default_data_structure_type.value().toStr() + if (default_map_type.has_value()) { + LOG(WARNING) << "Default type \"" << default_map_type.value().toStr() << "\" will be created instead."; - return create(default_data_structure_type.value(), params); + return create(default_map_type.value(), params); } LOG(ERROR) << "No default was set. Returning nullptr."; return nullptr; } -VolumetricDataStructureBase::Ptr VolumetricDataStructureFactory::create( - VolumetricDataStructureType data_structure_type, - const param::Value& params) { - switch (data_structure_type) { - case VolumetricDataStructureType::kHashedBlocks: { - if (const auto config = VolumetricDataStructureConfig::from(params); - config) { +MapBase::Ptr MapFactory::create(MapType map_type, const param::Value& params) { + switch (map_type) { + case MapType::kHashedBlocks: { + if (const auto config = MapBaseConfig::from(params); config) { return std::make_shared(config.value()); } else { LOG(ERROR) << "Hashed blocks volumetric data structure config could " @@ -39,7 +34,7 @@ VolumetricDataStructureBase::Ptr VolumetricDataStructureFactory::create( return nullptr; } } - case VolumetricDataStructureType::kOctree: { + case MapType::kOctree: { if (const auto config = VolumetricOctreeConfig::from(params); config) { return std::make_shared(config.value()); } else { @@ -48,7 +43,7 @@ VolumetricDataStructureBase::Ptr VolumetricDataStructureFactory::create( return nullptr; } } - case VolumetricDataStructureType::kWaveletOctree: { + case MapType::kWaveletOctree: { if (const auto config = WaveletOctreeConfig::from(params); config) { return std::make_shared(config.value()); } else { @@ -57,7 +52,7 @@ VolumetricDataStructureBase::Ptr VolumetricDataStructureFactory::create( return nullptr; } } - case VolumetricDataStructureType::kHashedWaveletOctree: { + case MapType::kHashedWaveletOctree: { if (const auto config = HashedWaveletOctreeConfig::from(params); config) { return std::make_shared(config.value()); } else { @@ -66,7 +61,7 @@ VolumetricDataStructureBase::Ptr VolumetricDataStructureFactory::create( return nullptr; } } - case VolumetricDataStructureType::kHashedChunkedWaveletOctree: { + case MapType::kHashedChunkedWaveletOctree: { if (const auto config = HashedChunkedWaveletOctreeConfig::from(params); config) { return std::make_shared(config.value()); @@ -78,7 +73,7 @@ VolumetricDataStructureBase::Ptr VolumetricDataStructureFactory::create( } default: LOG(ERROR) << "Attempted to create data structure with unknown type ID: " - << data_structure_type.toTypeId() << ". Returning nullptr."; + << map_type.toTypeId() << ". Returning nullptr."; return nullptr; } } diff --git a/libraries/wavemap/src/map/volumetric_octree.cc b/libraries/wavemap/src/map/volumetric_octree.cc index c76daa34c..c43268c96 100644 --- a/libraries/wavemap/src/map/volumetric_octree.cc +++ b/libraries/wavemap/src/map/volumetric_octree.cc @@ -112,7 +112,7 @@ Index3D VolumetricOctree::getMaxIndex() const { } void VolumetricOctree::forEachLeaf( - VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn) const { + MapBase::IndexedLeafVisitorFunction visitor_fn) const { struct StackElement { const OctreeIndex node_index; const NodeType& node; diff --git a/libraries/wavemap/src/map/wavelet_octree.cc b/libraries/wavemap/src/map/wavelet_octree.cc index 1783f2810..6b230035f 100644 --- a/libraries/wavemap/src/map/wavelet_octree.cc +++ b/libraries/wavemap/src/map/wavelet_octree.cc @@ -60,7 +60,7 @@ Index3D WaveletOctree::getMaxIndex() const { } void WaveletOctree::forEachLeaf( - VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn) const { + MapBase::IndexedLeafVisitorFunction visitor_fn) const { if (empty()) { return; } diff --git a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc index c0adb5046..75190f220 100644 --- a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc @@ -26,8 +26,7 @@ HashedBlocks FullEuclideanSDFGenerator::generate( propagate(occupancy_map, full_sdf, open); // Copy into regular data structure - const VolumetricDataStructureConfig config{min_cell_width, 0.f, - max_distance_}; + const MapBaseConfig config{min_cell_width, 0.f, max_distance_}; HashedBlocks sdf(config, max_distance_); full_sdf.forEachLeaf( [&sdf](const Index3D& cell_index, const VectorDistance& cell_value) { diff --git a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc index 305103f44..40b1d5a2b 100644 --- a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc @@ -11,8 +11,7 @@ HashedBlocks QuasiEuclideanSDFGenerator::generate( ZoneScoped; // Initialize the SDF data structure const FloatingPoint min_cell_width = occupancy_map.getMinCellWidth(); - const VolumetricDataStructureConfig config{min_cell_width, 0.f, - max_distance_}; + const MapBaseConfig config{min_cell_width, 0.f, max_distance_}; HashedBlocks sdf(config, max_distance_); // Initialize the bucketed priority queue diff --git a/libraries/wavemap/test/include/wavemap/test/config_generator.h b/libraries/wavemap/test/include/wavemap/test/config_generator.h index c899d5070..c2adf1fb1 100644 --- a/libraries/wavemap/test/include/wavemap/test/config_generator.h +++ b/libraries/wavemap/test/include/wavemap/test/config_generator.h @@ -10,7 +10,7 @@ #include "wavemap/integrator/ray_tracing/ray_tracing_integrator.h" #include "wavemap/map/hashed_chunked_wavelet_octree.h" #include "wavemap/map/hashed_wavelet_octree.h" -#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/map_base.h" #include "wavemap/map/volumetric_octree.h" #include "wavemap/map/wavelet_octree.h" #include "wavemap/utils/random_number_generator.h" @@ -23,9 +23,9 @@ ConfigT getRandomConfig(RandomNumberGenerator&, Args...); // Volumetric data structure configs template <> -inline VolumetricDataStructureConfig -getRandomConfig(RandomNumberGenerator& rng) { - VolumetricDataStructureConfig config; +inline MapBaseConfig getRandomConfig( + RandomNumberGenerator& rng) { + MapBaseConfig config; config.min_cell_width = rng.getRandomRealNumber(0.05f, 1.f); config.min_log_odds = rng.getRandomRealNumber(-10.f, 0.f); config.max_log_odds = rng.getRandomRealNumber(0.f, 10.f); diff --git a/libraries/wavemap/test/src/data_structure/test_hashed_blocks.cc b/libraries/wavemap/test/src/data_structure/test_hashed_blocks.cc index b2a48c086..d746e164b 100644 --- a/libraries/wavemap/test/src/data_structure/test_hashed_blocks.cc +++ b/libraries/wavemap/test/src/data_structure/test_hashed_blocks.cc @@ -13,8 +13,7 @@ class HashedBlocksTest : public FixtureBase, public GeometryGenerator {}; TEST_F(HashedBlocksTest, Initialization) { const FloatingPoint random_min_cell_width = getRandomMinCellWidth(); - HashedBlocks map( - VolumetricDataStructureConfig{random_min_cell_width, -2.f, 4.f}); + HashedBlocks map(MapBaseConfig{random_min_cell_width, -2.f, 4.f}); EXPECT_EQ(map.getMinCellWidth(), random_min_cell_width); EXPECT_TRUE(map.empty()); EXPECT_EQ(map.size(), 0u); diff --git a/libraries/wavemap/test/src/data_structure/test_volumetric_data_structure.cc b/libraries/wavemap/test/src/data_structure/test_volumetric_data_structure.cc index 7ccfdc7d9..4f31eee90 100644 --- a/libraries/wavemap/test/src/data_structure/test_volumetric_data_structure.cc +++ b/libraries/wavemap/test/src/data_structure/test_volumetric_data_structure.cc @@ -5,7 +5,7 @@ #include "wavemap/map/hashed_blocks.h" #include "wavemap/map/hashed_chunked_wavelet_octree.h" #include "wavemap/map/hashed_wavelet_octree.h" -#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/map_base.h" #include "wavemap/map/volumetric_octree.h" #include "wavemap/map/wavelet_octree.h" #include "wavemap/test/config_generator.h" @@ -30,8 +30,7 @@ TYPED_TEST_SUITE(VolumetricDataStructureTest, VolumetricDataStructureTypes, ); TYPED_TEST(VolumetricDataStructureTest, InitializationAndClearing) { const auto config = ConfigGenerator::getRandomConfig(); - std::unique_ptr map_base_ptr = - std::make_unique(config); + std::unique_ptr map_base_ptr = std::make_unique(config); // NOTE: Empty data structures are allowed to have size 0 or 1, such that the // tree based data structures can keep their root node allocated even @@ -56,8 +55,7 @@ TYPED_TEST(VolumetricDataStructureTest, InitializationAndClearing) { TYPED_TEST(VolumetricDataStructureTest, Pruning) { const auto config = ConfigGenerator::getRandomConfig(); - std::unique_ptr map_base_ptr = - std::make_unique(config); + std::unique_ptr map_base_ptr = std::make_unique(config); const size_t empty_map_memory_usage = map_base_ptr->getMemoryUsage(); // Check that pruning removes all zero cells @@ -98,8 +96,7 @@ TYPED_TEST(VolumetricDataStructureTest, MinMaxIndexGetters) { for (int i = 0; i < kNumRepetitions; ++i) { const auto config = ConfigGenerator::getRandomConfig(); - std::unique_ptr map_base_ptr = - std::make_unique(config); + std::unique_ptr map_base_ptr = std::make_unique(config); { const Index3D map_min_index = map_base_ptr->getMinIndex(); const Index3D map_max_index = map_base_ptr->getMaxIndex(); @@ -132,8 +129,7 @@ TYPED_TEST(VolumetricDataStructureTest, InsertionAndLeafVisitor) { // Create a random map const auto config = ConfigGenerator::getRandomConfig(); - std::unique_ptr map_base_ptr = - std::make_unique(config); + std::unique_ptr map_base_ptr = std::make_unique(config); const std::vector random_indices = GeometryGenerator::getRandomIndexVector<3>( 1000u, 2000u, Index3D::Constant(-5000), Index3D::Constant(5000)); diff --git a/libraries/wavemap/test/src/integrator/test_pointcloud_integrators.cc b/libraries/wavemap/test/src/integrator/test_pointcloud_integrators.cc index ca9cc81a1..0b6425402 100644 --- a/libraries/wavemap/test/src/integrator/test_pointcloud_integrators.cc +++ b/libraries/wavemap/test/src/integrator/test_pointcloud_integrators.cc @@ -15,7 +15,7 @@ #include "wavemap/integrator/ray_tracing/ray_tracing_integrator.h" #include "wavemap/map/hashed_blocks.h" #include "wavemap/map/hashed_wavelet_octree.h" -#include "wavemap/map/volumetric_data_structure_base.h" +#include "wavemap/map/map_base.h" #include "wavemap/map/volumetric_octree.h" #include "wavemap/map/wavelet_octree.h" #include "wavemap/test/config_generator.h" @@ -92,7 +92,7 @@ TYPED_TEST(PointcloudIntegratorTypedTest, *projection_model), projection_model, posed_range_image, beam_offset_image); - VolumetricDataStructureBase::Ptr reference_occupancy_map = + MapBase::Ptr reference_occupancy_map = std::make_shared(data_structure_config); IntegratorBase::Ptr reference_integrator = std::make_shared( @@ -149,8 +149,7 @@ TEST_F(PointcloudIntegratorTest, RayTracingIntegrator) { for (int idx = 0; idx < 3; ++idx) { const auto ray_tracing_integrator_config = getRandomConfig(); - const auto data_structure_config = - getRandomConfig(); + const auto data_structure_config = getRandomConfig(); const auto projection_model = SphericalProjector(getRandomConfig()); const FloatingPoint min_cell_width_inv = @@ -183,7 +182,7 @@ TEST_F(PointcloudIntegratorTest, RayTracingIntegrator) { } // Set up the occupancy map, integrator and integrate the pointcloud - VolumetricDataStructureBase::Ptr occupancy_map = + MapBase::Ptr occupancy_map = std::make_shared(data_structure_config); IntegratorBase::Ptr pointcloud_integrator = std::make_shared(ray_tracing_integrator_config, diff --git a/libraries/wavemap/test/src/utils/test_map_interpolator.cpp b/libraries/wavemap/test/src/utils/test_map_interpolator.cpp index e2f561c6b..4d1dfebd5 100644 --- a/libraries/wavemap/test/src/utils/test_map_interpolator.cpp +++ b/libraries/wavemap/test/src/utils/test_map_interpolator.cpp @@ -7,7 +7,7 @@ namespace wavemap { TEST(InterpolationUtilsTest, Trilinear) { - VolumetricDataStructureConfig config{1.f, -10.f, 10.f}; + MapBaseConfig config{1.f, -10.f, 10.f}; HashedBlocks map(config); const FloatingPoint cell_width = map.getMinCellWidth(); // Fill with known pattern diff --git a/libraries/wavemap_io/include/wavemap_io/file_conversions.h b/libraries/wavemap_io/include/wavemap_io/file_conversions.h index 2c2331583..6d1e81068 100644 --- a/libraries/wavemap_io/include/wavemap_io/file_conversions.h +++ b/libraries/wavemap_io/include/wavemap_io/file_conversions.h @@ -3,15 +3,13 @@ #include -#include +#include #include "wavemap_io/stream_conversions.h" namespace wavemap::io { -bool mapToFile(const VolumetricDataStructureBase& map, - const std::filesystem::path& file_path); -bool fileToMap(const std::filesystem::path& file_path, - VolumetricDataStructureBase::Ptr& map); +bool mapToFile(const MapBase& map, const std::filesystem::path& file_path); +bool fileToMap(const std::filesystem::path& file_path, MapBase::Ptr& map); } // namespace wavemap::io #endif // WAVEMAP_IO_FILE_CONVERSIONS_H_ diff --git a/libraries/wavemap_io/include/wavemap_io/stream_conversions.h b/libraries/wavemap_io/include/wavemap_io/stream_conversions.h index ca91a74d4..45d5dce3d 100644 --- a/libraries/wavemap_io/include/wavemap_io/stream_conversions.h +++ b/libraries/wavemap_io/include/wavemap_io/stream_conversions.h @@ -14,8 +14,8 @@ #include "wavemap_io/streamable_types.h" namespace wavemap::io { -bool mapToStream(const VolumetricDataStructureBase& map, std::ostream& ostream); -bool streamToMap(std::istream& istream, VolumetricDataStructureBase::Ptr& map); +bool mapToStream(const MapBase& map, std::ostream& ostream); +bool streamToMap(std::istream& istream, MapBase::Ptr& map); void mapToStream(const HashedBlocks& map, std::ostream& ostream); bool streamToMap(std::istream& istream, HashedBlocks::Ptr& map); diff --git a/libraries/wavemap_io/src/file_conversions.cc b/libraries/wavemap_io/src/file_conversions.cc index 2e45ae48e..df01defbc 100644 --- a/libraries/wavemap_io/src/file_conversions.cc +++ b/libraries/wavemap_io/src/file_conversions.cc @@ -3,8 +3,7 @@ #include namespace wavemap::io { -bool mapToFile(const VolumetricDataStructureBase& map, - const std::filesystem::path& file_path) { +bool mapToFile(const MapBase& map, const std::filesystem::path& file_path) { if (file_path.empty()) { LOG(WARNING) << "Could open file for writing. Specified file path is empty."; @@ -30,8 +29,7 @@ bool mapToFile(const VolumetricDataStructureBase& map, return static_cast(file_ostream); } -bool fileToMap(const std::filesystem::path& file_path, - VolumetricDataStructureBase::Ptr& map) { +bool fileToMap(const std::filesystem::path& file_path, MapBase::Ptr& map) { if (file_path.empty()) { LOG(WARNING) << "Could not open file for reading. Specified file path is empty."; diff --git a/libraries/wavemap_io/src/stream_conversions.cc b/libraries/wavemap_io/src/stream_conversions.cc index c1509705b..6e3cd61f6 100644 --- a/libraries/wavemap_io/src/stream_conversions.cc +++ b/libraries/wavemap_io/src/stream_conversions.cc @@ -1,8 +1,7 @@ #include "wavemap_io/stream_conversions.h" namespace wavemap::io { -bool mapToStream(const VolumetricDataStructureBase& map, - std::ostream& ostream) { +bool mapToStream(const MapBase& map, std::ostream& ostream) { // Call the appropriate mapToStream converter based on the map's derived type if (const auto* hashed_blocks = dynamic_cast(&map); hashed_blocks) { @@ -32,7 +31,7 @@ bool mapToStream(const VolumetricDataStructureBase& map, return false; } -bool streamToMap(std::istream& istream, VolumetricDataStructureBase::Ptr& map) { +bool streamToMap(std::istream& istream, MapBase::Ptr& map) { // Call the appropriate streamToMap converter based on the received map's type const auto storage_format = streamable::StorageFormat::peek(istream); switch (storage_format) { @@ -111,7 +110,7 @@ bool streamToMap(std::istream& istream, HashedBlocks::Ptr& map) { // Deserialize the map's config and initialize the data structure const auto hashed_blocks_header = streamable::HashedBlocksHeader::read(istream); - VolumetricDataStructureConfig config; + MapBaseConfig config; config.min_cell_width = hashed_blocks_header.min_cell_width; config.min_log_odds = hashed_blocks_header.min_log_odds; config.max_log_odds = hashed_blocks_header.max_log_odds; diff --git a/libraries/wavemap_io/test/src/test_file_conversions.cc b/libraries/wavemap_io/test/src/test_file_conversions.cc index f04410d44..e953b8ba0 100644 --- a/libraries/wavemap_io/test/src/test_file_conversions.cc +++ b/libraries/wavemap_io/test/src/test_file_conversions.cc @@ -2,7 +2,7 @@ #include #include #include -#include +#include #include #include #include @@ -39,14 +39,14 @@ TYPED_TEST(FileConversionsTest, MetadataPreservation) { } // Convert to base pointer - VolumetricDataStructureBase::ConstPtr map_base = map; + MapBase::ConstPtr map_base = map; ASSERT_EQ(map_base->getMinCellWidth(), config.min_cell_width); ASSERT_EQ(map_base->getMinLogOdds(), config.min_log_odds); ASSERT_EQ(map_base->getMaxLogOdds(), config.max_log_odds); // Serialize and deserialize ASSERT_TRUE(io::mapToFile(*map_base, TestFixture::kTemporaryFilePath)); - VolumetricDataStructureBase::Ptr map_base_round_trip; + MapBase::Ptr map_base_round_trip; ASSERT_TRUE( io::fileToMap(TestFixture::kTemporaryFilePath, map_base_round_trip)); ASSERT_TRUE(map_base_round_trip); @@ -97,7 +97,7 @@ TYPED_TEST(FileConversionsTest, InsertionAndLeafVisitor) { // Serialize and deserialize ASSERT_TRUE(io::mapToFile(map_original, TestFixture::kTemporaryFilePath)); - VolumetricDataStructureBase::Ptr map_base_round_trip; + MapBase::Ptr map_base_round_trip; ASSERT_TRUE( io::fileToMap(TestFixture::kTemporaryFilePath, map_base_round_trip)); ASSERT_TRUE(map_base_round_trip); diff --git a/ros/wavemap_ros/include/wavemap_ros/input_handler/depth_image_input_handler.h b/ros/wavemap_ros/include/wavemap_ros/input_handler/depth_image_input_handler.h index 696e8bdb0..5cdc923ea 100644 --- a/ros/wavemap_ros/include/wavemap_ros/input_handler/depth_image_input_handler.h +++ b/ros/wavemap_ros/include/wavemap_ros/input_handler/depth_image_input_handler.h @@ -70,7 +70,7 @@ class DepthImageInputHandler : public InputHandler { public: DepthImageInputHandler(const DepthImageInputHandlerConfig& config, const param::Value& params, std::string world_frame, - VolumetricDataStructureBase::Ptr occupancy_map, + MapBase::Ptr occupancy_map, std::shared_ptr transformer, std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private); diff --git a/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler.h b/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler.h index c06e0c656..0273b109a 100644 --- a/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler.h +++ b/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler.h @@ -11,7 +11,7 @@ #include #include #include -#include +#include #include #include diff --git a/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler_factory.h b/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler_factory.h index 9089c1f0c..f520eb677 100644 --- a/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler_factory.h +++ b/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler_factory.h @@ -12,8 +12,7 @@ class InputHandlerFactory { public: static std::unique_ptr create( const param::Value& params, std::string world_frame, - VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, + MapBase::Ptr occupancy_map, std::shared_ptr transformer, std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private, std::optional default_input_handler_type = @@ -21,7 +20,7 @@ class InputHandlerFactory { static std::unique_ptr create( InputHandlerType input_handler_type, const param::Value& params, - std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, + std::string world_frame, MapBase::Ptr occupancy_map, std::shared_ptr transformer, std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private); diff --git a/ros/wavemap_ros/include/wavemap_ros/input_handler/pointcloud_input_handler.h b/ros/wavemap_ros/include/wavemap_ros/input_handler/pointcloud_input_handler.h index a8009b9ce..59adca7ac 100644 --- a/ros/wavemap_ros/include/wavemap_ros/input_handler/pointcloud_input_handler.h +++ b/ros/wavemap_ros/include/wavemap_ros/input_handler/pointcloud_input_handler.h @@ -84,7 +84,7 @@ class PointcloudInputHandler : public InputHandler { public: PointcloudInputHandler(const PointcloudInputHandlerConfig& config, const param::Value& params, std::string world_frame, - VolumetricDataStructureBase::Ptr occupancy_map, + MapBase::Ptr occupancy_map, std::shared_ptr transformer, std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private); diff --git a/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h b/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h index 0a2dbb819..4e2634672 100644 --- a/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h +++ b/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h @@ -14,7 +14,7 @@ #include #include #include -#include +#include #include #include #include @@ -71,15 +71,13 @@ class WavemapServer { InputHandler* addInput(const param::Value& integrator_params, const ros::NodeHandle& nh, ros::NodeHandle nh_private); - VolumetricDataStructureBase::Ptr getMap() { return occupancy_map_; } - VolumetricDataStructureBase::ConstPtr getMap() const { - return occupancy_map_; - } + MapBase::Ptr getMap() { return occupancy_map_; } + MapBase::ConstPtr getMap() const { return occupancy_map_; } private: const WavemapServerConfig config_; - VolumetricDataStructureBase::Ptr occupancy_map_; + MapBase::Ptr occupancy_map_; std::shared_ptr transformer_; std::shared_ptr thread_pool_; diff --git a/ros/wavemap_ros/src/input_handler/depth_image_input_handler.cc b/ros/wavemap_ros/src/input_handler/depth_image_input_handler.cc index cd88e33dc..c6f5eec8f 100644 --- a/ros/wavemap_ros/src/input_handler/depth_image_input_handler.cc +++ b/ros/wavemap_ros/src/input_handler/depth_image_input_handler.cc @@ -32,7 +32,7 @@ bool DepthImageInputHandlerConfig::isValid(bool verbose) const { DepthImageInputHandler::DepthImageInputHandler( const DepthImageInputHandlerConfig& config, const param::Value& params, - std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, + std::string world_frame, MapBase::Ptr occupancy_map, std::shared_ptr transformer, std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private) diff --git a/ros/wavemap_ros/src/input_handler/input_handler.cc b/ros/wavemap_ros/src/input_handler/input_handler.cc index 53f1f9d8b..fd0fe110c 100644 --- a/ros/wavemap_ros/src/input_handler/input_handler.cc +++ b/ros/wavemap_ros/src/input_handler/input_handler.cc @@ -29,7 +29,7 @@ bool InputHandlerConfig::isValid(bool verbose) const { InputHandler::InputHandler(const InputHandlerConfig& config, const param::Value& params, std::string world_frame, - VolumetricDataStructureBase::Ptr occupancy_map, + MapBase::Ptr occupancy_map, std::shared_ptr transformer, std::shared_ptr thread_pool, const ros::NodeHandle& nh, diff --git a/ros/wavemap_ros/src/input_handler/input_handler_factory.cc b/ros/wavemap_ros/src/input_handler/input_handler_factory.cc index ee75896f9..a9adf188a 100644 --- a/ros/wavemap_ros/src/input_handler/input_handler_factory.cc +++ b/ros/wavemap_ros/src/input_handler/input_handler_factory.cc @@ -6,8 +6,7 @@ namespace wavemap { std::unique_ptr InputHandlerFactory::create( const param::Value& params, std::string world_frame, - VolumetricDataStructureBase::Ptr occupancy_map, - std::shared_ptr transformer, + MapBase::Ptr occupancy_map, std::shared_ptr transformer, std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private, std::optional default_input_handler_type) { @@ -33,7 +32,7 @@ std::unique_ptr InputHandlerFactory::create( std::unique_ptr InputHandlerFactory::create( InputHandlerType input_handler_type, const param::Value& params, - std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, + std::string world_frame, MapBase::Ptr occupancy_map, std::shared_ptr transformer, std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private) { diff --git a/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc b/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc index 7dfe42c1b..bd58cd9d2 100644 --- a/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc +++ b/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc @@ -32,7 +32,7 @@ bool PointcloudInputHandlerConfig::isValid(bool verbose) const { PointcloudInputHandler::PointcloudInputHandler( const PointcloudInputHandlerConfig& config, const param::Value& params, - std::string world_frame, VolumetricDataStructureBase::Ptr occupancy_map, + std::string world_frame, MapBase::Ptr occupancy_map, std::shared_ptr transformer, std::shared_ptr thread_pool, ros::NodeHandle nh, ros::NodeHandle nh_private) diff --git a/ros/wavemap_ros/src/wavemap_server.cc b/ros/wavemap_ros/src/wavemap_server.cc index 1fc557297..8216f97b7 100644 --- a/ros/wavemap_ros/src/wavemap_server.cc +++ b/ros/wavemap_ros/src/wavemap_server.cc @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include #include #include @@ -55,8 +55,8 @@ WavemapServer::WavemapServer(ros::NodeHandle nh, ros::NodeHandle nh_private, // Setup data structure const auto data_structure_params = param::convert::toParamValue(nh_private, "map/data_structure"); - occupancy_map_ = VolumetricDataStructureFactory::create( - data_structure_params, VolumetricDataStructureType::kHashedBlocks); + occupancy_map_ = + MapFactory::create(data_structure_params, MapType::kHashedBlocks); CHECK_NOTNULL(occupancy_map_); // Setup thread pool diff --git a/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h b/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h index 99849b4a2..439524f69 100644 --- a/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h +++ b/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h @@ -18,11 +18,9 @@ #include namespace wavemap::convert { -bool mapToRosMsg(const VolumetricDataStructureBase& map, - const std::string& frame_id, const ros::Time& stamp, - wavemap_msgs::Map& msg); -bool rosMsgToMap(const wavemap_msgs::Map& msg, - VolumetricDataStructureBase::Ptr& map); +bool mapToRosMsg(const MapBase& map, const std::string& frame_id, + const ros::Time& stamp, wavemap_msgs::Map& msg); +bool rosMsgToMap(const wavemap_msgs::Map& msg, MapBase::Ptr& map); void mapToRosMsg(const HashedBlocks& map, wavemap_msgs::HashedBlocks& msg); void rosMsgToMap(const wavemap_msgs::HashedBlocks& msg, HashedBlocks::Ptr& map); diff --git a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc index c9ed9f74c..9caf1935a 100644 --- a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc +++ b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc @@ -4,9 +4,8 @@ #include namespace wavemap::convert { -bool mapToRosMsg(const VolumetricDataStructureBase& map, - const std::string& frame_id, const ros::Time& stamp, - wavemap_msgs::Map& msg) { +bool mapToRosMsg(const MapBase& map, const std::string& frame_id, + const ros::Time& stamp, wavemap_msgs::Map& msg) { // Write the msg header msg.header.stamp = stamp; msg.header.frame_id = frame_id; @@ -43,8 +42,7 @@ bool mapToRosMsg(const VolumetricDataStructureBase& map, return false; } -bool rosMsgToMap(const wavemap_msgs::Map& msg, - VolumetricDataStructureBase::Ptr& map) { +bool rosMsgToMap(const wavemap_msgs::Map& msg, MapBase::Ptr& map) { ZoneScoped; // Check validity bool is_valid = true; @@ -139,7 +137,7 @@ void rosMsgToMap(const wavemap_msgs::HashedBlocks& msg, HashedBlocks::Ptr& map) { ZoneScoped; // Deserialize the map's config - VolumetricDataStructureConfig config; + MapBaseConfig config; config.min_cell_width = msg.min_cell_width; config.min_log_odds = msg.min_log_odds; config.max_log_odds = msg.max_log_odds; diff --git a/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc b/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc index 61f64beea..f482d2ad3 100644 --- a/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc +++ b/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc @@ -2,7 +2,7 @@ #include #include #include -#include +#include #include #include #include @@ -46,7 +46,7 @@ TYPED_TEST(MapMsgConversionsTest, MetadataPreservation) { } // Convert to base pointer - VolumetricDataStructureBase::ConstPtr map_base = map; + MapBase::ConstPtr map_base = map; ASSERT_EQ(map_base->getMinCellWidth(), config.min_cell_width); ASSERT_EQ(map_base->getMinLogOdds(), config.min_log_odds); ASSERT_EQ(map_base->getMaxLogOdds(), config.max_log_odds); @@ -55,7 +55,7 @@ TYPED_TEST(MapMsgConversionsTest, MetadataPreservation) { wavemap_msgs::Map map_msg; ASSERT_TRUE(convert::mapToRosMsg(*map_base, TestFixture::frame_id, TestFixture::stamp, map_msg)); - VolumetricDataStructureBase::Ptr map_base_round_trip; + MapBase::Ptr map_base_round_trip; ASSERT_TRUE(convert::rosMsgToMap(map_msg, map_base_round_trip)); ASSERT_TRUE(map_base_round_trip); @@ -111,7 +111,7 @@ TYPED_TEST(MapMsgConversionsTest, InsertionAndLeafVisitor) { wavemap_msgs::Map map_msg; ASSERT_TRUE(convert::mapToRosMsg(map_original, TestFixture::frame_id, TestFixture::stamp, map_msg)); - VolumetricDataStructureBase::Ptr map_base_round_trip; + MapBase::Ptr map_base_round_trip; ASSERT_TRUE(convert::rosMsgToMap(map_msg, map_base_round_trip)); ASSERT_TRUE(map_base_round_trip); diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h index dcb6248ea..da91a1e0a 100644 --- a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h +++ b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h @@ -3,11 +3,11 @@ #include -#include +#include namespace wavemap::rviz_plugin { struct MapAndMutex { - VolumetricDataStructureBase::Ptr map; + MapBase::Ptr map; std::mutex mutex; }; } // namespace wavemap::rviz_plugin diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h index abd68545d..3dfe6a2e2 100644 --- a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h +++ b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h @@ -27,7 +27,7 @@ class CellSelector : public QObject { void initializePropertyMenu(); - void setMap(const VolumetricDataStructureBase::ConstPtr& map); + void setMap(const MapBase::ConstPtr& map); bool shouldBeDrawn(const OctreeIndex& cell_index, FloatingPoint cell_log_odds) const; diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h index 2c1d2fc5b..afa59f28b 100644 --- a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h +++ b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h @@ -15,7 +15,7 @@ #include #include #include -#include +#include #include "wavemap_rviz_plugin/common.h" #include "wavemap_rviz_plugin/visuals/cell_layer.h" diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h index 5a40d8540..3cf523247 100644 --- a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h +++ b/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include "wavemap_rviz_plugin/common.h" diff --git a/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc b/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc index fc452f286..5afddca39 100644 --- a/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc +++ b/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc @@ -50,7 +50,7 @@ void CellSelector::initializePropertyMenu() { CellSelectionMode::kBand); } -void CellSelector::setMap(const VolumetricDataStructureBase::ConstPtr& map) { +void CellSelector::setMap(const MapBase::ConstPtr& map) { const auto hashed_map = std::dynamic_pointer_cast(map); if (hashed_map) { diff --git a/ros/wavemap_rviz_plugin/src/visuals/slice_visual.cc b/ros/wavemap_rviz_plugin/src/visuals/slice_visual.cc index f3bc29ef0..5b0656b5c 100644 --- a/ros/wavemap_rviz_plugin/src/visuals/slice_visual.cc +++ b/ros/wavemap_rviz_plugin/src/visuals/slice_visual.cc @@ -68,7 +68,7 @@ void SliceVisual::update() { // Lock the map mutex, to ensure it doesn't get written to while we read it std::scoped_lock lock(map_and_mutex_->mutex); - const VolumetricDataStructureBase::ConstPtr map = map_and_mutex_->map; + const MapBase::ConstPtr map = map_and_mutex_->map; if (!map) { ROS_INFO("Map is empty. Nothing to draw."); return; diff --git a/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc b/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc index 807adc7cc..25fe4a4b0 100644 --- a/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc +++ b/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc @@ -101,7 +101,7 @@ void VoxelVisual::updateMap(bool redraw_all) { // Lock the map mutex, to ensure it doesn't get written to while we read it { std::scoped_lock lock(map_and_mutex_->mutex); - VolumetricDataStructureBase::ConstPtr map = map_and_mutex_->map; + MapBase::ConstPtr map = map_and_mutex_->map; if (!map) { return; } @@ -188,7 +188,7 @@ void VoxelVisual::updateLOD(const Ogre::Camera& active_camera) { // Lock to the map mutex, to ensure it doesn't get written to while we read it std::scoped_lock lock(map_and_mutex_->mutex); - VolumetricDataStructureBase::ConstPtr map = map_and_mutex_->map; + MapBase::ConstPtr map = map_and_mutex_->map; if (!map) { return; } @@ -409,7 +409,7 @@ void VoxelVisual::processBlockUpdateQueue(const Point3D& camera_position) { // Get a shared-access lock to the map, // to ensure it doesn't get written to while we read it std::scoped_lock lock(map_and_mutex_->mutex); - VolumetricDataStructureBase::ConstPtr map = map_and_mutex_->map; + MapBase::ConstPtr map = map_and_mutex_->map; if (!map) { return; } From 6b6f74633852a2c22ca56aa7e3be894d2d77672d Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 29 Dec 2023 12:34:04 +0100 Subject: [PATCH 052/159] Refactor tests --- libraries/wavemap/CMakeLists.txt | 8 ++++---- .../ray_tracing/ray_tracing_integrator.h | 4 ++-- libraries/wavemap/src/map/map_base.cc | 2 +- .../{data_structure => map}/test_haar_cell.cc | 0 .../test_hashed_blocks.cc | 2 +- .../test_map.cc} | 20 +++++++++---------- .../test_volumetric_octree.cc | 2 +- .../test/src/test_file_conversions.cc | 6 +++--- .../wavemap_ros/input_handler/input_handler.h | 3 +-- .../test/src/test_map_msg_conversions.cc | 6 +++--- 10 files changed, 26 insertions(+), 27 deletions(-) rename libraries/wavemap/test/src/{data_structure => map}/test_haar_cell.cc (100%) rename libraries/wavemap/test/src/{data_structure => map}/test_hashed_blocks.cc (94%) rename libraries/wavemap/test/src/{data_structure/test_volumetric_data_structure.cc => map/test_map.cc} (93%) rename libraries/wavemap/test/src/{data_structure => map}/test_volumetric_octree.cc (99%) diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index c03892b9b..b065c15ac 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -91,13 +91,9 @@ if (CATKIN_ENABLE_TESTING) catkin_add_gtest( test_${PROJECT_NAME} test/src/data_structure/test_aabb.cc - test/src/data_structure/test_haar_cell.cc - test/src/data_structure/test_hashed_blocks.cc test/src/data_structure/test_image.cc test/src/data_structure/test_ndtree.cc test/src/data_structure/test_pointcloud.cc - test/src/data_structure/test_volumetric_data_structure.cc - test/src/data_structure/test_volumetric_octree.cc test/src/data_structure/test_sparse_vector.cc test/src/indexing/test_index_conversions.cc test/src/indexing/test_ndtree_index.cc @@ -111,6 +107,10 @@ if (CATKIN_ENABLE_TESTING) test/src/iterator/test_grid_iterator.cc test/src/iterator/test_ray_iterator.cc test/src/iterator/test_subtree_iterator.cc + test/src/map/test_haar_cell.cc + test/src/map/test_hashed_blocks.cc + test/src/map/test_map.cc + test/src/map/test_volumetric_octree.cc test/src/utils/test_adjacency.cc test/src/utils/test_approximate_trigonometry.cc test/src/utils/test_bit_manipulation.cc diff --git a/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h b/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h index b41468cd0..580136cdc 100644 --- a/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h +++ b/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h @@ -29,7 +29,7 @@ struct RayTracingIntegratorConfig : ConfigBase { class RayTracingIntegrator : public IntegratorBase { public: RayTracingIntegrator(const RayTracingIntegratorConfig& config, - VolumetricDataStructureBase::Ptr occupancy_map) + MapBase::Ptr occupancy_map) : config_(config.checkValid()), occupancy_map_(std::move(CHECK_NOTNULL(occupancy_map))) {} @@ -39,7 +39,7 @@ class RayTracingIntegrator : public IntegratorBase { using MeasurementModelType = ConstantRay; const RayTracingIntegratorConfig config_; - const VolumetricDataStructureBase::Ptr occupancy_map_; + const MapBase::Ptr occupancy_map_; }; } // namespace wavemap diff --git a/libraries/wavemap/src/map/map_base.cc b/libraries/wavemap/src/map/map_base.cc index 71b714f97..07d29c5d5 100644 --- a/libraries/wavemap/src/map/map_base.cc +++ b/libraries/wavemap/src/map/map_base.cc @@ -3,7 +3,7 @@ #include "wavemap/utils/meta/nameof.h" namespace wavemap { -DECLARE_CONFIG_MEMBERS(VolumetricDataStructureConfig, +DECLARE_CONFIG_MEMBERS(MapBaseConfig, (min_cell_width) (min_log_odds) (max_log_odds)); diff --git a/libraries/wavemap/test/src/data_structure/test_haar_cell.cc b/libraries/wavemap/test/src/map/test_haar_cell.cc similarity index 100% rename from libraries/wavemap/test/src/data_structure/test_haar_cell.cc rename to libraries/wavemap/test/src/map/test_haar_cell.cc diff --git a/libraries/wavemap/test/src/data_structure/test_hashed_blocks.cc b/libraries/wavemap/test/src/map/test_hashed_blocks.cc similarity index 94% rename from libraries/wavemap/test/src/data_structure/test_hashed_blocks.cc rename to libraries/wavemap/test/src/map/test_hashed_blocks.cc index d746e164b..403a6f99a 100644 --- a/libraries/wavemap/test/src/data_structure/test_hashed_blocks.cc +++ b/libraries/wavemap/test/src/map/test_hashed_blocks.cc @@ -9,7 +9,7 @@ namespace wavemap { class HashedBlocksTest : public FixtureBase, public GeometryGenerator {}; // NOTE: Insertion tests are performed as part of the test suite for the -// VolumetricDataStructure interface. +// Map interface. TEST_F(HashedBlocksTest, Initialization) { const FloatingPoint random_min_cell_width = getRandomMinCellWidth(); diff --git a/libraries/wavemap/test/src/data_structure/test_volumetric_data_structure.cc b/libraries/wavemap/test/src/map/test_map.cc similarity index 93% rename from libraries/wavemap/test/src/data_structure/test_volumetric_data_structure.cc rename to libraries/wavemap/test/src/map/test_map.cc index 4f31eee90..3f5ca6980 100644 --- a/libraries/wavemap/test/src/data_structure/test_volumetric_data_structure.cc +++ b/libraries/wavemap/test/src/map/test_map.cc @@ -14,20 +14,20 @@ #include "wavemap/test/geometry_generator.h" namespace wavemap { -template -class VolumetricDataStructureTest : public FixtureBase, - public GeometryGenerator, - public ConfigGenerator { +template +class MapTest : public FixtureBase, + public GeometryGenerator, + public ConfigGenerator { protected: static constexpr FloatingPoint kAcceptableReconstructionError = 5e-2f; }; -using VolumetricDataStructureTypes = +using MapTypes = ::testing::Types; -TYPED_TEST_SUITE(VolumetricDataStructureTest, VolumetricDataStructureTypes, ); +TYPED_TEST_SUITE(MapTest, MapTypes, ); -TYPED_TEST(VolumetricDataStructureTest, InitializationAndClearing) { +TYPED_TEST(MapTest, InitializationAndClearing) { const auto config = ConfigGenerator::getRandomConfig(); std::unique_ptr map_base_ptr = std::make_unique(config); @@ -52,7 +52,7 @@ TYPED_TEST(VolumetricDataStructureTest, InitializationAndClearing) { EXPECT_LE(map_base_ptr->getMemoryUsage(), empty_map_memory_usage); } -TYPED_TEST(VolumetricDataStructureTest, Pruning) { +TYPED_TEST(MapTest, Pruning) { const auto config = ConfigGenerator::getRandomConfig(); std::unique_ptr map_base_ptr = std::make_unique(config); @@ -91,7 +91,7 @@ TYPED_TEST(VolumetricDataStructureTest, Pruning) { } } -TYPED_TEST(VolumetricDataStructureTest, MinMaxIndexGetters) { +TYPED_TEST(MapTest, MinMaxIndexGetters) { constexpr int kNumRepetitions = 3; for (int i = 0; i < kNumRepetitions; ++i) { const auto config = @@ -123,7 +123,7 @@ TYPED_TEST(VolumetricDataStructureTest, MinMaxIndexGetters) { } } -TYPED_TEST(VolumetricDataStructureTest, InsertionAndLeafVisitor) { +TYPED_TEST(MapTest, InsertionAndLeafVisitor) { constexpr int kNumRepetitions = 3; for (int i = 0; i < kNumRepetitions; ++i) { // Create a random map diff --git a/libraries/wavemap/test/src/data_structure/test_volumetric_octree.cc b/libraries/wavemap/test/src/map/test_volumetric_octree.cc similarity index 99% rename from libraries/wavemap/test/src/data_structure/test_volumetric_octree.cc rename to libraries/wavemap/test/src/map/test_volumetric_octree.cc index e0221964b..43382aaf0 100644 --- a/libraries/wavemap/test/src/data_structure/test_volumetric_octree.cc +++ b/libraries/wavemap/test/src/map/test_volumetric_octree.cc @@ -12,7 +12,7 @@ class VolumetricOctreeTest : public FixtureBase, public ConfigGenerator {}; // NOTE: Insertion tests are performed as part of the test suite for the -// VolumetricDataStructure interface. +// Map interface. // TODO(victorr): Test whether out of bounds accesses/insertions are handled // correctly (e.g. throw error or do nothing and print error). diff --git a/libraries/wavemap_io/test/src/test_file_conversions.cc b/libraries/wavemap_io/test/src/test_file_conversions.cc index e953b8ba0..14329c670 100644 --- a/libraries/wavemap_io/test/src/test_file_conversions.cc +++ b/libraries/wavemap_io/test/src/test_file_conversions.cc @@ -11,7 +11,7 @@ #include "wavemap_io/file_conversions.h" namespace wavemap { -template +template class FileConversionsTest : public FixtureBase, public GeometryGenerator, public ConfigGenerator { @@ -20,10 +20,10 @@ class FileConversionsTest : public FixtureBase, static constexpr auto kTemporaryFilePath = "/tmp/tmp.wvmp"; }; -using VolumetricDataStructureTypes = +using MapTypes = ::testing::Types; -TYPED_TEST_SUITE(FileConversionsTest, VolumetricDataStructureTypes, ); +TYPED_TEST_SUITE(FileConversionsTest, MapTypes, ); TYPED_TEST(FileConversionsTest, MetadataPreservation) { const auto config = diff --git a/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler.h b/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler.h index 0273b109a..2f83f7c8b 100644 --- a/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler.h +++ b/ros/wavemap_ros/include/wavemap_ros/input_handler/input_handler.h @@ -57,8 +57,7 @@ struct InputHandlerConfig : public ConfigBase { class InputHandler { public: InputHandler(const InputHandlerConfig& config, const param::Value& params, - std::string world_frame, - VolumetricDataStructureBase::Ptr occupancy_map, + std::string world_frame, MapBase::Ptr occupancy_map, std::shared_ptr transformer, std::shared_ptr thread_pool, const ros::NodeHandle& nh, ros::NodeHandle nh_private); diff --git a/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc b/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc index f482d2ad3..f94bc8407 100644 --- a/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc +++ b/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc @@ -12,7 +12,7 @@ #include "wavemap_ros_conversions/map_msg_conversions.h" namespace wavemap { -template +template class MapMsgConversionsTest : public FixtureBase, public GeometryGenerator, public ConfigGenerator { @@ -27,10 +27,10 @@ class MapMsgConversionsTest : public FixtureBase, static constexpr FloatingPoint kAcceptableReconstructionError = 5e-2f; }; -using VolumetricDataStructureTypes = +using MapTypes = ::testing::Types; -TYPED_TEST_SUITE(MapMsgConversionsTest, VolumetricDataStructureTypes, ); +TYPED_TEST_SUITE(MapMsgConversionsTest, MapTypes, ); TYPED_TEST(MapMsgConversionsTest, MetadataPreservation) { const auto config = From 94e90a32d203caf19919a76003b79d647aa691d9 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 5 Jan 2024 14:51:24 +0100 Subject: [PATCH 053/159] Add option to only publish points for changed blocks --- .../operations/publish_pointcloud_operation.h | 7 +- .../publish_pointcloud_operation.cc | 81 ++++++++++++++----- 2 files changed, 64 insertions(+), 24 deletions(-) diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h b/ros/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h index 184744f49..ee7c6ed37 100644 --- a/ros/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h +++ b/ros/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h @@ -4,6 +4,7 @@ #include #include +#include #include #include #include @@ -16,13 +17,15 @@ namespace wavemap { * Config struct for obstacle pointcloud publishing operations. */ struct PublishPointcloudOperationConfig - : public ConfigBase { + : public ConfigBase { //! Time period controlling how often the pointcloud is published. Seconds once_every = 1.f; //! Threshold in log odds above which cells are classified as occupied. FloatingPoint occupancy_threshold_log_odds = 1e-3f; + bool only_publish_changed_blocks = true; + //! Name of the topic the pointcloud will be published on. std::string topic = "obstacle_pointcloud"; @@ -62,7 +65,7 @@ class PublishPointcloudOperation : public OperationBase { // Pointcloud publishing ros::Publisher pointcloud_pub_; Timestamp last_run_timestamp_internal_; - void publishPointcloud(const ros::Time& cell_index); + void publishPointcloud(const ros::Time& current_time); }; } // namespace wavemap diff --git a/ros/wavemap_ros/src/operations/publish_pointcloud_operation.cc b/ros/wavemap_ros/src/operations/publish_pointcloud_operation.cc index 6201cf37f..5377585bf 100644 --- a/ros/wavemap_ros/src/operations/publish_pointcloud_operation.cc +++ b/ros/wavemap_ros/src/operations/publish_pointcloud_operation.cc @@ -4,6 +4,8 @@ #include #include #include +#include +#include #include #include #include @@ -12,6 +14,7 @@ namespace wavemap { DECLARE_CONFIG_MEMBERS(PublishPointcloudOperationConfig, (once_every) (occupancy_threshold_log_odds) + (only_publish_changed_blocks) (topic)); bool PublishPointcloudOperationConfig::isValid(bool verbose) const { @@ -46,31 +49,65 @@ void PublishPointcloudOperation::publishPointcloud( obstacle_pointcloud_msg.header.frame_id = world_frame_; obstacle_pointcloud_msg.header.stamp = current_time; - // Add the points - occupancy_map_->forEachLeaf([&point_msg_vector = - obstacle_pointcloud_msg.points, - cell_width = occupancy_map_->getMinCellWidth(), - occupancy_threshold = - config_.occupancy_threshold_log_odds]( - const OctreeIndex& node_index, - FloatingPoint node_log_odds) { - if (occupancy_threshold < node_log_odds) { - if (node_index.height == 0) { - const Point3D center = - convert::indexToCenterPoint(node_index.position, cell_width); - point_msg_vector.emplace_back(convert::point3DToPoint32Msg(center)); - } else { - const Index3D node_min_corner = - convert::nodeIndexToMinCornerIndex(node_index); - const Index3D node_max_corner = - convert::nodeIndexToMaxCornerIndex(node_index); - for (const Index3D& index : Grid(node_min_corner, node_max_corner)) { - const Point3D center = convert::indexToCenterPoint(index, cell_width); - point_msg_vector.emplace_back(convert::point3DToPoint32Msg(center)); + // Define a functor that converts map leaf nodes into pointcloud points + auto add_points_for_leaf_node = + [min_cell_width = occupancy_map_->getMinCellWidth(), + occupancy_threshold = config_.occupancy_threshold_log_odds, + &point_msg_vector = obstacle_pointcloud_msg.points]( + const OctreeIndex& node_index, FloatingPoint node_log_odds) { + if (occupancy_threshold < node_log_odds) { + if (node_index.height == 0) { + const Point3D center = convert::indexToCenterPoint( + node_index.position, min_cell_width); + point_msg_vector.emplace_back(convert::point3DToPoint32Msg(center)); + } else { + const Index3D node_min_corner = + convert::nodeIndexToMinCornerIndex(node_index); + const Index3D node_max_corner = + convert::nodeIndexToMaxCornerIndex(node_index); + for (const Index3D& index : + Grid(node_min_corner, node_max_corner)) { + const Point3D center = + convert::indexToCenterPoint(index, min_cell_width); + point_msg_vector.emplace_back( + convert::point3DToPoint32Msg(center)); + } + } } + }; + + // Call the functor for each leaf + const Timestamp start_time_internal = Time::now(); + if (const auto* hashed_wavelet_octree = + dynamic_cast(occupancy_map_.get()); + hashed_wavelet_octree) { + // For hashed maps, we only process blocks that change + for (const auto& [block_index, block] : + hashed_wavelet_octree->getBlocks()) { + const bool block_changed = + last_run_timestamp_internal_ < block.getLastUpdatedStamp(); + if (!config_.only_publish_changed_blocks || block_changed) { + block.forEachLeaf(block_index, add_points_for_leaf_node); + } + } + } else if (const auto* hashed_chunked_wavelet_octree = + dynamic_cast( + occupancy_map_.get()); + hashed_chunked_wavelet_octree) { + // For hashed maps, we only process blocks that change + for (const auto& [block_index, block] : + hashed_chunked_wavelet_octree->getBlocks()) { + const bool block_changed = + last_run_timestamp_internal_ < block.getLastUpdatedStamp(); + if (!config_.only_publish_changed_blocks || block_changed) { + block.forEachLeaf(block_index, add_points_for_leaf_node); } } - }); + } else { + // For all other map types, simply process all leaves (fallback) + occupancy_map_->forEachLeaf(add_points_for_leaf_node); + } + last_run_timestamp_internal_ = start_time_internal; // Publish the msg sensor_msgs::PointCloud2 obstacle_pointcloud2_msg; From 4253df7704149a6f2cbad504c396bf289ac38ef2 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 10 Jan 2024 12:33:34 +0100 Subject: [PATCH 054/159] Refactor OccupancyClassifier and implement ClassifiedMap --- libraries/wavemap/CMakeLists.txt | 1 + .../impl/ndtree_block_hash_inl.h | 20 +++---- .../data_structure/ndtree_block_hash.h | 6 +-- .../wavemap/map/hashed_wavelet_octree.h | 6 +-- .../include/wavemap/map/wavelet_octree.h | 2 +- .../utils/neighbors/impl/adjacency_inl.h | 2 +- .../wavemap/utils/query/classified_map.h | 53 +++++++++++++++++++ .../wavemap/utils/query/impl/occupancy_inl.h | 16 ++++++ .../include/wavemap/utils/query/occupancy.h | 24 +++++++++ .../utils/query/occupancy_classifier.h | 48 +++++++++++------ .../wavemap/utils/query/query_accelerator.h | 20 ++----- .../wavemap/src/utils/query/classified_map.cc | 51 ++++++++++++++++++ .../src/utils/query/query_accelerator.cc | 9 ++-- .../utils/sdf/full_euclidean_sdf_generator.cc | 10 ++-- .../sdf/quasi_euclidean_sdf_generator.cc | 10 ++-- .../test/src/utils/test_query_accelerator.cc | 10 ++-- .../test/src/utils/test_sdf_generation.cc | 5 +- 17 files changed, 219 insertions(+), 74 deletions(-) create mode 100644 libraries/wavemap/include/wavemap/utils/query/classified_map.h create mode 100644 libraries/wavemap/include/wavemap/utils/query/impl/occupancy_inl.h create mode 100644 libraries/wavemap/include/wavemap/utils/query/occupancy.h create mode 100644 libraries/wavemap/src/utils/query/classified_map.cc diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index b065c15ac..3caac3e68 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -66,6 +66,7 @@ add_library(${PROJECT_NAME} src/map/wavelet_octree.cc src/map/map_base.cc src/map/map_factory.cc + src/utils/query/classified_map.cc src/utils/query/query_accelerator.cc src/utils/sdf/full_euclidean_sdf_generator.cc src/utils/sdf/quasi_euclidean_sdf_generator.cc diff --git a/libraries/wavemap/include/wavemap/data_structure/impl/ndtree_block_hash_inl.h b/libraries/wavemap/include/wavemap/data_structure/impl/ndtree_block_hash_inl.h index 291397bd4..b9f74e8bb 100644 --- a/libraries/wavemap/include/wavemap/data_structure/impl/ndtree_block_hash_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/impl/ndtree_block_hash_inl.h @@ -42,7 +42,7 @@ template inline typename NdtreeBlockHash::Block& NdtreeBlockHash::getOrAllocateBlock( const Index& block_index) { - return block_map_.getOrAllocateBlock(block_index, tree_height_, + return block_map_.getOrAllocateBlock(block_index, max_height_, default_value_); } @@ -105,7 +105,7 @@ NdtreeBlockHash::getNodeOrAncestor( const NdtreeIndex cell_index = indexToCellIndex(index); return block->getNodeOrAncestor(cell_index); } - return {nullptr, tree_height_}; + return {nullptr, max_height_}; } template @@ -189,7 +189,7 @@ void NdtreeBlockHash::forEachLeaf( Node& node; }; - block_map_.forEachBlock([&visitor_fn, tree_height = tree_height_]( + block_map_.forEachBlock([&visitor_fn, tree_height = max_height_]( const Index& block_index, Block& block) { std::stack stack; stack.emplace(StackElement{OctreeIndex{tree_height, block_index}, @@ -223,7 +223,7 @@ void NdtreeBlockHash::forEachLeaf( const Node& node; }; - block_map_.forEachBlock([&visitor_fn, tree_height = tree_height_]( + block_map_.forEachBlock([&visitor_fn, tree_height = max_height_]( const Index& block_index, const Block& block) { std::stack stack; @@ -256,8 +256,8 @@ template inline Index NdtreeBlockHash::indexToBlockIndex( const NdtreeIndex& index) const { DCHECK_GE(index.height, 0); - DCHECK_LE(index.height, tree_height_); - const IndexElement depth = tree_height_ - index.height; + DCHECK_LE(index.height, max_height_); + const IndexElement depth = max_height_ - index.height; return int_math::div_exp2_floor(index.position, depth); } @@ -265,8 +265,8 @@ template inline NdtreeIndex NdtreeBlockHash::indexToCellIndex( const NdtreeIndex& index) const { DCHECK_GE(index.height, 0); - DCHECK_LE(index.height, tree_height_); - const IndexElement depth = tree_height_ - index.height; + DCHECK_LE(index.height, max_height_); + const IndexElement depth = max_height_ - index.height; return {index.height, int_math::div_exp2_floor_remainder(index.position, depth)}; } @@ -276,8 +276,8 @@ inline NdtreeIndex NdtreeBlockHash::cellAndBlockIndexToIndex( const Index& block_index, const NdtreeIndex& cell_index) const { DCHECK_GE(cell_index.height, 0); - DCHECK_LE(cell_index.height, tree_height_); - const IndexElement depth = tree_height_ - cell_index.height; + DCHECK_LE(cell_index.height, max_height_); + const IndexElement depth = max_height_ - cell_index.height; const IndexElement cells_per_side_at_height = int_math::exp2(depth); return {cell_index.height, cell_index.position + cells_per_side_at_height * block_index}; diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h b/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h index 59fc06457..30362080c 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h +++ b/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h @@ -23,13 +23,13 @@ class NdtreeBlockHash { explicit NdtreeBlockHash(HeightType max_tree_height, CellDataT default_value = CellDataT{}) - : tree_height_(max_tree_height), default_value_(default_value) {} + : max_height_(max_tree_height), default_value_(default_value) {} bool empty() const { return block_map_.empty(); } size_t size() const; void clear() { block_map_.clear(); } - HeightType getMaxHeight() const { return tree_height_; } + HeightType getMaxHeight() const { return max_height_; } bool hasBlock(const Index& block_index) const; bool eraseBlock(const Index& block_index); @@ -77,7 +77,7 @@ class NdtreeBlockHash { void forEachLeaf(IndexedLeafVisitorFunction visitor_fn) const; protected: - const HeightType tree_height_; + const HeightType max_height_; const CellDataT default_value_; BlockHashMap block_map_; diff --git a/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h b/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h index 722958d06..b00a8bb15 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h +++ b/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h @@ -115,15 +115,15 @@ class HashedWaveletOctree : public MapBase { void forEachLeaf( typename MapBase::IndexedLeafVisitorFunction visitor_fn) const override; + BlockIndex indexToBlockIndex(const OctreeIndex& node_index) const; + CellIndex indexToCellIndex(OctreeIndex index) const; + private: const HashedWaveletOctreeConfig config_; const IndexElement cells_per_block_side_ = int_math::exp2(config_.tree_height); BlockHashMap block_map_; - - BlockIndex indexToBlockIndex(const OctreeIndex& node_index) const; - CellIndex indexToCellIndex(OctreeIndex index) const; }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/map/wavelet_octree.h b/libraries/wavemap/include/wavemap/map/wavelet_octree.h index 61e979d14..f55fcec4d 100644 --- a/libraries/wavemap/include/wavemap/map/wavelet_octree.h +++ b/libraries/wavemap/include/wavemap/map/wavelet_octree.h @@ -107,7 +107,7 @@ class WaveletOctree : public MapBase { private: const WaveletOctreeConfig config_; - Ndtree ndtree_{config_.tree_height - 1}; + Octree ndtree_{config_.tree_height - 1}; Coefficients::Scale root_scale_coefficient_{}; OctreeIndex getInternalRootNodeIndex() const { diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/impl/adjacency_inl.h b/libraries/wavemap/include/wavemap/utils/neighbors/impl/adjacency_inl.h index fffd68eaf..1d5549952 100644 --- a/libraries/wavemap/include/wavemap/utils/neighbors/impl/adjacency_inl.h +++ b/libraries/wavemap/include/wavemap/utils/neighbors/impl/adjacency_inl.h @@ -21,7 +21,7 @@ constexpr Adjacency::Mask Adjacency::toMask(Adjacency::Id type_id) { template constexpr Adjacency::Mask Adjacency::toMask() const { - return toMask(id_); + return toMask(static_cast(id_)); } } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/utils/query/classified_map.h b/libraries/wavemap/include/wavemap/utils/query/classified_map.h new file mode 100644 index 000000000..5e8237897 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/query/classified_map.h @@ -0,0 +1,53 @@ +#ifndef WAVEMAP_UTILS_QUERY_CLASSIFIED_MAP_H_ +#define WAVEMAP_UTILS_QUERY_CLASSIFIED_MAP_H_ + +#include +#include +#include +#include + +namespace wavemap { +class ClassifiedMap { + public: + struct NodeData { + std::bitset has_free; + std::bitset has_occupied; + std::bitset has_unobserved; + }; + using BlockHashMap = OctreeBlockHash; + using Node = BlockHashMap::Node; + static constexpr int kDim = 3; + + ClassifiedMap(FloatingPoint min_cell_width, IndexElement tree_height, + const OccupancyClassifier& classifier) + : min_cell_width_(min_cell_width), + classifier_(classifier), + block_map_(tree_height) {} + + ClassifiedMap(const HashedWaveletOctree& occupancy_map, + const OccupancyClassifier& classifier) + : ClassifiedMap(occupancy_map.getMinCellWidth(), + occupancy_map.getTreeHeight(), classifier) { + update(occupancy_map); + } + + FloatingPoint getMinCellWidth() const { return min_cell_width_; } + IndexElement getTreeHeight() const { return block_map_.getMaxHeight(); } + + void update(const HashedWaveletOctree& occupancy_map); + + template + void forEachOccupiedLeaf(VisitorFn visitor_fn) const {} + + private: + const FloatingPoint min_cell_width_; + const OccupancyClassifier classifier_; + BlockHashMap block_map_; + + void recursiveClassifier( + const HashedWaveletOctreeBlock::NodeType& occupancy_node, + FloatingPoint average_occupancy, Node& classified_node); +}; +} // namespace wavemap + +#endif // WAVEMAP_UTILS_QUERY_CLASSIFIED_MAP_H_ diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_inl.h new file mode 100644 index 000000000..07b9c20f1 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_inl.h @@ -0,0 +1,16 @@ +#ifndef WAVEMAP_UTILS_QUERY_IMPL_OCCUPANCY_INL_H_ +#define WAVEMAP_UTILS_QUERY_IMPL_OCCUPANCY_INL_H_ + +namespace wavemap { +constexpr Occupancy::Mask Occupancy::toMask(Occupancy::Id type_id) { + DCHECK(0 <= type_id); + DCHECK(type_id < static_cast(names.size())); + if (type_id < 3) { + return 1 << type_id; + } else { + return 0b011; + } +} +} // namespace wavemap + +#endif // WAVEMAP_UTILS_QUERY_IMPL_OCCUPANCY_INL_H_ diff --git a/libraries/wavemap/include/wavemap/utils/query/occupancy.h b/libraries/wavemap/include/wavemap/utils/query/occupancy.h new file mode 100644 index 000000000..55f9436d5 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/query/occupancy.h @@ -0,0 +1,24 @@ +#ifndef WAVEMAP_UTILS_QUERY_OCCUPANCY_H_ +#define WAVEMAP_UTILS_QUERY_OCCUPANCY_H_ + +#include "wavemap/common.h" +#include "wavemap/config/type_selector.h" + +namespace wavemap { +struct Occupancy : TypeSelector { + using TypeSelector::TypeSelector; + using Mask = uint8_t; + + enum Id : TypeId { kFree, kOccupied, kUnobserved, kObserved }; + static constexpr std::array names = {"free", "occupied", "unobserved", + "observed"}; + + // NOTE: For usage examples, please refer to the OccupancyClassifier class. + static constexpr Mask toMask(Id type_id); + constexpr Mask toMask() const { return toMask(static_cast(id_)); } +}; +} // namespace wavemap + +#include "wavemap/utils/query/impl/occupancy_inl.h" + +#endif // WAVEMAP_UTILS_QUERY_OCCUPANCY_H_ diff --git a/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h b/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h index 96cedbc0b..358f0eca7 100644 --- a/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h +++ b/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h @@ -1,36 +1,54 @@ #ifndef WAVEMAP_UTILS_QUERY_OCCUPANCY_CLASSIFIER_H_ #define WAVEMAP_UTILS_QUERY_OCCUPANCY_CLASSIFIER_H_ +#include "wavemap/utils/query/occupancy.h" + namespace wavemap { class OccupancyClassifier { public: explicit OccupancyClassifier(FloatingPoint log_odds_occupancy_threshold = 0.f) : occupancy_threshold_(log_odds_occupancy_threshold) {} - static bool isUnobserved(FloatingPoint log_odds_occupancy_value) { - return std::abs(log_odds_occupancy_value) < 1e-3f; + static constexpr bool isUnobserved(FloatingPoint log_odds_occupancy) { + return std::abs(log_odds_occupancy) < kUnobservedThreshold; } - static bool isObserved(FloatingPoint log_odds_occupancy_value) { - return !isUnobserved(log_odds_occupancy_value); + static constexpr bool isObserved(FloatingPoint log_odds_occupancy) { + return !isUnobserved(log_odds_occupancy); } - bool isFree(FloatingPoint log_odds_occupancy_value) const { - return log_odds_occupancy_value < occupancy_threshold_; - } - bool isOccupied(FloatingPoint log_odds_occupancy_value) const { - return occupancy_threshold_ < log_odds_occupancy_value; + constexpr bool is(FloatingPoint log_odds_occupancy, + Occupancy::Id occupancy_type) const { + switch (occupancy_type) { + case Occupancy::kFree: + return isObserved(log_odds_occupancy) && + log_odds_occupancy < occupancy_threshold_; + case Occupancy::kOccupied: + return isObserved(log_odds_occupancy) && + occupancy_threshold_ < log_odds_occupancy; + case Occupancy::kUnobserved: + return isUnobserved(log_odds_occupancy); + case Occupancy::kObserved: + return isObserved(log_odds_occupancy); + default: + return false; + } } - bool isObservedAndFree(FloatingPoint log_odds_occupancy_value) const { - return isObserved(log_odds_occupancy_value) && - isFree(log_odds_occupancy_value); + static constexpr bool has(Occupancy::Mask region_occupancy, + Occupancy::Id occupancy_type) { + return region_occupancy & Occupancy::toMask(occupancy_type); } - bool isObservedAndOccupied(FloatingPoint log_odds_occupancy_value) const { - return isObserved(log_odds_occupancy_value) && - isOccupied(log_odds_occupancy_value); + + static constexpr bool isFully(Occupancy::Mask region_occupancy, + Occupancy::Id occupancy_type) { + return region_occupancy == Occupancy::toMask(occupancy_type); } + static FloatingPoint getUnobservedThreshold() { return kUnobservedThreshold; } + FloatingPoint getOccupancyThreshold() const { return occupancy_threshold_; } + private: + static constexpr FloatingPoint kUnobservedThreshold = 1e-3f; const FloatingPoint occupancy_threshold_; }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h b/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h index da5de15df..622d2c410 100644 --- a/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h +++ b/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h @@ -70,8 +70,7 @@ class QueryAccelerator { public: static constexpr int kDim = HashedWaveletOctree::kDim; - explicit QueryAccelerator(const HashedWaveletOctree& map) - : block_map_(map.getHashMap()), tree_height_(map.getTreeHeight()) {} + explicit QueryAccelerator(const HashedWaveletOctree& map) : map_(map) {} FloatingPoint getCellValue(const Index3D& index) { return getCellValue(OctreeIndex{0, index}); @@ -80,13 +79,11 @@ class QueryAccelerator { FloatingPoint getCellValue(const OctreeIndex& index); private: - using Coefficients = HaarCoefficients; - using Transform = HaarTransform; - using BlockIndex = typename HashedWaveletOctree::BlockIndex; - using NodeType = NdtreeNode; + using BlockIndex = HashedWaveletOctree::BlockIndex; + using NodeType = HashedWaveletOctree::Block::NodeType; - const HashedWaveletOctree::BlockHashMap& block_map_; - const IndexElement tree_height_; + const HashedWaveletOctree& map_; + const IndexElement tree_height_ = map_.getTreeHeight(); std::array> node_stack_{}; std::array> value_stack_{}; @@ -95,13 +92,6 @@ class QueryAccelerator { BlockIndex ::Constant(std::numeric_limits::max()); MortonIndex morton_code_ = std::numeric_limits::max(); IndexElement height_ = tree_height_; - - BlockIndex computeBlockIndexFromIndex(const OctreeIndex& node_index) const { - const BlockIndex index = convert::nodeIndexToMinCornerIndex(node_index); - return int_math::div_exp2_floor(index, tree_height_); - } - - friend class QueryAcceleratorTest_Equivalence_Test; }; } // namespace wavemap diff --git a/libraries/wavemap/src/utils/query/classified_map.cc b/libraries/wavemap/src/utils/query/classified_map.cc new file mode 100644 index 000000000..39d2ff8a3 --- /dev/null +++ b/libraries/wavemap/src/utils/query/classified_map.cc @@ -0,0 +1,51 @@ +#include "wavemap/utils/query/classified_map.h" + +namespace wavemap { +void ClassifiedMap::update(const HashedWaveletOctree& occupancy_map) { + occupancy_map.forEachBlock( + [this](const Index3D& block_index, const auto& occupancy_block) { + auto& classified_block = block_map_.getOrAllocateBlock(block_index); + recursiveClassifier(occupancy_block.getRootNode(), + occupancy_block.getRootScale(), + classified_block.getRootNode()); + }); +} + +void ClassifiedMap::recursiveClassifier( // NOLINT + const HashedWaveletOctreeBlock::NodeType& occupancy_node, + FloatingPoint average_occupancy, ClassifiedMap::Node& classified_node) { + const auto child_occupancies = + HashedWaveletOctree::Block::Transform::backward( + {average_occupancy, occupancy_node.data()}); + for (int child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { + const FloatingPoint child_occupancy = child_occupancies[child_idx]; + // If the node has children, recurse + if (occupancy_node.hasChild(child_idx)) { + const auto* occupancy_child_node = occupancy_node.getChild(child_idx); + auto& classified_child_node = + classified_node.getOrAllocateChild(child_idx); + recursiveClassifier(*occupancy_child_node, child_occupancy, + classified_child_node); + const bool child_has_free = classified_child_node.data().has_free.any(); + const bool child_has_occupied = + classified_child_node.data().has_occupied.any(); + const bool child_has_unobserved = + classified_child_node.data().has_unobserved.any(); + classified_node.data().has_free.set(child_idx, child_has_free); + classified_node.data().has_occupied.set(child_idx, child_has_occupied); + classified_node.data().has_unobserved.set(child_idx, + child_has_unobserved); + if (child_has_free + child_has_occupied + child_has_unobserved == 1) { + classified_node.eraseChild(child_idx); + } + } else { // Otherwise, the node is a leaf + classified_node.data().has_free.set( + child_idx, classifier_.is(child_occupancy, Occupancy::kFree)); + classified_node.data().has_occupied.set( + child_idx, classifier_.is(child_occupancy, Occupancy::kOccupied)); + classified_node.data().has_unobserved.set( + child_idx, classifier_.is(child_occupancy, Occupancy::kUnobserved)); + } + } +} +} // namespace wavemap diff --git a/libraries/wavemap/src/utils/query/query_accelerator.cc b/libraries/wavemap/src/utils/query/query_accelerator.cc index fd59ecbd2..cec86db45 100644 --- a/libraries/wavemap/src/utils/query/query_accelerator.cc +++ b/libraries/wavemap/src/utils/query/query_accelerator.cc @@ -7,7 +7,7 @@ FloatingPoint QueryAccelerator::getCellValue( const BlockIndex previous_block_index = block_index_; const MortonIndex previous_morton_code = morton_code_; const IndexElement previous_height = height_; - block_index_ = computeBlockIndexFromIndex(index); + block_index_ = map_.indexToBlockIndex(index); morton_code_ = convert::nodeIndexToMorton(index); // Check whether we're in the same block as last time @@ -19,7 +19,7 @@ FloatingPoint QueryAccelerator::getCellValue( DCHECK_LE(height_, tree_height_); } else { // Test if the queried block exists - const auto* current_block = block_map_.getBlock(block_index_); + const auto* current_block = map_.getBlock(block_index_); if (current_block) { // If yes, load it node_stack_[tree_height_] = ¤t_block->getRootNode(); @@ -58,8 +58,9 @@ FloatingPoint QueryAccelerator::getCellValue( const NdtreeIndexRelativeChild child_idx = OctreeIndex::computeRelativeChildIndex(morton_code_, height_); --height_; - value_stack_[height_] = Transform::backwardSingleChild( - {parent_value, parent_node->data()}, child_idx); + value_stack_[height_] = + HashedWaveletOctree::Block::Transform::backwardSingleChild( + {parent_value, parent_node->data()}, child_idx); if (height_ == index.height || !parent_node->hasChild(child_idx)) { break; } diff --git a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc index 75190f220..6492ba602 100644 --- a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc @@ -52,8 +52,7 @@ void FullEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, const OctreeIndex& node_index, FloatingPoint node_occupancy) { // Only process obstacles - if (OccupancyClassifier::isUnobserved(node_occupancy) || - classifier_.isFree(node_occupancy)) { + if (!classifier_.is(node_occupancy, Occupancy::kOccupied)) { return; } @@ -79,8 +78,7 @@ void FullEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, // Skip the cell if it is not free const FloatingPoint occupancy = occupancy_query_accelerator.getCellValue(index); - if (OccupancyClassifier::isUnobserved(occupancy) || - classifier_.isOccupied(occupancy)) { + if (!classifier_.is(occupancy, Occupancy::kFree)) { continue; } @@ -145,11 +143,11 @@ void FullEuclideanSDFGenerator::propagate( const FloatingPoint neighbor_occupancy = occupancy_query_accelerator.getCellValue(neighbor_index); // Never initialize or update unknown cells - if (OccupancyClassifier::isUnobserved(neighbor_occupancy)) { + if (classifier_.is(neighbor_occupancy, Occupancy::kUnobserved)) { continue; } // Set the sign - if (classifier_.isOccupied(neighbor_occupancy)) { + if (classifier_.is(neighbor_occupancy, Occupancy::kOccupied)) { neighbor_sdf_value = -sdf.getDefaultValue().distance; } } diff --git a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc index 40b1d5a2b..243f3dbc1 100644 --- a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc @@ -42,8 +42,7 @@ void QuasiEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, const OctreeIndex& node_index, FloatingPoint node_occupancy) { // Only process obstacles - if (OccupancyClassifier::isUnobserved(node_occupancy) || - classifier_.isFree(node_occupancy)) { + if (!classifier_.is(node_occupancy, Occupancy::kOccupied)) { return; } @@ -69,8 +68,7 @@ void QuasiEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, // Skip the cell if it is not free const FloatingPoint occupancy = occupancy_query_accelerator.getCellValue(index); - if (OccupancyClassifier::isUnobserved(occupancy) || - classifier_.isOccupied(occupancy)) { + if (!classifier_.is(occupancy, Occupancy::kFree)) { continue; } @@ -136,11 +134,11 @@ void QuasiEuclideanSDFGenerator::propagate( const FloatingPoint neighbor_occupancy = occupancy_query_accelerator.getCellValue(neighbor_index); // Never initialize or update unknown cells - if (OccupancyClassifier::isUnobserved(neighbor_occupancy)) { + if (classifier_.is(neighbor_occupancy, Occupancy::kUnobserved)) { continue; } // Set the sign - if (classifier_.isOccupied(neighbor_occupancy)) { + if (classifier_.is(neighbor_occupancy, Occupancy::kOccupied)) { neighbor_sdf = -sdf.getDefaultValue(); } } diff --git a/libraries/wavemap/test/src/utils/test_query_accelerator.cc b/libraries/wavemap/test/src/utils/test_query_accelerator.cc index 3821fb4ef..6ef2806f2 100644 --- a/libraries/wavemap/test/src/utils/test_query_accelerator.cc +++ b/libraries/wavemap/test/src/utils/test_query_accelerator.cc @@ -52,16 +52,12 @@ TEST_F(QueryAcceleratorTest, Equivalence) { EXPECT_NEAR(query_accelerator.getCellValue(node_index), map.getCellValue(node_index), kEpsilon) << "For node_index " << node_index.toString() << " (in block" - << print::eigen::oneLine( - query_accelerator.computeBlockIndexFromIndex(node_index)) - << ")" + << print::eigen::oneLine(map.indexToBlockIndex(node_index)) << ")" << ", previous index " << previous_index.toString() << " (in block" - << print::eigen::oneLine( - query_accelerator.computeBlockIndexFromIndex(previous_index)) + << print::eigen::oneLine(map.indexToBlockIndex(previous_index)) << ")" << " tree height " << tree_height << " has block" - << map.hasBlock( - query_accelerator.computeBlockIndexFromIndex(node_index)); + << map.hasBlock(map.indexToBlockIndex(node_index)); previous_index = node_index; } } diff --git a/libraries/wavemap/test/src/utils/test_sdf_generation.cc b/libraries/wavemap/test/src/utils/test_sdf_generation.cc index 86aff0e1a..7835d2abd 100644 --- a/libraries/wavemap/test/src/utils/test_sdf_generation.cc +++ b/libraries/wavemap/test/src/utils/test_sdf_generation.cc @@ -84,7 +84,7 @@ TYPED_TEST(SdfGenerationTest, BruteForceEquivalence) { FloatingPoint sdf_brute_force = sdf.getDefaultValue(); Index3D parent_brute_force = Index3D::Constant(std::numeric_limits::max()); - if (classifier.isFree(occupancy_value)) { + if (classifier.is(occupancy_value, Occupancy::kFree)) { // In free space, the SDF should always be positive EXPECT_GT(sdf_value, 0.f); @@ -106,8 +106,7 @@ TYPED_TEST(SdfGenerationTest, BruteForceEquivalence) { node_index.position.array() + padding)) { const FloatingPoint neighbor_occupancy_value = map.getCellValue(neighbor_index); - if (classifier.isFree(neighbor_occupancy_value) && - OccupancyClassifier::isObserved(neighbor_occupancy_value)) { + if (classifier.is(neighbor_occupancy_value, Occupancy::kFree)) { const auto free_cell_aabb = convert::nodeIndexToAABB( OctreeIndex{0, neighbor_index}, min_cell_width); const FloatingPoint min_dist = From f4b1d81b8efedf9ebafe9efb46792cccc3023094 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 10 Jan 2024 13:55:31 +0100 Subject: [PATCH 055/159] Test occupancy classifier --- libraries/wavemap/CMakeLists.txt | 1 + .../query/impl/occupancy_classifier_inl.h | 48 ++++++ .../utils/query/occupancy_classifier.h | 35 +--- .../src/utils/test_occupancy_classifier.cc | 155 ++++++++++++++++++ 4 files changed, 211 insertions(+), 28 deletions(-) create mode 100644 libraries/wavemap/include/wavemap/utils/query/impl/occupancy_classifier_inl.h create mode 100644 libraries/wavemap/test/src/utils/test_occupancy_classifier.cc diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index 3caac3e68..72d3270ca 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -120,6 +120,7 @@ if (CATKIN_ENABLE_TESTING) test/src/utils/test_grid_neighborhood.cc test/src/utils/test_grid_adjacency.cc test/src/utils/test_ndtree_adjacency.cc + test/src/utils/test_occupancy_classifier.cc test/src/utils/test_int_math.cc test/src/utils/test_log_odds_converter.cc test/src/utils/test_map_interpolator.cpp diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_classifier_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_classifier_inl.h new file mode 100644 index 000000000..7246db0b9 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_classifier_inl.h @@ -0,0 +1,48 @@ +#ifndef WAVEMAP_UTILS_QUERY_IMPL_OCCUPANCY_CLASSIFIER_INL_H_ +#define WAVEMAP_UTILS_QUERY_IMPL_OCCUPANCY_CLASSIFIER_INL_H_ + +namespace wavemap { +constexpr bool OccupancyClassifier::isUnobserved( + FloatingPoint log_odds_occupancy) { + return std::abs(log_odds_occupancy) < kUnobservedThreshold; +} + +constexpr bool OccupancyClassifier::isObserved( + FloatingPoint log_odds_occupancy) { + return !isUnobserved(log_odds_occupancy); +} + +constexpr bool OccupancyClassifier::is(FloatingPoint log_odds_occupancy, + Occupancy::Id occupancy_type) const { + switch (occupancy_type) { + case Occupancy::kFree: + return isObserved(log_odds_occupancy) && + log_odds_occupancy < occupancy_threshold_; + case Occupancy::kOccupied: + return isObserved(log_odds_occupancy) && + occupancy_threshold_ < log_odds_occupancy; + case Occupancy::kUnobserved: + return isUnobserved(log_odds_occupancy); + case Occupancy::kObserved: + return isObserved(log_odds_occupancy); + default: + return false; + } +} + +constexpr bool OccupancyClassifier::has(Occupancy::Mask region_occupancy, + Occupancy::Id occupancy_type) { + // Check that at least one bit in the region matches the mask + return region_occupancy & Occupancy::toMask(occupancy_type); +} + +constexpr bool OccupancyClassifier::isFully(Occupancy::Mask region_occupancy, + Occupancy::Id occupancy_type) { + // Set stray bits beyond mask width to 0, as they should not influence result + const Occupancy::Mask region_occ_trimmed = region_occupancy & 0b111; + // Check that no bits in the region are set while not being in the mask + return !(region_occ_trimmed & ~Occupancy::toMask(occupancy_type)); +} +} // namespace wavemap + +#endif // WAVEMAP_UTILS_QUERY_IMPL_OCCUPANCY_CLASSIFIER_INL_H_ diff --git a/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h b/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h index 358f0eca7..24a488a49 100644 --- a/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h +++ b/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h @@ -9,40 +9,17 @@ class OccupancyClassifier { explicit OccupancyClassifier(FloatingPoint log_odds_occupancy_threshold = 0.f) : occupancy_threshold_(log_odds_occupancy_threshold) {} - static constexpr bool isUnobserved(FloatingPoint log_odds_occupancy) { - return std::abs(log_odds_occupancy) < kUnobservedThreshold; - } - static constexpr bool isObserved(FloatingPoint log_odds_occupancy) { - return !isUnobserved(log_odds_occupancy); - } + static constexpr bool isUnobserved(FloatingPoint log_odds_occupancy); + static constexpr bool isObserved(FloatingPoint log_odds_occupancy); constexpr bool is(FloatingPoint log_odds_occupancy, - Occupancy::Id occupancy_type) const { - switch (occupancy_type) { - case Occupancy::kFree: - return isObserved(log_odds_occupancy) && - log_odds_occupancy < occupancy_threshold_; - case Occupancy::kOccupied: - return isObserved(log_odds_occupancy) && - occupancy_threshold_ < log_odds_occupancy; - case Occupancy::kUnobserved: - return isUnobserved(log_odds_occupancy); - case Occupancy::kObserved: - return isObserved(log_odds_occupancy); - default: - return false; - } - } + Occupancy::Id occupancy_type) const; static constexpr bool has(Occupancy::Mask region_occupancy, - Occupancy::Id occupancy_type) { - return region_occupancy & Occupancy::toMask(occupancy_type); - } + Occupancy::Id occupancy_type); static constexpr bool isFully(Occupancy::Mask region_occupancy, - Occupancy::Id occupancy_type) { - return region_occupancy == Occupancy::toMask(occupancy_type); - } + Occupancy::Id occupancy_type); static FloatingPoint getUnobservedThreshold() { return kUnobservedThreshold; } FloatingPoint getOccupancyThreshold() const { return occupancy_threshold_; } @@ -53,4 +30,6 @@ class OccupancyClassifier { }; } // namespace wavemap +#include "wavemap/utils/query/impl/occupancy_classifier_inl.h" + #endif // WAVEMAP_UTILS_QUERY_OCCUPANCY_CLASSIFIER_H_ diff --git a/libraries/wavemap/test/src/utils/test_occupancy_classifier.cc b/libraries/wavemap/test/src/utils/test_occupancy_classifier.cc new file mode 100644 index 000000000..2a6ad434b --- /dev/null +++ b/libraries/wavemap/test/src/utils/test_occupancy_classifier.cc @@ -0,0 +1,155 @@ +#include + +#include "wavemap/utils/query/occupancy_classifier.h" + +namespace wavemap { +TEST(OccupancyClassifierTest, Initialization) { + // Check that default occupancy threshold is zero + EXPECT_NEAR(OccupancyClassifier{}.getOccupancyThreshold(), 0.f, kEpsilon); + // Check that non-default occupancy thresholds get set correctly + EXPECT_NEAR(OccupancyClassifier{1.f}.getOccupancyThreshold(), 1.f, kEpsilon); +} + +TEST(OccupancyClassifierTest, CellOccupancy) { + // Static observed/unobserved testers + EXPECT_FALSE(OccupancyClassifier::isUnobserved(-1.f)); + EXPECT_TRUE(OccupancyClassifier::isUnobserved(0.f)); + EXPECT_FALSE(OccupancyClassifier::isUnobserved(1.f)); + EXPECT_TRUE(OccupancyClassifier::isObserved(-1.f)); + EXPECT_FALSE(OccupancyClassifier::isObserved(0.f)); + EXPECT_TRUE(OccupancyClassifier::isObserved(1.f)); + + // Non-static methods + // Default threshold + EXPECT_TRUE(OccupancyClassifier{}.is(-1.f, Occupancy::kFree)); + EXPECT_FALSE(OccupancyClassifier{}.is(-1.f, Occupancy::kOccupied)); + EXPECT_FALSE(OccupancyClassifier{}.is(-1.f, Occupancy::kUnobserved)); + EXPECT_TRUE(OccupancyClassifier{}.is(-1.f, Occupancy::kObserved)); + EXPECT_FALSE(OccupancyClassifier{}.is(0.f, Occupancy::kFree)); + EXPECT_FALSE(OccupancyClassifier{}.is(0.f, Occupancy::kOccupied)); + EXPECT_TRUE(OccupancyClassifier{}.is(0.f, Occupancy::kUnobserved)); + EXPECT_FALSE(OccupancyClassifier{}.is(0.f, Occupancy::kObserved)); + EXPECT_FALSE(OccupancyClassifier{}.is(1.f, Occupancy::kFree)); + EXPECT_TRUE(OccupancyClassifier{}.is(1.f, Occupancy::kOccupied)); + EXPECT_FALSE(OccupancyClassifier{}.is(1.f, Occupancy::kUnobserved)); + EXPECT_TRUE(OccupancyClassifier{}.is(1.f, Occupancy::kObserved)); + // Non-default threshold + EXPECT_TRUE(OccupancyClassifier{-1.f}.is(-1.1f, Occupancy::kFree)); + EXPECT_FALSE(OccupancyClassifier{-1.f}.is(-1.1f, Occupancy::kOccupied)); + EXPECT_FALSE(OccupancyClassifier{-1.f}.is(-1.1f, Occupancy::kUnobserved)); + EXPECT_TRUE(OccupancyClassifier{-1.f}.is(-1.1f, Occupancy::kObserved)); + EXPECT_FALSE(OccupancyClassifier{-1.f}.is(-0.9f, Occupancy::kFree)); + EXPECT_TRUE(OccupancyClassifier{-1.f}.is(-0.9f, Occupancy::kOccupied)); + EXPECT_FALSE(OccupancyClassifier{-1.f}.is(-0.9f, Occupancy::kUnobserved)); + EXPECT_TRUE(OccupancyClassifier{-1.f}.is(-0.9f, Occupancy::kObserved)); + EXPECT_FALSE(OccupancyClassifier{-1.f}.is(0.f, Occupancy::kFree)); + EXPECT_FALSE(OccupancyClassifier{-1.f}.is(0.f, Occupancy::kOccupied)); + EXPECT_TRUE(OccupancyClassifier{-1.f}.is(0.f, Occupancy::kUnobserved)); + EXPECT_FALSE(OccupancyClassifier{-1.f}.is(0.f, Occupancy::kObserved)); + EXPECT_FALSE(OccupancyClassifier{-1.f}.is(1.f, Occupancy::kFree)); + EXPECT_TRUE(OccupancyClassifier{-1.f}.is(1.f, Occupancy::kOccupied)); + EXPECT_FALSE(OccupancyClassifier{-1.f}.is(1.f, Occupancy::kUnobserved)); + EXPECT_TRUE(OccupancyClassifier{-1.f}.is(1.f, Occupancy::kObserved)); +} + +TEST(OccupancyClassifierTest, RegionOccupancy) { + constexpr Occupancy::Mask kFullyFree = Occupancy::toMask(Occupancy::kFree); + EXPECT_TRUE(OccupancyClassifier::has(kFullyFree, Occupancy::kFree)); + EXPECT_FALSE(OccupancyClassifier::has(kFullyFree, Occupancy::kOccupied)); + EXPECT_FALSE(OccupancyClassifier::has(kFullyFree, Occupancy::kUnobserved)); + EXPECT_TRUE(OccupancyClassifier::has(kFullyFree, Occupancy::kObserved)); + EXPECT_TRUE(OccupancyClassifier::isFully(kFullyFree, Occupancy::kFree)); + EXPECT_FALSE(OccupancyClassifier::isFully(kFullyFree, Occupancy::kOccupied)); + EXPECT_FALSE( + OccupancyClassifier::isFully(kFullyFree, Occupancy::kUnobserved)); + EXPECT_TRUE(OccupancyClassifier::isFully(kFullyFree, Occupancy::kObserved)); + + constexpr Occupancy::Mask kFullyOccupied = + Occupancy::toMask(Occupancy::kOccupied); + EXPECT_FALSE(OccupancyClassifier::has(kFullyOccupied, Occupancy::kFree)); + EXPECT_TRUE(OccupancyClassifier::has(kFullyOccupied, Occupancy::kOccupied)); + EXPECT_FALSE( + OccupancyClassifier::has(kFullyOccupied, Occupancy::kUnobserved)); + EXPECT_TRUE(OccupancyClassifier::has(kFullyOccupied, Occupancy::kObserved)); + EXPECT_FALSE(OccupancyClassifier::isFully(kFullyOccupied, Occupancy::kFree)); + EXPECT_TRUE( + OccupancyClassifier::isFully(kFullyOccupied, Occupancy::kOccupied)); + EXPECT_FALSE( + OccupancyClassifier::isFully(kFullyOccupied, Occupancy::kUnobserved)); + EXPECT_TRUE( + OccupancyClassifier::isFully(kFullyOccupied, Occupancy::kObserved)); + + constexpr Occupancy::Mask kFullyUnobserved = + Occupancy::toMask(Occupancy::kUnobserved); + EXPECT_FALSE(OccupancyClassifier::has(kFullyUnobserved, Occupancy::kFree)); + EXPECT_FALSE( + OccupancyClassifier::has(kFullyUnobserved, Occupancy::kOccupied)); + EXPECT_TRUE( + OccupancyClassifier::has(kFullyUnobserved, Occupancy::kUnobserved)); + EXPECT_FALSE( + OccupancyClassifier::has(kFullyUnobserved, Occupancy::kObserved)); + EXPECT_FALSE( + OccupancyClassifier::isFully(kFullyUnobserved, Occupancy::kFree)); + EXPECT_FALSE( + OccupancyClassifier::isFully(kFullyUnobserved, Occupancy::kOccupied)); + EXPECT_TRUE( + OccupancyClassifier::isFully(kFullyUnobserved, Occupancy::kUnobserved)); + EXPECT_FALSE( + OccupancyClassifier::isFully(kFullyUnobserved, Occupancy::kObserved)); + + constexpr Occupancy::Mask kFreeOrUnobserved = + Occupancy::toMask(Occupancy::kFree) | + Occupancy::toMask(Occupancy::kUnobserved); + EXPECT_TRUE(OccupancyClassifier::has(kFreeOrUnobserved, Occupancy::kFree)); + EXPECT_FALSE( + OccupancyClassifier::has(kFreeOrUnobserved, Occupancy::kOccupied)); + EXPECT_TRUE( + OccupancyClassifier::has(kFreeOrUnobserved, Occupancy::kUnobserved)); + EXPECT_TRUE( + OccupancyClassifier::has(kFreeOrUnobserved, Occupancy::kObserved)); + EXPECT_FALSE( + OccupancyClassifier::isFully(kFreeOrUnobserved, Occupancy::kFree)); + EXPECT_FALSE( + OccupancyClassifier::isFully(kFreeOrUnobserved, Occupancy::kOccupied)); + EXPECT_FALSE( + OccupancyClassifier::isFully(kFreeOrUnobserved, Occupancy::kUnobserved)); + EXPECT_FALSE( + OccupancyClassifier::isFully(kFreeOrUnobserved, Occupancy::kObserved)); + + constexpr Occupancy::Mask kOccupiedOrUnobserved = + Occupancy::toMask(Occupancy::kOccupied) | + Occupancy::toMask(Occupancy::kUnobserved); + EXPECT_FALSE( + OccupancyClassifier::has(kOccupiedOrUnobserved, Occupancy::kFree)); + EXPECT_TRUE( + OccupancyClassifier::has(kOccupiedOrUnobserved, Occupancy::kOccupied)); + EXPECT_TRUE( + OccupancyClassifier::has(kOccupiedOrUnobserved, Occupancy::kUnobserved)); + EXPECT_TRUE( + OccupancyClassifier::has(kOccupiedOrUnobserved, Occupancy::kObserved)); + EXPECT_FALSE( + OccupancyClassifier::isFully(kOccupiedOrUnobserved, Occupancy::kFree)); + EXPECT_FALSE(OccupancyClassifier::isFully(kOccupiedOrUnobserved, + Occupancy::kOccupied)); + EXPECT_FALSE(OccupancyClassifier::isFully(kOccupiedOrUnobserved, + Occupancy::kUnobserved)); + EXPECT_FALSE(OccupancyClassifier::isFully(kOccupiedOrUnobserved, + Occupancy::kObserved)); + + constexpr Occupancy::Mask kFreeOrOccupied = + Occupancy::toMask(Occupancy::kFree) | + Occupancy::toMask(Occupancy::kOccupied); + EXPECT_TRUE(OccupancyClassifier::has(kFreeOrOccupied, Occupancy::kFree)); + EXPECT_TRUE(OccupancyClassifier::has(kFreeOrOccupied, Occupancy::kOccupied)); + EXPECT_FALSE( + OccupancyClassifier::has(kFreeOrOccupied, Occupancy::kUnobserved)); + EXPECT_TRUE(OccupancyClassifier::has(kFreeOrOccupied, Occupancy::kObserved)); + EXPECT_FALSE(OccupancyClassifier::isFully(kFreeOrOccupied, Occupancy::kFree)); + EXPECT_FALSE( + OccupancyClassifier::isFully(kFreeOrOccupied, Occupancy::kOccupied)); + EXPECT_FALSE( + OccupancyClassifier::isFully(kFreeOrOccupied, Occupancy::kUnobserved)); + EXPECT_TRUE( + OccupancyClassifier::isFully(kFreeOrOccupied, Occupancy::kObserved)); +} +} // namespace wavemap From 49f09700c3f2ca1f5e6f1869af29193516024b9d Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 10 Jan 2024 17:24:00 +0100 Subject: [PATCH 056/159] Add query methods and visitors to ClassifiedMap --- .../data_structure/ndtree_block_hash.h | 10 +- .../wavemap/utils/query/classified_map.h | 20 ++- .../utils/query/impl/classified_map_inl.h | 33 +++++ .../query/impl/occupancy_classifier_inl.h | 14 +- .../wavemap/utils/query/impl/occupancy_inl.h | 4 + .../include/wavemap/utils/query/occupancy.h | 1 + .../utils/query/occupancy_classifier.h | 14 +- .../wavemap/src/utils/query/classified_map.cc | 121 ++++++++++++++++++ 8 files changed, 206 insertions(+), 11 deletions(-) create mode 100644 libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h b/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h index 30362080c..73e6859dc 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h +++ b/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h @@ -76,15 +76,15 @@ class NdtreeBlockHash { template void forEachLeaf(IndexedLeafVisitorFunction visitor_fn) const; - protected: - const HeightType max_height_; - const CellDataT default_value_; - BlockHashMap block_map_; - Index indexToBlockIndex(const IndexType& index) const; IndexType indexToCellIndex(const IndexType& index) const; IndexType cellAndBlockIndexToIndex(const Index& block_index, const IndexType& cell_index) const; + + protected: + const HeightType max_height_; + const CellDataT default_value_; + BlockHashMap block_map_; }; template diff --git a/libraries/wavemap/include/wavemap/utils/query/classified_map.h b/libraries/wavemap/include/wavemap/utils/query/classified_map.h index 5e8237897..36c4cc1d5 100644 --- a/libraries/wavemap/include/wavemap/utils/query/classified_map.h +++ b/libraries/wavemap/include/wavemap/utils/query/classified_map.h @@ -15,6 +15,7 @@ class ClassifiedMap { std::bitset has_unobserved; }; using BlockHashMap = OctreeBlockHash; + using Block = BlockHashMap::Block; using Node = BlockHashMap::Node; static constexpr int kDim = 3; @@ -36,18 +37,33 @@ class ClassifiedMap { void update(const HashedWaveletOctree& occupancy_map); - template - void forEachOccupiedLeaf(VisitorFn visitor_fn) const {} + bool has(const OctreeIndex& index, Occupancy::Id occupancy_type) const; + bool has(const OctreeIndex& index, Occupancy::Mask occupancy_mask) const; + + bool isFully(const OctreeIndex& index, Occupancy::Id occupancy_type) const; + bool isFully(const OctreeIndex& index, Occupancy::Mask occupancy_mask) const; + + void forEachLeafMatching(Occupancy::Id occupancy_type, + std::function visitor_fn, + IndexElement termination_height = 0) const; + void forEachLeafMatching(Occupancy::Mask occupancy_mask, + std::function visitor_fn, + IndexElement termination_height = 0) const; private: const FloatingPoint min_cell_width_; const OccupancyClassifier classifier_; BlockHashMap block_map_; + static Occupancy::Mask childOccupancyMask(const Node& node, + NdtreeIndexRelativeChild child_idx); + void recursiveClassifier( const HashedWaveletOctreeBlock::NodeType& occupancy_node, FloatingPoint average_occupancy, Node& classified_node); }; } // namespace wavemap +#include "wavemap/utils/query/impl/classified_map_inl.h" + #endif // WAVEMAP_UTILS_QUERY_CLASSIFIED_MAP_H_ diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h new file mode 100644 index 000000000..012623b19 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h @@ -0,0 +1,33 @@ +#ifndef WAVEMAP_UTILS_QUERY_IMPL_CLASSIFIED_MAP_INL_H_ +#define WAVEMAP_UTILS_QUERY_IMPL_CLASSIFIED_MAP_INL_H_ + +#include + +namespace wavemap { +inline bool ClassifiedMap::has(const OctreeIndex& index, + Occupancy::Id occupancy_type) const { + return has(index, Occupancy::toMask(occupancy_type)); +} + +inline bool ClassifiedMap::isFully(const OctreeIndex& index, + Occupancy::Id occupancy_type) const { + return isFully(index, Occupancy::toMask(occupancy_type)); +} + +inline void ClassifiedMap::forEachLeafMatching( + Occupancy::Id occupancy_type, + std::function visitor_fn, + IndexElement termination_height) const { + forEachLeafMatching(Occupancy::toMask(occupancy_type), std::move(visitor_fn), + termination_height); +} + +inline Occupancy::Mask ClassifiedMap::childOccupancyMask( + const ClassifiedMap::Node& node, NdtreeIndexRelativeChild child_idx) { + return Occupancy::toMask(node.data().has_free[child_idx], + node.data().has_occupied[child_idx], + node.data().has_unobserved[child_idx]); +} +} // namespace wavemap + +#endif // WAVEMAP_UTILS_QUERY_IMPL_CLASSIFIED_MAP_INL_H_ diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_classifier_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_classifier_inl.h index 7246db0b9..c6f3f8e4d 100644 --- a/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_classifier_inl.h +++ b/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_classifier_inl.h @@ -32,16 +32,26 @@ constexpr bool OccupancyClassifier::is(FloatingPoint log_odds_occupancy, constexpr bool OccupancyClassifier::has(Occupancy::Mask region_occupancy, Occupancy::Id occupancy_type) { + return has(region_occupancy, Occupancy::toMask(occupancy_type)); +} + +constexpr bool OccupancyClassifier::has(Occupancy::Mask region_occupancy, + Occupancy::Mask occupancy_mask) { // Check that at least one bit in the region matches the mask - return region_occupancy & Occupancy::toMask(occupancy_type); + return region_occupancy & occupancy_mask; } constexpr bool OccupancyClassifier::isFully(Occupancy::Mask region_occupancy, Occupancy::Id occupancy_type) { + return isFully(region_occupancy, Occupancy::toMask(occupancy_type)); +} + +constexpr bool OccupancyClassifier::isFully(Occupancy::Mask region_occupancy, + Occupancy::Mask occupancy_mask) { // Set stray bits beyond mask width to 0, as they should not influence result const Occupancy::Mask region_occ_trimmed = region_occupancy & 0b111; // Check that no bits in the region are set while not being in the mask - return !(region_occ_trimmed & ~Occupancy::toMask(occupancy_type)); + return !(region_occ_trimmed & ~occupancy_mask); } } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_inl.h index 07b9c20f1..f57cb316a 100644 --- a/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_inl.h +++ b/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_inl.h @@ -11,6 +11,10 @@ constexpr Occupancy::Mask Occupancy::toMask(Occupancy::Id type_id) { return 0b011; } } +constexpr Occupancy::Mask Occupancy::toMask(bool free, bool occupied, + bool unobserved) { + return free | occupied << 1 | unobserved << 2; +} } // namespace wavemap #endif // WAVEMAP_UTILS_QUERY_IMPL_OCCUPANCY_INL_H_ diff --git a/libraries/wavemap/include/wavemap/utils/query/occupancy.h b/libraries/wavemap/include/wavemap/utils/query/occupancy.h index 55f9436d5..05fbc2cb9 100644 --- a/libraries/wavemap/include/wavemap/utils/query/occupancy.h +++ b/libraries/wavemap/include/wavemap/utils/query/occupancy.h @@ -15,6 +15,7 @@ struct Occupancy : TypeSelector { // NOTE: For usage examples, please refer to the OccupancyClassifier class. static constexpr Mask toMask(Id type_id); + static constexpr Mask toMask(bool free, bool occupied, bool unobserved); constexpr Mask toMask() const { return toMask(static_cast(id_)); } }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h b/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h index 24a488a49..66eb50317 100644 --- a/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h +++ b/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h @@ -9,6 +9,9 @@ class OccupancyClassifier { explicit OccupancyClassifier(FloatingPoint log_odds_occupancy_threshold = 0.f) : occupancy_threshold_(log_odds_occupancy_threshold) {} + static FloatingPoint getUnobservedThreshold() { return kUnobservedThreshold; } + FloatingPoint getOccupancyThreshold() const { return occupancy_threshold_; } + static constexpr bool isUnobserved(FloatingPoint log_odds_occupancy); static constexpr bool isObserved(FloatingPoint log_odds_occupancy); @@ -17,12 +20,19 @@ class OccupancyClassifier { static constexpr bool has(Occupancy::Mask region_occupancy, Occupancy::Id occupancy_type); + static constexpr bool has(Occupancy::Mask region_occupancy, + Occupancy::Mask occupancy_mask); static constexpr bool isFully(Occupancy::Mask region_occupancy, Occupancy::Id occupancy_type); + static constexpr bool isFully(Occupancy::Mask region_occupancy, + Occupancy::Mask occupancy_mask); - static FloatingPoint getUnobservedThreshold() { return kUnobservedThreshold; } - FloatingPoint getOccupancyThreshold() const { return occupancy_threshold_; } + // Forbid implicit Mask type conversions + template + static constexpr bool has(A region_occupancy, B occupancy_mask) = delete; + template + static constexpr bool isFully(A region_occupancy, B occupancy_mask) = delete; private: static constexpr FloatingPoint kUnobservedThreshold = 1e-3f; diff --git a/libraries/wavemap/src/utils/query/classified_map.cc b/libraries/wavemap/src/utils/query/classified_map.cc index 39d2ff8a3..5a86d1c5d 100644 --- a/libraries/wavemap/src/utils/query/classified_map.cc +++ b/libraries/wavemap/src/utils/query/classified_map.cc @@ -11,6 +11,127 @@ void ClassifiedMap::update(const HashedWaveletOctree& occupancy_map) { }); } +bool ClassifiedMap::has(const OctreeIndex& index, + Occupancy::Mask occupancy_mask) const { + // Cache the last block index + static Index3D block_index = + Index3D::Constant(std::numeric_limits::max()); + static const Block* block = nullptr; + + // Fetch the current block if not already cached + if (const Index3D new_block_index = block_map_.indexToBlockIndex(index); + new_block_index != block_index) { + block_index = new_block_index; + block = block_map_.getBlock(block_index); + } + + // If the block doesn't exist, we're done + if (!block) { + return false; + } + + // Otherwise, descend the tree + const MortonIndex morton_code = convert::nodeIndexToMorton(index); + const Node* node = &block->getRootNode(); + for (int parent_height = block_map_.getMaxHeight();; --parent_height) { + if (!node || parent_height <= index.height) { + // Return the result of OccupancyClassifier::has(region_occupancy, + // occupancy_mask), which is always true if this branch is reached. + return true; + } + const NdtreeIndexRelativeChild child_idx = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + const auto region_occupancy = childOccupancyMask(*node, child_idx); + if (OccupancyClassifier::isFully(region_occupancy, occupancy_mask)) { + return true; + } else if (!OccupancyClassifier::has(region_occupancy, occupancy_mask)) { + return false; + } + node = node->getChild(child_idx); + } + + return false; +} + +bool ClassifiedMap::isFully(const OctreeIndex& index, + Occupancy::Mask occupancy_mask) const { + // Cache the last block index + static Index3D block_index = + Index3D::Constant(std::numeric_limits::max()); + static const Block* block = nullptr; + + // Fetch the current block if not already cached + if (const Index3D new_block_index = block_map_.indexToBlockIndex(index); + new_block_index != block_index) { + block_index = new_block_index; + block = block_map_.getBlock(block_index); + } + + // If the block doesn't exist, we're done + if (!block) { + return false; + } + + // Otherwise, descend the tree + const MortonIndex morton_code = convert::nodeIndexToMorton(index); + const Node* node = &block->getRootNode(); + for (int parent_height = block_map_.getMaxHeight();; --parent_height) { + if (!node || parent_height <= index.height) { + // Return the result of OccupancyClassifier::isFully(region_occupancy, + // occupancy_mask), which is always false if this branch is reached. + return false; + } + const NdtreeIndexRelativeChild child_idx = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + const auto region_occupancy = childOccupancyMask(*node, child_idx); + if (OccupancyClassifier::isFully(region_occupancy, occupancy_mask)) { + return true; + } else if (!OccupancyClassifier::has(region_occupancy, occupancy_mask)) { + return false; + } + node = node->getChild(child_idx); + } + + return false; +} + +void ClassifiedMap::forEachLeafMatching( + Occupancy::Mask occupancy_mask, + std::function visitor_fn, + IndexElement termination_height) const { + block_map_.forEachBlock([occupancy_mask, termination_height, &visitor_fn]( + const Index3D& block_index, const Block& block) { + struct StackElement { + const OctreeIndex node_index; + const Node& node; + }; + std::stack stack; + stack.emplace(StackElement{OctreeIndex{block.getMaxHeight(), block_index}, + block.getRootNode()}); + while (!stack.empty()) { + const OctreeIndex node_index = stack.top().node_index; + const Node& node = stack.top().node; + stack.pop(); + + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + const auto region_occupancy = childOccupancyMask(node, child_idx); + if (!OccupancyClassifier::has(region_occupancy, occupancy_mask)) { + continue; + } + const OctreeIndex child_node_index = + node_index.computeChildIndex(child_idx); + if (OccupancyClassifier::isFully(region_occupancy, occupancy_mask)) { + visitor_fn(child_node_index); + } else if (const Node* child_node = node.getChild(child_idx); + child_node && termination_height < child_node_index.height) { + stack.emplace(StackElement{child_node_index, *child_node}); + } + } + } + }); +} + void ClassifiedMap::recursiveClassifier( // NOLINT const HashedWaveletOctreeBlock::NodeType& occupancy_node, FloatingPoint average_occupancy, ClassifiedMap::Node& classified_node) { From 5becb7ecd757eaffea7bebb0c34272d3cebb7477 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 11 Jan 2024 16:25:55 +0100 Subject: [PATCH 057/159] Expose additional getters --- .../wavemap/utils/query/classified_map.h | 20 ++++++++++++++++--- .../utils/query/impl/classified_map_inl.h | 14 ++++++++----- .../wavemap/src/utils/query/classified_map.cc | 10 +++------- 3 files changed, 29 insertions(+), 15 deletions(-) diff --git a/libraries/wavemap/include/wavemap/utils/query/classified_map.h b/libraries/wavemap/include/wavemap/utils/query/classified_map.h index 36c4cc1d5..b8f6faea6 100644 --- a/libraries/wavemap/include/wavemap/utils/query/classified_map.h +++ b/libraries/wavemap/include/wavemap/utils/query/classified_map.h @@ -13,6 +13,10 @@ class ClassifiedMap { std::bitset has_free; std::bitset has_occupied; std::bitset has_unobserved; + + Occupancy::Mask occupancyMask() const; + Occupancy::Mask childOccupancyMask( + NdtreeIndexRelativeChild child_idx) const; }; using BlockHashMap = OctreeBlockHash; using Block = BlockHashMap::Block; @@ -43,6 +47,19 @@ class ClassifiedMap { bool isFully(const OctreeIndex& index, Occupancy::Id occupancy_type) const; bool isFully(const OctreeIndex& index, Occupancy::Mask occupancy_mask) const; + bool hasBlock(const Index3D& block_index) const { + return block_map_.hasBlock(block_index); + } + Block* getBlock(const Index3D& block_index) { + return block_map_.getBlock(block_index); + } + const Block* getBlock(const Index3D& block_index) const { + return block_map_.getBlock(block_index); + } + Block& getOrAllocateBlock(const Index3D& block_index) { + return block_map_.getOrAllocateBlock(block_index); + } + void forEachLeafMatching(Occupancy::Id occupancy_type, std::function visitor_fn, IndexElement termination_height = 0) const; @@ -55,9 +72,6 @@ class ClassifiedMap { const OccupancyClassifier classifier_; BlockHashMap block_map_; - static Occupancy::Mask childOccupancyMask(const Node& node, - NdtreeIndexRelativeChild child_idx); - void recursiveClassifier( const HashedWaveletOctreeBlock::NodeType& occupancy_node, FloatingPoint average_occupancy, Node& classified_node); diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h index 012623b19..cfc8aa6c1 100644 --- a/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h +++ b/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h @@ -22,11 +22,15 @@ inline void ClassifiedMap::forEachLeafMatching( termination_height); } -inline Occupancy::Mask ClassifiedMap::childOccupancyMask( - const ClassifiedMap::Node& node, NdtreeIndexRelativeChild child_idx) { - return Occupancy::toMask(node.data().has_free[child_idx], - node.data().has_occupied[child_idx], - node.data().has_unobserved[child_idx]); +Occupancy::Mask ClassifiedMap::NodeData::occupancyMask() const { + return Occupancy::toMask(has_free.any(), has_occupied.any(), + has_unobserved.any()); +} + +inline Occupancy::Mask ClassifiedMap::NodeData::childOccupancyMask( + NdtreeIndexRelativeChild child_idx) const { + return Occupancy::toMask(has_free[child_idx], has_occupied[child_idx], + has_unobserved[child_idx]); } } // namespace wavemap diff --git a/libraries/wavemap/src/utils/query/classified_map.cc b/libraries/wavemap/src/utils/query/classified_map.cc index 5a86d1c5d..0c92424b7 100644 --- a/libraries/wavemap/src/utils/query/classified_map.cc +++ b/libraries/wavemap/src/utils/query/classified_map.cc @@ -41,7 +41,7 @@ bool ClassifiedMap::has(const OctreeIndex& index, } const NdtreeIndexRelativeChild child_idx = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - const auto region_occupancy = childOccupancyMask(*node, child_idx); + const auto region_occupancy = node->data().childOccupancyMask(child_idx); if (OccupancyClassifier::isFully(region_occupancy, occupancy_mask)) { return true; } else if (!OccupancyClassifier::has(region_occupancy, occupancy_mask)) { @@ -49,8 +49,6 @@ bool ClassifiedMap::has(const OctreeIndex& index, } node = node->getChild(child_idx); } - - return false; } bool ClassifiedMap::isFully(const OctreeIndex& index, @@ -83,7 +81,7 @@ bool ClassifiedMap::isFully(const OctreeIndex& index, } const NdtreeIndexRelativeChild child_idx = OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - const auto region_occupancy = childOccupancyMask(*node, child_idx); + const auto region_occupancy = node->data().childOccupancyMask(child_idx); if (OccupancyClassifier::isFully(region_occupancy, occupancy_mask)) { return true; } else if (!OccupancyClassifier::has(region_occupancy, occupancy_mask)) { @@ -91,8 +89,6 @@ bool ClassifiedMap::isFully(const OctreeIndex& index, } node = node->getChild(child_idx); } - - return false; } void ClassifiedMap::forEachLeafMatching( @@ -115,7 +111,7 @@ void ClassifiedMap::forEachLeafMatching( for (NdtreeIndexRelativeChild child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { - const auto region_occupancy = childOccupancyMask(node, child_idx); + const auto region_occupancy = node.data().childOccupancyMask(child_idx); if (!OccupancyClassifier::has(region_occupancy, occupancy_mask)) { continue; } From 1ab37c4da8d7818af2d0f5450c405f285dc97588 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 11 Jan 2024 16:56:35 +0100 Subject: [PATCH 058/159] Add getter for block hash map --- libraries/wavemap/include/wavemap/utils/query/classified_map.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/wavemap/include/wavemap/utils/query/classified_map.h b/libraries/wavemap/include/wavemap/utils/query/classified_map.h index b8f6faea6..b194e98eb 100644 --- a/libraries/wavemap/include/wavemap/utils/query/classified_map.h +++ b/libraries/wavemap/include/wavemap/utils/query/classified_map.h @@ -59,6 +59,8 @@ class ClassifiedMap { Block& getOrAllocateBlock(const Index3D& block_index) { return block_map_.getOrAllocateBlock(block_index); } + BlockHashMap& getBlockMap() { return block_map_; } + const BlockHashMap& getBlockMap() const { return block_map_; } void forEachLeafMatching(Occupancy::Id occupancy_type, std::function visitor_fn, From 6ce07add91f7e09599672964afc5e7f3e0ea4a40 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 11 Jan 2024 20:30:20 +0100 Subject: [PATCH 059/159] Remove unused member variable --- .../include/wavemap/data_structure/dense_block_hash.h | 6 ++---- libraries/wavemap/include/wavemap/map/hashed_blocks.h | 3 +-- .../wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc | 2 +- 3 files changed, 4 insertions(+), 7 deletions(-) diff --git a/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h b/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h index 94b018871..1d196f725 100644 --- a/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h +++ b/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h @@ -17,9 +17,8 @@ class DenseBlockHash { using Block = DenseGrid; using BlockHashMap = SpatialHash; - explicit DenseBlockHash(FloatingPoint min_cell_width, - CellDataT default_value = CellDataT{}) - : min_cell_width_(min_cell_width), default_value_(default_value) {} + explicit DenseBlockHash(CellDataT default_value = CellDataT{}) + : default_value_(default_value) {} bool empty() const { return block_map_.empty(); } size_t size() const { return Block::kCellsPerBlock * block_map_.size(); } @@ -57,7 +56,6 @@ class DenseBlockHash { void forEachLeaf(IndexedLeafVisitorFunction visitor_fn) const; protected: - const FloatingPoint min_cell_width_; const CellDataT default_value_; BlockHashMap block_map_; diff --git a/libraries/wavemap/include/wavemap/map/hashed_blocks.h b/libraries/wavemap/include/wavemap/map/hashed_blocks.h index dc4fbb03d..414320290 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_blocks.h +++ b/libraries/wavemap/include/wavemap/map/hashed_blocks.h @@ -23,8 +23,7 @@ class HashedBlocks : public MapBase, explicit HashedBlocks(const MapBaseConfig& config, FloatingPoint default_value = 0.f) - : MapBase(config.checkValid()), - DenseBlockHash(config.min_cell_width, default_value) {} + : MapBase(config.checkValid()), DenseBlockHash(default_value) {} bool empty() const override { return DenseBlockHash::empty(); } size_t size() const override { return DenseBlockHash::size(); } diff --git a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc index 6492ba602..ddce0176b 100644 --- a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc @@ -14,7 +14,7 @@ HashedBlocks FullEuclideanSDFGenerator::generate( const Index3D uninitialized_parent = Index3D::Constant(std::numeric_limits::max()); VectorDistanceField full_sdf( - min_cell_width, VectorDistance{uninitialized_parent, max_distance_}); + VectorDistance{uninitialized_parent, max_distance_}); // Initialize the bucketed priority queue const int num_bins = From 4547a1bff73880db352d3125865aac9765384f58 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 12 Jan 2024 00:20:14 +0100 Subject: [PATCH 060/159] Start implementing accelerated node getter for NdtreeBlockHashes --- .../data_structure/ndtree_block_hash.h | 2 +- .../utils/query/impl/query_accelerator_inl.h | 33 ++++++++++++++----- .../wavemap/utils/query/query_accelerator.h | 10 +++--- 3 files changed, 32 insertions(+), 13 deletions(-) diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h b/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h index 73e6859dc..3d3f433fa 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h +++ b/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h @@ -63,7 +63,7 @@ class NdtreeBlockHash { const IndexType& index) const; const CellDataT& getValueOrDefault(const IndexType& index) const; - const CellDataT& getDefaultCellValue() const { return default_value_; } + const CellDataT& getDefaultValue() const { return default_value_; } bool equalsDefaultValue(const CellDataT& value) const; template diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h index fd2b5cff4..8a7968e0d 100644 --- a/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h +++ b/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h @@ -30,11 +30,11 @@ template typename QueryAccelerator>::BlockType* QueryAccelerator>::getBlock( const Index& block_index) { - if (block_index != last_block_index_) { - last_block_index_ = block_index; - last_block_ = ndtree_block_hash_.getBlock(block_index); + if (block_index != block_index_) { + block_index_ = block_index; + block_ = ndtree_block_hash_.getBlock(block_index); } - return last_block_; + return block_; } template @@ -42,14 +42,31 @@ template typename QueryAccelerator>::BlockType& QueryAccelerator>::getOrAllocateBlock( const Index& block_index, DefaultArgs&&... args) { - if (block_index != last_block_index_ || !last_block_) { - last_block_index_ = block_index; - last_block_ = &ndtree_block_hash_.getOrAllocateBlock( + if (block_index != block_index_ || !block_) { + block_index_ = block_index; + block_ = &ndtree_block_hash_.getOrAllocateBlock( block_index, std::forward(args)...); } - return *last_block_; + return *block_; } +template +const typename QueryAccelerator>::NodeType* +QueryAccelerator>::getNode( + const OctreeIndex& index) { + // TODO(victorr): Finish implementing this for all heights, + // not just block roots + CHECK_EQ(index.height, tree_height_) + << "Loading non-root nodes is not yet implemented."; + if (index.position != block_index_) { + block_index_ = index.position; + block_ = ndtree_block_hash_.getBlock(index.position); + } + if (block_) { + return &block_->getRootNode(); + } + return nullptr; +} } // namespace wavemap #endif // WAVEMAP_UTILS_QUERY_IMPL_QUERY_ACCELERATOR_INL_H_ diff --git a/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h b/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h index 622d2c410..7b23721fb 100644 --- a/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h +++ b/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h @@ -45,6 +45,7 @@ class QueryAccelerator> { public: static constexpr int kDim = dim; using BlockType = typename NdtreeBlockHash::Block; + using NodeType = typename BlockType::NodeType; explicit QueryAccelerator(NdtreeBlockHash& ndtree_block_hash) : ndtree_block_hash_(ndtree_block_hash) {} @@ -54,14 +55,15 @@ class QueryAccelerator> { BlockType& getOrAllocateBlock(const Index& block_index, DefaultArgs&&... args); - // TODO(victorr): Implement accelerated cell accessors + const NodeType* getNode(const OctreeIndex& index); private: NdtreeBlockHash& ndtree_block_hash_; + const IndexElement tree_height_ = ndtree_block_hash_.getMaxHeight(); - Index last_block_index_ = - Index3D::Constant(std::numeric_limits::max()); - BlockType* last_block_ = nullptr; + Index block_index_ = + Index::Constant(std::numeric_limits::max()); + BlockType* block_ = nullptr; }; // Query accelerator for hashed wavelet octrees From 7280aedfdf967118b0abe632d61b2c97e7b44617 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 15 Jan 2024 11:46:10 +0100 Subject: [PATCH 061/159] Reorganize unit tests to match new utils structure --- libraries/wavemap/CMakeLists.txt | 38 ++-- .../test_bit_operations.cc} | 20 +- .../test_comparisons.cc} | 2 +- .../{test_fill_utils.cc => data/test_fill.cc} | 4 +- .../iterate}/test_grid_iterator.cc | 0 .../iterate}/test_ray_iterator.cc | 0 .../iterate}/test_subtree_iterator.cc | 0 .../test_approximate_trigonometry.cc | 0 .../src/utils/{ => math}/test_int_math.cc | 0 .../src/utils/{ => math}/test_tree_math.cc | 0 .../utils/{ => neighbors}/test_adjacency.cc | 0 .../{ => neighbors}/test_grid_adjacency.cc | 0 .../{ => neighbors}/test_grid_neighborhood.cc | 0 .../{ => neighbors}/test_ndtree_adjacency.cc | 0 .../{ => query}/test_map_interpolator.cpp | 2 +- .../{ => query}/test_occupancy_classifier.cc | 0 .../test_probability_conversions.cc} | 0 .../{ => query}/test_query_accelerator.cc | 0 .../test/src/utils/sdf/test_sdf_generators.cc | 174 ++++++++++++++++++ .../test/src/utils/test_sdf_generation.cc | 173 ----------------- 20 files changed, 207 insertions(+), 206 deletions(-) rename libraries/wavemap/test/src/utils/{test_bit_manipulation.cc => bits/test_bit_operations.cc} (96%) rename libraries/wavemap/test/src/utils/{test_data_utils.cc => data/test_comparisons.cc} (97%) rename libraries/wavemap/test/src/utils/{test_fill_utils.cc => data/test_fill.cc} (93%) rename libraries/wavemap/test/src/{iterator => utils/iterate}/test_grid_iterator.cc (100%) rename libraries/wavemap/test/src/{iterator => utils/iterate}/test_ray_iterator.cc (100%) rename libraries/wavemap/test/src/{iterator => utils/iterate}/test_subtree_iterator.cc (100%) rename libraries/wavemap/test/src/utils/{ => math}/test_approximate_trigonometry.cc (100%) rename libraries/wavemap/test/src/utils/{ => math}/test_int_math.cc (100%) rename libraries/wavemap/test/src/utils/{ => math}/test_tree_math.cc (100%) rename libraries/wavemap/test/src/utils/{ => neighbors}/test_adjacency.cc (100%) rename libraries/wavemap/test/src/utils/{ => neighbors}/test_grid_adjacency.cc (100%) rename libraries/wavemap/test/src/utils/{ => neighbors}/test_grid_neighborhood.cc (100%) rename libraries/wavemap/test/src/utils/{ => neighbors}/test_ndtree_adjacency.cc (100%) rename libraries/wavemap/test/src/utils/{ => query}/test_map_interpolator.cpp (98%) rename libraries/wavemap/test/src/utils/{ => query}/test_occupancy_classifier.cc (100%) rename libraries/wavemap/test/src/utils/{test_log_odds_converter.cc => query/test_probability_conversions.cc} (100%) rename libraries/wavemap/test/src/utils/{ => query}/test_query_accelerator.cc (100%) create mode 100644 libraries/wavemap/test/src/utils/sdf/test_sdf_generators.cc delete mode 100644 libraries/wavemap/test/src/utils/test_sdf_generation.cc diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index 72d3270ca..e8a753af9 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -105,29 +105,29 @@ if (CATKIN_ENABLE_TESTING) test/src/integrator/test_measurement_models.cc test/src/integrator/test_pointcloud_integrators.cc test/src/integrator/test_range_image_intersector.cc - test/src/iterator/test_grid_iterator.cc - test/src/iterator/test_ray_iterator.cc - test/src/iterator/test_subtree_iterator.cc test/src/map/test_haar_cell.cc test/src/map/test_hashed_blocks.cc test/src/map/test_map.cc test/src/map/test_volumetric_octree.cc - test/src/utils/test_adjacency.cc - test/src/utils/test_approximate_trigonometry.cc - test/src/utils/test_bit_manipulation.cc - test/src/utils/test_data_utils.cc - test/src/utils/test_fill_utils.cc - test/src/utils/test_grid_neighborhood.cc - test/src/utils/test_grid_adjacency.cc - test/src/utils/test_ndtree_adjacency.cc - test/src/utils/test_occupancy_classifier.cc - test/src/utils/test_int_math.cc - test/src/utils/test_log_odds_converter.cc - test/src/utils/test_map_interpolator.cpp - test/src/utils/test_query_accelerator.cc - test/src/utils/test_sdf_generation.cc - test/src/utils/test_thread_pool.cc - test/src/utils/test_tree_math.cc) + test/src/utils/bits/test_bit_operations.cc + test/src/utils/data/test_comparisons.cc + test/src/utils/data/test_fill.cc + test/src/utils/iterate/test_grid_iterator.cc + test/src/utils/iterate/test_ray_iterator.cc + test/src/utils/iterate/test_subtree_iterator.cc + test/src/utils/math/test_approximate_trigonometry.cc + test/src/utils/math/test_int_math.cc + test/src/utils/math/test_tree_math.cc + test/src/utils/neighbors/test_adjacency.cc + test/src/utils/neighbors/test_grid_adjacency.cc + test/src/utils/neighbors/test_grid_neighborhood.cc + test/src/utils/neighbors/test_ndtree_adjacency.cc + test/src/utils/query/test_map_interpolator.cpp + test/src/utils/query/test_occupancy_classifier.cc + test/src/utils/query/test_probability_conversions.cc + test/src/utils/query/test_query_accelerator.cc + test/src/utils/sdf/test_sdf_generators.cc + test/src/utils/test_thread_pool.cc) target_include_directories(test_${PROJECT_NAME} PRIVATE test/include) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} gtest_main) endif () diff --git a/libraries/wavemap/test/src/utils/test_bit_manipulation.cc b/libraries/wavemap/test/src/utils/bits/test_bit_operations.cc similarity index 96% rename from libraries/wavemap/test/src/utils/test_bit_manipulation.cc rename to libraries/wavemap/test/src/utils/bits/test_bit_operations.cc index 55a9bc731..eaa618142 100644 --- a/libraries/wavemap/test/src/utils/test_bit_manipulation.cc +++ b/libraries/wavemap/test/src/utils/bits/test_bit_operations.cc @@ -6,9 +6,9 @@ #include "wavemap/utils/bits/bit_operations.h" namespace wavemap { -using BitManipulationTest = FixtureBase; +using BitOperationsTest = FixtureBase; -TEST_F(BitManipulationTest, IsBitSet) { +TEST_F(BitOperationsTest, IsBitSet) { for (int iteration = 0; iteration < 1000; ++iteration) { const auto bitstring = getRandomInteger(0, std::numeric_limits::max()); @@ -18,7 +18,7 @@ TEST_F(BitManipulationTest, IsBitSet) { } } -TEST_F(BitManipulationTest, RotateLeft) { +TEST_F(BitOperationsTest, RotateLeft) { // Test default template argument deduction EXPECT_EQ(bit_ops::rotate_left(0b1011, 3), 0b1011000); @@ -71,7 +71,7 @@ TEST_F(BitManipulationTest, RotateLeft) { EXPECT_EQ(bit_ops::rotate_left(std::numeric_limits::lowest(), 3), 4); } -TEST_F(BitManipulationTest, RotateRight) { +TEST_F(BitOperationsTest, RotateRight) { // Test default template argument deduction EXPECT_EQ(bit_ops::rotate_right(0b1011000, 3), 0b1011); @@ -125,7 +125,7 @@ TEST_F(BitManipulationTest, RotateRight) { static_cast(std::numeric_limits::lowest()) >> 3); } -TEST_F(BitManipulationTest, SqueezeIn) { +TEST_F(BitOperationsTest, SqueezeIn) { EXPECT_EQ(bit_ops::squeeze_in(0b1001, false, 0), 0b10010); EXPECT_EQ(bit_ops::squeeze_in(0b1001, false, 1), 0b10001); EXPECT_EQ(bit_ops::squeeze_in(0b1001, false, 2), 0b10001); @@ -139,7 +139,7 @@ TEST_F(BitManipulationTest, SqueezeIn) { EXPECT_EQ(bit_ops::squeeze_in(0b1001, true, 4), 0b11001); } -TEST_F(BitManipulationTest, Popcount) { +TEST_F(BitOperationsTest, Popcount) { // Small numbers EXPECT_EQ(bit_ops::popcount(0b0), 0); EXPECT_EQ(bit_ops::popcount(0b1), 1); @@ -169,7 +169,7 @@ TEST_F(BitManipulationTest, Popcount) { 64); } -TEST_F(BitManipulationTest, Parity) { +TEST_F(BitOperationsTest, Parity) { EXPECT_EQ(bit_ops::parity(0b0), 0); EXPECT_EQ(bit_ops::parity(0b1), 1); EXPECT_EQ(bit_ops::parity(0b10), 1); @@ -179,7 +179,7 @@ TEST_F(BitManipulationTest, Parity) { EXPECT_EQ(bit_ops::parity(0b11101010010111), 1); } -TEST_F(BitManipulationTest, CountLeadingZeros) { +TEST_F(BitOperationsTest, CountLeadingZeros) { EXPECT_EQ(bit_ops::clz(static_cast(1) << 0), 31); EXPECT_EQ(bit_ops::clz(static_cast(1) << 30), 1); EXPECT_EQ(bit_ops::clz(static_cast(1) << 31), 0); @@ -195,7 +195,7 @@ TEST_F(BitManipulationTest, CountLeadingZeros) { EXPECT_EQ(bit_ops::clz(static_cast(1) << 62), 1); } -TEST_F(BitManipulationTest, RepeatBlock) { +TEST_F(BitOperationsTest, RepeatBlock) { EXPECT_EQ(bit_ops::repeat_block(2, 0b01), 0b01010101010101010101010101010101); EXPECT_EQ(bit_ops::repeat_block(3, 0b001), @@ -257,7 +257,7 @@ TEST_F(BitManipulationTest, RepeatBlock) { } #if defined(BIT_EXPAND_AVAILABLE) && defined(BIT_COMPRESS_AVAILABLE) -TEST_F(BitManipulationTest, ExpandCompress) { +TEST_F(BitOperationsTest, ExpandCompress) { for (uint64_t idx = 0u; idx < (1 << 20); ++idx) { const uint64_t source = getRandomInteger(0ul, std::numeric_limits::max()); diff --git a/libraries/wavemap/test/src/utils/test_data_utils.cc b/libraries/wavemap/test/src/utils/data/test_comparisons.cc similarity index 97% rename from libraries/wavemap/test/src/utils/test_data_utils.cc rename to libraries/wavemap/test/src/utils/data/test_comparisons.cc index 966ceb835..f0347b29f 100644 --- a/libraries/wavemap/test/src/utils/test_data_utils.cc +++ b/libraries/wavemap/test/src/utils/data/test_comparisons.cc @@ -3,7 +3,7 @@ #include "wavemap/utils/data/comparisons.h" namespace wavemap { -TEST(DataUtilsTest, IsNonzero) { +TEST(DataComparisonUtilsTest, IsNonzero) { EXPECT_EQ(data::is_nonzero(0), false); EXPECT_EQ(data::is_nonzero(-1), true); EXPECT_EQ(data::is_nonzero(1), true); diff --git a/libraries/wavemap/test/src/utils/test_fill_utils.cc b/libraries/wavemap/test/src/utils/data/test_fill.cc similarity index 93% rename from libraries/wavemap/test/src/utils/test_fill_utils.cc rename to libraries/wavemap/test/src/utils/data/test_fill.cc index 913f7eedf..0a2153ba1 100644 --- a/libraries/wavemap/test/src/utils/test_fill_utils.cc +++ b/libraries/wavemap/test/src/utils/data/test_fill.cc @@ -4,7 +4,7 @@ #include "wavemap/utils/data/fill.h" namespace wavemap { -TEST(FillUtils, Constant) { +TEST(DataFillUtilsTest, Constant) { EXPECT_EQ(data::fill::constant(false), false); EXPECT_EQ(data::fill::constant(true), true); @@ -21,7 +21,7 @@ TEST(FillUtils, Constant) { EXPECT_EQ(data::fill::constant(4.56f), Point3D::Constant(4.56f)); } -TEST(FillUtils, Zero) { +TEST(DataFillUtilsTest, Zero) { EXPECT_EQ(data::fill::zero(), false); EXPECT_EQ(data::fill::zero(), 0); EXPECT_EQ(data::fill::zero(), 0.f); diff --git a/libraries/wavemap/test/src/iterator/test_grid_iterator.cc b/libraries/wavemap/test/src/utils/iterate/test_grid_iterator.cc similarity index 100% rename from libraries/wavemap/test/src/iterator/test_grid_iterator.cc rename to libraries/wavemap/test/src/utils/iterate/test_grid_iterator.cc diff --git a/libraries/wavemap/test/src/iterator/test_ray_iterator.cc b/libraries/wavemap/test/src/utils/iterate/test_ray_iterator.cc similarity index 100% rename from libraries/wavemap/test/src/iterator/test_ray_iterator.cc rename to libraries/wavemap/test/src/utils/iterate/test_ray_iterator.cc diff --git a/libraries/wavemap/test/src/iterator/test_subtree_iterator.cc b/libraries/wavemap/test/src/utils/iterate/test_subtree_iterator.cc similarity index 100% rename from libraries/wavemap/test/src/iterator/test_subtree_iterator.cc rename to libraries/wavemap/test/src/utils/iterate/test_subtree_iterator.cc diff --git a/libraries/wavemap/test/src/utils/test_approximate_trigonometry.cc b/libraries/wavemap/test/src/utils/math/test_approximate_trigonometry.cc similarity index 100% rename from libraries/wavemap/test/src/utils/test_approximate_trigonometry.cc rename to libraries/wavemap/test/src/utils/math/test_approximate_trigonometry.cc diff --git a/libraries/wavemap/test/src/utils/test_int_math.cc b/libraries/wavemap/test/src/utils/math/test_int_math.cc similarity index 100% rename from libraries/wavemap/test/src/utils/test_int_math.cc rename to libraries/wavemap/test/src/utils/math/test_int_math.cc diff --git a/libraries/wavemap/test/src/utils/test_tree_math.cc b/libraries/wavemap/test/src/utils/math/test_tree_math.cc similarity index 100% rename from libraries/wavemap/test/src/utils/test_tree_math.cc rename to libraries/wavemap/test/src/utils/math/test_tree_math.cc diff --git a/libraries/wavemap/test/src/utils/test_adjacency.cc b/libraries/wavemap/test/src/utils/neighbors/test_adjacency.cc similarity index 100% rename from libraries/wavemap/test/src/utils/test_adjacency.cc rename to libraries/wavemap/test/src/utils/neighbors/test_adjacency.cc diff --git a/libraries/wavemap/test/src/utils/test_grid_adjacency.cc b/libraries/wavemap/test/src/utils/neighbors/test_grid_adjacency.cc similarity index 100% rename from libraries/wavemap/test/src/utils/test_grid_adjacency.cc rename to libraries/wavemap/test/src/utils/neighbors/test_grid_adjacency.cc diff --git a/libraries/wavemap/test/src/utils/test_grid_neighborhood.cc b/libraries/wavemap/test/src/utils/neighbors/test_grid_neighborhood.cc similarity index 100% rename from libraries/wavemap/test/src/utils/test_grid_neighborhood.cc rename to libraries/wavemap/test/src/utils/neighbors/test_grid_neighborhood.cc diff --git a/libraries/wavemap/test/src/utils/test_ndtree_adjacency.cc b/libraries/wavemap/test/src/utils/neighbors/test_ndtree_adjacency.cc similarity index 100% rename from libraries/wavemap/test/src/utils/test_ndtree_adjacency.cc rename to libraries/wavemap/test/src/utils/neighbors/test_ndtree_adjacency.cc diff --git a/libraries/wavemap/test/src/utils/test_map_interpolator.cpp b/libraries/wavemap/test/src/utils/query/test_map_interpolator.cpp similarity index 98% rename from libraries/wavemap/test/src/utils/test_map_interpolator.cpp rename to libraries/wavemap/test/src/utils/query/test_map_interpolator.cpp index 4d1dfebd5..a683c0107 100644 --- a/libraries/wavemap/test/src/utils/test_map_interpolator.cpp +++ b/libraries/wavemap/test/src/utils/query/test_map_interpolator.cpp @@ -6,7 +6,7 @@ #include "wavemap/utils/query/map_interpolator.h" namespace wavemap { -TEST(InterpolationUtilsTest, Trilinear) { +TEST(MapInterpolatorTest, Trilinear) { MapBaseConfig config{1.f, -10.f, 10.f}; HashedBlocks map(config); const FloatingPoint cell_width = map.getMinCellWidth(); diff --git a/libraries/wavemap/test/src/utils/test_occupancy_classifier.cc b/libraries/wavemap/test/src/utils/query/test_occupancy_classifier.cc similarity index 100% rename from libraries/wavemap/test/src/utils/test_occupancy_classifier.cc rename to libraries/wavemap/test/src/utils/query/test_occupancy_classifier.cc diff --git a/libraries/wavemap/test/src/utils/test_log_odds_converter.cc b/libraries/wavemap/test/src/utils/query/test_probability_conversions.cc similarity index 100% rename from libraries/wavemap/test/src/utils/test_log_odds_converter.cc rename to libraries/wavemap/test/src/utils/query/test_probability_conversions.cc diff --git a/libraries/wavemap/test/src/utils/test_query_accelerator.cc b/libraries/wavemap/test/src/utils/query/test_query_accelerator.cc similarity index 100% rename from libraries/wavemap/test/src/utils/test_query_accelerator.cc rename to libraries/wavemap/test/src/utils/query/test_query_accelerator.cc diff --git a/libraries/wavemap/test/src/utils/sdf/test_sdf_generators.cc b/libraries/wavemap/test/src/utils/sdf/test_sdf_generators.cc new file mode 100644 index 000000000..2d97e2b8c --- /dev/null +++ b/libraries/wavemap/test/src/utils/sdf/test_sdf_generators.cc @@ -0,0 +1,174 @@ +#include + +#include "wavemap/common.h" +#include "wavemap/map/hashed_blocks.h" +#include "wavemap/map/hashed_wavelet_octree.h" +#include "wavemap/test/config_generator.h" +#include "wavemap/test/fixture_base.h" +#include "wavemap/test/geometry_generator.h" +#include "wavemap/utils/sdf/full_euclidean_sdf_generator.h" +#include "wavemap/utils/sdf/quasi_euclidean_sdf_generator.h" + +namespace wavemap { +template +class SdfGeneratorTest : public FixtureBase, + public GeometryGenerator, + public ConfigGenerator {}; + +using SdfGeneratorTypes = + ::testing::Types; +TYPED_TEST_SUITE(SdfGeneratorTest, SdfGeneratorTypes, ); + +TYPED_TEST(SdfGeneratorTest, BruteForceEquivalence) { + constexpr int kNumIterations = 2; + for (int iteration = 0; iteration < kNumIterations; ++iteration) { + // Params + const Index3D min_index = Index3D::Constant(-10); + const Index3D max_index = Index3D::Constant(100); + const FloatingPoint kMaxSdfDistance = + FixtureBase::getRandomFloat(0.2f, 4.f); + + // Create the map and occupancy classification util + const auto config = + ConfigGenerator::getRandomConfig(); + HashedWaveletOctree map{config}; + const OccupancyClassifier classifier{}; + + // Generate random obstacles + auto obstacle_cells = GeometryGenerator::getRandomIndexVector<3>( + 10, 20, min_index, max_index); + // Add a cube + const FloatingPoint min_cell_width = map.getMinCellWidth(); + const IndexElement padding = std::ceil(kMaxSdfDistance / min_cell_width); + const Index3D cube_center{2, 6, 4}; + for (const Index3D& index : Grid<3>(cube_center.array() - padding, + cube_center.array() + padding)) { + obstacle_cells.emplace_back(index); + } + + // Set default occupancy to free + const OctreeIndex min_block_index = convert::indexAndHeightToNodeIndex<3>( + min_index.array() - padding, map.getTreeHeight()); + const OctreeIndex max_block_index = convert::indexAndHeightToNodeIndex<3>( + max_index.array() + padding, map.getTreeHeight()); + for (const auto& block_index : + Grid(min_block_index.position, max_block_index.position)) { + map.getOrAllocateBlock(block_index).getRootScale() = config.min_log_odds; + } + + // Set obstacles to occupied + for (const Index3D& index : obstacle_cells) { + map.setCellValue(index, config.max_log_odds); + } + + // Generate the SDF + TypeParam sdf_generator{kMaxSdfDistance}; + const auto sdf = sdf_generator.generate(map); + + // Compare the SDF distances to the brute force min distance + sdf.forEachLeaf( + [&map, &classifier, &sdf, &obstacle_cells, min_cell_width, padding]( + const OctreeIndex& node_index, FloatingPoint sdf_value) { + // In unobserved space, the SDF should be uninitialized + const FloatingPoint occupancy_value = map.getCellValue(node_index); + if (OccupancyClassifier::isUnobserved(occupancy_value)) { + // In unknown space the SDF should be uninitialized + EXPECT_NEAR(sdf_value, sdf.getDefaultValue(), kEpsilon); + return; + } + + const Point3D node_center = + convert::nodeIndexToCenterPoint(node_index, min_cell_width); + + // Find the closest surface using brute force + FloatingPoint sdf_brute_force = sdf.getDefaultValue(); + Index3D parent_brute_force = + Index3D::Constant(std::numeric_limits::max()); + if (classifier.is(occupancy_value, Occupancy::kFree)) { + // In free space, the SDF should always be positive + EXPECT_GT(sdf_value, 0.f); + + // Find the distance to the closest obstacle + for (const auto& obstacle_cell : obstacle_cells) { + const auto obstacle_aabb = convert::nodeIndexToAABB( + OctreeIndex{0, obstacle_cell}, min_cell_width); + const FloatingPoint min_dist = + obstacle_aabb.minDistanceTo(node_center); + if (min_dist < sdf_brute_force) { + sdf_brute_force = min_dist; + parent_brute_force = obstacle_cell; + } + } + } else { + // Find the distance to the closest free cell + for (const Index3D& neighbor_index : + Grid<3>(node_index.position.array() - padding, + node_index.position.array() + padding)) { + const FloatingPoint neighbor_occupancy_value = + map.getCellValue(neighbor_index); + if (classifier.is(neighbor_occupancy_value, Occupancy::kFree)) { + const auto free_cell_aabb = convert::nodeIndexToAABB( + OctreeIndex{0, neighbor_index}, min_cell_width); + const FloatingPoint min_dist = + free_cell_aabb.minDistanceTo(node_center); + if (min_dist < sdf_brute_force) { + sdf_brute_force = min_dist; + parent_brute_force = neighbor_index; + } + } + } + // Adjust the sign to reflect we're inside the obstacle + sdf_brute_force = -sdf_brute_force; + + // In occupied space, the SDF should be + if (std::abs(sdf_brute_force) < sdf.getDefaultValue()) { + // Negative + EXPECT_LT(sdf_value, 0.f); + } else { + // Or uninitialized + EXPECT_NEAR(sdf_value, sdf.getDefaultValue(), kEpsilon); + } + } + + // Check that the SDF accurately approximates the min obstacle + // distance + constexpr FloatingPoint kMaxRelativeUnderEstimate = + TypeParam::kMaxRelativeUnderEstimate; + constexpr FloatingPoint kMaxRelativeOverEstimate = + TypeParam::kMaxRelativeOverEstimate; + if (std::abs(sdf_brute_force) < sdf.getDefaultValue()) { + if (0.f < sdf_brute_force) { + EXPECT_LT(sdf_value, + sdf_brute_force * (1.f + kMaxRelativeOverEstimate)) + << "At index " << print::eigen::oneLine(node_index.position) + << " with nearest obstacle " + << print::eigen::oneLine(parent_brute_force); + EXPECT_GT(sdf_value, + sdf_brute_force * (1.f - kMaxRelativeUnderEstimate)) + << "At index " << print::eigen::oneLine(node_index.position) + << " with nearest obstacle " + << print::eigen::oneLine(parent_brute_force); + } else { + EXPECT_GT(sdf_value, + sdf_brute_force * (1.f + kMaxRelativeOverEstimate)) + << "At index " << print::eigen::oneLine(node_index.position) + << " with nearest free cell " + << print::eigen::oneLine(parent_brute_force); + EXPECT_LT(sdf_value, + sdf_brute_force * (1.f - kMaxRelativeUnderEstimate)) + << "At index " << print::eigen::oneLine(node_index.position) + << " with nearest free cell " + << print::eigen::oneLine(parent_brute_force); + } + } else { + EXPECT_LT(sdf_value, + sdf.getDefaultValue() * (1.f + kMaxRelativeOverEstimate)) + << "At index " << print::eigen::oneLine(node_index.position); + EXPECT_GT(sdf_value, + sdf.getDefaultValue() * (1.f - kMaxRelativeUnderEstimate)) + << "At index " << print::eigen::oneLine(node_index.position); + } + }); + } +} +} // namespace wavemap diff --git a/libraries/wavemap/test/src/utils/test_sdf_generation.cc b/libraries/wavemap/test/src/utils/test_sdf_generation.cc deleted file mode 100644 index 7835d2abd..000000000 --- a/libraries/wavemap/test/src/utils/test_sdf_generation.cc +++ /dev/null @@ -1,173 +0,0 @@ -#include - -#include "wavemap/common.h" -#include "wavemap/map/hashed_blocks.h" -#include "wavemap/map/hashed_wavelet_octree.h" -#include "wavemap/test/config_generator.h" -#include "wavemap/test/fixture_base.h" -#include "wavemap/test/geometry_generator.h" -#include "wavemap/utils/sdf/full_euclidean_sdf_generator.h" -#include "wavemap/utils/sdf/quasi_euclidean_sdf_generator.h" - -namespace wavemap { -template -class SdfGenerationTest : public FixtureBase, - public GeometryGenerator, - public ConfigGenerator {}; - -using SdfGeneratorTypes = - ::testing::Types; -TYPED_TEST_SUITE(SdfGenerationTest, SdfGeneratorTypes, ); - -TYPED_TEST(SdfGenerationTest, BruteForceEquivalence) { - constexpr int kNumIterations = 2; - for (int iteration = 0; iteration < kNumIterations; ++iteration) { - // Params - const Index3D min_index = Index3D::Constant(-10); - const Index3D max_index = Index3D::Constant(100); - const FloatingPoint kMaxSdfDistance = - FixtureBase::getRandomFloat(0.2f, 4.f); - - // Create the map and occupancy classification util - const auto config = - ConfigGenerator::getRandomConfig(); - HashedWaveletOctree map{config}; - const OccupancyClassifier classifier{}; - - // Generate random obstacles - auto obstacle_cells = GeometryGenerator::getRandomIndexVector<3>( - 10, 20, min_index, max_index); - // Add a cube - const FloatingPoint min_cell_width = map.getMinCellWidth(); - const IndexElement padding = std::ceil(kMaxSdfDistance / min_cell_width); - const Index3D cube_center{2, 6, 4}; - for (const Index3D& index : Grid<3>(cube_center.array() - padding, - cube_center.array() + padding)) { - obstacle_cells.emplace_back(index); - } - - // Set default occupancy to free - const OctreeIndex min_block_index = convert::indexAndHeightToNodeIndex<3>( - min_index.array() - padding, map.getTreeHeight()); - const OctreeIndex max_block_index = convert::indexAndHeightToNodeIndex<3>( - max_index.array() + padding, map.getTreeHeight()); - for (const auto& block_index : - Grid(min_block_index.position, max_block_index.position)) { - map.getOrAllocateBlock(block_index).getRootScale() = config.min_log_odds; - } - - // Set obstacles to occupied - for (const Index3D& index : obstacle_cells) { - map.setCellValue(index, config.max_log_odds); - } - - // Generate the SDF - TypeParam sdf_generator{kMaxSdfDistance}; - const auto sdf = sdf_generator.generate(map); - - // Compare the SDF distances to the brute force min distance - sdf.forEachLeaf([&map, &classifier, &sdf_generator, &sdf, &obstacle_cells, - min_cell_width, padding](const OctreeIndex& node_index, - FloatingPoint sdf_value) { - // In unobserved space, the SDF should be uninitialized - const FloatingPoint occupancy_value = map.getCellValue(node_index); - if (OccupancyClassifier::isUnobserved(occupancy_value)) { - // In unknown space the SDF should be uninitialized - EXPECT_NEAR(sdf_value, sdf.getDefaultValue(), kEpsilon); - return; - } - - const Point3D node_center = - convert::nodeIndexToCenterPoint(node_index, min_cell_width); - - // Find the closest surface using brute force - FloatingPoint sdf_brute_force = sdf.getDefaultValue(); - Index3D parent_brute_force = - Index3D::Constant(std::numeric_limits::max()); - if (classifier.is(occupancy_value, Occupancy::kFree)) { - // In free space, the SDF should always be positive - EXPECT_GT(sdf_value, 0.f); - - // Find the distance to the closest obstacle - for (const auto& obstacle_cell : obstacle_cells) { - const auto obstacle_aabb = convert::nodeIndexToAABB( - OctreeIndex{0, obstacle_cell}, min_cell_width); - const FloatingPoint min_dist = - obstacle_aabb.minDistanceTo(node_center); - if (min_dist < sdf_brute_force) { - sdf_brute_force = min_dist; - parent_brute_force = obstacle_cell; - } - } - } else { - // Find the distance to the closest free cell - for (const Index3D& neighbor_index : - Grid<3>(node_index.position.array() - padding, - node_index.position.array() + padding)) { - const FloatingPoint neighbor_occupancy_value = - map.getCellValue(neighbor_index); - if (classifier.is(neighbor_occupancy_value, Occupancy::kFree)) { - const auto free_cell_aabb = convert::nodeIndexToAABB( - OctreeIndex{0, neighbor_index}, min_cell_width); - const FloatingPoint min_dist = - free_cell_aabb.minDistanceTo(node_center); - if (min_dist < sdf_brute_force) { - sdf_brute_force = min_dist; - parent_brute_force = neighbor_index; - } - } - } - // Adjust the sign to reflect we're inside the obstacle - sdf_brute_force = -sdf_brute_force; - - // In occupied space, the SDF should be - if (std::abs(sdf_brute_force) < sdf.getDefaultValue()) { - // Negative - EXPECT_LT(sdf_value, 0.f); - } else { - // Or uninitialized - EXPECT_NEAR(sdf_value, sdf.getDefaultValue(), kEpsilon); - } - } - - // Check that the SDF accurately approximates the min obstacle distance - constexpr FloatingPoint kMaxRelativeUnderEstimate = - TypeParam::kMaxRelativeUnderEstimate; - constexpr FloatingPoint kMaxRelativeOverEstimate = - TypeParam::kMaxRelativeOverEstimate; - if (std::abs(sdf_brute_force) < sdf.getDefaultValue()) { - if (0.f < sdf_brute_force) { - EXPECT_LT(sdf_value, - sdf_brute_force * (1.f + kMaxRelativeOverEstimate)) - << "At index " << print::eigen::oneLine(node_index.position) - << " with nearest obstacle " - << print::eigen::oneLine(parent_brute_force); - EXPECT_GT(sdf_value, - sdf_brute_force * (1.f - kMaxRelativeUnderEstimate)) - << "At index " << print::eigen::oneLine(node_index.position) - << " with nearest obstacle " - << print::eigen::oneLine(parent_brute_force); - } else { - EXPECT_GT(sdf_value, - sdf_brute_force * (1.f + kMaxRelativeOverEstimate)) - << "At index " << print::eigen::oneLine(node_index.position) - << " with nearest free cell " - << print::eigen::oneLine(parent_brute_force); - EXPECT_LT(sdf_value, - sdf_brute_force * (1.f - kMaxRelativeUnderEstimate)) - << "At index " << print::eigen::oneLine(node_index.position) - << " with nearest free cell " - << print::eigen::oneLine(parent_brute_force); - } - } else { - EXPECT_LT(sdf_value, - sdf.getDefaultValue() * (1.f + kMaxRelativeOverEstimate)) - << "At index " << print::eigen::oneLine(node_index.position); - EXPECT_GT(sdf_value, - sdf.getDefaultValue() * (1.f - kMaxRelativeUnderEstimate)) - << "At index " << print::eigen::oneLine(node_index.position); - } - }); - } -} -} // namespace wavemap From 8286b152ced5657ae85e0bc11b7620c1d67b72d4 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 15 Jan 2024 16:20:23 +0100 Subject: [PATCH 062/159] Improve readability of initializer --- .../include/wavemap/utils/iterate/grid_iterator.h | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/libraries/wavemap/include/wavemap/utils/iterate/grid_iterator.h b/libraries/wavemap/include/wavemap/utils/iterate/grid_iterator.h index f9244369e..133372371 100644 --- a/libraries/wavemap/include/wavemap/utils/iterate/grid_iterator.h +++ b/libraries/wavemap/include/wavemap/utils/iterate/grid_iterator.h @@ -22,12 +22,11 @@ class Grid { explicit Iterator(const Grid& grid) : grid_(grid), current_index_(grid_.min_index_) {} - Iterator(const Grid& grid, bool end) - : grid_(grid), - current_index_( - end ? Index::Unit(dim - 1).select( - grid_.max_index_ + Index::Ones(), grid_.min_index_) - : grid_.min_index_) {} + Iterator(const Grid& grid, bool end) : Iterator(grid) { + if (end) { + current_index_[dim - 1] = grid_.max_index_[dim - 1] + 1; + } + } const Index& operator*() const { return current_index_; } Iterator& operator++() { // prefix ++ From 5157074d2fc0cbb93564feb7d1ab812655ba102b Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 15 Jan 2024 21:48:00 +0100 Subject: [PATCH 063/159] Minor refactoring --- examples/src/planning/occupancy_to_esdf.cc | 4 ++-- .../include/wavemap/data_structure/bucket_queue.h | 9 +++++++++ .../wavemap/data_structure/impl/bucket_queue_inl.h | 8 +------- .../wavemap/utils/query/impl/classified_map_inl.h | 2 +- 4 files changed, 13 insertions(+), 10 deletions(-) diff --git a/examples/src/planning/occupancy_to_esdf.cc b/examples/src/planning/occupancy_to_esdf.cc index 502fa8bd0..a2e1a2e3d 100644 --- a/examples/src/planning/occupancy_to_esdf.cc +++ b/examples/src/planning/occupancy_to_esdf.cc @@ -41,8 +41,8 @@ int main(int argc, char** argv) { const std::filesystem::path esdf_file_path = std::filesystem::path(map_file_path).replace_extension(".sdf.wvmp"); LOG(INFO) << "Generating ESDF"; - constexpr FloatingPoint kOccupancyThreshold = 0.f; - constexpr FloatingPoint kMaxDistance = 2.f; + constexpr FloatingPoint kOccupancyThreshold = 0.01f; + constexpr FloatingPoint kMaxDistance = 10.f; const QuasiEuclideanSDFGenerator sdf_generator{kMaxDistance, kOccupancyThreshold}; const auto esdf = sdf_generator.generate(*hashed_map); diff --git a/libraries/wavemap/include/wavemap/data_structure/bucket_queue.h b/libraries/wavemap/include/wavemap/data_structure/bucket_queue.h index 209184a68..642753709 100644 --- a/libraries/wavemap/include/wavemap/data_structure/bucket_queue.h +++ b/libraries/wavemap/include/wavemap/data_structure/bucket_queue.h @@ -31,6 +31,15 @@ class BucketQueue { void pop(); ValueT front(); + int computeBucketIndex(FloatingPoint key) const { + int bucket_index = + std::floor(std::abs(key) / max_key_ * (num_buckets_ - 1)); + if (num_buckets_ <= bucket_index) { + bucket_index = num_buckets_ - 1; + } + return bucket_index; + } + private: int num_buckets_; FloatingPoint max_key_; diff --git a/libraries/wavemap/include/wavemap/data_structure/impl/bucket_queue_inl.h b/libraries/wavemap/include/wavemap/data_structure/impl/bucket_queue_inl.h index 97ad085ba..76cf2321c 100644 --- a/libraries/wavemap/include/wavemap/data_structure/impl/bucket_queue_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/impl/bucket_queue_inl.h @@ -34,13 +34,7 @@ void BucketQueue::clear() { template void BucketQueue::push(FloatingPoint key, const ValueT& value) { DCHECK_NE(num_buckets_, 0); - if (key > max_key_) { - key = max_key_; - } - int bucket_index = std::floor(std::abs(key) / max_key_ * (num_buckets_ - 1)); - if (bucket_index >= num_buckets_) { - bucket_index = num_buckets_ - 1; - } + const int bucket_index = computeBucketIndex(key); if (bucket_index < last_bucket_index_) { last_bucket_index_ = bucket_index; } diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h index cfc8aa6c1..797a624c4 100644 --- a/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h +++ b/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h @@ -22,7 +22,7 @@ inline void ClassifiedMap::forEachLeafMatching( termination_height); } -Occupancy::Mask ClassifiedMap::NodeData::occupancyMask() const { +inline Occupancy::Mask ClassifiedMap::NodeData::occupancyMask() const { return Occupancy::toMask(has_free.any(), has_occupied.any(), has_unobserved.any()); } From 6a9b05d21a5171308b99121387d2bccb61692acf Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 16 Jan 2024 15:57:44 +0100 Subject: [PATCH 064/159] Test classified map --- libraries/wavemap/CMakeLists.txt | 1 + .../wavemap/utils/query/classified_map.h | 24 +++--- .../utils/query/impl/classified_map_inl.h | 41 ++++++++++ .../query/impl/occupancy_classifier_inl.h | 48 +++++++----- .../utils/query/occupancy_classifier.h | 6 +- .../src/utils/query/test_classified_map.cc | 76 +++++++++++++++++++ 6 files changed, 164 insertions(+), 32 deletions(-) create mode 100644 libraries/wavemap/test/src/utils/query/test_classified_map.cc diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index e8a753af9..018e8be9d 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -122,6 +122,7 @@ if (CATKIN_ENABLE_TESTING) test/src/utils/neighbors/test_grid_adjacency.cc test/src/utils/neighbors/test_grid_neighborhood.cc test/src/utils/neighbors/test_ndtree_adjacency.cc + test/src/utils/query/test_classified_map.cc test/src/utils/query/test_map_interpolator.cpp test/src/utils/query/test_occupancy_classifier.cc test/src/utils/query/test_probability_conversions.cc diff --git a/libraries/wavemap/include/wavemap/utils/query/classified_map.h b/libraries/wavemap/include/wavemap/utils/query/classified_map.h index b194e98eb..f443827d7 100644 --- a/libraries/wavemap/include/wavemap/utils/query/classified_map.h +++ b/libraries/wavemap/include/wavemap/utils/query/classified_map.h @@ -1,6 +1,8 @@ #ifndef WAVEMAP_UTILS_QUERY_CLASSIFIED_MAP_H_ #define WAVEMAP_UTILS_QUERY_CLASSIFIED_MAP_H_ +#include + #include #include #include @@ -18,6 +20,7 @@ class ClassifiedMap { Occupancy::Mask childOccupancyMask( NdtreeIndexRelativeChild child_idx) const; }; + using HeightType = IndexElement; using BlockHashMap = OctreeBlockHash; using Block = BlockHashMap::Block; using Node = BlockHashMap::Node; @@ -47,21 +50,18 @@ class ClassifiedMap { bool isFully(const OctreeIndex& index, Occupancy::Id occupancy_type) const; bool isFully(const OctreeIndex& index, Occupancy::Mask occupancy_mask) const; - bool hasBlock(const Index3D& block_index) const { - return block_map_.hasBlock(block_index); - } - Block* getBlock(const Index3D& block_index) { - return block_map_.getBlock(block_index); - } - const Block* getBlock(const Index3D& block_index) const { - return block_map_.getBlock(block_index); - } - Block& getOrAllocateBlock(const Index3D& block_index) { - return block_map_.getOrAllocateBlock(block_index); - } + bool hasBlock(const Index3D& block_index) const; + Block* getBlock(const Index3D& block_index); + const Block* getBlock(const Index3D& block_index) const; + Block& getOrAllocateBlock(const Index3D& block_index); BlockHashMap& getBlockMap() { return block_map_; } const BlockHashMap& getBlockMap() const { return block_map_; } + bool hasValue(const OctreeIndex& index) const; + std::optional getValue(const OctreeIndex& index) const; + std::pair, HeightType> getValueOrAncestor( + const OctreeIndex& index) const; + void forEachLeafMatching(Occupancy::Id occupancy_type, std::function visitor_fn, IndexElement termination_height = 0) const; diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h index 797a624c4..20fc16f02 100644 --- a/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h +++ b/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h @@ -32,6 +32,47 @@ inline Occupancy::Mask ClassifiedMap::NodeData::childOccupancyMask( return Occupancy::toMask(has_free[child_idx], has_occupied[child_idx], has_unobserved[child_idx]); } + +inline bool ClassifiedMap::hasBlock(const Index3D& block_index) const { + return block_map_.hasBlock(block_index); +} + +inline ClassifiedMap::Block* ClassifiedMap::getBlock( + const Index3D& block_index) { + return block_map_.getBlock(block_index); +} + +inline const ClassifiedMap::Block* ClassifiedMap::getBlock( + const Index3D& block_index) const { + return block_map_.getBlock(block_index); +} + +inline ClassifiedMap::Block& ClassifiedMap::getOrAllocateBlock( + const Index3D& block_index) { + return block_map_.getOrAllocateBlock(block_index); +} + +inline bool ClassifiedMap::hasValue(const OctreeIndex& index) const { + return block_map_.hasValue(index.computeParentIndex()); +} + +inline std::optional ClassifiedMap::getValue( + const OctreeIndex& index) const { + return getValueOrAncestor(index).first; +} + +inline std::pair, ClassifiedMap::HeightType> +ClassifiedMap::getValueOrAncestor(const OctreeIndex& index) const { + const auto [parent, parent_height] = + block_map_.getValueOrAncestor(index.computeParentIndex()); + if (parent) { + const MortonIndex morton = convert::nodeIndexToMorton(index); + const NdtreeIndexRelativeChild relative_child_idx = + OctreeIndex::computeRelativeChildIndex(morton, parent_height); + return {parent->childOccupancyMask(relative_child_idx), parent_height - 1}; + } + return {std::nullopt, getTreeHeight()}; +} } // namespace wavemap #endif // WAVEMAP_UTILS_QUERY_IMPL_CLASSIFIED_MAP_INL_H_ diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_classifier_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_classifier_inl.h index c6f3f8e4d..2a07aa63d 100644 --- a/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_classifier_inl.h +++ b/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_classifier_inl.h @@ -2,18 +2,20 @@ #define WAVEMAP_UTILS_QUERY_IMPL_OCCUPANCY_CLASSIFIER_INL_H_ namespace wavemap { -constexpr bool OccupancyClassifier::isUnobserved( - FloatingPoint log_odds_occupancy) { - return std::abs(log_odds_occupancy) < kUnobservedThreshold; -} - -constexpr bool OccupancyClassifier::isObserved( - FloatingPoint log_odds_occupancy) { - return !isUnobserved(log_odds_occupancy); +inline constexpr Occupancy::Id OccupancyClassifier::classify( + FloatingPoint log_odds_occupancy) const { + if (isUnobserved(log_odds_occupancy)) { + return Occupancy::kUnobserved; + } + if (log_odds_occupancy < occupancy_threshold_) { + return Occupancy::kFree; + } else { + return Occupancy::kOccupied; + } } -constexpr bool OccupancyClassifier::is(FloatingPoint log_odds_occupancy, - Occupancy::Id occupancy_type) const { +inline constexpr bool OccupancyClassifier::is( + FloatingPoint log_odds_occupancy, Occupancy::Id occupancy_type) const { switch (occupancy_type) { case Occupancy::kFree: return isObserved(log_odds_occupancy) && @@ -30,29 +32,39 @@ constexpr bool OccupancyClassifier::is(FloatingPoint log_odds_occupancy, } } -constexpr bool OccupancyClassifier::has(Occupancy::Mask region_occupancy, - Occupancy::Id occupancy_type) { +inline constexpr bool OccupancyClassifier::has(Occupancy::Mask region_occupancy, + Occupancy::Id occupancy_type) { return has(region_occupancy, Occupancy::toMask(occupancy_type)); } -constexpr bool OccupancyClassifier::has(Occupancy::Mask region_occupancy, - Occupancy::Mask occupancy_mask) { +inline constexpr bool OccupancyClassifier::has(Occupancy::Mask region_occupancy, + Occupancy::Mask occupancy_mask) { // Check that at least one bit in the region matches the mask return region_occupancy & occupancy_mask; } -constexpr bool OccupancyClassifier::isFully(Occupancy::Mask region_occupancy, - Occupancy::Id occupancy_type) { +inline constexpr bool OccupancyClassifier::isFully( + Occupancy::Mask region_occupancy, Occupancy::Id occupancy_type) { return isFully(region_occupancy, Occupancy::toMask(occupancy_type)); } -constexpr bool OccupancyClassifier::isFully(Occupancy::Mask region_occupancy, - Occupancy::Mask occupancy_mask) { +inline constexpr bool OccupancyClassifier::isFully( + Occupancy::Mask region_occupancy, Occupancy::Mask occupancy_mask) { // Set stray bits beyond mask width to 0, as they should not influence result const Occupancy::Mask region_occ_trimmed = region_occupancy & 0b111; // Check that no bits in the region are set while not being in the mask return !(region_occ_trimmed & ~occupancy_mask); } + +inline constexpr bool OccupancyClassifier::isUnobserved( + FloatingPoint log_odds_occupancy) { + return std::abs(log_odds_occupancy) < kUnobservedThreshold; +} + +inline constexpr bool OccupancyClassifier::isObserved( + FloatingPoint log_odds_occupancy) { + return !isUnobserved(log_odds_occupancy); +} } // namespace wavemap #endif // WAVEMAP_UTILS_QUERY_IMPL_OCCUPANCY_CLASSIFIER_INL_H_ diff --git a/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h b/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h index 66eb50317..fb05cae6f 100644 --- a/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h +++ b/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h @@ -12,8 +12,7 @@ class OccupancyClassifier { static FloatingPoint getUnobservedThreshold() { return kUnobservedThreshold; } FloatingPoint getOccupancyThreshold() const { return occupancy_threshold_; } - static constexpr bool isUnobserved(FloatingPoint log_odds_occupancy); - static constexpr bool isObserved(FloatingPoint log_odds_occupancy); + constexpr Occupancy::Id classify(FloatingPoint log_odds_occupancy) const; constexpr bool is(FloatingPoint log_odds_occupancy, Occupancy::Id occupancy_type) const; @@ -28,6 +27,9 @@ class OccupancyClassifier { static constexpr bool isFully(Occupancy::Mask region_occupancy, Occupancy::Mask occupancy_mask); + static constexpr bool isUnobserved(FloatingPoint log_odds_occupancy); + static constexpr bool isObserved(FloatingPoint log_odds_occupancy); + // Forbid implicit Mask type conversions template static constexpr bool has(A region_occupancy, B occupancy_mask) = delete; diff --git a/libraries/wavemap/test/src/utils/query/test_classified_map.cc b/libraries/wavemap/test/src/utils/query/test_classified_map.cc new file mode 100644 index 000000000..7a7054f9b --- /dev/null +++ b/libraries/wavemap/test/src/utils/query/test_classified_map.cc @@ -0,0 +1,76 @@ +#include + +#include "wavemap/common.h" +#include "wavemap/map/hashed_wavelet_octree.h" +#include "wavemap/test/config_generator.h" +#include "wavemap/test/fixture_base.h" +#include "wavemap/test/geometry_generator.h" +#include "wavemap/utils/iterate/grid_iterator.h" +#include "wavemap/utils/query/classified_map.h" +#include "wavemap/utils/query/query_accelerator.h" + +namespace wavemap { +class ClassifiedMapTest : public FixtureBase, + public GeometryGenerator, + public ConfigGenerator {}; + +TEST_F(ClassifiedMapTest, ClassificationResults) { + constexpr int kNumRepetitions = 3; + for (int i = 0; i < kNumRepetitions; ++i) { + // Create a random map + const auto config = + ConfigGenerator::getRandomConfig(); + HashedWaveletOctree map{config}; + const std::vector random_indices = + GeometryGenerator::getRandomIndexVector<3>( + 1000u, 2000u, Index3D::Constant(-5000), Index3D::Constant(5000)); + for (const Index3D& index : random_indices) { + const FloatingPoint update = getRandomUpdate(); + map.addToCellValue(index, update); + } + map.prune(); + + // Generate the classified map + const OccupancyClassifier classifier; + const ClassifiedMap classified_map{map, classifier}; + + // Test all leaves + map.forEachLeaf( + [&map, &classifier, &classified_map](const OctreeIndex& cell_index, + FloatingPoint cell_log_odds) { + const auto cell_occupancy_type = classifier.classify(cell_log_odds); + EXPECT_TRUE(classified_map.isFully(cell_index, cell_occupancy_type)); + for (IndexElement height = cell_index.height; + height < map.getTreeHeight(); ++height) { + EXPECT_TRUE(classified_map.has( + cell_index.computeParentIndex(height), cell_occupancy_type)); + } + }); + + // Test subregions + QueryAccelerator map_query_accelerator{map}; + for (int test_idx = 0; test_idx < 100; ++test_idx) { + const IndexElement region_height = getRandomNdtreeIndexHeight(0, 4); + const auto subregion = + OctreeIndex{0, random_indices[test_idx]}.computeParentIndex( + region_height); + + bool has_free = false; + bool has_occupied = false; + bool has_unobserved = false; + for (const auto& index : + Grid(convert::nodeIndexToMinCornerIndex(subregion), + convert::nodeIndexToMaxCornerIndex(subregion))) { + const FloatingPoint cell_log_odds = + map_query_accelerator.getCellValue(index); + has_free |= classifier.is(cell_log_odds, Occupancy::kFree); + has_occupied |= classifier.is(cell_log_odds, Occupancy::kOccupied); + has_unobserved |= classifier.is(cell_log_odds, Occupancy::kUnobserved); + } + + EXPECT_EQ(classified_map.getValue(subregion), + Occupancy::toMask(has_free, has_occupied, has_unobserved)); + } + } +} +} // namespace wavemap From 7e616099d1b8bfd9750c53c7b58d6fbee6e2e489 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 17 Jan 2024 11:45:58 +0100 Subject: [PATCH 065/159] Remove unsafe optimizations --- .../wavemap/src/utils/query/classified_map.cc | 28 ++++--------------- .../src/utils/query/test_classified_map.cc | 5 +--- 2 files changed, 7 insertions(+), 26 deletions(-) diff --git a/libraries/wavemap/src/utils/query/classified_map.cc b/libraries/wavemap/src/utils/query/classified_map.cc index 0c92424b7..ea5f1dce1 100644 --- a/libraries/wavemap/src/utils/query/classified_map.cc +++ b/libraries/wavemap/src/utils/query/classified_map.cc @@ -13,17 +13,9 @@ void ClassifiedMap::update(const HashedWaveletOctree& occupancy_map) { bool ClassifiedMap::has(const OctreeIndex& index, Occupancy::Mask occupancy_mask) const { - // Cache the last block index - static Index3D block_index = - Index3D::Constant(std::numeric_limits::max()); - static const Block* block = nullptr; - - // Fetch the current block if not already cached - if (const Index3D new_block_index = block_map_.indexToBlockIndex(index); - new_block_index != block_index) { - block_index = new_block_index; - block = block_map_.getBlock(block_index); - } + // Fetch the current block + const Index3D block_index = block_map_.indexToBlockIndex(index); + const Block* block = block_map_.getBlock(block_index); // If the block doesn't exist, we're done if (!block) { @@ -53,17 +45,9 @@ bool ClassifiedMap::has(const OctreeIndex& index, bool ClassifiedMap::isFully(const OctreeIndex& index, Occupancy::Mask occupancy_mask) const { - // Cache the last block index - static Index3D block_index = - Index3D::Constant(std::numeric_limits::max()); - static const Block* block = nullptr; - - // Fetch the current block if not already cached - if (const Index3D new_block_index = block_map_.indexToBlockIndex(index); - new_block_index != block_index) { - block_index = new_block_index; - block = block_map_.getBlock(block_index); - } + // Fetch the current block + const Index3D block_index = block_map_.indexToBlockIndex(index); + const Block* block = block_map_.getBlock(block_index); // If the block doesn't exist, we're done if (!block) { diff --git a/libraries/wavemap/test/src/utils/query/test_classified_map.cc b/libraries/wavemap/test/src/utils/query/test_classified_map.cc index 7a7054f9b..321f93225 100644 --- a/libraries/wavemap/test/src/utils/query/test_classified_map.cc +++ b/libraries/wavemap/test/src/utils/query/test_classified_map.cc @@ -7,7 +7,6 @@ #include "wavemap/test/geometry_generator.h" #include "wavemap/utils/iterate/grid_iterator.h" #include "wavemap/utils/query/classified_map.h" -#include "wavemap/utils/query/query_accelerator.h" namespace wavemap { class ClassifiedMapTest : public FixtureBase, @@ -48,7 +47,6 @@ TEST_F(ClassifiedMapTest, ClassificationResults) { }); // Test subregions - QueryAccelerator map_query_accelerator{map}; for (int test_idx = 0; test_idx < 100; ++test_idx) { const IndexElement region_height = getRandomNdtreeIndexHeight(0, 4); const auto subregion = @@ -61,8 +59,7 @@ TEST_F(ClassifiedMapTest, ClassificationResults) { for (const auto& index : Grid(convert::nodeIndexToMinCornerIndex(subregion), convert::nodeIndexToMaxCornerIndex(subregion))) { - const FloatingPoint cell_log_odds = - map_query_accelerator.getCellValue(index); + const FloatingPoint cell_log_odds = map.getCellValue(index); has_free |= classifier.is(cell_log_odds, Occupancy::kFree); has_occupied |= classifier.is(cell_log_odds, Occupancy::kOccupied); has_unobserved |= classifier.is(cell_log_odds, Occupancy::kUnobserved); From 7b81ca99a7c2c2d0377763f0c89b99a3572fbb66 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 19 Jan 2024 17:16:23 +0100 Subject: [PATCH 066/159] Also call callback for partial matches at termination height --- libraries/wavemap/src/utils/query/classified_map.cc | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/libraries/wavemap/src/utils/query/classified_map.cc b/libraries/wavemap/src/utils/query/classified_map.cc index ea5f1dce1..d5ae95e8d 100644 --- a/libraries/wavemap/src/utils/query/classified_map.cc +++ b/libraries/wavemap/src/utils/query/classified_map.cc @@ -1,7 +1,10 @@ #include "wavemap/utils/query/classified_map.h" +#include + namespace wavemap { void ClassifiedMap::update(const HashedWaveletOctree& occupancy_map) { + ZoneScoped; occupancy_map.forEachBlock( [this](const Index3D& block_index, const auto& occupancy_block) { auto& classified_block = block_map_.getOrAllocateBlock(block_index); @@ -101,10 +104,11 @@ void ClassifiedMap::forEachLeafMatching( } const OctreeIndex child_node_index = node_index.computeChildIndex(child_idx); - if (OccupancyClassifier::isFully(region_occupancy, occupancy_mask)) { + if (OccupancyClassifier::isFully(region_occupancy, occupancy_mask) || + child_node_index.height <= termination_height) { visitor_fn(child_node_index); } else if (const Node* child_node = node.getChild(child_idx); - child_node && termination_height < child_node_index.height) { + child_node) { stack.emplace(StackElement{child_node_index, *child_node}); } } From 536d8f322584182bba55f2a566c7ea33782223da Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 23 Jan 2024 16:51:14 +0100 Subject: [PATCH 067/159] Accelerate all ClassifiedMap query types --- .../wavemap/utils/query/classified_map.h | 41 +++- .../utils/query/impl/classified_map_inl.h | 54 +++-- .../utils/query/impl/query_accelerator_inl.h | 14 ++ .../wavemap/utils/query/query_accelerator.h | 6 + .../wavemap/src/utils/query/classified_map.cc | 225 +++++++++++++----- .../src/utils/query/query_accelerator.cc | 20 +- .../src/utils/query/test_classified_map.cc | 20 +- 7 files changed, 287 insertions(+), 93 deletions(-) diff --git a/libraries/wavemap/include/wavemap/utils/query/classified_map.h b/libraries/wavemap/include/wavemap/utils/query/classified_map.h index f443827d7..b07f2aab2 100644 --- a/libraries/wavemap/include/wavemap/utils/query/classified_map.h +++ b/libraries/wavemap/include/wavemap/utils/query/classified_map.h @@ -1,6 +1,7 @@ #ifndef WAVEMAP_UTILS_QUERY_CLASSIFIED_MAP_H_ #define WAVEMAP_UTILS_QUERY_CLASSIFIED_MAP_H_ +#include #include #include @@ -30,7 +31,8 @@ class ClassifiedMap { const OccupancyClassifier& classifier) : min_cell_width_(min_cell_width), classifier_(classifier), - block_map_(tree_height) {} + block_map_(tree_height), + query_cache_(tree_height) {} ClassifiedMap(const HashedWaveletOctree& occupancy_map, const OccupancyClassifier& classifier) @@ -51,12 +53,14 @@ class ClassifiedMap { bool isFully(const OctreeIndex& index, Occupancy::Mask occupancy_mask) const; bool hasBlock(const Index3D& block_index) const; - Block* getBlock(const Index3D& block_index); const Block* getBlock(const Index3D& block_index) const; - Block& getOrAllocateBlock(const Index3D& block_index); - BlockHashMap& getBlockMap() { return block_map_; } const BlockHashMap& getBlockMap() const { return block_map_; } + bool hasNode(const OctreeIndex& index) const { return getNode(index); } + const Node* getNode(const OctreeIndex& index) const; + std::pair getNodeOrAncestor( + const OctreeIndex& index) const; + bool hasValue(const OctreeIndex& index) const; std::optional getValue(const OctreeIndex& index) const; std::pair, HeightType> getValueOrAncestor( @@ -74,6 +78,35 @@ class ClassifiedMap { const OccupancyClassifier classifier_; BlockHashMap block_map_; + // Cache previous queries + struct QueryCache { + explicit QueryCache(IndexElement tree_height) : tree_height(tree_height) {} + + const IndexElement tree_height; + + Index3D block_index = + Index3D::Constant(std::numeric_limits::max()); + IndexElement height = tree_height; + MortonIndex morton_code = std::numeric_limits::max(); + + const Block* block = nullptr; + std::array> node_stack{}; + + const Block* getBlock(const Index3D& block_index, + const BlockHashMap& block_map); + std::pair getNodeOrAncestor( + const OctreeIndex& index, const BlockHashMap& block_map); + // TODO(victorr): Write more unit tests for these accelerated accessors + + bool has(const OctreeIndex& index, Occupancy::Mask occupancy_mask, + const BlockHashMap& block_map); + bool isFully(const OctreeIndex& index, Occupancy::Mask occupancy_mask, + const BlockHashMap& block_map); + + void reset(); + }; + mutable QueryCache query_cache_; + void recursiveClassifier( const HashedWaveletOctreeBlock::NodeType& occupancy_node, FloatingPoint average_occupancy, Node& classified_node); diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h index 20fc16f02..dca1b7886 100644 --- a/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h +++ b/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h @@ -1,6 +1,7 @@ #ifndef WAVEMAP_UTILS_QUERY_IMPL_CLASSIFIED_MAP_INL_H_ #define WAVEMAP_UTILS_QUERY_IMPL_CLASSIFIED_MAP_INL_H_ +#include #include namespace wavemap { @@ -9,11 +10,21 @@ inline bool ClassifiedMap::has(const OctreeIndex& index, return has(index, Occupancy::toMask(occupancy_type)); } +inline bool ClassifiedMap::has(const OctreeIndex& index, + Occupancy::Mask occupancy_mask) const { + return query_cache_.has(index, occupancy_mask, block_map_); +} + inline bool ClassifiedMap::isFully(const OctreeIndex& index, Occupancy::Id occupancy_type) const { return isFully(index, Occupancy::toMask(occupancy_type)); } +inline bool ClassifiedMap::isFully(const OctreeIndex& index, + Occupancy::Mask occupancy_mask) const { + return query_cache_.isFully(index, occupancy_mask, block_map_); +} + inline void ClassifiedMap::forEachLeafMatching( Occupancy::Id occupancy_type, std::function visitor_fn, @@ -34,26 +45,26 @@ inline Occupancy::Mask ClassifiedMap::NodeData::childOccupancyMask( } inline bool ClassifiedMap::hasBlock(const Index3D& block_index) const { - return block_map_.hasBlock(block_index); -} - -inline ClassifiedMap::Block* ClassifiedMap::getBlock( - const Index3D& block_index) { - return block_map_.getBlock(block_index); + return getBlock(block_index); } inline const ClassifiedMap::Block* ClassifiedMap::getBlock( const Index3D& block_index) const { - return block_map_.getBlock(block_index); + return query_cache_.getBlock(block_index, block_map_); } -inline ClassifiedMap::Block& ClassifiedMap::getOrAllocateBlock( - const Index3D& block_index) { - return block_map_.getOrAllocateBlock(block_index); +inline std::pair +ClassifiedMap::getNodeOrAncestor(const OctreeIndex& index) const { + return query_cache_.getNodeOrAncestor(index, block_map_); +} + +inline const ClassifiedMap::Node* ClassifiedMap::getNode( + const OctreeIndex& index) const { + return getNodeOrAncestor(index).first; } inline bool ClassifiedMap::hasValue(const OctreeIndex& index) const { - return block_map_.hasValue(index.computeParentIndex()); + return getValue(index).has_value(); } inline std::optional ClassifiedMap::getValue( @@ -61,17 +72,18 @@ inline std::optional ClassifiedMap::getValue( return getValueOrAncestor(index).first; } -inline std::pair, ClassifiedMap::HeightType> -ClassifiedMap::getValueOrAncestor(const OctreeIndex& index) const { - const auto [parent, parent_height] = - block_map_.getValueOrAncestor(index.computeParentIndex()); - if (parent) { - const MortonIndex morton = convert::nodeIndexToMorton(index); - const NdtreeIndexRelativeChild relative_child_idx = - OctreeIndex::computeRelativeChildIndex(morton, parent_height); - return {parent->childOccupancyMask(relative_child_idx), parent_height - 1}; +inline const ClassifiedMap::Block* ClassifiedMap::QueryCache::getBlock( + const Index3D& new_block_index, + const ClassifiedMap::BlockHashMap& block_map) { + if (new_block_index != block_index) { + block_index = new_block_index; + height = tree_height; + block = block_map.getBlock(new_block_index); + if (block) { + node_stack[tree_height] = &block->getRootNode(); + } } - return {std::nullopt, getTreeHeight()}; + return block; } } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h index 8a7968e0d..4235c9e2d 100644 --- a/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h +++ b/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h @@ -1,9 +1,17 @@ #ifndef WAVEMAP_UTILS_QUERY_IMPL_QUERY_ACCELERATOR_INL_H_ #define WAVEMAP_UTILS_QUERY_IMPL_QUERY_ACCELERATOR_INL_H_ +#include #include namespace wavemap { +template +void QueryAccelerator>::reset() { + last_block_index_ = + Index3D::Constant(std::numeric_limits::max()); + last_block_ = nullptr; +} + template BlockDataT* QueryAccelerator>::getBlock( const Index& block_index) { @@ -26,6 +34,12 @@ BlockDataT& QueryAccelerator>::getOrAllocateBlock( return *last_block_; } +template +void QueryAccelerator>::reset() { + block_index_ = Index::Constant(std::numeric_limits::max()); + block_ = nullptr; +} + template typename QueryAccelerator>::BlockType* QueryAccelerator>::getBlock( diff --git a/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h b/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h index 7b23721fb..f86327a9d 100644 --- a/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h +++ b/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h @@ -26,6 +26,8 @@ class QueryAccelerator> { explicit QueryAccelerator(SpatialHash& spatial_hash) : spatial_hash_(spatial_hash) {} + void reset(); + BlockDataT* getBlock(const Index& block_index); template BlockDataT& getOrAllocateBlock(const Index& block_index, @@ -50,6 +52,8 @@ class QueryAccelerator> { explicit QueryAccelerator(NdtreeBlockHash& ndtree_block_hash) : ndtree_block_hash_(ndtree_block_hash) {} + void reset(); + BlockType* getBlock(const Index& block_index); template BlockType& getOrAllocateBlock(const Index& block_index, @@ -74,6 +78,8 @@ class QueryAccelerator { explicit QueryAccelerator(const HashedWaveletOctree& map) : map_(map) {} + void reset(); + FloatingPoint getCellValue(const Index3D& index) { return getCellValue(OctreeIndex{0, index}); } diff --git a/libraries/wavemap/src/utils/query/classified_map.cc b/libraries/wavemap/src/utils/query/classified_map.cc index d5ae95e8d..440fbb431 100644 --- a/libraries/wavemap/src/utils/query/classified_map.cc +++ b/libraries/wavemap/src/utils/query/classified_map.cc @@ -5,6 +5,7 @@ namespace wavemap { void ClassifiedMap::update(const HashedWaveletOctree& occupancy_map) { ZoneScoped; + query_cache_.reset(); occupancy_map.forEachBlock( [this](const Index3D& block_index, const auto& occupancy_block) { auto& classified_block = block_map_.getOrAllocateBlock(block_index); @@ -14,68 +15,19 @@ void ClassifiedMap::update(const HashedWaveletOctree& occupancy_map) { }); } -bool ClassifiedMap::has(const OctreeIndex& index, - Occupancy::Mask occupancy_mask) const { - // Fetch the current block - const Index3D block_index = block_map_.indexToBlockIndex(index); - const Block* block = block_map_.getBlock(block_index); - - // If the block doesn't exist, we're done - if (!block) { - return false; - } - - // Otherwise, descend the tree - const MortonIndex morton_code = convert::nodeIndexToMorton(index); - const Node* node = &block->getRootNode(); - for (int parent_height = block_map_.getMaxHeight();; --parent_height) { - if (!node || parent_height <= index.height) { - // Return the result of OccupancyClassifier::has(region_occupancy, - // occupancy_mask), which is always true if this branch is reached. - return true; - } - const NdtreeIndexRelativeChild child_idx = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - const auto region_occupancy = node->data().childOccupancyMask(child_idx); - if (OccupancyClassifier::isFully(region_occupancy, occupancy_mask)) { - return true; - } else if (!OccupancyClassifier::has(region_occupancy, occupancy_mask)) { - return false; - } - node = node->getChild(child_idx); - } -} - -bool ClassifiedMap::isFully(const OctreeIndex& index, - Occupancy::Mask occupancy_mask) const { - // Fetch the current block - const Index3D block_index = block_map_.indexToBlockIndex(index); - const Block* block = block_map_.getBlock(block_index); - - // If the block doesn't exist, we're done - if (!block) { - return false; - } - - // Otherwise, descend the tree - const MortonIndex morton_code = convert::nodeIndexToMorton(index); - const Node* node = &block->getRootNode(); - for (int parent_height = block_map_.getMaxHeight();; --parent_height) { - if (!node || parent_height <= index.height) { - // Return the result of OccupancyClassifier::isFully(region_occupancy, - // occupancy_mask), which is always false if this branch is reached. - return false; - } - const NdtreeIndexRelativeChild child_idx = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - const auto region_occupancy = node->data().childOccupancyMask(child_idx); - if (OccupancyClassifier::isFully(region_occupancy, occupancy_mask)) { - return true; - } else if (!OccupancyClassifier::has(region_occupancy, occupancy_mask)) { - return false; - } - node = node->getChild(child_idx); +std::pair, ClassifiedMap::HeightType> +ClassifiedMap::getValueOrAncestor(const OctreeIndex& index) const { + const OctreeIndex parent_index = index.computeParentIndex(); + const auto [ancestor, ancestor_height] = + query_cache_.getNodeOrAncestor(parent_index, block_map_); + if (ancestor) { + const MortonIndex morton = convert::nodeIndexToMorton(index); + const NdtreeIndexRelativeChild relative_child_idx = + OctreeIndex::computeRelativeChildIndex(morton, ancestor_height); + return {ancestor->data().childOccupancyMask(relative_child_idx), + ancestor_height - 1}; } + return {std::nullopt, getTreeHeight()}; } void ClassifiedMap::forEachLeafMatching( @@ -116,6 +68,157 @@ void ClassifiedMap::forEachLeafMatching( }); } +std::pair +ClassifiedMap::QueryCache::getNodeOrAncestor( + const OctreeIndex& index, const ClassifiedMap::BlockHashMap& block_map) { + // Remember previous query indices and compute new ones + const IndexElement previous_height = height; + const MortonIndex previous_morton_code = morton_code; + morton_code = convert::nodeIndexToMorton(index); + + // Fetch the block if needed and return null if it doesn't exist + if (!getBlock(block_map.indexToBlockIndex(index), block_map)) { + return {nullptr, tree_height}; + } + + // Compute the last ancestor the current and previous query had in common + if (height != tree_height) { + auto last_common_ancestor = OctreeIndex::computeLastCommonAncestorHeight( + morton_code, index.height, previous_morton_code, previous_height); + height = last_common_ancestor; + } + DCHECK_LE(height, tree_height); + + if (height == index.height) { + DCHECK_NOTNULL(node_stack[height]); + return {node_stack[height], height}; + } + + // Walk down the tree from height to index.height + for (; index.height < height;) { + DCHECK_NOTNULL(node_stack[height]); + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, height); + // Check if the child is allocated + const Node* child = node_stack[height]->getChild(child_index); + if (!child) { + return {node_stack[height], height}; + } + node_stack[--height] = child; + } + + return {node_stack[height], height}; +} + +bool ClassifiedMap::QueryCache::has(const OctreeIndex& index, + Occupancy::Mask occupancy_mask, + const BlockHashMap& block_map) { + // Remember previous query indices and compute new ones + const IndexElement previous_height = height; + const MortonIndex previous_morton_code = morton_code; + morton_code = convert::nodeIndexToMorton(index); + + // Fetch the block if needed and return false if it doesn't exist + if (!getBlock(block_map.indexToBlockIndex(index), block_map)) { + return false; + } + + // Compute the last ancestor the current and previous query had in common + if (height != tree_height) { + auto last_common_ancestor = OctreeIndex::computeLastCommonAncestorHeight( + morton_code, index.height, previous_morton_code, previous_height); + height = last_common_ancestor; + } + DCHECK_LE(height, tree_height); + + if (height == index.height) { + DCHECK_NOTNULL(node_stack[height]); + const auto region_occupancy = node_stack[height]->data().occupancyMask(); + return OccupancyClassifier::has(region_occupancy, occupancy_mask); + } + + // Walk down the tree from height to index.height + while (true) { + DCHECK_NOTNULL(node_stack[height]); + const Node* parent_node = node_stack[height]; + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, height); + const auto region_occupancy = + parent_node->data().childOccupancyMask(child_index); + if (OccupancyClassifier::isFully(region_occupancy, occupancy_mask)) { + return true; + } else if (!OccupancyClassifier::has(region_occupancy, occupancy_mask)) { + return false; + } + // Check if the child is allocated + if (height - 1 == index.height || !parent_node->hasChild(child_index)) { + // Return the result of OccupancyClassifier::has(region_occupancy, + // occupancy_mask), which is always true if this branch is reached. + return true; + } + node_stack[--height] = parent_node->getChild(child_index); + } +} + +bool ClassifiedMap::QueryCache::isFully(const OctreeIndex& index, + Occupancy::Mask occupancy_mask, + const BlockHashMap& block_map) { + // Remember previous query indices and compute new ones + const IndexElement previous_height = height; + const MortonIndex previous_morton_code = morton_code; + morton_code = convert::nodeIndexToMorton(index); + + // Fetch the block if needed and return false if it doesn't exist + if (!getBlock(block_map.indexToBlockIndex(index), block_map)) { + return false; + } + + // Compute the last ancestor the current and previous query had in common + if (height != tree_height) { + auto last_common_ancestor = OctreeIndex::computeLastCommonAncestorHeight( + morton_code, index.height, previous_morton_code, previous_height); + height = last_common_ancestor; + } + DCHECK_LE(height, tree_height); + + if (height == index.height) { + DCHECK_NOTNULL(node_stack[height]); + const auto region_occupancy = node_stack[height]->data().occupancyMask(); + return OccupancyClassifier::isFully(region_occupancy, occupancy_mask); + } + + // Walk down the tree from height to index.height + while (true) { + DCHECK_NOTNULL(node_stack[height]); + const Node* parent_node = node_stack[height]; + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, height); + const auto region_occupancy = + parent_node->data().childOccupancyMask(child_index); + if (OccupancyClassifier::isFully(region_occupancy, occupancy_mask)) { + return true; + } else if (!OccupancyClassifier::has(region_occupancy, occupancy_mask)) { + return false; + } + // Check if the child is allocated + if (height - 1 == index.height || !parent_node->hasChild(child_index)) { + // Return the result of OccupancyClassifier::isFully(region_occupancy, + // occupancy_mask), which is always false if this branch is reached. + return false; + } + node_stack[--height] = parent_node->getChild(child_index); + } +} + +void ClassifiedMap::QueryCache::reset() { + block_index = Index3D::Constant(std::numeric_limits::max()); + height = tree_height; + morton_code = std::numeric_limits::max(); + + block = nullptr; + node_stack = std::array>{}; +} + void ClassifiedMap::recursiveClassifier( // NOLINT const HashedWaveletOctreeBlock::NodeType& occupancy_node, FloatingPoint average_occupancy, ClassifiedMap::Node& classified_node) { diff --git a/libraries/wavemap/src/utils/query/query_accelerator.cc b/libraries/wavemap/src/utils/query/query_accelerator.cc index cec86db45..95aece3c3 100644 --- a/libraries/wavemap/src/utils/query/query_accelerator.cc +++ b/libraries/wavemap/src/utils/query/query_accelerator.cc @@ -1,6 +1,15 @@ #include "wavemap/utils/query/query_accelerator.h" namespace wavemap { +void QueryAccelerator::reset() { + node_stack_ = std::array>{}; + value_stack_ = std::array>{}; + + block_index_ = BlockIndex::Constant(std::numeric_limits::max()); + morton_code_ = std::numeric_limits::max(); + height_ = tree_height_; +} + FloatingPoint QueryAccelerator::getCellValue( const OctreeIndex& index) { // Remember previous query indices and compute new ones @@ -12,6 +21,10 @@ FloatingPoint QueryAccelerator::getCellValue( // Check whether we're in the same block as last time if (block_index_ == previous_block_index) { + // If the block is the same, but it doesn't exist, return 'unknown' + if (!node_stack_[tree_height_]) { + return 0.f; + } // Compute the last ancestor the current and previous query had in common auto last_common_ancestor = OctreeIndex::computeLastCommonAncestorHeight( morton_code_, index.height, previous_morton_code, previous_height); @@ -26,9 +39,10 @@ FloatingPoint QueryAccelerator::getCellValue( value_stack_[tree_height_] = current_block->getRootScale(); height_ = tree_height_; } else { - // Otherwise return ignore this query and return 'unknown' - block_index_ = previous_block_index; - morton_code_ = previous_morton_code; + // Otherwise remember that it doesn't exist and return 'unknown' + node_stack_[tree_height_] = nullptr; + value_stack_[tree_height_] = 0.f; + height_ = tree_height_; return 0.f; } } diff --git a/libraries/wavemap/test/src/utils/query/test_classified_map.cc b/libraries/wavemap/test/src/utils/query/test_classified_map.cc index 321f93225..1248f15b0 100644 --- a/libraries/wavemap/test/src/utils/query/test_classified_map.cc +++ b/libraries/wavemap/test/src/utils/query/test_classified_map.cc @@ -33,16 +33,28 @@ TEST_F(ClassifiedMapTest, ClassificationResults) { const OccupancyClassifier classifier; const ClassifiedMap classified_map{map, classifier}; + auto perturb_classified_map_cache = [this, &classified_map]() { + const auto random_index = getRandomNdtreeIndex( + Index3D::Constant(-1000), Index3D::Constant(1000), 0, + classified_map.getTreeHeight()); + classified_map.getNodeOrAncestor(random_index); + }; + // Test all leaves map.forEachLeaf( - [&map, &classifier, &classified_map](const OctreeIndex& cell_index, - FloatingPoint cell_log_odds) { + [&map, &classifier, &classified_map, &perturb_classified_map_cache]( + const OctreeIndex& cell_index, FloatingPoint cell_log_odds) { + perturb_classified_map_cache(); const auto cell_occupancy_type = classifier.classify(cell_log_odds); - EXPECT_TRUE(classified_map.isFully(cell_index, cell_occupancy_type)); + EXPECT_TRUE(classified_map.isFully(cell_index, cell_occupancy_type)) + << "For cell_index: " << cell_index.toString(); for (IndexElement height = cell_index.height; height < map.getTreeHeight(); ++height) { + perturb_classified_map_cache(); EXPECT_TRUE(classified_map.has( - cell_index.computeParentIndex(height), cell_occupancy_type)); + cell_index.computeParentIndex(height), cell_occupancy_type)) + << "For cell_index.computeParentIndex(height): " + << cell_index.computeParentIndex(height).toString(); } }); From 13116f8f8407ed2d4ef18b029bb4d2b98b0d247f Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 23 Jan 2024 20:51:12 +0100 Subject: [PATCH 068/159] Implement accelerated node getters for generic NdtreeBlockHashes --- .../utils/query/impl/query_accelerator_inl.h | 97 ++++++++++++++++--- .../wavemap/utils/query/query_accelerator.h | 8 +- 2 files changed, 93 insertions(+), 12 deletions(-) diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h index 4235c9e2d..089beba41 100644 --- a/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h +++ b/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h @@ -37,7 +37,11 @@ BlockDataT& QueryAccelerator>::getOrAllocateBlock( template void QueryAccelerator>::reset() { block_index_ = Index::Constant(std::numeric_limits::max()); + height = tree_height_; + morton_code = std::numeric_limits::max(); + block_ = nullptr; + node_stack = std::array>{}; } template @@ -46,7 +50,11 @@ QueryAccelerator>::getBlock( const Index& block_index) { if (block_index != block_index_) { block_index_ = block_index; + height = tree_height_; block_ = ndtree_block_hash_.getBlock(block_index); + if (block_) { + node_stack[tree_height_] = &block_->getRootNode(); + } } return block_; } @@ -58,28 +66,95 @@ QueryAccelerator>::getOrAllocateBlock( const Index& block_index, DefaultArgs&&... args) { if (block_index != block_index_ || !block_) { block_index_ = block_index; + height = tree_height_; block_ = &ndtree_block_hash_.getOrAllocateBlock( block_index, std::forward(args)...); + node_stack[tree_height_] = &block_->getRootNode(); } return *block_; } template -const typename QueryAccelerator>::NodeType* +typename QueryAccelerator>::NodeType* QueryAccelerator>::getNode( const OctreeIndex& index) { - // TODO(victorr): Finish implementing this for all heights, - // not just block roots - CHECK_EQ(index.height, tree_height_) - << "Loading non-root nodes is not yet implemented."; - if (index.position != block_index_) { - block_index_ = index.position; - block_ = ndtree_block_hash_.getBlock(index.position); + // Remember previous query indices and compute new ones + const IndexElement previous_height = height; + const MortonIndex previous_morton_code = morton_code; + morton_code = convert::nodeIndexToMorton(index); + + // Fetch the block if needed and return null if it doesn't exist + if (!getBlock(ndtree_block_hash_.indexToBlockIndex(index))) { + return nullptr; + } + + // Compute the last ancestor the current and previous query had in common + if (height != tree_height_) { + auto last_common_ancestor = OctreeIndex::computeLastCommonAncestorHeight( + morton_code, index.height, previous_morton_code, previous_height); + height = last_common_ancestor; + } + DCHECK_LE(height, tree_height_); + + if (height == index.height) { + DCHECK_NOTNULL(node_stack[height]); + return node_stack[height]; + } + + // Walk down the tree from height to index.height + for (; index.height < height;) { + DCHECK_NOTNULL(node_stack[height]); + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, height); + // Check if the child is allocated + NodeType* child = node_stack[height]->getChild(child_index); + if (!child) { + return node_stack[height]; + } + node_stack[--height] = child; + } + + return node_stack[height]; +} + +template +template +typename QueryAccelerator>::NodeType& +QueryAccelerator>::getOrAllocateNode( + const OctreeIndex& index, DefaultArgs&&... args) { + // Remember previous query indices and compute new ones + const IndexElement previous_height = height; + const MortonIndex previous_morton_code = morton_code; + morton_code = convert::nodeIndexToMorton(index); + + // Make sure the block is allocated + getOrAllocateBlock(ndtree_block_hash_.indexToBlockIndex(index)); + + // Compute the last ancestor the current and previous query had in common + if (height != tree_height_) { + auto last_common_ancestor = OctreeIndex::computeLastCommonAncestorHeight( + morton_code, index.height, previous_morton_code, previous_height); + height = last_common_ancestor; } - if (block_) { - return &block_->getRootNode(); + DCHECK_LE(height, tree_height_); + + if (height == index.height) { + DCHECK_NOTNULL(node_stack[height]); + return *node_stack[height]; } - return nullptr; + + // Walk down the tree from height to index.height + for (; index.height < height;) { + DCHECK_NOTNULL(node_stack[height]); + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, height); + // Get or allocate the child + auto& child = node_stack[height]->getOrAllocateChild( + child_index, std::forward(args)...); + node_stack[--height] = &child; + } + + return *node_stack[height]; } } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h b/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h index f86327a9d..d925ad73a 100644 --- a/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h +++ b/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h @@ -59,7 +59,9 @@ class QueryAccelerator> { BlockType& getOrAllocateBlock(const Index& block_index, DefaultArgs&&... args); - const NodeType* getNode(const OctreeIndex& index); + NodeType* getNode(const OctreeIndex& index); + template + NodeType& getOrAllocateNode(const OctreeIndex& index, DefaultArgs&&... args); private: NdtreeBlockHash& ndtree_block_hash_; @@ -67,7 +69,11 @@ class QueryAccelerator> { Index block_index_ = Index::Constant(std::numeric_limits::max()); + IndexElement height = tree_height_; + MortonIndex morton_code = std::numeric_limits::max(); + BlockType* block_ = nullptr; + std::array> node_stack{}; }; // Query accelerator for hashed wavelet octrees From ed29b02c1b7d17bf0714aab728756863f36da6d8 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 23 Jan 2024 21:28:37 +0100 Subject: [PATCH 069/159] Make eraseBlockIf available for more spatial hash data structures --- .../include/wavemap/data_structure/dense_block_hash.h | 3 +++ .../wavemap/data_structure/impl/dense_block_hash_inl.h | 7 +++++++ .../wavemap/data_structure/impl/ndtree_block_hash_inl.h | 7 +++++++ .../include/wavemap/data_structure/ndtree_block_hash.h | 3 +++ 4 files changed, 20 insertions(+) diff --git a/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h b/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h index 1d196f725..54adcff3b 100644 --- a/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h +++ b/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h @@ -26,6 +26,9 @@ class DenseBlockHash { bool hasBlock(const Index& block_index) const; bool eraseBlock(const Index& block_index); + template + void eraseBlockIf(IndexedBlockVisitor indicator_fn); + Block* getBlock(const Index& block_index); const Block* getBlock(const Index& block_index) const; Block& getOrAllocateBlock(const Index& block_index); diff --git a/libraries/wavemap/include/wavemap/data_structure/impl/dense_block_hash_inl.h b/libraries/wavemap/include/wavemap/data_structure/impl/dense_block_hash_inl.h index a7653eeed..d4b69a94e 100644 --- a/libraries/wavemap/include/wavemap/data_structure/impl/dense_block_hash_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/impl/dense_block_hash_inl.h @@ -14,6 +14,13 @@ bool DenseBlockHash::eraseBlock( return block_map_.eraseBlock(block_index); } +template +template +void DenseBlockHash::eraseBlockIf( + IndexedBlockVisitor indicator_fn) { + block_map_.eraseBlockIf(indicator_fn); +} + template inline DenseGrid* DenseBlockHash::getBlock( diff --git a/libraries/wavemap/include/wavemap/data_structure/impl/ndtree_block_hash_inl.h b/libraries/wavemap/include/wavemap/data_structure/impl/ndtree_block_hash_inl.h index b9f74e8bb..0ce0c4492 100644 --- a/libraries/wavemap/include/wavemap/data_structure/impl/ndtree_block_hash_inl.h +++ b/libraries/wavemap/include/wavemap/data_structure/impl/ndtree_block_hash_inl.h @@ -26,6 +26,13 @@ bool NdtreeBlockHash::eraseBlock( return block_map_.eraseBlock(block_index); } +template +template +void NdtreeBlockHash::eraseBlockIf( + IndexedBlockVisitor indicator_fn) { + block_map_.eraseBlockIf(indicator_fn); +} + template inline typename NdtreeBlockHash::Block* NdtreeBlockHash::getBlock(const Index& block_index) { diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h b/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h index 3d3f433fa..3b11aa3ba 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h +++ b/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h @@ -33,6 +33,9 @@ class NdtreeBlockHash { bool hasBlock(const Index& block_index) const; bool eraseBlock(const Index& block_index); + template + void eraseBlockIf(IndexedBlockVisitor indicator_fn); + Block* getBlock(const Index& block_index); const Block* getBlock(const Index& block_index) const; Block& getOrAllocateBlock(const Index& block_index); From b25ad42253e5a5430563c48a692d251aadf41e12 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 23 Jan 2024 21:28:57 +0100 Subject: [PATCH 070/159] Minor improvements to ClassifiedMap --- .../wavemap/src/utils/query/classified_map.cc | 27 +++++++++++++------ 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/libraries/wavemap/src/utils/query/classified_map.cc b/libraries/wavemap/src/utils/query/classified_map.cc index 440fbb431..6f1c5446e 100644 --- a/libraries/wavemap/src/utils/query/classified_map.cc +++ b/libraries/wavemap/src/utils/query/classified_map.cc @@ -5,7 +5,16 @@ namespace wavemap { void ClassifiedMap::update(const HashedWaveletOctree& occupancy_map) { ZoneScoped; + // Reset the query cache query_cache_.reset(); + + // Erase blocks that no longer exist + block_map_.eraseBlockIf( + [&occupancy_map](const Index3D& block_index, const auto& /*block*/) { + return !occupancy_map.hasBlock(block_index); + }); + + // Update all existing blocks occupancy_map.forEachBlock( [this](const Index3D& block_index, const auto& occupancy_block) { auto& classified_block = block_map_.getOrAllocateBlock(block_index); @@ -40,7 +49,7 @@ void ClassifiedMap::forEachLeafMatching( const OctreeIndex node_index; const Node& node; }; - std::stack stack; + std::stack> stack; stack.emplace(StackElement{OctreeIndex{block.getMaxHeight(), block_index}, block.getRootNode()}); while (!stack.empty()) { @@ -58,7 +67,7 @@ void ClassifiedMap::forEachLeafMatching( node_index.computeChildIndex(child_idx); if (OccupancyClassifier::isFully(region_occupancy, occupancy_mask) || child_node_index.height <= termination_height) { - visitor_fn(child_node_index); + std::invoke(visitor_fn, child_node_index); } else if (const Node* child_node = node.getChild(child_idx); child_node) { stack.emplace(StackElement{child_node_index, *child_node}); @@ -247,12 +256,14 @@ void ClassifiedMap::recursiveClassifier( // NOLINT classified_node.eraseChild(child_idx); } } else { // Otherwise, the node is a leaf - classified_node.data().has_free.set( - child_idx, classifier_.is(child_occupancy, Occupancy::kFree)); - classified_node.data().has_occupied.set( - child_idx, classifier_.is(child_occupancy, Occupancy::kOccupied)); - classified_node.data().has_unobserved.set( - child_idx, classifier_.is(child_occupancy, Occupancy::kUnobserved)); + const bool is_free = classifier_.is(child_occupancy, Occupancy::kFree); + const bool is_occupied = + classifier_.is(child_occupancy, Occupancy::kOccupied); + const bool is_unobserved = + classifier_.is(child_occupancy, Occupancy::kUnobserved); + classified_node.data().has_free.set(child_idx, is_free); + classified_node.data().has_occupied.set(child_idx, is_occupied); + classified_node.data().has_unobserved.set(child_idx, is_unobserved); } } } From 234638738a4cb8e7bf11a9475cfe6d4f00bcc792 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 23 Jan 2024 22:13:26 +0100 Subject: [PATCH 071/159] Minor optimizations --- .../wavemap/utils/bits/bit_operations.h | 17 ++++++- .../utils/iterate/impl/ray_iterator_inl.h | 4 +- .../wavemap/utils/query/classified_map.h | 16 ++++-- .../utils/query/impl/classified_map_inl.h | 49 +++++++++++++++++-- .../wavemap/src/utils/query/classified_map.cc | 19 ++++--- 5 files changed, 85 insertions(+), 20 deletions(-) diff --git a/libraries/wavemap/include/wavemap/utils/bits/bit_operations.h b/libraries/wavemap/include/wavemap/utils/bits/bit_operations.h index bc476b742..4036ea3e0 100644 --- a/libraries/wavemap/include/wavemap/utils/bits/bit_operations.h +++ b/libraries/wavemap/include/wavemap/utils/bits/bit_operations.h @@ -107,7 +107,22 @@ constexpr std::make_signed_t bit_cast_signed(T bitstring) { template constexpr bool is_bit_set(T bitstring, int bit_index) { - return (bitstring >> bit_index) & 1; + return (bitstring >> bit_index) & static_cast(1); +} + +template +constexpr T set_bit(T bitstring, int bit_index) { + return bitstring | (static_cast(1) << bit_index); +} + +template +constexpr T unset_bit(T bitstring, int bit_index) { + return bitstring & ~(static_cast(1) << bit_index); +} + +template +constexpr T set_bit(T bitstring, int bit_index, bool value) { + return unset_bit(bitstring, bit_index) | (static_cast(value) << bit_index); } template diff --git a/libraries/wavemap/include/wavemap/utils/iterate/impl/ray_iterator_inl.h b/libraries/wavemap/include/wavemap/utils/iterate/impl/ray_iterator_inl.h index b4e402c6c..803ce93f1 100644 --- a/libraries/wavemap/include/wavemap/utils/iterate/impl/ray_iterator_inl.h +++ b/libraries/wavemap/include/wavemap/utils/iterate/impl/ray_iterator_inl.h @@ -38,8 +38,8 @@ Ray::Ray(const Point& start_point, const Point& end_point, : start_index_(convert::pointToNearestIndex(start_point, 1.f / cell_width)), end_index_(convert::pointToNearestIndex(end_point, 1.f / cell_width)), ray_length_in_steps_((end_index_ - start_index_).cwiseAbs().sum() + 1u) { - CHECK(!start_point.hasNaN()); - CHECK(!end_point.hasNaN()); + DCHECK(!start_point.hasNaN()); + DCHECK(!end_point.hasNaN()); if (ray_length_in_steps_ == 0u) { return; } diff --git a/libraries/wavemap/include/wavemap/utils/query/classified_map.h b/libraries/wavemap/include/wavemap/utils/query/classified_map.h index b07f2aab2..490296d6b 100644 --- a/libraries/wavemap/include/wavemap/utils/query/classified_map.h +++ b/libraries/wavemap/include/wavemap/utils/query/classified_map.h @@ -13,9 +13,19 @@ namespace wavemap { class ClassifiedMap { public: struct NodeData { - std::bitset has_free; - std::bitset has_occupied; - std::bitset has_unobserved; + uint8_t has_free{}; + uint8_t has_occupied{}; + uint8_t has_unobserved{}; + + void setFree(NdtreeIndexRelativeChild child_idx, bool value = true); + void setOccupied(NdtreeIndexRelativeChild child_idx, bool value = true); + void setUnobserved(NdtreeIndexRelativeChild child_idx, bool value = true); + bool isFree(NdtreeIndexRelativeChild child_idx) const; + bool isOccupied(NdtreeIndexRelativeChild child_idx) const; + bool isUnobserved(NdtreeIndexRelativeChild child_idx) const; + bool hasAnyFree() const; + bool hasAnyOccupied() const; + bool hasAnyUnobserved() const; Occupancy::Mask occupancyMask() const; Occupancy::Mask childOccupancyMask( diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h index dca1b7886..90936ab82 100644 --- a/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h +++ b/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h @@ -5,6 +5,48 @@ #include namespace wavemap { +inline void ClassifiedMap::NodeData::setFree(NdtreeIndexRelativeChild child_idx, + bool value) { + has_free = bit_ops::set_bit(has_free, child_idx, value); +} + +inline void ClassifiedMap::NodeData::setOccupied( + NdtreeIndexRelativeChild child_idx, bool value) { + has_occupied = bit_ops::set_bit(has_occupied, child_idx, value); +} + +inline void ClassifiedMap::NodeData::setUnobserved( + NdtreeIndexRelativeChild child_idx, bool value) { + has_unobserved = bit_ops::set_bit(has_unobserved, child_idx, value); +} + +inline bool ClassifiedMap::NodeData::isFree( + NdtreeIndexRelativeChild child_idx) const { + return bit_ops::is_bit_set(has_free, child_idx); +} + +inline bool ClassifiedMap::NodeData::isOccupied( + NdtreeIndexRelativeChild child_idx) const { + return bit_ops::is_bit_set(has_occupied, child_idx); +} + +inline bool ClassifiedMap::NodeData::isUnobserved( + NdtreeIndexRelativeChild child_idx) const { + return bit_ops::is_bit_set(has_unobserved, child_idx); +} + +inline bool ClassifiedMap::NodeData::hasAnyFree() const { + return has_free != static_cast(0); +} + +inline bool ClassifiedMap::NodeData::hasAnyOccupied() const { + return has_occupied != static_cast(0); +} + +inline bool ClassifiedMap::NodeData::hasAnyUnobserved() const { + return has_unobserved != static_cast(0); +} + inline bool ClassifiedMap::has(const OctreeIndex& index, Occupancy::Id occupancy_type) const { return has(index, Occupancy::toMask(occupancy_type)); @@ -34,14 +76,13 @@ inline void ClassifiedMap::forEachLeafMatching( } inline Occupancy::Mask ClassifiedMap::NodeData::occupancyMask() const { - return Occupancy::toMask(has_free.any(), has_occupied.any(), - has_unobserved.any()); + return Occupancy::toMask(hasAnyFree(), hasAnyOccupied(), hasAnyUnobserved()); } inline Occupancy::Mask ClassifiedMap::NodeData::childOccupancyMask( NdtreeIndexRelativeChild child_idx) const { - return Occupancy::toMask(has_free[child_idx], has_occupied[child_idx], - has_unobserved[child_idx]); + return Occupancy::toMask(isFree(child_idx), isOccupied(child_idx), + isUnobserved(child_idx)); } inline bool ClassifiedMap::hasBlock(const Index3D& block_index) const { diff --git a/libraries/wavemap/src/utils/query/classified_map.cc b/libraries/wavemap/src/utils/query/classified_map.cc index 6f1c5446e..5699ade1c 100644 --- a/libraries/wavemap/src/utils/query/classified_map.cc +++ b/libraries/wavemap/src/utils/query/classified_map.cc @@ -243,15 +243,14 @@ void ClassifiedMap::recursiveClassifier( // NOLINT classified_node.getOrAllocateChild(child_idx); recursiveClassifier(*occupancy_child_node, child_occupancy, classified_child_node); - const bool child_has_free = classified_child_node.data().has_free.any(); + const bool child_has_free = classified_child_node.data().hasAnyFree(); const bool child_has_occupied = - classified_child_node.data().has_occupied.any(); + classified_child_node.data().hasAnyOccupied(); const bool child_has_unobserved = - classified_child_node.data().has_unobserved.any(); - classified_node.data().has_free.set(child_idx, child_has_free); - classified_node.data().has_occupied.set(child_idx, child_has_occupied); - classified_node.data().has_unobserved.set(child_idx, - child_has_unobserved); + classified_child_node.data().hasAnyUnobserved(); + classified_node.data().setFree(child_idx, child_has_free); + classified_node.data().setOccupied(child_idx, child_has_occupied); + classified_node.data().setUnobserved(child_idx, child_has_unobserved); if (child_has_free + child_has_occupied + child_has_unobserved == 1) { classified_node.eraseChild(child_idx); } @@ -261,9 +260,9 @@ void ClassifiedMap::recursiveClassifier( // NOLINT classifier_.is(child_occupancy, Occupancy::kOccupied); const bool is_unobserved = classifier_.is(child_occupancy, Occupancy::kUnobserved); - classified_node.data().has_free.set(child_idx, is_free); - classified_node.data().has_occupied.set(child_idx, is_occupied); - classified_node.data().has_unobserved.set(child_idx, is_unobserved); + classified_node.data().setFree(child_idx, is_free); + classified_node.data().setOccupied(child_idx, is_occupied); + classified_node.data().setUnobserved(child_idx, is_unobserved); } } } From 30a554c32695920b111fd1d24341eb143bda7ad2 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 23 Jan 2024 22:51:28 +0100 Subject: [PATCH 072/159] Clean up syntax --- .../wavemap/utils/query/classified_map.h | 24 +++++---- .../utils/query/impl/classified_map_inl.h | 49 ++++--------------- .../wavemap/src/utils/query/classified_map.cc | 19 +++---- 3 files changed, 31 insertions(+), 61 deletions(-) diff --git a/libraries/wavemap/include/wavemap/utils/query/classified_map.h b/libraries/wavemap/include/wavemap/utils/query/classified_map.h index 490296d6b..8728e0ce5 100644 --- a/libraries/wavemap/include/wavemap/utils/query/classified_map.h +++ b/libraries/wavemap/include/wavemap/utils/query/classified_map.h @@ -10,22 +10,20 @@ #include namespace wavemap { +struct ChildBitset { + uint8_t bitset{}; + + void set(NdtreeIndexRelativeChild child_idx, bool value = true); + bool operator[](NdtreeIndexRelativeChild child_idx) const; + bool any() const; +}; + class ClassifiedMap { public: struct NodeData { - uint8_t has_free{}; - uint8_t has_occupied{}; - uint8_t has_unobserved{}; - - void setFree(NdtreeIndexRelativeChild child_idx, bool value = true); - void setOccupied(NdtreeIndexRelativeChild child_idx, bool value = true); - void setUnobserved(NdtreeIndexRelativeChild child_idx, bool value = true); - bool isFree(NdtreeIndexRelativeChild child_idx) const; - bool isOccupied(NdtreeIndexRelativeChild child_idx) const; - bool isUnobserved(NdtreeIndexRelativeChild child_idx) const; - bool hasAnyFree() const; - bool hasAnyOccupied() const; - bool hasAnyUnobserved() const; + ChildBitset has_free; + ChildBitset has_occupied; + ChildBitset has_unobserved; Occupancy::Mask occupancyMask() const; Occupancy::Mask childOccupancyMask( diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h index 90936ab82..47f3c4d41 100644 --- a/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h +++ b/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h @@ -5,46 +5,16 @@ #include namespace wavemap { -inline void ClassifiedMap::NodeData::setFree(NdtreeIndexRelativeChild child_idx, - bool value) { - has_free = bit_ops::set_bit(has_free, child_idx, value); +inline void ChildBitset::set(NdtreeIndexRelativeChild child_idx, bool value) { + bitset = bit_ops::set_bit(bitset, child_idx, value); } -inline void ClassifiedMap::NodeData::setOccupied( - NdtreeIndexRelativeChild child_idx, bool value) { - has_occupied = bit_ops::set_bit(has_occupied, child_idx, value); +inline bool ChildBitset::operator[](NdtreeIndexRelativeChild child_idx) const { + return bit_ops::is_bit_set(bitset, child_idx); } -inline void ClassifiedMap::NodeData::setUnobserved( - NdtreeIndexRelativeChild child_idx, bool value) { - has_unobserved = bit_ops::set_bit(has_unobserved, child_idx, value); -} - -inline bool ClassifiedMap::NodeData::isFree( - NdtreeIndexRelativeChild child_idx) const { - return bit_ops::is_bit_set(has_free, child_idx); -} - -inline bool ClassifiedMap::NodeData::isOccupied( - NdtreeIndexRelativeChild child_idx) const { - return bit_ops::is_bit_set(has_occupied, child_idx); -} - -inline bool ClassifiedMap::NodeData::isUnobserved( - NdtreeIndexRelativeChild child_idx) const { - return bit_ops::is_bit_set(has_unobserved, child_idx); -} - -inline bool ClassifiedMap::NodeData::hasAnyFree() const { - return has_free != static_cast(0); -} - -inline bool ClassifiedMap::NodeData::hasAnyOccupied() const { - return has_occupied != static_cast(0); -} - -inline bool ClassifiedMap::NodeData::hasAnyUnobserved() const { - return has_unobserved != static_cast(0); +inline bool ChildBitset::any() const { + return bitset != static_cast(0); } inline bool ClassifiedMap::has(const OctreeIndex& index, @@ -76,13 +46,14 @@ inline void ClassifiedMap::forEachLeafMatching( } inline Occupancy::Mask ClassifiedMap::NodeData::occupancyMask() const { - return Occupancy::toMask(hasAnyFree(), hasAnyOccupied(), hasAnyUnobserved()); + return Occupancy::toMask(has_free.any(), has_occupied.any(), + has_unobserved.any()); } inline Occupancy::Mask ClassifiedMap::NodeData::childOccupancyMask( NdtreeIndexRelativeChild child_idx) const { - return Occupancy::toMask(isFree(child_idx), isOccupied(child_idx), - isUnobserved(child_idx)); + return Occupancy::toMask(has_free[child_idx], has_occupied[child_idx], + has_unobserved[child_idx]); } inline bool ClassifiedMap::hasBlock(const Index3D& block_index) const { diff --git a/libraries/wavemap/src/utils/query/classified_map.cc b/libraries/wavemap/src/utils/query/classified_map.cc index 5699ade1c..6f1c5446e 100644 --- a/libraries/wavemap/src/utils/query/classified_map.cc +++ b/libraries/wavemap/src/utils/query/classified_map.cc @@ -243,14 +243,15 @@ void ClassifiedMap::recursiveClassifier( // NOLINT classified_node.getOrAllocateChild(child_idx); recursiveClassifier(*occupancy_child_node, child_occupancy, classified_child_node); - const bool child_has_free = classified_child_node.data().hasAnyFree(); + const bool child_has_free = classified_child_node.data().has_free.any(); const bool child_has_occupied = - classified_child_node.data().hasAnyOccupied(); + classified_child_node.data().has_occupied.any(); const bool child_has_unobserved = - classified_child_node.data().hasAnyUnobserved(); - classified_node.data().setFree(child_idx, child_has_free); - classified_node.data().setOccupied(child_idx, child_has_occupied); - classified_node.data().setUnobserved(child_idx, child_has_unobserved); + classified_child_node.data().has_unobserved.any(); + classified_node.data().has_free.set(child_idx, child_has_free); + classified_node.data().has_occupied.set(child_idx, child_has_occupied); + classified_node.data().has_unobserved.set(child_idx, + child_has_unobserved); if (child_has_free + child_has_occupied + child_has_unobserved == 1) { classified_node.eraseChild(child_idx); } @@ -260,9 +261,9 @@ void ClassifiedMap::recursiveClassifier( // NOLINT classifier_.is(child_occupancy, Occupancy::kOccupied); const bool is_unobserved = classifier_.is(child_occupancy, Occupancy::kUnobserved); - classified_node.data().setFree(child_idx, is_free); - classified_node.data().setOccupied(child_idx, is_occupied); - classified_node.data().setUnobserved(child_idx, is_unobserved); + classified_node.data().has_free.set(child_idx, is_free); + classified_node.data().has_occupied.set(child_idx, is_occupied); + classified_node.data().has_unobserved.set(child_idx, is_unobserved); } } } From 91c37fc13452afce8a39aff006601c474a93f965 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 25 Jan 2024 22:02:41 +0100 Subject: [PATCH 073/159] Improve consistency --- .../wavemap/data_structure/dense_block_hash.h | 11 ++++++---- .../data_structure/ndtree_block_hash.h | 3 +++ .../include/wavemap/map/hashed_blocks.h | 3 --- .../map/hashed_chunked_wavelet_octree.h | 16 +++++++------- .../wavemap/map/hashed_wavelet_octree.h | 16 +++++++------- .../impl/hashed_chunked_wavelet_octree_inl.h | 22 +++++++++++++++++++ .../map/impl/hashed_wavelet_octree_inl.h | 20 +++++++++++++++++ .../src/map_msg_conversions.cc | 4 ++-- 8 files changed, 70 insertions(+), 25 deletions(-) diff --git a/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h b/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h index 54adcff3b..bb5caaa94 100644 --- a/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h +++ b/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h @@ -48,6 +48,9 @@ class DenseBlockHash { const CellDataT& getDefaultValue() const { return default_value_; } bool equalsDefaultValue(const CellDataT& value) const; + auto& getHashMap() { return block_map_.getHashMap(); } + const auto& getHashMap() const { return block_map_.getHashMap(); } + template void forEachBlock(IndexedBlockVisitor visitor_fn); template @@ -58,14 +61,14 @@ class DenseBlockHash { template void forEachLeaf(IndexedLeafVisitorFunction visitor_fn) const; - protected: - const CellDataT default_value_; - BlockHashMap block_map_; - static Index indexToBlockIndex(const Index& index); static Index indexToCellIndex(const Index& index); static Index cellAndBlockIndexToIndex(const Index& block_index, const Index& cell_index); + + protected: + const CellDataT default_value_; + BlockHashMap block_map_; }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h b/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h index 3b11aa3ba..8b614401c 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h +++ b/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h @@ -69,6 +69,9 @@ class NdtreeBlockHash { const CellDataT& getDefaultValue() const { return default_value_; } bool equalsDefaultValue(const CellDataT& value) const; + auto& getHashMap() { return block_map_.getHashMap(); } + const auto& getHashMap() const { return block_map_.getHashMap(); } + template void forEachBlock(IndexedBlockVisitor visitor_fn); template diff --git a/libraries/wavemap/include/wavemap/map/hashed_blocks.h b/libraries/wavemap/include/wavemap/map/hashed_blocks.h index 414320290..2ff42a324 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_blocks.h +++ b/libraries/wavemap/include/wavemap/map/hashed_blocks.h @@ -48,9 +48,6 @@ class HashedBlocks : public MapBase, void setCellValue(const Index3D& index, FloatingPoint new_value) override; void addToCellValue(const Index3D& index, FloatingPoint update) override; - auto& getHashMap() { return block_map_; } - const auto& getHashMap() const { return block_map_; } - void forEachLeaf( typename MapBase::IndexedLeafVisitorFunction visitor_fn) const override; }; diff --git a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree.h b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree.h index 590be6bbb..ebba6c066 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree.h +++ b/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree.h @@ -102,21 +102,21 @@ class HashedChunkedWaveletOctree : public MapBase { void addToCellValue(const Index3D& index, FloatingPoint update) override; bool hasBlock(const Index3D& block_index) const; + bool eraseBlock(const BlockIndex& block_index); + template + void eraseBlockIf(IndexedBlockVisitor indicator_fn); + Block* getBlock(const Index3D& block_index); const Block* getBlock(const Index3D& block_index) const; Block& getOrAllocateBlock(const Index3D& block_index); - auto& getHashMap() { return block_map_; } - const auto& getHashMap() const { return block_map_; } + auto& getHashMap() { return block_map_.getHashMap(); } + const auto& getHashMap() const { return block_map_.getHashMap(); } template - void forEachBlock(IndexedBlockVisitor visitor_fn) { - block_map_.forEachBlock(visitor_fn); - } + void forEachBlock(IndexedBlockVisitor visitor_fn); template - void forEachBlock(IndexedBlockVisitor visitor_fn) const { - block_map_.forEachBlock(visitor_fn); - } + void forEachBlock(IndexedBlockVisitor visitor_fn) const; void forEachLeaf( typename MapBase::IndexedLeafVisitorFunction visitor_fn) const override; diff --git a/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h b/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h index b00a8bb15..f88ec4ca2 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h +++ b/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h @@ -96,21 +96,21 @@ class HashedWaveletOctree : public MapBase { void addToCellValue(const Index3D& index, FloatingPoint update) override; bool hasBlock(const Index3D& block_index) const; + bool eraseBlock(const BlockIndex& block_index); + template + void eraseBlockIf(IndexedBlockVisitor indicator_fn); + Block* getBlock(const Index3D& block_index); const Block* getBlock(const Index3D& block_index) const; Block& getOrAllocateBlock(const Index3D& block_index); - auto& getHashMap() { return block_map_; } - const auto& getHashMap() const { return block_map_; } + auto& getHashMap() { return block_map_.getHashMap(); } + const auto& getHashMap() const { return block_map_.getHashMap(); } template - void forEachBlock(IndexedBlockVisitor visitor_fn) { - block_map_.forEachBlock(visitor_fn); - } + void forEachBlock(IndexedBlockVisitor visitor_fn); template - void forEachBlock(IndexedBlockVisitor visitor_fn) const { - block_map_.forEachBlock(visitor_fn); - } + void forEachBlock(IndexedBlockVisitor visitor_fn) const; void forEachLeaf( typename MapBase::IndexedLeafVisitorFunction visitor_fn) const override; diff --git a/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_inl.h b/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_inl.h index 89824ff6c..2a60a6297 100644 --- a/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_inl.h +++ b/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_inl.h @@ -58,6 +58,17 @@ inline bool HashedChunkedWaveletOctree::hasBlock( return block_map_.hasBlock(block_index); } +inline bool HashedChunkedWaveletOctree::eraseBlock( + const HashedChunkedWaveletOctree::BlockIndex& block_index) { + return block_map_.eraseBlock(block_index); +} + +template +void HashedChunkedWaveletOctree::eraseBlockIf( + IndexedBlockVisitor indicator_fn) { + block_map_.eraseBlockIf(indicator_fn); +} + inline HashedChunkedWaveletOctree::Block* HashedChunkedWaveletOctree::getBlock( const Index3D& block_index) { return block_map_.getBlock(block_index); @@ -75,6 +86,17 @@ HashedChunkedWaveletOctree::getOrAllocateBlock(const Index3D& block_index) { config_.max_log_odds); } +template +void HashedChunkedWaveletOctree::forEachBlock(IndexedBlockVisitor visitor_fn) { + block_map_.forEachBlock(visitor_fn); +} + +template +void HashedChunkedWaveletOctree::forEachBlock( + IndexedBlockVisitor visitor_fn) const { + block_map_.forEachBlock(visitor_fn); +} + inline HashedChunkedWaveletOctree::BlockIndex HashedChunkedWaveletOctree::indexToBlockIndex( const OctreeIndex& node_index) const { diff --git a/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_inl.h b/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_inl.h index 137ddad6d..197735874 100644 --- a/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_inl.h +++ b/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_inl.h @@ -57,6 +57,16 @@ inline bool HashedWaveletOctree::hasBlock(const Index3D& block_index) const { return block_map_.hasBlock(block_index); } +inline bool HashedWaveletOctree::eraseBlock( + const HashedWaveletOctree::BlockIndex& block_index) { + return block_map_.eraseBlock(block_index); +} + +template +void HashedWaveletOctree::eraseBlockIf(IndexedBlockVisitor indicator_fn) { + block_map_.eraseBlockIf(indicator_fn); +} + inline HashedWaveletOctree::Block* HashedWaveletOctree::getBlock( const Index3D& block_index) { return block_map_.getBlock(block_index); @@ -74,6 +84,16 @@ inline HashedWaveletOctree::Block& HashedWaveletOctree::getOrAllocateBlock( config_.max_log_odds); } +template +void HashedWaveletOctree::forEachBlock(IndexedBlockVisitor visitor_fn) { + block_map_.forEachBlock(visitor_fn); +} + +template +void HashedWaveletOctree::forEachBlock(IndexedBlockVisitor visitor_fn) const { + block_map_.forEachBlock(visitor_fn); +} + inline HashedWaveletOctree::BlockIndex HashedWaveletOctree::indexToBlockIndex( const OctreeIndex& node_index) const { const Index3D index = convert::nodeIndexToMinCornerIndex(node_index); diff --git a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc index 9caf1935a..2306f9114 100644 --- a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc +++ b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc @@ -150,7 +150,7 @@ void rosMsgToMap(const wavemap_msgs::HashedBlocks& msg, allocated_blocks.emplace(block_index.x, block_index.y, block_index.z); } // Remove local blocks that should no longer exist according to the map msg - map->getHashMap().eraseBlockIf( + map->eraseBlockIf( [&allocated_blocks](const Index3D& block_index, const auto& /*block*/) { return !allocated_blocks.count(block_index); }); @@ -394,7 +394,7 @@ void rosMsgToMap(const wavemap_msgs::HashedWaveletOctree& msg, allocated_blocks.emplace(block_index.x, block_index.y, block_index.z); } // Remove local blocks that should no longer exist according to the map msg - map->getHashMap().eraseBlockIf( + map->eraseBlockIf( [&allocated_blocks](const Index3D& block_index, const auto& /*block*/) { return !allocated_blocks.count(block_index); }); From 20615e5a542a5b11bd609f426b192d2b6f607de9 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 25 Jan 2024 22:09:23 +0100 Subject: [PATCH 074/159] Reintroduce collision utils --- libraries/wavemap/CMakeLists.txt | 1 + .../wavemap/utils/query/collision_utils.h | 17 +++++ .../src/utils/query/collision_utils.cc | 63 +++++++++++++++++++ 3 files changed, 81 insertions(+) create mode 100644 libraries/wavemap/include/wavemap/utils/query/collision_utils.h create mode 100644 libraries/wavemap/src/utils/query/collision_utils.cc diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index 018e8be9d..3113aa734 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -67,6 +67,7 @@ add_library(${PROJECT_NAME} src/map/map_base.cc src/map/map_factory.cc src/utils/query/classified_map.cc + src/utils/query/collision_utils.cc src/utils/query/query_accelerator.cc src/utils/sdf/full_euclidean_sdf_generator.cc src/utils/sdf/quasi_euclidean_sdf_generator.cc diff --git a/libraries/wavemap/include/wavemap/utils/query/collision_utils.h b/libraries/wavemap/include/wavemap/utils/query/collision_utils.h new file mode 100644 index 000000000..6a8b2d499 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/query/collision_utils.h @@ -0,0 +1,17 @@ +#ifndef WAVEMAP_UTILS_QUERY_COLLISION_UTILS_H_ +#define WAVEMAP_UTILS_QUERY_COLLISION_UTILS_H_ + +#include "wavemap/common.h" +#include "wavemap/data_structure/aabb.h" +#include "wavemap/map/hashed_blocks.h" +#include "wavemap/map/map_base.h" +#include "wavemap/utils/random_number_generator.h" + +namespace wavemap { +std::optional getCollisionFreePosition( + const MapBase& occupancy_map, const HashedBlocks& esdf, + FloatingPoint robot_radius, + std::optional> aabb = std::nullopt); +} // namespace wavemap + +#endif // WAVEMAP_UTILS_QUERY_COLLISION_UTILS_H_ diff --git a/libraries/wavemap/src/utils/query/collision_utils.cc b/libraries/wavemap/src/utils/query/collision_utils.cc new file mode 100644 index 000000000..f011b2dbb --- /dev/null +++ b/libraries/wavemap/src/utils/query/collision_utils.cc @@ -0,0 +1,63 @@ +#include "wavemap/utils/query/collision_utils.h" + +namespace wavemap { +std::optional getCollisionFreePosition( + const MapBase& occupancy_map, const HashedBlocks& esdf, + FloatingPoint robot_radius, std::optional> aabb) { + RandomNumberGenerator rng; + + constexpr size_t kMaxAttempts = 1000; + for (size_t attempt_idx = 0; attempt_idx < kMaxAttempts; ++attempt_idx) { + Index3D global_index = Index3D::Zero(); + if (aabb) { + const Point3D point{rng.getRandomRealNumber(aabb->min[0], aabb->max[0]), + rng.getRandomRealNumber(aabb->min[1], aabb->max[1]), + rng.getRandomRealNumber(aabb->min[2], aabb->max[2])}; + global_index = + convert::pointToNearestIndex(point, 1.f / esdf.getMinCellWidth()); + } else { + const size_t nth_block = + rng.getRandomInteger(0ul, esdf.getHashMap().size() - 1ul); + auto it = esdf.getHashMap().begin(); + std::advance(it, nth_block); + if (it == esdf.getHashMap().end()) { + continue; + } + + const LinearIndex linear_cell_index = + rng.getRandomInteger(0, HashedBlocks::Block::kCellsPerBlock - 1); + + const Index3D& block_index = it->first; + const Index3D cell_index = + convert::linearIndexToIndex( + linear_cell_index); + global_index = + HashedBlocks::cellAndBlockIndexToIndex(block_index, cell_index); + } + + Point3D position = + convert::indexToCenterPoint(global_index, esdf.getMinCellWidth()); + if (aabb && !aabb->containsPoint(position)) { + continue; + } + + const FloatingPoint occupancy_value = + occupancy_map.getCellValue(global_index); + const bool is_free = occupancy_value < -1e-3f; + if (!is_free) { + continue; + } + + const FloatingPoint esdf_value = esdf.getCellValue(global_index); + if (esdf_value < robot_radius) { + continue; + } + + return position; + } + + LOG(WARNING) << "Could not find collision free position. Giving up after " + << kMaxAttempts << " attempts."; + return std::nullopt; +} +} // namespace wavemap From 236d5250da4ae9db005477c43ca50ce6adedb6db Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 31 Jan 2024 18:17:26 +0100 Subject: [PATCH 075/159] Improve index conversion vector op syntax --- .../wavemap/indexing/index_conversions.h | 30 ++++++++----------- 1 file changed, 13 insertions(+), 17 deletions(-) diff --git a/libraries/wavemap/include/wavemap/indexing/index_conversions.h b/libraries/wavemap/include/wavemap/indexing/index_conversions.h index 9b62fa749..0fc67ee14 100644 --- a/libraries/wavemap/include/wavemap/indexing/index_conversions.h +++ b/libraries/wavemap/include/wavemap/indexing/index_conversions.h @@ -15,26 +15,17 @@ namespace wavemap::convert { template inline Index scaledPointToNearestIndex(const Point& point) { - return (point - Vector::Constant(0.5f)) - .array() - .round() - .template cast(); + return (point.array() - 0.5f).round().template cast(); } template inline Index scaledPointToFloorIndex(const Point& point) { - return (point - Vector::Constant(0.5f)) - .array() - .floor() - .template cast(); + return (point.array() - 0.5f).floor().template cast(); } template inline Index scaledPointToCeilIndex(const Point& point) { - return (point - Vector::Constant(0.5f)) - .array() - .ceil() - .template cast(); + return (point.array() - 0.5f).ceil().template cast(); } template @@ -61,11 +52,16 @@ inline Point indexToMinCorner(const Index& index, return index.template cast() * cell_width; } +template +inline Point indexToMaxCorner(const Index& index, + FloatingPoint cell_width) { + return (index.template cast().array() + 1.f) * cell_width; +} + template inline Point indexToCenterPoint(const Index& index, FloatingPoint cell_width) { - return (index.template cast() + Vector::Constant(0.5f)) * - cell_width; + return (index.template cast().array() + 0.5f) * cell_width; } template @@ -140,8 +136,7 @@ inline Point nodeIndexToMaxCorner(const NdtreeIndex& node_index, FloatingPoint min_cell_width) { const FloatingPoint node_width = heightToCellWidth(min_cell_width, node_index.height); - return node_index.position.template cast() * node_width + - Vector::Constant(node_width); + return indexToMaxCorner(node_index.position, node_width); } template @@ -151,7 +146,8 @@ inline AABB> nodeIndexToAABB(const NdtreeIndex& node_index, heightToCellWidth(min_cell_width, node_index.height); const Point min_corner = indexToMinCorner(node_index.position, node_width); - const Point max_corner = min_corner + Vector::Constant(node_width); + const Point max_corner = + indexToMaxCorner(node_index.position, node_width); return {min_corner, max_corner}; } From 0c2ba156f0605ec7f433a3b44f98670c4c39c23a Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 31 Jan 2024 23:25:23 +0100 Subject: [PATCH 076/159] Add method to query the ray's length --- libraries/wavemap/include/wavemap/utils/iterate/ray_iterator.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/wavemap/include/wavemap/utils/iterate/ray_iterator.h b/libraries/wavemap/include/wavemap/utils/iterate/ray_iterator.h index 801c4eb44..5e4973af1 100644 --- a/libraries/wavemap/include/wavemap/utils/iterate/ray_iterator.h +++ b/libraries/wavemap/include/wavemap/utils/iterate/ray_iterator.h @@ -15,6 +15,8 @@ class Ray { Ray(const Point& start_point, const Point& end_point, FloatingPoint cell_width); + size_t size() const { return ray_length_in_steps_; } + class Iterator { public: explicit Iterator(const Ray& ray) From 8714abc3159950c0b15b9a6253b16572d3cd3f0b Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 1 Feb 2024 15:55:30 +0100 Subject: [PATCH 077/159] Extend random point sampling utils --- libraries/wavemap/CMakeLists.txt | 2 +- .../wavemap/utils/query/collision_utils.h | 17 ----- .../query/impl/occupancy_classifier_inl.h | 35 +++++----- .../wavemap/utils/query/impl/occupancy_inl.h | 4 +- .../utils/query/impl/point_sampler_inl.h | 31 +++++++++ .../include/wavemap/utils/query/occupancy.h | 4 +- .../utils/query/occupancy_classifier.h | 3 + .../wavemap/utils/query/point_sampler.h | 59 +++++++++++++++++ .../src/utils/query/collision_utils.cc | 63 ------------------ .../wavemap/src/utils/query/point_sampler.cc | 65 +++++++++++++++++++ 10 files changed, 181 insertions(+), 102 deletions(-) delete mode 100644 libraries/wavemap/include/wavemap/utils/query/collision_utils.h create mode 100644 libraries/wavemap/include/wavemap/utils/query/impl/point_sampler_inl.h create mode 100644 libraries/wavemap/include/wavemap/utils/query/point_sampler.h delete mode 100644 libraries/wavemap/src/utils/query/collision_utils.cc create mode 100644 libraries/wavemap/src/utils/query/point_sampler.cc diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index 3113aa734..507550ea4 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -67,8 +67,8 @@ add_library(${PROJECT_NAME} src/map/map_base.cc src/map/map_factory.cc src/utils/query/classified_map.cc - src/utils/query/collision_utils.cc src/utils/query/query_accelerator.cc + src/utils/query/point_sampler.cc src/utils/sdf/full_euclidean_sdf_generator.cc src/utils/sdf/quasi_euclidean_sdf_generator.cc src/utils/stopwatch.cc diff --git a/libraries/wavemap/include/wavemap/utils/query/collision_utils.h b/libraries/wavemap/include/wavemap/utils/query/collision_utils.h deleted file mode 100644 index 6a8b2d499..000000000 --- a/libraries/wavemap/include/wavemap/utils/query/collision_utils.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef WAVEMAP_UTILS_QUERY_COLLISION_UTILS_H_ -#define WAVEMAP_UTILS_QUERY_COLLISION_UTILS_H_ - -#include "wavemap/common.h" -#include "wavemap/data_structure/aabb.h" -#include "wavemap/map/hashed_blocks.h" -#include "wavemap/map/map_base.h" -#include "wavemap/utils/random_number_generator.h" - -namespace wavemap { -std::optional getCollisionFreePosition( - const MapBase& occupancy_map, const HashedBlocks& esdf, - FloatingPoint robot_radius, - std::optional> aabb = std::nullopt); -} // namespace wavemap - -#endif // WAVEMAP_UTILS_QUERY_COLLISION_UTILS_H_ diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_classifier_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_classifier_inl.h index 2a07aa63d..5c4cb27b3 100644 --- a/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_classifier_inl.h +++ b/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_classifier_inl.h @@ -7,29 +7,28 @@ inline constexpr Occupancy::Id OccupancyClassifier::classify( if (isUnobserved(log_odds_occupancy)) { return Occupancy::kUnobserved; } - if (log_odds_occupancy < occupancy_threshold_) { - return Occupancy::kFree; - } else { - return Occupancy::kOccupied; + const bool is_free = log_odds_occupancy < occupancy_threshold_; + return is_free ? Occupancy::kFree : Occupancy::kOccupied; +} + +inline constexpr Occupancy::Mask OccupancyClassifier::toMask( + FloatingPoint log_odds_occupancy) const { + if (isUnobserved(log_odds_occupancy)) { + return Occupancy::toMask(Occupancy::kUnobserved); } + const bool is_free = log_odds_occupancy < occupancy_threshold_; + return is_free ? Occupancy::toMask(Occupancy::kFree) + : Occupancy::toMask(Occupancy::kOccupied); } inline constexpr bool OccupancyClassifier::is( FloatingPoint log_odds_occupancy, Occupancy::Id occupancy_type) const { - switch (occupancy_type) { - case Occupancy::kFree: - return isObserved(log_odds_occupancy) && - log_odds_occupancy < occupancy_threshold_; - case Occupancy::kOccupied: - return isObserved(log_odds_occupancy) && - occupancy_threshold_ < log_odds_occupancy; - case Occupancy::kUnobserved: - return isUnobserved(log_odds_occupancy); - case Occupancy::kObserved: - return isObserved(log_odds_occupancy); - default: - return false; - } + return is(log_odds_occupancy, Occupancy::toMask(occupancy_type)); +} + +inline constexpr bool OccupancyClassifier::is( + FloatingPoint log_odds_occupancy, Occupancy::Mask occupancy_mask) const { + return has(toMask(log_odds_occupancy), occupancy_mask); } inline constexpr bool OccupancyClassifier::has(Occupancy::Mask region_occupancy, diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_inl.h index f57cb316a..a41904d2b 100644 --- a/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_inl.h +++ b/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_inl.h @@ -7,8 +7,10 @@ constexpr Occupancy::Mask Occupancy::toMask(Occupancy::Id type_id) { DCHECK(type_id < static_cast(names.size())); if (type_id < 3) { return 1 << type_id; - } else { + } else if (type_id == 3) { return 0b011; + } else { + return 0b111; } } constexpr Occupancy::Mask Occupancy::toMask(bool free, bool occupied, diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/point_sampler_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/point_sampler_inl.h new file mode 100644 index 000000000..4b5923bc1 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/query/impl/point_sampler_inl.h @@ -0,0 +1,31 @@ +#ifndef WAVEMAP_UTILS_QUERY_IMPL_POINT_SAMPLER_INL_H_ +#define WAVEMAP_UTILS_QUERY_IMPL_POINT_SAMPLER_INL_H_ + +namespace wavemap { +inline std::optional PointSampler::getRandomPoint( + Occupancy::Id occupancy_type, const std::optional>& aabb, + size_t max_attempts) { + return getRandomPoint(Occupancy::toMask(occupancy_type), aabb, max_attempts); +} + +inline std::optional PointSampler::getRandomPoint( + Occupancy::Mask occupancy_mask, const std::optional>& aabb, + size_t max_attempts) { + size_t attempt_idx = 0; + return getRandomPointImpl(occupancy_mask, aabb, attempt_idx, max_attempts); +} + +inline Point3D PointSampler::getRandomPointInAABB(const AABB& aabb) { + return {rng_.getRandomRealNumber(aabb.min[0], aabb.max[0]), + rng_.getRandomRealNumber(aabb.min[1], aabb.max[1]), + rng_.getRandomRealNumber(aabb.min[2], aabb.max[2])}; +} + +inline Point3D PointSampler::getRandomPointInBlock(const Index3D& block_index) { + const auto block_aabb = convert::nodeIndexToAABB( + OctreeIndex{block_height_, block_index}, min_cell_width_); + return getRandomPointInAABB(block_aabb); +} +} // namespace wavemap + +#endif // WAVEMAP_UTILS_QUERY_IMPL_POINT_SAMPLER_INL_H_ diff --git a/libraries/wavemap/include/wavemap/utils/query/occupancy.h b/libraries/wavemap/include/wavemap/utils/query/occupancy.h index 05fbc2cb9..ee2a5d13d 100644 --- a/libraries/wavemap/include/wavemap/utils/query/occupancy.h +++ b/libraries/wavemap/include/wavemap/utils/query/occupancy.h @@ -9,9 +9,9 @@ struct Occupancy : TypeSelector { using TypeSelector::TypeSelector; using Mask = uint8_t; - enum Id : TypeId { kFree, kOccupied, kUnobserved, kObserved }; + enum Id : TypeId { kFree, kOccupied, kUnobserved, kObserved, kAny }; static constexpr std::array names = {"free", "occupied", "unobserved", - "observed"}; + "observed", "any"}; // NOTE: For usage examples, please refer to the OccupancyClassifier class. static constexpr Mask toMask(Id type_id); diff --git a/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h b/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h index fb05cae6f..96521e32f 100644 --- a/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h +++ b/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h @@ -13,9 +13,12 @@ class OccupancyClassifier { FloatingPoint getOccupancyThreshold() const { return occupancy_threshold_; } constexpr Occupancy::Id classify(FloatingPoint log_odds_occupancy) const; + constexpr Occupancy::Mask toMask(FloatingPoint log_odds_occupancy) const; constexpr bool is(FloatingPoint log_odds_occupancy, Occupancy::Id occupancy_type) const; + constexpr bool is(FloatingPoint log_odds_occupancy, + Occupancy::Mask occupancy_mask) const; static constexpr bool has(Occupancy::Mask region_occupancy, Occupancy::Id occupancy_type); diff --git a/libraries/wavemap/include/wavemap/utils/query/point_sampler.h b/libraries/wavemap/include/wavemap/utils/query/point_sampler.h new file mode 100644 index 000000000..38b6bc813 --- /dev/null +++ b/libraries/wavemap/include/wavemap/utils/query/point_sampler.h @@ -0,0 +1,59 @@ +#ifndef WAVEMAP_UTILS_QUERY_POINT_SAMPLER_H_ +#define WAVEMAP_UTILS_QUERY_POINT_SAMPLER_H_ + +#include "wavemap/common.h" +#include "wavemap/data_structure/aabb.h" +#include "wavemap/map/hashed_blocks.h" +#include "wavemap/map/hashed_wavelet_octree.h" +#include "wavemap/utils/query/occupancy.h" +#include "wavemap/utils/query/occupancy_classifier.h" +#include "wavemap/utils/query/query_accelerator.h" +#include "wavemap/utils/random_number_generator.h" + +namespace wavemap { +class PointSampler { + public: + explicit PointSampler(const HashedWaveletOctree& occupancy_map, + OccupancyClassifier classifier = OccupancyClassifier{}) + : occupancy_map_(occupancy_map), classifier_(classifier) {} + + std::optional getRandomBlock(); + + std::optional getRandomPoint( + Occupancy::Id occupancy_type, + const std::optional>& aabb = std::nullopt, + size_t max_attempts = 1000); + std::optional getRandomPoint( + Occupancy::Mask occupancy_mask, + const std::optional>& aabb = std::nullopt, + size_t max_attempts = 1000); + + std::optional getRandomCollisionFreePoint( + const HashedBlocks& esdf, FloatingPoint robot_radius, + const std::optional>& aabb = std::nullopt, + size_t max_attempts = 1000); + + private: + const HashedWaveletOctree& occupancy_map_; + const OccupancyClassifier classifier_; + + QueryAccelerator query_accelerator_{occupancy_map_}; + + RandomNumberGenerator rng_; + const IndexElement block_height_ = occupancy_map_.getTreeHeight(); + const FloatingPoint min_cell_width_ = occupancy_map_.getMinCellWidth(); + const FloatingPoint min_cell_width_inv_ = 1.f / min_cell_width_; + + std::optional getRandomPointImpl( + Occupancy::Mask occupancy_mask, const std::optional>& aabb, + size_t& attempt_idx, size_t max_attempts); + + Point3D getRandomPointInAABB(const AABB& aabb); + Point3D getRandomPointInBlock(const Index3D& block_index); +}; + +} // namespace wavemap + +#include "wavemap/utils/query/impl/point_sampler_inl.h" + +#endif // WAVEMAP_UTILS_QUERY_POINT_SAMPLER_H_ diff --git a/libraries/wavemap/src/utils/query/collision_utils.cc b/libraries/wavemap/src/utils/query/collision_utils.cc deleted file mode 100644 index f011b2dbb..000000000 --- a/libraries/wavemap/src/utils/query/collision_utils.cc +++ /dev/null @@ -1,63 +0,0 @@ -#include "wavemap/utils/query/collision_utils.h" - -namespace wavemap { -std::optional getCollisionFreePosition( - const MapBase& occupancy_map, const HashedBlocks& esdf, - FloatingPoint robot_radius, std::optional> aabb) { - RandomNumberGenerator rng; - - constexpr size_t kMaxAttempts = 1000; - for (size_t attempt_idx = 0; attempt_idx < kMaxAttempts; ++attempt_idx) { - Index3D global_index = Index3D::Zero(); - if (aabb) { - const Point3D point{rng.getRandomRealNumber(aabb->min[0], aabb->max[0]), - rng.getRandomRealNumber(aabb->min[1], aabb->max[1]), - rng.getRandomRealNumber(aabb->min[2], aabb->max[2])}; - global_index = - convert::pointToNearestIndex(point, 1.f / esdf.getMinCellWidth()); - } else { - const size_t nth_block = - rng.getRandomInteger(0ul, esdf.getHashMap().size() - 1ul); - auto it = esdf.getHashMap().begin(); - std::advance(it, nth_block); - if (it == esdf.getHashMap().end()) { - continue; - } - - const LinearIndex linear_cell_index = - rng.getRandomInteger(0, HashedBlocks::Block::kCellsPerBlock - 1); - - const Index3D& block_index = it->first; - const Index3D cell_index = - convert::linearIndexToIndex( - linear_cell_index); - global_index = - HashedBlocks::cellAndBlockIndexToIndex(block_index, cell_index); - } - - Point3D position = - convert::indexToCenterPoint(global_index, esdf.getMinCellWidth()); - if (aabb && !aabb->containsPoint(position)) { - continue; - } - - const FloatingPoint occupancy_value = - occupancy_map.getCellValue(global_index); - const bool is_free = occupancy_value < -1e-3f; - if (!is_free) { - continue; - } - - const FloatingPoint esdf_value = esdf.getCellValue(global_index); - if (esdf_value < robot_radius) { - continue; - } - - return position; - } - - LOG(WARNING) << "Could not find collision free position. Giving up after " - << kMaxAttempts << " attempts."; - return std::nullopt; -} -} // namespace wavemap diff --git a/libraries/wavemap/src/utils/query/point_sampler.cc b/libraries/wavemap/src/utils/query/point_sampler.cc new file mode 100644 index 000000000..5666dc127 --- /dev/null +++ b/libraries/wavemap/src/utils/query/point_sampler.cc @@ -0,0 +1,65 @@ +#include "wavemap/utils/query/point_sampler.h" + +namespace wavemap { +std::optional PointSampler::getRandomBlock() { + if (occupancy_map_.empty()) { + return std::nullopt; + } + + const size_t num_blocks = occupancy_map_.getHashMap().size(); + const size_t nth_block = rng_.getRandomInteger(0ul, num_blocks - 1ul); + auto it = occupancy_map_.getHashMap().begin(); + std::advance(it, nth_block); + if (it == occupancy_map_.getHashMap().end()) { + return std::nullopt; + } + + return it->first; +} + +std::optional PointSampler::getRandomCollisionFreePoint( + const HashedBlocks& esdf, FloatingPoint robot_radius, + const std::optional>& aabb, size_t max_attempts) { + for (size_t attempt_idx = 0; attempt_idx < max_attempts; ++attempt_idx) { + auto point = + getRandomPointImpl(Occupancy::kFree, aabb, attempt_idx, max_attempts); + if (!point) { + continue; + } + const auto index = + convert::pointToNearestIndex(point.value(), min_cell_width_inv_); + const FloatingPoint distance = esdf.getCellValue(index); + if (robot_radius <= distance) { + return point.value(); + } + } + + return std::nullopt; +} + +std::optional PointSampler::getRandomPointImpl( + Occupancy::Mask occupancy_mask, const std::optional>& aabb, + size_t& attempt_idx, size_t max_attempts) { + if (occupancy_map_.empty()) { + return std::nullopt; + } + + for (; attempt_idx < max_attempts; ++attempt_idx) { + Point3D point; + if (aabb) { + point = getRandomPointInAABB(aabb.value()); + } else if (const auto block_index = getRandomBlock(); block_index) { + point = getRandomPointInBlock(block_index.value()); + } else { + continue; + } + const auto index = convert::pointToNearestIndex(point, min_cell_width_inv_); + const FloatingPoint log_odds = query_accelerator_.getCellValue(index); + if (classifier_.is(log_odds, occupancy_mask)) { + return point; + } + } + + return std::nullopt; +} +} // namespace wavemap From 6e14b053d9a967db0ee4c95387a35252cae74d64 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 2 Feb 2024 17:20:31 +0100 Subject: [PATCH 078/159] Add option to inflate the occupancy map based a robot's radius --- .../wavemap/data_structure/dense_block_hash.h | 1 + .../wavemap/utils/query/classified_map.h | 31 ++-- .../utils/query/impl/query_accelerator_inl.h | 30 ++++ .../wavemap/utils/query/query_accelerator.h | 28 ++++ .../wavemap/src/utils/query/classified_map.cc | 135 +++++++++++++++++- 5 files changed, 213 insertions(+), 12 deletions(-) diff --git a/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h b/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h index bb5caaa94..b4b86c023 100644 --- a/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h +++ b/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h @@ -16,6 +16,7 @@ class DenseBlockHash { using Block = DenseGrid; using BlockHashMap = SpatialHash; + using Cell = CellDataT; explicit DenseBlockHash(CellDataT default_value = CellDataT{}) : default_value_(default_value) {} diff --git a/libraries/wavemap/include/wavemap/utils/query/classified_map.h b/libraries/wavemap/include/wavemap/utils/query/classified_map.h index 8728e0ce5..0c50b98ca 100644 --- a/libraries/wavemap/include/wavemap/utils/query/classified_map.h +++ b/libraries/wavemap/include/wavemap/utils/query/classified_map.h @@ -6,8 +6,10 @@ #include #include +#include #include #include +#include namespace wavemap { struct ChildBitset { @@ -36,23 +38,21 @@ class ClassifiedMap { static constexpr int kDim = 3; ClassifiedMap(FloatingPoint min_cell_width, IndexElement tree_height, - const OccupancyClassifier& classifier) - : min_cell_width_(min_cell_width), - classifier_(classifier), - block_map_(tree_height), - query_cache_(tree_height) {} + const OccupancyClassifier& classifier); ClassifiedMap(const HashedWaveletOctree& occupancy_map, - const OccupancyClassifier& classifier) - : ClassifiedMap(occupancy_map.getMinCellWidth(), - occupancy_map.getTreeHeight(), classifier) { - update(occupancy_map); - } + const OccupancyClassifier& classifier); + + ClassifiedMap(const HashedWaveletOctree& occupancy_map, + const OccupancyClassifier& classifier, + const HashedBlocks& esdf_map, FloatingPoint robot_radius); FloatingPoint getMinCellWidth() const { return min_cell_width_; } IndexElement getTreeHeight() const { return block_map_.getMaxHeight(); } void update(const HashedWaveletOctree& occupancy_map); + void update(const HashedWaveletOctree& occupancy_map, + const HashedBlocks& esdf_map, FloatingPoint robot_radius); bool has(const OctreeIndex& index, Occupancy::Id occupancy_type) const; bool has(const OctreeIndex& index, Occupancy::Mask occupancy_mask) const; @@ -82,6 +82,10 @@ class ClassifiedMap { IndexElement termination_height = 0) const; private: + using HaarTransform = HashedWaveletOctree::Block::Transform; + using ChildAverages = + HashedWaveletOctree::Block::Coefficients::CoefficientsArray; + const FloatingPoint min_cell_width_; const OccupancyClassifier classifier_; BlockHashMap block_map_; @@ -118,6 +122,13 @@ class ClassifiedMap { void recursiveClassifier( const HashedWaveletOctreeBlock::NodeType& occupancy_node, FloatingPoint average_occupancy, Node& classified_node); + + void recursiveClassifier( + const OctreeIndex& node_index, + const HashedWaveletOctreeBlock::NodeType* occupancy_node, + FloatingPoint occupancy_average, + QueryAccelerator& esdf_map, + FloatingPoint robot_radius, Node& classified_node); }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h index 089beba41..b3c8e4b44 100644 --- a/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h +++ b/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h @@ -34,6 +34,36 @@ BlockDataT& QueryAccelerator>::getOrAllocateBlock( return *last_block_; } +template +void QueryAccelerator>::reset() { + block_index_ = Index::Constant(std::numeric_limits::max()); + block_ = nullptr; +} + +template +const typename QueryAccelerator< + DenseBlockHash>::BlockType* +QueryAccelerator>::getBlock( + const Index& block_index) { + if (block_index != block_index_) { + block_index_ = block_index; + block_ = dense_block_hash_.getBlock(block_index); + } + return block_; +} + +template +const CellDataT* +QueryAccelerator>::getValue( + const Index& index) { + const Index block_index = dense_block_hash_.indexToBlockIndex(index); + if (const BlockType* block = getBlock(block_index); block) { + const Index cell_index = dense_block_hash_.indexToCellIndex(index); + return &block->at(cell_index); + } + return nullptr; +} + template void QueryAccelerator>::reset() { block_index_ = Index::Constant(std::numeric_limits::max()); diff --git a/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h b/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h index d925ad73a..7c61cc472 100644 --- a/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h +++ b/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h @@ -3,6 +3,7 @@ #include +#include "wavemap/data_structure/dense_block_hash.h" #include "wavemap/data_structure/ndtree_block_hash.h" #include "wavemap/data_structure/spatial_hash.h" #include "wavemap/indexing/index_hashes.h" @@ -41,6 +42,33 @@ class QueryAccelerator> { BlockDataT* last_block_ = nullptr; }; +// Query accelerator for dense block hashes +template +class QueryAccelerator> { + public: + static constexpr int kDim = dim; + using BlockType = + typename DenseBlockHash::Block; + using CellType = + typename DenseBlockHash::Cell; + + explicit QueryAccelerator( + const DenseBlockHash& dense_block_hash) + : dense_block_hash_(dense_block_hash) {} + + void reset(); + + const BlockType* getBlock(const Index& block_index); + const CellDataT* getValue(const Index& index); + + private: + const DenseBlockHash& dense_block_hash_; + + Index block_index_ = + Index::Constant(std::numeric_limits::max()); + const BlockType* block_ = nullptr; +}; + // Query accelerator for ndtree block hashes template class QueryAccelerator> { diff --git a/libraries/wavemap/src/utils/query/classified_map.cc b/libraries/wavemap/src/utils/query/classified_map.cc index 6f1c5446e..7c35bfa22 100644 --- a/libraries/wavemap/src/utils/query/classified_map.cc +++ b/libraries/wavemap/src/utils/query/classified_map.cc @@ -3,6 +3,30 @@ #include namespace wavemap { +ClassifiedMap::ClassifiedMap(FloatingPoint min_cell_width, + IndexElement tree_height, + const OccupancyClassifier& classifier) + : min_cell_width_(min_cell_width), + classifier_(classifier), + block_map_(tree_height), + query_cache_(tree_height) {} + +ClassifiedMap::ClassifiedMap(const HashedWaveletOctree& occupancy_map, + const OccupancyClassifier& classifier) + : ClassifiedMap(occupancy_map.getMinCellWidth(), + occupancy_map.getTreeHeight(), classifier) { + update(occupancy_map); +} + +ClassifiedMap::ClassifiedMap(const HashedWaveletOctree& occupancy_map, + const OccupancyClassifier& classifier, + const HashedBlocks& esdf_map, + FloatingPoint robot_radius) + : ClassifiedMap(occupancy_map.getMinCellWidth(), + occupancy_map.getTreeHeight(), classifier) { + update(occupancy_map, esdf_map, robot_radius); +} + void ClassifiedMap::update(const HashedWaveletOctree& occupancy_map) { ZoneScoped; // Reset the query cache @@ -24,6 +48,39 @@ void ClassifiedMap::update(const HashedWaveletOctree& occupancy_map) { }); } +void ClassifiedMap::update(const HashedWaveletOctree& occupancy_map, + const HashedBlocks& esdf_map, + FloatingPoint robot_radius) { + ZoneScoped; + // Reset the query cache + query_cache_.reset(); + + // Check that the ESDF is valid for our query and compatible with the occ map + CHECK_GE(esdf_map.getDefaultValue(), robot_radius); + CHECK_GE(esdf_map.getMaxLogOdds(), robot_radius); + CHECK_NEAR(esdf_map.getMinCellWidth(), occupancy_map.getMinCellWidth(), + kEpsilon); + + // Erase blocks that no longer exist + block_map_.eraseBlockIf( + [&occupancy_map](const Index3D& block_index, const auto& /*block*/) { + return !occupancy_map.hasBlock(block_index); + }); + + // Update all existing blocks + QueryAccelerator esdf_accelerator{ + dynamic_cast(esdf_map)}; + occupancy_map.forEachBlock( + [this, &esdf_accelerator, block_height = occupancy_map.getTreeHeight(), + robot_radius](const Index3D& block_index, const auto& occupancy_block) { + const OctreeIndex block_node_index{block_height, block_index}; + auto& classified_block = block_map_.getOrAllocateBlock(block_index); + recursiveClassifier(block_node_index, &occupancy_block.getRootNode(), + occupancy_block.getRootScale(), esdf_accelerator, + robot_radius, classified_block.getRootNode()); + }); +} + std::pair, ClassifiedMap::HeightType> ClassifiedMap::getValueOrAncestor(const OctreeIndex& index) const { const OctreeIndex parent_index = index.computeParentIndex(); @@ -232,8 +289,7 @@ void ClassifiedMap::recursiveClassifier( // NOLINT const HashedWaveletOctreeBlock::NodeType& occupancy_node, FloatingPoint average_occupancy, ClassifiedMap::Node& classified_node) { const auto child_occupancies = - HashedWaveletOctree::Block::Transform::backward( - {average_occupancy, occupancy_node.data()}); + HaarTransform::backward({average_occupancy, occupancy_node.data()}); for (int child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { const FloatingPoint child_occupancy = child_occupancies[child_idx]; // If the node has children, recurse @@ -267,4 +323,79 @@ void ClassifiedMap::recursiveClassifier( // NOLINT } } } + +void ClassifiedMap::recursiveClassifier( // NOLINT + const OctreeIndex& node_index, + const HashedWaveletOctreeBlock::NodeType* occupancy_node, + FloatingPoint occupancy_average, + QueryAccelerator& esdf_map, + FloatingPoint robot_radius, ClassifiedMap::Node& classified_node) { + // Compute the child occupancies, if appropriate + ChildAverages child_occupancies{}; + if (occupancy_node) { + child_occupancies = + HaarTransform::backward({occupancy_average, occupancy_node->data()}); + } + + // Iterate over all children + for (int child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { + // Get the child's index, occupancy node pointer and occupancy + const OctreeIndex child_index = node_index.computeChildIndex(child_idx); + const auto* child_occupancy_node = + occupancy_node ? occupancy_node->getChild(child_idx) : nullptr; + const FloatingPoint child_occupancy = + occupancy_node ? child_occupancies[child_idx] : occupancy_average; + + // If the ESDF resolution has been reached, + // or we're in a region that's fully occupied or unobserved, classify + const bool esdf_resolution_reached = child_index.height == 0; + const bool is_free = classifier_.is(child_occupancy, Occupancy::kFree); + const bool is_non_free_leaf = !is_free && !child_occupancy_node; + if (esdf_resolution_reached || is_non_free_leaf) { + // If the child's occupancy is free, check if it's also free in the ESDF + if (is_free) { + DCHECK_EQ(child_index.height, 0); + if (auto* distance = esdf_map.getValue(child_index.position); + distance && *distance < robot_radius) { + // The child is not free in the ESDF (robot would be in collision) + classified_node.data().has_free.set(child_idx, false); + classified_node.data().has_occupied.set(child_idx, true); + classified_node.data().has_unobserved.set(child_idx, false); + continue; + } + // NOTE: The ESDF is defined up to ESDF.max_distance which we asserted + // to be larger than the robot_radius. So if a cell's ESDF + // distance is undefined, it must be safe to traverse. + } + // Otherwise, the classification result only depends on the occupancy + const bool is_occupied = + classifier_.is(child_occupancy, Occupancy::kOccupied); + const bool is_unobserved = + classifier_.is(child_occupancy, Occupancy::kUnobserved); + classified_node.data().has_free.set(child_idx, is_free); + classified_node.data().has_occupied.set(child_idx, is_occupied); + classified_node.data().has_unobserved.set(child_idx, is_unobserved); + continue; + } + + // Otherwise, recursively evaluate the child's descendants + auto& classified_child_node = classified_node.getOrAllocateChild(child_idx); + recursiveClassifier(child_index, child_occupancy_node, child_occupancy, + esdf_map, robot_radius, classified_child_node); + // Collect the results + const bool child_has_free = classified_child_node.data().has_free.any(); + const bool child_has_occupied = + classified_child_node.data().has_occupied.any(); + const bool child_has_unobserved = + classified_child_node.data().has_unobserved.any(); + // Store the results + classified_node.data().has_free.set(child_idx, child_has_free); + classified_node.data().has_occupied.set(child_idx, child_has_occupied); + classified_node.data().has_unobserved.set(child_idx, child_has_unobserved); + // Prune away homogeneous children + if (child_has_free + child_has_occupied + child_has_unobserved == 1) { + classified_node.eraseChild(child_idx); + } + } +} } // namespace wavemap From 5a40d0f0abeb14811c1c2aec9a7cb221b70dbea7 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Sat, 3 Feb 2024 13:34:28 +0100 Subject: [PATCH 079/159] Extend ClassifiedMap interfaces and convenience methods --- .../wavemap/data_structure/dense_block_hash.h | 3 + .../data_structure/ndtree_block_hash.h | 3 + .../wavemap/utils/query/classified_map.h | 35 ++++++++-- .../utils/query/impl/classified_map_inl.h | 28 +++++++- .../wavemap/src/utils/query/classified_map.cc | 67 +++++++++++++++---- .../wavemap_ros/impl/rosbag_processor_inl.h | 2 +- 6 files changed, 119 insertions(+), 19 deletions(-) diff --git a/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h b/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h index b4b86c023..7e5e6ce34 100644 --- a/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h +++ b/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h @@ -25,6 +25,9 @@ class DenseBlockHash { size_t size() const { return Block::kCellsPerBlock * block_map_.size(); } void clear() { block_map_.clear(); } + Index getMinBlockIndex() const { return block_map_.getMinBlockIndex(); } + Index getMaxBlockIndex() const { return block_map_.getMaxBlockIndex(); } + bool hasBlock(const Index& block_index) const; bool eraseBlock(const Index& block_index); template diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h b/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h index 8b614401c..d8e7dfcd4 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h +++ b/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h @@ -29,6 +29,9 @@ class NdtreeBlockHash { size_t size() const; void clear() { block_map_.clear(); } + Index getMinBlockIndex() const { return block_map_.getMinBlockIndex(); } + Index getMaxBlockIndex() const { return block_map_.getMaxBlockIndex(); } + HeightType getMaxHeight() const { return max_height_; } bool hasBlock(const Index& block_index) const; diff --git a/libraries/wavemap/include/wavemap/utils/query/classified_map.h b/libraries/wavemap/include/wavemap/utils/query/classified_map.h index 0c50b98ca..86aa73ac4 100644 --- a/libraries/wavemap/include/wavemap/utils/query/classified_map.h +++ b/libraries/wavemap/include/wavemap/utils/query/classified_map.h @@ -2,6 +2,7 @@ #define WAVEMAP_UTILS_QUERY_CLASSIFIED_MAP_H_ #include +#include #include #include @@ -37,6 +38,9 @@ class ClassifiedMap { using Node = BlockHashMap::Node; static constexpr int kDim = 3; + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + ClassifiedMap(FloatingPoint min_cell_width, IndexElement tree_height, const OccupancyClassifier& classifier); @@ -47,17 +51,28 @@ class ClassifiedMap { const OccupancyClassifier& classifier, const HashedBlocks& esdf_map, FloatingPoint robot_radius); + bool empty() const { return block_map_.empty(); } + FloatingPoint getMinCellWidth() const { return min_cell_width_; } IndexElement getTreeHeight() const { return block_map_.getMaxHeight(); } + Index3D getMinIndex() const; + Index3D getMaxIndex() const; + Index3D getMinBlockIndex() const { return block_map_.getMinBlockIndex(); } + Index3D getMaxBlockIndex() const { return block_map_.getMaxBlockIndex(); } + void update(const HashedWaveletOctree& occupancy_map); void update(const HashedWaveletOctree& occupancy_map, const HashedBlocks& esdf_map, FloatingPoint robot_radius); + bool has(const Index3D& index, Occupancy::Id occupancy_type) const; bool has(const OctreeIndex& index, Occupancy::Id occupancy_type) const; + bool has(const Index3D& index, Occupancy::Mask occupancy_mask) const; bool has(const OctreeIndex& index, Occupancy::Mask occupancy_mask) const; + bool isFully(const Index3D& index, Occupancy::Id occupancy_type) const; bool isFully(const OctreeIndex& index, Occupancy::Id occupancy_type) const; + bool isFully(const Index3D& index, Occupancy::Mask occupancy_mask) const; bool isFully(const OctreeIndex& index, Occupancy::Mask occupancy_mask) const; bool hasBlock(const Index3D& block_index) const; @@ -74,11 +89,21 @@ class ClassifiedMap { std::pair, HeightType> getValueOrAncestor( const OctreeIndex& index) const; + auto& getHashMap() { return block_map_.getHashMap(); } + const auto& getHashMap() const { return block_map_.getHashMap(); } + + template + void forEachBlock(IndexedBlockVisitor visitor_fn) const; + + using IndexedLeafVisitorFunction = + std::function; + void forEachLeaf(IndexedLeafVisitorFunction visitor_fn, + IndexElement termination_height = 0) const; void forEachLeafMatching(Occupancy::Id occupancy_type, - std::function visitor_fn, + IndexedLeafVisitorFunction visitor_fn, IndexElement termination_height = 0) const; void forEachLeafMatching(Occupancy::Mask occupancy_mask, - std::function visitor_fn, + IndexedLeafVisitorFunction visitor_fn, IndexElement termination_height = 0) const; private: @@ -86,9 +111,11 @@ class ClassifiedMap { using ChildAverages = HashedWaveletOctree::Block::Coefficients::CoefficientsArray; + const IndexElement tree_height_; const FloatingPoint min_cell_width_; + const IndexElement cells_per_block_side_ = int_math::exp2(tree_height_); const OccupancyClassifier classifier_; - BlockHashMap block_map_; + BlockHashMap block_map_{tree_height_}; // Cache previous queries struct QueryCache { @@ -117,7 +144,7 @@ class ClassifiedMap { void reset(); }; - mutable QueryCache query_cache_; + mutable QueryCache query_cache_{tree_height_}; void recursiveClassifier( const HashedWaveletOctreeBlock::NodeType& occupancy_node, diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h b/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h index 47f3c4d41..8f61ca638 100644 --- a/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h +++ b/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h @@ -17,29 +17,48 @@ inline bool ChildBitset::any() const { return bitset != static_cast(0); } +inline bool ClassifiedMap::has(const Index3D& index, + Occupancy::Id occupancy_type) const { + return has(OctreeIndex{0, index}, occupancy_type); +} + inline bool ClassifiedMap::has(const OctreeIndex& index, Occupancy::Id occupancy_type) const { return has(index, Occupancy::toMask(occupancy_type)); } +inline bool ClassifiedMap::has(const Index3D& index, + Occupancy::Mask occupancy_mask) const { + return has(OctreeIndex{0, index}, occupancy_mask); +} + inline bool ClassifiedMap::has(const OctreeIndex& index, Occupancy::Mask occupancy_mask) const { return query_cache_.has(index, occupancy_mask, block_map_); } +inline bool ClassifiedMap::isFully(const Index3D& index, + Occupancy::Id occupancy_type) const { + return isFully(OctreeIndex{0, index}, occupancy_type); +} + inline bool ClassifiedMap::isFully(const OctreeIndex& index, Occupancy::Id occupancy_type) const { return isFully(index, Occupancy::toMask(occupancy_type)); } +inline bool ClassifiedMap::isFully(const Index3D& index, + Occupancy::Mask occupancy_mask) const { + return isFully(OctreeIndex{0, index}, occupancy_mask); +} + inline bool ClassifiedMap::isFully(const OctreeIndex& index, Occupancy::Mask occupancy_mask) const { return query_cache_.isFully(index, occupancy_mask, block_map_); } inline void ClassifiedMap::forEachLeafMatching( - Occupancy::Id occupancy_type, - std::function visitor_fn, + Occupancy::Id occupancy_type, IndexedLeafVisitorFunction visitor_fn, IndexElement termination_height) const { forEachLeafMatching(Occupancy::toMask(occupancy_type), std::move(visitor_fn), termination_height); @@ -84,6 +103,11 @@ inline std::optional ClassifiedMap::getValue( return getValueOrAncestor(index).first; } +template +void ClassifiedMap::forEachBlock(IndexedBlockVisitor visitor_fn) const { + block_map_.forEachBlock(visitor_fn); +} + inline const ClassifiedMap::Block* ClassifiedMap::QueryCache::getBlock( const Index3D& new_block_index, const ClassifiedMap::BlockHashMap& block_map) { diff --git a/libraries/wavemap/src/utils/query/classified_map.cc b/libraries/wavemap/src/utils/query/classified_map.cc index 7c35bfa22..f2228e2bb 100644 --- a/libraries/wavemap/src/utils/query/classified_map.cc +++ b/libraries/wavemap/src/utils/query/classified_map.cc @@ -6,10 +6,9 @@ namespace wavemap { ClassifiedMap::ClassifiedMap(FloatingPoint min_cell_width, IndexElement tree_height, const OccupancyClassifier& classifier) - : min_cell_width_(min_cell_width), - classifier_(classifier), - block_map_(tree_height), - query_cache_(tree_height) {} + : tree_height_(tree_height), + min_cell_width_(min_cell_width), + classifier_(classifier) {} ClassifiedMap::ClassifiedMap(const HashedWaveletOctree& occupancy_map, const OccupancyClassifier& classifier) @@ -27,6 +26,17 @@ ClassifiedMap::ClassifiedMap(const HashedWaveletOctree& occupancy_map, update(occupancy_map, esdf_map, robot_radius); } +Index3D ClassifiedMap::getMinIndex() const { + return cells_per_block_side_ * getMinBlockIndex(); +} + +Index3D ClassifiedMap::getMaxIndex() const { + if (empty()) { + return Index3D::Zero(); + } + return cells_per_block_side_ * (getMaxBlockIndex().array() + 1) - 1; +} + void ClassifiedMap::update(const HashedWaveletOctree& occupancy_map) { ZoneScoped; // Reset the query cache @@ -96,10 +106,43 @@ ClassifiedMap::getValueOrAncestor(const OctreeIndex& index) const { return {std::nullopt, getTreeHeight()}; } -void ClassifiedMap::forEachLeafMatching( - Occupancy::Mask occupancy_mask, - std::function visitor_fn, - IndexElement termination_height) const { +void ClassifiedMap::forEachLeaf(IndexedLeafVisitorFunction visitor_fn, + IndexElement termination_height) const { + forEachBlock([&visitor_fn, termination_height](const Index3D& block_index, + const Block& block) { + struct StackElement { + const OctreeIndex node_index; + const Node& node; + }; + std::stack stack; + stack.emplace(StackElement{OctreeIndex{block.getMaxHeight(), block_index}, + block.getRootNode()}); + while (!stack.empty()) { + const OctreeIndex node_index = stack.top().node_index; + const Node& node = stack.top().node; + stack.pop(); + + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + const OctreeIndex child_node_index = + node_index.computeChildIndex(child_idx); + if (node.hasChild(child_idx) && + termination_height < child_node_index.height) { + const Node& child_node = *node.getChild(child_idx); + stack.emplace(StackElement{child_node_index, child_node}); + } else { + const Occupancy::Mask child_occupancy = + node.data().childOccupancyMask(child_idx); + std::invoke(visitor_fn, child_node_index, child_occupancy); + } + } + } + }); +} + +void ClassifiedMap::forEachLeafMatching(Occupancy::Mask occupancy_mask, + IndexedLeafVisitorFunction visitor_fn, + IndexElement termination_height) const { block_map_.forEachBlock([occupancy_mask, termination_height, &visitor_fn]( const Index3D& block_index, const Block& block) { struct StackElement { @@ -116,15 +159,15 @@ void ClassifiedMap::forEachLeafMatching( for (NdtreeIndexRelativeChild child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { - const auto region_occupancy = node.data().childOccupancyMask(child_idx); - if (!OccupancyClassifier::has(region_occupancy, occupancy_mask)) { + const auto child_occupancy = node.data().childOccupancyMask(child_idx); + if (!OccupancyClassifier::has(child_occupancy, occupancy_mask)) { continue; } const OctreeIndex child_node_index = node_index.computeChildIndex(child_idx); - if (OccupancyClassifier::isFully(region_occupancy, occupancy_mask) || + if (OccupancyClassifier::isFully(child_occupancy, occupancy_mask) || child_node_index.height <= termination_height) { - std::invoke(visitor_fn, child_node_index); + std::invoke(visitor_fn, child_node_index, child_occupancy); } else if (const Node* child_node = node.getChild(child_idx); child_node) { stack.emplace(StackElement{child_node_index, *child_node}); diff --git a/ros/wavemap_ros/include/wavemap_ros/impl/rosbag_processor_inl.h b/ros/wavemap_ros/include/wavemap_ros/impl/rosbag_processor_inl.h index 4a35542ef..3d4e48a23 100644 --- a/ros/wavemap_ros/include/wavemap_ros/impl/rosbag_processor_inl.h +++ b/ros/wavemap_ros/include/wavemap_ros/impl/rosbag_processor_inl.h @@ -24,7 +24,7 @@ void RosbagProcessor::addCallback( auto rosbag_callback = [function_ptr, object_ptr](const rosbag::MessageInstance& msg) { auto msg_instance = msg.instantiate(); - ((*object_ptr).*function_ptr)(*msg_instance); + std::invoke(function_ptr, *object_ptr, *msg_instance); }; callbacks_.try_emplace(ros_topic_name, rosbag_callback); } From 56368ef96d078bc10fdb6ec8cdde51a34beff362 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Sat, 3 Feb 2024 14:45:11 +0100 Subject: [PATCH 080/159] Make it possible to pass expressions in param validators --- .../include/wavemap/config/param_checks.h | 29 +++++++++---------- 1 file changed, 13 insertions(+), 16 deletions(-) diff --git a/libraries/wavemap/include/wavemap/config/param_checks.h b/libraries/wavemap/include/wavemap/config/param_checks.h index fe01a66fa..06026e4be 100644 --- a/libraries/wavemap/include/wavemap/config/param_checks.h +++ b/libraries/wavemap/include/wavemap/config/param_checks.h @@ -6,28 +6,25 @@ #include "wavemap/utils/meta/nameof.h" -#define IS_PARAM_EQ(value, threshold, verbose) \ - wavemap::is_param>(value, threshold, verbose, NAMEOF(value), \ - "==") +#define IS_PARAM_EQ(value, threshold, verbose) \ + wavemap::is_param>(value, threshold, verbose, #value, "==") -#define IS_PARAM_NE(value, threshold, verbose) \ - wavemap::is_param>(value, threshold, verbose, \ - NAMEOF(value), "!=") +#define IS_PARAM_NE(value, threshold, verbose) \ + wavemap::is_param>(value, threshold, verbose, #value, \ + "!=") #define IS_PARAM_LT(value, threshold, verbose) \ - wavemap::is_param>(value, threshold, verbose, NAMEOF(value), "<") + wavemap::is_param>(value, threshold, verbose, #value, "<") -#define IS_PARAM_LE(value, threshold, verbose) \ - wavemap::is_param>(value, threshold, verbose, \ - NAMEOF(value), "<=") +#define IS_PARAM_LE(value, threshold, verbose) \ + wavemap::is_param>(value, threshold, verbose, #value, "<=") -#define IS_PARAM_GT(value, threshold, verbose) \ - wavemap::is_param>(value, threshold, verbose, NAMEOF(value), \ - ">") +#define IS_PARAM_GT(value, threshold, verbose) \ + wavemap::is_param>(value, threshold, verbose, #value, ">") -#define IS_PARAM_GE(value, threshold, verbose) \ - wavemap::is_param>(value, threshold, verbose, \ - NAMEOF(value), ">=") +#define IS_PARAM_GE(value, threshold, verbose) \ + wavemap::is_param>(value, threshold, verbose, #value, \ + ">=") namespace wavemap { template From ecb714924c748c685c9ce776a39c49f5bd07464c Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Sat, 3 Feb 2024 21:29:15 +0100 Subject: [PATCH 081/159] Directly use the ClassifiedMap to sample random (collision free) points --- .../wavemap/utils/query/point_sampler.h | 28 +++++----------- .../wavemap/src/utils/query/point_sampler.cc | 33 ++++--------------- 2 files changed, 15 insertions(+), 46 deletions(-) diff --git a/libraries/wavemap/include/wavemap/utils/query/point_sampler.h b/libraries/wavemap/include/wavemap/utils/query/point_sampler.h index 38b6bc813..aebe45dbb 100644 --- a/libraries/wavemap/include/wavemap/utils/query/point_sampler.h +++ b/libraries/wavemap/include/wavemap/utils/query/point_sampler.h @@ -1,21 +1,19 @@ #ifndef WAVEMAP_UTILS_QUERY_POINT_SAMPLER_H_ #define WAVEMAP_UTILS_QUERY_POINT_SAMPLER_H_ +#include + #include "wavemap/common.h" #include "wavemap/data_structure/aabb.h" -#include "wavemap/map/hashed_blocks.h" -#include "wavemap/map/hashed_wavelet_octree.h" +#include "wavemap/utils/query/classified_map.h" #include "wavemap/utils/query/occupancy.h" -#include "wavemap/utils/query/occupancy_classifier.h" -#include "wavemap/utils/query/query_accelerator.h" #include "wavemap/utils/random_number_generator.h" namespace wavemap { class PointSampler { public: - explicit PointSampler(const HashedWaveletOctree& occupancy_map, - OccupancyClassifier classifier = OccupancyClassifier{}) - : occupancy_map_(occupancy_map), classifier_(classifier) {} + explicit PointSampler(ClassifiedMap::ConstPtr classified_map) + : classified_map_(std::move(classified_map)) {} std::optional getRandomBlock(); @@ -28,20 +26,13 @@ class PointSampler { const std::optional>& aabb = std::nullopt, size_t max_attempts = 1000); - std::optional getRandomCollisionFreePoint( - const HashedBlocks& esdf, FloatingPoint robot_radius, - const std::optional>& aabb = std::nullopt, - size_t max_attempts = 1000); - private: - const HashedWaveletOctree& occupancy_map_; - const OccupancyClassifier classifier_; - - QueryAccelerator query_accelerator_{occupancy_map_}; + const ClassifiedMap::ConstPtr classified_map_; RandomNumberGenerator rng_; - const IndexElement block_height_ = occupancy_map_.getTreeHeight(); - const FloatingPoint min_cell_width_ = occupancy_map_.getMinCellWidth(); + + const IndexElement block_height_ = classified_map_->getTreeHeight(); + const FloatingPoint min_cell_width_ = classified_map_->getMinCellWidth(); const FloatingPoint min_cell_width_inv_ = 1.f / min_cell_width_; std::optional getRandomPointImpl( @@ -51,7 +42,6 @@ class PointSampler { Point3D getRandomPointInAABB(const AABB& aabb); Point3D getRandomPointInBlock(const Index3D& block_index); }; - } // namespace wavemap #include "wavemap/utils/query/impl/point_sampler_inl.h" diff --git a/libraries/wavemap/src/utils/query/point_sampler.cc b/libraries/wavemap/src/utils/query/point_sampler.cc index 5666dc127..070b6a068 100644 --- a/libraries/wavemap/src/utils/query/point_sampler.cc +++ b/libraries/wavemap/src/utils/query/point_sampler.cc @@ -2,45 +2,25 @@ namespace wavemap { std::optional PointSampler::getRandomBlock() { - if (occupancy_map_.empty()) { + if (classified_map_->empty()) { return std::nullopt; } - const size_t num_blocks = occupancy_map_.getHashMap().size(); + const size_t num_blocks = classified_map_->getHashMap().size(); const size_t nth_block = rng_.getRandomInteger(0ul, num_blocks - 1ul); - auto it = occupancy_map_.getHashMap().begin(); + auto it = classified_map_->getHashMap().begin(); std::advance(it, nth_block); - if (it == occupancy_map_.getHashMap().end()) { + if (it == classified_map_->getHashMap().end()) { return std::nullopt; } return it->first; } -std::optional PointSampler::getRandomCollisionFreePoint( - const HashedBlocks& esdf, FloatingPoint robot_radius, - const std::optional>& aabb, size_t max_attempts) { - for (size_t attempt_idx = 0; attempt_idx < max_attempts; ++attempt_idx) { - auto point = - getRandomPointImpl(Occupancy::kFree, aabb, attempt_idx, max_attempts); - if (!point) { - continue; - } - const auto index = - convert::pointToNearestIndex(point.value(), min_cell_width_inv_); - const FloatingPoint distance = esdf.getCellValue(index); - if (robot_radius <= distance) { - return point.value(); - } - } - - return std::nullopt; -} - std::optional PointSampler::getRandomPointImpl( Occupancy::Mask occupancy_mask, const std::optional>& aabb, size_t& attempt_idx, size_t max_attempts) { - if (occupancy_map_.empty()) { + if (classified_map_->empty()) { return std::nullopt; } @@ -54,8 +34,7 @@ std::optional PointSampler::getRandomPointImpl( continue; } const auto index = convert::pointToNearestIndex(point, min_cell_width_inv_); - const FloatingPoint log_odds = query_accelerator_.getCellValue(index); - if (classifier_.is(log_odds, occupancy_mask)) { + if (classified_map_->isFully(index, occupancy_mask)) { return point; } } From 1e47bac923f06c1e2701b5c9cb9195ad00f019e7 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Sun, 4 Feb 2024 23:00:56 +0100 Subject: [PATCH 082/159] Improve SDF generator tests --- .../utils/sdf/full_euclidean_sdf_generator.h | 4 ++-- .../utils/sdf/quasi_euclidean_sdf_generator.h | 4 ++-- .../src/utils/sdf/quasi_euclidean_sdf_generator.cc | 4 +++- .../test/src/utils/sdf/test_sdf_generators.cc | 14 +++++++++++--- 4 files changed, 18 insertions(+), 8 deletions(-) diff --git a/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h b/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h index 2f8dc2ef4..b64c25b53 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h +++ b/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h @@ -24,8 +24,8 @@ using VectorDistanceField = DenseBlockHash; class FullEuclideanSDFGenerator { public: - static constexpr FloatingPoint kMaxRelativeUnderEstimate = 1e-3f; - static constexpr FloatingPoint kMaxRelativeOverEstimate = 1e-3f; + static constexpr FloatingPoint kMaxRelativeUnderEstimate = 1e-2f; + static constexpr FloatingPoint kMaxRelativeOverEstimate = 1e-2f; explicit FullEuclideanSDFGenerator(FloatingPoint max_distance = 2.f, FloatingPoint occupancy_threshold = 0.f) diff --git a/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h b/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h index e960a31a2..d68ebc721 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h +++ b/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h @@ -10,8 +10,8 @@ namespace wavemap { class QuasiEuclideanSDFGenerator { public: - static constexpr FloatingPoint kMaxRelativeUnderEstimate = 1e-3f; - static constexpr FloatingPoint kMaxRelativeOverEstimate = 0.125f + 1e-3f; + static constexpr FloatingPoint kMaxRelativeUnderEstimate = 1e-2f; + static constexpr FloatingPoint kMaxRelativeOverEstimate = 0.125f + 1e-2f; explicit QuasiEuclideanSDFGenerator(FloatingPoint max_distance = 2.f, FloatingPoint occupancy_threshold = 0.f) diff --git a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc index 243f3dbc1..1857a18e3 100644 --- a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc +++ b/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc @@ -144,7 +144,9 @@ void QuasiEuclideanSDFGenerator::propagate( } // Handle sign changes when propagating across the surface - if (std::signbit(neighbor_sdf) != std::signbit(sdf_value)) { + const bool crossed_surface = + std::signbit(neighbor_sdf) != std::signbit(sdf_value); + if (crossed_surface) { if (neighbor_sdf < 0.f) { // NOTE: When the opened cell and the neighbor cell have the same // sign, the distance field value and offset are summed to diff --git a/libraries/wavemap/test/src/utils/sdf/test_sdf_generators.cc b/libraries/wavemap/test/src/utils/sdf/test_sdf_generators.cc index 2d97e2b8c..61997a6ab 100644 --- a/libraries/wavemap/test/src/utils/sdf/test_sdf_generators.cc +++ b/libraries/wavemap/test/src/utils/sdf/test_sdf_generators.cc @@ -58,11 +58,18 @@ TYPED_TEST(SdfGeneratorTest, BruteForceEquivalence) { // Set obstacles to occupied for (const Index3D& index : obstacle_cells) { - map.setCellValue(index, config.max_log_odds); + map.addToCellValue(index, config.max_log_odds); + } + + // Check if all obstacles were set correctly + for (const Index3D& index : obstacle_cells) { + ASSERT_TRUE(classifier.is(map.getCellValue(index), Occupancy::kOccupied)) + << "For node " << print::eigen::oneLine(index); } // Generate the SDF - TypeParam sdf_generator{kMaxSdfDistance}; + TypeParam sdf_generator{kMaxSdfDistance, + classifier.getOccupancyThreshold()}; const auto sdf = sdf_generator.generate(map); // Compare the SDF distances to the brute force min distance @@ -107,6 +114,7 @@ TYPED_TEST(SdfGeneratorTest, BruteForceEquivalence) { const FloatingPoint neighbor_occupancy_value = map.getCellValue(neighbor_index); if (classifier.is(neighbor_occupancy_value, Occupancy::kFree)) { + ASSERT_TRUE(neighbor_index != node_index.position); const auto free_cell_aabb = convert::nodeIndexToAABB( OctreeIndex{0, neighbor_index}, min_cell_width); const FloatingPoint min_dist = @@ -137,7 +145,7 @@ TYPED_TEST(SdfGeneratorTest, BruteForceEquivalence) { constexpr FloatingPoint kMaxRelativeOverEstimate = TypeParam::kMaxRelativeOverEstimate; if (std::abs(sdf_brute_force) < sdf.getDefaultValue()) { - if (0.f < sdf_brute_force) { + if (classifier.is(occupancy_value, Occupancy::kFree)) { EXPECT_LT(sdf_value, sdf_brute_force * (1.f + kMaxRelativeOverEstimate)) << "At index " << print::eigen::oneLine(node_index.position) From b66ecefa8e977c18751f2d63354eb0cce0d6239b Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 16 Feb 2024 12:33:07 +0100 Subject: [PATCH 083/159] Expose height of last query result --- libraries/wavemap/include/wavemap/utils/query/classified_map.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/wavemap/include/wavemap/utils/query/classified_map.h b/libraries/wavemap/include/wavemap/utils/query/classified_map.h index 86aa73ac4..4d0f23a80 100644 --- a/libraries/wavemap/include/wavemap/utils/query/classified_map.h +++ b/libraries/wavemap/include/wavemap/utils/query/classified_map.h @@ -60,6 +60,7 @@ class ClassifiedMap { Index3D getMaxIndex() const; Index3D getMinBlockIndex() const { return block_map_.getMinBlockIndex(); } Index3D getMaxBlockIndex() const { return block_map_.getMaxBlockIndex(); } + IndexElement getLastResultHeight() const { return query_cache_.height; } void update(const HashedWaveletOctree& occupancy_map); void update(const HashedWaveletOctree& occupancy_map, From 7f4905c293285dfb2e1212fbf5aab27e366dd96a Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 16 Feb 2024 22:17:58 +0100 Subject: [PATCH 084/159] Improve param variant syntax and add method to get child as given type --- .../wavemap/config/impl/config_base_inl.h | 6 +++--- .../wavemap/config/impl/type_selector_inl.h | 4 ++-- .../wavemap/config/impl/value_with_unit_inl.h | 6 +++--- .../wavemap/include/wavemap/config/param.h | 17 +++++++++++++---- .../src/input_handler/input_handler.cc | 2 +- 5 files changed, 22 insertions(+), 13 deletions(-) diff --git a/libraries/wavemap/include/wavemap/config/impl/config_base_inl.h b/libraries/wavemap/include/wavemap/config/impl/config_base_inl.h index 228ac366e..2709759f1 100644 --- a/libraries/wavemap/include/wavemap/config/impl/config_base_inl.h +++ b/libraries/wavemap/include/wavemap/config/impl/config_base_inl.h @@ -68,10 +68,10 @@ void loadParam(const param::Name& param_name, const param::Value& param_value, ConfigDerivedT& config, MemberPtrT config_member_ptr) { ConfigValueT& config_value = config.*config_member_ptr; if (param_value.holds()) { - config_value = ConfigValueT{param_value.get().value()}; + config_value = ConfigValueT{param_value.as().value()}; return; } else if constexpr (std::is_same_v) { - if (const auto param_int = param_value.get(); param_int) { + if (const auto param_int = param_value.as(); param_int) { // If the param_value and config_value's types do not match exactly, we // still allow automatic conversions from ints to floats // NOTE: This is avoids pesky, potentially confusing errors when setting @@ -120,7 +120,7 @@ template ConfigBase::from( const param::Value& params) { - const auto param_map = params.get(); + const auto param_map = params.as(); if (!param_map) { LOG(WARNING) << "Tried to load config from a param that is not of type Map " "(dictionary). Will be ignored."; diff --git a/libraries/wavemap/include/wavemap/config/impl/type_selector_inl.h b/libraries/wavemap/include/wavemap/config/impl/type_selector_inl.h index ea1d4568e..b2e4890bc 100644 --- a/libraries/wavemap/include/wavemap/config/impl/type_selector_inl.h +++ b/libraries/wavemap/include/wavemap/config/impl/type_selector_inl.h @@ -74,7 +74,7 @@ std::optional TypeSelector::from( std::string type_name; if (params.holds()) { // If the param is of type string, read the name directly - type_name = params.get().value(); + type_name = params.as().value(); } else if (params.holds()) { // If the param is of type Map, try to read the name from its "type" subkey const auto type_param = params.getChild(param::kTypeSelectorKey); @@ -90,7 +90,7 @@ std::optional TypeSelector::from( return std::nullopt; } - type_name = type_param->get().value(); + type_name = type_param->as().value(); } else { LOG(WARNING) << "Type names can only be read from params of type string, " "or from param maps (dictionary) with a subkey named \"" diff --git a/libraries/wavemap/include/wavemap/config/impl/value_with_unit_inl.h b/libraries/wavemap/include/wavemap/config/impl/value_with_unit_inl.h index f918c456f..9df68a633 100644 --- a/libraries/wavemap/include/wavemap/config/impl/value_with_unit_inl.h +++ b/libraries/wavemap/include/wavemap/config/impl/value_with_unit_inl.h @@ -12,7 +12,7 @@ std::string ValueWithUnit::toStr() const { template std::optional> ValueWithUnit::from( const param::Value& params) { - const auto param_map = params.get(); + const auto param_map = params.as(); if (!param_map) { LOG(WARNING) << "Tried to load a value with annotated units from a param " "that is not of type Map (dictionary). Cannot perform " @@ -46,11 +46,11 @@ std::optional> ValueWithUnit::from( return std::nullopt; } - if (const auto param_float = param_value.get(); param_float) { + if (const auto param_float = param_value.as(); param_float) { return param_float.value() * conversion_factor; } - if (const auto param_int = param_value.get(); param_int) { + if (const auto param_int = param_value.as(); param_int) { return static_cast(param_int.value()) * conversion_factor; } diff --git a/libraries/wavemap/include/wavemap/config/param.h b/libraries/wavemap/include/wavemap/config/param.h index ded4a01b6..a33ddbafe 100644 --- a/libraries/wavemap/include/wavemap/config/param.h +++ b/libraries/wavemap/include/wavemap/config/param.h @@ -32,7 +32,7 @@ class ValueT { // Try to read the value using type T. Returns an empty optional if it fails. template - std::optional get() const { + std::optional as() const { if (holds()) { return std::get(data_); } else { @@ -42,14 +42,23 @@ class ValueT { // Methods to work with nested configs inline bool hasChild(const Name& key) const { - if (const auto map = get(); map) { + if (const auto map = as(); map) { return map.value().count(key); } return false; } std::optional getChild(const Name& key) const { - if (const auto map = get(); map) { - return map.value().at(key); + if (const auto map = as(); map) { + if (const auto it = map.value().find(key); it != map.value().end()) { + return it->second; + } + } + return std::nullopt; + } + template + std::optional getChildAs(const Name& key) const { + if (const auto child = getChild(key); child) { + return child.value().template as(); } return std::nullopt; } diff --git a/ros/wavemap_ros/src/input_handler/input_handler.cc b/ros/wavemap_ros/src/input_handler/input_handler.cc index fd0fe110c..24289bd37 100644 --- a/ros/wavemap_ros/src/input_handler/input_handler.cc +++ b/ros/wavemap_ros/src/input_handler/input_handler.cc @@ -45,7 +45,7 @@ InputHandler::InputHandler(const InputHandlerConfig& config, "params. Input handler will be disabled."); return; } - const auto integrators_array = integrators_param->get(); + const auto integrators_array = integrators_param->as(); if (!integrators_array) { ROS_WARN( "Key named \"integrators\" in input handler params is not " From 3fb15471a59aa8e9994d3e4e17c147c98be2dbf5 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Sat, 17 Feb 2024 16:14:06 +0100 Subject: [PATCH 085/159] Make it possible to fix the PointSampler's random seed --- .../include/wavemap/utils/query/point_sampler.h | 6 ++++-- .../include/wavemap/utils/random_number_generator.h | 13 ++++++------- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/libraries/wavemap/include/wavemap/utils/query/point_sampler.h b/libraries/wavemap/include/wavemap/utils/query/point_sampler.h index aebe45dbb..196b687e7 100644 --- a/libraries/wavemap/include/wavemap/utils/query/point_sampler.h +++ b/libraries/wavemap/include/wavemap/utils/query/point_sampler.h @@ -1,6 +1,7 @@ #ifndef WAVEMAP_UTILS_QUERY_POINT_SAMPLER_H_ #define WAVEMAP_UTILS_QUERY_POINT_SAMPLER_H_ +#include #include #include "wavemap/common.h" @@ -12,8 +13,9 @@ namespace wavemap { class PointSampler { public: - explicit PointSampler(ClassifiedMap::ConstPtr classified_map) - : classified_map_(std::move(classified_map)) {} + explicit PointSampler(ClassifiedMap::ConstPtr classified_map, + size_t random_seed = std::random_device()()) + : classified_map_(std::move(classified_map)), rng_(random_seed) {} std::optional getRandomBlock(); diff --git a/libraries/wavemap/include/wavemap/utils/random_number_generator.h b/libraries/wavemap/include/wavemap/utils/random_number_generator.h index 6f5be7821..ff6932c62 100644 --- a/libraries/wavemap/include/wavemap/utils/random_number_generator.h +++ b/libraries/wavemap/include/wavemap/utils/random_number_generator.h @@ -8,25 +8,24 @@ namespace wavemap { class RandomNumberGenerator { public: - RandomNumberGenerator() : random_number_generator_(std::random_device()()) {} - explicit RandomNumberGenerator(const size_t random_seed) + explicit RandomNumberGenerator(size_t random_seed = std::random_device()()) : random_number_generator_(random_seed) {} - void setRandomSeed(const size_t random_seed) { + void setRandomSeed(size_t random_seed) { random_number_generator_.seed(random_seed); } template - IntegerType getRandomInteger(const IntegerType lower_bound, - const IntegerType upper_bound) { + IntegerType getRandomInteger(IntegerType lower_bound, + IntegerType upper_bound) { std::uniform_int_distribution uniform_distribution( lower_bound, upper_bound); return uniform_distribution(random_number_generator_); } template - FloatingPoint getRandomRealNumber(const RealNumberType lower_bound, - const RealNumberType upper_bound) { + FloatingPoint getRandomRealNumber(RealNumberType lower_bound, + RealNumberType upper_bound) { std::uniform_real_distribution uniform_distribution( lower_bound, upper_bound); return uniform_distribution(random_number_generator_); From ec051ea380170d17348890658ea86139212a3498 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 27 Mar 2024 15:36:28 +0100 Subject: [PATCH 086/159] No longer use catkin_simple for wavemap_io --- libraries/wavemap_io/CMakeLists.txt | 30 +++++++++++++++++++++-------- libraries/wavemap_io/package.xml | 3 +-- 2 files changed, 23 insertions(+), 10 deletions(-) diff --git a/libraries/wavemap_io/CMakeLists.txt b/libraries/wavemap_io/CMakeLists.txt index 1436dd587..3409dbddc 100644 --- a/libraries/wavemap_io/CMakeLists.txt +++ b/libraries/wavemap_io/CMakeLists.txt @@ -1,20 +1,38 @@ cmake_minimum_required(VERSION 3.0.2) project(wavemap_io) -find_package(catkin_simple REQUIRED) -catkin_simple(ALL_DEPS_REQUIRED) -# TODO(victorr): Switch to regular catkin +# Dependencies +find_package(catkin REQUIRED COMPONENTS wavemap) # Compiler definitions and options add_wavemap_compile_definitions_and_options() +# Setup catkin package +catkin_package(INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS) + +# For all targets +include_directories(include ${catkin_INCLUDE_DIRS}) + # Libraries # cmake-lint: disable=C0301 -cs_add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} src/file_conversions.cc src/stream_conversions.cc) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +# Install targets +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + # Tests if (CATKIN_ENABLE_TESTING) catkin_add_gtest( @@ -22,7 +40,3 @@ if (CATKIN_ENABLE_TESTING) test/src/test_file_conversions.cc) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} gtest_main) endif () - -# Export -cs_install() -cs_export() diff --git a/libraries/wavemap_io/package.xml b/libraries/wavemap_io/package.xml index 233ecc0fb..e7c2c6ced 100644 --- a/libraries/wavemap_io/package.xml +++ b/libraries/wavemap_io/package.xml @@ -6,12 +6,11 @@ Victor Reijgwart BSD - https://github.com/victorreijgwart/wavemap + https://github.com/ethz-asl/wavemap Victor Reijgwart catkin - catkin_simple wavemap From ef94488f53622ecedab00194e96db71754be6a3d Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 27 Mar 2024 16:27:25 +0100 Subject: [PATCH 087/159] Rename wavemap server to make room for ROS agnostic server --- ros/wavemap_ros/CMakeLists.txt | 10 ++--- .../app/{server_node.cc => ros_server.cc} | 8 ++-- ros/wavemap_ros/app/rosbag_processor.cc | 4 +- .../{wavemap_server.h => ros_server.h} | 18 ++++---- .../launch/rosbag_processor.launch | 2 +- ros/wavemap_ros/launch/wavemap_server.launch | 2 +- .../src/{wavemap_server.cc => ros_server.cc} | 42 +++++++++---------- 7 files changed, 43 insertions(+), 43 deletions(-) rename ros/wavemap_ros/app/{server_node.cc => ros_server.cc} (71%) rename ros/wavemap_ros/include/wavemap_ros/{wavemap_server.h => ros_server.h} (85%) rename ros/wavemap_ros/src/{wavemap_server.cc => ros_server.cc} (79%) diff --git a/ros/wavemap_ros/CMakeLists.txt b/ros/wavemap_ros/CMakeLists.txt index 09e870159..1bb1101dc 100644 --- a/ros/wavemap_ros/CMakeLists.txt +++ b/ros/wavemap_ros/CMakeLists.txt @@ -32,16 +32,16 @@ cs_add_library(${PROJECT_NAME} src/utils/pointcloud_undistortion/pointcloud_undistorter.cc src/utils/rosbag_processor.cc src/utils/tf_transformer.cc - src/wavemap_server.cc) + src/ros_server.cc) # Link OpenCV explicitly to avoid issues on Jetson target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) # Binaries -cs_add_executable(wavemap_server_node app/server_node.cc) -target_link_libraries(wavemap_server_node ${PROJECT_NAME}) +cs_add_executable(ros_server app/ros_server.cc) +target_link_libraries(ros_server ${PROJECT_NAME}) -cs_add_executable(wavemap_rosbag_processor app/rosbag_processor.cc) -target_link_libraries(wavemap_rosbag_processor ${PROJECT_NAME}) +cs_add_executable(rosbag_processor app/rosbag_processor.cc) +target_link_libraries(rosbag_processor ${PROJECT_NAME}) # Export cs_install() diff --git a/ros/wavemap_ros/app/server_node.cc b/ros/wavemap_ros/app/ros_server.cc similarity index 71% rename from ros/wavemap_ros/app/server_node.cc rename to ros/wavemap_ros/app/ros_server.cc index 645e3d8fd..89ed24ffe 100644 --- a/ros/wavemap_ros/app/server_node.cc +++ b/ros/wavemap_ros/app/ros_server.cc @@ -1,9 +1,9 @@ -#include +#include "wavemap_ros/ros_server.h" -#include "wavemap_ros/wavemap_server.h" +#include int main(int argc, char** argv) { - ros::init(argc, argv, "wavemap_server"); + ros::init(argc, argv, "wavemap_ros_server"); google::InitGoogleLogging(argv[0]); google::ParseCommandLineFlags(&argc, &argv, false); @@ -13,7 +13,7 @@ int main(int argc, char** argv) { ros::NodeHandle nh; ros::NodeHandle nh_private("~"); - wavemap::WavemapServer wavemap_server(nh, nh_private); + wavemap::RosServer wavemap_server(nh, nh_private); ros::spin(); return 0; diff --git a/ros/wavemap_ros/app/rosbag_processor.cc b/ros/wavemap_ros/app/rosbag_processor.cc index 2171f683c..c2322db5b 100644 --- a/ros/wavemap_ros/app/rosbag_processor.cc +++ b/ros/wavemap_ros/app/rosbag_processor.cc @@ -6,7 +6,7 @@ #include "wavemap_ros/inputs/depth_image_input.h" #include "wavemap_ros/inputs/pointcloud_input.h" -#include "wavemap_ros/wavemap_server.h" +#include "wavemap_ros/ros_server.h" using namespace wavemap; // NOLINT int main(int argc, char** argv) { @@ -21,7 +21,7 @@ int main(int argc, char** argv) { // Setup the wavemap server node ros::NodeHandle nh; ros::NodeHandle nh_private("~"); - WavemapServer wavemap_server(nh, nh_private); + RosServer wavemap_server(nh, nh_private); // Read the required ROS params std::string rosbag_paths_str; diff --git a/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h b/ros/wavemap_ros/include/wavemap_ros/ros_server.h similarity index 85% rename from ros/wavemap_ros/include/wavemap_ros/wavemap_server.h rename to ros/wavemap_ros/include/wavemap_ros/ros_server.h index 0d72b78e4..8f61465ea 100644 --- a/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h +++ b/ros/wavemap_ros/include/wavemap_ros/ros_server.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ROS_WAVEMAP_SERVER_H_ -#define WAVEMAP_ROS_WAVEMAP_SERVER_H_ +#ifndef WAVEMAP_ROS_ROS_SERVER_H_ +#define WAVEMAP_ROS_ROS_SERVER_H_ #include #include @@ -25,7 +25,7 @@ namespace wavemap { /** * Config struct for wavemap's ROS server. */ -struct WavemapServerConfig : ConfigBase { +struct RosServerConfig : ConfigBase { //! Name of the coordinate frame in which to store the map. //! Will be used as the frame_id for ROS TF lookups. std::string world_frame = "odom"; @@ -43,11 +43,11 @@ struct WavemapServerConfig : ConfigBase { bool isValid(bool verbose) const override; }; -class WavemapServer { +class RosServer { public: - WavemapServer(ros::NodeHandle nh, ros::NodeHandle nh_private); - WavemapServer(ros::NodeHandle nh, ros::NodeHandle nh_private, - const WavemapServerConfig& config); + RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private); + RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, + const RosServerConfig& config); InputBase* addInput(const param::Value& integrator_params, const ros::NodeHandle& nh, ros::NodeHandle nh_private); @@ -63,7 +63,7 @@ class WavemapServer { bool loadMap(const std::filesystem::path& file_path); private: - const WavemapServerConfig config_; + const RosServerConfig config_; // Map data structure MapBase::Ptr occupancy_map_; @@ -86,4 +86,4 @@ class WavemapServer { }; } // namespace wavemap -#endif // WAVEMAP_ROS_WAVEMAP_SERVER_H_ +#endif // WAVEMAP_ROS_ROS_SERVER_H_ diff --git a/ros/wavemap_ros/launch/rosbag_processor.launch b/ros/wavemap_ros/launch/rosbag_processor.launch index 682baac61..e5250d392 100644 --- a/ros/wavemap_ros/launch/rosbag_processor.launch +++ b/ros/wavemap_ros/launch/rosbag_processor.launch @@ -27,7 +27,7 @@ diff --git a/ros/wavemap_ros/launch/wavemap_server.launch b/ros/wavemap_ros/launch/wavemap_server.launch index 1c2173355..41f24cff7 100644 --- a/ros/wavemap_ros/launch/wavemap_server.launch +++ b/ros/wavemap_ros/launch/wavemap_server.launch @@ -35,7 +35,7 @@ diff --git a/ros/wavemap_ros/src/wavemap_server.cc b/ros/wavemap_ros/src/ros_server.cc similarity index 79% rename from ros/wavemap_ros/src/wavemap_server.cc rename to ros/wavemap_ros/src/ros_server.cc index 20c5da2c3..356a9e70a 100644 --- a/ros/wavemap_ros/src/wavemap_server.cc +++ b/ros/wavemap_ros/src/ros_server.cc @@ -1,4 +1,4 @@ -#include "wavemap_ros/wavemap_server.h" +#include "wavemap_ros/ros_server.h" #include #include @@ -11,13 +11,13 @@ #include "wavemap_ros/operations/operation_factory.h" namespace wavemap { -DECLARE_CONFIG_MEMBERS(WavemapServerConfig, +DECLARE_CONFIG_MEMBERS(RosServerConfig, (world_frame) (num_threads) (logging_level) (allow_reset_map_service)); -bool WavemapServerConfig::isValid(bool verbose) const { +bool RosServerConfig::isValid(bool verbose) const { bool all_valid = true; all_valid &= IS_PARAM_NE(world_frame, std::string(""), verbose); @@ -28,14 +28,14 @@ bool WavemapServerConfig::isValid(bool verbose) const { // NOTE: If WavemapServerConfig::from(...) fails, accessing its value will throw // an exception and end the program. -WavemapServer::WavemapServer(ros::NodeHandle nh, ros::NodeHandle nh_private) - : WavemapServer(nh, nh_private, - WavemapServerConfig::from( - param::convert::toParamValue(nh_private, "general")) - .value()) {} - -WavemapServer::WavemapServer(ros::NodeHandle nh, ros::NodeHandle nh_private, - const WavemapServerConfig& config) +RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private) + : RosServer(nh, nh_private, + RosServerConfig::from( + param::convert::toParamValue(nh_private, "general")) + .value()) {} + +RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, + const RosServerConfig& config) : config_(config.checkValid()), transformer_(std::make_shared()) { // Set the ROS logging level @@ -76,9 +76,9 @@ WavemapServer::WavemapServer(ros::NodeHandle nh, ros::NodeHandle nh_private, advertiseServices(nh_private); } -InputBase* WavemapServer::addInput(const param::Value& integrator_params, - const ros::NodeHandle& nh, - ros::NodeHandle nh_private) { +InputBase* RosServer::addInput(const param::Value& integrator_params, + const ros::NodeHandle& nh, + ros::NodeHandle nh_private) { auto input_handler = InputFactory::create( integrator_params, config_.world_frame, occupancy_map_, transformer_, thread_pool_, nh, nh_private, std::nullopt, @@ -89,8 +89,8 @@ InputBase* WavemapServer::addInput(const param::Value& integrator_params, return nullptr; } -OperationBase* WavemapServer::addOperation(const param::Value& operation_params, - ros::NodeHandle nh_private) { +OperationBase* RosServer::addOperation(const param::Value& operation_params, + ros::NodeHandle nh_private) { auto operation_handler = OperationFactory::create( operation_params, config_.world_frame, occupancy_map_, transformer_, thread_pool_, nh_private); @@ -100,14 +100,14 @@ OperationBase* WavemapServer::addOperation(const param::Value& operation_params, return nullptr; } -void WavemapServer::runOperations(const ros::Time& current_time, - bool force_run_all) { +void RosServer::runOperations(const ros::Time& current_time, + bool force_run_all) { for (auto& operation : operations_) { operation->run(current_time, force_run_all); } } -bool WavemapServer::saveMap(const std::filesystem::path& file_path) const { +bool RosServer::saveMap(const std::filesystem::path& file_path) const { if (occupancy_map_) { occupancy_map_->threshold(); return io::mapToFile(*occupancy_map_, file_path); @@ -117,11 +117,11 @@ bool WavemapServer::saveMap(const std::filesystem::path& file_path) const { return false; } -bool WavemapServer::loadMap(const std::filesystem::path& file_path) { +bool RosServer::loadMap(const std::filesystem::path& file_path) { return io::fileToMap(file_path, occupancy_map_); } -void WavemapServer::advertiseServices(ros::NodeHandle& nh_private) { +void RosServer::advertiseServices(ros::NodeHandle& nh_private) { reset_map_srv_ = nh_private.advertiseService( "reset_map", [this](auto& /*request*/, auto& response) { From 1d1ce9db3e52476e1e540288367a8503b4573852 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 28 Mar 2024 11:44:56 +0100 Subject: [PATCH 088/159] Move wavemap_io into regular wavemap library for simplicity --- CMakeLists.txt | 1 - examples/src/io/load_map_from_file.cc | 2 +- examples/src/io/save_map_to_file.cc | 2 +- examples/src/planning/occupancy_to_esdf.cc | 2 +- libraries/wavemap/CMakeLists.txt | 3 + .../include/wavemap/io}/file_conversions.h | 5 +- .../wavemap/io}/impl/streamable_types_impl.h | 0 .../include/wavemap/io}/stream_conversions.h | 15 +++-- .../include/wavemap/io}/streamable_types.h | 4 +- .../src/io}/file_conversions.cc | 2 +- .../src/io}/stream_conversions.cc | 2 +- .../test/src/io}/test_file_conversions.cc | 20 +++---- libraries/wavemap_io/CHANGELOG.rst | 55 ------------------- libraries/wavemap_io/CMakeLists.txt | 42 -------------- libraries/wavemap_io/include/CPPLINT.cfg | 1 - libraries/wavemap_io/package.xml | 18 ------ ros/wavemap_ros/package.xml | 1 - ros/wavemap_ros/src/ros_server.cc | 2 +- ros/wavemap_rviz_plugin/CMakeLists.txt | 3 +- ros/wavemap_rviz_plugin/package.xml | 1 - .../src/wavemap_map_display.cc | 2 +- 21 files changed, 33 insertions(+), 150 deletions(-) rename libraries/{wavemap_io/include/wavemap_io => wavemap/include/wavemap/io}/file_conversions.h (81%) rename libraries/{wavemap_io/include/wavemap_io => wavemap/include/wavemap/io}/impl/streamable_types_impl.h (100%) rename libraries/{wavemap_io/include/wavemap_io => wavemap/include/wavemap/io}/stream_conversions.h (73%) rename libraries/{wavemap_io/include/wavemap_io => wavemap/include/wavemap/io}/streamable_types.h (96%) rename libraries/{wavemap_io/src => wavemap/src/io}/file_conversions.cc (97%) rename libraries/{wavemap_io/src => wavemap/src/io}/stream_conversions.cc (99%) rename libraries/{wavemap_io/test/src => wavemap/test/src/io}/test_file_conversions.cc (94%) delete mode 100644 libraries/wavemap_io/CHANGELOG.rst delete mode 100644 libraries/wavemap_io/CMakeLists.txt delete mode 100644 libraries/wavemap_io/include/CPPLINT.cfg delete mode 100644 libraries/wavemap_io/package.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index bfba9f57e..55dc1dec3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,6 @@ include_directories(${CATKIN_WS_DEVEL_PATH}/include) # Libraries add_subdirectory(libraries/wavemap) -add_subdirectory(libraries/wavemap_io) # ROS interfaces and tooling add_subdirectory(ros/wavemap_msgs) diff --git a/examples/src/io/load_map_from_file.cc b/examples/src/io/load_map_from_file.cc index c3c8ca06f..b26e951cb 100644 --- a/examples/src/io/load_map_from_file.cc +++ b/examples/src/io/load_map_from_file.cc @@ -1,4 +1,4 @@ -#include +#include int main(int, char**) { // Create a smart pointer that will own the loaded map diff --git a/examples/src/io/save_map_to_file.cc b/examples/src/io/save_map_to_file.cc index 6276fd729..a686cec90 100644 --- a/examples/src/io/save_map_to_file.cc +++ b/examples/src/io/save_map_to_file.cc @@ -1,4 +1,4 @@ -#include +#include int main(int, char**) { // Create an empty map for illustration purposes diff --git a/examples/src/planning/occupancy_to_esdf.cc b/examples/src/planning/occupancy_to_esdf.cc index a2e1a2e3d..59a60a994 100644 --- a/examples/src/planning/occupancy_to_esdf.cc +++ b/examples/src/planning/occupancy_to_esdf.cc @@ -2,10 +2,10 @@ #include #include +#include #include #include #include -#include using namespace wavemap; // NOLINT int main(int argc, char** argv) { diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index 507550ea4..59e7430b7 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -57,6 +57,8 @@ add_library(${PROJECT_NAME} src/integrator/ray_tracing/ray_tracing_integrator.cc src/integrator/integrator_base.cc src/integrator/integrator_factory.cc + src/io/file_conversions.cc + src/io/stream_conversions.cc src/map/hashed_blocks.cc src/map/hashed_chunked_wavelet_octree.cc src/map/hashed_chunked_wavelet_octree_block.cc @@ -106,6 +108,7 @@ if (CATKIN_ENABLE_TESTING) test/src/integrator/test_measurement_models.cc test/src/integrator/test_pointcloud_integrators.cc test/src/integrator/test_range_image_intersector.cc + test/src/io/test_file_conversions.cc test/src/map/test_haar_cell.cc test/src/map/test_hashed_blocks.cc test/src/map/test_map.cc diff --git a/libraries/wavemap_io/include/wavemap_io/file_conversions.h b/libraries/wavemap/include/wavemap/io/file_conversions.h similarity index 81% rename from libraries/wavemap_io/include/wavemap_io/file_conversions.h rename to libraries/wavemap/include/wavemap/io/file_conversions.h index 6d1e81068..17bbd6e5b 100644 --- a/libraries/wavemap_io/include/wavemap_io/file_conversions.h +++ b/libraries/wavemap/include/wavemap/io/file_conversions.h @@ -3,9 +3,8 @@ #include -#include - -#include "wavemap_io/stream_conversions.h" +#include "wavemap/io/stream_conversions.h" +#include "wavemap/map/map_base.h" namespace wavemap::io { bool mapToFile(const MapBase& map, const std::filesystem::path& file_path); diff --git a/libraries/wavemap_io/include/wavemap_io/impl/streamable_types_impl.h b/libraries/wavemap/include/wavemap/io/impl/streamable_types_impl.h similarity index 100% rename from libraries/wavemap_io/include/wavemap_io/impl/streamable_types_impl.h rename to libraries/wavemap/include/wavemap/io/impl/streamable_types_impl.h diff --git a/libraries/wavemap_io/include/wavemap_io/stream_conversions.h b/libraries/wavemap/include/wavemap/io/stream_conversions.h similarity index 73% rename from libraries/wavemap_io/include/wavemap_io/stream_conversions.h rename to libraries/wavemap/include/wavemap/io/stream_conversions.h index 45d5dce3d..4cac52703 100644 --- a/libraries/wavemap_io/include/wavemap_io/stream_conversions.h +++ b/libraries/wavemap/include/wavemap/io/stream_conversions.h @@ -4,14 +4,13 @@ #include #include -#include -#include -#include -#include -#include -#include - -#include "wavemap_io/streamable_types.h" +#include "wavemap/common.h" +#include "wavemap/io/streamable_types.h" +#include "wavemap/map/cell_types/haar_coefficients.h" +#include "wavemap/map/hashed_blocks.h" +#include "wavemap/map/hashed_chunked_wavelet_octree.h" +#include "wavemap/map/hashed_wavelet_octree.h" +#include "wavemap/map/wavelet_octree.h" namespace wavemap::io { bool mapToStream(const MapBase& map, std::ostream& ostream); diff --git a/libraries/wavemap_io/include/wavemap_io/streamable_types.h b/libraries/wavemap/include/wavemap/io/streamable_types.h similarity index 96% rename from libraries/wavemap_io/include/wavemap_io/streamable_types.h rename to libraries/wavemap/include/wavemap/io/streamable_types.h index d542b5296..d0e978c49 100644 --- a/libraries/wavemap_io/include/wavemap_io/streamable_types.h +++ b/libraries/wavemap/include/wavemap/io/streamable_types.h @@ -4,6 +4,8 @@ #include #include +#include "wavemap/config/type_selector.h" + namespace wavemap::io::streamable { // NOTE: This file defines the serialization format for all types that might be // used in wavemap map files. The idea is that these are used as common @@ -97,6 +99,6 @@ struct StorageFormat : TypeSelector { }; } // namespace wavemap::io::streamable -#include "wavemap_io/impl/streamable_types_impl.h" +#include "wavemap/io/impl/streamable_types_impl.h" #endif // WAVEMAP_IO_STREAMABLE_TYPES_H_ diff --git a/libraries/wavemap_io/src/file_conversions.cc b/libraries/wavemap/src/io/file_conversions.cc similarity index 97% rename from libraries/wavemap_io/src/file_conversions.cc rename to libraries/wavemap/src/io/file_conversions.cc index df01defbc..0c8fa9aba 100644 --- a/libraries/wavemap_io/src/file_conversions.cc +++ b/libraries/wavemap/src/io/file_conversions.cc @@ -1,4 +1,4 @@ -#include "wavemap_io/file_conversions.h" +#include "wavemap/io/file_conversions.h" #include diff --git a/libraries/wavemap_io/src/stream_conversions.cc b/libraries/wavemap/src/io/stream_conversions.cc similarity index 99% rename from libraries/wavemap_io/src/stream_conversions.cc rename to libraries/wavemap/src/io/stream_conversions.cc index 6e3cd61f6..4edeab938 100644 --- a/libraries/wavemap_io/src/stream_conversions.cc +++ b/libraries/wavemap/src/io/stream_conversions.cc @@ -1,4 +1,4 @@ -#include "wavemap_io/stream_conversions.h" +#include "wavemap/io/stream_conversions.h" namespace wavemap::io { bool mapToStream(const MapBase& map, std::ostream& ostream) { diff --git a/libraries/wavemap_io/test/src/test_file_conversions.cc b/libraries/wavemap/test/src/io/test_file_conversions.cc similarity index 94% rename from libraries/wavemap_io/test/src/test_file_conversions.cc rename to libraries/wavemap/test/src/io/test_file_conversions.cc index 14329c670..f8c5b8564 100644 --- a/libraries/wavemap_io/test/src/test_file_conversions.cc +++ b/libraries/wavemap/test/src/io/test_file_conversions.cc @@ -1,14 +1,14 @@ #include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "wavemap_io/file_conversions.h" + +#include "wavemap/common.h" +#include "wavemap/io/file_conversions.h" +#include "wavemap/map/hashed_chunked_wavelet_octree.h" +#include "wavemap/map/hashed_wavelet_octree.h" +#include "wavemap/map/map_base.h" +#include "wavemap/map/wavelet_octree.h" +#include "wavemap/test/config_generator.h" +#include "wavemap/test/fixture_base.h" +#include "wavemap/test/geometry_generator.h" namespace wavemap { template diff --git a/libraries/wavemap_io/CHANGELOG.rst b/libraries/wavemap_io/CHANGELOG.rst deleted file mode 100644 index 2da93821e..000000000 --- a/libraries/wavemap_io/CHANGELOG.rst +++ /dev/null @@ -1,55 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package wavemap_io -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.6.3 (2023-12-21) ------------------- - -1.6.2 (2023-12-11) ------------------- - -1.6.1 (2023-11-20) ------------------- - -1.6.0 (2023-10-17) ------------------- -* Refactor wavemap utils -* Contributors: Victor Reijgwart - -1.5.3 (2023-09-28) ------------------- - -1.5.2 (2023-09-19) ------------------- - -1.5.1 (2023-09-08) ------------------- - -1.5.0 (2023-09-05) ------------------- -* Improve error messages when reading/writing a file fails -* Contributors: Victor Reijgwart - -1.4.0 (2023-08-30) ------------------- - -1.3.2 (2023-08-28) ------------------- - -1.3.1 (2023-08-22) ------------------- -* Release the code under the BSD-3 license - -1.3.0 (2023-08-17) ------------------- - -1.2.0 (2023-08-11) ------------------- - -1.1.0 (2023-08-09) ------------------- -* Implement a small serializer to convert maps to/from byte streams and files -* Contributors: Victor Reijgwart - -1.0.0 (2023-08-08) ------------------- diff --git a/libraries/wavemap_io/CMakeLists.txt b/libraries/wavemap_io/CMakeLists.txt deleted file mode 100644 index 3409dbddc..000000000 --- a/libraries/wavemap_io/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(wavemap_io) - -# Dependencies -find_package(catkin REQUIRED COMPONENTS wavemap) - -# Compiler definitions and options -add_wavemap_compile_definitions_and_options() - -# Setup catkin package -catkin_package(INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS) - -# For all targets -include_directories(include ${catkin_INCLUDE_DIRS}) - -# Libraries -# cmake-lint: disable=C0301 -add_library(${PROJECT_NAME} - src/file_conversions.cc - src/stream_conversions.cc) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) - -# Install targets -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -# Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" -) - -# Tests -if (CATKIN_ENABLE_TESTING) - catkin_add_gtest( - test_${PROJECT_NAME} - test/src/test_file_conversions.cc) - target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} gtest_main) -endif () diff --git a/libraries/wavemap_io/include/CPPLINT.cfg b/libraries/wavemap_io/include/CPPLINT.cfg deleted file mode 100644 index 2fce9d52b..000000000 --- a/libraries/wavemap_io/include/CPPLINT.cfg +++ /dev/null @@ -1 +0,0 @@ -root=. diff --git a/libraries/wavemap_io/package.xml b/libraries/wavemap_io/package.xml deleted file mode 100644 index e7c2c6ced..000000000 --- a/libraries/wavemap_io/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - wavemap_io - 1.6.3 - (De)serialization of wavemap types to files. - - Victor Reijgwart - BSD - https://github.com/ethz-asl/wavemap - - Victor Reijgwart - - catkin - - wavemap - - gtest - diff --git a/ros/wavemap_ros/package.xml b/ros/wavemap_ros/package.xml index 485ba1692..b66b234b5 100644 --- a/ros/wavemap_ros/package.xml +++ b/ros/wavemap_ros/package.xml @@ -14,7 +14,6 @@ catkin_simple wavemap - wavemap_io roscpp rosbag diff --git a/ros/wavemap_ros/src/ros_server.cc b/ros/wavemap_ros/src/ros_server.cc index 356a9e70a..2d983a31a 100644 --- a/ros/wavemap_ros/src/ros_server.cc +++ b/ros/wavemap_ros/src/ros_server.cc @@ -2,8 +2,8 @@ #include #include +#include #include -#include #include #include diff --git a/ros/wavemap_rviz_plugin/CMakeLists.txt b/ros/wavemap_rviz_plugin/CMakeLists.txt index 1aa4e1177..d51b37831 100644 --- a/ros/wavemap_rviz_plugin/CMakeLists.txt +++ b/ros/wavemap_rviz_plugin/CMakeLists.txt @@ -10,7 +10,6 @@ find_package(catkin REQUIRED COMPONENTS rviz wavemap - wavemap_io wavemap_msgs wavemap_ros_conversions tracy_catkin) @@ -22,7 +21,7 @@ add_compile_options(-Wno-pedantic -Wno-register) # Setup catkin package catkin_package(CATKIN_DEPENDS - wavemap wavemap_io wavemap_msgs wavemap_ros_conversions tracy_catkin) + wavemap wavemap_msgs wavemap_ros_conversions tracy_catkin) # For all targets include_directories(include SYSTEM ${catkin_INCLUDE_DIRS}) diff --git a/ros/wavemap_rviz_plugin/package.xml b/ros/wavemap_rviz_plugin/package.xml index 3d69cfa60..496125bf4 100644 --- a/ros/wavemap_rviz_plugin/package.xml +++ b/ros/wavemap_rviz_plugin/package.xml @@ -16,7 +16,6 @@ rviz wavemap - wavemap_io wavemap_msgs wavemap_ros_conversions tracy_catkin diff --git a/ros/wavemap_rviz_plugin/src/wavemap_map_display.cc b/ros/wavemap_rviz_plugin/src/wavemap_map_display.cc index a4b89bbcf..1753cc4e5 100644 --- a/ros/wavemap_rviz_plugin/src/wavemap_map_display.cc +++ b/ros/wavemap_rviz_plugin/src/wavemap_map_display.cc @@ -7,7 +7,7 @@ #include #include #include -#include +#include #include #include "wavemap_rviz_plugin/utils/alert_dialog.h" From 09b55b27305f34a4679ed8d804aedb84bdadcfb9 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 28 Mar 2024 13:35:06 +0100 Subject: [PATCH 089/159] Remove dependencies on catkinized gflags, glog and Eigen --- examples/package.xml | 1 - examples/src/planning/occupancy_to_esdf.cc | 1 - libraries/wavemap/CMakeLists.txt | 24 +- .../wavemap/cmake/Modules/FindGlog.cmake | 387 ++++++++++++++++++ libraries/wavemap/package.xml | 8 +- ros/wavemap_ros/CMakeLists.txt | 2 +- ros/wavemap_ros/app/ros_server.cc | 3 +- ros/wavemap_ros/app/rosbag_processor.cc | 2 +- ros/wavemap_ros/package.xml | 6 +- ros/wavemap_ros_conversions/package.xml | 4 +- ros/wavemap_rviz_plugin/CMakeLists.txt | 11 +- ros/wavemap_rviz_plugin/package.xml | 4 +- tooling/packages/wavemap_all/package.xml | 1 - tooling/vcstool/wavemap_https.yml | 12 - tooling/vcstool/wavemap_ssh.yml | 12 - 15 files changed, 420 insertions(+), 58 deletions(-) create mode 100644 libraries/wavemap/cmake/Modules/FindGlog.cmake diff --git a/examples/package.xml b/examples/package.xml index 6faffbe49..a5199a3d8 100644 --- a/examples/package.xml +++ b/examples/package.xml @@ -14,7 +14,6 @@ catkin_simple wavemap - wavemap_io wavemap_msgs wavemap_ros_conversions diff --git a/examples/src/planning/occupancy_to_esdf.cc b/examples/src/planning/occupancy_to_esdf.cc index 59a60a994..51aa8bb9d 100644 --- a/examples/src/planning/occupancy_to_esdf.cc +++ b/examples/src/planning/occupancy_to_esdf.cc @@ -11,7 +11,6 @@ using namespace wavemap; // NOLINT int main(int argc, char** argv) { // Initialize GLOG google::InitGoogleLogging(argv[0]); - google::ParseCommandLineFlags(&argc, &argv, false); google::InstallFailureSignalHandler(); FLAGS_alsologtostderr = true; FLAGS_colorlogtostderr = true; diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index 59e7430b7..0fb832986 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -2,17 +2,21 @@ cmake_minimum_required(VERSION 3.0.2) project(wavemap) # Dependencies -find_package(catkin - REQUIRED COMPONENTS glog_catkin eigen_catkin tracy_catkin minkindr) +find_package(catkin REQUIRED COMPONENTS tracy_catkin minkindr) if (ENABLE_BENCHMARKING) find_package(benchmark REQUIRED) endif () + # Compiler definitions and options include(cmake/wavemap-extras.cmake) add_wavemap_compile_definitions_and_options() +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake/Modules") +find_package(Glog REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) + # Setup catkin package catkin_package( INCLUDE_DIRS @@ -20,9 +24,8 @@ catkin_package( test/include LIBRARIES ${PROJECT_NAME} + glog::glog # TODO(victorr): See if there's a better way. CATKIN_DEPENDS - glog_catkin - eigen_catkin tracy_catkin minkindr CFG_EXTRAS @@ -75,19 +78,20 @@ add_library(${PROJECT_NAME} src/utils/sdf/quasi_euclidean_sdf_generator.cc src/utils/stopwatch.cc src/utils/thread_pool.cc) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} glog::glog Eigen3::Eigen) # Install targets install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) # Mark cpp header files for installation install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" ) # Tests diff --git a/libraries/wavemap/cmake/Modules/FindGlog.cmake b/libraries/wavemap/cmake/Modules/FindGlog.cmake new file mode 100644 index 000000000..d4e42db8b --- /dev/null +++ b/libraries/wavemap/cmake/Modules/FindGlog.cmake @@ -0,0 +1,387 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 2023 Google Inc. All rights reserved. +# http://ceres-solver.org/ +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Google Inc. nor the names of its contributors may be +# used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: alexs.mac@gmail.com (Alex Stewart) +# + +# FindGlog.cmake - Find Google glog logging library. +# +# This module defines the following variables: +# +# GLOG_FOUND: TRUE iff glog is found. +# GLOG_INCLUDE_DIRS: Include directories for glog. +# GLOG_LIBRARIES: Libraries required to link glog. +# FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION: True iff the version of glog found +# was built & installed / exported +# as a CMake package. +# +# The following variables control the behaviour of this module: +# +# GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then +# then prefer using an exported CMake configuration +# generated by glog > 0.3.4 over searching for the +# glog components manually. Otherwise (FALSE) +# ignore any exported glog CMake configurations and +# always perform a manual search for the components. +# Default: TRUE iff user does not define this variable +# before we are called, and does NOT specify either +# GLOG_INCLUDE_DIR_HINTS or GLOG_LIBRARY_DIR_HINTS +# otherwise FALSE. +# GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to +# search for glog includes, e.g: /timbuktu/include. +# GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to +# search for glog libraries, e.g: /timbuktu/lib. +# +# The following variables are also defined by this module, but in line with +# CMake recommended FindPackage() module style should NOT be referenced directly +# by callers (use the plural variables detailed above instead). These variables +# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which +# are NOT re-called (i.e. search for library is not repeated) if these variables +# are set with valid values _in the CMake cache_. This means that if these +# variables are set directly in the cache, either by the user in the CMake GUI, +# or by the user passing -DVAR=VALUE directives to CMake when called (which +# explicitly defines a cache variable), then they will be used verbatim, +# bypassing the HINTS variables and other hard-coded search locations. +# +# GLOG_INCLUDE_DIR: Include directory for glog, not including the +# include directory of any dependencies. +# GLOG_LIBRARY: glog library, not including the libraries of any +# dependencies. + +# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when +# FindGlog was invoked. +macro(GLOG_RESET_FIND_LIBRARY_PREFIX) + if (MSVC AND CALLERS_CMAKE_FIND_LIBRARY_PREFIXES) + set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") + endif () +endmacro(GLOG_RESET_FIND_LIBRARY_PREFIX) + +# Called if we failed to find glog or any of it's required dependencies, +# unsets all public (designed to be used externally) variables and reports +# error message at priority depending upon [REQUIRED/QUIET/] argument. +# cmake-lint: disable=C0103 +macro(GLOG_REPORT_NOT_FOUND REASON_MSG) + unset(GLOG_FOUND) + unset(GLOG_INCLUDE_DIRS) + unset(GLOG_LIBRARIES) + # Make results of search visible in the CMake GUI if glog has not + # been found so that user does not have to toggle to advanced view. + mark_as_advanced(CLEAR GLOG_INCLUDE_DIR + GLOG_LIBRARY) + + glog_reset_find_library_prefix() + + # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() + # use the camelcase library name, not uppercase. + if (Glog_FIND_QUIETLY) + message(STATUS "Failed to find glog - " ${REASON_MSG} ${ARGN}) + elseif (Glog_FIND_REQUIRED) + message(FATAL_ERROR "Failed to find glog - " ${REASON_MSG} ${ARGN}) + else() + # Neither QUIETLY nor REQUIRED, use no priority which emits a message + # but continues configuration and allows generation. + message("-- Failed to find glog - " ${REASON_MSG} ${ARGN}) + endif () + return() +endmacro(GLOG_REPORT_NOT_FOUND) + +# glog_message([mode] "message text") +# +# Wraps the standard cmake 'message' command, but suppresses output +# if the QUIET flag was passed to the find_package(Glog ...) call. +function(GLOG_MESSAGE) + if (NOT Glog_FIND_QUIETLY) + message(${ARGN}) + endif () +endfunction() + +# Protect against any alternative find_package scripts for this library having +# been called previously (in a client project) which set GLOG_FOUND, but not +# the other variables we require / set here which could cause the search logic +# here to fail. +unset(GLOG_FOUND) + +# ----------------------------------------------------------------- +# By default, if the user has expressed no preference for using an exported +# glog CMake configuration over performing a search for the installed +# components, and has not specified any hints for the search locations, then +# prefer a glog exported configuration if available. +if (NOT DEFINED GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION + AND NOT GLOG_INCLUDE_DIR_HINTS + AND NOT GLOG_LIBRARY_DIR_HINTS) + glog_message(STATUS + "No preference for use of exported glog CMake configuration " + "set, and no hints for include/library directories provided. " + "Defaulting to preferring an installed/exported glog CMake configuration " + "if available.") + set(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION TRUE) +endif () + +# On macOS, add the Homebrew prefix (with appropriate suffixes) to the +# respective HINTS directories (after any user-specified locations). This +# handles Homebrew installations into non-standard locations (not /usr/local). +# We do not use CMAKE_PREFIX_PATH for this as given the search ordering of +# find_xxx(), doing so would override any user-specified HINTS locations with +# the Homebrew version if it exists. +if (CMAKE_SYSTEM_NAME MATCHES "Darwin") + find_program(HOMEBREW_EXECUTABLE brew) + mark_as_advanced(FORCE HOMEBREW_EXECUTABLE) + if (HOMEBREW_EXECUTABLE) + # Detected a Homebrew install, query for its install prefix. + execute_process(COMMAND ${HOMEBREW_EXECUTABLE} --prefix + OUTPUT_VARIABLE HOMEBREW_INSTALL_PREFIX + OUTPUT_STRIP_TRAILING_WHITESPACE) + glog_message(STATUS "Detected Homebrew with install prefix: " + "${HOMEBREW_INSTALL_PREFIX}, adding to CMake search paths.") + list(APPEND GLOG_INCLUDE_DIR_HINTS "${HOMEBREW_INSTALL_PREFIX}/include") + list(APPEND GLOG_LIBRARY_DIR_HINTS "${HOMEBREW_INSTALL_PREFIX}/lib") + endif () +endif () + +if (GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION) + # Try to find an exported CMake configuration for glog, as generated by + # glog versions > 0.3.4 + # + # We search twice, s/t we can invert the ordering of precedence used by + # find_package() for exported package build directories, and installed + # packages (found via CMAKE_SYSTEM_PREFIX_PATH), listed as items 6) and 7) + # respectively in [1]. + # + # By default, exported build directories are (in theory) detected first, and + # this is usually the case on Windows. However, on OS X & Linux, the install + # path (/usr/local) is typically present in the PATH environment variable + # which is checked in item 4) in [1] (i.e. before both of the above, unless + # NO_SYSTEM_ENVIRONMENT_PATH is passed). As such on those OSs installed + # packages are usually detected in preference to exported package build + # directories. + # + # To ensure a more consistent response across all OSs, and as users usually + # want to prefer an installed version of a package over a locally built one + # where both exist (esp. as the exported build directory might be removed + # after installation), we first search with NO_CMAKE_PACKAGE_REGISTRY which + # means any build directories exported by the user are ignored, and thus + # installed directories are preferred. If this fails to find the package + # we then research again, but without NO_CMAKE_PACKAGE_REGISTRY, so any + # exported build directories will now be detected. + # + # To prevent confusion on Windows, we also pass NO_CMAKE_BUILDS_PATH (which + # is item 5) in [1]), to not preferentially use projects that were built + # recently with the CMake GUI to ensure that we always prefer an installed + # version if available. + # + # NOTE: We use the NAMES option as glog erroneously uses 'google-glog' as its + # project name when built with CMake, but exports itself as just 'glog'. + # On Linux/OS X this does not break detection as the project name is + # not used as part of the install path for the CMake package files, + # e.g. /usr/local/lib/cmake/glog, where the suffix is hardcoded + # in glog's CMakeLists. However, on Windows the project name *is* + # part of the install prefix: + # C:/Program Files/google-glog/[include,lib] + # However, by default CMake checks: + # C:/Program Files/ which does not + # exist and thus detection fails. Thus we use the NAMES to force the + # search to use both google-glog & glog. + # + # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package + find_package(glog QUIET + NAMES google-glog glog + HINTS ${glog_DIR} ${HOMEBREW_INSTALL_PREFIX} + NO_MODULE + NO_CMAKE_PACKAGE_REGISTRY + NO_CMAKE_BUILDS_PATH) + if (glog_FOUND) + glog_message(STATUS "Found installed version of glog: ${glog_DIR}") + else () + # Failed to find an installed version of glog, repeat search allowing + # exported build directories. + glog_message(STATUS "Failed to find installed glog CMake configuration, " + "searching for glog build directories exported with CMake.") + # Again pass NO_CMAKE_BUILDS_PATH, as we know that glog is exported and + # do not want to treat projects built with the CMake GUI preferentially. + find_package(glog QUIET + NAMES google-glog glog + NO_MODULE + NO_CMAKE_BUILDS_PATH) + if (glog_FOUND) + glog_message(STATUS "Found exported glog build directory: ${glog_DIR}") + endif (glog_FOUND) + endif (glog_FOUND) + + set(FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION ${glog_FOUND}) + + if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) + glog_message(STATUS "Detected glog version: ${glog_VERSION}") + set(GLOG_FOUND ${glog_FOUND}) + # glog wraps the include directories into the exported glog::glog target. + set(GLOG_INCLUDE_DIR "") + set(GLOG_LIBRARY glog::glog) + else (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) + glog_message(STATUS "Failed to find an installed/exported CMake " + "configuration for glog, will perform search for installed glog " + "components.") + endif (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) +endif (GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION) + +if (NOT GLOG_FOUND) + # Either failed to find an exported glog CMake configuration, or user + # told us not to use one. Perform a manual search for all glog components. + + # Handle possible presence of lib prefix for libraries on MSVC, see + # also GLOG_RESET_FIND_LIBRARY_PREFIX(). + if (MSVC) + # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES + # s/t we can set it back before returning. + set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") + # The empty string in this list is important, it represents the case when + # the libraries have no prefix (shared libraries / DLLs). + set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") + endif (MSVC) + + # Search user-installed locations first, so that we prefer user installs + # to system installs where both exist. + list(APPEND GLOG_CHECK_INCLUDE_DIRS + /usr/local/include + /usr/local/homebrew/include # Mac OS X + /opt/local/var/macports/software # Mac OS X. + /opt/local/include + /usr/include) + # Windows (for C:/Program Files prefix). + list(APPEND GLOG_CHECK_PATH_SUFFIXES + glog/include + glog/Include + Glog/include + Glog/Include + google-glog/include # CMake installs with project name prefix. + google-glog/Include) + + list(APPEND GLOG_CHECK_LIBRARY_DIRS + /usr/local/lib + /usr/local/homebrew/lib # Mac OS X. + /opt/local/lib + /usr/lib) + # Windows (for C:/Program Files prefix). + list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES + glog/lib + glog/Lib + Glog/lib + Glog/Lib + google-glog/lib # CMake installs with project name prefix. + google-glog/Lib) + + # Search supplied hint directories first if supplied. + find_path(GLOG_INCLUDE_DIR + NAMES glog/logging.h + HINTS ${GLOG_INCLUDE_DIR_HINTS} + PATHS ${GLOG_CHECK_INCLUDE_DIRS} + PATH_SUFFIXES ${GLOG_CHECK_PATH_SUFFIXES}) + if (NOT GLOG_INCLUDE_DIR OR + NOT EXISTS ${GLOG_INCLUDE_DIR}) + glog_report_not_found( + "Could not find glog include directory, set GLOG_INCLUDE_DIR " + "to directory containing glog/logging.h") + endif (NOT GLOG_INCLUDE_DIR OR + NOT EXISTS ${GLOG_INCLUDE_DIR}) + + find_library(GLOG_LIBRARY NAMES glog + HINTS ${GLOG_LIBRARY_DIR_HINTS} + PATHS ${GLOG_CHECK_LIBRARY_DIRS} + PATH_SUFFIXES ${GLOG_CHECK_LIBRARY_SUFFIXES}) + if (NOT GLOG_LIBRARY OR + NOT EXISTS ${GLOG_LIBRARY}) + glog_report_not_found( + "Could not find glog library, set GLOG_LIBRARY " + "to full path to libglog.") + endif (NOT GLOG_LIBRARY OR + NOT EXISTS ${GLOG_LIBRARY}) + + # Mark internally as found, then verify. GLOG_REPORT_NOT_FOUND() unsets + # if called. + set(GLOG_FOUND TRUE) + + # Glog does not seem to provide any record of the version in its + # source tree, thus cannot extract version. + + # Catch case when caller has set GLOG_INCLUDE_DIR in the cache / GUI and + # thus FIND_[PATH/LIBRARY] are not called, but specified locations are + # invalid, otherwise we would report the library as found. + if (GLOG_INCLUDE_DIR AND + NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) + glog_report_not_found( + "Caller defined GLOG_INCLUDE_DIR:" + " ${GLOG_INCLUDE_DIR} does not contain glog/logging.h header.") + endif (GLOG_INCLUDE_DIR AND + NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) + # TODO: This regex for glog library is pretty primitive, we use lowercase + # for comparison to handle Windows using CamelCase library names, could + # this check be better? + string(TOLOWER "${GLOG_LIBRARY}" LOWERCASE_GLOG_LIBRARY) + if (GLOG_LIBRARY AND + NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") + glog_report_not_found( + "Caller defined GLOG_LIBRARY: " + "${GLOG_LIBRARY} does not match glog.") + endif (GLOG_LIBRARY AND + NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") + + # add glog::glog target + add_library(glog::glog INTERFACE IMPORTED) + target_include_directories(glog::glog INTERFACE ${GLOG_INCLUDE_DIRS}) + target_link_libraries(glog::glog INTERFACE ${GLOG_LIBRARY}) + + glog_reset_find_library_prefix() + +endif (NOT GLOG_FOUND) + +# Set standard CMake FindPackage variables if found. +if (GLOG_FOUND) + set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) + set(GLOG_LIBRARIES ${GLOG_LIBRARY}) +endif (GLOG_FOUND) + +# If we are using an exported CMake glog target, the include directories are +# wrapped into the target itself, and do not have to be (and are not) +# separately specified. In which case, we should not add GLOG_INCLUDE_DIRS +# to the list of required variables in order that glog be reported as found. +if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) + set(GLOG_REQUIRED_VARIABLES GLOG_LIBRARIES) +else () + set(GLOG_REQUIRED_VARIABLES GLOG_INCLUDE_DIRS GLOG_LIBRARIES) +endif () + +# Handle REQUIRED / QUIET optional arguments. +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Glog DEFAULT_MSG + ${GLOG_REQUIRED_VARIABLES}) + +# Only mark internal variables as advanced if we found glog, otherwise +# leave them visible in the standard GUI for the user to set manually. +if (GLOG_FOUND) + mark_as_advanced(FORCE GLOG_INCLUDE_DIR + GLOG_LIBRARY + glog_DIR) # Autogenerated by find_package(glog) +endif (GLOG_FOUND) diff --git a/libraries/wavemap/package.xml b/libraries/wavemap/package.xml index 602a28c1e..9bbab6797 100644 --- a/libraries/wavemap/package.xml +++ b/libraries/wavemap/package.xml @@ -12,10 +12,12 @@ catkin - glog_catkin - eigen_catkin - tracy_catkin + libgoogle-glog-dev + eigen + minkindr + tracy_catkin + gtest diff --git a/ros/wavemap_ros/CMakeLists.txt b/ros/wavemap_ros/CMakeLists.txt index 1bb1101dc..ac317feab 100644 --- a/ros/wavemap_ros/CMakeLists.txt +++ b/ros/wavemap_ros/CMakeLists.txt @@ -49,5 +49,5 @@ cs_export() # Export config files install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/config/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config ) diff --git a/ros/wavemap_ros/app/ros_server.cc b/ros/wavemap_ros/app/ros_server.cc index 89ed24ffe..9c145d615 100644 --- a/ros/wavemap_ros/app/ros_server.cc +++ b/ros/wavemap_ros/app/ros_server.cc @@ -1,12 +1,11 @@ #include "wavemap_ros/ros_server.h" -#include +#include int main(int argc, char** argv) { ros::init(argc, argv, "wavemap_ros_server"); google::InitGoogleLogging(argv[0]); - google::ParseCommandLineFlags(&argc, &argv, false); google::InstallFailureSignalHandler(); FLAGS_alsologtostderr = true; FLAGS_colorlogtostderr = true; diff --git a/ros/wavemap_ros/app/rosbag_processor.cc b/ros/wavemap_ros/app/rosbag_processor.cc index c2322db5b..958327ed2 100644 --- a/ros/wavemap_ros/app/rosbag_processor.cc +++ b/ros/wavemap_ros/app/rosbag_processor.cc @@ -1,5 +1,6 @@ #include "wavemap_ros/utils/rosbag_processor.h" +#include #include #include #include @@ -13,7 +14,6 @@ int main(int argc, char** argv) { ros::init(argc, argv, "wavemap_rosbag_processor"); google::InitGoogleLogging(argv[0]); - google::ParseCommandLineFlags(&argc, &argv, false); google::InstallFailureSignalHandler(); FLAGS_alsologtostderr = true; FLAGS_colorlogtostderr = true; diff --git a/ros/wavemap_ros/package.xml b/ros/wavemap_ros/package.xml index b66b234b5..6368c1b9e 100644 --- a/ros/wavemap_ros/package.xml +++ b/ros/wavemap_ros/package.xml @@ -14,18 +14,16 @@ catkin_simple wavemap + wavemap_msgs + wavemap_ros_conversions roscpp rosbag cv_bridge image_transport tf2_ros - tracy_catkin - wavemap_msgs std_srvs sensor_msgs visualization_msgs - - wavemap_ros_conversions diff --git a/ros/wavemap_ros_conversions/package.xml b/ros/wavemap_ros_conversions/package.xml index 2fb558147..0e168a32d 100644 --- a/ros/wavemap_ros_conversions/package.xml +++ b/ros/wavemap_ros_conversions/package.xml @@ -13,9 +13,9 @@ catkin catkin_simple - roscpp wavemap - tracy_catkin wavemap_msgs + + roscpp eigen_conversions diff --git a/ros/wavemap_rviz_plugin/CMakeLists.txt b/ros/wavemap_rviz_plugin/CMakeLists.txt index d51b37831..49c70ee7c 100644 --- a/ros/wavemap_rviz_plugin/CMakeLists.txt +++ b/ros/wavemap_rviz_plugin/CMakeLists.txt @@ -8,11 +8,10 @@ project(wavemap_rviz_plugin) # Dependencies find_package(catkin REQUIRED COMPONENTS - rviz - wavemap - wavemap_msgs - wavemap_ros_conversions - tracy_catkin) + rviz + wavemap + wavemap_msgs + wavemap_ros_conversions) # Compiler definitions and options add_wavemap_compile_definitions_and_options() @@ -21,7 +20,7 @@ add_compile_options(-Wno-pedantic -Wno-register) # Setup catkin package catkin_package(CATKIN_DEPENDS - wavemap wavemap_msgs wavemap_ros_conversions tracy_catkin) + wavemap wavemap_msgs wavemap_ros_conversions) # For all targets include_directories(include SYSTEM ${catkin_INCLUDE_DIRS}) diff --git a/ros/wavemap_rviz_plugin/package.xml b/ros/wavemap_rviz_plugin/package.xml index 496125bf4..dc3ac114e 100644 --- a/ros/wavemap_rviz_plugin/package.xml +++ b/ros/wavemap_rviz_plugin/package.xml @@ -14,11 +14,11 @@ catkin - rviz wavemap wavemap_msgs wavemap_ros_conversions - tracy_catkin + + rviz qtbase5-dev diff --git a/tooling/packages/wavemap_all/package.xml b/tooling/packages/wavemap_all/package.xml index 680fb4d69..3659fd78f 100644 --- a/tooling/packages/wavemap_all/package.xml +++ b/tooling/packages/wavemap_all/package.xml @@ -13,7 +13,6 @@ catkin wavemap - wavemap_io wavemap_msgs wavemap_ros_conversions wavemap_ros diff --git a/tooling/vcstool/wavemap_https.yml b/tooling/vcstool/wavemap_https.yml index 70132a994..e9be4aa35 100644 --- a/tooling/vcstool/wavemap_https.yml +++ b/tooling/vcstool/wavemap_https.yml @@ -3,22 +3,10 @@ repositories: type: git url: https://github.com/catkin/catkin_simple.git version: master - eigen_catkin: - type: git - url: https://github.com/ethz-asl/eigen_catkin.git - version: master eigen_checks: type: git url: https://github.com/ethz-asl/eigen_checks.git version: master - gflags_catkin: - type: git - url: https://github.com/ethz-asl/gflags_catkin.git - version: master - glog_catkin: - type: git - url: https://github.com/ethz-asl/glog_catkin.git - version: master tracy_catkin: type: git url: https://github.com/ethz-asl/tracy_catkin.git diff --git a/tooling/vcstool/wavemap_ssh.yml b/tooling/vcstool/wavemap_ssh.yml index 8fc124be7..adb60244b 100644 --- a/tooling/vcstool/wavemap_ssh.yml +++ b/tooling/vcstool/wavemap_ssh.yml @@ -3,22 +3,10 @@ repositories: type: git url: git@github.com:catkin/catkin_simple.git version: master - eigen_catkin: - type: git - url: git@github.com:ethz-asl/eigen_catkin.git - version: master eigen_checks: type: git url: git@github.com:ethz-asl/eigen_checks.git version: master - gflags_catkin: - type: git - url: git@github.com:ethz-asl/gflags_catkin.git - version: master - glog_catkin: - type: git - url: git@github.com:ethz-asl/glog_catkin.git - version: master tracy_catkin: type: git url: git@github.com:ethz-asl/tracy_catkin.git From a647ada54fc7207a24478888cad0626b22b435b8 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 28 Mar 2024 17:37:55 +0100 Subject: [PATCH 090/159] Find glog using its included pkgconfig instead of defining our own --- libraries/wavemap/CMakeLists.txt | 15 +- .../wavemap/cmake/Modules/FindGlog.cmake | 387 ------------------ tooling/vcstool/wavemap_https.yml | 4 - tooling/vcstool/wavemap_ssh.yml | 4 - 4 files changed, 7 insertions(+), 403 deletions(-) delete mode 100644 libraries/wavemap/cmake/Modules/FindGlog.cmake diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index 0fb832986..289e6395c 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -3,20 +3,18 @@ project(wavemap) # Dependencies find_package(catkin REQUIRED COMPONENTS tracy_catkin minkindr) +find_package(Eigen3 REQUIRED NO_MODULE) +find_package(PkgConfig REQUIRED) +pkg_check_modules(glog REQUIRED libglog) if (ENABLE_BENCHMARKING) find_package(benchmark REQUIRED) endif () - # Compiler definitions and options include(cmake/wavemap-extras.cmake) add_wavemap_compile_definitions_and_options() -list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake/Modules") -find_package(Glog REQUIRED) -find_package(Eigen3 REQUIRED NO_MODULE) - # Setup catkin package catkin_package( INCLUDE_DIRS @@ -24,7 +22,8 @@ catkin_package( test/include LIBRARIES ${PROJECT_NAME} - glog::glog # TODO(victorr): See if there's a better way. + DEPENDS + glog CATKIN_DEPENDS tracy_catkin minkindr @@ -78,8 +77,8 @@ add_library(${PROJECT_NAME} src/utils/sdf/quasi_euclidean_sdf_generator.cc src/utils/stopwatch.cc src/utils/thread_pool.cc) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} glog::glog Eigen3::Eigen) +target_include_directories(${PROJECT_NAME} PUBLIC ${glog_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen ${glog_LIBRARIES}) # Install targets install(TARGETS ${PROJECT_NAME} diff --git a/libraries/wavemap/cmake/Modules/FindGlog.cmake b/libraries/wavemap/cmake/Modules/FindGlog.cmake deleted file mode 100644 index d4e42db8b..000000000 --- a/libraries/wavemap/cmake/Modules/FindGlog.cmake +++ /dev/null @@ -1,387 +0,0 @@ -# Ceres Solver - A fast non-linear least squares minimizer -# Copyright 2023 Google Inc. All rights reserved. -# http://ceres-solver.org/ -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# * Neither the name of Google Inc. nor the names of its contributors may be -# used to endorse or promote products derived from this software without -# specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: alexs.mac@gmail.com (Alex Stewart) -# - -# FindGlog.cmake - Find Google glog logging library. -# -# This module defines the following variables: -# -# GLOG_FOUND: TRUE iff glog is found. -# GLOG_INCLUDE_DIRS: Include directories for glog. -# GLOG_LIBRARIES: Libraries required to link glog. -# FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION: True iff the version of glog found -# was built & installed / exported -# as a CMake package. -# -# The following variables control the behaviour of this module: -# -# GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then -# then prefer using an exported CMake configuration -# generated by glog > 0.3.4 over searching for the -# glog components manually. Otherwise (FALSE) -# ignore any exported glog CMake configurations and -# always perform a manual search for the components. -# Default: TRUE iff user does not define this variable -# before we are called, and does NOT specify either -# GLOG_INCLUDE_DIR_HINTS or GLOG_LIBRARY_DIR_HINTS -# otherwise FALSE. -# GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to -# search for glog includes, e.g: /timbuktu/include. -# GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to -# search for glog libraries, e.g: /timbuktu/lib. -# -# The following variables are also defined by this module, but in line with -# CMake recommended FindPackage() module style should NOT be referenced directly -# by callers (use the plural variables detailed above instead). These variables -# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which -# are NOT re-called (i.e. search for library is not repeated) if these variables -# are set with valid values _in the CMake cache_. This means that if these -# variables are set directly in the cache, either by the user in the CMake GUI, -# or by the user passing -DVAR=VALUE directives to CMake when called (which -# explicitly defines a cache variable), then they will be used verbatim, -# bypassing the HINTS variables and other hard-coded search locations. -# -# GLOG_INCLUDE_DIR: Include directory for glog, not including the -# include directory of any dependencies. -# GLOG_LIBRARY: glog library, not including the libraries of any -# dependencies. - -# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when -# FindGlog was invoked. -macro(GLOG_RESET_FIND_LIBRARY_PREFIX) - if (MSVC AND CALLERS_CMAKE_FIND_LIBRARY_PREFIXES) - set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") - endif () -endmacro(GLOG_RESET_FIND_LIBRARY_PREFIX) - -# Called if we failed to find glog or any of it's required dependencies, -# unsets all public (designed to be used externally) variables and reports -# error message at priority depending upon [REQUIRED/QUIET/] argument. -# cmake-lint: disable=C0103 -macro(GLOG_REPORT_NOT_FOUND REASON_MSG) - unset(GLOG_FOUND) - unset(GLOG_INCLUDE_DIRS) - unset(GLOG_LIBRARIES) - # Make results of search visible in the CMake GUI if glog has not - # been found so that user does not have to toggle to advanced view. - mark_as_advanced(CLEAR GLOG_INCLUDE_DIR - GLOG_LIBRARY) - - glog_reset_find_library_prefix() - - # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() - # use the camelcase library name, not uppercase. - if (Glog_FIND_QUIETLY) - message(STATUS "Failed to find glog - " ${REASON_MSG} ${ARGN}) - elseif (Glog_FIND_REQUIRED) - message(FATAL_ERROR "Failed to find glog - " ${REASON_MSG} ${ARGN}) - else() - # Neither QUIETLY nor REQUIRED, use no priority which emits a message - # but continues configuration and allows generation. - message("-- Failed to find glog - " ${REASON_MSG} ${ARGN}) - endif () - return() -endmacro(GLOG_REPORT_NOT_FOUND) - -# glog_message([mode] "message text") -# -# Wraps the standard cmake 'message' command, but suppresses output -# if the QUIET flag was passed to the find_package(Glog ...) call. -function(GLOG_MESSAGE) - if (NOT Glog_FIND_QUIETLY) - message(${ARGN}) - endif () -endfunction() - -# Protect against any alternative find_package scripts for this library having -# been called previously (in a client project) which set GLOG_FOUND, but not -# the other variables we require / set here which could cause the search logic -# here to fail. -unset(GLOG_FOUND) - -# ----------------------------------------------------------------- -# By default, if the user has expressed no preference for using an exported -# glog CMake configuration over performing a search for the installed -# components, and has not specified any hints for the search locations, then -# prefer a glog exported configuration if available. -if (NOT DEFINED GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION - AND NOT GLOG_INCLUDE_DIR_HINTS - AND NOT GLOG_LIBRARY_DIR_HINTS) - glog_message(STATUS - "No preference for use of exported glog CMake configuration " - "set, and no hints for include/library directories provided. " - "Defaulting to preferring an installed/exported glog CMake configuration " - "if available.") - set(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION TRUE) -endif () - -# On macOS, add the Homebrew prefix (with appropriate suffixes) to the -# respective HINTS directories (after any user-specified locations). This -# handles Homebrew installations into non-standard locations (not /usr/local). -# We do not use CMAKE_PREFIX_PATH for this as given the search ordering of -# find_xxx(), doing so would override any user-specified HINTS locations with -# the Homebrew version if it exists. -if (CMAKE_SYSTEM_NAME MATCHES "Darwin") - find_program(HOMEBREW_EXECUTABLE brew) - mark_as_advanced(FORCE HOMEBREW_EXECUTABLE) - if (HOMEBREW_EXECUTABLE) - # Detected a Homebrew install, query for its install prefix. - execute_process(COMMAND ${HOMEBREW_EXECUTABLE} --prefix - OUTPUT_VARIABLE HOMEBREW_INSTALL_PREFIX - OUTPUT_STRIP_TRAILING_WHITESPACE) - glog_message(STATUS "Detected Homebrew with install prefix: " - "${HOMEBREW_INSTALL_PREFIX}, adding to CMake search paths.") - list(APPEND GLOG_INCLUDE_DIR_HINTS "${HOMEBREW_INSTALL_PREFIX}/include") - list(APPEND GLOG_LIBRARY_DIR_HINTS "${HOMEBREW_INSTALL_PREFIX}/lib") - endif () -endif () - -if (GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION) - # Try to find an exported CMake configuration for glog, as generated by - # glog versions > 0.3.4 - # - # We search twice, s/t we can invert the ordering of precedence used by - # find_package() for exported package build directories, and installed - # packages (found via CMAKE_SYSTEM_PREFIX_PATH), listed as items 6) and 7) - # respectively in [1]. - # - # By default, exported build directories are (in theory) detected first, and - # this is usually the case on Windows. However, on OS X & Linux, the install - # path (/usr/local) is typically present in the PATH environment variable - # which is checked in item 4) in [1] (i.e. before both of the above, unless - # NO_SYSTEM_ENVIRONMENT_PATH is passed). As such on those OSs installed - # packages are usually detected in preference to exported package build - # directories. - # - # To ensure a more consistent response across all OSs, and as users usually - # want to prefer an installed version of a package over a locally built one - # where both exist (esp. as the exported build directory might be removed - # after installation), we first search with NO_CMAKE_PACKAGE_REGISTRY which - # means any build directories exported by the user are ignored, and thus - # installed directories are preferred. If this fails to find the package - # we then research again, but without NO_CMAKE_PACKAGE_REGISTRY, so any - # exported build directories will now be detected. - # - # To prevent confusion on Windows, we also pass NO_CMAKE_BUILDS_PATH (which - # is item 5) in [1]), to not preferentially use projects that were built - # recently with the CMake GUI to ensure that we always prefer an installed - # version if available. - # - # NOTE: We use the NAMES option as glog erroneously uses 'google-glog' as its - # project name when built with CMake, but exports itself as just 'glog'. - # On Linux/OS X this does not break detection as the project name is - # not used as part of the install path for the CMake package files, - # e.g. /usr/local/lib/cmake/glog, where the suffix is hardcoded - # in glog's CMakeLists. However, on Windows the project name *is* - # part of the install prefix: - # C:/Program Files/google-glog/[include,lib] - # However, by default CMake checks: - # C:/Program Files/ which does not - # exist and thus detection fails. Thus we use the NAMES to force the - # search to use both google-glog & glog. - # - # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package - find_package(glog QUIET - NAMES google-glog glog - HINTS ${glog_DIR} ${HOMEBREW_INSTALL_PREFIX} - NO_MODULE - NO_CMAKE_PACKAGE_REGISTRY - NO_CMAKE_BUILDS_PATH) - if (glog_FOUND) - glog_message(STATUS "Found installed version of glog: ${glog_DIR}") - else () - # Failed to find an installed version of glog, repeat search allowing - # exported build directories. - glog_message(STATUS "Failed to find installed glog CMake configuration, " - "searching for glog build directories exported with CMake.") - # Again pass NO_CMAKE_BUILDS_PATH, as we know that glog is exported and - # do not want to treat projects built with the CMake GUI preferentially. - find_package(glog QUIET - NAMES google-glog glog - NO_MODULE - NO_CMAKE_BUILDS_PATH) - if (glog_FOUND) - glog_message(STATUS "Found exported glog build directory: ${glog_DIR}") - endif (glog_FOUND) - endif (glog_FOUND) - - set(FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION ${glog_FOUND}) - - if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) - glog_message(STATUS "Detected glog version: ${glog_VERSION}") - set(GLOG_FOUND ${glog_FOUND}) - # glog wraps the include directories into the exported glog::glog target. - set(GLOG_INCLUDE_DIR "") - set(GLOG_LIBRARY glog::glog) - else (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) - glog_message(STATUS "Failed to find an installed/exported CMake " - "configuration for glog, will perform search for installed glog " - "components.") - endif (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) -endif (GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION) - -if (NOT GLOG_FOUND) - # Either failed to find an exported glog CMake configuration, or user - # told us not to use one. Perform a manual search for all glog components. - - # Handle possible presence of lib prefix for libraries on MSVC, see - # also GLOG_RESET_FIND_LIBRARY_PREFIX(). - if (MSVC) - # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES - # s/t we can set it back before returning. - set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") - # The empty string in this list is important, it represents the case when - # the libraries have no prefix (shared libraries / DLLs). - set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") - endif (MSVC) - - # Search user-installed locations first, so that we prefer user installs - # to system installs where both exist. - list(APPEND GLOG_CHECK_INCLUDE_DIRS - /usr/local/include - /usr/local/homebrew/include # Mac OS X - /opt/local/var/macports/software # Mac OS X. - /opt/local/include - /usr/include) - # Windows (for C:/Program Files prefix). - list(APPEND GLOG_CHECK_PATH_SUFFIXES - glog/include - glog/Include - Glog/include - Glog/Include - google-glog/include # CMake installs with project name prefix. - google-glog/Include) - - list(APPEND GLOG_CHECK_LIBRARY_DIRS - /usr/local/lib - /usr/local/homebrew/lib # Mac OS X. - /opt/local/lib - /usr/lib) - # Windows (for C:/Program Files prefix). - list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES - glog/lib - glog/Lib - Glog/lib - Glog/Lib - google-glog/lib # CMake installs with project name prefix. - google-glog/Lib) - - # Search supplied hint directories first if supplied. - find_path(GLOG_INCLUDE_DIR - NAMES glog/logging.h - HINTS ${GLOG_INCLUDE_DIR_HINTS} - PATHS ${GLOG_CHECK_INCLUDE_DIRS} - PATH_SUFFIXES ${GLOG_CHECK_PATH_SUFFIXES}) - if (NOT GLOG_INCLUDE_DIR OR - NOT EXISTS ${GLOG_INCLUDE_DIR}) - glog_report_not_found( - "Could not find glog include directory, set GLOG_INCLUDE_DIR " - "to directory containing glog/logging.h") - endif (NOT GLOG_INCLUDE_DIR OR - NOT EXISTS ${GLOG_INCLUDE_DIR}) - - find_library(GLOG_LIBRARY NAMES glog - HINTS ${GLOG_LIBRARY_DIR_HINTS} - PATHS ${GLOG_CHECK_LIBRARY_DIRS} - PATH_SUFFIXES ${GLOG_CHECK_LIBRARY_SUFFIXES}) - if (NOT GLOG_LIBRARY OR - NOT EXISTS ${GLOG_LIBRARY}) - glog_report_not_found( - "Could not find glog library, set GLOG_LIBRARY " - "to full path to libglog.") - endif (NOT GLOG_LIBRARY OR - NOT EXISTS ${GLOG_LIBRARY}) - - # Mark internally as found, then verify. GLOG_REPORT_NOT_FOUND() unsets - # if called. - set(GLOG_FOUND TRUE) - - # Glog does not seem to provide any record of the version in its - # source tree, thus cannot extract version. - - # Catch case when caller has set GLOG_INCLUDE_DIR in the cache / GUI and - # thus FIND_[PATH/LIBRARY] are not called, but specified locations are - # invalid, otherwise we would report the library as found. - if (GLOG_INCLUDE_DIR AND - NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) - glog_report_not_found( - "Caller defined GLOG_INCLUDE_DIR:" - " ${GLOG_INCLUDE_DIR} does not contain glog/logging.h header.") - endif (GLOG_INCLUDE_DIR AND - NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) - # TODO: This regex for glog library is pretty primitive, we use lowercase - # for comparison to handle Windows using CamelCase library names, could - # this check be better? - string(TOLOWER "${GLOG_LIBRARY}" LOWERCASE_GLOG_LIBRARY) - if (GLOG_LIBRARY AND - NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") - glog_report_not_found( - "Caller defined GLOG_LIBRARY: " - "${GLOG_LIBRARY} does not match glog.") - endif (GLOG_LIBRARY AND - NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") - - # add glog::glog target - add_library(glog::glog INTERFACE IMPORTED) - target_include_directories(glog::glog INTERFACE ${GLOG_INCLUDE_DIRS}) - target_link_libraries(glog::glog INTERFACE ${GLOG_LIBRARY}) - - glog_reset_find_library_prefix() - -endif (NOT GLOG_FOUND) - -# Set standard CMake FindPackage variables if found. -if (GLOG_FOUND) - set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) - set(GLOG_LIBRARIES ${GLOG_LIBRARY}) -endif (GLOG_FOUND) - -# If we are using an exported CMake glog target, the include directories are -# wrapped into the target itself, and do not have to be (and are not) -# separately specified. In which case, we should not add GLOG_INCLUDE_DIRS -# to the list of required variables in order that glog be reported as found. -if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) - set(GLOG_REQUIRED_VARIABLES GLOG_LIBRARIES) -else () - set(GLOG_REQUIRED_VARIABLES GLOG_INCLUDE_DIRS GLOG_LIBRARIES) -endif () - -# Handle REQUIRED / QUIET optional arguments. -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(Glog DEFAULT_MSG - ${GLOG_REQUIRED_VARIABLES}) - -# Only mark internal variables as advanced if we found glog, otherwise -# leave them visible in the standard GUI for the user to set manually. -if (GLOG_FOUND) - mark_as_advanced(FORCE GLOG_INCLUDE_DIR - GLOG_LIBRARY - glog_DIR) # Autogenerated by find_package(glog) -endif (GLOG_FOUND) diff --git a/tooling/vcstool/wavemap_https.yml b/tooling/vcstool/wavemap_https.yml index e9be4aa35..a35d3b789 100644 --- a/tooling/vcstool/wavemap_https.yml +++ b/tooling/vcstool/wavemap_https.yml @@ -3,10 +3,6 @@ repositories: type: git url: https://github.com/catkin/catkin_simple.git version: master - eigen_checks: - type: git - url: https://github.com/ethz-asl/eigen_checks.git - version: master tracy_catkin: type: git url: https://github.com/ethz-asl/tracy_catkin.git diff --git a/tooling/vcstool/wavemap_ssh.yml b/tooling/vcstool/wavemap_ssh.yml index adb60244b..74f09fd7c 100644 --- a/tooling/vcstool/wavemap_ssh.yml +++ b/tooling/vcstool/wavemap_ssh.yml @@ -3,10 +3,6 @@ repositories: type: git url: git@github.com:catkin/catkin_simple.git version: master - eigen_checks: - type: git - url: git@github.com:ethz-asl/eigen_checks.git - version: master tracy_catkin: type: git url: git@github.com:ethz-asl/tracy_catkin.git From 631c767df17be38bbd4cc53e930a7238c7df763f Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 4 Apr 2024 19:06:04 +0200 Subject: [PATCH 091/159] Start migration to pure CMake wavemap library --- .../ros1}/wavemap/CHANGELOG.rst | 0 .../ros1}/wavemap/package.xml | 0 .../ros1}/wavemap_msgs/CHANGELOG.rst | 0 .../ros1}/wavemap_msgs/CMakeLists.txt | 0 .../ros1}/wavemap_msgs/msg/HashedBlock.msg | 0 .../ros1}/wavemap_msgs/msg/HashedBlocks.msg | 0 .../wavemap_msgs/msg/HashedWaveletOctree.msg | 0 .../msg/HashedWaveletOctreeBlock.msg | 0 .../ros1}/wavemap_msgs/msg/Index3D.msg | 0 .../ros1}/wavemap_msgs/msg/Map.msg | 0 .../ros1}/wavemap_msgs/msg/OctreeNode.msg | 0 .../ros1}/wavemap_msgs/msg/WaveletOctree.msg | 0 .../wavemap_msgs/msg/WaveletOctreeNode.msg | 0 .../ros1}/wavemap_msgs/package.xml | 0 .../ros1}/wavemap_msgs/srv/FilePath.srv | 0 .../ros1}/wavemap_ros/CHANGELOG.rst | 0 .../ros1}/wavemap_ros/CMakeLists.txt | 0 .../ros1}/wavemap_ros/app/ros_server.cc | 0 .../ros1}/wavemap_ros/app/rosbag_processor.cc | 0 .../other_packages/fast_lio/livox_mid360.yaml | 0 .../other_packages/fast_lio/ouster_os0.yaml | 0 .../wavemap_ros/config/rviz/default.rviz | 0 .../wavemap_ros/config/rviz/live_demo.rviz | 0 .../config/sensor_configs/MID360_config.json | 0 .../config/wavemap_livox_mid360.yaml | 0 .../wavemap_livox_mid360_azure_kinect.yaml | 0 .../wavemap_livox_mid360_pico_flexx.yaml | 0 .../wavemap_livox_mid360_pico_monstar.yaml | 0 .../config/wavemap_ouster_os0.yaml | 0 .../wavemap_ouster_os0_pico_monstar.yaml | 0 .../config/wavemap_ouster_os1.yaml | 0 .../config/wavemap_panoptic_mapping_rgbd.yaml | 0 .../ros1/wavemap_ros}/include/CPPLINT.cfg | 0 .../wavemap_ros/impl/rosbag_processor_inl.h | 0 .../wavemap_ros/inputs/depth_image_input.h | 0 .../inputs/impl/pointcloud_input_impl.h | 0 .../include/wavemap_ros/inputs/input_base.h | 0 .../wavemap_ros/inputs/input_factory.h | 0 .../wavemap_ros/inputs/pointcloud_input.h | 0 .../operations/crop_map_operation.h | 0 .../impl/publish_map_operation_inl.h | 0 .../wavemap_ros/operations/operation_base.h | 0 .../operations/operation_factory.h | 0 .../operations/prune_map_operation.h | 0 .../operations/publish_map_operation.h | 0 .../operations/publish_pointcloud_operation.h | 0 .../operations/threshold_map_operation.h | 0 .../include/wavemap_ros/ros_server.h | 0 .../include/wavemap_ros/utils/logging_level.h | 0 .../pointcloud_undistorter.h | 0 .../stamped_pointcloud.h | 0 .../wavemap_ros/utils/rosbag_processor.h | 0 .../wavemap_ros/utils/tf_transformer.h | 0 .../launch/datasets/dataset_base.launch | 0 .../newer_college_os0_base.launch | 0 .../newer_college_os0_cloister.launch | 0 .../newer_college_os0_math.launch | 0 .../newer_college_os0_mine.launch | 0 .../newer_college_os0_park.launch | 0 .../panoptic_mapping_rgbd_flat.launch | 0 .../launch/online/livox_mid360.launch | 0 .../online/livox_mid360_azure_kinect.launch | 0 .../online/livox_mid360_pico_flexx.launch | 0 .../online/livox_mid360_pico_monstar.launch | 0 .../online/ouster_os0_pico_monstar.launch | 0 .../launch/rosbag_processor.launch | 0 .../wavemap_ros/launch/wavemap_server.launch | 0 .../ros1}/wavemap_ros/package.xml | 0 .../ros1}/wavemap_ros/scripts/csv_to_tf.py | 0 .../wavemap_ros/scripts/odom_msg_to_tf.py | 0 .../panoptic_mapping_flat_data_player.py | 0 .../src/inputs/depth_image_input.cc | 0 .../wavemap_ros/src/inputs/input_base.cc | 0 .../wavemap_ros/src/inputs/input_factory.cc | 0 .../src/inputs/pointcloud_input.cc | 0 .../src/operations/crop_map_operation.cc | 0 .../src/operations/operation_factory.cc | 0 .../src/operations/prune_map_operation.cc | 0 .../src/operations/publish_map_operation.cc | 0 .../publish_pointcloud_operation.cc | 0 .../src/operations/threshold_map_operation.cc | 0 .../ros1}/wavemap_ros/src/ros_server.cc | 0 .../pointcloud_undistorter.cc | 0 .../wavemap_ros/src/utils/rosbag_processor.cc | 0 .../wavemap_ros/src/utils/tf_transformer.cc | 0 .../wavemap_ros_conversions/CHANGELOG.rst | 0 .../wavemap_ros_conversions/CMakeLists.txt | 0 .../include/CPPLINT.cfg | 0 .../config_conversions.h | 0 .../geometry_msg_conversions.h | 0 .../map_msg_conversions.h | 0 .../time_conversions.h | 0 .../ros1}/wavemap_ros_conversions/package.xml | 0 .../src/config_conversions.cc | 0 .../src/map_msg_conversions.cc | 0 .../src/time_conversions.cc | 0 .../test/src/test_map_msg_conversions.cc | 0 .../ros1}/wavemap_rviz_plugin/CHANGELOG.rst | 0 .../ros1}/wavemap_rviz_plugin/CMakeLists.txt | 0 .../icons/classes/WavemapMap.png | Bin .../wavemap_rviz_plugin}/include/CPPLINT.cfg | 0 .../include/wavemap_rviz_plugin/common.h | 0 .../wavemap_rviz_plugin/utils/alert_dialog.h | 0 .../utils/button_property.h | 0 .../utils/color_conversions.h | 0 .../wavemap_rviz_plugin/utils/listeners.h | 0 .../wavemap_rviz_plugin/visuals/cell_layer.h | 0 .../visuals/cell_selector.h | 0 .../visuals/slice_visual.h | 0 .../visuals/voxel_visual.h | 0 .../wavemap_rviz_plugin/wavemap_map_display.h | 0 .../ros1}/wavemap_rviz_plugin/package.xml | 0 .../plugin_description.xml | 0 .../src/utils/color_conversions.cc | 0 .../src/utils/listeners.cc | 0 .../src/visuals/cell_layer.cc | 0 .../src/visuals/cell_selector.cc | 0 .../src/visuals/slice_visual.cc | 0 .../src/visuals/voxel_visual.cc | 0 .../src/wavemap_map_display.cc | 0 libraries/wavemap/CMakeLists.txt | 153 ------------------ library/wavemap/CMakeLists.txt | 46 ++++++ .../include => library/wavemap}/CPPLINT.cfg | 0 library/wavemap/benchmark/CMakeLists.txt | 8 + .../benchmark/benchmark_haar_transforms.cc | 6 +- .../benchmark/benchmark_sparse_vector.cc | 6 +- library/wavemap/cmake/compiler_options.cmake | 31 ++++ .../wavemap/cmake/wavemap-extras.cmake | 0 library/wavemap/core/CMakeLists.txt | 41 +++++ .../wavemap => library/wavemap/core}/common.h | 2 +- .../wavemap/core}/config/config_base.h | 10 +- .../core}/config/impl/config_base_inl.h | 0 .../core}/config/impl/type_selector_inl.h | 0 .../core}/config/impl/value_with_unit_inl.h | 0 .../wavemap/core}/config/param.h | 4 +- .../wavemap/core}/config/param_checks.h | 2 +- .../wavemap/core}/config/type_selector.h | 4 +- .../wavemap/core}/config/value_with_unit.cc | 2 +- .../wavemap/core}/config/value_with_unit.h | 8 +- .../wavemap/core}/data_structure/aabb.h | 6 +- .../core}/data_structure/bucket_queue.h | 4 +- .../chunked_ndtree/chunked_ndtree.h | 12 +- .../chunked_ndtree/chunked_ndtree_chunk.h | 8 +- .../chunked_ndtree_node_address.h | 8 +- .../impl/chunked_ndtree_chunk_inl.h | 2 +- .../chunked_ndtree/impl/chunked_ndtree_inl.h | 4 +- .../impl/chunked_ndtree_node_address_inl.h | 0 .../core}/data_structure/dense_block_hash.h | 8 +- .../wavemap/core}/data_structure/dense_grid.h | 8 +- .../wavemap/core}/data_structure/image.h | 6 +- .../data_structure/impl/bucket_queue_inl.h | 0 .../impl/dense_block_hash_inl.h | 0 .../impl/ndtree_block_hash_inl.h | 0 .../data_structure/impl/spatial_hash_inl.h | 0 .../data_structure/ndtree/impl/ndtree_inl.h | 4 +- .../ndtree/impl/ndtree_node_inl.h | 2 +- .../core}/data_structure/ndtree/ndtree.h | 10 +- .../core}/data_structure/ndtree/ndtree_node.h | 6 +- .../core}/data_structure/ndtree_block_hash.h | 8 +- .../wavemap/core}/data_structure/pointcloud.h | 6 +- .../core}/data_structure/posed_object.h | 2 +- .../core}/data_structure/sparse_vector.h | 0 .../core}/data_structure/spatial_hash.h | 10 +- .../core}/indexing/impl/ndtree_index_inl.h | 4 +- .../core}/indexing/index_conversions.h | 12 +- .../wavemap/core}/indexing/index_hashes.h | 4 +- .../wavemap/core}/indexing/ndtree_index.h | 6 +- .../integrator/impl/integrator_base_inl.h | 0 .../core}/integrator/integrator_base.cc | 2 +- .../core}/integrator/integrator_base.h | 10 +- .../core}/integrator/integrator_factory.cc | 20 +-- .../core}/integrator/integrator_factory.h | 6 +- .../approximate_gaussian_distribution.h | 2 +- .../measurement_model/constant_ray.cc | 2 +- .../measurement_model/constant_ray.h | 6 +- .../measurement_model/continuous_beam.cc | 2 +- .../measurement_model/continuous_beam.h | 14 +- .../measurement_model/continuous_ray.cc | 2 +- .../measurement_model/continuous_ray.h | 10 +- .../impl/continuous_beam_inl.h | 2 +- .../impl/continuous_ray_inl.h | 2 +- .../measurement_model_base.h | 10 +- .../measurement_model_factory.cc | 6 +- .../measurement_model_factory.h | 2 +- .../projection_model/circular_projector.cc | 2 +- .../projection_model/circular_projector.h | 4 +- .../impl/ouster_projector_inl.h | 2 +- .../impl/pinhole_camera_projector_inl.h | 0 .../impl/projector_base_inl.h | 0 .../impl/spherical_projector_inl.h | 2 +- .../projection_model/ouster_projector.cc | 4 +- .../projection_model/ouster_projector.h | 8 +- .../pinhole_camera_projector.cc | 2 +- .../pinhole_camera_projector.h | 6 +- .../projection_model/projector_base.h | 10 +- .../projection_model/projector_factory.cc | 8 +- .../projection_model/projector_factory.h | 2 +- .../projection_model/spherical_projector.cc | 4 +- .../projection_model/spherical_projector.h | 8 +- .../coarse_to_fine_integrator.cc | 4 +- .../coarse_to_fine_integrator.h | 8 +- .../hashed_chunked_wavelet_integrator.cc | 2 +- .../hashed_chunked_wavelet_integrator.h | 12 +- .../hashed_wavelet_integrator.cc | 2 +- .../hashed_wavelet_integrator.h | 12 +- .../hierarchical_range_bounds.h | 10 +- .../hashed_chunked_wavelet_integrator_inl.h | 0 .../impl/hashed_wavelet_integrator_inl.h | 0 .../impl/hierarchical_range_bounds_inl.h | 4 +- .../impl/range_image_intersector_inl.h | 2 +- .../impl/wavelet_integrator_inl.h | 0 .../coarse_to_fine/range_image_intersector.h | 16 +- .../coarse_to_fine/wavelet_integrator.cc | 2 +- .../coarse_to_fine/wavelet_integrator.h | 10 +- .../fixed_resolution_integrator.cc | 6 +- .../fixed_resolution_integrator.h | 6 +- .../impl/projective_integrator_inl.h | 0 .../projective/projective_integrator.cc | 2 +- .../projective/projective_integrator.h | 14 +- .../core}/integrator/projective/update_type.h | 4 +- .../ray_tracing/ray_tracing_integrator.cc | 2 +- .../ray_tracing/ray_tracing_integrator.h | 8 +- .../core}/map/cell_types/haar_coefficients.h | 6 +- .../core}/map/cell_types/haar_transform.h | 8 +- .../map/cell_types/impl/haar_transform_inl.h | 4 +- .../wavemap/core}/map/hashed_blocks.cc | 2 +- .../wavemap/core}/map/hashed_blocks.h | 10 +- .../map/hashed_chunked_wavelet_octree.cc | 2 +- .../core}/map/hashed_chunked_wavelet_octree.h | 16 +- .../hashed_chunked_wavelet_octree_block.cc | 2 +- .../map/hashed_chunked_wavelet_octree_block.h | 14 +- .../core}/map/hashed_wavelet_octree.cc | 2 +- .../wavemap/core}/map/hashed_wavelet_octree.h | 16 +- .../core}/map/hashed_wavelet_octree_block.cc | 2 +- .../core}/map/hashed_wavelet_octree_block.h | 14 +- .../core}/map/impl/hashed_blocks_inl.h | 4 +- .../hashed_chunked_wavelet_octree_block_inl.h | 2 +- .../impl/hashed_chunked_wavelet_octree_inl.h | 2 +- .../impl/hashed_wavelet_octree_block_inl.h | 2 +- .../map/impl/hashed_wavelet_octree_inl.h | 2 +- .../core}/map/impl/volumetric_octree_inl.h | 0 .../core}/map/impl/wavelet_octree_inl.h | 2 +- .../wavemap/core}/map/map_base.cc | 4 +- .../wavemap/core}/map/map_base.h | 8 +- .../wavemap/core}/map/map_factory.cc | 12 +- .../wavemap/core}/map/map_factory.h | 2 +- .../wavemap/core}/map/volumetric_octree.cc | 4 +- .../wavemap/core}/map/volumetric_octree.h | 12 +- .../wavemap/core}/map/wavelet_octree.cc | 4 +- .../wavemap/core}/map/wavelet_octree.h | 14 +- .../wavemap/core}/utils/bits/bit_operations.h | 2 +- .../core}/utils/bits/morton_encoding.h | 4 +- .../wavemap/core}/utils/data/comparisons.h | 2 +- .../wavemap/core}/utils/data/constants.h | 0 .../wavemap/core}/utils/data/eigen_checks.h | 6 +- .../wavemap/core}/utils/data/fill.h | 0 .../core}/utils/iterate/grid_iterator.h | 2 +- .../utils/iterate/impl/ray_iterator_inl.h | 0 .../utils/iterate/impl/subtree_iterator_inl.h | 0 .../core}/utils/iterate/pointcloud_iterator.h | 2 +- .../core}/utils/iterate/ray_iterator.h | 6 +- .../core}/utils/iterate/subtree_iterator.h | 6 +- .../core}/utils/math/angle_normalization.h | 2 +- .../utils/math/approximate_trigonometry.h | 2 +- .../wavemap/core}/utils/math/int_math.h | 4 +- .../wavemap/core}/utils/math/tree_math.h | 2 +- .../wavemap/core}/utils/meta/nameof.h | 0 .../wavemap/core}/utils/meta/type_utils.h | 0 .../wavemap/core}/utils/neighbors/adjacency.h | 6 +- .../core}/utils/neighbors/grid_adjacency.h | 6 +- .../core}/utils/neighbors/grid_neighborhood.h | 12 +- .../utils/neighbors/impl/adjacency_inl.h | 0 .../utils/neighbors/impl/grid_adjacency_inl.h | 2 +- .../neighbors/impl/grid_neighborhood_inl.h | 0 .../neighbors/impl/ndtree_adjacency_inl.h | 0 .../core}/utils/neighbors/ndtree_adjacency.h | 6 +- .../wavemap/core}/utils/print/container.h | 0 .../wavemap/core}/utils/print/eigen.h | 0 .../core}/utils/query/classified_map.cc | 2 +- .../core}/utils/query/classified_map.h | 14 +- .../utils/query/impl/classified_map_inl.h | 0 .../query/impl/occupancy_classifier_inl.h | 0 .../core}/utils/query/impl/occupancy_inl.h | 0 .../utils/query/impl/point_sampler_inl.h | 0 .../utils/query/impl/query_accelerator_inl.h | 0 .../core}/utils/query/map_interpolator.h | 8 +- .../wavemap/core}/utils/query/occupancy.h | 6 +- .../core}/utils/query/occupancy_classifier.h | 4 +- .../core}/utils/query/point_sampler.cc | 2 +- .../wavemap/core}/utils/query/point_sampler.h | 12 +- .../utils/query/probability_conversions.h | 2 +- .../core}/utils/query/query_accelerator.cc | 2 +- .../core}/utils/query/query_accelerator.h | 12 +- .../core}/utils/random_number_generator.h | 2 +- .../utils/sdf/full_euclidean_sdf_generator.cc | 6 +- .../utils/sdf/full_euclidean_sdf_generator.h | 10 +- .../sdf/quasi_euclidean_sdf_generator.cc | 6 +- .../utils/sdf/quasi_euclidean_sdf_generator.h | 10 +- .../wavemap/core}/utils/thread_pool.cc | 2 +- .../wavemap/core}/utils/thread_pool.h | 0 .../wavemap/core/utils/time}/stopwatch.cc | 2 +- .../wavemap/core}/utils/time/stopwatch.h | 2 +- .../wavemap/core}/utils/time/time.h | 0 library/wavemap/io/CMakeLists.txt | 6 + .../wavemap}/io/file_conversions.cc | 0 .../wavemap/io/file_conversions.h | 2 +- .../wavemap/io/impl/streamable_types_impl.h | 0 .../wavemap}/io/stream_conversions.cc | 0 .../wavemap/io/stream_conversions.h | 12 +- .../wavemap/io/streamable_types.h | 2 +- library/wavemap/pipeline/CMakeLists.txt | 0 library/wavemap/test/CMakeLists.txt | 43 +++++ .../wavemap/test/config_generator.h | 22 +-- .../test/core}/data_structure/test_aabb.cc | 6 +- .../test/core}/data_structure/test_image.cc | 4 +- .../test/core}/data_structure/test_ndtree.cc | 6 +- .../core}/data_structure/test_pointcloud.cc | 6 +- .../data_structure/test_sparse_vector.cc | 6 +- .../core}/indexing/test_index_conversions.cc | 6 +- .../test/core}/indexing/test_ndtree_index.cc | 8 +- .../test_circular_projector.cc | 4 +- .../projection_model/test_image_projectors.cc | 14 +- .../test_spherical_projector.cc | 6 +- .../test_hierarchical_range_image.cc | 8 +- .../integrator/test_measurement_models.cc | 12 +- .../integrator/test_pointcloud_integrators.cc | 36 ++--- .../test_range_image_intersector.cc | 10 +- .../wavemap/test/core}/map/test_haar_cell.cc | 6 +- .../test/core}/map/test_hashed_blocks.cc | 4 +- .../wavemap/test/core}/map/test_map.cc | 16 +- .../test/core}/map/test_volumetric_octree.cc | 4 +- .../core}/utils/bits/test_bit_operations.cc | 2 +- .../test/core}/utils/data/test_comparisons.cc | 2 +- .../test/core}/utils/data/test_fill.cc | 4 +- .../core}/utils/iterate/test_grid_iterator.cc | 6 +- .../core}/utils/iterate/test_ray_iterator.cc | 6 +- .../utils/iterate/test_subtree_iterator.cc | 6 +- .../math/test_approximate_trigonometry.cc | 4 +- .../test/core}/utils/math/test_int_math.cc | 4 +- .../test/core}/utils/math/test_tree_math.cc | 2 +- .../core}/utils/neighbors/test_adjacency.cc | 4 +- .../utils/neighbors/test_grid_adjacency.cc | 4 +- .../utils/neighbors/test_grid_neighborhood.cc | 4 +- .../utils/neighbors/test_ndtree_adjacency.cc | 6 +- .../core}/utils/query/test_classified_map.cc | 8 +- .../utils/query/test_map_interpolator.cpp | 8 +- .../utils/query/test_occupancy_classifier.cc | 2 +- .../query/test_probability_conversions.cc | 2 +- .../utils/query/test_query_accelerator.cc | 2 +- .../core}/utils/sdf/test_sdf_generators.cc | 10 +- .../test/core}/utils/test_thread_pool.cc | 2 +- .../wavemap/test/eigen_utils.h | 2 +- .../wavemap/test/fixture_base.h | 4 +- .../wavemap/test/geometry_generator.h | 6 +- .../wavemap/test}/io/test_file_conversions.cc | 12 +- 355 files changed, 740 insertions(+), 718 deletions(-) rename {libraries => interfaces/ros1}/wavemap/CHANGELOG.rst (100%) rename {libraries => interfaces/ros1}/wavemap/package.xml (100%) rename {ros => interfaces/ros1}/wavemap_msgs/CHANGELOG.rst (100%) rename {ros => interfaces/ros1}/wavemap_msgs/CMakeLists.txt (100%) rename {ros => interfaces/ros1}/wavemap_msgs/msg/HashedBlock.msg (100%) rename {ros => interfaces/ros1}/wavemap_msgs/msg/HashedBlocks.msg (100%) rename {ros => interfaces/ros1}/wavemap_msgs/msg/HashedWaveletOctree.msg (100%) rename {ros => interfaces/ros1}/wavemap_msgs/msg/HashedWaveletOctreeBlock.msg (100%) rename {ros => interfaces/ros1}/wavemap_msgs/msg/Index3D.msg (100%) rename {ros => interfaces/ros1}/wavemap_msgs/msg/Map.msg (100%) rename {ros => interfaces/ros1}/wavemap_msgs/msg/OctreeNode.msg (100%) rename {ros => interfaces/ros1}/wavemap_msgs/msg/WaveletOctree.msg (100%) rename {ros => interfaces/ros1}/wavemap_msgs/msg/WaveletOctreeNode.msg (100%) rename {ros => interfaces/ros1}/wavemap_msgs/package.xml (100%) rename {ros => interfaces/ros1}/wavemap_msgs/srv/FilePath.srv (100%) rename {ros => interfaces/ros1}/wavemap_ros/CHANGELOG.rst (100%) rename {ros => interfaces/ros1}/wavemap_ros/CMakeLists.txt (100%) rename {ros => interfaces/ros1}/wavemap_ros/app/ros_server.cc (100%) rename {ros => interfaces/ros1}/wavemap_ros/app/rosbag_processor.cc (100%) rename {ros => interfaces/ros1}/wavemap_ros/config/other_packages/fast_lio/livox_mid360.yaml (100%) rename {ros => interfaces/ros1}/wavemap_ros/config/other_packages/fast_lio/ouster_os0.yaml (100%) rename {ros => interfaces/ros1}/wavemap_ros/config/rviz/default.rviz (100%) rename {ros => interfaces/ros1}/wavemap_ros/config/rviz/live_demo.rviz (100%) rename {ros => interfaces/ros1}/wavemap_ros/config/sensor_configs/MID360_config.json (100%) rename {ros => interfaces/ros1}/wavemap_ros/config/wavemap_livox_mid360.yaml (100%) rename {ros => interfaces/ros1}/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml (100%) rename {ros => interfaces/ros1}/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml (100%) rename {ros => interfaces/ros1}/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml (100%) rename {ros => interfaces/ros1}/wavemap_ros/config/wavemap_ouster_os0.yaml (100%) rename {ros => interfaces/ros1}/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml (100%) rename {ros => interfaces/ros1}/wavemap_ros/config/wavemap_ouster_os1.yaml (100%) rename {ros => interfaces/ros1}/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml (100%) rename {libraries/wavemap => interfaces/ros1/wavemap_ros}/include/CPPLINT.cfg (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/impl/rosbag_processor_inl.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/inputs/impl/pointcloud_input_impl.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/inputs/input_base.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/inputs/input_factory.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/operations/impl/publish_map_operation_inl.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/operations/operation_base.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/operations/operation_factory.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/operations/prune_map_operation.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/operations/threshold_map_operation.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/ros_server.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/utils/logging_level.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/stamped_pointcloud.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/utils/rosbag_processor.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/include/wavemap_ros/utils/tf_transformer.h (100%) rename {ros => interfaces/ros1}/wavemap_ros/launch/datasets/dataset_base.launch (100%) rename {ros => interfaces/ros1}/wavemap_ros/launch/datasets/newer_college/newer_college_os0_base.launch (100%) rename {ros => interfaces/ros1}/wavemap_ros/launch/datasets/newer_college/newer_college_os0_cloister.launch (100%) rename {ros => interfaces/ros1}/wavemap_ros/launch/datasets/newer_college/newer_college_os0_math.launch (100%) rename {ros => interfaces/ros1}/wavemap_ros/launch/datasets/newer_college/newer_college_os0_mine.launch (100%) rename {ros => interfaces/ros1}/wavemap_ros/launch/datasets/newer_college/newer_college_os0_park.launch (100%) rename {ros => interfaces/ros1}/wavemap_ros/launch/datasets/panoptic_mapping/panoptic_mapping_rgbd_flat.launch (100%) rename {ros => interfaces/ros1}/wavemap_ros/launch/online/livox_mid360.launch (100%) rename {ros => interfaces/ros1}/wavemap_ros/launch/online/livox_mid360_azure_kinect.launch (100%) rename {ros => interfaces/ros1}/wavemap_ros/launch/online/livox_mid360_pico_flexx.launch (100%) rename {ros => interfaces/ros1}/wavemap_ros/launch/online/livox_mid360_pico_monstar.launch (100%) rename {ros => interfaces/ros1}/wavemap_ros/launch/online/ouster_os0_pico_monstar.launch (100%) rename {ros => interfaces/ros1}/wavemap_ros/launch/rosbag_processor.launch (100%) rename {ros => interfaces/ros1}/wavemap_ros/launch/wavemap_server.launch (100%) rename {ros => interfaces/ros1}/wavemap_ros/package.xml (100%) rename {ros => interfaces/ros1}/wavemap_ros/scripts/csv_to_tf.py (100%) rename {ros => interfaces/ros1}/wavemap_ros/scripts/odom_msg_to_tf.py (100%) rename {ros => interfaces/ros1}/wavemap_ros/scripts/panoptic_mapping_flat_data_player.py (100%) rename {ros => interfaces/ros1}/wavemap_ros/src/inputs/depth_image_input.cc (100%) rename {ros => interfaces/ros1}/wavemap_ros/src/inputs/input_base.cc (100%) rename {ros => interfaces/ros1}/wavemap_ros/src/inputs/input_factory.cc (100%) rename {ros => interfaces/ros1}/wavemap_ros/src/inputs/pointcloud_input.cc (100%) rename {ros => interfaces/ros1}/wavemap_ros/src/operations/crop_map_operation.cc (100%) rename {ros => interfaces/ros1}/wavemap_ros/src/operations/operation_factory.cc (100%) rename {ros => interfaces/ros1}/wavemap_ros/src/operations/prune_map_operation.cc (100%) rename {ros => interfaces/ros1}/wavemap_ros/src/operations/publish_map_operation.cc (100%) rename {ros => interfaces/ros1}/wavemap_ros/src/operations/publish_pointcloud_operation.cc (100%) rename {ros => interfaces/ros1}/wavemap_ros/src/operations/threshold_map_operation.cc (100%) rename {ros => interfaces/ros1}/wavemap_ros/src/ros_server.cc (100%) rename {ros => interfaces/ros1}/wavemap_ros/src/utils/pointcloud_undistortion/pointcloud_undistorter.cc (100%) rename {ros => interfaces/ros1}/wavemap_ros/src/utils/rosbag_processor.cc (100%) rename {ros => interfaces/ros1}/wavemap_ros/src/utils/tf_transformer.cc (100%) rename {ros => interfaces/ros1}/wavemap_ros_conversions/CHANGELOG.rst (100%) rename {ros => interfaces/ros1}/wavemap_ros_conversions/CMakeLists.txt (100%) rename {libraries/wavemap/test => interfaces/ros1/wavemap_ros_conversions}/include/CPPLINT.cfg (100%) rename {ros => interfaces/ros1}/wavemap_ros_conversions/include/wavemap_ros_conversions/config_conversions.h (100%) rename {ros => interfaces/ros1}/wavemap_ros_conversions/include/wavemap_ros_conversions/geometry_msg_conversions.h (100%) rename {ros => interfaces/ros1}/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h (100%) rename {ros => interfaces/ros1}/wavemap_ros_conversions/include/wavemap_ros_conversions/time_conversions.h (100%) rename {ros => interfaces/ros1}/wavemap_ros_conversions/package.xml (100%) rename {ros => interfaces/ros1}/wavemap_ros_conversions/src/config_conversions.cc (100%) rename {ros => interfaces/ros1}/wavemap_ros_conversions/src/map_msg_conversions.cc (100%) rename {ros => interfaces/ros1}/wavemap_ros_conversions/src/time_conversions.cc (100%) rename {ros => interfaces/ros1}/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/CHANGELOG.rst (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/CMakeLists.txt (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/icons/classes/WavemapMap.png (100%) rename {ros/wavemap_ros => interfaces/ros1/wavemap_rviz_plugin}/include/CPPLINT.cfg (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/alert_dialog.h (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/button_property.h (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/color_conversions.h (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/listeners.h (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_layer.h (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/include/wavemap_rviz_plugin/wavemap_map_display.h (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/package.xml (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/plugin_description.xml (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/src/utils/color_conversions.cc (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/src/utils/listeners.cc (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/src/visuals/cell_layer.cc (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/src/visuals/cell_selector.cc (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/src/visuals/slice_visual.cc (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/src/visuals/voxel_visual.cc (100%) rename {ros => interfaces/ros1}/wavemap_rviz_plugin/src/wavemap_map_display.cc (100%) delete mode 100644 libraries/wavemap/CMakeLists.txt create mode 100644 library/wavemap/CMakeLists.txt rename {ros/wavemap_ros_conversions/include => library/wavemap}/CPPLINT.cfg (100%) create mode 100644 library/wavemap/benchmark/CMakeLists.txt rename {libraries => library}/wavemap/benchmark/benchmark_haar_transforms.cc (97%) rename {libraries => library}/wavemap/benchmark/benchmark_sparse_vector.cc (91%) create mode 100644 library/wavemap/cmake/compiler_options.cmake rename {libraries => library}/wavemap/cmake/wavemap-extras.cmake (100%) create mode 100644 library/wavemap/core/CMakeLists.txt rename {libraries/wavemap/include/wavemap => library/wavemap/core}/common.h (98%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/config/config_base.h (93%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/config/impl/config_base_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/config/impl/type_selector_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/config/impl/value_with_unit_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/config/param.h (95%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/config/param_checks.h (97%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/config/type_selector.h (94%) rename {libraries/wavemap/src => library/wavemap/core}/config/value_with_unit.cc (96%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/config/value_with_unit.h (91%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/aabb.h (97%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/bucket_queue.h (94%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/chunked_ndtree/chunked_ndtree.h (88%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/chunked_ndtree/chunked_ndtree_chunk.h (92%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/chunked_ndtree/chunked_ndtree_node_address.h (93%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h (99%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h (98%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/dense_block_hash.h (93%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/dense_grid.h (91%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/image.h (95%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/impl/bucket_queue_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/impl/dense_block_hash_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/impl/ndtree_block_hash_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/impl/spatial_hash_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/ndtree/impl/ndtree_inl.h (98%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/ndtree/impl/ndtree_node_inl.h (98%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/ndtree/ndtree.h (87%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/ndtree/ndtree_node.h (91%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/ndtree_block_hash.h (94%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/pointcloud.h (94%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/posed_object.h (97%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/sparse_vector.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/data_structure/spatial_hash.h (87%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/indexing/impl/ndtree_index_inl.h (97%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/indexing/index_conversions.h (96%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/indexing/index_hashes.h (93%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/indexing/ndtree_index.h (94%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/impl/integrator_base_inl.h (100%) rename {libraries/wavemap/src => library/wavemap/core}/integrator/integrator_base.cc (87%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/integrator_base.h (85%) rename {libraries/wavemap/src => library/wavemap/core}/integrator/integrator_factory.cc (88%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/integrator_factory.h (83%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/measurement_model/approximate_gaussian_distribution.h (96%) rename {libraries/wavemap/src => library/wavemap/core}/integrator/measurement_model/constant_ray.cc (64%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/measurement_model/constant_ray.h (94%) rename {libraries/wavemap/src => library/wavemap/core}/integrator/measurement_model/continuous_beam.cc (89%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/measurement_model/continuous_beam.h (92%) rename {libraries/wavemap/src => library/wavemap/core}/integrator/measurement_model/continuous_ray.cc (88%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/measurement_model/continuous_ray.h (92%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/measurement_model/impl/continuous_beam_inl.h (98%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/measurement_model/impl/continuous_ray_inl.h (97%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/measurement_model/measurement_model_base.h (85%) rename {libraries/wavemap/src => library/wavemap/core}/integrator/measurement_model/measurement_model_factory.cc (91%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/measurement_model/measurement_model_factory.h (92%) rename {libraries/wavemap/src => library/wavemap/core}/integrator/projection_model/circular_projector.cc (89%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projection_model/circular_projector.h (97%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projection_model/impl/ouster_projector_inl.h (98%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projection_model/impl/pinhole_camera_projector_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projection_model/impl/projector_base_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projection_model/impl/spherical_projector_inl.h (97%) rename {libraries/wavemap/src => library/wavemap/core}/integrator/projection_model/ouster_projector.cc (97%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projection_model/ouster_projector.h (92%) rename {libraries/wavemap/src => library/wavemap/core}/integrator/projection_model/pinhole_camera_projector.cc (97%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projection_model/pinhole_camera_projector.h (94%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projection_model/projector_base.h (95%) rename {libraries/wavemap/src => library/wavemap/core}/integrator/projection_model/projector_factory.cc (86%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projection_model/projector_factory.h (88%) rename {libraries/wavemap/src => library/wavemap/core}/integrator/projection_model/spherical_projector.cc (96%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projection_model/spherical_projector.h (90%) rename {libraries/wavemap/src => library/wavemap/core}/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.cc (95%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h (84%) rename {libraries/wavemap/src => library/wavemap/core}/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc (98%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h (88%) rename {libraries/wavemap/src => library/wavemap/core}/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc (98%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h (85%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h (95%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h (99%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h (97%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projective/coarse_to_fine/range_image_intersector.h (77%) rename {libraries/wavemap/src => library/wavemap/core}/integrator/projective/coarse_to_fine/wavelet_integrator.cc (93%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projective/coarse_to_fine/wavelet_integrator.h (82%) rename {libraries/wavemap/src => library/wavemap/core}/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc (95%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projective/fixed_resolution/fixed_resolution_integrator.h (89%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projective/impl/projective_integrator_inl.h (100%) rename {libraries/wavemap/src => library/wavemap/core}/integrator/projective/projective_integrator.cc (97%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projective/projective_integrator.h (90%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/projective/update_type.h (88%) rename {libraries/wavemap/src => library/wavemap/core}/integrator/ray_tracing/ray_tracing_integrator.cc (95%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/integrator/ray_tracing/ray_tracing_integrator.h (85%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/map/cell_types/haar_coefficients.h (97%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/map/cell_types/haar_transform.h (96%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/map/cell_types/impl/haar_transform_inl.h (98%) rename {libraries/wavemap/src => library/wavemap/core}/map/hashed_blocks.cc (95%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/map/hashed_blocks.h (88%) rename {libraries/wavemap/src => library/wavemap/core}/map/hashed_chunked_wavelet_octree.cc (97%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/map/hashed_chunked_wavelet_octree.h (92%) rename {libraries/wavemap/src => library/wavemap/core}/map/hashed_chunked_wavelet_octree_block.cc (99%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/map/hashed_chunked_wavelet_octree_block.h (90%) rename {libraries/wavemap/src => library/wavemap/core}/map/hashed_wavelet_octree.cc (97%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/map/hashed_wavelet_octree.h (92%) rename {libraries/wavemap/src => library/wavemap/core}/map/hashed_wavelet_octree_block.cc (99%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/map/hashed_wavelet_octree_block.h (89%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/map/impl/hashed_blocks_inl.h (87%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/map/impl/hashed_chunked_wavelet_octree_block_inl.h (97%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/map/impl/hashed_chunked_wavelet_octree_inl.h (98%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/map/impl/hashed_wavelet_octree_block_inl.h (96%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/map/impl/hashed_wavelet_octree_inl.h (98%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/map/impl/volumetric_octree_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/map/impl/wavelet_octree_inl.h (99%) rename {libraries/wavemap/src => library/wavemap/core}/map/map_base.cc (83%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/map/map_base.h (94%) rename {libraries/wavemap/src => library/wavemap/core}/map/map_factory.cc (90%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/map/map_factory.h (91%) rename {libraries/wavemap/src => library/wavemap/core}/map/volumetric_octree.cc (98%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/map/volumetric_octree.h (93%) rename {libraries/wavemap/src => library/wavemap/core}/map/wavelet_octree.cc (98%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/map/wavelet_octree.h (93%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/bits/bit_operations.h (99%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/bits/morton_encoding.h (98%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/data/comparisons.h (98%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/data/constants.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/data/eigen_checks.h (96%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/data/fill.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/iterate/grid_iterator.h (98%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/iterate/impl/ray_iterator_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/iterate/impl/subtree_iterator_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/iterate/pointcloud_iterator.h (98%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/iterate/ray_iterator.h (93%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/iterate/subtree_iterator.h (96%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/math/angle_normalization.h (93%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/math/approximate_trigonometry.h (99%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/math/int_math.h (97%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/math/tree_math.h (95%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/meta/nameof.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/meta/type_utils.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/neighbors/adjacency.h (83%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/neighbors/grid_adjacency.h (90%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/neighbors/grid_neighborhood.h (68%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/neighbors/impl/adjacency_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/neighbors/impl/grid_adjacency_inl.h (97%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/neighbors/impl/grid_neighborhood_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/neighbors/impl/ndtree_adjacency_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/neighbors/ndtree_adjacency.h (88%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/print/container.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/print/eigen.h (100%) rename {libraries/wavemap/src => library/wavemap/core}/utils/query/classified_map.cc (99%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/query/classified_map.h (94%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/query/impl/classified_map_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/query/impl/occupancy_classifier_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/query/impl/occupancy_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/query/impl/point_sampler_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/query/impl/query_accelerator_inl.h (100%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/query/map_interpolator.h (93%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/query/occupancy.h (84%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/query/occupancy_classifier.h (94%) rename {libraries/wavemap/src => library/wavemap/core}/utils/query/point_sampler.cc (95%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/query/point_sampler.h (83%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/query/probability_conversions.h (96%) rename {libraries/wavemap/src => library/wavemap/core}/utils/query/query_accelerator.cc (98%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/query/query_accelerator.h (92%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/random_number_generator.h (97%) rename {libraries/wavemap/src => library/wavemap/core}/utils/sdf/full_euclidean_sdf_generator.cc (97%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/sdf/full_euclidean_sdf_generator.h (88%) rename {libraries/wavemap/src => library/wavemap/core}/utils/sdf/quasi_euclidean_sdf_generator.cc (97%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/sdf/quasi_euclidean_sdf_generator.h (82%) rename {libraries/wavemap/src => library/wavemap/core}/utils/thread_pool.cc (97%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/thread_pool.h (100%) rename {libraries/wavemap/src/utils => library/wavemap/core/utils/time}/stopwatch.cc (92%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/time/stopwatch.h (93%) rename {libraries/wavemap/include/wavemap => library/wavemap/core}/utils/time/time.h (100%) create mode 100644 library/wavemap/io/CMakeLists.txt rename {libraries/wavemap/src => library/wavemap}/io/file_conversions.cc (100%) rename {libraries/wavemap/include => library}/wavemap/io/file_conversions.h (90%) rename {libraries/wavemap/include => library}/wavemap/io/impl/streamable_types_impl.h (100%) rename {libraries/wavemap/src => library/wavemap}/io/stream_conversions.cc (100%) rename {libraries/wavemap/include => library}/wavemap/io/stream_conversions.h (74%) rename {libraries/wavemap/include => library}/wavemap/io/streamable_types.h (98%) create mode 100644 library/wavemap/pipeline/CMakeLists.txt create mode 100644 library/wavemap/test/CMakeLists.txt rename {libraries/wavemap/test/include => library}/wavemap/test/config_generator.h (89%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/data_structure/test_aabb.cc (97%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/data_structure/test_image.cc (93%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/data_structure/test_ndtree.cc (96%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/data_structure/test_pointcloud.cc (98%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/data_structure/test_sparse_vector.cc (90%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/indexing/test_index_conversions.cc (98%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/indexing/test_ndtree_index.cc (98%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/integrator/projection_model/test_circular_projector.cc (97%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/integrator/projection_model/test_image_projectors.cc (98%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/integrator/projection_model/test_spherical_projector.cc (96%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/integrator/test_hierarchical_range_image.cc (97%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/integrator/test_measurement_models.cc (67%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/integrator/test_pointcloud_integrators.cc (88%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/integrator/test_range_image_intersector.cc (95%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/map/test_haar_cell.cc (98%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/map/test_hashed_blocks.cc (90%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/map/test_map.cc (95%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/map/test_volumetric_octree.cc (98%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/bits/test_bit_operations.cc (99%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/data/test_comparisons.cc (97%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/data/test_fill.cc (93%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/iterate/test_grid_iterator.cc (96%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/iterate/test_ray_iterator.cc (97%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/iterate/test_subtree_iterator.cc (95%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/math/test_approximate_trigonometry.cc (86%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/math/test_int_math.cc (98%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/math/test_tree_math.cc (98%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/neighbors/test_adjacency.cc (96%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/neighbors/test_grid_adjacency.cc (97%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/neighbors/test_grid_neighborhood.cc (96%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/neighbors/test_ndtree_adjacency.cc (91%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/query/test_classified_map.cc (94%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/query/test_map_interpolator.cpp (93%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/query/test_occupancy_classifier.cc (99%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/query/test_probability_conversions.cc (93%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/query/test_query_accelerator.cc (97%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/sdf/test_sdf_generators.cc (96%) rename {libraries/wavemap/test/src => library/wavemap/test/core}/utils/test_thread_pool.cc (88%) rename {libraries/wavemap/test/include => library}/wavemap/test/eigen_utils.h (98%) rename {libraries/wavemap/test/include => library}/wavemap/test/fixture_base.h (90%) rename {libraries/wavemap/test/include => library}/wavemap/test/geometry_generator.h (98%) rename {libraries/wavemap/test/src => library/wavemap/test}/io/test_file_conversions.cc (95%) diff --git a/libraries/wavemap/CHANGELOG.rst b/interfaces/ros1/wavemap/CHANGELOG.rst similarity index 100% rename from libraries/wavemap/CHANGELOG.rst rename to interfaces/ros1/wavemap/CHANGELOG.rst diff --git a/libraries/wavemap/package.xml b/interfaces/ros1/wavemap/package.xml similarity index 100% rename from libraries/wavemap/package.xml rename to interfaces/ros1/wavemap/package.xml diff --git a/ros/wavemap_msgs/CHANGELOG.rst b/interfaces/ros1/wavemap_msgs/CHANGELOG.rst similarity index 100% rename from ros/wavemap_msgs/CHANGELOG.rst rename to interfaces/ros1/wavemap_msgs/CHANGELOG.rst diff --git a/ros/wavemap_msgs/CMakeLists.txt b/interfaces/ros1/wavemap_msgs/CMakeLists.txt similarity index 100% rename from ros/wavemap_msgs/CMakeLists.txt rename to interfaces/ros1/wavemap_msgs/CMakeLists.txt diff --git a/ros/wavemap_msgs/msg/HashedBlock.msg b/interfaces/ros1/wavemap_msgs/msg/HashedBlock.msg similarity index 100% rename from ros/wavemap_msgs/msg/HashedBlock.msg rename to interfaces/ros1/wavemap_msgs/msg/HashedBlock.msg diff --git a/ros/wavemap_msgs/msg/HashedBlocks.msg b/interfaces/ros1/wavemap_msgs/msg/HashedBlocks.msg similarity index 100% rename from ros/wavemap_msgs/msg/HashedBlocks.msg rename to interfaces/ros1/wavemap_msgs/msg/HashedBlocks.msg diff --git a/ros/wavemap_msgs/msg/HashedWaveletOctree.msg b/interfaces/ros1/wavemap_msgs/msg/HashedWaveletOctree.msg similarity index 100% rename from ros/wavemap_msgs/msg/HashedWaveletOctree.msg rename to interfaces/ros1/wavemap_msgs/msg/HashedWaveletOctree.msg diff --git a/ros/wavemap_msgs/msg/HashedWaveletOctreeBlock.msg b/interfaces/ros1/wavemap_msgs/msg/HashedWaveletOctreeBlock.msg similarity index 100% rename from ros/wavemap_msgs/msg/HashedWaveletOctreeBlock.msg rename to interfaces/ros1/wavemap_msgs/msg/HashedWaveletOctreeBlock.msg diff --git a/ros/wavemap_msgs/msg/Index3D.msg b/interfaces/ros1/wavemap_msgs/msg/Index3D.msg similarity index 100% rename from ros/wavemap_msgs/msg/Index3D.msg rename to interfaces/ros1/wavemap_msgs/msg/Index3D.msg diff --git a/ros/wavemap_msgs/msg/Map.msg b/interfaces/ros1/wavemap_msgs/msg/Map.msg similarity index 100% rename from ros/wavemap_msgs/msg/Map.msg rename to interfaces/ros1/wavemap_msgs/msg/Map.msg diff --git a/ros/wavemap_msgs/msg/OctreeNode.msg b/interfaces/ros1/wavemap_msgs/msg/OctreeNode.msg similarity index 100% rename from ros/wavemap_msgs/msg/OctreeNode.msg rename to interfaces/ros1/wavemap_msgs/msg/OctreeNode.msg diff --git a/ros/wavemap_msgs/msg/WaveletOctree.msg b/interfaces/ros1/wavemap_msgs/msg/WaveletOctree.msg similarity index 100% rename from ros/wavemap_msgs/msg/WaveletOctree.msg rename to interfaces/ros1/wavemap_msgs/msg/WaveletOctree.msg diff --git a/ros/wavemap_msgs/msg/WaveletOctreeNode.msg b/interfaces/ros1/wavemap_msgs/msg/WaveletOctreeNode.msg similarity index 100% rename from ros/wavemap_msgs/msg/WaveletOctreeNode.msg rename to interfaces/ros1/wavemap_msgs/msg/WaveletOctreeNode.msg diff --git a/ros/wavemap_msgs/package.xml b/interfaces/ros1/wavemap_msgs/package.xml similarity index 100% rename from ros/wavemap_msgs/package.xml rename to interfaces/ros1/wavemap_msgs/package.xml diff --git a/ros/wavemap_msgs/srv/FilePath.srv b/interfaces/ros1/wavemap_msgs/srv/FilePath.srv similarity index 100% rename from ros/wavemap_msgs/srv/FilePath.srv rename to interfaces/ros1/wavemap_msgs/srv/FilePath.srv diff --git a/ros/wavemap_ros/CHANGELOG.rst b/interfaces/ros1/wavemap_ros/CHANGELOG.rst similarity index 100% rename from ros/wavemap_ros/CHANGELOG.rst rename to interfaces/ros1/wavemap_ros/CHANGELOG.rst diff --git a/ros/wavemap_ros/CMakeLists.txt b/interfaces/ros1/wavemap_ros/CMakeLists.txt similarity index 100% rename from ros/wavemap_ros/CMakeLists.txt rename to interfaces/ros1/wavemap_ros/CMakeLists.txt diff --git a/ros/wavemap_ros/app/ros_server.cc b/interfaces/ros1/wavemap_ros/app/ros_server.cc similarity index 100% rename from ros/wavemap_ros/app/ros_server.cc rename to interfaces/ros1/wavemap_ros/app/ros_server.cc diff --git a/ros/wavemap_ros/app/rosbag_processor.cc b/interfaces/ros1/wavemap_ros/app/rosbag_processor.cc similarity index 100% rename from ros/wavemap_ros/app/rosbag_processor.cc rename to interfaces/ros1/wavemap_ros/app/rosbag_processor.cc diff --git a/ros/wavemap_ros/config/other_packages/fast_lio/livox_mid360.yaml b/interfaces/ros1/wavemap_ros/config/other_packages/fast_lio/livox_mid360.yaml similarity index 100% rename from ros/wavemap_ros/config/other_packages/fast_lio/livox_mid360.yaml rename to interfaces/ros1/wavemap_ros/config/other_packages/fast_lio/livox_mid360.yaml diff --git a/ros/wavemap_ros/config/other_packages/fast_lio/ouster_os0.yaml b/interfaces/ros1/wavemap_ros/config/other_packages/fast_lio/ouster_os0.yaml similarity index 100% rename from ros/wavemap_ros/config/other_packages/fast_lio/ouster_os0.yaml rename to interfaces/ros1/wavemap_ros/config/other_packages/fast_lio/ouster_os0.yaml diff --git a/ros/wavemap_ros/config/rviz/default.rviz b/interfaces/ros1/wavemap_ros/config/rviz/default.rviz similarity index 100% rename from ros/wavemap_ros/config/rviz/default.rviz rename to interfaces/ros1/wavemap_ros/config/rviz/default.rviz diff --git a/ros/wavemap_ros/config/rviz/live_demo.rviz b/interfaces/ros1/wavemap_ros/config/rviz/live_demo.rviz similarity index 100% rename from ros/wavemap_ros/config/rviz/live_demo.rviz rename to interfaces/ros1/wavemap_ros/config/rviz/live_demo.rviz diff --git a/ros/wavemap_ros/config/sensor_configs/MID360_config.json b/interfaces/ros1/wavemap_ros/config/sensor_configs/MID360_config.json similarity index 100% rename from ros/wavemap_ros/config/sensor_configs/MID360_config.json rename to interfaces/ros1/wavemap_ros/config/sensor_configs/MID360_config.json diff --git a/ros/wavemap_ros/config/wavemap_livox_mid360.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml similarity index 100% rename from ros/wavemap_ros/config/wavemap_livox_mid360.yaml rename to interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml diff --git a/ros/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml similarity index 100% rename from ros/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml rename to interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml diff --git a/ros/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml similarity index 100% rename from ros/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml rename to interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml diff --git a/ros/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml similarity index 100% rename from ros/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml rename to interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml diff --git a/ros/wavemap_ros/config/wavemap_ouster_os0.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml similarity index 100% rename from ros/wavemap_ros/config/wavemap_ouster_os0.yaml rename to interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml diff --git a/ros/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml similarity index 100% rename from ros/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml rename to interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml diff --git a/ros/wavemap_ros/config/wavemap_ouster_os1.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml similarity index 100% rename from ros/wavemap_ros/config/wavemap_ouster_os1.yaml rename to interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml diff --git a/ros/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml similarity index 100% rename from ros/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml rename to interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml diff --git a/libraries/wavemap/include/CPPLINT.cfg b/interfaces/ros1/wavemap_ros/include/CPPLINT.cfg similarity index 100% rename from libraries/wavemap/include/CPPLINT.cfg rename to interfaces/ros1/wavemap_ros/include/CPPLINT.cfg diff --git a/ros/wavemap_ros/include/wavemap_ros/impl/rosbag_processor_inl.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/impl/rosbag_processor_inl.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/impl/rosbag_processor_inl.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/impl/rosbag_processor_inl.h diff --git a/ros/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h diff --git a/ros/wavemap_ros/include/wavemap_ros/inputs/impl/pointcloud_input_impl.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/impl/pointcloud_input_impl.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/inputs/impl/pointcloud_input_impl.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/impl/pointcloud_input_impl.h diff --git a/ros/wavemap_ros/include/wavemap_ros/inputs/input_base.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_base.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/inputs/input_base.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_base.h diff --git a/ros/wavemap_ros/include/wavemap_ros/inputs/input_factory.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_factory.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/inputs/input_factory.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_factory.h diff --git a/ros/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/impl/publish_map_operation_inl.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/impl/publish_map_operation_inl.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/operations/impl/publish_map_operation_inl.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/impl/publish_map_operation_inl.h diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/operation_base.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_base.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/operations/operation_base.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_base.h diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/operation_factory.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_factory.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/operations/operation_factory.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_factory.h diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/prune_map_operation.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/prune_map_operation.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/operations/prune_map_operation.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/prune_map_operation.h diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h diff --git a/ros/wavemap_ros/include/wavemap_ros/operations/threshold_map_operation.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/threshold_map_operation.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/operations/threshold_map_operation.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/threshold_map_operation.h diff --git a/ros/wavemap_ros/include/wavemap_ros/ros_server.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/ros_server.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h diff --git a/ros/wavemap_ros/include/wavemap_ros/utils/logging_level.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/logging_level.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/utils/logging_level.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/logging_level.h diff --git a/ros/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h diff --git a/ros/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/stamped_pointcloud.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/stamped_pointcloud.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/stamped_pointcloud.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/stamped_pointcloud.h diff --git a/ros/wavemap_ros/include/wavemap_ros/utils/rosbag_processor.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/rosbag_processor.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/utils/rosbag_processor.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/rosbag_processor.h diff --git a/ros/wavemap_ros/include/wavemap_ros/utils/tf_transformer.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/tf_transformer.h similarity index 100% rename from ros/wavemap_ros/include/wavemap_ros/utils/tf_transformer.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/tf_transformer.h diff --git a/ros/wavemap_ros/launch/datasets/dataset_base.launch b/interfaces/ros1/wavemap_ros/launch/datasets/dataset_base.launch similarity index 100% rename from ros/wavemap_ros/launch/datasets/dataset_base.launch rename to interfaces/ros1/wavemap_ros/launch/datasets/dataset_base.launch diff --git a/ros/wavemap_ros/launch/datasets/newer_college/newer_college_os0_base.launch b/interfaces/ros1/wavemap_ros/launch/datasets/newer_college/newer_college_os0_base.launch similarity index 100% rename from ros/wavemap_ros/launch/datasets/newer_college/newer_college_os0_base.launch rename to interfaces/ros1/wavemap_ros/launch/datasets/newer_college/newer_college_os0_base.launch diff --git a/ros/wavemap_ros/launch/datasets/newer_college/newer_college_os0_cloister.launch b/interfaces/ros1/wavemap_ros/launch/datasets/newer_college/newer_college_os0_cloister.launch similarity index 100% rename from ros/wavemap_ros/launch/datasets/newer_college/newer_college_os0_cloister.launch rename to interfaces/ros1/wavemap_ros/launch/datasets/newer_college/newer_college_os0_cloister.launch diff --git a/ros/wavemap_ros/launch/datasets/newer_college/newer_college_os0_math.launch b/interfaces/ros1/wavemap_ros/launch/datasets/newer_college/newer_college_os0_math.launch similarity index 100% rename from ros/wavemap_ros/launch/datasets/newer_college/newer_college_os0_math.launch rename to interfaces/ros1/wavemap_ros/launch/datasets/newer_college/newer_college_os0_math.launch diff --git a/ros/wavemap_ros/launch/datasets/newer_college/newer_college_os0_mine.launch b/interfaces/ros1/wavemap_ros/launch/datasets/newer_college/newer_college_os0_mine.launch similarity index 100% rename from ros/wavemap_ros/launch/datasets/newer_college/newer_college_os0_mine.launch rename to interfaces/ros1/wavemap_ros/launch/datasets/newer_college/newer_college_os0_mine.launch diff --git a/ros/wavemap_ros/launch/datasets/newer_college/newer_college_os0_park.launch b/interfaces/ros1/wavemap_ros/launch/datasets/newer_college/newer_college_os0_park.launch similarity index 100% rename from ros/wavemap_ros/launch/datasets/newer_college/newer_college_os0_park.launch rename to interfaces/ros1/wavemap_ros/launch/datasets/newer_college/newer_college_os0_park.launch diff --git a/ros/wavemap_ros/launch/datasets/panoptic_mapping/panoptic_mapping_rgbd_flat.launch b/interfaces/ros1/wavemap_ros/launch/datasets/panoptic_mapping/panoptic_mapping_rgbd_flat.launch similarity index 100% rename from ros/wavemap_ros/launch/datasets/panoptic_mapping/panoptic_mapping_rgbd_flat.launch rename to interfaces/ros1/wavemap_ros/launch/datasets/panoptic_mapping/panoptic_mapping_rgbd_flat.launch diff --git a/ros/wavemap_ros/launch/online/livox_mid360.launch b/interfaces/ros1/wavemap_ros/launch/online/livox_mid360.launch similarity index 100% rename from ros/wavemap_ros/launch/online/livox_mid360.launch rename to interfaces/ros1/wavemap_ros/launch/online/livox_mid360.launch diff --git a/ros/wavemap_ros/launch/online/livox_mid360_azure_kinect.launch b/interfaces/ros1/wavemap_ros/launch/online/livox_mid360_azure_kinect.launch similarity index 100% rename from ros/wavemap_ros/launch/online/livox_mid360_azure_kinect.launch rename to interfaces/ros1/wavemap_ros/launch/online/livox_mid360_azure_kinect.launch diff --git a/ros/wavemap_ros/launch/online/livox_mid360_pico_flexx.launch b/interfaces/ros1/wavemap_ros/launch/online/livox_mid360_pico_flexx.launch similarity index 100% rename from ros/wavemap_ros/launch/online/livox_mid360_pico_flexx.launch rename to interfaces/ros1/wavemap_ros/launch/online/livox_mid360_pico_flexx.launch diff --git a/ros/wavemap_ros/launch/online/livox_mid360_pico_monstar.launch b/interfaces/ros1/wavemap_ros/launch/online/livox_mid360_pico_monstar.launch similarity index 100% rename from ros/wavemap_ros/launch/online/livox_mid360_pico_monstar.launch rename to interfaces/ros1/wavemap_ros/launch/online/livox_mid360_pico_monstar.launch diff --git a/ros/wavemap_ros/launch/online/ouster_os0_pico_monstar.launch b/interfaces/ros1/wavemap_ros/launch/online/ouster_os0_pico_monstar.launch similarity index 100% rename from ros/wavemap_ros/launch/online/ouster_os0_pico_monstar.launch rename to interfaces/ros1/wavemap_ros/launch/online/ouster_os0_pico_monstar.launch diff --git a/ros/wavemap_ros/launch/rosbag_processor.launch b/interfaces/ros1/wavemap_ros/launch/rosbag_processor.launch similarity index 100% rename from ros/wavemap_ros/launch/rosbag_processor.launch rename to interfaces/ros1/wavemap_ros/launch/rosbag_processor.launch diff --git a/ros/wavemap_ros/launch/wavemap_server.launch b/interfaces/ros1/wavemap_ros/launch/wavemap_server.launch similarity index 100% rename from ros/wavemap_ros/launch/wavemap_server.launch rename to interfaces/ros1/wavemap_ros/launch/wavemap_server.launch diff --git a/ros/wavemap_ros/package.xml b/interfaces/ros1/wavemap_ros/package.xml similarity index 100% rename from ros/wavemap_ros/package.xml rename to interfaces/ros1/wavemap_ros/package.xml diff --git a/ros/wavemap_ros/scripts/csv_to_tf.py b/interfaces/ros1/wavemap_ros/scripts/csv_to_tf.py similarity index 100% rename from ros/wavemap_ros/scripts/csv_to_tf.py rename to interfaces/ros1/wavemap_ros/scripts/csv_to_tf.py diff --git a/ros/wavemap_ros/scripts/odom_msg_to_tf.py b/interfaces/ros1/wavemap_ros/scripts/odom_msg_to_tf.py similarity index 100% rename from ros/wavemap_ros/scripts/odom_msg_to_tf.py rename to interfaces/ros1/wavemap_ros/scripts/odom_msg_to_tf.py diff --git a/ros/wavemap_ros/scripts/panoptic_mapping_flat_data_player.py b/interfaces/ros1/wavemap_ros/scripts/panoptic_mapping_flat_data_player.py similarity index 100% rename from ros/wavemap_ros/scripts/panoptic_mapping_flat_data_player.py rename to interfaces/ros1/wavemap_ros/scripts/panoptic_mapping_flat_data_player.py diff --git a/ros/wavemap_ros/src/inputs/depth_image_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc similarity index 100% rename from ros/wavemap_ros/src/inputs/depth_image_input.cc rename to interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc diff --git a/ros/wavemap_ros/src/inputs/input_base.cc b/interfaces/ros1/wavemap_ros/src/inputs/input_base.cc similarity index 100% rename from ros/wavemap_ros/src/inputs/input_base.cc rename to interfaces/ros1/wavemap_ros/src/inputs/input_base.cc diff --git a/ros/wavemap_ros/src/inputs/input_factory.cc b/interfaces/ros1/wavemap_ros/src/inputs/input_factory.cc similarity index 100% rename from ros/wavemap_ros/src/inputs/input_factory.cc rename to interfaces/ros1/wavemap_ros/src/inputs/input_factory.cc diff --git a/ros/wavemap_ros/src/inputs/pointcloud_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc similarity index 100% rename from ros/wavemap_ros/src/inputs/pointcloud_input.cc rename to interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc diff --git a/ros/wavemap_ros/src/operations/crop_map_operation.cc b/interfaces/ros1/wavemap_ros/src/operations/crop_map_operation.cc similarity index 100% rename from ros/wavemap_ros/src/operations/crop_map_operation.cc rename to interfaces/ros1/wavemap_ros/src/operations/crop_map_operation.cc diff --git a/ros/wavemap_ros/src/operations/operation_factory.cc b/interfaces/ros1/wavemap_ros/src/operations/operation_factory.cc similarity index 100% rename from ros/wavemap_ros/src/operations/operation_factory.cc rename to interfaces/ros1/wavemap_ros/src/operations/operation_factory.cc diff --git a/ros/wavemap_ros/src/operations/prune_map_operation.cc b/interfaces/ros1/wavemap_ros/src/operations/prune_map_operation.cc similarity index 100% rename from ros/wavemap_ros/src/operations/prune_map_operation.cc rename to interfaces/ros1/wavemap_ros/src/operations/prune_map_operation.cc diff --git a/ros/wavemap_ros/src/operations/publish_map_operation.cc b/interfaces/ros1/wavemap_ros/src/operations/publish_map_operation.cc similarity index 100% rename from ros/wavemap_ros/src/operations/publish_map_operation.cc rename to interfaces/ros1/wavemap_ros/src/operations/publish_map_operation.cc diff --git a/ros/wavemap_ros/src/operations/publish_pointcloud_operation.cc b/interfaces/ros1/wavemap_ros/src/operations/publish_pointcloud_operation.cc similarity index 100% rename from ros/wavemap_ros/src/operations/publish_pointcloud_operation.cc rename to interfaces/ros1/wavemap_ros/src/operations/publish_pointcloud_operation.cc diff --git a/ros/wavemap_ros/src/operations/threshold_map_operation.cc b/interfaces/ros1/wavemap_ros/src/operations/threshold_map_operation.cc similarity index 100% rename from ros/wavemap_ros/src/operations/threshold_map_operation.cc rename to interfaces/ros1/wavemap_ros/src/operations/threshold_map_operation.cc diff --git a/ros/wavemap_ros/src/ros_server.cc b/interfaces/ros1/wavemap_ros/src/ros_server.cc similarity index 100% rename from ros/wavemap_ros/src/ros_server.cc rename to interfaces/ros1/wavemap_ros/src/ros_server.cc diff --git a/ros/wavemap_ros/src/utils/pointcloud_undistortion/pointcloud_undistorter.cc b/interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistortion/pointcloud_undistorter.cc similarity index 100% rename from ros/wavemap_ros/src/utils/pointcloud_undistortion/pointcloud_undistorter.cc rename to interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistortion/pointcloud_undistorter.cc diff --git a/ros/wavemap_ros/src/utils/rosbag_processor.cc b/interfaces/ros1/wavemap_ros/src/utils/rosbag_processor.cc similarity index 100% rename from ros/wavemap_ros/src/utils/rosbag_processor.cc rename to interfaces/ros1/wavemap_ros/src/utils/rosbag_processor.cc diff --git a/ros/wavemap_ros/src/utils/tf_transformer.cc b/interfaces/ros1/wavemap_ros/src/utils/tf_transformer.cc similarity index 100% rename from ros/wavemap_ros/src/utils/tf_transformer.cc rename to interfaces/ros1/wavemap_ros/src/utils/tf_transformer.cc diff --git a/ros/wavemap_ros_conversions/CHANGELOG.rst b/interfaces/ros1/wavemap_ros_conversions/CHANGELOG.rst similarity index 100% rename from ros/wavemap_ros_conversions/CHANGELOG.rst rename to interfaces/ros1/wavemap_ros_conversions/CHANGELOG.rst diff --git a/ros/wavemap_ros_conversions/CMakeLists.txt b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt similarity index 100% rename from ros/wavemap_ros_conversions/CMakeLists.txt rename to interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt diff --git a/libraries/wavemap/test/include/CPPLINT.cfg b/interfaces/ros1/wavemap_ros_conversions/include/CPPLINT.cfg similarity index 100% rename from libraries/wavemap/test/include/CPPLINT.cfg rename to interfaces/ros1/wavemap_ros_conversions/include/CPPLINT.cfg diff --git a/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/config_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/config_conversions.h similarity index 100% rename from ros/wavemap_ros_conversions/include/wavemap_ros_conversions/config_conversions.h rename to interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/config_conversions.h diff --git a/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/geometry_msg_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/geometry_msg_conversions.h similarity index 100% rename from ros/wavemap_ros_conversions/include/wavemap_ros_conversions/geometry_msg_conversions.h rename to interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/geometry_msg_conversions.h diff --git a/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h similarity index 100% rename from ros/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h rename to interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h diff --git a/ros/wavemap_ros_conversions/include/wavemap_ros_conversions/time_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/time_conversions.h similarity index 100% rename from ros/wavemap_ros_conversions/include/wavemap_ros_conversions/time_conversions.h rename to interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/time_conversions.h diff --git a/ros/wavemap_ros_conversions/package.xml b/interfaces/ros1/wavemap_ros_conversions/package.xml similarity index 100% rename from ros/wavemap_ros_conversions/package.xml rename to interfaces/ros1/wavemap_ros_conversions/package.xml diff --git a/ros/wavemap_ros_conversions/src/config_conversions.cc b/interfaces/ros1/wavemap_ros_conversions/src/config_conversions.cc similarity index 100% rename from ros/wavemap_ros_conversions/src/config_conversions.cc rename to interfaces/ros1/wavemap_ros_conversions/src/config_conversions.cc diff --git a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc b/interfaces/ros1/wavemap_ros_conversions/src/map_msg_conversions.cc similarity index 100% rename from ros/wavemap_ros_conversions/src/map_msg_conversions.cc rename to interfaces/ros1/wavemap_ros_conversions/src/map_msg_conversions.cc diff --git a/ros/wavemap_ros_conversions/src/time_conversions.cc b/interfaces/ros1/wavemap_ros_conversions/src/time_conversions.cc similarity index 100% rename from ros/wavemap_ros_conversions/src/time_conversions.cc rename to interfaces/ros1/wavemap_ros_conversions/src/time_conversions.cc diff --git a/ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc b/interfaces/ros1/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc similarity index 100% rename from ros/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc rename to interfaces/ros1/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc diff --git a/ros/wavemap_rviz_plugin/CHANGELOG.rst b/interfaces/ros1/wavemap_rviz_plugin/CHANGELOG.rst similarity index 100% rename from ros/wavemap_rviz_plugin/CHANGELOG.rst rename to interfaces/ros1/wavemap_rviz_plugin/CHANGELOG.rst diff --git a/ros/wavemap_rviz_plugin/CMakeLists.txt b/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt similarity index 100% rename from ros/wavemap_rviz_plugin/CMakeLists.txt rename to interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt diff --git a/ros/wavemap_rviz_plugin/icons/classes/WavemapMap.png b/interfaces/ros1/wavemap_rviz_plugin/icons/classes/WavemapMap.png similarity index 100% rename from ros/wavemap_rviz_plugin/icons/classes/WavemapMap.png rename to interfaces/ros1/wavemap_rviz_plugin/icons/classes/WavemapMap.png diff --git a/ros/wavemap_ros/include/CPPLINT.cfg b/interfaces/ros1/wavemap_rviz_plugin/include/CPPLINT.cfg similarity index 100% rename from ros/wavemap_ros/include/CPPLINT.cfg rename to interfaces/ros1/wavemap_rviz_plugin/include/CPPLINT.cfg diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h similarity index 100% rename from ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h rename to interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/alert_dialog.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/alert_dialog.h similarity index 100% rename from ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/alert_dialog.h rename to interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/alert_dialog.h diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/button_property.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/button_property.h similarity index 100% rename from ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/button_property.h rename to interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/button_property.h diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/color_conversions.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/color_conversions.h similarity index 100% rename from ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/color_conversions.h rename to interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/color_conversions.h diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/listeners.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/listeners.h similarity index 100% rename from ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/listeners.h rename to interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/listeners.h diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_layer.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_layer.h similarity index 100% rename from ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_layer.h rename to interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_layer.h diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h similarity index 100% rename from ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h rename to interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h similarity index 100% rename from ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h rename to interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h similarity index 100% rename from ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h rename to interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h diff --git a/ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/wavemap_map_display.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/wavemap_map_display.h similarity index 100% rename from ros/wavemap_rviz_plugin/include/wavemap_rviz_plugin/wavemap_map_display.h rename to interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/wavemap_map_display.h diff --git a/ros/wavemap_rviz_plugin/package.xml b/interfaces/ros1/wavemap_rviz_plugin/package.xml similarity index 100% rename from ros/wavemap_rviz_plugin/package.xml rename to interfaces/ros1/wavemap_rviz_plugin/package.xml diff --git a/ros/wavemap_rviz_plugin/plugin_description.xml b/interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml similarity index 100% rename from ros/wavemap_rviz_plugin/plugin_description.xml rename to interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml diff --git a/ros/wavemap_rviz_plugin/src/utils/color_conversions.cc b/interfaces/ros1/wavemap_rviz_plugin/src/utils/color_conversions.cc similarity index 100% rename from ros/wavemap_rviz_plugin/src/utils/color_conversions.cc rename to interfaces/ros1/wavemap_rviz_plugin/src/utils/color_conversions.cc diff --git a/ros/wavemap_rviz_plugin/src/utils/listeners.cc b/interfaces/ros1/wavemap_rviz_plugin/src/utils/listeners.cc similarity index 100% rename from ros/wavemap_rviz_plugin/src/utils/listeners.cc rename to interfaces/ros1/wavemap_rviz_plugin/src/utils/listeners.cc diff --git a/ros/wavemap_rviz_plugin/src/visuals/cell_layer.cc b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_layer.cc similarity index 100% rename from ros/wavemap_rviz_plugin/src/visuals/cell_layer.cc rename to interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_layer.cc diff --git a/ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc similarity index 100% rename from ros/wavemap_rviz_plugin/src/visuals/cell_selector.cc rename to interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc diff --git a/ros/wavemap_rviz_plugin/src/visuals/slice_visual.cc b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/slice_visual.cc similarity index 100% rename from ros/wavemap_rviz_plugin/src/visuals/slice_visual.cc rename to interfaces/ros1/wavemap_rviz_plugin/src/visuals/slice_visual.cc diff --git a/ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc similarity index 100% rename from ros/wavemap_rviz_plugin/src/visuals/voxel_visual.cc rename to interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc diff --git a/ros/wavemap_rviz_plugin/src/wavemap_map_display.cc b/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc similarity index 100% rename from ros/wavemap_rviz_plugin/src/wavemap_map_display.cc rename to interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt deleted file mode 100644 index 289e6395c..000000000 --- a/libraries/wavemap/CMakeLists.txt +++ /dev/null @@ -1,153 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(wavemap) - -# Dependencies -find_package(catkin REQUIRED COMPONENTS tracy_catkin minkindr) -find_package(Eigen3 REQUIRED NO_MODULE) -find_package(PkgConfig REQUIRED) -pkg_check_modules(glog REQUIRED libglog) - -if (ENABLE_BENCHMARKING) - find_package(benchmark REQUIRED) -endif () - -# Compiler definitions and options -include(cmake/wavemap-extras.cmake) -add_wavemap_compile_definitions_and_options() - -# Setup catkin package -catkin_package( - INCLUDE_DIRS - include - test/include - LIBRARIES - ${PROJECT_NAME} - DEPENDS - glog - CATKIN_DEPENDS - tracy_catkin - minkindr - CFG_EXTRAS - wavemap-extras.cmake) -# NOTE: The test headers are normally not exported (e.g. by adding test/include -# to the catkin_package's INCLUDE_DIRS), but we make an exception for this base -# package's test headers, such that common tooling and test fixtures can be -# reused in downstream wavemap packages. - -# For all targets -include_directories(include ${catkin_INCLUDE_DIRS}) - -# Libraries -# cmake-lint: disable=C0301 -add_library(${PROJECT_NAME} - src/config/value_with_unit.cc - src/integrator/measurement_model/continuous_beam.cc - src/integrator/measurement_model/constant_ray.cc - src/integrator/measurement_model/continuous_ray.cc - src/integrator/measurement_model/measurement_model_factory.cc - src/integrator/projection_model/circular_projector.cc - src/integrator/projection_model/ouster_projector.cc - src/integrator/projection_model/pinhole_camera_projector.cc - src/integrator/projection_model/spherical_projector.cc - src/integrator/projection_model/projector_factory.cc - src/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.cc - src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc - src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc - src/integrator/projective/coarse_to_fine/wavelet_integrator.cc - src/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc - src/integrator/projective/projective_integrator.cc - src/integrator/ray_tracing/ray_tracing_integrator.cc - src/integrator/integrator_base.cc - src/integrator/integrator_factory.cc - src/io/file_conversions.cc - src/io/stream_conversions.cc - src/map/hashed_blocks.cc - src/map/hashed_chunked_wavelet_octree.cc - src/map/hashed_chunked_wavelet_octree_block.cc - src/map/hashed_wavelet_octree.cc - src/map/hashed_wavelet_octree_block.cc - src/map/volumetric_octree.cc - src/map/wavelet_octree.cc - src/map/map_base.cc - src/map/map_factory.cc - src/utils/query/classified_map.cc - src/utils/query/query_accelerator.cc - src/utils/query/point_sampler.cc - src/utils/sdf/full_euclidean_sdf_generator.cc - src/utils/sdf/quasi_euclidean_sdf_generator.cc - src/utils/stopwatch.cc - src/utils/thread_pool.cc) -target_include_directories(${PROJECT_NAME} PUBLIC ${glog_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen ${glog_LIBRARIES}) - -# Install targets -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -# Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" -) - -# Tests -if (CATKIN_ENABLE_TESTING) - catkin_add_gtest( - test_${PROJECT_NAME} - test/src/data_structure/test_aabb.cc - test/src/data_structure/test_image.cc - test/src/data_structure/test_ndtree.cc - test/src/data_structure/test_pointcloud.cc - test/src/data_structure/test_sparse_vector.cc - test/src/indexing/test_index_conversions.cc - test/src/indexing/test_ndtree_index.cc - test/src/integrator/projection_model/test_circular_projector.cc - test/src/integrator/projection_model/test_image_projectors.cc - test/src/integrator/projection_model/test_spherical_projector.cc - test/src/integrator/test_hierarchical_range_image.cc - test/src/integrator/test_measurement_models.cc - test/src/integrator/test_pointcloud_integrators.cc - test/src/integrator/test_range_image_intersector.cc - test/src/io/test_file_conversions.cc - test/src/map/test_haar_cell.cc - test/src/map/test_hashed_blocks.cc - test/src/map/test_map.cc - test/src/map/test_volumetric_octree.cc - test/src/utils/bits/test_bit_operations.cc - test/src/utils/data/test_comparisons.cc - test/src/utils/data/test_fill.cc - test/src/utils/iterate/test_grid_iterator.cc - test/src/utils/iterate/test_ray_iterator.cc - test/src/utils/iterate/test_subtree_iterator.cc - test/src/utils/math/test_approximate_trigonometry.cc - test/src/utils/math/test_int_math.cc - test/src/utils/math/test_tree_math.cc - test/src/utils/neighbors/test_adjacency.cc - test/src/utils/neighbors/test_grid_adjacency.cc - test/src/utils/neighbors/test_grid_neighborhood.cc - test/src/utils/neighbors/test_ndtree_adjacency.cc - test/src/utils/query/test_classified_map.cc - test/src/utils/query/test_map_interpolator.cpp - test/src/utils/query/test_occupancy_classifier.cc - test/src/utils/query/test_probability_conversions.cc - test/src/utils/query/test_query_accelerator.cc - test/src/utils/sdf/test_sdf_generators.cc - test/src/utils/test_thread_pool.cc) - target_include_directories(test_${PROJECT_NAME} PRIVATE test/include) - target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} gtest_main) -endif () - -# Benchmarks -if (ENABLE_BENCHMARKING) - add_executable(benchmark_haar_transforms - benchmark/benchmark_haar_transforms.cc) - target_link_libraries(benchmark_haar_transforms ${PROJECT_NAME} - benchmark::benchmark) - - add_executable(benchmark_sparse_vector benchmark/benchmark_sparse_vector.cc) - target_link_libraries(benchmark_sparse_vector ${PROJECT_NAME} - benchmark::benchmark) -endif () diff --git a/library/wavemap/CMakeLists.txt b/library/wavemap/CMakeLists.txt new file mode 100644 index 000000000..0b02e12cf --- /dev/null +++ b/library/wavemap/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.10) + +project(wavemap VERSION 1.6.3 LANGUAGES CXX) + +# Dependencies +find_package(Eigen3 REQUIRED NO_MODULE) +find_package(PkgConfig REQUIRED) +pkg_check_modules(glog REQUIRED libglog) + +# TODO(victorr): Make sure these deps are also available to downstream packages. +# This might require using an alternative to FetchContent. +include(FetchContent) +FetchContent_Declare(tracy + GIT_REPOSITORY https://github.com/wolfpld/tracy.git + GIT_TAG v0.10 + GIT_SHALLOW TRUE + GIT_PROGRESS TRUE +) +FetchContent_MakeAvailable(tracy) +FetchContent_Declare(minkindr + GIT_REPOSITORY https://github.com/ethz-asl/minkindr.git + GIT_TAG master + GIT_SHALLOW TRUE + GIT_PROGRESS TRUE +) +FetchContent_MakeAvailable(minkindr) +# Add minkindr as header-only library +add_library(minkindr INTERFACE) +target_include_directories(minkindr INTERFACE ${minkindr_SOURCE_DIR}/minkindr/include) + +include(cmake/compiler_options.cmake) + +# Libraries +add_subdirectory(core) +add_subdirectory(io) +add_subdirectory(pipeline) + +## Tests +#if (CATKIN_ENABLE_TESTING) +# add_subdirectory(test) +#endif () +# +## Benchmarks +#if (ENABLE_BENCHMARKING) +# add_subdirectory(benchmark) +#endif () diff --git a/ros/wavemap_ros_conversions/include/CPPLINT.cfg b/library/wavemap/CPPLINT.cfg similarity index 100% rename from ros/wavemap_ros_conversions/include/CPPLINT.cfg rename to library/wavemap/CPPLINT.cfg diff --git a/library/wavemap/benchmark/CMakeLists.txt b/library/wavemap/benchmark/CMakeLists.txt new file mode 100644 index 000000000..a1fc38404 --- /dev/null +++ b/library/wavemap/benchmark/CMakeLists.txt @@ -0,0 +1,8 @@ +add_executable(benchmark_haar_transforms + benchmark/benchmark_haar_transforms.cc) +target_link_libraries(benchmark_haar_transforms ${PROJECT_NAME} + benchmark::benchmark) + +add_executable(benchmark_sparse_vector benchmark/benchmark_sparse_vector.cc) +target_link_libraries(benchmark_sparse_vector ${PROJECT_NAME} + benchmark::benchmark) diff --git a/libraries/wavemap/benchmark/benchmark_haar_transforms.cc b/library/wavemap/benchmark/benchmark_haar_transforms.cc similarity index 97% rename from libraries/wavemap/benchmark/benchmark_haar_transforms.cc rename to library/wavemap/benchmark/benchmark_haar_transforms.cc index 39e367bf9..b85952296 100644 --- a/libraries/wavemap/benchmark/benchmark_haar_transforms.cc +++ b/library/wavemap/benchmark/benchmark_haar_transforms.cc @@ -1,8 +1,8 @@ #include -#include "wavemap/map/cell_types/haar_coefficients.h" -#include "wavemap/map/cell_types/haar_transform.h" -#include "wavemap/utils/random_number_generator.h" +#include "wavemap/core/map/cell_types/haar_coefficients.h" +#include "wavemap/core/map/cell_types/haar_transform.h" +#include "wavemap/core/utils/random_number_generator.h" namespace wavemap { template diff --git a/libraries/wavemap/benchmark/benchmark_sparse_vector.cc b/library/wavemap/benchmark/benchmark_sparse_vector.cc similarity index 91% rename from libraries/wavemap/benchmark/benchmark_sparse_vector.cc rename to library/wavemap/benchmark/benchmark_sparse_vector.cc index 20f64b61c..476c4459e 100644 --- a/libraries/wavemap/benchmark/benchmark_sparse_vector.cc +++ b/library/wavemap/benchmark/benchmark_sparse_vector.cc @@ -1,8 +1,8 @@ #include -#include "wavemap/common.h" -#include "wavemap/data_structure/sparse_vector.h" -#include "wavemap/utils/random_number_generator.h" +#include "wavemap/core/common.h" +#include "wavemap/core/data_structure/sparse_vector.h" +#include "wavemap/core/utils/random_number_generator.h" namespace wavemap { static void AccessDenseArray(benchmark::State& state) { diff --git a/library/wavemap/cmake/compiler_options.cmake b/library/wavemap/cmake/compiler_options.cmake new file mode 100644 index 000000000..cb61f33d6 --- /dev/null +++ b/library/wavemap/cmake/compiler_options.cmake @@ -0,0 +1,31 @@ +function(set_wavemap_target_properties target) + target_compile_features(${target} PUBLIC cxx_std_17) + target_compile_definitions(${target} PUBLIC $<$:_USE_MATH_DEFINES>) +# target_compile_options( +# ${target} +# PRIVATE # MSVC +# $<$:/W4> +# $<$:/WX> +# # Clang/AppleClang +# $<$:-fcolor-diagnostics> +# $<$:-Werror> +# $<$:-Wall> +# $<$:-Wextra> +# $<$:-Wconversion> +# $<$:-Wno-sign-conversion> +# # GNU +# $<$:-fdiagnostics-color=always> +# $<$:-Werror> +# $<$:-Wall> +# $<$:-Wextra> +# $<$:-pedantic> +# $<$:-Wcast-align> +# $<$:-Wcast-qual> +# $<$:-Wconversion> +# $<$:-Wdisabled-optimization> +# $<$:-Woverloaded-virtual>) + set(INCLUDE_DIRS ${PROJECT_SOURCE_DIR}) + get_filename_component(INCLUDE_DIRS ${INCLUDE_DIRS} PATH) + target_include_directories(${target} + PUBLIC $ $) +endfunction() diff --git a/libraries/wavemap/cmake/wavemap-extras.cmake b/library/wavemap/cmake/wavemap-extras.cmake similarity index 100% rename from libraries/wavemap/cmake/wavemap-extras.cmake rename to library/wavemap/cmake/wavemap-extras.cmake diff --git a/library/wavemap/core/CMakeLists.txt b/library/wavemap/core/CMakeLists.txt new file mode 100644 index 000000000..cea4a214b --- /dev/null +++ b/library/wavemap/core/CMakeLists.txt @@ -0,0 +1,41 @@ +add_library(core) +add_library(wavemap::core ALIAS core) +target_sources(core PRIVATE + config/value_with_unit.cc + integrator/measurement_model/continuous_beam.cc + integrator/measurement_model/constant_ray.cc + integrator/measurement_model/continuous_ray.cc + integrator/measurement_model/measurement_model_factory.cc + integrator/projection_model/circular_projector.cc + integrator/projection_model/ouster_projector.cc + integrator/projection_model/pinhole_camera_projector.cc + integrator/projection_model/spherical_projector.cc + integrator/projection_model/projector_factory.cc + integrator/projective/coarse_to_fine/coarse_to_fine_integrator.cc + integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc + integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc + integrator/projective/coarse_to_fine/wavelet_integrator.cc + integrator/projective/fixed_resolution/fixed_resolution_integrator.cc + integrator/projective/projective_integrator.cc + integrator/ray_tracing/ray_tracing_integrator.cc + integrator/integrator_base.cc + integrator/integrator_factory.cc + map/hashed_blocks.cc + map/hashed_chunked_wavelet_octree.cc + map/hashed_chunked_wavelet_octree_block.cc + map/hashed_wavelet_octree.cc + map/hashed_wavelet_octree_block.cc + map/volumetric_octree.cc + map/wavelet_octree.cc + map/map_base.cc + map/map_factory.cc + utils/query/classified_map.cc + utils/query/query_accelerator.cc + utils/query/point_sampler.cc + utils/sdf/full_euclidean_sdf_generator.cc + utils/sdf/quasi_euclidean_sdf_generator.cc + utils/time/stopwatch.cc + utils/thread_pool.cc) +target_include_directories(core PUBLIC ${glog_INCLUDE_DIRS}) +target_link_libraries(core PUBLIC Eigen3::Eigen ${glog_LIBRARIES} TracyClient minkindr) +set_wavemap_target_properties(core) diff --git a/libraries/wavemap/include/wavemap/common.h b/library/wavemap/core/common.h similarity index 98% rename from libraries/wavemap/include/wavemap/common.h rename to library/wavemap/core/common.h index fc9d9218b..e5de7ee5b 100644 --- a/libraries/wavemap/include/wavemap/common.h +++ b/library/wavemap/core/common.h @@ -8,7 +8,7 @@ #include #include -#include "wavemap/utils/data/constants.h" +#include "wavemap/core/utils/data/constants.h" namespace wavemap { using FloatingPoint = float; diff --git a/libraries/wavemap/include/wavemap/config/config_base.h b/library/wavemap/core/config/config_base.h similarity index 93% rename from libraries/wavemap/include/wavemap/config/config_base.h rename to library/wavemap/core/config/config_base.h index ea0a88f68..587831bb0 100644 --- a/libraries/wavemap/include/wavemap/config/config_base.h +++ b/library/wavemap/core/config/config_base.h @@ -3,10 +3,10 @@ #include -#include "wavemap/config/param.h" -#include "wavemap/config/param_checks.h" -#include "wavemap/config/value_with_unit.h" -#include "wavemap/utils/meta/type_utils.h" +#include "wavemap/core/config/param.h" +#include "wavemap/core/config/param_checks.h" +#include "wavemap/core/config/value_with_unit.h" +#include "wavemap/core/utils/meta/type_utils.h" namespace wavemap { template #include -#include "wavemap/common.h" -#include "wavemap/utils/meta/type_utils.h" +#include "wavemap/core/common.h" +#include "wavemap/core/utils/meta/type_utils.h" namespace wavemap::param { using Name = std::string; diff --git a/libraries/wavemap/include/wavemap/config/param_checks.h b/library/wavemap/core/config/param_checks.h similarity index 97% rename from libraries/wavemap/include/wavemap/config/param_checks.h rename to library/wavemap/core/config/param_checks.h index 06026e4be..70acb2c52 100644 --- a/libraries/wavemap/include/wavemap/config/param_checks.h +++ b/library/wavemap/core/config/param_checks.h @@ -4,7 +4,7 @@ #include #include -#include "wavemap/utils/meta/nameof.h" +#include "wavemap/core/utils/meta/nameof.h" #define IS_PARAM_EQ(value, threshold, verbose) \ wavemap::is_param>(value, threshold, verbose, #value, "==") diff --git a/libraries/wavemap/include/wavemap/config/type_selector.h b/library/wavemap/core/config/type_selector.h similarity index 94% rename from libraries/wavemap/include/wavemap/config/type_selector.h rename to library/wavemap/core/config/type_selector.h index 80fa75b9c..1fb5c7cdf 100644 --- a/libraries/wavemap/include/wavemap/config/type_selector.h +++ b/library/wavemap/core/config/type_selector.h @@ -3,7 +3,7 @@ #include -#include "wavemap/config/param.h" +#include "wavemap/core/config/param.h" namespace wavemap { namespace param { @@ -53,6 +53,6 @@ struct TypeSelector { }; } // namespace wavemap -#include "wavemap/config/impl/type_selector_inl.h" +#include "wavemap/core/config/impl/type_selector_inl.h" #endif // WAVEMAP_CONFIG_TYPE_SELECTOR_H_ diff --git a/libraries/wavemap/src/config/value_with_unit.cc b/library/wavemap/core/config/value_with_unit.cc similarity index 96% rename from libraries/wavemap/src/config/value_with_unit.cc rename to library/wavemap/core/config/value_with_unit.cc index 4c8cae007..6a2579202 100644 --- a/libraries/wavemap/src/config/value_with_unit.cc +++ b/library/wavemap/core/config/value_with_unit.cc @@ -1,4 +1,4 @@ -#include "wavemap/config/value_with_unit.h" +#include "wavemap/core/config/value_with_unit.h" namespace wavemap::param { // clang-format off diff --git a/libraries/wavemap/include/wavemap/config/value_with_unit.h b/library/wavemap/core/config/value_with_unit.h similarity index 91% rename from libraries/wavemap/include/wavemap/config/value_with_unit.h rename to library/wavemap/core/config/value_with_unit.h index f05d33be8..435a9db90 100644 --- a/libraries/wavemap/include/wavemap/config/value_with_unit.h +++ b/library/wavemap/core/config/value_with_unit.h @@ -3,9 +3,9 @@ #include -#include "wavemap/common.h" -#include "wavemap/config/param.h" -#include "wavemap/config/type_selector.h" +#include "wavemap/core/common.h" +#include "wavemap/core/config/param.h" +#include "wavemap/core/config/type_selector.h" namespace wavemap { struct SiUnit : TypeSelector { @@ -81,6 +81,6 @@ std::optional getUnitToSi(const std::string& unit); } // namespace param } // namespace wavemap -#include "wavemap/config/impl/value_with_unit_inl.h" +#include "wavemap/core/config/impl/value_with_unit_inl.h" #endif // WAVEMAP_CONFIG_VALUE_WITH_UNIT_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/aabb.h b/library/wavemap/core/data_structure/aabb.h similarity index 97% rename from libraries/wavemap/include/wavemap/data_structure/aabb.h rename to library/wavemap/core/data_structure/aabb.h index 334b86960..6a4c92ed8 100644 --- a/libraries/wavemap/include/wavemap/data_structure/aabb.h +++ b/library/wavemap/core/data_structure/aabb.h @@ -4,9 +4,9 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/utils/math/int_math.h" -#include "wavemap/utils/print/eigen.h" +#include "wavemap/core/common.h" +#include "wavemap/core/utils/math/int_math.h" +#include "wavemap/core/utils/print/eigen.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/data_structure/bucket_queue.h b/library/wavemap/core/data_structure/bucket_queue.h similarity index 94% rename from libraries/wavemap/include/wavemap/data_structure/bucket_queue.h rename to library/wavemap/core/data_structure/bucket_queue.h index 642753709..cf8ea98d1 100644 --- a/libraries/wavemap/include/wavemap/data_structure/bucket_queue.h +++ b/library/wavemap/core/data_structure/bucket_queue.h @@ -5,7 +5,7 @@ #include #include -#include +#include "wavemap/core/common.h" namespace wavemap { /** @@ -53,6 +53,6 @@ class BucketQueue { }; } // namespace wavemap -#include "wavemap/data_structure/impl/bucket_queue_inl.h" +#include "wavemap/core/data_structure/impl/bucket_queue_inl.h" #endif // WAVEMAP_DATA_STRUCTURE_BUCKET_QUEUE_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h b/library/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h similarity index 88% rename from libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h rename to library/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h index 243288b41..4a9b142d6 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree.h +++ b/library/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h @@ -5,11 +5,11 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/data_structure/chunked_ndtree/chunked_ndtree_chunk.h" -#include "wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_address.h" -#include "wavemap/indexing/ndtree_index.h" -#include "wavemap/utils/iterate/subtree_iterator.h" +#include "wavemap/core/common.h" +#include "wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_chunk.h" +#include "wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h" +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/utils/iterate/subtree_iterator.h" namespace wavemap { template @@ -76,6 +76,6 @@ template using ChunkedOctree = ChunkedNdtree; } // namespace wavemap -#include "wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h" +#include "wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h" #endif // WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_chunk.h b/library/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_chunk.h similarity index 92% rename from libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_chunk.h rename to library/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_chunk.h index f06dd40cb..6bbd3cdca 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_chunk.h +++ b/library/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_chunk.h @@ -6,9 +6,9 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/indexing/ndtree_index.h" -#include "wavemap/utils/math/tree_math.h" +#include "wavemap/core/common.h" +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/utils/math/tree_math.h" namespace wavemap { template @@ -77,6 +77,6 @@ class ChunkedNdtreeChunk { }; } // namespace wavemap -#include "wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h" +#include "wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h" #endif // WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_CHUNK_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_address.h b/library/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h similarity index 93% rename from libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_address.h rename to library/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h index b281149d4..e7a4c6050 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/chunked_ndtree_node_address.h +++ b/library/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h @@ -3,9 +3,9 @@ #include -#include "wavemap/common.h" -#include "wavemap/data_structure/chunked_ndtree/chunked_ndtree_chunk.h" -#include "wavemap/utils/data/comparisons.h" +#include "wavemap/core/common.h" +#include "wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_chunk.h" +#include "wavemap/core/utils/data/comparisons.h" namespace wavemap { template @@ -105,6 +105,6 @@ class ChunkedNdtreeNodeRef { }; } // namespace wavemap -#include "wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h" +#include "wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h" #endif // WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_NODE_ADDRESS_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h b/library/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h similarity index 99% rename from libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h rename to library/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h index 33d45db67..d42b70a0e 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h +++ b/library/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h @@ -4,7 +4,7 @@ #include #include -#include "wavemap/utils/data/comparisons.h" +#include "wavemap/core/utils/data/comparisons.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h b/library/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h similarity index 98% rename from libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h rename to library/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h index 8fba55d86..5f9347900 100644 --- a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h +++ b/library/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h @@ -5,8 +5,8 @@ #include #include -#include "wavemap/data_structure/pointcloud.h" -#include "wavemap/indexing/index_conversions.h" +#include "wavemap/core/data_structure/pointcloud.h" +#include "wavemap/core/indexing/index_conversions.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h b/library/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h rename to library/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h diff --git a/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h b/library/wavemap/core/data_structure/dense_block_hash.h similarity index 93% rename from libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h rename to library/wavemap/core/data_structure/dense_block_hash.h index 7e5e6ce34..371d7a7ed 100644 --- a/libraries/wavemap/include/wavemap/data_structure/dense_block_hash.h +++ b/library/wavemap/core/data_structure/dense_block_hash.h @@ -1,9 +1,9 @@ #ifndef WAVEMAP_DATA_STRUCTURE_DENSE_BLOCK_HASH_H_ #define WAVEMAP_DATA_STRUCTURE_DENSE_BLOCK_HASH_H_ -#include "wavemap/data_structure/dense_grid.h" -#include "wavemap/data_structure/spatial_hash.h" -#include "wavemap/utils/math/int_math.h" +#include "wavemap/core/data_structure/dense_grid.h" +#include "wavemap/core/data_structure/spatial_hash.h" +#include "wavemap/core/utils/math/int_math.h" namespace wavemap { template @@ -76,6 +76,6 @@ class DenseBlockHash { }; } // namespace wavemap -#include "wavemap/data_structure/impl/dense_block_hash_inl.h" +#include "wavemap/core/data_structure/impl/dense_block_hash_inl.h" #endif // WAVEMAP_DATA_STRUCTURE_DENSE_BLOCK_HASH_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/dense_grid.h b/library/wavemap/core/data_structure/dense_grid.h similarity index 91% rename from libraries/wavemap/include/wavemap/data_structure/dense_grid.h rename to library/wavemap/core/data_structure/dense_grid.h index 40b8848c6..acb70ef7d 100644 --- a/libraries/wavemap/include/wavemap/data_structure/dense_grid.h +++ b/library/wavemap/core/data_structure/dense_grid.h @@ -1,10 +1,10 @@ #ifndef WAVEMAP_DATA_STRUCTURE_DENSE_GRID_H_ #define WAVEMAP_DATA_STRUCTURE_DENSE_GRID_H_ -#include "wavemap/common.h" -#include "wavemap/indexing/index_conversions.h" -#include "wavemap/utils/data/eigen_checks.h" -#include "wavemap/utils/math/int_math.h" +#include "wavemap/core/common.h" +#include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/utils/data/eigen_checks.h" +#include "wavemap/core/utils/math/int_math.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/data_structure/image.h b/library/wavemap/core/data_structure/image.h similarity index 95% rename from libraries/wavemap/include/wavemap/data_structure/image.h rename to library/wavemap/core/data_structure/image.h index d5364902b..c3c556a04 100644 --- a/libraries/wavemap/include/wavemap/data_structure/image.h +++ b/library/wavemap/core/data_structure/image.h @@ -4,9 +4,9 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/data_structure/posed_object.h" -#include "wavemap/utils/data/fill.h" +#include "wavemap/core/common.h" +#include "wavemap/core/data_structure/posed_object.h" +#include "wavemap/core/utils/data/fill.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/data_structure/impl/bucket_queue_inl.h b/library/wavemap/core/data_structure/impl/bucket_queue_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/data_structure/impl/bucket_queue_inl.h rename to library/wavemap/core/data_structure/impl/bucket_queue_inl.h diff --git a/libraries/wavemap/include/wavemap/data_structure/impl/dense_block_hash_inl.h b/library/wavemap/core/data_structure/impl/dense_block_hash_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/data_structure/impl/dense_block_hash_inl.h rename to library/wavemap/core/data_structure/impl/dense_block_hash_inl.h diff --git a/libraries/wavemap/include/wavemap/data_structure/impl/ndtree_block_hash_inl.h b/library/wavemap/core/data_structure/impl/ndtree_block_hash_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/data_structure/impl/ndtree_block_hash_inl.h rename to library/wavemap/core/data_structure/impl/ndtree_block_hash_inl.h diff --git a/libraries/wavemap/include/wavemap/data_structure/impl/spatial_hash_inl.h b/library/wavemap/core/data_structure/impl/spatial_hash_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/data_structure/impl/spatial_hash_inl.h rename to library/wavemap/core/data_structure/impl/spatial_hash_inl.h diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_inl.h b/library/wavemap/core/data_structure/ndtree/impl/ndtree_inl.h similarity index 98% rename from libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_inl.h rename to library/wavemap/core/data_structure/ndtree/impl/ndtree_inl.h index cd593f32d..b76b6ba27 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_inl.h +++ b/library/wavemap/core/data_structure/ndtree/impl/ndtree_inl.h @@ -5,8 +5,8 @@ #include #include -#include "wavemap/data_structure/pointcloud.h" -#include "wavemap/indexing/index_conversions.h" +#include "wavemap/core/data_structure/pointcloud.h" +#include "wavemap/core/indexing/index_conversions.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_node_inl.h b/library/wavemap/core/data_structure/ndtree/impl/ndtree_node_inl.h similarity index 98% rename from libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_node_inl.h rename to library/wavemap/core/data_structure/ndtree/impl/ndtree_node_inl.h index 4cccb5ce9..b9dda8b4a 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree/impl/ndtree_node_inl.h +++ b/library/wavemap/core/data_structure/ndtree/impl/ndtree_node_inl.h @@ -4,7 +4,7 @@ #include #include -#include "wavemap/utils/data/comparisons.h" +#include "wavemap/core/utils/data/comparisons.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree.h b/library/wavemap/core/data_structure/ndtree/ndtree.h similarity index 87% rename from libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree.h rename to library/wavemap/core/data_structure/ndtree/ndtree.h index 25edd46f6..1e758c01e 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree.h +++ b/library/wavemap/core/data_structure/ndtree/ndtree.h @@ -5,10 +5,10 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/data_structure/ndtree/ndtree_node.h" -#include "wavemap/indexing/ndtree_index.h" -#include "wavemap/utils/iterate/subtree_iterator.h" +#include "wavemap/core/common.h" +#include "wavemap/core/data_structure/ndtree/ndtree_node.h" +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/utils/iterate/subtree_iterator.h" namespace wavemap { template @@ -64,6 +64,6 @@ template using Octree = Ndtree; } // namespace wavemap -#include "wavemap/data_structure/ndtree/impl/ndtree_inl.h" +#include "wavemap/core/data_structure/ndtree/impl/ndtree_inl.h" #endif // WAVEMAP_DATA_STRUCTURE_NDTREE_NDTREE_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree_node.h b/library/wavemap/core/data_structure/ndtree/ndtree_node.h similarity index 91% rename from libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree_node.h rename to library/wavemap/core/data_structure/ndtree/ndtree_node.h index b82b8da71..6c1fe5b88 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree/ndtree_node.h +++ b/library/wavemap/core/data_structure/ndtree/ndtree_node.h @@ -5,8 +5,8 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/indexing/ndtree_index.h" +#include "wavemap/core/common.h" +#include "wavemap/core/indexing/ndtree_index.h" namespace wavemap { template @@ -56,6 +56,6 @@ class NdtreeNode { }; } // namespace wavemap -#include "wavemap/data_structure/ndtree/impl/ndtree_node_inl.h" +#include "wavemap/core/data_structure/ndtree/impl/ndtree_node_inl.h" #endif // WAVEMAP_DATA_STRUCTURE_NDTREE_NDTREE_NODE_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h b/library/wavemap/core/data_structure/ndtree_block_hash.h similarity index 94% rename from libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h rename to library/wavemap/core/data_structure/ndtree_block_hash.h index d8e7dfcd4..1d554e703 100644 --- a/libraries/wavemap/include/wavemap/data_structure/ndtree_block_hash.h +++ b/library/wavemap/core/data_structure/ndtree_block_hash.h @@ -3,9 +3,9 @@ #include -#include "wavemap/data_structure/ndtree/ndtree.h" -#include "wavemap/data_structure/spatial_hash.h" -#include "wavemap/utils/math/int_math.h" +#include "wavemap/core/data_structure/ndtree/ndtree.h" +#include "wavemap/core/data_structure/spatial_hash.h" +#include "wavemap/core/utils/math/int_math.h" namespace wavemap { template @@ -104,6 +104,6 @@ template using OctreeBlockHash = NdtreeBlockHash; } // namespace wavemap -#include "wavemap/data_structure/impl/ndtree_block_hash_inl.h" +#include "wavemap/core/data_structure/impl/ndtree_block_hash_inl.h" #endif // WAVEMAP_DATA_STRUCTURE_NDTREE_BLOCK_HASH_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/pointcloud.h b/library/wavemap/core/data_structure/pointcloud.h similarity index 94% rename from libraries/wavemap/include/wavemap/data_structure/pointcloud.h rename to library/wavemap/core/data_structure/pointcloud.h index cb2b5b61d..852d0eb56 100644 --- a/libraries/wavemap/include/wavemap/data_structure/pointcloud.h +++ b/library/wavemap/core/data_structure/pointcloud.h @@ -4,9 +4,9 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/data_structure/posed_object.h" -#include "wavemap/utils/iterate/pointcloud_iterator.h" +#include "wavemap/core/common.h" +#include "wavemap/core/data_structure/posed_object.h" +#include "wavemap/core/utils/iterate/pointcloud_iterator.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/data_structure/posed_object.h b/library/wavemap/core/data_structure/posed_object.h similarity index 97% rename from libraries/wavemap/include/wavemap/data_structure/posed_object.h rename to library/wavemap/core/data_structure/posed_object.h index 255e3154b..7a126cc6e 100644 --- a/libraries/wavemap/include/wavemap/data_structure/posed_object.h +++ b/library/wavemap/core/data_structure/posed_object.h @@ -4,7 +4,7 @@ #include #include -#include "wavemap/common.h" +#include "wavemap/core/common.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/data_structure/sparse_vector.h b/library/wavemap/core/data_structure/sparse_vector.h similarity index 100% rename from libraries/wavemap/include/wavemap/data_structure/sparse_vector.h rename to library/wavemap/core/data_structure/sparse_vector.h diff --git a/libraries/wavemap/include/wavemap/data_structure/spatial_hash.h b/library/wavemap/core/data_structure/spatial_hash.h similarity index 87% rename from libraries/wavemap/include/wavemap/data_structure/spatial_hash.h rename to library/wavemap/core/data_structure/spatial_hash.h index 2499a55b1..bb03e917b 100644 --- a/libraries/wavemap/include/wavemap/data_structure/spatial_hash.h +++ b/library/wavemap/core/data_structure/spatial_hash.h @@ -3,10 +3,10 @@ #include -#include "wavemap/common.h" -#include "wavemap/indexing/index_conversions.h" -#include "wavemap/indexing/index_hashes.h" -#include "wavemap/utils/math/int_math.h" +#include "wavemap/core/common.h" +#include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/indexing/index_hashes.h" +#include "wavemap/core/utils/math/int_math.h" namespace wavemap { namespace convert { @@ -54,6 +54,6 @@ class SpatialHash { }; } // namespace wavemap -#include "wavemap/data_structure/impl/spatial_hash_inl.h" +#include "wavemap/core/data_structure/impl/spatial_hash_inl.h" #endif // WAVEMAP_DATA_STRUCTURE_SPATIAL_HASH_H_ diff --git a/libraries/wavemap/include/wavemap/indexing/impl/ndtree_index_inl.h b/library/wavemap/core/indexing/impl/ndtree_index_inl.h similarity index 97% rename from libraries/wavemap/include/wavemap/indexing/impl/ndtree_index_inl.h rename to library/wavemap/core/indexing/impl/ndtree_index_inl.h index 66c62f332..568af9766 100644 --- a/libraries/wavemap/include/wavemap/indexing/impl/ndtree_index_inl.h +++ b/library/wavemap/core/indexing/impl/ndtree_index_inl.h @@ -6,8 +6,8 @@ #include #include -#include "wavemap/utils/bits/bit_operations.h" -#include "wavemap/utils/math/tree_math.h" +#include "wavemap/core/utils/bits/bit_operations.h" +#include "wavemap/core/utils/math/tree_math.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/indexing/index_conversions.h b/library/wavemap/core/indexing/index_conversions.h similarity index 96% rename from libraries/wavemap/include/wavemap/indexing/index_conversions.h rename to library/wavemap/core/indexing/index_conversions.h index 0fc67ee14..3c74bdfe7 100644 --- a/libraries/wavemap/include/wavemap/indexing/index_conversions.h +++ b/library/wavemap/core/indexing/index_conversions.h @@ -5,12 +5,12 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/data_structure/aabb.h" -#include "wavemap/indexing/ndtree_index.h" -#include "wavemap/utils/bits/morton_encoding.h" -#include "wavemap/utils/data/eigen_checks.h" -#include "wavemap/utils/math/int_math.h" +#include "wavemap/core/common.h" +#include "wavemap/core/data_structure/aabb.h" +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/utils/bits/morton_encoding.h" +#include "wavemap/core/utils/data/eigen_checks.h" +#include "wavemap/core/utils/math/int_math.h" namespace wavemap::convert { template diff --git a/libraries/wavemap/include/wavemap/indexing/index_hashes.h b/library/wavemap/core/indexing/index_hashes.h similarity index 93% rename from libraries/wavemap/include/wavemap/indexing/index_hashes.h rename to library/wavemap/core/indexing/index_hashes.h index 13f0eab81..bf27ba310 100644 --- a/libraries/wavemap/include/wavemap/indexing/index_hashes.h +++ b/library/wavemap/core/indexing/index_hashes.h @@ -3,8 +3,8 @@ #include -#include "wavemap/common.h" -#include "wavemap/indexing/ndtree_index.h" +#include "wavemap/core/common.h" +#include "wavemap/core/indexing/ndtree_index.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/indexing/ndtree_index.h b/library/wavemap/core/indexing/ndtree_index.h similarity index 94% rename from libraries/wavemap/include/wavemap/indexing/ndtree_index.h rename to library/wavemap/core/indexing/ndtree_index.h index b1eb40505..dea684edc 100644 --- a/libraries/wavemap/include/wavemap/indexing/ndtree_index.h +++ b/library/wavemap/core/indexing/ndtree_index.h @@ -5,8 +5,8 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/utils/math/int_math.h" +#include "wavemap/core/common.h" +#include "wavemap/core/utils/math/int_math.h" namespace wavemap { using NdtreeIndexRelativeChild = uint8_t; @@ -61,6 +61,6 @@ using QuadtreeIndex = NdtreeIndex<2>; using OctreeIndex = NdtreeIndex<3>; } // namespace wavemap -#include "wavemap/indexing/impl/ndtree_index_inl.h" +#include "wavemap/core/indexing/impl/ndtree_index_inl.h" #endif // WAVEMAP_INDEXING_NDTREE_INDEX_H_ diff --git a/libraries/wavemap/include/wavemap/integrator/impl/integrator_base_inl.h b/library/wavemap/core/integrator/impl/integrator_base_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/integrator/impl/integrator_base_inl.h rename to library/wavemap/core/integrator/impl/integrator_base_inl.h diff --git a/libraries/wavemap/src/integrator/integrator_base.cc b/library/wavemap/core/integrator/integrator_base.cc similarity index 87% rename from libraries/wavemap/src/integrator/integrator_base.cc rename to library/wavemap/core/integrator/integrator_base.cc index c06be09dd..a71dc273f 100644 --- a/libraries/wavemap/src/integrator/integrator_base.cc +++ b/library/wavemap/core/integrator/integrator_base.cc @@ -1,4 +1,4 @@ -#include "wavemap/integrator/integrator_base.h" +#include "wavemap/core/integrator/integrator_base.h" namespace wavemap { bool IntegratorBase::isPointcloudValid(const PosedPointcloud<>& pointcloud) { diff --git a/libraries/wavemap/include/wavemap/integrator/integrator_base.h b/library/wavemap/core/integrator/integrator_base.h similarity index 85% rename from libraries/wavemap/include/wavemap/integrator/integrator_base.h rename to library/wavemap/core/integrator/integrator_base.h index a58d9157c..92cb52c65 100644 --- a/libraries/wavemap/include/wavemap/integrator/integrator_base.h +++ b/library/wavemap/core/integrator/integrator_base.h @@ -4,10 +4,10 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/config/config_base.h" -#include "wavemap/config/type_selector.h" -#include "wavemap/data_structure/pointcloud.h" +#include "wavemap/core/common.h" +#include "wavemap/core/config/config_base.h" +#include "wavemap/core/config/type_selector.h" +#include "wavemap/core/data_structure/pointcloud.h" namespace wavemap { struct IntegratorType : TypeSelector { @@ -48,6 +48,6 @@ class IntegratorBase { }; } // namespace wavemap -#include "wavemap/integrator/impl/integrator_base_inl.h" +#include "wavemap/core/integrator/impl/integrator_base_inl.h" #endif // WAVEMAP_INTEGRATOR_INTEGRATOR_BASE_H_ diff --git a/libraries/wavemap/src/integrator/integrator_factory.cc b/library/wavemap/core/integrator/integrator_factory.cc similarity index 88% rename from libraries/wavemap/src/integrator/integrator_factory.cc rename to library/wavemap/core/integrator/integrator_factory.cc index 5b32fc52e..7f7e11540 100644 --- a/libraries/wavemap/src/integrator/integrator_factory.cc +++ b/library/wavemap/core/integrator/integrator_factory.cc @@ -1,14 +1,14 @@ -#include "wavemap/integrator/integrator_factory.h" +#include "wavemap/core/integrator/integrator_factory.h" -#include "wavemap/integrator/integrator_base.h" -#include "wavemap/integrator/measurement_model/measurement_model_factory.h" -#include "wavemap/integrator/projection_model/projector_factory.h" -#include "wavemap/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h" -#include "wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h" -#include "wavemap/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h" -#include "wavemap/integrator/projective/coarse_to_fine/wavelet_integrator.h" -#include "wavemap/integrator/projective/fixed_resolution/fixed_resolution_integrator.h" -#include "wavemap/integrator/ray_tracing/ray_tracing_integrator.h" +#include "wavemap/core/integrator/integrator_base.h" +#include "wavemap/core/integrator/measurement_model/measurement_model_factory.h" +#include "wavemap/core/integrator/projection_model/projector_factory.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.h" +#include "wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.h" +#include "wavemap/core/integrator/ray_tracing/ray_tracing_integrator.h" namespace wavemap { IntegratorBase::Ptr IntegratorFactory::create( diff --git a/libraries/wavemap/include/wavemap/integrator/integrator_factory.h b/library/wavemap/core/integrator/integrator_factory.h similarity index 83% rename from libraries/wavemap/include/wavemap/integrator/integrator_factory.h rename to library/wavemap/core/integrator/integrator_factory.h index 884560dd5..3ec4c5d7d 100644 --- a/libraries/wavemap/include/wavemap/integrator/integrator_factory.h +++ b/library/wavemap/core/integrator/integrator_factory.h @@ -4,9 +4,9 @@ #include #include -#include "wavemap/integrator/integrator_base.h" -#include "wavemap/map/map_base.h" -#include "wavemap/utils/thread_pool.h" +#include "wavemap/core/integrator/integrator_base.h" +#include "wavemap/core/map/map_base.h" +#include "wavemap/core/utils/thread_pool.h" namespace wavemap { class IntegratorFactory { diff --git a/libraries/wavemap/include/wavemap/integrator/measurement_model/approximate_gaussian_distribution.h b/library/wavemap/core/integrator/measurement_model/approximate_gaussian_distribution.h similarity index 96% rename from libraries/wavemap/include/wavemap/integrator/measurement_model/approximate_gaussian_distribution.h rename to library/wavemap/core/integrator/measurement_model/approximate_gaussian_distribution.h index c2ef42d4b..420129615 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/approximate_gaussian_distribution.h +++ b/library/wavemap/core/integrator/measurement_model/approximate_gaussian_distribution.h @@ -1,7 +1,7 @@ #ifndef WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_APPROXIMATE_GAUSSIAN_DISTRIBUTION_H_ #define WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_APPROXIMATE_GAUSSIAN_DISTRIBUTION_H_ -#include "wavemap/common.h" +#include "wavemap/core/common.h" namespace wavemap { struct ApproximateGaussianDistribution { diff --git a/libraries/wavemap/src/integrator/measurement_model/constant_ray.cc b/library/wavemap/core/integrator/measurement_model/constant_ray.cc similarity index 64% rename from libraries/wavemap/src/integrator/measurement_model/constant_ray.cc rename to library/wavemap/core/integrator/measurement_model/constant_ray.cc index aa904c38f..cb08bf1fa 100644 --- a/libraries/wavemap/src/integrator/measurement_model/constant_ray.cc +++ b/library/wavemap/core/integrator/measurement_model/constant_ray.cc @@ -1,4 +1,4 @@ -#include "wavemap/integrator/measurement_model/constant_ray.h" +#include "wavemap/core/integrator/measurement_model/constant_ray.h" namespace wavemap { DECLARE_CONFIG_MEMBERS(ConstantRayConfig, (log_odds_occupied)(log_odds_free)); diff --git a/libraries/wavemap/include/wavemap/integrator/measurement_model/constant_ray.h b/library/wavemap/core/integrator/measurement_model/constant_ray.h similarity index 94% rename from libraries/wavemap/include/wavemap/integrator/measurement_model/constant_ray.h rename to library/wavemap/core/integrator/measurement_model/constant_ray.h index fc4f02fa5..1699bcaa8 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/constant_ray.h +++ b/library/wavemap/core/integrator/measurement_model/constant_ray.h @@ -1,9 +1,9 @@ #ifndef WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_CONSTANT_RAY_H_ #define WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_CONSTANT_RAY_H_ -#include "wavemap/common.h" -#include "wavemap/config/config_base.h" -#include "wavemap/indexing/index_conversions.h" +#include "wavemap/core/common.h" +#include "wavemap/core/config/config_base.h" +#include "wavemap/core/indexing/index_conversions.h" namespace wavemap { /** diff --git a/libraries/wavemap/src/integrator/measurement_model/continuous_beam.cc b/library/wavemap/core/integrator/measurement_model/continuous_beam.cc similarity index 89% rename from libraries/wavemap/src/integrator/measurement_model/continuous_beam.cc rename to library/wavemap/core/integrator/measurement_model/continuous_beam.cc index f40e6260b..9bb441e14 100644 --- a/libraries/wavemap/src/integrator/measurement_model/continuous_beam.cc +++ b/library/wavemap/core/integrator/measurement_model/continuous_beam.cc @@ -1,4 +1,4 @@ -#include "wavemap/integrator/measurement_model/continuous_beam.h" +#include "wavemap/core/integrator/measurement_model/continuous_beam.h" namespace wavemap { DECLARE_CONFIG_MEMBERS(ContinuousBeamConfig, diff --git a/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_beam.h b/library/wavemap/core/integrator/measurement_model/continuous_beam.h similarity index 92% rename from libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_beam.h rename to library/wavemap/core/integrator/measurement_model/continuous_beam.h index fffa1abd8..9a30b6201 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_beam.h +++ b/library/wavemap/core/integrator/measurement_model/continuous_beam.h @@ -5,12 +5,12 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/config/config_base.h" -#include "wavemap/indexing/index_conversions.h" -#include "wavemap/integrator/measurement_model/measurement_model_base.h" -#include "wavemap/integrator/projective/update_type.h" -#include "wavemap/utils/print/eigen.h" +#include "wavemap/core/common.h" +#include "wavemap/core/config/config_base.h" +#include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/integrator/measurement_model/measurement_model_base.h" +#include "wavemap/core/integrator/projective/update_type.h" +#include "wavemap/core/utils/print/eigen.h" namespace wavemap { /** @@ -115,6 +115,6 @@ class ContinuousBeam : public MeasurementModelBase { }; } // namespace wavemap -#include "wavemap/integrator/measurement_model/impl/continuous_beam_inl.h" +#include "wavemap/core/integrator/measurement_model/impl/continuous_beam_inl.h" #endif // WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_CONTINUOUS_BEAM_H_ diff --git a/libraries/wavemap/src/integrator/measurement_model/continuous_ray.cc b/library/wavemap/core/integrator/measurement_model/continuous_ray.cc similarity index 88% rename from libraries/wavemap/src/integrator/measurement_model/continuous_ray.cc rename to library/wavemap/core/integrator/measurement_model/continuous_ray.cc index 8ee6ec767..c1998fbf6 100644 --- a/libraries/wavemap/src/integrator/measurement_model/continuous_ray.cc +++ b/library/wavemap/core/integrator/measurement_model/continuous_ray.cc @@ -1,4 +1,4 @@ -#include "wavemap/integrator/measurement_model/continuous_ray.h" +#include "wavemap/core/integrator/measurement_model/continuous_ray.h" namespace wavemap { DECLARE_CONFIG_MEMBERS(ContinuousRayConfig, diff --git a/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_ray.h b/library/wavemap/core/integrator/measurement_model/continuous_ray.h similarity index 92% rename from libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_ray.h rename to library/wavemap/core/integrator/measurement_model/continuous_ray.h index faef614fb..bc48e4802 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_ray.h +++ b/library/wavemap/core/integrator/measurement_model/continuous_ray.h @@ -4,10 +4,10 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/config/config_base.h" -#include "wavemap/integrator/measurement_model/measurement_model_base.h" -#include "wavemap/integrator/projective/update_type.h" +#include "wavemap/core/common.h" +#include "wavemap/core/config/config_base.h" +#include "wavemap/core/integrator/measurement_model/measurement_model_base.h" +#include "wavemap/core/integrator/projective/update_type.h" namespace wavemap { /** @@ -92,6 +92,6 @@ class ContinuousRay : public MeasurementModelBase { }; } // namespace wavemap -#include "wavemap/integrator/measurement_model/impl/continuous_ray_inl.h" +#include "wavemap/core/integrator/measurement_model/impl/continuous_ray_inl.h" #endif // WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_CONTINUOUS_RAY_H_ diff --git a/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_beam_inl.h b/library/wavemap/core/integrator/measurement_model/impl/continuous_beam_inl.h similarity index 98% rename from libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_beam_inl.h rename to library/wavemap/core/integrator/measurement_model/impl/continuous_beam_inl.h index a3bc6534b..e942374f2 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_beam_inl.h +++ b/library/wavemap/core/integrator/measurement_model/impl/continuous_beam_inl.h @@ -3,7 +3,7 @@ #include -#include "wavemap/integrator/measurement_model/approximate_gaussian_distribution.h" +#include "wavemap/core/integrator/measurement_model/approximate_gaussian_distribution.h" namespace wavemap { inline FloatingPoint ContinuousBeam::computeWorstCaseApproximationError( diff --git a/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_ray_inl.h b/library/wavemap/core/integrator/measurement_model/impl/continuous_ray_inl.h similarity index 97% rename from libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_ray_inl.h rename to library/wavemap/core/integrator/measurement_model/impl/continuous_ray_inl.h index 56d6ed701..33740bce9 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/impl/continuous_ray_inl.h +++ b/library/wavemap/core/integrator/measurement_model/impl/continuous_ray_inl.h @@ -1,7 +1,7 @@ #ifndef WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_IMPL_CONTINUOUS_RAY_INL_H_ #define WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_IMPL_CONTINUOUS_RAY_INL_H_ -#include "wavemap/integrator/measurement_model/approximate_gaussian_distribution.h" +#include "wavemap/core/integrator/measurement_model/approximate_gaussian_distribution.h" namespace wavemap { inline FloatingPoint ContinuousRay::computeWorstCaseApproximationError( diff --git a/libraries/wavemap/include/wavemap/integrator/measurement_model/measurement_model_base.h b/library/wavemap/core/integrator/measurement_model/measurement_model_base.h similarity index 85% rename from libraries/wavemap/include/wavemap/integrator/measurement_model/measurement_model_base.h rename to library/wavemap/core/integrator/measurement_model/measurement_model_base.h index 6df7fd50b..f129c22b1 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/measurement_model_base.h +++ b/library/wavemap/core/integrator/measurement_model/measurement_model_base.h @@ -3,11 +3,11 @@ #include -#include "wavemap/common.h" -#include "wavemap/config/type_selector.h" -#include "wavemap/data_structure/image.h" -#include "wavemap/integrator/projection_model/projector_base.h" -#include "wavemap/integrator/projective/update_type.h" +#include "wavemap/core/common.h" +#include "wavemap/core/config/type_selector.h" +#include "wavemap/core/data_structure/image.h" +#include "wavemap/core/integrator/projection_model/projector_base.h" +#include "wavemap/core/integrator/projective/update_type.h" namespace wavemap { struct MeasurementModelType : TypeSelector { diff --git a/libraries/wavemap/src/integrator/measurement_model/measurement_model_factory.cc b/library/wavemap/core/integrator/measurement_model/measurement_model_factory.cc similarity index 91% rename from libraries/wavemap/src/integrator/measurement_model/measurement_model_factory.cc rename to library/wavemap/core/integrator/measurement_model/measurement_model_factory.cc index e755650dc..b0aca32b4 100644 --- a/libraries/wavemap/src/integrator/measurement_model/measurement_model_factory.cc +++ b/library/wavemap/core/integrator/measurement_model/measurement_model_factory.cc @@ -1,7 +1,7 @@ -#include "wavemap/integrator/measurement_model/measurement_model_factory.h" +#include "wavemap/core/integrator/measurement_model/measurement_model_factory.h" -#include "wavemap/integrator/measurement_model/continuous_beam.h" -#include "wavemap/integrator/measurement_model/continuous_ray.h" +#include "wavemap/core/integrator/measurement_model/continuous_beam.h" +#include "wavemap/core/integrator/measurement_model/continuous_ray.h" namespace wavemap { MeasurementModelBase::Ptr wavemap::MeasurementModelFactory::create( diff --git a/libraries/wavemap/include/wavemap/integrator/measurement_model/measurement_model_factory.h b/library/wavemap/core/integrator/measurement_model/measurement_model_factory.h similarity index 92% rename from libraries/wavemap/include/wavemap/integrator/measurement_model/measurement_model_factory.h rename to library/wavemap/core/integrator/measurement_model/measurement_model_factory.h index f8040aec5..018f147e9 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/measurement_model_factory.h +++ b/library/wavemap/core/integrator/measurement_model/measurement_model_factory.h @@ -3,7 +3,7 @@ #include -#include "wavemap/integrator/measurement_model/measurement_model_base.h" +#include "wavemap/core/integrator/measurement_model/measurement_model_base.h" namespace wavemap { class MeasurementModelFactory { diff --git a/libraries/wavemap/src/integrator/projection_model/circular_projector.cc b/library/wavemap/core/integrator/projection_model/circular_projector.cc similarity index 89% rename from libraries/wavemap/src/integrator/projection_model/circular_projector.cc rename to library/wavemap/core/integrator/projection_model/circular_projector.cc index 895b31122..ae22f63f6 100644 --- a/libraries/wavemap/src/integrator/projection_model/circular_projector.cc +++ b/library/wavemap/core/integrator/projection_model/circular_projector.cc @@ -1,4 +1,4 @@ -#include "wavemap/integrator/projection_model/circular_projector.h" +#include "wavemap/core/integrator/projection_model/circular_projector.h" namespace wavemap { DECLARE_CONFIG_MEMBERS(CircularProjectorConfig, diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/circular_projector.h b/library/wavemap/core/integrator/projection_model/circular_projector.h similarity index 97% rename from libraries/wavemap/include/wavemap/integrator/projection_model/circular_projector.h rename to library/wavemap/core/integrator/projection_model/circular_projector.h index 1fdd4e308..17046c2cd 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/circular_projector.h +++ b/library/wavemap/core/integrator/projection_model/circular_projector.h @@ -1,8 +1,8 @@ #ifndef WAVEMAP_INTEGRATOR_PROJECTION_MODEL_CIRCULAR_PROJECTOR_H_ #define WAVEMAP_INTEGRATOR_PROJECTION_MODEL_CIRCULAR_PROJECTOR_H_ -#include "wavemap/common.h" -#include "wavemap/config/config_base.h" +#include "wavemap/core/common.h" +#include "wavemap/core/config/config_base.h" namespace wavemap { /** diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/ouster_projector_inl.h b/library/wavemap/core/integrator/projection_model/impl/ouster_projector_inl.h similarity index 98% rename from libraries/wavemap/include/wavemap/integrator/projection_model/impl/ouster_projector_inl.h rename to library/wavemap/core/integrator/projection_model/impl/ouster_projector_inl.h index d6124bbe4..92f31d777 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/ouster_projector_inl.h +++ b/library/wavemap/core/integrator/projection_model/impl/ouster_projector_inl.h @@ -1,7 +1,7 @@ #ifndef WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_OUSTER_PROJECTOR_INL_H_ #define WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_OUSTER_PROJECTOR_INL_H_ -#include "wavemap/utils/math/approximate_trigonometry.h" +#include "wavemap/core/utils/math/approximate_trigonometry.h" namespace wavemap { inline SensorCoordinates OusterProjector::cartesianToSensor( diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/pinhole_camera_projector_inl.h b/library/wavemap/core/integrator/projection_model/impl/pinhole_camera_projector_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/integrator/projection_model/impl/pinhole_camera_projector_inl.h rename to library/wavemap/core/integrator/projection_model/impl/pinhole_camera_projector_inl.h diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/projector_base_inl.h b/library/wavemap/core/integrator/projection_model/impl/projector_base_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/integrator/projection_model/impl/projector_base_inl.h rename to library/wavemap/core/integrator/projection_model/impl/projector_base_inl.h diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/spherical_projector_inl.h b/library/wavemap/core/integrator/projection_model/impl/spherical_projector_inl.h similarity index 97% rename from libraries/wavemap/include/wavemap/integrator/projection_model/impl/spherical_projector_inl.h rename to library/wavemap/core/integrator/projection_model/impl/spherical_projector_inl.h index eb6cf3eaa..4b58ee4fe 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/spherical_projector_inl.h +++ b/library/wavemap/core/integrator/projection_model/impl/spherical_projector_inl.h @@ -3,7 +3,7 @@ #include -#include "wavemap/utils/math/approximate_trigonometry.h" +#include "wavemap/core/utils/math/approximate_trigonometry.h" namespace wavemap { inline SensorCoordinates SphericalProjector::cartesianToSensor( diff --git a/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc b/library/wavemap/core/integrator/projection_model/ouster_projector.cc similarity index 97% rename from libraries/wavemap/src/integrator/projection_model/ouster_projector.cc rename to library/wavemap/core/integrator/projection_model/ouster_projector.cc index 0657c6f79..0e34e2608 100644 --- a/libraries/wavemap/src/integrator/projection_model/ouster_projector.cc +++ b/library/wavemap/core/integrator/projection_model/ouster_projector.cc @@ -1,6 +1,6 @@ -#include "wavemap/integrator/projection_model/ouster_projector.h" +#include "wavemap/core/integrator/projection_model/ouster_projector.h" -#include "wavemap/utils/math/angle_normalization.h" +#include "wavemap/core/utils/math/angle_normalization.h" namespace wavemap { OusterProjector::OusterProjector(const OusterProjector::Config& config) diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h b/library/wavemap/core/integrator/projection_model/ouster_projector.h similarity index 92% rename from libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h rename to library/wavemap/core/integrator/projection_model/ouster_projector.h index f800f6031..f685bc008 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h +++ b/library/wavemap/core/integrator/projection_model/ouster_projector.h @@ -4,9 +4,9 @@ #include #include -#include "wavemap/config/config_base.h" -#include "wavemap/integrator/projection_model/circular_projector.h" -#include "wavemap/integrator/projection_model/projector_base.h" +#include "wavemap/core/config/config_base.h" +#include "wavemap/core/integrator/projection_model/circular_projector.h" +#include "wavemap/core/integrator/projection_model/projector_base.h" namespace wavemap { /** @@ -82,6 +82,6 @@ class OusterProjector : public ProjectorBase { }; } // namespace wavemap -#include "wavemap/integrator/projection_model/impl/ouster_projector_inl.h" +#include "wavemap/core/integrator/projection_model/impl/ouster_projector_inl.h" #endif // WAVEMAP_INTEGRATOR_PROJECTION_MODEL_OUSTER_PROJECTOR_H_ diff --git a/libraries/wavemap/src/integrator/projection_model/pinhole_camera_projector.cc b/library/wavemap/core/integrator/projection_model/pinhole_camera_projector.cc similarity index 97% rename from libraries/wavemap/src/integrator/projection_model/pinhole_camera_projector.cc rename to library/wavemap/core/integrator/projection_model/pinhole_camera_projector.cc index 43f7d54d0..3d1999357 100644 --- a/libraries/wavemap/src/integrator/projection_model/pinhole_camera_projector.cc +++ b/library/wavemap/core/integrator/projection_model/pinhole_camera_projector.cc @@ -1,4 +1,4 @@ -#include "wavemap/integrator/projection_model/pinhole_camera_projector.h" +#include "wavemap/core/integrator/projection_model/pinhole_camera_projector.h" namespace wavemap { PinholeCameraProjector::PinholeCameraProjector( diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/pinhole_camera_projector.h b/library/wavemap/core/integrator/projection_model/pinhole_camera_projector.h similarity index 94% rename from libraries/wavemap/include/wavemap/integrator/projection_model/pinhole_camera_projector.h rename to library/wavemap/core/integrator/projection_model/pinhole_camera_projector.h index c99a603dc..06b3e5ecd 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/pinhole_camera_projector.h +++ b/library/wavemap/core/integrator/projection_model/pinhole_camera_projector.h @@ -3,8 +3,8 @@ #include -#include "wavemap/config/config_base.h" -#include "wavemap/integrator/projection_model/projector_base.h" +#include "wavemap/core/config/config_base.h" +#include "wavemap/core/integrator/projection_model/projector_base.h" namespace wavemap { /** @@ -88,6 +88,6 @@ class PinholeCameraProjector : public ProjectorBase { }; } // namespace wavemap -#include "wavemap/integrator/projection_model/impl/pinhole_camera_projector_inl.h" +#include "wavemap/core/integrator/projection_model/impl/pinhole_camera_projector_inl.h" #endif // WAVEMAP_INTEGRATOR_PROJECTION_MODEL_PINHOLE_CAMERA_PROJECTOR_H_ diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h b/library/wavemap/core/integrator/projection_model/projector_base.h similarity index 95% rename from libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h rename to library/wavemap/core/integrator/projection_model/projector_base.h index 69a0379d8..26ae0e523 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h +++ b/library/wavemap/core/integrator/projection_model/projector_base.h @@ -4,10 +4,10 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/config/type_selector.h" -#include "wavemap/config/value_with_unit.h" -#include "wavemap/data_structure/aabb.h" +#include "wavemap/core/common.h" +#include "wavemap/core/config/type_selector.h" +#include "wavemap/core/config/value_with_unit.h" +#include "wavemap/core/data_structure/aabb.h" namespace wavemap { struct ProjectorType : TypeSelector { @@ -130,6 +130,6 @@ class ProjectorBase { }; } // namespace wavemap -#include "wavemap/integrator/projection_model/impl/projector_base_inl.h" +#include "wavemap/core/integrator/projection_model/impl/projector_base_inl.h" #endif // WAVEMAP_INTEGRATOR_PROJECTION_MODEL_PROJECTOR_BASE_H_ diff --git a/libraries/wavemap/src/integrator/projection_model/projector_factory.cc b/library/wavemap/core/integrator/projection_model/projector_factory.cc similarity index 86% rename from libraries/wavemap/src/integrator/projection_model/projector_factory.cc rename to library/wavemap/core/integrator/projection_model/projector_factory.cc index b0916a148..5c3ab5802 100644 --- a/libraries/wavemap/src/integrator/projection_model/projector_factory.cc +++ b/library/wavemap/core/integrator/projection_model/projector_factory.cc @@ -1,8 +1,8 @@ -#include "wavemap/integrator/projection_model/projector_factory.h" +#include "wavemap/core/integrator/projection_model/projector_factory.h" -#include "wavemap/integrator/projection_model/ouster_projector.h" -#include "wavemap/integrator/projection_model/pinhole_camera_projector.h" -#include "wavemap/integrator/projection_model/spherical_projector.h" +#include "wavemap/core/integrator/projection_model/ouster_projector.h" +#include "wavemap/core/integrator/projection_model/pinhole_camera_projector.h" +#include "wavemap/core/integrator/projection_model/spherical_projector.h" namespace wavemap { ProjectorBase::Ptr wavemap::ProjectorFactory::create( diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/projector_factory.h b/library/wavemap/core/integrator/projection_model/projector_factory.h similarity index 88% rename from libraries/wavemap/include/wavemap/integrator/projection_model/projector_factory.h rename to library/wavemap/core/integrator/projection_model/projector_factory.h index 1431c3cbf..7c5f4d28d 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/projector_factory.h +++ b/library/wavemap/core/integrator/projection_model/projector_factory.h @@ -1,7 +1,7 @@ #ifndef WAVEMAP_INTEGRATOR_PROJECTION_MODEL_PROJECTOR_FACTORY_H_ #define WAVEMAP_INTEGRATOR_PROJECTION_MODEL_PROJECTOR_FACTORY_H_ -#include "wavemap/integrator/projection_model/projector_base.h" +#include "wavemap/core/integrator/projection_model/projector_base.h" namespace wavemap { class ProjectorFactory { diff --git a/libraries/wavemap/src/integrator/projection_model/spherical_projector.cc b/library/wavemap/core/integrator/projection_model/spherical_projector.cc similarity index 96% rename from libraries/wavemap/src/integrator/projection_model/spherical_projector.cc rename to library/wavemap/core/integrator/projection_model/spherical_projector.cc index ff8b9906c..2532561f5 100644 --- a/libraries/wavemap/src/integrator/projection_model/spherical_projector.cc +++ b/library/wavemap/core/integrator/projection_model/spherical_projector.cc @@ -1,6 +1,6 @@ -#include "wavemap/integrator/projection_model/spherical_projector.h" +#include "wavemap/core/integrator/projection_model/spherical_projector.h" -#include "wavemap/utils/math/angle_normalization.h" +#include "wavemap/core/utils/math/angle_normalization.h" namespace wavemap { SphericalProjector::SphericalProjector(const SphericalProjector::Config& config) diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h b/library/wavemap/core/integrator/projection_model/spherical_projector.h similarity index 90% rename from libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h rename to library/wavemap/core/integrator/projection_model/spherical_projector.h index 30099662c..61e19a756 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h +++ b/library/wavemap/core/integrator/projection_model/spherical_projector.h @@ -4,9 +4,9 @@ #include #include -#include "wavemap/config/config_base.h" -#include "wavemap/integrator/projection_model/circular_projector.h" -#include "wavemap/integrator/projection_model/projector_base.h" +#include "wavemap/core/config/config_base.h" +#include "wavemap/core/integrator/projection_model/circular_projector.h" +#include "wavemap/core/integrator/projection_model/projector_base.h" namespace wavemap { /** @@ -68,6 +68,6 @@ class SphericalProjector : public ProjectorBase { }; } // namespace wavemap -#include "wavemap/integrator/projection_model/impl/spherical_projector_inl.h" +#include "wavemap/core/integrator/projection_model/impl/spherical_projector_inl.h" #endif // WAVEMAP_INTEGRATOR_PROJECTION_MODEL_SPHERICAL_PROJECTOR_H_ diff --git a/libraries/wavemap/src/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.cc b/library/wavemap/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.cc similarity index 95% rename from libraries/wavemap/src/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.cc rename to library/wavemap/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.cc index 37def5bce..967194a8b 100644 --- a/libraries/wavemap/src/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.cc +++ b/library/wavemap/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.cc @@ -1,8 +1,8 @@ -#include "wavemap/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h" #include -#include "wavemap/indexing/ndtree_index.h" +#include "wavemap/core/indexing/ndtree_index.h" namespace wavemap { void CoarseToFineIntegrator::updateMap() { diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h b/library/wavemap/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h similarity index 84% rename from libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h rename to library/wavemap/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h index c0331635b..3f987421b 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h +++ b/library/wavemap/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h @@ -4,10 +4,10 @@ #include #include -#include "wavemap/integrator/projective/coarse_to_fine/range_image_intersector.h" -#include "wavemap/integrator/projective/projective_integrator.h" -#include "wavemap/integrator/projective/update_type.h" -#include "wavemap/map/volumetric_octree.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h" +#include "wavemap/core/integrator/projective/projective_integrator.h" +#include "wavemap/core/integrator/projective/update_type.h" +#include "wavemap/core/map/volumetric_octree.h" namespace wavemap { class CoarseToFineIntegrator : public ProjectiveIntegrator { diff --git a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc b/library/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc similarity index 98% rename from libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc rename to library/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc index c661a1436..1d638dc47 100644 --- a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc +++ b/library/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc @@ -1,4 +1,4 @@ -#include "wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h" #include diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h b/library/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h similarity index 88% rename from libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h rename to library/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h index 320875151..e957dc541 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h +++ b/library/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h @@ -5,11 +5,11 @@ #include #include -#include "wavemap/integrator/projective/coarse_to_fine/range_image_intersector.h" -#include "wavemap/integrator/projective/projective_integrator.h" -#include "wavemap/map/hashed_chunked_wavelet_octree.h" -#include "wavemap/map/map_base.h" -#include "wavemap/utils/thread_pool.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h" +#include "wavemap/core/integrator/projective/projective_integrator.h" +#include "wavemap/core/map/hashed_chunked_wavelet_octree.h" +#include "wavemap/core/map/map_base.h" +#include "wavemap/core/utils/thread_pool.h" namespace wavemap { class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { @@ -74,6 +74,6 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { }; } // namespace wavemap -#include "wavemap/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h" #endif // WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_CHUNKED_WAVELET_INTEGRATOR_H_ diff --git a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc b/library/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc similarity index 98% rename from libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc rename to library/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc index ac140a35a..8df7f71a6 100644 --- a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc +++ b/library/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc @@ -1,4 +1,4 @@ -#include "wavemap/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h" #include diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h b/library/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h similarity index 85% rename from libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h rename to library/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h index e5f39d16c..f0d9ff612 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h +++ b/library/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h @@ -5,11 +5,11 @@ #include #include -#include "wavemap/integrator/projective/coarse_to_fine/range_image_intersector.h" -#include "wavemap/integrator/projective/projective_integrator.h" -#include "wavemap/map/hashed_wavelet_octree.h" -#include "wavemap/map/map_base.h" -#include "wavemap/utils/thread_pool.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h" +#include "wavemap/core/integrator/projective/projective_integrator.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" +#include "wavemap/core/map/map_base.h" +#include "wavemap/core/utils/thread_pool.h" namespace wavemap { class HashedWaveletIntegrator : public ProjectiveIntegrator { @@ -56,6 +56,6 @@ class HashedWaveletIntegrator : public ProjectiveIntegrator { }; } // namespace wavemap -#include "wavemap/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h" #endif // WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_WAVELET_INTEGRATOR_H_ diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h b/library/wavemap/core/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h similarity index 95% rename from libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h rename to library/wavemap/core/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h index 676567251..5c79f1ab8 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h +++ b/library/wavemap/core/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h @@ -9,10 +9,10 @@ #include #include -#include "wavemap/data_structure/image.h" -#include "wavemap/indexing/ndtree_index.h" -#include "wavemap/integrator/projection_model/projector_base.h" -#include "wavemap/integrator/projective/update_type.h" +#include "wavemap/core/data_structure/image.h" +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/integrator/projection_model/projector_base.h" +#include "wavemap/core/integrator/projective/update_type.h" namespace wavemap { class HierarchicalRangeBounds { @@ -141,6 +141,6 @@ class HierarchicalRangeBounds { }; } // namespace wavemap -#include "wavemap/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h" #endif // WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HIERARCHICAL_RANGE_BOUNDS_H_ diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h b/library/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h rename to library/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h b/library/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h rename to library/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h b/library/wavemap/core/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h similarity index 99% rename from libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h rename to library/wavemap/core/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h index 62a56072a..4a2926147 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h +++ b/library/wavemap/core/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h @@ -4,8 +4,8 @@ #include #include -#include "wavemap/utils/bits/bit_operations.h" -#include "wavemap/utils/iterate/grid_iterator.h" +#include "wavemap/core/utils/bits/bit_operations.h" +#include "wavemap/core/utils/iterate/grid_iterator.h" namespace wavemap { inline Bounds HierarchicalRangeBounds::getBounds( diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h b/library/wavemap/core/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h similarity index 97% rename from libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h rename to library/wavemap/core/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h index 2bc330922..ca7cdd609 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h +++ b/library/wavemap/core/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h @@ -5,7 +5,7 @@ #include #include -#include "wavemap/integrator/measurement_model/continuous_beam.h" +#include "wavemap/core/integrator/measurement_model/continuous_beam.h" namespace wavemap { inline UpdateType RangeImageIntersector::determineUpdateType( diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h b/library/wavemap/core/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h rename to library/wavemap/core/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/range_image_intersector.h b/library/wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h similarity index 77% rename from libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/range_image_intersector.h rename to library/wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h index 86604b783..282e51aa2 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/range_image_intersector.h +++ b/library/wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h @@ -5,13 +5,13 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/data_structure/aabb.h" -#include "wavemap/data_structure/image.h" -#include "wavemap/integrator/measurement_model/measurement_model_base.h" -#include "wavemap/integrator/projection_model/projector_base.h" -#include "wavemap/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h" -#include "wavemap/integrator/projective/update_type.h" +#include "wavemap/core/common.h" +#include "wavemap/core/data_structure/aabb.h" +#include "wavemap/core/data_structure/image.h" +#include "wavemap/core/integrator/measurement_model/measurement_model_base.h" +#include "wavemap/core/integrator/projection_model/projector_base.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h" +#include "wavemap/core/integrator/projective/update_type.h" namespace wavemap { class RangeImageIntersector { @@ -48,6 +48,6 @@ class RangeImageIntersector { }; } // namespace wavemap -#include "wavemap/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h" #endif // WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_RANGE_IMAGE_INTERSECTOR_H_ diff --git a/libraries/wavemap/src/integrator/projective/coarse_to_fine/wavelet_integrator.cc b/library/wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.cc similarity index 93% rename from libraries/wavemap/src/integrator/projective/coarse_to_fine/wavelet_integrator.cc rename to library/wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.cc index f559ce488..45b77fc07 100644 --- a/libraries/wavemap/src/integrator/projective/coarse_to_fine/wavelet_integrator.cc +++ b/library/wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.cc @@ -1,4 +1,4 @@ -#include "wavemap/integrator/projective/coarse_to_fine/wavelet_integrator.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.h" namespace wavemap { void WaveletIntegrator::updateMap() { diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/wavelet_integrator.h b/library/wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.h similarity index 82% rename from libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/wavelet_integrator.h rename to library/wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.h index e1f32ef4c..3c2da3ff5 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/wavelet_integrator.h +++ b/library/wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.h @@ -4,10 +4,10 @@ #include #include -#include "wavemap/integrator/projective/coarse_to_fine/range_image_intersector.h" -#include "wavemap/integrator/projective/projective_integrator.h" -#include "wavemap/map/map_base.h" -#include "wavemap/map/wavelet_octree.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h" +#include "wavemap/core/integrator/projective/projective_integrator.h" +#include "wavemap/core/map/map_base.h" +#include "wavemap/core/map/wavelet_octree.h" namespace wavemap { class WaveletIntegrator : public ProjectiveIntegrator { @@ -41,6 +41,6 @@ class WaveletIntegrator : public ProjectiveIntegrator { }; } // namespace wavemap -#include "wavemap/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h" #endif // WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_WAVELET_INTEGRATOR_H_ diff --git a/libraries/wavemap/src/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc b/library/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc similarity index 95% rename from libraries/wavemap/src/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc rename to library/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc index 12d746873..e68f6bb4c 100644 --- a/libraries/wavemap/src/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc +++ b/library/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc @@ -1,7 +1,7 @@ -#include "wavemap/integrator/projective/fixed_resolution/fixed_resolution_integrator.h" +#include "wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.h" -#include "wavemap/indexing/index_conversions.h" -#include "wavemap/utils/iterate/grid_iterator.h" +#include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/utils/iterate/grid_iterator.h" namespace wavemap { void FixedResolutionIntegrator::importPointcloud( diff --git a/libraries/wavemap/include/wavemap/integrator/projective/fixed_resolution/fixed_resolution_integrator.h b/library/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.h similarity index 89% rename from libraries/wavemap/include/wavemap/integrator/projective/fixed_resolution/fixed_resolution_integrator.h rename to library/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.h index 6f329244d..8d81cf7f5 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/fixed_resolution/fixed_resolution_integrator.h +++ b/library/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.h @@ -4,9 +4,9 @@ #include #include -#include "wavemap/data_structure/aabb.h" -#include "wavemap/data_structure/image.h" -#include "wavemap/integrator/projective/projective_integrator.h" +#include "wavemap/core/data_structure/aabb.h" +#include "wavemap/core/data_structure/image.h" +#include "wavemap/core/integrator/projective/projective_integrator.h" namespace wavemap { class FixedResolutionIntegrator : public ProjectiveIntegrator { diff --git a/libraries/wavemap/include/wavemap/integrator/projective/impl/projective_integrator_inl.h b/library/wavemap/core/integrator/projective/impl/projective_integrator_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/integrator/projective/impl/projective_integrator_inl.h rename to library/wavemap/core/integrator/projective/impl/projective_integrator_inl.h diff --git a/libraries/wavemap/src/integrator/projective/projective_integrator.cc b/library/wavemap/core/integrator/projective/projective_integrator.cc similarity index 97% rename from libraries/wavemap/src/integrator/projective/projective_integrator.cc rename to library/wavemap/core/integrator/projective/projective_integrator.cc index f4dfe5a41..6165fea21 100644 --- a/libraries/wavemap/src/integrator/projective/projective_integrator.cc +++ b/library/wavemap/core/integrator/projective/projective_integrator.cc @@ -1,4 +1,4 @@ -#include "wavemap/integrator/projective/projective_integrator.h" +#include "wavemap/core/integrator/projective/projective_integrator.h" #include diff --git a/libraries/wavemap/include/wavemap/integrator/projective/projective_integrator.h b/library/wavemap/core/integrator/projective/projective_integrator.h similarity index 90% rename from libraries/wavemap/include/wavemap/integrator/projective/projective_integrator.h rename to library/wavemap/core/integrator/projective/projective_integrator.h index 5c50b926f..103c8ea6b 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/projective_integrator.h +++ b/library/wavemap/core/integrator/projective/projective_integrator.h @@ -4,12 +4,12 @@ #include #include -#include "wavemap/config/type_selector.h" -#include "wavemap/data_structure/image.h" -#include "wavemap/integrator/integrator_base.h" -#include "wavemap/integrator/measurement_model/measurement_model_base.h" -#include "wavemap/integrator/projection_model/projector_base.h" -#include "wavemap/map/map_base.h" +#include "wavemap/core/config/type_selector.h" +#include "wavemap/core/data_structure/image.h" +#include "wavemap/core/integrator/integrator_base.h" +#include "wavemap/core/integrator/measurement_model/measurement_model_base.h" +#include "wavemap/core/integrator/projection_model/projector_base.h" +#include "wavemap/core/map/map_base.h" namespace wavemap { /** @@ -101,6 +101,6 @@ class ProjectiveIntegrator : public IntegratorBase { }; } // namespace wavemap -#include "wavemap/integrator/projective/impl/projective_integrator_inl.h" +#include "wavemap/core/integrator/projective/impl/projective_integrator_inl.h" #endif // WAVEMAP_INTEGRATOR_PROJECTIVE_PROJECTIVE_INTEGRATOR_H_ diff --git a/libraries/wavemap/include/wavemap/integrator/projective/update_type.h b/library/wavemap/core/integrator/projective/update_type.h similarity index 88% rename from libraries/wavemap/include/wavemap/integrator/projective/update_type.h rename to library/wavemap/core/integrator/projective/update_type.h index 945012ca5..5dcadf12c 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/update_type.h +++ b/library/wavemap/core/integrator/projective/update_type.h @@ -3,8 +3,8 @@ #include -#include "wavemap/common.h" -#include "wavemap/config/type_selector.h" +#include "wavemap/core/common.h" +#include "wavemap/core/config/type_selector.h" namespace wavemap { struct UpdateType : TypeSelector { diff --git a/libraries/wavemap/src/integrator/ray_tracing/ray_tracing_integrator.cc b/library/wavemap/core/integrator/ray_tracing/ray_tracing_integrator.cc similarity index 95% rename from libraries/wavemap/src/integrator/ray_tracing/ray_tracing_integrator.cc rename to library/wavemap/core/integrator/ray_tracing/ray_tracing_integrator.cc index 492bb1ac3..40afa0e23 100644 --- a/libraries/wavemap/src/integrator/ray_tracing/ray_tracing_integrator.cc +++ b/library/wavemap/core/integrator/ray_tracing/ray_tracing_integrator.cc @@ -1,4 +1,4 @@ -#include "wavemap/integrator/ray_tracing/ray_tracing_integrator.h" +#include "wavemap/core/integrator/ray_tracing/ray_tracing_integrator.h" namespace wavemap { DECLARE_CONFIG_MEMBERS(RayTracingIntegratorConfig, diff --git a/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h b/library/wavemap/core/integrator/ray_tracing/ray_tracing_integrator.h similarity index 85% rename from libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h rename to library/wavemap/core/integrator/ray_tracing/ray_tracing_integrator.h index 580136cdc..04baac12f 100644 --- a/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h +++ b/library/wavemap/core/integrator/ray_tracing/ray_tracing_integrator.h @@ -3,10 +3,10 @@ #include -#include "wavemap/integrator/integrator_base.h" -#include "wavemap/integrator/measurement_model/constant_ray.h" -#include "wavemap/map/map_base.h" -#include "wavemap/utils/iterate/ray_iterator.h" +#include "wavemap/core/integrator/integrator_base.h" +#include "wavemap/core/integrator/measurement_model/constant_ray.h" +#include "wavemap/core/map/map_base.h" +#include "wavemap/core/utils/iterate/ray_iterator.h" namespace wavemap { /** diff --git a/libraries/wavemap/include/wavemap/map/cell_types/haar_coefficients.h b/library/wavemap/core/map/cell_types/haar_coefficients.h similarity index 97% rename from libraries/wavemap/include/wavemap/map/cell_types/haar_coefficients.h rename to library/wavemap/core/map/cell_types/haar_coefficients.h index 7c879f906..0067af0a1 100644 --- a/libraries/wavemap/include/wavemap/map/cell_types/haar_coefficients.h +++ b/library/wavemap/core/map/cell_types/haar_coefficients.h @@ -5,9 +5,9 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/indexing/ndtree_index.h" -#include "wavemap/utils/math/int_math.h" +#include "wavemap/core/common.h" +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/utils/math/int_math.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/map/cell_types/haar_transform.h b/library/wavemap/core/map/cell_types/haar_transform.h similarity index 96% rename from libraries/wavemap/include/wavemap/map/cell_types/haar_transform.h rename to library/wavemap/core/map/cell_types/haar_transform.h index 680acaeac..eb653129d 100644 --- a/libraries/wavemap/include/wavemap/map/cell_types/haar_transform.h +++ b/library/wavemap/core/map/cell_types/haar_transform.h @@ -3,9 +3,9 @@ #include -#include "wavemap/common.h" -#include "wavemap/indexing/ndtree_index.h" -#include "wavemap/map/cell_types/haar_coefficients.h" +#include "wavemap/core/common.h" +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/map/cell_types/haar_coefficients.h" namespace wavemap { // Transform 2^d scale space coefficients into 1 coarse scale and 2^d - 1 @@ -135,6 +135,6 @@ struct HaarTransform { }; } // namespace wavemap -#include "wavemap/map/cell_types/impl/haar_transform_inl.h" +#include "wavemap/core/map/cell_types/impl/haar_transform_inl.h" #endif // WAVEMAP_MAP_CELL_TYPES_HAAR_TRANSFORM_H_ diff --git a/libraries/wavemap/include/wavemap/map/cell_types/impl/haar_transform_inl.h b/library/wavemap/core/map/cell_types/impl/haar_transform_inl.h similarity index 98% rename from libraries/wavemap/include/wavemap/map/cell_types/impl/haar_transform_inl.h rename to library/wavemap/core/map/cell_types/impl/haar_transform_inl.h index e14434f57..5dccb2491 100644 --- a/libraries/wavemap/include/wavemap/map/cell_types/impl/haar_transform_inl.h +++ b/library/wavemap/core/map/cell_types/impl/haar_transform_inl.h @@ -1,8 +1,8 @@ #ifndef WAVEMAP_MAP_CELL_TYPES_IMPL_HAAR_TRANSFORM_INL_H_ #define WAVEMAP_MAP_CELL_TYPES_IMPL_HAAR_TRANSFORM_INL_H_ -#include "wavemap/utils/bits/bit_operations.h" -#include "wavemap/utils/math/int_math.h" +#include "wavemap/core/utils/bits/bit_operations.h" +#include "wavemap/core/utils/math/int_math.h" namespace wavemap { template diff --git a/libraries/wavemap/src/map/hashed_blocks.cc b/library/wavemap/core/map/hashed_blocks.cc similarity index 95% rename from libraries/wavemap/src/map/hashed_blocks.cc rename to library/wavemap/core/map/hashed_blocks.cc index 39dc32d5a..038db29f9 100644 --- a/libraries/wavemap/src/map/hashed_blocks.cc +++ b/library/wavemap/core/map/hashed_blocks.cc @@ -1,4 +1,4 @@ -#include "wavemap/map/hashed_blocks.h" +#include "wavemap/core/map/hashed_blocks.h" namespace wavemap { Index3D HashedBlocks::getMinIndex() const { diff --git a/libraries/wavemap/include/wavemap/map/hashed_blocks.h b/library/wavemap/core/map/hashed_blocks.h similarity index 88% rename from libraries/wavemap/include/wavemap/map/hashed_blocks.h rename to library/wavemap/core/map/hashed_blocks.h index 2ff42a324..e4ce5c66e 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_blocks.h +++ b/library/wavemap/core/map/hashed_blocks.h @@ -5,10 +5,10 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/config/config_base.h" -#include "wavemap/data_structure/dense_block_hash.h" -#include "wavemap/map/map_base.h" +#include "wavemap/core/common.h" +#include "wavemap/core/config/config_base.h" +#include "wavemap/core/data_structure/dense_block_hash.h" +#include "wavemap/core/map/map_base.h" namespace wavemap { class HashedBlocks : public MapBase, @@ -53,6 +53,6 @@ class HashedBlocks : public MapBase, }; } // namespace wavemap -#include "wavemap/map/impl/hashed_blocks_inl.h" +#include "wavemap/core/map/impl/hashed_blocks_inl.h" #endif // WAVEMAP_MAP_HASHED_BLOCKS_H_ diff --git a/libraries/wavemap/src/map/hashed_chunked_wavelet_octree.cc b/library/wavemap/core/map/hashed_chunked_wavelet_octree.cc similarity index 97% rename from libraries/wavemap/src/map/hashed_chunked_wavelet_octree.cc rename to library/wavemap/core/map/hashed_chunked_wavelet_octree.cc index c6f99d268..eb85bb34c 100644 --- a/libraries/wavemap/src/map/hashed_chunked_wavelet_octree.cc +++ b/library/wavemap/core/map/hashed_chunked_wavelet_octree.cc @@ -1,4 +1,4 @@ -#include "wavemap/map/hashed_chunked_wavelet_octree.h" +#include "wavemap/core/map/hashed_chunked_wavelet_octree.h" #include diff --git a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree.h b/library/wavemap/core/map/hashed_chunked_wavelet_octree.h similarity index 92% rename from libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree.h rename to library/wavemap/core/map/hashed_chunked_wavelet_octree.h index ebba6c066..47948299c 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree.h +++ b/library/wavemap/core/map/hashed_chunked_wavelet_octree.h @@ -4,13 +4,13 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/config/config_base.h" -#include "wavemap/data_structure/spatial_hash.h" -#include "wavemap/indexing/index_hashes.h" -#include "wavemap/map/hashed_chunked_wavelet_octree_block.h" -#include "wavemap/map/map_base.h" -#include "wavemap/utils/math/int_math.h" +#include "wavemap/core/common.h" +#include "wavemap/core/config/config_base.h" +#include "wavemap/core/data_structure/spatial_hash.h" +#include "wavemap/core/indexing/index_hashes.h" +#include "wavemap/core/map/hashed_chunked_wavelet_octree_block.h" +#include "wavemap/core/map/map_base.h" +#include "wavemap/core/utils/math/int_math.h" namespace wavemap { /** @@ -133,6 +133,6 @@ class HashedChunkedWaveletOctree : public MapBase { }; } // namespace wavemap -#include "wavemap/map/impl/hashed_chunked_wavelet_octree_inl.h" +#include "wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h" #endif // WAVEMAP_MAP_HASHED_CHUNKED_WAVELET_OCTREE_H_ diff --git a/libraries/wavemap/src/map/hashed_chunked_wavelet_octree_block.cc b/library/wavemap/core/map/hashed_chunked_wavelet_octree_block.cc similarity index 99% rename from libraries/wavemap/src/map/hashed_chunked_wavelet_octree_block.cc rename to library/wavemap/core/map/hashed_chunked_wavelet_octree_block.cc index 86399634b..99c6d5237 100644 --- a/libraries/wavemap/src/map/hashed_chunked_wavelet_octree_block.cc +++ b/library/wavemap/core/map/hashed_chunked_wavelet_octree_block.cc @@ -1,4 +1,4 @@ -#include "wavemap/map/hashed_chunked_wavelet_octree_block.h" +#include "wavemap/core/map/hashed_chunked_wavelet_octree_block.h" #include diff --git a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h b/library/wavemap/core/map/hashed_chunked_wavelet_octree_block.h similarity index 90% rename from libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h rename to library/wavemap/core/map/hashed_chunked_wavelet_octree_block.h index fe8fd3b49..e7a3b6024 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_chunked_wavelet_octree_block.h +++ b/library/wavemap/core/map/hashed_chunked_wavelet_octree_block.h @@ -1,12 +1,12 @@ #ifndef WAVEMAP_MAP_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_H_ #define WAVEMAP_MAP_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_H_ -#include "wavemap/common.h" -#include "wavemap/data_structure/chunked_ndtree/chunked_ndtree.h" -#include "wavemap/map/cell_types/haar_coefficients.h" -#include "wavemap/map/cell_types/haar_transform.h" -#include "wavemap/map/map_base.h" -#include "wavemap/utils/time/time.h" +#include "wavemap/core/common.h" +#include "wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h" +#include "wavemap/core/map/cell_types/haar_coefficients.h" +#include "wavemap/core/map/cell_types/haar_transform.h" +#include "wavemap/core/map/map_base.h" +#include "wavemap/core/utils/time/time.h" namespace wavemap { class HashedChunkedWaveletOctreeBlock { @@ -106,6 +106,6 @@ class HashedChunkedWaveletOctreeBlock { }; } // namespace wavemap -#include "wavemap/map/impl/hashed_chunked_wavelet_octree_block_inl.h" +#include "wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h" #endif // WAVEMAP_MAP_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_H_ diff --git a/libraries/wavemap/src/map/hashed_wavelet_octree.cc b/library/wavemap/core/map/hashed_wavelet_octree.cc similarity index 97% rename from libraries/wavemap/src/map/hashed_wavelet_octree.cc rename to library/wavemap/core/map/hashed_wavelet_octree.cc index bf1404693..4bcf0e2c8 100644 --- a/libraries/wavemap/src/map/hashed_wavelet_octree.cc +++ b/library/wavemap/core/map/hashed_wavelet_octree.cc @@ -1,4 +1,4 @@ -#include "wavemap/map/hashed_wavelet_octree.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" #include diff --git a/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h b/library/wavemap/core/map/hashed_wavelet_octree.h similarity index 92% rename from libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h rename to library/wavemap/core/map/hashed_wavelet_octree.h index f88ec4ca2..6f591f2a3 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree.h +++ b/library/wavemap/core/map/hashed_wavelet_octree.h @@ -4,13 +4,13 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/config/config_base.h" -#include "wavemap/data_structure/spatial_hash.h" -#include "wavemap/indexing/index_hashes.h" -#include "wavemap/map/hashed_wavelet_octree_block.h" -#include "wavemap/map/map_base.h" -#include "wavemap/utils/math/int_math.h" +#include "wavemap/core/common.h" +#include "wavemap/core/config/config_base.h" +#include "wavemap/core/data_structure/spatial_hash.h" +#include "wavemap/core/indexing/index_hashes.h" +#include "wavemap/core/map/hashed_wavelet_octree_block.h" +#include "wavemap/core/map/map_base.h" +#include "wavemap/core/utils/math/int_math.h" namespace wavemap { /** @@ -127,6 +127,6 @@ class HashedWaveletOctree : public MapBase { }; } // namespace wavemap -#include "wavemap/map/impl/hashed_wavelet_octree_inl.h" +#include "wavemap/core/map/impl/hashed_wavelet_octree_inl.h" #endif // WAVEMAP_MAP_HASHED_WAVELET_OCTREE_H_ diff --git a/libraries/wavemap/src/map/hashed_wavelet_octree_block.cc b/library/wavemap/core/map/hashed_wavelet_octree_block.cc similarity index 99% rename from libraries/wavemap/src/map/hashed_wavelet_octree_block.cc rename to library/wavemap/core/map/hashed_wavelet_octree_block.cc index 69c371083..fb7c6c0e7 100644 --- a/libraries/wavemap/src/map/hashed_wavelet_octree_block.cc +++ b/library/wavemap/core/map/hashed_wavelet_octree_block.cc @@ -1,4 +1,4 @@ -#include "wavemap/map/hashed_wavelet_octree_block.h" +#include "wavemap/core/map/hashed_wavelet_octree_block.h" #include diff --git a/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree_block.h b/library/wavemap/core/map/hashed_wavelet_octree_block.h similarity index 89% rename from libraries/wavemap/include/wavemap/map/hashed_wavelet_octree_block.h rename to library/wavemap/core/map/hashed_wavelet_octree_block.h index 0f1a86f91..c48ce7fea 100644 --- a/libraries/wavemap/include/wavemap/map/hashed_wavelet_octree_block.h +++ b/library/wavemap/core/map/hashed_wavelet_octree_block.h @@ -1,12 +1,12 @@ #ifndef WAVEMAP_MAP_HASHED_WAVELET_OCTREE_BLOCK_H_ #define WAVEMAP_MAP_HASHED_WAVELET_OCTREE_BLOCK_H_ -#include "wavemap/common.h" -#include "wavemap/data_structure/ndtree/ndtree.h" -#include "wavemap/map/cell_types/haar_coefficients.h" -#include "wavemap/map/cell_types/haar_transform.h" -#include "wavemap/map/map_base.h" -#include "wavemap/utils/time/time.h" +#include "wavemap/core/common.h" +#include "wavemap/core/data_structure/ndtree/ndtree.h" +#include "wavemap/core/map/cell_types/haar_coefficients.h" +#include "wavemap/core/map/cell_types/haar_transform.h" +#include "wavemap/core/map/map_base.h" +#include "wavemap/core/utils/time/time.h" namespace wavemap { class HashedWaveletOctreeBlock { @@ -85,6 +85,6 @@ class HashedWaveletOctreeBlock { }; } // namespace wavemap -#include "wavemap/map/impl/hashed_wavelet_octree_block_inl.h" +#include "wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h" #endif // WAVEMAP_MAP_HASHED_WAVELET_OCTREE_BLOCK_H_ diff --git a/libraries/wavemap/include/wavemap/map/impl/hashed_blocks_inl.h b/library/wavemap/core/map/impl/hashed_blocks_inl.h similarity index 87% rename from libraries/wavemap/include/wavemap/map/impl/hashed_blocks_inl.h rename to library/wavemap/core/map/impl/hashed_blocks_inl.h index f7f318133..c2ed13b9c 100644 --- a/libraries/wavemap/include/wavemap/map/impl/hashed_blocks_inl.h +++ b/library/wavemap/core/map/impl/hashed_blocks_inl.h @@ -5,8 +5,8 @@ #include #include -#include "wavemap/indexing/index_conversions.h" -#include "wavemap/utils/iterate/grid_iterator.h" +#include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/utils/iterate/grid_iterator.h" namespace wavemap { inline FloatingPoint HashedBlocks::getCellValue(const Index3D& index) const { diff --git a/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_block_inl.h b/library/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h similarity index 97% rename from libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_block_inl.h rename to library/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h index 14b654650..cd773432e 100644 --- a/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_block_inl.h +++ b/library/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h @@ -1,7 +1,7 @@ #ifndef WAVEMAP_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ #define WAVEMAP_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ -#include "wavemap/utils/query/occupancy_classifier.h" +#include "wavemap/core/utils/query/occupancy_classifier.h" namespace wavemap { inline bool HashedChunkedWaveletOctreeBlock::empty() const { diff --git a/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_inl.h b/library/wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h similarity index 98% rename from libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_inl.h rename to library/wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h index 2a60a6297..b1e91b741 100644 --- a/libraries/wavemap/include/wavemap/map/impl/hashed_chunked_wavelet_octree_inl.h +++ b/library/wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h @@ -1,7 +1,7 @@ #ifndef WAVEMAP_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_INL_H_ #define WAVEMAP_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_INL_H_ -#include "wavemap/indexing/index_conversions.h" +#include "wavemap/core/indexing/index_conversions.h" namespace wavemap { inline size_t HashedChunkedWaveletOctree::size() const { diff --git a/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_block_inl.h b/library/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h similarity index 96% rename from libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_block_inl.h rename to library/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h index 85cce0ae9..b6c71dc57 100644 --- a/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_block_inl.h +++ b/library/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h @@ -1,7 +1,7 @@ #ifndef WAVEMAP_MAP_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ #define WAVEMAP_MAP_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ -#include "wavemap/utils/query/occupancy_classifier.h" +#include "wavemap/core/utils/query/occupancy_classifier.h" namespace wavemap { inline bool HashedWaveletOctreeBlock::empty() const { diff --git a/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_inl.h b/library/wavemap/core/map/impl/hashed_wavelet_octree_inl.h similarity index 98% rename from libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_inl.h rename to library/wavemap/core/map/impl/hashed_wavelet_octree_inl.h index 197735874..4f8366264 100644 --- a/libraries/wavemap/include/wavemap/map/impl/hashed_wavelet_octree_inl.h +++ b/library/wavemap/core/map/impl/hashed_wavelet_octree_inl.h @@ -1,7 +1,7 @@ #ifndef WAVEMAP_MAP_IMPL_HASHED_WAVELET_OCTREE_INL_H_ #define WAVEMAP_MAP_IMPL_HASHED_WAVELET_OCTREE_INL_H_ -#include "wavemap/indexing/index_conversions.h" +#include "wavemap/core/indexing/index_conversions.h" namespace wavemap { inline size_t HashedWaveletOctree::size() const { diff --git a/libraries/wavemap/include/wavemap/map/impl/volumetric_octree_inl.h b/library/wavemap/core/map/impl/volumetric_octree_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/map/impl/volumetric_octree_inl.h rename to library/wavemap/core/map/impl/volumetric_octree_inl.h diff --git a/libraries/wavemap/include/wavemap/map/impl/wavelet_octree_inl.h b/library/wavemap/core/map/impl/wavelet_octree_inl.h similarity index 99% rename from libraries/wavemap/include/wavemap/map/impl/wavelet_octree_inl.h rename to library/wavemap/core/map/impl/wavelet_octree_inl.h index 0dabc6a77..2f2f3808f 100644 --- a/libraries/wavemap/include/wavemap/map/impl/wavelet_octree_inl.h +++ b/library/wavemap/core/map/impl/wavelet_octree_inl.h @@ -6,7 +6,7 @@ #include #include -#include "wavemap/utils/query/occupancy_classifier.h" +#include "wavemap/core/utils/query/occupancy_classifier.h" namespace wavemap { inline bool WaveletOctree::empty() const { diff --git a/libraries/wavemap/src/map/map_base.cc b/library/wavemap/core/map/map_base.cc similarity index 83% rename from libraries/wavemap/src/map/map_base.cc rename to library/wavemap/core/map/map_base.cc index 07d29c5d5..1c86f31d3 100644 --- a/libraries/wavemap/src/map/map_base.cc +++ b/library/wavemap/core/map/map_base.cc @@ -1,6 +1,6 @@ -#include "wavemap/map/map_base.h" +#include "wavemap/core/map/map_base.h" -#include "wavemap/utils/meta/nameof.h" +#include "wavemap/core/utils/meta/nameof.h" namespace wavemap { DECLARE_CONFIG_MEMBERS(MapBaseConfig, diff --git a/libraries/wavemap/include/wavemap/map/map_base.h b/library/wavemap/core/map/map_base.h similarity index 94% rename from libraries/wavemap/include/wavemap/map/map_base.h rename to library/wavemap/core/map/map_base.h index c8ff80adf..5c0de882e 100644 --- a/libraries/wavemap/include/wavemap/map/map_base.h +++ b/library/wavemap/core/map/map_base.h @@ -5,10 +5,10 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/config/config_base.h" -#include "wavemap/config/type_selector.h" -#include "wavemap/indexing/ndtree_index.h" +#include "wavemap/core/common.h" +#include "wavemap/core/config/config_base.h" +#include "wavemap/core/config/type_selector.h" +#include "wavemap/core/indexing/ndtree_index.h" namespace wavemap { struct MapType : TypeSelector { diff --git a/libraries/wavemap/src/map/map_factory.cc b/library/wavemap/core/map/map_factory.cc similarity index 90% rename from libraries/wavemap/src/map/map_factory.cc rename to library/wavemap/core/map/map_factory.cc index 01abe5d7c..011aa48b0 100644 --- a/libraries/wavemap/src/map/map_factory.cc +++ b/library/wavemap/core/map/map_factory.cc @@ -1,10 +1,10 @@ -#include "wavemap/map/map_factory.h" +#include "wavemap/core/map/map_factory.h" -#include "wavemap/map/hashed_blocks.h" -#include "wavemap/map/hashed_chunked_wavelet_octree.h" -#include "wavemap/map/hashed_wavelet_octree.h" -#include "wavemap/map/volumetric_octree.h" -#include "wavemap/map/wavelet_octree.h" +#include "wavemap/core/map/hashed_blocks.h" +#include "wavemap/core/map/hashed_chunked_wavelet_octree.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" +#include "wavemap/core/map/volumetric_octree.h" +#include "wavemap/core/map/wavelet_octree.h" namespace wavemap { MapBase::Ptr MapFactory::create(const param::Value& params, diff --git a/libraries/wavemap/include/wavemap/map/map_factory.h b/library/wavemap/core/map/map_factory.h similarity index 91% rename from libraries/wavemap/include/wavemap/map/map_factory.h rename to library/wavemap/core/map/map_factory.h index 5e63638c0..fbdd261c9 100644 --- a/libraries/wavemap/include/wavemap/map/map_factory.h +++ b/library/wavemap/core/map/map_factory.h @@ -3,7 +3,7 @@ #include -#include "wavemap/map/map_base.h" +#include "wavemap/core/map/map_base.h" namespace wavemap { class MapFactory { diff --git a/libraries/wavemap/src/map/volumetric_octree.cc b/library/wavemap/core/map/volumetric_octree.cc similarity index 98% rename from libraries/wavemap/src/map/volumetric_octree.cc rename to library/wavemap/core/map/volumetric_octree.cc index c43268c96..19a9038f9 100644 --- a/libraries/wavemap/src/map/volumetric_octree.cc +++ b/library/wavemap/core/map/volumetric_octree.cc @@ -1,6 +1,6 @@ -#include "wavemap/map/volumetric_octree.h" +#include "wavemap/core/map/volumetric_octree.h" -#include "wavemap/utils/query/occupancy_classifier.h" +#include "wavemap/core/utils/query/occupancy_classifier.h" namespace wavemap { DECLARE_CONFIG_MEMBERS(VolumetricOctreeConfig, diff --git a/libraries/wavemap/include/wavemap/map/volumetric_octree.h b/library/wavemap/core/map/volumetric_octree.h similarity index 93% rename from libraries/wavemap/include/wavemap/map/volumetric_octree.h rename to library/wavemap/core/map/volumetric_octree.h index 24e69e1f4..1eb566f70 100644 --- a/libraries/wavemap/include/wavemap/map/volumetric_octree.h +++ b/library/wavemap/core/map/volumetric_octree.h @@ -4,11 +4,11 @@ #include #include -#include "wavemap/config/config_base.h" -#include "wavemap/data_structure/ndtree/ndtree.h" -#include "wavemap/indexing/index_conversions.h" -#include "wavemap/indexing/ndtree_index.h" -#include "wavemap/map/map_base.h" +#include "wavemap/core/config/config_base.h" +#include "wavemap/core/data_structure/ndtree/ndtree.h" +#include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/map/map_base.h" namespace wavemap { /** @@ -130,6 +130,6 @@ class VolumetricOctree : public MapBase { }; } // namespace wavemap -#include "wavemap/map/impl/volumetric_octree_inl.h" +#include "wavemap/core/map/impl/volumetric_octree_inl.h" #endif // WAVEMAP_MAP_VOLUMETRIC_OCTREE_H_ diff --git a/libraries/wavemap/src/map/wavelet_octree.cc b/library/wavemap/core/map/wavelet_octree.cc similarity index 98% rename from libraries/wavemap/src/map/wavelet_octree.cc rename to library/wavemap/core/map/wavelet_octree.cc index 6b230035f..a71df3be6 100644 --- a/libraries/wavemap/src/map/wavelet_octree.cc +++ b/library/wavemap/core/map/wavelet_octree.cc @@ -1,6 +1,6 @@ -#include "wavemap/map/wavelet_octree.h" +#include "wavemap/core/map/wavelet_octree.h" -#include "wavemap/utils/query/occupancy_classifier.h" +#include "wavemap/core/utils/query/occupancy_classifier.h" namespace wavemap { DECLARE_CONFIG_MEMBERS(WaveletOctreeConfig, diff --git a/libraries/wavemap/include/wavemap/map/wavelet_octree.h b/library/wavemap/core/map/wavelet_octree.h similarity index 93% rename from libraries/wavemap/include/wavemap/map/wavelet_octree.h rename to library/wavemap/core/map/wavelet_octree.h index f55fcec4d..5c7bb2343 100644 --- a/libraries/wavemap/include/wavemap/map/wavelet_octree.h +++ b/library/wavemap/core/map/wavelet_octree.h @@ -4,12 +4,12 @@ #include #include -#include "wavemap/config/config_base.h" -#include "wavemap/data_structure/ndtree/ndtree.h" -#include "wavemap/indexing/index_conversions.h" -#include "wavemap/indexing/ndtree_index.h" -#include "wavemap/map/cell_types/haar_transform.h" -#include "wavemap/map/map_base.h" +#include "wavemap/core/config/config_base.h" +#include "wavemap/core/data_structure/ndtree/ndtree.h" +#include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/map/cell_types/haar_transform.h" +#include "wavemap/core/map/map_base.h" namespace wavemap { /** @@ -145,6 +145,6 @@ class WaveletOctree : public MapBase { }; } // namespace wavemap -#include "wavemap/map/impl/wavelet_octree_inl.h" +#include "wavemap/core/map/impl/wavelet_octree_inl.h" #endif // WAVEMAP_MAP_WAVELET_OCTREE_H_ diff --git a/libraries/wavemap/include/wavemap/utils/bits/bit_operations.h b/library/wavemap/core/utils/bits/bit_operations.h similarity index 99% rename from libraries/wavemap/include/wavemap/utils/bits/bit_operations.h rename to library/wavemap/core/utils/bits/bit_operations.h index 4036ea3e0..4e532b829 100644 --- a/libraries/wavemap/include/wavemap/utils/bits/bit_operations.h +++ b/library/wavemap/core/utils/bits/bit_operations.h @@ -1,7 +1,7 @@ #ifndef WAVEMAP_UTILS_BITS_BIT_OPERATIONS_H_ #define WAVEMAP_UTILS_BITS_BIT_OPERATIONS_H_ -#include "wavemap/common.h" +#include "wavemap/core/common.h" #if defined(__BMI2__) || defined(__AVX2__) #include diff --git a/libraries/wavemap/include/wavemap/utils/bits/morton_encoding.h b/library/wavemap/core/utils/bits/morton_encoding.h similarity index 98% rename from libraries/wavemap/include/wavemap/utils/bits/morton_encoding.h rename to library/wavemap/core/utils/bits/morton_encoding.h index ff92b6906..5eaccba4c 100644 --- a/libraries/wavemap/include/wavemap/utils/bits/morton_encoding.h +++ b/library/wavemap/core/utils/bits/morton_encoding.h @@ -3,8 +3,8 @@ #include -#include "wavemap/utils/bits/bit_operations.h" -#include "wavemap/utils/math/int_math.h" +#include "wavemap/core/utils/bits/bit_operations.h" +#include "wavemap/core/utils/math/int_math.h" namespace wavemap::morton { template diff --git a/libraries/wavemap/include/wavemap/utils/data/comparisons.h b/library/wavemap/core/utils/data/comparisons.h similarity index 98% rename from libraries/wavemap/include/wavemap/utils/data/comparisons.h rename to library/wavemap/core/utils/data/comparisons.h index ffad33312..34366923f 100644 --- a/libraries/wavemap/include/wavemap/utils/data/comparisons.h +++ b/library/wavemap/core/utils/data/comparisons.h @@ -5,7 +5,7 @@ #include #include -#include "wavemap/common.h" +#include "wavemap/core/common.h" namespace wavemap::data { // Test strict equality to zero diff --git a/libraries/wavemap/include/wavemap/utils/data/constants.h b/library/wavemap/core/utils/data/constants.h similarity index 100% rename from libraries/wavemap/include/wavemap/utils/data/constants.h rename to library/wavemap/core/utils/data/constants.h diff --git a/libraries/wavemap/include/wavemap/utils/data/eigen_checks.h b/library/wavemap/core/utils/data/eigen_checks.h similarity index 96% rename from libraries/wavemap/include/wavemap/utils/data/eigen_checks.h rename to library/wavemap/core/utils/data/eigen_checks.h index d67bc2a6b..1cdfd4659 100644 --- a/libraries/wavemap/include/wavemap/utils/data/eigen_checks.h +++ b/library/wavemap/core/utils/data/eigen_checks.h @@ -5,9 +5,9 @@ #include -#include "wavemap/common.h" -#include "wavemap/utils/data/comparisons.h" -#include "wavemap/utils/print/eigen.h" +#include "wavemap/core/common.h" +#include "wavemap/core/utils/data/comparisons.h" +#include "wavemap/core/utils/print/eigen.h" // Define regular checks #define CHECK_EIGEN_EQ(MatrixA, MatrixB) \ diff --git a/libraries/wavemap/include/wavemap/utils/data/fill.h b/library/wavemap/core/utils/data/fill.h similarity index 100% rename from libraries/wavemap/include/wavemap/utils/data/fill.h rename to library/wavemap/core/utils/data/fill.h diff --git a/libraries/wavemap/include/wavemap/utils/iterate/grid_iterator.h b/library/wavemap/core/utils/iterate/grid_iterator.h similarity index 98% rename from libraries/wavemap/include/wavemap/utils/iterate/grid_iterator.h rename to library/wavemap/core/utils/iterate/grid_iterator.h index 133372371..2c0c8aa02 100644 --- a/libraries/wavemap/include/wavemap/utils/iterate/grid_iterator.h +++ b/library/wavemap/core/utils/iterate/grid_iterator.h @@ -3,7 +3,7 @@ #include -#include "wavemap/common.h" +#include "wavemap/core/common.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/utils/iterate/impl/ray_iterator_inl.h b/library/wavemap/core/utils/iterate/impl/ray_iterator_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/utils/iterate/impl/ray_iterator_inl.h rename to library/wavemap/core/utils/iterate/impl/ray_iterator_inl.h diff --git a/libraries/wavemap/include/wavemap/utils/iterate/impl/subtree_iterator_inl.h b/library/wavemap/core/utils/iterate/impl/subtree_iterator_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/utils/iterate/impl/subtree_iterator_inl.h rename to library/wavemap/core/utils/iterate/impl/subtree_iterator_inl.h diff --git a/libraries/wavemap/include/wavemap/utils/iterate/pointcloud_iterator.h b/library/wavemap/core/utils/iterate/pointcloud_iterator.h similarity index 98% rename from libraries/wavemap/include/wavemap/utils/iterate/pointcloud_iterator.h rename to library/wavemap/core/utils/iterate/pointcloud_iterator.h index 21d1e3ab8..9ee610544 100644 --- a/libraries/wavemap/include/wavemap/utils/iterate/pointcloud_iterator.h +++ b/library/wavemap/core/utils/iterate/pointcloud_iterator.h @@ -3,7 +3,7 @@ #include -#include "wavemap/common.h" +#include "wavemap/core/common.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/utils/iterate/ray_iterator.h b/library/wavemap/core/utils/iterate/ray_iterator.h similarity index 93% rename from libraries/wavemap/include/wavemap/utils/iterate/ray_iterator.h rename to library/wavemap/core/utils/iterate/ray_iterator.h index 5e4973af1..d95886f7b 100644 --- a/libraries/wavemap/include/wavemap/utils/iterate/ray_iterator.h +++ b/library/wavemap/core/utils/iterate/ray_iterator.h @@ -1,8 +1,8 @@ #ifndef WAVEMAP_UTILS_ITERATE_RAY_ITERATOR_H_ #define WAVEMAP_UTILS_ITERATE_RAY_ITERATOR_H_ -#include "wavemap/common.h" -#include "wavemap/indexing/index_conversions.h" +#include "wavemap/core/common.h" +#include "wavemap/core/indexing/index_conversions.h" namespace wavemap { // NOTE: The ray casting code in this class is largely based on voxblox's @@ -66,6 +66,6 @@ class Ray { }; } // namespace wavemap -#include "wavemap/utils/iterate/impl/ray_iterator_inl.h" +#include "wavemap/core/utils/iterate/impl/ray_iterator_inl.h" #endif // WAVEMAP_UTILS_ITERATE_RAY_ITERATOR_H_ diff --git a/libraries/wavemap/include/wavemap/utils/iterate/subtree_iterator.h b/library/wavemap/core/utils/iterate/subtree_iterator.h similarity index 96% rename from libraries/wavemap/include/wavemap/utils/iterate/subtree_iterator.h rename to library/wavemap/core/utils/iterate/subtree_iterator.h index 6cb0ddca1..03c60c187 100644 --- a/libraries/wavemap/include/wavemap/utils/iterate/subtree_iterator.h +++ b/library/wavemap/core/utils/iterate/subtree_iterator.h @@ -3,8 +3,8 @@ #include -#include "wavemap/data_structure/ndtree/ndtree_node.h" -#include "wavemap/indexing/ndtree_index.h" +#include "wavemap/core/data_structure/ndtree/ndtree_node.h" +#include "wavemap/core/indexing/ndtree_index.h" namespace wavemap { // TODO(victorr): Add iterative deepening DFS as an alternative to BFS that uses @@ -134,6 +134,6 @@ class Subtree { }; } // namespace wavemap -#include "wavemap/utils/iterate/impl/subtree_iterator_inl.h" +#include "wavemap/core/utils/iterate/impl/subtree_iterator_inl.h" #endif // WAVEMAP_UTILS_ITERATE_SUBTREE_ITERATOR_H_ diff --git a/libraries/wavemap/include/wavemap/utils/math/angle_normalization.h b/library/wavemap/core/utils/math/angle_normalization.h similarity index 93% rename from libraries/wavemap/include/wavemap/utils/math/angle_normalization.h rename to library/wavemap/core/utils/math/angle_normalization.h index 78a6736ed..475a56009 100644 --- a/libraries/wavemap/include/wavemap/utils/math/angle_normalization.h +++ b/library/wavemap/core/utils/math/angle_normalization.h @@ -1,7 +1,7 @@ #ifndef WAVEMAP_UTILS_MATH_ANGLE_NORMALIZATION_H_ #define WAVEMAP_UTILS_MATH_ANGLE_NORMALIZATION_H_ -#include "wavemap/common.h" +#include "wavemap/core/common.h" namespace wavemap::angle_math { inline FloatingPoint normalize(FloatingPoint angle) { diff --git a/libraries/wavemap/include/wavemap/utils/math/approximate_trigonometry.h b/library/wavemap/core/utils/math/approximate_trigonometry.h similarity index 99% rename from libraries/wavemap/include/wavemap/utils/math/approximate_trigonometry.h rename to library/wavemap/core/utils/math/approximate_trigonometry.h index 4c47c5ff5..5f9da273f 100644 --- a/libraries/wavemap/include/wavemap/utils/math/approximate_trigonometry.h +++ b/library/wavemap/core/utils/math/approximate_trigonometry.h @@ -4,7 +4,7 @@ #include #include -#include "wavemap/common.h" +#include "wavemap/core/common.h" namespace wavemap::approximate { struct atan : public std::unary_function { diff --git a/libraries/wavemap/include/wavemap/utils/math/int_math.h b/library/wavemap/core/utils/math/int_math.h similarity index 97% rename from libraries/wavemap/include/wavemap/utils/math/int_math.h rename to library/wavemap/core/utils/math/int_math.h index 598bb2f99..6ead9a46f 100644 --- a/libraries/wavemap/include/wavemap/utils/math/int_math.h +++ b/library/wavemap/core/utils/math/int_math.h @@ -3,8 +3,8 @@ #include -#include "wavemap/common.h" -#include "wavemap/utils/bits/bit_operations.h" +#include "wavemap/core/common.h" +#include "wavemap/core/utils/bits/bit_operations.h" namespace wavemap::int_math { template diff --git a/libraries/wavemap/include/wavemap/utils/math/tree_math.h b/library/wavemap/core/utils/math/tree_math.h similarity index 95% rename from libraries/wavemap/include/wavemap/utils/math/tree_math.h rename to library/wavemap/core/utils/math/tree_math.h index 9dce143bd..8870bfedc 100644 --- a/libraries/wavemap/include/wavemap/utils/math/tree_math.h +++ b/library/wavemap/core/utils/math/tree_math.h @@ -1,7 +1,7 @@ #ifndef WAVEMAP_UTILS_MATH_TREE_MATH_H_ #define WAVEMAP_UTILS_MATH_TREE_MATH_H_ -#include "wavemap/utils/math/int_math.h" +#include "wavemap/core/utils/math/int_math.h" namespace wavemap::tree_math::perfect_tree { template diff --git a/libraries/wavemap/include/wavemap/utils/meta/nameof.h b/library/wavemap/core/utils/meta/nameof.h similarity index 100% rename from libraries/wavemap/include/wavemap/utils/meta/nameof.h rename to library/wavemap/core/utils/meta/nameof.h diff --git a/libraries/wavemap/include/wavemap/utils/meta/type_utils.h b/library/wavemap/core/utils/meta/type_utils.h similarity index 100% rename from libraries/wavemap/include/wavemap/utils/meta/type_utils.h rename to library/wavemap/core/utils/meta/type_utils.h diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/adjacency.h b/library/wavemap/core/utils/neighbors/adjacency.h similarity index 83% rename from libraries/wavemap/include/wavemap/utils/neighbors/adjacency.h rename to library/wavemap/core/utils/neighbors/adjacency.h index 3dcc4334b..51aec4d8a 100644 --- a/libraries/wavemap/include/wavemap/utils/neighbors/adjacency.h +++ b/library/wavemap/core/utils/neighbors/adjacency.h @@ -1,8 +1,8 @@ #ifndef WAVEMAP_UTILS_NEIGHBORS_ADJACENCY_H_ #define WAVEMAP_UTILS_NEIGHBORS_ADJACENCY_H_ -#include "wavemap/common.h" -#include "wavemap/config/type_selector.h" +#include "wavemap/core/common.h" +#include "wavemap/core/config/type_selector.h" namespace wavemap { struct Adjacency : TypeSelector { @@ -30,6 +30,6 @@ struct Adjacency : TypeSelector { }; } // namespace wavemap -#include "wavemap/utils/neighbors/impl/adjacency_inl.h" +#include "wavemap/core/utils/neighbors/impl/adjacency_inl.h" #endif // WAVEMAP_UTILS_NEIGHBORS_ADJACENCY_H_ diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/grid_adjacency.h b/library/wavemap/core/utils/neighbors/grid_adjacency.h similarity index 90% rename from libraries/wavemap/include/wavemap/utils/neighbors/grid_adjacency.h rename to library/wavemap/core/utils/neighbors/grid_adjacency.h index 6c20d8cd4..b07d8477d 100644 --- a/libraries/wavemap/include/wavemap/utils/neighbors/grid_adjacency.h +++ b/library/wavemap/core/utils/neighbors/grid_adjacency.h @@ -1,8 +1,8 @@ #ifndef WAVEMAP_UTILS_NEIGHBORS_GRID_ADJACENCY_H_ #define WAVEMAP_UTILS_NEIGHBORS_GRID_ADJACENCY_H_ -#include "wavemap/common.h" -#include "wavemap/utils/neighbors/adjacency.h" +#include "wavemap/core/common.h" +#include "wavemap/core/utils/neighbors/adjacency.h" namespace wavemap { template @@ -39,6 +39,6 @@ constexpr bool isAdjacent(const Index& index_offset, T adjacency_mask) = delete; } // namespace wavemap -#include "wavemap/utils/neighbors/impl/grid_adjacency_inl.h" +#include "wavemap/core/utils/neighbors/impl/grid_adjacency_inl.h" #endif // WAVEMAP_UTILS_NEIGHBORS_GRID_ADJACENCY_H_ diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/grid_neighborhood.h b/library/wavemap/core/utils/neighbors/grid_neighborhood.h similarity index 68% rename from libraries/wavemap/include/wavemap/utils/neighbors/grid_neighborhood.h rename to library/wavemap/core/utils/neighbors/grid_neighborhood.h index e9cf3d3f1..00bae2a38 100644 --- a/libraries/wavemap/include/wavemap/utils/neighbors/grid_neighborhood.h +++ b/library/wavemap/core/utils/neighbors/grid_neighborhood.h @@ -1,11 +1,11 @@ #ifndef WAVEMAP_UTILS_NEIGHBORS_GRID_NEIGHBORHOOD_H_ #define WAVEMAP_UTILS_NEIGHBORS_GRID_NEIGHBORHOOD_H_ -#include "wavemap/common.h" -#include "wavemap/utils/iterate/grid_iterator.h" -#include "wavemap/utils/math/int_math.h" -#include "wavemap/utils/neighbors/adjacency.h" -#include "wavemap/utils/neighbors/grid_adjacency.h" +#include "wavemap/core/common.h" +#include "wavemap/core/utils/iterate/grid_iterator.h" +#include "wavemap/core/utils/math/int_math.h" +#include "wavemap/core/utils/neighbors/adjacency.h" +#include "wavemap/core/utils/neighbors/grid_adjacency.h" namespace wavemap { template @@ -22,6 +22,6 @@ struct GridNeighborhood { }; } // namespace wavemap -#include "wavemap/utils/neighbors/impl/grid_neighborhood_inl.h" +#include "wavemap/core/utils/neighbors/impl/grid_neighborhood_inl.h" #endif // WAVEMAP_UTILS_NEIGHBORS_GRID_NEIGHBORHOOD_H_ diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/impl/adjacency_inl.h b/library/wavemap/core/utils/neighbors/impl/adjacency_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/utils/neighbors/impl/adjacency_inl.h rename to library/wavemap/core/utils/neighbors/impl/adjacency_inl.h diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_adjacency_inl.h b/library/wavemap/core/utils/neighbors/impl/grid_adjacency_inl.h similarity index 97% rename from libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_adjacency_inl.h rename to library/wavemap/core/utils/neighbors/impl/grid_adjacency_inl.h index 663d15fa8..0500200e5 100644 --- a/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_adjacency_inl.h +++ b/library/wavemap/core/utils/neighbors/impl/grid_adjacency_inl.h @@ -1,7 +1,7 @@ #ifndef WAVEMAP_UTILS_NEIGHBORS_IMPL_GRID_ADJACENCY_INL_H_ #define WAVEMAP_UTILS_NEIGHBORS_IMPL_GRID_ADJACENCY_INL_H_ -#include "wavemap/utils/bits/bit_operations.h" +#include "wavemap/core/utils/bits/bit_operations.h" namespace wavemap { template diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_neighborhood_inl.h b/library/wavemap/core/utils/neighbors/impl/grid_neighborhood_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/utils/neighbors/impl/grid_neighborhood_inl.h rename to library/wavemap/core/utils/neighbors/impl/grid_neighborhood_inl.h diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/impl/ndtree_adjacency_inl.h b/library/wavemap/core/utils/neighbors/impl/ndtree_adjacency_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/utils/neighbors/impl/ndtree_adjacency_inl.h rename to library/wavemap/core/utils/neighbors/impl/ndtree_adjacency_inl.h diff --git a/libraries/wavemap/include/wavemap/utils/neighbors/ndtree_adjacency.h b/library/wavemap/core/utils/neighbors/ndtree_adjacency.h similarity index 88% rename from libraries/wavemap/include/wavemap/utils/neighbors/ndtree_adjacency.h rename to library/wavemap/core/utils/neighbors/ndtree_adjacency.h index e252abcc3..048e00a44 100644 --- a/libraries/wavemap/include/wavemap/utils/neighbors/ndtree_adjacency.h +++ b/library/wavemap/core/utils/neighbors/ndtree_adjacency.h @@ -1,8 +1,8 @@ #ifndef WAVEMAP_UTILS_NEIGHBORS_NDTREE_ADJACENCY_H_ #define WAVEMAP_UTILS_NEIGHBORS_NDTREE_ADJACENCY_H_ -#include "wavemap/common.h" -#include "wavemap/indexing/ndtree_index.h" +#include "wavemap/core/common.h" +#include "wavemap/core/indexing/ndtree_index.h" namespace wavemap { // Return true if node_1 and node_2 touch or overlap. @@ -28,6 +28,6 @@ static bool areAdjacent(const NdtreeIndex& index_1, } } // namespace wavemap -#include "wavemap/utils/neighbors/impl/ndtree_adjacency_inl.h" +#include "wavemap/core/utils/neighbors/impl/ndtree_adjacency_inl.h" #endif // WAVEMAP_UTILS_NEIGHBORS_NDTREE_ADJACENCY_H_ diff --git a/libraries/wavemap/include/wavemap/utils/print/container.h b/library/wavemap/core/utils/print/container.h similarity index 100% rename from libraries/wavemap/include/wavemap/utils/print/container.h rename to library/wavemap/core/utils/print/container.h diff --git a/libraries/wavemap/include/wavemap/utils/print/eigen.h b/library/wavemap/core/utils/print/eigen.h similarity index 100% rename from libraries/wavemap/include/wavemap/utils/print/eigen.h rename to library/wavemap/core/utils/print/eigen.h diff --git a/libraries/wavemap/src/utils/query/classified_map.cc b/library/wavemap/core/utils/query/classified_map.cc similarity index 99% rename from libraries/wavemap/src/utils/query/classified_map.cc rename to library/wavemap/core/utils/query/classified_map.cc index f2228e2bb..f95758c02 100644 --- a/libraries/wavemap/src/utils/query/classified_map.cc +++ b/library/wavemap/core/utils/query/classified_map.cc @@ -1,4 +1,4 @@ -#include "wavemap/utils/query/classified_map.h" +#include "wavemap/core/utils/query/classified_map.h" #include diff --git a/libraries/wavemap/include/wavemap/utils/query/classified_map.h b/library/wavemap/core/utils/query/classified_map.h similarity index 94% rename from libraries/wavemap/include/wavemap/utils/query/classified_map.h rename to library/wavemap/core/utils/query/classified_map.h index 4d0f23a80..394e1220a 100644 --- a/libraries/wavemap/include/wavemap/utils/query/classified_map.h +++ b/library/wavemap/core/utils/query/classified_map.h @@ -5,12 +5,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include "wavemap/core/data_structure/ndtree/ndtree.h" +#include "wavemap/core/data_structure/ndtree_block_hash.h" +#include "wavemap/core/map/hashed_blocks.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" +#include "wavemap/core/utils/query/occupancy_classifier.h" +#include "wavemap/core/utils/query/query_accelerator.h" namespace wavemap { struct ChildBitset { @@ -160,6 +160,6 @@ class ClassifiedMap { }; } // namespace wavemap -#include "wavemap/utils/query/impl/classified_map_inl.h" +#include "wavemap/core/utils/query/impl/classified_map_inl.h" #endif // WAVEMAP_UTILS_QUERY_CLASSIFIED_MAP_H_ diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h b/library/wavemap/core/utils/query/impl/classified_map_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/utils/query/impl/classified_map_inl.h rename to library/wavemap/core/utils/query/impl/classified_map_inl.h diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_classifier_inl.h b/library/wavemap/core/utils/query/impl/occupancy_classifier_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/utils/query/impl/occupancy_classifier_inl.h rename to library/wavemap/core/utils/query/impl/occupancy_classifier_inl.h diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/occupancy_inl.h b/library/wavemap/core/utils/query/impl/occupancy_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/utils/query/impl/occupancy_inl.h rename to library/wavemap/core/utils/query/impl/occupancy_inl.h diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/point_sampler_inl.h b/library/wavemap/core/utils/query/impl/point_sampler_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/utils/query/impl/point_sampler_inl.h rename to library/wavemap/core/utils/query/impl/point_sampler_inl.h diff --git a/libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h b/library/wavemap/core/utils/query/impl/query_accelerator_inl.h similarity index 100% rename from libraries/wavemap/include/wavemap/utils/query/impl/query_accelerator_inl.h rename to library/wavemap/core/utils/query/impl/query_accelerator_inl.h diff --git a/libraries/wavemap/include/wavemap/utils/query/map_interpolator.h b/library/wavemap/core/utils/query/map_interpolator.h similarity index 93% rename from libraries/wavemap/include/wavemap/utils/query/map_interpolator.h rename to library/wavemap/core/utils/query/map_interpolator.h index 3acb3cfd0..0f57271db 100644 --- a/libraries/wavemap/include/wavemap/utils/query/map_interpolator.h +++ b/library/wavemap/core/utils/query/map_interpolator.h @@ -1,10 +1,10 @@ #ifndef WAVEMAP_UTILS_QUERY_MAP_INTERPOLATOR_H_ #define WAVEMAP_UTILS_QUERY_MAP_INTERPOLATOR_H_ -#include "wavemap/common.h" -#include "wavemap/indexing/index_conversions.h" -#include "wavemap/map/map_base.h" -#include "wavemap/utils/data/eigen_checks.h" +#include "wavemap/core/common.h" +#include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/map/map_base.h" +#include "wavemap/core/utils/data/eigen_checks.h" namespace wavemap::interpolate { FloatingPoint trilinear(const wavemap::MapBase& map, diff --git a/libraries/wavemap/include/wavemap/utils/query/occupancy.h b/library/wavemap/core/utils/query/occupancy.h similarity index 84% rename from libraries/wavemap/include/wavemap/utils/query/occupancy.h rename to library/wavemap/core/utils/query/occupancy.h index ee2a5d13d..e65c7eff9 100644 --- a/libraries/wavemap/include/wavemap/utils/query/occupancy.h +++ b/library/wavemap/core/utils/query/occupancy.h @@ -1,8 +1,8 @@ #ifndef WAVEMAP_UTILS_QUERY_OCCUPANCY_H_ #define WAVEMAP_UTILS_QUERY_OCCUPANCY_H_ -#include "wavemap/common.h" -#include "wavemap/config/type_selector.h" +#include "wavemap/core/common.h" +#include "wavemap/core/config/type_selector.h" namespace wavemap { struct Occupancy : TypeSelector { @@ -20,6 +20,6 @@ struct Occupancy : TypeSelector { }; } // namespace wavemap -#include "wavemap/utils/query/impl/occupancy_inl.h" +#include "wavemap/core/utils/query/impl/occupancy_inl.h" #endif // WAVEMAP_UTILS_QUERY_OCCUPANCY_H_ diff --git a/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h b/library/wavemap/core/utils/query/occupancy_classifier.h similarity index 94% rename from libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h rename to library/wavemap/core/utils/query/occupancy_classifier.h index 96521e32f..dfd4a24d4 100644 --- a/libraries/wavemap/include/wavemap/utils/query/occupancy_classifier.h +++ b/library/wavemap/core/utils/query/occupancy_classifier.h @@ -1,7 +1,7 @@ #ifndef WAVEMAP_UTILS_QUERY_OCCUPANCY_CLASSIFIER_H_ #define WAVEMAP_UTILS_QUERY_OCCUPANCY_CLASSIFIER_H_ -#include "wavemap/utils/query/occupancy.h" +#include "wavemap/core/utils/query/occupancy.h" namespace wavemap { class OccupancyClassifier { @@ -45,6 +45,6 @@ class OccupancyClassifier { }; } // namespace wavemap -#include "wavemap/utils/query/impl/occupancy_classifier_inl.h" +#include "wavemap/core/utils/query/impl/occupancy_classifier_inl.h" #endif // WAVEMAP_UTILS_QUERY_OCCUPANCY_CLASSIFIER_H_ diff --git a/libraries/wavemap/src/utils/query/point_sampler.cc b/library/wavemap/core/utils/query/point_sampler.cc similarity index 95% rename from libraries/wavemap/src/utils/query/point_sampler.cc rename to library/wavemap/core/utils/query/point_sampler.cc index 070b6a068..0af04b72a 100644 --- a/libraries/wavemap/src/utils/query/point_sampler.cc +++ b/library/wavemap/core/utils/query/point_sampler.cc @@ -1,4 +1,4 @@ -#include "wavemap/utils/query/point_sampler.h" +#include "wavemap/core/utils/query/point_sampler.h" namespace wavemap { std::optional PointSampler::getRandomBlock() { diff --git a/libraries/wavemap/include/wavemap/utils/query/point_sampler.h b/library/wavemap/core/utils/query/point_sampler.h similarity index 83% rename from libraries/wavemap/include/wavemap/utils/query/point_sampler.h rename to library/wavemap/core/utils/query/point_sampler.h index 196b687e7..09b29ff38 100644 --- a/libraries/wavemap/include/wavemap/utils/query/point_sampler.h +++ b/library/wavemap/core/utils/query/point_sampler.h @@ -4,11 +4,11 @@ #include #include -#include "wavemap/common.h" -#include "wavemap/data_structure/aabb.h" -#include "wavemap/utils/query/classified_map.h" -#include "wavemap/utils/query/occupancy.h" -#include "wavemap/utils/random_number_generator.h" +#include "wavemap/core/common.h" +#include "wavemap/core/data_structure/aabb.h" +#include "wavemap/core/utils/query/classified_map.h" +#include "wavemap/core/utils/query/occupancy.h" +#include "wavemap/core/utils/random_number_generator.h" namespace wavemap { class PointSampler { @@ -46,6 +46,6 @@ class PointSampler { }; } // namespace wavemap -#include "wavemap/utils/query/impl/point_sampler_inl.h" +#include "wavemap/core/utils/query/impl/point_sampler_inl.h" #endif // WAVEMAP_UTILS_QUERY_POINT_SAMPLER_H_ diff --git a/libraries/wavemap/include/wavemap/utils/query/probability_conversions.h b/library/wavemap/core/utils/query/probability_conversions.h similarity index 96% rename from libraries/wavemap/include/wavemap/utils/query/probability_conversions.h rename to library/wavemap/core/utils/query/probability_conversions.h index 01ff75a0b..2369ceec1 100644 --- a/libraries/wavemap/include/wavemap/utils/query/probability_conversions.h +++ b/library/wavemap/core/utils/query/probability_conversions.h @@ -3,7 +3,7 @@ #include -#include "wavemap/common.h" +#include "wavemap/core/common.h" namespace wavemap::convert { constexpr FloatingPoint kLogOddsNumericalMin = diff --git a/libraries/wavemap/src/utils/query/query_accelerator.cc b/library/wavemap/core/utils/query/query_accelerator.cc similarity index 98% rename from libraries/wavemap/src/utils/query/query_accelerator.cc rename to library/wavemap/core/utils/query/query_accelerator.cc index 95aece3c3..f4abaaf08 100644 --- a/libraries/wavemap/src/utils/query/query_accelerator.cc +++ b/library/wavemap/core/utils/query/query_accelerator.cc @@ -1,4 +1,4 @@ -#include "wavemap/utils/query/query_accelerator.h" +#include "wavemap/core/utils/query/query_accelerator.h" namespace wavemap { void QueryAccelerator::reset() { diff --git a/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h b/library/wavemap/core/utils/query/query_accelerator.h similarity index 92% rename from libraries/wavemap/include/wavemap/utils/query/query_accelerator.h rename to library/wavemap/core/utils/query/query_accelerator.h index 7c61cc472..99d6d5fab 100644 --- a/libraries/wavemap/include/wavemap/utils/query/query_accelerator.h +++ b/library/wavemap/core/utils/query/query_accelerator.h @@ -3,11 +3,11 @@ #include -#include "wavemap/data_structure/dense_block_hash.h" -#include "wavemap/data_structure/ndtree_block_hash.h" -#include "wavemap/data_structure/spatial_hash.h" -#include "wavemap/indexing/index_hashes.h" -#include "wavemap/map/hashed_wavelet_octree.h" +#include "wavemap/core/data_structure/dense_block_hash.h" +#include "wavemap/core/data_structure/ndtree_block_hash.h" +#include "wavemap/core/data_structure/spatial_hash.h" +#include "wavemap/core/indexing/index_hashes.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" namespace wavemap { // Base template @@ -137,6 +137,6 @@ class QueryAccelerator { }; } // namespace wavemap -#include "wavemap/utils/query/impl/query_accelerator_inl.h" +#include "wavemap/core/utils/query/impl/query_accelerator_inl.h" #endif // WAVEMAP_UTILS_QUERY_QUERY_ACCELERATOR_H_ diff --git a/libraries/wavemap/include/wavemap/utils/random_number_generator.h b/library/wavemap/core/utils/random_number_generator.h similarity index 97% rename from libraries/wavemap/include/wavemap/utils/random_number_generator.h rename to library/wavemap/core/utils/random_number_generator.h index ff6932c62..7c7087346 100644 --- a/libraries/wavemap/include/wavemap/utils/random_number_generator.h +++ b/library/wavemap/core/utils/random_number_generator.h @@ -3,7 +3,7 @@ #include -#include "wavemap/common.h" +#include "wavemap/core/common.h" namespace wavemap { class RandomNumberGenerator { diff --git a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc b/library/wavemap/core/utils/sdf/full_euclidean_sdf_generator.cc similarity index 97% rename from libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc rename to library/wavemap/core/utils/sdf/full_euclidean_sdf_generator.cc index ddce0176b..b3cb99d0f 100644 --- a/libraries/wavemap/src/utils/sdf/full_euclidean_sdf_generator.cc +++ b/library/wavemap/core/utils/sdf/full_euclidean_sdf_generator.cc @@ -1,9 +1,9 @@ -#include "wavemap/utils/sdf/full_euclidean_sdf_generator.h" +#include "wavemap/core/utils/sdf/full_euclidean_sdf_generator.h" #include -#include "wavemap/utils/iterate/grid_iterator.h" -#include "wavemap/utils/query/query_accelerator.h" +#include "wavemap/core/utils/iterate/grid_iterator.h" +#include "wavemap/core/utils/query/query_accelerator.h" namespace wavemap { HashedBlocks FullEuclideanSDFGenerator::generate( diff --git a/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h b/library/wavemap/core/utils/sdf/full_euclidean_sdf_generator.h similarity index 88% rename from libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h rename to library/wavemap/core/utils/sdf/full_euclidean_sdf_generator.h index b64c25b53..d6c33b7d9 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/full_euclidean_sdf_generator.h +++ b/library/wavemap/core/utils/sdf/full_euclidean_sdf_generator.h @@ -1,11 +1,11 @@ #ifndef WAVEMAP_UTILS_SDF_FULL_EUCLIDEAN_SDF_GENERATOR_H_ #define WAVEMAP_UTILS_SDF_FULL_EUCLIDEAN_SDF_GENERATOR_H_ -#include "wavemap/common.h" -#include "wavemap/data_structure/bucket_queue.h" -#include "wavemap/map/hashed_blocks.h" -#include "wavemap/map/hashed_wavelet_octree.h" -#include "wavemap/utils/neighbors/grid_neighborhood.h" +#include "wavemap/core/common.h" +#include "wavemap/core/data_structure/bucket_queue.h" +#include "wavemap/core/map/hashed_blocks.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" +#include "wavemap/core/utils/neighbors/grid_neighborhood.h" namespace wavemap { struct VectorDistance { diff --git a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc b/library/wavemap/core/utils/sdf/quasi_euclidean_sdf_generator.cc similarity index 97% rename from libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc rename to library/wavemap/core/utils/sdf/quasi_euclidean_sdf_generator.cc index 1857a18e3..5414b929c 100644 --- a/libraries/wavemap/src/utils/sdf/quasi_euclidean_sdf_generator.cc +++ b/library/wavemap/core/utils/sdf/quasi_euclidean_sdf_generator.cc @@ -1,9 +1,9 @@ -#include "wavemap/utils/sdf/quasi_euclidean_sdf_generator.h" +#include "wavemap/core/utils/sdf/quasi_euclidean_sdf_generator.h" #include -#include "wavemap/utils/iterate/grid_iterator.h" -#include "wavemap/utils/query/query_accelerator.h" +#include "wavemap/core/utils/iterate/grid_iterator.h" +#include "wavemap/core/utils/query/query_accelerator.h" namespace wavemap { HashedBlocks QuasiEuclideanSDFGenerator::generate( diff --git a/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h b/library/wavemap/core/utils/sdf/quasi_euclidean_sdf_generator.h similarity index 82% rename from libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h rename to library/wavemap/core/utils/sdf/quasi_euclidean_sdf_generator.h index d68ebc721..fd5c69110 100644 --- a/libraries/wavemap/include/wavemap/utils/sdf/quasi_euclidean_sdf_generator.h +++ b/library/wavemap/core/utils/sdf/quasi_euclidean_sdf_generator.h @@ -1,11 +1,11 @@ #ifndef WAVEMAP_UTILS_SDF_QUASI_EUCLIDEAN_SDF_GENERATOR_H_ #define WAVEMAP_UTILS_SDF_QUASI_EUCLIDEAN_SDF_GENERATOR_H_ -#include "wavemap/data_structure/bucket_queue.h" -#include "wavemap/map/hashed_blocks.h" -#include "wavemap/map/hashed_wavelet_octree.h" -#include "wavemap/utils/neighbors/grid_neighborhood.h" -#include "wavemap/utils/query/occupancy_classifier.h" +#include "wavemap/core/data_structure/bucket_queue.h" +#include "wavemap/core/map/hashed_blocks.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" +#include "wavemap/core/utils/neighbors/grid_neighborhood.h" +#include "wavemap/core/utils/query/occupancy_classifier.h" namespace wavemap { class QuasiEuclideanSDFGenerator { diff --git a/libraries/wavemap/src/utils/thread_pool.cc b/library/wavemap/core/utils/thread_pool.cc similarity index 97% rename from libraries/wavemap/src/utils/thread_pool.cc rename to library/wavemap/core/utils/thread_pool.cc index 6f59c61f4..09c6276ce 100644 --- a/libraries/wavemap/src/utils/thread_pool.cc +++ b/library/wavemap/core/utils/thread_pool.cc @@ -1,4 +1,4 @@ -#include "wavemap/utils/thread_pool.h" +#include "wavemap/core/utils/thread_pool.h" #include diff --git a/libraries/wavemap/include/wavemap/utils/thread_pool.h b/library/wavemap/core/utils/thread_pool.h similarity index 100% rename from libraries/wavemap/include/wavemap/utils/thread_pool.h rename to library/wavemap/core/utils/thread_pool.h diff --git a/libraries/wavemap/src/utils/stopwatch.cc b/library/wavemap/core/utils/time/stopwatch.cc similarity index 92% rename from libraries/wavemap/src/utils/stopwatch.cc rename to library/wavemap/core/utils/time/stopwatch.cc index 328f08233..c023dc506 100644 --- a/libraries/wavemap/src/utils/stopwatch.cc +++ b/library/wavemap/core/utils/time/stopwatch.cc @@ -1,4 +1,4 @@ -#include "wavemap/utils/time/stopwatch.h" +#include "wavemap/core/utils/time/stopwatch.h" #include diff --git a/libraries/wavemap/include/wavemap/utils/time/stopwatch.h b/library/wavemap/core/utils/time/stopwatch.h similarity index 93% rename from libraries/wavemap/include/wavemap/utils/time/stopwatch.h rename to library/wavemap/core/utils/time/stopwatch.h index f6bbcbc89..d2386dd4f 100644 --- a/libraries/wavemap/include/wavemap/utils/time/stopwatch.h +++ b/library/wavemap/core/utils/time/stopwatch.h @@ -1,7 +1,7 @@ #ifndef WAVEMAP_UTILS_TIME_STOPWATCH_H_ #define WAVEMAP_UTILS_TIME_STOPWATCH_H_ -#include "wavemap/utils/time/time.h" +#include "wavemap/core/utils/time/time.h" namespace wavemap { class Stopwatch { diff --git a/libraries/wavemap/include/wavemap/utils/time/time.h b/library/wavemap/core/utils/time/time.h similarity index 100% rename from libraries/wavemap/include/wavemap/utils/time/time.h rename to library/wavemap/core/utils/time/time.h diff --git a/library/wavemap/io/CMakeLists.txt b/library/wavemap/io/CMakeLists.txt new file mode 100644 index 000000000..1026f55dd --- /dev/null +++ b/library/wavemap/io/CMakeLists.txt @@ -0,0 +1,6 @@ +add_library(io) +add_library(wavemap::io ALIAS io) +target_sources(io PRIVATE file_conversions.cc stream_conversions.cc) +target_include_directories(io PUBLIC ${glog_INCLUDE_DIRS}) +target_link_libraries(io PUBLIC Eigen3::Eigen ${glog_LIBRARIES}) +set_wavemap_target_properties(io) diff --git a/libraries/wavemap/src/io/file_conversions.cc b/library/wavemap/io/file_conversions.cc similarity index 100% rename from libraries/wavemap/src/io/file_conversions.cc rename to library/wavemap/io/file_conversions.cc diff --git a/libraries/wavemap/include/wavemap/io/file_conversions.h b/library/wavemap/io/file_conversions.h similarity index 90% rename from libraries/wavemap/include/wavemap/io/file_conversions.h rename to library/wavemap/io/file_conversions.h index 17bbd6e5b..6e0cb70f2 100644 --- a/libraries/wavemap/include/wavemap/io/file_conversions.h +++ b/library/wavemap/io/file_conversions.h @@ -3,8 +3,8 @@ #include +#include "wavemap/core/map/map_base.h" #include "wavemap/io/stream_conversions.h" -#include "wavemap/map/map_base.h" namespace wavemap::io { bool mapToFile(const MapBase& map, const std::filesystem::path& file_path); diff --git a/libraries/wavemap/include/wavemap/io/impl/streamable_types_impl.h b/library/wavemap/io/impl/streamable_types_impl.h similarity index 100% rename from libraries/wavemap/include/wavemap/io/impl/streamable_types_impl.h rename to library/wavemap/io/impl/streamable_types_impl.h diff --git a/libraries/wavemap/src/io/stream_conversions.cc b/library/wavemap/io/stream_conversions.cc similarity index 100% rename from libraries/wavemap/src/io/stream_conversions.cc rename to library/wavemap/io/stream_conversions.cc diff --git a/libraries/wavemap/include/wavemap/io/stream_conversions.h b/library/wavemap/io/stream_conversions.h similarity index 74% rename from libraries/wavemap/include/wavemap/io/stream_conversions.h rename to library/wavemap/io/stream_conversions.h index 4cac52703..47758522d 100644 --- a/libraries/wavemap/include/wavemap/io/stream_conversions.h +++ b/library/wavemap/io/stream_conversions.h @@ -4,13 +4,13 @@ #include #include -#include "wavemap/common.h" +#include "wavemap/core/common.h" +#include "wavemap/core/map/cell_types/haar_coefficients.h" +#include "wavemap/core/map/hashed_blocks.h" +#include "wavemap/core/map/hashed_chunked_wavelet_octree.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" +#include "wavemap/core/map/wavelet_octree.h" #include "wavemap/io/streamable_types.h" -#include "wavemap/map/cell_types/haar_coefficients.h" -#include "wavemap/map/hashed_blocks.h" -#include "wavemap/map/hashed_chunked_wavelet_octree.h" -#include "wavemap/map/hashed_wavelet_octree.h" -#include "wavemap/map/wavelet_octree.h" namespace wavemap::io { bool mapToStream(const MapBase& map, std::ostream& ostream); diff --git a/libraries/wavemap/include/wavemap/io/streamable_types.h b/library/wavemap/io/streamable_types.h similarity index 98% rename from libraries/wavemap/include/wavemap/io/streamable_types.h rename to library/wavemap/io/streamable_types.h index d0e978c49..3a82d09c5 100644 --- a/libraries/wavemap/include/wavemap/io/streamable_types.h +++ b/library/wavemap/io/streamable_types.h @@ -4,7 +4,7 @@ #include #include -#include "wavemap/config/type_selector.h" +#include "wavemap/core/config/type_selector.h" namespace wavemap::io::streamable { // NOTE: This file defines the serialization format for all types that might be diff --git a/library/wavemap/pipeline/CMakeLists.txt b/library/wavemap/pipeline/CMakeLists.txt new file mode 100644 index 000000000..e69de29bb diff --git a/library/wavemap/test/CMakeLists.txt b/library/wavemap/test/CMakeLists.txt new file mode 100644 index 000000000..0d0a26162 --- /dev/null +++ b/library/wavemap/test/CMakeLists.txt @@ -0,0 +1,43 @@ +catkin_add_gtest( + test_${PROJECT_NAME} + test/src/data_structure/test_aabb.cc + test/src/data_structure/test_image.cc + test/src/data_structure/test_ndtree.cc + test/src/data_structure/test_pointcloud.cc + test/src/data_structure/test_sparse_vector.cc + test/src/indexing/test_index_conversions.cc + test/src/indexing/test_ndtree_index.cc + test/src/integrator/projection_model/test_circular_projector.cc + test/src/integrator/projection_model/test_image_projectors.cc + test/src/integrator/projection_model/test_spherical_projector.cc + test/src/integrator/test_hierarchical_range_image.cc + test/src/integrator/test_measurement_models.cc + test/src/integrator/test_pointcloud_integrators.cc + test/src/integrator/test_range_image_intersector.cc + test/src/io/test_file_conversions.cc + test/src/map/test_haar_cell.cc + test/src/map/test_hashed_blocks.cc + test/src/map/test_map.cc + test/src/map/test_volumetric_octree.cc + test/src/utils/bits/test_bit_operations.cc + test/src/utils/data/test_comparisons.cc + test/src/utils/data/test_fill.cc + test/src/utils/iterate/test_grid_iterator.cc + test/src/utils/iterate/test_ray_iterator.cc + test/src/utils/iterate/test_subtree_iterator.cc + test/src/utils/math/test_approximate_trigonometry.cc + test/src/utils/math/test_int_math.cc + test/src/utils/math/test_tree_math.cc + test/src/utils/neighbors/test_adjacency.cc + test/src/utils/neighbors/test_grid_adjacency.cc + test/src/utils/neighbors/test_grid_neighborhood.cc + test/src/utils/neighbors/test_ndtree_adjacency.cc + test/src/utils/query/test_classified_map.cc + test/src/utils/query/test_map_interpolator.cpp + test/src/utils/query/test_occupancy_classifier.cc + test/src/utils/query/test_probability_conversions.cc + test/src/utils/query/test_query_accelerator.cc + test/src/utils/sdf/test_sdf_generators.cc + test/src/utils/test_thread_pool.cc) +target_include_directories(test_${PROJECT_NAME} PRIVATE test/include) +target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} gtest_main) diff --git a/libraries/wavemap/test/include/wavemap/test/config_generator.h b/library/wavemap/test/config_generator.h similarity index 89% rename from libraries/wavemap/test/include/wavemap/test/config_generator.h rename to library/wavemap/test/config_generator.h index c2adf1fb1..f6d7d4a6f 100644 --- a/libraries/wavemap/test/include/wavemap/test/config_generator.h +++ b/library/wavemap/test/config_generator.h @@ -3,17 +3,17 @@ #include -#include "wavemap/common.h" -#include "wavemap/integrator/measurement_model/continuous_beam.h" -#include "wavemap/integrator/projection_model/spherical_projector.h" -#include "wavemap/integrator/projective/projective_integrator.h" -#include "wavemap/integrator/ray_tracing/ray_tracing_integrator.h" -#include "wavemap/map/hashed_chunked_wavelet_octree.h" -#include "wavemap/map/hashed_wavelet_octree.h" -#include "wavemap/map/map_base.h" -#include "wavemap/map/volumetric_octree.h" -#include "wavemap/map/wavelet_octree.h" -#include "wavemap/utils/random_number_generator.h" +#include "wavemap/core/common.h" +#include "wavemap/core/integrator/measurement_model/continuous_beam.h" +#include "wavemap/core/integrator/projection_model/spherical_projector.h" +#include "wavemap/core/integrator/projective/projective_integrator.h" +#include "wavemap/core/integrator/ray_tracing/ray_tracing_integrator.h" +#include "wavemap/core/map/hashed_chunked_wavelet_octree.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" +#include "wavemap/core/map/map_base.h" +#include "wavemap/core/map/volumetric_octree.h" +#include "wavemap/core/map/wavelet_octree.h" +#include "wavemap/core/utils/random_number_generator.h" namespace wavemap { namespace detail { diff --git a/libraries/wavemap/test/src/data_structure/test_aabb.cc b/library/wavemap/test/core/data_structure/test_aabb.cc similarity index 97% rename from libraries/wavemap/test/src/data_structure/test_aabb.cc rename to library/wavemap/test/core/data_structure/test_aabb.cc index 0549e5227..9f77f2bc4 100644 --- a/libraries/wavemap/test/src/data_structure/test_aabb.cc +++ b/library/wavemap/test/core/data_structure/test_aabb.cc @@ -1,10 +1,10 @@ #include -#include "wavemap/common.h" -#include "wavemap/data_structure/aabb.h" +#include "wavemap/core/common.h" +#include "wavemap/core/data_structure/aabb.h" +#include "wavemap/core/utils/print/eigen.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" -#include "wavemap/utils/print/eigen.h" namespace wavemap { template diff --git a/libraries/wavemap/test/src/data_structure/test_image.cc b/library/wavemap/test/core/data_structure/test_image.cc similarity index 93% rename from libraries/wavemap/test/src/data_structure/test_image.cc rename to library/wavemap/test/core/data_structure/test_image.cc index d5d9ce6bd..f5a7d4cae 100644 --- a/libraries/wavemap/test/src/data_structure/test_image.cc +++ b/library/wavemap/test/core/data_structure/test_image.cc @@ -1,7 +1,7 @@ #include -#include "wavemap/common.h" -#include "wavemap/data_structure/image.h" +#include "wavemap/core/common.h" +#include "wavemap/core/data_structure/image.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" diff --git a/libraries/wavemap/test/src/data_structure/test_ndtree.cc b/library/wavemap/test/core/data_structure/test_ndtree.cc similarity index 96% rename from libraries/wavemap/test/src/data_structure/test_ndtree.cc rename to library/wavemap/test/core/data_structure/test_ndtree.cc index adaecaecf..72b6cb4cb 100644 --- a/libraries/wavemap/test/src/data_structure/test_ndtree.cc +++ b/library/wavemap/test/core/data_structure/test_ndtree.cc @@ -1,8 +1,8 @@ #include -#include "wavemap/data_structure/chunked_ndtree/chunked_ndtree.h" -#include "wavemap/data_structure/ndtree/ndtree.h" -#include "wavemap/indexing/index_hashes.h" +#include "wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h" +#include "wavemap/core/data_structure/ndtree/ndtree.h" +#include "wavemap/core/indexing/index_hashes.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" diff --git a/libraries/wavemap/test/src/data_structure/test_pointcloud.cc b/library/wavemap/test/core/data_structure/test_pointcloud.cc similarity index 98% rename from libraries/wavemap/test/src/data_structure/test_pointcloud.cc rename to library/wavemap/test/core/data_structure/test_pointcloud.cc index 2118dfb61..665ec04e6 100644 --- a/libraries/wavemap/test/src/data_structure/test_pointcloud.cc +++ b/library/wavemap/test/core/data_structure/test_pointcloud.cc @@ -1,10 +1,10 @@ #include -#include "wavemap/common.h" -#include "wavemap/data_structure/pointcloud.h" +#include "wavemap/core/common.h" +#include "wavemap/core/data_structure/pointcloud.h" +#include "wavemap/core/utils/print/eigen.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" -#include "wavemap/utils/print/eigen.h" namespace wavemap { template diff --git a/libraries/wavemap/test/src/data_structure/test_sparse_vector.cc b/library/wavemap/test/core/data_structure/test_sparse_vector.cc similarity index 90% rename from libraries/wavemap/test/src/data_structure/test_sparse_vector.cc rename to library/wavemap/test/core/data_structure/test_sparse_vector.cc index e86f6e40e..4d0f78b2a 100644 --- a/libraries/wavemap/test/src/data_structure/test_sparse_vector.cc +++ b/library/wavemap/test/core/data_structure/test_sparse_vector.cc @@ -1,8 +1,8 @@ #include -#include "wavemap/common.h" -#include "wavemap/data_structure/sparse_vector.h" -#include "wavemap/utils/random_number_generator.h" +#include "wavemap/core/common.h" +#include "wavemap/core/data_structure/sparse_vector.h" +#include "wavemap/core/utils/random_number_generator.h" namespace wavemap { TEST(SparseVectorTest, Insertion) { diff --git a/libraries/wavemap/test/src/indexing/test_index_conversions.cc b/library/wavemap/test/core/indexing/test_index_conversions.cc similarity index 98% rename from libraries/wavemap/test/src/indexing/test_index_conversions.cc rename to library/wavemap/test/core/indexing/test_index_conversions.cc index 2b714d5bb..b299e0d94 100644 --- a/libraries/wavemap/test/src/indexing/test_index_conversions.cc +++ b/library/wavemap/test/core/indexing/test_index_conversions.cc @@ -2,12 +2,12 @@ #include -#include "wavemap/common.h" -#include "wavemap/indexing/index_conversions.h" +#include "wavemap/core/common.h" +#include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/utils/print/eigen.h" #include "wavemap/test/eigen_utils.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" -#include "wavemap/utils/print/eigen.h" namespace wavemap { // TODO(victorr): Add tests for remaining index conversions: diff --git a/libraries/wavemap/test/src/indexing/test_ndtree_index.cc b/library/wavemap/test/core/indexing/test_ndtree_index.cc similarity index 98% rename from libraries/wavemap/test/src/indexing/test_ndtree_index.cc rename to library/wavemap/test/core/indexing/test_ndtree_index.cc index b5af233c7..9ae95c7d3 100644 --- a/libraries/wavemap/test/src/indexing/test_ndtree_index.cc +++ b/library/wavemap/test/core/indexing/test_ndtree_index.cc @@ -3,12 +3,12 @@ #include -#include "wavemap/common.h" -#include "wavemap/indexing/index_conversions.h" -#include "wavemap/indexing/ndtree_index.h" +#include "wavemap/core/common.h" +#include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/utils/print/eigen.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" -#include "wavemap/utils/print/eigen.h" namespace wavemap { template diff --git a/libraries/wavemap/test/src/integrator/projection_model/test_circular_projector.cc b/library/wavemap/test/core/integrator/projection_model/test_circular_projector.cc similarity index 97% rename from libraries/wavemap/test/src/integrator/projection_model/test_circular_projector.cc rename to library/wavemap/test/core/integrator/projection_model/test_circular_projector.cc index 36f8d28e6..29dc2c715 100644 --- a/libraries/wavemap/test/src/integrator/projection_model/test_circular_projector.cc +++ b/library/wavemap/test/core/integrator/projection_model/test_circular_projector.cc @@ -1,7 +1,7 @@ #include -#include "wavemap/common.h" -#include "wavemap/integrator/projection_model/circular_projector.h" +#include "wavemap/core/common.h" +#include "wavemap/core/integrator/projection_model/circular_projector.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" diff --git a/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc b/library/wavemap/test/core/integrator/projection_model/test_image_projectors.cc similarity index 98% rename from libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc rename to library/wavemap/test/core/integrator/projection_model/test_image_projectors.cc index 87387d0a4..bce57fcaf 100644 --- a/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc +++ b/library/wavemap/test/core/integrator/projection_model/test_image_projectors.cc @@ -1,15 +1,15 @@ #include -#include "wavemap/common.h" -#include "wavemap/integrator/projection_model/ouster_projector.h" -#include "wavemap/integrator/projection_model/pinhole_camera_projector.h" -#include "wavemap/integrator/projection_model/spherical_projector.h" +#include "wavemap/core/common.h" +#include "wavemap/core/integrator/projection_model/ouster_projector.h" +#include "wavemap/core/integrator/projection_model/pinhole_camera_projector.h" +#include "wavemap/core/integrator/projection_model/spherical_projector.h" +#include "wavemap/core/utils/math/angle_normalization.h" +#include "wavemap/core/utils/print/container.h" +#include "wavemap/core/utils/print/eigen.h" #include "wavemap/test/eigen_utils.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" -#include "wavemap/utils/math/angle_normalization.h" -#include "wavemap/utils/print/container.h" -#include "wavemap/utils/print/eigen.h" namespace wavemap { class Image2DProjectorTest : public FixtureBase, public GeometryGenerator { diff --git a/libraries/wavemap/test/src/integrator/projection_model/test_spherical_projector.cc b/library/wavemap/test/core/integrator/projection_model/test_spherical_projector.cc similarity index 96% rename from libraries/wavemap/test/src/integrator/projection_model/test_spherical_projector.cc rename to library/wavemap/test/core/integrator/projection_model/test_spherical_projector.cc index c6703ec9c..8d1a75698 100644 --- a/libraries/wavemap/test/src/integrator/projection_model/test_spherical_projector.cc +++ b/library/wavemap/test/core/integrator/projection_model/test_spherical_projector.cc @@ -1,11 +1,11 @@ #include -#include "wavemap/common.h" -#include "wavemap/integrator/projection_model/spherical_projector.h" +#include "wavemap/core/common.h" +#include "wavemap/core/integrator/projection_model/spherical_projector.h" +#include "wavemap/core/utils/print/eigen.h" #include "wavemap/test/eigen_utils.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" -#include "wavemap/utils/print/eigen.h" namespace wavemap { class SphericalProjectorTest : public FixtureBase, public GeometryGenerator { diff --git a/libraries/wavemap/test/src/integrator/test_hierarchical_range_image.cc b/library/wavemap/test/core/integrator/test_hierarchical_range_image.cc similarity index 97% rename from libraries/wavemap/test/src/integrator/test_hierarchical_range_image.cc rename to library/wavemap/test/core/integrator/test_hierarchical_range_image.cc index 1202840e1..e6ea439b6 100644 --- a/libraries/wavemap/test/src/integrator/test_hierarchical_range_image.cc +++ b/library/wavemap/test/core/integrator/test_hierarchical_range_image.cc @@ -1,11 +1,11 @@ #include -#include "wavemap/common.h" -#include "wavemap/indexing/index_conversions.h" -#include "wavemap/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h" +#include "wavemap/core/common.h" +#include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h" +#include "wavemap/core/utils/iterate/grid_iterator.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" -#include "wavemap/utils/iterate/grid_iterator.h" namespace wavemap { class HierarchicalRangeImage2DTest : public FixtureBase, diff --git a/libraries/wavemap/test/src/integrator/test_measurement_models.cc b/library/wavemap/test/core/integrator/test_measurement_models.cc similarity index 67% rename from libraries/wavemap/test/src/integrator/test_measurement_models.cc rename to library/wavemap/test/core/integrator/test_measurement_models.cc index 92c546912..dfe71c7a4 100644 --- a/libraries/wavemap/test/src/integrator/test_measurement_models.cc +++ b/library/wavemap/test/core/integrator/test_measurement_models.cc @@ -1,12 +1,12 @@ #include -#include "wavemap/common.h" -#include "wavemap/integrator/measurement_model/constant_ray.h" -#include "wavemap/integrator/measurement_model/continuous_beam.h" -#include "wavemap/integrator/measurement_model/continuous_ray.h" +#include "wavemap/core/common.h" +#include "wavemap/core/integrator/measurement_model/constant_ray.h" +#include "wavemap/core/integrator/measurement_model/continuous_beam.h" +#include "wavemap/core/integrator/measurement_model/continuous_ray.h" +#include "wavemap/core/utils/iterate/grid_iterator.h" +#include "wavemap/core/utils/iterate/ray_iterator.h" #include "wavemap/test/fixture_base.h" -#include "wavemap/utils/iterate/grid_iterator.h" -#include "wavemap/utils/iterate/ray_iterator.h" namespace wavemap { using MeasurementModelTest = FixtureBase; diff --git a/libraries/wavemap/test/src/integrator/test_pointcloud_integrators.cc b/library/wavemap/test/core/integrator/test_pointcloud_integrators.cc similarity index 88% rename from libraries/wavemap/test/src/integrator/test_pointcloud_integrators.cc rename to library/wavemap/test/core/integrator/test_pointcloud_integrators.cc index 0b6425402..7d21f4a23 100644 --- a/libraries/wavemap/test/src/integrator/test_pointcloud_integrators.cc +++ b/library/wavemap/test/core/integrator/test_pointcloud_integrators.cc @@ -2,27 +2,27 @@ #include -#include "wavemap/common.h" -#include "wavemap/indexing/index_conversions.h" -#include "wavemap/indexing/index_hashes.h" -#include "wavemap/integrator/integrator_base.h" -#include "wavemap/integrator/projection_model/spherical_projector.h" -#include "wavemap/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h" -#include "wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h" -#include "wavemap/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h" -#include "wavemap/integrator/projective/coarse_to_fine/wavelet_integrator.h" -#include "wavemap/integrator/projective/fixed_resolution/fixed_resolution_integrator.h" -#include "wavemap/integrator/ray_tracing/ray_tracing_integrator.h" -#include "wavemap/map/hashed_blocks.h" -#include "wavemap/map/hashed_wavelet_octree.h" -#include "wavemap/map/map_base.h" -#include "wavemap/map/volumetric_octree.h" -#include "wavemap/map/wavelet_octree.h" +#include "wavemap/core/common.h" +#include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/indexing/index_hashes.h" +#include "wavemap/core/integrator/integrator_base.h" +#include "wavemap/core/integrator/projection_model/spherical_projector.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.h" +#include "wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.h" +#include "wavemap/core/integrator/ray_tracing/ray_tracing_integrator.h" +#include "wavemap/core/map/hashed_blocks.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" +#include "wavemap/core/map/map_base.h" +#include "wavemap/core/map/volumetric_octree.h" +#include "wavemap/core/map/wavelet_octree.h" +#include "wavemap/core/utils/iterate/grid_iterator.h" +#include "wavemap/core/utils/print/eigen.h" #include "wavemap/test/config_generator.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" -#include "wavemap/utils/iterate/grid_iterator.h" -#include "wavemap/utils/print/eigen.h" namespace wavemap { class PointcloudIntegratorTest : public FixtureBase, diff --git a/libraries/wavemap/test/src/integrator/test_range_image_intersector.cc b/library/wavemap/test/core/integrator/test_range_image_intersector.cc similarity index 95% rename from libraries/wavemap/test/src/integrator/test_range_image_intersector.cc rename to library/wavemap/test/core/integrator/test_range_image_intersector.cc index 98f7d0710..79e27152f 100644 --- a/libraries/wavemap/test/src/integrator/test_range_image_intersector.cc +++ b/library/wavemap/test/core/integrator/test_range_image_intersector.cc @@ -1,10 +1,10 @@ #include -#include "wavemap/common.h" -#include "wavemap/integrator/measurement_model/continuous_beam.h" -#include "wavemap/integrator/projection_model/spherical_projector.h" -#include "wavemap/integrator/projective/coarse_to_fine/range_image_intersector.h" -#include "wavemap/integrator/projective/projective_integrator.h" +#include "wavemap/core/common.h" +#include "wavemap/core/integrator/measurement_model/continuous_beam.h" +#include "wavemap/core/integrator/projection_model/spherical_projector.h" +#include "wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h" +#include "wavemap/core/integrator/projective/projective_integrator.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" diff --git a/libraries/wavemap/test/src/map/test_haar_cell.cc b/library/wavemap/test/core/map/test_haar_cell.cc similarity index 98% rename from libraries/wavemap/test/src/map/test_haar_cell.cc rename to library/wavemap/test/core/map/test_haar_cell.cc index c9c0b9964..4a2e4b54a 100644 --- a/libraries/wavemap/test/src/map/test_haar_cell.cc +++ b/library/wavemap/test/core/map/test_haar_cell.cc @@ -1,9 +1,9 @@ #include -#include "wavemap/common.h" -#include "wavemap/map/cell_types/haar_transform.h" +#include "wavemap/core/common.h" +#include "wavemap/core/map/cell_types/haar_transform.h" +#include "wavemap/core/utils/print/container.h" #include "wavemap/test/fixture_base.h" -#include "wavemap/utils/print/container.h" namespace wavemap { template diff --git a/libraries/wavemap/test/src/map/test_hashed_blocks.cc b/library/wavemap/test/core/map/test_hashed_blocks.cc similarity index 90% rename from libraries/wavemap/test/src/map/test_hashed_blocks.cc rename to library/wavemap/test/core/map/test_hashed_blocks.cc index 403a6f99a..70b2ad609 100644 --- a/libraries/wavemap/test/src/map/test_hashed_blocks.cc +++ b/library/wavemap/test/core/map/test_hashed_blocks.cc @@ -1,7 +1,7 @@ #include -#include "wavemap/common.h" -#include "wavemap/map/hashed_blocks.h" +#include "wavemap/core/common.h" +#include "wavemap/core/map/hashed_blocks.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" diff --git a/libraries/wavemap/test/src/map/test_map.cc b/library/wavemap/test/core/map/test_map.cc similarity index 95% rename from libraries/wavemap/test/src/map/test_map.cc rename to library/wavemap/test/core/map/test_map.cc index 3f5ca6980..316b70136 100644 --- a/libraries/wavemap/test/src/map/test_map.cc +++ b/library/wavemap/test/core/map/test_map.cc @@ -1,13 +1,13 @@ #include -#include "wavemap/common.h" -#include "wavemap/indexing/index_conversions.h" -#include "wavemap/map/hashed_blocks.h" -#include "wavemap/map/hashed_chunked_wavelet_octree.h" -#include "wavemap/map/hashed_wavelet_octree.h" -#include "wavemap/map/map_base.h" -#include "wavemap/map/volumetric_octree.h" -#include "wavemap/map/wavelet_octree.h" +#include "wavemap/core/common.h" +#include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/map/hashed_blocks.h" +#include "wavemap/core/map/hashed_chunked_wavelet_octree.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" +#include "wavemap/core/map/map_base.h" +#include "wavemap/core/map/volumetric_octree.h" +#include "wavemap/core/map/wavelet_octree.h" #include "wavemap/test/config_generator.h" #include "wavemap/test/eigen_utils.h" #include "wavemap/test/fixture_base.h" diff --git a/libraries/wavemap/test/src/map/test_volumetric_octree.cc b/library/wavemap/test/core/map/test_volumetric_octree.cc similarity index 98% rename from libraries/wavemap/test/src/map/test_volumetric_octree.cc rename to library/wavemap/test/core/map/test_volumetric_octree.cc index 43382aaf0..6012493bd 100644 --- a/libraries/wavemap/test/src/map/test_volumetric_octree.cc +++ b/library/wavemap/test/core/map/test_volumetric_octree.cc @@ -1,7 +1,7 @@ #include -#include "wavemap/common.h" -#include "wavemap/map/volumetric_octree.h" +#include "wavemap/core/common.h" +#include "wavemap/core/map/volumetric_octree.h" #include "wavemap/test/config_generator.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" diff --git a/libraries/wavemap/test/src/utils/bits/test_bit_operations.cc b/library/wavemap/test/core/utils/bits/test_bit_operations.cc similarity index 99% rename from libraries/wavemap/test/src/utils/bits/test_bit_operations.cc rename to library/wavemap/test/core/utils/bits/test_bit_operations.cc index eaa618142..22f872546 100644 --- a/libraries/wavemap/test/src/utils/bits/test_bit_operations.cc +++ b/library/wavemap/test/core/utils/bits/test_bit_operations.cc @@ -2,8 +2,8 @@ #include +#include "wavemap/core/utils/bits/bit_operations.h" #include "wavemap/test/fixture_base.h" -#include "wavemap/utils/bits/bit_operations.h" namespace wavemap { using BitOperationsTest = FixtureBase; diff --git a/libraries/wavemap/test/src/utils/data/test_comparisons.cc b/library/wavemap/test/core/utils/data/test_comparisons.cc similarity index 97% rename from libraries/wavemap/test/src/utils/data/test_comparisons.cc rename to library/wavemap/test/core/utils/data/test_comparisons.cc index f0347b29f..c25b0663f 100644 --- a/libraries/wavemap/test/src/utils/data/test_comparisons.cc +++ b/library/wavemap/test/core/utils/data/test_comparisons.cc @@ -1,6 +1,6 @@ #include -#include "wavemap/utils/data/comparisons.h" +#include "wavemap/core/utils/data/comparisons.h" namespace wavemap { TEST(DataComparisonUtilsTest, IsNonzero) { diff --git a/libraries/wavemap/test/src/utils/data/test_fill.cc b/library/wavemap/test/core/utils/data/test_fill.cc similarity index 93% rename from libraries/wavemap/test/src/utils/data/test_fill.cc rename to library/wavemap/test/core/utils/data/test_fill.cc index 0a2153ba1..89ab3920c 100644 --- a/libraries/wavemap/test/src/utils/data/test_fill.cc +++ b/library/wavemap/test/core/utils/data/test_fill.cc @@ -1,7 +1,7 @@ #include -#include "wavemap/common.h" -#include "wavemap/utils/data/fill.h" +#include "wavemap/core/common.h" +#include "wavemap/core/utils/data/fill.h" namespace wavemap { TEST(DataFillUtilsTest, Constant) { diff --git a/libraries/wavemap/test/src/utils/iterate/test_grid_iterator.cc b/library/wavemap/test/core/utils/iterate/test_grid_iterator.cc similarity index 96% rename from libraries/wavemap/test/src/utils/iterate/test_grid_iterator.cc rename to library/wavemap/test/core/utils/iterate/test_grid_iterator.cc index 617ae19ca..80d743d91 100644 --- a/libraries/wavemap/test/src/utils/iterate/test_grid_iterator.cc +++ b/library/wavemap/test/core/utils/iterate/test_grid_iterator.cc @@ -1,10 +1,10 @@ #include -#include "wavemap/common.h" +#include "wavemap/core/common.h" +#include "wavemap/core/utils/iterate/grid_iterator.h" +#include "wavemap/core/utils/print/eigen.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" -#include "wavemap/utils/iterate/grid_iterator.h" -#include "wavemap/utils/print/eigen.h" namespace wavemap { template diff --git a/libraries/wavemap/test/src/utils/iterate/test_ray_iterator.cc b/library/wavemap/test/core/utils/iterate/test_ray_iterator.cc similarity index 97% rename from libraries/wavemap/test/src/utils/iterate/test_ray_iterator.cc rename to library/wavemap/test/core/utils/iterate/test_ray_iterator.cc index 7fb1f1273..79684ba02 100644 --- a/libraries/wavemap/test/src/utils/iterate/test_ray_iterator.cc +++ b/library/wavemap/test/core/utils/iterate/test_ray_iterator.cc @@ -1,10 +1,10 @@ #include -#include "wavemap/common.h" +#include "wavemap/core/common.h" +#include "wavemap/core/utils/iterate/ray_iterator.h" +#include "wavemap/core/utils/print/eigen.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" -#include "wavemap/utils/iterate/ray_iterator.h" -#include "wavemap/utils/print/eigen.h" namespace wavemap { template diff --git a/libraries/wavemap/test/src/utils/iterate/test_subtree_iterator.cc b/library/wavemap/test/core/utils/iterate/test_subtree_iterator.cc similarity index 95% rename from libraries/wavemap/test/src/utils/iterate/test_subtree_iterator.cc rename to library/wavemap/test/core/utils/iterate/test_subtree_iterator.cc index c8af0067a..ccb859161 100644 --- a/libraries/wavemap/test/src/utils/iterate/test_subtree_iterator.cc +++ b/library/wavemap/test/core/utils/iterate/test_subtree_iterator.cc @@ -1,9 +1,9 @@ #include -#include "wavemap/common.h" -#include "wavemap/data_structure/ndtree/ndtree_node.h" +#include "wavemap/core/common.h" +#include "wavemap/core/data_structure/ndtree/ndtree_node.h" +#include "wavemap/core/utils/iterate/subtree_iterator.h" #include "wavemap/test/fixture_base.h" -#include "wavemap/utils/iterate/subtree_iterator.h" namespace wavemap { class SubtreeIteratorTest : public FixtureBase { diff --git a/libraries/wavemap/test/src/utils/math/test_approximate_trigonometry.cc b/library/wavemap/test/core/utils/math/test_approximate_trigonometry.cc similarity index 86% rename from libraries/wavemap/test/src/utils/math/test_approximate_trigonometry.cc rename to library/wavemap/test/core/utils/math/test_approximate_trigonometry.cc index 1ebecdfea..603d384ac 100644 --- a/libraries/wavemap/test/src/utils/math/test_approximate_trigonometry.cc +++ b/library/wavemap/test/core/utils/math/test_approximate_trigonometry.cc @@ -1,7 +1,7 @@ #include -#include "wavemap/common.h" -#include "wavemap/utils/math/approximate_trigonometry.h" +#include "wavemap/core/common.h" +#include "wavemap/core/utils/math/approximate_trigonometry.h" namespace wavemap { TEST(ApproximateTrigonometryTest, ApproxAtan2) { diff --git a/libraries/wavemap/test/src/utils/math/test_int_math.cc b/library/wavemap/test/core/utils/math/test_int_math.cc similarity index 98% rename from libraries/wavemap/test/src/utils/math/test_int_math.cc rename to library/wavemap/test/core/utils/math/test_int_math.cc index ea6106425..95b5c2e98 100644 --- a/libraries/wavemap/test/src/utils/math/test_int_math.cc +++ b/library/wavemap/test/core/utils/math/test_int_math.cc @@ -2,8 +2,8 @@ #include -#include "wavemap/utils/math/int_math.h" -#include "wavemap/utils/random_number_generator.h" +#include "wavemap/core/utils/math/int_math.h" +#include "wavemap/core/utils/random_number_generator.h" namespace wavemap { TEST(IntMathTest, Exp2) { diff --git a/libraries/wavemap/test/src/utils/math/test_tree_math.cc b/library/wavemap/test/core/utils/math/test_tree_math.cc similarity index 98% rename from libraries/wavemap/test/src/utils/math/test_tree_math.cc rename to library/wavemap/test/core/utils/math/test_tree_math.cc index 7995fb8a3..6abe9f3ff 100644 --- a/libraries/wavemap/test/src/utils/math/test_tree_math.cc +++ b/library/wavemap/test/core/utils/math/test_tree_math.cc @@ -1,6 +1,6 @@ #include -#include "wavemap/utils/math/tree_math.h" +#include "wavemap/core/utils/math/tree_math.h" namespace wavemap { TEST(TreeMathTest, PerfectTreeLeafCounts) { diff --git a/libraries/wavemap/test/src/utils/neighbors/test_adjacency.cc b/library/wavemap/test/core/utils/neighbors/test_adjacency.cc similarity index 96% rename from libraries/wavemap/test/src/utils/neighbors/test_adjacency.cc rename to library/wavemap/test/core/utils/neighbors/test_adjacency.cc index 6adad8ba4..158a31db9 100644 --- a/libraries/wavemap/test/src/utils/neighbors/test_adjacency.cc +++ b/library/wavemap/test/core/utils/neighbors/test_adjacency.cc @@ -1,8 +1,8 @@ #include +#include "wavemap/core/utils/bits/bit_operations.h" +#include "wavemap/core/utils/neighbors/adjacency.h" #include "wavemap/test/fixture_base.h" -#include "wavemap/utils/bits/bit_operations.h" -#include "wavemap/utils/neighbors/adjacency.h" namespace wavemap { template diff --git a/libraries/wavemap/test/src/utils/neighbors/test_grid_adjacency.cc b/library/wavemap/test/core/utils/neighbors/test_grid_adjacency.cc similarity index 97% rename from libraries/wavemap/test/src/utils/neighbors/test_grid_adjacency.cc rename to library/wavemap/test/core/utils/neighbors/test_grid_adjacency.cc index 0782f8045..d8354cced 100644 --- a/libraries/wavemap/test/src/utils/neighbors/test_grid_adjacency.cc +++ b/library/wavemap/test/core/utils/neighbors/test_grid_adjacency.cc @@ -1,9 +1,9 @@ #include +#include "wavemap/core/utils/neighbors/grid_adjacency.h" +#include "wavemap/core/utils/neighbors/grid_neighborhood.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" -#include "wavemap/utils/neighbors/grid_adjacency.h" -#include "wavemap/utils/neighbors/grid_neighborhood.h" namespace wavemap { template diff --git a/libraries/wavemap/test/src/utils/neighbors/test_grid_neighborhood.cc b/library/wavemap/test/core/utils/neighbors/test_grid_neighborhood.cc similarity index 96% rename from libraries/wavemap/test/src/utils/neighbors/test_grid_neighborhood.cc rename to library/wavemap/test/core/utils/neighbors/test_grid_neighborhood.cc index d668ee617..e5493d794 100644 --- a/libraries/wavemap/test/src/utils/neighbors/test_grid_neighborhood.cc +++ b/library/wavemap/test/core/utils/neighbors/test_grid_neighborhood.cc @@ -1,8 +1,8 @@ #include +#include "wavemap/core/utils/neighbors/grid_neighborhood.h" +#include "wavemap/core/utils/print/eigen.h" #include "wavemap/test/fixture_base.h" -#include "wavemap/utils/neighbors/grid_neighborhood.h" -#include "wavemap/utils/print/eigen.h" namespace wavemap { template diff --git a/libraries/wavemap/test/src/utils/neighbors/test_ndtree_adjacency.cc b/library/wavemap/test/core/utils/neighbors/test_ndtree_adjacency.cc similarity index 91% rename from libraries/wavemap/test/src/utils/neighbors/test_ndtree_adjacency.cc rename to library/wavemap/test/core/utils/neighbors/test_ndtree_adjacency.cc index c527edcd0..82b54e500 100644 --- a/libraries/wavemap/test/src/utils/neighbors/test_ndtree_adjacency.cc +++ b/library/wavemap/test/core/utils/neighbors/test_ndtree_adjacency.cc @@ -1,8 +1,8 @@ +#include "wavemap/core/utils/neighbors/grid_adjacency.h" +#include "wavemap/core/utils/neighbors/grid_neighborhood.h" +#include "wavemap/core/utils/neighbors/ndtree_adjacency.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" -#include "wavemap/utils/neighbors/grid_adjacency.h" -#include "wavemap/utils/neighbors/grid_neighborhood.h" -#include "wavemap/utils/neighbors/ndtree_adjacency.h" namespace wavemap { template diff --git a/libraries/wavemap/test/src/utils/query/test_classified_map.cc b/library/wavemap/test/core/utils/query/test_classified_map.cc similarity index 94% rename from libraries/wavemap/test/src/utils/query/test_classified_map.cc rename to library/wavemap/test/core/utils/query/test_classified_map.cc index 1248f15b0..44b89451f 100644 --- a/libraries/wavemap/test/src/utils/query/test_classified_map.cc +++ b/library/wavemap/test/core/utils/query/test_classified_map.cc @@ -1,12 +1,12 @@ #include -#include "wavemap/common.h" -#include "wavemap/map/hashed_wavelet_octree.h" +#include "wavemap/core/common.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" +#include "wavemap/core/utils/iterate/grid_iterator.h" +#include "wavemap/core/utils/query/classified_map.h" #include "wavemap/test/config_generator.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" -#include "wavemap/utils/iterate/grid_iterator.h" -#include "wavemap/utils/query/classified_map.h" namespace wavemap { class ClassifiedMapTest : public FixtureBase, diff --git a/libraries/wavemap/test/src/utils/query/test_map_interpolator.cpp b/library/wavemap/test/core/utils/query/test_map_interpolator.cpp similarity index 93% rename from libraries/wavemap/test/src/utils/query/test_map_interpolator.cpp rename to library/wavemap/test/core/utils/query/test_map_interpolator.cpp index a683c0107..b77aa2247 100644 --- a/libraries/wavemap/test/src/utils/query/test_map_interpolator.cpp +++ b/library/wavemap/test/core/utils/query/test_map_interpolator.cpp @@ -1,9 +1,9 @@ #include -#include "wavemap/indexing/index_conversions.h" -#include "wavemap/map/hashed_blocks.h" -#include "wavemap/utils/iterate/grid_iterator.h" -#include "wavemap/utils/query/map_interpolator.h" +#include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/map/hashed_blocks.h" +#include "wavemap/core/utils/iterate/grid_iterator.h" +#include "wavemap/core/utils/query/map_interpolator.h" namespace wavemap { TEST(MapInterpolatorTest, Trilinear) { diff --git a/libraries/wavemap/test/src/utils/query/test_occupancy_classifier.cc b/library/wavemap/test/core/utils/query/test_occupancy_classifier.cc similarity index 99% rename from libraries/wavemap/test/src/utils/query/test_occupancy_classifier.cc rename to library/wavemap/test/core/utils/query/test_occupancy_classifier.cc index 2a6ad434b..5af7d16bd 100644 --- a/libraries/wavemap/test/src/utils/query/test_occupancy_classifier.cc +++ b/library/wavemap/test/core/utils/query/test_occupancy_classifier.cc @@ -1,6 +1,6 @@ #include -#include "wavemap/utils/query/occupancy_classifier.h" +#include "wavemap/core/utils/query/occupancy_classifier.h" namespace wavemap { TEST(OccupancyClassifierTest, Initialization) { diff --git a/libraries/wavemap/test/src/utils/query/test_probability_conversions.cc b/library/wavemap/test/core/utils/query/test_probability_conversions.cc similarity index 93% rename from libraries/wavemap/test/src/utils/query/test_probability_conversions.cc rename to library/wavemap/test/core/utils/query/test_probability_conversions.cc index ea06e7bc5..8110d8981 100644 --- a/libraries/wavemap/test/src/utils/query/test_probability_conversions.cc +++ b/library/wavemap/test/core/utils/query/test_probability_conversions.cc @@ -2,7 +2,7 @@ #include -#include "wavemap/utils/query/probability_conversions.h" +#include "wavemap/core/utils/query/probability_conversions.h" namespace wavemap { TEST(ProbabilityConversionsTest, LogOddsToProbability) { diff --git a/libraries/wavemap/test/src/utils/query/test_query_accelerator.cc b/library/wavemap/test/core/utils/query/test_query_accelerator.cc similarity index 97% rename from libraries/wavemap/test/src/utils/query/test_query_accelerator.cc rename to library/wavemap/test/core/utils/query/test_query_accelerator.cc index 6ef2806f2..a91dc17f6 100644 --- a/libraries/wavemap/test/src/utils/query/test_query_accelerator.cc +++ b/library/wavemap/test/core/utils/query/test_query_accelerator.cc @@ -6,7 +6,7 @@ #include #include -#include "wavemap/utils/query/query_accelerator.h" +#include "wavemap/core/utils/query/query_accelerator.h" namespace wavemap { class QueryAcceleratorTest : public FixtureBase, diff --git a/libraries/wavemap/test/src/utils/sdf/test_sdf_generators.cc b/library/wavemap/test/core/utils/sdf/test_sdf_generators.cc similarity index 96% rename from libraries/wavemap/test/src/utils/sdf/test_sdf_generators.cc rename to library/wavemap/test/core/utils/sdf/test_sdf_generators.cc index 61997a6ab..cf32c92ec 100644 --- a/libraries/wavemap/test/src/utils/sdf/test_sdf_generators.cc +++ b/library/wavemap/test/core/utils/sdf/test_sdf_generators.cc @@ -1,13 +1,13 @@ #include -#include "wavemap/common.h" -#include "wavemap/map/hashed_blocks.h" -#include "wavemap/map/hashed_wavelet_octree.h" +#include "wavemap/core/common.h" +#include "wavemap/core/map/hashed_blocks.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" +#include "wavemap/core/utils/sdf/full_euclidean_sdf_generator.h" +#include "wavemap/core/utils/sdf/quasi_euclidean_sdf_generator.h" #include "wavemap/test/config_generator.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" -#include "wavemap/utils/sdf/full_euclidean_sdf_generator.h" -#include "wavemap/utils/sdf/quasi_euclidean_sdf_generator.h" namespace wavemap { template diff --git a/libraries/wavemap/test/src/utils/test_thread_pool.cc b/library/wavemap/test/core/utils/test_thread_pool.cc similarity index 88% rename from libraries/wavemap/test/src/utils/test_thread_pool.cc rename to library/wavemap/test/core/utils/test_thread_pool.cc index 2f6233b63..6b1ab5dda 100644 --- a/libraries/wavemap/test/src/utils/test_thread_pool.cc +++ b/library/wavemap/test/core/utils/test_thread_pool.cc @@ -3,7 +3,7 @@ #include -#include "wavemap/utils/thread_pool.h" +#include "wavemap/core/utils/thread_pool.h" TEST(ThreadPoolTest, WaitAll) { auto dummy_fn = []() { diff --git a/libraries/wavemap/test/include/wavemap/test/eigen_utils.h b/library/wavemap/test/eigen_utils.h similarity index 98% rename from libraries/wavemap/test/include/wavemap/test/eigen_utils.h rename to library/wavemap/test/eigen_utils.h index 7e400e587..d85811709 100644 --- a/libraries/wavemap/test/include/wavemap/test/eigen_utils.h +++ b/library/wavemap/test/eigen_utils.h @@ -6,7 +6,7 @@ #include -#include "wavemap/utils/print/eigen.h" +#include "wavemap/core/utils/print/eigen.h" namespace wavemap { template diff --git a/libraries/wavemap/test/include/wavemap/test/fixture_base.h b/library/wavemap/test/fixture_base.h similarity index 90% rename from libraries/wavemap/test/include/wavemap/test/fixture_base.h rename to library/wavemap/test/fixture_base.h index 638e9fceb..567821726 100644 --- a/libraries/wavemap/test/include/wavemap/test/fixture_base.h +++ b/library/wavemap/test/fixture_base.h @@ -3,8 +3,8 @@ #include -#include "wavemap/common.h" -#include "wavemap/utils/random_number_generator.h" +#include "wavemap/core/common.h" +#include "wavemap/core/utils/random_number_generator.h" namespace wavemap { class FixtureBase : public ::testing::Test { diff --git a/libraries/wavemap/test/include/wavemap/test/geometry_generator.h b/library/wavemap/test/geometry_generator.h similarity index 98% rename from libraries/wavemap/test/include/wavemap/test/geometry_generator.h rename to library/wavemap/test/geometry_generator.h index 15d991caa..accd34530 100644 --- a/libraries/wavemap/test/include/wavemap/test/geometry_generator.h +++ b/library/wavemap/test/geometry_generator.h @@ -3,9 +3,9 @@ #include -#include "wavemap/common.h" -#include "wavemap/indexing/ndtree_index.h" -#include "wavemap/utils/random_number_generator.h" +#include "wavemap/core/common.h" +#include "wavemap/core/indexing/ndtree_index.h" +#include "wavemap/core/utils/random_number_generator.h" namespace wavemap { class GeometryGenerator { diff --git a/libraries/wavemap/test/src/io/test_file_conversions.cc b/library/wavemap/test/io/test_file_conversions.cc similarity index 95% rename from libraries/wavemap/test/src/io/test_file_conversions.cc rename to library/wavemap/test/io/test_file_conversions.cc index f8c5b8564..03490d929 100644 --- a/libraries/wavemap/test/src/io/test_file_conversions.cc +++ b/library/wavemap/test/io/test_file_conversions.cc @@ -1,11 +1,11 @@ #include -#include "wavemap/common.h" -#include "wavemap/io/file_conversions.h" -#include "wavemap/map/hashed_chunked_wavelet_octree.h" -#include "wavemap/map/hashed_wavelet_octree.h" -#include "wavemap/map/map_base.h" -#include "wavemap/map/wavelet_octree.h" +#include "wavemap/core/common.h" +#include "wavemap/core/io/file_conversions.h" +#include "wavemap/core/map/hashed_chunked_wavelet_octree.h" +#include "wavemap/core/map/hashed_wavelet_octree.h" +#include "wavemap/core/map/map_base.h" +#include "wavemap/core/map/wavelet_octree.h" #include "wavemap/test/config_generator.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" From 4c5f92133be86788010f54aef0ab55b616066cb2 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 3 May 2024 19:10:37 +0200 Subject: [PATCH 092/159] Clean up library file structure and (transitive) dependency handling --- library/CMakeLists.txt | 28 + .../{wavemap => }/benchmark/CMakeLists.txt | 0 .../benchmark/benchmark_haar_transforms.cc | 0 .../benchmark/benchmark_sparse_vector.cc | 0 library/cmake/compiler_options.cmake | 36 ++ .../{wavemap => }/cmake/wavemap-extras.cmake | 0 library/{wavemap => include}/CPPLINT.cfg | 0 .../wavemap/3rd_party/minkindr/LICENSE.txt | 24 + .../wavemap/3rd_party/minkindr/angle-axis.h | 171 +++++ .../wavemap/3rd_party/minkindr/common.h | 66 ++ .../3rd_party/minkindr/impl/angle-axis-inl.h | 269 ++++++++ .../minkindr/impl/quat-transformation-inl.h | 302 +++++++++ .../minkindr/impl/rotation-quaternion-inl.h | 591 ++++++++++++++++++ .../wavemap/3rd_party/minkindr/position.h | 41 ++ .../3rd_party/minkindr/quat-transformation.h | 216 +++++++ .../3rd_party/minkindr/rotation-quaternion.h | 241 +++++++ library/{ => include}/wavemap/core/common.h | 24 +- .../wavemap/core/config/config_base.h | 6 +- .../core/config/impl/config_base_inl.h | 6 +- .../core/config/impl/type_selector_inl.h | 6 +- .../core/config/impl/value_with_unit_inl.h | 6 +- .../{ => include}/wavemap/core/config/param.h | 6 +- .../wavemap/core/config/param_checks.h | 6 +- .../wavemap/core/config/type_selector.h | 6 +- .../wavemap/core/config/value_with_unit.h | 6 +- .../wavemap/core/data_structure/aabb.h | 6 +- .../core/data_structure/bucket_queue.h | 6 +- .../chunked_ndtree/chunked_ndtree.h | 6 +- .../chunked_ndtree/chunked_ndtree_chunk.h | 6 +- .../chunked_ndtree_node_address.h | 6 +- .../impl/chunked_ndtree_chunk_inl.h | 6 +- .../chunked_ndtree/impl/chunked_ndtree_inl.h | 6 +- .../impl/chunked_ndtree_node_address_inl.h | 6 +- .../core/data_structure/dense_block_hash.h | 6 +- .../wavemap/core/data_structure/dense_grid.h | 6 +- .../wavemap/core/data_structure/image.h | 6 +- .../data_structure/impl/bucket_queue_inl.h | 6 +- .../impl/dense_block_hash_inl.h | 6 +- .../impl/ndtree_block_hash_inl.h | 6 +- .../data_structure/impl/spatial_hash_inl.h | 6 +- .../data_structure/ndtree/impl/ndtree_inl.h | 6 +- .../ndtree/impl/ndtree_node_inl.h | 6 +- .../core/data_structure/ndtree/ndtree.h | 6 +- .../core/data_structure/ndtree/ndtree_node.h | 6 +- .../core/data_structure/ndtree_block_hash.h | 6 +- .../wavemap/core/data_structure/pointcloud.h | 6 +- .../core/data_structure/posed_object.h | 6 +- .../core/data_structure/sparse_vector.h | 6 +- .../core/data_structure/spatial_hash.h | 6 +- .../core/indexing/impl/ndtree_index_inl.h | 6 +- .../wavemap/core/indexing/index_conversions.h | 6 +- .../wavemap/core/indexing/index_hashes.h | 6 +- .../wavemap/core/indexing/ndtree_index.h | 6 +- .../integrator/impl/integrator_base_inl.h | 6 +- .../wavemap/core/integrator/integrator_base.h | 6 +- .../core/integrator/integrator_factory.h | 6 +- .../approximate_gaussian_distribution.h | 6 +- .../measurement_model/constant_ray.h | 6 +- .../measurement_model/continuous_beam.h | 6 +- .../measurement_model/continuous_ray.h | 6 +- .../impl/continuous_beam_inl.h | 6 +- .../impl/continuous_ray_inl.h | 6 +- .../measurement_model_base.h | 6 +- .../measurement_model_factory.h | 6 +- .../projection_model/circular_projector.h | 6 +- .../impl/ouster_projector_inl.h | 6 +- .../impl/pinhole_camera_projector_inl.h | 6 +- .../impl/projector_base_inl.h | 6 +- .../impl/spherical_projector_inl.h | 6 +- .../projection_model/ouster_projector.h | 6 +- .../pinhole_camera_projector.h | 6 +- .../projection_model/projector_base.h | 6 +- .../projection_model/projector_factory.h | 6 +- .../projection_model/spherical_projector.h | 6 +- .../coarse_to_fine_integrator.h | 6 +- .../hashed_chunked_wavelet_integrator.h | 6 +- .../hashed_wavelet_integrator.h | 6 +- .../hierarchical_range_bounds.h | 6 +- .../hashed_chunked_wavelet_integrator_inl.h | 6 +- .../impl/hashed_wavelet_integrator_inl.h | 6 +- .../impl/hierarchical_range_bounds_inl.h | 6 +- .../impl/range_image_intersector_inl.h | 6 +- .../impl/wavelet_integrator_inl.h | 6 +- .../coarse_to_fine/range_image_intersector.h | 6 +- .../coarse_to_fine/wavelet_integrator.h | 6 +- .../fixed_resolution_integrator.h | 6 +- .../impl/projective_integrator_inl.h | 6 +- .../projective/projective_integrator.h | 6 +- .../core/integrator/projective/update_type.h | 6 +- .../ray_tracing/ray_tracing_integrator.h | 6 +- .../core/map/cell_types/haar_coefficients.h | 6 +- .../core/map/cell_types/haar_transform.h | 6 +- .../map/cell_types/impl/haar_transform_inl.h | 6 +- .../wavemap/core/map/hashed_blocks.h | 6 +- .../core/map/hashed_chunked_wavelet_octree.h | 6 +- .../map/hashed_chunked_wavelet_octree_block.h | 6 +- .../wavemap/core/map/hashed_wavelet_octree.h | 6 +- .../core/map/hashed_wavelet_octree_block.h | 6 +- .../wavemap/core/map/impl/hashed_blocks_inl.h | 6 +- .../hashed_chunked_wavelet_octree_block_inl.h | 6 +- .../impl/hashed_chunked_wavelet_octree_inl.h | 6 +- .../impl/hashed_wavelet_octree_block_inl.h | 6 +- .../core/map/impl/hashed_wavelet_octree_inl.h | 6 +- .../core/map/impl/volumetric_octree_inl.h | 6 +- .../core/map/impl/wavelet_octree_inl.h | 6 +- .../{ => include}/wavemap/core/map/map_base.h | 6 +- .../wavemap/core/map/map_factory.h | 6 +- .../wavemap/core/map/volumetric_octree.h | 6 +- .../wavemap/core/map/wavelet_octree.h | 6 +- .../wavemap/core/utils/bits/bit_operations.h | 6 +- .../wavemap/core/utils/bits/morton_encoding.h | 6 +- .../wavemap/core/utils/data/comparisons.h | 6 +- .../wavemap/core/utils/data/constants.h | 6 +- .../wavemap/core/utils/data/eigen_checks.h | 6 +- .../wavemap/core/utils/data/fill.h | 6 +- .../core/utils/iterate/grid_iterator.h | 6 +- .../utils/iterate/impl/ray_iterator_inl.h | 6 +- .../utils/iterate/impl/subtree_iterator_inl.h | 6 +- .../core/utils/iterate/pointcloud_iterator.h | 6 +- .../wavemap/core/utils/iterate/ray_iterator.h | 6 +- .../core/utils/iterate/subtree_iterator.h | 6 +- .../core/utils/math/angle_normalization.h | 6 +- .../utils/math/approximate_trigonometry.h | 6 +- .../wavemap/core/utils/math/int_math.h | 6 +- .../wavemap/core/utils/math/tree_math.h | 6 +- .../wavemap/core/utils/meta/nameof.h | 6 +- .../wavemap/core/utils/meta/type_utils.h | 6 +- .../wavemap/core/utils/neighbors/adjacency.h | 6 +- .../core/utils/neighbors/grid_adjacency.h | 6 +- .../core/utils/neighbors/grid_neighborhood.h | 6 +- .../core/utils/neighbors/impl/adjacency_inl.h | 6 +- .../utils/neighbors/impl/grid_adjacency_inl.h | 6 +- .../neighbors/impl/grid_neighborhood_inl.h | 6 +- .../neighbors/impl/ndtree_adjacency_inl.h | 6 + .../core/utils/neighbors/ndtree_adjacency.h | 6 +- .../wavemap/core/utils/print/container.h | 6 +- .../wavemap/core/utils/print/eigen.h | 6 +- .../wavemap/core/utils/profiler_interface.h | 24 + .../wavemap/core/utils/query/classified_map.h | 6 +- .../utils/query/impl/classified_map_inl.h | 6 +- .../query/impl/occupancy_classifier_inl.h | 6 +- .../core/utils/query/impl/occupancy_inl.h | 6 +- .../core/utils/query/impl/point_sampler_inl.h | 6 +- .../utils/query/impl/query_accelerator_inl.h | 6 +- .../core/utils/query/map_interpolator.h | 6 +- .../wavemap/core/utils/query/occupancy.h | 6 +- .../core/utils/query/occupancy_classifier.h | 6 +- .../wavemap/core/utils/query/point_sampler.h | 6 +- .../utils/query/probability_conversions.h | 6 +- .../core/utils/query/query_accelerator.h | 6 +- .../core/utils/random_number_generator.h | 6 +- .../utils/sdf/full_euclidean_sdf_generator.h | 6 +- .../utils/sdf/quasi_euclidean_sdf_generator.h | 6 +- .../wavemap/core/utils/thread_pool.h | 6 +- .../wavemap/core/utils/time/stopwatch.h | 6 +- .../wavemap/core/utils/time/time.h | 6 +- .../wavemap/io/file_conversions.h | 0 .../wavemap/io/impl/streamable_types_impl.h | 0 .../wavemap/io/stream_conversions.h | 0 .../wavemap/io/streamable_types.h | 0 library/{wavemap => src}/core/CMakeLists.txt | 5 +- .../core/config/value_with_unit.cc | 0 .../core/integrator/integrator_base.cc | 0 .../core/integrator/integrator_factory.cc | 0 .../measurement_model/constant_ray.cc | 0 .../measurement_model/continuous_beam.cc | 0 .../measurement_model/continuous_ray.cc | 0 .../measurement_model_factory.cc | 0 .../projection_model/circular_projector.cc | 0 .../projection_model/ouster_projector.cc | 0 .../pinhole_camera_projector.cc | 0 .../projection_model/projector_factory.cc | 0 .../projection_model/spherical_projector.cc | 0 .../coarse_to_fine_integrator.cc | 0 .../hashed_chunked_wavelet_integrator.cc | 10 +- .../hashed_wavelet_integrator.cc | 10 +- .../coarse_to_fine/wavelet_integrator.cc | 0 .../fixed_resolution_integrator.cc | 0 .../projective/projective_integrator.cc | 10 +- .../ray_tracing/ray_tracing_integrator.cc | 0 .../core/map/hashed_blocks.cc | 0 .../core/map/hashed_chunked_wavelet_octree.cc | 10 +- .../hashed_chunked_wavelet_octree_block.cc | 8 +- .../core/map/hashed_wavelet_octree.cc | 10 +- .../core/map/hashed_wavelet_octree_block.cc | 10 +- library/{wavemap => src}/core/map/map_base.cc | 0 .../{wavemap => src}/core/map/map_factory.cc | 0 .../core/map/volumetric_octree.cc | 0 .../core/map/wavelet_octree.cc | 0 .../core/utils/query/classified_map.cc | 6 +- .../core/utils/query/point_sampler.cc | 0 .../core/utils/query/query_accelerator.cc | 0 .../utils/sdf/full_euclidean_sdf_generator.cc | 12 +- .../sdf/quasi_euclidean_sdf_generator.cc | 12 +- .../core/utils/thread_pool.cc | 4 +- .../core/utils/time/stopwatch.cc | 0 library/{wavemap => src}/io/CMakeLists.txt | 0 .../{wavemap => src}/io/file_conversions.cc | 0 .../{wavemap => src}/io/stream_conversions.cc | 0 .../{wavemap => src}/pipeline/CMakeLists.txt | 0 library/{wavemap => }/test/CMakeLists.txt | 0 .../test/core/data_structure/test_aabb.cc | 0 .../test/core/data_structure/test_image.cc | 0 .../test/core/data_structure/test_ndtree.cc | 0 .../core/data_structure/test_pointcloud.cc | 0 .../core/data_structure/test_sparse_vector.cc | 0 .../core/indexing/test_index_conversions.cc | 0 .../test/core/indexing/test_ndtree_index.cc | 0 .../test_circular_projector.cc | 0 .../projection_model/test_image_projectors.cc | 0 .../test_spherical_projector.cc | 0 .../test_hierarchical_range_image.cc | 0 .../integrator/test_measurement_models.cc | 0 .../integrator/test_pointcloud_integrators.cc | 0 .../test_range_image_intersector.cc | 0 .../test/core/map/test_haar_cell.cc | 0 .../test/core/map/test_hashed_blocks.cc | 0 .../{wavemap => }/test/core/map/test_map.cc | 0 .../test/core/map/test_volumetric_octree.cc | 0 .../core/utils/bits/test_bit_operations.cc | 0 .../test/core/utils/data/test_comparisons.cc | 0 .../test/core/utils/data/test_fill.cc | 0 .../core/utils/iterate/test_grid_iterator.cc | 0 .../core/utils/iterate/test_ray_iterator.cc | 0 .../utils/iterate/test_subtree_iterator.cc | 0 .../math/test_approximate_trigonometry.cc | 0 .../test/core/utils/math/test_int_math.cc | 0 .../test/core/utils/math/test_tree_math.cc | 0 .../core/utils/neighbors/test_adjacency.cc | 0 .../utils/neighbors/test_grid_adjacency.cc | 0 .../utils/neighbors/test_grid_neighborhood.cc | 0 .../utils/neighbors/test_ndtree_adjacency.cc | 0 .../core/utils/query/test_classified_map.cc | 0 .../utils/query/test_map_interpolator.cpp | 0 .../utils/query/test_occupancy_classifier.cc | 0 .../query/test_probability_conversions.cc | 0 .../utils/query/test_query_accelerator.cc | 0 .../core/utils/sdf/test_sdf_generators.cc | 0 .../test/core/utils/test_thread_pool.cc | 0 .../test}/include/CPPLINT.cfg | 0 .../include}/wavemap/test/config_generator.h | 0 .../include}/wavemap/test/eigen_utils.h | 0 .../include}/wavemap/test/fixture_base.h | 0 .../wavemap/test/geometry_generator.h | 0 .../test/io/test_file_conversions.cc | 0 library/wavemap/CMakeLists.txt | 46 -- library/wavemap/cmake/compiler_options.cmake | 31 - .../neighbors/impl/ndtree_adjacency_inl.h | 6 - 248 files changed, 2485 insertions(+), 566 deletions(-) create mode 100644 library/CMakeLists.txt rename library/{wavemap => }/benchmark/CMakeLists.txt (100%) rename library/{wavemap => }/benchmark/benchmark_haar_transforms.cc (100%) rename library/{wavemap => }/benchmark/benchmark_sparse_vector.cc (100%) create mode 100644 library/cmake/compiler_options.cmake rename library/{wavemap => }/cmake/wavemap-extras.cmake (100%) rename library/{wavemap => include}/CPPLINT.cfg (100%) create mode 100644 library/include/wavemap/3rd_party/minkindr/LICENSE.txt create mode 100644 library/include/wavemap/3rd_party/minkindr/angle-axis.h create mode 100644 library/include/wavemap/3rd_party/minkindr/common.h create mode 100644 library/include/wavemap/3rd_party/minkindr/impl/angle-axis-inl.h create mode 100644 library/include/wavemap/3rd_party/minkindr/impl/quat-transformation-inl.h create mode 100644 library/include/wavemap/3rd_party/minkindr/impl/rotation-quaternion-inl.h create mode 100644 library/include/wavemap/3rd_party/minkindr/position.h create mode 100644 library/include/wavemap/3rd_party/minkindr/quat-transformation.h create mode 100644 library/include/wavemap/3rd_party/minkindr/rotation-quaternion.h rename library/{ => include}/wavemap/core/common.h (69%) rename library/{ => include}/wavemap/core/config/config_base.h (96%) rename library/{ => include}/wavemap/core/config/impl/config_base_inl.h (98%) rename library/{ => include}/wavemap/core/config/impl/type_selector_inl.h (96%) rename library/{ => include}/wavemap/core/config/impl/value_with_unit_inl.h (92%) rename library/{ => include}/wavemap/core/config/param.h (94%) rename library/{ => include}/wavemap/core/config/param_checks.h (92%) rename library/{ => include}/wavemap/core/config/type_selector.h (92%) rename library/{ => include}/wavemap/core/config/value_with_unit.h (93%) rename library/{ => include}/wavemap/core/data_structure/aabb.h (96%) rename library/{ => include}/wavemap/core/data_structure/bucket_queue.h (90%) rename library/{ => include}/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h (93%) rename library/{ => include}/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_chunk.h (92%) rename library/{ => include}/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h (93%) rename library/{ => include}/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h (95%) rename library/{ => include}/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h (96%) rename library/{ => include}/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h (95%) rename library/{ => include}/wavemap/core/data_structure/dense_block_hash.h (94%) rename library/{ => include}/wavemap/core/data_structure/dense_grid.h (92%) rename library/{ => include}/wavemap/core/data_structure/image.h (95%) rename library/{ => include}/wavemap/core/data_structure/impl/bucket_queue_inl.h (90%) rename library/{ => include}/wavemap/core/data_structure/impl/dense_block_hash_inl.h (96%) rename library/{ => include}/wavemap/core/data_structure/impl/ndtree_block_hash_inl.h (98%) rename library/{ => include}/wavemap/core/data_structure/impl/spatial_hash_inl.h (95%) rename library/{ => include}/wavemap/core/data_structure/ndtree/impl/ndtree_inl.h (96%) rename library/{ => include}/wavemap/core/data_structure/ndtree/impl/ndtree_node_inl.h (93%) rename library/{ => include}/wavemap/core/data_structure/ndtree/ndtree.h (92%) rename library/{ => include}/wavemap/core/data_structure/ndtree/ndtree_node.h (90%) rename library/{ => include}/wavemap/core/data_structure/ndtree_block_hash.h (95%) rename library/{ => include}/wavemap/core/data_structure/pointcloud.h (94%) rename library/{ => include}/wavemap/core/data_structure/posed_object.h (89%) rename library/{ => include}/wavemap/core/data_structure/sparse_vector.h (91%) rename library/{ => include}/wavemap/core/data_structure/spatial_hash.h (91%) rename library/{ => include}/wavemap/core/indexing/impl/ndtree_index_inl.h (96%) rename library/{ => include}/wavemap/core/indexing/index_conversions.h (97%) rename library/{ => include}/wavemap/core/indexing/index_hashes.h (88%) rename library/{ => include}/wavemap/core/indexing/ndtree_index.h (94%) rename library/{ => include}/wavemap/core/integrator/impl/integrator_base_inl.h (86%) rename library/{ => include}/wavemap/core/integrator/integrator_base.h (90%) rename library/{ => include}/wavemap/core/integrator/integrator_factory.h (80%) rename library/{ => include}/wavemap/core/integrator/measurement_model/approximate_gaussian_distribution.h (75%) rename library/{ => include}/wavemap/core/integrator/measurement_model/constant_ray.h (91%) rename library/{ => include}/wavemap/core/integrator/measurement_model/continuous_beam.h (95%) rename library/{ => include}/wavemap/core/integrator/measurement_model/continuous_ray.h (94%) rename library/{ => include}/wavemap/core/integrator/measurement_model/impl/continuous_beam_inl.h (96%) rename library/{ => include}/wavemap/core/integrator/measurement_model/impl/continuous_ray_inl.h (93%) rename library/{ => include}/wavemap/core/integrator/measurement_model/measurement_model_base.h (87%) rename library/{ => include}/wavemap/core/integrator/measurement_model/measurement_model_factory.h (75%) rename library/{ => include}/wavemap/core/integrator/projection_model/circular_projector.h (93%) rename library/{ => include}/wavemap/core/integrator/projection_model/impl/ouster_projector_inl.h (94%) rename library/{ => include}/wavemap/core/integrator/projection_model/impl/pinhole_camera_projector_inl.h (86%) rename library/{ => include}/wavemap/core/integrator/projection_model/impl/projector_base_inl.h (94%) rename library/{ => include}/wavemap/core/integrator/projection_model/impl/spherical_projector_inl.h (91%) rename library/{ => include}/wavemap/core/integrator/projection_model/ouster_projector.h (94%) rename library/{ => include}/wavemap/core/integrator/projection_model/pinhole_camera_projector.h (93%) rename library/{ => include}/wavemap/core/integrator/projection_model/projector_base.h (96%) rename library/{ => include}/wavemap/core/integrator/projection_model/projector_factory.h (66%) rename library/{ => include}/wavemap/core/integrator/projection_model/spherical_projector.h (92%) rename library/{ => include}/wavemap/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h (84%) rename library/{ => include}/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h (92%) rename library/{ => include}/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h (90%) rename library/{ => include}/wavemap/core/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h (96%) rename library/{ => include}/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h (90%) rename library/{ => include}/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h (80%) rename library/{ => include}/wavemap/core/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h (98%) rename library/{ => include}/wavemap/core/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h (91%) rename library/{ => include}/wavemap/core/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h (94%) rename library/{ => include}/wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h (88%) rename library/{ => include}/wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.h (87%) rename library/{ => include}/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.h (81%) rename library/{ => include}/wavemap/core/integrator/projective/impl/projective_integrator_inl.h (73%) rename library/{ => include}/wavemap/core/integrator/projective/projective_integrator.h (95%) rename library/{ => include}/wavemap/core/integrator/projective/update_type.h (75%) rename library/{ => include}/wavemap/core/integrator/ray_tracing/ray_tracing_integrator.h (85%) rename library/{ => include}/wavemap/core/map/cell_types/haar_coefficients.h (96%) rename library/{ => include}/wavemap/core/map/cell_types/haar_transform.h (97%) rename library/{ => include}/wavemap/core/map/cell_types/impl/haar_transform_inl.h (97%) rename library/{ => include}/wavemap/core/map/hashed_blocks.h (93%) rename library/{ => include}/wavemap/core/map/hashed_chunked_wavelet_octree.h (96%) rename library/{ => include}/wavemap/core/map/hashed_chunked_wavelet_octree_block.h (95%) rename library/{ => include}/wavemap/core/map/hashed_wavelet_octree.h (96%) rename library/{ => include}/wavemap/core/map/hashed_wavelet_octree_block.h (94%) rename library/{ => include}/wavemap/core/map/impl/hashed_blocks_inl.h (82%) rename library/{ => include}/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h (91%) rename library/{ => include}/wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h (95%) rename library/{ => include}/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h (87%) rename library/{ => include}/wavemap/core/map/impl/hashed_wavelet_octree_inl.h (95%) rename library/{ => include}/wavemap/core/map/impl/volumetric_octree_inl.h (94%) rename library/{ => include}/wavemap/core/map/impl/wavelet_octree_inl.h (97%) rename library/{ => include}/wavemap/core/map/map_base.h (96%) rename library/{ => include}/wavemap/core/map/map_factory.h (73%) rename library/{ => include}/wavemap/core/map/volumetric_octree.h (97%) rename library/{ => include}/wavemap/core/map/wavelet_octree.h (97%) rename library/{ => include}/wavemap/core/utils/bits/bit_operations.h (97%) rename library/{ => include}/wavemap/core/utils/bits/morton_encoding.h (97%) rename library/{ => include}/wavemap/core/utils/data/comparisons.h (93%) rename library/{ => include}/wavemap/core/utils/data/constants.h (90%) rename library/{ => include}/wavemap/core/utils/data/eigen_checks.h (96%) rename library/{ => include}/wavemap/core/utils/data/fill.h (81%) rename library/{ => include}/wavemap/core/utils/iterate/grid_iterator.h (92%) rename library/{ => include}/wavemap/core/utils/iterate/impl/ray_iterator_inl.h (93%) rename library/{ => include}/wavemap/core/utils/iterate/impl/subtree_iterator_inl.h (94%) rename library/{ => include}/wavemap/core/utils/iterate/pointcloud_iterator.h (90%) rename library/{ => include}/wavemap/core/utils/iterate/ray_iterator.h (93%) rename library/{ => include}/wavemap/core/utils/iterate/subtree_iterator.h (96%) rename library/{ => include}/wavemap/core/utils/math/angle_normalization.h (68%) rename library/{ => include}/wavemap/core/utils/math/approximate_trigonometry.h (94%) rename library/{ => include}/wavemap/core/utils/math/int_math.h (96%) rename library/{ => include}/wavemap/core/utils/math/tree_math.h (87%) rename library/{ => include}/wavemap/core/utils/meta/nameof.h (93%) rename library/{ => include}/wavemap/core/utils/meta/type_utils.h (93%) rename library/{ => include}/wavemap/core/utils/neighbors/adjacency.h (82%) rename library/{ => include}/wavemap/core/utils/neighbors/grid_adjacency.h (89%) rename library/{ => include}/wavemap/core/utils/neighbors/grid_neighborhood.h (82%) rename library/{ => include}/wavemap/core/utils/neighbors/impl/adjacency_inl.h (74%) rename library/{ => include}/wavemap/core/utils/neighbors/impl/grid_adjacency_inl.h (88%) rename library/{ => include}/wavemap/core/utils/neighbors/impl/grid_neighborhood_inl.h (90%) create mode 100644 library/include/wavemap/core/utils/neighbors/impl/ndtree_adjacency_inl.h rename library/{ => include}/wavemap/core/utils/neighbors/ndtree_adjacency.h (86%) rename library/{ => include}/wavemap/core/utils/print/container.h (79%) rename library/{ => include}/wavemap/core/utils/print/eigen.h (74%) create mode 100644 library/include/wavemap/core/utils/profiler_interface.h rename library/{ => include}/wavemap/core/utils/query/classified_map.h (97%) rename library/{ => include}/wavemap/core/utils/query/impl/classified_map_inl.h (95%) rename library/{ => include}/wavemap/core/utils/query/impl/occupancy_classifier_inl.h (92%) rename library/{ => include}/wavemap/core/utils/query/impl/occupancy_inl.h (75%) rename library/{ => include}/wavemap/core/utils/query/impl/point_sampler_inl.h (85%) rename library/{ => include}/wavemap/core/utils/query/impl/query_accelerator_inl.h (97%) rename library/{ => include}/wavemap/core/utils/query/map_interpolator.h (93%) rename library/{ => include}/wavemap/core/utils/query/occupancy.h (84%) rename library/{ => include}/wavemap/core/utils/query/occupancy_classifier.h (91%) rename library/{ => include}/wavemap/core/utils/query/point_sampler.h (91%) rename library/{ => include}/wavemap/core/utils/query/probability_conversions.h (79%) rename library/{ => include}/wavemap/core/utils/query/query_accelerator.h (96%) rename library/{ => include}/wavemap/core/utils/random_number_generator.h (88%) rename library/{ => include}/wavemap/core/utils/sdf/full_euclidean_sdf_generator.h (91%) rename library/{ => include}/wavemap/core/utils/sdf/quasi_euclidean_sdf_generator.h (87%) rename library/{ => include}/wavemap/core/utils/thread_pool.h (95%) rename library/{ => include}/wavemap/core/utils/time/stopwatch.h (78%) rename library/{ => include}/wavemap/core/utils/time/time.h (75%) rename library/{ => include}/wavemap/io/file_conversions.h (100%) rename library/{ => include}/wavemap/io/impl/streamable_types_impl.h (100%) rename library/{ => include}/wavemap/io/stream_conversions.h (100%) rename library/{ => include}/wavemap/io/streamable_types.h (100%) rename library/{wavemap => src}/core/CMakeLists.txt (95%) rename library/{wavemap => src}/core/config/value_with_unit.cc (100%) rename library/{wavemap => src}/core/integrator/integrator_base.cc (100%) rename library/{wavemap => src}/core/integrator/integrator_factory.cc (100%) rename library/{wavemap => src}/core/integrator/measurement_model/constant_ray.cc (100%) rename library/{wavemap => src}/core/integrator/measurement_model/continuous_beam.cc (100%) rename library/{wavemap => src}/core/integrator/measurement_model/continuous_ray.cc (100%) rename library/{wavemap => src}/core/integrator/measurement_model/measurement_model_factory.cc (100%) rename library/{wavemap => src}/core/integrator/projection_model/circular_projector.cc (100%) rename library/{wavemap => src}/core/integrator/projection_model/ouster_projector.cc (100%) rename library/{wavemap => src}/core/integrator/projection_model/pinhole_camera_projector.cc (100%) rename library/{wavemap => src}/core/integrator/projection_model/projector_factory.cc (100%) rename library/{wavemap => src}/core/integrator/projection_model/spherical_projector.cc (100%) rename library/{wavemap => src}/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.cc (100%) rename library/{wavemap => src}/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc (97%) rename library/{wavemap => src}/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc (97%) rename library/{wavemap => src}/core/integrator/projective/coarse_to_fine/wavelet_integrator.cc (100%) rename library/{wavemap => src}/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc (100%) rename library/{wavemap => src}/core/integrator/projective/projective_integrator.cc (95%) rename library/{wavemap => src}/core/integrator/ray_tracing/ray_tracing_integrator.cc (100%) rename library/{wavemap => src}/core/map/hashed_blocks.cc (100%) rename library/{wavemap => src}/core/map/hashed_chunked_wavelet_octree.cc (94%) rename library/{wavemap => src}/core/map/hashed_chunked_wavelet_octree_block.cc (99%) rename library/{wavemap => src}/core/map/hashed_wavelet_octree.cc (93%) rename library/{wavemap => src}/core/map/hashed_wavelet_octree_block.cc (98%) rename library/{wavemap => src}/core/map/map_base.cc (100%) rename library/{wavemap => src}/core/map/map_factory.cc (100%) rename library/{wavemap => src}/core/map/volumetric_octree.cc (100%) rename library/{wavemap => src}/core/map/wavelet_octree.cc (100%) rename library/{wavemap => src}/core/utils/query/classified_map.cc (99%) rename library/{wavemap => src}/core/utils/query/point_sampler.cc (100%) rename library/{wavemap => src}/core/utils/query/query_accelerator.cc (100%) rename library/{wavemap => src}/core/utils/sdf/full_euclidean_sdf_generator.cc (97%) rename library/{wavemap => src}/core/utils/sdf/quasi_euclidean_sdf_generator.cc (96%) rename library/{wavemap => src}/core/utils/thread_pool.cc (94%) rename library/{wavemap => src}/core/utils/time/stopwatch.cc (100%) rename library/{wavemap => src}/io/CMakeLists.txt (100%) rename library/{wavemap => src}/io/file_conversions.cc (100%) rename library/{wavemap => src}/io/stream_conversions.cc (100%) rename library/{wavemap => src}/pipeline/CMakeLists.txt (100%) rename library/{wavemap => }/test/CMakeLists.txt (100%) rename library/{wavemap => }/test/core/data_structure/test_aabb.cc (100%) rename library/{wavemap => }/test/core/data_structure/test_image.cc (100%) rename library/{wavemap => }/test/core/data_structure/test_ndtree.cc (100%) rename library/{wavemap => }/test/core/data_structure/test_pointcloud.cc (100%) rename library/{wavemap => }/test/core/data_structure/test_sparse_vector.cc (100%) rename library/{wavemap => }/test/core/indexing/test_index_conversions.cc (100%) rename library/{wavemap => }/test/core/indexing/test_ndtree_index.cc (100%) rename library/{wavemap => }/test/core/integrator/projection_model/test_circular_projector.cc (100%) rename library/{wavemap => }/test/core/integrator/projection_model/test_image_projectors.cc (100%) rename library/{wavemap => }/test/core/integrator/projection_model/test_spherical_projector.cc (100%) rename library/{wavemap => }/test/core/integrator/test_hierarchical_range_image.cc (100%) rename library/{wavemap => }/test/core/integrator/test_measurement_models.cc (100%) rename library/{wavemap => }/test/core/integrator/test_pointcloud_integrators.cc (100%) rename library/{wavemap => }/test/core/integrator/test_range_image_intersector.cc (100%) rename library/{wavemap => }/test/core/map/test_haar_cell.cc (100%) rename library/{wavemap => }/test/core/map/test_hashed_blocks.cc (100%) rename library/{wavemap => }/test/core/map/test_map.cc (100%) rename library/{wavemap => }/test/core/map/test_volumetric_octree.cc (100%) rename library/{wavemap => }/test/core/utils/bits/test_bit_operations.cc (100%) rename library/{wavemap => }/test/core/utils/data/test_comparisons.cc (100%) rename library/{wavemap => }/test/core/utils/data/test_fill.cc (100%) rename library/{wavemap => }/test/core/utils/iterate/test_grid_iterator.cc (100%) rename library/{wavemap => }/test/core/utils/iterate/test_ray_iterator.cc (100%) rename library/{wavemap => }/test/core/utils/iterate/test_subtree_iterator.cc (100%) rename library/{wavemap => }/test/core/utils/math/test_approximate_trigonometry.cc (100%) rename library/{wavemap => }/test/core/utils/math/test_int_math.cc (100%) rename library/{wavemap => }/test/core/utils/math/test_tree_math.cc (100%) rename library/{wavemap => }/test/core/utils/neighbors/test_adjacency.cc (100%) rename library/{wavemap => }/test/core/utils/neighbors/test_grid_adjacency.cc (100%) rename library/{wavemap => }/test/core/utils/neighbors/test_grid_neighborhood.cc (100%) rename library/{wavemap => }/test/core/utils/neighbors/test_ndtree_adjacency.cc (100%) rename library/{wavemap => }/test/core/utils/query/test_classified_map.cc (100%) rename library/{wavemap => }/test/core/utils/query/test_map_interpolator.cpp (100%) rename library/{wavemap => }/test/core/utils/query/test_occupancy_classifier.cc (100%) rename library/{wavemap => }/test/core/utils/query/test_probability_conversions.cc (100%) rename library/{wavemap => }/test/core/utils/query/test_query_accelerator.cc (100%) rename library/{wavemap => }/test/core/utils/sdf/test_sdf_generators.cc (100%) rename library/{wavemap => }/test/core/utils/test_thread_pool.cc (100%) rename {ros/wavemap_rviz_plugin => library/test}/include/CPPLINT.cfg (100%) rename library/{ => test/include}/wavemap/test/config_generator.h (100%) rename library/{ => test/include}/wavemap/test/eigen_utils.h (100%) rename library/{ => test/include}/wavemap/test/fixture_base.h (100%) rename library/{ => test/include}/wavemap/test/geometry_generator.h (100%) rename library/{wavemap => }/test/io/test_file_conversions.cc (100%) delete mode 100644 library/wavemap/CMakeLists.txt delete mode 100644 library/wavemap/cmake/compiler_options.cmake delete mode 100644 library/wavemap/core/utils/neighbors/impl/ndtree_adjacency_inl.h diff --git a/library/CMakeLists.txt b/library/CMakeLists.txt new file mode 100644 index 000000000..1d38a70f9 --- /dev/null +++ b/library/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.10) + +project(wavemap VERSION 1.6.3 LANGUAGES CXX) + +# Dependencies +find_package(Eigen3 REQUIRED NO_MODULE) +find_package(tracy QUIET) # Optional dependency +find_package(PkgConfig REQUIRED) +pkg_check_modules(glog REQUIRED libglog) +# TODO(victorr): Check if tracy is correctly found and linked in when it is +# available as an installed (system) package. + +include(cmake/compiler_options.cmake) + +# Libraries +add_subdirectory(src/core) +add_subdirectory(src/io) +add_subdirectory(src/pipeline) + +## Tests +#if (CATKIN_ENABLE_TESTING) +# add_subdirectory(test) +#endif () + +## Benchmarks +#if (ENABLE_BENCHMARKING) +# add_subdirectory(benchmark) +#endif () diff --git a/library/wavemap/benchmark/CMakeLists.txt b/library/benchmark/CMakeLists.txt similarity index 100% rename from library/wavemap/benchmark/CMakeLists.txt rename to library/benchmark/CMakeLists.txt diff --git a/library/wavemap/benchmark/benchmark_haar_transforms.cc b/library/benchmark/benchmark_haar_transforms.cc similarity index 100% rename from library/wavemap/benchmark/benchmark_haar_transforms.cc rename to library/benchmark/benchmark_haar_transforms.cc diff --git a/library/wavemap/benchmark/benchmark_sparse_vector.cc b/library/benchmark/benchmark_sparse_vector.cc similarity index 100% rename from library/wavemap/benchmark/benchmark_sparse_vector.cc rename to library/benchmark/benchmark_sparse_vector.cc diff --git a/library/cmake/compiler_options.cmake b/library/cmake/compiler_options.cmake new file mode 100644 index 000000000..41c960365 --- /dev/null +++ b/library/cmake/compiler_options.cmake @@ -0,0 +1,36 @@ +# TODO(victorr): Unify this with the macro in wavemap-extras.cmake +# This function is used to set the compile features, definitions, options and +# include paths for all targets in the wavemap library. +function(set_wavemap_target_properties target) + target_compile_features(${target} PUBLIC cxx_std_17) + target_compile_definitions(${target} PUBLIC + $<$:_USE_MATH_DEFINES>) + # target_compile_options( + # ${target} + # PRIVATE # MSVC + # $<$:/W4> + # $<$:/WX> + # # Clang/AppleClang + # $<$:-fcolor-diagnostics> + # $<$:-Werror> + # $<$:-Wall> + # $<$:-Wextra> + # $<$:-Wconversion> + # $<$:-Wno-sign-conversion> + # # GNU + # $<$:-fdiagnostics-color=always> + # $<$:-Werror> + # $<$:-Wall> + # $<$:-Wextra> + # $<$:-pedantic> + # $<$:-Wcast-align> + # $<$:-Wcast-qual> + # $<$:-Wconversion> + # $<$:-Wdisabled-optimization> + # $<$:-Woverloaded-virtual>) + set(include_dirs ${PROJECT_SOURCE_DIR}/include/wavemap) + get_filename_component(include_dirs ${include_dirs} PATH) + target_include_directories(${target} PUBLIC + $ + $) +endfunction() diff --git a/library/wavemap/cmake/wavemap-extras.cmake b/library/cmake/wavemap-extras.cmake similarity index 100% rename from library/wavemap/cmake/wavemap-extras.cmake rename to library/cmake/wavemap-extras.cmake diff --git a/library/wavemap/CPPLINT.cfg b/library/include/CPPLINT.cfg similarity index 100% rename from library/wavemap/CPPLINT.cfg rename to library/include/CPPLINT.cfg diff --git a/library/include/wavemap/3rd_party/minkindr/LICENSE.txt b/library/include/wavemap/3rd_party/minkindr/LICENSE.txt new file mode 100644 index 000000000..439e869c9 --- /dev/null +++ b/library/include/wavemap/3rd_party/minkindr/LICENSE.txt @@ -0,0 +1,24 @@ +Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of ETH Zurich nor the names of its contributors may + be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL ETH ZURICH BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR +OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED +OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/library/include/wavemap/3rd_party/minkindr/angle-axis.h b/library/include/wavemap/3rd_party/minkindr/angle-axis.h new file mode 100644 index 000000000..b3c6c7563 --- /dev/null +++ b/library/include/wavemap/3rd_party/minkindr/angle-axis.h @@ -0,0 +1,171 @@ +// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#ifndef WAVEMAP_3RD_PARTY_MINKINDR_ANGLE_AXIS_H_ +#define WAVEMAP_3RD_PARTY_MINKINDR_ANGLE_AXIS_H_ + +#include + +#include + +namespace kindr { +namespace minimal { + +template +class RotationQuaternionTemplate; + +/// \class AngleAxis +/// \brief a minimal implementation of an angle and axis representation of +/// rotation +/// This rotation takes vectors from frame B to frame A, written +/// as \f${}_{A}\mathbf{v} = \mathbf{C}_{AB} {}_{B}\mathbf{v}\f$ +/// +/// The angle is assumed to be in radians everywhere +/// +/// In code, we write: +/// +/// \code{.cpp} +/// A_v = C_A_B.rotate(B_v); +/// \endcode +/// +template +class AngleAxisTemplate { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef Eigen::Matrix Vector3; + + typedef Eigen::Matrix Vector4; + + typedef Eigen::AngleAxis Implementation; + + typedef Eigen::Matrix RotationMatrix; + + /// \brief initialize to identity. + AngleAxisTemplate(); + AngleAxisTemplate(const AngleAxisTemplate& other) = default; + + /// \brief initialize from the angle and rotation axis (angle first). + AngleAxisTemplate(Scalar angle, Scalar v1, Scalar v2, Scalar v3); + + /// \brief initialize from the angle and rotation axis. + AngleAxisTemplate(Scalar angle, const Vector3& axis); + + /// \brief initialize from an Eigen angleAxis. + explicit AngleAxisTemplate(const Implementation& angleAxis); + + /// \brief initialize from a rotation matrix. + explicit AngleAxisTemplate(const RotationMatrix& matrix); + + /// \brief initialize from an Eigen quaternion. + explicit AngleAxisTemplate(const RotationQuaternionTemplate& quat); + + /// \brief initialize from a angle-scaled axis vector. + explicit AngleAxisTemplate(const Vector3& angleAxis); + + ~AngleAxisTemplate(); + + /// \brief Returns the rotation angle. + Scalar angle() const; + + /// \brief Sets the rotation angle. + void setAngle(Scalar angle); + + /// \brief Returns the rotation axis. + const Vector3& axis() const; + + /// \brief Sets the rotation axis. + void setAxis(const Vector3& axis); + + /// \brief Sets the rotation axis. + void setAxis(Scalar v1, Scalar v2, Scalar v3); + + /// \brief get the components of the angle/axis as a vector (angle first). + Vector4 vector() const; + + /// \brief get a copy of the representation that is unique. + AngleAxisTemplate getUnique() const; + + /// \brief set the angle/axis to its unique representation. + AngleAxisTemplate& setUnique(); + + /// \brief set the rotation to identity. + AngleAxisTemplate& setIdentity(); + + /// \brief get a copy of the rotation inverted. + AngleAxisTemplate inverse() const; + + /// \deprecated use inverse() instead. + AngleAxisTemplate inverted() const __attribute__((deprecated)); + + /// \brief rotate a vector, v. + Vector3 rotate(const Vector3& v) const; + + /// \brief rotate a vector, v. + Vector4 rotate4(const Vector4& v) const; + + /// \brief rotate a vector, v. + Vector3 inverseRotate(const Vector3& v) const; + + /// \brief rotate a vector, v. + Vector4 inverseRotate4(const Vector4& v) const; + + /// \brief cast to the implementation type. + Implementation& toImplementation(); + + /// \brief cast to the implementation type. + const Implementation& toImplementation() const; + + /// \brief get the angle between this and the other rotation. + Scalar getDisparityAngle(const AngleAxisTemplate& rhs) const; + + /// \brief enforce the unit length constraint. + AngleAxisTemplate& normalize(); + + /// \brief compose two rotations. + AngleAxisTemplate operator*( + const AngleAxisTemplate& rhs) const; + + /// \brief assignment operator. + AngleAxisTemplate& operator=(const AngleAxisTemplate& rhs); + + /// \brief get the rotation matrix. + RotationMatrix getRotationMatrix() const; + + private: + Implementation C_A_B_; +}; + +typedef AngleAxisTemplate AngleAxis; + +template +std::ostream& operator<<(std::ostream& out, + const AngleAxisTemplate& rhs); + +} // namespace minimal +} // namespace kindr + +#include + +#endif // WAVEMAP_3RD_PARTY_MINKINDR_ANGLE_AXIS_H_ diff --git a/library/include/wavemap/3rd_party/minkindr/common.h b/library/include/wavemap/3rd_party/minkindr/common.h new file mode 100644 index 000000000..48cac3501 --- /dev/null +++ b/library/include/wavemap/3rd_party/minkindr/common.h @@ -0,0 +1,66 @@ +// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#ifndef WAVEMAP_3RD_PARTY_MINKINDR_COMMON_H_ +#define WAVEMAP_3RD_PARTY_MINKINDR_COMMON_H_ + +#include +#include + +namespace kindr { +namespace minimal { + +inline void skewMatrix(const Eigen::Vector3d& v, Eigen::Matrix3d* skew) { + CHECK_NOTNULL(skew); + skew->setZero(); + (*skew)(0, 1) = -v[2]; + (*skew)(1, 0) = v[2]; + (*skew)(0, 2) = v[1]; + (*skew)(2, 0) = -v[1]; + (*skew)(1, 2) = -v[0]; + (*skew)(2, 1) = v[0]; +} + +inline void skewMatrix(const Eigen::Vector3d& v, + Eigen::Map* skew) { + CHECK_NOTNULL(skew); + skew->setZero(); + (*skew)(0, 1) = -v[2]; + (*skew)(1, 0) = v[2]; + (*skew)(0, 2) = v[1]; + (*skew)(2, 0) = -v[1]; + (*skew)(1, 2) = -v[0]; + (*skew)(2, 1) = v[0]; +} + +inline Eigen::Matrix3d skewMatrix(const Eigen::Vector3d& v) { + Eigen::Matrix3d skew; + skewMatrix(v, &skew); + return skew; +} + +} // namespace minimal +} // namespace kindr + +#endif // WAVEMAP_3RD_PARTY_MINKINDR_COMMON_H_ diff --git a/library/include/wavemap/3rd_party/minkindr/impl/angle-axis-inl.h b/library/include/wavemap/3rd_party/minkindr/impl/angle-axis-inl.h new file mode 100644 index 000000000..446b19e93 --- /dev/null +++ b/library/include/wavemap/3rd_party/minkindr/impl/angle-axis-inl.h @@ -0,0 +1,269 @@ +// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#ifndef WAVEMAP_3RD_PARTY_MINKINDR_IMPL_ANGLE_AXIS_INL_H_ +#define WAVEMAP_3RD_PARTY_MINKINDR_IMPL_ANGLE_AXIS_INL_H_ + +#include + +#include +#include +#include + +namespace kindr { +namespace minimal { + +template +AngleAxisTemplate::AngleAxisTemplate() + : C_A_B_(Implementation::Identity()) {} + +template +AngleAxisTemplate::AngleAxisTemplate(Scalar w, Scalar x, Scalar y, + Scalar z) + : C_A_B_(w, Vector3(x, y, z)) { + CHECK_NEAR(Vector3(x, y, z).squaredNorm(), static_cast(1.0), + static_cast(1e-4)); +} + +template +AngleAxisTemplate::AngleAxisTemplate( + Scalar angle, const typename AngleAxisTemplate::Vector3& axis) + : C_A_B_(angle, axis) { + CHECK_NEAR(axis.squaredNorm(), static_cast(1.0), + static_cast(1e-4)); +} + +template +AngleAxisTemplate::AngleAxisTemplate(const Implementation& angleAxis) + : C_A_B_(angleAxis) {} + +template +AngleAxisTemplate::AngleAxisTemplate(const RotationMatrix& matrix) + : C_A_B_(matrix) { + // \todo furgalep Check that the matrix was good... +} + +template +AngleAxisTemplate::AngleAxisTemplate(const Vector3& rotationVector) { + const Scalar angle_rad = rotationVector.norm(); + if (angle_rad < std::numeric_limits::epsilon()) { + setIdentity(); + } else { + Vector3 axis = rotationVector; + axis = axis / angle_rad; + + C_A_B_ = Implementation(angle_rad, axis); + } +} + +template +AngleAxisTemplate::AngleAxisTemplate( + const RotationQuaternionTemplate& quat) + : C_A_B_(quat.toImplementation()) {} + +template +AngleAxisTemplate::~AngleAxisTemplate() {} + +template +AngleAxisTemplate& AngleAxisTemplate::operator=( + const AngleAxisTemplate& rhs) { + if (this != &rhs) { + C_A_B_ = rhs.C_A_B_; + } + return *this; +} + +template +Scalar AngleAxisTemplate::angle() const { + return C_A_B_.angle(); +} + +template +void AngleAxisTemplate::setAngle(Scalar angle) { + C_A_B_.angle() = angle; +} + +template +const typename AngleAxisTemplate::Vector3& +AngleAxisTemplate::axis() const { + return C_A_B_.axis(); +} + +template +void AngleAxisTemplate::setAxis(const Vector3& axis) { + CHECK_NEAR(axis.squaredNorm(), static_cast(1.0), + static_cast(1e-4)); + C_A_B_.axis() = axis; +} + +template +void AngleAxisTemplate::setAxis(Scalar v1, Scalar v2, Scalar v3) { + C_A_B_.axis() = Vector3(v1, v2, v3); + CHECK_NEAR(C_A_B_.axis().squaredNorm(), static_cast(1.0), + static_cast(1e-4)); +} + +template +typename AngleAxisTemplate::Vector4 AngleAxisTemplate::vector() + const { + Vector4 vector; + vector(0) = angle(); + vector.template tail<3>() = C_A_B_.axis(); + return vector; +} + +template +AngleAxisTemplate AngleAxisTemplate::getUnique() const { + // first wraps angle into [-pi,pi) + AngleAxisTemplate aa(fmod(angle() + M_PI, 2 * M_PI) - M_PI, C_A_B_.axis()); + if (aa.angle() > 0) { + return aa; + } else if (aa.angle() < 0) { + if (aa.angle() != -M_PI) { + return AngleAxisTemplate(-aa.angle(), -aa.axis()); + } else { // angle == -pi, so axis must be viewed further, because -pi,axis + // does the same as -pi,-axis. + if (aa.axis()[0] < 0) { + return AngleAxisTemplate(-aa.angle(), -aa.axis()); + } else if (aa.axis()[0] > 0) { + return AngleAxisTemplate(-aa.angle(), aa.axis()); + } else { // v1 == 0 + if (aa.axis()[1] < 0) { + return AngleAxisTemplate(-aa.angle(), -aa.axis()); + } else if (aa.axis()[1] > 0) { + return AngleAxisTemplate(-aa.angle(), aa.axis()); + } else { // v2 == 0 + if (aa.axis()[2] < 0) { // v3 must be -1 or 1 + return AngleAxisTemplate(-aa.angle(), -aa.axis()); + } else { + return AngleAxisTemplate(-aa.angle(), aa.axis()); + } + } + } + } + } else { // angle == 0 + return AngleAxisTemplate(); + } +} + +template +AngleAxisTemplate& AngleAxisTemplate::setUnique() { + *this = getUnique(); + return *this; +} + +template +AngleAxisTemplate& AngleAxisTemplate::setIdentity() { + C_A_B_ = C_A_B_.Identity(); + return *this; +} + +template +AngleAxisTemplate AngleAxisTemplate::inverse() const { + return AngleAxisTemplate(C_A_B_.inverse()); +} + +template +AngleAxisTemplate AngleAxisTemplate::inverted() const { + return inverse(); +} + +template +typename AngleAxisTemplate::Vector3 AngleAxisTemplate::rotate( + const AngleAxisTemplate::Vector3& v) const { + return C_A_B_ * v; +} + +template +typename AngleAxisTemplate::Vector4 AngleAxisTemplate::rotate4( + const AngleAxisTemplate::Vector4& v) const { + AngleAxisTemplate::Vector4 vprime; + vprime[3] = v[3]; + vprime.template head<3>() = C_A_B_ * v.template head<3>(); + return vprime; +} + +template +typename AngleAxisTemplate::Vector3 +AngleAxisTemplate::inverseRotate( + const AngleAxisTemplate::Vector3& v) const { + return C_A_B_.inverse() * v; +} + +template +typename AngleAxisTemplate::Vector4 +AngleAxisTemplate::inverseRotate4( + const typename AngleAxisTemplate::Vector4& v) const { + Eigen::Vector4d vprime; + vprime[3] = v[3]; + vprime.template head<3>() = C_A_B_.inverse() * v.template head<3>(); + return vprime; +} + +template +typename AngleAxisTemplate::Implementation& +AngleAxisTemplate::toImplementation() { + return C_A_B_; +} + +template +const typename AngleAxisTemplate::Implementation& +AngleAxisTemplate::toImplementation() const { + return C_A_B_; +} + +template +AngleAxisTemplate& AngleAxisTemplate::normalize() { + C_A_B_.axis().normalize(); + return *this; +} + +template +AngleAxisTemplate AngleAxisTemplate::operator*( + const AngleAxisTemplate& rhs) const { + return AngleAxisTemplate(Implementation(C_A_B_ * rhs.C_A_B_)); +} + +template +Scalar AngleAxisTemplate::getDisparityAngle( + const AngleAxisTemplate& rhs) const { + return (rhs * this->inverse()).getUnique().angle(); +} + +template +std::ostream& operator<<(std::ostream& out, + const AngleAxisTemplate& rhs) { + out << rhs.vector().transpose(); + return out; +} + +template +typename AngleAxisTemplate::RotationMatrix +AngleAxisTemplate::getRotationMatrix() const { + return C_A_B_.matrix(); +} + +} // namespace minimal +} // namespace kindr +#endif // WAVEMAP_3RD_PARTY_MINKINDR_IMPL_ANGLE_AXIS_INL_H_ diff --git a/library/include/wavemap/3rd_party/minkindr/impl/quat-transformation-inl.h b/library/include/wavemap/3rd_party/minkindr/impl/quat-transformation-inl.h new file mode 100644 index 000000000..96fcaa4df --- /dev/null +++ b/library/include/wavemap/3rd_party/minkindr/impl/quat-transformation-inl.h @@ -0,0 +1,302 @@ +// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#ifndef WAVEMAP_3RD_PARTY_MINKINDR_IMPL_QUAT_TRANSFORMATION_INL_H_ +#define WAVEMAP_3RD_PARTY_MINKINDR_IMPL_QUAT_TRANSFORMATION_INL_H_ + +#include + +#include +#include + +namespace kindr { +namespace minimal { + +template +QuatTransformationTemplate::QuatTransformationTemplate() { + setIdentity(); +} + +template +QuatTransformationTemplate::QuatTransformationTemplate( + const RotationQuaternionTemplate& q_A_B, const Position& A_t_A_B) + : q_A_B_(q_A_B), A_t_A_B_(A_t_A_B) {} + +template +QuatTransformationTemplate::QuatTransformationTemplate( + const typename Rotation::Implementation& q_A_B, const Position& A_t_A_B) + : q_A_B_(q_A_B), A_t_A_B_(A_t_A_B) {} + +template +QuatTransformationTemplate::QuatTransformationTemplate( + const Position& A_t_A_B, const RotationQuaternionTemplate& q_A_B) + : q_A_B_(q_A_B), A_t_A_B_(A_t_A_B) {} + +template +QuatTransformationTemplate::QuatTransformationTemplate( + const Position& A_t_A_B, const typename Rotation::Implementation& q_A_B) + : q_A_B_(q_A_B), A_t_A_B_(A_t_A_B) {} + +template +QuatTransformationTemplate::QuatTransformationTemplate( + const TransformationMatrix& T) + : q_A_B_(T.template topLeftCorner<3, 3>().eval()), + A_t_A_B_(T.template topRightCorner<3, 1>().eval()) {} + +template +QuatTransformationTemplate::QuatTransformationTemplate( + const QuatTransformationTemplate::Vector6& x_t_r) + : q_A_B_(x_t_r.template tail<3>().eval()), + A_t_A_B_(x_t_r.template head<3>().eval()) {} + +template +QuatTransformationTemplate::~QuatTransformationTemplate() {} + +template +void QuatTransformationTemplate::setIdentity() { + q_A_B_.setIdentity(); + A_t_A_B_.setZero(); +} + +template +typename QuatTransformationTemplate::Position& +QuatTransformationTemplate::getPosition() { + return A_t_A_B_; +} + +template +const typename QuatTransformationTemplate::Position& +QuatTransformationTemplate::getPosition() const { + return A_t_A_B_; +} + +template +typename QuatTransformationTemplate::Rotation& +QuatTransformationTemplate::getRotation() { + return q_A_B_; +} + +template +const typename QuatTransformationTemplate::Rotation& +QuatTransformationTemplate::getRotation() const { + return q_A_B_; +} + +template +const Eigen::Quaternion& +QuatTransformationTemplate::getEigenQuaternion() const { + return getRotation().toImplementation(); +} + +template +typename QuatTransformationTemplate::TransformationMatrix +QuatTransformationTemplate::getTransformationMatrix() const { + TransformationMatrix transformation_matrix; + transformation_matrix.setIdentity(); + transformation_matrix.template topLeftCorner<3, 3>() = + q_A_B_.getRotationMatrix(); + transformation_matrix.template topRightCorner<3, 1>() = A_t_A_B_; + return transformation_matrix; +} + +template +Eigen::Matrix QuatTransformationTemplate::asVector() + const { + return (Eigen::Matrix() << q_A_B_.w(), q_A_B_.x(), q_A_B_.y(), + q_A_B_.z(), A_t_A_B_) + .finished(); +} + +template +typename QuatTransformationTemplate::RotationMatrix +QuatTransformationTemplate::getRotationMatrix() const { + return q_A_B_.getRotationMatrix(); +} + +template +QuatTransformationTemplate +QuatTransformationTemplate::operator*( + const QuatTransformationTemplate& rhs) const { + return QuatTransformationTemplate( + q_A_B_ * rhs.q_A_B_, A_t_A_B_ + q_A_B_.rotate(rhs.A_t_A_B_)); +} + +template +typename QuatTransformationTemplate::Vector3 +QuatTransformationTemplate::transform( + const typename QuatTransformationTemplate::Vector3& rhs) const { + return q_A_B_.rotate(rhs) + A_t_A_B_; +} + +template +typename QuatTransformationTemplate::Matrix3X +QuatTransformationTemplate::transformVectorized( + const typename QuatTransformationTemplate::Matrix3X& rhs) const { + CHECK_GT(rhs.cols(), 0); + return q_A_B_.rotateVectorized(rhs).colwise() + A_t_A_B_; +} + +template +typename QuatTransformationTemplate::Vector3 +QuatTransformationTemplate::operator*( + const typename QuatTransformationTemplate::Vector3& rhs) const { + return transform(rhs); +} + +template +typename QuatTransformationTemplate::Vector4 +QuatTransformationTemplate::transform4( + const typename QuatTransformationTemplate::Vector4& rhs) const { + QuatTransformationTemplate::Vector4 rval; + rval[3] = rhs[3]; + rval.template head<3>() = + q_A_B_.rotate(rhs.template head<3>()) + rhs[3] * A_t_A_B_; + return rval; +} + +template +typename QuatTransformationTemplate::Vector3 +QuatTransformationTemplate::inverseTransform(const Vector3& rhs) const { + return q_A_B_.inverseRotate(rhs - A_t_A_B_); +} + +template +typename QuatTransformationTemplate::Vector4 +QuatTransformationTemplate::inverseTransform4( + const typename QuatTransformationTemplate::Vector4& rhs) const { + typename QuatTransformationTemplate::Vector4 rval; + rval.template head<3>() = + q_A_B_.inverseRotate(rhs.template head<3>() - A_t_A_B_ * rhs[3]); + rval[3] = rhs[3]; + return rval; +} + +template +QuatTransformationTemplate QuatTransformationTemplate::inverse() + const { + return QuatTransformationTemplate(q_A_B_.inverse(), + -q_A_B_.inverseRotate(A_t_A_B_)); +} + +template +QuatTransformationTemplate +QuatTransformationTemplate::inverted() const { + return inverse(); +} + +template +typename QuatTransformationTemplate::Vector6 +QuatTransformationTemplate::log() const { + return log(*this); +} + +template +QuatTransformationTemplate QuatTransformationTemplate::exp( + const Vector6& vec) { + return QuatTransformationTemplate(vec); +} + +template +typename QuatTransformationTemplate::Vector6 +QuatTransformationTemplate::log( + const QuatTransformationTemplate& T) { + AngleAxisTemplate angleaxis(T.q_A_B_); + return (Vector6() << T.A_t_A_B_, T.q_A_B_.log()).finished(); +} + +template +QuatTransformationTemplate& +QuatTransformationTemplate::setRandom() { + q_A_B_.setRandom(); + A_t_A_B_.setRandom(); + return *this; +} + +template +QuatTransformationTemplate& +QuatTransformationTemplate::setRandom(Scalar norm_translation) { + setRandom(); + A_t_A_B_.normalize(); + A_t_A_B_ *= norm_translation; + return *this; +} + +template +QuatTransformationTemplate& +QuatTransformationTemplate::setRandom(Scalar norm_translation, + Scalar angle_rad) { + q_A_B_.setRandom(angle_rad); + A_t_A_B_.setRandom().normalize(); + A_t_A_B_ *= norm_translation; + return *this; +} + +template +std::ostream& operator<<(std::ostream& out, + const QuatTransformationTemplate& pose) { + out << pose.getTransformationMatrix(); + return out; +} + +template +bool QuatTransformationTemplate::operator==( + const QuatTransformationTemplate& rhs) const { + return q_A_B_ == rhs.q_A_B_ && A_t_A_B_ == rhs.A_t_A_B_; +} + +template +QuatTransformationTemplate +QuatTransformationTemplate::constructAndRenormalizeRotation( + const TransformationMatrix& T) { + return QuatTransformationTemplate( + Rotation::constructAndRenormalize( + T.template topLeftCorner<3, 3>().eval()), + T.template topRightCorner<3, 1>().eval()); +} + +template +template +QuatTransformationTemplate +QuatTransformationTemplate::cast() const { + return QuatTransformationTemplate( + getRotation().template cast(), + getPosition().template cast()); +} + +template +QuatTransformationTemplate interpolateComponentwise( + const QuatTransformationTemplate& T_a, + const QuatTransformationTemplate& T_b, const double lambda) { + CHECK_GE(lambda, 0.0); + CHECK_LE(lambda, 1.0); + const PositionTemplate p_int = + T_a.getPosition() + lambda * (T_b.getPosition() - T_a.getPosition()); + const Eigen::Quaternion q_int = + T_a.getEigenQuaternion().slerp(lambda, T_b.getEigenQuaternion()); + return QuatTransformationTemplate(q_int, p_int); +} + +} // namespace minimal +} // namespace kindr +#endif // WAVEMAP_3RD_PARTY_MINKINDR_IMPL_QUAT_TRANSFORMATION_INL_H_ diff --git a/library/include/wavemap/3rd_party/minkindr/impl/rotation-quaternion-inl.h b/library/include/wavemap/3rd_party/minkindr/impl/rotation-quaternion-inl.h new file mode 100644 index 000000000..7b2399616 --- /dev/null +++ b/library/include/wavemap/3rd_party/minkindr/impl/rotation-quaternion-inl.h @@ -0,0 +1,591 @@ +// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#ifndef WAVEMAP_3RD_PARTY_MINKINDR_IMPL_ROTATION_QUATERNION_INL_H_ +#define WAVEMAP_3RD_PARTY_MINKINDR_IMPL_ROTATION_QUATERNION_INL_H_ + +#include + +#include +#include +#include + +namespace kindr { +namespace minimal { + +template +struct EPS { + static constexpr Scalar value() { return static_cast(1.0e-5); } + static constexpr Scalar normalization_value() { + return static_cast(1.0e-4); + } +}; +template <> +struct EPS { + static constexpr double value() { return 1.0e-8; } + static constexpr double normalization_value() { return 1.0e-4; } +}; +template <> +struct EPS { + static constexpr float value() { return 1.0e-5f; } + static constexpr float normalization_value() { return 1.0e-4f; } +}; + +/// \brief initialize to identity +template +RotationQuaternionTemplate::RotationQuaternionTemplate() + : q_A_B_(Implementation::Identity()) {} + +/// \brief initialize from real and imaginary components (real first) +template +RotationQuaternionTemplate::RotationQuaternionTemplate(Scalar w, + Scalar x, + Scalar y, + Scalar z) + : q_A_B_(w, x, y, z) { + CHECK_NEAR(squaredNorm(), static_cast(1.0), + EPS::normalization_value()); +} + +/// \brief initialize from real and imaginary components +template +RotationQuaternionTemplate::RotationQuaternionTemplate( + Scalar real, + const typename RotationQuaternionTemplate::Vector3& imaginary) + : q_A_B_(real, imaginary[0], imaginary[1], imaginary[2]) { + CHECK_NEAR(squaredNorm(), static_cast(1.0), + EPS::normalization_value()); +} + +/// \brief initialize from an Eigen quaternion +template +RotationQuaternionTemplate::RotationQuaternionTemplate( + const Implementation& quaternion) + : q_A_B_(quaternion) { + CHECK_NEAR(squaredNorm(), static_cast(1.0), + EPS::normalization_value()); +} + +namespace detail { + +template +inline bool isLessThenEpsilons4thRoot(Scalar_ x) { + static const Scalar_ epsilon4thRoot = + pow(std::numeric_limits::epsilon(), 1.0 / 4.0); + return x < epsilon4thRoot; +} + +template +inline Scalar_ arcSinXOverX(Scalar_ x) { + if (isLessThenEpsilons4thRoot(fabs(x))) { + return Scalar_(1.0) + x * x * Scalar_(1.0 / 6.0); + } + return asin(x) / x; +} +} // namespace detail + +/// \brief initialize from axis-scaled angle vector +template +RotationQuaternionTemplate::RotationQuaternionTemplate( + const Vector3& axis_scaled_angle) { + *this = exp(axis_scaled_angle); +} + +/// \brief initialize from a rotation matrix +template +RotationQuaternionTemplate::RotationQuaternionTemplate( + const RotationMatrix& matrix) + : q_A_B_(matrix) { + CHECK(isValidRotationMatrix(matrix)) << matrix; +} + +template +RotationQuaternionTemplate +RotationQuaternionTemplate::fromApproximateRotationMatrix( + const RotationMatrix& matrix) { + // We still want the input matrix to resemble a rotation matrix to avoid + // bug hiding. + CHECK(isValidRotationMatrix( + matrix, static_cast(EPS::normalization_value()))); + // http://people.csail.mit.edu/bkph/articles/Nearest_Orthonormal_Matrix.pdf + // as discussed in https://github.com/ethz-asl/kindr/issues/55 , + // code by Philipp Krüsi. + Eigen::JacobiSVD svd(matrix, Eigen::ComputeFullV); + + RotationMatrix correction = + svd.matrixV().col(0) * svd.matrixV().col(0).transpose() / + svd.singularValues()(0) + + svd.matrixV().col(1) * svd.matrixV().col(1).transpose() / + svd.singularValues()(1) + + svd.matrixV().col(2) * svd.matrixV().col(2).transpose() / + svd.singularValues()(2); + + return RotationQuaternionTemplate( + RotationMatrix(matrix * correction)); +} + +template +RotationQuaternionTemplate::RotationQuaternionTemplate( + const AngleAxisTemplate& angleAxis) + : q_A_B_(angleAxis.toImplementation()) {} + +template +RotationQuaternionTemplate::~RotationQuaternionTemplate() {} + +/// \brief the real component of the quaternion +template +Scalar RotationQuaternionTemplate::w() const { + return q_A_B_.w(); +} + +/// \brief the first imaginary component of the quaternion +template +Scalar RotationQuaternionTemplate::x() const { + return q_A_B_.x(); +} + +/// \brief the second imaginary component of the quaternion +template +Scalar RotationQuaternionTemplate::y() const { + return q_A_B_.y(); +} + +/// \brief the third imaginary component of the quaternion +template +Scalar RotationQuaternionTemplate::z() const { + return q_A_B_.z(); +} + +/// \brief assignment operator +template +RotationQuaternionTemplate& +RotationQuaternionTemplate::operator=( + const RotationQuaternionTemplate& rhs) { + if (this != &rhs) { + q_A_B_ = rhs.q_A_B_; + } + return *this; +} + +/// \brief the imaginary components of the quaterion. +template +typename RotationQuaternionTemplate::Imaginary +RotationQuaternionTemplate::imaginary() const { + return Imaginary(q_A_B_.x(), q_A_B_.y(), q_A_B_.z()); +} + +/// \brief get the components of the quaternion as a vector (real first) +template +typename RotationQuaternionTemplate::Vector4 +RotationQuaternionTemplate::vector() const { + return Vector4(q_A_B_.w(), q_A_B_.x(), q_A_B_.y(), q_A_B_.z()); +} + +/// \brief set the quaternion by its values (real, imaginary) +template +void RotationQuaternionTemplate::setValues(Scalar w, Scalar x, Scalar y, + Scalar z) { + q_A_B_ = Implementation(w, x, y, z); + CHECK_NEAR(squaredNorm(), static_cast(1.0), + static_cast(1e-4)); +} + +/// \brief set the quaternion by its real and imaginary parts +template +void RotationQuaternionTemplate::setParts(Scalar real, + const Imaginary& imag) { + q_A_B_ = Implementation(real, imag[0], imag[1], imag[2]); + CHECK_NEAR(squaredNorm(), static_cast(1.0), + static_cast(1e-4)); +} + +/// \brief get a copy of the representation that is unique +template +RotationQuaternionTemplate +RotationQuaternionTemplate::getUnique() const { + if (this->w() > 0) { + return *this; + } else if (this->w() < 0) { + return RotationQuaternionTemplate(-this->w(), -this->x(), + -this->y(), -this->z()); + } + // w == 0 + if (this->x() > 0) { + return *this; + } else if (this->x() < 0) { + return RotationQuaternionTemplate(-this->w(), -this->x(), + -this->y(), -this->z()); + } + // x == 0 + if (this->y() > 0) { + return *this; + } else if (this->y() < 0) { + return RotationQuaternionTemplate(-this->w(), -this->x(), + -this->y(), -this->z()); + } + // y == 0 + if (this->z() > 0) { // z must be either -1 or 1 in this case + return *this; + } else { + return RotationQuaternionTemplate(-this->w(), -this->x(), + -this->y(), -this->z()); + } +} + +/// \brief set the quaternion to its unique representation +template +RotationQuaternionTemplate& +RotationQuaternionTemplate::setUnique() { + *this = getUnique(); + return *this; +} + +/// \brief set the quaternion to identity +template +RotationQuaternionTemplate& +RotationQuaternionTemplate::setIdentity() { + q_A_B_.setIdentity(); + return *this; +} + +/// \brief set to random rotation +template +RotationQuaternionTemplate& +RotationQuaternionTemplate::setRandom() { + Vector4 coeffs; + coeffs.setRandom().normalize(); + q_A_B_ = Implementation(coeffs(0), coeffs(1), coeffs(2), coeffs(3)); + this->setUnique(); + return *this; +} + +/// \brief set to random rotation with a given angle +template +RotationQuaternionTemplate& +RotationQuaternionTemplate::setRandom(Scalar angle_rad) { + Vector3 rotation_axis; + rotation_axis.setRandom().normalize(); + q_A_B_ = Implementation(Eigen::AngleAxis(angle_rad, rotation_axis)); + return *this; +} + +/// \brief get a copy of the quaternion inverted. +template +RotationQuaternionTemplate RotationQuaternionTemplate::inverse() + const { + return conjugated(); +} + +/// \brief get a copy of the quaternion inverted. +template +RotationQuaternionTemplate +RotationQuaternionTemplate::inverted() const { + return inverse(); +} + +/// \brief get a copy of the conjugate of the quaternion. +template +RotationQuaternionTemplate +RotationQuaternionTemplate::conjugated() const { + // Own implementation since Eigen::conjugate does not use the correct + // scalar type for the greater than zero comparison. + return RotationQuaternionTemplate( + Implementation(q_A_B_.w(), -q_A_B_.x(), -q_A_B_.y(), -q_A_B_.z())); +} + +/// \brief rotate a vector, v +template +typename RotationQuaternionTemplate::Vector3 +RotationQuaternionTemplate::rotate( + const typename RotationQuaternionTemplate::Vector3& v) const { + return q_A_B_ * v; +} + +/// \brief rotate vectors, v +template +typename RotationQuaternionTemplate::Matrix3X +RotationQuaternionTemplate::rotateVectorized( + const typename RotationQuaternionTemplate::Matrix3X& v) const { + CHECK_GT(v.cols(), 0); + return q_A_B_.toRotationMatrix() * v; +} + +/// \brief rotate a vector, v +template +typename RotationQuaternionTemplate::Vector4 +RotationQuaternionTemplate::rotate4( + const typename RotationQuaternionTemplate::Vector4& v) const { + typename RotationQuaternionTemplate::Vector4 vprime; + vprime[3] = v[3]; + vprime.template head<3>() = q_A_B_ * v.template head<3>(); + return vprime; +} + +/// \brief rotate a vector, v +template +typename RotationQuaternionTemplate::Vector3 +RotationQuaternionTemplate::inverseRotate( + const typename RotationQuaternionTemplate::Vector3& v) const { + return q_A_B_.inverse() * v; +} + +/// \brief rotate a vector, v +template +typename RotationQuaternionTemplate::Vector4 +RotationQuaternionTemplate::inverseRotate4( + const typename RotationQuaternionTemplate::Vector4& v) const { + typename RotationQuaternionTemplate::Vector4 vprime; + vprime[3] = v[3]; + vprime.template head<3>() = q_A_B_.inverse() * v.template head<3>(); + return vprime; +} + +/// \brief cast to the implementation type +template +typename RotationQuaternionTemplate::Implementation& +RotationQuaternionTemplate::toImplementation() { + return q_A_B_; +} + +/// \brief cast to the implementation type +template +const typename RotationQuaternionTemplate::Implementation& +RotationQuaternionTemplate::toImplementation() const { + return q_A_B_; +} + +/// \brief get the norm of the quaternion +template +Scalar RotationQuaternionTemplate::norm() const { + return q_A_B_.norm(); +} + +/// \brief get the squared norm of the quaternion +template +Scalar RotationQuaternionTemplate::squaredNorm() const { + return q_A_B_.squaredNorm(); +} + +/// \brief enforce the unit length constraint +template +RotationQuaternionTemplate& +RotationQuaternionTemplate::normalize() { + q_A_B_.normalize(); + return *this; +} + +template +RotationQuaternionTemplate +RotationQuaternionTemplate::operator*( + const RotationQuaternionTemplate& rhs) const { + CHECK(!std::is_arithmetic::value) + << "Please provide a specialized " + "function for this specific arithmetic type. This function is only a " + "workaround for non-arithmetic types."; + Implementation result = q_A_B_ * rhs.q_A_B_; + + // Check if the multiplication has resulted in the quaternion no longer being + // approximately normalized. + // Cover the case of non-arithmetic types that may not provide an + // implementation of std::abs. + Scalar signed_norm_diff = result.squaredNorm() - static_cast(1.0); + if ((signed_norm_diff > EPS::normalization_value()) || + (signed_norm_diff < -EPS::normalization_value())) { + // renormalize + result.normalize(); + } + return RotationQuaternionTemplate(result); +} + +template <> +inline RotationQuaternionTemplate +RotationQuaternionTemplate::operator*( + const RotationQuaternionTemplate& rhs) const { + Implementation result = q_A_B_ * rhs.q_A_B_; + normalizationHelper(&result); + return RotationQuaternionTemplate(result); +} + +template <> +inline RotationQuaternionTemplate +RotationQuaternionTemplate::operator*( + const RotationQuaternionTemplate& rhs) const { + Implementation result = q_A_B_ * rhs.q_A_B_; + normalizationHelper(&result); + return RotationQuaternionTemplate(result); +} + +template +RotationQuaternionTemplate +RotationQuaternionTemplate::operator*( + const AngleAxisTemplate& rhs) const { + return *this * RotationQuaternionTemplate(rhs); +} + +template +std::ostream& operator<<(std::ostream& out, + const RotationQuaternionTemplate& rhs) { + out << rhs.vector(); + return out; +} + +template +typename RotationQuaternionTemplate::RotationMatrix +RotationQuaternionTemplate::getRotationMatrix() const { + return q_A_B_.matrix(); +} + +template +Scalar RotationQuaternionTemplate::getDisparityAngle( + const RotationQuaternionTemplate& rhs) const { + return AngleAxisTemplate(rhs * this->inverse()).getUnique().angle(); +} + +template +Scalar RotationQuaternionTemplate::getDisparityAngle( + const AngleAxisTemplate& rhs) const { + return AngleAxisTemplate(rhs * this->inverse()).getUnique().angle(); +} + +template +typename RotationQuaternionTemplate::Vector3 +RotationQuaternionTemplate::log( + const RotationQuaternionTemplate& q) { + const Eigen::Matrix a = q.imaginary(); + const Scalar na = a.norm(); + const Scalar eta = q.w(); + Scalar scale; + if (fabs(eta) < na) { // use eta because it is more precise than na to + // calculate the scale. No singularities here. + // check sign of eta so that we can be sure that log(-q) = log(q) + if (eta >= 0) { + scale = acos(eta) / na; + } else { + scale = -acos(-eta) / na; + } + } else { + /* + * In this case more precision is in na than in eta so lets use na only to + * calculate the scale: + * + * assume first eta > 0 and 1 > na > 0. + * u = asin (na) / na (this implies u in [1, pi/2], because + * na i in [0, 1] sin (u * na) = na sin^2 (u * na) = na^2 cos^2 (u * na) = 1 + * - na^2 (1 = ||q|| = eta^2 + na^2) cos^2 (u * na) = eta^2 (eta > 0, u * + * na = asin(na) in [0, pi/2] => cos(u * na) >= 0 ) cos (u * na) = eta (u * + * na in [ 0, pi/2] ) u = acos (eta) / na + * + * So the for eta > 0 it is acos(eta) / na == asin(na) / na. + * From some geometric considerations (mirror the setting at the hyper plane + * q==0) it follows for eta < 0 that (pi - asin(na)) / na = acos(eta) / na. + */ + if (eta > 0) { + // For asin(na)/ na the singularity na == 0 can be removed. We can ask + // (e.g. Wolfram alpha) for its series expansion at na = 0. And that is + // done in the following function. + scale = detail::arcSinXOverX(na); + } else { + // the negative is here so that log(-q) == log(q) + scale = -detail::arcSinXOverX(na); + } + } + return a * (Scalar(2.0) * scale); +} + +template +RotationQuaternionTemplate RotationQuaternionTemplate::exp( + const Vector3& dx) { + // Method of implementing this function that is accurate to numerical + // precision from Grassia, F. S. (1998). Practical parameterization of + // rotations using the exponential map. journal of graphics, gpu, and game + // tools, 3(3):29–48. + double theta = dx.norm(); + // na is 1/theta sin(theta/2) + double na; + if (detail::isLessThenEpsilons4thRoot(theta)) { + static const double one_over_48 = 1.0 / 48.0; + na = 0.5 + (theta * theta) * one_over_48; + } else { + na = sin(theta * 0.5) / theta; + } + double ct = cos(theta * 0.5); + return RotationQuaternionTemplate(ct, dx[0] * na, dx[1] * na, + dx[2] * na); +} + +template +typename RotationQuaternionTemplate::Vector3 +RotationQuaternionTemplate::log() const { + return log(*this); +} + +template +bool RotationQuaternionTemplate::isValidRotationMatrix( + const RotationMatrix& matrix) { + return isValidRotationMatrix(matrix, EPS::value()); +} + +template +bool RotationQuaternionTemplate::isValidRotationMatrix( + const RotationMatrix& matrix, const Scalar threshold) { + if (std::fabs(matrix.determinant() - static_cast(1.0)) > threshold) { + VLOG(200) << matrix.determinant(); + VLOG(200) << matrix.determinant() - static_cast(1.0); + return false; + } + if ((matrix * matrix.transpose() - RotationMatrix::Identity()) + .cwiseAbs() + .maxCoeff() > threshold) { + VLOG(200) << matrix * matrix.transpose(); + VLOG(200) << matrix * matrix.transpose() - RotationMatrix::Identity(); + return false; + } + return true; +} + +template +template +RotationQuaternionTemplate +RotationQuaternionTemplate::cast() const { + // renormalization needed to allow casting to increased precision + return RotationQuaternionTemplate::constructAndRenormalize( + getRotationMatrix().template cast()); +} + +template +void RotationQuaternionTemplate::normalizationHelper( + Implementation* quaternion) const { + CHECK_NOTNULL(quaternion); + // check if the multiplication has resulted in the quaternion no longer being + // approximately normalized. + if (std::abs(quaternion->squaredNorm() - static_cast(1)) > + EPS::normalization_value()) { + // renormalize + quaternion->normalize(); + } +} + +} // namespace minimal +} // namespace kindr +#endif // WAVEMAP_3RD_PARTY_MINKINDR_IMPL_ROTATION_QUATERNION_INL_H_ diff --git a/library/include/wavemap/3rd_party/minkindr/position.h b/library/include/wavemap/3rd_party/minkindr/position.h new file mode 100644 index 000000000..651f9b70c --- /dev/null +++ b/library/include/wavemap/3rd_party/minkindr/position.h @@ -0,0 +1,41 @@ +// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#ifndef WAVEMAP_3RD_PARTY_MINKINDR_POSITION_H_ +#define WAVEMAP_3RD_PARTY_MINKINDR_POSITION_H_ + +#include + +namespace kindr { +namespace minimal { + +template +using PositionTemplate = Eigen::Matrix; + +typedef PositionTemplate Position; + +} // namespace minimal +} // namespace kindr + +#endif // WAVEMAP_3RD_PARTY_MINKINDR_POSITION_H_ diff --git a/library/include/wavemap/3rd_party/minkindr/quat-transformation.h b/library/include/wavemap/3rd_party/minkindr/quat-transformation.h new file mode 100644 index 000000000..f89227a87 --- /dev/null +++ b/library/include/wavemap/3rd_party/minkindr/quat-transformation.h @@ -0,0 +1,216 @@ +// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#ifndef WAVEMAP_3RD_PARTY_MINKINDR_QUAT_TRANSFORMATION_H_ +#define WAVEMAP_3RD_PARTY_MINKINDR_QUAT_TRANSFORMATION_H_ + +#include +#include + +#include +#include + +namespace kindr { +namespace minimal { + +/// \class QuatTransformation +/// \brief A frame transformation built from a quaternion and a point +/// +/// This transformation takes points from frame B to frame A, written +/// as \f${}_{A}\mathbf{p} = \mathbf{T}_{AB} {}_{B}\mathbf{p}\f$ +/// +/// In code, we write: +/// +/// \code{.cpp} +/// A_p = T_A_B.transform(B_p); +/// \endcode +/// +template +class QuatTransformationTemplate { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef Eigen::Matrix Vector3; + typedef Eigen::Matrix Vector4; + typedef Eigen::Matrix Vector6; + + typedef Eigen::Matrix Matrix3X; + + typedef PositionTemplate Position; + typedef RotationQuaternionTemplate Rotation; + typedef Eigen::Matrix RotationMatrix; + typedef Eigen::Matrix TransformationMatrix; + + /// \brief Constructor of identity transformation. + QuatTransformationTemplate(); + QuatTransformationTemplate(const QuatTransformationTemplate& other) = default; + + explicit QuatTransformationTemplate( + const RotationQuaternionTemplate& q_A_B, const Position& A_t_A_B); + explicit QuatTransformationTemplate( + const typename Rotation::Implementation& q_A_B, const Position& A_t_A_B); + + explicit QuatTransformationTemplate(const Position& A_t_A_B, + const Rotation& q_A_B); + explicit QuatTransformationTemplate( + const Position& A_t_A_B, const typename Rotation::Implementation& q_A_B); + + explicit QuatTransformationTemplate(const TransformationMatrix& T); + + /// \brief a constructor based on the exponential map. + /// translational part in the first 3 dimensions, + /// rotational part in the last 3 dimensions. + explicit QuatTransformationTemplate(const Vector6& x_t_r); + + ~QuatTransformationTemplate(); + + void setIdentity(); + + /// \brief set to random transformation. + QuatTransformationTemplate& setRandom(); + + /// \brief set to random transformation with a given translation norm. + QuatTransformationTemplate& setRandom(Scalar norm_translation); + + /// \brief set to random transformation with a given translation norm and + /// rotation angle. + QuatTransformationTemplate& setRandom(Scalar norm_translation, + Scalar angle_rad); + + /// \brief get the position component. + Position& getPosition(); + + /// \brief get the position component. + const Position& getPosition() const; + + /// \brief get the rotation component. + Rotation& getRotation(); + + /// \brief get the rotation component. + const Rotation& getRotation() const; + + /// \brief get the rotation component as an Eigen Quaternion directly. + const Eigen::Quaternion& getEigenQuaternion() const; + + /// \brief get the transformation matrix. + TransformationMatrix getTransformationMatrix() const; + + /// \brief get the rotation matrix. + RotationMatrix getRotationMatrix() const; + + /// \brief get the quaternion of rotation and the position as a vector. + /// [w x y z, x y z] + Eigen::Matrix asVector() const; + + /// \brief compose two transformations. + QuatTransformationTemplate operator*( + const QuatTransformationTemplate& rhs) const; + + /// \brief transform a point. + Vector3 operator*(const Vector3& rhs) const; + + /// \brief transform a point. + Vector3 transform(const Vector3& rhs) const; + + /// \brief transform points. + Matrix3X transformVectorized(const Matrix3X& rhs) const; + + /// \brief transform a point. + Vector4 transform4(const Vector4& rhs) const; + + /// \brief transform a point by the inverse. + Vector3 inverseTransform(const Vector3& rhs) const; + + /// \brief transform a point by the inverse. + Vector4 inverseTransform4(const Vector4& rhs) const; + + /// \brief get the logarithmic map of the transformation + /// note: this is the log map of SO(3)xR(3) and not SE(3) + /// \return vector form of log map with first 3 components the translational + /// part and the last three the rotational part. + Vector6 log() const; + + /// \brief get the exponential map of the parameters, resulting in a valid + /// transformation note: this is the exp map of SO(3)xR(3) and not SE(3) + /// \param[in] vec vector form of log map with first 3 components the + /// translational + /// part and the last three the rotational part. + /// \return The corresponding Transformation. + static QuatTransformationTemplate exp(const Vector6& vec); + + /// \brief get the logarithmic map of the transformation + /// note: this is the log map of SO(3)xR(3) and not SE(3) + /// \return vector form of log map with first 3 components the translational + /// part and the last three the rotational part. + static Vector6 log(const QuatTransformationTemplate& vec); + + /// \brief return a copy of the transformation inverted. + QuatTransformationTemplate inverse() const; + + /// \deprecated use inverse() instead. + QuatTransformationTemplate inverted() const + __attribute__((deprecated)); + + /// \brief check for binary equality. + bool operator==(const QuatTransformationTemplate& rhs) const; + + /// \brief Factory to construct a QuatTransformTemplate from a transformation + /// matrix with a near orthonormal rotation matrix. + static QuatTransformationTemplate constructAndRenormalizeRotation( + const TransformationMatrix& T); + + /// \brief cast scalar elements to another type + template + QuatTransformationTemplate cast() const; + + private: + /// The quaternion that takes vectors from B to A. + /// + /// \code{.cpp} + /// A_v = q_A_B_.rotate(B_v); + /// \endcode + Rotation q_A_B_; + /// The vector from the origin of A to the origin of B + /// expressed in A + Position A_t_A_B_; +}; + +typedef QuatTransformationTemplate QuatTransformation; + +template +std::ostream& operator<<(std::ostream& out, + const QuatTransformationTemplate& pose); + +// Exponential interpolation (i.e., Slerp) in SO(3) and linear interpolation in +// R3. Lambda is in [0, 1], with 0 returning T_a, and 1 returning T_b. +template +inline QuatTransformationTemplate interpolateComponentwise( + const QuatTransformationTemplate& T_a, + const QuatTransformationTemplate& T_b, const double lambda); +} // namespace minimal +} // namespace kindr + +#include + +#endif // WAVEMAP_3RD_PARTY_MINKINDR_QUAT_TRANSFORMATION_H_ diff --git a/library/include/wavemap/3rd_party/minkindr/rotation-quaternion.h b/library/include/wavemap/3rd_party/minkindr/rotation-quaternion.h new file mode 100644 index 000000000..7ea6a7a5b --- /dev/null +++ b/library/include/wavemap/3rd_party/minkindr/rotation-quaternion.h @@ -0,0 +1,241 @@ +// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#ifndef WAVEMAP_3RD_PARTY_MINKINDR_ROTATION_QUATERNION_H_ +#define WAVEMAP_3RD_PARTY_MINKINDR_ROTATION_QUATERNION_H_ + +#include + +#include + +namespace kindr { +namespace minimal { + +template +class AngleAxisTemplate; + +/// \class RotationQuaternion +/// \brief a minimal implementation of a passive Hamiltonian rotation +/// (unit-length) quaternion +/// +/// This rotation takes vectors from frame B to frame A, written +/// as \f${}_{A}\mathbf{v} = \mathbf{C}_{AB} {}_{B}\mathbf{v}\f$ +/// +/// In code, we write: +/// +/// \code{.cpp} +/// A_v = q_A_B.rotate(B_v); +/// \endcode +/// +template +class RotationQuaternionTemplate { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef Eigen::Matrix Vector3; + + typedef Vector3 Imaginary; + + typedef Eigen::Matrix Vector4; + + typedef Eigen::Matrix Matrix3X; + + typedef Eigen::Quaternion Implementation; + + typedef Eigen::Matrix RotationMatrix; + + /// \brief initialize to identity. + RotationQuaternionTemplate(); + RotationQuaternionTemplate(const RotationQuaternionTemplate& other) = default; + + /// \brief initialize from angle scaled axis. + explicit RotationQuaternionTemplate(const Vector3& angle_scaled_axis); + + /// \brief initialize from real and imaginary components (real first). + explicit RotationQuaternionTemplate(Scalar w, Scalar x, Scalar y, Scalar z); + + /// \brief initialize from real and imaginary components. + explicit RotationQuaternionTemplate(Scalar real, const Imaginary& imaginary); + + /// \brief initialize from an Eigen quaternion. + explicit RotationQuaternionTemplate(const Implementation& quaternion); + + /// \brief initialize from a rotation matrix. + explicit RotationQuaternionTemplate(const RotationMatrix& matrix); + + /// \brief take an approximate rotation matrix, recover the closest matrix + /// in SO(3) and construct. + static RotationQuaternionTemplate fromApproximateRotationMatrix( + const RotationMatrix& matrix); + + /// \brief initialize from an AngleAxis. + explicit RotationQuaternionTemplate( + const AngleAxisTemplate& angleAxis); + + ~RotationQuaternionTemplate(); + + /// \brief the real component of the quaternion. + Scalar w() const; + /// \brief the first imaginary component of the quaternion. + Scalar x() const; + /// \brief the second imaginary component of the quaternion. + Scalar y() const; + /// \brief the third imaginary component of the quaternion. + Scalar z() const; + + /// \brief the imaginary components of the quaterion. + Imaginary imaginary() const; + + /// \brief get the components of the quaternion as a vector (real first). + Vector4 vector() const; + + /// \brief set the quaternion by its values (real, imaginary). + void setValues(Scalar w, Scalar x, Scalar y, Scalar z); + + /// \brief set the quaternion by its real and imaginary parts. + void setParts(Scalar real, const Imaginary& imag); + + /// \brief get a copy of the representation that is unique. + RotationQuaternionTemplate getUnique() const; + + /// \brief set the quaternion to its unique representation. + RotationQuaternionTemplate& setUnique(); + + /// \brief set the quaternion to identity. + RotationQuaternionTemplate& setIdentity(); + + /// \brief set to random rotation. + RotationQuaternionTemplate& setRandom(); + + /// \brief set to random rotation with a given angle. + RotationQuaternionTemplate& setRandom(Scalar angle_rad); + + /// \brief get a copy of the quaternion inverted. + RotationQuaternionTemplate inverse() const; + + /// \deprecated use inverse instead. + RotationQuaternionTemplate inverted() const + __attribute__((deprecated)); + + /// \brief get a copy of the conjugate of the quaternion. + RotationQuaternionTemplate conjugated() const; + + /// \brief rotate a vector, v. + Vector3 rotate(const Vector3& v) const; + + /// \brief rotate vectors v. + Matrix3X rotateVectorized(const Matrix3X& v) const; + + /// \brief rotate a vector, v. + Vector4 rotate4(const Vector4& v) const; + + /// \brief rotate a vector, v. + Vector3 inverseRotate(const Vector3& v) const; + + /// \brief rotate a vector, v. + Vector4 inverseRotate4(const Vector4& v) const; + + /// \brief cast to the implementation type. + Implementation& toImplementation(); + + /// \brief cast to the implementation type. + const Implementation& toImplementation() const; + + /// \brief get the norm of the quaternion. + Scalar norm() const; + + /// \brief get the squared norm of the quaternion. + Scalar squaredNorm() const; + + /// \brief get the angle [rad] between this and the other quaternion. + Scalar getDisparityAngle(const RotationQuaternionTemplate& rhs) const; + + /// \brief get the angle [rad] between this and the angle axis. + Scalar getDisparityAngle(const AngleAxisTemplate& rhs) const; + + /// \brief enforce the unit length constraint. + RotationQuaternionTemplate& normalize(); + + /// \brief compose two quaternions. + RotationQuaternionTemplate operator*( + const RotationQuaternionTemplate& rhs) const; + + /// \brief compose quaternion and angle axis. + RotationQuaternionTemplate operator*( + const AngleAxisTemplate& rhs) const; + + /// \brief assignment operator. + RotationQuaternionTemplate& operator=( + const RotationQuaternionTemplate& rhs); + + /// \brief get the rotation matrix. + RotationMatrix getRotationMatrix() const; + + /// \brief check for binary equality. + bool operator==(const RotationQuaternionTemplate& rhs) const { + return vector() == rhs.vector(); + } + + // Compute the matrix log of the quaternion. + static Vector3 log(const RotationQuaternionTemplate& q); + + // Compute the matrix exponential of the quaternion. + static RotationQuaternionTemplate exp(const Vector3& dx); + + Vector3 log() const; + + /// \brief Check the validity of a rotation matrix. + static bool isValidRotationMatrix(const RotationMatrix& matrix); + static bool isValidRotationMatrix(const RotationMatrix& matrix, + const Scalar threshold); + + /// \brief Factory to construct a RotationQuaternionTemplate from a near + /// orthonormal rotation matrix. + inline static RotationQuaternionTemplate constructAndRenormalize( + const RotationMatrix& R) { + return RotationQuaternionTemplate(Implementation(R).normalized()); + } + + /// \brief cast scalar elements to another type + template + RotationQuaternionTemplate cast() const; + + private: + void normalizationHelper(Implementation* quaternion) const; + + Implementation q_A_B_; +}; + +typedef RotationQuaternionTemplate RotationQuaternion; + +template +std::ostream& operator<<(std::ostream& out, + const RotationQuaternionTemplate& rhs); + +} // namespace minimal +} // namespace kindr + +#include + +#endif // WAVEMAP_3RD_PARTY_MINKINDR_ROTATION_QUATERNION_H_ diff --git a/library/wavemap/core/common.h b/library/include/wavemap/core/common.h similarity index 69% rename from library/wavemap/core/common.h rename to library/include/wavemap/core/common.h index e5de7ee5b..56304b4e4 100644 --- a/library/wavemap/core/common.h +++ b/library/include/wavemap/core/common.h @@ -1,12 +1,11 @@ -#ifndef WAVEMAP_COMMON_H_ -#define WAVEMAP_COMMON_H_ +#ifndef WAVEMAP_CORE_COMMON_H_ +#define WAVEMAP_CORE_COMMON_H_ #include #include #include -#include -#include +#include #include "wavemap/core/utils/data/constants.h" @@ -42,31 +41,16 @@ using Point = Vector; using Point2D = Point<2>; using Point3D = Point<3>; -using Transformation2D = - kindr::minimal::Transformation2DTemplate; using Transformation3D = kindr::minimal::QuatTransformationTemplate; -template -using Transformation = typename std::conditional_t< - dim == 2, Transformation2D, - std::conditional_t>; -using Rotation2D = Transformation2D::Rotation; using Rotation3D = Transformation3D::Rotation; -template -using Rotation = typename std::conditional_t< - dim == 2, Rotation2D, - std::conditional_t>; template inline constexpr int dim_v = T::RowsAtCompileTime; template <> -inline constexpr int dim_v = 2; -template <> inline constexpr int dim_v = 3; template <> -inline constexpr int dim_v = 2; -template <> inline constexpr int dim_v = 3; using ImageCoordinates = Vector2D; @@ -84,4 +68,4 @@ constexpr auto kHalfPi = constants::kHalfPi; constexpr auto kQuarterPi = constants::kQuarterPi; } // namespace wavemap -#endif // WAVEMAP_COMMON_H_ +#endif // WAVEMAP_CORE_COMMON_H_ diff --git a/library/wavemap/core/config/config_base.h b/library/include/wavemap/core/config/config_base.h similarity index 96% rename from library/wavemap/core/config/config_base.h rename to library/include/wavemap/core/config/config_base.h index 587831bb0..49df84729 100644 --- a/library/wavemap/core/config/config_base.h +++ b/library/include/wavemap/core/config/config_base.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_CONFIG_CONFIG_BASE_H_ -#define WAVEMAP_CONFIG_CONFIG_BASE_H_ +#ifndef WAVEMAP_CORE_CONFIG_CONFIG_BASE_H_ +#define WAVEMAP_CORE_CONFIG_CONFIG_BASE_H_ #include @@ -90,4 +90,4 @@ struct ConfigBase { #include "wavemap/core/config/impl/config_base_inl.h" -#endif // WAVEMAP_CONFIG_CONFIG_BASE_H_ +#endif // WAVEMAP_CORE_CONFIG_CONFIG_BASE_H_ diff --git a/library/wavemap/core/config/impl/config_base_inl.h b/library/include/wavemap/core/config/impl/config_base_inl.h similarity index 98% rename from library/wavemap/core/config/impl/config_base_inl.h rename to library/include/wavemap/core/config/impl/config_base_inl.h index 2709759f1..da14f3386 100644 --- a/library/wavemap/core/config/impl/config_base_inl.h +++ b/library/include/wavemap/core/config/impl/config_base_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_CONFIG_IMPL_CONFIG_BASE_INL_H_ -#define WAVEMAP_CONFIG_IMPL_CONFIG_BASE_INL_H_ +#ifndef WAVEMAP_CORE_CONFIG_IMPL_CONFIG_BASE_INL_H_ +#define WAVEMAP_CORE_CONFIG_IMPL_CONFIG_BASE_INL_H_ #include @@ -184,4 +184,4 @@ ConfigBase::from( } } // namespace wavemap -#endif // WAVEMAP_CONFIG_IMPL_CONFIG_BASE_INL_H_ +#endif // WAVEMAP_CORE_CONFIG_IMPL_CONFIG_BASE_INL_H_ diff --git a/library/wavemap/core/config/impl/type_selector_inl.h b/library/include/wavemap/core/config/impl/type_selector_inl.h similarity index 96% rename from library/wavemap/core/config/impl/type_selector_inl.h rename to library/include/wavemap/core/config/impl/type_selector_inl.h index b2e4890bc..e4157083b 100644 --- a/library/wavemap/core/config/impl/type_selector_inl.h +++ b/library/include/wavemap/core/config/impl/type_selector_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_CONFIG_IMPL_TYPE_SELECTOR_INL_H_ -#define WAVEMAP_CONFIG_IMPL_TYPE_SELECTOR_INL_H_ +#ifndef WAVEMAP_CORE_CONFIG_IMPL_TYPE_SELECTOR_INL_H_ +#define WAVEMAP_CORE_CONFIG_IMPL_TYPE_SELECTOR_INL_H_ #include #include @@ -133,4 +133,4 @@ std::optional TypeSelector::from( } } // namespace wavemap -#endif // WAVEMAP_CONFIG_IMPL_TYPE_SELECTOR_INL_H_ +#endif // WAVEMAP_CORE_CONFIG_IMPL_TYPE_SELECTOR_INL_H_ diff --git a/library/wavemap/core/config/impl/value_with_unit_inl.h b/library/include/wavemap/core/config/impl/value_with_unit_inl.h similarity index 92% rename from library/wavemap/core/config/impl/value_with_unit_inl.h rename to library/include/wavemap/core/config/impl/value_with_unit_inl.h index 9df68a633..46ea7cf30 100644 --- a/library/wavemap/core/config/impl/value_with_unit_inl.h +++ b/library/include/wavemap/core/config/impl/value_with_unit_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_CONFIG_IMPL_VALUE_WITH_UNIT_INL_H_ -#define WAVEMAP_CONFIG_IMPL_VALUE_WITH_UNIT_INL_H_ +#ifndef WAVEMAP_CORE_CONFIG_IMPL_VALUE_WITH_UNIT_INL_H_ +#define WAVEMAP_CORE_CONFIG_IMPL_VALUE_WITH_UNIT_INL_H_ #include @@ -61,4 +61,4 @@ std::optional> ValueWithUnit::from( } } // namespace wavemap -#endif // WAVEMAP_CONFIG_IMPL_VALUE_WITH_UNIT_INL_H_ +#endif // WAVEMAP_CORE_CONFIG_IMPL_VALUE_WITH_UNIT_INL_H_ diff --git a/library/wavemap/core/config/param.h b/library/include/wavemap/core/config/param.h similarity index 94% rename from library/wavemap/core/config/param.h rename to library/include/wavemap/core/config/param.h index fd99c2445..c46ea5620 100644 --- a/library/wavemap/core/config/param.h +++ b/library/include/wavemap/core/config/param.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_CONFIG_PARAM_H_ -#define WAVEMAP_CONFIG_PARAM_H_ +#ifndef WAVEMAP_CORE_CONFIG_PARAM_H_ +#define WAVEMAP_CORE_CONFIG_PARAM_H_ #include #include @@ -74,4 +74,4 @@ using Array = Value::Array; using Map = Value::Map; } // namespace wavemap::param -#endif // WAVEMAP_CONFIG_PARAM_H_ +#endif // WAVEMAP_CORE_CONFIG_PARAM_H_ diff --git a/library/wavemap/core/config/param_checks.h b/library/include/wavemap/core/config/param_checks.h similarity index 92% rename from library/wavemap/core/config/param_checks.h rename to library/include/wavemap/core/config/param_checks.h index 70acb2c52..0bda4584b 100644 --- a/library/wavemap/core/config/param_checks.h +++ b/library/include/wavemap/core/config/param_checks.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_CONFIG_PARAM_CHECKS_H_ -#define WAVEMAP_CONFIG_PARAM_CHECKS_H_ +#ifndef WAVEMAP_CORE_CONFIG_PARAM_CHECKS_H_ +#define WAVEMAP_CORE_CONFIG_PARAM_CHECKS_H_ #include #include @@ -45,4 +45,4 @@ bool is_param(A value, B threshold, bool verbose, const std::string& value_name, } } // namespace wavemap -#endif // WAVEMAP_CONFIG_PARAM_CHECKS_H_ +#endif // WAVEMAP_CORE_CONFIG_PARAM_CHECKS_H_ diff --git a/library/wavemap/core/config/type_selector.h b/library/include/wavemap/core/config/type_selector.h similarity index 92% rename from library/wavemap/core/config/type_selector.h rename to library/include/wavemap/core/config/type_selector.h index 1fb5c7cdf..d3011dc41 100644 --- a/library/wavemap/core/config/type_selector.h +++ b/library/include/wavemap/core/config/type_selector.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_CONFIG_TYPE_SELECTOR_H_ -#define WAVEMAP_CONFIG_TYPE_SELECTOR_H_ +#ifndef WAVEMAP_CORE_CONFIG_TYPE_SELECTOR_H_ +#define WAVEMAP_CORE_CONFIG_TYPE_SELECTOR_H_ #include @@ -55,4 +55,4 @@ struct TypeSelector { #include "wavemap/core/config/impl/type_selector_inl.h" -#endif // WAVEMAP_CONFIG_TYPE_SELECTOR_H_ +#endif // WAVEMAP_CORE_CONFIG_TYPE_SELECTOR_H_ diff --git a/library/wavemap/core/config/value_with_unit.h b/library/include/wavemap/core/config/value_with_unit.h similarity index 93% rename from library/wavemap/core/config/value_with_unit.h rename to library/include/wavemap/core/config/value_with_unit.h index 435a9db90..970025a39 100644 --- a/library/wavemap/core/config/value_with_unit.h +++ b/library/include/wavemap/core/config/value_with_unit.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_CONFIG_VALUE_WITH_UNIT_H_ -#define WAVEMAP_CONFIG_VALUE_WITH_UNIT_H_ +#ifndef WAVEMAP_CORE_CONFIG_VALUE_WITH_UNIT_H_ +#define WAVEMAP_CORE_CONFIG_VALUE_WITH_UNIT_H_ #include @@ -83,4 +83,4 @@ std::optional getUnitToSi(const std::string& unit); #include "wavemap/core/config/impl/value_with_unit_inl.h" -#endif // WAVEMAP_CONFIG_VALUE_WITH_UNIT_H_ +#endif // WAVEMAP_CORE_CONFIG_VALUE_WITH_UNIT_H_ diff --git a/library/wavemap/core/data_structure/aabb.h b/library/include/wavemap/core/data_structure/aabb.h similarity index 96% rename from library/wavemap/core/data_structure/aabb.h rename to library/include/wavemap/core/data_structure/aabb.h index 6a4c92ed8..2f60c2351 100644 --- a/library/wavemap/core/data_structure/aabb.h +++ b/library/include/wavemap/core/data_structure/aabb.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_AABB_H_ -#define WAVEMAP_DATA_STRUCTURE_AABB_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_AABB_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_AABB_H_ #include #include @@ -123,4 +123,4 @@ struct AABB { }; } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_AABB_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_AABB_H_ diff --git a/library/wavemap/core/data_structure/bucket_queue.h b/library/include/wavemap/core/data_structure/bucket_queue.h similarity index 90% rename from library/wavemap/core/data_structure/bucket_queue.h rename to library/include/wavemap/core/data_structure/bucket_queue.h index cf8ea98d1..960f63208 100644 --- a/library/wavemap/core/data_structure/bucket_queue.h +++ b/library/include/wavemap/core/data_structure/bucket_queue.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_BUCKET_QUEUE_H_ -#define WAVEMAP_DATA_STRUCTURE_BUCKET_QUEUE_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_BUCKET_QUEUE_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_BUCKET_QUEUE_H_ #include #include @@ -55,4 +55,4 @@ class BucketQueue { #include "wavemap/core/data_structure/impl/bucket_queue_inl.h" -#endif // WAVEMAP_DATA_STRUCTURE_BUCKET_QUEUE_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_BUCKET_QUEUE_H_ diff --git a/library/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h b/library/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h similarity index 93% rename from library/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h rename to library/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h index 4a9b142d6..0b530aaa7 100644 --- a/library/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h +++ b/library/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_H_ -#define WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_H_ #include #include @@ -78,4 +78,4 @@ using ChunkedOctree = ChunkedNdtree; #include "wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h" -#endif // WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_H_ diff --git a/library/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_chunk.h b/library/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_chunk.h similarity index 92% rename from library/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_chunk.h rename to library/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_chunk.h index 6bbd3cdca..9b584da90 100644 --- a/library/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_chunk.h +++ b/library/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_chunk.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_CHUNK_H_ -#define WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_CHUNK_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_CHUNK_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_CHUNK_H_ #include #include @@ -79,4 +79,4 @@ class ChunkedNdtreeChunk { #include "wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h" -#endif // WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_CHUNK_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_CHUNK_H_ diff --git a/library/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h b/library/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h similarity index 93% rename from library/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h rename to library/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h index e7a4c6050..3f05d8854 100644 --- a/library/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h +++ b/library/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_NODE_ADDRESS_H_ -#define WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_NODE_ADDRESS_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_NODE_ADDRESS_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_NODE_ADDRESS_H_ #include @@ -107,4 +107,4 @@ class ChunkedNdtreeNodeRef { #include "wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h" -#endif // WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_NODE_ADDRESS_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_CHUNKED_NDTREE_CHUNKED_NDTREE_NODE_ADDRESS_H_ diff --git a/library/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h b/library/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h similarity index 95% rename from library/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h rename to library/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h index d42b70a0e..470986b23 100644 --- a/library/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h +++ b/library/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_CHUNK_INL_H_ -#define WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_CHUNK_INL_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_CHUNK_INL_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_CHUNK_INL_H_ #include #include @@ -160,4 +160,4 @@ bool ChunkedNdtreeChunk::nodeHasAtLeastOneChild( } } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_CHUNK_INL_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_CHUNK_INL_H_ diff --git a/library/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h b/library/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h similarity index 96% rename from library/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h rename to library/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h index 5f9347900..85764f822 100644 --- a/library/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h +++ b/library/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_INL_H_ -#define WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_INL_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_INL_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_INL_H_ #include #include @@ -189,4 +189,4 @@ auto ChunkedNdtree::getChunkIterator() const { } } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_INL_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_INL_H_ diff --git a/library/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h b/library/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h similarity index 95% rename from library/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h rename to library/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h index d14efeb72..f2d71c297 100644 --- a/library/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h +++ b/library/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_node_address_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_NODE_ADDRESS_INL_H_ -#define WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_NODE_ADDRESS_INL_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_NODE_ADDRESS_INL_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_NODE_ADDRESS_INL_H_ #include @@ -190,4 +190,4 @@ LinearIndex ChunkedNdtreeNodeRef::computeChildLevelTraversalDistance( } } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_NODE_ADDRESS_INL_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_CHUNKED_NDTREE_IMPL_CHUNKED_NDTREE_NODE_ADDRESS_INL_H_ diff --git a/library/wavemap/core/data_structure/dense_block_hash.h b/library/include/wavemap/core/data_structure/dense_block_hash.h similarity index 94% rename from library/wavemap/core/data_structure/dense_block_hash.h rename to library/include/wavemap/core/data_structure/dense_block_hash.h index 371d7a7ed..bc1454e4f 100644 --- a/library/wavemap/core/data_structure/dense_block_hash.h +++ b/library/include/wavemap/core/data_structure/dense_block_hash.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_DENSE_BLOCK_HASH_H_ -#define WAVEMAP_DATA_STRUCTURE_DENSE_BLOCK_HASH_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_DENSE_BLOCK_HASH_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_DENSE_BLOCK_HASH_H_ #include "wavemap/core/data_structure/dense_grid.h" #include "wavemap/core/data_structure/spatial_hash.h" @@ -78,4 +78,4 @@ class DenseBlockHash { #include "wavemap/core/data_structure/impl/dense_block_hash_inl.h" -#endif // WAVEMAP_DATA_STRUCTURE_DENSE_BLOCK_HASH_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_DENSE_BLOCK_HASH_H_ diff --git a/library/wavemap/core/data_structure/dense_grid.h b/library/include/wavemap/core/data_structure/dense_grid.h similarity index 92% rename from library/wavemap/core/data_structure/dense_grid.h rename to library/include/wavemap/core/data_structure/dense_grid.h index acb70ef7d..6c134c094 100644 --- a/library/wavemap/core/data_structure/dense_grid.h +++ b/library/include/wavemap/core/data_structure/dense_grid.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_DENSE_GRID_H_ -#define WAVEMAP_DATA_STRUCTURE_DENSE_GRID_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_DENSE_GRID_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_DENSE_GRID_H_ #include "wavemap/core/common.h" #include "wavemap/core/indexing/index_conversions.h" @@ -59,4 +59,4 @@ class DenseGrid { }; } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_DENSE_GRID_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_DENSE_GRID_H_ diff --git a/library/wavemap/core/data_structure/image.h b/library/include/wavemap/core/data_structure/image.h similarity index 95% rename from library/wavemap/core/data_structure/image.h rename to library/include/wavemap/core/data_structure/image.h index c3c556a04..b75f23fbd 100644 --- a/library/wavemap/core/data_structure/image.h +++ b/library/include/wavemap/core/data_structure/image.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_IMAGE_H_ -#define WAVEMAP_DATA_STRUCTURE_IMAGE_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_IMAGE_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_IMAGE_H_ #include #include @@ -81,4 +81,4 @@ template using PosedImage = PosedObject>; } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_IMAGE_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_IMAGE_H_ diff --git a/library/wavemap/core/data_structure/impl/bucket_queue_inl.h b/library/include/wavemap/core/data_structure/impl/bucket_queue_inl.h similarity index 90% rename from library/wavemap/core/data_structure/impl/bucket_queue_inl.h rename to library/include/wavemap/core/data_structure/impl/bucket_queue_inl.h index 76cf2321c..25296a9af 100644 --- a/library/wavemap/core/data_structure/impl/bucket_queue_inl.h +++ b/library/include/wavemap/core/data_structure/impl/bucket_queue_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_IMPL_BUCKET_QUEUE_INL_H_ -#define WAVEMAP_DATA_STRUCTURE_IMPL_BUCKET_QUEUE_INL_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_IMPL_BUCKET_QUEUE_INL_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_IMPL_BUCKET_QUEUE_INL_H_ #include @@ -69,4 +69,4 @@ ValueT BucketQueue::front() { } } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_IMPL_BUCKET_QUEUE_INL_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_IMPL_BUCKET_QUEUE_INL_H_ diff --git a/library/wavemap/core/data_structure/impl/dense_block_hash_inl.h b/library/include/wavemap/core/data_structure/impl/dense_block_hash_inl.h similarity index 96% rename from library/wavemap/core/data_structure/impl/dense_block_hash_inl.h rename to library/include/wavemap/core/data_structure/impl/dense_block_hash_inl.h index d4b69a94e..30b66828c 100644 --- a/library/wavemap/core/data_structure/impl/dense_block_hash_inl.h +++ b/library/include/wavemap/core/data_structure/impl/dense_block_hash_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_IMPL_DENSE_BLOCK_HASH_INL_H_ -#define WAVEMAP_DATA_STRUCTURE_IMPL_DENSE_BLOCK_HASH_INL_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_IMPL_DENSE_BLOCK_HASH_INL_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_IMPL_DENSE_BLOCK_HASH_INL_H_ namespace wavemap { template @@ -164,4 +164,4 @@ DenseBlockHash::cellAndBlockIndexToIndex( } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_IMPL_DENSE_BLOCK_HASH_INL_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_IMPL_DENSE_BLOCK_HASH_INL_H_ diff --git a/library/wavemap/core/data_structure/impl/ndtree_block_hash_inl.h b/library/include/wavemap/core/data_structure/impl/ndtree_block_hash_inl.h similarity index 98% rename from library/wavemap/core/data_structure/impl/ndtree_block_hash_inl.h rename to library/include/wavemap/core/data_structure/impl/ndtree_block_hash_inl.h index 0ce0c4492..15b5d847a 100644 --- a/library/wavemap/core/data_structure/impl/ndtree_block_hash_inl.h +++ b/library/include/wavemap/core/data_structure/impl/ndtree_block_hash_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_IMPL_NDTREE_BLOCK_HASH_INL_H_ -#define WAVEMAP_DATA_STRUCTURE_IMPL_NDTREE_BLOCK_HASH_INL_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_IMPL_NDTREE_BLOCK_HASH_INL_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_IMPL_NDTREE_BLOCK_HASH_INL_H_ #include #include @@ -291,4 +291,4 @@ NdtreeBlockHash::cellAndBlockIndexToIndex( } } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_IMPL_NDTREE_BLOCK_HASH_INL_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_IMPL_NDTREE_BLOCK_HASH_INL_H_ diff --git a/library/wavemap/core/data_structure/impl/spatial_hash_inl.h b/library/include/wavemap/core/data_structure/impl/spatial_hash_inl.h similarity index 95% rename from library/wavemap/core/data_structure/impl/spatial_hash_inl.h rename to library/include/wavemap/core/data_structure/impl/spatial_hash_inl.h index b5bc57ef7..4da012e9e 100644 --- a/library/wavemap/core/data_structure/impl/spatial_hash_inl.h +++ b/library/include/wavemap/core/data_structure/impl/spatial_hash_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_IMPL_SPATIAL_HASH_INL_H_ -#define WAVEMAP_DATA_STRUCTURE_IMPL_SPATIAL_HASH_INL_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_IMPL_SPATIAL_HASH_INL_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_IMPL_SPATIAL_HASH_INL_H_ #include #include @@ -122,4 +122,4 @@ void SpatialHash::forEachBlock( } } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_IMPL_SPATIAL_HASH_INL_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_IMPL_SPATIAL_HASH_INL_H_ diff --git a/library/wavemap/core/data_structure/ndtree/impl/ndtree_inl.h b/library/include/wavemap/core/data_structure/ndtree/impl/ndtree_inl.h similarity index 96% rename from library/wavemap/core/data_structure/ndtree/impl/ndtree_inl.h rename to library/include/wavemap/core/data_structure/ndtree/impl/ndtree_inl.h index b76b6ba27..ec56158ee 100644 --- a/library/wavemap/core/data_structure/ndtree/impl/ndtree_inl.h +++ b/library/include/wavemap/core/data_structure/ndtree/impl/ndtree_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_NDTREE_IMPL_NDTREE_INL_H_ -#define WAVEMAP_DATA_STRUCTURE_NDTREE_IMPL_NDTREE_INL_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_NDTREE_IMPL_NDTREE_INL_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_NDTREE_IMPL_NDTREE_INL_H_ #include #include @@ -149,4 +149,4 @@ auto Ndtree::getIterator() const { } } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_NDTREE_IMPL_NDTREE_INL_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_NDTREE_IMPL_NDTREE_INL_H_ diff --git a/library/wavemap/core/data_structure/ndtree/impl/ndtree_node_inl.h b/library/include/wavemap/core/data_structure/ndtree/impl/ndtree_node_inl.h similarity index 93% rename from library/wavemap/core/data_structure/ndtree/impl/ndtree_node_inl.h rename to library/include/wavemap/core/data_structure/ndtree/impl/ndtree_node_inl.h index b9dda8b4a..ef34bdba0 100644 --- a/library/wavemap/core/data_structure/ndtree/impl/ndtree_node_inl.h +++ b/library/include/wavemap/core/data_structure/ndtree/impl/ndtree_node_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_NDTREE_IMPL_NDTREE_NODE_INL_H_ -#define WAVEMAP_DATA_STRUCTURE_NDTREE_IMPL_NDTREE_NODE_INL_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_NDTREE_IMPL_NDTREE_NODE_INL_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_NDTREE_IMPL_NDTREE_NODE_INL_H_ #include #include @@ -108,4 +108,4 @@ NdtreeNode& NdtreeNode::getOrAllocateChild( } } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_NDTREE_IMPL_NDTREE_NODE_INL_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_NDTREE_IMPL_NDTREE_NODE_INL_H_ diff --git a/library/wavemap/core/data_structure/ndtree/ndtree.h b/library/include/wavemap/core/data_structure/ndtree/ndtree.h similarity index 92% rename from library/wavemap/core/data_structure/ndtree/ndtree.h rename to library/include/wavemap/core/data_structure/ndtree/ndtree.h index 1e758c01e..3d6ddc7c9 100644 --- a/library/wavemap/core/data_structure/ndtree/ndtree.h +++ b/library/include/wavemap/core/data_structure/ndtree/ndtree.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_NDTREE_NDTREE_H_ -#define WAVEMAP_DATA_STRUCTURE_NDTREE_NDTREE_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_NDTREE_NDTREE_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_NDTREE_NDTREE_H_ #include #include @@ -66,4 +66,4 @@ using Octree = Ndtree; #include "wavemap/core/data_structure/ndtree/impl/ndtree_inl.h" -#endif // WAVEMAP_DATA_STRUCTURE_NDTREE_NDTREE_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_NDTREE_NDTREE_H_ diff --git a/library/wavemap/core/data_structure/ndtree/ndtree_node.h b/library/include/wavemap/core/data_structure/ndtree/ndtree_node.h similarity index 90% rename from library/wavemap/core/data_structure/ndtree/ndtree_node.h rename to library/include/wavemap/core/data_structure/ndtree/ndtree_node.h index 6c1fe5b88..56782db39 100644 --- a/library/wavemap/core/data_structure/ndtree/ndtree_node.h +++ b/library/include/wavemap/core/data_structure/ndtree/ndtree_node.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_NDTREE_NDTREE_NODE_H_ -#define WAVEMAP_DATA_STRUCTURE_NDTREE_NDTREE_NODE_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_NDTREE_NDTREE_NODE_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_NDTREE_NDTREE_NODE_H_ #include #include @@ -58,4 +58,4 @@ class NdtreeNode { #include "wavemap/core/data_structure/ndtree/impl/ndtree_node_inl.h" -#endif // WAVEMAP_DATA_STRUCTURE_NDTREE_NDTREE_NODE_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_NDTREE_NDTREE_NODE_H_ diff --git a/library/wavemap/core/data_structure/ndtree_block_hash.h b/library/include/wavemap/core/data_structure/ndtree_block_hash.h similarity index 95% rename from library/wavemap/core/data_structure/ndtree_block_hash.h rename to library/include/wavemap/core/data_structure/ndtree_block_hash.h index 1d554e703..910c5a5e2 100644 --- a/library/wavemap/core/data_structure/ndtree_block_hash.h +++ b/library/include/wavemap/core/data_structure/ndtree_block_hash.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_NDTREE_BLOCK_HASH_H_ -#define WAVEMAP_DATA_STRUCTURE_NDTREE_BLOCK_HASH_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_NDTREE_BLOCK_HASH_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_NDTREE_BLOCK_HASH_H_ #include @@ -106,4 +106,4 @@ using OctreeBlockHash = NdtreeBlockHash; #include "wavemap/core/data_structure/impl/ndtree_block_hash_inl.h" -#endif // WAVEMAP_DATA_STRUCTURE_NDTREE_BLOCK_HASH_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_NDTREE_BLOCK_HASH_H_ diff --git a/library/wavemap/core/data_structure/pointcloud.h b/library/include/wavemap/core/data_structure/pointcloud.h similarity index 94% rename from library/wavemap/core/data_structure/pointcloud.h rename to library/include/wavemap/core/data_structure/pointcloud.h index 852d0eb56..f32291f57 100644 --- a/library/wavemap/core/data_structure/pointcloud.h +++ b/library/include/wavemap/core/data_structure/pointcloud.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_POINTCLOUD_H_ -#define WAVEMAP_DATA_STRUCTURE_POINTCLOUD_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_POINTCLOUD_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_POINTCLOUD_H_ #include #include @@ -81,4 +81,4 @@ class PosedPointcloud : public PosedObject> { }; } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_POINTCLOUD_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_POINTCLOUD_H_ diff --git a/library/wavemap/core/data_structure/posed_object.h b/library/include/wavemap/core/data_structure/posed_object.h similarity index 89% rename from library/wavemap/core/data_structure/posed_object.h rename to library/include/wavemap/core/data_structure/posed_object.h index 7a126cc6e..d16ae7891 100644 --- a/library/wavemap/core/data_structure/posed_object.h +++ b/library/include/wavemap/core/data_structure/posed_object.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_POSED_OBJECT_H_ -#define WAVEMAP_DATA_STRUCTURE_POSED_OBJECT_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_POSED_OBJECT_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_POSED_OBJECT_H_ #include #include @@ -48,4 +48,4 @@ class PosedObject : public ObjectT { }; } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_POSED_OBJECT_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_POSED_OBJECT_H_ diff --git a/library/wavemap/core/data_structure/sparse_vector.h b/library/include/wavemap/core/data_structure/sparse_vector.h similarity index 91% rename from library/wavemap/core/data_structure/sparse_vector.h rename to library/include/wavemap/core/data_structure/sparse_vector.h index 060f84262..b0838a1d0 100644 --- a/library/wavemap/core/data_structure/sparse_vector.h +++ b/library/include/wavemap/core/data_structure/sparse_vector.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_SPARSE_VECTOR_H_ -#define WAVEMAP_DATA_STRUCTURE_SPARSE_VECTOR_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_SPARSE_VECTOR_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_SPARSE_VECTOR_H_ #include #include @@ -56,4 +56,4 @@ const std::bitset SparseVector::kOnes = std::bitset{}.set(); } // namespace wavemap -#endif // WAVEMAP_DATA_STRUCTURE_SPARSE_VECTOR_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_SPARSE_VECTOR_H_ diff --git a/library/wavemap/core/data_structure/spatial_hash.h b/library/include/wavemap/core/data_structure/spatial_hash.h similarity index 91% rename from library/wavemap/core/data_structure/spatial_hash.h rename to library/include/wavemap/core/data_structure/spatial_hash.h index bb03e917b..a9071da68 100644 --- a/library/wavemap/core/data_structure/spatial_hash.h +++ b/library/include/wavemap/core/data_structure/spatial_hash.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_DATA_STRUCTURE_SPATIAL_HASH_H_ -#define WAVEMAP_DATA_STRUCTURE_SPATIAL_HASH_H_ +#ifndef WAVEMAP_CORE_DATA_STRUCTURE_SPATIAL_HASH_H_ +#define WAVEMAP_CORE_DATA_STRUCTURE_SPATIAL_HASH_H_ #include @@ -56,4 +56,4 @@ class SpatialHash { #include "wavemap/core/data_structure/impl/spatial_hash_inl.h" -#endif // WAVEMAP_DATA_STRUCTURE_SPATIAL_HASH_H_ +#endif // WAVEMAP_CORE_DATA_STRUCTURE_SPATIAL_HASH_H_ diff --git a/library/wavemap/core/indexing/impl/ndtree_index_inl.h b/library/include/wavemap/core/indexing/impl/ndtree_index_inl.h similarity index 96% rename from library/wavemap/core/indexing/impl/ndtree_index_inl.h rename to library/include/wavemap/core/indexing/impl/ndtree_index_inl.h index 568af9766..6c474e5a9 100644 --- a/library/wavemap/core/indexing/impl/ndtree_index_inl.h +++ b/library/include/wavemap/core/indexing/impl/ndtree_index_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INDEXING_IMPL_NDTREE_INDEX_INL_H_ -#define WAVEMAP_INDEXING_IMPL_NDTREE_INDEX_INL_H_ +#ifndef WAVEMAP_CORE_INDEXING_IMPL_NDTREE_INDEX_INL_H_ +#define WAVEMAP_CORE_INDEXING_IMPL_NDTREE_INDEX_INL_H_ #include #include @@ -130,4 +130,4 @@ std::string NdtreeIndex::toString() const { } } // namespace wavemap -#endif // WAVEMAP_INDEXING_IMPL_NDTREE_INDEX_INL_H_ +#endif // WAVEMAP_CORE_INDEXING_IMPL_NDTREE_INDEX_INL_H_ diff --git a/library/wavemap/core/indexing/index_conversions.h b/library/include/wavemap/core/indexing/index_conversions.h similarity index 97% rename from library/wavemap/core/indexing/index_conversions.h rename to library/include/wavemap/core/indexing/index_conversions.h index 3c74bdfe7..77fb78aa3 100644 --- a/library/wavemap/core/indexing/index_conversions.h +++ b/library/include/wavemap/core/indexing/index_conversions.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INDEXING_INDEX_CONVERSIONS_H_ -#define WAVEMAP_INDEXING_INDEX_CONVERSIONS_H_ +#ifndef WAVEMAP_CORE_INDEXING_INDEX_CONVERSIONS_H_ +#define WAVEMAP_CORE_INDEXING_INDEX_CONVERSIONS_H_ #include #include @@ -196,4 +196,4 @@ inline MortonIndex nodeIndexToMorton(const NdtreeIndex& node_index) { } } // namespace wavemap::convert -#endif // WAVEMAP_INDEXING_INDEX_CONVERSIONS_H_ +#endif // WAVEMAP_CORE_INDEXING_INDEX_CONVERSIONS_H_ diff --git a/library/wavemap/core/indexing/index_hashes.h b/library/include/wavemap/core/indexing/index_hashes.h similarity index 88% rename from library/wavemap/core/indexing/index_hashes.h rename to library/include/wavemap/core/indexing/index_hashes.h index bf27ba310..5b9e2e23c 100644 --- a/library/wavemap/core/indexing/index_hashes.h +++ b/library/include/wavemap/core/indexing/index_hashes.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INDEXING_INDEX_HASHES_H_ -#define WAVEMAP_INDEXING_INDEX_HASHES_H_ +#ifndef WAVEMAP_CORE_INDEXING_INDEX_HASHES_H_ +#define WAVEMAP_CORE_INDEXING_INDEX_HASHES_H_ #include @@ -37,4 +37,4 @@ using QuadtreeIndexHash = NdtreeIndexHash<2>; using OctreeIndexHash = NdtreeIndexHash<3>; } // namespace wavemap -#endif // WAVEMAP_INDEXING_INDEX_HASHES_H_ +#endif // WAVEMAP_CORE_INDEXING_INDEX_HASHES_H_ diff --git a/library/wavemap/core/indexing/ndtree_index.h b/library/include/wavemap/core/indexing/ndtree_index.h similarity index 94% rename from library/wavemap/core/indexing/ndtree_index.h rename to library/include/wavemap/core/indexing/ndtree_index.h index dea684edc..ccbaa0b37 100644 --- a/library/wavemap/core/indexing/ndtree_index.h +++ b/library/include/wavemap/core/indexing/ndtree_index.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INDEXING_NDTREE_INDEX_H_ -#define WAVEMAP_INDEXING_NDTREE_INDEX_H_ +#ifndef WAVEMAP_CORE_INDEXING_NDTREE_INDEX_H_ +#define WAVEMAP_CORE_INDEXING_NDTREE_INDEX_H_ #include #include @@ -63,4 +63,4 @@ using OctreeIndex = NdtreeIndex<3>; #include "wavemap/core/indexing/impl/ndtree_index_inl.h" -#endif // WAVEMAP_INDEXING_NDTREE_INDEX_H_ +#endif // WAVEMAP_CORE_INDEXING_NDTREE_INDEX_H_ diff --git a/library/wavemap/core/integrator/impl/integrator_base_inl.h b/library/include/wavemap/core/integrator/impl/integrator_base_inl.h similarity index 86% rename from library/wavemap/core/integrator/impl/integrator_base_inl.h rename to library/include/wavemap/core/integrator/impl/integrator_base_inl.h index 9905186f0..80fc2eb0c 100644 --- a/library/wavemap/core/integrator/impl/integrator_base_inl.h +++ b/library/include/wavemap/core/integrator/impl/integrator_base_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_IMPL_INTEGRATOR_BASE_INL_H_ -#define WAVEMAP_INTEGRATOR_IMPL_INTEGRATOR_BASE_INL_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_IMPL_INTEGRATOR_BASE_INL_H_ +#define WAVEMAP_CORE_INTEGRATOR_IMPL_INTEGRATOR_BASE_INL_H_ namespace wavemap { inline bool IntegratorBase::isMeasurementValid(const Point3D& C_end_point) { @@ -36,4 +36,4 @@ inline Point3D IntegratorBase::getEndPointOrMaxRange( } } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_IMPL_INTEGRATOR_BASE_INL_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_IMPL_INTEGRATOR_BASE_INL_H_ diff --git a/library/wavemap/core/integrator/integrator_base.h b/library/include/wavemap/core/integrator/integrator_base.h similarity index 90% rename from library/wavemap/core/integrator/integrator_base.h rename to library/include/wavemap/core/integrator/integrator_base.h index 92cb52c65..4db619730 100644 --- a/library/wavemap/core/integrator/integrator_base.h +++ b/library/include/wavemap/core/integrator/integrator_base.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_INTEGRATOR_BASE_H_ -#define WAVEMAP_INTEGRATOR_INTEGRATOR_BASE_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_INTEGRATOR_BASE_H_ +#define WAVEMAP_CORE_INTEGRATOR_INTEGRATOR_BASE_H_ #include #include @@ -50,4 +50,4 @@ class IntegratorBase { #include "wavemap/core/integrator/impl/integrator_base_inl.h" -#endif // WAVEMAP_INTEGRATOR_INTEGRATOR_BASE_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_INTEGRATOR_BASE_H_ diff --git a/library/wavemap/core/integrator/integrator_factory.h b/library/include/wavemap/core/integrator/integrator_factory.h similarity index 80% rename from library/wavemap/core/integrator/integrator_factory.h rename to library/include/wavemap/core/integrator/integrator_factory.h index 3ec4c5d7d..8aec64518 100644 --- a/library/wavemap/core/integrator/integrator_factory.h +++ b/library/include/wavemap/core/integrator/integrator_factory.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_INTEGRATOR_FACTORY_H_ -#define WAVEMAP_INTEGRATOR_INTEGRATOR_FACTORY_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_INTEGRATOR_FACTORY_H_ +#define WAVEMAP_CORE_INTEGRATOR_INTEGRATOR_FACTORY_H_ #include #include @@ -23,4 +23,4 @@ class IntegratorFactory { }; } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_INTEGRATOR_FACTORY_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_INTEGRATOR_FACTORY_H_ diff --git a/library/wavemap/core/integrator/measurement_model/approximate_gaussian_distribution.h b/library/include/wavemap/core/integrator/measurement_model/approximate_gaussian_distribution.h similarity index 75% rename from library/wavemap/core/integrator/measurement_model/approximate_gaussian_distribution.h rename to library/include/wavemap/core/integrator/measurement_model/approximate_gaussian_distribution.h index 420129615..a254dd308 100644 --- a/library/wavemap/core/integrator/measurement_model/approximate_gaussian_distribution.h +++ b/library/include/wavemap/core/integrator/measurement_model/approximate_gaussian_distribution.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_APPROXIMATE_GAUSSIAN_DISTRIBUTION_H_ -#define WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_APPROXIMATE_GAUSSIAN_DISTRIBUTION_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_APPROXIMATE_GAUSSIAN_DISTRIBUTION_H_ +#define WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_APPROXIMATE_GAUSSIAN_DISTRIBUTION_H_ #include "wavemap/core/common.h" @@ -27,4 +27,4 @@ struct ApproximateGaussianDistribution { }; } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_APPROXIMATE_GAUSSIAN_DISTRIBUTION_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_APPROXIMATE_GAUSSIAN_DISTRIBUTION_H_ diff --git a/library/wavemap/core/integrator/measurement_model/constant_ray.h b/library/include/wavemap/core/integrator/measurement_model/constant_ray.h similarity index 91% rename from library/wavemap/core/integrator/measurement_model/constant_ray.h rename to library/include/wavemap/core/integrator/measurement_model/constant_ray.h index 1699bcaa8..be4b15b23 100644 --- a/library/wavemap/core/integrator/measurement_model/constant_ray.h +++ b/library/include/wavemap/core/integrator/measurement_model/constant_ray.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_CONSTANT_RAY_H_ -#define WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_CONSTANT_RAY_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_CONSTANT_RAY_H_ +#define WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_CONSTANT_RAY_H_ #include "wavemap/core/common.h" #include "wavemap/core/config/config_base.h" @@ -71,4 +71,4 @@ class ConstantRay { }; } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_CONSTANT_RAY_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_CONSTANT_RAY_H_ diff --git a/library/wavemap/core/integrator/measurement_model/continuous_beam.h b/library/include/wavemap/core/integrator/measurement_model/continuous_beam.h similarity index 95% rename from library/wavemap/core/integrator/measurement_model/continuous_beam.h rename to library/include/wavemap/core/integrator/measurement_model/continuous_beam.h index 9a30b6201..63241fa6d 100644 --- a/library/wavemap/core/integrator/measurement_model/continuous_beam.h +++ b/library/include/wavemap/core/integrator/measurement_model/continuous_beam.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_CONTINUOUS_BEAM_H_ -#define WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_CONTINUOUS_BEAM_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_CONTINUOUS_BEAM_H_ +#define WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_CONTINUOUS_BEAM_H_ #include #include @@ -117,4 +117,4 @@ class ContinuousBeam : public MeasurementModelBase { #include "wavemap/core/integrator/measurement_model/impl/continuous_beam_inl.h" -#endif // WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_CONTINUOUS_BEAM_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_CONTINUOUS_BEAM_H_ diff --git a/library/wavemap/core/integrator/measurement_model/continuous_ray.h b/library/include/wavemap/core/integrator/measurement_model/continuous_ray.h similarity index 94% rename from library/wavemap/core/integrator/measurement_model/continuous_ray.h rename to library/include/wavemap/core/integrator/measurement_model/continuous_ray.h index bc48e4802..5ed8d83aa 100644 --- a/library/wavemap/core/integrator/measurement_model/continuous_ray.h +++ b/library/include/wavemap/core/integrator/measurement_model/continuous_ray.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_CONTINUOUS_RAY_H_ -#define WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_CONTINUOUS_RAY_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_CONTINUOUS_RAY_H_ +#define WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_CONTINUOUS_RAY_H_ #include #include @@ -94,4 +94,4 @@ class ContinuousRay : public MeasurementModelBase { #include "wavemap/core/integrator/measurement_model/impl/continuous_ray_inl.h" -#endif // WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_CONTINUOUS_RAY_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_CONTINUOUS_RAY_H_ diff --git a/library/wavemap/core/integrator/measurement_model/impl/continuous_beam_inl.h b/library/include/wavemap/core/integrator/measurement_model/impl/continuous_beam_inl.h similarity index 96% rename from library/wavemap/core/integrator/measurement_model/impl/continuous_beam_inl.h rename to library/include/wavemap/core/integrator/measurement_model/impl/continuous_beam_inl.h index e942374f2..3eedd8ce9 100644 --- a/library/wavemap/core/integrator/measurement_model/impl/continuous_beam_inl.h +++ b/library/include/wavemap/core/integrator/measurement_model/impl/continuous_beam_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_IMPL_CONTINUOUS_BEAM_INL_H_ -#define WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_IMPL_CONTINUOUS_BEAM_INL_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_IMPL_CONTINUOUS_BEAM_INL_H_ +#define WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_IMPL_CONTINUOUS_BEAM_INL_H_ #include @@ -157,4 +157,4 @@ inline FloatingPoint ContinuousBeam::computeBeamUpdate( } } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_IMPL_CONTINUOUS_BEAM_INL_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_IMPL_CONTINUOUS_BEAM_INL_H_ diff --git a/library/wavemap/core/integrator/measurement_model/impl/continuous_ray_inl.h b/library/include/wavemap/core/integrator/measurement_model/impl/continuous_ray_inl.h similarity index 93% rename from library/wavemap/core/integrator/measurement_model/impl/continuous_ray_inl.h rename to library/include/wavemap/core/integrator/measurement_model/impl/continuous_ray_inl.h index 33740bce9..51dd462ec 100644 --- a/library/wavemap/core/integrator/measurement_model/impl/continuous_ray_inl.h +++ b/library/include/wavemap/core/integrator/measurement_model/impl/continuous_ray_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_IMPL_CONTINUOUS_RAY_INL_H_ -#define WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_IMPL_CONTINUOUS_RAY_INL_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_IMPL_CONTINUOUS_RAY_INL_H_ +#define WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_IMPL_CONTINUOUS_RAY_INL_H_ #include "wavemap/core/integrator/measurement_model/approximate_gaussian_distribution.h" @@ -94,4 +94,4 @@ inline FloatingPoint ContinuousRay::computeFreeSpaceBeamUpdate() const { } } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_IMPL_CONTINUOUS_RAY_INL_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_IMPL_CONTINUOUS_RAY_INL_H_ diff --git a/library/wavemap/core/integrator/measurement_model/measurement_model_base.h b/library/include/wavemap/core/integrator/measurement_model/measurement_model_base.h similarity index 87% rename from library/wavemap/core/integrator/measurement_model/measurement_model_base.h rename to library/include/wavemap/core/integrator/measurement_model/measurement_model_base.h index f129c22b1..0e695413b 100644 --- a/library/wavemap/core/integrator/measurement_model/measurement_model_base.h +++ b/library/include/wavemap/core/integrator/measurement_model/measurement_model_base.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_MEASUREMENT_MODEL_BASE_H_ -#define WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_MEASUREMENT_MODEL_BASE_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_MEASUREMENT_MODEL_BASE_H_ +#define WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_MEASUREMENT_MODEL_BASE_H_ #include @@ -47,4 +47,4 @@ class MeasurementModelBase { }; } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_MEASUREMENT_MODEL_BASE_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_MEASUREMENT_MODEL_BASE_H_ diff --git a/library/wavemap/core/integrator/measurement_model/measurement_model_factory.h b/library/include/wavemap/core/integrator/measurement_model/measurement_model_factory.h similarity index 75% rename from library/wavemap/core/integrator/measurement_model/measurement_model_factory.h rename to library/include/wavemap/core/integrator/measurement_model/measurement_model_factory.h index 018f147e9..bad7137a9 100644 --- a/library/wavemap/core/integrator/measurement_model/measurement_model_factory.h +++ b/library/include/wavemap/core/integrator/measurement_model/measurement_model_factory.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_MEASUREMENT_MODEL_FACTORY_H_ -#define WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_MEASUREMENT_MODEL_FACTORY_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_MEASUREMENT_MODEL_FACTORY_H_ +#define WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_MEASUREMENT_MODEL_FACTORY_H_ #include @@ -22,4 +22,4 @@ class MeasurementModelFactory { }; } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_MEASUREMENT_MODEL_MEASUREMENT_MODEL_FACTORY_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_MEASUREMENT_MODEL_MEASUREMENT_MODEL_FACTORY_H_ diff --git a/library/wavemap/core/integrator/projection_model/circular_projector.h b/library/include/wavemap/core/integrator/projection_model/circular_projector.h similarity index 93% rename from library/wavemap/core/integrator/projection_model/circular_projector.h rename to library/include/wavemap/core/integrator/projection_model/circular_projector.h index 17046c2cd..2ab07b16e 100644 --- a/library/wavemap/core/integrator/projection_model/circular_projector.h +++ b/library/include/wavemap/core/integrator/projection_model/circular_projector.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTION_MODEL_CIRCULAR_PROJECTOR_H_ -#define WAVEMAP_INTEGRATOR_PROJECTION_MODEL_CIRCULAR_PROJECTOR_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_CIRCULAR_PROJECTOR_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_CIRCULAR_PROJECTOR_H_ #include "wavemap/core/common.h" #include "wavemap/core/config/config_base.h" @@ -89,4 +89,4 @@ class CircularProjector { }; } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_PROJECTION_MODEL_CIRCULAR_PROJECTOR_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_CIRCULAR_PROJECTOR_H_ diff --git a/library/wavemap/core/integrator/projection_model/impl/ouster_projector_inl.h b/library/include/wavemap/core/integrator/projection_model/impl/ouster_projector_inl.h similarity index 94% rename from library/wavemap/core/integrator/projection_model/impl/ouster_projector_inl.h rename to library/include/wavemap/core/integrator/projection_model/impl/ouster_projector_inl.h index 92f31d777..b5cadc57e 100644 --- a/library/wavemap/core/integrator/projection_model/impl/ouster_projector_inl.h +++ b/library/include/wavemap/core/integrator/projection_model/impl/ouster_projector_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_OUSTER_PROJECTOR_INL_H_ -#define WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_OUSTER_PROJECTOR_INL_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_IMPL_OUSTER_PROJECTOR_INL_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_IMPL_OUSTER_PROJECTOR_INL_H_ #include "wavemap/core/utils/math/approximate_trigonometry.h" @@ -87,4 +87,4 @@ inline FloatingPoint OusterProjector::cartesianToSensorZ( } } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_OUSTER_PROJECTOR_INL_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_IMPL_OUSTER_PROJECTOR_INL_H_ diff --git a/library/wavemap/core/integrator/projection_model/impl/pinhole_camera_projector_inl.h b/library/include/wavemap/core/integrator/projection_model/impl/pinhole_camera_projector_inl.h similarity index 86% rename from library/wavemap/core/integrator/projection_model/impl/pinhole_camera_projector_inl.h rename to library/include/wavemap/core/integrator/projection_model/impl/pinhole_camera_projector_inl.h index cb436719a..bf4f41f54 100644 --- a/library/wavemap/core/integrator/projection_model/impl/pinhole_camera_projector_inl.h +++ b/library/include/wavemap/core/integrator/projection_model/impl/pinhole_camera_projector_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_PINHOLE_CAMERA_PROJECTOR_INL_H_ -#define WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_PINHOLE_CAMERA_PROJECTOR_INL_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_IMPL_PINHOLE_CAMERA_PROJECTOR_INL_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_IMPL_PINHOLE_CAMERA_PROJECTOR_INL_H_ #include @@ -45,4 +45,4 @@ PinholeCameraProjector::imageOffsetsToErrorSquaredNorms( } } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_PINHOLE_CAMERA_PROJECTOR_INL_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_IMPL_PINHOLE_CAMERA_PROJECTOR_INL_H_ diff --git a/library/wavemap/core/integrator/projection_model/impl/projector_base_inl.h b/library/include/wavemap/core/integrator/projection_model/impl/projector_base_inl.h similarity index 94% rename from library/wavemap/core/integrator/projection_model/impl/projector_base_inl.h rename to library/include/wavemap/core/integrator/projection_model/impl/projector_base_inl.h index 41c50162a..94371053f 100644 --- a/library/wavemap/core/integrator/projection_model/impl/projector_base_inl.h +++ b/library/include/wavemap/core/integrator/projection_model/impl/projector_base_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_PROJECTOR_BASE_INL_H_ -#define WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_PROJECTOR_BASE_INL_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_IMPL_PROJECTOR_BASE_INL_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_IMPL_PROJECTOR_BASE_INL_H_ #include @@ -119,4 +119,4 @@ inline Vector2D ProjectorBase::imageToIndexReal( } } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_PROJECTOR_BASE_INL_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_IMPL_PROJECTOR_BASE_INL_H_ diff --git a/library/wavemap/core/integrator/projection_model/impl/spherical_projector_inl.h b/library/include/wavemap/core/integrator/projection_model/impl/spherical_projector_inl.h similarity index 91% rename from library/wavemap/core/integrator/projection_model/impl/spherical_projector_inl.h rename to library/include/wavemap/core/integrator/projection_model/impl/spherical_projector_inl.h index 4b58ee4fe..501271955 100644 --- a/library/wavemap/core/integrator/projection_model/impl/spherical_projector_inl.h +++ b/library/include/wavemap/core/integrator/projection_model/impl/spherical_projector_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_SPHERICAL_PROJECTOR_INL_H_ -#define WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_SPHERICAL_PROJECTOR_INL_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_IMPL_SPHERICAL_PROJECTOR_INL_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_IMPL_SPHERICAL_PROJECTOR_INL_H_ #include @@ -69,4 +69,4 @@ inline FloatingPoint SphericalProjector::cartesianToSensorZ( } } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_PROJECTION_MODEL_IMPL_SPHERICAL_PROJECTOR_INL_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_IMPL_SPHERICAL_PROJECTOR_INL_H_ diff --git a/library/wavemap/core/integrator/projection_model/ouster_projector.h b/library/include/wavemap/core/integrator/projection_model/ouster_projector.h similarity index 94% rename from library/wavemap/core/integrator/projection_model/ouster_projector.h rename to library/include/wavemap/core/integrator/projection_model/ouster_projector.h index f685bc008..f03ec70b2 100644 --- a/library/wavemap/core/integrator/projection_model/ouster_projector.h +++ b/library/include/wavemap/core/integrator/projection_model/ouster_projector.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTION_MODEL_OUSTER_PROJECTOR_H_ -#define WAVEMAP_INTEGRATOR_PROJECTION_MODEL_OUSTER_PROJECTOR_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_OUSTER_PROJECTOR_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_OUSTER_PROJECTOR_H_ #include #include @@ -84,4 +84,4 @@ class OusterProjector : public ProjectorBase { #include "wavemap/core/integrator/projection_model/impl/ouster_projector_inl.h" -#endif // WAVEMAP_INTEGRATOR_PROJECTION_MODEL_OUSTER_PROJECTOR_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_OUSTER_PROJECTOR_H_ diff --git a/library/wavemap/core/integrator/projection_model/pinhole_camera_projector.h b/library/include/wavemap/core/integrator/projection_model/pinhole_camera_projector.h similarity index 93% rename from library/wavemap/core/integrator/projection_model/pinhole_camera_projector.h rename to library/include/wavemap/core/integrator/projection_model/pinhole_camera_projector.h index 06b3e5ecd..8017e329e 100644 --- a/library/wavemap/core/integrator/projection_model/pinhole_camera_projector.h +++ b/library/include/wavemap/core/integrator/projection_model/pinhole_camera_projector.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTION_MODEL_PINHOLE_CAMERA_PROJECTOR_H_ -#define WAVEMAP_INTEGRATOR_PROJECTION_MODEL_PINHOLE_CAMERA_PROJECTOR_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_PINHOLE_CAMERA_PROJECTOR_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_PINHOLE_CAMERA_PROJECTOR_H_ #include @@ -90,4 +90,4 @@ class PinholeCameraProjector : public ProjectorBase { #include "wavemap/core/integrator/projection_model/impl/pinhole_camera_projector_inl.h" -#endif // WAVEMAP_INTEGRATOR_PROJECTION_MODEL_PINHOLE_CAMERA_PROJECTOR_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_PINHOLE_CAMERA_PROJECTOR_H_ diff --git a/library/wavemap/core/integrator/projection_model/projector_base.h b/library/include/wavemap/core/integrator/projection_model/projector_base.h similarity index 96% rename from library/wavemap/core/integrator/projection_model/projector_base.h rename to library/include/wavemap/core/integrator/projection_model/projector_base.h index 26ae0e523..a55ddbec8 100644 --- a/library/wavemap/core/integrator/projection_model/projector_base.h +++ b/library/include/wavemap/core/integrator/projection_model/projector_base.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTION_MODEL_PROJECTOR_BASE_H_ -#define WAVEMAP_INTEGRATOR_PROJECTION_MODEL_PROJECTOR_BASE_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_PROJECTOR_BASE_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_PROJECTOR_BASE_H_ #include #include @@ -132,4 +132,4 @@ class ProjectorBase { #include "wavemap/core/integrator/projection_model/impl/projector_base_inl.h" -#endif // WAVEMAP_INTEGRATOR_PROJECTION_MODEL_PROJECTOR_BASE_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_PROJECTOR_BASE_H_ diff --git a/library/wavemap/core/integrator/projection_model/projector_factory.h b/library/include/wavemap/core/integrator/projection_model/projector_factory.h similarity index 66% rename from library/wavemap/core/integrator/projection_model/projector_factory.h rename to library/include/wavemap/core/integrator/projection_model/projector_factory.h index 7c5f4d28d..d62a868dd 100644 --- a/library/wavemap/core/integrator/projection_model/projector_factory.h +++ b/library/include/wavemap/core/integrator/projection_model/projector_factory.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTION_MODEL_PROJECTOR_FACTORY_H_ -#define WAVEMAP_INTEGRATOR_PROJECTION_MODEL_PROJECTOR_FACTORY_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_PROJECTOR_FACTORY_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_PROJECTOR_FACTORY_H_ #include "wavemap/core/integrator/projection_model/projector_base.h" @@ -15,4 +15,4 @@ class ProjectorFactory { }; } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_PROJECTION_MODEL_PROJECTOR_FACTORY_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_PROJECTOR_FACTORY_H_ diff --git a/library/wavemap/core/integrator/projection_model/spherical_projector.h b/library/include/wavemap/core/integrator/projection_model/spherical_projector.h similarity index 92% rename from library/wavemap/core/integrator/projection_model/spherical_projector.h rename to library/include/wavemap/core/integrator/projection_model/spherical_projector.h index 61e19a756..8a013e105 100644 --- a/library/wavemap/core/integrator/projection_model/spherical_projector.h +++ b/library/include/wavemap/core/integrator/projection_model/spherical_projector.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTION_MODEL_SPHERICAL_PROJECTOR_H_ -#define WAVEMAP_INTEGRATOR_PROJECTION_MODEL_SPHERICAL_PROJECTOR_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_SPHERICAL_PROJECTOR_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_SPHERICAL_PROJECTOR_H_ #include #include @@ -70,4 +70,4 @@ class SphericalProjector : public ProjectorBase { #include "wavemap/core/integrator/projection_model/impl/spherical_projector_inl.h" -#endif // WAVEMAP_INTEGRATOR_PROJECTION_MODEL_SPHERICAL_PROJECTOR_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_SPHERICAL_PROJECTOR_H_ diff --git a/library/wavemap/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h b/library/include/wavemap/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h similarity index 84% rename from library/wavemap/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h rename to library/include/wavemap/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h index 3f987421b..9acf317f3 100644 --- a/library/wavemap/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h +++ b/library/include/wavemap/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_COARSE_TO_FINE_INTEGRATOR_H_ -#define WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_COARSE_TO_FINE_INTEGRATOR_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_COARSE_TO_FINE_INTEGRATOR_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_COARSE_TO_FINE_INTEGRATOR_H_ #include #include @@ -36,4 +36,4 @@ class CoarseToFineIntegrator : public ProjectiveIntegrator { }; } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_COARSE_TO_FINE_INTEGRATOR_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_COARSE_TO_FINE_INTEGRATOR_H_ diff --git a/library/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h b/library/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h similarity index 92% rename from library/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h rename to library/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h index e957dc541..496e7ab59 100644 --- a/library/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h +++ b/library/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_CHUNKED_WAVELET_INTEGRATOR_H_ -#define WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_CHUNKED_WAVELET_INTEGRATOR_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_CHUNKED_WAVELET_INTEGRATOR_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_CHUNKED_WAVELET_INTEGRATOR_H_ #include #include @@ -76,4 +76,4 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { #include "wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h" -#endif // WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_CHUNKED_WAVELET_INTEGRATOR_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_CHUNKED_WAVELET_INTEGRATOR_H_ diff --git a/library/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h b/library/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h similarity index 90% rename from library/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h rename to library/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h index f0d9ff612..ea726ff84 100644 --- a/library/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h +++ b/library/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_WAVELET_INTEGRATOR_H_ -#define WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_WAVELET_INTEGRATOR_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_WAVELET_INTEGRATOR_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_WAVELET_INTEGRATOR_H_ #include #include @@ -58,4 +58,4 @@ class HashedWaveletIntegrator : public ProjectiveIntegrator { #include "wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h" -#endif // WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_WAVELET_INTEGRATOR_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_WAVELET_INTEGRATOR_H_ diff --git a/library/wavemap/core/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h b/library/include/wavemap/core/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h similarity index 96% rename from library/wavemap/core/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h rename to library/include/wavemap/core/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h index 5c79f1ab8..e30596e4a 100644 --- a/library/wavemap/core/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h +++ b/library/include/wavemap/core/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HIERARCHICAL_RANGE_BOUNDS_H_ -#define WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HIERARCHICAL_RANGE_BOUNDS_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HIERARCHICAL_RANGE_BOUNDS_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HIERARCHICAL_RANGE_BOUNDS_H_ #include #include @@ -143,4 +143,4 @@ class HierarchicalRangeBounds { #include "wavemap/core/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h" -#endif // WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HIERARCHICAL_RANGE_BOUNDS_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HIERARCHICAL_RANGE_BOUNDS_H_ diff --git a/library/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h b/library/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h similarity index 90% rename from library/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h rename to library/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h index f3073904a..0d5e83c05 100644 --- a/library/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h +++ b/library/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_CHUNKED_WAVELET_INTEGRATOR_INL_H_ -#define WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_CHUNKED_WAVELET_INTEGRATOR_INL_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_CHUNKED_WAVELET_INTEGRATOR_INL_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_CHUNKED_WAVELET_INTEGRATOR_INL_H_ #include @@ -82,4 +82,4 @@ inline void HashedChunkedWaveletIntegrator::updateLeavesBatch( } } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_CHUNKED_WAVELET_INTEGRATOR_INL_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_CHUNKED_WAVELET_INTEGRATOR_INL_H_ diff --git a/library/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h b/library/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h similarity index 80% rename from library/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h rename to library/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h index 9569980c3..47ac96f5d 100644 --- a/library/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h +++ b/library/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_WAVELET_INTEGRATOR_INL_H_ -#define WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_WAVELET_INTEGRATOR_INL_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_WAVELET_INTEGRATOR_INL_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_WAVELET_INTEGRATOR_INL_H_ #include @@ -38,4 +38,4 @@ inline void HashedWaveletIntegrator::recursiveTester( // NOLINT } } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_WAVELET_INTEGRATOR_INL_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_WAVELET_INTEGRATOR_INL_H_ diff --git a/library/wavemap/core/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h b/library/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h similarity index 98% rename from library/wavemap/core/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h rename to library/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h index 4a2926147..e056385a2 100644 --- a/library/wavemap/core/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h +++ b/library/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hierarchical_range_bounds_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HIERARCHICAL_RANGE_BOUNDS_INL_H_ -#define WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HIERARCHICAL_RANGE_BOUNDS_INL_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HIERARCHICAL_RANGE_BOUNDS_INL_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HIERARCHICAL_RANGE_BOUNDS_INL_H_ #include #include @@ -379,4 +379,4 @@ std::vector> HierarchicalRangeBounds::computeReducedPyramid( } } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HIERARCHICAL_RANGE_BOUNDS_INL_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HIERARCHICAL_RANGE_BOUNDS_INL_H_ diff --git a/library/wavemap/core/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h b/library/include/wavemap/core/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h similarity index 91% rename from library/wavemap/core/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h rename to library/include/wavemap/core/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h index ca7cdd609..a014d4255 100644 --- a/library/wavemap/core/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h +++ b/library/include/wavemap/core/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_RANGE_IMAGE_INTERSECTOR_INL_H_ -#define WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_RANGE_IMAGE_INTERSECTOR_INL_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_RANGE_IMAGE_INTERSECTOR_INL_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_RANGE_IMAGE_INTERSECTOR_INL_H_ #include #include @@ -71,4 +71,4 @@ inline UpdateType RangeImageIntersector::determineUpdateType( } } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_RANGE_IMAGE_INTERSECTOR_INL_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_RANGE_IMAGE_INTERSECTOR_INL_H_ diff --git a/library/wavemap/core/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h b/library/include/wavemap/core/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h similarity index 94% rename from library/wavemap/core/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h rename to library/include/wavemap/core/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h index 3a867cc97..db1ae58e5 100644 --- a/library/wavemap/core/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h +++ b/library/include/wavemap/core/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_WAVELET_INTEGRATOR_INL_H_ -#define WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_WAVELET_INTEGRATOR_INL_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_WAVELET_INTEGRATOR_INL_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_WAVELET_INTEGRATOR_INL_H_ #include @@ -101,4 +101,4 @@ inline FloatingPoint WaveletIntegrator::recursiveSamplerCompressor( // NOLINT } } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_WAVELET_INTEGRATOR_INL_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_WAVELET_INTEGRATOR_INL_H_ diff --git a/library/wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h b/library/include/wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h similarity index 88% rename from library/wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h rename to library/include/wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h index 282e51aa2..d4f893c0f 100644 --- a/library/wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h +++ b/library/include/wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_RANGE_IMAGE_INTERSECTOR_H_ -#define WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_RANGE_IMAGE_INTERSECTOR_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_RANGE_IMAGE_INTERSECTOR_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_RANGE_IMAGE_INTERSECTOR_H_ #include #include @@ -50,4 +50,4 @@ class RangeImageIntersector { #include "wavemap/core/integrator/projective/coarse_to_fine/impl/range_image_intersector_inl.h" -#endif // WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_RANGE_IMAGE_INTERSECTOR_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_RANGE_IMAGE_INTERSECTOR_H_ diff --git a/library/wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.h b/library/include/wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.h similarity index 87% rename from library/wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.h rename to library/include/wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.h index 3c2da3ff5..6957e5438 100644 --- a/library/wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.h +++ b/library/include/wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_WAVELET_INTEGRATOR_H_ -#define WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_WAVELET_INTEGRATOR_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_WAVELET_INTEGRATOR_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_WAVELET_INTEGRATOR_H_ #include #include @@ -43,4 +43,4 @@ class WaveletIntegrator : public ProjectiveIntegrator { #include "wavemap/core/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h" -#endif // WAVEMAP_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_WAVELET_INTEGRATOR_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_WAVELET_INTEGRATOR_H_ diff --git a/library/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.h b/library/include/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.h similarity index 81% rename from library/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.h rename to library/include/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.h index 8d81cf7f5..7e11aebc7 100644 --- a/library/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.h +++ b/library/include/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTIVE_FIXED_RESOLUTION_FIXED_RESOLUTION_INTEGRATOR_H_ -#define WAVEMAP_INTEGRATOR_PROJECTIVE_FIXED_RESOLUTION_FIXED_RESOLUTION_INTEGRATOR_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_FIXED_RESOLUTION_FIXED_RESOLUTION_INTEGRATOR_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_FIXED_RESOLUTION_FIXED_RESOLUTION_INTEGRATOR_H_ #include #include @@ -33,4 +33,4 @@ class FixedResolutionIntegrator : public ProjectiveIntegrator { }; } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_PROJECTIVE_FIXED_RESOLUTION_FIXED_RESOLUTION_INTEGRATOR_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_FIXED_RESOLUTION_FIXED_RESOLUTION_INTEGRATOR_H_ diff --git a/library/wavemap/core/integrator/projective/impl/projective_integrator_inl.h b/library/include/wavemap/core/integrator/projective/impl/projective_integrator_inl.h similarity index 73% rename from library/wavemap/core/integrator/projective/impl/projective_integrator_inl.h rename to library/include/wavemap/core/integrator/projective/impl/projective_integrator_inl.h index d366bf9d4..29c0e96cc 100644 --- a/library/wavemap/core/integrator/projective/impl/projective_integrator_inl.h +++ b/library/include/wavemap/core/integrator/projective/impl/projective_integrator_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTIVE_IMPL_PROJECTIVE_INTEGRATOR_INL_H_ -#define WAVEMAP_INTEGRATOR_PROJECTIVE_IMPL_PROJECTIVE_INTEGRATOR_INL_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_IMPL_PROJECTIVE_INTEGRATOR_INL_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_IMPL_PROJECTIVE_INTEGRATOR_INL_H_ namespace wavemap { inline FloatingPoint ProjectiveIntegrator::computeUpdate( @@ -20,4 +20,4 @@ inline FloatingPoint ProjectiveIntegrator::computeUpdate( } } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_PROJECTIVE_IMPL_PROJECTIVE_INTEGRATOR_INL_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_IMPL_PROJECTIVE_INTEGRATOR_INL_H_ diff --git a/library/wavemap/core/integrator/projective/projective_integrator.h b/library/include/wavemap/core/integrator/projective/projective_integrator.h similarity index 95% rename from library/wavemap/core/integrator/projective/projective_integrator.h rename to library/include/wavemap/core/integrator/projective/projective_integrator.h index 103c8ea6b..047a08fd4 100644 --- a/library/wavemap/core/integrator/projective/projective_integrator.h +++ b/library/include/wavemap/core/integrator/projective/projective_integrator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTIVE_PROJECTIVE_INTEGRATOR_H_ -#define WAVEMAP_INTEGRATOR_PROJECTIVE_PROJECTIVE_INTEGRATOR_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_PROJECTIVE_INTEGRATOR_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_PROJECTIVE_INTEGRATOR_H_ #include #include @@ -103,4 +103,4 @@ class ProjectiveIntegrator : public IntegratorBase { #include "wavemap/core/integrator/projective/impl/projective_integrator_inl.h" -#endif // WAVEMAP_INTEGRATOR_PROJECTIVE_PROJECTIVE_INTEGRATOR_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_PROJECTIVE_INTEGRATOR_H_ diff --git a/library/wavemap/core/integrator/projective/update_type.h b/library/include/wavemap/core/integrator/projective/update_type.h similarity index 75% rename from library/wavemap/core/integrator/projective/update_type.h rename to library/include/wavemap/core/integrator/projective/update_type.h index 5dcadf12c..f2e4ab8a2 100644 --- a/library/wavemap/core/integrator/projective/update_type.h +++ b/library/include/wavemap/core/integrator/projective/update_type.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_PROJECTIVE_UPDATE_TYPE_H_ -#define WAVEMAP_INTEGRATOR_PROJECTIVE_UPDATE_TYPE_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_UPDATE_TYPE_H_ +#define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_UPDATE_TYPE_H_ #include @@ -23,4 +23,4 @@ struct UpdateType : TypeSelector { }; } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_PROJECTIVE_UPDATE_TYPE_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_UPDATE_TYPE_H_ diff --git a/library/wavemap/core/integrator/ray_tracing/ray_tracing_integrator.h b/library/include/wavemap/core/integrator/ray_tracing/ray_tracing_integrator.h similarity index 85% rename from library/wavemap/core/integrator/ray_tracing/ray_tracing_integrator.h rename to library/include/wavemap/core/integrator/ray_tracing/ray_tracing_integrator.h index 04baac12f..c94098e0e 100644 --- a/library/wavemap/core/integrator/ray_tracing/ray_tracing_integrator.h +++ b/library/include/wavemap/core/integrator/ray_tracing/ray_tracing_integrator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_INTEGRATOR_RAY_TRACING_RAY_TRACING_INTEGRATOR_H_ -#define WAVEMAP_INTEGRATOR_RAY_TRACING_RAY_TRACING_INTEGRATOR_H_ +#ifndef WAVEMAP_CORE_INTEGRATOR_RAY_TRACING_RAY_TRACING_INTEGRATOR_H_ +#define WAVEMAP_CORE_INTEGRATOR_RAY_TRACING_RAY_TRACING_INTEGRATOR_H_ #include @@ -43,4 +43,4 @@ class RayTracingIntegrator : public IntegratorBase { }; } // namespace wavemap -#endif // WAVEMAP_INTEGRATOR_RAY_TRACING_RAY_TRACING_INTEGRATOR_H_ +#endif // WAVEMAP_CORE_INTEGRATOR_RAY_TRACING_RAY_TRACING_INTEGRATOR_H_ diff --git a/library/wavemap/core/map/cell_types/haar_coefficients.h b/library/include/wavemap/core/map/cell_types/haar_coefficients.h similarity index 96% rename from library/wavemap/core/map/cell_types/haar_coefficients.h rename to library/include/wavemap/core/map/cell_types/haar_coefficients.h index 0067af0a1..6de02e55a 100644 --- a/library/wavemap/core/map/cell_types/haar_coefficients.h +++ b/library/include/wavemap/core/map/cell_types/haar_coefficients.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_CELL_TYPES_HAAR_COEFFICIENTS_H_ -#define WAVEMAP_MAP_CELL_TYPES_HAAR_COEFFICIENTS_H_ +#ifndef WAVEMAP_CORE_MAP_CELL_TYPES_HAAR_COEFFICIENTS_H_ +#define WAVEMAP_CORE_MAP_CELL_TYPES_HAAR_COEFFICIENTS_H_ #include #include @@ -138,4 +138,4 @@ struct HaarCoefficients { }; } // namespace wavemap -#endif // WAVEMAP_MAP_CELL_TYPES_HAAR_COEFFICIENTS_H_ +#endif // WAVEMAP_CORE_MAP_CELL_TYPES_HAAR_COEFFICIENTS_H_ diff --git a/library/wavemap/core/map/cell_types/haar_transform.h b/library/include/wavemap/core/map/cell_types/haar_transform.h similarity index 97% rename from library/wavemap/core/map/cell_types/haar_transform.h rename to library/include/wavemap/core/map/cell_types/haar_transform.h index eb653129d..ddbe07e63 100644 --- a/library/wavemap/core/map/cell_types/haar_transform.h +++ b/library/include/wavemap/core/map/cell_types/haar_transform.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_CELL_TYPES_HAAR_TRANSFORM_H_ -#define WAVEMAP_MAP_CELL_TYPES_HAAR_TRANSFORM_H_ +#ifndef WAVEMAP_CORE_MAP_CELL_TYPES_HAAR_TRANSFORM_H_ +#define WAVEMAP_CORE_MAP_CELL_TYPES_HAAR_TRANSFORM_H_ #include @@ -137,4 +137,4 @@ struct HaarTransform { #include "wavemap/core/map/cell_types/impl/haar_transform_inl.h" -#endif // WAVEMAP_MAP_CELL_TYPES_HAAR_TRANSFORM_H_ +#endif // WAVEMAP_CORE_MAP_CELL_TYPES_HAAR_TRANSFORM_H_ diff --git a/library/wavemap/core/map/cell_types/impl/haar_transform_inl.h b/library/include/wavemap/core/map/cell_types/impl/haar_transform_inl.h similarity index 97% rename from library/wavemap/core/map/cell_types/impl/haar_transform_inl.h rename to library/include/wavemap/core/map/cell_types/impl/haar_transform_inl.h index 5dccb2491..a2f040497 100644 --- a/library/wavemap/core/map/cell_types/impl/haar_transform_inl.h +++ b/library/include/wavemap/core/map/cell_types/impl/haar_transform_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_CELL_TYPES_IMPL_HAAR_TRANSFORM_INL_H_ -#define WAVEMAP_MAP_CELL_TYPES_IMPL_HAAR_TRANSFORM_INL_H_ +#ifndef WAVEMAP_CORE_MAP_CELL_TYPES_IMPL_HAAR_TRANSFORM_INL_H_ +#define WAVEMAP_CORE_MAP_CELL_TYPES_IMPL_HAAR_TRANSFORM_INL_H_ #include "wavemap/core/utils/bits/bit_operations.h" #include "wavemap/core/utils/math/int_math.h" @@ -158,4 +158,4 @@ typename HaarCoefficients::Scale BackwardSingleChild( } } // namespace wavemap -#endif // WAVEMAP_MAP_CELL_TYPES_IMPL_HAAR_TRANSFORM_INL_H_ +#endif // WAVEMAP_CORE_MAP_CELL_TYPES_IMPL_HAAR_TRANSFORM_INL_H_ diff --git a/library/wavemap/core/map/hashed_blocks.h b/library/include/wavemap/core/map/hashed_blocks.h similarity index 93% rename from library/wavemap/core/map/hashed_blocks.h rename to library/include/wavemap/core/map/hashed_blocks.h index e4ce5c66e..703badbb1 100644 --- a/library/wavemap/core/map/hashed_blocks.h +++ b/library/include/wavemap/core/map/hashed_blocks.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_HASHED_BLOCKS_H_ -#define WAVEMAP_MAP_HASHED_BLOCKS_H_ +#ifndef WAVEMAP_CORE_MAP_HASHED_BLOCKS_H_ +#define WAVEMAP_CORE_MAP_HASHED_BLOCKS_H_ #include #include @@ -55,4 +55,4 @@ class HashedBlocks : public MapBase, #include "wavemap/core/map/impl/hashed_blocks_inl.h" -#endif // WAVEMAP_MAP_HASHED_BLOCKS_H_ +#endif // WAVEMAP_CORE_MAP_HASHED_BLOCKS_H_ diff --git a/library/wavemap/core/map/hashed_chunked_wavelet_octree.h b/library/include/wavemap/core/map/hashed_chunked_wavelet_octree.h similarity index 96% rename from library/wavemap/core/map/hashed_chunked_wavelet_octree.h rename to library/include/wavemap/core/map/hashed_chunked_wavelet_octree.h index 47948299c..4b3ce8298 100644 --- a/library/wavemap/core/map/hashed_chunked_wavelet_octree.h +++ b/library/include/wavemap/core/map/hashed_chunked_wavelet_octree.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_HASHED_CHUNKED_WAVELET_OCTREE_H_ -#define WAVEMAP_MAP_HASHED_CHUNKED_WAVELET_OCTREE_H_ +#ifndef WAVEMAP_CORE_MAP_HASHED_CHUNKED_WAVELET_OCTREE_H_ +#define WAVEMAP_CORE_MAP_HASHED_CHUNKED_WAVELET_OCTREE_H_ #include #include @@ -135,4 +135,4 @@ class HashedChunkedWaveletOctree : public MapBase { #include "wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h" -#endif // WAVEMAP_MAP_HASHED_CHUNKED_WAVELET_OCTREE_H_ +#endif // WAVEMAP_CORE_MAP_HASHED_CHUNKED_WAVELET_OCTREE_H_ diff --git a/library/wavemap/core/map/hashed_chunked_wavelet_octree_block.h b/library/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h similarity index 95% rename from library/wavemap/core/map/hashed_chunked_wavelet_octree_block.h rename to library/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h index e7a3b6024..3e885eff7 100644 --- a/library/wavemap/core/map/hashed_chunked_wavelet_octree_block.h +++ b/library/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_H_ -#define WAVEMAP_MAP_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_H_ +#ifndef WAVEMAP_CORE_MAP_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_H_ +#define WAVEMAP_CORE_MAP_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_H_ #include "wavemap/core/common.h" #include "wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h" @@ -108,4 +108,4 @@ class HashedChunkedWaveletOctreeBlock { #include "wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h" -#endif // WAVEMAP_MAP_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_H_ +#endif // WAVEMAP_CORE_MAP_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_H_ diff --git a/library/wavemap/core/map/hashed_wavelet_octree.h b/library/include/wavemap/core/map/hashed_wavelet_octree.h similarity index 96% rename from library/wavemap/core/map/hashed_wavelet_octree.h rename to library/include/wavemap/core/map/hashed_wavelet_octree.h index 6f591f2a3..764012102 100644 --- a/library/wavemap/core/map/hashed_wavelet_octree.h +++ b/library/include/wavemap/core/map/hashed_wavelet_octree.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_HASHED_WAVELET_OCTREE_H_ -#define WAVEMAP_MAP_HASHED_WAVELET_OCTREE_H_ +#ifndef WAVEMAP_CORE_MAP_HASHED_WAVELET_OCTREE_H_ +#define WAVEMAP_CORE_MAP_HASHED_WAVELET_OCTREE_H_ #include #include @@ -129,4 +129,4 @@ class HashedWaveletOctree : public MapBase { #include "wavemap/core/map/impl/hashed_wavelet_octree_inl.h" -#endif // WAVEMAP_MAP_HASHED_WAVELET_OCTREE_H_ +#endif // WAVEMAP_CORE_MAP_HASHED_WAVELET_OCTREE_H_ diff --git a/library/wavemap/core/map/hashed_wavelet_octree_block.h b/library/include/wavemap/core/map/hashed_wavelet_octree_block.h similarity index 94% rename from library/wavemap/core/map/hashed_wavelet_octree_block.h rename to library/include/wavemap/core/map/hashed_wavelet_octree_block.h index c48ce7fea..3be70a41e 100644 --- a/library/wavemap/core/map/hashed_wavelet_octree_block.h +++ b/library/include/wavemap/core/map/hashed_wavelet_octree_block.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_HASHED_WAVELET_OCTREE_BLOCK_H_ -#define WAVEMAP_MAP_HASHED_WAVELET_OCTREE_BLOCK_H_ +#ifndef WAVEMAP_CORE_MAP_HASHED_WAVELET_OCTREE_BLOCK_H_ +#define WAVEMAP_CORE_MAP_HASHED_WAVELET_OCTREE_BLOCK_H_ #include "wavemap/core/common.h" #include "wavemap/core/data_structure/ndtree/ndtree.h" @@ -87,4 +87,4 @@ class HashedWaveletOctreeBlock { #include "wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h" -#endif // WAVEMAP_MAP_HASHED_WAVELET_OCTREE_BLOCK_H_ +#endif // WAVEMAP_CORE_MAP_HASHED_WAVELET_OCTREE_BLOCK_H_ diff --git a/library/wavemap/core/map/impl/hashed_blocks_inl.h b/library/include/wavemap/core/map/impl/hashed_blocks_inl.h similarity index 82% rename from library/wavemap/core/map/impl/hashed_blocks_inl.h rename to library/include/wavemap/core/map/impl/hashed_blocks_inl.h index c2ed13b9c..99683c40a 100644 --- a/library/wavemap/core/map/impl/hashed_blocks_inl.h +++ b/library/include/wavemap/core/map/impl/hashed_blocks_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_IMPL_HASHED_BLOCKS_INL_H_ -#define WAVEMAP_MAP_IMPL_HASHED_BLOCKS_INL_H_ +#ifndef WAVEMAP_CORE_MAP_IMPL_HASHED_BLOCKS_INL_H_ +#define WAVEMAP_CORE_MAP_IMPL_HASHED_BLOCKS_INL_H_ #include #include @@ -25,4 +25,4 @@ inline void HashedBlocks::addToCellValue(const Index3D& index, } } // namespace wavemap -#endif // WAVEMAP_MAP_IMPL_HASHED_BLOCKS_INL_H_ +#endif // WAVEMAP_CORE_MAP_IMPL_HASHED_BLOCKS_INL_H_ diff --git a/library/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h b/library/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h similarity index 91% rename from library/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h rename to library/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h index cd773432e..42fc84154 100644 --- a/library/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h +++ b/library/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ -#define WAVEMAP_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ +#ifndef WAVEMAP_CORE_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ +#define WAVEMAP_CORE_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ #include "wavemap/core/utils/query/occupancy_classifier.h" @@ -62,4 +62,4 @@ inline FloatingPoint HashedChunkedWaveletOctreeBlock::getCellValue( } } // namespace wavemap -#endif // WAVEMAP_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ +#endif // WAVEMAP_CORE_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ diff --git a/library/wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h b/library/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h similarity index 95% rename from library/wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h rename to library/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h index b1e91b741..f6182d909 100644 --- a/library/wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h +++ b/library/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_INL_H_ -#define WAVEMAP_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_INL_H_ +#ifndef WAVEMAP_CORE_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_INL_H_ +#define WAVEMAP_CORE_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_INL_H_ #include "wavemap/core/indexing/index_conversions.h" @@ -114,4 +114,4 @@ HashedChunkedWaveletOctree::indexToCellIndex(OctreeIndex index) const { } } // namespace wavemap -#endif // WAVEMAP_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_INL_H_ +#endif // WAVEMAP_CORE_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_INL_H_ diff --git a/library/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h b/library/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h similarity index 87% rename from library/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h rename to library/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h index b6c71dc57..2319ec385 100644 --- a/library/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h +++ b/library/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ -#define WAVEMAP_MAP_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ +#ifndef WAVEMAP_CORE_MAP_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ +#define WAVEMAP_CORE_MAP_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ #include "wavemap/core/utils/query/occupancy_classifier.h" @@ -36,4 +36,4 @@ inline FloatingPoint HashedWaveletOctreeBlock::getCellValue( } } // namespace wavemap -#endif // WAVEMAP_MAP_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ +#endif // WAVEMAP_CORE_MAP_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ diff --git a/library/wavemap/core/map/impl/hashed_wavelet_octree_inl.h b/library/include/wavemap/core/map/impl/hashed_wavelet_octree_inl.h similarity index 95% rename from library/wavemap/core/map/impl/hashed_wavelet_octree_inl.h rename to library/include/wavemap/core/map/impl/hashed_wavelet_octree_inl.h index 4f8366264..3866b97dd 100644 --- a/library/wavemap/core/map/impl/hashed_wavelet_octree_inl.h +++ b/library/include/wavemap/core/map/impl/hashed_wavelet_octree_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_IMPL_HASHED_WAVELET_OCTREE_INL_H_ -#define WAVEMAP_MAP_IMPL_HASHED_WAVELET_OCTREE_INL_H_ +#ifndef WAVEMAP_CORE_MAP_IMPL_HASHED_WAVELET_OCTREE_INL_H_ +#define WAVEMAP_CORE_MAP_IMPL_HASHED_WAVELET_OCTREE_INL_H_ #include "wavemap/core/indexing/index_conversions.h" @@ -110,4 +110,4 @@ inline HashedWaveletOctree::CellIndex HashedWaveletOctree::indexToCellIndex( } } // namespace wavemap -#endif // WAVEMAP_MAP_IMPL_HASHED_WAVELET_OCTREE_INL_H_ +#endif // WAVEMAP_CORE_MAP_IMPL_HASHED_WAVELET_OCTREE_INL_H_ diff --git a/library/wavemap/core/map/impl/volumetric_octree_inl.h b/library/include/wavemap/core/map/impl/volumetric_octree_inl.h similarity index 94% rename from library/wavemap/core/map/impl/volumetric_octree_inl.h rename to library/include/wavemap/core/map/impl/volumetric_octree_inl.h index 0e033a44f..9d880fdd8 100644 --- a/library/wavemap/core/map/impl/volumetric_octree_inl.h +++ b/library/include/wavemap/core/map/impl/volumetric_octree_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_IMPL_VOLUMETRIC_OCTREE_INL_H_ -#define WAVEMAP_MAP_IMPL_VOLUMETRIC_OCTREE_INL_H_ +#ifndef WAVEMAP_CORE_MAP_IMPL_VOLUMETRIC_OCTREE_INL_H_ +#define WAVEMAP_CORE_MAP_IMPL_VOLUMETRIC_OCTREE_INL_H_ #include #include @@ -80,4 +80,4 @@ VolumetricOctree::getDeepestNodeAtIndex(const Index3D& index) const { } } // namespace wavemap -#endif // WAVEMAP_MAP_IMPL_VOLUMETRIC_OCTREE_INL_H_ +#endif // WAVEMAP_CORE_MAP_IMPL_VOLUMETRIC_OCTREE_INL_H_ diff --git a/library/wavemap/core/map/impl/wavelet_octree_inl.h b/library/include/wavemap/core/map/impl/wavelet_octree_inl.h similarity index 97% rename from library/wavemap/core/map/impl/wavelet_octree_inl.h rename to library/include/wavemap/core/map/impl/wavelet_octree_inl.h index 2f2f3808f..99f32624a 100644 --- a/library/wavemap/core/map/impl/wavelet_octree_inl.h +++ b/library/include/wavemap/core/map/impl/wavelet_octree_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_IMPL_WAVELET_OCTREE_INL_H_ -#define WAVEMAP_MAP_IMPL_WAVELET_OCTREE_INL_H_ +#ifndef WAVEMAP_CORE_MAP_IMPL_WAVELET_OCTREE_INL_H_ +#define WAVEMAP_CORE_MAP_IMPL_WAVELET_OCTREE_INL_H_ #include #include @@ -148,4 +148,4 @@ inline void WaveletOctree::addToCellValue(const OctreeIndex& index, } } // namespace wavemap -#endif // WAVEMAP_MAP_IMPL_WAVELET_OCTREE_INL_H_ +#endif // WAVEMAP_CORE_MAP_IMPL_WAVELET_OCTREE_INL_H_ diff --git a/library/wavemap/core/map/map_base.h b/library/include/wavemap/core/map/map_base.h similarity index 96% rename from library/wavemap/core/map/map_base.h rename to library/include/wavemap/core/map/map_base.h index 5c0de882e..6f31b439c 100644 --- a/library/wavemap/core/map/map_base.h +++ b/library/include/wavemap/core/map/map_base.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_MAP_BASE_H_ -#define WAVEMAP_MAP_MAP_BASE_H_ +#ifndef WAVEMAP_CORE_MAP_MAP_BASE_H_ +#define WAVEMAP_CORE_MAP_MAP_BASE_H_ #include #include @@ -103,4 +103,4 @@ class MapBase { }; } // namespace wavemap -#endif // WAVEMAP_MAP_MAP_BASE_H_ +#endif // WAVEMAP_CORE_MAP_MAP_BASE_H_ diff --git a/library/wavemap/core/map/map_factory.h b/library/include/wavemap/core/map/map_factory.h similarity index 73% rename from library/wavemap/core/map/map_factory.h rename to library/include/wavemap/core/map/map_factory.h index fbdd261c9..3b1ccb7b7 100644 --- a/library/wavemap/core/map/map_factory.h +++ b/library/include/wavemap/core/map/map_factory.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_MAP_FACTORY_H_ -#define WAVEMAP_MAP_MAP_FACTORY_H_ +#ifndef WAVEMAP_CORE_MAP_MAP_FACTORY_H_ +#define WAVEMAP_CORE_MAP_MAP_FACTORY_H_ #include @@ -16,4 +16,4 @@ class MapFactory { }; } // namespace wavemap -#endif // WAVEMAP_MAP_MAP_FACTORY_H_ +#endif // WAVEMAP_CORE_MAP_MAP_FACTORY_H_ diff --git a/library/wavemap/core/map/volumetric_octree.h b/library/include/wavemap/core/map/volumetric_octree.h similarity index 97% rename from library/wavemap/core/map/volumetric_octree.h rename to library/include/wavemap/core/map/volumetric_octree.h index 1eb566f70..b763f5a11 100644 --- a/library/wavemap/core/map/volumetric_octree.h +++ b/library/include/wavemap/core/map/volumetric_octree.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_VOLUMETRIC_OCTREE_H_ -#define WAVEMAP_MAP_VOLUMETRIC_OCTREE_H_ +#ifndef WAVEMAP_CORE_MAP_VOLUMETRIC_OCTREE_H_ +#define WAVEMAP_CORE_MAP_VOLUMETRIC_OCTREE_H_ #include #include @@ -132,4 +132,4 @@ class VolumetricOctree : public MapBase { #include "wavemap/core/map/impl/volumetric_octree_inl.h" -#endif // WAVEMAP_MAP_VOLUMETRIC_OCTREE_H_ +#endif // WAVEMAP_CORE_MAP_VOLUMETRIC_OCTREE_H_ diff --git a/library/wavemap/core/map/wavelet_octree.h b/library/include/wavemap/core/map/wavelet_octree.h similarity index 97% rename from library/wavemap/core/map/wavelet_octree.h rename to library/include/wavemap/core/map/wavelet_octree.h index 5c7bb2343..95e6beffc 100644 --- a/library/wavemap/core/map/wavelet_octree.h +++ b/library/include/wavemap/core/map/wavelet_octree.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_MAP_WAVELET_OCTREE_H_ -#define WAVEMAP_MAP_WAVELET_OCTREE_H_ +#ifndef WAVEMAP_CORE_MAP_WAVELET_OCTREE_H_ +#define WAVEMAP_CORE_MAP_WAVELET_OCTREE_H_ #include #include @@ -147,4 +147,4 @@ class WaveletOctree : public MapBase { #include "wavemap/core/map/impl/wavelet_octree_inl.h" -#endif // WAVEMAP_MAP_WAVELET_OCTREE_H_ +#endif // WAVEMAP_CORE_MAP_WAVELET_OCTREE_H_ diff --git a/library/wavemap/core/utils/bits/bit_operations.h b/library/include/wavemap/core/utils/bits/bit_operations.h similarity index 97% rename from library/wavemap/core/utils/bits/bit_operations.h rename to library/include/wavemap/core/utils/bits/bit_operations.h index 4e532b829..2db127bcc 100644 --- a/library/wavemap/core/utils/bits/bit_operations.h +++ b/library/include/wavemap/core/utils/bits/bit_operations.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_BITS_BIT_OPERATIONS_H_ -#define WAVEMAP_UTILS_BITS_BIT_OPERATIONS_H_ +#ifndef WAVEMAP_CORE_UTILS_BITS_BIT_OPERATIONS_H_ +#define WAVEMAP_CORE_UTILS_BITS_BIT_OPERATIONS_H_ #include "wavemap/core/common.h" @@ -199,4 +199,4 @@ T compress(T source, T selector) { } } // namespace wavemap::bit_ops -#endif // WAVEMAP_UTILS_BITS_BIT_OPERATIONS_H_ +#endif // WAVEMAP_CORE_UTILS_BITS_BIT_OPERATIONS_H_ diff --git a/library/wavemap/core/utils/bits/morton_encoding.h b/library/include/wavemap/core/utils/bits/morton_encoding.h similarity index 97% rename from library/wavemap/core/utils/bits/morton_encoding.h rename to library/include/wavemap/core/utils/bits/morton_encoding.h index 5eaccba4c..c3464d1e1 100644 --- a/library/wavemap/core/utils/bits/morton_encoding.h +++ b/library/include/wavemap/core/utils/bits/morton_encoding.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_BITS_MORTON_ENCODING_H_ -#define WAVEMAP_UTILS_BITS_MORTON_ENCODING_H_ +#ifndef WAVEMAP_CORE_UTILS_BITS_MORTON_ENCODING_H_ +#define WAVEMAP_CORE_UTILS_BITS_MORTON_ENCODING_H_ #include @@ -140,4 +140,4 @@ Index decode(MortonIndex morton) { } } // namespace wavemap::morton -#endif // WAVEMAP_UTILS_BITS_MORTON_ENCODING_H_ +#endif // WAVEMAP_CORE_UTILS_BITS_MORTON_ENCODING_H_ diff --git a/library/wavemap/core/utils/data/comparisons.h b/library/include/wavemap/core/utils/data/comparisons.h similarity index 93% rename from library/wavemap/core/utils/data/comparisons.h rename to library/include/wavemap/core/utils/data/comparisons.h index 34366923f..791aa8ebc 100644 --- a/library/wavemap/core/utils/data/comparisons.h +++ b/library/include/wavemap/core/utils/data/comparisons.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_DATA_COMPARISONS_H_ -#define WAVEMAP_UTILS_DATA_COMPARISONS_H_ +#ifndef WAVEMAP_CORE_UTILS_DATA_COMPARISONS_H_ +#define WAVEMAP_CORE_UTILS_DATA_COMPARISONS_H_ #include #include @@ -56,4 +56,4 @@ bool EigenCwiseNear(const FirstType& matrix_a, const SecondType& matrix_b, } } // namespace wavemap::data -#endif // WAVEMAP_UTILS_DATA_COMPARISONS_H_ +#endif // WAVEMAP_CORE_UTILS_DATA_COMPARISONS_H_ diff --git a/library/wavemap/core/utils/data/constants.h b/library/include/wavemap/core/utils/data/constants.h similarity index 90% rename from library/wavemap/core/utils/data/constants.h rename to library/include/wavemap/core/utils/data/constants.h index 43cf24242..3d1b07136 100644 --- a/library/wavemap/core/utils/data/constants.h +++ b/library/include/wavemap/core/utils/data/constants.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_DATA_CONSTANTS_H_ -#define WAVEMAP_UTILS_DATA_CONSTANTS_H_ +#ifndef WAVEMAP_CORE_UTILS_DATA_CONSTANTS_H_ +#define WAVEMAP_CORE_UTILS_DATA_CONSTANTS_H_ namespace wavemap { // NOTE: We define commonly used constants here instead of directly using the @@ -24,4 +24,4 @@ struct constants { }; } // namespace wavemap -#endif // WAVEMAP_UTILS_DATA_CONSTANTS_H_ +#endif // WAVEMAP_CORE_UTILS_DATA_CONSTANTS_H_ diff --git a/library/wavemap/core/utils/data/eigen_checks.h b/library/include/wavemap/core/utils/data/eigen_checks.h similarity index 96% rename from library/wavemap/core/utils/data/eigen_checks.h rename to library/include/wavemap/core/utils/data/eigen_checks.h index 1cdfd4659..8e57c3340 100644 --- a/library/wavemap/core/utils/data/eigen_checks.h +++ b/library/include/wavemap/core/utils/data/eigen_checks.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_DATA_EIGEN_CHECKS_H_ -#define WAVEMAP_UTILS_DATA_EIGEN_CHECKS_H_ +#ifndef WAVEMAP_CORE_UTILS_DATA_EIGEN_CHECKS_H_ +#define WAVEMAP_CORE_UTILS_DATA_EIGEN_CHECKS_H_ #include @@ -91,4 +91,4 @@ #endif -#endif // WAVEMAP_UTILS_DATA_EIGEN_CHECKS_H_ +#endif // WAVEMAP_CORE_UTILS_DATA_EIGEN_CHECKS_H_ diff --git a/library/wavemap/core/utils/data/fill.h b/library/include/wavemap/core/utils/data/fill.h similarity index 81% rename from library/wavemap/core/utils/data/fill.h rename to library/include/wavemap/core/utils/data/fill.h index b05a60e90..610b7cc94 100644 --- a/library/wavemap/core/utils/data/fill.h +++ b/library/include/wavemap/core/utils/data/fill.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_DATA_FILL_H_ -#define WAVEMAP_UTILS_DATA_FILL_H_ +#ifndef WAVEMAP_CORE_UTILS_DATA_FILL_H_ +#define WAVEMAP_CORE_UTILS_DATA_FILL_H_ #include @@ -23,4 +23,4 @@ T zero() { } } // namespace wavemap::data::fill -#endif // WAVEMAP_UTILS_DATA_FILL_H_ +#endif // WAVEMAP_CORE_UTILS_DATA_FILL_H_ diff --git a/library/wavemap/core/utils/iterate/grid_iterator.h b/library/include/wavemap/core/utils/iterate/grid_iterator.h similarity index 92% rename from library/wavemap/core/utils/iterate/grid_iterator.h rename to library/include/wavemap/core/utils/iterate/grid_iterator.h index 2c0c8aa02..363f16b96 100644 --- a/library/wavemap/core/utils/iterate/grid_iterator.h +++ b/library/include/wavemap/core/utils/iterate/grid_iterator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_ITERATE_GRID_ITERATOR_H_ -#define WAVEMAP_UTILS_ITERATE_GRID_ITERATOR_H_ +#ifndef WAVEMAP_CORE_UTILS_ITERATE_GRID_ITERATOR_H_ +#define WAVEMAP_CORE_UTILS_ITERATE_GRID_ITERATOR_H_ #include @@ -65,4 +65,4 @@ class Grid { }; } // namespace wavemap -#endif // WAVEMAP_UTILS_ITERATE_GRID_ITERATOR_H_ +#endif // WAVEMAP_CORE_UTILS_ITERATE_GRID_ITERATOR_H_ diff --git a/library/wavemap/core/utils/iterate/impl/ray_iterator_inl.h b/library/include/wavemap/core/utils/iterate/impl/ray_iterator_inl.h similarity index 93% rename from library/wavemap/core/utils/iterate/impl/ray_iterator_inl.h rename to library/include/wavemap/core/utils/iterate/impl/ray_iterator_inl.h index 803ce93f1..7b02bc790 100644 --- a/library/wavemap/core/utils/iterate/impl/ray_iterator_inl.h +++ b/library/include/wavemap/core/utils/iterate/impl/ray_iterator_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_ITERATE_IMPL_RAY_ITERATOR_INL_H_ -#define WAVEMAP_UTILS_ITERATE_IMPL_RAY_ITERATOR_INL_H_ +#ifndef WAVEMAP_CORE_UTILS_ITERATE_IMPL_RAY_ITERATOR_INL_H_ +#define WAVEMAP_CORE_UTILS_ITERATE_IMPL_RAY_ITERATOR_INL_H_ namespace wavemap { template @@ -78,4 +78,4 @@ typename Ray::Iterator& Ray::Iterator::operator++() { } } // namespace wavemap -#endif // WAVEMAP_UTILS_ITERATE_IMPL_RAY_ITERATOR_INL_H_ +#endif // WAVEMAP_CORE_UTILS_ITERATE_IMPL_RAY_ITERATOR_INL_H_ diff --git a/library/wavemap/core/utils/iterate/impl/subtree_iterator_inl.h b/library/include/wavemap/core/utils/iterate/impl/subtree_iterator_inl.h similarity index 94% rename from library/wavemap/core/utils/iterate/impl/subtree_iterator_inl.h rename to library/include/wavemap/core/utils/iterate/impl/subtree_iterator_inl.h index 35114cfd2..6aeda437e 100644 --- a/library/wavemap/core/utils/iterate/impl/subtree_iterator_inl.h +++ b/library/include/wavemap/core/utils/iterate/impl/subtree_iterator_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_ITERATE_IMPL_SUBTREE_ITERATOR_INL_H_ -#define WAVEMAP_UTILS_ITERATE_IMPL_SUBTREE_ITERATOR_INL_H_ +#ifndef WAVEMAP_CORE_UTILS_ITERATE_IMPL_SUBTREE_ITERATOR_INL_H_ +#define WAVEMAP_CORE_UTILS_ITERATE_IMPL_SUBTREE_ITERATOR_INL_H_ namespace wavemap { template @@ -98,4 +98,4 @@ void SubtreeIterator::enqueueNodeChildren( } } // namespace wavemap -#endif // WAVEMAP_UTILS_ITERATE_IMPL_SUBTREE_ITERATOR_INL_H_ +#endif // WAVEMAP_CORE_UTILS_ITERATE_IMPL_SUBTREE_ITERATOR_INL_H_ diff --git a/library/wavemap/core/utils/iterate/pointcloud_iterator.h b/library/include/wavemap/core/utils/iterate/pointcloud_iterator.h similarity index 90% rename from library/wavemap/core/utils/iterate/pointcloud_iterator.h rename to library/include/wavemap/core/utils/iterate/pointcloud_iterator.h index 9ee610544..d7b5136e2 100644 --- a/library/wavemap/core/utils/iterate/pointcloud_iterator.h +++ b/library/include/wavemap/core/utils/iterate/pointcloud_iterator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_ITERATE_POINTCLOUD_ITERATOR_H_ -#define WAVEMAP_UTILS_ITERATE_POINTCLOUD_ITERATOR_H_ +#ifndef WAVEMAP_CORE_UTILS_ITERATE_POINTCLOUD_ITERATOR_H_ +#define WAVEMAP_CORE_UTILS_ITERATE_POINTCLOUD_ITERATOR_H_ #include @@ -52,4 +52,4 @@ class PointcloudIterator { }; } // namespace wavemap -#endif // WAVEMAP_UTILS_ITERATE_POINTCLOUD_ITERATOR_H_ +#endif // WAVEMAP_CORE_UTILS_ITERATE_POINTCLOUD_ITERATOR_H_ diff --git a/library/wavemap/core/utils/iterate/ray_iterator.h b/library/include/wavemap/core/utils/iterate/ray_iterator.h similarity index 93% rename from library/wavemap/core/utils/iterate/ray_iterator.h rename to library/include/wavemap/core/utils/iterate/ray_iterator.h index d95886f7b..7ae792434 100644 --- a/library/wavemap/core/utils/iterate/ray_iterator.h +++ b/library/include/wavemap/core/utils/iterate/ray_iterator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_ITERATE_RAY_ITERATOR_H_ -#define WAVEMAP_UTILS_ITERATE_RAY_ITERATOR_H_ +#ifndef WAVEMAP_CORE_UTILS_ITERATE_RAY_ITERATOR_H_ +#define WAVEMAP_CORE_UTILS_ITERATE_RAY_ITERATOR_H_ #include "wavemap/core/common.h" #include "wavemap/core/indexing/index_conversions.h" @@ -68,4 +68,4 @@ class Ray { #include "wavemap/core/utils/iterate/impl/ray_iterator_inl.h" -#endif // WAVEMAP_UTILS_ITERATE_RAY_ITERATOR_H_ +#endif // WAVEMAP_CORE_UTILS_ITERATE_RAY_ITERATOR_H_ diff --git a/library/wavemap/core/utils/iterate/subtree_iterator.h b/library/include/wavemap/core/utils/iterate/subtree_iterator.h similarity index 96% rename from library/wavemap/core/utils/iterate/subtree_iterator.h rename to library/include/wavemap/core/utils/iterate/subtree_iterator.h index 03c60c187..9050a0db0 100644 --- a/library/wavemap/core/utils/iterate/subtree_iterator.h +++ b/library/include/wavemap/core/utils/iterate/subtree_iterator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_ITERATE_SUBTREE_ITERATOR_H_ -#define WAVEMAP_UTILS_ITERATE_SUBTREE_ITERATOR_H_ +#ifndef WAVEMAP_CORE_UTILS_ITERATE_SUBTREE_ITERATOR_H_ +#define WAVEMAP_CORE_UTILS_ITERATE_SUBTREE_ITERATOR_H_ #include @@ -136,4 +136,4 @@ class Subtree { #include "wavemap/core/utils/iterate/impl/subtree_iterator_inl.h" -#endif // WAVEMAP_UTILS_ITERATE_SUBTREE_ITERATOR_H_ +#endif // WAVEMAP_CORE_UTILS_ITERATE_SUBTREE_ITERATOR_H_ diff --git a/library/wavemap/core/utils/math/angle_normalization.h b/library/include/wavemap/core/utils/math/angle_normalization.h similarity index 68% rename from library/wavemap/core/utils/math/angle_normalization.h rename to library/include/wavemap/core/utils/math/angle_normalization.h index 475a56009..49cda1671 100644 --- a/library/wavemap/core/utils/math/angle_normalization.h +++ b/library/include/wavemap/core/utils/math/angle_normalization.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_MATH_ANGLE_NORMALIZATION_H_ -#define WAVEMAP_UTILS_MATH_ANGLE_NORMALIZATION_H_ +#ifndef WAVEMAP_CORE_UTILS_MATH_ANGLE_NORMALIZATION_H_ +#define WAVEMAP_CORE_UTILS_MATH_ANGLE_NORMALIZATION_H_ #include "wavemap/core/common.h" @@ -18,4 +18,4 @@ inline FloatingPoint normalize_near(FloatingPoint angle) { } } // namespace wavemap::angle_math -#endif // WAVEMAP_UTILS_MATH_ANGLE_NORMALIZATION_H_ +#endif // WAVEMAP_CORE_UTILS_MATH_ANGLE_NORMALIZATION_H_ diff --git a/library/wavemap/core/utils/math/approximate_trigonometry.h b/library/include/wavemap/core/utils/math/approximate_trigonometry.h similarity index 94% rename from library/wavemap/core/utils/math/approximate_trigonometry.h rename to library/include/wavemap/core/utils/math/approximate_trigonometry.h index 5f9da273f..541dba076 100644 --- a/library/wavemap/core/utils/math/approximate_trigonometry.h +++ b/library/include/wavemap/core/utils/math/approximate_trigonometry.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_MATH_APPROXIMATE_TRIGONOMETRY_H_ -#define WAVEMAP_UTILS_MATH_APPROXIMATE_TRIGONOMETRY_H_ +#ifndef WAVEMAP_CORE_UTILS_MATH_APPROXIMATE_TRIGONOMETRY_H_ +#define WAVEMAP_CORE_UTILS_MATH_APPROXIMATE_TRIGONOMETRY_H_ #include #include @@ -79,4 +79,4 @@ struct atan2 }; } // namespace wavemap::approximate -#endif // WAVEMAP_UTILS_MATH_APPROXIMATE_TRIGONOMETRY_H_ +#endif // WAVEMAP_CORE_UTILS_MATH_APPROXIMATE_TRIGONOMETRY_H_ diff --git a/library/wavemap/core/utils/math/int_math.h b/library/include/wavemap/core/utils/math/int_math.h similarity index 96% rename from library/wavemap/core/utils/math/int_math.h rename to library/include/wavemap/core/utils/math/int_math.h index 6ead9a46f..f48431b18 100644 --- a/library/wavemap/core/utils/math/int_math.h +++ b/library/include/wavemap/core/utils/math/int_math.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_MATH_INT_MATH_H_ -#define WAVEMAP_UTILS_MATH_INT_MATH_H_ +#ifndef WAVEMAP_CORE_UTILS_MATH_INT_MATH_H_ +#define WAVEMAP_CORE_UTILS_MATH_INT_MATH_H_ #include @@ -132,4 +132,4 @@ constexpr T binomial(T n, T m) { } } // namespace wavemap::int_math -#endif // WAVEMAP_UTILS_MATH_INT_MATH_H_ +#endif // WAVEMAP_CORE_UTILS_MATH_INT_MATH_H_ diff --git a/library/wavemap/core/utils/math/tree_math.h b/library/include/wavemap/core/utils/math/tree_math.h similarity index 87% rename from library/wavemap/core/utils/math/tree_math.h rename to library/include/wavemap/core/utils/math/tree_math.h index 8870bfedc..5b69fb633 100644 --- a/library/wavemap/core/utils/math/tree_math.h +++ b/library/include/wavemap/core/utils/math/tree_math.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_MATH_TREE_MATH_H_ -#define WAVEMAP_UTILS_MATH_TREE_MATH_H_ +#ifndef WAVEMAP_CORE_UTILS_MATH_TREE_MATH_H_ +#define WAVEMAP_CORE_UTILS_MATH_TREE_MATH_H_ #include "wavemap/core/utils/math/int_math.h" @@ -32,4 +32,4 @@ constexpr size_t num_leaf_nodes(size_t tree_height) { } } // namespace wavemap::tree_math::perfect_tree -#endif // WAVEMAP_UTILS_MATH_TREE_MATH_H_ +#endif // WAVEMAP_CORE_UTILS_MATH_TREE_MATH_H_ diff --git a/library/wavemap/core/utils/meta/nameof.h b/library/include/wavemap/core/utils/meta/nameof.h similarity index 93% rename from library/wavemap/core/utils/meta/nameof.h rename to library/include/wavemap/core/utils/meta/nameof.h index 07ecc043c..032743e9a 100644 --- a/library/wavemap/core/utils/meta/nameof.h +++ b/library/include/wavemap/core/utils/meta/nameof.h @@ -27,8 +27,8 @@ ** ****************************************************************************/ -#ifndef WAVEMAP_UTILS_META_NAMEOF_H_ -#define WAVEMAP_UTILS_META_NAMEOF_H_ +#ifndef WAVEMAP_CORE_UTILS_META_NAMEOF_H_ +#define WAVEMAP_CORE_UTILS_META_NAMEOF_H_ #define NAMEOF(x) wavemap::meta::nameof<0>(#x, sizeof(x)) // NOLINT @@ -49,4 +49,4 @@ std::string nameof(const std::string& x, std::size_t) { } } // namespace wavemap::meta -#endif // WAVEMAP_UTILS_META_NAMEOF_H_ +#endif // WAVEMAP_CORE_UTILS_META_NAMEOF_H_ diff --git a/library/wavemap/core/utils/meta/type_utils.h b/library/include/wavemap/core/utils/meta/type_utils.h similarity index 93% rename from library/wavemap/core/utils/meta/type_utils.h rename to library/include/wavemap/core/utils/meta/type_utils.h index 854252d83..9a4c8b8d4 100644 --- a/library/wavemap/core/utils/meta/type_utils.h +++ b/library/include/wavemap/core/utils/meta/type_utils.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_META_TYPE_UTILS_H_ -#define WAVEMAP_UTILS_META_TYPE_UTILS_H_ +#ifndef WAVEMAP_CORE_UTILS_META_TYPE_UTILS_H_ +#define WAVEMAP_CORE_UTILS_META_TYPE_UTILS_H_ #include @@ -76,4 +76,4 @@ template constexpr bool has_to_str_member_fn_v = has_to_str_member_fn::value; } // namespace wavemap::meta -#endif // WAVEMAP_UTILS_META_TYPE_UTILS_H_ +#endif // WAVEMAP_CORE_UTILS_META_TYPE_UTILS_H_ diff --git a/library/wavemap/core/utils/neighbors/adjacency.h b/library/include/wavemap/core/utils/neighbors/adjacency.h similarity index 82% rename from library/wavemap/core/utils/neighbors/adjacency.h rename to library/include/wavemap/core/utils/neighbors/adjacency.h index 51aec4d8a..b4e06a928 100644 --- a/library/wavemap/core/utils/neighbors/adjacency.h +++ b/library/include/wavemap/core/utils/neighbors/adjacency.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_NEIGHBORS_ADJACENCY_H_ -#define WAVEMAP_UTILS_NEIGHBORS_ADJACENCY_H_ +#ifndef WAVEMAP_CORE_UTILS_NEIGHBORS_ADJACENCY_H_ +#define WAVEMAP_CORE_UTILS_NEIGHBORS_ADJACENCY_H_ #include "wavemap/core/common.h" #include "wavemap/core/config/type_selector.h" @@ -32,4 +32,4 @@ struct Adjacency : TypeSelector { #include "wavemap/core/utils/neighbors/impl/adjacency_inl.h" -#endif // WAVEMAP_UTILS_NEIGHBORS_ADJACENCY_H_ +#endif // WAVEMAP_CORE_UTILS_NEIGHBORS_ADJACENCY_H_ diff --git a/library/wavemap/core/utils/neighbors/grid_adjacency.h b/library/include/wavemap/core/utils/neighbors/grid_adjacency.h similarity index 89% rename from library/wavemap/core/utils/neighbors/grid_adjacency.h rename to library/include/wavemap/core/utils/neighbors/grid_adjacency.h index b07d8477d..3c57e9fe1 100644 --- a/library/wavemap/core/utils/neighbors/grid_adjacency.h +++ b/library/include/wavemap/core/utils/neighbors/grid_adjacency.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_NEIGHBORS_GRID_ADJACENCY_H_ -#define WAVEMAP_UTILS_NEIGHBORS_GRID_ADJACENCY_H_ +#ifndef WAVEMAP_CORE_UTILS_NEIGHBORS_GRID_ADJACENCY_H_ +#define WAVEMAP_CORE_UTILS_NEIGHBORS_GRID_ADJACENCY_H_ #include "wavemap/core/common.h" #include "wavemap/core/utils/neighbors/adjacency.h" @@ -41,4 +41,4 @@ constexpr bool isAdjacent(const Index& index_offset, #include "wavemap/core/utils/neighbors/impl/grid_adjacency_inl.h" -#endif // WAVEMAP_UTILS_NEIGHBORS_GRID_ADJACENCY_H_ +#endif // WAVEMAP_CORE_UTILS_NEIGHBORS_GRID_ADJACENCY_H_ diff --git a/library/wavemap/core/utils/neighbors/grid_neighborhood.h b/library/include/wavemap/core/utils/neighbors/grid_neighborhood.h similarity index 82% rename from library/wavemap/core/utils/neighbors/grid_neighborhood.h rename to library/include/wavemap/core/utils/neighbors/grid_neighborhood.h index 00bae2a38..6e8b262a4 100644 --- a/library/wavemap/core/utils/neighbors/grid_neighborhood.h +++ b/library/include/wavemap/core/utils/neighbors/grid_neighborhood.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_NEIGHBORS_GRID_NEIGHBORHOOD_H_ -#define WAVEMAP_UTILS_NEIGHBORS_GRID_NEIGHBORHOOD_H_ +#ifndef WAVEMAP_CORE_UTILS_NEIGHBORS_GRID_NEIGHBORHOOD_H_ +#define WAVEMAP_CORE_UTILS_NEIGHBORS_GRID_NEIGHBORHOOD_H_ #include "wavemap/core/common.h" #include "wavemap/core/utils/iterate/grid_iterator.h" @@ -24,4 +24,4 @@ struct GridNeighborhood { #include "wavemap/core/utils/neighbors/impl/grid_neighborhood_inl.h" -#endif // WAVEMAP_UTILS_NEIGHBORS_GRID_NEIGHBORHOOD_H_ +#endif // WAVEMAP_CORE_UTILS_NEIGHBORS_GRID_NEIGHBORHOOD_H_ diff --git a/library/wavemap/core/utils/neighbors/impl/adjacency_inl.h b/library/include/wavemap/core/utils/neighbors/impl/adjacency_inl.h similarity index 74% rename from library/wavemap/core/utils/neighbors/impl/adjacency_inl.h rename to library/include/wavemap/core/utils/neighbors/impl/adjacency_inl.h index 1d5549952..e36503e45 100644 --- a/library/wavemap/core/utils/neighbors/impl/adjacency_inl.h +++ b/library/include/wavemap/core/utils/neighbors/impl/adjacency_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_NEIGHBORS_IMPL_ADJACENCY_INL_H_ -#define WAVEMAP_UTILS_NEIGHBORS_IMPL_ADJACENCY_INL_H_ +#ifndef WAVEMAP_CORE_UTILS_NEIGHBORS_IMPL_ADJACENCY_INL_H_ +#define WAVEMAP_CORE_UTILS_NEIGHBORS_IMPL_ADJACENCY_INL_H_ namespace wavemap { template @@ -25,4 +25,4 @@ constexpr Adjacency::Mask Adjacency::toMask() const { } } // namespace wavemap -#endif // WAVEMAP_UTILS_NEIGHBORS_IMPL_ADJACENCY_INL_H_ +#endif // WAVEMAP_CORE_UTILS_NEIGHBORS_IMPL_ADJACENCY_INL_H_ diff --git a/library/wavemap/core/utils/neighbors/impl/grid_adjacency_inl.h b/library/include/wavemap/core/utils/neighbors/impl/grid_adjacency_inl.h similarity index 88% rename from library/wavemap/core/utils/neighbors/impl/grid_adjacency_inl.h rename to library/include/wavemap/core/utils/neighbors/impl/grid_adjacency_inl.h index 0500200e5..20adcb6c0 100644 --- a/library/wavemap/core/utils/neighbors/impl/grid_adjacency_inl.h +++ b/library/include/wavemap/core/utils/neighbors/impl/grid_adjacency_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_NEIGHBORS_IMPL_GRID_ADJACENCY_INL_H_ -#define WAVEMAP_UTILS_NEIGHBORS_IMPL_GRID_ADJACENCY_INL_H_ +#ifndef WAVEMAP_CORE_UTILS_NEIGHBORS_IMPL_GRID_ADJACENCY_INL_H_ +#define WAVEMAP_CORE_UTILS_NEIGHBORS_IMPL_GRID_ADJACENCY_INL_H_ #include "wavemap/core/utils/bits/bit_operations.h" @@ -47,4 +47,4 @@ constexpr bool isAdjacent(const Index& index_offset, } } // namespace wavemap -#endif // WAVEMAP_UTILS_NEIGHBORS_IMPL_GRID_ADJACENCY_INL_H_ +#endif // WAVEMAP_CORE_UTILS_NEIGHBORS_IMPL_GRID_ADJACENCY_INL_H_ diff --git a/library/wavemap/core/utils/neighbors/impl/grid_neighborhood_inl.h b/library/include/wavemap/core/utils/neighbors/impl/grid_neighborhood_inl.h similarity index 90% rename from library/wavemap/core/utils/neighbors/impl/grid_neighborhood_inl.h rename to library/include/wavemap/core/utils/neighbors/impl/grid_neighborhood_inl.h index 78c0b6ad8..b3b81c62d 100644 --- a/library/wavemap/core/utils/neighbors/impl/grid_neighborhood_inl.h +++ b/library/include/wavemap/core/utils/neighbors/impl/grid_neighborhood_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_NEIGHBORS_IMPL_GRID_NEIGHBORHOOD_INL_H_ -#define WAVEMAP_UTILS_NEIGHBORS_IMPL_GRID_NEIGHBORHOOD_INL_H_ +#ifndef WAVEMAP_CORE_UTILS_NEIGHBORS_IMPL_GRID_NEIGHBORHOOD_INL_H_ +#define WAVEMAP_CORE_UTILS_NEIGHBORS_IMPL_GRID_NEIGHBORHOOD_INL_H_ #include @@ -53,4 +53,4 @@ GridNeighborhood::computeOffsetLengths( } } // namespace wavemap -#endif // WAVEMAP_UTILS_NEIGHBORS_IMPL_GRID_NEIGHBORHOOD_INL_H_ +#endif // WAVEMAP_CORE_UTILS_NEIGHBORS_IMPL_GRID_NEIGHBORHOOD_INL_H_ diff --git a/library/include/wavemap/core/utils/neighbors/impl/ndtree_adjacency_inl.h b/library/include/wavemap/core/utils/neighbors/impl/ndtree_adjacency_inl.h new file mode 100644 index 000000000..45c5201de --- /dev/null +++ b/library/include/wavemap/core/utils/neighbors/impl/ndtree_adjacency_inl.h @@ -0,0 +1,6 @@ +#ifndef WAVEMAP_CORE_UTILS_NEIGHBORS_IMPL_NDTREE_ADJACENCY_INL_H_ +#define WAVEMAP_CORE_UTILS_NEIGHBORS_IMPL_NDTREE_ADJACENCY_INL_H_ + +namespace wavemap {} + +#endif // WAVEMAP_CORE_UTILS_NEIGHBORS_IMPL_NDTREE_ADJACENCY_INL_H_ diff --git a/library/wavemap/core/utils/neighbors/ndtree_adjacency.h b/library/include/wavemap/core/utils/neighbors/ndtree_adjacency.h similarity index 86% rename from library/wavemap/core/utils/neighbors/ndtree_adjacency.h rename to library/include/wavemap/core/utils/neighbors/ndtree_adjacency.h index 048e00a44..d93367dea 100644 --- a/library/wavemap/core/utils/neighbors/ndtree_adjacency.h +++ b/library/include/wavemap/core/utils/neighbors/ndtree_adjacency.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_NEIGHBORS_NDTREE_ADJACENCY_H_ -#define WAVEMAP_UTILS_NEIGHBORS_NDTREE_ADJACENCY_H_ +#ifndef WAVEMAP_CORE_UTILS_NEIGHBORS_NDTREE_ADJACENCY_H_ +#define WAVEMAP_CORE_UTILS_NEIGHBORS_NDTREE_ADJACENCY_H_ #include "wavemap/core/common.h" #include "wavemap/core/indexing/ndtree_index.h" @@ -30,4 +30,4 @@ static bool areAdjacent(const NdtreeIndex& index_1, #include "wavemap/core/utils/neighbors/impl/ndtree_adjacency_inl.h" -#endif // WAVEMAP_UTILS_NEIGHBORS_NDTREE_ADJACENCY_H_ +#endif // WAVEMAP_CORE_UTILS_NEIGHBORS_NDTREE_ADJACENCY_H_ diff --git a/library/wavemap/core/utils/print/container.h b/library/include/wavemap/core/utils/print/container.h similarity index 79% rename from library/wavemap/core/utils/print/container.h rename to library/include/wavemap/core/utils/print/container.h index 7e248107e..d725601fb 100644 --- a/library/wavemap/core/utils/print/container.h +++ b/library/include/wavemap/core/utils/print/container.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_PRINT_CONTAINER_H_ -#define WAVEMAP_UTILS_PRINT_CONTAINER_H_ +#ifndef WAVEMAP_CORE_UTILS_PRINT_CONTAINER_H_ +#define WAVEMAP_CORE_UTILS_PRINT_CONTAINER_H_ #include #include @@ -16,4 +16,4 @@ inline std::string container(const SequenceContainerT& container) { } } // namespace wavemap::print -#endif // WAVEMAP_UTILS_PRINT_CONTAINER_H_ +#endif // WAVEMAP_CORE_UTILS_PRINT_CONTAINER_H_ diff --git a/library/wavemap/core/utils/print/eigen.h b/library/include/wavemap/core/utils/print/eigen.h similarity index 74% rename from library/wavemap/core/utils/print/eigen.h rename to library/include/wavemap/core/utils/print/eigen.h index d53cf4e19..5ec1d48f6 100644 --- a/library/wavemap/core/utils/print/eigen.h +++ b/library/include/wavemap/core/utils/print/eigen.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_PRINT_EIGEN_H_ -#define WAVEMAP_UTILS_PRINT_EIGEN_H_ +#ifndef WAVEMAP_CORE_UTILS_PRINT_EIGEN_H_ +#define WAVEMAP_CORE_UTILS_PRINT_EIGEN_H_ #include @@ -12,4 +12,4 @@ static Eigen::WithFormat oneLine(const Derived& matrix) { } } // namespace wavemap::print::eigen -#endif // WAVEMAP_UTILS_PRINT_EIGEN_H_ +#endif // WAVEMAP_CORE_UTILS_PRINT_EIGEN_H_ diff --git a/library/include/wavemap/core/utils/profiler_interface.h b/library/include/wavemap/core/utils/profiler_interface.h new file mode 100644 index 000000000..ab47ee6fd --- /dev/null +++ b/library/include/wavemap/core/utils/profiler_interface.h @@ -0,0 +1,24 @@ +#ifndef WAVEMAP_CORE_UTILS_PROFILER_INTERFACE_H_ +#define WAVEMAP_CORE_UTILS_PROFILER_INTERFACE_H_ + +#ifdef TRACY_ENABLE + +#include + +#define ProfilerZoneScoped +#define ProfilerZoneScopedN(x) +#define ProfilerPlot(x, y) + +#define ProfilerSetThreadName(x) tracy::SetThreadName(x) + +#else + +#define ProfilerZoneScoped +#define ProfilerZoneScopedN(x) +#define ProfilerPlot(x, y) + +#define ProfilerSetThreadName(x) + +#endif + +#endif // WAVEMAP_CORE_UTILS_PROFILER_INTERFACE_H_ diff --git a/library/wavemap/core/utils/query/classified_map.h b/library/include/wavemap/core/utils/query/classified_map.h similarity index 97% rename from library/wavemap/core/utils/query/classified_map.h rename to library/include/wavemap/core/utils/query/classified_map.h index 394e1220a..28fd7c079 100644 --- a/library/wavemap/core/utils/query/classified_map.h +++ b/library/include/wavemap/core/utils/query/classified_map.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_QUERY_CLASSIFIED_MAP_H_ -#define WAVEMAP_UTILS_QUERY_CLASSIFIED_MAP_H_ +#ifndef WAVEMAP_CORE_UTILS_QUERY_CLASSIFIED_MAP_H_ +#define WAVEMAP_CORE_UTILS_QUERY_CLASSIFIED_MAP_H_ #include #include @@ -162,4 +162,4 @@ class ClassifiedMap { #include "wavemap/core/utils/query/impl/classified_map_inl.h" -#endif // WAVEMAP_UTILS_QUERY_CLASSIFIED_MAP_H_ +#endif // WAVEMAP_CORE_UTILS_QUERY_CLASSIFIED_MAP_H_ diff --git a/library/wavemap/core/utils/query/impl/classified_map_inl.h b/library/include/wavemap/core/utils/query/impl/classified_map_inl.h similarity index 95% rename from library/wavemap/core/utils/query/impl/classified_map_inl.h rename to library/include/wavemap/core/utils/query/impl/classified_map_inl.h index 8f61ca638..03729691d 100644 --- a/library/wavemap/core/utils/query/impl/classified_map_inl.h +++ b/library/include/wavemap/core/utils/query/impl/classified_map_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_QUERY_IMPL_CLASSIFIED_MAP_INL_H_ -#define WAVEMAP_UTILS_QUERY_IMPL_CLASSIFIED_MAP_INL_H_ +#ifndef WAVEMAP_CORE_UTILS_QUERY_IMPL_CLASSIFIED_MAP_INL_H_ +#define WAVEMAP_CORE_UTILS_QUERY_IMPL_CLASSIFIED_MAP_INL_H_ #include #include @@ -123,4 +123,4 @@ inline const ClassifiedMap::Block* ClassifiedMap::QueryCache::getBlock( } } // namespace wavemap -#endif // WAVEMAP_UTILS_QUERY_IMPL_CLASSIFIED_MAP_INL_H_ +#endif // WAVEMAP_CORE_UTILS_QUERY_IMPL_CLASSIFIED_MAP_INL_H_ diff --git a/library/wavemap/core/utils/query/impl/occupancy_classifier_inl.h b/library/include/wavemap/core/utils/query/impl/occupancy_classifier_inl.h similarity index 92% rename from library/wavemap/core/utils/query/impl/occupancy_classifier_inl.h rename to library/include/wavemap/core/utils/query/impl/occupancy_classifier_inl.h index 5c4cb27b3..085a4abc9 100644 --- a/library/wavemap/core/utils/query/impl/occupancy_classifier_inl.h +++ b/library/include/wavemap/core/utils/query/impl/occupancy_classifier_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_QUERY_IMPL_OCCUPANCY_CLASSIFIER_INL_H_ -#define WAVEMAP_UTILS_QUERY_IMPL_OCCUPANCY_CLASSIFIER_INL_H_ +#ifndef WAVEMAP_CORE_UTILS_QUERY_IMPL_OCCUPANCY_CLASSIFIER_INL_H_ +#define WAVEMAP_CORE_UTILS_QUERY_IMPL_OCCUPANCY_CLASSIFIER_INL_H_ namespace wavemap { inline constexpr Occupancy::Id OccupancyClassifier::classify( @@ -66,4 +66,4 @@ inline constexpr bool OccupancyClassifier::isObserved( } } // namespace wavemap -#endif // WAVEMAP_UTILS_QUERY_IMPL_OCCUPANCY_CLASSIFIER_INL_H_ +#endif // WAVEMAP_CORE_UTILS_QUERY_IMPL_OCCUPANCY_CLASSIFIER_INL_H_ diff --git a/library/wavemap/core/utils/query/impl/occupancy_inl.h b/library/include/wavemap/core/utils/query/impl/occupancy_inl.h similarity index 75% rename from library/wavemap/core/utils/query/impl/occupancy_inl.h rename to library/include/wavemap/core/utils/query/impl/occupancy_inl.h index a41904d2b..77c7c3d85 100644 --- a/library/wavemap/core/utils/query/impl/occupancy_inl.h +++ b/library/include/wavemap/core/utils/query/impl/occupancy_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_QUERY_IMPL_OCCUPANCY_INL_H_ -#define WAVEMAP_UTILS_QUERY_IMPL_OCCUPANCY_INL_H_ +#ifndef WAVEMAP_CORE_UTILS_QUERY_IMPL_OCCUPANCY_INL_H_ +#define WAVEMAP_CORE_UTILS_QUERY_IMPL_OCCUPANCY_INL_H_ namespace wavemap { constexpr Occupancy::Mask Occupancy::toMask(Occupancy::Id type_id) { @@ -19,4 +19,4 @@ constexpr Occupancy::Mask Occupancy::toMask(bool free, bool occupied, } } // namespace wavemap -#endif // WAVEMAP_UTILS_QUERY_IMPL_OCCUPANCY_INL_H_ +#endif // WAVEMAP_CORE_UTILS_QUERY_IMPL_OCCUPANCY_INL_H_ diff --git a/library/wavemap/core/utils/query/impl/point_sampler_inl.h b/library/include/wavemap/core/utils/query/impl/point_sampler_inl.h similarity index 85% rename from library/wavemap/core/utils/query/impl/point_sampler_inl.h rename to library/include/wavemap/core/utils/query/impl/point_sampler_inl.h index 4b5923bc1..b925d66fd 100644 --- a/library/wavemap/core/utils/query/impl/point_sampler_inl.h +++ b/library/include/wavemap/core/utils/query/impl/point_sampler_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_QUERY_IMPL_POINT_SAMPLER_INL_H_ -#define WAVEMAP_UTILS_QUERY_IMPL_POINT_SAMPLER_INL_H_ +#ifndef WAVEMAP_CORE_UTILS_QUERY_IMPL_POINT_SAMPLER_INL_H_ +#define WAVEMAP_CORE_UTILS_QUERY_IMPL_POINT_SAMPLER_INL_H_ namespace wavemap { inline std::optional PointSampler::getRandomPoint( @@ -28,4 +28,4 @@ inline Point3D PointSampler::getRandomPointInBlock(const Index3D& block_index) { } } // namespace wavemap -#endif // WAVEMAP_UTILS_QUERY_IMPL_POINT_SAMPLER_INL_H_ +#endif // WAVEMAP_CORE_UTILS_QUERY_IMPL_POINT_SAMPLER_INL_H_ diff --git a/library/wavemap/core/utils/query/impl/query_accelerator_inl.h b/library/include/wavemap/core/utils/query/impl/query_accelerator_inl.h similarity index 97% rename from library/wavemap/core/utils/query/impl/query_accelerator_inl.h rename to library/include/wavemap/core/utils/query/impl/query_accelerator_inl.h index b3c8e4b44..b04dc0c31 100644 --- a/library/wavemap/core/utils/query/impl/query_accelerator_inl.h +++ b/library/include/wavemap/core/utils/query/impl/query_accelerator_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_QUERY_IMPL_QUERY_ACCELERATOR_INL_H_ -#define WAVEMAP_UTILS_QUERY_IMPL_QUERY_ACCELERATOR_INL_H_ +#ifndef WAVEMAP_CORE_UTILS_QUERY_IMPL_QUERY_ACCELERATOR_INL_H_ +#define WAVEMAP_CORE_UTILS_QUERY_IMPL_QUERY_ACCELERATOR_INL_H_ #include #include @@ -188,4 +188,4 @@ QueryAccelerator>::getOrAllocateNode( } } // namespace wavemap -#endif // WAVEMAP_UTILS_QUERY_IMPL_QUERY_ACCELERATOR_INL_H_ +#endif // WAVEMAP_CORE_UTILS_QUERY_IMPL_QUERY_ACCELERATOR_INL_H_ diff --git a/library/wavemap/core/utils/query/map_interpolator.h b/library/include/wavemap/core/utils/query/map_interpolator.h similarity index 93% rename from library/wavemap/core/utils/query/map_interpolator.h rename to library/include/wavemap/core/utils/query/map_interpolator.h index 0f57271db..0b57860ae 100644 --- a/library/wavemap/core/utils/query/map_interpolator.h +++ b/library/include/wavemap/core/utils/query/map_interpolator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_QUERY_MAP_INTERPOLATOR_H_ -#define WAVEMAP_UTILS_QUERY_MAP_INTERPOLATOR_H_ +#ifndef WAVEMAP_CORE_UTILS_QUERY_MAP_INTERPOLATOR_H_ +#define WAVEMAP_CORE_UTILS_QUERY_MAP_INTERPOLATOR_H_ #include "wavemap/core/common.h" #include "wavemap/core/indexing/index_conversions.h" @@ -56,4 +56,4 @@ FloatingPoint trilinear(const wavemap::MapBase& map, } } // namespace wavemap::interpolate -#endif // WAVEMAP_UTILS_QUERY_MAP_INTERPOLATOR_H_ +#endif // WAVEMAP_CORE_UTILS_QUERY_MAP_INTERPOLATOR_H_ diff --git a/library/wavemap/core/utils/query/occupancy.h b/library/include/wavemap/core/utils/query/occupancy.h similarity index 84% rename from library/wavemap/core/utils/query/occupancy.h rename to library/include/wavemap/core/utils/query/occupancy.h index e65c7eff9..150432539 100644 --- a/library/wavemap/core/utils/query/occupancy.h +++ b/library/include/wavemap/core/utils/query/occupancy.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_QUERY_OCCUPANCY_H_ -#define WAVEMAP_UTILS_QUERY_OCCUPANCY_H_ +#ifndef WAVEMAP_CORE_UTILS_QUERY_OCCUPANCY_H_ +#define WAVEMAP_CORE_UTILS_QUERY_OCCUPANCY_H_ #include "wavemap/core/common.h" #include "wavemap/core/config/type_selector.h" @@ -22,4 +22,4 @@ struct Occupancy : TypeSelector { #include "wavemap/core/utils/query/impl/occupancy_inl.h" -#endif // WAVEMAP_UTILS_QUERY_OCCUPANCY_H_ +#endif // WAVEMAP_CORE_UTILS_QUERY_OCCUPANCY_H_ diff --git a/library/wavemap/core/utils/query/occupancy_classifier.h b/library/include/wavemap/core/utils/query/occupancy_classifier.h similarity index 91% rename from library/wavemap/core/utils/query/occupancy_classifier.h rename to library/include/wavemap/core/utils/query/occupancy_classifier.h index dfd4a24d4..177844ba3 100644 --- a/library/wavemap/core/utils/query/occupancy_classifier.h +++ b/library/include/wavemap/core/utils/query/occupancy_classifier.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_QUERY_OCCUPANCY_CLASSIFIER_H_ -#define WAVEMAP_UTILS_QUERY_OCCUPANCY_CLASSIFIER_H_ +#ifndef WAVEMAP_CORE_UTILS_QUERY_OCCUPANCY_CLASSIFIER_H_ +#define WAVEMAP_CORE_UTILS_QUERY_OCCUPANCY_CLASSIFIER_H_ #include "wavemap/core/utils/query/occupancy.h" @@ -47,4 +47,4 @@ class OccupancyClassifier { #include "wavemap/core/utils/query/impl/occupancy_classifier_inl.h" -#endif // WAVEMAP_UTILS_QUERY_OCCUPANCY_CLASSIFIER_H_ +#endif // WAVEMAP_CORE_UTILS_QUERY_OCCUPANCY_CLASSIFIER_H_ diff --git a/library/wavemap/core/utils/query/point_sampler.h b/library/include/wavemap/core/utils/query/point_sampler.h similarity index 91% rename from library/wavemap/core/utils/query/point_sampler.h rename to library/include/wavemap/core/utils/query/point_sampler.h index 09b29ff38..e02da0ed0 100644 --- a/library/wavemap/core/utils/query/point_sampler.h +++ b/library/include/wavemap/core/utils/query/point_sampler.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_QUERY_POINT_SAMPLER_H_ -#define WAVEMAP_UTILS_QUERY_POINT_SAMPLER_H_ +#ifndef WAVEMAP_CORE_UTILS_QUERY_POINT_SAMPLER_H_ +#define WAVEMAP_CORE_UTILS_QUERY_POINT_SAMPLER_H_ #include #include @@ -48,4 +48,4 @@ class PointSampler { #include "wavemap/core/utils/query/impl/point_sampler_inl.h" -#endif // WAVEMAP_UTILS_QUERY_POINT_SAMPLER_H_ +#endif // WAVEMAP_CORE_UTILS_QUERY_POINT_SAMPLER_H_ diff --git a/library/wavemap/core/utils/query/probability_conversions.h b/library/include/wavemap/core/utils/query/probability_conversions.h similarity index 79% rename from library/wavemap/core/utils/query/probability_conversions.h rename to library/include/wavemap/core/utils/query/probability_conversions.h index 2369ceec1..7dd995235 100644 --- a/library/wavemap/core/utils/query/probability_conversions.h +++ b/library/include/wavemap/core/utils/query/probability_conversions.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_QUERY_PROBABILITY_CONVERSIONS_H_ -#define WAVEMAP_UTILS_QUERY_PROBABILITY_CONVERSIONS_H_ +#ifndef WAVEMAP_CORE_UTILS_QUERY_PROBABILITY_CONVERSIONS_H_ +#define WAVEMAP_CORE_UTILS_QUERY_PROBABILITY_CONVERSIONS_H_ #include @@ -23,4 +23,4 @@ constexpr FloatingPoint probabilityToLogOdds(FloatingPoint probability) { } } // namespace wavemap::convert -#endif // WAVEMAP_UTILS_QUERY_PROBABILITY_CONVERSIONS_H_ +#endif // WAVEMAP_CORE_UTILS_QUERY_PROBABILITY_CONVERSIONS_H_ diff --git a/library/wavemap/core/utils/query/query_accelerator.h b/library/include/wavemap/core/utils/query/query_accelerator.h similarity index 96% rename from library/wavemap/core/utils/query/query_accelerator.h rename to library/include/wavemap/core/utils/query/query_accelerator.h index 99d6d5fab..8c4a17df6 100644 --- a/library/wavemap/core/utils/query/query_accelerator.h +++ b/library/include/wavemap/core/utils/query/query_accelerator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_QUERY_QUERY_ACCELERATOR_H_ -#define WAVEMAP_UTILS_QUERY_QUERY_ACCELERATOR_H_ +#ifndef WAVEMAP_CORE_UTILS_QUERY_QUERY_ACCELERATOR_H_ +#define WAVEMAP_CORE_UTILS_QUERY_QUERY_ACCELERATOR_H_ #include @@ -139,4 +139,4 @@ class QueryAccelerator { #include "wavemap/core/utils/query/impl/query_accelerator_inl.h" -#endif // WAVEMAP_UTILS_QUERY_QUERY_ACCELERATOR_H_ +#endif // WAVEMAP_CORE_UTILS_QUERY_QUERY_ACCELERATOR_H_ diff --git a/library/wavemap/core/utils/random_number_generator.h b/library/include/wavemap/core/utils/random_number_generator.h similarity index 88% rename from library/wavemap/core/utils/random_number_generator.h rename to library/include/wavemap/core/utils/random_number_generator.h index 7c7087346..d5d265fa0 100644 --- a/library/wavemap/core/utils/random_number_generator.h +++ b/library/include/wavemap/core/utils/random_number_generator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_RANDOM_NUMBER_GENERATOR_H_ -#define WAVEMAP_UTILS_RANDOM_NUMBER_GENERATOR_H_ +#ifndef WAVEMAP_CORE_UTILS_RANDOM_NUMBER_GENERATOR_H_ +#define WAVEMAP_CORE_UTILS_RANDOM_NUMBER_GENERATOR_H_ #include @@ -41,4 +41,4 @@ class RandomNumberGenerator { }; } // namespace wavemap -#endif // WAVEMAP_UTILS_RANDOM_NUMBER_GENERATOR_H_ +#endif // WAVEMAP_CORE_UTILS_RANDOM_NUMBER_GENERATOR_H_ diff --git a/library/wavemap/core/utils/sdf/full_euclidean_sdf_generator.h b/library/include/wavemap/core/utils/sdf/full_euclidean_sdf_generator.h similarity index 91% rename from library/wavemap/core/utils/sdf/full_euclidean_sdf_generator.h rename to library/include/wavemap/core/utils/sdf/full_euclidean_sdf_generator.h index d6c33b7d9..5943550a6 100644 --- a/library/wavemap/core/utils/sdf/full_euclidean_sdf_generator.h +++ b/library/include/wavemap/core/utils/sdf/full_euclidean_sdf_generator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_SDF_FULL_EUCLIDEAN_SDF_GENERATOR_H_ -#define WAVEMAP_UTILS_SDF_FULL_EUCLIDEAN_SDF_GENERATOR_H_ +#ifndef WAVEMAP_CORE_UTILS_SDF_FULL_EUCLIDEAN_SDF_GENERATOR_H_ +#define WAVEMAP_CORE_UTILS_SDF_FULL_EUCLIDEAN_SDF_GENERATOR_H_ #include "wavemap/core/common.h" #include "wavemap/core/data_structure/bucket_queue.h" @@ -54,4 +54,4 @@ class FullEuclideanSDFGenerator { }; } // namespace wavemap -#endif // WAVEMAP_UTILS_SDF_FULL_EUCLIDEAN_SDF_GENERATOR_H_ +#endif // WAVEMAP_CORE_UTILS_SDF_FULL_EUCLIDEAN_SDF_GENERATOR_H_ diff --git a/library/wavemap/core/utils/sdf/quasi_euclidean_sdf_generator.h b/library/include/wavemap/core/utils/sdf/quasi_euclidean_sdf_generator.h similarity index 87% rename from library/wavemap/core/utils/sdf/quasi_euclidean_sdf_generator.h rename to library/include/wavemap/core/utils/sdf/quasi_euclidean_sdf_generator.h index fd5c69110..0e7c86bb0 100644 --- a/library/wavemap/core/utils/sdf/quasi_euclidean_sdf_generator.h +++ b/library/include/wavemap/core/utils/sdf/quasi_euclidean_sdf_generator.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_SDF_QUASI_EUCLIDEAN_SDF_GENERATOR_H_ -#define WAVEMAP_UTILS_SDF_QUASI_EUCLIDEAN_SDF_GENERATOR_H_ +#ifndef WAVEMAP_CORE_UTILS_SDF_QUASI_EUCLIDEAN_SDF_GENERATOR_H_ +#define WAVEMAP_CORE_UTILS_SDF_QUASI_EUCLIDEAN_SDF_GENERATOR_H_ #include "wavemap/core/data_structure/bucket_queue.h" #include "wavemap/core/map/hashed_blocks.h" @@ -35,4 +35,4 @@ class QuasiEuclideanSDFGenerator { }; } // namespace wavemap -#endif // WAVEMAP_UTILS_SDF_QUASI_EUCLIDEAN_SDF_GENERATOR_H_ +#endif // WAVEMAP_CORE_UTILS_SDF_QUASI_EUCLIDEAN_SDF_GENERATOR_H_ diff --git a/library/wavemap/core/utils/thread_pool.h b/library/include/wavemap/core/utils/thread_pool.h similarity index 95% rename from library/wavemap/core/utils/thread_pool.h rename to library/include/wavemap/core/utils/thread_pool.h index b58ef10ba..9cbdfc057 100644 --- a/library/wavemap/core/utils/thread_pool.h +++ b/library/include/wavemap/core/utils/thread_pool.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_THREAD_POOL_H_ -#define WAVEMAP_UTILS_THREAD_POOL_H_ +#ifndef WAVEMAP_CORE_UTILS_THREAD_POOL_H_ +#define WAVEMAP_CORE_UTILS_THREAD_POOL_H_ #include #include @@ -100,4 +100,4 @@ std::future> ThreadPool::add_task( } } // namespace wavemap -#endif // WAVEMAP_UTILS_THREAD_POOL_H_ +#endif // WAVEMAP_CORE_UTILS_THREAD_POOL_H_ diff --git a/library/wavemap/core/utils/time/stopwatch.h b/library/include/wavemap/core/utils/time/stopwatch.h similarity index 78% rename from library/wavemap/core/utils/time/stopwatch.h rename to library/include/wavemap/core/utils/time/stopwatch.h index d2386dd4f..2021ceb46 100644 --- a/library/wavemap/core/utils/time/stopwatch.h +++ b/library/include/wavemap/core/utils/time/stopwatch.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_TIME_STOPWATCH_H_ -#define WAVEMAP_UTILS_TIME_STOPWATCH_H_ +#ifndef WAVEMAP_CORE_UTILS_TIME_STOPWATCH_H_ +#define WAVEMAP_CORE_UTILS_TIME_STOPWATCH_H_ #include "wavemap/core/utils/time/time.h" @@ -25,4 +25,4 @@ class Stopwatch { }; } // namespace wavemap -#endif // WAVEMAP_UTILS_TIME_STOPWATCH_H_ +#endif // WAVEMAP_CORE_UTILS_TIME_STOPWATCH_H_ diff --git a/library/wavemap/core/utils/time/time.h b/library/include/wavemap/core/utils/time/time.h similarity index 75% rename from library/wavemap/core/utils/time/time.h rename to library/include/wavemap/core/utils/time/time.h index 418c1a08b..000bc37c6 100644 --- a/library/wavemap/core/utils/time/time.h +++ b/library/include/wavemap/core/utils/time/time.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_UTILS_TIME_TIME_H_ -#define WAVEMAP_UTILS_TIME_TIME_H_ +#ifndef WAVEMAP_CORE_UTILS_TIME_TIME_H_ +#define WAVEMAP_CORE_UTILS_TIME_TIME_H_ #include @@ -16,4 +16,4 @@ T to_seconds(Duration duration) { } // namespace time } // namespace wavemap -#endif // WAVEMAP_UTILS_TIME_TIME_H_ +#endif // WAVEMAP_CORE_UTILS_TIME_TIME_H_ diff --git a/library/wavemap/io/file_conversions.h b/library/include/wavemap/io/file_conversions.h similarity index 100% rename from library/wavemap/io/file_conversions.h rename to library/include/wavemap/io/file_conversions.h diff --git a/library/wavemap/io/impl/streamable_types_impl.h b/library/include/wavemap/io/impl/streamable_types_impl.h similarity index 100% rename from library/wavemap/io/impl/streamable_types_impl.h rename to library/include/wavemap/io/impl/streamable_types_impl.h diff --git a/library/wavemap/io/stream_conversions.h b/library/include/wavemap/io/stream_conversions.h similarity index 100% rename from library/wavemap/io/stream_conversions.h rename to library/include/wavemap/io/stream_conversions.h diff --git a/library/wavemap/io/streamable_types.h b/library/include/wavemap/io/streamable_types.h similarity index 100% rename from library/wavemap/io/streamable_types.h rename to library/include/wavemap/io/streamable_types.h diff --git a/library/wavemap/core/CMakeLists.txt b/library/src/core/CMakeLists.txt similarity index 95% rename from library/wavemap/core/CMakeLists.txt rename to library/src/core/CMakeLists.txt index cea4a214b..b2dbd872a 100644 --- a/library/wavemap/core/CMakeLists.txt +++ b/library/src/core/CMakeLists.txt @@ -37,5 +37,8 @@ target_sources(core PRIVATE utils/time/stopwatch.cc utils/thread_pool.cc) target_include_directories(core PUBLIC ${glog_INCLUDE_DIRS}) -target_link_libraries(core PUBLIC Eigen3::Eigen ${glog_LIBRARIES} TracyClient minkindr) +target_link_libraries(core PUBLIC Eigen3::Eigen ${glog_LIBRARIES} minkindr) +if (tracy_FOUND) + target_link_libraries(core PUBLIC TracyClient) +endif () set_wavemap_target_properties(core) diff --git a/library/wavemap/core/config/value_with_unit.cc b/library/src/core/config/value_with_unit.cc similarity index 100% rename from library/wavemap/core/config/value_with_unit.cc rename to library/src/core/config/value_with_unit.cc diff --git a/library/wavemap/core/integrator/integrator_base.cc b/library/src/core/integrator/integrator_base.cc similarity index 100% rename from library/wavemap/core/integrator/integrator_base.cc rename to library/src/core/integrator/integrator_base.cc diff --git a/library/wavemap/core/integrator/integrator_factory.cc b/library/src/core/integrator/integrator_factory.cc similarity index 100% rename from library/wavemap/core/integrator/integrator_factory.cc rename to library/src/core/integrator/integrator_factory.cc diff --git a/library/wavemap/core/integrator/measurement_model/constant_ray.cc b/library/src/core/integrator/measurement_model/constant_ray.cc similarity index 100% rename from library/wavemap/core/integrator/measurement_model/constant_ray.cc rename to library/src/core/integrator/measurement_model/constant_ray.cc diff --git a/library/wavemap/core/integrator/measurement_model/continuous_beam.cc b/library/src/core/integrator/measurement_model/continuous_beam.cc similarity index 100% rename from library/wavemap/core/integrator/measurement_model/continuous_beam.cc rename to library/src/core/integrator/measurement_model/continuous_beam.cc diff --git a/library/wavemap/core/integrator/measurement_model/continuous_ray.cc b/library/src/core/integrator/measurement_model/continuous_ray.cc similarity index 100% rename from library/wavemap/core/integrator/measurement_model/continuous_ray.cc rename to library/src/core/integrator/measurement_model/continuous_ray.cc diff --git a/library/wavemap/core/integrator/measurement_model/measurement_model_factory.cc b/library/src/core/integrator/measurement_model/measurement_model_factory.cc similarity index 100% rename from library/wavemap/core/integrator/measurement_model/measurement_model_factory.cc rename to library/src/core/integrator/measurement_model/measurement_model_factory.cc diff --git a/library/wavemap/core/integrator/projection_model/circular_projector.cc b/library/src/core/integrator/projection_model/circular_projector.cc similarity index 100% rename from library/wavemap/core/integrator/projection_model/circular_projector.cc rename to library/src/core/integrator/projection_model/circular_projector.cc diff --git a/library/wavemap/core/integrator/projection_model/ouster_projector.cc b/library/src/core/integrator/projection_model/ouster_projector.cc similarity index 100% rename from library/wavemap/core/integrator/projection_model/ouster_projector.cc rename to library/src/core/integrator/projection_model/ouster_projector.cc diff --git a/library/wavemap/core/integrator/projection_model/pinhole_camera_projector.cc b/library/src/core/integrator/projection_model/pinhole_camera_projector.cc similarity index 100% rename from library/wavemap/core/integrator/projection_model/pinhole_camera_projector.cc rename to library/src/core/integrator/projection_model/pinhole_camera_projector.cc diff --git a/library/wavemap/core/integrator/projection_model/projector_factory.cc b/library/src/core/integrator/projection_model/projector_factory.cc similarity index 100% rename from library/wavemap/core/integrator/projection_model/projector_factory.cc rename to library/src/core/integrator/projection_model/projector_factory.cc diff --git a/library/wavemap/core/integrator/projection_model/spherical_projector.cc b/library/src/core/integrator/projection_model/spherical_projector.cc similarity index 100% rename from library/wavemap/core/integrator/projection_model/spherical_projector.cc rename to library/src/core/integrator/projection_model/spherical_projector.cc diff --git a/library/wavemap/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.cc b/library/src/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.cc similarity index 100% rename from library/wavemap/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.cc rename to library/src/core/integrator/projective/coarse_to_fine/coarse_to_fine_integrator.cc diff --git a/library/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc b/library/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc similarity index 97% rename from library/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc rename to library/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc index 1d638dc47..c84bdf8f5 100644 --- a/library/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc +++ b/library/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc @@ -2,14 +2,14 @@ #include -#include +#include namespace wavemap { void HashedChunkedWaveletIntegrator::updateMap() { - ZoneScoped; + ProfilerZoneScoped; // Update the range image intersector { - ZoneScopedN("updateRangeImageIntersector"); + ProfilerZoneScopedN("updateRangeImageIntersector"); range_image_intersector_ = std::make_shared( posed_range_image_, projection_model_, *measurement_model_, config_.min_range, config_.max_range); @@ -18,7 +18,7 @@ void HashedChunkedWaveletIntegrator::updateMap() { // Find all the indices of blocks that need updating BlockList blocks_to_update; { - ZoneScopedN("selectBlocksToUpdate"); + ProfilerZoneScopedN("selectBlocksToUpdate"); const auto [fov_min_idx, fov_max_idx] = getFovMinMaxIndices(posed_range_image_->getOrigin()); for (const auto& block_index : @@ -68,7 +68,7 @@ HashedChunkedWaveletIntegrator::getFovMinMaxIndices( void HashedChunkedWaveletIntegrator::updateBlock( HashedChunkedWaveletOctree::Block& block, const HashedChunkedWaveletOctree::BlockIndex& block_index) { - ZoneScoped; + ProfilerZoneScoped; block.setNeedsPruning(); block.setLastUpdatedStamp(); diff --git a/library/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc b/library/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc similarity index 97% rename from library/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc rename to library/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc index 8df7f71a6..167d6d7b4 100644 --- a/library/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc +++ b/library/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc @@ -2,14 +2,14 @@ #include -#include +#include namespace wavemap { void HashedWaveletIntegrator::updateMap() { - ZoneScoped; + ProfilerZoneScoped; // Update the range image intersector { - ZoneScopedN("updateRangeImageIntersector"); + ProfilerZoneScopedN("updateRangeImageIntersector"); range_image_intersector_ = std::make_shared( posed_range_image_, projection_model_, *measurement_model_, config_.min_range, config_.max_range); @@ -18,7 +18,7 @@ void HashedWaveletIntegrator::updateMap() { // Find all the indices of blocks that need updating BlockList blocks_to_update; { - ZoneScopedN("selectBlocksToUpdate"); + ProfilerZoneScopedN("selectBlocksToUpdate"); const auto [fov_min_idx, fov_max_idx] = getFovMinMaxIndices(posed_range_image_->getOrigin()); for (const auto& block_index : @@ -68,7 +68,7 @@ HashedWaveletIntegrator::getFovMinMaxIndices( void HashedWaveletIntegrator::updateBlock(HashedWaveletOctree::Block& block, const OctreeIndex& block_index) { - ZoneScoped; + ProfilerZoneScoped; HashedWaveletOctreeBlock::NodeType& root_node = block.getRootNode(); HashedWaveletOctreeBlock::Coefficients::Scale& root_node_scale = block.getRootScale(); diff --git a/library/wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.cc b/library/src/core/integrator/projective/coarse_to_fine/wavelet_integrator.cc similarity index 100% rename from library/wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.cc rename to library/src/core/integrator/projective/coarse_to_fine/wavelet_integrator.cc diff --git a/library/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc b/library/src/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc similarity index 100% rename from library/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc rename to library/src/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.cc diff --git a/library/wavemap/core/integrator/projective/projective_integrator.cc b/library/src/core/integrator/projective/projective_integrator.cc similarity index 95% rename from library/wavemap/core/integrator/projective/projective_integrator.cc rename to library/src/core/integrator/projective/projective_integrator.cc index 6165fea21..3f6178ef8 100644 --- a/library/wavemap/core/integrator/projective/projective_integrator.cc +++ b/library/src/core/integrator/projective/projective_integrator.cc @@ -1,6 +1,6 @@ #include "wavemap/core/integrator/projective/projective_integrator.h" -#include +#include namespace wavemap { DECLARE_CONFIG_MEMBERS(ProjectiveIntegratorConfig, @@ -23,7 +23,7 @@ bool ProjectiveIntegratorConfig::isValid(bool verbose) const { void ProjectiveIntegrator::integratePointcloud( const PosedPointcloud>& pointcloud) { - ZoneScoped; + ProfilerZoneScoped; if (!isPointcloudValid(pointcloud)) { return; } @@ -33,14 +33,14 @@ void ProjectiveIntegrator::integratePointcloud( void ProjectiveIntegrator::integrateRangeImage( const PosedImage<>& range_image) { - ZoneScoped; + ProfilerZoneScoped; importRangeImage(range_image); updateMap(); } void ProjectiveIntegrator::importPointcloud( const PosedPointcloud<>& pointcloud) { - ZoneScoped; + ProfilerZoneScoped; // Reset the posed range image and the beam offset image posed_range_image_->resetToInitialValue(); posed_range_image_->setPose(pointcloud.getPose()); @@ -79,7 +79,7 @@ void ProjectiveIntegrator::importPointcloud( void ProjectiveIntegrator::importRangeImage( const PosedImage<>& range_image_input) { - ZoneScoped; + ProfilerZoneScoped; *posed_range_image_ = range_image_input; beam_offset_image_->resetToInitialValue(); } diff --git a/library/wavemap/core/integrator/ray_tracing/ray_tracing_integrator.cc b/library/src/core/integrator/ray_tracing/ray_tracing_integrator.cc similarity index 100% rename from library/wavemap/core/integrator/ray_tracing/ray_tracing_integrator.cc rename to library/src/core/integrator/ray_tracing/ray_tracing_integrator.cc diff --git a/library/wavemap/core/map/hashed_blocks.cc b/library/src/core/map/hashed_blocks.cc similarity index 100% rename from library/wavemap/core/map/hashed_blocks.cc rename to library/src/core/map/hashed_blocks.cc diff --git a/library/wavemap/core/map/hashed_chunked_wavelet_octree.cc b/library/src/core/map/hashed_chunked_wavelet_octree.cc similarity index 94% rename from library/wavemap/core/map/hashed_chunked_wavelet_octree.cc rename to library/src/core/map/hashed_chunked_wavelet_octree.cc index eb85bb34c..6f33b017f 100644 --- a/library/wavemap/core/map/hashed_chunked_wavelet_octree.cc +++ b/library/src/core/map/hashed_chunked_wavelet_octree.cc @@ -2,7 +2,7 @@ #include -#include +#include namespace wavemap { DECLARE_CONFIG_MEMBERS(HashedChunkedWaveletOctreeConfig, @@ -24,14 +24,14 @@ bool HashedChunkedWaveletOctreeConfig::isValid(bool verbose) const { } void HashedChunkedWaveletOctree::threshold() { - ZoneScoped; + ProfilerZoneScoped; forEachBlock([](const BlockIndex& /*block_index*/, Block& block) { block.threshold(); }); } void HashedChunkedWaveletOctree::prune() { - ZoneScoped; + ProfilerZoneScoped; block_map_.eraseBlockIf([](const BlockIndex& /*block_index*/, Block& block) { block.prune(); return block.empty(); @@ -39,7 +39,7 @@ void HashedChunkedWaveletOctree::prune() { } void HashedChunkedWaveletOctree::pruneSmart() { - ZoneScoped; + ProfilerZoneScoped; block_map_.eraseBlockIf( [&config = config_](const BlockIndex& /*block_index*/, Block& block) { if (config.only_prune_blocks_if_unused_for < @@ -51,7 +51,7 @@ void HashedChunkedWaveletOctree::pruneSmart() { } size_t HashedChunkedWaveletOctree::getMemoryUsage() const { - ZoneScoped; + ProfilerZoneScoped; // TODO(victorr): Also include the memory usage of the unordered map itself size_t memory_usage = 0u; forEachBlock( diff --git a/library/wavemap/core/map/hashed_chunked_wavelet_octree_block.cc b/library/src/core/map/hashed_chunked_wavelet_octree_block.cc similarity index 99% rename from library/wavemap/core/map/hashed_chunked_wavelet_octree_block.cc rename to library/src/core/map/hashed_chunked_wavelet_octree_block.cc index 99c6d5237..329eba90a 100644 --- a/library/wavemap/core/map/hashed_chunked_wavelet_octree_block.cc +++ b/library/src/core/map/hashed_chunked_wavelet_octree_block.cc @@ -1,10 +1,10 @@ #include "wavemap/core/map/hashed_chunked_wavelet_octree_block.h" -#include +#include namespace wavemap { void HashedChunkedWaveletOctreeBlock::threshold() { - ZoneScoped; + ProfilerZoneScoped; if (getNeedsThresholding()) { root_scale_coefficient_ = recursiveThreshold(chunked_ndtree_.getRootChunk(), root_scale_coefficient_) @@ -14,7 +14,7 @@ void HashedChunkedWaveletOctreeBlock::threshold() { } void HashedChunkedWaveletOctreeBlock::prune() { - ZoneScoped; + ProfilerZoneScoped; if (getNeedsPruning()) { threshold(); recursivePrune(chunked_ndtree_.getRootChunk()); @@ -147,7 +147,7 @@ void HashedChunkedWaveletOctreeBlock::forEachLeaf( const BlockIndex& block_index, MapBase::IndexedLeafVisitorFunction visitor_fn, IndexElement termination_height) const { - ZoneScoped; + ProfilerZoneScoped; if (empty()) { return; } diff --git a/library/wavemap/core/map/hashed_wavelet_octree.cc b/library/src/core/map/hashed_wavelet_octree.cc similarity index 93% rename from library/wavemap/core/map/hashed_wavelet_octree.cc rename to library/src/core/map/hashed_wavelet_octree.cc index 4bcf0e2c8..7d9a9abf2 100644 --- a/library/wavemap/core/map/hashed_wavelet_octree.cc +++ b/library/src/core/map/hashed_wavelet_octree.cc @@ -2,7 +2,7 @@ #include -#include +#include namespace wavemap { DECLARE_CONFIG_MEMBERS(HashedWaveletOctreeConfig, @@ -23,14 +23,14 @@ bool HashedWaveletOctreeConfig::isValid(bool verbose) const { } void HashedWaveletOctree::threshold() { - ZoneScoped; + ProfilerZoneScoped; forEachBlock([](const BlockIndex& /*block_index*/, Block& block) { block.threshold(); }); } void HashedWaveletOctree::prune() { - ZoneScoped; + ProfilerZoneScoped; block_map_.eraseBlockIf([](const BlockIndex& /*block_index*/, Block& block) { block.prune(); return block.empty(); @@ -38,7 +38,7 @@ void HashedWaveletOctree::prune() { } void HashedWaveletOctree::pruneSmart() { - ZoneScoped; + ProfilerZoneScoped; block_map_.eraseBlockIf( [&config = config_](const BlockIndex& /*block_index*/, Block& block) { if (config.only_prune_blocks_if_unused_for < @@ -50,7 +50,7 @@ void HashedWaveletOctree::pruneSmart() { } size_t HashedWaveletOctree::getMemoryUsage() const { - ZoneScoped; + ProfilerZoneScoped; // TODO(victorr): Also include the memory usage of the unordered map itself size_t memory_usage = 0u; forEachBlock( diff --git a/library/wavemap/core/map/hashed_wavelet_octree_block.cc b/library/src/core/map/hashed_wavelet_octree_block.cc similarity index 98% rename from library/wavemap/core/map/hashed_wavelet_octree_block.cc rename to library/src/core/map/hashed_wavelet_octree_block.cc index fb7c6c0e7..92359f2c5 100644 --- a/library/wavemap/core/map/hashed_wavelet_octree_block.cc +++ b/library/src/core/map/hashed_wavelet_octree_block.cc @@ -1,10 +1,10 @@ #include "wavemap/core/map/hashed_wavelet_octree_block.h" -#include +#include namespace wavemap { void HashedWaveletOctreeBlock::threshold() { - ZoneScoped; + ProfilerZoneScoped; if (getNeedsThresholding()) { root_scale_coefficient_ -= recursiveThreshold(ndtree_.getRootNode(), root_scale_coefficient_); @@ -13,7 +13,7 @@ void HashedWaveletOctreeBlock::threshold() { } void HashedWaveletOctreeBlock::prune() { - ZoneScoped; + ProfilerZoneScoped; if (getNeedsPruning()) { threshold(); recursivePrune(ndtree_.getRootNode()); @@ -22,7 +22,7 @@ void HashedWaveletOctreeBlock::prune() { } void HashedWaveletOctreeBlock::clear() { - ZoneScoped; + ProfilerZoneScoped; root_scale_coefficient_ = Coefficients::Scale{}; ndtree_.clear(); last_updated_stamp_ = Time::now(); @@ -105,7 +105,7 @@ void HashedWaveletOctreeBlock::forEachLeaf( const BlockIndex& block_index, MapBase::IndexedLeafVisitorFunction visitor_fn, IndexElement termination_height) const { - ZoneScoped; + ProfilerZoneScoped; if (empty()) { return; } diff --git a/library/wavemap/core/map/map_base.cc b/library/src/core/map/map_base.cc similarity index 100% rename from library/wavemap/core/map/map_base.cc rename to library/src/core/map/map_base.cc diff --git a/library/wavemap/core/map/map_factory.cc b/library/src/core/map/map_factory.cc similarity index 100% rename from library/wavemap/core/map/map_factory.cc rename to library/src/core/map/map_factory.cc diff --git a/library/wavemap/core/map/volumetric_octree.cc b/library/src/core/map/volumetric_octree.cc similarity index 100% rename from library/wavemap/core/map/volumetric_octree.cc rename to library/src/core/map/volumetric_octree.cc diff --git a/library/wavemap/core/map/wavelet_octree.cc b/library/src/core/map/wavelet_octree.cc similarity index 100% rename from library/wavemap/core/map/wavelet_octree.cc rename to library/src/core/map/wavelet_octree.cc diff --git a/library/wavemap/core/utils/query/classified_map.cc b/library/src/core/utils/query/classified_map.cc similarity index 99% rename from library/wavemap/core/utils/query/classified_map.cc rename to library/src/core/utils/query/classified_map.cc index f95758c02..e3579c529 100644 --- a/library/wavemap/core/utils/query/classified_map.cc +++ b/library/src/core/utils/query/classified_map.cc @@ -1,6 +1,6 @@ #include "wavemap/core/utils/query/classified_map.h" -#include +#include namespace wavemap { ClassifiedMap::ClassifiedMap(FloatingPoint min_cell_width, @@ -38,7 +38,7 @@ Index3D ClassifiedMap::getMaxIndex() const { } void ClassifiedMap::update(const HashedWaveletOctree& occupancy_map) { - ZoneScoped; + ProfilerZoneScoped; // Reset the query cache query_cache_.reset(); @@ -61,7 +61,7 @@ void ClassifiedMap::update(const HashedWaveletOctree& occupancy_map) { void ClassifiedMap::update(const HashedWaveletOctree& occupancy_map, const HashedBlocks& esdf_map, FloatingPoint robot_radius) { - ZoneScoped; + ProfilerZoneScoped; // Reset the query cache query_cache_.reset(); diff --git a/library/wavemap/core/utils/query/point_sampler.cc b/library/src/core/utils/query/point_sampler.cc similarity index 100% rename from library/wavemap/core/utils/query/point_sampler.cc rename to library/src/core/utils/query/point_sampler.cc diff --git a/library/wavemap/core/utils/query/query_accelerator.cc b/library/src/core/utils/query/query_accelerator.cc similarity index 100% rename from library/wavemap/core/utils/query/query_accelerator.cc rename to library/src/core/utils/query/query_accelerator.cc diff --git a/library/wavemap/core/utils/sdf/full_euclidean_sdf_generator.cc b/library/src/core/utils/sdf/full_euclidean_sdf_generator.cc similarity index 97% rename from library/wavemap/core/utils/sdf/full_euclidean_sdf_generator.cc rename to library/src/core/utils/sdf/full_euclidean_sdf_generator.cc index b3cb99d0f..fe4747215 100644 --- a/library/wavemap/core/utils/sdf/full_euclidean_sdf_generator.cc +++ b/library/src/core/utils/sdf/full_euclidean_sdf_generator.cc @@ -1,6 +1,6 @@ #include "wavemap/core/utils/sdf/full_euclidean_sdf_generator.h" -#include +#include #include "wavemap/core/utils/iterate/grid_iterator.h" #include "wavemap/core/utils/query/query_accelerator.h" @@ -8,7 +8,7 @@ namespace wavemap { HashedBlocks FullEuclideanSDFGenerator::generate( const HashedWaveletOctree& occupancy_map) const { - ZoneScoped; + ProfilerZoneScoped; // Initialize the SDF data structure const FloatingPoint min_cell_width = occupancy_map.getMinCellWidth(); const Index3D uninitialized_parent = @@ -39,7 +39,7 @@ HashedBlocks FullEuclideanSDFGenerator::generate( void FullEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, VectorDistanceField& sdf, BucketQueue& open_queue) const { - ZoneScoped; + ProfilerZoneScoped; // Create an occupancy query accelerator QueryAccelerator occupancy_query_accelerator{occupancy_map}; @@ -105,7 +105,7 @@ void FullEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, void FullEuclideanSDFGenerator::propagate( const HashedWaveletOctree& occupancy_map, VectorDistanceField& sdf, BucketQueue& open_queue) const { - ZoneScoped; + ProfilerZoneScoped; // Create an occupancy query accelerator QueryAccelerator occupancy_query_accelerator{occupancy_map}; @@ -116,11 +116,11 @@ void FullEuclideanSDFGenerator::propagate( // Propagate the distance while (!open_queue.empty()) { - TracyPlot("QueueLength", static_cast(open_queue.size())); + ProfilerPlot("QueueLength", static_cast(open_queue.size())); const Index3D index = open_queue.front(); const auto& [sdf_parent, sdf_value] = sdf.getValueOrDefault(index); const FloatingPoint df_value = std::abs(sdf_value); - TracyPlot("Distance", df_value); + ProfilerPlot("Distance", df_value); open_queue.pop(); for (const Index3D& index_offset : kNeighborIndexOffsets) { diff --git a/library/wavemap/core/utils/sdf/quasi_euclidean_sdf_generator.cc b/library/src/core/utils/sdf/quasi_euclidean_sdf_generator.cc similarity index 96% rename from library/wavemap/core/utils/sdf/quasi_euclidean_sdf_generator.cc rename to library/src/core/utils/sdf/quasi_euclidean_sdf_generator.cc index 5414b929c..30b1bbe8c 100644 --- a/library/wavemap/core/utils/sdf/quasi_euclidean_sdf_generator.cc +++ b/library/src/core/utils/sdf/quasi_euclidean_sdf_generator.cc @@ -1,6 +1,6 @@ #include "wavemap/core/utils/sdf/quasi_euclidean_sdf_generator.h" -#include +#include #include "wavemap/core/utils/iterate/grid_iterator.h" #include "wavemap/core/utils/query/query_accelerator.h" @@ -8,7 +8,7 @@ namespace wavemap { HashedBlocks QuasiEuclideanSDFGenerator::generate( const HashedWaveletOctree& occupancy_map) const { - ZoneScoped; + ProfilerZoneScoped; // Initialize the SDF data structure const FloatingPoint min_cell_width = occupancy_map.getMinCellWidth(); const MapBaseConfig config{min_cell_width, 0.f, max_distance_}; @@ -29,7 +29,7 @@ HashedBlocks QuasiEuclideanSDFGenerator::generate( void QuasiEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, HashedBlocks& sdf, BucketQueue& open_queue) const { - ZoneScoped; + ProfilerZoneScoped; // Create an occupancy query accelerator QueryAccelerator occupancy_query_accelerator{occupancy_map}; @@ -93,7 +93,7 @@ void QuasiEuclideanSDFGenerator::seed(const HashedWaveletOctree& occupancy_map, void QuasiEuclideanSDFGenerator::propagate( const HashedWaveletOctree& occupancy_map, HashedBlocks& sdf, BucketQueue& open_queue) const { - ZoneScoped; + ProfilerZoneScoped; // Create an occupancy query accelerator QueryAccelerator occupancy_query_accelerator{occupancy_map}; @@ -107,11 +107,11 @@ void QuasiEuclideanSDFGenerator::propagate( // Propagate the distance while (!open_queue.empty()) { - TracyPlot("QueueLength", static_cast(open_queue.size())); + ProfilerPlot("QueueLength", static_cast(open_queue.size())); const Index3D index = open_queue.front(); const FloatingPoint sdf_value = sdf.getCellValue(index); const FloatingPoint df_value = std::abs(sdf_value); - TracyPlot("Distance", df_value); + ProfilerPlot("Distance", df_value); open_queue.pop(); for (size_t neighbor_idx = 0; neighbor_idx < kNeighborIndexOffsets.size(); diff --git a/library/wavemap/core/utils/thread_pool.cc b/library/src/core/utils/thread_pool.cc similarity index 94% rename from library/wavemap/core/utils/thread_pool.cc rename to library/src/core/utils/thread_pool.cc index 09c6276ce..6d213c1f8 100644 --- a/library/wavemap/core/utils/thread_pool.cc +++ b/library/src/core/utils/thread_pool.cc @@ -1,6 +1,6 @@ #include "wavemap/core/utils/thread_pool.h" -#include +#include namespace wavemap { ThreadPool::ThreadPool(size_t thread_count) @@ -11,7 +11,7 @@ ThreadPool::ThreadPool(size_t thread_count) const std::string thread_name = "pool_" + std::to_string(pool_id) + "_worker_" + std::to_string(i); workers_.emplace_back([this, thread_name] { - tracy::SetThreadName(thread_name.c_str()); + ProfilerSetThreadName(thread_name.c_str()); worker_loop(); }); } diff --git a/library/wavemap/core/utils/time/stopwatch.cc b/library/src/core/utils/time/stopwatch.cc similarity index 100% rename from library/wavemap/core/utils/time/stopwatch.cc rename to library/src/core/utils/time/stopwatch.cc diff --git a/library/wavemap/io/CMakeLists.txt b/library/src/io/CMakeLists.txt similarity index 100% rename from library/wavemap/io/CMakeLists.txt rename to library/src/io/CMakeLists.txt diff --git a/library/wavemap/io/file_conversions.cc b/library/src/io/file_conversions.cc similarity index 100% rename from library/wavemap/io/file_conversions.cc rename to library/src/io/file_conversions.cc diff --git a/library/wavemap/io/stream_conversions.cc b/library/src/io/stream_conversions.cc similarity index 100% rename from library/wavemap/io/stream_conversions.cc rename to library/src/io/stream_conversions.cc diff --git a/library/wavemap/pipeline/CMakeLists.txt b/library/src/pipeline/CMakeLists.txt similarity index 100% rename from library/wavemap/pipeline/CMakeLists.txt rename to library/src/pipeline/CMakeLists.txt diff --git a/library/wavemap/test/CMakeLists.txt b/library/test/CMakeLists.txt similarity index 100% rename from library/wavemap/test/CMakeLists.txt rename to library/test/CMakeLists.txt diff --git a/library/wavemap/test/core/data_structure/test_aabb.cc b/library/test/core/data_structure/test_aabb.cc similarity index 100% rename from library/wavemap/test/core/data_structure/test_aabb.cc rename to library/test/core/data_structure/test_aabb.cc diff --git a/library/wavemap/test/core/data_structure/test_image.cc b/library/test/core/data_structure/test_image.cc similarity index 100% rename from library/wavemap/test/core/data_structure/test_image.cc rename to library/test/core/data_structure/test_image.cc diff --git a/library/wavemap/test/core/data_structure/test_ndtree.cc b/library/test/core/data_structure/test_ndtree.cc similarity index 100% rename from library/wavemap/test/core/data_structure/test_ndtree.cc rename to library/test/core/data_structure/test_ndtree.cc diff --git a/library/wavemap/test/core/data_structure/test_pointcloud.cc b/library/test/core/data_structure/test_pointcloud.cc similarity index 100% rename from library/wavemap/test/core/data_structure/test_pointcloud.cc rename to library/test/core/data_structure/test_pointcloud.cc diff --git a/library/wavemap/test/core/data_structure/test_sparse_vector.cc b/library/test/core/data_structure/test_sparse_vector.cc similarity index 100% rename from library/wavemap/test/core/data_structure/test_sparse_vector.cc rename to library/test/core/data_structure/test_sparse_vector.cc diff --git a/library/wavemap/test/core/indexing/test_index_conversions.cc b/library/test/core/indexing/test_index_conversions.cc similarity index 100% rename from library/wavemap/test/core/indexing/test_index_conversions.cc rename to library/test/core/indexing/test_index_conversions.cc diff --git a/library/wavemap/test/core/indexing/test_ndtree_index.cc b/library/test/core/indexing/test_ndtree_index.cc similarity index 100% rename from library/wavemap/test/core/indexing/test_ndtree_index.cc rename to library/test/core/indexing/test_ndtree_index.cc diff --git a/library/wavemap/test/core/integrator/projection_model/test_circular_projector.cc b/library/test/core/integrator/projection_model/test_circular_projector.cc similarity index 100% rename from library/wavemap/test/core/integrator/projection_model/test_circular_projector.cc rename to library/test/core/integrator/projection_model/test_circular_projector.cc diff --git a/library/wavemap/test/core/integrator/projection_model/test_image_projectors.cc b/library/test/core/integrator/projection_model/test_image_projectors.cc similarity index 100% rename from library/wavemap/test/core/integrator/projection_model/test_image_projectors.cc rename to library/test/core/integrator/projection_model/test_image_projectors.cc diff --git a/library/wavemap/test/core/integrator/projection_model/test_spherical_projector.cc b/library/test/core/integrator/projection_model/test_spherical_projector.cc similarity index 100% rename from library/wavemap/test/core/integrator/projection_model/test_spherical_projector.cc rename to library/test/core/integrator/projection_model/test_spherical_projector.cc diff --git a/library/wavemap/test/core/integrator/test_hierarchical_range_image.cc b/library/test/core/integrator/test_hierarchical_range_image.cc similarity index 100% rename from library/wavemap/test/core/integrator/test_hierarchical_range_image.cc rename to library/test/core/integrator/test_hierarchical_range_image.cc diff --git a/library/wavemap/test/core/integrator/test_measurement_models.cc b/library/test/core/integrator/test_measurement_models.cc similarity index 100% rename from library/wavemap/test/core/integrator/test_measurement_models.cc rename to library/test/core/integrator/test_measurement_models.cc diff --git a/library/wavemap/test/core/integrator/test_pointcloud_integrators.cc b/library/test/core/integrator/test_pointcloud_integrators.cc similarity index 100% rename from library/wavemap/test/core/integrator/test_pointcloud_integrators.cc rename to library/test/core/integrator/test_pointcloud_integrators.cc diff --git a/library/wavemap/test/core/integrator/test_range_image_intersector.cc b/library/test/core/integrator/test_range_image_intersector.cc similarity index 100% rename from library/wavemap/test/core/integrator/test_range_image_intersector.cc rename to library/test/core/integrator/test_range_image_intersector.cc diff --git a/library/wavemap/test/core/map/test_haar_cell.cc b/library/test/core/map/test_haar_cell.cc similarity index 100% rename from library/wavemap/test/core/map/test_haar_cell.cc rename to library/test/core/map/test_haar_cell.cc diff --git a/library/wavemap/test/core/map/test_hashed_blocks.cc b/library/test/core/map/test_hashed_blocks.cc similarity index 100% rename from library/wavemap/test/core/map/test_hashed_blocks.cc rename to library/test/core/map/test_hashed_blocks.cc diff --git a/library/wavemap/test/core/map/test_map.cc b/library/test/core/map/test_map.cc similarity index 100% rename from library/wavemap/test/core/map/test_map.cc rename to library/test/core/map/test_map.cc diff --git a/library/wavemap/test/core/map/test_volumetric_octree.cc b/library/test/core/map/test_volumetric_octree.cc similarity index 100% rename from library/wavemap/test/core/map/test_volumetric_octree.cc rename to library/test/core/map/test_volumetric_octree.cc diff --git a/library/wavemap/test/core/utils/bits/test_bit_operations.cc b/library/test/core/utils/bits/test_bit_operations.cc similarity index 100% rename from library/wavemap/test/core/utils/bits/test_bit_operations.cc rename to library/test/core/utils/bits/test_bit_operations.cc diff --git a/library/wavemap/test/core/utils/data/test_comparisons.cc b/library/test/core/utils/data/test_comparisons.cc similarity index 100% rename from library/wavemap/test/core/utils/data/test_comparisons.cc rename to library/test/core/utils/data/test_comparisons.cc diff --git a/library/wavemap/test/core/utils/data/test_fill.cc b/library/test/core/utils/data/test_fill.cc similarity index 100% rename from library/wavemap/test/core/utils/data/test_fill.cc rename to library/test/core/utils/data/test_fill.cc diff --git a/library/wavemap/test/core/utils/iterate/test_grid_iterator.cc b/library/test/core/utils/iterate/test_grid_iterator.cc similarity index 100% rename from library/wavemap/test/core/utils/iterate/test_grid_iterator.cc rename to library/test/core/utils/iterate/test_grid_iterator.cc diff --git a/library/wavemap/test/core/utils/iterate/test_ray_iterator.cc b/library/test/core/utils/iterate/test_ray_iterator.cc similarity index 100% rename from library/wavemap/test/core/utils/iterate/test_ray_iterator.cc rename to library/test/core/utils/iterate/test_ray_iterator.cc diff --git a/library/wavemap/test/core/utils/iterate/test_subtree_iterator.cc b/library/test/core/utils/iterate/test_subtree_iterator.cc similarity index 100% rename from library/wavemap/test/core/utils/iterate/test_subtree_iterator.cc rename to library/test/core/utils/iterate/test_subtree_iterator.cc diff --git a/library/wavemap/test/core/utils/math/test_approximate_trigonometry.cc b/library/test/core/utils/math/test_approximate_trigonometry.cc similarity index 100% rename from library/wavemap/test/core/utils/math/test_approximate_trigonometry.cc rename to library/test/core/utils/math/test_approximate_trigonometry.cc diff --git a/library/wavemap/test/core/utils/math/test_int_math.cc b/library/test/core/utils/math/test_int_math.cc similarity index 100% rename from library/wavemap/test/core/utils/math/test_int_math.cc rename to library/test/core/utils/math/test_int_math.cc diff --git a/library/wavemap/test/core/utils/math/test_tree_math.cc b/library/test/core/utils/math/test_tree_math.cc similarity index 100% rename from library/wavemap/test/core/utils/math/test_tree_math.cc rename to library/test/core/utils/math/test_tree_math.cc diff --git a/library/wavemap/test/core/utils/neighbors/test_adjacency.cc b/library/test/core/utils/neighbors/test_adjacency.cc similarity index 100% rename from library/wavemap/test/core/utils/neighbors/test_adjacency.cc rename to library/test/core/utils/neighbors/test_adjacency.cc diff --git a/library/wavemap/test/core/utils/neighbors/test_grid_adjacency.cc b/library/test/core/utils/neighbors/test_grid_adjacency.cc similarity index 100% rename from library/wavemap/test/core/utils/neighbors/test_grid_adjacency.cc rename to library/test/core/utils/neighbors/test_grid_adjacency.cc diff --git a/library/wavemap/test/core/utils/neighbors/test_grid_neighborhood.cc b/library/test/core/utils/neighbors/test_grid_neighborhood.cc similarity index 100% rename from library/wavemap/test/core/utils/neighbors/test_grid_neighborhood.cc rename to library/test/core/utils/neighbors/test_grid_neighborhood.cc diff --git a/library/wavemap/test/core/utils/neighbors/test_ndtree_adjacency.cc b/library/test/core/utils/neighbors/test_ndtree_adjacency.cc similarity index 100% rename from library/wavemap/test/core/utils/neighbors/test_ndtree_adjacency.cc rename to library/test/core/utils/neighbors/test_ndtree_adjacency.cc diff --git a/library/wavemap/test/core/utils/query/test_classified_map.cc b/library/test/core/utils/query/test_classified_map.cc similarity index 100% rename from library/wavemap/test/core/utils/query/test_classified_map.cc rename to library/test/core/utils/query/test_classified_map.cc diff --git a/library/wavemap/test/core/utils/query/test_map_interpolator.cpp b/library/test/core/utils/query/test_map_interpolator.cpp similarity index 100% rename from library/wavemap/test/core/utils/query/test_map_interpolator.cpp rename to library/test/core/utils/query/test_map_interpolator.cpp diff --git a/library/wavemap/test/core/utils/query/test_occupancy_classifier.cc b/library/test/core/utils/query/test_occupancy_classifier.cc similarity index 100% rename from library/wavemap/test/core/utils/query/test_occupancy_classifier.cc rename to library/test/core/utils/query/test_occupancy_classifier.cc diff --git a/library/wavemap/test/core/utils/query/test_probability_conversions.cc b/library/test/core/utils/query/test_probability_conversions.cc similarity index 100% rename from library/wavemap/test/core/utils/query/test_probability_conversions.cc rename to library/test/core/utils/query/test_probability_conversions.cc diff --git a/library/wavemap/test/core/utils/query/test_query_accelerator.cc b/library/test/core/utils/query/test_query_accelerator.cc similarity index 100% rename from library/wavemap/test/core/utils/query/test_query_accelerator.cc rename to library/test/core/utils/query/test_query_accelerator.cc diff --git a/library/wavemap/test/core/utils/sdf/test_sdf_generators.cc b/library/test/core/utils/sdf/test_sdf_generators.cc similarity index 100% rename from library/wavemap/test/core/utils/sdf/test_sdf_generators.cc rename to library/test/core/utils/sdf/test_sdf_generators.cc diff --git a/library/wavemap/test/core/utils/test_thread_pool.cc b/library/test/core/utils/test_thread_pool.cc similarity index 100% rename from library/wavemap/test/core/utils/test_thread_pool.cc rename to library/test/core/utils/test_thread_pool.cc diff --git a/ros/wavemap_rviz_plugin/include/CPPLINT.cfg b/library/test/include/CPPLINT.cfg similarity index 100% rename from ros/wavemap_rviz_plugin/include/CPPLINT.cfg rename to library/test/include/CPPLINT.cfg diff --git a/library/wavemap/test/config_generator.h b/library/test/include/wavemap/test/config_generator.h similarity index 100% rename from library/wavemap/test/config_generator.h rename to library/test/include/wavemap/test/config_generator.h diff --git a/library/wavemap/test/eigen_utils.h b/library/test/include/wavemap/test/eigen_utils.h similarity index 100% rename from library/wavemap/test/eigen_utils.h rename to library/test/include/wavemap/test/eigen_utils.h diff --git a/library/wavemap/test/fixture_base.h b/library/test/include/wavemap/test/fixture_base.h similarity index 100% rename from library/wavemap/test/fixture_base.h rename to library/test/include/wavemap/test/fixture_base.h diff --git a/library/wavemap/test/geometry_generator.h b/library/test/include/wavemap/test/geometry_generator.h similarity index 100% rename from library/wavemap/test/geometry_generator.h rename to library/test/include/wavemap/test/geometry_generator.h diff --git a/library/wavemap/test/io/test_file_conversions.cc b/library/test/io/test_file_conversions.cc similarity index 100% rename from library/wavemap/test/io/test_file_conversions.cc rename to library/test/io/test_file_conversions.cc diff --git a/library/wavemap/CMakeLists.txt b/library/wavemap/CMakeLists.txt deleted file mode 100644 index 0b02e12cf..000000000 --- a/library/wavemap/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -cmake_minimum_required(VERSION 3.10) - -project(wavemap VERSION 1.6.3 LANGUAGES CXX) - -# Dependencies -find_package(Eigen3 REQUIRED NO_MODULE) -find_package(PkgConfig REQUIRED) -pkg_check_modules(glog REQUIRED libglog) - -# TODO(victorr): Make sure these deps are also available to downstream packages. -# This might require using an alternative to FetchContent. -include(FetchContent) -FetchContent_Declare(tracy - GIT_REPOSITORY https://github.com/wolfpld/tracy.git - GIT_TAG v0.10 - GIT_SHALLOW TRUE - GIT_PROGRESS TRUE -) -FetchContent_MakeAvailable(tracy) -FetchContent_Declare(minkindr - GIT_REPOSITORY https://github.com/ethz-asl/minkindr.git - GIT_TAG master - GIT_SHALLOW TRUE - GIT_PROGRESS TRUE -) -FetchContent_MakeAvailable(minkindr) -# Add minkindr as header-only library -add_library(minkindr INTERFACE) -target_include_directories(minkindr INTERFACE ${minkindr_SOURCE_DIR}/minkindr/include) - -include(cmake/compiler_options.cmake) - -# Libraries -add_subdirectory(core) -add_subdirectory(io) -add_subdirectory(pipeline) - -## Tests -#if (CATKIN_ENABLE_TESTING) -# add_subdirectory(test) -#endif () -# -## Benchmarks -#if (ENABLE_BENCHMARKING) -# add_subdirectory(benchmark) -#endif () diff --git a/library/wavemap/cmake/compiler_options.cmake b/library/wavemap/cmake/compiler_options.cmake deleted file mode 100644 index cb61f33d6..000000000 --- a/library/wavemap/cmake/compiler_options.cmake +++ /dev/null @@ -1,31 +0,0 @@ -function(set_wavemap_target_properties target) - target_compile_features(${target} PUBLIC cxx_std_17) - target_compile_definitions(${target} PUBLIC $<$:_USE_MATH_DEFINES>) -# target_compile_options( -# ${target} -# PRIVATE # MSVC -# $<$:/W4> -# $<$:/WX> -# # Clang/AppleClang -# $<$:-fcolor-diagnostics> -# $<$:-Werror> -# $<$:-Wall> -# $<$:-Wextra> -# $<$:-Wconversion> -# $<$:-Wno-sign-conversion> -# # GNU -# $<$:-fdiagnostics-color=always> -# $<$:-Werror> -# $<$:-Wall> -# $<$:-Wextra> -# $<$:-pedantic> -# $<$:-Wcast-align> -# $<$:-Wcast-qual> -# $<$:-Wconversion> -# $<$:-Wdisabled-optimization> -# $<$:-Woverloaded-virtual>) - set(INCLUDE_DIRS ${PROJECT_SOURCE_DIR}) - get_filename_component(INCLUDE_DIRS ${INCLUDE_DIRS} PATH) - target_include_directories(${target} - PUBLIC $ $) -endfunction() diff --git a/library/wavemap/core/utils/neighbors/impl/ndtree_adjacency_inl.h b/library/wavemap/core/utils/neighbors/impl/ndtree_adjacency_inl.h deleted file mode 100644 index d92274702..000000000 --- a/library/wavemap/core/utils/neighbors/impl/ndtree_adjacency_inl.h +++ /dev/null @@ -1,6 +0,0 @@ -#ifndef WAVEMAP_UTILS_NEIGHBORS_IMPL_NDTREE_ADJACENCY_INL_H_ -#define WAVEMAP_UTILS_NEIGHBORS_IMPL_NDTREE_ADJACENCY_INL_H_ - -namespace wavemap {} - -#endif // WAVEMAP_UTILS_NEIGHBORS_IMPL_NDTREE_ADJACENCY_INL_H_ From 2c23eb658ca3b88bd83af7d3a2489df34752d01d Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 6 May 2024 18:40:59 +0200 Subject: [PATCH 093/159] Make ROS1 interfaces compatible with CMake-only library --- CMakeLists.txt | 15 +-- examples/CMakeLists.txt | 23 ++++- examples/src/planning/occupancy_to_esdf.cc | 8 +- examples/src/queries/accelerated_queries.cc | 6 +- examples/src/queries/classification.cc | 2 +- examples/src/queries/fixed_resolution.cc | 2 +- examples/src/queries/multi_resolution.cc | 4 +- .../queries/nearest_neighbor_interpolation.cc | 4 +- .../src/queries/trilinear_interpolation.cc | 4 +- interfaces/ros1/wavemap/CMakeLists.txt | 27 ++++++ interfaces/ros1/wavemap/package.xml | 4 - interfaces/ros1/wavemap_ros/CMakeLists.txt | 12 +-- .../wavemap_ros/inputs/depth_image_input.h | 5 +- .../include/wavemap_ros/inputs/input_base.h | 14 +-- .../wavemap_ros/inputs/input_factory.h | 2 +- .../operations/crop_map_operation.h | 4 +- .../impl/publish_map_operation_inl.h | 6 +- .../wavemap_ros/operations/operation_base.h | 2 +- .../operations/operation_factory.h | 4 +- .../operations/prune_map_operation.h | 4 +- .../operations/publish_map_operation.h | 8 +- .../operations/publish_pointcloud_operation.h | 6 +- .../operations/threshold_map_operation.h | 4 +- .../include/wavemap_ros/ros_server.h | 12 +-- .../include/wavemap_ros/utils/logging_level.h | 2 +- .../pointcloud_undistorter.h | 4 +- .../stamped_pointcloud.h | 2 +- .../wavemap_ros/utils/tf_transformer.h | 2 +- .../src/inputs/depth_image_input.cc | 12 +-- .../ros1/wavemap_ros/src/inputs/input_base.cc | 10 +- .../src/inputs/pointcloud_input.cc | 12 +-- .../src/operations/crop_map_operation.cc | 6 +- .../src/operations/publish_map_operation.cc | 4 +- .../publish_pointcloud_operation.cc | 12 +-- interfaces/ros1/wavemap_ros/src/ros_server.cc | 3 +- .../pointcloud_undistorter.cc | 4 +- .../wavemap_ros/src/utils/rosbag_processor.cc | 10 +- .../wavemap_ros_conversions/CMakeLists.txt | 5 +- .../config_conversions.h | 2 +- .../map_msg_conversions.h | 12 +-- .../src/map_msg_conversions.cc | 22 ++--- .../ros1/wavemap_rviz_plugin/CMakeLists.txt | 5 +- .../include/wavemap_rviz_plugin/common.h | 2 +- .../utils/color_conversions.h | 2 +- .../visuals/cell_selector.h | 6 +- .../visuals/slice_visual.h | 2 +- .../visuals/voxel_visual.h | 8 +- .../src/visuals/cell_selector.cc | 2 +- .../src/visuals/slice_visual.cc | 2 +- .../src/visuals/voxel_visual.cc | 32 +++---- .../src/wavemap_map_display.cc | 28 +++--- library/CMakeLists.txt | 12 ++- library/cmake/compiler_options.cmake | 91 +++++++++++++------ library/cmake/wavemap-extras.cmake | 67 -------------- .../wavemap/core/utils/profiler_interface.h | 14 ++- library/src/core/CMakeLists.txt | 3 +- library/src/io/CMakeLists.txt | 3 +- 57 files changed, 307 insertions(+), 278 deletions(-) create mode 100644 interfaces/ros1/wavemap/CMakeLists.txt delete mode 100644 library/cmake/wavemap-extras.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 55dc1dec3..124663679 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,14 +9,15 @@ execute_process(COMMAND catkin locate --devel OUTPUT_STRIP_TRAILING_WHITESPACE OUTPUT_VARIABLE CATKIN_WS_DEVEL_PATH) include_directories(${CATKIN_WS_DEVEL_PATH}/include) -# Libraries -add_subdirectory(libraries/wavemap) +## Libraries +#add_subdirectory(library) # ROS interfaces and tooling -add_subdirectory(ros/wavemap_msgs) -add_subdirectory(ros/wavemap_ros_conversions) -add_subdirectory(ros/wavemap_ros) -add_subdirectory(ros/wavemap_rviz_plugin) +add_subdirectory(interfaces/ros1/wavemap) +add_subdirectory(interfaces/ros1/wavemap_msgs) +add_subdirectory(interfaces/ros1/wavemap_ros_conversions) +add_subdirectory(interfaces/ros1/wavemap_ros) +add_subdirectory(interfaces/ros1/wavemap_rviz_plugin) -# Usage examples +## Usage examples add_subdirectory(examples) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index e54cf7eea..819700067 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -5,7 +5,6 @@ find_package(catkin_simple REQUIRED) catkin_simple(ALL_DEPS_REQUIRED) # Compiler definitions and options -add_wavemap_compile_definitions_and_options() add_compile_options(-Wno-suggest-attribute=const) # For all targets @@ -14,27 +13,49 @@ include_directories(include) # Binaries cs_add_executable(save_map_to_file src/io/save_map_to_file.cc) +set_wavemap_target_properties(save_map_to_file) +target_link_libraries(save_map_to_file glog) cs_add_executable(load_map_from_file src/io/load_map_from_file.cc) +set_wavemap_target_properties(load_map_from_file) +target_link_libraries(load_map_from_file glog) cs_add_executable(receive_map_over_ros src/io/receive_map_over_ros.cc) +set_wavemap_target_properties(receive_map_over_ros) +target_link_libraries(receive_map_over_ros glog) cs_add_executable(send_map_over_ros src/io/send_map_over_ros.cc) +set_wavemap_target_properties(send_map_over_ros) +target_link_libraries(send_map_over_ros glog) cs_add_executable(fixed_resolution src/queries/fixed_resolution.cc) +set_wavemap_target_properties(fixed_resolution) +target_link_libraries(fixed_resolution glog) cs_add_executable(multi_resolution src/queries/multi_resolution.cc) +set_wavemap_target_properties(multi_resolution) +target_link_libraries(multi_resolution glog) cs_add_executable(accelerated_queries src/queries/accelerated_queries.cc) +set_wavemap_target_properties(accelerated_queries) +target_link_libraries(accelerated_queries glog) cs_add_executable(nearest_neighbor_interpolation src/queries/nearest_neighbor_interpolation.cc) +set_wavemap_target_properties(nearest_neighbor_interpolation) +target_link_libraries(nearest_neighbor_interpolation glog) cs_add_executable(trilinear_interpolation src/queries/trilinear_interpolation.cc) +set_wavemap_target_properties(trilinear_interpolation) +target_link_libraries(trilinear_interpolation glog) cs_add_executable(classification src/queries/classification.cc) +set_wavemap_target_properties(classification) +target_link_libraries(classification glog) cs_add_executable(occupancy_to_esdf src/planning/occupancy_to_esdf.cc) +set_wavemap_target_properties(occupancy_to_esdf) +target_link_libraries(occupancy_to_esdf glog) diff --git a/examples/src/planning/occupancy_to_esdf.cc b/examples/src/planning/occupancy_to_esdf.cc index 51aa8bb9d..4b6912ab9 100644 --- a/examples/src/planning/occupancy_to_esdf.cc +++ b/examples/src/planning/occupancy_to_esdf.cc @@ -1,11 +1,11 @@ #include #include -#include +#include +#include +#include +#include #include -#include -#include -#include using namespace wavemap; // NOLINT int main(int argc, char** argv) { diff --git a/examples/src/queries/accelerated_queries.cc b/examples/src/queries/accelerated_queries.cc index 756ec350f..67870918d 100644 --- a/examples/src/queries/accelerated_queries.cc +++ b/examples/src/queries/accelerated_queries.cc @@ -1,6 +1,6 @@ -#include -#include -#include +#include +#include +#include #include "wavemap_examples/common.h" diff --git a/examples/src/queries/classification.cc b/examples/src/queries/classification.cc index 62e931d41..6b3d941c4 100644 --- a/examples/src/queries/classification.cc +++ b/examples/src/queries/classification.cc @@ -1,4 +1,4 @@ -#include +#include #include "wavemap_examples/common.h" diff --git a/examples/src/queries/fixed_resolution.cc b/examples/src/queries/fixed_resolution.cc index 0e1aa6297..ce0bc72d9 100644 --- a/examples/src/queries/fixed_resolution.cc +++ b/examples/src/queries/fixed_resolution.cc @@ -1,4 +1,4 @@ -#include +#include #include "wavemap_examples/common.h" diff --git a/examples/src/queries/multi_resolution.cc b/examples/src/queries/multi_resolution.cc index 07d5c1fe6..92aebeb26 100644 --- a/examples/src/queries/multi_resolution.cc +++ b/examples/src/queries/multi_resolution.cc @@ -1,5 +1,5 @@ -#include -#include +#include +#include #include "wavemap_examples/common.h" diff --git a/examples/src/queries/nearest_neighbor_interpolation.cc b/examples/src/queries/nearest_neighbor_interpolation.cc index 471db57f4..de976965c 100644 --- a/examples/src/queries/nearest_neighbor_interpolation.cc +++ b/examples/src/queries/nearest_neighbor_interpolation.cc @@ -1,5 +1,5 @@ -#include -#include +#include +#include #include "wavemap_examples/common.h" diff --git a/examples/src/queries/trilinear_interpolation.cc b/examples/src/queries/trilinear_interpolation.cc index d52f22aeb..15a5fca46 100644 --- a/examples/src/queries/trilinear_interpolation.cc +++ b/examples/src/queries/trilinear_interpolation.cc @@ -1,5 +1,5 @@ -#include -#include +#include +#include #include "wavemap_examples/common.h" diff --git a/interfaces/ros1/wavemap/CMakeLists.txt b/interfaces/ros1/wavemap/CMakeLists.txt new file mode 100644 index 000000000..264f278d4 --- /dev/null +++ b/interfaces/ros1/wavemap/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.0.2) +project(wavemap) + +# Setup catkin package +find_package(catkin REQUIRED) +catkin_package( + INCLUDE_DIRS + ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/include + LIBRARIES core io + CFG_EXTRAS + ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/cmake/compiler_options.cmake +) + +# Load the library +add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../../../library + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}) + +# Install targets +install(TARGETS core io + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +# Mark cpp header files for installation +install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/include + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h") diff --git a/interfaces/ros1/wavemap/package.xml b/interfaces/ros1/wavemap/package.xml index 9bbab6797..bdf9c8cf5 100644 --- a/interfaces/ros1/wavemap/package.xml +++ b/interfaces/ros1/wavemap/package.xml @@ -15,9 +15,5 @@ libgoogle-glog-dev eigen - minkindr - - tracy_catkin - gtest diff --git a/interfaces/ros1/wavemap_ros/CMakeLists.txt b/interfaces/ros1/wavemap_ros/CMakeLists.txt index ac317feab..4e43ee388 100644 --- a/interfaces/ros1/wavemap_ros/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros/CMakeLists.txt @@ -14,9 +14,6 @@ if (livox_ros_driver2_FOUND) add_compile_definitions(LIVOX_AVAILABLE) endif () -# Compiler definitions and options -add_wavemap_compile_definitions_and_options() - # Libraries cs_add_library(${PROJECT_NAME} src/inputs/depth_image_input.cc @@ -33,14 +30,16 @@ cs_add_library(${PROJECT_NAME} src/utils/rosbag_processor.cc src/utils/tf_transformer.cc src/ros_server.cc) -# Link OpenCV explicitly to avoid issues on Jetson -target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) +set_wavemap_target_properties(${PROJECT_NAME}) +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES} glog) # Binaries cs_add_executable(ros_server app/ros_server.cc) +set_wavemap_target_properties(ros_server) target_link_libraries(ros_server ${PROJECT_NAME}) cs_add_executable(rosbag_processor app/rosbag_processor.cc) +set_wavemap_target_properties(rosbag_processor) target_link_libraries(rosbag_processor ${PROJECT_NAME}) # Export @@ -49,5 +48,4 @@ cs_export() # Export config files install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/config/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config -) + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h index 9d93f5d00..f375bbbaa 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h @@ -8,8 +8,9 @@ #include #include -#include -#include +#include +#include +#include #include "wavemap_ros/inputs/input_base.h" diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_base.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_base.h index 5ba4f04bf..475bb7025 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_base.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_base.h @@ -7,13 +7,13 @@ #include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include "wavemap_ros/utils/tf_transformer.h" diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_factory.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_factory.h index d33ff815e..f4441cf26 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_factory.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_factory.h @@ -4,7 +4,7 @@ #include #include -#include "wavemap/utils/thread_pool.h" +#include "wavemap/core/utils/thread_pool.h" #include "wavemap_ros/inputs/input_base.h" namespace wavemap { diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h index 487b1eb6f..b7928fb2c 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h @@ -5,8 +5,8 @@ #include #include -#include -#include +#include +#include #include "wavemap_ros/operations/operation_base.h" #include "wavemap_ros/utils/tf_transformer.h" diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/impl/publish_map_operation_inl.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/impl/publish_map_operation_inl.h index 32991f9fe..5b0322e9b 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/impl/publish_map_operation_inl.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/impl/publish_map_operation_inl.h @@ -3,8 +3,8 @@ #include -#include -#include +#include +#include #include #include @@ -48,7 +48,7 @@ void PublishMapOperation::publishHashedMap(const ros::Time& current_time, map_msg.hashed_wavelet_octree.emplace_back(), blocks_to_publish, thread_pool_); { - ZoneScopedN("publishMapRosMsg"); + ProfilerZoneScopedN("publishMapRosMsg"); map_pub_.publish(map_msg); } diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_base.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_base.h index 2d1be30de..50780536c 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_base.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_base.h @@ -2,7 +2,7 @@ #define WAVEMAP_ROS_OPERATIONS_OPERATION_BASE_H_ #include -#include +#include namespace wavemap { struct OperationType : public TypeSelector { diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_factory.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_factory.h index 320ce54ef..07afb1571 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_factory.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_factory.h @@ -5,8 +5,8 @@ #include #include -#include -#include +#include +#include #include "wavemap_ros/operations/operation_base.h" #include "wavemap_ros/utils/tf_transformer.h" diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/prune_map_operation.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/prune_map_operation.h index a0c121b85..b5b1d70f8 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/prune_map_operation.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/prune_map_operation.h @@ -3,8 +3,8 @@ #include -#include -#include +#include +#include #include "wavemap_ros/operations/operation_base.h" diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h index 9a56f9b2c..f3da987a5 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h @@ -5,10 +5,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include "wavemap_ros/operations/operation_base.h" diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h index 7f725af48..65f6d6a00 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h @@ -6,9 +6,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include "wavemap_ros/operations/operation_base.h" diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/threshold_map_operation.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/threshold_map_operation.h index 94aba38dc..5c05b1b77 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/threshold_map_operation.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/threshold_map_operation.h @@ -3,8 +3,8 @@ #include -#include -#include +#include +#include #include "wavemap_ros/operations/operation_base.h" diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h index 8f61465ea..a0b49cdeb 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h @@ -9,12 +9,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include "wavemap_ros/inputs/input_base.h" #include "wavemap_ros/operations/operation_base.h" diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/logging_level.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/logging_level.h index 10f6f318f..75ad9cbeb 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/logging_level.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/logging_level.h @@ -2,7 +2,7 @@ #define WAVEMAP_ROS_UTILS_LOGGING_LEVEL_H_ #include -#include +#include namespace wavemap { struct LoggingLevel : public TypeSelector { diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h index e06999b6c..212179d57 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h @@ -7,8 +7,8 @@ #include #include -#include -#include +#include +#include #include "wavemap_ros/utils/pointcloud_undistortion/stamped_pointcloud.h" #include "wavemap_ros/utils/tf_transformer.h" diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/stamped_pointcloud.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/stamped_pointcloud.h index 469f3ce30..853d3bac7 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/stamped_pointcloud.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/stamped_pointcloud.h @@ -6,7 +6,7 @@ #include #include -#include +#include namespace wavemap { struct StampedPoint { diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/tf_transformer.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/tf_transformer.h index 9ec596f38..9905c1e30 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/tf_transformer.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/tf_transformer.h @@ -5,7 +5,7 @@ #include #include -#include +#include namespace wavemap { class TfTransformer { diff --git a/interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc index 6fedd7008..37bd13b58 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc @@ -2,9 +2,9 @@ #include #include -#include -#include -#include +#include +#include +#include namespace wavemap { DECLARE_CONFIG_MEMBERS(DepthImageInputConfig, @@ -61,7 +61,7 @@ DepthImageInput::DepthImageInput( } void DepthImageInput::processQueue() { - ZoneScoped; + ProfilerZoneScoped; while (!depth_image_queue_.empty()) { const sensor_msgs::Image& oldest_msg = depth_image_queue_.front(); const std::string sensor_frame_id = config_.sensor_frame_id.empty() @@ -140,7 +140,7 @@ void DepthImageInput::processQueue() { } } } - FrameMarkNamed("DepthImage"); + ProfilerFrameMarkNamed("DepthImage"); // Remove the depth image from the queue depth_image_queue_.pop(); @@ -149,7 +149,7 @@ void DepthImageInput::processQueue() { PosedPointcloud<> DepthImageInput::reproject( const PosedImage<>& posed_range_image) { - ZoneScoped; + ProfilerZoneScoped; auto projective_integrator = std::dynamic_pointer_cast(integrators_.front()); if (!projective_integrator) { diff --git a/interfaces/ros1/wavemap_ros/src/inputs/input_base.cc b/interfaces/ros1/wavemap_ros/src/inputs/input_base.cc index 64ef6e855..7d4a24579 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/input_base.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/input_base.cc @@ -5,9 +5,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include namespace wavemap { DECLARE_CONFIG_MEMBERS(InputBaseConfig, @@ -86,7 +86,7 @@ bool InputBase::shouldPublishReprojectedPointcloud() const { void InputBase::publishReprojectedPointcloud( const ros::Time& stamp, const PosedPointcloud<>& posed_pointcloud) { - ZoneScoped; + ProfilerZoneScoped; sensor_msgs::PointCloud pointcloud_msg; pointcloud_msg.header.stamp = stamp; pointcloud_msg.header.frame_id = world_frame_; @@ -111,7 +111,7 @@ bool InputBase::shouldPublishProjectedRangeImage() const { void InputBase::publishProjectedRangeImage(const ros::Time& stamp, const Image<>& range_image) { - ZoneScoped; + ProfilerZoneScoped; cv_bridge::CvImage cv_image; cv_image.header.stamp = stamp; cv_image.encoding = "32FC1"; diff --git a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc index 64c14778b..79e466b24 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc @@ -1,8 +1,8 @@ #include "wavemap_ros/inputs/pointcloud_input.h" #include -#include -#include +#include +#include #include namespace wavemap { @@ -52,7 +52,7 @@ PointcloudInput::PointcloudInput( } void PointcloudInput::callback(const sensor_msgs::PointCloud2& pointcloud_msg) { - ZoneScoped; + ProfilerZoneScoped; // Skip empty clouds const size_t num_points = pointcloud_msg.height * pointcloud_msg.width; if (num_points == 0) { @@ -128,7 +128,7 @@ void PointcloudInput::callback(const sensor_msgs::PointCloud2& pointcloud_msg) { #ifdef LIVOX_AVAILABLE void PointcloudInput::callback( const livox_ros_driver2::CustomMsg& pointcloud_msg) { - ZoneScoped; + ProfilerZoneScoped; // Skip empty clouds if (pointcloud_msg.points.empty()) { ROS_WARN_STREAM("Skipping empty pointcloud with timestamp " @@ -155,7 +155,7 @@ void PointcloudInput::callback( #endif void PointcloudInput::processQueue() { - ZoneScoped; + ProfilerZoneScoped; while (!pointcloud_queue_.empty()) { auto& oldest_msg = pointcloud_queue_.front(); @@ -267,7 +267,7 @@ void PointcloudInput::processQueue() { } } } - FrameMarkNamed("Pointcloud"); + ProfilerFrameMarkNamed("Pointcloud"); // Remove the pointcloud from the queue pointcloud_queue_.pop(); diff --git a/interfaces/ros1/wavemap_ros/src/operations/crop_map_operation.cc b/interfaces/ros1/wavemap_ros/src/operations/crop_map_operation.cc index 9dbfb9a5e..6f47f1637 100644 --- a/interfaces/ros1/wavemap_ros/src/operations/crop_map_operation.cc +++ b/interfaces/ros1/wavemap_ros/src/operations/crop_map_operation.cc @@ -1,8 +1,8 @@ #include "wavemap_ros/operations/crop_map_operation.h" -#include -#include -#include +#include +#include +#include namespace wavemap { DECLARE_CONFIG_MEMBERS(CropMapOperationConfig, diff --git a/interfaces/ros1/wavemap_ros/src/operations/publish_map_operation.cc b/interfaces/ros1/wavemap_ros/src/operations/publish_map_operation.cc index becaa786f..3c6414840 100644 --- a/interfaces/ros1/wavemap_ros/src/operations/publish_map_operation.cc +++ b/interfaces/ros1/wavemap_ros/src/operations/publish_map_operation.cc @@ -1,7 +1,7 @@ #include "wavemap_ros/operations/publish_map_operation.h" #include -#include +#include #include #include @@ -42,7 +42,7 @@ PublishMapOperation::PublishMapOperation( void PublishMapOperation::publishMap(const ros::Time& current_time, bool republish_whole_map) { - ZoneScoped; + ProfilerZoneScoped; // If the map is empty, there's no work to do if (occupancy_map_->empty()) { return; diff --git a/interfaces/ros1/wavemap_ros/src/operations/publish_pointcloud_operation.cc b/interfaces/ros1/wavemap_ros/src/operations/publish_pointcloud_operation.cc index a4e328d52..a635a73cd 100644 --- a/interfaces/ros1/wavemap_ros/src/operations/publish_pointcloud_operation.cc +++ b/interfaces/ros1/wavemap_ros/src/operations/publish_pointcloud_operation.cc @@ -3,11 +3,11 @@ #include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include namespace wavemap { @@ -38,7 +38,7 @@ PublishPointcloudOperation::PublishPointcloudOperation( void PublishPointcloudOperation::publishPointcloud( const ros::Time& current_time) { - ZoneScoped; + ProfilerZoneScoped; // If the map is empty, there's no work to do if (occupancy_map_->empty()) { return; diff --git a/interfaces/ros1/wavemap_ros/src/ros_server.cc b/interfaces/ros1/wavemap_ros/src/ros_server.cc index 2d983a31a..e40a26e73 100644 --- a/interfaces/ros1/wavemap_ros/src/ros_server.cc +++ b/interfaces/ros1/wavemap_ros/src/ros_server.cc @@ -1,9 +1,8 @@ #include "wavemap_ros/ros_server.h" #include -#include +#include #include -#include #include #include diff --git a/interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistortion/pointcloud_undistorter.cc b/interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistortion/pointcloud_undistorter.cc index 979f6b899..2ee03f75d 100644 --- a/interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistortion/pointcloud_undistorter.cc +++ b/interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistortion/pointcloud_undistorter.cc @@ -1,13 +1,13 @@ #include "wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h" -#include +#include #include namespace wavemap { PointcloudUndistorter::Result PointcloudUndistorter::undistortPointcloud( StampedPointcloud& stamped_pointcloud, PosedPointcloud<>& undistorted_pointcloud, const std::string& fixed_frame) { - ZoneScoped; + ProfilerZoneScoped; // Get the time interval const uint64_t start_time = stamped_pointcloud.getStartTime(); const uint64_t end_time = stamped_pointcloud.getEndTime(); diff --git a/interfaces/ros1/wavemap_ros/src/utils/rosbag_processor.cc b/interfaces/ros1/wavemap_ros/src/utils/rosbag_processor.cc index aa0aefb0e..734db02fc 100644 --- a/interfaces/ros1/wavemap_ros/src/utils/rosbag_processor.cc +++ b/interfaces/ros1/wavemap_ros/src/utils/rosbag_processor.cc @@ -1,6 +1,6 @@ #include "wavemap_ros/utils/rosbag_processor.h" -#include +#include namespace wavemap { RosbagProcessor::~RosbagProcessor() { @@ -37,7 +37,7 @@ bool RosbagProcessor::bagsContainTopic(const std::string& topic_name) { } bool RosbagProcessor::processAll() { - ZoneScoped; + ProfilerZoneScoped; ros::Time side_tasks_last_timestamp(0); const ros::Duration kSideTasksDt(0.01); @@ -55,14 +55,14 @@ bool RosbagProcessor::processAll() { // Handle callbacks if (auto it = callbacks_.find(msg.getTopic()); it != callbacks_.end()) { - ZoneScopedN("rosbagMsgHandleCallbacks"); + ProfilerZoneScopedN("rosbagMsgHandleCallbacks"); it->second(msg); } // Handle republishing if (auto it = republishers_.find(msg.getTopic()); it != republishers_.end()) { - ZoneScopedN("rosbagMsgPublishToTopic"); + ProfilerZoneScopedN("rosbagMsgPublishToTopic"); it->second.publish(msg); } @@ -80,7 +80,7 @@ bool RosbagProcessor::processAll() { // Process node callbacks (publishers, timers,...) { - ZoneScopedN("rosbagSpinOnce"); + ProfilerZoneScopedN("rosbagSpinOnce"); ros::spinOnce(); } } diff --git a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt index 16b42ef4d..626d0d03a 100644 --- a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt @@ -4,14 +4,12 @@ project(wavemap_ros_conversions) find_package(catkin_simple REQUIRED) catkin_simple(ALL_DEPS_REQUIRED) -# Compiler definitions and options -add_wavemap_compile_definitions_and_options() - # Libraries cs_add_library(${PROJECT_NAME} src/config_conversions.cc src/map_msg_conversions.cc src/time_conversions.cc) +set_wavemap_target_properties(${PROJECT_NAME}) # Tests if (CATKIN_ENABLE_TESTING) @@ -19,6 +17,7 @@ if (CATKIN_ENABLE_TESTING) test_${PROJECT_NAME} test/src/test_map_msg_conversions.cc) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} gtest_main) + set_wavemap_target_properties(test_${PROJECT_NAME}) endif () # Export diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/config_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/config_conversions.h index 27ed3647f..42204b47d 100644 --- a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/config_conversions.h +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/config_conversions.h @@ -4,7 +4,7 @@ #include #include -#include +#include #include namespace wavemap::param::convert { diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h index 439524f69..8d230c14f 100644 --- a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h @@ -9,12 +9,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include namespace wavemap::convert { diff --git a/interfaces/ros1/wavemap_ros_conversions/src/map_msg_conversions.cc b/interfaces/ros1/wavemap_ros_conversions/src/map_msg_conversions.cc index 2306f9114..5ec72fe29 100644 --- a/interfaces/ros1/wavemap_ros_conversions/src/map_msg_conversions.cc +++ b/interfaces/ros1/wavemap_ros_conversions/src/map_msg_conversions.cc @@ -1,7 +1,7 @@ #include "wavemap_ros_conversions/map_msg_conversions.h" #include -#include +#include namespace wavemap::convert { bool mapToRosMsg(const MapBase& map, const std::string& frame_id, @@ -43,7 +43,7 @@ bool mapToRosMsg(const MapBase& map, const std::string& frame_id, } bool rosMsgToMap(const wavemap_msgs::Map& msg, MapBase::Ptr& map) { - ZoneScoped; + ProfilerZoneScoped; // Check validity bool is_valid = true; std::string error_msg = @@ -101,7 +101,7 @@ bool rosMsgToMap(const wavemap_msgs::Map& msg, MapBase::Ptr& map) { } void mapToRosMsg(const HashedBlocks& map, wavemap_msgs::HashedBlocks& msg) { - ZoneScoped; + ProfilerZoneScoped; // Serialize the map and data structure's metadata msg.min_cell_width = map.getMinCellWidth(); msg.min_log_odds = map.getMinLogOdds(); @@ -135,7 +135,7 @@ void mapToRosMsg(const HashedBlocks& map, wavemap_msgs::HashedBlocks& msg) { void rosMsgToMap(const wavemap_msgs::HashedBlocks& msg, HashedBlocks::Ptr& map) { - ZoneScoped; + ProfilerZoneScoped; // Deserialize the map's config MapBaseConfig config; config.min_cell_width = msg.min_cell_width; @@ -176,7 +176,7 @@ void rosMsgToMap(const wavemap_msgs::HashedBlocks& msg, } void mapToRosMsg(const WaveletOctree& map, wavemap_msgs::WaveletOctree& msg) { - ZoneScoped; + ProfilerZoneScoped; // Serialize the map and data structure's metadata msg.min_cell_width = map.getMinCellWidth(); msg.min_log_odds = map.getMinLogOdds(); @@ -204,7 +204,7 @@ void mapToRosMsg(const WaveletOctree& map, wavemap_msgs::WaveletOctree& msg) { void rosMsgToMap(const wavemap_msgs::WaveletOctree& msg, WaveletOctree::Ptr& map) { - ZoneScoped; + ProfilerZoneScoped; // Deserialize the map's config WaveletOctreeConfig config; config.min_cell_width = msg.min_cell_width; @@ -253,7 +253,7 @@ void mapToRosMsg( const HashedWaveletOctree& map, wavemap_msgs::HashedWaveletOctree& msg, std::optional> include_blocks, std::shared_ptr thread_pool) { - ZoneScoped; + ProfilerZoneScoped; // Constants constexpr FloatingPoint kNumericalNoise = 1e-3f; const auto min_log_odds = map.getMinLogOdds() + kNumericalNoise; @@ -325,7 +325,7 @@ void blockToRosMsg(const HashedWaveletOctree::BlockIndex& block_index, const HashedWaveletOctree::Block& block, FloatingPoint min_log_odds, FloatingPoint max_log_odds, wavemap_msgs::HashedWaveletOctreeBlock& msg) { - ZoneScoped; + ProfilerZoneScoped; // Convenience type for elements on the stack used to iterate over the map struct StackElement { const FloatingPoint scale; @@ -378,7 +378,7 @@ void blockToRosMsg(const HashedWaveletOctree::BlockIndex& block_index, void rosMsgToMap(const wavemap_msgs::HashedWaveletOctree& msg, HashedWaveletOctree::Ptr& map) { - ZoneScoped; + ProfilerZoneScoped; // Deserialize the map's config and initialize the data structure HashedWaveletOctreeConfig config; config.min_cell_width = msg.min_cell_width; @@ -451,7 +451,7 @@ void mapToRosMsg( wavemap_msgs::HashedWaveletOctree& msg, std::optional> include_blocks, std::shared_ptr thread_pool) { - ZoneScoped; + ProfilerZoneScoped; // Constants constexpr FloatingPoint kNumericalNoise = 1e-3f; const auto min_log_odds = map.getMinLogOdds() + kNumericalNoise; @@ -524,7 +524,7 @@ void blockToRosMsg(const HashedChunkedWaveletOctree::BlockIndex& block_index, const HashedChunkedWaveletOctree::Block& block, FloatingPoint min_log_odds, FloatingPoint max_log_odds, wavemap_msgs::HashedWaveletOctreeBlock& msg) { - ZoneScoped; + ProfilerZoneScoped; // Define convenience types and constants struct StackElement { const FloatingPoint scale; diff --git a/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt b/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt index 49c70ee7c..208996c83 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt +++ b/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt @@ -13,9 +13,8 @@ find_package(catkin wavemap_msgs wavemap_ros_conversions) -# Compiler definitions and options -add_wavemap_compile_definitions_and_options() # Disable some warnings coming from Qt code in combination with C++17 +# TODO(victorr): Only add to relevant target, not globally add_compile_options(-Wno-pedantic -Wno-register) # Setup catkin package @@ -55,6 +54,7 @@ endif () # Avoid Qt signals and slots defining "emit", "slots", etc. # because they can conflict with boost signals. +# TODO(victorr): Only add to relevant target, not globally add_definitions(-DQT_NO_KEYWORDS) # Libraries @@ -67,6 +67,7 @@ add_library(${PROJECT_NAME} src/visuals/cell_selector.cc src/wavemap_map_display.cc ${QT_MOC}) +set_wavemap_target_properties(${PROJECT_NAME}) target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) # Install diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h index da91a1e0a..2a9093a3b 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h @@ -3,7 +3,7 @@ #include -#include +#include namespace wavemap::rviz_plugin { struct MapAndMutex { diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/color_conversions.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/color_conversions.h index bdbc1f748..7873cf960 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/color_conversions.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/utils/color_conversions.h @@ -2,7 +2,7 @@ #define WAVEMAP_RVIZ_PLUGIN_UTILS_COLOR_CONVERSIONS_H_ #include -#include +#include namespace wavemap::rviz_plugin { // Map a voxel's log-odds value to a color (grey value) diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h index 3dfe6a2e2..41ca0d023 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h @@ -5,9 +5,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #endif namespace wavemap::rviz_plugin { diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h index afa59f28b..5b71a87e5 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/slice_visual.h @@ -15,7 +15,7 @@ #include #include #include -#include +#include #include "wavemap_rviz_plugin/common.h" #include "wavemap_rviz_plugin/visuals/cell_layer.h" diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h index 3cf523247..863b9fa57 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h @@ -17,10 +17,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include "wavemap_rviz_plugin/common.h" #include "wavemap_rviz_plugin/utils/color_conversions.h" diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc index 5afddca39..045b94935 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc @@ -1,6 +1,6 @@ #include "wavemap_rviz_plugin/visuals/cell_selector.h" -#include +#include namespace wavemap::rviz_plugin { static const auto kNeighborOffsets = diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/slice_visual.cc b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/slice_visual.cc index 5b0656b5c..be39236da 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/slice_visual.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/slice_visual.cc @@ -1,7 +1,7 @@ #include "wavemap_rviz_plugin/visuals/slice_visual.h" #include -#include +#include #include "wavemap_rviz_plugin/utils/color_conversions.h" diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc index 25fe4a4b0..120d9ba7a 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc @@ -2,9 +2,9 @@ #include #include -#include -#include -#include +#include +#include +#include namespace wavemap::rviz_plugin { VoxelVisual::VoxelVisual(Ogre::SceneManager* scene_manager, @@ -93,7 +93,7 @@ VoxelVisual::~VoxelVisual() { } void VoxelVisual::updateMap(bool redraw_all) { - ZoneScoped; + ProfilerZoneScoped; if (!visibility_property_.getBool()) { return; } @@ -181,7 +181,7 @@ void VoxelVisual::updateMap(bool redraw_all) { } void VoxelVisual::updateLOD(const Ogre::Camera& active_camera) { - ZoneScoped; + ProfilerZoneScoped; if (!visibility_property_.getBool()) { return; } @@ -237,7 +237,7 @@ IndexElement VoxelVisual::computeRecommendedBlockLodHeight( const Ogre::Camera& active_camera, const OctreeIndex& block_index, FloatingPoint min_cell_width, IndexElement min_height, IndexElement max_height) { - ZoneScoped; + ProfilerZoneScoped; // TODO(victorr): Compute the LoD level using the camera's projection matrix // and the screen's pixel density, to better generalize across // displays (high/low DPI) and alternative projection modes @@ -263,7 +263,7 @@ IndexElement VoxelVisual::computeRecommendedBlockLodHeight( std::optional VoxelVisual::getCurrentBlockLodHeight( IndexElement map_tree_height, const Index3D& block_idx) { - ZoneScoped; + ProfilerZoneScoped; if (block_voxel_layers_map_.count(block_idx)) { return map_tree_height - (static_cast(block_voxel_layers_map_[block_idx].size()) - 1); @@ -274,17 +274,17 @@ std::optional VoxelVisual::getCurrentBlockLodHeight( // Position and orientation are passed through to the SceneNode void VoxelVisual::setFramePosition(const Ogre::Vector3& position) { - ZoneScoped; + ProfilerZoneScoped; frame_node_->setPosition(position); } void VoxelVisual::setFrameOrientation(const Ogre::Quaternion& orientation) { - ZoneScoped; + ProfilerZoneScoped; frame_node_->setOrientation(orientation); } void VoxelVisual::visibilityUpdateCallback() { - ZoneScoped; + ProfilerZoneScoped; if (visibility_property_.getBool()) { updateMap(true); } else { @@ -293,13 +293,13 @@ void VoxelVisual::visibilityUpdateCallback() { } void VoxelVisual::opacityUpdateCallback() { - ZoneScoped; + ProfilerZoneScoped; FloatingPoint alpha = opacity_property_.getFloat(); setAlpha(alpha); } void VoxelVisual::colorModeUpdateCallback() { - ZoneScoped; + ProfilerZoneScoped; // Update the cached color mode value const VoxelColorMode old_color_mode = voxel_color_mode_; voxel_color_mode_ = VoxelColorMode(color_mode_property_.getStdString()); @@ -314,7 +314,7 @@ void VoxelVisual::colorModeUpdateCallback() { } void VoxelVisual::flatColorUpdateCallback() { - ZoneScoped; + ProfilerZoneScoped; // Update the cached color value const Ogre::ColourValue old_flat_color = voxel_flat_color_; voxel_flat_color_ = flat_color_property_.getOgreColor(); @@ -369,7 +369,7 @@ void VoxelVisual::drawMultiResolutionVoxels(IndexElement tree_height, FloatingPoint alpha, VoxelsPerLevel& voxels_per_level, VoxelLayers& voxel_layer_visuals) { - ZoneScoped; + ProfilerZoneScoped; // Add a voxel layer for each scale level const std::string prefix = "voxel_layer_" + std::to_string(Index3DHash()(block_index)) + "_"; @@ -401,7 +401,7 @@ void VoxelVisual::drawMultiResolutionVoxels(IndexElement tree_height, } void VoxelVisual::processBlockUpdateQueue(const Point3D& camera_position) { - ZoneScoped; + ProfilerZoneScoped; if (!visibility_property_.getBool()) { return; } @@ -523,7 +523,7 @@ void VoxelVisual::setAlpha(FloatingPoint alpha) { } void VoxelVisual::prerenderCallback(Ogre::Camera* active_camera) { - ZoneScoped; + ProfilerZoneScoped; CHECK_NOTNULL(active_camera); // Recompute the desired LOD level for each block in the map if // the camera moved significantly or an update was requested explicitly diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc b/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc index 1753cc4e5..28b963bbd 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc @@ -6,7 +6,7 @@ #include #include #include -#include +#include #include #include @@ -28,7 +28,7 @@ WavemapMapDisplay::WavemapMapDisplay() { // call our immediate super-class's onInitialize() function, since it // does important stuff setting up the message filter. void WavemapMapDisplay::onInitialize() { - ZoneScoped; + ProfilerZoneScoped; MFDClass::onInitialize(); voxel_visual_ = std::make_unique( scene_manager_, context_->getViewManager(), scene_node_, @@ -39,20 +39,20 @@ void WavemapMapDisplay::onInitialize() { // Clear the visuals by deleting their objects. void WavemapMapDisplay::reset() { - ZoneScoped; + ProfilerZoneScoped; MFDClass::reset(); voxel_visual_->clear(); slice_visual_->clear(); } bool WavemapMapDisplay::hasMap() { - ZoneScoped; + ProfilerZoneScoped; std::scoped_lock lock(map_and_mutex_->mutex); return static_cast(map_and_mutex_->map); } void WavemapMapDisplay::clearMap() { - ZoneScoped; + ProfilerZoneScoped; std::scoped_lock lock(map_and_mutex_->mutex); if (map_and_mutex_->map) { map_and_mutex_->map->clear(); @@ -60,13 +60,13 @@ void WavemapMapDisplay::clearMap() { } bool WavemapMapDisplay::loadMapFromDisk(const std::filesystem::path& filepath) { - ZoneScoped; + ProfilerZoneScoped; std::scoped_lock lock(map_and_mutex_->mutex); return io::fileToMap(filepath, map_and_mutex_->map); } void WavemapMapDisplay::updateVisuals(bool redraw_all) { - ZoneScoped; + ProfilerZoneScoped; if (!hasMap()) { return; } @@ -77,7 +77,7 @@ void WavemapMapDisplay::updateVisuals(bool redraw_all) { // This is our callback to handle an incoming message void WavemapMapDisplay::processMessage( const wavemap_msgs::Map::ConstPtr& map_msg) { - ZoneScoped; + ProfilerZoneScoped; // Deserialize the octree if (!map_msg) { ROS_WARN("Ignoring request to process non-existent octree msg (nullptr)."); @@ -113,7 +113,7 @@ void WavemapMapDisplay::processMessage( } void WavemapMapDisplay::updateMapFromRosMsg(const wavemap_msgs::Map& map_msg) { - ZoneScoped; + ProfilerZoneScoped; std::scoped_lock lock(map_and_mutex_->mutex); if (!convert::rosMsgToMap(map_msg, map_and_mutex_->map)) { ROS_WARN("Failed to parse map message."); @@ -121,7 +121,7 @@ void WavemapMapDisplay::updateMapFromRosMsg(const wavemap_msgs::Map& map_msg) { } void WavemapMapDisplay::updateSourceModeCallback() { - ZoneScoped; + ProfilerZoneScoped; // Update the cached source mode value const SourceMode old_source_mode = source_mode_; source_mode_ = SourceMode(source_mode_property_.getStdString()); @@ -153,7 +153,7 @@ void WavemapMapDisplay::updateSourceModeCallback() { } void WavemapMapDisplay::requestWavemapServerResetCallback() { - ZoneScoped; + ProfilerZoneScoped; // Resolve name of the service based on the map topic const auto& map_topic = sub_.getTopic(); const auto service_name = resolveWavemapServerNamespaceFromMapTopic( @@ -216,7 +216,7 @@ void WavemapMapDisplay::requestWavemapServerResetCallback() { } void WavemapMapDisplay::requestWholeMapCallback() { - ZoneScoped; + ProfilerZoneScoped; // Resolve name of the service based on the map topic const auto& map_topic = sub_.getTopic(); const auto service_name = resolveWavemapServerNamespaceFromMapTopic( @@ -270,7 +270,7 @@ void WavemapMapDisplay::requestWholeMapCallback() { } void WavemapMapDisplay::loadMapFromDiskCallback() { - ZoneScoped; + ProfilerZoneScoped; // Open file selection dialog const auto filepath_qt = QFileDialog::getOpenFileName(); @@ -297,7 +297,7 @@ void WavemapMapDisplay::loadMapFromDiskCallback() { std::optional WavemapMapDisplay::resolveWavemapServerNamespaceFromMapTopic( const std::string& map_topic, const std::string& child_topic) { - ZoneScoped; + ProfilerZoneScoped; const auto pos = map_topic.rfind('/'); if (pos == std::string::npos) { return std::nullopt; diff --git a/library/CMakeLists.txt b/library/CMakeLists.txt index 1d38a70f9..c3f319cc1 100644 --- a/library/CMakeLists.txt +++ b/library/CMakeLists.txt @@ -2,11 +2,16 @@ cmake_minimum_required(VERSION 3.10) project(wavemap VERSION 1.6.3 LANGUAGES CXX) +# User options +option(USE_CLANG_TIDY "Generate necessary files to run clang-tidy" OFF) + # Dependencies find_package(Eigen3 REQUIRED NO_MODULE) -find_package(tracy QUIET) # Optional dependency find_package(PkgConfig REQUIRED) pkg_check_modules(glog REQUIRED libglog) + +# Optional dependencies +find_package(tracy QUIET) # TODO(victorr): Check if tracy is correctly found and linked in when it is # available as an installed (system) package. @@ -26,3 +31,8 @@ add_subdirectory(src/pipeline) #if (ENABLE_BENCHMARKING) # add_subdirectory(benchmark) #endif () + +# Tooling +if (USE_CLANG_TIDY) + set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +endif () diff --git a/library/cmake/compiler_options.cmake b/library/cmake/compiler_options.cmake index 41c960365..d6790d0b7 100644 --- a/library/cmake/compiler_options.cmake +++ b/library/cmake/compiler_options.cmake @@ -1,36 +1,73 @@ -# TODO(victorr): Unify this with the macro in wavemap-extras.cmake +# User options +option(DCHECK_ALWAYS_ON + "Enable GLOG DCHECKs even when not compiling in debug mode" OFF) +option(USE_UBSAN "Compile with undefined behavior sanitizer enabled" OFF) +option(USE_ASAN "Compile with address sanitizer enabled" OFF) +option(USE_TSAN "Compile with thread sanitizer enabled" OFF) +option(ENABLE_COVERAGE_TESTING + "Compile with necessary flags for coverage testing" OFF) + # This function is used to set the compile features, definitions, options and # include paths for all targets in the wavemap library. function(set_wavemap_target_properties target) - target_compile_features(${target} PUBLIC cxx_std_17) - target_compile_definitions(${target} PUBLIC - $<$:_USE_MATH_DEFINES>) - # target_compile_options( - # ${target} - # PRIVATE # MSVC - # $<$:/W4> - # $<$:/WX> - # # Clang/AppleClang - # $<$:-fcolor-diagnostics> - # $<$:-Werror> - # $<$:-Wall> - # $<$:-Wextra> - # $<$:-Wconversion> - # $<$:-Wno-sign-conversion> - # # GNU - # $<$:-fdiagnostics-color=always> - # $<$:-Werror> - # $<$:-Wall> - # $<$:-Wextra> - # $<$:-pedantic> - # $<$:-Wcast-align> - # $<$:-Wcast-qual> - # $<$:-Wconversion> - # $<$:-Wdisabled-optimization> - # $<$:-Woverloaded-virtual>) + # Configure the include dirs set(include_dirs ${PROJECT_SOURCE_DIR}/include/wavemap) get_filename_component(include_dirs ${include_dirs} PATH) target_include_directories(${target} PUBLIC $ $) + + # Require C++17 + target_compile_features(${target} PUBLIC cxx_std_17) + set_target_properties(${target} PROPERTIES CXX_EXTENSIONS OFF) + + # General compilation options + set_target_properties(${target} PROPERTIES POSITION_INDEPENDENT_CODE ON) + target_compile_options(${target} PUBLIC -march=native) + target_compile_options(${target} PRIVATE + -Wall -Wextra -Wpedantic -Wsuggest-attribute=const + -Wno-deprecated-copy -Wno-class-memaccess) + + # General C++ defines + target_compile_definitions(${target} PUBLIC EIGEN_INITIALIZE_MATRICES_BY_NAN) + + # Conditional compilation options + if (DCHECK_ALWAYS_ON) + target_compile_definitions(${target} PUBLIC DCHECK_ALWAYS_ON) + endif () + if (USE_UBSAN) + target_compile_options(${target} + -fsanitize=undefined + -fsanitize=shift + -fsanitize=integer-divide-by-zero + -fsanitize=null + -fsanitize=return + -fsanitize=signed-integer-overflow + -fsanitize=bounds-strict + -fsanitize=alignment + -fsanitize=float-divide-by-zero + -fsanitize=float-cast-overflow + -fsanitize=enum + -fsanitize=vptr + -fsanitize=pointer-overflow + -fsanitize=builtin + -fno-omit-frame-pointer + -g) + add_link_options(-fsanitize=undefined) + endif () + if (USE_ASAN) + target_compile_options(${target} + -fsanitize=address -fsanitize-address-use-after-scope + -fno-omit-frame-pointer -g) + target_link_options(${target} -fsanitize=address) + endif () + if (USE_TSAN) + target_compile_options(${target} + -fsanitize=thread -fno-omit-frame-pointer -g) + target_link_options(${target} -fsanitize=thread) + endif () + if (ENABLE_COVERAGE_TESTING) + target_compile_options(${target} -fprofile-arcs -ftest-coverage -O0 -g) + target_link_options(${target} -fprofile-arcs) + endif () endfunction() diff --git a/library/cmake/wavemap-extras.cmake b/library/cmake/wavemap-extras.cmake deleted file mode 100644 index 2270a3571..000000000 --- a/library/cmake/wavemap-extras.cmake +++ /dev/null @@ -1,67 +0,0 @@ -# NOTE: For useful info about catkin and using CFG_EXTRAS: -# http://wiki.ros.org/catkin/CMakeLists.txt -# https://docs.ros.org/en/api/catkin/html/dev_guide/generated_cmake_api.html - -# Add the compiler definitions and options consistently across all wavemap pkgs -macro (ADD_WAVEMAP_COMPILE_DEFINITIONS_AND_OPTIONS) - option(DCHECK_ALWAYS_ON - "Enable GLOG DCHECKs even when not compiling in debug mode" OFF) - option(USE_UBSAN "Compile with undefined behavior sanitizer enabled" OFF) - option(USE_ASAN "Compile with address sanitizer enabled" OFF) - option(USE_TSAN "Compile with thread sanitizer enabled" OFF) - option(USE_CLANG_TIDY "Generate necessary files to run clang-tidy" OFF) - option(ENABLE_BENCHMARKING "Compile benchmarking targets" OFF) - - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_POSITION_INDEPENDENT_CODE ON) - add_compile_definitions(EIGEN_INITIALIZE_MATRICES_BY_NAN) - add_compile_options( - -march=native -Wall -Wextra -Wpedantic -Wsuggest-attribute=const - -Wno-deprecated-copy -Wno-class-memaccess) - - if (DCHECK_ALWAYS_ON) - add_compile_definitions(DCHECK_ALWAYS_ON) - endif () - - if (USE_UBSAN) - add_compile_options( - -fsanitize=undefined - -fsanitize=shift - -fsanitize=integer-divide-by-zero - -fsanitize=null - -fsanitize=return - -fsanitize=signed-integer-overflow - -fsanitize=bounds-strict - -fsanitize=alignment - -fsanitize=float-divide-by-zero - -fsanitize=float-cast-overflow - -fsanitize=enum - -fsanitize=vptr - -fsanitize=pointer-overflow - -fsanitize=builtin - -fno-omit-frame-pointer - -g) - add_link_options(-fsanitize=undefined) - endif () - - if (USE_ASAN) - add_compile_options(-fsanitize=address -fsanitize-address-use-after-scope - -fno-omit-frame-pointer -g) - add_link_options(-fsanitize=address) - endif () - - if (USE_TSAN) - add_compile_options(-fsanitize=thread -fno-omit-frame-pointer -g) - add_link_options(-fsanitize=thread) - endif () - - if (USE_CLANG_TIDY) - set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - endif () - - if (CATKIN_ENABLE_TESTING AND ENABLE_COVERAGE_TESTING) - add_compile_options(-fprofile-arcs -ftest-coverage -O0 -g) - add_link_options(-fprofile-arcs) - endif () -endmacro () diff --git a/library/include/wavemap/core/utils/profiler_interface.h b/library/include/wavemap/core/utils/profiler_interface.h index ab47ee6fd..9362b4938 100644 --- a/library/include/wavemap/core/utils/profiler_interface.h +++ b/library/include/wavemap/core/utils/profiler_interface.h @@ -5,9 +5,13 @@ #include -#define ProfilerZoneScoped -#define ProfilerZoneScopedN(x) -#define ProfilerPlot(x, y) +#define ProfilerZoneScoped ZoneScoped +#define ProfilerZoneScopedN(x) ZoneScopedN(x) + +#define ProfilerFrameMark FrameMark +#define ProfilerFrameMarkNamed(x) FrameMarkNamed(x) + +#define ProfilerPlot(x, y) TracyPlot(x, y) #define ProfilerSetThreadName(x) tracy::SetThreadName(x) @@ -15,6 +19,10 @@ #define ProfilerZoneScoped #define ProfilerZoneScopedN(x) + +#define ProfilerFrameMark +#define ProfilerFrameMarkNamed(x) + #define ProfilerPlot(x, y) #define ProfilerSetThreadName(x) diff --git a/library/src/core/CMakeLists.txt b/library/src/core/CMakeLists.txt index b2dbd872a..07e0af2e8 100644 --- a/library/src/core/CMakeLists.txt +++ b/library/src/core/CMakeLists.txt @@ -36,8 +36,7 @@ target_sources(core PRIVATE utils/sdf/quasi_euclidean_sdf_generator.cc utils/time/stopwatch.cc utils/thread_pool.cc) -target_include_directories(core PUBLIC ${glog_INCLUDE_DIRS}) -target_link_libraries(core PUBLIC Eigen3::Eigen ${glog_LIBRARIES} minkindr) +target_link_libraries(core PUBLIC Eigen3::Eigen glog) if (tracy_FOUND) target_link_libraries(core PUBLIC TracyClient) endif () diff --git a/library/src/io/CMakeLists.txt b/library/src/io/CMakeLists.txt index 1026f55dd..08d5a7337 100644 --- a/library/src/io/CMakeLists.txt +++ b/library/src/io/CMakeLists.txt @@ -1,6 +1,5 @@ add_library(io) add_library(wavemap::io ALIAS io) target_sources(io PRIVATE file_conversions.cc stream_conversions.cc) -target_include_directories(io PUBLIC ${glog_INCLUDE_DIRS}) -target_link_libraries(io PUBLIC Eigen3::Eigen ${glog_LIBRARIES}) +target_link_libraries(io PUBLIC Eigen3::Eigen glog) set_wavemap_target_properties(io) From c59f773e98207fa4f83cbc446a5e58b746200512 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 7 May 2024 14:30:43 +0200 Subject: [PATCH 094/159] Enable unit testing in CMake-only projects --- .pre-commit-config.yaml | 16 +++---- interfaces/ros1/wavemap/CMakeLists.txt | 6 +-- library/CMakeLists.txt | 34 +++++-------- library/benchmark/CMakeLists.txt | 8 ++-- ...ler_options.cmake => wavemap-extras.cmake} | 29 ++++++----- library/src/CMakeLists.txt | 12 +++++ library/src/core/CMakeLists.txt | 15 +++--- library/src/io/CMakeLists.txt | 13 +++-- library/src/pipeline/CMakeLists.txt | 0 library/test/CMakeLists.txt | 48 ++----------------- .../include/wavemap/test/geometry_generator.h | 5 +- library/test/src/core/CMakeLists.txt | 48 +++++++++++++++++++ .../core/data_structure/test_aabb.cc | 0 .../core/data_structure/test_image.cc | 0 .../core/data_structure/test_ndtree.cc | 0 .../core/data_structure/test_pointcloud.cc | 9 ++-- .../core/data_structure/test_sparse_vector.cc | 0 .../core/indexing/test_index_conversions.cc | 0 .../core/indexing/test_ndtree_index.cc | 0 .../test_circular_projector.cc | 0 .../projection_model/test_image_projectors.cc | 2 +- .../test_spherical_projector.cc | 0 .../test_hierarchical_range_image.cc | 0 .../integrator/test_measurement_models.cc | 0 .../integrator/test_pointcloud_integrators.cc | 2 +- .../test_range_image_intersector.cc | 2 +- .../test/{ => src}/core/map/test_haar_cell.cc | 0 .../{ => src}/core/map/test_hashed_blocks.cc | 0 library/test/{ => src}/core/map/test_map.cc | 0 .../core/map/test_volumetric_octree.cc | 0 .../core/utils/bits/test_bit_operations.cc | 0 .../core/utils/data/test_comparisons.cc | 0 .../{ => src}/core/utils/data/test_fill.cc | 0 .../core/utils/iterate/test_grid_iterator.cc | 0 .../core/utils/iterate/test_ray_iterator.cc | 0 .../utils/iterate/test_subtree_iterator.cc | 0 .../math/test_approximate_trigonometry.cc | 0 .../core/utils/math/test_int_math.cc | 0 .../core/utils/math/test_tree_math.cc | 0 .../core/utils/neighbors/test_adjacency.cc | 0 .../utils/neighbors/test_grid_adjacency.cc | 0 .../utils/neighbors/test_grid_neighborhood.cc | 0 .../utils/neighbors/test_ndtree_adjacency.cc | 0 .../core/utils/query/test_classified_map.cc | 0 .../utils/query/test_map_interpolator.cpp | 0 .../utils/query/test_occupancy_classifier.cc | 0 .../query/test_probability_conversions.cc | 0 .../utils/query/test_query_accelerator.cc | 6 +-- .../core/utils/sdf/test_sdf_generators.cc | 0 .../{ => src}/core/utils/test_thread_pool.cc | 0 library/test/src/io/CMakeLists.txt | 10 ++++ .../{ => src}/io/test_file_conversions.cc | 2 +- 52 files changed, 149 insertions(+), 118 deletions(-) rename library/cmake/{compiler_options.cmake => wavemap-extras.cmake} (70%) create mode 100644 library/src/CMakeLists.txt delete mode 100644 library/src/pipeline/CMakeLists.txt create mode 100644 library/test/src/core/CMakeLists.txt rename library/test/{ => src}/core/data_structure/test_aabb.cc (100%) rename library/test/{ => src}/core/data_structure/test_image.cc (100%) rename library/test/{ => src}/core/data_structure/test_ndtree.cc (100%) rename library/test/{ => src}/core/data_structure/test_pointcloud.cc (97%) rename library/test/{ => src}/core/data_structure/test_sparse_vector.cc (100%) rename library/test/{ => src}/core/indexing/test_index_conversions.cc (100%) rename library/test/{ => src}/core/indexing/test_ndtree_index.cc (100%) rename library/test/{ => src}/core/integrator/projection_model/test_circular_projector.cc (100%) rename library/test/{ => src}/core/integrator/projection_model/test_image_projectors.cc (99%) rename library/test/{ => src}/core/integrator/projection_model/test_spherical_projector.cc (100%) rename library/test/{ => src}/core/integrator/test_hierarchical_range_image.cc (100%) rename library/test/{ => src}/core/integrator/test_measurement_models.cc (100%) rename library/test/{ => src}/core/integrator/test_pointcloud_integrators.cc (99%) rename library/test/{ => src}/core/integrator/test_range_image_intersector.cc (99%) rename library/test/{ => src}/core/map/test_haar_cell.cc (100%) rename library/test/{ => src}/core/map/test_hashed_blocks.cc (100%) rename library/test/{ => src}/core/map/test_map.cc (100%) rename library/test/{ => src}/core/map/test_volumetric_octree.cc (100%) rename library/test/{ => src}/core/utils/bits/test_bit_operations.cc (100%) rename library/test/{ => src}/core/utils/data/test_comparisons.cc (100%) rename library/test/{ => src}/core/utils/data/test_fill.cc (100%) rename library/test/{ => src}/core/utils/iterate/test_grid_iterator.cc (100%) rename library/test/{ => src}/core/utils/iterate/test_ray_iterator.cc (100%) rename library/test/{ => src}/core/utils/iterate/test_subtree_iterator.cc (100%) rename library/test/{ => src}/core/utils/math/test_approximate_trigonometry.cc (100%) rename library/test/{ => src}/core/utils/math/test_int_math.cc (100%) rename library/test/{ => src}/core/utils/math/test_tree_math.cc (100%) rename library/test/{ => src}/core/utils/neighbors/test_adjacency.cc (100%) rename library/test/{ => src}/core/utils/neighbors/test_grid_adjacency.cc (100%) rename library/test/{ => src}/core/utils/neighbors/test_grid_neighborhood.cc (100%) rename library/test/{ => src}/core/utils/neighbors/test_ndtree_adjacency.cc (100%) rename library/test/{ => src}/core/utils/query/test_classified_map.cc (100%) rename library/test/{ => src}/core/utils/query/test_map_interpolator.cpp (100%) rename library/test/{ => src}/core/utils/query/test_occupancy_classifier.cc (100%) rename library/test/{ => src}/core/utils/query/test_probability_conversions.cc (100%) rename library/test/{ => src}/core/utils/query/test_query_accelerator.cc (95%) rename library/test/{ => src}/core/utils/sdf/test_sdf_generators.cc (100%) rename library/test/{ => src}/core/utils/test_thread_pool.cc (100%) create mode 100644 library/test/src/io/CMakeLists.txt rename library/test/{ => src}/io/test_file_conversions.cc (99%) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index ef2daf4d3..a87c21e1e 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -6,7 +6,7 @@ default_language_version: repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.1.0 + rev: v4.6.0 hooks: - name: Trim trailing whitespaces id: trailing-whitespace @@ -31,13 +31,13 @@ repos: id: check-merge-conflict - repo: https://github.com/google/yapf - rev: v0.31.0 + rev: v0.40.2 hooks: - name: Format python files with yapf id: yapf - repo: https://github.com/PyCQA/pylint - rev: v2.12.2 + rev: v3.1.0 hooks: - name: Lint python files with pylint id: pylint @@ -81,20 +81,20 @@ repos: additional_dependencies: [ pyyaml>=5.1 ] - repo: https://github.com/shellcheck-py/shellcheck-py - rev: v0.8.0.3 + rev: v0.10.0.1 hooks: - name: Check shell scripts with shellcheck id: shellcheck - repo: https://github.com/hadolint/hadolint - rev: v2.8.0 + rev: v2.12.0 hooks: - name: Lint dockerfiles with hadolint id: hadolint args: [ --config=tooling/git_hook_configs/.hadolint.yaml ] - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.24.1 + rev: 0.28.2 hooks: - name: Check GitHub workflow file schema conformance id: check-github-workflows @@ -109,14 +109,14 @@ repos: args: [ --schemafile=tooling/schemas/wavemap/wavemap_config.json ] - repo: https://github.com/markdownlint/markdownlint - rev: v0.11.0 + rev: v0.12.0 hooks: - name: Lint markdown files with markdownlint id: markdownlint args: [ --rules="~MD013" ] - repo: https://github.com/rstcheck/rstcheck - rev: v6.1.2 + rev: v6.2.0 hooks: - name: Check syntax of reStructuredText id: rstcheck diff --git a/interfaces/ros1/wavemap/CMakeLists.txt b/interfaces/ros1/wavemap/CMakeLists.txt index 264f278d4..5cdbd535c 100644 --- a/interfaces/ros1/wavemap/CMakeLists.txt +++ b/interfaces/ros1/wavemap/CMakeLists.txt @@ -6,9 +6,9 @@ find_package(catkin REQUIRED) catkin_package( INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/include - LIBRARIES core io + LIBRARIES wavemap_core wavemap_io CFG_EXTRAS - ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/cmake/compiler_options.cmake + ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/cmake/wavemap-extras.cmake ) # Load the library @@ -16,7 +16,7 @@ add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../../../library ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}) # Install targets -install(TARGETS core io +install(TARGETS wavemap_core wavemap_io ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/library/CMakeLists.txt b/library/CMakeLists.txt index c3f319cc1..f4cde52c2 100644 --- a/library/CMakeLists.txt +++ b/library/CMakeLists.txt @@ -1,36 +1,24 @@ cmake_minimum_required(VERSION 3.10) - project(wavemap VERSION 1.6.3 LANGUAGES CXX) # User options option(USE_CLANG_TIDY "Generate necessary files to run clang-tidy" OFF) -# Dependencies -find_package(Eigen3 REQUIRED NO_MODULE) -find_package(PkgConfig REQUIRED) -pkg_check_modules(glog REQUIRED libglog) - -# Optional dependencies -find_package(tracy QUIET) -# TODO(victorr): Check if tracy is correctly found and linked in when it is -# available as an installed (system) package. - -include(cmake/compiler_options.cmake) +# CMake helpers +include(cmake/wavemap-extras.cmake) # Libraries -add_subdirectory(src/core) -add_subdirectory(src/io) -add_subdirectory(src/pipeline) +add_subdirectory(src) -## Tests -#if (CATKIN_ENABLE_TESTING) -# add_subdirectory(test) -#endif () +# Tests +if (ENABLE_TESTING OR CATKIN_ENABLE_TESTING) + add_subdirectory(test) +endif () -## Benchmarks -#if (ENABLE_BENCHMARKING) -# add_subdirectory(benchmark) -#endif () +# Benchmarks +if (ENABLE_BENCHMARKING) + add_subdirectory(benchmark) +endif () # Tooling if (USE_CLANG_TIDY) diff --git a/library/benchmark/CMakeLists.txt b/library/benchmark/CMakeLists.txt index a1fc38404..1046e5b1b 100644 --- a/library/benchmark/CMakeLists.txt +++ b/library/benchmark/CMakeLists.txt @@ -1,8 +1,8 @@ add_executable(benchmark_haar_transforms benchmark/benchmark_haar_transforms.cc) -target_link_libraries(benchmark_haar_transforms ${PROJECT_NAME} - benchmark::benchmark) +target_link_libraries(benchmark_haar_transforms + wavemap_core benchmark::benchmark) add_executable(benchmark_sparse_vector benchmark/benchmark_sparse_vector.cc) -target_link_libraries(benchmark_sparse_vector ${PROJECT_NAME} - benchmark::benchmark) +target_link_libraries(benchmark_sparse_vector + wavemap_core benchmark::benchmark) diff --git a/library/cmake/compiler_options.cmake b/library/cmake/wavemap-extras.cmake similarity index 70% rename from library/cmake/compiler_options.cmake rename to library/cmake/wavemap-extras.cmake index d6790d0b7..49705e5cd 100644 --- a/library/cmake/compiler_options.cmake +++ b/library/cmake/wavemap-extras.cmake @@ -7,16 +7,22 @@ option(USE_TSAN "Compile with thread sanitizer enabled" OFF) option(ENABLE_COVERAGE_TESTING "Compile with necessary flags for coverage testing" OFF) -# This function is used to set the compile features, definitions, options and -# include paths for all targets in the wavemap library. -function(set_wavemap_target_properties target) +# Adds the include paths of the wavemap library to the given target. +function(add_wavemap_include_directories target) # Configure the include dirs set(include_dirs ${PROJECT_SOURCE_DIR}/include/wavemap) get_filename_component(include_dirs ${include_dirs} PATH) target_include_directories(${target} PUBLIC $ $) +endfunction() +# Adds the default wavemap library compile features, definitions and options +# to the given target. We use it to ensure that all targets in the wavemap +# library are configured consistently. The function can also be called for +# targets that depend on wavemap (e.g. in a downstream package), to ensure +# their configuration is compatible. +function(set_wavemap_target_properties target) # Require C++17 target_compile_features(${target} PUBLIC cxx_std_17) set_target_properties(${target} PROPERTIES CXX_EXTENSIONS OFF) @@ -36,7 +42,7 @@ function(set_wavemap_target_properties target) target_compile_definitions(${target} PUBLIC DCHECK_ALWAYS_ON) endif () if (USE_UBSAN) - target_compile_options(${target} + target_compile_options(${target} PUBLIC -fsanitize=undefined -fsanitize=shift -fsanitize=integer-divide-by-zero @@ -53,21 +59,22 @@ function(set_wavemap_target_properties target) -fsanitize=builtin -fno-omit-frame-pointer -g) - add_link_options(-fsanitize=undefined) + target_link_options(${target} PUBLIC -fsanitize=undefined) endif () if (USE_ASAN) - target_compile_options(${target} + target_compile_options(${target} PUBLIC -fsanitize=address -fsanitize-address-use-after-scope -fno-omit-frame-pointer -g) - target_link_options(${target} -fsanitize=address) + target_link_options(${target} PUBLIC -fsanitize=address) endif () if (USE_TSAN) - target_compile_options(${target} + target_compile_options(${target} PUBLIC -fsanitize=thread -fno-omit-frame-pointer -g) - target_link_options(${target} -fsanitize=thread) + target_link_options(${target} PUBLIC -fsanitize=thread) endif () if (ENABLE_COVERAGE_TESTING) - target_compile_options(${target} -fprofile-arcs -ftest-coverage -O0 -g) - target_link_options(${target} -fprofile-arcs) + target_compile_options(${target} PUBLIC + -fprofile-arcs -ftest-coverage -O0 -g) + target_link_options(${target} PUBLIC -fprofile-arcs) endif () endfunction() diff --git a/library/src/CMakeLists.txt b/library/src/CMakeLists.txt new file mode 100644 index 000000000..2c5444ad0 --- /dev/null +++ b/library/src/CMakeLists.txt @@ -0,0 +1,12 @@ +# Dependencies +find_package(Eigen3 REQUIRED NO_MODULE) +find_package(PkgConfig REQUIRED) +pkg_check_modules(glog REQUIRED libglog) + +# Optional dependencies +find_package(tracy QUIET) +# TODO(victorr): Check if tracy is correctly found and linked in when it is +# available as an installed (system) package. + +add_subdirectory(core) +add_subdirectory(io) diff --git a/library/src/core/CMakeLists.txt b/library/src/core/CMakeLists.txt index 07e0af2e8..3eaabd793 100644 --- a/library/src/core/CMakeLists.txt +++ b/library/src/core/CMakeLists.txt @@ -1,6 +1,8 @@ -add_library(core) -add_library(wavemap::core ALIAS core) -target_sources(core PRIVATE +add_library(wavemap_core) +add_library(wavemap::wavemap_core ALIAS wavemap_core) + +add_wavemap_include_directories(wavemap_core) +target_sources(wavemap_core PRIVATE config/value_with_unit.cc integrator/measurement_model/continuous_beam.cc integrator/measurement_model/constant_ray.cc @@ -36,8 +38,9 @@ target_sources(core PRIVATE utils/sdf/quasi_euclidean_sdf_generator.cc utils/time/stopwatch.cc utils/thread_pool.cc) -target_link_libraries(core PUBLIC Eigen3::Eigen glog) + +set_wavemap_target_properties(wavemap_core) +target_link_libraries(wavemap_core PUBLIC Eigen3::Eigen glog) if (tracy_FOUND) - target_link_libraries(core PUBLIC TracyClient) + target_link_libraries(wavemap_core PUBLIC TracyClient) endif () -set_wavemap_target_properties(core) diff --git a/library/src/io/CMakeLists.txt b/library/src/io/CMakeLists.txt index 08d5a7337..1f5ae5cd3 100644 --- a/library/src/io/CMakeLists.txt +++ b/library/src/io/CMakeLists.txt @@ -1,5 +1,8 @@ -add_library(io) -add_library(wavemap::io ALIAS io) -target_sources(io PRIVATE file_conversions.cc stream_conversions.cc) -target_link_libraries(io PUBLIC Eigen3::Eigen glog) -set_wavemap_target_properties(io) +add_library(wavemap_io) +add_library(wavemap::wavemap_io ALIAS wavemap_io) + +add_wavemap_include_directories(wavemap_io) +target_sources(wavemap_io PRIVATE file_conversions.cc stream_conversions.cc) + +set_wavemap_target_properties(wavemap_io) +target_link_libraries(wavemap_io PUBLIC Eigen3::Eigen glog) diff --git a/library/src/pipeline/CMakeLists.txt b/library/src/pipeline/CMakeLists.txt deleted file mode 100644 index e69de29bb..000000000 diff --git a/library/test/CMakeLists.txt b/library/test/CMakeLists.txt index 0d0a26162..03dce2654 100644 --- a/library/test/CMakeLists.txt +++ b/library/test/CMakeLists.txt @@ -1,43 +1,5 @@ -catkin_add_gtest( - test_${PROJECT_NAME} - test/src/data_structure/test_aabb.cc - test/src/data_structure/test_image.cc - test/src/data_structure/test_ndtree.cc - test/src/data_structure/test_pointcloud.cc - test/src/data_structure/test_sparse_vector.cc - test/src/indexing/test_index_conversions.cc - test/src/indexing/test_ndtree_index.cc - test/src/integrator/projection_model/test_circular_projector.cc - test/src/integrator/projection_model/test_image_projectors.cc - test/src/integrator/projection_model/test_spherical_projector.cc - test/src/integrator/test_hierarchical_range_image.cc - test/src/integrator/test_measurement_models.cc - test/src/integrator/test_pointcloud_integrators.cc - test/src/integrator/test_range_image_intersector.cc - test/src/io/test_file_conversions.cc - test/src/map/test_haar_cell.cc - test/src/map/test_hashed_blocks.cc - test/src/map/test_map.cc - test/src/map/test_volumetric_octree.cc - test/src/utils/bits/test_bit_operations.cc - test/src/utils/data/test_comparisons.cc - test/src/utils/data/test_fill.cc - test/src/utils/iterate/test_grid_iterator.cc - test/src/utils/iterate/test_ray_iterator.cc - test/src/utils/iterate/test_subtree_iterator.cc - test/src/utils/math/test_approximate_trigonometry.cc - test/src/utils/math/test_int_math.cc - test/src/utils/math/test_tree_math.cc - test/src/utils/neighbors/test_adjacency.cc - test/src/utils/neighbors/test_grid_adjacency.cc - test/src/utils/neighbors/test_grid_neighborhood.cc - test/src/utils/neighbors/test_ndtree_adjacency.cc - test/src/utils/query/test_classified_map.cc - test/src/utils/query/test_map_interpolator.cpp - test/src/utils/query/test_occupancy_classifier.cc - test/src/utils/query/test_probability_conversions.cc - test/src/utils/query/test_query_accelerator.cc - test/src/utils/sdf/test_sdf_generators.cc - test/src/utils/test_thread_pool.cc) -target_include_directories(test_${PROJECT_NAME} PRIVATE test/include) -target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} gtest_main) +find_package(GTest REQUIRED) +include(GoogleTest) + +add_subdirectory(src/core) +add_subdirectory(src/io) diff --git a/library/test/include/wavemap/test/geometry_generator.h b/library/test/include/wavemap/test/geometry_generator.h index accd34530..ee2c2f0c3 100644 --- a/library/test/include/wavemap/test/geometry_generator.h +++ b/library/test/include/wavemap/test/geometry_generator.h @@ -59,9 +59,8 @@ class GeometryGenerator { return random_number_generator_.getRandomRealNumber(min_angle, max_angle); } - template - Transformation getRandomTransformation() const { - Transformation random_transformation; + static Transformation3D getRandomTransformation() { + Transformation3D random_transformation; random_transformation.setRandom(); return random_transformation; } diff --git a/library/test/src/core/CMakeLists.txt b/library/test/src/core/CMakeLists.txt new file mode 100644 index 000000000..e0a8e94a0 --- /dev/null +++ b/library/test/src/core/CMakeLists.txt @@ -0,0 +1,48 @@ +add_executable(test_wavemap_core) + +target_include_directories(test_wavemap_core PRIVATE + ${PROJECT_SOURCE_DIR}/test/include) +target_sources(test_wavemap_core PRIVATE + data_structure/test_aabb.cc + data_structure/test_image.cc + data_structure/test_ndtree.cc + data_structure/test_pointcloud.cc + data_structure/test_sparse_vector.cc + indexing/test_index_conversions.cc + indexing/test_ndtree_index.cc + integrator/projection_model/test_circular_projector.cc + integrator/projection_model/test_image_projectors.cc + integrator/projection_model/test_spherical_projector.cc + integrator/test_hierarchical_range_image.cc + integrator/test_measurement_models.cc + integrator/test_pointcloud_integrators.cc + integrator/test_range_image_intersector.cc + map/test_haar_cell.cc + map/test_hashed_blocks.cc + map/test_map.cc + map/test_volumetric_octree.cc + utils/bits/test_bit_operations.cc + utils/data/test_comparisons.cc + utils/data/test_fill.cc + utils/iterate/test_grid_iterator.cc + utils/iterate/test_ray_iterator.cc + utils/iterate/test_subtree_iterator.cc + utils/math/test_approximate_trigonometry.cc + utils/math/test_int_math.cc + utils/math/test_tree_math.cc + utils/neighbors/test_adjacency.cc + utils/neighbors/test_grid_adjacency.cc + utils/neighbors/test_grid_neighborhood.cc + utils/neighbors/test_ndtree_adjacency.cc + utils/query/test_classified_map.cc + utils/query/test_map_interpolator.cpp + utils/query/test_occupancy_classifier.cc + utils/query/test_probability_conversions.cc + utils/query/test_query_accelerator.cc + utils/sdf/test_sdf_generators.cc + utils/test_thread_pool.cc) + +set_wavemap_target_properties(test_wavemap_core) +target_link_libraries(test_wavemap_core wavemap_core GTest::gtest_main) + +gtest_discover_tests(test_wavemap_core) diff --git a/library/test/core/data_structure/test_aabb.cc b/library/test/src/core/data_structure/test_aabb.cc similarity index 100% rename from library/test/core/data_structure/test_aabb.cc rename to library/test/src/core/data_structure/test_aabb.cc diff --git a/library/test/core/data_structure/test_image.cc b/library/test/src/core/data_structure/test_image.cc similarity index 100% rename from library/test/core/data_structure/test_image.cc rename to library/test/src/core/data_structure/test_image.cc diff --git a/library/test/core/data_structure/test_ndtree.cc b/library/test/src/core/data_structure/test_ndtree.cc similarity index 100% rename from library/test/core/data_structure/test_ndtree.cc rename to library/test/src/core/data_structure/test_ndtree.cc diff --git a/library/test/core/data_structure/test_pointcloud.cc b/library/test/src/core/data_structure/test_pointcloud.cc similarity index 97% rename from library/test/core/data_structure/test_pointcloud.cc rename to library/test/src/core/data_structure/test_pointcloud.cc index 665ec04e6..e44d06c98 100644 --- a/library/test/core/data_structure/test_pointcloud.cc +++ b/library/test/src/core/data_structure/test_pointcloud.cc @@ -167,12 +167,12 @@ TYPED_TEST_SUITE(PosedPointcloudTest, PointTypes, ); TYPED_TEST(PosedPointcloudTest, InitializationAndCopying) { constexpr int kDim = dim_v; using PointType = TypeParam; - using PoseType = Transformation; + using PoseType = Transformation3D; constexpr int kNumRepetitions = 100; for (int i = 0; i < kNumRepetitions; ++i) { // Initialize const PoseType random_transformation = - GeometryGenerator::getRandomTransformation(); + GeometryGenerator::getRandomTransformation(); const Pointcloud random_pointcloud( GeometryGenerator::getRandomPointVector()); PosedPointcloud random_posed_pointcloud(random_transformation, @@ -200,13 +200,12 @@ TYPED_TEST(PosedPointcloudTest, InitializationAndCopying) { TYPED_TEST(PosedPointcloudTest, Transformations) { constexpr int kDim = dim_v; using PointType = TypeParam; - using PoseType = Transformation; + using PoseType = Transformation3D; constexpr int kNumRepetitions = 100; for (int i = 0; i < kNumRepetitions; ++i) { const std::vector random_points_C = GeometryGenerator::getRandomPointVector(); - const PoseType random_T_W_C = - GeometryGenerator::getRandomTransformation(); + const PoseType random_T_W_C = GeometryGenerator::getRandomTransformation(); const PosedPointcloud random_posed_pointcloud( random_T_W_C, Pointcloud(random_points_C)); diff --git a/library/test/core/data_structure/test_sparse_vector.cc b/library/test/src/core/data_structure/test_sparse_vector.cc similarity index 100% rename from library/test/core/data_structure/test_sparse_vector.cc rename to library/test/src/core/data_structure/test_sparse_vector.cc diff --git a/library/test/core/indexing/test_index_conversions.cc b/library/test/src/core/indexing/test_index_conversions.cc similarity index 100% rename from library/test/core/indexing/test_index_conversions.cc rename to library/test/src/core/indexing/test_index_conversions.cc diff --git a/library/test/core/indexing/test_ndtree_index.cc b/library/test/src/core/indexing/test_ndtree_index.cc similarity index 100% rename from library/test/core/indexing/test_ndtree_index.cc rename to library/test/src/core/indexing/test_ndtree_index.cc diff --git a/library/test/core/integrator/projection_model/test_circular_projector.cc b/library/test/src/core/integrator/projection_model/test_circular_projector.cc similarity index 100% rename from library/test/core/integrator/projection_model/test_circular_projector.cc rename to library/test/src/core/integrator/projection_model/test_circular_projector.cc diff --git a/library/test/core/integrator/projection_model/test_image_projectors.cc b/library/test/src/core/integrator/projection_model/test_image_projectors.cc similarity index 99% rename from library/test/core/integrator/projection_model/test_image_projectors.cc rename to library/test/src/core/integrator/projection_model/test_image_projectors.cc index bce57fcaf..0f3ae257a 100644 --- a/library/test/core/integrator/projection_model/test_image_projectors.cc +++ b/library/test/src/core/integrator/projection_model/test_image_projectors.cc @@ -118,7 +118,7 @@ class Image2DProjectorTest : public FixtureBase, public GeometryGenerator { aabb.max + t_random}; const AABB aabb_scaled_translated{ aabb_scaled.min + t_random, aabb_scaled.max + t_random}; - const Transformation3D T_W_C_random = getRandomTransformation<3>(); + const Transformation3D T_W_C_random = getRandomTransformation(); aabbs_and_poses.emplace_back(aabb_scaled, Transformation3D()); aabbs_and_poses.emplace_back(aabb_translated, Transformation3D()); aabbs_and_poses.emplace_back(aabb_scaled_translated, diff --git a/library/test/core/integrator/projection_model/test_spherical_projector.cc b/library/test/src/core/integrator/projection_model/test_spherical_projector.cc similarity index 100% rename from library/test/core/integrator/projection_model/test_spherical_projector.cc rename to library/test/src/core/integrator/projection_model/test_spherical_projector.cc diff --git a/library/test/core/integrator/test_hierarchical_range_image.cc b/library/test/src/core/integrator/test_hierarchical_range_image.cc similarity index 100% rename from library/test/core/integrator/test_hierarchical_range_image.cc rename to library/test/src/core/integrator/test_hierarchical_range_image.cc diff --git a/library/test/core/integrator/test_measurement_models.cc b/library/test/src/core/integrator/test_measurement_models.cc similarity index 100% rename from library/test/core/integrator/test_measurement_models.cc rename to library/test/src/core/integrator/test_measurement_models.cc diff --git a/library/test/core/integrator/test_pointcloud_integrators.cc b/library/test/src/core/integrator/test_pointcloud_integrators.cc similarity index 99% rename from library/test/core/integrator/test_pointcloud_integrators.cc rename to library/test/src/core/integrator/test_pointcloud_integrators.cc index 7d21f4a23..0447a25cb 100644 --- a/library/test/core/integrator/test_pointcloud_integrators.cc +++ b/library/test/src/core/integrator/test_pointcloud_integrators.cc @@ -51,7 +51,7 @@ class PointcloudIntegratorTest : public FixtureBase, {projection_model.indexToImage(image_index), 1.f}); } - return PosedPointcloud<>(getRandomTransformation<3>(), pointcloud); + return PosedPointcloud<>(getRandomTransformation(), pointcloud); } }; diff --git a/library/test/core/integrator/test_range_image_intersector.cc b/library/test/src/core/integrator/test_range_image_intersector.cc similarity index 99% rename from library/test/core/integrator/test_range_image_intersector.cc rename to library/test/src/core/integrator/test_range_image_intersector.cc index 79e27152f..a2fb8b673 100644 --- a/library/test/core/integrator/test_range_image_intersector.cc +++ b/library/test/src/core/integrator/test_range_image_intersector.cc @@ -58,7 +58,7 @@ class RangeImage2DIntersectorTest : public FixtureBase, const FloatingPoint range = getRandomSignedDistance(min_range, max_range); posed_range_image.at(index) = range; } - posed_range_image.setPose(getRandomTransformation<3>()); + posed_range_image.setPose(getRandomTransformation()); return posed_range_image; } diff --git a/library/test/core/map/test_haar_cell.cc b/library/test/src/core/map/test_haar_cell.cc similarity index 100% rename from library/test/core/map/test_haar_cell.cc rename to library/test/src/core/map/test_haar_cell.cc diff --git a/library/test/core/map/test_hashed_blocks.cc b/library/test/src/core/map/test_hashed_blocks.cc similarity index 100% rename from library/test/core/map/test_hashed_blocks.cc rename to library/test/src/core/map/test_hashed_blocks.cc diff --git a/library/test/core/map/test_map.cc b/library/test/src/core/map/test_map.cc similarity index 100% rename from library/test/core/map/test_map.cc rename to library/test/src/core/map/test_map.cc diff --git a/library/test/core/map/test_volumetric_octree.cc b/library/test/src/core/map/test_volumetric_octree.cc similarity index 100% rename from library/test/core/map/test_volumetric_octree.cc rename to library/test/src/core/map/test_volumetric_octree.cc diff --git a/library/test/core/utils/bits/test_bit_operations.cc b/library/test/src/core/utils/bits/test_bit_operations.cc similarity index 100% rename from library/test/core/utils/bits/test_bit_operations.cc rename to library/test/src/core/utils/bits/test_bit_operations.cc diff --git a/library/test/core/utils/data/test_comparisons.cc b/library/test/src/core/utils/data/test_comparisons.cc similarity index 100% rename from library/test/core/utils/data/test_comparisons.cc rename to library/test/src/core/utils/data/test_comparisons.cc diff --git a/library/test/core/utils/data/test_fill.cc b/library/test/src/core/utils/data/test_fill.cc similarity index 100% rename from library/test/core/utils/data/test_fill.cc rename to library/test/src/core/utils/data/test_fill.cc diff --git a/library/test/core/utils/iterate/test_grid_iterator.cc b/library/test/src/core/utils/iterate/test_grid_iterator.cc similarity index 100% rename from library/test/core/utils/iterate/test_grid_iterator.cc rename to library/test/src/core/utils/iterate/test_grid_iterator.cc diff --git a/library/test/core/utils/iterate/test_ray_iterator.cc b/library/test/src/core/utils/iterate/test_ray_iterator.cc similarity index 100% rename from library/test/core/utils/iterate/test_ray_iterator.cc rename to library/test/src/core/utils/iterate/test_ray_iterator.cc diff --git a/library/test/core/utils/iterate/test_subtree_iterator.cc b/library/test/src/core/utils/iterate/test_subtree_iterator.cc similarity index 100% rename from library/test/core/utils/iterate/test_subtree_iterator.cc rename to library/test/src/core/utils/iterate/test_subtree_iterator.cc diff --git a/library/test/core/utils/math/test_approximate_trigonometry.cc b/library/test/src/core/utils/math/test_approximate_trigonometry.cc similarity index 100% rename from library/test/core/utils/math/test_approximate_trigonometry.cc rename to library/test/src/core/utils/math/test_approximate_trigonometry.cc diff --git a/library/test/core/utils/math/test_int_math.cc b/library/test/src/core/utils/math/test_int_math.cc similarity index 100% rename from library/test/core/utils/math/test_int_math.cc rename to library/test/src/core/utils/math/test_int_math.cc diff --git a/library/test/core/utils/math/test_tree_math.cc b/library/test/src/core/utils/math/test_tree_math.cc similarity index 100% rename from library/test/core/utils/math/test_tree_math.cc rename to library/test/src/core/utils/math/test_tree_math.cc diff --git a/library/test/core/utils/neighbors/test_adjacency.cc b/library/test/src/core/utils/neighbors/test_adjacency.cc similarity index 100% rename from library/test/core/utils/neighbors/test_adjacency.cc rename to library/test/src/core/utils/neighbors/test_adjacency.cc diff --git a/library/test/core/utils/neighbors/test_grid_adjacency.cc b/library/test/src/core/utils/neighbors/test_grid_adjacency.cc similarity index 100% rename from library/test/core/utils/neighbors/test_grid_adjacency.cc rename to library/test/src/core/utils/neighbors/test_grid_adjacency.cc diff --git a/library/test/core/utils/neighbors/test_grid_neighborhood.cc b/library/test/src/core/utils/neighbors/test_grid_neighborhood.cc similarity index 100% rename from library/test/core/utils/neighbors/test_grid_neighborhood.cc rename to library/test/src/core/utils/neighbors/test_grid_neighborhood.cc diff --git a/library/test/core/utils/neighbors/test_ndtree_adjacency.cc b/library/test/src/core/utils/neighbors/test_ndtree_adjacency.cc similarity index 100% rename from library/test/core/utils/neighbors/test_ndtree_adjacency.cc rename to library/test/src/core/utils/neighbors/test_ndtree_adjacency.cc diff --git a/library/test/core/utils/query/test_classified_map.cc b/library/test/src/core/utils/query/test_classified_map.cc similarity index 100% rename from library/test/core/utils/query/test_classified_map.cc rename to library/test/src/core/utils/query/test_classified_map.cc diff --git a/library/test/core/utils/query/test_map_interpolator.cpp b/library/test/src/core/utils/query/test_map_interpolator.cpp similarity index 100% rename from library/test/core/utils/query/test_map_interpolator.cpp rename to library/test/src/core/utils/query/test_map_interpolator.cpp diff --git a/library/test/core/utils/query/test_occupancy_classifier.cc b/library/test/src/core/utils/query/test_occupancy_classifier.cc similarity index 100% rename from library/test/core/utils/query/test_occupancy_classifier.cc rename to library/test/src/core/utils/query/test_occupancy_classifier.cc diff --git a/library/test/core/utils/query/test_probability_conversions.cc b/library/test/src/core/utils/query/test_probability_conversions.cc similarity index 100% rename from library/test/core/utils/query/test_probability_conversions.cc rename to library/test/src/core/utils/query/test_probability_conversions.cc diff --git a/library/test/core/utils/query/test_query_accelerator.cc b/library/test/src/core/utils/query/test_query_accelerator.cc similarity index 95% rename from library/test/core/utils/query/test_query_accelerator.cc rename to library/test/src/core/utils/query/test_query_accelerator.cc index a91dc17f6..a09505151 100644 --- a/library/test/core/utils/query/test_query_accelerator.cc +++ b/library/test/src/core/utils/query/test_query_accelerator.cc @@ -1,7 +1,7 @@ #include -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/library/test/core/utils/sdf/test_sdf_generators.cc b/library/test/src/core/utils/sdf/test_sdf_generators.cc similarity index 100% rename from library/test/core/utils/sdf/test_sdf_generators.cc rename to library/test/src/core/utils/sdf/test_sdf_generators.cc diff --git a/library/test/core/utils/test_thread_pool.cc b/library/test/src/core/utils/test_thread_pool.cc similarity index 100% rename from library/test/core/utils/test_thread_pool.cc rename to library/test/src/core/utils/test_thread_pool.cc diff --git a/library/test/src/io/CMakeLists.txt b/library/test/src/io/CMakeLists.txt new file mode 100644 index 000000000..5cf212161 --- /dev/null +++ b/library/test/src/io/CMakeLists.txt @@ -0,0 +1,10 @@ +add_executable(test_wavemap_io) + +target_include_directories(test_wavemap_io PRIVATE + ${PROJECT_SOURCE_DIR}/test/include) +target_sources(test_wavemap_io PRIVATE test_file_conversions.cc) + +set_wavemap_target_properties(test_wavemap_io) +target_link_libraries(test_wavemap_io wavemap_core wavemap_io GTest::gtest_main) + +gtest_discover_tests(test_wavemap_io) diff --git a/library/test/io/test_file_conversions.cc b/library/test/src/io/test_file_conversions.cc similarity index 99% rename from library/test/io/test_file_conversions.cc rename to library/test/src/io/test_file_conversions.cc index 03490d929..b22805d96 100644 --- a/library/test/io/test_file_conversions.cc +++ b/library/test/src/io/test_file_conversions.cc @@ -1,11 +1,11 @@ #include #include "wavemap/core/common.h" -#include "wavemap/core/io/file_conversions.h" #include "wavemap/core/map/hashed_chunked_wavelet_octree.h" #include "wavemap/core/map/hashed_wavelet_octree.h" #include "wavemap/core/map/map_base.h" #include "wavemap/core/map/wavelet_octree.h" +#include "wavemap/io/file_conversions.h" #include "wavemap/test/config_generator.h" #include "wavemap/test/fixture_base.h" #include "wavemap/test/geometry_generator.h" From 13d3385efdd8ae492d9a058ab584a0e2ea2824e7 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 7 May 2024 14:56:58 +0200 Subject: [PATCH 095/159] Update unit tests in catkin packages and tidy up CMake files --- examples/CMakeLists.txt | 10 +++++++--- interfaces/ros1/wavemap/CMakeLists.txt | 3 +-- .../ros1/wavemap_ros/scripts/csv_to_tf.py | 1 + .../wavemap_ros_conversions/CMakeLists.txt | 5 ++++- .../test/src/test_map_msg_conversions.cc | 10 +++++----- .../ros1/wavemap_rviz_plugin/CMakeLists.txt | 19 +++++++++---------- 6 files changed, 27 insertions(+), 21 deletions(-) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 819700067..3c803d4a0 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -4,9 +4,6 @@ project(wavemap_examples) find_package(catkin_simple REQUIRED) catkin_simple(ALL_DEPS_REQUIRED) -# Compiler definitions and options -add_compile_options(-Wno-suggest-attribute=const) - # For all targets include_directories(include) @@ -15,14 +12,17 @@ cs_add_executable(save_map_to_file src/io/save_map_to_file.cc) set_wavemap_target_properties(save_map_to_file) target_link_libraries(save_map_to_file glog) + cs_add_executable(load_map_from_file src/io/load_map_from_file.cc) set_wavemap_target_properties(load_map_from_file) target_link_libraries(load_map_from_file glog) + cs_add_executable(receive_map_over_ros src/io/receive_map_over_ros.cc) set_wavemap_target_properties(receive_map_over_ros) target_link_libraries(receive_map_over_ros glog) + cs_add_executable(send_map_over_ros src/io/send_map_over_ros.cc) set_wavemap_target_properties(send_map_over_ros) @@ -32,10 +32,12 @@ cs_add_executable(fixed_resolution src/queries/fixed_resolution.cc) set_wavemap_target_properties(fixed_resolution) target_link_libraries(fixed_resolution glog) + cs_add_executable(multi_resolution src/queries/multi_resolution.cc) set_wavemap_target_properties(multi_resolution) target_link_libraries(multi_resolution glog) + cs_add_executable(accelerated_queries src/queries/accelerated_queries.cc) set_wavemap_target_properties(accelerated_queries) @@ -45,6 +47,7 @@ cs_add_executable(nearest_neighbor_interpolation src/queries/nearest_neighbor_interpolation.cc) set_wavemap_target_properties(nearest_neighbor_interpolation) target_link_libraries(nearest_neighbor_interpolation glog) + cs_add_executable(trilinear_interpolation src/queries/trilinear_interpolation.cc) set_wavemap_target_properties(trilinear_interpolation) @@ -54,6 +57,7 @@ cs_add_executable(classification src/queries/classification.cc) set_wavemap_target_properties(classification) target_link_libraries(classification glog) +target_compile_options(classification PRIVATE -Wno-suggest-attribute=const) cs_add_executable(occupancy_to_esdf src/planning/occupancy_to_esdf.cc) diff --git a/interfaces/ros1/wavemap/CMakeLists.txt b/interfaces/ros1/wavemap/CMakeLists.txt index 5cdbd535c..c6fe55ade 100644 --- a/interfaces/ros1/wavemap/CMakeLists.txt +++ b/interfaces/ros1/wavemap/CMakeLists.txt @@ -8,8 +8,7 @@ catkin_package( ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/include LIBRARIES wavemap_core wavemap_io CFG_EXTRAS - ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/cmake/wavemap-extras.cmake -) + ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/cmake/wavemap-extras.cmake) # Load the library add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../../../library diff --git a/interfaces/ros1/wavemap_ros/scripts/csv_to_tf.py b/interfaces/ros1/wavemap_ros/scripts/csv_to_tf.py index ec3f05ffb..28e15bbce 100755 --- a/interfaces/ros1/wavemap_ros/scripts/csv_to_tf.py +++ b/interfaces/ros1/wavemap_ros/scripts/csv_to_tf.py @@ -6,6 +6,7 @@ class StampedTransform: + def __init__(self, csv_row): self.stamp = rospy.Time(secs=int(csv_row['#sec']), nsecs=int(csv_row['nsec'])) diff --git a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt index 626d0d03a..2ad1b2829 100644 --- a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt @@ -16,8 +16,11 @@ if (CATKIN_ENABLE_TESTING) catkin_add_gtest( test_${PROJECT_NAME} test/src/test_map_msg_conversions.cc) - target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} gtest_main) set_wavemap_target_properties(test_${PROJECT_NAME}) + target_include_directories(test_${PROJECT_NAME} PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/test/include) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} glog GTest::gtest_main) endif () # Export diff --git a/interfaces/ros1/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc b/interfaces/ros1/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc index f94bc8407..eec4a4baf 100644 --- a/interfaces/ros1/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc +++ b/interfaces/ros1/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc @@ -1,9 +1,9 @@ #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include diff --git a/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt b/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt index 208996c83..86663de79 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt +++ b/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt @@ -13,10 +13,6 @@ find_package(catkin wavemap_msgs wavemap_ros_conversions) -# Disable some warnings coming from Qt code in combination with C++17 -# TODO(victorr): Only add to relevant target, not globally -add_compile_options(-Wno-pedantic -Wno-register) - # Setup catkin package catkin_package(CATKIN_DEPENDS wavemap wavemap_msgs wavemap_ros_conversions) @@ -52,11 +48,6 @@ else () QT5_WRAP_CPP(QT_MOC ${HEADERS_TO_MOC}) endif () -# Avoid Qt signals and slots defining "emit", "slots", etc. -# because they can conflict with boost signals. -# TODO(victorr): Only add to relevant target, not globally -add_definitions(-DQT_NO_KEYWORDS) - # Libraries add_library(${PROJECT_NAME} src/utils/color_conversions.cc @@ -68,7 +59,15 @@ add_library(${PROJECT_NAME} src/wavemap_map_display.cc ${QT_MOC}) set_wavemap_target_properties(${PROJECT_NAME}) -target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} PUBLIC + ${QT_LIBRARIES} ${catkin_LIBRARIES}) + +# Avoid Qt signals and slots defining "emit", "slots", etc. +# because they can conflict with boost signals. +target_compile_definitions(${PROJECT_NAME} PUBLIC -DQT_NO_KEYWORDS) + +# Disable some warnings coming from Qt code in combination with C++17 +target_compile_options(${PROJECT_NAME} PRIVATE -Wno-pedantic -Wno-register) # Install install(TARGETS From ed35ebf022799f198af6cd47430251638c7fd1c6 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 7 May 2024 15:16:53 +0200 Subject: [PATCH 096/159] Auto-select supported packages in toplevel CMakeLists (for IDEs) --- CMakeLists.txt | 47 ++++++++++++++++++++++++++++++++--------------- 1 file changed, 32 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 124663679..99288abe2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,23 +1,40 @@ cmake_minimum_required(VERSION 3.0.2) -project(wavemap) +project(wavemap_ide) # NOTE: This CMakeLists file is not used by ROS and catkin, it only exists to # make it easy to load and edit all packages simultaneously in IDEs. -# Include the catkin workspace's include dir, e.g. for ros msg definitions -execute_process(COMMAND catkin locate --devel - OUTPUT_STRIP_TRAILING_WHITESPACE OUTPUT_VARIABLE CATKIN_WS_DEVEL_PATH) -include_directories(${CATKIN_WS_DEVEL_PATH}/include) +if ("$ENV{ROS_VERSION}" STREQUAL "1") + # Load as a ROS1 project + # NOTE: In this mode, introspection is available for + # the wavemap library and ROS1 interfaces. -## Libraries -#add_subdirectory(library) + # Include the catkin workspace's include dir, e.g. for ros msg definitions + execute_process(COMMAND catkin locate --devel + OUTPUT_STRIP_TRAILING_WHITESPACE OUTPUT_VARIABLE CATKIN_WS_DEVEL_PATH) + include_directories(${CATKIN_WS_DEVEL_PATH}/include) -# ROS interfaces and tooling -add_subdirectory(interfaces/ros1/wavemap) -add_subdirectory(interfaces/ros1/wavemap_msgs) -add_subdirectory(interfaces/ros1/wavemap_ros_conversions) -add_subdirectory(interfaces/ros1/wavemap_ros) -add_subdirectory(interfaces/ros1/wavemap_rviz_plugin) -## Usage examples -add_subdirectory(examples) + # ROS interfaces and tooling + add_subdirectory(interfaces/ros1/wavemap) + add_subdirectory(interfaces/ros1/wavemap_msgs) + add_subdirectory(interfaces/ros1/wavemap_ros_conversions) + add_subdirectory(interfaces/ros1/wavemap_ros) + add_subdirectory(interfaces/ros1/wavemap_rviz_plugin) + + ## Usage examples + add_subdirectory(examples) + +elseif ("$ENV{ROS_VERSION}" STREQUAL "2") + # Load as a ROS2 project + # NOTE: In this mode, introspection is available for + # the wavemap library and ROS2 interfaces. + # TODO(victorr): Populate this once the ROS2 interface is under construction + +else () + # Load in pure CMake mode + # NOTE: In this mode, introspection is available + # only for the wavemap library. + add_subdirectory(library) + +endif () From 44705cbfc640ac1543c7bf4580df07e7ab1c45ab Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 8 Jul 2024 19:40:31 +0200 Subject: [PATCH 097/159] Minor minkindr code cleanup --- .../wavemap/3rd_party/minkindr/angle-axis.h | 10 +++------- .../include/wavemap/3rd_party/minkindr/common.h | 8 ++------ .../3rd_party/minkindr/impl/angle-axis-inl.h | 12 +++++------- .../minkindr/impl/quat-transformation-inl.h | 10 ++++------ .../minkindr/impl/rotation-quaternion-inl.h | 12 +++++------- .../include/wavemap/3rd_party/minkindr/position.h | 8 ++------ .../3rd_party/minkindr/quat-transformation.h | 14 ++++++-------- .../3rd_party/minkindr/rotation-quaternion.h | 9 +++------ 8 files changed, 30 insertions(+), 53 deletions(-) diff --git a/library/include/wavemap/3rd_party/minkindr/angle-axis.h b/library/include/wavemap/3rd_party/minkindr/angle-axis.h index b3c6c7563..e81e42c6b 100644 --- a/library/include/wavemap/3rd_party/minkindr/angle-axis.h +++ b/library/include/wavemap/3rd_party/minkindr/angle-axis.h @@ -29,9 +29,7 @@ #include -namespace kindr { -namespace minimal { - +namespace kindr::minimal { template class RotationQuaternionTemplate; @@ -162,10 +160,8 @@ typedef AngleAxisTemplate AngleAxis; template std::ostream& operator<<(std::ostream& out, const AngleAxisTemplate& rhs); +} // namespace kindr::minimal -} // namespace minimal -} // namespace kindr - -#include +#include "wavemap/3rd_party/minkindr/impl/angle-axis-inl.h" #endif // WAVEMAP_3RD_PARTY_MINKINDR_ANGLE_AXIS_H_ diff --git a/library/include/wavemap/3rd_party/minkindr/common.h b/library/include/wavemap/3rd_party/minkindr/common.h index 48cac3501..9778202ed 100644 --- a/library/include/wavemap/3rd_party/minkindr/common.h +++ b/library/include/wavemap/3rd_party/minkindr/common.h @@ -28,9 +28,7 @@ #include #include -namespace kindr { -namespace minimal { - +namespace kindr::minimal { inline void skewMatrix(const Eigen::Vector3d& v, Eigen::Matrix3d* skew) { CHECK_NOTNULL(skew); skew->setZero(); @@ -59,8 +57,6 @@ inline Eigen::Matrix3d skewMatrix(const Eigen::Vector3d& v) { skewMatrix(v, &skew); return skew; } - -} // namespace minimal -} // namespace kindr +} // namespace kindr::minimal #endif // WAVEMAP_3RD_PARTY_MINKINDR_COMMON_H_ diff --git a/library/include/wavemap/3rd_party/minkindr/impl/angle-axis-inl.h b/library/include/wavemap/3rd_party/minkindr/impl/angle-axis-inl.h index 446b19e93..7169fec21 100644 --- a/library/include/wavemap/3rd_party/minkindr/impl/angle-axis-inl.h +++ b/library/include/wavemap/3rd_party/minkindr/impl/angle-axis-inl.h @@ -28,12 +28,11 @@ #include #include -#include -#include -namespace kindr { -namespace minimal { +#include "wavemap/3rd_party/minkindr/angle-axis.h" +#include "wavemap/3rd_party/minkindr/rotation-quaternion.h" +namespace kindr::minimal { template AngleAxisTemplate::AngleAxisTemplate() : C_A_B_(Implementation::Identity()) {} @@ -83,7 +82,7 @@ AngleAxisTemplate::AngleAxisTemplate( : C_A_B_(quat.toImplementation()) {} template -AngleAxisTemplate::~AngleAxisTemplate() {} +AngleAxisTemplate::~AngleAxisTemplate() = default; template AngleAxisTemplate& AngleAxisTemplate::operator=( @@ -263,7 +262,6 @@ typename AngleAxisTemplate::RotationMatrix AngleAxisTemplate::getRotationMatrix() const { return C_A_B_.matrix(); } +} // namespace kindr::minimal -} // namespace minimal -} // namespace kindr #endif // WAVEMAP_3RD_PARTY_MINKINDR_IMPL_ANGLE_AXIS_INL_H_ diff --git a/library/include/wavemap/3rd_party/minkindr/impl/quat-transformation-inl.h b/library/include/wavemap/3rd_party/minkindr/impl/quat-transformation-inl.h index 96fcaa4df..8ab389262 100644 --- a/library/include/wavemap/3rd_party/minkindr/impl/quat-transformation-inl.h +++ b/library/include/wavemap/3rd_party/minkindr/impl/quat-transformation-inl.h @@ -28,11 +28,10 @@ #include #include -#include -namespace kindr { -namespace minimal { +#include "wavemap/3rd_party/minkindr/quat-transformation.h" +namespace kindr::minimal { template QuatTransformationTemplate::QuatTransformationTemplate() { setIdentity(); @@ -71,7 +70,7 @@ QuatTransformationTemplate::QuatTransformationTemplate( A_t_A_B_(x_t_r.template head<3>().eval()) {} template -QuatTransformationTemplate::~QuatTransformationTemplate() {} +QuatTransformationTemplate::~QuatTransformationTemplate() = default; template void QuatTransformationTemplate::setIdentity() { @@ -296,7 +295,6 @@ QuatTransformationTemplate interpolateComponentwise( T_a.getEigenQuaternion().slerp(lambda, T_b.getEigenQuaternion()); return QuatTransformationTemplate(q_int, p_int); } +} // namespace kindr::minimal -} // namespace minimal -} // namespace kindr #endif // WAVEMAP_3RD_PARTY_MINKINDR_IMPL_QUAT_TRANSFORMATION_INL_H_ diff --git a/library/include/wavemap/3rd_party/minkindr/impl/rotation-quaternion-inl.h b/library/include/wavemap/3rd_party/minkindr/impl/rotation-quaternion-inl.h index 7b2399616..b1970104b 100644 --- a/library/include/wavemap/3rd_party/minkindr/impl/rotation-quaternion-inl.h +++ b/library/include/wavemap/3rd_party/minkindr/impl/rotation-quaternion-inl.h @@ -28,12 +28,11 @@ #include #include -#include -#include -namespace kindr { -namespace minimal { +#include "wavemap/3rd_party/minkindr/angle-axis.h" +#include "wavemap/3rd_party/minkindr/rotation-quaternion.h" +namespace kindr::minimal { template struct EPS { static constexpr Scalar value() { return static_cast(1.0e-5); } @@ -151,7 +150,7 @@ RotationQuaternionTemplate::RotationQuaternionTemplate( : q_A_B_(angleAxis.toImplementation()) {} template -RotationQuaternionTemplate::~RotationQuaternionTemplate() {} +RotationQuaternionTemplate::~RotationQuaternionTemplate() = default; /// \brief the real component of the quaternion template @@ -585,7 +584,6 @@ void RotationQuaternionTemplate::normalizationHelper( quaternion->normalize(); } } +} // namespace kindr::minimal -} // namespace minimal -} // namespace kindr #endif // WAVEMAP_3RD_PARTY_MINKINDR_IMPL_ROTATION_QUATERNION_INL_H_ diff --git a/library/include/wavemap/3rd_party/minkindr/position.h b/library/include/wavemap/3rd_party/minkindr/position.h index 651f9b70c..a72e9ce77 100644 --- a/library/include/wavemap/3rd_party/minkindr/position.h +++ b/library/include/wavemap/3rd_party/minkindr/position.h @@ -27,15 +27,11 @@ #include -namespace kindr { -namespace minimal { - +namespace kindr::minimal { template using PositionTemplate = Eigen::Matrix; typedef PositionTemplate Position; - -} // namespace minimal -} // namespace kindr +} // namespace kindr::minimal #endif // WAVEMAP_3RD_PARTY_MINKINDR_POSITION_H_ diff --git a/library/include/wavemap/3rd_party/minkindr/quat-transformation.h b/library/include/wavemap/3rd_party/minkindr/quat-transformation.h index f89227a87..bc63abef9 100644 --- a/library/include/wavemap/3rd_party/minkindr/quat-transformation.h +++ b/library/include/wavemap/3rd_party/minkindr/quat-transformation.h @@ -28,11 +28,10 @@ #include #include -#include -#include +#include "wavemap/3rd_party/minkindr/position.h" +#include "wavemap/3rd_party/minkindr/rotation-quaternion.h" -namespace kindr { -namespace minimal { +namespace kindr::minimal { /// \class QuatTransformation /// \brief A frame transformation built from a quaternion and a point @@ -163,7 +162,7 @@ class QuatTransformationTemplate { /// note: this is the log map of SO(3)xR(3) and not SE(3) /// \return vector form of log map with first 3 components the translational /// part and the last three the rotational part. - static Vector6 log(const QuatTransformationTemplate& vec); + static Vector6 log(const QuatTransformationTemplate& T); /// \brief return a copy of the transformation inverted. QuatTransformationTemplate inverse() const; @@ -208,9 +207,8 @@ template inline QuatTransformationTemplate interpolateComponentwise( const QuatTransformationTemplate& T_a, const QuatTransformationTemplate& T_b, const double lambda); -} // namespace minimal -} // namespace kindr +} // namespace kindr::minimal -#include +#include "wavemap/3rd_party/minkindr/impl/quat-transformation-inl.h" #endif // WAVEMAP_3RD_PARTY_MINKINDR_QUAT_TRANSFORMATION_H_ diff --git a/library/include/wavemap/3rd_party/minkindr/rotation-quaternion.h b/library/include/wavemap/3rd_party/minkindr/rotation-quaternion.h index 7ea6a7a5b..26ab8b4ad 100644 --- a/library/include/wavemap/3rd_party/minkindr/rotation-quaternion.h +++ b/library/include/wavemap/3rd_party/minkindr/rotation-quaternion.h @@ -29,9 +29,7 @@ #include -namespace kindr { -namespace minimal { - +namespace kindr::minimal { template class AngleAxisTemplate; @@ -233,9 +231,8 @@ template std::ostream& operator<<(std::ostream& out, const RotationQuaternionTemplate& rhs); -} // namespace minimal -} // namespace kindr +} // namespace kindr::minimal -#include +#include "wavemap/3rd_party/minkindr/impl/rotation-quaternion-inl.h" #endif // WAVEMAP_3RD_PARTY_MINKINDR_ROTATION_QUATERNION_H_ From a3bc0d92f17875aa349b2ad8d8b0cbfc1cb2789f Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 17 Jul 2024 10:52:48 +0200 Subject: [PATCH 098/159] Make measurement integration and map operation pipeline ROS-agnostic --- interfaces/ros1/wavemap/CMakeLists.txt | 4 +- interfaces/ros1/wavemap_ros/CMakeLists.txt | 10 +- .../ros1/wavemap_ros/app/rosbag_processor.cc | 46 +++--- .../wavemap_ros/inputs/depth_image_input.h | 39 +++--- .../include/wavemap_ros/inputs/input_base.h | 53 ++----- .../wavemap_ros/inputs/input_factory.h | 18 +-- .../wavemap_ros/inputs/pointcloud_input.h | 41 ++++-- .../crop_map_operation.h | 29 ++-- .../impl/publish_map_operation_inl.h | 6 +- .../map_ros_operation_factory.h | 33 +++++ .../map_operations/map_ros_operation_types.h | 17 +++ .../publish_map_operation.h | 33 ++--- .../publish_pointcloud_operation.h | 29 ++-- .../wavemap_ros/operations/operation_base.h | 35 ----- .../operations/operation_factory.h | 31 ---- .../operations/prune_map_operation.h | 51 ------- .../operations/threshold_map_operation.h | 54 ------- .../include/wavemap_ros/ros_server.h | 31 ++-- .../src/inputs/depth_image_input.cc | 132 +++++++++--------- .../ros1/wavemap_ros/src/inputs/input_base.cc | 104 ++------------ .../wavemap_ros/src/inputs/input_factory.cc | 86 +++++++----- .../src/inputs/pointcloud_input.cc | 123 +++++++++++----- .../crop_map_operation.cc | 18 ++- .../map_ros_operation_factory.cc | 69 +++++++++ .../publish_map_operation.cc | 26 +++- .../publish_pointcloud_operation.cc | 24 +++- .../src/operations/operation_factory.cc | 96 ------------- .../src/operations/prune_map_operation.cc | 10 -- .../src/operations/threshold_map_operation.cc | 10 -- interfaces/ros1/wavemap_ros/src/ros_server.cc | 106 ++++++++++---- .../core/config/impl/type_selector_inl.h | 73 +++++----- .../wavemap/core/config/type_selector.h | 3 +- .../wavemap/core/integrator/integrator_base.h | 4 +- .../core/integrator/integrator_factory.h | 4 +- .../projective/projective_integrator.h | 5 +- .../ray_tracing/ray_tracing_integrator.h | 8 +- .../wavemap/core/utils/print/container.h | 26 +++- .../wavemap/pipeline/impl/pipeline_inl.h | 30 ++++ .../map_operations/map_operation_base.h | 31 ++++ .../map_operations/map_operation_factory.h | 23 +++ .../map_operations/prune_map_operation.h | 41 ++++++ .../map_operations/threshold_map_operation.h | 42 ++++++ library/include/wavemap/pipeline/pipeline.h | 72 ++++++++++ library/src/CMakeLists.txt | 1 + .../src/core/integrator/integrator_factory.cc | 16 +-- .../projective/projective_integrator.cc | 6 +- .../ray_tracing/ray_tracing_integrator.cc | 3 +- library/src/pipeline/CMakeLists.txt | 12 ++ .../map_operations/map_operation_factory.cc | 50 +++++++ .../map_operations/prune_map_operation.cc | 23 +++ .../map_operations/threshold_map_operation.cc | 23 +++ library/src/pipeline/pipeline.cc | 69 +++++++++ .../projection_model/test_image_projectors.cc | 6 +- .../integrator/test_pointcloud_integrators.cc | 6 +- library/test/src/core/map/test_haar_cell.cc | 12 +- 55 files changed, 1122 insertions(+), 831 deletions(-) rename interfaces/ros1/wavemap_ros/include/wavemap_ros/{operations => map_operations}/crop_map_operation.h (59%) rename interfaces/ros1/wavemap_ros/include/wavemap_ros/{operations => map_operations}/impl/publish_map_operation_inl.h (91%) create mode 100644 interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/map_ros_operation_factory.h create mode 100644 interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/map_ros_operation_types.h rename interfaces/ros1/wavemap_ros/include/wavemap_ros/{operations => map_operations}/publish_map_operation.h (73%) rename interfaces/ros1/wavemap_ros/include/wavemap_ros/{operations => map_operations}/publish_pointcloud_operation.h (65%) delete mode 100644 interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_base.h delete mode 100644 interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_factory.h delete mode 100644 interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/prune_map_operation.h delete mode 100644 interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/threshold_map_operation.h rename interfaces/ros1/wavemap_ros/src/{operations => map_operations}/crop_map_operation.cc (78%) create mode 100644 interfaces/ros1/wavemap_ros/src/map_operations/map_ros_operation_factory.cc rename interfaces/ros1/wavemap_ros/src/{operations => map_operations}/publish_map_operation.cc (74%) rename interfaces/ros1/wavemap_ros/src/{operations => map_operations}/publish_pointcloud_operation.cc (86%) delete mode 100644 interfaces/ros1/wavemap_ros/src/operations/operation_factory.cc delete mode 100644 interfaces/ros1/wavemap_ros/src/operations/prune_map_operation.cc delete mode 100644 interfaces/ros1/wavemap_ros/src/operations/threshold_map_operation.cc create mode 100644 library/include/wavemap/pipeline/impl/pipeline_inl.h create mode 100644 library/include/wavemap/pipeline/map_operations/map_operation_base.h create mode 100644 library/include/wavemap/pipeline/map_operations/map_operation_factory.h create mode 100644 library/include/wavemap/pipeline/map_operations/prune_map_operation.h create mode 100644 library/include/wavemap/pipeline/map_operations/threshold_map_operation.h create mode 100644 library/include/wavemap/pipeline/pipeline.h create mode 100644 library/src/pipeline/CMakeLists.txt create mode 100644 library/src/pipeline/map_operations/map_operation_factory.cc create mode 100644 library/src/pipeline/map_operations/prune_map_operation.cc create mode 100644 library/src/pipeline/map_operations/threshold_map_operation.cc create mode 100644 library/src/pipeline/pipeline.cc diff --git a/interfaces/ros1/wavemap/CMakeLists.txt b/interfaces/ros1/wavemap/CMakeLists.txt index c6fe55ade..ebe08d393 100644 --- a/interfaces/ros1/wavemap/CMakeLists.txt +++ b/interfaces/ros1/wavemap/CMakeLists.txt @@ -6,7 +6,7 @@ find_package(catkin REQUIRED) catkin_package( INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/include - LIBRARIES wavemap_core wavemap_io + LIBRARIES wavemap_core wavemap_io wavemap_pipeline CFG_EXTRAS ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/cmake/wavemap-extras.cmake) @@ -15,7 +15,7 @@ add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../../../library ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}) # Install targets -install(TARGETS wavemap_core wavemap_io +install(TARGETS wavemap_core wavemap_io wavemap_pipeline ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/interfaces/ros1/wavemap_ros/CMakeLists.txt b/interfaces/ros1/wavemap_ros/CMakeLists.txt index 4e43ee388..bae3b2cc8 100644 --- a/interfaces/ros1/wavemap_ros/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros/CMakeLists.txt @@ -20,12 +20,10 @@ cs_add_library(${PROJECT_NAME} src/inputs/input_base.cc src/inputs/input_factory.cc src/inputs/pointcloud_input.cc - src/operations/crop_map_operation.cc - src/operations/operation_factory.cc - src/operations/prune_map_operation.cc - src/operations/publish_map_operation.cc - src/operations/publish_pointcloud_operation.cc - src/operations/threshold_map_operation.cc + src/map_operations/crop_map_operation.cc + src/map_operations/map_ros_operation_factory.cc + src/map_operations/publish_map_operation.cc + src/map_operations/publish_pointcloud_operation.cc src/utils/pointcloud_undistortion/pointcloud_undistorter.cc src/utils/rosbag_processor.cc src/utils/tf_transformer.cc diff --git a/interfaces/ros1/wavemap_ros/app/rosbag_processor.cc b/interfaces/ros1/wavemap_ros/app/rosbag_processor.cc index 958327ed2..7045db1c4 100644 --- a/interfaces/ros1/wavemap_ros/app/rosbag_processor.cc +++ b/interfaces/ros1/wavemap_ros/app/rosbag_processor.cc @@ -36,30 +36,28 @@ int main(int argc, char** argv) { } // Setup input handlers - const param::Array integrator_params_array = - param::convert::toParamArray(nh_private, "inputs"); - for (const auto& integrator_params : integrator_params_array) { - InputBase* input_handler = - wavemap_server.addInput(integrator_params, nh, nh_private); - if (input_handler) { - switch (input_handler->getType()) { - case InputType::kPointcloud: { - auto pointcloud_handler = - dynamic_cast(input_handler); - PointcloudInput::registerCallback( - pointcloud_handler->getTopicType(), [&](auto callback_ptr) { - rosbag_processor.addCallback(input_handler->getTopicName(), - callback_ptr, pointcloud_handler); - }); - } - continue; - case InputType::kDepthImage: - rosbag_processor.addCallback( - input_handler->getTopicName(), &DepthImageInput::callback, - dynamic_cast(input_handler)); - continue; - } + size_t input_idx = 0u; + for (const auto& input : wavemap_server.getInputs()) { + if (auto pointcloud_input = dynamic_cast(input.get()); + pointcloud_input) { + PointcloudInput::registerCallback( + pointcloud_input->getTopicType(), [&](auto callback_ptr) { + rosbag_processor.addCallback(input->getTopicName(), callback_ptr, + pointcloud_input); + }); + } else if (auto depth_image_input = + dynamic_cast(input.get()); + depth_image_input) { + rosbag_processor.addCallback( + input->getTopicName(), &DepthImageInput::callback, depth_image_input); + } else { + ROS_WARN_STREAM( + "Failed to register callback for input number " + << input_idx << ", with topic \"" << input->getTopicName() + << "\". Support for inputs of type \"" << input->getType().toStr() + << "\" is not yet implemented in the rosbag processing script."); } + ++input_idx; } // Republish TFs @@ -79,7 +77,7 @@ int main(int argc, char** argv) { } wavemap_server.getMap()->prune(); - wavemap_server.runOperations(ros::Time::now(), /*force_run_all*/ true); + wavemap_server.getPipeline().runOperations(/*force_run_all*/ true); if (nh_private.param("keep_alive", false)) { ros::spin(); diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h index f375bbbaa..9b07781ed 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include "wavemap_ros/inputs/input_base.h" @@ -18,7 +18,7 @@ namespace wavemap { /** * Config struct for the depth image input handler. */ -struct DepthImageInputConfig : public ConfigBase { +struct DepthImageInputConfig : public ConfigBase { //! Name of the ROS topic to subscribe to. std::string topic_name = "scan"; //! Queue length to use when subscribing to the ROS topic. @@ -46,21 +46,16 @@ struct DepthImageInputConfig : public ConfigBase { //! known (e.g. through calibration) but not corrected by the sensor's driver. Seconds time_offset = 0.f; - //! Name of the topic on which to republish the depth images as pointclouds. + //! Name of the topic on which to publish the depth images as pointclouds. //! Useful to share the pointclouds with other ROS nodes and for debugging. //! Disabled if not set. - std::string reprojected_pointcloud_topic_name; - //! Name of the topic on which to republish the range image computed from - //! the pointclouds. Useful for debugging. Disabled if not set. - std::string projected_range_image_topic_name; + std::string projected_pointcloud_topic_name; static MemberMap memberMap; // Conversion to InputHandler base config operator InputBaseConfig() const { // NOLINT - return {topic_name, topic_queue_length, processing_retry_period, - reprojected_pointcloud_topic_name, - projected_range_image_topic_name}; + return {topic_name, topic_queue_length, processing_retry_period}; } bool isValid(bool verbose) const override; @@ -68,13 +63,12 @@ struct DepthImageInputConfig : public ConfigBase { class DepthImageInput : public InputBase { public: - DepthImageInput( - const DepthImageInputConfig& config, const param::Value& params, - std::string world_frame, MapBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh, - ros::NodeHandle nh_private, - std::function map_update_callback = {}); + DepthImageInput(const DepthImageInputConfig& config, + std::shared_ptr pipeline, + std::vector integrator_names, + std::shared_ptr transformer, + std::string world_frame, ros::NodeHandle nh, + ros::NodeHandle nh_private); InputType getType() const override { return InputType::kDepthImage; } @@ -87,13 +81,20 @@ class DepthImageInput : public InputBase { private: const DepthImageInputConfig config_; - std::vector scanwise_integrators_; + + Stopwatch integration_timer_; image_transport::Subscriber depth_image_sub_; std::queue depth_image_queue_; void processQueue() override; - PosedPointcloud<> reproject(const PosedImage<>& posed_range_image); + void publishProjectedPointcloudIfEnabled( + const ros::Time& stamp, + const PosedImage& posed_depth_image); + static PosedPointcloud project( + const PosedImage<>& posed_depth_image, + const ProjectorBase& projection_model); + ros::Publisher projected_pointcloud_pub_; }; } // namespace wavemap diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_base.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_base.h index 475bb7025..73d2cd1ef 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_base.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_base.h @@ -6,14 +6,8 @@ #include #include -#include #include -#include -#include -#include -#include -#include -#include +#include #include "wavemap_ros/utils/tf_transformer.h" @@ -26,42 +20,31 @@ struct InputType : public TypeSelector { static constexpr std::array names = {"pointcloud", "depth_image"}; }; -struct InputBaseConfig : public ConfigBase { +struct InputBaseConfig : public ConfigBase { std::string topic_name = "scan"; int topic_queue_length = 10; Seconds processing_retry_period = 0.05f; - std::string reprojected_pointcloud_topic_name; // Leave blank to disable - std::string projected_range_image_topic_name; // Leave blank to disable - static MemberMap memberMap; // Constructors InputBaseConfig() = default; InputBaseConfig(std::string topic_name, int topic_queue_length, - FloatingPoint processing_retry_period, - std::string reprojected_pointcloud_topic_name, - std::string projected_range_image_topic_name) + FloatingPoint processing_retry_period) : topic_name(std::move(topic_name)), topic_queue_length(topic_queue_length), - processing_retry_period(processing_retry_period), - reprojected_pointcloud_topic_name( - std::move(reprojected_pointcloud_topic_name)), - projected_range_image_topic_name( - std::move(projected_range_image_topic_name)) {} + processing_retry_period(processing_retry_period) {} bool isValid(bool verbose) const override; }; class InputBase { public: - InputBase(const InputBaseConfig& config, const param::Value& params, - std::string world_frame, MapBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, const ros::NodeHandle& nh, - ros::NodeHandle nh_private, - std::function map_update_callback = {}); + InputBase(const InputBaseConfig& config, std::shared_ptr pipeline, + std::vector integrator_names, + std::shared_ptr transformer, std::string world_frame, + const ros::NodeHandle& nh, ros::NodeHandle nh_private); virtual ~InputBase() = default; virtual InputType getType() const = 0; @@ -69,27 +52,15 @@ class InputBase { protected: const InputBaseConfig config_; - const std::string world_frame_; - const std::shared_ptr transformer_; + std::shared_ptr pipeline_; + const std::vector integrator_names_; - std::vector integrators_; - Stopwatch integration_timer_; + const std::shared_ptr transformer_; + const std::string world_frame_; virtual void processQueue() = 0; ros::Timer queue_processing_retry_timer_; - - std::function map_update_callback_; - - bool shouldPublishReprojectedPointcloud() const; - void publishReprojectedPointcloud(const ros::Time& stamp, - const PosedPointcloud<>& posed_pointcloud); - ros::Publisher reprojected_pointcloud_pub_; - - bool shouldPublishProjectedRangeImage() const; - void publishProjectedRangeImage(const ros::Time& stamp, - const Image<>& range_image); - image_transport::Publisher projected_range_image_pub_; }; } // namespace wavemap diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_factory.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_factory.h index f4441cf26..02591b2fa 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_factory.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_factory.h @@ -11,19 +11,15 @@ namespace wavemap { class InputFactory { public: static std::unique_ptr create( - const param::Value& params, std::string world_frame, - MapBase::Ptr occupancy_map, std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh, - ros::NodeHandle nh_private, - std::optional default_input_type = std::nullopt, - std::function map_update_callback = {}); + const param::Value& params, std::shared_ptr pipeline, + std::shared_ptr transformer, std::string world_frame, + ros::NodeHandle nh, ros::NodeHandle nh_private); static std::unique_ptr create( - InputType input_type, const param::Value& params, std::string world_frame, - MapBase::Ptr occupancy_map, std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh, - ros::NodeHandle nh_private, - std::function map_update_callback = {}); + InputType input_type, const param::Value& params, + std::shared_ptr pipeline, + std::shared_ptr transformer, std::string world_frame, + ros::NodeHandle nh, ros::NodeHandle nh_private); }; } // namespace wavemap diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h index 5c268481d..b15f23080 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h @@ -4,8 +4,11 @@ #include #include #include +#include +#include #include +#include #include "wavemap_ros/inputs/input_base.h" #include "wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h" @@ -59,22 +62,20 @@ struct PointcloudInputConfig //! undistortion. int num_undistortion_interpolation_intervals_per_cloud = 100; - //! Name of the topic on which to republish the motion-undistorted - //! pointclouds. Useful to share the undistorted pointclouds with other ROS - //! nodes and for debugging. Disabled if not set. - std::string reprojected_pointcloud_topic_name; - //! Name of the topic on which to republish the range image computed from the + //! Name of the topic on which to publish the range image generated from the //! pointclouds. Useful for debugging, to see how well the projection model //! matches the LiDAR. Disabled if not set. std::string projected_range_image_topic_name; + //! Name of the topic on which to republish the motion-undistorted + //! pointclouds. Useful to share the undistorted pointclouds with other ROS + //! nodes and for debugging. Disabled if not set. + std::string undistorted_pointcloud_topic_name; static MemberMap memberMap; // Conversion to InputHandler base config operator InputBaseConfig() const { // NOLINT - return {topic_name, topic_queue_length, processing_retry_period, - reprojected_pointcloud_topic_name, - projected_range_image_topic_name}; + return {topic_name, topic_queue_length, processing_retry_period}; } bool isValid(bool verbose) const override; @@ -82,13 +83,12 @@ struct PointcloudInputConfig class PointcloudInput : public InputBase { public: - PointcloudInput( - const PointcloudInputConfig& config, const param::Value& params, - std::string world_frame, MapBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh, - ros::NodeHandle nh_private, - std::function map_update_callback = {}); + PointcloudInput(const PointcloudInputConfig& config, + std::shared_ptr pipeline, + std::vector integrator_names, + std::shared_ptr transformer, + std::string world_frame, ros::NodeHandle nh, + ros::NodeHandle nh_private); InputType getType() const override { return InputType::kPointcloud; } PointcloudTopicType getTopicType() const { return config_.topic_type; } @@ -104,13 +104,24 @@ class PointcloudInput : public InputBase { private: const PointcloudInputConfig config_; + Stopwatch integration_timer_; + PointcloudUndistorter pointcloud_undistorter_; + ros::Subscriber pointcloud_sub_; std::queue pointcloud_queue_; void processQueue() override; static bool hasField(const sensor_msgs::PointCloud2& msg, const std::string& field_name); + + void publishProjectedRangeImageIfEnabled( + const ros::Time& stamp, const PosedPointcloud<>& posed_pointcloud); + image_transport::Publisher projected_range_image_pub_; + + void publishUndistortedPointcloudIfEnabled( + const ros::Time& stamp, const PosedPointcloud<>& undistorted_pointcloud); + ros::Publisher undistorted_pointcloud_pub_; }; } // namespace wavemap diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/crop_map_operation.h similarity index 59% rename from interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/crop_map_operation.h index b7928fb2c..9f133d3fc 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/crop_map_operation.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/crop_map_operation.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ROS_OPERATIONS_CROP_MAP_OPERATION_H_ -#define WAVEMAP_ROS_OPERATIONS_CROP_MAP_OPERATION_H_ +#ifndef WAVEMAP_ROS_MAP_OPERATIONS_CROP_MAP_OPERATION_H_ +#define WAVEMAP_ROS_MAP_OPERATIONS_CROP_MAP_OPERATION_H_ #include #include @@ -7,8 +7,8 @@ #include #include +#include -#include "wavemap_ros/operations/operation_base.h" #include "wavemap_ros/utils/tf_transformer.h" namespace wavemap { @@ -32,32 +32,23 @@ struct CropMapOperationConfig : public ConfigBase { bool isValid(bool verbose) const override; }; -class CropMapOperation : public OperationBase { +class CropMapOperation : public MapOperationBase { public: CropMapOperation(const CropMapOperationConfig& config, - std::string world_frame, + MapBase::Ptr occupancy_map, std::shared_ptr transformer, - MapBase::Ptr occupancy_map) - : config_(config.checkValid()), - world_frame_(std::move(world_frame)), - transformer_(std::move(transformer)), - occupancy_map_(std::move(occupancy_map)) {} + std::string world_frame); - OperationType getType() const override { return OperationType::kCropMap; } + bool shouldRun(const ros::Time& current_time); - bool shouldRun(const ros::Time& current_time) { - return config_.once_every < (current_time - last_run_timestamp_).toSec(); - } - - void run(const ros::Time& current_time, bool force_run) override; + void run(bool force_run) override; private: const CropMapOperationConfig config_; - const std::string world_frame_; const std::shared_ptr transformer_; - const MapBase::Ptr occupancy_map_; + const std::string world_frame_; ros::Time last_run_timestamp_; }; } // namespace wavemap -#endif // WAVEMAP_ROS_OPERATIONS_CROP_MAP_OPERATION_H_ +#endif // WAVEMAP_ROS_MAP_OPERATIONS_CROP_MAP_OPERATION_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/impl/publish_map_operation_inl.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/impl/publish_map_operation_inl.h similarity index 91% rename from interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/impl/publish_map_operation_inl.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/impl/publish_map_operation_inl.h index 5b0322e9b..73010269b 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/impl/publish_map_operation_inl.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/impl/publish_map_operation_inl.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ROS_OPERATIONS_IMPL_PUBLISH_MAP_OPERATION_INL_H_ -#define WAVEMAP_ROS_OPERATIONS_IMPL_PUBLISH_MAP_OPERATION_INL_H_ +#ifndef WAVEMAP_ROS_MAP_OPERATIONS_IMPL_PUBLISH_MAP_OPERATION_INL_H_ +#define WAVEMAP_ROS_MAP_OPERATIONS_IMPL_PUBLISH_MAP_OPERATION_INL_H_ #include @@ -60,4 +60,4 @@ void PublishMapOperation::publishHashedMap(const ros::Time& current_time, } } // namespace wavemap -#endif // WAVEMAP_ROS_OPERATIONS_IMPL_PUBLISH_MAP_OPERATION_INL_H_ +#endif // WAVEMAP_ROS_MAP_OPERATIONS_IMPL_PUBLISH_MAP_OPERATION_INL_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/map_ros_operation_factory.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/map_ros_operation_factory.h new file mode 100644 index 000000000..09375d947 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/map_ros_operation_factory.h @@ -0,0 +1,33 @@ +#ifndef WAVEMAP_ROS_MAP_OPERATIONS_MAP_ROS_OPERATION_FACTORY_H_ +#define WAVEMAP_ROS_MAP_OPERATIONS_MAP_ROS_OPERATION_FACTORY_H_ + +#include +#include + +#include +#include +#include +#include +#include + +#include "wavemap_ros/map_operations/map_ros_operation_types.h" +#include "wavemap_ros/utils/tf_transformer.h" + +namespace wavemap { +class MapRosOperationFactory { + public: + static std::unique_ptr create( + const param::Value& params, MapBase::Ptr occupancy_map, + std::shared_ptr thread_pool, + std::shared_ptr transformer, std::string world_frame, + ros::NodeHandle nh_private); + + static std::unique_ptr create( + MapRosOperationType ros_operation_type, const param::Value& params, + MapBase::Ptr occupancy_map, std::shared_ptr thread_pool, + std::shared_ptr transformer, std::string world_frame, + ros::NodeHandle nh_private); +}; +} // namespace wavemap + +#endif // WAVEMAP_ROS_MAP_OPERATIONS_MAP_ROS_OPERATION_FACTORY_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/map_ros_operation_types.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/map_ros_operation_types.h new file mode 100644 index 000000000..f5f9147c9 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/map_ros_operation_types.h @@ -0,0 +1,17 @@ +#ifndef WAVEMAP_ROS_MAP_OPERATIONS_MAP_ROS_OPERATION_TYPES_H_ +#define WAVEMAP_ROS_MAP_OPERATIONS_MAP_ROS_OPERATION_TYPES_H_ + +#include + +namespace wavemap { +struct MapRosOperationType : public TypeSelector { + using TypeSelector::TypeSelector; + + enum Id : TypeId { kPublishMap, kPublishPointcloud, kCropMap }; + + static constexpr std::array names = {"publish_map", "publish_pointcloud", + "crop_map"}; +}; +} // namespace wavemap + +#endif // WAVEMAP_ROS_MAP_OPERATIONS_MAP_ROS_OPERATION_TYPES_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/publish_map_operation.h similarity index 73% rename from interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/publish_map_operation.h index f3da987a5..c42861e93 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/publish_map_operation.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/publish_map_operation.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ROS_OPERATIONS_PUBLISH_MAP_OPERATION_H_ -#define WAVEMAP_ROS_OPERATIONS_PUBLISH_MAP_OPERATION_H_ +#ifndef WAVEMAP_ROS_MAP_OPERATIONS_PUBLISH_MAP_OPERATION_H_ +#define WAVEMAP_ROS_MAP_OPERATIONS_PUBLISH_MAP_OPERATION_H_ #include #include @@ -9,8 +9,7 @@ #include #include #include - -#include "wavemap_ros/operations/operation_base.h" +#include namespace wavemap { /** @@ -36,31 +35,21 @@ struct PublishMapOperationConfig bool isValid(bool verbose) const override; }; -class PublishMapOperation : public OperationBase { +class PublishMapOperation : public MapOperationBase { public: PublishMapOperation(const PublishMapOperationConfig& config, - std::string world_frame, MapBase::Ptr occupancy_map, + MapBase::Ptr occupancy_map, std::shared_ptr thread_pool, - ros::NodeHandle nh_private); - - OperationType getType() const override { return OperationType::kPublishMap; } + std::string world_frame, ros::NodeHandle nh_private); - bool shouldRun(const ros::Time& current_time) { - return config_.once_every < (current_time - last_run_timestamp_).toSec(); - } + bool shouldRun(const ros::Time& current_time); - void run(const ros::Time& current_time, bool force_run) override { - if (force_run || shouldRun(current_time)) { - publishMap(current_time, false); - last_run_timestamp_ = current_time; - } - } + void run(bool force_run) override; private: const PublishMapOperationConfig config_; - const std::string world_frame_; - const MapBase::Ptr occupancy_map_; const std::shared_ptr thread_pool_; + const std::string world_frame_; ros::Time last_run_timestamp_; // Map publishing @@ -87,6 +76,6 @@ class PublishMapOperation : public OperationBase { }; } // namespace wavemap -#include "wavemap_ros/operations/impl/publish_map_operation_inl.h" +#include "wavemap_ros/map_operations/impl/publish_map_operation_inl.h" -#endif // WAVEMAP_ROS_OPERATIONS_PUBLISH_MAP_OPERATION_H_ +#endif // WAVEMAP_ROS_MAP_OPERATIONS_PUBLISH_MAP_OPERATION_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/publish_pointcloud_operation.h similarity index 65% rename from interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/publish_pointcloud_operation.h index 65f6d6a00..7503ada96 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/publish_pointcloud_operation.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/publish_pointcloud_operation.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ROS_OPERATIONS_PUBLISH_POINTCLOUD_OPERATION_H_ -#define WAVEMAP_ROS_OPERATIONS_PUBLISH_POINTCLOUD_OPERATION_H_ +#ifndef WAVEMAP_ROS_MAP_OPERATIONS_PUBLISH_POINTCLOUD_OPERATION_H_ +#define WAVEMAP_ROS_MAP_OPERATIONS_PUBLISH_POINTCLOUD_OPERATION_H_ #include #include @@ -9,8 +9,7 @@ #include #include #include - -#include "wavemap_ros/operations/operation_base.h" +#include namespace wavemap { /** @@ -34,32 +33,20 @@ struct PublishPointcloudOperationConfig bool isValid(bool verbose) const override; }; -class PublishPointcloudOperation : public OperationBase { +class PublishPointcloudOperation : public MapOperationBase { public: PublishPointcloudOperation(const PublishPointcloudOperationConfig& config, - std::string world_frame, MapBase::Ptr occupancy_map, + std::string world_frame, ros::NodeHandle nh_private); - OperationType getType() const override { - return OperationType::kPublishPointcloud; - } - - bool shouldRun(const ros::Time& current_time) { - return config_.once_every < (current_time - last_run_timestamp_).toSec(); - } + bool shouldRun(const ros::Time& current_time); - void run(const ros::Time& current_time, bool force_run) override { - if (force_run || shouldRun(current_time)) { - publishPointcloud(current_time); - last_run_timestamp_ = current_time; - } - } + void run(bool force_run) override; private: const PublishPointcloudOperationConfig config_; const std::string world_frame_; - const MapBase::Ptr occupancy_map_; ros::Time last_run_timestamp_; // Pointcloud publishing @@ -69,4 +56,4 @@ class PublishPointcloudOperation : public OperationBase { }; } // namespace wavemap -#endif // WAVEMAP_ROS_OPERATIONS_PUBLISH_POINTCLOUD_OPERATION_H_ +#endif // WAVEMAP_ROS_MAP_OPERATIONS_PUBLISH_POINTCLOUD_OPERATION_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_base.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_base.h deleted file mode 100644 index 50780536c..000000000 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_base.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef WAVEMAP_ROS_OPERATIONS_OPERATION_BASE_H_ -#define WAVEMAP_ROS_OPERATIONS_OPERATION_BASE_H_ - -#include -#include - -namespace wavemap { -struct OperationType : public TypeSelector { - using TypeSelector::TypeSelector; - - enum Id : TypeId { - kThresholdMap, - kPruneMap, - kPublishMap, - kPublishPointcloud, - kCropMap - }; - - static constexpr std::array names = {"threshold_map", "prune_map", - "publish_map", "publish_pointcloud", - "crop_map"}; -}; - -class OperationBase { - public: - OperationBase() = default; - virtual ~OperationBase() = default; - - virtual OperationType getType() const = 0; - - virtual void run(const ros::Time& current_time, bool force_run) = 0; -}; -} // namespace wavemap - -#endif // WAVEMAP_ROS_OPERATIONS_OPERATION_BASE_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_factory.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_factory.h deleted file mode 100644 index 07afb1571..000000000 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/operation_factory.h +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef WAVEMAP_ROS_OPERATIONS_OPERATION_FACTORY_H_ -#define WAVEMAP_ROS_OPERATIONS_OPERATION_FACTORY_H_ - -#include -#include - -#include -#include -#include - -#include "wavemap_ros/operations/operation_base.h" -#include "wavemap_ros/utils/tf_transformer.h" - -namespace wavemap { -class OperationFactory { - public: - static std::unique_ptr create( - const param::Value& params, std::string world_frame, - MapBase::Ptr occupancy_map, std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh_private, - std::optional default_operation_type = std::nullopt); - - static std::unique_ptr create( - OperationType operation_type, const param::Value& params, - std::string world_frame, MapBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh_private); -}; -} // namespace wavemap - -#endif // WAVEMAP_ROS_OPERATIONS_OPERATION_FACTORY_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/prune_map_operation.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/prune_map_operation.h deleted file mode 100644 index b5b1d70f8..000000000 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/prune_map_operation.h +++ /dev/null @@ -1,51 +0,0 @@ -#ifndef WAVEMAP_ROS_OPERATIONS_PRUNE_MAP_OPERATION_H_ -#define WAVEMAP_ROS_OPERATIONS_PRUNE_MAP_OPERATION_H_ - -#include - -#include -#include - -#include "wavemap_ros/operations/operation_base.h" - -namespace wavemap { -/** - * Config struct for map pruning operations. - */ -struct PruneMapOperationConfig : public ConfigBase { - //! Time period controlling how often the map is pruned. - Seconds once_every = 10.f; - - static MemberMap memberMap; - - bool isValid(bool verbose) const override; -}; - -class PruneMapOperation : public OperationBase { - public: - PruneMapOperation(const PruneMapOperationConfig& config, - MapBase::Ptr occupancy_map) - : config_(config.checkValid()), - occupancy_map_(std::move(occupancy_map)) {} - - OperationType getType() const override { return OperationType::kPruneMap; } - - bool shouldRun(const ros::Time& current_time) { - return config_.once_every < (current_time - last_run_timestamp_).toSec(); - } - - void run(const ros::Time& current_time, bool force_run) override { - if (force_run || shouldRun(current_time)) { - occupancy_map_->pruneSmart(); - last_run_timestamp_ = current_time; - } - } - - private: - const PruneMapOperationConfig config_; - const MapBase::Ptr occupancy_map_; - ros::Time last_run_timestamp_; -}; -} // namespace wavemap - -#endif // WAVEMAP_ROS_OPERATIONS_PRUNE_MAP_OPERATION_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/threshold_map_operation.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/threshold_map_operation.h deleted file mode 100644 index 5c05b1b77..000000000 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/operations/threshold_map_operation.h +++ /dev/null @@ -1,54 +0,0 @@ -#ifndef WAVEMAP_ROS_OPERATIONS_THRESHOLD_MAP_OPERATION_H_ -#define WAVEMAP_ROS_OPERATIONS_THRESHOLD_MAP_OPERATION_H_ - -#include - -#include -#include - -#include "wavemap_ros/operations/operation_base.h" - -namespace wavemap { -/** - * Config struct for map thresholding operations. - */ -struct ThresholdMapOperationConfig - : public ConfigBase { - //! Time period controlling how often the map is thresholded. - Seconds once_every = 2.f; - - static MemberMap memberMap; - - bool isValid(bool verbose) const override; -}; - -class ThresholdMapOperation : public OperationBase { - public: - ThresholdMapOperation(const ThresholdMapOperationConfig& config, - MapBase::Ptr occupancy_map) - : config_(config.checkValid()), - occupancy_map_(std::move(occupancy_map)) {} - - OperationType getType() const override { - return OperationType::kThresholdMap; - } - - bool shouldRun(const ros::Time& current_time) { - return config_.once_every < (current_time - last_run_timestamp_).toSec(); - } - - void run(const ros::Time& current_time, bool force_run) override { - if (force_run || shouldRun(current_time)) { - occupancy_map_->threshold(); - last_run_timestamp_ = current_time; - } - } - - private: - const ThresholdMapOperationConfig config_; - const MapBase::Ptr occupancy_map_; - ros::Time last_run_timestamp_; -}; -} // namespace wavemap - -#endif // WAVEMAP_ROS_OPERATIONS_THRESHOLD_MAP_OPERATION_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h index a0b49cdeb..2c47b8606 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h @@ -15,9 +15,9 @@ #include #include #include +#include #include "wavemap_ros/inputs/input_base.h" -#include "wavemap_ros/operations/operation_base.h" #include "wavemap_ros/utils/logging_level.h" #include "wavemap_ros/utils/tf_transformer.h" @@ -49,16 +49,23 @@ class RosServer { RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, const RosServerConfig& config); - InputBase* addInput(const param::Value& integrator_params, - const ros::NodeHandle& nh, ros::NodeHandle nh_private); - OperationBase* addOperation(const param::Value& operation_params, - ros::NodeHandle nh_private); - - void runOperations(const ros::Time& current_time, bool force_run_all = false); + void clear(); MapBase::Ptr getMap() { return occupancy_map_; } MapBase::ConstPtr getMap() const { return occupancy_map_; } + Pipeline& getPipeline() { return *pipeline_; } + const Pipeline& getPipeline() const { return *pipeline_; } + + MapOperationBase* addOperation(const param::Value& operation_params, + ros::NodeHandle nh_private); + + InputBase* addInput(const param::Value& integrator_params, + const ros::NodeHandle& nh, ros::NodeHandle nh_private); + InputBase* addInput(std::unique_ptr input); + const std::vector>& getInputs() { return inputs_; } + void clearInputs() { inputs_.clear(); } + bool saveMap(const std::filesystem::path& file_path) const; bool loadMap(const std::filesystem::path& file_path); @@ -71,12 +78,12 @@ class RosServer { // Threadpool shared among all input handlers and operations std::shared_ptr thread_pool_; - // Transform and depth inputs - std::shared_ptr transformer_; - std::vector> input_handlers_; + // Map management pipeline + std::shared_ptr pipeline_; - // Operations to perform after map updates - std::vector> operations_; + // Measurement and pose inputs + std::vector> inputs_; + std::shared_ptr transformer_; // ROS services void advertiseServices(ros::NodeHandle& nh_private); diff --git a/interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc index 37bd13b58..72ed766bb 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc @@ -2,6 +2,10 @@ #include #include +#include +#include +#include +#include #include #include #include @@ -16,8 +20,7 @@ DECLARE_CONFIG_MEMBERS(DepthImageInputConfig, (image_transport_hints) (depth_scale_factor) (time_offset) - (reprojected_pointcloud_topic_name) - (projected_range_image_topic_name)); + (projected_pointcloud_topic_name)); bool DepthImageInputConfig::isValid(bool verbose) const { bool all_valid = true; @@ -30,34 +33,27 @@ bool DepthImageInputConfig::isValid(bool verbose) const { return all_valid; } -DepthImageInput::DepthImageInput( - const DepthImageInputConfig& config, const param::Value& params, - std::string world_frame, MapBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh, - ros::NodeHandle nh_private, - std::function map_update_callback) - : InputBase(config, params, std::move(world_frame), - std::move(occupancy_map), std::move(transformer), - std::move(thread_pool), nh, nh_private, - std::move(map_update_callback)), +DepthImageInput::DepthImageInput(const DepthImageInputConfig& config, + std::shared_ptr pipeline, + std::vector integrator_names, + std::shared_ptr transformer, + std::string world_frame, ros::NodeHandle nh, + ros::NodeHandle nh_private) + : InputBase(config, std::move(pipeline), std::move(integrator_names), + std::move(transformer), std::move(world_frame), nh, nh_private), config_(config.checkValid()) { - // Get pointers to the underlying scanwise integrators - for (const auto& integrator : integrators_) { - auto scanwise_integrator = - std::dynamic_pointer_cast(integrator); - CHECK(scanwise_integrator) - << "Depth image inputs are currently only supported in " - "combination with projective integrators."; - scanwise_integrators_.emplace_back(std::move(scanwise_integrator)); - } - // Subscribe to the depth image input image_transport::ImageTransport it(nh); depth_image_sub_ = it.subscribe( config_.topic_name, config_.topic_queue_length, &DepthImageInput::callback, this, image_transport::TransportHints(config_.image_transport_hints)); + + // Advertise the projected pointcloud publisher if enabled + if (!config_.projected_pointcloud_topic_name.empty()) { + projected_pointcloud_pub_ = nh_private.advertise( + config_.projected_pointcloud_topic_name, config_.topic_queue_length); + } } void DepthImageInput::processQueue() { @@ -100,46 +96,26 @@ void DepthImageInput::processQueue() { cv_image->image.convertTo(cv_image->image, CV_32FC1, config_.depth_scale_factor); - // Create the posed range image input - PosedImage<> posed_range_image(cv_image->image.rows, cv_image->image.cols); - cv::cv2eigen(cv_image->image, posed_range_image.getData()); - posed_range_image.setPose(T_W_C); + // Create the posed depth image input + PosedImage<> posed_depth_image(cv_image->image.rows, cv_image->image.cols); + cv::cv2eigen(cv_image->image, posed_depth_image.getData()); + posed_depth_image.setPose(T_W_C); // Integrate the depth image ROS_DEBUG_STREAM("Inserting depth image with " - << print::eigen::oneLine(posed_range_image.getDimensions()) + << print::eigen::oneLine(posed_depth_image.getDimensions()) << " points. Remaining pointclouds in queue: " << depth_image_queue_.size() - 1 << "."); integration_timer_.start(); - for (const auto& integrator : scanwise_integrators_) { - integrator->integrateRangeImage(posed_range_image); - } + pipeline_->runPipeline(integrator_names_, posed_depth_image); integration_timer_.stop(); ROS_DEBUG_STREAM("Integrated new depth image in " << integration_timer_.getLastEpisodeDuration() << "s. Total integration time: " << integration_timer_.getTotalDuration() << "s."); - // Notify subscribers that the map was updated - if (map_update_callback_) { - std::invoke(map_update_callback_, stamp); - } - // Publish debugging visualizations - if (shouldPublishReprojectedPointcloud()) { - const auto posed_pointcloud = reproject(posed_range_image); - publishReprojectedPointcloud(stamp, posed_pointcloud); - } - if (shouldPublishProjectedRangeImage()) { - auto projective_integrator = - std::dynamic_pointer_cast(integrators_.front()); - if (projective_integrator) { - const auto& range_image = projective_integrator->getPosedRangeImage(); - if (range_image) { - publishProjectedRangeImage(stamp, *range_image); - } - } - } + publishProjectedPointcloudIfEnabled(stamp, posed_depth_image); ProfilerFrameMarkNamed("DepthImage"); // Remove the depth image from the queue @@ -147,28 +123,58 @@ void DepthImageInput::processQueue() { } } -PosedPointcloud<> DepthImageInput::reproject( - const PosedImage<>& posed_range_image) { +void DepthImageInput::publishProjectedPointcloudIfEnabled( + const ros::Time& stamp, + const PosedImage& /*posed_depth_image*/) { ProfilerZoneScoped; - auto projective_integrator = - std::dynamic_pointer_cast(integrators_.front()); - if (!projective_integrator) { - return {}; + if (config_.projected_pointcloud_topic_name.empty() || + projected_pointcloud_pub_.getNumSubscribers() <= 0) { + return; } - const auto& projection_model = projective_integrator->getProjectionModel(); + sensor_msgs::PointCloud pointcloud_msg; + pointcloud_msg.header.stamp = stamp; + pointcloud_msg.header.frame_id = world_frame_; + + // TODO(victorr): Reimplement this + // auto projective_integrator = + // std::dynamic_pointer_cast(integrators_.front()); + // if (!projective_integrator) { + // return {}; + // } + // const auto& projection_model = + // projective_integrator->getProjectionModel(); + + // const auto posed_pointcloud = project(posed_depth_image, + // projection_model); pointcloud_msg.points.reserve(posed_pointcloud.size()); + // for (const auto& point : posed_pointcloud.getPointsGlobal()) { + // auto& point_msg = pointcloud_msg.points.emplace_back(); + // point_msg.x = point.x(); + // point_msg.y = point.y(); + // point_msg.z = point.z(); + // } + // + // sensor_msgs::PointCloud2 pointcloud2_msg; + // sensor_msgs::convertPointCloudToPointCloud2(pointcloud_msg, + // pointcloud2_msg); projected_pointcloud_pub_.publish(pointcloud2_msg); +} + +PosedPointcloud DepthImageInput::project( + const PosedImage<>& posed_depth_image, + const ProjectorBase& projection_model) { + ProfilerZoneScoped; std::vector pointcloud; - pointcloud.reserve(posed_range_image.size()); + pointcloud.reserve(posed_depth_image.size()); for (const Index2D& index : Grid<2>(Index2D::Zero(), - posed_range_image.getDimensions() - Index2D::Ones())) { - const Vector2D image_xy = projection_model->indexToImage(index); - const FloatingPoint image_z = posed_range_image.at(index); + posed_depth_image.getDimensions() - Index2D::Ones())) { + const Vector2D image_xy = projection_model.indexToImage(index); + const FloatingPoint image_z = posed_depth_image.at(index); const Point3D C_point = - projection_model->sensorToCartesian(image_xy, image_z); + projection_model.sensorToCartesian(image_xy, image_z); pointcloud.emplace_back(C_point); } - return PosedPointcloud<>(posed_range_image.getPose(), pointcloud); + return PosedPointcloud{posed_depth_image.getPose(), pointcloud}; } } // namespace wavemap diff --git a/interfaces/ros1/wavemap_ros/src/inputs/input_base.cc b/interfaces/ros1/wavemap_ros/src/inputs/input_base.cc index 7d4a24579..56cee3217 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/input_base.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/input_base.cc @@ -1,21 +1,10 @@ #include "wavemap_ros/inputs/input_base.h" -#include -#include -#include -#include -#include -#include -#include -#include - namespace wavemap { DECLARE_CONFIG_MEMBERS(InputBaseConfig, (topic_name) (topic_queue_length) - (processing_retry_period) - (reprojected_pointcloud_topic_name) - (projected_range_image_topic_name)); + (processing_retry_period)); bool InputBaseConfig::isValid(bool verbose) const { bool all_valid = true; @@ -27,95 +16,20 @@ bool InputBaseConfig::isValid(bool verbose) const { return all_valid; } -InputBase::InputBase(const InputBaseConfig& config, const param::Value& params, - std::string world_frame, MapBase::Ptr occupancy_map, +InputBase::InputBase(const InputBaseConfig& config, + std::shared_ptr pipeline, + std::vector integrator_names, std::shared_ptr transformer, - std::shared_ptr thread_pool, - const ros::NodeHandle& nh, ros::NodeHandle nh_private, - std::function map_update_callback) + std::string world_frame, const ros::NodeHandle& nh, + ros::NodeHandle /*nh_private*/) : config_(config.checkValid()), - world_frame_(std::move(world_frame)), + pipeline_(std::move(pipeline)), + integrator_names_(std::move(integrator_names)), transformer_(std::move(transformer)), - map_update_callback_(std::move(map_update_callback)) { - // Create the integrators - const auto integrators_param = params.getChild("integrators"); - if (!integrators_param) { - ROS_WARN( - "Could not find key named \"integrators\" in input handler " - "params. Input handler will be disabled."); - return; - } - const auto integrators_array = integrators_param->as(); - if (!integrators_array) { - ROS_WARN( - "Key named \"integrators\" in input handler params is not " - "of type Array (list). Input handler will be disabled."); - return; - } - for (const auto& integrator_params : integrators_array.value()) { - auto integrator = - IntegratorFactory::create(integrator_params, occupancy_map, thread_pool, - IntegratorType::kRayTracingIntegrator); - CHECK_NOTNULL(integrator); - integrators_.emplace_back(std::move(integrator)); - } - + world_frame_(std::move(world_frame)) { // Start the queue processing retry timer queue_processing_retry_timer_ = nh.createTimer(ros::Duration(config_.processing_retry_period), [this](const auto& /*event*/) { processQueue(); }); - - // Advertise the reprojected pointcloud publisher if enabled - if (!config_.reprojected_pointcloud_topic_name.empty()) { - reprojected_pointcloud_pub_ = - nh_private.advertise( - config_.reprojected_pointcloud_topic_name, - config_.topic_queue_length); - } - if (!config_.projected_range_image_topic_name.empty()) { - image_transport::ImageTransport it_private(nh_private); - projected_range_image_pub_ = it_private.advertise( - config_.projected_range_image_topic_name, config_.topic_queue_length); - } -} - -bool InputBase::shouldPublishReprojectedPointcloud() const { - return !config_.reprojected_pointcloud_topic_name.empty() && - 0 < reprojected_pointcloud_pub_.getNumSubscribers(); -} - -void InputBase::publishReprojectedPointcloud( - const ros::Time& stamp, const PosedPointcloud<>& posed_pointcloud) { - ProfilerZoneScoped; - sensor_msgs::PointCloud pointcloud_msg; - pointcloud_msg.header.stamp = stamp; - pointcloud_msg.header.frame_id = world_frame_; - - pointcloud_msg.points.reserve(posed_pointcloud.size()); - for (const auto& point : posed_pointcloud.getPointsGlobal()) { - auto& point_msg = pointcloud_msg.points.emplace_back(); - point_msg.x = point.x(); - point_msg.y = point.y(); - point_msg.z = point.z(); - } - - sensor_msgs::PointCloud2 pointcloud2_msg; - sensor_msgs::convertPointCloudToPointCloud2(pointcloud_msg, pointcloud2_msg); - reprojected_pointcloud_pub_.publish(pointcloud2_msg); -} - -bool InputBase::shouldPublishProjectedRangeImage() const { - return !config_.projected_range_image_topic_name.empty() && - 0 < projected_range_image_pub_.getNumSubscribers(); -} - -void InputBase::publishProjectedRangeImage(const ros::Time& stamp, - const Image<>& range_image) { - ProfilerZoneScoped; - cv_bridge::CvImage cv_image; - cv_image.header.stamp = stamp; - cv_image.encoding = "32FC1"; - cv::eigen2cv(range_image.getData(), cv_image.image); - projected_range_image_pub_.publish(cv_image.toImageMsg()); } } // namespace wavemap diff --git a/interfaces/ros1/wavemap_ros/src/inputs/input_factory.cc b/interfaces/ros1/wavemap_ros/src/inputs/input_factory.cc index f132ee6ca..ded052c78 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/input_factory.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/input_factory.cc @@ -5,63 +5,79 @@ namespace wavemap { std::unique_ptr InputFactory::create( - const param::Value& params, std::string world_frame, - MapBase::Ptr occupancy_map, std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh, - ros::NodeHandle nh_private, std::optional default_input_type, - std::function map_update_callback) { + const param::Value& params, std::shared_ptr pipeline, + std::shared_ptr transformer, std::string world_frame, + ros::NodeHandle nh, ros::NodeHandle nh_private) { if (const auto type = InputType::from(params); type) { - return create(type.value(), params, world_frame, occupancy_map, - std::move(transformer), std::move(thread_pool), nh, - nh_private, std::move(map_update_callback)); + return create(type.value(), params, std::move(pipeline), + std::move(transformer), std::move(world_frame), nh, + nh_private); } - if (default_input_type.has_value()) { - ROS_WARN_STREAM("Default type \"" << default_input_type.value().toStr() - << "\" will be created instead."); - return create(default_input_type.value(), params, std::move(world_frame), - std::move(occupancy_map), std::move(transformer), - std::move(thread_pool), nh, nh_private, - std::move(map_update_callback)); - } - - ROS_ERROR("No default was set. Returning nullptr."); + LOG(ERROR) << "Could not create input handler. Returning nullptr."; return nullptr; } std::unique_ptr InputFactory::create( - InputType input_type, const param::Value& params, std::string world_frame, - MapBase::Ptr occupancy_map, std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh, - ros::NodeHandle nh_private, - std::function map_update_callback) { + InputType input_type, const param::Value& params, + std::shared_ptr pipeline, + std::shared_ptr transformer, std::string world_frame, + ros::NodeHandle nh, ros::NodeHandle nh_private) { if (!input_type.isValid()) { ROS_ERROR("Received request to create input handler with invalid type."); return nullptr; } + // Get the integrator names + std::vector integrator_names; + if (const auto names_param = params.getChild("measurement_integrator"); + names_param) { + if (const auto name_str = names_param->as(); name_str) { + integrator_names.emplace_back(name_str.value()); + } else if (const auto name_arr = names_param->as(); + name_arr) { + size_t name_idx = 0u; + for (const auto& name_el : name_arr.value()) { + if (const auto name_el_str = name_el.as(); name_el_str) { + integrator_names.emplace_back(name_el_str.value()); + } else { + ROS_WARN_STREAM( + "Skipping \"measurement_integrator\" element at index " + << name_idx << ". Could not be parsed as string."); + } + ++name_idx; + } + } else { + ROS_ERROR( + "Param \"measurement_integrator\" should be a string " + "or list of strings."); + } + } else { + ROS_ERROR( + "No integrator name(s) specified. Provide them by setting param " + "\"measurement_integrator\" to a string or list of strings."); + } + if (integrator_names.empty()) { + ROS_ERROR("Could not create input. Returning nullptr."); + return nullptr; + } + // Create the input handler switch (input_type) { case InputType::kPointcloud: - if (const auto config = PointcloudInputConfig::from(params, "general"); - config) { + if (const auto config = PointcloudInputConfig::from(params); config) { return std::make_unique( - config.value(), params, std::move(world_frame), - std::move(occupancy_map), std::move(transformer), - std::move(thread_pool), nh, nh_private, - std::move(map_update_callback)); + config.value(), std::move(pipeline), std::move(integrator_names), + std::move(transformer), std::move(world_frame), nh, nh_private); } else { ROS_ERROR("Pointcloud input handler config could not be loaded."); return nullptr; } case InputType::kDepthImage: - if (const auto config = DepthImageInputConfig::from(params, "general"); - config) { + if (const auto config = DepthImageInputConfig::from(params); config) { return std::make_unique( - config.value(), params, std::move(world_frame), - std::move(occupancy_map), std::move(transformer), - std::move(thread_pool), nh, nh_private, - std::move(map_update_callback)); + config.value(), std::move(pipeline), std::move(integrator_names), + std::move(transformer), std::move(world_frame), nh, nh_private); } else { ROS_ERROR("Depth image input handler config could not be loaded."); return nullptr; diff --git a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc index 79e466b24..fe1b10249 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc @@ -1,6 +1,12 @@ #include "wavemap_ros/inputs/pointcloud_input.h" +#include +#include +#include +#include #include +#include +#include #include #include #include @@ -16,8 +22,8 @@ DECLARE_CONFIG_MEMBERS(PointcloudInputConfig, (time_offset) (undistort_motion) (num_undistortion_interpolation_intervals_per_cloud) - (reprojected_pointcloud_topic_name) - (projected_range_image_topic_name)); + (projected_range_image_topic_name) + (undistorted_pointcloud_topic_name)); bool PointcloudInputConfig::isValid(bool verbose) const { bool all_valid = true; @@ -30,16 +36,14 @@ bool PointcloudInputConfig::isValid(bool verbose) const { return all_valid; } -PointcloudInput::PointcloudInput( - const PointcloudInputConfig& config, const param::Value& params, - std::string world_frame, MapBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh, - ros::NodeHandle nh_private, - std::function map_update_callback) - : InputBase(config, params, std::move(world_frame), - std::move(occupancy_map), transformer, std::move(thread_pool), - nh, nh_private, std::move(map_update_callback)), +PointcloudInput::PointcloudInput(const PointcloudInputConfig& config, + std::shared_ptr pipeline, + std::vector integrator_names, + std::shared_ptr transformer, + std::string world_frame, ros::NodeHandle nh, + ros::NodeHandle nh_private) + : InputBase(config, std::move(pipeline), std::move(integrator_names), + transformer, std::move(world_frame), nh, nh_private), config_(config.checkValid()), pointcloud_undistorter_( transformer, @@ -49,6 +53,19 @@ PointcloudInput::PointcloudInput( pointcloud_sub_ = nh.subscribe( config_.topic_name, config_.topic_queue_length, callback_ptr, this); }); + + // Advertise the range image and undistorted pointcloud publishers if enabled + if (!config_.projected_range_image_topic_name.empty()) { + image_transport::ImageTransport it_private(nh_private); + projected_range_image_pub_ = it_private.advertise( + config_.projected_range_image_topic_name, config_.topic_queue_length); + } + if (!config_.undistorted_pointcloud_topic_name.empty()) { + undistorted_pointcloud_pub_ = + nh_private.advertise( + config_.undistorted_pointcloud_topic_name, + config_.topic_queue_length); + } } void PointcloudInput::callback(const sensor_msgs::PointCloud2& pointcloud_msg) { @@ -234,39 +251,20 @@ void PointcloudInput::processQueue() { << " points. Remaining pointclouds in queue: " << pointcloud_queue_.size() - 1 << "."); integration_timer_.start(); - for (const auto& integrator : integrators_) { - integrator->integratePointcloud(posed_pointcloud); - } + pipeline_->runPipeline(integrator_names_, posed_pointcloud); integration_timer_.stop(); ROS_DEBUG_STREAM("Integrated new pointcloud in " << integration_timer_.getLastEpisodeDuration() << "s. Total integration time: " << integration_timer_.getTotalDuration() << "s."); - // Notify subscribers that the map was updated - if (map_update_callback_) { - std::invoke(map_update_callback_, - convert::nanoSecondsToRosTime(oldest_msg.getEndTime())); - } - // Publish debugging visualizations - if (shouldPublishReprojectedPointcloud()) { - publishReprojectedPointcloud( - convert::nanoSecondsToRosTime(oldest_msg.getMedianTime()), - posed_pointcloud); - } - if (shouldPublishProjectedRangeImage()) { - auto projective_integrator = - std::dynamic_pointer_cast(integrators_.front()); - if (projective_integrator) { - const auto& range_image = projective_integrator->getPosedRangeImage(); - if (range_image) { - publishProjectedRangeImage( - convert::nanoSecondsToRosTime(oldest_msg.getMedianTime()), - *range_image); - } - } - } + publishProjectedRangeImageIfEnabled( + convert::nanoSecondsToRosTime(oldest_msg.getMedianTime()), + posed_pointcloud); + publishUndistortedPointcloudIfEnabled( + convert::nanoSecondsToRosTime(oldest_msg.getMedianTime()), + posed_pointcloud); ProfilerFrameMarkNamed("Pointcloud"); // Remove the pointcloud from the queue @@ -282,4 +280,53 @@ bool PointcloudInput::hasField(const sensor_msgs::PointCloud2& msg, return field.name == field_name; }); } + +void PointcloudInput::publishProjectedRangeImageIfEnabled( + const ros::Time& /*stamp*/, const PosedPointcloud<>& /*posed_pointcloud*/) { + ProfilerZoneScoped; + if (config_.projected_range_image_topic_name.empty() || + projected_range_image_pub_.getNumSubscribers() <= 0) { + return; + } + + // TODO(victorr): Reimplement this + // auto projective_integrator = + // std::dynamic_pointer_cast(integrators_.front()); + // if (projective_integrator) { + // const auto& range_image = projective_integrator->getPosedRangeImage(); + // if (range_image) { + // } + // } + // + // cv_bridge::CvImage cv_image; + // cv_image.header.stamp = stamp; + // cv_image.encoding = "32FC1"; + // cv::eigen2cv(range_image.getData(), cv_image.image); + // projected_range_image_pub_.publish(cv_image.toImageMsg()); +} + +void PointcloudInput::publishUndistortedPointcloudIfEnabled( + const ros::Time& stamp, const PosedPointcloud<>& undistorted_pointcloud) { + ProfilerZoneScoped; + if (config_.undistorted_pointcloud_topic_name.empty() || + undistorted_pointcloud_pub_.getNumSubscribers() <= 0) { + return; + } + + sensor_msgs::PointCloud pointcloud_msg; + pointcloud_msg.header.stamp = stamp; + pointcloud_msg.header.frame_id = world_frame_; + + pointcloud_msg.points.reserve(undistorted_pointcloud.size()); + for (const auto& point : undistorted_pointcloud.getPointsGlobal()) { + auto& point_msg = pointcloud_msg.points.emplace_back(); + point_msg.x = point.x(); + point_msg.y = point.y(); + point_msg.z = point.z(); + } + + sensor_msgs::PointCloud2 pointcloud2_msg; + sensor_msgs::convertPointCloudToPointCloud2(pointcloud_msg, pointcloud2_msg); + undistorted_pointcloud_pub_.publish(pointcloud2_msg); +} } // namespace wavemap diff --git a/interfaces/ros1/wavemap_ros/src/operations/crop_map_operation.cc b/interfaces/ros1/wavemap_ros/src/map_operations/crop_map_operation.cc similarity index 78% rename from interfaces/ros1/wavemap_ros/src/operations/crop_map_operation.cc rename to interfaces/ros1/wavemap_ros/src/map_operations/crop_map_operation.cc index 6f47f1637..2e944ed6b 100644 --- a/interfaces/ros1/wavemap_ros/src/operations/crop_map_operation.cc +++ b/interfaces/ros1/wavemap_ros/src/map_operations/crop_map_operation.cc @@ -1,4 +1,4 @@ -#include "wavemap_ros/operations/crop_map_operation.h" +#include "wavemap_ros/map_operations/crop_map_operation.h" #include #include @@ -20,7 +20,21 @@ bool CropMapOperationConfig::isValid(bool verbose) const { return all_valid; } -void CropMapOperation::run(const ros::Time& current_time, bool force_run) { +CropMapOperation::CropMapOperation(const CropMapOperationConfig& config, + MapBase::Ptr occupancy_map, + std::shared_ptr transformer, + std::string world_frame) + : MapOperationBase(std::move(occupancy_map)), + config_(config.checkValid()), + transformer_(std::move(transformer)), + world_frame_(std::move(world_frame)) {} + +bool CropMapOperation::shouldRun(const ros::Time& current_time) { + return config_.once_every < (current_time - last_run_timestamp_).toSec(); +} + +void CropMapOperation::run(bool force_run) { + const ros::Time current_time = ros::Time::now(); if (!force_run && !shouldRun(current_time)) { return; } diff --git a/interfaces/ros1/wavemap_ros/src/map_operations/map_ros_operation_factory.cc b/interfaces/ros1/wavemap_ros/src/map_operations/map_ros_operation_factory.cc new file mode 100644 index 000000000..3df1e889a --- /dev/null +++ b/interfaces/ros1/wavemap_ros/src/map_operations/map_ros_operation_factory.cc @@ -0,0 +1,69 @@ +#include "wavemap_ros/map_operations/map_ros_operation_factory.h" + +#include "wavemap_ros/map_operations/crop_map_operation.h" +#include "wavemap_ros/map_operations/publish_map_operation.h" +#include "wavemap_ros/map_operations/publish_pointcloud_operation.h" + +namespace wavemap { +std::unique_ptr MapRosOperationFactory::create( + const param::Value& params, MapBase::Ptr occupancy_map, + std::shared_ptr thread_pool, + std::shared_ptr transformer, std::string world_frame, + ros::NodeHandle nh_private) { + if (const auto type = MapRosOperationType::from(params); type) { + return create(type.value(), params, std::move(occupancy_map), + std::move(thread_pool), std::move(transformer), + std::move(world_frame), nh_private); + } + + LOG(ERROR) << "Could not create map operation. Returning nullptr."; + return nullptr; +} + +std::unique_ptr MapRosOperationFactory::create( + MapRosOperationType ros_operation_type, const param::Value& params, + MapBase::Ptr occupancy_map, std::shared_ptr thread_pool, + std::shared_ptr transformer, std::string world_frame, + ros::NodeHandle nh_private) { + if (!ros_operation_type.isValid()) { + LOG(ERROR) << "Received request to create map operation with invalid type."; + return nullptr; + } + + // Create the operation handler + switch (ros_operation_type) { + case MapRosOperationType::kPublishMap: + if (const auto config = PublishMapOperationConfig::from(params); config) { + return std::make_unique( + config.value(), std::move(occupancy_map), std::move(thread_pool), + std::move(world_frame), nh_private); + } else { + ROS_ERROR("Publish map operation config could not be loaded."); + return nullptr; + } + case MapRosOperationType::kPublishPointcloud: + if (const auto config = PublishPointcloudOperationConfig::from(params); + config) { + return std::make_unique( + config.value(), std::move(occupancy_map), std::move(world_frame), + nh_private); + } else { + ROS_ERROR("Publish pointcloud operation config could not be loaded."); + return nullptr; + } + case MapRosOperationType::kCropMap: + if (const auto config = CropMapOperationConfig::from(params); config) { + return std::make_unique( + config.value(), std::move(occupancy_map), std::move(transformer), + std::move(world_frame)); + } else { + ROS_ERROR("Crop map operation config could not be loaded."); + return nullptr; + } + } + + LOG(ERROR) << "Factory does not (yet) support creation of map operation type " + << ros_operation_type.toStr() << "."; + return nullptr; +} +} // namespace wavemap diff --git a/interfaces/ros1/wavemap_ros/src/operations/publish_map_operation.cc b/interfaces/ros1/wavemap_ros/src/map_operations/publish_map_operation.cc similarity index 74% rename from interfaces/ros1/wavemap_ros/src/operations/publish_map_operation.cc rename to interfaces/ros1/wavemap_ros/src/map_operations/publish_map_operation.cc index 3c6414840..924ae4782 100644 --- a/interfaces/ros1/wavemap_ros/src/operations/publish_map_operation.cc +++ b/interfaces/ros1/wavemap_ros/src/map_operations/publish_map_operation.cc @@ -1,4 +1,4 @@ -#include "wavemap_ros/operations/publish_map_operation.h" +#include "wavemap_ros/map_operations/publish_map_operation.h" #include #include @@ -22,13 +22,13 @@ bool PublishMapOperationConfig::isValid(bool verbose) const { } PublishMapOperation::PublishMapOperation( - const PublishMapOperationConfig& config, std::string world_frame, - MapBase::Ptr occupancy_map, std::shared_ptr thread_pool, + const PublishMapOperationConfig& config, MapBase::Ptr occupancy_map, + std::shared_ptr thread_pool, std::string world_frame, ros::NodeHandle nh_private) - : config_(config.checkValid()), - world_frame_(std::move(world_frame)), - occupancy_map_(std::move(occupancy_map)), - thread_pool_(std::move(thread_pool)) { + : MapOperationBase(std::move(occupancy_map)), + config_(config.checkValid()), + thread_pool_(std::move(thread_pool)), + world_frame_(std::move(world_frame)) { map_pub_ = nh_private.advertise(config_.topic, 10); republish_whole_map_srv_ = nh_private.advertiseService #include @@ -27,15 +27,27 @@ bool PublishPointcloudOperationConfig::isValid(bool verbose) const { } PublishPointcloudOperation::PublishPointcloudOperation( - const PublishPointcloudOperationConfig& config, std::string world_frame, - MapBase::Ptr occupancy_map, ros::NodeHandle nh_private) - : config_(config.checkValid()), - world_frame_(std::move(world_frame)), - occupancy_map_(std::move(occupancy_map)) { + const PublishPointcloudOperationConfig& config, MapBase::Ptr occupancy_map, + std::string world_frame, ros::NodeHandle nh_private) + : MapOperationBase(std::move(occupancy_map)), + config_(config.checkValid()), + world_frame_(std::move(world_frame)) { pointcloud_pub_ = nh_private.advertise(config_.topic, 10); } +bool PublishPointcloudOperation::shouldRun(const ros::Time& current_time) { + return config_.once_every < (current_time - last_run_timestamp_).toSec(); +} + +void PublishPointcloudOperation::run(bool force_run) { + const ros::Time current_time = ros::Time::now(); + if (force_run || shouldRun(current_time)) { + publishPointcloud(current_time); + last_run_timestamp_ = current_time; + } +} + void PublishPointcloudOperation::publishPointcloud( const ros::Time& current_time) { ProfilerZoneScoped; diff --git a/interfaces/ros1/wavemap_ros/src/operations/operation_factory.cc b/interfaces/ros1/wavemap_ros/src/operations/operation_factory.cc deleted file mode 100644 index 3a52654bb..000000000 --- a/interfaces/ros1/wavemap_ros/src/operations/operation_factory.cc +++ /dev/null @@ -1,96 +0,0 @@ -#include "wavemap_ros/operations/operation_factory.h" - -#include "wavemap_ros/operations/crop_map_operation.h" -#include "wavemap_ros/operations/prune_map_operation.h" -#include "wavemap_ros/operations/publish_map_operation.h" -#include "wavemap_ros/operations/publish_pointcloud_operation.h" -#include "wavemap_ros/operations/threshold_map_operation.h" - -namespace wavemap { -std::unique_ptr OperationFactory::create( - const param::Value& params, std::string world_frame, - MapBase::Ptr occupancy_map, std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh_private, - std::optional default_operation_type) { - if (const auto type = OperationType::from(params); type) { - return create(type.value(), params, std::move(world_frame), - std::move(occupancy_map), std::move(transformer), - std::move(thread_pool), nh_private); - } - - if (default_operation_type.has_value()) { - ROS_WARN_STREAM("Default type \"" << default_operation_type.value().toStr() - << "\" will be created instead."); - return create(default_operation_type.value(), params, - std::move(world_frame), std::move(occupancy_map), - std::move(transformer), std::move(thread_pool), nh_private); - } - - ROS_ERROR("No default was set. Returning nullptr."); - return nullptr; -} - -std::unique_ptr OperationFactory::create( - OperationType operation_type, const param::Value& params, - std::string world_frame, MapBase::Ptr occupancy_map, - std::shared_ptr transformer, - std::shared_ptr thread_pool, ros::NodeHandle nh_private) { - if (!operation_type.isValid()) { - ROS_ERROR("Received request to create operation with invalid type."); - return nullptr; - } - - // Create the operation handler - switch (operation_type) { - case OperationType::kThresholdMap: - if (const auto config = ThresholdMapOperationConfig::from(params); - config) { - return std::make_unique( - config.value(), std::move(occupancy_map)); - } else { - ROS_ERROR("Threshold map operation config could not be loaded."); - return nullptr; - } - case OperationType::kPruneMap: - if (const auto config = PruneMapOperationConfig::from(params); config) { - return std::make_unique(config.value(), - std::move(occupancy_map)); - } else { - ROS_ERROR("Prune map operation config could not be loaded."); - return nullptr; - } - case OperationType::kPublishMap: - if (const auto config = PublishMapOperationConfig::from(params); config) { - return std::make_unique( - config.value(), std::move(world_frame), std::move(occupancy_map), - std::move(thread_pool), nh_private); - } else { - ROS_ERROR("Publish map operation config could not be loaded."); - return nullptr; - } - case OperationType::kPublishPointcloud: - if (const auto config = PublishPointcloudOperationConfig::from(params); - config) { - return std::make_unique( - config.value(), std::move(world_frame), std::move(occupancy_map), - nh_private); - } else { - ROS_ERROR("Publish pointcloud operation config could not be loaded."); - return nullptr; - } - case OperationType::kCropMap: - if (const auto config = CropMapOperationConfig::from(params); config) { - return std::make_unique( - config.value(), std::move(world_frame), std::move(transformer), - std::move(occupancy_map)); - } else { - ROS_ERROR("Crop map operation config could not be loaded."); - return nullptr; - } - } - - ROS_ERROR_STREAM("Factory does not (yet) support creation of operation type " - << operation_type.toStr() << "."); - return nullptr; -} -} // namespace wavemap diff --git a/interfaces/ros1/wavemap_ros/src/operations/prune_map_operation.cc b/interfaces/ros1/wavemap_ros/src/operations/prune_map_operation.cc deleted file mode 100644 index 57e8156b6..000000000 --- a/interfaces/ros1/wavemap_ros/src/operations/prune_map_operation.cc +++ /dev/null @@ -1,10 +0,0 @@ -#include "wavemap_ros/operations/prune_map_operation.h" - -namespace wavemap { -DECLARE_CONFIG_MEMBERS(PruneMapOperationConfig, - (once_every)); - -bool PruneMapOperationConfig::isValid(bool verbose) const { - return IS_PARAM_GT(once_every, 0.f, verbose); -} -} // namespace wavemap diff --git a/interfaces/ros1/wavemap_ros/src/operations/threshold_map_operation.cc b/interfaces/ros1/wavemap_ros/src/operations/threshold_map_operation.cc deleted file mode 100644 index 6bdb9aef0..000000000 --- a/interfaces/ros1/wavemap_ros/src/operations/threshold_map_operation.cc +++ /dev/null @@ -1,10 +0,0 @@ -#include "wavemap_ros/operations/threshold_map_operation.h" - -namespace wavemap { -DECLARE_CONFIG_MEMBERS(ThresholdMapOperationConfig, - (once_every)); - -bool ThresholdMapOperationConfig::isValid(bool verbose) const { - return IS_PARAM_GT(once_every, 0.f, verbose); -} -} // namespace wavemap diff --git a/interfaces/ros1/wavemap_ros/src/ros_server.cc b/interfaces/ros1/wavemap_ros/src/ros_server.cc index e40a26e73..2c11141e7 100644 --- a/interfaces/ros1/wavemap_ros/src/ros_server.cc +++ b/interfaces/ros1/wavemap_ros/src/ros_server.cc @@ -3,11 +3,12 @@ #include #include #include +#include #include #include #include "wavemap_ros/inputs/input_factory.h" -#include "wavemap_ros/operations/operation_factory.h" +#include "wavemap_ros/map_operations/map_ros_operation_factory.h" namespace wavemap { DECLARE_CONFIG_MEMBERS(RosServerConfig, @@ -57,53 +58,98 @@ RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, thread_pool_ = std::make_shared(config_.num_threads); CHECK_NOTNULL(thread_pool_); - // Setup input handlers - const param::Array integrator_params_array = - param::convert::toParamArray(nh_private, "inputs"); - for (const auto& integrator_params : integrator_params_array) { - addInput(integrator_params, nh, nh_private); - } + // Setup the pipeline + pipeline_ = std::make_shared(occupancy_map_, thread_pool_); + CHECK_NOTNULL(pipeline_); - // Setup operation handlers - const param::Array operation_params_array = - param::convert::toParamArray(nh_private, "operations_pipeline"); - for (const auto& operation_params : operation_params_array) { + // Add map operations to pipeline + const param::Array map_operation_param_array = + param::convert::toParamArray(nh_private, "map_operations_pipeline"); + for (const auto& operation_params : map_operation_param_array) { addOperation(operation_params, nh_private); } + // Add measurement integrators to pipeline + const param::Map measurement_integrator_param_map = + param::convert::toParamMap(nh_private, "measurement_integrators"); + for (const auto& [integrator_name, integrator_params] : + measurement_integrator_param_map) { + pipeline_->addIntegrator(integrator_name, integrator_params); + } + + // Setup measurement inputs + const param::Array input_param_array = + param::convert::toParamArray(nh_private, "inputs"); + for (const auto& integrator_params : input_param_array) { + addInput(integrator_params, nh, nh_private); + } + // Connect to ROS advertiseServices(nh_private); } +void RosServer::clear() { + clearInputs(); + if (pipeline_) { + pipeline_->clear(); + } + if (occupancy_map_) { + occupancy_map_->clear(); + } +} + InputBase* RosServer::addInput(const param::Value& integrator_params, const ros::NodeHandle& nh, ros::NodeHandle nh_private) { - auto input_handler = InputFactory::create( - integrator_params, config_.world_frame, occupancy_map_, transformer_, - thread_pool_, nh, nh_private, std::nullopt, - [this](const ros::Time& current_time) { runOperations(current_time); }); - if (input_handler) { - return input_handlers_.emplace_back(std::move(input_handler)).get(); - } - return nullptr; + auto input = InputFactory::create(integrator_params, pipeline_, transformer_, + config_.world_frame, nh, nh_private); + return addInput(std::move(input)); } -OperationBase* RosServer::addOperation(const param::Value& operation_params, - ros::NodeHandle nh_private) { - auto operation_handler = OperationFactory::create( - operation_params, config_.world_frame, occupancy_map_, transformer_, - thread_pool_, nh_private); - if (operation_handler) { - return operations_.emplace_back(std::move(operation_handler)).get(); +InputBase* RosServer::addInput(std::unique_ptr input) { + if (input) { + return inputs_.emplace_back(std::move(input)).get(); } + + ROS_WARN("Ignoring request to add input. Input is null pointer."); return nullptr; } -void RosServer::runOperations(const ros::Time& current_time, - bool force_run_all) { - for (auto& operation : operations_) { - operation->run(current_time, force_run_all); +MapOperationBase* RosServer::addOperation(const param::Value& operation_params, + ros::NodeHandle nh_private) { + // Read the operation type name from params + const auto type_name = param::getTypeStr(operation_params); + if (!type_name) { + // No type name was defined + // NOTE: A message explaining the failure is already printed by getTypeStr. + ROS_WARN_STREAM( + "Could not add operation. No operation type specified. " + "Please set it by adding a param with key \"" + << param::kTypeSelectorKey << "\"."); + return nullptr; } + + if (const auto type = MapRosOperationType{type_name.value()}; + type.isValid()) { + auto operation = MapRosOperationFactory::create( + type, operation_params, occupancy_map_, thread_pool_, transformer_, + config_.world_frame, nh_private); + return pipeline_->addOperation(std::move(operation)); + } + + if (const auto type = MapOperationType{type_name.value()}; type.isValid()) { + auto operation = + MapOperationFactory::create(type, operation_params, occupancy_map_); + return pipeline_->addOperation(std::move(operation)); + } + + LOG(WARNING) << "Value of type name param \"" << param::kTypeSelectorKey + << "\": \"" << type_name.value() + << "\" does not match a known operation type name. Supported " + "type names are [" + << print::sequence(MapRosOperationType::names) << ", " + << print::sequence(MapOperationType::names) << "]."; + return nullptr; } bool RosServer::saveMap(const std::filesystem::path& file_path) const { diff --git a/library/include/wavemap/core/config/impl/type_selector_inl.h b/library/include/wavemap/core/config/impl/type_selector_inl.h index e4157083b..e2871dce7 100644 --- a/library/include/wavemap/core/config/impl/type_selector_inl.h +++ b/library/include/wavemap/core/config/impl/type_selector_inl.h @@ -5,7 +5,40 @@ #include #include +#include "wavemap/core/utils/print/container.h" + namespace wavemap { +inline std::optional param::getTypeStr( + const param::Value& params) { + // If the param is of type string, read the name directly + if (params.holds()) { + return params.as().value(); + } + + // If the param is of type Map, try to read the name from its "type" subkey + if (params.holds()) { + const auto type_param = params.getChild(param::kTypeSelectorKey); + if (!type_param) { + LOG(WARNING) << "Nested type name (\"" << param::kTypeSelectorKey + << "\") is not set."; + return std::nullopt; + } + + if (!type_param->holds()) { + LOG(WARNING) << "Nested type name (\"" << param::kTypeSelectorKey + << "\") is not a string."; + return std::nullopt; + } + + return type_param->as().value(); + } + + LOG(WARNING) << "Type names can only be read from params of type string, " + "or from param maps (dictionary) with a subkey named \"" + << param::kTypeSelectorKey << "\"."; + return std::nullopt; +} + template TypeSelector::TypeSelector( const TypeSelector::TypeName& type_name) { @@ -71,47 +104,21 @@ template std::optional TypeSelector::from( const param::Value& params) { // Read the type name from params - std::string type_name; - if (params.holds()) { - // If the param is of type string, read the name directly - type_name = params.as().value(); - } else if (params.holds()) { - // If the param is of type Map, try to read the name from its "type" subkey - const auto type_param = params.getChild(param::kTypeSelectorKey); - if (!type_param) { - LOG(WARNING) << "Nested type name (\"" << param::kTypeSelectorKey - << "\") is not set."; - return std::nullopt; - } - - if (!type_param->holds()) { - LOG(WARNING) << "Nested type name (\"" << param::kTypeSelectorKey - << "\") is not a string."; - return std::nullopt; - } - - type_name = type_param->as().value(); - } else { - LOG(WARNING) << "Type names can only be read from params of type string, " - "or from param maps (dictionary) with a subkey named \"" - << param::kTypeSelectorKey << "\"."; + const auto type_name = param::getTypeStr(params); + if (!type_name) { + // No type name was defined + // NOTE: A message explaining the failure is already printed by getTypeStr. return std::nullopt; } // Match the type name to a type id - DerivedNamedTypeSetT type_id(type_name); + DerivedNamedTypeSetT type_id(type_name.value()); if (!type_id.isValid()) { LOG(WARNING) << "Value of type name param \"" << param::kTypeSelectorKey << "\": \"" - << type_name + << type_name.value() << "\" does not match a known type name. Supported type names are [" - << std::accumulate(std::next(DerivedNamedTypeSetT::names.cbegin()), - DerivedNamedTypeSetT::names.cend(), - std::string(DerivedNamedTypeSetT::names[0]), - [](auto str, const auto& el) -> std::string { - return std::move(str) + ", " + std::string(el); - }) - << "]."; + << print::sequence(DerivedNamedTypeSetT::names) << "]."; return std::nullopt; } diff --git a/library/include/wavemap/core/config/type_selector.h b/library/include/wavemap/core/config/type_selector.h index d3011dc41..0a6ad7561 100644 --- a/library/include/wavemap/core/config/type_selector.h +++ b/library/include/wavemap/core/config/type_selector.h @@ -8,7 +8,8 @@ namespace wavemap { namespace param { static constexpr auto kTypeSelectorKey = "type"; -} +std::optional getTypeStr(const param::Value& params); +} // namespace param template struct TypeSelector { diff --git a/library/include/wavemap/core/integrator/integrator_base.h b/library/include/wavemap/core/integrator/integrator_base.h index 4db619730..728c9f7e3 100644 --- a/library/include/wavemap/core/integrator/integrator_base.h +++ b/library/include/wavemap/core/integrator/integrator_base.h @@ -7,6 +7,7 @@ #include "wavemap/core/common.h" #include "wavemap/core/config/config_base.h" #include "wavemap/core/config/type_selector.h" +#include "wavemap/core/data_structure/image.h" #include "wavemap/core/data_structure/pointcloud.h" namespace wavemap { @@ -35,7 +36,8 @@ class IntegratorBase { IntegratorBase() = default; virtual ~IntegratorBase() = default; - virtual void integratePointcloud(const PosedPointcloud<>& pointcloud) = 0; + virtual void integrate(const PosedPointcloud<>& pointcloud) = 0; + virtual void integrate(const PosedImage<>& range_image) = 0; protected: static bool isPointcloudValid(const PosedPointcloud<>& pointcloud); diff --git a/library/include/wavemap/core/integrator/integrator_factory.h b/library/include/wavemap/core/integrator/integrator_factory.h index 8aec64518..a678eb486 100644 --- a/library/include/wavemap/core/integrator/integrator_factory.h +++ b/library/include/wavemap/core/integrator/integrator_factory.h @@ -11,12 +11,12 @@ namespace wavemap { class IntegratorFactory { public: - static IntegratorBase::Ptr create( + static std::unique_ptr create( const param::Value& params, MapBase::Ptr occupancy_map, std::shared_ptr thread_pool = nullptr, std::optional default_integrator_type = std::nullopt); - static IntegratorBase::Ptr create( + static std::unique_ptr create( IntegratorType integrator_type, const param::Value& params, MapBase::Ptr occupancy_map, std::shared_ptr thread_pool = nullptr); diff --git a/library/include/wavemap/core/integrator/projective/projective_integrator.h b/library/include/wavemap/core/integrator/projective/projective_integrator.h index 047a08fd4..b2d73c3b1 100644 --- a/library/include/wavemap/core/integrator/projective/projective_integrator.h +++ b/library/include/wavemap/core/integrator/projective/projective_integrator.h @@ -62,9 +62,8 @@ class ProjectiveIntegrator : public IntegratorBase { measurement_model_(std::move(CHECK_NOTNULL(measurement_model))) {} // Methods to integrate new pointclouds / depth images into the map - void integratePointcloud( - const PosedPointcloud>& pointcloud) override; - void integrateRangeImage(const PosedImage<>& range_image); + void integrate(const PosedPointcloud<>& pointcloud) override; + void integrate(const PosedImage<>& range_image) override; // Accessors for debugging and visualization // NOTE: These accessors are for introspection only, not for modifying the diff --git a/library/include/wavemap/core/integrator/ray_tracing/ray_tracing_integrator.h b/library/include/wavemap/core/integrator/ray_tracing/ray_tracing_integrator.h index c94098e0e..699eb0620 100644 --- a/library/include/wavemap/core/integrator/ray_tracing/ray_tracing_integrator.h +++ b/library/include/wavemap/core/integrator/ray_tracing/ray_tracing_integrator.h @@ -33,7 +33,13 @@ class RayTracingIntegrator : public IntegratorBase { : config_(config.checkValid()), occupancy_map_(std::move(CHECK_NOTNULL(occupancy_map))) {} - void integratePointcloud(const PosedPointcloud<>& pointcloud) override; + void integrate(const PosedPointcloud<>& pointcloud) override; + + // TODO(victorr): Remove the RayTracingIntegrator and move + // ProjectiveIntegrator interface directly into IntegratorBase + void integrate(const PosedImage<>& /*range_image*/) override { + LOG(FATAL) << "Not implemented"; + } private: using MeasurementModelType = ConstantRay; diff --git a/library/include/wavemap/core/utils/print/container.h b/library/include/wavemap/core/utils/print/container.h index d725601fb..b98f1f390 100644 --- a/library/include/wavemap/core/utils/print/container.h +++ b/library/include/wavemap/core/utils/print/container.h @@ -6,12 +6,26 @@ #include namespace wavemap::print { -template -inline std::string container(const SequenceContainerT& container) { - return std::accumulate(std::next(container.cbegin()), container.cend(), - std::to_string(container[0]), - [](auto str, const auto& el) -> std::string { - return std::move(str) + ", " + std::to_string(el); +template +inline auto element(const ElementT& element) + -> std::enable_if_t, std::string> { + return std::to_string(element); +} + +template +inline auto element(const ElementT& element) + -> std::enable_if_t, std::string> { + return std::string(element); +} + +template +inline std::string sequence(const SequenceT& sequence, + const std::string& separator = ", ") { + return std::accumulate(std::next(sequence.cbegin()), sequence.cend(), + print::element(sequence[0]), + [separator](auto str, const auto& el) -> std::string { + return std::move(str) + separator + + print::element(el); }); } } // namespace wavemap::print diff --git a/library/include/wavemap/pipeline/impl/pipeline_inl.h b/library/include/wavemap/pipeline/impl/pipeline_inl.h new file mode 100644 index 000000000..99840497d --- /dev/null +++ b/library/include/wavemap/pipeline/impl/pipeline_inl.h @@ -0,0 +1,30 @@ +#ifndef WAVEMAP_PIPELINE_IMPL_PIPELINE_INL_H_ +#define WAVEMAP_PIPELINE_IMPL_PIPELINE_INL_H_ + +#include +#include + +namespace wavemap { +template +bool Pipeline::runIntegrators(const std::vector& integrator_names, + const MeasurementT& measurement) { + for (const auto& integrator_name : integrator_names) { + if (auto* integrator = getIntegrator(integrator_name); integrator) { + integrator->integrate(measurement); + } else { + return false; + } + } + return true; +} + +template +bool Pipeline::runPipeline(const std::vector& integrator_names, + const MeasurementT& measurement) { + const bool success = runIntegrators(integrator_names, measurement); + runOperations(); + return success; +} +} // namespace wavemap + +#endif // WAVEMAP_PIPELINE_IMPL_PIPELINE_INL_H_ diff --git a/library/include/wavemap/pipeline/map_operations/map_operation_base.h b/library/include/wavemap/pipeline/map_operations/map_operation_base.h new file mode 100644 index 000000000..cec8ffb24 --- /dev/null +++ b/library/include/wavemap/pipeline/map_operations/map_operation_base.h @@ -0,0 +1,31 @@ +#ifndef WAVEMAP_PIPELINE_MAP_OPERATIONS_MAP_OPERATION_BASE_H_ +#define WAVEMAP_PIPELINE_MAP_OPERATIONS_MAP_OPERATION_BASE_H_ + +#include + +#include "wavemap/core/config/type_selector.h" + +namespace wavemap { +struct MapOperationType : public TypeSelector { + using TypeSelector::TypeSelector; + + enum Id : TypeId { kThresholdMap, kPruneMap }; + + static constexpr std::array names = {"threshold_map", "prune_map"}; +}; + +class MapOperationBase { + public: + explicit MapOperationBase(MapBase::Ptr occupancy_map) + : occupancy_map_(std::move(occupancy_map)) {} + + virtual ~MapOperationBase() = default; + + virtual void run(bool force_run) = 0; + + protected: + const MapBase::Ptr occupancy_map_; +}; +} // namespace wavemap + +#endif // WAVEMAP_PIPELINE_MAP_OPERATIONS_MAP_OPERATION_BASE_H_ diff --git a/library/include/wavemap/pipeline/map_operations/map_operation_factory.h b/library/include/wavemap/pipeline/map_operations/map_operation_factory.h new file mode 100644 index 000000000..82e837c96 --- /dev/null +++ b/library/include/wavemap/pipeline/map_operations/map_operation_factory.h @@ -0,0 +1,23 @@ +#ifndef WAVEMAP_PIPELINE_MAP_OPERATIONS_MAP_OPERATION_FACTORY_H_ +#define WAVEMAP_PIPELINE_MAP_OPERATIONS_MAP_OPERATION_FACTORY_H_ + +#include +#include + +#include "wavemap/core/map/map_base.h" +#include "wavemap/core/utils/thread_pool.h" +#include "wavemap/pipeline/map_operations/map_operation_base.h" + +namespace wavemap { +class MapOperationFactory { + public: + static std::unique_ptr create(const param::Value& params, + MapBase::Ptr occupancy_map); + + static std::unique_ptr create( + MapOperationType operation_type, const param::Value& params, + MapBase::Ptr occupancy_map); +}; +} // namespace wavemap + +#endif // WAVEMAP_PIPELINE_MAP_OPERATIONS_MAP_OPERATION_FACTORY_H_ diff --git a/library/include/wavemap/pipeline/map_operations/prune_map_operation.h b/library/include/wavemap/pipeline/map_operations/prune_map_operation.h new file mode 100644 index 000000000..f045f12ac --- /dev/null +++ b/library/include/wavemap/pipeline/map_operations/prune_map_operation.h @@ -0,0 +1,41 @@ +#ifndef WAVEMAP_PIPELINE_MAP_OPERATIONS_PRUNE_MAP_OPERATION_H_ +#define WAVEMAP_PIPELINE_MAP_OPERATIONS_PRUNE_MAP_OPERATION_H_ + +#include + +#include "wavemap/core/config/config_base.h" +#include "wavemap/core/map/map_base.h" +#include "wavemap/core/utils/time/time.h" +#include "wavemap/pipeline/map_operations/map_operation_base.h" + +namespace wavemap { +/** + * Config struct for map pruning operations. + */ +struct PruneMapOperationConfig : public ConfigBase { + //! Time period controlling how often the map is pruned. + Seconds once_every = 10.f; + + static MemberMap memberMap; + + bool isValid(bool verbose) const override; +}; + +class PruneMapOperation : public MapOperationBase { + public: + PruneMapOperation(const PruneMapOperationConfig& config, + MapBase::Ptr occupancy_map) + : MapOperationBase(std::move(occupancy_map)), + config_(config.checkValid()) {} + + bool shouldRun(const Timestamp& current_time); + + void run(bool force_run) override; + + private: + const PruneMapOperationConfig config_; + Timestamp last_run_timestamp_; +}; +} // namespace wavemap + +#endif // WAVEMAP_PIPELINE_MAP_OPERATIONS_PRUNE_MAP_OPERATION_H_ diff --git a/library/include/wavemap/pipeline/map_operations/threshold_map_operation.h b/library/include/wavemap/pipeline/map_operations/threshold_map_operation.h new file mode 100644 index 000000000..ba85a4063 --- /dev/null +++ b/library/include/wavemap/pipeline/map_operations/threshold_map_operation.h @@ -0,0 +1,42 @@ +#ifndef WAVEMAP_PIPELINE_MAP_OPERATIONS_THRESHOLD_MAP_OPERATION_H_ +#define WAVEMAP_PIPELINE_MAP_OPERATIONS_THRESHOLD_MAP_OPERATION_H_ + +#include + +#include "wavemap/core/config/config_base.h" +#include "wavemap/core/map/map_base.h" +#include "wavemap/core/utils/time/time.h" +#include "wavemap/pipeline/map_operations/map_operation_base.h" + +namespace wavemap { +/** + * Config struct for map thresholding operations. + */ +struct ThresholdMapOperationConfig + : public ConfigBase { + //! Time period controlling how often the map is thresholded. + Seconds once_every = 2.f; + + static MemberMap memberMap; + + bool isValid(bool verbose) const override; +}; + +class ThresholdMapOperation : public MapOperationBase { + public: + ThresholdMapOperation(const ThresholdMapOperationConfig& config, + MapBase::Ptr occupancy_map) + : MapOperationBase(std::move(occupancy_map)), + config_(config.checkValid()) {} + + bool shouldRun(const Timestamp& current_time = Time::now()); + + void run(bool force_run) override; + + private: + const ThresholdMapOperationConfig config_; + Timestamp last_run_timestamp_; +}; +} // namespace wavemap + +#endif // WAVEMAP_PIPELINE_MAP_OPERATIONS_THRESHOLD_MAP_OPERATION_H_ diff --git a/library/include/wavemap/pipeline/pipeline.h b/library/include/wavemap/pipeline/pipeline.h new file mode 100644 index 000000000..f1588e49a --- /dev/null +++ b/library/include/wavemap/pipeline/pipeline.h @@ -0,0 +1,72 @@ +#ifndef WAVEMAP_PIPELINE_PIPELINE_H_ +#define WAVEMAP_PIPELINE_PIPELINE_H_ + +#include +#include +#include +#include +#include + +#include "wavemap/core/integrator/integrator_base.h" +#include "wavemap/core/map/map_base.h" +#include "wavemap/core/utils/thread_pool.h" +#include "wavemap/pipeline/map_operations/map_operation_base.h" +#include "wavemap/pipeline/map_operations/map_operation_factory.h" + +namespace wavemap { +class Pipeline { + public: + using IntegratorMap = + std::unordered_map>; + using OperationsArray = std::vector>; + + explicit Pipeline(MapBase::Ptr occupancy_map, + std::shared_ptr thread_pool = nullptr) + : occupancy_map_(std::move(occupancy_map)), + thread_pool_(thread_pool ? std::move(thread_pool) + : std::make_shared()) {} + + void clear(); + + bool hasIntegrator(const std::string& integrator_name) const; + bool eraseIntegrator(const std::string& integrator_name); + IntegratorBase* addIntegrator(const std::string& integrator_name, + const param::Value& integrator_params); + IntegratorBase* addIntegrator(const std::string& integrator_name, + std::unique_ptr integrator); + IntegratorBase* getIntegrator(const std::string& integrator_name); + const IntegratorMap& getIntegrators() { return integrators_; } + void clearIntegrators() { integrators_.clear(); } + + MapOperationBase* addOperation(const param::Value& operation_params); + MapOperationBase* addOperation(std::unique_ptr operation); + const OperationsArray& getOperations() { return operations_; } + void clearOperations() { operations_.clear(); } + + template + bool runIntegrators(const std::vector& integrator_names, + const MeasurementT& measurement); + void runOperations(bool force_run_all = false); + + template + bool runPipeline(const std::vector& integrator_names, + const MeasurementT& measurement); + + private: + // Map data structure + const MapBase::Ptr occupancy_map_; + + // Threadpool shared among all input handlers and operations + const std::shared_ptr thread_pool_; + + // Measurement integrators that update the map + IntegratorMap integrators_; + + // Operations to perform after map updates + OperationsArray operations_; +}; +} // namespace wavemap + +#include "wavemap/pipeline/impl/pipeline_inl.h" + +#endif // WAVEMAP_PIPELINE_PIPELINE_H_ diff --git a/library/src/CMakeLists.txt b/library/src/CMakeLists.txt index 2c5444ad0..9500dccab 100644 --- a/library/src/CMakeLists.txt +++ b/library/src/CMakeLists.txt @@ -10,3 +10,4 @@ find_package(tracy QUIET) add_subdirectory(core) add_subdirectory(io) +add_subdirectory(pipeline) diff --git a/library/src/core/integrator/integrator_factory.cc b/library/src/core/integrator/integrator_factory.cc index 7f7e11540..4e371b35b 100644 --- a/library/src/core/integrator/integrator_factory.cc +++ b/library/src/core/integrator/integrator_factory.cc @@ -11,7 +11,7 @@ #include "wavemap/core/integrator/ray_tracing/ray_tracing_integrator.h" namespace wavemap { -IntegratorBase::Ptr IntegratorFactory::create( +std::unique_ptr IntegratorFactory::create( const param::Value& params, MapBase::Ptr occupancy_map, std::shared_ptr thread_pool, std::optional default_integrator_type) { @@ -32,7 +32,7 @@ IntegratorBase::Ptr IntegratorFactory::create( return nullptr; } -IntegratorBase::Ptr IntegratorFactory::create( +std::unique_ptr IntegratorFactory::create( IntegratorType integrator_type, const param::Value& params, MapBase::Ptr occupancy_map, std::shared_ptr thread_pool) { // If we're using a ray tracing based integrator, we can build it directly @@ -40,7 +40,7 @@ IntegratorBase::Ptr IntegratorFactory::create( if (const auto config = RayTracingIntegratorConfig::from(params, "integration_method"); config) { - return std::make_shared(config.value(), + return std::make_unique(config.value(), std::move(occupancy_map)); } else { LOG(ERROR) << "Ray tracing integrator config could not be loaded."; @@ -83,7 +83,7 @@ IntegratorBase::Ptr IntegratorFactory::create( // Assemble the projective integrator switch (integrator_type) { case IntegratorType::kFixedResolutionIntegrator: { - return std::make_shared( + return std::make_unique( integrator_config.value(), projection_model, posed_range_image, beam_offset_image, measurement_model, std::move(occupancy_map)); } @@ -91,7 +91,7 @@ IntegratorBase::Ptr IntegratorFactory::create( auto octree_map = std::dynamic_pointer_cast(occupancy_map); if (octree_map) { - return std::make_shared( + return std::make_unique( integrator_config.value(), projection_model, posed_range_image, beam_offset_image, measurement_model, std::move(octree_map)); } else { @@ -106,7 +106,7 @@ IntegratorBase::Ptr IntegratorFactory::create( auto wavelet_map = std::dynamic_pointer_cast(occupancy_map); if (wavelet_map) { - return std::make_shared( + return std::make_unique( integrator_config.value(), projection_model, posed_range_image, beam_offset_image, measurement_model, std::move(wavelet_map)); } else { @@ -121,7 +121,7 @@ IntegratorBase::Ptr IntegratorFactory::create( auto hashed_wavelet_map = std::dynamic_pointer_cast(occupancy_map); if (hashed_wavelet_map) { - return std::make_shared( + return std::make_unique( integrator_config.value(), projection_model, posed_range_image, beam_offset_image, measurement_model, std::move(hashed_wavelet_map), std::move(thread_pool)); @@ -137,7 +137,7 @@ IntegratorBase::Ptr IntegratorFactory::create( auto hashed_chunked_wavelet_map = std::dynamic_pointer_cast(occupancy_map); if (hashed_chunked_wavelet_map) { - return std::make_shared( + return std::make_unique( integrator_config.value(), projection_model, posed_range_image, beam_offset_image, measurement_model, std::move(hashed_chunked_wavelet_map), std::move(thread_pool)); diff --git a/library/src/core/integrator/projective/projective_integrator.cc b/library/src/core/integrator/projective/projective_integrator.cc index 3f6178ef8..5c222ab12 100644 --- a/library/src/core/integrator/projective/projective_integrator.cc +++ b/library/src/core/integrator/projective/projective_integrator.cc @@ -21,8 +21,7 @@ bool ProjectiveIntegratorConfig::isValid(bool verbose) const { return is_valid; } -void ProjectiveIntegrator::integratePointcloud( - const PosedPointcloud>& pointcloud) { +void ProjectiveIntegrator::integrate(const PosedPointcloud<>& pointcloud) { ProfilerZoneScoped; if (!isPointcloudValid(pointcloud)) { return; @@ -31,8 +30,7 @@ void ProjectiveIntegrator::integratePointcloud( updateMap(); } -void ProjectiveIntegrator::integrateRangeImage( - const PosedImage<>& range_image) { +void ProjectiveIntegrator::integrate(const PosedImage<>& range_image) { ProfilerZoneScoped; importRangeImage(range_image); updateMap(); diff --git a/library/src/core/integrator/ray_tracing/ray_tracing_integrator.cc b/library/src/core/integrator/ray_tracing/ray_tracing_integrator.cc index 40afa0e23..981a38967 100644 --- a/library/src/core/integrator/ray_tracing/ray_tracing_integrator.cc +++ b/library/src/core/integrator/ray_tracing/ray_tracing_integrator.cc @@ -15,8 +15,7 @@ bool RayTracingIntegratorConfig::isValid(bool verbose) const { return is_valid; } -void RayTracingIntegrator::integratePointcloud( - const PosedPointcloud<>& pointcloud) { +void RayTracingIntegrator::integrate(const PosedPointcloud<>& pointcloud) { if (!isPointcloudValid(pointcloud)) { return; } diff --git a/library/src/pipeline/CMakeLists.txt b/library/src/pipeline/CMakeLists.txt new file mode 100644 index 000000000..504e8652b --- /dev/null +++ b/library/src/pipeline/CMakeLists.txt @@ -0,0 +1,12 @@ +add_library(wavemap_pipeline) +add_library(wavemap::wavemap_pipeline ALIAS wavemap_pipeline) + +add_wavemap_include_directories(wavemap_pipeline) +target_sources(wavemap_pipeline PRIVATE + map_operations/map_operation_factory.cc + map_operations/prune_map_operation.cc + map_operations/threshold_map_operation.cc + pipeline.cc) + +set_wavemap_target_properties(wavemap_pipeline) +target_link_libraries(wavemap_pipeline PUBLIC Eigen3::Eigen glog) diff --git a/library/src/pipeline/map_operations/map_operation_factory.cc b/library/src/pipeline/map_operations/map_operation_factory.cc new file mode 100644 index 000000000..b5659e4fe --- /dev/null +++ b/library/src/pipeline/map_operations/map_operation_factory.cc @@ -0,0 +1,50 @@ +#include "wavemap/pipeline/map_operations/map_operation_factory.h" + +#include "wavemap/pipeline/map_operations/prune_map_operation.h" +#include "wavemap/pipeline/map_operations/threshold_map_operation.h" + +namespace wavemap { +std::unique_ptr MapOperationFactory::create( + const param::Value& params, MapBase::Ptr occupancy_map) { + if (const auto type = MapOperationType::from(params); type) { + return create(type.value(), params, std::move(occupancy_map)); + } + + LOG(ERROR) << "Could not create map operation. Returning nullptr."; + return nullptr; +} + +std::unique_ptr MapOperationFactory::create( + MapOperationType operation_type, const param::Value& params, + MapBase::Ptr occupancy_map) { + if (!operation_type.isValid()) { + LOG(ERROR) << "Received request to create map operation with invalid type."; + return nullptr; + } + + // Create the operation handler + switch (operation_type) { + case MapOperationType::kThresholdMap: + if (const auto config = ThresholdMapOperationConfig::from(params); + config) { + return std::make_unique( + config.value(), std::move(occupancy_map)); + } else { + LOG(ERROR) << "Threshold map operation config could not be loaded."; + return nullptr; + } + case MapOperationType::kPruneMap: + if (const auto config = PruneMapOperationConfig::from(params); config) { + return std::make_unique(config.value(), + std::move(occupancy_map)); + } else { + LOG(ERROR) << "Prune map operation config could not be loaded."; + return nullptr; + } + } + + LOG(ERROR) << "Factory does not (yet) support creation of map operation type " + << operation_type.toStr() << "."; + return nullptr; +} +} // namespace wavemap diff --git a/library/src/pipeline/map_operations/prune_map_operation.cc b/library/src/pipeline/map_operations/prune_map_operation.cc new file mode 100644 index 000000000..892aba54e --- /dev/null +++ b/library/src/pipeline/map_operations/prune_map_operation.cc @@ -0,0 +1,23 @@ +#include "wavemap/pipeline/map_operations/prune_map_operation.h" + +namespace wavemap { +DECLARE_CONFIG_MEMBERS(PruneMapOperationConfig, + (once_every)); + +bool PruneMapOperationConfig::isValid(bool verbose) const { + return IS_PARAM_GT(once_every, 0.f, verbose); +} + +bool PruneMapOperation::shouldRun(const Timestamp& current_time) { + return config_.once_every < + time::to_seconds(current_time - last_run_timestamp_); +} + +void PruneMapOperation::run(bool force_run) { + const Timestamp current_time = Time::now(); + if (force_run || shouldRun(current_time)) { + occupancy_map_->pruneSmart(); + last_run_timestamp_ = current_time; + } +} +} // namespace wavemap diff --git a/library/src/pipeline/map_operations/threshold_map_operation.cc b/library/src/pipeline/map_operations/threshold_map_operation.cc new file mode 100644 index 000000000..fe3105303 --- /dev/null +++ b/library/src/pipeline/map_operations/threshold_map_operation.cc @@ -0,0 +1,23 @@ +#include "wavemap/pipeline/map_operations/threshold_map_operation.h" + +namespace wavemap { +DECLARE_CONFIG_MEMBERS(ThresholdMapOperationConfig, + (once_every)); + +bool ThresholdMapOperationConfig::isValid(bool verbose) const { + return IS_PARAM_GT(once_every, 0.f, verbose); +} + +bool ThresholdMapOperation::shouldRun(const Timestamp& current_time) { + return config_.once_every < + time::to_seconds(current_time - last_run_timestamp_); +} + +void ThresholdMapOperation::run(bool force_run) { + const Timestamp current_time = Time::now(); + if (force_run || shouldRun(current_time)) { + occupancy_map_->threshold(); + last_run_timestamp_ = current_time; + } +} +} // namespace wavemap diff --git a/library/src/pipeline/pipeline.cc b/library/src/pipeline/pipeline.cc new file mode 100644 index 000000000..1c554aeb4 --- /dev/null +++ b/library/src/pipeline/pipeline.cc @@ -0,0 +1,69 @@ +#include "wavemap/pipeline/pipeline.h" + +#include "wavemap/core/integrator/integrator_factory.h" + +namespace wavemap { +void Pipeline::clear() { + clearIntegrators(); + clearOperations(); +} + +bool Pipeline::hasIntegrator(const std::string& integrator_name) const { + return integrators_.count(integrator_name); +} + +bool Pipeline::eraseIntegrator(const std::string& integrator_name) { + return integrators_.erase(integrator_name); +} + +IntegratorBase* Pipeline::getIntegrator(const std::string& integrator_name) { + if (auto it = integrators_.find(integrator_name); it != integrators_.end()) { + return it->second.get(); + } + return nullptr; +} + +IntegratorBase* Pipeline::addIntegrator(const std::string& integrator_name, + const param::Value& integrator_params) { + auto integrator = IntegratorFactory::create(integrator_params, occupancy_map_, + thread_pool_); + return addIntegrator(integrator_name, std::move(integrator)); +} + +IntegratorBase* Pipeline::addIntegrator( + const std::string& integrator_name, + std::unique_ptr integrator) { + if (integrator) { + auto [it, success] = + integrators_.try_emplace(integrator_name, std::move(integrator)); + if (success) { + return it->second.get(); + } else { + LOG(WARNING) << "Could not add integrator. Another integrator with the " + "same name already exists."; + } + } + + LOG(WARNING) << "Ignoring request to add integrator. " + "Integrator is null pointer."; + return nullptr; +} + +MapOperationBase* Pipeline::addOperation(const param::Value& operation_params) { + auto operation_handler = + MapOperationFactory::create(operation_params, occupancy_map_); + return addOperation(std::move(operation_handler)); +} + +MapOperationBase* Pipeline::addOperation( + std::unique_ptr operation) { + return operation ? operations_.emplace_back(std::move(operation)).get() + : nullptr; +} + +void Pipeline::runOperations(bool force_run_all) { + for (auto& operation : operations_) { + operation->run(force_run_all); + } +} +} // namespace wavemap diff --git a/library/test/src/core/integrator/projection_model/test_image_projectors.cc b/library/test/src/core/integrator/projection_model/test_image_projectors.cc index 0f3ae257a..09a9caeaf 100644 --- a/library/test/src/core/integrator/projection_model/test_image_projectors.cc +++ b/library/test/src/core/integrator/projection_model/test_image_projectors.cc @@ -366,10 +366,10 @@ TYPED_TEST(Image2DProjectorTypedTest, SensorCoordinateAABBs) { std::cerr << "For\n-W_aabb: " << test.W_aabb.toString() << "\n-T_W_C:\n" << test.T_W_C << "\nWith C_cell_corners:\n" << C_t_C_corners << "\nsensor X-coordinates:\n" - << print::container(corners_x) << "\nsensor Y-coordinates:\n" - << print::container(corners_y) << print::container(corners_x) + << print::sequence(corners_x) << "\nsensor Y-coordinates:\n" + << print::sequence(corners_y) << print::sequence(corners_x) << "\nsensor Z-coordinates:\n" - << print::container(corners_z) + << print::sequence(corners_z) << "\nand reference min/max sensor coordinates: " << print::eigen::oneLine(reference_aabb.min) << ", " << print::eigen::oneLine(reference_aabb.max) diff --git a/library/test/src/core/integrator/test_pointcloud_integrators.cc b/library/test/src/core/integrator/test_pointcloud_integrators.cc index 0447a25cb..20dbd1fba 100644 --- a/library/test/src/core/integrator/test_pointcloud_integrators.cc +++ b/library/test/src/core/integrator/test_pointcloud_integrators.cc @@ -110,8 +110,8 @@ TYPED_TEST(PointcloudIntegratorTypedTest, for (int cloud_idx = 0; cloud_idx < kNumPointclouds; ++cloud_idx) { const PosedPointcloud<> random_pointcloud = TestFixture::getRandomPointcloud(*projection_model); - reference_integrator->integratePointcloud(random_pointcloud); - evaluated_integrator->integratePointcloud(random_pointcloud); + reference_integrator->integrate(random_pointcloud); + evaluated_integrator->integrate(random_pointcloud); evaluated_occupancy_map->prune(); } @@ -187,7 +187,7 @@ TEST_F(PointcloudIntegratorTest, RayTracingIntegrator) { IntegratorBase::Ptr pointcloud_integrator = std::make_shared(ray_tracing_integrator_config, occupancy_map); - pointcloud_integrator->integratePointcloud(random_pointcloud); + pointcloud_integrator->integrate(random_pointcloud); // Check the map // NOTE: This test may generate false positives if the pointcloud contains diff --git a/library/test/src/core/map/test_haar_cell.cc b/library/test/src/core/map/test_haar_cell.cc index 4a2e4b54a..694d9b929 100644 --- a/library/test/src/core/map/test_haar_cell.cc +++ b/library/test/src/core/map/test_haar_cell.cc @@ -133,9 +133,9 @@ TYPED_TEST(HaarCellTest, ParallelAndLiftedBackwardTransformEquivalence) { } EXPECT_TRUE(results_are_equal) << "The parallel implementation's child scales are\n[" - << print::container(parallel_child_scale_coefficients) + << print::sequence(parallel_child_scale_coefficients) << "] and the lifted child scales are\n[" - << print::container(lifted_child_scale_coefficients) << "]"; + << print::sequence(lifted_child_scale_coefficients) << "]"; } } @@ -189,9 +189,9 @@ TYPED_TEST(HaarCellTest, LosslessReconstruction) { } EXPECT_TRUE(round_trip_equals_original) << "The full original array was\n[" - << print::container(child_scale_coefficients) + << print::sequence(child_scale_coefficients) << "] and after the roundtrip\n[" - << print::container(roundtrip_child_scale_coefficients) << "]"; + << print::sequence(roundtrip_child_scale_coefficients) << "]"; } } @@ -229,9 +229,9 @@ TYPED_TEST(HaarCellTest, SingleChildForwardTransforms) { } EXPECT_TRUE(single_child_transform_equals_regular) << "The single child transform returned parent coefficients\n[" - << print::container(parent_from_single_child) + << print::sequence(parent_from_single_child) << "] while the regular transform returns\n[" - << print::container(parent_from_sparse_array) << "]"; + << print::sequence(parent_from_sparse_array) << "]"; } } } From d256d1d1bb3b4d5155caf0ac2f648e9f28f8d45c Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 17 Jul 2024 13:14:51 +0200 Subject: [PATCH 099/159] Update config files and schemas --- .../config/wavemap_livox_mid360.yaml | 66 +++--- .../wavemap_livox_mid360_azure_kinect.yaml | 163 +++++++------- .../wavemap_livox_mid360_pico_flexx.yaml | 201 +++++++++--------- .../wavemap_livox_mid360_pico_monstar.yaml | 163 +++++++------- .../config/wavemap_ouster_os0.yaml | 72 ++++--- .../wavemap_ouster_os0_pico_monstar.yaml | 169 ++++++++------- .../config/wavemap_ouster_os1.yaml | 64 +++--- .../config/wavemap_panoptic_mapping_rgbd.yaml | 50 ++--- .../wavemap/inputs/depth_image_input.json | 111 +++++----- .../inputs/{input.json => input_base.json} | 0 .../wavemap/inputs/pointcloud_input.json | 130 ++++++----- tooling/schemas/wavemap/map.json | 169 --------------- .../schemas/wavemap/map/hashed_blocks.json | 23 ++ .../map/hashed_chunked_wavelet_octree.json | 31 +++ .../wavemap/map/hashed_wavelet_octree.json | 31 +++ tooling/schemas/wavemap/map/map_base.json | 38 ++++ tooling/schemas/wavemap/map/octree.json | 27 +++ .../schemas/wavemap/map/wavelet_octree.json | 27 +++ .../crop_map_operation.json | 0 .../map_operation_base.json} | 0 .../prune_map_operation.json | 0 .../publish_map_operation.json | 0 .../publish_pointcloud_operation.json | 0 .../threshold_map_operation.json | 0 .../integration_method.json | 0 .../measurement_integrator_base.json} | 0 .../measurement_model.json | 0 .../projection_model.json | 0 tooling/schemas/wavemap/wavemap_config.json | 35 +-- 29 files changed, 806 insertions(+), 764 deletions(-) rename tooling/schemas/wavemap/inputs/{input.json => input_base.json} (100%) delete mode 100644 tooling/schemas/wavemap/map.json create mode 100644 tooling/schemas/wavemap/map/hashed_blocks.json create mode 100644 tooling/schemas/wavemap/map/hashed_chunked_wavelet_octree.json create mode 100644 tooling/schemas/wavemap/map/hashed_wavelet_octree.json create mode 100644 tooling/schemas/wavemap/map/map_base.json create mode 100644 tooling/schemas/wavemap/map/octree.json create mode 100644 tooling/schemas/wavemap/map/wavelet_octree.json rename tooling/schemas/wavemap/{operations => map_operations_pipeline}/crop_map_operation.json (100%) rename tooling/schemas/wavemap/{operations/operations_pipeline.json => map_operations_pipeline/map_operation_base.json} (100%) rename tooling/schemas/wavemap/{operations => map_operations_pipeline}/prune_map_operation.json (100%) rename tooling/schemas/wavemap/{operations => map_operations_pipeline}/publish_map_operation.json (100%) rename tooling/schemas/wavemap/{operations => map_operations_pipeline}/publish_pointcloud_operation.json (100%) rename tooling/schemas/wavemap/{operations => map_operations_pipeline}/threshold_map_operation.json (100%) rename tooling/schemas/wavemap/{inputs/measurement_integrator => measurement_integrators}/integration_method.json (100%) rename tooling/schemas/wavemap/{inputs/measurement_integrator/measurement_integrator.json => measurement_integrators/measurement_integrator_base.json} (100%) rename tooling/schemas/wavemap/{inputs/measurement_integrator => measurement_integrators}/measurement_model.json (100%) rename tooling/schemas/wavemap/{inputs/measurement_integrator => measurement_integrators}/projection_model.json (100%) diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml index 823432fa0..047f998da 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml @@ -7,7 +7,7 @@ map: type: hashed_chunked_wavelet_octree min_cell_width: { meters: 0.1 } -operations_pipeline: +map_operations_pipeline: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map @@ -15,36 +15,38 @@ operations_pipeline: - type: publish_map once_every: { seconds: 2.0 } +measurement_integrators: + livox_mid360: + projection_model: + type: spherical_projector + elevation: + num_cells: 32 + min_angle: { degrees: -7.0 } + max_angle: { degrees: 52.0 } + azimuth: + num_cells: 512 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.05 } + # NOTE: For best results, we recommend setting range_sigma to + # max(min_cell_width / 2, ouster_noise) where ouster_noise = 0.05 + range_sigma: { meters: 0.08 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 0.5 } + max_range: { meters: 12.0 } + termination_update_error: 0.1 + inputs: - type: pointcloud - general: - topic_name: "/livox/lidar" - topic_type: livox - undistort_motion: true - reprojected_pointcloud_topic_name: "/wavemap/input" - topic_queue_length: 10 - max_wait_for_pose: { seconds: 1.0 } - integrators: - - integration_method: - type: hashed_chunked_wavelet_integrator - min_range: { meters: 0.5 } - max_range: { meters: 12.0 } - termination_update_error: 0.1 - projection_model: - type: spherical_projector - elevation: - num_cells: 32 - min_angle: { degrees: -7.0 } - max_angle: { degrees: 52.0 } - azimuth: - num_cells: 512 - min_angle: { degrees: -180.0 } - max_angle: { degrees: 180.0 } - measurement_model: - type: continuous_beam - angle_sigma: { degrees: 0.05 } - # NOTE: For best results, we recommend setting range_sigma to - # max(min_cell_width / 2, ouster_noise) where ouster_noise = 0.05 - range_sigma: { meters: 0.08 } - scaling_free: 0.2 - scaling_occupied: 0.4 + topic_name: "/livox/lidar" + topic_type: livox + measurement_integrator: livox_mid360 + undistort_motion: true + reprojected_pointcloud_topic_name: "/wavemap/input" + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml index 05b109a4e..27b721a9a 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml @@ -7,7 +7,7 @@ map: type: hashed_chunked_wavelet_octree min_cell_width: { meters: 0.01 } -operations_pipeline: +map_operations_pipeline: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map @@ -15,85 +15,90 @@ operations_pipeline: - type: publish_map once_every: { seconds: 2.0 } +measurement_integrators: + livox_mid360: + projection_model: + type: spherical_projector + elevation: + num_cells: 32 + min_angle: { degrees: -7.0 } + max_angle: { degrees: 52.0 } + azimuth: + num_cells: 512 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.05 } + # NOTE: For best results, we recommend setting range_sigma to + # max(min_cell_width / 2, ouster_noise) where ouster_noise = 0.05 + range_sigma: { meters: 0.08 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 1.5 } + max_range: { meters: 10.0 } + termination_update_error: 0.1 + termination_height: 3 + + kinect_short_range: + projection_model: + type: pinhole_camera_projector + width: 640 + height: 576 + fx: 504.7727966308594 + fy: 504.8245544433594 + cx: 324.58349609375 + cy: 333.283447265625 + measurement_model: + type: continuous_ray + range_sigma: { meters: 0.005 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 0.05 } + max_range: { meters: 1.0 } + termination_update_error: 0.1 + termination_height: 0 + + kinect_medium_range: + projection_model: + type: pinhole_camera_projector + width: 640 + height: 576 + fx: 504.7727966308594 + fy: 504.8245544433594 + cx: 324.58349609375 + cy: 333.283447265625 + measurement_model: + type: continuous_ray + range_sigma: { meters: 0.01 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 1.0 } + max_range: { meters: 2.0 } + termination_update_error: 0.1 + termination_height: 1 + inputs: - type: depth_image - general: - topic_name: "/depth/image_rect" - # reprojected_topic_name: "/wavemap/kinect_input" - depth_scale_factor: 0.001 - topic_queue_length: 1 - max_wait_for_pose: { seconds: 1.0 } - time_offset: { seconds: -0.025 } - integrators: - - integration_method: - type: hashed_chunked_wavelet_integrator - min_range: { meters: 0.05 } - max_range: { meters: 1.0 } - termination_update_error: 0.1 - termination_height: 0 - projection_model: - type: pinhole_camera_projector - width: 640 - height: 576 - fx: 504.7727966308594 - fy: 504.8245544433594 - cx: 324.58349609375 - cy: 333.283447265625 - measurement_model: - type: continuous_ray - range_sigma: { meters: 0.005 } - scaling_free: 0.2 - scaling_occupied: 0.4 - - integration_method: - type: hashed_chunked_wavelet_integrator - min_range: { meters: 1.0 } - max_range: { meters: 2.0 } - termination_update_error: 0.1 - termination_height: 1 - projection_model: - type: pinhole_camera_projector - width: 640 - height: 576 - fx: 504.7727966308594 - fy: 504.8245544433594 - cx: 324.58349609375 - cy: 333.283447265625 - measurement_model: - type: continuous_ray - range_sigma: { meters: 0.01 } - scaling_free: 0.2 - scaling_occupied: 0.4 + topic_name: "/depth/image_rect" + measurement_integrator: [ kinect_short_range, kinect_medium_range ] + depth_scale_factor: 0.001 + topic_queue_length: 1 + max_wait_for_pose: { seconds: 1.0 } + time_offset: { seconds: -0.025 } + # reprojected_topic_name: "/wavemap/kinect_input" - type: pointcloud - general: - topic_name: "/livox/lidar" - topic_type: livox - undistort_motion: true - # reprojected_topic_name: "/wavemap/livox_input" - topic_queue_length: 1 - max_wait_for_pose: { seconds: 1.0 } - integrators: - - integration_method: - type: hashed_chunked_wavelet_integrator - min_range: { meters: 1.5 } - max_range: { meters: 10.0 } - termination_update_error: 0.1 - termination_height: 3 - projection_model: - type: spherical_projector - elevation: - num_cells: 32 - min_angle: { degrees: -7.0 } - max_angle: { degrees: 52.0 } - azimuth: - num_cells: 512 - min_angle: { degrees: -180.0 } - max_angle: { degrees: 180.0 } - measurement_model: - type: continuous_beam - angle_sigma: { degrees: 0.05 } - # NOTE: For best results, we recommend setting range_sigma to - # max(min_cell_width / 2, ouster_noise) where ouster_noise = 0.05 - range_sigma: { meters: 0.08 } - scaling_free: 0.2 - scaling_occupied: 0.4 + topic_name: "/livox/lidar" + topic_type: livox + measurement_integrator: livox_mid360 + undistort_motion: true + topic_queue_length: 1 + max_wait_for_pose: { seconds: 1.0 } + # reprojected_topic_name: "/wavemap/livox_input" diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml index 0b5a02b98..ed92f6947 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml @@ -8,7 +8,7 @@ map: min_cell_width: { meters: 0.01 } tree_height: 9 -operations_pipeline: +map_operations_pipeline: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map @@ -16,103 +16,110 @@ operations_pipeline: - type: publish_map once_every: { seconds: 2.0 } +measurement_integrators: + livox_mid360: + projection_model: + type: spherical_projector + elevation: + num_cells: 64 + min_angle: { degrees: -7.0 } + max_angle: { degrees: 52.0 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.035 } + # NOTE: For best results, we recommend setting range_sigma to + # max(min_cell_width / 2, ouster_noise) where ouster_noise = 0.05 + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 1.0 } + max_range: { meters: 8.0 } + termination_update_error: 0.1 + termination_height: 3 + + monstar_short_range: + projection_model: + type: pinhole_camera_projector + width: 224 + height: 172 + fx: 204.2710723876953 + fy: 204.2710723876953 + cx: 109.85344696044922 + cy: 86.22309112548828 + measurement_model: + type: continuous_ray + range_sigma: { meters: 0.005 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 0.05 } + max_range: { meters: 1.0 } + termination_update_error: 0.1 + termination_height: 0 + + monstar_medium_range: + projection_model: + type: pinhole_camera_projector + width: 224 + height: 172 + fx: 204.2710723876953 + fy: 204.2710723876953 + cx: 109.85344696044922 + cy: 86.22309112548828 + measurement_model: + type: continuous_ray + range_sigma: { meters: 0.01 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 1.0 } + max_range: { meters: 2.0 } + termination_update_error: 0.1 + termination_height: 1 + + monstar_long_range: + projection_model: + type: pinhole_camera_projector + width: 224 + height: 172 + fx: 204.2710723876953 + fy: 204.2710723876953 + cx: 109.85344696044922 + cy: 86.22309112548828 + measurement_model: + type: continuous_ray + range_sigma: { meters: 0.02 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 2.0 } + max_range: { meters: 3.0 } + termination_update_error: 0.1 + termination_height: 2 + inputs: - type: depth_image - general: - topic_name: "/pico_flexx/image_rect" - # reprojected_topic_name: "/wavemap/depth_cam_input" - # depth_scale_factor: 0.001 - topic_queue_length: 1 - max_wait_for_pose: { seconds: 1.0 } - time_offset: { seconds: -0.00727598 } - integrators: - - integration_method: - type: hashed_chunked_wavelet_integrator - min_range: { meters: 0.05 } - max_range: { meters: 1.0 } - termination_update_error: 0.1 - termination_height: 0 - projection_model: - type: pinhole_camera_projector - width: 224 - height: 172 - fx: 204.2710723876953 - fy: 204.2710723876953 - cx: 109.85344696044922 - cy: 86.22309112548828 - measurement_model: - type: continuous_ray - range_sigma: { meters: 0.005 } - scaling_free: 0.2 - scaling_occupied: 0.4 - - integration_method: - type: hashed_chunked_wavelet_integrator - min_range: { meters: 1.0 } - max_range: { meters: 2.0 } - termination_update_error: 0.1 - termination_height: 1 - projection_model: - type: pinhole_camera_projector - width: 224 - height: 172 - fx: 204.2710723876953 - fy: 204.2710723876953 - cx: 109.85344696044922 - cy: 86.22309112548828 - measurement_model: - type: continuous_ray - range_sigma: { meters: 0.01 } - scaling_free: 0.2 - scaling_occupied: 0.4 - - integration_method: - type: hashed_chunked_wavelet_integrator - min_range: { meters: 2.0 } - max_range: { meters: 3.0 } - termination_update_error: 0.1 - termination_height: 2 - projection_model: - type: pinhole_camera_projector - width: 224 - height: 172 - fx: 204.2710723876953 - fy: 204.2710723876953 - cx: 109.85344696044922 - cy: 86.22309112548828 - measurement_model: - type: continuous_ray - range_sigma: { meters: 0.02 } - scaling_free: 0.2 - scaling_occupied: 0.4 + topic_name: "/pico_flexx/image_rect" + measurement_integrator: [ monstar_short_range, monstar_medium_range, monstar_long_range ] + topic_queue_length: 1 + max_wait_for_pose: { seconds: 1.0 } + time_offset: { seconds: -0.00727598 } + # reprojected_topic_name: "/wavemap/depth_cam_input" + # depth_scale_factor: 0.001 - type: pointcloud - general: - topic_name: "/livox/lidar" - topic_type: livox - # reprojected_topic_name: "/wavemap/lidar_input" - topic_queue_length: 1 - max_wait_for_pose: { seconds: 1.0 } - integrators: - - integration_method: - type: hashed_chunked_wavelet_integrator - min_range: { meters: 1.0 } - max_range: { meters: 8.0 } - termination_update_error: 0.1 - termination_height: 3 - projection_model: - type: spherical_projector - elevation: - num_cells: 64 - min_angle: { degrees: -7.0 } - max_angle: { degrees: 52.0 } - azimuth: - num_cells: 1024 - min_angle: { degrees: -180.0 } - max_angle: { degrees: 180.0 } - measurement_model: - type: continuous_beam - angle_sigma: { degrees: 0.035 } - # NOTE: For best results, we recommend setting range_sigma to - # max(min_cell_width / 2, ouster_noise) where ouster_noise = 0.05 - range_sigma: { meters: 0.05 } - scaling_free: 0.2 - scaling_occupied: 0.4 + topic_name: "/livox/lidar" + topic_type: livox + measurement_integrator: livox_mid360 + topic_queue_length: 1 + max_wait_for_pose: { seconds: 1.0 } + # reprojected_topic_name: "/wavemap/lidar_input" diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml index 0652712af..ceffda703 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml @@ -8,7 +8,7 @@ map: min_cell_width: { meters: 0.01 } tree_height: 9 -operations_pipeline: +map_operations_pipeline: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map @@ -16,85 +16,90 @@ operations_pipeline: - type: publish_map once_every: { seconds: 2.0 } +measurement_integrators: + monstar_short_range: + projection_model: + type: pinhole_camera_projector + width: 352 + height: 287 + fx: 208.751708984375 + fy: 208.751708984375 + cx: 172.19403076171875 + cy: 146.1179962158203 + measurement_model: + type: continuous_ray + range_sigma: { meters: 0.005 } + scaling_free: 0.4 + scaling_occupied: 0.8 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 0.3 } + max_range: { meters: 1.0 } + termination_update_error: 0.1 + termination_height: 0 + + monstar_medium_range: + projection_model: + type: pinhole_camera_projector + width: 352 + height: 287 + fx: 208.751708984375 + fy: 208.751708984375 + cx: 172.19403076171875 + cy: 146.1179962158203 + measurement_model: + type: continuous_ray + range_sigma: { meters: 0.01 } + scaling_free: 0.4 + scaling_occupied: 0.8 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 1.0 } + max_range: { meters: 2.0 } + termination_update_error: 0.1 + termination_height: 1 + + livox_mid360: + projection_model: + type: spherical_projector + elevation: + num_cells: 32 + min_angle: { degrees: -7.0 } + max_angle: { degrees: 52.0 } + azimuth: + num_cells: 512 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.035 } + # NOTE: For best results, we recommend setting range_sigma to + # max(min_cell_width / 2, ouster_noise) where ouster_noise = 0.05 + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 2.0 } + max_range: { meters: 8.0 } + termination_update_error: 0.1 + termination_height: 3 + inputs: - type: depth_image - general: - topic_name: "/pico_monstar/image_rect" - topic_queue_length: 1 - max_wait_for_pose: { seconds: 1.0 } - time_offset: { seconds: 0.00201747 } - # depth_scale_factor: 0.001 - # reprojected_pointcloud_topic_name: "/wavemap/depth_cam_input" - integrators: - - integration_method: - type: hashed_chunked_wavelet_integrator - min_range: { meters: 0.3 } - max_range: { meters: 1.0 } - termination_update_error: 0.1 - termination_height: 0 - projection_model: - type: pinhole_camera_projector - width: 352 - height: 287 - fx: 208.751708984375 - fy: 208.751708984375 - cx: 172.19403076171875 - cy: 146.1179962158203 - measurement_model: - type: continuous_ray - range_sigma: { meters: 0.005 } - scaling_free: 0.4 - scaling_occupied: 0.8 - - integration_method: - type: hashed_chunked_wavelet_integrator - min_range: { meters: 1.0 } - max_range: { meters: 2.0 } - termination_update_error: 0.1 - termination_height: 1 - projection_model: - type: pinhole_camera_projector - width: 352 - height: 287 - fx: 208.751708984375 - fy: 208.751708984375 - cx: 172.19403076171875 - cy: 146.1179962158203 - measurement_model: - type: continuous_ray - range_sigma: { meters: 0.01 } - scaling_free: 0.4 - scaling_occupied: 0.8 + topic_name: "/pico_monstar/image_rect" + measurement_integrator: [monstar_short_range, monstar_medium_range] + topic_queue_length: 1 + max_wait_for_pose: { seconds: 1.0 } + time_offset: { seconds: 0.00201747 } + # depth_scale_factor: 0.001 + # reprojected_pointcloud_topic_name: "/wavemap/depth_cam_input" - type: pointcloud - general: - topic_name: "/livox/lidar" - topic_type: livox - topic_queue_length: 1 - max_wait_for_pose: { seconds: 1.0 } - # reprojected_pointcloud_topic_name: "/wavemap/lidar_input" - # projected_range_image_topic_name: "/wavemap/lidar_range_image" - integrators: - - integration_method: - type: hashed_chunked_wavelet_integrator - min_range: { meters: 2.0 } - max_range: { meters: 8.0 } - termination_update_error: 0.1 - termination_height: 3 - projection_model: - type: spherical_projector - elevation: - num_cells: 32 - min_angle: { degrees: -7.0 } - max_angle: { degrees: 52.0 } - azimuth: - num_cells: 512 - min_angle: { degrees: -180.0 } - max_angle: { degrees: 180.0 } - measurement_model: - type: continuous_beam - angle_sigma: { degrees: 0.035 } - # NOTE: For best results, we recommend setting range_sigma to - # max(min_cell_width / 2, ouster_noise) where ouster_noise = 0.05 - range_sigma: { meters: 0.05 } - scaling_free: 0.2 - scaling_occupied: 0.4 + topic_name: "/livox/lidar" + topic_type: livox + measurement_integrator: livox_mid360 + topic_queue_length: 1 + max_wait_for_pose: { seconds: 1.0 } + # reprojected_pointcloud_topic_name: "/wavemap/lidar_input" + # projected_range_image_topic_name: "/wavemap/lidar_range_image" diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml index becc40df4..8ec3a9834 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml @@ -7,7 +7,7 @@ map: type: hashed_chunked_wavelet_octree min_cell_width: { meters: 0.1 } -operations_pipeline: +map_operations_pipeline: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map @@ -15,39 +15,41 @@ operations_pipeline: - type: publish_map once_every: { seconds: 2.0 } +measurement_integrators: + ouster_os0: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 27.67 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 36.18 } + elevation: + num_cells: 128 + min_angle: { degrees: -45.73 } + max_angle: { degrees: 46.27 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.035 } + # NOTE: For best results, we recommend setting range_sigma to + # max(min_cell_width / 2, ouster_noise) where ouster_noise = 0.05 + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 1.0 } + max_range: { meters: 15.0 } + termination_update_error: 0.1 + inputs: - type: pointcloud - general: - topic_name: "/os_cloud_node/points" - topic_type: ouster - undistort_motion: true - topic_queue_length: 10 - max_wait_for_pose: { seconds: 1.0 } - # reprojected_pointcloud_topic_name: "/wavemap/reprojected_pointcloud" - # projected_range_image_topic_name: "/wavemap/projected_range_image" - integrators: - - integration_method: - type: hashed_chunked_wavelet_integrator - min_range: { meters: 1.0 } - max_range: { meters: 15.0 } - termination_update_error: 0.1 - projection_model: - type: ouster_projector - lidar_origin_to_beam_origin: { millimeters: 27.67 } - lidar_origin_to_sensor_origin_z_offset: { millimeters: 36.18 } - elevation: - num_cells: 128 - min_angle: { degrees: -45.73 } - max_angle: { degrees: 46.27 } - azimuth: - num_cells: 1024 - min_angle: { degrees: -180.0 } - max_angle: { degrees: 180.0 } - measurement_model: - type: continuous_beam - angle_sigma: { degrees: 0.035 } - # NOTE: For best results, we recommend setting range_sigma to - # max(min_cell_width / 2, ouster_noise) where ouster_noise = 0.05 - range_sigma: { meters: 0.05 } - scaling_free: 0.2 - scaling_occupied: 0.4 + topic_name: "/os_cloud_node/points" + topic_type: ouster + measurement_integrator: ouster_os0 + undistort_motion: true + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } + # reprojected_pointcloud_topic_name: "/wavemap/reprojected_pointcloud" + # projected_range_image_topic_name: "/wavemap/projected_range_image" diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml index 20bfdd7d8..84e993a50 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml @@ -8,7 +8,7 @@ map: min_cell_width: { meters: 0.01 } tree_height: 9 -operations_pipeline: +map_operations_pipeline: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map @@ -16,88 +16,93 @@ operations_pipeline: - type: publish_map once_every: { seconds: 2.0 } +measurement_integrators: + ouster_os0: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 27.67 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 36.18 } + elevation: + num_cells: 128 + min_angle: { degrees: -45.73 } + max_angle: { degrees: 46.27 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.035 } + # NOTE: For best results, we recommend setting range_sigma to + # max(min_cell_width / 2, ouster_noise) where ouster_noise = 0.05 + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 1.5 } + max_range: { meters: 15.0 } + termination_update_error: 0.1 + termination_height: 3 + + monstar_short_range: + projection_model: + type: pinhole_camera_projector + width: 352 + height: 287 + fx: 208.751708984375 + fy: 208.751708984375 + cx: 172.19403076171875 + cy: 146.1179962158203 + measurement_model: + type: continuous_ray + range_sigma: { meters: 0.005 } + scaling_free: 0.4 + scaling_occupied: 0.8 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 0.3 } + max_range: { meters: 1.0 } + termination_update_error: 0.1 + termination_height: 0 + + monstar_medium_range: + projection_model: + type: pinhole_camera_projector + width: 352 + height: 287 + fx: 208.751708984375 + fy: 208.751708984375 + cx: 172.19403076171875 + cy: 146.1179962158203 + measurement_model: + type: continuous_ray + range_sigma: { meters: 0.01 } + scaling_free: 0.4 + scaling_occupied: 0.8 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 1.0 } + max_range: { meters: 2.0 } + termination_update_error: 0.1 + termination_height: 1 + inputs: - type: depth_image - general: - topic_name: "/pico_monstar/image_rect" - topic_queue_length: 1 - max_wait_for_pose: { seconds: 1.0 } - time_offset: { seconds: 0.0118462 } - # depth_scale_factor: 0.001 - # reprojected_pointcloud_topic_name: "/wavemap/reprojected_pointcloud" - integrators: - - integration_method: - type: hashed_chunked_wavelet_integrator - min_range: { meters: 0.3 } - max_range: { meters: 1.0 } - termination_update_error: 0.1 - termination_height: 0 - projection_model: - type: pinhole_camera_projector - width: 352 - height: 287 - fx: 208.751708984375 - fy: 208.751708984375 - cx: 172.19403076171875 - cy: 146.1179962158203 - measurement_model: - type: continuous_ray - range_sigma: { meters: 0.005 } - scaling_free: 0.4 - scaling_occupied: 0.8 - - integration_method: - type: hashed_chunked_wavelet_integrator - min_range: { meters: 1.0 } - max_range: { meters: 2.0 } - termination_update_error: 0.1 - termination_height: 1 - projection_model: - type: pinhole_camera_projector - width: 352 - height: 287 - fx: 208.751708984375 - fy: 208.751708984375 - cx: 172.19403076171875 - cy: 146.1179962158203 - measurement_model: - type: continuous_ray - range_sigma: { meters: 0.01 } - scaling_free: 0.4 - scaling_occupied: 0.8 + topic_name: "/pico_monstar/image_rect" + measurement_integrator: [ monstar_short_range, monstar_medium_range ] + topic_queue_length: 1 + max_wait_for_pose: { seconds: 1.0 } + time_offset: { seconds: 0.0118462 } + # depth_scale_factor: 0.001 + # reprojected_pointcloud_topic_name: "/wavemap/reprojected_pointcloud" - type: pointcloud - general: - topic_name: "/os_cloud_node/points" - topic_type: ouster - undistort_motion: true - topic_queue_length: 10 - max_wait_for_pose: { seconds: 1.0 } - # reprojected_pointcloud_topic_name: "/wavemap/reprojected_pointcloud" - # projected_range_image_topic_name: "/wavemap/projected_range_image" - integrators: - - integration_method: - type: hashed_chunked_wavelet_integrator - min_range: { meters: 1.5 } - max_range: { meters: 15.0 } - termination_update_error: 0.1 - termination_height: 3 - projection_model: - type: ouster_projector - lidar_origin_to_beam_origin: { millimeters: 27.67 } - lidar_origin_to_sensor_origin_z_offset: { millimeters: 36.18 } - elevation: - num_cells: 128 - min_angle: { degrees: -45.73 } - max_angle: { degrees: 46.27 } - azimuth: - num_cells: 1024 - min_angle: { degrees: -180.0 } - max_angle: { degrees: 180.0 } - measurement_model: - type: continuous_beam - angle_sigma: { degrees: 0.035 } - # NOTE: For best results, we recommend setting range_sigma to - # max(min_cell_width / 2, ouster_noise) where ouster_noise = 0.05 - range_sigma: { meters: 0.05 } - scaling_free: 0.2 - scaling_occupied: 0.4 + topic_name: "/os_cloud_node/points" + topic_type: ouster + measurement_integrator: ouster_os0 + undistort_motion: true + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } + # reprojected_pointcloud_topic_name: "/wavemap/reprojected_pointcloud" + # projected_range_image_topic_name: "/wavemap/projected_range_image" diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml index a1a788059..3fdd617f9 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml @@ -7,7 +7,7 @@ map: type: wavelet_octree min_cell_width: { meters: 0.2 } -operations_pipeline: +map_operations_pipeline: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map @@ -15,35 +15,37 @@ operations_pipeline: - type: publish_map once_every: { seconds: 10.0 } +measurement_integrators: + ouster_os1: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 27.67 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 36.18 } + elevation: + num_cells: 64 + min_angle: { degrees: -15.594 } + max_angle: { degrees: 17.743 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.05 } + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: wavelet_integrator + min_range: { meters: 1.0 } + max_range: { meters: 20.0 } + termination_update_error: 0.1 + inputs: - type: pointcloud - general: - topic_name: "/os1_cloud_node/points" - topic_type: ouster - undistort_motion: true - topic_queue_length: 10 - max_wait_for_pose: { seconds: 1.0 } - integrators: - - integration_method: - type: wavelet_integrator - min_range: { meters: 1.0 } - max_range: { meters: 20.0 } - termination_update_error: 0.1 - projection_model: - type: ouster_projector - lidar_origin_to_beam_origin: { millimeters: 27.67 } - lidar_origin_to_sensor_origin_z_offset: { millimeters: 36.18 } - elevation: - num_cells: 64 - min_angle: { degrees: -15.594 } - max_angle: { degrees: 17.743 } - azimuth: - num_cells: 1024 - min_angle: { degrees: -180.0 } - max_angle: { degrees: 180.0 } - measurement_model: - type: continuous_beam - angle_sigma: { degrees: 0.05 } - range_sigma: { meters: 0.05 } - scaling_free: 0.2 - scaling_occupied: 0.4 + topic_name: "/os1_cloud_node/points" + topic_type: ouster + measurement_integrator: ouster_os1 + undistort_motion: true + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml index a206873d7..2eb77282e 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml @@ -7,7 +7,7 @@ map: type: hashed_chunked_wavelet_octree min_cell_width: { meters: 0.02 } -operations_pipeline: +map_operations_pipeline: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map @@ -15,28 +15,30 @@ operations_pipeline: - type: publish_map once_every: { seconds: 2.0 } +measurement_integrators: + panoptic_mapping_camera: + projection_model: + type: pinhole_camera_projector + width: 640 + height: 480 + fx: 320.0 + fy: 320.0 + cx: 320.0 + cy: 240.0 + measurement_model: + type: continuous_ray + range_sigma: { meters: 0.01 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 0.1 } + max_range: { meters: 5.0 } + termination_update_error: 0.1 + inputs: - type: depth_image - general: - topic_name: "/data/depth_image" - topic_queue_length: 10 - max_wait_for_pose: { seconds: 1.0 } - integrators: - - integration_method: - type: hashed_chunked_wavelet_integrator - min_range: { meters: 0.1 } - max_range: { meters: 5.0 } - termination_update_error: 0.1 - projection_model: - type: pinhole_camera_projector - width: 640 - height: 480 - fx: 320.0 - fy: 320.0 - cx: 320.0 - cy: 240.0 - measurement_model: - type: continuous_ray - range_sigma: { meters: 0.01 } - scaling_free: 0.2 - scaling_occupied: 0.4 + topic_name: "/data/depth_image" + measurement_integrator: panoptic_mapping_camera + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } diff --git a/tooling/schemas/wavemap/inputs/depth_image_input.json b/tooling/schemas/wavemap/inputs/depth_image_input.json index e4c005aac..94f81bb26 100644 --- a/tooling/schemas/wavemap/inputs/depth_image_input.json +++ b/tooling/schemas/wavemap/inputs/depth_image_input.json @@ -4,72 +4,69 @@ "type": "object", "additionalProperties": false, "required": [ - "general", - "integrators" + "topic_name", + "measurement_integrator" ], "properties": { "type": { "const": "depth_image" }, - "general": { - "description": "General properties of the measurement input.", - "type": "object", - "additionalProperties": false, - "required": [ - "topic_name" - ], - "properties": { - "topic_name": { - "description": "Name of the ROS topic to subscribe to.", - "type": "string" - }, - "topic_queue_length": { - "description": "Queue length to use when subscribing to the ROS topic.", - "type": "integer", - "exclusiveMinimum": 0 - }, - "processing_retry_period": { - "description": "Time period used to control the rate at which to retry getting the sensor pose when ROS TF lookups fail.", - "$ref": "../value_with_unit/convertible_to_seconds.json", - "exclusiveMinimum": 0 - }, - "max_wait_for_pose": { - "description": "Maximum amount of time to wait for the sensor pose to become available when ROS TF lookups fail.", - "$ref": "../value_with_unit/convertible_to_seconds.json", - "minimum": 0 - }, - "sensor_frame_id": { - "description": "The frame_id to use to look up the sensor pose using ROS TFs. Note that setting this is optional, by default the header.frame_id of the measurement's msg is used.", - "type": "string" - }, - "image_transport_hints": { - "description": "Custom image_transport::TransportHints to use when subscribing to the depth image topic. Defaults to 'raw'. For more info, see http://wiki.ros.org/image_transport.", - "type": "string" - }, - "depth_scale_factor": { - "description": "Scale factor used to convert the depth image's values to meters.", - "type": "number" - }, - "time_offset": { - "description": "Time offset to apply to the header.stamp of the measurement's msg when looking up its pose using ROS TFs. Can be used when the time offset is known (e.g. through calibration) but not corrected by the sensor's driver.", - "$ref": "../value_with_unit/convertible_to_seconds.json" - }, - "reprojected_pointcloud_topic_name": { - "description": "Name of the topic on which to republish the depth images as pointclouds. Useful to share the pointclouds with other ROS nodes and for debugging.", + "topic_name": { + "description": "Name of the ROS topic to subscribe to.", + "type": "string" + }, + "measurement_integrator": { + "description": "Name of the measurement integrator(s) used to integrate the measurements into the map.", + "oneOf": [ + { "type": "string" }, - "projected_range_image_topic_name": { - "description": "Name of the topic on which to republish the range image computed from the pointclouds. Useful for debugging.", - "type": "string" + { + "type": "array", + "items": { + "type": "string" + } } - } + ] + }, + "topic_queue_length": { + "description": "Queue length to use when subscribing to the ROS topic.", + "type": "integer", + "exclusiveMinimum": 0 + }, + "processing_retry_period": { + "description": "Time period used to control the rate at which to retry getting the sensor pose when ROS TF lookups fail.", + "$ref": "../value_with_unit/convertible_to_seconds.json", + "exclusiveMinimum": 0 + }, + "max_wait_for_pose": { + "description": "Maximum amount of time to wait for the sensor pose to become available when ROS TF lookups fail.", + "$ref": "../value_with_unit/convertible_to_seconds.json", + "minimum": 0 + }, + "sensor_frame_id": { + "description": "The frame_id to use to look up the sensor pose using ROS TFs. Note that setting this is optional, by default the header.frame_id of the measurement's msg is used.", + "type": "string" + }, + "image_transport_hints": { + "description": "Custom image_transport::TransportHints to use when subscribing to the depth image topic. Defaults to 'raw'. For more info, see http://wiki.ros.org/image_transport.", + "type": "string" + }, + "depth_scale_factor": { + "description": "Scale factor used to convert the depth image's values to meters.", + "type": "number" + }, + "time_offset": { + "description": "Time offset to apply to the header.stamp of the measurement's msg when looking up its pose using ROS TFs. Can be used when the time offset is known (e.g. through calibration) but not corrected by the sensor's driver.", + "$ref": "../value_with_unit/convertible_to_seconds.json" + }, + "reprojected_pointcloud_topic_name": { + "description": "Name of the topic on which to republish the depth images as pointclouds. Useful to share the pointclouds with other ROS nodes and for debugging.", + "type": "string" }, - "integrators": { - "description": "List of integrators to call whenever a new measurement arrives.", - "type": "array", - "items": { - "$ref": "measurement_integrator/measurement_integrator.json" - } + "projected_range_image_topic_name": { + "description": "Name of the topic on which to republish the range image computed from the pointclouds. Useful for debugging.", + "type": "string" } } } diff --git a/tooling/schemas/wavemap/inputs/input.json b/tooling/schemas/wavemap/inputs/input_base.json similarity index 100% rename from tooling/schemas/wavemap/inputs/input.json rename to tooling/schemas/wavemap/inputs/input_base.json diff --git a/tooling/schemas/wavemap/inputs/pointcloud_input.json b/tooling/schemas/wavemap/inputs/pointcloud_input.json index d5ea6a402..6c40ce5d7 100644 --- a/tooling/schemas/wavemap/inputs/pointcloud_input.json +++ b/tooling/schemas/wavemap/inputs/pointcloud_input.json @@ -4,81 +4,79 @@ "type": "object", "additionalProperties": false, "required": [ - "general", - "integrators" + "topic_name", + "topic_type", + "measurement_integrator" ], "properties": { "type": { "const": "pointcloud" }, - "general": { - "type": "object", - "additionalProperties": false, - "required": [ - "topic_name", - "topic_type" - ], - "properties": { - "topic_name": { - "description": "Name of the ROS topic to subscribe to.", - "type": "string" - }, - "topic_type": { - "description": "Message type of the ROS topic to subscribe to.", - "type": "string", - "enum": [ - "PointCloud2", - "ouster", - "livox" - ] - }, - "topic_queue_length": { - "description": "Queue length to use when subscribing to the ROS topic.", - "type": "integer", - "exclusiveMinimum": 0 - }, - "processing_retry_period": { - "description": "Time period used to control the rate at which to retry getting the sensor pose when ROS TF lookups fail.", - "$ref": "../value_with_unit/convertible_to_seconds.json", - "exclusiveMinimum": 0 - }, - "max_wait_for_pose": { - "description": "Maximum amount of time to wait for the sensor pose to become available when ROS TF lookups fail.", - "$ref": "../value_with_unit/convertible_to_seconds.json", - "minimum": 0 - }, - "sensor_frame_id": { - "description": "The frame_id to use to look up the sensor pose using ROS TFs. Note that setting this is optional, by default the header.frame_id of the measurement's msg is used.", - "type": "string" - }, - "time_offset": { - "description": "Time offset to apply to the header.stamp of the measurement's msg when looking up its pose using ROS TFs. Can be used when the time offset is known (e.g. through calibration) but not corrected by the sensor's driver.", - "$ref": "../value_with_unit/convertible_to_seconds.json" - }, - "undistort_motion": { - "description": "Whether to undistort each pointcloud based on the sensor's motion while it was captured. We strongly recommend turning this on, unless the robot's odometry is very jerky or the sensor's driver already performs motion undistortion.", - "type": "boolean" - }, - "num_undistortion_interpolation_intervals_per_cloud": { - "description": "Number of intermediate poses to sample from ROS TFs when performing motion undistortion.", - "type": "integer" - }, - "reprojected_pointcloud_topic_name": { - "description": "Name of the topic on which to republish the motion-undistorted pointclouds. Useful to share the undistorted pointclouds with other ROS nodes and for debugging.", + "topic_name": { + "description": "Name of the ROS topic to subscribe to.", + "type": "string" + }, + "topic_type": { + "description": "Message type of the ROS topic to subscribe to.", + "type": "string", + "enum": [ + "PointCloud2", + "ouster", + "livox" + ] + }, + "measurement_integrator": { + "description": "Name of the measurement integrator(s) used to integrate the measurements into the map.", + "oneOf": [ + { "type": "string" }, - "projected_range_image_topic_name": { - "description": "Name of the topic on which to republish the range image computed from the pointclouds. Useful for debugging, to see how well the projection model matches the LiDAR.", - "type": "string" + { + "type": "array", + "items": { + "type": "string" + } } - } + ] + }, + "topic_queue_length": { + "description": "Queue length to use when subscribing to the ROS topic.", + "type": "integer", + "exclusiveMinimum": 0 + }, + "processing_retry_period": { + "description": "Time period used to control the rate at which to retry getting the sensor pose when ROS TF lookups fail.", + "$ref": "../value_with_unit/convertible_to_seconds.json", + "exclusiveMinimum": 0 + }, + "max_wait_for_pose": { + "description": "Maximum amount of time to wait for the sensor pose to become available when ROS TF lookups fail.", + "$ref": "../value_with_unit/convertible_to_seconds.json", + "minimum": 0 + }, + "sensor_frame_id": { + "description": "The frame_id to use to look up the sensor pose using ROS TFs. Note that setting this is optional, by default the header.frame_id of the measurement's msg is used.", + "type": "string" + }, + "time_offset": { + "description": "Time offset to apply to the header.stamp of the measurement's msg when looking up its pose using ROS TFs. Can be used when the time offset is known (e.g. through calibration) but not corrected by the sensor's driver.", + "$ref": "../value_with_unit/convertible_to_seconds.json" + }, + "undistort_motion": { + "description": "Whether to undistort each pointcloud based on the sensor's motion while it was captured. We strongly recommend turning this on, unless the robot's odometry is very jerky or the sensor's driver already performs motion undistortion.", + "type": "boolean" + }, + "num_undistortion_interpolation_intervals_per_cloud": { + "description": "Number of intermediate poses to sample from ROS TFs when performing motion undistortion.", + "type": "integer" + }, + "reprojected_pointcloud_topic_name": { + "description": "Name of the topic on which to republish the motion-undistorted pointclouds. Useful to share the undistorted pointclouds with other ROS nodes and for debugging.", + "type": "string" }, - "integrators": { - "description": "List of integrators to call whenever a new measurement arrives.", - "type": "array", - "items": { - "$ref": "measurement_integrator/measurement_integrator.json" - } + "projected_range_image_topic_name": { + "description": "Name of the topic on which to republish the range image computed from the pointclouds. Useful for debugging, to see how well the projection model matches the LiDAR.", + "type": "string" } } } diff --git a/tooling/schemas/wavemap/map.json b/tooling/schemas/wavemap/map.json deleted file mode 100644 index e0f44345e..000000000 --- a/tooling/schemas/wavemap/map.json +++ /dev/null @@ -1,169 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "description": "Properties of the data structure used to store the map.", - "type": "object", - "$defs": { - "hashed_blocks": { - "type": "object", - "additionalProperties": false, - "properties": { - "type": { - "const": "hashed_blocks" - }, - "min_cell_width": { - "description": "Maximum resolution of the map, set as the width of the smallest cell that it can represent.", - "$ref": "value_with_unit/convertible_to_meters.json" - }, - "min_log_odds": { - "description": "Lower threshold for the occupancy values stored in the map, in log-odds.", - "type": "number" - }, - "max_log_odds": { - "description": "Upper threshold for the occupancy values stored in the map, in log-odds.", - "type": "number" - } - } - }, - "octree": { - "type": "object", - "additionalProperties": false, - "properties": { - "type": { - "const": "octree" - }, - "min_cell_width": { - "description": "Maximum resolution of the map, set as the width of the smallest cell that it can represent.", - "$ref": "value_with_unit/convertible_to_meters.json" - }, - "min_log_odds": { - "description": "Lower threshold for the occupancy values stored in the map, in log-odds.", - "type": "number" - }, - "max_log_odds": { - "description": "Upper threshold for the occupancy values stored in the map, in log-odds.", - "type": "number" - }, - "tree_height": { - "description": "Height of the octree used to store the map.", - "type": "integer" - } - } - }, - "wavelet_octree": { - "type": "object", - "additionalProperties": false, - "properties": { - "type": { - "const": "wavelet_octree" - }, - "min_cell_width": { - "description": "Maximum resolution of the map, set as the width of the smallest cell that it can represent.", - "$ref": "value_with_unit/convertible_to_meters.json" - }, - "min_log_odds": { - "description": "Lower threshold for the occupancy values stored in the map, in log-odds.", - "type": "number" - }, - "max_log_odds": { - "description": "Upper threshold for the occupancy values stored in the map, in log-odds.", - "type": "number" - }, - "tree_height": { - "description": "Height of the octree used to store the map.", - "type": "integer" - } - } - }, - "hashed_wavelet_octree": { - "type": "object", - "additionalProperties": false, - "properties": { - "type": { - "const": "hashed_wavelet_octree" - }, - "min_cell_width": { - "description": "Maximum resolution of the map, set as the width of the smallest cell that it can represent.", - "$ref": "value_with_unit/convertible_to_meters.json" - }, - "min_log_odds": { - "description": "Lower threshold for the occupancy values stored in the map, in log-odds.", - "type": "number" - }, - "max_log_odds": { - "description": "Upper threshold for the occupancy values stored in the map, in log-odds.", - "type": "number" - }, - "tree_height": { - "description": "Height of the octree in each hashed block.", - "type": "integer" - }, - "only_prune_blocks_if_unused_for": { - "description": "Only prune blocks if they have not been updated for at least this amount of time. Useful to avoid pruning blocks that are still being updated, whose nodes would most likely directly be reallocated if pruned.", - "$ref": "value_with_unit/convertible_to_seconds.json" - } - } - }, - "hashed_chunked_wavelet_octree": { - "type": "object", - "additionalProperties": false, - "properties": { - "type": { - "const": "hashed_chunked_wavelet_octree" - }, - "min_cell_width": { - "description": "Maximum resolution of the map, set as the width of the smallest cell that it can represent.", - "$ref": "value_with_unit/convertible_to_meters.json" - }, - "min_log_odds": { - "description": "Lower threshold for the occupancy values stored in the map, in log-odds.", - "type": "number" - }, - "max_log_odds": { - "description": "Upper threshold for the occupancy values stored in the map, in log-odds.", - "type": "number" - }, - "tree_height": { - "description": "Height of the octree in each hashed block.", - "type": "integer" - }, - "only_prune_blocks_if_unused_for": { - "description": "Only prune blocks if they have not been updated for at least this amount of time. Useful to avoid pruning blocks that are still being updated, whose nodes would most likely directly be reallocated if pruned.", - "$ref": "value_with_unit/convertible_to_seconds.json" - } - } - } - }, - "required": [ - "type" - ], - "properties": { - "type": { - "description": "Type of the data structure that should be used.", - "type": "string", - "enum": [ - "hashed_blocks", - "octree", - "wavelet_octree", - "hashed_wavelet_octree", - "hashed_chunked_wavelet_octree" - ] - } - }, - "oneOf": [ - { - "$ref": "#/$defs/hashed_blocks" - }, - { - "$ref": "#/$defs/octree" - }, - { - "$ref": "#/$defs/wavelet_octree" - }, - { - "$ref": "#/$defs/hashed_wavelet_octree" - }, - { - "$ref": "#/$defs/hashed_chunked_wavelet_octree" - } - ] -} diff --git a/tooling/schemas/wavemap/map/hashed_blocks.json b/tooling/schemas/wavemap/map/hashed_blocks.json new file mode 100644 index 000000000..725d3e46a --- /dev/null +++ b/tooling/schemas/wavemap/map/hashed_blocks.json @@ -0,0 +1,23 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "description": "Properties of the hashed blocks map data structure.", + "type": "object", + "additionalProperties": false, + "properties": { + "type": { + "const": "hashed_blocks" + }, + "min_cell_width": { + "description": "Maximum resolution of the map, set as the width of the smallest cell that it can represent.", + "$ref": "../value_with_unit/convertible_to_meters.json" + }, + "min_log_odds": { + "description": "Lower threshold for the occupancy values stored in the map, in log-odds.", + "type": "number" + }, + "max_log_odds": { + "description": "Upper threshold for the occupancy values stored in the map, in log-odds.", + "type": "number" + } + } +} diff --git a/tooling/schemas/wavemap/map/hashed_chunked_wavelet_octree.json b/tooling/schemas/wavemap/map/hashed_chunked_wavelet_octree.json new file mode 100644 index 000000000..86e465020 --- /dev/null +++ b/tooling/schemas/wavemap/map/hashed_chunked_wavelet_octree.json @@ -0,0 +1,31 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "description": "Properties of the hashed chunked wavelet octree map data structure.", + "type": "object", + "additionalProperties": false, + "properties": { + "type": { + "const": "hashed_chunked_wavelet_octree" + }, + "min_cell_width": { + "description": "Maximum resolution of the map, set as the width of the smallest cell that it can represent.", + "$ref": "../value_with_unit/convertible_to_meters.json" + }, + "min_log_odds": { + "description": "Lower threshold for the occupancy values stored in the map, in log-odds.", + "type": "number" + }, + "max_log_odds": { + "description": "Upper threshold for the occupancy values stored in the map, in log-odds.", + "type": "number" + }, + "tree_height": { + "description": "Height of the octree in each hashed block.", + "type": "integer" + }, + "only_prune_blocks_if_unused_for": { + "description": "Only prune blocks if they have not been updated for at least this amount of time. Useful to avoid pruning blocks that are still being updated, whose nodes would most likely directly be reallocated if pruned.", + "$ref": "../value_with_unit/convertible_to_seconds.json" + } + } +} diff --git a/tooling/schemas/wavemap/map/hashed_wavelet_octree.json b/tooling/schemas/wavemap/map/hashed_wavelet_octree.json new file mode 100644 index 000000000..d94b8ee52 --- /dev/null +++ b/tooling/schemas/wavemap/map/hashed_wavelet_octree.json @@ -0,0 +1,31 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "description": "Properties of the hashed wavelet octree map data structure.", + "type": "object", + "additionalProperties": false, + "properties": { + "type": { + "const": "hashed_wavelet_octree" + }, + "min_cell_width": { + "description": "Maximum resolution of the map, set as the width of the smallest cell that it can represent.", + "$ref": "../value_with_unit/convertible_to_meters.json" + }, + "min_log_odds": { + "description": "Lower threshold for the occupancy values stored in the map, in log-odds.", + "type": "number" + }, + "max_log_odds": { + "description": "Upper threshold for the occupancy values stored in the map, in log-odds.", + "type": "number" + }, + "tree_height": { + "description": "Height of the octree in each hashed block.", + "type": "integer" + }, + "only_prune_blocks_if_unused_for": { + "description": "Only prune blocks if they have not been updated for at least this amount of time. Useful to avoid pruning blocks that are still being updated, whose nodes would most likely directly be reallocated if pruned.", + "$ref": "../value_with_unit/convertible_to_seconds.json" + } + } +} diff --git a/tooling/schemas/wavemap/map/map_base.json b/tooling/schemas/wavemap/map/map_base.json new file mode 100644 index 000000000..4895a3baa --- /dev/null +++ b/tooling/schemas/wavemap/map/map_base.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "description": "Properties of the data structure used to store the map.", + "type": "object", + "required": [ + "type" + ], + "properties": { + "type": { + "description": "Type of the data structure that should be used.", + "type": "string", + "enum": [ + "hashed_blocks", + "octree", + "wavelet_octree", + "hashed_wavelet_octree", + "hashed_chunked_wavelet_octree" + ] + } + }, + "oneOf": [ + { + "$ref": "hashed_blocks.json" + }, + { + "$ref": "octree.json" + }, + { + "$ref": "wavelet_octree.json" + }, + { + "$ref": "hashed_wavelet_octree.json" + }, + { + "$ref": "hashed_chunked_wavelet_octree.json" + } + ] +} diff --git a/tooling/schemas/wavemap/map/octree.json b/tooling/schemas/wavemap/map/octree.json new file mode 100644 index 000000000..69eceff43 --- /dev/null +++ b/tooling/schemas/wavemap/map/octree.json @@ -0,0 +1,27 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "description": "Properties of the octree map data structure.", + "type": "object", + "additionalProperties": false, + "properties": { + "type": { + "const": "octree" + }, + "min_cell_width": { + "description": "Maximum resolution of the map, set as the width of the smallest cell that it can represent.", + "$ref": "../value_with_unit/convertible_to_meters.json" + }, + "min_log_odds": { + "description": "Lower threshold for the occupancy values stored in the map, in log-odds.", + "type": "number" + }, + "max_log_odds": { + "description": "Upper threshold for the occupancy values stored in the map, in log-odds.", + "type": "number" + }, + "tree_height": { + "description": "Height of the octree used to store the map.", + "type": "integer" + } + } +} diff --git a/tooling/schemas/wavemap/map/wavelet_octree.json b/tooling/schemas/wavemap/map/wavelet_octree.json new file mode 100644 index 000000000..85466c234 --- /dev/null +++ b/tooling/schemas/wavemap/map/wavelet_octree.json @@ -0,0 +1,27 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "description": "Properties of the wavelet octree map data structure.", + "type": "object", + "additionalProperties": false, + "properties": { + "type": { + "const": "wavelet_octree" + }, + "min_cell_width": { + "description": "Maximum resolution of the map, set as the width of the smallest cell that it can represent.", + "$ref": "../value_with_unit/convertible_to_meters.json" + }, + "min_log_odds": { + "description": "Lower threshold for the occupancy values stored in the map, in log-odds.", + "type": "number" + }, + "max_log_odds": { + "description": "Upper threshold for the occupancy values stored in the map, in log-odds.", + "type": "number" + }, + "tree_height": { + "description": "Height of the octree used to store the map.", + "type": "integer" + } + } +} diff --git a/tooling/schemas/wavemap/operations/crop_map_operation.json b/tooling/schemas/wavemap/map_operations_pipeline/crop_map_operation.json similarity index 100% rename from tooling/schemas/wavemap/operations/crop_map_operation.json rename to tooling/schemas/wavemap/map_operations_pipeline/crop_map_operation.json diff --git a/tooling/schemas/wavemap/operations/operations_pipeline.json b/tooling/schemas/wavemap/map_operations_pipeline/map_operation_base.json similarity index 100% rename from tooling/schemas/wavemap/operations/operations_pipeline.json rename to tooling/schemas/wavemap/map_operations_pipeline/map_operation_base.json diff --git a/tooling/schemas/wavemap/operations/prune_map_operation.json b/tooling/schemas/wavemap/map_operations_pipeline/prune_map_operation.json similarity index 100% rename from tooling/schemas/wavemap/operations/prune_map_operation.json rename to tooling/schemas/wavemap/map_operations_pipeline/prune_map_operation.json diff --git a/tooling/schemas/wavemap/operations/publish_map_operation.json b/tooling/schemas/wavemap/map_operations_pipeline/publish_map_operation.json similarity index 100% rename from tooling/schemas/wavemap/operations/publish_map_operation.json rename to tooling/schemas/wavemap/map_operations_pipeline/publish_map_operation.json diff --git a/tooling/schemas/wavemap/operations/publish_pointcloud_operation.json b/tooling/schemas/wavemap/map_operations_pipeline/publish_pointcloud_operation.json similarity index 100% rename from tooling/schemas/wavemap/operations/publish_pointcloud_operation.json rename to tooling/schemas/wavemap/map_operations_pipeline/publish_pointcloud_operation.json diff --git a/tooling/schemas/wavemap/operations/threshold_map_operation.json b/tooling/schemas/wavemap/map_operations_pipeline/threshold_map_operation.json similarity index 100% rename from tooling/schemas/wavemap/operations/threshold_map_operation.json rename to tooling/schemas/wavemap/map_operations_pipeline/threshold_map_operation.json diff --git a/tooling/schemas/wavemap/inputs/measurement_integrator/integration_method.json b/tooling/schemas/wavemap/measurement_integrators/integration_method.json similarity index 100% rename from tooling/schemas/wavemap/inputs/measurement_integrator/integration_method.json rename to tooling/schemas/wavemap/measurement_integrators/integration_method.json diff --git a/tooling/schemas/wavemap/inputs/measurement_integrator/measurement_integrator.json b/tooling/schemas/wavemap/measurement_integrators/measurement_integrator_base.json similarity index 100% rename from tooling/schemas/wavemap/inputs/measurement_integrator/measurement_integrator.json rename to tooling/schemas/wavemap/measurement_integrators/measurement_integrator_base.json diff --git a/tooling/schemas/wavemap/inputs/measurement_integrator/measurement_model.json b/tooling/schemas/wavemap/measurement_integrators/measurement_model.json similarity index 100% rename from tooling/schemas/wavemap/inputs/measurement_integrator/measurement_model.json rename to tooling/schemas/wavemap/measurement_integrators/measurement_model.json diff --git a/tooling/schemas/wavemap/inputs/measurement_integrator/projection_model.json b/tooling/schemas/wavemap/measurement_integrators/projection_model.json similarity index 100% rename from tooling/schemas/wavemap/inputs/measurement_integrator/projection_model.json rename to tooling/schemas/wavemap/measurement_integrators/projection_model.json diff --git a/tooling/schemas/wavemap/wavemap_config.json b/tooling/schemas/wavemap/wavemap_config.json index 97055ba0a..d0c88dc1b 100644 --- a/tooling/schemas/wavemap/wavemap_config.json +++ b/tooling/schemas/wavemap/wavemap_config.json @@ -3,31 +3,40 @@ "title": "Wavemap config", "description": "Schema to provide code completion and validation for wavemap configuration files.", "type": "object", + "required": [ + "general", + "map", + "map_operations_pipeline", + "measurement_integrators", + "inputs" + ], "properties": { "general": { "$ref": "general.json" }, "map": { - "$ref": "map.json" + "$ref": "map/map_base.json" }, - "inputs": { - "description": "A list of measurement inputs.", + "map_operations_pipeline": { + "description": "A list of operations to perform after map updates.", "type": "array", "items": { - "$ref": "inputs/input.json" + "$ref": "map_operations_pipeline/map_operation_base.json" } }, - "operations_pipeline": { - "description": "A list of operations to perform after map updates.", + "measurement_integrators": { + "description": "A dictionary of measurement integrators.", + "type": "object", + "additionalProperties": { + "$ref": "measurement_integrators/measurement_integrator_base.json" + } + }, + "inputs": { + "description": "A list of measurement inputs.", "type": "array", "items": { - "$ref": "operations/operations_pipeline.json" + "$ref": "inputs/input_base.json" } } - }, - "required": [ - "general", - "map", - "inputs" - ] + } } From 33f08da8462d9f26751b31096ab7926bb6679fc6 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 17 Jul 2024 14:07:23 +0200 Subject: [PATCH 100/159] Update all factories to return unique_ptrs The motivation is that unique_ptrs can automatically be converted to shared_ptrs, while the opposite is not true. More generally, unique_ptrs are a better default than shared_ptrs in our codebase for efficiency and tidiness reasons. --- .../measurement_model_factory.h | 4 ++-- .../projection_model/projector_factory.h | 8 +++++--- library/include/wavemap/core/map/map_factory.h | 6 ++++-- .../measurement_model_factory.cc | 8 ++++---- .../projection_model/projector_factory.cc | 10 +++++----- library/src/core/map/map_factory.cc | 17 +++++++++-------- 6 files changed, 29 insertions(+), 24 deletions(-) diff --git a/library/include/wavemap/core/integrator/measurement_model/measurement_model_factory.h b/library/include/wavemap/core/integrator/measurement_model/measurement_model_factory.h index bad7137a9..db7bd1f81 100644 --- a/library/include/wavemap/core/integrator/measurement_model/measurement_model_factory.h +++ b/library/include/wavemap/core/integrator/measurement_model/measurement_model_factory.h @@ -8,14 +8,14 @@ namespace wavemap { class MeasurementModelFactory { public: - static MeasurementModelBase::Ptr create( + static std::unique_ptr create( const param::Value& params, ProjectorBase::ConstPtr projection_model, Image<>::ConstPtr range_image, Image::ConstPtr beam_offset_image = nullptr, std::optional default_measurement_model_type = std::nullopt); - static MeasurementModelBase::Ptr create( + static std::unique_ptr create( MeasurementModelType measurement_model_type, ProjectorBase::ConstPtr projection_model, Image<>::ConstPtr range_image, Image::ConstPtr beam_offset_image, const param::Value& params); diff --git a/library/include/wavemap/core/integrator/projection_model/projector_factory.h b/library/include/wavemap/core/integrator/projection_model/projector_factory.h index d62a868dd..21953d1b1 100644 --- a/library/include/wavemap/core/integrator/projection_model/projector_factory.h +++ b/library/include/wavemap/core/integrator/projection_model/projector_factory.h @@ -1,17 +1,19 @@ #ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_PROJECTOR_FACTORY_H_ #define WAVEMAP_CORE_INTEGRATOR_PROJECTION_MODEL_PROJECTOR_FACTORY_H_ +#include + #include "wavemap/core/integrator/projection_model/projector_base.h" namespace wavemap { class ProjectorFactory { public: - static ProjectorBase::Ptr create( + static std::unique_ptr create( const param::Value& params, std::optional default_projector_type = std::nullopt); - static ProjectorBase::Ptr create(ProjectorType projector_type, - const param::Value& params); + static std::unique_ptr create(ProjectorType projector_type, + const param::Value& params); }; } // namespace wavemap diff --git a/library/include/wavemap/core/map/map_factory.h b/library/include/wavemap/core/map/map_factory.h index 3b1ccb7b7..160c3a613 100644 --- a/library/include/wavemap/core/map/map_factory.h +++ b/library/include/wavemap/core/map/map_factory.h @@ -1,6 +1,7 @@ #ifndef WAVEMAP_CORE_MAP_MAP_FACTORY_H_ #define WAVEMAP_CORE_MAP_MAP_FACTORY_H_ +#include #include #include "wavemap/core/map/map_base.h" @@ -8,11 +9,12 @@ namespace wavemap { class MapFactory { public: - static MapBase::Ptr create( + static std::unique_ptr create( const param::Value& params, std::optional default_map_type = std::nullopt); - static MapBase::Ptr create(MapType map_type, const param::Value& params); + static std::unique_ptr create(MapType map_type, + const param::Value& params); }; } // namespace wavemap diff --git a/library/src/core/integrator/measurement_model/measurement_model_factory.cc b/library/src/core/integrator/measurement_model/measurement_model_factory.cc index b0aca32b4..51f7b6ed2 100644 --- a/library/src/core/integrator/measurement_model/measurement_model_factory.cc +++ b/library/src/core/integrator/measurement_model/measurement_model_factory.cc @@ -4,7 +4,7 @@ #include "wavemap/core/integrator/measurement_model/continuous_ray.h" namespace wavemap { -MeasurementModelBase::Ptr wavemap::MeasurementModelFactory::create( +std::unique_ptr wavemap::MeasurementModelFactory::create( const param::Value& params, ProjectorBase::ConstPtr projection_model, Image<>::ConstPtr range_image, Image::ConstPtr beam_offset_image, std::optional default_measurement_model_type) { @@ -27,7 +27,7 @@ MeasurementModelBase::Ptr wavemap::MeasurementModelFactory::create( return nullptr; } -MeasurementModelBase::Ptr wavemap::MeasurementModelFactory::create( +std::unique_ptr wavemap::MeasurementModelFactory::create( MeasurementModelType measurement_model_type, ProjectorBase::ConstPtr projection_model, Image<>::ConstPtr range_image, Image::ConstPtr beam_offset_image, const param::Value& params) { @@ -36,7 +36,7 @@ MeasurementModelBase::Ptr wavemap::MeasurementModelFactory::create( if (const auto config = ContinuousRayConfig::from(params, "measurement_model"); config) { - return std::make_shared(config.value(), + return std::make_unique(config.value(), std::move(projection_model), std::move(range_image)); } else { @@ -49,7 +49,7 @@ MeasurementModelBase::Ptr wavemap::MeasurementModelFactory::create( if (const auto config = ContinuousBeamConfig::from(params, "measurement_model"); config) { - return std::make_shared( + return std::make_unique( config.value(), std::move(projection_model), std::move(range_image), std::move(beam_offset_image)); } else { diff --git a/library/src/core/integrator/projection_model/projector_factory.cc b/library/src/core/integrator/projection_model/projector_factory.cc index 5c3ab5802..54ee6cb76 100644 --- a/library/src/core/integrator/projection_model/projector_factory.cc +++ b/library/src/core/integrator/projection_model/projector_factory.cc @@ -5,7 +5,7 @@ #include "wavemap/core/integrator/projection_model/spherical_projector.h" namespace wavemap { -ProjectorBase::Ptr wavemap::ProjectorFactory::create( +std::unique_ptr wavemap::ProjectorFactory::create( const param::Value& params, std::optional default_projector_type) { if (const auto type = ProjectorType::from(params, "projection_model"); type) { @@ -22,14 +22,14 @@ ProjectorBase::Ptr wavemap::ProjectorFactory::create( return nullptr; } -ProjectorBase::Ptr wavemap::ProjectorFactory::create( +std::unique_ptr wavemap::ProjectorFactory::create( ProjectorType projector_type, const param::Value& params) { switch (projector_type) { case ProjectorType::kSphericalProjector: { if (const auto config = SphericalProjectorConfig::from(params, "projection_model"); config) { - return std::make_shared(config.value()); + return std::make_unique(config.value()); } else { LOG(ERROR) << "Spherical projector config could not be loaded."; return nullptr; @@ -39,7 +39,7 @@ ProjectorBase::Ptr wavemap::ProjectorFactory::create( if (const auto config = OusterProjectorConfig::from(params, "projection_model"); config) { - return std::make_shared(config.value()); + return std::make_unique(config.value()); } else { LOG(ERROR) << "Ouster projector config could not be loaded."; return nullptr; @@ -49,7 +49,7 @@ ProjectorBase::Ptr wavemap::ProjectorFactory::create( if (const auto config = PinholeCameraProjectorConfig::from(params, "projection_model"); config) { - return std::make_shared(config.value()); + return std::make_unique(config.value()); } else { LOG(ERROR) << "Pinhole projector config could not be loaded."; return nullptr; diff --git a/library/src/core/map/map_factory.cc b/library/src/core/map/map_factory.cc index 011aa48b0..19ad56173 100644 --- a/library/src/core/map/map_factory.cc +++ b/library/src/core/map/map_factory.cc @@ -7,8 +7,8 @@ #include "wavemap/core/map/wavelet_octree.h" namespace wavemap { -MapBase::Ptr MapFactory::create(const param::Value& params, - std::optional default_map_type) { +std::unique_ptr MapFactory::create( + const param::Value& params, std::optional default_map_type) { if (const auto type = MapType::from(params); type) { return create(type.value(), params); } @@ -23,11 +23,12 @@ MapBase::Ptr MapFactory::create(const param::Value& params, return nullptr; } -MapBase::Ptr MapFactory::create(MapType map_type, const param::Value& params) { +std::unique_ptr MapFactory::create(MapType map_type, + const param::Value& params) { switch (map_type) { case MapType::kHashedBlocks: { if (const auto config = MapBaseConfig::from(params); config) { - return std::make_shared(config.value()); + return std::make_unique(config.value()); } else { LOG(ERROR) << "Hashed blocks volumetric data structure config could " "not be loaded."; @@ -36,7 +37,7 @@ MapBase::Ptr MapFactory::create(MapType map_type, const param::Value& params) { } case MapType::kOctree: { if (const auto config = VolumetricOctreeConfig::from(params); config) { - return std::make_shared(config.value()); + return std::make_unique(config.value()); } else { LOG(ERROR) << "Octree volumetric data structure config could not be loaded."; @@ -45,7 +46,7 @@ MapBase::Ptr MapFactory::create(MapType map_type, const param::Value& params) { } case MapType::kWaveletOctree: { if (const auto config = WaveletOctreeConfig::from(params); config) { - return std::make_shared(config.value()); + return std::make_unique(config.value()); } else { LOG(ERROR) << "Wavelet octree volumetric data structure config could " "not be loaded."; @@ -54,7 +55,7 @@ MapBase::Ptr MapFactory::create(MapType map_type, const param::Value& params) { } case MapType::kHashedWaveletOctree: { if (const auto config = HashedWaveletOctreeConfig::from(params); config) { - return std::make_shared(config.value()); + return std::make_unique(config.value()); } else { LOG(ERROR) << "Hashed wavelet octree volumetric data structure config " "could not be loaded."; @@ -64,7 +65,7 @@ MapBase::Ptr MapFactory::create(MapType map_type, const param::Value& params) { case MapType::kHashedChunkedWaveletOctree: { if (const auto config = HashedChunkedWaveletOctreeConfig::from(params); config) { - return std::make_shared(config.value()); + return std::make_unique(config.value()); } else { LOG(ERROR) << "Hashed chunked wavelet octree volumetric data structure " "config could not be loaded."; From 7fa862be7ac299c389fc341bfff00e5ed564fcc0 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 17 Jul 2024 15:27:02 +0200 Subject: [PATCH 101/159] Move pointcloud undistortion code into ROS-agnostic library --- interfaces/ros1/wavemap_ros/CMakeLists.txt | 2 +- .../wavemap_ros/inputs/pointcloud_input.h | 4 +- .../pointcloud_undistorter.h | 15 +-- .../stamped_pointcloud.h | 92 ---------------- .../src/inputs/pointcloud_input.cc | 4 +- .../src/utils/pointcloud_undistorter.cc | 61 ++++++++++ .../pointcloud_undistorter.cc | 104 ------------------ .../undistortion/pointcloud_undistortion.h | 13 +++ .../utils/undistortion/stamped_pointcloud.h | 69 ++++++++++++ .../utils/undistortion/stamped_pose_buffer.h | 16 +++ .../core/utils/undistortion/timestamps.h | 9 ++ library/src/core/CMakeLists.txt | 2 + .../undistortion/pointcloud_undistortion.cc | 68 ++++++++++++ .../utils/undistortion/stamped_pointcloud.cc | 41 +++++++ 14 files changed, 292 insertions(+), 208 deletions(-) rename interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/{pointcloud_undistortion => }/pointcloud_undistorter.h (62%) delete mode 100644 interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/stamped_pointcloud.h create mode 100644 interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistorter.cc delete mode 100644 interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistortion/pointcloud_undistorter.cc create mode 100644 library/include/wavemap/core/utils/undistortion/pointcloud_undistortion.h create mode 100644 library/include/wavemap/core/utils/undistortion/stamped_pointcloud.h create mode 100644 library/include/wavemap/core/utils/undistortion/stamped_pose_buffer.h create mode 100644 library/include/wavemap/core/utils/undistortion/timestamps.h create mode 100644 library/src/core/utils/undistortion/pointcloud_undistortion.cc create mode 100644 library/src/core/utils/undistortion/stamped_pointcloud.cc diff --git a/interfaces/ros1/wavemap_ros/CMakeLists.txt b/interfaces/ros1/wavemap_ros/CMakeLists.txt index bae3b2cc8..dfbb57646 100644 --- a/interfaces/ros1/wavemap_ros/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros/CMakeLists.txt @@ -24,7 +24,7 @@ cs_add_library(${PROJECT_NAME} src/map_operations/map_ros_operation_factory.cc src/map_operations/publish_map_operation.cc src/map_operations/publish_pointcloud_operation.cc - src/utils/pointcloud_undistortion/pointcloud_undistorter.cc + src/utils/pointcloud_undistorter.cc src/utils/rosbag_processor.cc src/utils/tf_transformer.cc src/ros_server.cc) diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h index b15f23080..67b9fbb1c 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h @@ -11,7 +11,7 @@ #include #include "wavemap_ros/inputs/input_base.h" -#include "wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h" +#include "wavemap_ros/utils/pointcloud_undistorter.h" #ifdef LIVOX_AVAILABLE #include @@ -109,7 +109,7 @@ class PointcloudInput : public InputBase { PointcloudUndistorter pointcloud_undistorter_; ros::Subscriber pointcloud_sub_; - std::queue pointcloud_queue_; + std::queue pointcloud_queue_; void processQueue() override; static bool hasField(const sensor_msgs::PointCloud2& msg, diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistorter.h similarity index 62% rename from interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistorter.h index 212179d57..bc14eecc9 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistorter.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ROS_UTILS_POINTCLOUD_UNDISTORTION_POINTCLOUD_UNDISTORTER_H_ -#define WAVEMAP_ROS_UTILS_POINTCLOUD_UNDISTORTION_POINTCLOUD_UNDISTORTER_H_ +#ifndef WAVEMAP_ROS_UTILS_POINTCLOUD_UNDISTORTER_H_ +#define WAVEMAP_ROS_UTILS_POINTCLOUD_UNDISTORTER_H_ #include #include @@ -9,8 +9,8 @@ #include #include +#include -#include "wavemap_ros/utils/pointcloud_undistortion/stamped_pointcloud.h" #include "wavemap_ros/utils/tf_transformer.h" namespace wavemap { @@ -29,9 +29,10 @@ class PointcloudUndistorter { num_interpolation_intervals_per_cloud_( num_interpolation_intervals_per_cloud) {} - Result undistortPointcloud(StampedPointcloud& stamped_pointcloud, - PosedPointcloud<>& undistorted_pointcloud, - const std::string& fixed_frame); + Result undistortPointcloud( + undistortion::StampedPointcloud& stamped_pointcloud, + PosedPointcloud<>& undistorted_pointcloud, + const std::string& fixed_frame); private: std::shared_ptr transformer_; @@ -39,4 +40,4 @@ class PointcloudUndistorter { }; } // namespace wavemap -#endif // WAVEMAP_ROS_UTILS_POINTCLOUD_UNDISTORTION_POINTCLOUD_UNDISTORTER_H_ +#endif // WAVEMAP_ROS_UTILS_POINTCLOUD_UNDISTORTER_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/stamped_pointcloud.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/stamped_pointcloud.h deleted file mode 100644 index 853d3bac7..000000000 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/pointcloud_undistortion/stamped_pointcloud.h +++ /dev/null @@ -1,92 +0,0 @@ -#ifndef WAVEMAP_ROS_UTILS_POINTCLOUD_UNDISTORTION_STAMPED_POINTCLOUD_H_ -#define WAVEMAP_ROS_UTILS_POINTCLOUD_UNDISTORTION_STAMPED_POINTCLOUD_H_ - -#include -#include -#include -#include - -#include - -namespace wavemap { -struct StampedPoint { - Point3D position; - uint32_t time_offset; - - StampedPoint(FloatingPoint x, FloatingPoint y, FloatingPoint z, - uint64_t time_offset) - : position(x, y, z), time_offset(time_offset) {} - - std::string toStr() const { - return "[" + std::to_string(position[0]) + ", " + - std::to_string(position[1]) + ", " + std::to_string(position[2]) + - ", " + std::to_string(time_offset) + "]"; - } -}; - -class StampedPointcloud { - public: - StampedPointcloud(uint64_t time_base, std::string sensor_frame, - size_t expected_num_points = 0) - : sensor_frame_(std::move(sensor_frame)), time_base_(time_base) { - points_.reserve(expected_num_points); - } - - // Add points - template - auto emplace(Args... args) { - is_sorted = false; - return points_.emplace_back(std::forward(args)...); - } - - // Frame related getters - std::string& getSensorFrame() { return sensor_frame_; } - const std::string& getSensorFrame() const { return sensor_frame_; } - - // Time related getters - uint64_t& getTimeBase() { return time_base_; } - const uint64_t& getTimeBase() const { return time_base_; } - - uint64_t getStartTime() { - sort(); - return time_base_ + points_.front().time_offset; - } - uint64_t getMedianTime() { - sort(); - return time_base_ + points_[points_.size() / 2].time_offset; - } - uint64_t getEndTime() { - sort(); - return time_base_ + points_.back().time_offset; - } - - // Point related getters - std::vector& getPoints() { return points_; } - const std::vector& getPoints() const { return points_; } - - StampedPoint& operator[](size_t point_idx) { return points_[point_idx]; } - const StampedPoint& operator[](size_t point_idx) const { - return points_[point_idx]; - } - - // Sort points by time - void sort() { - if (!is_sorted) { - std::sort(points_.begin(), points_.end(), - [](const auto& lhs, const auto& rhs) { - return lhs.time_offset < rhs.time_offset; - }); - is_sorted = true; - } - } - - private: - std::string sensor_frame_; - uint64_t time_base_; - std::vector points_; - - bool is_sorted = false; -}; -} // namespace wavemap - -#endif // WAVEMAP_ROS_UTILS_POINTCLOUD_UNDISTORTION_STAMPED_POINTCLOUD_H_ diff --git a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc index fe1b10249..814e4fb9c 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc @@ -99,8 +99,8 @@ void PointcloudInput::callback(const sensor_msgs::PointCloud2& pointcloud_msg) { std::string sensor_frame_id = config_.sensor_frame_id.empty() ? pointcloud_msg.header.frame_id : config_.sensor_frame_id; - StampedPointcloud stamped_pointcloud{stamp_nsec, std::move(sensor_frame_id), - num_points}; + undistortion::StampedPointcloud stamped_pointcloud{ + stamp_nsec, std::move(sensor_frame_id), num_points}; // Load the points with time information if undistortion is enabled bool loaded = false; diff --git a/interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistorter.cc b/interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistorter.cc new file mode 100644 index 000000000..33e06899e --- /dev/null +++ b/interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistorter.cc @@ -0,0 +1,61 @@ +#include "wavemap_ros/utils/pointcloud_undistorter.h" + +#include +#include +#include + +namespace wavemap { +PointcloudUndistorter::Result PointcloudUndistorter::undistortPointcloud( + undistortion::StampedPointcloud& stamped_pointcloud, + PosedPointcloud<>& undistorted_pointcloud, const std::string& fixed_frame) { + ProfilerZoneScoped; + using undistortion::TimeAbsolute; + + // Get the time interval + const TimeAbsolute start_time = stamped_pointcloud.getStartTime(); + const TimeAbsolute end_time = stamped_pointcloud.getEndTime(); + + // Calculate the step size for the undistortion transform buffer + const auto& points = stamped_pointcloud.getPoints(); + const int num_time_steps = num_interpolation_intervals_per_cloud_ + 1; + const TimeAbsolute step_size = + (points.back().time_offset - points.front().time_offset) / + (num_interpolation_intervals_per_cloud_ - 1); + const TimeAbsolute buffer_start_time = start_time - step_size; + const TimeAbsolute buffer_end_time = end_time + step_size; + + // Make sure all transforms are available + if (!transformer_->isTransformAvailable( + fixed_frame, stamped_pointcloud.getSensorFrame(), + convert::nanoSecondsToRosTime(buffer_start_time))) { + return Result::kStartTimeNotInTfBuffer; + } + if (!transformer_->isTransformAvailable( + fixed_frame, stamped_pointcloud.getSensorFrame(), + convert::nanoSecondsToRosTime(buffer_end_time))) { + return Result::kEndTimeNotInTfBuffer; + } + + // Buffer the transforms + undistortion::StampedPoseBuffer pose_buffer; + pose_buffer.reserve(num_time_steps); + for (int step_idx = 0; step_idx < num_time_steps; ++step_idx) { + auto& stamped_pose = pose_buffer.emplace_back(); + stamped_pose.stamp = start_time + step_idx * step_size; + if (!transformer_->lookupTransform( + fixed_frame, stamped_pointcloud.getSensorFrame(), + convert::nanoSecondsToRosTime(stamped_pose.stamp), + stamped_pose.pose)) { + ROS_WARN_STREAM("Failed to buffer intermediate pose at time " + << convert::nanoSecondsToRosTime(stamped_pose.stamp) + << "."); + return Result::kIntermediateTimeNotInTfBuffer; + } + } + + // Apply motion undistortion and return the result + undistorted_pointcloud = + undistortion::compensate_motion(pose_buffer, stamped_pointcloud); + return Result::kSuccess; +} +} // namespace wavemap diff --git a/interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistortion/pointcloud_undistorter.cc b/interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistortion/pointcloud_undistorter.cc deleted file mode 100644 index 2ee03f75d..000000000 --- a/interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistortion/pointcloud_undistorter.cc +++ /dev/null @@ -1,104 +0,0 @@ -#include "wavemap_ros/utils/pointcloud_undistortion/pointcloud_undistorter.h" - -#include -#include - -namespace wavemap { -PointcloudUndistorter::Result PointcloudUndistorter::undistortPointcloud( - StampedPointcloud& stamped_pointcloud, - PosedPointcloud<>& undistorted_pointcloud, const std::string& fixed_frame) { - ProfilerZoneScoped; - // Get the time interval - const uint64_t start_time = stamped_pointcloud.getStartTime(); - const uint64_t end_time = stamped_pointcloud.getEndTime(); - - // Calculate the step size for the undistortion transform buffer - const auto& points = stamped_pointcloud.getPoints(); - const int num_time_steps = num_interpolation_intervals_per_cloud_ + 1; - const uint64_t step_size = - (points.back().time_offset - points.front().time_offset) / - (num_interpolation_intervals_per_cloud_ - 1); - const uint64_t buffer_start_time = start_time - step_size; - const uint64_t buffer_end_time = end_time + step_size; - - // Make sure all transforms are available - if (!transformer_->isTransformAvailable( - fixed_frame, stamped_pointcloud.getSensorFrame(), - convert::nanoSecondsToRosTime(buffer_start_time))) { - return Result::kStartTimeNotInTfBuffer; - } - if (!transformer_->isTransformAvailable( - fixed_frame, stamped_pointcloud.getSensorFrame(), - convert::nanoSecondsToRosTime(buffer_end_time))) { - return Result::kEndTimeNotInTfBuffer; - } - - // Buffer the transforms - std::vector> timed_poses; - timed_poses.reserve(num_time_steps); - for (int step_idx = 0; step_idx < num_time_steps; ++step_idx) { - auto& timed_pose = timed_poses.emplace_back(); - timed_pose.first = start_time + step_idx * step_size; - if (!transformer_->lookupTransform( - fixed_frame, stamped_pointcloud.getSensorFrame(), - convert::nanoSecondsToRosTime(timed_pose.first), - timed_pose.second)) { - ROS_WARN_STREAM("Failed to buffer intermediate pose at time " - << convert::nanoSecondsToRosTime(timed_pose.first) - << "."); - return Result::kIntermediateTimeNotInTfBuffer; - } - } - - // Motion undistort - // NOTE: The undistortion is done by transforming the points into a fixed - // (inertial) frame using the sensor's pose at each point's timestamp. - const auto num_points = static_cast(points.size()); - Eigen::Matrix t_W_points; - t_W_points.setZero(3, num_points); - uint64_t previous_point_time = -1u; - int pose_left_idx = 0u; - Transformation3D T_WCi; - Transformation3D T_WCmedian; - for (int idx = 0u; idx < num_points; ++idx) { - const auto& point = points[idx]; - const Point3D& Ci_p = point.position; - - // Get the sensor pose at the current point's time stamp - const uint64_t time = stamped_pointcloud.getTimeBase() + point.time_offset; - if (time != previous_point_time) { - previous_point_time = time; - while (timed_poses[pose_left_idx + 1].first < time && - pose_left_idx + 2 < num_time_steps) { - ++pose_left_idx; - } - CHECK_LT(pose_left_idx + 1, timed_poses.size()); - const uint64_t time_left = timed_poses[pose_left_idx].first; - const uint64_t time_right = timed_poses[pose_left_idx + 1].first; - const Transformation3D& T_WCleft = timed_poses[pose_left_idx].second; - const Transformation3D& T_WCright = timed_poses[pose_left_idx + 1].second; - FloatingPoint a = static_cast((time - time_left)) / - static_cast((time_right - time_left)); - DCHECK_GE(a, 0.f); - DCHECK_LE(a, 1.f); - T_WCi = interpolateComponentwise(T_WCleft, T_WCright, a); - } - - // Transform the current point into the fixed frame - t_W_points.col(idx) = T_WCi * Ci_p; - - // Store the sensor's pose at the median timestamp - if (idx == num_points / 2) { - T_WCmedian = T_WCi; - } - } - - // Transform the undistorted pointcloud back into sensor frame, - // as needed by the integrators - auto t_C_points = T_WCmedian.inverse().transformVectorized(t_W_points); - - // Return the result - undistorted_pointcloud = PosedPointcloud<>(T_WCmedian, std::move(t_C_points)); - return Result::kSuccess; -} -} // namespace wavemap diff --git a/library/include/wavemap/core/utils/undistortion/pointcloud_undistortion.h b/library/include/wavemap/core/utils/undistortion/pointcloud_undistortion.h new file mode 100644 index 000000000..35110f8d1 --- /dev/null +++ b/library/include/wavemap/core/utils/undistortion/pointcloud_undistortion.h @@ -0,0 +1,13 @@ +#ifndef WAVEMAP_CORE_UTILS_UNDISTORTION_POINTCLOUD_UNDISTORTION_H_ +#define WAVEMAP_CORE_UTILS_UNDISTORTION_POINTCLOUD_UNDISTORTION_H_ + +#include "wavemap/core/data_structure/pointcloud.h" +#include "wavemap/core/utils/undistortion/stamped_pointcloud.h" +#include "wavemap/core/utils/undistortion/stamped_pose_buffer.h" + +namespace wavemap::undistortion { +PosedPointcloud<> compensate_motion(const StampedPoseBuffer& pose_buffer, + StampedPointcloud& stamped_pointcloud); +} // namespace wavemap::undistortion + +#endif // WAVEMAP_CORE_UTILS_UNDISTORTION_POINTCLOUD_UNDISTORTION_H_ diff --git a/library/include/wavemap/core/utils/undistortion/stamped_pointcloud.h b/library/include/wavemap/core/utils/undistortion/stamped_pointcloud.h new file mode 100644 index 000000000..aff568c0c --- /dev/null +++ b/library/include/wavemap/core/utils/undistortion/stamped_pointcloud.h @@ -0,0 +1,69 @@ +#ifndef WAVEMAP_CORE_UTILS_UNDISTORTION_STAMPED_POINTCLOUD_H_ +#define WAVEMAP_CORE_UTILS_UNDISTORTION_STAMPED_POINTCLOUD_H_ + +#include +#include +#include +#include + +#include "wavemap/core/common.h" +#include "wavemap/core/utils/undistortion/timestamps.h" + +namespace wavemap::undistortion { +struct StampedPoint { + Point3D position; + TimeOffset time_offset; + + StampedPoint(FloatingPoint x, FloatingPoint y, FloatingPoint z, + TimeOffset time_offset) + : position(x, y, z), time_offset(time_offset) {} + + std::string toStr() const; +}; + +class StampedPointcloud { + public: + StampedPointcloud(TimeAbsolute time_base, std::string sensor_frame, + size_t expected_num_points = 0); + + // Add points + template + auto emplace(Args... args) { + is_sorted = false; + return points_.emplace_back(std::forward(args)...); + } + + // Frame related getters + std::string& getSensorFrame() { return sensor_frame_; } + const std::string& getSensorFrame() const { return sensor_frame_; } + + // Time related getters + TimeAbsolute& getTimeBase() { return time_base_; } + const TimeAbsolute& getTimeBase() const { return time_base_; } + + TimeAbsolute getStartTime(); + TimeAbsolute getMedianTime(); + TimeAbsolute getEndTime(); + + // Point related getters + std::vector& getPoints() { return points_; } + const std::vector& getPoints() const { return points_; } + + StampedPoint& operator[](size_t point_idx) { return points_[point_idx]; } + const StampedPoint& operator[](size_t point_idx) const { + return points_[point_idx]; + } + + // Sort points by time + void sort(); + + private: + std::string sensor_frame_; + TimeAbsolute time_base_; + std::vector points_; + + bool is_sorted = false; +}; +} // namespace wavemap::undistortion + +#endif // WAVEMAP_CORE_UTILS_UNDISTORTION_STAMPED_POINTCLOUD_H_ diff --git a/library/include/wavemap/core/utils/undistortion/stamped_pose_buffer.h b/library/include/wavemap/core/utils/undistortion/stamped_pose_buffer.h new file mode 100644 index 000000000..f2a155eab --- /dev/null +++ b/library/include/wavemap/core/utils/undistortion/stamped_pose_buffer.h @@ -0,0 +1,16 @@ +#ifndef WAVEMAP_CORE_UTILS_UNDISTORTION_STAMPED_POSE_BUFFER_H_ +#define WAVEMAP_CORE_UTILS_UNDISTORTION_STAMPED_POSE_BUFFER_H_ + +#include + +#include "wavemap/core/utils/undistortion/timestamps.h" + +namespace wavemap::undistortion { +struct StampedPose { + TimeAbsolute stamp; + Transformation3D pose; +}; +using StampedPoseBuffer = std::vector; +} // namespace wavemap::undistortion + +#endif // WAVEMAP_CORE_UTILS_UNDISTORTION_STAMPED_POSE_BUFFER_H_ diff --git a/library/include/wavemap/core/utils/undistortion/timestamps.h b/library/include/wavemap/core/utils/undistortion/timestamps.h new file mode 100644 index 000000000..3802737b7 --- /dev/null +++ b/library/include/wavemap/core/utils/undistortion/timestamps.h @@ -0,0 +1,9 @@ +#ifndef WAVEMAP_CORE_UTILS_UNDISTORTION_TIMESTAMPS_H_ +#define WAVEMAP_CORE_UTILS_UNDISTORTION_TIMESTAMPS_H_ + +namespace wavemap::undistortion { +using TimeAbsolute = uint64_t; +using TimeOffset = uint32_t; +} // namespace wavemap::undistortion + +#endif // WAVEMAP_CORE_UTILS_UNDISTORTION_TIMESTAMPS_H_ diff --git a/library/src/core/CMakeLists.txt b/library/src/core/CMakeLists.txt index 3eaabd793..6dd09c792 100644 --- a/library/src/core/CMakeLists.txt +++ b/library/src/core/CMakeLists.txt @@ -37,6 +37,8 @@ target_sources(wavemap_core PRIVATE utils/sdf/full_euclidean_sdf_generator.cc utils/sdf/quasi_euclidean_sdf_generator.cc utils/time/stopwatch.cc + utils/undistortion/pointcloud_undistortion.cc + utils/undistortion/stamped_pointcloud.cc utils/thread_pool.cc) set_wavemap_target_properties(wavemap_core) diff --git a/library/src/core/utils/undistortion/pointcloud_undistortion.cc b/library/src/core/utils/undistortion/pointcloud_undistortion.cc new file mode 100644 index 000000000..9c8fe65ee --- /dev/null +++ b/library/src/core/utils/undistortion/pointcloud_undistortion.cc @@ -0,0 +1,68 @@ +#include "wavemap/core/utils/undistortion/pointcloud_undistortion.h" + +namespace wavemap { +PosedPointcloud<> undistortion::compensate_motion( + const StampedPoseBuffer& pose_buffer, + undistortion::StampedPointcloud& stamped_pointcloud) { + // Check that timestamps of all points fall within range of pose buffer + { + const auto& buffer_start_time = pose_buffer.front().stamp; + const auto& buffer_end_time = pose_buffer.back().stamp; + CHECK_GE(stamped_pointcloud.getStartTime(), buffer_start_time); + CHECK_LE(stamped_pointcloud.getEndTime(), buffer_end_time); + } + + // Motion undistort + // NOTE: The undistortion is done by transforming the points into a fixed + // (inertial) frame using the sensor's pose at each point's timestamp. + const auto& points = stamped_pointcloud.getPoints(); + const auto num_points = static_cast(points.size()); + const auto num_time_steps = static_cast(pose_buffer.size()); + Eigen::Matrix t_W_points; + t_W_points.setZero(3, num_points); + TimeAbsolute previous_point_time = -1u; + int pose_left_idx = 0u; + Transformation3D T_WCi; + Transformation3D T_WCmedian; + for (int idx = 0u; idx < num_points; ++idx) { + const auto& point = points[idx]; + const Point3D& Ci_p = point.position; + + // Get the sensor pose at the current point's time stamp + const TimeAbsolute time = + stamped_pointcloud.getTimeBase() + point.time_offset; + if (time != previous_point_time) { + previous_point_time = time; + while (pose_buffer[pose_left_idx + 1].stamp < time && + pose_left_idx + 2 < num_time_steps) { + ++pose_left_idx; + } + CHECK_LT(pose_left_idx + 1, pose_buffer.size()); + const TimeAbsolute time_left = pose_buffer[pose_left_idx].stamp; + const TimeAbsolute time_right = pose_buffer[pose_left_idx + 1].stamp; + const Transformation3D& T_WCleft = pose_buffer[pose_left_idx].pose; + const Transformation3D& T_WCright = pose_buffer[pose_left_idx + 1].pose; + FloatingPoint a = static_cast((time - time_left)) / + static_cast((time_right - time_left)); + DCHECK_GE(a, 0.f); + DCHECK_LE(a, 1.f); + T_WCi = interpolateComponentwise(T_WCleft, T_WCright, a); + } + + // Transform the current point into the fixed frame + t_W_points.col(idx) = T_WCi * Ci_p; + + // Store the sensor's pose at the median timestamp + if (idx == num_points / 2) { + T_WCmedian = T_WCi; + } + } + + // Transform the undistorted pointcloud back into sensor frame, + // as needed by the integrators + auto t_C_points = T_WCmedian.inverse().transformVectorized(t_W_points); + + // Return the result + return PosedPointcloud<>{T_WCmedian, std::move(t_C_points)}; +} +} // namespace wavemap diff --git a/library/src/core/utils/undistortion/stamped_pointcloud.cc b/library/src/core/utils/undistortion/stamped_pointcloud.cc new file mode 100644 index 000000000..ce70644de --- /dev/null +++ b/library/src/core/utils/undistortion/stamped_pointcloud.cc @@ -0,0 +1,41 @@ +#include "wavemap/core/utils/undistortion/stamped_pointcloud.h" + +namespace wavemap::undistortion { +std::string StampedPoint::toStr() const { + return "[" + std::to_string(position[0]) + ", " + + std::to_string(position[1]) + ", " + std::to_string(position[2]) + + ", " + std::to_string(time_offset) + "]"; +} + +StampedPointcloud::StampedPointcloud(TimeAbsolute time_base, + std::string sensor_frame, + size_t expected_num_points) + : sensor_frame_(std::move(sensor_frame)), time_base_(time_base) { + points_.reserve(expected_num_points); +} + +TimeAbsolute StampedPointcloud::getStartTime() { + sort(); + return time_base_ + points_.front().time_offset; +} + +TimeAbsolute StampedPointcloud::getMedianTime() { + sort(); + return time_base_ + points_[points_.size() / 2].time_offset; +} + +TimeAbsolute StampedPointcloud::getEndTime() { + sort(); + return time_base_ + points_.back().time_offset; +} + +void StampedPointcloud::sort() { + if (!is_sorted) { + std::sort(points_.begin(), points_.end(), + [](const auto& lhs, const auto& rhs) { + return lhs.time_offset < rhs.time_offset; + }); + is_sorted = true; + } +} +} // namespace wavemap::undistortion From a4a649bac5c172534021b23a4473b940ad010658 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 17 Jul 2024 23:00:22 +0200 Subject: [PATCH 102/159] Make integrator max_update_resolution easier to configure --- .../config/wavemap_livox_mid360.yaml | 1 - .../wavemap_livox_mid360_azure_kinect.yaml | 9 +-- .../wavemap_livox_mid360_pico_flexx.yaml | 12 ++-- .../wavemap_livox_mid360_pico_monstar.yaml | 57 +++++++++---------- .../config/wavemap_ouster_os0.yaml | 1 - .../wavemap_ouster_os0_pico_monstar.yaml | 9 +-- .../config/wavemap_ouster_os1.yaml | 1 - .../config/wavemap_panoptic_mapping_rgbd.yaml | 1 - .../wavemap/core/integrator/integrator_base.h | 2 +- .../hashed_chunked_wavelet_integrator.h | 7 ++- .../hashed_wavelet_integrator.h | 13 +++-- .../impl/wavelet_integrator_inl.h | 2 +- .../coarse_to_fine/wavelet_integrator.h | 10 +++- .../projective/projective_integrator.h | 11 ++-- .../src/core/integrator/integrator_base.cc | 6 +- .../hashed_chunked_wavelet_integrator.cc | 2 +- .../hashed_wavelet_integrator.cc | 2 +- .../projective/projective_integrator.cc | 9 ++- .../ray_tracing/ray_tracing_integrator.cc | 2 +- .../integration_method.json | 15 +++-- .../measurement_model.json | 6 +- .../projection_model.json | 8 +-- 22 files changed, 93 insertions(+), 93 deletions(-) diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml index 047f998da..dec986ccf 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml @@ -39,7 +39,6 @@ measurement_integrators: type: hashed_chunked_wavelet_integrator min_range: { meters: 0.5 } max_range: { meters: 12.0 } - termination_update_error: 0.1 inputs: - type: pointcloud diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml index 27b721a9a..d4029b865 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml @@ -39,8 +39,7 @@ measurement_integrators: type: hashed_chunked_wavelet_integrator min_range: { meters: 1.5 } max_range: { meters: 10.0 } - termination_update_error: 0.1 - termination_height: 3 + max_update_resolution: { meters: 0.08 } kinect_short_range: projection_model: @@ -60,8 +59,7 @@ measurement_integrators: type: hashed_chunked_wavelet_integrator min_range: { meters: 0.05 } max_range: { meters: 1.0 } - termination_update_error: 0.1 - termination_height: 0 + max_update_resolution: { meters: 0.01 } kinect_medium_range: projection_model: @@ -81,8 +79,7 @@ measurement_integrators: type: hashed_chunked_wavelet_integrator min_range: { meters: 1.0 } max_range: { meters: 2.0 } - termination_update_error: 0.1 - termination_height: 1 + max_update_resolution: { meters: 0.02 } inputs: - type: depth_image diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml index ed92f6947..2ac2438cb 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml @@ -40,8 +40,7 @@ measurement_integrators: type: hashed_chunked_wavelet_integrator min_range: { meters: 1.0 } max_range: { meters: 8.0 } - termination_update_error: 0.1 - termination_height: 3 + max_update_resolution: { meters: 0.08 } monstar_short_range: projection_model: @@ -61,8 +60,7 @@ measurement_integrators: type: hashed_chunked_wavelet_integrator min_range: { meters: 0.05 } max_range: { meters: 1.0 } - termination_update_error: 0.1 - termination_height: 0 + max_update_resolution: { meters: 0.01 } monstar_medium_range: projection_model: @@ -82,8 +80,7 @@ measurement_integrators: type: hashed_chunked_wavelet_integrator min_range: { meters: 1.0 } max_range: { meters: 2.0 } - termination_update_error: 0.1 - termination_height: 1 + max_update_resolution: { meters: 0.02 } monstar_long_range: projection_model: @@ -103,8 +100,7 @@ measurement_integrators: type: hashed_chunked_wavelet_integrator min_range: { meters: 2.0 } max_range: { meters: 3.0 } - termination_update_error: 0.1 - termination_height: 2 + max_update_resolution: { meters: 0.04 } inputs: - type: depth_image diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml index ceffda703..14cbdb699 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml @@ -17,6 +17,31 @@ map_operations_pipeline: once_every: { seconds: 2.0 } measurement_integrators: + livox_mid360: + projection_model: + type: spherical_projector + elevation: + num_cells: 32 + min_angle: { degrees: -7.0 } + max_angle: { degrees: 52.0 } + azimuth: + num_cells: 512 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.035 } + # NOTE: For best results, we recommend setting range_sigma to + # max(min_cell_width / 2, ouster_noise) where ouster_noise = 0.05 + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 2.0 } + max_range: { meters: 8.0 } + max_update_resolution: { meters: 0.08 } + monstar_short_range: projection_model: type: pinhole_camera_projector @@ -35,8 +60,7 @@ measurement_integrators: type: hashed_chunked_wavelet_integrator min_range: { meters: 0.3 } max_range: { meters: 1.0 } - termination_update_error: 0.1 - termination_height: 0 + max_update_resolution: { meters: 0.01 } monstar_medium_range: projection_model: @@ -56,34 +80,7 @@ measurement_integrators: type: hashed_chunked_wavelet_integrator min_range: { meters: 1.0 } max_range: { meters: 2.0 } - termination_update_error: 0.1 - termination_height: 1 - - livox_mid360: - projection_model: - type: spherical_projector - elevation: - num_cells: 32 - min_angle: { degrees: -7.0 } - max_angle: { degrees: 52.0 } - azimuth: - num_cells: 512 - min_angle: { degrees: -180.0 } - max_angle: { degrees: 180.0 } - measurement_model: - type: continuous_beam - angle_sigma: { degrees: 0.035 } - # NOTE: For best results, we recommend setting range_sigma to - # max(min_cell_width / 2, ouster_noise) where ouster_noise = 0.05 - range_sigma: { meters: 0.05 } - scaling_free: 0.2 - scaling_occupied: 0.4 - integration_method: - type: hashed_chunked_wavelet_integrator - min_range: { meters: 2.0 } - max_range: { meters: 8.0 } - termination_update_error: 0.1 - termination_height: 3 + max_update_resolution: { meters: 0.02 } inputs: - type: depth_image diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml index 8ec3a9834..2e889b181 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml @@ -41,7 +41,6 @@ measurement_integrators: type: hashed_chunked_wavelet_integrator min_range: { meters: 1.0 } max_range: { meters: 15.0 } - termination_update_error: 0.1 inputs: - type: pointcloud diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml index 84e993a50..0c06e3a6f 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml @@ -42,8 +42,7 @@ measurement_integrators: type: hashed_chunked_wavelet_integrator min_range: { meters: 1.5 } max_range: { meters: 15.0 } - termination_update_error: 0.1 - termination_height: 3 + max_update_resolution: { meters: 0.08 } monstar_short_range: projection_model: @@ -63,8 +62,7 @@ measurement_integrators: type: hashed_chunked_wavelet_integrator min_range: { meters: 0.3 } max_range: { meters: 1.0 } - termination_update_error: 0.1 - termination_height: 0 + max_update_resolution: { meters: 0.01 } monstar_medium_range: projection_model: @@ -84,8 +82,7 @@ measurement_integrators: type: hashed_chunked_wavelet_integrator min_range: { meters: 1.0 } max_range: { meters: 2.0 } - termination_update_error: 0.1 - termination_height: 1 + max_update_resolution: { meters: 0.02 } inputs: - type: depth_image diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml index 3fdd617f9..c8424c3dd 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml @@ -39,7 +39,6 @@ measurement_integrators: type: wavelet_integrator min_range: { meters: 1.0 } max_range: { meters: 20.0 } - termination_update_error: 0.1 inputs: - type: pointcloud diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml index 2eb77282e..1467ad966 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml @@ -34,7 +34,6 @@ measurement_integrators: type: hashed_chunked_wavelet_integrator min_range: { meters: 0.1 } max_range: { meters: 5.0 } - termination_update_error: 0.1 inputs: - type: depth_image diff --git a/library/include/wavemap/core/integrator/integrator_base.h b/library/include/wavemap/core/integrator/integrator_base.h index 728c9f7e3..adad0271d 100644 --- a/library/include/wavemap/core/integrator/integrator_base.h +++ b/library/include/wavemap/core/integrator/integrator_base.h @@ -40,7 +40,7 @@ class IntegratorBase { virtual void integrate(const PosedImage<>& range_image) = 0; protected: - static bool isPointcloudValid(const PosedPointcloud<>& pointcloud); + static bool isPoseValid(const Transformation3D& T_W_C); static bool isMeasurementValid(const Point3D& C_end_point); static Point3D getEndPointOrMaxRange(const Point3D& W_start_point, diff --git a/library/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h b/library/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h index 496e7ab59..ff8cfc916 100644 --- a/library/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h +++ b/library/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h @@ -36,7 +36,7 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { std::shared_ptr thread_pool_; std::shared_ptr range_image_intersector_; - // Cache/pre-computed commonly used values + // Cache/pre-compute commonly used values static constexpr FloatingPoint kNoiseThreshold = 1e-3f; static constexpr auto kUnitCubeHalfDiagonal = constants::kSqrt3 / 2.f; @@ -49,6 +49,11 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { const FloatingPoint max_log_odds_padded_ = max_log_odds_ + kNoiseThreshold; const IndexElement tree_height_ = occupancy_map_->getTreeHeight(); const IndexElement chunk_height_ = occupancy_map_->getChunkHeight(); + const IndexElement termination_height_ = + min_cell_width_ < config_.max_update_resolution + ? static_cast(std::round( + std::log2(config_.max_update_resolution / min_cell_width_))) + : 0; std::pair getFovMinMaxIndices( const Point3D& sensor_origin) const; diff --git a/library/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h b/library/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h index ea726ff84..44d3b78fa 100644 --- a/library/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h +++ b/library/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h @@ -30,16 +30,21 @@ class HashedWaveletIntegrator : public ProjectiveIntegrator { private: const HashedWaveletOctree::Ptr occupancy_map_; - const FloatingPoint min_cell_width_ = occupancy_map_->getMinCellWidth(); - const FloatingPoint min_cell_width_inv_ = 1.f / min_cell_width_; std::shared_ptr thread_pool_; + std::shared_ptr range_image_intersector_; + // Cache/pre-compute commonly used values + const FloatingPoint min_cell_width_ = occupancy_map_->getMinCellWidth(); + const FloatingPoint min_cell_width_inv_ = 1.f / min_cell_width_; static constexpr FloatingPoint kNoiseThreshold = 1e-4f; const FloatingPoint min_log_odds_ = occupancy_map_->getMinLogOdds(); const FloatingPoint max_log_odds_ = occupancy_map_->getMaxLogOdds(); const IndexElement tree_height_ = occupancy_map_->getTreeHeight(); - - std::shared_ptr range_image_intersector_; + const IndexElement termination_height_ = + min_cell_width_ < config_.max_update_resolution + ? static_cast(std::round( + std::log2(config_.max_update_resolution / min_cell_width_))) + : 0; static constexpr auto kUnitCubeHalfDiagonal = constants::kSqrt3 / 2.f; diff --git a/library/include/wavemap/core/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h b/library/include/wavemap/core/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h index db1ae58e5..a5fb984d5 100644 --- a/library/include/wavemap/core/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h +++ b/library/include/wavemap/core/integrator/projective/coarse_to_fine/impl/wavelet_integrator_inl.h @@ -11,7 +11,7 @@ inline FloatingPoint WaveletIntegrator::recursiveSamplerCompressor( // NOLINT constexpr FloatingPoint kNoiseThreshold = 1e-4f; // If we're at the leaf level, directly update the node - if (node_index.height == config_.termination_height) { + if (node_index.height <= termination_height_) { const Point3D W_node_center = convert::nodeIndexToCenterPoint(node_index, min_cell_width_); const Point3D C_node_center = diff --git a/library/include/wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.h b/library/include/wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.h index 6957e5438..e4d5199e1 100644 --- a/library/include/wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.h +++ b/library/include/wavemap/core/integrator/projective/coarse_to_fine/wavelet_integrator.h @@ -26,9 +26,15 @@ class WaveletIntegrator : public ProjectiveIntegrator { private: const WaveletOctree::Ptr occupancy_map_; - const FloatingPoint min_cell_width_; - std::shared_ptr range_image_intersector_; + + // Cache/pre-computed commonly used values + const FloatingPoint min_cell_width_; + const IndexElement termination_height_ = + min_cell_width_ < config_.max_update_resolution + ? static_cast(std::round( + std::log2(config_.max_update_resolution / min_cell_width_))) + : 0; static constexpr auto kUnitCubeHalfDiagonal = constants::kSqrt3 / 2.f; diff --git a/library/include/wavemap/core/integrator/projective/projective_integrator.h b/library/include/wavemap/core/integrator/projective/projective_integrator.h index b2d73c3b1..09f5f51d3 100644 --- a/library/include/wavemap/core/integrator/projective/projective_integrator.h +++ b/library/include/wavemap/core/integrator/projective/projective_integrator.h @@ -24,12 +24,11 @@ struct ProjectiveIntegratorConfig : ConfigBase { //! up to the maximum range. Meters max_range = 20.f; - //! Maximum resolution to use for this integrator, set as the height at which - //! to terminate the coarse-to-fine measurement updates. Defaults to 0 (max - //! res). Can be set to 1 for 1/2 of the max resolution, to 2 for 1/4 of the - //! max resolution, etc.This can be used to fuse multiple inputs with - //! different maximum resolutions into a single map. - IndexElement termination_height = 0; + //! Maximum resolution at which to update the map. Can be used to fuse + //! multiple inputs with different maximum resolutions into a single map. + //! Set to zero to match the map's maximum resolution. + Meters max_update_resolution = 0.f; + //! The update error threshold at which the coarse-to-fine measurement //! integrator is allowed to terminate, in log-odds. For more information, //! please refer to: https://www.roboticsproceedings.org/rss19/p065.pdf. diff --git a/library/src/core/integrator/integrator_base.cc b/library/src/core/integrator/integrator_base.cc index a71dc273f..3c140c8ad 100644 --- a/library/src/core/integrator/integrator_base.cc +++ b/library/src/core/integrator/integrator_base.cc @@ -1,11 +1,11 @@ #include "wavemap/core/integrator/integrator_base.h" namespace wavemap { -bool IntegratorBase::isPointcloudValid(const PosedPointcloud<>& pointcloud) { - if (const Point3D& origin = pointcloud.getOrigin(); origin.hasNaN()) { +bool IntegratorBase::isPoseValid(const Transformation3D& T_W_C) { + if (T_W_C.getPosition().hasNaN()) { LOG(WARNING) << "Ignoring request to integrate pointcloud whose origin " "contains NaNs:\n" - << origin; + << T_W_C.getPosition(); return false; } diff --git a/library/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc b/library/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc index c84bdf8f5..f795f4682 100644 --- a/library/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc +++ b/library/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc @@ -172,7 +172,7 @@ void HashedChunkedWaveletIntegrator::updateNodeRecursive( // NOLINT child_node_in_chunk_index); // If we're at the leaf level, directly compute the update - if (child_height == config_.termination_height + 1) { + if (child_height <= termination_height_ + 1) { updateLeavesBatch(child_index, child_value, child_details); } else { // Otherwise, recurse diff --git a/library/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc b/library/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc index 167d6d7b4..6dd9e3c99 100644 --- a/library/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc +++ b/library/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc @@ -121,7 +121,7 @@ void HashedWaveletIntegrator::updateBlock(HashedWaveletOctree::Block& block, DCHECK_GE(node_index.height, 0); // If we're at the leaf level, directly update the node - if (node_index.height == config_.termination_height) { + if (node_index.height <= termination_height_) { const Point3D W_node_center = convert::nodeIndexToCenterPoint(node_index, min_cell_width_); const Point3D C_node_center = diff --git a/library/src/core/integrator/projective/projective_integrator.cc b/library/src/core/integrator/projective/projective_integrator.cc index 5c222ab12..acacc2003 100644 --- a/library/src/core/integrator/projective/projective_integrator.cc +++ b/library/src/core/integrator/projective/projective_integrator.cc @@ -6,7 +6,7 @@ namespace wavemap { DECLARE_CONFIG_MEMBERS(ProjectiveIntegratorConfig, (min_range) (max_range) - (termination_height) + (max_update_resolution) (termination_update_error)); bool ProjectiveIntegratorConfig::isValid(bool verbose) const { @@ -15,7 +15,7 @@ bool ProjectiveIntegratorConfig::isValid(bool verbose) const { is_valid &= IS_PARAM_GT(min_range, 0.f, verbose); is_valid &= IS_PARAM_GT(max_range, 0.f, verbose); is_valid &= IS_PARAM_LT(min_range, max_range, verbose); - is_valid &= IS_PARAM_GE(termination_height, 0, verbose); + is_valid &= IS_PARAM_GE(max_update_resolution, 0.f, verbose); is_valid &= IS_PARAM_GT(termination_update_error, 0.f, verbose); return is_valid; @@ -23,7 +23,7 @@ bool ProjectiveIntegratorConfig::isValid(bool verbose) const { void ProjectiveIntegrator::integrate(const PosedPointcloud<>& pointcloud) { ProfilerZoneScoped; - if (!isPointcloudValid(pointcloud)) { + if (!isPoseValid(pointcloud.getPose())) { return; } importPointcloud(pointcloud); @@ -32,6 +32,9 @@ void ProjectiveIntegrator::integrate(const PosedPointcloud<>& pointcloud) { void ProjectiveIntegrator::integrate(const PosedImage<>& range_image) { ProfilerZoneScoped; + if (!isPoseValid(range_image.getPose())) { + return; + } importRangeImage(range_image); updateMap(); } diff --git a/library/src/core/integrator/ray_tracing/ray_tracing_integrator.cc b/library/src/core/integrator/ray_tracing/ray_tracing_integrator.cc index 981a38967..d60e22ca4 100644 --- a/library/src/core/integrator/ray_tracing/ray_tracing_integrator.cc +++ b/library/src/core/integrator/ray_tracing/ray_tracing_integrator.cc @@ -16,7 +16,7 @@ bool RayTracingIntegratorConfig::isValid(bool verbose) const { } void RayTracingIntegrator::integrate(const PosedPointcloud<>& pointcloud) { - if (!isPointcloudValid(pointcloud)) { + if (!isPoseValid(pointcloud.getPose())) { return; } diff --git a/tooling/schemas/wavemap/measurement_integrators/integration_method.json b/tooling/schemas/wavemap/measurement_integrators/integration_method.json index 66bd90f76..57940f8f5 100644 --- a/tooling/schemas/wavemap/measurement_integrators/integration_method.json +++ b/tooling/schemas/wavemap/measurement_integrators/integration_method.json @@ -12,11 +12,11 @@ }, "min_range": { "description": "Minimum range measurements should have to be considered. Measurements below this threshold are ignored.", - "$ref": "../../value_with_unit/convertible_to_meters.json" + "$ref": "../value_with_unit/convertible_to_meters.json" }, "max_range": { "description": "Maximum range up to which to update the map. Measurements that exceed this range are used as free-space beams, up to the maximum range.", - "$ref": "../../value_with_unit/convertible_to_meters.json" + "$ref": "../value_with_unit/convertible_to_meters.json" } } }, @@ -36,16 +36,15 @@ }, "min_range": { "description": "Minimum range measurements should have to be considered. Measurements below this threshold are ignored.", - "$ref": "../../value_with_unit/convertible_to_meters.json" + "$ref": "../value_with_unit/convertible_to_meters.json" }, "max_range": { "description": "Maximum range up to which to update the map. Measurements that exceed this range are used as free-space beams, up to the maximum range.", - "$ref": "../../value_with_unit/convertible_to_meters.json" + "$ref": "../value_with_unit/convertible_to_meters.json" }, - "termination_height": { - "description": "Maximum resolution to use for this integrator, set as the height at which to terminate the coarse-to-fine measurement updates. Defaults to 0 [max res]. Can be set to 1 for 1/2 of the max resolution, to 2 for 1/4 of the max resolution, etc. This can be used to fuse multiple inputs with different maximum resolutions into a single map.", - "type": "integer", - "minimum": 0 + "max_update_resolution": { + "description": "Maximum resolution at which to update the map. Can be used to fuse multiple inputs with different maximum resolutions into a single map. Set to zero to match the map's maximum resolution.", + "$ref": "../value_with_unit/convertible_to_meters.json" }, "termination_update_error": { "description": "The update error threshold at which the coarse-to-fine measurement integrator is allowed to terminate, in log-odds. For more information, please refer to: https://www.roboticsproceedings.org/rss19/p065.pdf.", diff --git a/tooling/schemas/wavemap/measurement_integrators/measurement_model.json b/tooling/schemas/wavemap/measurement_integrators/measurement_model.json index 6e502c421..9649c2775 100644 --- a/tooling/schemas/wavemap/measurement_integrators/measurement_model.json +++ b/tooling/schemas/wavemap/measurement_integrators/measurement_model.json @@ -37,7 +37,7 @@ }, "range_sigma": { "description": "Uncertainty along the range axis.", - "$ref": "../../value_with_unit/convertible_to_meters.json" + "$ref": "../value_with_unit/convertible_to_meters.json" }, "scaling_free": { "description": "Scale factor to apply to the continuous Bayesian occupancy model for cells that are observed as free. This can, for example, be used to give a higher weight to occupied updates than free updates.", @@ -61,11 +61,11 @@ }, "angle_sigma": { "description": "Uncertainty along the angle axis.", - "$ref": "../../value_with_unit/convertible_to_radians.json" + "$ref": "../value_with_unit/convertible_to_radians.json" }, "range_sigma": { "description": "Uncertainty along the range axis.", - "$ref": "../../value_with_unit/convertible_to_meters.json" + "$ref": "../value_with_unit/convertible_to_meters.json" }, "scaling_free": { "description": "Scale factor to apply to the continuous Bayesian occupancy model for cells that are observed as free. This can, for example, be used to give a higher weight to occupied updates than free updates.", diff --git a/tooling/schemas/wavemap/measurement_integrators/projection_model.json b/tooling/schemas/wavemap/measurement_integrators/projection_model.json index f2fcb4f4f..e4fcd61bb 100644 --- a/tooling/schemas/wavemap/measurement_integrators/projection_model.json +++ b/tooling/schemas/wavemap/measurement_integrators/projection_model.json @@ -9,11 +9,11 @@ "properties": { "min_angle": { "description": "Minimum angle along this axis.", - "$ref": "../../value_with_unit/convertible_to_radians.json" + "$ref": "../value_with_unit/convertible_to_radians.json" }, "max_angle": { "description": "Maximum angle along this axis.", - "$ref": "../../value_with_unit/convertible_to_radians.json" + "$ref": "../value_with_unit/convertible_to_radians.json" }, "num_cells": { "description": "Resolution of the image along this axis, set as the number of cells along the axis.", @@ -55,11 +55,11 @@ }, "lidar_origin_to_beam_origin": { "description": "Offset between the Ouster LiDAR frame's origin and the laser beam's start point (radial direction). For illustrations and more information, please see the Ouster sensor's manual.", - "$ref": "../../value_with_unit/convertible_to_meters.json" + "$ref": "../value_with_unit/convertible_to_meters.json" }, "lidar_origin_to_sensor_origin_z_offset": { "description": "Offset between the Ouster sensor and LiDAR frame's origins (z-direction). For illustrations and more information, please see the Ouster sensor's manual.", - "$ref": "../../value_with_unit/convertible_to_meters.json" + "$ref": "../value_with_unit/convertible_to_meters.json" } } }, From e9491946f44c8c6d94cd39f5d8e14455d0a6f38b Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 18 Jul 2024 17:46:42 +0200 Subject: [PATCH 103/159] Handle integrator names natively through config system --- .../config/wavemap_livox_mid360.yaml | 2 +- .../wavemap_livox_mid360_azure_kinect.yaml | 4 +- .../wavemap_livox_mid360_pico_flexx.yaml | 4 +- .../wavemap_livox_mid360_pico_monstar.yaml | 4 +- .../config/wavemap_ouster_os0.yaml | 2 +- .../wavemap_ouster_os0_pico_monstar.yaml | 4 +- .../config/wavemap_ouster_os1.yaml | 2 +- .../config/wavemap_panoptic_mapping_rgbd.yaml | 2 +- .../wavemap_ros/inputs/depth_image_input.h | 12 ++++-- .../include/wavemap_ros/inputs/input_base.h | 9 ++-- .../wavemap_ros/inputs/pointcloud_input.h | 12 ++++-- .../src/inputs/depth_image_input.cc | 9 ++-- .../ros1/wavemap_ros/src/inputs/input_base.cc | 3 +- .../wavemap_ros/src/inputs/input_factory.cc | 42 ++----------------- .../src/inputs/pointcloud_input.cc | 9 ++-- .../include/wavemap/core/config/string_list.h | 38 +++++++++++++++++ library/src/core/CMakeLists.txt | 1 + library/src/core/config/string_list.cc | 34 +++++++++++++++ .../wavemap/inputs/depth_image_input.json | 6 +-- .../wavemap/inputs/pointcloud_input.json | 6 +-- 20 files changed, 130 insertions(+), 75 deletions(-) create mode 100644 library/include/wavemap/core/config/string_list.h create mode 100644 library/src/core/config/string_list.cc diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml index dec986ccf..1ef763153 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml @@ -44,7 +44,7 @@ inputs: - type: pointcloud topic_name: "/livox/lidar" topic_type: livox - measurement_integrator: livox_mid360 + measurement_integrator_names: livox_mid360 undistort_motion: true reprojected_pointcloud_topic_name: "/wavemap/input" topic_queue_length: 10 diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml index d4029b865..d03a2c467 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml @@ -84,7 +84,7 @@ measurement_integrators: inputs: - type: depth_image topic_name: "/depth/image_rect" - measurement_integrator: [ kinect_short_range, kinect_medium_range ] + measurement_integrator_names: [ kinect_short_range, kinect_medium_range ] depth_scale_factor: 0.001 topic_queue_length: 1 max_wait_for_pose: { seconds: 1.0 } @@ -94,7 +94,7 @@ inputs: - type: pointcloud topic_name: "/livox/lidar" topic_type: livox - measurement_integrator: livox_mid360 + measurement_integrator_names: livox_mid360 undistort_motion: true topic_queue_length: 1 max_wait_for_pose: { seconds: 1.0 } diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml index 2ac2438cb..21e9af7e4 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml @@ -105,7 +105,7 @@ measurement_integrators: inputs: - type: depth_image topic_name: "/pico_flexx/image_rect" - measurement_integrator: [ monstar_short_range, monstar_medium_range, monstar_long_range ] + measurement_integrator_names: [ monstar_short_range, monstar_medium_range, monstar_long_range ] topic_queue_length: 1 max_wait_for_pose: { seconds: 1.0 } time_offset: { seconds: -0.00727598 } @@ -115,7 +115,7 @@ inputs: - type: pointcloud topic_name: "/livox/lidar" topic_type: livox - measurement_integrator: livox_mid360 + measurement_integrator_names: livox_mid360 topic_queue_length: 1 max_wait_for_pose: { seconds: 1.0 } # reprojected_topic_name: "/wavemap/lidar_input" diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml index 14cbdb699..29e7c1593 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml @@ -85,7 +85,7 @@ measurement_integrators: inputs: - type: depth_image topic_name: "/pico_monstar/image_rect" - measurement_integrator: [monstar_short_range, monstar_medium_range] + measurement_integrator_names: [monstar_short_range, monstar_medium_range] topic_queue_length: 1 max_wait_for_pose: { seconds: 1.0 } time_offset: { seconds: 0.00201747 } @@ -95,7 +95,7 @@ inputs: - type: pointcloud topic_name: "/livox/lidar" topic_type: livox - measurement_integrator: livox_mid360 + measurement_integrator_names: livox_mid360 topic_queue_length: 1 max_wait_for_pose: { seconds: 1.0 } # reprojected_pointcloud_topic_name: "/wavemap/lidar_input" diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml index 2e889b181..e306dc729 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml @@ -46,7 +46,7 @@ inputs: - type: pointcloud topic_name: "/os_cloud_node/points" topic_type: ouster - measurement_integrator: ouster_os0 + measurement_integrator_names: ouster_os0 undistort_motion: true topic_queue_length: 10 max_wait_for_pose: { seconds: 1.0 } diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml index 0c06e3a6f..1785276c7 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml @@ -87,7 +87,7 @@ measurement_integrators: inputs: - type: depth_image topic_name: "/pico_monstar/image_rect" - measurement_integrator: [ monstar_short_range, monstar_medium_range ] + measurement_integrator_names: [ monstar_short_range, monstar_medium_range ] topic_queue_length: 1 max_wait_for_pose: { seconds: 1.0 } time_offset: { seconds: 0.0118462 } @@ -97,7 +97,7 @@ inputs: - type: pointcloud topic_name: "/os_cloud_node/points" topic_type: ouster - measurement_integrator: ouster_os0 + measurement_integrator_names: ouster_os0 undistort_motion: true topic_queue_length: 10 max_wait_for_pose: { seconds: 1.0 } diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml index c8424c3dd..80c9f2580 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml @@ -44,7 +44,7 @@ inputs: - type: pointcloud topic_name: "/os1_cloud_node/points" topic_type: ouster - measurement_integrator: ouster_os1 + measurement_integrator_names: ouster_os1 undistort_motion: true topic_queue_length: 10 max_wait_for_pose: { seconds: 1.0 } diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml index 1467ad966..c887afa7a 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml @@ -38,6 +38,6 @@ measurement_integrators: inputs: - type: depth_image topic_name: "/data/depth_image" - measurement_integrator: panoptic_mapping_camera + measurement_integrator_names: panoptic_mapping_camera topic_queue_length: 10 max_wait_for_pose: { seconds: 1.0 } diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h index 9b07781ed..caf02dd5e 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -18,12 +19,17 @@ namespace wavemap { /** * Config struct for the depth image input handler. */ -struct DepthImageInputConfig : public ConfigBase { +struct DepthImageInputConfig + : public ConfigBase { //! Name of the ROS topic to subscribe to. std::string topic_name = "scan"; //! Queue length to use when subscribing to the ROS topic. int topic_queue_length = 10; + //! Name(s) of the measurement integrator(s) used to integrate the + //! measurements into the map. + StringList measurement_integrator_names; + //! Time period used to control the rate at which to retry getting the sensor //! pose when ROS TF lookups fail. Seconds processing_retry_period = 0.05f; @@ -55,7 +61,8 @@ struct DepthImageInputConfig : public ConfigBase { // Conversion to InputHandler base config operator InputBaseConfig() const { // NOLINT - return {topic_name, topic_queue_length, processing_retry_period}; + return {topic_name, topic_queue_length, measurement_integrator_names, + processing_retry_period}; } bool isValid(bool verbose) const override; @@ -65,7 +72,6 @@ class DepthImageInput : public InputBase { public: DepthImageInput(const DepthImageInputConfig& config, std::shared_ptr pipeline, - std::vector integrator_names, std::shared_ptr transformer, std::string world_frame, ros::NodeHandle nh, ros::NodeHandle nh_private); diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_base.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_base.h index 73d2cd1ef..e07250e49 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_base.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_base.h @@ -7,6 +7,7 @@ #include #include +#include #include #include "wavemap_ros/utils/tf_transformer.h" @@ -20,10 +21,12 @@ struct InputType : public TypeSelector { static constexpr std::array names = {"pointcloud", "depth_image"}; }; -struct InputBaseConfig : public ConfigBase { +struct InputBaseConfig : public ConfigBase { std::string topic_name = "scan"; int topic_queue_length = 10; + StringList measurement_integrator_names; + Seconds processing_retry_period = 0.05f; static MemberMap memberMap; @@ -31,9 +34,11 @@ struct InputBaseConfig : public ConfigBase { // Constructors InputBaseConfig() = default; InputBaseConfig(std::string topic_name, int topic_queue_length, + StringList measurement_integrator_names, FloatingPoint processing_retry_period) : topic_name(std::move(topic_name)), topic_queue_length(topic_queue_length), + measurement_integrator_names(std::move(measurement_integrator_names)), processing_retry_period(processing_retry_period) {} bool isValid(bool verbose) const override; @@ -42,7 +47,6 @@ struct InputBaseConfig : public ConfigBase { class InputBase { public: InputBase(const InputBaseConfig& config, std::shared_ptr pipeline, - std::vector integrator_names, std::shared_ptr transformer, std::string world_frame, const ros::NodeHandle& nh, ros::NodeHandle nh_private); virtual ~InputBase() = default; @@ -54,7 +58,6 @@ class InputBase { const InputBaseConfig config_; std::shared_ptr pipeline_; - const std::vector integrator_names_; const std::shared_ptr transformer_; const std::string world_frame_; diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h index 67b9fbb1c..9c9fb2018 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h @@ -8,6 +8,7 @@ #include #include +#include #include #include "wavemap_ros/inputs/input_base.h" @@ -30,7 +31,8 @@ struct PointcloudTopicType : public TypeSelector { * Config struct for the pointcloud input handler. */ struct PointcloudInputConfig - : public ConfigBase { + : public ConfigBase { //! Name of the ROS topic to subscribe to. std::string topic_name = "scan"; //! Message type of the ROS topic to subscribe to. @@ -38,6 +40,10 @@ struct PointcloudInputConfig //! Queue length to use when subscribing to the ROS topic. int topic_queue_length = 10; + //! Name(s) of the measurement integrator(s) used to integrate the + //! measurements into the map. + StringList measurement_integrator_names; + //! Time period used to control the rate at which to retry getting the sensor //! pose when ROS TF lookups fail. Seconds processing_retry_period = 0.05f; @@ -75,7 +81,8 @@ struct PointcloudInputConfig // Conversion to InputHandler base config operator InputBaseConfig() const { // NOLINT - return {topic_name, topic_queue_length, processing_retry_period}; + return {topic_name, topic_queue_length, measurement_integrator_names, + processing_retry_period}; } bool isValid(bool verbose) const override; @@ -85,7 +92,6 @@ class PointcloudInput : public InputBase { public: PointcloudInput(const PointcloudInputConfig& config, std::shared_ptr pipeline, - std::vector integrator_names, std::shared_ptr transformer, std::string world_frame, ros::NodeHandle nh, ros::NodeHandle nh_private); diff --git a/interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc index 72ed766bb..84ff86e00 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc @@ -14,6 +14,7 @@ namespace wavemap { DECLARE_CONFIG_MEMBERS(DepthImageInputConfig, (topic_name) (topic_queue_length) + (measurement_integrator_names) (processing_retry_period) (max_wait_for_pose) (sensor_frame_id) @@ -35,12 +36,11 @@ bool DepthImageInputConfig::isValid(bool verbose) const { DepthImageInput::DepthImageInput(const DepthImageInputConfig& config, std::shared_ptr pipeline, - std::vector integrator_names, std::shared_ptr transformer, std::string world_frame, ros::NodeHandle nh, ros::NodeHandle nh_private) - : InputBase(config, std::move(pipeline), std::move(integrator_names), - std::move(transformer), std::move(world_frame), nh, nh_private), + : InputBase(config, std::move(pipeline), std::move(transformer), + std::move(world_frame), nh, nh_private), config_(config.checkValid()) { // Subscribe to the depth image input image_transport::ImageTransport it(nh); @@ -107,7 +107,8 @@ void DepthImageInput::processQueue() { << " points. Remaining pointclouds in queue: " << depth_image_queue_.size() - 1 << "."); integration_timer_.start(); - pipeline_->runPipeline(integrator_names_, posed_depth_image); + pipeline_->runPipeline(config_.measurement_integrator_names, + posed_depth_image); integration_timer_.stop(); ROS_DEBUG_STREAM("Integrated new depth image in " << integration_timer_.getLastEpisodeDuration() diff --git a/interfaces/ros1/wavemap_ros/src/inputs/input_base.cc b/interfaces/ros1/wavemap_ros/src/inputs/input_base.cc index 56cee3217..feb843e96 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/input_base.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/input_base.cc @@ -4,6 +4,7 @@ namespace wavemap { DECLARE_CONFIG_MEMBERS(InputBaseConfig, (topic_name) (topic_queue_length) + (measurement_integrator_names) (processing_retry_period)); bool InputBaseConfig::isValid(bool verbose) const { @@ -18,13 +19,11 @@ bool InputBaseConfig::isValid(bool verbose) const { InputBase::InputBase(const InputBaseConfig& config, std::shared_ptr pipeline, - std::vector integrator_names, std::shared_ptr transformer, std::string world_frame, const ros::NodeHandle& nh, ros::NodeHandle /*nh_private*/) : config_(config.checkValid()), pipeline_(std::move(pipeline)), - integrator_names_(std::move(integrator_names)), transformer_(std::move(transformer)), world_frame_(std::move(world_frame)) { // Start the queue processing retry timer diff --git a/interfaces/ros1/wavemap_ros/src/inputs/input_factory.cc b/interfaces/ros1/wavemap_ros/src/inputs/input_factory.cc index ded052c78..2e4452e91 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/input_factory.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/input_factory.cc @@ -28,47 +28,13 @@ std::unique_ptr InputFactory::create( return nullptr; } - // Get the integrator names - std::vector integrator_names; - if (const auto names_param = params.getChild("measurement_integrator"); - names_param) { - if (const auto name_str = names_param->as(); name_str) { - integrator_names.emplace_back(name_str.value()); - } else if (const auto name_arr = names_param->as(); - name_arr) { - size_t name_idx = 0u; - for (const auto& name_el : name_arr.value()) { - if (const auto name_el_str = name_el.as(); name_el_str) { - integrator_names.emplace_back(name_el_str.value()); - } else { - ROS_WARN_STREAM( - "Skipping \"measurement_integrator\" element at index " - << name_idx << ". Could not be parsed as string."); - } - ++name_idx; - } - } else { - ROS_ERROR( - "Param \"measurement_integrator\" should be a string " - "or list of strings."); - } - } else { - ROS_ERROR( - "No integrator name(s) specified. Provide them by setting param " - "\"measurement_integrator\" to a string or list of strings."); - } - if (integrator_names.empty()) { - ROS_ERROR("Could not create input. Returning nullptr."); - return nullptr; - } - // Create the input handler switch (input_type) { case InputType::kPointcloud: if (const auto config = PointcloudInputConfig::from(params); config) { return std::make_unique( - config.value(), std::move(pipeline), std::move(integrator_names), - std::move(transformer), std::move(world_frame), nh, nh_private); + config.value(), std::move(pipeline), std::move(transformer), + std::move(world_frame), nh, nh_private); } else { ROS_ERROR("Pointcloud input handler config could not be loaded."); return nullptr; @@ -76,8 +42,8 @@ std::unique_ptr InputFactory::create( case InputType::kDepthImage: if (const auto config = DepthImageInputConfig::from(params); config) { return std::make_unique( - config.value(), std::move(pipeline), std::move(integrator_names), - std::move(transformer), std::move(world_frame), nh, nh_private); + config.value(), std::move(pipeline), std::move(transformer), + std::move(world_frame), nh, nh_private); } else { ROS_ERROR("Depth image input handler config could not be loaded."); return nullptr; diff --git a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc index 814e4fb9c..709b06bf3 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc @@ -16,6 +16,7 @@ DECLARE_CONFIG_MEMBERS(PointcloudInputConfig, (topic_name) (topic_type) (topic_queue_length) + (measurement_integrator_names) (processing_retry_period) (max_wait_for_pose) (sensor_frame_id) @@ -38,12 +39,11 @@ bool PointcloudInputConfig::isValid(bool verbose) const { PointcloudInput::PointcloudInput(const PointcloudInputConfig& config, std::shared_ptr pipeline, - std::vector integrator_names, std::shared_ptr transformer, std::string world_frame, ros::NodeHandle nh, ros::NodeHandle nh_private) - : InputBase(config, std::move(pipeline), std::move(integrator_names), - transformer, std::move(world_frame), nh, nh_private), + : InputBase(config, std::move(pipeline), transformer, + std::move(world_frame), nh, nh_private), config_(config.checkValid()), pointcloud_undistorter_( transformer, @@ -251,7 +251,8 @@ void PointcloudInput::processQueue() { << " points. Remaining pointclouds in queue: " << pointcloud_queue_.size() - 1 << "."); integration_timer_.start(); - pipeline_->runPipeline(integrator_names_, posed_pointcloud); + pipeline_->runPipeline(config_.measurement_integrator_names, + posed_pointcloud); integration_timer_.stop(); ROS_DEBUG_STREAM("Integrated new pointcloud in " << integration_timer_.getLastEpisodeDuration() diff --git a/library/include/wavemap/core/config/string_list.h b/library/include/wavemap/core/config/string_list.h new file mode 100644 index 000000000..07d4446f6 --- /dev/null +++ b/library/include/wavemap/core/config/string_list.h @@ -0,0 +1,38 @@ +#ifndef WAVEMAP_CORE_CONFIG_STRING_LIST_H_ +#define WAVEMAP_CORE_CONFIG_STRING_LIST_H_ + +#include +#include +#include +#include + +#include "wavemap/core/config/param.h" + +namespace wavemap { +struct StringList { + using ValueType = std::vector; + ValueType value{}; + + // Constructors + StringList() = default; + StringList(ValueType value) : value(std::move(value)) {} // NOLINT + + // Assignment operator + StringList& operator=(ValueType rhs) { + value = std::move(rhs); + return *this; + } + + // Allow implicit conversions to the underlying type + operator ValueType&() { return value; } + operator const ValueType&() const { return value; } + + // Method to load from configs + static std::optional from(const param::Value& param); + + // Method to facilitate printing + std::string toStr() const; +}; +} // namespace wavemap + +#endif // WAVEMAP_CORE_CONFIG_STRING_LIST_H_ diff --git a/library/src/core/CMakeLists.txt b/library/src/core/CMakeLists.txt index 6dd09c792..7798c6e3a 100644 --- a/library/src/core/CMakeLists.txt +++ b/library/src/core/CMakeLists.txt @@ -3,6 +3,7 @@ add_library(wavemap::wavemap_core ALIAS wavemap_core) add_wavemap_include_directories(wavemap_core) target_sources(wavemap_core PRIVATE + config/string_list.cc config/value_with_unit.cc integrator/measurement_model/continuous_beam.cc integrator/measurement_model/constant_ray.cc diff --git a/library/src/core/config/string_list.cc b/library/src/core/config/string_list.cc new file mode 100644 index 000000000..2c05f1fb8 --- /dev/null +++ b/library/src/core/config/string_list.cc @@ -0,0 +1,34 @@ +#include "wavemap/core/config/string_list.h" + +#include "wavemap/core/utils/print/container.h" + +namespace wavemap { +std::optional StringList::from(const param::Value& param) { + ValueType string_list; + + if (const auto single_string = param.as(); single_string) { + string_list.emplace_back(single_string.value()); + } else if (const auto array = param.as(); array) { + size_t idx = 0u; + for (const auto& element : array.value()) { + if (const auto element_string = element.as(); + element_string) { + string_list.emplace_back(element_string.value()); + } else { + LOG(WARNING) << "Skipping element at index " << idx + << ". Could not be parsed as string."; + } + ++idx; + } + } else { + LOG(ERROR) << "Param should be a string or list of strings."; + return std::nullopt; + } + + return string_list; +} + +std::string StringList::toStr() const { + return "[" + print::sequence(value) + "]"; +} +} // namespace wavemap diff --git a/tooling/schemas/wavemap/inputs/depth_image_input.json b/tooling/schemas/wavemap/inputs/depth_image_input.json index 94f81bb26..10b77bbf1 100644 --- a/tooling/schemas/wavemap/inputs/depth_image_input.json +++ b/tooling/schemas/wavemap/inputs/depth_image_input.json @@ -5,7 +5,7 @@ "additionalProperties": false, "required": [ "topic_name", - "measurement_integrator" + "measurement_integrator_names" ], "properties": { "type": { @@ -15,8 +15,8 @@ "description": "Name of the ROS topic to subscribe to.", "type": "string" }, - "measurement_integrator": { - "description": "Name of the measurement integrator(s) used to integrate the measurements into the map.", + "measurement_integrator_names": { + "description": "Name(s) of the measurement integrator(s) used to integrate the measurements into the map.", "oneOf": [ { "type": "string" diff --git a/tooling/schemas/wavemap/inputs/pointcloud_input.json b/tooling/schemas/wavemap/inputs/pointcloud_input.json index 6c40ce5d7..2ff9df4f0 100644 --- a/tooling/schemas/wavemap/inputs/pointcloud_input.json +++ b/tooling/schemas/wavemap/inputs/pointcloud_input.json @@ -6,7 +6,7 @@ "required": [ "topic_name", "topic_type", - "measurement_integrator" + "measurement_integrator_names" ], "properties": { "type": { @@ -25,8 +25,8 @@ "livox" ] }, - "measurement_integrator": { - "description": "Name of the measurement integrator(s) used to integrate the measurements into the map.", + "measurement_integrator_names": { + "description": "Name(s) of the measurement integrator(s) used to integrate the measurements into the map.", "oneOf": [ { "type": "string" From 521f53bf0e40025ddee883f661428a0ab8f46ab2 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 18 Jul 2024 18:21:08 +0200 Subject: [PATCH 104/159] Print clear error msgs when measurement_integrator_names fail to parse --- .../src/inputs/depth_image_input.cc | 1 + .../ros1/wavemap_ros/src/inputs/input_base.cc | 1 + .../src/inputs/pointcloud_input.cc | 1 + .../wavemap/core/config/param_checks.h | 28 ++++++++++++------- .../include/wavemap/core/config/string_list.h | 5 ++++ 5 files changed, 26 insertions(+), 10 deletions(-) diff --git a/interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc index 84ff86e00..b0e49b021 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc @@ -28,6 +28,7 @@ bool DepthImageInputConfig::isValid(bool verbose) const { all_valid &= IS_PARAM_NE(topic_name, std::string(""), verbose); all_valid &= IS_PARAM_GT(topic_queue_length, 0, verbose); + all_valid &= IS_PARAM_FALSE(measurement_integrator_names.empty(), verbose); all_valid &= IS_PARAM_GT(processing_retry_period, 0.f, verbose); all_valid &= IS_PARAM_GE(max_wait_for_pose, 0.f, verbose); diff --git a/interfaces/ros1/wavemap_ros/src/inputs/input_base.cc b/interfaces/ros1/wavemap_ros/src/inputs/input_base.cc index feb843e96..2bee4db81 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/input_base.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/input_base.cc @@ -12,6 +12,7 @@ bool InputBaseConfig::isValid(bool verbose) const { all_valid &= IS_PARAM_NE(topic_name, std::string(""), verbose); all_valid &= IS_PARAM_GT(topic_queue_length, 0, verbose); + all_valid &= IS_PARAM_FALSE(measurement_integrator_names.empty(), verbose); all_valid &= IS_PARAM_GT(processing_retry_period, 0.f, verbose); return all_valid; diff --git a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc index 709b06bf3..afc48af47 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc @@ -31,6 +31,7 @@ bool PointcloudInputConfig::isValid(bool verbose) const { all_valid &= IS_PARAM_NE(topic_name, std::string(""), verbose); all_valid &= IS_PARAM_GT(topic_queue_length, 0, verbose); + all_valid &= IS_PARAM_FALSE(measurement_integrator_names.empty(), verbose); all_valid &= IS_PARAM_GT(processing_retry_period, 0.f, verbose); all_valid &= IS_PARAM_GE(max_wait_for_pose, 0.f, verbose); diff --git a/library/include/wavemap/core/config/param_checks.h b/library/include/wavemap/core/config/param_checks.h index 0bda4584b..0b1ee7a14 100644 --- a/library/include/wavemap/core/config/param_checks.h +++ b/library/include/wavemap/core/config/param_checks.h @@ -7,24 +7,31 @@ #include "wavemap/core/utils/meta/nameof.h" #define IS_PARAM_EQ(value, threshold, verbose) \ - wavemap::is_param>(value, threshold, verbose, #value, "==") + wavemap::is_param>(value, threshold, verbose, #value, " == ") #define IS_PARAM_NE(value, threshold, verbose) \ wavemap::is_param>(value, threshold, verbose, #value, \ - "!=") + " != ") #define IS_PARAM_LT(value, threshold, verbose) \ - wavemap::is_param>(value, threshold, verbose, #value, "<") + wavemap::is_param>(value, threshold, verbose, #value, " < ") -#define IS_PARAM_LE(value, threshold, verbose) \ - wavemap::is_param>(value, threshold, verbose, #value, "<=") +#define IS_PARAM_LE(value, threshold, verbose) \ + wavemap::is_param>(value, threshold, verbose, #value, \ + " <= ") #define IS_PARAM_GT(value, threshold, verbose) \ - wavemap::is_param>(value, threshold, verbose, #value, ">") + wavemap::is_param>(value, threshold, verbose, #value, " > ") #define IS_PARAM_GE(value, threshold, verbose) \ wavemap::is_param>(value, threshold, verbose, #value, \ - ">=") + " >= ") + +#define IS_PARAM_TRUE(value, verbose) \ + wavemap::is_param>(value, true, verbose, #value) + +#define IS_PARAM_FALSE(value, verbose) \ + wavemap::is_param>(value, false, verbose, #value) namespace wavemap { template @@ -34,12 +41,13 @@ bool is_param(A value, B threshold) { template bool is_param(A value, B threshold, bool verbose, const std::string& value_name, - const std::string& comparison_op_name) { + const std::string& comparison_op_string = " ") { if (is_param(value, threshold)) { return true; } else { - LOG_IF(WARNING, verbose) << "Param \"" << value_name << "\" is not " - << comparison_op_name << " " << threshold; + LOG_IF(WARNING, verbose) + << "Param \"" << value_name << "\" is not" << comparison_op_string + << std::boolalpha << threshold; return false; } } diff --git a/library/include/wavemap/core/config/string_list.h b/library/include/wavemap/core/config/string_list.h index 07d4446f6..10e783193 100644 --- a/library/include/wavemap/core/config/string_list.h +++ b/library/include/wavemap/core/config/string_list.h @@ -23,6 +23,11 @@ struct StringList { return *this; } + // Comparison operators + bool empty() const { return value.empty(); } + bool operator==(const StringList& rhs) const { return value == rhs.value; } + bool operator!=(const StringList& rhs) const { return value != rhs.value; } + // Allow implicit conversions to the underlying type operator ValueType&() { return value; } operator const ValueType&() const { return value; } From 3425759b82162f16a197a46c65b417bcc7872bcd Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 18 Jul 2024 18:40:54 +0200 Subject: [PATCH 105/159] Fetch schema standard over HTTPS not HTTP --- tooling/schemas/wavemap/general.json | 2 +- tooling/schemas/wavemap/inputs/depth_image_input.json | 2 +- tooling/schemas/wavemap/inputs/input_base.json | 2 +- tooling/schemas/wavemap/inputs/pointcloud_input.json | 2 +- tooling/schemas/wavemap/map/hashed_blocks.json | 2 +- tooling/schemas/wavemap/map/hashed_chunked_wavelet_octree.json | 2 +- tooling/schemas/wavemap/map/hashed_wavelet_octree.json | 2 +- tooling/schemas/wavemap/map/map_base.json | 2 +- tooling/schemas/wavemap/map/octree.json | 2 +- tooling/schemas/wavemap/map/wavelet_octree.json | 2 +- .../wavemap/map_operations_pipeline/crop_map_operation.json | 2 +- .../wavemap/map_operations_pipeline/map_operation_base.json | 2 +- .../wavemap/map_operations_pipeline/prune_map_operation.json | 2 +- .../wavemap/map_operations_pipeline/publish_map_operation.json | 2 +- .../map_operations_pipeline/publish_pointcloud_operation.json | 2 +- .../map_operations_pipeline/threshold_map_operation.json | 2 +- .../wavemap/measurement_integrators/integration_method.json | 2 +- .../measurement_integrators/measurement_integrator_base.json | 2 +- .../wavemap/measurement_integrators/measurement_model.json | 2 +- .../wavemap/measurement_integrators/projection_model.json | 2 +- .../schemas/wavemap/value_with_unit/convertible_to_meters.json | 2 +- .../schemas/wavemap/value_with_unit/convertible_to_pixels.json | 2 +- .../schemas/wavemap/value_with_unit/convertible_to_radians.json | 2 +- .../schemas/wavemap/value_with_unit/convertible_to_seconds.json | 2 +- tooling/schemas/wavemap/wavemap_config.json | 2 +- 25 files changed, 25 insertions(+), 25 deletions(-) diff --git a/tooling/schemas/wavemap/general.json b/tooling/schemas/wavemap/general.json index 57df38460..d164c0cab 100644 --- a/tooling/schemas/wavemap/general.json +++ b/tooling/schemas/wavemap/general.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "General properties of the wavemap server.", "type": "object", "additionalProperties": false, diff --git a/tooling/schemas/wavemap/inputs/depth_image_input.json b/tooling/schemas/wavemap/inputs/depth_image_input.json index 10b77bbf1..1d588f292 100644 --- a/tooling/schemas/wavemap/inputs/depth_image_input.json +++ b/tooling/schemas/wavemap/inputs/depth_image_input.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "Properties of a single depth image input.", "type": "object", "additionalProperties": false, diff --git a/tooling/schemas/wavemap/inputs/input_base.json b/tooling/schemas/wavemap/inputs/input_base.json index 1f2169f04..8827311cd 100644 --- a/tooling/schemas/wavemap/inputs/input_base.json +++ b/tooling/schemas/wavemap/inputs/input_base.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "Properties of a single measurement input.", "type": "object", "required": [ diff --git a/tooling/schemas/wavemap/inputs/pointcloud_input.json b/tooling/schemas/wavemap/inputs/pointcloud_input.json index 2ff9df4f0..f7e48c9cb 100644 --- a/tooling/schemas/wavemap/inputs/pointcloud_input.json +++ b/tooling/schemas/wavemap/inputs/pointcloud_input.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "Properties of a single pointcloud input.", "type": "object", "additionalProperties": false, diff --git a/tooling/schemas/wavemap/map/hashed_blocks.json b/tooling/schemas/wavemap/map/hashed_blocks.json index 725d3e46a..e6e175cc4 100644 --- a/tooling/schemas/wavemap/map/hashed_blocks.json +++ b/tooling/schemas/wavemap/map/hashed_blocks.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "Properties of the hashed blocks map data structure.", "type": "object", "additionalProperties": false, diff --git a/tooling/schemas/wavemap/map/hashed_chunked_wavelet_octree.json b/tooling/schemas/wavemap/map/hashed_chunked_wavelet_octree.json index 86e465020..ce4565c4e 100644 --- a/tooling/schemas/wavemap/map/hashed_chunked_wavelet_octree.json +++ b/tooling/schemas/wavemap/map/hashed_chunked_wavelet_octree.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "Properties of the hashed chunked wavelet octree map data structure.", "type": "object", "additionalProperties": false, diff --git a/tooling/schemas/wavemap/map/hashed_wavelet_octree.json b/tooling/schemas/wavemap/map/hashed_wavelet_octree.json index d94b8ee52..ba9375834 100644 --- a/tooling/schemas/wavemap/map/hashed_wavelet_octree.json +++ b/tooling/schemas/wavemap/map/hashed_wavelet_octree.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "Properties of the hashed wavelet octree map data structure.", "type": "object", "additionalProperties": false, diff --git a/tooling/schemas/wavemap/map/map_base.json b/tooling/schemas/wavemap/map/map_base.json index 4895a3baa..754438bdb 100644 --- a/tooling/schemas/wavemap/map/map_base.json +++ b/tooling/schemas/wavemap/map/map_base.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "Properties of the data structure used to store the map.", "type": "object", "required": [ diff --git a/tooling/schemas/wavemap/map/octree.json b/tooling/schemas/wavemap/map/octree.json index 69eceff43..299c18dfe 100644 --- a/tooling/schemas/wavemap/map/octree.json +++ b/tooling/schemas/wavemap/map/octree.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "Properties of the octree map data structure.", "type": "object", "additionalProperties": false, diff --git a/tooling/schemas/wavemap/map/wavelet_octree.json b/tooling/schemas/wavemap/map/wavelet_octree.json index 85466c234..5ffafe6fc 100644 --- a/tooling/schemas/wavemap/map/wavelet_octree.json +++ b/tooling/schemas/wavemap/map/wavelet_octree.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "Properties of the wavelet octree map data structure.", "type": "object", "additionalProperties": false, diff --git a/tooling/schemas/wavemap/map_operations_pipeline/crop_map_operation.json b/tooling/schemas/wavemap/map_operations_pipeline/crop_map_operation.json index 427259030..d155cc872 100644 --- a/tooling/schemas/wavemap/map_operations_pipeline/crop_map_operation.json +++ b/tooling/schemas/wavemap/map_operations_pipeline/crop_map_operation.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "Properties of a single map cropping operation.", "type": "object", "additionalProperties": false, diff --git a/tooling/schemas/wavemap/map_operations_pipeline/map_operation_base.json b/tooling/schemas/wavemap/map_operations_pipeline/map_operation_base.json index ca8a6ef20..3d573e55a 100644 --- a/tooling/schemas/wavemap/map_operations_pipeline/map_operation_base.json +++ b/tooling/schemas/wavemap/map_operations_pipeline/map_operation_base.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "Properties of a single operation to perform after map updates.", "type": "object", "required": [ diff --git a/tooling/schemas/wavemap/map_operations_pipeline/prune_map_operation.json b/tooling/schemas/wavemap/map_operations_pipeline/prune_map_operation.json index 97b863c8f..5018cfba3 100644 --- a/tooling/schemas/wavemap/map_operations_pipeline/prune_map_operation.json +++ b/tooling/schemas/wavemap/map_operations_pipeline/prune_map_operation.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "Properties of a single map pruning operation.", "type": "object", "additionalProperties": false, diff --git a/tooling/schemas/wavemap/map_operations_pipeline/publish_map_operation.json b/tooling/schemas/wavemap/map_operations_pipeline/publish_map_operation.json index 25fc73c88..d9b3e71d0 100644 --- a/tooling/schemas/wavemap/map_operations_pipeline/publish_map_operation.json +++ b/tooling/schemas/wavemap/map_operations_pipeline/publish_map_operation.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "Properties of a single map publishing operation.", "type": "object", "additionalProperties": false, diff --git a/tooling/schemas/wavemap/map_operations_pipeline/publish_pointcloud_operation.json b/tooling/schemas/wavemap/map_operations_pipeline/publish_pointcloud_operation.json index 8acad43ad..5a13b001d 100644 --- a/tooling/schemas/wavemap/map_operations_pipeline/publish_pointcloud_operation.json +++ b/tooling/schemas/wavemap/map_operations_pipeline/publish_pointcloud_operation.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "Properties of a single obstacle pointcloud publishing operation.", "type": "object", "additionalProperties": false, diff --git a/tooling/schemas/wavemap/map_operations_pipeline/threshold_map_operation.json b/tooling/schemas/wavemap/map_operations_pipeline/threshold_map_operation.json index bc93bc11c..7897026ae 100644 --- a/tooling/schemas/wavemap/map_operations_pipeline/threshold_map_operation.json +++ b/tooling/schemas/wavemap/map_operations_pipeline/threshold_map_operation.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "Properties of a single map thresholding operation.", "type": "object", "additionalProperties": false, diff --git a/tooling/schemas/wavemap/measurement_integrators/integration_method.json b/tooling/schemas/wavemap/measurement_integrators/integration_method.json index 57940f8f5..8b522f020 100644 --- a/tooling/schemas/wavemap/measurement_integrators/integration_method.json +++ b/tooling/schemas/wavemap/measurement_integrators/integration_method.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "Method used to integrate the measurements into the map.", "type": "object", "$defs": { diff --git a/tooling/schemas/wavemap/measurement_integrators/measurement_integrator_base.json b/tooling/schemas/wavemap/measurement_integrators/measurement_integrator_base.json index e0585f4a1..af082f78a 100644 --- a/tooling/schemas/wavemap/measurement_integrators/measurement_integrator_base.json +++ b/tooling/schemas/wavemap/measurement_integrators/measurement_integrator_base.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "Properties of a single measurement integrator.", "type": "object", "additionalProperties": false, diff --git a/tooling/schemas/wavemap/measurement_integrators/measurement_model.json b/tooling/schemas/wavemap/measurement_integrators/measurement_model.json index 9649c2775..15078c295 100644 --- a/tooling/schemas/wavemap/measurement_integrators/measurement_model.json +++ b/tooling/schemas/wavemap/measurement_integrators/measurement_model.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "Inverse measurement model used to convert measurements into corresponding map updates.", "type": "object", "$defs": { diff --git a/tooling/schemas/wavemap/measurement_integrators/projection_model.json b/tooling/schemas/wavemap/measurement_integrators/projection_model.json index e4fcd61bb..0f727a631 100644 --- a/tooling/schemas/wavemap/measurement_integrators/projection_model.json +++ b/tooling/schemas/wavemap/measurement_integrators/projection_model.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "description": "Projection model used to convert sensor coordinates to/from cartesian coordinates.", "type": "object", "$defs": { diff --git a/tooling/schemas/wavemap/value_with_unit/convertible_to_meters.json b/tooling/schemas/wavemap/value_with_unit/convertible_to_meters.json index 04077b7eb..6328f534c 100644 --- a/tooling/schemas/wavemap/value_with_unit/convertible_to_meters.json +++ b/tooling/schemas/wavemap/value_with_unit/convertible_to_meters.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "type": "object", "additionalProperties": false, "minProperties": 1, diff --git a/tooling/schemas/wavemap/value_with_unit/convertible_to_pixels.json b/tooling/schemas/wavemap/value_with_unit/convertible_to_pixels.json index 8e715099a..fcd3f46ba 100644 --- a/tooling/schemas/wavemap/value_with_unit/convertible_to_pixels.json +++ b/tooling/schemas/wavemap/value_with_unit/convertible_to_pixels.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "type": "object", "additionalProperties": false, "minProperties": 1, diff --git a/tooling/schemas/wavemap/value_with_unit/convertible_to_radians.json b/tooling/schemas/wavemap/value_with_unit/convertible_to_radians.json index 0b39efce7..8700e5659 100644 --- a/tooling/schemas/wavemap/value_with_unit/convertible_to_radians.json +++ b/tooling/schemas/wavemap/value_with_unit/convertible_to_radians.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "type": "object", "additionalProperties": false, "minProperties": 1, diff --git a/tooling/schemas/wavemap/value_with_unit/convertible_to_seconds.json b/tooling/schemas/wavemap/value_with_unit/convertible_to_seconds.json index 01f835a3c..69bb9bb3f 100644 --- a/tooling/schemas/wavemap/value_with_unit/convertible_to_seconds.json +++ b/tooling/schemas/wavemap/value_with_unit/convertible_to_seconds.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "type": "object", "additionalProperties": false, "minProperties": 1, diff --git a/tooling/schemas/wavemap/wavemap_config.json b/tooling/schemas/wavemap/wavemap_config.json index d0c88dc1b..d8d33dad3 100644 --- a/tooling/schemas/wavemap/wavemap_config.json +++ b/tooling/schemas/wavemap/wavemap_config.json @@ -1,5 +1,5 @@ { - "$schema": "http://json-schema.org/draft-07/schema#", + "$schema": "https://json-schema.org/draft-07/schema", "title": "Wavemap config", "description": "Schema to provide code completion and validation for wavemap configuration files.", "type": "object", From 85c7ac57784ccbfb53e20e1f3b26f5d64098df27 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 19 Jul 2024 15:19:50 +0200 Subject: [PATCH 106/159] Make wavemap catkin installable --- interfaces/ros1/wavemap/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interfaces/ros1/wavemap/CMakeLists.txt b/interfaces/ros1/wavemap/CMakeLists.txt index ebe08d393..ebb4a59d8 100644 --- a/interfaces/ros1/wavemap/CMakeLists.txt +++ b/interfaces/ros1/wavemap/CMakeLists.txt @@ -21,6 +21,6 @@ install(TARGETS wavemap_core wavemap_io wavemap_pipeline RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) # Mark cpp header files for installation -install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/include +install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/include/wavemap/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h") From 912cb6680a872ba117012b87debfd79835c5b093 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 19 Jul 2024 16:55:07 +0200 Subject: [PATCH 107/159] Make 'catkin test wavemap' run the library's tests --- interfaces/ros1/wavemap/CMakeLists.txt | 5 +++++ .../wavemap/cmake/append_to_catkin_tests.cmake | 17 +++++++++++++++++ 2 files changed, 22 insertions(+) create mode 100644 interfaces/ros1/wavemap/cmake/append_to_catkin_tests.cmake diff --git a/interfaces/ros1/wavemap/CMakeLists.txt b/interfaces/ros1/wavemap/CMakeLists.txt index ebb4a59d8..ae605781a 100644 --- a/interfaces/ros1/wavemap/CMakeLists.txt +++ b/interfaces/ros1/wavemap/CMakeLists.txt @@ -14,6 +14,11 @@ catkin_package( add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../../../library ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}) +# Include the library's tests in catkin's run_tests target +include(cmake/append_to_catkin_tests.cmake) +append_to_catkin_tests(test_wavemap_io) +append_to_catkin_tests(test_wavemap_core) + # Install targets install(TARGETS wavemap_core wavemap_io wavemap_pipeline ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/interfaces/ros1/wavemap/cmake/append_to_catkin_tests.cmake b/interfaces/ros1/wavemap/cmake/append_to_catkin_tests.cmake new file mode 100644 index 000000000..e4c3ad049 --- /dev/null +++ b/interfaces/ros1/wavemap/cmake/append_to_catkin_tests.cmake @@ -0,0 +1,17 @@ +# A small helper method to append tests defined in a pure CMake project to +# catkin's run_tests target, s.t. they're included when running: +# catkin test +function(append_to_catkin_tests target) + if (TARGET ${target}) + # Make sure the target is built before running tests + add_dependencies(tests ${target}) + + # Add it to catkin's run_tests target + get_target_property(_target_path ${target} RUNTIME_OUTPUT_DIRECTORY) + # cmake-lint: disable=C0301 + set(cmd "${_target_path}/${target} --gtest_output=xml:${CATKIN_TEST_RESULTS_DIR}/${PROJECT_NAME}/gtest-${target}.xml") + catkin_run_tests_target("gtest" ${target} "gtest-${target}.xml" + COMMAND ${cmd} + DEPENDENCIES ${target}) + endif () +endfunction() From b52b6820dee7cee7672f9c50eeec09ef583c213c Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 24 Jul 2024 13:21:43 +0200 Subject: [PATCH 108/159] Remove minkindr install dep as it is no longer needed --- tooling/vcstool/wavemap_https.yml | 8 -------- tooling/vcstool/wavemap_ssh.yml | 8 -------- 2 files changed, 16 deletions(-) diff --git a/tooling/vcstool/wavemap_https.yml b/tooling/vcstool/wavemap_https.yml index a35d3b789..fd2152547 100644 --- a/tooling/vcstool/wavemap_https.yml +++ b/tooling/vcstool/wavemap_https.yml @@ -3,11 +3,3 @@ repositories: type: git url: https://github.com/catkin/catkin_simple.git version: master - tracy_catkin: - type: git - url: https://github.com/ethz-asl/tracy_catkin.git - version: main - minkindr: - type: git - url: https://github.com/ethz-asl/minkindr.git - version: master diff --git a/tooling/vcstool/wavemap_ssh.yml b/tooling/vcstool/wavemap_ssh.yml index 74f09fd7c..bc13032bc 100644 --- a/tooling/vcstool/wavemap_ssh.yml +++ b/tooling/vcstool/wavemap_ssh.yml @@ -3,11 +3,3 @@ repositories: type: git url: git@github.com:catkin/catkin_simple.git version: master - tracy_catkin: - type: git - url: git@github.com:ethz-asl/tracy_catkin.git - version: main - minkindr: - type: git - url: git@github.com:ethz-asl/minkindr.git - version: master From 24d8a723b6a1601da569688e23395d635970c90e Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 24 Jul 2024 13:50:12 +0200 Subject: [PATCH 109/159] Remove Docker stage to build from-source deps as no longer needed --- tooling/docker/full.Dockerfile | 34 ++++------------------------------ 1 file changed, 4 insertions(+), 30 deletions(-) diff --git a/tooling/docker/full.Dockerfile b/tooling/docker/full.Dockerfile index 3708289ae..171b70794 100644 --- a/tooling/docker/full.Dockerfile +++ b/tooling/docker/full.Dockerfile @@ -76,33 +76,7 @@ ARG CCACHE_DIR ENV PATH="/usr/lib/ccache:${PATH}" CCACHE_DIR=$CCACHE_DIR -FROM system-deps-installer AS workspace-deps-builder - -# Load the dependencies source code and our package's manifest -# to resolve which dependencies should be built -ARG CATKIN_WS_PATH -ARG REPOSITORY_NAME -WORKDIR $CATKIN_WS_PATH -COPY --from=cacher /tmp/filtered_sources/src/dependencies src/dependencies -COPY --from=cacher /tmp/manifests/src/$REPOSITORY_NAME src/$REPOSITORY_NAME - -# Pull in ccache's cache -ARG CCACHE_DIR -COPY ccache $CCACHE_DIR - -# Setup the catkin workspace and build the dependencies -SHELL ["/bin/bash", "-o", "pipefail", "-c"] -# hadolint ignore=SC2046 -RUN . /opt/ros/noetic/setup.sh && \ - catkin init && \ - catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release && \ - dependencies=$(catkin list --deps --directory src/$REPOSITORY_NAME -u | grep -oP '(?<= - ).*?(?=$)' | grep -v $REPOSITORY_NAME | sort -u) && \ - catkin_packages=$(catkin list -u | sort -u) && \ - dependencies_to_catkin_build=$(comm -12 <(echo "$dependencies") <(echo "$catkin_packages")) && \ - catkin build --no-status --force-color $(echo "$dependencies_to_catkin_build") - - -FROM workspace-deps-builder AS workspace-full-builder +FROM system-deps-installer AS workspace-full-builder # Load package source code ARG CATKIN_WS_PATH @@ -152,7 +126,7 @@ FROM workspace-underlay AS workspace-built-deps # Pull in the compiled workspace ARG CATKIN_WS_PATH WORKDIR $CATKIN_WS_PATH -COPY --from=workspace-deps-builder $CATKIN_WS_PATH . +COPY --from=system-deps-installer $CATKIN_WS_PATH . FROM workspace-underlay AS workspace-built-full @@ -163,11 +137,11 @@ WORKDIR $CATKIN_WS_PATH COPY --from=workspace-full-builder $CATKIN_WS_PATH . -FROM scratch AS workspace-deps-builder-ccache-extractor +FROM scratch AS system-deps-installer-ccache-extractor ARG CCACHE_DIR WORKDIR / -COPY --from=workspace-deps-builder $CCACHE_DIR . +COPY --from=system-deps-installer $CCACHE_DIR . FROM scratch AS workspace-full-builder-ccache-extractor From b29c0aa1251c54bdb897229c7f4a567d81c94819 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 24 Jul 2024 14:41:41 +0200 Subject: [PATCH 110/159] Build wavemap_msgs without catkin_simple --- interfaces/ros1/wavemap_msgs/CMakeLists.txt | 28 +++++++++++++++++---- interfaces/ros1/wavemap_msgs/package.xml | 1 - 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/interfaces/ros1/wavemap_msgs/CMakeLists.txt b/interfaces/ros1/wavemap_msgs/CMakeLists.txt index 4874e8f80..58dba387c 100644 --- a/interfaces/ros1/wavemap_msgs/CMakeLists.txt +++ b/interfaces/ros1/wavemap_msgs/CMakeLists.txt @@ -1,9 +1,27 @@ cmake_minimum_required(VERSION 3.0.2) project(wavemap_msgs) -find_package(catkin_simple REQUIRED) -catkin_simple(ALL_DEPS_REQUIRED) +# Load catkin and required dependencies to generate msgs +find_package(catkin REQUIRED COMPONENTS message_generation std_msgs) -# Export -cs_install() -cs_export() +# Declare message files to be built +add_message_files(FILES + HashedBlock.msg + HashedBlocks.msg + HashedWaveletOctree.msg + HashedWaveletOctreeBlock.msg + Index3D.msg + Map.msg + OctreeNode.msg + WaveletOctree.msg + WaveletOctreeNode.msg) + +# Declare service files to be built +add_service_files(FILES + FilePath.srv) + +# Generate the language-specific message and service files +generate_messages(DEPENDENCIES std_msgs) + +# Finish registering wavemap_msgs as a catkin pacakge +catkin_package(CATKIN_DEPENDS message_runtime std_msgs) diff --git a/interfaces/ros1/wavemap_msgs/package.xml b/interfaces/ros1/wavemap_msgs/package.xml index a6bd3aef7..c3e90e5a4 100644 --- a/interfaces/ros1/wavemap_msgs/package.xml +++ b/interfaces/ros1/wavemap_msgs/package.xml @@ -11,7 +11,6 @@ Victor Reijgwart catkin - catkin_simple message_generation message_runtime From 7cf16b683a70bcd0afea8c3c791766fcd7bd4d7b Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 24 Jul 2024 14:54:39 +0200 Subject: [PATCH 111/159] Simplify Docker and CI scripts now that we no longer build deps from src --- .github/workflows/cd.yml | 56 +------------------------ .github/workflows/ci.yml | 53 +---------------------- tooling/docker/full.Dockerfile | 77 +++++++++++----------------------- 3 files changed, 27 insertions(+), 159 deletions(-) diff --git a/.github/workflows/cd.yml b/.github/workflows/cd.yml index 6ac58d775..8c3c3482d 100644 --- a/.github/workflows/cd.yml +++ b/.github/workflows/cd.yml @@ -15,7 +15,7 @@ env: DOCKER_TARGET_ROS_PKG: wavemap_all DOCKER_CI_REGISTRY: hub.wavemap.vwire.ch DOCKER_RELEASE_REGISTRY: ghcr.io - DOCKER_RELEASE_TARGET: workspace-built-deps + DOCKER_RELEASE_TARGET: workspace ROS_HOME: /home/ci/.ros CATKIN_WS_PATH: /home/ci/catkin_ws CCACHE_DIR: /home/ci/ccache @@ -74,30 +74,6 @@ jobs: - name: Set up Docker Buildx uses: docker/setup-buildx-action@v2 - - name: Prepare ccache to accelerate the build - id: get-date - # NOTE: Installing tar is required for actions/cache@v3 to work properly - # on docker:20.10.9-dind. - run: | - apk add --no-cache tar - mkdir -p ccache - echo "date=$(date -u "+%Y-%m-%d_%H-%M-%S")" >> $GITHUB_OUTPUT - - - name: Pull in ccache's cache - uses: actions/cache@v3 - with: - path: ccache - key: ccache-${{ secrets.CCACHE_CACHE_VERSION }}-noetic-gcc-docker-${{ github.sha }}-${{ steps.get-date.outputs.date }} - restore-keys: | - ccache-${{ secrets.CCACHE_CACHE_VERSION }}-noetic-gcc-docker-${{ github.sha }}- - ccache-${{ secrets.CCACHE_CACHE_VERSION }}-noetic-gcc-docker- - - - name: Start measuring the build time - id: start-time - run: | - stamp=$(date +%s) - echo "stamp=${stamp}" >> $GITHUB_OUTPUT - - name: Build the ${{ env.DOCKER_RELEASE_TARGET }} image uses: docker/build-push-action@v3 with: @@ -116,18 +92,6 @@ jobs: cache-to: ${{ env.CACHE_IMAGE_NAME }},mode=max tags: ${{ env.LOCAL_IMAGE_NAME }} - - name: Determine whether the ccache cache should be updated - id: should-writeback-ccache - run: | - runtime=$(( $(date +%s) - ${{ steps.start-time.outputs.stamp }} )) - threshold=300 # seconds - echo $runtime - answer=false - if [ $threshold -le $runtime ]; then - answer=true - fi - echo "answer=${answer}" >> $GITHUB_OUTPUT - - name: Test the ${{ env.DOCKER_RELEASE_TARGET }} image run: docker run --rm ${{ env.LOCAL_IMAGE_NAME }} @@ -148,24 +112,6 @@ jobs: cache-from: ${{ env.CACHE_IMAGE_NAME }} tags: ${{ env.LOCAL_IMAGE_NAME }} - - name: Write back the build's ccache cache - if: steps.should-writeback-ccache.outputs.answer == 'true' - uses: docker/build-push-action@v3 - with: - context: . - file: ${{ env.REPOSITORY_NAME }}/tooling/docker/full.Dockerfile - target: workspace-deps-builder-ccache-extractor - build-args: | - FROM_IMAGE=${{ env.DOCKER_FROM_IMAGE }} - CATKIN_WS_PATH=${{ env.CATKIN_WS_PATH }} - CCACHE_DIR=${{ env.CCACHE_DIR }} - ROS_HOME=${{ env.ROS_HOME }} - REPOSITORY_NAME=${{ env.REPOSITORY_NAME }} - PACKAGE_NAME=${{ env.DOCKER_TARGET_ROS_PKG }} - cache-from: ${{ env.CACHE_IMAGE_NAME }} - tags: ${{ env.LOCAL_IMAGE_NAME }} - outputs: type=local,dest=ccache - - name: Extract metadata to annotate the image id: meta uses: docker/metadata-action@9ec57ed1fcdbf14dcef7dfbe97b2010124a938b7 diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 0803bb268..c71835e10 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -12,7 +12,7 @@ env: DOCKER_FROM_IMAGE: ros:noetic-ros-base-focal DOCKER_TARGET_ROS_PKG: wavemap_all DOCKER_CI_REGISTRY: hub.wavemap.vwire.ch - DOCKER_CI_TARGET: workspace-built-deps + DOCKER_CI_TARGET: workspace ROS_HOME: /home/ci/.ros CATKIN_WS_PATH: /home/ci/catkin_ws CCACHE_DIR: /home/ci/ccache @@ -58,27 +58,6 @@ jobs: # on docker:20.10.9-dind. run: apk add --no-cache tar git - - name: Prepare ccache to accelerate the build - id: get-date - run: | - mkdir -p ccache - echo "date=$(date -u "+%Y-%m-%d_%H-%M-%S")" >> $GITHUB_OUTPUT - - - name: Pull in ccache's cache - uses: actions/cache@v3 - with: - path: ccache - key: ccache-${{ secrets.CCACHE_CACHE_VERSION }}-noetic-gcc-docker-${{ github.sha }}-${{ steps.get-date.outputs.date }} - restore-keys: | - ccache-${{ secrets.CCACHE_CACHE_VERSION }}-noetic-gcc-docker-${{ github.sha }}- - ccache-${{ secrets.CCACHE_CACHE_VERSION }}-noetic-gcc-docker- - - - name: Start measuring the build time - id: start-time - run: | - stamp=$(date +%s) - echo "stamp=${stamp}" >> $GITHUB_OUTPUT - - name: Build the ${{ env.DOCKER_CI_TARGET }} image uses: docker/build-push-action@v3 with: @@ -97,18 +76,6 @@ jobs: cache-to: ${{ env.CACHE_IMAGE_NAME }},mode=max tags: ${{ env.LOCAL_IMAGE_NAME }} - - name: Determine whether the ccache cache should be updated - id: should-writeback-ccache - run: | - runtime=$(( $(date +%s) - ${{ steps.start-time.outputs.stamp }} )) - threshold=300 # seconds - echo $runtime - answer=false - if [ $threshold -le $runtime ]; then - answer=true - fi - echo "answer=${answer}" >> $GITHUB_OUTPUT - - name: Test the ${{ env.DOCKER_CI_TARGET }} image run: docker run --rm ${{ env.LOCAL_IMAGE_NAME }} @@ -129,24 +96,6 @@ jobs: cache-from: ${{ env.CACHE_IMAGE_NAME }} tags: ${{ env.LOCAL_IMAGE_NAME }} - - name: Write back the build's ccache cache - if: steps.should-writeback-ccache.outputs.answer == 'true' - uses: docker/build-push-action@v3 - with: - context: . - file: ${{ env.REPOSITORY_NAME }}/tooling/docker/full.Dockerfile - target: workspace-deps-builder-ccache-extractor - build-args: | - FROM_IMAGE=${{ env.DOCKER_FROM_IMAGE }} - CATKIN_WS_PATH=${{ env.CATKIN_WS_PATH }} - CCACHE_DIR=${{ env.CCACHE_DIR }} - ROS_HOME=${{ env.ROS_HOME }} - REPOSITORY_NAME=${{ env.REPOSITORY_NAME }} - PACKAGE_NAME=${{ env.DOCKER_TARGET_ROS_PKG }} - cache-from: ${{ env.CACHE_IMAGE_NAME }} - tags: ${{ env.LOCAL_IMAGE_NAME }} - outputs: type=local,dest=ccache - lint: name: Lint needs: [ common-variables ] diff --git a/tooling/docker/full.Dockerfile b/tooling/docker/full.Dockerfile index 171b70794..4fbe048da 100644 --- a/tooling/docker/full.Dockerfile +++ b/tooling/docker/full.Dockerfile @@ -6,7 +6,7 @@ ARG REPOSITORY_NAME ARG PACKAGE_NAME # hadolint ignore=DL3006 -FROM $FROM_IMAGE AS cacher +FROM $FROM_IMAGE AS source-filter # Install vcstool ARG DEBIAN_FRONTEND=noninteractive @@ -49,12 +49,12 @@ RUN mkdir -p /tmp/filtered_sources && \ # hadolint ignore=DL3006 -FROM $FROM_IMAGE AS system-deps-installer +FROM $FROM_IMAGE AS dependency-installer -# Load the cached manifests +# Load the catkin package manifest files ARG CATKIN_WS_PATH WORKDIR $CATKIN_WS_PATH -COPY --from=cacher /tmp/manifests . +COPY --from=source-filter /tmp/manifests . # Install general and ROS-related system dependencies # NOTE: Manually installing opencv_viz is a temporary workaround to satisfy @@ -76,76 +76,49 @@ ARG CCACHE_DIR ENV PATH="/usr/lib/ccache:${PATH}" CCACHE_DIR=$CCACHE_DIR -FROM system-deps-installer AS workspace-full-builder - -# Load package source code -ARG CATKIN_WS_PATH -ARG REPOSITORY_NAME -ARG PACKAGE_NAME -WORKDIR $CATKIN_WS_PATH -COPY --from=cacher $CATKIN_WS_PATH/src/$REPOSITORY_NAME src/$REPOSITORY_NAME - -# Build the package -RUN . /opt/ros/noetic/setup.sh && \ - catkin build --no-status --force-color $PACKAGE_NAME - - -FROM system-deps-installer AS workspace-underlay - -# Update the entrypoint to source the workspace -# NOTE: The devel/setup.bash will only be pulled in in a subsequent stage, so -# images built only up to the current stage (i.e. with -# --target=workspace-underlay) cannot yet successfully be booted. -# hadolint ignore=SC2086 -RUN sed --in-place \ - 's|^source .*|source "'$CATKIN_WS_PATH'/devel/setup.bash"|' \ - /ros_entrypoint.sh && \ - echo "source '$CATKIN_WS_PATH'/devel/setup.bash" >> ~/.bashrc - -# Load the workspace sources -COPY --from=cacher $CATKIN_WS_PATH/src src +FROM dependency-installer AS workspace - -FROM workspace-underlay AS workspace +# Load the catkin workspace's source files +COPY --from=source-filter $CATKIN_WS_PATH/src src # Configure and bootstrap catkin -# NOTE: We build an (arbitrary) small package to create +# NOTE: We build a small dummy package to create # catkin_ws/devel/setup.bash, such that we can directly # source our workspace in the container entrypoint script -ARG CATKIN_WS_PATH -WORKDIR $CATKIN_WS_PATH RUN . /opt/ros/noetic/setup.sh && \ catkin init && \ catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release && \ catkin build catkin_setup --no-status && \ ccache --clear +# Update the entrypoint to source the workspace +# hadolint ignore=SC2086 +RUN sed --in-place \ + 's|^source .*|source "'$CATKIN_WS_PATH'/devel/setup.bash"|' \ + /ros_entrypoint.sh && \ + echo "source '$CATKIN_WS_PATH'/devel/setup.bash" >> ~/.bashrc + -FROM workspace-underlay AS workspace-built-deps +FROM workspace AS workspace-builder -# Pull in the compiled workspace +# Build our package ARG CATKIN_WS_PATH +ARG PACKAGE_NAME WORKDIR $CATKIN_WS_PATH -COPY --from=system-deps-installer $CATKIN_WS_PATH . +RUN catkin build --no-status --force-color $PACKAGE_NAME -FROM workspace-underlay AS workspace-built-full +FROM workspace AS workspace-built -# Pull in the compiled workspace +# Pull in the compiled catkin workspace (but without ccache files etc) ARG CATKIN_WS_PATH WORKDIR $CATKIN_WS_PATH -COPY --from=workspace-full-builder $CATKIN_WS_PATH . - - -FROM scratch AS system-deps-installer-ccache-extractor - -ARG CCACHE_DIR -WORKDIR / -COPY --from=system-deps-installer $CCACHE_DIR . +COPY --from=workspace-builder $CATKIN_WS_PATH . -FROM scratch AS workspace-full-builder-ccache-extractor +FROM scratch AS workspace-builder-ccache-extractor +# Extract the ccache cache directory from the workspace-builder stage ARG CCACHE_DIR WORKDIR / -COPY --from=workspace-full-builder $CCACHE_DIR . +COPY --from=workspace-builder $CCACHE_DIR . From f59e376bd512d5f06496a66cdd44e5703548b44c Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 24 Jul 2024 15:12:38 +0200 Subject: [PATCH 112/159] Build wavemap_ros_conversions without catkin_simple --- .../wavemap_ros_conversions/CMakeLists.txt | 20 ++++++++++++------- .../ros1/wavemap_ros_conversions/package.xml | 1 - 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt index 2ad1b2829..af388a5e2 100644 --- a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt @@ -1,15 +1,25 @@ cmake_minimum_required(VERSION 3.0.2) project(wavemap_ros_conversions) -find_package(catkin_simple REQUIRED) -catkin_simple(ALL_DEPS_REQUIRED) +# Find dependencies +find_package(catkin REQUIRED + COMPONENTS roscpp eigen_conversions wavemap wavemap_msgs) + +# Register catkin package +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS roscpp eigen_conversions wavemap wavemap_msgs) # Libraries -cs_add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} src/config_conversions.cc src/map_msg_conversions.cc src/time_conversions.cc) set_wavemap_target_properties(${PROJECT_NAME}) +target_include_directories(${PROJECT_NAME} + PUBLIC include ${catkin_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES}) # Tests if (CATKIN_ENABLE_TESTING) @@ -22,7 +32,3 @@ if (CATKIN_ENABLE_TESTING) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} glog GTest::gtest_main) endif () - -# Export -cs_install() -cs_export() diff --git a/interfaces/ros1/wavemap_ros_conversions/package.xml b/interfaces/ros1/wavemap_ros_conversions/package.xml index 0e168a32d..34d3180b0 100644 --- a/interfaces/ros1/wavemap_ros_conversions/package.xml +++ b/interfaces/ros1/wavemap_ros_conversions/package.xml @@ -11,7 +11,6 @@ Victor Reijgwart catkin - catkin_simple wavemap wavemap_msgs From f94c97f4f086bb5b785380d7aa4f3563b59446e8 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 24 Jul 2024 15:25:50 +0200 Subject: [PATCH 113/159] Mark wavemap_ros_conversions library and headers for installation --- .../ros1/wavemap_ros_conversions/CMakeLists.txt | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt index af388a5e2..f433f7680 100644 --- a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt @@ -32,3 +32,14 @@ if (CATKIN_ENABLE_TESTING) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} glog GTest::gtest_main) endif () + +# Install targets +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +# Mark cpp header files for installation +install(DIRECTORY include + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h") From 65e867fe47ba6a1825971d58b616c700a727b2d3 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 24 Jul 2024 15:51:48 +0200 Subject: [PATCH 114/159] Reintroduce clang-tidy CI integration --- interfaces/ros1/wavemap_ros/CMakeLists.txt | 3 +++ .../ros1/wavemap_ros_conversions/CMakeLists.txt | 3 +++ library/CMakeLists.txt | 11 +++-------- library/cmake/wavemap-extras.cmake | 9 +++++++++ 4 files changed, 18 insertions(+), 8 deletions(-) diff --git a/interfaces/ros1/wavemap_ros/CMakeLists.txt b/interfaces/ros1/wavemap_ros/CMakeLists.txt index dfbb57646..fd0f7bbb7 100644 --- a/interfaces/ros1/wavemap_ros/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros/CMakeLists.txt @@ -14,6 +14,9 @@ if (livox_ros_driver2_FOUND) add_compile_definitions(LIVOX_AVAILABLE) endif () +# Enable general wavemap tooling (e.g. to run clang-tidy CI) +enable_wavemap_general_tooling() + # Libraries cs_add_library(${PROJECT_NAME} src/inputs/depth_image_input.cc diff --git a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt index f433f7680..e98a44bbd 100644 --- a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt @@ -11,6 +11,9 @@ catkin_package( LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS roscpp eigen_conversions wavemap wavemap_msgs) +# Enable general wavemap tooling (e.g. to run clang-tidy CI) +enable_wavemap_general_tooling() + # Libraries add_library(${PROJECT_NAME} src/config_conversions.cc diff --git a/library/CMakeLists.txt b/library/CMakeLists.txt index f4cde52c2..6ac83f6b0 100644 --- a/library/CMakeLists.txt +++ b/library/CMakeLists.txt @@ -1,12 +1,12 @@ cmake_minimum_required(VERSION 3.10) project(wavemap VERSION 1.6.3 LANGUAGES CXX) -# User options -option(USE_CLANG_TIDY "Generate necessary files to run clang-tidy" OFF) - # CMake helpers include(cmake/wavemap-extras.cmake) +# Enable general wavemap tooling (e.g. to run clang-tidy CI) +enable_wavemap_general_tooling() + # Libraries add_subdirectory(src) @@ -19,8 +19,3 @@ endif () if (ENABLE_BENCHMARKING) add_subdirectory(benchmark) endif () - -# Tooling -if (USE_CLANG_TIDY) - set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -endif () diff --git a/library/cmake/wavemap-extras.cmake b/library/cmake/wavemap-extras.cmake index 49705e5cd..ecd761aa7 100644 --- a/library/cmake/wavemap-extras.cmake +++ b/library/cmake/wavemap-extras.cmake @@ -6,6 +6,15 @@ option(USE_ASAN "Compile with address sanitizer enabled" OFF) option(USE_TSAN "Compile with thread sanitizer enabled" OFF) option(ENABLE_COVERAGE_TESTING "Compile with necessary flags for coverage testing" OFF) +option(USE_CLANG_TIDY "Generate necessary files to run clang-tidy" OFF) + +# Enable general wavemap tooling for the calling CMake project +function(enable_wavemap_general_tooling) + # Export compilation database for compatibility with clang-tidy + if (USE_CLANG_TIDY) + set(CMAKE_EXPORT_COMPILE_COMMANDS ON PARENT_SCOPE) + endif () +endfunction() # Adds the include paths of the wavemap library to the given target. function(add_wavemap_include_directories target) From 9cb149d07993324abb26837a620bf343ff48e12e Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 24 Jul 2024 16:07:19 +0200 Subject: [PATCH 115/159] Update doc generation scripts --- docs/conf.py | 2 +- docs/pages/intro.rst | 16 ++++++++-------- .../wavemap_utils/scripts/preview_docs.sh | 2 +- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/docs/conf.py b/docs/conf.py index 158e4c319..4da1a1a04 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -10,7 +10,7 @@ copyright = 'Victor Reijgwart, ASL ETHZ.' # pylint: disable=redefined-builtin # The full version, including alpha/beta/rc tags -release = lxml.etree.parse('../libraries/wavemap/package.xml').find( +release = lxml.etree.parse('../interfaces/ros1/wavemap/package.xml').find( 'version').text # The short X.Y version version = release diff --git a/docs/pages/intro.rst b/docs/pages/intro.rst index 0c84d3289..19f56cde6 100644 --- a/docs/pages/intro.rst +++ b/docs/pages/intro.rst @@ -61,20 +61,20 @@ For other citation styles, you can use the `Crosscite's citation formatter Date: Wed, 24 Jul 2024 16:18:02 +0200 Subject: [PATCH 116/159] Fix catkin install errors found in CI --- interfaces/ros1/wavemap_ros/CMakeLists.txt | 2 +- .../ros1/wavemap_ros_conversions/CMakeLists.txt | 2 +- interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt | 14 ++++---------- 3 files changed, 6 insertions(+), 12 deletions(-) diff --git a/interfaces/ros1/wavemap_ros/CMakeLists.txt b/interfaces/ros1/wavemap_ros/CMakeLists.txt index fd0f7bbb7..0d6ae71f4 100644 --- a/interfaces/ros1/wavemap_ros/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros/CMakeLists.txt @@ -48,5 +48,5 @@ cs_install() cs_export() # Export config files -install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/config/ +install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) diff --git a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt index e98a44bbd..ad6b047c2 100644 --- a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt @@ -43,6 +43,6 @@ install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) # Mark cpp header files for installation -install(DIRECTORY include +install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h") diff --git a/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt b/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt index 86663de79..9035a3312 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt +++ b/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt @@ -6,12 +6,8 @@ cmake_minimum_required(VERSION 3.0.2) project(wavemap_rviz_plugin) # Dependencies -find_package(catkin - REQUIRED COMPONENTS - rviz - wavemap - wavemap_msgs - wavemap_ros_conversions) +find_package(catkin REQUIRED + COMPONENTS rviz wavemap wavemap_msgs wavemap_ros_conversions) # Setup catkin package catkin_package(CATKIN_DEPENDS @@ -70,14 +66,12 @@ target_compile_definitions(${PROJECT_NAME} PUBLIC -DQT_NO_KEYWORDS) target_compile_options(${PROJECT_NAME} PRIVATE -Wno-pedantic -Wno-register) # Install -install(TARGETS - ${PROJECT_NAME} +install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -install(FILES - plugin_description.xml +install(FILES plugin_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) install(DIRECTORY icons/ From b56f0899c8bd8c4ef346f30c362faa5dce6abe4d Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 24 Jul 2024 16:26:36 +0200 Subject: [PATCH 117/159] Revert doc page paths due to issues with PDF generation --- docs/pages/intro.rst | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/docs/pages/intro.rst b/docs/pages/intro.rst index 19f56cde6..0c84d3289 100644 --- a/docs/pages/intro.rst +++ b/docs/pages/intro.rst @@ -61,20 +61,20 @@ For other citation styles, you can use the `Crosscite's citation formatter Date: Wed, 24 Jul 2024 16:34:52 +0200 Subject: [PATCH 118/159] Bump GitHub actions versions to address deprecation warnings --- .github/actions/setup-ccache/action.yml | 2 +- .github/workflows/cd.yml | 8 +++---- .github/workflows/ci.yml | 30 ++++++++++++------------ .github/workflows/release_management.yml | 6 ++--- 4 files changed, 23 insertions(+), 23 deletions(-) diff --git a/.github/actions/setup-ccache/action.yml b/.github/actions/setup-ccache/action.yml index 8c0d54535..1234c1222 100644 --- a/.github/actions/setup-ccache/action.yml +++ b/.github/actions/setup-ccache/action.yml @@ -17,7 +17,7 @@ runs: run: echo "date=$(date -u "+%Y-%m-%d_%H-%M-%S")" >> $GITHUB_OUTPUT - name: Setup ccache cache sharing - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ${{ env.CCACHE_DIR }} key: ccache-${{ inputs.cache-version }}-${{ inputs.cache-group }}-${{ github.sha }}-${{ steps.get-date.outputs.date }} diff --git a/.github/workflows/cd.yml b/.github/workflows/cd.yml index 8c3c3482d..948397686 100644 --- a/.github/workflows/cd.yml +++ b/.github/workflows/cd.yml @@ -39,7 +39,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Checkout code - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Create Release id: create_release uses: softprops/action-gh-release@v1 @@ -60,7 +60,7 @@ jobs: LOCAL_IMAGE_NAME: ${{ needs.common-variables.outputs.local_ci_image_name }} steps: - name: Fetch the package's repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: path: ${{ env.REPOSITORY_NAME }} @@ -72,7 +72,7 @@ jobs: password: ${{ secrets.GITHUB_TOKEN }} - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v2 + uses: docker/setup-buildx-action@v3 - name: Build the ${{ env.DOCKER_RELEASE_TARGET }} image uses: docker/build-push-action@v3 @@ -145,7 +145,7 @@ jobs: image: ${{ needs.build-image.outputs.image }} steps: - name: Fetch the package's repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Install dependencies (doxygen+sphinx+breathe+exhale toolchain) run: | diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index c71835e10..4b24f5f5c 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -46,18 +46,18 @@ jobs: LOCAL_IMAGE_NAME: ${{ needs.common-variables.outputs.local_ci_image_name }} steps: - name: Fetch the package's repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: path: ${{ env.REPOSITORY_NAME }} - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v2 - - name: Install dependencies - # NOTE: Installing tar is required for actions/cache@v3 to work properly + # NOTE: Installing tar is required for actions/cache@v4 to work properly # on docker:20.10.9-dind. run: apk add --no-cache tar git + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 + - name: Build the ${{ env.DOCKER_CI_TARGET }} image uses: docker/build-push-action@v3 with: @@ -120,7 +120,7 @@ jobs: chmod +x /bin/hadolint - name: Fetch the package's repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 # NOTE: This has to be done after installing pre-commit, s.t. the # pre-commit hooks are automatically initialized. @@ -128,7 +128,7 @@ jobs: run: echo "PRE_COMMIT_PYTHON_VERSION=$(python -VV | sha256sum | cut -d' ' -f1)" >> $GITHUB_ENV - name: Setup pre-commit cache sharing - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ${{ env.PRE_COMMIT_DIR }} key: pre-commit|${{ env.PRE_COMMIT_PYTHON_VERSION }}|${{ hashFiles('.pre-commit-config.yaml') }} @@ -149,7 +149,7 @@ jobs: image: ${{ needs.workspace-container.outputs.image }} steps: - name: Fetch the package's repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Install dependencies (doxygen+sphinx+breathe+exhale toolchain) run: | @@ -203,7 +203,7 @@ jobs: image: ${{ needs.workspace-container.outputs.image }} steps: - name: Fetch the package's repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 # NOTE: Even though the repo is already present in the container, we # also need to check it out at GitHub Actions' preferred location # for private actions and problem matchers to work. @@ -233,7 +233,7 @@ jobs: image: ${{ needs.workspace-container.outputs.image }} steps: - name: Fetch the package's repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 # NOTE: Even though the repo is already present in the container, we # also need to check it out at GitHub Actions' preferred location # for private actions and problem matchers to work. @@ -271,7 +271,7 @@ jobs: image: ${{ needs.workspace-container.outputs.image }} steps: - name: Fetch the package's repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 # NOTE: Even though the repo is already present in the container, we # also need to check it out at GitHub Actions' preferred location # for private actions and problem matchers to work. @@ -314,7 +314,7 @@ jobs: image: ${{ needs.workspace-container.outputs.image }} steps: - name: Fetch the package's repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 # NOTE: Even though the repo is already present in the container, we # also need to check it out at GitHub Actions' preferred location # for private actions and problem matchers to work. @@ -365,7 +365,7 @@ jobs: image: ${{ needs.workspace-container.outputs.image }} steps: - name: Fetch the package's repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 # NOTE: Even though the repo is already present in the container, we # also need to check it out at GitHub Actions' preferred location # for private actions and problem matchers to work. @@ -467,7 +467,7 @@ jobs: fail-fast: false steps: - name: Fetch the package's repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 # NOTE: Even though the repo is already present in the container, we # also need to check it out at GitHub Actions' preferred location # for private actions and problem matchers to work. @@ -520,7 +520,7 @@ jobs: image: ${{ needs.workspace-container.outputs.image }} steps: - name: Fetch the package's repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 # NOTE: Even though the repo is already present in the container, we # also need to check it out at GitHub Actions' preferred location # for private actions and problem matchers to work. diff --git a/.github/workflows/release_management.yml b/.github/workflows/release_management.yml index 7d9224f5e..7f831a195 100644 --- a/.github/workflows/release_management.yml +++ b/.github/workflows/release_management.yml @@ -67,7 +67,7 @@ jobs: apt-get update apt-get install -q -y --no-install-recommends git python3-catkin-tools gh - name: Fetch the repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 # To everything, instead of only the last commit token: ${{ secrets.PAT }} @@ -103,7 +103,7 @@ jobs: continue-on-error: true steps: - name: Checkout the current branch - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Compare local and main version numbers shell: bash run: | @@ -122,7 +122,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Checkout the current branch - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 token: ${{ secrets.PAT }} From 3c53eed05a6f4a61e351410dcb6361d4998fee6c Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 25 Jul 2024 16:07:43 +0200 Subject: [PATCH 119/159] Shorten map_operations_pipeline param name to map_operations --- .../ros1/wavemap_ros/config/wavemap_livox_mid360.yaml | 2 +- .../config/wavemap_livox_mid360_azure_kinect.yaml | 2 +- .../wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml | 2 +- .../config/wavemap_livox_mid360_pico_monstar.yaml | 2 +- interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml | 2 +- .../wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml | 2 +- interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml | 2 +- .../wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml | 2 +- interfaces/ros1/wavemap_ros/src/ros_server.cc | 2 +- .../crop_map_operation.json | 0 .../map_operation_base.json | 0 .../prune_map_operation.json | 0 .../publish_map_operation.json | 0 .../publish_pointcloud_operation.json | 0 .../threshold_map_operation.json | 0 tooling/schemas/wavemap/wavemap_config.json | 6 +++--- 16 files changed, 12 insertions(+), 12 deletions(-) rename tooling/schemas/wavemap/{map_operations_pipeline => map_operations}/crop_map_operation.json (100%) rename tooling/schemas/wavemap/{map_operations_pipeline => map_operations}/map_operation_base.json (100%) rename tooling/schemas/wavemap/{map_operations_pipeline => map_operations}/prune_map_operation.json (100%) rename tooling/schemas/wavemap/{map_operations_pipeline => map_operations}/publish_map_operation.json (100%) rename tooling/schemas/wavemap/{map_operations_pipeline => map_operations}/publish_pointcloud_operation.json (100%) rename tooling/schemas/wavemap/{map_operations_pipeline => map_operations}/threshold_map_operation.json (100%) diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml index 1ef763153..a1637fee4 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml @@ -7,7 +7,7 @@ map: type: hashed_chunked_wavelet_octree min_cell_width: { meters: 0.1 } -map_operations_pipeline: +map_operations: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml index d03a2c467..89c79d6fc 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml @@ -7,7 +7,7 @@ map: type: hashed_chunked_wavelet_octree min_cell_width: { meters: 0.01 } -map_operations_pipeline: +map_operations: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml index 21e9af7e4..e299eda33 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml @@ -8,7 +8,7 @@ map: min_cell_width: { meters: 0.01 } tree_height: 9 -map_operations_pipeline: +map_operations: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml index 29e7c1593..299228e0e 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml @@ -8,7 +8,7 @@ map: min_cell_width: { meters: 0.01 } tree_height: 9 -map_operations_pipeline: +map_operations: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml index e306dc729..429e460ed 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml @@ -7,7 +7,7 @@ map: type: hashed_chunked_wavelet_octree min_cell_width: { meters: 0.1 } -map_operations_pipeline: +map_operations: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml index 1785276c7..ea0289620 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml @@ -8,7 +8,7 @@ map: min_cell_width: { meters: 0.01 } tree_height: 9 -map_operations_pipeline: +map_operations: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml index 80c9f2580..a6b95ad07 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml @@ -7,7 +7,7 @@ map: type: wavelet_octree min_cell_width: { meters: 0.2 } -map_operations_pipeline: +map_operations: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml index c887afa7a..6a19837cc 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml @@ -7,7 +7,7 @@ map: type: hashed_chunked_wavelet_octree min_cell_width: { meters: 0.02 } -map_operations_pipeline: +map_operations: - type: threshold_map once_every: { seconds: 2.0 } - type: prune_map diff --git a/interfaces/ros1/wavemap_ros/src/ros_server.cc b/interfaces/ros1/wavemap_ros/src/ros_server.cc index 2c11141e7..68142ae3c 100644 --- a/interfaces/ros1/wavemap_ros/src/ros_server.cc +++ b/interfaces/ros1/wavemap_ros/src/ros_server.cc @@ -64,7 +64,7 @@ RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, // Add map operations to pipeline const param::Array map_operation_param_array = - param::convert::toParamArray(nh_private, "map_operations_pipeline"); + param::convert::toParamArray(nh_private, "map_operations"); for (const auto& operation_params : map_operation_param_array) { addOperation(operation_params, nh_private); } diff --git a/tooling/schemas/wavemap/map_operations_pipeline/crop_map_operation.json b/tooling/schemas/wavemap/map_operations/crop_map_operation.json similarity index 100% rename from tooling/schemas/wavemap/map_operations_pipeline/crop_map_operation.json rename to tooling/schemas/wavemap/map_operations/crop_map_operation.json diff --git a/tooling/schemas/wavemap/map_operations_pipeline/map_operation_base.json b/tooling/schemas/wavemap/map_operations/map_operation_base.json similarity index 100% rename from tooling/schemas/wavemap/map_operations_pipeline/map_operation_base.json rename to tooling/schemas/wavemap/map_operations/map_operation_base.json diff --git a/tooling/schemas/wavemap/map_operations_pipeline/prune_map_operation.json b/tooling/schemas/wavemap/map_operations/prune_map_operation.json similarity index 100% rename from tooling/schemas/wavemap/map_operations_pipeline/prune_map_operation.json rename to tooling/schemas/wavemap/map_operations/prune_map_operation.json diff --git a/tooling/schemas/wavemap/map_operations_pipeline/publish_map_operation.json b/tooling/schemas/wavemap/map_operations/publish_map_operation.json similarity index 100% rename from tooling/schemas/wavemap/map_operations_pipeline/publish_map_operation.json rename to tooling/schemas/wavemap/map_operations/publish_map_operation.json diff --git a/tooling/schemas/wavemap/map_operations_pipeline/publish_pointcloud_operation.json b/tooling/schemas/wavemap/map_operations/publish_pointcloud_operation.json similarity index 100% rename from tooling/schemas/wavemap/map_operations_pipeline/publish_pointcloud_operation.json rename to tooling/schemas/wavemap/map_operations/publish_pointcloud_operation.json diff --git a/tooling/schemas/wavemap/map_operations_pipeline/threshold_map_operation.json b/tooling/schemas/wavemap/map_operations/threshold_map_operation.json similarity index 100% rename from tooling/schemas/wavemap/map_operations_pipeline/threshold_map_operation.json rename to tooling/schemas/wavemap/map_operations/threshold_map_operation.json diff --git a/tooling/schemas/wavemap/wavemap_config.json b/tooling/schemas/wavemap/wavemap_config.json index d8d33dad3..05f52b5d5 100644 --- a/tooling/schemas/wavemap/wavemap_config.json +++ b/tooling/schemas/wavemap/wavemap_config.json @@ -6,7 +6,7 @@ "required": [ "general", "map", - "map_operations_pipeline", + "map_operations", "measurement_integrators", "inputs" ], @@ -17,11 +17,11 @@ "map": { "$ref": "map/map_base.json" }, - "map_operations_pipeline": { + "map_operations": { "description": "A list of operations to perform after map updates.", "type": "array", "items": { - "$ref": "map_operations_pipeline/map_operation_base.json" + "$ref": "map_operations/map_operation_base.json" } }, "measurement_integrators": { From b87384f1d5c85df530d6797e29509771e2935be3 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 25 Jul 2024 17:15:25 +0200 Subject: [PATCH 120/159] Fix GCC hanging when compiling with UBSAN enabled --- library/include/wavemap/core/data_structure/dense_grid.h | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/library/include/wavemap/core/data_structure/dense_grid.h b/library/include/wavemap/core/data_structure/dense_grid.h index 6c134c094..343f6722d 100644 --- a/library/include/wavemap/core/data_structure/dense_grid.h +++ b/library/include/wavemap/core/data_structure/dense_grid.h @@ -24,9 +24,7 @@ class DenseGrid { using DataArrayType = std::array; explicit DenseGrid(const CellDataT& default_value) { - if (default_value != CellDataT{}) { - data_.fill(default_value); - } + data_.fill(default_value); } CellDataT& operator[](size_t linear_index) { @@ -55,7 +53,7 @@ class DenseGrid { const DataArrayType& data() const { return data_; } private: - DataArrayType data_{}; + DataArrayType data_; }; } // namespace wavemap From 77313b4a36c05c6cf53e0933af2a3cd62129b013 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 25 Jul 2024 17:18:28 +0200 Subject: [PATCH 121/159] Bump docker/build-push-action from v3 to v6 --- .github/workflows/cd.yml | 6 +++--- .github/workflows/ci.yml | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/workflows/cd.yml b/.github/workflows/cd.yml index 948397686..ed0d9864f 100644 --- a/.github/workflows/cd.yml +++ b/.github/workflows/cd.yml @@ -75,7 +75,7 @@ jobs: uses: docker/setup-buildx-action@v3 - name: Build the ${{ env.DOCKER_RELEASE_TARGET }} image - uses: docker/build-push-action@v3 + uses: docker/build-push-action@v6 with: context: . file: ${{ env.REPOSITORY_NAME }}/tooling/docker/full.Dockerfile @@ -96,7 +96,7 @@ jobs: run: docker run --rm ${{ env.LOCAL_IMAGE_NAME }} - name: Push the ${{ env.DOCKER_RELEASE_TARGET }} image locally - uses: docker/build-push-action@v3 + uses: docker/build-push-action@v6 with: context: . file: ${{ env.REPOSITORY_NAME }}/tooling/docker/full.Dockerfile @@ -120,7 +120,7 @@ jobs: - name: Publish the ${{ env.DOCKER_RELEASE_TARGET }} image if: startsWith(github.event.ref, 'refs/tags/v') - uses: docker/build-push-action@v3 + uses: docker/build-push-action@v6 with: context: . file: ${{ env.REPOSITORY_NAME }}/tooling/docker/full.Dockerfile diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 4b24f5f5c..be6534f9d 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -59,7 +59,7 @@ jobs: uses: docker/setup-buildx-action@v3 - name: Build the ${{ env.DOCKER_CI_TARGET }} image - uses: docker/build-push-action@v3 + uses: docker/build-push-action@v6 with: context: . file: ${{ env.REPOSITORY_NAME }}/tooling/docker/full.Dockerfile @@ -80,7 +80,7 @@ jobs: run: docker run --rm ${{ env.LOCAL_IMAGE_NAME }} - name: Push the ${{ env.DOCKER_CI_TARGET }} image - uses: docker/build-push-action@v3 + uses: docker/build-push-action@v6 with: context: . file: ${{ env.REPOSITORY_NAME }}/tooling/docker/full.Dockerfile From e02a642105a44432cc956b5caa6acaf0e7f59c10 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 26 Jul 2024 14:57:38 +0200 Subject: [PATCH 122/159] Restructure examples into generic cpp and ros1-specific examples --- CMakeLists.txt | 5 +- examples/CMakeLists.txt | 65 ------------------- examples/cpp/CMakeLists.txt | 11 ++++ examples/{src => cpp}/CPPLINT.cfg | 0 .../wavemap_examples => cpp}/common.h | 6 +- examples/cpp/io/CMakeLists.txt | 10 +++ .../{src => cpp}/io/load_map_from_file.cc | 0 examples/{src => cpp}/io/save_map_to_file.cc | 0 examples/cpp/planning/CMakeLists.txt | 5 ++ .../planning/occupancy_to_esdf.cc | 0 examples/cpp/queries/CMakeLists.txt | 26 ++++++++ .../queries/accelerated_queries.cc | 2 +- .../{src => cpp}/queries/classification.cc | 2 +- .../{src => cpp}/queries/fixed_resolution.cc | 2 +- .../{src => cpp}/queries/multi_resolution.cc | 2 +- .../queries/nearest_neighbor_interpolation.cc | 2 +- .../queries/trilinear_interpolation.cc | 2 +- examples/include/CPPLINT.cfg | 1 - examples/{ => ros1}/CHANGELOG.rst | 0 examples/ros1/CMakeLists.txt | 14 ++++ examples/ros1/CPPLINT.cfg | 1 + examples/ros1/io/CMakeLists.txt | 10 +++ .../{src => ros1}/io/receive_map_over_ros.cc | 0 .../{src => ros1}/io/send_map_over_ros.cc | 0 examples/{ => ros1}/package.xml | 6 +- interfaces/ros1/wavemap/CMakeLists.txt | 3 +- .../ros1}/wavemap_all/CHANGELOG.rst | 0 .../ros1}/wavemap_all/CMakeLists.txt | 0 .../ros1}/wavemap_all/package.xml | 2 +- library/src/io/CMakeLists.txt | 2 +- library/src/pipeline/CMakeLists.txt | 2 +- 31 files changed, 96 insertions(+), 85 deletions(-) delete mode 100644 examples/CMakeLists.txt create mode 100644 examples/cpp/CMakeLists.txt rename examples/{src => cpp}/CPPLINT.cfg (100%) rename examples/{include/wavemap_examples => cpp}/common.h (76%) create mode 100644 examples/cpp/io/CMakeLists.txt rename examples/{src => cpp}/io/load_map_from_file.cc (100%) rename examples/{src => cpp}/io/save_map_to_file.cc (100%) create mode 100644 examples/cpp/planning/CMakeLists.txt rename examples/{src => cpp}/planning/occupancy_to_esdf.cc (100%) create mode 100644 examples/cpp/queries/CMakeLists.txt rename examples/{src => cpp}/queries/accelerated_queries.cc (95%) rename examples/{src => cpp}/queries/classification.cc (98%) rename examples/{src => cpp}/queries/fixed_resolution.cc (94%) rename examples/{src => cpp}/queries/multi_resolution.cc (96%) rename examples/{src => cpp}/queries/nearest_neighbor_interpolation.cc (95%) rename examples/{src => cpp}/queries/trilinear_interpolation.cc (94%) delete mode 100644 examples/include/CPPLINT.cfg rename examples/{ => ros1}/CHANGELOG.rst (100%) create mode 100644 examples/ros1/CMakeLists.txt create mode 100644 examples/ros1/CPPLINT.cfg create mode 100644 examples/ros1/io/CMakeLists.txt rename examples/{src => ros1}/io/receive_map_over_ros.cc (100%) rename examples/{src => ros1}/io/send_map_over_ros.cc (100%) rename examples/{ => ros1}/package.xml (75%) rename {tooling/packages => interfaces/ros1}/wavemap_all/CHANGELOG.rst (100%) rename {tooling/packages => interfaces/ros1}/wavemap_all/CMakeLists.txt (100%) rename {tooling/packages => interfaces/ros1}/wavemap_all/package.xml (93%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 99288abe2..e98c840c3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,8 +22,8 @@ if ("$ENV{ROS_VERSION}" STREQUAL "1") add_subdirectory(interfaces/ros1/wavemap_ros) add_subdirectory(interfaces/ros1/wavemap_rviz_plugin) - ## Usage examples - add_subdirectory(examples) + # Usage examples + add_subdirectory(examples/ros1) elseif ("$ENV{ROS_VERSION}" STREQUAL "2") # Load as a ROS2 project @@ -36,5 +36,6 @@ else () # NOTE: In this mode, introspection is available # only for the wavemap library. add_subdirectory(library) + add_subdirectory(examples/cpp) endif () diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt deleted file mode 100644 index 3c803d4a0..000000000 --- a/examples/CMakeLists.txt +++ /dev/null @@ -1,65 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(wavemap_examples) - -find_package(catkin_simple REQUIRED) -catkin_simple(ALL_DEPS_REQUIRED) - -# For all targets -include_directories(include) - -# Binaries -cs_add_executable(save_map_to_file - src/io/save_map_to_file.cc) -set_wavemap_target_properties(save_map_to_file) -target_link_libraries(save_map_to_file glog) - -cs_add_executable(load_map_from_file - src/io/load_map_from_file.cc) -set_wavemap_target_properties(load_map_from_file) -target_link_libraries(load_map_from_file glog) - -cs_add_executable(receive_map_over_ros - src/io/receive_map_over_ros.cc) -set_wavemap_target_properties(receive_map_over_ros) -target_link_libraries(receive_map_over_ros glog) - -cs_add_executable(send_map_over_ros - src/io/send_map_over_ros.cc) -set_wavemap_target_properties(send_map_over_ros) -target_link_libraries(send_map_over_ros glog) - -cs_add_executable(fixed_resolution - src/queries/fixed_resolution.cc) -set_wavemap_target_properties(fixed_resolution) -target_link_libraries(fixed_resolution glog) - -cs_add_executable(multi_resolution - src/queries/multi_resolution.cc) -set_wavemap_target_properties(multi_resolution) -target_link_libraries(multi_resolution glog) - -cs_add_executable(accelerated_queries - src/queries/accelerated_queries.cc) -set_wavemap_target_properties(accelerated_queries) -target_link_libraries(accelerated_queries glog) - -cs_add_executable(nearest_neighbor_interpolation - src/queries/nearest_neighbor_interpolation.cc) -set_wavemap_target_properties(nearest_neighbor_interpolation) -target_link_libraries(nearest_neighbor_interpolation glog) - -cs_add_executable(trilinear_interpolation - src/queries/trilinear_interpolation.cc) -set_wavemap_target_properties(trilinear_interpolation) -target_link_libraries(trilinear_interpolation glog) - -cs_add_executable(classification - src/queries/classification.cc) -set_wavemap_target_properties(classification) -target_link_libraries(classification glog) -target_compile_options(classification PRIVATE -Wno-suggest-attribute=const) - -cs_add_executable(occupancy_to_esdf - src/planning/occupancy_to_esdf.cc) -set_wavemap_target_properties(occupancy_to_esdf) -target_link_libraries(occupancy_to_esdf glog) diff --git a/examples/cpp/CMakeLists.txt b/examples/cpp/CMakeLists.txt new file mode 100644 index 000000000..f45a0c3b3 --- /dev/null +++ b/examples/cpp/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.0.2) +project(wavemap_examples_cpp) + +# Load the library +add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../../library + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}) + +# Add each set of examples +add_subdirectory(io) +add_subdirectory(queries) +add_subdirectory(planning) diff --git a/examples/src/CPPLINT.cfg b/examples/cpp/CPPLINT.cfg similarity index 100% rename from examples/src/CPPLINT.cfg rename to examples/cpp/CPPLINT.cfg diff --git a/examples/include/wavemap_examples/common.h b/examples/cpp/common.h similarity index 76% rename from examples/include/wavemap_examples/common.h rename to examples/cpp/common.h index 59e6351ee..1b4aa5e78 100644 --- a/examples/include/wavemap_examples/common.h +++ b/examples/cpp/common.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_EXAMPLES_COMMON_H_ -#define WAVEMAP_EXAMPLES_COMMON_H_ +#ifndef EXAMPLES_CPP_COMMON_H_ +#define EXAMPLES_CPP_COMMON_H_ namespace wavemap::examples { /** @@ -11,4 +11,4 @@ template void doSomething([[maybe_unused]] T... t) {} } // namespace wavemap::examples -#endif // WAVEMAP_EXAMPLES_COMMON_H_ +#endif // EXAMPLES_CPP_COMMON_H_ diff --git a/examples/cpp/io/CMakeLists.txt b/examples/cpp/io/CMakeLists.txt new file mode 100644 index 000000000..2537509b7 --- /dev/null +++ b/examples/cpp/io/CMakeLists.txt @@ -0,0 +1,10 @@ +# Binaries +add_executable(save_map_to_file save_map_to_file.cc) +set_wavemap_target_properties(save_map_to_file) +target_link_libraries(save_map_to_file PUBLIC + wavemap::wavemap_core wavemap::wavemap_io) + +add_executable(load_map_from_file load_map_from_file.cc) +set_wavemap_target_properties(load_map_from_file) +target_link_libraries(load_map_from_file PUBLIC + wavemap::wavemap_core wavemap::wavemap_io) diff --git a/examples/src/io/load_map_from_file.cc b/examples/cpp/io/load_map_from_file.cc similarity index 100% rename from examples/src/io/load_map_from_file.cc rename to examples/cpp/io/load_map_from_file.cc diff --git a/examples/src/io/save_map_to_file.cc b/examples/cpp/io/save_map_to_file.cc similarity index 100% rename from examples/src/io/save_map_to_file.cc rename to examples/cpp/io/save_map_to_file.cc diff --git a/examples/cpp/planning/CMakeLists.txt b/examples/cpp/planning/CMakeLists.txt new file mode 100644 index 000000000..13cd14524 --- /dev/null +++ b/examples/cpp/planning/CMakeLists.txt @@ -0,0 +1,5 @@ +# Binaries +add_executable(occupancy_to_esdf occupancy_to_esdf.cc) +set_wavemap_target_properties(occupancy_to_esdf) +target_link_libraries(occupancy_to_esdf PUBLIC + wavemap::wavemap_core wavemap::wavemap_io) diff --git a/examples/src/planning/occupancy_to_esdf.cc b/examples/cpp/planning/occupancy_to_esdf.cc similarity index 100% rename from examples/src/planning/occupancy_to_esdf.cc rename to examples/cpp/planning/occupancy_to_esdf.cc diff --git a/examples/cpp/queries/CMakeLists.txt b/examples/cpp/queries/CMakeLists.txt new file mode 100644 index 000000000..1aaea9444 --- /dev/null +++ b/examples/cpp/queries/CMakeLists.txt @@ -0,0 +1,26 @@ +# Binaries +add_executable(fixed_resolution fixed_resolution.cc) +set_wavemap_target_properties(fixed_resolution) +target_link_libraries(fixed_resolution PUBLIC wavemap::wavemap_core) + +add_executable(multi_resolution multi_resolution.cc) +set_wavemap_target_properties(multi_resolution) +target_link_libraries(multi_resolution PUBLIC wavemap::wavemap_core) + +add_executable(accelerated_queries accelerated_queries.cc) +set_wavemap_target_properties(accelerated_queries) +target_link_libraries(accelerated_queries PUBLIC wavemap::wavemap_core) + +add_executable(nearest_neighbor_interpolation nearest_neighbor_interpolation.cc) +set_wavemap_target_properties(nearest_neighbor_interpolation) +target_link_libraries(nearest_neighbor_interpolation PUBLIC + wavemap::wavemap_core) + +add_executable(trilinear_interpolation trilinear_interpolation.cc) +set_wavemap_target_properties(trilinear_interpolation) +target_link_libraries(trilinear_interpolation PUBLIC wavemap::wavemap_core) + +add_executable(classification classification.cc) +set_wavemap_target_properties(classification) +target_link_libraries(classification PUBLIC wavemap::wavemap_core) +target_compile_options(classification PRIVATE -Wno-suggest-attribute=const) diff --git a/examples/src/queries/accelerated_queries.cc b/examples/cpp/queries/accelerated_queries.cc similarity index 95% rename from examples/src/queries/accelerated_queries.cc rename to examples/cpp/queries/accelerated_queries.cc index 67870918d..af754eb19 100644 --- a/examples/src/queries/accelerated_queries.cc +++ b/examples/cpp/queries/accelerated_queries.cc @@ -2,7 +2,7 @@ #include #include -#include "wavemap_examples/common.h" +#include "../common.h" using namespace wavemap; int main(int, char**) { diff --git a/examples/src/queries/classification.cc b/examples/cpp/queries/classification.cc similarity index 98% rename from examples/src/queries/classification.cc rename to examples/cpp/queries/classification.cc index 6b3d941c4..6bd37e504 100644 --- a/examples/src/queries/classification.cc +++ b/examples/cpp/queries/classification.cc @@ -1,6 +1,6 @@ #include -#include "wavemap_examples/common.h" +#include "../common.h" using namespace wavemap; int main(int, char**) { diff --git a/examples/src/queries/fixed_resolution.cc b/examples/cpp/queries/fixed_resolution.cc similarity index 94% rename from examples/src/queries/fixed_resolution.cc rename to examples/cpp/queries/fixed_resolution.cc index ce0bc72d9..36e208aed 100644 --- a/examples/src/queries/fixed_resolution.cc +++ b/examples/cpp/queries/fixed_resolution.cc @@ -1,6 +1,6 @@ #include -#include "wavemap_examples/common.h" +#include "../common.h" using namespace wavemap; int main(int, char**) { diff --git a/examples/src/queries/multi_resolution.cc b/examples/cpp/queries/multi_resolution.cc similarity index 96% rename from examples/src/queries/multi_resolution.cc rename to examples/cpp/queries/multi_resolution.cc index 92aebeb26..405eb66ef 100644 --- a/examples/src/queries/multi_resolution.cc +++ b/examples/cpp/queries/multi_resolution.cc @@ -1,7 +1,7 @@ #include #include -#include "wavemap_examples/common.h" +#include "../common.h" using namespace wavemap; int main(int, char**) { diff --git a/examples/src/queries/nearest_neighbor_interpolation.cc b/examples/cpp/queries/nearest_neighbor_interpolation.cc similarity index 95% rename from examples/src/queries/nearest_neighbor_interpolation.cc rename to examples/cpp/queries/nearest_neighbor_interpolation.cc index de976965c..b1df52609 100644 --- a/examples/src/queries/nearest_neighbor_interpolation.cc +++ b/examples/cpp/queries/nearest_neighbor_interpolation.cc @@ -1,7 +1,7 @@ #include #include -#include "wavemap_examples/common.h" +#include "../common.h" using namespace wavemap; int main(int, char**) { diff --git a/examples/src/queries/trilinear_interpolation.cc b/examples/cpp/queries/trilinear_interpolation.cc similarity index 94% rename from examples/src/queries/trilinear_interpolation.cc rename to examples/cpp/queries/trilinear_interpolation.cc index 15a5fca46..53632270d 100644 --- a/examples/src/queries/trilinear_interpolation.cc +++ b/examples/cpp/queries/trilinear_interpolation.cc @@ -1,7 +1,7 @@ #include #include -#include "wavemap_examples/common.h" +#include "../common.h" using namespace wavemap; int main(int, char**) { diff --git a/examples/include/CPPLINT.cfg b/examples/include/CPPLINT.cfg deleted file mode 100644 index 2fce9d52b..000000000 --- a/examples/include/CPPLINT.cfg +++ /dev/null @@ -1 +0,0 @@ -root=. diff --git a/examples/CHANGELOG.rst b/examples/ros1/CHANGELOG.rst similarity index 100% rename from examples/CHANGELOG.rst rename to examples/ros1/CHANGELOG.rst diff --git a/examples/ros1/CMakeLists.txt b/examples/ros1/CMakeLists.txt new file mode 100644 index 000000000..c0731ef72 --- /dev/null +++ b/examples/ros1/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.0.2) +project(wavemap_examples_ros1) + +# Find dependencies +find_package(catkin REQUIRED + COMPONENTS roscpp wavemap wavemap_msgs wavemap_ros_conversions) + +# Register catkin package +catkin_package( + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS roscpp wavemap wavemap_msgs wavemap_ros_conversions) + +# Add each set of examples +add_subdirectory(io) diff --git a/examples/ros1/CPPLINT.cfg b/examples/ros1/CPPLINT.cfg new file mode 100644 index 000000000..03c80b6f9 --- /dev/null +++ b/examples/ros1/CPPLINT.cfg @@ -0,0 +1 @@ +filter=-build/namespaces diff --git a/examples/ros1/io/CMakeLists.txt b/examples/ros1/io/CMakeLists.txt new file mode 100644 index 000000000..8528527a5 --- /dev/null +++ b/examples/ros1/io/CMakeLists.txt @@ -0,0 +1,10 @@ +# Binaries +add_executable(receive_map_over_ros receive_map_over_ros.cc) +set_wavemap_target_properties(receive_map_over_ros) +target_include_directories(receive_map_over_ros PUBLIC ${catkin_INCLUDE_DIRS}) +target_link_libraries(receive_map_over_ros PUBLIC glog ${catkin_LIBRARIES}) + +add_executable(send_map_over_ros send_map_over_ros.cc) +set_wavemap_target_properties(send_map_over_ros) +target_include_directories(send_map_over_ros PUBLIC ${catkin_INCLUDE_DIRS}) +target_link_libraries(send_map_over_ros PUBLIC glog ${catkin_LIBRARIES}) diff --git a/examples/src/io/receive_map_over_ros.cc b/examples/ros1/io/receive_map_over_ros.cc similarity index 100% rename from examples/src/io/receive_map_over_ros.cc rename to examples/ros1/io/receive_map_over_ros.cc diff --git a/examples/src/io/send_map_over_ros.cc b/examples/ros1/io/send_map_over_ros.cc similarity index 100% rename from examples/src/io/send_map_over_ros.cc rename to examples/ros1/io/send_map_over_ros.cc diff --git a/examples/package.xml b/examples/ros1/package.xml similarity index 75% rename from examples/package.xml rename to examples/ros1/package.xml index a5199a3d8..0e24c0817 100644 --- a/examples/package.xml +++ b/examples/ros1/package.xml @@ -1,8 +1,8 @@ - wavemap_examples + wavemap_examples_ros1 1.6.3 - Usages examples for wavemap. + Usages examples for wavemap's ROS1 interface. Victor Reijgwart BSD @@ -11,8 +11,8 @@ Victor Reijgwart catkin - catkin_simple + roscpp wavemap wavemap_msgs wavemap_ros_conversions diff --git a/interfaces/ros1/wavemap/CMakeLists.txt b/interfaces/ros1/wavemap/CMakeLists.txt index ae605781a..b5ce4dfe0 100644 --- a/interfaces/ros1/wavemap/CMakeLists.txt +++ b/interfaces/ros1/wavemap/CMakeLists.txt @@ -4,8 +4,7 @@ project(wavemap) # Setup catkin package find_package(catkin REQUIRED) catkin_package( - INCLUDE_DIRS - ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/include + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/include LIBRARIES wavemap_core wavemap_io wavemap_pipeline CFG_EXTRAS ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/cmake/wavemap-extras.cmake) diff --git a/tooling/packages/wavemap_all/CHANGELOG.rst b/interfaces/ros1/wavemap_all/CHANGELOG.rst similarity index 100% rename from tooling/packages/wavemap_all/CHANGELOG.rst rename to interfaces/ros1/wavemap_all/CHANGELOG.rst diff --git a/tooling/packages/wavemap_all/CMakeLists.txt b/interfaces/ros1/wavemap_all/CMakeLists.txt similarity index 100% rename from tooling/packages/wavemap_all/CMakeLists.txt rename to interfaces/ros1/wavemap_all/CMakeLists.txt diff --git a/tooling/packages/wavemap_all/package.xml b/interfaces/ros1/wavemap_all/package.xml similarity index 93% rename from tooling/packages/wavemap_all/package.xml rename to interfaces/ros1/wavemap_all/package.xml index 3659fd78f..92218bc48 100644 --- a/tooling/packages/wavemap_all/package.xml +++ b/interfaces/ros1/wavemap_all/package.xml @@ -18,7 +18,7 @@ wavemap_ros wavemap_rviz_plugin wavemap_utils - wavemap_examples + wavemap_examples_ros1 diff --git a/library/src/io/CMakeLists.txt b/library/src/io/CMakeLists.txt index 1f5ae5cd3..f8745f0d2 100644 --- a/library/src/io/CMakeLists.txt +++ b/library/src/io/CMakeLists.txt @@ -5,4 +5,4 @@ add_wavemap_include_directories(wavemap_io) target_sources(wavemap_io PRIVATE file_conversions.cc stream_conversions.cc) set_wavemap_target_properties(wavemap_io) -target_link_libraries(wavemap_io PUBLIC Eigen3::Eigen glog) +target_link_libraries(wavemap_io PUBLIC Eigen3::Eigen glog wavemap_core) diff --git a/library/src/pipeline/CMakeLists.txt b/library/src/pipeline/CMakeLists.txt index 504e8652b..0eeb91713 100644 --- a/library/src/pipeline/CMakeLists.txt +++ b/library/src/pipeline/CMakeLists.txt @@ -9,4 +9,4 @@ target_sources(wavemap_pipeline PRIVATE pipeline.cc) set_wavemap_target_properties(wavemap_pipeline) -target_link_libraries(wavemap_pipeline PUBLIC Eigen3::Eigen glog) +target_link_libraries(wavemap_pipeline PUBLIC Eigen3::Eigen glog wavemap_core) From 2501fde169b175bd625f17a369c92f6ca873d6f9 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 29 Jul 2024 17:05:23 +0200 Subject: [PATCH 123/159] Simplify DECLARE_CONFIG_MEMBERS macro --- .../core/config/impl/config_base_inl.h | 40 ++++++------------- 1 file changed, 12 insertions(+), 28 deletions(-) diff --git a/library/include/wavemap/core/config/impl/config_base_inl.h b/library/include/wavemap/core/config/impl/config_base_inl.h index da14f3386..fd900666e 100644 --- a/library/include/wavemap/core/config/impl/config_base_inl.h +++ b/library/include/wavemap/core/config/impl/config_base_inl.h @@ -3,54 +3,38 @@ #include -#include -#include -#include -#include #include #include -#include #include -#include -#include -#include namespace wavemap { -#define MEMBER_NAME_FROM_TUPLE(member_name_tuple) \ - BOOST_PP_TUPLE_ELEM(0, member_name_tuple) - -#define ASSERT_CONFIG_MEMBER_TYPE_IS_SUPPORTED(r, class_name, \ - member_name_tuple) \ - static_assert( \ - class_name::MemberTypes::contains_t, \ +#define ASSERT_CONFIG_MEMBER_TYPE_IS_SUPPORTED(r, class_name, member_name) \ + static_assert( \ + class_name::MemberTypes::contains_t, \ BOOST_PP_STRINGIZE( \ - The type of class_name::MEMBER_NAME_FROM_TUPLE(member_name_tuple) \ + The type of class_name::member_name \ is not supported by default and has not been announced as a \ CustomMemberType. Make sure to include this custom type in the \ CustomMemberTypes parameter pack passed to ConfigBase.)); #define ASSERT_ALL_CONFIG_MEMBERS_DECLARED(class_name, member_sequence) \ static_assert( \ - class_name::kNumMembers == \ - BOOST_PP_SEQ_SIZE(BOOST_PP_VARIADIC_SEQ_TO_SEQ(member_sequence)), \ + class_name::kNumMembers == BOOST_PP_SEQ_SIZE(member_sequence), \ "The number of config members declared through DECLARE_CONFIG_MEMBERS " \ "must match the number of members announced through the num_members " \ "template argument passed to ConfigBase."); #define ASSERT_ALL_CONFIG_MEMBER_TYPES_SUPPORTED(class_name, member_sequence) \ BOOST_PP_SEQ_FOR_EACH(ASSERT_CONFIG_MEMBER_TYPE_IS_SUPPORTED, class_name, \ - BOOST_PP_VARIADIC_SEQ_TO_SEQ(member_sequence)) + member_sequence) -#define APPEND_CONFIG_MEMBER_METADATA(r, class_name, member_name_tuple) \ - MemberMetadata{ \ - BOOST_PP_STRINGIZE(MEMBER_NAME_FROM_TUPLE(member_name_tuple)), \ - &class_name::BOOST_PP_TUPLE_REM() member_name_tuple}, +#define APPEND_CONFIG_MEMBER_METADATA(r, class_name, member_name) \ + MemberMetadata{BOOST_PP_STRINGIZE(member_name), &class_name::member_name}, -#define GENERATE_CONFIG_MEMBER_MAP(class_name, member_sequence) \ - class_name::MemberMap class_name::memberMap { \ - BOOST_PP_SEQ_FOR_EACH(APPEND_CONFIG_MEMBER_METADATA, class_name, \ - BOOST_PP_VARIADIC_SEQ_TO_SEQ(member_sequence)) \ +#define GENERATE_CONFIG_MEMBER_MAP(class_name, member_sequence) \ + class_name::MemberMap class_name::memberMap { \ + BOOST_PP_SEQ_FOR_EACH(APPEND_CONFIG_MEMBER_METADATA, class_name, \ + member_sequence) \ } #define DECLARE_CONFIG_MEMBERS(class_name, member_sequence) \ From 66b5a149f573f1a9722921d53292e25cbbc8255c Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 31 Jul 2024 16:48:56 +0200 Subject: [PATCH 124/159] Add option to pull in missing dependencies with FetchContent --- library/CMakeLists.txt | 16 ++++-- library/cmake/boost_preprocessor/LICENSE | 23 +++++++++ .../boost_preprocessor.cmake | 6 +++ library/cmake/eigen/LICENSE | 18 +++++++ library/cmake/eigen/eigen.cmake | 49 +++++++++++++++++++ library/cmake/find_dependencies.cmake | 47 ++++++++++++++++++ library/cmake/glog/LICENSE | 27 ++++++++++ library/cmake/glog/glog.cmake | 20 ++++++++ library/src/CMakeLists.txt | 13 ----- library/src/core/CMakeLists.txt | 3 +- 10 files changed, 203 insertions(+), 19 deletions(-) create mode 100644 library/cmake/boost_preprocessor/LICENSE create mode 100644 library/cmake/boost_preprocessor/boost_preprocessor.cmake create mode 100644 library/cmake/eigen/LICENSE create mode 100644 library/cmake/eigen/eigen.cmake create mode 100644 library/cmake/find_dependencies.cmake create mode 100644 library/cmake/glog/LICENSE create mode 100644 library/cmake/glog/glog.cmake delete mode 100644 library/src/CMakeLists.txt diff --git a/library/CMakeLists.txt b/library/CMakeLists.txt index 6ac83f6b0..b04e19612 100644 --- a/library/CMakeLists.txt +++ b/library/CMakeLists.txt @@ -1,14 +1,20 @@ cmake_minimum_required(VERSION 3.10) -project(wavemap VERSION 1.6.3 LANGUAGES CXX) +project(wavemap VERSION 2.0.0 LANGUAGES CXX) -# CMake helpers +# CMake helpers and general wavemap tooling (e.g. to run clang-tidy CI) include(cmake/wavemap-extras.cmake) - -# Enable general wavemap tooling (e.g. to run clang-tidy CI) enable_wavemap_general_tooling() +# Dependencies +option(USE_SYSTEM_EIGEN "Use system pre-installed Eigen" ON) +option(USE_SYSTEM_GLOG "Use system pre-installed glog" ON) +option(USE_SYSTEM_BOOST "Use system pre-installed Boost" ON) +include(cmake/find_dependencies.cmake) + # Libraries -add_subdirectory(src) +add_subdirectory(src/core) +add_subdirectory(src/io) +add_subdirectory(src/pipeline) # Tests if (ENABLE_TESTING OR CATKIN_ENABLE_TESTING) diff --git a/library/cmake/boost_preprocessor/LICENSE b/library/cmake/boost_preprocessor/LICENSE new file mode 100644 index 000000000..36b7cd93c --- /dev/null +++ b/library/cmake/boost_preprocessor/LICENSE @@ -0,0 +1,23 @@ +Boost Software License - Version 1.0 - August 17th, 2003 + +Permission is hereby granted, free of charge, to any person or organization +obtaining a copy of the software and accompanying documentation covered by +this license (the "Software") to use, reproduce, display, distribute, +execute, and transmit the Software, and to prepare derivative works of the +Software, and to permit third-parties to whom the Software is furnished to +do so, all subject to the following: + +The copyright notices in the Software and this entire statement, including +the above license grant, this restriction and the following disclaimer, +must be included in all copies of the Software, in whole or in part, and +all derivative works of the Software, unless such copies or derivative +works are solely in the form of machine-executable object code generated by +a source language processor. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT +SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE +FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, +ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +DEALINGS IN THE SOFTWARE. diff --git a/library/cmake/boost_preprocessor/boost_preprocessor.cmake b/library/cmake/boost_preprocessor/boost_preprocessor.cmake new file mode 100644 index 000000000..178194269 --- /dev/null +++ b/library/cmake/boost_preprocessor/boost_preprocessor.cmake @@ -0,0 +1,6 @@ +include(FetchContent) +FetchContent_Declare(BoostPreprocessor + GIT_REPOSITORY https://github.com/boostorg/preprocessor.git + GIT_TAG boost-1.71.0 + GIT_SHALLOW ON) +FetchContent_MakeAvailable(BoostPreprocessor) diff --git a/library/cmake/eigen/LICENSE b/library/cmake/eigen/LICENSE new file mode 100644 index 000000000..de5b63215 --- /dev/null +++ b/library/cmake/eigen/LICENSE @@ -0,0 +1,18 @@ +Eigen is primarily MPL2 licensed. See COPYING.MPL2 and these links: + http://www.mozilla.org/MPL/2.0/ + http://www.mozilla.org/MPL/2.0/FAQ.html + +Some files contain third-party code under BSD or LGPL licenses, whence the other +COPYING.* files here. + +All the LGPL code is either LGPL 2.1-only, or LGPL 2.1-or-later. +For this reason, the COPYING.LGPL file contains the LGPL 2.1 text. + +If you want to guarantee that the Eigen code that you are #including is licensed +under the MPL2 and possibly more permissive licenses (like BSD), #define this +preprocessor symbol: + EIGEN_MPL2_ONLY +For example, with most compilers, you could add this to your project CXXFLAGS: + -DEIGEN_MPL2_ONLY +This will cause a compilation error to be generated if you #include any code that is +LGPL licensed. diff --git a/library/cmake/eigen/eigen.cmake b/library/cmake/eigen/eigen.cmake new file mode 100644 index 000000000..bc8504a15 --- /dev/null +++ b/library/cmake/eigen/eigen.cmake @@ -0,0 +1,49 @@ +# MIT License +# +# Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill +# Stachniss. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +set(EIGEN_BUILD_DOC OFF CACHE BOOL "Don't build Eigen docs") +set(EIGEN_BUILD_TESTING OFF CACHE BOOL "Don't build Eigen tests") +set(EIGEN_BUILD_PKGCONFIG OFF CACHE BOOL "Don't build Eigen pkg-config") +set(EIGEN_BUILD_BLAS OFF CACHE BOOL "Don't build blas module") +set(EIGEN_BUILD_LAPACK OFF CACHE BOOL "Don't build lapack module") + +include(FetchContent) +FetchContent_Declare(eigen + URL https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-3.4.0.tar.gz + UPDATE_DISCONNECTED 1) +FetchContent_GetProperties(eigen) +if(NOT eigen_POPULATED) + FetchContent_Populate(eigen) + if(${CMAKE_VERSION} GREATER_EQUAL 3.25) + add_subdirectory(${eigen_SOURCE_DIR} ${eigen_BINARY_DIR} + SYSTEM EXCLUDE_FROM_ALL) + else() + # Emulate the SYSTEM flag introduced in CMake 3.25. Withouth this flag the + # compiler will consider this 3rdparty headers as source code and fail due + # the -Werror flag. + add_subdirectory(${eigen_SOURCE_DIR} ${eigen_BINARY_DIR} EXCLUDE_FROM_ALL) + get_target_property(eigen_include_dirs eigen INTERFACE_INCLUDE_DIRECTORIES) + set_target_properties(eigen + PROPERTIES INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${eigen_include_dirs}") + endif() +endif() diff --git a/library/cmake/find_dependencies.cmake b/library/cmake/find_dependencies.cmake new file mode 100644 index 000000000..24617d058 --- /dev/null +++ b/library/cmake/find_dependencies.cmake @@ -0,0 +1,47 @@ +# Eigen +if (USE_SYSTEM_EIGEN) + find_package(Eigen3 QUIET NO_MODULE) +endif () +if (USE_SYSTEM_EIGEN AND TARGET Eigen3::Eigen) + message(STATUS "Using system Eigen") +else () + message(STATUS "Fetching external Eigen") + set(USE_SYSTEM_EIGEN OFF) + include("${CMAKE_CURRENT_LIST_DIR}/eigen/eigen.cmake") +endif () + +# GLOG +if (USE_SYSTEM_GLOG) + find_package(PkgConfig REQUIRED) + pkg_check_modules(glog REQUIRED libglog) +endif () +if (USE_SYSTEM_GLOG AND glog_FOUND) + message(STATUS "Using system Glog") +else () + message(STATUS "Fetching external Glog") + set(USE_SYSTEM_GLOG OFF) + include("${CMAKE_CURRENT_LIST_DIR}/glog/glog.cmake") +endif () + +# Boost's Preprocessor +if (USE_SYSTEM_BOOST) + find_package(Boost 1.71 COMPONENTS headers) + # Boost's system package exposes all headers through a single Boost::headers + # target. However, wavemap only needs Boost's Preprocessor component which we + # selectively pull in as Boost::preprocessor when using FetchContent. To allow + # targets to link against Boost::preprocessor regardless of which install mode + # is used, we define an alias from Boost::preprocessor to Boost::headers. + set_target_properties(Boost::headers PROPERTIES IMPORTED_GLOBAL TRUE) + add_library(Boost::preprocessor ALIAS Boost::headers) +endif () +if (USE_SYSTEM_BOOST AND TARGET Boost::preprocessor) + message(STATUS "Using system Boost") +else () + message(STATUS "Fetching external Boost") + set(USE_SYSTEM_BOOST OFF) + include( + "${CMAKE_CURRENT_LIST_DIR}/boost_preprocessor/boost_preprocessor.cmake") +endif () + +# Optional dependencies +find_package(tracy QUIET) diff --git a/library/cmake/glog/LICENSE b/library/cmake/glog/LICENSE new file mode 100644 index 000000000..15a95f955 --- /dev/null +++ b/library/cmake/glog/LICENSE @@ -0,0 +1,27 @@ +Copyright © 2024, Google Inc. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are +met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +* Redistributions in binary form must reproduce the above copyright notice, this + list of conditions and the following disclaimer in the documentation and/or + other materials provided with the distribution. +* Neither the name of Google Inc. nor the names of its contributors may be used + to endorse or promote products derived from this software without specific + prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/library/cmake/glog/glog.cmake b/library/cmake/glog/glog.cmake new file mode 100644 index 000000000..0b2accc42 --- /dev/null +++ b/library/cmake/glog/glog.cmake @@ -0,0 +1,20 @@ +include(FetchContent) +FetchContent_Declare(glog + GIT_REPOSITORY https://github.com/google/glog.git + GIT_TAG v0.6.0) +FetchContent_GetProperties(glog) +if (NOT glog_POPULATED) + FetchContent_Populate(glog) + if(${CMAKE_VERSION} GREATER_EQUAL 3.25) + add_subdirectory(${glog_SOURCE_DIR} ${glog_BINARY_DIR} + SYSTEM EXCLUDE_FROM_ALL) + else() + # Emulate the SYSTEM flag introduced in CMake 3.25. Withouth this flag the + # compiler will consider this 3rdparty headers as source code and fail due + # the -Werror flag. + add_subdirectory(${glog_SOURCE_DIR} ${glog_BINARY_DIR} EXCLUDE_FROM_ALL) + get_target_property(glog_include_dirs glog INTERFACE_INCLUDE_DIRECTORIES) + set_target_properties(glog + PROPERTIES INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${glog_include_dirs}") + endif() +endif () diff --git a/library/src/CMakeLists.txt b/library/src/CMakeLists.txt deleted file mode 100644 index 9500dccab..000000000 --- a/library/src/CMakeLists.txt +++ /dev/null @@ -1,13 +0,0 @@ -# Dependencies -find_package(Eigen3 REQUIRED NO_MODULE) -find_package(PkgConfig REQUIRED) -pkg_check_modules(glog REQUIRED libglog) - -# Optional dependencies -find_package(tracy QUIET) -# TODO(victorr): Check if tracy is correctly found and linked in when it is -# available as an installed (system) package. - -add_subdirectory(core) -add_subdirectory(io) -add_subdirectory(pipeline) diff --git a/library/src/core/CMakeLists.txt b/library/src/core/CMakeLists.txt index 7798c6e3a..308981793 100644 --- a/library/src/core/CMakeLists.txt +++ b/library/src/core/CMakeLists.txt @@ -43,7 +43,8 @@ target_sources(wavemap_core PRIVATE utils/thread_pool.cc) set_wavemap_target_properties(wavemap_core) -target_link_libraries(wavemap_core PUBLIC Eigen3::Eigen glog) +target_link_libraries(wavemap_core + PUBLIC Eigen3::Eigen glog Boost::preprocessor) if (tracy_FOUND) target_link_libraries(wavemap_core PUBLIC TracyClient) endif () From 26110b8a30fbd5b2b3796a0f6457f6448d6c6d53 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Wed, 31 Jul 2024 17:08:23 +0200 Subject: [PATCH 125/159] Minor dependency management fixes after testing in Docker --- library/cmake/find_dependencies.cmake | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/library/cmake/find_dependencies.cmake b/library/cmake/find_dependencies.cmake index 24617d058..6bb85297b 100644 --- a/library/cmake/find_dependencies.cmake +++ b/library/cmake/find_dependencies.cmake @@ -12,8 +12,13 @@ endif () # GLOG if (USE_SYSTEM_GLOG) - find_package(PkgConfig REQUIRED) - pkg_check_modules(glog REQUIRED libglog) + find_package(glog QUIET) + if (NOT glog_FOUND) + find_package(PkgConfig QUIET) + if (PkgConfig_FOUND) + pkg_check_modules(glog REQUIRED libglog) + endif () + endif () endif () if (USE_SYSTEM_GLOG AND glog_FOUND) message(STATUS "Using system Glog") @@ -25,14 +30,16 @@ endif () # Boost's Preprocessor if (USE_SYSTEM_BOOST) - find_package(Boost 1.71 COMPONENTS headers) + find_package(Boost 1.71 QUIET COMPONENTS headers) # Boost's system package exposes all headers through a single Boost::headers # target. However, wavemap only needs Boost's Preprocessor component which we # selectively pull in as Boost::preprocessor when using FetchContent. To allow # targets to link against Boost::preprocessor regardless of which install mode # is used, we define an alias from Boost::preprocessor to Boost::headers. - set_target_properties(Boost::headers PROPERTIES IMPORTED_GLOBAL TRUE) - add_library(Boost::preprocessor ALIAS Boost::headers) + if (Boost_FOUND) + set_target_properties(Boost::headers PROPERTIES IMPORTED_GLOBAL TRUE) + add_library(Boost::preprocessor ALIAS Boost::headers) + endif () endif () if (USE_SYSTEM_BOOST AND TARGET Boost::preprocessor) message(STATUS "Using system Boost") From 9be94210f880ecba27600fe843eb5e2cc47cb5ba Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 2 Aug 2024 15:29:31 +0200 Subject: [PATCH 126/159] Make wavemap library installable --- examples/cpp/CMakeLists.txt | 30 ++++++++++++++--- interfaces/ros1/wavemap/CMakeLists.txt | 3 +- library/CMakeLists.txt | 32 ++++++++++++++++++- ...ndencies.cmake => find-wavemap-deps.cmake} | 0 library/cmake/wavemap-config.cmake.in | 20 ++++++++++++ library/src/core/CMakeLists.txt | 27 ++++++++++++---- library/src/io/CMakeLists.txt | 19 +++++++++-- library/src/pipeline/CMakeLists.txt | 19 +++++++++-- 8 files changed, 134 insertions(+), 16 deletions(-) rename library/cmake/{find_dependencies.cmake => find-wavemap-deps.cmake} (100%) create mode 100644 library/cmake/wavemap-config.cmake.in diff --git a/examples/cpp/CMakeLists.txt b/examples/cpp/CMakeLists.txt index f45a0c3b3..2f35bac8d 100644 --- a/examples/cpp/CMakeLists.txt +++ b/examples/cpp/CMakeLists.txt @@ -1,9 +1,31 @@ cmake_minimum_required(VERSION 3.0.2) -project(wavemap_examples_cpp) +project(wavemap_examples_cpp VERSION 2.0.0 LANGUAGES CXX) -# Load the library -add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../../library - ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}) +# Load the wavemap library +# First, try to load it from sources +if (EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/../../library) + message(STATUS "Using wavemap library from local sources") + add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../../library + ${CMAKE_CURRENT_BINARY_DIR}/wavemap) +else () + # If sources are not available, see if wavemap is installed as a system lib + find_package(wavemap QUIET) + if (wavemap_FOUND) + message(STATUS "Using wavemap library installed on system") + else () # Otherwise, fetch wavemap's code from GitHub + set(WAVEMAP_TAG develop/v2.0) + message(STATUS "Fetching wavemap library from GitHub (tag ${WAVEMAP_TAG})") + cmake_minimum_required(VERSION 3.18) + include(FetchContent) + FetchContent_Declare( + ext_wavemap PREFIX wavemap + GIT_REPOSITORY https://github.com/ethz-asl/wavemap.git + GIT_TAG ${WAVEMAP_TAG} + GIT_SHALLOW 1 + SOURCE_SUBDIR library) + FetchContent_MakeAvailable(ext_wavemap) + endif () +endif () # Add each set of examples add_subdirectory(io) diff --git a/interfaces/ros1/wavemap/CMakeLists.txt b/interfaces/ros1/wavemap/CMakeLists.txt index b5ce4dfe0..eefb3ce21 100644 --- a/interfaces/ros1/wavemap/CMakeLists.txt +++ b/interfaces/ros1/wavemap/CMakeLists.txt @@ -11,7 +11,8 @@ catkin_package( # Load the library add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../../../library - ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}) + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME} + EXCLUDE_FROM_ALL) # Include the library's tests in catkin's run_tests target include(cmake/append_to_catkin_tests.cmake) diff --git a/library/CMakeLists.txt b/library/CMakeLists.txt index b04e19612..7e91e7163 100644 --- a/library/CMakeLists.txt +++ b/library/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.10) project(wavemap VERSION 2.0.0 LANGUAGES CXX) # CMake helpers and general wavemap tooling (e.g. to run clang-tidy CI) +include(GNUInstallDirs) include(cmake/wavemap-extras.cmake) enable_wavemap_general_tooling() @@ -9,7 +10,7 @@ enable_wavemap_general_tooling() option(USE_SYSTEM_EIGEN "Use system pre-installed Eigen" ON) option(USE_SYSTEM_GLOG "Use system pre-installed glog" ON) option(USE_SYSTEM_BOOST "Use system pre-installed Boost" ON) -include(cmake/find_dependencies.cmake) +include(cmake/find-wavemap-deps.cmake) # Libraries add_subdirectory(src/core) @@ -25,3 +26,32 @@ endif () if (ENABLE_BENCHMARKING) add_subdirectory(benchmark) endif () + +# Install +# Mark embedded 3rd party header-only libraries for installation +install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/3rd_party/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/3rd_party + FILES_MATCHING PATTERN "*.h") +# Install the CMake exports file +install(EXPORT wavemap-targets + FILE wavemap-targets.cmake + NAMESPACE wavemap:: + DESTINATION ${CMAKE_INSTALL_DATADIR}/wavemap/cmake) +# Generate and install a version file +include(CMakePackageConfigHelpers) +write_basic_package_version_file(wavemap-config-version.cmake + VERSION ${PACKAGE_VERSION} + COMPATIBILITY AnyNewerVersion) +install(FILES "${CMAKE_CURRENT_BINARY_DIR}/wavemap-config-version.cmake" + DESTINATION ${CMAKE_INSTALL_DATADIR}/wavemap/cmake) +# Install our CMake helper functions +install(FILES "${CMAKE_CURRENT_SOURCE_DIR}/cmake/wavemap-extras.cmake" + DESTINATION ${CMAKE_INSTALL_DATADIR}/wavemap/cmake) +# Generate and install a package config file +configure_package_config_file( + "${CMAKE_CURRENT_SOURCE_DIR}/cmake/wavemap-config.cmake.in" + "${CMAKE_CURRENT_BINARY_DIR}/wavemap-config.cmake" + INSTALL_DESTINATION ${CMAKE_INSTALL_DATADIR}/wavemap/cmake + NO_CHECK_REQUIRED_COMPONENTS_MACRO) +install(FILES "${CMAKE_CURRENT_BINARY_DIR}/wavemap-config.cmake" + DESTINATION ${CMAKE_INSTALL_DATADIR}/wavemap/cmake) diff --git a/library/cmake/find_dependencies.cmake b/library/cmake/find-wavemap-deps.cmake similarity index 100% rename from library/cmake/find_dependencies.cmake rename to library/cmake/find-wavemap-deps.cmake diff --git a/library/cmake/wavemap-config.cmake.in b/library/cmake/wavemap-config.cmake.in new file mode 100644 index 000000000..544574a0a --- /dev/null +++ b/library/cmake/wavemap-config.cmake.in @@ -0,0 +1,20 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) + +# Find dependencies +find_dependency(Eigen3 REQUIRED) +find_dependency(Boost 1.71 QUIET COMPONENTS headers) +# Boost's system package exposes all headers through a single Boost::headers +# target. However, wavemap only needs Boost's Preprocessor component which we +# selectively pull in as Boost::preprocessor when using FetchContent. To allow +# targets to link against Boost::preprocessor regardless of which install mode +# is used, we define an alias from Boost::preprocessor to Boost::headers. +set_target_properties(Boost::headers PROPERTIES IMPORTED_GLOBAL TRUE) +add_library(Boost::preprocessor ALIAS Boost::headers) + +# Include the file specifying wavemap's CMake targets +include("${CMAKE_CURRENT_LIST_DIR}/wavemap-targets.cmake") + +# Include our CMake helper functions +include("${CMAKE_CURRENT_LIST_DIR}/wavemap-extras.cmake") diff --git a/library/src/core/CMakeLists.txt b/library/src/core/CMakeLists.txt index 308981793..408d2e7aa 100644 --- a/library/src/core/CMakeLists.txt +++ b/library/src/core/CMakeLists.txt @@ -1,7 +1,17 @@ +# Declare the wavemap core library add_library(wavemap_core) add_library(wavemap::wavemap_core ALIAS wavemap_core) +# Configure target properties, include directories and linking +set_wavemap_target_properties(wavemap_core) add_wavemap_include_directories(wavemap_core) +target_link_libraries(wavemap_core + PUBLIC Eigen3::Eigen glog Boost::preprocessor) +if (tracy_FOUND) + target_link_libraries(wavemap_core PUBLIC TracyClient) +endif () + +# Set sources target_sources(wavemap_core PRIVATE config/string_list.cc config/value_with_unit.cc @@ -42,9 +52,14 @@ target_sources(wavemap_core PRIVATE utils/undistortion/stamped_pointcloud.cc utils/thread_pool.cc) -set_wavemap_target_properties(wavemap_core) -target_link_libraries(wavemap_core - PUBLIC Eigen3::Eigen glog Boost::preprocessor) -if (tracy_FOUND) - target_link_libraries(wavemap_core PUBLIC TracyClient) -endif () +# Mark headers for installation +install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/core/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/core + FILES_MATCHING PATTERN "*.h") + +# Mark target for installation and add it to the CMake exports file +install(TARGETS wavemap_core + EXPORT wavemap-targets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) diff --git a/library/src/io/CMakeLists.txt b/library/src/io/CMakeLists.txt index f8745f0d2..28ff091c7 100644 --- a/library/src/io/CMakeLists.txt +++ b/library/src/io/CMakeLists.txt @@ -1,8 +1,23 @@ +# Declare the wavemap io library add_library(wavemap_io) add_library(wavemap::wavemap_io ALIAS wavemap_io) +# Configure target properties, include directories and linking +set_wavemap_target_properties(wavemap_io) add_wavemap_include_directories(wavemap_io) +target_link_libraries(wavemap_io PUBLIC Eigen3::Eigen glog wavemap_core) + +# Set sources target_sources(wavemap_io PRIVATE file_conversions.cc stream_conversions.cc) -set_wavemap_target_properties(wavemap_io) -target_link_libraries(wavemap_io PUBLIC Eigen3::Eigen glog wavemap_core) +# Mark headers for installation +install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/io/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/io + FILES_MATCHING PATTERN "*.h") + +# Mark target for installation and add it to the CMake exports file +install(TARGETS wavemap_io + EXPORT wavemap-targets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) diff --git a/library/src/pipeline/CMakeLists.txt b/library/src/pipeline/CMakeLists.txt index 0eeb91713..0ce6c9930 100644 --- a/library/src/pipeline/CMakeLists.txt +++ b/library/src/pipeline/CMakeLists.txt @@ -1,12 +1,27 @@ +# Declare the wavemap pipeline library add_library(wavemap_pipeline) add_library(wavemap::wavemap_pipeline ALIAS wavemap_pipeline) +# Configure target properties, include directories and linking +set_wavemap_target_properties(wavemap_pipeline) add_wavemap_include_directories(wavemap_pipeline) +target_link_libraries(wavemap_pipeline PUBLIC Eigen3::Eigen glog wavemap_core) + +# Set sources target_sources(wavemap_pipeline PRIVATE map_operations/map_operation_factory.cc map_operations/prune_map_operation.cc map_operations/threshold_map_operation.cc pipeline.cc) -set_wavemap_target_properties(wavemap_pipeline) -target_link_libraries(wavemap_pipeline PUBLIC Eigen3::Eigen glog wavemap_core) +# Mark headers for installation +install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/pipeline/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/pipeline + FILES_MATCHING PATTERN "*.h") + +# Mark target for installation and add it to the CMake exports file +install(TARGETS wavemap_pipeline + EXPORT wavemap-targets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) From fa579e51c4cc1873d1c02b0a0cc174197a12245a Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 2 Aug 2024 16:06:25 +0200 Subject: [PATCH 127/159] Disable option to install if not all deps are available as system libs --- library/CMakeLists.txt | 68 +++++++++++++++++------------ library/src/core/CMakeLists.txt | 23 +++++----- library/src/io/CMakeLists.txt | 23 +++++----- library/src/pipeline/CMakeLists.txt | 31 +++++++------ 4 files changed, 83 insertions(+), 62 deletions(-) diff --git a/library/CMakeLists.txt b/library/CMakeLists.txt index 7e91e7163..fd095a77b 100644 --- a/library/CMakeLists.txt +++ b/library/CMakeLists.txt @@ -12,6 +12,16 @@ option(USE_SYSTEM_GLOG "Use system pre-installed glog" ON) option(USE_SYSTEM_BOOST "Use system pre-installed Boost" ON) include(cmake/find-wavemap-deps.cmake) +# Only allow installation if all dependencies are available as system libs +# NOTE: We automatically load missing dependencies with FetchContent, but since +# these libraries are not installed they are not guaranteed to remain +# available for the entire lifetime of an installed wavemap library. +if (NOT (USE_SYSTEM_EIGEN AND USE_SYSTEM_GLOG AND USE_SYSTEM_BOOST)) + message(STATUS + "Disabling option to install. Not all dependencies are system libraries.") + set(CMAKE_SKIP_INSTALL_RULES YES) +endif () + # Libraries add_subdirectory(src/core) add_subdirectory(src/io) @@ -27,31 +37,33 @@ if (ENABLE_BENCHMARKING) add_subdirectory(benchmark) endif () -# Install -# Mark embedded 3rd party header-only libraries for installation -install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/3rd_party/ - DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/3rd_party - FILES_MATCHING PATTERN "*.h") -# Install the CMake exports file -install(EXPORT wavemap-targets - FILE wavemap-targets.cmake - NAMESPACE wavemap:: - DESTINATION ${CMAKE_INSTALL_DATADIR}/wavemap/cmake) -# Generate and install a version file -include(CMakePackageConfigHelpers) -write_basic_package_version_file(wavemap-config-version.cmake - VERSION ${PACKAGE_VERSION} - COMPATIBILITY AnyNewerVersion) -install(FILES "${CMAKE_CURRENT_BINARY_DIR}/wavemap-config-version.cmake" - DESTINATION ${CMAKE_INSTALL_DATADIR}/wavemap/cmake) -# Install our CMake helper functions -install(FILES "${CMAKE_CURRENT_SOURCE_DIR}/cmake/wavemap-extras.cmake" - DESTINATION ${CMAKE_INSTALL_DATADIR}/wavemap/cmake) -# Generate and install a package config file -configure_package_config_file( - "${CMAKE_CURRENT_SOURCE_DIR}/cmake/wavemap-config.cmake.in" - "${CMAKE_CURRENT_BINARY_DIR}/wavemap-config.cmake" - INSTALL_DESTINATION ${CMAKE_INSTALL_DATADIR}/wavemap/cmake - NO_CHECK_REQUIRED_COMPONENTS_MACRO) -install(FILES "${CMAKE_CURRENT_BINARY_DIR}/wavemap-config.cmake" - DESTINATION ${CMAKE_INSTALL_DATADIR}/wavemap/cmake) +# Support installs +if (NOT CMAKE_SKIP_INSTALL_RULES) + # Mark embedded 3rd party header-only libraries for installation + install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/3rd_party/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/3rd_party + FILES_MATCHING PATTERN "*.h") + # Install the CMake exports file + install(EXPORT wavemap-targets + FILE wavemap-targets.cmake + NAMESPACE wavemap:: + DESTINATION ${CMAKE_INSTALL_DATADIR}/wavemap/cmake) + # Generate and install a version file + include(CMakePackageConfigHelpers) + write_basic_package_version_file(wavemap-config-version.cmake + VERSION ${PACKAGE_VERSION} + COMPATIBILITY AnyNewerVersion) + install(FILES "${CMAKE_CURRENT_BINARY_DIR}/wavemap-config-version.cmake" + DESTINATION ${CMAKE_INSTALL_DATADIR}/wavemap/cmake) + # Install our CMake helper functions + install(FILES "${CMAKE_CURRENT_SOURCE_DIR}/cmake/wavemap-extras.cmake" + DESTINATION ${CMAKE_INSTALL_DATADIR}/wavemap/cmake) + # Generate and install a package config file + configure_package_config_file( + "${CMAKE_CURRENT_SOURCE_DIR}/cmake/wavemap-config.cmake.in" + "${CMAKE_CURRENT_BINARY_DIR}/wavemap-config.cmake" + INSTALL_DESTINATION ${CMAKE_INSTALL_DATADIR}/wavemap/cmake + NO_CHECK_REQUIRED_COMPONENTS_MACRO) + install(FILES "${CMAKE_CURRENT_BINARY_DIR}/wavemap-config.cmake" + DESTINATION ${CMAKE_INSTALL_DATADIR}/wavemap/cmake) +endif () diff --git a/library/src/core/CMakeLists.txt b/library/src/core/CMakeLists.txt index 408d2e7aa..12d6f0ed7 100644 --- a/library/src/core/CMakeLists.txt +++ b/library/src/core/CMakeLists.txt @@ -52,14 +52,17 @@ target_sources(wavemap_core PRIVATE utils/undistortion/stamped_pointcloud.cc utils/thread_pool.cc) -# Mark headers for installation -install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/core/ - DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/core - FILES_MATCHING PATTERN "*.h") +# Support installs +if (NOT CMAKE_SKIP_INSTALL_RULES) + # Mark headers for installation + install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/core/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/core + FILES_MATCHING PATTERN "*.h") -# Mark target for installation and add it to the CMake exports file -install(TARGETS wavemap_core - EXPORT wavemap-targets - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) + # Mark target for installation and add it to the CMake exports file + install(TARGETS wavemap_core + EXPORT wavemap-targets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) +endif () diff --git a/library/src/io/CMakeLists.txt b/library/src/io/CMakeLists.txt index 28ff091c7..21b209bd8 100644 --- a/library/src/io/CMakeLists.txt +++ b/library/src/io/CMakeLists.txt @@ -10,14 +10,17 @@ target_link_libraries(wavemap_io PUBLIC Eigen3::Eigen glog wavemap_core) # Set sources target_sources(wavemap_io PRIVATE file_conversions.cc stream_conversions.cc) -# Mark headers for installation -install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/io/ - DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/io - FILES_MATCHING PATTERN "*.h") +# Support installs +if (NOT CMAKE_SKIP_INSTALL_RULES) + # Mark headers for installation + install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/io/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/io + FILES_MATCHING PATTERN "*.h") -# Mark target for installation and add it to the CMake exports file -install(TARGETS wavemap_io - EXPORT wavemap-targets - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) + # Mark target for installation and add it to the CMake exports file + install(TARGETS wavemap_io + EXPORT wavemap-targets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) +endif () diff --git a/library/src/pipeline/CMakeLists.txt b/library/src/pipeline/CMakeLists.txt index 0ce6c9930..6db2c87cd 100644 --- a/library/src/pipeline/CMakeLists.txt +++ b/library/src/pipeline/CMakeLists.txt @@ -9,19 +9,22 @@ target_link_libraries(wavemap_pipeline PUBLIC Eigen3::Eigen glog wavemap_core) # Set sources target_sources(wavemap_pipeline PRIVATE - map_operations/map_operation_factory.cc - map_operations/prune_map_operation.cc - map_operations/threshold_map_operation.cc - pipeline.cc) + map_operations/map_operation_factory.cc + map_operations/prune_map_operation.cc + map_operations/threshold_map_operation.cc + pipeline.cc) -# Mark headers for installation -install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/pipeline/ - DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/pipeline - FILES_MATCHING PATTERN "*.h") +# Support installs +if (NOT CMAKE_SKIP_INSTALL_RULES) + # Mark headers for installation + install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/pipeline/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/pipeline + FILES_MATCHING PATTERN "*.h") -# Mark target for installation and add it to the CMake exports file -install(TARGETS wavemap_pipeline - EXPORT wavemap-targets - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) + # Mark target for installation and add it to the CMake exports file + install(TARGETS wavemap_pipeline + EXPORT wavemap-targets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) +endif () From 155cb1f52eb3724b430656910b88a6704add605c Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Fri, 2 Aug 2024 21:26:19 +0200 Subject: [PATCH 128/159] Fix conflicts between plain CMake and catkin install rules --- interfaces/ros1/wavemap/CMakeLists.txt | 6 +++--- library/CMakeLists.txt | 18 ++++++++++++------ library/src/core/CMakeLists.txt | 2 +- library/src/io/CMakeLists.txt | 2 +- library/src/pipeline/CMakeLists.txt | 2 +- 5 files changed, 18 insertions(+), 12 deletions(-) diff --git a/interfaces/ros1/wavemap/CMakeLists.txt b/interfaces/ros1/wavemap/CMakeLists.txt index eefb3ce21..970ed3819 100644 --- a/interfaces/ros1/wavemap/CMakeLists.txt +++ b/interfaces/ros1/wavemap/CMakeLists.txt @@ -10,9 +10,9 @@ catkin_package( ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/cmake/wavemap-extras.cmake) # Load the library +set(GENERATE_WAVEMAP_INSTALL_RULES OFF) add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../../../library - ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME} - EXCLUDE_FROM_ALL) + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}) # Include the library's tests in catkin's run_tests target include(cmake/append_to_catkin_tests.cmake) @@ -25,7 +25,7 @@ install(TARGETS wavemap_core wavemap_io wavemap_pipeline LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -# Mark cpp header files for installation +# Mark header files for installation install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/include/wavemap/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h") diff --git a/library/CMakeLists.txt b/library/CMakeLists.txt index fd095a77b..ed2066d1a 100644 --- a/library/CMakeLists.txt +++ b/library/CMakeLists.txt @@ -1,25 +1,31 @@ cmake_minimum_required(VERSION 3.10) project(wavemap VERSION 2.0.0 LANGUAGES CXX) +# General options +cmake_policy(SET CMP0077 NEW) +option(GENERATE_WAVEMAP_INSTALL_RULES + "Whether to generate install rules for the wavemap library" ON) +option(USE_SYSTEM_EIGEN "Use system pre-installed Eigen" ON) +option(USE_SYSTEM_GLOG "Use system pre-installed glog" ON) +option(USE_SYSTEM_BOOST "Use system pre-installed Boost" ON) + # CMake helpers and general wavemap tooling (e.g. to run clang-tidy CI) include(GNUInstallDirs) include(cmake/wavemap-extras.cmake) enable_wavemap_general_tooling() # Dependencies -option(USE_SYSTEM_EIGEN "Use system pre-installed Eigen" ON) -option(USE_SYSTEM_GLOG "Use system pre-installed glog" ON) -option(USE_SYSTEM_BOOST "Use system pre-installed Boost" ON) include(cmake/find-wavemap-deps.cmake) # Only allow installation if all dependencies are available as system libs # NOTE: We automatically load missing dependencies with FetchContent, but since # these libraries are not installed they are not guaranteed to remain # available for the entire lifetime of an installed wavemap library. -if (NOT (USE_SYSTEM_EIGEN AND USE_SYSTEM_GLOG AND USE_SYSTEM_BOOST)) +if (GENERATE_WAVEMAP_INSTALL_RULES + AND NOT (USE_SYSTEM_EIGEN AND USE_SYSTEM_GLOG AND USE_SYSTEM_BOOST)) message(STATUS "Disabling option to install. Not all dependencies are system libraries.") - set(CMAKE_SKIP_INSTALL_RULES YES) + set(GENERATE_WAVEMAP_INSTALL_RULES OFF) endif () # Libraries @@ -38,7 +44,7 @@ if (ENABLE_BENCHMARKING) endif () # Support installs -if (NOT CMAKE_SKIP_INSTALL_RULES) +if (GENERATE_WAVEMAP_INSTALL_RULES) # Mark embedded 3rd party header-only libraries for installation install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/3rd_party/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/3rd_party diff --git a/library/src/core/CMakeLists.txt b/library/src/core/CMakeLists.txt index 12d6f0ed7..d1363df99 100644 --- a/library/src/core/CMakeLists.txt +++ b/library/src/core/CMakeLists.txt @@ -53,7 +53,7 @@ target_sources(wavemap_core PRIVATE utils/thread_pool.cc) # Support installs -if (NOT CMAKE_SKIP_INSTALL_RULES) +if (GENERATE_WAVEMAP_INSTALL_RULES) # Mark headers for installation install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/core/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/core diff --git a/library/src/io/CMakeLists.txt b/library/src/io/CMakeLists.txt index 21b209bd8..8a8fb375c 100644 --- a/library/src/io/CMakeLists.txt +++ b/library/src/io/CMakeLists.txt @@ -11,7 +11,7 @@ target_link_libraries(wavemap_io PUBLIC Eigen3::Eigen glog wavemap_core) target_sources(wavemap_io PRIVATE file_conversions.cc stream_conversions.cc) # Support installs -if (NOT CMAKE_SKIP_INSTALL_RULES) +if (GENERATE_WAVEMAP_INSTALL_RULES) # Mark headers for installation install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/io/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/io diff --git a/library/src/pipeline/CMakeLists.txt b/library/src/pipeline/CMakeLists.txt index 6db2c87cd..40ba789f1 100644 --- a/library/src/pipeline/CMakeLists.txt +++ b/library/src/pipeline/CMakeLists.txt @@ -15,7 +15,7 @@ target_sources(wavemap_pipeline PRIVATE pipeline.cc) # Support installs -if (NOT CMAKE_SKIP_INSTALL_RULES) +if (GENERATE_WAVEMAP_INSTALL_RULES) # Mark headers for installation install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/pipeline/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/pipeline From 4b27488f2cc4061618a47c321909b9207276c5dd Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Sat, 3 Aug 2024 12:23:53 +0200 Subject: [PATCH 129/159] Start updating the documentation --- .github/workflows/cd.yml | 2 +- .github/workflows/ci.yml | 2 +- docs/conf.py | 3 +- docs/latex_index.rst | 4 +- docs/pages/configuration/general.rst | 2 + docs/pages/configuration/index.rst | 3 + docs/pages/configuration/map_operations.rst | 2 + .../configuration/measurement_integrators.rst | 2 + docs/pages/demos.rst | 14 +- docs/pages/faq.rst | 2 +- docs/pages/installation/cmake.rst | 142 ++++++++++++++++++ docs/pages/installation/index.rst | 15 ++ docs/pages/installation/python.rst | 4 + .../ros1.rst} | 19 +-- docs/pages/intro.rst | 6 +- docs/pages/usage_examples.rst | 68 --------- docs/pages/usage_examples/cpp.rst | 61 ++++++++ docs/pages/usage_examples/index.rst | 12 ++ docs/pages/usage_examples/python.rst | 4 + docs/pages/usage_examples/ros1.rst | 33 ++++ 20 files changed, 300 insertions(+), 100 deletions(-) create mode 100644 docs/pages/configuration/general.rst create mode 100644 docs/pages/configuration/map_operations.rst create mode 100644 docs/pages/configuration/measurement_integrators.rst create mode 100644 docs/pages/installation/cmake.rst create mode 100644 docs/pages/installation/index.rst create mode 100644 docs/pages/installation/python.rst rename docs/pages/{installation.rst => installation/ros1.rst} (89%) delete mode 100644 docs/pages/usage_examples.rst create mode 100644 docs/pages/usage_examples/cpp.rst create mode 100644 docs/pages/usage_examples/index.rst create mode 100644 docs/pages/usage_examples/python.rst create mode 100644 docs/pages/usage_examples/ros1.rst diff --git a/.github/workflows/cd.yml b/.github/workflows/cd.yml index ed0d9864f..f6161a1b6 100644 --- a/.github/workflows/cd.yml +++ b/.github/workflows/cd.yml @@ -152,7 +152,7 @@ jobs: apt-get update apt-get install -q -y --no-install-recommends python3-pip doxygen apt-get install -q -y --no-install-recommends latexmk texlive-latex-extra tex-gyre texlive-fonts-recommended texlive-latex-recommended - pip3 install exhale sphinx-sitemap + pip3 install exhale sphinx-sitemap sphinx-design pip3 install sphinxawesome-theme --pre pip3 install "sphinx<7,>6" diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index be6534f9d..497a90363 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -156,7 +156,7 @@ jobs: apt-get update apt-get install -q -y --no-install-recommends python3-pip doxygen apt-get install -q -y --no-install-recommends latexmk texlive-latex-extra tex-gyre texlive-fonts-recommended texlive-latex-recommended - pip3 install exhale sphinx-sitemap + pip3 install exhale sphinx-sitemap sphinx-design pip3 install sphinxawesome-theme --pre pip3 install "sphinx<7,>6" diff --git a/docs/conf.py b/docs/conf.py index 4da1a1a04..c800e4d64 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -18,7 +18,8 @@ # General configuration extensions = [ 'sphinx.ext.mathjax', "sphinx.ext.extlinks", 'sphinx.ext.githubpages', - "sphinx_sitemap", 'breathe', 'exhale', "sphinxawesome_theme.highlighting" + 'sphinx_design', 'sphinx_sitemap', 'breathe', 'exhale', + 'sphinxawesome_theme.highlighting' ] templates_path = ['_templates'] source_suffix = ['.rst', '.md'] diff --git a/docs/latex_index.rst b/docs/latex_index.rst index 2fb4df3da..17e6504d5 100644 --- a/docs/latex_index.rst +++ b/docs/latex_index.rst @@ -6,10 +6,10 @@ Wavemap documentation :maxdepth: 2 pages/latex_intro - pages/installation + pages/installation/index pages/demos pages/configuration/index - pages/usage_examples + pages/usage_examples/index pages/contributing pages/faq diff --git a/docs/pages/configuration/general.rst b/docs/pages/configuration/general.rst new file mode 100644 index 000000000..69f5e2d2b --- /dev/null +++ b/docs/pages/configuration/general.rst @@ -0,0 +1,2 @@ +General +####### diff --git a/docs/pages/configuration/index.rst b/docs/pages/configuration/index.rst index 7e2448765..86841b030 100644 --- a/docs/pages/configuration/index.rst +++ b/docs/pages/configuration/index.rst @@ -27,5 +27,8 @@ For reference, an overview of all available config options is provided below. :caption: Full options :maxdepth: 2 + general map + map_operations + measurement_integrators inputs diff --git a/docs/pages/configuration/map_operations.rst b/docs/pages/configuration/map_operations.rst new file mode 100644 index 000000000..01f4e0e50 --- /dev/null +++ b/docs/pages/configuration/map_operations.rst @@ -0,0 +1,2 @@ +Map operations +############## diff --git a/docs/pages/configuration/measurement_integrators.rst b/docs/pages/configuration/measurement_integrators.rst new file mode 100644 index 000000000..94de16028 --- /dev/null +++ b/docs/pages/configuration/measurement_integrators.rst @@ -0,0 +1,2 @@ +Measurement integrators +####################### diff --git a/docs/pages/demos.rst b/docs/pages/demos.rst index cb596fd3a..f7b0e3fcb 100644 --- a/docs/pages/demos.rst +++ b/docs/pages/demos.rst @@ -4,6 +4,8 @@ Demos .. include:: .. rstcheck: ignore-roles=gh_file +All of the demos currently use wavemap's ROS1 interface. In case they interest you but you do not use ROS1, we recommend trying them through a :ref:`temporary Docker container `. + Quick start *********** To get an impression of the maps wavemap can generate, you can download the maps of our `RSS paper `__ and directly view them in Rviz. To do so, @@ -40,18 +42,6 @@ To process it with wavemap, run:: To experiment with different wavemap settings, modify :gh_file:`this config file `. -Your own data -************* -The only requirements for running wavemap are: - -1. a source of depth measurements, -2. sensor pose (estimates) for each measurement. - -We usually use depth measurements from depth cameras or 3D LiDARs, but any source would work as long as a sufficiently good :ref:`projection ` and :ref:`measurement ` model is available. Wavemap's ROS interface natively supports depth image and pointcloud inputs, and automatically queries the sensor poses from the TF tree. - -To help you get started quickly, we provide various example :gh_file:`config ` and ROS :gh_file:`launch ` files. -For a brief introduction on how to edit wavemap configs and an overview of all the available settings, see the :doc:`configuration page `. - Multi-sensor live demo ********************** This section provides instructions to reproduce the interactive multi-sensor, multi-resolution mapping demo we performed at several events, including `RSS 2023 `__ and the `Swiss Robotics Day 2023 `__. In this demo, wavemap fuses measurements from a depth camera up to a resolution of 1cm and a LiDAR up to a range of 15m in real-time on a laptop. The odometry is obtained by running FastLIO2 using the LiDAR's pointclouds and built-in IMU. diff --git a/docs/pages/faq.rst b/docs/pages/faq.rst index ff4105478..588166c75 100644 --- a/docs/pages/faq.rst +++ b/docs/pages/faq.rst @@ -5,7 +5,7 @@ If you have a question that is not yet answered below, feel free to open a `GitH How do I query if a point in the map is occupied? ================================================= -Please see the :doc:`usage examples ` on :ref:`interpolation ` and :ref:`classification `. +Please see the :doc:`usage examples ` on :ref:`interpolation ` and :ref:`classification `. Does wavemap support (Euclidean) Signed Distance Fields? ======================================================== diff --git a/docs/pages/installation/cmake.rst b/docs/pages/installation/cmake.rst new file mode 100644 index 000000000..086879a11 --- /dev/null +++ b/docs/pages/installation/cmake.rst @@ -0,0 +1,142 @@ +C++ Library (CMake) +################### +.. highlight:: bash +.. rstcheck: ignore-directives=tab-set-code +.. rstcheck: ignore-roles=gh_file + +Wavemap's C++ library can be used as standard CMake package. +In the following sections, we'll discuss how you can include it in your own CMake project and link it to your libraries or executables. + +Note that if you intend to use wavemap with ROS1, you can skip this guide and proceed directly to the :doc:`ROS1 installation page `. + +Including it in your project +**************************** + +We'll cover three ways in which you can make wavemap's library available in an existing CMake project. + +Before you start, make sure your system has the necessary tools installed to build C++ projects with CMake. On Ubuntu, we recommend installing:: + + sudo apt install cmake build-essential git + +FetchContent +============ + +The fastest way to make wavemap's C++ library available in an existing CMake project is to use FetchContent, by adding the following lines to your project's CMakeLists.txt: + +.. code-block:: cmake + + set(WAVEMAP_TAG develop/v2.0) + + cmake_minimum_required(VERSION 3.18) + message(STATUS "Fetching wavemap ${WAVEMAP_TAG} from GitHub") + include(FetchContent) + FetchContent_Declare(wavemap + GIT_REPOSITORY https://github.com/ethz-asl/wavemap.git + GIT_TAG ${WAVEMAP_TAG} + GIT_SHALLOW 1 + SOURCE_SUBDIR library) + FetchContent_MakeAvailable(wavemap) + +Subdirectory +============ + +The second option is to load wavemap's library into a subfolder of your project. This might be more convenient if you expect to modify its code in the future: + +.. tab-set-code:: + + .. code-block:: HTTPS + :class: no-header + :language: bash + + cd /dependencies + git clone https://github.com/ethz-asl/wavemap.git + + .. code-block:: SSH + :class: no-header + :language: bash + + cd /dependencies + git clone git@github.com:ethz-asl/wavemap.git + + .. code-block:: Submodule + :class: no-header + :language: bash + + cd /dependencies + git submodule add git@github.com:ethz-asl/wavemap.git + +You can then include it into your CMake project by adding the following lines to your CMakeLists.txt file: + +.. code-block:: cmake + + add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/dependencies/wavemap/library + ${CMAKE_CURRENT_BINARY_DIR}/wavemap + +System install +============== + +The last option is to build wavemap as a standalone project, install on your system and then load it using CMake's `find_package`. This option is convenient when you want to use wavemap in multiple projects on your machine, while only having to download and compile it once. + +First, make sure that all of wavemap's dependencies are available as system libraries. On Ubuntu, we recommend installing:: + + sudo apt install libeigen3-dev libgoogle-glog-dev libboost-dev + +Next, download the code: + +.. tab-set-code:: + + .. code-block:: HTTPS + :class: no-header + :language: bash + + git clone https://github.com/ethz-asl/wavemap.git + + .. code-block:: SSH + :class: no-header + :language: bash + + git clone git@github.com:ethz-asl/wavemap.git + +Build it by running:: + + cd wavemap/library + cmake -S . -B build + cmake --build build -j $(nproc) + +You can then install wavemap as a system library by running:: + + cmake --install build # possibly needs sudo + +To load wavemap's library into your own CMake project, you can now simply call `find_package` in your CMakeLists.txt: + +.. code-block:: cmake + + find_package(wavemap) + + +Linking it to your code +*********************** + +After following either of the methods above, you're ready to link wavemap's C++ library against your own CMake targets and start using it inside your code. + +The library contains three main components: + +* `wavemap_core`: The framework's core algorithms, data structures and utilities +* `wavemap_io`: Functions to read and write maps to streams and files +* `wavemap_pipeline`: A measurement integration and map management pipeline + +For an example executable that performs some operations on a map after reading it from a file, you would only need to link: + +.. code-block:: cmake + + add_executable(example_executable example.cc) + target_link_libraries(example_executable + PUBLIC wavemap::wavemap_core wavemap::wavemap_io) + +Note that this will automatically make wavemap's headers (includes) available to `example.cc`. + +Finally, we strongly recommend using the `set_wavemap_target_properties` helper function to ensure your target's compilation flags are compatible with those of wavemap: + +.. code-block:: cmake + + set_wavemap_target_properties(example_executable) diff --git a/docs/pages/installation/index.rst b/docs/pages/installation/index.rst new file mode 100644 index 000000000..3d9f9e6e3 --- /dev/null +++ b/docs/pages/installation/index.rst @@ -0,0 +1,15 @@ +Installation +############ + +Wavemap can be installed on your system in several ways. +All of the framework's main functionalities are available through a lightweight, high performance C++ library that can be installed and used as a pure CMake package. +To support faster prototyping and easier integration with learning-based systems, we are also actively working on making wavemap available as a Python module. +For roboticists working with ROS, we provide packages that tightly integrate wavemap into ROS1's ecosystem. If you're interested in using wavemap with ROS2 and willing to contribute, please get in touch with us `here `_. + +.. toctree:: + :caption: Installation tutorials + :maxdepth: 2 + + cmake + python + ros1 diff --git a/docs/pages/installation/python.rst b/docs/pages/installation/python.rst new file mode 100644 index 000000000..e6c3daad6 --- /dev/null +++ b/docs/pages/installation/python.rst @@ -0,0 +1,4 @@ +Python +###### + +Wavemap's Python API is under active development. We will add it to the documentation soon. diff --git a/docs/pages/installation.rst b/docs/pages/installation/ros1.rst similarity index 89% rename from docs/pages/installation.rst rename to docs/pages/installation/ros1.rst index f449c140c..855d3c243 100644 --- a/docs/pages/installation.rst +++ b/docs/pages/installation/ros1.rst @@ -1,14 +1,16 @@ -Installation -############ +ROS1 (catkin) +############# .. highlight:: bash +.. rstcheck: ignore-directives=tab-set-code .. rstcheck: ignore-roles=gh_file If you would like to quickly try out wavemap or if you're not already using Ubuntu Noetic and ROS1, we recommend running wavemap in Docker. For active wavemap development, it is probably easiest to install wavemap directly on your system. - Docker ****** +.. _installation-ros1-docker: + If you have not yet installed Docker on your computer, please follow `these instructions `_. We also recommend executing the `post-installation steps for Linux `_, to make Docker available without ``sudo`` priviliges. To build wavemap's Docker image, simply run:: @@ -19,10 +21,10 @@ This will create a local image on your machine containing the latest version of There are many ways to work with Docker containers, with different pros and cons depending on the application. -One documented example of how to run wavemap containers with GUI (e.g. Rviz) support is provided in this :gh_file:`run_in_docker.sh ` script. This example should suffice to run all the :doc:`demos`. +One documented example of how to run wavemap containers with GUI (e.g. Rviz) support is provided in this :gh_file:`run_in_docker.sh ` script. This example should suffice to run all the :doc:`demos <../demos>`. -System install with ROS -*********************** +Native install +************** We recommend using `ROS Noetic `_, as installed following the `standard instructions `_. Make sure these required dependencies are installed:: @@ -64,8 +66,3 @@ Build all of wavemap's packages, including its ROS interface and the Rviz plugin cd ~/catkin_ws/ catkin build wavemap_all - - -System install without ROS -************************** -Wavemap's core libraries (in the `library` folder) are ROS agnostic and only have a few lightweight dependencies. They can be built and integrated into your project using CMake. diff --git a/docs/pages/intro.rst b/docs/pages/intro.rst index 0c84d3289..ade952c20 100644 --- a/docs/pages/intro.rst +++ b/docs/pages/intro.rst @@ -61,15 +61,15 @@ For other citation styles, you can use the `Crosscite's citation formatter ` or :doc:`ROS1 packages <../installation/ros1>`. + +Note that an example CMake package that contains all of the examples that follow can be found :gh_file:`here `. + +Serialization +************* + +Files +===== +Saving maps to files: + +.. literalinclude:: ../../../examples/cpp/io/save_map_to_file.cc + :language: c++ + +Loading maps from files: + +.. literalinclude:: ../../../examples/cpp/io/load_map_from_file.cc + :language: c++ + +Queries +******* + +Fixed resolution +================ +.. literalinclude:: ../../../examples/cpp/queries/fixed_resolution.cc + :language: c++ + +Multi-res averages +================== +.. literalinclude:: ../../../examples/cpp/queries/multi_resolution.cc + :language: c++ + +Accelerators +============ +.. literalinclude:: ../../../examples/cpp/queries/accelerated_queries.cc + :language: c++ + +Interpolation +============= +.. _examples-interpolation: + +Nearest neighbor interpolation: + +.. literalinclude:: ../../../examples/cpp/queries/nearest_neighbor_interpolation.cc + :language: c++ + +Trilinear interpolation: + +.. literalinclude:: ../../../examples/cpp/queries/trilinear_interpolation.cc + :language: c++ + +Classification +============== +.. _examples-classification: + +.. literalinclude:: ../../../examples/cpp/queries/classification.cc + :language: c++ diff --git a/docs/pages/usage_examples/index.rst b/docs/pages/usage_examples/index.rst new file mode 100644 index 000000000..f9070fd95 --- /dev/null +++ b/docs/pages/usage_examples/index.rst @@ -0,0 +1,12 @@ +Usage examples +############## + +This section provides hands-on examples of how to use wavemap's C++ and Python APIs, and its ROS1 interface. + +.. toctree:: + :caption: Examples per interface + :maxdepth: 1 + + cpp + python + ros1 diff --git a/docs/pages/usage_examples/python.rst b/docs/pages/usage_examples/python.rst new file mode 100644 index 000000000..38ec512f5 --- /dev/null +++ b/docs/pages/usage_examples/python.rst @@ -0,0 +1,4 @@ +Python API +########## + +Wavemap's Python API is under active development. We will add it to the documentation soon. diff --git a/docs/pages/usage_examples/ros1.rst b/docs/pages/usage_examples/ros1.rst new file mode 100644 index 000000000..b0a032982 --- /dev/null +++ b/docs/pages/usage_examples/ros1.rst @@ -0,0 +1,33 @@ +ROS1 Interface +############## +.. rstcheck: ignore-roles=gh_file + +On the :doc:`demos page <../demos>`, we saw several examples of how to run wavemap on existing datasets. In the sections that follow, we'll first discuss how the ROS packages that are included in wavemap's ROS1 interface can be configured to process your own sensor data. We'll then explain how you can interact with wavemaps in your own ROS nodes to enable new, custom use cases. + +Your own data +************* +The only requirements to run wavemap on your own dataset or robot are: + +1. a source of depth measurements, +2. sensor pose (estimates) for each measurement. + +We usually use depth measurements from depth cameras or 3D LiDARs, but any source would work as long as a sufficiently good :ref:`projection ` and :ref:`measurement ` model is available. Wavemap's ROS interface natively supports depth image and pointcloud inputs, and automatically queries the sensor poses from the TF tree. + +To help you get started quickly, we provide example :gh_file:`config ` and ROS :gh_file:`launch ` files for various sensor setups and use cases. An overview of all the available settings is provided on the :doc:`configuration page <../configuration/index>`. + +Your own code +************* + +Since wavemap's ROS1 interface extends its C++ API, all of the :doc:`C++ API's usage examples ` can directly be used in ROS. + +The only code required to receive maps over a ROS topic in your own ROS node is: + +.. literalinclude:: ../../../examples/ros1/io/receive_map_over_ros.cc + :language: c++ + +To send a map, the following code can be used: + +.. literalinclude:: ../../../examples/ros1/io/send_map_over_ros.cc + :language: c++ + +Note that an example catkin package that includes the above code is provided :gh_file:`here `. From d562b2ef75d1af8fb61c7d02b421b9f2d23a4ca4 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 6 Aug 2024 16:12:26 +0200 Subject: [PATCH 130/159] Cleaner dependency handling in catkin --- examples/ros1/io/CMakeLists.txt | 4 ++-- interfaces/ros1/wavemap/CMakeLists.txt | 13 ++++++++++++- interfaces/ros1/wavemap_ros/CMakeLists.txt | 2 +- .../ros1/wavemap_ros_conversions/CMakeLists.txt | 2 +- 4 files changed, 16 insertions(+), 5 deletions(-) diff --git a/examples/ros1/io/CMakeLists.txt b/examples/ros1/io/CMakeLists.txt index 8528527a5..1075711a1 100644 --- a/examples/ros1/io/CMakeLists.txt +++ b/examples/ros1/io/CMakeLists.txt @@ -2,9 +2,9 @@ add_executable(receive_map_over_ros receive_map_over_ros.cc) set_wavemap_target_properties(receive_map_over_ros) target_include_directories(receive_map_over_ros PUBLIC ${catkin_INCLUDE_DIRS}) -target_link_libraries(receive_map_over_ros PUBLIC glog ${catkin_LIBRARIES}) +target_link_libraries(receive_map_over_ros PUBLIC ${catkin_LIBRARIES}) add_executable(send_map_over_ros send_map_over_ros.cc) set_wavemap_target_properties(send_map_over_ros) target_include_directories(send_map_over_ros PUBLIC ${catkin_INCLUDE_DIRS}) -target_link_libraries(send_map_over_ros PUBLIC glog ${catkin_LIBRARIES}) +target_link_libraries(send_map_over_ros PUBLIC ${catkin_LIBRARIES}) diff --git a/interfaces/ros1/wavemap/CMakeLists.txt b/interfaces/ros1/wavemap/CMakeLists.txt index 970ed3819..3ef23d696 100644 --- a/interfaces/ros1/wavemap/CMakeLists.txt +++ b/interfaces/ros1/wavemap/CMakeLists.txt @@ -1,11 +1,22 @@ cmake_minimum_required(VERSION 3.0.2) project(wavemap) -# Setup catkin package +# Find dependencies find_package(catkin REQUIRED) +find_package(Eigen3 QUIET NO_MODULE) +# Manually set the lib name for glog and remap the include dir var for Eigen3 +# cmake-lint: disable=C0103 +set(glog_LIBRARIES glog) +set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) +# NOTE: These are workarounds to handle wavemap's dependencies in catkin despite +# glog not yet supporting CMake's find_package on Ubuntu 20.04 and Eigen3 +# naming its variables differently from what catkin expects. + +# Register catkin package catkin_package( INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/include LIBRARIES wavemap_core wavemap_io wavemap_pipeline + DEPENDS Eigen3 glog CFG_EXTRAS ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/cmake/wavemap-extras.cmake) diff --git a/interfaces/ros1/wavemap_ros/CMakeLists.txt b/interfaces/ros1/wavemap_ros/CMakeLists.txt index 0d6ae71f4..d7388c1f1 100644 --- a/interfaces/ros1/wavemap_ros/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros/CMakeLists.txt @@ -32,7 +32,7 @@ cs_add_library(${PROJECT_NAME} src/utils/tf_transformer.cc src/ros_server.cc) set_wavemap_target_properties(${PROJECT_NAME}) -target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES} glog) +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) # Binaries cs_add_executable(ros_server app/ros_server.cc) diff --git a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt index ad6b047c2..3d19c7094 100644 --- a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt @@ -33,7 +33,7 @@ if (CATKIN_ENABLE_TESTING) target_include_directories(test_${PROJECT_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/test/include) target_link_libraries(test_${PROJECT_NAME} - ${PROJECT_NAME} glog GTest::gtest_main) + ${PROJECT_NAME} GTest::gtest_main) endif () # Install targets From d03dbef6df15222c213e571e0639fe5fecf95966 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 6 Aug 2024 18:42:01 +0200 Subject: [PATCH 131/159] Use more future proof names for ROS input handlers --- interfaces/ros1/wavemap_ros/CMakeLists.txt | 8 +- .../ros1/wavemap_ros/app/rosbag_processor.cc | 14 ++-- .../config/wavemap_livox_mid360.yaml | 2 +- .../wavemap_livox_mid360_azure_kinect.yaml | 4 +- .../wavemap_livox_mid360_pico_flexx.yaml | 4 +- .../wavemap_livox_mid360_pico_monstar.yaml | 4 +- .../config/wavemap_ouster_os0.yaml | 2 +- .../wavemap_ouster_os0_pico_monstar.yaml | 4 +- .../config/wavemap_ouster_os1.yaml | 2 +- .../config/wavemap_panoptic_mapping_rgbd.yaml | 2 +- ...mage_input.h => depth_image_topic_input.h} | 34 +++++---- ...t_impl.h => pointcloud_topic_input_impl.h} | 14 ++-- .../include/wavemap_ros/inputs/input_base.h | 70 ------------------ ...cloud_input.h => pointcloud_topic_input.h} | 36 ++++----- .../wavemap_ros/inputs/ros_input_base.h | 73 +++++++++++++++++++ .../{input_factory.h => ros_input_factory.h} | 16 ++-- .../include/wavemap_ros/ros_server.h | 14 ++-- ...ge_input.cc => depth_image_topic_input.cc} | 28 +++---- ...oud_input.cc => pointcloud_topic_input.cc} | 33 +++++---- .../{input_base.cc => ros_input_base.cc} | 16 ++-- ...{input_factory.cc => ros_input_factory.cc} | 28 +++---- interfaces/ros1/wavemap_ros/src/ros_server.cc | 15 ++-- .../wavemap/inputs/depth_image_input.json | 2 +- .../schemas/wavemap/inputs/input_base.json | 4 +- .../wavemap/inputs/pointcloud_input.json | 2 +- 25 files changed, 223 insertions(+), 208 deletions(-) rename interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/{depth_image_input.h => depth_image_topic_input.h} (78%) rename interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/impl/{pointcloud_input_impl.h => pointcloud_topic_input_impl.h} (68%) delete mode 100644 interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_base.h rename interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/{pointcloud_input.h => pointcloud_topic_input.h} (82%) create mode 100644 interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/ros_input_base.h rename interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/{input_factory.h => ros_input_factory.h} (58%) rename interfaces/ros1/wavemap_ros/src/inputs/{depth_image_input.cc => depth_image_topic_input.cc} (89%) rename interfaces/ros1/wavemap_ros/src/inputs/{pointcloud_input.cc => pointcloud_topic_input.cc} (92%) rename interfaces/ros1/wavemap_ros/src/inputs/{input_base.cc => ros_input_base.cc} (66%) rename interfaces/ros1/wavemap_ros/src/inputs/{input_factory.cc => ros_input_factory.cc} (66%) diff --git a/interfaces/ros1/wavemap_ros/CMakeLists.txt b/interfaces/ros1/wavemap_ros/CMakeLists.txt index d7388c1f1..fc4754734 100644 --- a/interfaces/ros1/wavemap_ros/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros/CMakeLists.txt @@ -19,10 +19,10 @@ enable_wavemap_general_tooling() # Libraries cs_add_library(${PROJECT_NAME} - src/inputs/depth_image_input.cc - src/inputs/input_base.cc - src/inputs/input_factory.cc - src/inputs/pointcloud_input.cc + src/inputs/depth_image_topic_input.cc + src/inputs/pointcloud_topic_input.cc + src/inputs/ros_input_base.cc + src/inputs/ros_input_factory.cc src/map_operations/crop_map_operation.cc src/map_operations/map_ros_operation_factory.cc src/map_operations/publish_map_operation.cc diff --git a/interfaces/ros1/wavemap_ros/app/rosbag_processor.cc b/interfaces/ros1/wavemap_ros/app/rosbag_processor.cc index 7045db1c4..132bef375 100644 --- a/interfaces/ros1/wavemap_ros/app/rosbag_processor.cc +++ b/interfaces/ros1/wavemap_ros/app/rosbag_processor.cc @@ -5,8 +5,8 @@ #include #include -#include "wavemap_ros/inputs/depth_image_input.h" -#include "wavemap_ros/inputs/pointcloud_input.h" +#include "wavemap_ros/inputs/depth_image_topic_input.h" +#include "wavemap_ros/inputs/pointcloud_topic_input.h" #include "wavemap_ros/ros_server.h" using namespace wavemap; // NOLINT @@ -38,18 +38,20 @@ int main(int argc, char** argv) { // Setup input handlers size_t input_idx = 0u; for (const auto& input : wavemap_server.getInputs()) { - if (auto pointcloud_input = dynamic_cast(input.get()); + if (auto pointcloud_input = + dynamic_cast(input.get()); pointcloud_input) { - PointcloudInput::registerCallback( + PointcloudTopicInput::registerCallback( pointcloud_input->getTopicType(), [&](auto callback_ptr) { rosbag_processor.addCallback(input->getTopicName(), callback_ptr, pointcloud_input); }); } else if (auto depth_image_input = - dynamic_cast(input.get()); + dynamic_cast(input.get()); depth_image_input) { rosbag_processor.addCallback( - input->getTopicName(), &DepthImageInput::callback, depth_image_input); + input->getTopicName(), &DepthImageTopicInput::callback, + depth_image_input); } else { ROS_WARN_STREAM( "Failed to register callback for input number " diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml index a1637fee4..c1cc45c54 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360.yaml @@ -41,7 +41,7 @@ measurement_integrators: max_range: { meters: 12.0 } inputs: - - type: pointcloud + - type: pointcloud_topic topic_name: "/livox/lidar" topic_type: livox measurement_integrator_names: livox_mid360 diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml index 89c79d6fc..6ac8f1cfb 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_azure_kinect.yaml @@ -82,7 +82,7 @@ measurement_integrators: max_update_resolution: { meters: 0.02 } inputs: - - type: depth_image + - type: depth_image_topic topic_name: "/depth/image_rect" measurement_integrator_names: [ kinect_short_range, kinect_medium_range ] depth_scale_factor: 0.001 @@ -91,7 +91,7 @@ inputs: time_offset: { seconds: -0.025 } # reprojected_topic_name: "/wavemap/kinect_input" - - type: pointcloud + - type: pointcloud_topic topic_name: "/livox/lidar" topic_type: livox measurement_integrator_names: livox_mid360 diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml index e299eda33..f943d8ce1 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_flexx.yaml @@ -103,7 +103,7 @@ measurement_integrators: max_update_resolution: { meters: 0.04 } inputs: - - type: depth_image + - type: depth_image_topic topic_name: "/pico_flexx/image_rect" measurement_integrator_names: [ monstar_short_range, monstar_medium_range, monstar_long_range ] topic_queue_length: 1 @@ -112,7 +112,7 @@ inputs: # reprojected_topic_name: "/wavemap/depth_cam_input" # depth_scale_factor: 0.001 - - type: pointcloud + - type: pointcloud_topic topic_name: "/livox/lidar" topic_type: livox measurement_integrator_names: livox_mid360 diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml index 299228e0e..57a1254dc 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_livox_mid360_pico_monstar.yaml @@ -83,7 +83,7 @@ measurement_integrators: max_update_resolution: { meters: 0.02 } inputs: - - type: depth_image + - type: depth_image_topic topic_name: "/pico_monstar/image_rect" measurement_integrator_names: [monstar_short_range, monstar_medium_range] topic_queue_length: 1 @@ -92,7 +92,7 @@ inputs: # depth_scale_factor: 0.001 # reprojected_pointcloud_topic_name: "/wavemap/depth_cam_input" - - type: pointcloud + - type: pointcloud_topic topic_name: "/livox/lidar" topic_type: livox measurement_integrator_names: livox_mid360 diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml index 429e460ed..c9752d0e9 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0.yaml @@ -43,7 +43,7 @@ measurement_integrators: max_range: { meters: 15.0 } inputs: - - type: pointcloud + - type: pointcloud_topic topic_name: "/os_cloud_node/points" topic_type: ouster measurement_integrator_names: ouster_os0 diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml index ea0289620..e8b3b880f 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os0_pico_monstar.yaml @@ -85,7 +85,7 @@ measurement_integrators: max_update_resolution: { meters: 0.02 } inputs: - - type: depth_image + - type: depth_image_topic topic_name: "/pico_monstar/image_rect" measurement_integrator_names: [ monstar_short_range, monstar_medium_range ] topic_queue_length: 1 @@ -94,7 +94,7 @@ inputs: # depth_scale_factor: 0.001 # reprojected_pointcloud_topic_name: "/wavemap/reprojected_pointcloud" - - type: pointcloud + - type: pointcloud_topic topic_name: "/os_cloud_node/points" topic_type: ouster measurement_integrator_names: ouster_os0 diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml index a6b95ad07..154a25749 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_os1.yaml @@ -41,7 +41,7 @@ measurement_integrators: max_range: { meters: 20.0 } inputs: - - type: pointcloud + - type: pointcloud_topic topic_name: "/os1_cloud_node/points" topic_type: ouster measurement_integrator_names: ouster_os1 diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml index 6a19837cc..85ace41fb 100644 --- a/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml +++ b/interfaces/ros1/wavemap_ros/config/wavemap_panoptic_mapping_rgbd.yaml @@ -36,7 +36,7 @@ measurement_integrators: max_range: { meters: 5.0 } inputs: - - type: depth_image + - type: depth_image_topic topic_name: "/data/depth_image" measurement_integrator_names: panoptic_mapping_camera topic_queue_length: 10 diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_topic_input.h similarity index 78% rename from interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_topic_input.h index caf02dd5e..e80f476f2 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_input.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/depth_image_topic_input.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ROS_INPUTS_DEPTH_IMAGE_INPUT_H_ -#define WAVEMAP_ROS_INPUTS_DEPTH_IMAGE_INPUT_H_ +#ifndef WAVEMAP_ROS_INPUTS_DEPTH_IMAGE_TOPIC_INPUT_H_ +#define WAVEMAP_ROS_INPUTS_DEPTH_IMAGE_TOPIC_INPUT_H_ #include #include @@ -13,14 +13,14 @@ #include #include -#include "wavemap_ros/inputs/input_base.h" +#include "wavemap_ros/inputs/ros_input_base.h" namespace wavemap { /** * Config struct for the depth image input handler. */ -struct DepthImageInputConfig - : public ConfigBase { +struct DepthImageTopicInputConfig + : public ConfigBase { //! Name of the ROS topic to subscribe to. std::string topic_name = "scan"; //! Queue length to use when subscribing to the ROS topic. @@ -60,7 +60,7 @@ struct DepthImageInputConfig static MemberMap memberMap; // Conversion to InputHandler base config - operator InputBaseConfig() const { // NOLINT + operator RosInputBaseConfig() const { // NOLINT return {topic_name, topic_queue_length, measurement_integrator_names, processing_retry_period}; } @@ -68,15 +68,17 @@ struct DepthImageInputConfig bool isValid(bool verbose) const override; }; -class DepthImageInput : public InputBase { +class DepthImageTopicInput : public RosInputBase { public: - DepthImageInput(const DepthImageInputConfig& config, - std::shared_ptr pipeline, - std::shared_ptr transformer, - std::string world_frame, ros::NodeHandle nh, - ros::NodeHandle nh_private); - - InputType getType() const override { return InputType::kDepthImage; } + DepthImageTopicInput(const DepthImageTopicInputConfig& config, + std::shared_ptr pipeline, + std::shared_ptr transformer, + std::string world_frame, ros::NodeHandle nh, + ros::NodeHandle nh_private); + + RosInputType getType() const override { + return RosInputType::kDepthImageTopic; + } void callback(const sensor_msgs::ImageConstPtr& depth_image_msg) { callback(*depth_image_msg); @@ -86,7 +88,7 @@ class DepthImageInput : public InputBase { } private: - const DepthImageInputConfig config_; + const DepthImageTopicInputConfig config_; Stopwatch integration_timer_; @@ -104,4 +106,4 @@ class DepthImageInput : public InputBase { }; } // namespace wavemap -#endif // WAVEMAP_ROS_INPUTS_DEPTH_IMAGE_INPUT_H_ +#endif // WAVEMAP_ROS_INPUTS_DEPTH_IMAGE_TOPIC_INPUT_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/impl/pointcloud_input_impl.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/impl/pointcloud_topic_input_impl.h similarity index 68% rename from interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/impl/pointcloud_input_impl.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/impl/pointcloud_topic_input_impl.h index df09860f9..112e792a0 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/impl/pointcloud_input_impl.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/impl/pointcloud_topic_input_impl.h @@ -1,17 +1,17 @@ -#ifndef WAVEMAP_ROS_INPUTS_IMPL_POINTCLOUD_INPUT_IMPL_H_ -#define WAVEMAP_ROS_INPUTS_IMPL_POINTCLOUD_INPUT_IMPL_H_ +#ifndef WAVEMAP_ROS_INPUTS_IMPL_POINTCLOUD_TOPIC_INPUT_IMPL_H_ +#define WAVEMAP_ROS_INPUTS_IMPL_POINTCLOUD_TOPIC_INPUT_IMPL_H_ namespace wavemap { template -bool PointcloudInput::registerCallback(PointcloudTopicType type, - RegistrarT registrar) { +bool PointcloudTopicInput::registerCallback(PointcloudTopicType type, + RegistrarT registrar) { switch (type) { case PointcloudTopicType::kPointCloud2: case PointcloudTopicType::kOuster: // clang-format off - registrar(static_cast( - &PointcloudInput::callback)); + &PointcloudTopicInput::callback)); // clang-format on return true; case PointcloudTopicType::kLivox: @@ -37,4 +37,4 @@ bool PointcloudInput::registerCallback(PointcloudTopicType type, } } // namespace wavemap -#endif // WAVEMAP_ROS_INPUTS_IMPL_POINTCLOUD_INPUT_IMPL_H_ +#endif // WAVEMAP_ROS_INPUTS_IMPL_POINTCLOUD_TOPIC_INPUT_IMPL_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_base.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_base.h deleted file mode 100644 index e07250e49..000000000 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_base.h +++ /dev/null @@ -1,70 +0,0 @@ -#ifndef WAVEMAP_ROS_INPUTS_INPUT_BASE_H_ -#define WAVEMAP_ROS_INPUTS_INPUT_BASE_H_ - -#include -#include -#include -#include - -#include -#include -#include - -#include "wavemap_ros/utils/tf_transformer.h" - -namespace wavemap { -struct InputType : public TypeSelector { - using TypeSelector::TypeSelector; - - enum Id : TypeId { kPointcloud, kDepthImage }; - - static constexpr std::array names = {"pointcloud", "depth_image"}; -}; - -struct InputBaseConfig : public ConfigBase { - std::string topic_name = "scan"; - int topic_queue_length = 10; - - StringList measurement_integrator_names; - - Seconds processing_retry_period = 0.05f; - - static MemberMap memberMap; - - // Constructors - InputBaseConfig() = default; - InputBaseConfig(std::string topic_name, int topic_queue_length, - StringList measurement_integrator_names, - FloatingPoint processing_retry_period) - : topic_name(std::move(topic_name)), - topic_queue_length(topic_queue_length), - measurement_integrator_names(std::move(measurement_integrator_names)), - processing_retry_period(processing_retry_period) {} - - bool isValid(bool verbose) const override; -}; - -class InputBase { - public: - InputBase(const InputBaseConfig& config, std::shared_ptr pipeline, - std::shared_ptr transformer, std::string world_frame, - const ros::NodeHandle& nh, ros::NodeHandle nh_private); - virtual ~InputBase() = default; - - virtual InputType getType() const = 0; - const std::string& getTopicName() { return config_.topic_name; } - - protected: - const InputBaseConfig config_; - - std::shared_ptr pipeline_; - - const std::shared_ptr transformer_; - const std::string world_frame_; - - virtual void processQueue() = 0; - ros::Timer queue_processing_retry_timer_; -}; -} // namespace wavemap - -#endif // WAVEMAP_ROS_INPUTS_INPUT_BASE_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_topic_input.h similarity index 82% rename from interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_topic_input.h index 9c9fb2018..6dad1b9b2 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_input.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_topic_input.h @@ -1,5 +1,5 @@ -#ifndef WAVEMAP_ROS_INPUTS_POINTCLOUD_INPUT_H_ -#define WAVEMAP_ROS_INPUTS_POINTCLOUD_INPUT_H_ +#ifndef WAVEMAP_ROS_INPUTS_POINTCLOUD_TOPIC_INPUT_H_ +#define WAVEMAP_ROS_INPUTS_POINTCLOUD_TOPIC_INPUT_H_ #include #include @@ -11,7 +11,7 @@ #include #include -#include "wavemap_ros/inputs/input_base.h" +#include "wavemap_ros/inputs/ros_input_base.h" #include "wavemap_ros/utils/pointcloud_undistorter.h" #ifdef LIVOX_AVAILABLE @@ -30,8 +30,8 @@ struct PointcloudTopicType : public TypeSelector { /** * Config struct for the pointcloud input handler. */ -struct PointcloudInputConfig - : public ConfigBase { //! Name of the ROS topic to subscribe to. std::string topic_name = "scan"; @@ -80,7 +80,7 @@ struct PointcloudInputConfig static MemberMap memberMap; // Conversion to InputHandler base config - operator InputBaseConfig() const { // NOLINT + operator RosInputBaseConfig() const { // NOLINT return {topic_name, topic_queue_length, measurement_integrator_names, processing_retry_period}; } @@ -88,15 +88,17 @@ struct PointcloudInputConfig bool isValid(bool verbose) const override; }; -class PointcloudInput : public InputBase { +class PointcloudTopicInput : public RosInputBase { public: - PointcloudInput(const PointcloudInputConfig& config, - std::shared_ptr pipeline, - std::shared_ptr transformer, - std::string world_frame, ros::NodeHandle nh, - ros::NodeHandle nh_private); - - InputType getType() const override { return InputType::kPointcloud; } + PointcloudTopicInput(const PointcloudTopicInputConfig& config, + std::shared_ptr pipeline, + std::shared_ptr transformer, + std::string world_frame, ros::NodeHandle nh, + ros::NodeHandle nh_private); + + RosInputType getType() const override { + return RosInputType::kPointcloudTopic; + } PointcloudTopicType getTopicType() const { return config_.topic_type; } void callback(const sensor_msgs::PointCloud2& pointcloud_msg); @@ -108,7 +110,7 @@ class PointcloudInput : public InputBase { static bool registerCallback(PointcloudTopicType type, RegistrarT registrar); private: - const PointcloudInputConfig config_; + const PointcloudTopicInputConfig config_; Stopwatch integration_timer_; @@ -131,6 +133,6 @@ class PointcloudInput : public InputBase { }; } // namespace wavemap -#include "wavemap_ros/inputs/impl/pointcloud_input_impl.h" +#include "wavemap_ros/inputs/impl/pointcloud_topic_input_impl.h" -#endif // WAVEMAP_ROS_INPUTS_POINTCLOUD_INPUT_H_ +#endif // WAVEMAP_ROS_INPUTS_POINTCLOUD_TOPIC_INPUT_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/ros_input_base.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/ros_input_base.h new file mode 100644 index 000000000..850adb515 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/ros_input_base.h @@ -0,0 +1,73 @@ +#ifndef WAVEMAP_ROS_INPUTS_ROS_INPUT_BASE_H_ +#define WAVEMAP_ROS_INPUTS_ROS_INPUT_BASE_H_ + +#include +#include +#include +#include + +#include +#include +#include + +#include "wavemap_ros/utils/tf_transformer.h" + +namespace wavemap { +struct RosInputType : public TypeSelector { + using TypeSelector::TypeSelector; + + enum Id : TypeId { kPointcloudTopic, kDepthImageTopic }; + + static constexpr std::array names = {"pointcloud_topic", "depth_image_topic"}; +}; + +struct RosInputBaseConfig + : public ConfigBase { + std::string topic_name = "scan"; + int topic_queue_length = 10; + + StringList measurement_integrator_names; + + Seconds processing_retry_period = 0.05f; + + static MemberMap memberMap; + + // Constructors + RosInputBaseConfig() = default; + RosInputBaseConfig(std::string topic_name, int topic_queue_length, + StringList measurement_integrator_names, + FloatingPoint processing_retry_period) + : topic_name(std::move(topic_name)), + topic_queue_length(topic_queue_length), + measurement_integrator_names(std::move(measurement_integrator_names)), + processing_retry_period(processing_retry_period) {} + + bool isValid(bool verbose) const override; +}; + +class RosInputBase { + public: + RosInputBase(const RosInputBaseConfig& config, + std::shared_ptr pipeline, + std::shared_ptr transformer, + std::string world_frame, const ros::NodeHandle& nh, + ros::NodeHandle nh_private); + virtual ~RosInputBase() = default; + + virtual RosInputType getType() const = 0; + const std::string& getTopicName() { return config_.topic_name; } + + protected: + const RosInputBaseConfig config_; + + std::shared_ptr pipeline_; + + const std::shared_ptr transformer_; + const std::string world_frame_; + + virtual void processQueue() = 0; + ros::Timer queue_processing_retry_timer_; +}; +} // namespace wavemap + +#endif // WAVEMAP_ROS_INPUTS_ROS_INPUT_BASE_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_factory.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/ros_input_factory.h similarity index 58% rename from interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_factory.h rename to interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/ros_input_factory.h index 02591b2fa..34741324a 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/input_factory.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/ros_input_factory.h @@ -1,26 +1,26 @@ -#ifndef WAVEMAP_ROS_INPUTS_INPUT_FACTORY_H_ -#define WAVEMAP_ROS_INPUTS_INPUT_FACTORY_H_ +#ifndef WAVEMAP_ROS_INPUTS_ROS_INPUT_FACTORY_H_ +#define WAVEMAP_ROS_INPUTS_ROS_INPUT_FACTORY_H_ #include #include #include "wavemap/core/utils/thread_pool.h" -#include "wavemap_ros/inputs/input_base.h" +#include "wavemap_ros/inputs/ros_input_base.h" namespace wavemap { -class InputFactory { +class RosInputFactory { public: - static std::unique_ptr create( + static std::unique_ptr create( const param::Value& params, std::shared_ptr pipeline, std::shared_ptr transformer, std::string world_frame, ros::NodeHandle nh, ros::NodeHandle nh_private); - static std::unique_ptr create( - InputType input_type, const param::Value& params, + static std::unique_ptr create( + RosInputType input_type, const param::Value& params, std::shared_ptr pipeline, std::shared_ptr transformer, std::string world_frame, ros::NodeHandle nh, ros::NodeHandle nh_private); }; } // namespace wavemap -#endif // WAVEMAP_ROS_INPUTS_INPUT_FACTORY_H_ +#endif // WAVEMAP_ROS_INPUTS_ROS_INPUT_FACTORY_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h index 2c47b8606..7771d6f75 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h @@ -17,7 +17,7 @@ #include #include -#include "wavemap_ros/inputs/input_base.h" +#include "wavemap_ros/inputs/ros_input_base.h" #include "wavemap_ros/utils/logging_level.h" #include "wavemap_ros/utils/tf_transformer.h" @@ -60,10 +60,12 @@ class RosServer { MapOperationBase* addOperation(const param::Value& operation_params, ros::NodeHandle nh_private); - InputBase* addInput(const param::Value& integrator_params, - const ros::NodeHandle& nh, ros::NodeHandle nh_private); - InputBase* addInput(std::unique_ptr input); - const std::vector>& getInputs() { return inputs_; } + RosInputBase* addInput(const param::Value& integrator_params, + const ros::NodeHandle& nh, ros::NodeHandle nh_private); + RosInputBase* addInput(std::unique_ptr input); + const std::vector>& getInputs() { + return inputs_; + } void clearInputs() { inputs_.clear(); } bool saveMap(const std::filesystem::path& file_path) const; @@ -82,7 +84,7 @@ class RosServer { std::shared_ptr pipeline_; // Measurement and pose inputs - std::vector> inputs_; + std::vector> inputs_; std::shared_ptr transformer_; // ROS services diff --git a/interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/depth_image_topic_input.cc similarity index 89% rename from interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc rename to interfaces/ros1/wavemap_ros/src/inputs/depth_image_topic_input.cc index b0e49b021..31b44ee42 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/depth_image_input.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/depth_image_topic_input.cc @@ -1,4 +1,4 @@ -#include "wavemap_ros/inputs/depth_image_input.h" +#include "wavemap_ros/inputs/depth_image_topic_input.h" #include #include @@ -11,7 +11,7 @@ #include namespace wavemap { -DECLARE_CONFIG_MEMBERS(DepthImageInputConfig, +DECLARE_CONFIG_MEMBERS(DepthImageTopicInputConfig, (topic_name) (topic_queue_length) (measurement_integrator_names) @@ -23,7 +23,7 @@ DECLARE_CONFIG_MEMBERS(DepthImageInputConfig, (time_offset) (projected_pointcloud_topic_name)); -bool DepthImageInputConfig::isValid(bool verbose) const { +bool DepthImageTopicInputConfig::isValid(bool verbose) const { bool all_valid = true; all_valid &= IS_PARAM_NE(topic_name, std::string(""), verbose); @@ -35,19 +35,19 @@ bool DepthImageInputConfig::isValid(bool verbose) const { return all_valid; } -DepthImageInput::DepthImageInput(const DepthImageInputConfig& config, - std::shared_ptr pipeline, - std::shared_ptr transformer, - std::string world_frame, ros::NodeHandle nh, - ros::NodeHandle nh_private) - : InputBase(config, std::move(pipeline), std::move(transformer), - std::move(world_frame), nh, nh_private), +DepthImageTopicInput::DepthImageTopicInput( + const DepthImageTopicInputConfig& config, + std::shared_ptr pipeline, + std::shared_ptr transformer, std::string world_frame, + ros::NodeHandle nh, ros::NodeHandle nh_private) + : RosInputBase(config, std::move(pipeline), std::move(transformer), + std::move(world_frame), nh, nh_private), config_(config.checkValid()) { // Subscribe to the depth image input image_transport::ImageTransport it(nh); depth_image_sub_ = it.subscribe( config_.topic_name, config_.topic_queue_length, - &DepthImageInput::callback, this, + &DepthImageTopicInput::callback, this, image_transport::TransportHints(config_.image_transport_hints)); // Advertise the projected pointcloud publisher if enabled @@ -57,7 +57,7 @@ DepthImageInput::DepthImageInput(const DepthImageInputConfig& config, } } -void DepthImageInput::processQueue() { +void DepthImageTopicInput::processQueue() { ProfilerZoneScoped; while (!depth_image_queue_.empty()) { const sensor_msgs::Image& oldest_msg = depth_image_queue_.front(); @@ -125,7 +125,7 @@ void DepthImageInput::processQueue() { } } -void DepthImageInput::publishProjectedPointcloudIfEnabled( +void DepthImageTopicInput::publishProjectedPointcloudIfEnabled( const ros::Time& stamp, const PosedImage& /*posed_depth_image*/) { ProfilerZoneScoped; @@ -161,7 +161,7 @@ void DepthImageInput::publishProjectedPointcloudIfEnabled( // pointcloud2_msg); projected_pointcloud_pub_.publish(pointcloud2_msg); } -PosedPointcloud DepthImageInput::project( +PosedPointcloud DepthImageTopicInput::project( const PosedImage<>& posed_depth_image, const ProjectorBase& projection_model) { ProfilerZoneScoped; diff --git a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc similarity index 92% rename from interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc rename to interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc index afc48af47..f1c597076 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_input.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc @@ -1,4 +1,4 @@ -#include "wavemap_ros/inputs/pointcloud_input.h" +#include "wavemap_ros/inputs/pointcloud_topic_input.h" #include #include @@ -12,7 +12,7 @@ #include namespace wavemap { -DECLARE_CONFIG_MEMBERS(PointcloudInputConfig, +DECLARE_CONFIG_MEMBERS(PointcloudTopicInputConfig, (topic_name) (topic_type) (topic_queue_length) @@ -26,7 +26,7 @@ DECLARE_CONFIG_MEMBERS(PointcloudInputConfig, (projected_range_image_topic_name) (undistorted_pointcloud_topic_name)); -bool PointcloudInputConfig::isValid(bool verbose) const { +bool PointcloudTopicInputConfig::isValid(bool verbose) const { bool all_valid = true; all_valid &= IS_PARAM_NE(topic_name, std::string(""), verbose); @@ -38,13 +38,13 @@ bool PointcloudInputConfig::isValid(bool verbose) const { return all_valid; } -PointcloudInput::PointcloudInput(const PointcloudInputConfig& config, - std::shared_ptr pipeline, - std::shared_ptr transformer, - std::string world_frame, ros::NodeHandle nh, - ros::NodeHandle nh_private) - : InputBase(config, std::move(pipeline), transformer, - std::move(world_frame), nh, nh_private), +PointcloudTopicInput::PointcloudTopicInput( + const PointcloudTopicInputConfig& config, + std::shared_ptr pipeline, + std::shared_ptr transformer, std::string world_frame, + ros::NodeHandle nh, ros::NodeHandle nh_private) + : RosInputBase(config, std::move(pipeline), transformer, + std::move(world_frame), nh, nh_private), config_(config.checkValid()), pointcloud_undistorter_( transformer, @@ -69,7 +69,8 @@ PointcloudInput::PointcloudInput(const PointcloudInputConfig& config, } } -void PointcloudInput::callback(const sensor_msgs::PointCloud2& pointcloud_msg) { +void PointcloudTopicInput::callback( + const sensor_msgs::PointCloud2& pointcloud_msg) { ProfilerZoneScoped; // Skip empty clouds const size_t num_points = pointcloud_msg.height * pointcloud_msg.width; @@ -172,7 +173,7 @@ void PointcloudInput::callback( } #endif -void PointcloudInput::processQueue() { +void PointcloudTopicInput::processQueue() { ProfilerZoneScoped; while (!pointcloud_queue_.empty()) { auto& oldest_msg = pointcloud_queue_.front(); @@ -274,8 +275,8 @@ void PointcloudInput::processQueue() { } } -bool PointcloudInput::hasField(const sensor_msgs::PointCloud2& msg, - const std::string& field_name) { +bool PointcloudTopicInput::hasField(const sensor_msgs::PointCloud2& msg, + const std::string& field_name) { return std::any_of(msg.fields.cbegin(), msg.fields.cend(), [&field_name = std::as_const(field_name)]( const sensor_msgs::PointField& field) { @@ -283,7 +284,7 @@ bool PointcloudInput::hasField(const sensor_msgs::PointCloud2& msg, }); } -void PointcloudInput::publishProjectedRangeImageIfEnabled( +void PointcloudTopicInput::publishProjectedRangeImageIfEnabled( const ros::Time& /*stamp*/, const PosedPointcloud<>& /*posed_pointcloud*/) { ProfilerZoneScoped; if (config_.projected_range_image_topic_name.empty() || @@ -307,7 +308,7 @@ void PointcloudInput::publishProjectedRangeImageIfEnabled( // projected_range_image_pub_.publish(cv_image.toImageMsg()); } -void PointcloudInput::publishUndistortedPointcloudIfEnabled( +void PointcloudTopicInput::publishUndistortedPointcloudIfEnabled( const ros::Time& stamp, const PosedPointcloud<>& undistorted_pointcloud) { ProfilerZoneScoped; if (config_.undistorted_pointcloud_topic_name.empty() || diff --git a/interfaces/ros1/wavemap_ros/src/inputs/input_base.cc b/interfaces/ros1/wavemap_ros/src/inputs/ros_input_base.cc similarity index 66% rename from interfaces/ros1/wavemap_ros/src/inputs/input_base.cc rename to interfaces/ros1/wavemap_ros/src/inputs/ros_input_base.cc index 2bee4db81..e2855ea1c 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/input_base.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/ros_input_base.cc @@ -1,13 +1,13 @@ -#include "wavemap_ros/inputs/input_base.h" +#include "wavemap_ros/inputs/ros_input_base.h" namespace wavemap { -DECLARE_CONFIG_MEMBERS(InputBaseConfig, +DECLARE_CONFIG_MEMBERS(RosInputBaseConfig, (topic_name) (topic_queue_length) (measurement_integrator_names) (processing_retry_period)); -bool InputBaseConfig::isValid(bool verbose) const { +bool RosInputBaseConfig::isValid(bool verbose) const { bool all_valid = true; all_valid &= IS_PARAM_NE(topic_name, std::string(""), verbose); @@ -18,11 +18,11 @@ bool InputBaseConfig::isValid(bool verbose) const { return all_valid; } -InputBase::InputBase(const InputBaseConfig& config, - std::shared_ptr pipeline, - std::shared_ptr transformer, - std::string world_frame, const ros::NodeHandle& nh, - ros::NodeHandle /*nh_private*/) +RosInputBase::RosInputBase(const RosInputBaseConfig& config, + std::shared_ptr pipeline, + std::shared_ptr transformer, + std::string world_frame, const ros::NodeHandle& nh, + ros::NodeHandle /*nh_private*/) : config_(config.checkValid()), pipeline_(std::move(pipeline)), transformer_(std::move(transformer)), diff --git a/interfaces/ros1/wavemap_ros/src/inputs/input_factory.cc b/interfaces/ros1/wavemap_ros/src/inputs/ros_input_factory.cc similarity index 66% rename from interfaces/ros1/wavemap_ros/src/inputs/input_factory.cc rename to interfaces/ros1/wavemap_ros/src/inputs/ros_input_factory.cc index 2e4452e91..c4ff7bedb 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/input_factory.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/ros_input_factory.cc @@ -1,14 +1,14 @@ -#include "wavemap_ros/inputs/input_factory.h" +#include "wavemap_ros/inputs/ros_input_factory.h" -#include "wavemap_ros/inputs/depth_image_input.h" -#include "wavemap_ros/inputs/pointcloud_input.h" +#include "wavemap_ros/inputs/depth_image_topic_input.h" +#include "wavemap_ros/inputs/pointcloud_topic_input.h" namespace wavemap { -std::unique_ptr InputFactory::create( +std::unique_ptr RosInputFactory::create( const param::Value& params, std::shared_ptr pipeline, std::shared_ptr transformer, std::string world_frame, ros::NodeHandle nh, ros::NodeHandle nh_private) { - if (const auto type = InputType::from(params); type) { + if (const auto type = RosInputType::from(params); type) { return create(type.value(), params, std::move(pipeline), std::move(transformer), std::move(world_frame), nh, nh_private); @@ -18,8 +18,8 @@ std::unique_ptr InputFactory::create( return nullptr; } -std::unique_ptr InputFactory::create( - InputType input_type, const param::Value& params, +std::unique_ptr RosInputFactory::create( + RosInputType input_type, const param::Value& params, std::shared_ptr pipeline, std::shared_ptr transformer, std::string world_frame, ros::NodeHandle nh, ros::NodeHandle nh_private) { @@ -30,18 +30,20 @@ std::unique_ptr InputFactory::create( // Create the input handler switch (input_type) { - case InputType::kPointcloud: - if (const auto config = PointcloudInputConfig::from(params); config) { - return std::make_unique( + case RosInputType::kPointcloudTopic: + if (const auto config = PointcloudTopicInputConfig::from(params); + config) { + return std::make_unique( config.value(), std::move(pipeline), std::move(transformer), std::move(world_frame), nh, nh_private); } else { ROS_ERROR("Pointcloud input handler config could not be loaded."); return nullptr; } - case InputType::kDepthImage: - if (const auto config = DepthImageInputConfig::from(params); config) { - return std::make_unique( + case RosInputType::kDepthImageTopic: + if (const auto config = DepthImageTopicInputConfig::from(params); + config) { + return std::make_unique( config.value(), std::move(pipeline), std::move(transformer), std::move(world_frame), nh, nh_private); } else { diff --git a/interfaces/ros1/wavemap_ros/src/ros_server.cc b/interfaces/ros1/wavemap_ros/src/ros_server.cc index 68142ae3c..77fe090e1 100644 --- a/interfaces/ros1/wavemap_ros/src/ros_server.cc +++ b/interfaces/ros1/wavemap_ros/src/ros_server.cc @@ -7,7 +7,7 @@ #include #include -#include "wavemap_ros/inputs/input_factory.h" +#include "wavemap_ros/inputs/ros_input_factory.h" #include "wavemap_ros/map_operations/map_ros_operation_factory.h" namespace wavemap { @@ -98,15 +98,16 @@ void RosServer::clear() { } } -InputBase* RosServer::addInput(const param::Value& integrator_params, - const ros::NodeHandle& nh, - ros::NodeHandle nh_private) { - auto input = InputFactory::create(integrator_params, pipeline_, transformer_, - config_.world_frame, nh, nh_private); +RosInputBase* RosServer::addInput(const param::Value& integrator_params, + const ros::NodeHandle& nh, + ros::NodeHandle nh_private) { + auto input = + RosInputFactory::create(integrator_params, pipeline_, transformer_, + config_.world_frame, nh, nh_private); return addInput(std::move(input)); } -InputBase* RosServer::addInput(std::unique_ptr input) { +RosInputBase* RosServer::addInput(std::unique_ptr input) { if (input) { return inputs_.emplace_back(std::move(input)).get(); } diff --git a/tooling/schemas/wavemap/inputs/depth_image_input.json b/tooling/schemas/wavemap/inputs/depth_image_input.json index 1d588f292..5cd74b861 100644 --- a/tooling/schemas/wavemap/inputs/depth_image_input.json +++ b/tooling/schemas/wavemap/inputs/depth_image_input.json @@ -9,7 +9,7 @@ ], "properties": { "type": { - "const": "depth_image" + "const": "depth_image_topic" }, "topic_name": { "description": "Name of the ROS topic to subscribe to.", diff --git a/tooling/schemas/wavemap/inputs/input_base.json b/tooling/schemas/wavemap/inputs/input_base.json index 8827311cd..9682e779e 100644 --- a/tooling/schemas/wavemap/inputs/input_base.json +++ b/tooling/schemas/wavemap/inputs/input_base.json @@ -10,8 +10,8 @@ "description": "Type of the measurement input handler that should be used.", "type": "string", "enum": [ - "pointcloud", - "depth_image" + "pointcloud_topic", + "depth_image_topic" ] } }, diff --git a/tooling/schemas/wavemap/inputs/pointcloud_input.json b/tooling/schemas/wavemap/inputs/pointcloud_input.json index f7e48c9cb..56e2d5f78 100644 --- a/tooling/schemas/wavemap/inputs/pointcloud_input.json +++ b/tooling/schemas/wavemap/inputs/pointcloud_input.json @@ -10,7 +10,7 @@ ], "properties": { "type": { - "const": "pointcloud" + "const": "pointcloud_topic" }, "topic_name": { "description": "Name of the ROS topic to subscribe to.", From 55a9dc3eeacd005d0f8dec5211ec9600f3050257 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Tue, 6 Aug 2024 18:57:47 +0200 Subject: [PATCH 132/159] Restructure docs and extend C++ and ROS1 specific tutorials --- .github/workflows/cd.yml | 10 + .github/workflows/ci.yml | 10 + .gitignore | 5 +- docs/{Doxyfile => Doxyfile_cpp} | 10 +- docs/Doxyfile_ros1 | 2538 +++++++++++++++++ docs/Makefile | 2 +- docs/conf.py | 14 +- docs/latex_index.rst | 4 +- docs/pages/configuration/general.rst | 2 - docs/pages/configuration/index.rst | 34 - docs/pages/configuration/map_operations.rst | 2 - .../configuration/measurement_integrators.rst | 2 - docs/pages/demos.rst | 8 +- docs/pages/faq.rst | 2 +- docs/pages/installation/cmake.rst | 62 +- docs/pages/installation/index.rst | 11 +- docs/pages/installation/python.rst | 1 - docs/pages/installation/ros1.rst | 24 +- docs/pages/intro.rst | 15 +- docs/pages/parameters/general.rst | 14 + docs/pages/parameters/index.rst | 38 + docs/pages/parameters/inputs.rst | 36 + .../{configuration => parameters}/map.rst | 34 +- docs/pages/parameters/map_operations.rst | 40 + .../measurement_integrators.rst} | 63 +- docs/pages/tutorials/cpp.rst | 91 + .../{usage_examples => tutorials}/index.rst | 7 +- .../{usage_examples => tutorials}/python.rst | 1 - docs/pages/tutorials/ros1.rst | 97 + docs/pages/usage_examples/cpp.rst | 61 - docs/pages/usage_examples/ros1.rst | 33 - .../wavemap_utils/scripts/preview_docs.sh | 9 +- 32 files changed, 2970 insertions(+), 310 deletions(-) rename docs/{Doxyfile => Doxyfile_cpp} (99%) create mode 100644 docs/Doxyfile_ros1 delete mode 100644 docs/pages/configuration/general.rst delete mode 100644 docs/pages/configuration/index.rst delete mode 100644 docs/pages/configuration/map_operations.rst delete mode 100644 docs/pages/configuration/measurement_integrators.rst create mode 100644 docs/pages/parameters/general.rst create mode 100644 docs/pages/parameters/index.rst create mode 100644 docs/pages/parameters/inputs.rst rename docs/pages/{configuration => parameters}/map.rst (66%) create mode 100644 docs/pages/parameters/map_operations.rst rename docs/pages/{configuration/inputs.rst => parameters/measurement_integrators.rst} (75%) create mode 100644 docs/pages/tutorials/cpp.rst rename docs/pages/{usage_examples => tutorials}/index.rst (71%) rename docs/pages/{usage_examples => tutorials}/python.rst (99%) create mode 100644 docs/pages/tutorials/ros1.rst delete mode 100644 docs/pages/usage_examples/cpp.rst delete mode 100644 docs/pages/usage_examples/ros1.rst diff --git a/.github/workflows/cd.yml b/.github/workflows/cd.yml index f6161a1b6..167a4cd3f 100644 --- a/.github/workflows/cd.yml +++ b/.github/workflows/cd.yml @@ -156,6 +156,16 @@ jobs: pip3 install sphinxawesome-theme --pre pip3 install "sphinx<7,>6" + - name: Parse C++ library with Doxygen + working-directory: ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs + shell: bash + run: doxygen Doxyfile_cpp + + - name: Parse ROS1 interface with Doxygen + working-directory: ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs + shell: bash + run: doxygen Doxyfile_ros1 + - name: Build documentation site working-directory: ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs shell: bash diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 497a90363..6876acec6 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -160,6 +160,16 @@ jobs: pip3 install sphinxawesome-theme --pre pip3 install "sphinx<7,>6" + - name: Parse C++ library with Doxygen + working-directory: ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs + shell: bash + run: doxygen Doxyfile_cpp + + - name: Parse ROS1 interface with Doxygen + working-directory: ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs + shell: bash + run: doxygen Doxyfile_ros1 + - name: Build documentation site working-directory: ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs shell: bash diff --git a/.gitignore b/.gitignore index 0a56c8aa8..136aa91d0 100644 --- a/.gitignore +++ b/.gitignore @@ -55,6 +55,7 @@ CATKIN_IGNORE **cmake-build-* # Docs -docs/api -docs/_doxygen +docs/cpp_api +docs/_doxygen_cpp +docs/_doxygen_ros1 docs/_build diff --git a/docs/Doxyfile b/docs/Doxyfile_cpp similarity index 99% rename from docs/Doxyfile rename to docs/Doxyfile_cpp index 35a9a0101..033fcc11b 100644 --- a/docs/Doxyfile +++ b/docs/Doxyfile_cpp @@ -32,7 +32,7 @@ DOXYFILE_ENCODING = UTF-8 # title of most generated pages and in a few other places. # The default value is: My Project. -PROJECT_NAME = "wavemap" +PROJECT_NAME = "wavemap_cpp" # The PROJECT_NUMBER tag can be used to enter a project or revision number. This # could be handy for archiving the generated documentation or if some version @@ -58,7 +58,7 @@ PROJECT_LOGO = # entered, it will be relative to the location where doxygen was started. If # left blank the current directory will be used. -OUTPUT_DIRECTORY = ./_doxygen +OUTPUT_DIRECTORY = ./_doxygen_cpp # If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub- # directories (in 2 levels) under the output directory of each output format and @@ -762,7 +762,7 @@ CITE_BIB_FILES = # messages are off. # The default value is: NO. -QUIET = NO +QUIET = YES # The WARNINGS tag can be used to turn on/off the warning messages that are # generated to standard error (stderr) by doxygen. If WARNINGS is set to YES @@ -829,7 +829,7 @@ WARN_LOGFILE = # spaces. See also FILE_PATTERNS and EXTENSION_MAPPING # Note: If this tag is empty the current directory is searched. -INPUT = ../ +INPUT = ../library # This tag can be used to specify the character encoding of the source files # that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses @@ -875,7 +875,7 @@ RECURSIVE = YES # Note that relative paths are relative to the directory from which doxygen is # run. -EXCLUDE = +EXCLUDE = ../library/include/wavemap/3rd_party # The EXCLUDE_SYMLINKS tag can be used to select whether or not files or # directories that are symbolic links (a Unix file system feature) are excluded diff --git a/docs/Doxyfile_ros1 b/docs/Doxyfile_ros1 new file mode 100644 index 000000000..8ca75caab --- /dev/null +++ b/docs/Doxyfile_ros1 @@ -0,0 +1,2538 @@ +# Doxyfile 1.8.17 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project. +# +# All text after a double hash (##) is considered a comment and is placed in +# front of the TAG it is preceding. +# +# All text after a single hash (#) is considered a comment and will be ignored. +# The format is: +# TAG = value [value, ...] +# For lists, items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (\" \"). + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the configuration +# file that follow. The default is UTF-8 which is also the encoding used for all +# text before the first occurrence of this tag. Doxygen uses libiconv (or the +# iconv built into libc) for the transcoding. See +# https://www.gnu.org/software/libiconv/ for the list of possible encodings. +# The default value is: UTF-8. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by +# double-quotes, unless you are using Doxywizard) that should identify the +# project for which the documentation is generated. This name is used in the +# title of most generated pages and in a few other places. +# The default value is: My Project. + +PROJECT_NAME = "wavemap_ros1" + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. This +# could be handy for archiving the generated documentation or if some version +# control system is used. + +PROJECT_NUMBER = + +# Using the PROJECT_BRIEF tag one can provide an optional one line description +# for a project that appears at the top of each page and should give viewer a +# quick idea about the purpose of the project. Keep the description short. + +PROJECT_BRIEF = + +# With the PROJECT_LOGO tag one can specify a logo or an icon that is included +# in the documentation. The maximum height of the logo should not exceed 55 +# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy +# the logo to the output directory. + +PROJECT_LOGO = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path +# into which the generated documentation will be written. If a relative path is +# entered, it will be relative to the location where doxygen was started. If +# left blank the current directory will be used. + +OUTPUT_DIRECTORY = ./_doxygen_ros1 + +# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub- +# directories (in 2 levels) under the output directory of each output format and +# will distribute the generated files over these directories. Enabling this +# option can be useful when feeding doxygen a huge amount of source files, where +# putting all generated files in the same directory would otherwise causes +# performance problems for the file system. +# The default value is: NO. + +CREATE_SUBDIRS = NO + +# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII +# characters to appear in the names of generated files. If set to NO, non-ASCII +# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode +# U+3044. +# The default value is: NO. + +ALLOW_UNICODE_NAMES = NO + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese, +# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States), +# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian, +# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages), +# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian, +# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian, +# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish, +# Ukrainian and Vietnamese. +# The default value is: English. + +OUTPUT_LANGUAGE = English + +# The OUTPUT_TEXT_DIRECTION tag is used to specify the direction in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all generated output in the proper direction. +# Possible values are: None, LTR, RTL and Context. +# The default value is: None. + +OUTPUT_TEXT_DIRECTION = None + +# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member +# descriptions after the members that are listed in the file and class +# documentation (similar to Javadoc). Set to NO to disable this. +# The default value is: YES. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief +# description of a member or function before the detailed description +# +# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. +# The default value is: YES. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator that is +# used to form the text in various listings. Each string in this list, if found +# as the leading text of the brief description, will be stripped from the text +# and the result, after processing the whole list, is used as the annotated +# text. Otherwise, the brief description is used as-is. If left blank, the +# following values are used ($name is automatically replaced with the name of +# the entity):The $name class, The $name widget, The $name file, is, provides, +# specifies, contains, represents, a, an and the. + +ABBREVIATE_BRIEF = "The $name class" \ + "The $name widget" \ + "The $name file" \ + is \ + provides \ + specifies \ + contains \ + represents \ + a \ + an \ + the + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# doxygen will generate a detailed section even if there is only a brief +# description. +# The default value is: NO. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. +# The default value is: NO. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path +# before files name in the file list and in the header files. If set to NO the +# shortest path that makes the file name unique will be used +# The default value is: YES. + +FULL_PATH_NAMES = YES + +# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path. +# Stripping is only done if one of the specified strings matches the left-hand +# part of the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the path to +# strip. +# +# Note that you can specify absolute paths here, but also relative paths, which +# will be relative from the directory where doxygen is started. +# This tag requires that the tag FULL_PATH_NAMES is set to YES. + +STRIP_FROM_PATH = .. + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the +# path mentioned in the documentation of a class, which tells the reader which +# header file to include in order to use a class. If left blank only the name of +# the header file containing the class definition is used. Otherwise one should +# specify the list of include paths that are normally passed to the compiler +# using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but +# less readable) file names. This can be useful is your file systems doesn't +# support long names like on DOS, Mac, or CD-ROM. +# The default value is: NO. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the +# first line (until the first dot) of a Javadoc-style comment as the brief +# description. If set to NO, the Javadoc-style will behave just like regular Qt- +# style comments (thus requiring an explicit @brief command for a brief +# description.) +# The default value is: NO. + +JAVADOC_AUTOBRIEF = YES + +# If the JAVADOC_BANNER tag is set to YES then doxygen will interpret a line +# such as +# /*************** +# as being the beginning of a Javadoc-style comment "banner". If set to NO, the +# Javadoc-style will behave just like regular comments and it will not be +# interpreted by doxygen. +# The default value is: NO. + +JAVADOC_BANNER = NO + +# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first +# line (until the first dot) of a Qt-style comment as the brief description. If +# set to NO, the Qt-style will behave just like regular Qt-style comments (thus +# requiring an explicit \brief command for a brief description.) +# The default value is: NO. + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a +# multi-line C++ special comment block (i.e. a block of //! or /// comments) as +# a brief description. This used to be the default behavior. The new default is +# to treat a multi-line C++ comment block as a detailed description. Set this +# tag to YES if you prefer the old behavior instead. +# +# Note that setting this tag to YES also means that rational rose comments are +# not recognized any more. +# The default value is: NO. + +MULTILINE_CPP_IS_BRIEF = NO + +# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the +# documentation from any documented member that it re-implements. +# The default value is: YES. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new +# page for each member. If set to NO, the documentation of a member will be part +# of the file/class/namespace that contains it. +# The default value is: NO. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen +# uses this value to replace tabs by spaces in code fragments. +# Minimum value: 1, maximum value: 16, default value: 4. + +TAB_SIZE = 2 + +# This tag can be used to specify a number of aliases that act as commands in +# the documentation. An alias has the form: +# name=value +# For example adding +# "sideeffect=@par Side Effects:\n" +# will allow you to put the command \sideeffect (or @sideeffect) in the +# documentation, which will result in a user-defined paragraph with heading +# "Side Effects:". You can put \n's in the value part of an alias to insert +# newlines (in the resulting output). You can put ^^ in the value part of an +# alias to insert a newline as if a physical newline was in the original file. +# When you need a literal { or } or , in the value part of an alias you have to +# escape them by means of a backslash (\), this can lead to conflicts with the +# commands \{ and \} for these it is advised to use the version @{ and @} or use +# a double escape (\\{ and \\}) + +ALIASES = + +# This tag can be used to specify a number of word-keyword mappings (TCL only). +# A mapping has the form "name=value". For example adding "class=itcl::class" +# will allow you to use the command class in the itcl::class meaning. + +TCL_SUBST = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources +# only. Doxygen will then generate output that is more tailored for C. For +# instance, some of the names that are used will be different. The list of all +# members will be omitted, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_FOR_C = NO + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or +# Python sources only. Doxygen will then generate output that is more tailored +# for that language. For instance, namespaces will be presented as packages, +# qualified scopes will look different, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources. Doxygen will then generate output that is tailored for Fortran. +# The default value is: NO. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for VHDL. +# The default value is: NO. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Set the OPTIMIZE_OUTPUT_SLICE tag to YES if your project consists of Slice +# sources only. Doxygen will then generate output that is more tailored for that +# language. For instance, namespaces will be presented as modules, types will be +# separated into more groups, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_SLICE = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given +# extension. Doxygen has a built-in mapping, but you can override or extend it +# using this tag. The format is ext=language, where ext is a file extension, and +# language is one of the parsers supported by doxygen: IDL, Java, JavaScript, +# Csharp (C#), C, C++, D, PHP, md (Markdown), Objective-C, Python, Slice, +# Fortran (fixed format Fortran: FortranFixed, free formatted Fortran: +# FortranFree, unknown formatted Fortran: Fortran. In the later case the parser +# tries to guess whether the code is fixed or free formatted code, this is the +# default for Fortran type files), VHDL, tcl. For instance to make doxygen treat +# .inc files as Fortran files (default is PHP), and .f files as C (default is +# Fortran), use: inc=Fortran f=C. +# +# Note: For files without extension you can use no_extension as a placeholder. +# +# Note that for custom extensions you also need to set FILE_PATTERNS otherwise +# the files are not read by doxygen. + +EXTENSION_MAPPING = + +# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments +# according to the Markdown format, which allows for more readable +# documentation. See https://daringfireball.net/projects/markdown/ for details. +# The output of markdown processing is further processed by doxygen, so you can +# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in +# case of backward compatibilities issues. +# The default value is: YES. + +MARKDOWN_SUPPORT = YES + +# When the TOC_INCLUDE_HEADINGS tag is set to a non-zero value, all headings up +# to that level are automatically included in the table of contents, even if +# they do not have an id attribute. +# Note: This feature currently applies only to Markdown headings. +# Minimum value: 0, maximum value: 99, default value: 5. +# This tag requires that the tag MARKDOWN_SUPPORT is set to YES. + +TOC_INCLUDE_HEADINGS = 5 + +# When enabled doxygen tries to link words that correspond to documented +# classes, or namespaces to their corresponding documentation. Such a link can +# be prevented in individual cases by putting a % sign in front of the word or +# globally by setting AUTOLINK_SUPPORT to NO. +# The default value is: YES. + +AUTOLINK_SUPPORT = YES + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should set this +# tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); +# versus func(std::string) {}). This also make the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. +# The default value is: NO. + +BUILTIN_STL_SUPPORT = NO + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. +# The default value is: NO. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip (see: +# https://www.riverbankcomputing.com/software/sip/intro) sources only. Doxygen +# will parse them like normal C++ but will assume all classes use public instead +# of private inheritance when no explicit protection keyword is present. +# The default value is: NO. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate +# getter and setter methods for a property. Setting this option to YES will make +# doxygen to replace the get and set methods by a property in the documentation. +# This will only work if the methods are indeed getting or setting a simple +# type. If this is not the case, or you want to show the methods anyway, you +# should set this option to NO. +# The default value is: YES. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. +# The default value is: NO. + +DISTRIBUTE_GROUP_DOC = NO + +# If one adds a struct or class to a group and this option is enabled, then also +# any nested class or struct is added to the same group. By default this option +# is disabled and one has to add nested compounds explicitly via \ingroup. +# The default value is: NO. + +GROUP_NESTED_COMPOUNDS = NO + +# Set the SUBGROUPING tag to YES to allow class member groups of the same type +# (for instance a group of public functions) to be put as a subgroup of that +# type (e.g. under the Public Functions section). Set it to NO to prevent +# subgrouping. Alternatively, this can be done per class using the +# \nosubgrouping command. +# The default value is: YES. + +SUBGROUPING = YES + +# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions +# are shown inside the group in which they are included (e.g. using \ingroup) +# instead of on a separate page (for HTML and Man pages) or section (for LaTeX +# and RTF). +# +# Note that this feature does not work in combination with +# SEPARATE_MEMBER_PAGES. +# The default value is: NO. + +INLINE_GROUPED_CLASSES = NO + +# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions +# with only public data fields or simple typedef fields will be shown inline in +# the documentation of the scope in which they are defined (i.e. file, +# namespace, or group documentation), provided this scope is documented. If set +# to NO, structs, classes, and unions are shown on a separate page (for HTML and +# Man pages) or section (for LaTeX and RTF). +# The default value is: NO. + +INLINE_SIMPLE_STRUCTS = NO + +# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or +# enum is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically be +# useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. +# The default value is: NO. + +TYPEDEF_HIDES_STRUCT = NO + +# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This +# cache is used to resolve symbols given their name and scope. Since this can be +# an expensive process and often the same symbol appears multiple times in the +# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small +# doxygen will become slower. If the cache is too large, memory is wasted. The +# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range +# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536 +# symbols. At the end of a run doxygen will report the cache usage and suggest +# the optimal cache size from a speed point of view. +# Minimum value: 0, maximum value: 9, default value: 0. + +LOOKUP_CACHE_SIZE = 0 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in +# documentation are documented, even if no documentation was available. Private +# class members and static file members will be hidden unless the +# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES. +# Note: This will also disable the warnings about undocumented members that are +# normally produced when WARNINGS is set to YES. +# The default value is: NO. + +EXTRACT_ALL = YES + +# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will +# be included in the documentation. +# The default value is: NO. + +EXTRACT_PRIVATE = NO + +# If the EXTRACT_PRIV_VIRTUAL tag is set to YES, documented private virtual +# methods of a class will be included in the documentation. +# The default value is: NO. + +EXTRACT_PRIV_VIRTUAL = NO + +# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal +# scope will be included in the documentation. +# The default value is: NO. + +EXTRACT_PACKAGE = NO + +# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be +# included in the documentation. +# The default value is: NO. + +EXTRACT_STATIC = YES + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined +# locally in source files will be included in the documentation. If set to NO, +# only classes defined in header files are included. Does not have any effect +# for Java sources. +# The default value is: YES. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. If set to YES, local methods, +# which are defined in the implementation section but not in the interface are +# included in the documentation. If set to NO, only methods in the interface are +# included. +# The default value is: NO. + +EXTRACT_LOCAL_METHODS = NO + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base name of +# the file that contains the anonymous namespace. By default anonymous namespace +# are hidden. +# The default value is: NO. + +EXTRACT_ANON_NSPACES = NO + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all +# undocumented members inside documented classes or files. If set to NO these +# members will be included in the various overviews, but no documentation +# section is generated. This option has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. If set +# to NO, these classes will be included in the various overviews. This option +# has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend +# declarations. If set to NO, these declarations will be included in the +# documentation. +# The default value is: NO. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any +# documentation blocks found inside the body of a function. If set to NO, these +# blocks will be appended to the function's detailed documentation block. +# The default value is: NO. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation that is typed after a +# \internal command is included. If the tag is set to NO then the documentation +# will be excluded. Set it to YES to include the internal documentation. +# The default value is: NO. + +INTERNAL_DOCS = NO + +# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file +# names in lower-case letters. If set to YES, upper-case letters are also +# allowed. This is useful if you have classes or files whose names only differ +# in case and if your file system supports case sensitive file names. Windows +# (including Cygwin) ands Mac users are advised to set this option to NO. +# The default value is: system dependent. + +CASE_SENSE_NAMES = YES + +# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with +# their full class and namespace scopes in the documentation. If set to YES, the +# scope will be hidden. +# The default value is: NO. + +HIDE_SCOPE_NAMES = NO + +# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will +# append additional text to a page's title, such as Class Reference. If set to +# YES the compound reference will be hidden. +# The default value is: NO. + +HIDE_COMPOUND_REFERENCE= NO + +# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of +# the files that are included by a file in the documentation of that file. +# The default value is: YES. + +SHOW_INCLUDE_FILES = YES + +# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each +# grouped member an include statement to the documentation, telling the reader +# which file to include in order to use the member. +# The default value is: NO. + +SHOW_GROUPED_MEMB_INC = NO + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include +# files with double quotes in the documentation rather than with sharp brackets. +# The default value is: NO. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the +# documentation for inline members. +# The default value is: YES. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the +# (detailed) documentation of file and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. +# The default value is: YES. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief +# descriptions of file, namespace and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. Note that +# this will also influence the order of the classes in the class list. +# The default value is: NO. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the +# (brief and detailed) documentation of class members so that constructors and +# destructors are listed first. If set to NO the constructors will appear in the +# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS. +# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief +# member documentation. +# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting +# detailed member documentation. +# The default value is: NO. + +SORT_MEMBERS_CTORS_1ST = NO + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy +# of group names into alphabetical order. If set to NO the group names will +# appear in their defined order. +# The default value is: NO. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by +# fully-qualified names, including namespaces. If set to NO, the class list will +# be sorted only by class name, not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the alphabetical +# list. +# The default value is: NO. + +SORT_BY_SCOPE_NAME = NO + +# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper +# type resolution of all parameters of a function it will reject a match between +# the prototype and the implementation of a member function even if there is +# only one candidate or it is obvious which candidate to choose by doing a +# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still +# accept a match between prototype and implementation in such cases. +# The default value is: NO. + +STRICT_PROTO_MATCHING = NO + +# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo +# list. This list is created by putting \todo commands in the documentation. +# The default value is: YES. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test +# list. This list is created by putting \test commands in the documentation. +# The default value is: YES. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug +# list. This list is created by putting \bug commands in the documentation. +# The default value is: YES. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO) +# the deprecated list. This list is created by putting \deprecated commands in +# the documentation. +# The default value is: YES. + +GENERATE_DEPRECATEDLIST= YES + +# The ENABLED_SECTIONS tag can be used to enable conditional documentation +# sections, marked by \if ... \endif and \cond +# ... \endcond blocks. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the +# initial value of a variable or macro / define can have for it to appear in the +# documentation. If the initializer consists of more lines than specified here +# it will be hidden. Use a value of 0 to hide initializers completely. The +# appearance of the value of individual variables and macros / defines can be +# controlled using \showinitializer or \hideinitializer command in the +# documentation regardless of this setting. +# Minimum value: 0, maximum value: 10000, default value: 30. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at +# the bottom of the documentation of classes and structs. If set to YES, the +# list will mention the files that were used to generate the documentation. +# The default value is: YES. + +SHOW_USED_FILES = YES + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This +# will remove the Files entry from the Quick Index and from the Folder Tree View +# (if specified). +# The default value is: YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces +# page. This will remove the Namespaces entry from the Quick Index and from the +# Folder Tree View (if specified). +# The default value is: YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command command input-file, where command is the value of the +# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided +# by doxygen. Whatever the program writes to standard output is used as the file +# version. For an example see the documentation. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. To create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. You can +# optionally specify a file name after the option, if omitted DoxygenLayout.xml +# will be used as the name of the layout file. +# +# Note that if you run doxygen from a directory containing a file called +# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE +# tag is left empty. + +LAYOUT_FILE = + +# The CITE_BIB_FILES tag can be used to specify one or more bib files containing +# the reference definitions. This must be a list of .bib files. The .bib +# extension is automatically appended if omitted. This requires the bibtex tool +# to be installed. See also https://en.wikipedia.org/wiki/BibTeX for more info. +# For LaTeX the style of the bibliography can be controlled using +# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the +# search path. See also \cite for info how to create references. + +CITE_BIB_FILES = + +#--------------------------------------------------------------------------- +# Configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated to +# standard output by doxygen. If QUIET is set to YES this implies that the +# messages are off. +# The default value is: NO. + +QUIET = YES + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES +# this implies that the warnings are on. +# +# Tip: Turn warnings on while writing the documentation. +# The default value is: YES. + +WARNINGS = YES + +# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate +# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag +# will automatically be disabled. +# The default value is: YES. + +WARN_IF_UNDOCUMENTED = YES + +# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some parameters +# in a documented function, or documenting parameters that don't exist or using +# markup commands wrongly. +# The default value is: YES. + +WARN_IF_DOC_ERROR = YES + +# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that +# are documented, but have no documentation for their parameters or return +# value. If set to NO, doxygen will only warn about wrong or incomplete +# parameter documentation, but not about the absence of documentation. If +# EXTRACT_ALL is set to YES then this flag will automatically be disabled. +# The default value is: NO. + +WARN_NO_PARAMDOC = NO + +# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when +# a warning is encountered. +# The default value is: NO. + +WARN_AS_ERROR = NO + +# The WARN_FORMAT tag determines the format of the warning messages that doxygen +# can produce. The string should contain the $file, $line, and $text tags, which +# will be replaced by the file and line number from which the warning originated +# and the warning text. Optionally the format may contain $version, which will +# be replaced by the version of the file (if it could be obtained via +# FILE_VERSION_FILTER) +# The default value is: $file:$line: $text. + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning and error +# messages should be written. If left blank the output is written to standard +# error (stderr). + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# Configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag is used to specify the files and/or directories that contain +# documented source files. You may enter file names like myfile.cpp or +# directories like /usr/src/myproject. Separate the files or directories with +# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING +# Note: If this tag is empty the current directory is searched. + +INPUT = ../interfaces/ros1/wavemap_ros_conversions \ + ../interfaces/ros1/wavemap_ros + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses +# libiconv (or the iconv built into libc) for the transcoding. See the libiconv +# documentation (see: https://www.gnu.org/software/libiconv/) for the list of +# possible encodings. +# The default value is: UTF-8. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and +# *.h) to filter out the source-files in the directories. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# read by doxygen. +# +# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp, +# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, +# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc, +# *.m, *.markdown, *.md, *.mm, *.dox (to be provided as doxygen C comment), +# *.doc (to be provided as doxygen C comment), *.txt (to be provided as doxygen +# C comment), *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, *.f, *.for, *.tcl, *.vhd, +# *.vhdl, *.ucf, *.qsf and *.ice. + +FILE_PATTERNS = *.h \ + *.hh \ + *.hxx \ + *.hpp \ + *.h++ + +# The RECURSIVE tag can be used to specify whether or not subdirectories should +# be searched for input files as well. +# The default value is: NO. + +RECURSIVE = YES + +# The EXCLUDE tag can be used to specify files and/or directories that should be +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. +# +# Note that relative paths are relative to the directory from which doxygen is +# run. + +EXCLUDE = + +# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or +# directories that are symbolic links (a Unix file system feature) are excluded +# from the input. +# The default value is: NO. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories for example use the pattern */test/* + +EXCLUDE_PATTERNS = */test/* + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories use the pattern */test/* + +EXCLUDE_SYMBOLS = + +# The EXAMPLE_PATH tag can be used to specify one or more files or directories +# that contain example code fragments that are included (see the \include +# command). + +EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and +# *.h) to filter out the source-files in the directories. If left blank all +# files are included. + +EXAMPLE_PATTERNS = * + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude commands +# irrespective of the value of the RECURSIVE tag. +# The default value is: NO. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or directories +# that contain images that are to be included in the documentation (see the +# \image command). + +IMAGE_PATH = + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command: +# +# +# +# where is the value of the INPUT_FILTER tag, and is the +# name of an input file. Doxygen will then use the output that the filter +# program writes to standard output. If FILTER_PATTERNS is specified, this tag +# will be ignored. +# +# Note that the filter must not add or remove lines; it is applied before the +# code is scanned, but not when the output code is generated. If lines are added +# or removed, the anchors will not be placed correctly. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# properly processed by doxygen. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. The filters are a list of the form: pattern=filter +# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how +# filters are used. If the FILTER_PATTERNS tag is empty or if none of the +# patterns match the file name, INPUT_FILTER is applied. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# properly processed by doxygen. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will also be used to filter the input files that are used for +# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES). +# The default value is: NO. + +FILTER_SOURCE_FILES = NO + +# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file +# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and +# it is also possible to disable source filtering for a specific pattern using +# *.ext= (so without naming a filter). +# This tag requires that the tag FILTER_SOURCE_FILES is set to YES. + +FILTER_SOURCE_PATTERNS = + +# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that +# is part of the input, its contents will be placed on the main page +# (index.html). This can be useful if you have a project on for instance GitHub +# and want to reuse the introduction page also for the doxygen output. + +USE_MDFILE_AS_MAINPAGE = + +#--------------------------------------------------------------------------- +# Configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will be +# generated. Documented entities will be cross-referenced with these sources. +# +# Note: To get rid of all source code in the generated output, make sure that +# also VERBATIM_HEADERS is set to NO. +# The default value is: NO. + +SOURCE_BROWSER = NO + +# Setting the INLINE_SOURCES tag to YES will include the body of functions, +# classes and enums directly into the documentation. +# The default value is: NO. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any +# special comment blocks from generated source code fragments. Normal C, C++ and +# Fortran comments will always remain visible. +# The default value is: YES. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES then for each documented +# entity all documented functions referencing it will be listed. +# The default value is: NO. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES then for each documented function +# all documented entities called/used by that function will be listed. +# The default value is: NO. + +REFERENCES_RELATION = NO + +# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set +# to YES then the hyperlinks from functions in REFERENCES_RELATION and +# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will +# link to the documentation. +# The default value is: YES. + +REFERENCES_LINK_SOURCE = YES + +# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the +# source code will show a tooltip with additional information such as prototype, +# brief description and links to the definition and documentation. Since this +# will make the HTML file larger and loading of large files a bit slower, you +# can opt to disable this feature. +# The default value is: YES. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +SOURCE_TOOLTIPS = YES + +# If the USE_HTAGS tag is set to YES then the references to source code will +# point to the HTML generated by the htags(1) tool instead of doxygen built-in +# source browser. The htags tool is part of GNU's global source tagging system +# (see https://www.gnu.org/software/global/global.html). You will need version +# 4.8.6 or higher. +# +# To use it do the following: +# - Install the latest version of global +# - Enable SOURCE_BROWSER and USE_HTAGS in the configuration file +# - Make sure the INPUT points to the root of the source tree +# - Run doxygen as normal +# +# Doxygen will invoke htags (and that will in turn invoke gtags), so these +# tools must be available from the command line (i.e. in the search path). +# +# The result: instead of the source browser generated by doxygen, the links to +# source code will now point to the output of htags. +# The default value is: NO. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a +# verbatim copy of the header file for each class for which an include is +# specified. Set to NO to disable this. +# See also: Section \class. +# The default value is: YES. + +VERBATIM_HEADERS = YES + +# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the +# clang parser (see: http://clang.llvm.org/) for more accurate parsing at the +# cost of reduced performance. This can be particularly helpful with template +# rich C++ code for which doxygen's built-in parser lacks the necessary type +# information. +# Note: The availability of this option depends on whether or not doxygen was +# generated with the -Duse_libclang=ON option for CMake. +# The default value is: NO. + +CLANG_ASSISTED_PARSING = NO + +# If clang assisted parsing is enabled you can provide the compiler with command +# line options that you would normally use when invoking the compiler. Note that +# the include paths will already be set by doxygen for the files and directories +# specified with INPUT and INCLUDE_PATH. +# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES. + +CLANG_OPTIONS = + +# If clang assisted parsing is enabled you can provide the clang parser with the +# path to the compilation database (see: +# http://clang.llvm.org/docs/HowToSetupToolingForLLVM.html) used when the files +# were built. This is equivalent to specifying the "-p" option to a clang tool, +# such as clang-check. These options will then be passed to the parser. +# Note: The availability of this option depends on whether or not doxygen was +# generated with the -Duse_libclang=ON option for CMake. + +CLANG_DATABASE_PATH = + +#--------------------------------------------------------------------------- +# Configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all +# compounds will be generated. Enable this if the project contains a lot of +# classes, structs, unions or interfaces. +# The default value is: YES. + +ALPHABETICAL_INDEX = YES + +# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in +# which the alphabetical index list will be split. +# Minimum value: 1, maximum value: 20, default value: 5. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +COLS_IN_ALPHA_INDEX = 5 + +# In case all classes in a project start with a common prefix, all classes will +# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag +# can be used to specify a prefix (or a list of prefixes) that should be ignored +# while generating the index headers. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output +# The default value is: YES. + +GENERATE_HTML = NO + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. +# The default directory is: html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_OUTPUT = html + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each +# generated HTML page (for example: .htm, .php, .asp). +# The default value is: .html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a user-defined HTML header file for +# each generated HTML page. If the tag is left blank doxygen will generate a +# standard header. +# +# To get valid HTML the header file that includes any scripts and style sheets +# that doxygen needs, which is dependent on the configuration options used (e.g. +# the setting GENERATE_TREEVIEW). It is highly recommended to start with a +# default header using +# doxygen -w html new_header.html new_footer.html new_stylesheet.css +# YourConfigFile +# and then modify the file new_header.html. See also section "Doxygen usage" +# for information on how to generate the default header that doxygen normally +# uses. +# Note: The header is subject to change so you typically have to regenerate the +# default header when upgrading to a newer version of doxygen. For a description +# of the possible markers and block names see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each +# generated HTML page. If the tag is left blank doxygen will generate a standard +# footer. See HTML_HEADER for more information on how to generate a default +# footer and what special commands can be used inside the footer. See also +# section "Doxygen usage" for information on how to generate the default footer +# that doxygen normally uses. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style +# sheet that is used by each HTML page. It can be used to fine-tune the look of +# the HTML output. If left blank doxygen will generate a default style sheet. +# See also section "Doxygen usage" for information on how to generate the style +# sheet that doxygen normally uses. +# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as +# it is more robust and this tag (HTML_STYLESHEET) will in the future become +# obsolete. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_STYLESHEET = + +# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined +# cascading style sheets that are included after the standard style sheets +# created by doxygen. Using this option one can overrule certain style aspects. +# This is preferred over using HTML_STYLESHEET since it does not replace the +# standard style sheet and is therefore more robust against future updates. +# Doxygen will copy the style sheet files to the output directory. +# Note: The order of the extra style sheet files is of importance (e.g. the last +# style sheet in the list overrules the setting of the previous ones in the +# list). For an example see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_STYLESHEET = + +# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the HTML output directory. Note +# that these files will be copied to the base HTML output directory. Use the +# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these +# files. In the HTML_STYLESHEET file, use the file name only. Also note that the +# files will be copied as-is; there are no commands or markers available. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_FILES = + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen +# will adjust the colors in the style sheet and background images according to +# this color. Hue is specified as an angle on a colorwheel, see +# https://en.wikipedia.org/wiki/Hue for more information. For instance the value +# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300 +# purple, and 360 is red again. +# Minimum value: 0, maximum value: 359, default value: 220. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_HUE = 220 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors +# in the HTML output. For a value of 0 the output will use grayscales only. A +# value of 255 will produce the most vivid colors. +# Minimum value: 0, maximum value: 255, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_SAT = 100 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the +# luminance component of the colors in the HTML output. Values below 100 +# gradually make the output lighter, whereas values above 100 make the output +# darker. The value divided by 100 is the actual gamma applied, so 80 represents +# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not +# change the gamma. +# Minimum value: 40, maximum value: 240, default value: 80. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_GAMMA = 80 + +# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML +# page will contain the date and time when the page was generated. Setting this +# to YES can help to show when doxygen was last run and thus if the +# documentation is up to date. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_TIMESTAMP = NO + +# If the HTML_DYNAMIC_MENUS tag is set to YES then the generated HTML +# documentation will contain a main index with vertical navigation menus that +# are dynamically created via JavaScript. If disabled, the navigation index will +# consists of multiple levels of tabs that are statically embedded in every HTML +# page. Disable this option to support browsers that do not have JavaScript, +# like the Qt help browser. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_DYNAMIC_MENUS = YES + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_DYNAMIC_SECTIONS = NO + +# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries +# shown in the various tree structured indices initially; the user can expand +# and collapse entries dynamically later on. Doxygen will expand the tree to +# such a level that at most the specified number of entries are visible (unless +# a fully collapsed tree already exceeds this amount). So setting the number of +# entries 1 will produce a full collapsed tree by default. 0 is a special value +# representing an infinite number of entries and will result in a full expanded +# tree by default. +# Minimum value: 0, maximum value: 9999, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_INDEX_NUM_ENTRIES = 100 + +# If the GENERATE_DOCSET tag is set to YES, additional index files will be +# generated that can be used as input for Apple's Xcode 3 integrated development +# environment (see: https://developer.apple.com/xcode/), introduced with OSX +# 10.5 (Leopard). To create a documentation set, doxygen will generate a +# Makefile in the HTML output directory. Running make will produce the docset in +# that directory and running make install will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at +# startup. See https://developer.apple.com/library/archive/featuredarticles/Doxy +# genXcode/_index.html for more information. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_DOCSET = NO + +# This tag determines the name of the docset feed. A documentation feed provides +# an umbrella under which multiple documentation sets from a single provider +# (such as a company or product suite) can be grouped. +# The default value is: Doxygen generated docs. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# This tag specifies a string that should uniquely identify the documentation +# set bundle. This should be a reverse domain-name style string, e.g. +# com.mycompany.MyDocSet. Doxygen will append .docset to the name. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify +# the documentation publisher. This should be a reverse domain-name style +# string, e.g. com.mycompany.MyDocSet.documentation. +# The default value is: org.doxygen.Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher. +# The default value is: Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three +# additional HTML index files: index.hhp, index.hhc, and index.hhk. The +# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop +# (see: https://www.microsoft.com/en-us/download/details.aspx?id=21138) on +# Windows. +# +# The HTML Help Workshop contains a compiler that can convert all HTML output +# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML +# files are now used as the Windows 98 help format, and will replace the old +# Windows help format (.hlp) on all Windows platforms in the future. Compressed +# HTML files also contain an index, a table of contents, and you can search for +# words in the documentation. The HTML workshop also contains a viewer for +# compressed HTML files. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_HTMLHELP = NO + +# The CHM_FILE tag can be used to specify the file name of the resulting .chm +# file. You can add a path in front of the file if the result should not be +# written to the html output directory. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_FILE = + +# The HHC_LOCATION tag can be used to specify the location (absolute path +# including file name) of the HTML help compiler (hhc.exe). If non-empty, +# doxygen will try to run the HTML help compiler on the generated index.hhp. +# The file has to be specified with full path. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +HHC_LOCATION = + +# The GENERATE_CHI flag controls if a separate .chi index file is generated +# (YES) or that it should be included in the master .chm file (NO). +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +GENERATE_CHI = NO + +# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc) +# and project file content. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_INDEX_ENCODING = + +# The BINARY_TOC flag controls whether a binary table of contents is generated +# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it +# enables the Previous and Next buttons. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members to +# the table of contents of the HTML help documentation and to the tree view. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +TOC_EXPAND = NO + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that +# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help +# (.qch) of the generated HTML documentation. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify +# the file name of the resulting .qch file. The path specified is relative to +# the HTML output folder. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help +# Project output. For more information please see Qt Help Project / Namespace +# (see: https://doc.qt.io/archives/qt-4.8/qthelpproject.html#namespace). +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt +# Help Project output. For more information please see Qt Help Project / Virtual +# Folders (see: https://doc.qt.io/archives/qt-4.8/qthelpproject.html#virtual- +# folders). +# The default value is: doc. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_VIRTUAL_FOLDER = doc + +# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom +# filter to add. For more information please see Qt Help Project / Custom +# Filters (see: https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom- +# filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see Qt Help Project / Custom +# Filters (see: https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom- +# filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's filter section matches. Qt Help Project / Filter Attributes (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#filter-attributes). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_SECT_FILTER_ATTRS = + +# The QHG_LOCATION tag can be used to specify the location of Qt's +# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the +# generated .qhp file. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be +# generated, together with the HTML files, they form an Eclipse help plugin. To +# install this plugin and make it available under the help contents menu in +# Eclipse, the contents of the directory containing the HTML and XML files needs +# to be copied into the plugins directory of eclipse. The name of the directory +# within the plugins directory should be the same as the ECLIPSE_DOC_ID value. +# After copying Eclipse needs to be restarted before the help appears. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the Eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have this +# name. Each documentation set should have its own identifier. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# If you want full control over the layout of the generated HTML pages it might +# be necessary to disable the index and replace it with your own. The +# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top +# of each HTML page. A value of NO enables the index and the value YES disables +# it. Since the tabs in the index contain the same information as the navigation +# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +DISABLE_INDEX = NO + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. If the tag +# value is set to YES, a side panel will be generated containing a tree-like +# index structure (just like the one that is generated for HTML Help). For this +# to work a browser that supports JavaScript, DHTML, CSS and frames is required +# (i.e. any modern browser). Windows users are probably better off using the +# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can +# further fine-tune the look of the index. As an example, the default style +# sheet generated by doxygen has an example that shows how to put an image at +# the root of the tree instead of the PROJECT_NAME. Since the tree basically has +# the same information as the tab index, you could consider setting +# DISABLE_INDEX to YES when enabling this option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_TREEVIEW = NO + +# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that +# doxygen will group on one line in the generated HTML documentation. +# +# Note that a value of 0 will completely suppress the enum values from appearing +# in the overview section. +# Minimum value: 0, maximum value: 20, default value: 4. +# This tag requires that the tag GENERATE_HTML is set to YES. + +ENUM_VALUES_PER_LINE = 4 + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used +# to set the initial width (in pixels) of the frame in which the tree is shown. +# Minimum value: 0, maximum value: 1500, default value: 250. +# This tag requires that the tag GENERATE_HTML is set to YES. + +TREEVIEW_WIDTH = 250 + +# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to +# external symbols imported via tag files in a separate window. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +EXT_LINKS_IN_WINDOW = NO + +# Use this tag to change the font size of LaTeX formulas included as images in +# the HTML documentation. When you change the font size after a successful +# doxygen run you need to manually remove any form_*.png images from the HTML +# output directory to force them to be regenerated. +# Minimum value: 8, maximum value: 50, default value: 10. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_FONTSIZE = 10 + +# Use the FORMULA_TRANSPARENT tag to determine whether or not the images +# generated for formulas are transparent PNGs. Transparent PNGs are not +# supported properly for IE 6.0, but are supported on all modern browsers. +# +# Note that when changing this option you need to delete any form_*.png files in +# the HTML output directory before the changes have effect. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_TRANSPARENT = YES + +# The FORMULA_MACROFILE can contain LaTeX \newcommand and \renewcommand commands +# to create new LaTeX commands to be used in formulas as building blocks. See +# the section "Including formulas" for details. + +FORMULA_MACROFILE = + +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see +# https://www.mathjax.org) which uses client side JavaScript for the rendering +# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX +# installed or if you want to formulas look prettier in the HTML output. When +# enabled you may also need to install MathJax separately and configure the path +# to it using the MATHJAX_RELPATH option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +USE_MATHJAX = NO + +# When MathJax is enabled you can set the default output format to be used for +# the MathJax output. See the MathJax site (see: +# http://docs.mathjax.org/en/latest/output.html) for more details. +# Possible values are: HTML-CSS (which is slower, but has the best +# compatibility), NativeMML (i.e. MathML) and SVG. +# The default value is: HTML-CSS. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_FORMAT = HTML-CSS + +# When MathJax is enabled you need to specify the location relative to the HTML +# output directory using the MATHJAX_RELPATH option. The destination directory +# should contain the MathJax.js script. For instance, if the mathjax directory +# is located at the same level as the HTML output directory, then +# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax +# Content Delivery Network so you can quickly see the result without installing +# MathJax. However, it is strongly recommended to install a local copy of +# MathJax from https://www.mathjax.org before deployment. +# The default value is: https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.5/. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_RELPATH = https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.5/ + +# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax +# extension names that should be enabled during MathJax rendering. For example +# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_EXTENSIONS = + +# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces +# of code that will be used on startup of the MathJax code. See the MathJax site +# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an +# example see the documentation. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_CODEFILE = + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box for +# the HTML output. The underlying search engine uses javascript and DHTML and +# should work on any modern browser. Note that when using HTML help +# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET) +# there is already a search function so this one should typically be disabled. +# For large projects the javascript based search engine can be slow, then +# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to +# search using the keyboard; to jump to the search box use + S +# (what the is depends on the OS and browser, but it is typically +# , /