Depth image renderer

docs/pages/tutorials/cpp.rst

@@ -40,6 +40,21 @@ Code examples
 *************
 In the following sections, you'll find sample code for common tasks. If you'd like to request examples for additional tasks or contribute new examples, please don't hesitate to `contact us <https://github.com/ethz-asl/wavemap/issues>`_.
 
+Mapping
+=======
+The only requirements to build wavemap maps are that you have a set of
+
+1. 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 corresponding :ref:`projection <configuration_projection_models>` and :ref:`measurement <configuration_measurement_models>` model is available. To help you get started quickly, we provide example configs for various sensor setups :gh_file:`here <interfaces/ros1/wavemap_ros/config>`. An overview of all the available settings is provided on the :doc:`parameters page <../parameters/index>`.
+
+Example pipeline
+----------------
+
+.. literalinclude:: ../../../examples/cpp/mapping/example_pipeline.cc
+    :language: cpp
+
 Serializing maps
 ================
 In this section, we'll demonstrate how to serialize and deserialize maps using wavemap's lightweight and efficient binary format. This format is consistent across wavemap's C++, Python, and ROS interfaces. For instance, you can create maps on a robot with ROS and later load them into a rendering engine plugin that only depends on wavemap's C++ library.

examples/cpp/CMakeLists.txt

@@ -32,5 +32,6 @@ endif ()
 
 # Add each set of examples
 add_subdirectory(io)
+add_subdirectory(mapping)
 add_subdirectory(queries)
 add_subdirectory(planning)

examples/cpp/mapping/.gitignore

@@ -0,0 +1 @@
+example_map.wvmp

examples/cpp/mapping/CMakeLists.txt

@@ -0,0 +1,7 @@
+# Binaries
+add_executable(example_pipeline example_pipeline.cc)
+set_wavemap_target_properties(example_pipeline)
+target_link_libraries(example_pipeline PUBLIC
+    wavemap::wavemap_core
+    wavemap::wavemap_io
+    wavemap::wavemap_pipeline)

examples/cpp/mapping/example_config.yaml

@@ -0,0 +1,33 @@
+# NOTE: More examples can be found in the `interfaces/ros1/wavemap_ros/config`
+#       directory, and all available params are documented at:
+#       https://ethz-asl.github.io/wavemap/pages/parameters
+
+map:
+  type: hashed_chunked_wavelet_octree
+  min_cell_width: { meters: 0.02 }
+
+map_operations:
+  - type: threshold_map
+    once_every: { seconds: 2.0 }
+  - type: prune_map
+    once_every: { seconds: 10.0 }
+
+measurement_integrators:
+  your_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 }

examples/cpp/mapping/example_pipeline.cc

@@ -0,0 +1,76 @@
+#include <iostream>
+#include <string>
+
+#include <wavemap/core/map/map_factory.h>
+#include <wavemap/io/config/file_conversions.h>
+#include <wavemap/io/map/file_conversions.h>
+#include <wavemap/pipeline/pipeline.h>
+
+using namespace wavemap;
+int main(int, char** argv) {
+  // Settings
+  const std::string config_name = "example_config.yaml";
+  const std::string output_map_name = "example_map.wvmp";
+  const std::filesystem::path current_dir =
+      std::filesystem::canonical(__FILE__).parent_path();
+  const std::filesystem::path config_path = current_dir / config_name;
+  const std::filesystem::path output_map_path = current_dir / output_map_name;
+
+  // Load the config
+  std::cout << "Loading config file: " << config_path << std::endl;
+  const auto params = io::yamlFileToParams(config_path);
+  CHECK(params.has_value());
+
+  // Create the map
+  const auto map_config = params->getChild("map");
+  CHECK(map_config.has_value());
+  MapBase::Ptr map = MapFactory::create(map_config.value());
+  CHECK_NOTNULL(map);
+
+  // Create measurement integration pipeline
+  Pipeline pipeline{map};
+
+  // Add map operations to pipeline
+  const auto map_operations =
+      params->getChildAs<param::Array>("map_operations");
+  CHECK(map_operations);
+  for (const auto& operation_params : map_operations.value()) {
+    pipeline.addOperation(operation_params);
+  }
+
+  // Add measurement integrators to pipeline
+  const auto measurement_integrators =
+      params->getChildAs<param::Map>("measurement_integrators");
+  CHECK(measurement_integrators);
+  for (const auto& [integrator_name, integrator_params] :
+       measurement_integrators.value()) {
+    pipeline.addIntegrator(integrator_name, integrator_params);
+  }
+
+  // Define a depth camera image for illustration
+  const int width = 640;
+  const int height = 480;
+  Image<> depth_image{width, height};
+  depth_image.setToConstant(2.f);
+
+  // Define a depth camera pose for illustration
+  Transformation3D T_W_C{};
+  // Set the camera's [x, y, z] position
+  T_W_C.getPosition() = {0.f, 0.f, 0.f};
+  // Set the camera's orientation
+  // For example as a quaternion's [w, x, y, z] coefficients
+  T_W_C.getRotation() = Rotation3D{0.5f, -0.5f, -0.5f, 0.5f};
+  // NOTE: Alternatively, the rotation can also be loaded from a rotation
+  //       matrix, or T_W_C can be initialized from a transformation matrix.
+
+  // Integrate the measurement
+  pipeline.runPipeline({"your_camera"}, PosedImage<>{T_W_C, depth_image});
+
+  // Measure the map's size
+  const size_t map_size_KB = map->getMemoryUsage() / 1024;
+  std::cout << "Created map of size: " << map_size_KB << " KB" << std::endl;
+
+  // Save the map to disk
+  std::cout << "Saving it to: " << output_map_path << std::endl;
+  io::mapToFile(*map, output_map_path);
+}

