Map editing tools and improved local mapping

docs/python_api/index.rst

@@ -44,6 +44,7 @@ Python API
 .. automethod:: pywavemap.convert.height_to_cell_width
 .. automethod:: pywavemap.convert.point_to_nearest_index
 .. automethod:: pywavemap.convert.point_to_node_index
+.. automethod:: pywavemap.convert.node_index_to_center_point
 
 .. automodule:: pywavemap.logging
 .. automethod:: pywavemap.logging.set_level

examples/cpp/CMakeLists.txt

@@ -31,6 +31,7 @@ else ()
 endif ()
 
 # Add each set of examples
+add_subdirectory(edit)
+add_subdirectory(conversions)
 add_subdirectory(io)
 add_subdirectory(queries)
-add_subdirectory(planning)

examples/cpp/planning/CMakeLists.txt → examples/cpp/conversions/CMakeLists.txt

@@ -3,3 +3,8 @@ 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)
+
+add_executable(occupancy_to_ply occupancy_to_ply.cc)
+set_wavemap_target_properties(occupancy_to_ply)
+target_link_libraries(occupancy_to_ply PUBLIC
+    wavemap::wavemap_core wavemap::wavemap_io)

examples/cpp/conversions/occupancy_to_ply.cc

@@ -0,0 +1,94 @@
+#include <filesystem>
+#include <fstream>
+#include <vector>
+
+#include <glog/logging.h>
+#include <wavemap/core/common.h>
+#include <wavemap/core/map/hashed_wavelet_octree.h>
+#include <wavemap/core/map/map_base.h>
+#include <wavemap/io/file_conversions.h>
+
+using namespace wavemap;  // NOLINT
+int main(int argc, char** argv) {
+  // Initialize GLOG
+  google::InitGoogleLogging(argv[0]);
+  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
+  MapBase::Ptr occupancy_map;
+  if (!io::fileToMap(map_file_path, occupancy_map)) {
+    LOG(WARNING) << "Could not open map for reading: " << map_file_path;
+    return EXIT_FAILURE;
+  }
+
+  // Convert each occupied leaf node to high-res points and add to pointcloud
+  std::vector<Point3D> pointcloud;
+  constexpr FloatingPoint kOccupancyThreshold = 0.01f;
+  const FloatingPoint min_cell_width = occupancy_map->getMinCellWidth();
+  occupancy_map->forEachLeaf([min_cell_width, &pointcloud](
+                                 const OctreeIndex& node_index,
+                                 FloatingPoint node_log_odds) {
+    if (kOccupancyThreshold < node_log_odds) {
+      if (node_index.height == 0) {
+        const Point3D center =
+            convert::indexToCenterPoint(node_index.position, min_cell_width);
+        pointcloud.emplace_back(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);
+          pointcloud.emplace_back(center);
+        }
+      }
+    }
+  });
+
+  // Create the PLY output file
+  const std::filesystem::path ply_file_path =
+      std::filesystem::path(map_file_path).replace_extension(".ply");
+  LOG(INFO) << "Creating PLY file: " << ply_file_path;
+  std::ofstream ply_file(ply_file_path,
+                         std::ofstream::out | std::ofstream::binary);
+  if (!ply_file.is_open()) {
+    LOG(WARNING) << "Could not open file for writing. Error: "
+                 << strerror(errno);
+    return EXIT_FAILURE;
+  }
+
+  // Write the PLY header
+  // clang-format off
+  ply_file << "ply\n"
+              "format ascii 1.0\n"
+              "comment The voxel size is " << min_cell_width << " meters.\n"
+              "element vertex " << pointcloud.size() << "\n"
+              "property float x\n"
+              "property float y\n"
+              "property float z\n"
+              "end_header"
+           << std::endl;
+  // clang-format on
+
+  // Add the points
+  for (const Point3D& point : pointcloud) {
+    ply_file << point.x() << " " << point.y() << " " << point.z() << "\n";
+  }
+  ply_file.flush();
+
+  // Close the file and communicate whether writing succeeded
+  ply_file.close();
+  return static_cast<bool>(ply_file) ? EXIT_SUCCESS : EXIT_FAILURE;
+}

examples/cpp/edit/CMakeLists.txt

@@ -0,0 +1,20 @@
+# Binaries
+add_executable(crop_map crop_map.cc)
+set_wavemap_target_properties(crop_map)
+target_link_libraries(crop_map PUBLIC
+    wavemap::wavemap_core wavemap::wavemap_io)
+
+add_executable(multiply_map multiply_map.cc)
+set_wavemap_target_properties(multiply_map)
+target_link_libraries(multiply_map PUBLIC
+    wavemap::wavemap_core wavemap::wavemap_io)
+
+add_executable(sum_map sum_map.cc)
+set_wavemap_target_properties(sum_map)
+target_link_libraries(sum_map PUBLIC
+    wavemap::wavemap_core wavemap::wavemap_io)
+
+add_executable(transform_map transform_map.cc)
+set_wavemap_target_properties(transform_map)
+target_link_libraries(transform_map PUBLIC
+    wavemap::wavemap_core wavemap::wavemap_io)

