Feature/lifelong compression

CMakeLists.txt

@@ -3,7 +3,7 @@ project(slam_toolbox)
 
 set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 17)
 endif()
 cmake_policy(SET CMP0077 NEW)
 
@@ -70,6 +70,7 @@ set(libraries
     sync_slam_toolbox
     localization_slam_toolbox
     lifelong_slam_toolbox
+    information_estimates
 )
 
 find_package(PkgConfig REQUIRED)
@@ -144,6 +145,10 @@ ament_target_dependencies(toolbox_common
 target_link_libraries(toolbox_common kartoSlamToolbox ${Boost_LIBRARIES})
 rosidl_target_interfaces(toolbox_common ${PROJECT_NAME} "rosidl_typesupport_cpp")
 
+#### Adding information estimates library
+add_library(information_estimates src/experimental/information_estimates.cpp src/experimental/utils.cpp)
+target_link_libraries(information_estimates toolbox_common kartoSlamToolbox ${Boost_LIBRARIES})
+
 #### Mapping executibles
 add_library(async_slam_toolbox src/slam_toolbox_async.cpp)
 target_link_libraries(async_slam_toolbox toolbox_common kartoSlamToolbox ${Boost_LIBRARIES})
@@ -163,7 +168,7 @@ target_link_libraries(localization_slam_toolbox_node localization_slam_toolbox)
 add_library(lifelong_slam_toolbox src/experimental/slam_toolbox_lifelong.cpp)
 target_link_libraries(lifelong_slam_toolbox toolbox_common kartoSlamToolbox ${Boost_LIBRARIES})
 add_executable(lifelong_slam_toolbox_node src/experimental/slam_toolbox_lifelong_node.cpp)
-target_link_libraries(lifelong_slam_toolbox_node lifelong_slam_toolbox)
+target_link_libraries(lifelong_slam_toolbox_node lifelong_slam_toolbox information_estimates)
 
 #### Merging maps tool
 add_executable(merge_maps_kinematic src/merge_maps_kinematic.cpp)
@@ -179,6 +184,15 @@ target_link_libraries(merge_maps_kinematic kartoSlamToolbox toolbox_common)
 #  target_link_libraries(lifelong_metrics_test lifelong_slam_toolbox)
 #endif()
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  include_directories(test)
+  ament_add_gtest(information_estimates_test test/information_estimates_test.cpp)
+  target_link_libraries(information_estimates_test information_estimates)
+endif()
+
 #### Install
 install(TARGETS async_slam_toolbox_node
                 sync_slam_toolbox_node

include/slam_toolbox/experimental/information_estimates.hpp

@@ -0,0 +1,126 @@
+#ifndef SLAM_TOOLBOX__EXPERIMENTAL__THEORETIC_INFORMATION_HPP_
+#define SLAM_TOOLBOX__EXPERIMENTAL__THEORETIC_INFORMATION_HPP_
+
+#include "utils.hpp"
+
+class InformationEstimates
+{
+    typedef std::map<karto::Vector2<int>, std::vector<std::vector<kt_double>>> map_cell_probabilities;
+    typedef std::tuple<int, int, int> map_tuple;
+    typedef std::unordered_map<map_tuple, kt_double, utils::tuple_hash::HashTuple> probability_map;
+
+public:
+    InformationEstimates(kt_double resolution);
+    InformationEstimates();
+    virtual ~InformationEstimates() {}
+
+public:
+    std::vector<kt_double> findMutualInfo(std::vector<karto::LocalizedRangeScan*> const& range_scans);
+
+private:
+    struct map_dimensions
+    {
+        kt_double x;
+        kt_double y;
+    };
+
+    struct scan_position_limits
+    {
+        karto::Vector2<kt_double> lower;
+        karto::Vector2<kt_double> upper;
+    };
+
+private:
+    void calculateAndAppendCellProbabilities(
+        std::vector<karto::Vector2<int>> & visited_cells,
+        std::vector<kt_double> const & distances,
+        karto::Vector2<int> const & cell
+    );
+
+    std::optional<std::vector<kt_double>> calculateBeamAndCellIntersections(
+        utils::Segment2<kt_double> const & beam_segment,
+        karto::Vector2<int> const & cell
+    );
+
+    std::optional<kt_double> adjustBeamReadingDistance(
+        kt_double const & beam_distance,
+        kt_double const & distance_to_cell
+    );
+
+    karto::Vector2<int> getLaserBeamCell(
+        kt_double const & angle_to_cell,
+        kt_double const & reading_distance
+    );
+
+    std::vector<karto::Vector2<int>> getScanGroupVisitedCells(
+        std::vector<karto::LocalizedRangeScan *> const & range_scans,
+        karto::LaserRangeFinder *laser_range_finder,
+        int const & scan_to_skip
+    );
+
+    void calculateScanGroupMutualInformation(
+        std::vector<karto::Vector2<int>> const & visited_cells,
+        std::vector<kt_double> & scans_mutual_information
+    );
+
+    std::optional<int> findClosestLaserIndexToCell(
+        kt_double const & angle_to_cell,
+        kt_double const & scan_pose_heading,
+        karto::LaserRangeFinder *laser_range_finder
+    );
+
+    void calculateCellProbabilities(
+        std::vector<karto::LocalizedRangeScan *> const & range_scans,
+        std::vector<karto::Vector2<int>> & visited_cells,
+        karto::Vector2<int> const & cell,
+        karto::LaserRangeFinder *laser_range_finder,
+        int const & scan_to_skip
+    );
+
+    void resizeGridFromScans(
+        std::vector<karto::LocalizedRangeScan *> const & range_scans
+    );
+
+    std::vector<kt_double> getScanGroupMutualInformation(
+        std::vector<karto::LocalizedRangeScan *> const & range_scans
+    );
+
+    // Mutual information
+    kt_double calculateInformationContent(kt_double prob);
+    kt_double measurementOutcomeEntropy(map_tuple const& meas_outcome);
+    kt_double calculateProbabilityFromLogOdds(kt_double log);
+
+    // Measurement outcomes probabilities
+    void appendCellProbabilities(std::vector<kt_double>& measurements, karto::Vector2<int> const & cell);
+    probability_map computeMeasurementOutcomesHistogram(std::vector<std::vector<kt_double>>& meas_outcm);
+    void insertMeasurementOutcome(map_tuple tuple, kt_double probability, probability_map& map);
+
+    // Measurements calculations <P(free), P(Occ), P(Unk)>
+    kt_double calculateScanMassProbabilityBetween(kt_double range_1, kt_double range_2);
+
+private:
+    // Data structures
+    map_cell_probabilities m_cell_probabilities;
+
+    // Configuration
+    kt_double m_cell_resol;
+    kt_double m_max_sensor_range;
+
+    // Probability constants
+    const kt_double l_free;
+    const kt_double l_occ;
+    const kt_double l_o;
+
+    // Factor to calculate exponential distribution rate
+    const kt_double lambda_factor { 6.0 };
+
+    // Map grids
+    Eigen::MatrixXd m_mutual_grid;
+    Eigen::MatrixXi m_visited_grid;
+
+    // Map limits
+    map_dimensions m_map_dim;
+    scan_position_limits m_scan_limits;
+};
+
+#endif

include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp

@@ -21,12 +21,14 @@
 
 #include <memory>
 #include "slam_toolbox/slam_toolbox_common.hpp"
+#include "slam_toolbox/experimental/information_estimates.hpp"
 
 namespace slam_toolbox
 {
 
 class LifelongSlamToolbox : public SlamToolbox
 {
+
 public:
   explicit LifelongSlamToolbox(rclcpp::NodeOptions options);
   ~LifelongSlamToolbox() {}
@@ -74,6 +76,9 @@ class LifelongSlamToolbox : public SlamToolbox
   double candidates_scale_;
   double iou_match_;
   double nearby_penalty_;
+
+  InformationEstimates inf_estimates_{ 0.1 };
+
 };
 
 }  // namespace slam_toolbox

include/slam_toolbox/experimental/utils.hpp

@@ -0,0 +1,78 @@
+#ifndef SLAM_TOOLBOX__EXPERIMENTAL__UTILS_HPP_
+#define SLAM_TOOLBOX__EXPERIMENTAL__UTILS_HPP_
+
+#include <algorithm>
+#include <array>
+#include <memory>
+#include <vector>
+#include <tuple>
+#include <cmath>
+#include <map>
+#include <unordered_map>
+#include "lib/karto_sdk/include/karto_sdk/Karto.h"
+#include "Eigen/Core"
+
+#include <optional>
+
+namespace utils
+{
+    template<typename T>
+    struct Segment2
+    {
+        karto::Vector2<T> start;
+        karto::Vector2<T> end;
+    };
+
+    namespace grid_operations
+    {
+        const kt_double cell_limit_eps { 0.001 };
+
+        void updateCellLimits(
+            std::array<karto::Vector2<kt_double>, 4> & initial_points,
+            std::array<karto::Vector2<kt_double>, 4> & final_points,
+            karto::Vector2<kt_double> const & cell_reference_pos,
+            std::array<kt_double, 4> & cell_limits,
+            utils::Segment2<int> const & discretized_segment,
+            kt_double const & resolution
+        );
+
+        int signum(int num);
+
+        karto::Vector2<int> discretize(
+            karto::Vector2<kt_double> const & position,
+            kt_double const & resolution
+        );
+
+        karto::Vector2<kt_double> calculateCellIntersectionPoints(
+            utils::Segment2<kt_double> const & segment_1,
+            utils::Segment2<kt_double> const & segment_2
+        );
+
+        std::pair<std::vector<kt_double>, std::vector<kt_double>> computeLineBoxIntersection(
+            utils::Segment2<kt_double> const & segment,
+            karto::Vector2<kt_double> const & cell_reference_pos,
+            kt_double const & resolution
+        );
+    } // namespace grid_operations
+
+    namespace tuple_hash
+    {
+        struct HashTuple
+        {
+            std::size_t operator() (std::tuple<int, int, int> const& key) const
+            {
+                /**
+                 * Tuple Hashing
+                */
+                std::size_t hash = 5381u;
+                hash = (hash << 5) + hash + std::get<0>(key);
+                hash = (hash << 5) + hash + std::get<1>(key);
+                hash = (hash << 5) + hash + std::get<2>(key);
+                return hash;
+            }
+        };
+    } // namespace tuple_hash
+
+} // namespace utils
+
+#endif