Clarify that wavemap returns estimates in log odds; add conversion utils

docs/pages/faq.rst

@@ -1,6 +1,12 @@
 FAQ
 ###
 
-We do not yet have FAQs.
+If you have a question that is not yet answered below, feel free to open a `GitHub Issue <https://github.com/ethz-asl/wavemap/issues>`_ or contact us over email.
 
-If you have a question, please do not hesitate to open a `GitHub Issue <https://github.com/ethz-asl/wavemap/issues>`_.
+How do I query if a point in the map is occupied?
+=================================================
+Please see the :doc:`usage examples <usage_examples>` on :ref:`interpolation <examples-interpolation>` and :ref:`classification <examples-classification>`.
+
+Does wavemap support (Euclidean) Signed Distance Fields?
+========================================================
+Not yet, but we will add this feature in the near future.

docs/pages/usage_examples.rst

@@ -48,6 +48,8 @@ Accelerators
 
 Interpolation
 =============
+.. _examples-interpolation:
+
 Nearest neighbor interpolation:
 
 .. literalinclude:: ../../examples/src/queries/nearest_neighbor_interpolation.cc
@@ -57,3 +59,10 @@ Trilinear interpolation:
 
 .. literalinclude:: ../../examples/src/queries/trilinear_interpolation.cc
     :language: c++
+
+Classification
+==============
+.. _examples-classification:
+
+.. literalinclude:: ../../examples/src/queries/classification.cc
+    :language: c++

examples/CMakeLists.txt

@@ -19,13 +19,18 @@ cs_add_executable(receive_map_over_ros
         src/io/receive_map_over_ros.cc)
 cs_add_executable(send_map_over_ros
         src/io/send_map_over_ros.cc)
+
 cs_add_executable(fixed_resolution
         src/queries/fixed_resolution.cc)
 cs_add_executable(multi_resolution
         src/queries/multi_resolution.cc)
 cs_add_executable(accelerated_queries
         src/queries/accelerated_queries.cc)
+
 cs_add_executable(nearest_neighbor_interpolation
         src/queries/nearest_neighbor_interpolation.cc)
 cs_add_executable(trilinear_interpolation
         src/queries/trilinear_interpolation.cc)
+
+cs_add_executable(classification
+        src/queries/classification.cc)

examples/src/queries/accelerated_queries.cc

@@ -17,7 +17,8 @@ int main(int, char**) {
   // Query all points within a grid
   for (const auto& query_index :
        Grid<3>(Index3D::Constant(-10), Index3D::Constant(10))) {
-    const FloatingPoint value = query_accelerator.getCellValue(query_index);
-    examples::doSomething(value);
+    const FloatingPoint occupancy_log_odds =
+        query_accelerator.getCellValue(query_index);
+    examples::doSomething(occupancy_log_odds);
   }
 }

examples/src/queries/classification.cc

@@ -0,0 +1,52 @@
+#include <wavemap/utils/query/probability_conversions.h>
+
+#include "wavemap_examples/common.h"
+
+using namespace wavemap;
+int main(int, char**) {
+  // Declare a floating point value representing the occupancy posterior in log
+  // odds as queried from the map in one of the previous examples
+  const FloatingPoint occupancy_log_odds{};
+
+  // A point is considered unobserved if its occupancy posterior is equal to the
+  // prior. Wavemap assumes that an unobserved point is equally likely to be
+  // free or occupied. In other words, the prior occupancy probability is 0.5,
+  // which corresponds to a log odds value of 0.0. Accounting for numerical
+  // noise, checking whether a point is unobserved can be done as follows:
+  constexpr FloatingPoint kUnobservedThreshold = 1e-3;
+  const bool is_unobserved =
+      std::abs(occupancy_log_odds) < kUnobservedThreshold;
+  examples::doSomething(is_unobserved);
+
+  // In case you would like to convert log odds into probabilities, we provide
+  // the following convenience function:
+  const FloatingPoint occupancy_probability =
+      convert::logOddsToProbability(occupancy_log_odds);
+  examples::doSomething(occupancy_probability);
+
+  // To classify whether a point is estimated to be occupied or free, you need
+  // to choose a discrimination threshold. A reasonable default threshold is 0.5
+  // (probability), which corresponds to 0.0 log odds.
+  constexpr FloatingPoint kOccupancyThresholdProb = 0.5;
+  constexpr FloatingPoint kOccupancyThresholdLogOdds = 0.0;
+
+  // NOTE: To taylor the threshold, we recommend running wavemap on a dataset
+  //       that is representative of your application and analyzing the Receiver
+  //       Operating Characteristic curve.
+
+  // Once a threshold has been chosen, you can either classify in log space
+  {
+    const bool is_occupied = kOccupancyThresholdLogOdds < occupancy_log_odds;
+    const bool is_free = occupancy_log_odds < kOccupancyThresholdLogOdds;
+    examples::doSomething(is_occupied);
+    examples::doSomething(is_free);
+  }
+
+  // Or in probability space
+  {
+    const bool is_occupied = kOccupancyThresholdProb < occupancy_probability;
+    const bool is_free = occupancy_probability < kOccupancyThresholdProb;
+    examples::doSomething(is_occupied);
+    examples::doSomething(is_free);
+  }
+}

