From e1cc2ac13c41229241a711e37c1c7de00313fe67 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Fri, 22 Oct 2021 06:13:45 -0500 Subject: [PATCH 01/83] [DOC]: Initial commit and loop for grid calculation --- src/experimental/slam_toolbox_lifelong.cpp | 25 ++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 38caaffe6..ca26d43b8 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -78,6 +78,31 @@ LifelongSlamToolbox::LifelongSlamToolbox(rclcpp::NodeOptions options) enable_interactive_mode_ = false; } +/*****************************************************************************/ +/** + * The are implementation notes from Camilo OCT 21 + * + * First step + * Get the occupancy probability for each cell in the map. This solution is based + * on the Log-Odds rather than the Bayesian filter, only for computation performance + * Initialization for this elements should be made before we start the following loop: + * Gather the sensor data. + * Inverse measurement model (Tranforming scan to measure occupancy belief map) + * Log-Odds calculation for the current reading and update the grid or map. + * Probability map calculation and update for current reading (This is done for the cells)\ + * + * NOTE + * Once we get the probabilities we can calculate the entropy of each grid cell. I am assuming + * this should be done inside the loop as we want to calculate this entropy at each time. + + * Log odds assume values from −∞ to ∞. The Bayes filter for updating beliefs in + * log odds representation is computationally elegant. It avoids truncation problems that + * arise for probabilities close to 0 or 1. + * + * + * We are using std::vector !! +/*****************************************************************************/ + /*****************************************************************************/ void LifelongSlamToolbox::laserCallback( sensor_msgs::msg::LaserScan::ConstSharedPtr scan) From f986ba75f1432726091c3ebce7361cda6d08950b Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Mon, 25 Oct 2021 12:15:10 -0500 Subject: [PATCH 02/83] [WIP]: Initial test for gris position mapping --- .../experimental/slam_toolbox_lifelong.hpp | 8 ++ src/experimental/slam_toolbox_lifelong.cpp | 85 +++++++++++++++++++ src/slam_toolbox_common.cpp | 2 + 3 files changed, 95 insertions(+) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index d77bdda90..5ef20cd77 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -66,6 +66,14 @@ class LifelongSlamToolbox : public SlamToolbox void updateScoresSlamGraph(const double & score, Vertex * vertex); void checkIsNotNormalized(const double & value); + /*****************************************************************************/ + /*******************************Implementation********************************/ + void scannerTest(); + std::vector getGridPosition(float x, float y, float resolution); + std::vector getLaserHit(std::vector const& robot_pose, float distance, float angle); + /*****************************************************************************/ + + bool use_tree_; double iou_thresh_; double removal_score_; diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index ca26d43b8..6e3d0bceb 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -18,8 +18,12 @@ #include #include +#include +#include #include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp" +#define PI 3.14159265 + namespace slam_toolbox { @@ -76,6 +80,8 @@ LifelongSlamToolbox::LifelongSlamToolbox(rclcpp::NodeOptions options) // in lifelong mode, we cannot have interactive mode enabled enable_interactive_mode_ = false; + + scannerTest(); } /*****************************************************************************/ @@ -101,8 +107,87 @@ LifelongSlamToolbox::LifelongSlamToolbox(rclcpp::NodeOptions options) * * * We are using std::vector !! + * + * + * + * According tothe the last meeting we handle, I will start with a prrof of concept + * Create a grid + * Calculate the scan position (WRO a given cell). Consider where is my initial cell + * Calculate the probability of seing an grid given a set of measurements + * Create the histogram with Algrotihm 1 +/*****************************************************************************/ + +/*****************************************************************************/ +/*******************************Implementation********************************/ + +void LifelongSlamToolbox::scannerTest() +{ + RCLCPP_WARN(get_logger(), "<-------- Scanner Test -------->"); + + float resolution = 0.5f; // Cell resolution - 1 meter + float map_dist = 20.0f; // Total map distance + + int number_cells = map_dist / resolution; + std::vector> grid(number_cells); // 20 meters in Y dimension + for (int i=0; i robot_pose{11.0f, 7.0f, 0.0f}; + std::vector robot_pose{11.0f, 7.0f, -PI/2}; + + // std::vector grid_pos = getGridPosition(robot_pose[0], robot_pose[1], resolution); + + // Maximum sensor range is 5 meters + // std::vector ranges{5.0f, 2.75f, 5.0f, 5.0f, 3.4f}; + std::vector ranges{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}; + // std::vector angles{-50.0f, -25.0f, 0.0f, 25.0f, 50.0f}; + std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + + // Angles will be -50, -25, 0, 25, 50 + // +-50 = +-0.87266 : +-25 = +-0.43633 + + // Current yaw + beam angle + // -PI/2 (-1.570795) - 0.87266 = 2.44345 (-50 degrees) + for (int i=0; i laser_grid = getLaserHit(robot_pose, ranges[i], angles[i]); + std::vector grid_pos = getGridPosition(laser_grid[0], laser_grid[1], resolution); + } +} + +std::vector LifelongSlamToolbox::getGridPosition(float x, float y, float resolution) +{ + int x_cell = ceil((1 / resolution) * x); + int y_cell = ceil((1 / resolution) * y); + + return {x_cell, y_cell}; +} + +std::vector LifelongSlamToolbox::getLaserHit(std::vector const& robot_pose, float distance, float angle) +{ + // This one is working OK + float angle_r = atan2(sin(robot_pose[2] + angle), cos(robot_pose[2] + angle)); + float x_occ = distance * cos(angle_r) + robot_pose[0]; // This is X + float y_occ = -distance * sin(angle_r) + robot_pose[1]; // This is Y + + std::vector grid_pos = getGridPosition(x_occ, y_occ, 0.5f); + + std::cout << grid_pos[0] << ", " << grid_pos[1] << std::endl; + std::cout << x_occ << ", " << y_occ << std::endl; + std::cout << " ---------------------- " << std::endl; + + return {x_occ, y_occ}; +} /*****************************************************************************/ + /*****************************************************************************/ void LifelongSlamToolbox::laserCallback( sensor_msgs::msg::LaserScan::ConstSharedPtr scan) diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 2263e99ec..35056721e 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -340,9 +340,11 @@ LaserRangeFinder * SlamToolbox::getLaser( sensor_msgs::msg::LaserScan::ConstSharedPtr & scan) /*****************************************************************************/ { + // RCLCPP_ERROR(get_logger(), "Here!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); const std::string & frame = scan->header.frame_id; if (lasers_.find(frame) == lasers_.end()) { try { + // RCLCPP_WARN(get_logger(), "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! Adding !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); lasers_[frame] = laser_assistant_->toLaserMetadata(*scan); dataset_->Add(lasers_[frame].getLaser(), true); } catch (tf2::TransformException & e) { From 47b60980b87aa2de6b85f548127c25484d49cdd9 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Tue, 26 Oct 2021 10:48:03 -0500 Subject: [PATCH 03/83] [WIP]: Adding new functions for ray tracing, and probability calculation --- .../experimental/slam_toolbox_lifelong.hpp | 7 +- src/experimental/slam_toolbox_lifelong.cpp | 133 ++++++++++++++++-- 2 files changed, 128 insertions(+), 12 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index 5ef20cd77..cf2ec2713 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -70,7 +70,12 @@ class LifelongSlamToolbox : public SlamToolbox /*******************************Implementation********************************/ void scannerTest(); std::vector getGridPosition(float x, float y, float resolution); - std::vector getLaserHit(std::vector const& robot_pose, float distance, float angle); + std::vector getLaserHit(std::vector const& robot_pose, float distance, float angle); + std::pair, std::vector> Bresenham(int x1, int y1, int x2, int y2); + int getSign(int x1, int x2); + float calculateProbability(float range); + + /*****************************************************************************/ diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 6e3d0bceb..b1a8cd436 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp" #define PI 3.14159265 @@ -139,31 +140,57 @@ void LifelongSlamToolbox::scannerTest() } // I will have 5 lasers for each reading (Located at 0, +-45, +-90) - // std::vector robot_pose{11.0f, 7.0f, 0.0f}; std::vector robot_pose{11.0f, 7.0f, -PI/2}; - // std::vector grid_pos = getGridPosition(robot_pose[0], robot_pose[1], resolution); - + // This is the first one + std::vector robot_grid_pos = getGridPosition(robot_pose[0], robot_pose[1], resolution); // Maximum sensor range is 5 meters - // std::vector ranges{5.0f, 2.75f, 5.0f, 5.0f, 3.4f}; std::vector ranges{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}; - // std::vector angles{-50.0f, -25.0f, 0.0f, 25.0f, 50.0f}; std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; - // Angles will be -50, -25, 0, 25, 50 - // +-50 = +-0.87266 : +-25 = +-0.43633 + // Angles will be -50, -25, 0, 25, 50 //----// +-50 = +-0.87266 : +-25 = +-0.43633 - // Current yaw + beam angle - // -PI/2 (-1.570795) - 0.87266 = 2.44345 (-50 degrees) + // Current yaw + beam angle: -PI/2 (-1.570795) - 0.87266 = 2.44345 (-50 degrees) for (int i=0; i laser_grid = getLaserHit(robot_pose, ranges[i], angles[i]); - std::vector grid_pos = getGridPosition(laser_grid[0], laser_grid[1], resolution); + std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1], resolution); + // robot_grid_pos[0] // X1 // robot_grid_pos[1] // Y1 + // final_grid_pos[0] // X2 // final_grid_pos[1] // Y2 + + std::vector res_x; + std::vector res_y; + std::pair, std::vector> res_pair = Bresenham(robot_grid_pos[0], robot_grid_pos[1], final_grid_pos[0], final_grid_pos[1]); + + res_x = res_pair.first; + res_y = res_pair.second; + } +} + +float LifelongSlamToolbox::calculateProbability(float range) +{ + /* + Calculates the probability of a cell being observed by a given measurement + */ + float max_range = 5.0f; + float lambda = 0.285f; + float nu = 1.0f / lambda; + if (range <= max_range) + { + return nu*lambda*exp(-lambda*range); } + return 0.0f; + + /* + std::cout << calculateProbability(5.0f) << std::endl; + std::cout << calculateProbability(2.5f) << std::endl; + std::cout << calculateProbability(0.0f) << std::endl; + */ } std::vector LifelongSlamToolbox::getGridPosition(float x, float y, float resolution) { + // Map the distance into the grid int x_cell = ceil((1 / resolution) * x); int y_cell = ceil((1 / resolution) * y); @@ -172,7 +199,7 @@ std::vector LifelongSlamToolbox::getGridPosition(float x, float y, float re std::vector LifelongSlamToolbox::getLaserHit(std::vector const& robot_pose, float distance, float angle) { - // This one is working OK + // Returns the distance where the laser beam hits something float angle_r = atan2(sin(robot_pose[2] + angle), cos(robot_pose[2] + angle)); float x_occ = distance * cos(angle_r) + robot_pose[0]; // This is X float y_occ = -distance * sin(angle_r) + robot_pose[1]; // This is Y @@ -185,6 +212,90 @@ std::vector LifelongSlamToolbox::getLaserHit(std::vector const& ro return {x_occ, y_occ}; } + + +std::pair, std::vector> LifelongSlamToolbox::Bresenham(int x1, int y1, int x2, int y2) +{ + // This one is working + std::vector x_bres; + std::vector y_bres; + + int x = x1; + int y = y1; + + int delta_x = abs(x2 - x1); + int delta_y = abs(y2 - y1); + + int s_x = getSign(x1, x2); + int s_y = getSign(y1, y2); + bool interchange = false; + + if (delta_y > delta_x) + { + int temp = delta_x; + delta_x = delta_y; + delta_y = temp; + interchange = true; + } + else + { + interchange = false; + } + + int a_res = 2 * delta_y; + int b_res = 2 * (delta_y - delta_x); + int e_res = (2 * delta_y) - delta_x; + + x_bres.push_back(x); + y_bres.push_back(y); + + for (int i = 1; i < delta_x; ++i) + { + if (e_res < 0) + { + if (interchange) + { + y += s_y; + } + else + { + x += s_x; + } + e_res += a_res; + } + else + { + y += s_y; + x += s_x; + e_res += b_res; + } + x_bres.push_back(x); + y_bres.push_back(y); + } + return std::pair, std::vector>{x_bres, y_bres}; + + /* + std::vector res_x; + std::vector res_y; + std::pair, std::vector> res_pair = Bresenham(-1, -4, 3, 2); + + res_x = res_pair.first; + res_y = res_pair.second; + for (int i=0; i Date: Thu, 28 Oct 2021 11:07:20 -0500 Subject: [PATCH 04/83] [WIP]: Adding tools for measuring distances to cells --- .../experimental/slam_toolbox_lifelong.hpp | 6 +- src/experimental/slam_toolbox_lifelong.cpp | 122 +++++++++++++++--- 2 files changed, 108 insertions(+), 20 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index cf2ec2713..17f9a0a02 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -71,9 +71,11 @@ class LifelongSlamToolbox : public SlamToolbox void scannerTest(); std::vector getGridPosition(float x, float y, float resolution); std::vector getLaserHit(std::vector const& robot_pose, float distance, float angle); - std::pair, std::vector> Bresenham(int x1, int y1, int x2, int y2); - int getSign(int x1, int x2); + std::pair, std::vector> Bresenham(int x_1, int y_1, int x_2, int y_2); + int getSign(int n_1, int n_2); float calculateProbability(float range); + std::vector calculateBeamCoordinates(float x_1, float y_1, float slope, float time); + float calculateSlope(float x_1, float y_1, float x_2, float y_2); /*****************************************************************************/ diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index b1a8cd436..9a1922ddc 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -140,10 +140,11 @@ void LifelongSlamToolbox::scannerTest() } // I will have 5 lasers for each reading (Located at 0, +-45, +-90) - std::vector robot_pose{11.0f, 7.0f, -PI/2}; + std::vector robot_pose{10.0f, 7.0f, -PI/2}; // This is the first one std::vector robot_grid_pos = getGridPosition(robot_pose[0], robot_pose[1], resolution); + // Maximum sensor range is 5 meters std::vector ranges{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}; std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; @@ -153,20 +154,104 @@ void LifelongSlamToolbox::scannerTest() // Current yaw + beam angle: -PI/2 (-1.570795) - 0.87266 = 2.44345 (-50 degrees) for (int i=0; i laser_grid = getLaserHit(robot_pose, ranges[i], angles[i]); std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1], resolution); // robot_grid_pos[0] // X1 // robot_grid_pos[1] // Y1 // final_grid_pos[0] // X2 // final_grid_pos[1] // Y2 - - std::vector res_x; - std::vector res_y; + std::vector cells_x, cells_y; std::pair, std::vector> res_pair = Bresenham(robot_grid_pos[0], robot_grid_pos[1], final_grid_pos[0], final_grid_pos[1]); - res_x = res_pair.first; - res_y = res_pair.second; + // Cells visited by this laser beam + cells_x = res_pair.first; + cells_y = res_pair.second; + + // Laser beam discretization + int samples = 100; + float step = ranges[i] / samples; + float current_dis = 0.0f; + + // Discretization of the laser beam + std::vector x_coord, y_coord; + x_coord.reserve(samples + 1); + y_coord.reserve(samples + 1); + + while(current_dis < (ranges[i] + step)) + { + // This loop containts the coordinates for each laser beam + float x_p = current_dis * cos(angles[i]); + float y_p = current_dis * sin(angles[i]); + x_coord.emplace_back(x_p); + y_coord.emplace_back(y_p); + current_dis += step; + } + // I would need to validate if this works for all the quadrants + // std::cout << limit_x << ", " << limit_y << std::endl; + // std::cout << limit_x + resolution << ", " << limit_y << std::endl; + // std::cout << limit_x << ", " << limit_y + resolution<< std::endl; + // std::cout << limit_x + resolution << ", " << limit_y + resolution << std::endl; + + // Move alongside all the cells that the laser beam hits + for (int i = 0; i < cells_x.size(); ++i) + { + std::cout << cells_x[i] << ", " << cells_y[i] << std::endl; + float limit_x = cells_x[i] * resolution; + float limit_y = cells_y[i] * resolution; + + for (int j = 0; j < x_coord.size(); ++j) + { + // Adding the robot position to the laser beam reading + float read_x = x_coord[i] + robot_pose[0]; + float read_y = y_coord[i] + robot_pose[1]; + + // Distances of the laser hit + if ((abs(read_x) > abs(limit_x)) && + (abs(read_x) < abs(limit_x + resolution)) && + (abs(read_y) > abs(limit_y)) && + (abs(read_y) < abs(limit_y + resolution))) + { + ; + } + } + } + + // + + // To discretize a beam + + // Discretize the line + // Punto inicial es la posicion del robot + // Punto final es la lectura del laser + + + // I could get the distance for each cell + + // As an initial approach I could use the euclidean distance to a given cell + + // I need to calculate + /* + d1 -> Distance from the robot position + */ + } } +float LifelongSlamToolbox::calculateSlope(float x_1, float y_1, float x_2, float y_2) +{ + // Calculates the scope of a given laser beam based on its coordinates + return (y_2 - y_1) / (x_2 - x_1); +} + +std::vector LifelongSlamToolbox::calculateBeamCoordinates(float x_1, float y_1, float slope, float time) +{ + // Coeficients + // Time step + std::cout << slope << std::endl; + + +} + float LifelongSlamToolbox::calculateProbability(float range) { /* @@ -206,28 +291,28 @@ std::vector LifelongSlamToolbox::getLaserHit(std::vector const& ro std::vector grid_pos = getGridPosition(x_occ, y_occ, 0.5f); - std::cout << grid_pos[0] << ", " << grid_pos[1] << std::endl; - std::cout << x_occ << ", " << y_occ << std::endl; - std::cout << " ---------------------- " << std::endl; + // std::cout << grid_pos[0] << ", " << grid_pos[1] << std::endl; + // std::cout << x_occ << ", " << y_occ << std::endl; + // std::cout << " ---------------------- " << std::endl; return {x_occ, y_occ}; } -std::pair, std::vector> LifelongSlamToolbox::Bresenham(int x1, int y1, int x2, int y2) +std::pair, std::vector> LifelongSlamToolbox::Bresenham(int x_1, int y_1, int x_2, int y_2) { // This one is working std::vector x_bres; std::vector y_bres; - int x = x1; - int y = y1; + int x = x_1; + int y = y_1; - int delta_x = abs(x2 - x1); - int delta_y = abs(y2 - y1); + int delta_x = abs(x_2 - x_1); + int delta_y = abs(y_2 - y_1); - int s_x = getSign(x1, x2); - int s_y = getSign(y1, y2); + int s_x = getSign(x_1, x_2); + int s_y = getSign(y_1, y_2); bool interchange = false; if (delta_y > delta_x) @@ -288,9 +373,10 @@ std::pair, std::vector> LifelongSlamToolbox::Bresenham(int */ } -int LifelongSlamToolbox::getSign(int n1, int n2) +int LifelongSlamToolbox::getSign(int n_1, int n_2) { - int difference = n2 - n1; + // Returns the sign of an operation, used for Bresenham algorithm + int difference = n_2 - n_1; if (difference == 0) { return 0; } else if (difference < 0) { return -1; } From 7aa82b9ef2b0b21d208720f6a81d67c78456d3b3 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Fri, 29 Oct 2021 11:49:38 -0500 Subject: [PATCH 05/83] [WIP]: Updating function to calculate intersection of beams and cells --- .../experimental/slam_toolbox_lifelong.hpp | 2 + src/experimental/slam_toolbox_lifelong.cpp | 112 ++++++++++++++---- 2 files changed, 90 insertions(+), 24 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index 17f9a0a02..7b6ad6c86 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -76,6 +76,8 @@ class LifelongSlamToolbox : public SlamToolbox float calculateProbability(float range); std::vector calculateBeamCoordinates(float x_1, float y_1, float slope, float time); float calculateSlope(float x_1, float y_1, float x_2, float y_2); + float calculateDistance(float x_1, float y_1, float x_2, float y_2); + /*****************************************************************************/ diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 9a1922ddc..861cbc637 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -139,29 +139,38 @@ void LifelongSlamToolbox::scannerTest() } } - // I will have 5 lasers for each reading (Located at 0, +-45, +-90) - std::vector robot_pose{10.0f, 7.0f, -PI/2}; + // I will have 5 lasers for each reading (Located at 0, +-25, +-50) + std::vector robot_pose{5.5f, 6.0f, -PI/2}; - // This is the first one + // This is the initial point std::vector robot_grid_pos = getGridPosition(robot_pose[0], robot_pose[1], resolution); + std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; - // Maximum sensor range is 5 meters - std::vector ranges{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}; - std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + // Creating the laser scan with 5 beams ------- Angles will be -55, -25, 0, 25, 55 //----// +-55 = +-0.87266 : +-25 = +-0.43633 + // std::vector ranges{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters + // std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; - // Angles will be -50, -25, 0, 25, 50 //----// +-50 = +-0.87266 : +-25 = +-0.43633 + std::vector ranges{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters + std::vector angles{0.0f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; - // Current yaw + beam angle: -PI/2 (-1.570795) - 0.87266 = 2.44345 (-50 degrees) - for (int i=0; i laser_grid = getLaserHit(robot_pose, ranges[i], angles[i]); std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1], resolution); + std::cout << "Laser end: " << final_grid_pos[0] << ", " << final_grid_pos[1] << std::endl; // robot_grid_pos[0] // X1 // robot_grid_pos[1] // Y1 // final_grid_pos[0] // X2 // final_grid_pos[1] // Y2 std::vector cells_x, cells_y; std::pair, std::vector> res_pair = Bresenham(robot_grid_pos[0], robot_grid_pos[1], final_grid_pos[0], final_grid_pos[1]); + /** + * Need to append here the last posint, the algorithm is not doing it + */ // Cells visited by this laser beam cells_x = res_pair.first; @@ -172,49 +181,96 @@ void LifelongSlamToolbox::scannerTest() float step = ranges[i] / samples; float current_dis = 0.0f; - // Discretization of the laser beam + // Vectors for discretization of the laser beam std::vector x_coord, y_coord; x_coord.reserve(samples + 1); y_coord.reserve(samples + 1); + // for (int j = 0; j < cells_x.size(); ++j) + // { + // std::cout << cells_x[j] << ", " << cells_y[j] << std::endl; + // } + while(current_dis < (ranges[i] + step)) { + // Need to shift the angles as the fron is my X + // This will match the global frame // This loop containts the coordinates for each laser beam - float x_p = current_dis * cos(angles[i]); - float y_p = current_dis * sin(angles[i]); + float x_p = current_dis * sin(angles[i]); + float y_p = current_dis * cos(angles[i]); x_coord.emplace_back(x_p); y_coord.emplace_back(y_p); current_dis += step; } + std::cout << " uuuuuuuuuuuuuuuu " << std::endl; + std::cout << x_coord[10] << ", " << y_coord[10] << ", " << x_coord.size() << std::endl; // I would need to validate if this works for all the quadrants // std::cout << limit_x << ", " << limit_y << std::endl; // std::cout << limit_x + resolution << ", " << limit_y << std::endl; // std::cout << limit_x << ", " << limit_y + resolution<< std::endl; // std::cout << limit_x + resolution << ", " << limit_y + resolution << std::endl; + int count_idx = 0; + float dist_carry = 0.0f; + + std::vector initial_point(2), final_point(2); + + initial_point[0] = 0.0f; + initial_point[1] = 0.0f; + // Move alongside all the cells that the laser beam hits for (int i = 0; i < cells_x.size(); ++i) { - std::cout << cells_x[i] << ", " << cells_y[i] << std::endl; + // std::cout << cells_x[i] << ", " << cells_y[i] << std::endl; + // Converting the cell into distance (Global) float limit_x = cells_x[i] * resolution; float limit_y = cells_y[i] * resolution; - for (int j = 0; j < x_coord.size(); ++j) + std::cout << "******** New cell ********" << std::endl; + + std::cout << "Initial point: " << initial_point[0] << ", " << initial_point[1] << std::endl; + std::cout << "starting at: " << count_idx << std::endl; + + for (int j = count_idx; j < x_coord.size() + 1; ++j) { - // Adding the robot position to the laser beam reading - float read_x = x_coord[i] + robot_pose[0]; - float read_y = y_coord[i] + robot_pose[1]; - - // Distances of the laser hit - if ((abs(read_x) > abs(limit_x)) && - (abs(read_x) < abs(limit_x + resolution)) && - (abs(read_y) > abs(limit_y)) && - (abs(read_y) < abs(limit_y + resolution))) + // Adding the robot position to the laser beam reading - Shift X and Y + float read_x = x_coord[j] + robot_pose[0]; + float read_y = y_coord[j] + robot_pose[1]; + + // std::cout << "Limit X: " << limit_x << ", " << limit_x + resolution << std::endl; + // std::cout << "Limit Y: " << limit_y << ", " << limit_y + resolution << std::endl; + std::cout << "Count: " << j << std::endl; + + // Evaluate to what cell corresponds the current reading + if (((abs(read_x) >= abs(limit_x)) && (abs(read_x) <= abs(limit_x + resolution))) && + ((abs(read_y) >= abs(limit_y)) && (abs(read_y) <= abs(limit_y + resolution)))) { - ; + // std::cout << "Reading: " << read_x << ", " << read_y << std::endl; + // std::cout << "In cell: " << cells_x[i] << ", " << cells_y[i] << std::endl; + final_point[0] = x_coord[j]; + final_point[1] = y_coord[j]; + // std::cout << "Count: " << j << std::endl; + ++count_idx; + } + else + { + // This one is working + break; + } } + std::cout << "Final point: " << final_point[0] << ", " << final_point[1] << std::endl; + // From the start of the cell to the end - This is an approximation - From the robot pose + float distance = calculateDistance(initial_point[0], initial_point[1], final_point[0], final_point[1]); + dist_carry += distance; + std::cout << "Distance: " << distance << ", " << dist_carry << std::endl; + + // Final point becomes the initial for the next movement + initial_point[0] = final_point[0]; + initial_point[1] = final_point[1]; + } + std::cout << "*************************" << std::endl; // @@ -237,6 +293,14 @@ void LifelongSlamToolbox::scannerTest() } } +float LifelongSlamToolbox::calculateDistance(float x_1, float y_1, float x_2, float y_2) +{ + float diff_x = x_2 - x_1; + float diff_y = y_2 - y_1; + + return sqrt(diff_x*diff_x + diff_y*diff_y); +} + float LifelongSlamToolbox::calculateSlope(float x_1, float y_1, float x_2, float y_2) { // Calculates the scope of a given laser beam based on its coordinates From 80bc5eb819912b34ec97ea895d9894a06435ca44 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Tue, 2 Nov 2021 06:04:46 -0500 Subject: [PATCH 06/83] [WIP]: Cleaning comments --- .../experimental/slam_toolbox_lifelong.hpp | 8 +- src/experimental/slam_toolbox_lifelong.cpp | 99 ++++--------------- 2 files changed, 21 insertions(+), 86 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index 7b6ad6c86..fdd9d4072 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -68,21 +68,15 @@ class LifelongSlamToolbox : public SlamToolbox /*****************************************************************************/ /*******************************Implementation********************************/ - void scannerTest(); std::vector getGridPosition(float x, float y, float resolution); std::vector getLaserHit(std::vector const& robot_pose, float distance, float angle); std::pair, std::vector> Bresenham(int x_1, int y_1, int x_2, int y_2); int getSign(int n_1, int n_2); float calculateProbability(float range); - std::vector calculateBeamCoordinates(float x_1, float y_1, float slope, float time); - float calculateSlope(float x_1, float y_1, float x_2, float y_2); float calculateDistance(float x_1, float y_1, float x_2, float y_2); - - - + void scannerTest(); /*****************************************************************************/ - bool use_tree_; double iou_thresh_; double removal_score_; diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 861cbc637..fbb844dc8 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -153,7 +153,6 @@ void LifelongSlamToolbox::scannerTest() std::vector ranges{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters std::vector angles{0.0f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; - // Current yaw + beam angle: -PI/2 (-1.570795) -0.87266 = 2.44345 (-55 degrees) // for (int i = 0; i < ranges.size(); ++i) for (int i = 0; i < 1; ++i) // One reading only @@ -164,12 +163,12 @@ void LifelongSlamToolbox::scannerTest() std::vector laser_grid = getLaserHit(robot_pose, ranges[i], angles[i]); std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1], resolution); std::cout << "Laser end: " << final_grid_pos[0] << ", " << final_grid_pos[1] << std::endl; - // robot_grid_pos[0] // X1 // robot_grid_pos[1] // Y1 - // final_grid_pos[0] // X2 // final_grid_pos[1] // Y2 + // robot_grid_pos[0] // X1 - robot_grid_pos[1] // Y1 + // final_grid_pos[0] // X2 - final_grid_pos[1] // Y2 std::vector cells_x, cells_y; std::pair, std::vector> res_pair = Bresenham(robot_grid_pos[0], robot_grid_pos[1], final_grid_pos[0], final_grid_pos[1]); /** - * Need to append here the last posint, the algorithm is not doing it + * Need to append here the last point, the algorithm is not doing it */ // Cells visited by this laser beam @@ -186,29 +185,15 @@ void LifelongSlamToolbox::scannerTest() x_coord.reserve(samples + 1); y_coord.reserve(samples + 1); - // for (int j = 0; j < cells_x.size(); ++j) - // { - // std::cout << cells_x[j] << ", " << cells_y[j] << std::endl; - // } - while(current_dis < (ranges[i] + step)) { - // Need to shift the angles as the fron is my X - // This will match the global frame - // This loop containts the coordinates for each laser beam + // Discretization loop float x_p = current_dis * sin(angles[i]); float y_p = current_dis * cos(angles[i]); x_coord.emplace_back(x_p); y_coord.emplace_back(y_p); current_dis += step; } - std::cout << " uuuuuuuuuuuuuuuu " << std::endl; - std::cout << x_coord[10] << ", " << y_coord[10] << ", " << x_coord.size() << std::endl; - // I would need to validate if this works for all the quadrants - // std::cout << limit_x << ", " << limit_y << std::endl; - // std::cout << limit_x + resolution << ", " << limit_y << std::endl; - // std::cout << limit_x << ", " << limit_y + resolution<< std::endl; - // std::cout << limit_x + resolution << ", " << limit_y + resolution << std::endl; int count_idx = 0; float dist_carry = 0.0f; @@ -272,50 +257,21 @@ void LifelongSlamToolbox::scannerTest() } std::cout << "*************************" << std::endl; - // - - // To discretize a beam - - // Discretize the line - // Punto inicial es la posicion del robot - // Punto final es la lectura del laser - - - // I could get the distance for each cell - - // As an initial approach I could use the euclidean distance to a given cell - - // I need to calculate - /* - d1 -> Distance from the robot position - */ - } } + float LifelongSlamToolbox::calculateDistance(float x_1, float y_1, float x_2, float y_2) { + /* + Calculates the euclidean distance between two points + */ float diff_x = x_2 - x_1; float diff_y = y_2 - y_1; return sqrt(diff_x*diff_x + diff_y*diff_y); } -float LifelongSlamToolbox::calculateSlope(float x_1, float y_1, float x_2, float y_2) -{ - // Calculates the scope of a given laser beam based on its coordinates - return (y_2 - y_1) / (x_2 - x_1); -} - -std::vector LifelongSlamToolbox::calculateBeamCoordinates(float x_1, float y_1, float slope, float time) -{ - // Coeficients - // Time step - std::cout << slope << std::endl; - - -} - float LifelongSlamToolbox::calculateProbability(float range) { /* @@ -329,17 +285,13 @@ float LifelongSlamToolbox::calculateProbability(float range) return nu*lambda*exp(-lambda*range); } return 0.0f; - - /* - std::cout << calculateProbability(5.0f) << std::endl; - std::cout << calculateProbability(2.5f) << std::endl; - std::cout << calculateProbability(0.0f) << std::endl; - */ } std::vector LifelongSlamToolbox::getGridPosition(float x, float y, float resolution) { - // Map the distance into the grid + /* + Maps the distance into grid coordinates + */ int x_cell = ceil((1 / resolution) * x); int y_cell = ceil((1 / resolution) * y); @@ -348,24 +300,24 @@ std::vector LifelongSlamToolbox::getGridPosition(float x, float y, float re std::vector LifelongSlamToolbox::getLaserHit(std::vector const& robot_pose, float distance, float angle) { - // Returns the distance where the laser beam hits something + /* + Returns the distance where the laser beam hits something + */ float angle_r = atan2(sin(robot_pose[2] + angle), cos(robot_pose[2] + angle)); float x_occ = distance * cos(angle_r) + robot_pose[0]; // This is X float y_occ = -distance * sin(angle_r) + robot_pose[1]; // This is Y std::vector grid_pos = getGridPosition(x_occ, y_occ, 0.5f); - // std::cout << grid_pos[0] << ", " << grid_pos[1] << std::endl; - // std::cout << x_occ << ", " << y_occ << std::endl; - // std::cout << " ---------------------- " << std::endl; - return {x_occ, y_occ}; } std::pair, std::vector> LifelongSlamToolbox::Bresenham(int x_1, int y_1, int x_2, int y_2) { - // This one is working + /* + Returns the set of cells hit by a laser beam + */ std::vector x_bres; std::vector y_bres; @@ -422,24 +374,13 @@ std::pair, std::vector> LifelongSlamToolbox::Bresenham(int y_bres.push_back(y); } return std::pair, std::vector>{x_bres, y_bres}; - - /* - std::vector res_x; - std::vector res_y; - std::pair, std::vector> res_pair = Bresenham(-1, -4, 3, 2); - - res_x = res_pair.first; - res_y = res_pair.second; - for (int i=0; i Date: Tue, 2 Nov 2021 07:15:48 -0500 Subject: [PATCH 07/83] [FIX]: Adding last cell to the calculation so we get the whole distance --- src/experimental/slam_toolbox_lifelong.cpp | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index fbb844dc8..88ebf4afb 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -160,21 +160,24 @@ void LifelongSlamToolbox::scannerTest() std::cout << "........ New laser ........" << std::endl; std::cout << "Distance: " << ranges[i] << ", Angle: " << angles[i] << std::endl; + // Laser continuous distance std::vector laser_grid = getLaserHit(robot_pose, ranges[i], angles[i]); + // Laser final cell std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1], resolution); - std::cout << "Laser end: " << final_grid_pos[0] << ", " << final_grid_pos[1] << std::endl; // robot_grid_pos[0] // X1 - robot_grid_pos[1] // Y1 // final_grid_pos[0] // X2 - final_grid_pos[1] // Y2 + std::vector cells_x, cells_y; std::pair, std::vector> res_pair = Bresenham(robot_grid_pos[0], robot_grid_pos[1], final_grid_pos[0], final_grid_pos[1]); - /** - * Need to append here the last point, the algorithm is not doing it - */ // Cells visited by this laser beam cells_x = res_pair.first; cells_y = res_pair.second; + // Adding last hit cell to the set + cells_x.push_back(final_grid_pos[0]); + cells_y.push_back(final_grid_pos[1]); + // Laser beam discretization int samples = 100; float step = ranges[i] / samples; @@ -203,6 +206,10 @@ void LifelongSlamToolbox::scannerTest() initial_point[0] = 0.0f; initial_point[1] = 0.0f; + std::cout << "Size: " << x_coord.size() << std::endl; + std::cout << "Value 1: " << x_coord[91] << ", " << y_coord[91] << std::endl; + std::cout << "Value 2: " << x_coord[100] << ", " << y_coord[100] << std::endl; + // Move alongside all the cells that the laser beam hits for (int i = 0; i < cells_x.size(); ++i) { @@ -215,7 +222,7 @@ void LifelongSlamToolbox::scannerTest() std::cout << "Initial point: " << initial_point[0] << ", " << initial_point[1] << std::endl; std::cout << "starting at: " << count_idx << std::endl; - + for (int j = count_idx; j < x_coord.size() + 1; ++j) { // Adding the robot position to the laser beam reading - Shift X and Y @@ -224,7 +231,7 @@ void LifelongSlamToolbox::scannerTest() // std::cout << "Limit X: " << limit_x << ", " << limit_x + resolution << std::endl; // std::cout << "Limit Y: " << limit_y << ", " << limit_y + resolution << std::endl; - std::cout << "Count: " << j << std::endl; + // std::cout << "Count: " << j << std::endl; // Evaluate to what cell corresponds the current reading if (((abs(read_x) >= abs(limit_x)) && (abs(read_x) <= abs(limit_x + resolution))) && @@ -256,7 +263,6 @@ void LifelongSlamToolbox::scannerTest() } std::cout << "*************************" << std::endl; - } } @@ -307,8 +313,6 @@ std::vector LifelongSlamToolbox::getLaserHit(std::vector const& ro float x_occ = distance * cos(angle_r) + robot_pose[0]; // This is X float y_occ = -distance * sin(angle_r) + robot_pose[1]; // This is Y - std::vector grid_pos = getGridPosition(x_occ, y_occ, 0.5f); - return {x_occ, y_occ}; } From 579c56293bd8150dab7cf27ccc677bdb0cc8a5ae Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Tue, 2 Nov 2021 11:35:24 -0500 Subject: [PATCH 08/83] [WIP]: Distance to each cell is being calculated. This is not working on all quadrants --- src/experimental/slam_toolbox_lifelong.cpp | 93 +++++++++++++++------- 1 file changed, 66 insertions(+), 27 deletions(-) diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 88ebf4afb..07df743ab 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -146,12 +146,13 @@ void LifelongSlamToolbox::scannerTest() std::vector robot_grid_pos = getGridPosition(robot_pose[0], robot_pose[1], resolution); std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; - // Creating the laser scan with 5 beams ------- Angles will be -55, -25, 0, 25, 55 //----// +-55 = +-0.87266 : +-25 = +-0.43633 + // Creating the laser scan with 5 beams ------- Angles will be -50, -25, 0, 25, 50 //----// +-50 = +-0.87266 : +-25 = +-0.43633 // std::vector ranges{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters - // std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + // std::vector angles{0.785398f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; std::vector ranges{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters - std::vector angles{0.0f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + // std::vector angles{0.0f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // Current yaw + beam angle: -PI/2 (-1.570795) -0.87266 = 2.44345 (-55 degrees) // for (int i = 0; i < ranges.size(); ++i) @@ -175,11 +176,19 @@ void LifelongSlamToolbox::scannerTest() cells_y = res_pair.second; // Adding last hit cell to the set - cells_x.push_back(final_grid_pos[0]); + cells_x.push_back(final_grid_pos[0]); // Here make the change cells_y.push_back(final_grid_pos[1]); + // std::cout << "Cells" << std::endl; + // for (int c = 0; c < cells_x.size(); ++c) // One reading only + // { + // std::cout << cells_x[c] << ", " << cells_y[c] << std::endl; + // } + // std::cout << "End of cells" << std::endl; + + // Laser beam discretization - int samples = 100; + int samples = 200; float step = ranges[i] / samples; float current_dis = 0.0f; @@ -206,62 +215,92 @@ void LifelongSlamToolbox::scannerTest() initial_point[0] = 0.0f; initial_point[1] = 0.0f; - std::cout << "Size: " << x_coord.size() << std::endl; - std::cout << "Value 1: " << x_coord[91] << ", " << y_coord[91] << std::endl; - std::cout << "Value 2: " << x_coord[100] << ", " << y_coord[100] << std::endl; + // Let's save the initial and the final points + std::pair, std::vector> initial_pts, final_pts; + + // initial_pts.first.push_back(initial_point[0]); + // initial_pts.second.push_back(initial_point[1]); + + // std::cout << "Size: " << x_coord.size() << std::endl; + // std::cout << "Value 1: " << x_coord[26] << ", " << y_coord[26] << std::endl; + // std::cout << "Value 2: " << x_coord[27] << ", " << y_coord[27] << std::endl; // Move alongside all the cells that the laser beam hits for (int i = 0; i < cells_x.size(); ++i) { + bool first = true; // std::cout << cells_x[i] << ", " << cells_y[i] << std::endl; // Converting the cell into distance (Global) float limit_x = cells_x[i] * resolution; float limit_y = cells_y[i] * resolution; std::cout << "******** New cell ********" << std::endl; + // initial_pts.first.push_back(initial_point[0]); + // initial_pts.second.push_back(initial_point[1]); - std::cout << "Initial point: " << initial_point[0] << ", " << initial_point[1] << std::endl; - std::cout << "starting at: " << count_idx << std::endl; + // std::cout << "Cell: " << cells_x[i] << ", " << cells_y[i] << std::endl; + // std::cout << "************************" << std::endl; + // std::cout << "Starting at: " << count_idx << std::endl; + // std::cout << "Size: " << x_coord.size() << std::endl; - for (int j = count_idx; j < x_coord.size() + 1; ++j) + for (int j = count_idx; j < x_coord.size(); ++j) { // Adding the robot position to the laser beam reading - Shift X and Y float read_x = x_coord[j] + robot_pose[0]; float read_y = y_coord[j] + robot_pose[1]; - // std::cout << "Limit X: " << limit_x << ", " << limit_x + resolution << std::endl; - // std::cout << "Limit Y: " << limit_y << ", " << limit_y + resolution << std::endl; + // std::cout << "Reading: " << read_x - 5.5f << ", " << read_y - 6.0f<< std::endl; + // std::cout << "Limit X: " << limit_x - 5.5f << ", " << limit_x + resolution - 5.5f << std::endl; + // std::cout << "Limit Y: " << limit_y - 6.0f << ", " << limit_y + resolution - 6.0f << std::endl; + + std::cout << "Reading: " << abs(read_x) << ", " << abs(read_y) << std::endl; + std::cout << "Limit X: " << abs(limit_x) << ", " << abs(limit_x - resolution) << std::endl; + std::cout << "Limit Y: " << abs(limit_y) << ", " << abs(limit_y + resolution) << std::endl; // std::cout << "Count: " << j << std::endl; // Evaluate to what cell corresponds the current reading - if (((abs(read_x) >= abs(limit_x)) && (abs(read_x) <= abs(limit_x + resolution))) && + if (((abs(read_x) >= abs(limit_x)) && (abs(read_x) <= abs(limit_x - resolution))) && ((abs(read_y) >= abs(limit_y)) && (abs(read_y) <= abs(limit_y + resolution)))) { - // std::cout << "Reading: " << read_x << ", " << read_y << std::endl; - // std::cout << "In cell: " << cells_x[i] << ", " << cells_y[i] << std::endl; + if (first) + { + // First reading inside a cell will be the initial point + // On this way we avoid useless corners + // std::cout << "Initial point: " << initial_point[0] << ", " << initial_point[1] << std::endl; + initial_point[0] = x_coord[j]; + initial_point[1] = y_coord[j]; + initial_pts.first.push_back(initial_point[0]); + initial_pts.second.push_back(initial_point[1]); + first = false; + } + + // std::cout << "Reading: " << read_x - 5.5f << ", " << read_y - 6.0f << std::endl; + // std::cout << "Count: " << j << std::endl; final_point[0] = x_coord[j]; final_point[1] = y_coord[j]; - // std::cout << "Count: " << j << std::endl; ++count_idx; - } - else - { - // This one is working - break; - } } + + final_pts.first.push_back(final_point[0]); + final_pts.second.push_back(final_point[1]); + std::cout << "Final point: " << final_point[0] << ", " << final_point[1] << std::endl; // From the start of the cell to the end - This is an approximation - From the robot pose float distance = calculateDistance(initial_point[0], initial_point[1], final_point[0], final_point[1]); dist_carry += distance; std::cout << "Distance: " << distance << ", " << dist_carry << std::endl; + } - // Final point becomes the initial for the next movement - initial_point[0] = final_point[0]; - initial_point[1] = final_point[1]; + // std::cout << "-------- Initial points --------" << std::endl; - } + // for (int i = 0; i < initial_pts.first.size(); ++i) + // { + // std::cout << "----------------" << std::endl; + // std::cout << "Initial: " << initial_pts.first[i] << ", " << initial_pts.second[i] << std::endl; + // std::cout << "Final: " << final_pts.first[i] << ", " << final_pts.second[i] << std::endl; + + // } std::cout << "*************************" << std::endl; } } From 2fa788a57e0fead7901ae6e3dd7f3ff46bb8007b Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Wed, 3 Nov 2021 11:17:32 -0500 Subject: [PATCH 09/83] [ADD]: Probabilities calculation --- src/experimental/slam_toolbox_lifelong.cpp | 126 ++++++++++++--------- 1 file changed, 71 insertions(+), 55 deletions(-) diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 07df743ab..0a6b9498f 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -141,18 +141,18 @@ void LifelongSlamToolbox::scannerTest() // I will have 5 lasers for each reading (Located at 0, +-25, +-50) std::vector robot_pose{5.5f, 6.0f, -PI/2}; + // std::vector robot_pose{5.5f, 6.0f, PI/2}; + // std::vector robot_pose{5.5f, 6.0f, 0.0f}; // This is the initial point std::vector robot_grid_pos = getGridPosition(robot_pose[0], robot_pose[1], resolution); std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; // Creating the laser scan with 5 beams ------- Angles will be -50, -25, 0, 25, 50 //----// +-50 = +-0.87266 : +-25 = +-0.43633 - // std::vector ranges{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters - std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + std::vector ranges{2.75f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters + std::vector angles{0.0f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // std::vector angles{0.785398f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; - - std::vector ranges{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters - // std::vector angles{0.0f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + // std::vector angles{0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // Current yaw + beam angle: -PI/2 (-1.570795) -0.87266 = 2.44345 (-55 degrees) // for (int i = 0; i < ranges.size(); ++i) @@ -179,14 +179,6 @@ void LifelongSlamToolbox::scannerTest() cells_x.push_back(final_grid_pos[0]); // Here make the change cells_y.push_back(final_grid_pos[1]); - // std::cout << "Cells" << std::endl; - // for (int c = 0; c < cells_x.size(); ++c) // One reading only - // { - // std::cout << cells_x[c] << ", " << cells_y[c] << std::endl; - // } - // std::cout << "End of cells" << std::endl; - - // Laser beam discretization int samples = 200; float step = ranges[i] / samples; @@ -215,81 +207,98 @@ void LifelongSlamToolbox::scannerTest() initial_point[0] = 0.0f; initial_point[1] = 0.0f; - // Let's save the initial and the final points + // Initial and final points std::pair, std::vector> initial_pts, final_pts; // initial_pts.first.push_back(initial_point[0]); // initial_pts.second.push_back(initial_point[1]); - // std::cout << "Size: " << x_coord.size() << std::endl; - // std::cout << "Value 1: " << x_coord[26] << ", " << y_coord[26] << std::endl; - // std::cout << "Value 2: " << x_coord[27] << ", " << y_coord[27] << std::endl; - // Move alongside all the cells that the laser beam hits - for (int i = 0; i < cells_x.size(); ++i) + for (int j = 0; j < cells_x.size(); ++j) { bool first = true; - // std::cout << cells_x[i] << ", " << cells_y[i] << std::endl; - // Converting the cell into distance (Global) - float limit_x = cells_x[i] * resolution; - float limit_y = cells_y[i] * resolution; + + // Converting the cell into distance (Global) + float limit_x = cells_x[j] * resolution; + float limit_y = cells_y[j] * resolution; std::cout << "******** New cell ********" << std::endl; - // initial_pts.first.push_back(initial_point[0]); - // initial_pts.second.push_back(initial_point[1]); - - // std::cout << "Cell: " << cells_x[i] << ", " << cells_y[i] << std::endl; - // std::cout << "************************" << std::endl; - // std::cout << "Starting at: " << count_idx << std::endl; - // std::cout << "Size: " << x_coord.size() << std::endl; + std::cout << "Starting at: " << count_idx << std::endl; - for (int j = count_idx; j < x_coord.size(); ++j) + for (int k = count_idx; k < x_coord.size(); ++k) { // Adding the robot position to the laser beam reading - Shift X and Y - float read_x = x_coord[j] + robot_pose[0]; - float read_y = y_coord[j] + robot_pose[1]; - - // std::cout << "Reading: " << read_x - 5.5f << ", " << read_y - 6.0f<< std::endl; - // std::cout << "Limit X: " << limit_x - 5.5f << ", " << limit_x + resolution - 5.5f << std::endl; - // std::cout << "Limit Y: " << limit_y - 6.0f << ", " << limit_y + resolution - 6.0f << std::endl; + float read_x = x_coord[k] + robot_pose[0]; + float read_y = y_coord[k] + robot_pose[1]; - std::cout << "Reading: " << abs(read_x) << ", " << abs(read_y) << std::endl; - std::cout << "Limit X: " << abs(limit_x) << ", " << abs(limit_x - resolution) << std::endl; - std::cout << "Limit Y: " << abs(limit_y) << ", " << abs(limit_y + resolution) << std::endl; + // // std::cout << "Reading x: " << abs(read_x) << ", Reading y: " << abs(read_y) << std::endl; + // std::cout << "Limit X: " << abs(limit_x) - 5.5f << ", " << abs(limit_x + resolution) - 5.5f << std::endl; + // std::cout << "Limit Y: " << abs(limit_y) - 6.0f << ", " << abs(limit_y + resolution) - 6.0f << std::endl; // std::cout << "Count: " << j << std::endl; + // if (((abs(read_x) >= abs(limit_x)) && (abs(read_x) <= abs(limit_x + resolution)))) + // { + // // This is working correctly for X + // std::cout << "+++++++++++++ True +++++++++++++" << std::endl; + // } + + // if (((abs(read_y) >= abs(limit_y)) && (abs(read_y) <= abs(limit_y + resolution)))) + // { + // // This is working correctly for Y + // std::cout << "+++++++++++++ True +++++++++++++" << std::endl; + // } + // Evaluate to what cell corresponds the current reading - if (((abs(read_x) >= abs(limit_x)) && (abs(read_x) <= abs(limit_x - resolution))) && + if (((abs(read_x) >= abs(limit_x)) && (abs(read_x) <= abs(limit_x + resolution))) && ((abs(read_y) >= abs(limit_y)) && (abs(read_y) <= abs(limit_y + resolution)))) { if (first) { // First reading inside a cell will be the initial point - // On this way we avoid useless corners + // std::cout << "True at: " << x_coord[j] << "," << y_coord[j] << std::endl; + initial_point[0] = x_coord[k]; + initial_point[1] = y_coord[k]; // std::cout << "Initial point: " << initial_point[0] << ", " << initial_point[1] << std::endl; - initial_point[0] = x_coord[j]; - initial_point[1] = y_coord[j]; initial_pts.first.push_back(initial_point[0]); initial_pts.second.push_back(initial_point[1]); first = false; } - - // std::cout << "Reading: " << read_x - 5.5f << ", " << read_y - 6.0f << std::endl; - // std::cout << "Count: " << j << std::endl; - final_point[0] = x_coord[j]; - final_point[1] = y_coord[j]; + final_point[0] = x_coord[k]; + final_point[1] = y_coord[k]; ++count_idx; } } - final_pts.first.push_back(final_point[0]); final_pts.second.push_back(final_point[1]); - std::cout << "Final point: " << final_point[0] << ", " << final_point[1] << std::endl; // From the start of the cell to the end - This is an approximation - From the robot pose float distance = calculateDistance(initial_point[0], initial_point[1], final_point[0], final_point[1]); dist_carry += distance; - std::cout << "Distance: " << distance << ", " << dist_carry << std::endl; + // std::cout << "Distance: " << distance << ", " << dist_carry << std::endl; + // Before moving into the next cell we can calculate the interal here + // We have the initial point and the final point so we can perform this calculation + float probability = calculateProbability(ranges[i]); // This is the right place for calculating this + + // Distances + float d1 = calculateDistance(0, 0, initial_point[0], initial_point[1]); + float d2 = calculateDistance(0, 0, final_point[0], final_point[1]); + + std::cout << "Distances: " << d1 << ", " << d2 << std::endl; + + // Unobserved + float unobserved = d1 * probability; + // Occupied + float occupied = probability * (d2 - d1); + // Free + float free = probability * (5.0 - d2); // Maximum range + + std::cout << "Unobserved: " << unobserved << std::endl; + std::cout << "Occupied: " << occupied << std::endl; + std::cout << "Free: " << free << std::endl; + + // std::cout << "Initial point: " << initial_point[0] << ", " << initial_point[1] << std::endl; + // std::cout << "Final point: " << final_point[0] << ", " << final_point[1] << std::endl; + // std::cout << "Probability: " << probability << std::endl; } // std::cout << "-------- Initial points --------" << std::endl; @@ -337,8 +346,15 @@ std::vector LifelongSlamToolbox::getGridPosition(float x, float y, float re /* Maps the distance into grid coordinates */ - int x_cell = ceil((1 / resolution) * x); - int y_cell = ceil((1 / resolution) * y); + // int x_cell = ceil((1 / resolution) * x); + // int y_cell = ceil((1 / resolution) * y); + int x_cell = floor((1 / resolution) * x); + int y_cell = floor((1 / resolution) * y); + + /** + * No matter what I will always get the left bottom corner + * */ + return {x_cell, y_cell}; } @@ -347,6 +363,7 @@ std::vector LifelongSlamToolbox::getLaserHit(std::vector const& ro { /* Returns the distance where the laser beam hits something + This is done in the global coordinates */ float angle_r = atan2(sin(robot_pose[2] + angle), cos(robot_pose[2] + angle)); float x_occ = distance * cos(angle_r) + robot_pose[0]; // This is X @@ -355,7 +372,6 @@ std::vector LifelongSlamToolbox::getLaserHit(std::vector const& ro return {x_occ, y_occ}; } - std::pair, std::vector> LifelongSlamToolbox::Bresenham(int x_1, int y_1, int x_2, int y_2) { /* From d91e4c703b8a8f14aab3dbd95769d1b875c82387 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Thu, 4 Nov 2021 07:14:28 -0500 Subject: [PATCH 10/83] [FIX]: RBT transformation for getting the final point --- .../experimental/slam_toolbox_lifelong.hpp | 2 +- src/experimental/slam_toolbox_lifelong.cpp | 37 +++++++++++++------ 2 files changed, 27 insertions(+), 12 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index fdd9d4072..e49006b46 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -69,7 +69,7 @@ class LifelongSlamToolbox : public SlamToolbox /*****************************************************************************/ /*******************************Implementation********************************/ std::vector getGridPosition(float x, float y, float resolution); - std::vector getLaserHit(std::vector const& robot_pose, float distance, float angle); + std::vector getLaserHit(std::vector const& robot_pose, float range, float angle); std::pair, std::vector> Bresenham(int x_1, int y_1, int x_2, int y_2); int getSign(int n_1, int n_2); float calculateProbability(float range); diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 0a6b9498f..c79573d35 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -140,7 +140,7 @@ void LifelongSlamToolbox::scannerTest() } // I will have 5 lasers for each reading (Located at 0, +-25, +-50) - std::vector robot_pose{5.5f, 6.0f, -PI/2}; + std::vector robot_pose{5.6f, 6.0f, PI/2}; // std::vector robot_pose{5.5f, 6.0f, PI/2}; // std::vector robot_pose{5.5f, 6.0f, 0.0f}; @@ -148,8 +148,12 @@ void LifelongSlamToolbox::scannerTest() std::vector robot_grid_pos = getGridPosition(robot_pose[0], robot_pose[1], resolution); std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; + // 3.0 to 3.5 would be the range + float probability1 = calculateProbability(3.15); + std::cout << "Probability: " << probability1 << std::endl; + // Creating the laser scan with 5 beams ------- Angles will be -50, -25, 0, 25, 50 //----// +-50 = +-0.87266 : +-25 = +-0.43633 - std::vector ranges{2.75f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters + std::vector ranges{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters std::vector angles{0.0f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // std::vector angles{0.785398f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // std::vector angles{0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; @@ -179,8 +183,15 @@ void LifelongSlamToolbox::scannerTest() cells_x.push_back(final_grid_pos[0]); // Here make the change cells_y.push_back(final_grid_pos[1]); + std::cout << "Cells" << std::endl; + for (int c = 0; c < cells_x.size(); ++c) // One reading only + { + std::cout << cells_x[c] << ", " << cells_y[c] << std::endl; + } + std::cout << "End of cells" << std::endl; + // Laser beam discretization - int samples = 200; + int samples = 300; float step = ranges[i] / samples; float current_dis = 0.0f; @@ -274,7 +285,7 @@ void LifelongSlamToolbox::scannerTest() // From the start of the cell to the end - This is an approximation - From the robot pose float distance = calculateDistance(initial_point[0], initial_point[1], final_point[0], final_point[1]); dist_carry += distance; - // std::cout << "Distance: " << distance << ", " << dist_carry << std::endl; + std::cout << "Distance: " << distance << ", " << dist_carry << std::endl; // Before moving into the next cell we can calculate the interal here // We have the initial point and the final point so we can perform this calculation float probability = calculateProbability(ranges[i]); // This is the right place for calculating this @@ -285,10 +296,14 @@ void LifelongSlamToolbox::scannerTest() std::cout << "Distances: " << d1 << ", " << d2 << std::endl; + // Only one of them applies - Posibilities // Unobserved float unobserved = d1 * probability; // Occupied - float occupied = probability * (d2 - d1); + float occupied = probability * (d2 - d1); // A change on this one is required + // Get the point, if not maximum add resolution to the initial point + // I can do a ray tracing with the whole range + // I have the distance where this is cut so I cand o some mathematical stuff // Free float free = probability * (5.0 - d2); // Maximum range @@ -359,17 +374,17 @@ std::vector LifelongSlamToolbox::getGridPosition(float x, float y, float re return {x_cell, y_cell}; } -std::vector LifelongSlamToolbox::getLaserHit(std::vector const& robot_pose, float distance, float angle) +std::vector LifelongSlamToolbox::getLaserHit(std::vector const& robot_pose, float range, float angle) { /* Returns the distance where the laser beam hits something - This is done in the global coordinates + Rigid Body Trasnformation from the global to the sensor frame */ - float angle_r = atan2(sin(robot_pose[2] + angle), cos(robot_pose[2] + angle)); - float x_occ = distance * cos(angle_r) + robot_pose[0]; // This is X - float y_occ = -distance * sin(angle_r) + robot_pose[1]; // This is Y - return {x_occ, y_occ}; + float x_tf = (range * cos(robot_pose[2]) * cos(angle)) - (range * sin(robot_pose[2]) * sin(angle)) + robot_pose[0]; + float y_tf = (range * sin(robot_pose[2]) * cos(angle)) + (range * cos(robot_pose[2]) * sin(angle)) + robot_pose[1]; + + return {x_tf, y_tf}; } std::pair, std::vector> LifelongSlamToolbox::Bresenham(int x_1, int y_1, int x_2, int y_2) From c3f41556ce9728f113842352da740b5daa8c3add Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Fri, 5 Nov 2021 11:17:29 -0500 Subject: [PATCH 11/83] [FEAT]: Finally getting the cutting points to each individual cell --- .../experimental/slam_toolbox_lifelong.hpp | 2 + src/experimental/slam_toolbox_lifelong.cpp | 395 +++++++++++++----- 2 files changed, 286 insertions(+), 111 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index e49006b46..06e362ca3 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -71,10 +71,12 @@ class LifelongSlamToolbox : public SlamToolbox std::vector getGridPosition(float x, float y, float resolution); std::vector getLaserHit(std::vector const& robot_pose, float range, float angle); std::pair, std::vector> Bresenham(int x_1, int y_1, int x_2, int y_2); + std::vector calculateIntersection(std::vector laser_start, std::vector laser_end, std::vector cell_start, std::vector cell_end); int getSign(int n_1, int n_2); float calculateProbability(float range); float calculateDistance(float x_1, float y_1, float x_2, float y_2); void scannerTest(); + std::vector getCellPosition(std::vector grid_cell, float resolution); /*****************************************************************************/ bool use_tree_; diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index c79573d35..ac7023570 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -88,46 +88,46 @@ LifelongSlamToolbox::LifelongSlamToolbox(rclcpp::NodeOptions options) /*****************************************************************************/ /** * The are implementation notes from Camilo OCT 21 - * + * * First step * Get the occupancy probability for each cell in the map. This solution is based * on the Log-Odds rather than the Bayesian filter, only for computation performance * Initialization for this elements should be made before we start the following loop: - * Gather the sensor data. + * Gather the sensor data. * Inverse measurement model (Tranforming scan to measure occupancy belief map) * Log-Odds calculation for the current reading and update the grid or map. * Probability map calculation and update for current reading (This is done for the cells)\ - * + * * NOTE * Once we get the probabilities we can calculate the entropy of each grid cell. I am assuming - * this should be done inside the loop as we want to calculate this entropy at each time. + * this should be done inside the loop as we want to calculate this entropy at each time. * Log odds assume values from −∞ to ∞. The Bayes filter for updating beliefs in * log odds representation is computationally elegant. It avoids truncation problems that * arise for probabilities close to 0 or 1. - * - * + * + * * We are using std::vector !! - * - * - * + * + * + * * According tothe the last meeting we handle, I will start with a prrof of concept * Create a grid - * Calculate the scan position (WRO a given cell). Consider where is my initial cell + * Calculate the scan position (WRO a given cell). Consider where is my initial cell * Calculate the probability of seing an grid given a set of measurements * Create the histogram with Algrotihm 1 /*****************************************************************************/ /*****************************************************************************/ /*******************************Implementation********************************/ - + void LifelongSlamToolbox::scannerTest() { RCLCPP_WARN(get_logger(), "<-------- Scanner Test -------->"); - + float resolution = 0.5f; // Cell resolution - 1 meter float map_dist = 20.0f; // Total map distance - + int number_cells = map_dist / resolution; std::vector> grid(number_cells); // 20 meters in Y dimension for (int i=0; i robot_pose{5.6f, 6.0f, PI/2}; + // std::vector robot_pose{5.6f, 6.0f, PI/4}; std::vector robot_pose{5.6f, 6.0f, PI/2}; - // std::vector robot_pose{5.5f, 6.0f, PI/2}; + // std::vector robot_pose{5.5f, 6.0f, -PI/4}; + // std::vector robot_pose{5.5f, 6.0f, -3*PI/4}; + // -PI is an unhandled case + // std::vector robot_pose{5.5f, 6.0f, PI}; // std::vector robot_pose{5.5f, 6.0f, 0.0f}; // This is the initial point std::vector robot_grid_pos = getGridPosition(robot_pose[0], robot_pose[1], resolution); std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; - // 3.0 to 3.5 would be the range - float probability1 = calculateProbability(3.15); - std::cout << "Probability: " << probability1 << std::endl; - // Creating the laser scan with 5 beams ------- Angles will be -50, -25, 0, 25, 50 //----// +-50 = +-0.87266 : +-25 = +-0.43633 std::vector ranges{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters - std::vector angles{0.0f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // std::vector angles{0.785398f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // std::vector angles{0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; @@ -169,6 +170,12 @@ void LifelongSlamToolbox::scannerTest() std::vector laser_grid = getLaserHit(robot_pose, ranges[i], angles[i]); // Laser final cell std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1], resolution); + std::cout << final_grid_pos[0] << ", " << final_grid_pos[1] << std::endl; + + + std::vector cell_pos = getCellPosition({11, 12}, resolution); + std::cout << cell_pos[0] << ", " << cell_pos[1] << std::endl; + // robot_grid_pos[0] // X1 - robot_grid_pos[1] // Y1 // final_grid_pos[0] // X2 - final_grid_pos[1] // Y2 @@ -179,16 +186,18 @@ void LifelongSlamToolbox::scannerTest() cells_x = res_pair.first; cells_y = res_pair.second; - // Adding last hit cell to the set + // Adding last hit cell to the set cells_x.push_back(final_grid_pos[0]); // Here make the change cells_y.push_back(final_grid_pos[1]); - std::cout << "Cells" << std::endl; - for (int c = 0; c < cells_x.size(); ++c) // One reading only - { - std::cout << cells_x[c] << ", " << cells_y[c] << std::endl; - } - std::cout << "End of cells" << std::endl; + // std::cout << "Cells" << std::endl; + // for (int c = 0; c < cells_x.size(); ++c) // One reading only + // { + // std::cout << cells_x[c] << ", " << cells_y[c] << std::endl; + // } + // std::cout << "End of cells" << std::endl; + + // Discretization is no longer required // Laser beam discretization int samples = 300; @@ -210,38 +219,149 @@ void LifelongSlamToolbox::scannerTest() current_dis += step; } + int count_idx = 0; float dist_carry = 0.0f; std::vector initial_point(2), final_point(2); - - initial_point[0] = 0.0f; + + initial_point[0] = 0.0f; initial_point[1] = 0.0f; - // Initial and final points + // Initial and final points std::pair, std::vector> initial_pts, final_pts; - + // initial_pts.first.push_back(initial_point[0]); // initial_pts.second.push_back(initial_point[1]); // Move alongside all the cells that the laser beam hits + std::cout << " +++++++++++++ " << std::endl; + for (int j = 0; j < cells_x.size(); ++j) { - bool first = true; + // Cells visutalization + std::cout << cells_x[j] << ", " << cells_y[j] << std::endl; - // Converting the cell into distance (Global) + // Converting the cell into distance float limit_x = cells_x[j] * resolution; float limit_y = cells_y[j] * resolution; + + std::vector initial_x {limit_x, limit_x, limit_x + resolution, limit_x + resolution}; + std::vector initial_y {limit_y, limit_y, limit_y + resolution, limit_y + resolution}; + + std::vector final_x {limit_x + resolution, limit_x, limit_x + resolution, limit_x}; + std::vector final_y {limit_y, limit_y + resolution, limit_y, limit_y + resolution}; - std::cout << "******** New cell ********" << std::endl; - std::cout << "Starting at: " << count_idx << std::endl; + float min_x = limit_x; + float max_x = limit_x + resolution; + float min_y = limit_y; + float max_y = limit_y + resolution; - for (int k = count_idx; k < x_coord.size(); ++k) + if (final_grid_pos[0] < robot_grid_pos[0] && final_grid_pos[1] >= robot_grid_pos[1]) { - // Adding the robot position to the laser beam reading - Shift X and Y - float read_x = x_coord[k] + robot_pose[0]; - float read_y = y_coord[k] + robot_pose[1]; + std::cout << "1 " << std::endl; + + // X minor and Y greater. WRO final points + initial_x[2] = limit_x - resolution; + initial_x[3] = limit_x - resolution; + final_x[0] = limit_x - resolution; + final_x[2] = limit_x - resolution; + + min_x = limit_x - resolution; + max_x = limit_x; + } + + if (final_grid_pos[0] >= robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) + { + std::cout << "2 " << std::endl; + + // X greater and Y minor. WRO final points + initial_y[2] = limit_y - resolution; + initial_y[3] = limit_y - resolution; + + final_y[1] = limit_y - resolution; + final_y[3] = limit_y - resolution; + + min_y = limit_y - resolution; + max_y = limit_y; + } + + if (final_grid_pos[0] < robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) + { + std::cout << "3 " << std::endl; + + // X minor and Y minor. WRO final points + initial_x[2] = limit_x - resolution; + initial_x[3] = limit_x - resolution; + initial_y[2] = limit_y - resolution; + initial_y[3] = limit_y - resolution; + + final_x[0] = limit_x - resolution; + final_x[2] = limit_x - resolution; + final_y[1] = limit_y - resolution; + final_y[3] = limit_y - resolution; + + min_x = limit_x - resolution; + max_x = limit_x; + min_y = limit_y - resolution; + max_y = limit_y; + } + + for (int k = 0; k < 4; ++k) + { + // std::cout << "Points: " << initial_x[k] << ", " << initial_y[k] << ", " << final_x[k] << ", " << final_y[k] << std::endl; + std::vector intersection = calculateIntersection(robot_pose, laser_grid, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); + if(intersection.size() != 0) + { + if ((abs(intersection[0]) >= abs(min_x)) && + (abs(intersection[0]) <= abs(max_x)) && + (abs(intersection[1]) >= abs(min_y)) && + (abs(intersection[1]) <= abs(max_y))) + { + std::cout << "Interception at: " << intersection[0] << ", " << intersection[1] << std::endl; + } + } + else + { + std::cout << "No interception " << std::endl; + } + } + + // Current cell limits + // float inf_x = limit_x; + // float sup_x = limit_x + resolution; + // float inf_y = limit_y; + // float sup_y = limit_y + resolution; + + // // // I need to transform that point + // std::vector res = calculateIntersection(robot_pose, laser_grid, {5.5f, 6.5f}, {6.0f, 6.5f}); + + // if(res.size() != 0) + // { + // std::cout << "Intercept" << std::endl; + // std::cout << res[0] << ", " << res[1] << std::endl; + // } + std::cout << " +++++++++++++ " << std::endl; + + // std::cout << limit_x << ", " << limit_y << std::endl; + // std::cout << limit_x + resolution << ", " << limit_y << std::endl; + // std::cout << limit_x << ", " << limit_y + resolution << std::endl; + // std::cout << limit_x + resolution<< ", " << limit_y + resolution << std::endl; + + + // std::cout << "******** New cell ********" << std::endl; + // std::cout << "Starting at: " << count_idx << std::endl; + + // for (int k = count_idx; k < x_coord.size(); ++k) + // { + // // Adding the robot position to the laser beam reading - Shift X and Y + // float read_x = x_coord[k] + robot_pose[0]; + // float read_y = y_coord[k] + robot_pose[1]; + + // float x_tf = (range * cos(robot_pose[2]) * cos(angle)) - (range * sin(robot_pose[2]) * sin(angle)) + robot_pose[0]; + // float y_tf = (range * sin(robot_pose[2]) * cos(angle)) + (range * cos(robot_pose[2]) * sin(angle)) + robot_pose[1]; + // // std::cout << "Reading x: " << abs(read_x) << ", Reading y: " << abs(read_y) << std::endl; // std::cout << "Limit X: " << abs(limit_x) - 5.5f << ", " << abs(limit_x + resolution) - 5.5f << std::endl; // std::cout << "Limit Y: " << abs(limit_y) - 6.0f << ", " << abs(limit_y + resolution) - 6.0f << std::endl; @@ -260,60 +380,60 @@ void LifelongSlamToolbox::scannerTest() // } // Evaluate to what cell corresponds the current reading - if (((abs(read_x) >= abs(limit_x)) && (abs(read_x) <= abs(limit_x + resolution))) && - ((abs(read_y) >= abs(limit_y)) && (abs(read_y) <= abs(limit_y + resolution)))) - { - if (first) - { - // First reading inside a cell will be the initial point - // std::cout << "True at: " << x_coord[j] << "," << y_coord[j] << std::endl; - initial_point[0] = x_coord[k]; - initial_point[1] = y_coord[k]; - // std::cout << "Initial point: " << initial_point[0] << ", " << initial_point[1] << std::endl; - initial_pts.first.push_back(initial_point[0]); - initial_pts.second.push_back(initial_point[1]); - first = false; - } - final_point[0] = x_coord[k]; - final_point[1] = y_coord[k]; - ++count_idx; - } - } - final_pts.first.push_back(final_point[0]); - final_pts.second.push_back(final_point[1]); - - // From the start of the cell to the end - This is an approximation - From the robot pose - float distance = calculateDistance(initial_point[0], initial_point[1], final_point[0], final_point[1]); - dist_carry += distance; - std::cout << "Distance: " << distance << ", " << dist_carry << std::endl; - // Before moving into the next cell we can calculate the interal here - // We have the initial point and the final point so we can perform this calculation - float probability = calculateProbability(ranges[i]); // This is the right place for calculating this - - // Distances - float d1 = calculateDistance(0, 0, initial_point[0], initial_point[1]); - float d2 = calculateDistance(0, 0, final_point[0], final_point[1]); - - std::cout << "Distances: " << d1 << ", " << d2 << std::endl; - - // Only one of them applies - Posibilities - // Unobserved - float unobserved = d1 * probability; - // Occupied - float occupied = probability * (d2 - d1); // A change on this one is required - // Get the point, if not maximum add resolution to the initial point - // I can do a ray tracing with the whole range - // I have the distance where this is cut so I cand o some mathematical stuff - // Free - float free = probability * (5.0 - d2); // Maximum range - - std::cout << "Unobserved: " << unobserved << std::endl; - std::cout << "Occupied: " << occupied << std::endl; - std::cout << "Free: " << free << std::endl; - - // std::cout << "Initial point: " << initial_point[0] << ", " << initial_point[1] << std::endl; + // if (((abs(read_x) >= abs(limit_x)) && (abs(read_x) <= abs(limit_x + resolution))) && + // ((abs(read_y) >= abs(limit_y)) && (abs(read_y) <= abs(limit_y + resolution)))) + // { + // if (first) + // { + // // First reading inside a cell will be the initial point + // // std::cout << "True at: " << x_coord[j] << "," << y_coord[j] << std::endl; + // initial_point[0] = x_coord[k]; + // initial_point[1] = y_coord[k]; + // // std::cout << "Initial point: " << initial_point[0] << ", " << initial_point[1] << std::endl; + // initial_pts.first.push_back(initial_point[0]); + // initial_pts.second.push_back(initial_point[1]); + // first = false; + // } + // final_point[0] = x_coord[k]; + // final_point[1] = y_coord[k]; + // ++count_idx; + // } + // } + // final_pts.first.push_back(final_point[0]); + // final_pts.second.push_back(final_point[1]); + + // // From the start of the cell to the end - This is an approximation - From the robot pose + // float distance = calculateDistance(initial_point[0], initial_point[1], final_point[0], final_point[1]); + // dist_carry += distance; + // std::cout << "Distance: " << distance << ", " << dist_carry << std::endl; + // // Before moving into the next cell we can calculate the interal here + // // We have the initial point and the final point so we can perform this calculation + // float probability = calculateProbability(ranges[i]); // This is the right place for calculating this + + // // Distances + // float d1 = calculateDistance(0, 0, initial_point[0], initial_point[1]); + // float d2 = calculateDistance(0, 0, final_point[0], final_point[1]); + + // std::cout << "Distances: " << d1 << ", " << d2 << std::endl; + + // // Only one of them applies - Posibilities + // // Unobserved + // float unobserved = d1 * probability; + // // Occupied + // float occupied = probability * (d2 - d1); // A change on this one is required + // // Get the point, if not maximum add resolution to the initial point + // // I can do a ray tracing with the whole range + // // I have the distance where this is cut so I cand o some mathematical stuff + // // Free + // float free = probability * (5.0 - d2); // Maximum range + + // std::cout << "Unobserved: " << unobserved << std::endl; + // std::cout << "Occupied: " << occupied << std::endl; + // std::cout << "Free: " << free << std::endl; + + // std::cout << "Initial point: " << initial_point[0] << ", " << initial_point[1] << std::endl; // std::cout << "Final point: " << final_point[0] << ", " << final_point[1] << std::endl; - // std::cout << "Probability: " << probability << std::endl; + // std::cout << "Probability: " << probability << std::endl; } // std::cout << "-------- Initial points --------" << std::endl; @@ -329,10 +449,53 @@ void LifelongSlamToolbox::scannerTest() } } +std::vector LifelongSlamToolbox::getCellPosition(std::vector grid_cell, float resolution) +{ + float x = (grid_cell[0] * resolution) + (resolution / 2); + float y = (grid_cell[1] * resolution) + (resolution / 2); + + return {x, y}; +} + + + +std::vector LifelongSlamToolbox::calculateIntersection( + std::vector laser_start, std::vector laser_end, + std::vector cell_start, std::vector cell_end) +{ + // Initial point laser beam - laser_start + // Final point laser beam - laser_end + // Initial point cell - cell_start + // Final point cell - cell_end + + float x1 = laser_start[0]; + float x2 = laser_end[0]; + float x3 = cell_start[0]; + float x4 = cell_end[0]; + + float y1 = laser_start[1]; + float y2 = laser_end[1]; + float y3 = cell_start[1]; + float y4 = cell_end[1]; + + float den = ((x2-x1)*(y4-y3) - (x4-x3)*(y2-y1)); + if (den == 0.0f) + { + // Parallel lines or not intersection at all + return {}; + } + else + { + float x = ((x2*y1 - x1*y2)*(x4 - x3) - (x4*y3 - x3*y4)*(x2-x1)) / den; + float y = ((x2*y1 - x1*y2)*(y4 - y3) - (x4*y3 - x3*y4)*(y2-y1)) / den; + // std::cout << x<< ", " << y << std::endl; + return {x, y}; + } +} float LifelongSlamToolbox::calculateDistance(float x_1, float y_1, float x_2, float y_2) { - /* + /* Calculates the euclidean distance between two points */ float diff_x = x_2 - x_1; @@ -343,7 +506,7 @@ float LifelongSlamToolbox::calculateDistance(float x_1, float y_1, float x_2, fl float LifelongSlamToolbox::calculateProbability(float range) { - /* + /* Calculates the probability of a cell being observed by a given measurement */ float max_range = 5.0f; @@ -358,13 +521,11 @@ float LifelongSlamToolbox::calculateProbability(float range) std::vector LifelongSlamToolbox::getGridPosition(float x, float y, float resolution) { - /* - Maps the distance into grid coordinates + /* + Maps the distance into grid coordinates */ - // int x_cell = ceil((1 / resolution) * x); - // int y_cell = ceil((1 / resolution) * y); - int x_cell = floor((1 / resolution) * x); - int y_cell = floor((1 / resolution) * y); + int x_cell = floor((1 / resolution) * x); + int y_cell = floor((1 / resolution) * y); /** * No matter what I will always get the left bottom corner @@ -374,12 +535,24 @@ std::vector LifelongSlamToolbox::getGridPosition(float x, float y, float re return {x_cell, y_cell}; } +// std::vector LifelongSlamToolbox::transformPoint(std::vector const& robot_pose, float range, float angle) +// { +// /* +// Returns the distance where the laser beam hits something +// Rigid Body Trasnformation from the global to the sensor frame +// */ + +// float x_tf = (range * cos(robot_pose[2]) * cos(angle)) - (range * sin(robot_pose[2]) * sin(angle)) + robot_pose[0]; +// float y_tf = (range * sin(robot_pose[2]) * cos(angle)) + (range * cos(robot_pose[2]) * sin(angle)) + robot_pose[1]; + +// } + std::vector LifelongSlamToolbox::getLaserHit(std::vector const& robot_pose, float range, float angle) { - /* - Returns the distance where the laser beam hits something - Rigid Body Trasnformation from the global to the sensor frame - */ + /* + Returns the distance where the laser beam hits something + Rigid Body Trasnformation from the global to the sensor frame + */ float x_tf = (range * cos(robot_pose[2]) * cos(angle)) - (range * sin(robot_pose[2]) * sin(angle)) + robot_pose[0]; float y_tf = (range * sin(robot_pose[2]) * cos(angle)) + (range * cos(robot_pose[2]) * sin(angle)) + robot_pose[1]; @@ -389,15 +562,15 @@ std::vector LifelongSlamToolbox::getLaserHit(std::vector const& ro std::pair, std::vector> LifelongSlamToolbox::Bresenham(int x_1, int y_1, int x_2, int y_2) { - /* - Returns the set of cells hit by a laser beam + /* + Returns the set of cells hit by a laser beam */ std::vector x_bres; std::vector y_bres; int x = x_1; int y = y_1; - + int delta_x = abs(x_2 - x_1); int delta_y = abs(y_2 - y_1); @@ -409,7 +582,7 @@ std::pair, std::vector> LifelongSlamToolbox::Bresenham(int { int temp = delta_x; delta_x = delta_y; - delta_y = temp; + delta_y = temp; interchange = true; } else @@ -424,7 +597,7 @@ std::pair, std::vector> LifelongSlamToolbox::Bresenham(int x_bres.push_back(x); y_bres.push_back(y); - for (int i = 1; i < delta_x; ++i) + for (int i = 1; i < delta_x; ++i) { if (e_res < 0) { @@ -438,7 +611,7 @@ std::pair, std::vector> LifelongSlamToolbox::Bresenham(int } e_res += a_res; } - else + else { y += s_y; x += s_x; @@ -452,11 +625,11 @@ std::pair, std::vector> LifelongSlamToolbox::Bresenham(int int LifelongSlamToolbox::getSign(int n_1, int n_2) { - /* + /* Returns the sign of an operation, used for Bresenham algorithm */ int difference = n_2 - n_1; - + if (difference == 0) { return 0; } else if (difference < 0) { return -1; } else { return 1; } From 33bc43ce022a49785c25f608cbce8169843ce9d7 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Fri, 5 Nov 2021 12:36:07 -0500 Subject: [PATCH 12/83] [FIX]: Probability function update --- .../experimental/slam_toolbox_lifelong.hpp | 2 +- src/experimental/slam_toolbox_lifelong.cpp | 16 ++++++++++++---- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index 06e362ca3..9fcb26a10 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -73,7 +73,7 @@ class LifelongSlamToolbox : public SlamToolbox std::pair, std::vector> Bresenham(int x_1, int y_1, int x_2, int y_2); std::vector calculateIntersection(std::vector laser_start, std::vector laser_end, std::vector cell_start, std::vector cell_end); int getSign(int n_1, int n_2); - float calculateProbability(float range); + float calculateProbability(float range_1, float range_2); float calculateDistance(float x_1, float y_1, float x_2, float y_2); void scannerTest(); std::vector getCellPosition(std::vector grid_cell, float resolution); diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index ac7023570..d8b9d391a 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -504,19 +504,27 @@ float LifelongSlamToolbox::calculateDistance(float x_1, float y_1, float x_2, fl return sqrt(diff_x*diff_x + diff_y*diff_y); } -float LifelongSlamToolbox::calculateProbability(float range) +float LifelongSlamToolbox::calculateProbability(float range_1, float range_2) { /* Calculates the probability of a cell being observed by a given measurement + This is the new function that we should integrate (If measurement is greater than max range so calculate the whole value) + range_1 lower limit + range_2 upper limit */ float max_range = 5.0f; float lambda = 0.285f; float nu = 1.0f / lambda; - if (range <= max_range) + + // When measurement is greater we need to calculate the whole sumatory after this point it will no add anything + if (range_2 > max_range) // Ternarr would be cleaner { - return nu*lambda*exp(-lambda*range); + range_2 = max_range; } - return 0.0f; + // https://www.wolframalpha.com/input/?i2d=true&i=Integrate%5Bk*Power%5Be%2C-c*x%5D%2C%7Bx%2Ca%2Cb%7D%5D + return (nu * lambda)*((exp(-lambda*range_1) - exp(-lambda*range_2)) / lambda); + + // return 0.0f; } std::vector LifelongSlamToolbox::getGridPosition(float x, float y, float resolution) From a012859ec3717396efe7e89810f2e8f13f392265 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Mon, 8 Nov 2021 09:48:48 -0500 Subject: [PATCH 13/83] [WIP]: Probability calculation is reliable --- src/experimental/slam_toolbox_lifelong.cpp | 327 ++++++--------------- 1 file changed, 95 insertions(+), 232 deletions(-) diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index d8b9d391a..249e790cd 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -154,7 +154,7 @@ void LifelongSlamToolbox::scannerTest() std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; // Creating the laser scan with 5 beams ------- Angles will be -50, -25, 0, 25, 50 //----// +-50 = +-0.87266 : +-25 = +-0.43633 - std::vector ranges{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters + std::vector ranges{2.8f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // std::vector angles{0.785398f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // std::vector angles{0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; @@ -172,7 +172,6 @@ void LifelongSlamToolbox::scannerTest() std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1], resolution); std::cout << final_grid_pos[0] << ", " << final_grid_pos[1] << std::endl; - std::vector cell_pos = getCellPosition({11, 12}, resolution); std::cout << cell_pos[0] << ", " << cell_pos[1] << std::endl; @@ -187,65 +186,40 @@ void LifelongSlamToolbox::scannerTest() cells_y = res_pair.second; // Adding last hit cell to the set - cells_x.push_back(final_grid_pos[0]); // Here make the change + cells_x.push_back(final_grid_pos[0]); cells_y.push_back(final_grid_pos[1]); - // std::cout << "Cells" << std::endl; - // for (int c = 0; c < cells_x.size(); ++c) // One reading only - // { - // std::cout << cells_x[c] << ", " << cells_y[c] << std::endl; - // } - // std::cout << "End of cells" << std::endl; - - // Discretization is no longer required - - // Laser beam discretization - int samples = 300; - float step = ranges[i] / samples; - float current_dis = 0.0f; + std::cout << "Cells" << std::endl; + std::cout << "Robot start: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; - // Vectors for discretization of the laser beam - std::vector x_coord, y_coord; - x_coord.reserve(samples + 1); - y_coord.reserve(samples + 1); - - while(current_dis < (ranges[i] + step)) + for (int c = 0; c < cells_x.size(); ++c) // One reading only { - // Discretization loop - float x_p = current_dis * sin(angles[i]); - float y_p = current_dis * cos(angles[i]); - x_coord.emplace_back(x_p); - y_coord.emplace_back(y_p); - current_dis += step; + std::cout << cells_x[c] << ", " << cells_y[c] << std::endl; } - - - int count_idx = 0; - float dist_carry = 0.0f; + std::cout << "End of cells" << std::endl; std::vector initial_point(2), final_point(2); initial_point[0] = 0.0f; initial_point[1] = 0.0f; - // Initial and final points - std::pair, std::vector> initial_pts, final_pts; - - // initial_pts.first.push_back(initial_point[0]); - // initial_pts.second.push_back(initial_point[1]); - - // Move alongside all the cells that the laser beam hits std::cout << " +++++++++++++ " << std::endl; - + // Visiting the cells this beam travels trough for (int j = 0; j < cells_x.size(); ++j) { - // Cells visutalization + if ((robot_grid_pos[0] == cells_x[j]) && (robot_grid_pos[1] == cells_y[j])) + { + // We do not need to calculate in the first cell + continue; + } + + // Cells visualization std::cout << cells_x[j] << ", " << cells_y[j] << std::endl; // Converting the cell into distance float limit_x = cells_x[j] * resolution; float limit_y = cells_y[j] * resolution; - + std::vector initial_x {limit_x, limit_x, limit_x + resolution, limit_x + resolution}; std::vector initial_y {limit_y, limit_y, limit_y + resolution, limit_y + resolution}; @@ -259,12 +233,10 @@ void LifelongSlamToolbox::scannerTest() if (final_grid_pos[0] < robot_grid_pos[0] && final_grid_pos[1] >= robot_grid_pos[1]) { - std::cout << "1 " << std::endl; - // X minor and Y greater. WRO final points initial_x[2] = limit_x - resolution; initial_x[3] = limit_x - resolution; - + final_x[0] = limit_x - resolution; final_x[2] = limit_x - resolution; @@ -274,8 +246,6 @@ void LifelongSlamToolbox::scannerTest() if (final_grid_pos[0] >= robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) { - std::cout << "2 " << std::endl; - // X greater and Y minor. WRO final points initial_y[2] = limit_y - resolution; initial_y[3] = limit_y - resolution; @@ -289,8 +259,6 @@ void LifelongSlamToolbox::scannerTest() if (final_grid_pos[0] < robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) { - std::cout << "3 " << std::endl; - // X minor and Y minor. WRO final points initial_x[2] = limit_x - resolution; initial_x[3] = limit_x - resolution; @@ -308,189 +276,108 @@ void LifelongSlamToolbox::scannerTest() max_y = limit_y; } + std::vector inter_x, inter_y; for (int k = 0; k < 4; ++k) { - // std::cout << "Points: " << initial_x[k] << ", " << initial_y[k] << ", " << final_x[k] << ", " << final_y[k] << std::endl; + //std::cout << "Points: " << initial_x[k] << ", " << initial_y[k] << ", " << final_x[k] << ", " << final_y[k] << std::endl; std::vector intersection = calculateIntersection(robot_pose, laser_grid, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); if(intersection.size() != 0) { - if ((abs(intersection[0]) >= abs(min_x)) && - (abs(intersection[0]) <= abs(max_x)) && - (abs(intersection[1]) >= abs(min_y)) && - (abs(intersection[1]) <= abs(max_y))) + // If the laser and a cell intersects, we need to make sure it happens in the right bounds + if ((abs(intersection[0]) >= abs(min_x - 0.001f)) && + (abs(intersection[0]) <= abs(max_x + 0.001f)) && + (abs(intersection[1]) >= abs(min_y - 0.001f)) && + (abs(intersection[1]) <= abs(max_y + 0.001f))) { + /* + 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) + */ std::cout << "Interception at: " << intersection[0] << ", " << intersection[1] << std::endl; + inter_x.push_back(intersection[0]); + inter_y.push_back(intersection[1]); } } - else - { - std::cout << "No interception " << std::endl; - } + } + + // Enter (d1) and Exit (d2) distances + std::vector distances; + for (int k = 0; k < inter_x.size(); ++k) + { + /* + dist_point[0]: d1 + dist_point[1]: d2 + */ + float dist_point = calculateDistance(robot_pose[0], robot_pose[1], inter_x[k], inter_y[k]); + distances.push_back(dist_point); + std::cout << "Distance: " << dist_point << std::endl; } - // Current cell limits - // float inf_x = limit_x; - // float sup_x = limit_x + resolution; - // float inf_y = limit_y; - // float sup_y = limit_y + resolution; - - // // // I need to transform that point - // std::vector res = calculateIntersection(robot_pose, laser_grid, {5.5f, 6.5f}, {6.0f, 6.5f}); - - // if(res.size() != 0) - // { - // std::cout << "Intercept" << std::endl; - // std::cout << res[0] << ", " << res[1] << std::endl; - // } - std::cout << " +++++++++++++ " << std::endl; - - // std::cout << limit_x << ", " << limit_y << std::endl; - // std::cout << limit_x + resolution << ", " << limit_y << std::endl; - // std::cout << limit_x << ", " << limit_y + resolution << std::endl; - // std::cout << limit_x + resolution<< ", " << limit_y + resolution << std::endl; - - - // std::cout << "******** New cell ********" << std::endl; - // std::cout << "Starting at: " << count_idx << std::endl; - - // for (int k = count_idx; k < x_coord.size(); ++k) - // { - // // Adding the robot position to the laser beam reading - Shift X and Y - // float read_x = x_coord[k] + robot_pose[0]; - // float read_y = y_coord[k] + robot_pose[1]; - - // float x_tf = (range * cos(robot_pose[2]) * cos(angle)) - (range * sin(robot_pose[2]) * sin(angle)) + robot_pose[0]; - // float y_tf = (range * sin(robot_pose[2]) * cos(angle)) + (range * cos(robot_pose[2]) * sin(angle)) + robot_pose[1]; - - // // std::cout << "Reading x: " << abs(read_x) << ", Reading y: " << abs(read_y) << std::endl; - // std::cout << "Limit X: " << abs(limit_x) - 5.5f << ", " << abs(limit_x + resolution) - 5.5f << std::endl; - // std::cout << "Limit Y: " << abs(limit_y) - 6.0f << ", " << abs(limit_y + resolution) - 6.0f << std::endl; - // std::cout << "Count: " << j << std::endl; - - // if (((abs(read_x) >= abs(limit_x)) && (abs(read_x) <= abs(limit_x + resolution)))) - // { - // // This is working correctly for X - // std::cout << "+++++++++++++ True +++++++++++++" << std::endl; - // } - - // if (((abs(read_y) >= abs(limit_y)) && (abs(read_y) <= abs(limit_y + resolution)))) - // { - // // This is working correctly for Y - // std::cout << "+++++++++++++ True +++++++++++++" << std::endl; - // } - - // Evaluate to what cell corresponds the current reading - // if (((abs(read_x) >= abs(limit_x)) && (abs(read_x) <= abs(limit_x + resolution))) && - // ((abs(read_y) >= abs(limit_y)) && (abs(read_y) <= abs(limit_y + resolution)))) - // { - // if (first) - // { - // // First reading inside a cell will be the initial point - // // std::cout << "True at: " << x_coord[j] << "," << y_coord[j] << std::endl; - // initial_point[0] = x_coord[k]; - // initial_point[1] = y_coord[k]; - // // std::cout << "Initial point: " << initial_point[0] << ", " << initial_point[1] << std::endl; - // initial_pts.first.push_back(initial_point[0]); - // initial_pts.second.push_back(initial_point[1]); - // first = false; - // } - // final_point[0] = x_coord[k]; - // final_point[1] = y_coord[k]; - // ++count_idx; - // } - // } - // final_pts.first.push_back(final_point[0]); - // final_pts.second.push_back(final_point[1]); - - // // From the start of the cell to the end - This is an approximation - From the robot pose - // float distance = calculateDistance(initial_point[0], initial_point[1], final_point[0], final_point[1]); - // dist_carry += distance; - // std::cout << "Distance: " << distance << ", " << dist_carry << std::endl; - // // Before moving into the next cell we can calculate the interal here - // // We have the initial point and the final point so we can perform this calculation - // float probability = calculateProbability(ranges[i]); // This is the right place for calculating this - - // // Distances - // float d1 = calculateDistance(0, 0, initial_point[0], initial_point[1]); - // float d2 = calculateDistance(0, 0, final_point[0], final_point[1]); - - // std::cout << "Distances: " << d1 << ", " << d2 << std::endl; - - // // Only one of them applies - Posibilities - // // Unobserved - // float unobserved = d1 * probability; - // // Occupied - // float occupied = probability * (d2 - d1); // A change on this one is required - // // Get the point, if not maximum add resolution to the initial point - // // I can do a ray tracing with the whole range - // // I have the distance where this is cut so I cand o some mathematical stuff - // // Free - // float free = probability * (5.0 - d2); // Maximum range - - // std::cout << "Unobserved: " << unobserved << std::endl; - // std::cout << "Occupied: " << occupied << std::endl; - // std::cout << "Free: " << free << std::endl; - - // std::cout << "Initial point: " << initial_point[0] << ", " << initial_point[1] << std::endl; - // std::cout << "Final point: " << final_point[0] << ", " << final_point[1] << std::endl; - // std::cout << "Probability: " << probability << std::endl; - } + /* + For last cell I need to modify something + */ - // std::cout << "-------- Initial points --------" << std::endl; + // Integral 1: 0 to d1 which is distance from robot pose to first point where the cell is cut + // Integral 2: d1 which is distance from robot pose to first point where the cell is cut to + // d2 which is distance from robot pose to second point where the cell is cut + // Integral 3: d2 which is distance from robot pose to second point where the cell is cut to z_max - // for (int i = 0; i < initial_pts.first.size(); ++i) - // { - // std::cout << "----------------" << std::endl; - // std::cout << "Initial: " << initial_pts.first[i] << ", " << initial_pts.second[i] << std::endl; - // std::cout << "Final: " << final_pts.first[i] << ", " << final_pts.second[i] << std::endl; + // I think this one is working well + float prob_not = calculateProbability(0.0f, distances[0]); // 3.8 - Does not oberserve + float prob_occ = calculateProbability(distances[0], distances[1]); // 3.9 - Occupied + float prob_free = calculateProbability(distances[1], 5.0f); // 3.10 - Free + std::cout << "Probabilities: " << prob_not << ", " << prob_occ << ", " << prob_free << std::endl; - // } - std::cout << "*************************" << std::endl; + // The entropy of a grid cell C given a set of measurement outcomes z is given by 3.5 + // I need to calculate the entropy of a cell given a set of measurements + + std::cout << " ++++++++++++++++++++++++ " << std::endl; + } } } std::vector LifelongSlamToolbox::getCellPosition(std::vector grid_cell, float resolution) { float x = (grid_cell[0] * resolution) + (resolution / 2); - float y = (grid_cell[1] * resolution) + (resolution / 2); + float y = (grid_cell[1] * resolution) + (resolution / 2); return {x, y}; } - - std::vector LifelongSlamToolbox::calculateIntersection( std::vector laser_start, std::vector laser_end, std::vector cell_start, std::vector cell_end) { - // Initial point laser beam - laser_start - // Final point laser beam - laser_end - // Initial point cell - cell_start - // Final point cell - cell_end - - float x1 = laser_start[0]; - float x2 = laser_end[0]; - float x3 = cell_start[0]; - float x4 = cell_end[0]; - - float y1 = laser_start[1]; - float y2 = laser_end[1]; - float y3 = cell_start[1]; - float y4 = cell_end[1]; - - float den = ((x2-x1)*(y4-y3) - (x4-x3)*(y2-y1)); - if (den == 0.0f) - { - // Parallel lines or not intersection at all - return {}; - } - else - { - float x = ((x2*y1 - x1*y2)*(x4 - x3) - (x4*y3 - x3*y4)*(x2-x1)) / den; - float y = ((x2*y1 - x1*y2)*(y4 - y3) - (x4*y3 - x3*y4)*(y2-y1)) / den; - // std::cout << x<< ", " << y << std::endl; - return {x, y}; - } + /* + Initial point laser beam: laser_start + Final point laser beam: laser_end + Initial point cell: cell_start + Final point cell: cell_end + */ + float x1 = laser_start[0]; + float x2 = laser_end[0]; + float x3 = cell_start[0]; + float x4 = cell_end[0]; + + float y1 = laser_start[1]; + float y2 = laser_end[1]; + float y3 = cell_start[1]; + float y4 = cell_end[1]; + + float den = ((x2-x1)*(y4-y3) - (x4-x3)*(y2-y1)); + if (den == 0.0f) + { + // Parallel lines or not intersection at all + return {}; + } + else + { + float x = ((x2*y1 - x1*y2)*(x4 - x3) - (x4*y3 - x3*y4)*(x2-x1)) / den; + float y = ((x2*y1 - x1*y2)*(y4 - y3) - (x4*y3 - x3*y4)*(y2-y1)) / den; + // std::cout << x<< ", " << y << std::endl; + return {x, y}; + } } float LifelongSlamToolbox::calculateDistance(float x_1, float y_1, float x_2, float y_2) @@ -508,23 +395,16 @@ float LifelongSlamToolbox::calculateProbability(float range_1, float range_2) { /* Calculates the probability of a cell being observed by a given measurement - This is the new function that we should integrate (If measurement is greater than max range so calculate the whole value) - range_1 lower limit - range_2 upper limit + range_1: lower limit, range_2: upper limit */ float max_range = 5.0f; float lambda = 0.285f; float nu = 1.0f / lambda; - // When measurement is greater we need to calculate the whole sumatory after this point it will no add anything - if (range_2 > max_range) // Ternarr would be cleaner - { - range_2 = max_range; - } - // https://www.wolframalpha.com/input/?i2d=true&i=Integrate%5Bk*Power%5Be%2C-c*x%5D%2C%7Bx%2Ca%2Cb%7D%5D - return (nu * lambda)*((exp(-lambda*range_1) - exp(-lambda*range_2)) / lambda); + range_2 = (range_2 > max_range) ? max_range : range_2; - // return 0.0f; + // https://www.wolframalpha.com/input/?i2d=true&i=Integrate%5Bn*c*Power%5Be%2C-c*x%5D%2C%7Bx%2Ca%2Cb%7D%5D + return nu * (exp(-lambda*range_1) - exp(-lambda*range_2)); } std::vector LifelongSlamToolbox::getGridPosition(float x, float y, float resolution) @@ -535,26 +415,9 @@ std::vector LifelongSlamToolbox::getGridPosition(float x, float y, float re int x_cell = floor((1 / resolution) * x); int y_cell = floor((1 / resolution) * y); - /** - * No matter what I will always get the left bottom corner - * */ - - return {x_cell, y_cell}; } -// std::vector LifelongSlamToolbox::transformPoint(std::vector const& robot_pose, float range, float angle) -// { -// /* -// Returns the distance where the laser beam hits something -// Rigid Body Trasnformation from the global to the sensor frame -// */ - -// float x_tf = (range * cos(robot_pose[2]) * cos(angle)) - (range * sin(robot_pose[2]) * sin(angle)) + robot_pose[0]; -// float y_tf = (range * sin(robot_pose[2]) * cos(angle)) + (range * cos(robot_pose[2]) * sin(angle)) + robot_pose[1]; - -// } - std::vector LifelongSlamToolbox::getLaserHit(std::vector const& robot_pose, float range, float angle) { /* From a2c83e0fabb235e0335e4990f396ad8a6710dfb2 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Wed, 10 Nov 2021 09:33:41 -0500 Subject: [PATCH 14/83] [WIP]: Adding inverseModel function to detect cell occupancy, adding functions for updating cell probability and cells logs --- .../experimental/slam_toolbox_lifelong.hpp | 4 + src/experimental/slam_toolbox_lifelong.cpp | 104 ++++++++++++++++-- 2 files changed, 99 insertions(+), 9 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index 9fcb26a10..461802b1a 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -77,6 +77,10 @@ class LifelongSlamToolbox : public SlamToolbox float calculateDistance(float x_1, float y_1, float x_2, float y_2); void scannerTest(); std::vector getCellPosition(std::vector grid_cell, float resolution); + void inverseMeasurement(std::vector>& grid_prob, std::vector& cells_x, std::vector& cells_y, std::vector& robot_grid_pos, float range, float angle, float resolution); + void updateCellProbability(std::vector>& grid_prob, float probability, int cell_x, int cell_y); + void updateCellLogs(); + /*****************************************************************************/ bool use_tree_; diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 249e790cd..bcf0bb582 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -127,18 +127,29 @@ void LifelongSlamToolbox::scannerTest() float resolution = 0.5f; // Cell resolution - 1 meter float map_dist = 20.0f; // Total map distance - int number_cells = map_dist / resolution; + + float unknown_prob = 0.5f; + float initial_log = log(unknown_prob / (1.0f - unknown_prob)); std::vector> grid(number_cells); // 20 meters in Y dimension + std::vector> grid_prob(number_cells); // 20 meters in Y dimension + std::vector> grid_logs(number_cells); // 20 meters in Y dimension + for (int i=0; i robot_pose{5.6f, 6.0f, PI/2}; // std::vector robot_pose{5.6f, 6.0f, PI/4}; @@ -154,8 +165,11 @@ void LifelongSlamToolbox::scannerTest() std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; // Creating the laser scan with 5 beams ------- Angles will be -50, -25, 0, 25, 50 //----// +-50 = +-0.87266 : +-25 = +-0.43633 - std::vector ranges{2.8f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters - std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + std::vector ranges{1.65f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters + // std::vector ranges{2.8f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters + + std::vector angles{0.0f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + // std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // std::vector angles{0.785398f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // std::vector angles{0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; @@ -168,6 +182,7 @@ void LifelongSlamToolbox::scannerTest() // Laser continuous distance std::vector laser_grid = getLaserHit(robot_pose, ranges[i], angles[i]); + std::cout << laser_grid[0] << "----- , ----- " << laser_grid[1] << std::endl; // Laser final cell std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1], resolution); std::cout << final_grid_pos[0] << ", " << final_grid_pos[1] << std::endl; @@ -190,14 +205,19 @@ void LifelongSlamToolbox::scannerTest() cells_y.push_back(final_grid_pos[1]); std::cout << "Cells" << std::endl; - std::cout << "Robot start: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; - for (int c = 0; c < cells_x.size(); ++c) // One reading only { std::cout << cells_x[c] << ", " << cells_y[c] << std::endl; } std::cout << "End of cells" << std::endl; + + inverseMeasurement(grid_prob, cells_x, cells_y, robot_grid_pos, ranges[i], angles[i], resolution); + + // std::cout << "Probability: " << grid_prob[9][8] << std::endl; + // std::cout << "Probability: " << grid_prob[11][13] << std::endl; + // std::cout << "Probability: " << grid_prob[11][15] << std::endl; + std::vector initial_point(2), final_point(2); initial_point[0] = 0.0f; @@ -311,6 +331,7 @@ void LifelongSlamToolbox::scannerTest() */ float dist_point = calculateDistance(robot_pose[0], robot_pose[1], inter_x[k], inter_y[k]); distances.push_back(dist_point); + std::cout << "Distance: " << dist_point << std::endl; } @@ -328,15 +349,81 @@ void LifelongSlamToolbox::scannerTest() float prob_occ = calculateProbability(distances[0], distances[1]); // 3.9 - Occupied float prob_free = calculateProbability(distances[1], 5.0f); // 3.10 - Free std::cout << "Probabilities: " << prob_not << ", " << prob_occ << ", " << prob_free << std::endl; - + + // The entropy of a grid cell C given a set of measurement outcomes z is given by 3.5 // I need to calculate the entropy of a cell given a set of measurements + // Inverse model + // Probabiloityu + // Entropy std::cout << " ++++++++++++++++++++++++ " << std::endl; } } } + +void LifelongSlamToolbox::inverseMeasurement( + std::vector>& grid_prob, + std::vector& cells_x, + std::vector& cells_y, + std::vector& robot_grid_pos, + float range, float angle, float resolution) +{ + /* + On this function we update a probability map based + We capture the cells that are hitted by the ray and the conditions + And we update the probability as being occupied or free + */ + + std::cout << "Inverse scanner" << std::endl; + float alpha = 1.0f; + float max_r = 5.0f; + + for (int i = 0; i < cells_x.size(); ++i) // One reading only + { + // This ditances will be calculated to the center of mass + // mxi - pos_x //--// myi - pos_y + float dx = (cells_x[i] - robot_grid_pos[0]) * resolution; + float dy = (cells_y[i] - robot_grid_pos[1]) * resolution; + + float r = sqrt(pow(dx , 2) + pow(dy, 2)); + float phi = atan2(dy, dx) - robot_grid_pos[2]; + + // Cell is occupied -- This condition might be different + if ((range < max_r) && (abs(r - range) < (alpha / 2.0f))) + { + std::cout << "Occupied" << std::endl; + updateCellProbability(grid_prob, 0.7f, cells_x[i], cells_y[i]); + } + // Cell is free + else if (r <= range) + { + std::cout << "Free" << std::endl; + updateCellProbability(grid_prob, 0.3f, cells_x[i], cells_y[i]); + } + + std::cout << "Cells: " << cells_x[i] << ", " << cells_y[i] << std::endl; + std::cout << "Relative range: " << r << ", Angle: " << phi << std::endl; + + } +} +// Also need and update grid function + +void LifelongSlamToolbox::updateCellProbability(std::vector>& grid_prob, float probability, int cell_x, int cell_y) +{ + /* + To perform the probability update at the given cell + */ + grid_prob[cell_x][cell_x] = probability; +} + + +void LifelongSlamToolbox::updateLogs(initial logs, ) +{ + +} + std::vector LifelongSlamToolbox::getCellPosition(std::vector grid_cell, float resolution) { float x = (grid_cell[0] * resolution) + (resolution / 2); @@ -424,9 +511,8 @@ std::vector LifelongSlamToolbox::getLaserHit(std::vector const& ro Returns the distance where the laser beam hits something Rigid Body Trasnformation from the global to the sensor frame */ - - float x_tf = (range * cos(robot_pose[2]) * cos(angle)) - (range * sin(robot_pose[2]) * sin(angle)) + robot_pose[0]; - float y_tf = (range * sin(robot_pose[2]) * cos(angle)) + (range * cos(robot_pose[2]) * sin(angle)) + robot_pose[1]; + float x_tf = (range * cos(robot_pose[2] + angle)) + robot_pose[0]; + float y_tf = (range * sin(robot_pose[2] + angle)) + robot_pose[1]; return {x_tf, y_tf}; } From d6f6db1ecaafc9c4aaa5dbc2b9a721c3fddcbc3d Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Wed, 10 Nov 2021 10:37:08 -0500 Subject: [PATCH 15/83] [WIP]: Logs calculation --- .../experimental/slam_toolbox_lifelong.hpp | 5 +- src/experimental/slam_toolbox_lifelong.cpp | 46 ++++++++++++++----- 2 files changed, 37 insertions(+), 14 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index 461802b1a..b44dda6ae 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -77,9 +77,10 @@ class LifelongSlamToolbox : public SlamToolbox float calculateDistance(float x_1, float y_1, float x_2, float y_2); void scannerTest(); std::vector getCellPosition(std::vector grid_cell, float resolution); - void inverseMeasurement(std::vector>& grid_prob, std::vector& cells_x, std::vector& cells_y, std::vector& robot_grid_pos, float range, float angle, float resolution); + void inverseMeasurement(std::vector>& grid_prob, std::vector>& grid_logs, std::vector& cells_x, std::vector& cells_y, std::vector& robot_grid_pos, float range, float angle, float resolution); void updateCellProbability(std::vector>& grid_prob, float probability, int cell_x, int cell_y); - void updateCellLogs(); + float calculateLogs(float probability); + void updateCellLogs(std::vector>& grid_prob, std::vector>& grid_logs, int cell_x, int cell_y, float initial_log); /*****************************************************************************/ diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index bcf0bb582..231d1e64e 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -130,7 +130,7 @@ void LifelongSlamToolbox::scannerTest() int number_cells = map_dist / resolution; float unknown_prob = 0.5f; - float initial_log = log(unknown_prob / (1.0f - unknown_prob)); + float initial_log = calculateLogs(unknown_prob); std::vector> grid(number_cells); // 20 meters in Y dimension std::vector> grid_prob(number_cells); // 20 meters in Y dimension std::vector> grid_logs(number_cells); // 20 meters in Y dimension @@ -182,7 +182,6 @@ void LifelongSlamToolbox::scannerTest() // Laser continuous distance std::vector laser_grid = getLaserHit(robot_pose, ranges[i], angles[i]); - std::cout << laser_grid[0] << "----- , ----- " << laser_grid[1] << std::endl; // Laser final cell std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1], resolution); std::cout << final_grid_pos[0] << ", " << final_grid_pos[1] << std::endl; @@ -211,10 +210,15 @@ void LifelongSlamToolbox::scannerTest() } std::cout << "End of cells" << std::endl; + std::cout << " ...---...---...---...---...---...--- " << std::endl; - inverseMeasurement(grid_prob, cells_x, cells_y, robot_grid_pos, ranges[i], angles[i], resolution); + inverseMeasurement(grid_prob, grid_logs, cells_x, cells_y, robot_grid_pos, ranges[i], angles[i], resolution); + + std::cout << " ...---...---...---...---...---...--- " << std::endl; + + std::cout << "Probability: " << grid_prob[9][8] << std::endl; + std::cout << "Logs: " << grid_logs[9][8] << std::endl; - // std::cout << "Probability: " << grid_prob[9][8] << std::endl; // std::cout << "Probability: " << grid_prob[11][13] << std::endl; // std::cout << "Probability: " << grid_prob[11][15] << std::endl; @@ -365,6 +369,7 @@ void LifelongSlamToolbox::scannerTest() void LifelongSlamToolbox::inverseMeasurement( std::vector>& grid_prob, + std::vector>& grid_logs, std::vector& cells_x, std::vector& cells_y, std::vector& robot_grid_pos, @@ -376,12 +381,12 @@ void LifelongSlamToolbox::inverseMeasurement( And we update the probability as being occupied or free */ - std::cout << "Inverse scanner" << std::endl; float alpha = 1.0f; float max_r = 5.0f; for (int i = 0; i < cells_x.size(); ++i) // One reading only { + std::cout << "Cells: " << cells_x[i] << ", " << cells_y[i] << std::endl; // This ditances will be calculated to the center of mass // mxi - pos_x //--// myi - pos_y float dx = (cells_x[i] - robot_grid_pos[0]) * resolution; @@ -394,34 +399,51 @@ void LifelongSlamToolbox::inverseMeasurement( if ((range < max_r) && (abs(r - range) < (alpha / 2.0f))) { std::cout << "Occupied" << std::endl; + // Update the probability updateCellProbability(grid_prob, 0.7f, cells_x[i], cells_y[i]); + // Update the log-odds + updateCellLogs(grid_prob, grid_logs, cells_x[i], cells_y[i], 0.0f); } // Cell is free else if (r <= range) { std::cout << "Free" << std::endl; + // Update the probability updateCellProbability(grid_prob, 0.3f, cells_x[i], cells_y[i]); + // Update the log-odds + updateCellLogs(grid_prob, grid_logs, cells_x[i], cells_y[i], 0.0f); } - std::cout << "Cells: " << cells_x[i] << ", " << cells_y[i] << std::endl; std::cout << "Relative range: " << r << ", Angle: " << phi << std::endl; - } } -// Also need and update grid function + +void LifelongSlamToolbox::updateCellLogs(std::vector>& grid_prob, std::vector>& grid_logs, int cell_x, int cell_y, float initial_log) +{ + /* + To update the log-odds matrix + */ + grid_logs[cell_x][cell_y] = grid_logs[cell_x][cell_y] + calculateLogs(grid_prob[cell_x][cell_y]) - initial_log; + // std::cout << "Cell Logs: " << grid_logs[cell_x][cell_y] << std::endl; + std::cout << "Cell Logs: " << calculateLogs(grid_prob[cell_x][cell_y]) << std::endl; +} void LifelongSlamToolbox::updateCellProbability(std::vector>& grid_prob, float probability, int cell_x, int cell_y) { /* To perform the probability update at the given cell */ - grid_prob[cell_x][cell_x] = probability; + grid_prob[cell_x][cell_y] = probability; + std::cout << "Cell probability: " << grid_prob[cell_x][cell_y] << std::endl; } - -void LifelongSlamToolbox::updateLogs(initial logs, ) +float LifelongSlamToolbox::calculateLogs(float probability) { - + /* + To calculate the log-odds + */ + // This is natural algorithm + return log(probability / (1 - probability)); } std::vector LifelongSlamToolbox::getCellPosition(std::vector grid_cell, float resolution) From f294d0d5489e39a12d3b18f2d76471890e3af409 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Wed, 10 Nov 2021 11:00:57 -0500 Subject: [PATCH 16/83] [WIP]: Getting probability from log-odds --- .../experimental/slam_toolbox_lifelong.hpp | 3 +- src/experimental/slam_toolbox_lifelong.cpp | 37 ++++++++++++++++++- 2 files changed, 37 insertions(+), 3 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index b44dda6ae..f1b8817b7 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -81,7 +81,8 @@ class LifelongSlamToolbox : public SlamToolbox void updateCellProbability(std::vector>& grid_prob, float probability, int cell_x, int cell_y); float calculateLogs(float probability); void updateCellLogs(std::vector>& grid_prob, std::vector>& grid_logs, int cell_x, int cell_y, float initial_log); - + float logToProbability(float log); + void calculateCellEntropy(); /*****************************************************************************/ bool use_tree_; diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 231d1e64e..261fadf21 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -394,6 +394,9 @@ void LifelongSlamToolbox::inverseMeasurement( float r = sqrt(pow(dx , 2) + pow(dy, 2)); float phi = atan2(dy, dx) - robot_grid_pos[2]; + + + // I can assing only one value and calculate it once (Half the code) // Cell is occupied -- This condition might be different if ((range < max_r) && (abs(r - range) < (alpha / 2.0f))) @@ -403,6 +406,8 @@ void LifelongSlamToolbox::inverseMeasurement( updateCellProbability(grid_prob, 0.7f, cells_x[i], cells_y[i]); // Update the log-odds updateCellLogs(grid_prob, grid_logs, cells_x[i], cells_y[i], 0.0f); + // Log-odds to probability + float prob = logToProbability(grid_logs[cells_x[i]][cells_y[i]]); } // Cell is free else if (r <= range) @@ -412,12 +417,38 @@ void LifelongSlamToolbox::inverseMeasurement( updateCellProbability(grid_prob, 0.3f, cells_x[i], cells_y[i]); // Update the log-odds updateCellLogs(grid_prob, grid_logs, cells_x[i], cells_y[i], 0.0f); + // Log-odds to probability + float prob = logToProbability(grid_logs[cells_x[i]][cells_y[i]]); } std::cout << "Relative range: " << r << ", Angle: " << phi << std::endl; } } + +/* + Next steps + Done - log-odds will be acumulating the probability based on different observations - Bayes filter + Transform the log-odds into probability + Calculate the entropy of a given cell +*/ + +void LifelongSlamToolbox::calculateCellEntropy() +{ + // Need for this one a matrix with all the entropies + // Need to define where this calculation will take place + ; +} + + +float LifelongSlamToolbox::logToProbability(float log) +{ + /* + To transform the Log-odds into probability + */ + return (exp(log) / (1 + exp(log))); +} + void LifelongSlamToolbox::updateCellLogs(std::vector>& grid_prob, std::vector>& grid_logs, int cell_x, int cell_y, float initial_log) { /* @@ -425,7 +456,6 @@ void LifelongSlamToolbox::updateCellLogs(std::vector>& grid_p */ grid_logs[cell_x][cell_y] = grid_logs[cell_x][cell_y] + calculateLogs(grid_prob[cell_x][cell_y]) - initial_log; // std::cout << "Cell Logs: " << grid_logs[cell_x][cell_y] << std::endl; - std::cout << "Cell Logs: " << calculateLogs(grid_prob[cell_x][cell_y]) << std::endl; } void LifelongSlamToolbox::updateCellProbability(std::vector>& grid_prob, float probability, int cell_x, int cell_y) @@ -434,7 +464,7 @@ void LifelongSlamToolbox::updateCellProbability(std::vector>& To perform the probability update at the given cell */ grid_prob[cell_x][cell_y] = probability; - std::cout << "Cell probability: " << grid_prob[cell_x][cell_y] << std::endl; + // std::cout << "Cell probability: " << grid_prob[cell_x][cell_y] << std::endl; } float LifelongSlamToolbox::calculateLogs(float probability) @@ -448,6 +478,9 @@ float LifelongSlamToolbox::calculateLogs(float probability) std::vector LifelongSlamToolbox::getCellPosition(std::vector grid_cell, float resolution) { + /* + To get the current cell + */ float x = (grid_cell[0] * resolution) + (resolution / 2); float y = (grid_cell[1] * resolution) + (resolution / 2); From c08fd6990682670aa08175ff7fe5248bdbded050 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Thu, 11 Nov 2021 09:24:49 -0500 Subject: [PATCH 17/83] [WIP]: Adding map entropy calculation --- .../experimental/slam_toolbox_lifelong.hpp | 8 +- src/experimental/slam_toolbox_lifelong.cpp | 114 +++++++++++++----- 2 files changed, 89 insertions(+), 33 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index f1b8817b7..d28c7dc41 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -77,12 +77,14 @@ class LifelongSlamToolbox : public SlamToolbox float calculateDistance(float x_1, float y_1, float x_2, float y_2); void scannerTest(); std::vector getCellPosition(std::vector grid_cell, float resolution); - void inverseMeasurement(std::vector>& grid_prob, std::vector>& grid_logs, std::vector& cells_x, std::vector& cells_y, std::vector& robot_grid_pos, float range, float angle, float resolution); + void inverseMeasurement(std::vector>& grid_prob, std::vector>& grid_logs, std::vector>& grid_etp, std::vector& cells_x, std::vector& cells_y, std::vector& robot_grid_pos, float range, float angle, float resolution); void updateCellProbability(std::vector>& grid_prob, float probability, int cell_x, int cell_y); float calculateLogs(float probability); void updateCellLogs(std::vector>& grid_prob, std::vector>& grid_logs, int cell_x, int cell_y, float initial_log); - float logToProbability(float log); - void calculateCellEntropy(); + float probabilityFromLog(float log); // Recovering the probability + float entropyFromProbability(float prob); + void updateCellEntropy(std::vector>& grid_etp, float cell_x, float cell_y, float entropy); + float calculateMapEntropy(std::vector>& grid_etp); /*****************************************************************************/ bool use_tree_; diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 261fadf21..f0ebf1e23 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -134,21 +134,28 @@ void LifelongSlamToolbox::scannerTest() std::vector> grid(number_cells); // 20 meters in Y dimension std::vector> grid_prob(number_cells); // 20 meters in Y dimension std::vector> grid_logs(number_cells); // 20 meters in Y dimension + std::vector> grid_etp(number_cells); // 20 meters in Y dimension + for (int i=0; i robot_pose{5.6f, 6.0f, PI/2}; @@ -203,21 +210,23 @@ void LifelongSlamToolbox::scannerTest() cells_x.push_back(final_grid_pos[0]); cells_y.push_back(final_grid_pos[1]); - std::cout << "Cells" << std::endl; - for (int c = 0; c < cells_x.size(); ++c) // One reading only - { - std::cout << cells_x[c] << ", " << cells_y[c] << std::endl; - } - std::cout << "End of cells" << std::endl; + // std::cout << "Cells" << std::endl; + // for (int c = 0; c < cells_x.size(); ++c) // One reading only + // { + // std::cout << cells_x[c] << ", " << cells_y[c] << std::endl; + // } + // std::cout << "End of cells" << std::endl; std::cout << " ...---...---...---...---...---...--- " << std::endl; - inverseMeasurement(grid_prob, grid_logs, cells_x, cells_y, robot_grid_pos, ranges[i], angles[i], resolution); + inverseMeasurement(grid_prob, grid_logs, grid_etp, cells_x, cells_y, robot_grid_pos, ranges[i], angles[i], resolution); + std::cout << "Map entropy" << std::endl; + float etp = calculateMapEntropy(grid_etp); std::cout << " ...---...---...---...---...---...--- " << std::endl; - std::cout << "Probability: " << grid_prob[9][8] << std::endl; - std::cout << "Logs: " << grid_logs[9][8] << std::endl; + // std::cout << "Probability: " << grid_prob[9][8] << std::endl; + // std::cout << "Logs: " << grid_logs[9][8] << std::endl; // std::cout << "Probability: " << grid_prob[11][13] << std::endl; // std::cout << "Probability: " << grid_prob[11][15] << std::endl; @@ -366,10 +375,25 @@ void LifelongSlamToolbox::scannerTest() } } +float LifelongSlamToolbox::calculateMapEntropy(std::vector>& grid_etp) +{ + /* + To calculate the map entropy + */ + float map_etp = 0.0f; + for (auto vct : grid_etp) + { + std::for_each(vct.begin(), vct.end(), [&] (float entropy) { + map_etp += entropy; + }); + } + return map_etp; +} void LifelongSlamToolbox::inverseMeasurement( std::vector>& grid_prob, std::vector>& grid_logs, + std::vector>& grid_etp, std::vector& cells_x, std::vector& cells_y, std::vector& robot_grid_pos, @@ -392,36 +416,59 @@ void LifelongSlamToolbox::inverseMeasurement( float dx = (cells_x[i] - robot_grid_pos[0]) * resolution; float dy = (cells_y[i] - robot_grid_pos[1]) * resolution; + // std::cout << "dx: " << dx << ", dy: " << dy << std::endl; + float r = sqrt(pow(dx , 2) + pow(dy, 2)); - float phi = atan2(dy, dx) - robot_grid_pos[2]; - - - // I can assing only one value and calculate it once (Half the code) - + // float phi = atan2(dy, dx) - robot_grid_pos[2]; + float occ_prob = 0.5f; // Unkwnown + // Cell is occupied -- This condition might be different if ((range < max_r) && (abs(r - range) < (alpha / 2.0f))) { std::cout << "Occupied" << std::endl; + occ_prob = 0.7f; // Update the probability - updateCellProbability(grid_prob, 0.7f, cells_x[i], cells_y[i]); + // updateCellProbability(grid_prob, 0.7f, cells_x[i], cells_y[i]); // Update the log-odds - updateCellLogs(grid_prob, grid_logs, cells_x[i], cells_y[i], 0.0f); + // updateCellLogs(grid_prob, grid_logs, cells_x[i], cells_y[i], 0.0f); // Log-odds to probability - float prob = logToProbability(grid_logs[cells_x[i]][cells_y[i]]); + // float prob = probabilityFromLog(grid_logs[cells_x[i]][cells_y[i]]); } // Cell is free else if (r <= range) { std::cout << "Free" << std::endl; + occ_prob = 0.3f; // Update the probability - updateCellProbability(grid_prob, 0.3f, cells_x[i], cells_y[i]); + // updateCellProbability(grid_prob, 0.3f, cells_x[i], cells_y[i]); // Update the log-odds - updateCellLogs(grid_prob, grid_logs, cells_x[i], cells_y[i], 0.0f); + // updateCellLogs(grid_prob, grid_logs, cells_x[i], cells_y[i], 0.0f); // Log-odds to probability - float prob = logToProbability(grid_logs[cells_x[i]][cells_y[i]]); + // float prob = probabilityFromLog(grid_logs[cells_x[i]][cells_y[i]]); + // float entropy = entropyFromProbability(probabilityFromLog(grid_logs[cells_x[i]][cells_y[i]])); + // updateCellEntropy(grid_etp, cells_x[i], cells_y[i], entropy); } - std::cout << "Relative range: " << r << ", Angle: " << phi << std::endl; + // Update the probability + updateCellProbability(grid_prob, occ_prob, cells_x[i], cells_y[i]); + // std::cout << "Probability: " << grid_prob[cells_x[i]][cells_y[i]] << std::endl; + // Update the log-odds + updateCellLogs(grid_prob, grid_logs, cells_x[i], cells_y[i], 0.0f); + // std::cout << "Log-Odds: " << grid_logs[cells_x[i]][cells_y[i]] << std::endl; + // Log-odds to probability + float entropy = entropyFromProbability(probabilityFromLog(grid_logs[cells_x[i]][cells_y[i]])); + // Update the entropy + updateCellEntropy(grid_etp, cells_x[i], cells_y[i], entropy); + // std::cout << "Entropy: " << grid_etp[cells_x[i]][cells_y[i]] << std::endl; + + // std::cout << "Probabilities---------------: " << grid_prob[cells_x[i]][cells_y[i]] << std::endl; + // std::cout << "Logs---------------: " << grid_logs[cells_x[i]][cells_y[i]] << std::endl; + // std::cout << "Log to Prob---------------: " << probabilityFromLog(grid_logs[cells_x[i]][cells_y[i]]) << std::endl; + // std::cout << "Entropies---------------: " << grid_etp[cells_x[i]][cells_y[i]] << std::endl; + + // std::cout << "Relative range: " << r << ", Angle: " << phi << std::endl; + std::cout << "Relative range: " << r << std::endl; + std::cout << " ++++++++++++++++++++++++ " << std::endl; } } @@ -433,15 +480,25 @@ void LifelongSlamToolbox::inverseMeasurement( Calculate the entropy of a given cell */ -void LifelongSlamToolbox::calculateCellEntropy() +void LifelongSlamToolbox::updateCellEntropy(std::vector>& grid_etp, float cell_x, float cell_y, float entropy) { - // Need for this one a matrix with all the entropies - // Need to define where this calculation will take place - ; + /* + To update the cell entropy + - H = H - Entropy; + */ + grid_etp[cell_x][cell_y] = grid_etp[cell_x][cell_y] - entropy; } +float LifelongSlamToolbox::entropyFromProbability(float prob) +{ + /* + To calculate the cell entropy from the probability + - Entropy = (p*log(p) + (1-p)*log(1-p)); + */ + return (prob * log(prob)) * ((1 - prob) * log(1 - prob)); +} -float LifelongSlamToolbox::logToProbability(float log) +float LifelongSlamToolbox::probabilityFromLog(float log) { /* To transform the Log-odds into probability @@ -455,7 +512,6 @@ void LifelongSlamToolbox::updateCellLogs(std::vector>& grid_p To update the log-odds matrix */ grid_logs[cell_x][cell_y] = grid_logs[cell_x][cell_y] + calculateLogs(grid_prob[cell_x][cell_y]) - initial_log; - // std::cout << "Cell Logs: " << grid_logs[cell_x][cell_y] << std::endl; } void LifelongSlamToolbox::updateCellProbability(std::vector>& grid_prob, float probability, int cell_x, int cell_y) @@ -464,7 +520,6 @@ void LifelongSlamToolbox::updateCellProbability(std::vector>& To perform the probability update at the given cell */ grid_prob[cell_x][cell_y] = probability; - // std::cout << "Cell probability: " << grid_prob[cell_x][cell_y] << std::endl; } float LifelongSlamToolbox::calculateLogs(float probability) @@ -472,7 +527,6 @@ float LifelongSlamToolbox::calculateLogs(float probability) /* To calculate the log-odds */ - // This is natural algorithm return log(probability / (1 - probability)); } From e66435c31651ef6f13521f68ecca85d936baa3ae Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Mon, 22 Nov 2021 11:35:15 -0500 Subject: [PATCH 18/83] [ADD]: Adding algorithm 1 base, it is in adaptation process --- .../experimental/slam_toolbox_lifelong.hpp | 12 + src/experimental/slam_toolbox_lifelong.cpp | 337 ++++++++++++++---- 2 files changed, 285 insertions(+), 64 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index d28c7dc41..0dc9d03be 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -85,6 +85,18 @@ class LifelongSlamToolbox : public SlamToolbox float entropyFromProbability(float prob); void updateCellEntropy(std::vector>& grid_etp, float cell_x, float cell_y, float entropy); float calculateMapEntropy(std::vector>& grid_etp); + + // For algorithm 1 + void appendCellProbabilities(std::vector cell, std::vector& probabilities); + void computeProbabilities(std::vector cell, std::vector>& meas_outcm); + void retreiveProbabilities(); + std::vector unhashIndex(int hash); + + // Data structures + // Keep track of the probabilities for each cell + std::map, std::vector>> m_cell_probabilities; + int m_cell_x; + int m_cell_y; /*****************************************************************************/ bool use_tree_; diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index f0ebf1e23..4bd6412cf 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -125,7 +125,7 @@ void LifelongSlamToolbox::scannerTest() { RCLCPP_WARN(get_logger(), "<-------- Scanner Test -------->"); - float resolution = 0.5f; // Cell resolution - 1 meter + float resolution = 0.5f; // Cell resolution - 0.5m float map_dist = 20.0f; // Total map distance int number_cells = map_dist / resolution; @@ -139,7 +139,7 @@ void LifelongSlamToolbox::scannerTest() for (int i=0; i ranges{1.65f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters // std::vector ranges{2.8f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters - std::vector angles{0.0f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + std::vector angles{0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // std::vector angles{0.785398f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // std::vector angles{0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // Current yaw + beam angle: -PI/2 (-1.570795) -0.87266 = 2.44345 (-55 degrees) // for (int i = 0; i < ranges.size(); ++i) - for (int i = 0; i < 1; ++i) // One reading only + for (int i = 0; i < 2; ++i) // One reading only { std::cout << "........ New laser ........" << std::endl; std::cout << "Distance: " << ranges[i] << ", Angle: " << angles[i] << std::endl; @@ -193,9 +193,6 @@ void LifelongSlamToolbox::scannerTest() std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1], resolution); std::cout << final_grid_pos[0] << ", " << final_grid_pos[1] << std::endl; - std::vector cell_pos = getCellPosition({11, 12}, resolution); - std::cout << cell_pos[0] << ", " << cell_pos[1] << std::endl; - // robot_grid_pos[0] // X1 - robot_grid_pos[1] // Y1 // final_grid_pos[0] // X2 - final_grid_pos[1] // Y2 @@ -217,19 +214,15 @@ void LifelongSlamToolbox::scannerTest() // } // std::cout << "End of cells" << std::endl; - std::cout << " ...---...---...---...---...---...--- " << std::endl; - - inverseMeasurement(grid_prob, grid_logs, grid_etp, cells_x, cells_y, robot_grid_pos, ranges[i], angles[i], resolution); - std::cout << "Map entropy" << std::endl; - float etp = calculateMapEntropy(grid_etp); - std::cout << " ...---...---...---...---...---...--- " << std::endl; + // std::cout << " ...---...---...---...---...---...--- " << std::endl; + // For now this will not be used + // Cell probability, log-odds and entropy calculation + // inverseMeasurement(grid_prob, grid_logs, grid_etp, cells_x, cells_y, robot_grid_pos, ranges[i], angles[i], resolution); + // Map entropy calculation + // float etp = calculateMapEntropy(grid_etp); + // std::cout << " ...---...---...---...---...---...--- " << std::endl; - // std::cout << "Probability: " << grid_prob[9][8] << std::endl; - // std::cout << "Logs: " << grid_logs[9][8] << std::endl; - - // std::cout << "Probability: " << grid_prob[11][13] << std::endl; - // std::cout << "Probability: " << grid_prob[11][15] << std::endl; std::vector initial_point(2), final_point(2); @@ -237,7 +230,8 @@ void LifelongSlamToolbox::scannerTest() initial_point[1] = 0.0f; std::cout << " +++++++++++++ " << std::endl; - // Visiting the cells this beam travels trough + + // Start of the loop for for (int j = 0; j < cells_x.size(); ++j) { if ((robot_grid_pos[0] == cells_x[j]) && (robot_grid_pos[1] == cells_y[j])) @@ -358,23 +352,97 @@ void LifelongSlamToolbox::scannerTest() // Integral 3: d2 which is distance from robot pose to second point where the cell is cut to z_max // I think this one is working well - float prob_not = calculateProbability(0.0f, distances[0]); // 3.8 - Does not oberserve - float prob_occ = calculateProbability(distances[0], distances[1]); // 3.9 - Occupied - float prob_free = calculateProbability(distances[1], 5.0f); // 3.10 - Free - std::cout << "Probabilities: " << prob_not << ", " << prob_occ << ", " << prob_free << std::endl; + // float prob_not = calculateProbability(0.0f, distances[0]); // 3.8 - Does not oberserve + // float prob_occ = calculateProbability(distances[0], distances[1]); // 3.9 - Occupied + // float prob_free = calculateProbability(distances[1], 5.0f); // 3.10 - Free + + std::vector probabilities { + calculateProbability(distances[1], 5.0f), + calculateProbability(distances[0], distances[1]), + calculateProbability(0.0f, distances[0]) + }; + + + // Need to check here the cell reference. I need to pass the cells in a better way. Hate to create unnecessary copies + // Here is where I need to extract the probabilities of each cell and pass both the cell and the probabilities as argument + // Cell would be better if just keep two integers and reassign them + + appendCellProbabilities({cells_x[j], cells_y[j]}, probabilities); + + /* + We have all the cells appended + We check all the cells we have appended + */ + + + // computeProbabilities({cells_x[j], cells_y[j]}, meas_outcm); + + /* + On this point I have the probabilities of a cell being observed - - // The entropy of a grid cell C given a set of measurement outcomes z is given by 3.5 - // I need to calculate the entropy of a cell given a set of measurements + As soon as I calculate the probabilities of a cell I need to append the probabilities into my data structure + + Then another external function will be in charge of calculating the combined probabilities of an specific cell + with this function computeProbabilities + + Probabilities calculation is done for a specific cell, so we read the data for one cell and propagate the probabilities. + + I need to investigate if the function for extracting the measurement outcomes for a specific cell is required + */ - // Inverse model - // Probabiloityu - // Entropy + std::cout << "Probabilities: " << probabilities[0] << ", " << probabilities[1] << ", " << probabilities[2] << std::endl; std::cout << " ++++++++++++++++++++++++ " << std::endl; } } } +void LifelongSlamToolbox::retreiveProbabilities() +{ + // Iterator for getting the cell + std::map, std::vector>>::iterator it_cells; + + it_cells = m_cell_probabilities.find({m_cell_x, m_cell_y}); + + + if (it_cells != m_cell_probabilities.end()) + { + std::cout << "Current cell: " << it_cells->first[0] << ", " << it_cells->first[1] << std::endl; + + // Exploring the measurement outcomes for the specific cell + for (int i = 0; i < it_cells->second.size(); ++i) + { + std::cout << "Measurement outcomes: "; + std::cout << it_cells->second[i][0] << ", " << it_cells->second[i][1] << ", " << it_cells->second[i][2] << std::endl; + } + } +} + +void LifelongSlamToolbox::appendCellProbabilities(std::vector cell, std::vector& probabilities) +{ + /* + To append a new measurement outcome for a specific cell + */ + + // Iterator for getting the cell + std::map, std::vector>>::iterator it_cell; + + it_cell = m_cell_probabilities.find({cell[0], cell[1]}); + + if (it_cell == m_cell_probabilities.end()) + { + // Cell is not present in the map + m_cell_probabilities.insert(std::pair, std::vector>>( + {cell[0], cell[1]}, + {{probabilities[0], probabilities[1], probabilities[2]}} + )); + } + else + { + // Cell is already in the map, only add the next measurement outcome + it_cell->second.push_back({probabilities[0], probabilities[1], probabilities[2]}); + } +} + float LifelongSlamToolbox::calculateMapEntropy(std::vector>& grid_etp) { /* @@ -415,70 +483,197 @@ void LifelongSlamToolbox::inverseMeasurement( // mxi - pos_x //--// myi - pos_y float dx = (cells_x[i] - robot_grid_pos[0]) * resolution; float dy = (cells_y[i] - robot_grid_pos[1]) * resolution; - - // std::cout << "dx: " << dx << ", dy: " << dy << std::endl; - float r = sqrt(pow(dx , 2) + pow(dy, 2)); - // float phi = atan2(dy, dx) - robot_grid_pos[2]; float occ_prob = 0.5f; // Unkwnown - // Cell is occupied -- This condition might be different + // Cell is occupied if ((range < max_r) && (abs(r - range) < (alpha / 2.0f))) { std::cout << "Occupied" << std::endl; occ_prob = 0.7f; - // Update the probability - // updateCellProbability(grid_prob, 0.7f, cells_x[i], cells_y[i]); - // Update the log-odds - // updateCellLogs(grid_prob, grid_logs, cells_x[i], cells_y[i], 0.0f); - // Log-odds to probability - // float prob = probabilityFromLog(grid_logs[cells_x[i]][cells_y[i]]); } // Cell is free else if (r <= range) { std::cout << "Free" << std::endl; occ_prob = 0.3f; - // Update the probability - // updateCellProbability(grid_prob, 0.3f, cells_x[i], cells_y[i]); - // Update the log-odds - // updateCellLogs(grid_prob, grid_logs, cells_x[i], cells_y[i], 0.0f); - // Log-odds to probability - // float prob = probabilityFromLog(grid_logs[cells_x[i]][cells_y[i]]); - // float entropy = entropyFromProbability(probabilityFromLog(grid_logs[cells_x[i]][cells_y[i]])); - // updateCellEntropy(grid_etp, cells_x[i], cells_y[i], entropy); } // Update the probability updateCellProbability(grid_prob, occ_prob, cells_x[i], cells_y[i]); - // std::cout << "Probability: " << grid_prob[cells_x[i]][cells_y[i]] << std::endl; + // Update the log-odds updateCellLogs(grid_prob, grid_logs, cells_x[i], cells_y[i], 0.0f); - // std::cout << "Log-Odds: " << grid_logs[cells_x[i]][cells_y[i]] << std::endl; // Log-odds to probability float entropy = entropyFromProbability(probabilityFromLog(grid_logs[cells_x[i]][cells_y[i]])); // Update the entropy updateCellEntropy(grid_etp, cells_x[i], cells_y[i], entropy); + + // std::cout << "Probability: " << grid_prob[cells_x[i]][cells_y[i]] << std::endl; + // std::cout << "Log-Odds: " << grid_logs[cells_x[i]][cells_y[i]] << std::endl; // std::cout << "Entropy: " << grid_etp[cells_x[i]][cells_y[i]] << std::endl; - // std::cout << "Probabilities---------------: " << grid_prob[cells_x[i]][cells_y[i]] << std::endl; - // std::cout << "Logs---------------: " << grid_logs[cells_x[i]][cells_y[i]] << std::endl; - // std::cout << "Log to Prob---------------: " << probabilityFromLog(grid_logs[cells_x[i]][cells_y[i]]) << std::endl; - // std::cout << "Entropies---------------: " << grid_etp[cells_x[i]][cells_y[i]] << std::endl; - - // std::cout << "Relative range: " << r << ", Angle: " << phi << std::endl; std::cout << "Relative range: " << r << std::endl; std::cout << " ++++++++++++++++++++++++ " << std::endl; } } +std::vector LifelongSlamToolbox::unhashIndex(int hash) +{ + /* + To get the index of the current cell combination + */ -/* - Next steps - Done - log-odds will be acumulating the probability based on different observations - Bayes filter - Transform the log-odds into probability - Calculate the entropy of a given cell -*/ + // Hundreds, dozens, units + int hash_h, hash_d, hash_u; + + std::vector index; + index.reserve(3); + + if (hash > 99 && hash < 999) + { + hash_h = hash / 100; + hash_d = (hash - (hash_h * 100)) / 10; + hash_u = hash - (hash_h * 100) - (hash_d * 10); + index.emplace_back(hash_u); + index.emplace_back(hash_d); + index.emplace_back(hash_h); + } + else if (hash > 10 && hash <= 99) + { + hash_d = hash / 10; + hash_u = hash - (hash_d * 10); + index.emplace_back(hash_u); + index.emplace_back(hash_d); + index.emplace_back(0); + } + else + { + index.emplace_back(hash); + index.emplace_back(0); + index.emplace_back(0); + } + return index; +} + +void LifelongSlamToolbox::computeProbabilities(std::vector cell, std::vector>& meas_outcm) +{ + /* + Input: + - Cell: std::vector& const cell + - Set of measurement outcomes: std::vector>& const meas_outcm + Output + - It should be a vector of probabilties + */ + + // Number off measurement outcomes for the current cell + int k = meas_outcm.size(); + std::cout << "K: " << k << std::endl; + + // Current reading + int r = 1; + + // Map for the combinations and the propagated probability + std::map>, float> probabilities; + // Initial condition + probabilities.insert(std::pair>, float>({{0, 0, 0}}, 1.0f)); + + float p_free = meas_outcm[0][2]; + float p_occ = meas_outcm[0][1]; + float p_un = meas_outcm[0][0]; + + // Inserting the first node r = 1 + probabilities.insert(std::pair>, float>({{1, 0, 0}}, p_free)); + probabilities.insert(std::pair>, float>({{0, 1, 0}}, p_occ)); + probabilities.insert(std::pair>, float>({{0, 0, 1}}, p_un)); + + std::cout << p_free << ", " << p_occ << ", " << p_un << std::endl; + std::cout << "++++++++++++++++++++++++++++" << std::endl; + + std::map>, float>::iterator it; + + std::vector::iterator it_combinations; + + for (int i = r; r < k; ++r) + { + std::cout << "R: " << r << std::endl; + + // Vector for saving the cells combinations + std::vector combinations; + // Vector for saving the cells combinatios probabilities + std::vector acc_prob; + + for (it = probabilities.begin(); it != probabilities.end(); ++it) + { + int fr_idx = it->first[0][2]; + int oc_idx = it->first[0][1]; + int un_idx = it->first[0][0]; + + float free_prop = meas_outcm[r][0]; + float occ_prop = meas_outcm[r][1]; + float un_prop = meas_outcm[r][2]; + + if (fr_idx + oc_idx + un_idx == r) + { + // std::cout << it->first[0][2] << ", " << it->first[0][1] << ", " << it->first[0][0] << ", " << it->second <second * free_prop; + } + else + { + combinations.push_back(hash_free); // Adding new probabilities + acc_prob.push_back(it->second * free_prop); // Appending probabilities + } + + it_combinations = std::find(combinations.begin(), combinations.end(), hash_occ); + if (it_combinations != combinations.end()) + { + acc_prob[it_combinations - combinations.begin()] += it->second * occ_prop; + } + else + { + combinations.push_back(hash_occ); // Adding new probabilities + acc_prob.push_back(it->second * occ_prop); // Appending probabilities + } + + it_combinations = std::find(combinations.begin(), combinations.end(), hash_un); + if (it_combinations != combinations.end()) + { + acc_prob[it_combinations - combinations.begin()] += it->second * un_prop; + } + else + { + combinations.push_back(hash_un); // Adding new probabilities + acc_prob.push_back(it->second * un_prop); // Appending probabilities + } + std::cout << "------------------" << std::endl; + } + } + + std::cout << "//------------------//" << std::endl; + for (int k = 0; k < combinations.size(); ++k) + { + std::cout << combinations[k] << ", " << acc_prob[k] << std::endl; + + probabilities.insert(std::pair>, float>({unhashIndex(combinations[k])}, acc_prob[k])); + + } + std::cout << "//------------------//" << std::endl; + } +} void LifelongSlamToolbox::updateCellEntropy(std::vector>& grid_etp, float cell_x, float cell_y, float entropy) { @@ -592,11 +787,25 @@ float LifelongSlamToolbox::calculateProbability(float range_1, float range_2) /* Calculates the probability of a cell being observed by a given measurement range_1: lower limit, range_2: upper limit + https://www.probabilitycourse.com/chapter4/4_2_4_Gamma_distribution.php + https://homepage.divms.uiowa.edu/~mbognar/applets/gamma.html + https://www.youtube.com/watch?v=jWh4HY_Geaw + + This distribution needs to be changed + From my perspective we should use a Gamma distribution instead of an exponential + That would change the integral as well. It might be more complex with Gamma + + - float lambda = 0.285f; + - float nu = 1.0f / lambda; + + https://www.wolframalpha.com/input/?i=plot+0.28*0.35*e%5E%28-0.35*x%29+from+x%3D0+to+15 */ float max_range = 5.0f; - float lambda = 0.285f; - float nu = 1.0f / lambda; + float lambda = 0.35f; + float nu = 0.28f; + + range_1 = (range_1 > max_range) ? max_range : range_1; range_2 = (range_2 > max_range) ? max_range : range_2; // https://www.wolframalpha.com/input/?i2d=true&i=Integrate%5Bn*c*Power%5Be%2C-c*x%5D%2C%7Bx%2Ca%2Cb%7D%5D From eb2c75494f5201d052a968e31108c70044d6b5f8 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Tue, 23 Nov 2021 10:45:20 -0500 Subject: [PATCH 19/83] [ADD]: Algorithm 1 MVP (Currently working), but testing is required --- .../experimental/slam_toolbox_lifelong.hpp | 6 +- src/experimental/slam_toolbox_lifelong.cpp | 113 ++++++++++-------- 2 files changed, 64 insertions(+), 55 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index 0dc9d03be..db3d546f1 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -87,9 +87,9 @@ class LifelongSlamToolbox : public SlamToolbox float calculateMapEntropy(std::vector>& grid_etp); // For algorithm 1 - void appendCellProbabilities(std::vector cell, std::vector& probabilities); - void computeProbabilities(std::vector cell, std::vector>& meas_outcm); - void retreiveProbabilities(); + void appendCellProbabilities(std::vector& meas_outcomes); + void computeProbabilities(std::vector>& meas_outcm); + std::vector> retreiveMeasurementOutcomes(); std::vector unhashIndex(int hash); // Data structures diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 4bd6412cf..7ea217295 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp" #define PI 3.14159265 @@ -160,7 +161,7 @@ void LifelongSlamToolbox::scannerTest() // I will have 5 lasers for each reading (Located at 0, +-25, +-50) // std::vector robot_pose{5.6f, 6.0f, PI/2}; // std::vector robot_pose{5.6f, 6.0f, PI/4}; - std::vector robot_pose{5.6f, 6.0f, PI/2}; + std::vector robot_pose{5.6f, 6.0f, PI/2}; // Create another set of poses // std::vector robot_pose{5.5f, 6.0f, -PI/4}; // std::vector robot_pose{5.5f, 6.0f, -3*PI/4}; // -PI is an unhandled case @@ -223,15 +224,9 @@ void LifelongSlamToolbox::scannerTest() // float etp = calculateMapEntropy(grid_etp); // std::cout << " ...---...---...---...---...---...--- " << std::endl; + std::cout << "Loop for visiting the cells " << std::endl; - std::vector initial_point(2), final_point(2); - - initial_point[0] = 0.0f; - initial_point[1] = 0.0f; - - std::cout << " +++++++++++++ " << std::endl; - - // Start of the loop for + // Visiting the cells for (int j = 0; j < cells_x.size(); ++j) { if ((robot_grid_pos[0] == cells_x[j]) && (robot_grid_pos[1] == cells_y[j])) @@ -241,7 +236,7 @@ void LifelongSlamToolbox::scannerTest() } // Cells visualization - std::cout << cells_x[j] << ", " << cells_y[j] << std::endl; + std::cout << "Current cell: " < probabilities { calculateProbability(distances[1], 5.0f), calculateProbability(distances[0], distances[1]), calculateProbability(0.0f, distances[0]) }; + // Assigning the cells + m_cell_x = cells_x[j]; + m_cell_y = cells_y[j]; - // Need to check here the cell reference. I need to pass the cells in a better way. Hate to create unnecessary copies - // Here is where I need to extract the probabilities of each cell and pass both the cell and the probabilities as argument - // Cell would be better if just keep two integers and reassign them + // Appending new measurement outcomes for the current cell + appendCellProbabilities(probabilities); - appendCellProbabilities({cells_x[j], cells_y[j]}, probabilities); - - /* - We have all the cells appended - We check all the cells we have appended - */ - - - // computeProbabilities({cells_x[j], cells_y[j]}, meas_outcm); - - /* - On this point I have the probabilities of a cell being observed + // Get all the measurement outcomes for the current cell + std::vector> meas_outcomes = retreiveMeasurementOutcomes(); - As soon as I calculate the probabilities of a cell I need to append the probabilities into my data structure + // Compute all the possible combinations + computeProbabilities(meas_outcomes); - Then another external function will be in charge of calculating the combined probabilities of an specific cell - with this function computeProbabilities - - Probabilities calculation is done for a specific cell, so we read the data for one cell and propagate the probabilities. + /* + Note for entropy + I am not sure if the paper is also writing it wrong, but fot all the possible measurement outcomes, + I do have the probability (Also the cell combinations) which is the one I can use to get the entropy. - I need to investigate if the function for extracting the measurement outcomes for a specific cell is required + Also need to add new elements to one cell + This would be the next step. */ - std::cout << "Probabilities: " << probabilities[0] << ", " << probabilities[1] << ", " << probabilities[2] << std::endl; - std::cout << " ++++++++++++++++++++++++ " << std::endl; + //std::cout << "Probabilities: " << probabilities[0] << ", " << probabilities[1] << ", " << probabilities[2] << std::endl; + std::cout << "++++++++++++++++++++++++" << std::endl; } } } -void LifelongSlamToolbox::retreiveProbabilities() +std::vector> LifelongSlamToolbox::retreiveMeasurementOutcomes() { + /* + To get all the measurement outcomes for the current cell + */ + // Iterator for getting the cell std::map, std::vector>>::iterator it_cells; it_cells = m_cell_probabilities.find({m_cell_x, m_cell_y}); - + std::vector> meas_outcomes; if (it_cells != m_cell_probabilities.end()) { - std::cout << "Current cell: " << it_cells->first[0] << ", " << it_cells->first[1] << std::endl; - // Exploring the measurement outcomes for the specific cell for (int i = 0; i < it_cells->second.size(); ++i) { - std::cout << "Measurement outcomes: "; - std::cout << it_cells->second[i][0] << ", " << it_cells->second[i][1] << ", " << it_cells->second[i][2] << std::endl; + // Free , occupied, not observed + meas_outcomes.push_back({it_cells->second[i][0], it_cells->second[i][1], it_cells->second[i][2]}); } } + return meas_outcomes; } -void LifelongSlamToolbox::appendCellProbabilities(std::vector cell, std::vector& probabilities) +void LifelongSlamToolbox::appendCellProbabilities(std::vector& meas_outcomes) { /* To append a new measurement outcome for a specific cell @@ -426,20 +419,20 @@ void LifelongSlamToolbox::appendCellProbabilities(std::vector cell, std::ve // Iterator for getting the cell std::map, std::vector>>::iterator it_cell; - it_cell = m_cell_probabilities.find({cell[0], cell[1]}); + it_cell = m_cell_probabilities.find({m_cell_x, m_cell_y}); if (it_cell == m_cell_probabilities.end()) { // Cell is not present in the map m_cell_probabilities.insert(std::pair, std::vector>>( - {cell[0], cell[1]}, - {{probabilities[0], probabilities[1], probabilities[2]}} + {m_cell_x, m_cell_y}, + {{meas_outcomes[0], meas_outcomes[1], meas_outcomes[2]}} )); } else { // Cell is already in the map, only add the next measurement outcome - it_cell->second.push_back({probabilities[0], probabilities[1], probabilities[2]}); + it_cell->second.push_back({meas_outcomes[0], meas_outcomes[1], meas_outcomes[2]}); } } @@ -514,7 +507,7 @@ void LifelongSlamToolbox::inverseMeasurement( // std::cout << "Entropy: " << grid_etp[cells_x[i]][cells_y[i]] << std::endl; std::cout << "Relative range: " << r << std::endl; - std::cout << " ++++++++++++++++++++++++ " << std::endl; + std::cout << "++++++++++++++++++++++++ " << std::endl; } } @@ -556,11 +549,10 @@ std::vector LifelongSlamToolbox::unhashIndex(int hash) return index; } -void LifelongSlamToolbox::computeProbabilities(std::vector cell, std::vector>& meas_outcm) +void LifelongSlamToolbox::computeProbabilities(std::vector>& meas_outcm) { /* Input: - - Cell: std::vector& const cell - Set of measurement outcomes: std::vector>& const meas_outcm Output - It should be a vector of probabilties @@ -575,6 +567,20 @@ void LifelongSlamToolbox::computeProbabilities(std::vector cell, std::vecto // Map for the combinations and the propagated probability std::map>, float> probabilities; + // + // 10, 11 + // { + // {1, 0, 1}, => 101 + // {2, 0, 0}, + // {0, 2, 0}, + // } + + // Hash maps -> unordered_map + // Key -> Array 3 + // Value -> Probabilidad + // Struct for the key : Tuple + + // Initial condition probabilities.insert(std::pair>, float>({{0, 0, 0}}, 1.0f)); @@ -588,14 +594,17 @@ void LifelongSlamToolbox::computeProbabilities(std::vector cell, std::vecto probabilities.insert(std::pair>, float>({{0, 0, 1}}, p_un)); std::cout << p_free << ", " << p_occ << ", " << p_un << std::endl; - std::cout << "++++++++++++++++++++++++++++" << std::endl; + std::cout << "**********************" << std::endl; std::map>, float>::iterator it; - std::vector::iterator it_combinations; + + std::cout << "Prepared for loop" << std::endl; + for (int i = r; r < k; ++r) { + std::cout << "Not entering loop" << std::endl; std::cout << "R: " << r << std::endl; // Vector for saving the cells combinations From 6cab676760143dc119147c4c0aba6e33db27f2f0 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Mon, 29 Nov 2021 12:16:13 -0500 Subject: [PATCH 20/83] [WIP]: Algorithm 1 completed. Adding now new poses and scans --- .../experimental/slam_toolbox_lifelong.hpp | 23 ++ src/experimental/slam_toolbox_lifelong.cpp | 338 ++++++++---------- 2 files changed, 175 insertions(+), 186 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index db3d546f1..f81ae01e1 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -97,6 +97,29 @@ class LifelongSlamToolbox : public SlamToolbox std::map, std::vector>> m_cell_probabilities; int m_cell_x; int m_cell_y; + + // Struct for cell occupancy + struct Occupancy + { + int fr, oc ,un; + + bool operator==(Occupancy const& st) const + { + return (st.fr == fr) && (st.oc == oc) && (st.un == un); + } + + struct CombinationsHash + { + std::size_t operator()(Occupancy const& key) const + { + std::size_t hash = 5381u; + hash = (hash << 5) + hash + key.fr; + hash = (hash << 5) + hash + key.oc; + hash = (hash << 5) + hash + key.un; + return hash; + } + }; + }; /*****************************************************************************/ bool use_tree_; diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 7ea217295..660cd5bcf 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -131,7 +131,7 @@ void LifelongSlamToolbox::scannerTest() int number_cells = map_dist / resolution; float unknown_prob = 0.5f; - float initial_log = calculateLogs(unknown_prob); + float initial_log = calculateLogs(unknown_prob); std::vector> grid(number_cells); // 20 meters in Y dimension std::vector> grid_prob(number_cells); // 20 meters in Y dimension std::vector> grid_logs(number_cells); // 20 meters in Y dimension @@ -147,43 +147,35 @@ void LifelongSlamToolbox::scannerTest() for (int j=0; j robot_pose{5.6f, 6.0f, PI/2}; // std::vector robot_pose{5.6f, 6.0f, PI/4}; - std::vector robot_pose{5.6f, 6.0f, PI/2}; // Create another set of poses - // std::vector robot_pose{5.5f, 6.0f, -PI/4}; - // std::vector robot_pose{5.5f, 6.0f, -3*PI/4}; - // -PI is an unhandled case - // std::vector robot_pose{5.5f, 6.0f, PI}; - // std::vector robot_pose{5.5f, 6.0f, 0.0f}; + + std::vector> robot_poses {{5.6f, 6.0f, PI/2}, {5.6f, 6.0f, PI/2}}; + + std::vector robot_pose{5.6f, 6.0f, PI/2}; + std::vector ranges{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters + std::vector angles{-0.43633f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + // std::vector ranges{1.65f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters + // std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // This is the initial point std::vector robot_grid_pos = getGridPosition(robot_pose[0], robot_pose[1], resolution); std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; // Creating the laser scan with 5 beams ------- Angles will be -50, -25, 0, 25, 50 //----// +-50 = +-0.87266 : +-25 = +-0.43633 - std::vector ranges{1.65f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters // std::vector ranges{2.8f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters - std::vector angles{0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; - // std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; - // std::vector angles{0.785398f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; - // std::vector angles{0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // Current yaw + beam angle: -PI/2 (-1.570795) -0.87266 = 2.44345 (-55 degrees) // for (int i = 0; i < ranges.size(); ++i) - for (int i = 0; i < 2; ++i) // One reading only + for (int i = 0; i < 1; ++i) // One reading only { std::cout << "........ New laser ........" << std::endl; std::cout << "Distance: " << ranges[i] << ", Angle: " << angles[i] << std::endl; @@ -192,7 +184,8 @@ void LifelongSlamToolbox::scannerTest() std::vector laser_grid = getLaserHit(robot_pose, ranges[i], angles[i]); // Laser final cell std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1], resolution); - std::cout << final_grid_pos[0] << ", " << final_grid_pos[1] << std::endl; + std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; + std::cout << "Final grid position: " << final_grid_pos[0] << ", " << final_grid_pos[1] << std::endl; // robot_grid_pos[0] // X1 - robot_grid_pos[1] // Y1 // final_grid_pos[0] // X2 - final_grid_pos[1] // Y2 @@ -205,7 +198,7 @@ void LifelongSlamToolbox::scannerTest() cells_y = res_pair.second; // Adding last hit cell to the set - cells_x.push_back(final_grid_pos[0]); + cells_x.push_back(final_grid_pos[0]); cells_y.push_back(final_grid_pos[1]); // std::cout << "Cells" << std::endl; @@ -231,14 +224,12 @@ void LifelongSlamToolbox::scannerTest() { if ((robot_grid_pos[0] == cells_x[j]) && (robot_grid_pos[1] == cells_y[j])) { - // We do not need to calculate in the first cell continue; } // Cells visualization - std::cout << "Current cell: " <= robot_grid_pos[1]) { + std::cout << "---------------------Case 1" << std::endl; + std::cout << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; + std::cout << final_grid_pos[0] << ", " << final_grid_pos[1] << std::endl; + + // std::cout << final_grid_pos[0] << ", " << robot_grid_pos[0] << std::endl; + // std::cout << final_grid_pos[1] << ", " << robot_grid_pos[1] << std::endl; + // X minor and Y greater. WRO final points initial_x[2] = limit_x - resolution; initial_x[3] = limit_x - resolution; @@ -262,12 +260,23 @@ void LifelongSlamToolbox::scannerTest() final_x[0] = limit_x - resolution; final_x[2] = limit_x - resolution; - min_x = limit_x - resolution; - max_x = limit_x; + // min_x = limit_x - 2*resolution; + // max_x = limit_x - resolution; + + min_y = limit_y - resolution; + max_y = limit_y; + + // min_y = limit_y; + // max_y = limit_y + resolution; + std::cout << "Points: " << initial_x[2] << ", " << initial_x[3] << ", " << final_x[0] << ", " << final_x[2] << std::endl; + std::cout << "Limits: " << min_x << ", " << max_x << ", " << min_y << ", " << max_y << std::endl; } + if (final_grid_pos[0] >= robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) { + std::cout << "---------------------Case 2" << std::endl; + // X greater and Y minor. WRO final points initial_y[2] = limit_y - resolution; initial_y[3] = limit_y - resolution; @@ -277,10 +286,13 @@ void LifelongSlamToolbox::scannerTest() min_y = limit_y - resolution; max_y = limit_y; + } if (final_grid_pos[0] < robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) { + std::cout << "---------------------Case 3" << std::endl; + // X minor and Y minor. WRO final points initial_x[2] = limit_x - resolution; initial_x[3] = limit_x - resolution; @@ -298,19 +310,42 @@ void LifelongSlamToolbox::scannerTest() max_y = limit_y; } - std::vector inter_x, inter_y; + + std::vector inter_x, inter_y; for (int k = 0; k < 4; ++k) { - //std::cout << "Points: " << initial_x[k] << ", " << initial_y[k] << ", " << final_x[k] << ", " << final_y[k] << std::endl; + std::cout << "----------------" << std::endl; + + // std::cout << "Points: " << initial_x[k] << ", " << initial_y[k] << ", " << final_x[k] << ", " << final_y[k] << std::endl; std::vector intersection = calculateIntersection(robot_pose, laser_grid, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); + std::cout << "Intersection size: " << intersection.size() << std::endl; + std::cout << "Initial point: " << initial_x[k] << ", " << initial_y[k] << std::endl; + std::cout << "Final point: " << final_x[k] << ", " << final_y[k] << std::endl; + + + if(intersection.size() != 0) { + std::cout << "Comparing values" << std::endl; + std::cout << abs(intersection[0]) << "," << abs(min_x - 0.001f) << std::endl; + std::cout << abs(intersection[0]) << "," << abs(max_x + 0.001f) << std::endl; + std::cout << abs(intersection[1]) << "," << abs(min_y - 0.001f) << std::endl; + std::cout << abs(intersection[1]) << "," << abs(max_y + 0.001f) << std::endl; + + std::cout << "Results" << std::endl; + std::cout << (abs(intersection[0]) >= abs(min_x - 0.001f)) << std::endl; + std::cout << (abs(intersection[0]) <= abs(max_x + 0.001f)) << std::endl; + std::cout << (abs(intersection[1]) >= abs(min_y - 0.001f)) << std::endl; + std::cout << (abs(intersection[1]) <= abs(max_y + 0.001f)) << std::endl; + // If the laser and a cell intersects, we need to make sure it happens in the right bounds if ((abs(intersection[0]) >= abs(min_x - 0.001f)) && (abs(intersection[0]) <= abs(max_x + 0.001f)) && (abs(intersection[1]) >= abs(min_y - 0.001f)) && (abs(intersection[1]) <= abs(max_y + 0.001f))) { + std::cout << "Intersections!!!! " << std::endl; + /* Two points where the beam cuts the cell - A laser beam can cut the cell at least 1 time (Enter) @@ -322,7 +357,9 @@ void LifelongSlamToolbox::scannerTest() } } } - + + std::cout << "Size: " << inter_x.size() << std::endl; + // Enter (d1) and Exit (d2) distances std::vector distances; for (int k = 0; k < inter_x.size(); ++k) @@ -331,19 +368,15 @@ void LifelongSlamToolbox::scannerTest() This could be a ring buffer - Actually some elements can be a ring buffer dist_point[0]: d1 dist_point[1]: d2 - */ + */ float dist_point = calculateDistance(robot_pose[0], robot_pose[1], inter_x[k], inter_y[k]); distances.push_back(dist_point); // std::cout << "Distance: " << dist_point << std::endl; } - /* - For last cell I need to modify something - */ - // Integral 1: 0 to d1 which is distance from robot pose to first point where the cell is cut - // Integral 2: d1 which is distance from robot pose to first point where the cell is cut to + // Integral 2: d1 which is distance from robot pose to first point where the cell is cut to // d2 which is distance from robot pose to second point where the cell is cut // Integral 3: d2 which is distance from robot pose to second point where the cell is cut to z_max @@ -351,6 +384,9 @@ void LifelongSlamToolbox::scannerTest() // float prob_occ = calculateProbability(distances[0], distances[1]); // 3.9 - Occupied // float prob_free = calculateProbability(distances[1], 5.0f); // 3.10 - Free + std::cout << "Debugeishon" << std::endl; + std::cout << "Size: " << distances.size() << std::endl; + // Measurement outcomes vector std::vector probabilities { calculateProbability(distances[1], 5.0f), @@ -363,11 +399,11 @@ void LifelongSlamToolbox::scannerTest() m_cell_y = cells_y[j]; // Appending new measurement outcomes for the current cell - appendCellProbabilities(probabilities); + appendCellProbabilities(probabilities); + + // Get all the measurement outcomes for the current cell + std::vector> meas_outcomes = retreiveMeasurementOutcomes(); - // Get all the measurement outcomes for the current cell - std::vector> meas_outcomes = retreiveMeasurementOutcomes(); - // Compute all the possible combinations computeProbabilities(meas_outcomes); @@ -389,7 +425,7 @@ void LifelongSlamToolbox::scannerTest() std::vector> LifelongSlamToolbox::retreiveMeasurementOutcomes() { /* - To get all the measurement outcomes for the current cell + To get all the measurement outcomes for the current cell */ // Iterator for getting the cell @@ -418,14 +454,13 @@ void LifelongSlamToolbox::appendCellProbabilities(std::vector& meas_outco // Iterator for getting the cell std::map, std::vector>>::iterator it_cell; - - it_cell = m_cell_probabilities.find({m_cell_x, m_cell_y}); + it_cell = m_cell_probabilities.find({m_cell_x, m_cell_y}); if (it_cell == m_cell_probabilities.end()) { // Cell is not present in the map m_cell_probabilities.insert(std::pair, std::vector>>( - {m_cell_x, m_cell_y}, + {m_cell_x, m_cell_y}, {{meas_outcomes[0], meas_outcomes[1], meas_outcomes[2]}} )); } @@ -441,7 +476,7 @@ float LifelongSlamToolbox::calculateMapEntropy(std::vector>& /* To calculate the map entropy */ - float map_etp = 0.0f; + float map_etp = 0.0f; for (auto vct : grid_etp) { std::for_each(vct.begin(), vct.end(), [&] (float entropy) { @@ -452,16 +487,16 @@ float LifelongSlamToolbox::calculateMapEntropy(std::vector>& } void LifelongSlamToolbox::inverseMeasurement( - std::vector>& grid_prob, - std::vector>& grid_logs, - std::vector>& grid_etp, - std::vector& cells_x, - std::vector& cells_y, - std::vector& robot_grid_pos, + std::vector>& grid_prob, + std::vector>& grid_logs, + std::vector>& grid_etp, + std::vector& cells_x, + std::vector& cells_y, + std::vector& robot_grid_pos, float range, float angle, float resolution) { /* - On this function we update a probability map based + On this function we update a probability map based We capture the cells that are hitted by the ray and the conditions And we update the probability as being occupied or free */ @@ -477,9 +512,9 @@ void LifelongSlamToolbox::inverseMeasurement( float dx = (cells_x[i] - robot_grid_pos[0]) * resolution; float dy = (cells_y[i] - robot_grid_pos[1]) * resolution; float r = sqrt(pow(dx , 2) + pow(dy, 2)); - float occ_prob = 0.5f; // Unkwnown + float occ_prob = 0.5f; // Unkwnown - // Cell is occupied + // Cell is occupied if ((range < max_r) && (abs(r - range) < (alpha / 2.0f))) { std::cout << "Occupied" << std::endl; @@ -499,188 +534,118 @@ void LifelongSlamToolbox::inverseMeasurement( updateCellLogs(grid_prob, grid_logs, cells_x[i], cells_y[i], 0.0f); // Log-odds to probability float entropy = entropyFromProbability(probabilityFromLog(grid_logs[cells_x[i]][cells_y[i]])); - // Update the entropy + // Update the entropy updateCellEntropy(grid_etp, cells_x[i], cells_y[i], entropy); // std::cout << "Probability: " << grid_prob[cells_x[i]][cells_y[i]] << std::endl; // std::cout << "Log-Odds: " << grid_logs[cells_x[i]][cells_y[i]] << std::endl; // std::cout << "Entropy: " << grid_etp[cells_x[i]][cells_y[i]] << std::endl; - + std::cout << "Relative range: " << r << std::endl; std::cout << "++++++++++++++++++++++++ " << std::endl; } } -std::vector LifelongSlamToolbox::unhashIndex(int hash) -{ - /* - To get the index of the current cell combination - */ - - // Hundreds, dozens, units - int hash_h, hash_d, hash_u; - - std::vector index; - index.reserve(3); - - if (hash > 99 && hash < 999) - { - hash_h = hash / 100; - hash_d = (hash - (hash_h * 100)) / 10; - hash_u = hash - (hash_h * 100) - (hash_d * 10); - index.emplace_back(hash_u); - index.emplace_back(hash_d); - index.emplace_back(hash_h); - } - else if (hash > 10 && hash <= 99) - { - hash_d = hash / 10; - hash_u = hash - (hash_d * 10); - index.emplace_back(hash_u); - index.emplace_back(hash_d); - index.emplace_back(0); - } - else - { - index.emplace_back(hash); - index.emplace_back(0); - index.emplace_back(0); - } - return index; -} void LifelongSlamToolbox::computeProbabilities(std::vector>& meas_outcm) { /* - Input: - - Set of measurement outcomes: std::vector>& const meas_outcm - Output - - It should be a vector of probabilties + To compute all the possible combinations of a grid cell, given a set of measurement outcomes */ - - // Number off measurement outcomes for the current cell int k = meas_outcm.size(); std::cout << "K: " << k << std::endl; - - // Current reading + int r = 1; - - // Map for the combinations and the propagated probability - std::map>, float> probabilities; - // - // 10, 11 - // { - // {1, 0, 1}, => 101 - // {2, 0, 0}, - // {0, 2, 0}, - // } - - // Hash maps -> unordered_map - // Key -> Array 3 - // Value -> Probabilidad - // Struct for the key : Tuple - - - // Initial condition - probabilities.insert(std::pair>, float>({{0, 0, 0}}, 1.0f)); - + float p_free = meas_outcm[0][2]; float p_occ = meas_outcm[0][1]; float p_un = meas_outcm[0][0]; - - // Inserting the first node r = 1 - probabilities.insert(std::pair>, float>({{1, 0, 0}}, p_free)); - probabilities.insert(std::pair>, float>({{0, 1, 0}}, p_occ)); - probabilities.insert(std::pair>, float>({{0, 0, 1}}, p_un)); - std::cout << p_free << ", " << p_occ << ", " << p_un << std::endl; - std::cout << "**********************" << std::endl; - - std::map>, float>::iterator it; - std::vector::iterator it_combinations; + std::unordered_map un_cmb; + std::unordered_map::iterator it_un; - std::cout << "Prepared for loop" << std::endl; + // Initial node + un_cmb.insert(std::pair({0, 0, 0}, 1.0f)); + un_cmb.insert(std::pair({0, 0, 1}, p_free)); + un_cmb.insert(std::pair({0, 1, 0}, p_occ)); + un_cmb.insert(std::pair({1, 0, 0}, p_un)); - for (int i = r; r < k; ++r) { - std::cout << "Not entering loop" << std::endl; std::cout << "R: " << r << std::endl; - // Vector for saving the cells combinations - std::vector combinations; - // Vector for saving the cells combinatios probabilities + std::vector occ_vct; std::vector acc_prob; - - for (it = probabilities.begin(); it != probabilities.end(); ++it) - { - int fr_idx = it->first[0][2]; - int oc_idx = it->first[0][1]; - int un_idx = it->first[0][0]; - - float free_prop = meas_outcm[r][0]; + + for (it_un = un_cmb.begin(); it_un != un_cmb.end(); ++it_un) + { + // Current combination state + int fr_idx = it_un->first.fr; + int oc_idx = it_un->first.oc; + int un_idx = it_un->first.un; + + // Measurement outcome probability + float free_prop = meas_outcm[r][0]; float occ_prop = meas_outcm[r][1]; float un_prop = meas_outcm[r][2]; - + if (fr_idx + oc_idx + un_idx == r) { - // std::cout << it->first[0][2] << ", " << it->first[0][1] << ", " << it->first[0][0] << ", " << it->second <second * free_prop; + acc_prob[it_comb - occ_vct.begin()] += it_un->second * free_prop; } else { - combinations.push_back(hash_free); // Adding new probabilities - acc_prob.push_back(it->second * free_prop); // Appending probabilities + occ_vct.push_back(Occupancy{fr_idx + 1, oc_idx, un_idx}); + acc_prob.push_back(it_un->second * free_prop); } - - it_combinations = std::find(combinations.begin(), combinations.end(), hash_occ); - if (it_combinations != combinations.end()) + + it_comb = std::find(occ_vct.begin(), occ_vct.end(), Occupancy{fr_idx, oc_idx + 1, un_idx}); + // Occupied + if (it_comb != occ_vct.end()) { - acc_prob[it_combinations - combinations.begin()] += it->second * occ_prop; + acc_prob[it_comb - occ_vct.begin()] += it_un->second * occ_prop; } else { - combinations.push_back(hash_occ); // Adding new probabilities - acc_prob.push_back(it->second * occ_prop); // Appending probabilities + occ_vct.push_back(Occupancy{fr_idx, oc_idx + 1, un_idx}); + acc_prob.push_back(it_un->second * occ_prop); } - - it_combinations = std::find(combinations.begin(), combinations.end(), hash_un); - if (it_combinations != combinations.end()) + + it_comb = std::find(occ_vct.begin(), occ_vct.end(), Occupancy{fr_idx, oc_idx, un_idx + 1}); + // Unobserved + if (it_comb != occ_vct.end()) { - acc_prob[it_combinations - combinations.begin()] += it->second * un_prop; + acc_prob[it_comb - occ_vct.begin()] += it_un->second * un_prop; } else { - combinations.push_back(hash_un); // Adding new probabilities - acc_prob.push_back(it->second * un_prop); // Appending probabilities + occ_vct.push_back(Occupancy{fr_idx, oc_idx, un_idx + 1}); + acc_prob.push_back(it_un->second * un_prop); } - std::cout << "------------------" << std::endl; } } - - std::cout << "//------------------//" << std::endl; - for (int k = 0; k < combinations.size(); ++k) + + // Inserting the elements into the map + for (int k = 0; k < occ_vct.size(); ++k) { - std::cout << combinations[k] << ", " << acc_prob[k] << std::endl; - - probabilities.insert(std::pair>, float>({unhashIndex(combinations[k])}, acc_prob[k])); - + un_cmb.insert(std::pair(occ_vct[k], acc_prob[k])); } - std::cout << "//------------------//" << std::endl; + } + + std::unordered_map::iterator it_show; + for (it_show = un_cmb.begin(); it_show != un_cmb.end(); ++it_show) + { + std::cout << it_show->first.fr << ", " << it_show->first.oc << ", " << it_show->first.un << ", " << it_show->second <>& gri To update the cell entropy - H = H - Entropy; */ - grid_etp[cell_x][cell_y] = grid_etp[cell_x][cell_y] - entropy; + grid_etp[cell_x][cell_y] = grid_etp[cell_x][cell_y] - entropy; } float LifelongSlamToolbox::entropyFromProbability(float prob) @@ -704,7 +669,7 @@ float LifelongSlamToolbox::entropyFromProbability(float prob) float LifelongSlamToolbox::probabilityFromLog(float log) { - /* + /* To transform the Log-odds into probability */ return (exp(log) / (1 + exp(log))); @@ -769,13 +734,14 @@ std::vector LifelongSlamToolbox::calculateIntersection( if (den == 0.0f) { // Parallel lines or not intersection at all + std::cout << " -Parallels- " << std::endl; return {}; } else { float x = ((x2*y1 - x1*y2)*(x4 - x3) - (x4*y3 - x3*y4)*(x2-x1)) / den; float y = ((x2*y1 - x1*y2)*(y4 - y3) - (x4*y3 - x3*y4)*(y2-y1)) / den; - // std::cout << x<< ", " << y << std::endl; + std::cout << "Intersection point: "<< x << ", " << y << std::endl; return {x, y}; } } @@ -799,11 +765,11 @@ float LifelongSlamToolbox::calculateProbability(float range_1, float range_2) https://www.probabilitycourse.com/chapter4/4_2_4_Gamma_distribution.php https://homepage.divms.uiowa.edu/~mbognar/applets/gamma.html https://www.youtube.com/watch?v=jWh4HY_Geaw - + This distribution needs to be changed From my perspective we should use a Gamma distribution instead of an exponential That would change the integral as well. It might be more complex with Gamma - + - float lambda = 0.285f; - float nu = 1.0f / lambda; @@ -816,7 +782,7 @@ float LifelongSlamToolbox::calculateProbability(float range_1, float range_2) range_1 = (range_1 > max_range) ? max_range : range_1; range_2 = (range_2 > max_range) ? max_range : range_2; - + // https://www.wolframalpha.com/input/?i2d=true&i=Integrate%5Bn*c*Power%5Be%2C-c*x%5D%2C%7Bx%2Ca%2Cb%7D%5D return nu * (exp(-lambda*range_1) - exp(-lambda*range_2)); } From 285d07ea0510f5d91f2103c77dd561f1a3e40349 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Mon, 29 Nov 2021 18:46:19 -0500 Subject: [PATCH 21/83] [WIP]: Adding conditions for some cells. New intersections are handled with this logic --- src/experimental/slam_toolbox_lifelong.cpp | 94 ++++++++-------------- 1 file changed, 33 insertions(+), 61 deletions(-) diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 660cd5bcf..f2c5b9eb5 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -161,9 +161,9 @@ void LifelongSlamToolbox::scannerTest() std::vector robot_pose{5.6f, 6.0f, PI/2}; std::vector ranges{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters - std::vector angles{-0.43633f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + // std::vector angles{-0.43633f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // std::vector ranges{1.65f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters - // std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + std::vector angles{0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // This is the initial point std::vector robot_grid_pos = getGridPosition(robot_pose[0], robot_pose[1], resolution); @@ -172,7 +172,6 @@ void LifelongSlamToolbox::scannerTest() // Creating the laser scan with 5 beams ------- Angles will be -50, -25, 0, 25, 50 //----// +-50 = +-0.87266 : +-25 = +-0.43633 // std::vector ranges{2.8f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters - // Current yaw + beam angle: -PI/2 (-1.570795) -0.87266 = 2.44345 (-55 degrees) // for (int i = 0; i < ranges.size(); ++i) for (int i = 0; i < 1; ++i) // One reading only @@ -182,10 +181,14 @@ void LifelongSlamToolbox::scannerTest() // Laser continuous distance std::vector laser_grid = getLaserHit(robot_pose, ranges[i], angles[i]); + std::cout << "Laser hit: " << laser_grid[0] << ", " << laser_grid[1] << std::endl; + // Laser final cell std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1], resolution); - std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; - std::cout << "Final grid position: " << final_grid_pos[0] << ", " << final_grid_pos[1] << std::endl; + + // std::cout << "Laser hit: " << laser_grid[0] << ", " << laser_grid[1] << std::endl; + // std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; + // std::cout << "Final grid position: " << final_grid_pos[0] << ", " << final_grid_pos[1] << std::endl; // robot_grid_pos[0] // X1 - robot_grid_pos[1] // Y1 // final_grid_pos[0] // X2 - final_grid_pos[1] // Y2 @@ -201,12 +204,12 @@ void LifelongSlamToolbox::scannerTest() cells_x.push_back(final_grid_pos[0]); cells_y.push_back(final_grid_pos[1]); - // std::cout << "Cells" << std::endl; - // for (int c = 0; c < cells_x.size(); ++c) // One reading only - // { - // std::cout << cells_x[c] << ", " << cells_y[c] << std::endl; - // } - // std::cout << "End of cells" << std::endl; + std::cout << "Cells" << std::endl; + for (int c = 0; c < cells_x.size(); ++c) // One reading only + { + std::cout << cells_x[c] << ", " << cells_y[c] << std::endl; + } + std::cout << "End of cells" << std::endl; // std::cout << " ...---...---...---...---...---...--- " << std::endl; @@ -217,8 +220,6 @@ void LifelongSlamToolbox::scannerTest() // float etp = calculateMapEntropy(grid_etp); // std::cout << " ...---...---...---...---...---...--- " << std::endl; - std::cout << "Loop for visiting the cells " << std::endl; - // Visiting the cells for (int j = 0; j < cells_x.size(); ++j) { @@ -246,37 +247,16 @@ void LifelongSlamToolbox::scannerTest() if (final_grid_pos[0] < robot_grid_pos[0] && final_grid_pos[1] >= robot_grid_pos[1]) { - std::cout << "---------------------Case 1" << std::endl; - std::cout << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; - std::cout << final_grid_pos[0] << ", " << final_grid_pos[1] << std::endl; - - // std::cout << final_grid_pos[0] << ", " << robot_grid_pos[0] << std::endl; - // std::cout << final_grid_pos[1] << ", " << robot_grid_pos[1] << std::endl; - - // X minor and Y greater. WRO final points - initial_x[2] = limit_x - resolution; - initial_x[3] = limit_x - resolution; - - final_x[0] = limit_x - resolution; - final_x[2] = limit_x - resolution; - - // min_x = limit_x - 2*resolution; - // max_x = limit_x - resolution; + // X greater and Y greater. WRO final points + final_x[0] = limit_x + resolution; + final_x[2] = limit_x + resolution; min_y = limit_y - resolution; max_y = limit_y; - - // min_y = limit_y; - // max_y = limit_y + resolution; - std::cout << "Points: " << initial_x[2] << ", " << initial_x[3] << ", " << final_x[0] << ", " << final_x[2] << std::endl; - std::cout << "Limits: " << min_x << ", " << max_x << ", " << min_y << ", " << max_y << std::endl; } - if (final_grid_pos[0] >= robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) { - std::cout << "---------------------Case 2" << std::endl; - // X greater and Y minor. WRO final points initial_y[2] = limit_y - resolution; initial_y[3] = limit_y - resolution; @@ -286,13 +266,10 @@ void LifelongSlamToolbox::scannerTest() min_y = limit_y - resolution; max_y = limit_y; - } if (final_grid_pos[0] < robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) { - std::cout << "---------------------Case 3" << std::endl; - // X minor and Y minor. WRO final points initial_x[2] = limit_x - resolution; initial_x[3] = limit_x - resolution; @@ -314,47 +291,42 @@ void LifelongSlamToolbox::scannerTest() std::vector inter_x, inter_y; for (int k = 0; k < 4; ++k) { - std::cout << "----------------" << std::endl; - - // std::cout << "Points: " << initial_x[k] << ", " << initial_y[k] << ", " << final_x[k] << ", " << final_y[k] << std::endl; std::vector intersection = calculateIntersection(robot_pose, laser_grid, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); std::cout << "Intersection size: " << intersection.size() << std::endl; std::cout << "Initial point: " << initial_x[k] << ", " << initial_y[k] << std::endl; std::cout << "Final point: " << final_x[k] << ", " << final_y[k] << std::endl; - - if(intersection.size() != 0) { - std::cout << "Comparing values" << std::endl; - std::cout << abs(intersection[0]) << "," << abs(min_x - 0.001f) << std::endl; - std::cout << abs(intersection[0]) << "," << abs(max_x + 0.001f) << std::endl; - std::cout << abs(intersection[1]) << "," << abs(min_y - 0.001f) << std::endl; - std::cout << abs(intersection[1]) << "," << abs(max_y + 0.001f) << std::endl; + std::cout << "Comparing values" << std::endl; + std::cout << abs(intersection[0]) << "," << abs(min_x - 0.01f) << std::endl; + std::cout << abs(intersection[0]) << "," << abs(max_x + 0.01f) << std::endl; + std::cout << abs(intersection[1]) << "," << abs(min_y - 0.01f) << std::endl; + std::cout << abs(intersection[1]) << "," << abs(max_y + 0.01f) << std::endl; + std::cout << "Results" << std::endl; - std::cout << (abs(intersection[0]) >= abs(min_x - 0.001f)) << std::endl; - std::cout << (abs(intersection[0]) <= abs(max_x + 0.001f)) << std::endl; - std::cout << (abs(intersection[1]) >= abs(min_y - 0.001f)) << std::endl; - std::cout << (abs(intersection[1]) <= abs(max_y + 0.001f)) << std::endl; + std::cout << (abs(intersection[0]) >= abs(min_x - 0.01f)) << std::endl; + std::cout << (abs(intersection[0]) <= abs(max_x + 0.01f)) << std::endl; + std::cout << (abs(intersection[1]) >= abs(min_y - 0.01f)) << std::endl; + std::cout << (abs(intersection[1]) <= abs(max_y + 0.01f)) << std::endl; // If the laser and a cell intersects, we need to make sure it happens in the right bounds - if ((abs(intersection[0]) >= abs(min_x - 0.001f)) && - (abs(intersection[0]) <= abs(max_x + 0.001f)) && - (abs(intersection[1]) >= abs(min_y - 0.001f)) && - (abs(intersection[1]) <= abs(max_y + 0.001f))) + if ((abs(intersection[0]) >= abs(min_x - 0.01f)) && + (abs(intersection[0]) <= abs(max_x + 0.01f)) && + (abs(intersection[1]) >= abs(min_y - 0.01f)) && + (abs(intersection[1]) <= abs(max_y + 0.01f))) { - std::cout << "Intersections!!!! " << std::endl; - /* 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) */ - // std::cout << "Interception at: " << intersection[0] << ", " << intersection[1] << std::endl; inter_x.push_back(intersection[0]); inter_y.push_back(intersection[1]); } + + std::cout << " ...---...---...---...---...---...--- " << std::endl; } } From ec51e0b3cfade3cab33fdc394f0c071040058e06 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Tue, 30 Nov 2021 07:29:47 -0500 Subject: [PATCH 22/83] [FIX]: Handling cells seen by Bresenham but without intersections --- src/experimental/slam_toolbox_lifelong.cpp | 70 ++++++---------------- 1 file changed, 18 insertions(+), 52 deletions(-) diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index f2c5b9eb5..08767975e 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -163,7 +163,8 @@ void LifelongSlamToolbox::scannerTest() std::vector ranges{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters // std::vector angles{-0.43633f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // std::vector ranges{1.65f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters - std::vector angles{0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + // std::vector angles{0.43633f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // This is the initial point std::vector robot_grid_pos = getGridPosition(robot_pose[0], robot_pose[1], resolution); @@ -173,22 +174,17 @@ void LifelongSlamToolbox::scannerTest() // std::vector ranges{2.8f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters // Current yaw + beam angle: -PI/2 (-1.570795) -0.87266 = 2.44345 (-55 degrees) - // for (int i = 0; i < ranges.size(); ++i) - for (int i = 0; i < 1; ++i) // One reading only + for (int i = 0; i < ranges.size(); ++i) + // for (int i = 0; i < 1; ++i) // One reading only { std::cout << "........ New laser ........" << std::endl; std::cout << "Distance: " << ranges[i] << ", Angle: " << angles[i] << std::endl; // Laser continuous distance std::vector laser_grid = getLaserHit(robot_pose, ranges[i], angles[i]); - std::cout << "Laser hit: " << laser_grid[0] << ", " << laser_grid[1] << std::endl; // Laser final cell std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1], resolution); - - // std::cout << "Laser hit: " << laser_grid[0] << ", " << laser_grid[1] << std::endl; - // std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; - // std::cout << "Final grid position: " << final_grid_pos[0] << ", " << final_grid_pos[1] << std::endl; // robot_grid_pos[0] // X1 - robot_grid_pos[1] // Y1 // final_grid_pos[0] // X2 - final_grid_pos[1] // Y2 @@ -204,21 +200,12 @@ void LifelongSlamToolbox::scannerTest() cells_x.push_back(final_grid_pos[0]); cells_y.push_back(final_grid_pos[1]); - std::cout << "Cells" << std::endl; - for (int c = 0; c < cells_x.size(); ++c) // One reading only - { - std::cout << cells_x[c] << ", " << cells_y[c] << std::endl; - } - std::cout << "End of cells" << std::endl; - - - // std::cout << " ...---...---...---...---...---...--- " << std::endl; - // For now this will not be used - // Cell probability, log-odds and entropy calculation - // inverseMeasurement(grid_prob, grid_logs, grid_etp, cells_x, cells_y, robot_grid_pos, ranges[i], angles[i], resolution); - // Map entropy calculation - // float etp = calculateMapEntropy(grid_etp); - // std::cout << " ...---...---...---...---...---...--- " << std::endl; + // std::cout << "Cells" << std::endl; + // for (int c = 0; c < cells_x.size(); ++c) // One reading only + // { + // std::cout << cells_x[c] << ", " << cells_y[c] << std::endl; + // } + // std::cout << "End of cells" << std::endl; // Visiting the cells for (int j = 0; j < cells_x.size(); ++j) @@ -251,8 +238,8 @@ void LifelongSlamToolbox::scannerTest() final_x[0] = limit_x + resolution; final_x[2] = limit_x + resolution; - min_y = limit_y - resolution; - max_y = limit_y; + min_y = limit_y; + max_y = limit_y + resolution; } if (final_grid_pos[0] >= robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) @@ -287,30 +274,14 @@ void LifelongSlamToolbox::scannerTest() max_y = limit_y; } - std::vector inter_x, inter_y; for (int k = 0; k < 4; ++k) { std::vector intersection = calculateIntersection(robot_pose, laser_grid, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); - std::cout << "Intersection size: " << intersection.size() << std::endl; - std::cout << "Initial point: " << initial_x[k] << ", " << initial_y[k] << std::endl; - std::cout << "Final point: " << final_x[k] << ", " << final_y[k] << std::endl; + if(intersection.size() != 0) { - - std::cout << "Comparing values" << std::endl; - std::cout << abs(intersection[0]) << "," << abs(min_x - 0.01f) << std::endl; - std::cout << abs(intersection[0]) << "," << abs(max_x + 0.01f) << std::endl; - std::cout << abs(intersection[1]) << "," << abs(min_y - 0.01f) << std::endl; - std::cout << abs(intersection[1]) << "," << abs(max_y + 0.01f) << std::endl; - - std::cout << "Results" << std::endl; - std::cout << (abs(intersection[0]) >= abs(min_x - 0.01f)) << std::endl; - std::cout << (abs(intersection[0]) <= abs(max_x + 0.01f)) << std::endl; - std::cout << (abs(intersection[1]) >= abs(min_y - 0.01f)) << std::endl; - std::cout << (abs(intersection[1]) <= abs(max_y + 0.01f)) << std::endl; - // If the laser and a cell intersects, we need to make sure it happens in the right bounds if ((abs(intersection[0]) >= abs(min_x - 0.01f)) && (abs(intersection[0]) <= abs(max_x + 0.01f)) && @@ -325,12 +296,12 @@ void LifelongSlamToolbox::scannerTest() inter_x.push_back(intersection[0]); inter_y.push_back(intersection[1]); } - - std::cout << " ...---...---...---...---...---...--- " << std::endl; } } - std::cout << "Size: " << inter_x.size() << std::endl; + // When a cell is marked by Bresenham but there is not intersection points + if (inter_x.size() == 0) + continue; // Enter (d1) and Exit (d2) distances std::vector distances; @@ -356,9 +327,6 @@ void LifelongSlamToolbox::scannerTest() // float prob_occ = calculateProbability(distances[0], distances[1]); // 3.9 - Occupied // float prob_free = calculateProbability(distances[1], 5.0f); // 3.10 - Free - std::cout << "Debugeishon" << std::endl; - std::cout << "Size: " << distances.size() << std::endl; - // Measurement outcomes vector std::vector probabilities { calculateProbability(distances[1], 5.0f), @@ -489,13 +457,11 @@ void LifelongSlamToolbox::inverseMeasurement( // Cell is occupied if ((range < max_r) && (abs(r - range) < (alpha / 2.0f))) { - std::cout << "Occupied" << std::endl; occ_prob = 0.7f; } // Cell is free else if (r <= range) { - std::cout << "Free" << std::endl; occ_prob = 0.3f; } @@ -706,14 +672,14 @@ std::vector LifelongSlamToolbox::calculateIntersection( if (den == 0.0f) { // Parallel lines or not intersection at all - std::cout << " -Parallels- " << std::endl; + // std::cout << " -Parallels- " << std::endl; return {}; } else { float x = ((x2*y1 - x1*y2)*(x4 - x3) - (x4*y3 - x3*y4)*(x2-x1)) / den; float y = ((x2*y1 - x1*y2)*(y4 - y3) - (x4*y3 - x3*y4)*(y2-y1)) / den; - std::cout << "Intersection point: "<< x << ", " << y << std::endl; + // std::cout << "Intersection point: "<< x << ", " << y << std::endl; return {x, y}; } } From 116abf7aeaa8208f941637821a5d88d8f211eb87 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Tue, 30 Nov 2021 11:24:00 -0500 Subject: [PATCH 23/83] [WIP]: Final adjustments --- src/experimental/slam_toolbox_lifelong.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 08767975e..a29c4e63f 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -160,19 +160,14 @@ void LifelongSlamToolbox::scannerTest() std::vector> robot_poses {{5.6f, 6.0f, PI/2}, {5.6f, 6.0f, PI/2}}; std::vector robot_pose{5.6f, 6.0f, PI/2}; - std::vector ranges{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters - // std::vector angles{-0.43633f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; - // std::vector ranges{1.65f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters + std::vector ranges{1.65f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters + // Angles will be -50, -25, 0, 25, 50 //----// +-50 = +-0.87266 : +-25 = +-0.43633 std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; - // std::vector angles{0.43633f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // This is the initial point std::vector robot_grid_pos = getGridPosition(robot_pose[0], robot_pose[1], resolution); std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; - // Creating the laser scan with 5 beams ------- Angles will be -50, -25, 0, 25, 50 //----// +-50 = +-0.87266 : +-25 = +-0.43633 - // std::vector ranges{2.8f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters - // Current yaw + beam angle: -PI/2 (-1.570795) -0.87266 = 2.44345 (-55 degrees) for (int i = 0; i < ranges.size(); ++i) // for (int i = 0; i < 1; ++i) // One reading only From 5f88cd6fb7cfbe13b61980f14864fe55e74c3005 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Tue, 7 Dec 2021 11:26:17 -0500 Subject: [PATCH 24/83] [ADD]: Adding measurement outcome entropy (3.12) calculation --- .../experimental/slam_toolbox_lifelong.hpp | 62 ++++++----- src/experimental/slam_toolbox_lifelong.cpp | 102 ++++++++++++------ 2 files changed, 104 insertions(+), 60 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index f81ae01e1..4d13db6c4 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -48,6 +48,30 @@ class LifelongSlamToolbox : public SlamToolbox LocalizedRangeScan * s1, LocalizedRangeScan * s2, double & x_l, double & x_u, double & y_l, double & y_u); +public: + // Struct for cell occupancy + struct Occupancy + { + int fr, oc ,un; + + bool operator==(Occupancy const& st) const + { + return (st.fr == fr) && (st.oc == oc) && (st.un == un); + } + + struct CombinationsHash + { + std::size_t operator()(Occupancy const& key) const + { + std::size_t hash = 5381u; + hash = (hash << 5) + hash + key.fr; + hash = (hash << 5) + hash + key.oc; + hash = (hash << 5) + hash + key.un; + return hash; + } + }; + }; + protected: void laserCallback( sensor_msgs::msg::LaserScan::ConstSharedPtr scan) override; @@ -79,13 +103,20 @@ class LifelongSlamToolbox : public SlamToolbox std::vector getCellPosition(std::vector grid_cell, float resolution); void inverseMeasurement(std::vector>& grid_prob, std::vector>& grid_logs, std::vector>& grid_etp, std::vector& cells_x, std::vector& cells_y, std::vector& robot_grid_pos, float range, float angle, float resolution); void updateCellProbability(std::vector>& grid_prob, float probability, int cell_x, int cell_y); - float calculateLogs(float probability); void updateCellLogs(std::vector>& grid_prob, std::vector>& grid_logs, int cell_x, int cell_y, float initial_log); - float probabilityFromLog(float log); // Recovering the probability float entropyFromProbability(float prob); void updateCellEntropy(std::vector>& grid_etp, float cell_x, float cell_y, float entropy); float calculateMapEntropy(std::vector>& grid_etp); + // For mutual information + + // Mutual information provisional approach + float measurementOutcomeEntropy(Occupancy const& meas_outcome); + void recoverProbability(); + float calculateLogs(float probability); + float probabilityFromLogs(float log); + float calculateEntropy(float probability); + // For algorithm 1 void appendCellProbabilities(std::vector& meas_outcomes); void computeProbabilities(std::vector>& meas_outcm); @@ -93,33 +124,10 @@ class LifelongSlamToolbox : public SlamToolbox std::vector unhashIndex(int hash); // Data structures - // Keep track of the probabilities for each cell + std::unordered_map m_un_cmb; std::map, std::vector>> m_cell_probabilities; int m_cell_x; - int m_cell_y; - - // Struct for cell occupancy - struct Occupancy - { - int fr, oc ,un; - - bool operator==(Occupancy const& st) const - { - return (st.fr == fr) && (st.oc == oc) && (st.un == un); - } - - struct CombinationsHash - { - std::size_t operator()(Occupancy const& key) const - { - std::size_t hash = 5381u; - hash = (hash << 5) + hash + key.fr; - hash = (hash << 5) + hash + key.oc; - hash = (hash << 5) + hash + key.un; - return hash; - } - }; - }; + int m_cell_y; /*****************************************************************************/ bool use_tree_; diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index a29c4e63f..9d1e37057 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -149,14 +149,14 @@ void LifelongSlamToolbox::scannerTest() grid[i][j] = 0; grid_prob[i][j] = unknown_prob; grid_logs[i][j] = initial_log; - grid_etp[i][j] = entropyFromProbability(probabilityFromLog(grid_logs[i][j])); + grid_etp[i][j] = entropyFromProbability(probabilityFromLogs(grid_logs[i][j])); } } // I will have 5 lasers for each reading (Located at 0, +-25, +-50) // std::vector robot_pose{5.6f, 6.0f, PI/2}; // std::vector robot_pose{5.6f, 6.0f, PI/4}; - + std::vector> robot_poses {{5.6f, 6.0f, PI/2}, {5.6f, 6.0f, PI/2}}; std::vector robot_pose{5.6f, 6.0f, PI/2}; @@ -295,7 +295,7 @@ void LifelongSlamToolbox::scannerTest() } // When a cell is marked by Bresenham but there is not intersection points - if (inter_x.size() == 0) + if (inter_x.size() == 0) continue; // Enter (d1) and Exit (d2) distances @@ -340,23 +340,47 @@ void LifelongSlamToolbox::scannerTest() std::vector> meas_outcomes = retreiveMeasurementOutcomes(); // Compute all the possible combinations - computeProbabilities(meas_outcomes); + computeProbabilities(meas_outcomes); // Algorithm 1 + + std::unordered_map::iterator it_show; + std::cout << "Measurement outcomes size: " << meas_outcomes.size() << std::endl; - /* - Note for entropy - I am not sure if the paper is also writing it wrong, but fot all the possible measurement outcomes, - I do have the probability (Also the cell combinations) which is the one I can use to get the entropy. + float sum_carry = 0.0f; + + for (it_show = m_un_cmb.begin(); it_show != m_un_cmb.end(); ++it_show) + { + // Interested in the final measurement outcomes + if (it_show->first.fr + it_show->first.oc + it_show->first.un == meas_outcomes.size()) + { + // Sum calculation from 3.12 + sum_carry += it_show->second * measurementOutcomeEntropy(it_show->first); + } + } - Also need to add new elements to one cell - This would be the next step. - */ + std::cout << sum_carry << std::endl; - //std::cout << "Probabilities: " << probabilities[0] << ", " << probabilities[1] << ", " << probabilities[2] << std::endl; std::cout << "++++++++++++++++++++++++" << std::endl; } } } +float LifelongSlamToolbox::measurementOutcomeEntropy(Occupancy const& meas_outcome) +{ + /* + To calculate the measurement outcome entropy (Measurement outcome in the form ) + */ + float cell_logs = (meas_outcome.fr * calculateLogs(0.3f)) + (meas_outcome.oc * calculateLogs(0.7f)) + (meas_outcome.un * calculateLogs(0.5f)); + return calculateEntropy(probabilityFromLogs(cell_logs)); +} + +float LifelongSlamToolbox::calculateEntropy(float probability) +{ + /* + To calculate the entropy + */ + return probability * log(probability); +} + std::vector> LifelongSlamToolbox::retreiveMeasurementOutcomes() { /* @@ -384,7 +408,7 @@ std::vector> LifelongSlamToolbox::retreiveMeasurementOutcomes void LifelongSlamToolbox::appendCellProbabilities(std::vector& meas_outcomes) { /* - To append a new measurement outcome for a specific cell + To append a new measurement for a specific cell */ // Iterator for getting the cell @@ -462,14 +486,16 @@ void LifelongSlamToolbox::inverseMeasurement( // Update the probability updateCellProbability(grid_prob, occ_prob, cells_x[i], cells_y[i]); - // Update the log-odds updateCellLogs(grid_prob, grid_logs, cells_x[i], cells_y[i], 0.0f); // Log-odds to probability - float entropy = entropyFromProbability(probabilityFromLog(grid_logs[cells_x[i]][cells_y[i]])); + float entropy = entropyFromProbability(probabilityFromLogs(grid_logs[cells_x[i]][cells_y[i]])); + // Update the entropy updateCellEntropy(grid_etp, cells_x[i], cells_y[i], entropy); + + // std::cout << "Probability: " << grid_prob[cells_x[i]][cells_y[i]] << std::endl; // std::cout << "Log-Odds: " << grid_logs[cells_x[i]][cells_y[i]] << std::endl; // std::cout << "Entropy: " << grid_etp[cells_x[i]][cells_y[i]] << std::endl; @@ -485,8 +511,16 @@ void LifelongSlamToolbox::computeProbabilities(std::vector>& /* To compute all the possible combinations of a grid cell, given a set of measurement outcomes */ - int k = meas_outcm.size(); - std::cout << "K: " << k << std::endl; + + /* + 1 Measurement will add one level + + + */ + + m_un_cmb.clear(); + int k = meas_outcm.size(); // The number of measurements + // std::cout << "K: " << k << std::endl; int r = 1; @@ -494,14 +528,14 @@ void LifelongSlamToolbox::computeProbabilities(std::vector>& float p_occ = meas_outcm[0][1]; float p_un = meas_outcm[0][0]; - std::unordered_map un_cmb; + // std::unordered_map m_un_cmb; std::unordered_map::iterator it_un; // Initial node - un_cmb.insert(std::pair({0, 0, 0}, 1.0f)); - un_cmb.insert(std::pair({0, 0, 1}, p_free)); - un_cmb.insert(std::pair({0, 1, 0}, p_occ)); - un_cmb.insert(std::pair({1, 0, 0}, p_un)); + m_un_cmb.insert(std::pair({0, 0, 0}, 1.0f)); + m_un_cmb.insert(std::pair({0, 0, 1}, p_free)); + m_un_cmb.insert(std::pair({0, 1, 0}, p_occ)); + m_un_cmb.insert(std::pair({1, 0, 0}, p_un)); for (int i = r; r < k; ++r) { @@ -510,7 +544,7 @@ void LifelongSlamToolbox::computeProbabilities(std::vector>& std::vector occ_vct; std::vector acc_prob; - for (it_un = un_cmb.begin(); it_un != un_cmb.end(); ++it_un) + for (it_un = m_un_cmb.begin(); it_un != m_un_cmb.end(); ++it_un) { // Current combination state int fr_idx = it_un->first.fr; @@ -571,15 +605,16 @@ void LifelongSlamToolbox::computeProbabilities(std::vector>& // Inserting the elements into the map for (int k = 0; k < occ_vct.size(); ++k) { - un_cmb.insert(std::pair(occ_vct[k], acc_prob[k])); + m_un_cmb.insert(std::pair(occ_vct[k], acc_prob[k])); } } - std::unordered_map::iterator it_show; - for (it_show = un_cmb.begin(); it_show != un_cmb.end(); ++it_show) - { - std::cout << it_show->first.fr << ", " << it_show->first.oc << ", " << it_show->first.un << ", " << it_show->second <::iterator it_show; + // for (it_show = m_un_cmb.begin(); it_show != m_un_cmb.end(); ++it_show) + // { + // std::cout << it_show->first.fr << ", " << it_show->first.oc << ", " << it_show->first.un << ", " << it_show->second <>& grid_etp, float cell_x, float cell_y, float entropy) @@ -595,12 +630,13 @@ float LifelongSlamToolbox::entropyFromProbability(float prob) { /* To calculate the cell entropy from the probability - - Entropy = (p*log(p) + (1-p)*log(1-p)); + - Entropy = (p*log(p)); + - Entropy_Old = (p*log(p) + (1-p)*log(1-p)); */ - return (prob * log(prob)) * ((1 - prob) * log(1 - prob)); + return (prob * log(prob)); } -float LifelongSlamToolbox::probabilityFromLog(float log) +float LifelongSlamToolbox::probabilityFromLogs(float log) { /* To transform the Log-odds into probability @@ -715,7 +751,7 @@ float LifelongSlamToolbox::calculateProbability(float range_1, float range_2) range_1 = (range_1 > max_range) ? max_range : range_1; range_2 = (range_2 > max_range) ? max_range : range_2; - + // https://www.wolframalpha.com/input/?i2d=true&i=Integrate%5Bn*c*Power%5Be%2C-c*x%5D%2C%7Bx%2Ca%2Cb%7D%5D return nu * (exp(-lambda*range_1) - exp(-lambda*range_2)); } From b5078421e95cf0bbd87d366a06146f7319f2aa38 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Thu, 9 Dec 2021 12:05:16 -0500 Subject: [PATCH 25/83] [ADD]: Map mutual information calculation and code structure, it is way clear now comparing to the last modification. --- .../experimental/slam_toolbox_lifelong.hpp | 46 +- src/experimental/slam_toolbox_lifelong.cpp | 404 +++++++----------- 2 files changed, 184 insertions(+), 266 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index 4d13db6c4..0b0d28970 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -49,11 +49,10 @@ class LifelongSlamToolbox : public SlamToolbox double & x_u, double & y_l, double & y_u); public: - // Struct for cell occupancy + // Cell occupancy struct for unordered map struct Occupancy { int fr, oc ,un; - bool operator==(Occupancy const& st) const { return (st.fr == fr) && (st.oc == oc) && (st.un == un); @@ -90,34 +89,40 @@ class LifelongSlamToolbox : public SlamToolbox void updateScoresSlamGraph(const double & score, Vertex * vertex); void checkIsNotNormalized(const double & value); - /*****************************************************************************/ /*******************************Implementation********************************/ - std::vector getGridPosition(float x, float y, float resolution); - std::vector getLaserHit(std::vector const& robot_pose, float range, float angle); + void scannerTest(); + + // Grid operations + void initializeGrids(); + void calculateLimits( + std::vector & initial_x, std::vector & initial_y, std::vector & final_x, std::vector & final_y, + float & limit_x, float & limit_y, float & min_x, float & max_x, float & min_y, float & max_y, std::vector & robot_grid_pos, + std::vector & final_grid_pos); + + // Grid and position information std::pair, std::vector> Bresenham(int x_1, int y_1, int x_2, int y_2); + std::vector getGridPosition(float x, float y); + std::vector getLaserHit(std::vector const& robot_pose, float range, float angle); std::vector calculateIntersection(std::vector laser_start, std::vector laser_end, std::vector cell_start, std::vector cell_end); int getSign(int n_1, int n_2); + + // Measurements calculations float calculateProbability(float range_1, float range_2); float calculateDistance(float x_1, float y_1, float x_2, float y_2); - void scannerTest(); - std::vector getCellPosition(std::vector grid_cell, float resolution); - void inverseMeasurement(std::vector>& grid_prob, std::vector>& grid_logs, std::vector>& grid_etp, std::vector& cells_x, std::vector& cells_y, std::vector& robot_grid_pos, float range, float angle, float resolution); - void updateCellProbability(std::vector>& grid_prob, float probability, int cell_x, int cell_y); - void updateCellLogs(std::vector>& grid_prob, std::vector>& grid_logs, int cell_x, int cell_y, float initial_log); float entropyFromProbability(float prob); - void updateCellEntropy(std::vector>& grid_etp, float cell_x, float cell_y, float entropy); - float calculateMapEntropy(std::vector>& grid_etp); - // For mutual information - - // Mutual information provisional approach + // Mutual information float measurementOutcomeEntropy(Occupancy const& meas_outcome); void recoverProbability(); float calculateLogs(float probability); float probabilityFromLogs(float log); float calculateEntropy(float probability); - // For algorithm 1 + float calculateMapMutualInformation(); + void updateCellMutualInformation(float mut_inf_val); + + + // Measurement outcomes probabilities void appendCellProbabilities(std::vector& meas_outcomes); void computeProbabilities(std::vector>& meas_outcm); std::vector> retreiveMeasurementOutcomes(); @@ -126,10 +131,17 @@ class LifelongSlamToolbox : public SlamToolbox // Data structures std::unordered_map m_un_cmb; std::map, std::vector>> m_cell_probabilities; + std::vector> m_mutual_grid; + + float m_map_dist; + float m_resolution; int m_cell_x; int m_cell_y; - /*****************************************************************************/ + int m_num_cells; + std::vector> m_grid; + + /*****************************************************************************/ bool use_tree_; double iou_thresh_; double removal_score_; diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 9d1e37057..0e63e0983 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -83,6 +83,15 @@ LifelongSlamToolbox::LifelongSlamToolbox(rclcpp::NodeOptions options) // in lifelong mode, we cannot have interactive mode enabled enable_interactive_mode_ = false; + m_resolution = 0.5f; // Map resolution + m_map_dist = 20.0f; // Total map distance + m_num_cells = static_cast(m_map_dist / m_resolution); + + // Grids initialization (Occupancy and Mutual information) + m_grid.resize(m_num_cells); + m_mutual_grid.resize(m_num_cells); + initializeGrids(); + scannerTest(); } @@ -126,51 +135,21 @@ void LifelongSlamToolbox::scannerTest() { RCLCPP_WARN(get_logger(), "<-------- Scanner Test -------->"); - float resolution = 0.5f; // Cell resolution - 0.5m - float map_dist = 20.0f; // Total map distance - int number_cells = map_dist / resolution; - - float unknown_prob = 0.5f; - float initial_log = calculateLogs(unknown_prob); - std::vector> grid(number_cells); // 20 meters in Y dimension - std::vector> grid_prob(number_cells); // 20 meters in Y dimension - std::vector> grid_logs(number_cells); // 20 meters in Y dimension - std::vector> grid_etp(number_cells); // 20 meters in Y dimension - - - for (int i=0; i robot_pose{5.6f, 6.0f, PI/2}; - // std::vector robot_pose{5.6f, 6.0f, PI/4}; - std::vector> robot_poses {{5.6f, 6.0f, PI/2}, {5.6f, 6.0f, PI/2}}; std::vector robot_pose{5.6f, 6.0f, PI/2}; std::vector ranges{1.65f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters - // Angles will be -50, -25, 0, 25, 50 //----// +-50 = +-0.87266 : +-25 = +-0.43633 + + // Angles will be -50, -25, 0, 25, 50 std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // This is the initial point - std::vector robot_grid_pos = getGridPosition(robot_pose[0], robot_pose[1], resolution); + std::vector robot_grid_pos = getGridPosition(robot_pose[0], robot_pose[1]); std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; // Current yaw + beam angle: -PI/2 (-1.570795) -0.87266 = 2.44345 (-55 degrees) for (int i = 0; i < ranges.size(); ++i) - // for (int i = 0; i < 1; ++i) // One reading only { std::cout << "........ New laser ........" << std::endl; std::cout << "Distance: " << ranges[i] << ", Angle: " << angles[i] << std::endl; @@ -179,15 +158,14 @@ void LifelongSlamToolbox::scannerTest() std::vector laser_grid = getLaserHit(robot_pose, ranges[i], angles[i]); // Laser final cell - std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1], resolution); + std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1]); // robot_grid_pos[0] // X1 - robot_grid_pos[1] // Y1 // final_grid_pos[0] // X2 - final_grid_pos[1] // Y2 + // Ray tracing for getting the visited cells std::vector cells_x, cells_y; std::pair, std::vector> res_pair = Bresenham(robot_grid_pos[0], robot_grid_pos[1], final_grid_pos[0], final_grid_pos[1]); - - // Cells visited by this laser beam cells_x = res_pair.first; cells_y = res_pair.second; @@ -207,74 +185,35 @@ void LifelongSlamToolbox::scannerTest() { if ((robot_grid_pos[0] == cells_x[j]) && (robot_grid_pos[1] == cells_y[j])) { + // Current cell is not relevant at all continue; } // Cells visualization std::cout << "Current cell: " << cells_x[j] << ", " << cells_y[j] << std::endl; - float limit_x = cells_x[j] * resolution; - float limit_y = cells_y[j] * resolution; - - std::vector initial_x {limit_x, limit_x, limit_x + resolution, limit_x + resolution}; - std::vector initial_y {limit_y, limit_y, limit_y + resolution, limit_y + resolution}; - - std::vector final_x {limit_x + resolution, limit_x, limit_x + resolution, limit_x}; - std::vector final_y {limit_y, limit_y + resolution, limit_y, limit_y + resolution}; + // Cells limits + float limit_x = cells_x[j] * m_resolution; + float limit_y = cells_y[j] * m_resolution; float min_x = limit_x; - float max_x = limit_x + resolution; + float max_x = limit_x + m_resolution; float min_y = limit_y; - float max_y = limit_y + resolution; - - if (final_grid_pos[0] < robot_grid_pos[0] && final_grid_pos[1] >= robot_grid_pos[1]) - { - // X greater and Y greater. WRO final points - final_x[0] = limit_x + resolution; - final_x[2] = limit_x + resolution; + float max_y = limit_y + m_resolution; - min_y = limit_y; - max_y = limit_y + resolution; - } - - if (final_grid_pos[0] >= robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) - { - // X greater and Y minor. WRO final points - initial_y[2] = limit_y - resolution; - initial_y[3] = limit_y - resolution; - - final_y[1] = limit_y - resolution; - final_y[3] = limit_y - resolution; + std::vector initial_x {limit_x, limit_x, limit_x + m_resolution, limit_x + m_resolution}; + std::vector initial_y {limit_y, limit_y, limit_y + m_resolution, limit_y + m_resolution}; - min_y = limit_y - resolution; - max_y = limit_y; - } + std::vector final_x {limit_x + m_resolution, limit_x, limit_x + m_resolution, limit_x}; + std::vector final_y {limit_y, limit_y + m_resolution, limit_y, limit_y + m_resolution}; - if (final_grid_pos[0] < robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) - { - // X minor and Y minor. WRO final points - initial_x[2] = limit_x - resolution; - initial_x[3] = limit_x - resolution; - initial_y[2] = limit_y - resolution; - initial_y[3] = limit_y - resolution; - - final_x[0] = limit_x - resolution; - final_x[2] = limit_x - resolution; - final_y[1] = limit_y - resolution; - final_y[3] = limit_y - resolution; - - min_x = limit_x - resolution; - max_x = limit_x; - min_y = limit_y - resolution; - max_y = limit_y; - } + calculateLimits(initial_x, initial_y, final_x, final_y, limit_x, limit_y, min_x, max_x, min_y, max_y, robot_grid_pos, final_grid_pos); std::vector inter_x, inter_y; for (int k = 0; k < 4; ++k) { std::vector intersection = calculateIntersection(robot_pose, laser_grid, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); - if(intersection.size() != 0) { // If the laser and a cell intersects, we need to make sure it happens in the right bounds @@ -302,15 +241,8 @@ void LifelongSlamToolbox::scannerTest() std::vector distances; for (int k = 0; k < inter_x.size(); ++k) { - /* - This could be a ring buffer - Actually some elements can be a ring buffer - dist_point[0]: d1 - dist_point[1]: d2 - */ float dist_point = calculateDistance(robot_pose[0], robot_pose[1], inter_x[k], inter_y[k]); distances.push_back(dist_point); - - // std::cout << "Distance: " << dist_point << std::endl; } // Integral 1: 0 to d1 which is distance from robot pose to first point where the cell is cut @@ -318,15 +250,11 @@ void LifelongSlamToolbox::scannerTest() // d2 which is distance from robot pose to second point where the cell is cut // Integral 3: d2 which is distance from robot pose to second point where the cell is cut to z_max - // float prob_not = calculateProbability(0.0f, distances[0]); // 3.8 - Does not oberserve - // float prob_occ = calculateProbability(distances[0], distances[1]); // 3.9 - Occupied - // float prob_free = calculateProbability(distances[1], 5.0f); // 3.10 - Free - // Measurement outcomes vector std::vector probabilities { - calculateProbability(distances[1], 5.0f), - calculateProbability(distances[0], distances[1]), - calculateProbability(0.0f, distances[0]) + calculateProbability(distances[1], 5.0f), // Free + calculateProbability(distances[0], distances[1]), // Occupied + calculateProbability(0.0f, distances[0]) // Not observed }; // Assigning the cells @@ -336,32 +264,136 @@ void LifelongSlamToolbox::scannerTest() // Appending new measurement outcomes for the current cell appendCellProbabilities(probabilities); + // From this point the approximation should be done in a different function // Get all the measurement outcomes for the current cell std::vector> meas_outcomes = retreiveMeasurementOutcomes(); - - // Compute all the possible combinations - computeProbabilities(meas_outcomes); // Algorithm 1 - - std::unordered_map::iterator it_show; - std::cout << "Measurement outcomes size: " << meas_outcomes.size() << std::endl; - - float sum_carry = 0.0f; - - for (it_show = m_un_cmb.begin(); it_show != m_un_cmb.end(); ++it_show) + // Compute all the possible combinations for the current cell - algorithm 1 + computeProbabilities(meas_outcomes); + + // Calculate 3.12 + std::unordered_map::iterator it_mutual; + std::cout << "Number of measurements: " << meas_outcomes.size() << std::endl; + float cell_mutual_inf = 0.0f; + for (it_mutual = m_un_cmb.begin(); it_mutual != m_un_cmb.end(); ++it_mutual) { // Interested in the final measurement outcomes - if (it_show->first.fr + it_show->first.oc + it_show->first.un == meas_outcomes.size()) + if (it_mutual->first.fr + it_mutual->first.oc + it_mutual->first.un == meas_outcomes.size()) { - // Sum calculation from 3.12 - sum_carry += it_show->second * measurementOutcomeEntropy(it_show->first); + cell_mutual_inf += it_mutual->second * measurementOutcomeEntropy(it_mutual->first); } } - std::cout << sum_carry << std::endl; - + /* + At this point I would need to calculate the H(C) as it stills and icognite here + + - I need to separate here the calculations. Which means Entropy and Mutual Information + must be calculated outside this loop + - Start documenting this section + - I need to add more poses and more ranges in order to see mutual information impact + */ + std::cout << cell_mutual_inf << std::endl; + updateCellMutualInformation(cell_mutual_inf); std::cout << "++++++++++++++++++++++++" << std::endl; } } + float mutual = calculateMapMutualInformation(); + std::cout << "Mutual information: " << mutual << std::endl; +} + +void LifelongSlamToolbox::updateCellMutualInformation(float mut_inf_val) +{ + /* + To update the mutual information for each individual cell + This is the result of the summation of 3.12 + */ + // std::cout << "Updating cell: " << m_cell_x << ", " << m_cell_y << " , with: " << mut_inf_val << std::endl; + m_mutual_grid[m_cell_x][m_cell_x] = mut_inf_val; +} + +float LifelongSlamToolbox::calculateMapMutualInformation() +{ + /* + To calculate map mutual information, this is the summation + of all cells mutual information + */ + float sum = 0.0f; + for (int i = 0; i < m_num_cells; ++i) + { + for (int j = 0; j < m_num_cells; ++j) + { + sum += m_mutual_grid[i][j]; + } + } + return sum; +} + +void LifelongSlamToolbox::calculateLimits( + std::vector & initial_x, std::vector & initial_y, std::vector & final_x, std::vector & final_y, + float & limit_x, float & limit_y, float & min_x, float & max_x, float & min_y, float & max_y, std::vector & robot_grid_pos, + std::vector & final_grid_pos) +{ + /* + To calculate grid grid limits for intersection + */ + if (final_grid_pos[0] < robot_grid_pos[0] && final_grid_pos[1] >= robot_grid_pos[1]) + { + // X greater and Y greater. WRO final points + final_x[0] = limit_x + m_resolution; + final_x[2] = limit_x + m_resolution; + + min_y = limit_y; + max_y = limit_y + m_resolution; + } + + if (final_grid_pos[0] >= robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) + { + // X greater and Y minor. WRO final points + initial_y[2] = limit_y - m_resolution; + initial_y[3] = limit_y - m_resolution; + + final_y[1] = limit_y - m_resolution; + final_y[3] = limit_y - m_resolution; + + min_y = limit_y - m_resolution; + max_y = limit_y; + } + + if (final_grid_pos[0] < robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) + { + // X minor and Y minor. WRO final points + initial_x[2] = limit_x - m_resolution; + initial_x[3] = limit_x - m_resolution; + initial_y[2] = limit_y - m_resolution; + initial_y[3] = limit_y - m_resolution; + + final_x[0] = limit_x - m_resolution; + final_x[2] = limit_x - m_resolution; + final_y[1] = limit_y - m_resolution; + final_y[3] = limit_y - m_resolution; + + min_x = limit_x - m_resolution; + max_x = limit_x; + min_y = limit_y - m_resolution; + max_y = limit_y; + } +} + +void LifelongSlamToolbox::initializeGrids() +{ + /* + To create the grid + */ + for (int i = 0; i < m_num_cells; ++i) + { + // Adding columns + m_grid[i].resize(m_num_cells); + m_mutual_grid[i].resize(m_num_cells); + for (int j = 0; j < m_num_cells; ++j) + { + m_grid[i][j] = 0; + m_mutual_grid[i][j] = 0.0f; + } + } } float LifelongSlamToolbox::measurementOutcomeEntropy(Occupancy const& meas_outcome) @@ -387,7 +419,6 @@ std::vector> LifelongSlamToolbox::retreiveMeasurementOutcomes To get all the measurement outcomes for the current cell */ - // Iterator for getting the cell std::map, std::vector>>::iterator it_cells; it_cells = m_cell_probabilities.find({m_cell_x, m_cell_y}); @@ -430,94 +461,12 @@ void LifelongSlamToolbox::appendCellProbabilities(std::vector& meas_outco } } -float LifelongSlamToolbox::calculateMapEntropy(std::vector>& grid_etp) -{ - /* - To calculate the map entropy - */ - float map_etp = 0.0f; - for (auto vct : grid_etp) - { - std::for_each(vct.begin(), vct.end(), [&] (float entropy) { - map_etp += entropy; - }); - } - return map_etp; -} - -void LifelongSlamToolbox::inverseMeasurement( - std::vector>& grid_prob, - std::vector>& grid_logs, - std::vector>& grid_etp, - std::vector& cells_x, - std::vector& cells_y, - std::vector& robot_grid_pos, - float range, float angle, float resolution) -{ - /* - On this function we update a probability map based - We capture the cells that are hitted by the ray and the conditions - And we update the probability as being occupied or free - */ - - float alpha = 1.0f; - float max_r = 5.0f; - - for (int i = 0; i < cells_x.size(); ++i) // One reading only - { - std::cout << "Cells: " << cells_x[i] << ", " << cells_y[i] << std::endl; - // This ditances will be calculated to the center of mass - // mxi - pos_x //--// myi - pos_y - float dx = (cells_x[i] - robot_grid_pos[0]) * resolution; - float dy = (cells_y[i] - robot_grid_pos[1]) * resolution; - float r = sqrt(pow(dx , 2) + pow(dy, 2)); - float occ_prob = 0.5f; // Unkwnown - - // Cell is occupied - if ((range < max_r) && (abs(r - range) < (alpha / 2.0f))) - { - occ_prob = 0.7f; - } - // Cell is free - else if (r <= range) - { - occ_prob = 0.3f; - } - - // Update the probability - updateCellProbability(grid_prob, occ_prob, cells_x[i], cells_y[i]); - // Update the log-odds - updateCellLogs(grid_prob, grid_logs, cells_x[i], cells_y[i], 0.0f); - // Log-odds to probability - float entropy = entropyFromProbability(probabilityFromLogs(grid_logs[cells_x[i]][cells_y[i]])); - - // Update the entropy - updateCellEntropy(grid_etp, cells_x[i], cells_y[i], entropy); - - - - // std::cout << "Probability: " << grid_prob[cells_x[i]][cells_y[i]] << std::endl; - // std::cout << "Log-Odds: " << grid_logs[cells_x[i]][cells_y[i]] << std::endl; - // std::cout << "Entropy: " << grid_etp[cells_x[i]][cells_y[i]] << std::endl; - - std::cout << "Relative range: " << r << std::endl; - std::cout << "++++++++++++++++++++++++ " << std::endl; - } -} - - void LifelongSlamToolbox::computeProbabilities(std::vector>& meas_outcm) { /* To compute all the possible combinations of a grid cell, given a set of measurement outcomes */ - /* - 1 Measurement will add one level - - - */ - m_un_cmb.clear(); int k = meas_outcm.size(); // The number of measurements // std::cout << "K: " << k << std::endl; @@ -608,22 +557,6 @@ void LifelongSlamToolbox::computeProbabilities(std::vector>& m_un_cmb.insert(std::pair(occ_vct[k], acc_prob[k])); } } - - // For displaying - // std::unordered_map::iterator it_show; - // for (it_show = m_un_cmb.begin(); it_show != m_un_cmb.end(); ++it_show) - // { - // std::cout << it_show->first.fr << ", " << it_show->first.oc << ", " << it_show->first.un << ", " << it_show->second <>& grid_etp, float cell_x, float cell_y, float entropy) -{ - /* - To update the cell entropy - - H = H - Entropy; - */ - grid_etp[cell_x][cell_y] = grid_etp[cell_x][cell_y] - entropy; } float LifelongSlamToolbox::entropyFromProbability(float prob) @@ -644,22 +577,6 @@ float LifelongSlamToolbox::probabilityFromLogs(float log) return (exp(log) / (1 + exp(log))); } -void LifelongSlamToolbox::updateCellLogs(std::vector>& grid_prob, std::vector>& grid_logs, int cell_x, int cell_y, float initial_log) -{ - /* - To update the log-odds matrix - */ - grid_logs[cell_x][cell_y] = grid_logs[cell_x][cell_y] + calculateLogs(grid_prob[cell_x][cell_y]) - initial_log; -} - -void LifelongSlamToolbox::updateCellProbability(std::vector>& grid_prob, float probability, int cell_x, int cell_y) -{ - /* - To perform the probability update at the given cell - */ - grid_prob[cell_x][cell_y] = probability; -} - float LifelongSlamToolbox::calculateLogs(float probability) { /* @@ -668,17 +585,6 @@ float LifelongSlamToolbox::calculateLogs(float probability) return log(probability / (1 - probability)); } -std::vector LifelongSlamToolbox::getCellPosition(std::vector grid_cell, float resolution) -{ - /* - To get the current cell - */ - float x = (grid_cell[0] * resolution) + (resolution / 2); - float y = (grid_cell[1] * resolution) + (resolution / 2); - - return {x, y}; -} - std::vector LifelongSlamToolbox::calculateIntersection( std::vector laser_start, std::vector laser_end, std::vector cell_start, std::vector cell_end) @@ -718,7 +624,7 @@ std::vector LifelongSlamToolbox::calculateIntersection( float LifelongSlamToolbox::calculateDistance(float x_1, float y_1, float x_2, float y_2) { /* - Calculates the euclidean distance between two points + To calculate the euclidean distance between two points */ float diff_x = x_2 - x_1; float diff_y = y_2 - y_1; @@ -756,13 +662,13 @@ float LifelongSlamToolbox::calculateProbability(float range_1, float range_2) return nu * (exp(-lambda*range_1) - exp(-lambda*range_2)); } -std::vector LifelongSlamToolbox::getGridPosition(float x, float y, float resolution) +std::vector LifelongSlamToolbox::getGridPosition(float x, float y) { /* - Maps the distance into grid coordinates + To maps the current position into grid coordinates */ - int x_cell = floor((1 / resolution) * x); - int y_cell = floor((1 / resolution) * y); + int x_cell = floor((1 / m_resolution) * x); + int y_cell = floor((1 / m_resolution) * y); return {x_cell, y_cell}; } @@ -770,7 +676,7 @@ std::vector LifelongSlamToolbox::getGridPosition(float x, float y, float re std::vector LifelongSlamToolbox::getLaserHit(std::vector const& robot_pose, float range, float angle) { /* - Returns the distance where the laser beam hits something + To get the distance where the laser beam hits something Rigid Body Trasnformation from the global to the sensor frame */ float x_tf = (range * cos(robot_pose[2] + angle)) + robot_pose[0]; @@ -782,7 +688,7 @@ std::vector LifelongSlamToolbox::getLaserHit(std::vector const& ro std::pair, std::vector> LifelongSlamToolbox::Bresenham(int x_1, int y_1, int x_2, int y_2) { /* - Returns the set of cells hit by a laser beam + To find the set of cells hit by a laser beam */ std::vector x_bres; std::vector y_bres; @@ -845,7 +751,7 @@ std::pair, std::vector> LifelongSlamToolbox::Bresenham(int int LifelongSlamToolbox::getSign(int n_1, int n_2) { /* - Returns the sign of an operation, used for Bresenham algorithm + To get the sign of an operation, used for Bresenham algorithm */ int difference = n_2 - n_1; From 384a0b5f7c6dde4248ef5e41445093f0d04ec1c3 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Fri, 10 Dec 2021 06:58:57 -0500 Subject: [PATCH 26/83] [WIP]: Adding new robot pose for mutual information verification --- .../experimental/slam_toolbox_lifelong.hpp | 12 +- src/experimental/slam_toolbox_lifelong.cpp | 268 +++++++++--------- 2 files changed, 141 insertions(+), 139 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index 0b0d28970..bcf1fdc69 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -22,6 +22,8 @@ #include #include "slam_toolbox/slam_toolbox_common.hpp" +#define PI 3.14159265 + namespace slam_toolbox { @@ -121,7 +123,6 @@ class LifelongSlamToolbox : public SlamToolbox float calculateMapMutualInformation(); void updateCellMutualInformation(float mut_inf_val); - // Measurement outcomes probabilities void appendCellProbabilities(std::vector& meas_outcomes); void computeProbabilities(std::vector>& meas_outcm); @@ -132,14 +133,19 @@ class LifelongSlamToolbox : public SlamToolbox std::unordered_map m_un_cmb; std::map, std::vector>> m_cell_probabilities; std::vector> m_mutual_grid; - + std::vector> m_grid; float m_map_dist; float m_resolution; int m_cell_x; int m_cell_y; int m_num_cells; - std::vector> m_grid; + // Robot information - Defining values just for testing + std::vector> robot_poses {{5.6f, 6.0f, PI/2}, {3.5f, 9.0f, 0.0f}}; + std::vector> laser_ranges {{1.65f, 5.0f, 5.0f, 5.0f, 5.0f}, {5.0f, 5.0f, 4.0f, 5.0f, 5.0f}}; + std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + + /*****************************************************************************/ bool use_tree_; diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 0e63e0983..eee107ab4 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -24,7 +24,6 @@ #include #include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp" -#define PI 3.14159265 namespace slam_toolbox { @@ -133,171 +132,168 @@ LifelongSlamToolbox::LifelongSlamToolbox(rclcpp::NodeOptions options) void LifelongSlamToolbox::scannerTest() { - RCLCPP_WARN(get_logger(), "<-------- Scanner Test -------->"); - - // I will have 5 lasers for each reading (Located at 0, +-25, +-50) - std::vector> robot_poses {{5.6f, 6.0f, PI/2}, {5.6f, 6.0f, PI/2}}; + // Loop through the different robot poses + for (int r = 0; r < robot_poses.size(); ++r) + { + std::cout << "---------- Robot pose ----------: " << r << std::endl; - std::vector robot_pose{5.6f, 6.0f, PI/2}; - std::vector ranges{1.65f, 5.0f, 5.0f, 5.0f, 5.0f}; // Maximum sensor range is 5 meters - - // Angles will be -50, -25, 0, 25, 50 - std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + // Angles {-50, -25, 0, 25, 50} in degrees + std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; - // This is the initial point - std::vector robot_grid_pos = getGridPosition(robot_pose[0], robot_pose[1]); - std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; + // Initial point + std::vector robot_grid_pos = getGridPosition(robot_poses[r][0], robot_poses[r][1]); + std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; - // Current yaw + beam angle: -PI/2 (-1.570795) -0.87266 = 2.44345 (-55 degrees) - for (int i = 0; i < ranges.size(); ++i) - { - std::cout << "........ New laser ........" << std::endl; - std::cout << "Distance: " << ranges[i] << ", Angle: " << angles[i] << std::endl; + // Current yaw + beam angle: -PI/2 (-1.570795) -0.87266 = 2.44345 (-55 degrees) + for (int i = 0; i < laser_ranges[r].size(); ++i) + { + std::cout << "........ New laser ........" << std::endl; + std::cout << "Distance: " << laser_ranges[r][i] << ", Angle: " << angles[i] << std::endl; - // Laser continuous distance - std::vector laser_grid = getLaserHit(robot_pose, ranges[i], angles[i]); + // Laser continuous distance + std::vector laser_grid = getLaserHit(robot_poses[r], laser_ranges[r][i], angles[i]); - // Laser final cell - std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1]); + // Laser final cell + std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1]); - // robot_grid_pos[0] // X1 - robot_grid_pos[1] // Y1 - // final_grid_pos[0] // X2 - final_grid_pos[1] // Y2 + // robot_grid_pos[0] // X1 - robot_grid_pos[1] // Y1 + // final_grid_pos[0] // X2 - final_grid_pos[1] // Y2 - // Ray tracing for getting the visited cells - std::vector cells_x, cells_y; - std::pair, std::vector> res_pair = Bresenham(robot_grid_pos[0], robot_grid_pos[1], final_grid_pos[0], final_grid_pos[1]); - cells_x = res_pair.first; - cells_y = res_pair.second; + // Ray tracing for getting the visited cells + std::vector cells_x, cells_y; + std::pair, std::vector> res_pair = Bresenham(robot_grid_pos[0], robot_grid_pos[1], final_grid_pos[0], final_grid_pos[1]); + cells_x = res_pair.first; + cells_y = res_pair.second; - // Adding last hit cell to the set - cells_x.push_back(final_grid_pos[0]); - cells_y.push_back(final_grid_pos[1]); + // Adding last hit cell to the set + cells_x.push_back(final_grid_pos[0]); + cells_y.push_back(final_grid_pos[1]); - // std::cout << "Cells" << std::endl; - // for (int c = 0; c < cells_x.size(); ++c) // One reading only - // { - // std::cout << cells_x[c] << ", " << cells_y[c] << std::endl; - // } - // std::cout << "End of cells" << std::endl; + // std::cout << "Cells" << std::endl; + // for (int c = 0; c < cells_x.size(); ++c) // One reading only + // { + // std::cout << cells_x[c] << ", " << cells_y[c] << std::endl; + // } + // std::cout << "End of cells" << std::endl; - // Visiting the cells - for (int j = 0; j < cells_x.size(); ++j) - { - if ((robot_grid_pos[0] == cells_x[j]) && (robot_grid_pos[1] == cells_y[j])) + // Visiting the cells + for (int j = 0; j < cells_x.size(); ++j) { - // Current cell is not relevant at all - continue; - } + if ((robot_grid_pos[0] == cells_x[j]) && (robot_grid_pos[1] == cells_y[j])) + { + // Current cell is not relevant at all + continue; + } - // Cells visualization - std::cout << "Current cell: " << cells_x[j] << ", " << cells_y[j] << std::endl; + // Cells visualization + std::cout << "Current cell: " << cells_x[j] << ", " << cells_y[j] << std::endl; - // Cells limits - float limit_x = cells_x[j] * m_resolution; - float limit_y = cells_y[j] * m_resolution; + // Cells limits + float limit_x = cells_x[j] * m_resolution; + float limit_y = cells_y[j] * m_resolution; - float min_x = limit_x; - float max_x = limit_x + m_resolution; - float min_y = limit_y; - float max_y = limit_y + m_resolution; + float min_x = limit_x; + float max_x = limit_x + m_resolution; + float min_y = limit_y; + float max_y = limit_y + m_resolution; - std::vector initial_x {limit_x, limit_x, limit_x + m_resolution, limit_x + m_resolution}; - std::vector initial_y {limit_y, limit_y, limit_y + m_resolution, limit_y + m_resolution}; + std::vector initial_x {limit_x, limit_x, limit_x + m_resolution, limit_x + m_resolution}; + std::vector initial_y {limit_y, limit_y, limit_y + m_resolution, limit_y + m_resolution}; - std::vector final_x {limit_x + m_resolution, limit_x, limit_x + m_resolution, limit_x}; - std::vector final_y {limit_y, limit_y + m_resolution, limit_y, limit_y + m_resolution}; + std::vector final_x {limit_x + m_resolution, limit_x, limit_x + m_resolution, limit_x}; + std::vector final_y {limit_y, limit_y + m_resolution, limit_y, limit_y + m_resolution}; - calculateLimits(initial_x, initial_y, final_x, final_y, limit_x, limit_y, min_x, max_x, min_y, max_y, robot_grid_pos, final_grid_pos); + calculateLimits(initial_x, initial_y, final_x, final_y, limit_x, limit_y, min_x, max_x, min_y, max_y, robot_grid_pos, final_grid_pos); - std::vector inter_x, inter_y; - for (int k = 0; k < 4; ++k) - { - std::vector intersection = calculateIntersection(robot_pose, laser_grid, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); - - if(intersection.size() != 0) + std::vector inter_x, inter_y; + for (int k = 0; k < 4; ++k) { - // If the laser and a cell intersects, we need to make sure it happens in the right bounds - if ((abs(intersection[0]) >= abs(min_x - 0.01f)) && - (abs(intersection[0]) <= abs(max_x + 0.01f)) && - (abs(intersection[1]) >= abs(min_y - 0.01f)) && - (abs(intersection[1]) <= abs(max_y + 0.01f))) + std::vector intersection = calculateIntersection(robot_poses[r], laser_grid, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); + + if(intersection.size() != 0) { - /* - 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[0]); - inter_y.push_back(intersection[1]); + // If the laser and a cell intersects, we need to make sure it happens in the right bounds + if ((abs(intersection[0]) >= abs(min_x - 0.01f)) && + (abs(intersection[0]) <= abs(max_x + 0.01f)) && + (abs(intersection[1]) >= abs(min_y - 0.01f)) && + (abs(intersection[1]) <= abs(max_y + 0.01f))) + { + /* + 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[0]); + inter_y.push_back(intersection[1]); + } } } - } - // When a cell is marked by Bresenham but there is not intersection points - if (inter_x.size() == 0) - continue; + // When a cell is marked by Bresenham but there is not intersection points + if (inter_x.size() == 0) + continue; - // Enter (d1) and Exit (d2) distances - std::vector distances; - for (int k = 0; k < inter_x.size(); ++k) - { - float dist_point = calculateDistance(robot_pose[0], robot_pose[1], inter_x[k], inter_y[k]); - distances.push_back(dist_point); - } + // Enter (d1) and Exit (d2) distances + std::vector distances; + for (int k = 0; k < inter_x.size(); ++k) + { + float dist_point = calculateDistance(robot_poses[r][0], robot_poses[r][1], inter_x[k], inter_y[k]); + distances.push_back(dist_point); + } - // Integral 1: 0 to d1 which is distance from robot pose to first point where the cell is cut - // Integral 2: d1 which is distance from robot pose to first point where the cell is cut to - // d2 which is distance from robot pose to second point where the cell is cut - // Integral 3: d2 which is distance from robot pose to second point where the cell is cut to z_max - - // Measurement outcomes vector - std::vector probabilities { - calculateProbability(distances[1], 5.0f), // Free - calculateProbability(distances[0], distances[1]), // Occupied - calculateProbability(0.0f, distances[0]) // Not observed - }; - - // Assigning the cells - m_cell_x = cells_x[j]; - m_cell_y = cells_y[j]; - - // Appending new measurement outcomes for the current cell - appendCellProbabilities(probabilities); - - // From this point the approximation should be done in a different function - // Get all the measurement outcomes for the current cell - std::vector> meas_outcomes = retreiveMeasurementOutcomes(); - // Compute all the possible combinations for the current cell - algorithm 1 - computeProbabilities(meas_outcomes); - - // Calculate 3.12 - std::unordered_map::iterator it_mutual; - std::cout << "Number of measurements: " << meas_outcomes.size() << std::endl; - float cell_mutual_inf = 0.0f; - for (it_mutual = m_un_cmb.begin(); it_mutual != m_un_cmb.end(); ++it_mutual) - { - // Interested in the final measurement outcomes - if (it_mutual->first.fr + it_mutual->first.oc + it_mutual->first.un == meas_outcomes.size()) + // Integral 1: 0 to d1 which is distance from robot pose to first point where the cell is cut + // Integral 2: d1 which is distance from robot pose to first point where the cell is cut to + // d2 which is distance from robot pose to second point where the cell is cut + // Integral 3: d2 which is distance from robot pose to second point where the cell is cut to z_max + + // Measurement outcomes vector + std::vector probabilities { + calculateProbability(distances[1], 5.0f), // Free + calculateProbability(distances[0], distances[1]), // Occupied + calculateProbability(0.0f, distances[0]) // Not observed + }; + + // Assigning the cells + m_cell_x = cells_x[j]; + m_cell_y = cells_y[j]; + + // Appending new measurement outcomes for the current cell + appendCellProbabilities(probabilities); + + // Get all the measurement outcomes for the current cell + std::vector> meas_outcomes = retreiveMeasurementOutcomes(); + // Compute all the possible combinations for the current cell - algorithm 1 + computeProbabilities(meas_outcomes); + + // Calculate 3.12 + std::unordered_map::iterator it_mutual; + std::cout << "Number of measurements: " << meas_outcomes.size() << std::endl; + float cell_mutual_inf = 0.0f; + for (it_mutual = m_un_cmb.begin(); it_mutual != m_un_cmb.end(); ++it_mutual) { - cell_mutual_inf += it_mutual->second * measurementOutcomeEntropy(it_mutual->first); + // Interested in the final measurement outcomes + if (it_mutual->first.fr + it_mutual->first.oc + it_mutual->first.un == meas_outcomes.size()) + { + cell_mutual_inf += it_mutual->second * measurementOutcomeEntropy(it_mutual->first); + } } - } - /* - At this point I would need to calculate the H(C) as it stills and icognite here - - - I need to separate here the calculations. Which means Entropy and Mutual Information - must be calculated outside this loop - - Start documenting this section - - I need to add more poses and more ranges in order to see mutual information impact - */ - std::cout << cell_mutual_inf << std::endl; - updateCellMutualInformation(cell_mutual_inf); - std::cout << "++++++++++++++++++++++++" << std::endl; + /* + At this point I would need to calculate the H(C) as it stills and icognite here + + - I need to separate here the calculations. Which means Entropy and Mutual Information + must be calculated outside this loop + - Start documenting this section + - I need to add more poses and more ranges in order to see mutual information impact + */ + std::cout << cell_mutual_inf << std::endl; + updateCellMutualInformation(cell_mutual_inf); + std::cout << "++++++++++++++++++++++++" << std::endl; + } } + float mutual = calculateMapMutualInformation(); + std::cout << "Mutual information: " << mutual << std::endl; } - float mutual = calculateMapMutualInformation(); - std::cout << "Mutual information: " << mutual << std::endl; } void LifelongSlamToolbox::updateCellMutualInformation(float mut_inf_val) From c825cc6932a44c684100ce87a0d5ecd25dbe99a4 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Fri, 10 Dec 2021 11:42:20 -0500 Subject: [PATCH 27/83] [WIP]> Notations and calculations improvements --- .../experimental/slam_toolbox_lifelong.hpp | 9 ++--- src/experimental/slam_toolbox_lifelong.cpp | 35 ++++++++++++------- 2 files changed, 25 insertions(+), 19 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index bcf1fdc69..64b6aad31 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -20,10 +20,10 @@ #define SLAM_TOOLBOX__EXPERIMENTAL__SLAM_TOOLBOX_LIFELONG_HPP_ #include +#include +#include #include "slam_toolbox/slam_toolbox_common.hpp" -#define PI 3.14159265 - namespace slam_toolbox { @@ -111,7 +111,6 @@ class LifelongSlamToolbox : public SlamToolbox // Measurements calculations float calculateProbability(float range_1, float range_2); float calculateDistance(float x_1, float y_1, float x_2, float y_2); - float entropyFromProbability(float prob); // Mutual information float measurementOutcomeEntropy(Occupancy const& meas_outcome); @@ -141,12 +140,10 @@ class LifelongSlamToolbox : public SlamToolbox int m_num_cells; // Robot information - Defining values just for testing - std::vector> robot_poses {{5.6f, 6.0f, PI/2}, {3.5f, 9.0f, 0.0f}}; + std::vector> robot_poses {{5.6f, 6.0f, M_PI/2}, {3.5f, 9.0f, 0.0f}}; std::vector> laser_ranges {{1.65f, 5.0f, 5.0f, 5.0f, 5.0f}, {5.0f, 5.0f, 4.0f, 5.0f, 5.0f}}; std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; - - /*****************************************************************************/ bool use_tree_; double iou_thresh_; diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index eee107ab4..75aed0365 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include #include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp" @@ -260,11 +259,14 @@ void LifelongSlamToolbox::scannerTest() // Appending new measurement outcomes for the current cell appendCellProbabilities(probabilities); + + // Get all the measurement outcomes for the current cell std::vector> meas_outcomes = retreiveMeasurementOutcomes(); // Compute all the possible combinations for the current cell - algorithm 1 computeProbabilities(meas_outcomes); + // Calculate 3.12 std::unordered_map::iterator it_mutual; std::cout << "Number of measurements: " << meas_outcomes.size() << std::endl; @@ -281,12 +283,14 @@ void LifelongSlamToolbox::scannerTest() /* At this point I would need to calculate the H(C) as it stills and icognite here + - According to explanation provided in section 2 - I need to separate here the calculations. Which means Entropy and Mutual Information must be calculated outside this loop - Start documenting this section - I need to add more poses and more ranges in order to see mutual information impact */ std::cout << cell_mutual_inf << std::endl; + // Here should be the H(C) = 0.5 : 0.5 - SUM(P*H) updateCellMutualInformation(cell_mutual_inf); std::cout << "++++++++++++++++++++++++" << std::endl; } @@ -296,6 +300,8 @@ void LifelongSlamToolbox::scannerTest() } } + + void LifelongSlamToolbox::updateCellMutualInformation(float mut_inf_val) { /* @@ -398,7 +404,20 @@ float LifelongSlamToolbox::measurementOutcomeEntropy(Occupancy const& meas_outco To calculate the measurement outcome entropy (Measurement outcome in the form ) */ float cell_logs = (meas_outcome.fr * calculateLogs(0.3f)) + (meas_outcome.oc * calculateLogs(0.7f)) + (meas_outcome.un * calculateLogs(0.5f)); - return calculateEntropy(probabilityFromLogs(cell_logs)); + + // This is the right calculation + // meas_outcome.fr * calculateEntropy(probabilityFromLogs(calculateLogs(0.3f))) + // + meas_outcome.oc * calculateEntropy(probabilityFromLogs(calculateLogs(0.7f))) + // + meas_outcome.un * calculateEntropy(probabilityFromLogs(calculateLogs(0.5f))) + + + /* + The entropy calculation will be negative - Which is this function + Then the sumatory of 3.12 should be substracted + Mutual information should have a positive value + */ + // This might be different because this summatory should be + return -calculateEntropy(probabilityFromLogs(cell_logs)); } float LifelongSlamToolbox::calculateEntropy(float probability) @@ -406,7 +425,7 @@ float LifelongSlamToolbox::calculateEntropy(float probability) /* To calculate the entropy */ - return probability * log(probability); + return probability * log2(probability); } std::vector> LifelongSlamToolbox::retreiveMeasurementOutcomes() @@ -555,16 +574,6 @@ void LifelongSlamToolbox::computeProbabilities(std::vector>& } } -float LifelongSlamToolbox::entropyFromProbability(float prob) -{ - /* - To calculate the cell entropy from the probability - - Entropy = (p*log(p)); - - Entropy_Old = (p*log(p) + (1-p)*log(1-p)); - */ - return (prob * log(prob)); -} - float LifelongSlamToolbox::probabilityFromLogs(float log) { /* From ea9ef270358a4ad9a477050ca6b11835b2d40197 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Tue, 14 Dec 2021 11:53:53 -0500 Subject: [PATCH 28/83] [ADD]: New theoretical information object --- CMakeLists.txt | 2 +- .../experimental/slam_toolbox_lifelong.hpp | 27 +- .../experimental/theoretic_information.hpp | 88 +++ src/experimental/slam_toolbox_lifelong.cpp | 142 ++-- src/experimental/theoretic_information.cpp | 605 ++++++++++++++++++ 5 files changed, 793 insertions(+), 71 deletions(-) create mode 100644 include/slam_toolbox/experimental/theoretic_information.hpp create mode 100644 src/experimental/theoretic_information.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 40f27939d..45d78106a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -162,7 +162,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) +add_executable(lifelong_slam_toolbox_node src/experimental/slam_toolbox_lifelong_node.cpp src/experimental/theoretic_information.cpp) target_link_libraries(lifelong_slam_toolbox_node lifelong_slam_toolbox) #### Merging maps tool diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index 64b6aad31..35feac730 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -22,13 +22,18 @@ #include #include #include +#include #include "slam_toolbox/slam_toolbox_common.hpp" +#include "slam_toolbox/experimental/theoretic_information.hpp" namespace slam_toolbox { class LifelongSlamToolbox : public SlamToolbox { + typedef std::tuple map_tuple; + typedef std::pair map_pair; + public: explicit LifelongSlamToolbox(rclcpp::NodeOptions options); ~LifelongSlamToolbox() {} @@ -52,6 +57,18 @@ class LifelongSlamToolbox : public SlamToolbox public: // Cell occupancy struct for unordered map + + struct HashTuple + { + std::size_t operator() (map_tuple const& key) const + { + 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; + } + }; struct Occupancy { int fr, oc ,un; @@ -104,16 +121,17 @@ class LifelongSlamToolbox : public SlamToolbox // Grid and position information std::pair, std::vector> Bresenham(int x_1, int y_1, int x_2, int y_2); std::vector getGridPosition(float x, float y); - std::vector getLaserHit(std::vector const& robot_pose, float range, float angle); + std::vector laserHitDistance(std::vector const& robot_pose, float range, float angle); std::vector calculateIntersection(std::vector laser_start, std::vector laser_end, std::vector cell_start, std::vector cell_end); int getSign(int n_1, int n_2); // Measurements calculations - float calculateProbability(float range_1, float range_2); - float calculateDistance(float x_1, float y_1, float x_2, float y_2); + float probabilityFromObservation(float range_1, float range_2); + float euclideanDistance(float x_1, float y_1, float x_2, float y_2); // Mutual information - float measurementOutcomeEntropy(Occupancy const& meas_outcome); + float measurementOutcomeEntropy(map_tuple const& meas_outcome); + void recoverProbability(); float calculateLogs(float probability); float probabilityFromLogs(float log); @@ -129,6 +147,7 @@ class LifelongSlamToolbox : public SlamToolbox std::vector unhashIndex(int hash); // Data structures + std::unordered_map m_map_out; std::unordered_map m_un_cmb; std::map, std::vector>> m_cell_probabilities; std::vector> m_mutual_grid; diff --git a/include/slam_toolbox/experimental/theoretic_information.hpp b/include/slam_toolbox/experimental/theoretic_information.hpp new file mode 100644 index 000000000..b9fe731b6 --- /dev/null +++ b/include/slam_toolbox/experimental/theoretic_information.hpp @@ -0,0 +1,88 @@ +#ifndef SLAM_TOOLBOX__EXPERIMENTAL__THEORETIC_INFORMATION_HPP_ +#define SLAM_TOOLBOX__EXPERIMENTAL__THEORETIC_INFORMATION_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +class TeorethicInformation +{ + typedef std::tuple map_tuple; + typedef std::pair map_pair; + +public: + TeorethicInformation(); // Empty contructor for now + ~TeorethicInformation() {} + +public: + struct HashTuple + { + std::size_t operator() (map_tuple const& key) const + { + 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; + } + }; + +private: + // Testing + void scannerTest(); + + // Grid operations + void initializeGrids(); + void updateCellLimits( + std::vector & initial_x, std::vector & initial_y, std::vector & final_x, std::vector & final_y, + float & limit_x, float & limit_y, std::vector & cell_limits, std::vector & robot_grid_pos, std::vector & final_grid_pos); + + // Grid and position information + std::pair, std::vector> Bresenham(int x_1, int y_1, int x_2, int y_2); + std::vector getGridPosition(float x, float y); + std::vector laserHitDistance(std::vector const& robot_pose, float range, float angle); + std::vector calculateCellIntersectionPoints(std::vector & laser_start, std::vector & laser_end, std::vector & cell_start, std::vector & cell_end); + int getSign(int n_1, int n_2); + + // Measurements calculations + float probabilityFromObservation(float range_1, float range_2); + float euclideanDistance(float x_1, float y_1, float x_2, float y_2); + + // Mutual information + float calculateEntropy(float probability); + float calculateLogs(float probability); + float calculateMapMutualInformation(); + float measurementOutcomeEntropy(map_tuple const& meas_outcome); + float probabilityFromLogs(float log); + void recoverProbability(); + void updateCellMutualInformation(float mut_inf); + + // Measurement outcomes probabilities + void appendCellProbabilities(std::vector& measurements); + void computeProbabilities(std::vector>& meas_outcm); + std::vector> retreiveMeasurementOutcomes(); + std::vector unhashIndex(int hash); + +private: + // Data structures + std::unordered_map m_map_out; + std::map, std::vector>> m_cell_probabilities; + std::vector> m_mutual_grid; + std::vector> m_grid; + float m_map_dist; + float m_resolution; + int m_cell_x; + int m_cell_y; + int m_num_cells; + + // Robot information - Defining values just for testing + std::vector> robot_poses {{5.6f, 6.0f, M_PI/2}, {3.5f, 9.0f, 0.0f}}; + std::vector> laser_ranges {{1.65f, 5.0f, 5.0f, 5.0f, 5.0f}, {5.0f, 5.0f, 4.0f, 5.0f, 5.0f}}; + std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; +}; + +#endif \ No newline at end of file diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 75aed0365..cd8f4bd8c 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -81,16 +81,17 @@ LifelongSlamToolbox::LifelongSlamToolbox(rclcpp::NodeOptions options) // in lifelong mode, we cannot have interactive mode enabled enable_interactive_mode_ = false; - m_resolution = 0.5f; // Map resolution - m_map_dist = 20.0f; // Total map distance - m_num_cells = static_cast(m_map_dist / m_resolution); + // m_resolution = 0.5f; // Map resolution + // m_map_dist = 20.0f; // Total map distance + // m_num_cells = static_cast(m_map_dist / m_resolution); // Grids initialization (Occupancy and Mutual information) - m_grid.resize(m_num_cells); - m_mutual_grid.resize(m_num_cells); - initializeGrids(); + // m_grid.resize(m_num_cells); + // m_mutual_grid.resize(m_num_cells); + // initializeGrids(); - scannerTest(); + // scannerTest(); + TeorethicInformation information; } /*****************************************************************************/ @@ -150,7 +151,7 @@ void LifelongSlamToolbox::scannerTest() std::cout << "Distance: " << laser_ranges[r][i] << ", Angle: " << angles[i] << std::endl; // Laser continuous distance - std::vector laser_grid = getLaserHit(robot_poses[r], laser_ranges[r][i], angles[i]); + std::vector laser_grid = laserHitDistance(robot_poses[r], laser_ranges[r][i], angles[i]); // Laser final cell std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1]); @@ -236,7 +237,7 @@ void LifelongSlamToolbox::scannerTest() std::vector distances; for (int k = 0; k < inter_x.size(); ++k) { - float dist_point = calculateDistance(robot_poses[r][0], robot_poses[r][1], inter_x[k], inter_y[k]); + float dist_point = euclideanDistance(robot_poses[r][0], robot_poses[r][1], inter_x[k], inter_y[k]); distances.push_back(dist_point); } @@ -247,9 +248,9 @@ void LifelongSlamToolbox::scannerTest() // Measurement outcomes vector std::vector probabilities { - calculateProbability(distances[1], 5.0f), // Free - calculateProbability(distances[0], distances[1]), // Occupied - calculateProbability(0.0f, distances[0]) // Not observed + probabilityFromObservation(distances[1], 5.0f), // Free + probabilityFromObservation(distances[0], distances[1]), // Occupied + probabilityFromObservation(0.0f, distances[0]) // Not observed }; // Assigning the cells @@ -268,13 +269,15 @@ void LifelongSlamToolbox::scannerTest() // Calculate 3.12 - std::unordered_map::iterator it_mutual; - std::cout << "Number of measurements: " << meas_outcomes.size() << std::endl; + std::unordered_map::iterator it_mutual; + + std::cout << "Number of measurements: " << meas_outcomes.size() << std::endl; float cell_mutual_inf = 0.0f; - for (it_mutual = m_un_cmb.begin(); it_mutual != m_un_cmb.end(); ++it_mutual) + for (it_mutual = m_map_out.begin(); it_mutual != m_map_out.end(); ++it_mutual) { // Interested in the final measurement outcomes - if (it_mutual->first.fr + it_mutual->first.oc + it_mutual->first.un == meas_outcomes.size()) + if (std::get<0>(it_mutual->first) + std::get<1>(it_mutual->first) + std::get<2>(it_mutual->first) == meas_outcomes.size()) + // if (it_mutual->first.fr + it_mutual->first.oc + it_mutual->first.un == meas_outcomes.size()) { cell_mutual_inf += it_mutual->second * measurementOutcomeEntropy(it_mutual->first); } @@ -282,15 +285,15 @@ void LifelongSlamToolbox::scannerTest() /* At this point I would need to calculate the H(C) as it stills and icognite here - - - According to explanation provided in section 2 + + - According to explanation provided in section 2 - I need to separate here the calculations. Which means Entropy and Mutual Information must be calculated outside this loop - Start documenting this section - I need to add more poses and more ranges in order to see mutual information impact */ std::cout << cell_mutual_inf << std::endl; - // Here should be the H(C) = 0.5 : 0.5 - SUM(P*H) + // Here should be the H(C) = 0.5 : 0.5 - SUM(P*H) updateCellMutualInformation(cell_mutual_inf); std::cout << "++++++++++++++++++++++++" << std::endl; } @@ -315,7 +318,7 @@ void LifelongSlamToolbox::updateCellMutualInformation(float mut_inf_val) float LifelongSlamToolbox::calculateMapMutualInformation() { /* - To calculate map mutual information, this is the summation + To calculate map mutual information, this is the summation of all cells mutual information */ float sum = 0.0f; @@ -330,8 +333,8 @@ float LifelongSlamToolbox::calculateMapMutualInformation() } void LifelongSlamToolbox::calculateLimits( - std::vector & initial_x, std::vector & initial_y, std::vector & final_x, std::vector & final_y, - float & limit_x, float & limit_y, float & min_x, float & max_x, float & min_y, float & max_y, std::vector & robot_grid_pos, + std::vector & initial_x, std::vector & initial_y, std::vector & final_x, std::vector & final_y, + float & limit_x, float & limit_y, float & min_x, float & max_x, float & min_y, float & max_y, std::vector & robot_grid_pos, std::vector & final_grid_pos) { /* @@ -398,26 +401,29 @@ void LifelongSlamToolbox::initializeGrids() } } -float LifelongSlamToolbox::measurementOutcomeEntropy(Occupancy const& meas_outcome) +float LifelongSlamToolbox::measurementOutcomeEntropy(map_tuple const& meas_outcome) { /* To calculate the measurement outcome entropy (Measurement outcome in the form ) + Calculate Log-Odds from initial probability guess + Calculate the probability from those logs + Calculate the entropy with the retrieved probability */ - float cell_logs = (meas_outcome.fr * calculateLogs(0.3f)) + (meas_outcome.oc * calculateLogs(0.7f)) + (meas_outcome.un * calculateLogs(0.5f)); - - // This is the right calculation - // meas_outcome.fr * calculateEntropy(probabilityFromLogs(calculateLogs(0.3f))) - // + meas_outcome.oc * calculateEntropy(probabilityFromLogs(calculateLogs(0.7f))) - // + meas_outcome.un * calculateEntropy(probabilityFromLogs(calculateLogs(0.5f))) + // float cells = (std::get<0>(meas_outcome) * calculateLogs(0.3f)) + (std::get<1>(meas_outcome) * calculateLogs(0.7f)) + (std::get<2>(meas_outcome) * calculateLogs(0.5f)); + // This might change + float entropy = std::get<0>(meas_outcome) * calculateEntropy(probabilityFromLogs(calculateLogs(0.3f))) + + std::get<1>(meas_outcome) * calculateEntropy(probabilityFromLogs(calculateLogs(0.7f))) + + std::get<2>(meas_outcome) * calculateEntropy(probabilityFromLogs(calculateLogs(0.5f))); /* The entropy calculation will be negative - Which is this function Then the sumatory of 3.12 should be substracted Mutual information should have a positive value */ - // This might be different because this summatory should be - return -calculateEntropy(probabilityFromLogs(cell_logs)); + // This might be different because this summatory should be + // return -calculateEntropy(probabilityFromLogs(cell_logs)); + return -entropy; } float LifelongSlamToolbox::calculateEntropy(float probability) @@ -425,7 +431,7 @@ float LifelongSlamToolbox::calculateEntropy(float probability) /* To calculate the entropy */ - return probability * log2(probability); + return probability * log2(probability); } std::vector> LifelongSlamToolbox::retreiveMeasurementOutcomes() @@ -482,7 +488,7 @@ void LifelongSlamToolbox::computeProbabilities(std::vector>& To compute all the possible combinations of a grid cell, given a set of measurement outcomes */ - m_un_cmb.clear(); + m_map_out.clear(); int k = meas_outcm.size(); // The number of measurements // std::cout << "K: " << k << std::endl; @@ -492,28 +498,28 @@ void LifelongSlamToolbox::computeProbabilities(std::vector>& float p_occ = meas_outcm[0][1]; float p_un = meas_outcm[0][0]; - // std::unordered_map m_un_cmb; - std::unordered_map::iterator it_un; + std::unordered_map::iterator it_out; + // Root + m_map_out.insert(map_pair(std::make_tuple(0, 0, 0), 1.0f)); - // Initial node - m_un_cmb.insert(std::pair({0, 0, 0}, 1.0f)); - m_un_cmb.insert(std::pair({0, 0, 1}, p_free)); - m_un_cmb.insert(std::pair({0, 1, 0}, p_occ)); - m_un_cmb.insert(std::pair({1, 0, 0}, p_un)); + // First measurement + m_map_out.insert(map_pair(std::make_tuple(1, 0, 0), p_free)); + m_map_out.insert(map_pair(std::make_tuple(0, 1, 0), p_occ)); + m_map_out.insert(map_pair(std::make_tuple(0, 0, 1), p_un)); for (int i = r; r < k; ++r) { std::cout << "R: " << r << std::endl; - std::vector occ_vct; + std::vector tup_vct; std::vector acc_prob; - for (it_un = m_un_cmb.begin(); it_un != m_un_cmb.end(); ++it_un) + for (it_out = m_map_out.begin(); it_out != m_map_out.end(); ++it_out) { - // Current combination state - int fr_idx = it_un->first.fr; - int oc_idx = it_un->first.oc; - int un_idx = it_un->first.un; + // Index + int fr_idx = std::get<0>(it_out->first); + int oc_idx = std::get<1>(it_out->first); + int un_idx = std::get<2>(it_out->first); // Measurement outcome probability float free_prop = meas_outcm[r][0]; @@ -528,48 +534,52 @@ void LifelongSlamToolbox::computeProbabilities(std::vector>& // std::cout << fr_idx << ", " << oc_idx << ", " << un_idx + 1 << std::endl; // Searching for the current combination in this level - auto it_comb = std::find(occ_vct.begin(), occ_vct.end(), Occupancy{fr_idx + 1, oc_idx, un_idx}); + std::vector::iterator it_comb; + it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(fr_idx + 1, oc_idx, un_idx)); + // Free - if (it_comb != occ_vct.end()) + if (it_comb != tup_vct.end()) { - acc_prob[it_comb - occ_vct.begin()] += it_un->second * free_prop; + acc_prob[it_comb - tup_vct.begin()] += it_out->second * free_prop; } else { - occ_vct.push_back(Occupancy{fr_idx + 1, oc_idx, un_idx}); - acc_prob.push_back(it_un->second * free_prop); + tup_vct.push_back(std::make_tuple(fr_idx + 1, oc_idx, un_idx)); + acc_prob.push_back(it_out->second * free_prop); } - it_comb = std::find(occ_vct.begin(), occ_vct.end(), Occupancy{fr_idx, oc_idx + 1, un_idx}); + it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(fr_idx, oc_idx + 1, un_idx)); + // Occupied - if (it_comb != occ_vct.end()) + if (it_comb != tup_vct.end()) { - acc_prob[it_comb - occ_vct.begin()] += it_un->second * occ_prop; + acc_prob[it_comb - tup_vct.begin()] += it_out->second * occ_prop; } else { - occ_vct.push_back(Occupancy{fr_idx, oc_idx + 1, un_idx}); - acc_prob.push_back(it_un->second * occ_prop); + tup_vct.push_back(std::make_tuple(fr_idx, oc_idx + 1, un_idx)); + acc_prob.push_back(it_out->second * occ_prop); } - it_comb = std::find(occ_vct.begin(), occ_vct.end(), Occupancy{fr_idx, oc_idx, un_idx + 1}); + it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(fr_idx, oc_idx, un_idx + 1)); + // Unobserved - if (it_comb != occ_vct.end()) + if (it_comb != tup_vct.end()) { - acc_prob[it_comb - occ_vct.begin()] += it_un->second * un_prop; + acc_prob[it_comb - tup_vct.begin()] += it_out->second * un_prop; } else { - occ_vct.push_back(Occupancy{fr_idx, oc_idx, un_idx + 1}); - acc_prob.push_back(it_un->second * un_prop); + tup_vct.push_back(std::make_tuple(fr_idx, oc_idx, un_idx + 1)); + acc_prob.push_back(it_out->second * un_prop); } } } // Inserting the elements into the map - for (int k = 0; k < occ_vct.size(); ++k) + for (int k = 0; k < tup_vct.size(); ++k) { - m_un_cmb.insert(std::pair(occ_vct[k], acc_prob[k])); + m_map_out.insert(map_pair(tup_vct[k], acc_prob[k])); } } } @@ -626,7 +636,7 @@ std::vector LifelongSlamToolbox::calculateIntersection( } } -float LifelongSlamToolbox::calculateDistance(float x_1, float y_1, float x_2, float y_2) +float LifelongSlamToolbox::euclideanDistance(float x_1, float y_1, float x_2, float y_2) { /* To calculate the euclidean distance between two points @@ -637,7 +647,7 @@ float LifelongSlamToolbox::calculateDistance(float x_1, float y_1, float x_2, fl return sqrt(diff_x*diff_x + diff_y*diff_y); } -float LifelongSlamToolbox::calculateProbability(float range_1, float range_2) +float LifelongSlamToolbox::probabilityFromObservation(float range_1, float range_2) { /* Calculates the probability of a cell being observed by a given measurement @@ -678,7 +688,7 @@ std::vector LifelongSlamToolbox::getGridPosition(float x, float y) return {x_cell, y_cell}; } -std::vector LifelongSlamToolbox::getLaserHit(std::vector const& robot_pose, float range, float angle) +std::vector LifelongSlamToolbox::laserHitDistance(std::vector const& robot_pose, float range, float angle) { /* To get the distance where the laser beam hits something diff --git a/src/experimental/theoretic_information.cpp b/src/experimental/theoretic_information.cpp new file mode 100644 index 000000000..c525587e2 --- /dev/null +++ b/src/experimental/theoretic_information.cpp @@ -0,0 +1,605 @@ +#include +#include "slam_toolbox/experimental/theoretic_information.hpp" + +TeorethicInformation::TeorethicInformation() +{ + std::cout << "Constructor of Theoretic information" << std::endl; + m_resolution = 0.5f; // Map resolution + m_map_dist = 20.0f; // Total map distance + m_num_cells = static_cast(m_map_dist / m_resolution); + + // Grids initialization (Occupancy and Mutual information) + m_grid.resize(m_num_cells); + m_mutual_grid.resize(m_num_cells); + initializeGrids(); + + scannerTest(); +} + +void TeorethicInformation::scannerTest() +{ + // Loop through the different robot poses + for (int r = 0; r < robot_poses.size(); ++r) + { + std::cout << "---------- Robot pose ----------: " << r << std::endl; + + // Angles {-50, -25, 0, 25, 50} in degrees + std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + + // Initial point + std::vector robot_grid_pos = getGridPosition(robot_poses[r][0], robot_poses[r][1]); + std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; + + // Current yaw + beam angle: -PI/2 (-1.570795) -0.87266 = 2.44345 (-55 degrees) + for (int i = 0; i < laser_ranges[r].size(); ++i) + { + std::cout << "........ New laser ........" << std::endl; + std::cout << "Distance: " << laser_ranges[r][i] << ", Angle: " << angles[i] << std::endl; + + // Laser continuous distance + std::vector laser_grid = laserHitDistance(robot_poses[r], laser_ranges[r][i], angles[i]); + + // Laser final cell + std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1]); + + // robot_grid_pos[0] // X1 - robot_grid_pos[1] // Y1 + // final_grid_pos[0] // X2 - final_grid_pos[1] // Y2 + + // Ray tracing for getting the visited cells + std::vector cells_x, cells_y; + std::pair, std::vector> res_pair = Bresenham(robot_grid_pos[0], robot_grid_pos[1], final_grid_pos[0], final_grid_pos[1]); + cells_x = res_pair.first; + cells_y = res_pair.second; + + // Deleting the current robot cell + cells_x.erase(cells_x.begin()); + cells_y.erase(cells_y.begin()); + + // Adding last hit cell to the set + cells_x.push_back(final_grid_pos[0]); + cells_y.push_back(final_grid_pos[1]); + + + // Visiting the cells + for (int j = 0; j < cells_x.size(); ++j) + { + // Cells visualization + std::cout << "Current cell: " << cells_x[j] << ", " << cells_y[j] << std::endl; + + // Inidividual cell limits + float limit_x = cells_x[j] * m_resolution; + float limit_y = cells_y[j] * m_resolution; + + // Cell limits: min_x, max_x, min_y, max_y + std::vector cell_limits {limit_x, limit_x + m_resolution, limit_y, limit_y + m_resolution}; + + // Initial points for each of the 4 corners + std::vector initial_x {limit_x, limit_x, limit_x + m_resolution, limit_x + m_resolution}; + std::vector initial_y {limit_y, limit_y, limit_y + m_resolution, limit_y + m_resolution}; + + // Final points for each of the 4 corners + std::vector final_x {limit_x + m_resolution, limit_x, limit_x + m_resolution, limit_x}; + std::vector final_y {limit_y, limit_y + m_resolution, limit_y, limit_y + m_resolution}; + + // Set the new cell limits + updateCellLimits(initial_x, initial_y, final_x, final_y, limit_x, limit_y, cell_limits, robot_grid_pos, final_grid_pos); + + std::vector inter_x, inter_y; + for (int k = 0; k < 4; ++k) + { + std::vector intersection = calculateCellIntersectionPoints(robot_poses[r], laser_grid, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); + if(intersection.size() != 0) + { + // If the laser and a cell intersects, we need to make sure it happens in the right bounds + if ((abs(intersection[0]) >= abs(cell_limits[0] - 0.01f)) && + (abs(intersection[0]) <= abs(cell_limits[1] + 0.01f)) && + (abs(intersection[1]) >= abs(cell_limits[2] - 0.01f)) && + (abs(intersection[1]) <= abs(cell_limits[3] + 0.01f))) + { + /* + 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[0]); + inter_y.push_back(intersection[1]); + } + } + } + + // When a cell is marked by Bresenham but there is not intersection points + if (inter_x.size() == 0) + continue; + + // Enter (d1) and Exit (d2) distances + std::vector distances; + for (int k = 0; k < inter_x.size(); ++k) + { + float dist_point = euclideanDistance(robot_poses[r][0], robot_poses[r][1], inter_x[k], inter_y[k]); + distances.push_back(dist_point); + } + + // Integral 1: 0 to d1 which is distance from robot pose to first point where the cell is cut + // Integral 2: d1 which is distance from robot pose to first point where the cell is cut to + // d2 which is distance from robot pose to second point where the cell is cut + // Integral 3: d2 which is distance from robot pose to second point where the cell is cut to z_max + + // Measurement outcomes vector + std::vector probabilities { + probabilityFromObservation(distances[1], 5.0f), // Free + probabilityFromObservation(distances[0], distances[1]), // Occupied + probabilityFromObservation(0.0f, distances[0]) // Not observed + }; + + // Assigning the cells + m_cell_x = cells_x[j]; + m_cell_y = cells_y[j]; + + // Appending new measurement outcomes for the current cell + appendCellProbabilities(probabilities); + + // Get all the measurement outcomes for the current cell + std::vector> meas_outcomes = retreiveMeasurementOutcomes(); + // Compute all the possible combinations for the current cell - algorithm 1 + computeProbabilities(meas_outcomes); + + // Calculate 3.12 + std::unordered_map::iterator it_mutual; + + std::cout << "Number of measurements: " << meas_outcomes.size() << std::endl; + float cell_mutual_inf = 0.0f; + for (it_mutual = m_map_out.begin(); it_mutual != m_map_out.end(); ++it_mutual) + { + // Interested in the final measurement outcomes + if (std::get<0>(it_mutual->first) + std::get<1>(it_mutual->first) + std::get<2>(it_mutual->first) == meas_outcomes.size()) + { + cell_mutual_inf += it_mutual->second * measurementOutcomeEntropy(it_mutual->first); + } + } + + std::cout << cell_mutual_inf << std::endl; + // Here should be the H(C) = 0.5 : 0.5 - SUM(P*H) + updateCellMutualInformation(cell_mutual_inf); + std::cout << "++++++++++++++++++++++++" << std::endl; + } + } + float mutual = calculateMapMutualInformation(); + std::cout << "Mutual information: " << mutual << std::endl; + } +} + +void TeorethicInformation::appendCellProbabilities(std::vector& measurements) +{ + /* + To append a new measurement for a specific cell + */ + + // Iterator for getting the cell + std::map, std::vector>>::iterator it_cell; + + it_cell = m_cell_probabilities.find({m_cell_x, m_cell_y}); + if (it_cell == m_cell_probabilities.end()) + { + // Cell is not present in the map + m_cell_probabilities.insert(std::pair, std::vector>>( + {m_cell_x, m_cell_y}, + {{measurements[0], measurements[1], measurements[2]}} + )); + } + else + { + // Cell is already in the map, only add the next measurement outcome + it_cell->second.push_back({measurements[0], measurements[1], measurements[2]}); + } +} + + +std::pair, std::vector> TeorethicInformation::Bresenham(int x_1, int y_1, int x_2, int y_2) +{ + /* + To find the set of cells hit by a laser beam + */ + std::vector x_bres; + std::vector y_bres; + + int x = x_1; + int y = y_1; + + int delta_x = abs(x_2 - x_1); + int delta_y = abs(y_2 - y_1); + + int s_x = getSign(x_1, x_2); + int s_y = getSign(y_1, y_2); + bool interchange = false; + + if (delta_y > delta_x) + { + int temp = delta_x; + delta_x = delta_y; + delta_y = temp; + interchange = true; + } + else + { + interchange = false; + } + + int a_res = 2 * delta_y; + int b_res = 2 * (delta_y - delta_x); + int e_res = (2 * delta_y) - delta_x; + + x_bres.push_back(x); + y_bres.push_back(y); + + for (int i = 1; i < delta_x; ++i) + { + if (e_res < 0) + { + if (interchange) + { + y += s_y; + } + else + { + x += s_x; + } + e_res += a_res; + } + else + { + y += s_y; + x += s_x; + e_res += b_res; + } + x_bres.push_back(x); + y_bres.push_back(y); + } + return std::pair, std::vector>{x_bres, y_bres}; +} + +float TeorethicInformation::calculateEntropy(float probability) +{ + /* + To calculate the entropy + */ + return probability * log2(probability); +} + + +std::vector TeorethicInformation::calculateCellIntersectionPoints( + std::vector & laser_start, std::vector & laser_end, + std::vector & cell_start, std::vector & cell_end) +{ + /* + Initial point laser beam: laser_start + Final point laser beam: laser_end + Initial point cell: cell_start + Final point cell: cell_end + */ + float x1 = laser_start[0]; + float x2 = laser_end[0]; + float x3 = cell_start[0]; + float x4 = cell_end[0]; + + float y1 = laser_start[1]; + float y2 = laser_end[1]; + float y3 = cell_start[1]; + float y4 = cell_end[1]; + + float den = ((x2-x1)*(y4-y3) - (x4-x3)*(y2-y1)); + if (den == 0.0f) + { + // Parallel lines or not intersection at all + return {}; + } + else + { + float x = ((x2*y1 - x1*y2)*(x4 - x3) - (x4*y3 - x3*y4)*(x2-x1)) / den; + float y = ((x2*y1 - x1*y2)*(y4 - y3) - (x4*y3 - x3*y4)*(y2-y1)) / den; + return {x, y}; + } +} + +void TeorethicInformation::updateCellLimits( + std::vector & initial_x, std::vector & initial_y, std::vector & final_x, std::vector & final_y, + float & limit_x, float & limit_y, std::vector & cell_limits, std::vector & robot_grid_pos, std::vector & final_grid_pos) +{ + /* + To calculate grid grid limits for intersection + */ + if (final_grid_pos[0] < robot_grid_pos[0] && final_grid_pos[1] >= robot_grid_pos[1]) + { + // X greater and Y greater. WRO final points + final_x[0] = limit_x + m_resolution; + final_x[2] = limit_x + m_resolution; + + cell_limits[2] = limit_y; + cell_limits[3] = limit_y + m_resolution; + } + + if (final_grid_pos[0] >= robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) + { + // X greater and Y minor. WRO final points + initial_y[2] = limit_y - m_resolution; + initial_y[3] = limit_y - m_resolution; + + final_y[1] = limit_y - m_resolution; + final_y[3] = limit_y - m_resolution; + + cell_limits[2] = limit_y - m_resolution; + cell_limits[3] = limit_y; + } + + if (final_grid_pos[0] < robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) + { + // X minor and Y minor. WRO final points + initial_x[2] = limit_x - m_resolution; + initial_x[3] = limit_x - m_resolution; + initial_y[2] = limit_y - m_resolution; + initial_y[3] = limit_y - m_resolution; + + final_x[0] = limit_x - m_resolution; + final_x[2] = limit_x - m_resolution; + final_y[1] = limit_y - m_resolution; + final_y[3] = limit_y - m_resolution; + + cell_limits[0] = limit_x - m_resolution; + cell_limits[1] = limit_x; + cell_limits[2] = limit_y - m_resolution; + cell_limits[3] = limit_y; + } +} + +float TeorethicInformation::calculateLogs(float probability) +{ + /* + To calculate the log-odds + */ + return log(probability / (1 - probability)); +} + +float TeorethicInformation::calculateMapMutualInformation() +{ + /* + To calculate map mutual information, this is the summation + of all cells mutual information + */ + float sum = 0.0f; + for (int i = 0; i < m_num_cells; ++i) + { + for (int j = 0; j < m_num_cells; ++j) + { + sum += m_mutual_grid[i][j]; + } + } + return sum; +} + +void TeorethicInformation::computeProbabilities(std::vector>& meas_outcm) +{ + /* + To compute all the possible combinations of a grid cell, given a set of measurement outcomes + */ + // Cleaning measurement outcomes map + m_map_out.clear(); + std::unordered_map::iterator it_out; + + // The number of measurements + int k = meas_outcm.size(); + int r = 1; + + float p_free = meas_outcm[0][2]; + float p_occ = meas_outcm[0][1]; + float p_un = meas_outcm[0][0]; + + // Root + m_map_out.insert(map_pair(std::make_tuple(0, 0, 0), 1.0f)); + + // First measurement + m_map_out.insert(map_pair(std::make_tuple(1, 0, 0), p_free)); + m_map_out.insert(map_pair(std::make_tuple(0, 1, 0), p_occ)); + m_map_out.insert(map_pair(std::make_tuple(0, 0, 1), p_un)); + + for (int i = r; r < k; ++r) + { + std::vector tup_vct; + std::vector acc_prob; + + for (it_out = m_map_out.begin(); it_out != m_map_out.end(); ++it_out) + { + // Index + int fr_idx = std::get<0>(it_out->first); + int oc_idx = std::get<1>(it_out->first); + int un_idx = std::get<2>(it_out->first); + + // Measurement outcome probability + float free_prop = meas_outcm[r][0]; + float occ_prop = meas_outcm[r][1]; + float un_prop = meas_outcm[r][2]; + + if (fr_idx + oc_idx + un_idx == r) + { + // Searching for the current combination in this level + std::vector::iterator it_comb; + it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(fr_idx + 1, oc_idx, un_idx)); + + // Free + if (it_comb != tup_vct.end()) + { + acc_prob[it_comb - tup_vct.begin()] += it_out->second * free_prop; + } + else + { + tup_vct.push_back(std::make_tuple(fr_idx + 1, oc_idx, un_idx)); + acc_prob.push_back(it_out->second * free_prop); + } + + it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(fr_idx, oc_idx + 1, un_idx)); + + // Occupied + if (it_comb != tup_vct.end()) + { + acc_prob[it_comb - tup_vct.begin()] += it_out->second * occ_prop; + } + else + { + tup_vct.push_back(std::make_tuple(fr_idx, oc_idx + 1, un_idx)); + acc_prob.push_back(it_out->second * occ_prop); + } + + it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(fr_idx, oc_idx, un_idx + 1)); + + // Unobserved + if (it_comb != tup_vct.end()) + { + acc_prob[it_comb - tup_vct.begin()] += it_out->second * un_prop; + } + else + { + tup_vct.push_back(std::make_tuple(fr_idx, oc_idx, un_idx + 1)); + acc_prob.push_back(it_out->second * un_prop); + } + } + } + // Inserting the elements into the map + for (int k = 0; k < tup_vct.size(); ++k) + { + m_map_out.insert(map_pair(tup_vct[k], acc_prob[k])); + } + } +} + +float TeorethicInformation::euclideanDistance(float x_1, float y_1, float x_2, float y_2) +{ + /* + To calculate the euclidean distance between two points + */ + float diff_x = x_2 - x_1; + float diff_y = y_2 - y_1; + + return sqrt(diff_x*diff_x + diff_y*diff_y); +} + +int TeorethicInformation::getSign(int n_1, int n_2) +{ + /* + To get the sign of an operation, used for Bresenham algorithm + */ + int difference = n_2 - n_1; + + if (difference == 0) { return 0; } + else if (difference < 0) { return -1; } + else { return 1; } +} + +std::vector TeorethicInformation::getGridPosition(float x, float y) +{ + /* + To maps the current position into grid coordinates + */ + int x_cell = floor((1 / m_resolution) * x); + int y_cell = floor((1 / m_resolution) * y); + + return {x_cell, y_cell}; +} + +std::vector TeorethicInformation::laserHitDistance(std::vector const& robot_pose, float range, float angle) +{ + /* + To get the distance where the laser beam hits something + - Applying RBT from the global to the sensor frame + */ + float x_tf = (range * cos(robot_pose[2] + angle)) + robot_pose[0]; + float y_tf = (range * sin(robot_pose[2] + angle)) + robot_pose[1]; + + return {x_tf, y_tf}; +} + +void TeorethicInformation::initializeGrids() +{ + /* + To create the grid + */ + for (int i = 0; i < m_num_cells; ++i) + { + // Adding columns + m_grid[i].resize(m_num_cells); + m_mutual_grid[i].resize(m_num_cells); + for (int j = 0; j < m_num_cells; ++j) + { + m_grid[i][j] = 0; + m_mutual_grid[i][j] = 0.0f; + } + } +} + +float TeorethicInformation::measurementOutcomeEntropy(map_tuple const& meas_outcome) +{ + /* + To calculate the measurement outcome entropy (Measurement outcome in the form ) + - Calculate Log-Odds from initial probability guess + - Calculate the probability from those logs + - Calculate the entropy with the retrieved probability + */ + float entropy = std::get<0>(meas_outcome) * calculateEntropy(probabilityFromLogs(calculateLogs(0.3f))) + + std::get<1>(meas_outcome) * calculateEntropy(probabilityFromLogs(calculateLogs(0.7f))) + + std::get<2>(meas_outcome) * calculateEntropy(probabilityFromLogs(calculateLogs(0.5f))); + return -entropy; +} + +float TeorethicInformation::probabilityFromLogs(float log) +{ + /* + To transform the Log-odds into probability + */ + return (exp(log) / (1 + exp(log))); +} + +float TeorethicInformation::probabilityFromObservation(float range_1, float range_2) +{ + /* + To calculate the probability of a cell being observed by a given measurement + */ + + float max_range = 5.0f; + float lambda = 0.35f; + float nu = 0.28f; + + range_1 = (range_1 > max_range) ? max_range : range_1; + range_2 = (range_2 > max_range) ? max_range : range_2; + + return nu * (exp(-lambda*range_1) - exp(-lambda*range_2)); +} + +std::vector> TeorethicInformation::retreiveMeasurementOutcomes() +{ + /* + To get all the measurement outcomes for the current cell + */ + + std::map, std::vector>>::iterator it_cells; + + it_cells = m_cell_probabilities.find({m_cell_x, m_cell_y}); + std::vector> meas_outcomes; + + if (it_cells != m_cell_probabilities.end()) + { + // Exploring the measurement outcomes for the specific cell + for (int i = 0; i < it_cells->second.size(); ++i) + { + // Free , occupied, not observed + meas_outcomes.push_back({it_cells->second[i][0], it_cells->second[i][1], it_cells->second[i][2]}); + } + } + return meas_outcomes; +} + + +void TeorethicInformation::updateCellMutualInformation(float mut_inf) +{ + /* + To update the mutual information for each individual cell + This is the result of the summation of 3.12 + */ + m_mutual_grid[m_cell_x][m_cell_x] = mut_inf; +} From 488868b6bfa384044e9f7f7fb969e060a15e230b Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Wed, 15 Dec 2021 08:21:16 -0500 Subject: [PATCH 29/83] [WIP]: Completing theoretical information object --- .../experimental/theoretic_information.hpp | 10 +-- src/experimental/theoretic_information.cpp | 72 ++++++++++--------- 2 files changed, 45 insertions(+), 37 deletions(-) diff --git a/include/slam_toolbox/experimental/theoretic_information.hpp b/include/slam_toolbox/experimental/theoretic_information.hpp index b9fe731b6..9d9d91b68 100644 --- a/include/slam_toolbox/experimental/theoretic_information.hpp +++ b/include/slam_toolbox/experimental/theoretic_information.hpp @@ -45,7 +45,7 @@ class TeorethicInformation std::pair, std::vector> Bresenham(int x_1, int y_1, int x_2, int y_2); std::vector getGridPosition(float x, float y); std::vector laserHitDistance(std::vector const& robot_pose, float range, float angle); - std::vector calculateCellIntersectionPoints(std::vector & laser_start, std::vector & laser_end, std::vector & cell_start, std::vector & cell_end); + std::vector calculateCellIntersectionPoints(std::vector & laser_start, std::vector & laser_end, std::vector cell_start, std::vector cell_end); int getSign(int n_1, int n_2); // Measurements calculations @@ -59,12 +59,12 @@ class TeorethicInformation float measurementOutcomeEntropy(map_tuple const& meas_outcome); float probabilityFromLogs(float log); void recoverProbability(); - void updateCellMutualInformation(float mut_inf); + void updateCellMutualInformation(float mut_inf, std::vector cell); // Measurement outcomes probabilities - void appendCellProbabilities(std::vector& measurements); - void computeProbabilities(std::vector>& meas_outcm); - std::vector> retreiveMeasurementOutcomes(); + void appendCellProbabilities(std::vector& measurements, std::vector cell); + std::unordered_map computeProbabilities(std::vector>& meas_outcm); + std::vector> retreiveMeasurementOutcomes(std::vector cell); std::vector unhashIndex(int hash); private: diff --git a/src/experimental/theoretic_information.cpp b/src/experimental/theoretic_information.cpp index c525587e2..8ed3a1a6b 100644 --- a/src/experimental/theoretic_information.cpp +++ b/src/experimental/theoretic_information.cpp @@ -115,6 +115,7 @@ void TeorethicInformation::scannerTest() std::vector distances; for (int k = 0; k < inter_x.size(); ++k) { + // From robot position to intersection points float dist_point = euclideanDistance(robot_poses[r][0], robot_poses[r][1], inter_x[k], inter_y[k]); distances.push_back(dist_point); } @@ -124,11 +125,11 @@ void TeorethicInformation::scannerTest() // d2 which is distance from robot pose to second point where the cell is cut // Integral 3: d2 which is distance from robot pose to second point where the cell is cut to z_max - // Measurement outcomes vector + // Measurement outcomes vector {Pfree, Pocc, Pun} std::vector probabilities { - probabilityFromObservation(distances[1], 5.0f), // Free - probabilityFromObservation(distances[0], distances[1]), // Occupied - probabilityFromObservation(0.0f, distances[0]) // Not observed + probabilityFromObservation(distances[1], 5.0f), + probabilityFromObservation(distances[0], distances[1]), + probabilityFromObservation(0.0f, distances[0]) }; // Assigning the cells @@ -136,30 +137,37 @@ void TeorethicInformation::scannerTest() m_cell_y = cells_y[j]; // Appending new measurement outcomes for the current cell - appendCellProbabilities(probabilities); + appendCellProbabilities(probabilities, {cells_x[j], cells_y[j]}); // Get all the measurement outcomes for the current cell - std::vector> meas_outcomes = retreiveMeasurementOutcomes(); + std::vector> meas_outcomes = retreiveMeasurementOutcomes({cells_x[j], cells_y[j]}); + // Compute all the possible combinations for the current cell - algorithm 1 - computeProbabilities(meas_outcomes); + std::unordered_map meas_out_prob = computeProbabilities(meas_outcomes); // Calculate 3.12 std::unordered_map::iterator it_mutual; - std::cout << "Number of measurements: " << meas_outcomes.size() << std::endl; float cell_mutual_inf = 0.0f; - for (it_mutual = m_map_out.begin(); it_mutual != m_map_out.end(); ++it_mutual) + for (it_mutual = meas_out_prob.begin(); it_mutual != meas_out_prob.end(); ++it_mutual) { // Interested in the final measurement outcomes if (std::get<0>(it_mutual->first) + std::get<1>(it_mutual->first) + std::get<2>(it_mutual->first) == meas_outcomes.size()) { + // measurementOutcomeEntropy is negative cell_mutual_inf += it_mutual->second * measurementOutcomeEntropy(it_mutual->first); + // std::cout << "----------Entropy---------- " << measurementOutcomeEntropy(it_mutual->first) << std::endl; + // std::cout << "----------Entropy---------- " << 0.3*log2(0.3) << std::endl; + // std::cout << "----------Entropy---------- " << calculateEntropy(0.7) << std::endl; + // std::cout << "----------Entropy---------- " << calculateEntropy(probabilityFromLogs(calculateLogs(0.7f))) << std::endl; } + } - std::cout << cell_mutual_inf << std::endl; // Here should be the H(C) = 0.5 : 0.5 - SUM(P*H) - updateCellMutualInformation(cell_mutual_inf); + std::cout << "Cell mutual information: " << 0.5 - cell_mutual_inf << std::endl; + // Mutual information of cell x, y given a set of measurements + updateCellMutualInformation(0.5 - cell_mutual_inf, {cells_x[j], cells_y[j]}); std::cout << "++++++++++++++++++++++++" << std::endl; } } @@ -168,7 +176,7 @@ void TeorethicInformation::scannerTest() } } -void TeorethicInformation::appendCellProbabilities(std::vector& measurements) +void TeorethicInformation::appendCellProbabilities(std::vector& measurements, std::vector cell) { /* To append a new measurement for a specific cell @@ -177,12 +185,12 @@ void TeorethicInformation::appendCellProbabilities(std::vector& measureme // Iterator for getting the cell std::map, std::vector>>::iterator it_cell; - it_cell = m_cell_probabilities.find({m_cell_x, m_cell_y}); + it_cell = m_cell_probabilities.find({cell[0], cell[1]}); if (it_cell == m_cell_probabilities.end()) { - // Cell is not present in the map + // Cell is not present in the map, so append it m_cell_probabilities.insert(std::pair, std::vector>>( - {m_cell_x, m_cell_y}, + {cell[0], cell[1]}, {{measurements[0], measurements[1], measurements[2]}} )); } @@ -268,7 +276,7 @@ float TeorethicInformation::calculateEntropy(float probability) std::vector TeorethicInformation::calculateCellIntersectionPoints( std::vector & laser_start, std::vector & laser_end, - std::vector & cell_start, std::vector & cell_end) + std::vector cell_start, std::vector cell_end) { /* Initial point laser beam: laser_start @@ -375,13 +383,13 @@ float TeorethicInformation::calculateMapMutualInformation() return sum; } -void TeorethicInformation::computeProbabilities(std::vector>& meas_outcm) +std::unordered_map TeorethicInformation::computeProbabilities(std::vector>& meas_outcm) { /* To compute all the possible combinations of a grid cell, given a set of measurement outcomes */ // Cleaning measurement outcomes map - m_map_out.clear(); + std::unordered_map map_out; std::unordered_map::iterator it_out; // The number of measurements @@ -393,19 +401,19 @@ void TeorethicInformation::computeProbabilities(std::vector>& float p_un = meas_outcm[0][0]; // Root - m_map_out.insert(map_pair(std::make_tuple(0, 0, 0), 1.0f)); + map_out.insert(map_pair(std::make_tuple(0, 0, 0), 1.0f)); // First measurement - m_map_out.insert(map_pair(std::make_tuple(1, 0, 0), p_free)); - m_map_out.insert(map_pair(std::make_tuple(0, 1, 0), p_occ)); - m_map_out.insert(map_pair(std::make_tuple(0, 0, 1), p_un)); + map_out.insert(map_pair(std::make_tuple(1, 0, 0), p_free)); + map_out.insert(map_pair(std::make_tuple(0, 1, 0), p_occ)); + map_out.insert(map_pair(std::make_tuple(0, 0, 1), p_un)); for (int i = r; r < k; ++r) { std::vector tup_vct; std::vector acc_prob; - for (it_out = m_map_out.begin(); it_out != m_map_out.end(); ++it_out) + for (it_out = map_out.begin(); it_out != map_out.end(); ++it_out) { // Index int fr_idx = std::get<0>(it_out->first); @@ -464,9 +472,10 @@ void TeorethicInformation::computeProbabilities(std::vector>& // Inserting the elements into the map for (int k = 0; k < tup_vct.size(); ++k) { - m_map_out.insert(map_pair(tup_vct[k], acc_prob[k])); + map_out.insert(map_pair(tup_vct[k], acc_prob[k])); } } + return map_out; } float TeorethicInformation::euclideanDistance(float x_1, float y_1, float x_2, float y_2) @@ -544,7 +553,7 @@ float TeorethicInformation::measurementOutcomeEntropy(map_tuple const& meas_outc float entropy = std::get<0>(meas_outcome) * calculateEntropy(probabilityFromLogs(calculateLogs(0.3f))) + std::get<1>(meas_outcome) * calculateEntropy(probabilityFromLogs(calculateLogs(0.7f))) + std::get<2>(meas_outcome) * calculateEntropy(probabilityFromLogs(calculateLogs(0.5f))); - return -entropy; + return entropy; } float TeorethicInformation::probabilityFromLogs(float log) @@ -571,23 +580,22 @@ float TeorethicInformation::probabilityFromObservation(float range_1, float rang return nu * (exp(-lambda*range_1) - exp(-lambda*range_2)); } -std::vector> TeorethicInformation::retreiveMeasurementOutcomes() +std::vector> TeorethicInformation::retreiveMeasurementOutcomes(std::vector cell) { /* To get all the measurement outcomes for the current cell */ - std::map, std::vector>>::iterator it_cells; - - it_cells = m_cell_probabilities.find({m_cell_x, m_cell_y}); std::vector> meas_outcomes; + std::map, std::vector>>::iterator it_cells; + it_cells = m_cell_probabilities.find({cell[0], cell[1]}); if (it_cells != m_cell_probabilities.end()) { // Exploring the measurement outcomes for the specific cell for (int i = 0; i < it_cells->second.size(); ++i) { - // Free , occupied, not observed + // Append the measurement outcomes for the current cell meas_outcomes.push_back({it_cells->second[i][0], it_cells->second[i][1], it_cells->second[i][2]}); } } @@ -595,11 +603,11 @@ std::vector> TeorethicInformation::retreiveMeasurementOutcome } -void TeorethicInformation::updateCellMutualInformation(float mut_inf) +void TeorethicInformation::updateCellMutualInformation(float mut_inf, std::vector cell) { /* To update the mutual information for each individual cell This is the result of the summation of 3.12 */ - m_mutual_grid[m_cell_x][m_cell_x] = mut_inf; + m_mutual_grid[cell[0]][cell[1]] = mut_inf; } From 64b8756d6b89639827a3318152523945969a8492 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Wed, 15 Dec 2021 10:25:25 -0500 Subject: [PATCH 30/83] [WIP]: Completing theoretical information object, removing old code and adding metric for repeated measurements --- .../experimental/slam_toolbox_lifelong.hpp | 91 --- .../experimental/theoretic_information.hpp | 6 +- src/experimental/slam_toolbox_lifelong.cpp | 694 ------------------ src/experimental/theoretic_information.cpp | 81 +- src/slam_toolbox_common.cpp | 2 - 5 files changed, 60 insertions(+), 814 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index 35feac730..e91bfb3a2 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -55,41 +55,6 @@ class LifelongSlamToolbox : public SlamToolbox LocalizedRangeScan * s1, LocalizedRangeScan * s2, double & x_l, double & x_u, double & y_l, double & y_u); -public: - // Cell occupancy struct for unordered map - - struct HashTuple - { - std::size_t operator() (map_tuple const& key) const - { - 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; - } - }; - struct Occupancy - { - int fr, oc ,un; - bool operator==(Occupancy const& st) const - { - return (st.fr == fr) && (st.oc == oc) && (st.un == un); - } - - struct CombinationsHash - { - std::size_t operator()(Occupancy const& key) const - { - std::size_t hash = 5381u; - hash = (hash << 5) + hash + key.fr; - hash = (hash << 5) + hash + key.oc; - hash = (hash << 5) + hash + key.un; - return hash; - } - }; - }; - protected: void laserCallback( sensor_msgs::msg::LaserScan::ConstSharedPtr scan) override; @@ -108,62 +73,6 @@ class LifelongSlamToolbox : public SlamToolbox void updateScoresSlamGraph(const double & score, Vertex * vertex); void checkIsNotNormalized(const double & value); - /*******************************Implementation********************************/ - void scannerTest(); - - // Grid operations - void initializeGrids(); - void calculateLimits( - std::vector & initial_x, std::vector & initial_y, std::vector & final_x, std::vector & final_y, - float & limit_x, float & limit_y, float & min_x, float & max_x, float & min_y, float & max_y, std::vector & robot_grid_pos, - std::vector & final_grid_pos); - - // Grid and position information - std::pair, std::vector> Bresenham(int x_1, int y_1, int x_2, int y_2); - std::vector getGridPosition(float x, float y); - std::vector laserHitDistance(std::vector const& robot_pose, float range, float angle); - std::vector calculateIntersection(std::vector laser_start, std::vector laser_end, std::vector cell_start, std::vector cell_end); - int getSign(int n_1, int n_2); - - // Measurements calculations - float probabilityFromObservation(float range_1, float range_2); - float euclideanDistance(float x_1, float y_1, float x_2, float y_2); - - // Mutual information - float measurementOutcomeEntropy(map_tuple const& meas_outcome); - - void recoverProbability(); - float calculateLogs(float probability); - float probabilityFromLogs(float log); - float calculateEntropy(float probability); - - float calculateMapMutualInformation(); - void updateCellMutualInformation(float mut_inf_val); - - // Measurement outcomes probabilities - void appendCellProbabilities(std::vector& meas_outcomes); - void computeProbabilities(std::vector>& meas_outcm); - std::vector> retreiveMeasurementOutcomes(); - std::vector unhashIndex(int hash); - - // Data structures - std::unordered_map m_map_out; - std::unordered_map m_un_cmb; - std::map, std::vector>> m_cell_probabilities; - std::vector> m_mutual_grid; - std::vector> m_grid; - float m_map_dist; - float m_resolution; - int m_cell_x; - int m_cell_y; - int m_num_cells; - - // Robot information - Defining values just for testing - std::vector> robot_poses {{5.6f, 6.0f, M_PI/2}, {3.5f, 9.0f, 0.0f}}; - std::vector> laser_ranges {{1.65f, 5.0f, 5.0f, 5.0f, 5.0f}, {5.0f, 5.0f, 4.0f, 5.0f, 5.0f}}; - std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; - - /*****************************************************************************/ bool use_tree_; double iou_thresh_; double removal_score_; diff --git a/include/slam_toolbox/experimental/theoretic_information.hpp b/include/slam_toolbox/experimental/theoretic_information.hpp index 9d9d91b68..189a6b826 100644 --- a/include/slam_toolbox/experimental/theoretic_information.hpp +++ b/include/slam_toolbox/experimental/theoretic_information.hpp @@ -67,6 +67,8 @@ class TeorethicInformation std::vector> retreiveMeasurementOutcomes(std::vector cell); std::vector unhashIndex(int hash); + void clearVisitedCells(); + private: // Data structures std::unordered_map m_map_out; @@ -75,14 +77,14 @@ class TeorethicInformation std::vector> m_grid; float m_map_dist; float m_resolution; - int m_cell_x; - int m_cell_y; int m_num_cells; // Robot information - Defining values just for testing std::vector> robot_poses {{5.6f, 6.0f, M_PI/2}, {3.5f, 9.0f, 0.0f}}; std::vector> laser_ranges {{1.65f, 5.0f, 5.0f, 5.0f, 5.0f}, {5.0f, 5.0f, 4.0f, 5.0f, 5.0f}}; std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + + std::vector> visited_cells; }; #endif \ No newline at end of file diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index cd8f4bd8c..a973073f9 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -80,703 +80,9 @@ LifelongSlamToolbox::LifelongSlamToolbox(rclcpp::NodeOptions options) // in lifelong mode, we cannot have interactive mode enabled enable_interactive_mode_ = false; - - // m_resolution = 0.5f; // Map resolution - // m_map_dist = 20.0f; // Total map distance - // m_num_cells = static_cast(m_map_dist / m_resolution); - - // Grids initialization (Occupancy and Mutual information) - // m_grid.resize(m_num_cells); - // m_mutual_grid.resize(m_num_cells); - // initializeGrids(); - - // scannerTest(); TeorethicInformation information; } -/*****************************************************************************/ -/** - * The are implementation notes from Camilo OCT 21 - * - * First step - * Get the occupancy probability for each cell in the map. This solution is based - * on the Log-Odds rather than the Bayesian filter, only for computation performance - * Initialization for this elements should be made before we start the following loop: - * Gather the sensor data. - * Inverse measurement model (Tranforming scan to measure occupancy belief map) - * Log-Odds calculation for the current reading and update the grid or map. - * Probability map calculation and update for current reading (This is done for the cells)\ - * - * NOTE - * Once we get the probabilities we can calculate the entropy of each grid cell. I am assuming - * this should be done inside the loop as we want to calculate this entropy at each time. - - * Log odds assume values from −∞ to ∞. The Bayes filter for updating beliefs in - * log odds representation is computationally elegant. It avoids truncation problems that - * arise for probabilities close to 0 or 1. - * - * - * We are using std::vector !! - * - * - * - * According tothe the last meeting we handle, I will start with a prrof of concept - * Create a grid - * Calculate the scan position (WRO a given cell). Consider where is my initial cell - * Calculate the probability of seing an grid given a set of measurements - * Create the histogram with Algrotihm 1 -/*****************************************************************************/ - -/*****************************************************************************/ -/*******************************Implementation********************************/ - -void LifelongSlamToolbox::scannerTest() -{ - // Loop through the different robot poses - for (int r = 0; r < robot_poses.size(); ++r) - { - std::cout << "---------- Robot pose ----------: " << r << std::endl; - - // Angles {-50, -25, 0, 25, 50} in degrees - std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; - - // Initial point - std::vector robot_grid_pos = getGridPosition(robot_poses[r][0], robot_poses[r][1]); - std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; - - // Current yaw + beam angle: -PI/2 (-1.570795) -0.87266 = 2.44345 (-55 degrees) - for (int i = 0; i < laser_ranges[r].size(); ++i) - { - std::cout << "........ New laser ........" << std::endl; - std::cout << "Distance: " << laser_ranges[r][i] << ", Angle: " << angles[i] << std::endl; - - // Laser continuous distance - std::vector laser_grid = laserHitDistance(robot_poses[r], laser_ranges[r][i], angles[i]); - - // Laser final cell - std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1]); - - // robot_grid_pos[0] // X1 - robot_grid_pos[1] // Y1 - // final_grid_pos[0] // X2 - final_grid_pos[1] // Y2 - - // Ray tracing for getting the visited cells - std::vector cells_x, cells_y; - std::pair, std::vector> res_pair = Bresenham(robot_grid_pos[0], robot_grid_pos[1], final_grid_pos[0], final_grid_pos[1]); - cells_x = res_pair.first; - cells_y = res_pair.second; - - // Adding last hit cell to the set - cells_x.push_back(final_grid_pos[0]); - cells_y.push_back(final_grid_pos[1]); - - // std::cout << "Cells" << std::endl; - // for (int c = 0; c < cells_x.size(); ++c) // One reading only - // { - // std::cout << cells_x[c] << ", " << cells_y[c] << std::endl; - // } - // std::cout << "End of cells" << std::endl; - - // Visiting the cells - for (int j = 0; j < cells_x.size(); ++j) - { - if ((robot_grid_pos[0] == cells_x[j]) && (robot_grid_pos[1] == cells_y[j])) - { - // Current cell is not relevant at all - continue; - } - - // Cells visualization - std::cout << "Current cell: " << cells_x[j] << ", " << cells_y[j] << std::endl; - - // Cells limits - float limit_x = cells_x[j] * m_resolution; - float limit_y = cells_y[j] * m_resolution; - - float min_x = limit_x; - float max_x = limit_x + m_resolution; - float min_y = limit_y; - float max_y = limit_y + m_resolution; - - std::vector initial_x {limit_x, limit_x, limit_x + m_resolution, limit_x + m_resolution}; - std::vector initial_y {limit_y, limit_y, limit_y + m_resolution, limit_y + m_resolution}; - - std::vector final_x {limit_x + m_resolution, limit_x, limit_x + m_resolution, limit_x}; - std::vector final_y {limit_y, limit_y + m_resolution, limit_y, limit_y + m_resolution}; - - calculateLimits(initial_x, initial_y, final_x, final_y, limit_x, limit_y, min_x, max_x, min_y, max_y, robot_grid_pos, final_grid_pos); - - std::vector inter_x, inter_y; - for (int k = 0; k < 4; ++k) - { - std::vector intersection = calculateIntersection(robot_poses[r], laser_grid, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); - - if(intersection.size() != 0) - { - // If the laser and a cell intersects, we need to make sure it happens in the right bounds - if ((abs(intersection[0]) >= abs(min_x - 0.01f)) && - (abs(intersection[0]) <= abs(max_x + 0.01f)) && - (abs(intersection[1]) >= abs(min_y - 0.01f)) && - (abs(intersection[1]) <= abs(max_y + 0.01f))) - { - /* - 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[0]); - inter_y.push_back(intersection[1]); - } - } - } - - // When a cell is marked by Bresenham but there is not intersection points - if (inter_x.size() == 0) - continue; - - // Enter (d1) and Exit (d2) distances - std::vector distances; - for (int k = 0; k < inter_x.size(); ++k) - { - float dist_point = euclideanDistance(robot_poses[r][0], robot_poses[r][1], inter_x[k], inter_y[k]); - distances.push_back(dist_point); - } - - // Integral 1: 0 to d1 which is distance from robot pose to first point where the cell is cut - // Integral 2: d1 which is distance from robot pose to first point where the cell is cut to - // d2 which is distance from robot pose to second point where the cell is cut - // Integral 3: d2 which is distance from robot pose to second point where the cell is cut to z_max - - // Measurement outcomes vector - std::vector probabilities { - probabilityFromObservation(distances[1], 5.0f), // Free - probabilityFromObservation(distances[0], distances[1]), // Occupied - probabilityFromObservation(0.0f, distances[0]) // Not observed - }; - - // Assigning the cells - m_cell_x = cells_x[j]; - m_cell_y = cells_y[j]; - - // Appending new measurement outcomes for the current cell - appendCellProbabilities(probabilities); - - - - // Get all the measurement outcomes for the current cell - std::vector> meas_outcomes = retreiveMeasurementOutcomes(); - // Compute all the possible combinations for the current cell - algorithm 1 - computeProbabilities(meas_outcomes); - - - // Calculate 3.12 - std::unordered_map::iterator it_mutual; - - std::cout << "Number of measurements: " << meas_outcomes.size() << std::endl; - float cell_mutual_inf = 0.0f; - for (it_mutual = m_map_out.begin(); it_mutual != m_map_out.end(); ++it_mutual) - { - // Interested in the final measurement outcomes - if (std::get<0>(it_mutual->first) + std::get<1>(it_mutual->first) + std::get<2>(it_mutual->first) == meas_outcomes.size()) - // if (it_mutual->first.fr + it_mutual->first.oc + it_mutual->first.un == meas_outcomes.size()) - { - cell_mutual_inf += it_mutual->second * measurementOutcomeEntropy(it_mutual->first); - } - } - - /* - At this point I would need to calculate the H(C) as it stills and icognite here - - - According to explanation provided in section 2 - - I need to separate here the calculations. Which means Entropy and Mutual Information - must be calculated outside this loop - - Start documenting this section - - I need to add more poses and more ranges in order to see mutual information impact - */ - std::cout << cell_mutual_inf << std::endl; - // Here should be the H(C) = 0.5 : 0.5 - SUM(P*H) - updateCellMutualInformation(cell_mutual_inf); - std::cout << "++++++++++++++++++++++++" << std::endl; - } - } - float mutual = calculateMapMutualInformation(); - std::cout << "Mutual information: " << mutual << std::endl; - } -} - - - -void LifelongSlamToolbox::updateCellMutualInformation(float mut_inf_val) -{ - /* - To update the mutual information for each individual cell - This is the result of the summation of 3.12 - */ - // std::cout << "Updating cell: " << m_cell_x << ", " << m_cell_y << " , with: " << mut_inf_val << std::endl; - m_mutual_grid[m_cell_x][m_cell_x] = mut_inf_val; -} - -float LifelongSlamToolbox::calculateMapMutualInformation() -{ - /* - To calculate map mutual information, this is the summation - of all cells mutual information - */ - float sum = 0.0f; - for (int i = 0; i < m_num_cells; ++i) - { - for (int j = 0; j < m_num_cells; ++j) - { - sum += m_mutual_grid[i][j]; - } - } - return sum; -} - -void LifelongSlamToolbox::calculateLimits( - std::vector & initial_x, std::vector & initial_y, std::vector & final_x, std::vector & final_y, - float & limit_x, float & limit_y, float & min_x, float & max_x, float & min_y, float & max_y, std::vector & robot_grid_pos, - std::vector & final_grid_pos) -{ - /* - To calculate grid grid limits for intersection - */ - if (final_grid_pos[0] < robot_grid_pos[0] && final_grid_pos[1] >= robot_grid_pos[1]) - { - // X greater and Y greater. WRO final points - final_x[0] = limit_x + m_resolution; - final_x[2] = limit_x + m_resolution; - - min_y = limit_y; - max_y = limit_y + m_resolution; - } - - if (final_grid_pos[0] >= robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) - { - // X greater and Y minor. WRO final points - initial_y[2] = limit_y - m_resolution; - initial_y[3] = limit_y - m_resolution; - - final_y[1] = limit_y - m_resolution; - final_y[3] = limit_y - m_resolution; - - min_y = limit_y - m_resolution; - max_y = limit_y; - } - - if (final_grid_pos[0] < robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) - { - // X minor and Y minor. WRO final points - initial_x[2] = limit_x - m_resolution; - initial_x[3] = limit_x - m_resolution; - initial_y[2] = limit_y - m_resolution; - initial_y[3] = limit_y - m_resolution; - - final_x[0] = limit_x - m_resolution; - final_x[2] = limit_x - m_resolution; - final_y[1] = limit_y - m_resolution; - final_y[3] = limit_y - m_resolution; - - min_x = limit_x - m_resolution; - max_x = limit_x; - min_y = limit_y - m_resolution; - max_y = limit_y; - } -} - -void LifelongSlamToolbox::initializeGrids() -{ - /* - To create the grid - */ - for (int i = 0; i < m_num_cells; ++i) - { - // Adding columns - m_grid[i].resize(m_num_cells); - m_mutual_grid[i].resize(m_num_cells); - for (int j = 0; j < m_num_cells; ++j) - { - m_grid[i][j] = 0; - m_mutual_grid[i][j] = 0.0f; - } - } -} - -float LifelongSlamToolbox::measurementOutcomeEntropy(map_tuple const& meas_outcome) -{ - /* - To calculate the measurement outcome entropy (Measurement outcome in the form ) - Calculate Log-Odds from initial probability guess - Calculate the probability from those logs - Calculate the entropy with the retrieved probability - */ - // float cells = (std::get<0>(meas_outcome) * calculateLogs(0.3f)) + (std::get<1>(meas_outcome) * calculateLogs(0.7f)) + (std::get<2>(meas_outcome) * calculateLogs(0.5f)); - - // This might change - float entropy = std::get<0>(meas_outcome) * calculateEntropy(probabilityFromLogs(calculateLogs(0.3f))) + - std::get<1>(meas_outcome) * calculateEntropy(probabilityFromLogs(calculateLogs(0.7f))) + - std::get<2>(meas_outcome) * calculateEntropy(probabilityFromLogs(calculateLogs(0.5f))); - - /* - The entropy calculation will be negative - Which is this function - Then the sumatory of 3.12 should be substracted - Mutual information should have a positive value - */ - // This might be different because this summatory should be - // return -calculateEntropy(probabilityFromLogs(cell_logs)); - return -entropy; -} - -float LifelongSlamToolbox::calculateEntropy(float probability) -{ - /* - To calculate the entropy - */ - return probability * log2(probability); -} - -std::vector> LifelongSlamToolbox::retreiveMeasurementOutcomes() -{ - /* - To get all the measurement outcomes for the current cell - */ - - std::map, std::vector>>::iterator it_cells; - - it_cells = m_cell_probabilities.find({m_cell_x, m_cell_y}); - std::vector> meas_outcomes; - - if (it_cells != m_cell_probabilities.end()) - { - // Exploring the measurement outcomes for the specific cell - for (int i = 0; i < it_cells->second.size(); ++i) - { - // Free , occupied, not observed - meas_outcomes.push_back({it_cells->second[i][0], it_cells->second[i][1], it_cells->second[i][2]}); - } - } - return meas_outcomes; -} - -void LifelongSlamToolbox::appendCellProbabilities(std::vector& meas_outcomes) -{ - /* - To append a new measurement for a specific cell - */ - - // Iterator for getting the cell - std::map, std::vector>>::iterator it_cell; - - it_cell = m_cell_probabilities.find({m_cell_x, m_cell_y}); - if (it_cell == m_cell_probabilities.end()) - { - // Cell is not present in the map - m_cell_probabilities.insert(std::pair, std::vector>>( - {m_cell_x, m_cell_y}, - {{meas_outcomes[0], meas_outcomes[1], meas_outcomes[2]}} - )); - } - else - { - // Cell is already in the map, only add the next measurement outcome - it_cell->second.push_back({meas_outcomes[0], meas_outcomes[1], meas_outcomes[2]}); - } -} - -void LifelongSlamToolbox::computeProbabilities(std::vector>& meas_outcm) -{ - /* - To compute all the possible combinations of a grid cell, given a set of measurement outcomes - */ - - m_map_out.clear(); - int k = meas_outcm.size(); // The number of measurements - // std::cout << "K: " << k << std::endl; - - int r = 1; - - float p_free = meas_outcm[0][2]; - float p_occ = meas_outcm[0][1]; - float p_un = meas_outcm[0][0]; - - std::unordered_map::iterator it_out; - // Root - m_map_out.insert(map_pair(std::make_tuple(0, 0, 0), 1.0f)); - - // First measurement - m_map_out.insert(map_pair(std::make_tuple(1, 0, 0), p_free)); - m_map_out.insert(map_pair(std::make_tuple(0, 1, 0), p_occ)); - m_map_out.insert(map_pair(std::make_tuple(0, 0, 1), p_un)); - - for (int i = r; r < k; ++r) - { - std::cout << "R: " << r << std::endl; - - std::vector tup_vct; - std::vector acc_prob; - - for (it_out = m_map_out.begin(); it_out != m_map_out.end(); ++it_out) - { - // Index - int fr_idx = std::get<0>(it_out->first); - int oc_idx = std::get<1>(it_out->first); - int un_idx = std::get<2>(it_out->first); - - // Measurement outcome probability - float free_prop = meas_outcm[r][0]; - float occ_prop = meas_outcm[r][1]; - float un_prop = meas_outcm[r][2]; - - if (fr_idx + oc_idx + un_idx == r) - { - // std::cout << "Propagating" << std::endl; - // std::cout << fr_idx + 1 << ", " << oc_idx << ", " << un_idx << std::endl; - // std::cout << fr_idx << ", " << oc_idx + 1 << ", " << un_idx << std::endl; - // std::cout << fr_idx << ", " << oc_idx << ", " << un_idx + 1 << std::endl; - - // Searching for the current combination in this level - std::vector::iterator it_comb; - it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(fr_idx + 1, oc_idx, un_idx)); - - // Free - if (it_comb != tup_vct.end()) - { - acc_prob[it_comb - tup_vct.begin()] += it_out->second * free_prop; - } - else - { - tup_vct.push_back(std::make_tuple(fr_idx + 1, oc_idx, un_idx)); - acc_prob.push_back(it_out->second * free_prop); - } - - it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(fr_idx, oc_idx + 1, un_idx)); - - // Occupied - if (it_comb != tup_vct.end()) - { - acc_prob[it_comb - tup_vct.begin()] += it_out->second * occ_prop; - } - else - { - tup_vct.push_back(std::make_tuple(fr_idx, oc_idx + 1, un_idx)); - acc_prob.push_back(it_out->second * occ_prop); - } - - it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(fr_idx, oc_idx, un_idx + 1)); - - // Unobserved - if (it_comb != tup_vct.end()) - { - acc_prob[it_comb - tup_vct.begin()] += it_out->second * un_prop; - } - else - { - tup_vct.push_back(std::make_tuple(fr_idx, oc_idx, un_idx + 1)); - acc_prob.push_back(it_out->second * un_prop); - } - } - } - - // Inserting the elements into the map - for (int k = 0; k < tup_vct.size(); ++k) - { - m_map_out.insert(map_pair(tup_vct[k], acc_prob[k])); - } - } -} - -float LifelongSlamToolbox::probabilityFromLogs(float log) -{ - /* - To transform the Log-odds into probability - */ - return (exp(log) / (1 + exp(log))); -} - -float LifelongSlamToolbox::calculateLogs(float probability) -{ - /* - To calculate the log-odds - */ - return log(probability / (1 - probability)); -} - -std::vector LifelongSlamToolbox::calculateIntersection( - std::vector laser_start, std::vector laser_end, - std::vector cell_start, std::vector cell_end) -{ - /* - Initial point laser beam: laser_start - Final point laser beam: laser_end - Initial point cell: cell_start - Final point cell: cell_end - */ - float x1 = laser_start[0]; - float x2 = laser_end[0]; - float x3 = cell_start[0]; - float x4 = cell_end[0]; - - float y1 = laser_start[1]; - float y2 = laser_end[1]; - float y3 = cell_start[1]; - float y4 = cell_end[1]; - - float den = ((x2-x1)*(y4-y3) - (x4-x3)*(y2-y1)); - if (den == 0.0f) - { - // Parallel lines or not intersection at all - // std::cout << " -Parallels- " << std::endl; - return {}; - } - else - { - float x = ((x2*y1 - x1*y2)*(x4 - x3) - (x4*y3 - x3*y4)*(x2-x1)) / den; - float y = ((x2*y1 - x1*y2)*(y4 - y3) - (x4*y3 - x3*y4)*(y2-y1)) / den; - // std::cout << "Intersection point: "<< x << ", " << y << std::endl; - return {x, y}; - } -} - -float LifelongSlamToolbox::euclideanDistance(float x_1, float y_1, float x_2, float y_2) -{ - /* - To calculate the euclidean distance between two points - */ - float diff_x = x_2 - x_1; - float diff_y = y_2 - y_1; - - return sqrt(diff_x*diff_x + diff_y*diff_y); -} - -float LifelongSlamToolbox::probabilityFromObservation(float range_1, float range_2) -{ - /* - Calculates the probability of a cell being observed by a given measurement - range_1: lower limit, range_2: upper limit - https://www.probabilitycourse.com/chapter4/4_2_4_Gamma_distribution.php - https://homepage.divms.uiowa.edu/~mbognar/applets/gamma.html - https://www.youtube.com/watch?v=jWh4HY_Geaw - - This distribution needs to be changed - From my perspective we should use a Gamma distribution instead of an exponential - That would change the integral as well. It might be more complex with Gamma - - - float lambda = 0.285f; - - float nu = 1.0f / lambda; - - https://www.wolframalpha.com/input/?i=plot+0.28*0.35*e%5E%28-0.35*x%29+from+x%3D0+to+15 - */ - float max_range = 5.0f; - - float lambda = 0.35f; - float nu = 0.28f; - - range_1 = (range_1 > max_range) ? max_range : range_1; - range_2 = (range_2 > max_range) ? max_range : range_2; - - // https://www.wolframalpha.com/input/?i2d=true&i=Integrate%5Bn*c*Power%5Be%2C-c*x%5D%2C%7Bx%2Ca%2Cb%7D%5D - return nu * (exp(-lambda*range_1) - exp(-lambda*range_2)); -} - -std::vector LifelongSlamToolbox::getGridPosition(float x, float y) -{ - /* - To maps the current position into grid coordinates - */ - int x_cell = floor((1 / m_resolution) * x); - int y_cell = floor((1 / m_resolution) * y); - - return {x_cell, y_cell}; -} - -std::vector LifelongSlamToolbox::laserHitDistance(std::vector const& robot_pose, float range, float angle) -{ - /* - To get the distance where the laser beam hits something - Rigid Body Trasnformation from the global to the sensor frame - */ - float x_tf = (range * cos(robot_pose[2] + angle)) + robot_pose[0]; - float y_tf = (range * sin(robot_pose[2] + angle)) + robot_pose[1]; - - return {x_tf, y_tf}; -} - -std::pair, std::vector> LifelongSlamToolbox::Bresenham(int x_1, int y_1, int x_2, int y_2) -{ - /* - To find the set of cells hit by a laser beam - */ - std::vector x_bres; - std::vector y_bres; - - int x = x_1; - int y = y_1; - - int delta_x = abs(x_2 - x_1); - int delta_y = abs(y_2 - y_1); - - int s_x = getSign(x_1, x_2); - int s_y = getSign(y_1, y_2); - bool interchange = false; - - if (delta_y > delta_x) - { - int temp = delta_x; - delta_x = delta_y; - delta_y = temp; - interchange = true; - } - else - { - interchange = false; - } - - int a_res = 2 * delta_y; - int b_res = 2 * (delta_y - delta_x); - int e_res = (2 * delta_y) - delta_x; - - x_bres.push_back(x); - y_bres.push_back(y); - - for (int i = 1; i < delta_x; ++i) - { - if (e_res < 0) - { - if (interchange) - { - y += s_y; - } - else - { - x += s_x; - } - e_res += a_res; - } - else - { - y += s_y; - x += s_x; - e_res += b_res; - } - x_bres.push_back(x); - y_bres.push_back(y); - } - return std::pair, std::vector>{x_bres, y_bres}; -} - -int LifelongSlamToolbox::getSign(int n_1, int n_2) -{ - /* - To get the sign of an operation, used for Bresenham algorithm - */ - int difference = n_2 - n_1; - - if (difference == 0) { return 0; } - else if (difference < 0) { return -1; } - else { return 1; } -} -/*****************************************************************************/ - - /*****************************************************************************/ void LifelongSlamToolbox::laserCallback( sensor_msgs::msg::LaserScan::ConstSharedPtr scan) diff --git a/src/experimental/theoretic_information.cpp b/src/experimental/theoretic_information.cpp index 8ed3a1a6b..63693bc22 100644 --- a/src/experimental/theoretic_information.cpp +++ b/src/experimental/theoretic_information.cpp @@ -11,6 +11,7 @@ TeorethicInformation::TeorethicInformation() // Grids initialization (Occupancy and Mutual information) m_grid.resize(m_num_cells); m_mutual_grid.resize(m_num_cells); + visited_cells.resize(m_num_cells); initializeGrids(); scannerTest(); @@ -30,7 +31,9 @@ void TeorethicInformation::scannerTest() std::vector robot_grid_pos = getGridPosition(robot_poses[r][0], robot_poses[r][1]); std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; - // Current yaw + beam angle: -PI/2 (-1.570795) -0.87266 = 2.44345 (-55 degrees) + // Set as false the current boolean map + clearVisitedCells(); + for (int i = 0; i < laser_ranges[r].size(); ++i) { std::cout << "........ New laser ........" << std::endl; @@ -42,9 +45,6 @@ void TeorethicInformation::scannerTest() // Laser final cell std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1]); - // robot_grid_pos[0] // X1 - robot_grid_pos[1] // Y1 - // final_grid_pos[0] // X2 - final_grid_pos[1] // Y2 - // Ray tracing for getting the visited cells std::vector cells_x, cells_y; std::pair, std::vector> res_pair = Bresenham(robot_grid_pos[0], robot_grid_pos[1], final_grid_pos[0], final_grid_pos[1]); @@ -58,7 +58,13 @@ void TeorethicInformation::scannerTest() // Adding last hit cell to the set cells_x.push_back(final_grid_pos[0]); cells_y.push_back(final_grid_pos[1]); - + + /* + We have an issue here + a measurement is watching two times the same cell + I need to find a way for avoiding that scenario + We can compute the average of all measurements + */ // Visiting the cells for (int j = 0; j < cells_x.size(); ++j) @@ -132,10 +138,6 @@ void TeorethicInformation::scannerTest() probabilityFromObservation(0.0f, distances[0]) }; - // Assigning the cells - m_cell_x = cells_x[j]; - m_cell_y = cells_y[j]; - // Appending new measurement outcomes for the current cell appendCellProbabilities(probabilities, {cells_x[j], cells_y[j]}); @@ -154,19 +156,11 @@ void TeorethicInformation::scannerTest() // Interested in the final measurement outcomes if (std::get<0>(it_mutual->first) + std::get<1>(it_mutual->first) + std::get<2>(it_mutual->first) == meas_outcomes.size()) { - // measurementOutcomeEntropy is negative cell_mutual_inf += it_mutual->second * measurementOutcomeEntropy(it_mutual->first); - // std::cout << "----------Entropy---------- " << measurementOutcomeEntropy(it_mutual->first) << std::endl; - // std::cout << "----------Entropy---------- " << 0.3*log2(0.3) << std::endl; - // std::cout << "----------Entropy---------- " << calculateEntropy(0.7) << std::endl; - // std::cout << "----------Entropy---------- " << calculateEntropy(probabilityFromLogs(calculateLogs(0.7f))) << std::endl; } - } - - // Here should be the H(C) = 0.5 : 0.5 - SUM(P*H) - std::cout << "Cell mutual information: " << 0.5 - cell_mutual_inf << std::endl; // Mutual information of cell x, y given a set of measurements + updateCellMutualInformation(0.5 - cell_mutual_inf, {cells_x[j], cells_y[j]}); std::cout << "++++++++++++++++++++++++" << std::endl; } @@ -176,16 +170,31 @@ void TeorethicInformation::scannerTest() } } +void TeorethicInformation::clearVisitedCells() +{ + /* + To clear the visited cell + */ + + std::cout << "Clearing cells " << std::endl; + for (int i = 0; i < visited_cells.size(); ++i) + { + for (int j = 0; j < visited_cells[0].size(); ++j) + { + visited_cells[i][j] = false; + } + } +} + void TeorethicInformation::appendCellProbabilities(std::vector& measurements, std::vector cell) { /* To append a new measurement for a specific cell */ - - // Iterator for getting the cell std::map, std::vector>>::iterator it_cell; it_cell = m_cell_probabilities.find({cell[0], cell[1]}); + if (it_cell == m_cell_probabilities.end()) { // Cell is not present in the map, so append it @@ -193,15 +202,37 @@ void TeorethicInformation::appendCellProbabilities(std::vector& measureme {cell[0], cell[1]}, {{measurements[0], measurements[1], measurements[2]}} )); + visited_cells[cell[0]][cell[1]] = true; } else { - // Cell is already in the map, only add the next measurement outcome - it_cell->second.push_back({measurements[0], measurements[1], measurements[2]}); + if(visited_cells[cell[0]][cell[1]] == true) + { + /* + Compare the unknown probability, the smallest it is the most information we will have + from the occupied or free state + */ + int idx = it_cell->second.size() - 1; + if(measurements[2] < it_cell->second[idx][2]) + { + // std::cout << "Replacing:" << it_cell->second[idx][0] << ", " << it_cell->second[idx][1] << ", " << it_cell->second[idx][2] << std::endl; + // std::cout << "With:" << measurements[0] << ", " << measurements[1] << ", " << measurements[2] << std::endl; + + // Replacing + it_cell->second[idx][0] = measurements[0]; + it_cell->second[idx][1] = measurements[1]; + it_cell->second[idx][2] = measurements[2]; + } + } + else + { + // Cell is already in the map, only add the next measurement outcome + it_cell->second.push_back({measurements[0], measurements[1], measurements[2]}); + visited_cells[cell[0]][cell[1]] = true; + } } } - std::pair, std::vector> TeorethicInformation::Bresenham(int x_1, int y_1, int x_2, int y_2) { /* @@ -534,10 +565,12 @@ void TeorethicInformation::initializeGrids() // Adding columns m_grid[i].resize(m_num_cells); m_mutual_grid[i].resize(m_num_cells); + visited_cells[i].resize(m_num_cells); for (int j = 0; j < m_num_cells; ++j) { m_grid[i][j] = 0; m_mutual_grid[i][j] = 0.0f; + visited_cells[i][j] = false; } } } @@ -569,7 +602,6 @@ float TeorethicInformation::probabilityFromObservation(float range_1, float rang /* To calculate the probability of a cell being observed by a given measurement */ - float max_range = 5.0f; float lambda = 0.35f; float nu = 0.28f; @@ -585,7 +617,6 @@ std::vector> TeorethicInformation::retreiveMeasurementOutcome /* To get all the measurement outcomes for the current cell */ - std::vector> meas_outcomes; std::map, std::vector>>::iterator it_cells; it_cells = m_cell_probabilities.find({cell[0], cell[1]}); diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 35056721e..2263e99ec 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -340,11 +340,9 @@ LaserRangeFinder * SlamToolbox::getLaser( sensor_msgs::msg::LaserScan::ConstSharedPtr & scan) /*****************************************************************************/ { - // RCLCPP_ERROR(get_logger(), "Here!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); const std::string & frame = scan->header.frame_id; if (lasers_.find(frame) == lasers_.end()) { try { - // RCLCPP_WARN(get_logger(), "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! Adding !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); lasers_[frame] = laser_assistant_->toLaserMetadata(*scan); dataset_->Add(lasers_[frame].getLaser(), true); } catch (tf2::TransformException & e) { From 86910b8f4a1a2def665246cfc0bc935ae75817de Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Wed, 15 Dec 2021 10:27:20 -0500 Subject: [PATCH 31/83] [RMV]: Removing unused dependencies --- src/experimental/slam_toolbox_lifelong.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index a973073f9..45b47a539 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -18,12 +18,8 @@ #include #include -#include -#include -#include #include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp" - namespace slam_toolbox { From 36f2fa9688b83f2deea6d0ad2fa21756df961f67 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Thu, 16 Dec 2021 12:02:59 -0500 Subject: [PATCH 32/83] [FIX]: Floating point absolute value changed in cell limits --- .../experimental/theoretic_information.hpp | 9 +- src/experimental/theoretic_information.cpp | 85 ++++++++++--------- 2 files changed, 52 insertions(+), 42 deletions(-) diff --git a/include/slam_toolbox/experimental/theoretic_information.hpp b/include/slam_toolbox/experimental/theoretic_information.hpp index 189a6b826..39a649815 100644 --- a/include/slam_toolbox/experimental/theoretic_information.hpp +++ b/include/slam_toolbox/experimental/theoretic_information.hpp @@ -38,14 +38,14 @@ class TeorethicInformation // Grid operations void initializeGrids(); void updateCellLimits( - std::vector & initial_x, std::vector & initial_y, std::vector & final_x, std::vector & final_y, - float & limit_x, float & limit_y, std::vector & cell_limits, std::vector & robot_grid_pos, std::vector & final_grid_pos); + std::vector & initial_x, std::vector & initial_y, std::vector & final_x, std::vector & final_y, + double & limit_x, double & limit_y, std::vector & cell_limits, std::vector & robot_grid_pos, std::vector & final_grid_pos); // Grid and position information std::pair, std::vector> Bresenham(int x_1, int y_1, int x_2, int y_2); std::vector getGridPosition(float x, float y); std::vector laserHitDistance(std::vector const& robot_pose, float range, float angle); - std::vector calculateCellIntersectionPoints(std::vector & laser_start, std::vector & laser_end, std::vector cell_start, std::vector cell_end); + std::vector calculateCellIntersectionPoints(std::vector & laser_start, std::vector & laser_end, std::vector cell_start, std::vector cell_end); int getSign(int n_1, int n_2); // Measurements calculations @@ -80,8 +80,11 @@ class TeorethicInformation int m_num_cells; // Robot information - Defining values just for testing + // std::vector> robot_poses {{5.6f, 6.0f, M_PI/2}, {7.75f, 9.22f, -M_PI/2}}; + // std::vector> robot_poses {{7.75f, 9.22f, -M_PI/2}, {3.5f, 9.0f, 0.0f}}; std::vector> robot_poses {{5.6f, 6.0f, M_PI/2}, {3.5f, 9.0f, 0.0f}}; std::vector> laser_ranges {{1.65f, 5.0f, 5.0f, 5.0f, 5.0f}, {5.0f, 5.0f, 4.0f, 5.0f, 5.0f}}; + // std::vector> laser_ranges {{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}, {5.0f, 5.0f, 4.0f, 5.0f, 5.0f}}; std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; std::vector> visited_cells; diff --git a/src/experimental/theoretic_information.cpp b/src/experimental/theoretic_information.cpp index 63693bc22..cd11e44b3 100644 --- a/src/experimental/theoretic_information.cpp +++ b/src/experimental/theoretic_information.cpp @@ -19,10 +19,23 @@ TeorethicInformation::TeorethicInformation() void TeorethicInformation::scannerTest() { + /* + La lectura de menor ganancia + + Dado uno computamos el score - Esta informacion esta en el toolbox + Buscar scans en el mismo rango de vision + + // Set -> information + // Nodo -> Que tanta ganancia aporta (Ganancia de informacion) + Batch elimination (Hiteresis) - Groups + + Lista de adyacencia + */ + // Loop through the different robot poses for (int r = 0; r < robot_poses.size(); ++r) { - std::cout << "---------- Robot pose ----------: " << r << std::endl; + std::cout << "-------------------- Robot pose --------------------: " << r << std::endl; // Angles {-50, -25, 0, 25, 50} in degrees std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; @@ -36,7 +49,7 @@ void TeorethicInformation::scannerTest() for (int i = 0; i < laser_ranges[r].size(); ++i) { - std::cout << "........ New laser ........" << std::endl; + std::cout << "................ New laser ................" << std::endl; std::cout << "Distance: " << laser_ranges[r][i] << ", Angle: " << angles[i] << std::endl; // Laser continuous distance @@ -59,13 +72,6 @@ void TeorethicInformation::scannerTest() cells_x.push_back(final_grid_pos[0]); cells_y.push_back(final_grid_pos[1]); - /* - We have an issue here - a measurement is watching two times the same cell - I need to find a way for avoiding that scenario - We can compute the average of all measurements - */ - // Visiting the cells for (int j = 0; j < cells_x.size(); ++j) { @@ -73,34 +79,36 @@ void TeorethicInformation::scannerTest() std::cout << "Current cell: " << cells_x[j] << ", " << cells_y[j] << std::endl; // Inidividual cell limits - float limit_x = cells_x[j] * m_resolution; - float limit_y = cells_y[j] * m_resolution; + double limit_x = cells_x[j] * m_resolution; + double limit_y = cells_y[j] * m_resolution; // Cell limits: min_x, max_x, min_y, max_y - std::vector cell_limits {limit_x, limit_x + m_resolution, limit_y, limit_y + m_resolution}; + std::vector cell_limits {limit_x, limit_x + m_resolution, limit_y, limit_y + m_resolution}; // Initial points for each of the 4 corners - std::vector initial_x {limit_x, limit_x, limit_x + m_resolution, limit_x + m_resolution}; - std::vector initial_y {limit_y, limit_y, limit_y + m_resolution, limit_y + m_resolution}; + std::vector initial_x {limit_x, limit_x, limit_x + m_resolution, limit_x + m_resolution}; + std::vector initial_y {limit_y, limit_y, limit_y + m_resolution, limit_y + m_resolution}; // Final points for each of the 4 corners - std::vector final_x {limit_x + m_resolution, limit_x, limit_x + m_resolution, limit_x}; - std::vector final_y {limit_y, limit_y + m_resolution, limit_y, limit_y + m_resolution}; + std::vector final_x {limit_x + m_resolution, limit_x, limit_x + m_resolution, limit_x}; + std::vector final_y {limit_y, limit_y + m_resolution, limit_y, limit_y + m_resolution}; // Set the new cell limits + + // Need to take care of this function updateCellLimits(initial_x, initial_y, final_x, final_y, limit_x, limit_y, cell_limits, robot_grid_pos, final_grid_pos); - std::vector inter_x, inter_y; + std::vector inter_x, inter_y; + for (int k = 0; k < 4; ++k) { - std::vector intersection = calculateCellIntersectionPoints(robot_poses[r], laser_grid, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); + std::vector intersection = calculateCellIntersectionPoints(robot_poses[r], laser_grid, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); if(intersection.size() != 0) { - // If the laser and a cell intersects, we need to make sure it happens in the right bounds - if ((abs(intersection[0]) >= abs(cell_limits[0] - 0.01f)) && - (abs(intersection[0]) <= abs(cell_limits[1] + 0.01f)) && - (abs(intersection[1]) >= abs(cell_limits[2] - 0.01f)) && - (abs(intersection[1]) <= abs(cell_limits[3] + 0.01f))) + if ((fabs(intersection[0]) >= (fabs(cell_limits[0]) - 0.001)) && + (fabs(intersection[0]) <= (fabs(cell_limits[1]) + 0.001)) && + (fabs(intersection[1]) >= (fabs(cell_limits[2]) - 0.001)) && + (fabs(intersection[1]) <= (fabs(cell_limits[3]) + 0.001))) { /* Two points where the beam cuts the cell @@ -112,7 +120,6 @@ void TeorethicInformation::scannerTest() } } } - // When a cell is marked by Bresenham but there is not intersection points if (inter_x.size() == 0) continue; @@ -305,9 +312,9 @@ float TeorethicInformation::calculateEntropy(float probability) } -std::vector TeorethicInformation::calculateCellIntersectionPoints( +std::vector TeorethicInformation::calculateCellIntersectionPoints( std::vector & laser_start, std::vector & laser_end, - std::vector cell_start, std::vector cell_end) + std::vector cell_start, std::vector cell_end) { /* Initial point laser beam: laser_start @@ -315,17 +322,17 @@ std::vector TeorethicInformation::calculateCellIntersectionPoints( Initial point cell: cell_start Final point cell: cell_end */ - float x1 = laser_start[0]; - float x2 = laser_end[0]; - float x3 = cell_start[0]; - float x4 = cell_end[0]; + double x1 = laser_start[0]; + double x2 = laser_end[0]; + double x3 = cell_start[0]; + double x4 = cell_end[0]; - float y1 = laser_start[1]; - float y2 = laser_end[1]; - float y3 = cell_start[1]; - float y4 = cell_end[1]; + double y1 = laser_start[1]; + double y2 = laser_end[1]; + double y3 = cell_start[1]; + double y4 = cell_end[1]; - float den = ((x2-x1)*(y4-y3) - (x4-x3)*(y2-y1)); + double den = ((x2-x1)*(y4-y3) - (x4-x3)*(y2-y1)); if (den == 0.0f) { // Parallel lines or not intersection at all @@ -333,15 +340,15 @@ std::vector TeorethicInformation::calculateCellIntersectionPoints( } else { - float x = ((x2*y1 - x1*y2)*(x4 - x3) - (x4*y3 - x3*y4)*(x2-x1)) / den; - float y = ((x2*y1 - x1*y2)*(y4 - y3) - (x4*y3 - x3*y4)*(y2-y1)) / den; + double x = ((x2*y1 - x1*y2)*(x4 - x3) - (x4*y3 - x3*y4)*(x2-x1)) / den; + double y = ((x2*y1 - x1*y2)*(y4 - y3) - (x4*y3 - x3*y4)*(y2-y1)) / den; return {x, y}; } } void TeorethicInformation::updateCellLimits( - std::vector & initial_x, std::vector & initial_y, std::vector & final_x, std::vector & final_y, - float & limit_x, float & limit_y, std::vector & cell_limits, std::vector & robot_grid_pos, std::vector & final_grid_pos) + std::vector & initial_x, std::vector & initial_y, std::vector & final_x, std::vector & final_y, + double & limit_x, double & limit_y, std::vector & cell_limits, std::vector & robot_grid_pos, std::vector & final_grid_pos) { /* To calculate grid grid limits for intersection From 79805799cf4416ba432e3aa23b2dc3bd8148de42 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Fri, 17 Dec 2021 11:47:16 -0500 Subject: [PATCH 33/83] [WIP]: Changes in measurement outcomes computations and code structure --- .../experimental/slam_toolbox_lifelong.hpp | 2 - .../experimental/theoretic_information.hpp | 53 +++--- src/experimental/theoretic_information.cpp | 155 ++++++++++-------- 3 files changed, 109 insertions(+), 101 deletions(-) diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index e91bfb3a2..c8f2ea103 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -31,8 +31,6 @@ namespace slam_toolbox class LifelongSlamToolbox : public SlamToolbox { - typedef std::tuple map_tuple; - typedef std::pair map_pair; public: explicit LifelongSlamToolbox(rclcpp::NodeOptions options); diff --git a/include/slam_toolbox/experimental/theoretic_information.hpp b/include/slam_toolbox/experimental/theoretic_information.hpp index 39a649815..37274e239 100644 --- a/include/slam_toolbox/experimental/theoretic_information.hpp +++ b/include/slam_toolbox/experimental/theoretic_information.hpp @@ -12,7 +12,7 @@ class TeorethicInformation { typedef std::tuple map_tuple; - typedef std::pair map_pair; + typedef std::pair map_pair; public: TeorethicInformation(); // Empty contructor for now @@ -43,49 +43,48 @@ class TeorethicInformation // Grid and position information std::pair, std::vector> Bresenham(int x_1, int y_1, int x_2, int y_2); - std::vector getGridPosition(float x, float y); - std::vector laserHitDistance(std::vector const& robot_pose, float range, float angle); - std::vector calculateCellIntersectionPoints(std::vector & laser_start, std::vector & laser_end, std::vector cell_start, std::vector cell_end); + std::vector getGridPosition(double x, double y); + std::vector laserHitDistance(std::vector const& robot_pose, double range, double angle); + std::vector calculateCellIntersectionPoints(std::vector & laser_start, std::vector & laser_end, std::vector cell_start, std::vector cell_end); int getSign(int n_1, int n_2); // Measurements calculations - float probabilityFromObservation(float range_1, float range_2); - float euclideanDistance(float x_1, float y_1, float x_2, float y_2); + double probabilityFromObservation(double range_1, double range_2); + double euclideanDistance(double x_1, double y_1, double x_2, double y_2); // Mutual information - float calculateEntropy(float probability); - float calculateLogs(float probability); - float calculateMapMutualInformation(); - float measurementOutcomeEntropy(map_tuple const& meas_outcome); - float probabilityFromLogs(float log); + double calculateEntropy(double probability); + double calculateLogs(double probability); + double calculateMapMutualInformation(); + double measurementOutcomeEntropy(map_tuple const& meas_outcome); + double probabilityFromLogs(double log); void recoverProbability(); - void updateCellMutualInformation(float mut_inf, std::vector cell); + void updateCellMutualInformation(double mut_inf, std::vector cell); // Measurement outcomes probabilities - void appendCellProbabilities(std::vector& measurements, std::vector cell); - std::unordered_map computeProbabilities(std::vector>& meas_outcm); - std::vector> retreiveMeasurementOutcomes(std::vector cell); - std::vector unhashIndex(int hash); + void appendCellProbabilities(std::vector& measurements, std::vector cell); + std::unordered_map computeMeasurementOutcomesHistogram(std::vector>& meas_outcm); + std::vector> retreiveMeasurementOutcomes(std::vector cell); void clearVisitedCells(); private: // Data structures - std::unordered_map m_map_out; - std::map, std::vector>> m_cell_probabilities; - std::vector> m_mutual_grid; + std::unordered_map m_map_out; + std::map, std::vector>> m_cell_probabilities; + std::vector> m_mutual_grid; std::vector> m_grid; - float m_map_dist; - float m_resolution; + double m_map_dist; + double m_resolution; int m_num_cells; // Robot information - Defining values just for testing - // std::vector> robot_poses {{5.6f, 6.0f, M_PI/2}, {7.75f, 9.22f, -M_PI/2}}; - // std::vector> robot_poses {{7.75f, 9.22f, -M_PI/2}, {3.5f, 9.0f, 0.0f}}; - std::vector> robot_poses {{5.6f, 6.0f, M_PI/2}, {3.5f, 9.0f, 0.0f}}; - std::vector> laser_ranges {{1.65f, 5.0f, 5.0f, 5.0f, 5.0f}, {5.0f, 5.0f, 4.0f, 5.0f, 5.0f}}; - // std::vector> laser_ranges {{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}, {5.0f, 5.0f, 4.0f, 5.0f, 5.0f}}; - std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + // std::vector> robot_poses {{5.6f, 6.0f, M_PI/2}, {7.75f, 9.22f, -M_PI/2}}; + // std::vector> robot_poses {{7.75f, 9.22f, -M_PI/2}, {3.5f, 9.0f, 0.0f}}; + std::vector> robot_poses {{5.6f, 6.0f, M_PI/2}, {3.5f, 9.0f, 0.0f}}; + std::vector> laser_ranges {{1.65f, 5.0f, 5.0f, 5.0f, 5.0f}, {5.0f, 5.0f, 4.0f, 5.0f, 5.0f}}; + // std::vector> laser_ranges {{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}, {5.0f, 5.0f, 4.0f, 5.0f, 5.0f}}; + std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; std::vector> visited_cells; }; diff --git a/src/experimental/theoretic_information.cpp b/src/experimental/theoretic_information.cpp index cd11e44b3..c7aa0bced 100644 --- a/src/experimental/theoretic_information.cpp +++ b/src/experimental/theoretic_information.cpp @@ -38,7 +38,7 @@ void TeorethicInformation::scannerTest() std::cout << "-------------------- Robot pose --------------------: " << r << std::endl; // Angles {-50, -25, 0, 25, 50} in degrees - std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; // Initial point std::vector robot_grid_pos = getGridPosition(robot_poses[r][0], robot_poses[r][1]); @@ -53,7 +53,7 @@ void TeorethicInformation::scannerTest() std::cout << "Distance: " << laser_ranges[r][i] << ", Angle: " << angles[i] << std::endl; // Laser continuous distance - std::vector laser_grid = laserHitDistance(robot_poses[r], laser_ranges[r][i], angles[i]); + std::vector laser_grid = laserHitDistance(robot_poses[r], laser_ranges[r][i], angles[i]); // Laser final cell std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1]); @@ -125,11 +125,11 @@ void TeorethicInformation::scannerTest() continue; // Enter (d1) and Exit (d2) distances - std::vector distances; + std::vector distances; for (int k = 0; k < inter_x.size(); ++k) { // From robot position to intersection points - float dist_point = euclideanDistance(robot_poses[r][0], robot_poses[r][1], inter_x[k], inter_y[k]); + double dist_point = euclideanDistance(robot_poses[r][0], robot_poses[r][1], inter_x[k], inter_y[k]); distances.push_back(dist_point); } @@ -139,7 +139,7 @@ void TeorethicInformation::scannerTest() // Integral 3: d2 which is distance from robot pose to second point where the cell is cut to z_max // Measurement outcomes vector {Pfree, Pocc, Pun} - std::vector probabilities { + std::vector probabilities { probabilityFromObservation(distances[1], 5.0f), probabilityFromObservation(distances[0], distances[1]), probabilityFromObservation(0.0f, distances[0]) @@ -149,30 +149,28 @@ void TeorethicInformation::scannerTest() appendCellProbabilities(probabilities, {cells_x[j], cells_y[j]}); // Get all the measurement outcomes for the current cell - std::vector> meas_outcomes = retreiveMeasurementOutcomes({cells_x[j], cells_y[j]}); + std::vector> meas_outcomes = retreiveMeasurementOutcomes({cells_x[j], cells_y[j]}); // Compute all the possible combinations for the current cell - algorithm 1 - std::unordered_map meas_out_prob = computeProbabilities(meas_outcomes); - + std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(meas_outcomes); + // Calculate 3.12 - std::unordered_map::iterator it_mutual; - std::cout << "Number of measurements: " << meas_outcomes.size() << std::endl; - float cell_mutual_inf = 0.0f; + std::unordered_map::iterator it_mutual; + + // std::cout << "Number of measurements: " << meas_outcomes.size() << std::endl; + + double cell_mutual_inf = 0.0f; for (it_mutual = meas_out_prob.begin(); it_mutual != meas_out_prob.end(); ++it_mutual) { - // Interested in the final measurement outcomes - if (std::get<0>(it_mutual->first) + std::get<1>(it_mutual->first) + std::get<2>(it_mutual->first) == meas_outcomes.size()) - { - cell_mutual_inf += it_mutual->second * measurementOutcomeEntropy(it_mutual->first); - } + cell_mutual_inf += it_mutual->second * measurementOutcomeEntropy(it_mutual->first); } - // Mutual information of cell x, y given a set of measurements - + + // Mutual information of cell x, y given a set of measurements updateCellMutualInformation(0.5 - cell_mutual_inf, {cells_x[j], cells_y[j]}); std::cout << "++++++++++++++++++++++++" << std::endl; } } - float mutual = calculateMapMutualInformation(); + double mutual = calculateMapMutualInformation(); std::cout << "Mutual information: " << mutual << std::endl; } } @@ -193,19 +191,19 @@ void TeorethicInformation::clearVisitedCells() } } -void TeorethicInformation::appendCellProbabilities(std::vector& measurements, std::vector cell) +void TeorethicInformation::appendCellProbabilities(std::vector& measurements, std::vector cell) { /* To append a new measurement for a specific cell */ - std::map, std::vector>>::iterator it_cell; + std::map, std::vector>>::iterator it_cell; it_cell = m_cell_probabilities.find({cell[0], cell[1]}); 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>>( + m_cell_probabilities.insert(std::pair, std::vector>>( {cell[0], cell[1]}, {{measurements[0], measurements[1], measurements[2]}} )); @@ -303,7 +301,7 @@ std::pair, std::vector> TeorethicInformation::Bresenham(in return std::pair, std::vector>{x_bres, y_bres}; } -float TeorethicInformation::calculateEntropy(float probability) +double TeorethicInformation::calculateEntropy(double probability) { /* To calculate the entropy @@ -313,7 +311,7 @@ float TeorethicInformation::calculateEntropy(float probability) std::vector TeorethicInformation::calculateCellIntersectionPoints( - std::vector & laser_start, std::vector & laser_end, + std::vector & laser_start, std::vector & laser_end, std::vector cell_start, std::vector cell_end) { /* @@ -396,7 +394,7 @@ void TeorethicInformation::updateCellLimits( } } -float TeorethicInformation::calculateLogs(float probability) +double TeorethicInformation::calculateLogs(double probability) { /* To calculate the log-odds @@ -404,13 +402,13 @@ float TeorethicInformation::calculateLogs(float probability) return log(probability / (1 - probability)); } -float TeorethicInformation::calculateMapMutualInformation() +double TeorethicInformation::calculateMapMutualInformation() { /* To calculate map mutual information, this is the summation of all cells mutual information */ - float sum = 0.0f; + double sum = 0.0f; for (int i = 0; i < m_num_cells; ++i) { for (int j = 0; j < m_num_cells; ++j) @@ -421,47 +419,48 @@ float TeorethicInformation::calculateMapMutualInformation() return sum; } -std::unordered_map TeorethicInformation::computeProbabilities(std::vector>& meas_outcm) +std::unordered_map TeorethicInformation::computeMeasurementOutcomesHistogram(std::vector>& meas_outcm) { /* To compute all the possible combinations of a grid cell, given a set of measurement outcomes */ - // Cleaning measurement outcomes map - std::unordered_map map_out; - std::unordered_map::iterator it_out; + std::unordered_map temp_map; + std::unordered_map::iterator it_temp; // The number of measurements int k = meas_outcm.size(); int r = 1; - float p_free = meas_outcm[0][2]; - float p_occ = meas_outcm[0][1]; - float p_un = meas_outcm[0][0]; + double p_free = meas_outcm[0][2]; + double p_occ = meas_outcm[0][1]; + double p_un = meas_outcm[0][0]; + temp_map.clear(); + // Root - map_out.insert(map_pair(std::make_tuple(0, 0, 0), 1.0f)); + temp_map[std::make_tuple(0, 0, 0)] = 1.0f; // First measurement - map_out.insert(map_pair(std::make_tuple(1, 0, 0), p_free)); - map_out.insert(map_pair(std::make_tuple(0, 1, 0), p_occ)); - map_out.insert(map_pair(std::make_tuple(0, 0, 1), p_un)); + temp_map[std::make_tuple(1, 0, 0)] = p_free; + temp_map[std::make_tuple(0, 1, 0)] = p_occ; + temp_map[std::make_tuple(0, 0, 1)] = p_un; for (int i = r; r < k; ++r) { std::vector tup_vct; - std::vector acc_prob; + std::vector acc_prob; - for (it_out = map_out.begin(); it_out != map_out.end(); ++it_out) + for (it_temp = temp_map.begin(); it_temp != temp_map.end(); ++it_temp) { // Index - int fr_idx = std::get<0>(it_out->first); - int oc_idx = std::get<1>(it_out->first); - int un_idx = std::get<2>(it_out->first); + int fr_idx = std::get<0>(it_temp->first); + int oc_idx = std::get<1>(it_temp->first); + int un_idx = std::get<2>(it_temp->first); // Measurement outcome probability - float free_prop = meas_outcm[r][0]; - float occ_prop = meas_outcm[r][1]; - float un_prop = meas_outcm[r][2]; + double free_prop = meas_outcm[r][0]; + double occ_prop = meas_outcm[r][1]; + double un_prop = meas_outcm[r][2]; if (fr_idx + oc_idx + un_idx == r) { @@ -472,12 +471,12 @@ std::unordered_mapsecond * free_prop; + acc_prob[it_comb - tup_vct.begin()] += it_temp->second * free_prop; } else { tup_vct.push_back(std::make_tuple(fr_idx + 1, oc_idx, un_idx)); - acc_prob.push_back(it_out->second * free_prop); + acc_prob.push_back(it_temp->second * free_prop); } it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(fr_idx, oc_idx + 1, un_idx)); @@ -485,12 +484,12 @@ std::unordered_mapsecond * occ_prop; + acc_prob[it_comb - tup_vct.begin()] += it_temp->second * occ_prop; } else { tup_vct.push_back(std::make_tuple(fr_idx, oc_idx + 1, un_idx)); - acc_prob.push_back(it_out->second * occ_prop); + acc_prob.push_back(it_temp->second * occ_prop); } it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(fr_idx, oc_idx, un_idx + 1)); @@ -498,31 +497,43 @@ std::unordered_mapsecond * un_prop; + acc_prob[it_comb - tup_vct.begin()] += it_temp->second * un_prop; } else { tup_vct.push_back(std::make_tuple(fr_idx, oc_idx, un_idx + 1)); - acc_prob.push_back(it_out->second * un_prop); + acc_prob.push_back(it_temp->second * un_prop); } } } // Inserting the elements into the map for (int k = 0; k < tup_vct.size(); ++k) { - map_out.insert(map_pair(tup_vct[k], acc_prob[k])); + temp_map[tup_vct[k]] = acc_prob[k]; } } - return map_out; + + // Leaving in the map only the final outcomes + std::unordered_map out_map; + std::unordered_map::iterator it_out; + for (it_out = temp_map.begin(); it_out != temp_map.end(); ++it_out) + { + if (std::get<0>(it_out->first) + std::get<1>(it_out->first) + std::get<2>(it_out->first) == k) + { + out_map[it_out->first] = it_out->second; + } + } + + return out_map; } -float TeorethicInformation::euclideanDistance(float x_1, float y_1, float x_2, float y_2) +double TeorethicInformation::euclideanDistance(double x_1, double y_1, double x_2, double y_2) { /* To calculate the euclidean distance between two points */ - float diff_x = x_2 - x_1; - float diff_y = y_2 - y_1; + double diff_x = x_2 - x_1; + double diff_y = y_2 - y_1; return sqrt(diff_x*diff_x + diff_y*diff_y); } @@ -539,7 +550,7 @@ int TeorethicInformation::getSign(int n_1, int n_2) else { return 1; } } -std::vector TeorethicInformation::getGridPosition(float x, float y) +std::vector TeorethicInformation::getGridPosition(double x, double y) { /* To maps the current position into grid coordinates @@ -550,14 +561,14 @@ std::vector TeorethicInformation::getGridPosition(float x, float y) return {x_cell, y_cell}; } -std::vector TeorethicInformation::laserHitDistance(std::vector const& robot_pose, float range, float angle) +std::vector TeorethicInformation::laserHitDistance(std::vector const& robot_pose, double range, double angle) { /* To get the distance where the laser beam hits something - Applying RBT from the global to the sensor frame */ - float x_tf = (range * cos(robot_pose[2] + angle)) + robot_pose[0]; - float y_tf = (range * sin(robot_pose[2] + angle)) + robot_pose[1]; + double x_tf = (range * cos(robot_pose[2] + angle)) + robot_pose[0]; + double y_tf = (range * sin(robot_pose[2] + angle)) + robot_pose[1]; return {x_tf, y_tf}; } @@ -582,7 +593,7 @@ void TeorethicInformation::initializeGrids() } } -float TeorethicInformation::measurementOutcomeEntropy(map_tuple const& meas_outcome) +double TeorethicInformation::measurementOutcomeEntropy(map_tuple const& meas_outcome) { /* To calculate the measurement outcome entropy (Measurement outcome in the form ) @@ -590,13 +601,13 @@ float TeorethicInformation::measurementOutcomeEntropy(map_tuple const& meas_outc - Calculate the probability from those logs - Calculate the entropy with the retrieved probability */ - float entropy = std::get<0>(meas_outcome) * calculateEntropy(probabilityFromLogs(calculateLogs(0.3f))) + + double entropy = std::get<0>(meas_outcome) * calculateEntropy(probabilityFromLogs(calculateLogs(0.3f))) + std::get<1>(meas_outcome) * calculateEntropy(probabilityFromLogs(calculateLogs(0.7f))) + std::get<2>(meas_outcome) * calculateEntropy(probabilityFromLogs(calculateLogs(0.5f))); return entropy; } -float TeorethicInformation::probabilityFromLogs(float log) +double TeorethicInformation::probabilityFromLogs(double log) { /* To transform the Log-odds into probability @@ -604,14 +615,14 @@ float TeorethicInformation::probabilityFromLogs(float log) return (exp(log) / (1 + exp(log))); } -float TeorethicInformation::probabilityFromObservation(float range_1, float range_2) +double TeorethicInformation::probabilityFromObservation(double range_1, double range_2) { /* To calculate the probability of a cell being observed by a given measurement */ - float max_range = 5.0f; - float lambda = 0.35f; - float nu = 0.28f; + double max_range = 5.0f; + double lambda = 0.35f; + double nu = 0.28f; range_1 = (range_1 > max_range) ? max_range : range_1; range_2 = (range_2 > max_range) ? max_range : range_2; @@ -619,13 +630,13 @@ float TeorethicInformation::probabilityFromObservation(float range_1, float rang return nu * (exp(-lambda*range_1) - exp(-lambda*range_2)); } -std::vector> TeorethicInformation::retreiveMeasurementOutcomes(std::vector cell) +std::vector> TeorethicInformation::retreiveMeasurementOutcomes(std::vector cell) { /* To get all the measurement outcomes for the current cell */ - std::vector> meas_outcomes; - std::map, std::vector>>::iterator it_cells; + std::vector> meas_outcomes; + std::map, std::vector>>::iterator it_cells; it_cells = m_cell_probabilities.find({cell[0], cell[1]}); if (it_cells != m_cell_probabilities.end()) @@ -641,7 +652,7 @@ std::vector> TeorethicInformation::retreiveMeasurementOutcome } -void TeorethicInformation::updateCellMutualInformation(float mut_inf, std::vector cell) +void TeorethicInformation::updateCellMutualInformation(double mut_inf, std::vector cell) { /* To update the mutual information for each individual cell From 89664a236532b4da5d232623a8b1e869dd747860 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Mon, 20 Dec 2021 12:10:54 -0500 Subject: [PATCH 34/83] [WIP]: Functions reordering to improve readability --- CMakeLists.txt | 2 +- include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp | 2 +- src/experimental/slam_toolbox_lifelong.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 45d78106a..0a194988a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -162,7 +162,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 src/experimental/theoretic_information.cpp) +add_executable(lifelong_slam_toolbox_node src/experimental/slam_toolbox_lifelong_node.cpp src/experimental/information_estimates.cpp) target_link_libraries(lifelong_slam_toolbox_node lifelong_slam_toolbox) #### Merging maps tool diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index c8f2ea103..a9ae1ce44 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -24,7 +24,7 @@ #include #include #include "slam_toolbox/slam_toolbox_common.hpp" -#include "slam_toolbox/experimental/theoretic_information.hpp" +#include "slam_toolbox/experimental/information_estimates.hpp" namespace slam_toolbox { diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 45b47a539..30413671c 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -76,7 +76,7 @@ LifelongSlamToolbox::LifelongSlamToolbox(rclcpp::NodeOptions options) // in lifelong mode, we cannot have interactive mode enabled enable_interactive_mode_ = false; - TeorethicInformation information; + InformationEstimates information; } /*****************************************************************************/ From dc970e0a435ef619f5b3cfbf8492d5010d30ef0c Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Tue, 21 Dec 2021 11:32:35 -0500 Subject: [PATCH 35/83] [WIP]: Adding new functions for readability --- ...ormation.hpp => information_estimates.hpp} | 50 ++- ...ormation.cpp => information_estimates.cpp} | 361 +++++++++--------- 2 files changed, 224 insertions(+), 187 deletions(-) rename include/slam_toolbox/experimental/{theoretic_information.hpp => information_estimates.hpp} (60%) rename src/experimental/{theoretic_information.cpp => information_estimates.cpp} (61%) diff --git a/include/slam_toolbox/experimental/theoretic_information.hpp b/include/slam_toolbox/experimental/information_estimates.hpp similarity index 60% rename from include/slam_toolbox/experimental/theoretic_information.hpp rename to include/slam_toolbox/experimental/information_estimates.hpp index 37274e239..19b5b9d2d 100644 --- a/include/slam_toolbox/experimental/theoretic_information.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -9,14 +9,14 @@ #include #include -class TeorethicInformation +class InformationEstimates { typedef std::tuple map_tuple; typedef std::pair map_pair; public: - TeorethicInformation(); // Empty contructor for now - ~TeorethicInformation() {} + InformationEstimates(); // Empty contructor for now + virtual ~InformationEstimates() {} public: struct HashTuple @@ -38,33 +38,38 @@ class TeorethicInformation // Grid operations void initializeGrids(); void updateCellLimits( - std::vector & initial_x, std::vector & initial_y, std::vector & final_x, std::vector & final_y, - double & limit_x, double & limit_y, std::vector & cell_limits, std::vector & robot_grid_pos, std::vector & final_grid_pos); + std::vector& initial_x, std::vector& initial_y, std::vector& final_x, std::vector& final_y, + double limit_x, double limit_y, std::vector& cell_limits, std::vector const& robot_grid_pos, std::vector const& final_grid_pos); // Grid and position information - std::pair, std::vector> Bresenham(int x_1, int y_1, int x_2, int y_2); + std::pair, std::vector> rayCasting(int x_1, int y_1, int x_2, int y_2); std::vector getGridPosition(double x, double y); std::vector laserHitDistance(std::vector const& robot_pose, double range, double angle); - std::vector calculateCellIntersectionPoints(std::vector & laser_start, std::vector & laser_end, std::vector cell_start, std::vector cell_end); - int getSign(int n_1, int n_2); + std::vector calculateCellIntersectionPoints(std::vector const& laser_start, std::vector const& laser_end, std::vector cell_start, std::vector cell_end); + int signum(int num); + + std::pair, std::vector> computeLineBoxIntersection( + std::vector const& laser_start, std::vector const& laser_end, + std::vector const& robot_grid_pos, std::vector const& final_grid_pos, + double limit_x, double limit_y); // Measurements calculations - double probabilityFromObservation(double range_1, double range_2); + double calculateScanMassProbabilityBetween(double range_1, double range_2); double euclideanDistance(double x_1, double y_1, double x_2, double y_2); // Mutual information - double calculateEntropy(double probability); - double calculateLogs(double probability); + double calculateInformationContent(double prob); + double calculateLogOddsFromProbability(double probability); double calculateMapMutualInformation(); double measurementOutcomeEntropy(map_tuple const& meas_outcome); - double probabilityFromLogs(double log); + double calculateProbabilityFromLogOdds(double log); void recoverProbability(); void updateCellMutualInformation(double mut_inf, std::vector cell); // Measurement outcomes probabilities void appendCellProbabilities(std::vector& measurements, std::vector cell); std::unordered_map computeMeasurementOutcomesHistogram(std::vector>& meas_outcm); - std::vector> retreiveMeasurementOutcomes(std::vector cell); + std::vector> retreiveCellProbabilities(std::vector cell); void clearVisitedCells(); @@ -74,10 +79,15 @@ class TeorethicInformation std::map, std::vector>> m_cell_probabilities; std::vector> m_mutual_grid; std::vector> m_grid; + double m_map_dist; - double m_resolution; + double m_cell_resol; int m_num_cells; + const double l_free = log(0.3 / (1.0 - 0.3)); + const double l_occ = log(0.7 / (1.0 - 0.7)); + const double l_o = log(0.5 / (1.0 - 0.5)); + // Robot information - Defining values just for testing // std::vector> robot_poses {{5.6f, 6.0f, M_PI/2}, {7.75f, 9.22f, -M_PI/2}}; // std::vector> robot_poses {{7.75f, 9.22f, -M_PI/2}, {3.5f, 9.0f, 0.0f}}; @@ -87,6 +97,16 @@ class TeorethicInformation std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; std::vector> visited_cells; + + double m_max_sensor_range = 5.0; + double m_obs_lambda = 0.35; + double m_obs_nu = 0.28; + + void setMaxSensorRange(double const sensor_range); + void setObservationLambda(double const lambda); + void setObservationNu(double const nu); + void setCellResolution(double const resolution); + void setMapDistance(double const distance); }; -#endif \ No newline at end of file +#endif diff --git a/src/experimental/theoretic_information.cpp b/src/experimental/information_estimates.cpp similarity index 61% rename from src/experimental/theoretic_information.cpp rename to src/experimental/information_estimates.cpp index c7aa0bced..99bbcf1df 100644 --- a/src/experimental/theoretic_information.cpp +++ b/src/experimental/information_estimates.cpp @@ -1,12 +1,14 @@ #include -#include "slam_toolbox/experimental/theoretic_information.hpp" +#include "slam_toolbox/experimental/information_estimates.hpp" -TeorethicInformation::TeorethicInformation() +InformationEstimates::InformationEstimates() { - std::cout << "Constructor of Theoretic information" << std::endl; - m_resolution = 0.5f; // Map resolution + /* + Need to add the new elements for the constructor + */ + m_cell_resol = 0.5f; // Map resolution m_map_dist = 20.0f; // Total map distance - m_num_cells = static_cast(m_map_dist / m_resolution); + m_num_cells = static_cast(m_map_dist / m_cell_resol); // Grids initialization (Occupancy and Mutual information) m_grid.resize(m_num_cells); @@ -17,7 +19,7 @@ TeorethicInformation::TeorethicInformation() scannerTest(); } -void TeorethicInformation::scannerTest() +void InformationEstimates::scannerTest() { /* La lectura de menor ganancia @@ -53,25 +55,17 @@ void TeorethicInformation::scannerTest() std::cout << "Distance: " << laser_ranges[r][i] << ", Angle: " << angles[i] << std::endl; // Laser continuous distance - std::vector laser_grid = laserHitDistance(robot_poses[r], laser_ranges[r][i], angles[i]); + std::vector laser_end = laserHitDistance(robot_poses[r], laser_ranges[r][i], angles[i]); // Laser final cell - std::vector final_grid_pos = getGridPosition(laser_grid[0], laser_grid[1]); + std::vector final_grid_pos = getGridPosition(laser_end[0], laser_end[1]); // Ray tracing for getting the visited cells std::vector cells_x, cells_y; - std::pair, std::vector> res_pair = Bresenham(robot_grid_pos[0], robot_grid_pos[1], final_grid_pos[0], final_grid_pos[1]); + std::pair, std::vector> res_pair = rayCasting(robot_grid_pos[0], robot_grid_pos[1], final_grid_pos[0], final_grid_pos[1]); cells_x = res_pair.first; cells_y = res_pair.second; - // Deleting the current robot cell - cells_x.erase(cells_x.begin()); - cells_y.erase(cells_y.begin()); - - // Adding last hit cell to the set - cells_x.push_back(final_grid_pos[0]); - cells_y.push_back(final_grid_pos[1]); - // Visiting the cells for (int j = 0; j < cells_x.size(); ++j) { @@ -79,57 +73,20 @@ void TeorethicInformation::scannerTest() std::cout << "Current cell: " << cells_x[j] << ", " << cells_y[j] << std::endl; // Inidividual cell limits - double limit_x = cells_x[j] * m_resolution; - double limit_y = cells_y[j] * m_resolution; - - // Cell limits: min_x, max_x, min_y, max_y - std::vector cell_limits {limit_x, limit_x + m_resolution, limit_y, limit_y + m_resolution}; - - // Initial points for each of the 4 corners - std::vector initial_x {limit_x, limit_x, limit_x + m_resolution, limit_x + m_resolution}; - std::vector initial_y {limit_y, limit_y, limit_y + m_resolution, limit_y + m_resolution}; - - // Final points for each of the 4 corners - std::vector final_x {limit_x + m_resolution, limit_x, limit_x + m_resolution, limit_x}; - std::vector final_y {limit_y, limit_y + m_resolution, limit_y, limit_y + m_resolution}; - - // Set the new cell limits - - // Need to take care of this function - updateCellLimits(initial_x, initial_y, final_x, final_y, limit_x, limit_y, cell_limits, robot_grid_pos, final_grid_pos); + double limit_x = cells_x[j] * m_cell_resol; + double limit_y = cells_y[j] * m_cell_resol; - std::vector inter_x, inter_y; + std::pair, std::vector> intersections = computeLineBoxIntersection(robot_poses[r], laser_end, robot_grid_pos, final_grid_pos, limit_x, limit_y); - for (int k = 0; k < 4; ++k) - { - std::vector intersection = calculateCellIntersectionPoints(robot_poses[r], laser_grid, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); - if(intersection.size() != 0) - { - if ((fabs(intersection[0]) >= (fabs(cell_limits[0]) - 0.001)) && - (fabs(intersection[0]) <= (fabs(cell_limits[1]) + 0.001)) && - (fabs(intersection[1]) >= (fabs(cell_limits[2]) - 0.001)) && - (fabs(intersection[1]) <= (fabs(cell_limits[3]) + 0.001))) - { - /* - 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[0]); - inter_y.push_back(intersection[1]); - } - } - } - // When a cell is marked by Bresenham but there is not intersection points - if (inter_x.size() == 0) + if (intersections.first.size() == 0) continue; // Enter (d1) and Exit (d2) distances std::vector distances; - for (int k = 0; k < inter_x.size(); ++k) + for (int k = 0; k < intersections.first.size(); ++k) { // From robot position to intersection points - double dist_point = euclideanDistance(robot_poses[r][0], robot_poses[r][1], inter_x[k], inter_y[k]); + double dist_point = euclideanDistance(robot_poses[r][0], robot_poses[r][1], intersections.first[k], intersections.second[k]); distances.push_back(dist_point); } @@ -139,27 +96,24 @@ void TeorethicInformation::scannerTest() // Integral 3: d2 which is distance from robot pose to second point where the cell is cut to z_max // Measurement outcomes vector {Pfree, Pocc, Pun} + // Map of tuples std::vector probabilities { - probabilityFromObservation(distances[1], 5.0f), - probabilityFromObservation(distances[0], distances[1]), - probabilityFromObservation(0.0f, distances[0]) + calculateScanMassProbabilityBetween(distances[1], 5.0f), + calculateScanMassProbabilityBetween(distances[0], distances[1]), + calculateScanMassProbabilityBetween(0.0f, distances[0]) }; // Appending new measurement outcomes for the current cell appendCellProbabilities(probabilities, {cells_x[j], cells_y[j]}); // Get all the measurement outcomes for the current cell - std::vector> meas_outcomes = retreiveMeasurementOutcomes({cells_x[j], cells_y[j]}); + std::vector> cell_prob = retreiveCellProbabilities({cells_x[j], cells_y[j]}); // Compute all the possible combinations for the current cell - algorithm 1 - std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(meas_outcomes); - - // Calculate 3.12 - std::unordered_map::iterator it_mutual; - - // std::cout << "Number of measurements: " << meas_outcomes.size() << std::endl; + std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(cell_prob); double cell_mutual_inf = 0.0f; + std::unordered_map::iterator it_mutual; for (it_mutual = meas_out_prob.begin(); it_mutual != meas_out_prob.end(); ++it_mutual) { cell_mutual_inf += it_mutual->second * measurementOutcomeEntropy(it_mutual->first); @@ -175,7 +129,7 @@ void TeorethicInformation::scannerTest() } } -void TeorethicInformation::clearVisitedCells() +void InformationEstimates::clearVisitedCells() { /* To clear the visited cell @@ -191,7 +145,7 @@ void TeorethicInformation::clearVisitedCells() } } -void TeorethicInformation::appendCellProbabilities(std::vector& measurements, std::vector cell) +void InformationEstimates::appendCellProbabilities(std::vector& measurements, std::vector cell) { /* To append a new measurement for a specific cell @@ -238,10 +192,11 @@ void TeorethicInformation::appendCellProbabilities(std::vector& measurem } } -std::pair, std::vector> TeorethicInformation::Bresenham(int x_1, int y_1, int x_2, int y_2) +std::pair, std::vector> InformationEstimates::rayCasting(int x_1, int y_1, int x_2, int y_2) { /* To find the set of cells hit by a laser beam + This is based on Bresenham algorithm */ std::vector x_bres; std::vector y_bres; @@ -252,8 +207,8 @@ std::pair, std::vector> TeorethicInformation::Bresenham(in int delta_x = abs(x_2 - x_1); int delta_y = abs(y_2 - y_1); - int s_x = getSign(x_1, x_2); - int s_y = getSign(y_1, y_2); + int s_x = signum(x_2 - x_1); + int s_y = signum(y_2 - y_1); bool interchange = false; if (delta_y > delta_x) @@ -298,20 +253,27 @@ std::pair, std::vector> TeorethicInformation::Bresenham(in x_bres.push_back(x); y_bres.push_back(y); } + // Delete the current robot cell + x_bres.erase(x_bres.begin()); + y_bres.erase(y_bres.begin()); + + // Adding last hit cell to the set + x_bres.push_back(x_2); + y_bres.push_back(y_2); return std::pair, std::vector>{x_bres, y_bres}; } -double TeorethicInformation::calculateEntropy(double probability) +double InformationEstimates::calculateInformationContent(double prob) { /* - To calculate the entropy + To calculate the information content or self-information + based on the proability of being occupied */ - return probability * log2(probability); + return - (prob * log2(prob)) - ((1 - prob) * log2(1 - prob)); } - -std::vector TeorethicInformation::calculateCellIntersectionPoints( - std::vector & laser_start, std::vector & laser_end, +std::vector InformationEstimates::calculateCellIntersectionPoints( + std::vector const& laser_start, std::vector const& laser_end, std::vector cell_start, std::vector cell_end) { /* @@ -344,9 +306,53 @@ std::vector TeorethicInformation::calculateCellIntersectionPoints( } } -void TeorethicInformation::updateCellLimits( - std::vector & initial_x, std::vector & initial_y, std::vector & final_x, std::vector & final_y, - double & limit_x, double & limit_y, std::vector & cell_limits, std::vector & robot_grid_pos, std::vector & final_grid_pos) +std::pair, std::vector> InformationEstimates::computeLineBoxIntersection( + std::vector const& laser_start, std::vector const& laser_end, + std::vector const& robot_grid_pos, std::vector const& final_grid_pos, + double limit_x, double limit_y) +{ + // Cell limits: min_x, max_x, min_y, max_y + std::vector cell_limits {limit_x, limit_x + m_cell_resol, limit_y, limit_y + m_cell_resol}; + + // Initial points for each of the 4 corners + std::vector initial_x {limit_x, limit_x, limit_x + m_cell_resol, limit_x + m_cell_resol}; + std::vector initial_y {limit_y, limit_y, limit_y + m_cell_resol, limit_y + m_cell_resol}; + + // Final points for each of the 4 corners + std::vector final_x {limit_x + m_cell_resol, limit_x, limit_x + m_cell_resol, limit_x}; + std::vector final_y {limit_y, limit_y + m_cell_resol, limit_y, limit_y + m_cell_resol}; + + // Set the new cell limits + updateCellLimits(initial_x, initial_y, final_x, final_y, limit_x, limit_y, cell_limits, robot_grid_pos, final_grid_pos); + + std::vector inter_x, inter_y; + + for (int k = 0; k < 4; ++k) + { + std::vector intersection = calculateCellIntersectionPoints(laser_start, laser_end, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); + if(intersection.size() != 0) + { + if ((fabs(intersection[0]) >= (fabs(cell_limits[0]) - 0.001)) && + (fabs(intersection[0]) <= (fabs(cell_limits[1]) + 0.001)) && + (fabs(intersection[1]) >= (fabs(cell_limits[2]) - 0.001)) && + (fabs(intersection[1]) <= (fabs(cell_limits[3]) + 0.001))) + { + /* + 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[0]); + inter_y.push_back(intersection[1]); + } + } + } + return std::pair, std::vector>{inter_x, inter_y}; +} + +void InformationEstimates::updateCellLimits( + std::vector& initial_x, std::vector& initial_y, std::vector& final_x, std::vector& final_y, + double limit_x, double limit_y, std::vector& cell_limits, std::vector const& robot_grid_pos, std::vector const& final_grid_pos) { /* To calculate grid grid limits for intersection @@ -354,55 +360,56 @@ void TeorethicInformation::updateCellLimits( if (final_grid_pos[0] < robot_grid_pos[0] && final_grid_pos[1] >= robot_grid_pos[1]) { // X greater and Y greater. WRO final points - final_x[0] = limit_x + m_resolution; - final_x[2] = limit_x + m_resolution; + final_x[0] = limit_x + m_cell_resol; + final_x[2] = limit_x + m_cell_resol; cell_limits[2] = limit_y; - cell_limits[3] = limit_y + m_resolution; + cell_limits[3] = limit_y + m_cell_resol; } if (final_grid_pos[0] >= robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) { // X greater and Y minor. WRO final points - initial_y[2] = limit_y - m_resolution; - initial_y[3] = limit_y - m_resolution; + initial_y[2] = limit_y - m_cell_resol; + initial_y[3] = limit_y - m_cell_resol; - final_y[1] = limit_y - m_resolution; - final_y[3] = limit_y - m_resolution; + final_y[1] = limit_y - m_cell_resol; + final_y[3] = limit_y - m_cell_resol; - cell_limits[2] = limit_y - m_resolution; + cell_limits[2] = limit_y - m_cell_resol; cell_limits[3] = limit_y; } if (final_grid_pos[0] < robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) { // X minor and Y minor. WRO final points - initial_x[2] = limit_x - m_resolution; - initial_x[3] = limit_x - m_resolution; - initial_y[2] = limit_y - m_resolution; - initial_y[3] = limit_y - m_resolution; + initial_x[2] = limit_x - m_cell_resol; + initial_x[3] = limit_x - m_cell_resol; + initial_y[2] = limit_y - m_cell_resol; + initial_y[3] = limit_y - m_cell_resol; - final_x[0] = limit_x - m_resolution; - final_x[2] = limit_x - m_resolution; - final_y[1] = limit_y - m_resolution; - final_y[3] = limit_y - m_resolution; + final_x[0] = limit_x - m_cell_resol; + final_x[2] = limit_x - m_cell_resol; + final_y[1] = limit_y - m_cell_resol; + final_y[3] = limit_y - m_cell_resol; - cell_limits[0] = limit_x - m_resolution; + cell_limits[0] = limit_x - m_cell_resol; cell_limits[1] = limit_x; - cell_limits[2] = limit_y - m_resolution; + cell_limits[2] = limit_y - m_cell_resol; cell_limits[3] = limit_y; } } -double TeorethicInformation::calculateLogs(double probability) +double InformationEstimates::calculateLogOddsFromProbability(double probability) { /* To calculate the log-odds + This should be a free function */ return log(probability / (1 - probability)); } -double TeorethicInformation::calculateMapMutualInformation() +double InformationEstimates::calculateMapMutualInformation() { /* To calculate map mutual information, this is the summation @@ -419,7 +426,16 @@ double TeorethicInformation::calculateMapMutualInformation() return sum; } -std::unordered_map TeorethicInformation::computeMeasurementOutcomesHistogram(std::vector>& meas_outcm) +double InformationEstimates::calculateProbabilityFromLogOdds(double log) +{ + /* + To transform the Log-odds into probability + This should be a free function + */ + return (exp(log) / (1 + exp(log))); +} + +std::unordered_map InformationEstimates::computeMeasurementOutcomesHistogram(std::vector>& meas_outcm) { /* To compute all the possible combinations of a grid cell, given a set of measurement outcomes @@ -445,28 +461,27 @@ std::unordered_map tup_vct; std::vector acc_prob; + // Measurement outcome probability + double free_prop = meas_outcm[r][0]; + double occ_prop = meas_outcm[r][1]; + double un_prop = meas_outcm[r][2]; + for (it_temp = temp_map.begin(); it_temp != temp_map.end(); ++it_temp) { // Index - int fr_idx = std::get<0>(it_temp->first); - int oc_idx = std::get<1>(it_temp->first); - int un_idx = std::get<2>(it_temp->first); - - // Measurement outcome probability - double free_prop = meas_outcm[r][0]; - double occ_prop = meas_outcm[r][1]; - double un_prop = meas_outcm[r][2]; - - if (fr_idx + oc_idx + un_idx == r) + int idx_free, idx_occ, idx_unk; + std::tie(idx_free, idx_occ, idx_unk) = it_temp->first; + + if (idx_free + idx_occ + idx_unk == r) { // Searching for the current combination in this level std::vector::iterator it_comb; - it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(fr_idx + 1, oc_idx, un_idx)); + it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(idx_free + 1, idx_occ, idx_unk)); // Free if (it_comb != tup_vct.end()) @@ -475,11 +490,11 @@ std::unordered_mapsecond * free_prop); } - it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(fr_idx, oc_idx + 1, un_idx)); + it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(idx_free, idx_occ + 1, idx_unk)); // Occupied if (it_comb != tup_vct.end()) @@ -488,11 +503,11 @@ std::unordered_mapsecond * occ_prop); } - it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(fr_idx, oc_idx, un_idx + 1)); + it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(idx_free, idx_occ, idx_unk + 1)); // Unobserved if (it_comb != tup_vct.end()) @@ -501,7 +516,7 @@ std::unordered_mapsecond * un_prop); } } @@ -523,11 +538,10 @@ std::unordered_mapfirst] = it_out->second; } } - return out_map; } -double TeorethicInformation::euclideanDistance(double x_1, double y_1, double x_2, double y_2) +double InformationEstimates::euclideanDistance(double x_1, double y_1, double x_2, double y_2) { /* To calculate the euclidean distance between two points @@ -538,30 +552,28 @@ double TeorethicInformation::euclideanDistance(double x_1, double y_1, double x_ return sqrt(diff_x*diff_x + diff_y*diff_y); } -int TeorethicInformation::getSign(int n_1, int n_2) +int InformationEstimates::signum(int num) { /* - To get the sign of an operation, used for Bresenham algorithm + To get the sign of an operation, used by Bresenham algorithm */ - int difference = n_2 - n_1; - - if (difference == 0) { return 0; } - else if (difference < 0) { return -1; } - else { return 1; } + if (num < 0) return -1; + if (num >= 1) return 1; + return 0; } -std::vector TeorethicInformation::getGridPosition(double x, double y) +std::vector InformationEstimates::getGridPosition(double x, double y) { /* To maps the current position into grid coordinates */ - int x_cell = floor((1 / m_resolution) * x); - int y_cell = floor((1 / m_resolution) * y); + int x_cell = floor((x / m_cell_resol)); + int y_cell = floor((y / m_cell_resol)); return {x_cell, y_cell}; } -std::vector TeorethicInformation::laserHitDistance(std::vector const& robot_pose, double range, double angle) +std::vector InformationEstimates::laserHitDistance(std::vector const& robot_pose, double range, double angle) { /* To get the distance where the laser beam hits something @@ -573,7 +585,7 @@ std::vector TeorethicInformation::laserHitDistance(std::vector c return {x_tf, y_tf}; } -void TeorethicInformation::initializeGrids() +void InformationEstimates::initializeGrids() { /* To create the grid @@ -593,7 +605,7 @@ void TeorethicInformation::initializeGrids() } } -double TeorethicInformation::measurementOutcomeEntropy(map_tuple const& meas_outcome) +double InformationEstimates::measurementOutcomeEntropy(map_tuple const& meas_outcome) { /* To calculate the measurement outcome entropy (Measurement outcome in the form ) @@ -601,58 +613,38 @@ double TeorethicInformation::measurementOutcomeEntropy(map_tuple const& meas_out - Calculate the probability from those logs - Calculate the entropy with the retrieved probability */ - double entropy = std::get<0>(meas_outcome) * calculateEntropy(probabilityFromLogs(calculateLogs(0.3f))) + - std::get<1>(meas_outcome) * calculateEntropy(probabilityFromLogs(calculateLogs(0.7f))) + - std::get<2>(meas_outcome) * calculateEntropy(probabilityFromLogs(calculateLogs(0.5f))); - return entropy; -} - -double TeorethicInformation::probabilityFromLogs(double log) -{ - /* - To transform the Log-odds into probability - */ - return (exp(log) / (1 + exp(log))); + int num_free, num_occ, num_unk; + std::tie(num_free, num_occ, num_unk) = meas_outcome; + + double log_occ = (num_free * l_free) + (num_occ * l_occ) - ((num_free + num_occ - 1) * l_o); + double prob_occ = calculateProbabilityFromLogOdds(log_occ); + return calculateInformationContent(prob_occ); } -double TeorethicInformation::probabilityFromObservation(double range_1, double range_2) +double InformationEstimates::calculateScanMassProbabilityBetween(double range_1, double range_2) { /* - To calculate the probability of a cell being observed by a given measurement + To calculate the mass probability of a cell being observed by a given measurement */ - double max_range = 5.0f; - double lambda = 0.35f; - double nu = 0.28f; - range_1 = (range_1 > max_range) ? max_range : range_1; - range_2 = (range_2 > max_range) ? max_range : range_2; + 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; - return nu * (exp(-lambda*range_1) - exp(-lambda*range_2)); + return m_obs_nu * (exp(-m_obs_lambda*range_1) - exp(-m_obs_lambda*range_2)); } -std::vector> TeorethicInformation::retreiveMeasurementOutcomes(std::vector cell) +std::vector> InformationEstimates::retreiveCellProbabilities(std::vector cell) { /* - To get all the measurement outcomes for the current cell + To get all the cell probabilities */ - std::vector> meas_outcomes; std::map, std::vector>>::iterator it_cells; it_cells = m_cell_probabilities.find({cell[0], cell[1]}); - if (it_cells != m_cell_probabilities.end()) - { - // Exploring the measurement outcomes for the specific cell - for (int i = 0; i < it_cells->second.size(); ++i) - { - // Append the measurement outcomes for the current cell - meas_outcomes.push_back({it_cells->second[i][0], it_cells->second[i][1], it_cells->second[i][2]}); - } - } - return meas_outcomes; + return it_cells->second; } - -void TeorethicInformation::updateCellMutualInformation(double mut_inf, std::vector cell) +void InformationEstimates::updateCellMutualInformation(double mut_inf, std::vector cell) { /* To update the mutual information for each individual cell @@ -660,3 +652,28 @@ void TeorethicInformation::updateCellMutualInformation(double mut_inf, std::vect */ m_mutual_grid[cell[0]][cell[1]] = mut_inf; } + +void InformationEstimates::setMaxSensorRange(double const sensor_range) +{ + m_max_sensor_range = sensor_range; +} + +void InformationEstimates::setObservationLambda(double const lambda) +{ + m_obs_lambda = lambda; +} + +void InformationEstimates::setObservationNu(double const nu) +{ + m_obs_nu = nu; +} + +void InformationEstimates::setCellResolution(double const resolution) +{ + m_cell_resol = resolution; +} + +void InformationEstimates::setMapDistance(double const distance) +{ + m_map_dist = distance; +} From e2a13eff85caa7851e6b46cf0a290840ff9320bf Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Wed, 22 Dec 2021 12:06:29 -0500 Subject: [PATCH 36/83] [WIP]: Changes in algorithm 1 --- .../experimental/information_estimates.hpp | 6 ++++ src/experimental/information_estimates.cpp | 33 +++++++++---------- 2 files changed, 21 insertions(+), 18 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index 19b5b9d2d..4d69b180a 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -8,6 +8,7 @@ #include #include #include +#include "lib/karto_sdk/include/karto_sdk/Karto.h" class InformationEstimates { @@ -107,6 +108,11 @@ class InformationEstimates void setObservationNu(double const nu); void setCellResolution(double const resolution); void setMapDistance(double const distance); + + karto::Vector2 point; + // This pose will save actual robot poses + karto::Pose2 pose; + }; #endif diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 99bbcf1df..2fb892f0f 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -113,10 +113,9 @@ void InformationEstimates::scannerTest() std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(cell_prob); double cell_mutual_inf = 0.0f; - std::unordered_map::iterator it_mutual; - for (it_mutual = meas_out_prob.begin(); it_mutual != meas_out_prob.end(); ++it_mutual) + for (auto& pair : meas_out_prob) { - cell_mutual_inf += it_mutual->second * measurementOutcomeEntropy(it_mutual->first); + cell_mutual_inf += pair.second * measurementOutcomeEntropy(pair.first); } // Mutual information of cell x, y given a set of measurements @@ -441,15 +440,14 @@ std::unordered_map temp_map; - std::unordered_map::iterator it_temp; // The number of measurements int k = meas_outcm.size(); int r = 1; - double p_free = meas_outcm[0][2]; + double p_free = meas_outcm[0][0]; double p_occ = meas_outcm[0][1]; - double p_un = meas_outcm[0][0]; + double p_un = meas_outcm[0][2]; temp_map.clear(); @@ -471,11 +469,11 @@ std::unordered_mapfirst; + std::tie(idx_free, idx_occ, idx_unk) = pair.first; if (idx_free + idx_occ + idx_unk == r) { @@ -486,12 +484,12 @@ std::unordered_mapsecond * free_prop; + acc_prob[it_comb - tup_vct.begin()] += pair.second * free_prop; } else { tup_vct.push_back(std::make_tuple(idx_free + 1, idx_occ, idx_unk)); - acc_prob.push_back(it_temp->second * free_prop); + acc_prob.push_back(pair.second * free_prop); } it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(idx_free, idx_occ + 1, idx_unk)); @@ -499,12 +497,12 @@ std::unordered_mapsecond * occ_prop; + acc_prob[it_comb - tup_vct.begin()] += pair.second * occ_prop; } else { tup_vct.push_back(std::make_tuple(idx_free, idx_occ + 1, idx_unk)); - acc_prob.push_back(it_temp->second * occ_prop); + acc_prob.push_back(pair.second * occ_prop); } it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(idx_free, idx_occ, idx_unk + 1)); @@ -512,12 +510,12 @@ std::unordered_mapsecond * un_prop; + acc_prob[it_comb - tup_vct.begin()] += pair.second * un_prop; } else { tup_vct.push_back(std::make_tuple(idx_free, idx_occ, idx_unk + 1)); - acc_prob.push_back(it_temp->second * un_prop); + acc_prob.push_back(pair.second * un_prop); } } } @@ -530,12 +528,11 @@ std::unordered_map out_map; - std::unordered_map::iterator it_out; - for (it_out = temp_map.begin(); it_out != temp_map.end(); ++it_out) + for (auto& pair : temp_map) { - if (std::get<0>(it_out->first) + std::get<1>(it_out->first) + std::get<2>(it_out->first) == k) + if (std::get<0>(pair.first) + std::get<1>(pair.first) + std::get<2>(pair.first) == k) { - out_map[it_out->first] = it_out->second; + out_map[pair.first] = pair.second; } } return out_map; From 43ce3bf5ff039cc08058ed9e8c476e585c965211 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Thu, 23 Dec 2021 11:44:57 -0500 Subject: [PATCH 37/83] [WIP]: Function return values modifications to match Karto --- .../experimental/information_estimates.hpp | 10 +- src/experimental/information_estimates.cpp | 92 +++++++++++-------- 2 files changed, 59 insertions(+), 43 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index 4d69b180a..4669f9526 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -40,18 +40,18 @@ class InformationEstimates void initializeGrids(); void updateCellLimits( std::vector& initial_x, std::vector& initial_y, std::vector& final_x, std::vector& final_y, - double limit_x, double limit_y, std::vector& cell_limits, std::vector const& robot_grid_pos, std::vector const& final_grid_pos); + double limit_x, double limit_y, std::vector& cell_limits, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos); // Grid and position information - std::pair, std::vector> rayCasting(int x_1, int y_1, int x_2, int y_2); - std::vector getGridPosition(double x, double y); + std::pair, std::vector> rayCasting(karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt); + karto::Vector2 getGridPosition(double x, double y); std::vector laserHitDistance(std::vector const& robot_pose, double range, double angle); std::vector calculateCellIntersectionPoints(std::vector const& laser_start, std::vector const& laser_end, std::vector cell_start, std::vector cell_end); int signum(int num); std::pair, std::vector> computeLineBoxIntersection( std::vector const& laser_start, std::vector const& laser_end, - std::vector const& robot_grid_pos, std::vector const& final_grid_pos, + karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, double limit_x, double limit_y); // Measurements calculations @@ -112,6 +112,8 @@ class InformationEstimates karto::Vector2 point; // This pose will save actual robot poses karto::Pose2 pose; + + std::vector poses; }; diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 2fb892f0f..ac43b78fa 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -17,23 +17,18 @@ InformationEstimates::InformationEstimates() initializeGrids(); scannerTest(); -} - -void InformationEstimates::scannerTest() -{ - /* - La lectura de menor ganancia - Dado uno computamos el score - Esta informacion esta en el toolbox - Buscar scans en el mismo rango de vision - // Set -> information - // Nodo -> Que tanta ganancia aporta (Ganancia de informacion) - Batch elimination (Hiteresis) - Groups + poses.push_back(karto::Pose2{1.78512, 2.1213 , M_PI}); - Lista de adyacencia - */ + karto::Pose2 pose = poses[0]; + std::cout << pose.GetX() << std::endl; + std::cout << pose.GetY() << std::endl; + std::cout << pose.GetHeading() << std::endl; +} +void InformationEstimates::scannerTest() +{ // Loop through the different robot poses for (int r = 0; r < robot_poses.size(); ++r) { @@ -42,27 +37,43 @@ void InformationEstimates::scannerTest() // Angles {-50, -25, 0, 25, 50} in degrees std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; - // Initial point - std::vector robot_grid_pos = getGridPosition(robot_poses[r][0], robot_poses[r][1]); - std::cout << "Robot position: " << robot_grid_pos[0] << ", " << robot_grid_pos[1] << std::endl; + karto::Vector2 robot_grid_pos_1 = getGridPosition(robot_poses[r][0], robot_poses[r][1]); // Set as false the current boolean map clearVisitedCells(); + /* + * Laser callback message + sensor_msgs::msg::LaserScan::ConstSharedPtr scan + + * Double checking that current laser can be used + LaserRangeFinder * laser = getLaser(scan); + + * Localized range scan when the laser object, the laser scan and the pose are given + LocalizedRangeScan * range_scan = addScan(laser, scan, pose); + + * Point readings + const PointVectorDouble & pts = candidate_scan->GetPointReadings(true); + PointVectorDouble::const_iterator pt_it; + */ + for (int i = 0; i < laser_ranges[r].size(); ++i) { std::cout << "................ New laser ................" << std::endl; std::cout << "Distance: " << laser_ranges[r][i] << ", Angle: " << angles[i] << std::endl; - // Laser continuous distance + // This is the transformation - Should use Transform from Karto + + // Laser continuous distance -- This function will potentnally not be used std::vector laser_end = laserHitDistance(robot_poses[r], laser_ranges[r][i], angles[i]); // Laser final cell - std::vector final_grid_pos = getGridPosition(laser_end[0], laser_end[1]); + karto::Vector2 final_grid_pos_1 = getGridPosition(laser_end[0], laser_end[1]); // Ray tracing for getting the visited cells std::vector cells_x, cells_y; - std::pair, std::vector> res_pair = rayCasting(robot_grid_pos[0], robot_grid_pos[1], final_grid_pos[0], final_grid_pos[1]); + // I can change the returnign type as well + std::pair, std::vector> res_pair = rayCasting(robot_grid_pos_1, final_grid_pos_1); cells_x = res_pair.first; cells_y = res_pair.second; @@ -76,7 +87,7 @@ void InformationEstimates::scannerTest() double limit_x = cells_x[j] * m_cell_resol; double limit_y = cells_y[j] * m_cell_resol; - std::pair, std::vector> intersections = computeLineBoxIntersection(robot_poses[r], laser_end, robot_grid_pos, final_grid_pos, limit_x, limit_y); + std::pair, std::vector> intersections = computeLineBoxIntersection(robot_poses[r], laser_end, robot_grid_pos_1, final_grid_pos_1, limit_x, limit_y); if (intersections.first.size() == 0) continue; @@ -191,7 +202,7 @@ void InformationEstimates::appendCellProbabilities(std::vector& measurem } } -std::pair, std::vector> InformationEstimates::rayCasting(int x_1, int y_1, int x_2, int y_2) +std::pair, std::vector> InformationEstimates::rayCasting(karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt) { /* To find the set of cells hit by a laser beam @@ -200,14 +211,14 @@ std::pair, std::vector> InformationEstimates::rayCasting(i std::vector x_bres; std::vector y_bres; - int x = x_1; - int y = y_1; + int x = initial_pt.GetX(); + int y = initial_pt.GetY(); - int delta_x = abs(x_2 - x_1); - int delta_y = abs(y_2 - y_1); + int delta_x = abs(final_pt.GetX() - initial_pt.GetX()); + int delta_y = abs(final_pt.GetY() - initial_pt.GetY()); - int s_x = signum(x_2 - x_1); - int s_y = signum(y_2 - y_1); + int s_x = signum(final_pt.GetX() - initial_pt.GetX()); + int s_y = signum(final_pt.GetY() - initial_pt.GetY()); bool interchange = false; if (delta_y > delta_x) @@ -257,8 +268,9 @@ std::pair, std::vector> InformationEstimates::rayCasting(i y_bres.erase(y_bres.begin()); // Adding last hit cell to the set - x_bres.push_back(x_2); - y_bres.push_back(y_2); + x_bres.push_back(final_pt.GetX()); + y_bres.push_back(final_pt.GetY()); + return std::pair, std::vector>{x_bres, y_bres}; } @@ -307,7 +319,7 @@ std::vector InformationEstimates::calculateCellIntersectionPoints( std::pair, std::vector> InformationEstimates::computeLineBoxIntersection( std::vector const& laser_start, std::vector const& laser_end, - std::vector const& robot_grid_pos, std::vector const& final_grid_pos, + karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, double limit_x, double limit_y) { // Cell limits: min_x, max_x, min_y, max_y @@ -349,14 +361,18 @@ std::pair, std::vector> InformationEstimates::comput return std::pair, std::vector>{inter_x, inter_y}; } +// void InformationEstimates::updateCellLimits( +// std::vector& initial_x, std::vector& initial_y, std::vector& final_x, std::vector& final_y, +// double limit_x, double limit_y, std::vector& cell_limits, std::vector const& robot_grid_pos, std::vector const& final_grid_pos) +// { void InformationEstimates::updateCellLimits( std::vector& initial_x, std::vector& initial_y, std::vector& final_x, std::vector& final_y, - double limit_x, double limit_y, std::vector& cell_limits, std::vector const& robot_grid_pos, std::vector const& final_grid_pos) + double limit_x, double limit_y, std::vector& cell_limits, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos) { /* To calculate grid grid limits for intersection */ - if (final_grid_pos[0] < robot_grid_pos[0] && final_grid_pos[1] >= robot_grid_pos[1]) + if (final_grid_pos.GetX() < robot_grid_pos.GetX() && final_grid_pos.GetY() >= robot_grid_pos.GetY()) { // X greater and Y greater. WRO final points final_x[0] = limit_x + m_cell_resol; @@ -366,7 +382,7 @@ void InformationEstimates::updateCellLimits( cell_limits[3] = limit_y + m_cell_resol; } - if (final_grid_pos[0] >= robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) + if (final_grid_pos.GetX() >= robot_grid_pos.GetX() && final_grid_pos.GetY() < robot_grid_pos.GetY()) { // X greater and Y minor. WRO final points initial_y[2] = limit_y - m_cell_resol; @@ -379,7 +395,7 @@ void InformationEstimates::updateCellLimits( cell_limits[3] = limit_y; } - if (final_grid_pos[0] < robot_grid_pos[0] && final_grid_pos[1] < robot_grid_pos[1]) + if (final_grid_pos.GetX() < robot_grid_pos.GetX() && final_grid_pos.GetY() < robot_grid_pos.GetY()) { // X minor and Y minor. WRO final points initial_x[2] = limit_x - m_cell_resol; @@ -559,17 +575,15 @@ int InformationEstimates::signum(int num) return 0; } -std::vector InformationEstimates::getGridPosition(double x, double y) +karto::Vector2 InformationEstimates::getGridPosition(double x, double y) { - /* - To maps the current position into grid coordinates - */ int x_cell = floor((x / m_cell_resol)); int y_cell = floor((y / m_cell_resol)); - return {x_cell, y_cell}; + return karto::Vector2{x_cell, y_cell}; } + std::vector InformationEstimates::laserHitDistance(std::vector const& robot_pose, double range, double angle) { /* From 71f59303c4a221c35219369b812cec8530d6e08c Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Mon, 27 Dec 2021 13:21:07 -0500 Subject: [PATCH 38/83] [WIP]: Integration of new elements --- .../experimental/information_estimates.hpp | 25 +-- src/experimental/information_estimates.cpp | 202 +++++++----------- 2 files changed, 84 insertions(+), 143 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index 4669f9526..314b63487 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -10,6 +10,9 @@ #include #include "lib/karto_sdk/include/karto_sdk/Karto.h" +#include "slam_toolbox/slam_toolbox_common.hpp" +#include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp" + class InformationEstimates { typedef std::tuple map_tuple; @@ -32,6 +35,9 @@ class InformationEstimates } }; +public: + float calculateMutualInformation(karto::PointVectorDouble const& laser_readings, karto::Pose2 const& karto_pose); + private: // Testing void scannerTest(); @@ -44,15 +50,13 @@ class InformationEstimates // Grid and position information std::pair, std::vector> rayCasting(karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt); - karto::Vector2 getGridPosition(double x, double y); - std::vector laserHitDistance(std::vector const& robot_pose, double range, double angle); - std::vector calculateCellIntersectionPoints(std::vector const& laser_start, std::vector const& laser_end, std::vector cell_start, std::vector cell_end); - int signum(int num); - + karto::Vector2 getGridPosition(karto::Vector2 const& pose); + std::vector calculateCellIntersectionPoints(karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, std::vector cell_start, std::vector cell_end); std::pair, std::vector> computeLineBoxIntersection( - std::vector const& laser_start, std::vector const& laser_end, - karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, + karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, + karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, double limit_x, double limit_y); + int signum(int num); // Measurements calculations double calculateScanMassProbabilityBetween(double range_1, double range_2); @@ -108,13 +112,6 @@ class InformationEstimates void setObservationNu(double const nu); void setCellResolution(double const resolution); void setMapDistance(double const distance); - - karto::Vector2 point; - // This pose will save actual robot poses - karto::Pose2 pose; - - std::vector poses; - }; #endif diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index ac43b78fa..233444de8 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -1,4 +1,5 @@ #include +#include #include "slam_toolbox/experimental/information_estimates.hpp" InformationEstimates::InformationEstimates() @@ -6,8 +7,9 @@ InformationEstimates::InformationEstimates() /* Need to add the new elements for the constructor */ - m_cell_resol = 0.5f; // Map resolution - m_map_dist = 20.0f; // Total map distance + + m_cell_resol = 0.05f; // Map resolution + m_map_dist = 200.0f; // Total map distance m_num_cells = static_cast(m_map_dist / m_cell_resol); // Grids initialization (Occupancy and Mutual information) @@ -16,127 +18,81 @@ InformationEstimates::InformationEstimates() visited_cells.resize(m_num_cells); initializeGrids(); - scannerTest(); - - - poses.push_back(karto::Pose2{1.78512, 2.1213 , M_PI}); - - karto::Pose2 pose = poses[0]; - std::cout << pose.GetX() << std::endl; - std::cout << pose.GetY() << std::endl; - std::cout << pose.GetHeading() << std::endl; + // scannerTest(); } -void InformationEstimates::scannerTest() +float InformationEstimates::calculateMutualInformation(karto::PointVectorDouble const& laser_readings, karto::Pose2 const& robot_pose) { - // Loop through the different robot poses - for (int r = 0; r < robot_poses.size(); ++r) - { - std::cout << "-------------------- Robot pose --------------------: " << r << std::endl; - - // Angles {-50, -25, 0, 25, 50} in degrees - std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; - - karto::Vector2 robot_grid_pos_1 = getGridPosition(robot_poses[r][0], robot_poses[r][1]); - - // Set as false the current boolean map - clearVisitedCells(); + karto::Vector2 robot_grid_pos_1 = getGridPosition(robot_pose.GetPosition()); - /* - * Laser callback message - sensor_msgs::msg::LaserScan::ConstSharedPtr scan + // Set as false the current boolean map + clearVisitedCells(); - * Double checking that current laser can be used - LaserRangeFinder * laser = getLaser(scan); - - * Localized range scan when the laser object, the laser scan and the pose are given - LocalizedRangeScan * range_scan = addScan(laser, scan, pose); + // for (int i = 0; i < laser_ranges[r].size(); ++i) + for (int i = 0; i < laser_readings.size(); ++i) + { + // Laser final cell + karto::Vector2 final_grid_pos_1 = getGridPosition(laser_readings[i]); - * Point readings - const PointVectorDouble & pts = candidate_scan->GetPointReadings(true); - PointVectorDouble::const_iterator pt_it; - */ + // Ray tracing for getting the visited cells + std::vector cells_x, cells_y; + std::pair, std::vector> res_pair = rayCasting(robot_grid_pos_1, final_grid_pos_1); + cells_x = res_pair.first; + cells_y = res_pair.second; - for (int i = 0; i < laser_ranges[r].size(); ++i) + // Visiting the cells + for (int j = 0; j < cells_x.size(); ++j) { - std::cout << "................ New laser ................" << std::endl; - std::cout << "Distance: " << laser_ranges[r][i] << ", Angle: " << angles[i] << std::endl; - - // This is the transformation - Should use Transform from Karto + // Inidividual cell limits + double limit_x = cells_x[j] * m_cell_resol; + double limit_y = cells_y[j] * m_cell_resol; - // Laser continuous distance -- This function will potentnally not be used - std::vector laser_end = laserHitDistance(robot_poses[r], laser_ranges[r][i], angles[i]); + std::pair, std::vector> intersections = computeLineBoxIntersection(robot_pose.GetPosition(), laser_readings[i], robot_grid_pos_1, final_grid_pos_1, limit_x, limit_y); - // Laser final cell - karto::Vector2 final_grid_pos_1 = getGridPosition(laser_end[0], laser_end[1]); + if (intersections.first.size() == 0) + continue; - // Ray tracing for getting the visited cells - std::vector cells_x, cells_y; - // I can change the returnign type as well - std::pair, std::vector> res_pair = rayCasting(robot_grid_pos_1, final_grid_pos_1); - cells_x = res_pair.first; - cells_y = res_pair.second; - - // Visiting the cells - for (int j = 0; j < cells_x.size(); ++j) + // Enter (d1) and Exit (d2) distances + std::vector distances; + for (int k = 0; k < intersections.first.size(); ++k) { - // Cells visualization - std::cout << "Current cell: " << cells_x[j] << ", " << cells_y[j] << std::endl; - - // Inidividual cell limits - double limit_x = cells_x[j] * m_cell_resol; - double limit_y = cells_y[j] * m_cell_resol; - - std::pair, std::vector> intersections = computeLineBoxIntersection(robot_poses[r], laser_end, robot_grid_pos_1, final_grid_pos_1, limit_x, limit_y); + // From robot position to intersection points + karto::Vector2 intersection{intersections.first[k], intersections.second[k]}; + kt_double distance = robot_pose.GetPosition().Distance(intersection); + distances.push_back(distance); + } - if (intersections.first.size() == 0) - continue; + // Measurement outcomes vector {Pfree, Pocc, Pun} + std::vector probabilities { + calculateScanMassProbabilityBetween(distances[1], 5.0f), + calculateScanMassProbabilityBetween(distances[0], distances[1]), + calculateScanMassProbabilityBetween(0.0f, distances[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 - double dist_point = euclideanDistance(robot_poses[r][0], robot_poses[r][1], intersections.first[k], intersections.second[k]); - distances.push_back(dist_point); - } + // Appending new measurement outcomes for the current cell + appendCellProbabilities(probabilities, {cells_x[j], cells_y[j]}); - // Integral 1: 0 to d1 which is distance from robot pose to first point where the cell is cut - // Integral 2: d1 which is distance from robot pose to first point where the cell is cut to - // d2 which is distance from robot pose to second point where the cell is cut - // Integral 3: d2 which is distance from robot pose to second point where the cell is cut to z_max - - // Measurement outcomes vector {Pfree, Pocc, Pun} - // Map of tuples - std::vector probabilities { - calculateScanMassProbabilityBetween(distances[1], 5.0f), - calculateScanMassProbabilityBetween(distances[0], distances[1]), - calculateScanMassProbabilityBetween(0.0f, distances[0]) - }; - - // Appending new measurement outcomes for the current cell - appendCellProbabilities(probabilities, {cells_x[j], cells_y[j]}); - - // Get all the measurement outcomes for the current cell - std::vector> cell_prob = retreiveCellProbabilities({cells_x[j], cells_y[j]}); - - // Compute all the possible combinations for the current cell - algorithm 1 - std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(cell_prob); - - double cell_mutual_inf = 0.0f; - for (auto& pair : meas_out_prob) - { - cell_mutual_inf += pair.second * measurementOutcomeEntropy(pair.first); - } + // Get all the measurement outcomes for the current cell + std::vector> cell_prob = retreiveCellProbabilities({cells_x[j], cells_y[j]}); - // Mutual information of cell x, y given a set of measurements - updateCellMutualInformation(0.5 - cell_mutual_inf, {cells_x[j], cells_y[j]}); - std::cout << "++++++++++++++++++++++++" << std::endl; + // Compute all the possible combinations for the current cell - algorithm 1 + std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(cell_prob); + + 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 + updateCellMutualInformation(0.5 - cell_mutual_inf, {cells_x[j], cells_y[j]}); + std::cout << "++++++++++++++++++++++++" << std::endl; } - double mutual = calculateMapMutualInformation(); - std::cout << "Mutual information: " << mutual << std::endl; } + double mutual = calculateMapMutualInformation(); + std::cout << "Mutual information: " << mutual << std::endl; + + return 0.5f; } void InformationEstimates::clearVisitedCells() @@ -184,9 +140,6 @@ void InformationEstimates::appendCellProbabilities(std::vector& measurem int idx = it_cell->second.size() - 1; if(measurements[2] < it_cell->second[idx][2]) { - // std::cout << "Replacing:" << it_cell->second[idx][0] << ", " << it_cell->second[idx][1] << ", " << it_cell->second[idx][2] << std::endl; - // std::cout << "With:" << measurements[0] << ", " << measurements[1] << ", " << measurements[2] << std::endl; - // Replacing it_cell->second[idx][0] = measurements[0]; it_cell->second[idx][1] = measurements[1]; @@ -284,7 +237,7 @@ double InformationEstimates::calculateInformationContent(double prob) } std::vector InformationEstimates::calculateCellIntersectionPoints( - std::vector const& laser_start, std::vector const& laser_end, + karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, std::vector cell_start, std::vector cell_end) { /* @@ -293,13 +246,17 @@ std::vector InformationEstimates::calculateCellIntersectionPoints( Initial point cell: cell_start Final point cell: cell_end */ - double x1 = laser_start[0]; - double x2 = laser_end[0]; + // double x1 = laser_start[0]; + // double x2 = laser_end[0]; + double x1 = laser_start.GetX(); + double x2 = laser_end.GetX(); double x3 = cell_start[0]; double x4 = cell_end[0]; - double y1 = laser_start[1]; - double y2 = laser_end[1]; + // double y1 = laser_start[1]; + // double y2 = laser_end[1]; + double y1 = laser_start.GetY(); + double y2 = laser_end.GetY(); double y3 = cell_start[1]; double y4 = cell_end[1]; @@ -318,7 +275,7 @@ std::vector InformationEstimates::calculateCellIntersectionPoints( } std::pair, std::vector> InformationEstimates::computeLineBoxIntersection( - std::vector const& laser_start, std::vector const& laser_end, + karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, double limit_x, double limit_y) { @@ -575,27 +532,14 @@ int InformationEstimates::signum(int num) return 0; } -karto::Vector2 InformationEstimates::getGridPosition(double x, double y) +karto::Vector2 InformationEstimates::getGridPosition(karto::Vector2 const& pose) { - int x_cell = floor((x / m_cell_resol)); - int y_cell = floor((y / m_cell_resol)); + int x_cell = floor((pose.GetX() / m_cell_resol)); + int y_cell = floor((pose.GetY() / m_cell_resol)); return karto::Vector2{x_cell, y_cell}; } - -std::vector InformationEstimates::laserHitDistance(std::vector const& robot_pose, double range, double angle) -{ - /* - To get the distance where the laser beam hits something - - Applying RBT from the global to the sensor frame - */ - double x_tf = (range * cos(robot_pose[2] + angle)) + robot_pose[0]; - double y_tf = (range * sin(robot_pose[2] + angle)) + robot_pose[1]; - - return {x_tf, y_tf}; -} - void InformationEstimates::initializeGrids() { /* From ed0f586faede35851cd5853c9785183ee253f94e Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Tue, 28 Dec 2021 08:37:39 -0500 Subject: [PATCH 39/83] [WIP]: Replacing data types from double to kt_double --- .../experimental/information_estimates.hpp | 78 +++---- src/experimental/information_estimates.cpp | 203 ++++++++---------- 2 files changed, 120 insertions(+), 161 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index 314b63487..7c67af080 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -16,7 +16,7 @@ class InformationEstimates { typedef std::tuple map_tuple; - typedef std::pair map_pair; + typedef std::pair map_pair; public: InformationEstimates(); // Empty contructor for now @@ -39,79 +39,69 @@ class InformationEstimates float calculateMutualInformation(karto::PointVectorDouble const& laser_readings, karto::Pose2 const& karto_pose); private: - // Testing - void scannerTest(); - // Grid operations void initializeGrids(); void updateCellLimits( - std::vector& initial_x, std::vector& initial_y, std::vector& final_x, std::vector& final_y, - double limit_x, double limit_y, std::vector& cell_limits, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos); + std::vector& initial_x, std::vector& initial_y, std::vector& final_x, std::vector& final_y, + kt_double limit_x, kt_double limit_y, std::vector& cell_limits, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos); // Grid and position information std::pair, std::vector> rayCasting(karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt); karto::Vector2 getGridPosition(karto::Vector2 const& pose); - std::vector calculateCellIntersectionPoints(karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, std::vector cell_start, std::vector cell_end); - std::pair, std::vector> computeLineBoxIntersection( + std::vector calculateCellIntersectionPoints(karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, std::vector cell_start, std::vector cell_end); + std::pair, std::vector> computeLineBoxIntersection( karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, - double limit_x, double limit_y); + kt_double limit_x, kt_double limit_y); int signum(int num); // Measurements calculations - double calculateScanMassProbabilityBetween(double range_1, double range_2); - double euclideanDistance(double x_1, double y_1, double x_2, double y_2); + kt_double calculateScanMassProbabilityBetween(kt_double range_1, kt_double range_2); // Mutual information - double calculateInformationContent(double prob); - double calculateLogOddsFromProbability(double probability); - double calculateMapMutualInformation(); - double measurementOutcomeEntropy(map_tuple const& meas_outcome); - double calculateProbabilityFromLogOdds(double log); + kt_double calculateInformationContent(kt_double prob); + kt_double calculateLogOddsFromProbability(kt_double probability); + kt_double calculateMapMutualInformation(); + kt_double measurementOutcomeEntropy(map_tuple const& meas_outcome); + kt_double calculateProbabilityFromLogOdds(kt_double log); void recoverProbability(); - void updateCellMutualInformation(double mut_inf, std::vector cell); + void updateCellMutualInformation(kt_double mut_inf, std::vector cell); // Measurement outcomes probabilities - void appendCellProbabilities(std::vector& measurements, std::vector cell); - std::unordered_map computeMeasurementOutcomesHistogram(std::vector>& meas_outcm); - std::vector> retreiveCellProbabilities(std::vector cell); + void appendCellProbabilities(std::vector& measurements, std::vector cell); + std::unordered_map computeMeasurementOutcomesHistogram(std::vector>& meas_outcm); + std::vector> retreiveCellProbabilities(std::vector cell); void clearVisitedCells(); private: // Data structures - std::unordered_map m_map_out; - std::map, std::vector>> m_cell_probabilities; - std::vector> m_mutual_grid; + std::unordered_map m_map_out; + std::map, std::vector>> m_cell_probabilities; + std::vector> m_mutual_grid; std::vector> m_grid; - double m_map_dist; - double m_cell_resol; + kt_double m_map_dist; + kt_double m_cell_resol; int m_num_cells; - const double l_free = log(0.3 / (1.0 - 0.3)); - const double l_occ = log(0.7 / (1.0 - 0.7)); - const double l_o = log(0.5 / (1.0 - 0.5)); - - // Robot information - Defining values just for testing - // std::vector> robot_poses {{5.6f, 6.0f, M_PI/2}, {7.75f, 9.22f, -M_PI/2}}; - // std::vector> robot_poses {{7.75f, 9.22f, -M_PI/2}, {3.5f, 9.0f, 0.0f}}; - std::vector> robot_poses {{5.6f, 6.0f, M_PI/2}, {3.5f, 9.0f, 0.0f}}; - std::vector> laser_ranges {{1.65f, 5.0f, 5.0f, 5.0f, 5.0f}, {5.0f, 5.0f, 4.0f, 5.0f, 5.0f}}; - // std::vector> laser_ranges {{5.0f, 5.0f, 5.0f, 5.0f, 5.0f}, {5.0f, 5.0f, 4.0f, 5.0f, 5.0f}}; - std::vector angles{-0.87266f, -0.43633f, 0.0f, 0.43633f, 0.87266f}; + 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)); std::vector> visited_cells; - double m_max_sensor_range = 5.0; - double m_obs_lambda = 0.35; - double m_obs_nu = 0.28; + kt_double m_max_sensor_range = 5.0; + kt_double m_obs_lambda = 0.35; + kt_double m_obs_nu = 0.28; - void setMaxSensorRange(double const sensor_range); - void setObservationLambda(double const lambda); - void setObservationNu(double const nu); - void setCellResolution(double const resolution); - void setMapDistance(double const distance); +public: + // Setters + void setMaxSensorRange(kt_double const sensor_range); + void setObservationLambda(kt_double const lambda); + void setObservationNu(kt_double const nu); + void setCellResolution(kt_double const resolution); + void setMapDistance(kt_double const distance); }; #endif diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 233444de8..e00ed72b9 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -18,25 +18,30 @@ InformationEstimates::InformationEstimates() visited_cells.resize(m_num_cells); initializeGrids(); - // scannerTest(); + + // Test + karto::Vector2 point_2{3.0, 2.0}; + karto::Vector2 point_1{0.0, 0.0}; + kt_double distance = point_1.Distance(point_2); + + std::cout << distance << std::endl; } float InformationEstimates::calculateMutualInformation(karto::PointVectorDouble const& laser_readings, karto::Pose2 const& robot_pose) { - karto::Vector2 robot_grid_pos_1 = getGridPosition(robot_pose.GetPosition()); + karto::Vector2 robot_grid = getGridPosition(robot_pose.GetPosition()); // Set as false the current boolean map clearVisitedCells(); - // for (int i = 0; i < laser_ranges[r].size(); ++i) for (int i = 0; i < laser_readings.size(); ++i) { // Laser final cell - karto::Vector2 final_grid_pos_1 = getGridPosition(laser_readings[i]); + karto::Vector2 beam_grid = getGridPosition(laser_readings[i]); // Ray tracing for getting the visited cells std::vector cells_x, cells_y; - std::pair, std::vector> res_pair = rayCasting(robot_grid_pos_1, final_grid_pos_1); + std::pair, std::vector> res_pair = rayCasting(robot_grid, beam_grid); cells_x = res_pair.first; cells_y = res_pair.second; @@ -44,16 +49,16 @@ float InformationEstimates::calculateMutualInformation(karto::PointVectorDouble for (int j = 0; j < cells_x.size(); ++j) { // Inidividual cell limits - double limit_x = cells_x[j] * m_cell_resol; - double limit_y = cells_y[j] * m_cell_resol; + kt_double limit_x = cells_x[j] * m_cell_resol; + kt_double limit_y = cells_y[j] * m_cell_resol; - std::pair, std::vector> intersections = computeLineBoxIntersection(robot_pose.GetPosition(), laser_readings[i], robot_grid_pos_1, final_grid_pos_1, limit_x, limit_y); + std::pair, std::vector> intersections = computeLineBoxIntersection(robot_pose.GetPosition(), laser_readings[i], robot_grid, beam_grid, limit_x, limit_y); if (intersections.first.size() == 0) continue; // Enter (d1) and Exit (d2) distances - std::vector distances; + std::vector distances; for (int k = 0; k < intersections.first.size(); ++k) { // From robot position to intersection points @@ -63,7 +68,7 @@ float InformationEstimates::calculateMutualInformation(karto::PointVectorDouble } // Measurement outcomes vector {Pfree, Pocc, Pun} - std::vector probabilities { + std::vector probabilities { calculateScanMassProbabilityBetween(distances[1], 5.0f), calculateScanMassProbabilityBetween(distances[0], distances[1]), calculateScanMassProbabilityBetween(0.0f, distances[0]) @@ -73,12 +78,12 @@ float InformationEstimates::calculateMutualInformation(karto::PointVectorDouble appendCellProbabilities(probabilities, {cells_x[j], cells_y[j]}); // Get all the measurement outcomes for the current cell - std::vector> cell_prob = retreiveCellProbabilities({cells_x[j], cells_y[j]}); + std::vector> cell_prob = retreiveCellProbabilities({cells_x[j], cells_y[j]}); // Compute all the possible combinations for the current cell - algorithm 1 - std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(cell_prob); + std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(cell_prob); - double cell_mutual_inf = 0.0f; + kt_double cell_mutual_inf = 0.0f; for (auto& pair : meas_out_prob) { cell_mutual_inf += pair.second * measurementOutcomeEntropy(pair.first); @@ -86,13 +91,9 @@ float InformationEstimates::calculateMutualInformation(karto::PointVectorDouble // Mutual information of cell x, y given a set of measurements updateCellMutualInformation(0.5 - cell_mutual_inf, {cells_x[j], cells_y[j]}); - std::cout << "++++++++++++++++++++++++" << std::endl; } } - double mutual = calculateMapMutualInformation(); - std::cout << "Mutual information: " << mutual << std::endl; - - return 0.5f; + return calculateMapMutualInformation(); } void InformationEstimates::clearVisitedCells() @@ -100,8 +101,6 @@ void InformationEstimates::clearVisitedCells() /* To clear the visited cell */ - - std::cout << "Clearing cells " << std::endl; for (int i = 0; i < visited_cells.size(); ++i) { for (int j = 0; j < visited_cells[0].size(); ++j) @@ -111,32 +110,27 @@ void InformationEstimates::clearVisitedCells() } } -void InformationEstimates::appendCellProbabilities(std::vector& measurements, std::vector cell) +void InformationEstimates::appendCellProbabilities(std::vector& measurements, std::vector cell) { /* To append a new measurement for a specific cell */ - std::map, std::vector>>::iterator it_cell; + std::map, std::vector>>::iterator it_cell; it_cell = m_cell_probabilities.find({cell[0], cell[1]}); - 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[0], cell[1]}, - {{measurements[0], measurements[1], measurements[2]}} - )); + m_cell_probabilities.insert(std::pair, std::vector>>( + {cell[0], cell[1]}, {{measurements[0], measurements[1], measurements[2]}})); visited_cells[cell[0]][cell[1]] = true; } else { if(visited_cells[cell[0]][cell[1]] == true) { - /* - Compare the unknown probability, the smallest it is the most information we will have - from the occupied or free state - */ + // Compare the unknown probability, the smallest it is the most information we will have + // from the occupied or free state int idx = it_cell->second.size() - 1; if(measurements[2] < it_cell->second[idx][2]) { @@ -181,10 +175,7 @@ std::pair, std::vector> InformationEstimates::rayCasting(k delta_y = temp; interchange = true; } - else - { - interchange = false; - } + else { interchange = false; } int a_res = 2 * delta_y; int b_res = 2 * (delta_y - delta_x); @@ -197,14 +188,8 @@ std::pair, std::vector> InformationEstimates::rayCasting(k { if (e_res < 0) { - if (interchange) - { - y += s_y; - } - else - { - x += s_x; - } + if (interchange) { y += s_y; } + else { x += s_x; } e_res += a_res; } else @@ -227,7 +212,7 @@ std::pair, std::vector> InformationEstimates::rayCasting(k return std::pair, std::vector>{x_bres, y_bres}; } -double InformationEstimates::calculateInformationContent(double prob) +kt_double InformationEstimates::calculateInformationContent(kt_double prob) { /* To calculate the information content or self-information @@ -236,9 +221,9 @@ double InformationEstimates::calculateInformationContent(double prob) return - (prob * log2(prob)) - ((1 - prob) * log2(1 - prob)); } -std::vector InformationEstimates::calculateCellIntersectionPoints( +std::vector InformationEstimates::calculateCellIntersectionPoints( karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, - std::vector cell_start, std::vector cell_end) + std::vector cell_start, std::vector cell_end) { /* Initial point laser beam: laser_start @@ -246,21 +231,17 @@ std::vector InformationEstimates::calculateCellIntersectionPoints( Initial point cell: cell_start Final point cell: cell_end */ - // double x1 = laser_start[0]; - // double x2 = laser_end[0]; - double x1 = laser_start.GetX(); - double x2 = laser_end.GetX(); - double x3 = cell_start[0]; - double x4 = cell_end[0]; - - // double y1 = laser_start[1]; - // double y2 = laser_end[1]; - double y1 = laser_start.GetY(); - double y2 = laser_end.GetY(); - double y3 = cell_start[1]; - double y4 = cell_end[1]; - - double den = ((x2-x1)*(y4-y3) - (x4-x3)*(y2-y1)); + kt_double x1 = laser_start.GetX(); + kt_double x2 = laser_end.GetX(); + kt_double x3 = cell_start[0]; + kt_double x4 = cell_end[0]; + + kt_double y1 = laser_start.GetY(); + kt_double y2 = laser_end.GetY(); + kt_double y3 = cell_start[1]; + kt_double y4 = cell_end[1]; + + kt_double den = ((x2-x1)*(y4-y3) - (x4-x3)*(y2-y1)); if (den == 0.0f) { // Parallel lines or not intersection at all @@ -268,36 +249,36 @@ std::vector InformationEstimates::calculateCellIntersectionPoints( } else { - double x = ((x2*y1 - x1*y2)*(x4 - x3) - (x4*y3 - x3*y4)*(x2-x1)) / den; - double y = ((x2*y1 - x1*y2)*(y4 - y3) - (x4*y3 - x3*y4)*(y2-y1)) / den; + kt_double x = ((x2*y1 - x1*y2)*(x4 - x3) - (x4*y3 - x3*y4)*(x2 - x1)) / den; + kt_double y = ((x2*y1 - x1*y2)*(y4 - y3) - (x4*y3 - x3*y4)*(y2 - y1)) / den; return {x, y}; } } -std::pair, std::vector> InformationEstimates::computeLineBoxIntersection( +std::pair, std::vector> InformationEstimates::computeLineBoxIntersection( karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, - double limit_x, double limit_y) + kt_double limit_x, kt_double limit_y) { // Cell limits: min_x, max_x, min_y, max_y - std::vector cell_limits {limit_x, limit_x + m_cell_resol, limit_y, limit_y + m_cell_resol}; + std::vector cell_limits {limit_x, limit_x + m_cell_resol, limit_y, limit_y + m_cell_resol}; // Initial points for each of the 4 corners - std::vector initial_x {limit_x, limit_x, limit_x + m_cell_resol, limit_x + m_cell_resol}; - std::vector initial_y {limit_y, limit_y, limit_y + m_cell_resol, limit_y + m_cell_resol}; + std::vector initial_x {limit_x, limit_x, limit_x + m_cell_resol, limit_x + m_cell_resol}; + std::vector initial_y {limit_y, limit_y, limit_y + m_cell_resol, limit_y + m_cell_resol}; // Final points for each of the 4 corners - std::vector final_x {limit_x + m_cell_resol, limit_x, limit_x + m_cell_resol, limit_x}; - std::vector final_y {limit_y, limit_y + m_cell_resol, limit_y, limit_y + m_cell_resol}; + std::vector final_x {limit_x + m_cell_resol, limit_x, limit_x + m_cell_resol, limit_x}; + std::vector final_y {limit_y, limit_y + m_cell_resol, limit_y, limit_y + m_cell_resol}; // Set the new cell limits updateCellLimits(initial_x, initial_y, final_x, final_y, limit_x, limit_y, cell_limits, robot_grid_pos, final_grid_pos); - std::vector inter_x, inter_y; + std::vector inter_x, inter_y; for (int k = 0; k < 4; ++k) { - std::vector intersection = calculateCellIntersectionPoints(laser_start, laser_end, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); + std::vector intersection = calculateCellIntersectionPoints(laser_start, laser_end, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); if(intersection.size() != 0) { if ((fabs(intersection[0]) >= (fabs(cell_limits[0]) - 0.001)) && @@ -315,16 +296,12 @@ std::pair, std::vector> InformationEstimates::comput } } } - return std::pair, std::vector>{inter_x, inter_y}; + return std::pair, std::vector>{inter_x, inter_y}; } -// void InformationEstimates::updateCellLimits( -// std::vector& initial_x, std::vector& initial_y, std::vector& final_x, std::vector& final_y, -// double limit_x, double limit_y, std::vector& cell_limits, std::vector const& robot_grid_pos, std::vector const& final_grid_pos) -// { void InformationEstimates::updateCellLimits( - std::vector& initial_x, std::vector& initial_y, std::vector& final_x, std::vector& final_y, - double limit_x, double limit_y, std::vector& cell_limits, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos) + std::vector& initial_x, std::vector& initial_y, std::vector& final_x, std::vector& final_y, + kt_double limit_x, kt_double limit_y, std::vector& cell_limits, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos) { /* To calculate grid grid limits for intersection @@ -372,7 +349,7 @@ void InformationEstimates::updateCellLimits( } } -double InformationEstimates::calculateLogOddsFromProbability(double probability) +kt_double InformationEstimates::calculateLogOddsFromProbability(kt_double probability) { /* To calculate the log-odds @@ -381,13 +358,13 @@ double InformationEstimates::calculateLogOddsFromProbability(double probability) return log(probability / (1 - probability)); } -double InformationEstimates::calculateMapMutualInformation() +kt_double InformationEstimates::calculateMapMutualInformation() { /* To calculate map mutual information, this is the summation of all cells mutual information */ - double sum = 0.0f; + kt_double sum = 0.0f; for (int i = 0; i < m_num_cells; ++i) { for (int j = 0; j < m_num_cells; ++j) @@ -398,7 +375,7 @@ double InformationEstimates::calculateMapMutualInformation() return sum; } -double InformationEstimates::calculateProbabilityFromLogOdds(double log) +kt_double InformationEstimates::calculateProbabilityFromLogOdds(kt_double log) { /* To transform the Log-odds into probability @@ -407,20 +384,20 @@ double InformationEstimates::calculateProbabilityFromLogOdds(double log) return (exp(log) / (1 + exp(log))); } -std::unordered_map InformationEstimates::computeMeasurementOutcomesHistogram(std::vector>& meas_outcm) +std::unordered_map InformationEstimates::computeMeasurementOutcomesHistogram(std::vector>& meas_outcm) { /* To compute all the possible combinations of a grid cell, given a set of measurement outcomes */ - std::unordered_map temp_map; + std::unordered_map temp_map; // The number of measurements int k = meas_outcm.size(); int r = 1; - double p_free = meas_outcm[0][0]; - double p_occ = meas_outcm[0][1]; - double p_un = meas_outcm[0][2]; + kt_double p_free = meas_outcm[0][0]; + kt_double p_occ = meas_outcm[0][1]; + kt_double p_un = meas_outcm[0][2]; temp_map.clear(); @@ -435,12 +412,12 @@ std::unordered_map tup_vct; - std::vector acc_prob; + std::vector acc_prob; // Measurement outcome probability - double free_prop = meas_outcm[r][0]; - double occ_prop = meas_outcm[r][1]; - double un_prop = meas_outcm[r][2]; + kt_double free_prop = meas_outcm[r][0]; + kt_double occ_prop = meas_outcm[r][1]; + kt_double un_prop = meas_outcm[r][2]; for (auto& pair : temp_map) { @@ -491,6 +468,7 @@ std::unordered_map out_map; + std::unordered_map out_map; for (auto& pair : temp_map) { - if (std::get<0>(pair.first) + std::get<1>(pair.first) + std::get<2>(pair.first) == k) + int idx_free, idx_occ, idx_unk; + std::tie(idx_free, idx_occ, idx_unk) = pair.first; + if (idx_free + idx_occ + unk == k) { out_map[pair.first] = pair.second; } @@ -511,17 +491,6 @@ std::unordered_map) @@ -571,12 +540,12 @@ double InformationEstimates::measurementOutcomeEntropy(map_tuple const& meas_out int num_free, num_occ, num_unk; std::tie(num_free, num_occ, num_unk) = meas_outcome; - double log_occ = (num_free * l_free) + (num_occ * l_occ) - ((num_free + num_occ - 1) * l_o); - double prob_occ = calculateProbabilityFromLogOdds(log_occ); + 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); } -double InformationEstimates::calculateScanMassProbabilityBetween(double range_1, double range_2) +kt_double InformationEstimates::calculateScanMassProbabilityBetween(kt_double range_1, kt_double range_2) { /* To calculate the mass probability of a cell being observed by a given measurement @@ -588,18 +557,18 @@ double InformationEstimates::calculateScanMassProbabilityBetween(double range_1, return m_obs_nu * (exp(-m_obs_lambda*range_1) - exp(-m_obs_lambda*range_2)); } -std::vector> InformationEstimates::retreiveCellProbabilities(std::vector cell) +std::vector> InformationEstimates::retreiveCellProbabilities(std::vector cell) { /* To get all the cell probabilities */ - std::map, std::vector>>::iterator it_cells; + std::map, std::vector>>::iterator it_cells; it_cells = m_cell_probabilities.find({cell[0], cell[1]}); return it_cells->second; } -void InformationEstimates::updateCellMutualInformation(double mut_inf, std::vector cell) +void InformationEstimates::updateCellMutualInformation(kt_double mut_inf, std::vector cell) { /* To update the mutual information for each individual cell @@ -608,27 +577,27 @@ void InformationEstimates::updateCellMutualInformation(double mut_inf, std::vect m_mutual_grid[cell[0]][cell[1]] = mut_inf; } -void InformationEstimates::setMaxSensorRange(double const sensor_range) +void InformationEstimates::setMaxSensorRange(kt_double const sensor_range) { m_max_sensor_range = sensor_range; } -void InformationEstimates::setObservationLambda(double const lambda) +void InformationEstimates::setObservationLambda(kt_double const lambda) { m_obs_lambda = lambda; } -void InformationEstimates::setObservationNu(double const nu) +void InformationEstimates::setObservationNu(kt_double const nu) { m_obs_nu = nu; } -void InformationEstimates::setCellResolution(double const resolution) +void InformationEstimates::setCellResolution(kt_double const resolution) { m_cell_resol = resolution; } -void InformationEstimates::setMapDistance(double const distance) +void InformationEstimates::setMapDistance(kt_double const distance) { m_map_dist = distance; } From 542b239a8aebd030da029474e0e74b0ca9493cf4 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Tue, 28 Dec 2021 11:49:09 -0500 Subject: [PATCH 40/83] [WIP]: Functions modification. At this point we now have an utils file (Not added yet) --- .../experimental/information_estimates.hpp | 7 +++-- src/experimental/information_estimates.cpp | 29 ++++++++----------- 2 files changed, 16 insertions(+), 20 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index 7c67af080..e315a44fe 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -10,6 +10,8 @@ #include #include "lib/karto_sdk/include/karto_sdk/Karto.h" +// #include "utils.hpp" + #include "slam_toolbox/slam_toolbox_common.hpp" #include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp" @@ -54,6 +56,7 @@ class InformationEstimates karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, kt_double limit_x, kt_double limit_y); int signum(int num); + void clearVisitedCells(); // Measurements calculations kt_double calculateScanMassProbabilityBetween(kt_double range_1, kt_double range_2); @@ -72,14 +75,13 @@ class InformationEstimates std::unordered_map computeMeasurementOutcomesHistogram(std::vector>& meas_outcm); std::vector> retreiveCellProbabilities(std::vector cell); - void clearVisitedCells(); private: // Data structures std::unordered_map m_map_out; std::map, std::vector>> m_cell_probabilities; std::vector> m_mutual_grid; - std::vector> m_grid; + std::vector> m_visited_grid; kt_double m_map_dist; kt_double m_cell_resol; @@ -89,7 +91,6 @@ class InformationEstimates const kt_double l_occ = log(0.7 / (1.0 - 0.7)); const kt_double l_o = log(0.5 / (1.0 - 0.5)); - std::vector> visited_cells; kt_double m_max_sensor_range = 5.0; kt_double m_obs_lambda = 0.35; diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index e00ed72b9..0f865f1b9 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -7,18 +7,17 @@ InformationEstimates::InformationEstimates() /* Need to add the new elements for the constructor */ + // Here we can modify the dimensions, so we can handle not only square grids m_cell_resol = 0.05f; // Map resolution m_map_dist = 200.0f; // Total map distance m_num_cells = static_cast(m_map_dist / m_cell_resol); // Grids initialization (Occupancy and Mutual information) - m_grid.resize(m_num_cells); m_mutual_grid.resize(m_num_cells); - visited_cells.resize(m_num_cells); + m_visited_grid.resize(m_num_cells); initializeGrids(); - // Test karto::Vector2 point_2{3.0, 2.0}; karto::Vector2 point_1{0.0, 0.0}; @@ -99,13 +98,13 @@ float InformationEstimates::calculateMutualInformation(karto::PointVectorDouble void InformationEstimates::clearVisitedCells() { /* - To clear the visited cell + To clear the visited cells */ - for (int i = 0; i < visited_cells.size(); ++i) + for (int i = 0; i < m_visited_grid.size(); ++i) { - for (int j = 0; j < visited_cells[0].size(); ++j) + for (int j = 0; j < m_visited_grid[0].size(); ++j) { - visited_cells[i][j] = false; + m_visited_grid[i][j] = false; } } } @@ -123,11 +122,11 @@ void InformationEstimates::appendCellProbabilities(std::vector& measu // Cell is not present in the map, so append it m_cell_probabilities.insert(std::pair, std::vector>>( {cell[0], cell[1]}, {{measurements[0], measurements[1], measurements[2]}})); - visited_cells[cell[0]][cell[1]] = true; + m_visited_grid[cell[0]][cell[1]] = true; } else { - if(visited_cells[cell[0]][cell[1]] == true) + if(m_visited_grid[cell[0]][cell[1]] == true) { // Compare the unknown probability, the smallest it is the most information we will have // from the occupied or free state @@ -144,7 +143,7 @@ void InformationEstimates::appendCellProbabilities(std::vector& measu { // Cell is already in the map, only add the next measurement outcome it_cell->second.push_back({measurements[0], measurements[1], measurements[2]}); - visited_cells[cell[0]][cell[1]] = true; + m_visited_grid[cell[0]][cell[1]] = true; } } } @@ -468,7 +467,6 @@ std::unordered_map Date: Tue, 28 Dec 2021 12:02:39 -0500 Subject: [PATCH 41/83] [ADD]: Adding utils namespace --- CMakeLists.txt | 2 +- .../experimental/information_estimates.hpp | 2 +- include/slam_toolbox/experimental/utils.hpp | 39 +++ src/experimental/utils.cpp | 228 ++++++++++++++++++ 4 files changed, 269 insertions(+), 2 deletions(-) create mode 100644 include/slam_toolbox/experimental/utils.hpp create mode 100644 src/experimental/utils.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 0a194988a..8fd62e570 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -162,7 +162,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 src/experimental/information_estimates.cpp) +add_executable(lifelong_slam_toolbox_node src/experimental/slam_toolbox_lifelong_node.cpp src/experimental/information_estimates.cpp src/experimental/utils.cpp) target_link_libraries(lifelong_slam_toolbox_node lifelong_slam_toolbox) #### Merging maps tool diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index e315a44fe..17a176381 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -10,7 +10,7 @@ #include #include "lib/karto_sdk/include/karto_sdk/Karto.h" -// #include "utils.hpp" +#include "utils.hpp" #include "slam_toolbox/slam_toolbox_common.hpp" #include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp" diff --git a/include/slam_toolbox/experimental/utils.hpp b/include/slam_toolbox/experimental/utils.hpp new file mode 100644 index 000000000..7466a137e --- /dev/null +++ b/include/slam_toolbox/experimental/utils.hpp @@ -0,0 +1,39 @@ +#ifndef SLAM_TOOLBOX__EXPERIMENTAL__UTILS_HPP_ +#define SLAM_TOOLBOX__EXPERIMENTAL__UTILS_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "lib/karto_sdk/include/karto_sdk/Karto.h" + +namespace utils +{ + namespace grid_operations + { + void updateCellLimits(std::vector& initial_x, std::vector& initial_y, std::vector& final_x, + std::vector& final_y, kt_double limit_x, kt_double limit_y, std::vector& cell_limits, + karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, kt_double resolution); + + int signum(int num); + std::pair, std::vector> rayCasting(karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt); + karto::Vector2 getGridPosition(karto::Vector2 const& pose, kt_double resolution); + + std::vector calculateCellIntersectionPoints(karto::Vector2 const & laser_start, + karto::Vector2 const & laser_end, std::vector cell_start, std::vector cell_end); + + std::pair, std::vector> computeLineBoxIntersection( + karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, + karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, + kt_double limit_x, kt_double limit_y, kt_double resolution); + + template + void initializeGrid(std::vector> & grid, int num_rows, int num_columns); + } // namespace grid_operations +} // namespace utils + +#endif diff --git a/src/experimental/utils.cpp b/src/experimental/utils.cpp new file mode 100644 index 000000000..3d585f491 --- /dev/null +++ b/src/experimental/utils.cpp @@ -0,0 +1,228 @@ +#include "slam_toolbox/experimental/utils.hpp" + +void utils::grid_operations::updateCellLimits(std::vector& initial_x, std::vector& initial_y, std::vector& final_x, + std::vector& final_y, kt_double limit_x, kt_double limit_y, std::vector& cell_limits, karto::Vector2 const& robot_grid_pos, + karto::Vector2 const& final_grid_pos, kt_double resolution) +{ + /* + To calculate grid grid limits for intersection + */ + if (final_grid_pos.GetX() < robot_grid_pos.GetX() && final_grid_pos.GetY() >= robot_grid_pos.GetY()) + { + // X greater and Y greater. WRO final points + final_x[0] = limit_x + resolution; + final_x[2] = limit_x + resolution; + + cell_limits[2] = limit_y; + cell_limits[3] = limit_y + resolution; + } + + if (final_grid_pos.GetX() >= robot_grid_pos.GetX() && final_grid_pos.GetY() < robot_grid_pos.GetY()) + { + // X greater and Y minor. WRO final points + initial_y[2] = limit_y - resolution; + initial_y[3] = limit_y - resolution; + + final_y[1] = limit_y - resolution; + final_y[3] = limit_y - resolution; + + cell_limits[2] = limit_y - resolution; + cell_limits[3] = limit_y; + } + + if (final_grid_pos.GetX() < robot_grid_pos.GetX() && final_grid_pos.GetY() < robot_grid_pos.GetY()) + { + // X minor and Y minor. WRO final points + initial_x[2] = limit_x - resolution; + initial_x[3] = limit_x - resolution; + initial_y[2] = limit_y - resolution; + initial_y[3] = limit_y - resolution; + + final_x[0] = limit_x - resolution; + final_x[2] = limit_x - resolution; + final_y[1] = limit_y - resolution; + final_y[3] = limit_y - resolution; + + cell_limits[0] = limit_x - resolution; + cell_limits[1] = limit_x; + cell_limits[2] = limit_y - resolution; + cell_limits[3] = limit_y; + } +} + +int utils::grid_operations::signum(int num) +{ + /* + To get the sign of an operation, used by Bresenham algorithm + */ + if (num < 0) return -1; + if (num >= 1) return 1; + return 0; +} + +std::pair, std::vector> utils::grid_operations::rayCasting( + karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt) +{ + /* + To find the set of cells hit by a laser beam + This is based on Bresenham algorithm + */ + std::vector x_bres; + std::vector y_bres; + + int x = initial_pt.GetX(); + int y = initial_pt.GetY(); + + int delta_x = abs(final_pt.GetX() - initial_pt.GetX()); + int delta_y = abs(final_pt.GetY() - initial_pt.GetY()); + + int s_x = signum(final_pt.GetX() - initial_pt.GetX()); + int s_y = signum(final_pt.GetY() - initial_pt.GetY()); + bool interchange = false; + + if (delta_y > delta_x) + { + int temp = delta_x; + delta_x = delta_y; + delta_y = temp; + interchange = true; + } + else { interchange = false; } + + int a_res = 2 * delta_y; + int b_res = 2 * (delta_y - delta_x); + int e_res = (2 * delta_y) - delta_x; + + x_bres.push_back(x); + y_bres.push_back(y); + + for (int i = 1; i < delta_x; ++i) + { + if (e_res < 0) + { + if (interchange) { y += s_y; } + else { x += s_x; } + e_res += a_res; + } + else + { + y += s_y; + x += s_x; + e_res += b_res; + } + x_bres.push_back(x); + y_bres.push_back(y); + } + // Delete the current robot cell + x_bres.erase(x_bres.begin()); + y_bres.erase(y_bres.begin()); + + // Adding last hit cell to the set + x_bres.push_back(final_pt.GetX()); + y_bres.push_back(final_pt.GetY()); + + return std::pair, std::vector>{x_bres, y_bres}; +} + + +karto::Vector2 utils::grid_operations::getGridPosition(karto::Vector2 const& pose, kt_double resolution) +{ + int x_cell = floor((pose.GetX() / resolution)); + int y_cell = floor((pose.GetY() / resolution)); + + return karto::Vector2{x_cell, y_cell}; +} + +std::vector utils::grid_operations::calculateCellIntersectionPoints(karto::Vector2 const & laser_start, + karto::Vector2 const & laser_end, std::vector cell_start, std::vector cell_end) +{ + /* + Initial point laser beam: laser_start + Final point laser beam: laser_end + Initial point cell: cell_start + Final point cell: cell_end + */ + kt_double x1 = laser_start.GetX(); + kt_double x2 = laser_end.GetX(); + kt_double x3 = cell_start[0]; + kt_double x4 = cell_end[0]; + + kt_double y1 = laser_start.GetY(); + kt_double y2 = laser_end.GetY(); + kt_double y3 = cell_start[1]; + kt_double y4 = cell_end[1]; + + kt_double den = ((x2 - x1)*(y4 - y3) - (x4 - x3)*(y2 - y1)); + if (den == 0.0f) + { + // Parallel lines or not intersection at all + return {}; + } + else + { + kt_double x = ((x2*y1 - x1*y2)*(x4 - x3) - (x4*y3 - x3*y4)*(x2 - x1)) / den; + kt_double y = ((x2*y1 - x1*y2)*(y4 - y3) - (x4*y3 - x3*y4)*(y2 - y1)) / den; + return {x, y}; + } +} + +std::pair, std::vector> utils::grid_operations::computeLineBoxIntersection( + karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, + karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, + kt_double limit_x, kt_double limit_y, kt_double resolution) +{ + // Cell limits: min_x, max_x, min_y, max_y + std::vector cell_limits {limit_x, limit_x + resolution, limit_y, limit_y + resolution}; + + // Initial points for each of the 4 corners + std::vector initial_x {limit_x, limit_x, limit_x + resolution, limit_x + resolution}; + std::vector initial_y {limit_y, limit_y, limit_y + resolution, limit_y + resolution}; + + // Final points for each of the 4 corners + std::vector final_x {limit_x + resolution, limit_x, limit_x + resolution, limit_x}; + std::vector final_y {limit_y, limit_y + resolution, limit_y, limit_y + resolution}; + + // Set the new cell limits + updateCellLimits(initial_x, initial_y, final_x, final_y, limit_x, limit_y, cell_limits, robot_grid_pos, final_grid_pos, resolution); + + std::vector inter_x, inter_y; + + for (int k = 0; k < 4; ++k) + { + std::vector intersection = calculateCellIntersectionPoints(laser_start, laser_end, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); + if(intersection.size() != 0) + { + if ((fabs(intersection[0]) >= (fabs(cell_limits[0]) - 0.001)) && + (fabs(intersection[0]) <= (fabs(cell_limits[1]) + 0.001)) && + (fabs(intersection[1]) >= (fabs(cell_limits[2]) - 0.001)) && + (fabs(intersection[1]) <= (fabs(cell_limits[3]) + 0.001))) + { + /* + 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[0]); + inter_y.push_back(intersection[1]); + } + } + } + return std::pair, std::vector>{inter_x, inter_y}; +} + +template +void utils::grid_operations::initializeGrid(std::vector> & grid, int num_rows, int num_columns) +{ + /* + To create the grid + */ + for (int i = 0; i < num_rows; ++i) + { + // Adding columns + grid[i].resize(num_columns); + for (int j = 0; j < num_columns; ++j) + { + grid[i][j] = static_cast(0); + } + } +} From e5ac4e53386222c6c316dd6a00409df8d5bbe01b Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Wed, 29 Dec 2021 09:39:02 -0500 Subject: [PATCH 42/83] [INT]: Utils integration --- .../experimental/information_estimates.hpp | 72 +--- include/slam_toolbox/experimental/utils.hpp | 47 +- src/experimental/information_estimates.cpp | 314 ++------------ src/experimental/utils.cpp | 405 +++++++++--------- 4 files changed, 308 insertions(+), 530 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index 17a176381..ab4923ca9 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -1,84 +1,33 @@ #ifndef SLAM_TOOLBOX__EXPERIMENTAL__THEORETIC_INFORMATION_HPP_ #define SLAM_TOOLBOX__EXPERIMENTAL__THEORETIC_INFORMATION_HPP_ -#include -#include -#include -#include -#include -#include -#include -#include "lib/karto_sdk/include/karto_sdk/Karto.h" - #include "utils.hpp" -#include "slam_toolbox/slam_toolbox_common.hpp" -#include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp" - class InformationEstimates { typedef std::tuple map_tuple; - typedef std::pair map_pair; public: InformationEstimates(); // Empty contructor for now virtual ~InformationEstimates() {} -public: - struct HashTuple - { - std::size_t operator() (map_tuple const& key) const - { - 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; - } - }; - -public: - float calculateMutualInformation(karto::PointVectorDouble const& laser_readings, karto::Pose2 const& karto_pose); - private: - // Grid operations - void initializeGrids(); - void updateCellLimits( - std::vector& initial_x, std::vector& initial_y, std::vector& final_x, std::vector& final_y, - kt_double limit_x, kt_double limit_y, std::vector& cell_limits, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos); - - // Grid and position information - std::pair, std::vector> rayCasting(karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt); - karto::Vector2 getGridPosition(karto::Vector2 const& pose); - std::vector calculateCellIntersectionPoints(karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, std::vector cell_start, std::vector cell_end); - std::pair, std::vector> computeLineBoxIntersection( - karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, - karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, - kt_double limit_x, kt_double limit_y); - int signum(int num); - void clearVisitedCells(); - - // Measurements calculations - kt_double calculateScanMassProbabilityBetween(kt_double range_1, kt_double range_2); - // Mutual information kt_double calculateInformationContent(kt_double prob); - kt_double calculateLogOddsFromProbability(kt_double probability); kt_double calculateMapMutualInformation(); kt_double measurementOutcomeEntropy(map_tuple const& meas_outcome); kt_double calculateProbabilityFromLogOdds(kt_double log); - void recoverProbability(); void updateCellMutualInformation(kt_double mut_inf, std::vector cell); - // Measurement outcomes probabilities void appendCellProbabilities(std::vector& measurements, std::vector cell); - std::unordered_map computeMeasurementOutcomesHistogram(std::vector>& meas_outcm); + std::unordered_map computeMeasurementOutcomesHistogram(std::vector>& meas_outcm); std::vector> retreiveCellProbabilities(std::vector cell); - + // Measurements calculations + kt_double calculateScanMassProbabilityBetween(kt_double range_1, kt_double range_2); + void updateLaserMutualInformation(); private: // Data structures - std::unordered_map m_map_out; std::map, std::vector>> m_cell_probabilities; std::vector> m_mutual_grid; std::vector> m_visited_grid; @@ -91,18 +40,29 @@ class InformationEstimates 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 = 5.0; kt_double m_obs_lambda = 0.35; kt_double m_obs_nu = 0.28; + kt_double m_map_mutual_info; + kt_double m_laser_mutual_info; + public: + // Main function + float calculateMutualInformation(karto::PointVectorDouble const& laser_readings, karto::Pose2 const& karto_pose); + // Setters void setMaxSensorRange(kt_double const sensor_range); void setObservationLambda(kt_double const lambda); void setObservationNu(kt_double const nu); void setCellResolution(kt_double const resolution); void setMapDistance(kt_double const distance); + + + // Getters + kt_double getMapMutualInformation(); + kt_double getLaserMutualInformation(); + }; #endif diff --git a/include/slam_toolbox/experimental/utils.hpp b/include/slam_toolbox/experimental/utils.hpp index 7466a137e..d2d4b3056 100644 --- a/include/slam_toolbox/experimental/utils.hpp +++ b/include/slam_toolbox/experimental/utils.hpp @@ -15,6 +15,7 @@ namespace utils { namespace grid_operations { + void test(); void updateCellLimits(std::vector& initial_x, std::vector& initial_y, std::vector& final_x, std::vector& final_y, kt_double limit_x, kt_double limit_y, std::vector& cell_limits, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, kt_double resolution); @@ -31,9 +32,53 @@ namespace utils karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, kt_double limit_x, kt_double limit_y, kt_double resolution); + + template + void clearVisitedCells(std::vector> & grid) + { + for (int i = 0; i < grid.size(); ++i) + { + for (int j = 0; j < grid[0].size(); ++j) + { + grid[i][j] = static_cast(0); + } + } + } + template - void initializeGrid(std::vector> & grid, int num_rows, int num_columns); + void initializeGrid(std::vector> & grid, int num_rows, int num_columns) + { + /* + To create the grid + */ + grid.resize(num_rows); + for (int i = 0; i < num_rows; ++i) + { + // Adding columns + grid[i].resize(num_columns); + for (int j = 0; j < num_columns; ++j) + { + grid[i][j] = static_cast(0); + } + } + } } // namespace grid_operations + + namespace tuple_hash + { + struct HashTuple + { + std::size_t operator() (std::tuple const& key) const + { + 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 index 0f865f1b9..5a47f3de7 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -1,46 +1,35 @@ -#include #include #include "slam_toolbox/experimental/information_estimates.hpp" InformationEstimates::InformationEstimates() { - /* - Need to add the new elements for the constructor - */ - // Here we can modify the dimensions, so we can handle not only square grids - + m_map_mutual_info = 0.0; + m_laser_mutual_info = 0.0; m_cell_resol = 0.05f; // Map resolution m_map_dist = 200.0f; // Total map distance m_num_cells = static_cast(m_map_dist / m_cell_resol); - // Grids initialization (Occupancy and Mutual information) - m_mutual_grid.resize(m_num_cells); - m_visited_grid.resize(m_num_cells); - initializeGrids(); - - // Test - karto::Vector2 point_2{3.0, 2.0}; - karto::Vector2 point_1{0.0, 0.0}; - kt_double distance = point_1.Distance(point_2); - - std::cout << distance << std::endl; + utils::grid_operations::initializeGrid(m_mutual_grid, m_num_cells, m_num_cells); + utils::grid_operations::initializeGrid(m_visited_grid, m_num_cells, m_num_cells); } float InformationEstimates::calculateMutualInformation(karto::PointVectorDouble const& laser_readings, karto::Pose2 const& robot_pose) { - karto::Vector2 robot_grid = getGridPosition(robot_pose.GetPosition()); + m_laser_mutual_info = m_laser_mutual_info; + karto::Vector2 robot_grid = utils::grid_operations::getGridPosition(robot_pose.GetPosition(), m_cell_resol); // Set as false the current boolean map - clearVisitedCells(); + utils::grid_operations::clearVisitedCells(m_visited_grid); for (int i = 0; i < laser_readings.size(); ++i) { // Laser final cell - karto::Vector2 beam_grid = getGridPosition(laser_readings[i]); + karto::Vector2 beam_grid = utils::grid_operations::getGridPosition(laser_readings[i], m_cell_resol); // Ray tracing for getting the visited cells std::vector cells_x, cells_y; - std::pair, std::vector> res_pair = rayCasting(robot_grid, beam_grid); + std::pair, std::vector> res_pair = utils::grid_operations::rayCasting(robot_grid, beam_grid); + cells_x = res_pair.first; cells_y = res_pair.second; @@ -51,7 +40,7 @@ float InformationEstimates::calculateMutualInformation(karto::PointVectorDouble kt_double limit_x = cells_x[j] * m_cell_resol; kt_double limit_y = cells_y[j] * m_cell_resol; - std::pair, std::vector> intersections = computeLineBoxIntersection(robot_pose.GetPosition(), laser_readings[i], robot_grid, beam_grid, limit_x, limit_y); + std::pair, std::vector> intersections = utils::grid_operations::computeLineBoxIntersection(robot_pose.GetPosition(), laser_readings[i], robot_grid, beam_grid, limit_x, limit_y, m_cell_resol); if (intersections.first.size() == 0) continue; @@ -80,7 +69,7 @@ float InformationEstimates::calculateMutualInformation(karto::PointVectorDouble std::vector> cell_prob = retreiveCellProbabilities({cells_x[j], cells_y[j]}); // Compute all the possible combinations for the current cell - algorithm 1 - std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(cell_prob); + std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(cell_prob); kt_double cell_mutual_inf = 0.0f; for (auto& pair : meas_out_prob) @@ -92,21 +81,11 @@ float InformationEstimates::calculateMutualInformation(karto::PointVectorDouble updateCellMutualInformation(0.5 - cell_mutual_inf, {cells_x[j], cells_y[j]}); } } - return calculateMapMutualInformation(); -} - -void InformationEstimates::clearVisitedCells() -{ - /* - To clear the visited cells - */ - for (int i = 0; i < m_visited_grid.size(); ++i) - { - for (int j = 0; j < m_visited_grid[0].size(); ++j) - { - m_visited_grid[i][j] = false; - } - } + m_map_mutual_info = calculateMapMutualInformation(); + + // Extract the mutual information provided by this laser scan + updateLaserMutualInformation(); + return m_map_mutual_info; } void InformationEstimates::appendCellProbabilities(std::vector& measurements, std::vector cell) @@ -148,69 +127,6 @@ void InformationEstimates::appendCellProbabilities(std::vector& measu } } -std::pair, std::vector> InformationEstimates::rayCasting(karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt) -{ - /* - To find the set of cells hit by a laser beam - This is based on Bresenham algorithm - */ - std::vector x_bres; - std::vector y_bres; - - int x = initial_pt.GetX(); - int y = initial_pt.GetY(); - - int delta_x = abs(final_pt.GetX() - initial_pt.GetX()); - int delta_y = abs(final_pt.GetY() - initial_pt.GetY()); - - int s_x = signum(final_pt.GetX() - initial_pt.GetX()); - int s_y = signum(final_pt.GetY() - initial_pt.GetY()); - bool interchange = false; - - if (delta_y > delta_x) - { - int temp = delta_x; - delta_x = delta_y; - delta_y = temp; - interchange = true; - } - else { interchange = false; } - - int a_res = 2 * delta_y; - int b_res = 2 * (delta_y - delta_x); - int e_res = (2 * delta_y) - delta_x; - - x_bres.push_back(x); - y_bres.push_back(y); - - for (int i = 1; i < delta_x; ++i) - { - if (e_res < 0) - { - if (interchange) { y += s_y; } - else { x += s_x; } - e_res += a_res; - } - else - { - y += s_y; - x += s_x; - e_res += b_res; - } - x_bres.push_back(x); - y_bres.push_back(y); - } - // Delete the current robot cell - x_bres.erase(x_bres.begin()); - y_bres.erase(y_bres.begin()); - - // Adding last hit cell to the set - x_bres.push_back(final_pt.GetX()); - y_bres.push_back(final_pt.GetY()); - - return std::pair, std::vector>{x_bres, y_bres}; -} - kt_double InformationEstimates::calculateInformationContent(kt_double prob) { /* @@ -220,143 +136,6 @@ kt_double InformationEstimates::calculateInformationContent(kt_double prob) return - (prob * log2(prob)) - ((1 - prob) * log2(1 - prob)); } -std::vector InformationEstimates::calculateCellIntersectionPoints( - karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, - std::vector cell_start, std::vector cell_end) -{ - /* - Initial point laser beam: laser_start - Final point laser beam: laser_end - Initial point cell: cell_start - Final point cell: cell_end - */ - kt_double x1 = laser_start.GetX(); - kt_double x2 = laser_end.GetX(); - kt_double x3 = cell_start[0]; - kt_double x4 = cell_end[0]; - - kt_double y1 = laser_start.GetY(); - kt_double y2 = laser_end.GetY(); - kt_double y3 = cell_start[1]; - kt_double y4 = cell_end[1]; - - kt_double den = ((x2-x1)*(y4-y3) - (x4-x3)*(y2-y1)); - if (den == 0.0f) - { - // Parallel lines or not intersection at all - return {}; - } - else - { - kt_double x = ((x2*y1 - x1*y2)*(x4 - x3) - (x4*y3 - x3*y4)*(x2 - x1)) / den; - kt_double y = ((x2*y1 - x1*y2)*(y4 - y3) - (x4*y3 - x3*y4)*(y2 - y1)) / den; - return {x, y}; - } -} - -std::pair, std::vector> InformationEstimates::computeLineBoxIntersection( - karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, - karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, - kt_double limit_x, kt_double limit_y) -{ - // Cell limits: min_x, max_x, min_y, max_y - std::vector cell_limits {limit_x, limit_x + m_cell_resol, limit_y, limit_y + m_cell_resol}; - - // Initial points for each of the 4 corners - std::vector initial_x {limit_x, limit_x, limit_x + m_cell_resol, limit_x + m_cell_resol}; - std::vector initial_y {limit_y, limit_y, limit_y + m_cell_resol, limit_y + m_cell_resol}; - - // Final points for each of the 4 corners - std::vector final_x {limit_x + m_cell_resol, limit_x, limit_x + m_cell_resol, limit_x}; - std::vector final_y {limit_y, limit_y + m_cell_resol, limit_y, limit_y + m_cell_resol}; - - // Set the new cell limits - updateCellLimits(initial_x, initial_y, final_x, final_y, limit_x, limit_y, cell_limits, robot_grid_pos, final_grid_pos); - - std::vector inter_x, inter_y; - - for (int k = 0; k < 4; ++k) - { - std::vector intersection = calculateCellIntersectionPoints(laser_start, laser_end, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); - if(intersection.size() != 0) - { - if ((fabs(intersection[0]) >= (fabs(cell_limits[0]) - 0.001)) && - (fabs(intersection[0]) <= (fabs(cell_limits[1]) + 0.001)) && - (fabs(intersection[1]) >= (fabs(cell_limits[2]) - 0.001)) && - (fabs(intersection[1]) <= (fabs(cell_limits[3]) + 0.001))) - { - /* - 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[0]); - inter_y.push_back(intersection[1]); - } - } - } - return std::pair, std::vector>{inter_x, inter_y}; -} - -void InformationEstimates::updateCellLimits( - std::vector& initial_x, std::vector& initial_y, std::vector& final_x, std::vector& final_y, - kt_double limit_x, kt_double limit_y, std::vector& cell_limits, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos) -{ - /* - To calculate grid grid limits for intersection - */ - if (final_grid_pos.GetX() < robot_grid_pos.GetX() && final_grid_pos.GetY() >= robot_grid_pos.GetY()) - { - // X greater and Y greater. WRO final points - final_x[0] = limit_x + m_cell_resol; - final_x[2] = limit_x + m_cell_resol; - - cell_limits[2] = limit_y; - cell_limits[3] = limit_y + m_cell_resol; - } - - if (final_grid_pos.GetX() >= robot_grid_pos.GetX() && final_grid_pos.GetY() < robot_grid_pos.GetY()) - { - // X greater and Y minor. WRO final points - initial_y[2] = limit_y - m_cell_resol; - initial_y[3] = limit_y - m_cell_resol; - - final_y[1] = limit_y - m_cell_resol; - final_y[3] = limit_y - m_cell_resol; - - cell_limits[2] = limit_y - m_cell_resol; - cell_limits[3] = limit_y; - } - - if (final_grid_pos.GetX() < robot_grid_pos.GetX() && final_grid_pos.GetY() < robot_grid_pos.GetY()) - { - // X minor and Y minor. WRO final points - initial_x[2] = limit_x - m_cell_resol; - initial_x[3] = limit_x - m_cell_resol; - initial_y[2] = limit_y - m_cell_resol; - initial_y[3] = limit_y - m_cell_resol; - - final_x[0] = limit_x - m_cell_resol; - final_x[2] = limit_x - m_cell_resol; - final_y[1] = limit_y - m_cell_resol; - final_y[3] = limit_y - m_cell_resol; - - cell_limits[0] = limit_x - m_cell_resol; - cell_limits[1] = limit_x; - cell_limits[2] = limit_y - m_cell_resol; - cell_limits[3] = limit_y; - } -} - -kt_double InformationEstimates::calculateLogOddsFromProbability(kt_double probability) -{ - /* - To calculate the log-odds - This should be a free function - */ - return log(probability / (1 - probability)); -} - kt_double InformationEstimates::calculateMapMutualInformation() { /* @@ -383,12 +162,12 @@ kt_double InformationEstimates::calculateProbabilityFromLogOdds(kt_double log) return (exp(log) / (1 + exp(log))); } -std::unordered_map InformationEstimates::computeMeasurementOutcomesHistogram(std::vector>& meas_outcm) +std::unordered_map InformationEstimates::computeMeasurementOutcomesHistogram(std::vector>& meas_outcm) { /* To compute all the possible combinations of a grid cell, given a set of measurement outcomes */ - std::unordered_map temp_map; + std::unordered_map temp_map; // The number of measurements int k = meas_outcm.size(); @@ -476,7 +255,7 @@ std::unordered_map out_map; + std::unordered_map out_map; for (auto& pair : temp_map) { int idx_free, idx_occ, idx_unk; @@ -489,42 +268,6 @@ std::unordered_map= 1) return 1; - return 0; -} - -karto::Vector2 InformationEstimates::getGridPosition(karto::Vector2 const& pose) -{ - int x_cell = floor((pose.GetX() / m_cell_resol)); - int y_cell = floor((pose.GetY() / m_cell_resol)); - - return karto::Vector2{x_cell, y_cell}; -} - -void InformationEstimates::initializeGrids() -{ - /* - To create the grid - */ - for (int i = 0; i < m_num_cells; ++i) - { - // Adding columns - m_mutual_grid[i].resize(m_num_cells); - m_visited_grid[i].resize(m_num_cells); - for (int j = 0; j < m_num_cells; ++j) - { - m_mutual_grid[i][j] = 0.0f; - m_visited_grid[i][j] = false; - } - } -} - kt_double InformationEstimates::measurementOutcomeEntropy(map_tuple const& meas_outcome) { /* @@ -563,6 +306,11 @@ std::vector> InformationEstimates::retreiveCellProbabilit return it_cells->second; } +void InformationEstimates::updateLaserMutualInformation() +{ + m_laser_mutual_info = fabs(m_laser_mutual_info - m_map_mutual_info); +} + void InformationEstimates::updateCellMutualInformation(kt_double mut_inf, std::vector cell) { /* @@ -596,3 +344,15 @@ void InformationEstimates::setMapDistance(kt_double const distance) { m_map_dist = distance; } + + +kt_double InformationEstimates::getMapMutualInformation() +{ + return m_map_mutual_info; +} + +kt_double InformationEstimates::getLaserMutualInformation() +{ + return m_laser_mutual_info; +} + diff --git a/src/experimental/utils.cpp b/src/experimental/utils.cpp index 3d585f491..b79e06a45 100644 --- a/src/experimental/utils.cpp +++ b/src/experimental/utils.cpp @@ -1,228 +1,241 @@ #include "slam_toolbox/experimental/utils.hpp" +#include -void utils::grid_operations::updateCellLimits(std::vector& initial_x, std::vector& initial_y, std::vector& final_x, - std::vector& final_y, kt_double limit_x, kt_double limit_y, std::vector& cell_limits, karto::Vector2 const& robot_grid_pos, - karto::Vector2 const& final_grid_pos, kt_double resolution) +namespace utils { - /* - To calculate grid grid limits for intersection - */ - if (final_grid_pos.GetX() < robot_grid_pos.GetX() && final_grid_pos.GetY() >= robot_grid_pos.GetY()) + namespace grid_operations { - // X greater and Y greater. WRO final points - final_x[0] = limit_x + resolution; - final_x[2] = limit_x + resolution; + void updateCellLimits(std::vector& initial_x, std::vector& initial_y, std::vector& final_x, + std::vector& final_y, kt_double limit_x, kt_double limit_y, std::vector& cell_limits, karto::Vector2 const& robot_grid_pos, + karto::Vector2 const& final_grid_pos, kt_double resolution) + { + /* + To calculate grid grid limits for intersection + */ + if (final_grid_pos.GetX() < robot_grid_pos.GetX() && final_grid_pos.GetY() >= robot_grid_pos.GetY()) + { + // X greater and Y greater. WRO final points + final_x[0] = limit_x + resolution; + final_x[2] = limit_x + resolution; - cell_limits[2] = limit_y; - cell_limits[3] = limit_y + resolution; - } + cell_limits[2] = limit_y; + cell_limits[3] = limit_y + resolution; + } - if (final_grid_pos.GetX() >= robot_grid_pos.GetX() && final_grid_pos.GetY() < robot_grid_pos.GetY()) - { - // X greater and Y minor. WRO final points - initial_y[2] = limit_y - resolution; - initial_y[3] = limit_y - resolution; + if (final_grid_pos.GetX() >= robot_grid_pos.GetX() && final_grid_pos.GetY() < robot_grid_pos.GetY()) + { + // X greater and Y minor. WRO final points + initial_y[2] = limit_y - resolution; + initial_y[3] = limit_y - resolution; - final_y[1] = limit_y - resolution; - final_y[3] = limit_y - resolution; + final_y[1] = limit_y - resolution; + final_y[3] = limit_y - resolution; - cell_limits[2] = limit_y - resolution; - cell_limits[3] = limit_y; - } + cell_limits[2] = limit_y - resolution; + cell_limits[3] = limit_y; + } - if (final_grid_pos.GetX() < robot_grid_pos.GetX() && final_grid_pos.GetY() < robot_grid_pos.GetY()) - { - // X minor and Y minor. WRO final points - initial_x[2] = limit_x - resolution; - initial_x[3] = limit_x - resolution; - initial_y[2] = limit_y - resolution; - initial_y[3] = limit_y - resolution; - - final_x[0] = limit_x - resolution; - final_x[2] = limit_x - resolution; - final_y[1] = limit_y - resolution; - final_y[3] = limit_y - resolution; - - cell_limits[0] = limit_x - resolution; - cell_limits[1] = limit_x; - cell_limits[2] = limit_y - resolution; - cell_limits[3] = limit_y; - } -} - -int utils::grid_operations::signum(int num) -{ - /* - To get the sign of an operation, used by Bresenham algorithm - */ - if (num < 0) return -1; - if (num >= 1) return 1; - return 0; -} - -std::pair, std::vector> utils::grid_operations::rayCasting( - karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt) -{ - /* - To find the set of cells hit by a laser beam - This is based on Bresenham algorithm - */ - std::vector x_bres; - std::vector y_bres; + if (final_grid_pos.GetX() < robot_grid_pos.GetX() && final_grid_pos.GetY() < robot_grid_pos.GetY()) + { + // X minor and Y minor. WRO final points + initial_x[2] = limit_x - resolution; + initial_x[3] = limit_x - resolution; + initial_y[2] = limit_y - resolution; + initial_y[3] = limit_y - resolution; + + final_x[0] = limit_x - resolution; + final_x[2] = limit_x - resolution; + final_y[1] = limit_y - resolution; + final_y[3] = limit_y - resolution; + + cell_limits[0] = limit_x - resolution; + cell_limits[1] = limit_x; + cell_limits[2] = limit_y - resolution; + cell_limits[3] = limit_y; + } + } - int x = initial_pt.GetX(); - int y = initial_pt.GetY(); + int signum(int num) + { + /* + To get the sign of an operation, used by Bresenham algorithm + */ + if (num < 0) return -1; + if (num >= 1) return 1; + return 0; + } - int delta_x = abs(final_pt.GetX() - initial_pt.GetX()); - int delta_y = abs(final_pt.GetY() - initial_pt.GetY()); + std::pair, std::vector> rayCasting( + karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt) + { + /* + To find the set of cells hit by a laser beam + This is based on Bresenham algorithm + */ + std::vector x_bres; + std::vector y_bres; - int s_x = signum(final_pt.GetX() - initial_pt.GetX()); - int s_y = signum(final_pt.GetY() - initial_pt.GetY()); - bool interchange = false; + int x = initial_pt.GetX(); + int y = initial_pt.GetY(); - if (delta_y > delta_x) - { - int temp = delta_x; - delta_x = delta_y; - delta_y = temp; - interchange = true; - } - else { interchange = false; } + int delta_x = abs(final_pt.GetX() - initial_pt.GetX()); + int delta_y = abs(final_pt.GetY() - initial_pt.GetY()); - int a_res = 2 * delta_y; - int b_res = 2 * (delta_y - delta_x); - int e_res = (2 * delta_y) - delta_x; + int s_x = signum(final_pt.GetX() - initial_pt.GetX()); + int s_y = signum(final_pt.GetY() - initial_pt.GetY()); + bool interchange = false; - x_bres.push_back(x); - y_bres.push_back(y); + if (delta_y > delta_x) + { + int temp = delta_x; + delta_x = delta_y; + delta_y = temp; + interchange = true; + } + else { interchange = false; } - for (int i = 1; i < delta_x; ++i) - { - if (e_res < 0) - { - if (interchange) { y += s_y; } - else { x += s_x; } - e_res += a_res; - } - else - { - y += s_y; - x += s_x; - e_res += b_res; + int a_res = 2 * delta_y; + int b_res = 2 * (delta_y - delta_x); + int e_res = (2 * delta_y) - delta_x; + + x_bres.push_back(x); + y_bres.push_back(y); + + for (int i = 1; i < delta_x; ++i) + { + if (e_res < 0) + { + if (interchange) { y += s_y; } + else { x += s_x; } + e_res += a_res; + } + else + { + y += s_y; + x += s_x; + e_res += b_res; + } + x_bres.push_back(x); + y_bres.push_back(y); + } + // Delete the current robot cell + x_bres.erase(x_bres.begin()); + y_bres.erase(y_bres.begin()); + + // Adding last hit cell to the set + x_bres.push_back(final_pt.GetX()); + y_bres.push_back(final_pt.GetY()); + + return std::pair, std::vector>{x_bres, y_bres}; } - x_bres.push_back(x); - y_bres.push_back(y); - } - // Delete the current robot cell - x_bres.erase(x_bres.begin()); - y_bres.erase(y_bres.begin()); - - // Adding last hit cell to the set - x_bres.push_back(final_pt.GetX()); - y_bres.push_back(final_pt.GetY()); - return std::pair, std::vector>{x_bres, y_bres}; -} + karto::Vector2 getGridPosition(karto::Vector2 const& pose, kt_double resolution) + { + int x_cell = floor((pose.GetX() / resolution)); + int y_cell = floor((pose.GetY() / resolution)); -karto::Vector2 utils::grid_operations::getGridPosition(karto::Vector2 const& pose, kt_double resolution) -{ - int x_cell = floor((pose.GetX() / resolution)); - int y_cell = floor((pose.GetY() / resolution)); + return karto::Vector2{x_cell, y_cell}; + } - return karto::Vector2{x_cell, y_cell}; -} + std::vector calculateCellIntersectionPoints(karto::Vector2 const & laser_start, + karto::Vector2 const & laser_end, std::vector cell_start, std::vector cell_end) + { + /* + Initial point laser beam: laser_start + Final point laser beam: laser_end + Initial point cell: cell_start + Final point cell: cell_end + */ + kt_double x1 = laser_start.GetX(); + kt_double x2 = laser_end.GetX(); + kt_double x3 = cell_start[0]; + kt_double x4 = cell_end[0]; + + kt_double y1 = laser_start.GetY(); + kt_double y2 = laser_end.GetY(); + kt_double y3 = cell_start[1]; + kt_double y4 = cell_end[1]; + + kt_double den = ((x2 - x1)*(y4 - y3) - (x4 - x3)*(y2 - y1)); + if (den == 0.0f) + { + // Parallel lines or not intersection at all + return {}; + } + else + { + kt_double x = ((x2*y1 - x1*y2)*(x4 - x3) - (x4*y3 - x3*y4)*(x2 - x1)) / den; + kt_double y = ((x2*y1 - x1*y2)*(y4 - y3) - (x4*y3 - x3*y4)*(y2 - y1)) / den; + return {x, y}; + } + } -std::vector utils::grid_operations::calculateCellIntersectionPoints(karto::Vector2 const & laser_start, - karto::Vector2 const & laser_end, std::vector cell_start, std::vector cell_end) -{ - /* - Initial point laser beam: laser_start - Final point laser beam: laser_end - Initial point cell: cell_start - Final point cell: cell_end - */ - kt_double x1 = laser_start.GetX(); - kt_double x2 = laser_end.GetX(); - kt_double x3 = cell_start[0]; - kt_double x4 = cell_end[0]; - - kt_double y1 = laser_start.GetY(); - kt_double y2 = laser_end.GetY(); - kt_double y3 = cell_start[1]; - kt_double y4 = cell_end[1]; - - kt_double den = ((x2 - x1)*(y4 - y3) - (x4 - x3)*(y2 - y1)); - if (den == 0.0f) - { - // Parallel lines or not intersection at all - return {}; - } - else - { - kt_double x = ((x2*y1 - x1*y2)*(x4 - x3) - (x4*y3 - x3*y4)*(x2 - x1)) / den; - kt_double y = ((x2*y1 - x1*y2)*(y4 - y3) - (x4*y3 - x3*y4)*(y2 - y1)) / den; - return {x, y}; - } -} - -std::pair, std::vector> utils::grid_operations::computeLineBoxIntersection( - karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, - karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, - kt_double limit_x, kt_double limit_y, kt_double resolution) -{ - // Cell limits: min_x, max_x, min_y, max_y - std::vector cell_limits {limit_x, limit_x + resolution, limit_y, limit_y + resolution}; + std::pair, std::vector> computeLineBoxIntersection( + karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, + karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, + kt_double limit_x, kt_double limit_y, kt_double resolution) + { + // Cell limits: min_x, max_x, min_y, max_y + std::vector cell_limits {limit_x, limit_x + resolution, limit_y, limit_y + resolution}; - // Initial points for each of the 4 corners - std::vector initial_x {limit_x, limit_x, limit_x + resolution, limit_x + resolution}; - std::vector initial_y {limit_y, limit_y, limit_y + resolution, limit_y + resolution}; - - // Final points for each of the 4 corners - std::vector final_x {limit_x + resolution, limit_x, limit_x + resolution, limit_x}; - std::vector final_y {limit_y, limit_y + resolution, limit_y, limit_y + resolution}; + // Initial points for each of the 4 corners + std::vector initial_x {limit_x, limit_x, limit_x + resolution, limit_x + resolution}; + std::vector initial_y {limit_y, limit_y, limit_y + resolution, limit_y + resolution}; + + // Final points for each of the 4 corners + std::vector final_x {limit_x + resolution, limit_x, limit_x + resolution, limit_x}; + std::vector final_y {limit_y, limit_y + resolution, limit_y, limit_y + resolution}; - // Set the new cell limits - updateCellLimits(initial_x, initial_y, final_x, final_y, limit_x, limit_y, cell_limits, robot_grid_pos, final_grid_pos, resolution); + // Set the new cell limits + updateCellLimits(initial_x, initial_y, final_x, final_y, limit_x, limit_y, cell_limits, robot_grid_pos, final_grid_pos, resolution); - std::vector inter_x, inter_y; + std::vector inter_x, inter_y; - for (int k = 0; k < 4; ++k) - { - std::vector intersection = calculateCellIntersectionPoints(laser_start, laser_end, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); - if(intersection.size() != 0) - { - if ((fabs(intersection[0]) >= (fabs(cell_limits[0]) - 0.001)) && - (fabs(intersection[0]) <= (fabs(cell_limits[1]) + 0.001)) && - (fabs(intersection[1]) >= (fabs(cell_limits[2]) - 0.001)) && - (fabs(intersection[1]) <= (fabs(cell_limits[3]) + 0.001))) + for (int k = 0; k < 4; ++k) { - /* - 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[0]); - inter_y.push_back(intersection[1]); + std::vector intersection = calculateCellIntersectionPoints(laser_start, laser_end, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); + if(intersection.size() != 0) + { + if ((fabs(intersection[0]) >= (fabs(cell_limits[0]) - 0.001)) && + (fabs(intersection[0]) <= (fabs(cell_limits[1]) + 0.001)) && + (fabs(intersection[1]) >= (fabs(cell_limits[2]) - 0.001)) && + (fabs(intersection[1]) <= (fabs(cell_limits[3]) + 0.001))) + { + /* + 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[0]); + inter_y.push_back(intersection[1]); + } + } } + return std::pair, std::vector>{inter_x, inter_y}; } - } - return std::pair, std::vector>{inter_x, inter_y}; -} -template -void utils::grid_operations::initializeGrid(std::vector> & grid, int num_rows, int num_columns) -{ - /* - To create the grid - */ - for (int i = 0; i < num_rows; ++i) - { - // Adding columns - grid[i].resize(num_columns); - for (int j = 0; j < num_columns; ++j) + // template + // void initializeGrid(std::vector> & grid, int num_rows, int num_columns) + // { + // /* + // To create the grid + // */ + // std::cout << "Grid Initializaion -----------------" << std::endl; + // for (int i = 0; i < num_rows; ++i) + // { + // // Adding columns + // grid[i].resize(num_columns); + // for (int j = 0; j < num_columns; ++j) + // { + // grid[i][j] = static_cast(0); + // } + // } + // } + + void test() { - grid[i][j] = static_cast(0); + std::cout << "Test" << std::endl; } - } -} + }// namespace grid_operations +} // namespace utils \ No newline at end of file From 6869573624700887cc732c7b4efa477ae4593132 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Wed, 29 Dec 2021 12:21:56 -0500 Subject: [PATCH 43/83] [WIP]: Changing main function arguments --- .../experimental/information_estimates.hpp | 3 +- include/slam_toolbox/experimental/utils.hpp | 1 - src/experimental/information_estimates.cpp | 130 ++++++++++-------- src/experimental/utils.cpp | 22 --- 4 files changed, 78 insertions(+), 78 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index ab4923ca9..95afbde66 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -49,7 +49,8 @@ class InformationEstimates public: // Main function - float calculateMutualInformation(karto::PointVectorDouble const& laser_readings, karto::Pose2 const& karto_pose); + // float calculateMutualInformation(karto::PointVectorDouble const& laser_readings, karto::Pose2 const& karto_pose); + float calculateMutualInformation(std::vector const& range_scans); // Setters void setMaxSensorRange(kt_double const sensor_range); diff --git a/include/slam_toolbox/experimental/utils.hpp b/include/slam_toolbox/experimental/utils.hpp index d2d4b3056..c9577c104 100644 --- a/include/slam_toolbox/experimental/utils.hpp +++ b/include/slam_toolbox/experimental/utils.hpp @@ -15,7 +15,6 @@ namespace utils { namespace grid_operations { - void test(); void updateCellLimits(std::vector& initial_x, std::vector& initial_y, std::vector& final_x, std::vector& final_y, kt_double limit_x, kt_double limit_y, std::vector& cell_limits, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, kt_double resolution); diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 5a47f3de7..190176807 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -13,78 +13,100 @@ InformationEstimates::InformationEstimates() utils::grid_operations::initializeGrid(m_visited_grid, m_num_cells, m_num_cells); } -float InformationEstimates::calculateMutualInformation(karto::PointVectorDouble const& laser_readings, karto::Pose2 const& robot_pose) +float InformationEstimates::calculateMutualInformation(std::vector const& range_scans) { - m_laser_mutual_info = m_laser_mutual_info; - karto::Vector2 robot_grid = utils::grid_operations::getGridPosition(robot_pose.GetPosition(), m_cell_resol); - - // Set as false the current boolean map - utils::grid_operations::clearVisitedCells(m_visited_grid); + /* + I should receive a vector of LocalizedRangeScan, + For this I have two options, + Either the vector is made upd outside this scope + Or I have a function for appending the range scans and the call this function for processing + */ - for (int i = 0; i < laser_readings.size(); ++i) + for (auto & scan : range_scans) { - // Laser final cell - karto::Vector2 beam_grid = utils::grid_operations::getGridPosition(laser_readings[i], m_cell_resol); + karto::Pose2 robot_pose = scan.GetOdometricPose(); + karto::PointVectorDouble laser_readings = scan.GetPointReadings(true); - // Ray tracing for getting the visited cells - std::vector cells_x, cells_y; - std::pair, std::vector> res_pair = utils::grid_operations::rayCasting(robot_grid, beam_grid); + m_laser_mutual_info = m_laser_mutual_info; + karto::Vector2 robot_grid = utils::grid_operations::getGridPosition(robot_pose.GetPosition(), m_cell_resol); - cells_x = res_pair.first; - cells_y = res_pair.second; + // Set as false the current boolean map + utils::grid_operations::clearVisitedCells(m_visited_grid); - // Visiting the cells - for (int j = 0; j < cells_x.size(); ++j) + for (int i = 0; i < laser_readings.size(); ++i) { - // Inidividual cell limits - kt_double limit_x = cells_x[j] * m_cell_resol; - kt_double limit_y = cells_y[j] * m_cell_resol; + // Laser final cell + karto::Vector2 beam_grid = utils::grid_operations::getGridPosition(laser_readings[i], m_cell_resol); - std::pair, std::vector> intersections = utils::grid_operations::computeLineBoxIntersection(robot_pose.GetPosition(), laser_readings[i], robot_grid, beam_grid, limit_x, limit_y, m_cell_resol); + // Ray tracing for getting the visited cells + std::vector cells_x, cells_y; + std::pair, std::vector> res_pair = utils::grid_operations::rayCasting(robot_grid, beam_grid); - if (intersections.first.size() == 0) - continue; + cells_x = res_pair.first; + cells_y = res_pair.second; - // Enter (d1) and Exit (d2) distances - std::vector distances; - for (int k = 0; k < intersections.first.size(); ++k) + // Visiting the cells + for (int j = 0; j < cells_x.size(); ++j) { - // From robot position to intersection points - karto::Vector2 intersection{intersections.first[k], intersections.second[k]}; - kt_double distance = robot_pose.GetPosition().Distance(intersection); - distances.push_back(distance); - } + // Inidividual cell limits + kt_double limit_x = cells_x[j] * m_cell_resol; + kt_double limit_y = cells_y[j] * m_cell_resol; + + std::pair, std::vector> intersections = utils::grid_operations::computeLineBoxIntersection(robot_pose.GetPosition(), laser_readings[i], robot_grid, beam_grid, limit_x, limit_y, m_cell_resol); - // Measurement outcomes vector {Pfree, Pocc, Pun} - std::vector probabilities { - calculateScanMassProbabilityBetween(distances[1], 5.0f), - calculateScanMassProbabilityBetween(distances[0], distances[1]), - calculateScanMassProbabilityBetween(0.0f, distances[0]) - }; + if (intersections.first.size() == 0) + continue; - // Appending new measurement outcomes for the current cell - appendCellProbabilities(probabilities, {cells_x[j], cells_y[j]}); + // 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 = robot_pose.GetPosition().Distance(intersection); + distances.push_back(distance); + } - // Get all the measurement outcomes for the current cell - std::vector> cell_prob = retreiveCellProbabilities({cells_x[j], cells_y[j]}); + // Measurement outcomes vector {Pfree, Pocc, Pun} + std::vector probabilities { + calculateScanMassProbabilityBetween(distances[1], 5.0f), + calculateScanMassProbabilityBetween(distances[0], distances[1]), + calculateScanMassProbabilityBetween(0.0f, distances[0]) + }; - // Compute all the possible combinations for the current cell - algorithm 1 - std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(cell_prob); - - kt_double cell_mutual_inf = 0.0f; - for (auto& pair : meas_out_prob) - { - cell_mutual_inf += pair.second * measurementOutcomeEntropy(pair.first); - } + // Appending new measurement outcomes for the current cell + appendCellProbabilities(probabilities, {cells_x[j], cells_y[j]}); - // Mutual information of cell x, y given a set of measurements - updateCellMutualInformation(0.5 - cell_mutual_inf, {cells_x[j], cells_y[j]}); + // Get all the measurement outcomes for the current cell + std::vector> cell_prob = retreiveCellProbabilities({cells_x[j], cells_y[j]}); + + // Compute all the possible combinations for the current cell - algorithm 1 + std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(cell_prob); + + 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 + updateCellMutualInformation(0.5 - cell_mutual_inf, {cells_x[j], cells_y[j]}); + } } + m_map_mutual_info = calculateMapMutualInformation(); + + // Extract the mutual information provided by this laser scan + updateLaserMutualInformation(); } - m_map_mutual_info = calculateMapMutualInformation(); - - // Extract the mutual information provided by this laser scan - updateLaserMutualInformation(); + + // This would need to change + /* + I am returning the whole mutual information + But I can return either the index of the laser which provided minimal information and the actual value + + Also I need to add function to clear the cells + And I need to add functions to get the right data + */ return m_map_mutual_info; } diff --git a/src/experimental/utils.cpp b/src/experimental/utils.cpp index b79e06a45..b753a0a9f 100644 --- a/src/experimental/utils.cpp +++ b/src/experimental/utils.cpp @@ -215,27 +215,5 @@ namespace utils return std::pair, std::vector>{inter_x, inter_y}; } - // template - // void initializeGrid(std::vector> & grid, int num_rows, int num_columns) - // { - // /* - // To create the grid - // */ - // std::cout << "Grid Initializaion -----------------" << std::endl; - // for (int i = 0; i < num_rows; ++i) - // { - // // Adding columns - // grid[i].resize(num_columns); - // for (int j = 0; j < num_columns; ++j) - // { - // grid[i][j] = static_cast(0); - // } - // } - // } - - void test() - { - std::cout << "Test" << std::endl; - } }// namespace grid_operations } // namespace utils \ No newline at end of file From b2cac9f8b2378292c37e0ecd2ed75ab2c27ba9cc Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Thu, 30 Dec 2021 12:53:35 -0500 Subject: [PATCH 44/83] [WIP]: More modification to make the code clear --- .../experimental/information_estimates.hpp | 44 ++---- .../experimental/slam_toolbox_lifelong.hpp | 3 - include/slam_toolbox/experimental/utils.hpp | 3 +- src/experimental/information_estimates.cpp | 148 ++++++++---------- src/experimental/utils.cpp | 21 +-- 5 files changed, 91 insertions(+), 128 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index 95afbde66..775bcd59e 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -8,9 +8,14 @@ class InformationEstimates typedef std::tuple map_tuple; public: - InformationEstimates(); // Empty contructor for now + InformationEstimates(kt_double sensor_range, kt_double resolution, kt_double lambda, kt_double nu); + InformationEstimates(); virtual ~InformationEstimates() {} +public: + // Main function + std::tuple calculateMutualInformation(std::vector const& range_scans); + private: // Mutual information kt_double calculateInformationContent(kt_double prob); @@ -21,10 +26,10 @@ class InformationEstimates // Measurement outcomes probabilities void appendCellProbabilities(std::vector& measurements, std::vector cell); std::unordered_map computeMeasurementOutcomesHistogram(std::vector>& meas_outcm); - std::vector> retreiveCellProbabilities(std::vector cell); + std::vector> retrieveCellProbabilities(std::vector cell); // Measurements calculations kt_double calculateScanMassProbabilityBetween(kt_double range_1, kt_double range_2); - void updateLaserMutualInformation(); + kt_double calculateLaserMutualInformation(kt_double const & map_info, kt_double const & curr_info); private: // Data structures @@ -32,38 +37,19 @@ class InformationEstimates std::vector> m_mutual_grid; std::vector> m_visited_grid; - kt_double m_map_dist; - kt_double m_cell_resol; - int m_num_cells; - 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 = 5.0; - kt_double m_obs_lambda = 0.35; - kt_double m_obs_nu = 0.28; - - kt_double m_map_mutual_info; - kt_double m_laser_mutual_info; - -public: - // Main function - // float calculateMutualInformation(karto::PointVectorDouble const& laser_readings, karto::Pose2 const& karto_pose); - float calculateMutualInformation(std::vector const& range_scans); - - // Setters - void setMaxSensorRange(kt_double const sensor_range); - void setObservationLambda(kt_double const lambda); - void setObservationNu(kt_double const nu); - void setCellResolution(kt_double const resolution); - void setMapDistance(kt_double const distance); - + kt_double m_max_sensor_range; + kt_double m_cell_resol; + kt_double m_obs_lambda; + kt_double m_obs_nu; - // Getters - kt_double getMapMutualInformation(); - kt_double getLaserMutualInformation(); + kt_double m_map_dist; + int m_num_cells; + kt_double m_curr_mut_info; }; #endif diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index a9ae1ce44..efca2b78f 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -20,9 +20,6 @@ #define SLAM_TOOLBOX__EXPERIMENTAL__SLAM_TOOLBOX_LIFELONG_HPP_ #include -#include -#include -#include #include "slam_toolbox/slam_toolbox_common.hpp" #include "slam_toolbox/experimental/information_estimates.hpp" diff --git a/include/slam_toolbox/experimental/utils.hpp b/include/slam_toolbox/experimental/utils.hpp index c9577c104..eae124892 100644 --- a/include/slam_toolbox/experimental/utils.hpp +++ b/include/slam_toolbox/experimental/utils.hpp @@ -8,7 +8,6 @@ #include #include #include - #include "lib/karto_sdk/include/karto_sdk/Karto.h" namespace utils @@ -20,7 +19,7 @@ namespace utils karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, kt_double resolution); int signum(int num); - std::pair, std::vector> rayCasting(karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt); + std::vector> rayCasting(karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt); karto::Vector2 getGridPosition(karto::Vector2 const& pose, kt_double resolution); std::vector calculateCellIntersectionPoints(karto::Vector2 const & laser_start, diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 190176807..17a14bfb4 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -1,56 +1,73 @@ #include #include "slam_toolbox/experimental/information_estimates.hpp" +InformationEstimates::InformationEstimates(kt_double sensor_range, kt_double resolution, kt_double lambda, kt_double nu) +{ + m_max_sensor_range = sensor_range; + m_cell_resol = resolution; + m_obs_lambda = lambda; + m_obs_nu = nu; + + m_map_dist = 200.0f; + m_num_cells = static_cast(m_map_dist / m_cell_resol); + + m_curr_mut_info = 0.0; + + utils::grid_operations::initializeGrid(m_mutual_grid, m_num_cells, m_num_cells); + utils::grid_operations::initializeGrid(m_visited_grid, m_num_cells, m_num_cells); +} + InformationEstimates::InformationEstimates() { - m_map_mutual_info = 0.0; - m_laser_mutual_info = 0.0; - m_cell_resol = 0.05f; // Map resolution - m_map_dist = 200.0f; // Total map distance + m_max_sensor_range = 5.0; + m_cell_resol = 0.05; + m_obs_lambda = 0.35; + m_obs_nu = 0.28; + + m_map_dist = 200.0f; m_num_cells = static_cast(m_map_dist / m_cell_resol); + m_curr_mut_info = 0.0; + utils::grid_operations::initializeGrid(m_mutual_grid, m_num_cells, m_num_cells); utils::grid_operations::initializeGrid(m_visited_grid, m_num_cells, m_num_cells); + + // Eigen::MatrixX mat(m_num_cells, m_num_cells); } -float InformationEstimates::calculateMutualInformation(std::vector const& range_scans) + +std::tuple InformationEstimates::calculateMutualInformation(std::vector const& range_scans) { - /* - I should receive a vector of LocalizedRangeScan, - For this I have two options, - Either the vector is made upd outside this scope - Or I have a function for appending the range scans and the call this function for processing - */ + int min_idx = 0; + int curr_idx = 0; + m_curr_mut_info = 0.0; + + // Initial guess of mutual information + kt_double mut_info = 1000.0; for (auto & scan : range_scans) { karto::Pose2 robot_pose = scan.GetOdometricPose(); karto::PointVectorDouble laser_readings = scan.GetPointReadings(true); - m_laser_mutual_info = m_laser_mutual_info; karto::Vector2 robot_grid = utils::grid_operations::getGridPosition(robot_pose.GetPosition(), m_cell_resol); // Set as false the current boolean map - utils::grid_operations::clearVisitedCells(m_visited_grid); + utils::grid_operations::clearVisitedCells(m_visited_grid); for (int i = 0; i < laser_readings.size(); ++i) { // Laser final cell karto::Vector2 beam_grid = utils::grid_operations::getGridPosition(laser_readings[i], m_cell_resol); + + // Visited cells by this laser beam + std::vector> cells = utils::grid_operations::rayCasting(robot_grid, beam_grid); - // Ray tracing for getting the visited cells - std::vector cells_x, cells_y; - std::pair, std::vector> res_pair = utils::grid_operations::rayCasting(robot_grid, beam_grid); - - cells_x = res_pair.first; - cells_y = res_pair.second; - - // Visiting the cells - for (int j = 0; j < cells_x.size(); ++j) + for (auto & cell : cells) { // Inidividual cell limits - kt_double limit_x = cells_x[j] * m_cell_resol; - kt_double limit_y = cells_y[j] * m_cell_resol; + kt_double limit_x = cell.GetX() * m_cell_resol; + kt_double limit_y = cell.GetY() * m_cell_resol; std::pair, std::vector> intersections = utils::grid_operations::computeLineBoxIntersection(robot_pose.GetPosition(), laser_readings[i], robot_grid, beam_grid, limit_x, limit_y, m_cell_resol); @@ -69,16 +86,16 @@ float InformationEstimates::calculateMutualInformation(std::vector probabilities { - calculateScanMassProbabilityBetween(distances[1], 5.0f), + calculateScanMassProbabilityBetween(distances[1], m_max_sensor_range), calculateScanMassProbabilityBetween(distances[0], distances[1]), calculateScanMassProbabilityBetween(0.0f, distances[0]) }; // Appending new measurement outcomes for the current cell - appendCellProbabilities(probabilities, {cells_x[j], cells_y[j]}); + appendCellProbabilities(probabilities, {cell.GetX(), cell.GetY()}); // Get all the measurement outcomes for the current cell - std::vector> cell_prob = retreiveCellProbabilities({cells_x[j], cells_y[j]}); + std::vector> cell_prob = retrieveCellProbabilities({cell.GetX(), cell.GetY()}); // Compute all the possible combinations for the current cell - algorithm 1 std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(cell_prob); @@ -90,24 +107,30 @@ float InformationEstimates::calculateMutualInformation(std::vector(m_mutual_grid); - Also I need to add function to clear the cells - And I need to add functions to get the right data - */ - return m_map_mutual_info; + return std::make_tuple(min_idx, mut_info); } void InformationEstimates::appendCellProbabilities(std::vector& measurements, std::vector cell) @@ -165,11 +188,11 @@ kt_double InformationEstimates::calculateMapMutualInformation() of all cells mutual information */ kt_double sum = 0.0f; - for (int i = 0; i < m_num_cells; ++i) + for (auto & rows : m_mutual_grid) { - for (int j = 0; j < m_num_cells; ++j) + for (auto & column : rows) { - sum += m_mutual_grid[i][j]; + sum += column; } } return sum; @@ -317,7 +340,7 @@ kt_double InformationEstimates::calculateScanMassProbabilityBetween(kt_double ra return m_obs_nu * (exp(-m_obs_lambda*range_1) - exp(-m_obs_lambda*range_2)); } -std::vector> InformationEstimates::retreiveCellProbabilities(std::vector cell) +std::vector> InformationEstimates::retrieveCellProbabilities(std::vector cell) { /* To get all the cell probabilities @@ -328,9 +351,9 @@ std::vector> InformationEstimates::retreiveCellProbabilit return it_cells->second; } -void InformationEstimates::updateLaserMutualInformation() +kt_double InformationEstimates::calculateLaserMutualInformation(kt_double const & map_info, kt_double const & curr_info) { - m_laser_mutual_info = fabs(m_laser_mutual_info - m_map_mutual_info); + return map_info - curr_info; } void InformationEstimates::updateCellMutualInformation(kt_double mut_inf, std::vector cell) @@ -341,40 +364,3 @@ void InformationEstimates::updateCellMutualInformation(kt_double mut_inf, std::v */ m_mutual_grid[cell[0]][cell[1]] = mut_inf; } - -void InformationEstimates::setMaxSensorRange(kt_double const sensor_range) -{ - m_max_sensor_range = sensor_range; -} - -void InformationEstimates::setObservationLambda(kt_double const lambda) -{ - m_obs_lambda = lambda; -} - -void InformationEstimates::setObservationNu(kt_double const nu) -{ - m_obs_nu = nu; -} - -void InformationEstimates::setCellResolution(kt_double const resolution) -{ - m_cell_resol = resolution; -} - -void InformationEstimates::setMapDistance(kt_double const distance) -{ - m_map_dist = distance; -} - - -kt_double InformationEstimates::getMapMutualInformation() -{ - return m_map_mutual_info; -} - -kt_double InformationEstimates::getLaserMutualInformation() -{ - return m_laser_mutual_info; -} - diff --git a/src/experimental/utils.cpp b/src/experimental/utils.cpp index b753a0a9f..a4576ad85 100644 --- a/src/experimental/utils.cpp +++ b/src/experimental/utils.cpp @@ -65,15 +65,14 @@ namespace utils return 0; } - std::pair, std::vector> rayCasting( + std::vector> rayCasting( karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt) { /* To find the set of cells hit by a laser beam This is based on Bresenham algorithm */ - std::vector x_bres; - std::vector y_bres; + std::vector> cells; int x = initial_pt.GetX(); int y = initial_pt.GetY(); @@ -98,8 +97,7 @@ namespace utils int b_res = 2 * (delta_y - delta_x); int e_res = (2 * delta_y) - delta_x; - x_bres.push_back(x); - y_bres.push_back(y); + cells.push_back(karto::Vector2{x, y}); for (int i = 1; i < delta_x; ++i) { @@ -115,18 +113,15 @@ namespace utils x += s_x; e_res += b_res; } - x_bres.push_back(x); - y_bres.push_back(y); + cells.push_back(karto::Vector2{x, y}); } // Delete the current robot cell - x_bres.erase(x_bres.begin()); - y_bres.erase(y_bres.begin()); - + cells.erase(cells.begin()); + // Adding last hit cell to the set - x_bres.push_back(final_pt.GetX()); - y_bres.push_back(final_pt.GetY()); + cells.push_back(karto::Vector2{final_pt.GetX(), final_pt.GetY()}); - return std::pair, std::vector>{x_bres, y_bres}; + return cells; } From 8ba213be5d67ad7428414d8b26ab0162e9d9cfd1 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Fri, 31 Dec 2021 08:25:45 -0500 Subject: [PATCH 45/83] [ADD]: Eigen addition and matrix replacement --- .../experimental/information_estimates.hpp | 12 +++-- include/slam_toolbox/experimental/utils.hpp | 33 ++----------- src/experimental/information_estimates.cpp | 49 ++++++++----------- src/experimental/utils.cpp | 22 +++++++++ 4 files changed, 52 insertions(+), 64 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index 775bcd59e..6ad0f6183 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -22,11 +22,12 @@ class InformationEstimates kt_double calculateMapMutualInformation(); kt_double measurementOutcomeEntropy(map_tuple const& meas_outcome); kt_double calculateProbabilityFromLogOdds(kt_double log); - void updateCellMutualInformation(kt_double mut_inf, std::vector cell); + void updateCellMutualInformation(kt_double mut_inf, karto::Vector2 const & cell); + // Measurement outcomes probabilities - void appendCellProbabilities(std::vector& measurements, std::vector cell); + void appendCellProbabilities(std::vector& measurements, karto::Vector2 const & cell); std::unordered_map computeMeasurementOutcomesHistogram(std::vector>& meas_outcm); - std::vector> retrieveCellProbabilities(std::vector cell); + std::vector> retrieveCellProbabilities(karto::Vector2 const & cell); // Measurements calculations kt_double calculateScanMassProbabilityBetween(kt_double range_1, kt_double range_2); kt_double calculateLaserMutualInformation(kt_double const & map_info, kt_double const & curr_info); @@ -34,8 +35,6 @@ class InformationEstimates private: // Data structures std::map, std::vector>> m_cell_probabilities; - std::vector> m_mutual_grid; - std::vector> m_visited_grid; const kt_double l_free = log(0.3 / (1.0 - 0.3)); const kt_double l_occ = log(0.7 / (1.0 - 0.7)); @@ -50,6 +49,9 @@ class InformationEstimates int m_num_cells; kt_double m_curr_mut_info; + + Eigen::MatrixXd m_mutual_grid; + Eigen::MatrixXi m_visited_grid; }; #endif diff --git a/include/slam_toolbox/experimental/utils.hpp b/include/slam_toolbox/experimental/utils.hpp index eae124892..fe73da5ec 100644 --- a/include/slam_toolbox/experimental/utils.hpp +++ b/include/slam_toolbox/experimental/utils.hpp @@ -9,6 +9,7 @@ #include #include #include "lib/karto_sdk/include/karto_sdk/Karto.h" +#include "Eigen/Core" namespace utils { @@ -30,36 +31,8 @@ namespace utils karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, kt_double limit_x, kt_double limit_y, kt_double resolution); - - template - void clearVisitedCells(std::vector> & grid) - { - for (int i = 0; i < grid.size(); ++i) - { - for (int j = 0; j < grid[0].size(); ++j) - { - grid[i][j] = static_cast(0); - } - } - } - - template - void initializeGrid(std::vector> & grid, int num_rows, int num_columns) - { - /* - To create the grid - */ - grid.resize(num_rows); - for (int i = 0; i < num_rows; ++i) - { - // Adding columns - grid[i].resize(num_columns); - for (int j = 0; j < num_columns; ++j) - { - grid[i][j] = static_cast(0); - } - } - } + void clearVisitedCells(Eigen::MatrixXd & grid); + void clearVisitedCells(Eigen::MatrixXi & grid); } // namespace grid_operations namespace tuple_hash diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 17a14bfb4..ffae8c5b3 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -13,8 +13,8 @@ InformationEstimates::InformationEstimates(kt_double sensor_range, kt_double res m_curr_mut_info = 0.0; - utils::grid_operations::initializeGrid(m_mutual_grid, m_num_cells, m_num_cells); - utils::grid_operations::initializeGrid(m_visited_grid, m_num_cells, m_num_cells); + m_mutual_grid.resize(m_num_cells, m_num_cells); + m_visited_grid.resize(m_num_cells, m_num_cells); } InformationEstimates::InformationEstimates() @@ -29,10 +29,8 @@ InformationEstimates::InformationEstimates() m_curr_mut_info = 0.0; - utils::grid_operations::initializeGrid(m_mutual_grid, m_num_cells, m_num_cells); - utils::grid_operations::initializeGrid(m_visited_grid, m_num_cells, m_num_cells); - - // Eigen::MatrixX mat(m_num_cells, m_num_cells); + m_mutual_grid.resize(m_num_cells, m_num_cells); + m_visited_grid.resize(m_num_cells, m_num_cells); } @@ -53,7 +51,7 @@ std::tuple InformationEstimates::calculateMutualInformation(std: karto::Vector2 robot_grid = utils::grid_operations::getGridPosition(robot_pose.GetPosition(), m_cell_resol); // Set as false the current boolean map - utils::grid_operations::clearVisitedCells(m_visited_grid); + utils::grid_operations::clearVisitedCells(m_visited_grid); for (int i = 0; i < laser_readings.size(); ++i) { @@ -92,10 +90,10 @@ std::tuple InformationEstimates::calculateMutualInformation(std: }; // Appending new measurement outcomes for the current cell - appendCellProbabilities(probabilities, {cell.GetX(), cell.GetY()}); + appendCellProbabilities(probabilities, cell); // Get all the measurement outcomes for the current cell - std::vector> cell_prob = retrieveCellProbabilities({cell.GetX(), cell.GetY()}); + std::vector> cell_prob = retrieveCellProbabilities(cell); // Compute all the possible combinations for the current cell - algorithm 1 std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(cell_prob); @@ -107,7 +105,7 @@ std::tuple InformationEstimates::calculateMutualInformation(std: } // Mutual information of cell x, y given a set of measurements - updateCellMutualInformation(1.0 - cell_mutual_inf, {cell.GetX(), cell.GetY()}); + updateCellMutualInformation(1.0 - cell_mutual_inf, cell); } } @@ -128,29 +126,29 @@ std::tuple InformationEstimates::calculateMutualInformation(std: } // Clearing the cells for the next time it is called - utils::grid_operations::clearVisitedCells(m_mutual_grid); + utils::grid_operations::clearVisitedCells(m_mutual_grid); return std::make_tuple(min_idx, mut_info); } -void InformationEstimates::appendCellProbabilities(std::vector& measurements, std::vector cell) +void InformationEstimates::appendCellProbabilities(std::vector& measurements, karto::Vector2 const & cell) { /* To append a new measurement for a specific cell */ std::map, std::vector>>::iterator it_cell; - it_cell = m_cell_probabilities.find({cell[0], cell[1]}); + it_cell = m_cell_probabilities.find({cell.GetX(), cell.GetY()}); 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[0], cell[1]}, {{measurements[0], measurements[1], measurements[2]}})); - m_visited_grid[cell[0]][cell[1]] = true; + {cell.GetX(), cell.GetY()}, {{measurements[0], measurements[1], measurements[2]}})); + m_visited_grid(cell.GetX(), cell.GetY()) = 1; } else { - if(m_visited_grid[cell[0]][cell[1]] == true) + if(m_visited_grid(cell.GetX(), cell.GetY()) == 1) { // Compare the unknown probability, the smallest it is the most information we will have // from the occupied or free state @@ -167,7 +165,7 @@ void InformationEstimates::appendCellProbabilities(std::vector& measu { // Cell is already in the map, only add the next measurement outcome it_cell->second.push_back({measurements[0], measurements[1], measurements[2]}); - m_visited_grid[cell[0]][cell[1]] = true; + m_visited_grid(cell.GetX(), cell.GetY()) = 1; } } } @@ -187,14 +185,7 @@ kt_double InformationEstimates::calculateMapMutualInformation() To calculate map mutual information, this is the summation of all cells mutual information */ - kt_double sum = 0.0f; - for (auto & rows : m_mutual_grid) - { - for (auto & column : rows) - { - sum += column; - } - } + kt_double sum = m_mutual_grid.sum(); return sum; } @@ -340,13 +331,13 @@ kt_double InformationEstimates::calculateScanMassProbabilityBetween(kt_double ra return m_obs_nu * (exp(-m_obs_lambda*range_1) - exp(-m_obs_lambda*range_2)); } -std::vector> InformationEstimates::retrieveCellProbabilities(std::vector cell) +std::vector> InformationEstimates::retrieveCellProbabilities(karto::Vector2 const & cell) { /* To get all the cell probabilities */ std::map, std::vector>>::iterator it_cells; - it_cells = m_cell_probabilities.find({cell[0], cell[1]}); + it_cells = m_cell_probabilities.find({cell.GetX(), cell.GetY()}); return it_cells->second; } @@ -356,11 +347,11 @@ kt_double InformationEstimates::calculateLaserMutualInformation(kt_double const return map_info - curr_info; } -void InformationEstimates::updateCellMutualInformation(kt_double mut_inf, std::vector cell) +void InformationEstimates::updateCellMutualInformation(kt_double mut_inf, karto::Vector2 const & cell) { /* To update the mutual information for each individual cell This is the result of the summation of 3.12 */ - m_mutual_grid[cell[0]][cell[1]] = mut_inf; + m_mutual_grid(cell.GetX(), cell.GetY()) = mut_inf; } diff --git a/src/experimental/utils.cpp b/src/experimental/utils.cpp index a4576ad85..b5a221441 100644 --- a/src/experimental/utils.cpp +++ b/src/experimental/utils.cpp @@ -65,6 +65,28 @@ namespace utils return 0; } + void clearVisitedCells(Eigen::MatrixXd & grid) + { + for (int i = 0; i < grid.rows(); ++i) + { + for (int j = 0; j < grid.cols(); ++j) + { + grid(i, j) = 0.0; + } + } + } + + void clearVisitedCells(Eigen::MatrixXi & grid) + { + for (int i = 0; i < grid.rows(); ++i) + { + for (int j = 0; j < grid.cols(); ++j) + { + grid(i, j) = 0; + } + } + } + std::vector> rayCasting( karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt) { From 88d35619d4e9db3bc12adcdf1773083139c3b0b9 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Tue, 4 Jan 2022 08:47:10 -0500 Subject: [PATCH 46/83] [DOC]: Adding code documentation --- .../experimental/information_estimates.hpp | 8 +- include/slam_toolbox/experimental/utils.hpp | 7 +- src/experimental/information_estimates.cpp | 116 +++++++++++++----- src/experimental/utils.cpp | 104 ++++++++++++---- 4 files changed, 174 insertions(+), 61 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index 6ad0f6183..bc6ec884f 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -14,7 +14,7 @@ class InformationEstimates public: // Main function - std::tuple calculateMutualInformation(std::vector const& range_scans); + std::tuple calculateMutualInformation(std::vector const& range_scans); private: // Mutual information @@ -28,6 +28,7 @@ class InformationEstimates void appendCellProbabilities(std::vector& measurements, karto::Vector2 const & cell); std::unordered_map computeMeasurementOutcomesHistogram(std::vector>& meas_outcm); std::vector> retrieveCellProbabilities(karto::Vector2 const & cell); + // Measurements calculations kt_double calculateScanMassProbabilityBetween(kt_double range_1, kt_double range_2); kt_double calculateLaserMutualInformation(kt_double const & map_info, kt_double const & curr_info); @@ -44,12 +45,11 @@ class InformationEstimates kt_double m_cell_resol; kt_double m_obs_lambda; kt_double m_obs_nu; - + kt_double m_curr_mut_info; kt_double m_map_dist; int m_num_cells; - kt_double m_curr_mut_info; - + // Map grids Eigen::MatrixXd m_mutual_grid; Eigen::MatrixXi m_visited_grid; }; diff --git a/include/slam_toolbox/experimental/utils.hpp b/include/slam_toolbox/experimental/utils.hpp index fe73da5ec..62c9d8dac 100644 --- a/include/slam_toolbox/experimental/utils.hpp +++ b/include/slam_toolbox/experimental/utils.hpp @@ -18,19 +18,15 @@ namespace utils void updateCellLimits(std::vector& initial_x, std::vector& initial_y, std::vector& final_x, std::vector& final_y, kt_double limit_x, kt_double limit_y, std::vector& cell_limits, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, kt_double resolution); - int signum(int num); std::vector> rayCasting(karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt); karto::Vector2 getGridPosition(karto::Vector2 const& pose, kt_double resolution); - std::vector calculateCellIntersectionPoints(karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, std::vector cell_start, std::vector cell_end); - std::pair, std::vector> computeLineBoxIntersection( karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, karto::Vector2 const& robot_grid_pos, karto::Vector2 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 @@ -41,6 +37,9 @@ namespace utils { 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); diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index ffae8c5b3..ac57e8ec4 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -1,6 +1,8 @@ #include #include "slam_toolbox/experimental/information_estimates.hpp" +#include + InformationEstimates::InformationEstimates(kt_double sensor_range, kt_double resolution, kt_double lambda, kt_double nu) { m_max_sensor_range = sensor_range; @@ -33,20 +35,27 @@ InformationEstimates::InformationEstimates() m_visited_grid.resize(m_num_cells, m_num_cells); } - -std::tuple InformationEstimates::calculateMutualInformation(std::vector const& range_scans) +std::tuple InformationEstimates::calculateMutualInformation(std::vector const& range_scans) { + /** + * Find and return the laser scan of a set of LocalizedRangeScan with the minimal mutual information + * Arguments: + * range_scans [std::vector]: Vector of LocalizedRangeScan pointers + * Return: + * std::tuple: Tuple containing the index of the LozalizedRangeScan and its corresponding mutual information + */ int min_idx = 0; int curr_idx = 0; m_curr_mut_info = 0.0; // Initial guess of mutual information - kt_double mut_info = 1000.0; + kt_double mut_info = 100000.0; for (auto & scan : range_scans) { - karto::Pose2 robot_pose = scan.GetOdometricPose(); - karto::PointVectorDouble laser_readings = scan.GetPointReadings(true); + karto::Pose2 robot_pose = scan->GetOdometricPose(); + karto::PointVectorDouble laser_readings = scan->GetPointReadings(true); + std::cout << laser_readings.size() << std::endl; karto::Vector2 robot_grid = utils::grid_operations::getGridPosition(robot_pose.GetPosition(), m_cell_resol); @@ -110,6 +119,7 @@ std::tuple InformationEstimates::calculateMutualInformation(std: } kt_double map_mut_info = calculateMapMutualInformation(); + std::cout << map_mut_info << std::endl; // Extract the mutual information provided by this laser scan kt_double laser_mut_info = calculateLaserMutualInformation(map_mut_info, m_curr_mut_info); @@ -133,8 +143,13 @@ std::tuple InformationEstimates::calculateMutualInformation(std: void InformationEstimates::appendCellProbabilities(std::vector& measurements, karto::Vector2 const & cell) { - /* - To append a new measurement for a specific 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; @@ -172,18 +187,24 @@ void InformationEstimates::appendCellProbabilities(std::vector& measu kt_double InformationEstimates::calculateInformationContent(kt_double prob) { - /* - To calculate the information content or self-information - based on the proability of being occupied + /** + * 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::calculateMapMutualInformation() { - /* - To calculate map mutual information, this is the summation - of all cells mutual information + /** + * Calculate the mutual information of the current map, this is the summation of all cells mutual information + * Arguments: + * Void + * Return: + * kt_double: Map mutual information */ kt_double sum = m_mutual_grid.sum(); return sum; @@ -191,17 +212,25 @@ kt_double InformationEstimates::calculateMapMutualInformation() kt_double InformationEstimates::calculateProbabilityFromLogOdds(kt_double log) { - /* - To transform the Log-odds into probability - This should be a free function + /** + * Map Log-Odds into probability + * Arguments: + * log [kt_double]: Log-Odds + * Return: + * kt_double: Probability */ return (exp(log) / (1 + exp(log))); } std::unordered_map InformationEstimates::computeMeasurementOutcomesHistogram(std::vector>& meas_outcm) { - /* - To compute all the possible combinations of a grid cell, given a set of measurement outcomes + /** + * 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 temp_map; @@ -306,11 +335,15 @@ std::unordered_map) - - Calculate Log-Odds from initial probability guess - - Calculate the probability from those logs - - Calculate the entropy with the retrieved probability + /** + * 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 + * Arguments: + * meas_outcome [map_tuple]: Measurement outcome in the form {p_free, p_occ, p_unk} + * Return: + * kt_double: Measurement outcome entropy */ int num_free, num_occ, num_unk; std::tie(num_free, num_occ, num_unk) = meas_outcome; @@ -321,10 +354,14 @@ kt_double InformationEstimates::measurementOutcomeEntropy(map_tuple const& meas_ kt_double InformationEstimates::calculateScanMassProbabilityBetween(kt_double range_1, kt_double range_2) { - /* - To calculate the mass probability of a cell being observed by a given measurement + /** + * Calculate the mass probability of a cell being observed by a given measurement + * Arguments: + * range_1 [kt_double]: Lower bound + * range_2 [kt_double]: Upper bound + * Return: + * kt_double: Mass probability */ - 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; @@ -333,8 +370,12 @@ kt_double InformationEstimates::calculateScanMassProbabilityBetween(kt_double ra std::vector> InformationEstimates::retrieveCellProbabilities(karto::Vector2 const & cell) { - /* - To get all the cell probabilities + /** + * Retrieve the cell probabilities (Measurement outcomes) + * Arguments: + * cell [karto::Vector2]: Cell coordinates + * Return: + * std::vector>: Vector of cell probabilities in the form {p_free, p_occ, p_unk} (Measurement outcomes) */ std::map, std::vector>>::iterator it_cells; it_cells = m_cell_probabilities.find({cell.GetX(), cell.GetY()}); @@ -344,14 +385,27 @@ std::vector> InformationEstimates::retrieveCellProbabilit kt_double InformationEstimates::calculateLaserMutualInformation(kt_double const & map_info, kt_double const & curr_info) { + /** + * Calculate the laser mutual information considering the map mutual information and the current mutual information calculation + * Arguments: + * map_info [kt_double]: Map mutual information + * curr_info [kt_double]: Current mutual information + * Return: + * kt_double: Laser mutual information + */ return map_info - curr_info; } void InformationEstimates::updateCellMutualInformation(kt_double mut_inf, karto::Vector2 const & cell) { - /* - To update the mutual information for each individual cell - This is the result of the summation of 3.12 + /** + * Note: Result of the summation 3.12 + * Update the mutual information for each individual cell + * Arguments: + * mut_inf [kt_double]: Cell mutual information + * cell [karto::Vector2]: Cell coordinates + * Return: + * Void */ m_mutual_grid(cell.GetX(), cell.GetY()) = mut_inf; } diff --git a/src/experimental/utils.cpp b/src/experimental/utils.cpp index b5a221441..40043c197 100644 --- a/src/experimental/utils.cpp +++ b/src/experimental/utils.cpp @@ -9,9 +9,22 @@ namespace utils std::vector& final_y, kt_double limit_x, kt_double limit_y, std::vector& cell_limits, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, kt_double resolution) { - /* - To calculate grid grid limits for intersection - */ + /** + * To calculate grid limits for intersection + * Arguments: + * initial_x [std::vector]: Cell initial limits in x (4 possible limits) + * initial_y [std::vector]: Cell initial limits in y (4 possible limits) + * final_x [std::vector]: Cell final limits in x (4 possible limits) + * final_y [std::vector]: Cell final limits in y (4 possible limits) + * limit_x [kt_double]: Current cell position in x (Transformed from int to kt_double) + * limit_y [kt_double]: Current cell position in y (Transformed from int to kt_double) + * cell_limits [std::vector]: Cell final points for assertion in x and y + * robot_grip_position [std::vector]: Initial laser beam position + * final_grid_position [std::vector]: Final laser beam position + * resolution [kt_double]: Cell resolution + * Return: + * Void + */ if (final_grid_pos.GetX() < robot_grid_pos.GetX() && final_grid_pos.GetY() >= robot_grid_pos.GetY()) { // X greater and Y greater. WRO final points @@ -57,9 +70,13 @@ namespace utils int signum(int num) { - /* - To get the sign of an operation, used by Bresenham algorithm - */ + /** + * Get the sign of an operation, used by Bresenham algorithm + * Arguments: + * num [int]: Number for perform the sign operation + * Return: + * int: Sign + */ if (num < 0) return -1; if (num >= 1) return 1; return 0; @@ -67,6 +84,13 @@ namespace utils void clearVisitedCells(Eigen::MatrixXd & grid) { + /** + * Clear the given floating Eigen::Matrix + * Arguments: + * grid [Eigen::Matrix]: Grid for cleaning + * Return: + * Void + */ for (int i = 0; i < grid.rows(); ++i) { for (int j = 0; j < grid.cols(); ++j) @@ -78,6 +102,14 @@ namespace utils void clearVisitedCells(Eigen::MatrixXi & grid) { + /** + * Clear the given integer Eigen::Matrix + * Arguments: + * grid [Eigen::Matrix]: Grid for cleaning + * Return: + * Void + */ + for for (int i = 0; i < grid.rows(); ++i) { for (int j = 0; j < grid.cols(); ++j) @@ -90,10 +122,14 @@ namespace utils std::vector> rayCasting( karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt) { - /* - To find the set of cells hit by a laser beam - This is based on Bresenham algorithm - */ + /** + * Find the set of cells hit by a laser beam (Bresenham algorithm) + * Arguments: + * initial_pt [karto::Vector2]: Laser beam initial position + * final_pt [karto::Vector2]: Laser beam final position + * Return: + * std::vector>: Vector of cells visited by the given laser beam + */ std::vector> cells; int x = initial_pt.GetX(); @@ -149,6 +185,14 @@ namespace utils karto::Vector2 getGridPosition(karto::Vector2 const& pose, kt_double resolution) { + /** + * Mapping a continuous position into a grid position + * Arguments: + * pose [karto::Vector2]: Continuos pose + * resolution [kt_double]: Cell resolution + * Return: + * karto::Vector2: Grid position + */ int x_cell = floor((pose.GetX() / resolution)); int y_cell = floor((pose.GetY() / resolution)); @@ -158,12 +202,16 @@ namespace utils std::vector calculateCellIntersectionPoints(karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, std::vector cell_start, std::vector cell_end) { - /* - Initial point laser beam: laser_start - Final point laser beam: laser_end - Initial point cell: cell_start - Final point cell: cell_end - */ + /** + * Find the intersection point between a cell line and a laser beam + * Arguments: + * laser_start [karto::Vector2]: Laser initial point in x and y + * laser_end [karto::Vector2]: Laser final point in x and y + * cell_start [std::vector]: Cell initial point in x and y + * cell_end [std::vector]: Cell final point in x and y + * Return: + * std::vector: Intersection point + */ kt_double x1 = laser_start.GetX(); kt_double x2 = laser_end.GetX(); kt_double x3 = cell_start[0]; @@ -177,7 +225,7 @@ namespace utils kt_double den = ((x2 - x1)*(y4 - y3) - (x4 - x3)*(y2 - y1)); if (den == 0.0f) { - // Parallel lines or not intersection at all + // Parallel lines return {}; } else @@ -193,6 +241,20 @@ namespace utils karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, kt_double limit_x, kt_double limit_y, kt_double resolution) { + /** + * Compute intersection between a cell and a laser beam + * Arguments: + * laser_start [karto::Vector2]: Laser initial point in x and y + * laser_end [karto::Vector2]: Laser final point in x and y + * robot_grid_pos [karto::Vector2]: Initial grid position in x and y + * final_grid_pos [karto::Vector2]: Final grid position in x and y + * limit_x [kt_double]: Current cell position in x (Transformed from int to kt_double) + * limit_y [kt_double]: Current cell position in y (Transformed from int to kt_double) + * resolution [kt_double]: Cell resolution + * Return: + * std::vector: Intersection point + */ + // Cell limits: min_x, max_x, min_y, max_y std::vector cell_limits {limit_x, limit_x + resolution, limit_y, limit_y + resolution}; @@ -219,11 +281,9 @@ namespace utils (fabs(intersection[1]) >= (fabs(cell_limits[2]) - 0.001)) && (fabs(intersection[1]) <= (fabs(cell_limits[3]) + 0.001))) { - /* - 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) - */ + // 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[0]); inter_y.push_back(intersection[1]); } From 93a1809aabf665b64569204b1107b0fbe76ea4b2 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Fri, 7 Jan 2022 17:22:55 -0500 Subject: [PATCH 47/83] [ADD]: Adding basic test logic --- CMakeLists.txt | 18 ++++++++++++++++-- src/experimental/utils.cpp | 3 +-- test/information_estimates_test.cpp | 25 +++++++++++++++++++++++++ 3 files changed, 42 insertions(+), 4 deletions(-) create mode 100644 test/information_estimates_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 8fd62e570..3333bc205 100644 --- a/CMakeLists.txt +++ b/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}) @@ -162,8 +167,8 @@ 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 src/experimental/information_estimates.cpp src/experimental/utils.cpp) -target_link_libraries(lifelong_slam_toolbox_node lifelong_slam_toolbox) +add_executable(lifelong_slam_toolbox_node src/experimental/slam_toolbox_lifelong_node.cpp) +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/src/experimental/utils.cpp b/src/experimental/utils.cpp index 40043c197..729408524 100644 --- a/src/experimental/utils.cpp +++ b/src/experimental/utils.cpp @@ -109,7 +109,6 @@ namespace utils * Return: * Void */ - for for (int i = 0; i < grid.rows(); ++i) { for (int j = 0; j < grid.cols(); ++j) @@ -293,4 +292,4 @@ namespace utils } }// namespace grid_operations -} // namespace utils \ No newline at end of file +} // namespace utils diff --git a/test/information_estimates_test.cpp b/test/information_estimates_test.cpp new file mode 100644 index 000000000..db389d271 --- /dev/null +++ b/test/information_estimates_test.cpp @@ -0,0 +1,25 @@ + +#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(InformationEstimatesTests, BareTest) +{ + InformationEstimates information_estimates; + + ASSERT_EQ(utils::grid_operations::signum(10), 0) << "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"; +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + + +#endif // SLAM_TOOLBOX_SLAM_TOOLBOX_INFORMATION_ESTIMATES_TEST_H_ From 899607fdfbd3fc77966edf69770196eeb9170b66 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Sat, 8 Jan 2022 10:53:22 -0500 Subject: [PATCH 48/83] [ADD]: Adding tests for Information Estimates and Utils --- test/information_estimates_test.cpp | 76 ++++++++++++++++++++++++++++- 1 file changed, 74 insertions(+), 2 deletions(-) diff --git a/test/information_estimates_test.cpp b/test/information_estimates_test.cpp index db389d271..b3ca00cad 100644 --- a/test/information_estimates_test.cpp +++ b/test/information_estimates_test.cpp @@ -6,20 +6,92 @@ #include "slam_toolbox/experimental/information_estimates.hpp" -TEST(InformationEstimatesTests, BareTest) +TEST(UtilsInformationEstimatesTests, SigNumTest) { InformationEstimates information_estimates; - ASSERT_EQ(utils::grid_operations::signum(10), 0) << "FAIL in positive"; + 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, RayCastTest) +{ + InformationEstimates information_estimates; + + karto::Vector2 initial_pt{22,43}; + karto::Vector2 final_pt{29,38}; + + std::vector> cells = utils::grid_operations::rayCasting(initial_pt, final_pt); + + ASSERT_EQ(cells[2].GetX(), 25) << "FAIL in X position for cell 3"; + ASSERT_EQ(cells[2].GetY(), 41) << "FAIL in Y position for cell 3"; + + ASSERT_EQ(cells[4].GetX(), 27) << "FAIL in X position for cell 5"; + ASSERT_EQ(cells[4].GetY(), 39) << "FAIL in Y position for cell 5"; +} + +TEST(UtilsInformationEstimatesTests, IntersectionPointsTest) +{ + karto::Vector2 laser_start{1.5, 1.6}; + karto::Vector2 laser_end{6.1, 8.4}; + std::vector cell_start{3.6, 8.2}; + std::vector cell_end{4.7, 1.6}; + + std::vector int_points = utils::grid_operations::calculateCellIntersectionPoints(laser_start, laser_end, cell_start, cell_end); + + ASSERT_FLOAT_EQ(int_points[0], 4.06744) << "FAIL in X coordinate"; + ASSERT_FLOAT_EQ(int_points[1], 5.39535) << "FAIL in Y coordinate"; +} + +TEST(InformationEstimatesTests, MutualInformationTest) +{ + InformationEstimates inf_estimates; + + karto::LocalizedRangeScan * scan_1 = new karto::LocalizedRangeScan(); + karto::LocalizedRangeScan * scan_2 = new karto::LocalizedRangeScan(); + + karto::Pose2 pose_1 = karto::Pose2(3.5, 4.0, 0.0); + karto::Pose2 pose_2 = karto::Pose2(3.5, 5.5, 0.0); + + scan_1->SetBarycenterPose(pose_1); + scan_2->SetBarycenterPose(pose_2); + + karto::BoundingBox2 bb1, bb2; + 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)); + + scan_1->SetBoundingBox(bb1); + scan_2->SetBoundingBox(bb2); + karto::PointVectorDouble points; + points.push_back(karto::Vector2(3.0, 5.0)); + points.push_back(karto::Vector2(3.0, 3.1)); + + scan_1->SetPointReadings(points, true); + + karto::PointVectorDouble laser_readings = scan_1->GetPointReadings(true); + + EXPECT_FLOAT_EQ(laser_readings[0].GetX(), 3.0) << "EXPECT FAIL in X coordinate"; + EXPECT_FLOAT_EQ(laser_readings[0].GetY(), 5.1) << "EXPECT FAIL in Y coordinate"; + + ASSERT_FLOAT_EQ(laser_readings[0].GetX(), 3.0) << "FAIL in X coordinate"; + ASSERT_FLOAT_EQ(laser_readings[0].GetY(), 5.0) << "FAIL in Y coordinate"; + + // bool dirty = false; + // s1->SetIsDirty(dirty); + // s2->SetIsDirty(dirty); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } +// colcon build --packages-select slam_toolbox --cmake-args -DBUILD_TESTING=ON +// colcon test --packages-select slam_toolbox --event-handlers console_direct+ + #endif // SLAM_TOOLBOX_SLAM_TOOLBOX_INFORMATION_ESTIMATES_TEST_H_ From 1b432f3731b65dbc4f0e919d392da1efe6824d9d Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Mon, 10 Jan 2022 09:18:39 -0500 Subject: [PATCH 49/83] [WIP]: Adding more test cases --- src/experimental/information_estimates.cpp | 12 ++-- test/information_estimates_test.cpp | 66 +++++++++++----------- 2 files changed, 37 insertions(+), 41 deletions(-) diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index ac57e8ec4..d21574b73 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -49,14 +49,12 @@ std::tuple InformationEstimates::calculateMutualInformation(std: m_curr_mut_info = 0.0; // Initial guess of mutual information - kt_double mut_info = 100000.0; + kt_double mut_info = 1000000.0; for (auto & scan : range_scans) { - karto::Pose2 robot_pose = scan->GetOdometricPose(); + karto::Pose2 robot_pose = scan->GetBarycenterPose(); karto::PointVectorDouble laser_readings = scan->GetPointReadings(true); - std::cout << laser_readings.size() << std::endl; - karto::Vector2 robot_grid = utils::grid_operations::getGridPosition(robot_pose.GetPosition(), m_cell_resol); // Set as false the current boolean map @@ -117,18 +115,16 @@ std::tuple InformationEstimates::calculateMutualInformation(std: updateCellMutualInformation(1.0 - cell_mutual_inf, cell); } } - kt_double map_mut_info = calculateMapMutualInformation(); - std::cout << map_mut_info << std::endl; // Extract the mutual information provided by this laser scan kt_double laser_mut_info = calculateLaserMutualInformation(map_mut_info, m_curr_mut_info); m_curr_mut_info = map_mut_info; // Compare to assign the minimal value in the index and the mutual information. - if (laser_mut_info < mut_info) + if ((laser_mut_info < mut_info) && (laser_mut_info != 0.0)) { - // This is the assingation of the result + // This is the assignation of the result mut_info = laser_mut_info; min_idx = curr_idx; } diff --git a/test/information_estimates_test.cpp b/test/information_estimates_test.cpp index b3ca00cad..bf6c73093 100644 --- a/test/information_estimates_test.cpp +++ b/test/information_estimates_test.cpp @@ -48,40 +48,44 @@ TEST(InformationEstimatesTests, MutualInformationTest) { InformationEstimates inf_estimates; - karto::LocalizedRangeScan * scan_1 = new karto::LocalizedRangeScan(); - karto::LocalizedRangeScan * scan_2 = new karto::LocalizedRangeScan(); - - karto::Pose2 pose_1 = karto::Pose2(3.5, 4.0, 0.0); - karto::Pose2 pose_2 = karto::Pose2(3.5, 5.5, 0.0); - - scan_1->SetBarycenterPose(pose_1); - scan_2->SetBarycenterPose(pose_2); - + karto::LocalizedRangeScan * s1 = new karto::LocalizedRangeScan(); + karto::LocalizedRangeScan * s2 = new karto::LocalizedRangeScan(); + karto::Pose2 p1 = karto::Pose2(3.5, 4.0, 0.0); + karto::Pose2 p2 = karto::Pose2(3.5, 5.5, 0.0); + s1->SetBarycenterPose(p1); + s2->SetBarycenterPose(p2); karto::BoundingBox2 bb1, bb2; 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)); - - scan_1->SetBoundingBox(bb1); - scan_2->SetBoundingBox(bb2); - karto::PointVectorDouble points; - points.push_back(karto::Vector2(3.0, 5.0)); - points.push_back(karto::Vector2(3.0, 3.1)); - - scan_1->SetPointReadings(points, true); - - karto::PointVectorDouble laser_readings = scan_1->GetPointReadings(true); - - EXPECT_FLOAT_EQ(laser_readings[0].GetX(), 3.0) << "EXPECT FAIL in X coordinate"; - EXPECT_FLOAT_EQ(laser_readings[0].GetY(), 5.1) << "EXPECT FAIL in Y coordinate"; - - ASSERT_FLOAT_EQ(laser_readings[0].GetX(), 3.0) << "FAIL in X coordinate"; - ASSERT_FLOAT_EQ(laser_readings[0].GetY(), 5.0) << "FAIL in Y coordinate"; - - // bool dirty = false; - // s1->SetIsDirty(dirty); - // s2->SetIsDirty(dirty); + s1->SetBoundingBox(bb1); + s2->SetBoundingBox(bb2); + karto::PointVectorDouble pts1, pts2; + 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)); + s1->SetPointReadings(pts1, true); + s2->SetPointReadings(pts2, true); + bool dirty = false; + s1->SetIsDirty(dirty); + s2->SetIsDirty(dirty); + + std::vector range_scan_vct; + range_scan_vct.push_back(s1); + range_scan_vct.push_back(s2); + + std::tuple min_inf = inf_estimates.calculateMutualInformation(range_scan_vct); + + // El que contiene menor informacion es s2, index 1 + int idx; + kt_double mut_inf; + std::tie(idx, mut_inf) = min_inf; + + EXPECT_EQ(idx, 1) << "FAIL in laser index"; + EXPECT_NE(mut_inf, 0.0) << "FAIL in mutual information equality"; } int main(int argc, char ** argv) @@ -90,8 +94,4 @@ int main(int argc, char ** argv) return RUN_ALL_TESTS(); } -// colcon build --packages-select slam_toolbox --cmake-args -DBUILD_TESTING=ON -// colcon test --packages-select slam_toolbox --event-handlers console_direct+ - - #endif // SLAM_TOOLBOX_SLAM_TOOLBOX_INFORMATION_ESTIMATES_TEST_H_ From b5752c7d60a8b353b92aed5b0e8a48114eaa8869 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Wed, 12 Jan 2022 16:57:14 -0500 Subject: [PATCH 50/83] [WIP]: Tests updating --- include/slam_toolbox/experimental/utils.hpp | 5 +- src/experimental/information_estimates.cpp | 19 +++++- src/experimental/utils.cpp | 48 ++++++++------- test/information_estimates_test.cpp | 65 ++++++++++++++------- 4 files changed, 92 insertions(+), 45 deletions(-) diff --git a/include/slam_toolbox/experimental/utils.hpp b/include/slam_toolbox/experimental/utils.hpp index 62c9d8dac..c3d6e3b3b 100644 --- a/include/slam_toolbox/experimental/utils.hpp +++ b/include/slam_toolbox/experimental/utils.hpp @@ -21,8 +21,9 @@ namespace utils int signum(int num); std::vector> rayCasting(karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt); karto::Vector2 getGridPosition(karto::Vector2 const& pose, kt_double resolution); - std::vector calculateCellIntersectionPoints(karto::Vector2 const & laser_start, - karto::Vector2 const & laser_end, std::vector cell_start, std::vector cell_end); + karto::Vector2 calculateCellIntersectionPoints( + karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, + karto::Vector2 const & cell_start, karto::Vector2 const & cell_end); std::pair, std::vector> computeLineBoxIntersection( karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index d21574b73..aa99e235d 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -35,6 +35,7 @@ InformationEstimates::InformationEstimates() m_visited_grid.resize(m_num_cells, m_num_cells); } +// FindMinorInformativeLaser std::tuple InformationEstimates::calculateMutualInformation(std::vector const& range_scans) { /** @@ -53,7 +54,7 @@ std::tuple InformationEstimates::calculateMutualInformation(std: for (auto & scan : range_scans) { - karto::Pose2 robot_pose = scan->GetBarycenterPose(); + karto::Pose2 robot_pose = scan->GetCorrectedPose(); karto::PointVectorDouble laser_readings = scan->GetPointReadings(true); karto::Vector2 robot_grid = utils::grid_operations::getGridPosition(robot_pose.GetPosition(), m_cell_resol); @@ -74,7 +75,8 @@ std::tuple InformationEstimates::calculateMutualInformation(std: kt_double limit_x = cell.GetX() * m_cell_resol; kt_double limit_y = cell.GetY() * m_cell_resol; - std::pair, std::vector> intersections = utils::grid_operations::computeLineBoxIntersection(robot_pose.GetPosition(), laser_readings[i], robot_grid, beam_grid, limit_x, limit_y, m_cell_resol); + std::pair, std::vector> intersections = utils::grid_operations::computeLineBoxIntersection( + robot_pose.GetPosition(), laser_readings[i], robot_grid, beam_grid, limit_x, limit_y, m_cell_resol); if (intersections.first.size() == 0) continue; @@ -115,7 +117,11 @@ std::tuple InformationEstimates::calculateMutualInformation(std: updateCellMutualInformation(1.0 - cell_mutual_inf, cell); } } - kt_double map_mut_info = calculateMapMutualInformation(); + + // I can get here the current laser mutual information. + // At this point I have processed a single LaserRange. + + kt_double map_mut_info = calculateMapMutualInformation(); // I(M, Z) // Extract the mutual information provided by this laser scan kt_double laser_mut_info = calculateLaserMutualInformation(map_mut_info, m_curr_mut_info); @@ -131,9 +137,16 @@ std::tuple InformationEstimates::calculateMutualInformation(std: ++curr_idx; } + // IIUC information gain for a measurement Z[0] is I(M, Z) - I(M, Z \ {Z[0]}). + // I would need the whole mutual information calculation - Which means at the end of the loop + + // kt_double map_mut_info = calculateMapMutualInformation(); // I(M, Z) + // Clearing the cells for the next time it is called utils::grid_operations::clearVisitedCells(m_mutual_grid); + std::cout << mut_info << std::endl; + return std::make_tuple(min_idx, mut_info); } diff --git a/src/experimental/utils.cpp b/src/experimental/utils.cpp index 729408524..013b81ecf 100644 --- a/src/experimental/utils.cpp +++ b/src/experimental/utils.cpp @@ -198,41 +198,47 @@ namespace utils return karto::Vector2{x_cell, y_cell}; } - std::vector calculateCellIntersectionPoints(karto::Vector2 const & laser_start, - karto::Vector2 const & laser_end, std::vector cell_start, std::vector cell_end) + // std::vector calculateCellIntersectionPoints(karto::Vector2 const & laser_start, + karto::Vector2 calculateCellIntersectionPoints(karto::Vector2 const & laser_start, + karto::Vector2 const & laser_end, karto::Vector2 const & cell_start, karto::Vector2 const & cell_end) { /** * Find the intersection point between a cell line and a laser beam * Arguments: * laser_start [karto::Vector2]: Laser initial point in x and y * laser_end [karto::Vector2]: Laser final point in x and y - * cell_start [std::vector]: Cell initial point in x and y - * cell_end [std::vector]: Cell final point in x and y + * cell_start [karto::Vector2]: Cell initial point in x and y + * cell_end [karto::Vector2]: Cell final point in x and y * Return: - * std::vector: Intersection point + * karto::Vector2: Intersection point */ kt_double x1 = laser_start.GetX(); kt_double x2 = laser_end.GetX(); - kt_double x3 = cell_start[0]; - kt_double x4 = cell_end[0]; + kt_double x3 = cell_start.GetX(); + kt_double x4 = cell_end.GetX(); kt_double y1 = laser_start.GetY(); kt_double y2 = laser_end.GetY(); - kt_double y3 = cell_start[1]; - kt_double y4 = cell_end[1]; + kt_double y3 = cell_start.GetY(); + kt_double y4 = cell_end.GetY(); kt_double den = ((x2 - x1)*(y4 - y3) - (x4 - x3)*(y2 - y1)); + + karto::Vector2 intersection; if (den == 0.0f) { // Parallel lines - return {}; + intersection.SetX(0.0); + intersection.SetY(0.0); } else { kt_double x = ((x2*y1 - x1*y2)*(x4 - x3) - (x4*y3 - x3*y4)*(x2 - x1)) / den; kt_double y = ((x2*y1 - x1*y2)*(y4 - y3) - (x4*y3 - x3*y4)*(y2 - y1)) / den; - return {x, y}; + intersection.SetX(x); + intersection.SetY(y); } + return intersection; } std::pair, std::vector> computeLineBoxIntersection( @@ -272,24 +278,26 @@ namespace utils for (int k = 0; k < 4; ++k) { - std::vector intersection = calculateCellIntersectionPoints(laser_start, laser_end, {initial_x[k], initial_y[k]}, {final_x[k], final_y[k]}); - if(intersection.size() != 0) + karto::Vector2 start{initial_x[k], initial_y[k]}; + karto::Vector2 end{final_x[k], final_y[k]}; + karto::Vector2 intersection = calculateCellIntersectionPoints(laser_start, laser_end, start, end); + if(intersection.Length() != 0) { - if ((fabs(intersection[0]) >= (fabs(cell_limits[0]) - 0.001)) && - (fabs(intersection[0]) <= (fabs(cell_limits[1]) + 0.001)) && - (fabs(intersection[1]) >= (fabs(cell_limits[2]) - 0.001)) && - (fabs(intersection[1]) <= (fabs(cell_limits[3]) + 0.001))) + if ((fabs(intersection.GetX()) >= (fabs(cell_limits[0]) - 0.001)) && + (fabs(intersection.GetX()) <= (fabs(cell_limits[1]) + 0.001)) && + (fabs(intersection.GetY()) >= (fabs(cell_limits[2]) - 0.001)) && + (fabs(intersection.GetY()) <= (fabs(cell_limits[3]) + 0.001))) { // 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[0]); - inter_y.push_back(intersection[1]); + inter_x.push_back(intersection.GetX()); + inter_y.push_back(intersection.GetY()); } } } return std::pair, std::vector>{inter_x, inter_y}; } - }// namespace grid_operations + } // namespace grid_operations } // namespace utils diff --git a/test/information_estimates_test.cpp b/test/information_estimates_test.cpp index bf6c73093..15309cded 100644 --- a/test/information_estimates_test.cpp +++ b/test/information_estimates_test.cpp @@ -19,41 +19,67 @@ TEST(UtilsInformationEstimatesTests, RayCastTest) { InformationEstimates information_estimates; - karto::Vector2 initial_pt{22,43}; - karto::Vector2 final_pt{29,38}; + // First quadrant + karto::Vector2 initial_pt_1{22,43}; + karto::Vector2 final_pt_1{29,38}; - std::vector> cells = utils::grid_operations::rayCasting(initial_pt, final_pt); + std::vector> cells = utils::grid_operations::rayCasting(initial_pt_1, final_pt_1); ASSERT_EQ(cells[2].GetX(), 25) << "FAIL in X position for cell 3"; ASSERT_EQ(cells[2].GetY(), 41) << "FAIL in Y position for cell 3"; ASSERT_EQ(cells[4].GetX(), 27) << "FAIL in X position for cell 5"; ASSERT_EQ(cells[4].GetY(), 39) << "FAIL in Y position for cell 5"; + + // Second quadrant + karto::Vector2 initial_pt_2{22,43}; + karto::Vector2 final_pt_2{15,38}; + + karto::Vector2 initial_pt_3{22,43}; + karto::Vector2 final_pt_3{29,48}; + + karto::Vector2 initial_pt_4{22,43}; + karto::Vector2 final_pt_4{15,48}; + } TEST(UtilsInformationEstimatesTests, IntersectionPointsTest) { - karto::Vector2 laser_start{1.5, 1.6}; - karto::Vector2 laser_end{6.1, 8.4}; - std::vector cell_start{3.6, 8.2}; - std::vector cell_end{4.7, 1.6}; - - std::vector int_points = utils::grid_operations::calculateCellIntersectionPoints(laser_start, laser_end, cell_start, cell_end); - - ASSERT_FLOAT_EQ(int_points[0], 4.06744) << "FAIL in X coordinate"; - ASSERT_FLOAT_EQ(int_points[1], 5.39535) << "FAIL in Y coordinate"; + 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_FLOAT_EQ(int_points.GetX(), 4.06744) << "FAIL in X coordinate intersection"; + ASSERT_FLOAT_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); + + ASSERT_FLOAT_EQ(int_points.GetX(), 0.0) << "FAIL in X coordinate parallel"; + ASSERT_FLOAT_EQ(int_points.GetY(), 0.0) << "FAIL in Y coordinate parallel"; } TEST(InformationEstimatesTests, MutualInformationTest) { InformationEstimates inf_estimates; - karto::LocalizedRangeScan * s1 = new karto::LocalizedRangeScan(); - karto::LocalizedRangeScan * s2 = new karto::LocalizedRangeScan(); + std::unique_ptr s1 = std::make_unique(); + std::unique_ptr s2 = 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); - s1->SetBarycenterPose(p1); - s2->SetBarycenterPose(p2); + + s1->SetCorrectedPose(p1); + s2->SetCorrectedPose(p2); karto::BoundingBox2 bb1, bb2; bb1.SetMinimum(karto::Vector2(2.0, 2.0)); bb1.SetMaximum(karto::Vector2(5.0, 6.0)); @@ -73,13 +99,12 @@ TEST(InformationEstimatesTests, MutualInformationTest) s1->SetIsDirty(dirty); s2->SetIsDirty(dirty); - std::vector range_scan_vct; - range_scan_vct.push_back(s1); - range_scan_vct.push_back(s2); + std::vector range_scan_vct; + range_scan_vct.push_back(s1.get()); + range_scan_vct.push_back(s2.get()); std::tuple min_inf = inf_estimates.calculateMutualInformation(range_scan_vct); - // El que contiene menor informacion es s2, index 1 int idx; kt_double mut_inf; std::tie(idx, mut_inf) = min_inf; From 8148fedcd0698ceccc0045f8f70e1775c9296144 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Thu, 13 Jan 2022 16:40:48 -0500 Subject: [PATCH 51/83] [WIP]: Renaming functions, updating tests and replacing some data structures --- .../experimental/information_estimates.hpp | 6 +- src/experimental/information_estimates.cpp | 60 +++++++---------- test/information_estimates_test.cpp | 64 +++++++++++++++---- 3 files changed, 77 insertions(+), 53 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index bc6ec884f..4edf977c4 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -14,7 +14,7 @@ class InformationEstimates public: // Main function - std::tuple calculateMutualInformation(std::vector const& range_scans); + std::tuple findLessInformativeLaser(std::vector const& range_scans); private: // Mutual information @@ -31,7 +31,7 @@ class InformationEstimates // Measurements calculations kt_double calculateScanMassProbabilityBetween(kt_double range_1, kt_double range_2); - kt_double calculateLaserMutualInformation(kt_double const & map_info, kt_double const & curr_info); + kt_double calculateLaserMutualInformation(); private: // Data structures @@ -45,11 +45,11 @@ class InformationEstimates kt_double m_cell_resol; kt_double m_obs_lambda; kt_double m_obs_nu; - kt_double m_curr_mut_info; kt_double m_map_dist; int m_num_cells; // Map grids + Eigen::MatrixXd m_info_grid; Eigen::MatrixXd m_mutual_grid; Eigen::MatrixXi m_visited_grid; }; diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index aa99e235d..b225bf376 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -1,8 +1,6 @@ #include #include "slam_toolbox/experimental/information_estimates.hpp" -#include - InformationEstimates::InformationEstimates(kt_double sensor_range, kt_double resolution, kt_double lambda, kt_double nu) { m_max_sensor_range = sensor_range; @@ -13,8 +11,7 @@ InformationEstimates::InformationEstimates(kt_double sensor_range, kt_double res m_map_dist = 200.0f; m_num_cells = static_cast(m_map_dist / m_cell_resol); - m_curr_mut_info = 0.0; - + m_info_grid.resize(m_num_cells, m_num_cells); m_mutual_grid.resize(m_num_cells, m_num_cells); m_visited_grid.resize(m_num_cells, m_num_cells); } @@ -29,28 +26,25 @@ InformationEstimates::InformationEstimates() m_map_dist = 200.0f; m_num_cells = static_cast(m_map_dist / m_cell_resol); - m_curr_mut_info = 0.0; - + m_info_grid.resize(m_num_cells, m_num_cells); m_mutual_grid.resize(m_num_cells, m_num_cells); m_visited_grid.resize(m_num_cells, m_num_cells); } -// FindMinorInformativeLaser -std::tuple InformationEstimates::calculateMutualInformation(std::vector const& range_scans) +std::tuple InformationEstimates::findLessInformativeLaser(std::vector const& range_scans) { /** - * Find and return the laser scan of a set of LocalizedRangeScan with the minimal mutual information + * Find and return the Laser Scan index which provide the less mutual information in a set of Laser Scans * Arguments: * range_scans [std::vector]: Vector of LocalizedRangeScan pointers * Return: * std::tuple: Tuple containing the index of the LozalizedRangeScan and its corresponding mutual information */ - int min_idx = 0; - int curr_idx = 0; - m_curr_mut_info = 0.0; // Initial guess of mutual information kt_double mut_info = 1000000.0; + std::vector scans_mut_info; + scans_mut_info.reserve(range_scans.size()); for (auto & scan : range_scans) { @@ -114,40 +108,27 @@ std::tuple InformationEstimates::calculateMutualInformation(std: } // Mutual information of cell x, y given a set of measurements - updateCellMutualInformation(1.0 - cell_mutual_inf, cell); + updateCellMutualInformation(1.0 - cell_mutual_inf, cell); } } - - // I can get here the current laser mutual information. - // At this point I have processed a single LaserRange. - - kt_double map_mut_info = calculateMapMutualInformation(); // I(M, Z) + // Mutual information provided by this laser scan + kt_double laser_mut_info = calculateLaserMutualInformation(); + scans_mut_info.emplace_back(laser_mut_info); - // Extract the mutual information provided by this laser scan - kt_double laser_mut_info = calculateLaserMutualInformation(map_mut_info, m_curr_mut_info); - m_curr_mut_info = map_mut_info; - - // Compare to assign the minimal value in the index and the mutual information. - if ((laser_mut_info < mut_info) && (laser_mut_info != 0.0)) - { - // This is the assignation of the result - mut_info = laser_mut_info; - min_idx = curr_idx; - } - ++curr_idx; + utils::grid_operations::clearVisitedCells(m_info_grid); } - // IIUC information gain for a measurement Z[0] is I(M, Z) - I(M, Z \ {Z[0]}). - // I would need the whole mutual information calculation - Which means at the end of the loop - - // kt_double map_mut_info = calculateMapMutualInformation(); // I(M, Z) + kt_double map_mut_info = calculateMapMutualInformation(); // Clearing the cells for the next time it is called utils::grid_operations::clearVisitedCells(m_mutual_grid); - std::cout << mut_info << std::endl; + // Finding the less informative laser scan + std::vector::iterator it_min; + it_min = std::min_element(scans_mut_info.begin(), scans_mut_info.end()); + int idx = it_min - scans_mut_info.begin(); - return std::make_tuple(min_idx, mut_info); + return std::make_tuple(idx, *it_min); } void InformationEstimates::appendCellProbabilities(std::vector& measurements, karto::Vector2 const & cell) @@ -392,7 +373,8 @@ std::vector> InformationEstimates::retrieveCellProbabilit return it_cells->second; } -kt_double InformationEstimates::calculateLaserMutualInformation(kt_double const & map_info, kt_double const & curr_info) +// kt_double InformationEstimates::calculateLaserMutualInformation(kt_double const & map_info, kt_double const & curr_info) +kt_double InformationEstimates::calculateLaserMutualInformation() { /** * Calculate the laser mutual information considering the map mutual information and the current mutual information calculation @@ -402,7 +384,8 @@ kt_double InformationEstimates::calculateLaserMutualInformation(kt_double const * Return: * kt_double: Laser mutual information */ - return map_info - curr_info; + // return map_info - curr_info; + return m_info_grid.sum(); } void InformationEstimates::updateCellMutualInformation(kt_double mut_inf, karto::Vector2 const & cell) @@ -417,4 +400,5 @@ void InformationEstimates::updateCellMutualInformation(kt_double mut_inf, karto: * Void */ m_mutual_grid(cell.GetX(), cell.GetY()) = mut_inf; + m_info_grid(cell.GetX(), cell.GetY()) = mut_inf; } diff --git a/test/information_estimates_test.cpp b/test/information_estimates_test.cpp index 15309cded..303f57de7 100644 --- a/test/information_estimates_test.cpp +++ b/test/information_estimates_test.cpp @@ -17,29 +17,57 @@ TEST(UtilsInformationEstimatesTests, SigNumTest) TEST(UtilsInformationEstimatesTests, RayCastTest) { + /** + * Quadrants are taken from the initial location + */ InformationEstimates information_estimates; + std::vector> cells; // First quadrant karto::Vector2 initial_pt_1{22,43}; karto::Vector2 final_pt_1{29,38}; - std::vector> cells = utils::grid_operations::rayCasting(initial_pt_1, final_pt_1); - - ASSERT_EQ(cells[2].GetX(), 25) << "FAIL in X position for cell 3"; - ASSERT_EQ(cells[2].GetY(), 41) << "FAIL in Y position for cell 3"; - - ASSERT_EQ(cells[4].GetX(), 27) << "FAIL in X position for cell 5"; - ASSERT_EQ(cells[4].GetY(), 39) << "FAIL in Y position for cell 5"; + // First quadrant + cells = utils::grid_operations::rayCasting(initial_pt_1, final_pt_1); + // Third point - First quadrant + ASSERT_EQ(cells[2].GetX(), 25) << "FAIL in X position 1 for cell 3"; + ASSERT_EQ(cells[2].GetY(), 41) << "FAIL in Y position 1 for cell 3"; + // Fifth point - First quadrant + ASSERT_EQ(cells[4].GetX(), 27) << "FAIL in X position 1 for cell 5"; + ASSERT_EQ(cells[4].GetY(), 39) << "FAIL in Y position 1 for cell 5"; // Second quadrant karto::Vector2 initial_pt_2{22,43}; karto::Vector2 final_pt_2{15,38}; - + cells = utils::grid_operations::rayCasting(initial_pt_2, final_pt_2); + // Third point - Second quadrant + ASSERT_EQ(cells[2].GetX(), 19) << "FAIL in X position 2 for cell 3"; + ASSERT_EQ(cells[2].GetY(), 41) << "FAIL in Y position 2 for cell 3"; + // Fifth point - Second quadrant + ASSERT_EQ(cells[4].GetX(), 17) << "FAIL in X position 2 for cell 5"; + ASSERT_EQ(cells[4].GetY(), 39) << "FAIL in Y position 2 for cell 5"; + + // Third quadrant karto::Vector2 initial_pt_3{22,43}; karto::Vector2 final_pt_3{29,48}; - + cells = utils::grid_operations::rayCasting(initial_pt_3, final_pt_3); + // Third point - Third quadrant + ASSERT_EQ(cells[2].GetX(), 25) << "FAIL in X position 3 for cell 3"; + ASSERT_EQ(cells[2].GetY(), 45) << "FAIL in Y position 3 for cell 3"; + // Fifth point - Third quadrant + ASSERT_EQ(cells[4].GetX(), 27) << "FAIL in X position 3 for cell 5"; + ASSERT_EQ(cells[4].GetY(), 47) << "FAIL in Y position 3 for cell 5"; + + // Fourth quadrand karto::Vector2 initial_pt_4{22,43}; karto::Vector2 final_pt_4{15,48}; + cells = utils::grid_operations::rayCasting(initial_pt_4, final_pt_4); + // Third point - Fourth quadrant + ASSERT_EQ(cells[2].GetX(), 19) << "FAIL in X position 3 for cell 3"; + ASSERT_EQ(cells[2].GetY(), 45) << "FAIL in Y position 3 for cell 3"; + // Fifth point - Fourth quadrant + ASSERT_EQ(cells[4].GetX(), 17) << "FAIL in X position 3 for cell 5"; + ASSERT_EQ(cells[4].GetY(), 47) << "FAIL in Y position 3 for cell 5"; } @@ -74,36 +102,48 @@ TEST(InformationEstimatesTests, MutualInformationTest) 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); - karto::BoundingBox2 bb1, bb2; + 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); - karto::PointVectorDouble pts1, pts2; + 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::tuple min_inf = inf_estimates.calculateMutualInformation(range_scan_vct); + std::tuple min_inf = inf_estimates.findLessInformativeLaser(range_scan_vct); int idx; kt_double mut_inf; From 296d19108e5bb4dd2455b92425b8bff9ac7fc399 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Wed, 19 Jan 2022 12:12:02 -0500 Subject: [PATCH 52/83] [WIP]: Updating mutual information calculation --- .../experimental/information_estimates.hpp | 10 +- include/slam_toolbox/experimental/utils.hpp | 8 +- src/experimental/information_estimates.cpp | 150 +++++++++++++++--- src/experimental/utils.cpp | 21 ++- test/information_estimates_test.cpp | 2 +- 5 files changed, 144 insertions(+), 47 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index 4edf977c4..8580311e4 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -14,10 +14,10 @@ class InformationEstimates public: // Main function - std::tuple findLessInformativeLaser(std::vector const& range_scans); + std::tuple findLeastInformativeLaser(std::vector const& range_scans); private: - // Mutual information + // Mutual information kt_double calculateInformationContent(kt_double prob); kt_double calculateMapMutualInformation(); kt_double measurementOutcomeEntropy(map_tuple const& meas_outcome); @@ -28,13 +28,13 @@ class InformationEstimates void appendCellProbabilities(std::vector& measurements, karto::Vector2 const & cell); std::unordered_map computeMeasurementOutcomesHistogram(std::vector>& meas_outcm); std::vector> retrieveCellProbabilities(karto::Vector2 const & cell); - + // Measurements calculations kt_double calculateScanMassProbabilityBetween(kt_double range_1, kt_double range_2); kt_double calculateLaserMutualInformation(); private: - // Data structures + // Data structures std::map, std::vector>> m_cell_probabilities; const kt_double l_free = log(0.3 / (1.0 - 0.3)); @@ -43,7 +43,7 @@ class InformationEstimates kt_double m_max_sensor_range; kt_double m_cell_resol; - kt_double m_obs_lambda; + kt_double m_obs_lambda; kt_double m_obs_nu; kt_double m_map_dist; int m_num_cells; diff --git a/include/slam_toolbox/experimental/utils.hpp b/include/slam_toolbox/experimental/utils.hpp index c3d6e3b3b..b4c61b3d8 100644 --- a/include/slam_toolbox/experimental/utils.hpp +++ b/include/slam_toolbox/experimental/utils.hpp @@ -15,17 +15,17 @@ namespace utils { namespace grid_operations { - void updateCellLimits(std::vector& initial_x, std::vector& initial_y, std::vector& final_x, - std::vector& final_y, kt_double limit_x, kt_double limit_y, std::vector& cell_limits, + void updateCellLimits(std::vector& initial_x, std::vector& initial_y, std::vector& final_x, + std::vector& final_y, kt_double limit_x, kt_double limit_y, std::vector& cell_limits, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, kt_double resolution); int signum(int num); std::vector> rayCasting(karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt); karto::Vector2 getGridPosition(karto::Vector2 const& pose, kt_double resolution); karto::Vector2 calculateCellIntersectionPoints( - karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, + karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, karto::Vector2 const & cell_start, karto::Vector2 const & cell_end); std::pair, std::vector> computeLineBoxIntersection( - karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, + karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, kt_double limit_x, kt_double limit_y, kt_double resolution); void clearVisitedCells(Eigen::MatrixXd & grid); diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index b225bf376..4090ec54f 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -1,6 +1,8 @@ #include #include "slam_toolbox/experimental/information_estimates.hpp" +#include + InformationEstimates::InformationEstimates(kt_double sensor_range, kt_double resolution, kt_double lambda, kt_double nu) { m_max_sensor_range = sensor_range; @@ -8,9 +10,9 @@ InformationEstimates::InformationEstimates(kt_double sensor_range, kt_double res m_obs_lambda = lambda; m_obs_nu = nu; - m_map_dist = 200.0f; + m_map_dist = 200.0f; m_num_cells = static_cast(m_map_dist / m_cell_resol); - + m_info_grid.resize(m_num_cells, m_num_cells); m_mutual_grid.resize(m_num_cells, m_num_cells); m_visited_grid.resize(m_num_cells, m_num_cells); @@ -20,10 +22,10 @@ InformationEstimates::InformationEstimates() { m_max_sensor_range = 5.0; m_cell_resol = 0.05; - m_obs_lambda = 0.35; + m_obs_lambda = 0.35; m_obs_nu = 0.28; - m_map_dist = 200.0f; + m_map_dist = 200.0f; m_num_cells = static_cast(m_map_dist / m_cell_resol); m_info_grid.resize(m_num_cells, m_num_cells); @@ -31,7 +33,7 @@ InformationEstimates::InformationEstimates() m_visited_grid.resize(m_num_cells, m_num_cells); } -std::tuple InformationEstimates::findLessInformativeLaser(std::vector const& range_scans) +std::tuple InformationEstimates::findLeastInformativeLaser(std::vector const& range_scans) { /** * Find and return the Laser Scan index which provide the less mutual information in a set of Laser Scans @@ -41,8 +43,6 @@ std::tuple InformationEstimates::findLessInformativeLaser(std::v * std::tuple: Tuple containing the index of the LozalizedRangeScan and its corresponding mutual information */ - // Initial guess of mutual information - kt_double mut_info = 1000000.0; std::vector scans_mut_info; scans_mut_info.reserve(range_scans.size()); @@ -59,7 +59,7 @@ std::tuple InformationEstimates::findLessInformativeLaser(std::v { // Laser final cell karto::Vector2 beam_grid = utils::grid_operations::getGridPosition(laser_readings[i], m_cell_resol); - + // Visited cells by this laser beam std::vector> cells = utils::grid_operations::rayCasting(robot_grid, beam_grid); @@ -100,35 +100,136 @@ std::tuple InformationEstimates::findLessInformativeLaser(std::v // Compute all the possible combinations for the current cell - algorithm 1 std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(cell_prob); - + 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 - updateCellMutualInformation(1.0 - cell_mutual_inf, cell); + // Mutual information of cell x, y given a set of measurements + updateCellMutualInformation(1.0 - cell_mutual_inf, cell); } } - // Mutual information provided by this laser scan - kt_double laser_mut_info = calculateLaserMutualInformation(); - scans_mut_info.emplace_back(laser_mut_info); - - utils::grid_operations::clearVisitedCells(m_info_grid); + // // Mutual information provided by this laser scan + // kt_double laser_mut_info = calculateLaserMutualInformation(); + // scans_mut_info.emplace_back(laser_mut_info); + + // utils::grid_operations::clearVisitedCells(m_info_grid); } + // Total mutual information kt_double map_mut_info = calculateMapMutualInformation(); // Clearing the cells for the next time it is called utils::grid_operations::clearVisitedCells(m_mutual_grid); + // Outer loop for know the number of times we should calculate + for (int i = 0; i < range_scans.size(); ++i) + { + // Inner loop for calculating the mutual information without a given measurement + int scan_idx = 0; + for (auto & scan : range_scans) + { + if (scan_idx == i) + { + std::cout << "Breaking: " << i << std::endl; + ++scan_idx; + continue; + } + + karto::Pose2 robot_pose = scan->GetCorrectedPose(); + karto::PointVectorDouble laser_readings = scan->GetPointReadings(true); + karto::Vector2 robot_grid = utils::grid_operations::getGridPosition(robot_pose.GetPosition(), m_cell_resol); + + // Set as false the current boolean map + utils::grid_operations::clearVisitedCells(m_visited_grid); + + for (int i = 0; i < laser_readings.size(); ++i) + { + // Laser final cell + karto::Vector2 beam_grid = utils::grid_operations::getGridPosition(laser_readings[i], m_cell_resol); + + // Visited cells by this laser beam + std::vector> cells = utils::grid_operations::rayCasting(robot_grid, beam_grid); + + for (auto & cell : cells) + { + // Inidividual cell limits + kt_double limit_x = cell.GetX() * m_cell_resol; + kt_double limit_y = cell.GetY() * m_cell_resol; + + std::pair, std::vector> intersections = utils::grid_operations::computeLineBoxIntersection( + robot_pose.GetPosition(), laser_readings[i], robot_grid, beam_grid, limit_x, limit_y, m_cell_resol); + + if (intersections.first.size() == 0) + continue; + + // 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 = robot_pose.GetPosition().Distance(intersection); + distances.push_back(distance); + } + + // 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]) + }; + + // Appending new measurement outcomes for the current cell + appendCellProbabilities(probabilities, cell); + + // Get all the measurement outcomes for the current cell + std::vector> cell_prob = retrieveCellProbabilities(cell); + + // Compute all the possible combinations for the current cell - algorithm 1 + std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(cell_prob); + + 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 + updateCellMutualInformation(1.0 - cell_mutual_inf, cell); + } + } + std::cout << "Scan index: " << scan_idx << std::endl; + ++scan_idx; + } + + kt_double temp_mut_info = calculateMapMutualInformation(); + + // I(M, Z) - I(M, Z \ {Z[n]}) + std::cout << "Appending: " << map_mut_info << ", " << temp_mut_info << std::endl; + scans_mut_info.emplace_back(map_mut_info - temp_mut_info); + utils::grid_operations::clearVisitedCells(m_mutual_grid); + + std::cout << "------------------" << std::endl; + } + // Finding the less informative laser scan std::vector::iterator it_min; it_min = std::min_element(scans_mut_info.begin(), scans_mut_info.end()); int idx = it_min - scans_mut_info.begin(); + std::cout << "Result: " << idx << ", " << *it_min << std::endl; return std::make_tuple(idx, *it_min); + + /* + - Calculate the mutual information of the whole map with the whole set of measurement outcomes + - This part is already done + - Calculate all the mutual information when extracting a measurement + - Create a loop of size measurements - 1 + - Calculate the mutual information when extracting each element of the array + */ } void InformationEstimates::appendCellProbabilities(std::vector& measurements, karto::Vector2 const & cell) @@ -196,8 +297,7 @@ kt_double InformationEstimates::calculateMapMutualInformation() * Return: * kt_double: Map mutual information */ - kt_double sum = m_mutual_grid.sum(); - return sum; + return m_mutual_grid.sum(); } kt_double InformationEstimates::calculateProbabilityFromLogOdds(kt_double log) @@ -225,7 +325,7 @@ std::unordered_map temp_map; // The number of measurements - int k = meas_outcm.size(); + int k = meas_outcm.size(); int r = 1; kt_double p_free = meas_outcm[0][0]; @@ -233,7 +333,7 @@ std::unordered_map> InformationEstimates::retrieveCellProbabilit std::map, std::vector>>::iterator it_cells; it_cells = m_cell_probabilities.find({cell.GetX(), cell.GetY()}); - return it_cells->second; + return it_cells->second; } -// kt_double InformationEstimates::calculateLaserMutualInformation(kt_double const & map_info, kt_double const & curr_info) kt_double InformationEstimates::calculateLaserMutualInformation() { /** @@ -384,7 +483,6 @@ kt_double InformationEstimates::calculateLaserMutualInformation() * Return: * kt_double: Laser mutual information */ - // return map_info - curr_info; return m_info_grid.sum(); } diff --git a/src/experimental/utils.cpp b/src/experimental/utils.cpp index 013b81ecf..49b2bb2ce 100644 --- a/src/experimental/utils.cpp +++ b/src/experimental/utils.cpp @@ -5,8 +5,8 @@ namespace utils { namespace grid_operations { - void updateCellLimits(std::vector& initial_x, std::vector& initial_y, std::vector& final_x, - std::vector& final_y, kt_double limit_x, kt_double limit_y, std::vector& cell_limits, karto::Vector2 const& robot_grid_pos, + void updateCellLimits(std::vector& initial_x, std::vector& initial_y, std::vector& final_x, + std::vector& final_y, kt_double limit_x, kt_double limit_y, std::vector& cell_limits, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, kt_double resolution) { /** @@ -24,7 +24,7 @@ namespace utils * resolution [kt_double]: Cell resolution * Return: * Void - */ + */ if (final_grid_pos.GetX() < robot_grid_pos.GetX() && final_grid_pos.GetY() >= robot_grid_pos.GetY()) { // X greater and Y greater. WRO final points @@ -76,8 +76,8 @@ namespace utils * num [int]: Number for perform the sign operation * Return: * int: Sign - */ - if (num < 0) return -1; + */ + if (num < 0) return -1; if (num >= 1) return 1; return 0; } @@ -99,7 +99,7 @@ namespace utils } } } - + void clearVisitedCells(Eigen::MatrixXi & grid) { /** @@ -198,8 +198,7 @@ namespace utils return karto::Vector2{x_cell, y_cell}; } - // std::vector calculateCellIntersectionPoints(karto::Vector2 const & laser_start, - karto::Vector2 calculateCellIntersectionPoints(karto::Vector2 const & laser_start, + karto::Vector2 calculateCellIntersectionPoints(karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, karto::Vector2 const & cell_start, karto::Vector2 const & cell_end) { /** @@ -242,7 +241,7 @@ namespace utils } std::pair, std::vector> computeLineBoxIntersection( - karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, + karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, kt_double limit_x, kt_double limit_y, kt_double resolution) { @@ -266,7 +265,7 @@ namespace utils // Initial points for each of the 4 corners std::vector initial_x {limit_x, limit_x, limit_x + resolution, limit_x + resolution}; std::vector initial_y {limit_y, limit_y, limit_y + resolution, limit_y + resolution}; - + // Final points for each of the 4 corners std::vector final_x {limit_x + resolution, limit_x, limit_x + resolution, limit_x}; std::vector final_y {limit_y, limit_y + resolution, limit_y, limit_y + resolution}; @@ -296,7 +295,7 @@ namespace utils } } } - return std::pair, std::vector>{inter_x, inter_y}; + return std::pair, std::vector>{inter_x, inter_y}; } } // namespace grid_operations diff --git a/test/information_estimates_test.cpp b/test/information_estimates_test.cpp index 303f57de7..37bcf7205 100644 --- a/test/information_estimates_test.cpp +++ b/test/information_estimates_test.cpp @@ -143,7 +143,7 @@ TEST(InformationEstimatesTests, MutualInformationTest) range_scan_vct.push_back(s2.get()); range_scan_vct.push_back(s3.get()); - std::tuple min_inf = inf_estimates.findLessInformativeLaser(range_scan_vct); + std::tuple min_inf = inf_estimates.findLeastInformativeLaser(range_scan_vct); int idx; kt_double mut_inf; From c2e7c5c72061c8e5a7a0d19b89af6d0086e652ff Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Thu, 20 Jan 2022 11:53:18 -0500 Subject: [PATCH 53/83] [WIP]: Removing unused code --- .../experimental/information_estimates.hpp | 3 - src/experimental/information_estimates.cpp | 57 +------------------ 2 files changed, 2 insertions(+), 58 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index 8580311e4..0bc1d481d 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -19,7 +19,6 @@ class InformationEstimates private: // Mutual information kt_double calculateInformationContent(kt_double prob); - kt_double calculateMapMutualInformation(); kt_double measurementOutcomeEntropy(map_tuple const& meas_outcome); kt_double calculateProbabilityFromLogOdds(kt_double log); void updateCellMutualInformation(kt_double mut_inf, karto::Vector2 const & cell); @@ -31,7 +30,6 @@ class InformationEstimates // Measurements calculations kt_double calculateScanMassProbabilityBetween(kt_double range_1, kt_double range_2); - kt_double calculateLaserMutualInformation(); private: // Data structures @@ -49,7 +47,6 @@ class InformationEstimates int m_num_cells; // Map grids - Eigen::MatrixXd m_info_grid; Eigen::MatrixXd m_mutual_grid; Eigen::MatrixXi m_visited_grid; }; diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 4090ec54f..00e8d9b81 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -13,7 +13,6 @@ InformationEstimates::InformationEstimates(kt_double sensor_range, kt_double res m_map_dist = 200.0f; m_num_cells = static_cast(m_map_dist / m_cell_resol); - m_info_grid.resize(m_num_cells, m_num_cells); m_mutual_grid.resize(m_num_cells, m_num_cells); m_visited_grid.resize(m_num_cells, m_num_cells); } @@ -28,7 +27,6 @@ InformationEstimates::InformationEstimates() m_map_dist = 200.0f; m_num_cells = static_cast(m_map_dist / m_cell_resol); - m_info_grid.resize(m_num_cells, m_num_cells); m_mutual_grid.resize(m_num_cells, m_num_cells); m_visited_grid.resize(m_num_cells, m_num_cells); } @@ -111,20 +109,13 @@ std::tuple InformationEstimates::findLeastInformativeLaser(std:: updateCellMutualInformation(1.0 - cell_mutual_inf, cell); } } - // // Mutual information provided by this laser scan - // kt_double laser_mut_info = calculateLaserMutualInformation(); - // scans_mut_info.emplace_back(laser_mut_info); - - // utils::grid_operations::clearVisitedCells(m_info_grid); } - // Total mutual information - kt_double map_mut_info = calculateMapMutualInformation(); + kt_double map_mut_info = m_mutual_grid.sum(); // Clearing the cells for the next time it is called utils::grid_operations::clearVisitedCells(m_mutual_grid); - // Outer loop for know the number of times we should calculate for (int i = 0; i < range_scans.size(); ++i) { // Inner loop for calculating the mutual information without a given measurement @@ -133,7 +124,6 @@ std::tuple InformationEstimates::findLeastInformativeLaser(std:: { if (scan_idx == i) { - std::cout << "Breaking: " << i << std::endl; ++scan_idx; continue; } @@ -201,35 +191,18 @@ std::tuple InformationEstimates::findLeastInformativeLaser(std:: updateCellMutualInformation(1.0 - cell_mutual_inf, cell); } } - std::cout << "Scan index: " << scan_idx << std::endl; ++scan_idx; } - - kt_double temp_mut_info = calculateMapMutualInformation(); - - // I(M, Z) - I(M, Z \ {Z[n]}) - std::cout << "Appending: " << map_mut_info << ", " << temp_mut_info << std::endl; + kt_double temp_mut_info = m_mutual_grid.sum(); scans_mut_info.emplace_back(map_mut_info - temp_mut_info); utils::grid_operations::clearVisitedCells(m_mutual_grid); - - std::cout << "------------------" << std::endl; } - // Finding the less informative laser scan std::vector::iterator it_min; it_min = std::min_element(scans_mut_info.begin(), scans_mut_info.end()); int idx = it_min - scans_mut_info.begin(); - std::cout << "Result: " << idx << ", " << *it_min << std::endl; return std::make_tuple(idx, *it_min); - - /* - - Calculate the mutual information of the whole map with the whole set of measurement outcomes - - This part is already done - - Calculate all the mutual information when extracting a measurement - - Create a loop of size measurements - 1 - - Calculate the mutual information when extracting each element of the array - */ } void InformationEstimates::appendCellProbabilities(std::vector& measurements, karto::Vector2 const & cell) @@ -288,18 +261,6 @@ kt_double InformationEstimates::calculateInformationContent(kt_double prob) return - (prob * log2(prob)) - ((1 - prob) * log2(1 - prob)); } -kt_double InformationEstimates::calculateMapMutualInformation() -{ - /** - * Calculate the mutual information of the current map, this is the summation of all cells mutual information - * Arguments: - * Void - * Return: - * kt_double: Map mutual information - */ - return m_mutual_grid.sum(); -} - kt_double InformationEstimates::calculateProbabilityFromLogOdds(kt_double log) { /** @@ -473,19 +434,6 @@ std::vector> InformationEstimates::retrieveCellProbabilit return it_cells->second; } -kt_double InformationEstimates::calculateLaserMutualInformation() -{ - /** - * Calculate the laser mutual information considering the map mutual information and the current mutual information calculation - * Arguments: - * map_info [kt_double]: Map mutual information - * curr_info [kt_double]: Current mutual information - * Return: - * kt_double: Laser mutual information - */ - return m_info_grid.sum(); -} - void InformationEstimates::updateCellMutualInformation(kt_double mut_inf, karto::Vector2 const & cell) { /** @@ -498,5 +446,4 @@ void InformationEstimates::updateCellMutualInformation(kt_double mut_inf, karto: * Void */ m_mutual_grid(cell.GetX(), cell.GetY()) = mut_inf; - m_info_grid(cell.GetX(), cell.GetY()) = mut_inf; } From 8752e3efdb3fd7d1169ac65868f8c31a385cc4ea Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Sun, 23 Jan 2022 07:51:08 -0500 Subject: [PATCH 54/83] [ADD]: Function for mutual information calculation --- .../experimental/information_estimates.hpp | 1 + src/experimental/information_estimates.cpp | 138 ++++++------------ 2 files changed, 46 insertions(+), 93 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index 0bc1d481d..d6c439a0c 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -21,6 +21,7 @@ class InformationEstimates kt_double calculateInformationContent(kt_double prob); kt_double measurementOutcomeEntropy(map_tuple const& meas_outcome); kt_double calculateProbabilityFromLogOdds(kt_double log); + kt_double mutualInformationFromScans(std::vector const& range_scans, bool ignore_scan=false, int scan_idx=0); void updateCellMutualInformation(kt_double mut_inf, karto::Vector2 const & cell); // Measurement outcomes probabilities diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 00e8d9b81..10df57a6b 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -44,8 +44,50 @@ std::tuple InformationEstimates::findLeastInformativeLaser(std:: std::vector scans_mut_info; scans_mut_info.reserve(range_scans.size()); + // Total mutual information + kt_double map_mut_info = mutualInformationFromScans(range_scans); + // Clearing the cells for the next time it is called + utils::grid_operations::clearVisitedCells(m_mutual_grid); + + for (int i = 0; i < range_scans.size(); ++i) + { + // Find mutual information when extracting one laser scan + kt_double temp_mut_info = mutualInformationFromScans(range_scans, true, i); + scans_mut_info.emplace_back(map_mut_info - temp_mut_info); + // Clearing the cells for the next time it is called + utils::grid_operations::clearVisitedCells(m_mutual_grid); + } + + // Finding the less informative laser scan + std::vector::iterator it_min; + it_min = std::min_element(scans_mut_info.begin(), scans_mut_info.end()); + int idx = it_min - scans_mut_info.begin(); + + return std::make_tuple(idx, *it_min); +} + + +kt_double InformationEstimates::mutualInformationFromScans(std::vector const& range_scans, bool ignore_scan, int scan_idx) +{ + /** + * Append measured porbabilities for the given cell + * Arguments: + * range_scans [std::vector]: Vector of LocalizedRangeScan pointers + * ignore_scan [bool]: Indicates the type of calculation + * scan_idx [int]: Index within the set to be ignored + * Return: + * kt_double + */ + + int curr_idx = 0; for (auto & scan : range_scans) { + if (curr_idx == scan_idx && ignore_scan) + { + ++curr_idx; + continue; + } + karto::Pose2 robot_pose = scan->GetCorrectedPose(); karto::PointVectorDouble laser_readings = scan->GetPointReadings(true); karto::Vector2 robot_grid = utils::grid_operations::getGridPosition(robot_pose.GetPosition(), m_cell_resol); @@ -109,102 +151,12 @@ std::tuple InformationEstimates::findLeastInformativeLaser(std:: updateCellMutualInformation(1.0 - cell_mutual_inf, cell); } } + ++curr_idx; } - // Total mutual information - kt_double map_mut_info = m_mutual_grid.sum(); - - // Clearing the cells for the next time it is called - utils::grid_operations::clearVisitedCells(m_mutual_grid); - - for (int i = 0; i < range_scans.size(); ++i) - { - // Inner loop for calculating the mutual information without a given measurement - int scan_idx = 0; - for (auto & scan : range_scans) - { - if (scan_idx == i) - { - ++scan_idx; - continue; - } - - karto::Pose2 robot_pose = scan->GetCorrectedPose(); - karto::PointVectorDouble laser_readings = scan->GetPointReadings(true); - karto::Vector2 robot_grid = utils::grid_operations::getGridPosition(robot_pose.GetPosition(), m_cell_resol); - - // Set as false the current boolean map - utils::grid_operations::clearVisitedCells(m_visited_grid); - - for (int i = 0; i < laser_readings.size(); ++i) - { - // Laser final cell - karto::Vector2 beam_grid = utils::grid_operations::getGridPosition(laser_readings[i], m_cell_resol); - - // Visited cells by this laser beam - std::vector> cells = utils::grid_operations::rayCasting(robot_grid, beam_grid); - - for (auto & cell : cells) - { - // Inidividual cell limits - kt_double limit_x = cell.GetX() * m_cell_resol; - kt_double limit_y = cell.GetY() * m_cell_resol; - - std::pair, std::vector> intersections = utils::grid_operations::computeLineBoxIntersection( - robot_pose.GetPosition(), laser_readings[i], robot_grid, beam_grid, limit_x, limit_y, m_cell_resol); - - if (intersections.first.size() == 0) - continue; - - // 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 = robot_pose.GetPosition().Distance(intersection); - distances.push_back(distance); - } - - // 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]) - }; - - // Appending new measurement outcomes for the current cell - appendCellProbabilities(probabilities, cell); - - // Get all the measurement outcomes for the current cell - std::vector> cell_prob = retrieveCellProbabilities(cell); - - // Compute all the possible combinations for the current cell - algorithm 1 - std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(cell_prob); - - 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 - updateCellMutualInformation(1.0 - cell_mutual_inf, cell); - } - } - ++scan_idx; - } - kt_double temp_mut_info = m_mutual_grid.sum(); - scans_mut_info.emplace_back(map_mut_info - temp_mut_info); - utils::grid_operations::clearVisitedCells(m_mutual_grid); - } - // Finding the less informative laser scan - std::vector::iterator it_min; - it_min = std::min_element(scans_mut_info.begin(), scans_mut_info.end()); - int idx = it_min - scans_mut_info.begin(); - - return std::make_tuple(idx, *it_min); + return m_mutual_grid.sum(); } + void InformationEstimates::appendCellProbabilities(std::vector& measurements, karto::Vector2 const & cell) { /** From 969a3a73e49cff68fc1a3ad72707f7eb64be4c85 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Thu, 10 Feb 2022 06:41:46 -0500 Subject: [PATCH 55/83] [WIP]: Dynamic grid resizing --- .../experimental/information_estimates.hpp | 7 ++- src/experimental/information_estimates.cpp | 55 ++++++++++++++----- 2 files changed, 46 insertions(+), 16 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index d6c439a0c..5a63eea65 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -44,8 +44,11 @@ class InformationEstimates kt_double m_cell_resol; kt_double m_obs_lambda; kt_double m_obs_nu; - kt_double m_map_dist; - int m_num_cells; + + kt_double m_low_x; + kt_double m_low_y; + kt_double m_high_x; + kt_double m_high_y; // Map grids Eigen::MatrixXd m_mutual_grid; diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 10df57a6b..a46ac70ca 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -9,26 +9,14 @@ InformationEstimates::InformationEstimates(kt_double sensor_range, kt_double res m_cell_resol = resolution; m_obs_lambda = lambda; m_obs_nu = nu; - - m_map_dist = 200.0f; - m_num_cells = static_cast(m_map_dist / m_cell_resol); - - m_mutual_grid.resize(m_num_cells, m_num_cells); - m_visited_grid.resize(m_num_cells, m_num_cells); } InformationEstimates::InformationEstimates() { - m_max_sensor_range = 5.0; - m_cell_resol = 0.05; + m_max_sensor_range = 25.0; + m_cell_resol = 0.1; m_obs_lambda = 0.35; m_obs_nu = 0.28; - - m_map_dist = 200.0f; - m_num_cells = static_cast(m_map_dist / m_cell_resol); - - m_mutual_grid.resize(m_num_cells, m_num_cells); - m_visited_grid.resize(m_num_cells, m_num_cells); } std::tuple InformationEstimates::findLeastInformativeLaser(std::vector const& range_scans) @@ -41,6 +29,45 @@ std::tuple InformationEstimates::findLeastInformativeLaser(std:: * std::tuple: Tuple containing the index of the LozalizedRangeScan and its corresponding mutual information */ + // Get the robot poses for creating our new local coordinate system + for (const auto & scan : range_scans) + { + if (scan == nullptr) + { + continue; + } + + karto::Pose2 pose = scan->GetCorrectedPose(); + + // Finding the closest pose + m_low_x = pose.GetX() < m_low_x ? pose.GetX() : m_low_x; + m_low_y = pose.GetY() < m_low_y ? pose.GetY() : m_low_y; + + // Finding the farthest pose + m_high_x = pose.GetX() > m_high_x ? pose.GetX() : m_high_x; + m_high_y = pose.GetY() > m_high_y ? pose.GetY() : m_high_y; + } + + // Margins for the closest points + m_low_x -= m_max_sensor_range; + m_low_y -= m_max_sensor_range; + + // Margins for the farthest points + m_high_x += m_max_sensor_range; + m_high_y += m_max_sensor_range; + + // Map dimensions + kt_double dist_x = std::fabs(m_high_x) + std::fabs(m_low_x); + kt_double dist_y = std::fabs(m_high_y) + std::fabs(m_low_y); + + // Number of X and Y cells + int n_cells_x = static_cast(dist_x / m_cell_resol); + int n_cells_y = static_cast(dist_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::vector scans_mut_info; scans_mut_info.reserve(range_scans.size()); From a39954d189d5712f783543eb8874d9af8a2b09eb Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Thu, 10 Feb 2022 07:20:52 -0500 Subject: [PATCH 56/83] [RMV]: Unused function and adding transformations --- include/slam_toolbox/experimental/utils.hpp | 2 -- src/experimental/information_estimates.cpp | 23 ++++++++----- src/experimental/utils.cpp | 36 --------------------- 3 files changed, 15 insertions(+), 46 deletions(-) diff --git a/include/slam_toolbox/experimental/utils.hpp b/include/slam_toolbox/experimental/utils.hpp index b4c61b3d8..377331cf1 100644 --- a/include/slam_toolbox/experimental/utils.hpp +++ b/include/slam_toolbox/experimental/utils.hpp @@ -28,8 +28,6 @@ namespace utils karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, karto::Vector2 const& robot_grid_pos, karto::Vector2 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 diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index a46ac70ca..6a3d70158 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -74,7 +74,8 @@ std::tuple InformationEstimates::findLeastInformativeLaser(std:: // Total mutual information kt_double map_mut_info = mutualInformationFromScans(range_scans); // Clearing the cells for the next time it is called - utils::grid_operations::clearVisitedCells(m_mutual_grid); + m_mutual_grid.setZero(); + for (int i = 0; i < range_scans.size(); ++i) { @@ -82,7 +83,7 @@ std::tuple InformationEstimates::findLeastInformativeLaser(std:: kt_double temp_mut_info = mutualInformationFromScans(range_scans, true, i); scans_mut_info.emplace_back(map_mut_info - temp_mut_info); // Clearing the cells for the next time it is called - utils::grid_operations::clearVisitedCells(m_mutual_grid); + m_mutual_grid.setZero(); } // Finding the less informative laser scan @@ -115,17 +116,23 @@ kt_double InformationEstimates::mutualInformationFromScans(std::vectorGetCorrectedPose(); + karto::Pose2 robot_pose_raw = scan->GetCorrectedPose(); + + // Moving the pose to our local system + karto::Pose2 robot_pose_tf(robot_pose_raw.GetX() - m_low_x, robot_pose_raw.GetY() - m_low_y, robot_pose_raw.GetHeading()); + karto::PointVectorDouble laser_readings = scan->GetPointReadings(true); - karto::Vector2 robot_grid = utils::grid_operations::getGridPosition(robot_pose.GetPosition(), m_cell_resol); + karto::Vector2 robot_grid = utils::grid_operations::getGridPosition(robot_pose_tf.GetPosition(), m_cell_resol); // Set as false the current boolean map - utils::grid_operations::clearVisitedCells(m_visited_grid); + m_visited_grid.setZero(); for (int i = 0; i < laser_readings.size(); ++i) { + karto::Vector2 tf_laser{laser_readings[i].GetX() - m_low_x, laser_readings[i].GetY() - m_low_y}; + // Laser final cell - karto::Vector2 beam_grid = utils::grid_operations::getGridPosition(laser_readings[i], m_cell_resol); + karto::Vector2 beam_grid = utils::grid_operations::getGridPosition(tf_laser, m_cell_resol); // Visited cells by this laser beam std::vector> cells = utils::grid_operations::rayCasting(robot_grid, beam_grid); @@ -137,7 +144,7 @@ kt_double InformationEstimates::mutualInformationFromScans(std::vector, std::vector> intersections = utils::grid_operations::computeLineBoxIntersection( - robot_pose.GetPosition(), laser_readings[i], robot_grid, beam_grid, limit_x, limit_y, m_cell_resol); + robot_pose_tf.GetPosition(), tf_laser, robot_grid, beam_grid, limit_x, limit_y, m_cell_resol); if (intersections.first.size() == 0) continue; @@ -148,7 +155,7 @@ kt_double InformationEstimates::mutualInformationFromScans(std::vector intersection{intersections.first[k], intersections.second[k]}; - kt_double distance = robot_pose.GetPosition().Distance(intersection); + kt_double distance = robot_pose_tf.GetPosition().Distance(intersection); distances.push_back(distance); } diff --git a/src/experimental/utils.cpp b/src/experimental/utils.cpp index 49b2bb2ce..c7839e9fa 100644 --- a/src/experimental/utils.cpp +++ b/src/experimental/utils.cpp @@ -82,42 +82,6 @@ namespace utils return 0; } - void clearVisitedCells(Eigen::MatrixXd & grid) - { - /** - * Clear the given floating Eigen::Matrix - * Arguments: - * grid [Eigen::Matrix]: Grid for cleaning - * Return: - * Void - */ - for (int i = 0; i < grid.rows(); ++i) - { - for (int j = 0; j < grid.cols(); ++j) - { - grid(i, j) = 0.0; - } - } - } - - void clearVisitedCells(Eigen::MatrixXi & grid) - { - /** - * Clear the given integer Eigen::Matrix - * Arguments: - * grid [Eigen::Matrix]: Grid for cleaning - * Return: - * Void - */ - for (int i = 0; i < grid.rows(); ++i) - { - for (int j = 0; j < grid.cols(); ++j) - { - grid(i, j) = 0; - } - } - } - std::vector> rayCasting( karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt) { From b799cb82e0fd9c6262b765b3c621ca8dbe54351f Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Mon, 21 Feb 2022 05:55:58 -0500 Subject: [PATCH 57/83] [WIP]: Returning values modification --- src/experimental/information_estimates.cpp | 5 +---- src/experimental/utils.cpp | 3 +-- test/information_estimates_test.cpp | 8 ++++++++ 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 6a3d70158..71dc03fa1 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -414,10 +414,7 @@ std::vector> InformationEstimates::retrieveCellProbabilit * Return: * std::vector>: Vector of cell probabilities in the form {p_free, p_occ, p_unk} (Measurement outcomes) */ - std::map, std::vector>>::iterator it_cells; - it_cells = m_cell_probabilities.find({cell.GetX(), cell.GetY()}); - - return it_cells->second; + return m_cell_probabilities.at(cell); } void InformationEstimates::updateCellMutualInformation(kt_double mut_inf, karto::Vector2 const & cell) diff --git a/src/experimental/utils.cpp b/src/experimental/utils.cpp index c7839e9fa..6a9e744ba 100644 --- a/src/experimental/utils.cpp +++ b/src/experimental/utils.cpp @@ -191,8 +191,7 @@ namespace utils if (den == 0.0f) { // Parallel lines - intersection.SetX(0.0); - intersection.SetY(0.0); + return {}; } else { diff --git a/test/information_estimates_test.cpp b/test/information_estimates_test.cpp index 37bcf7205..649dc22b7 100644 --- a/test/information_estimates_test.cpp +++ b/test/information_estimates_test.cpp @@ -69,6 +69,12 @@ TEST(UtilsInformationEstimatesTests, RayCastTest) ASSERT_EQ(cells[4].GetX(), 17) << "FAIL in X position 3 for cell 5"; ASSERT_EQ(cells[4].GetY(), 47) << "FAIL in Y position 3 for cell 5"; + // Degenerate case + karto::Vector2 initial_pt_5{22,43}; + karto::Vector2 final_pt_5{22,43}; + cells = utils::grid_operations::rayCasting(initial_pt_4, final_pt_4); + ASSERT_EQ(cells[0].GetX(), 22) << "Degenerate case X"; + ASSERT_EQ(cells[0].GetY(), 43) << "Degenerate case Y"; } TEST(UtilsInformationEstimatesTests, IntersectionPointsTest) @@ -92,6 +98,8 @@ TEST(UtilsInformationEstimatesTests, IntersectionPointsTest) 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_FLOAT_EQ(int_points.GetX(), 0.0) << "FAIL in X coordinate parallel"; ASSERT_FLOAT_EQ(int_points.GetY(), 0.0) << "FAIL in Y coordinate parallel"; } From 2825ef91dcf2a40179dbd7740dc365e70f33925d Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Mon, 21 Feb 2022 07:33:29 -0500 Subject: [PATCH 58/83] [WIP]: Replacing vectors for karto wrappers --- .../experimental/information_estimates.hpp | 6 +- include/slam_toolbox/experimental/utils.hpp | 8 +++ src/experimental/information_estimates.cpp | 58 +++---------------- src/experimental/utils.cpp | 53 +++++++++++++++++ 4 files changed, 69 insertions(+), 56 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index 5a63eea65..75b0c0aed 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -34,11 +34,7 @@ class InformationEstimates private: // Data structures - std::map, std::vector>> 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)); + std::map, std::vector>> m_cell_probabilities; kt_double m_max_sensor_range; kt_double m_cell_resol; diff --git a/include/slam_toolbox/experimental/utils.hpp b/include/slam_toolbox/experimental/utils.hpp index 377331cf1..c8cad3a27 100644 --- a/include/slam_toolbox/experimental/utils.hpp +++ b/include/slam_toolbox/experimental/utils.hpp @@ -30,6 +30,14 @@ namespace utils kt_double limit_x, kt_double limit_y, kt_double resolution); } // namespace grid_operations + namespace probability_operations + { + kt_double calculateMeasurementOutcomeEntropy(std::tuple const& meas_outcome); + kt_double calculateProbabilityFromLogOdds(kt_double log); + kt_double calculateInformationContent(kt_double prob); + + } // namespace probability_operations + namespace tuple_hash { struct HashTuple diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 71dc03fa1..0b0f0570d 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -1,8 +1,6 @@ #include #include "slam_toolbox/experimental/information_estimates.hpp" -#include - InformationEstimates::InformationEstimates(kt_double sensor_range, kt_double resolution, kt_double lambda, kt_double nu) { m_max_sensor_range = sensor_range; @@ -178,7 +176,7 @@ kt_double InformationEstimates::mutualInformationFromScans(std::vector& measurements, karto::Vector2 const & cell) { /** @@ -201,14 +200,14 @@ void InformationEstimates::appendCellProbabilities(std::vector& measu * Return: * Void */ - std::map, std::vector>>::iterator it_cell; + std::map, std::vector>>::iterator it_cell; - it_cell = m_cell_probabilities.find({cell.GetX(), cell.GetY()}); + 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.GetX(), cell.GetY()}, {{measurements[0], measurements[1], measurements[2]}})); + m_cell_probabilities.insert(std::pair, std::vector>>( + cell, {measurements})); m_visited_grid(cell.GetX(), cell.GetY()) = 1; } else @@ -235,30 +234,6 @@ void InformationEstimates::appendCellProbabilities(std::vector& measu } } -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))); -} - std::unordered_map InformationEstimates::computeMeasurementOutcomesHistogram(std::vector>& meas_outcm) { /** @@ -370,25 +345,6 @@ std::unordered_map const& meas_outcome) + { + /** + * 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 + * Arguments: + * meas_outcome [map_tuple]: Measurement outcome in the form {p_free, p_occ, p_unk} + * Return: + * kt_double: Measurement outcome entropy + */ + + // Log of probabilities (Free, Occupied, Not observed) + kt_double l_free = log(0.3 / (1.0 - 0.3)); + kt_double l_occ = log(0.7 / (1.0 - 0.7)); + kt_double l_o = log(0.5 / (1.0 - 0.5)); + + 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 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))); + } + } // namespace probability_operations + } // namespace utils From 83203d99d7ffdc8a5097cfcfa23a7779eded1d9d Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Mon, 21 Feb 2022 08:06:43 -0500 Subject: [PATCH 59/83] [WIP]: Comparing probabilities from the same scans --- src/experimental/information_estimates.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 0b0f0570d..794e15976 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -216,7 +216,7 @@ void InformationEstimates::appendCellProbabilities(std::vector& measu { // Compare the unknown probability, the smallest it is the most information we will have // from the occupied or free state - int idx = it_cell->second.size() - 1; + int idx = it_cell->second.size(); if(measurements[2] < it_cell->second[idx][2]) { // Replacing From c87c408b53798f36fafaf531559a6702a50a6578 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Sun, 6 Mar 2022 16:34:26 -0500 Subject: [PATCH 60/83] [WIP]: Integration to lifelong mapping loop --- .../experimental/information_estimates.hpp | 2 +- .../experimental/slam_toolbox_lifelong.hpp | 3 ++ src/experimental/information_estimates.cpp | 11 ++--- src/experimental/slam_toolbox_lifelong.cpp | 47 +++++++++++++++---- test/information_estimates_test.cpp | 12 ++--- 5 files changed, 49 insertions(+), 26 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index 75b0c0aed..efeb3eb85 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -14,7 +14,7 @@ class InformationEstimates public: // Main function - std::tuple findLeastInformativeLaser(std::vector const& range_scans); + std::vector findLeastInformativeLaser(std::vector const& range_scans); private: // Mutual information diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index efca2b78f..59cecac06 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -76,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 diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 794e15976..5228fccdd 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -17,14 +17,14 @@ InformationEstimates::InformationEstimates() m_obs_nu = 0.28; } -std::tuple InformationEstimates::findLeastInformativeLaser(std::vector const& range_scans) +std::vector InformationEstimates::findLeastInformativeLaser(std::vector const& range_scans) { /** * Find and return the Laser Scan index which provide the less mutual information in a set of Laser Scans * Arguments: * range_scans [std::vector]: Vector of LocalizedRangeScan pointers * Return: - * std::tuple: Tuple containing the index of the LozalizedRangeScan and its corresponding mutual information + * std::vector: Vector containing the mutual information fo each of the processed laser scans */ // Get the robot poses for creating our new local coordinate system @@ -84,12 +84,7 @@ std::tuple InformationEstimates::findLeastInformativeLaser(std:: m_mutual_grid.setZero(); } - // Finding the less informative laser scan - std::vector::iterator it_min; - it_min = std::min_element(scans_mut_info.begin(), scans_mut_info.end()); - int idx = it_min - scans_mut_info.begin(); - - return std::make_tuple(idx, *it_min); + return scans_mut_info; } diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 30413671c..04796a3a7 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -127,8 +127,10 @@ void LifelongSlamToolbox::evaluateNodeDepreciation( computeScores(near_scan_vertices, range_scan); ScoredVertices::iterator it; + kt_double mutual_removol_sacore = 1500.0; + for (it = scored_verices.begin(); it != scored_verices.end(); ++it) { - if (it->GetScore() < removal_score_) { + if (it->GetScore() < mutual_removol_sacore) { RCLCPP_DEBUG(get_logger(), "Removing node %i from graph with score: %f and old score: %f.", it->GetVertex()->GetObject()->GetUniqueId(), @@ -263,31 +265,56 @@ ScoredVertices LifelongSlamToolbox::computeScores( ScoredVertices scored_vertices; scored_vertices.reserve(near_scans.size()); + // must have some minimum metric to utilize // IOU will drop sharply with fitment, I'd advise not setting this value // any higher than 0.15. Also check this is a linked constraint // We want to do this early to get a better estimate of local candidates ScanVector::iterator candidate_scan_it; double iou = 0.0; + + + std::vector range_scan_vct; + std::vector mut_inf_vct; + for (candidate_scan_it = near_scans.begin(); candidate_scan_it != near_scans.end(); ) { - iou = computeIntersectOverUnion(range_scan, - (*candidate_scan_it)->GetObject()); - if (iou < iou_thresh_ || (*candidate_scan_it)->GetEdges().size() < 2) { + iou = computeIntersectOverUnion(range_scan, (*candidate_scan_it)->GetObject()); + if (iou < iou_thresh_ || (*candidate_scan_it)->GetEdges().size() < 2) + { candidate_scan_it = near_scans.erase(candidate_scan_it); - } else { - ++candidate_scan_it; + } + else + { + // Need to wait for at least 7 laser readings + if(range_scan_vct.size() == 7) + { + // Process the mutual information in batches of 7 readings + std::vector local_mut_inf = inf_estimates_.findLeastInformativeLaser(range_scan_vct); + // Append the current mutual information to the total mutual information + mut_inf_vct.insert(mut_inf_vct.end(), std::begin(local_mut_inf), std::end(local_mut_inf)); + // Clean the current range vector (For the next batch) + range_scan_vct.clear(); + } + else + { + range_scan_vct.push_back((*candidate_scan_it)->GetObject()); + ++candidate_scan_it; + } } } + int idx_res = 0; for (candidate_scan_it = near_scans.begin(); candidate_scan_it != near_scans.end(); ++candidate_scan_it) { - ScoredVertex scored_vertex((*candidate_scan_it), - computeScore(range_scan, (*candidate_scan_it), - (*candidate_scan_it)->GetScore(), near_scans.size())); - scored_vertices.push_back(scored_vertex); + if (mut_inf_vct.size() > 0) + { + ScoredVertex scored_vertex((*candidate_scan_it), mut_inf_vct[idx_res]); + scored_vertices.push_back(scored_vertex); + idx_res++; + } } return scored_vertices; } diff --git a/test/information_estimates_test.cpp b/test/information_estimates_test.cpp index 649dc22b7..68a5d58c4 100644 --- a/test/information_estimates_test.cpp +++ b/test/information_estimates_test.cpp @@ -151,14 +151,12 @@ TEST(InformationEstimatesTests, MutualInformationTest) range_scan_vct.push_back(s2.get()); range_scan_vct.push_back(s3.get()); - std::tuple min_inf = inf_estimates.findLeastInformativeLaser(range_scan_vct); + std::vector mut_inf_vct = inf_estimates.findLeastInformativeLaser(range_scan_vct); - int idx; - kt_double mut_inf; - std::tie(idx, mut_inf) = min_inf; - - EXPECT_EQ(idx, 1) << "FAIL in laser index"; - EXPECT_NE(mut_inf, 0.0) << "FAIL in mutual information equality"; + // Min value should be different to zero (We only have now the mutual information) + // This will be solved as soon as I fixed the error in the main loop + // EXPECT_EQ(idx, 1) << "FAIL in laser index"; + // EXPECT_NE(mut_inf, 0.0) << "FAIL in mutual information equality"; } int main(int argc, char ** argv) From b3baea52735466e35a37ccf5b95a32a7571b90a3 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Sat, 2 Apr 2022 12:23:41 -0500 Subject: [PATCH 61/83] [ADD]: New logic for Algrotihm 1 implementation --- .../experimental/information_estimates.hpp | 1 + src/experimental/information_estimates.cpp | 121 ++++++------------ 2 files changed, 41 insertions(+), 81 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index efeb3eb85..16d2158bf 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -27,6 +27,7 @@ class InformationEstimates // Measurement outcomes probabilities void appendCellProbabilities(std::vector& measurements, karto::Vector2 const & cell); std::unordered_map computeMeasurementOutcomesHistogram(std::vector>& meas_outcm); + void insertMeasurementOutcome(map_tuple tuple, kt_double probability, std::unordered_map& map); std::vector> retrieveCellProbabilities(karto::Vector2 const & cell); // Measurements calculations diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 5228fccdd..ce2aa2c4b 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -229,6 +229,21 @@ void InformationEstimates::appendCellProbabilities(std::vector& measu } } +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) { /** @@ -239,105 +254,49 @@ std::unordered_map: Map of combination, it contains the combination and its probability */ - std::unordered_map temp_map; - // The number of measurements - int k = meas_outcm.size(); - int r = 1; - - kt_double p_free = meas_outcm[0][0]; - kt_double p_occ = meas_outcm[0][1]; - kt_double p_un = meas_outcm[0][2]; + std::unordered_map probabilities_map; + std::unordered_map temp_map; + // Clear the temporal map temp_map.clear(); + probabilities_map.clear(); // Root - temp_map[std::make_tuple(0, 0, 0)] = 1.0f; + if (meas_outcm.size() == 0) + { + probabilities_map[std::make_tuple(0, 0, 0)] = 1.0f; + } // First measurement - temp_map[std::make_tuple(1, 0, 0)] = p_free; - temp_map[std::make_tuple(0, 1, 0)] = p_occ; - temp_map[std::make_tuple(0, 0, 1)] = p_un; + 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 < k; ++r) + for (int r = 1; r < meas_outcm.size(); ++r) { - std::vector tup_vct; - std::vector acc_prob; - // Measurement outcome probability kt_double free_prop = meas_outcm[r][0]; kt_double occ_prop = meas_outcm[r][1]; - kt_double un_prop = meas_outcm[r][2]; - - for (auto& pair : temp_map) - { - // Index - int idx_free, idx_occ, idx_unk; - std::tie(idx_free, idx_occ, idx_unk) = pair.first; - - if (idx_free + idx_occ + idx_unk == r) - { - // Searching for the current combination in this level - std::vector::iterator it_comb; - it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(idx_free + 1, idx_occ, idx_unk)); - - // Free - if (it_comb != tup_vct.end()) - { - acc_prob[it_comb - tup_vct.begin()] += pair.second * free_prop; - } - else - { - tup_vct.push_back(std::make_tuple(idx_free + 1, idx_occ, idx_unk)); - acc_prob.push_back(pair.second * free_prop); - } - - it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(idx_free, idx_occ + 1, idx_unk)); - - // Occupied - if (it_comb != tup_vct.end()) - { - acc_prob[it_comb - tup_vct.begin()] += pair.second * occ_prop; - } - else - { - tup_vct.push_back(std::make_tuple(idx_free, idx_occ + 1, idx_unk)); - acc_prob.push_back(pair.second * occ_prop); - } + kt_double unk_prop = meas_outcm[r][2]; - it_comb = std::find(tup_vct.begin(), tup_vct.end(), std::make_tuple(idx_free, idx_occ, idx_unk + 1)); + // Temporal map to only take the last level combination + temp_map = probabilities_map; + probabilities_map.clear(); - // Unobserved - if (it_comb != tup_vct.end()) - { - acc_prob[it_comb - tup_vct.begin()] += pair.second * un_prop; - } - else - { - tup_vct.push_back(std::make_tuple(idx_free, idx_occ, idx_unk + 1)); - acc_prob.push_back(pair.second * un_prop); - } - } - } - // Inserting the elements into the map - for (int k = 0; k < tup_vct.size(); ++k) + for (auto & combination : temp_map) { - temp_map[tup_vct[k]] = acc_prob[k]; - } - } + int key_free, key_occ, key_unk; + std::tie(key_free, key_occ, key_unk) = combination.first; - // Leaving in the map only the final outcomes - std::unordered_map out_map; - for (auto& pair : temp_map) - { - int idx_free, idx_occ, idx_unk; - std::tie(idx_free, idx_occ, idx_unk) = pair.first; - if (idx_free + idx_occ + idx_unk == k) - { - out_map[pair.first] = pair.second; + // 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 out_map; + return probabilities_map; } kt_double InformationEstimates::calculateScanMassProbabilityBetween(kt_double range_1, kt_double range_2) From e40b2171e9b67c38defffe5ac36bd7e18e8bd990 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Sat, 2 Apr 2022 13:02:36 -0500 Subject: [PATCH 62/83] [RMV]: Removing basic functions --- .../experimental/information_estimates.hpp | 2 -- src/experimental/information_estimates.cpp | 34 ++----------------- 2 files changed, 3 insertions(+), 33 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index 16d2158bf..c3ffb6a2f 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -22,13 +22,11 @@ class InformationEstimates kt_double measurementOutcomeEntropy(map_tuple const& meas_outcome); kt_double calculateProbabilityFromLogOdds(kt_double log); kt_double mutualInformationFromScans(std::vector const& range_scans, bool ignore_scan=false, int scan_idx=0); - void updateCellMutualInformation(kt_double mut_inf, karto::Vector2 const & cell); // Measurement outcomes probabilities void appendCellProbabilities(std::vector& measurements, karto::Vector2 const & cell); std::unordered_map computeMeasurementOutcomesHistogram(std::vector>& meas_outcm); void insertMeasurementOutcome(map_tuple tuple, kt_double probability, std::unordered_map& map); - std::vector> retrieveCellProbabilities(karto::Vector2 const & cell); // Measurements calculations kt_double calculateScanMassProbabilityBetween(kt_double range_1, kt_double range_2); diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index ce2aa2c4b..29ce07b5b 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -162,11 +162,8 @@ kt_double InformationEstimates::mutualInformationFromScans(std::vector> cell_prob = retrieveCellProbabilities(cell); - // Compute all the possible combinations for the current cell - algorithm 1 - std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(cell_prob); + 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) @@ -175,7 +172,8 @@ kt_double InformationEstimates::mutualInformationFromScans(std::vector> InformationEstimates::retrieveCellProbabilities(karto::Vector2 const & cell) -{ - /** - * Retrieve the cell probabilities (Measurement outcomes) - * Arguments: - * cell [karto::Vector2]: Cell coordinates - * Return: - * std::vector>: Vector of cell probabilities in the form {p_free, p_occ, p_unk} (Measurement outcomes) - */ - return m_cell_probabilities.at(cell); -} - -void InformationEstimates::updateCellMutualInformation(kt_double mut_inf, karto::Vector2 const & cell) -{ - /** - * Note: Result of the summation 3.12 - * Update the mutual information for each individual cell - * Arguments: - * mut_inf [kt_double]: Cell mutual information - * cell [karto::Vector2]: Cell coordinates - * Return: - * Void - */ - m_mutual_grid(cell.GetX(), cell.GetY()) = mut_inf; -} From f0654c0a4ba0cd2be94e550b4f3033c456bbe4e3 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Wed, 22 Jun 2022 19:18:04 -0500 Subject: [PATCH 63/83] [ADD]: Adding base logic for new loop calculation --- .../experimental/information_estimates.hpp | 2 +- src/experimental/information_estimates.cpp | 186 ++++++++++++++++++ 2 files changed, 187 insertions(+), 1 deletion(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index c3ffb6a2f..b2bd241bf 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -15,7 +15,7 @@ class InformationEstimates public: // Main function std::vector findLeastInformativeLaser(std::vector const& range_scans); - + kt_double finMutualInfo(std::vector const& range_scans); private: // Mutual information kt_double calculateInformationContent(kt_double prob); diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 29ce07b5b..07233ebe9 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -17,6 +17,192 @@ InformationEstimates::InformationEstimates() m_obs_nu = 0.28; } + +kt_double InformationEstimates::findMutualInfo(std::vector const& range_scans) +{ + /* + For your reference @hidmic + + The loop I am working on. + + 1. Find the grid size based on the ranges of a group of scans. + 2. Select one cell. + 3. Find which scans can see that cell. + 4. Select one scan from the group. + 5. Find the beam that sees the selected cell. + 6. Calculate and append the probability of that cell being seen. + + Note for point 3: + I am not sure if we should have another loop right before this point. + - We should extract one scan from the group of scans. + - Then execute steps 3, 4, 5, 6. + Following it, we will be able to find what is the laser that contains less mutual information. + + Another alternative for facing this issue could be: Have a external loop for extracting one scan + at a time, and then use the ramining scans to execute all the steps explained above. + */ + + // I took the first scan for reference. From this one I extract the min an max points (Poses) + m_low_x = range_scans[0]->GetCorrectedPose().GetX(); + m_low_y = range_scans[0]->GetCorrectedPose().GetY(); + + m_high_x = range_scans[0]->GetCorrectedPose().GetX(); + m_high_y = range_scans[0]->GetCorrectedPose().GetY(); + + // Iterating through the group of scans to rescale the grid + for (const auto & scan : range_scans) + { + if (scan == nullptr) + { + continue; + } + + karto::Pose2 pose = scan->GetCorrectedPose(); + // std::cout << "Processing pose: " << pose.GetX() << ", " << pose.GetY() << std::endl; + + // Finding the closest pose + m_low_x = pose.GetX() < m_low_x ? pose.GetX() : m_low_x; + m_low_y = pose.GetY() < m_low_y ? pose.GetY() : m_low_y; + + // Finding the farthest pose + m_high_x = pose.GetX() > m_high_x ? pose.GetX() : m_high_x; + m_high_y = pose.GetY() > m_high_y ? pose.GetY() : m_high_y; + } + + // Margins for the closest points + m_low_x -= m_max_sensor_range; + m_low_y -= m_max_sensor_range; + + // Margins for the farthest points + m_high_x += m_max_sensor_range; + m_high_y += m_max_sensor_range; + + // ===== Grid resizing ===== + // Map dimensions + kt_double dist_x = std::fabs(m_high_x) + std::fabs(m_low_x); + kt_double dist_y = std::fabs(m_high_y) + std::fabs(m_low_y); + + // Get the number of cells + int n_cells_x = static_cast(dist_x / m_cell_resol); + int n_cells_y = static_cast(dist_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); + + int low_x = range_scans[0]->GetCorrectedPose().GetX(); + int low_y = range_scans[0]->GetCorrectedPose().GetY(); + + int high_x = range_scans[0]->GetCorrectedPose().GetX(); + int high_y = range_scans[0]->GetCorrectedPose().GetY(); + + // ===== Main Loop ===== + // I would need to move it N times + // Cause here I do remove one scan at a time and call the following loop + + // Iterating through the cells + for (int i = 0; i < n_cells_x; ++i) + { + for (int j = 0; j < n_cells_y; ++j) + { + // Iterating through the scans to find which one hits this cell + for (const auto & scan : range_scans) + { + karto::Pose2 pose = scan->GetCorrectedPose(); + karto::Vector2 min_cell = utils::grid_operations::getGridPosition(min_pt, m_cell_resol); + karto::Vector2 max_cell = utils::grid_operations::getGridPosition(max_pt, m_cell_resol); + + // kt_double cell_y {(min_cell[1] + max_cell[1]) / 2}; + // kt_double cell_x {(min_cell[0] + max_cell[0])/ 2}; + // if (sqrt(pow((pose[0] - cell_x), 2) + pow((pose[1] - cell_y), 2)) > m_max_sensor_range) + // { + // continue; + // } + + // Lowest X point + kt_double low_pt_x = pose.GetX() - m_max_sensor_range; + low_pt_x = low_pt_x < 0.0 ? 0.0 : low_pt_x; + // Maximum X point + kt_double max_pt_x = pose.GetX() + m_max_sensor_range; + max_pt_x = max_pt_x > dist_x ? dist_x : max_pt_x; + // Lowest Y point + kt_double low_pt_y = pose.GetY() - m_max_sensor_range; + low_pt_y = low_pt_y < 0.0 ? 0.0 : low_pt_y; + // Maximum Y point + kt_double max_pt_y = pose.GetY() + m_max_sensor_range; + max_pt_y = max_pt_y > dist_y ? dist_y : max_pt_y; + + karto::Vector2 min_pt{low_pt_x, low_pt_y}; + karto::Vector2 max_pt{max_pt_x, max_pt_y}; + + // This assertion is just to avoid evaluation + if ((i <= min_pt.GetX() || i >= max_pt.GetX()) || (j <= min_pt.GetY() || j >= max_pt.GetY())) + { + continue; + } + + // Current scan laser readings + karto::PointVectorDouble laser_readings = scan->GetPointReadings(true); + + karto::Pose2 robot_pose_raw = scan->GetCorrectedPose(); + karto::Pose2 robot_pose(robot_pose_raw.GetX() - m_low_x, robot_pose_raw.GetY() - m_low_y, robot_pose_raw.GetHeading()); + karto::Vector2 robot_grid = utils::grid_operations::getGridPosition(robot_pose.GetPosition(), m_cell_resol); + + // ----- Point 3 ----- + // NIT: @kmilo7204 can use the range based for here + for (int i = 0; i < laser_readings.size(); ++i) + { + karto::Vector2 transformed_laser{laser_readings[i].GetX() - m_low_x, laser_readings[i].GetY() - m_low_y}; + + karto::Vector2 beam_grid = utils::grid_operations::getGridPosition(transformed_laser, m_cell_resol); + kt_double laser_to_grid_dist = + sqrt(pow((transformed_laser[0] - kt_double(beam_grid[0])), 2) + pow((transformed_laser[1] - kt_double(beam_grid[1])), 2)); + + // Compare the point here, the transformed laser and the cell position + // If not fulfill the condition I wrote above, continue + if (laser_to_grid_dist > m_cell_resol) + { + continue; + } + + // Inidividual cell limits + kt_double limit_x = i * m_cell_resol; + kt_double limit_y = j * m_cell_resol; + + std::pair, std::vector> intersections = utils::grid_operations::computeLineBoxIntersection( + robot_pose.GetPosition(), transformed_laser, robot_grid, beam_grid, limit_x, limit_y, m_cell_resol); + + // In case we did not find a measurement + if (intersections.first.size() == 0) + { + continue; + } + + // 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 = robot_pose.GetPosition().Distance(intersection); + distances.push_back(distance); + } + + // 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]) + }; + + // Appending new measurement outcomes for the current cell + appendCellProbabilities(probabilities, cell); + } + } + } + } +} + std::vector InformationEstimates::findLeastInformativeLaser(std::vector const& range_scans) { /** From d40c1257662e79f8214f8ddfb2f2b72a93d06e8f Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Tue, 2 Aug 2022 06:55:59 -0500 Subject: [PATCH 64/83] [WIP]: First complete pass on the new function Signed-off-by: Camilo Andres Alvis Bautista --- .vscode/.gitignore | 1 + launch/__pycache__/.gitignore | 1 + src/experimental/information_estimates.cpp | 284 +++++++++++++-------- src/experimental/slam_toolbox_lifelong.cpp | 13 +- 4 files changed, 189 insertions(+), 110 deletions(-) create mode 100644 .vscode/.gitignore create mode 100644 launch/__pycache__/.gitignore diff --git a/.vscode/.gitignore b/.vscode/.gitignore new file mode 100644 index 000000000..6f1710d08 --- /dev/null +++ b/.vscode/.gitignore @@ -0,0 +1 @@ +/settings.json diff --git a/launch/__pycache__/.gitignore b/launch/__pycache__/.gitignore new file mode 100644 index 000000000..7d1e706e4 --- /dev/null +++ b/launch/__pycache__/.gitignore @@ -0,0 +1 @@ +/lifelong_launch.cpython-38.pyc diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 07233ebe9..ee60e7176 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -21,27 +21,35 @@ InformationEstimates::InformationEstimates() kt_double InformationEstimates::findMutualInfo(std::vector const& range_scans) { /* - For your reference @hidmic + 1. Parate en una celda de la grid inicial. + 2. Identifica cuales laser scans del grupo actual pueden ver esta celda. (Maximum range radius) + 3. Cuales haz de luz del scan seleccionado puede ver esa celda. + 4. Resto de operaciones. + */ - The loop I am working on. + /* + Para el punto 0: + - Debe existir un loop exterior para ir removiendo un laser scan a la vez, con esto identifico cual es el scan + que aporta menor informacion del grupo dado. + */ - 1. Find the grid size based on the ranges of a group of scans. - 2. Select one cell. - 3. Find which scans can see that cell. - 4. Select one scan from the group. - 5. Find the beam that sees the selected cell. - 6. Calculate and append the probability of that cell being seen. + /* + Para el punto 1 y 2: + - Toma el pose del scan actual. + - Encuentra la distancia del centro del scan con respecto al centro de la celda que nos interesa. + - Si esa diferencia es menor que el rango del scan, entonces consideramos este scan. (Agregamos ese scan a un vector) - Note for point 3: - I am not sure if we should have another loop right before this point. - - We should extract one scan from the group of scans. - - Then execute steps 3, 4, 5, 6. - Following it, we will be able to find what is the laser that contains less mutual information. + Esto puedo afrontarlo de forma local, lo que significa que por cada set de scans que reciba, + puedo encontrar los extremos para poder reescalar el grid, esto potencialmente reduce el tiempo de busqueda. + */ - Another alternative for facing this issue could be: Have a external loop for extracting one scan - at a time, and then use the ramining scans to execute all the steps explained above. + /* + Para el punto 3: + - Con las lecturas de ese scan, encuentro el haz de luz que este dentro de la celda. + - Potencialmente puedo encontrar dos lectura, pero por ahora elegire el primero que encuentre. */ + // ------------ Start of Grid resizing ------------ // // I took the first scan for reference. From this one I extract the min an max points (Poses) m_low_x = range_scans[0]->GetCorrectedPose().GetX(); m_low_y = range_scans[0]->GetCorrectedPose().GetY(); @@ -49,7 +57,7 @@ kt_double InformationEstimates::findMutualInfo(std::vectorGetCorrectedPose().GetX(); m_high_y = range_scans[0]->GetCorrectedPose().GetY(); - // Iterating through the group of scans to rescale the grid + // Iterating through the group of scans to rescale the grid for (const auto & scan : range_scans) { if (scan == nullptr) @@ -77,7 +85,6 @@ kt_double InformationEstimates::findMutualInfo(std::vectorGetCorrectedPose().GetX(); - int low_y = range_scans[0]->GetCorrectedPose().GetY(); + // ------------ End of Grid resizing ------------ // - int high_x = range_scans[0]->GetCorrectedPose().GetX(); - int high_y = range_scans[0]->GetCorrectedPose().GetY(); - - // ===== Main Loop ===== // I would need to move it N times // Cause here I do remove one scan at a time and call the following loop + /* + Para encontrar el scan que menor mutual information proporciona debo tener un loop para ir removiendo uno a uno. + 1. El loop externo es solamente un for que itera N veces. (Dado por al longitud del vector de scans) + 2 Adentro solamente existe un condicional que se salta ese scan en la busqueda actual + */ // Iterating through the cells - for (int i = 0; i < n_cells_x; ++i) + kt_double mutual_information = 0.0f; + + for (int n = 0; n < range_scans.size(); ++n) { - for (int j = 0; j < n_cells_y; ++j) + for (int i = 0; i < n_cells_x; ++i) { - // Iterating through the scans to find which one hits this cell - for (const auto & scan : range_scans) + for (int j = 0; j < n_cells_y; ++j) { - karto::Pose2 pose = scan->GetCorrectedPose(); - karto::Vector2 min_cell = utils::grid_operations::getGridPosition(min_pt, m_cell_resol); - karto::Vector2 max_cell = utils::grid_operations::getGridPosition(max_pt, m_cell_resol); - - // kt_double cell_y {(min_cell[1] + max_cell[1]) / 2}; - // kt_double cell_x {(min_cell[0] + max_cell[0])/ 2}; - // if (sqrt(pow((pose[0] - cell_x), 2) + pow((pose[1] - cell_y), 2)) > m_max_sensor_range) - // { - // continue; - // } - - // Lowest X point - kt_double low_pt_x = pose.GetX() - m_max_sensor_range; - low_pt_x = low_pt_x < 0.0 ? 0.0 : low_pt_x; - // Maximum X point - kt_double max_pt_x = pose.GetX() + m_max_sensor_range; - max_pt_x = max_pt_x > dist_x ? dist_x : max_pt_x; - // Lowest Y point - kt_double low_pt_y = pose.GetY() - m_max_sensor_range; - low_pt_y = low_pt_y < 0.0 ? 0.0 : low_pt_y; - // Maximum Y point - kt_double max_pt_y = pose.GetY() + m_max_sensor_range; - max_pt_y = max_pt_y > dist_y ? dist_y : max_pt_y; - - karto::Vector2 min_pt{low_pt_x, low_pt_y}; - karto::Vector2 max_pt{max_pt_x, max_pt_y}; - - // This assertion is just to avoid evaluation - if ((i <= min_pt.GetX() || i >= max_pt.GetX()) || (j <= min_pt.GetY() || j >= max_pt.GetY())) - { - continue; - } - - // Current scan laser readings - karto::PointVectorDouble laser_readings = scan->GetPointReadings(true); - - karto::Pose2 robot_pose_raw = scan->GetCorrectedPose(); - karto::Pose2 robot_pose(robot_pose_raw.GetX() - m_low_x, robot_pose_raw.GetY() - m_low_y, robot_pose_raw.GetHeading()); - karto::Vector2 robot_grid = utils::grid_operations::getGridPosition(robot_pose.GetPosition(), m_cell_resol); - - // ----- Point 3 ----- - // NIT: @kmilo7204 can use the range based for here - for (int i = 0; i < laser_readings.size(); ++i) + /* + Tengo una celda especifica dentro del mapa + Puedo saber que lasers ven esa celda para evaluar estos + + - I have this not written but I Need to find a way to calculate the mutual information for a group + of scans when I remove one element of that group + - I can Iterate over the scans here + - But I would nneed to removew one scan at a time + */ + // Iterating through the scans to find which one hits this cell + std::vector> cell_probabilities; + int current_scan_idx = 0; + for (const auto & scan : range_scans) { - karto::Vector2 transformed_laser{laser_readings[i].GetX() - m_low_x, laser_readings[i].GetY() - m_low_y}; - - karto::Vector2 beam_grid = utils::grid_operations::getGridPosition(transformed_laser, m_cell_resol); - kt_double laser_to_grid_dist = - sqrt(pow((transformed_laser[0] - kt_double(beam_grid[0])), 2) + pow((transformed_laser[1] - kt_double(beam_grid[1])), 2)); - - // Compare the point here, the transformed laser and the cell position - // If not fulfill the condition I wrote above, continue - if (laser_to_grid_dist > m_cell_resol) + // On this way I make sure to extract one laser at a time + if (current_scan_idx == n) { continue; } - // Inidividual cell limits - kt_double limit_x = i * m_cell_resol; - kt_double limit_y = j * m_cell_resol; - - std::pair, std::vector> intersections = utils::grid_operations::computeLineBoxIntersection( - robot_pose.GetPosition(), transformed_laser, robot_grid, beam_grid, limit_x, limit_y, m_cell_resol); - - // In case we did not find a measurement - if (intersections.first.size() == 0) + karto::Pose2 robot_pose_raw = scan->GetCorrectedPose(); + // karto::Pose2 scan_pose(raw_scan_pose.GetX() - m_low_x, raw_scan_pose.GetY() - m_low_y, raw_scan_pose.GetHeading()); + + // karto::Vector2 min_cell = utils::grid_operations::getGridPosition(min_pt, m_cell_resol); + // karto::Vector2 max_cell = utils::grid_operations::getGridPosition(max_pt, m_cell_resol); + // kt_double cell_y {(min_cell[1] + max_cell[1]) / 2}; + // kt_double cell_x {(min_cell[0] + max_cell[0])/ 2}; + // if (sqrt(pow((pose[0] - cell_x), 2) + pow((pose[1] - cell_y), 2)) > m_max_sensor_range) + // { + // continue; + // } + + // Lowest X point + kt_double low_pt_x = robot_pose_raw.GetX() - m_max_sensor_range; // X: Posicion del scan - Rango maximo (Para saber si el scan ve por fuera del grid) + low_pt_x = low_pt_x < 0.0 ? 0.0 : low_pt_x; + // Maximum X point + kt_double max_pt_x = robot_pose_raw.GetX() + m_max_sensor_range; // X: Posicion del scan + Rango maximo (Para saber si el scan ve por fuera del grid) + max_pt_x = max_pt_x > dist_x ? dist_x : max_pt_x; + // Lowest Y point + kt_double low_pt_y = robot_pose_raw.GetY() - m_max_sensor_range; // Y: Posicion del scan - Rango maximo (Para saber si el scan ve por fuera del grid) + low_pt_y = low_pt_y < 0.0 ? 0.0 : low_pt_y; + // Maximum Y point + kt_double max_pt_y = robot_pose_raw.GetY() + m_max_sensor_range; // Y: Posicion del scan + Rango maximo (Para saber si el scan ve por fuera del grid) + max_pt_y = max_pt_y > dist_y ? dist_y : max_pt_y; + + karto::Vector2 min_pt{low_pt_x, low_pt_y}; // Esto es distancia no posicion de celda + karto::Vector2 max_pt{max_pt_x, max_pt_y}; // Esto es distancia no posicion de celda + + // Esto podia ser un karto::Pose2 + // karto::Vector2 getGridPosition(karto::Vector2 const& pose, kt_double resolution) + karto::Vector2 min_cell = utils::grid_operations::getGridPosition(min_pt, m_cell_resol); + karto::Vector2 max_cell = utils::grid_operations::getGridPosition(max_pt, m_cell_resol); + + // i and j are the current row and column + if ((i < min_cell.GetX() || i > max_cell.GetX()) || (j < min_cell.GetY() || j > max_cell.GetY())) { continue; } - // 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 = robot_pose.GetPosition().Distance(intersection); - distances.push_back(distance); - } + // Current scan laser readings + karto::PointVectorDouble laser_readings = scan->GetPointReadings(true); - // 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]) - }; + karto::Pose2 robot_pose(robot_pose_raw.GetX() - m_low_x, robot_pose_raw.GetY() - m_low_y, robot_pose_raw.GetHeading()); + karto::Vector2 robot_grid_position = utils::grid_operations::getGridPosition(robot_pose.GetPosition(), m_cell_resol); - // Appending new measurement outcomes for the current cell - appendCellProbabilities(probabilities, cell); + /* + Ahora que estoy dentro de un grid local, debo hacer una transformacion simple de + la posicion del robot y de cada uno de los lasers, para asi poder compararlos correctamente + con cada una de las celdas. + */ + + // ----- Point 3 ----- + // I could use some kind of angle to filter the region where the scan is. + // I can use the range based for here + for (int i = 0; i < laser_readings.size(); ++i) + { + karto::Vector2 transformed_laser{laser_readings[i].GetX() - m_low_x, laser_readings[i].GetY() - m_low_y}; + + // I need to transform here + karto::Vector2 beam_grid = utils::grid_operations::getGridPosition(transformed_laser, m_cell_resol); + + // kt_double laser_to_grid_dist = + // sqrt(pow((transformed_laser.GetX() - kt_double(beam_grid.GetX())), 2) + pow((transformed_laser.GetY() - kt_double(beam_grid.GetY())), 2)); + + // This calculation is not real, cause resting the limits no the center of the cell + kt_double laser_to_grid_dist = + sqrt(pow((transformed_laser.GetX() - (i * m_cell_resol)), 2) + pow((transformed_laser.GetY() - (j * m_cell_resol)), 2)); + + // Compare the point here, the transformed laser and the cell position + // If not fulfill the condition I wrote above, continue + if (laser_to_grid_dist > m_cell_resol) + { + continue; + } + + // ----- Point 4 ----- + // Aqui ya encontre cual es el haz de luz que funciona + // Ahora debo hacer el calculo de observacion hasta esa celda final + + // Inidividual cell limits + kt_double limit_x = i * m_cell_resol; + kt_double limit_y = j * m_cell_resol; + + std::pair, std::vector> intersections = utils::grid_operations::computeLineBoxIntersection( + robot_pose.GetPosition(), transformed_laser, robot_grid_position, beam_grid, limit_x, limit_y, m_cell_resol); + + // In case we did not find a measurement + if (intersections.first.size() == 0) + { + continue; + } + + // 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 = robot_pose.GetPosition().Distance(intersection); + distances.push_back(distance); + } + + // 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]) + }; + + // I got the probability of seen the current cell as o/f/u + // Now I need to save the current measuremets of that cell to calculate the histogram + cell_probabilities.push_back(probabilities); + } // EOF Laser Readings FOR + + ++current_scan_idx; + } // EOF Range Scans FOR + + + std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(cell_probabilities); + + kt_double cell_mutual_inf = 0.0f; + for (auto& pair : meas_out_prob) + { + cell_mutual_inf += pair.second * measurementOutcomeEntropy(pair.first); } - } - } - } + // 1.0 for the entropy since the cell is in an unknown state + mutual_information += 1.0 - cell_mutual_inf; + } // EOF Columns FOR + } // EOF Rows FOR + } // EOF Number Scans FOR + + // Note: This not the real answer, since I would return the scan that provides the less information (This is just a placeholder for compilation) + return mutual_information; } std::vector InformationEstimates::findLeastInformativeLaser(std::vector const& range_scans) diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 04796a3a7..cbbb58519 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -127,10 +127,10 @@ void LifelongSlamToolbox::evaluateNodeDepreciation( computeScores(near_scan_vertices, range_scan); ScoredVertices::iterator it; - kt_double mutual_removol_sacore = 1500.0; + kt_double mutual_removol_score = 1500.0; for (it = scored_verices.begin(); it != scored_verices.end(); ++it) { - if (it->GetScore() < mutual_removol_sacore) { + if (it->GetScore() < mutual_removol_score) { RCLCPP_DEBUG(get_logger(), "Removing node %i from graph with score: %f and old score: %f.", it->GetVertex()->GetObject()->GetUniqueId(), @@ -143,6 +143,15 @@ void LifelongSlamToolbox::evaluateNodeDepreciation( } } + +// +/* + nullptr evaluation + Test minimizar el radio del scan para procesar menor cantidad de scans. + Test constraint en el tama;o del grafo. El siguiente nodo despues del limite va a ser borrado +*/ +// + /*****************************************************************************/ Vertices LifelongSlamToolbox::FindScansWithinRadius( LocalizedRangeScan * scan, const double & radius) From 8c206a80e767d5f77f553ea6540abc958c6cc2db Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Fri, 9 Sep 2022 18:52:44 -0500 Subject: [PATCH 65/83] [ADD]: Adding cell angle functionality. Still under testing Signed-off-by: Camilo Andres Alvis Bautista --- src/experimental/information_estimates.cpp | 354 ++++++++++----------- 1 file changed, 176 insertions(+), 178 deletions(-) diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index ee60e7176..5c5a27590 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -51,12 +51,15 @@ kt_double InformationEstimates::findMutualInfo(std::vector Step 0 ... " << std::endl; + m_low_x = range_scans[0]->GetCorrectedPose().GetX(); m_low_y = range_scans[0]->GetCorrectedPose().GetY(); m_high_x = range_scans[0]->GetCorrectedPose().GetX(); m_high_y = range_scans[0]->GetCorrectedPose().GetY(); + // std::cout << "-----------------> Step 1 ... " << std::endl; // Iterating through the group of scans to rescale the grid for (const auto & scan : range_scans) { @@ -66,209 +69,204 @@ kt_double InformationEstimates::findMutualInfo(std::vectorGetCorrectedPose(); + karto::PointVectorDouble laser_readings = scan->GetPointReadings(false); + // std::cout << "Laser reading size: " << laser_readings.size() << std::endl; + // LaserRangeFinder* pLaserRangeFinder = scan->GetLaserRangeFinder(); + // kt_double rangeThreshold = scan->GetLaserRangeFinder()->GetRangeThreshold(); + + kt_double minimumAngle = scan->GetLaserRangeFinder()->GetMinimumAngle(); + kt_double angularResolution = scan->GetLaserRangeFinder()->GetAngularResolution(); + + kt_double rangeReading = scan->GetRangeReadings()[0]; + int size = scan->GetRangeReadingsVector().size(); + // std::cout << "Range reading: " << rangeReading << std::endl; + // std::cout << "Rangescan size: " << size << std::endl; + + // kt_double rangeReading = GetRangeReadings()[i]; + // std::cout << "Sensor pose: " << scan->GetSensorPose().GetHeading() << std::endl; + // std::cout << "Laser reading 0 - X : " << laser_readings[0].GetX() << std::endl; + // std::cout << "Laser reading 0 - Y : " << laser_readings[0].GetY() << std::endl; + // std::cout << "Laser reading 0 - Distance : " << laser_readings[0].Length() << std::endl; + // // std::cout << "Laser reading 0 - Distance : " << laser_readings[0]. << std::endl; + // std::cout << "Processing pose: " << pose.GetX() << ", " << pose.GetY() << std::endl; - // Finding the closest pose - m_low_x = pose.GetX() < m_low_x ? pose.GetX() : m_low_x; - m_low_y = pose.GetY() < m_low_y ? pose.GetY() : m_low_y; + // Finding the lower limit pose + m_low_x = std::min(pose.GetX(), m_low_x); + m_low_y = std::min(pose.GetY(), m_low_y); - // Finding the farthest pose - m_high_x = pose.GetX() > m_high_x ? pose.GetX() : m_high_x; - m_high_y = pose.GetY() > m_high_y ? pose.GetY() : m_high_y; + // Finding the higher limit pose + m_high_x = std::max(pose.GetX(), m_high_x); + m_high_y = std::max(pose.GetY(), m_high_y); } - // Margins for the closest points - m_low_x -= m_max_sensor_range; - m_low_y -= m_max_sensor_range; + // std::cout << "-----------------> Step 2 ... " << std::endl; - // Margins for the farthest points - m_high_x += m_max_sensor_range; - m_high_y += m_max_sensor_range; + + // std::cout << "Low X Position: " << m_low_x << std::endl; + // std::cout << "High X Position: " << m_high_x << std::endl; + + // std::cout << "Low Y Position: " << m_low_y << std::endl; + // std::cout << "High Y Position: " << m_high_y << std::endl; // Map dimensions - kt_double dist_x = std::fabs(m_high_x) + std::fabs(m_low_x); - kt_double dist_y = std::fabs(m_high_y) + std::fabs(m_low_y); + kt_double dim_x = std::fabs(m_high_x - m_low_x) + (2 * m_max_sensor_range); + kt_double dim_y = std::fabs(m_high_y - m_low_y) + (2 * m_max_sensor_range); + + // std::cout << "X Dimension: " << dim_x << std::endl; + // std::cout << "Y Dimension: " << dim_y << std::endl; // Get the number of cells - int n_cells_x = static_cast(dist_x / m_cell_resol); - int n_cells_y = static_cast(dist_y / m_cell_resol); + int n_cells_x = static_cast(dim_x / m_cell_resol); + int n_cells_y = static_cast(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); - // ------------ End of Grid resizing ------------ // + // // ------------ End of Grid resizing ------------ // - // I would need to move it N times - // Cause here I do remove one scan at a time and call the following loop + // // I would need to move it N times + // // Cause here I do remove one scan at a time and call the following loop - /* - Para encontrar el scan que menor mutual information proporciona debo tener un loop para ir removiendo uno a uno. - 1. El loop externo es solamente un for que itera N veces. (Dado por al longitud del vector de scans) - 2 Adentro solamente existe un condicional que se salta ese scan en la busqueda actual - */ - // Iterating through the cells - kt_double mutual_information = 0.0f; + // /* + // Para encontrar el scan que menor mutual information proporciona debo tener un loop para ir removiendo uno a uno. + // 1. El loop externo es solamente un for que itera N veces. (Dado por al longitud del vector de scans) + // 2 Adentro solamente existe un condicional que se salta ese scan en la busqueda actual + // */ + // // Iterating through the cells + // kt_double mutual_information = 0.0f; - for (int n = 0; n < range_scans.size(); ++n) + // for (int n = 0; n < range_scans.size(); ++n) + for (int n = 0; n < 1; ++n) { - for (int i = 0; i < n_cells_x; ++i) + karto::Pose2 robot_pose_raw = range_scans[n]->GetCorrectedPose(); + // Get the current robot pose and extract the extra range for locating it at the lowest X and Y + karto::Pose2 local_grid_robot_pose{ robot_pose_raw.GetX() - (m_low_x - m_max_sensor_range), robot_pose_raw.GetY() - (m_low_y - m_max_sensor_range), robot_pose_raw.GetHeading() }; + // std::cout << "Local robot pose: " << local_grid_robot_pose.GetX() << ", " << local_grid_robot_pose.GetY() << std::endl; + + // @NIT + // This is an ideal point to add an assertion, since the robotr pose must be positive in all of the scenarios + + // Minimum X point + kt_double lower_limit_x = std::max(0.0, local_grid_robot_pose.GetX() - m_max_sensor_range); + kt_double upper_limit_x = std::min(dim_x, local_grid_robot_pose.GetX() + m_max_sensor_range); + kt_double lower_limit_y = std::max(0.0, local_grid_robot_pose.GetY() - m_max_sensor_range); + kt_double upper_limit_y = std::min(dim_y, local_grid_robot_pose.GetY() + m_max_sensor_range); + + karto::Vector2 lower_limit{ lower_limit_x, lower_limit_y }; + karto::Vector2 upper_limit{ upper_limit_x, upper_limit_y }; + + // @NIT + // I can use an rvalue reference here + karto::Vector2 lower_limit_cell = utils::grid_operations::getGridPosition(lower_limit, m_cell_resol); + karto::Vector2 upper_limit_cell = utils::grid_operations::getGridPosition(upper_limit, m_cell_resol); + + karto::Vector2 local_robot_cell = utils::grid_operations::getGridPosition(local_grid_robot_pose.GetPosition(), m_cell_resol); + + std::cout << " =================== " << std::endl; + // std::cout << "Lower limit cell: " << lower_limit_cell.GetX() * m_cell_resol << ", " << lower_limit_cell.GetY() * m_cell_resol << std::endl; + // std::cout << "Upper limit cell: " << upper_limit_cell.GetX() * m_cell_resol << ", " << upper_limit_cell.GetY() * m_cell_resol << std::endl; + + std::cout << "Lower limit cell: " << lower_limit_cell.GetX() << ", " << lower_limit_cell.GetY() << std::endl; + std::cout << "Upper limit cell: " << upper_limit_cell.GetX() << ", " << upper_limit_cell.GetY() << std::endl; + + // ======================= TESTING ======================= // + + // // Cel 0,0 is 0.5, 0.5 in distance + // x2 and y2 are the robot pose + // x1 and y1 are the cell pose + + // kt_double cell_laser_angle = atan2(0.5 - local_grid_robot_pose.GetY() , 0.5 - local_grid_robot_pose.GetX()); + + // // std::cout << "Angle: " << cell_laser_angle << std::endl; + // // ======================= END OF TESTING ======================= // + // // ======================= START OF CELLS ITERATIONS ======================= // + // for (int i = lower_limit_cell.GetX(); i < upper_limit_cell.GetX(); ++i) + for (int i = lower_limit_cell.GetX(); i < local_robot_cell.GetX(); ++i) { - for (int j = 0; j < n_cells_y; ++j) + for(int j = lower_limit_cell.GetY(); j < local_robot_cell.GetY(); ++j) { - /* - Tengo una celda especifica dentro del mapa - Puedo saber que lasers ven esa celda para evaluar estos - - - I have this not written but I Need to find a way to calculate the mutual information for a group - of scans when I remove one element of that group - - I can Iterate over the scans here - - But I would nneed to removew one scan at a time - */ - // Iterating through the scans to find which one hits this cell - std::vector> cell_probabilities; - int current_scan_idx = 0; - for (const auto & scan : range_scans) + // This is not fully necessary, but I will leave it here for now ===> Testing + if ((i < lower_limit_cell.GetX() || i > upper_limit_cell.GetX()) || (j < lower_limit_cell.GetY() || j > upper_limit_cell.GetY())) { - // On this way I make sure to extract one laser at a time - if (current_scan_idx == n) - { - continue; - } - - karto::Pose2 robot_pose_raw = scan->GetCorrectedPose(); - // karto::Pose2 scan_pose(raw_scan_pose.GetX() - m_low_x, raw_scan_pose.GetY() - m_low_y, raw_scan_pose.GetHeading()); - - // karto::Vector2 min_cell = utils::grid_operations::getGridPosition(min_pt, m_cell_resol); - // karto::Vector2 max_cell = utils::grid_operations::getGridPosition(max_pt, m_cell_resol); - // kt_double cell_y {(min_cell[1] + max_cell[1]) / 2}; - // kt_double cell_x {(min_cell[0] + max_cell[0])/ 2}; - // if (sqrt(pow((pose[0] - cell_x), 2) + pow((pose[1] - cell_y), 2)) > m_max_sensor_range) - // { - // continue; - // } - - // Lowest X point - kt_double low_pt_x = robot_pose_raw.GetX() - m_max_sensor_range; // X: Posicion del scan - Rango maximo (Para saber si el scan ve por fuera del grid) - low_pt_x = low_pt_x < 0.0 ? 0.0 : low_pt_x; - // Maximum X point - kt_double max_pt_x = robot_pose_raw.GetX() + m_max_sensor_range; // X: Posicion del scan + Rango maximo (Para saber si el scan ve por fuera del grid) - max_pt_x = max_pt_x > dist_x ? dist_x : max_pt_x; - // Lowest Y point - kt_double low_pt_y = robot_pose_raw.GetY() - m_max_sensor_range; // Y: Posicion del scan - Rango maximo (Para saber si el scan ve por fuera del grid) - low_pt_y = low_pt_y < 0.0 ? 0.0 : low_pt_y; - // Maximum Y point - kt_double max_pt_y = robot_pose_raw.GetY() + m_max_sensor_range; // Y: Posicion del scan + Rango maximo (Para saber si el scan ve por fuera del grid) - max_pt_y = max_pt_y > dist_y ? dist_y : max_pt_y; - - karto::Vector2 min_pt{low_pt_x, low_pt_y}; // Esto es distancia no posicion de celda - karto::Vector2 max_pt{max_pt_x, max_pt_y}; // Esto es distancia no posicion de celda - - // Esto podia ser un karto::Pose2 - // karto::Vector2 getGridPosition(karto::Vector2 const& pose, kt_double resolution) - karto::Vector2 min_cell = utils::grid_operations::getGridPosition(min_pt, m_cell_resol); - karto::Vector2 max_cell = utils::grid_operations::getGridPosition(max_pt, m_cell_resol); - - // i and j are the current row and column - if ((i < min_cell.GetX() || i > max_cell.GetX()) || (j < min_cell.GetY() || j > max_cell.GetY())) - { - continue; - } - - // Current scan laser readings - karto::PointVectorDouble laser_readings = scan->GetPointReadings(true); - - karto::Pose2 robot_pose(robot_pose_raw.GetX() - m_low_x, robot_pose_raw.GetY() - m_low_y, robot_pose_raw.GetHeading()); - karto::Vector2 robot_grid_position = utils::grid_operations::getGridPosition(robot_pose.GetPosition(), m_cell_resol); - - /* - Ahora que estoy dentro de un grid local, debo hacer una transformacion simple de - la posicion del robot y de cada uno de los lasers, para asi poder compararlos correctamente - con cada una de las celdas. - */ - - // ----- Point 3 ----- - // I could use some kind of angle to filter the region where the scan is. - // I can use the range based for here - for (int i = 0; i < laser_readings.size(); ++i) - { - karto::Vector2 transformed_laser{laser_readings[i].GetX() - m_low_x, laser_readings[i].GetY() - m_low_y}; - - // I need to transform here - karto::Vector2 beam_grid = utils::grid_operations::getGridPosition(transformed_laser, m_cell_resol); - - // kt_double laser_to_grid_dist = - // sqrt(pow((transformed_laser.GetX() - kt_double(beam_grid.GetX())), 2) + pow((transformed_laser.GetY() - kt_double(beam_grid.GetY())), 2)); - - // This calculation is not real, cause resting the limits no the center of the cell - kt_double laser_to_grid_dist = - sqrt(pow((transformed_laser.GetX() - (i * m_cell_resol)), 2) + pow((transformed_laser.GetY() - (j * m_cell_resol)), 2)); - - // Compare the point here, the transformed laser and the cell position - // If not fulfill the condition I wrote above, continue - if (laser_to_grid_dist > m_cell_resol) - { - continue; - } - - // ----- Point 4 ----- - // Aqui ya encontre cual es el haz de luz que funciona - // Ahora debo hacer el calculo de observacion hasta esa celda final - - // Inidividual cell limits - kt_double limit_x = i * m_cell_resol; - kt_double limit_y = j * m_cell_resol; - - std::pair, std::vector> intersections = utils::grid_operations::computeLineBoxIntersection( - robot_pose.GetPosition(), transformed_laser, robot_grid_position, beam_grid, limit_x, limit_y, m_cell_resol); - - // In case we did not find a measurement - if (intersections.first.size() == 0) - { - continue; - } - - // 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 = robot_pose.GetPosition().Distance(intersection); - distances.push_back(distance); - } - - // 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]) - }; - - // I got the probability of seen the current cell as o/f/u - // Now I need to save the current measuremets of that cell to calculate the histogram - cell_probabilities.push_back(probabilities); - } // EOF Laser Readings FOR - - ++current_scan_idx; - } // EOF Range Scans FOR - - - std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(cell_probabilities); + std::cout << "Out of limits" << std::endl; + continue; + } - kt_double cell_mutual_inf = 0.0f; - for (auto& pair : meas_out_prob) + // There should be an operator for this, but this is working good. + karto::Vector2 cell_center{ i * m_cell_resol + (m_cell_resol / 2), j * m_cell_resol + (m_cell_resol / 2) }; + kt_double pose_to_cell_angle = atan2(cell_center.GetY() - local_grid_robot_pose.GetY(), cell_center.GetX() - local_grid_robot_pose.GetX()); + pose_to_cell_angle = pose_to_cell_angle > 0 ? pose_to_cell_angle : (2.0 * M_PI + pose_to_cell_angle); + + kt_double robot_heading = robot_pose_raw.GetHeading(); + robot_heading = robot_heading < 0.0 ? (2.0 * M_PI + robot_heading) : robot_heading; + + int initial_index = robot_heading / 0.0174533; + int angle_idx = pose_to_cell_angle / 0.0174533; + + if (initial_index == angle_idx) + { + // std::cout << "Heading match the cell" << std::endl; + } + int aux_index = 0; + if (initial_index > 180) + { + // std::cout << "Case 2" << std::endl; + aux_index = 360 - initial_index + angle_idx; + } + else if (initial_index >= 0 && initial_index <= 180) + { + // std::cout << "Case 3" << std::endl; + aux_index = initial_index + (initial_index - angle_idx); + } + + // kt_double reading_distance = range_scans[n]->GetRangeReadings()[aux_index]; // Previous + kt_double reading_distance = range_scans[n]->GetRangeReadings()[angle_idx]; + + kt_double distance_to_cell = sqrt(pow(local_grid_robot_pose.GetX() - cell_center.GetX(), 2) + pow(local_grid_robot_pose.GetY() - cell_center.GetY(), 2)); + if (reading_distance < distance_to_cell) { - cell_mutual_inf += pair.second * measurementOutcomeEntropy(pair.first); + continue; + } + else + { + reading_distance = distance_to_cell; + } + + if (reading_distance > m_max_sensor_range) + { + continue; + } + + kt_double point_x = local_grid_robot_pose.GetX() + (reading_distance * cos(angle_idx)); + kt_double point_y = local_grid_robot_pose.GetY() + (reading_distance * sin(angle_idx)); + + if( sqrt(pow((i * m_cell_resol) - point_x, 2) + pow(j * m_cell_resol - point_y, 2)) > 0.5) + { + std::cout << "Too big" << std::endl; + continue; + // Aqui probablmente pueda correr un for con los siguientes con dos indices anteriores y dos indices siguientes } - // 1.0 for the entropy since the cell is in an unknown state - mutual_information += 1.0 - cell_mutual_inf; - } // EOF Columns FOR - } // EOF Rows FOR - } // EOF Number Scans FOR - - // Note: This not the real answer, since I would return the scan that provides the less information (This is just a placeholder for compilation) - return mutual_information; + + karto::Vector2 laser_point = utils::grid_operations::getGridPosition({point_x, point_y}, m_cell_resol); + std::cout << "Robot heading: " << robot_heading * (180 / M_PI) << std::endl; + std::cout << "Angle to cell: " << pose_to_cell_angle * (180 / M_PI) << std::endl; + std::cout << "Projected index to angle: " << aux_index << std::endl; + std::cout << "Distance to cell: " << distance_to_cell << std::endl; + std::cout << "Reading distance: " << reading_distance << std::endl; + std::cout << "Robot pose: " << local_robot_cell.GetX() << ", " << local_robot_cell.GetY() << std::endl; + std::cout << "Current coordinates: " << i * m_cell_resol << ", " << j * m_cell_resol << std::endl; + std::cout << "Laser coordinates: " << point_x << ", " << point_y << std::endl; + std::cout << " =================== " << std::endl; + + // std::cout << pose_to_cell_angle * (180.0 / M_PI) << std::endl; + // int initial_index = (M_PI + pose_to_cell_angle - local_grid_robot_pose.GetHeading()) * (180.0 / M_PI); + } + } + std::cout << "<------------------------->" << std::endl; + } + return 5.0; } std::vector InformationEstimates::findLeastInformativeLaser(std::vector const& range_scans) From 75d20f6084afd72a0813b62a93cf05b7c06d284e Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Tue, 11 Oct 2022 12:53:22 -0500 Subject: [PATCH 66/83] [WIP]: Calculations up to entropy are now correct Signed-off-by: Camilo Andres Alvis Bautista --- src/experimental/information_estimates.cpp | 222 +++++++-------------- 1 file changed, 68 insertions(+), 154 deletions(-) diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 5c5a27590..b7659ea85 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -20,38 +20,6 @@ InformationEstimates::InformationEstimates() kt_double InformationEstimates::findMutualInfo(std::vector const& range_scans) { - /* - 1. Parate en una celda de la grid inicial. - 2. Identifica cuales laser scans del grupo actual pueden ver esta celda. (Maximum range radius) - 3. Cuales haz de luz del scan seleccionado puede ver esa celda. - 4. Resto de operaciones. - */ - - /* - Para el punto 0: - - Debe existir un loop exterior para ir removiendo un laser scan a la vez, con esto identifico cual es el scan - que aporta menor informacion del grupo dado. - */ - - /* - Para el punto 1 y 2: - - Toma el pose del scan actual. - - Encuentra la distancia del centro del scan con respecto al centro de la celda que nos interesa. - - Si esa diferencia es menor que el rango del scan, entonces consideramos este scan. (Agregamos ese scan a un vector) - - Esto puedo afrontarlo de forma local, lo que significa que por cada set de scans que reciba, - puedo encontrar los extremos para poder reescalar el grid, esto potencialmente reduce el tiempo de busqueda. - */ - - /* - Para el punto 3: - - Con las lecturas de ese scan, encuentro el haz de luz que este dentro de la celda. - - Potencialmente puedo encontrar dos lectura, pero por ahora elegire el primero que encuentre. - */ - - // ------------ Start of Grid resizing ------------ // - // I took the first scan for reference. From this one I extract the min an max points (Poses) - // std::cout << "-----------------> Step 0 ... " << std::endl; m_low_x = range_scans[0]->GetCorrectedPose().GetX(); m_low_y = range_scans[0]->GetCorrectedPose().GetY(); @@ -59,9 +27,8 @@ kt_double InformationEstimates::findMutualInfo(std::vectorGetCorrectedPose().GetX(); m_high_y = range_scans[0]->GetCorrectedPose().GetY(); - // std::cout << "-----------------> Step 1 ... " << std::endl; // Iterating through the group of scans to rescale the grid - for (const auto & scan : range_scans) + for (const auto &scan : range_scans) { if (scan == nullptr) { @@ -70,26 +37,12 @@ kt_double InformationEstimates::findMutualInfo(std::vectorGetCorrectedPose(); karto::PointVectorDouble laser_readings = scan->GetPointReadings(false); - // std::cout << "Laser reading size: " << laser_readings.size() << std::endl; - // LaserRangeFinder* pLaserRangeFinder = scan->GetLaserRangeFinder(); - // kt_double rangeThreshold = scan->GetLaserRangeFinder()->GetRangeThreshold(); kt_double minimumAngle = scan->GetLaserRangeFinder()->GetMinimumAngle(); kt_double angularResolution = scan->GetLaserRangeFinder()->GetAngularResolution(); kt_double rangeReading = scan->GetRangeReadings()[0]; int size = scan->GetRangeReadingsVector().size(); - // std::cout << "Range reading: " << rangeReading << std::endl; - // std::cout << "Rangescan size: " << size << std::endl; - - // kt_double rangeReading = GetRangeReadings()[i]; - // std::cout << "Sensor pose: " << scan->GetSensorPose().GetHeading() << std::endl; - // std::cout << "Laser reading 0 - X : " << laser_readings[0].GetX() << std::endl; - // std::cout << "Laser reading 0 - Y : " << laser_readings[0].GetY() << std::endl; - // std::cout << "Laser reading 0 - Distance : " << laser_readings[0].Length() << std::endl; - // // std::cout << "Laser reading 0 - Distance : " << laser_readings[0]. << std::endl; - - // std::cout << "Processing pose: " << pose.GetX() << ", " << pose.GetY() << std::endl; // Finding the lower limit pose m_low_x = std::min(pose.GetX(), m_low_x); @@ -100,22 +53,10 @@ kt_double InformationEstimates::findMutualInfo(std::vector Step 2 ... " << std::endl; - - - // std::cout << "Low X Position: " << m_low_x << std::endl; - // std::cout << "High X Position: " << m_high_x << std::endl; - - // std::cout << "Low Y Position: " << m_low_y << std::endl; - // std::cout << "High Y Position: " << m_high_y << std::endl; - // Map dimensions kt_double dim_x = std::fabs(m_high_x - m_low_x) + (2 * m_max_sensor_range); kt_double dim_y = std::fabs(m_high_y - m_low_y) + (2 * m_max_sensor_range); - // std::cout << "X Dimension: " << dim_x << std::endl; - // std::cout << "Y Dimension: " << dim_y << std::endl; - // Get the number of cells int n_cells_x = static_cast(dim_x / m_cell_resol); int n_cells_y = static_cast(dim_y / m_cell_resol); @@ -124,29 +65,10 @@ kt_double InformationEstimates::findMutualInfo(std::vectorGetCorrectedPose(); - // Get the current robot pose and extract the extra range for locating it at the lowest X and Y - karto::Pose2 local_grid_robot_pose{ robot_pose_raw.GetX() - (m_low_x - m_max_sensor_range), robot_pose_raw.GetY() - (m_low_y - m_max_sensor_range), robot_pose_raw.GetHeading() }; - // std::cout << "Local robot pose: " << local_grid_robot_pose.GetX() << ", " << local_grid_robot_pose.GetY() << std::endl; - - // @NIT - // This is an ideal point to add an assertion, since the robotr pose must be positive in all of the scenarios + karto::Pose2 local_grid_robot_pose{robot_pose_raw.GetX() - (m_low_x - m_max_sensor_range), robot_pose_raw.GetY() - (m_low_y - m_max_sensor_range), robot_pose_raw.GetHeading()}; // Minimum X point kt_double lower_limit_x = std::max(0.0, local_grid_robot_pose.GetX() - m_max_sensor_range); @@ -154,117 +76,109 @@ kt_double InformationEstimates::findMutualInfo(std::vector lower_limit{ lower_limit_x, lower_limit_y }; - karto::Vector2 upper_limit{ upper_limit_x, upper_limit_y }; + karto::Vector2 lower_limit{lower_limit_x, lower_limit_y}; + karto::Vector2 upper_limit{upper_limit_x, upper_limit_y}; - // @NIT - // I can use an rvalue reference here karto::Vector2 lower_limit_cell = utils::grid_operations::getGridPosition(lower_limit, m_cell_resol); karto::Vector2 upper_limit_cell = utils::grid_operations::getGridPosition(upper_limit, m_cell_resol); karto::Vector2 local_robot_cell = utils::grid_operations::getGridPosition(local_grid_robot_pose.GetPosition(), m_cell_resol); - std::cout << " =================== " << std::endl; - // std::cout << "Lower limit cell: " << lower_limit_cell.GetX() * m_cell_resol << ", " << lower_limit_cell.GetY() * m_cell_resol << std::endl; - // std::cout << "Upper limit cell: " << upper_limit_cell.GetX() * m_cell_resol << ", " << upper_limit_cell.GetY() * m_cell_resol << std::endl; - - std::cout << "Lower limit cell: " << lower_limit_cell.GetX() << ", " << lower_limit_cell.GetY() << std::endl; - std::cout << "Upper limit cell: " << upper_limit_cell.GetX() << ", " << upper_limit_cell.GetY() << std::endl; + karto::LaserRangeFinder *laser_range_finder = range_scans[n]->GetLaserRangeFinder(); + kt_double range_threshold = laser_range_finder->GetRangeThreshold(); + kt_double angle_increment = laser_range_finder->GetAngularResolution(); + kt_double min_angle = laser_range_finder->GetMinimumAngle(); + kt_double max_angle = laser_range_finder->GetMaximumAngle(); + kt_double total_range = max_angle - min_angle; - // ======================= TESTING ======================= // + std::cout << "Ranges X: " << lower_limit_cell.GetX() << ", " << local_robot_cell.GetX() << std::endl; + std::cout << "Ranges Y: " << local_robot_cell.GetY() << ", " << upper_limit_cell.GetY() << std::endl; - // // Cel 0,0 is 0.5, 0.5 in distance - // x2 and y2 are the robot pose - // x1 and y1 are the cell pose - - // kt_double cell_laser_angle = atan2(0.5 - local_grid_robot_pose.GetY() , 0.5 - local_grid_robot_pose.GetX()); - - // // std::cout << "Angle: " << cell_laser_angle << std::endl; - // // ======================= END OF TESTING ======================= // - // // ======================= START OF CELLS ITERATIONS ======================= // - // for (int i = lower_limit_cell.GetX(); i < upper_limit_cell.GetX(); ++i) - for (int i = lower_limit_cell.GetX(); i < local_robot_cell.GetX(); ++i) + for (int i = lower_limit_cell.GetX(); i < upper_limit_cell.GetX(); ++i) { - for(int j = lower_limit_cell.GetY(); j < local_robot_cell.GetY(); ++j) + for (int j = lower_limit_cell.GetY(); j < upper_limit_cell.GetY(); ++j) { - // This is not fully necessary, but I will leave it here for now ===> Testing - if ((i < lower_limit_cell.GetX() || i > upper_limit_cell.GetX()) || (j < lower_limit_cell.GetY() || j > upper_limit_cell.GetY())) - { - std::cout << "Out of limits" << std::endl; - continue; - } - - // There should be an operator for this, but this is working good. - karto::Vector2 cell_center{ i * m_cell_resol + (m_cell_resol / 2), j * m_cell_resol + (m_cell_resol / 2) }; - kt_double pose_to_cell_angle = atan2(cell_center.GetY() - local_grid_robot_pose.GetY(), cell_center.GetX() - local_grid_robot_pose.GetX()); - pose_to_cell_angle = pose_to_cell_angle > 0 ? pose_to_cell_angle : (2.0 * M_PI + pose_to_cell_angle); + karto::Vector2 cell_center{i * m_cell_resol + (m_cell_resol / 2), j * m_cell_resol + (m_cell_resol / 2)}; + kt_double angle_to_cell = atan2(cell_center.GetY() - local_grid_robot_pose.GetY(), cell_center.GetX() - local_grid_robot_pose.GetX()); kt_double robot_heading = robot_pose_raw.GetHeading(); - robot_heading = robot_heading < 0.0 ? (2.0 * M_PI + robot_heading) : robot_heading; - int initial_index = robot_heading / 0.0174533; - int angle_idx = pose_to_cell_angle / 0.0174533; + kt_double angle_between = angle_to_cell - robot_heading; - if (initial_index == angle_idx) + kt_double angle_diff = atan2(sin(angle_between), cos(angle_between)); + if (angle_diff < min_angle || angle_diff > max_angle) { - // std::cout << "Heading match the cell" << std::endl; - } - int aux_index = 0; - if (initial_index > 180) - { - // std::cout << "Case 2" << std::endl; - aux_index = 360 - initial_index + angle_idx; + // We are outside the laser FOV, but we cannot cancel it + // because we will not evaluate other cells + continue; } - else if (initial_index >= 0 && initial_index <= 180) + + int laser_idx = ((total_range / 2.0) + angle_between) / angle_increment; + kt_double laser_read_dist = range_scans[n]->GetRangeReadings()[laser_idx]; + kt_double laser_read_raw = range_scans[n]->GetRangeReadings()[laser_idx]; + if (laser_read_dist > range_threshold) { - // std::cout << "Case 3" << std::endl; - aux_index = initial_index + (initial_index - angle_idx); + continue; } - // kt_double reading_distance = range_scans[n]->GetRangeReadings()[aux_index]; // Previous - kt_double reading_distance = range_scans[n]->GetRangeReadings()[angle_idx]; - kt_double distance_to_cell = sqrt(pow(local_grid_robot_pose.GetX() - cell_center.GetX(), 2) + pow(local_grid_robot_pose.GetY() - cell_center.GetY(), 2)); - if (reading_distance < distance_to_cell) + if (laser_read_dist < distance_to_cell) { continue; } else { - reading_distance = distance_to_cell; + laser_read_dist = distance_to_cell; } - if (reading_distance > m_max_sensor_range) + kt_double angle_to_cell_proj = angle_to_cell > 0 ? angle_to_cell : (2.0 * M_PI + angle_to_cell); + + kt_double point_x_raw = local_grid_robot_pose.GetX() + (laser_read_raw * cos(angle_to_cell_proj)); + kt_double point_y_raw = local_grid_robot_pose.GetY() + (laser_read_raw * sin(angle_to_cell_proj)); + + karto::Vector2 laser_beam_cell_raw = utils::grid_operations::getGridPosition({point_x_raw, point_y_raw}, m_cell_resol); + + // Inidividual cell limits + kt_double limit_x = i * m_cell_resol; + kt_double limit_y = j * m_cell_resol; + + karto::PointVectorDouble laser_readings = range_scans[n]->GetPointReadings(true); + karto::Vector2 transformed_laser{laser_readings[laser_idx].GetX() - m_low_x, laser_readings[laser_idx].GetY() - m_low_y}; + + std::pair, std::vector> intersections = utils::grid_operations::computeLineBoxIntersection( + local_grid_robot_pose.GetPosition(), {point_x_raw, point_y_raw}, local_robot_cell, laser_beam_cell_raw, limit_x, limit_y, m_cell_resol); + + // In case we did not find a measurement + if (intersections.first.size() == 0) { continue; } - kt_double point_x = local_grid_robot_pose.GetX() + (reading_distance * cos(angle_idx)); - kt_double point_y = local_grid_robot_pose.GetY() + (reading_distance * sin(angle_idx)); - - if( sqrt(pow((i * m_cell_resol) - point_x, 2) + pow(j * m_cell_resol - point_y, 2)) > 0.5) + // Enter (d1) and Exit (d2) distances + std::vector distances; + for (int k = 0; k < intersections.first.size(); ++k) { - std::cout << "Too big" << std::endl; - continue; - // Aqui probablmente pueda correr un for con los siguientes con dos indices anteriores y dos indices siguientes + // From robot position to intersection points + karto::Vector2 intersection{intersections.first[k], intersections.second[k]}; + kt_double distance = local_grid_robot_pose.GetPosition().Distance(intersection); + distances.push_back(distance); + std::sort(distances.begin(), distances.end()); } - karto::Vector2 laser_point = utils::grid_operations::getGridPosition({point_x, point_y}, m_cell_resol); - std::cout << "Robot heading: " << robot_heading * (180 / M_PI) << std::endl; - std::cout << "Angle to cell: " << pose_to_cell_angle * (180 / M_PI) << std::endl; - std::cout << "Projected index to angle: " << aux_index << std::endl; - std::cout << "Distance to cell: " << distance_to_cell << std::endl; - std::cout << "Reading distance: " << reading_distance << std::endl; - std::cout << "Robot pose: " << local_robot_cell.GetX() << ", " << local_robot_cell.GetY() << std::endl; - std::cout << "Current coordinates: " << i * m_cell_resol << ", " << j * m_cell_resol << std::endl; - std::cout << "Laser coordinates: " << point_x << ", " << point_y << std::endl; - std::cout << " =================== " << std::endl; - - // std::cout << pose_to_cell_angle * (180.0 / M_PI) << std::endl; - // int initial_index = (M_PI + pose_to_cell_angle - local_grid_robot_pose.GetHeading()) * (180.0 / M_PI); + // 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])}; + + // Appending new measurement outcomes for the current cell + karto::Vector2 cell{i, j}; + appendCellProbabilities(probabilities, cell); + return 0.5; } } std::cout << "<------------------------->" << std::endl; + // m_visited_grid.setZero(); } return 5.0; } From e2a99fc7860ae3085f09ef7231868231df42fff5 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Tue, 18 Oct 2022 11:19:34 -0500 Subject: [PATCH 67/83] Fully functional loop Signed-off-by: Camilo Andres Alvis Bautista --- src/experimental/information_estimates.cpp | 200 ++++++++++++--------- 1 file changed, 120 insertions(+), 80 deletions(-) diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index b7659ea85..6c9f3bbac 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -18,16 +18,15 @@ InformationEstimates::InformationEstimates() } -kt_double InformationEstimates::findMutualInfo(std::vector const& range_scans) +kt_double InformationEstimates::findMutualInfo(std::vector const &range_scans) { - m_low_x = range_scans[0]->GetCorrectedPose().GetX(); m_low_y = range_scans[0]->GetCorrectedPose().GetY(); m_high_x = range_scans[0]->GetCorrectedPose().GetX(); m_high_y = range_scans[0]->GetCorrectedPose().GetY(); - // Iterating through the group of scans to rescale the grid + for (const auto &scan : range_scans) { if (scan == nullptr) @@ -65,10 +64,19 @@ kt_double InformationEstimates::findMutualInfo(std::vectorGetCorrectedPose(); + // Get the current robot pose and extract the extra range for locating it at the lowest X and Y karto::Pose2 local_grid_robot_pose{robot_pose_raw.GetX() - (m_low_x - m_max_sensor_range), robot_pose_raw.GetY() - (m_low_y - m_max_sensor_range), robot_pose_raw.GetHeading()}; + // std::cout << "Local robot pose: " << local_grid_robot_pose.GetX() << ", " << local_grid_robot_pose.GetY() << std::endl; + + // @NIT + // This is an ideal point to add an assertion, since the robot pose must be positive in all of the scenarios // Minimum X point kt_double lower_limit_x = std::max(0.0, local_grid_robot_pose.GetX() - m_max_sensor_range); @@ -79,6 +87,8 @@ kt_double InformationEstimates::findMutualInfo(std::vector lower_limit{lower_limit_x, lower_limit_y}; karto::Vector2 upper_limit{upper_limit_x, upper_limit_y}; + // @NIT + // I can use an rvalue reference here karto::Vector2 lower_limit_cell = utils::grid_operations::getGridPosition(lower_limit, m_cell_resol); karto::Vector2 upper_limit_cell = utils::grid_operations::getGridPosition(upper_limit, m_cell_resol); @@ -91,94 +101,124 @@ kt_double InformationEstimates::findMutualInfo(std::vectorGetMaximumAngle(); kt_double total_range = max_angle - min_angle; - std::cout << "Ranges X: " << lower_limit_cell.GetX() << ", " << local_robot_cell.GetX() << std::endl; - std::cout << "Ranges Y: " << local_robot_cell.GetY() << ", " << upper_limit_cell.GetY() << std::endl; + std::vector> vis_cells; + bool printed = false; for (int i = lower_limit_cell.GetX(); i < upper_limit_cell.GetX(); ++i) { for (int j = lower_limit_cell.GetY(); j < upper_limit_cell.GetY(); ++j) { - karto::Vector2 cell_center{i * m_cell_resol + (m_cell_resol / 2), j * m_cell_resol + (m_cell_resol / 2)}; - kt_double angle_to_cell = atan2(cell_center.GetY() - local_grid_robot_pose.GetY(), cell_center.GetX() - local_grid_robot_pose.GetX()); - - kt_double robot_heading = robot_pose_raw.GetHeading(); - - kt_double angle_between = angle_to_cell - robot_heading; - - kt_double angle_diff = atan2(sin(angle_between), cos(angle_between)); - if (angle_diff < min_angle || angle_diff > max_angle) - { - // We are outside the laser FOV, but we cannot cancel it - // because we will not evaluate other cells - continue; - } - - int laser_idx = ((total_range / 2.0) + angle_between) / angle_increment; - kt_double laser_read_dist = range_scans[n]->GetRangeReadings()[laser_idx]; - kt_double laser_read_raw = range_scans[n]->GetRangeReadings()[laser_idx]; - if (laser_read_dist > range_threshold) + // Here I would need to find which of the scans can see this cell + // this is with the distance we have found + // Euclidean distance between cell center and robot position + for (int l = 0; l < range_scans.size(); ++l) { - continue; - } - - kt_double distance_to_cell = sqrt(pow(local_grid_robot_pose.GetX() - cell_center.GetX(), 2) + pow(local_grid_robot_pose.GetY() - cell_center.GetY(), 2)); - if (laser_read_dist < distance_to_cell) - { - continue; - } - else - { - laser_read_dist = distance_to_cell; - } - - kt_double angle_to_cell_proj = angle_to_cell > 0 ? angle_to_cell : (2.0 * M_PI + angle_to_cell); - - kt_double point_x_raw = local_grid_robot_pose.GetX() + (laser_read_raw * cos(angle_to_cell_proj)); - kt_double point_y_raw = local_grid_robot_pose.GetY() + (laser_read_raw * sin(angle_to_cell_proj)); - - karto::Vector2 laser_beam_cell_raw = utils::grid_operations::getGridPosition({point_x_raw, point_y_raw}, m_cell_resol); - - // Inidividual cell limits - kt_double limit_x = i * m_cell_resol; - kt_double limit_y = j * m_cell_resol; - - karto::PointVectorDouble laser_readings = range_scans[n]->GetPointReadings(true); - karto::Vector2 transformed_laser{laser_readings[laser_idx].GetX() - m_low_x, laser_readings[laser_idx].GetY() - m_low_y}; - - std::pair, std::vector> intersections = utils::grid_operations::computeLineBoxIntersection( + // Evaluate if the current laser that will be evaluated should be excluded + if (n == l) + { + // Current laser will not be evaluated + continue; + } + + // Evaluate if the laser can see the current cell + karto::Vector2 cell_center{i * m_cell_resol + (m_cell_resol / 2), j * m_cell_resol + (m_cell_resol / 2)}; + kt_double distance_to_cell = sqrt(pow(local_grid_robot_pose.GetX() - cell_center.GetX(), 2) + pow(local_grid_robot_pose.GetY() - cell_center.GetY(), 2)); + if(distance_to_cell > m_max_sensor_range) + { + // It means the robot cannot see the current cell + continue; + } + + // Continue the staff here + kt_double angle_to_cell = atan2(cell_center.GetY() - local_grid_robot_pose.GetY(), cell_center.GetX() - local_grid_robot_pose.GetX()); + kt_double robot_heading = robot_pose_raw.GetHeading(); + kt_double angle_between = angle_to_cell - robot_heading; + + kt_double angle_diff = atan2(sin(angle_between), cos(angle_between)); + if (angle_diff < min_angle || angle_diff > max_angle) + { + // We are outside the laser FOV, but we cannot cancel it + // because we will not evaluate other cells + continue; + } + + int laser_idx = ((total_range / 2.0) + angle_between) / angle_increment; + kt_double laser_read_dist = range_scans[n]->GetRangeReadings()[laser_idx]; + kt_double laser_read_raw = range_scans[n]->GetRangeReadings()[laser_idx]; + if (laser_read_dist > range_threshold) + { + continue; + } + + if (laser_read_dist < distance_to_cell) + { + continue; + } + else + { + laser_read_dist = distance_to_cell; + } + + kt_double angle_to_cell_proj = angle_to_cell > 0 ? angle_to_cell : (2.0 * M_PI + angle_to_cell); + kt_double point_x_raw = local_grid_robot_pose.GetX() + (laser_read_raw * cos(angle_to_cell_proj)); + kt_double point_y_raw = local_grid_robot_pose.GetY() + (laser_read_raw * sin(angle_to_cell_proj)); + karto::Vector2 laser_beam_cell_raw = utils::grid_operations::getGridPosition({point_x_raw, point_y_raw}, m_cell_resol); + + // Inidividual cell limits + kt_double limit_x = i * m_cell_resol; + kt_double limit_y = j * m_cell_resol; + + karto::PointVectorDouble laser_readings = range_scans[n]->GetPointReadings(true); + karto::Vector2 transformed_laser{laser_readings[laser_idx].GetX() - m_low_x, laser_readings[laser_idx].GetY() - m_low_y}; + + std::pair, std::vector> intersections = utils::grid_operations::computeLineBoxIntersection( local_grid_robot_pose.GetPosition(), {point_x_raw, point_y_raw}, local_robot_cell, laser_beam_cell_raw, limit_x, limit_y, m_cell_resol); - // In case we did not find a measurement - if (intersections.first.size() == 0) - { - continue; - } - - // 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 = local_grid_robot_pose.GetPosition().Distance(intersection); - distances.push_back(distance); - std::sort(distances.begin(), distances.end()); + if (intersections.first.size() == 0) + { + continue; + } + + // 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 = local_grid_robot_pose.GetPosition().Distance(intersection); + distances.push_back(distance); + std::sort(distances.begin(), distances.end()); + } + + // 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])}; + + karto::Vector2 cell{i, j}; + vis_cells.push_back(cell); + + // Appending new measurement outcomes for the current cell + appendCellProbabilities(probabilities, 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])}; - - // Appending new measurement outcomes for the current cell - karto::Vector2 cell{i, j}; - appendCellProbabilities(probabilities, cell); - return 0.5; + for (auto &cell : vis_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; } - std::cout << "<------------------------->" << std::endl; - // m_visited_grid.setZero(); + std::cout << "Mutual information: " << m_mutual_grid.sum() << std::endl; + m_cell_probabilities.clear(); } return 5.0; } From 91bd220090de6599e6598d2552c822df9057dcae Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Fri, 21 Oct 2022 14:11:58 -0500 Subject: [PATCH 68/83] Complete main function update Signed-off-by: Camilo Andres Alvis Bautista --- .../experimental/information_estimates.hpp | 3 +- src/experimental/information_estimates.cpp | 36 ++++++++++++++++--- 2 files changed, 33 insertions(+), 6 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index b2bd241bf..a9a555b37 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -15,7 +15,8 @@ class InformationEstimates public: // Main function std::vector findLeastInformativeLaser(std::vector const& range_scans); - kt_double finMutualInfo(std::vector const& range_scans); + std::vector findMutualInfo(std::vector const &range_scans); + private: // Mutual information kt_double calculateInformationContent(kt_double prob); diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 6c9f3bbac..7c13e4d70 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -17,9 +17,16 @@ InformationEstimates::InformationEstimates() m_obs_nu = 0.28; } - -kt_double InformationEstimates::findMutualInfo(std::vector const &range_scans) +std::vector InformationEstimates::findMutualInfo(std::vector const &range_scans) { + /* + Note: In fact, I should calculate the mutual information with all the elements + and then calculate the mutual information of each element. + + -- Keep this in mind: + Entre menos informacion mutua apora, mayor va a ser el resultado total del la operacion al extraerlo. + */ + std::vector result_vector; m_low_x = range_scans[0]->GetCorrectedPose().GetX(); m_low_y = range_scans[0]->GetCorrectedPose().GetY(); @@ -94,6 +101,10 @@ kt_double InformationEstimates::findMutualInfo(std::vector local_robot_cell = utils::grid_operations::getGridPosition(local_grid_robot_pose.GetPosition(), m_cell_resol); + // std::cout << " =================== " << std::endl; + // std::cout << "Lower limit cell: " << lower_limit_cell.GetX() << ", " << lower_limit_cell.GetY() << std::endl; + // std::cout << "Upper limit cell: " << upper_limit_cell.GetX() << ", " << upper_limit_cell.GetY() << std::endl; + karto::LaserRangeFinder *laser_range_finder = range_scans[n]->GetLaserRangeFinder(); kt_double range_threshold = laser_range_finder->GetRangeThreshold(); kt_double angle_increment = laser_range_finder->GetAngularResolution(); @@ -116,7 +127,12 @@ kt_double InformationEstimates::findMutualInfo(std::vector InformationEstimates::findLeastInformativeLaser(std::vector const& range_scans) { /** From bf2ede3d49a71d5f7d286f06638a7256c7e795ed Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Mon, 24 Oct 2022 13:54:16 -0500 Subject: [PATCH 69/83] Checkpoint for code refactoring Signed-off-by: Camilo Andres Alvis Bautista --- src/experimental/information_estimates.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 7c13e4d70..c86892859 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -19,6 +19,7 @@ InformationEstimates::InformationEstimates() std::vector InformationEstimates::findMutualInfo(std::vector const &range_scans) { + std::cout << "Vector size: " << range_scans.size() << std::endl; /* Note: In fact, I should calculate the mutual information with all the elements and then calculate the mutual information of each element. @@ -63,6 +64,9 @@ std::vector InformationEstimates::findMutualInfo(std::vector(dim_x / m_cell_resol); int n_cells_y = static_cast(dim_y / m_cell_resol); @@ -71,6 +75,7 @@ std::vector InformationEstimates::findMutualInfo(std::vector InformationEstimates::findMutualInfo(std::vector" << std::endl; for (auto &cell : vis_cells) { std::unordered_map meas_out_prob = computeMeasurementOutcomesHistogram(m_cell_probabilities.at(cell)); From 60930e380ee9382d6589f64d442d21e14e0f39b2 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Wed, 26 Oct 2022 09:59:30 -0500 Subject: [PATCH 70/83] Add new function for separaitng the jobs Signed-off-by: Camilo Andres Alvis Bautista --- .../experimental/information_estimates.hpp | 11 + src/experimental/information_estimates.cpp | 210 ++++++++++++++++++ 2 files changed, 221 insertions(+) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index a9a555b37..c2219e0ff 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -18,6 +18,9 @@ class InformationEstimates std::vector findMutualInfo(std::vector const &range_scans); private: + void resizeGridFromScans(std::vector const & range_scans); + std::vector iterateCells(std::vector const & range_scans); + // Mutual information kt_double calculateInformationContent(kt_double prob); kt_double measurementOutcomeEntropy(map_tuple const& meas_outcome); @@ -46,9 +49,17 @@ class InformationEstimates kt_double m_high_x; kt_double m_high_y; + kt_double m_upper_limit_x; + kt_double m_upper_limit_y; + kt_double m_lower_limit_x; + kt_double m_lower_limit_y; + // Map grids Eigen::MatrixXd m_mutual_grid; Eigen::MatrixXi m_visited_grid; + + kt_double m_map_dim_x; + kt_double m_map_dim_y; }; #endif diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index c86892859..09435fe7d 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -17,6 +17,216 @@ InformationEstimates::InformationEstimates() m_obs_nu = 0.28; } +void InformationEstimates::resizeGridFromScans(std::vector const & range_scans) +{ + m_lower_limit_x = range_scans[0]->GetCorrectedPose().GetX(); + m_lower_limit_y = range_scans[0]->GetCorrectedPose().GetY(); + m_upper_limit_x = range_scans[0]->GetCorrectedPose().GetX(); + m_upper_limit_y = range_scans[0]->GetCorrectedPose().GetY(); + + 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_lower_limit_x = std::min(pose.GetX(), m_lower_limit_x); + m_lower_limit_y = std::min(pose.GetY(), m_lower_limit_y); + + // Finding the higher limit pose + m_upper_limit_x = std::max(pose.GetX(), m_upper_limit_x); + m_upper_limit_y = std::max(pose.GetY(), m_upper_limit_y); + } + + // Map dimensions + m_map_dim_x = std::fabs(m_upper_limit_x - m_lower_limit_x) + (2 * m_max_sensor_range); + m_map_dim_y = std::fabs(m_upper_limit_y - m_lower_limit_y) + (2 * m_max_sensor_range); + + // Get 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::vector InformationEstimates::iterateCells(std::vector const & range_scans) +{ + // Since this information is the same for all the lectures I can move it outside + karto::LaserRangeFinder *laser_range_finder = range_scans[0]->GetLaserRangeFinder(); + kt_double range_threshold = laser_range_finder->GetRangeThreshold(); + kt_double angle_increment = laser_range_finder->GetAngularResolution(); + kt_double min_angle = laser_range_finder->GetMinimumAngle(); + kt_double max_angle = laser_range_finder->GetMaximumAngle(); + kt_double total_range = max_angle - min_angle; + + // This will be calculate donly once + kt_double lower_limit_x = std::max(0.0, m_lower_limit_x - (m_lower_limit_x - m_max_sensor_range) - m_max_sensor_range); + kt_double upper_limit_x = std::min(m_map_dim_x, m_upper_limit_x - (m_lower_limit_x - m_max_sensor_range) + m_max_sensor_range); + kt_double lower_limit_y = std::max(0.0, m_lower_limit_y - (m_lower_limit_y - m_max_sensor_range) - m_max_sensor_range); + kt_double upper_limit_y = std::min(m_map_dim_y, m_upper_limit_y - (m_lower_limit_y - m_max_sensor_range) + m_max_sensor_range); + + karto::Vector2 lower_limit_cell = utils::grid_operations::getGridPosition({ lower_limit_x, lower_limit_y }, m_cell_resol); + karto::Vector2 upper_limit_cell = utils::grid_operations::getGridPosition({ upper_limit_x, upper_limit_y }, m_cell_resol); + + std::vector result_vector; + + for (int n = 0; n < range_scans.size(); ++n) + { + m_mutual_grid.setZero(); + m_visited_grid.setZero(); + + std::vector> vis_cells; + for (int i = lower_limit_cell.GetX(); i < upper_limit_cell.GetX(); ++i) + { + for (int j = lower_limit_cell.GetY(); j < upper_limit_cell.GetY(); ++j) + { + // Calculate cell probabilities + for (int l = 0; l < range_scans.size(); ++l) + { + // Evaluate if the current laser that will be evaluated should be excluded + if (n == l) + { + continue; + } + + // This will be the pose for the current laser + karto::Pose2 pose_raw = range_scans[l]->GetCorrectedPose(); + karto::Pose2 local_grid_pose_raw { + pose_raw.GetX() - (m_lower_limit_x - m_max_sensor_range), + pose_raw.GetY() - (m_lower_limit_y - m_max_sensor_range), + pose_raw.GetHeading() + }; + karto::Vector2 pose_raw_cell = utils::grid_operations::getGridPosition(local_grid_pose_raw.GetPosition(), m_cell_resol); + + karto::Vector2 cell_center{i * m_cell_resol + (m_cell_resol / 2), j * m_cell_resol + (m_cell_resol / 2)}; + kt_double distance_to_cell = sqrt(pow(local_grid_pose_raw.GetX() - cell_center.GetX(), 2) + pow(local_grid_pose_raw.GetY() - cell_center.GetY(), 2)); + + if(distance_to_cell > m_max_sensor_range) + { + // It means the robot cannot see the current cell + continue; + } + + kt_double angle_to_cell = atan2(cell_center.GetY() - local_grid_pose_raw.GetY(), cell_center.GetX() - local_grid_pose_raw.GetX()); + kt_double heading = pose_raw.GetHeading(); + kt_double angle_between = angle_to_cell - heading; + + kt_double angle_diff = atan2(sin(angle_between), cos(angle_between)); + if (angle_diff < min_angle || angle_diff > max_angle) + { + // We are outside the laser FOV, but we cannot cancel it + // because we will not evaluate other cells + continue; + } + + int laser_idx = ((total_range / 2.0) + angle_between) / angle_increment; + kt_double laser_read_dist = range_scans[l]->GetRangeReadings()[laser_idx]; + kt_double laser_read_raw = range_scans[l]->GetRangeReadings()[laser_idx]; + if (laser_read_dist > range_threshold) + { + continue; + } + + if (laser_read_dist < distance_to_cell) + { + continue; + } + else + { + laser_read_dist = distance_to_cell; + } + + kt_double angle_to_cell_proj = angle_to_cell > 0 ? angle_to_cell : (2.0 * M_PI + angle_to_cell); + kt_double point_x_raw = local_grid_pose_raw.GetX() + (laser_read_dist * cos(angle_to_cell_proj)); + kt_double point_y_raw = local_grid_pose_raw.GetY() + (laser_read_dist * sin(angle_to_cell_proj)); + karto::Vector2 laser_beam_cell_raw = utils::grid_operations::getGridPosition({point_x_raw, point_y_raw}, m_cell_resol); + + // Inidividual cell limitslocal_grid_robot_pose + kt_double limit_x = i * m_cell_resol; + kt_double limit_y = j * m_cell_resol; + + karto::PointVectorDouble laser_readings = range_scans[l]->GetPointReadings(true); + karto::Vector2 transformed_laser{ + laser_readings[laser_idx].GetX() - m_lower_limit_x, + laser_readings[laser_idx].GetY() - m_lower_limit_y + }; + + // Calculate intersections + std::pair, std::vector> intersections = + utils::grid_operations::computeLineBoxIntersection( + local_grid_pose_raw.GetPosition(), + {point_x_raw, point_y_raw}, + pose_raw_cell, + laser_beam_cell_raw, + limit_x, + limit_y, + m_cell_resol + ); + + if (intersections.first.size() == 0) + { + continue; + } + + // Calculate distances to cell + // 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 = local_grid_pose_raw.GetPosition().Distance(intersection); + distances.push_back(distance); + std::sort(distances.begin(), distances.end()); + } + + // 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]) + }; + + karto::Vector2 cell{i, j}; + vis_cells.push_back(cell); + + // Appending new measurement outcomes for the current cell + appendCellProbabilities(probabilities, cell); + } + } + } + + for (auto &cell : vis_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; + } + // std::vector result_vector; + result_vector.push_back(m_mutual_grid.sum()); + + m_cell_probabilities.clear(); + } + return result_vector; +} + std::vector InformationEstimates::findMutualInfo(std::vector const &range_scans) { std::cout << "Vector size: " << range_scans.size() << std::endl; From c7d61166f0bfb29ae825135d660db9ea705d1c66 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Mon, 31 Oct 2022 16:06:39 -0500 Subject: [PATCH 71/83] Code refactoring phase 1 Signed-off-by: Camilo Andres Alvis Bautista --- .../experimental/information_estimates.hpp | 85 +- include/slam_toolbox/experimental/utils.hpp | 52 +- src/experimental/information_estimates.cpp | 842 +++++++----------- src/experimental/utils.cpp | 203 ++--- 4 files changed, 472 insertions(+), 710 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index c2219e0ff..22ad88e3f 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -13,19 +13,77 @@ class InformationEstimates virtual ~InformationEstimates() {} public: - // Main function - std::vector findLeastInformativeLaser(std::vector const& range_scans); - std::vector findMutualInfo(std::vector const &range_scans); + std::vector findMutualInfo(std::vector const& range_scans); private: - void resizeGridFromScans(std::vector const & range_scans); - std::vector iterateCells(std::vector const & range_scans); + + void calculateAndAppendCellProbabilities( + std::vector> & visited_cells, + std::vector const & distances, + karto::Vector2 const & cell + ); + + std::vector calculateBeamAndCellIntersections( + kt_bool & skip_cell_eval, + karto::Vector2 const & scan_position, + karto::Vector2 const & beam_read_point, + karto::Vector2 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 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 + ); + + int findClosestLaserIndexToCell( + karto::Vector2 const & cell, + karto::Pose2 const & grid_scan_pose); + + 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); - kt_double mutualInformationFromScans(std::vector const& range_scans, bool ignore_scan=false, int scan_idx=0); // Measurement outcomes probabilities void appendCellProbabilities(std::vector& measurements, karto::Vector2 const & cell); @@ -39,25 +97,24 @@ class InformationEstimates // Data structures std::map, std::vector>> 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; - kt_double m_low_x; - kt_double m_low_y; - kt_double m_high_x; - kt_double m_high_y; + 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; - // Map grids - Eigen::MatrixXd m_mutual_grid; - Eigen::MatrixXi m_visited_grid; - kt_double m_map_dim_x; kt_double m_map_dim_y; }; diff --git a/include/slam_toolbox/experimental/utils.hpp b/include/slam_toolbox/experimental/utils.hpp index c8cad3a27..918be8cf4 100644 --- a/include/slam_toolbox/experimental/utils.hpp +++ b/include/slam_toolbox/experimental/utils.hpp @@ -15,28 +15,46 @@ namespace utils { namespace grid_operations { - void updateCellLimits(std::vector& initial_x, std::vector& initial_y, std::vector& final_x, - std::vector& final_y, kt_double limit_x, kt_double limit_y, std::vector& cell_limits, - karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, kt_double resolution); + void updateCellLimits(std::vector& initial_x, + std::vector& initial_y, + std::vector& final_x, + std::vector& final_y, + kt_double limit_x, + kt_double limit_y, + std::vector& cell_limits, + karto::Vector2 const& robot_grid_pos, + karto::Vector2 const& final_grid_pos, + kt_double resolution + ); + int signum(int num); - std::vector> rayCasting(karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt); - karto::Vector2 getGridPosition(karto::Vector2 const& pose, kt_double resolution); + + karto::Vector2 getGridPosition( + karto::Vector2 const& position, + kt_double resolution + ); + karto::Vector2 calculateCellIntersectionPoints( - karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, - karto::Vector2 const & cell_start, karto::Vector2 const & cell_end); + karto::Vector2 const & laser_start, + karto::Vector2 const & laser_end, + karto::Vector2 const & cell_start, + karto::Vector2 const & cell_end + ); + std::pair, std::vector> computeLineBoxIntersection( - karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, - karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, - kt_double limit_x, kt_double limit_y, kt_double resolution); - } // namespace grid_operations + karto::Vector2 const & laser_start, + karto::Vector2 const & laser_end, + karto::Vector2 const& robot_grid_pos, + karto::Vector2 const& final_grid_pos, + kt_double limit_x, + kt_double limit_y, + kt_double resolution + ); - namespace probability_operations - { - kt_double calculateMeasurementOutcomeEntropy(std::tuple const& meas_outcome); - kt_double calculateProbabilityFromLogOdds(kt_double log); - kt_double calculateInformationContent(kt_double prob); + void clearVisitedCells(Eigen::MatrixXd & grid); - } // namespace probability_operations + void clearVisitedCells(Eigen::MatrixXi & grid); + } // namespace grid_operations namespace tuple_hash { diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 09435fe7d..e6d6d8fd4 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -1,22 +1,26 @@ #include +#include #include "slam_toolbox/experimental/information_estimates.hpp" +#include + InformationEstimates::InformationEstimates(kt_double sensor_range, kt_double resolution, kt_double lambda, kt_double nu) + : m_max_sensor_range{sensor_range}, + m_cell_resol{resolution}, + m_obs_lambda{lambda}, + m_obs_nu{nu} { - m_max_sensor_range = sensor_range; - m_cell_resol = resolution; - m_obs_lambda = lambda; - m_obs_nu = nu; } InformationEstimates::InformationEstimates() + : m_max_sensor_range{25.0}, + m_cell_resol{0.1}, + m_obs_lambda{0.35}, + m_obs_nu{0.28} { - m_max_sensor_range = 25.0; - m_cell_resol = 0.1; - m_obs_lambda = 0.35; - m_obs_nu = 0.28; } + void InformationEstimates::resizeGridFromScans(std::vector const & range_scans) { m_lower_limit_x = range_scans[0]->GetCorrectedPose().GetX(); @@ -57,610 +61,308 @@ void InformationEstimates::resizeGridFromScans(std::vector InformationEstimates::iterateCells(std::vector const & range_scans) + +int InformationEstimates::findClosestLaserIndexToCell( + kt_bool & skip_cell_eval, + kt_double const & angle_to_cell, + kt_double const & scan_pose_heading, + karto::LaserRangeFinder *laser_range_finder) { - // Since this information is the same for all the lectures I can move it outside - karto::LaserRangeFinder *laser_range_finder = range_scans[0]->GetLaserRangeFinder(); - kt_double range_threshold = laser_range_finder->GetRangeThreshold(); - kt_double angle_increment = laser_range_finder->GetAngularResolution(); - kt_double min_angle = laser_range_finder->GetMinimumAngle(); - kt_double max_angle = laser_range_finder->GetMaximumAngle(); - kt_double total_range = max_angle - min_angle; + kt_double angle_between = angle_to_cell - scan_pose_heading; + kt_double angle_diff = atan2(sin(angle_between), cos(angle_between)); - // This will be calculate donly once - kt_double lower_limit_x = std::max(0.0, m_lower_limit_x - (m_lower_limit_x - m_max_sensor_range) - m_max_sensor_range); - kt_double upper_limit_x = std::min(m_map_dim_x, m_upper_limit_x - (m_lower_limit_x - m_max_sensor_range) + m_max_sensor_range); - kt_double lower_limit_y = std::max(0.0, m_lower_limit_y - (m_lower_limit_y - m_max_sensor_range) - m_max_sensor_range); - kt_double upper_limit_y = std::min(m_map_dim_y, m_upper_limit_y - (m_lower_limit_y - m_max_sensor_range) + m_max_sensor_range); + 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 + skip_cell_eval = true; + } - karto::Vector2 lower_limit_cell = utils::grid_operations::getGridPosition({ lower_limit_x, lower_limit_y }, m_cell_resol); - karto::Vector2 upper_limit_cell = utils::grid_operations::getGridPosition({ upper_limit_x, upper_limit_y }, m_cell_resol); + kt_double laser_middle_point = ( laser_range_finder->GetMaximumAngle() - laser_range_finder->GetMinimumAngle()) / 2.0; + return (laser_middle_point + angle_between) / laser_range_finder->GetAngularResolution(); +} - std::vector result_vector; - for (int n = 0; n < range_scans.size(); ++n) +std::vector InformationEstimates::calculateBeamAndCellIntersections( + kt_bool & skip_cell_eval, + karto::Vector2 const & scan_position, + karto::Vector2 const & beam_read_point, + karto::Vector2 const & cell) +{ + karto::Vector2 scan_position_cell = utils::grid_operations::getGridPosition(scan_position, m_cell_resol); + + karto::Vector2 beam_read_cell = + utils::grid_operations::getGridPosition({beam_read_point.GetX(), beam_read_point.GetY()}, m_cell_resol); + + std::pair, std::vector> intersections = + utils::grid_operations::computeLineBoxIntersection( + scan_position, + beam_read_point, + scan_position_cell, + beam_read_cell, + cell.GetX() * m_cell_resol, + cell.GetY() * m_cell_resol, + m_cell_resol + ); + + if (intersections.first.size() == 0) { - m_mutual_grid.setZero(); - m_visited_grid.setZero(); + skip_cell_eval = true; + } - std::vector> vis_cells; - for (int i = lower_limit_cell.GetX(); i < upper_limit_cell.GetX(); ++i) - { - for (int j = lower_limit_cell.GetY(); j < upper_limit_cell.GetY(); ++j) - { - // Calculate cell probabilities - for (int l = 0; l < range_scans.size(); ++l) - { - // Evaluate if the current laser that will be evaluated should be excluded - if (n == l) - { - continue; - } - - // This will be the pose for the current laser - karto::Pose2 pose_raw = range_scans[l]->GetCorrectedPose(); - karto::Pose2 local_grid_pose_raw { - pose_raw.GetX() - (m_lower_limit_x - m_max_sensor_range), - pose_raw.GetY() - (m_lower_limit_y - m_max_sensor_range), - pose_raw.GetHeading() - }; - karto::Vector2 pose_raw_cell = utils::grid_operations::getGridPosition(local_grid_pose_raw.GetPosition(), m_cell_resol); - - karto::Vector2 cell_center{i * m_cell_resol + (m_cell_resol / 2), j * m_cell_resol + (m_cell_resol / 2)}; - kt_double distance_to_cell = sqrt(pow(local_grid_pose_raw.GetX() - cell_center.GetX(), 2) + pow(local_grid_pose_raw.GetY() - cell_center.GetY(), 2)); - - if(distance_to_cell > m_max_sensor_range) - { - // It means the robot cannot see the current cell - continue; - } - - kt_double angle_to_cell = atan2(cell_center.GetY() - local_grid_pose_raw.GetY(), cell_center.GetX() - local_grid_pose_raw.GetX()); - kt_double heading = pose_raw.GetHeading(); - kt_double angle_between = angle_to_cell - heading; - - kt_double angle_diff = atan2(sin(angle_between), cos(angle_between)); - if (angle_diff < min_angle || angle_diff > max_angle) - { - // We are outside the laser FOV, but we cannot cancel it - // because we will not evaluate other cells - continue; - } - - int laser_idx = ((total_range / 2.0) + angle_between) / angle_increment; - kt_double laser_read_dist = range_scans[l]->GetRangeReadings()[laser_idx]; - kt_double laser_read_raw = range_scans[l]->GetRangeReadings()[laser_idx]; - if (laser_read_dist > range_threshold) - { - continue; - } - - if (laser_read_dist < distance_to_cell) - { - continue; - } - else - { - laser_read_dist = distance_to_cell; - } - - kt_double angle_to_cell_proj = angle_to_cell > 0 ? angle_to_cell : (2.0 * M_PI + angle_to_cell); - kt_double point_x_raw = local_grid_pose_raw.GetX() + (laser_read_dist * cos(angle_to_cell_proj)); - kt_double point_y_raw = local_grid_pose_raw.GetY() + (laser_read_dist * sin(angle_to_cell_proj)); - karto::Vector2 laser_beam_cell_raw = utils::grid_operations::getGridPosition({point_x_raw, point_y_raw}, m_cell_resol); - - // Inidividual cell limitslocal_grid_robot_pose - kt_double limit_x = i * m_cell_resol; - kt_double limit_y = j * m_cell_resol; - - karto::PointVectorDouble laser_readings = range_scans[l]->GetPointReadings(true); - karto::Vector2 transformed_laser{ - laser_readings[laser_idx].GetX() - m_lower_limit_x, - laser_readings[laser_idx].GetY() - m_lower_limit_y - }; - - // Calculate intersections - std::pair, std::vector> intersections = - utils::grid_operations::computeLineBoxIntersection( - local_grid_pose_raw.GetPosition(), - {point_x_raw, point_y_raw}, - pose_raw_cell, - laser_beam_cell_raw, - limit_x, - limit_y, - m_cell_resol - ); - - if (intersections.first.size() == 0) - { - continue; - } - - // Calculate distances to cell - // 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 = local_grid_pose_raw.GetPosition().Distance(intersection); - distances.push_back(distance); - std::sort(distances.begin(), distances.end()); - } - - // 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]) - }; - - karto::Vector2 cell{i, j}; - vis_cells.push_back(cell); - - // Appending new measurement outcomes for the current cell - appendCellProbabilities(probabilities, cell); - } - } - } + // 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()); + } - for (auto &cell : vis_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); - } + return distances; +} - // Mutual information of cell x, y given a set of measurements - m_mutual_grid(cell.GetX(), cell.GetY()) = 1.0 - cell_mutual_inf; - } - // std::vector result_vector; - result_vector.push_back(m_mutual_grid.sum()); - m_cell_probabilities.clear(); +void InformationEstimates::adjustBeamReadingDistance( + kt_bool & skip_cell_eval, + kt_double & beam_distance, + kt_double const & distance_to_cell, + karto::LaserRangeFinder *laser_range_finder) +{ + if ((beam_distance > laser_range_finder->GetRangeThreshold()) || (beam_distance < distance_to_cell)) + { + skip_cell_eval = true; } - return result_vector; + + beam_distance = distance_to_cell; } -std::vector InformationEstimates::findMutualInfo(std::vector const &range_scans) + +void InformationEstimates::calculateAndAppendCellProbabilities( + std::vector> & visited_cells, + std::vector const & distances, + karto::Vector2 const & cell) { - std::cout << "Vector size: " << range_scans.size() << std::endl; - /* - Note: In fact, I should calculate the mutual information with all the elements - and then calculate the mutual information of each element. + // 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]) + }; - -- Keep this in mind: - Entre menos informacion mutua apora, mayor va a ser el resultado total del la operacion al extraerlo. - */ - std::vector result_vector; - m_low_x = range_scans[0]->GetCorrectedPose().GetX(); - m_low_y = range_scans[0]->GetCorrectedPose().GetY(); + visited_cells.push_back( {cell.GetX(), cell.GetY()}); - m_high_x = range_scans[0]->GetCorrectedPose().GetX(); - m_high_y = range_scans[0]->GetCorrectedPose().GetY(); + // Appending new measurement outcomes for the current cell + appendCellProbabilities(probabilities, cell); +} - for (const auto &scan : range_scans) +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) { - if (scan == nullptr) + // Skip the given scan, because we are evaluating the mutual information + // of the group, excluding it. + if (scan_to_skip == s) { continue; } + kt_bool skip_cell_eval = false; + + // Scan pose + karto::Pose2 scan_pose = range_scans[s]->GetCorrectedPose(); + karto::Pose2 grid_scan_pose { + scan_pose.GetX() - (m_lower_limit_x - m_max_sensor_range), + scan_pose.GetY() - (m_lower_limit_y - 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) + { + // It means the robot cannot see the current cell + continue; + } - karto::Pose2 pose = scan->GetCorrectedPose(); - karto::PointVectorDouble laser_readings = scan->GetPointReadings(false); - - kt_double minimumAngle = scan->GetLaserRangeFinder()->GetMinimumAngle(); - kt_double angularResolution = scan->GetLaserRangeFinder()->GetAngularResolution(); - - kt_double rangeReading = scan->GetRangeReadings()[0]; - int size = scan->GetRangeReadingsVector().size(); - - // Finding the lower limit pose - m_low_x = std::min(pose.GetX(), m_low_x); - m_low_y = std::min(pose.GetY(), m_low_y); - - // Finding the higher limit pose - m_high_x = std::max(pose.GetX(), m_high_x); - m_high_y = std::max(pose.GetY(), m_high_y); - } - - // Map dimensions - kt_double dim_x = std::fabs(m_high_x - m_low_x) + (2 * m_max_sensor_range); - kt_double dim_y = std::fabs(m_high_y - m_low_y) + (2 * m_max_sensor_range); - - // std::cout << "X Dimension: " << dim_x << std::endl; - // std::cout << "Y Dimension: " << dim_y << std::endl; - - // Get the number of cells - int n_cells_x = static_cast(dim_x / m_cell_resol); - int n_cells_y = static_cast(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); - - // for (int n = 0; n < 1; ++n) - int skip_laser = 0; - for (int n = 0; n < range_scans.size(); ++n) - { - m_mutual_grid.setZero(); - m_visited_grid.setZero(); - - karto::Pose2 robot_pose_raw = range_scans[n]->GetCorrectedPose(); - // Get the current robot pose and extract the extra range for locating it at the lowest X and Y - karto::Pose2 local_grid_robot_pose{robot_pose_raw.GetX() - (m_low_x - m_max_sensor_range), robot_pose_raw.GetY() - (m_low_y - m_max_sensor_range), robot_pose_raw.GetHeading()}; - // std::cout << "Local robot pose: " << local_grid_robot_pose.GetX() << ", " << local_grid_robot_pose.GetY() << std::endl; - - // @NIT - // This is an ideal point to add an assertion, since the robot pose must be positive in all of the scenarios - - // Minimum X point - kt_double lower_limit_x = std::max(0.0, local_grid_robot_pose.GetX() - m_max_sensor_range); - kt_double upper_limit_x = std::min(dim_x, local_grid_robot_pose.GetX() + m_max_sensor_range); - kt_double lower_limit_y = std::max(0.0, local_grid_robot_pose.GetY() - m_max_sensor_range); - kt_double upper_limit_y = std::min(dim_y, local_grid_robot_pose.GetY() + m_max_sensor_range); - - karto::Vector2 lower_limit{lower_limit_x, lower_limit_y}; - karto::Vector2 upper_limit{upper_limit_x, upper_limit_y}; - - // @NIT - // I can use an rvalue reference here - karto::Vector2 lower_limit_cell = utils::grid_operations::getGridPosition(lower_limit, m_cell_resol); - karto::Vector2 upper_limit_cell = utils::grid_operations::getGridPosition(upper_limit, m_cell_resol); - - karto::Vector2 local_robot_cell = utils::grid_operations::getGridPosition(local_grid_robot_pose.GetPosition(), m_cell_resol); - - // std::cout << " =================== " << std::endl; - // std::cout << "Lower limit cell: " << lower_limit_cell.GetX() << ", " << lower_limit_cell.GetY() << std::endl; - // std::cout << "Upper limit cell: " << upper_limit_cell.GetX() << ", " << upper_limit_cell.GetY() << std::endl; - - karto::LaserRangeFinder *laser_range_finder = range_scans[n]->GetLaserRangeFinder(); - kt_double range_threshold = laser_range_finder->GetRangeThreshold(); - kt_double angle_increment = laser_range_finder->GetAngularResolution(); - kt_double min_angle = laser_range_finder->GetMinimumAngle(); - kt_double max_angle = laser_range_finder->GetMaximumAngle(); - kt_double total_range = max_angle - min_angle; - - std::vector> vis_cells; + int laser_idx = findClosestLaserIndexToCell( + skip_cell_eval, + angle_to_cell, + scan_pose.GetHeading(), + laser_range_finder + ); - bool printed = false; - for (int i = lower_limit_cell.GetX(); i < upper_limit_cell.GetX(); ++i) + if (skip_cell_eval) { - for (int j = lower_limit_cell.GetY(); j < upper_limit_cell.GetY(); ++j) - { - // Here I would need to find which of the scans can see this cell - // this is with the distance we have found - // Euclidean distance between cell center and robot position - for (int l = 0; l < range_scans.size(); ++l) - { - // Evaluate if the current laser that will be evaluated should be excluded - if (n == l) - { - // Current laser will noe be evaluated - if (!printed) - { - // std::cout << "Skipping: " << l << std::endl; - printed = true; - } - continue; - } - - // Evaluate if the laser can see the current cell - karto::Vector2 cell_center{i * m_cell_resol + (m_cell_resol / 2), j * m_cell_resol + (m_cell_resol / 2)}; - kt_double distance_to_cell = sqrt(pow(local_grid_robot_pose.GetX() - cell_center.GetX(), 2) + pow(local_grid_robot_pose.GetY() - cell_center.GetY(), 2)); - if(distance_to_cell > m_max_sensor_range) - { - // It means the robot cannot see the current cell - continue; - } - - // Continue the staff here - kt_double angle_to_cell = atan2(cell_center.GetY() - local_grid_robot_pose.GetY(), cell_center.GetX() - local_grid_robot_pose.GetX()); - kt_double robot_heading = robot_pose_raw.GetHeading(); - kt_double angle_between = angle_to_cell - robot_heading; - - kt_double angle_diff = atan2(sin(angle_between), cos(angle_between)); - if (angle_diff < min_angle || angle_diff > max_angle) - { - // We are outside the laser FOV, but we cannot cancel it - // because we will not evaluate other cells - continue; - } - - int laser_idx = ((total_range / 2.0) + angle_between) / angle_increment; - kt_double laser_read_dist = range_scans[n]->GetRangeReadings()[laser_idx]; - kt_double laser_read_raw = range_scans[n]->GetRangeReadings()[laser_idx]; - if (laser_read_dist > range_threshold) - { - continue; - } - - if (laser_read_dist < distance_to_cell) - { - continue; - } - else - { - laser_read_dist = distance_to_cell; - } - - kt_double angle_to_cell_proj = angle_to_cell > 0 ? angle_to_cell : (2.0 * M_PI + angle_to_cell); - kt_double point_x_raw = local_grid_robot_pose.GetX() + (laser_read_raw * cos(angle_to_cell_proj)); - kt_double point_y_raw = local_grid_robot_pose.GetY() + (laser_read_raw * sin(angle_to_cell_proj)); - karto::Vector2 laser_beam_cell_raw = utils::grid_operations::getGridPosition({point_x_raw, point_y_raw}, m_cell_resol); - - // Inidividual cell limits - kt_double limit_x = i * m_cell_resol; - kt_double limit_y = j * m_cell_resol; - - karto::PointVectorDouble laser_readings = range_scans[n]->GetPointReadings(true); - karto::Vector2 transformed_laser{laser_readings[laser_idx].GetX() - m_low_x, laser_readings[laser_idx].GetY() - m_low_y}; - - std::pair, std::vector> intersections = utils::grid_operations::computeLineBoxIntersection( - local_grid_robot_pose.GetPosition(), {point_x_raw, point_y_raw}, local_robot_cell, laser_beam_cell_raw, limit_x, limit_y, m_cell_resol); - - if (intersections.first.size() == 0) - { - continue; - } - - // 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 = local_grid_robot_pose.GetPosition().Distance(intersection); - distances.push_back(distance); - std::sort(distances.begin(), distances.end()); - } - - // 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])}; - - karto::Vector2 cell{i, j}; - vis_cells.push_back(cell); - - // Appending new measurement outcomes for the current cell - appendCellProbabilities(probabilities, cell); - } - } + continue; } - // std::cout << "<------------------------->" << std::endl; - for (auto &cell : vis_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); - } + kt_double beam_read_dist = range_scans[s]->GetRangeReadings()[laser_idx]; + adjustBeamReadingDistance(skip_cell_eval, beam_read_dist, distance_to_cell, laser_range_finder); - // Mutual information of cell x, y given a set of measurements - m_mutual_grid(cell.GetX(), cell.GetY()) = 1.0 - cell_mutual_inf; + if (skip_cell_eval) + { + continue; } - // std::cout << "Mutual information: " << m_mutual_grid.sum() << std::endl; - result_vector.push_back(m_mutual_grid.sum()); - - // Puedo tener un vector que me almacene el resultado - // Entonces el push va a ser secuencial - // Cada resultado va a demostrar la informacion mutua calculada cuando se toma todo el grupo - el scan de interes - // Por ejemplo vct[0] = Index 0 es el calculo de la informacion mutua excluyendo el primer scan - // Por ejemplo vct[1] = Index 1 es el calculo de la informacion mutua excluyendo el segundo scan - // Por ejemplo vct[2] = Index 2 es el calculo de la informacion mutua excluyendo el tercer scan - m_cell_probabilities.clear(); - } - return result_vector; -} + kt_double angle_to_cell_proj = angle_to_cell > 0 ? angle_to_cell : (2.0 * M_PI + angle_to_cell); + karto::Vector2 beam_read_point { + grid_scan_pose.GetX() + (beam_read_dist * cos(angle_to_cell_proj)), + grid_scan_pose.GetY() + (beam_read_dist * sin(angle_to_cell_proj)) + }; -std::vector InformationEstimates::findLeastInformativeLaser(std::vector const& range_scans) -{ - /** - * Find and return the Laser Scan index which provide the less mutual information in a set of Laser Scans - * Arguments: - * range_scans [std::vector]: Vector of LocalizedRangeScan pointers - * Return: - * std::vector: Vector containing the mutual information fo each of the processed laser scans - */ + std::vector distances = calculateBeamAndCellIntersections( + skip_cell_eval, + grid_scan_pose.GetPosition(), + beam_read_point, + cell + ); - // Get the robot poses for creating our new local coordinate system - for (const auto & scan : range_scans) - { - if (scan == nullptr) + if (skip_cell_eval) { continue; } - karto::Pose2 pose = scan->GetCorrectedPose(); - - // Finding the closest pose - m_low_x = pose.GetX() < m_low_x ? pose.GetX() : m_low_x; - m_low_y = pose.GetY() < m_low_y ? pose.GetY() : m_low_y; - - // Finding the farthest pose - m_high_x = pose.GetX() > m_high_x ? pose.GetX() : m_high_x; - m_high_y = pose.GetY() > m_high_y ? pose.GetY() : m_high_y; + calculateAndAppendCellProbabilities( + visited_cells, + distances, + cell + ); } +} - // Margins for the closest points - m_low_x -= m_max_sensor_range; - m_low_y -= m_max_sensor_range; - - // Margins for the farthest points - m_high_x += m_max_sensor_range; - m_high_y += m_max_sensor_range; - - // Map dimensions - kt_double dist_x = std::fabs(m_high_x) + std::fabs(m_low_x); - kt_double dist_y = std::fabs(m_high_y) + std::fabs(m_low_y); - - // Number of X and Y cells - int n_cells_x = static_cast(dist_x / m_cell_resol); - int n_cells_y = static_cast(dist_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::vector scans_mut_info; - scans_mut_info.reserve(range_scans.size()); - // Total mutual information - kt_double map_mut_info = mutualInformationFromScans(range_scans); - // Clearing the cells for the next time it is called - m_mutual_grid.setZero(); +std::vector InformationEstimates::getScanGroupMutualInformation( + std::vector const & range_scans) +{ + // Since this information is the same for all the lectures I can move it outside + karto::LaserRangeFinder *laser_range_finder = range_scans[0]->GetLaserRangeFinder(); + kt_double range_threshold = laser_range_finder->GetRangeThreshold(); + kt_double angle_increment = laser_range_finder->GetAngularResolution(); + kt_double min_angle = laser_range_finder->GetMinimumAngle(); + kt_double max_angle = laser_range_finder->GetMaximumAngle(); + kt_double total_range = max_angle - min_angle; + std::vector scans_mutual_information; - for (int i = 0; i < range_scans.size(); ++i) + for (int n = 0; n < range_scans.size(); ++n) { - // Find mutual information when extracting one laser scan - kt_double temp_mut_info = mutualInformationFromScans(range_scans, true, i); - scans_mut_info.emplace_back(map_mut_info - temp_mut_info); - // Clearing the cells for the next time it is called m_mutual_grid.setZero(); - } + m_visited_grid.setZero(); - return scans_mut_info; + std::vector> visited_cells = getScanGroupVisitedCells(range_scans, laser_range_finder, n); + calculateScanGroupMutualInformation(visited_cells, scans_mutual_information); + } + return scans_mutual_information; } -kt_double InformationEstimates::mutualInformationFromScans(std::vector const& range_scans, bool ignore_scan, int scan_idx) +void InformationEstimates::calculateScanGroupMutualInformation( + std::vector> const & visited_cells, + std::vector & scans_mutual_information) { - /** - * Append measured porbabilities for the given cell - * Arguments: - * range_scans [std::vector]: Vector of LocalizedRangeScan pointers - * ignore_scan [bool]: Indicates the type of calculation - * scan_idx [int]: Index within the set to be ignored - * Return: - * kt_double - */ - - int curr_idx = 0; - for (auto & scan : range_scans) + for (auto &cell : visited_cells) { - if (curr_idx == scan_idx && ignore_scan) + 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) { - ++curr_idx; - continue; + cell_mutual_inf += pair.second * measurementOutcomeEntropy(pair.first); } - karto::Pose2 robot_pose_raw = scan->GetCorrectedPose(); + // 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(); +} - // Moving the pose to our local system - karto::Pose2 robot_pose_tf(robot_pose_raw.GetX() - m_low_x, robot_pose_raw.GetY() - m_low_y, robot_pose_raw.GetHeading()); - karto::PointVectorDouble laser_readings = scan->GetPointReadings(true); - karto::Vector2 robot_grid = utils::grid_operations::getGridPosition(robot_pose_tf.GetPosition(), m_cell_resol); +std::vector> InformationEstimates::getScanGroupVisitedCells( + std::vector const & range_scans, + karto::LaserRangeFinder *laser_range_finder, + int const & scan_to_skip) +{ + std::vector> visited_cells; - // Set as false the current boolean map - m_visited_grid.setZero(); + 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 < laser_readings.size(); ++i) + for (int i = 0; i < x_upper_limit; ++i) + { + for (int j = 0; j < y_upper_limit; ++j) { - karto::Vector2 tf_laser{laser_readings[i].GetX() - m_low_x, laser_readings[i].GetY() - m_low_y}; - - // Laser final cell - karto::Vector2 beam_grid = utils::grid_operations::getGridPosition(tf_laser, m_cell_resol); + calculateCellProbabilities(range_scans, visited_cells, {i, j}, laser_range_finder, scan_to_skip); + } + } + return visited_cells; +} - // Visited cells by this laser beam - std::vector> cells = utils::grid_operations::rayCasting(robot_grid, beam_grid); - for (auto & cell : cells) - { - // Inidividual cell limits - kt_double limit_x = cell.GetX() * m_cell_resol; - kt_double limit_y = cell.GetY() * m_cell_resol; - - std::pair, std::vector> intersections = utils::grid_operations::computeLineBoxIntersection( - robot_pose_tf.GetPosition(), tf_laser, robot_grid, beam_grid, limit_x, limit_y, m_cell_resol); - - if (intersections.first.size() == 0) - continue; - - // 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 = robot_pose_tf.GetPosition().Distance(intersection); - distances.push_back(distance); - } - - // 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]) - }; - - // Appending new measurement outcomes for the current cell - appendCellProbabilities(probabilities, cell); - - // Compute all the possible combinations for the current cell - algorithm 1 - 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 * utils::probability_operations::calculateMeasurementOutcomeEntropy(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; +std::vector InformationEstimates::findMutualInfo( + std::vector const &range_scans) +{ + std::cout << "Vector size: " << range_scans.size() << std::endl; - } - } - ++curr_idx; - } - m_cell_probabilities.clear(); + std::vector result_vector; + resizeGridFromScans(range_scans); - return m_mutual_grid.sum(); + std::vector mutual_information_vct = getScanGroupMutualInformation(range_scans); + return mutual_information_vct; } -void InformationEstimates::appendCellProbabilities(std::vector& measurements, karto::Vector2 const & cell) + +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 + * measurements [std::vector]: Vector of LocalizedRangeScan pointers + * cell [karto::Vector2]: Cell for appending the data * Return: - * Void - */ - std::map, std::vector>>::iterator it_cell; + * 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 + /** + * 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) + if (m_visited_grid(cell.GetX(), cell.GetY()) == 1) // It must be the longitude { - // Compare the unknown probability, the smallest it is the most information we will have - // from the occupied or free state - int idx = it_cell->second.size(); - if(measurements[2] < it_cell->second[idx][2]) + /** + * Compare the unknown probability, the smallest it is the most information we will have + * from the occupied of free state + */ + int idx = it_cell->second.size() - 1; + if (measurements[2] < it_cell->second[idx][2]) { - // Replacing + /** + * Replacing + */ it_cell->second[idx][0] = measurements[0]; it_cell->second[idx][1] = measurements[1]; it_cell->second[idx][2] = measurements[2]; @@ -668,14 +370,48 @@ void InformationEstimates::appendCellProbabilities(std::vector& measu } else { - // Cell is already in the map, only add the next measurement outcome - it_cell->second.push_back({measurements[0], measurements[1], measurements[2]}); + /** + * Cell is already in the map, only add the next measurement outcome + */ + it_cell->second.push_back(measurements); m_visited_grid(cell.GetX(), cell.GetY()) = 1; } } } -void InformationEstimates::insertMeasurementOutcome(map_tuple tuple, kt_double probability, std::unordered_map& map) + +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; @@ -690,16 +426,18 @@ void InformationEstimates::insertMeasurementOutcome(map_tuple tuple, kt_double p } } -std::unordered_map InformationEstimates::computeMeasurementOutcomesHistogram(std::vector>& meas_outcm) + +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} + * 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: Map of combination, it contains the combination and its probability + */ std::unordered_map probabilities_map; std::unordered_map temp_map; @@ -730,7 +468,7 @@ std::unordered_map m_max_sensor_range) ? m_max_sensor_range : range_1; range_2 = (range_2 > m_max_sensor_range) ? m_max_sensor_range : range_2; - return m_obs_nu * (exp(-m_obs_lambda*range_1) - exp(-m_obs_lambda*range_2)); + // 12.2 because that is roughly the max range when lambda is equals to 1 + kt_double lambda = 12.2 / m_max_sensor_range; + + return -(exp(-lambda * range_2) - exp(-lambda * range_1)); } diff --git a/src/experimental/utils.cpp b/src/experimental/utils.cpp index 1a9ec2efc..c1d2d2d36 100644 --- a/src/experimental/utils.cpp +++ b/src/experimental/utils.cpp @@ -5,10 +5,20 @@ namespace utils { namespace grid_operations { - void updateCellLimits(std::vector& initial_x, std::vector& initial_y, std::vector& final_x, - std::vector& final_y, kt_double limit_x, kt_double limit_y, std::vector& cell_limits, karto::Vector2 const& robot_grid_pos, - karto::Vector2 const& final_grid_pos, kt_double resolution) + void updateCellLimits( + std::vector& initial_x, + std::vector& initial_y, + std::vector& final_x, + std::vector& final_y, + kt_double limit_x, + kt_double limit_y, + std::vector& cell_limits, + karto::Vector2 const& robot_grid_pos, + karto::Vector2 const& final_grid_pos, + kt_double resolution + ) { + // This function needs a verification /** * To calculate grid limits for intersection * Arguments: @@ -27,7 +37,6 @@ namespace utils */ if (final_grid_pos.GetX() < robot_grid_pos.GetX() && final_grid_pos.GetY() >= robot_grid_pos.GetY()) { - // X greater and Y greater. WRO final points final_x[0] = limit_x + resolution; final_x[2] = limit_x + resolution; @@ -37,7 +46,6 @@ namespace utils if (final_grid_pos.GetX() >= robot_grid_pos.GetX() && final_grid_pos.GetY() < robot_grid_pos.GetY()) { - // X greater and Y minor. WRO final points initial_y[2] = limit_y - resolution; initial_y[3] = limit_y - resolution; @@ -47,27 +55,9 @@ namespace utils cell_limits[2] = limit_y - resolution; cell_limits[3] = limit_y; } - - if (final_grid_pos.GetX() < robot_grid_pos.GetX() && final_grid_pos.GetY() < robot_grid_pos.GetY()) - { - // X minor and Y minor. WRO final points - initial_x[2] = limit_x - resolution; - initial_x[3] = limit_x - resolution; - initial_y[2] = limit_y - resolution; - initial_y[3] = limit_y - resolution; - - final_x[0] = limit_x - resolution; - final_x[2] = limit_x - resolution; - final_y[1] = limit_y - resolution; - final_y[3] = limit_y - resolution; - - cell_limits[0] = limit_x - resolution; - cell_limits[1] = limit_x; - cell_limits[2] = limit_y - resolution; - cell_limits[3] = limit_y; - } } + int signum(int num) { /** @@ -82,71 +72,35 @@ namespace utils return 0; } - std::vector> rayCasting( - karto::Vector2 const& initial_pt, karto::Vector2 const& final_pt) + + void clearVisitedCells(Eigen::MatrixXd & grid) { /** - * Find the set of cells hit by a laser beam (Bresenham algorithm) + * Clear the given floating Eigen::Matrix * Arguments: - * initial_pt [karto::Vector2]: Laser beam initial position - * final_pt [karto::Vector2]: Laser beam final position - * Return: - * std::vector>: Vector of cells visited by the given laser beam + * grid [Eigen::Matrix]: Grid for cleaning + * Return: + * Void */ - std::vector> cells; - - int x = initial_pt.GetX(); - int y = initial_pt.GetY(); - - int delta_x = abs(final_pt.GetX() - initial_pt.GetX()); - int delta_y = abs(final_pt.GetY() - initial_pt.GetY()); - - int s_x = signum(final_pt.GetX() - initial_pt.GetX()); - int s_y = signum(final_pt.GetY() - initial_pt.GetY()); - bool interchange = false; - - if (delta_y > delta_x) - { - int temp = delta_x; - delta_x = delta_y; - delta_y = temp; - interchange = true; - } - else { interchange = false; } - - int a_res = 2 * delta_y; - int b_res = 2 * (delta_y - delta_x); - int e_res = (2 * delta_y) - delta_x; - - cells.push_back(karto::Vector2{x, y}); + grid.setZero(); + } - for (int i = 1; i < delta_x; ++i) - { - if (e_res < 0) - { - if (interchange) { y += s_y; } - else { x += s_x; } - e_res += a_res; - } - else - { - y += s_y; - x += s_x; - e_res += b_res; - } - cells.push_back(karto::Vector2{x, y}); - } - // Delete the current robot cell - cells.erase(cells.begin()); - // Adding last hit cell to the set - cells.push_back(karto::Vector2{final_pt.GetX(), final_pt.GetY()}); + void clearVisitedCells(Eigen::MatrixXi & grid) + { + /** + * Clear the given integer Eigen::Matrix + * Arguments: + * grid [Eigen::Matrix]: Grid for cleaning + * Return: + * Void + */ - return cells; + grid.setZero(); } - karto::Vector2 getGridPosition(karto::Vector2 const& pose, kt_double resolution) + karto::Vector2 getGridPosition(karto::Vector2 const& position, kt_double resolution) { /** * Mapping a continuous position into a grid position @@ -156,12 +110,13 @@ namespace utils * Return: * karto::Vector2: Grid position */ - int x_cell = floor((pose.GetX() / resolution)); - int y_cell = floor((pose.GetY() / resolution)); + int x_cell = floor((position.GetX() / resolution)); + int y_cell = floor((position.GetY() / resolution)); return karto::Vector2{x_cell, y_cell}; } + karto::Vector2 calculateCellIntersectionPoints(karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, karto::Vector2 const & cell_start, karto::Vector2 const & cell_end) { @@ -175,6 +130,7 @@ namespace utils * Return: * karto::Vector2: Intersection point */ + kt_double x1 = laser_start.GetX(); kt_double x2 = laser_end.GetX(); kt_double x3 = cell_start.GetX(); @@ -190,8 +146,7 @@ namespace utils karto::Vector2 intersection; if (den == 0.0f) { - // Parallel lines - return {}; + return{}; } else { @@ -200,13 +155,20 @@ namespace utils intersection.SetX(x); intersection.SetY(y); } + return intersection; } + std::pair, std::vector> computeLineBoxIntersection( - karto::Vector2 const & laser_start, karto::Vector2 const & laser_end, - karto::Vector2 const& robot_grid_pos, karto::Vector2 const& final_grid_pos, - kt_double limit_x, kt_double limit_y, kt_double resolution) + karto::Vector2 const & laser_start, + karto::Vector2 const & laser_end, + karto::Vector2 const& robot_grid_pos, + karto::Vector2 const& final_grid_pos, + kt_double limit_x, + kt_double limit_y, + kt_double resolution + ) { /** * Compute intersection between a cell and a laser beam @@ -238,11 +200,26 @@ namespace utils std::vector inter_x, inter_y; + /* + initial {limit_x, limit_y} + final {limit_x + res, limit_y} + + initial {limit_x, limit_y} + final {limit_x, limit_y + res} + + initial {limit_x + res, limit_y + res} + final {limit_x + res, limit_y} + + initial {limit_x + res, limit_y + res} + final {limit_x, limit_y + res} + */ + for (int k = 0; k < 4; ++k) { karto::Vector2 start{initial_x[k], initial_y[k]}; karto::Vector2 end{final_x[k], final_y[k]}; karto::Vector2 intersection = calculateCellIntersectionPoints(laser_start, laser_end, start, end); + if(intersection.Length() != 0) { if ((fabs(intersection.GetX()) >= (fabs(cell_limits[0]) - 0.001)) && @@ -258,61 +235,9 @@ namespace utils } } } + return std::pair, std::vector>{inter_x, inter_y}; } } // namespace grid_operations - - namespace probability_operations - { - kt_double 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 calculateMeasurementOutcomeEntropy(std::tuple const& meas_outcome) - { - /** - * 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 - * Arguments: - * meas_outcome [map_tuple]: Measurement outcome in the form {p_free, p_occ, p_unk} - * Return: - * kt_double: Measurement outcome entropy - */ - - // Log of probabilities (Free, Occupied, Not observed) - kt_double l_free = log(0.3 / (1.0 - 0.3)); - kt_double l_occ = log(0.7 / (1.0 - 0.7)); - kt_double l_o = log(0.5 / (1.0 - 0.5)); - - 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 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))); - } - } // namespace probability_operations - } // namespace utils From 156be31bf47fe0df33ff04e75bb25cd1fbd3bc73 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Mon, 31 Oct 2022 16:23:36 -0500 Subject: [PATCH 72/83] Remove undesired folders Signed-off-by: Camilo Andres Alvis Bautista --- .vscode/.gitignore | 1 - launch/__pycache__/.gitignore | 1 - 2 files changed, 2 deletions(-) delete mode 100644 .vscode/.gitignore delete mode 100644 launch/__pycache__/.gitignore diff --git a/.vscode/.gitignore b/.vscode/.gitignore deleted file mode 100644 index 6f1710d08..000000000 --- a/.vscode/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/settings.json diff --git a/launch/__pycache__/.gitignore b/launch/__pycache__/.gitignore deleted file mode 100644 index 7d1e706e4..000000000 --- a/launch/__pycache__/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/lifelong_launch.cpython-38.pyc From 002345b4a9cbf762d8fe9d873717a59974cccf84 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Fri, 4 Nov 2022 16:44:10 -0500 Subject: [PATCH 73/83] Updating test for mutual information --- src/experimental/information_estimates.cpp | 2 - src/experimental/slam_toolbox_lifelong.cpp | 93 +++++++++-------- src/experimental/utils.cpp | 1 - test/information_estimates_test.cpp | 111 +++++++++------------ 4 files changed, 95 insertions(+), 112 deletions(-) diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index e6d6d8fd4..6ac24cafe 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -314,8 +314,6 @@ std::vector> InformationEstimates::getScanGroupVisitedCells( std::vector InformationEstimates::findMutualInfo( std::vector const &range_scans) { - std::cout << "Vector size: " << range_scans.size() << std::endl; - std::vector result_vector; resizeGridFromScans(range_scans); diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index cbbb58519..241fe479a 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -274,57 +274,56 @@ ScoredVertices LifelongSlamToolbox::computeScores( ScoredVertices scored_vertices; scored_vertices.reserve(near_scans.size()); - // must have some minimum metric to utilize // IOU will drop sharply with fitment, I'd advise not setting this value // any higher than 0.15. Also check this is a linked constraint // We want to do this early to get a better estimate of local candidates - ScanVector::iterator candidate_scan_it; - double iou = 0.0; - - - std::vector range_scan_vct; - std::vector mut_inf_vct; - - for (candidate_scan_it = near_scans.begin(); - candidate_scan_it != near_scans.end(); ) - { - iou = computeIntersectOverUnion(range_scan, (*candidate_scan_it)->GetObject()); - if (iou < iou_thresh_ || (*candidate_scan_it)->GetEdges().size() < 2) - { - candidate_scan_it = near_scans.erase(candidate_scan_it); - } - else - { - // Need to wait for at least 7 laser readings - if(range_scan_vct.size() == 7) - { - // Process the mutual information in batches of 7 readings - std::vector local_mut_inf = inf_estimates_.findLeastInformativeLaser(range_scan_vct); - // Append the current mutual information to the total mutual information - mut_inf_vct.insert(mut_inf_vct.end(), std::begin(local_mut_inf), std::end(local_mut_inf)); - // Clean the current range vector (For the next batch) - range_scan_vct.clear(); - } - else - { - range_scan_vct.push_back((*candidate_scan_it)->GetObject()); - ++candidate_scan_it; - } - } - } - - int idx_res = 0; - for (candidate_scan_it = near_scans.begin(); - candidate_scan_it != near_scans.end(); ++candidate_scan_it) - { - if (mut_inf_vct.size() > 0) - { - ScoredVertex scored_vertex((*candidate_scan_it), mut_inf_vct[idx_res]); - scored_vertices.push_back(scored_vertex); - idx_res++; - } - } + // ScanVector::iterator candidate_scan_it; + // double iou = 0.0; + + + // std::vector range_scan_vct; + // std::vector mut_inf_vct; + + // for (candidate_scan_it = near_scans.begin(); + // candidate_scan_it != near_scans.end(); ) + // { + // iou = computeIntersectOverUnion(range_scan, (*candidate_scan_it)->GetObject()); + // if (iou < iou_thresh_ || (*candidate_scan_it)->GetEdges().size() < 2) + // { + // candidate_scan_it = near_scans.erase(candidate_scan_it); + // } + // else + // { + // // Need to wait for at least 7 laser readings + // if(range_scan_vct.size() == 7) + // { + // // Process the mutual information in batches of 7 readings + // std::vector local_mut_inf = inf_estimates_.findLeastInformativeLaser(range_scan_vct); + // // Append the current mutual information to the total mutual information + // mut_inf_vct.insert(mut_inf_vct.end(), std::begin(local_mut_inf), std::end(local_mut_inf)); + // // Clean the current range vector (For the next batch) + // range_scan_vct.clear(); + // } + // else + // { + // range_scan_vct.push_back((*candidate_scan_it)->GetObject()); + // ++candidate_scan_it; + // } + // } + // } + + // int idx_res = 0; + // for (candidate_scan_it = near_scans.begin(); + // candidate_scan_it != near_scans.end(); ++candidate_scan_it) + // { + // if (mut_inf_vct.size() > 0) + // { + // ScoredVertex scored_vertex((*candidate_scan_it), mut_inf_vct[idx_res]); + // scored_vertices.push_back(scored_vertex); + // idx_res++; + // } + // } return scored_vertices; } diff --git a/src/experimental/utils.cpp b/src/experimental/utils.cpp index c1d2d2d36..7ee60ddd4 100644 --- a/src/experimental/utils.cpp +++ b/src/experimental/utils.cpp @@ -18,7 +18,6 @@ namespace utils kt_double resolution ) { - // This function needs a verification /** * To calculate grid limits for intersection * Arguments: diff --git a/test/information_estimates_test.cpp b/test/information_estimates_test.cpp index 68a5d58c4..431375186 100644 --- a/test/information_estimates_test.cpp +++ b/test/information_estimates_test.cpp @@ -5,7 +5,6 @@ #include #include "slam_toolbox/experimental/information_estimates.hpp" - TEST(UtilsInformationEstimatesTests, SigNumTest) { InformationEstimates information_estimates; @@ -15,66 +14,54 @@ TEST(UtilsInformationEstimatesTests, SigNumTest) ASSERT_EQ(utils::grid_operations::signum(0), 0) << "FAIL in zero"; } -TEST(UtilsInformationEstimatesTests, RayCastTest) +TEST(UtilsInformationEstimatesTests, ) { - /** - * Quadrants are taken from the initial location - */ - InformationEstimates information_estimates; - std::vector> cells; - - // First quadrant - karto::Vector2 initial_pt_1{22,43}; - karto::Vector2 final_pt_1{29,38}; - - // First quadrant - cells = utils::grid_operations::rayCasting(initial_pt_1, final_pt_1); - // Third point - First quadrant - ASSERT_EQ(cells[2].GetX(), 25) << "FAIL in X position 1 for cell 3"; - ASSERT_EQ(cells[2].GetY(), 41) << "FAIL in Y position 1 for cell 3"; - // Fifth point - First quadrant - ASSERT_EQ(cells[4].GetX(), 27) << "FAIL in X position 1 for cell 5"; - ASSERT_EQ(cells[4].GetY(), 39) << "FAIL in Y position 1 for cell 5"; - - // Second quadrant - karto::Vector2 initial_pt_2{22,43}; - karto::Vector2 final_pt_2{15,38}; - cells = utils::grid_operations::rayCasting(initial_pt_2, final_pt_2); - // Third point - Second quadrant - ASSERT_EQ(cells[2].GetX(), 19) << "FAIL in X position 2 for cell 3"; - ASSERT_EQ(cells[2].GetY(), 41) << "FAIL in Y position 2 for cell 3"; - // Fifth point - Second quadrant - ASSERT_EQ(cells[4].GetX(), 17) << "FAIL in X position 2 for cell 5"; - ASSERT_EQ(cells[4].GetY(), 39) << "FAIL in Y position 2 for cell 5"; - - // Third quadrant - karto::Vector2 initial_pt_3{22,43}; - karto::Vector2 final_pt_3{29,48}; - cells = utils::grid_operations::rayCasting(initial_pt_3, final_pt_3); - // Third point - Third quadrant - ASSERT_EQ(cells[2].GetX(), 25) << "FAIL in X position 3 for cell 3"; - ASSERT_EQ(cells[2].GetY(), 45) << "FAIL in Y position 3 for cell 3"; - // Fifth point - Third quadrant - ASSERT_EQ(cells[4].GetX(), 27) << "FAIL in X position 3 for cell 5"; - ASSERT_EQ(cells[4].GetY(), 47) << "FAIL in Y position 3 for cell 5"; - - // Fourth quadrand - karto::Vector2 initial_pt_4{22,43}; - karto::Vector2 final_pt_4{15,48}; - cells = utils::grid_operations::rayCasting(initial_pt_4, final_pt_4); - // Third point - Fourth quadrant - ASSERT_EQ(cells[2].GetX(), 19) << "FAIL in X position 3 for cell 3"; - ASSERT_EQ(cells[2].GetY(), 45) << "FAIL in Y position 3 for cell 3"; - // Fifth point - Fourth quadrant - ASSERT_EQ(cells[4].GetX(), 17) << "FAIL in X position 3 for cell 5"; - ASSERT_EQ(cells[4].GetY(), 47) << "FAIL in Y position 3 for cell 5"; - - // Degenerate case - karto::Vector2 initial_pt_5{22,43}; - karto::Vector2 final_pt_5{22,43}; - cells = utils::grid_operations::rayCasting(initial_pt_4, final_pt_4); - ASSERT_EQ(cells[0].GetX(), 22) << "Degenerate case X"; - ASSERT_EQ(cells[0].GetY(), 43) << "Degenerate case Y"; + // calculateInformationContent +} + +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 { 12, 13 }; + + // Result could be not intersection or intersection + utils::grid_operations::computeLineBoxIntersection( + scan_position, + beam_end_point, + scan_position_cell, + beam_end_cell + cell.GetX() * resolution, + cell.GetY() * resolution, + resolution + ); + + // Assert size; + // Assert values + + // std::pair, std::vector> computeLineBoxIntersection( + // karto::Vector2 const &laser_start, + // karto::Vector2 const &laser_end, + // karto::Vector2 const &robot_grid_pos, + // karto::Vector2 const &final_grid_pos, + // kt_double limit_x, + // kt_double limit_y, + // kt_double resolution); } TEST(UtilsInformationEstimatesTests, IntersectionPointsTest) @@ -151,7 +138,7 @@ TEST(InformationEstimatesTests, MutualInformationTest) range_scan_vct.push_back(s2.get()); range_scan_vct.push_back(s3.get()); - std::vector mut_inf_vct = inf_estimates.findLeastInformativeLaser(range_scan_vct); + // std::vector mut_inf_vct = inf_estimates.findLeastInformativeLaser(range_scan_vct); // Min value should be different to zero (We only have now the mutual information) // This will be solved as soon as I fixed the error in the main loop @@ -159,7 +146,7 @@ TEST(InformationEstimatesTests, MutualInformationTest) // EXPECT_NE(mut_inf, 0.0) << "FAIL in mutual information equality"; } -int main(int argc, char ** argv) +int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); From 656206cdf0e3b345660f471c9ed2035444de4a20 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Tue, 8 Nov 2022 17:04:52 -0500 Subject: [PATCH 74/83] [ADD]: Test cases --- src/experimental/information_estimates.cpp | 3 + test/information_estimates_test.cpp | 77 ++++++++++++++-------- 2 files changed, 52 insertions(+), 28 deletions(-) diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 6ac24cafe..d01866c41 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -380,6 +380,8 @@ void InformationEstimates::appendCellProbabilities( kt_double InformationEstimates::calculateInformationContent(kt_double prob) { + // @hidmic is this a good candidate for an static function? + // or even an utils function /** * Calculate the information content or self-information based on the probability of cell being occupied * Arguments: @@ -506,6 +508,7 @@ kt_double InformationEstimates::calculateScanMassProbabilityBetween(kt_double ra { /** * Calculate the mass probability of a cell being observed by a given measurement + * This functions follows and exponential distribution * Arguments: * range_1 [kt_double]: Lower bound * range_2 [kt_double]: Upper bound diff --git a/test/information_estimates_test.cpp b/test/information_estimates_test.cpp index 431375186..863155894 100644 --- a/test/information_estimates_test.cpp +++ b/test/information_estimates_test.cpp @@ -14,9 +14,22 @@ TEST(UtilsInformationEstimatesTests, SigNumTest) ASSERT_EQ(utils::grid_operations::signum(0), 0) << "FAIL in zero"; } -TEST(UtilsInformationEstimatesTests, ) +TEST(UtilsInformationEstimatesTests, InformationContentTest) { - // calculateInformationContent + 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) @@ -38,30 +51,38 @@ TEST(UtilsInformationEstimatesTests, LineBoxIntersectionTest) 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 { 12, 13 }; + karto::Vector2 const & cell { 11, 6 }; // Result could be not intersection or intersection - utils::grid_operations::computeLineBoxIntersection( - scan_position, - beam_end_point, - scan_position_cell, - beam_end_cell - cell.GetX() * resolution, - cell.GetY() * resolution, - resolution - ); - - // Assert size; - // Assert values - - // std::pair, std::vector> computeLineBoxIntersection( - // karto::Vector2 const &laser_start, - // karto::Vector2 const &laser_end, - // karto::Vector2 const &robot_grid_pos, - // karto::Vector2 const &final_grid_pos, - // kt_double limit_x, - // kt_double limit_y, - // kt_double resolution); + 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) @@ -75,8 +96,8 @@ TEST(UtilsInformationEstimatesTests, IntersectionPointsTest) 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_FLOAT_EQ(int_points.GetX(), 4.06744) << "FAIL in X coordinate intersection"; - ASSERT_FLOAT_EQ(int_points.GetY(), 5.39535) << "FAIL in Y coordinate intersection"; + 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}; @@ -87,8 +108,8 @@ TEST(UtilsInformationEstimatesTests, IntersectionPointsTest) // I should change here the longitud instead // .size() or.Lenght() - ASSERT_FLOAT_EQ(int_points.GetX(), 0.0) << "FAIL in X coordinate parallel"; - ASSERT_FLOAT_EQ(int_points.GetY(), 0.0) << "FAIL in Y coordinate parallel"; + 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) From 97b45af06a557ee4fb0727bf93cb36b9fc036472 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Mon, 14 Nov 2022 09:45:40 -0500 Subject: [PATCH 75/83] Improving redability, addition of segment and box structures, adding optional return values for some functions Signed-off-by: Camilo Andres Alvis Bautista --- .../experimental/information_estimates.hpp | 41 +++- include/slam_toolbox/experimental/utils.hpp | 62 ++--- src/experimental/information_estimates.cpp | 210 ++++++++-------- src/experimental/utils.cpp | 232 ++++++++---------- test/information_estimates_test.cpp | 54 +++- 5 files changed, 327 insertions(+), 272 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index 22ad88e3f..e13bfceb5 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -13,6 +13,8 @@ class InformationEstimates virtual ~InformationEstimates() {} public: + // Main function + // std::vector findLeastInformativeLaser(std::vector const& range_scans); std::vector findMutualInfo(std::vector const& range_scans); private: @@ -23,10 +25,8 @@ class InformationEstimates karto::Vector2 const & cell ); - std::vector calculateBeamAndCellIntersections( - kt_bool & skip_cell_eval, - karto::Vector2 const & scan_position, - karto::Vector2 const & beam_read_point, + std::optional> calculateBeamAndCellIntersections( + utils::Segment2 const & beam_segment, karto::Vector2 const & cell ); @@ -37,9 +37,8 @@ class InformationEstimates karto::LaserRangeFinder *laser_range_finder ); - void adjustBeamReadingDistance( - kt_bool & skip_cell_eval, - kt_double & beam_distance, + std::optional adjustBeamReadingDistance( + kt_double const & beam_distance, kt_double const & distance_to_cell, karto::LaserRangeFinder *laser_range_finder ); @@ -60,9 +59,11 @@ class InformationEstimates std::vector & scans_mutual_information ); - int findClosestLaserIndexToCell( - karto::Vector2 const & cell, - karto::Pose2 const & grid_scan_pose); + std::optional InformationEstimates::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, @@ -86,9 +87,20 @@ class InformationEstimates kt_double calculateProbabilityFromLogOdds(kt_double log); // Measurement outcomes probabilities - void appendCellProbabilities(std::vector& measurements, karto::Vector2 const & cell); - std::unordered_map computeMeasurementOutcomesHistogram(std::vector>& meas_outcm); - void insertMeasurementOutcome(map_tuple tuple, kt_double probability, std::unordered_map& map); + void appendCellProbabilities( + std::vector& measurements, + karto::Vector2 const & cell + ); + + std::unordered_map computeMeasurementOutcomesHistogram( + std::vector>& meas_outcm + ); + + void insertMeasurementOutcome( + map_tuple tuple, + kt_double probability, + std::unordered_map& map + ); // Measurements calculations kt_double calculateScanMassProbabilityBetween(kt_double range_1, kt_double range_2); @@ -107,6 +119,9 @@ class InformationEstimates kt_double m_obs_nu; kt_double m_map_dist; + int m_num_cells; + + // Map grids Eigen::MatrixXd m_mutual_grid; Eigen::MatrixXi m_visited_grid; diff --git a/include/slam_toolbox/experimental/utils.hpp b/include/slam_toolbox/experimental/utils.hpp index 918be8cf4..7130e3cb0 100644 --- a/include/slam_toolbox/experimental/utils.hpp +++ b/include/slam_toolbox/experimental/utils.hpp @@ -2,6 +2,7 @@ #define SLAM_TOOLBOX__EXPERIMENTAL__UTILS_HPP_ #include +#include #include #include #include @@ -11,49 +12,54 @@ #include "lib/karto_sdk/include/karto_sdk/Karto.h" #include "Eigen/Core" +#include + namespace utils { + template + struct Segment2 { + karto::Vector2 start; + karto::Vector2 end; + }; + + template + struct Box2 { + karto::Vector2 bl_corner; + karto::Vector2 br_corner; + karto::Vector2 tl_corner; + karto::Vector2 tr_corner; + }; + namespace grid_operations { - void updateCellLimits(std::vector& initial_x, - std::vector& initial_y, - std::vector& final_x, - std::vector& final_y, - kt_double limit_x, - kt_double limit_y, - std::vector& cell_limits, - karto::Vector2 const& robot_grid_pos, - karto::Vector2 const& final_grid_pos, - kt_double resolution + void updateCellLimits( + std::array, 4> & initial_points, + std::array, 4> & final_points, + karto::Vector2 const & current_point, + std::array & cell_limits, + utils::Segment2 const & discretized_segment, + kt_double const & resolution ); int signum(int num); - karto::Vector2 getGridPosition( - karto::Vector2 const& position, - kt_double resolution + karto::Vector2 discretize( + karto::Vector2 const & position, + kt_double const & resolution ); karto::Vector2 calculateCellIntersectionPoints( - karto::Vector2 const & laser_start, - karto::Vector2 const & laser_end, - karto::Vector2 const & cell_start, - karto::Vector2 const & cell_end + utils::Segment2 const & segment_1, + utils::Segment2 const & segment_2 ); + std::optional returnint(bool b); + std::pair, std::vector> computeLineBoxIntersection( - karto::Vector2 const & laser_start, - karto::Vector2 const & laser_end, - karto::Vector2 const& robot_grid_pos, - karto::Vector2 const& final_grid_pos, - kt_double limit_x, - kt_double limit_y, - kt_double resolution + utils::Segment2 const & segment, + karto::Vector2 const & current_point, + kt_double const & resolution ); - - void clearVisitedCells(Eigen::MatrixXd & grid); - - void clearVisitedCells(Eigen::MatrixXi & grid); } // namespace grid_operations namespace tuple_hash diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index d01866c41..2d3e6f708 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -6,21 +6,22 @@ InformationEstimates::InformationEstimates(kt_double sensor_range, kt_double resolution, kt_double lambda, kt_double nu) : m_max_sensor_range{sensor_range}, - m_cell_resol{resolution}, - m_obs_lambda{lambda}, - m_obs_nu{nu} + m_cell_resol{resolution}, + m_obs_lambda{lambda}, + m_obs_nu{nu} { } InformationEstimates::InformationEstimates() : m_max_sensor_range{25.0}, - m_cell_resol{0.1}, - m_obs_lambda{0.35}, - m_obs_nu{0.28} + m_cell_resol{0.1}, + m_obs_lambda{0.35}, + m_obs_nu{0.28} { } - +/*************************************************************************/ +// void InformationEstimates::resizeGridFromScans(std::vector const & range_scans) { m_lower_limit_x = range_scans[0]->GetCorrectedPose().GetX(); @@ -60,54 +61,52 @@ void InformationEstimates::resizeGridFromScans(std::vector InformationEstimates::findClosestLaserIndexToCell( kt_double const & angle_to_cell, kt_double const & scan_pose_heading, karto::LaserRangeFinder *laser_range_finder) { - kt_double angle_between = angle_to_cell - scan_pose_heading; - kt_double angle_diff = atan2(sin(angle_between), cos(angle_between)); + 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 - skip_cell_eval = true; + return {}; } - kt_double laser_middle_point = ( laser_range_finder->GetMaximumAngle() - laser_range_finder->GetMinimumAngle()) / 2.0; - return (laser_middle_point + angle_between) / laser_range_finder->GetAngularResolution(); + 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::vector InformationEstimates::calculateBeamAndCellIntersections( - kt_bool & skip_cell_eval, - karto::Vector2 const & scan_position, - karto::Vector2 const & beam_read_point, +/*************************************************************************/ +// +std::optional> InformationEstimates::calculateBeamAndCellIntersections( + utils::Segment2 const & beam_segment, karto::Vector2 const & cell) { - karto::Vector2 scan_position_cell = utils::grid_operations::getGridPosition(scan_position, m_cell_resol); - - karto::Vector2 beam_read_cell = - utils::grid_operations::getGridPosition({beam_read_point.GetX(), beam_read_point.GetY()}, m_cell_resol); + karto::Vector2 current_point{ cell.GetX() * m_cell_resol, cell.GetY() * m_cell_resol }; std::pair, std::vector> intersections = utils::grid_operations::computeLineBoxIntersection( - scan_position, - beam_read_point, - scan_position_cell, - beam_read_cell, - cell.GetX() * m_cell_resol, - cell.GetY() * m_cell_resol, + beam_segment, + current_point, m_cell_resol ); if (intersections.first.size() == 0) { - skip_cell_eval = true; + return {}; } // Enter (d1) and Exit (d2) distances @@ -119,35 +118,41 @@ std::vector InformationEstimates::calculateBeamAndCellIntersections( intersections.first[k], intersections.second[k] }; - kt_double distance = scan_position.Distance(intersection); + kt_double distance = beam_segment.start.Distance(intersection); distances.push_back(distance); - std::sort(distances.begin(), distances.end()); } + std::sort(distances.begin(), distances.end()); - return distances; + return std::optional{ distances }; } +// +/*************************************************************************/ - -void InformationEstimates::adjustBeamReadingDistance( - kt_bool & skip_cell_eval, - kt_double & beam_distance, +/*************************************************************************/ +// +std::optional InformationEstimates::adjustBeamReadingDistance( + kt_double const & beam_distance, kt_double const & distance_to_cell, karto::LaserRangeFinder *laser_range_finder) { if ((beam_distance > laser_range_finder->GetRangeThreshold()) || (beam_distance < distance_to_cell)) { - skip_cell_eval = true; + return {}; } - - beam_distance = distance_to_cell; + 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), @@ -160,8 +165,12 @@ void InformationEstimates::calculateAndAppendCellProbabilities( // Appending new measurement outcomes for the current cell appendCellProbabilities(probabilities, cell); } +// +/*************************************************************************/ +/*************************************************************************/ +// void InformationEstimates::calculateCellProbabilities( std::vector const & range_scans, std::vector> & visited_cells, @@ -177,7 +186,6 @@ void InformationEstimates::calculateCellProbabilities( { continue; } - kt_bool skip_cell_eval = false; // Scan pose karto::Pose2 scan_pose = range_scans[s]->GetCorrectedPose(); @@ -191,62 +199,68 @@ void InformationEstimates::calculateCellProbabilities( 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) { - // It means the robot cannot see the current cell + // Cell is not being seen continue; } - int laser_idx = findClosestLaserIndexToCell( - skip_cell_eval, + std::optional laser_idx = findClosestLaserIndexToCell( angle_to_cell, scan_pose.GetHeading(), laser_range_finder ); - if (skip_cell_eval) + if (!laser_idx.has_value()) { continue; } - kt_double beam_read_dist = range_scans[s]->GetRangeReadings()[laser_idx]; - adjustBeamReadingDistance(skip_cell_eval, beam_read_dist, distance_to_cell, laser_range_finder); + kt_double beam_read_dist = range_scans[s]->GetRangeReadings()[*laser_idx]; + std::optional beam_distance = adjustBeamReadingDistance(beam_read_dist, distance_to_cell, laser_range_finder); - if (skip_cell_eval) + if (!beam_distance.has_value()) { continue; } kt_double angle_to_cell_proj = angle_to_cell > 0 ? angle_to_cell : (2.0 * M_PI + angle_to_cell); - karto::Vector2 beam_read_point { - grid_scan_pose.GetX() + (beam_read_dist * cos(angle_to_cell_proj)), - grid_scan_pose.GetY() + (beam_read_dist * sin(angle_to_cell_proj)) + karto::Vector2 beam_point { + grid_scan_pose.GetX() + (*beam_distance * cos(angle_to_cell_proj)), + grid_scan_pose.GetY() + (*beam_distance * sin(angle_to_cell_proj)) }; - std::vector distances = calculateBeamAndCellIntersections( - skip_cell_eval, + utils::Segment2 beam_segment { grid_scan_pose.GetPosition(), - beam_read_point, + beam_point + }; + + std::optional> distances = calculateBeamAndCellIntersections( + beam_segment, cell ); - if (skip_cell_eval) + if (!distances.has_value()) { continue; } calculateAndAppendCellProbabilities( visited_cells, - distances, + *distances, cell ); } } +// +/*************************************************************************/ +/*************************************************************************/ +// std::vector InformationEstimates::getScanGroupMutualInformation( - std::vector const & range_scans) + std::vector const & range_scans +) { - // Since this information is the same for all the lectures I can move it outside karto::LaserRangeFinder *laser_range_finder = range_scans[0]->GetLaserRangeFinder(); kt_double range_threshold = laser_range_finder->GetRangeThreshold(); kt_double angle_increment = laser_range_finder->GetAngularResolution(); @@ -266,11 +280,16 @@ std::vector InformationEstimates::getScanGroupMutualInformation( } return scans_mutual_information; } +// +/*************************************************************************/ +/*************************************************************************/ +// void InformationEstimates::calculateScanGroupMutualInformation( std::vector> const & visited_cells, - std::vector & scans_mutual_information) + std::vector & scans_mutual_information +) { for (auto &cell : visited_cells) { @@ -288,12 +307,17 @@ void InformationEstimates::calculateScanGroupMutualInformation( 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) + int const & scan_to_skip +) { std::vector> visited_cells; @@ -309,11 +333,14 @@ std::vector> InformationEstimates::getScanGroupVisitedCells( } return visited_cells; } +// +/*************************************************************************/ -std::vector InformationEstimates::findMutualInfo( - std::vector const &range_scans) +/*************************************************************************/ +std::vector InformationEstimates::findMutualInfo(std::vector const &range_scans) { + std::vector result_vector; resizeGridFromScans(range_scans); @@ -321,10 +348,7 @@ std::vector InformationEstimates::findMutualInfo( return mutual_information_vct; } - -void InformationEstimates::appendCellProbabilities( - std::vector &measurements, - karto::Vector2 const &cell) +void InformationEstimates::appendCellProbabilities(std::vector &measurements, karto::Vector2 const &cell) { /** * Append measured porbabilities for the given cell @@ -335,32 +359,44 @@ void InformationEstimates::appendCellProbabilities( * Void */ + /* + This is what is going on: + - In mutualInformationFromScans is a line for cleaning the visited cells m_visited_grid.setZero(), it takes place + right before we start processing the visited cells for a new scan. + - Some operations are done to calculate the probability of see a cell as occupied, free or unknown. + - The probabilities are given to appendCellProbabilities, so we store them into our map. + - Inside appendCellProbabilities we evaluate if the each cell in the current laser reading has been visited: + - Since we consider that a cell can be seen only once for one beam of the laser scan. In theory two beams should not + see the same cell, but as this is a grid map, it might happen due to the resolution. For avoiding it, we are performing + the following evaluation on each of the laser beams of the current laser scan. + - Has this cell been seen by a beam?. If not then add it to our map, and mark this cells as a visited. EOF the function + - Is the cell in the map and has been marked as visited by a beam?. If yes, then replace it because we don't want to have + duplicate values + - Is the cell in the map and has not been marked as visited by the current beam. + Case 3 Other beam of other laser scan sees the cell and it has not been marked as visited, because this new laser + just discovered that cell + + I need to triger some testing for this case + */ 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 - */ + // Cell is not present in the map, so append it m_cell_probabilities.insert(std::pair, std::vector>>( - cell, {measurements})); + 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 { - /** - * Compare the unknown probability, the smallest it is the most information we will have - * from the occupied of free state - */ int idx = it_cell->second.size() - 1; if (measurements[2] < it_cell->second[idx][2]) { - /** - * Replacing - */ + // Replacing it_cell->second[idx][0] = measurements[0]; it_cell->second[idx][1] = measurements[1]; it_cell->second[idx][2] = measurements[2]; @@ -368,20 +404,14 @@ void InformationEstimates::appendCellProbabilities( } else { - /** - * Cell is already in the map, only add the next measurement outcome - */ it_cell->second.push_back(measurements); m_visited_grid(cell.GetX(), cell.GetY()) = 1; } } } - kt_double InformationEstimates::calculateInformationContent(kt_double prob) { - // @hidmic is this a good candidate for an static function? - // or even an utils function /** * Calculate the information content or self-information based on the probability of cell being occupied * Arguments: @@ -392,7 +422,6 @@ kt_double InformationEstimates::calculateInformationContent(kt_double prob) return -(prob * log2(prob)) - ((1 - prob) * log2(1 - prob)); } - kt_double InformationEstimates::calculateProbabilityFromLogOdds(kt_double log) { /** @@ -405,13 +434,7 @@ kt_double InformationEstimates::calculateProbabilityFromLogOdds(kt_double log) return (exp(log) / (1 + exp(log))); } - -void InformationEstimates::insertMeasurementOutcome( - map_tuple tuple, - kt_double probability, - std::unordered_map &map) +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; @@ -426,9 +449,7 @@ void InformationEstimates::insertMeasurementOutcome( } } - -std::unordered_map InformationEstimates::computeMeasurementOutcomesHistogram( - std::vector> &meas_outcm) +std::unordered_map InformationEstimates::computeMeasurementOutcomesHistogram(std::vector> &meas_outcm) { /** * Implementation of algorithm 1 @@ -483,7 +504,6 @@ std::unordered_map + namespace utils { namespace grid_operations { void updateCellLimits( - std::vector& initial_x, - std::vector& initial_y, - std::vector& final_x, - std::vector& final_y, - kt_double limit_x, - kt_double limit_y, - std::vector& cell_limits, - karto::Vector2 const& robot_grid_pos, - karto::Vector2 const& final_grid_pos, - kt_double resolution + std::array, 4> & initial_points, + std::array, 4> & final_points, + karto::Vector2 const & current_point, + std::array & cell_limits, + utils::Segment2 const & discretized_segment, + kt_double const & resolution ) { /** * To calculate grid limits for intersection * Arguments: - * initial_x [std::vector]: Cell initial limits in x (4 possible limits) - * initial_y [std::vector]: Cell initial limits in y (4 possible limits) - * final_x [std::vector]: Cell final limits in x (4 possible limits) - * final_y [std::vector]: Cell final limits in y (4 possible limits) - * limit_x [kt_double]: Current cell position in x (Transformed from int to kt_double) - * limit_y [kt_double]: Current cell position in y (Transformed from int to kt_double) - * cell_limits [std::vector]: Cell final points for assertion in x and y + * initial_points [std::array, 4>]: Cell initial point in x and y (4 corners) + * final_points [std::array, 4>]: Cell final point in x and y (4 corners) + * current_point [karto::Vector2]: Current cell position + * cell_limits [std::array]: Cell final points for assertion in x and y * robot_grip_position [std::vector]: Initial laser beam position * final_grid_position [std::vector]: Final laser beam position * resolution [kt_double]: Cell resolution * Return: * Void */ - if (final_grid_pos.GetX() < robot_grid_pos.GetX() && final_grid_pos.GetY() >= robot_grid_pos.GetY()) + if (discretized_segment.end.GetX() < discretized_segment.start.GetX() + && discretized_segment.end.GetY() >= discretized_segment.start.GetY() + ) { - final_x[0] = limit_x + resolution; - final_x[2] = limit_x + resolution; + final_points[0].SetX(current_point.GetX() + resolution); + final_points[2].SetX(current_point.GetX() + resolution); - cell_limits[2] = limit_y; - cell_limits[3] = limit_y + resolution; + cell_limits[2] = current_point.GetY(); + cell_limits[3] = current_point.GetY() + resolution; } - if (final_grid_pos.GetX() >= robot_grid_pos.GetX() && final_grid_pos.GetY() < robot_grid_pos.GetY()) + if (discretized_segment.end.GetX() >= discretized_segment.start.GetX() + && discretized_segment.end.GetY() < discretized_segment.start.GetY() + ) { - initial_y[2] = limit_y - resolution; - initial_y[3] = limit_y - resolution; + initial_points[2].SetY(current_point.GetY() - resolution); + initial_points[3].SetY(current_point.GetY() - resolution); - final_y[1] = limit_y - resolution; - final_y[3] = limit_y - resolution; + final_points[1].SetY(current_point.GetY() - resolution); + final_points[3].SetY(current_point.GetY() - resolution); - cell_limits[2] = limit_y - resolution; - cell_limits[3] = limit_y; + cell_limits[2] = current_point.GetY() - resolution; + cell_limits[3] = current_point.GetY(); } } @@ -72,34 +70,7 @@ namespace utils } - void clearVisitedCells(Eigen::MatrixXd & grid) - { - /** - * Clear the given floating Eigen::Matrix - * Arguments: - * grid [Eigen::Matrix]: Grid for cleaning - * Return: - * Void - */ - grid.setZero(); - } - - - void clearVisitedCells(Eigen::MatrixXi & grid) - { - /** - * Clear the given integer Eigen::Matrix - * Arguments: - * grid [Eigen::Matrix]: Grid for cleaning - * Return: - * Void - */ - - grid.setZero(); - } - - - karto::Vector2 getGridPosition(karto::Vector2 const& position, kt_double resolution) + karto::Vector2 discretize(karto::Vector2 const & position, kt_double const & resolution) { /** * Mapping a continuous position into a grid position @@ -109,64 +80,63 @@ namespace utils * Return: * karto::Vector2: Grid position */ - int x_cell = floor((position.GetX() / resolution)); - int y_cell = floor((position.GetY() / resolution)); + const int x_cell = floor((position.GetX() / resolution)); + const int y_cell = floor((position.GetY() / resolution)); return karto::Vector2{x_cell, y_cell}; } + std::optional returnint(bool b) + { + if (b) + { + return 2; + } + return {}; + } - karto::Vector2 calculateCellIntersectionPoints(karto::Vector2 const & laser_start, - karto::Vector2 const & laser_end, karto::Vector2 const & cell_start, karto::Vector2 const & cell_end) + + karto::Vector2 calculateCellIntersectionPoints( + utils::Segment2 const & segment_1, + utils::Segment2 const & segment_2 + ) { /** - * Find the intersection point between a cell line and a laser beam + * Find the intersection point between two segments (cell and laser beam) * Arguments: - * laser_start [karto::Vector2]: Laser initial point in x and y - * laser_end [karto::Vector2]: Laser final point in x and y - * cell_start [karto::Vector2]: Cell initial point in x and y - * cell_end [karto::Vector2]: Cell final point in x and y + * segment_1 [utils::Segment2]: First segment points + * segment_2 [utils::Segment2]: Second segment points * Return: * karto::Vector2: Intersection point */ - kt_double x1 = laser_start.GetX(); - kt_double x2 = laser_end.GetX(); - kt_double x3 = cell_start.GetX(); - kt_double x4 = cell_end.GetX(); + 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(); - kt_double y1 = laser_start.GetY(); - kt_double y2 = laser_end.GetY(); - kt_double y3 = cell_start.GetY(); - kt_double y4 = cell_end.GetY(); + 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(); - kt_double den = ((x2 - x1)*(y4 - y3) - (x4 - x3)*(y2 - y1)); + const kt_double den = ((x2 - x1)*(y4 - y3) - (x4 - x3)*(y2 - y1)); - karto::Vector2 intersection; - if (den == 0.0f) + if (den != 0.0f) { - return{}; + 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; } - else - { - kt_double x = ((x2*y1 - x1*y2)*(x4 - x3) - (x4*y3 - x3*y4)*(x2 - x1)) / den; - kt_double y = ((x2*y1 - x1*y2)*(y4 - y3) - (x4*y3 - x3*y4)*(y2 - y1)) / den; - intersection.SetX(x); - intersection.SetY(y); - } - - return intersection; + return {}; } std::pair, std::vector> computeLineBoxIntersection( - karto::Vector2 const & laser_start, - karto::Vector2 const & laser_end, - karto::Vector2 const& robot_grid_pos, - karto::Vector2 const& final_grid_pos, - kt_double limit_x, - kt_double limit_y, - kt_double resolution + utils::Segment2 const & segment, + karto::Vector2 const & current_point, + kt_double const & resolution ) { /** @@ -176,55 +146,72 @@ namespace utils * laser_end [karto::Vector2]: Laser final point in x and y * robot_grid_pos [karto::Vector2]: Initial grid position in x and y * final_grid_pos [karto::Vector2]: Final grid position in x and y - * limit_x [kt_double]: Current cell position in x (Transformed from int to kt_double) - * limit_y [kt_double]: Current cell position in y (Transformed from int to kt_double) + * current_point [kt_double]: Current cell position in x and y * resolution [kt_double]: Cell resolution * Return: * std::vector: Intersection point */ - - // Cell limits: min_x, max_x, min_y, max_y - std::vector cell_limits {limit_x, limit_x + resolution, limit_y, limit_y + resolution}; - - // Initial points for each of the 4 corners - std::vector initial_x {limit_x, limit_x, limit_x + resolution, limit_x + resolution}; - std::vector initial_y {limit_y, limit_y, limit_y + resolution, limit_y + resolution}; - - // Final points for each of the 4 corners - std::vector final_x {limit_x + resolution, limit_x, limit_x + resolution, limit_x}; - std::vector final_y {limit_y, limit_y + resolution, limit_y, limit_y + resolution}; + utils::Segment2 discretized_segment { + discretize(segment.start, resolution), + discretize(segment.end, resolution) + }; + + std::array cell_limits { + current_point.GetX(), + current_point.GetX() + resolution, + current_point.GetY(), + current_point.GetY() + resolution + }; + + std::array, 4> initial_points{{ + {current_point.GetX(), current_point.GetY()}, + {current_point.GetX(), current_point.GetY()}, + {current_point.GetX() + resolution, current_point.GetY() + resolution}, + {current_point.GetX() + resolution, current_point.GetY() + resolution} + }}; + + std::array, 4> final_points{{ + {current_point.GetX() + resolution, current_point.GetY()}, + {current_point.GetX(), current_point.GetY() + resolution}, + {current_point.GetX() + resolution, current_point.GetY()}, + {current_point.GetX(), current_point.GetY() + resolution} + }}; // Set the new cell limits - updateCellLimits(initial_x, initial_y, final_x, final_y, limit_x, limit_y, cell_limits, robot_grid_pos, final_grid_pos, resolution); + updateCellLimits(initial_points, final_points, current_point, cell_limits, discretized_segment, resolution); std::vector inter_x, inter_y; /* - initial {limit_x, limit_y} - final {limit_x + res, limit_y} + initial {current_point.GetX(), current_point.GetY()} + final {current_point.GetX() + res, current_point.GetY()} - initial {limit_x, limit_y} - final {limit_x, limit_y + res} + initial {current_point.GetX(), current_point.GetY()} + final {current_point.GetX(), current_point.GetY() + res} - initial {limit_x + res, limit_y + res} - final {limit_x + res, limit_y} + initial {current_point.GetX() + res, current_point.GetY() + res} + final {current_point.GetX() + res, current_point.GetY()} - initial {limit_x + res, limit_y + res} - final {limit_x, limit_y + res} + initial {current_point.GetX() + res, current_point.GetY() + res} + final {current_point.GetX(), current_point.GetY() + res} */ for (int k = 0; k < 4; ++k) { - karto::Vector2 start{initial_x[k], initial_y[k]}; - karto::Vector2 end{final_x[k], final_y[k]}; - karto::Vector2 intersection = calculateCellIntersectionPoints(laser_start, laser_end, start, end); + 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]) - 0.001)) && - (fabs(intersection.GetX()) <= (fabs(cell_limits[1]) + 0.001)) && - (fabs(intersection.GetY()) >= (fabs(cell_limits[2]) - 0.001)) && - (fabs(intersection.GetY()) <= (fabs(cell_limits[3]) + 0.001))) + (fabs(intersection.GetX()) <= (fabs(cell_limits[1]) + 0.001)) && + (fabs(intersection.GetY()) >= (fabs(cell_limits[2]) - 0.001)) && + (fabs(intersection.GetY()) <= (fabs(cell_limits[3]) + 0.001)) + ) { // Two points where the beam cuts the cell // - A laser beam can cut the cell at least 1 time (Enter) @@ -237,6 +224,5 @@ namespace utils return std::pair, std::vector>{inter_x, inter_y}; } - } // namespace grid_operations } // namespace utils diff --git a/test/information_estimates_test.cpp b/test/information_estimates_test.cpp index 863155894..614b88213 100644 --- a/test/information_estimates_test.cpp +++ b/test/information_estimates_test.cpp @@ -5,6 +5,20 @@ #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; @@ -14,12 +28,12 @@ TEST(UtilsInformationEstimatesTests, SigNumTest) 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, InformationContentTest) +// { +// InformationEstimates inf_estimates; +// kt_double result = inf_estimates.calculateInformationContent(0.75); +// ASSERT_DOUBLE_EQ(result, 0.81125); +// } TEST(UtilsInformationEstimatesTests, ScanMassProbabilityBetweenTest) { @@ -159,12 +173,28 @@ TEST(InformationEstimatesTests, MutualInformationTest) range_scan_vct.push_back(s2.get()); range_scan_vct.push_back(s3.get()); - // std::vector mut_inf_vct = inf_estimates.findLeastInformativeLaser(range_scan_vct); - - // Min value should be different to zero (We only have now the mutual information) - // This will be solved as soon as I fixed the error in the main loop - // EXPECT_EQ(idx, 1) << "FAIL in laser index"; - // EXPECT_NE(mut_inf, 0.0) << "FAIL in mutual information equality"; + // 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) From 85674bfbb84fa71cd8bdddec54bd3dcba4a43e47 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Sat, 10 Dec 2022 11:38:57 -0500 Subject: [PATCH 76/83] [RMV]: Revert test changes in the main file Signed-off-by: Camilo Andres Alvis Bautista --- src/experimental/slam_toolbox_lifelong.cpp | 75 +++++++--------------- 1 file changed, 22 insertions(+), 53 deletions(-) diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 241fe479a..113326209 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -127,10 +127,9 @@ void LifelongSlamToolbox::evaluateNodeDepreciation( computeScores(near_scan_vertices, range_scan); ScoredVertices::iterator it; - kt_double mutual_removol_score = 1500.0; for (it = scored_verices.begin(); it != scored_verices.end(); ++it) { - if (it->GetScore() < mutual_removol_score) { + if (it->GetScore() < removal_score_) { RCLCPP_DEBUG(get_logger(), "Removing node %i from graph with score: %f and old score: %f.", it->GetVertex()->GetObject()->GetUniqueId(), @@ -143,15 +142,6 @@ void LifelongSlamToolbox::evaluateNodeDepreciation( } } - -// -/* - nullptr evaluation - Test minimizar el radio del scan para procesar menor cantidad de scans. - Test constraint en el tama;o del grafo. El siguiente nodo despues del limite va a ser borrado -*/ -// - /*****************************************************************************/ Vertices LifelongSlamToolbox::FindScansWithinRadius( LocalizedRangeScan * scan, const double & radius) @@ -281,49 +271,28 @@ ScoredVertices LifelongSlamToolbox::computeScores( // ScanVector::iterator candidate_scan_it; // double iou = 0.0; + ScanVector::iterator candidate_scan_it; + double iou = 0.0; + for (candidate_scan_it = near_scans.begin(); + candidate_scan_it != near_scans.end(); ) + { + iou = computeIntersectOverUnion(range_scan, + (*candidate_scan_it)->GetObject()); + if (iou < iou_thresh_ || (*candidate_scan_it)->GetEdges().size() < 2) { + candidate_scan_it = near_scans.erase(candidate_scan_it); + } else { + ++candidate_scan_it; + } + } - // std::vector range_scan_vct; - // std::vector mut_inf_vct; - - // for (candidate_scan_it = near_scans.begin(); - // candidate_scan_it != near_scans.end(); ) - // { - // iou = computeIntersectOverUnion(range_scan, (*candidate_scan_it)->GetObject()); - // if (iou < iou_thresh_ || (*candidate_scan_it)->GetEdges().size() < 2) - // { - // candidate_scan_it = near_scans.erase(candidate_scan_it); - // } - // else - // { - // // Need to wait for at least 7 laser readings - // if(range_scan_vct.size() == 7) - // { - // // Process the mutual information in batches of 7 readings - // std::vector local_mut_inf = inf_estimates_.findLeastInformativeLaser(range_scan_vct); - // // Append the current mutual information to the total mutual information - // mut_inf_vct.insert(mut_inf_vct.end(), std::begin(local_mut_inf), std::end(local_mut_inf)); - // // Clean the current range vector (For the next batch) - // range_scan_vct.clear(); - // } - // else - // { - // range_scan_vct.push_back((*candidate_scan_it)->GetObject()); - // ++candidate_scan_it; - // } - // } - // } - - // int idx_res = 0; - // for (candidate_scan_it = near_scans.begin(); - // candidate_scan_it != near_scans.end(); ++candidate_scan_it) - // { - // if (mut_inf_vct.size() > 0) - // { - // ScoredVertex scored_vertex((*candidate_scan_it), mut_inf_vct[idx_res]); - // scored_vertices.push_back(scored_vertex); - // idx_res++; - // } - // } + for (candidate_scan_it = near_scans.begin(); + candidate_scan_it != near_scans.end(); ++candidate_scan_it) + { + ScoredVertex scored_vertex((*candidate_scan_it), + computeScore(range_scan, (*candidate_scan_it), + (*candidate_scan_it)->GetScore(), near_scans.size())); + scored_vertices.push_back(scored_vertex); + } return scored_vertices; } From be2197ad93831bdbcb9b959c4adbfa66cb45f0a0 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Sat, 10 Dec 2022 11:39:44 -0500 Subject: [PATCH 77/83] [UPDT]: Update C++ STD version to use optional function Signed-off-by: Camilo Andres Alvis Bautista --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3333bc205..29b7279b2 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) From be745b7f554545e46f0b097739ea24c484b1252d Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Sat, 10 Dec 2022 11:40:12 -0500 Subject: [PATCH 78/83] [WIP]: Structures modification to simplify readability Signed-off-by: Camilo Andres Alvis Bautista --- .../experimental/information_estimates.hpp | 44 ++---- include/slam_toolbox/experimental/utils.hpp | 16 +- src/experimental/information_estimates.cpp | 6 +- src/experimental/utils.cpp | 137 +++++++--------- test/information_estimates_test.cpp | 146 +++++++++--------- 5 files changed, 153 insertions(+), 196 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index e13bfceb5..a1e3f591b 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -5,7 +5,9 @@ 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 sensor_range, kt_double resolution, kt_double lambda, kt_double nu); @@ -13,12 +15,9 @@ class InformationEstimates virtual ~InformationEstimates() {} public: - // Main function - // std::vector findLeastInformativeLaser(std::vector const& range_scans); std::vector findMutualInfo(std::vector const& range_scans); private: - void calculateAndAppendCellProbabilities( std::vector> & visited_cells, std::vector const & distances, @@ -30,13 +29,6 @@ class InformationEstimates karto::Vector2 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 - ); - std::optional adjustBeamReadingDistance( kt_double const & beam_distance, kt_double const & distance_to_cell, @@ -59,7 +51,7 @@ class InformationEstimates std::vector & scans_mutual_information ); - std::optional InformationEstimates::findClosestLaserIndexToCell( + std::optional findClosestLaserIndexToCell( kt_double const & angle_to_cell, kt_double const & scan_pose_heading, karto::LaserRangeFinder *laser_range_finder @@ -87,37 +79,26 @@ class InformationEstimates kt_double calculateProbabilityFromLogOdds(kt_double log); // Measurement outcomes probabilities - void appendCellProbabilities( - std::vector& measurements, - karto::Vector2 const & cell - ); - - std::unordered_map computeMeasurementOutcomesHistogram( - std::vector>& meas_outcm - ); - - void insertMeasurementOutcome( - map_tuple tuple, - kt_double probability, - std::unordered_map& map - ); + void appendCellProbabilities(std::vector& measurements, karto::Vector2 const & cell); + std::unordered_map computeMeasurementOutcomesHistogram(std::vector>& meas_outcm); + void insertMeasurementOutcome(map_tuple tuple, kt_double probability, std::unordered_map& map); // Measurements calculations kt_double calculateScanMassProbabilityBetween(kt_double range_1, kt_double range_2); private: // Data structures - std::map, std::vector>> m_cell_probabilities; + // std::map, std::vector>> m_cell_probabilities; + map_cell_probabilities 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)); + 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; int m_num_cells; @@ -125,11 +106,14 @@ class InformationEstimates Eigen::MatrixXd m_mutual_grid; Eigen::MatrixXi m_visited_grid; + // Box limits (maybe it work better here + // min_x, max_x, min_y, max_y) kt_double m_upper_limit_x; kt_double m_upper_limit_y; kt_double m_lower_limit_x; kt_double m_lower_limit_y; + // I can define an struct for this kt_double m_map_dim_x; kt_double m_map_dim_y; }; diff --git a/include/slam_toolbox/experimental/utils.hpp b/include/slam_toolbox/experimental/utils.hpp index 7130e3cb0..b03159b0c 100644 --- a/include/slam_toolbox/experimental/utils.hpp +++ b/include/slam_toolbox/experimental/utils.hpp @@ -22,20 +22,14 @@ namespace utils karto::Vector2 end; }; - template - struct Box2 { - karto::Vector2 bl_corner; - karto::Vector2 br_corner; - karto::Vector2 tl_corner; - karto::Vector2 tr_corner; - }; - 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 & current_point, + karto::Vector2 const & cell_reference_pos, std::array & cell_limits, utils::Segment2 const & discretized_segment, kt_double const & resolution @@ -53,11 +47,9 @@ namespace utils utils::Segment2 const & segment_2 ); - std::optional returnint(bool b); - std::pair, std::vector> computeLineBoxIntersection( utils::Segment2 const & segment, - karto::Vector2 const & current_point, + karto::Vector2 const & cell_reference_pos, kt_double const & resolution ); } // namespace grid_operations diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 2d3e6f708..81e735d8f 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -95,12 +95,12 @@ std::optional> InformationEstimates::calculateBeamAndCell utils::Segment2 const & beam_segment, karto::Vector2 const & cell) { - karto::Vector2 current_point{ cell.GetX() * m_cell_resol, cell.GetY() * m_cell_resol }; + 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_point, + current_cell_position, m_cell_resol ); @@ -152,7 +152,6 @@ void InformationEstimates::calculateAndAppendCellProbabilities( std::vector const & distances, karto::Vector2 const & cell) { - // Measurement outcomes vector {Pfree, Pocc, Pun} std::vector probabilities{ calculateScanMassProbabilityBetween(distances[1], m_max_sensor_range), @@ -340,7 +339,6 @@ std::vector> InformationEstimates::getScanGroupVisitedCells( /*************************************************************************/ std::vector InformationEstimates::findMutualInfo(std::vector const &range_scans) { - std::vector result_vector; resizeGridFromScans(range_scans); diff --git a/src/experimental/utils.cpp b/src/experimental/utils.cpp index 2c50dd7c1..0c10b593c 100644 --- a/src/experimental/utils.cpp +++ b/src/experimental/utils.cpp @@ -1,6 +1,7 @@ #include "slam_toolbox/experimental/utils.hpp" #include +#include namespace utils { @@ -9,48 +10,47 @@ namespace utils void updateCellLimits( std::array, 4> & initial_points, std::array, 4> & final_points, - karto::Vector2 const & current_point, + karto::Vector2 const & cell_reference_pos, std::array & cell_limits, - utils::Segment2 const & discretized_segment, + utils::Segment2 const & beam_discrete_segment, kt_double const & resolution ) { /** * To calculate grid limits for intersection * Arguments: - * initial_points [std::array, 4>]: Cell initial point in x and y (4 corners) - * final_points [std::array, 4>]: Cell final point in x and y (4 corners) - * current_point [karto::Vector2]: Current cell position - * cell_limits [std::array]: Cell final points for assertion in x and y - * robot_grip_position [std::vector]: Initial laser beam position - * final_grid_position [std::vector]: Final laser beam position + * 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 (discretized_segment.end.GetX() < discretized_segment.start.GetX() - && discretized_segment.end.GetY() >= discretized_segment.start.GetY() + 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(current_point.GetX() + resolution); - final_points[2].SetX(current_point.GetX() + resolution); + final_points[0].SetX(cell_reference_pos.GetX() + resolution); + final_points[2].SetX(cell_reference_pos.GetX() + resolution); - cell_limits[2] = current_point.GetY(); - cell_limits[3] = current_point.GetY() + resolution; + cell_limits[2] = cell_reference_pos.GetY(); + cell_limits[3] = cell_reference_pos.GetY() + resolution; } - if (discretized_segment.end.GetX() >= discretized_segment.start.GetX() - && discretized_segment.end.GetY() < discretized_segment.start.GetY() + 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(current_point.GetY() - resolution); - initial_points[3].SetY(current_point.GetY() - resolution); + initial_points[2].SetY(cell_reference_pos.GetY() - resolution); + initial_points[3].SetY(cell_reference_pos.GetY() - resolution); - final_points[1].SetY(current_point.GetY() - resolution); - final_points[3].SetY(current_point.GetY() - resolution); + final_points[1].SetY(cell_reference_pos.GetY() - resolution); + final_points[3].SetY(cell_reference_pos.GetY() - resolution); - cell_limits[2] = current_point.GetY() - resolution; - cell_limits[3] = current_point.GetY(); + cell_limits[2] = cell_reference_pos.GetY() - resolution; + cell_limits[3] = cell_reference_pos.GetY(); } } @@ -60,9 +60,9 @@ namespace utils /** * Get the sign of an operation, used by Bresenham algorithm * Arguments: - * num [int]: Number for perform the sign operation + * num [int]: Number to perform the sign operation * Return: - * int: Sign + * int: Integer representing the sign */ if (num < 0) return -1; if (num >= 1) return 1; @@ -75,7 +75,7 @@ namespace utils /** * Mapping a continuous position into a grid position * Arguments: - * pose [karto::Vector2]: Continuos pose + * position [karto::Vector2]: Position to be discretized * resolution [kt_double]: Cell resolution * Return: * karto::Vector2: Grid position @@ -83,16 +83,7 @@ namespace utils const int x_cell = floor((position.GetX() / resolution)); const int y_cell = floor((position.GetY() / resolution)); - return karto::Vector2{x_cell, y_cell}; - } - - std::optional returnint(bool b) - { - if (b) - { - return 2; - } - return {}; + return karto::Vector2{ x_cell, y_cell }; } @@ -109,7 +100,6 @@ namespace utils * 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(); @@ -135,82 +125,76 @@ namespace utils std::pair, std::vector> computeLineBoxIntersection( utils::Segment2 const & segment, - karto::Vector2 const & current_point, + karto::Vector2 const & cell_reference_pos, kt_double const & resolution ) { /** * Compute intersection between a cell and a laser beam * Arguments: - * laser_start [karto::Vector2]: Laser initial point in x and y - * laser_end [karto::Vector2]: Laser final point in x and y - * robot_grid_pos [karto::Vector2]: Initial grid position in x and y - * final_grid_pos [karto::Vector2]: Final grid position in x and y - * current_point [kt_double]: Current cell position in x and y + * segment [karto::Segment2]: Laser beam initial and final points + * cell_reference_pos [karto::Vector2]: Current cell position * resolution [kt_double]: Cell resolution * Return: - * std::vector: Intersection point + * std::pair, std::vector>: Intersection points for current beam and cell */ - utils::Segment2 discretized_segment { + utils::Segment2 beam_discrete_segment { discretize(segment.start, resolution), discretize(segment.end, resolution) }; std::array cell_limits { - current_point.GetX(), - current_point.GetX() + resolution, - current_point.GetY(), - current_point.GetY() + resolution + cell_reference_pos.GetX(), + cell_reference_pos.GetX() + resolution, + cell_reference_pos.GetY(), + cell_reference_pos.GetY() + resolution }; - std::array, 4> initial_points{{ - {current_point.GetX(), current_point.GetY()}, - {current_point.GetX(), current_point.GetY()}, - {current_point.GetX() + resolution, current_point.GetY() + resolution}, - {current_point.GetX() + resolution, current_point.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{{ - {current_point.GetX() + resolution, current_point.GetY()}, - {current_point.GetX(), current_point.GetY() + resolution}, - {current_point.GetX() + resolution, current_point.GetY()}, - {current_point.GetX(), current_point.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, current_point, cell_limits, discretized_segment, resolution); - - std::vector inter_x, inter_y; + updateCellLimits(initial_points, final_points, cell_reference_pos, cell_limits, beam_discrete_segment, resolution); /* - initial {current_point.GetX(), current_point.GetY()} - final {current_point.GetX() + res, current_point.GetY()} + initial {cell_reference_pos.GetX(), cell_reference_pos.GetY()} + final {cell_reference_pos.GetX() + res, cell_reference_pos.GetY()} - initial {current_point.GetX(), current_point.GetY()} - final {current_point.GetX(), current_point.GetY() + res} + initial {cell_reference_pos.GetX(), cell_reference_pos.GetY()} + final {cell_reference_pos.GetX(), cell_reference_pos.GetY() + res} - initial {current_point.GetX() + res, current_point.GetY() + res} - final {current_point.GetX() + res, current_point.GetY()} + initial {cell_reference_pos.GetX() + res, cell_reference_pos.GetY() + res} + final {cell_reference_pos.GetX() + res, cell_reference_pos.GetY()} - initial {current_point.GetX() + res, current_point.GetY() + res} - final {current_point.GetX(), current_point.GetY() + res} + 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]) - 0.001)) && - (fabs(intersection.GetX()) <= (fabs(cell_limits[1]) + 0.001)) && - (fabs(intersection.GetY()) >= (fabs(cell_limits[2]) - 0.001)) && - (fabs(intersection.GetY()) <= (fabs(cell_limits[3]) + 0.001)) + 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 @@ -221,8 +205,7 @@ namespace utils } } } - - return std::pair, std::vector>{inter_x, inter_y}; + 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 index 614b88213..cc8979b2f 100644 --- a/test/information_estimates_test.cpp +++ b/test/information_estimates_test.cpp @@ -37,93 +37,93 @@ TEST(UtilsInformationEstimatesTests, SigNumTest) TEST(UtilsInformationEstimatesTests, ScanMassProbabilityBetweenTest) { - kt_double range_1 = 4.25; - kt_double range_2 = 6.12; + // 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); + // InformationEstimates inf_estimates; + // kt_double probability = inf_estimates.calculateScanMassProbabilityBetween(range_1, range_2); - ASSERT_DOUBLE_EQ(probability, 0.07522); + // 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); + // 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); + // 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"; + // 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) From 89a8a182b7ca7a835665d14024a27c6956cdf433 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Sat, 10 Dec 2022 11:43:20 -0500 Subject: [PATCH 79/83] [RMV]: Reverting changes in main file Signed-off-by: Camilo Andres Alvis Bautista --- src/experimental/slam_toolbox_lifelong.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 113326209..30413671c 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -127,7 +127,6 @@ void LifelongSlamToolbox::evaluateNodeDepreciation( computeScores(near_scan_vertices, range_scan); ScoredVertices::iterator it; - for (it = scored_verices.begin(); it != scored_verices.end(); ++it) { if (it->GetScore() < removal_score_) { RCLCPP_DEBUG(get_logger(), @@ -268,9 +267,6 @@ ScoredVertices LifelongSlamToolbox::computeScores( // IOU will drop sharply with fitment, I'd advise not setting this value // any higher than 0.15. Also check this is a linked constraint // We want to do this early to get a better estimate of local candidates - // ScanVector::iterator candidate_scan_it; - // double iou = 0.0; - ScanVector::iterator candidate_scan_it; double iou = 0.0; for (candidate_scan_it = near_scans.begin(); From 5ef80112152577bdb1214dc3d2931d094bf1491d Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Sat, 10 Dec 2022 12:48:50 -0500 Subject: [PATCH 80/83] [WIP]: Remove not necessary PI addition to cell angle Signed-off-by: Camilo Andres Alvis Bautista --- include/slam_toolbox/experimental/utils.hpp | 3 ++- src/experimental/information_estimates.cpp | 26 ++------------------- 2 files changed, 4 insertions(+), 25 deletions(-) diff --git a/include/slam_toolbox/experimental/utils.hpp b/include/slam_toolbox/experimental/utils.hpp index b03159b0c..ef2a84165 100644 --- a/include/slam_toolbox/experimental/utils.hpp +++ b/include/slam_toolbox/experimental/utils.hpp @@ -17,7 +17,8 @@ namespace utils { template - struct Segment2 { + struct Segment2 + { karto::Vector2 start; karto::Vector2 end; }; diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 81e735d8f..ccafb2348 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -221,11 +221,9 @@ void InformationEstimates::calculateCellProbabilities( continue; } - kt_double angle_to_cell_proj = angle_to_cell > 0 ? angle_to_cell : (2.0 * M_PI + angle_to_cell); - karto::Vector2 beam_point { - grid_scan_pose.GetX() + (*beam_distance * cos(angle_to_cell_proj)), - grid_scan_pose.GetY() + (*beam_distance * sin(angle_to_cell_proj)) + grid_scan_pose.GetX() + (*beam_distance * cos(angle_to_cell)), + grid_scan_pose.GetY() + (*beam_distance * sin(angle_to_cell)) }; utils::Segment2 beam_segment { @@ -356,26 +354,6 @@ void InformationEstimates::appendCellProbabilities(std::vector &measu * Return: * Void */ - - /* - This is what is going on: - - In mutualInformationFromScans is a line for cleaning the visited cells m_visited_grid.setZero(), it takes place - right before we start processing the visited cells for a new scan. - - Some operations are done to calculate the probability of see a cell as occupied, free or unknown. - - The probabilities are given to appendCellProbabilities, so we store them into our map. - - Inside appendCellProbabilities we evaluate if the each cell in the current laser reading has been visited: - - Since we consider that a cell can be seen only once for one beam of the laser scan. In theory two beams should not - see the same cell, but as this is a grid map, it might happen due to the resolution. For avoiding it, we are performing - the following evaluation on each of the laser beams of the current laser scan. - - Has this cell been seen by a beam?. If not then add it to our map, and mark this cells as a visited. EOF the function - - Is the cell in the map and has been marked as visited by a beam?. If yes, then replace it because we don't want to have - duplicate values - - Is the cell in the map and has not been marked as visited by the current beam. - Case 3 Other beam of other laser scan sees the cell and it has not been marked as visited, because this new laser - just discovered that cell - - I need to triger some testing for this case - */ std::map, std::vector>>::iterator it_cell; it_cell = m_cell_probabilities.find(cell); From 582eda95ad8fe31ca195a174cc6e60a59524b1a2 Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Mon, 12 Dec 2022 09:04:10 -0500 Subject: [PATCH 81/83] [ADD]: Addition of new map dimensions structures to improve readability Signed-off-by: Camilo Andres Alvis Bautista --- .../experimental/information_estimates.hpp | 37 ++++++----- src/experimental/information_estimates.cpp | 62 +++++++++++-------- 2 files changed, 58 insertions(+), 41 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index a1e3f591b..6fd6f1e25 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -17,6 +17,19 @@ class 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, @@ -88,34 +101,26 @@ class InformationEstimates private: // Data structures - // std::map, std::vector>> m_cell_probabilities; map_cell_probabilities 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)) }; + // Probability constants + const kt_double l_free; + const kt_double l_occ; + const kt_double l_o; + // Configuration kt_double m_max_sensor_range; kt_double m_cell_resol; kt_double m_obs_lambda; kt_double m_obs_nu; - int m_num_cells; - // Map grids Eigen::MatrixXd m_mutual_grid; Eigen::MatrixXi m_visited_grid; - // Box limits (maybe it work better here - // min_x, max_x, min_y, max_y) - kt_double m_upper_limit_x; - kt_double m_upper_limit_y; - kt_double m_lower_limit_x; - kt_double m_lower_limit_y; - - // I can define an struct for this - kt_double m_map_dim_x; - kt_double m_map_dim_y; + // Map limits + map_dimensions m_map_dim; + scan_position_limits m_scan_limits; }; #endif diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index ccafb2348..00dfc9959 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -5,18 +5,28 @@ #include InformationEstimates::InformationEstimates(kt_double sensor_range, kt_double resolution, kt_double lambda, kt_double nu) - : m_max_sensor_range{sensor_range}, - m_cell_resol{resolution}, - m_obs_lambda{lambda}, - m_obs_nu{nu} + : 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_max_sensor_range { sensor_range }, + m_cell_resol { resolution }, + m_obs_lambda { lambda }, + m_obs_nu { nu }, + m_map_dim { 0.0, 0.0 }, + m_scan_limits { {0.0, 0.0}, {0.0, 0.0} } { } InformationEstimates::InformationEstimates() - : m_max_sensor_range{25.0}, - m_cell_resol{0.1}, - m_obs_lambda{0.35}, - m_obs_nu{0.28} + : 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_max_sensor_range{25.0}, + m_cell_resol{0.1}, + m_obs_lambda{0.35}, + m_obs_nu{0.28}, + m_map_dim { 0.0, 0.0 }, + m_scan_limits { {0.0, 0.0}, {0.0, 0.0} } { } @@ -24,10 +34,10 @@ InformationEstimates::InformationEstimates() // void InformationEstimates::resizeGridFromScans(std::vector const & range_scans) { - m_lower_limit_x = range_scans[0]->GetCorrectedPose().GetX(); - m_lower_limit_y = range_scans[0]->GetCorrectedPose().GetY(); - m_upper_limit_x = range_scans[0]->GetCorrectedPose().GetX(); - m_upper_limit_y = range_scans[0]->GetCorrectedPose().GetY(); + 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()); for (const auto &scan : range_scans) { @@ -41,21 +51,21 @@ void InformationEstimates::resizeGridFromScans(std::vectorGetLaserRangeFinder()->GetAngularResolution(); // Finding the lower limit pose - m_lower_limit_x = std::min(pose.GetX(), m_lower_limit_x); - m_lower_limit_y = std::min(pose.GetY(), m_lower_limit_y); + 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_upper_limit_x = std::max(pose.GetX(), m_upper_limit_x); - m_upper_limit_y = std::max(pose.GetY(), m_upper_limit_y); + 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_upper_limit_x - m_lower_limit_x) + (2 * m_max_sensor_range); - m_map_dim_y = std::fabs(m_upper_limit_y - m_lower_limit_y) + (2 * m_max_sensor_range); + 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); - // Get 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); + // 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); @@ -188,11 +198,13 @@ void InformationEstimates::calculateCellProbabilities( // Scan pose karto::Pose2 scan_pose = range_scans[s]->GetCorrectedPose(); + karto::Pose2 grid_scan_pose { - scan_pose.GetX() - (m_lower_limit_x - m_max_sensor_range), - scan_pose.GetY() - (m_lower_limit_y - m_max_sensor_range), + 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)); @@ -318,8 +330,8 @@ std::vector> InformationEstimates::getScanGroupVisitedCells( { 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); + 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) { From 54ecdc1de384fdfba9a06e03ffd1831bf4cd18bf Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Mon, 12 Dec 2022 09:08:03 -0500 Subject: [PATCH 82/83] [WIP]: Typedef for probaility map Signed-off-by: Camilo Andres Alvis Bautista --- include/slam_toolbox/experimental/information_estimates.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index 6fd6f1e25..24198da61 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -93,8 +93,8 @@ class InformationEstimates // Measurement outcomes probabilities void appendCellProbabilities(std::vector& measurements, karto::Vector2 const & cell); - std::unordered_map computeMeasurementOutcomesHistogram(std::vector>& meas_outcm); - void insertMeasurementOutcome(map_tuple tuple, kt_double probability, std::unordered_map& map); + 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); From 82a2141b4cac59a207cd7fb0344f6c1a771958eb Mon Sep 17 00:00:00 2001 From: Camilo Andres Alvis Bautista Date: Tue, 13 Dec 2022 08:20:41 -0500 Subject: [PATCH 83/83] [WIP]: Updating exponential distribution rate and deleting unused attributes Signed-off-by: Camilo Andres Alvis Bautista --- .../experimental/information_estimates.hpp | 16 ++--- .../experimental/slam_toolbox_lifelong.hpp | 2 +- src/experimental/information_estimates.cpp | 64 +++++++++---------- 3 files changed, 38 insertions(+), 44 deletions(-) diff --git a/include/slam_toolbox/experimental/information_estimates.hpp b/include/slam_toolbox/experimental/information_estimates.hpp index 24198da61..7644691cb 100644 --- a/include/slam_toolbox/experimental/information_estimates.hpp +++ b/include/slam_toolbox/experimental/information_estimates.hpp @@ -10,7 +10,7 @@ class InformationEstimates typedef std::unordered_map probability_map; public: - InformationEstimates(kt_double sensor_range, kt_double resolution, kt_double lambda, kt_double nu); + InformationEstimates(kt_double resolution); InformationEstimates(); virtual ~InformationEstimates() {} @@ -44,8 +44,7 @@ class InformationEstimates std::optional adjustBeamReadingDistance( kt_double const & beam_distance, - kt_double const & distance_to_cell, - karto::LaserRangeFinder *laser_range_finder + kt_double const & distance_to_cell ); karto::Vector2 getLaserBeamCell( @@ -103,16 +102,17 @@ class InformationEstimates // 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; - // Configuration - kt_double m_max_sensor_range; - kt_double m_cell_resol; - kt_double m_obs_lambda; - kt_double m_obs_nu; + // Factor to calculate exponential distribution rate + const kt_double lambda_factor { 6.0 }; // Map grids Eigen::MatrixXd m_mutual_grid; diff --git a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index 59cecac06..5099e1349 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -77,7 +77,7 @@ class LifelongSlamToolbox : public SlamToolbox double iou_match_; double nearby_penalty_; - InformationEstimates inf_estimates_{10.0, 0.05, 0.35, 0.28}; + InformationEstimates inf_estimates_{ 0.1 }; }; diff --git a/src/experimental/information_estimates.cpp b/src/experimental/information_estimates.cpp index 00dfc9959..5474defcf 100644 --- a/src/experimental/information_estimates.cpp +++ b/src/experimental/information_estimates.cpp @@ -4,27 +4,23 @@ #include -InformationEstimates::InformationEstimates(kt_double sensor_range, kt_double resolution, kt_double lambda, kt_double nu) - : l_free { log(0.3 / (1.0 - 0.3)) }, +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_max_sensor_range { sensor_range }, - m_cell_resol { resolution }, - m_obs_lambda { lambda }, - m_obs_nu { nu }, m_map_dim { 0.0, 0.0 }, m_scan_limits { {0.0, 0.0}, {0.0, 0.0} } { } InformationEstimates::InformationEstimates() - : l_free { log(0.3 / (1.0 - 0.3)) }, + : 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_max_sensor_range{25.0}, - m_cell_resol{0.1}, - m_obs_lambda{0.35}, - m_obs_nu{0.28}, m_map_dim { 0.0, 0.0 }, m_scan_limits { {0.0, 0.0}, {0.0, 0.0} } { @@ -39,6 +35,10 @@ void InformationEstimates::resizeGridFromScans(std::vectorGetCorrectedPose().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) @@ -142,10 +142,9 @@ std::optional> InformationEstimates::calculateBeamAndCell // std::optional InformationEstimates::adjustBeamReadingDistance( kt_double const & beam_distance, - kt_double const & distance_to_cell, - karto::LaserRangeFinder *laser_range_finder) + kt_double const & distance_to_cell) { - if ((beam_distance > laser_range_finder->GetRangeThreshold()) || (beam_distance < distance_to_cell)) + if ((beam_distance > m_max_sensor_range) || (beam_distance < distance_to_cell)) { return {}; } @@ -226,7 +225,7 @@ void InformationEstimates::calculateCellProbabilities( } kt_double beam_read_dist = range_scans[s]->GetRangeReadings()[*laser_idx]; - std::optional beam_distance = adjustBeamReadingDistance(beam_read_dist, distance_to_cell, laser_range_finder); + std::optional beam_distance = adjustBeamReadingDistance(beam_read_dist, distance_to_cell); if (!beam_distance.has_value()) { @@ -271,11 +270,8 @@ std::vector InformationEstimates::getScanGroupMutualInformation( ) { karto::LaserRangeFinder *laser_range_finder = range_scans[0]->GetLaserRangeFinder(); - kt_double range_threshold = laser_range_finder->GetRangeThreshold(); kt_double angle_increment = laser_range_finder->GetAngularResolution(); - kt_double min_angle = laser_range_finder->GetMinimumAngle(); - kt_double max_angle = laser_range_finder->GetMaximumAngle(); - kt_double total_range = max_angle - min_angle; + kt_double total_range = laser_range_finder->GetMaximumAngle() - laser_range_finder->GetMinimumAngle(); std::vector scans_mutual_information; @@ -495,14 +491,13 @@ std::unordered_map m_max_sensor_range) ? m_max_sensor_range : range_1; range_2 = (range_2 > m_max_sensor_range) ? m_max_sensor_range : range_2; - // 12.2 because that is roughly the max range when lambda is equals to 1 - kt_double lambda = 12.2 / m_max_sensor_range; + kt_double lambda = lambda_factor / m_max_sensor_range; return -(exp(-lambda * range_2) - exp(-lambda * range_1)); }