Feature/lifelong compression

CMakeLists.txt

@@ -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,122 @@
+#ifndef SLAM_TOOLBOX__EXPERIMENTAL__THEORETIC_INFORMATION_HPP_
+#define SLAM_TOOLBOX__EXPERIMENTAL__THEORETIC_INFORMATION_HPP_
+
+#include "utils.hpp"
+
+class InformationEstimates
+{
+    typedef std::tuple<int, int, int> map_tuple;
+
+public:
+    InformationEstimates(kt_double sensor_range, kt_double resolution, kt_double lambda, kt_double nu);
+    InformationEstimates();
+    virtual ~InformationEstimates() {}
+
+public:
+    std::vector<kt_double> findMutualInfo(std::vector<karto::LocalizedRangeScan*> const& range_scans);
+
+private:
+
+    void calculateAndAppendCellProbabilities(
+        std::vector<karto::Vector2<int>> & visited_cells,
+        std::vector<kt_double> const & distances,
+        karto::Vector2<int> const & cell
+    );
+
+    std::vector<kt_double> calculateBeamAndCellIntersections(
+        kt_bool & skip_cell_eval,
+        karto::Vector2<kt_double> const & scan_position,
+        karto::Vector2<kt_double> const & beam_read_point,
+        karto::Vector2<int> const & cell
+    );
+
+    int findClosestLaserIndexToCell(
+        kt_bool & skip_cell_eval,
+        kt_double const & angle_to_cell,
+        kt_double const & scan_pose_heading,
+        karto::LaserRangeFinder *laser_range_finder
+    );
+
+    void adjustBeamReadingDistance(
+        kt_bool & skip_cell_eval,
+        kt_double & beam_distance,
+        kt_double const & distance_to_cell,
+        karto::LaserRangeFinder *laser_range_finder
+    );
+
+    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
+    );
+
+    int findClosestLaserIndexToCell(
+        karto::Vector2<int> const & cell,
+        karto::Pose2 const & grid_scan_pose);
+
+    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);
+    std::unordered_map<map_tuple, kt_double, utils::tuple_hash::HashTuple> computeMeasurementOutcomesHistogram(std::vector<std::vector<kt_double>>& meas_outcm);
+    void insertMeasurementOutcome(map_tuple tuple, kt_double probability, std::unordered_map<map_tuple, kt_double, utils::tuple_hash::HashTuple>& map);
+
+    // Measurements calculations <P(free), P(Occ), P(Unk)>
+    kt_double calculateScanMassProbabilityBetween(kt_double range_1, kt_double range_2);
+
+private:
+    // Data structures
+    std::map<karto::Vector2<int>, std::vector<std::vector<kt_double>>> m_cell_probabilities;
+
+    const kt_double l_free = log(0.3 / (1.0 - 0.3));
+    const kt_double l_occ = log(0.7 / (1.0 - 0.7));
+    const kt_double l_o = log(0.5 / (1.0 - 0.5));
+
+    kt_double m_max_sensor_range;
+    kt_double m_cell_resol;
+    kt_double m_obs_lambda;
+    kt_double m_obs_nu;
+    kt_double m_map_dist;
+
+    Eigen::MatrixXd m_mutual_grid;
+    Eigen::MatrixXi m_visited_grid;
+
+    kt_double m_upper_limit_x;
+    kt_double m_upper_limit_y;
+    kt_double m_lower_limit_x;
+    kt_double m_lower_limit_y;
+
+    kt_double m_map_dim_x;
+    kt_double m_map_dim_y;
+};
+
+#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_{10.0, 0.05, 0.35, 0.28};
+
 };
 
 }  // namespace slam_toolbox

include/slam_toolbox/experimental/utils.hpp

@@ -0,0 +1,79 @@
+#ifndef SLAM_TOOLBOX__EXPERIMENTAL__UTILS_HPP_
+#define SLAM_TOOLBOX__EXPERIMENTAL__UTILS_HPP_
+
+#include <algorithm>
+#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"
+
+namespace utils
+{
+    namespace grid_operations
+    {
+        void updateCellLimits(std::vector<kt_double>& initial_x,
+            std::vector<kt_double>& initial_y,
+            std::vector<kt_double>& final_x,
+            std::vector<kt_double>& final_y,
+            kt_double limit_x,
+            kt_double limit_y,
+            std::vector<kt_double>& cell_limits,
+            karto::Vector2<int> const& robot_grid_pos,
+            karto::Vector2<int> const& final_grid_pos,
+            kt_double resolution
+        );
+
+        int signum(int num);
+
+        karto::Vector2<int> getGridPosition(
+            karto::Vector2<kt_double> const& position,
+            kt_double resolution
+        );
+
+        karto::Vector2<kt_double> calculateCellIntersectionPoints(
+            karto::Vector2<kt_double> const & laser_start,
+            karto::Vector2<kt_double> const & laser_end,
+            karto::Vector2<kt_double> const & cell_start,
+            karto::Vector2<kt_double> const & cell_end
+        );
+
+        std::pair<std::vector<kt_double>, std::vector<kt_double>> computeLineBoxIntersection(
+            karto::Vector2<kt_double> const & laser_start,
+            karto::Vector2<kt_double> const & laser_end,
+            karto::Vector2<int> const& robot_grid_pos,
+            karto::Vector2<int> const& final_grid_pos,
+            kt_double limit_x,
+            kt_double limit_y,
+            kt_double resolution
+        );
+
+        void clearVisitedCells(Eigen::MatrixXd & grid);
+
+        void clearVisitedCells(Eigen::MatrixXi & grid);
+    } // 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