examples/python/render/raycast_depth_image.py

@@ -0,0 +1,79 @@
+"""
+Extract depth maps from wavemap maps at a given sensor pose and intrinsics
+"""
+
+import time
+from pathlib import Path
+
+import numpy as np
+from PIL import Image
+import pywavemap as wm
+
+
+def save_depth_as_png(depth_map: np.ndarray, output_path: Path):
+    depth_min = np.min(depth_map)
+    depth_max = np.max(depth_map)
+
+    # Avoid division by zero in case all values are the same
+    if depth_max - depth_min > 0:
+        depth_map_normalized = (depth_map - depth_min) / (depth_max -
+                                                          depth_min)
+    else:
+        depth_map_normalized = np.zeros_like(depth_map)
+
+    # Convert floats (meters) to uint8 and save to png
+    depth_map_8bit = (depth_map_normalized * 255).astype(np.uint8)
+    image = Image.fromarray(depth_map_8bit)
+    image.save(output_path)
+
+
+if __name__ == "__main__":
+    map_path = Path.home() \
+               / "data/panoptic_mapping/flat_dataset/run2/your_map.wvmp"
+    out_path = Path(__file__).parent / "depth.png"
+
+    # Configure the virtual sensor's projection model
+    # NOTE: The projection model can be configured through a Python dictionary.
+    #       Diagnostics for invalid configurations are printed to the terminal.
+    #       For an overview of all the available models and their params, see:
+    #       pylint: disable=line-too-long
+    #       https://ethz-asl.github.io/wavemap/pages/parameters/measurement_integrators.html#projection-models
+    #       Examples for a broad selection of common sensors can be found in the
+    #       `interfaces/ros1/wavemap_ros/config` directory.
+    #       The intrinsics below match the Zed 2i depth camera.
+    projection_model = wm.Projector.create({
+        "type": "pinhole_camera_projector",
+        "width": 1280,
+        "height": 720,
+        "fx": 526.21539307,
+        "fy": 526.21539307,
+        "cx": 642.309021,
+        "cy": 368.69949341
+    })
+
+    # Load map from file
+    your_map = wm.Map.load(map_path)
+
+    # Create the depth image renderer
+    log_odds_occupancy_threshold = 0.1
+    max_range = 6.0
+    default_depth_value = -1.0
+    renderer = wm.RaycastingRenderer(your_map, projection_model,
+                                     log_odds_occupancy_threshold, max_range,
+                                     default_depth_value)
+
+    # Create pose
+    rotation = wm.Rotation(np.eye(3))
+    translation = np.zeros(3)
+    pose = wm.Pose(rotation, translation)
+
+    # Extract depth
+    t1 = time.perf_counter()
+    depth_image = renderer.render(pose)
+    t2 = time.perf_counter()
+    print(f"Depth image of size {depth_image.width}x{depth_image.height} "
+          f"created in {t2 - t1:.02f} seconds")
+    save_depth_as_png(depth_image.data, out_path)
+
+    # Avoids leak warnings on old Python versions with lazy garbage collectors
+    del renderer, depth_image

interfaces/ros1/wavemap_ros/CMakeLists.txt

@@ -10,6 +10,9 @@ find_package(catkin REQUIRED COMPONENTS
     roscpp rosbag cv_bridge image_transport tf2_ros
     std_srvs sensor_msgs visualization_msgs)
 
