├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── cfg └── AMCL.cfg ├── examples ├── amcl_diff.launch └── amcl_omni.launch ├── include └── amcl │ ├── map │ └── map.h │ ├── pf │ ├── eig3.h │ ├── pf.h │ ├── pf_kdtree.h │ ├── pf_pdf.h │ └── pf_vector.h │ └── sensors │ ├── amcl_laser.h │ ├── amcl_odom.h │ ├── amcl_sensor.h │ └── compute_sample_weight.cuh ├── package.xml ├── src ├── amcl │ ├── map │ │ ├── map.c │ │ ├── map_cspace.cpp │ │ ├── map_draw.c │ │ ├── map_range.c │ │ └── map_store.c │ ├── pf │ │ ├── eig3.c │ │ ├── pf.cpp │ │ ├── pf_draw.c │ │ ├── pf_kdtree.c │ │ ├── pf_pdf.c │ │ └── pf_vector.c │ └── sensors │ │ ├── amcl_laser.cpp │ │ ├── amcl_odom.cpp │ │ ├── amcl_sensor.cpp │ │ └── compute_sample_weight.cu ├── amcl_node.cpp └── include │ └── portable_utils.hpp └── test ├── basic_localization.py ├── basic_localization_stage.xml ├── global_localization_stage.xml ├── rosie_multilaser.xml ├── set_initial_pose.xml ├── set_initial_pose_delayed.xml ├── set_pose.py ├── small_loop_crazy_driving_prg.xml ├── small_loop_crazy_driving_prg_corrected.xml ├── small_loop_prf.xml ├── texas_greenroom_loop.xml ├── texas_greenroom_loop_corrected.xml ├── texas_willow_hallway_loop.xml └── texas_willow_hallway_loop_corrected.xml /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package amcl 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.16.2 (2018-07-31) 6 | ------------------- 7 | * Merge pull request `#773 `_ from ros-planning/packaging_fixes 8 | packaging fixes 9 | * update amcl to have proper depends 10 | * add geometry_msgs 11 | * add tf2_msgs 12 | * fix alphabetical order 13 | * Contributors: Michael Ferguson 14 | 15 | 1.16.1 (2018-07-28) 16 | ------------------- 17 | * Merge pull request `#770 `_ from ros-planning/fix_debians 18 | Fix debian builds (closes `#769 `_) 19 | * make AMCL depend on sensor_msgs 20 | previously, amcl depended on TF, which depended on 21 | sensor_msgs. 22 | * Contributors: Michael Ferguson 23 | 24 | 1.16.0 (2018-07-25) 25 | ------------------- 26 | * Switch to TF2 `#755 `_ 27 | * Merge pull request `#734 `_ from ros-planning/melodic_731 28 | AMCL dynamic reconfigure: Extend parameter range (Forward port `#731 `_) 29 | * Merge pull request `#728 `_ from ros-planning/melodic_tf2_conversion 30 | switch AMCL to use TF2 31 | * fix swapped odom1/4 in omni model, fixes `#499 `_ 32 | * Merge pull request `#730 `_ from Glowcloud/melodic-devel 33 | Fix for Potential Memory Leak in AmclNode::reconfigureCB `#729 `_ 34 | * Fix for Potential Memory Leak in AmclNode::reconfigureCB 35 | * switch AMCL to use TF2 36 | * Merge pull request `#727 `_ from ros-planning/melodic_668 37 | Update laser_model_type enum on AMCL.cfg (Melodic port of `#668 `_) 38 | * Update laser_model_type enum on AMCL.cfg 39 | Adding likelihood_field_prob laser model option on AMCL.cfg to be able to control dynamic parameters with this laser sensor model. 40 | * Merge pull request `#723 `_ from moriarty/melodic-buildfarm-errors 41 | Melodic buildfarm errors 42 | * include for std::shared_ptr 43 | * Merge pull request `#718 `_ from moriarty/tf2-buffer-ptr 44 | [melodic] tf2_buffer\_ -> tf2_buffer_ptr\_ 45 | * [melodic] tf2_buffer\_ -> tf2_buffer_ptr\_ 46 | Change required due to changes in upstream dependencies: 47 | `ros/geometry#163 `_: "Maintain & expose tf2 Buffer in shared_ptr for tf" 48 | fixes `ros-planning/navigation#717 `_ (for compile errors at least.) 49 | * Contributors: Alexander Moriarty, Glowcloud, Martin Ganeff, Michael Ferguson, Miguel Cordero, Vincent Rabaud, maracuya-robotics 50 | 51 | 1.15.2 (2018-03-22) 52 | ------------------- 53 | * Fix minor typo (`#682 `_) 54 | This typo caused some confusion because we were searching for a semicolon in our configuration. 55 | * Merge pull request `#677 `_ from ros-planning/lunar_634 56 | removing recomputation of cluster stats causing assertion error (`#634 `_) 57 | * Merge pull request `#673 `_ from ros-planning/email_update_lunar 58 | update maintainer email (lunar) 59 | * Remove Dead Code [Lunar] (`#646 `_) 60 | * Clean up navfn 61 | * Cleanup amcl 62 | * Merge pull request `#649 `_ from aaronhoy/lunar_add_ahoy 63 | Add myself as a maintainer. 64 | * Contributors: Aaron Hoy, David V. Lu!!, Michael Ferguson, stevemacenski 65 | 66 | 1.15.1 (2017-08-14) 67 | ------------------- 68 | 69 | 1.15.0 (2017-08-07) 70 | ------------------- 71 | * Reference Issue `#592 `_ Added warning to AMCL when map is published on ... (`#604 `_) 72 | * rebase fixups 73 | * convert packages to format2 74 | * recompute cluster stat when force_publication 75 | * Fix CMakeLists + package.xmls (`#548 `_) 76 | * amcl: fix compilation with gcc v7 77 | * Added deps to amcl costmap_2d move_base (`#512 `_) 78 | * fix order of parameters (closes `#553 `_) 79 | * Fix potential string overflow and resource leak 80 | * Contributors: Dmitry Rozhkov, Laurent GEORGE, Martin Günther, Michael Ferguson, Mikael Arguedas, Peter Harliman Liem, mryellow, vik748 81 | 82 | 1.14.0 (2016-05-20) 83 | ------------------- 84 | * Allow AMCL to run from bag file to allow very fast testing. 85 | * Fixes interpretation of a delayed initialpose message (see `#424 `_). 86 | The tf lookup as it was before this change was very likely to fail as 87 | ros::Time::now() was used to look up a tf without waiting on the tf's 88 | availability. Additionally, the computation of the "new pose" by 89 | multiplying the delta that the robot moved from the initialpose's 90 | timestamp to ros::Time::now() was wrong. That delta has to by multiplied 91 | from the right to the "old pose". 92 | This commit also changes the reference frame to look up this delta to be 93 | the odom frame as this one is supposed to be smooth and therefore the 94 | best reference to get relative robot motion in the robot (base link) frame. 95 | * New unit test for proper interpretation of a delayed initialpose message. 96 | Modifies the set_pose.py script to be able to send an initial pose with 97 | a user defined time stamp at a user defined time. Adds a rostest to 98 | exercise this new option. 99 | This reveals the issues mentioned in `#424 `_ (the new test fails). 100 | * Contributors: Derek King, Stephan Wirth 101 | 102 | 1.13.1 (2015-10-29) 103 | ------------------- 104 | * adds the set_map service to amcl 105 | * fix pthread_mutex_lock on shutdown 106 | * Contributors: Michael Ferguson, Stephan Wirth 107 | 108 | 1.13.0 (2015-03-17) 109 | ------------------- 110 | * amcl_node will now save latest pose on shutdown 111 | * Contributors: Ian Danforth 112 | 113 | 1.12.0 (2015-02-04) 114 | ------------------- 115 | * update maintainer email 116 | * Contributors: Michael Ferguson 117 | 118 | 1.11.15 (2015-02-03) 119 | -------------------- 120 | 121 | 1.11.14 (2014-12-05) 122 | -------------------- 123 | 124 | 1.11.13 (2014-10-02) 125 | -------------------- 126 | 127 | 1.11.12 (2014-10-01) 128 | -------------------- 129 | * Bug fix to remove particle weights being reset when motion model is updated 130 | * Integrated new sensor model which calculates the observation likelihood in a probabilistic manner 131 | Also includes the option to do beam-skipping (to better handle observations from dynamic obstacles) 132 | * Pose pulled from parameter server when new map received 133 | * Contributors: Steven Kordell, hes3pal 134 | 135 | 1.11.11 (2014-07-23) 136 | -------------------- 137 | 138 | 1.11.10 (2014-06-25) 139 | -------------------- 140 | 141 | 1.11.9 (2014-06-10) 142 | ------------------- 143 | 144 | 1.11.8 (2014-05-21) 145 | ------------------- 146 | 147 | 1.11.7 (2014-05-21) 148 | ------------------- 149 | * removes useless this->z_max = z_max assignment 150 | * Fix warning string. 151 | * Contributors: Jeremiah Via, enriquefernandez 152 | 153 | 1.11.5 (2014-01-30) 154 | ------------------- 155 | * Fix for `#160 `_ 156 | * Download test data from download.ros.org instead of willow 157 | * Change maintainer from Hersh to Lu 158 | 159 | 1.11.4 (2013-09-27) 160 | ------------------- 161 | * Package URL Updates 162 | * amcl_pose and particle cloud are now published latched 163 | * Fixed or commented out failing amcl tests. 164 | 165 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | project(amcl) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 11) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | set(CMAKE_CXX_EXTENSIONS OFF) 8 | endif() 9 | 10 | ## add optimization flag 11 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANGCXX) 12 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") 13 | endif() 14 | 15 | if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) 16 | if (NOT CMAKE_BUILD_TYPE) 17 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) 18 | endif() 19 | endif() 20 | 21 | find_package(catkin REQUIRED 22 | COMPONENTS 23 | diagnostic_updater 24 | dynamic_reconfigure 25 | geometry_msgs 26 | message_filters 27 | nav_msgs 28 | rosbag 29 | roscpp 30 | sensor_msgs 31 | std_srvs 32 | tf2 33 | tf2_geometry_msgs 34 | tf2_msgs 35 | tf2_ros 36 | ) 37 | 38 | find_package(Boost REQUIRED) 39 | 40 | # CUDA PACKAGE 41 | find_package(CUDA REQUIRED) 42 | 43 | # dynamic reconfigure 44 | generate_dynamic_reconfigure_options( 45 | cfg/AMCL.cfg 46 | ) 47 | 48 | catkin_package( 49 | CATKIN_DEPENDS 50 | diagnostic_updater 51 | dynamic_reconfigure 52 | geometry_msgs 53 | nav_msgs 54 | rosbag 55 | roscpp 56 | sensor_msgs 57 | std_srvs 58 | tf2 59 | tf2_msgs 60 | tf2_ros 61 | INCLUDE_DIRS include 62 | LIBRARIES amcl_sensors amcl_map amcl_pf 63 | ) 64 | 65 | include_directories(include) 66 | include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 67 | include_directories(src/include) 68 | include(CheckSymbolExists) 69 | 70 | check_include_file(unistd.h HAVE_UNISTD_H) 71 | if (HAVE_UNISTD_H) 72 | add_definitions(-DHAVE_UNISTD_H) 73 | endif (HAVE_UNISTD_H) 74 | 75 | check_symbol_exists(drand48 stdlib.h HAVE_DRAND48) 76 | if (HAVE_DRAND48) 77 | add_definitions(-DHAVE_DRAND48) 78 | endif (HAVE_DRAND48) 79 | 80 | # NVCC flags 81 | set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS};-Xcompiler -std=c++11") 82 | set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS};-gencode arch=compute_61,code=sm_61") 83 | if(CMAKE_BUILD_TYPE STREQUAL "Debug") 84 | set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS};-G;-g;-lineinfo;--ptxas-options=-v") 85 | else() 86 | set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS};-O3;-g;-lineinfo;--ptxas-options=-v") 87 | endif() 88 | 89 | add_library(amcl_pf 90 | src/amcl/pf/pf.cpp 91 | src/amcl/pf/pf_kdtree.c 92 | src/amcl/pf/pf_pdf.c 93 | src/amcl/pf/pf_vector.c 94 | src/amcl/pf/eig3.c 95 | src/amcl/pf/pf_draw.c) 96 | 97 | add_library(amcl_map 98 | src/amcl/map/map.c 99 | src/amcl/map/map_cspace.cpp 100 | src/amcl/map/map_range.c 101 | src/amcl/map/map_store.c 102 | src/amcl/map/map_draw.c) 103 | 104 | cuda_add_library(amcl_sensors 105 | src/amcl/sensors/amcl_sensor.cpp 106 | src/amcl/sensors/amcl_odom.cpp 107 | src/amcl/sensors/amcl_laser.cpp 108 | src/amcl/sensors/compute_sample_weight.cu) 109 | 110 | target_link_libraries(amcl_sensors amcl_map amcl_pf) 111 | 112 | 113 | add_executable(amcl 114 | src/amcl_node.cpp) 115 | add_dependencies(amcl ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 116 | 117 | target_link_libraries(amcl 118 | amcl_sensors amcl_map amcl_pf 119 | ${Boost_LIBRARIES} 120 | ${catkin_LIBRARIES} 121 | ) 122 | 123 | install( TARGETS 124 | amcl 125 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 126 | ) 127 | 128 | install( TARGETS 129 | amcl_sensors amcl_map amcl_pf 130 | DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 131 | ) 132 | 133 | install(DIRECTORY include/amcl/ 134 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 135 | ) 136 | 137 | install(DIRECTORY examples/ 138 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/examples 139 | ) 140 | 141 | ## Configure Tests 142 | if(CATKIN_ENABLE_TESTING) 143 | find_package(rostest REQUIRED) 144 | 145 | # Bags 146 | catkin_download_test_data(${PROJECT_NAME}_basic_localization_stage_indexed.bag 147 | http://download.ros.org/data/amcl/basic_localization_stage_indexed.bag 148 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test 149 | MD5 41fe43af189ec71e5e48eb9ed661a655) 150 | catkin_download_test_data(${PROJECT_NAME}_global_localization_stage_indexed.bag 151 | http://download.ros.org/data/amcl/global_localization_stage_indexed.bag 152 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test 153 | MD5 752f711cf4f6e8d1d660675e2da096b0) 154 | catkin_download_test_data(${PROJECT_NAME}_small_loop_prf_indexed.bag 155 | http://download.ros.org/data/amcl/small_loop_prf_indexed.bag 156 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test 157 | MD5 e4ef0fc006872b43f12ed8a7ce7dcd81) 158 | catkin_download_test_data(${PROJECT_NAME}_small_loop_crazy_driving_prg_indexed.bag 159 | http://download.ros.org/data/amcl/small_loop_crazy_driving_prg_indexed.bag 160 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test 161 | MD5 4a58d1a7962914009d99000d06e5939c) 162 | catkin_download_test_data(${PROJECT_NAME}_texas_greenroom_loop_indexed.bag 163 | http://download.ros.org/data/amcl/texas_greenroom_loop_indexed.bag 164 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test 165 | MD5 6e3432115cccdca1247f6c807038e13d) 166 | catkin_download_test_data(${PROJECT_NAME}_texas_willow_hallway_loop_indexed.bag 167 | http://download.ros.org/data/amcl/texas_willow_hallway_loop_indexed.bag 168 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test 169 | MD5 27deb742fdcd3af44cf446f39f2688a8) 170 | catkin_download_test_data(${PROJECT_NAME}_rosie_localization_stage.bag 171 | http://download.ros.org/data/amcl/rosie_localization_stage.bag 172 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test 173 | MD5 3347bf3835724cfa45e958c5c1846066) 174 | 175 | # Maps 176 | catkin_download_test_data(${PROJECT_NAME}_willow-full.pgm 177 | http://download.ros.org/data/amcl/willow-full.pgm 178 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test 179 | MD5 b84465cdbbfe3e2fb9eb4579e0bcaf0e) 180 | catkin_download_test_data(${PROJECT_NAME}_willow-full-0.05.pgm 181 | http://download.ros.org/data/amcl/willow-full-0.05.pgm 182 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test 183 | MD5 b61694296e08965096c5e78611fd9765) 184 | 185 | # Tests 186 | add_rostest(test/set_initial_pose.xml) 187 | add_rostest(test/set_initial_pose_delayed.xml) 188 | add_rostest(test/basic_localization_stage.xml) 189 | add_rostest(test/small_loop_prf.xml) 190 | add_rostest(test/small_loop_crazy_driving_prg.xml) 191 | add_rostest(test/texas_greenroom_loop.xml) 192 | add_rostest(test/rosie_multilaser.xml) 193 | add_rostest(test/texas_willow_hallway_loop.xml) 194 | 195 | # Not sure when or if this actually passed. 196 | # 197 | # The point of this is that you start with an even probability 198 | # distribution over the whole map and the robot localizes itself after 199 | # some number of iterations of sensing and motion. 200 | # 201 | # add_rostest(test/global_localization_stage.xml) 202 | endif() 203 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # amcl 2 | 3 | ## Introduction 4 | This is a CUDA implementation of ROS `amcl` package. 5 | And, this repogitory was derived from . 6 | 7 | The `amcl` is a probabilistic localization system for a robot moving in 2D. It implements the adaptive (or KLD-sampling) Monte Carlo localization approach (as described by Dieter Fox), which uses a particle filter to track the pose of a robot against a known map. 8 | 9 | ## Requirements 10 | * CUDA ToolKit 11 | 12 | ## Preparation 13 | Please specify appropriate `arch` and `code` for your GPU in `CMakeLists.txt`. 14 | You can find information in . 15 | An example for GeForce GTX 1060(`Compute Capability` is 6.1) is shown below. 16 | 17 | ```cmake 18 | set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS};-gencode arch=compute_61,code=sm_61") 19 | ``` 20 | 21 | ## Build Instructions 22 | ```shell 23 | $ mkdir -p ~/catkin_ws/src 24 | $ cd ~/catkin_ws/src 25 | $ git clone https://github.com/atinfinity/amcl.git 26 | $ cd ~/catkin_ws 27 | $ catkin_make 28 | $ source devel/setup.bash 29 | ``` 30 | -------------------------------------------------------------------------------- /cfg/AMCL.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE = 'amcl' 4 | 5 | from math import pi 6 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, int_t, double_t, str_t, bool_t 7 | 8 | gen = ParameterGenerator() 9 | 10 | gen.add("min_particles", int_t, 0, "Minimum allowed number of particles.", 100, 0, 1000) 11 | gen.add("max_particles", int_t, 0, "Mamimum allowed number of particles.", 5000, 0, 10000) 12 | 13 | gen.add("kld_err", double_t, 0, "Maximum error between the true distribution and the estimated distribution.", .01, 0, 1) 14 | gen.add("kld_z", double_t, 0, "Upper standard normal quantile for (1 - p), where p is the probability that the error on the estimated distrubition will be less than kld_err.", .99, 0, 1) 15 | 16 | gen.add("update_min_d", double_t, 0, "Translational movement required before performing a filter update.", .2, 0, 5) 17 | gen.add("update_min_a", double_t, 0, "Rotational movement required before performing a filter update.", pi/6, 0, 2*pi) 18 | 19 | gen.add("resample_interval", int_t, 0, "Number of filter updates required before resampling.", 2, 0, 20) 20 | 21 | gen.add("transform_tolerance", double_t, 0, "Time with which to post-date the transform that is published, to indicate that this transform is valid into the future.", .1, 0, 2) 22 | 23 | gen.add("recovery_alpha_slow", double_t, 0, "Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.001.", 0, 0, .5) 24 | gen.add("recovery_alpha_fast", double_t, 0, "Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.1.", 0, 0, 1) 25 | 26 | gen.add("do_beamskip", bool_t, 0, "When true skips laser scans when a scan doesnt work for a majority of particles", False) 27 | gen.add("beam_skip_distance", double_t, 0, "Distance from a valid map point before scan is considered invalid", 0.5, 0, 2) 28 | gen.add("beam_skip_threshold", double_t, 0, "Ratio of samples for which the scans are valid to consider as valid scan", 0.3, 0, 1) 29 | 30 | gen.add("tf_broadcast", bool_t, 0, "When true (the default), publish results via TF. When false, do not.", True) 31 | gen.add("gui_publish_rate", double_t, 0, "Maximum rate (Hz) at which scans and paths are published for visualization, -1.0 to disable.", -1, -1, 100) 32 | gen.add("save_pose_rate", double_t, 0, "Maximum rate (Hz) at which to store the last estimated pose and covariance to the parameter server, in the variables ~initial_pose_* and ~initial_cov_*. This saved pose will be used on subsequent runs to initialize the filter. -1.0 to disable.", .5, -1, 10) 33 | 34 | gen.add("use_map_topic", bool_t, 0, "When set to true, AMCL will subscribe to the map topic rather than making a service call to receive its map.", False) 35 | gen.add("first_map_only", bool_t, 0, "When set to true, AMCL will only use the first map it subscribes to, rather than updating each time a new one is received.", False) 36 | 37 | # Laser Model Parameters 38 | gen.add("laser_min_range", double_t, 0, "Minimum scan range to be considered; -1.0 will cause the laser's reported minimum range to be used.", -1, -1, 1000) 39 | gen.add("laser_max_range", double_t, 0, "Maximum scan range to be considered; -1.0 will cause the laser's reported maximum range to be used.", -1, -1, 1000) 40 | 41 | gen.add("laser_max_beams", int_t, 0, "How many evenly-spaced beams in each scan to be used when updating the filter.", 30, 0, 250) 42 | 43 | gen.add("laser_z_hit", double_t, 0, "Mixture weight for the z_hit part of the model.", .95, 0, 10) 44 | gen.add("laser_z_short", double_t, 0, "Mixture weight for the z_short part of the model.", .1, 0, 10) 45 | gen.add("laser_z_max", double_t, 0, "Mixture weight for the z_max part of the model.", .05, 0, 10) 46 | gen.add("laser_z_rand", double_t, 0, "Mixture weight for the z_rand part of the model.", .05, 0, 10) 47 | 48 | gen.add("laser_sigma_hit", double_t, 0, "Standard deviation for Gaussian model used in z_hit part of the model.", .2, 0, 10) 49 | gen.add("laser_lambda_short", double_t, 0, "Exponential decay parameter for z_short part of model.", .1, 0, 10) 50 | gen.add("laser_likelihood_max_dist", double_t, 0, "Maximum distance to do obstacle inflation on map, for use in likelihood_field model.", 2, 0, 20) 51 | 52 | lmt = gen.enum([gen.const("beam_const", str_t, "beam", "Use beam laser model"), gen.const("likelihood_field_const", str_t, "likelihood_field", "Use likelihood_field laser model"), gen.const("likelihood_field_prob", str_t, "likelihood_field_prob", "Use likelihood_field_prob laser model")], "Laser Models") 53 | gen.add("laser_model_type", str_t, 0, "Which model to use, either beam, likelihood_field or likelihood_field_prob.", "likelihood_field", edit_method=lmt) 54 | 55 | # Odometry Model Parameters 56 | odt = gen.enum([gen.const("diff_const", str_t, "diff", "Use diff odom model"), 57 | gen.const("omni_const", str_t, "omni", "Use omni odom model"), 58 | gen.const("diff_corrected_const", str_t, "diff-corrected", "Use corrected diff odom model"), 59 | gen.const("omni_corrected_const", str_t, "omni-corrected", "Use corrected omni odom model")], 60 | "Odom Models") 61 | gen.add("odom_model_type", str_t, 0, "Which model to use, diff, omni, diff-corrected, or omni-corrected", "diff", edit_method=odt) 62 | 63 | gen.add("odom_alpha1", double_t, 0, "Specifies the expected noise in odometry's rotation estimate from the rotational component of the robot's motion.", .2, 0, 10) 64 | gen.add("odom_alpha2", double_t, 0, "Specifies the expected noise in odometry's rotation estimate from the translational component of the robot's motion.", .2, 0, 10) 65 | gen.add("odom_alpha3", double_t, 0, "Specifies the expected noise in odometry's translation estimate from the translational component of the robot's motion.", .2, 0, 10) 66 | gen.add("odom_alpha4", double_t, 0, "Specifies the expected noise in odometry's translation estimate from the rotational component of the robot's motion.", .2, 0, 10) 67 | gen.add("odom_alpha5", double_t, 0, "Translation-related noise parameter (only used if model is omni).", .2, 0, 10) 68 | 69 | gen.add("odom_frame_id", str_t, 0, "Which frame to use for odometry.", "odom") 70 | gen.add("base_frame_id", str_t, 0, "Which frame to use for the robot base.", "base_link") 71 | gen.add("global_frame_id", str_t, 0, "The name of the coordinate frame published by the localization system.", "map") 72 | 73 | gen.add("restore_defaults", bool_t, 0, "Retsore the default configuration", False) 74 | 75 | exit(gen.generate(PACKAGE, "amcl_node", "AMCL")) 76 | -------------------------------------------------------------------------------- /examples/amcl_diff.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /examples/amcl_omni.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /include/amcl/map/map.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: Global map (grid-based) 23 | * Author: Andrew Howard 24 | * Date: 6 Feb 2003 25 | * CVS: $Id: map.h 1713 2003-08-23 04:03:43Z inspectorg $ 26 | **************************************************************************/ 27 | 28 | #ifndef MAP_H 29 | #define MAP_H 30 | 31 | #include 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | // Forward declarations 38 | struct _rtk_fig_t; 39 | 40 | 41 | // Limits 42 | #define MAP_WIFI_MAX_LEVELS 8 43 | 44 | 45 | // Description for a single map cell. 46 | typedef struct 47 | { 48 | // Occupancy state (-1 = free, 0 = unknown, +1 = occ) 49 | int occ_state; 50 | 51 | // Distance to the nearest occupied cell 52 | double occ_dist; 53 | 54 | // Wifi levels 55 | //int wifi_levels[MAP_WIFI_MAX_LEVELS]; 56 | 57 | } map_cell_t; 58 | 59 | 60 | // Description for a map 61 | typedef struct 62 | { 63 | // Map origin; the map is a viewport onto a conceptual larger map. 64 | double origin_x, origin_y; 65 | 66 | // Map scale (m/cell) 67 | double scale; 68 | 69 | // Map dimensions (number of cells) 70 | int size_x, size_y; 71 | 72 | // The map data, stored as a grid 73 | map_cell_t *cells; 74 | 75 | // Max distance at which we care about obstacles, for constructing 76 | // likelihood field 77 | double max_occ_dist; 78 | 79 | } map_t; 80 | 81 | 82 | 83 | /************************************************************************** 84 | * Basic map functions 85 | **************************************************************************/ 86 | 87 | // Create a new (empty) map 88 | map_t *map_alloc(void); 89 | 90 | // Destroy a map 91 | void map_free(map_t *map); 92 | 93 | // Get the cell at the given point 94 | map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa); 95 | 96 | // Load an occupancy map 97 | int map_load_occ(map_t *map, const char *filename, double scale, int negate); 98 | 99 | // Load a wifi signal strength map 100 | //int map_load_wifi(map_t *map, const char *filename, int index); 101 | 102 | // Update the cspace distances 103 | void map_update_cspace(map_t *map, double max_occ_dist); 104 | 105 | 106 | /************************************************************************** 107 | * Range functions 108 | **************************************************************************/ 109 | 110 | // Extract a single range reading from the map 111 | double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range); 112 | 113 | 114 | /************************************************************************** 115 | * GUI/diagnostic functions 116 | **************************************************************************/ 117 | 118 | // Draw the occupancy grid 119 | void map_draw_occ(map_t *map, struct _rtk_fig_t *fig); 120 | 121 | // Draw the cspace map 122 | void map_draw_cspace(map_t *map, struct _rtk_fig_t *fig); 123 | 124 | // Draw a wifi map 125 | void map_draw_wifi(map_t *map, struct _rtk_fig_t *fig, int index); 126 | 127 | 128 | /************************************************************************** 129 | * Map manipulation macros 130 | **************************************************************************/ 131 | 132 | // Convert from map index to world coords 133 | #define MAP_WXGX(map, i) (map->origin_x + ((i) - map->size_x / 2) * map->scale) 134 | #define MAP_WYGY(map, j) (map->origin_y + ((j) - map->size_y / 2) * map->scale) 135 | 136 | // Convert from world coords to map coords 137 | #define MAP_GXWX(map, x) (floor((x - map->origin_x) / map->scale + 0.5) + map->size_x / 2) 138 | #define MAP_GYWY(map, y) (floor((y - map->origin_y) / map->scale + 0.5) + map->size_y / 2) 139 | 140 | // Test to see if the given map coords lie within the absolute map bounds. 141 | #define MAP_VALID(map, i, j) ((i >= 0) && (i < map->size_x) && (j >= 0) && (j < map->size_y)) 142 | 143 | // Compute the cell index for the given map coords. 144 | #define MAP_INDEX(map, i, j) ((i) + (j) * map->size_x) 145 | 146 | #ifdef __cplusplus 147 | } 148 | #endif 149 | 150 | #endif 151 | -------------------------------------------------------------------------------- /include/amcl/pf/eig3.h: -------------------------------------------------------------------------------- 1 | 2 | /* Eigen-decomposition for symmetric 3x3 real matrices. 3 | Public domain, copied from the public domain Java library JAMA. */ 4 | 5 | #ifndef _eig_h 6 | 7 | /* Symmetric matrix A => eigenvectors in columns of V, corresponding 8 | eigenvalues in d. */ 9 | void eigen_decomposition(double A[3][3], double V[3][3], double d[3]); 10 | 11 | #endif 12 | -------------------------------------------------------------------------------- /include/amcl/pf/pf.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: Simple particle filter for localization. 23 | * Author: Andrew Howard 24 | * Date: 10 Dec 2002 25 | * CVS: $Id: pf.h 3293 2005-11-19 08:37:45Z gerkey $ 26 | *************************************************************************/ 27 | 28 | #ifndef PF_H 29 | #define PF_H 30 | 31 | #include "pf_vector.h" 32 | #include "pf_kdtree.h" 33 | 34 | #ifdef __cplusplus 35 | extern "C" { 36 | #endif 37 | 38 | // Forward declarations 39 | struct _pf_t; 40 | struct _rtk_fig_t; 41 | struct _pf_sample_set_t; 42 | 43 | // Function prototype for the initialization model; generates a sample pose from 44 | // an appropriate distribution. 45 | typedef pf_vector_t (*pf_init_model_fn_t) (void *init_data); 46 | 47 | // Function prototype for the action model; generates a sample pose from 48 | // an appropriate distribution 49 | typedef void (*pf_action_model_fn_t) (void *action_data, 50 | struct _pf_sample_set_t* set); 51 | 52 | // Function prototype for the sensor model; determines the probability 53 | // for the given set of sample poses. 54 | typedef double (*pf_sensor_model_fn_t) (void *sensor_data, 55 | struct _pf_sample_set_t* set); 56 | 57 | 58 | // Information for a single sample 59 | typedef struct 60 | { 61 | // Pose represented by this sample 62 | pf_vector_t pose; 63 | 64 | // Weight for this pose 65 | double weight; 66 | 67 | } pf_sample_t; 68 | 69 | 70 | // Information for a cluster of samples 71 | typedef struct 72 | { 73 | // Number of samples 74 | int count; 75 | 76 | // Total weight of samples in this cluster 77 | double weight; 78 | 79 | // Cluster statistics 80 | pf_vector_t mean; 81 | pf_matrix_t cov; 82 | 83 | // Workspace 84 | double m[4], c[2][2]; 85 | 86 | } pf_cluster_t; 87 | 88 | 89 | // Information for a set of samples 90 | typedef struct _pf_sample_set_t 91 | { 92 | // The samples 93 | int sample_count; 94 | pf_sample_t *samples; 95 | 96 | // A kdtree encoding the histogram 97 | pf_kdtree_t *kdtree; 98 | 99 | // Clusters 100 | int cluster_count, cluster_max_count; 101 | pf_cluster_t *clusters; 102 | 103 | // Filter statistics 104 | pf_vector_t mean; 105 | pf_matrix_t cov; 106 | int converged; 107 | } pf_sample_set_t; 108 | 109 | 110 | // Information for an entire filter 111 | typedef struct _pf_t 112 | { 113 | // This min and max number of samples 114 | int min_samples, max_samples; 115 | 116 | // Population size parameters 117 | double pop_err, pop_z; 118 | 119 | // The sample sets. We keep two sets and use [current_set] 120 | // to identify the active set. 121 | int current_set; 122 | pf_sample_set_t sets[2]; 123 | 124 | // Running averages, slow and fast, of likelihood 125 | double w_slow, w_fast; 126 | 127 | // Decay rates for running averages 128 | double alpha_slow, alpha_fast; 129 | 130 | // Function used to draw random pose samples 131 | pf_init_model_fn_t random_pose_fn; 132 | void *random_pose_data; 133 | 134 | double dist_threshold; //distance threshold in each axis over which the pf is considered to not be converged 135 | int converged; 136 | } pf_t; 137 | 138 | 139 | // Create a new filter 140 | pf_t *pf_alloc(int min_samples, int max_samples, 141 | double alpha_slow, double alpha_fast, 142 | pf_init_model_fn_t random_pose_fn, void *random_pose_data); 143 | 144 | // Free an existing filter 145 | void pf_free(pf_t *pf); 146 | 147 | // Initialize the filter using a guassian 148 | void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov); 149 | 150 | // Initialize the filter using some model 151 | void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data); 152 | 153 | // Update the filter with some new action 154 | void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data); 155 | 156 | // Update the filter with some new sensor observation 157 | void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data); 158 | 159 | // Resample the distribution 160 | void pf_update_resample(pf_t *pf); 161 | 162 | // Compute the CEP statistics (mean and variance). 163 | void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var); 164 | 165 | // Compute the statistics for a particular cluster. Returns 0 if 166 | // there is no such cluster. 167 | int pf_get_cluster_stats(pf_t *pf, int cluster, double *weight, 168 | pf_vector_t *mean, pf_matrix_t *cov); 169 | 170 | // Re-compute the cluster statistics for a sample set 171 | void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set); 172 | 173 | 174 | // Display the sample set 175 | void pf_draw_samples(pf_t *pf, struct _rtk_fig_t *fig, int max_samples); 176 | 177 | // Draw the histogram (kdtree) 178 | void pf_draw_hist(pf_t *pf, struct _rtk_fig_t *fig); 179 | 180 | // Draw the CEP statistics 181 | void pf_draw_cep_stats(pf_t *pf, struct _rtk_fig_t *fig); 182 | 183 | // Draw the cluster statistics 184 | void pf_draw_cluster_stats(pf_t *pf, struct _rtk_fig_t *fig); 185 | 186 | //calculate if the particle filter has converged - 187 | //and sets the converged flag in the current set and the pf 188 | int pf_update_converged(pf_t *pf); 189 | 190 | //sets the current set and pf converged values to zero 191 | void pf_init_converged(pf_t *pf); 192 | 193 | #ifdef __cplusplus 194 | } 195 | #endif 196 | 197 | 198 | #endif 199 | -------------------------------------------------------------------------------- /include/amcl/pf/pf_kdtree.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: KD tree functions 23 | * Author: Andrew Howard 24 | * Date: 18 Dec 2002 25 | * CVS: $Id: pf_kdtree.h 6532 2008-06-11 02:45:56Z gbiggs $ 26 | *************************************************************************/ 27 | 28 | #ifndef PF_KDTREE_H 29 | #define PF_KDTREE_H 30 | 31 | #ifdef __cplusplus 32 | extern "C" { 33 | #endif 34 | 35 | #ifdef INCLUDE_RTKGUI 36 | #include "rtk.h" 37 | #endif 38 | 39 | 40 | // Info for a node in the tree 41 | typedef struct pf_kdtree_node 42 | { 43 | // Depth in the tree 44 | int leaf, depth; 45 | 46 | // Pivot dimension and value 47 | int pivot_dim; 48 | double pivot_value; 49 | 50 | // The key for this node 51 | int key[3]; 52 | 53 | // The value for this node 54 | double value; 55 | 56 | // The cluster label (leaf nodes) 57 | int cluster; 58 | 59 | // Child nodes 60 | struct pf_kdtree_node *children[2]; 61 | 62 | } pf_kdtree_node_t; 63 | 64 | 65 | // A kd tree 66 | typedef struct 67 | { 68 | // Cell size 69 | double size[3]; 70 | 71 | // The root node of the tree 72 | pf_kdtree_node_t *root; 73 | 74 | // The number of nodes in the tree 75 | int node_count, node_max_count; 76 | pf_kdtree_node_t *nodes; 77 | 78 | // The number of leaf nodes in the tree 79 | int leaf_count; 80 | 81 | } pf_kdtree_t; 82 | 83 | 84 | // Create a tree 85 | extern pf_kdtree_t *pf_kdtree_alloc(int max_size); 86 | 87 | // Destroy a tree 88 | extern void pf_kdtree_free(pf_kdtree_t *self); 89 | 90 | // Clear all entries from the tree 91 | extern void pf_kdtree_clear(pf_kdtree_t *self); 92 | 93 | // Insert a pose into the tree 94 | extern void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value); 95 | 96 | // Cluster the leaves in the tree 97 | extern void pf_kdtree_cluster(pf_kdtree_t *self); 98 | 99 | // Determine the probability estimate for the given pose 100 | extern double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose); 101 | 102 | // Determine the cluster label for the given pose 103 | extern int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose); 104 | 105 | 106 | #ifdef INCLUDE_RTKGUI 107 | 108 | // Draw the tree 109 | extern void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig); 110 | 111 | #endif 112 | 113 | #ifdef __cplusplus 114 | } 115 | #endif 116 | 117 | #endif 118 | -------------------------------------------------------------------------------- /include/amcl/pf/pf_pdf.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: Useful pdf functions 23 | * Author: Andrew Howard 24 | * Date: 10 Dec 2002 25 | * CVS: $Id: pf_pdf.h 6345 2008-04-17 01:36:39Z gerkey $ 26 | *************************************************************************/ 27 | 28 | #ifndef PF_PDF_H 29 | #define PF_PDF_H 30 | 31 | #include "pf_vector.h" 32 | 33 | //#include 34 | //#include 35 | 36 | #ifdef __cplusplus 37 | extern "C" { 38 | #endif 39 | 40 | /************************************************************************** 41 | * Gaussian 42 | *************************************************************************/ 43 | 44 | // Gaussian PDF info 45 | typedef struct 46 | { 47 | // Mean, covariance and inverse covariance 48 | pf_vector_t x; 49 | pf_matrix_t cx; 50 | //pf_matrix_t cxi; 51 | double cxdet; 52 | 53 | // Decomposed covariance matrix (rotation * diagonal) 54 | pf_matrix_t cr; 55 | pf_vector_t cd; 56 | 57 | // A random number generator 58 | //gsl_rng *rng; 59 | 60 | } pf_pdf_gaussian_t; 61 | 62 | 63 | // Create a gaussian pdf 64 | pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx); 65 | 66 | // Destroy the pdf 67 | void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf); 68 | 69 | // Compute the value of the pdf at some point [z]. 70 | //double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t z); 71 | 72 | // Draw randomly from a zero-mean Gaussian distribution, with standard 73 | // deviation sigma. 74 | // We use the polar form of the Box-Muller transformation, explained here: 75 | // http://www.taygeta.com/random/gaussian.html 76 | double pf_ran_gaussian(double sigma); 77 | 78 | // Generate a sample from the pdf. 79 | pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf); 80 | 81 | #ifdef __cplusplus 82 | } 83 | #endif 84 | 85 | #endif 86 | -------------------------------------------------------------------------------- /include/amcl/pf/pf_vector.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: Vector functions 23 | * Author: Andrew Howard 24 | * Date: 10 Dec 2002 25 | * CVS: $Id: pf_vector.h 6345 2008-04-17 01:36:39Z gerkey $ 26 | *************************************************************************/ 27 | 28 | #ifndef PF_VECTOR_H 29 | #define PF_VECTOR_H 30 | 31 | #ifdef __cplusplus 32 | extern "C" { 33 | #endif 34 | 35 | #include 36 | 37 | // The basic vector 38 | typedef struct 39 | { 40 | double v[3]; 41 | } pf_vector_t; 42 | 43 | 44 | // The basic matrix 45 | typedef struct 46 | { 47 | double m[3][3]; 48 | } pf_matrix_t; 49 | 50 | 51 | // Return a zero vector 52 | pf_vector_t pf_vector_zero(); 53 | 54 | // Check for NAN or INF in any component 55 | int pf_vector_finite(pf_vector_t a); 56 | 57 | // Print a vector 58 | void pf_vector_fprintf(pf_vector_t s, FILE *file, const char *fmt); 59 | 60 | // Simple vector addition 61 | pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b); 62 | 63 | // Simple vector subtraction 64 | pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b); 65 | 66 | // Transform from local to global coords (a + b) 67 | pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b); 68 | 69 | // Transform from global to local coords (a - b) 70 | pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b); 71 | 72 | 73 | // Return a zero matrix 74 | pf_matrix_t pf_matrix_zero(); 75 | 76 | // Check for NAN or INF in any component 77 | int pf_matrix_finite(pf_matrix_t a); 78 | 79 | // Print a matrix 80 | void pf_matrix_fprintf(pf_matrix_t s, FILE *file, const char *fmt); 81 | 82 | // Compute the matrix inverse. Will also return the determinant, 83 | // which should be checked for underflow (indicated singular matrix). 84 | //pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det); 85 | 86 | // Decompose a covariance matrix [a] into a rotation matrix [r] and a 87 | // diagonal matrix [d] such that a = r * d * r^T. 88 | void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a); 89 | 90 | #ifdef __cplusplus 91 | } 92 | #endif 93 | 94 | #endif 95 | -------------------------------------------------------------------------------- /include/amcl/sensors/amcl_laser.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey et al. 4 | * 5 | * This library is free software; you can redistribute it and/or 6 | * modify it under the terms of the GNU Lesser General Public 7 | * License as published by the Free Software Foundation; either 8 | * version 2.1 of the License, or (at your option) any later version. 9 | * 10 | * This library is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | * Lesser General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU Lesser General Public 16 | * License along with this library; if not, write to the Free Software 17 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 18 | * 19 | */ 20 | /////////////////////////////////////////////////////////////////////////// 21 | // 22 | // Desc: LASER sensor model for AMCL 23 | // Author: Andrew Howard 24 | // Date: 17 Aug 2003 25 | // CVS: $Id: amcl_laser.h 6443 2008-05-15 19:46:11Z gerkey $ 26 | // 27 | /////////////////////////////////////////////////////////////////////////// 28 | 29 | #ifndef AMCL_LASER_H 30 | #define AMCL_LASER_H 31 | 32 | #include "amcl_sensor.h" 33 | #include "../map/map.h" 34 | 35 | namespace amcl 36 | { 37 | 38 | typedef enum 39 | { 40 | LASER_MODEL_BEAM, 41 | LASER_MODEL_LIKELIHOOD_FIELD, 42 | LASER_MODEL_LIKELIHOOD_FIELD_PROB 43 | } laser_model_t; 44 | 45 | // Laser sensor data 46 | class AMCLLaserData : public AMCLSensorData 47 | { 48 | public: 49 | AMCLLaserData () {ranges=NULL;}; 50 | virtual ~AMCLLaserData() {delete [] ranges;}; 51 | // Laser range data (range, bearing tuples) 52 | public: int range_count; 53 | public: double range_max; 54 | public: double (*ranges)[2]; 55 | }; 56 | 57 | 58 | // Laseretric sensor model 59 | class AMCLLaser : public AMCLSensor 60 | { 61 | // Default constructor 62 | public: AMCLLaser(size_t max_beams, map_t* map); 63 | 64 | public: virtual ~AMCLLaser(); 65 | 66 | public: void SetModelBeam(double z_hit, 67 | double z_short, 68 | double z_max, 69 | double z_rand, 70 | double sigma_hit, 71 | double labda_short, 72 | double chi_outlier); 73 | 74 | public: void SetModelLikelihoodField(double z_hit, 75 | double z_rand, 76 | double sigma_hit, 77 | double max_occ_dist); 78 | 79 | //a more probabilistically correct model - also with the option to do beam skipping 80 | public: void SetModelLikelihoodFieldProb(double z_hit, 81 | double z_rand, 82 | double sigma_hit, 83 | double max_occ_dist, 84 | bool do_beamskip, 85 | double beam_skip_distance, 86 | double beam_skip_threshold, 87 | double beam_skip_error_threshold); 88 | 89 | // Update the filter based on the sensor model. Returns true if the 90 | // filter has been updated. 91 | public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data); 92 | 93 | // Set the laser's pose after construction 94 | public: void SetLaserPose(pf_vector_t& laser_pose) 95 | {this->laser_pose = laser_pose;} 96 | 97 | // Determine the probability for the given pose 98 | private: static double BeamModel(AMCLLaserData *data, 99 | pf_sample_set_t* set); 100 | // Determine the probability for the given pose 101 | private: static double LikelihoodFieldModel(AMCLLaserData *data, 102 | pf_sample_set_t* set); 103 | 104 | // Determine the probability for the given pose - more probablistic model 105 | private: static double LikelihoodFieldModelProb(AMCLLaserData *data, 106 | pf_sample_set_t* set); 107 | 108 | private: void reallocTempData(int max_samples, int max_obs); 109 | 110 | private: laser_model_t model_type; 111 | 112 | // Current data timestamp 113 | private: double time; 114 | 115 | // The laser map 116 | public: map_t *map; // [TODO] 117 | 118 | // Laser offset relative to robot 119 | public: pf_vector_t laser_pose; // [TODO] 120 | 121 | // Max beams to consider 122 | public: int max_beams; // [TODO] 123 | 124 | // Beam skipping parameters (used by LikelihoodFieldModelProb model) 125 | private: bool do_beamskip; 126 | private: double beam_skip_distance; 127 | private: double beam_skip_threshold; 128 | //threshold for the ratio of invalid beams - at which all beams are integrated to the likelihoods 129 | //this would be an error condition 130 | private: double beam_skip_error_threshold; 131 | 132 | //temp data that is kept before observations are integrated to each particle (requried for beam skipping) 133 | private: int max_samples; 134 | private: int max_obs; 135 | private: double **temp_obs; 136 | 137 | // Laser model params 138 | // 139 | // Mixture params for the components of the model; must sum to 1 140 | public: double z_hit; // [TODO] 141 | private: double z_short; 142 | private: double z_max; 143 | public: double z_rand; // [TODO] 144 | // 145 | // Stddev of Gaussian model for laser hits. 146 | public: double sigma_hit; // [TODO] 147 | // Decay rate of exponential model for short readings. 148 | private: double lambda_short; 149 | // Threshold for outlier rejection (unused) 150 | private: double chi_outlier; 151 | }; 152 | 153 | 154 | } 155 | 156 | #endif 157 | -------------------------------------------------------------------------------- /include/amcl/sensors/amcl_odom.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey et al. 4 | * 5 | * This library is free software; you can redistribute it and/or 6 | * modify it under the terms of the GNU Lesser General Public 7 | * License as published by the Free Software Foundation; either 8 | * version 2.1 of the License, or (at your option) any later version. 9 | * 10 | * This library is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | * Lesser General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU Lesser General Public 16 | * License along with this library; if not, write to the Free Software 17 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 18 | * 19 | */ 20 | /////////////////////////////////////////////////////////////////////////// 21 | // 22 | // Desc: Odometry sensor model for AMCL 23 | // Author: Andrew Howard 24 | // Date: 17 Aug 2003 25 | // CVS: $Id: amcl_odom.h 4135 2007-08-23 19:58:48Z gerkey $ 26 | // 27 | /////////////////////////////////////////////////////////////////////////// 28 | 29 | #ifndef AMCL_ODOM_H 30 | #define AMCL_ODOM_H 31 | 32 | #include "amcl_sensor.h" 33 | #include "../pf/pf_pdf.h" 34 | 35 | namespace amcl 36 | { 37 | 38 | typedef enum 39 | { 40 | ODOM_MODEL_DIFF, 41 | ODOM_MODEL_OMNI, 42 | ODOM_MODEL_DIFF_CORRECTED, 43 | ODOM_MODEL_OMNI_CORRECTED 44 | } odom_model_t; 45 | 46 | // Odometric sensor data 47 | class AMCLOdomData : public AMCLSensorData 48 | { 49 | // Odometric pose 50 | public: pf_vector_t pose; 51 | 52 | // Change in odometric pose 53 | public: pf_vector_t delta; 54 | }; 55 | 56 | 57 | // Odometric sensor model 58 | class AMCLOdom : public AMCLSensor 59 | { 60 | // Default constructor 61 | public: AMCLOdom(); 62 | 63 | public: void SetModelDiff(double alpha1, 64 | double alpha2, 65 | double alpha3, 66 | double alpha4); 67 | 68 | public: void SetModelOmni(double alpha1, 69 | double alpha2, 70 | double alpha3, 71 | double alpha4, 72 | double alpha5); 73 | 74 | public: void SetModel( odom_model_t type, 75 | double alpha1, 76 | double alpha2, 77 | double alpha3, 78 | double alpha4, 79 | double alpha5 = 0 ); 80 | 81 | // Update the filter based on the action model. Returns true if the filter 82 | // has been updated. 83 | public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data); 84 | 85 | // Current data timestamp 86 | private: double time; 87 | 88 | // Model type 89 | private: odom_model_t model_type; 90 | 91 | // Drift parameters 92 | private: double alpha1, alpha2, alpha3, alpha4, alpha5; 93 | }; 94 | 95 | 96 | } 97 | 98 | #endif 99 | -------------------------------------------------------------------------------- /include/amcl/sensors/amcl_sensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey et al. 4 | * 5 | * This library is free software; you can redistribute it and/or 6 | * modify it under the terms of the GNU Lesser General Public 7 | * License as published by the Free Software Foundation; either 8 | * version 2.1 of the License, or (at your option) any later version. 9 | * 10 | * This library is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | * Lesser General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU Lesser General Public 16 | * License along with this library; if not, write to the Free Software 17 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 18 | */ 19 | /////////////////////////////////////////////////////////////////////////// 20 | // 21 | // Desc: Adaptive Monte-Carlo localization 22 | // Author: Andrew Howard 23 | // Date: 6 Feb 2003 24 | // CVS: $Id: amcl_sensor.h 6443 2008-05-15 19:46:11Z gerkey $ 25 | // 26 | /////////////////////////////////////////////////////////////////////////// 27 | 28 | #ifndef AMCL_SENSOR_H 29 | #define AMCL_SENSOR_H 30 | 31 | #include "../pf/pf.h" 32 | 33 | namespace amcl 34 | { 35 | 36 | // Forward declarations 37 | class AMCLSensorData; 38 | 39 | 40 | // Base class for all AMCL sensors 41 | class AMCLSensor 42 | { 43 | // Default constructor 44 | public: AMCLSensor(); 45 | 46 | // Default destructor 47 | public: virtual ~AMCLSensor(); 48 | 49 | // Update the filter based on the action model. Returns true if the filter 50 | // has been updated. 51 | public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data); 52 | 53 | // Initialize the filter based on the sensor model. Returns true if the 54 | // filter has been initialized. 55 | public: virtual bool InitSensor(pf_t *pf, AMCLSensorData *data); 56 | 57 | // Update the filter based on the sensor model. Returns true if the 58 | // filter has been updated. 59 | public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data); 60 | 61 | // Flag is true if this is the action sensor 62 | public: bool is_action; 63 | 64 | // Action pose (action sensors only) 65 | public: pf_vector_t pose; 66 | 67 | // AMCL Base 68 | //protected: AdaptiveMCL & AMCL; 69 | 70 | #ifdef INCLUDE_RTKGUI 71 | // Setup the GUI 72 | public: virtual void SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig); 73 | 74 | // Finalize the GUI 75 | public: virtual void ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig); 76 | 77 | // Draw sensor data 78 | public: virtual void UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data); 79 | #endif 80 | }; 81 | 82 | 83 | 84 | // Base class for all AMCL sensor measurements 85 | class AMCLSensorData 86 | { 87 | // Pointer to sensor that generated the data 88 | public: AMCLSensor *sensor; 89 | virtual ~AMCLSensorData() {} 90 | 91 | // Data timestamp 92 | public: double time; 93 | }; 94 | 95 | } 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /include/amcl/sensors/compute_sample_weight.cuh: -------------------------------------------------------------------------------- 1 | #ifndef PF_GPU_H 2 | #define PF_GPU_H 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include 9 | #include "amcl/pf/pf.h" 10 | #include "amcl/sensors/amcl_laser.h" 11 | 12 | double cu_compute_sample_weight(const amcl::AMCLLaserData *data, pf_sample_t *samples, const int sample_count); 13 | 14 | #ifdef __cplusplus 15 | } 16 | #endif 17 | 18 | #endif // PF_GPU_H 19 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | amcl 5 | 1.16.2 6 | 7 |

