forked from SteveMacenski/slam_toolbox
-
Notifications
You must be signed in to change notification settings - Fork 1
Feature/lifelong compression #2
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Open
kmilo7204
wants to merge
83
commits into
foxy-devel
Choose a base branch
from
feature/lifelong_compression
base: foxy-devel
Could not load branches
Branch not found: {{ refName }}
Loading
Could not load tags
Nothing to show
Loading
Are you sure you want to change the base?
Some commits from the old base branch may be removed from the timeline,
and old review comments may become outdated.
Open
Changes from all commits
Commits
Show all changes
83 commits
Select commit
Hold shift + click to select a range
e1cc2ac
[DOC]: Initial commit and loop for grid calculation
kmilo7204 f986ba7
[WIP]: Initial test for gris position mapping
kmilo7204 47b6098
[WIP]: Adding new functions for ray tracing, and probability calculation
kmilo7204 95de0b4
[WIP]: Adding tools for measuring distances to cells
kmilo7204 7aa82b9
[WIP]: Updating function to calculate intersection of beams and cells
kmilo7204 80bc5eb
[WIP]: Cleaning comments
kmilo7204 d07ddeb
[FIX]: Adding last cell to the calculation so we get the whole distance
kmilo7204 579c562
[WIP]: Distance to each cell is being calculated. This is not working…
kmilo7204 2fa788a
[ADD]: Probabilities calculation
kmilo7204 d91e4c7
[FIX]: RBT transformation for getting the final point
kmilo7204 c3f4155
[FEAT]: Finally getting the cutting points to each individual cell
kmilo7204 33bc43c
[FIX]: Probability function update
kmilo7204 a012859
[WIP]: Probability calculation is reliable
kmilo7204 a2c83e0
[WIP]: Adding inverseModel function to detect cell occupancy, adding …
kmilo7204 d6f6db1
[WIP]: Logs calculation
kmilo7204 f294d0d
[WIP]: Getting probability from log-odds
kmilo7204 c08fd69
[WIP]: Adding map entropy calculation
kmilo7204 e66435c
[ADD]: Adding algorithm 1 base, it is in adaptation process
kmilo7204 eb2c754
[ADD]: Algorithm 1 MVP (Currently working), but testing is required
kmilo7204 6cab676
[WIP]: Algorithm 1 completed. Adding now new poses and scans
kmilo7204 285d07e
[WIP]: Adding conditions for some cells. New intersections are handle…
kmilo7204 ec51e0b
[FIX]: Handling cells seen by Bresenham but without intersections
kmilo7204 116abf7
[WIP]: Final adjustments
kmilo7204 5f88cd6
[ADD]: Adding measurement outcome entropy (3.12) calculation
kmilo7204 b507842
[ADD]: Map mutual information calculation and code structure, it is w…
kmilo7204 384a0b5
[WIP]: Adding new robot pose for mutual information verification
kmilo7204 c825cc6
[WIP]> Notations and calculations improvements
kmilo7204 ea9ef27
[ADD]: New theoretical information object
kmilo7204 488868b
[WIP]: Completing theoretical information object
kmilo7204 64b8756
[WIP]: Completing theoretical information object, removing old code a…
kmilo7204 86910b8
[RMV]: Removing unused dependencies
kmilo7204 36f2fa9
[FIX]: Floating point absolute value changed in cell limits
kmilo7204 7980579
[WIP]: Changes in measurement outcomes computations and code structure
kmilo7204 89664a2
[WIP]: Functions reordering to improve readability
kmilo7204 dc970e0
[WIP]: Adding new functions for readability
kmilo7204 e2a13ef
[WIP]: Changes in algorithm 1
kmilo7204 43ce3bf
[WIP]: Function return values modifications to match Karto
kmilo7204 71f5930
[WIP]: Integration of new elements
kmilo7204 ed0f586
[WIP]: Replacing data types from double to kt_double
kmilo7204 542b239
[WIP]: Functions modification. At this point we now have an utils fil…
kmilo7204 340f6d5
[ADD]: Adding utils namespace
kmilo7204 e5ac4e5
[INT]: Utils integration
kmilo7204 6869573
[WIP]: Changing main function arguments
kmilo7204 b2cac9f
[WIP]: More modification to make the code clear
kmilo7204 8ba213b
[ADD]: Eigen addition and matrix replacement
kmilo7204 88d3561
[DOC]: Adding code documentation
kmilo7204 93a1809
[ADD]: Adding basic test logic
kmilo7204 899607f
[ADD]: Adding tests for Information Estimates and Utils
kmilo7204 1b432f3
[WIP]: Adding more test cases
kmilo7204 b5752c7
[WIP]: Tests updating
kmilo7204 8148fed
[WIP]: Renaming functions, updating tests and replacing some data str…
kmilo7204 296d191
[WIP]: Updating mutual information calculation
kmilo7204 c2e7c5c
[WIP]: Removing unused code
kmilo7204 8752e3e
[ADD]: Function for mutual information calculation
kmilo7204 969a3a7
[WIP]: Dynamic grid resizing
kmilo7204 a39954d
[RMV]: Unused function and adding transformations
kmilo7204 b799cb8
[WIP]: Returning values modification
kmilo7204 2825ef9
[WIP]: Replacing vectors for karto wrappers
kmilo7204 83203d9
[WIP]: Comparing probabilities from the same scans
kmilo7204 c87c408
[WIP]: Integration to lifelong mapping loop
kmilo7204 b3baea5
[ADD]: New logic for Algrotihm 1 implementation
kmilo7204 e40b217
[RMV]: Removing basic functions
kmilo7204 f0654c0
[ADD]: Adding base logic for new loop calculation
kmilo7204 d40c125
[WIP]: First complete pass on the new function
kmilo7204 8c206a8
[ADD]: Adding cell angle functionality. Still under testing
kmilo7204 75d20f6
[WIP]: Calculations up to entropy are now correct
kmilo7204 e2a99fc
Fully functional loop
kmilo7204 91bd220
Complete main function update
kmilo7204 bf2ede3
Checkpoint for code refactoring
kmilo7204 60930e3
Add new function for separaitng the jobs
kmilo7204 c7d6116
Code refactoring phase 1
kmilo7204 156be31
Remove undesired folders
kmilo7204 002345b
Updating test for mutual information
kmilo7204 656206c
[ADD]: Test cases
kmilo7204 97b45af
Improving redability, addition of segment and box structures, adding …
kmilo7204 85674bf
[RMV]: Revert test changes in the main file
kmilo7204 be2197a
[UPDT]: Update C++ STD version to use optional function
kmilo7204 be745b7
[WIP]: Structures modification to simplify readability
kmilo7204 89a8a18
[RMV]: Reverting changes in main file
kmilo7204 5ef8011
[WIP]: Remove not necessary PI addition to cell angle
kmilo7204 582eda9
[ADD]: Addition of new map dimensions structures to improve readability
kmilo7204 54ecdc1
[WIP]: Typedef for probaility map
kmilo7204 82a2141
[WIP]: Updating exponential distribution rate and deleting unused at…
kmilo7204 File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
126 changes: 126 additions & 0 deletions
126
include/slam_toolbox/experimental/information_estimates.hpp
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 |
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Uh oh!
There was an error while loading. Please reload this page.