examples/cpp/edit/crop_map.cc

@@ -0,0 +1,32 @@
+#include <memory>
+
+#include <wavemap/core/common.h>
+#include <wavemap/core/utils/edit/crop.h>
+#include <wavemap/io/file_conversions.h>
+
+using namespace wavemap;  // NOLINT
+int main(int, char**) {
+  // Load the map
+  const std::filesystem::path home_dir = CHECK_NOTNULL(getenv("HOME"));
+  const std::filesystem::path input_map_path = home_dir / "your_map.wvmp";
+  MapBase::Ptr map_base;
+  bool success = io::fileToMap(input_map_path, map_base);
+  CHECK(success);
+
+  // Downcast it to a concrete map type
+  auto map = std::dynamic_pointer_cast<HashedWaveletOctree>(map_base);
+  CHECK_NOTNULL(map);
+
+  // Crop the map
+  const Point3D t_W_center{-2.2, -1.4, 0.0};
+  const FloatingPoint radius = 3.0;
+  const Sphere cropping_sphere{t_W_center, radius};
+  auto thread_pool = std::make_shared<ThreadPool>();  // Optional
+  edit::crop(*map, cropping_sphere, 0, thread_pool);
+
+  // Save the map
+  const std::filesystem::path output_map_path =
+      home_dir / "your_map_cropped.wvmp";
+  success &= io::mapToFile(*map, output_map_path);
+  CHECK(success);
+}

examples/cpp/edit/multiply_map.cc

@@ -0,0 +1,30 @@
+#include <memory>
+
+#include <wavemap/core/common.h>
+#include <wavemap/core/utils/edit/multiply.h>
+#include <wavemap/io/file_conversions.h>
+
+using namespace wavemap;  // NOLINT
+int main(int, char**) {
+  // Load the map
+  const std::filesystem::path home_dir = CHECK_NOTNULL(getenv("HOME"));
+  const std::filesystem::path input_map_path = home_dir / "your_map.wvmp";
+  MapBase::Ptr map_base;
+  bool success = io::fileToMap(input_map_path, map_base);
+  CHECK(success);
+
+  // Downcast it to a concrete map type
+  auto map = std::dynamic_pointer_cast<HashedWaveletOctree>(map_base);
+  CHECK_NOTNULL(map);
+
+  // Use the multiply method to implement exponential forgetting
+  const FloatingPoint decay_factor = 0.9;
+  auto thread_pool = std::make_shared<ThreadPool>();  // Optional
+  edit::multiply(*map, decay_factor, thread_pool);
+
+  // Save the map
+  const std::filesystem::path output_map_path =
+      home_dir / "your_map_decayed.wvmp";
+  success &= io::mapToFile(*map, output_map_path);
+  CHECK(success);
+}

examples/cpp/edit/sum_map.cc

@@ -0,0 +1,52 @@
+#include <memory>
+
+#include <wavemap/core/common.h>
+#include <wavemap/core/utils/edit/crop.h>
+#include <wavemap/core/utils/edit/sum.h>
+#include <wavemap/core/utils/edit/transform.h>
+#include <wavemap/core/utils/shape/aabb.h>
+#include <wavemap/core/utils/shape/sphere.h>
+#include <wavemap/io/file_conversions.h>
+
+using namespace wavemap;  // NOLINT
+int main(int, char**) {
+  // Load the map
+  const std::filesystem::path home_dir = CHECK_NOTNULL(getenv("HOME"));
+  const std::filesystem::path input_map_path = home_dir / "your_map.wvmp";
+  MapBase::Ptr map_base;
+  bool success = io::fileToMap(input_map_path, map_base);
+  CHECK(success);
+
+  // Downcast it to a concrete map type
+  auto map = std::dynamic_pointer_cast<HashedWaveletOctree>(map_base);
+  CHECK_NOTNULL(map);
+
+  // Crop the map
+  const Point3D t_W_center{-2.2, -1.4, 0.0};
+  const FloatingPoint radius = 3.0;
+  const Sphere cropping_sphere{t_W_center, radius};
+  auto thread_pool = std::make_shared<ThreadPool>();  // Optional
+  edit::crop(*map, cropping_sphere, 0, thread_pool);
+
+  // Create a translated copy
+  Transformation3D T_AB;
+  T_AB.getPosition() = {5.0, 5.0, 0.0};
+  auto map_translated = edit::transform(*map, T_AB, thread_pool);
+
+  // Merge them together
+  edit::sum(*map, *map_translated);
+
+  // Set a box in the map to free
+  AABB<Point3D> box{{6.f, 6.f, -2.f}, {10.f, 10.f, 2.f}};
+  edit::sum(*map, box, -1.f, thread_pool);
+
+  // Set a sphere in the map to occupied
+  Sphere<Point3D> sphere{{8.f, 8.f, 0.f}, 1.5f};
+  edit::sum(*map, sphere, 2.f, thread_pool);
+
+  // Save the map
+  const std::filesystem::path output_map_path =
+      home_dir / "your_map_merged.wvmp";
+  success &= io::mapToFile(*map, output_map_path);
+  CHECK(success);
+}

