├── maps ├── bucket.png ├── cubicles.png ├── classroom_incomplete.png ├── building_31_incomplete.png ├── classroom_incomplete_small.png ├── bucket.yaml ├── classroom_incomplete.yaml ├── building_31_incomplete.yaml ├── classroom_incomplete_small.yaml └── cubicles.yaml ├── msg └── MIGrid.msg ├── .gitignore ├── test ├── CMakeLists.txt ├── helpers.hpp ├── profile_grid_mi.cpp ├── demo.cpp ├── profile_mi.cpp ├── profile_grid_fsmi.cpp ├── numerical_mi.cpp └── numerical_mi_distorted.cpp ├── node ├── CMakeLists.txt ├── map_saver.py ├── mi_grid_heatmap.py ├── occupancy_grid_mi.cpp └── monte_carlo_mi.cpp ├── launch ├── monte_carlo_mi.launch ├── occupancy_grid_mi.launch └── params.yaml ├── include └── range_mi │ ├── grid_line.hpp │ ├── p_not_measured.hpp │ ├── grid_mi.hpp │ ├── barely_distorted.hpp │ └── distorted.hpp ├── pysrc ├── demo.py ├── setup.py ├── grid_mi.pyx └── image_mi.py ├── package.xml ├── CMakeLists.txt ├── README.md └── src ├── grid_line.cpp └── grid_mi.cpp /maps/bucket.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sportdeath/range_mi/HEAD/maps/bucket.png -------------------------------------------------------------------------------- /msg/MIGrid.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64[] data 3 | uint32 height 4 | uint32 width 5 | -------------------------------------------------------------------------------- /maps/cubicles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sportdeath/range_mi/HEAD/maps/cubicles.png -------------------------------------------------------------------------------- /maps/classroom_incomplete.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sportdeath/range_mi/HEAD/maps/classroom_incomplete.png -------------------------------------------------------------------------------- /maps/building_31_incomplete.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sportdeath/range_mi/HEAD/maps/building_31_incomplete.png -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | *.sw* 3 | *.pdf 4 | *.txt 5 | *.png 6 | *.yaml 7 | *.pgm 8 | 9 | pysrc/*.so 10 | pysrc/*.cpp 11 | -------------------------------------------------------------------------------- /maps/classroom_incomplete_small.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sportdeath/range_mi/HEAD/maps/classroom_incomplete_small.png -------------------------------------------------------------------------------- /maps/bucket.yaml: -------------------------------------------------------------------------------- 1 | image: bucket.png 2 | resolution: 0.5 3 | origin: [0, 0, 0] 4 | negate: 0 5 | occupied_thresh: 1 6 | free_thresh: 0 7 | mode: scale 8 | -------------------------------------------------------------------------------- /maps/classroom_incomplete.yaml: -------------------------------------------------------------------------------- 1 | image: classroom_incomplete.png 2 | resolution: 0.03 3 | origin: [0, 0, 0] 4 | negate: 0 5 | occupied_thresh: 1 6 | free_thresh: 0 7 | mode: scale 8 | -------------------------------------------------------------------------------- /maps/building_31_incomplete.yaml: -------------------------------------------------------------------------------- 1 | image: building_31_incomplete.png 2 | resolution: 0.05 3 | origin: [0, 0, 0] 4 | negate: 0 5 | occupied_thresh: 1 6 | free_thresh: 0 7 | mode: scale 8 | -------------------------------------------------------------------------------- /maps/classroom_incomplete_small.yaml: -------------------------------------------------------------------------------- 1 | image: classroom_incomplete_small.png 2 | resolution: 1 3 | origin: [0, 0, 0] 4 | negate: 0 5 | occupied_thresh: 1 6 | free_thresh: 0 7 | mode: scale 8 | -------------------------------------------------------------------------------- /maps/cubicles.yaml: -------------------------------------------------------------------------------- 1 | image: cubicles.png 2 | # For other figure use resolution: 1 3 | resolution: 0.01 4 | origin: [0, 0, 0] 5 | negate: 0 6 | occupied_thresh: 1 7 | free_thresh: 0 8 | mode: scale 9 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Enable testing 2 | enable_testing() 3 | 4 | # Compile all the test binaries 5 | file(GLOB SRC_FILES *.cpp) 6 | foreach(_file ${SRC_FILES}) 7 | get_filename_component(_name ${_file} NAME_WE) 8 | add_executable(${_name} ${_file}) 9 | target_link_libraries(${_name} ${LIBS}) 10 | add_test(${_name} ${_name}) 11 | endforeach() 12 | -------------------------------------------------------------------------------- /node/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Include catkin_libraries 2 | include_directories(${catkin_INCLUDE_DIRS}) 3 | set(LIBS ${LIBS} ${catkin_LIBRARIES}) 4 | 5 | # Add the nodes 6 | file(GLOB SRC_FILES *.cpp) 7 | foreach(_file ${SRC_FILES}) 8 | get_filename_component(_name ${_file} NAME_WE) 9 | add_executable(${_name} ${_file}) 10 | add_dependencies(${_name} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 11 | target_link_libraries(${_name} ${LIBS}) 12 | endforeach() 13 | -------------------------------------------------------------------------------- /launch/monte_carlo_mi.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /launch/occupancy_grid_mi.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /include/range_mi/grid_line.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace range_mi { 4 | namespace grid_line { 5 | 6 | void draw( 7 | unsigned int height, 8 | unsigned int widht, 9 | double x, 10 | double y, 11 | double theta, 12 | unsigned int * const line, 13 | double * const widths, 14 | unsigned int & num_cells); 15 | 16 | void sample( 17 | unsigned int height, 18 | unsigned int width, 19 | double theta, 20 | double & x, 21 | double & y, 22 | double & spatial_interpolation); 23 | 24 | }} 25 | -------------------------------------------------------------------------------- /pysrc/demo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import numpy as np 4 | from grid_mi import grid_mi 5 | 6 | # Create an occupancy map 7 | map_ = np.array( 8 | [[0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8], 9 | [0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8], 10 | [0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8], 11 | [0.8, 0.01, 0.99, 0.99, 0.99, 0.01, 0.8], 12 | [0.8, 0.01, 0.99, 0.99, 0.99, 0.01, 0.8], 13 | [0.8, 0.01, 0.99, 0.99, 0.99, 0.01, 0.8], 14 | [0.8, 0.01, 0.99, 0.99, 0.99, 0.01, 0.8]]) 15 | 16 | num_beams = 200 17 | 18 | # Compute! 19 | print(grid_mi(map_, num_beams)) 20 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | range_mi 4 | 0.0.0 5 | Tools for computing the mutual information between occupancy maps and range measurements. 6 | 7 | tfh 8 | 9 | MIT 10 | catkin 11 | roscpp 12 | nav_msgs 13 | geometry_msgs 14 | message_generation 15 | message_runtime 16 | 17 | -------------------------------------------------------------------------------- /pysrc/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from Cython.Build import cythonize 3 | from distutils.extension import Extension 4 | import numpy as np 5 | import os 6 | 7 | # Add install locations 8 | include_dirs = ["/usr/local/include", np.get_include()] 9 | library_dirs = ["/usr/local/lib"] 10 | 11 | extensions = [ 12 | Extension( 13 | "grid_mi", 14 | ["grid_mi.pyx"], 15 | libraries=["range_mi"], 16 | include_dirs=include_dirs, 17 | library_dirs=library_dirs, 18 | language='c++', 19 | extra_compile_args=["-std=c++11", "-Ofast"], 20 | extra_link_args=["-std=c++11"] 21 | )] 22 | 23 | setup( 24 | ext_modules=cythonize(extensions) 25 | ) 26 | -------------------------------------------------------------------------------- /pysrc/grid_mi.pyx: -------------------------------------------------------------------------------- 1 | import cython 2 | import numpy as np 3 | cimport numpy as np 4 | from libcpp.vector cimport vector 5 | 6 | cdef extern from "range_mi/grid_mi.hpp" namespace "range_mi": 7 | 8 | cppclass GridMI: 9 | 10 | GridMI(unsigned int height, unsigned int width) 11 | 12 | void compute_mi(const double * const vacancy, unsigned int num_beams) 13 | 14 | const vector[double] & mi() 15 | 16 | """ 17 | This function turns a map of occupancy 18 | probabilities into a map indicating where 19 | the highest mutual information is. 20 | """ 21 | def grid_mi( 22 | np.ndarray[double, ndim=2, mode="c"] vacancy, 23 | unsigned int num_beams): 24 | 25 | # Create a mutual information computation device 26 | mi_computer = new GridMI(vacancy.shape[0], vacancy.shape[1]); 27 | 28 | # Compute the mutual information 29 | mi_computer.compute_mi( vacancy.data, num_beams) 30 | 31 | # Copy the output 32 | return np.array(mi_computer.mi()).reshape((vacancy.shape[0], vacancy.shape[1])) 33 | -------------------------------------------------------------------------------- /node/map_saver.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import numpy as np 4 | from PIL import Image 5 | 6 | import rospy 7 | from nav_msgs.msg import OccupancyGrid 8 | 9 | """ 10 | Save ROS maps to images. 11 | """ 12 | class MapSaver: 13 | 14 | def __init__(self): 15 | topic = "/mapped" 16 | 17 | # Subscribe to vectorized maps 18 | self.sub = rospy.Subscriber( 19 | topic, 20 | OccupancyGrid, 21 | self.map_callback, 22 | queue_size=1) 23 | 24 | def map_callback(self, map_): 25 | # Plot the map! 26 | Z = np.array(map_.data).reshape(map_.info.height, map_.info.width) 27 | Z = (1 - Z/100.) 28 | # Z[np.logical_or(Z < 1, Z > 0)] = 0.5 29 | Z = np.flip(Z,0) 30 | Z *= 255 31 | Z = Z.astype(np.uint8) 32 | 33 | img = Image.fromarray(Z) 34 | img.save('occupancy_grid.png') 35 | print("saved!") 36 | 37 | if __name__ == "__main__": 38 | rospy.init_node("map_saver") 39 | ms = MapSaver() 40 | rospy.spin() 41 | -------------------------------------------------------------------------------- /include/range_mi/p_not_measured.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace range_mi { 6 | namespace p_not_measured { 7 | 8 | template 9 | void line( 10 | const unsigned int * const line, 11 | const double * const vacancy, 12 | const double * const width, 13 | unsigned int num_cells, 14 | double dtheta, 15 | double * const output_) { 16 | 17 | double width_sum = 0; 18 | double miss_p_product = 1; 19 | double pnm = 0; 20 | for (unsigned int i = 0; i < num_cells; i++) { 21 | unsigned int j = line[i]; 22 | 23 | // Scale the probability by r^d to account 24 | // for radial overlap as well as the width 25 | // to account for aliasing. 26 | output_[j] += 27 | width[i] * dtheta * 28 | std::pow(width_sum, dimension - 1) * 29 | pnm; 30 | 31 | // The probability of the next cell not being 32 | // measured is the probability some previous 33 | // combination of cells were missed followed 34 | // by a hit. 35 | double miss_p = std::pow(vacancy[j], width[i]); 36 | pnm += miss_p_product * (1 - miss_p); 37 | width_sum += width[i]; 38 | miss_p_product *= miss_p; 39 | } 40 | } 41 | 42 | }} 43 | -------------------------------------------------------------------------------- /node/mi_grid_heatmap.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | 6 | import rospy 7 | from range_mi.msg import MIGrid 8 | 9 | """ 10 | Save mutual information surfaces to images. 11 | """ 12 | class MIGridHeatmap: 13 | 14 | def __init__(self): 15 | topic = "/mi" 16 | 17 | # Subscribe to vectorized maps 18 | self.sub = rospy.Subscriber( 19 | topic, 20 | MIGrid, 21 | self.map_callback, 22 | queue_size=1) 23 | 24 | def map_callback(self, map_): 25 | # Plot the map! 26 | Z = np.array(map_.data).reshape(map_.height, map_.width) 27 | plt.imshow(Z, origin='lower', cmap='inferno') 28 | 29 | # Get rid of axes 30 | plt.box(False) 31 | plt.yticks([]) 32 | plt.xticks([]) 33 | plt.tick_params(direction='in') 34 | 35 | # Save it! 36 | plt.savefig("mi_grid.pdf", bbox_inches='tight', pad_inches=-0.03, transparent=False) 37 | plt.colorbar() 38 | plt.savefig("mi_grid_colorbar.pdf", bbox_inches='tight', pad_inches=0, transparent=False) 39 | print("Made map!") 40 | 41 | if __name__ == "__main__": 42 | rospy.init_node("mi_grid_heatmap") 43 | hm = MIGridHeatmap() 44 | rospy.spin() 45 | -------------------------------------------------------------------------------- /test/helpers.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // Initialize random generator 4 | std::random_device random_device; 5 | std::mt19937 gen(random_device()); 6 | std::uniform_real_distribution dist(0.,1.); 7 | 8 | // ... and a way to make random vectors 9 | void random_p(std::vector & p) { 10 | for (size_t i = 0; i < p.size(); i++) { 11 | p[i] = dist(gen); 12 | } 13 | } 14 | 15 | void numerical_pdf( 16 | const unsigned int * const line, 17 | const double * const vacancy, 18 | const double * const width, 19 | unsigned int num_cells, 20 | double integration_step, 21 | double * const pdf, 22 | unsigned int & pdf_size) { 23 | 24 | unsigned int i = 0; 25 | double r = 0; 26 | double width_sum = 0; 27 | double pdf_decay = 1; 28 | pdf_size = 0; 29 | while (i < num_cells) { 30 | unsigned int j = line[i]; 31 | 32 | // Compute the pdf 33 | pdf[pdf_size++] = 34 | pdf_decay * ( 35 | -std::pow(vacancy[j], r - width_sum) * 36 | std::log(vacancy[j])); 37 | 38 | // Make a step 39 | r += integration_step; 40 | if (r - width_sum > width[i]) { 41 | // Cell completed, 42 | // move to the next cell! 43 | pdf_decay *= std::pow(vacancy[j], width[i]); 44 | width_sum += width[i]; 45 | i++; 46 | } 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /test/profile_grid_mi.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Compute the time to compute the mutual information 3 | * in a grid at a variety of map sizes. 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "helpers.hpp" 13 | 14 | int main() { 15 | 16 | unsigned int num_beams = 200; 17 | unsigned int max_side_length = 1000; 18 | unsigned int max_area = max_side_length * max_side_length; 19 | 20 | // Randomize the occupancy grid 21 | std::vector vacancy(max_area); 22 | random_p(vacancy); 23 | 24 | // Output file 25 | std::ofstream f; 26 | f.open("profile_grid_mi_results.txt"); 27 | 28 | for (unsigned int side_length = 1; side_length < max_side_length; side_length++) { 29 | std::cout << side_length << std::endl; 30 | // Initialize MI computation 31 | range_mi::GridMI mi_computer(side_length, side_length); 32 | 33 | // Set the clock to zero 34 | auto start = std::chrono::high_resolution_clock::now(); 35 | 36 | // Compute MI 37 | mi_computer.compute_mi(vacancy.data(), num_beams); 38 | 39 | auto end = std::chrono::high_resolution_clock::now(); 40 | std::chrono::duration total = end - start; 41 | 42 | f << side_length * side_length << " " << total.count() << std::endl; 43 | } 44 | 45 | f.close(); 46 | 47 | } 48 | -------------------------------------------------------------------------------- /pysrc/image_mi.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """ 4 | Convert a folder full of occupancy map 5 | images to MI surface images. 6 | """ 7 | 8 | import sys, os 9 | from PIL import Image 10 | import numpy as np 11 | from grid_mi import grid_mi 12 | from matplotlib import cm 13 | 14 | # Fetch arguments 15 | if len(sys.argv) != 5: 16 | print("Usage: ./maps_to_mi num_beams resolution input_dir output_dir") 17 | sys.exit() 18 | 19 | num_beams = int(sys.argv[1]) 20 | resolution = float(sys.argv[2]) 21 | input_dir = sys.argv[3] 22 | output_dir = sys.argv[4] 23 | 24 | # Iterate over files 25 | i = 0 26 | for f in os.listdir(input_dir): 27 | # Open the array and convert to numpy 28 | im = Image.open(os.path.join(input_dir, f)) 29 | m = np.array(im) 30 | 31 | # Only take the first 2 dimensions 32 | while len(m.shape) > 2: 33 | m = np.take(m, -1, axis=-1) 34 | 35 | # Convert to double 36 | m = m.astype(np.double)/(2**8 - 1.) 37 | 38 | # Account for resolution 39 | m = np.power(m, resolution) 40 | 41 | # Compute the MI 42 | mi = grid_mi(m, num_beams) 43 | 44 | # Normalize 45 | mi = mi/np.max(mi) 46 | 47 | # Turn back into an image 48 | mi_im = Image.fromarray((cm.inferno(mi) * (2**8 - 1)).astype(np.uint8)) 49 | mi_im.save(os.path.join(output_dir, f)) 50 | 51 | print(i) 52 | i += 1 53 | -------------------------------------------------------------------------------- /launch/params.yaml: -------------------------------------------------------------------------------- 1 | ########################## 2 | # Computation Parameters # 3 | ########################## 4 | # The number of beam angles for mutual 5 | # information computation. 6 | num_beams: 600 7 | # The number of beams angles used to 8 | # condition the map at a single point. 9 | condition_steps: 50000 10 | # The standard deviation of the sensor noise. 11 | # If the noise dev is <= 0 the barely distorted 12 | # computation will be used. 13 | noise_dev: 0 # Meters 14 | # The number of standard deviations before the 15 | # noise is truncated. 16 | noise_truncation: 4 # Std Deviations 17 | # An integration step for the sensor noise 18 | noise_integration_step: 0.25 # Meters 19 | 20 | ############## 21 | # I/O Topics # 22 | ############## 23 | # The map input (occupancy grid) 24 | map_topic: "/map" 25 | # The mutual surface output (MIGrid, see .msg file) 26 | mi_topic: "/mi" 27 | 28 | ################# 29 | # Visualization # 30 | ################# 31 | # Publish a mutual information surface to 32 | # RVIZ once it is computed 33 | visualize: true 34 | # Publish incomplete mutual information surfaces to 35 | # RVIZ as they are being computed 36 | visualize_more: true 37 | # Visualize mutual information surfaces 38 | mi_map_topic: "/mi_map" 39 | # Visualize the map of conditional probabilities. 40 | conditional_map_topic: "/conditional_map" 41 | # Click on the map to condition a point 42 | click_condition_topic: "/clicked_point" 43 | # Visualize a randomized map for monte carlo compuation. 44 | binary_map_topic: "/binary_map" 45 | -------------------------------------------------------------------------------- /test/demo.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This is meant to be a human-readable example 3 | * of how this library can be used to compute 4 | * mutual information surfaces. 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | int main() { 15 | 16 | // Create a map 17 | unsigned int height = 7; 18 | unsigned int width = 7; 19 | // The map has a vacant region in the bottom 20 | // center. This region is is bordered on the left 21 | // and right by occupied "walls". Elsewhere there 22 | // is relatively unknown space. 23 | std::vector vacancy = { 24 | 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 25 | 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 26 | 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 27 | 0.8, 0.01, 0.99, 0.99, 0.99, 0.01, 0.8, 28 | 0.8, 0.01, 0.99, 0.99, 0.99, 0.01, 0.8, 29 | 0.8, 0.01, 0.99, 0.99, 0.99, 0.01, 0.8, 30 | 0.8, 0.01, 0.99, 0.99, 0.99, 0.01, 0.8, 31 | }; 32 | 33 | // The number of beams to compute mutual information with. 34 | unsigned int num_beams = 200; 35 | 36 | // Initialize the mutual information computation device 37 | range_mi::GridMI mi_computer(height, width); 38 | 39 | // Compute the mutual information 40 | mi_computer.compute_mi(vacancy.data(), num_beams); 41 | 42 | // Print out the mutual information map 43 | std::cout << "Mutual information surface: " << std::endl; 44 | std::cout << std::fixed; 45 | std::cout << std::setprecision(0); 46 | for (unsigned int i = 0; i < height; i++) { 47 | for (unsigned int j = 0; j < width; j++) { 48 | std::cout << std::setw(6) << mi_computer.mi()[j + i * width]; 49 | } 50 | std::cout << std::endl; 51 | } 52 | 53 | } 54 | -------------------------------------------------------------------------------- /include/range_mi/grid_mi.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace range_mi { 6 | 7 | class GridMI { 8 | 9 | private: 10 | // The computed mutual information 11 | std::vector mi_; 12 | std::vector p_not_measured_; 13 | std::vector p_not_measured_single; 14 | 15 | // Map parameters 16 | unsigned int height, width; 17 | 18 | // Noise parameters 19 | double noise_dev, noise_half_width, integration_step; 20 | std::vector pdf; 21 | 22 | // Temporary storage for lines 23 | std::vector line; 24 | std::vector widths; 25 | double x, y; 26 | unsigned int num_cells; 27 | 28 | public: 29 | GridMI() {} 30 | 31 | GridMI( 32 | unsigned int height, 33 | unsigned int width, 34 | double noise_dev=0, 35 | double noise_half_width=0, 36 | double integration_step=0); 37 | 38 | void compute_mi_beam( 39 | const double * const vacancy, 40 | double theta, 41 | double dtheta, 42 | double & spatial_interpolation); 43 | 44 | void compute_mi( 45 | const double * const vacancy, 46 | unsigned int num_beams); 47 | 48 | void condition( 49 | const double * const vacancy, 50 | double x, 51 | double y, 52 | double theta_min, 53 | double theta_max, 54 | double dtheta); 55 | 56 | void reset_mi() { 57 | std::fill(mi_.begin(), mi_.end(), 0); 58 | } 59 | void reset_p_not_measured() { 60 | std::fill(p_not_measured_.begin(), p_not_measured_.end(), 1); 61 | } 62 | 63 | const std::vector & mi() const { 64 | return mi_; 65 | } 66 | const std::vector & p_not_measured() const { 67 | return p_not_measured_; 68 | } 69 | 70 | static const unsigned int dimension = 2; 71 | static const bool lower_bound = true; 72 | }; 73 | 74 | } 75 | -------------------------------------------------------------------------------- /test/profile_mi.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This test computes the mutual information 3 | * of a large number of random beams to determine 4 | * the average time spent computing mutual information 5 | * per cell 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #include "helpers.hpp" 16 | 17 | // Define constants 18 | unsigned int num_iterations = 100000; 19 | unsigned int num_cells = 100; 20 | double dtheta = 0.1; 21 | const unsigned int dimension = 1; 22 | 23 | int main() { 24 | // Initialize the inputs and outputs 25 | std::vector vacancy(num_cells); 26 | std::vector p_not_measured(num_cells); 27 | std::vector width(num_cells); 28 | std::vector mi(num_cells, 0); 29 | 30 | // Make a line 31 | std::vector line(num_cells); 32 | std::iota(line.begin(), line.end(), 0); 33 | 34 | // Set the clock to zero 35 | std::chrono::duration elapsed(0); 36 | 37 | for (unsigned int i = 0; i < num_iterations; i++) { 38 | 39 | // Randomize the inputs 40 | random_p(vacancy); 41 | random_p(p_not_measured); 42 | random_p(width); 43 | 44 | auto start = std::chrono::high_resolution_clock::now(); 45 | 46 | // Compute the entropy 47 | range_mi::barely_distorted::line( 48 | line.data(), 49 | vacancy.data(), 50 | p_not_measured.data(), 51 | width.data(), 52 | num_cells, 53 | dtheta, 54 | mi.data()); 55 | 56 | auto end = std::chrono::high_resolution_clock::now(); 57 | elapsed += end - start; 58 | } 59 | 60 | std::cout << "Computed " << num_iterations << " beams " << 61 | " with " << num_cells << " cells in " << 62 | elapsed.count() << " seconds." << std::endl; 63 | 64 | std::cout << "That makes an average time of " << 65 | (elapsed.count() * std::nano::den)/(num_iterations * num_cells) << 66 | " nanoseconds per cell" << std::endl; 67 | } 68 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | 3 | project(range_mi) 4 | 5 | set(CMAKE_CXX_STANDARD 11) 6 | 7 | if(NOT CMAKE_BUILD_TYPE) 8 | set (CMAKE_BUILD_TYPE Release) 9 | endif() 10 | 11 | set(CMAKE_CXX_FLAGS "-Wall -Wextra") 12 | set(CMAKE_CXX_FLAGS_DEBUG "-g") 13 | set(CMAKE_CXX_FLAGS_RELEASE "-Ofast") 14 | 15 | 16 | ###################################### 17 | # Compile the library > 18 | ###################################### 19 | 20 | # Add includes 21 | include_directories(include) 22 | file(GLOB SRC_FILES src/*.cpp) 23 | add_library(${PROJECT_NAME} ${SRC_FILES}) 24 | target_link_libraries(${PROJECT_NAME} ${LIBS}) 25 | set(LIBS ${LIBS} ${PROJECT_NAME}) 26 | 27 | # Install the library to CMAKE_INSTALL_PREFIX 28 | # which defaults to /usr/local 29 | install(TARGETS ${PROJECT_NAME} 30 | DESTINATION lib) 31 | install(DIRECTORY include/${PROJECT_NAME} 32 | DESTINATION include) 33 | 34 | ###################################### 35 | # < End compile the library 36 | ###################################### 37 | 38 | 39 | ###################################### 40 | # Add tests and ROS > 41 | ###################################### 42 | 43 | option(BUILD_TESTS "BUILD_TESTS" OFF) 44 | if (BUILD_TESTS) 45 | include_directories(test) 46 | add_subdirectory(test) 47 | endif() 48 | 49 | if(DEFINED CATKIN_DEVEL_PREFIX) 50 | # Find build dependencies 51 | find_package(catkin REQUIRED COMPONENTS 52 | roscpp 53 | nav_msgs 54 | geometry_msgs 55 | message_generation 56 | ) 57 | 58 | # Add the messages 59 | file(GLOB MSG_FILES msg/*.msg) 60 | foreach(_file ${MSG_FILES}) 61 | get_filename_component(_name ${_file} NAME) 62 | add_message_files( 63 | FILES 64 | ${_name} 65 | ) 66 | endforeach() 67 | 68 | # Generate messages 69 | generate_messages( 70 | DEPENDENCIES 71 | std_msgs 72 | ) 73 | 74 | # Define the package 75 | catkin_package( 76 | INCLUDE_DIRS include 77 | LIBRARIES ${PROJECT_NAME} 78 | CATKIN_DEPENDS message_runtime 79 | ) 80 | 81 | # Add nodes 82 | include_directories(node) 83 | add_subdirectory(node) 84 | 85 | # Install the library 86 | install(TARGETS ${PROJECT_NAME} 87 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 88 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 89 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 90 | ) 91 | install(DIRECTORY include/${PROJECT_NAME}/ 92 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 93 | ) 94 | endif() 95 | 96 | ###################################### 97 | # < End add tests and ROS 98 | ###################################### 99 | -------------------------------------------------------------------------------- /test/profile_grid_fsmi.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Compute the time to compute the mutual information 3 | * in a grid at a variety of map sizes. 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "helpers.hpp" 13 | 14 | double func(double delta, double vacancy) { 15 | vacancy = std::max(vacancy, 0.00000001); 16 | vacancy = std::min(vacancy, 0.99999999); 17 | double r = (1 - vacancy)/vacancy; 18 | return std::log((r + 1)/(r + 1/delta)) - std::log(delta)/(r * delta + 1); 19 | } 20 | 21 | int main() { 22 | 23 | unsigned int num_beams = 200; 24 | unsigned int max_side_length = 1000; 25 | unsigned int max_area = max_side_length * max_side_length; 26 | 27 | // FSMI parameters 28 | double delta_occ = 1.5; 29 | double delta_emp = 1/delta_occ; 30 | 31 | // Randomize the occupancy grid 32 | std::vector vacancy(max_area); 33 | random_p(vacancy); 34 | 35 | // Initialize a place for mutual information 36 | std::vector mi(max_area); 37 | 38 | // Initialize data 39 | std::vector line(2*max_side_length); 40 | std::vector widths(2*max_side_length); 41 | 42 | // Output file 43 | std::ofstream f; 44 | f.open("profile_grid_fsmi_results.txt"); 45 | 46 | for (unsigned int side_length = 1; side_length < max_side_length; side_length++) { 47 | std::cout << side_length << std::endl; 48 | 49 | double theta; 50 | unsigned int num_cells; 51 | double p_previous_empty; 52 | double p_i_first_non_empty; 53 | double info_gain, info_loss; 54 | 55 | auto start = std::chrono::high_resolution_clock::now(); 56 | for (unsigned int x = 0; x < side_length; x++) { 57 | for (unsigned int y = 0; y < side_length; y++) { 58 | for (unsigned int b = 0; b < num_beams; b++) { 59 | theta = b/num_beams; 60 | 61 | // Compute the intersections of 62 | // the line with the grid 63 | range_mi::grid_line::draw( 64 | side_length, side_length, 65 | x, y, theta, 66 | line.data(), 67 | widths.data(), 68 | num_cells); 69 | 70 | // Compute the mutual information 71 | p_previous_empty = 1; 72 | info_loss = 0; 73 | mi[x + y * side_length] = 0; 74 | for (unsigned int i = 0; i < num_cells; i++) { 75 | p_i_first_non_empty = p_previous_empty * (1 - vacancy[line[i]]); 76 | p_previous_empty *= vacancy[line[i]]; 77 | 78 | info_gain = info_loss + func(delta_occ, vacancy[line[i]]); 79 | info_loss += func(delta_emp, vacancy[line[i]]); 80 | 81 | // Assume the noise width is zero to be fair, 82 | // so no inner loop 83 | mi[x + y * side_length] += p_i_first_non_empty * info_gain; 84 | } 85 | } 86 | } 87 | } 88 | auto end = std::chrono::high_resolution_clock::now(); 89 | std::chrono::duration total = end - start; 90 | 91 | f << side_length * side_length << " " << total.count() << std::endl; 92 | } 93 | 94 | f.close(); 95 | } 96 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Range Mutual Information 2 | 3 | This package defines functions that compute the amount of information that is expected to be gained about an incomplete occupancy map from a potential range measurement. Range measurement sources could include lidar, sonar, depth cameras, etc. Below is an example output produced by this code. 4 | The package can be built standalone or as a ROS library. 5 | 6 | For more information on the algorithm see the related [ICRA 2020 publication](https://ieeexplore.ieee.org/document/9196592) and for an earlier but more detailed overview see [my MEng thesis](https://dspace.mit.edu/handle/1721.1/124248). 7 | 8 | ![mi_surface](https://live.staticflickr.com/65535/49493728457_f33ba30d11_o_d.png) 9 | 10 | 11 | ## Usage 12 | 13 | ### C++ API 14 | 15 | Clone the library and build with ```cmake```: 16 | 17 | git clone https://github.com/sportdeath/range_mi.git 18 | cd range_mi 19 | mkdir build 20 | cd build 21 | cmake .. 22 | make 23 | 24 | To build the tests you can add a flag to ```cmake``` before making: 25 | 26 | cmake -DBUILD_TESTS=ON .. 27 | make 28 | 29 | To run a test, for example the ```test/demo.cpp``` test, execute: 30 | 31 | ./test/demo 32 | 33 | The other test cases compare the output of the algorithm to numerical integration and benchmark speed. 34 | 35 | ### Python API 36 | 37 | After building the C++ library as described above also install the library: 38 | 39 | sudo make install 40 | 41 | Then compile the python library (requires ```cython```): 42 | 43 | python setup.py build_ext --inplace 44 | 45 | This builds a function ```grid_mi``` that simply takes as input a 2D occupancy map and returns a 2D mutual information surface (both numpy arrays). A simple demo is included in the ```pysrc/demo.py``` file. 46 | 47 | ### ROS Demo Nodes 48 | 49 | Clone the library into your catkin workspace: 50 | 51 | cd ~/catkin_ws/src 52 | git clone https://github.com/sportdeath/range_mi.git 53 | cd ~/catkin_ws 54 | catkin_make 55 | 56 | Then to compute the mutual information of an occupancy grid, you can run: 57 | 58 | roslaunch range_mi occupancy_grid_mi.launch 59 | 60 | In ```rviz``` the occupancy grid map will be displayed on the topic ```/map``` and the mutual information surface will be displayed on the topic ```/mi_map```. The topics names, number of range measurement beams and other parameters are available in the ```launch/params.yaml``` file. By running the ```node/mi_grid_heatmap.py``` script, the mutual information surface can be saved as colored image like the one above. 61 | 62 | By using the "Publish Point" tooltip in ```rviz```, you can click on a point in the map to condition the mutual information on a measurement being made at that point. This will likely reduce the information gain within the view of the clicked point. 63 | 64 | We have also included another script that computes the mutual information by averaging the information gain over random occupancy assigments. This will take much longer than the included algorithm, but it will eventually converge to the same result. Run it on a map as follows: 65 | 66 | roslaunch range_mi monte_carlo_mi.launch 67 | -------------------------------------------------------------------------------- /src/grid_line.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "range_mi/grid_line.hpp" 5 | 6 | void range_mi::grid_line::draw( 7 | unsigned int height, 8 | unsigned int width, 9 | double x, 10 | double y, 11 | double theta, 12 | unsigned int * const line, 13 | double * const widths, 14 | unsigned int & num_cells) { 15 | 16 | // Pre-compute sin and cos 17 | double sin = std::sin(theta); 18 | double cos = std::cos(theta); 19 | 20 | // Reset the line 21 | num_cells = 0; 22 | 23 | // Initialize the cell indices 24 | int floor_x = std::floor(x); 25 | int floor_y = std::floor(y); 26 | 27 | // Initialize the distances along the line to the 28 | // next horizontal cell boundary (dx) or vertical 29 | // cell boundary (dy). The maximum distance for 30 | // either is dx/y_unit. 31 | double dx_unit = 1/std::abs(cos); 32 | double dy_unit = 1/std::abs(sin); 33 | double dx = dx_unit * ((cos > 0) ? floor_x + 1 - x : x - floor_x); 34 | double dy = dy_unit * ((sin > 0) ? floor_y + 1 - y : y - floor_y); 35 | 36 | // Compute the sign of the steps taken 37 | int x_step = (cos > 0) ? 1 : -1; 38 | int y_step = (sin > 0) ? 1 : -1; 39 | 40 | // While we are inside the map 41 | while ( 42 | floor_x >= 0 and 43 | floor_y >= 0 and 44 | floor_x < (int) width and 45 | floor_y < (int) height) { 46 | 47 | // Add the cell index 48 | line[num_cells] = floor_y * width + floor_x; 49 | 50 | // The width is the minimum distance to either cell 51 | widths[num_cells] = std::min(dx, dy); 52 | 53 | // Subtract the width from the line boundaries 54 | // and the distance to the boundary 55 | dx -= widths[num_cells]; 56 | dy -= widths[num_cells]; 57 | 58 | // Replenish if necessary 59 | if (dx <= 0) { 60 | dx = dx_unit; 61 | floor_x += x_step; 62 | } 63 | if (dy <= 0) { 64 | dy = dy_unit; 65 | floor_y += y_step; 66 | } 67 | 68 | // Increment the cell number 69 | num_cells++; 70 | } 71 | } 72 | 73 | void range_mi::grid_line::sample( 74 | unsigned int height, 75 | unsigned int width, 76 | double theta, 77 | double & x, 78 | double & y, 79 | double & spatial_interpolation) { 80 | 81 | // Account for small offset 82 | theta += 0.000000000001; 83 | 84 | // Pre-compute trig values 85 | double s = std::sin(theta); 86 | double c = std::cos(theta); 87 | double s_abs = std::abs(s); 88 | double c_abs = std::abs(c); 89 | 90 | // Compute the normalized spatial dimensions 91 | double normalized_width = s_abs * width; 92 | double normalized_height = c_abs * height; 93 | 94 | double perimeter = 95 | (normalized_width + normalized_height) * 96 | spatial_interpolation; 97 | 98 | // Compute the number of steps that must be made 99 | double steps = (normalized_width + normalized_height)/(c_abs + s_abs); 100 | // Use it to update the interpolation 101 | spatial_interpolation += 1./steps; 102 | // Reset if necessary 103 | if (spatial_interpolation >= 1) { 104 | spatial_interpolation = 0; 105 | } 106 | 107 | if (perimeter < normalized_width) { 108 | x = perimeter/s_abs; 109 | y = 0; 110 | } else { 111 | // The switch won't happen exactly at the 112 | // normalized_width, so perform integer division 113 | double switch_steps = std::floor(normalized_width/(c_abs + s_abs)); 114 | double switch_perimeter = switch_steps * (c_abs + s_abs); 115 | y = (perimeter - switch_perimeter)/c_abs; 116 | x = 0; 117 | } 118 | 119 | // Account for other quadrants 120 | if (s < 0) y = height - y - 0.000001; 121 | if (c < 0) x = width - x - 0.000001; 122 | } 123 | -------------------------------------------------------------------------------- /test/numerical_mi.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This test file computes the mutual information 3 | * using the provided algorithm as well as numerical 4 | * integration and presents the differences between the 5 | * two results. 6 | * 7 | * The results are presented for dimensions 1 through 5. 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | #include "helpers.hpp" 19 | 20 | // Define constants 21 | double integration_step = 0.000001; 22 | double vacancy_scaling = 0.1; 23 | double dtheta = 0.1; 24 | unsigned int num_cells = 100; 25 | const unsigned int num_dimensions = 5; 26 | 27 | double numerical_mi( 28 | const double * const pdf, 29 | unsigned pdf_size, 30 | unsigned int dimension, 31 | double dtheta, 32 | double integration_step) { 33 | 34 | double h_Z = 0; 35 | double h_Z_given_M = 0; 36 | for (unsigned int i = 0; i < pdf_size; i++) { 37 | double r = i * integration_step; 38 | 39 | h_Z += 40 | pdf[i] * 41 | std::pow(r, dimension - 1) * 42 | -std::log(pdf[i]) * 43 | integration_step * 44 | dtheta; 45 | 46 | h_Z_given_M += 47 | (1 - std::log(range_mi::barely_distorted::noise_l)) * 48 | pdf[i] * 49 | std::pow(r, dimension - 1) * 50 | integration_step * 51 | dtheta; 52 | } 53 | 54 | return h_Z - h_Z_given_M; 55 | } 56 | 57 | int main() { 58 | // Initialize the inputs and outputs 59 | std::vector vacancy(num_cells); 60 | std::vector p_not_measured(num_cells, 1); 61 | std::vector width(num_cells); 62 | // Randomize then 63 | random_p(vacancy); 64 | random_p(width); 65 | 66 | // Scale vacancy to make everything generally lower 67 | for (unsigned int i = 0; i < num_cells; i++) { 68 | vacancy[i] = std::pow(vacancy[i], vacancy_scaling); 69 | } 70 | 71 | // Make a line 72 | std::vector line(num_cells); 73 | std::iota(line.begin(), line.end(), 0); 74 | 75 | std::cout << "Computing the barely distorted pdf" << std::endl; 76 | 77 | // Compute the pdf 78 | unsigned int pdf_size; 79 | std::vector pdf(num_cells/integration_step); 80 | numerical_pdf( 81 | line.data(), 82 | vacancy.data(), 83 | width.data(), 84 | num_cells, 85 | integration_step, 86 | pdf.data(), 87 | pdf_size); 88 | 89 | std::cout << "Computing the barely distorted mutual information numerically..." << std::endl; 90 | 91 | std::vector numerical_mi_(num_dimensions); 92 | for (unsigned int i = 0; i < num_dimensions; i++) { 93 | numerical_mi_[i] = numerical_mi(pdf.data(), pdf_size, i + 1, dtheta, integration_step); 94 | } 95 | 96 | std::cout << "Computing the barely distorted mutual information exactly..." << std::endl; 97 | 98 | std::vector exact_mi(num_dimensions, 0); 99 | std::vector mi(num_cells); 100 | 101 | // Clear the output 102 | std::fill(mi.begin(), mi.end(), 0); 103 | range_mi::barely_distorted::line<1>( 104 | line.data(), vacancy.data(), p_not_measured.data(), 105 | width.data(), num_cells, dtheta, mi.data()); 106 | exact_mi[0] = mi[0]; 107 | std::fill(mi.begin(), mi.end(), 0); 108 | range_mi::barely_distorted::line<2>( 109 | line.data(), vacancy.data(), p_not_measured.data(), 110 | width.data(), num_cells, dtheta, mi.data()); 111 | exact_mi[1] = mi[0]; 112 | std::fill(mi.begin(), mi.end(), 0); 113 | range_mi::barely_distorted::line<3>( 114 | line.data(), vacancy.data(), p_not_measured.data(), 115 | width.data(), num_cells, dtheta, mi.data()); 116 | exact_mi[2] = mi[0]; 117 | std::fill(mi.begin(), mi.end(), 0); 118 | range_mi::barely_distorted::line<4>( 119 | line.data(), vacancy.data(), p_not_measured.data(), 120 | width.data(), num_cells, dtheta, mi.data()); 121 | exact_mi[3] = mi[0]; 122 | std::fill(mi.begin(), mi.end(), 0); 123 | range_mi::barely_distorted::line<5>( 124 | line.data(), vacancy.data(), p_not_measured.data(), 125 | width.data(), num_cells, dtheta, mi.data()); 126 | exact_mi[4] = mi[0]; 127 | 128 | std::cout << std::endl; 129 | for (unsigned int i = 0; i < num_dimensions; i++) { 130 | std::cout << "d" << i + 1 << ": " << numerical_mi_[i] << ", " << exact_mi[i] << 131 | ", difference: " << std::abs(numerical_mi_[i] - exact_mi[i]) << 132 | " or " << 100 * std::abs(numerical_mi_[i] - exact_mi[i])/exact_mi[i] << "%" << 133 | std::endl; 134 | } 135 | } 136 | -------------------------------------------------------------------------------- /src/grid_mi.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "range_mi/barely_distorted.hpp" 4 | #include "range_mi/distorted.hpp" 5 | #include "range_mi/p_not_measured.hpp" 6 | #include "range_mi/grid_line.hpp" 7 | #include "range_mi/grid_mi.hpp" 8 | 9 | range_mi::GridMI::GridMI( 10 | unsigned int height_, 11 | unsigned int width_, 12 | double noise_dev_, 13 | double noise_half_width_, 14 | double integration_step_) : 15 | height(height_), 16 | width(width_), 17 | noise_dev(noise_dev_), 18 | noise_half_width(noise_half_width_), 19 | integration_step(integration_step_) { 20 | 21 | // Set the mutual information to zero 22 | mi_ = std::vector(height * width, 0); 23 | 24 | // Initialize the conditioning map 25 | p_not_measured_ = std::vector(height * width, 1); 26 | 27 | // Initialize the storage vectors 28 | line = std::vector(2 * std::max(height, width)); 29 | widths = std::vector(2 * std::max(height, width)); 30 | p_not_measured_single = std::vector(height * width, 0); 31 | 32 | if (noise_dev > 0) { 33 | // Allocate free space for computing density functions 34 | pdf = std::vector((4 * noise_half_width + 2)/integration_step); 35 | } 36 | } 37 | 38 | void range_mi::GridMI::compute_mi_beam( 39 | const double * const vacancy, 40 | double theta, 41 | double dtheta, 42 | double & spatial_interpolation) { 43 | 44 | // Convert the interpolation parameters to 45 | // x, y, theta 46 | range_mi::grid_line::sample( 47 | height, width, 48 | theta, x, y, 49 | spatial_interpolation); 50 | 51 | // Compute the intersections of 52 | // the line with the grid 53 | range_mi::grid_line::draw( 54 | height, width, 55 | x, y, theta, 56 | line.data(), 57 | widths.data(), 58 | num_cells); 59 | 60 | // Accumulate the mutual information 61 | if (noise_dev <= 0) { 62 | range_mi::barely_distorted::line( 63 | line.data(), 64 | vacancy, 65 | p_not_measured_.data(), 66 | widths.data(), 67 | num_cells, 68 | dtheta, 69 | mi_.data()); 70 | } else { 71 | range_mi::distorted::line( 72 | line.data(), 73 | vacancy, 74 | widths.data(), 75 | num_cells, 76 | noise_dev, 77 | noise_half_width, 78 | integration_step, 79 | dtheta, 80 | pdf.data(), 81 | mi_.data()); 82 | } 83 | } 84 | 85 | void range_mi::GridMI::compute_mi( 86 | const double * const vacancy, 87 | unsigned int num_beams) { 88 | 89 | // Iterate over beams spanning across the map 90 | double spatial_interpolation = 0; 91 | double theta = 0; 92 | double dtheta = (2 * M_PI)/num_beams; 93 | while (theta < 2 * M_PI) { 94 | 95 | // Compute the mutual information along 96 | // the beam in direction "theta", translated 97 | // by "spatial_interpolation". The spatial_interpolation 98 | // is updated by the compute_mi_beam function. 99 | compute_mi_beam( 100 | vacancy, 101 | theta, 102 | dtheta, 103 | spatial_interpolation); 104 | 105 | // Check to see if the spatial interpolation has 106 | // wrapped back to zero. 107 | if (spatial_interpolation == 0) { 108 | // If so, move to the next beam 109 | theta += dtheta; 110 | } 111 | } 112 | } 113 | 114 | 115 | void range_mi::GridMI::condition( 116 | const double * const vacancy, 117 | double x, 118 | double y, 119 | double theta_min, 120 | double theta_max, 121 | double dtheta) { 122 | 123 | // Clear the old p_measured 124 | std::fill(p_not_measured_single.begin(), p_not_measured_single.end(), 0); 125 | 126 | // Compute the new one for the given point 127 | for (double theta = theta_min; theta < theta_max; theta+=dtheta) { 128 | // Draw a line 129 | range_mi::grid_line::draw( 130 | height, width, 131 | x, y, theta, 132 | line.data(), 133 | widths.data(), 134 | num_cells); 135 | 136 | // Accumulate p_not_measured along the line 137 | range_mi::p_not_measured::line( 138 | line.data(), 139 | vacancy, 140 | widths.data(), 141 | num_cells, 142 | dtheta, 143 | p_not_measured_single.data()); 144 | } 145 | 146 | // Update the probabilities 147 | for (unsigned int i = 0; i < p_not_measured_.size(); i++) { 148 | if (lower_bound) { 149 | p_not_measured_[i] -= 1 - p_not_measured_single[i]; 150 | } else { 151 | p_not_measured_[i] = std::min(p_not_measured_[i], p_not_measured_single[i]); 152 | } 153 | p_not_measured_[i] = std::min(p_not_measured_[i], 1.); 154 | p_not_measured_[i] = std::max(p_not_measured_[i], 0.); 155 | } 156 | } 157 | -------------------------------------------------------------------------------- /include/range_mi/barely_distorted.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace range_mi { 7 | namespace barely_distorted { 8 | 9 | const double noise_l = 9e100; 10 | 11 | template 12 | void line( 13 | const unsigned int * const line, 14 | const double * const vacancy, 15 | const double * const p_not_measured, 16 | const double * const width, 17 | unsigned int num_cells, 18 | double dtheta, 19 | double * const output) { 20 | 21 | // Precompute noise quantities 22 | double neg_log_noise_l = -std::log(noise_l); 23 | 24 | // Precompute factorials 25 | std::array factorial; 26 | factorial[0] = 1; 27 | for (unsigned int k = 1; k <= dimension; k++) { 28 | factorial[k] = factorial[k - 1] * k; 29 | } 30 | 31 | // Initialize the expected values 32 | // E[R^kI(R)] to E[N^kI(N)] 33 | std::array distk_info; 34 | double noise_l_inv = 1; 35 | for (unsigned int k = 0; k < dimension; k++) { 36 | distk_info[k] = 37 | noise_l_inv * (factorial[k+1] + factorial[k] * neg_log_noise_l); 38 | noise_l_inv /= noise_l; 39 | } 40 | 41 | // Initialize the expected values 42 | // E[R^k] to E[N^k] 43 | std::array distk; 44 | noise_l_inv = 1; 45 | for (unsigned int k = 0; k < dimension; k++) { 46 | distk[k] = noise_l_inv * factorial[k]; 47 | noise_l_inv /= noise_l; 48 | } 49 | 50 | // Also initialize a space for gamma values 51 | std::array gamma; 52 | // and powers of w 53 | std::array w_to_the; 54 | // And negative powers of l 55 | std::array l_to_the_neg; 56 | 57 | // Iterate backwards over the cells in the line 58 | for (int line_index = num_cells - 1; line_index >= 0; line_index--) { 59 | 60 | // Fetch the vacancy, p_not_measured, 61 | // and width of the current cell 62 | unsigned int map_index = line[line_index]; 63 | double v = vacancy[map_index]; 64 | double pnm = p_not_measured[map_index]; 65 | double w = width[line_index]; 66 | 67 | // Compute the negative log of the vacancy 68 | double l; 69 | if (v <= 0) { 70 | l = noise_l; 71 | } else { 72 | l = -std::log(v); 73 | } 74 | 75 | // Clip the vacancy to the maximum 76 | l = std::min(l, noise_l); 77 | 78 | // Then compute the negative log of l 79 | double neg_log_l; 80 | if (l <= 0) { 81 | // When l = 0 this terms is zeroed out by gammas 82 | neg_log_l = 0; 83 | } else { 84 | neg_log_l = -std::log(l); 85 | } 86 | 87 | // Precompute the probability of missing 88 | // the constant region 89 | double p_miss = std::exp(-l * w); 90 | 91 | // Precompute the gamma values 92 | gamma[0] = 1 - p_miss; 93 | double wl = 1; 94 | for (unsigned int k = 1; k <= dimension; k++) { 95 | // Update (wl)^k 96 | wl *= w * l; 97 | 98 | // Update the gamma function 99 | gamma[k] = k * gamma[k - 1] - p_miss * wl; 100 | } 101 | 102 | // Precompute powers of w 103 | w_to_the[0] = 1; 104 | for (unsigned int k = 1; k < dimension; k++) { 105 | w_to_the[k] = w_to_the[k - 1] * w; 106 | } 107 | 108 | // precompute 109 | l_to_the_neg[0] = 1; 110 | for (unsigned int k = 1; k < dimension; k++) { 111 | if (l > 0) { 112 | l_to_the_neg[k] = l_to_the_neg[k - 1]/l; 113 | } else { 114 | // Watch out for division by zero 115 | // When l = 0 this is zeroed out by gammas 116 | l_to_the_neg[k] = 1; 117 | } 118 | } 119 | 120 | // Update the expected values 121 | for (int k = dimension - 1; k >= 0; k--) { 122 | 123 | double miss_distk_info = 0; 124 | double miss_distk = 0; 125 | for (int i = 0; i <= k; i++) { 126 | unsigned int binom = 127 | factorial[k]/(factorial[i] * factorial[k - i]); 128 | 129 | if (lower_bound) { 130 | miss_distk_info += 131 | pnm * binom * w_to_the[k - i] * (distk_info[i] + w * l * distk[i]); 132 | } else { 133 | miss_distk_info += 134 | binom * w_to_the[k - i] * (distk_info[i] + pnm * w * l * distk[i]); 135 | } 136 | 137 | miss_distk += 138 | binom * w_to_the[k - i] * distk[i]; 139 | } 140 | distk_info[k] = p_miss * miss_distk_info; 141 | distk_info[k] += 142 | pnm * l_to_the_neg[k] * (gamma[k + 1] + neg_log_l * gamma[k]); 143 | 144 | distk[k] = p_miss * miss_distk; 145 | distk[k] += l_to_the_neg[k] * gamma[k]; 146 | 147 | if (lower_bound) { 148 | distk_info[k] += (1 - pnm) * distk[k] * (1 + neg_log_noise_l); 149 | } else { 150 | distk_info[k] += (1 - pnm) * l_to_the_neg[k] * gamma[k] * (1 + neg_log_noise_l); 151 | } 152 | } 153 | 154 | // MI += E[R^(n-1)I(R)]dtheta 155 | output[map_index] += distk_info[dimension - 1] * dtheta; 156 | // MI -= (1 - log Lambda) E[R^(n-1)]dtheta 157 | output[map_index] -= (1 + neg_log_noise_l) * distk[dimension - 1] * dtheta; 158 | } 159 | } 160 | 161 | }} 162 | -------------------------------------------------------------------------------- /test/numerical_mi_distorted.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This test file computes the mutual information 3 | * using the provided algorithm as well as numerical 4 | * integration and presents the differences between the 5 | * two results. The range measurement is assumed to be 6 | * distorted with Gaussian noise. 7 | * 8 | * The results are presented for dimensions 1 through 3. 9 | */ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #include "helpers.hpp" 20 | 21 | // Define constants 22 | double integration_step = 0.0005; 23 | double dtheta = 0.1; 24 | unsigned int num_cells = 100; 25 | double vacancy_scaling = 0.1; 26 | double noise_dev = 4; 27 | double noise_half_width = noise_dev * 4; 28 | double noise_integration_step = 0.0005; 29 | unsigned int num_dimensions = 3; 30 | 31 | double numerical_mi_distorted( 32 | const double * const pdf, 33 | const double * const pdf_distorted, 34 | unsigned pdf_size, 35 | unsigned pdf_distorted_size, 36 | unsigned int dimension, 37 | double dtheta, 38 | double noise_dev, 39 | double integration_step) { 40 | 41 | double h_Z = 0; 42 | for (unsigned int i = 0; i < pdf_distorted_size; i++) { 43 | double r = i * integration_step - noise_half_width; 44 | 45 | h_Z += 46 | pdf_distorted[i] * 47 | std::pow(r, dimension - 1) * 48 | -std::log(pdf_distorted[i]) * 49 | integration_step * 50 | dtheta; 51 | } 52 | 53 | double h_Z_given_M = 0; 54 | for (unsigned int i = 0; i < pdf_size; i++) { 55 | double r = i * integration_step; 56 | 57 | h_Z_given_M += 58 | std::log(noise_dev * std::sqrt(2 * M_PI * M_E)) * 59 | pdf[i] * 60 | std::pow(r, dimension - 1) * 61 | integration_step * 62 | dtheta; 63 | } 64 | 65 | if (dimension == 3) { 66 | h_Z_given_M += 67 | noise_dev * noise_dev * 68 | std::log(noise_dev * std::sqrt(2 * M_PI * M_E * M_E * M_E)) 69 | * dtheta; 70 | } 71 | 72 | return h_Z - h_Z_given_M; 73 | } 74 | 75 | int main() { 76 | // Initialize the inputs and outputs 77 | std::vector vacancy(num_cells); 78 | std::vector width(num_cells); 79 | // Randomize then 80 | random_p(vacancy); 81 | random_p(width); 82 | 83 | // Scale vacancy to make everything generally lower 84 | for (unsigned int i = 0; i < num_cells; i++) { 85 | vacancy[i] = std::pow(vacancy[i], vacancy_scaling); 86 | } 87 | 88 | // Make a line 89 | std::vector line(num_cells); 90 | std::iota(line.begin(), line.end(), 0); 91 | 92 | std::cout << "Computing the pdf" << std::endl; 93 | 94 | // Compute the pdf 95 | unsigned int pdf_size; 96 | std::vector pdf(num_cells/integration_step); 97 | numerical_pdf( 98 | line.data(), 99 | vacancy.data(), 100 | width.data(), 101 | num_cells, 102 | integration_step, 103 | pdf.data(), 104 | pdf_size); 105 | 106 | std::cout << "Convolving pdf with noise" << std::endl; 107 | 108 | unsigned int noise_size = (2 * noise_half_width)/integration_step; 109 | unsigned int pdf_distorted_size = pdf_size + noise_size - 1; 110 | std::vector pdf_distorted(pdf_distorted_size, 0); 111 | for (unsigned int i = 0; i < pdf_size; i++) { 112 | for (unsigned int j = 0; j < noise_size; j++) { 113 | double r = j * integration_step - noise_half_width; 114 | pdf_distorted[i + j] += 115 | pdf[i] * 116 | range_mi::distorted::normal_pdf(r, 0, noise_dev) * 117 | integration_step; 118 | } 119 | } 120 | 121 | std::cout << "Computing the distorted mutual information numerically..." << std::endl; 122 | 123 | std::vector numerical_mi_(num_dimensions); 124 | for (unsigned int i = 0; i < num_dimensions; i++) { 125 | numerical_mi_[i] = numerical_mi_distorted( 126 | pdf.data(), 127 | pdf_distorted.data(), 128 | pdf_size, 129 | pdf_distorted_size, 130 | i + 1, 131 | dtheta, 132 | noise_dev, 133 | integration_step); 134 | } 135 | 136 | std::cout << "Computing the distorted mutual information exactly..." << std::endl; 137 | 138 | std::vector exact_mi(num_dimensions, 0); 139 | std::vector mi(num_cells); 140 | std::vector pdf_((num_cells + 2 * noise_half_width)/noise_integration_step); 141 | 142 | // Clear the output 143 | std::fill(mi.begin(), mi.end(), 0); 144 | range_mi::distorted::line<1>( 145 | line.data(), vacancy.data(), width.data(), 146 | num_cells, noise_dev, noise_half_width, noise_integration_step, 147 | dtheta, pdf_.data(), mi.data()); 148 | exact_mi[0] = mi[0]; 149 | std::fill(mi.begin(), mi.end(), 0); 150 | range_mi::distorted::line<2>( 151 | line.data(), vacancy.data(), width.data(), 152 | num_cells, noise_dev, noise_half_width, noise_integration_step, 153 | dtheta, pdf_.data(), mi.data()); 154 | exact_mi[1] = mi[0]; 155 | std::fill(mi.begin(), mi.end(), 0); 156 | range_mi::distorted::line<3>( 157 | line.data(), vacancy.data(), width.data(), 158 | num_cells, noise_dev, noise_half_width, noise_integration_step, 159 | dtheta, pdf_.data(), mi.data()); 160 | exact_mi[2] = mi[0]; 161 | 162 | std::cout << std::endl; 163 | for (unsigned int i = 0; i < num_dimensions; i++) { 164 | std::cout << "d" << i + 1 << ": " << numerical_mi_[i] << ", " << exact_mi[i] << 165 | ", difference: " << std::abs(numerical_mi_[i] - exact_mi[i]) << 166 | " or " << 100 * std::abs(numerical_mi_[i] - exact_mi[i])/exact_mi[i] << "%" << 167 | std::endl; 168 | } 169 | } 170 | -------------------------------------------------------------------------------- /node/occupancy_grid_mi.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | class OccupancyGridMI { 12 | 13 | public: 14 | OccupancyGridMI() { 15 | // Initialize the node handle 16 | n = ros::NodeHandle("~"); 17 | 18 | // Fetch the ROS parameters 19 | std::string map_topic, mi_topic; 20 | n.getParam("map_topic", map_topic); 21 | n.getParam("mi_topic", mi_topic); 22 | // Ray tracing parameters 23 | n.getParam("num_beams", num_beams); 24 | n.getParam("condition_steps", condition_steps); 25 | // Noise parameters 26 | n.getParam("noise_dev", noise_dev); 27 | n.getParam("noise_truncation", noise_truncation); 28 | n.getParam("noise_integration_step", noise_integration_step); 29 | // Visualization vvv 30 | std::string click_condition_topic, mi_map_topic, conditional_map_topic; 31 | n.getParam("visualize", visualize); 32 | n.getParam("visualize_more", visualize_more); 33 | n.getParam("click_condition_topic", click_condition_topic); 34 | n.getParam("mi_map_topic", mi_map_topic); 35 | n.getParam("conditional_map_topic", conditional_map_topic); 36 | 37 | // Construct a publisher for mutual information 38 | mi_pub = n.advertise(mi_topic, 1, true); 39 | mi_map_pub = n.advertise(mi_map_topic, 1, true); 40 | conditional_map_pub = n.advertise(conditional_map_topic, 1, true); 41 | 42 | // Subscribe to maps and clicked points 43 | map_sub = n.subscribe(map_topic, 1, &OccupancyGridMI::map_callback, this); 44 | click_sub = n.subscribe(click_condition_topic, 1, &OccupancyGridMI::click_callback, this); 45 | } 46 | 47 | void map_callback(const nav_msgs::OccupancyGrid & map_msg) { 48 | // Store map information 49 | map_info = map_msg.info; 50 | map_header = map_msg.header; 51 | 52 | // Convert to probability 53 | vacancy = std::vector(map_info.height * map_info.width); 54 | for (unsigned int i = 0; i < vacancy.size(); i++) { 55 | vacancy[i] = 1 - map_msg.data[i]/99.; 56 | if (vacancy[i] < 0 or vacancy[i] > 1) { 57 | std::cout << "Vacancy out of bounds! " << vacancy[i] << std::endl; 58 | vacancy[i] = 0; 59 | } 60 | 61 | vacancy[i] = std::pow(vacancy[i], map_info.resolution); 62 | //if (vacancy[i] > 0 and vacancy[i] < 1) { 63 | //vacancy[i] = 0.99; 64 | //} 65 | } 66 | 67 | // Initialize mutual information computation on the grid 68 | mi_computer = range_mi::GridMI( 69 | map_info.height, 70 | map_info.width, 71 | noise_dev, 72 | noise_dev * noise_truncation, 73 | noise_integration_step); 74 | 75 | compute_mi(); 76 | } 77 | 78 | void compute_mi() { 79 | mi_computer.reset_mi(); 80 | 81 | auto start = std::chrono::high_resolution_clock::now(); 82 | 83 | double spatial_interpolation = 0; 84 | double theta = 0; 85 | double dtheta = (2 * M_PI)/num_beams; 86 | while (theta < 2 * M_PI) { 87 | // Add the beam 88 | mi_computer.compute_mi_beam( 89 | vacancy.data(), 90 | theta, 91 | dtheta, 92 | spatial_interpolation); 93 | 94 | if (spatial_interpolation == 0) { 95 | // Move to the next beam 96 | theta += dtheta; 97 | 98 | // Draw every time a spatial section is completed 99 | if (visualize and visualize_more) { 100 | draw_map(); 101 | } 102 | 103 | if (theta < 2 * M_PI) { 104 | //mi_computer.reset_mi(); 105 | } 106 | if (not ros::ok()) break; 107 | } 108 | } 109 | 110 | auto end = std::chrono::high_resolution_clock::now(); 111 | std::chrono::duration elapsed = end - start; 112 | std::cout << "Ending MI computation" << std::endl; 113 | std::cout << "Computing mutual information of a " << 114 | map_info.height << "x" << map_info.width << " map with " << 115 | num_beams << " beams took " << elapsed.count() << "seconds." << std::endl; 116 | 117 | if (visualize) draw_map(); 118 | publish_mi(); 119 | } 120 | 121 | void click_callback(const geometry_msgs::PointStamped & click_msg) { 122 | // Condition the map on the clicked point 123 | double x = click_msg.point.x/map_info.resolution; 124 | double y = click_msg.point.y/map_info.resolution; 125 | double dtheta = (2 * M_PI)/condition_steps; 126 | mi_computer.condition( 127 | vacancy.data(), 128 | x, y, 129 | 0, 2 * M_PI, 130 | dtheta); 131 | 132 | // Update the mutual information 133 | compute_mi(); 134 | } 135 | 136 | void publish_mi() { 137 | // Construct a message for the mutual information 138 | range_mi::MIGrid mi_msg; 139 | mi_msg.header = map_header; 140 | mi_msg.data = mi_computer.mi(); 141 | mi_msg.height = map_info.height; 142 | mi_msg.width = map_info.width; 143 | 144 | // Publish 145 | mi_pub.publish(mi_msg); 146 | } 147 | 148 | void draw_map() { 149 | // Convert to an occupancy map for visualization 150 | nav_msgs::OccupancyGrid mi_map_msg; 151 | mi_map_msg.header = map_header; 152 | mi_map_msg.info = map_info; 153 | mi_map_msg.data = std::vector(mi_computer.mi().size()); 154 | double mi_max = *std::max_element(mi_computer.mi().begin(), mi_computer.mi().end()); 155 | for (size_t i = 0; i < mi_computer.mi().size(); i++) { 156 | // Normalize between 0 and 1 157 | double normalized = mi_computer.mi()[i]/mi_max; 158 | // Change to 100% 159 | mi_map_msg.data[i] = 100 * (1 - normalized); 160 | } 161 | mi_map_pub.publish(mi_map_msg); 162 | 163 | // Do the same with p not measured 164 | nav_msgs::OccupancyGrid conditional_map_msg = mi_map_msg; 165 | for (size_t i = 0; i < conditional_map_msg.data.size(); i++) 166 | conditional_map_msg.data[i] = 100 * (1 - mi_computer.p_not_measured()[i]); 167 | conditional_map_pub.publish(conditional_map_msg); 168 | } 169 | 170 | private: 171 | ros::NodeHandle n; 172 | ros::Subscriber map_sub, click_sub; 173 | ros::Publisher mi_pub, 174 | mi_map_pub, 175 | conditional_map_pub; 176 | 177 | // Parameters 178 | int num_beams; 179 | int condition_steps; 180 | bool visualize, visualize_more; 181 | 182 | // Noise parameters 183 | double noise_dev; 184 | double noise_truncation; 185 | double noise_integration_step; 186 | 187 | // Map data 188 | nav_msgs::MapMetaData map_info; 189 | std_msgs::Header map_header; 190 | std::vector vacancy; 191 | 192 | // Computation devices 193 | range_mi::GridMI mi_computer; 194 | }; 195 | 196 | int main(int argc, char ** argv) { 197 | ros::init(argc, argv, "occupancy_grid_mi"); 198 | OccupancyGridMI ogmi; 199 | ros::spin(); 200 | return 0; 201 | } 202 | -------------------------------------------------------------------------------- /include/range_mi/distorted.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace range_mi { 7 | namespace distorted { 8 | 9 | double normal_pdf( 10 | double x, 11 | double mean, 12 | double std_dev) { 13 | double x_norm = (x - mean)/std_dev; 14 | return std::exp(-0.5 * x_norm * x_norm)/(std_dev * std::sqrt(2 * M_PI)); 15 | } 16 | 17 | void range_pdf( 18 | const unsigned int * const line, 19 | const double * const vacancy, 20 | const double * const width, 21 | unsigned int num_cells, 22 | double noise_dev, 23 | double noise_half_width, 24 | double integration_step, 25 | double pdf_width, 26 | double * const pdf, 27 | unsigned int & pdf_size) { 28 | 29 | double pdf_decay = 1; 30 | double width_sum = 0; 31 | pdf_size = 0; 32 | 33 | // Iterate over the cells 34 | for (unsigned int line_index = 0; line_index < num_cells; line_index++) { 35 | // If the width is too large the cells are of no use 36 | if (width_sum > pdf_width + 2 * noise_half_width) break; 37 | 38 | unsigned int map_index = line[line_index]; 39 | double v = vacancy[map_index]; 40 | double w = width[line_index]; 41 | 42 | // Vacancy of one contributes nothing to the pdf 43 | if (v >= 1) continue; 44 | 45 | double p_miss, mass, center_of_mass; 46 | if (v <= 0) { 47 | p_miss = 0; 48 | mass = 1; 49 | center_of_mass = 0; 50 | } else { 51 | // Compute the negative log of the vacancy 52 | double l = -std::log(v); 53 | 54 | // Pre-compute the probability of missing 55 | // the constant region 56 | p_miss = std::exp(-l * w); 57 | 58 | // The probability mass of a hit 59 | mass = 1 - p_miss; 60 | 61 | // The center of mass 62 | // (1 - exp(-l*w))/2 = 1 - exp(-l*com) 63 | center_of_mass = -std::log(0.5 * (1 + p_miss))/l; 64 | // Note that l > 0 because v < 1 65 | } 66 | 67 | double z = -noise_half_width; 68 | unsigned int z_index = width_sum/integration_step; 69 | while (z < w + noise_half_width and z + width_sum < pdf_width + noise_half_width) { 70 | 71 | // The value of the convolution is the normal 72 | // centered at the center of mass, multiplied 73 | double value = pdf_decay * mass * normal_pdf(z, center_of_mass, noise_dev); 74 | 75 | // If this is the first time the cell 76 | // has been touched, clear it 77 | if (z_index == pdf_size) { 78 | pdf[z_index] = 0; 79 | pdf_size++; 80 | } 81 | 82 | // Accumulate 83 | pdf[z_index] += value; 84 | 85 | z += integration_step; 86 | z_index++; 87 | } 88 | 89 | // Update width and decay 90 | width_sum += w; 91 | pdf_decay *= p_miss; 92 | } 93 | } 94 | 95 | template 96 | void line( 97 | const unsigned int * const line, 98 | const double * const vacancy, 99 | const double * const width, 100 | unsigned int num_cells, 101 | double noise_dev, 102 | double noise_half_width, 103 | double integration_step, 104 | double dtheta, 105 | double * const pdf, 106 | double * const output) { 107 | 108 | // Pre-compute factorials 109 | std::array factorial; 110 | factorial[0] = 1; 111 | for (unsigned int k = 1; k <= dimension; k++) { 112 | factorial[k] = factorial[k - 1] * k; 113 | } 114 | 115 | // Pre-compute E[N^kI(N)] 116 | std::array distk_info_noise; 117 | switch(dimension) { 118 | case 3: 119 | distk_info_noise[2] = 120 | noise_dev * noise_dev * 121 | std::log(noise_dev * std::sqrt(2 * M_PI * std::pow(M_E,3))); 122 | [[fallthrough]]; 123 | case 2: 124 | distk_info_noise[1] = 0; 125 | [[fallthrough]]; 126 | case 1: 127 | distk_info_noise[0] = std::log(noise_dev * std::sqrt(2 * M_PI * M_E)); 128 | } 129 | 130 | // Initialize the expected values 131 | // E[Z^kI(Z^k)] to E[N^kI(N^k)] 132 | std::array distk_info; 133 | for (unsigned int k = 0; k < dimension; k++) { 134 | distk_info[k] = distk_info_noise[k]; 135 | } 136 | 137 | // Initialize the expected values E[R^k] 138 | std::array distk; 139 | distk[0] = 1; 140 | for (unsigned int k = 1; k < dimension; k++) { 141 | distk[k] = 0; 142 | } 143 | 144 | // Also initialize a space for gamma values 145 | std::array gamma; 146 | // and powers of w 147 | std::array w_to_the; 148 | // And negative powers of l 149 | std::array l_to_the_neg; 150 | 151 | // Iterate backwards over the cells in the line 152 | for (int line_index = num_cells - 1; line_index >= 0; line_index--) { 153 | 154 | // Fetch the vacancy 155 | // and width of the current cell 156 | unsigned int map_index = line[line_index]; 157 | double v = vacancy[map_index]; 158 | double w = width[line_index]; 159 | 160 | // Compute the negative log of the vacancy 161 | double l; 162 | if (v <= 0) { 163 | l = 9999999999999999; 164 | } else { 165 | l = -std::log(v); 166 | } 167 | 168 | // Precompute the probability of missing 169 | // the constant region 170 | double p_miss = std::exp(-l * w); 171 | 172 | // Precompute the gamma values 173 | gamma[0] = 1 - p_miss; 174 | double wl = 1; 175 | for (unsigned int k = 1; k <= dimension; k++) { 176 | // Update (wl)^k 177 | wl *= w * l; 178 | 179 | // Update the gamma function 180 | gamma[k] = k * gamma[k - 1] - p_miss * wl; 181 | } 182 | 183 | // Precompute powers of w 184 | w_to_the[0] = 1; 185 | for (unsigned int k = 1; k < dimension; k++) { 186 | w_to_the[k] = w_to_the[k - 1] * w; 187 | } 188 | 189 | // precompute 190 | l_to_the_neg[0] = 1; 191 | for (unsigned int k = 1; k < dimension; k++) { 192 | if (l > 0) { 193 | l_to_the_neg[k] = l_to_the_neg[k - 1]/l; 194 | } else { 195 | // Watch out for division by zero 196 | // When l = 0 this is zeroed out by gammas 197 | l_to_the_neg[k] = 1; 198 | } 199 | } 200 | 201 | // Precompute the density functions 202 | unsigned int hit_pdf_size; 203 | range_pdf( 204 | line + line_index, 205 | vacancy, 206 | width + line_index, 207 | num_cells - line_index, 208 | noise_dev, 209 | noise_half_width, 210 | integration_step, 211 | w, 212 | pdf, 213 | hit_pdf_size); 214 | unsigned int miss_pdf_size; 215 | range_pdf( 216 | line + line_index + 1, 217 | vacancy, 218 | width + line_index + 1, 219 | num_cells - line_index - 1, 220 | noise_dev, 221 | noise_half_width, 222 | integration_step, 223 | 0, 224 | pdf + hit_pdf_size, 225 | miss_pdf_size); 226 | 227 | // Update the expected values 228 | for (int k = dimension - 1; k >= 0; k--) { 229 | 230 | double miss_distk_info = 0; 231 | double miss_distk = 0; 232 | for (int i = 0; i <= k; i++) { 233 | unsigned int binom = 234 | factorial[k]/(factorial[i] * factorial[k - i]); 235 | miss_distk_info += 236 | binom * w_to_the[k - i] * (distk_info[i] + w * l * distk[i]); 237 | if (i == 2) { 238 | miss_distk_info += 239 | binom * w_to_the[k - i] * w * l * noise_dev * noise_dev; 240 | } 241 | miss_distk += 242 | binom * w_to_the[k - i] * distk[i]; 243 | } 244 | distk_info[k] = p_miss * miss_distk_info; 245 | 246 | // Perform the numerical integration 247 | for (unsigned int i = 0; i < hit_pdf_size; i++) { 248 | double r = integration_step * i - noise_half_width; 249 | if (pdf[i] > 0) { 250 | distk_info[k] -= 251 | pdf[i] * std::pow(r, k) * 252 | std::log(pdf[i]) * integration_step; 253 | } 254 | } 255 | for (unsigned int i = 0; i < miss_pdf_size; i++) { 256 | double r = integration_step * i - noise_half_width; 257 | if (pdf[hit_pdf_size + i] > 0) { 258 | distk_info[k] += 259 | p_miss * pdf[hit_pdf_size + i] * std::pow(r + w, k) * 260 | (std::log(pdf[hit_pdf_size + i]) - l * w) * integration_step; 261 | } 262 | } 263 | 264 | distk[k] = p_miss * miss_distk; 265 | distk[k] += l_to_the_neg[k] * gamma[k]; 266 | } 267 | 268 | // MI += E[R^(n-1)I(R)]dtheta 269 | double mi = distk_info[dimension - 1] * dtheta; 270 | // MI -= E[I(N)]E[R^(n-1)]dtheta 271 | mi -= distk_info_noise[0] * distk[dimension - 1] * dtheta; 272 | if (dimension == 3) { 273 | // MI -= E[N^2I(N)]dtheta 274 | mi -= distk_info_noise[2] * dtheta; 275 | } 276 | if (mi > 0) output[map_index] += mi; 277 | } 278 | } 279 | 280 | }} 281 | -------------------------------------------------------------------------------- /node/monte_carlo_mi.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | unsigned int num_iterations = 999; 11 | 12 | class MonteCarloMI { 13 | 14 | public: 15 | MonteCarloMI() { 16 | // Initialize the uniform distribution 17 | gen = std::mt19937(random_device()); 18 | dist = std::uniform_real_distribution(0, 1); 19 | 20 | // Initialize the node handle 21 | n = ros::NodeHandle("~"); 22 | 23 | // Fetch the ROS parameters 24 | std::string map_topic, mi_topic; 25 | n.getParam("map_topic", map_topic); 26 | n.getParam("mi_topic", mi_topic); 27 | // Ray tracing parameters 28 | n.getParam("num_beams", num_beams); 29 | // Visualization vvv 30 | std::string mi_map_topic, 31 | binary_map_topic, conditional_map_topic; 32 | n.getParam("visualize", visualize); 33 | n.getParam("visualize_more", visualize_more); 34 | n.getParam("mi_map_topic", mi_map_topic); 35 | n.getParam("binary_map_topic", binary_map_topic); 36 | n.getParam("conditional_map_topic", conditional_map_topic); 37 | 38 | // Construct a publisher for mutual information 39 | mi_pub = n.advertise(mi_topic, 1, true); 40 | mi_map_pub = n.advertise(mi_map_topic, 1, true); 41 | binary_map_pub = n.advertise(binary_map_topic, 1, true); 42 | conditional_map_pub = n.advertise(conditional_map_topic, 1, true); 43 | 44 | // Subscribe to the map 45 | map_sub = n.subscribe(map_topic, 1, &MonteCarloMI::map_callback, this); 46 | } 47 | 48 | void map_callback(const nav_msgs::OccupancyGrid & map_msg) { 49 | // Store map information 50 | map_info = map_msg.info; 51 | map_header = map_msg.header; 52 | 53 | // Convert to probability 54 | vacancy = std::vector(map_info.height * map_info.width); 55 | for (unsigned int i = 0; i < vacancy.size(); i++) { 56 | vacancy[i] = 1 - map_msg.data[i]/99.; 57 | if (vacancy[i] < 0 or vacancy[i] > 1) { 58 | vacancy[i] = 0; 59 | } 60 | 61 | vacancy[i] = std::pow(vacancy[i], map_info.resolution); 62 | } 63 | 64 | // Also initialize a place for randomized and conditional vacancies 65 | binary_vacancy = std::vector(map_info.height * map_info.width); 66 | conditional_vacancy = std::vector(map_info.height * map_info.width); 67 | cconditional_vacancy = std::vector(map_info.height * map_info.width); 68 | conditional_vacancy_viz = std::vector(map_info.height * map_info.width); 69 | 70 | // And a place for drawing lines 71 | line = std::vector(2 * std::max(map_info.height, map_info.width)); 72 | widths = std::vector(2 * std::max(map_info.height, map_info.width)); 73 | 74 | // And the mutual information map 75 | mi = std::vector(map_info.height * map_info.width, 0); 76 | 77 | compute_mi(); 78 | } 79 | 80 | static double entropy(const std::vector & v) { 81 | double e = 0; 82 | for (unsigned int i = 0; i < v.size(); i++) { 83 | if (v[i] > 0 and v[i] < 1) { 84 | // Accumulate the binary entropy 85 | e -= 86 | v[i] * std::log(v[i]) + 87 | (1 - v[i]) * std::log(1 - v[i]); 88 | } 89 | } 90 | 91 | return e; 92 | } 93 | 94 | void compute_mi() { 95 | // Clear the mutual information 96 | std::fill(mi.begin(), mi.end(), 0); 97 | 98 | // Compute the entropy of the map 99 | double map_entropy = entropy(vacancy); 100 | 101 | for (unsigned int it = 0; it < num_iterations and ros::ok(); it++) { 102 | // Randomly assign binary vacancy values based 103 | // on the vacancy probabilities 104 | for (unsigned int cell = 0; cell < vacancy.size(); cell++) { 105 | double random = dist(gen); 106 | if (random < vacancy[cell]) { 107 | binary_vacancy[cell] = 1; 108 | } else { 109 | binary_vacancy[cell] = 0; 110 | } 111 | } 112 | 113 | // Initialize the map to equal the original vacancies 114 | for (unsigned int i = 0; i < vacancy.size(); i++) { 115 | cconditional_vacancy[i] = vacancy[i]; 116 | } 117 | 118 | // If we are conditioning 119 | if (false) { 120 | // Compute a scan at the conditioned point 121 | for (int beam = 0; beam < num_beams; beam++) { 122 | double theta = beam * (2 * M_PI)/(num_beams); 123 | range_mi::grid_line::draw( 124 | map_info.height, 125 | map_info.width, 126 | 73.5, 44.5, theta, 127 | line.data(), 128 | widths.data(), 129 | num_cells); 130 | 131 | // Iterate over the beam 132 | for (unsigned int i = 0; i < num_cells; i++) { 133 | unsigned int line_cell = line[i]; 134 | 135 | cconditional_vacancy[line_cell] = binary_vacancy[line_cell]; 136 | 137 | // Stop if we have reached an occupied cell 138 | if (binary_vacancy[line_cell] == 0) break; 139 | } 140 | } 141 | 142 | // Compute the entropy of the map 143 | map_entropy = entropy(cconditional_vacancy); 144 | } 145 | 146 | // Given the randomized "ground truth" values compute 147 | // the result of a scan in the map and how the entropy changed. 148 | for (unsigned int cell = 0; cell < vacancy.size(); cell++) { 149 | // Initialize the map to equal the conditioned vacancies 150 | for (unsigned int i = 0; i < vacancy.size(); i++) { 151 | conditional_vacancy[i] = cconditional_vacancy[i]; 152 | } 153 | 154 | // For each beam cast rays and update the map 155 | for (int beam = 0; beam < num_beams; beam++) { 156 | double theta = beam * (2 * M_PI)/(num_beams); 157 | 158 | double y = std::floor(cell/map_info.width); 159 | double x = cell - y * map_info.width; 160 | y += 0.5; 161 | x += 0.5; 162 | 163 | // Draw a beam 164 | range_mi::grid_line::draw( 165 | map_info.height, 166 | map_info.width, 167 | x, y, theta, 168 | line.data(), 169 | widths.data(), 170 | num_cells); 171 | 172 | // Iterate over the beam 173 | for (unsigned int i = 0; i < num_cells; i++) { 174 | unsigned int line_cell = line[i]; 175 | 176 | conditional_vacancy[line_cell] = binary_vacancy[line_cell]; 177 | 178 | // Stop if we have reached an occupied cell 179 | if (binary_vacancy[line_cell] == 0) break; 180 | } 181 | } 182 | 183 | // Choose the center cell for visualization purposes 184 | if (cell == (map_info.height * map_info.width)/2 + map_info.width/2) { 185 | for (unsigned int i = 0; i < vacancy.size(); i++) { 186 | conditional_vacancy_viz[i] = conditional_vacancy[i]; 187 | } 188 | conditional_vacancy_viz[cell] = 0; 189 | } 190 | 191 | // Compute the entropy of the new map 192 | double conditional_map_entropy = entropy(conditional_vacancy); 193 | 194 | // Accumulate 195 | mi[cell] += (map_entropy - conditional_map_entropy); 196 | } 197 | 198 | // Visualize 199 | if (visualize and visualize_more) draw_map(); 200 | 201 | // Plot 202 | publish_mi(); 203 | } 204 | } 205 | 206 | void publish_mi() { 207 | // Construct a message for the mutual information 208 | range_mi::MIGrid mi_msg; 209 | mi_msg.header = map_header; 210 | mi_msg.data = mi; 211 | mi_msg.height = map_info.height; 212 | mi_msg.width = map_info.width; 213 | 214 | // Publish 215 | mi_pub.publish(mi_msg); 216 | } 217 | 218 | void draw_map() { 219 | // Convert to an occupancy map for visualization 220 | nav_msgs::OccupancyGrid mi_map_msg; 221 | mi_map_msg.header = map_header; 222 | mi_map_msg.info = map_info; 223 | mi_map_msg.data = std::vector(mi.size()); 224 | double mi_max = *std::max_element(mi.begin(), mi.end()); 225 | for (size_t i = 0; i < mi.size(); i++) { 226 | // Normalize between 0 and 1 227 | double normalized = mi[i]/mi_max; 228 | // Change to 100% 229 | mi_map_msg.data[i] = 100 * (1 - normalized); 230 | } 231 | mi_map_pub.publish(mi_map_msg); 232 | 233 | // Publish the binary map 234 | nav_msgs::OccupancyGrid binary_map_msg = mi_map_msg; 235 | for (size_t i = 0; i < mi.size(); i++) { 236 | binary_map_msg.data[i] = 100 * (1 - binary_vacancy[i]); 237 | } 238 | binary_map_pub.publish(binary_map_msg); 239 | 240 | // Publish the conditioned map 241 | nav_msgs::OccupancyGrid conditional_map_msg = mi_map_msg; 242 | for (size_t i = 0; i < mi.size(); i++) { 243 | conditional_map_msg.data[i] = 100 * (1 - conditional_vacancy_viz[i]); 244 | } 245 | conditional_map_pub.publish(conditional_map_msg); 246 | } 247 | 248 | private: 249 | ros::NodeHandle n; 250 | ros::Subscriber map_sub; 251 | ros::Publisher mi_pub; 252 | ros::Publisher 253 | mi_map_pub, 254 | binary_map_pub, 255 | conditional_map_pub; 256 | 257 | // Random number generator 258 | std::random_device random_device; 259 | std::mt19937 gen; 260 | std::uniform_real_distribution dist; 261 | 262 | // Parameters 263 | int num_beams; 264 | bool visualize, visualize_more; 265 | 266 | // Map data 267 | nav_msgs::MapMetaData map_info; 268 | std_msgs::Header map_header; 269 | std::vector mi; 270 | std::vector vacancy, binary_vacancy, conditional_vacancy, cconditional_vacancy, conditional_vacancy_viz; 271 | 272 | // Line data 273 | std::vector line; 274 | std::vector widths; 275 | unsigned int num_cells; 276 | }; 277 | 278 | int main(int argc, char ** argv) { 279 | ros::init(argc, argv, "monte_carlo_mi"); 280 | MonteCarloMI mc_mi; 281 | ros::spin(); 282 | return 0; 283 | } 284 | --------------------------------------------------------------------------------