+# Optional dependencies
+find_package(livox_ros_driver2 QUIET)
+
 # Register catkin package
 catkin_package(
     INCLUDE_DIRS include
@@ -19,14 +22,6 @@ catkin_package(
     roscpp rosbag cv_bridge image_transport tf2_ros
     std_srvs sensor_msgs visualization_msgs)
 
-
-# Optional dependencies
-find_package(livox_ros_driver2 QUIET)
-if (livox_ros_driver2_FOUND)
-  include_directories(${livox_ros_driver2_INCLUDE_DIRS})
-  add_compile_definitions(LIVOX_AVAILABLE)
-endif ()
-
 # Libraries
 add_library(${PROJECT_NAME}
     src/inputs/depth_image_topic_input.cc
@@ -48,6 +43,13 @@ target_include_directories(${PROJECT_NAME}
 target_link_libraries(${PROJECT_NAME}
     PUBLIC ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
 
+# Optional Livox support
+if (livox_ros_driver2_FOUND)
+  target_include_directories(${PROJECT_NAME} PUBLIC
+      ${livox_ros_driver2_INCLUDE_DIRS})
+  target_compile_definitions(${PROJECT_NAME} PUBLIC LIVOX_AVAILABLE)
+endif ()
+
 # Binaries
 add_executable(ros_server app/ros_server.cc)
 set_wavemap_target_properties(ros_server)

interfaces/ros1/wavemap_ros/src/ros_server.cc

@@ -5,7 +5,7 @@
 
 #include <std_srvs/Trigger.h>
 #include <wavemap/core/map/map_factory.h>
-#include <wavemap/io/file_conversions.h>
+#include <wavemap/io/map/file_conversions.h>
 #include <wavemap/pipeline/map_operations/map_operation_factory.h>
 #include <wavemap_msgs/FilePath.h>
 #include <wavemap_ros_conversions/config_conversions.h>
@@ -33,10 +33,10 @@ bool RosServerConfig::isValid(bool verbose) const {
 // NOTE: If RosServerConfig::from(...) fails, accessing its value will throw
 //       an exception and end the program.
 RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private)
-    : RosServer(nh, nh_private,
-                RosServerConfig::from(
-                    param::convert::toParamValue(nh_private, "general"))
-                    .value()) {}
+    : RosServer(
+          nh, nh_private,
+          RosServerConfig::from(convert::rosToParams(nh_private, "general"))
+              .value()) {}
 
 RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private,
                      const RosServerConfig& config)
@@ -47,8 +47,7 @@ RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private,
   config_.logging_level.applyToRosConsole();
 
   // Setup data structure
-  const auto data_structure_params =
-      param::convert::toParamValue(nh_private, "map");
+  const auto data_structure_params = convert::rosToParams(nh_private, "map");
   occupancy_map_ =
       MapFactory::create(data_structure_params, MapType::kHashedBlocks);
   CHECK_NOTNULL(occupancy_map_);
@@ -65,22 +64,22 @@ 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");
+      convert::rosToParamArray(nh_private, "map_operations");
   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");
+      convert::rosToParamMap(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");
+      convert::rosToParamArray(nh_private, "inputs");
   for (const auto& integrator_params : input_param_array) {
     addInput(integrator_params, nh, nh_private);
   }

interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/config_conversions.h

@@ -7,14 +7,14 @@
 #include <wavemap/core/config/config_base.h>
 #include <xmlrpcpp/XmlRpcValue.h>
 
-namespace wavemap::param::convert {
-param::Map toParamMap(const ros::NodeHandle& nh, const std::string& ns);
-param::Array toParamArray(const ros::NodeHandle& nh, const std::string& ns);
-param::Value toParamValue(const ros::NodeHandle& nh, const std::string& ns);
+namespace wavemap::convert {
+param::Map xmlRpcToParamMap(const XmlRpc::XmlRpcValue& xml_rpc_value);
+param::Array xmlRpcToParamArray(const XmlRpc::XmlRpcValue& xml_rpc_value);
+param::Value xmlRpcToParams(const XmlRpc::XmlRpcValue& xml_rpc_value);
 
-param::Map toParamMap(const XmlRpc::XmlRpcValue& xml_rpc_value);
-param::Array toParamArray(const XmlRpc::XmlRpcValue& xml_rpc_value);
-param::Value toParamValue(const XmlRpc::XmlRpcValue& xml_rpc_value);
-}  // namespace wavemap::param::convert
+param::Map rosToParamMap(const ros::NodeHandle& nh, const std::string& ns);
+param::Array rosToParamArray(const ros::NodeHandle& nh, const std::string& ns);
+param::Value rosToParams(const ros::NodeHandle& nh, const std::string& ns);
+}  // namespace wavemap::convert
 
 #endif  // WAVEMAP_ROS_CONVERSIONS_CONFIG_CONVERSIONS_H_