examples/cpp/edit/transform_map.cc

@@ -0,0 +1,33 @@
+#include <memory>
+
+#include <wavemap/core/common.h>
+#include <wavemap/core/utils/edit/transform.h>
+#include <wavemap/io/file_conversions.h>
+
+using namespace wavemap;  // NOLINT
+int main(int, char**) {
+  // Load the map
+  const std::filesystem::path home_dir = CHECK_NOTNULL(getenv("HOME"));
+  const std::filesystem::path input_map_path = home_dir / "your_map.wvmp";
+  MapBase::Ptr map_base;
+  bool success = io::fileToMap(input_map_path, map_base);
+  CHECK(success);
+
+  // Downcast it to a concrete map type
+  auto map = std::dynamic_pointer_cast<HashedWaveletOctree>(map_base);
+  CHECK_NOTNULL(map);
+
+  // Define a transformation that flips the map upside down, for illustration
+  Transformation3D T_AB;
+  T_AB.getRotation() = Rotation3D{0.f, 1.f, 0.f, 0.f};
+
+  // Transform the map
+  auto thread_pool = std::make_shared<ThreadPool>();  // Optional
+  map = edit::transform(*map, T_AB, thread_pool);
+
+  // Save the map
+  const std::filesystem::path output_map_path =
+      home_dir / "your_map_transformed.wvmp";
+  success &= io::mapToFile(*map, output_map_path);
+  CHECK(success);
+}

examples/cpp/io/load_map_from_file.cc

@@ -1,10 +1,11 @@
 #include <wavemap/io/file_conversions.h>
 
+using namespace wavemap;  // NOLINT
 int main(int, char**) {
   // Create a smart pointer that will own the loaded map
-  wavemap::MapBase::Ptr loaded_map;
+  MapBase::Ptr loaded_map;
 
   // Load the map
-  const bool success =
-      wavemap::io::fileToMap("/path/to/your/map.wvmp", loaded_map);
+  const bool success = io::fileToMap("/path/to/your/map.wvmp", loaded_map);
+  CHECK(success);
 }

examples/cpp/io/load_map_from_stream.cc

@@ -2,13 +2,15 @@
 
 #include <wavemap/io/stream_conversions.h>
 
+using namespace wavemap;  // NOLINT
 int main(int, char**) {
   // Create a smart pointer that will own the loaded map
-  wavemap::MapBase::Ptr loaded_map;
+  MapBase::Ptr loaded_map;
 
   // Create an input stream for illustration purposes
   std::istrstream input_stream{""};
 
   // Load the map
-  const bool success = wavemap::io::streamToMap(input_stream, loaded_map);
+  const bool success = io::streamToMap(input_stream, loaded_map);
+  CHECK(success);
 }

examples/cpp/io/save_map_to_file.cc

@@ -1,10 +1,12 @@
 #include <wavemap/io/file_conversions.h>
 
+using namespace wavemap;  // NOLINT
 int main(int, char**) {
   // Create an empty map for illustration purposes
-  wavemap::HashedWaveletOctreeConfig config;
-  wavemap::HashedWaveletOctree map(config);
+  HashedWaveletOctreeConfig config;
+  HashedWaveletOctree map(config);
 
   // Save the map
-  const bool success = wavemap::io::mapToFile(map, "/path/to/your/map.wvmp");
+  const bool success = io::mapToFile(map, "/path/to/your/map.wvmp");
+  CHECK(success);
 }

examples/cpp/io/save_map_to_stream.cc

@@ -2,16 +2,18 @@
 
 #include <wavemap/io/stream_conversions.h>
 
+using namespace wavemap;  // NOLINT
 int main(int, char**) {
   // Create an empty map for illustration purposes
-  wavemap::HashedWaveletOctreeConfig config;
-  wavemap::HashedWaveletOctree map(config);
+  HashedWaveletOctreeConfig config;
+  HashedWaveletOctree map(config);
 
   // Create an output stream for illustration purposes
   std::ostrstream output_stream;
 
   // Save the map
-  bool success = wavemap::io::mapToStream(map, output_stream);
+  bool success = io::mapToStream(map, output_stream);
   output_stream.flush();
   success &= output_stream.good();
+  CHECK(success);
 }

examples/python/edit/crop_map.py

@@ -0,0 +1,16 @@
+import os
+import numpy as np
+import pywavemap as wave
+
+# Load the map
+user_home = os.path.expanduser('~')
+input_map_path = os.path.join(user_home, "your_map.wvmp")
+your_map = wave.Map.load(input_map_path)
+
+# Crop the map
+cropping_sphere = wave.Sphere(center=np.array([-2.2, -1.4, 0.0]), radius=3.0)
+wave.edit.crop(your_map, cropping_sphere)
+
+# Save the map
+output_map_path = os.path.join(user_home, "your_map_cropped.wvmp")
+your_map.store(output_map_path)