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,5 @@
+# Binaries
+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/transform_map.cc

@@ -0,0 +1,36 @@
+#include <memory>
+
+#include <wavemap/core/common.h>
+#include <wavemap/core/utils/edit/transform.h>
+#include <wavemap/io/file_conversions.h>
+
+int main(int, char**) {
+  // Settings
+  std::filesystem::path input_map_path =
+      "/home/victor/data/wavemaps/newer_college_cloister_10cm.wvmp";
+  std::filesystem::path output_map_path =
+      "/home/victor/data/wavemaps/newer_college_cloister_10cm_tranformed.wvmp";
+
+  // Create a smart pointer that will own the loaded map
+  wavemap::MapBase::Ptr map_base;
+
+  // Load the map
+  bool success = wavemap::io::fileToMap(input_map_path, map_base);
+  CHECK(success);
+
+  // Downcast it to a concrete map type
+  auto map = std::dynamic_pointer_cast<wavemap::HashedWaveletOctree>(map_base);
+  CHECK_NOTNULL(map);
+
+  // Define a transformation that flips the map upside down, for illustration
+  wavemap::Transformation3D T_AB;
+  T_AB.getRotation() = wavemap::Rotation3D{0.f, 1.f, 0.f, 0.f};
+
+  // Transform the map
+  map = wavemap::edit::transform(*map, T_AB,
+                                 std::make_shared<wavemap::ThreadPool>());
+
+  // Save the map
+  success &= wavemap::io::mapToFile(*map, output_map_path);
+  CHECK(success);
+}

examples/python/queries/occupied_cell_lists.py

@@ -0,0 +1,17 @@
+import _dummy_objects
+
+# Load a map
+your_map = _dummy_objects.example_map()
+
+# Log odds threshold above which to consider a cell occupied
+log_odds_occupancy_threshold = 1e-3
+
+# Get the node indices of all occupied leaf nodes
+occupied_nodes = your_map.get_occupied_node_indices(
+    log_odds_occupancy_threshold)
+print(occupied_nodes)
+
+# Get the center points of all occupied cells at the highest resolution
+occupied_points = your_map.get_occupied_pointcloud(
+    log_odds_occupancy_threshold)
+print(occupied_points)

interfaces/ros1/wavemap_ros/CMakeLists.txt