8 | amcl is a probabilistic localization system for a robot moving in 9 | 2D. It implements the adaptive (or KLD-sampling) Monte Carlo 10 | localization approach (as described by Dieter Fox), which uses a 11 | particle filter to track the pose of a robot against a known map. 12 |

13 |

14 | This node is derived, with thanks, from Andrew Howard's excellent 15 | 'amcl' Player driver. 16 |

17 | 18 | http://wiki.ros.org/amcl 19 | Brian P. Gerkey 20 | contradict@gmail.com 21 | David V. Lu!! 22 | Michael Ferguson 23 | Aaron Hoy 24 | LGPL 25 | 26 | catkin 27 | 28 | message_filters 29 | tf2_geometry_msgs 30 | 31 | diagnostic_updater 32 | dynamic_reconfigure 33 | geometry_msgs 34 | nav_msgs 35 | rosbag 36 | roscpp 37 | sensor_msgs 38 | std_srvs 39 | tf2 40 | tf2_msgs 41 | tf2_ros 42 | 43 | map_server 44 | rostest 45 | python_orocos_kdl 46 | 47 | -------------------------------------------------------------------------------- /src/amcl/map/map.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: Global map (grid-based) 23 | * Author: Andrew Howard 24 | * Date: 6 Feb 2003 25 | * CVS: $Id: map.c 1713 2003-08-23 04:03:43Z inspectorg $ 26 | **************************************************************************/ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include "amcl/map/map.h" 35 | 36 | 37 | // Create a new map 38 | map_t *map_alloc(void) 39 | { 40 | map_t *map; 41 | 42 | map = (map_t*) malloc(sizeof(map_t)); 43 | 44 | // Assume we start at (0, 0) 45 | map->origin_x = 0; 46 | map->origin_y = 0; 47 | 48 | // Make the size odd 49 | map->size_x = 0; 50 | map->size_y = 0; 51 | map->scale = 0; 52 | 53 | // Allocate storage for main map 54 | map->cells = (map_cell_t*) NULL; 55 | 56 | return map; 57 | } 58 | 59 | 60 | // Destroy a map 61 | void map_free(map_t *map) 62 | { 63 | free(map->cells); 64 | free(map); 65 | return; 66 | } 67 | 68 | 69 | // Get the cell at the given point 70 | map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa) 71 | { 72 | int i, j; 73 | map_cell_t *cell; 74 | 75 | i = MAP_GXWX(map, ox); 76 | j = MAP_GYWY(map, oy); 77 | 78 | if (!MAP_VALID(map, i, j)) 79 | return NULL; 80 | 81 | cell = map->cells + MAP_INDEX(map, i, j); 82 | return cell; 83 | } 84 | 85 | -------------------------------------------------------------------------------- /src/amcl/map/map_cspace.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include "amcl/map/map.h" 27 | 28 | class CellData 29 | { 30 | public: 31 | map_t* map_; 32 | unsigned int i_, j_; 33 | unsigned int src_i_, src_j_; 34 | }; 35 | 36 | class CachedDistanceMap 37 | { 38 | public: 39 | CachedDistanceMap(double scale, double max_dist) : 40 | distances_(NULL), scale_(scale), max_dist_(max_dist) 41 | { 42 | cell_radius_ = max_dist / scale; 43 | distances_ = new double *[cell_radius_+2]; 44 | for(int i=0; i<=cell_radius_+1; i++) 45 | { 46 | distances_[i] = new double[cell_radius_+2]; 47 | for(int j=0; j<=cell_radius_+1; j++) 48 | { 49 | distances_[i][j] = sqrt(i*i + j*j); 50 | } 51 | } 52 | } 53 | ~CachedDistanceMap() 54 | { 55 | if(distances_) 56 | { 57 | for(int i=0; i<=cell_radius_+1; i++) 58 | delete[] distances_[i]; 59 | delete[] distances_; 60 | } 61 | } 62 | double** distances_; 63 | double scale_; 64 | double max_dist_; 65 | int cell_radius_; 66 | }; 67 | 68 | 69 | bool operator<(const CellData& a, const CellData& b) 70 | { 71 | return a.map_->cells[MAP_INDEX(a.map_, a.i_, a.j_)].occ_dist > a.map_->cells[MAP_INDEX(b.map_, b.i_, b.j_)].occ_dist; 72 | } 73 | 74 | CachedDistanceMap* 75 | get_distance_map(double scale, double max_dist) 76 | { 77 | static CachedDistanceMap* cdm = NULL; 78 | 79 | if(!cdm || (cdm->scale_ != scale) || (cdm->max_dist_ != max_dist)) 80 | { 81 | if(cdm) 82 | delete cdm; 83 | cdm = new CachedDistanceMap(scale, max_dist); 84 | } 85 | 86 | return cdm; 87 | } 88 | 89 | void enqueue(map_t* map, int i, int j, 90 | int src_i, int src_j, 91 | std::priority_queue& Q, 92 | CachedDistanceMap* cdm, 93 | unsigned char* marked) 94 | { 95 | if(marked[MAP_INDEX(map, i, j)]) 96 | return; 97 | 98 | int di = abs(i - src_i); 99 | int dj = abs(j - src_j); 100 | double distance = cdm->distances_[di][dj]; 101 | 102 | if(distance > cdm->cell_radius_) 103 | return; 104 | 105 | map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale; 106 | 107 | CellData cell; 108 | cell.map_ = map; 109 | cell.i_ = i; 110 | cell.j_ = j; 111 | cell.src_i_ = src_i; 112 | cell.src_j_ = src_j; 113 | 114 | Q.push(cell); 115 | 116 | marked[MAP_INDEX(map, i, j)] = 1; 117 | } 118 | 119 | // Update the cspace distance values 120 | void map_update_cspace(map_t *map, double max_occ_dist) 121 | { 122 | unsigned char* marked; 123 | std::priority_queue Q; 124 | 125 | marked = new unsigned char[map->size_x*map->size_y]; 126 | memset(marked, 0, sizeof(unsigned char) * map->size_x*map->size_y); 127 | 128 | map->max_occ_dist = max_occ_dist; 129 | 130 | CachedDistanceMap* cdm = get_distance_map(map->scale, map->max_occ_dist); 131 | 132 | // Enqueue all the obstacle cells 133 | CellData cell; 134 | cell.map_ = map; 135 | for(int i=0; isize_x; i++) 136 | { 137 | cell.src_i_ = cell.i_ = i; 138 | for(int j=0; jsize_y; j++) 139 | { 140 | if(map->cells[MAP_INDEX(map, i, j)].occ_state == +1) 141 | { 142 | map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0; 143 | cell.src_j_ = cell.j_ = j; 144 | marked[MAP_INDEX(map, i, j)] = 1; 145 | Q.push(cell); 146 | } 147 | else 148 | map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist; 149 | } 150 | } 151 | 152 | while(!Q.empty()) 153 | { 154 | CellData current_cell = Q.top(); 155 | if(current_cell.i_ > 0) 156 | enqueue(map, current_cell.i_-1, current_cell.j_, 157 | current_cell.src_i_, current_cell.src_j_, 158 | Q, cdm, marked); 159 | if(current_cell.j_ > 0) 160 | enqueue(map, current_cell.i_, current_cell.j_-1, 161 | current_cell.src_i_, current_cell.src_j_, 162 | Q, cdm, marked); 163 | if((int)current_cell.i_ < map->size_x - 1) 164 | enqueue(map, current_cell.i_+1, current_cell.j_, 165 | current_cell.src_i_, current_cell.src_j_, 166 | Q, cdm, marked); 167 | if((int)current_cell.j_ < map->size_y - 1) 168 | enqueue(map, current_cell.i_, current_cell.j_+1, 169 | current_cell.src_i_, current_cell.src_j_, 170 | Q, cdm, marked); 171 | 172 | Q.pop(); 173 | } 174 | 175 | delete[] marked; 176 | } 177 | -------------------------------------------------------------------------------- /src/amcl/map/map_draw.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: Local map GUI functions 23 | * Author: Andrew Howard 24 | * Date: 18 Jan 2003 25 | * CVS: $Id: map_draw.c 7057 2008-10-02 00:44:06Z gbiggs $ 26 | **************************************************************************/ 27 | 28 | #ifdef INCLUDE_RTKGUI 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include "amcl/map/map.h" 37 | 38 | 39 | //////////////////////////////////////////////////////////////////////////// 40 | // Draw the occupancy map 41 | void map_draw_occ(map_t *map, rtk_fig_t *fig) 42 | { 43 | int i, j; 44 | int col; 45 | map_cell_t *cell; 46 | uint16_t *image; 47 | uint16_t *pixel; 48 | 49 | image = malloc(map->size_x * map->size_y * sizeof(image[0])); 50 | 51 | // Draw occupancy 52 | for (j = 0; j < map->size_y; j++) 53 | { 54 | for (i = 0; i < map->size_x; i++) 55 | { 56 | cell = map->cells + MAP_INDEX(map, i, j); 57 | pixel = image + (j * map->size_x + i); 58 | 59 | col = 127 - 127 * cell->occ_state; 60 | *pixel = RTK_RGB16(col, col, col); 61 | } 62 | } 63 | 64 | // Draw the entire occupancy map as an image 65 | rtk_fig_image(fig, map->origin_x, map->origin_y, 0, 66 | map->scale, map->size_x, map->size_y, 16, image, NULL); 67 | 68 | free(image); 69 | 70 | return; 71 | } 72 | 73 | 74 | //////////////////////////////////////////////////////////////////////////// 75 | // Draw the cspace map 76 | void map_draw_cspace(map_t *map, rtk_fig_t *fig) 77 | { 78 | int i, j; 79 | int col; 80 | map_cell_t *cell; 81 | uint16_t *image; 82 | uint16_t *pixel; 83 | 84 | image = malloc(map->size_x * map->size_y * sizeof(image[0])); 85 | 86 | // Draw occupancy 87 | for (j = 0; j < map->size_y; j++) 88 | { 89 | for (i = 0; i < map->size_x; i++) 90 | { 91 | cell = map->cells + MAP_INDEX(map, i, j); 92 | pixel = image + (j * map->size_x + i); 93 | 94 | col = 255 * cell->occ_dist / map->max_occ_dist; 95 | 96 | *pixel = RTK_RGB16(col, col, col); 97 | } 98 | } 99 | 100 | // Draw the entire occupancy map as an image 101 | rtk_fig_image(fig, map->origin_x, map->origin_y, 0, 102 | map->scale, map->size_x, map->size_y, 16, image, NULL); 103 | 104 | free(image); 105 | 106 | return; 107 | } 108 | 109 | 110 | //////////////////////////////////////////////////////////////////////////// 111 | // Draw a wifi map 112 | void map_draw_wifi(map_t *map, rtk_fig_t *fig, int index) 113 | { 114 | int i, j; 115 | int level, col; 116 | map_cell_t *cell; 117 | uint16_t *image, *mask; 118 | uint16_t *ipix, *mpix; 119 | 120 | image = malloc(map->size_x * map->size_y * sizeof(image[0])); 121 | mask = malloc(map->size_x * map->size_y * sizeof(mask[0])); 122 | 123 | // Draw wifi levels 124 | for (j = 0; j < map->size_y; j++) 125 | { 126 | for (i = 0; i < map->size_x; i++) 127 | { 128 | cell = map->cells + MAP_INDEX(map, i, j); 129 | ipix = image + (j * map->size_x + i); 130 | mpix = mask + (j * map->size_x + i); 131 | 132 | level = cell->wifi_levels[index]; 133 | 134 | if (cell->occ_state == -1 && level != 0) 135 | { 136 | col = 255 * (100 + level) / 100; 137 | *ipix = RTK_RGB16(col, col, col); 138 | *mpix = 1; 139 | } 140 | else 141 | { 142 | *mpix = 0; 143 | } 144 | } 145 | } 146 | 147 | // Draw the entire occupancy map as an image 148 | rtk_fig_image(fig, map->origin_x, map->origin_y, 0, 149 | map->scale, map->size_x, map->size_y, 16, image, mask); 150 | 151 | free(mask); 152 | free(image); 153 | 154 | return; 155 | } 156 | 157 | 158 | #endif 159 | -------------------------------------------------------------------------------- /src/amcl/map/map_range.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: Range routines 23 | * Author: Andrew Howard 24 | * Date: 18 Jan 2003 25 | * CVS: $Id: map_range.c 1347 2003-05-05 06:24:33Z inspectorg $ 26 | **************************************************************************/ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include "amcl/map/map.h" 34 | 35 | // Extract a single range reading from the map. Unknown cells and/or 36 | // out-of-bound cells are treated as occupied, which makes it easy to 37 | // use Stage bitmap files. 38 | double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range) 39 | { 40 | // Bresenham raytracing 41 | int x0,x1,y0,y1; 42 | int x,y; 43 | int xstep, ystep; 44 | char steep; 45 | int tmp; 46 | int deltax, deltay, error, deltaerr; 47 | 48 | x0 = MAP_GXWX(map,ox); 49 | y0 = MAP_GYWY(map,oy); 50 | 51 | x1 = MAP_GXWX(map,ox + max_range * cos(oa)); 52 | y1 = MAP_GYWY(map,oy + max_range * sin(oa)); 53 | 54 | if(abs(y1-y0) > abs(x1-x0)) 55 | steep = 1; 56 | else 57 | steep = 0; 58 | 59 | if(steep) 60 | { 61 | tmp = x0; 62 | x0 = y0; 63 | y0 = tmp; 64 | 65 | tmp = x1; 66 | x1 = y1; 67 | y1 = tmp; 68 | } 69 | 70 | deltax = abs(x1-x0); 71 | deltay = abs(y1-y0); 72 | error = 0; 73 | deltaerr = deltay; 74 | 75 | x = x0; 76 | y = y0; 77 | 78 | if(x0 < x1) 79 | xstep = 1; 80 | else 81 | xstep = -1; 82 | if(y0 < y1) 83 | ystep = 1; 84 | else 85 | ystep = -1; 86 | 87 | if(steep) 88 | { 89 | if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1) 90 | return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale; 91 | } 92 | else 93 | { 94 | if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1) 95 | return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale; 96 | } 97 | 98 | while(x != (x1 + xstep * 1)) 99 | { 100 | x += xstep; 101 | error += deltaerr; 102 | if(2*error >= deltax) 103 | { 104 | y += ystep; 105 | error -= deltax; 106 | } 107 | 108 | if(steep) 109 | { 110 | if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1) 111 | return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale; 112 | } 113 | else 114 | { 115 | if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1) 116 | return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale; 117 | } 118 | } 119 | return max_range; 120 | } 121 | -------------------------------------------------------------------------------- /src/amcl/map/map_store.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: Global map storage functions 23 | * Author: Andrew Howard 24 | * Date: 6 Feb 2003 25 | * CVS: $Id: map_store.c 2951 2005-08-19 00:48:20Z gerkey $ 26 | **************************************************************************/ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include "amcl/map/map.h" 35 | 36 | 37 | //////////////////////////////////////////////////////////////////////////// 38 | // Load an occupancy grid 39 | int map_load_occ(map_t *map, const char *filename, double scale, int negate) 40 | { 41 | FILE *file; 42 | char magic[3]; 43 | int i, j; 44 | int ch, occ; 45 | int width, height, depth; 46 | map_cell_t *cell; 47 | 48 | // Open file 49 | file = fopen(filename, "r"); 50 | if (file == NULL) 51 | { 52 | fprintf(stderr, "%s: %s\n", strerror(errno), filename); 53 | return -1; 54 | } 55 | 56 | // Read ppm header 57 | 58 | if ((fscanf(file, "%2s \n", magic) != 1) || (strcmp(magic, "P5") != 0)) 59 | { 60 | fprintf(stderr, "incorrect image format; must be PGM/binary"); 61 | fclose(file); 62 | return -1; 63 | } 64 | 65 | // Ignore comments 66 | while ((ch = fgetc(file)) == '#') 67 | while (fgetc(file) != '\n'); 68 | ungetc(ch, file); 69 | 70 | // Read image dimensions 71 | if(fscanf(file, " %d %d \n %d \n", &width, &height, &depth) != 3) 72 | { 73 | fprintf(stderr, "Failed ot read image dimensions"); 74 | return -1; 75 | } 76 | 77 | // Allocate space in the map 78 | if (map->cells == NULL) 79 | { 80 | map->scale = scale; 81 | map->size_x = width; 82 | map->size_y = height; 83 | map->cells = calloc(width * height, sizeof(map->cells[0])); 84 | } 85 | else 86 | { 87 | if (width != map->size_x || height != map->size_y) 88 | { 89 | //PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions"); 90 | return -1; 91 | } 92 | } 93 | 94 | // Read in the image 95 | for (j = height - 1; j >= 0; j--) 96 | { 97 | for (i = 0; i < width; i++) 98 | { 99 | ch = fgetc(file); 100 | 101 | // Black-on-white images 102 | if (!negate) 103 | { 104 | if (ch < depth / 4) 105 | occ = +1; 106 | else if (ch > 3 * depth / 4) 107 | occ = -1; 108 | else 109 | occ = 0; 110 | } 111 | 112 | // White-on-black images 113 | else 114 | { 115 | if (ch < depth / 4) 116 | occ = -1; 117 | else if (ch > 3 * depth / 4) 118 | occ = +1; 119 | else 120 | occ = 0; 121 | } 122 | 123 | if (!MAP_VALID(map, i, j)) 124 | continue; 125 | cell = map->cells + MAP_INDEX(map, i, j); 126 | cell->occ_state = occ; 127 | } 128 | } 129 | 130 | fclose(file); 131 | 132 | return 0; 133 | } 134 | 135 | 136 | //////////////////////////////////////////////////////////////////////////// 137 | // Load a wifi signal strength map 138 | /* 139 | int map_load_wifi(map_t *map, const char *filename, int index) 140 | { 141 | FILE *file; 142 | char magic[3]; 143 | int i, j; 144 | int ch, level; 145 | int width, height, depth; 146 | map_cell_t *cell; 147 | 148 | // Open file 149 | file = fopen(filename, "r"); 150 | if (file == NULL) 151 | { 152 | fprintf(stderr, "%s: %s\n", strerror(errno), filename); 153 | return -1; 154 | } 155 | 156 | // Read ppm header 157 | fscanf(file, "%10s \n", magic); 158 | if (strcmp(magic, "P5") != 0) 159 | { 160 | fprintf(stderr, "incorrect image format; must be PGM/binary"); 161 | return -1; 162 | } 163 | 164 | // Ignore comments 165 | while ((ch = fgetc(file)) == '#') 166 | while (fgetc(file) != '\n'); 167 | ungetc(ch, file); 168 | 169 | // Read image dimensions 170 | fscanf(file, " %d %d \n %d \n", &width, &height, &depth); 171 | 172 | // Allocate space in the map 173 | if (map->cells == NULL) 174 | { 175 | map->size_x = width; 176 | map->size_y = height; 177 | map->cells = calloc(width * height, sizeof(map->cells[0])); 178 | } 179 | else 180 | { 181 | if (width != map->size_x || height != map->size_y) 182 | { 183 | //PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions"); 184 | return -1; 185 | } 186 | } 187 | 188 | // Read in the image 189 | for (j = height - 1; j >= 0; j--) 190 | { 191 | for (i = 0; i < width; i++) 192 | { 193 | ch = fgetc(file); 194 | 195 | if (!MAP_VALID(map, i, j)) 196 | continue; 197 | 198 | if (ch == 0) 199 | level = 0; 200 | else 201 | level = ch * 100 / 255 - 100; 202 | 203 | cell = map->cells + MAP_INDEX(map, i, j); 204 | cell->wifi_levels[index] = level; 205 | } 206 | } 207 | 208 | fclose(file); 209 | 210 | return 0; 211 | } 212 | */ 213 | 214 | 215 | 216 | -------------------------------------------------------------------------------- /src/amcl/pf/eig3.c: -------------------------------------------------------------------------------- 1 | 2 | /* Eigen decomposition code for symmetric 3x3 matrices, copied from the public 3 | domain Java Matrix library JAMA. */ 4 | 5 | #include 6 | 7 | #ifndef MAX 8 | #define MAX(a, b) ((a)>(b)?(a):(b)) 9 | #endif 10 | 11 | #ifdef _MSC_VER 12 | #define n 3 13 | #else 14 | static int n = 3; 15 | #endif 16 | 17 | static double hypot2(double x, double y) { 18 | return sqrt(x*x+y*y); 19 | } 20 | 21 | // Symmetric Householder reduction to tridiagonal form. 22 | 23 | static void tred2(double V[n][n], double d[n], double e[n]) { 24 | 25 | // This is derived from the Algol procedures tred2 by 26 | // Bowdler, Martin, Reinsch, and Wilkinson, Handbook for 27 | // Auto. Comp., Vol.ii-Linear Algebra, and the corresponding 28 | // Fortran subroutine in EISPACK. 29 | 30 | int i,j,k; 31 | double f,g,h,hh; 32 | for (j = 0; j < n; j++) { 33 | d[j] = V[n-1][j]; 34 | } 35 | 36 | // Householder reduction to tridiagonal form. 37 | 38 | for (i = n-1; i > 0; i--) { 39 | 40 | // Scale to avoid under/overflow. 41 | 42 | double scale = 0.0; 43 | double h = 0.0; 44 | for (k = 0; k < i; k++) { 45 | scale = scale + fabs(d[k]); 46 | } 47 | if (scale == 0.0) { 48 | e[i] = d[i-1]; 49 | for (j = 0; j < i; j++) { 50 | d[j] = V[i-1][j]; 51 | V[i][j] = 0.0; 52 | V[j][i] = 0.0; 53 | } 54 | } else { 55 | 56 | // Generate Householder vector. 57 | 58 | for (k = 0; k < i; k++) { 59 | d[k] /= scale; 60 | h += d[k] * d[k]; 61 | } 62 | f = d[i-1]; 63 | g = sqrt(h); 64 | if (f > 0) { 65 | g = -g; 66 | } 67 | e[i] = scale * g; 68 | h = h - f * g; 69 | d[i-1] = f - g; 70 | for (j = 0; j < i; j++) { 71 | e[j] = 0.0; 72 | } 73 | 74 | // Apply similarity transformation to remaining columns. 75 | 76 | for (j = 0; j < i; j++) { 77 | f = d[j]; 78 | V[j][i] = f; 79 | g = e[j] + V[j][j] * f; 80 | for (k = j+1; k <= i-1; k++) { 81 | g += V[k][j] * d[k]; 82 | e[k] += V[k][j] * f; 83 | } 84 | e[j] = g; 85 | } 86 | f = 0.0; 87 | for (j = 0; j < i; j++) { 88 | e[j] /= h; 89 | f += e[j] * d[j]; 90 | } 91 | hh = f / (h + h); 92 | for (j = 0; j < i; j++) { 93 | e[j] -= hh * d[j]; 94 | } 95 | for (j = 0; j < i; j++) { 96 | f = d[j]; 97 | g = e[j]; 98 | for (k = j; k <= i-1; k++) { 99 | V[k][j] -= (f * e[k] + g * d[k]); 100 | } 101 | d[j] = V[i-1][j]; 102 | V[i][j] = 0.0; 103 | } 104 | } 105 | d[i] = h; 106 | } 107 | 108 | // Accumulate transformations. 109 | 110 | for (i = 0; i < n-1; i++) { 111 | V[n-1][i] = V[i][i]; 112 | V[i][i] = 1.0; 113 | h = d[i+1]; 114 | if (h != 0.0) { 115 | for (k = 0; k <= i; k++) { 116 | d[k] = V[k][i+1] / h; 117 | } 118 | for (j = 0; j <= i; j++) { 119 | g = 0.0; 120 | for (k = 0; k <= i; k++) { 121 | g += V[k][i+1] * V[k][j]; 122 | } 123 | for (k = 0; k <= i; k++) { 124 | V[k][j] -= g * d[k]; 125 | } 126 | } 127 | } 128 | for (k = 0; k <= i; k++) { 129 | V[k][i+1] = 0.0; 130 | } 131 | } 132 | for (j = 0; j < n; j++) { 133 | d[j] = V[n-1][j]; 134 | V[n-1][j] = 0.0; 135 | } 136 | V[n-1][n-1] = 1.0; 137 | e[0] = 0.0; 138 | } 139 | 140 | // Symmetric tridiagonal QL algorithm. 141 | 142 | static void tql2(double V[n][n], double d[n], double e[n]) { 143 | 144 | // This is derived from the Algol procedures tql2, by 145 | // Bowdler, Martin, Reinsch, and Wilkinson, Handbook for 146 | // Auto. Comp., Vol.ii-Linear Algebra, and the corresponding 147 | // Fortran subroutine in EISPACK. 148 | 149 | int i,j,m,l,k; 150 | double g,p,r,dl1,h,f,tst1,eps; 151 | double c,c2,c3,el1,s,s2; 152 | 153 | for (i = 1; i < n; i++) { 154 | e[i-1] = e[i]; 155 | } 156 | e[n-1] = 0.0; 157 | 158 | f = 0.0; 159 | tst1 = 0.0; 160 | eps = pow(2.0,-52.0); 161 | for (l = 0; l < n; l++) { 162 | 163 | // Find small subdiagonal element 164 | 165 | tst1 = MAX(tst1,fabs(d[l]) + fabs(e[l])); 166 | m = l; 167 | while (m < n) { 168 | if (fabs(e[m]) <= eps*tst1) { 169 | break; 170 | } 171 | m++; 172 | } 173 | 174 | // If m == l, d[l] is an eigenvalue, 175 | // otherwise, iterate. 176 | 177 | if (m > l) { 178 | int iter = 0; 179 | do { 180 | iter = iter + 1; // (Could check iteration count here.) 181 | 182 | // Compute implicit shift 183 | 184 | g = d[l]; 185 | p = (d[l+1] - g) / (2.0 * e[l]); 186 | r = hypot2(p,1.0); 187 | if (p < 0) { 188 | r = -r; 189 | } 190 | d[l] = e[l] / (p + r); 191 | d[l+1] = e[l] * (p + r); 192 | dl1 = d[l+1]; 193 | h = g - d[l]; 194 | for (i = l+2; i < n; i++) { 195 | d[i] -= h; 196 | } 197 | f = f + h; 198 | 199 | // Implicit QL transformation. 200 | 201 | p = d[m]; 202 | c = 1.0; 203 | c2 = c; 204 | c3 = c; 205 | el1 = e[l+1]; 206 | s = 0.0; 207 | s2 = 0.0; 208 | for (i = m-1; i >= l; i--) { 209 | c3 = c2; 210 | c2 = c; 211 | s2 = s; 212 | g = c * e[i]; 213 | h = c * p; 214 | r = hypot2(p,e[i]); 215 | e[i+1] = s * r; 216 | s = e[i] / r; 217 | c = p / r; 218 | p = c * d[i] - s * g; 219 | d[i+1] = h + s * (c * g + s * d[i]); 220 | 221 | // Accumulate transformation. 222 | 223 | for (k = 0; k < n; k++) { 224 | h = V[k][i+1]; 225 | V[k][i+1] = s * V[k][i] + c * h; 226 | V[k][i] = c * V[k][i] - s * h; 227 | } 228 | } 229 | p = -s * s2 * c3 * el1 * e[l] / dl1; 230 | e[l] = s * p; 231 | d[l] = c * p; 232 | 233 | // Check for convergence. 234 | 235 | } while (fabs(e[l]) > eps*tst1); 236 | } 237 | d[l] = d[l] + f; 238 | e[l] = 0.0; 239 | } 240 | 241 | // Sort eigenvalues and corresponding vectors. 242 | 243 | for (i = 0; i < n-1; i++) { 244 | k = i; 245 | p = d[i]; 246 | for (j = i+1; j < n; j++) { 247 | if (d[j] < p) { 248 | k = j; 249 | p = d[j]; 250 | } 251 | } 252 | if (k != i) { 253 | d[k] = d[i]; 254 | d[i] = p; 255 | for (j = 0; j < n; j++) { 256 | p = V[j][i]; 257 | V[j][i] = V[j][k]; 258 | V[j][k] = p; 259 | } 260 | } 261 | } 262 | } 263 | 264 | void eigen_decomposition(double A[n][n], double V[n][n], double d[n]) { 265 | int i,j; 266 | double e[n]; 267 | for (i = 0; i < n; i++) { 268 | for (j = 0; j < n; j++) { 269 | V[i][j] = A[i][j]; 270 | } 271 | } 272 | tred2(V, d, e); 273 | tql2(V, d, e); 274 | } 275 | -------------------------------------------------------------------------------- /src/amcl/pf/pf.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: Simple particle filter for localization. 23 | * Author: Andrew Howard 24 | * Date: 10 Dec 2002 25 | * CVS: $Id: pf.c 6345 2008-04-17 01:36:39Z gerkey $ 26 | *************************************************************************/ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include "amcl/pf/pf.h" 34 | #include "amcl/pf/pf_pdf.h" 35 | #include "amcl/pf/pf_kdtree.h" 36 | #include "portable_utils.hpp" 37 | 38 | 39 | // Compute the required number of samples, given that there are k bins 40 | // with samples in them. 41 | static int pf_resample_limit(pf_t *pf, int k); 42 | 43 | 44 | 45 | // Create a new filter 46 | pf_t *pf_alloc(int min_samples, int max_samples, 47 | double alpha_slow, double alpha_fast, 48 | pf_init_model_fn_t random_pose_fn, void *random_pose_data) 49 | { 50 | int i, j; 51 | pf_t *pf; 52 | pf_sample_set_t *set; 53 | pf_sample_t *sample; 54 | 55 | srand48(time(NULL)); 56 | 57 | pf = (pf_t *)calloc(1, sizeof(pf_t)); 58 | 59 | pf->random_pose_fn = random_pose_fn; 60 | pf->random_pose_data = random_pose_data; 61 | 62 | pf->min_samples = min_samples; 63 | pf->max_samples = max_samples; 64 | 65 | // Control parameters for the population size calculation. [err] is 66 | // the max error between the true distribution and the estimated 67 | // distribution. [z] is the upper standard normal quantile for (1 - 68 | // p), where p is the probability that the error on the estimated 69 | // distrubition will be less than [err]. 70 | pf->pop_err = 0.01; 71 | pf->pop_z = 3; 72 | pf->dist_threshold = 0.5; 73 | 74 | pf->current_set = 0; 75 | for (j = 0; j < 2; j++) 76 | { 77 | set = pf->sets + j; 78 | 79 | set->sample_count = max_samples; 80 | set->samples = (pf_sample_t *)calloc(max_samples, sizeof(pf_sample_t)); 81 | 82 | for (i = 0; i < set->sample_count; i++) 83 | { 84 | sample = set->samples + i; 85 | sample->pose.v[0] = 0.0; 86 | sample->pose.v[1] = 0.0; 87 | sample->pose.v[2] = 0.0; 88 | sample->weight = 1.0 / max_samples; 89 | } 90 | 91 | // HACK: is 3 times max_samples enough? 92 | set->kdtree = pf_kdtree_alloc(3 * max_samples); 93 | 94 | set->cluster_count = 0; 95 | set->cluster_max_count = max_samples; 96 | set->clusters = (pf_cluster_t *)calloc(set->cluster_max_count, sizeof(pf_cluster_t)); 97 | 98 | set->mean = pf_vector_zero(); 99 | set->cov = pf_matrix_zero(); 100 | } 101 | 102 | pf->w_slow = 0.0; 103 | pf->w_fast = 0.0; 104 | 105 | pf->alpha_slow = alpha_slow; 106 | pf->alpha_fast = alpha_fast; 107 | 108 | //set converged to 0 109 | pf_init_converged(pf); 110 | 111 | return pf; 112 | } 113 | 114 | // Free an existing filter 115 | void pf_free(pf_t *pf) 116 | { 117 | int i; 118 | 119 | for (i = 0; i < 2; i++) 120 | { 121 | free(pf->sets[i].clusters); 122 | pf_kdtree_free(pf->sets[i].kdtree); 123 | free(pf->sets[i].samples); 124 | } 125 | free(pf); 126 | 127 | return; 128 | } 129 | 130 | // Initialize the filter using a guassian 131 | void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov) 132 | { 133 | int i; 134 | pf_sample_set_t *set; 135 | pf_sample_t *sample; 136 | pf_pdf_gaussian_t *pdf; 137 | 138 | set = pf->sets + pf->current_set; 139 | 140 | // Create the kd tree for adaptive sampling 141 | pf_kdtree_clear(set->kdtree); 142 | 143 | set->sample_count = pf->max_samples; 144 | 145 | pdf = pf_pdf_gaussian_alloc(mean, cov); 146 | 147 | // Compute the new sample poses 148 | for (i = 0; i < set->sample_count; i++) 149 | { 150 | sample = set->samples + i; 151 | sample->weight = 1.0 / pf->max_samples; 152 | sample->pose = pf_pdf_gaussian_sample(pdf); 153 | 154 | // Add sample to histogram 155 | pf_kdtree_insert(set->kdtree, sample->pose, sample->weight); 156 | } 157 | 158 | pf->w_slow = pf->w_fast = 0.0; 159 | 160 | pf_pdf_gaussian_free(pdf); 161 | 162 | // Re-compute cluster statistics 163 | pf_cluster_stats(pf, set); 164 | 165 | //set converged to 0 166 | pf_init_converged(pf); 167 | 168 | return; 169 | } 170 | 171 | 172 | // Initialize the filter using some model 173 | void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data) 174 | { 175 | int i; 176 | pf_sample_set_t *set; 177 | pf_sample_t *sample; 178 | 179 | set = pf->sets + pf->current_set; 180 | 181 | // Create the kd tree for adaptive sampling 182 | pf_kdtree_clear(set->kdtree); 183 | 184 | set->sample_count = pf->max_samples; 185 | 186 | // Compute the new sample poses 187 | for (i = 0; i < set->sample_count; i++) 188 | { 189 | sample = set->samples + i; 190 | sample->weight = 1.0 / pf->max_samples; 191 | sample->pose = (*init_fn) (init_data); 192 | 193 | // Add sample to histogram 194 | pf_kdtree_insert(set->kdtree, sample->pose, sample->weight); 195 | } 196 | 197 | pf->w_slow = pf->w_fast = 0.0; 198 | 199 | // Re-compute cluster statistics 200 | pf_cluster_stats(pf, set); 201 | 202 | //set converged to 0 203 | pf_init_converged(pf); 204 | 205 | return; 206 | } 207 | 208 | void pf_init_converged(pf_t *pf){ 209 | pf_sample_set_t *set; 210 | set = pf->sets + pf->current_set; 211 | set->converged = 0; 212 | pf->converged = 0; 213 | } 214 | 215 | int pf_update_converged(pf_t *pf) 216 | { 217 | int i; 218 | pf_sample_set_t *set; 219 | pf_sample_t *sample; 220 | double total; 221 | 222 | set = pf->sets + pf->current_set; 223 | double mean_x = 0, mean_y = 0; 224 | 225 | for (i = 0; i < set->sample_count; i++){ 226 | sample = set->samples + i; 227 | 228 | mean_x += sample->pose.v[0]; 229 | mean_y += sample->pose.v[1]; 230 | } 231 | mean_x /= set->sample_count; 232 | mean_y /= set->sample_count; 233 | 234 | for (i = 0; i < set->sample_count; i++){ 235 | sample = set->samples + i; 236 | if(fabs(sample->pose.v[0] - mean_x) > pf->dist_threshold || 237 | fabs(sample->pose.v[1] - mean_y) > pf->dist_threshold){ 238 | set->converged = 0; 239 | pf->converged = 0; 240 | return 0; 241 | } 242 | } 243 | set->converged = 1; 244 | pf->converged = 1; 245 | return 1; 246 | } 247 | 248 | // Update the filter with some new action 249 | void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data) 250 | { 251 | pf_sample_set_t *set; 252 | 253 | set = pf->sets + pf->current_set; 254 | 255 | (*action_fn) (action_data, set); 256 | 257 | return; 258 | } 259 | 260 | 261 | #include 262 | // Update the filter with some new sensor observation 263 | void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data) 264 | { 265 | int i; 266 | pf_sample_set_t *set; 267 | pf_sample_t *sample; 268 | double total; 269 | 270 | set = pf->sets + pf->current_set; 271 | 272 | // Compute the sample weights 273 | total = (*sensor_fn) (sensor_data, set); 274 | 275 | if (total > 0.0) 276 | { 277 | // Normalize weights 278 | double w_avg=0.0; 279 | for (i = 0; i < set->sample_count; i++) 280 | { 281 | sample = set->samples + i; 282 | w_avg += sample->weight; 283 | sample->weight /= total; 284 | } 285 | // Update running averages of likelihood of samples (Prob Rob p258) 286 | w_avg /= set->sample_count; 287 | if(pf->w_slow == 0.0) 288 | pf->w_slow = w_avg; 289 | else 290 | pf->w_slow += pf->alpha_slow * (w_avg - pf->w_slow); 291 | if(pf->w_fast == 0.0) 292 | pf->w_fast = w_avg; 293 | else 294 | pf->w_fast += pf->alpha_fast * (w_avg - pf->w_fast); 295 | //printf("w_avg: %e slow: %e fast: %e\n", 296 | //w_avg, pf->w_slow, pf->w_fast); 297 | } 298 | else 299 | { 300 | // Handle zero total 301 | for (i = 0; i < set->sample_count; i++) 302 | { 303 | sample = set->samples + i; 304 | sample->weight = 1.0 / set->sample_count; 305 | } 306 | } 307 | 308 | return; 309 | } 310 | 311 | 312 | // Resample the distribution 313 | void pf_update_resample(pf_t *pf) 314 | { 315 | int i; 316 | double total; 317 | pf_sample_set_t *set_a, *set_b; 318 | pf_sample_t *sample_a, *sample_b; 319 | 320 | //double r,c,U; 321 | //int m; 322 | //double count_inv; 323 | double* c; 324 | 325 | double w_diff; 326 | 327 | set_a = pf->sets + pf->current_set; 328 | set_b = pf->sets + (pf->current_set + 1) % 2; 329 | 330 | // Build up cumulative probability table for resampling. 331 | // TODO: Replace this with a more efficient procedure 332 | // (e.g., http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html) 333 | c = (double*)malloc(sizeof(double)*(set_a->sample_count+1)); 334 | c[0] = 0.0; 335 | for (i=0;isample_count;i++) 336 | { 337 | c[i+1] = c[i] + set_a->samples[i].weight; 338 | } 339 | 340 | // Create the kd tree for adaptive sampling 341 | pf_kdtree_clear(set_b->kdtree); 342 | 343 | // Draw samples from set a to create set b. 344 | total = 0; 345 | set_b->sample_count = 0; 346 | 347 | w_diff = 1.0 - pf->w_fast / pf->w_slow; 348 | if (w_diff < 0.0) 349 | { 350 | w_diff = 0.0; 351 | } 352 | //printf("w_diff: %9.6f\n", w_diff); 353 | 354 | // Can't (easily) combine low-variance sampler with KLD adaptive 355 | // sampling, so we'll take the more traditional route. 356 | /* 357 | // Low-variance resampler, taken from Probabilistic Robotics, p110 358 | count_inv = 1.0/set_a->sample_count; 359 | r = drand48() * count_inv; 360 | c = set_a->samples[0].weight; 361 | i = 0; 362 | m = 0; 363 | */ 364 | 365 | while (set_b->sample_count < pf->max_samples) 366 | { 367 | sample_b = set_b->samples + set_b->sample_count++; 368 | 369 | if (drand48() < w_diff) 370 | { 371 | sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data); 372 | } 373 | else 374 | { 375 | // Can't (easily) combine low-variance sampler with KLD adaptive 376 | // sampling, so we'll take the more traditional route. 377 | /* 378 | // Low-variance resampler, taken from Probabilistic Robotics, p110 379 | U = r + m * count_inv; 380 | while(U>c) 381 | { 382 | i++; 383 | // Handle wrap-around by resetting counters and picking a new random 384 | // number 385 | if(i >= set_a->sample_count) 386 | { 387 | r = drand48() * count_inv; 388 | c = set_a->samples[0].weight; 389 | i = 0; 390 | m = 0; 391 | U = r + m * count_inv; 392 | continue; 393 | } 394 | c += set_a->samples[i].weight; 395 | } 396 | m++; 397 | */ 398 | 399 | // Naive discrete event sampler 400 | double r; 401 | r = drand48(); 402 | for (i=0;isample_count;i++) 403 | { 404 | if ((c[i] <= r) && (r < c[i+1])) 405 | { 406 | break; 407 | } 408 | } 409 | assert(isample_count); 410 | 411 | sample_a = set_a->samples + i; 412 | 413 | assert(sample_a->weight > 0); 414 | 415 | // Add sample to list 416 | sample_b->pose = sample_a->pose; 417 | } 418 | 419 | sample_b->weight = 1.0; 420 | total += sample_b->weight; 421 | 422 | // Add sample to histogram 423 | pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight); 424 | 425 | // See if we have enough samples yet 426 | if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count)) 427 | { 428 | break; 429 | } 430 | } 431 | 432 | // Reset averages, to avoid spiraling off into complete randomness. 433 | if(w_diff > 0.0) 434 | { 435 | pf->w_slow = pf->w_fast = 0.0; 436 | } 437 | 438 | //fprintf(stderr, "\n\n"); 439 | 440 | // Normalize weights 441 | for (i = 0; i < set_b->sample_count; i++) 442 | { 443 | sample_b = set_b->samples + i; 444 | sample_b->weight /= total; 445 | } 446 | 447 | // Re-compute cluster statistics 448 | pf_cluster_stats(pf, set_b); 449 | 450 | // Use the newly created sample set 451 | pf->current_set = (pf->current_set + 1) % 2; 452 | 453 | pf_update_converged(pf); 454 | 455 | free(c); 456 | return; 457 | } 458 | 459 | 460 | // Compute the required number of samples, given that there are k bins 461 | // with samples in them. This is taken directly from Fox et al. 462 | int pf_resample_limit(pf_t *pf, int k) 463 | { 464 | double a, b, c, x; 465 | int n; 466 | 467 | if (k <= 1) 468 | return pf->max_samples; 469 | 470 | a = 1; 471 | b = 2 / (9 * ((double) k - 1)); 472 | c = sqrt(2 / (9 * ((double) k - 1))) * pf->pop_z; 473 | x = a - b + c; 474 | 475 | n = (int) ceil((k - 1) / (2 * pf->pop_err) * x * x * x); 476 | 477 | if (n < pf->min_samples) 478 | return pf->min_samples; 479 | if (n > pf->max_samples) 480 | return pf->max_samples; 481 | 482 | return n; 483 | } 484 | 485 | 486 | // Re-compute the cluster statistics for a sample set 487 | void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set) 488 | { 489 | int i, j, k, cidx; 490 | pf_sample_t *sample; 491 | pf_cluster_t *cluster; 492 | 493 | // Workspace 494 | double m[4], c[2][2]; 495 | size_t count; 496 | double weight; 497 | 498 | // Cluster the samples 499 | pf_kdtree_cluster(set->kdtree); 500 | 501 | // Initialize cluster stats 502 | set->cluster_count = 0; 503 | 504 | for (i = 0; i < set->cluster_max_count; i++) 505 | { 506 | cluster = set->clusters + i; 507 | cluster->count = 0; 508 | cluster->weight = 0; 509 | cluster->mean = pf_vector_zero(); 510 | cluster->cov = pf_matrix_zero(); 511 | 512 | for (j = 0; j < 4; j++) 513 | cluster->m[j] = 0.0; 514 | for (j = 0; j < 2; j++) 515 | for (k = 0; k < 2; k++) 516 | cluster->c[j][k] = 0.0; 517 | } 518 | 519 | // Initialize overall filter stats 520 | count = 0; 521 | weight = 0.0; 522 | set->mean = pf_vector_zero(); 523 | set->cov = pf_matrix_zero(); 524 | for (j = 0; j < 4; j++) 525 | m[j] = 0.0; 526 | for (j = 0; j < 2; j++) 527 | for (k = 0; k < 2; k++) 528 | c[j][k] = 0.0; 529 | 530 | // Compute cluster stats 531 | for (i = 0; i < set->sample_count; i++) 532 | { 533 | sample = set->samples + i; 534 | 535 | //printf("%d %f %f %f\n", i, sample->pose.v[0], sample->pose.v[1], sample->pose.v[2]); 536 | 537 | // Get the cluster label for this sample 538 | cidx = pf_kdtree_get_cluster(set->kdtree, sample->pose); 539 | assert(cidx >= 0); 540 | if (cidx >= set->cluster_max_count) 541 | continue; 542 | if (cidx + 1 > set->cluster_count) 543 | set->cluster_count = cidx + 1; 544 | 545 | cluster = set->clusters + cidx; 546 | 547 | cluster->count += 1; 548 | cluster->weight += sample->weight; 549 | 550 | count += 1; 551 | weight += sample->weight; 552 | 553 | // Compute mean 554 | cluster->m[0] += sample->weight * sample->pose.v[0]; 555 | cluster->m[1] += sample->weight * sample->pose.v[1]; 556 | cluster->m[2] += sample->weight * cos(sample->pose.v[2]); 557 | cluster->m[3] += sample->weight * sin(sample->pose.v[2]); 558 | 559 | m[0] += sample->weight * sample->pose.v[0]; 560 | m[1] += sample->weight * sample->pose.v[1]; 561 | m[2] += sample->weight * cos(sample->pose.v[2]); 562 | m[3] += sample->weight * sin(sample->pose.v[2]); 563 | 564 | // Compute covariance in linear components 565 | for (j = 0; j < 2; j++) 566 | for (k = 0; k < 2; k++) 567 | { 568 | cluster->c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k]; 569 | c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k]; 570 | } 571 | } 572 | 573 | // Normalize 574 | for (i = 0; i < set->cluster_count; i++) 575 | { 576 | cluster = set->clusters + i; 577 | 578 | cluster->mean.v[0] = cluster->m[0] / cluster->weight; 579 | cluster->mean.v[1] = cluster->m[1] / cluster->weight; 580 | cluster->mean.v[2] = atan2(cluster->m[3], cluster->m[2]); 581 | 582 | cluster->cov = pf_matrix_zero(); 583 | 584 | // Covariance in linear components 585 | for (j = 0; j < 2; j++) 586 | for (k = 0; k < 2; k++) 587 | cluster->cov.m[j][k] = cluster->c[j][k] / cluster->weight - 588 | cluster->mean.v[j] * cluster->mean.v[k]; 589 | 590 | // Covariance in angular components; I think this is the correct 591 | // formula for circular statistics. 592 | cluster->cov.m[2][2] = -2 * log(sqrt(cluster->m[2] * cluster->m[2] + 593 | cluster->m[3] * cluster->m[3])); 594 | 595 | //printf("cluster %d %d %f (%f %f %f)\n", i, cluster->count, cluster->weight, 596 | //cluster->mean.v[0], cluster->mean.v[1], cluster->mean.v[2]); 597 | //pf_matrix_fprintf(cluster->cov, stdout, "%e"); 598 | } 599 | 600 | // Compute overall filter stats 601 | set->mean.v[0] = m[0] / weight; 602 | set->mean.v[1] = m[1] / weight; 603 | set->mean.v[2] = atan2(m[3], m[2]); 604 | 605 | // Covariance in linear components 606 | for (j = 0; j < 2; j++) 607 | for (k = 0; k < 2; k++) 608 | set->cov.m[j][k] = c[j][k] / weight - set->mean.v[j] * set->mean.v[k]; 609 | 610 | // Covariance in angular components; I think this is the correct 611 | // formula for circular statistics. 612 | set->cov.m[2][2] = -2 * log(sqrt(m[2] * m[2] + m[3] * m[3])); 613 | 614 | return; 615 | } 616 | 617 | 618 | // Compute the CEP statistics (mean and variance). 619 | void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var) 620 | { 621 | int i; 622 | double mn, mx, my, mrr; 623 | pf_sample_set_t *set; 624 | pf_sample_t *sample; 625 | 626 | set = pf->sets + pf->current_set; 627 | 628 | mn = 0.0; 629 | mx = 0.0; 630 | my = 0.0; 631 | mrr = 0.0; 632 | 633 | for (i = 0; i < set->sample_count; i++) 634 | { 635 | sample = set->samples + i; 636 | 637 | mn += sample->weight; 638 | mx += sample->weight * sample->pose.v[0]; 639 | my += sample->weight * sample->pose.v[1]; 640 | mrr += sample->weight * sample->pose.v[0] * sample->pose.v[0]; 641 | mrr += sample->weight * sample->pose.v[1] * sample->pose.v[1]; 642 | } 643 | 644 | mean->v[0] = mx / mn; 645 | mean->v[1] = my / mn; 646 | mean->v[2] = 0.0; 647 | 648 | *var = mrr / mn - (mx * mx / (mn * mn) + my * my / (mn * mn)); 649 | 650 | return; 651 | } 652 | 653 | 654 | // Get the statistics for a particular cluster. 655 | int pf_get_cluster_stats(pf_t *pf, int clabel, double *weight, 656 | pf_vector_t *mean, pf_matrix_t *cov) 657 | { 658 | pf_sample_set_t *set; 659 | pf_cluster_t *cluster; 660 | 661 | set = pf->sets + pf->current_set; 662 | 663 | if (clabel >= set->cluster_count) 664 | return 0; 665 | cluster = set->clusters + clabel; 666 | 667 | *weight = cluster->weight; 668 | *mean = cluster->mean; 669 | *cov = cluster->cov; 670 | 671 | return 1; 672 | } 673 | 674 | 675 | -------------------------------------------------------------------------------- /src/amcl/pf/pf_draw.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: Particle filter; drawing routines 23 | * Author: Andrew Howard 24 | * Date: 10 Dec 2002 25 | * CVS: $Id: pf_draw.c 7057 2008-10-02 00:44:06Z gbiggs $ 26 | *************************************************************************/ 27 | 28 | #ifdef INCLUDE_RTKGUI 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | 35 | #include "rtk.h" 36 | 37 | #include "pf.h" 38 | #include "pf_pdf.h" 39 | #include "pf_kdtree.h" 40 | 41 | 42 | // Draw the statistics 43 | void pf_draw_statistics(pf_t *pf, rtk_fig_t *fig); 44 | 45 | 46 | // Draw the sample set 47 | void pf_draw_samples(pf_t *pf, rtk_fig_t *fig, int max_samples) 48 | { 49 | int i; 50 | double px, py, pa; 51 | pf_sample_set_t *set; 52 | pf_sample_t *sample; 53 | 54 | set = pf->sets + pf->current_set; 55 | max_samples = MIN(max_samples, set->sample_count); 56 | 57 | for (i = 0; i < max_samples; i++) 58 | { 59 | sample = set->samples + i; 60 | 61 | px = sample->pose.v[0]; 62 | py = sample->pose.v[1]; 63 | pa = sample->pose.v[2]; 64 | 65 | //printf("%f %f\n", px, py); 66 | 67 | rtk_fig_point(fig, px, py); 68 | rtk_fig_arrow(fig, px, py, pa, 0.1, 0.02); 69 | //rtk_fig_rectangle(fig, px, py, 0, 0.1, 0.1, 0); 70 | } 71 | 72 | return; 73 | } 74 | 75 | 76 | // Draw the hitogram (kd tree) 77 | void pf_draw_hist(pf_t *pf, rtk_fig_t *fig) 78 | { 79 | pf_sample_set_t *set; 80 | 81 | set = pf->sets + pf->current_set; 82 | 83 | rtk_fig_color(fig, 0.0, 0.0, 1.0); 84 | pf_kdtree_draw(set->kdtree, fig); 85 | 86 | return; 87 | } 88 | 89 | 90 | // Draw the CEP statistics 91 | void pf_draw_cep_stats(pf_t *pf, rtk_fig_t *fig) 92 | { 93 | pf_vector_t mean; 94 | double var; 95 | 96 | pf_get_cep_stats(pf, &mean, &var); 97 | var = sqrt(var); 98 | 99 | rtk_fig_color(fig, 0, 0, 1); 100 | rtk_fig_ellipse(fig, mean.v[0], mean.v[1], mean.v[2], 3 * var, 3 * var, 0); 101 | 102 | return; 103 | } 104 | 105 | 106 | // Draw the cluster statistics 107 | void pf_draw_cluster_stats(pf_t *pf, rtk_fig_t *fig) 108 | { 109 | int i; 110 | pf_cluster_t *cluster; 111 | pf_sample_set_t *set; 112 | pf_vector_t mean; 113 | pf_matrix_t cov; 114 | pf_matrix_t r, d; 115 | double weight, o, d1, d2; 116 | 117 | set = pf->sets + pf->current_set; 118 | 119 | for (i = 0; i < set->cluster_count; i++) 120 | { 121 | cluster = set->clusters + i; 122 | 123 | weight = cluster->weight; 124 | mean = cluster->mean; 125 | cov = cluster->cov; 126 | 127 | // Compute unitary representation S = R D R^T 128 | pf_matrix_unitary(&r, &d, cov); 129 | 130 | /* Debugging 131 | printf("mean = \n"); 132 | pf_vector_fprintf(mean, stdout, "%e"); 133 | printf("cov = \n"); 134 | pf_matrix_fprintf(cov, stdout, "%e"); 135 | printf("r = \n"); 136 | pf_matrix_fprintf(r, stdout, "%e"); 137 | printf("d = \n"); 138 | pf_matrix_fprintf(d, stdout, "%e"); 139 | */ 140 | 141 | // Compute the orientation of the error ellipse (first eigenvector) 142 | o = atan2(r.m[1][0], r.m[0][0]); 143 | d1 = 6 * sqrt(d.m[0][0]); 144 | d2 = 6 * sqrt(d.m[1][1]); 145 | 146 | if (d1 > 1e-3 && d2 > 1e-3) 147 | { 148 | // Draw the error ellipse 149 | rtk_fig_ellipse(fig, mean.v[0], mean.v[1], o, d1, d2, 0); 150 | rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o, d1); 151 | rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o + M_PI / 2, d2); 152 | } 153 | 154 | // Draw a direction indicator 155 | rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2], 0.50, 0.10); 156 | rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] + 3 * sqrt(cov.m[2][2]), 0.50, 0.10); 157 | rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] - 3 * sqrt(cov.m[2][2]), 0.50, 0.10); 158 | } 159 | 160 | return; 161 | } 162 | 163 | #endif 164 | -------------------------------------------------------------------------------- /src/amcl/pf/pf_kdtree.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: kd-tree functions 23 | * Author: Andrew Howard 24 | * Date: 18 Dec 2002 25 | * CVS: $Id: pf_kdtree.c 7057 2008-10-02 00:44:06Z gbiggs $ 26 | *************************************************************************/ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | 34 | #include "amcl/pf/pf_vector.h" 35 | #include "amcl/pf/pf_kdtree.h" 36 | 37 | 38 | // Compare keys to see if they are equal 39 | static int pf_kdtree_equal(pf_kdtree_t *self, int key_a[], int key_b[]); 40 | 41 | // Insert a node into the tree 42 | static pf_kdtree_node_t *pf_kdtree_insert_node(pf_kdtree_t *self, pf_kdtree_node_t *parent, 43 | pf_kdtree_node_t *node, int key[], double value); 44 | 45 | // Recursive node search 46 | static pf_kdtree_node_t *pf_kdtree_find_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int key[]); 47 | 48 | // Recursively label nodes in this cluster 49 | static void pf_kdtree_cluster_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int depth); 50 | 51 | // Recursive node printing 52 | //static void pf_kdtree_print_node(pf_kdtree_t *self, pf_kdtree_node_t *node); 53 | 54 | 55 | #ifdef INCLUDE_RTKGUI 56 | 57 | // Recursively draw nodes 58 | static void pf_kdtree_draw_node(pf_kdtree_t *self, pf_kdtree_node_t *node, rtk_fig_t *fig); 59 | 60 | #endif 61 | 62 | 63 | 64 | //////////////////////////////////////////////////////////////////////////////// 65 | // Create a tree 66 | pf_kdtree_t *pf_kdtree_alloc(int max_size) 67 | { 68 | pf_kdtree_t *self; 69 | 70 | self = calloc(1, sizeof(pf_kdtree_t)); 71 | 72 | self->size[0] = 0.50; 73 | self->size[1] = 0.50; 74 | self->size[2] = (10 * M_PI / 180); 75 | 76 | self->root = NULL; 77 | 78 | self->node_count = 0; 79 | self->node_max_count = max_size; 80 | self->nodes = calloc(self->node_max_count, sizeof(pf_kdtree_node_t)); 81 | 82 | self->leaf_count = 0; 83 | 84 | return self; 85 | } 86 | 87 | 88 | //////////////////////////////////////////////////////////////////////////////// 89 | // Destroy a tree 90 | void pf_kdtree_free(pf_kdtree_t *self) 91 | { 92 | free(self->nodes); 93 | free(self); 94 | return; 95 | } 96 | 97 | 98 | //////////////////////////////////////////////////////////////////////////////// 99 | // Clear all entries from the tree 100 | void pf_kdtree_clear(pf_kdtree_t *self) 101 | { 102 | self->root = NULL; 103 | self->leaf_count = 0; 104 | self->node_count = 0; 105 | 106 | return; 107 | } 108 | 109 | 110 | //////////////////////////////////////////////////////////////////////////////// 111 | // Insert a pose into the tree. 112 | void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value) 113 | { 114 | int key[3]; 115 | 116 | key[0] = floor(pose.v[0] / self->size[0]); 117 | key[1] = floor(pose.v[1] / self->size[1]); 118 | key[2] = floor(pose.v[2] / self->size[2]); 119 | 120 | self->root = pf_kdtree_insert_node(self, NULL, self->root, key, value); 121 | 122 | // Test code 123 | /* 124 | printf("find %d %d %d\n", key[0], key[1], key[2]); 125 | assert(pf_kdtree_find_node(self, self->root, key) != NULL); 126 | 127 | pf_kdtree_print_node(self, self->root); 128 | 129 | printf("\n"); 130 | 131 | for (i = 0; i < self->node_count; i++) 132 | { 133 | node = self->nodes + i; 134 | if (node->leaf) 135 | { 136 | printf("find %d %d %d\n", node->key[0], node->key[1], node->key[2]); 137 | assert(pf_kdtree_find_node(self, self->root, node->key) == node); 138 | } 139 | } 140 | printf("\n\n"); 141 | */ 142 | 143 | return; 144 | } 145 | 146 | 147 | //////////////////////////////////////////////////////////////////////////////// 148 | // Determine the probability estimate for the given pose. TODO: this 149 | // should do a kernel density estimate rather than a simple histogram. 150 | double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose) 151 | { 152 | int key[3]; 153 | pf_kdtree_node_t *node; 154 | 155 | key[0] = floor(pose.v[0] / self->size[0]); 156 | key[1] = floor(pose.v[1] / self->size[1]); 157 | key[2] = floor(pose.v[2] / self->size[2]); 158 | 159 | node = pf_kdtree_find_node(self, self->root, key); 160 | if (node == NULL) 161 | return 0.0; 162 | return node->value; 163 | } 164 | 165 | 166 | //////////////////////////////////////////////////////////////////////////////// 167 | // Determine the cluster label for the given pose 168 | int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose) 169 | { 170 | int key[3]; 171 | pf_kdtree_node_t *node; 172 | 173 | key[0] = floor(pose.v[0] / self->size[0]); 174 | key[1] = floor(pose.v[1] / self->size[1]); 175 | key[2] = floor(pose.v[2] / self->size[2]); 176 | 177 | node = pf_kdtree_find_node(self, self->root, key); 178 | if (node == NULL) 179 | return -1; 180 | return node->cluster; 181 | } 182 | 183 | 184 | //////////////////////////////////////////////////////////////////////////////// 185 | // Compare keys to see if they are equal 186 | int pf_kdtree_equal(pf_kdtree_t *self, int key_a[], int key_b[]) 187 | { 188 | //double a, b; 189 | 190 | if (key_a[0] != key_b[0]) 191 | return 0; 192 | if (key_a[1] != key_b[1]) 193 | return 0; 194 | 195 | if (key_a[2] != key_b[2]) 196 | return 0; 197 | 198 | /* TODO: make this work (pivot selection needs fixing, too) 199 | // Normalize angles 200 | a = key_a[2] * self->size[2]; 201 | a = atan2(sin(a), cos(a)) / self->size[2]; 202 | b = key_b[2] * self->size[2]; 203 | b = atan2(sin(b), cos(b)) / self->size[2]; 204 | 205 | if ((int) a != (int) b) 206 | return 0; 207 | */ 208 | 209 | return 1; 210 | } 211 | 212 | 213 | //////////////////////////////////////////////////////////////////////////////// 214 | // Insert a node into the tree 215 | pf_kdtree_node_t *pf_kdtree_insert_node(pf_kdtree_t *self, pf_kdtree_node_t *parent, 216 | pf_kdtree_node_t *node, int key[], double value) 217 | { 218 | int i; 219 | int split, max_split; 220 | 221 | // If the node doesnt exist yet... 222 | if (node == NULL) 223 | { 224 | assert(self->node_count < self->node_max_count); 225 | node = self->nodes + self->node_count++; 226 | memset(node, 0, sizeof(pf_kdtree_node_t)); 227 | 228 | node->leaf = 1; 229 | 230 | if (parent == NULL) 231 | node->depth = 0; 232 | else 233 | node->depth = parent->depth + 1; 234 | 235 | for (i = 0; i < 3; i++) 236 | node->key[i] = key[i]; 237 | 238 | node->value = value; 239 | self->leaf_count += 1; 240 | } 241 | 242 | // If the node exists, and it is a leaf node... 243 | else if (node->leaf) 244 | { 245 | // If the keys are equal, increment the value 246 | if (pf_kdtree_equal(self, key, node->key)) 247 | { 248 | node->value += value; 249 | } 250 | 251 | // The keys are not equal, so split this node 252 | else 253 | { 254 | // Find the dimension with the largest variance and do a mean 255 | // split 256 | max_split = 0; 257 | node->pivot_dim = -1; 258 | for (i = 0; i < 3; i++) 259 | { 260 | split = abs(key[i] - node->key[i]); 261 | if (split > max_split) 262 | { 263 | max_split = split; 264 | node->pivot_dim = i; 265 | } 266 | } 267 | assert(node->pivot_dim >= 0); 268 | 269 | node->pivot_value = (key[node->pivot_dim] + node->key[node->pivot_dim]) / 2.0; 270 | 271 | if (key[node->pivot_dim] < node->pivot_value) 272 | { 273 | node->children[0] = pf_kdtree_insert_node(self, node, NULL, key, value); 274 | node->children[1] = pf_kdtree_insert_node(self, node, NULL, node->key, node->value); 275 | } 276 | else 277 | { 278 | node->children[0] = pf_kdtree_insert_node(self, node, NULL, node->key, node->value); 279 | node->children[1] = pf_kdtree_insert_node(self, node, NULL, key, value); 280 | } 281 | 282 | node->leaf = 0; 283 | self->leaf_count -= 1; 284 | } 285 | } 286 | 287 | // If the node exists, and it has children... 288 | else 289 | { 290 | assert(node->children[0] != NULL); 291 | assert(node->children[1] != NULL); 292 | 293 | if (key[node->pivot_dim] < node->pivot_value) 294 | pf_kdtree_insert_node(self, node, node->children[0], key, value); 295 | else 296 | pf_kdtree_insert_node(self, node, node->children[1], key, value); 297 | } 298 | 299 | return node; 300 | } 301 | 302 | 303 | //////////////////////////////////////////////////////////////////////////////// 304 | // Recursive node search 305 | pf_kdtree_node_t *pf_kdtree_find_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int key[]) 306 | { 307 | if (node->leaf) 308 | { 309 | //printf("find : leaf %p %d %d %d\n", node, node->key[0], node->key[1], node->key[2]); 310 | 311 | // If the keys are the same... 312 | if (pf_kdtree_equal(self, key, node->key)) 313 | return node; 314 | else 315 | return NULL; 316 | } 317 | else 318 | { 319 | //printf("find : brch %p %d %f\n", node, node->pivot_dim, node->pivot_value); 320 | 321 | assert(node->children[0] != NULL); 322 | assert(node->children[1] != NULL); 323 | 324 | // If the keys are different... 325 | if (key[node->pivot_dim] < node->pivot_value) 326 | return pf_kdtree_find_node(self, node->children[0], key); 327 | else 328 | return pf_kdtree_find_node(self, node->children[1], key); 329 | } 330 | 331 | return NULL; 332 | } 333 | 334 | 335 | //////////////////////////////////////////////////////////////////////////////// 336 | // Recursive node printing 337 | /* 338 | void pf_kdtree_print_node(pf_kdtree_t *self, pf_kdtree_node_t *node) 339 | { 340 | if (node->leaf) 341 | { 342 | printf("(%+02d %+02d %+02d)\n", node->key[0], node->key[1], node->key[2]); 343 | printf("%*s", node->depth * 11, ""); 344 | } 345 | else 346 | { 347 | printf("(%+02d %+02d %+02d) ", node->key[0], node->key[1], node->key[2]); 348 | pf_kdtree_print_node(self, node->children[0]); 349 | pf_kdtree_print_node(self, node->children[1]); 350 | } 351 | return; 352 | } 353 | */ 354 | 355 | 356 | //////////////////////////////////////////////////////////////////////////////// 357 | // Cluster the leaves in the tree 358 | void pf_kdtree_cluster(pf_kdtree_t *self) 359 | { 360 | int i; 361 | int queue_count, cluster_count; 362 | pf_kdtree_node_t **queue, *node; 363 | 364 | queue_count = 0; 365 | queue = calloc(self->node_count, sizeof(queue[0])); 366 | 367 | // Put all the leaves in a queue 368 | for (i = 0; i < self->node_count; i++) 369 | { 370 | node = self->nodes + i; 371 | if (node->leaf) 372 | { 373 | node->cluster = -1; 374 | assert(queue_count < self->node_count); 375 | queue[queue_count++] = node; 376 | 377 | // TESTING; remove 378 | assert(node == pf_kdtree_find_node(self, self->root, node->key)); 379 | } 380 | } 381 | 382 | cluster_count = 0; 383 | 384 | // Do connected components for each node 385 | while (queue_count > 0) 386 | { 387 | node = queue[--queue_count]; 388 | 389 | // If this node has already been labelled, skip it 390 | if (node->cluster >= 0) 391 | continue; 392 | 393 | // Assign a label to this cluster 394 | node->cluster = cluster_count++; 395 | 396 | // Recursively label nodes in this cluster 397 | pf_kdtree_cluster_node(self, node, 0); 398 | } 399 | 400 | free(queue); 401 | return; 402 | } 403 | 404 | 405 | //////////////////////////////////////////////////////////////////////////////// 406 | // Recursively label nodes in this cluster 407 | void pf_kdtree_cluster_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int depth) 408 | { 409 | int i; 410 | int nkey[3]; 411 | pf_kdtree_node_t *nnode; 412 | 413 | for (i = 0; i < 3 * 3 * 3; i++) 414 | { 415 | nkey[0] = node->key[0] + (i / 9) - 1; 416 | nkey[1] = node->key[1] + ((i % 9) / 3) - 1; 417 | nkey[2] = node->key[2] + ((i % 9) % 3) - 1; 418 | 419 | nnode = pf_kdtree_find_node(self, self->root, nkey); 420 | if (nnode == NULL) 421 | continue; 422 | 423 | assert(nnode->leaf); 424 | 425 | // This node already has a label; skip it. The label should be 426 | // consistent, however. 427 | if (nnode->cluster >= 0) 428 | { 429 | assert(nnode->cluster == node->cluster); 430 | continue; 431 | } 432 | 433 | // Label this node and recurse 434 | nnode->cluster = node->cluster; 435 | 436 | pf_kdtree_cluster_node(self, nnode, depth + 1); 437 | } 438 | return; 439 | } 440 | 441 | 442 | 443 | #ifdef INCLUDE_RTKGUI 444 | 445 | //////////////////////////////////////////////////////////////////////////////// 446 | // Draw the tree 447 | void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig) 448 | { 449 | if (self->root != NULL) 450 | pf_kdtree_draw_node(self, self->root, fig); 451 | return; 452 | } 453 | 454 | 455 | //////////////////////////////////////////////////////////////////////////////// 456 | // Recursively draw nodes 457 | void pf_kdtree_draw_node(pf_kdtree_t *self, pf_kdtree_node_t *node, rtk_fig_t *fig) 458 | { 459 | double ox, oy; 460 | char text[64]; 461 | 462 | if (node->leaf) 463 | { 464 | ox = (node->key[0] + 0.5) * self->size[0]; 465 | oy = (node->key[1] + 0.5) * self->size[1]; 466 | 467 | rtk_fig_rectangle(fig, ox, oy, 0.0, self->size[0], self->size[1], 0); 468 | 469 | //snprintf(text, sizeof(text), "%0.3f", node->value); 470 | //rtk_fig_text(fig, ox, oy, 0.0, text); 471 | 472 | snprintf(text, sizeof(text), "%d", node->cluster); 473 | rtk_fig_text(fig, ox, oy, 0.0, text); 474 | } 475 | else 476 | { 477 | assert(node->children[0] != NULL); 478 | assert(node->children[1] != NULL); 479 | pf_kdtree_draw_node(self, node->children[0], fig); 480 | pf_kdtree_draw_node(self, node->children[1], fig); 481 | } 482 | 483 | return; 484 | } 485 | 486 | #endif 487 | -------------------------------------------------------------------------------- /src/amcl/pf/pf_pdf.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: Useful pdf functions 23 | * Author: Andrew Howard 24 | * Date: 10 Dec 2002 25 | * CVS: $Id: pf_pdf.c 6348 2008-04-17 02:53:17Z gerkey $ 26 | *************************************************************************/ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | //#include 33 | //#include 34 | 35 | #include "amcl/pf/pf_pdf.h" 36 | #include "portable_utils.hpp" 37 | 38 | // Random number generator seed value 39 | static unsigned int pf_pdf_seed; 40 | 41 | 42 | /************************************************************************** 43 | * Gaussian 44 | *************************************************************************/ 45 | 46 | // Create a gaussian pdf 47 | pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx) 48 | { 49 | pf_matrix_t cd; 50 | pf_pdf_gaussian_t *pdf; 51 | 52 | pdf = calloc(1, sizeof(pf_pdf_gaussian_t)); 53 | 54 | pdf->x = x; 55 | pdf->cx = cx; 56 | //pdf->cxi = pf_matrix_inverse(cx, &pdf->cxdet); 57 | 58 | // Decompose the convariance matrix into a rotation 59 | // matrix and a diagonal matrix. 60 | pf_matrix_unitary(&pdf->cr, &cd, pdf->cx); 61 | pdf->cd.v[0] = sqrt(cd.m[0][0]); 62 | pdf->cd.v[1] = sqrt(cd.m[1][1]); 63 | pdf->cd.v[2] = sqrt(cd.m[2][2]); 64 | 65 | // Initialize the random number generator 66 | //pdf->rng = gsl_rng_alloc(gsl_rng_taus); 67 | //gsl_rng_set(pdf->rng, ++pf_pdf_seed); 68 | srand48(++pf_pdf_seed); 69 | 70 | return pdf; 71 | } 72 | 73 | 74 | // Destroy the pdf 75 | void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf) 76 | { 77 | //gsl_rng_free(pdf->rng); 78 | free(pdf); 79 | return; 80 | } 81 | 82 | 83 | /* 84 | // Compute the value of the pdf at some point [x]. 85 | double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t x) 86 | { 87 | int i, j; 88 | pf_vector_t z; 89 | double zz, p; 90 | 91 | z = pf_vector_sub(x, pdf->x); 92 | 93 | zz = 0; 94 | for (i = 0; i < 3; i++) 95 | for (j = 0; j < 3; j++) 96 | zz += z.v[i] * pdf->cxi.m[i][j] * z.v[j]; 97 | 98 | p = 1 / (2 * M_PI * pdf->cxdet) * exp(-zz / 2); 99 | 100 | return p; 101 | } 102 | */ 103 | 104 | 105 | // Generate a sample from the pdf. 106 | pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf) 107 | { 108 | int i, j; 109 | pf_vector_t r; 110 | pf_vector_t x; 111 | 112 | // Generate a random vector 113 | for (i = 0; i < 3; i++) 114 | { 115 | //r.v[i] = gsl_ran_gaussian(pdf->rng, pdf->cd.v[i]); 116 | r.v[i] = pf_ran_gaussian(pdf->cd.v[i]); 117 | } 118 | 119 | for (i = 0; i < 3; i++) 120 | { 121 | x.v[i] = pdf->x.v[i]; 122 | for (j = 0; j < 3; j++) 123 | x.v[i] += pdf->cr.m[i][j] * r.v[j]; 124 | } 125 | 126 | return x; 127 | } 128 | 129 | // Draw randomly from a zero-mean Gaussian distribution, with standard 130 | // deviation sigma. 131 | // We use the polar form of the Box-Muller transformation, explained here: 132 | // http://www.taygeta.com/random/gaussian.html 133 | double pf_ran_gaussian(double sigma) 134 | { 135 | double x1, x2, w, r; 136 | 137 | do 138 | { 139 | do { r = drand48(); } while (r==0.0); 140 | x1 = 2.0 * r - 1.0; 141 | do { r = drand48(); } while (r==0.0); 142 | x2 = 2.0 * r - 1.0; 143 | w = x1*x1 + x2*x2; 144 | } while(w > 1.0 || w==0.0); 145 | 146 | return(sigma * x2 * sqrt(-2.0*log(w)/w)); 147 | } 148 | -------------------------------------------------------------------------------- /src/amcl/pf/pf_vector.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: Vector functions 23 | * Author: Andrew Howard 24 | * Date: 10 Dec 2002 25 | * CVS: $Id: pf_vector.c 6345 2008-04-17 01:36:39Z gerkey $ 26 | *************************************************************************/ 27 | 28 | #include 29 | //#include 30 | //#include 31 | //#include 32 | 33 | #include "amcl/pf/pf_vector.h" 34 | #include "amcl/pf/eig3.h" 35 | 36 | 37 | // Return a zero vector 38 | pf_vector_t pf_vector_zero() 39 | { 40 | pf_vector_t c; 41 | 42 | c.v[0] = 0.0; 43 | c.v[1] = 0.0; 44 | c.v[2] = 0.0; 45 | 46 | return c; 47 | } 48 | 49 | 50 | // Check for NAN or INF in any component 51 | int pf_vector_finite(pf_vector_t a) 52 | { 53 | int i; 54 | 55 | for (i = 0; i < 3; i++) 56 | if (!isfinite(a.v[i])) 57 | return 0; 58 | 59 | return 1; 60 | } 61 | 62 | 63 | // Print a vector 64 | void pf_vector_fprintf(pf_vector_t a, FILE *file, const char *fmt) 65 | { 66 | int i; 67 | 68 | for (i = 0; i < 3; i++) 69 | { 70 | fprintf(file, fmt, a.v[i]); 71 | fprintf(file, " "); 72 | } 73 | fprintf(file, "\n"); 74 | 75 | return; 76 | } 77 | 78 | 79 | // Simple vector addition 80 | pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b) 81 | { 82 | pf_vector_t c; 83 | 84 | c.v[0] = a.v[0] + b.v[0]; 85 | c.v[1] = a.v[1] + b.v[1]; 86 | c.v[2] = a.v[2] + b.v[2]; 87 | 88 | return c; 89 | } 90 | 91 | 92 | // Simple vector subtraction 93 | pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b) 94 | { 95 | pf_vector_t c; 96 | 97 | c.v[0] = a.v[0] - b.v[0]; 98 | c.v[1] = a.v[1] - b.v[1]; 99 | c.v[2] = a.v[2] - b.v[2]; 100 | 101 | return c; 102 | } 103 | 104 | 105 | // Transform from local to global coords (a + b) 106 | pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b) 107 | { 108 | pf_vector_t c; 109 | 110 | c.v[0] = b.v[0] + a.v[0] * cos(b.v[2]) - a.v[1] * sin(b.v[2]); 111 | c.v[1] = b.v[1] + a.v[0] * sin(b.v[2]) + a.v[1] * cos(b.v[2]); 112 | c.v[2] = b.v[2] + a.v[2]; 113 | c.v[2] = atan2(sin(c.v[2]), cos(c.v[2])); 114 | 115 | return c; 116 | } 117 | 118 | 119 | // Transform from global to local coords (a - b) 120 | pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b) 121 | { 122 | pf_vector_t c; 123 | 124 | c.v[0] = +(a.v[0] - b.v[0]) * cos(b.v[2]) + (a.v[1] - b.v[1]) * sin(b.v[2]); 125 | c.v[1] = -(a.v[0] - b.v[0]) * sin(b.v[2]) + (a.v[1] - b.v[1]) * cos(b.v[2]); 126 | c.v[2] = a.v[2] - b.v[2]; 127 | c.v[2] = atan2(sin(c.v[2]), cos(c.v[2])); 128 | 129 | return c; 130 | } 131 | 132 | 133 | // Return a zero matrix 134 | pf_matrix_t pf_matrix_zero() 135 | { 136 | int i, j; 137 | pf_matrix_t c; 138 | 139 | for (i = 0; i < 3; i++) 140 | for (j = 0; j < 3; j++) 141 | c.m[i][j] = 0.0; 142 | 143 | return c; 144 | } 145 | 146 | 147 | // Check for NAN or INF in any component 148 | int pf_matrix_finite(pf_matrix_t a) 149 | { 150 | int i, j; 151 | 152 | for (i = 0; i < 3; i++) 153 | for (j = 0; j < 3; j++) 154 | if (!isfinite(a.m[i][j])) 155 | return 0; 156 | 157 | return 1; 158 | } 159 | 160 | 161 | // Print a matrix 162 | void pf_matrix_fprintf(pf_matrix_t a, FILE *file, const char *fmt) 163 | { 164 | int i, j; 165 | 166 | for (i = 0; i < 3; i++) 167 | { 168 | for (j = 0; j < 3; j++) 169 | { 170 | fprintf(file, fmt, a.m[i][j]); 171 | fprintf(file, " "); 172 | } 173 | fprintf(file, "\n"); 174 | } 175 | return; 176 | } 177 | 178 | 179 | /* 180 | // Compute the matrix inverse 181 | pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det) 182 | { 183 | double lndet; 184 | int signum; 185 | gsl_permutation *p; 186 | gsl_matrix_view A, Ai; 187 | 188 | pf_matrix_t ai; 189 | 190 | A = gsl_matrix_view_array((double*) a.m, 3, 3); 191 | Ai = gsl_matrix_view_array((double*) ai.m, 3, 3); 192 | 193 | // Do LU decomposition 194 | p = gsl_permutation_alloc(3); 195 | gsl_linalg_LU_decomp(&A.matrix, p, &signum); 196 | 197 | // Check for underflow 198 | lndet = gsl_linalg_LU_lndet(&A.matrix); 199 | if (lndet < -1000) 200 | { 201 | //printf("underflow in matrix inverse lndet = %f", lndet); 202 | gsl_matrix_set_zero(&Ai.matrix); 203 | } 204 | else 205 | { 206 | // Compute inverse 207 | gsl_linalg_LU_invert(&A.matrix, p, &Ai.matrix); 208 | } 209 | 210 | gsl_permutation_free(p); 211 | 212 | if (det) 213 | *det = exp(lndet); 214 | 215 | return ai; 216 | } 217 | */ 218 | 219 | 220 | // Decompose a covariance matrix [a] into a rotation matrix [r] and a diagonal 221 | // matrix [d] such that a = r d r^T. 222 | void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a) 223 | { 224 | int i, j; 225 | /* 226 | gsl_matrix *aa; 227 | gsl_vector *eval; 228 | gsl_matrix *evec; 229 | gsl_eigen_symmv_workspace *w; 230 | 231 | aa = gsl_matrix_alloc(3, 3); 232 | eval = gsl_vector_alloc(3); 233 | evec = gsl_matrix_alloc(3, 3); 234 | */ 235 | 236 | double aa[3][3]; 237 | double eval[3]; 238 | double evec[3][3]; 239 | 240 | for (i = 0; i < 3; i++) 241 | { 242 | for (j = 0; j < 3; j++) 243 | { 244 | //gsl_matrix_set(aa, i, j, a.m[i][j]); 245 | aa[i][j] = a.m[i][j]; 246 | } 247 | } 248 | 249 | // Compute eigenvectors/values 250 | /* 251 | w = gsl_eigen_symmv_alloc(3); 252 | gsl_eigen_symmv(aa, eval, evec, w); 253 | gsl_eigen_symmv_free(w); 254 | */ 255 | 256 | eigen_decomposition(aa,evec,eval); 257 | 258 | *d = pf_matrix_zero(); 259 | for (i = 0; i < 3; i++) 260 | { 261 | //d->m[i][i] = gsl_vector_get(eval, i); 262 | d->m[i][i] = eval[i]; 263 | for (j = 0; j < 3; j++) 264 | { 265 | //r->m[i][j] = gsl_matrix_get(evec, i, j); 266 | r->m[i][j] = evec[i][j]; 267 | } 268 | } 269 | 270 | //gsl_matrix_free(evec); 271 | //gsl_vector_free(eval); 272 | //gsl_matrix_free(aa); 273 | 274 | return; 275 | } 276 | 277 | -------------------------------------------------------------------------------- /src/amcl/sensors/amcl_laser.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /////////////////////////////////////////////////////////////////////////// 22 | // 23 | // Desc: AMCL laser routines 24 | // Author: Andrew Howard 25 | // Date: 6 Feb 2003 26 | // CVS: $Id: amcl_laser.cc 7057 2008-10-02 00:44:06Z gbiggs $ 27 | // 28 | /////////////////////////////////////////////////////////////////////////// 29 | 30 | #include // required by Darwin 31 | #include 32 | #include 33 | #include 34 | #include 35 | #ifdef HAVE_UNISTD_H 36 | #include 37 | #endif 38 | 39 | //#define ENABLE_TEST 40 | 41 | #ifdef ENABLE_TEST 42 | // for debug 43 | #include 44 | #endif 45 | 46 | #include "amcl/sensors/amcl_laser.h" 47 | #include "amcl/sensors/compute_sample_weight.cuh" 48 | 49 | using namespace amcl; 50 | 51 | //////////////////////////////////////////////////////////////////////////////// 52 | // Default constructor 53 | AMCLLaser::AMCLLaser(size_t max_beams, map_t* map) : AMCLSensor(), 54 | max_samples(0), max_obs(0), 55 | temp_obs(NULL) 56 | { 57 | this->time = 0.0; 58 | this->max_beams = max_beams; 59 | this->map = map; 60 | 61 | return; 62 | } 63 | 64 | AMCLLaser::~AMCLLaser() 65 | { 66 | if (temp_obs) 67 | { 68 | for (int k=0; k < max_samples; k++) 69 | { 70 | delete [] temp_obs[k]; 71 | } 72 | delete []temp_obs; 73 | } 74 | } 75 | 76 | void 77 | AMCLLaser::SetModelBeam(double z_hit, 78 | double z_short, 79 | double z_max, 80 | double z_rand, 81 | double sigma_hit, 82 | double lambda_short, 83 | double chi_outlier) 84 | { 85 | this->model_type = LASER_MODEL_BEAM; 86 | this->z_hit = z_hit; 87 | this->z_short = z_short; 88 | this->z_max = z_max; 89 | this->z_rand = z_rand; 90 | this->sigma_hit = sigma_hit; 91 | this->lambda_short = lambda_short; 92 | this->chi_outlier = chi_outlier; 93 | } 94 | 95 | void 96 | AMCLLaser::SetModelLikelihoodField(double z_hit, 97 | double z_rand, 98 | double sigma_hit, 99 | double max_occ_dist) 100 | { 101 | this->model_type = LASER_MODEL_LIKELIHOOD_FIELD; 102 | this->z_hit = z_hit; 103 | this->z_rand = z_rand; 104 | this->sigma_hit = sigma_hit; 105 | 106 | map_update_cspace(this->map, max_occ_dist); 107 | } 108 | 109 | void 110 | AMCLLaser::SetModelLikelihoodFieldProb(double z_hit, 111 | double z_rand, 112 | double sigma_hit, 113 | double max_occ_dist, 114 | bool do_beamskip, 115 | double beam_skip_distance, 116 | double beam_skip_threshold, 117 | double beam_skip_error_threshold) 118 | { 119 | this->model_type = LASER_MODEL_LIKELIHOOD_FIELD_PROB; 120 | this->z_hit = z_hit; 121 | this->z_rand = z_rand; 122 | this->sigma_hit = sigma_hit; 123 | this->do_beamskip = do_beamskip; 124 | this->beam_skip_distance = beam_skip_distance; 125 | this->beam_skip_threshold = beam_skip_threshold; 126 | this->beam_skip_error_threshold = beam_skip_error_threshold; 127 | map_update_cspace(this->map, max_occ_dist); 128 | } 129 | 130 | 131 | //////////////////////////////////////////////////////////////////////////////// 132 | // Apply the laser sensor model 133 | bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data) 134 | { 135 | if (this->max_beams < 2) 136 | { 137 | return false; 138 | } 139 | 140 | // Apply the laser sensor model 141 | if (this->model_type == LASER_MODEL_BEAM) 142 | { 143 | pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data); 144 | } 145 | else if (this->model_type == LASER_MODEL_LIKELIHOOD_FIELD) 146 | { 147 | pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModel, data); 148 | } 149 | else if (this->model_type == LASER_MODEL_LIKELIHOOD_FIELD_PROB) 150 | { 151 | pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModelProb, data); 152 | } 153 | else 154 | { 155 | pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data); 156 | } 157 | 158 | return true; 159 | } 160 | 161 | 162 | //////////////////////////////////////////////////////////////////////////////// 163 | // Determine the probability for the given pose 164 | double AMCLLaser::BeamModel(AMCLLaserData *data, pf_sample_set_t* set) 165 | { 166 | AMCLLaser *self; 167 | int i, j, step; 168 | double z, pz; 169 | double p; 170 | double map_range; 171 | double obs_range, obs_bearing; 172 | double total_weight; 173 | pf_sample_t *sample; 174 | pf_vector_t pose; 175 | 176 | self = (AMCLLaser*) data->sensor; 177 | 178 | total_weight = 0.0; 179 | 180 | // Compute the sample weights 181 | for (j = 0; j < set->sample_count; j++) 182 | { 183 | sample = set->samples + j; 184 | pose = sample->pose; 185 | 186 | // Take account of the laser pose relative to the robot 187 | pose = pf_vector_coord_add(self->laser_pose, pose); 188 | 189 | p = 1.0; 190 | 191 | step = (data->range_count - 1) / (self->max_beams - 1); 192 | for (i = 0; i < data->range_count; i += step) 193 | { 194 | obs_range = data->ranges[i][0]; 195 | obs_bearing = data->ranges[i][1]; 196 | 197 | // Compute the range according to the map 198 | map_range = map_calc_range(self->map, pose.v[0], pose.v[1], 199 | pose.v[2] + obs_bearing, data->range_max); 200 | pz = 0.0; 201 | 202 | // Part 1: good, but noisy, hit 203 | z = obs_range - map_range; 204 | pz += self->z_hit * exp(-(z * z) / (2 * self->sigma_hit * self->sigma_hit)); 205 | 206 | // Part 2: short reading from unexpected obstacle (e.g., a person) 207 | if (z < 0) 208 | { 209 | pz += self->z_short * self->lambda_short * exp(-self->lambda_short*obs_range); 210 | } 211 | 212 | // Part 3: Failure to detect obstacle, reported as max-range 213 | if (obs_range == data->range_max) 214 | { 215 | pz += self->z_max * 1.0; 216 | } 217 | 218 | // Part 4: Random measurements 219 | if (obs_range < data->range_max) 220 | { 221 | pz += self->z_rand * 1.0/data->range_max; 222 | } 223 | 224 | // TODO: outlier rejection for short readings 225 | 226 | assert(pz <= 1.0); 227 | assert(pz >= 0.0); 228 | // p *= pz; 229 | // here we have an ad-hoc weighting scheme for combining beam probs 230 | // works well, though... 231 | p += pz*pz*pz; 232 | } 233 | 234 | sample->weight *= p; 235 | total_weight += sample->weight; 236 | } 237 | 238 | return(total_weight); 239 | } 240 | 241 | double compute_sample_weight(const AMCLLaserData *data, pf_sample_t *samples, const int sample_count) 242 | { 243 | AMCLLaser *self = (AMCLLaser*) data->sensor; 244 | double z, pz; 245 | double p; 246 | double obs_range, obs_bearing; 247 | double total_weight = 0.0; 248 | pf_sample_t *sample; 249 | pf_vector_t pose; 250 | pf_vector_t hit; 251 | 252 | self = (AMCLLaser*) data->sensor; 253 | 254 | // pre-compute a couple of things 255 | const pf_vector_t laser_pose = self->laser_pose; 256 | const double sigma_hit = self->sigma_hit; 257 | const int max_beams = self->max_beams; 258 | const map_t *map = self->map; 259 | const double z_hit = self->z_hit; 260 | const double z_rand = self->z_rand; 261 | 262 | // Compute the sample weights 263 | for (int j = 0; j < sample_count; j++) 264 | { 265 | sample = samples + j; 266 | pose = sample->pose; 267 | 268 | // Take account of the laser pose relative to the robot 269 | pose = pf_vector_coord_add(laser_pose, pose); 270 | 271 | p = 1.0; 272 | 273 | // Pre-compute a couple of things 274 | double z_hit_denom = 2 * sigma_hit * sigma_hit; 275 | double z_rand_mult = 1.0 / data->range_max; 276 | 277 | int step = (data->range_count - 1) / (max_beams - 1); 278 | 279 | // Step size must be at least 1 280 | if (step < 1) 281 | { 282 | step = 1; 283 | } 284 | 285 | for (int i = 0; i < data->range_count; i += step) 286 | { 287 | obs_range = data->ranges[i][0]; 288 | obs_bearing = data->ranges[i][1]; 289 | 290 | // This model ignores max range readings 291 | if (obs_range >= data->range_max) 292 | { 293 | continue; 294 | } 295 | 296 | // Check for NaN 297 | if (obs_range != obs_range) 298 | { 299 | continue; 300 | } 301 | 302 | pz = 0.0; 303 | 304 | // Compute the endpoint of the beam 305 | hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing); 306 | hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing); 307 | 308 | // Convert to map grid coords. 309 | int mi, mj; 310 | mi = MAP_GXWX(map, hit.v[0]); 311 | mj = MAP_GYWY(map, hit.v[1]); 312 | 313 | // Part 1: Get distance from the hit to closest obstacle. 314 | // Off-map penalized as max distance 315 | if(!MAP_VALID(map, mi, mj)) 316 | { 317 | z = map->max_occ_dist; 318 | } 319 | else 320 | { 321 | z = map->cells[MAP_INDEX(map, mi, mj)].occ_dist; 322 | } 323 | // Gaussian model 324 | // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma) 325 | pz += z_hit * exp(-(z * z) / z_hit_denom); 326 | // Part 2: random measurements 327 | pz += z_rand * z_rand_mult; 328 | 329 | // TODO: outlier rejection for short readings 330 | 331 | assert(pz <= 1.0); 332 | assert(pz >= 0.0); 333 | // p *= pz; 334 | // here we have an ad-hoc weighting scheme for combining beam probs 335 | // works well, though... 336 | p += pz*pz*pz; 337 | } 338 | 339 | sample->weight *= p; 340 | total_weight += sample->weight; 341 | } 342 | 343 | return(total_weight); 344 | } 345 | 346 | double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set) 347 | { 348 | const int sample_count = set->sample_count; 349 | pf_sample_t *samples = set->samples; 350 | 351 | #ifdef ENABLE_TEST 352 | // store input samples for testing 353 | pf_sample_t *samples_ = (pf_sample_t *)malloc(sizeof(pf_sample_t) * sample_count); 354 | memcpy(samples_, samples, sizeof(pf_sample_t) * sample_count); 355 | #endif // ENABLE_TEST 356 | 357 | std::chrono::system_clock::time_point start, end; 358 | start = std::chrono::system_clock::now(); 359 | double total_weight = cu_compute_sample_weight(data, samples, sample_count); 360 | end = std::chrono::system_clock::now(); 361 | double elapsed_gpu = std::chrono::duration_cast(end-start).count(); 362 | 363 | #ifdef ENABLE_TEST 364 | start = std::chrono::system_clock::now(); 365 | double total_weight_cpu = compute_sample_weight(data, samples_, sample_count); 366 | end = std::chrono::system_clock::now(); 367 | double elapsed_cpu = std::chrono::duration_cast(end-start).count(); 368 | //ROS_INFO("[CPU] elapsed = %f ms.", elapsed_cpu); 369 | //ROS_INFO("[GPU] elapsed = %f ms.", elapsed_gpu); 370 | 371 | if (fabs(total_weight - total_weight_cpu) >= 1e-5) 372 | { 373 | ROS_INFO("[FAILED] total_weight = %f, total_weight_cpu = %f", total_weight, total_weight_cpu); 374 | } 375 | free(samples_); 376 | #endif // ENABLE_TEST 377 | 378 | return(total_weight); 379 | } 380 | 381 | double AMCLLaser::LikelihoodFieldModelProb(AMCLLaserData *data, pf_sample_set_t* set) 382 | { 383 | AMCLLaser *self; 384 | int i, j, step; 385 | double z, pz; 386 | double log_p; 387 | double obs_range, obs_bearing; 388 | double total_weight; 389 | pf_sample_t *sample; 390 | pf_vector_t pose; 391 | pf_vector_t hit; 392 | 393 | self = (AMCLLaser*) data->sensor; 394 | 395 | total_weight = 0.0; 396 | 397 | step = ceil((data->range_count) / static_cast(self->max_beams)); 398 | 399 | // Step size must be at least 1 400 | if (step < 1) 401 | { 402 | step = 1; 403 | } 404 | 405 | // Pre-compute a couple of things 406 | double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit; 407 | double z_rand_mult = 1.0/data->range_max; 408 | 409 | double max_dist_prob = exp(-(self->map->max_occ_dist * self->map->max_occ_dist) / z_hit_denom); 410 | 411 | //Beam skipping - ignores beams for which a majoirty of particles do not agree with the map 412 | //prevents correct particles from getting down weighted because of unexpected obstacles 413 | //such as humans 414 | 415 | bool do_beamskip = self->do_beamskip; 416 | double beam_skip_distance = self->beam_skip_distance; 417 | double beam_skip_threshold = self->beam_skip_threshold; 418 | 419 | //we only do beam skipping if the filter has converged 420 | if (do_beamskip && !set->converged) 421 | { 422 | do_beamskip = false; 423 | } 424 | 425 | //we need a count the no of particles for which the beam agreed with the map 426 | int *obs_count = new int[self->max_beams](); 427 | 428 | //we also need a mask of which observations to integrate (to decide which beams to integrate to all particles) 429 | bool *obs_mask = new bool[self->max_beams](); 430 | int beam_ind = 0; 431 | 432 | //realloc indicates if we need to reallocate the temp data structure needed to do beamskipping 433 | bool realloc = false; 434 | 435 | if (do_beamskip) 436 | { 437 | if (self->max_obs < self->max_beams) 438 | { 439 | realloc = true; 440 | } 441 | 442 | if (self->max_samples < set->sample_count) 443 | { 444 | realloc = true; 445 | } 446 | 447 | if (realloc) 448 | { 449 | self->reallocTempData(set->sample_count, self->max_beams); 450 | fprintf(stderr, "Reallocing temp weights %d - %d\n", self->max_samples, self->max_obs); 451 | } 452 | } 453 | 454 | // Compute the sample weights 455 | for (j = 0; j < set->sample_count; j++) 456 | { 457 | sample = set->samples + j; 458 | pose = sample->pose; 459 | 460 | // Take account of the laser pose relative to the robot 461 | pose = pf_vector_coord_add(self->laser_pose, pose); 462 | 463 | log_p = 0; 464 | beam_ind = 0; 465 | 466 | for (i = 0; i < data->range_count; i += step, beam_ind++) 467 | { 468 | obs_range = data->ranges[i][0]; 469 | obs_bearing = data->ranges[i][1]; 470 | 471 | // This model ignores max range readings 472 | if (obs_range >= data->range_max) 473 | { 474 | continue; 475 | } 476 | 477 | // Check for NaN 478 | if (obs_range != obs_range) 479 | { 480 | continue; 481 | } 482 | 483 | pz = 0.0; 484 | 485 | // Compute the endpoint of the beam 486 | hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing); 487 | hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing); 488 | 489 | // Convert to map grid coords. 490 | int mi, mj; 491 | mi = MAP_GXWX(self->map, hit.v[0]); 492 | mj = MAP_GYWY(self->map, hit.v[1]); 493 | 494 | // Part 1: Get distance from the hit to closest obstacle. 495 | // Off-map penalized as max distance 496 | if (!MAP_VALID(self->map, mi, mj)) 497 | { 498 | pz += self->z_hit * max_dist_prob; 499 | } 500 | else 501 | { 502 | z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist; 503 | if (z < beam_skip_distance) 504 | { 505 | obs_count[beam_ind] += 1; 506 | } 507 | pz += self->z_hit * exp(-(z * z) / z_hit_denom); 508 | } 509 | 510 | // Gaussian model 511 | // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma) 512 | 513 | // Part 2: random measurements 514 | pz += self->z_rand * z_rand_mult; 515 | 516 | assert(pz <= 1.0); 517 | assert(pz >= 0.0); 518 | 519 | // TODO: outlier rejection for short readings 520 | if (!do_beamskip) 521 | { 522 | log_p += log(pz); 523 | } 524 | else 525 | { 526 | self->temp_obs[j][beam_ind] = pz; 527 | } 528 | } 529 | if (!do_beamskip) 530 | { 531 | sample->weight *= exp(log_p); 532 | total_weight += sample->weight; 533 | } 534 | } 535 | 536 | if (do_beamskip) 537 | { 538 | int skipped_beam_count = 0; 539 | for (beam_ind = 0; beam_ind < self->max_beams; beam_ind++) 540 | { 541 | if ((obs_count[beam_ind] / static_cast(set->sample_count)) > beam_skip_threshold) 542 | { 543 | obs_mask[beam_ind] = true; 544 | } 545 | else 546 | { 547 | obs_mask[beam_ind] = false; 548 | skipped_beam_count++; 549 | } 550 | } 551 | 552 | //we check if there is at least a critical number of beams that agreed with the map 553 | //otherwise it probably indicates that the filter converged to a wrong solution 554 | //if that's the case we integrate all the beams and hope the filter might converge to 555 | //the right solution 556 | bool error = false; 557 | 558 | if (skipped_beam_count >= (beam_ind * self->beam_skip_error_threshold)) 559 | { 560 | fprintf(stderr, "Over %f%% of the observations were not in the map - pf may have converged to wrong pose - integrating all observations\n", (100 * self->beam_skip_error_threshold)); 561 | error = true; 562 | } 563 | 564 | for (j = 0; j < set->sample_count; j++) 565 | { 566 | sample = set->samples + j; 567 | pose = sample->pose; 568 | 569 | log_p = 0; 570 | 571 | for (beam_ind = 0; beam_ind < self->max_beams; beam_ind++) 572 | { 573 | if(error || obs_mask[beam_ind]) 574 | { 575 | log_p += log(self->temp_obs[j][beam_ind]); 576 | } 577 | } 578 | sample->weight *= exp(log_p); 579 | total_weight += sample->weight; 580 | } 581 | } 582 | 583 | delete [] obs_count; 584 | delete [] obs_mask; 585 | return(total_weight); 586 | } 587 | 588 | void AMCLLaser::reallocTempData(int new_max_samples, int new_max_obs){ 589 | if (temp_obs) 590 | { 591 | for (int k=0; k < max_samples; k++) 592 | { 593 | delete [] temp_obs[k]; 594 | } 595 | delete []temp_obs; 596 | } 597 | max_obs = new_max_obs; 598 | max_samples = fmax(max_samples, new_max_samples); 599 | 600 | temp_obs = new double*[max_samples](); 601 | for (int k=0; k < max_samples; k++) 602 | { 603 | temp_obs[k] = new double[max_obs](); 604 | } 605 | } 606 | -------------------------------------------------------------------------------- /src/amcl/sensors/amcl_odom.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /////////////////////////////////////////////////////////////////////////// 22 | // 23 | // Desc: AMCL odometry routines 24 | // Author: Andrew Howard 25 | // Date: 6 Feb 2003 26 | // CVS: $Id: amcl_odom.cc 7057 2008-10-02 00:44:06Z gbiggs $ 27 | // 28 | /////////////////////////////////////////////////////////////////////////// 29 | 30 | #include 31 | 32 | #include // required by Darwin 33 | #include 34 | 35 | #include "amcl/sensors/amcl_odom.h" 36 | 37 | using namespace amcl; 38 | 39 | static double 40 | normalize(double z) 41 | { 42 | return atan2(sin(z),cos(z)); 43 | } 44 | static double 45 | angle_diff(double a, double b) 46 | { 47 | double d1, d2; 48 | a = normalize(a); 49 | b = normalize(b); 50 | d1 = a-b; 51 | d2 = 2*M_PI - fabs(d1); 52 | if(d1 > 0) 53 | d2 *= -1.0; 54 | if(fabs(d1) < fabs(d2)) 55 | return(d1); 56 | else 57 | return(d2); 58 | } 59 | 60 | //////////////////////////////////////////////////////////////////////////////// 61 | // Default constructor 62 | AMCLOdom::AMCLOdom() : AMCLSensor() 63 | { 64 | this->time = 0.0; 65 | } 66 | 67 | void 68 | AMCLOdom::SetModelDiff(double alpha1, 69 | double alpha2, 70 | double alpha3, 71 | double alpha4) 72 | { 73 | this->model_type = ODOM_MODEL_DIFF; 74 | this->alpha1 = alpha1; 75 | this->alpha2 = alpha2; 76 | this->alpha3 = alpha3; 77 | this->alpha4 = alpha4; 78 | } 79 | 80 | void 81 | AMCLOdom::SetModelOmni(double alpha1, 82 | double alpha2, 83 | double alpha3, 84 | double alpha4, 85 | double alpha5) 86 | { 87 | this->model_type = ODOM_MODEL_OMNI; 88 | this->alpha1 = alpha1; 89 | this->alpha2 = alpha2; 90 | this->alpha3 = alpha3; 91 | this->alpha4 = alpha4; 92 | this->alpha5 = alpha5; 93 | } 94 | 95 | void 96 | AMCLOdom::SetModel( odom_model_t type, 97 | double alpha1, 98 | double alpha2, 99 | double alpha3, 100 | double alpha4, 101 | double alpha5 ) 102 | { 103 | this->model_type = type; 104 | this->alpha1 = alpha1; 105 | this->alpha2 = alpha2; 106 | this->alpha3 = alpha3; 107 | this->alpha4 = alpha4; 108 | this->alpha5 = alpha5; 109 | } 110 | 111 | //////////////////////////////////////////////////////////////////////////////// 112 | // Apply the action model 113 | bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data) 114 | { 115 | AMCLOdomData *ndata; 116 | ndata = (AMCLOdomData*) data; 117 | 118 | // Compute the new sample poses 119 | pf_sample_set_t *set; 120 | 121 | set = pf->sets + pf->current_set; 122 | pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta); 123 | 124 | switch( this->model_type ) 125 | { 126 | case ODOM_MODEL_OMNI: 127 | { 128 | double delta_trans, delta_rot, delta_bearing; 129 | double delta_trans_hat, delta_rot_hat, delta_strafe_hat; 130 | 131 | delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] + 132 | ndata->delta.v[1]*ndata->delta.v[1]); 133 | delta_rot = ndata->delta.v[2]; 134 | 135 | // Precompute a couple of things 136 | double trans_hat_stddev = (alpha3 * (delta_trans*delta_trans) + 137 | alpha1 * (delta_rot*delta_rot)); 138 | double rot_hat_stddev = (alpha4 * (delta_rot*delta_rot) + 139 | alpha2 * (delta_trans*delta_trans)); 140 | double strafe_hat_stddev = (alpha1 * (delta_rot*delta_rot) + 141 | alpha5 * (delta_trans*delta_trans)); 142 | 143 | for (int i = 0; i < set->sample_count; i++) 144 | { 145 | pf_sample_t* sample = set->samples + i; 146 | 147 | delta_bearing = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]), 148 | old_pose.v[2]) + sample->pose.v[2]; 149 | double cs_bearing = cos(delta_bearing); 150 | double sn_bearing = sin(delta_bearing); 151 | 152 | // Sample pose differences 153 | delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev); 154 | delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev); 155 | delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev); 156 | // Apply sampled update to particle pose 157 | sample->pose.v[0] += (delta_trans_hat * cs_bearing + 158 | delta_strafe_hat * sn_bearing); 159 | sample->pose.v[1] += (delta_trans_hat * sn_bearing - 160 | delta_strafe_hat * cs_bearing); 161 | sample->pose.v[2] += delta_rot_hat ; 162 | } 163 | } 164 | break; 165 | case ODOM_MODEL_DIFF: 166 | { 167 | // Implement sample_motion_odometry (Prob Rob p 136) 168 | double delta_rot1, delta_trans, delta_rot2; 169 | double delta_rot1_hat, delta_trans_hat, delta_rot2_hat; 170 | double delta_rot1_noise, delta_rot2_noise; 171 | 172 | // Avoid computing a bearing from two poses that are extremely near each 173 | // other (happens on in-place rotation). 174 | if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] + 175 | ndata->delta.v[0]*ndata->delta.v[0]) < 0.01) 176 | delta_rot1 = 0.0; 177 | else 178 | delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]), 179 | old_pose.v[2]); 180 | delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] + 181 | ndata->delta.v[1]*ndata->delta.v[1]); 182 | delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1); 183 | 184 | // We want to treat backward and forward motion symmetrically for the 185 | // noise model to be applied below. The standard model seems to assume 186 | // forward motion. 187 | delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)), 188 | fabs(angle_diff(delta_rot1,M_PI))); 189 | delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)), 190 | fabs(angle_diff(delta_rot2,M_PI))); 191 | 192 | for (int i = 0; i < set->sample_count; i++) 193 | { 194 | pf_sample_t* sample = set->samples + i; 195 | 196 | // Sample pose differences 197 | delta_rot1_hat = angle_diff(delta_rot1, 198 | pf_ran_gaussian(this->alpha1*delta_rot1_noise*delta_rot1_noise + 199 | this->alpha2*delta_trans*delta_trans)); 200 | delta_trans_hat = delta_trans - 201 | pf_ran_gaussian(this->alpha3*delta_trans*delta_trans + 202 | this->alpha4*delta_rot1_noise*delta_rot1_noise + 203 | this->alpha4*delta_rot2_noise*delta_rot2_noise); 204 | delta_rot2_hat = angle_diff(delta_rot2, 205 | pf_ran_gaussian(this->alpha1*delta_rot2_noise*delta_rot2_noise + 206 | this->alpha2*delta_trans*delta_trans)); 207 | 208 | // Apply sampled update to particle pose 209 | sample->pose.v[0] += delta_trans_hat * 210 | cos(sample->pose.v[2] + delta_rot1_hat); 211 | sample->pose.v[1] += delta_trans_hat * 212 | sin(sample->pose.v[2] + delta_rot1_hat); 213 | sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat; 214 | } 215 | } 216 | break; 217 | case ODOM_MODEL_OMNI_CORRECTED: 218 | { 219 | double delta_trans, delta_rot, delta_bearing; 220 | double delta_trans_hat, delta_rot_hat, delta_strafe_hat; 221 | 222 | delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] + 223 | ndata->delta.v[1]*ndata->delta.v[1]); 224 | delta_rot = ndata->delta.v[2]; 225 | 226 | // Precompute a couple of things 227 | double trans_hat_stddev = sqrt( alpha3 * (delta_trans*delta_trans) + 228 | alpha4 * (delta_rot*delta_rot) ); 229 | double rot_hat_stddev = sqrt( alpha1 * (delta_rot*delta_rot) + 230 | alpha2 * (delta_trans*delta_trans) ); 231 | double strafe_hat_stddev = sqrt( alpha4 * (delta_rot*delta_rot) + 232 | alpha5 * (delta_trans*delta_trans) ); 233 | 234 | for (int i = 0; i < set->sample_count; i++) 235 | { 236 | pf_sample_t* sample = set->samples + i; 237 | 238 | delta_bearing = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]), 239 | old_pose.v[2]) + sample->pose.v[2]; 240 | double cs_bearing = cos(delta_bearing); 241 | double sn_bearing = sin(delta_bearing); 242 | 243 | // Sample pose differences 244 | delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev); 245 | delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev); 246 | delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev); 247 | // Apply sampled update to particle pose 248 | sample->pose.v[0] += (delta_trans_hat * cs_bearing + 249 | delta_strafe_hat * sn_bearing); 250 | sample->pose.v[1] += (delta_trans_hat * sn_bearing - 251 | delta_strafe_hat * cs_bearing); 252 | sample->pose.v[2] += delta_rot_hat ; 253 | } 254 | } 255 | break; 256 | case ODOM_MODEL_DIFF_CORRECTED: 257 | { 258 | // Implement sample_motion_odometry (Prob Rob p 136) 259 | double delta_rot1, delta_trans, delta_rot2; 260 | double delta_rot1_hat, delta_trans_hat, delta_rot2_hat; 261 | double delta_rot1_noise, delta_rot2_noise; 262 | 263 | // Avoid computing a bearing from two poses that are extremely near each 264 | // other (happens on in-place rotation). 265 | if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] + 266 | ndata->delta.v[0]*ndata->delta.v[0]) < 0.01) 267 | delta_rot1 = 0.0; 268 | else 269 | delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]), 270 | old_pose.v[2]); 271 | delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] + 272 | ndata->delta.v[1]*ndata->delta.v[1]); 273 | delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1); 274 | 275 | // We want to treat backward and forward motion symmetrically for the 276 | // noise model to be applied below. The standard model seems to assume 277 | // forward motion. 278 | delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)), 279 | fabs(angle_diff(delta_rot1,M_PI))); 280 | delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)), 281 | fabs(angle_diff(delta_rot2,M_PI))); 282 | 283 | for (int i = 0; i < set->sample_count; i++) 284 | { 285 | pf_sample_t* sample = set->samples + i; 286 | 287 | // Sample pose differences 288 | delta_rot1_hat = angle_diff(delta_rot1, 289 | pf_ran_gaussian(sqrt(this->alpha1*delta_rot1_noise*delta_rot1_noise + 290 | this->alpha2*delta_trans*delta_trans))); 291 | delta_trans_hat = delta_trans - 292 | pf_ran_gaussian(sqrt(this->alpha3*delta_trans*delta_trans + 293 | this->alpha4*delta_rot1_noise*delta_rot1_noise + 294 | this->alpha4*delta_rot2_noise*delta_rot2_noise)); 295 | delta_rot2_hat = angle_diff(delta_rot2, 296 | pf_ran_gaussian(sqrt(this->alpha1*delta_rot2_noise*delta_rot2_noise + 297 | this->alpha2*delta_trans*delta_trans))); 298 | 299 | // Apply sampled update to particle pose 300 | sample->pose.v[0] += delta_trans_hat * 301 | cos(sample->pose.v[2] + delta_rot1_hat); 302 | sample->pose.v[1] += delta_trans_hat * 303 | sin(sample->pose.v[2] + delta_rot1_hat); 304 | sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat; 305 | } 306 | } 307 | break; 308 | } 309 | return true; 310 | } 311 | -------------------------------------------------------------------------------- /src/amcl/sensors/amcl_sensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /////////////////////////////////////////////////////////////////////////// 22 | // 23 | // Desc: AMCL sensor 24 | // Author: Andrew Howard 25 | // Date: 6 Feb 2003 26 | // CVS: $Id: amcl_sensor.cc 7057 2008-10-02 00:44:06Z gbiggs $ 27 | // 28 | /////////////////////////////////////////////////////////////////////////// 29 | 30 | 31 | #include "amcl/sensors/amcl_sensor.h" 32 | 33 | using namespace amcl; 34 | 35 | //////////////////////////////////////////////////////////////////////////////// 36 | // Default constructor 37 | AMCLSensor::AMCLSensor() 38 | { 39 | return; 40 | } 41 | 42 | AMCLSensor::~AMCLSensor() 43 | { 44 | } 45 | 46 | //////////////////////////////////////////////////////////////////////////////// 47 | // Apply the action model 48 | bool AMCLSensor::UpdateAction(pf_t *pf, AMCLSensorData *data) 49 | { 50 | return false; 51 | } 52 | 53 | 54 | //////////////////////////////////////////////////////////////////////////////// 55 | // Initialize the filter 56 | bool AMCLSensor::InitSensor(pf_t *pf, AMCLSensorData *data) 57 | { 58 | return false; 59 | } 60 | 61 | 62 | //////////////////////////////////////////////////////////////////////////////// 63 | // Apply the sensor model 64 | bool AMCLSensor::UpdateSensor(pf_t *pf, AMCLSensorData *data) 65 | { 66 | return false; 67 | } 68 | 69 | 70 | #ifdef INCLUDE_RTKGUI 71 | 72 | //////////////////////////////////////////////////////////////////////////////// 73 | // Setup the GUI 74 | void AMCLSensor::SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig) 75 | { 76 | return; 77 | } 78 | 79 | 80 | //////////////////////////////////////////////////////////////////////////////// 81 | // Shutdown the GUI 82 | void AMCLSensor::ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig) 83 | { 84 | return; 85 | } 86 | 87 | 88 | //////////////////////////////////////////////////////////////////////////////// 89 | // Draw sensor data 90 | void AMCLSensor::UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data) 91 | { 92 | return; 93 | } 94 | 95 | #endif 96 | -------------------------------------------------------------------------------- /src/amcl/sensors/compute_sample_weight.cu: -------------------------------------------------------------------------------- 1 | #include 2 | #include "amcl/sensors/compute_sample_weight.cuh" 3 | 4 | using namespace amcl; 5 | 6 | #define CUDA_SAFE_CALL(call) \ 7 | { \ 8 | const cudaError_t error = call; \ 9 | if (error != cudaSuccess) \ 10 | { \ 11 | printf("Error: %s:%d, ", __FILE__, __LINE__); \ 12 | printf("code:%d, reason: %s\n", error, \ 13 | cudaGetErrorString(error)); \ 14 | exit(1); \ 15 | } \ 16 | } 17 | 18 | __device__ 19 | pf_vector_t dev_pf_vector_coord_add(const pf_vector_t a, const pf_vector_t b) 20 | { 21 | pf_vector_t c; 22 | 23 | c.v[0] = b.v[0] + a.v[0] * cos(b.v[2]) - a.v[1] * sin(b.v[2]); 24 | c.v[1] = b.v[1] + a.v[0] * sin(b.v[2]) + a.v[1] * cos(b.v[2]); 25 | c.v[2] = b.v[2] + a.v[2]; 26 | c.v[2] = atan2(sin(c.v[2]), cos(c.v[2])); 27 | 28 | return c; 29 | } 30 | 31 | __device__ 32 | double atomicAdd_double(double* address, double val) 33 | { 34 | unsigned long long int* address_as_ull =(unsigned long long int*)address; 35 | unsigned long long int old = *address_as_ull, assumed; 36 | 37 | do { 38 | assumed = old; 39 | old = atomicCAS(address_as_ull, assumed, 40 | __double_as_longlong(val + __longlong_as_double(assumed))); 41 | } while (assumed != old); 42 | 43 | return __longlong_as_double(old); 44 | } 45 | 46 | __global__ 47 | void dev_compute_sample_weight(pf_sample_t samples[], const int max_count, 48 | const double z_hit, const double z_rand, double const z_hit_denom, const double z_rand_mult, 49 | const int step, const pf_vector_t laser_pose, const map_t *map, const map_cell_t map_cells[], 50 | const int range_count, const double range_max, const double (*dev_ranges)[2], double *total_weight) 51 | { 52 | int i = blockIdx.x * blockDim.x + threadIdx.x; 53 | 54 | i = max(i, 0); 55 | i = min(i, max_count); 56 | 57 | if (i < max_count) 58 | { 59 | // Take account of the laser pose relative to the robot 60 | pf_vector_t pose; 61 | pose = dev_pf_vector_coord_add(laser_pose, samples[i].pose); 62 | 63 | double p = 1.0; 64 | int j; 65 | 66 | for (j = 0; j < range_count; j += step) 67 | { 68 | double z, pz; 69 | pf_vector_t hit; 70 | int mi, mj; 71 | double obs_range = dev_ranges[j][0]; 72 | double obs_bearing = dev_ranges[j][1]; 73 | 74 | // This model ignores max range readings 75 | if (obs_range >= range_max) 76 | { 77 | continue; 78 | } 79 | 80 | // Check for NaN 81 | if (obs_range != obs_range) 82 | { 83 | continue; 84 | } 85 | 86 | pz = 0.0; 87 | 88 | // Compute the endpoint of the beam 89 | hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing); 90 | hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing); 91 | 92 | // Convert to map grid coords. 93 | mi = MAP_GXWX(map, hit.v[0]); 94 | mj = MAP_GYWY(map, hit.v[1]); 95 | 96 | // Part 1: Get distance from the hit to closest obstacle. 97 | // Off-map penalized as max distance 98 | z = (!MAP_VALID(map, mi, mj)) ? (map->max_occ_dist) : (map_cells[MAP_INDEX(map,mi,mj)].occ_dist); 99 | 100 | // Gaussian model 101 | // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma) 102 | pz += z_hit * exp(-(z * z) / z_hit_denom); 103 | 104 | // Part 2: random measurements 105 | pz += z_rand * z_rand_mult; 106 | 107 | // TODO: outlier rejection for short readings 108 | 109 | // p *= pz; 110 | // here we have an ad-hoc weighting scheme for combining beam probs 111 | // works well, though... 112 | p += pz*pz*pz; 113 | } 114 | 115 | samples[i].weight *= p; 116 | atomicAdd_double(total_weight, samples[i].weight); 117 | } 118 | } 119 | 120 | 121 | double cu_compute_sample_weight(const amcl::AMCLLaserData *data, pf_sample_t *samples, const int sample_count) 122 | { 123 | AMCLLaser *self = (AMCLLaser*) data->sensor; 124 | double total_weight = 0.0; 125 | 126 | // pre-compute a couple of things 127 | const pf_vector_t laser_pose = self->laser_pose; 128 | const double sigma_hit = self->sigma_hit; 129 | const int max_beams = self->max_beams; 130 | const map_t *map = self->map; 131 | const double z_hit = self->z_hit; 132 | const double z_rand = self->z_rand; 133 | const double z_hit_denom = 2 * sigma_hit * sigma_hit; 134 | const double z_rand_mult = 1.0 / data->range_max; 135 | const int range_count = data->range_count; 136 | const double range_max = data->range_max; 137 | int step = (range_count - 1) / (max_beams - 1); 138 | 139 | // Step size must be at least 1 140 | if (step < 1) 141 | { 142 | step = 1; 143 | } 144 | 145 | pf_sample_t *dev_samples; 146 | map_t *dev_map; 147 | map_cell_t *dev_map_cells; 148 | double (*dev_ranges)[2]; 149 | double *dev_total_weight; 150 | 151 | CUDA_SAFE_CALL(cudaMalloc(&dev_samples, sizeof(pf_sample_t) * sample_count)); 152 | CUDA_SAFE_CALL(cudaMalloc(&dev_map, sizeof(map_t))); 153 | CUDA_SAFE_CALL(cudaMalloc(&dev_map_cells, sizeof(map_cell_t) * map->size_x * map->size_y)); 154 | CUDA_SAFE_CALL(cudaMalloc(&dev_ranges, sizeof(double) * range_count * 2)); 155 | CUDA_SAFE_CALL(cudaMalloc(&dev_total_weight, sizeof(double))); 156 | 157 | CUDA_SAFE_CALL(cudaMemcpy(dev_samples, samples, sizeof(pf_sample_t) * sample_count, cudaMemcpyHostToDevice)); 158 | CUDA_SAFE_CALL(cudaMemcpy(dev_map, map, sizeof(map_t), cudaMemcpyHostToDevice)); 159 | CUDA_SAFE_CALL(cudaMemcpy(dev_map_cells, map->cells, sizeof(map_cell_t) * map->size_x * map->size_y, cudaMemcpyHostToDevice)); 160 | CUDA_SAFE_CALL(cudaMemcpy(dev_ranges, data->ranges, sizeof(double) * range_count * 2, cudaMemcpyHostToDevice)); 161 | CUDA_SAFE_CALL(cudaMemcpy(dev_total_weight, &total_weight, sizeof(double), cudaMemcpyHostToDevice)); 162 | 163 | dim3 block(256); 164 | dim3 grid((sample_count + block.x - 1) / block.x); 165 | 166 | dev_compute_sample_weight<<>>(dev_samples, sample_count, 167 | z_hit, z_rand, z_hit_denom, z_rand_mult, step, laser_pose, 168 | dev_map, dev_map_cells, range_count, range_max, 169 | dev_ranges, dev_total_weight); 170 | 171 | CUDA_SAFE_CALL(cudaGetLastError()); 172 | CUDA_SAFE_CALL(cudaDeviceSynchronize()); 173 | 174 | CUDA_SAFE_CALL(cudaMemcpy(samples, dev_samples, sizeof(pf_sample_t) * sample_count, cudaMemcpyDeviceToHost)); 175 | CUDA_SAFE_CALL(cudaMemcpy(&total_weight, dev_total_weight, sizeof(double), cudaMemcpyDeviceToHost)); 176 | 177 | CUDA_SAFE_CALL(cudaFree(dev_samples)); 178 | CUDA_SAFE_CALL(cudaFree(dev_map)); 179 | CUDA_SAFE_CALL(cudaFree(dev_map_cells)); 180 | CUDA_SAFE_CALL(cudaFree(dev_ranges)); 181 | CUDA_SAFE_CALL(cudaFree(dev_total_weight)); 182 | 183 | return(total_weight); 184 | } 185 | -------------------------------------------------------------------------------- /src/include/portable_utils.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PORTABLE_UTILS_H 2 | #define PORTABLE_UTILS_H 3 | 4 | #include 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | 10 | #ifndef HAVE_DRAND48 11 | // Some system (e.g., Windows) doesn't come with drand48(), srand48(). 12 | // Use rand, and srand for such system. 13 | static double drand48(void) 14 | { 15 | return ((double)rand())/RAND_MAX; 16 | } 17 | 18 | static void srand48(long int seedval) 19 | { 20 | srand(seedval); 21 | } 22 | #endif 23 | 24 | #ifdef __cplusplus 25 | } 26 | #endif 27 | 28 | #endif -------------------------------------------------------------------------------- /test/basic_localization.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import time 5 | from math import fmod, pi 6 | 7 | import unittest 8 | import rospy 9 | import rostest 10 | 11 | from tf2_msgs.msg import TFMessage 12 | import PyKDL 13 | from std_srvs.srv import Empty 14 | 15 | 16 | class TestBasicLocalization(unittest.TestCase): 17 | def setUp(self): 18 | self.tf = None 19 | self.target_x = None 20 | self.target_y = None 21 | self.target_a = None 22 | 23 | def tf_cb(self, msg): 24 | for t in msg.transforms: 25 | if t.header.frame_id == 'map': 26 | self.tf = t.transform 27 | (a_curr, a_diff) = self.compute_angle_diff() 28 | print 'Curr:\t %16.6f %16.6f %16.6f' % (self.tf.translation.x, self.tf.translation.y, a_curr) 29 | print 'Target:\t %16.6f %16.6f %16.6f' % (self.target_x, self.target_y, self.target_a) 30 | print 'Diff:\t %16.6f %16.6f %16.6f' % ( 31 | abs(self.tf.translation.x - self.target_x), abs(self.tf.translation.y - self.target_y), a_diff) 32 | 33 | def compute_angle_diff(self): 34 | rot = self.tf.rotation 35 | a = PyKDL.Rotation.Quaternion(rot.x, rot.y, rot.z, rot.w).GetRPY()[2] 36 | d_a = self.target_a 37 | 38 | return (a, abs(fmod(a - d_a + 5*pi, 2*pi) - pi)) 39 | 40 | def test_basic_localization(self): 41 | global_localization = int(sys.argv[1]) 42 | self.target_x = float(sys.argv[2]) 43 | self.target_y = float(sys.argv[3]) 44 | self.target_a = float(sys.argv[4]) 45 | tolerance_d = float(sys.argv[5]) 46 | tolerance_a = float(sys.argv[6]) 47 | target_time = float(sys.argv[7]) 48 | 49 | if global_localization == 1: 50 | #print 'Waiting for service global_localization' 51 | rospy.wait_for_service('global_localization') 52 | global_localization = rospy.ServiceProxy('global_localization', Empty) 53 | global_localization() 54 | 55 | rospy.init_node('test', anonymous=True) 56 | while(rospy.rostime.get_time() == 0.0): 57 | #print 'Waiting for initial time publication' 58 | time.sleep(0.1) 59 | start_time = rospy.rostime.get_time() 60 | # TODO: This should be replace by a pytf listener 61 | rospy.Subscriber('/tf', TFMessage, self.tf_cb) 62 | 63 | while (rospy.rostime.get_time() - start_time) < target_time: 64 | #print 'Waiting for end time %.6f (current: %.6f)'%(target_time,(rospy.rostime.get_time() - start_time)) 65 | time.sleep(0.1) 66 | 67 | (a_curr, a_diff) = self.compute_angle_diff() 68 | print 'Curr:\t %16.6f %16.6f %16.6f' % (self.tf.translation.x, self.tf.translation.y, a_curr) 69 | print 'Target:\t %16.6f %16.6f %16.6f' % (self.target_x, self.target_y, self.target_a) 70 | print 'Diff:\t %16.6f %16.6f %16.6f' % ( 71 | abs(self.tf.translation.x - self.target_x), abs(self.tf.translation.y - self.target_y), a_diff) 72 | self.assertNotEquals(self.tf, None) 73 | self.assertTrue(abs(self.tf.translation.x - self.target_x) <= tolerance_d) 74 | self.assertTrue(abs(self.tf.translation.y - self.target_y) <= tolerance_d) 75 | self.assertTrue(a_diff <= tolerance_a) 76 | 77 | if __name__ == '__main__': 78 | rostest.run('amcl', 'amcl_localization', 79 | TestBasicLocalization, sys.argv) 80 | -------------------------------------------------------------------------------- /test/basic_localization_stage.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 49 | 50 | -------------------------------------------------------------------------------- /test/global_localization_stage.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 49 | 50 | -------------------------------------------------------------------------------- /test/rosie_multilaser.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 49 | 50 | -------------------------------------------------------------------------------- /test/set_initial_pose.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 47 | 48 | 50 | 51 | -------------------------------------------------------------------------------- /test/set_initial_pose_delayed.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 8 | 9 | 10 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 51 | 52 | 54 | 55 | -------------------------------------------------------------------------------- /test/set_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | 5 | import math 6 | import PyKDL 7 | from geometry_msgs.msg import PoseWithCovarianceStamped 8 | 9 | 10 | class PoseSetter(rospy.SubscribeListener): 11 | def __init__(self, pose, stamp, publish_time): 12 | self.pose = pose 13 | self.stamp = stamp 14 | self.publish_time = publish_time 15 | 16 | def peer_subscribe(self, topic_name, topic_publish, peer_publish): 17 | p = PoseWithCovarianceStamped() 18 | p.header.frame_id = "map" 19 | p.header.stamp = self.stamp 20 | p.pose.pose.position.x = self.pose[0] 21 | p.pose.pose.position.y = self.pose[1] 22 | (p.pose.pose.orientation.x, 23 | p.pose.pose.orientation.y, 24 | p.pose.pose.orientation.z, 25 | p.pose.pose.orientation.w) = PyKDL.Rotation.RPY(0, 0, self.pose[2]).GetQuaternion() 26 | p.pose.covariance[6*0+0] = 0.5 * 0.5 27 | p.pose.covariance[6*1+1] = 0.5 * 0.5 28 | p.pose.covariance[6*3+3] = math.pi/12.0 * math.pi/12.0 29 | # wait for the desired publish time 30 | while rospy.get_rostime() < self.publish_time: 31 | rospy.sleep(0.01) 32 | peer_publish(p) 33 | 34 | 35 | if __name__ == '__main__': 36 | pose = map(float, rospy.myargv()[1:4]) 37 | t_stamp = rospy.Time() 38 | t_publish = rospy.Time() 39 | if len(rospy.myargv()) > 4: 40 | t_stamp = rospy.Time.from_sec(float(rospy.myargv()[4])) 41 | if len(rospy.myargv()) > 5: 42 | t_publish = rospy.Time.from_sec(float(rospy.myargv()[5])) 43 | rospy.init_node('pose_setter', anonymous=True) 44 | rospy.loginfo("Going to publish pose {} with stamp {} at {}".format(pose, t_stamp.to_sec(), t_publish.to_sec())) 45 | pub = rospy.Publisher("initialpose", PoseWithCovarianceStamped, PoseSetter(pose, stamp=t_stamp, publish_time=t_publish), queue_size=1) 46 | rospy.spin() 47 | -------------------------------------------------------------------------------- /test/small_loop_crazy_driving_prg.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 49 | 50 | -------------------------------------------------------------------------------- /test/small_loop_crazy_driving_prg_corrected.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 49 | 50 | -------------------------------------------------------------------------------- /test/small_loop_prf.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 46 | 47 | -------------------------------------------------------------------------------- /test/texas_greenroom_loop.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 37 | 38 | -------------------------------------------------------------------------------- /test/texas_greenroom_loop_corrected.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 37 | 38 | -------------------------------------------------------------------------------- /test/texas_willow_hallway_loop.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 37 | 38 | 48 | 49 | -------------------------------------------------------------------------------- /test/texas_willow_hallway_loop_corrected.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 37 | 38 | --------------------------------------------------------------------------------