examples/src/queries/fixed_resolution.cc

@@ -15,6 +15,6 @@ int main(int, char**) {
   const Index3D query_index = Index3D::Zero();
 
   // Query the map
-  const FloatingPoint value = map->getCellValue(query_index);
-  examples::doSomething(value);
+  const FloatingPoint occupancy_log_odds = map->getCellValue(query_index);
+  examples::doSomething(occupancy_log_odds);
 }

examples/src/queries/multi_resolution.cc

@@ -22,6 +22,6 @@ int main(int, char**) {
       convert::pointToNodeIndex(query_point, map_min_cell_width, query_height);
 
   // Query the map
-  const FloatingPoint value = map->getCellValue(query_index);
-  examples::doSomething(value);
+  const FloatingPoint occupancy_log_odds = map->getCellValue(query_index);
+  examples::doSomething(occupancy_log_odds);
 }

examples/src/queries/nearest_neighbor_interpolation.cc

@@ -19,6 +19,7 @@ int main(int, char**) {
       convert::pointToNearestIndex(query_point, min_cell_width_inv);
 
   // Query the map
-  const FloatingPoint value = map->getCellValue(nearest_neighbor_index);
-  examples::doSomething(value);
+  const FloatingPoint occupancy_log_odds =
+      map->getCellValue(nearest_neighbor_index);
+  examples::doSomething(occupancy_log_odds);
 }

examples/src/queries/trilinear_interpolation.cc

@@ -14,6 +14,7 @@ int main(int, char**) {
   const Point3D query_point = Point3D::Zero();
 
   // Query the map and compute the interpolated value
-  const FloatingPoint value = interpolate::trilinear(*map, query_point);
-  examples::doSomething(value);
+  const FloatingPoint occupancy_log_odds =
+      interpolate::trilinear(*map, query_point);
+  examples::doSomething(occupancy_log_odds);
 }

libraries/wavemap/CMakeLists.txt

@@ -113,6 +113,7 @@ if (CATKIN_ENABLE_TESTING)
       test/src/utils/test_data_utils.cc
       test/src/utils/test_fill_utils.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_thread_pool.cc

libraries/wavemap/include/wavemap/utils/query/probability_conversions.h

@@ -0,0 +1,26 @@
+#ifndef WAVEMAP_UTILS_QUERY_PROBABILITY_CONVERSIONS_H_
+#define WAVEMAP_UTILS_QUERY_PROBABILITY_CONVERSIONS_H_
+
+#include <limits>
+
+#include "wavemap/common.h"
+
+namespace wavemap::convert {
+constexpr FloatingPoint kLogOddsNumericalMin =
+    std::numeric_limits<FloatingPoint>::lowest();
+const FloatingPoint kLogOddsNumericalMax =
+    std::nextafter(std::log(std::numeric_limits<FloatingPoint>::max()), 0.f);
+
+constexpr FloatingPoint logOddsToProbability(FloatingPoint log_odds) {
+  const FloatingPoint odds = std::exp(log_odds);
+  const FloatingPoint prob = odds / (1.f + odds);
+  return prob;
+}
+
+constexpr FloatingPoint probabilityToLogOdds(FloatingPoint probability) {
+  const FloatingPoint odds = probability / (1.f - probability);
+  return std::log(odds);
+}
+}  // namespace wavemap::convert
+
+#endif  // WAVEMAP_UTILS_QUERY_PROBABILITY_CONVERSIONS_H_

libraries/wavemap/test/src/utils/test_log_odds_converter.cc

@@ -0,0 +1,27 @@
+#include <cmath>
+
+#include <gtest/gtest.h>
+
+#include "wavemap/utils/query/probability_conversions.h"
+
+namespace wavemap {
+TEST(ProbabilityConversionsTest, LogOddsToProbability) {
+  constexpr auto kInf = std::numeric_limits<FloatingPoint>::infinity();
+  EXPECT_NEAR(convert::logOddsToProbability(convert::kLogOddsNumericalMin), 0.f,
+              kEpsilon);
+  EXPECT_NEAR(convert::logOddsToProbability(convert::kLogOddsNumericalMax), 1.f,
+              kEpsilon);
+  EXPECT_EQ(convert::probabilityToLogOdds(0.f), -kInf);
+  EXPECT_EQ(convert::probabilityToLogOdds(1.f), kInf);
+
+  constexpr int kNumSteps = 1000;
+  for (int step_idx = 1; step_idx < kNumSteps; ++step_idx) {
+    const FloatingPoint probability = static_cast<FloatingPoint>(step_idx) /
+                                      static_cast<FloatingPoint>(kNumSteps);
+    const FloatingPoint log_odds = convert::probabilityToLogOdds(probability);
+    const FloatingPoint probability_round_trip =
+        convert::logOddsToProbability(log_odds);
+    EXPECT_NEAR(probability_round_trip, probability, kEpsilon);
+  }
+}
+}  // namespace wavemap