@@ -34,6 +34,7 @@ add_library(${PROJECT_NAME}
     src/inputs/ros_input_base.cc
     src/inputs/ros_input_factory.cc
     src/map_operations/crop_map_operation.cc
+    src/map_operations/decay_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

interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/crop_map_operation.h

@@ -3,10 +3,11 @@
 
 #include <memory>
 #include <string>
-#include <utility>
 
 #include <wavemap/core/config/config_base.h>
 #include <wavemap/core/map/map_base.h>
+#include <wavemap/core/utils/thread_pool.h>
+#include <wavemap/core/utils/time/stopwatch.h>
 #include <wavemap/pipeline/map_operations/map_operation_base.h>
 
 #include "wavemap_ros/utils/tf_transformer.h"
@@ -15,17 +16,28 @@ namespace wavemap {
 /**
  * Config struct for map cropping operations.
  */
-struct CropMapOperationConfig : public ConfigBase<CropMapOperationConfig, 3> {
+struct CropMapOperationConfig : public ConfigBase<CropMapOperationConfig, 5> {
   //! Time period controlling how often the map is cropped.
   Seconds<FloatingPoint> 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.
+  //! body frame. When the cropper runs, all cells that are further than
+  //! `radius` from this point are deleted.
   std::string body_frame = "body";
 
-  //! Distance beyond which blocks are deleted when the cropper is executed.
-  Meters<FloatingPoint> remove_blocks_beyond_distance;
+  //! Time offset applied when retrieving the transform from body_frame to
+  //! world_frame. Set to -1 to use the most recent transform available in ROS
+  //! TF, ignoring timestamps (default). If set to a non-negative value, the
+  //! transform lookup uses a timestamp of `ros::Time::now() - tf_time_offset`.
+  Seconds<FloatingPoint> tf_time_offset = -1.f;
+
+  //! Distance beyond which to remove cells from the map.
+  Meters<FloatingPoint> radius;
+
+  //! Maximum resolution at which the crop is applied. Set to 0 to match the
+  //! map's maximum resolution (default). Setting a higher value reduces
+  //! computation but produces more jagged borders.
+  Meters<FloatingPoint> max_update_resolution = 0.f;
 
   static MemberMap memberMap;
 
@@ -36,6 +48,7 @@ class CropMapOperation : public MapOperationBase {
  public:
   CropMapOperation(const CropMapOperationConfig& config,
                    MapBase::Ptr occupancy_map,
+                   std::shared_ptr<ThreadPool> thread_pool,
                    std::shared_ptr<TfTransformer> transformer,
                    std::string world_frame);
 
@@ -45,9 +58,18 @@ class CropMapOperation : public MapOperationBase {
 
  private:
   const CropMapOperationConfig config_;
+  const std::shared_ptr<ThreadPool> thread_pool_;
   const std::shared_ptr<TfTransformer> transformer_;
   const std::string world_frame_;
   ros::Time last_run_timestamp_;
+  Stopwatch timer_;
+
+  const FloatingPoint min_cell_width_ = occupancy_map_->getMinCellWidth();
+  const IndexElement termination_height_ =
+      min_cell_width_ < config_.max_update_resolution
+          ? static_cast<IndexElement>(std::round(
+                std::log2(config_.max_update_resolution / min_cell_width_)))
+          : 0;
 };
 }  // namespace wavemap
 

interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/decay_map_operation.h

@@ -0,0 +1,48 @@
+#ifndef WAVEMAP_ROS_MAP_OPERATIONS_DECAY_MAP_OPERATION_H_
+#define WAVEMAP_ROS_MAP_OPERATIONS_DECAY_MAP_OPERATION_H_
+
+#include <memory>
+
+#include <ros/ros.h>
+#include <wavemap/core/config/config_base.h>
+#include <wavemap/core/map/map_base.h>
+#include <wavemap/core/utils/thread_pool.h>
+#include <wavemap/core/utils/time/stopwatch.h>
+#include <wavemap/pipeline/map_operations/map_operation_base.h>
+
+namespace wavemap {
+/**
+ * Config struct for map decaying operations.
+ */
+struct DecayMapOperationConfig : public ConfigBase<DecayMapOperationConfig, 2> {
+  //! Time period controlling how often the map is decayed.
+  Seconds<FloatingPoint> once_every = 2.f;
+
+  //! Decay rate in the range (0, 1), applied to each map value as
+  //! `map_value *= decay_rate` at each operation run.
+  FloatingPoint decay_rate = 0.9f;
+
+  static MemberMap memberMap;
+
+  bool isValid(bool verbose) const override;
+};
+
+class DecayMapOperation : public MapOperationBase {
+ public:
+  DecayMapOperation(const DecayMapOperationConfig& config,
+                    MapBase::Ptr occupancy_map,
+                    std::shared_ptr<ThreadPool> thread_pool);
+
+  bool shouldRun(const ros::Time& current_time = ros::Time::now());
+
+  void run(bool force_run) override;
+
+ private:
+  const DecayMapOperationConfig config_;
+  const std::shared_ptr<ThreadPool> thread_pool_;
+  ros::Time last_run_timestamp_;
+  Stopwatch timer_;
+};
+}  // namespace wavemap
+
+#endif  // WAVEMAP_ROS_MAP_OPERATIONS_DECAY_MAP_OPERATION_H_

interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/map_ros_operation_types.h

@@ -7,10 +7,10 @@ namespace wavemap {
 struct MapRosOperationType : public TypeSelector<MapRosOperationType> {
   using TypeSelector<MapRosOperationType>::TypeSelector;
 
-  enum Id : TypeId { kPublishMap, kPublishPointcloud, kCropMap };
+  enum Id : TypeId { kPublishMap, kPublishPointcloud, kCropMap, kDecayMap };
 
   static constexpr std::array names = {"publish_map", "publish_pointcloud",
-                                       "crop_map"};
+                                       "crop_map", "decay_map"};
 };
 }  // namespace wavemap