diff --git a/CMakeLists.txt b/CMakeLists.txt index 40f27939..29b7279b 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp new file mode 100644 index 00000000..7644691c --- /dev/null +++ b/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, std::vector>> map_cell_probabilities; + typedef std::tuple map_tuple; + typedef std::unordered_map probability_map; + +public: + InformationEstimates(kt_double resolution); + InformationEstimates(); + virtual ~InformationEstimates() {} + +public: + std::vector findMutualInfo(std::vector const& range_scans); + +private: + struct map_dimensions + { + kt_double x; + kt_double y; + }; + + struct scan_position_limits + { + karto::Vector2 lower; + karto::Vector2 upper; + }; + +private: + void calculateAndAppendCellProbabilities( + std::vector> & visited_cells, + std::vector const & distances, + karto::Vector2 const & cell + ); + + std::optional> calculateBeamAndCellIntersections( + utils::Segment2 const & beam_segment, + karto::Vector2 const & cell + ); + + std::optional adjustBeamReadingDistance( + kt_double const & beam_distance, + kt_double const & distance_to_cell + ); + + karto::Vector2 getLaserBeamCell( + kt_double const & angle_to_cell, + kt_double const & reading_distance + ); + + std::vector> getScanGroupVisitedCells( + std::vector const & range_scans, + karto::LaserRangeFinder *laser_range_finder, + int const & scan_to_skip + ); + + void calculateScanGroupMutualInformation( + std::vector> const & visited_cells, + std::vector & scans_mutual_information + ); + + std::optional findClosestLaserIndexToCell( + kt_double const & angle_to_cell, + kt_double const & scan_pose_heading, + karto::LaserRangeFinder *laser_range_finder + ); + + void calculateCellProbabilities( + std::vector const & range_scans, + std::vector> & visited_cells, + karto::Vector2 const & cell, + karto::LaserRangeFinder *laser_range_finder, + int const & scan_to_skip + ); + + void resizeGridFromScans( + std::vector const & range_scans + ); + + std::vector getScanGroupMutualInformation( + std::vector 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& measurements, karto::Vector2 const & cell); + probability_map computeMeasurementOutcomesHistogram(std::vector>& meas_outcm); + void insertMeasurementOutcome(map_tuple tuple, kt_double probability, probability_map& map); + + // Measurements calculations + 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 diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index d77bdda9..5099e134 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -21,12 +21,14 @@ #include #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 diff --git a/include/slam_toolbox/experimental/utils.hpp b/include/slam_toolbox/experimental/utils.hpp new file mode 100644 index 00000000..ef2a8416 --- /dev/null +++ b/include/slam_toolbox/experimental/utils.hpp @@ -0,0 +1,78 @@ +#ifndef SLAM_TOOLBOX__EXPERIMENTAL__UTILS_HPP_ +#define SLAM_TOOLBOX__EXPERIMENTAL__UTILS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "lib/karto_sdk/include/karto_sdk/Karto.h" +#include "Eigen/Core" + +#include + +namespace utils +{ + template + struct Segment2 + { + karto::Vector2 start; + karto::Vector2 end; + }; + + namespace grid_operations + { + const kt_double cell_limit_eps { 0.001 }; + + void updateCellLimits( + std::array, 4> & initial_points, + std::array, 4> & final_points, + karto::Vector2 const & cell_reference_pos, + std::array & cell_limits, + utils::Segment2 const & discretized_segment, + kt_double const & resolution + ); + + int signum(int num); + + karto::Vector2 discretize( + karto::Vector2 const & position, + kt_double const & resolution + ); + + karto::Vector2 calculateCellIntersectionPoints( + utils::Segment2 const & segment_1, + utils::Segment2 const & segment_2 + ); + + std::pair, std::vector> computeLineBoxIntersection( + utils::Segment2 const & segment, + karto::Vector2 const & cell_reference_pos, + kt_double const & resolution + ); + } // namespace grid_operations + + namespace tuple_hash + { + struct HashTuple + { + std::size_t operator() (std::tuple 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 diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp new file mode 100644 index 00000000..5474defc --- /dev/null +++ b/src/experimental/information_estimates.cpp @@ -0,0 +1,525 @@ +#include +#include +#include "slam_toolbox/experimental/information_estimates.hpp" + +#include + +InformationEstimates::InformationEstimates(kt_double resolution) + : m_cell_resol { resolution }, + m_max_sensor_range { 25.0 }, + l_free { log(0.3 / (1.0 - 0.3)) }, + l_occ { log(0.7 / (1.0 - 0.7)) }, + l_o { log(0.5 / (1.0 - 0.5)) }, + m_map_dim { 0.0, 0.0 }, + m_scan_limits { {0.0, 0.0}, {0.0, 0.0} } +{ +} + +InformationEstimates::InformationEstimates() + : m_cell_resol { 0.1 }, + m_max_sensor_range { 25.0 }, + l_free { log(0.3 / (1.0 - 0.3)) }, + l_occ { log(0.7 / (1.0 - 0.7)) }, + l_o { log(0.5 / (1.0 - 0.5)) }, + m_map_dim { 0.0, 0.0 }, + m_scan_limits { {0.0, 0.0}, {0.0, 0.0} } +{ +} + +/*************************************************************************/ +// +void InformationEstimates::resizeGridFromScans(std::vector const & range_scans) +{ + m_scan_limits.lower.SetX(range_scans[0]->GetCorrectedPose().GetX()); + m_scan_limits.lower.SetY(range_scans[0]->GetCorrectedPose().GetY()); + m_scan_limits.upper.SetX(range_scans[0]->GetCorrectedPose().GetX()); + m_scan_limits.upper.SetY(range_scans[0]->GetCorrectedPose().GetY()); + + + karto::LaserRangeFinder *laser_range_finder = range_scans[0]->GetLaserRangeFinder(); + m_max_sensor_range = laser_range_finder->GetRangeThreshold(); + + for (const auto &scan : range_scans) + { + if (scan == nullptr) + { + continue; + } + + karto::Pose2 pose = scan->GetCorrectedPose(); + kt_double minimumAngle = scan->GetLaserRangeFinder()->GetMinimumAngle(); + kt_double angularResolution = scan->GetLaserRangeFinder()->GetAngularResolution(); + + // Finding the lower limit pose + m_scan_limits.lower.SetX(std::min(pose.GetX(), m_scan_limits.lower.GetX())); + m_scan_limits.lower.SetY(std::min(pose.GetY(), m_scan_limits.lower.GetY())); + + // Finding the higher limit pose + m_scan_limits.upper.SetX(std::max(pose.GetX(), m_scan_limits.upper.GetX())); + m_scan_limits.upper.SetY(std::max(pose.GetY(), m_scan_limits.upper.GetY())); + } + + // Map dimensions + m_map_dim.x = std::fabs(m_scan_limits.upper.GetX() - m_scan_limits.lower.GetX()) + (2 * m_max_sensor_range); + m_map_dim.y = std::fabs(m_scan_limits.upper.GetY() - m_scan_limits.lower.GetY()) + (2 * m_max_sensor_range); + + // Calculate the number of cells + int n_cells_x = static_cast(m_map_dim.x / m_cell_resol); + int n_cells_y = static_cast(m_map_dim.y / m_cell_resol); + + // Resize the grid with new number of cells + m_mutual_grid.resize(n_cells_x, n_cells_y); + m_visited_grid.resize(n_cells_x, n_cells_y); +} +// +/*************************************************************************/ + + +/*************************************************************************/ +// +std::optional InformationEstimates::findClosestLaserIndexToCell( + kt_double const & angle_to_cell, + kt_double const & scan_pose_heading, + karto::LaserRangeFinder *laser_range_finder) +{ + const kt_double angle_between = angle_to_cell - scan_pose_heading; + const kt_double angle_diff = atan2(sin(angle_between), cos(angle_between)); + + if (angle_diff < laser_range_finder->GetMinimumAngle() || angle_diff > laser_range_finder->GetMaximumAngle()) + { + // We are outside the laser FOV, but we cannot cancel it + // because we will not evaluate other cells + return {}; + } + + const kt_double laser_middle_point = ( laser_range_finder->GetMaximumAngle() - laser_range_finder->GetMinimumAngle()) / 2.0; + return std::optional { (laser_middle_point + angle_between) / laser_range_finder->GetAngularResolution() }; +} +// +/*************************************************************************/ + + +/*************************************************************************/ +// +std::optional> InformationEstimates::calculateBeamAndCellIntersections( + utils::Segment2 const & beam_segment, + karto::Vector2 const & cell) +{ + karto::Vector2 current_cell_position{ cell.GetX() * m_cell_resol, cell.GetY() * m_cell_resol }; + + std::pair, std::vector> intersections = + utils::grid_operations::computeLineBoxIntersection( + beam_segment, + current_cell_position, + m_cell_resol + ); + + if (intersections.first.size() == 0) + { + return {}; + } + + // Enter (d1) and Exit (d2) distances + std::vector distances; + for (int k = 0; k < intersections.first.size(); ++k) + { + // From robot position to intersection points + karto::Vector2 intersection{ + intersections.first[k], + intersections.second[k] + }; + kt_double distance = beam_segment.start.Distance(intersection); + distances.push_back(distance); + } + std::sort(distances.begin(), distances.end()); + + return std::optional{ distances }; +} +// +/*************************************************************************/ + +/*************************************************************************/ +// +std::optional InformationEstimates::adjustBeamReadingDistance( + kt_double const & beam_distance, + kt_double const & distance_to_cell) +{ + if ((beam_distance > m_max_sensor_range) || (beam_distance < distance_to_cell)) + { + return {}; + } + return std::optional { distance_to_cell }; +} +// +/*************************************************************************/ + + +/*************************************************************************/ +// +void InformationEstimates::calculateAndAppendCellProbabilities( + std::vector> & visited_cells, + std::vector const & distances, + karto::Vector2 const & cell) +{ + // Measurement outcomes vector {Pfree, Pocc, Pun} + std::vector probabilities{ + calculateScanMassProbabilityBetween(distances[1], m_max_sensor_range), + calculateScanMassProbabilityBetween(distances[0], distances[1]), + calculateScanMassProbabilityBetween(0.0f, distances[0]) + }; + + visited_cells.push_back( {cell.GetX(), cell.GetY()}); + + // Appending new measurement outcomes for the current cell + appendCellProbabilities(probabilities, cell); +} +// +/*************************************************************************/ + + +/*************************************************************************/ +// +void InformationEstimates::calculateCellProbabilities( + std::vector const & range_scans, + std::vector> & visited_cells, + karto::Vector2 const & cell, + karto::LaserRangeFinder *laser_range_finder, + int const & scan_to_skip) +{ + for (int s = 0; s < range_scans.size(); ++s) + { + // Skip the given scan, because we are evaluating the mutual information + // of the group, excluding it. + if (scan_to_skip == s) + { + continue; + } + + // Scan pose + karto::Pose2 scan_pose = range_scans[s]->GetCorrectedPose(); + + karto::Pose2 grid_scan_pose { + scan_pose.GetX() - (m_scan_limits.lower.GetX() - m_max_sensor_range), + scan_pose.GetY() - (m_scan_limits.lower.GetY() - m_max_sensor_range), + scan_pose.GetHeading() + }; + + karto::Vector2 cell_center{cell.GetX() * m_cell_resol + (m_cell_resol / 2), cell.GetY() * m_cell_resol + (m_cell_resol / 2)}; + kt_double angle_to_cell = atan2(cell_center.GetY() - grid_scan_pose.GetY(), cell_center.GetX() - grid_scan_pose.GetX()); + kt_double distance_to_cell = sqrt(pow(grid_scan_pose.GetX() - cell_center.GetX(), 2) + pow(grid_scan_pose.GetY() - cell_center.GetY(), 2)); + if(distance_to_cell > m_max_sensor_range) + { + // Cell is not being seen + continue; + } + + std::optional laser_idx = findClosestLaserIndexToCell( + angle_to_cell, + scan_pose.GetHeading(), + laser_range_finder + ); + + if (!laser_idx.has_value()) + { + continue; + } + + kt_double beam_read_dist = range_scans[s]->GetRangeReadings()[*laser_idx]; + std::optional beam_distance = adjustBeamReadingDistance(beam_read_dist, distance_to_cell); + + if (!beam_distance.has_value()) + { + continue; + } + + karto::Vector2 beam_point { + grid_scan_pose.GetX() + (*beam_distance * cos(angle_to_cell)), + grid_scan_pose.GetY() + (*beam_distance * sin(angle_to_cell)) + }; + + utils::Segment2 beam_segment { + grid_scan_pose.GetPosition(), + beam_point + }; + + std::optional> distances = calculateBeamAndCellIntersections( + beam_segment, + cell + ); + + if (!distances.has_value()) + { + continue; + } + + calculateAndAppendCellProbabilities( + visited_cells, + *distances, + cell + ); + } +} +// +/*************************************************************************/ + + +/*************************************************************************/ +// +std::vector InformationEstimates::getScanGroupMutualInformation( + std::vector const & range_scans +) +{ + karto::LaserRangeFinder *laser_range_finder = range_scans[0]->GetLaserRangeFinder(); + kt_double angle_increment = laser_range_finder->GetAngularResolution(); + kt_double total_range = laser_range_finder->GetMaximumAngle() - laser_range_finder->GetMinimumAngle(); + + std::vector scans_mutual_information; + + for (int n = 0; n < range_scans.size(); ++n) + { + m_mutual_grid.setZero(); + m_visited_grid.setZero(); + + std::vector> visited_cells = getScanGroupVisitedCells(range_scans, laser_range_finder, n); + calculateScanGroupMutualInformation(visited_cells, scans_mutual_information); + } + return scans_mutual_information; +} +// +/*************************************************************************/ + + +/*************************************************************************/ +// +void InformationEstimates::calculateScanGroupMutualInformation( + std::vector> const & visited_cells, + std::vector & scans_mutual_information +) +{ + for (auto &cell : visited_cells) + { + std::unordered_map meas_out_prob = + computeMeasurementOutcomesHistogram(m_cell_probabilities.at(cell)); + kt_double cell_mutual_inf = 0.0f; + for (auto &pair : meas_out_prob) + { + cell_mutual_inf += pair.second * measurementOutcomeEntropy(pair.first); + } + + // Mutual information of cell x, y given a set of measurements + m_mutual_grid(cell.GetX(), cell.GetY()) = 1.0 - cell_mutual_inf; + } + scans_mutual_information.push_back(m_mutual_grid.sum()); + m_cell_probabilities.clear(); +} +// +/*************************************************************************/ + + +/*************************************************************************/ +// +std::vector> InformationEstimates::getScanGroupVisitedCells( + std::vector const & range_scans, + karto::LaserRangeFinder *laser_range_finder, + int const & scan_to_skip +) +{ + std::vector> visited_cells; + + int x_upper_limit = floor(m_map_dim.x / m_cell_resol); + int y_upper_limit = floor(m_map_dim.y / m_cell_resol); + + for (int i = 0; i < x_upper_limit; ++i) + { + for (int j = 0; j < y_upper_limit; ++j) + { + calculateCellProbabilities(range_scans, visited_cells, {i, j}, laser_range_finder, scan_to_skip); + } + } + return visited_cells; +} +// +/*************************************************************************/ + + +/*************************************************************************/ +std::vector InformationEstimates::findMutualInfo(std::vector const &range_scans) +{ + std::vector result_vector; + resizeGridFromScans(range_scans); + + std::vector mutual_information_vct = getScanGroupMutualInformation(range_scans); + return mutual_information_vct; +} + +void InformationEstimates::appendCellProbabilities(std::vector &measurements, karto::Vector2 const &cell) +{ + /** + * Append measured porbabilities for the given cell + * Arguments: + * measurements [std::vector]: Vector of LocalizedRangeScan pointers + * cell [karto::Vector2]: Cell for appending the data + * Return: + * Void + */ + std::map, std::vector>>::iterator it_cell; + it_cell = m_cell_probabilities.find(cell); + + if (it_cell == m_cell_probabilities.end()) + { + // Cell is not present in the map, so append it + m_cell_probabilities.insert(std::pair, std::vector>>( + cell, {measurements}) + ); + m_visited_grid(cell.GetX(), cell.GetY()) = 1; + } + else + { + if (m_visited_grid(cell.GetX(), cell.GetY()) == 1) // It must be the longitude + { + int idx = it_cell->second.size() - 1; + if (measurements[2] < it_cell->second[idx][2]) + { + // Replacing + it_cell->second[idx][0] = measurements[0]; + it_cell->second[idx][1] = measurements[1]; + it_cell->second[idx][2] = measurements[2]; + } + } + else + { + it_cell->second.push_back(measurements); + m_visited_grid(cell.GetX(), cell.GetY()) = 1; + } + } +} + +kt_double InformationEstimates::calculateInformationContent(kt_double prob) +{ + /** + * Calculate the information content or self-information based on the probability of cell being occupied + * Arguments: + * prob [kt_double]: Probability of being occupied + * Return: + * kt_double: Information content + */ + return -(prob * log2(prob)) - ((1 - prob) * log2(1 - prob)); +} + +kt_double InformationEstimates::calculateProbabilityFromLogOdds(kt_double log) +{ + /** + * Map Log-Odds into probability + * Arguments: + * log [kt_double]: Log-Odds + * Return: + * kt_double: Probability + */ + return (exp(log) / (1 + exp(log))); +} + +void InformationEstimates::insertMeasurementOutcome(map_tuple tuple, kt_double probability, std::unordered_map &map) +{ + int key_free, key_occ, key_unk; + std::tie(key_free, key_occ, key_unk) = tuple; + + if (map[std::make_tuple(key_free, key_occ, key_unk)]) + { + map[std::make_tuple(key_free, key_occ, key_unk)] += probability; + } + else + { + map[std::make_tuple(key_free, key_occ, key_unk)] = probability; + } +} + +std::unordered_map InformationEstimates::computeMeasurementOutcomesHistogram(std::vector> &meas_outcm) +{ + /** + * Implementation of algorithm 1 + * Compute all the possible combinations of a grid cell, given a set of measurement outcomes + * Arguments: + * meas_outcm [std::vector>]: Vector of measurement outcomes in the form {p_free, p_occ, p_unk} + * Return: + * std::unordered_map: Map of combination, it contains the combination and its probability + */ + + std::unordered_map probabilities_map; + std::unordered_map temp_map; + + // Clear the temporal map + temp_map.clear(); + probabilities_map.clear(); + + // Root + if (meas_outcm.size() == 0) + { + probabilities_map[std::make_tuple(0, 0, 0)] = 1.0f; + } + + // First measurement + probabilities_map[std::make_tuple(1, 0, 0)] = meas_outcm[0][0]; // Probability free + probabilities_map[std::make_tuple(0, 1, 0)] = meas_outcm[0][1]; // Probability occupied + probabilities_map[std::make_tuple(0, 0, 1)] = meas_outcm[0][2]; // Probability unknown + + for (int r = 1; r < meas_outcm.size(); ++r) + { + // Measurement outcome probability + kt_double free_prop = meas_outcm[r][0]; + kt_double occ_prop = meas_outcm[r][1]; + kt_double unk_prop = meas_outcm[r][2]; + + // Temporal map to only take the last level combination + temp_map = probabilities_map; + probabilities_map.clear(); + + for (auto &combination : temp_map) + { + int key_free, key_occ, key_unk; + std::tie(key_free, key_occ, key_unk) = combination.first; + + // Adding next level measurement outcomes + insertMeasurementOutcome(std::make_tuple(key_free + 1, key_occ, key_unk), combination.second * free_prop, probabilities_map); + insertMeasurementOutcome(std::make_tuple(key_free, key_occ + 1, key_unk), combination.second * occ_prop, probabilities_map); + insertMeasurementOutcome(std::make_tuple(key_free, key_occ, key_unk + 1), combination.second * unk_prop, probabilities_map); + } + temp_map.clear(); + } + return probabilities_map; +} + +kt_double InformationEstimates::measurementOutcomeEntropy(map_tuple const &meas_outcome) +{ + /** + * @brief Calculate the measurement outcome entropy in the following steps: + * Calculate the measurement outcome entropy + * Calculate Log-Odds from initial probability guess + * Calculate the probability from those logs + * Calculate the entropy with the retrieved probability + * @arg meas_outcome [map_tuple]: Measurement outcome combinations + * @return [kt_double]: Measurement outcome entropy + */ + int num_free, num_occ, num_unk; + std::tie(num_free, num_occ, num_unk) = meas_outcome; + kt_double log_occ = (num_free * l_free) + (num_occ * l_occ) - ((num_free + num_occ - 1) * l_o); + kt_double prob_occ = calculateProbabilityFromLogOdds(log_occ); + return calculateInformationContent(prob_occ); +} + +kt_double InformationEstimates::calculateScanMassProbabilityBetween(kt_double range_1, kt_double range_2) +{ + /** + * @brief Calculate the mass probability of a cell being observed by a given measurement. + * An exponential distribution is implemeted where the rate parameter is calculated + * based on the max laser range. + * @arg range_1 [kt_double]: Lower bound + * @arg range_2 [kt_double]: Upper bound + * @return [kt_double]: Mass probability between given ranges + */ + range_1 = (range_1 > m_max_sensor_range) ? m_max_sensor_range : range_1; + range_2 = (range_2 > m_max_sensor_range) ? m_max_sensor_range : range_2; + + kt_double lambda = lambda_factor / m_max_sensor_range; + + return -(exp(-lambda * range_2) - exp(-lambda * range_1)); +} diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 38caaffe..30413671 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -76,6 +76,7 @@ LifelongSlamToolbox::LifelongSlamToolbox(rclcpp::NodeOptions options) // in lifelong mode, we cannot have interactive mode enabled enable_interactive_mode_ = false; + InformationEstimates information; } /*****************************************************************************/ diff --git a/src/experimental/utils.cpp b/src/experimental/utils.cpp new file mode 100644 index 00000000..0c10b593 --- /dev/null +++ b/src/experimental/utils.cpp @@ -0,0 +1,211 @@ +#include "slam_toolbox/experimental/utils.hpp" +#include + +#include + +namespace utils +{ + namespace grid_operations + { + void updateCellLimits( + std::array, 4> & initial_points, + std::array, 4> & final_points, + karto::Vector2 const & cell_reference_pos, + std::array & cell_limits, + utils::Segment2 const & beam_discrete_segment, + kt_double const & resolution + ) + { + /** + * To calculate grid limits for intersection + * Arguments: + * initial_points [std::array, 4>]: Cell initial points in x and y (4 corners) + * final_points [std::array, 4>]: Cell final points in x and y (4 corners) + * cell_reference_pos [karto::Vector2]: Current cell position + * cell_limits [std::array]: Cell limits in x and y + * beam_discrete_segment [utils::Segment2]: Beam segment to identify its direction + * resolution [kt_double]: Cell resolution + * Return: + * Void + */ + if (beam_discrete_segment.end.GetX() < beam_discrete_segment.start.GetX() + && beam_discrete_segment.end.GetY() >= beam_discrete_segment.start.GetY() + ) + { + final_points[0].SetX(cell_reference_pos.GetX() + resolution); + final_points[2].SetX(cell_reference_pos.GetX() + resolution); + + cell_limits[2] = cell_reference_pos.GetY(); + cell_limits[3] = cell_reference_pos.GetY() + resolution; + } + + if (beam_discrete_segment.end.GetX() >= beam_discrete_segment.start.GetX() + && beam_discrete_segment.end.GetY() < beam_discrete_segment.start.GetY() + ) + { + initial_points[2].SetY(cell_reference_pos.GetY() - resolution); + initial_points[3].SetY(cell_reference_pos.GetY() - resolution); + + final_points[1].SetY(cell_reference_pos.GetY() - resolution); + final_points[3].SetY(cell_reference_pos.GetY() - resolution); + + cell_limits[2] = cell_reference_pos.GetY() - resolution; + cell_limits[3] = cell_reference_pos.GetY(); + } + } + + + int signum(int num) + { + /** + * Get the sign of an operation, used by Bresenham algorithm + * Arguments: + * num [int]: Number to perform the sign operation + * Return: + * int: Integer representing the sign + */ + if (num < 0) return -1; + if (num >= 1) return 1; + return 0; + } + + + karto::Vector2 discretize(karto::Vector2 const & position, kt_double const & resolution) + { + /** + * Mapping a continuous position into a grid position + * Arguments: + * position [karto::Vector2]: Position to be discretized + * resolution [kt_double]: Cell resolution + * Return: + * karto::Vector2: Grid position + */ + const int x_cell = floor((position.GetX() / resolution)); + const int y_cell = floor((position.GetY() / resolution)); + + return karto::Vector2{ x_cell, y_cell }; + } + + + karto::Vector2 calculateCellIntersectionPoints( + utils::Segment2 const & segment_1, + utils::Segment2 const & segment_2 + ) + { + /** + * Find the intersection point between two segments (cell and laser beam) + * Arguments: + * segment_1 [utils::Segment2]: First segment points + * segment_2 [utils::Segment2]: Second segment points + * Return: + * karto::Vector2: Intersection point + */ + const kt_double x1 = segment_1.start.GetX(); + const kt_double x2 = segment_1.end.GetX(); + const kt_double x3 = segment_2.start.GetX(); + const kt_double x4 = segment_2.end.GetX(); + + const kt_double y1 = segment_1.start.GetY(); + const kt_double y2 = segment_1.end.GetY(); + const kt_double y3 = segment_2.start.GetY(); + const kt_double y4 = segment_2.end.GetY(); + + const kt_double den = ((x2 - x1)*(y4 - y3) - (x4 - x3)*(y2 - y1)); + + if (den != 0.0f) + { + const kt_double x = ((x2*y1 - x1*y2)*(x4 - x3) - (x4*y3 - x3*y4)*(x2 - x1)) / den; + const kt_double y = ((x2*y1 - x1*y2)*(y4 - y3) - (x4*y3 - x3*y4)*(y2 - y1)) / den; + const karto::Vector2 intersection { x, y }; + return intersection; + } + return {}; + } + + + std::pair, std::vector> computeLineBoxIntersection( + utils::Segment2 const & segment, + karto::Vector2 const & cell_reference_pos, + kt_double const & resolution + ) + { + /** + * Compute intersection between a cell and a laser beam + * Arguments: + * segment [karto::Segment2]: Laser beam initial and final points + * cell_reference_pos [karto::Vector2]: Current cell position + * resolution [kt_double]: Cell resolution + * Return: + * std::pair, std::vector>: Intersection points for current beam and cell + */ + utils::Segment2 beam_discrete_segment { + discretize(segment.start, resolution), + discretize(segment.end, resolution) + }; + + std::array cell_limits { + cell_reference_pos.GetX(), + cell_reference_pos.GetX() + resolution, + cell_reference_pos.GetY(), + cell_reference_pos.GetY() + resolution + }; + + std::array, 4> initial_points {{ + {cell_reference_pos.GetX(), cell_reference_pos.GetY()}, + {cell_reference_pos.GetX(), cell_reference_pos.GetY()}, + {cell_reference_pos.GetX() + resolution, cell_reference_pos.GetY() + resolution}, + {cell_reference_pos.GetX() + resolution, cell_reference_pos.GetY() + resolution} + }}; + + std::array, 4> final_points {{ + {cell_reference_pos.GetX() + resolution, cell_reference_pos.GetY()}, + {cell_reference_pos.GetX(), cell_reference_pos.GetY() + resolution}, + {cell_reference_pos.GetX() + resolution, cell_reference_pos.GetY()}, + {cell_reference_pos.GetX(), cell_reference_pos.GetY() + resolution} + }}; + + // Set the new cell limits + updateCellLimits(initial_points, final_points, cell_reference_pos, cell_limits, beam_discrete_segment, resolution); + + /* + initial {cell_reference_pos.GetX(), cell_reference_pos.GetY()} + final {cell_reference_pos.GetX() + res, cell_reference_pos.GetY()} + + initial {cell_reference_pos.GetX(), cell_reference_pos.GetY()} + final {cell_reference_pos.GetX(), cell_reference_pos.GetY() + res} + + initial {cell_reference_pos.GetX() + res, cell_reference_pos.GetY() + res} + final {cell_reference_pos.GetX() + res, cell_reference_pos.GetY()} + + initial {cell_reference_pos.GetX() + res, cell_reference_pos.GetY() + res} + final {cell_reference_pos.GetX(), cell_reference_pos.GetY() + res} + */ + + std::vector inter_x, inter_y; + for (int k = 0; k < 4; ++k) + { + karto::Vector2 start { initial_points[k].GetX(), initial_points[k].GetY() }; + karto::Vector2 end { final_points[k].GetX(), final_points[k].GetY() }; + utils::Segment2 cell_segment { start, end }; + + karto::Vector2 intersection = calculateCellIntersectionPoints(segment, cell_segment); + if(intersection.Length() != 0) + { + if ((fabs(intersection.GetX()) >= (fabs(cell_limits[0]) - cell_limit_eps)) && + (fabs(intersection.GetX()) <= (fabs(cell_limits[1]) + cell_limit_eps)) && + (fabs(intersection.GetY()) >= (fabs(cell_limits[2]) - cell_limit_eps)) && + (fabs(intersection.GetY()) <= (fabs(cell_limits[3]) + cell_limit_eps)) + ) + { + // Two points where the beam cuts the cell + // - A laser beam can cut the cell at least 1 time (Enter) + // - A laser beam can cut the cell at most 2 times (Enter an exit) + inter_x.push_back(intersection.GetX()); + inter_y.push_back(intersection.GetY()); + } + } + } + return std::make_pair(inter_x, inter_y); + } + } // namespace grid_operations +} // namespace utils diff --git a/test/information_estimates_test.cpp b/test/information_estimates_test.cpp new file mode 100644 index 00000000..cc8979b2 --- /dev/null +++ b/test/information_estimates_test.cpp @@ -0,0 +1,206 @@ + +#ifndef SLAM_TOOLBOX_SLAM_TOOLBOX_INFORMATION_ESTIMATES_TEST_H_ +#define SLAM_TOOLBOX_SLAM_TOOLBOX_INFORMATION_ESTIMATES_TEST_H_ + +#include +#include "slam_toolbox/experimental/information_estimates.hpp" + +/* + Test comments + List of possible new tests + 1. resizeGridFromScans + 2. findClosestLaserIndexToCell + 3. adjustBeamReadingDistance + + Requirements for tests: + 1. Laser with complete information + 2. Laser with complete information and able to prodive a laser range finder + 3. Laser with complete information and able to prodive a laser range finder +*/ + + +TEST(UtilsInformationEstimatesTests, SigNumTest) +{ + InformationEstimates information_estimates; + + ASSERT_EQ(utils::grid_operations::signum(10), 1) << "FAIL in positive"; + ASSERT_EQ(utils::grid_operations::signum(-10), -1) << "FAIL in negative"; + ASSERT_EQ(utils::grid_operations::signum(0), 0) << "FAIL in zero"; +} + +// TEST(UtilsInformationEstimatesTests, InformationContentTest) +// { +// InformationEstimates inf_estimates; +// kt_double result = inf_estimates.calculateInformationContent(0.75); +// ASSERT_DOUBLE_EQ(result, 0.81125); +// } + +TEST(UtilsInformationEstimatesTests, ScanMassProbabilityBetweenTest) +{ + // kt_double range_1 = 4.25; + // kt_double range_2 = 6.12; + + // InformationEstimates inf_estimates; + // kt_double probability = inf_estimates.calculateScanMassProbabilityBetween(range_1, range_2); + + // ASSERT_DOUBLE_EQ(probability, 0.07522); +} + +TEST(UtilsInformationEstimatesTests, GridPositionsTest) +{ + // karto::Vector2 position{10.3, 52.7}; + // kt_double resolution = 0.5; + // karto::Vector2 grid_position = utils::grid_operations::getGridPosition(position, resolution); + // ASSERT_EQ(grid_position[0], 20); + // ASSERT_EQ(grid_position[1], 105); +} + +TEST(UtilsInformationEstimatesTests, LineBoxIntersectionTest) +{ + // kt_double resolution = 0.5; + + // karto::Vector2 scan_position{ 18.3, 16.1 }; + // karto::Vector2 beam_end_point{ 22.2, 12.7 }; + + // karto::Vector2 scan_position_cell = utils::grid_operations::getGridPosition(scan_position, resolution); + // karto::Vector2 beam_end_cell = utils::grid_operations::getGridPosition(beam_end_point, resolution); + + // karto::Vector2 const & cell { 11, 6 }; + + // // Result could be not intersection or intersection + // std::pair, std::vector> intersections = + // utils::grid_operations::computeLineBoxIntersection( + // scan_position, + // beam_end_point, + // scan_position_cell, + // beam_end_cell + // cell.GetX() * resolution, + // cell.GetY() * resolution, + // resolution + // ); + + // ASSERT_TRUE(intersections.first.size() != 0) + + // // Enter (d1) and Exit (d2) distances + // std::vector distances; + // for (int k = 0; k < intersections.first.size(); ++k) + // { + // // From robot position to intersection points + // karto::Vector2 intersection{ + // intersections.first[k], + // intersections.second[k] + // }; + // kt_double distance = scan_position.Distance(intersection); + // distances.push_back(distance); + // std::sort(distances.begin(), distances.end()); + // } + + // ASSERT_DOUBLE_EQ(distance[0], 21.28235); + // ASSERT_DOUBLE_EQ(distance[1], 13.5); +} + +TEST(UtilsInformationEstimatesTests, IntersectionPointsTest) +{ + // karto::Vector2 int_points; + + // // Intersection + // karto::Vector2 laser_start_i{1.5, 1.6}; + // karto::Vector2 laser_end_i{6.1, 8.4}; + // karto::Vector2 cell_start_i{3.6, 8.2}; + // karto::Vector2 cell_end_i{4.7, 1.6}; + // int_points = utils::grid_operations::calculateCellIntersectionPoints(laser_start_i, laser_end_i, cell_start_i, cell_end_i); + + // ASSERT_DOUBLE_EQ(int_points.GetX(), 4.06744) << "FAIL in X coordinate intersection"; + // ASSERT_DOUBLE_EQ(int_points.GetY(), 5.39535) << "FAIL in Y coordinate intersection"; + + // // Parallel lines + // karto::Vector2 laser_start_p{1.5, 1.5}; + // karto::Vector2 laser_end_p{6.1, 6.1}; + // karto::Vector2 cell_start_p{3.6, 3.6}; + // karto::Vector2 cell_end_p{8.7, 8.7}; + // int_points = utils::grid_operations::calculateCellIntersectionPoints(laser_start_p, laser_end_p, cell_start_p, cell_end_p); + + // // I should change here the longitud instead + // // .size() or.Lenght() + // ASSERT_DOUBLE_EQ(int_points.GetX(), 0.0) << "FAIL in X coordinate parallel"; + // ASSERT_DOUBLE_EQ(int_points.GetY(), 0.0) << "FAIL in Y coordinate parallel"; +} + +TEST(InformationEstimatesTests, MutualInformationTest) +{ + InformationEstimates inf_estimates; + + std::unique_ptr s1 = std::make_unique(); + std::unique_ptr s2 = std::make_unique(); + std::unique_ptr s3 = std::make_unique(); + + karto::Pose2 p1 = karto::Pose2(3.5, 4.0, 0.0); + karto::Pose2 p2 = karto::Pose2(3.5, 5.5, 0.0); + karto::Pose2 p3 = karto::Pose2(5.2, 7.3, 0.0); + + s1->SetCorrectedPose(p1); + s2->SetCorrectedPose(p2); + s3->SetCorrectedPose(p3); + karto::BoundingBox2 bb1, bb2, bb3; + bb1.SetMinimum(karto::Vector2(2.0, 2.0)); + bb1.SetMaximum(karto::Vector2(5.0, 6.0)); + bb2.SetMinimum(karto::Vector2(2.0, 4.0)); + bb2.SetMaximum(karto::Vector2(5.0, 7.0)); + bb3.SetMinimum(karto::Vector2(2.0, 5.0)); + bb3.SetMaximum(karto::Vector2(5.0, 9.0)); + s1->SetBoundingBox(bb1); + s2->SetBoundingBox(bb2); + s3->SetBoundingBox(bb3); + karto::PointVectorDouble pts1, pts2, pts3; + pts1.push_back(karto::Vector2(3.0, 5.0)); + pts1.push_back(karto::Vector2(3.0, 3.0)); + pts1.push_back(karto::Vector2(1.5, 7.0)); + pts2.push_back(karto::Vector2(4.0, 5.0)); + pts2.push_back(karto::Vector2(4.0, 2.9)); + pts3.push_back(karto::Vector2(3.0, 5.0)); + pts3.push_back(karto::Vector2(5.0, 6.0)); + pts3.push_back(karto::Vector2(3.5, 7.2)); + s1->SetPointReadings(pts1, true); + s2->SetPointReadings(pts2, true); + s3->SetPointReadings(pts3, true); + bool dirty = false; + s1->SetIsDirty(dirty); + s2->SetIsDirty(dirty); + s3->SetIsDirty(dirty); + + std::vector range_scan_vct; + range_scan_vct.push_back(s1.get()); + range_scan_vct.push_back(s2.get()); + range_scan_vct.push_back(s3.get()); + + // std::vector mut_inf_vct = inf_estimates.findMutualInfo(range_scan_vct); + + // kt_double prev_mutual_info = 0.0; + + // int idx = 0; + // int final_idx = 0; + + // for (auto & mutual_info : mut_inf_vct) + // { + // if (mutual_info > prev_mutual_info) + // { + // prev_mutual_info = mutual_info; + // final_idx = idx; + // } + // ++idx; + // } + // // Max value will be the one having the largest mutual information value + // // Since we are extracting one element at a time: + // // So the element with the less mutual information will have a group with the largest result + // // In this case index 1 will have the less mutual information + // EXPECT_EQ(final_idx, 1) << "FAIL in laser index"; + // EXPECT_NE(prev_mutual_info, 0.0) << "FAIL in mutual information equality"; +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +#endif // SLAM_TOOLBOX_SLAM_TOOLBOX_INFORMATION_ESTIMATES_TEST_H_