├── .gitignore ├── CMakeCache.txt ├── CMakeFiles └── cmake.check_cache ├── CMakeLists.txt ├── README.md ├── TODO ├── cmake ├── AddFSLAM.cmake ├── CMakeCache.txt ├── CMakeFiles │ └── cmake.check_cache ├── FindEigen3.cmake ├── FindGooglePerfTools.cmake └── FindOpenCL.cmake ├── cpp ├── README ├── core │ ├── CMakeLists.txt │ ├── KF_cholesky_update.cpp │ ├── KF_cholesky_update.h │ ├── KF_joseph_update.cpp │ ├── KF_joseph_update.h │ ├── TransformToGlobal.cpp │ ├── TransformToGlobal.h │ ├── add_control_noise.cpp │ ├── add_control_noise.h │ ├── add_feature.cpp │ ├── add_feature.h │ ├── add_observation_noise.cpp │ ├── add_observation_noise.h │ ├── compute_jacobians.cpp │ ├── compute_jacobians.h │ ├── compute_steering.cpp │ ├── compute_steering.h │ ├── configfile.cpp │ ├── configfile.h │ ├── data_associate_known.cpp │ ├── data_associate_known.h │ ├── example_webmap.mat │ ├── example_webmap_6D.mat │ ├── example_webmap_simple.mat │ ├── feature_update.cpp │ ├── feature_update.h │ ├── get_observations.cpp │ ├── get_observations.h │ ├── line_plot_conversion.cpp │ ├── line_plot_conversion.h │ ├── multivariate_gauss.cpp │ ├── multivariate_gauss.h │ ├── particle.cpp │ ├── particle.h │ ├── pi_to_pi.cpp │ ├── pi_to_pi.h │ ├── predict_true.cpp │ ├── predict_true.h │ ├── printMat.h │ ├── read_input_file.cpp │ ├── read_input_file.h │ ├── resample_particles.cpp │ ├── resample_particles.h │ ├── stratified_random.cpp │ ├── stratified_random.h │ ├── stratified_resample.cpp │ ├── stratified_resample.h │ └── utilities.h ├── fastslam1 │ ├── CMakeLists.txt │ ├── compute_weight.cpp │ ├── compute_weight.h │ ├── fastslam1_sim.cpp │ ├── fastslam1_sim.h │ ├── main.cpp │ ├── predict.cpp │ └── predict.h ├── fastslam2 │ ├── CMakeLists.txt │ ├── CMakeLists.txt.real │ ├── CMakeLists_empty.txt │ ├── CMakeLists_real.txt │ ├── fastslam2_sim.cpp │ ├── fastslam2_sim.h │ ├── gauss_evaluate.cpp │ ├── gauss_evaluate.h │ ├── main.cpp │ ├── observe_heading.cpp │ ├── observe_heading.h │ ├── predict.cpp │ ├── predict.h │ ├── sample_proposal.cpp │ └── sample_proposal.h ├── gtest │ ├── CMakeCache.txt │ ├── CMakeFiles │ │ └── cmake.check_cache │ ├── CMakeLists.txt │ ├── TestFS1AgainstMatlab.cpp │ ├── example_webmap.mat │ └── test_main.cc └── log ├── matlab ├── KF_cholesky_update.m ├── KF_joseph_update.m ├── TransformToGlobal.m ├── add_control_noise.m ├── add_feature.m ├── add_observation_noise.m ├── compute_jacobians.m ├── compute_steering.m ├── configfile.m ├── data_associate_known.m ├── example_webmap.mat ├── fast_to_ekf_diag.m ├── fastslam1 │ ├── compute_weight.m │ ├── fastslam1_sim.m │ └── predict.m ├── fastslam2 │ ├── README │ ├── compute_weight.m │ ├── fastslam2_sim.m │ ├── gauss_evaluate.m │ ├── observe_heading.m │ ├── predict.m │ ├── proposal.mws │ └── sample_proposal.m ├── fastslam2r │ ├── compute_weightr.m │ ├── fastslam2r_sim.m │ ├── observe_heading.m │ ├── predict.m │ ├── readme.txt │ └── sample_proposal.m ├── feature_update.m ├── frontend.fig ├── frontend.m ├── get_observations.m ├── line_plot_conversion.m ├── multivariate_gauss.m ├── pi_to_pi.m ├── predict_true.m ├── readme.txt ├── resample_particles.m ├── sqrtm_2by2.m ├── stratified_random.m └── stratified_resample.m ├── testcode ├── CMakeLists.txt ├── openCLWrapper.cpp ├── openCLWrapper.h ├── test_main.cc ├── utilities.h ├── vector_add_cpu.cpp ├── vector_add_cpu.h └── vector_add_gpu.cl └── thirdparty └── gtest ├── CHANGES ├── CONTRIBUTORS ├── COPYING ├── README ├── include └── gtest │ └── gtest.h └── src ├── gtest-all.cc └── gtest_main.cc /.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | test/**/* 3 | *~ 4 | *.o 5 | exec 6 | build 7 | .DS_Store 8 | -------------------------------------------------------------------------------- /CMakeCache.txt: -------------------------------------------------------------------------------- 1 | # This is the CMakeCache file. 2 | # For build in directory: /Users/ylee8/FastSLAM 3 | # It was generated by CMake: /opt/local/bin/cmake 4 | # You can edit this file to change values found and used by cmake. 5 | # If you do not want to change any of the values, simply exit the editor. 6 | # If you do want to change a value, simply edit, save, and exit the editor. 7 | # The syntax for the file is as follows: 8 | # KEY:TYPE=VALUE 9 | # KEY is the name of a variable in the cache. 10 | # TYPE is a hint to GUI's for the type of VALUE, DO NOT EDIT TYPE!. 11 | # VALUE is the current value for the KEY. 12 | 13 | ######################## 14 | # EXTERNAL cache entries 15 | ######################## 16 | 17 | 18 | ######################## 19 | # INTERNAL cache entries 20 | ######################## 21 | 22 | //This is the directory where this CMakeCache.txt was created 23 | CMAKE_CACHEFILE_DIR:INTERNAL=/Users/ylee8/FastSLAM 24 | //Major version of cmake used to create the current loaded cache 25 | CMAKE_CACHE_MAJOR_VERSION:INTERNAL=2 26 | //Minor version of cmake used to create the current loaded cache 27 | CMAKE_CACHE_MINOR_VERSION:INTERNAL=8 28 | //Patch version of cmake used to create the current loaded cache 29 | CMAKE_CACHE_PATCH_VERSION:INTERNAL=9 30 | //Path to CMake executable. 31 | CMAKE_COMMAND:INTERNAL=/opt/local/bin/cmake 32 | //Path to cpack program executable. 33 | CMAKE_CPACK_COMMAND:INTERNAL=/opt/local/bin/cpack 34 | //Path to ctest program executable. 35 | CMAKE_CTEST_COMMAND:INTERNAL=/opt/local/bin/ctest 36 | //Path to cache edit program executable. 37 | CMAKE_EDIT_COMMAND:INTERNAL=/opt/local/bin/ccmake 38 | //Path to CMake installation. 39 | CMAKE_ROOT:INTERNAL=/opt/local/share/cmake-2.8 40 | 41 | -------------------------------------------------------------------------------- /CMakeFiles/cmake.check_cache: -------------------------------------------------------------------------------- 1 | # This file is generated by cmake for dependency checking of the CMakeCache.txt file 2 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(FSLAM CXX) 2 | cmake_minimum_required(VERSION 2.8) 3 | 4 | # Add path for custom modules 5 | set(CMAKE_MODULE_PATH 6 | ${CMAKE_MODULE_PATH} 7 | "${CMAKE_CURRENT_SOURCE_DIR}/cmake" 8 | ) 9 | 10 | set(PACKAGE_VERSION "0.1") 11 | set(PACKAGE_NAME "FastSLAM") 12 | set(PACKAGE_STRING "${PACKAGE_NAME} ${PACKAGE_VERSION}") 13 | set(PACKAGE_BUGREPORT "yeonjin.lee@nasa.gov") 14 | 15 | # Local Custom Options 16 | option(FSLAM_BUILD_TOOLS 17 | "Build the FastSLAM test tools." ON) 18 | option(BUILD_SHARED_LIBS 19 | "Produce shared libraries." TRUE) 20 | option(ENABLE_RPATHS 21 | "Use RPaths in executables to find shared libraries." TRUE) 22 | option(ENABLE_PROFILING 23 | "Link against the google perftools." FALSE) 24 | option(ENABLE_TCMALLOC 25 | "Link against the google perftools's tcmalloc." FALSE) 26 | if (ENABLE_RPATHS) 27 | set(CMAKE_SKIP_RPATH FALSE CACHE BOOL "") 28 | set(CMAKE_SKIP_BUILD_RPATH FALSE CACHE BOOL "") 29 | set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE CACHE BOOL "") 30 | set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") 31 | set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE CACHE BOOL "") 32 | if(APPLE) 33 | set(CMAKE_INSTALL_NAME_DIR ${CMAKE_INSTALL_PREFIX}/lib) 34 | endif(APPLE) 35 | mark_as_advanced( 36 | CMAKE_SKIP_RPATH 37 | CMAKE_SKIP_BUILD_RPATH 38 | CMAKE_BUILD_WITH_INSTALL_RPATH 39 | CMAKE_INSTALL_RPATH 40 | CMAKE_INSTALL_RPATH_USE_LINK_PATH 41 | ) 42 | endif(ENABLE_RPATHS) 43 | 44 | # Find Dependencies 45 | include(AddFSLAM) 46 | find_package(Eigen3 REQUIRED) 47 | find_package(GooglePerfTools) 48 | find_package(OpenCL REQUIRED) 49 | 50 | #Find Boost 51 | #FIND_PACKAGE(Boost) 52 | #IF (Boost_FOUND) 53 | # INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIR}) 54 | # ADD_DEFINITIONS( "-DHAS_BOOST" ) 55 | #ENDIF() 56 | find_package(Boost 1.44.0) 57 | 58 | # uncomment the following line to let the env variable BOOST_DIR set the Boost_DIR path 59 | # set(Boost_DIR $ENV{BOOST_ROOT}) 60 | 61 | # just print out the variables to see their values 62 | message("Boost_DIR : " ${Boost_DIR}) 63 | message("BOOST_ROOT: " $ENV{BOOST_ROOT}) 64 | 65 | # now see if boost was actually found 66 | if (Boost_FOUND) 67 | message("Boost WAS found!") 68 | endif (Boost_FOUND) 69 | 70 | 71 | 72 | # Finish up local custom options based on what was found above 73 | fslam_enable_testing() 74 | if (ENABLE_PROFILING AND GOOGLE_PERFTOOLS_FOUND) 75 | set(FSLAM_ENABLE_PROFILING ON ) 76 | else() 77 | set(FSLAM_ENABLE_PROFILING OFF ) 78 | endif() 79 | 80 | if (ENABLE_TCMALLOC AND GOOGLE_PERFTOOLS_FOUND) 81 | set(FSLAM_ENABLE_TCMALLOC ON) 82 | else() 83 | set(FSLAM_ENABLE_TCMALLOC OFF) 84 | endif() 85 | 86 | # Add common search locations 87 | include_directories(${FSLAM_SOURCE_DIR}/cpp) 88 | include_directories(${FSLAM_BINARY_DIR}/cpp) 89 | 90 | # Adding build directories 91 | add_subdirectory(cpp/core) 92 | add_subdirectory(cpp/fastslam1) 93 | add_subdirectory(cpp/fastslam2) 94 | add_subdirectory(cpp/gtest) 95 | 96 | # Adding test directories 97 | add_subdirectory(testcode) 98 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | FastSLAM 2 | ======== 3 | 4 | Retrieval 5 | 6 | Prediction 7 | 8 | Measurement Update 9 | 10 | Importance Weight 11 | 12 | Resampling 13 | 14 | -------------------------------------------------------------------------------- /TODO: -------------------------------------------------------------------------------- 1 | multivariate_gauss and other files that use instance of "nRandMat" are wrong. Replace with boost library's normally distributed random function? 2 | -------------------------------------------------------------------------------- /cmake/AddFSLAM.cmake: -------------------------------------------------------------------------------- 1 | macro(add_fslam_executable name) 2 | add_executable(${name} ${ARGN}) 3 | foreach(lib ${FSLAM_USED_LIBS}) 4 | if( NOT ${lib} STREQUAL "optimized" AND 5 | NOT ${lib} STREQUAL "debug" ) 6 | target_link_libraries( ${name} ${lib} ) 7 | endif() 8 | endforeach(lib) 9 | if ( FSLAM_ENABLE_PROFILING ) 10 | target_link_libraries( ${name} ${PROFILER_LIBRARY} ) 11 | endif() 12 | if ( FSLAM_ENABLE_TCMALLOC ) 13 | target_link_libraries( ${name} ${TCMALLOC_LIBRARY} ) 14 | endif() 15 | if ( MKL_LIBRARIES ) 16 | target_link_libraries( ${name} ${MKL_LIBRARIES} ) 17 | endif() 18 | endmacro(add_fslam_executable name) 19 | 20 | macro(add_fslam_tool name) 21 | add_fslam_executable(${name} ${ARGN}) 22 | if( FSLAM_BUILD_TOOLS ) 23 | message(STATUS "Installing: " ${name} ) 24 | install(TARGETS ${name} RUNTIME DESTINATION bin) 25 | endif() 26 | set_target_properties(${name} PROPERTIES FOLDER "tools") 27 | endmacro(add_fslam_tool name) 28 | 29 | function(fslam_enable_testing) 30 | set(GTEST_DIR ${CMAKE_SOURCE_DIR}/thirdparty/gtest) 31 | include_directories(${VISIONWORKBENCH_INCLUDE_DIRS}) 32 | include_directories(${GTEST_DIR}/include) 33 | add_library(gtest SHARED EXCLUDE_FROM_ALL 34 | ${GTEST_DIR}/src/gtest-all.cc 35 | ${CMAKE_SOURCE_DIR}/cpp/gtest/test_main.cc 36 | ) 37 | target_link_libraries(gtest 38 | ${VISIONWORKBENCH_CORE_LIBRARY} 39 | ${Boost_FILESYSTEM_LIBRARY} 40 | ${Boost_SYSTEM_LIBRARY} 41 | ) 42 | set_target_properties(gtest 43 | PROPERTIES 44 | COMPILE_FLAGS "-DTEST_SRCDIR=\\\"${CMAKE_CURRENT_SOURCE_DIR}\\\" -DTEST_DSTDIR=\\\"${CMAKE_CURRENT_BINARY_DIR}\\\"" 45 | ) 46 | 47 | add_custom_target(check) 48 | add_dependencies(check gtest) 49 | endfunction(fslam_enable_testing) 50 | 51 | 52 | function(fslam_add_test source_file) 53 | string(REGEX REPLACE "^([A-Za-z0-9_]*)\\.([A-Za-z0-9]*)" "\\1" executable "${source_file}") 54 | 55 | add_executable(${executable} EXCLUDE_FROM_ALL ${source_file} ) 56 | target_link_libraries( ${executable} gtest ) 57 | foreach(lib ${FSLAM_USED_LIBS}) 58 | target_link_libraries( ${executable} ${lib} ) 59 | endforeach(lib) 60 | 61 | set_target_properties(${executable} 62 | PROPERTIES 63 | COMPILE_FLAGS "-DTEST_SRCDIR=\\\"${CMAKE_CURRENT_SOURCE_DIR}\\\" -DTEST_DSTDIR=\\\"${CMAKE_CURRENT_BINARY_DIR}\\\"" 64 | ) 65 | add_custom_target(${executable}Exec ${CMAKE_CURRENT_BINARY_DIR}/${executable} 66 | DEPENDS ${executable} 67 | ) 68 | add_dependencies(check ${executable}Exec) 69 | 70 | if ( MKL_LIBRARIES ) 71 | target_link_libraries( ${executable} ${MKL_LIBRARIES} ) 72 | endif() 73 | 74 | # file(READ "${source_file}" contents) 75 | # string(REGEX MATCHALL "TEST_?F?\\(([A-Za-z_0-9 ,]+)\\)" found_tests ${contents}) 76 | # foreach(hit ${found_tests}) 77 | # string(REGEX REPLACE ".*\\( *([A-Za-z_0-9]+), *([A-Za-z_0-9]+) *\\).*" "\\1.\\2" test_name ${hit}) 78 | # message("Added test name: " ${executable} "\t" ${test_name}) 79 | # add_test(${test_name} ${executable} --gtest_filter=${test_name}) 80 | # endforeach() 81 | endfunction(fslam_add_test) 82 | -------------------------------------------------------------------------------- /cmake/CMakeCache.txt: -------------------------------------------------------------------------------- 1 | # This is the CMakeCache file. 2 | # For build in directory: /Users/ylee8/FastSLAM/cmake 3 | # It was generated by CMake: /Users/ylee8/packages/local/bin/cmake 4 | # You can edit this file to change values found and used by cmake. 5 | # If you do not want to change any of the values, simply exit the editor. 6 | # If you do want to change a value, simply edit, save, and exit the editor. 7 | # The syntax for the file is as follows: 8 | # KEY:TYPE=VALUE 9 | # KEY is the name of a variable in the cache. 10 | # TYPE is a hint to GUI's for the type of VALUE, DO NOT EDIT TYPE!. 11 | # VALUE is the current value for the KEY. 12 | 13 | ######################## 14 | # EXTERNAL cache entries 15 | ######################## 16 | 17 | 18 | ######################## 19 | # INTERNAL cache entries 20 | ######################## 21 | 22 | //This is the directory where this CMakeCache.txt was created 23 | CMAKE_CACHEFILE_DIR:INTERNAL=/Users/ylee8/FastSLAM/cmake 24 | //Major version of cmake used to create the current loaded cache 25 | CMAKE_CACHE_MAJOR_VERSION:INTERNAL=2 26 | //Minor version of cmake used to create the current loaded cache 27 | CMAKE_CACHE_MINOR_VERSION:INTERNAL=8 28 | //Patch version of cmake used to create the current loaded cache 29 | CMAKE_CACHE_PATCH_VERSION:INTERNAL=9 30 | //Path to CMake executable. 31 | CMAKE_COMMAND:INTERNAL=/Users/ylee8/packages/local/bin/cmake 32 | //Path to cpack program executable. 33 | CMAKE_CPACK_COMMAND:INTERNAL=/Users/ylee8/packages/local/bin/cpack 34 | //Path to ctest program executable. 35 | CMAKE_CTEST_COMMAND:INTERNAL=/Users/ylee8/packages/local/bin/ctest 36 | //Path to cache edit program executable. 37 | CMAKE_EDIT_COMMAND:INTERNAL=/Users/ylee8/packages/local/bin/ccmake 38 | //Path to CMake installation. 39 | CMAKE_ROOT:INTERNAL=/Users/ylee8/packages/local/share/cmake-2.8 40 | 41 | -------------------------------------------------------------------------------- /cmake/CMakeFiles/cmake.check_cache: -------------------------------------------------------------------------------- 1 | # This file is generated by cmake for dependency checking of the CMakeCache.txt file 2 | -------------------------------------------------------------------------------- /cmake/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN3_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN3_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 79 | 80 | endif(EIGEN3_INCLUDE_DIR) 81 | 82 | -------------------------------------------------------------------------------- /cmake/FindGooglePerfTools.cmake: -------------------------------------------------------------------------------- 1 | # -*- cmake -*- 2 | 3 | # - Find Google perftools 4 | # Find the Google perftools includes and libraries 5 | # This module defines 6 | # GOOGLE_PERFTOOLS_INCLUDE_DIR, where to find heap-profiler.h, etc. 7 | # GOOGLE_PERFTOOLS_FOUND, If false, do not try to use Google perftools. 8 | # also defined for general use are 9 | # TCMALLOC_LIBRARIES, where to find the tcmalloc library. 10 | # STACKTRACE_LIBRARIES, where to find the stacktrace library. 11 | # PROFILER_LIBRARIES, where to find the profiler library. 12 | 13 | FIND_PATH(GOOGLE_PERFTOOLS_INCLUDE_DIR google/heap-profiler.h 14 | HINTS 15 | $ENV{GOOGLE_PERFTOOLS_ROOT} 16 | ${GOOGLE_PERFTOOLS_ROOT} 17 | /usr/local/ 18 | /usr 19 | /opt/local 20 | PATH_SUFFIXES include 21 | ) 22 | 23 | SET(TCMALLOC_NAMES ${TCMALLOC_NAMES} tcmalloc) 24 | FIND_LIBRARY(TCMALLOC_LIBRARY 25 | NAMES ${TCMALLOC_NAMES} 26 | HINTS /usr /usr/local /opt/local 27 | ${GOOGLE_PERFTOOLS_INCLUDE_DIR}/.. 28 | PATH_SUFFIXES lib lib64 29 | ) 30 | 31 | IF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) 32 | SET(TCMALLOC_LIBRARIES ${TCMALLOC_LIBRARY}) 33 | SET(GOOGLE_PERFTOOLS_FOUND "YES") 34 | ELSE (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) 35 | SET(GOOGLE_PERFTOOLS_FOUND "NO") 36 | ENDIF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) 37 | 38 | SET(STACKTRACE_NAMES ${STACKTRACE_NAMES} stacktrace) 39 | FIND_LIBRARY(STACKTRACE_LIBRARY 40 | NAMES ${STACKTRACE_NAMES} 41 | HINTS /usr /usr/local /opt/local 42 | ${GOOGLE_PERFTOOLS_INCLUDE_DIR}/.. 43 | PATH_SUFFIXES lib lib64 44 | ) 45 | 46 | IF (STACKTRACE_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) 47 | SET(STACKTRACE_LIBRARIES ${STACKTRACE_LIBRARY}) 48 | ENDIF (STACKTRACE_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) 49 | 50 | SET(PROFILER_NAMES ${PROFILER_NAMES} profiler) 51 | FIND_LIBRARY(PROFILER_LIBRARY 52 | NAMES ${PROFILER_NAMES} 53 | HINTS /usr /usr/local /opt/local 54 | ${GOOGLE_PERFTOOLS_INCLUDE_DIR}/.. 55 | PATH_SUFFIXES lib lib64 56 | ) 57 | 58 | IF (PROFILER_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) 59 | SET(PROFILER_LIBRARIES ${PROFILER_LIBRARY}) 60 | ENDIF (PROFILER_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) 61 | 62 | IF (GOOGLE_PERFTOOLS_FOUND) 63 | IF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY) 64 | MESSAGE(STATUS "Found Google perftools: ${GOOGLE_PERFTOOLS_LIBRARIES}") 65 | ENDIF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY) 66 | ELSE (GOOGLE_PERFTOOLS_FOUND) 67 | IF (GOOGLE_PERFTOOLS_FIND_REQUIRED) 68 | MESSAGE(FATAL_ERROR "Could not find Google perftools library") 69 | ENDIF (GOOGLE_PERFTOOLS_FIND_REQUIRED) 70 | ENDIF (GOOGLE_PERFTOOLS_FOUND) 71 | 72 | MARK_AS_ADVANCED( 73 | TCMALLOC_LIBRARY 74 | STACKTRACE_LIBRARY 75 | PROFILER_LIBRARY 76 | GOOGLE_PERFTOOLS_INCLUDE_DIR 77 | ) 78 | -------------------------------------------------------------------------------- /cmake/FindOpenCL.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # This file taken from FindOpenCL project @ http://gitorious.com/findopencl 3 | # 4 | # - Try to find OpenCL 5 | # This module tries to find an OpenCL implementation on your system. It supports 6 | # AMD / ATI, Apple and NVIDIA implementations, but shoudl work, too. 7 | # 8 | # Once done this will define 9 | # OPENCL_FOUND - system has OpenCL 10 | # OPENCL_INCLUDE_DIRS - the OpenCL include directory 11 | # OPENCL_LIBRARIES - link these to use OpenCL 12 | # 13 | # WIN32 should work, but is untested 14 | 15 | FIND_PACKAGE( PackageHandleStandardArgs ) 16 | 17 | SET (OPENCL_VERSION_STRING "0.1.0") 18 | SET (OPENCL_VERSION_MAJOR 0) 19 | SET (OPENCL_VERSION_MINOR 1) 20 | SET (OPENCL_VERSION_PATCH 0) 21 | 22 | IF (APPLE) 23 | 24 | FIND_LIBRARY(OPENCL_LIBRARIES OpenCL DOC "OpenCL lib for OSX") 25 | FIND_PATH(OPENCL_INCLUDE_DIRS OpenCL/cl.h DOC "Include for OpenCL on OSX") 26 | FIND_PATH(_OPENCL_CPP_INCLUDE_DIRS OpenCL/cl.hpp DOC "Include for OpenCL CPP bindings on OSX") 27 | 28 | ELSE (APPLE) 29 | 30 | IF (WIN32) 31 | 32 | FIND_PATH(OPENCL_INCLUDE_DIRS CL/cl.h) 33 | FIND_PATH(_OPENCL_CPP_INCLUDE_DIRS CL/cl.hpp) 34 | 35 | # The AMD SDK currently installs both x86 and x86_64 libraries 36 | # This is only a hack to find out architecture 37 | IF( ${CMAKE_SYSTEM_PROCESSOR} STREQUAL "AMD64" ) 38 | SET(OPENCL_LIB_DIR "$ENV{ATISTREAMSDKROOT}/lib/x86_64") 39 | SET(OPENCL_LIB_DIR "$ENV{ATIINTERNALSTREAMSDKROOT}/lib/x86_64") 40 | ELSE (${CMAKE_SYSTEM_PROCESSOR} STREQUAL "AMD64") 41 | SET(OPENCL_LIB_DIR "$ENV{ATISTREAMSDKROOT}/lib/x86") 42 | SET(OPENCL_LIB_DIR "$ENV{ATIINTERNALSTREAMSDKROOT}/lib/x86") 43 | ENDIF( ${CMAKE_SYSTEM_PROCESSOR} STREQUAL "AMD64" ) 44 | 45 | # find out if the user asked for a 64-bit build, and use the corresponding 46 | # 64 or 32 bit NVIDIA library paths to the search: 47 | STRING(REGEX MATCH "Win64" ISWIN64 ${CMAKE_GENERATOR}) 48 | IF("${ISWIN64}" STREQUAL "Win64") 49 | FIND_LIBRARY(OPENCL_LIBRARIES OpenCL.lib $ENV{CUDA_PATH}/lib/x64 ${OPENCL_LIB_DIR} ) 50 | ELSE("${ISWIN64}" STREQUAL "Win64") 51 | FIND_LIBRARY(OPENCL_LIBRARIES OpenCL.lib $ENV{CUDA_PATH}/lib/Win32 ${OPENCL_LIB_DIR} ) 52 | ENDIF("${ISWIN64}" STREQUAL "Win64") 53 | 54 | GET_FILENAME_COMPONENT(_OPENCL_INC_CAND ${OPENCL_LIB_DIR}/../../include ABSOLUTE) 55 | 56 | # On Win32 search relative to the library 57 | FIND_PATH(OPENCL_INCLUDE_DIRS CL/cl.h PATHS "${_OPENCL_INC_CAND}" $ENV{CUDA_INC_PATH} $ENV{CUDA_PATH}/include) 58 | FIND_PATH(_OPENCL_CPP_INCLUDE_DIRS CL/cl.hpp PATHS "${_OPENCL_INC_CAND}" $ENV{CUDA_INC_PATH} $ENV{CUDA_PATH}/include) 59 | 60 | ELSE (WIN32) 61 | 62 | # Unix style platforms 63 | FIND_LIBRARY(OPENCL_LIBRARIES OpenCL 64 | ENV LD_LIBRARY_PATH 65 | ) 66 | 67 | GET_FILENAME_COMPONENT(OPENCL_LIB_DIR ${OPENCL_LIBRARIES} PATH) 68 | GET_FILENAME_COMPONENT(_OPENCL_INC_CAND ${OPENCL_LIB_DIR}/../../include ABSOLUTE) 69 | 70 | # The AMD SDK currently does not place its headers 71 | # in /usr/include, therefore also search relative 72 | # to the library 73 | FIND_PATH(OPENCL_INCLUDE_DIRS CL/cl.h PATHS ${_OPENCL_INC_CAND} "/usr/local/cuda/include") 74 | FIND_PATH(_OPENCL_CPP_INCLUDE_DIRS CL/cl.hpp PATHS ${_OPENCL_INC_CAND} "/usr/local/cuda/include") 75 | 76 | ENDIF (WIN32) 77 | 78 | ENDIF (APPLE) 79 | 80 | FIND_PACKAGE_HANDLE_STANDARD_ARGS( OpenCL DEFAULT_MSG OPENCL_LIBRARIES OPENCL_INCLUDE_DIRS ) 81 | 82 | IF( _OPENCL_CPP_INCLUDE_DIRS ) 83 | SET( OPENCL_HAS_CPP_BINDINGS TRUE ) 84 | LIST( APPEND OPENCL_INCLUDE_DIRS ${_OPENCL_CPP_INCLUDE_DIRS} ) 85 | # This is often the same, so clean up 86 | LIST( REMOVE_DUPLICATES OPENCL_INCLUDE_DIRS ) 87 | ENDIF( _OPENCL_CPP_INCLUDE_DIRS ) 88 | 89 | MARK_AS_ADVANCED( 90 | OPENCL_INCLUDE_DIRS 91 | ) 92 | 93 | -------------------------------------------------------------------------------- /cpp/README: -------------------------------------------------------------------------------- 1 | ++++++++++++++++++++++ 2 | Function Description 3 | ++++++++++++++++++++++ 4 | ***core*** 5 | 6 | ==add_control_noise== 7 | adds noise to control input (i.e velocities) 8 | 9 | ==compute_jacobians== 10 | mostly used for Kalman Filter measurement update 11 | 12 | ==data_associate_known== 13 | for this FastSLAM implementation, we assume that 14 | data_association is known (landmarks are identifiable) 15 | 16 | ==KF_cholesky_update== 17 | 18 | ==multivariate_gauss== 19 | 20 | 21 | ==predict_true== 22 | 23 | ==stratified_resample== 24 | Resamples particles based on importance weight 25 | 26 | ==add_feature== 27 | 28 | ==compute_steering== 29 | ==feature_update== 30 | ==KF_joseph_update== 31 | ==particle== 32 | ==resample_particles== 33 | ==TransformToGlobal== 34 | ==add_observation_noise== 35 | ==configfile== 36 | ==get_observations== 37 | ==line_plot_conversion== 38 | ==pi_to_pi== 39 | ==stratified_random== 40 | 41 | ***fastslam1*** 42 | ==compute_weight== 43 | computes weight of the particle for importance sampling 44 | to generate new set of particles 45 | 46 | ==fastslam1_sim == 47 | where all the other functions are called. 48 | 49 | ==main== 50 | calls fastslam1_sim, which returns final set of particles 51 | 52 | ==predict== 53 | predicts robot position 54 | 55 | +++++++++++++++++++++ 56 | Algorithm 57 | +++++++++++++++++++++ 58 | 59 | Do the following M times: 60 | - Retrieve a pose x from particle set Y 61 | - Sample a new pose x given previous pose and controls: p(x_t | x_t-1, u_t) 62 | - For each observed feature, identify correspondence for measurement and incorporate measurement into corresponding EKF by updating mean and covariance 63 | - Calculate importance weight for new particle 64 | - Resampling: sample (with replacement!) M particles (using weight). 65 | 66 | ++++++++++++++++++++++ 67 | Variables 68 | ++++++++++++++++++++++ 69 | 70 | =====particles===== 71 | w = importance weight 72 | xv = robot pose? (x, y, theta) - aka path estimate 73 | Pv = control input (i.e. velocities) 74 | xf = means for EKF 75 | Pf = set of covariance matrix for kalman filter gaussian representing nth feature location 76 | length should equal number of landmarks the particle sees at time t 77 | da = not sure (it's never used) 78 | 79 | w.lm = landmark positions (range, bearing) 80 | w.wp = way points defining robots path (bcs this is known, cond'l independence is possible 81 | 82 | 83 | =====Particle Filter======= 84 | with M particles and N features, number of particle filters will be 1+MN 85 | particle filter is used to estimate path posterior (robot position). 86 | 87 | =====Kalman Filter (EKF)======== 88 | Kalman filter is used to estimate the map feature locations. 89 | Each particle posses its own set of EKF since each EKF is conditioned on a robot path. 90 | 91 | =====Fast SLAM Notes======= 92 | Features are conditionally indpendent given the robot path 93 | 94 | 95 | -------------------------------------------------------------------------------- /cpp/core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(${EIGEN3_INCLUDE_DIR}) 2 | 3 | include_directories(${CMAKE_CURRENT_BINARY_DIR}/..) 4 | 5 | # Produce a FastSLAM shared library. (For re-use of executables) 6 | add_library(FastSLAM_core 7 | add_control_noise.cpp compute_steering.cpp get_observations.cpp stratified_resample.cpp 8 | add_feature.cpp configfile.cpp KF_cholesky_update.cpp multivariate_gauss.cpp resample_particles.cpp TransformToGlobal.cpp 9 | add_observation_noise.cpp data_associate_known.cpp KF_joseph_update.cpp particle.cpp predict_true.cpp 10 | compute_jacobians.cpp feature_update.cpp line_plot_conversion.cpp pi_to_pi.cpp stratified_random.cpp 11 | read_input_file.cpp 12 | ) 13 | target_link_libraries(FastSLAM_core) # We don't depend on anything 14 | 15 | install(FILES 16 | add_control_noise.h compute_jacobians.h data_associate_known.h KF_cholesky_update.h multivariate_gauss.h stratified_random.h predict_true.h 17 | add_feature.h compute_steering.h feature_update.h KF_joseph_update.h particle.h printMat.h stratified_resample.h 18 | add_observation_noise.h configfile.h get_observations.h line_plot_conversion.h pi_to_pi.h resample_particles.h TransformToGlobal.h 19 | utilities.h read_input_file.h 20 | DESTINATION include/fslam/core 21 | ) 22 | 23 | install(TARGETS 24 | FastSLAM_core 25 | DESTINATION lib 26 | ) 27 | 28 | # Add files that are just example data 29 | install( FILES example_webmap.mat 30 | PERMISSIONS GROUP_READ OWNER_READ 31 | DESTINATION bin ) 32 | -------------------------------------------------------------------------------- /cpp/core/KF_cholesky_update.cpp: -------------------------------------------------------------------------------- 1 | #include "KF_cholesky_update.h" 2 | 3 | void KF_cholesky_update(VectorXf &x, MatrixXf &P,VectorXf v,MatrixXf R,MatrixXf H) 4 | { 5 | MatrixXf PHt = P*H.transpose(); 6 | MatrixXf S = H*PHt + R; 7 | 8 | S = (S+S.transpose()) * 0.5; //make symmetric 9 | MatrixXf SChol = S.llt().matrixL(); 10 | SChol.transpose(); 11 | SChol.conjugate(); 12 | 13 | MatrixXf SCholInv = SChol.inverse(); //tri matrix 14 | MatrixXf W1 = PHt * SCholInv; 15 | MatrixXf W = W1 * SCholInv.transpose(); 16 | 17 | x = x + W*v; 18 | P = P - W1*W1.transpose(); 19 | } 20 | -------------------------------------------------------------------------------- /cpp/core/KF_cholesky_update.h: -------------------------------------------------------------------------------- 1 | #ifndef KF_CHOLESKY_UPDATE_H 2 | #define KF_CHOLESKY_UPDATE_H 3 | 4 | #include 5 | 6 | using namespace Eigen; 7 | 8 | void KF_cholesky_update(VectorXf &x,MatrixXf &P,VectorXf v,MatrixXf R,MatrixXf H); 9 | 10 | #endif 11 | -------------------------------------------------------------------------------- /cpp/core/KF_joseph_update.cpp: -------------------------------------------------------------------------------- 1 | #include "KF_joseph_update.h" 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | 7 | void KF_joseph_update(VectorXf &x, MatrixXf &P,float v,float R, MatrixXf H) 8 | { 9 | VectorXf PHt = P*H.transpose(); 10 | MatrixXf S = H*PHt; 11 | S(0,0) += R; 12 | MatrixXf Si = S.inverse(); 13 | Si = make_symmetric(Si); 14 | MatrixXf PSD_check = Si.llt().matrixL(); //chol of scalar is sqrt 15 | PSD_check.transpose(); 16 | PSD_check.conjugate(); 17 | 18 | VectorXf W = PHt*Si; 19 | x = x+W*v; 20 | 21 | //Joseph-form covariance update 22 | MatrixXf eye(P.rows(), P.cols()); 23 | eye.setIdentity(); 24 | MatrixXf C = eye - W*H; 25 | P = C*P*C.transpose() + W*R*W.transpose(); 26 | 27 | float eps = 2.2204*pow(10.0,-16); //numerical safety 28 | P = P+eye*eps; 29 | 30 | PSD_check = P.llt().matrixL(); 31 | PSD_check.transpose(); 32 | PSD_check.conjugate(); //for upper tri 33 | } 34 | 35 | MatrixXf make_symmetric(MatrixXf P) 36 | { 37 | return (P + P.transpose())*0.5; 38 | } 39 | -------------------------------------------------------------------------------- /cpp/core/KF_joseph_update.h: -------------------------------------------------------------------------------- 1 | #ifndef KF_JOSEPH_UPDATE_H 2 | #define KF_JOSEPH_UPDATE_H 3 | 4 | #include 5 | 6 | using namespace Eigen; 7 | 8 | void KF_joseph_update(VectorXf &x,MatrixXf &P,float v,float R, MatrixXf H); 9 | MatrixXf make_symmetric(MatrixXf P); 10 | 11 | #endif 12 | -------------------------------------------------------------------------------- /cpp/core/TransformToGlobal.cpp: -------------------------------------------------------------------------------- 1 | #include "TransformToGlobal.h" 2 | #include "pi_to_pi.h" 3 | 4 | void TransformToGlobal(MatrixXf &p, VectorXf b) 5 | { 6 | //rotate 7 | MatrixXf rot(2,2); 8 | rot< 5 | #include 6 | 7 | using namespace Eigen; 8 | 9 | void TransformToGlobal(MatrixXf &p, VectorXf b); 10 | 11 | #endif //TRANSFORMGLOBAL_H 12 | -------------------------------------------------------------------------------- /cpp/core/add_control_noise.cpp: -------------------------------------------------------------------------------- 1 | #include "add_control_noise.h" 2 | #include 3 | 4 | using namespace std; 5 | 6 | void add_control_noise(float V, float G, Matrix2f Q, int addnoise, float* VnGn) 7 | { 8 | if (addnoise ==1) { 9 | VectorXf A(2); 10 | A(0) = V; 11 | A(1) = G; 12 | VectorXf C(2); 13 | C = multivariate_gauss(A,Q,1); 14 | VnGn[0] = C(0); 15 | VnGn[1] = C(1); 16 | } 17 | } 18 | -------------------------------------------------------------------------------- /cpp/core/add_control_noise.h: -------------------------------------------------------------------------------- 1 | #ifndef ADD_CONTROL_NOISE 2 | #define ADD_CONTROL_NOISE 3 | 4 | #include 5 | #include "multivariate_gauss.h" 6 | # include 7 | # include 8 | 9 | using namespace Eigen; 10 | 11 | //MatrixXf randn(int m, int n); 12 | //MatrixXf rand(int m, int n); 13 | void add_control_noise(float V, float G, Matrix2f Q, int addnoise,float* VnGn); 14 | 15 | #endif //ADD_CONTROL_NOISE 16 | -------------------------------------------------------------------------------- /cpp/core/add_feature.cpp: -------------------------------------------------------------------------------- 1 | #include "add_feature.h" 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | 8 | // 9 | // add new features 10 | // 11 | void add_feature(Particle &particle, vector z, MatrixXf R) 12 | { 13 | int lenz = z.size(); 14 | vector xf; 15 | vector Pf; 16 | VectorXf xv = particle.xv(); 17 | 18 | float r,b,s,c; 19 | MatrixXf Gz(2,2); 20 | 21 | for (int i=0; i ii; 38 | for (int i=lenx; i 5 | 6 | #include "particle.h" 7 | 8 | using namespace Eigen; 9 | 10 | void add_feature(Particle &particle, vector z, MatrixXf R); 11 | 12 | #endif //ADD_FEATURE_H 13 | 14 | -------------------------------------------------------------------------------- /cpp/core/add_observation_noise.cpp: -------------------------------------------------------------------------------- 1 | #include "add_observation_noise.h" 2 | #if 0 3 | //http://moby.ihme.washington.edu/bradbell/mat2cpp/randn.cpp.xml 4 | MatrixXf randn(int m, int n) 5 | { 6 | // use formula 30.3 of Statistical Distributions (3rd ed) 7 | // Merran Evans, Nicholas Hastings, and Brian Peacock 8 | int urows = m * n + 1; 9 | MatrixXf u(urows, 1); 10 | 11 | //u is a random matrix 12 | for (int r=0; r &z, MatrixXf R, int addnoise) 57 | { 58 | float LO = -1.0f; 59 | float HI = 1.0f; 60 | 61 | if (addnoise == 1){ 62 | int len = z.size(); 63 | if (len > 0) { 64 | //MatrixXf randM1 = nRandMat::randn(1,len); 65 | MatrixXf randM1(1,len); 66 | for (int i=0; i< len; i++) { 67 | float r3 = LO + (float)rand()/((float)RAND_MAX/(HI-LO)); 68 | randM1(0,i) = r3; 69 | } 70 | //MatrixXf randM2 = nRandMat::randn(1,len); 71 | MatrixXf randM2(1,len); 72 | for (int j=0; j< len; j++) { 73 | float r4 = LO + (float)rand()/((float)RAND_MAX/(HI-LO)); 74 | randM2(0,j) = r4; 75 | } 76 | //cout<<"randM1"< 5 | #include 6 | #include 7 | 8 | using namespace Eigen; 9 | using namespace std; 10 | 11 | namespace nRandMat{ 12 | MatrixXf randn(int m, int n); 13 | MatrixXf rand(int m, int n); 14 | } 15 | 16 | void add_observation_noise(vector &z, MatrixXf R, int addnoise); 17 | 18 | #endif //ADD_OBSERVATION_NOISE_H 19 | -------------------------------------------------------------------------------- /cpp/core/compute_jacobians.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "compute_jacobians.h" 3 | #include "math.h" 4 | #include "pi_to_pi.h" 5 | 6 | void compute_jacobians(Particle particle, 7 | vector idf, 8 | MatrixXf R, 9 | vector &zp, //measurement (range, bearing) 10 | vector *Hv, // jacobians of function h (deriv of h wrt pose) 11 | vector *Hf, // jacobians of function h (deriv of h wrt mean) 12 | vector *Sf) //measurement covariance 13 | { 14 | VectorXf xv = particle.xv(); 15 | 16 | int rows = particle.xf().size(); 17 | vector xf; 18 | vector Pf; 19 | 20 | unsigned i; 21 | int r; 22 | for (unsigned i=0; ipush_back(HvMat); 54 | Hf->push_back(HfMat); 55 | 56 | //innovation covariance of feature observation given the vehicle' 57 | MatrixXf SfMat = HfMat*Pf[i]*HfMat.transpose() + R; 58 | Sf->push_back(SfMat); 59 | } 60 | } 61 | -------------------------------------------------------------------------------- /cpp/core/compute_jacobians.h: -------------------------------------------------------------------------------- 1 | #ifndef COMPUTE_JACOBIANS_H 2 | #define COMPUTE_JACOBIANS_H 3 | 4 | #include 5 | #include "particle.h" 6 | #include 7 | 8 | using namespace std; 9 | using namespace Eigen; 10 | 11 | void compute_jacobians(Particle particle, 12 | vector idf, 13 | MatrixXf R, 14 | vector &zp, 15 | vector *Hv, 16 | vector *Hf, 17 | vector *Sf); 18 | 19 | #endif //COMPUTE_JACOBIANS_H 20 | -------------------------------------------------------------------------------- /cpp/core/compute_steering.cpp: -------------------------------------------------------------------------------- 1 | #include "compute_steering.h" 2 | #include "pi_to_pi.h" 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace std; 9 | 10 | void compute_steering(VectorXf x, MatrixXf wp, int& iwp, float minD, 11 | float& G, float rateG, float maxG, float dt) 12 | { 13 | /* 14 | % INPUTS: 15 | % x - true position 16 | % wp - waypoints 17 | % iwp - index to current waypoint 18 | % minD - minimum distance to current waypoint before switching to next 19 | % G - current steering angle 20 | % rateG - max steering rate (rad/s) 21 | % maxG - max steering angle (rad) 22 | % dt - timestep 23 | */ 24 | 25 | //determine if current waypoint reached 26 | Vector2d cwp; 27 | cwp[0] = wp(0,iwp); //-1 since indexed from 0 28 | cwp[1] = wp(1,iwp); 29 | 30 | float d2 = pow((cwp[0] - x[0]),2) + pow((cwp[1]-x[1]),2); 31 | 32 | if (d2 < minD*minD) { 33 | iwp++; //switch to next 34 | if (iwp >= wp.cols()) { 35 | iwp =-1; 36 | return; 37 | } 38 | 39 | cwp[0] = wp(0,iwp); //-1 since indexed from 0 40 | cwp[1] = wp(1,iwp); 41 | } 42 | 43 | //compute change in G to point towards current waypoint 44 | float deltaG = atan2(cwp[1]-x[1], cwp[0]-x[0]) - x[2] - G; 45 | deltaG = pi_to_pi(deltaG); 46 | 47 | //limit rate 48 | float maxDelta = rateG*dt; 49 | if (abs(deltaG) > maxDelta) { 50 | int sign = (deltaG > 0) ? 1 : ((deltaG < 0) ? -1 : 0); 51 | deltaG = sign*maxDelta; 52 | } 53 | 54 | //limit angle 55 | G = G+deltaG; 56 | if (abs(G) > maxG) { 57 | int sign2 = (G > 0) ? 1: ((G < 0) ? -1 : 0); 58 | G = sign2*maxG; 59 | } 60 | } 61 | -------------------------------------------------------------------------------- /cpp/core/compute_steering.h: -------------------------------------------------------------------------------- 1 | #ifndef COMPUTE_STEERING_H 2 | #define COMPUTE_STEERING_H 3 | 4 | #include 5 | 6 | using namespace Eigen; 7 | 8 | void compute_steering(VectorXf x, MatrixXf wp, int& iwp, float minD, 9 | float& G, float rateG, float maxG, float dt); 10 | 11 | #endif //COMPUTE_STEERING_H 12 | -------------------------------------------------------------------------------- /cpp/core/configfile.cpp: -------------------------------------------------------------------------------- 1 | #include "configfile.h" 2 | 3 | #define pi 3.14159265 4 | 5 | // Configuration file 6 | //Permits various adjustments to parameters of the FastSLAM algorithm. 7 | // See fastslam_sim.h for more information 8 | 9 | // control parameters 10 | float config::V= 3.0; // m/s 11 | float config::MAXG= 30*pi/180; // radians, maximum steering angle (-MAXG < g < MAXG) 12 | float config::RATEG= 20*pi/180; // rad/s, maximum rate of change in steer angle 13 | int config::WHEELBASE= 4; // metres, vehicle wheel-base 14 | float config::DT_CONTROLS= 0.025; // seconds, time interval between control signals 15 | 16 | // control noises 17 | float config::sigmaV= 0.3; // m/s 18 | float config::sigmaG= (3.0*pi/180); // radians 19 | 20 | Eigen::MatrixXf config::Q(2,2); 21 | 22 | // observation parameters 23 | float config::MAX_RANGE= 30.0; // metres 24 | float config::DT_OBSERVE= 8* config::DT_CONTROLS; // seconds, time interval between observations 25 | 26 | // observation noises 27 | float config::sigmaR= 0.1; // metres 28 | float config::sigmaB= (1.0*pi/180); // radians 29 | 30 | Eigen::MatrixXf config::R(2,2); 31 | 32 | // waypoint proximity 33 | float config::AT_WAYPOINT= 1.0; // metres, distance from current waypoint at which to switch to next waypoint 34 | int config::NUMBER_LOOPS= 2; // number of loops through the waypoint list 35 | 36 | // resampling 37 | unsigned int config::NPARTICLES= 100; 38 | float config::NEFFECTIVE= 0.75* config::NPARTICLES; // minimum number of effective particles before resampling 39 | 40 | // switches 41 | int config::SWITCH_CONTROL_NOISE= 1; 42 | int config::SWITCH_SENSOR_NOISE = 1; 43 | int config::SWITCH_INFLATE_NOISE= 0; 44 | int config::SWITCH_PREDICT_NOISE = 0; // sample noise from predict (usually 1 for fastslam1.0 and 0 for fastslam2.0) 45 | int config::SWITCH_SAMPLE_PROPOSAL = 1; // sample from proposal (no effect on fastslam1.0 and usually 1 for fastslam2.0) 46 | int config::SWITCH_HEADING_KNOWN= 0; 47 | int config::SWITCH_RESAMPLE= 1; 48 | int config::SWITCH_PROFILE= 1; 49 | int config::SWITCH_SEED_RANDOM= 0; // if not 0, seed the randn() with its value at beginning of simulation (for repeatability) 50 | 51 | -------------------------------------------------------------------------------- /cpp/core/configfile.h: -------------------------------------------------------------------------------- 1 | #ifndef CONFIGFILE_H 2 | #define CONFIGFILE_H 3 | 4 | #include 5 | 6 | //****************** 7 | // Global Variables 8 | //****************** 9 | 10 | namespace config { 11 | extern float V; 12 | extern float MAXG; 13 | extern float RATEG; 14 | extern int WHEELBASE; 15 | extern float DT_CONTROLS; 16 | 17 | extern float sigmaV; 18 | extern float sigmaG; 19 | 20 | extern Eigen::MatrixXf Q; 21 | 22 | extern float MAX_RANGE; 23 | extern float DT_OBSERVE; 24 | 25 | extern float sigmaR; 26 | extern float sigmaB; 27 | 28 | extern Eigen::MatrixXf R; 29 | 30 | extern float AT_WAYPOINT; 31 | extern int NUMBER_LOOPS; 32 | 33 | extern unsigned int NPARTICLES; 34 | extern float NEFFECTIVE; 35 | 36 | extern int SWITCH_CONTROL_NOISE; 37 | extern int SWITCH_SENSOR_NOISE; 38 | extern int SWITCH_INFLATE_NOISE; 39 | extern int SWITCH_PREDICT_NOISE; 40 | 41 | extern int SWITCH_SAMPLE_PROPOSAL; 42 | extern int SWITCH_HEADING_KNOWN; 43 | extern int SWITCH_RESAMPLE; 44 | extern int SWITCH_PROFILE; 45 | extern int SWITCH_SEED_RANDOM; 46 | } 47 | #endif //CONFIGFILE_H 48 | -------------------------------------------------------------------------------- /cpp/core/data_associate_known.cpp: -------------------------------------------------------------------------------- 1 | #include "data_associate_known.h" 2 | #include 3 | 4 | //z is range and bearing of visible landmarks 5 | void data_associate_known(vector z, vector idz, VectorXf &table, int Nf, \ 6 | vector &zf, vector &idf, vector &zn) 7 | { 8 | idf.clear(); 9 | vector idn; 10 | 11 | unsigned i,ii,r; 12 | 13 | for (i =0; i< idz.size(); i++){ 14 | ii = idz[i]; 15 | VectorXf z_i; 16 | if (table(ii) ==-1) { //new feature 17 | z_i = z[i]; 18 | zn.push_back(z_i); 19 | idn.push_back(ii); 20 | } 21 | else { 22 | z_i = z[i]; 23 | zf.push_back(z_i); 24 | idf.push_back(table(ii)); 25 | } 26 | } 27 | 28 | assert(idn.size() == zn.size()); 29 | for (int i=0; i 5 | #include 6 | 7 | using namespace std; 8 | using namespace Eigen; 9 | 10 | void data_associate_known(vector z, vector idz, VectorXf &table, int Nf, \ 11 | vector &zf, vector &idf, vector &zn); 12 | 13 | #endif //DATA_ASSOCIATE_KNOWN_H 14 | -------------------------------------------------------------------------------- /cpp/core/example_webmap.mat: -------------------------------------------------------------------------------- 1 | #type columns rows 2 | lm 2 35 3 | 2.9922 -25.7009 4 | 32.8988 -33.1776 5 | 24.7991 -68.3801 6 | 75.2664 -65.5763 7 | 73.7087 -35.6698 8 | 98.6308 3.8941 9 | 49.4097 26.9470 10 | 80.2508 59.9688 11 | 54.7056 88.6293 12 | 13.2726 80.8411 13 | -16.3224 49.0654 14 | -65.8551 83.6449 15 | -96.3847 60.5919 16 | -76.1355 36.2928 17 | -87.3505 -21.3396 18 | -103.5498 -32.2430 19 | -92.9579 -77.7259 20 | -55.8863 -55.9190 21 | -35.3255 -19.1589 22 | -56.1978 16.3551 23 | -7.2882 19.7819 24 | 25.7336 3.2710 25 | -19.5610 80.1444 26 | -41.7506 46.2325 27 | 25.4021 26.8543 28 | 91.3870 28.2385 29 | -13.7216 -79.0338 30 | -52.8454 -92.1833 31 | -86.1298 15.0890 32 | -127.0053 22.7018 33 | -25.9843 4.7078 34 | 56.3508 -17.4388 35 | 51.6793 -83.1863 36 | 21.3146 -96.3358 37 | 48.1757 57.9979 38 | 39 | wp 2 17 40 | 12.6495 -41.5888 41 | 44.7368 -54.9844 42 | 85.5467 -45.0156 43 | 93.6464 -17.2897 44 | 64.9860 5.7632 45 | 71.8396 31.6199 46 | 71.2165 70.5607 47 | 33.5218 76.4798 48 | 12.9611 51.5576 49 | -32.5218 67.4455 50 | -74.8894 69.0031 51 | -97.3193 41.9003 52 | -107.5997 6.3863 53 | -86.4159 -25.7009 54 | -83.9237 -64.0187 55 | -39.3754 -81.4642 56 | -17.5685 -51.5576 57 | -------------------------------------------------------------------------------- /cpp/core/example_webmap_6D.mat: -------------------------------------------------------------------------------- 1 | #type columns rows 2 | lm 2 35 3 | 2.9922 -25.7009 4 | 32.8988 -33.1776 5 | 24.7991 -68.3801 6 | 75.2664 -65.5763 7 | 73.7087 -35.6698 8 | 98.6308 3.8941 9 | 49.4097 26.9470 10 | 80.2508 59.9688 11 | 54.7056 88.6293 12 | 13.2726 80.8411 13 | -16.3224 49.0654 14 | -65.8551 83.6449 15 | -96.3847 60.5919 16 | -76.1355 36.2928 17 | -87.3505 -21.3396 18 | -103.5498 -32.2430 19 | -92.9579 -77.7259 20 | -55.8863 -55.9190 21 | -35.3255 -19.1589 22 | -56.1978 16.3551 23 | -7.2882 19.7819 24 | 25.7336 3.2710 25 | -19.5610 80.1444 26 | -41.7506 46.2325 27 | 25.4021 26.8543 28 | 91.3870 28.2385 29 | -13.7216 -79.0338 30 | -52.8454 -92.1833 31 | -86.1298 15.0890 32 | -127.0053 22.7018 33 | -25.9843 4.7078 34 | 56.3508 -17.4388 35 | 51.6793 -83.1863 36 | 21.3146 -96.3358 37 | 48.1757 57.9979 38 | 39 | wp 2 17 40 | 12.6495 -41.5888 41 | 44.7368 -54.9844 42 | 85.5467 -45.0156 43 | 93.6464 -17.2897 44 | 64.9860 5.7632 45 | 71.8396 31.6199 46 | 71.2165 70.5607 47 | 33.5218 76.4798 48 | 12.9611 51.5576 49 | -32.5218 67.4455 50 | -74.8894 69.0031 51 | -97.3193 41.9003 52 | -107.5997 6.3863 53 | -86.4159 -25.7009 54 | -83.9237 -64.0187 55 | -39.3754 -81.4642 56 | -17.5685 -51.5576 57 | -------------------------------------------------------------------------------- /cpp/core/example_webmap_simple.mat: -------------------------------------------------------------------------------- 1 | #type columns rows 2 | lm 2 3 3 | -16.9355 1.0234 4 | -3.1106 32.3099 5 | 19.0092 17.3977 6 | 7 | wp 2 4 8 | 0 0 9 | -19.9309 8.3333 10 | 11.1751 29.0936 11 | 15.3226 10.9649 12 | -------------------------------------------------------------------------------- /cpp/core/feature_update.cpp: -------------------------------------------------------------------------------- 1 | #include "feature_update.h" 2 | #include 3 | 4 | using namespace std; 5 | 6 | //z is the list of measurements conditioned on the particle. 7 | void feature_update(Particle &particle, vector z, vectoridf, MatrixXf R) 8 | { 9 | //Having selected a new pose from the proposal distribution, this pose is assumed perfect and each feature update maybe computed independently and without pose uncertainty 10 | int rows = 2; //2d mean for EKF 11 | vector xf; //updated EKF means 12 | vector Pf; //updated EKF covariances 13 | 14 | for (unsigned i=0; i zp; 20 | vector Hv; 21 | vector Hf; 22 | vector Sf; 23 | 24 | compute_jacobians(particle,idf,R,zp,&Hv,&Hf,&Sf); 25 | 26 | vector v; //difference btw two measurements (used to update mean) 27 | for (int i=0; i 5 | #include 6 | 7 | #include "particle.h" 8 | #include "compute_jacobians.h" 9 | #include "pi_to_pi.h" 10 | #include "KF_cholesky_update.h" 11 | 12 | using namespace Eigen; 13 | using namespace std; 14 | 15 | 16 | void feature_update(Particle &particle, vector z, vectoridf, MatrixXf R); 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /cpp/core/get_observations.cpp: -------------------------------------------------------------------------------- 1 | #include "get_observations.h" 2 | #include 3 | #include 4 | 5 | vector get_observations(VectorXf x, MatrixXf lm, vector &idf, float rmax) 6 | { 7 | get_visible_landmarks(x,lm,idf,rmax); 8 | return compute_range_bearing(x,lm); 9 | } 10 | 11 | void get_visible_landmarks(VectorXf x, MatrixXf &lm, vector &idf, float rmax) 12 | { 13 | //select set of landmarks that are visible within vehicle's 14 | //semi-circular field of view 15 | vector dx; 16 | vector dy; 17 | 18 | for (int c=0; c ii = find2(dx,dy,phi,rmax); 27 | 28 | MatrixXf lm_new (lm.rows(), ii.size()); 29 | unsigned j,k; 30 | for (j=0; j idf_backup(idf); 38 | idf.clear(); 39 | for(int i=0; i compute_range_bearing(VectorXf x, MatrixXf lm) 45 | { 46 | vector dx; 47 | vector dy; 48 | 49 | for (int c=0; c z; 59 | 60 | for (int i =0; i find2(vector dx, vector dy, float phi, float rmax) 70 | { 71 | vector index; 72 | //incremental tests for bounding semi-circle 73 | for (int j=0; j 0.0) 76 | && (((float)pow(dx[j],2) + (float)pow(dy[j],2)) < (float)pow(rmax,2))){ 77 | index.push_back(j); 78 | } 79 | } 80 | return index; 81 | } 82 | -------------------------------------------------------------------------------- /cpp/core/get_observations.h: -------------------------------------------------------------------------------- 1 | #ifndef GET_OBSERVATIONS_H 2 | #define GET_OBSERVATIONS_H 3 | 4 | #include 5 | #include 6 | 7 | using namespace Eigen; 8 | using namespace std; 9 | 10 | vector get_observations(VectorXf x,MatrixXf lm,vector &idf,float rmax); 11 | void get_visible_landmarks(VectorXf x, MatrixXf &lm,vector &idf, float rmax); 12 | vector compute_range_bearing(VectorXf x, MatrixXf lm); 13 | vector find2(vector dx, vector dy, float phi, float rmax); 14 | 15 | #endif 16 | -------------------------------------------------------------------------------- /cpp/core/line_plot_conversion.cpp: -------------------------------------------------------------------------------- 1 | #include "line_plot_conversion.h" 2 | #include 3 | 4 | using namespace std; 5 | 6 | MatrixXf line_plot_conversion(MatrixXf lnes) 7 | { 8 | int len = lnes.cols()*3 -1; 9 | MatrixXf p(2,len); 10 | 11 | for (int j=0; j 5 | 6 | using namespace Eigen; 7 | 8 | MatrixXf line_plot_conversion(MatrixXf lnes); 9 | 10 | #endif //LINE_PLOT_CONVERSION_H 11 | -------------------------------------------------------------------------------- /cpp/core/multivariate_gauss.cpp: -------------------------------------------------------------------------------- 1 | #include "multivariate_gauss.h" 2 | #include "add_observation_noise.h" 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | //x is mean vector 8 | //P is covariance matrix 9 | //n obtain n samples 10 | 11 | //output: sample set 12 | 13 | VectorXf multivariate_gauss(VectorXf x, MatrixXf P, int n) 14 | { 15 | int len = x.size(); 16 | //choleksy decomposition 17 | MatrixXf S = P.llt().matrixL(); 18 | MatrixXf X(len,n); 19 | 20 | 21 | float LO = -1.0f; 22 | float HI = 1.0f; 23 | 24 | for (int i = 0; i < len; i++) { 25 | for (int j=0; j< n; j++) { 26 | float r3 = LO + (float)rand()/((float)RAND_MAX/(HI-LO)); 27 | X(i,j) = r3; 28 | } 29 | } 30 | 31 | //TODO: this does not work. Also fixed other instances of nRandMat 32 | //X = nRandMat::randn(len,n); 33 | 34 | MatrixXf ones = MatrixXf::Ones(1,n); 35 | return S*X + x*ones; 36 | } 37 | 38 | -------------------------------------------------------------------------------- /cpp/core/multivariate_gauss.h: -------------------------------------------------------------------------------- 1 | #ifndef MULTIVARIATE_GAUSS_H 2 | #define MULTIVARIATE_GAUSS_H 3 | 4 | #include 5 | 6 | using namespace Eigen; 7 | 8 | VectorXf multivariate_gauss(VectorXf x, MatrixXf P, int n); 9 | 10 | #endif //MULTIVARIATE_GAUSS_H 11 | 12 | -------------------------------------------------------------------------------- /cpp/core/particle.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "particle.h" 3 | #include 4 | 5 | /************* 6 | ** Particle 7 | *************/ 8 | 9 | Particle::Particle() 10 | { 11 | _w = 1.0; 12 | _xv = VectorXf(3); 13 | _xv.setZero(); 14 | _Pv = MatrixXf(3,3); 15 | _Pv.setZero(); 16 | _da = NULL; 17 | } 18 | 19 | Particle::Particle(float w, VectorXf &xv, MatrixXf &Pv, vector &xf, vector &Pf, float* da) 20 | { 21 | _w = w; 22 | _xv = xv; 23 | _Pv = Pv; 24 | _xf = xf; 25 | _Pf = Pf; 26 | _da = da; 27 | } 28 | 29 | Particle::~Particle() 30 | { 31 | } 32 | 33 | //getters 34 | float Particle::w() const 35 | { 36 | return _w; 37 | } 38 | 39 | VectorXf Particle::xv() const 40 | { 41 | return _xv; 42 | } 43 | 44 | MatrixXf Particle::Pv() const 45 | { 46 | return _Pv; 47 | } 48 | 49 | vector Particle::xf() const 50 | { 51 | return _xf; 52 | } 53 | 54 | vector Particle::Pf() const 55 | { 56 | return _Pf; 57 | } 58 | 59 | float* Particle::da() const 60 | { 61 | return _da; 62 | } 63 | 64 | //setters 65 | void Particle::setW(float w) 66 | { 67 | _w = w; 68 | } 69 | 70 | void Particle::setXv(VectorXf &xv) 71 | { 72 | _xv = xv; 73 | } 74 | 75 | void Particle::setPv(MatrixXf &Pv) 76 | { 77 | _Pv = Pv; 78 | } 79 | 80 | void Particle::setXf(vector &xf) 81 | { 82 | _xf = xf; 83 | } 84 | 85 | void Particle::setXfi(int i, VectorXf &vec) 86 | { 87 | if (i >= _xf.size()){ 88 | _xf.resize(i+1); 89 | } 90 | _xf[i] = vec; 91 | } 92 | 93 | void Particle::setPf(vector &Pf) 94 | { 95 | _Pf = Pf; 96 | } 97 | 98 | void Particle::setPfi(int i, MatrixXf &m) 99 | { 100 | if(i >= _Pf.size()) { 101 | _Pf.resize(i+1); 102 | } 103 | _Pf[i] = m; 104 | } 105 | 106 | void Particle::setDa(float* da) 107 | { 108 | _da = da; 109 | } 110 | 111 | -------------------------------------------------------------------------------- /cpp/core/particle.h: -------------------------------------------------------------------------------- 1 | #ifndef PARTICLES_H 2 | #define PARTICLES_H 3 | 4 | #include 5 | #include 6 | 7 | using namespace Eigen; 8 | using namespace std; 9 | 10 | class Particle{ 11 | public: 12 | Particle(); 13 | Particle(float w, VectorXf &xv, MatrixXf &Pv, vector &xf, vector &Pf, float* da); 14 | ~Particle(); 15 | 16 | //getters 17 | float w() const; 18 | VectorXf xv() const; //robot pose: x,y,theta (heading dir) 19 | MatrixXf Pv() const; //controls: velocities 20 | vector xf() const; //2d means of EKF 21 | vector Pf() const; //covariance matrices for EKF 22 | float* da() const; // 23 | 24 | //setters 25 | void setW(float w); 26 | void setXv(VectorXf &xv); 27 | void setPv(MatrixXf &Pv); 28 | void setXf(vector &xf); 29 | void setXfi(int i, VectorXf &vec); 30 | void setPf(vector &Pf); 31 | void setPfi(int i, MatrixXf &m); 32 | void setDa(float* da); 33 | 34 | private: 35 | float _w; 36 | VectorXf _xv; 37 | MatrixXf _Pv; 38 | vector _xf; 39 | vector _Pf; 40 | float* _da; 41 | }; 42 | 43 | #endif //PARTICLES_H 44 | -------------------------------------------------------------------------------- /cpp/core/pi_to_pi.cpp: -------------------------------------------------------------------------------- 1 | #include "pi_to_pi.h" 2 | #include 3 | 4 | void pi_to_pi(VectorXf &angle) 5 | { 6 | int n; 7 | for (int i=0; i (2*pi))) { 9 | n=floor(angle[i]/(2*pi)); 10 | angle[i] = angle[i]-n*(2*pi); 11 | 12 | if (angle[i] > pi) { 13 | angle[i] = angle[i] - (2*pi); 14 | } 15 | if (angle[i] < -pi) { 16 | angle[i] = angle[i] + (2*pi); 17 | } 18 | } 19 | } 20 | } 21 | 22 | float pi_to_pi(float ang) 23 | { 24 | int n; 25 | if ((ang < (-2*pi)) || (ang > (2*pi))) { 26 | n=floor(ang/(2*pi)); 27 | ang = ang-n*(2*pi); 28 | 29 | if (ang > pi) { 30 | ang = ang - (2*pi); 31 | } 32 | if (ang < -pi) { 33 | ang = ang + (2*pi); 34 | } 35 | } 36 | return ang; 37 | } 38 | 39 | -------------------------------------------------------------------------------- /cpp/core/pi_to_pi.h: -------------------------------------------------------------------------------- 1 | #ifndef PI_TO_PI_H 2 | #define PI_TO_PI_H 3 | 4 | #include 5 | #include 6 | #include 7 | #define pi 3.1416 8 | 9 | using namespace Eigen; 10 | using namespace std; 11 | 12 | void pi_to_pi(VectorXf &angle); //takes in array of floats, returna array 13 | float pi_to_pi(float ang); 14 | //vector find1(VectorXf input); 15 | 16 | #endif //PI_TO_PI_H 17 | -------------------------------------------------------------------------------- /cpp/core/predict_true.cpp: -------------------------------------------------------------------------------- 1 | #include "predict_true.h" 2 | #include "pi_to_pi.h" 3 | 4 | void predict_true(VectorXf &xv, float V, float G, int WB, float dt) 5 | { 6 | xv(0) = xv(0) + V*dt*cos(G+xv(2)); 7 | xv(1) = xv(1) + V*dt*sin(G+xv(2)); 8 | xv(2) = pi_to_pi(xv(2) + V*dt*sin(G)/WB); 9 | } 10 | -------------------------------------------------------------------------------- /cpp/core/predict_true.h: -------------------------------------------------------------------------------- 1 | #ifndef PREDICT_TRUE_H 2 | #define PREDICT_TRUE_H 3 | 4 | #include 5 | 6 | using namespace Eigen; 7 | 8 | void predict_true(VectorXf &xv,float V,float G,int WB,float dt); 9 | //V is m/s 10 | // G is steering angle 11 | //WB = WHEELBASE 12 | 13 | #endif //PREDICT_TRUE_H 14 | -------------------------------------------------------------------------------- /cpp/core/printMat.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace boost::numeric::ublas; 5 | 6 | void printMat(matrix A) 7 | { 8 | for (int r = 0; r< A.size1(); A++) { 9 | for (int c = 0; c< A.size2(); A++) { 10 | printf("%f ", A(r,c)); 11 | } 12 | printf("\n"); 13 | } 14 | printf("\n"); 15 | } 16 | -------------------------------------------------------------------------------- /cpp/core/read_input_file.cpp: -------------------------------------------------------------------------------- 1 | #include "read_input_file.h" 2 | #include 3 | 4 | void read_input_file(const string s, MatrixXf *lm, MatrixXf *wp) 5 | { 6 | using std::ifstream; 7 | using std::istringstream; 8 | 9 | if(access(s.c_str(),R_OK) == -1) { 10 | std::cerr << "Unable to read input file" << s << std::endl; 11 | exit(EXIT_FAILURE); 12 | } 13 | ifstream in(s.c_str()); 14 | 15 | int lineno = 0; 16 | int lm_rows =0; 17 | int lm_cols =0; 18 | int wp_rows =0; 19 | int wp_cols =0; 20 | 21 | while(in) { 22 | lineno++; 23 | string str; 24 | getline(in,str); 25 | istringstream line(str); 26 | 27 | vector tokens; 28 | copy(istream_iterator(line), 29 | istream_iterator(), 30 | back_inserter > (tokens)); 31 | 32 | if(tokens.size() ==0) { 33 | continue; 34 | } 35 | else if (tokens[0][0] =='#') { 36 | continue; 37 | } 38 | else if (tokens[0] == "lm") { 39 | if(tokens.size() != 3) { 40 | std::cerr<<"Wrong args for lm!"<resize(lm_rows,lm_cols); 49 | for (int c =0; c tokens; 58 | copy(istream_iterator(line), 59 | istream_iterator(), 60 | back_inserter > (tokens)); 61 | if(tokens.size() < lm_rows) { 62 | std::cerr<<"invalid line for lm coordinate!"<resize(wp_rows, wp_cols); 83 | for (int c =0; c tokens; 92 | copy(istream_iterator(line), 93 | istream_iterator(), 94 | back_inserter > (tokens)); 95 | if(tokens.size() < wp_rows) { 96 | std::cerr<<"invalid line for wp coordinate!"< 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | using namespace Eigen; 12 | using namespace std; 13 | 14 | void read_input_file(const string s, MatrixXf *lm, MatrixXf *wp); -------------------------------------------------------------------------------- /cpp/core/resample_particles.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "resample_particles.h" 3 | #include "configfile.h" 4 | #include "stratified_resample.h" 5 | #include 6 | #include 7 | 8 | using namespace std; 9 | 10 | void resample_particles(vector &particles, int Nmin, int doresample) 11 | { 12 | int N = particles.size(); 13 | VectorXf w(N); 14 | w.setZero(); 15 | 16 | for (int i=0; i keep; 33 | stratified_resample(w,keep,Neff); 34 | 35 | vector old_particles = vector(particles); 36 | particles.resize(keep.size()); 37 | //do I need to clear this? 38 | 39 | 40 | if ((Neff < Nmin) && (doresample == 1)) { 41 | for(int i=0; i< keep.size(); i++) { 42 | particles[i] = old_particles[keep[i]]; 43 | } 44 | for (int i=0; i 5 | #include 6 | #include "particle.h" 7 | 8 | 9 | using namespace Eigen; 10 | using namespace std; 11 | 12 | void resample_particles(vector &particles, int Nmin, int doresample); 13 | 14 | #endif //RESAMPLE_PARTICLES_H 15 | -------------------------------------------------------------------------------- /cpp/core/stratified_random.cpp: -------------------------------------------------------------------------------- 1 | #include "stratified_random.h" 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | 7 | void stratified_random(int N, vector &di) 8 | { 9 | float k = 1.0/(float)N; 10 | 11 | //deterministic intervals 12 | float temp = k/2; 13 | di.push_back(temp); 14 | while (temp < 1-k/2) { 15 | temp = temp+k; 16 | di.push_back(temp); 17 | } 18 | /* 19 | cout<<"di"<::iterator diter; 30 | for (diter = di.begin(); diter != di.end(); diter++) { 31 | *diter = (*diter) + unifRand() * k - (k/2); 32 | } 33 | } 34 | 35 | // 36 | // Generate a random number between 0 and 1 37 | // return a uniform number in [0,1]. 38 | double unifRand() 39 | { 40 | return rand() / double(RAND_MAX); 41 | } 42 | -------------------------------------------------------------------------------- /cpp/core/stratified_random.h: -------------------------------------------------------------------------------- 1 | #ifndef STRATIFIED_RANDOM_H 2 | #define STRATIFIED_RANDOM_H 3 | 4 | #include 5 | #include 6 | 7 | using namespace Eigen; 8 | using namespace std; 9 | 10 | void stratified_random(int N, vector &di); 11 | double unifRand(); 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /cpp/core/stratified_resample.cpp: -------------------------------------------------------------------------------- 1 | #include "stratified_resample.h" 2 | #include "stratified_random.h" 3 | #include 4 | 5 | using namespace std; 6 | 7 | void stratified_resample(VectorXf w, vector &keep, float &Neff) 8 | { 9 | VectorXf wsqrd(w.size()); 10 | float wsum = w.sum(); 11 | 12 | for (int i=0; i select; 26 | stratified_random(len,select); 27 | /* 28 | select.push_back(0.0948); 29 | select.push_back(0.1334); 30 | select.push_back(0.239); 31 | select.push_back(0.315); 32 | select.push_back(0.4334); 33 | select.push_back(0.5554); 34 | select.push_back(0.655); 35 | select.push_back(0.716); 36 | select.push_back(0.8117); 37 | select.push_back(0.9399); 38 | */ 39 | cumsum(w); 40 | 41 | int ctr=0; 42 | for (int i=0; i 5 | #include 6 | 7 | using namespace Eigen; 8 | using namespace std; 9 | 10 | void stratified_resample(VectorXf w, vector &keep, float &Neff); 11 | void cumsum(VectorXf &w); 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /cpp/core/utilities.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | //////////////////////////////////////////////////////////////////////////////// 5 | // MyTimer 6 | //////////////////////////////////////////////////////////////////////////////// 7 | 8 | // little helper class to time stuff 9 | // this should be in a utilities header file 10 | class MyTimer 11 | { 12 | public: 13 | MyTimer() 14 | { 15 | } 16 | 17 | void Start() 18 | { 19 | gettimeofday(&tv1, NULL); 20 | } 21 | double Stop() 22 | { 23 | gettimeofday(&tv2, NULL); 24 | int sec = tv2.tv_sec - tv1.tv_sec; 25 | int usec = tv2.tv_usec - tv1.tv_usec; 26 | 27 | if (usec < 0) 28 | { 29 | sec--; 30 | usec = 1000000 + usec; 31 | } 32 | 33 | return (double)sec + (double)usec / 1000000.0; 34 | } 35 | void Print(const char *label) 36 | { 37 | int sec = tv2.tv_sec - tv1.tv_sec; 38 | int usec = tv2.tv_usec - tv1.tv_usec; 39 | 40 | if (usec < 0) 41 | { 42 | sec--; 43 | usec = 1000000 + usec; 44 | } 45 | printf("%s took %d sec %d usec\n", label, sec, usec); 46 | } 47 | 48 | private: 49 | struct timeval tv1, tv2; 50 | }; 51 | 52 | -------------------------------------------------------------------------------- /cpp/fastslam1/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | include_directories(${EIGEN3_INCLUDE_DIR}) 3 | 4 | include_directories(${CMAKE_CURRENT_BINARY_DIR}/..) 5 | 6 | # Produce a FastSLAM shared library. (For re-use of executables) 7 | add_library(FastSLAM1 8 | compute_weight.cpp fastslam1_sim.cpp predict.cpp 9 | ) 10 | 11 | target_link_libraries(FastSLAM1 FastSLAM_core) # We don't depend on anything 12 | 13 | install(FILES 14 | compute_weight.h fastslam1_sim.h predict.h 15 | DESTINATION include/fslam/fastslam1 16 | ) 17 | 18 | install(TARGETS 19 | FastSLAM1 20 | DESTINATION lib 21 | ) 22 | 23 | # Add an executable that currentlyi runs the simulator 24 | set(FSLAM_USED_LIBS FastSLAM1) 25 | add_fslam_tool(fastslam1_simulation main.cpp) 26 | 27 | -------------------------------------------------------------------------------- /cpp/fastslam1/compute_weight.cpp: -------------------------------------------------------------------------------- 1 | #include "compute_weight.h" 2 | #include 3 | #include "core/pi_to_pi.h" 4 | #include "core/compute_jacobians.h" 5 | #include 6 | #include 7 | 8 | #define pi 3.1416 9 | 10 | using namespace std; 11 | // 12 | //compute particle weight for sampling 13 | // 14 | float compute_weight(Particle &particle, vector z, vector idf, 15 | MatrixXf R) 16 | { 17 | vector Hv; 18 | vector Hf; 19 | vector Sf; 20 | vector zp; 21 | 22 | //process each feature, incrementally refine proposal distribution 23 | compute_jacobians(particle,idf,R,zp,&Hv,&Hf,&Sf); 24 | 25 | vector v; 26 | for (int j =0; j 5 | #include 6 | #include "core/particle.h" 7 | 8 | using namespace Eigen; 9 | using namespace std; 10 | 11 | float compute_weight(Particle &particle, vector z, vector idf, MatrixXf R); 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /cpp/fastslam1/fastslam1_sim.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "fastslam1_sim.h" 6 | #include "core/add_control_noise.h" 7 | #include "core/get_observations.h" 8 | #include "core/add_observation_noise.h" 9 | #include "core/TransformToGlobal.h" 10 | #include "core/line_plot_conversion.h" 11 | #include "core/data_associate_known.h" 12 | #include "core/feature_update.h" 13 | #include "core/resample_particles.h" 14 | #include "core/add_feature.h" 15 | #include "compute_weight.h" 16 | #include "predict.h" 17 | 18 | using namespace config; 19 | using namespace std; 20 | 21 | int counter=0; 22 | vector fastslam1_sim(MatrixXf lm, MatrixXf wp) 23 | { 24 | if (SWITCH_PREDICT_NOISE) { 25 | printf("Sampling from predict noise usually OFF for FastSLAM 2.0\n"); 26 | } 27 | 28 | //normally initialized configfile.h 29 | Q << pow(sigmaV,2), 0, 30 | 0 , pow(sigmaG,2); 31 | 32 | R << sigmaR*sigmaR, 0, 33 | 0, sigmaB*sigmaB; 34 | 35 | float veh[2][3] = {{0,-WHEELBASE,-WHEELBASE},{0,-1,1}}; 36 | 37 | //vector of particles (their count will change) 38 | vector particles(NPARTICLES); 39 | for (int i=0; i ftag; //identifier for each landmark 56 | for (int i=0; i< lm.cols(); i++) { 57 | ftag.push_back(i); 58 | } 59 | 60 | //data ssociation table 61 | VectorXf da_table(lm.cols()); 62 | for (int s=0; s ftag_visible; 83 | vector z; //range and bearings of visible landmarks 84 | 85 | //Main loop 86 | while (iwp !=-1) { 87 | compute_steering(xtrue, wp, iwp, AT_WAYPOINT, G, RATEG, MAXG, dt); 88 | if (iwp ==-1 && NUMBER_LOOPS > 1) { 89 | iwp = 0; 90 | NUMBER_LOOPS = NUMBER_LOOPS-1; 91 | } 92 | predict_true(xtrue,V,G,WHEELBASE,dt); 93 | 94 | //add process noise 95 | float* VnGn = new float[2]; 96 | add_control_noise(V,G,Q,SWITCH_CONTROL_NOISE,VnGn); 97 | float Vn = VnGn[0]; 98 | float Gn = VnGn[1]; 99 | 100 | //Predict step 101 | for (unsigned int i=0; i< NPARTICLES; i++) { 102 | predict(particles[i],Vn,Gn,Qe,WHEELBASE,dt,SWITCH_PREDICT_NOISE); 103 | if (SWITCH_HEADING_KNOWN) { 104 | for (int j=0; j< particles[i].xf().size(); j++) { 105 | VectorXf xf_j = particles[i].xf()[j]; 106 | xf_j[2] = xtrue[2]; 107 | particles[i].setXfi(j,xf_j); 108 | } 109 | } 110 | } 111 | 112 | //Observe step 113 | dtsum = dtsum+dt; 114 | if (dtsum >= DT_OBSERVE) { 115 | dtsum=0; 116 | 117 | //Compute true data, then add noise 118 | ftag_visible = vector(ftag); //modify the copy, not the ftag 119 | 120 | //z is the range and bearing of the observed landmark 121 | z = get_observations(xtrue,lm,ftag_visible,MAX_RANGE); 122 | add_observation_noise(z,R,SWITCH_SENSOR_NOISE); 123 | 124 | if (!z.empty()){ 125 | plines = make_laser_lines(z,xtrue); 126 | } 127 | 128 | //Compute (known) data associations 129 | int Nf = particles[0].xf().size(); 130 | vector idf; 131 | vector zf; 132 | vector zn; 133 | 134 | bool testflag= false; 135 | data_associate_known(z,ftag_visible,da_table,Nf,zf,idf,zn); 136 | 137 | //perform update 138 | for (int i =0; i rb, VectorXf xv) 164 | { 165 | if (rb.empty()) { 166 | return MatrixXf(0,0); 167 | } 168 | 169 | int len = rb.size(); 170 | MatrixXf lnes(4,len); 171 | 172 | MatrixXf globalMat(2,rb.size()); 173 | int j; 174 | for (j=0; j 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "core/configfile.h" 12 | #include "core/compute_steering.h" 13 | #include "core/predict_true.h" 14 | #include "core/particle.h" 15 | 16 | using namespace std; 17 | using namespace Eigen; 18 | 19 | vector fastslam1_sim(MatrixXf lm, MatrixXf wp); 20 | MatrixXf make_laser_lines(vector rb, VectorXf xv); 21 | 22 | #endif //FASTSLAM2_SIM_H 23 | -------------------------------------------------------------------------------- /cpp/fastslam1/main.cpp: -------------------------------------------------------------------------------- 1 | #include "fastslam1_sim.h" 2 | #include "core/read_input_file.h" 3 | #include "core/particle.h" 4 | #include "core/utilities.h" 5 | 6 | using namespace Eigen; 7 | using namespace std; 8 | 9 | int main (int argc, char *argv[]) 10 | { 11 | MyTimer Timer = MyTimer(); 12 | Timer.Start(); 13 | MatrixXf lm; //landmark positions 14 | MatrixXf wp; //way points 15 | 16 | read_input_file("example_webmap.mat", &lm, &wp); 17 | vector data = fastslam1_sim(lm,wp); 18 | for (int i=0; i 3 | #include 4 | 5 | #define pi 3.1416 6 | 7 | using namespace std; 8 | 9 | void predict(Particle &particle,float V,float G,Matrix2f Q, float WB,float dt, int addrandom) 10 | { 11 | //optional: add random noise to predicted state 12 | if (addrandom ==1) { 13 | VectorXf A(2); 14 | A(0) = V; 15 | A(1) = G; 16 | VectorXf VG(2); 17 | VG = multivariate_gauss(A,Q,1); 18 | V = VG(0); 19 | G = VG(1); 20 | } 21 | 22 | //predict state 23 | VectorXf xv = particle.xv(); 24 | VectorXf xv_temp(3); 25 | xv_temp << xv(0) + V*dt*cos(G+xv(2)), 26 | xv(1) + V*dt*sin(G+xv(2)), 27 | pi_to_pi2(xv(2) + V*dt*sin(G/WB)); 28 | particle.setXv(xv_temp); 29 | } 30 | 31 | float pi_to_pi2(float ang) 32 | { 33 | if (ang > pi) { 34 | ang = ang - (2*pi); 35 | } 36 | if (ang < -pi) { 37 | ang = ang + (2*pi); 38 | } 39 | return ang; 40 | } 41 | 42 | -------------------------------------------------------------------------------- /cpp/fastslam1/predict.h: -------------------------------------------------------------------------------- 1 | #ifndef PREDICT_H 2 | #define PREDICT_H 3 | 4 | #include 5 | #include "core/particle.h" 6 | #include "core/multivariate_gauss.h" 7 | 8 | using namespace Eigen; 9 | 10 | void predict(Particle &particle,float V,float G,Matrix2f Q, float WB,float dt, int addrandom); 11 | float pi_to_pi2(float ang); 12 | 13 | #endif //PREDICT_H 14 | -------------------------------------------------------------------------------- /cpp/fastslam2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(${EIGEN3_INCLUDE_DIR}) 2 | 3 | include_directories(${CMAKE_CURRENT_BINARY_DIR}/..) 4 | 5 | # Produce a FastSLAM shared library. (For re-use of executables) 6 | add_library(FastSLAM2 7 | fastslam2_sim.cpp gauss_evaluate.cpp observe_heading.cpp predict.cpp sample_proposal.cpp 8 | ) 9 | target_link_libraries(FastSLAM2 FastSLAM_core) # We don't depend on anything 10 | 11 | install(FILES 12 | fastslam2_sim.h gauss_evaluate.h observe_heading.h predict.h sample_proposal.h 13 | DESTINATION include/fslam/fastslam2 14 | ) 15 | 16 | install(TARGETS 17 | FastSLAM2 18 | DESTINATION lib 19 | ) 20 | 21 | # Add an executable that currentlyi runs the simulator 22 | set(FSLAM_USED_LIBS FastSLAM2) 23 | add_fslam_tool(fastslam2_simulation main.cpp) 24 | 25 | -------------------------------------------------------------------------------- /cpp/fastslam2/CMakeLists.txt.real: -------------------------------------------------------------------------------- 1 | include_directories(${EIGEN3_INCLUDE_DIR}) 2 | 3 | include_directories(${CMAKE_CURRENT_BINARY_DIR}/..) 4 | 5 | # Produce a FastSLAM shared library. (For re-use of executables) 6 | add_library(FastSLAM2 7 | fastslam2_sim.cpp gauss_evaluate.cpp observe_heading.cpp predict.cpp sample_proposal.cpp 8 | ) 9 | target_link_libraries(FastSLAM2 FastSLAM_core) # We don't depend on anything 10 | 11 | install(FILES 12 | fastslam2_sim.h gauss_evaluate.h observe_heading.h predict.h sample_proposal.h 13 | DESTINATION include/fslam/fastslam2 14 | ) 15 | 16 | install(TARGETS 17 | FastSLAM2 18 | DESTINATION lib 19 | ) 20 | 21 | # Add an executable that currentlyi runs the simulator 22 | set(FSLAM_USED_LIBS FastSLAM2) 23 | add_fslam_tool(fastslam2_simulation main.cpp) 24 | 25 | -------------------------------------------------------------------------------- /cpp/fastslam2/CMakeLists_empty.txt: -------------------------------------------------------------------------------- 1 | #include_directories(${EIGEN3_INCLUDE_DIR}) 2 | 3 | #include_directories(${CMAKE_CURRENT_BINARY_DIR}/..) 4 | 5 | ## Produce a FastSLAM shared library. (For re-use of executables) 6 | #add_library(FastSLAM2 7 | # fastslam2_sim.cpp gauss_evaluate.cpp main.cpp observe_heading.cpp predict.cpp sample_proposal.cpp 8 | #) 9 | 10 | #target_link_libraries(FastSLAM2 FastSLAM_core) # We don't depend on anything 11 | 12 | #install(FILES 13 | # fastslam2_sim.h gauss_evaluate.h observe_heading.h predict.h sample_proposal.h 14 | # DESTINATION include/fslam/fastslam2 15 | # ) 16 | 17 | #install(TARGETS 18 | # FastSLAM2 19 | # DESTINATION lib 20 | # ) 21 | 22 | ## Add an executable that currentlyi runs the simulator 23 | #set(FSLAM_USED_LIBS FastSLAM2) 24 | #add_fslam_tool(fastslam2_simulation main.cpp) 25 | 26 | -------------------------------------------------------------------------------- /cpp/fastslam2/CMakeLists_real.txt: -------------------------------------------------------------------------------- 1 | include_directories(${EIGEN3_INCLUDE_DIR}) 2 | 3 | include_directories(${CMAKE_CURRENT_BINARY_DIR}/..) 4 | 5 | # Produce a FastSLAM shared library. (For re-use of executables) 6 | add_library(FastSLAM2 7 | fastslam2_sim.cpp gauss_evaluate.cpp observe_heading.cpp predict.cpp sample_proposal.cpp 8 | ) 9 | target_link_libraries(FastSLAM2 FastSLAM_core) # We don't depend on anything 10 | 11 | install(FILES 12 | fastslam2_sim.h gauss_evaluate.h observe_heading.h predict.h sample_proposal.h 13 | DESTINATION include/fslam/fastslam2 14 | ) 15 | 16 | install(TARGETS 17 | FastSLAM2 18 | DESTINATION lib 19 | ) 20 | 21 | # Add an executable that currentlyi runs the simulator 22 | set(FSLAM_USED_LIBS FastSLAM2) 23 | add_fslam_tool(fastslam2_simulation main.cpp) 24 | 25 | -------------------------------------------------------------------------------- /cpp/fastslam2/fastslam2_sim.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "fastslam2_sim.h" 7 | #include "core/add_control_noise.h" 8 | #include "predict.h" 9 | #include "observe_heading.h" 10 | #include "core/get_observations.h" 11 | #include "core/add_observation_noise.h" 12 | #include "core/TransformToGlobal.h" 13 | #include "core/line_plot_conversion.h" 14 | #include "core/data_associate_known.h" 15 | #include "sample_proposal.h" 16 | #include "core/feature_update.h" 17 | #include "core/resample_particles.h" 18 | #include "core/add_feature.h" 19 | 20 | using namespace config; 21 | using namespace std; 22 | 23 | vector fastslam2_sim(MatrixXf lm, MatrixXf wp) 24 | { 25 | if (SWITCH_PREDICT_NOISE) { 26 | printf("Sampling from predict noise usually OFF for FastSLAM 2.0\n"); 27 | } 28 | if (SWITCH_SAMPLE_PROPOSAL == 0) { 29 | printf("Sampling from optimal proposal is usually ON for FastSLAM 2.0\n"); 30 | } 31 | //normally initialized configfile.h 32 | Q << pow(sigmaV,2), 0, 33 | 0, pow(sigmaG,2); 34 | 35 | R << sigmaR*sigmaR, 0, 36 | 0, sigmaB*sigmaB; 37 | 38 | float veh[2][3] = {{0,-WHEELBASE,-WHEELBASE},{0,-1,1}}; 39 | 40 | //vector of particles (their count will change) 41 | vector particles(NPARTICLES); 42 | for (int i=0; i ftag; //identifier for each landmark 59 | for (int i=0; i< lm.cols(); i++) { 60 | ftag.push_back(i); 61 | } 62 | 63 | //data association table 64 | VectorXf da_table(lm.cols()); 65 | for (int s=0; s ftag_visible; 85 | vector z; 86 | 87 | //Main loop 88 | while (iwp !=-1) { 89 | compute_steering(xtrue, wp, iwp, AT_WAYPOINT, G, RATEG, MAXG, dt); 90 | if (iwp ==-1 && NUMBER_LOOPS > 1) { 91 | iwp = 0; 92 | NUMBER_LOOPS = NUMBER_LOOPS-1; 93 | } 94 | predict_true(xtrue,V,G,WHEELBASE,dt); 95 | 96 | //add process noise 97 | float* VnGn = new float[2]; 98 | add_control_noise(V,G,Q,SWITCH_CONTROL_NOISE,VnGn); 99 | float Vn = VnGn[0]; 100 | float Gn = VnGn[1]; 101 | 102 | //Predict step 103 | for (unsigned int i=0; i< NPARTICLES; i++) { 104 | predict(particles[i],Vn,Gn,Qe,WHEELBASE,dt,SWITCH_PREDICT_NOISE); 105 | assert(isfinite(particles[i].w())); 106 | observe_heading(particles[i], xtrue(2), SWITCH_HEADING_KNOWN); //if heading known, observe heading 107 | assert(isfinite(particles[i].w())); 108 | } 109 | 110 | //Observe step 111 | dtsum = dtsum+dt; 112 | if (dtsum >= DT_OBSERVE) { 113 | dtsum=0; 114 | 115 | //Compute true data, then add noise 116 | ftag_visible = vector(ftag); //modify the copy, not the ftag 117 | 118 | //z is the range and bearing of the observed landmark 119 | z = get_observations(xtrue,lm,ftag_visible,MAX_RANGE); 120 | add_observation_noise(z,R,SWITCH_SENSOR_NOISE); 121 | 122 | //Compute (known) data associations 123 | int Nf = particles[0].xf().size(); 124 | vector idf; 125 | vector zf; 126 | vector zn; 127 | 128 | bool testflag= false; 129 | data_associate_known(z,ftag_visible,da_table,Nf,zf,idf,zn); 130 | 131 | //observe map features 132 | if (!zf.empty()) { 133 | //isample from 'optimal' proposal distribution, then update map 134 | for (unsigned i=0; i 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "core/configfile.h" 12 | #include "core/compute_steering.h" 13 | #include "core/predict_true.h" 14 | #include "core/particle.h" 15 | 16 | using namespace std; 17 | using namespace Eigen; 18 | 19 | vector fastslam2_sim(MatrixXf lm, MatrixXf wp); 20 | //MatrixXf make_laser_lines(vector rb, VectorXf xv); 21 | 22 | #endif //FASTSLAM2_SIM_H 23 | -------------------------------------------------------------------------------- /cpp/fastslam2/gauss_evaluate.cpp: -------------------------------------------------------------------------------- 1 | #include "gauss_evaluate.h" 2 | #include 3 | #include 4 | 5 | #define pi 3.14159 6 | 7 | using namespace std; 8 | 9 | //approximation of the desired sampling distribution. 10 | float gauss_evaluate(VectorXf v, MatrixXf S, int logflag) 11 | { 12 | int D = v.size(); 13 | MatrixXf Sc = S.llt().matrixL(); 14 | 15 | //normalised innovation 16 | VectorXf ni = Sc.jacobiSvd(ComputeThinU | ComputeThinV).solve(v); 17 | 18 | float E=0.f; 19 | VectorXf nin(ni.size()); 20 | for (int s=0; s< ni.size(); s++) { 21 | nin[s] = ni[s]*ni[s]; 22 | } 23 | E = -0.5f * nin.sum(); 24 | 25 | int i,j; 26 | float C,w; 27 | unsigned m = min(Sc.rows(), Sc.cols()); 28 | 29 | if (logflag !=1) { 30 | float prod = 1; 31 | for (i=0; i 5 | 6 | using namespace Eigen; 7 | 8 | float gauss_evaluate(VectorXf v, MatrixXf S, int logflag); 9 | 10 | /* 11 | **INPUTS: 12 | ** v - a set of innovation vectors 13 | ** S - the covariance matrix for the innovations 14 | ** logflat - - if 1 computes the log-likelihood, otherwise computes 15 | ** the likelihood. 16 | ** 17 | **OUTPUT: 18 | ** w- set of Gaussian likelihoods or log-likelihoods for ech v(:i). 19 | ** 20 | **This implementaiton uses the Cholesky factor of S to compute the likelihoods 21 | **and so is more numerically stable than a simple full covariance form. 22 | **This function is identical to gauss_likelihood(). 23 | */ 24 | 25 | #endif 26 | 27 | -------------------------------------------------------------------------------- /cpp/fastslam2/main.cpp: -------------------------------------------------------------------------------- 1 | #include "fastslam2_sim.h" 2 | #include "core/read_input_file.h" 3 | #include "core/particle.h" 4 | #include "core/utilities.h" 5 | 6 | using namespace Eigen; 7 | using namespace std; 8 | 9 | int main (int argc, char *argv[]) 10 | { 11 | MyTimer Timer = MyTimer(); 12 | Timer.Start(); 13 | 14 | MatrixXf lm; //landmark positions 15 | MatrixXf wp; //way points 16 | 17 | read_input_file("example_webmap.mat", &lm, &wp); 18 | vector data = fastslam2_sim(lm,wp); 19 | for (int i=0; i 6 | #include 7 | 8 | //observe_heading doesn't get called unless I change SWITCH_HEADING KNOWN 9 | 10 | using namespace std; 11 | 12 | void observe_heading(Particle &particle, float phi, int useheading) 13 | { 14 | if (useheading ==0){ 15 | return; 16 | } 17 | float sigmaPhi = 0.01*pi/180.0; 18 | //cout<<"sigmaPhi "< 5 | #include "core/particle.h" 6 | 7 | void observe_heading(Particle &particle, float phi, int useheading); 8 | 9 | #endif 10 | -------------------------------------------------------------------------------- /cpp/fastslam2/predict.cpp: -------------------------------------------------------------------------------- 1 | #include "predict.h" 2 | #include 3 | #include 4 | 5 | 6 | #define pi 3.1416 7 | //checked all values. everything works 8 | 9 | using namespace std; 10 | 11 | void predict(Particle &particle,float V,float G,Matrix2f Q, float WB,float dt, int addrandom) 12 | { 13 | VectorXf xv = particle.xv(); 14 | MatrixXf Pv = particle.Pv(); 15 | #if 0 16 | cout<<"particle.xv() in predict"< pi) { 67 | ang = ang - (2*pi); 68 | } 69 | if (ang < -pi) { 70 | ang = ang + (2*pi); 71 | } 72 | return ang; 73 | } 74 | 75 | -------------------------------------------------------------------------------- /cpp/fastslam2/predict.h: -------------------------------------------------------------------------------- 1 | #ifndef PREDICT_H 2 | #define PREDICT_H 3 | 4 | #include 5 | #include "core/particle.h" 6 | #include "core/multivariate_gauss.h" 7 | 8 | using namespace Eigen; 9 | 10 | void predict(Particle &particle,float V,float G,Matrix2f Q, float WB,float dt, int addrandom); 11 | 12 | float pi_to_pi2(float ang); 13 | 14 | #endif //PREDICT_H 15 | -------------------------------------------------------------------------------- /cpp/fastslam2/sample_proposal.cpp: -------------------------------------------------------------------------------- 1 | #include "sample_proposal.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "assert.h" 7 | 8 | //compute proposal distribution, then sample from it, and compute new particle weight 9 | void sample_proposal(Particle &particle, vector z, vector idf, MatrixXf R) 10 | { 11 | assert(isfinite(particle.w())); 12 | VectorXf xv = VectorXf(particle.xv()); //robot position 13 | MatrixXf Pv = MatrixXf(particle.Pv()); //controls (motion command) 14 | 15 | VectorXf xv0 = VectorXf(xv); 16 | MatrixXf Pv0 = MatrixXf(Pv); 17 | 18 | vector Hv; 19 | vector Hf; 20 | vector Sf; 21 | vector zp; 22 | 23 | VectorXf zpi; 24 | MatrixXf Hvi; 25 | MatrixXf Hfi; 26 | MatrixXf Sfi; 27 | 28 | //process each feature, incrementally refine proposal distribution 29 | unsigned i,r; 30 | vector j; 31 | for (i =0; i z, vectoridf, MatrixXf R) 139 | { 140 | float w = 1; 141 | 142 | vector Hv; 143 | vector Hf; 144 | vector Sf; 145 | vector zp; 146 | 147 | unsigned j,k; 148 | vector idfi; 149 | 150 | for (j=0; j 5 | #include 6 | 7 | #include "core/particle.h" 8 | #include "core/compute_jacobians.h" 9 | #include "core/multivariate_gauss.h" 10 | #include "gauss_evaluate.h" 11 | #include "core/pi_to_pi.h" 12 | 13 | using namespace Eigen; 14 | using namespace std; 15 | 16 | void sample_proposal(Particle &particle, vector z, vector idf, MatrixXf R); 17 | //float likelihood_given_xv(Particle particle, MatrixXf z, vectoridf, MatrixXf R); 18 | float likelihood_given_xv(Particle particle, vector z, vectoridf, MatrixXf R); 19 | VectorXf delta_xv(VectorXf xv1, VectorXf xv2); 20 | 21 | #endif 22 | -------------------------------------------------------------------------------- /cpp/gtest/CMakeCache.txt: -------------------------------------------------------------------------------- 1 | # This is the CMakeCache file. 2 | # For build in directory: /Users/ylee8/FastSLAM/cpp/gtest 3 | # It was generated by CMake: /Users/ylee8/packages/local/bin/cmake 4 | # You can edit this file to change values found and used by cmake. 5 | # If you do not want to change any of the values, simply exit the editor. 6 | # If you do want to change a value, simply edit, save, and exit the editor. 7 | # The syntax for the file is as follows: 8 | # KEY:TYPE=VALUE 9 | # KEY is the name of a variable in the cache. 10 | # TYPE is a hint to GUI's for the type of VALUE, DO NOT EDIT TYPE!. 11 | # VALUE is the current value for the KEY. 12 | 13 | ######################## 14 | # EXTERNAL cache entries 15 | ######################## 16 | 17 | 18 | ######################## 19 | # INTERNAL cache entries 20 | ######################## 21 | 22 | //This is the directory where this CMakeCache.txt was created 23 | CMAKE_CACHEFILE_DIR:INTERNAL=/Users/ylee8/FastSLAM/cpp/gtest 24 | //Major version of cmake used to create the current loaded cache 25 | CMAKE_CACHE_MAJOR_VERSION:INTERNAL=2 26 | //Minor version of cmake used to create the current loaded cache 27 | CMAKE_CACHE_MINOR_VERSION:INTERNAL=8 28 | //Patch version of cmake used to create the current loaded cache 29 | CMAKE_CACHE_PATCH_VERSION:INTERNAL=9 30 | //Path to CMake executable. 31 | CMAKE_COMMAND:INTERNAL=/Users/ylee8/packages/local/bin/cmake 32 | //Path to cpack program executable. 33 | CMAKE_CPACK_COMMAND:INTERNAL=/Users/ylee8/packages/local/bin/cpack 34 | //Path to ctest program executable. 35 | CMAKE_CTEST_COMMAND:INTERNAL=/Users/ylee8/packages/local/bin/ctest 36 | //Path to cache edit program executable. 37 | CMAKE_EDIT_COMMAND:INTERNAL=/Users/ylee8/packages/local/bin/ccmake 38 | //Path to CMake installation. 39 | CMAKE_ROOT:INTERNAL=/Users/ylee8/packages/local/share/cmake-2.8 40 | 41 | -------------------------------------------------------------------------------- /cpp/gtest/CMakeFiles/cmake.check_cache: -------------------------------------------------------------------------------- 1 | # This file is generated by cmake for dependency checking of the CMakeCache.txt file 2 | -------------------------------------------------------------------------------- /cpp/gtest/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(${EIGEN3_INCLUDE_DIR}) 2 | include_directories(${CMAKE_CURRENT_BINARY_DIR}/..) 3 | 4 | # Define here any libraries that you'll use later on in this 5 | # file. Things like OpenCL or Boost libraries would go here. (Or your 6 | # own FSLAM libs). 7 | set(FSLAM_USED_LIBS FastSLAM1 FastSLAM2 FastSLAM_core) 8 | 9 | # Tests 10 | fslam_add_test(TestFS1AgainstMatlab.cpp) 11 | 12 | # Support Files 13 | file(COPY ${CMAKE_SOURCE_DIR}/cpp/gtest/example_webmap.mat DESTINATION ${CMAKE_BINARY_DIR}/cpp/gtest/) 14 | -------------------------------------------------------------------------------- /cpp/gtest/example_webmap.mat: -------------------------------------------------------------------------------- 1 | #type columns rows 2 | lm 2 35 3 | 2.9922 -25.7009 4 | 32.8988 -33.1776 5 | 24.7991 -68.3801 6 | 75.2664 -65.5763 7 | 73.7087 -35.6698 8 | 98.6308 3.8941 9 | 49.4097 26.9470 10 | 80.2508 59.9688 11 | 54.7056 88.6293 12 | 13.2726 80.8411 13 | -16.3224 49.0654 14 | -65.8551 83.6449 15 | -96.3847 60.5919 16 | -76.1355 36.2928 17 | -87.3505 -21.3396 18 | -103.5498 -32.2430 19 | -92.9579 -77.7259 20 | -55.8863 -55.9190 21 | -35.3255 -19.1589 22 | -56.1978 16.3551 23 | -7.2882 19.7819 24 | 25.7336 3.2710 25 | -19.5610 80.1444 26 | -41.7506 46.2325 27 | 25.4021 26.8543 28 | 91.3870 28.2385 29 | -13.7216 -79.0338 30 | -52.8454 -92.1833 31 | -86.1298 15.0890 32 | -127.0053 22.7018 33 | -25.9843 4.7078 34 | 56.3508 -17.4388 35 | 51.6793 -83.1863 36 | 21.3146 -96.3358 37 | 48.1757 57.9979 38 | 39 | wp 2 17 40 | 12.6495 -41.5888 41 | 44.7368 -54.9844 42 | 85.5467 -45.0156 43 | 93.6464 -17.2897 44 | 64.9860 5.7632 45 | 71.8396 31.6199 46 | 71.2165 70.5607 47 | 33.5218 76.4798 48 | 12.9611 51.5576 49 | -32.5218 67.4455 50 | -74.8894 69.0031 51 | -97.3193 41.9003 52 | -107.5997 6.3863 53 | -86.4159 -25.7009 54 | -83.9237 -64.0187 55 | -39.3754 -81.4642 56 | -17.5685 -51.5576 57 | -------------------------------------------------------------------------------- /cpp/gtest/test_main.cc: -------------------------------------------------------------------------------- 1 | // PLEASE DO NOT MODIFY ME! I'M REQUIRED TO BUILD ALL YOUR OTHER UNIT 2 | // TESTS! UNIT TESTS DO NOT GO INSIDE ME! 3 | 4 | #include 5 | 6 | int main( int argc, char **argv ) { 7 | testing::InitGoogleTest(&argc, argv); 8 | return RUN_ALL_TESTS(); 9 | } 10 | -------------------------------------------------------------------------------- /cpp/log: -------------------------------------------------------------------------------- 1 | 8/23/12 2 | Went through and did unity tests on all files and they seem to generate correct result for one given set of inputs. There is a matrix dimensionality error and at this point, I need to read the papers and identify each variables and rename them and think about what they do... 3 | 4 | 5 | 8/21/12 6 | even a little bit of numerical error results in exp(E) to go zero (when E<0) in gauss evaluate. 7 | The problem is the input to gauss evaluate, v. It's set in sample_proposal. line 172. v is wrong... 8 | 9 | Do a check: 10 | is likelhood_given_xv returning a 0 with the right inputs? Then, the function has a problem. If the inputs are wrong, then sample_particles might be wrong. Check this (add a break in likelihood_given_xv. 11 | 12 | 13 | w is zero in resample particles. w is set in sample_proposal. The likelihood given xv returns a 0 everytime, causing w to be 0. This results in nan's, which should throw a division by 0 error 14 | 15 | 7/25/12 16 | fix the multivariate gauss's use of Random() (from eigen). It doesn't return diff. random numbers at each turn 17 | -------------------------------------------------------------------------------- /matlab/KF_cholesky_update.m: -------------------------------------------------------------------------------- 1 | function [x,P]= KF_cholesky_update(x,P,v,R,H) 2 | %function [x,P]= KF_cholesky_update(x,P,v,R,H) 3 | % 4 | % Calculate the KF (or EKF) update given the prior state [x,P] 5 | % the innovation [v,R] and the (linearised) observation model H. 6 | % The result is calculated using Cholesky factorisation, which 7 | % is more numerically stable than a naive implementation. 8 | % 9 | % Tim Bailey 2003 10 | % Developed by Jose Guivant 11 | 12 | PHt= P*H'; 13 | S= H*PHt + R; 14 | 15 | S= (S+S')*0.5; % make symmetric 16 | SChol= chol(S); 17 | 18 | SCholInv= inv(SChol); % triangular matrix 19 | W1= PHt * SCholInv; 20 | W= W1 * SCholInv'; 21 | 22 | x= x + W*v; % update 23 | P= P - W1*W1'; 24 | -------------------------------------------------------------------------------- /matlab/KF_joseph_update.m: -------------------------------------------------------------------------------- 1 | function [x,P]= KF_joseph_update(x,P,v,R,H) 2 | %function [x,P]= KF_joseph_update(x,P,v,R,H) 3 | % 4 | % This module is identical to KF_simple_update() except that 5 | % it uses the Joseph-form covariance update, as shown in 6 | % Bar-Shalom "Estimation with Applications...", 2001, p302. 7 | % 8 | % Tim Bailey 2003 9 | 10 | PHt= P*H'; 11 | S= H*PHt + R; 12 | Si= inv(S); 13 | Si= make_symmetric(Si); 14 | PSD_check= chol(Si); 15 | W= PHt*Si; 16 | 17 | x= x + W*v; 18 | 19 | % Joseph-form covariance update 20 | C= eye(size(P)) - W*H; 21 | P= C*P*C' + W*R*W'; 22 | 23 | P= P + eye(size(P))*eps; % a little numerical safety 24 | PSD_check= chol(P); 25 | 26 | function P= make_symmetric(P) 27 | P= (P+P')*0.5; 28 | -------------------------------------------------------------------------------- /matlab/TransformToGlobal.m: -------------------------------------------------------------------------------- 1 | function p = TransformToGlobal(p, b) 2 | % function p = TransformToGlobal(p, b) 3 | % 4 | % Transform a list of poses [x;y;phi] so that they are global wrt a base pose 5 | % 6 | % Tim Bailey 1999 7 | 8 | % rotate 9 | rot = [cos(b(3)) -sin(b(3)); sin(b(3)) cos(b(3))]; 10 | p(1:2,:) = rot*p(1:2,:); 11 | 12 | % translate 13 | p(1,:) = p(1,:) + b(1); 14 | p(2,:) = p(2,:) + b(2); 15 | 16 | % if p is a pose and not a point 17 | if size(p,1)==3 18 | p(3,:) = pi_to_pi(p(3,:) + b(3)); 19 | end 20 | -------------------------------------------------------------------------------- /matlab/add_control_noise.m: -------------------------------------------------------------------------------- 1 | function [V,G]= add_control_noise(V,G,Q, addnoise) 2 | % Add random noise to nominal control values 3 | 4 | if addnoise == 1 5 | % V= V + randn(1)*sqrt(Q(1,1)); % if assume Q is diagonal 6 | % G= G + randn(1)*sqrt(Q(2,2)); 7 | 8 | C= multivariate_gauss([V;G],Q, 1); % if Q might be correlated 9 | V= C(1); G= C(2); 10 | end 11 | -------------------------------------------------------------------------------- /matlab/add_feature.m: -------------------------------------------------------------------------------- 1 | function particle= add_feature(particle, z,R) 2 | % add new features 3 | 4 | lenz= size(z,2); 5 | xf= zeros(2,lenz); 6 | Pf= zeros(2,2,lenz); 7 | xv= particle.xv; 8 | 9 | for i=1:lenz 10 | r= z(1,i); b= z(2,i); 11 | s= sin(xv(3)+b); 12 | c= cos(xv(3)+b); 13 | 14 | xf(:,i)= [xv(1) + r*c; 15 | xv(2) + r*s]; 16 | 17 | Gz= [c -r*s; 18 | s r*c]; 19 | Pf(:,:,i)= Gz*R*Gz'; 20 | end 21 | 22 | lenx= size(particle.xf,2); 23 | ii= (lenx+1):(lenx+lenz); 24 | particle.xf(:,ii)= xf; 25 | particle.Pf(:,:,ii)= Pf; 26 | -------------------------------------------------------------------------------- /matlab/add_observation_noise.m: -------------------------------------------------------------------------------- 1 | function z= add_observation_noise(z,R, addnoise) 2 | % Add random measurement noise. We assume R is diagonal. 3 | 4 | if addnoise == 1 5 | len= size(z,2); 6 | if len > 0 7 | z(1,:)= z(1,:) + randn(1,len)*sqrt(R(1,1)); 8 | z(2,:)= z(2,:) + randn(1,len)*sqrt(R(2,2)); 9 | end 10 | end 11 | -------------------------------------------------------------------------------- /matlab/compute_jacobians.m: -------------------------------------------------------------------------------- 1 | function [zp,Hv,Hf,Sf]= compute_jacobians(particle, idf, R) 2 | 3 | xv= particle.xv; 4 | xf= particle.xf(:,idf); 5 | Pf= particle.Pf(:,:,idf); 6 | 7 | for i=1:length(idf) 8 | dx= xf(1,i)-xv(1); 9 | dy= xf(2,i)-xv(2); 10 | d2= dx^2 + dy^2; 11 | d= sqrt(d2); 12 | 13 | zp(:,i)= [d; pi_to_pi(atan2(dy,dx) - xv(3))]; % predicted observation 14 | 15 | Hv(:,:,i)= [-dx/d, -dy/d, 0; % Jacobian wrt vehicle states 16 | dy/d2, -dx/d2, -1]; 17 | Hf(:,:,i)= [ dx/d, dy/d; % Jacobian wrt feature states 18 | -dy/d2, dx/d2]; 19 | Sf(:,:,i)= Hf(:,:,i) * Pf(:,:,i) * Hf(:,:,i)' + R; % innovation covariance of 'feature observation given the vehicle' 20 | end 21 | -------------------------------------------------------------------------------- /matlab/compute_steering.m: -------------------------------------------------------------------------------- 1 | function [G,iwp]= compute_steering(x, wp, iwp, minD, G, rateG, maxG, dt) 2 | % 3 | % INPUTS: 4 | % x - true position 5 | % wp - waypoints 6 | % iwp - index to current waypoint 7 | % minD - minimum distance to current waypoint before switching to next 8 | % G - current steering angle 9 | % rateG - max steering rate (rad/s) 10 | % maxG - max steering angle (rad) 11 | % dt - timestep 12 | 13 | % determine if current waypoint reached 14 | cwp= wp(:,iwp); 15 | d2= (cwp(1)-x(1))^2 + (cwp(2)-x(2))^2; 16 | if d2 < minD^2 17 | iwp= iwp+1; % switch to next 18 | if iwp > size(wp,2) % reached final waypoint, flag and return 19 | iwp=0; 20 | return; 21 | end 22 | cwp= wp(:,iwp); % next waypoint 23 | end 24 | 25 | % compute change in G to point towards current waypoint 26 | deltaG= pi_to_pi(atan2(cwp(2)-x(2), cwp(1)-x(1)) - x(3) - G); 27 | 28 | % limit rate 29 | maxDelta= rateG*dt; 30 | if abs(deltaG) > maxDelta 31 | deltaG= sign(deltaG)*maxDelta; 32 | end 33 | 34 | % limit angle 35 | G= G+deltaG; 36 | if abs(G) > maxG 37 | G= sign(G)*maxG; 38 | end 39 | -------------------------------------------------------------------------------- /matlab/configfile.m: -------------------------------------------------------------------------------- 1 | %%% Configuration file 2 | %%% Permits various adjustments to parameters of the FastSLAM algorithm. 3 | %%% See fastslam_sim.m for more information 4 | 5 | % control parameters 6 | V= 3; % m/s 7 | MAXG= 30*pi/180; % radians, maximum steering angle (-MAXG < g < MAXG) 8 | RATEG= 20*pi/180; % rad/s, maximum rate of change in steer angle 9 | WHEELBASE= 4; % metres, vehicle wheel-base 10 | DT_CONTROLS= 0.025; % seconds, time interval between control signals 11 | 12 | % control noises 13 | sigmaV= 0.3; % m/s 14 | sigmaG= (3.0*pi/180); % radians 15 | Q= [sigmaV^2 0; 0 sigmaG^2]; 16 | 17 | % observation parameters 18 | MAX_RANGE= 30.0; % metres 19 | DT_OBSERVE= 8*DT_CONTROLS; % seconds, time interval between observations 20 | 21 | % observation noises 22 | sigmaR= 0.1; % metres 23 | sigmaB= (1.0*pi/180); % radians 24 | R= [sigmaR^2 0; 0 sigmaB^2]; 25 | 26 | % waypoint proximity 27 | AT_WAYPOINT= 1.0; % metres, distance from current waypoint at which to switch to next waypoint 28 | NUMBER_LOOPS= 2; % number of loops through the waypoint list 29 | 30 | % resampling 31 | NPARTICLES= 100; 32 | NEFFECTIVE= 0.75*NPARTICLES; % minimum number of effective particles before resampling 33 | 34 | % switches 35 | SWITCH_CONTROL_NOISE= 1; 36 | SWITCH_SENSOR_NOISE = 1; 37 | SWITCH_INFLATE_NOISE= 0; 38 | SWITCH_PREDICT_NOISE = 0; % sample noise from predict (usually 1 for fastslam1.0 and 0 for fastslam2.0) 39 | SWITCH_SAMPLE_PROPOSAL = 1; % sample from proposal (no effect on fastslam1.0 and usually 1 for fastslam2.0) 40 | SWITCH_HEADING_KNOWN= 0; 41 | SWITCH_RESAMPLE= 1; 42 | SWITCH_PROFILE= 1; 43 | SWITCH_SEED_RANDOM= 0; % if not 0, seed the randn() with its value at beginning of simulation (for repeatability) 44 | 45 | -------------------------------------------------------------------------------- /matlab/data_associate_known.m: -------------------------------------------------------------------------------- 1 | function [zf,idf,zn,table]= data_associate_known(z, idz, table, Nf) 2 | % 3 | 4 | zf= []; zn= []; 5 | idf= []; idn= []; 6 | 7 | % find associations (zf) and new features (zn) 8 | for i=1:length(idz) 9 | ii= idz(i); 10 | if table(ii) == 0 % new feature 11 | zn= [zn z(:,i)]; 12 | idn= [idn ii]; 13 | else 14 | zf= [zf z(:,i)]; 15 | idf= [idf table(ii)]; 16 | end 17 | end 18 | 19 | % add new feature IDs to lookup table 20 | table(idn)= Nf + (1:size(zn,2)); % add new feature positions to lookup table 21 | -------------------------------------------------------------------------------- /matlab/example_webmap.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yglee/FastSLAM/fa3b24d789c18dad56f5c5bbb99f69f8c0ef6bdb/matlab/example_webmap.mat -------------------------------------------------------------------------------- /matlab/fast_to_ekf_diag.m: -------------------------------------------------------------------------------- 1 | function [x,P]= fast_to_ekf_diag(p) 2 | %function [x,P]= fast_to_ekf_diag(particles) 3 | % 4 | % particles(i) has xv, Pv, {xf, Pf} 5 | % For this code to work, the particles must be of equal weight. 6 | % Also assume all particles possess the same features (in the 7 | % same order). 8 | 9 | N= length(p); 10 | lenf= size(p(1).xf, 2); 11 | lenx= 3 + 2*lenf; 12 | x= zeros(lenx, 1); 13 | P= zeros(lenx, lenx); 14 | 15 | % vehicle mean 16 | ii= 1:3; 17 | x(ii)= mean([p.xv],2); 18 | 19 | % vehicle covariance 20 | Pvv= zeros(3); 21 | for i=1:N 22 | xv= p(i).xv; 23 | Pvv= Pvv + xv * xv'; 24 | end 25 | P(ii,ii)= Pvv/N - x(ii) * x(ii)'; 26 | 27 | % feature-related parts 28 | for i=1:lenf 29 | xfs= [0;0]; 30 | Pvf= zeros(3,2); 31 | Pff= zeros(2); 32 | for k=1:N 33 | xv= p(k).xv; 34 | xf= p(k).xf(:,i); 35 | xfs= xfs + xf; 36 | Pvf= Pvf + xv * xf'; 37 | Pff= Pff + xf * xf' + p(k).Pf(:,:,i); 38 | end 39 | 40 | % feature mean 41 | jj= 2*i + 2; 42 | jj= jj:(jj+1); 43 | x(jj)= xfs/N; 44 | 45 | % feature covariance 46 | P(jj,jj)= Pff/N - x(jj) * x(jj)'; 47 | 48 | % vehicle-feature correlations 49 | P(ii,jj)= Pvf/N - x(ii) * x(jj)'; 50 | P(jj,ii)= P(ii,jj)'; 51 | end 52 | -------------------------------------------------------------------------------- /matlab/fastslam1/compute_weight.m: -------------------------------------------------------------------------------- 1 | function w= compute_weight(particle, z, idf, R) 2 | 3 | [zp,Hv,Hf,Sf]= compute_jacobians(particle, idf, R); 4 | 5 | v= z-zp; 6 | v(2,:)= pi_to_pi(v(2,:)); 7 | 8 | w= 1; 9 | for i=1:size(z,2) 10 | S= Sf(:,:,i); 11 | den= 2*pi*sqrt(det(S)); 12 | num= exp(-0.5 * v(:,i)' * inv(S) * v(:,i)); 13 | w = w* num/den; 14 | end 15 | -------------------------------------------------------------------------------- /matlab/fastslam1/fastslam1_sim.m: -------------------------------------------------------------------------------- 1 | function data= fastslam1_sim(lm, wp) 2 | %function data= fastslam1_sim(lm, wp) 3 | % 4 | % INPUTS: 5 | % lm - set of landmarks 6 | % wp - set of waypoints 7 | % 8 | % OUTPUTS: 9 | % data - set of particles representing final state 10 | % 11 | % NOTES: 12 | % This program is a FastSLAM 1.0 simulator. To use, create a set of landmarks and 13 | % vehicle waypoints (ie, waypoints for the desired vehicle path). The program 14 | % 'frontend.m' may be used to create this simulated environment - type 15 | % 'help frontend' for more information. 16 | % The configuration of the simulator is managed by the script file 17 | % 'configfile.m'. To alter the parameters of the vehicle, sensors, etc 18 | % adjust this file. There are also several switches that control certain 19 | % filter options. 20 | % 21 | % Tim Bailey and Juan Nieto 2004. 22 | % Version 1.0 23 | 24 | format compact 25 | path(path, '../') 26 | configfile; 27 | 28 | if SWITCH_PREDICT_NOISE==0, warning('Sampling from predict noise is necessary for FastSLAM 1.0 particle diversity'), end 29 | 30 | h= setup_animations(lm,wp); 31 | veh= [0 -WHEELBASE -WHEELBASE; 0 -1 1]; 32 | plines=[]; 33 | 34 | % initialisations 35 | particles= initialise_particles(NPARTICLES); 36 | xtrue= zeros(3,1); 37 | 38 | dt= DT_CONTROLS; % change in time between predicts 39 | dtsum= 0; % change in time since last observation 40 | ftag= 1:size(lm,2); % identifier for each landmark 41 | da_table= zeros(1,size(lm,2)); % data association table 42 | iwp= 1; % index to first waypoint 43 | G= 0; % initial steer angle 44 | 45 | if SWITCH_SEED_RANDOM ~= 0, rand('state',SWITCH_SEED_RANDOM), randn('state',SWITCH_SEED_RANDOM), end 46 | 47 | Qe= Q; Re= R; 48 | if SWITCH_INFLATE_NOISE==1, Qe= 2*Q; Re= 2*R; end 49 | 50 | if SWITCH_PROFILE, profile on -detail builtin, end 51 | 52 | % Main loop 53 | while iwp ~= 0 54 | 55 | % Compute true data 56 | [G,iwp]= compute_steering(xtrue, wp, iwp, AT_WAYPOINT, G, RATEG, MAXG, dt); 57 | if iwp==0 & NUMBER_LOOPS > 1, iwp=1; NUMBER_LOOPS= NUMBER_LOOPS-1; end % path loopfs repeat 58 | xtrue= predict_true(xtrue, V,G, WHEELBASE,dt); 59 | 60 | % Add process noise 61 | [Vn,Gn]= add_control_noise(V,G,Q, SWITCH_CONTROL_NOISE); 62 | 63 | % Predict step 64 | for i=1:NPARTICLES 65 | particles(i)= predict (particles(i), Vn,Gn,Qe, WHEELBASE,dt, SWITCH_PREDICT_NOISE); 66 | if SWITCH_HEADING_KNOWN==1, particles(i).xf(3)= xtrue(3); end 67 | end 68 | 69 | % Observe step 70 | dtsum= dtsum + dt; 71 | if dtsum >= DT_OBSERVE 72 | dtsum= 0; 73 | 74 | % Compute true data, then add noise 75 | [z,ftag_visible]= get_observations(xtrue, lm, ftag, MAX_RANGE); 76 | z= add_observation_noise(z,R, SWITCH_SENSOR_NOISE); 77 | if ~isempty(z), plines= make_laser_lines (z,xtrue); end 78 | 79 | % Compute (known) data associations 80 | Nf= size(particles(1).xf,2); 81 | [zf,idf,zn,da_table]= data_associate_known(z, ftag_visible, da_table, Nf); 82 | 83 | % Perform update 84 | for i=1:NPARTICLES 85 | if ~isempty(zf) % observe map features 86 | w= compute_weight(particles(i), zf,idf, R); % w = p(z_k | x_k) 87 | particles(i).w= particles(i).w * w; 88 | particles(i)= feature_update(particles(i), zf, idf, R); 89 | end 90 | 91 | if ~isempty(zn) % observe new features, augment map 92 | particles(i)= add_feature(particles(i), zn,R); 93 | end 94 | end 95 | 96 | particles= resample_particles(particles, NEFFECTIVE, SWITCH_RESAMPLE); 97 | end 98 | 99 | % Plots 100 | do_plot(h, particles, xtrue, plines, veh); 101 | end 102 | 103 | if SWITCH_PROFILE, profile report, end 104 | 105 | data= particles; 106 | 107 | % 108 | % 109 | 110 | function h= setup_animations(lm,wp) 111 | figure 112 | plot(lm(1,:),lm(2,:),'g*') 113 | hold on, axis equal 114 | plot(wp(1,:),wp(2,:), wp(1,:),wp(2,:),'ro') 115 | 116 | h.xt= patch(0,0,'g','erasemode','xor'); % vehicle true 117 | h.xm= patch(0,0,'r','erasemode','xor'); % mean vehicle estimate 118 | h.obs= plot(0,0,'y','erasemode','xor'); % observations 119 | h.xfp= plot(0,0,'r.','erasemode','background'); % estimated features (particle means) 120 | h.xvp= plot(0,0,'r.','erasemode','xor'); % estimated vehicle (particles) 121 | h.cov= plot(0,0,'erasemode','xor'); % covariances of max weight particle 122 | 123 | function do_plot(h, particles, xtrue, plines, veh) 124 | xvp = [particles.xv]; 125 | xfp = [particles.xf]; 126 | w = [particles.w]; 127 | 128 | ii= find(w== max(w)); 129 | xvmax= xvp(:,ii); 130 | 131 | xt= transformtoglobal(veh,xtrue); 132 | xm= transformtoglobal(veh,xvmax); 133 | set(h.xt, 'xdata', xt(1,:), 'ydata', xt(2,:)) 134 | set(h.xm, 'xdata', xm(1,:), 'ydata', xm(2,:)) 135 | set(h.xvp, 'xdata', xvp(1,:), 'ydata', xvp(2,:)) 136 | if ~isempty(xfp), set(h.xfp, 'xdata', xfp(1,:), 'ydata', xfp(2,:)), end 137 | if ~isempty(plines), set(h.obs, 'xdata', plines(1,:), 'ydata', plines(2,:)), end 138 | pcov= make_covariance_ellipses(particles(ii(1))); 139 | if ~isempty(pcov), set(h.cov, 'xdata', pcov(1,:), 'ydata', pcov(2,:)); end 140 | 141 | drawnow 142 | 143 | function p= make_laser_lines (rb,xv) 144 | if isempty(rb), p=[]; return, end 145 | len= size(rb,2); 146 | lnes(1,:)= zeros(1,len)+ xv(1); 147 | lnes(2,:)= zeros(1,len)+ xv(2); 148 | lnes(3:4,:)= transformtoglobal([rb(1,:).*cos(rb(2,:)); rb(1,:).*sin(rb(2,:))], xv); 149 | p= line_plot_conversion (lnes); 150 | 151 | function p= initialise_particles(np) 152 | for i=1:np 153 | p(i).w= 1/np; 154 | p(i).xv= [0;0;0]; 155 | p(i).xf= []; 156 | p(i).Pf= []; 157 | end 158 | 159 | function p= make_covariance_ellipses(particle) 160 | % part of plotting routines 161 | p= []; 162 | lenf= size(particle.xf,2); 163 | 164 | if lenf > 0 165 | N= 10; 166 | inc= 2*pi/N; 167 | phi= 0:inc:2*pi; 168 | circ= 2*[cos(phi); sin(phi)]; 169 | 170 | xf= particle.xf; 171 | Pf= particle.Pf; 172 | p= zeros (2, lenf*(N+2)); 173 | 174 | ctr= 1; 175 | for i=1:lenf 176 | ii= ctr:(ctr+N+1); 177 | p(:,ii)= make_ellipse(xf(:,i), Pf(:,:,i), circ); 178 | ctr= ctr+N+2; 179 | end 180 | end 181 | 182 | function p= make_ellipse(x,P,circ) 183 | % make a single 2-D ellipse 184 | r= sqrtm_2by2(P); 185 | a= r*circ; 186 | p(2,:)= [a(2,:)+x(2) NaN]; 187 | p(1,:)= [a(1,:)+x(1) NaN]; 188 | -------------------------------------------------------------------------------- /matlab/fastslam1/predict.m: -------------------------------------------------------------------------------- 1 | function particle= predict(particle, V,G,Q, WB,dt, addrandom) 2 | % 3 | 4 | % add random noise to controls 5 | if addrandom == 1 6 | VG= multivariate_gauss([V;G], Q, 1); 7 | V= VG(1); G= VG(2); 8 | end 9 | 10 | % predict state 11 | xv= particle.xv; 12 | particle.xv= [xv(1) + V*dt*cos(G+xv(3,:)); 13 | xv(2) + V*dt*sin(G+xv(3,:)); 14 | pi_to_pi(xv(3) + V*dt*sin(G)/WB)]; 15 | 16 | -------------------------------------------------------------------------------- /matlab/fastslam2/README: -------------------------------------------------------------------------------- 1 | How to invoke the fastslam code in matlab 2 | 3 | first load the "example_webmap.mat" code by typing: 4 | "S = load('example_webmap.mat')" inside matlab command window. 5 | 6 | Then go inside fastslam2 directory. Type in the command window: 7 | fastslam2_sim(S.lm, S.wp) 8 | 9 | This will run your simulation 10 | -------------------------------------------------------------------------------- /matlab/fastslam2/compute_weight.m: -------------------------------------------------------------------------------- 1 | function w= compute_weight(particle, z, idf, R) 2 | 3 | %% 4 | %% Not used - weight is now computed within sample_proposal.m 5 | %% -------------------------------------------------------------------------------- /matlab/fastslam2/fastslam2_sim.m: -------------------------------------------------------------------------------- 1 | function data= fastslam2_sim(lm, wp) 2 | %function data= fastslam2_sim(lm, wp) 3 | % 4 | % INPUTS: 5 | % lm - set of landmarks 6 | % wp - set of waypoints 7 | % 8 | % OUTPUTS: 9 | % data - set of particles representing final state 10 | % 11 | % NOTES: 12 | % This program is a FastSLAM 2.0 simulator. To use, create a set of landmarks and 13 | % vehicle waypoints (ie, waypoints for the desired vehicle path). The program 14 | % 'frontend.m' may be used to create this simulated environment - type 15 | % 'help frontend' for more information. 16 | % The configuration of the simulator is managed by the script file 17 | % 'configfile.m'. To alter the parameters of the vehicle, sensors, etc 18 | % adjust this file. There are also several switches that control certain 19 | % filter options. 20 | % 21 | % Tim Bailey and Juan Nieto 2006. 22 | % Version 2.0 23 | 24 | format compact 25 | path(path, '../') 26 | configfile; 27 | 28 | if SWITCH_PREDICT_NOISE, warning('Sampling from predict noise usually OFF for FastSLAM 2.0'), end 29 | if SWITCH_SAMPLE_PROPOSAL==0, warning('Sampling from optimal proposal is usually ON for FastSLAM 2.0'), end 30 | 31 | h= setup_animations(lm,wp); 32 | veh= [0 -WHEELBASE -WHEELBASE; 0 -1 1]; 33 | 34 | % Initialisations 35 | particles= initialise_particles(NPARTICLES); 36 | xtrue= zeros(3,1); 37 | 38 | dt= DT_CONTROLS; % change in time between predicts 39 | dtsum= 0; % change in time since last observation 40 | ftag= 1:size(lm,2); % identifier for each landmark 41 | da_table= zeros(1,size(lm,2)); % data association table 42 | iwp= 1; % index to first waypoint 43 | G= 0; % initial steer angle 44 | plines=[]; 45 | if SWITCH_SEED_RANDOM ~= 0, rand('state',SWITCH_SEED_RANDOM), randn('state',SWITCH_SEED_RANDOM), end 46 | 47 | Qe= Q; Re= R; 48 | if SWITCH_INFLATE_NOISE==1, Qe= 2*Q; Re= 2*R; end 49 | 50 | if SWITCH_PROFILE, profile on -detail builtin, end 51 | 52 | % Main loop 53 | while iwp ~= 0 54 | 55 | % compute true data 56 | [G,iwp]= compute_steering(xtrue, wp, iwp, AT_WAYPOINT, G, RATEG, MAXG, dt); 57 | if iwp==0 & NUMBER_LOOPS > 1, iwp=1; NUMBER_LOOPS= NUMBER_LOOPS-1; end 58 | xtrue= predict_true(xtrue, V,G, WHEELBASE,dt); 59 | 60 | % add process noise 61 | [Vn,Gn]= add_control_noise(V,G,Q, SWITCH_CONTROL_NOISE); 62 | 63 | % Predict step 64 | for i=1:NPARTICLES 65 | particles(i)= predict (particles(i), Vn,Gn,Qe, WHEELBASE,dt, SWITCH_PREDICT_NOISE); 66 | particles(i)= observe_heading(particles(i), xtrue(3), SWITCH_HEADING_KNOWN); % if heading known, observe heading 67 | end 68 | 69 | % Observe step 70 | dtsum= dtsum + dt; 71 | if dtsum >= DT_OBSERVE 72 | dtsum= 0; 73 | 74 | % Compute true data, then add noise 75 | [z,ftag_visible]= get_observations(xtrue, lm, ftag, MAX_RANGE); 76 | z= add_observation_noise(z,R, SWITCH_SENSOR_NOISE); 77 | if ~isempty(z), plines= make_laser_lines (z,xtrue); end 78 | 79 | % Compute (known) data associations 80 | Nf= size(particles(1).xf,2); 81 | [zf,idf,zn,da_table]= data_associate_known(z, ftag_visible, da_table, Nf); 82 | 83 | % Observe map features 84 | if ~isempty(zf) 85 | 86 | % sample from "optimal" proposal distribution, then update map 87 | for i=1:NPARTICLES 88 | particles(i)= sample_proposal(particles(i), zf,idf, Re); 89 | particles(i)= feature_update(particles(i), zf, idf, Re); 90 | end 91 | 92 | % resample 93 | particles= resample_particles(particles, NEFFECTIVE, SWITCH_RESAMPLE); 94 | end 95 | 96 | % Observe new features, augment map 97 | if ~isempty(zn) 98 | for i=1:NPARTICLES 99 | if isempty(zf) % sample from proposal distribution (if we have not already done so above) 100 | particles(i).xv= multivariate_gauss(particles(i).xv, particles(i).Pv, 1); 101 | particles(i).Pv= zeros(3); 102 | end 103 | particles(i)= add_feature(particles(i), zn,Re); 104 | end 105 | end 106 | 107 | end 108 | 109 | % plots 110 | do_plot(h, particles, xtrue, plines, veh) 111 | end 112 | 113 | if SWITCH_PROFILE, profile report, end 114 | 115 | data= particles; 116 | 117 | % 118 | % 119 | 120 | function p= make_laser_lines (rb,xv) 121 | if isempty(rb), p=[]; return, end 122 | len= size(rb,2); 123 | lnes(1,:)= zeros(1,len)+ xv(1); 124 | lnes(2,:)= zeros(1,len)+ xv(2); 125 | lnes(3:4,:)= transformtoglobal([rb(1,:).*cos(rb(2,:)); rb(1,:).*sin(rb(2,:))], xv); 126 | p= line_plot_conversion (lnes); 127 | 128 | function p= initialise_particles(np) 129 | for i=1:np 130 | p(i).w= 1/np; 131 | p(i).xv= [0;0;0]; 132 | p(i).Pv= zeros(3); 133 | p(i).xf= []; 134 | p(i).Pf= []; 135 | p(i).da= []; 136 | end 137 | 138 | %don't implement this 139 | function p= make_covariance_ellipses(particle) 140 | N= 10; 141 | inc= 2*pi/N; 142 | phi= 0:inc:2*pi; 143 | circ= 2*[cos(phi); sin(phi)]; 144 | 145 | p= make_ellipse(particle.xv(1:2), particle.Pv(1:2,1:2) + eye(2)*eps, circ); 146 | 147 | lenf= size(particle.xf,2); 148 | if lenf > 0 149 | 150 | xf= particle.xf; 151 | Pf= particle.Pf; 152 | p= [p zeros(2, lenf*(N+2))]; 153 | 154 | ctr= N+3; 155 | for i=1:lenf 156 | ii= ctr:(ctr+N+1); 157 | p(:,ii)= make_ellipse(xf(:,i), Pf(:,:,i), circ); 158 | ctr= ctr+N+2; 159 | end 160 | end 161 | 162 | %don't implement this 163 | function p= make_ellipse(x,P,circ) 164 | % make a single 2-D ellipse 165 | r= sqrtm_2by2(P); 166 | a= r*circ; 167 | p(2,:)= [a(2,:)+x(2) NaN]; 168 | p(1,:)= [a(1,:)+x(1) NaN]; 169 | 170 | % 171 | % 172 | 173 | %don't implement this 174 | function h= setup_animations(lm,wp) 175 | figure 176 | plot(lm(1,:),lm(2,:),'g*') 177 | hold on, axis equal 178 | plot(wp(1,:),wp(2,:), wp(1,:),wp(2,:),'ro') 179 | 180 | h.xt= patch(0,0,'g','erasemode','xor'); % vehicle true 181 | h.xm= patch(0,0,'r','erasemode','xor'); % mean vehicle estimate 182 | h.obs= plot(0,0,'y','erasemode','xor'); % observations 183 | h.xfp= plot(0,0,'r.','erasemode','background'); % estimated features (particle means) 184 | h.xvp= plot(0,0,'r.','erasemode','xor'); % estimated vehicle (particles) 185 | h.cov= plot(0,0,'erasemode','xor'); % covariances of max weight particle 186 | 187 | %dont implement this 188 | function do_plot(h, particles, xtrue, plines, veh) 189 | 190 | xvp = [particles.xv]; 191 | xfp = [particles.xf]; 192 | w = [particles.w]; 193 | 194 | ii= find(w== max(w)); 195 | xvmax= xvp(:,ii); 196 | 197 | xt= transformtoglobal(veh,xtrue); 198 | xm= transformtoglobal(veh,xvmax); 199 | set(h.xt, 'xdata', xt(1,:), 'ydata', xt(2,:)) 200 | set(h.xm, 'xdata', xm(1,:), 'ydata', xm(2,:)) 201 | set(h.xvp, 'xdata', xvp(1,:), 'ydata', xvp(2,:)) 202 | if ~isempty(xfp), set(h.xfp, 'xdata', xfp(1,:), 'ydata', xfp(2,:)), end 203 | if ~isempty(plines), set(h.obs, 'xdata', plines(1,:), 'ydata', plines(2,:)), end 204 | pcov= make_covariance_ellipses(particles(ii(1))); 205 | if ~isempty(pcov), set(h.cov, 'xdata', pcov(1,:), 'ydata', pcov(2,:)); end 206 | 207 | drawnow 208 | -------------------------------------------------------------------------------- /matlab/fastslam2/gauss_evaluate.m: -------------------------------------------------------------------------------- 1 | function w = gauss_evaluate(v,S,logflag) 2 | %function w = gauss_evaluate(v,S,logflag) 3 | % 4 | % INPUTS: 5 | % v - a set of innovation vectors. 6 | % S - the covariance matrix for the innovations. 7 | % logflag - - if 1 computes the log-likelihood, otherwise computes 8 | % the likelihood. 9 | % 10 | % OUTPUT: 11 | % w - set of Gaussian likelihoods or log-likelihoods for each v(:,i). 12 | % 13 | % This implementation uses the Cholesky factor of S to compute the likelihoods 14 | % and so is more numerically stable than a simple full covariance form. 15 | % This function is identical to gauss_likelihood(). 16 | % 17 | % Tim Bailey 2005. 18 | 19 | if nargin == 2, logflag = 0; end 20 | 21 | D = size(v,1); 22 | Sc = chol(S)'; 23 | nin = Sc\v; % normalised innovation 24 | E = -0.5 * sum(nin.*nin, 1); % Gaussian exponential term 25 | % Note: writing sum(x.*x, 1) is a fast way to compute sets of inner-products. 26 | 27 | if logflag ~= 1 28 | C = (2*pi)^(D/2) * prod(diag(Sc)); % normalising term (makes Gaussian hyper-volume equal 1) 29 | w = exp(E) / C; % likelihood 30 | else 31 | C = 0.5*D*log(2*pi) + sum(log(diag(Sc))); % log of normalising term 32 | w = E - C; % log-likelihood 33 | end 34 | -------------------------------------------------------------------------------- /matlab/fastslam2/observe_heading.m: -------------------------------------------------------------------------------- 1 | function particle= observe_heading(particle, phi, useheading) 2 | 3 | if useheading==0, return, end 4 | sigmaPhi= 0.01*pi/180; % radians, heading uncertainty 5 | 6 | xv= particle.xv; 7 | Pv= particle.Pv; 8 | 9 | H= [0 0 1]; 10 | v= pi_to_pi(phi - xv(3)); 11 | 12 | [xv, Pv]= KF_joseph_update(xv, Pv, v, sigmaPhi^2,H); 13 | particle.xv= xv; 14 | particle.Pv= Pv; 15 | -------------------------------------------------------------------------------- /matlab/fastslam2/predict.m: -------------------------------------------------------------------------------- 1 | function particle= predict(particle, V,G,Q, WB,dt, addrandom) 2 | % 3 | % INPUTS: 4 | % xv - vehicle pose sample 5 | % Pv - vehicle pose predict covariance 6 | % 7 | % Note: Pv must be zeroed after each observation. It accumulates the 8 | % vehicle pose uncertainty between measurements. 9 | 10 | xv= particle.xv; 11 | Pv= particle.Pv; 12 | 13 | % Jacobians 14 | phi= xv(3); 15 | Gv= [1 0 -V*dt*sin(G+phi); 16 | 0 1 V*dt*cos(G+phi); 17 | 0 0 1]; 18 | Gu= [dt*cos(G+phi) -V*dt*sin(G+phi); 19 | dt*sin(G+phi) V*dt*cos(G+phi); 20 | dt*sin(G)/WB V*dt*cos(G)/WB]; 21 | 22 | % predict covariance 23 | particle.Pv= Gv*Pv*Gv' + Gu*Q*Gu'; 24 | 25 | % optional: add random noise to predicted state 26 | if addrandom == 1 27 | VG= multivariate_gauss([V;G], Q, 1); 28 | V= VG(1); G= VG(2); 29 | end 30 | 31 | % predict state 32 | particle.xv= [xv(1) + V*dt*cos(G+xv(3,:)); 33 | xv(2) + V*dt*sin(G+xv(3,:)); 34 | pi_to_pi(xv(3) + V*dt*sin(G)/WB)]; 35 | 36 | % 37 | % 38 | 39 | function x= pi_to_pi(x) 40 | if x > pi 41 | x= x - 2*pi; 42 | elseif x < -pi 43 | x= x + 2*pi; 44 | end 45 | -------------------------------------------------------------------------------- /matlab/fastslam2/proposal.mws: -------------------------------------------------------------------------------- 1 | {VERSION 2 3 "IBM INTEL NT" "2.3" } 2 | {USTYLETAB {CSTYLE "Maple Input" -1 0 "Courier" 0 1 255 0 0 1 0 1 0 0 3 | 1 0 0 0 0 }{CSTYLE "2D Math" -1 2 "Times" 0 1 0 0 0 0 0 0 2 0 0 0 0 0 4 | 0 }{CSTYLE "2D Output" 2 20 "" 0 1 0 0 255 1 0 0 0 0 0 0 0 0 0 } 5 | {PSTYLE "Normal" -1 0 1 {CSTYLE "" -1 -1 "" 0 1 0 0 0 0 0 0 0 0 0 0 0 6 | 0 0 }0 0 0 -1 -1 -1 0 0 0 0 0 0 -1 0 }{PSTYLE "Text Output" -1 2 1 7 | {CSTYLE "" -1 -1 "Courier" 1 10 0 0 255 1 0 0 0 0 0 1 3 0 0 }1 0 0 -1 8 | -1 -1 0 0 0 0 0 0 -1 0 }{PSTYLE "Warning" 2 7 1 {CSTYLE "" -1 -1 "" 0 9 | 1 0 0 255 1 0 0 0 0 0 0 1 0 0 }0 0 0 -1 -1 -1 0 0 0 0 0 0 -1 0 } 10 | {PSTYLE "Maple Output" 0 11 1 {CSTYLE "" -1 -1 "" 0 1 0 0 0 0 0 0 0 0 11 | 0 0 0 0 0 }3 3 0 -1 -1 -1 0 0 0 0 0 0 -1 0 }{PSTYLE "" 11 12 1 12 | {CSTYLE "" -1 -1 "" 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 }1 0 0 -1 -1 -1 0 0 13 | 0 0 0 0 -1 0 }} 14 | {SECT 0 {EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 13 "with(linalg):" }} 15 | {PARA 7 "" 1 "" {TEXT -1 32 "Warning, new definition for norm" }} 16 | {PARA 7 "" 1 "" {TEXT -1 33 "Warning, new definition for trace" }}} 17 | {EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 0 "" }}}{EXCHG {PARA 0 "" 0 "" 18 | {TEXT -1 10 "Variables:" }{MPLTEXT 1 0 0 "" }}}{EXCHG {PARA 0 "> " 0 " 19 | " {MPLTEXT 1 0 29 "Xv:= array([[xv],[yv],[pv]]):" }}}{EXCHG {PARA 0 "> 20 | " 0 "" {MPLTEXT 1 0 27 "Xf1:= array([[xf1],[yf1]]):" }}}{EXCHG {PARA 21 | 0 "> " 0 "" {MPLTEXT 1 0 27 "Xf2:= array([[xf2],[yf2]]):" }}}{EXCHG 22 | {PARA 0 "> " 0 "" {MPLTEXT 1 0 0 "" }}}{EXCHG {PARA 0 "> " 0 "" 23 | {MPLTEXT 1 0 71 "Pv:= array([[Pvxx, Pvxy, Pvxp],[Pvxy, Pvyy, Pvyp],[Pv 24 | xp, Pvyp, Pvpp]]):" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 45 "Pf1:= 25 | array([[Pf1xx, Pf1xy],[Pf1xy, Pf1yy]]):" }}}{EXCHG {PARA 0 "> " 0 "" 26 | {MPLTEXT 1 0 45 "Pf2:= array([[Pf2xx, Pf2xy],[Pf2xy, Pf2yy]]):" }}} 27 | {EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 60 "Pvf:= array([[Pvf11, Pvf12], 28 | [Pvf21, Pvf22],[Pvf31, Pvf32]]):" }}}{EXCHG {PARA 0 "> " 0 "" 29 | {MPLTEXT 1 0 0 "" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 53 "Hv:= ar 30 | ray([[Hv11, Hv12, Hv13], [Hv21, Hv22, Hv23]]):" }}}{EXCHG {PARA 0 "> \+ 31 | " 0 "" {MPLTEXT 1 0 41 "Hf:= array([[Hf11, Hf12], [Hf21, Hf22]]):" }}} 32 | {EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 0 "" }}}{EXCHG {PARA 0 "> " 0 " 33 | " {MPLTEXT 1 0 23 "v:= array([[v1],[v2]]):" }}}{EXCHG {PARA 0 "> " 0 " 34 | " {MPLTEXT 1 0 27 "R:= array([[r1,0],[0,r2]]):" }}}{EXCHG {PARA 0 "> \+ 35 | " 0 "" {MPLTEXT 1 0 0 "" }}}{EXCHG {PARA 0 "" 0 "" {TEXT -1 10 "Method 36 | 1: " }{MPLTEXT 1 0 0 "" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 18 " 37 | X:= stack(Xv,Xf2):" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 18 "P:= d 38 | iag(Pv, Pf2):" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 19 "H:= concat 39 | (Hv, Hf):" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 38 "S:= evalm(H &* 40 | P &* transpose(H) + R):" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 43 41 | "W:= evalm(P &* transpose(H) &* inverse(S)):" }}}{EXCHG {PARA 0 "> " 42 | 0 "" {MPLTEXT 1 0 23 "Xa:= evalm(X + W &* v):" }}}{EXCHG {PARA 0 "> " 43 | 0 "" {MPLTEXT 1 0 39 "Pa:= evalm(P - W &* S &* transpose(W)):" }}} 44 | {EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 0 "" }}}{EXCHG {PARA 0 "" 0 "" 45 | {TEXT -1 9 "Method 2:" }{MPLTEXT 1 0 0 "" }}}{EXCHG {PARA 0 "> " 0 "" 46 | {MPLTEXT 1 0 22 "X:= stack(Xv,Xf1,Xf2):" }}}{EXCHG {PARA 0 "> " 0 "" 47 | {MPLTEXT 1 0 21 "P:= diag(Pv,Pf1,Pf2):" }}}{EXCHG {PARA 0 "> " 0 "" 48 | {MPLTEXT 1 0 113 "P[1,4]:= Pvf[1,1]: P[1,5]:= Pvf[1,2]: P[2,4]:= Pvf[2 49 | ,1]: P[2,5]:= Pvf[2,2]: P[3,4]:= Pvf[3,1]: P[3,5]:= Pvf[3,2]:" }}} 50 | {EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 127 "P[4,1]:= Pvf[1,1]: P[5,1]:= 51 | Pvf[1,2]: P[4,2]:= Pvf[2,1]: P[5,2]:= Pvf[2,2]: P[4,3]:= Pvf[3,1]: P[ 52 | 5,3]:= Pvf[3,2]: P:= evalm(P):" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 53 | 1 0 40 "H:= concat(Hv,array([[0,0],[0,0]]), Hf):" }}}{EXCHG {PARA 0 "> 54 | " 0 "" {MPLTEXT 1 0 38 "S:= evalm(H &* P &* transpose(H) + R):" }}} 55 | {EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 43 "W:= evalm(P &* transpose(H) \+ 56 | &* inverse(S)):" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 23 "Xb:= eva 57 | lm(X + W &* v):" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 39 "Pb:= eva 58 | lm(P - W &* S &* transpose(W)):" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 59 | 1 0 0 "" }}}{EXCHG {PARA 0 "" 0 "" {TEXT -1 10 "Method 3: " }{MPLTEXT 60 | 1 0 0 "" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 41 "S:= evalm(Hv &* \+ 61 | Pv &* transpose(Hv) + R):" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 62 | 45 "W:= evalm(Pv &* transpose(Hv) &* inverse(S)):" }}}{EXCHG {PARA 0 " 63 | > " 0 "" {MPLTEXT 1 0 24 "Xc:= evalm(Xv + W &* v):" }}}{EXCHG {PARA 0 64 | "> " 0 "" {MPLTEXT 1 0 40 "Pc:= evalm(Pv - W &* S &* transpose(W)):" } 65 | }}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 0 "" }}}{EXCHG {PARA 0 "" 0 " 66 | " {TEXT -1 20 "Method 4: Montemerlo" }}}{EXCHG {PARA 0 "> " 0 "" 67 | {MPLTEXT 1 0 35 "Q:= Hf &* Pf2 &* transpose(Hf) + R:" }}}{EXCHG {PARA 68 | 0 "> " 0 "" {MPLTEXT 1 0 62 "Pd:= inverse(transpose(Hv) &* inverse(Q) \+ 69 | &* Hv + inverse(Pv)):" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 49 "Xd 70 | := Xv + Pd &* transpose(Hv) &* inverse(Q) &* v:" }}}{EXCHG {PARA 0 "> \+ 71 | " 0 "" {MPLTEXT 1 0 0 "" }}}{EXCHG {PARA 0 "" 0 "" {TEXT -1 8 "Compare 72 | :" }{MPLTEXT 1 0 0 "" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 28 "Xa: 73 | = submatrix(Xa,1..3,[1]):" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 74 | 28 "Xb:= submatrix(Xb,1..3,[1]):" }}}{EXCHG {PARA 0 "> " 0 "" 75 | {MPLTEXT 1 0 18 "dX:= evalm(Xa-Xb):" }}}{EXCHG {PARA 0 "> " 0 "" 76 | {MPLTEXT 1 0 0 "" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 29 "Pa:= su 77 | bmatrix(Pa,1..3,1..3):" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 29 "P 78 | b:= submatrix(Pb,1..3,1..3):" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 79 | 0 18 "dP:= evalm(Pa-Pb):" }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 0 " 80 | " }}}{EXCHG {PARA 0 "> " 0 "" {MPLTEXT 1 0 19 "dX2:= evalm(Xb-Xc);" }} 81 | {PARA 12 "" 1 "" {XPPMATH 20 "6#>%$dX2G-%'MATRIXG6#7%7#,**&,&*(,(*&%%H 82 | v11G\"\"\"%%PvxxGF1F1*&%%Hv12GF1%%PvxyGF1F1*&%%Hv13GF1%%PvxpGF1F1F1,6* 83 | &%%Hv21G\"\"#F2F1F1*(F;F1%%Hv22GF1F5F1F<*(F;F1%%Hv23GF1F8F1F<*&F>F<%%P 84 | vyyGF1F1*(F>F1F@F1%%PvypGF1F<*&F@F<%%PvppGF1F1*&%%Hf21GF<%&Pf2xxGF1F1* 85 | (FHF1%%Hf22GF1%&Pf2xyGF1F<*&FKF<%&Pf2yyGF1F1%#r2GF1F1,b\\l*(F0FF1F7F1FDF1FHF1FYF1FIF1F<*.F>F1F7F1FDF1FH 88 | F1FTF1FLF1F<*.F>F1F7F1FDF1FKF1FYF1FLF1F<*.F>F1F7F1FDF1FKF1FTF1FNF1F<*, 89 | F@FF1F4 94 | F1FBF1F<*.F;F1F0F1F2F1F>F1F7F1FDF1F<*.F;F1F0F1F2F1F@F1F4F1FDF1F<*.F;F1 95 | F0F1F2F1F@F1F7F1FFF1F<*.F;F1F0F1F2F1FHF1FYF1FIF1F<*.F;F1F0F1F2F1FHF1FT 96 | F1FLF1F<*.F;F1F0F1F2F1FKF1FYF1FLF1F<*.F;F1F0F1F2F1FKF1FTF1FNF1F<*,F;F< 97 | F4F1F5F1F7F1F8F1F<*,F;F1F4FF1F4F1FBF1F< 100 | *,F;F1F7FF1FDF1F<*.F;F1F7F1F8F1FHF1FYF1FIF1F<*.F;F1F7F1F8F1FHF1 101 | FTF1FLF1F<*.F;F1F7F1F8F1FKF1FYF1FLF1F<*.F;F1F7F1F8F1FKF1FTF1FNF1F<*,F> 102 | FF1F0FF1F0F1F5F1F@F1F7F1FFF1 103 | F<*.F>F1F0F1F5F1FHF1FYF1FIF1F<*.F>F1F0F1F5F1FHF1FTF1FLF1F<*.F>F1F0F1F5 104 | F1FKF1FYF1FLF1F<*.F>F1F0F1F5F1FKF1FTF1FNF1F<*.F>F1F4F1FBF1F@F1F0F1F8F1 105 | F<*.F>F1F4F1FBF1F@F1F7F1FFF1F<*.F>F1F4F1FBF1FHF1FYF1FIF1F<*.F>F1F4F1FB 106 | F1FHF1FTF1FLF1F<*.F>F1F4F1FBF1FKF1FYF1FLF1F<*.F>F1F4F1FBF1FKF1FTF1FNF1 107 | F<*(FYFF1F@F1FDF1!\"#**F0FF1Fhq*.F0F1F4F1F5F1F;F1F@F1F8F1Fhq*.F0F1F4F1F5F1F>F1F@F1FDF1F 110 | hq*,F0F1F4F1F5F1F@FF1F5F1Fhq*,F0 112 | F1F7F1F8FFF1F@F1FD 113 | F1Fhq*,F0F1F7F1F8F1FHFF1F5F1Fhq*.F4F1F7F1FD 117 | F1F;F1F@F1F8F1Fhq*,F4F1F7F1FDFF1F@F1Fhq*,F4F1F7F1FDF1FHFF1F5F1Fhq**F7FFFFF1F5F1Fhq*, 124 | FYFFF1F@F1FDF1Fhq** 125 | FYFF1F5F1Fcr*.FYF1FTF1FLF1F;F1F@F1F8F1Fcr*,FYF1FTF1FLF1F>FF1F@F1FDF1Fcr*,FYF1FTF1FLF1F@FF1F5F1Fhq*,FTFFF1F@F1FDF1Fhq**FTFF1F5F1Fhq**FVF 132 | 1F;F1F@F1F8F1Fhq**FVF1F>F1F@F1FDF1Fhq**FVF1FHF1FKF1FLF1Fhq*(FVF1F>FFF1F5F1F1*&F@F1F8F1F1F1,<*(F;F1F0F1F2F1F1*(F;F1F4F1F5 135 | F1F1*(F;F1F7F1F8F1F1*(F>F1F0F1F5F1F1*(F>F1F4F1FBF1F1*(F>F1F7F1FDF1F1*( 136 | F@F1F0F1F8F1F1*(F@F1F4F1FDF1F1*(F@F1F7F1FFF1F1*(FHF1FYF1FIF1F1*(FHF1FT 137 | F1FLF1F1*(FKF1FYF1FLF1F1*(FKF1FTF1FNF1F1F1FPFRF1F1%#v1GF1F1*&,&*(F.F1F 138 | awF1FPFRF1*(F]wF1,6*&F0FF1FBF1F1*&F@F1FDF1F1F1F 149 | awF1FPFRF1F1F_xF1F1*&,&*(F`zF1FawF1FPFRF1*(FezF1FdxF1FPFRFRF1F^yF1F1*& 150 | ,&*(F`zF1FbyF1FcyFRFR*(FezF1FeyF1FcyFRF1F1F_xF1FR*&,&*(F`zF1FeyF1FcyFR 151 | F1*(FezF1FjyF1FcyFRFRF1F^yF1FR7#,**&,&*(,(*&F0F1F8F1F1*&F4F1FDF1F1*&F7 152 | F1FFF1F1F1F9F1FPFRFR*(,(*&F;F1F8F1F1*&F>F1FDF1F1*&F@F1FFF1F1F1FawF1FPF 153 | RF1F1F_xF1F1*&,&*(Fj[lF1FawF1FPFRF1*(F_\\lF1FdxF1FPFRFRF1F^yF1F1*&,&*( 154 | Fj[lF1FbyF1FcyFRFR*(F_\\lF1FeyF1FcyFRF1F1F_xF1FR*&,&*(Fj[lF1FeyF1FcyFR 155 | F1*(F_\\lF1FjyF1FcyFRFRF1F^yF1FR" }}}{EXCHG {PARA 0 "> " 0 "" 156 | {MPLTEXT 1 0 0 "" }}}}{MARK "50 0 0" 17 }{VIEWOPTS 1 1 0 1 1 1803 } 157 | -------------------------------------------------------------------------------- /matlab/fastslam2/sample_proposal.m: -------------------------------------------------------------------------------- 1 | function particle= sample_proposal(particle, z,idf, R) 2 | % Compute proposal distribution, then sample from it, and compute new 3 | % particle weight. 4 | 5 | xv= particle.xv; 6 | Pv= particle.Pv; 7 | xv0= xv; 8 | Pv0= Pv; 9 | 10 | % process each feature, incrementally refine proposal distribution 11 | for i=1:length(idf) 12 | j= idf(i); 13 | 14 | [zpi,Hvi,Hfi,Sf]= compute_jacobians(particle, j, R); 15 | Sfi= inv(Sf); 16 | vi= z(:,i)-zpi; 17 | vi(2)= pi_to_pi(vi(2)); 18 | 19 | Pv= inv(Hvi' * Sfi * Hvi + inv(Pv)); % proposal covariance 20 | xv= xv + Pv * Hvi' * Sfi * vi; % proposal mean 21 | 22 | particle.xv= xv; 23 | particle.Pv= Pv; 24 | end 25 | 26 | % sample from proposal distribution 27 | xvs= multivariate_gauss(xv,Pv,1); 28 | particle.xv= xvs; 29 | particle.Pv= zeros(3); 30 | 31 | % compute sample weight: w = w * p(z|xk) p(xk|xk-1) / proposal 32 | like = likelihood_given_xv(particle, z,idf, R); 33 | prior = gauss_evaluate(delta_xv(xv0,xvs), Pv0); 34 | prop = gauss_evaluate(delta_xv(xv, xvs), Pv); 35 | particle.w = particle.w * like * prior / prop; 36 | 37 | % 38 | % 39 | 40 | function w = likelihood_given_xv(particle, z,idf, R) 41 | % For FastSLAM, p(z|xv) requires the map part to be marginalised from p(z|xv,m) 42 | w = 1; 43 | for i=1:length(idf) 44 | [zp,Hv,Hf,Sf]= compute_jacobians(particle, idf(i), R); 45 | v= z(:,i)-zp; 46 | v(2)= pi_to_pi(v(2)); 47 | 48 | w = w * gauss_evaluate(v,Sf); 49 | end 50 | 51 | % 52 | % 53 | 54 | function dx = delta_xv(xv1, xv2) 55 | % Compute innovation between two xv estimates, normalising the heading component 56 | dx = xv1 - xv2; 57 | dx(3,:) = pi_to_pi(dx(3,:)); 58 | -------------------------------------------------------------------------------- /matlab/fastslam2r/compute_weightr.m: -------------------------------------------------------------------------------- 1 | function w= compute_weight(particle, z, idf, R) 2 | 3 | [zp,Hv,Hf,Sf]= compute_jacobians(particle, idf, R); 4 | Pv= particle.Pv; 5 | 6 | v= z-zp; 7 | v(2,:)= pi_to_pi(v(2,:)); 8 | 9 | % combine jacobians into joint 10 | N = size(z,2)*2; 11 | vj = reshape(v,N,1); 12 | Hvj = zeros(N,3); 13 | Sfj = zeros(N); 14 | 15 | for i=1:size(z,2) 16 | ii = 2*i + (-1:0); 17 | Hvj(ii,:) = Hv(:,:,i); 18 | Sfj(ii,ii) = Sf(:,:,i); 19 | end 20 | 21 | % evaluate likelihood of joint 22 | S= Hvj*Pv*Hvj' + Sfj; % innovation covariance, includes pose and feature uncertainty 23 | den= sqrt((2*pi)^N * det(S)); 24 | num= exp(-0.5 * vj' * inv(S) * vj); 25 | w = num/den; 26 | -------------------------------------------------------------------------------- /matlab/fastslam2r/fastslam2r_sim.m: -------------------------------------------------------------------------------- 1 | function data= fastslam2_sim(lm, wp) 2 | %function data= fastslam2_sim(lm, wp) 3 | % 4 | % INPUTS: 5 | % lm - set of landmarks 6 | % wp - set of waypoints 7 | % 8 | % OUTPUTS: 9 | % data - set of particles representing final state 10 | % 11 | % NOTES: 12 | % This program is a FastSLAM 2.0 simulator. To use, create a set of landmarks and 13 | % vehicle waypoints (ie, waypoints for the desired vehicle path). The program 14 | % 'frontend.m' may be used to create this simulated environment - type 15 | % 'help frontend' for more information. 16 | % The configuration of the simulator is managed by the script file 17 | % 'configfile.m'. To alter the parameters of the vehicle, sensors, etc 18 | % adjust this file. There are also several switches that control certain 19 | % filter options. 20 | % 21 | % Tim Bailey and Juan Nieto 2004. 22 | % Version 1.0 23 | 24 | format compact 25 | path(path, '../') 26 | configfile; 27 | 28 | if SWITCH_PREDICT_NOISE, warning('Sampling from predict noise usually OFF for FastSLAM 2.0'), end 29 | if SWITCH_SAMPLE_PROPOSAL==0, warning('Sampling from optimal proposal is usually ON for FastSLAM 2.0'), end 30 | 31 | h= setup_animations(lm,wp); 32 | veh= [0 -WHEELBASE -WHEELBASE; 0 -1 1]; 33 | 34 | % initialisations 35 | particles= initialise_particles(NPARTICLES); 36 | xtrue= zeros(3,1); 37 | 38 | dt= DT_CONTROLS; % change in time between predicts 39 | dtsum= 0; % change in time since last observation 40 | ftag= 1:size(lm,2); % identifier for each landmark 41 | da_table= zeros(1,size(lm,2)); % data association table 42 | iwp= 1; % index to first waypoint 43 | G= 0; % initial steer angle 44 | plines=[]; 45 | if SWITCH_SEED_RANDOM ~= 0, rand('state',SWITCH_SEED_RANDOM), randn('state',SWITCH_SEED_RANDOM), end 46 | 47 | Qe= Q; Re= R; 48 | if SWITCH_INFLATE_NOISE==1, Qe= 2*Q; Re= 2*R; end 49 | 50 | if SWITCH_PROFILE, profile on -detail builtin, end 51 | 52 | % main loop 53 | while iwp ~= 0 54 | 55 | % compute true data 56 | [G,iwp]= compute_steering(xtrue, wp, iwp, AT_WAYPOINT, G, RATEG, MAXG, dt); 57 | if iwp==0 & NUMBER_LOOPS > 1, iwp=1; NUMBER_LOOPS= NUMBER_LOOPS-1; end 58 | xtrue= predict_true(xtrue, V,G, WHEELBASE,dt); 59 | 60 | % add process noise 61 | [Vn,Gn]= add_control_noise(V,G,Q, SWITCH_CONTROL_NOISE); 62 | 63 | % Predict step 64 | for i=1:NPARTICLES 65 | particles(i)= predict (particles(i), Vn,Gn,Qe, WHEELBASE,dt, SWITCH_PREDICT_NOISE); 66 | particles(i)= observe_heading(particles(i), xtrue(3), SWITCH_HEADING_KNOWN); % if heading known, observe heading 67 | end 68 | 69 | % Observe step 70 | dtsum= dtsum + dt; 71 | if dtsum >= DT_OBSERVE 72 | dtsum= 0; 73 | 74 | % Compute true data, then add noise 75 | [z,ftag_visible]= get_observations(xtrue, lm, ftag, MAX_RANGE); 76 | z= add_observation_noise(z,R, SWITCH_SENSOR_NOISE); 77 | if ~isempty(z), plines= make_laser_lines (z,xtrue); end 78 | 79 | % Compute (known) data associations 80 | Nf= size(particles(1).xf,2); 81 | [zf,idf,zn,da_table]= data_associate_known(z, ftag_visible, da_table, Nf); 82 | 83 | % Observe map features 84 | if ~isempty(zf) 85 | 86 | % compute weights w = w * p(z_k | x_k-1) 87 | for i=1:NPARTICLES 88 | w= compute_weightr(particles(i), zf,idf, Re); 89 | particles(i).w= particles(i).w * w; 90 | end 91 | 92 | % resampling *before* computing proposal permits better particle diversity 93 | particles= resample_particles(particles, NEFFECTIVE, SWITCH_RESAMPLE); 94 | 95 | % sample from "optimal" proposal distribution, then update map 96 | for i=1:NPARTICLES 97 | particles(i)= sample_proposal(particles(i), zf,idf, Re, SWITCH_SAMPLE_PROPOSAL); 98 | particles(i)= feature_update(particles(i), zf, idf, Re); 99 | end 100 | end 101 | 102 | % Observe new features, augment map 103 | if ~isempty(zn) 104 | for i=1:NPARTICLES 105 | if isempty(zf) % sample from proposal distribution (if we have not already done so above) 106 | particles(i).xv= multivariate_gauss(particles(i).xv, particles(i).Pv, 1); 107 | particles(i).Pv= zeros(3); 108 | end 109 | particles(i)= add_feature(particles(i), zn,Re); 110 | end 111 | end 112 | 113 | end 114 | 115 | % plots 116 | do_plot(h, particles, xtrue, plines, veh) 117 | end 118 | 119 | if SWITCH_PROFILE, profile report, end 120 | 121 | data= particles; 122 | 123 | % 124 | % 125 | 126 | function p= make_laser_lines (rb,xv) 127 | if isempty(rb), p=[]; return, end 128 | len= size(rb,2); 129 | lnes(1,:)= zeros(1,len)+ xv(1); 130 | lnes(2,:)= zeros(1,len)+ xv(2); 131 | lnes(3:4,:)= transformtoglobal([rb(1,:).*cos(rb(2,:)); rb(1,:).*sin(rb(2,:))], xv); 132 | p= line_plot_conversion (lnes); 133 | 134 | function p= initialise_particles(np) 135 | for i=1:np 136 | p(i).w= 1/np; 137 | p(i).xv= [0;0;0]; 138 | p(i).Pv= zeros(3); 139 | p(i).xf= []; 140 | p(i).Pf= []; 141 | p(i).da= []; 142 | end 143 | 144 | function p= make_covariance_ellipses(particle) 145 | N= 10; 146 | inc= 2*pi/N; 147 | phi= 0:inc:2*pi; 148 | circ= 2*[cos(phi); sin(phi)]; 149 | 150 | p= make_ellipse(particle.xv(1:2), particle.Pv(1:2,1:2) + eye(2)*eps, circ); 151 | 152 | lenf= size(particle.xf,2); 153 | if lenf > 0 154 | 155 | xf= particle.xf; 156 | Pf= particle.Pf; 157 | p= [p zeros(2, lenf*(N+2))]; 158 | 159 | ctr= N+3; 160 | for i=1:lenf 161 | ii= ctr:(ctr+N+1); 162 | p(:,ii)= make_ellipse(xf(:,i), Pf(:,:,i), circ); 163 | ctr= ctr+N+2; 164 | end 165 | end 166 | 167 | function p= make_ellipse(x,P,circ) 168 | % make a single 2-D ellipse 169 | r= sqrtm_2by2(P); 170 | a= r*circ; 171 | p(2,:)= [a(2,:)+x(2) NaN]; 172 | p(1,:)= [a(1,:)+x(1) NaN]; 173 | 174 | % 175 | % 176 | 177 | function h= setup_animations(lm,wp) 178 | figure 179 | plot(lm(1,:),lm(2,:),'g*') 180 | hold on, axis equal 181 | plot(wp(1,:),wp(2,:), wp(1,:),wp(2,:),'ro') 182 | 183 | h.xt= patch(0,0,'g','erasemode','xor'); % vehicle true 184 | h.xm= patch(0,0,'r','erasemode','xor'); % mean vehicle estimate 185 | h.obs= plot(0,0,'y','erasemode','xor'); % observations 186 | h.xfp= plot(0,0,'r.','erasemode','background'); % estimated features (particle means) 187 | h.xvp= plot(0,0,'r.','erasemode','xor'); % estimated vehicle (particles) 188 | h.cov= plot(0,0,'erasemode','xor'); % covariances of max weight particle 189 | 190 | 191 | function do_plot(h, particles, xtrue, plines, veh) 192 | 193 | xvp = [particles.xv]; 194 | xfp = [particles.xf]; 195 | w = [particles.w]; 196 | 197 | ii= find(w== max(w)); 198 | xvmax= xvp(:,ii); 199 | 200 | xt= transformtoglobal(veh,xtrue); 201 | xm= transformtoglobal(veh,xvmax); 202 | set(h.xt, 'xdata', xt(1,:), 'ydata', xt(2,:)) 203 | set(h.xm, 'xdata', xm(1,:), 'ydata', xm(2,:)) 204 | set(h.xvp, 'xdata', xvp(1,:), 'ydata', xvp(2,:)) 205 | if ~isempty(xfp), set(h.xfp, 'xdata', xfp(1,:), 'ydata', xfp(2,:)), end 206 | if ~isempty(plines), set(h.obs, 'xdata', plines(1,:), 'ydata', plines(2,:)), end 207 | pcov= make_covariance_ellipses(particles(ii(1))); 208 | if ~isempty(pcov), set(h.cov, 'xdata', pcov(1,:), 'ydata', pcov(2,:)); end 209 | 210 | drawnow 211 | -------------------------------------------------------------------------------- /matlab/fastslam2r/observe_heading.m: -------------------------------------------------------------------------------- 1 | function particle= observe_heading(particle, phi, useheading) 2 | 3 | if useheading==0, return, end 4 | sigmaPhi= 0.01*pi/180; % radians, heading uncertainty 5 | 6 | xv= particle.xv; 7 | Pv= particle.Pv; 8 | 9 | H= [0 0 1]; 10 | v= pi_to_pi(phi - xv(3)); 11 | 12 | [xv, Pv]= KF_joseph_update(xv, Pv, v, sigmaPhi^2,H); 13 | particle.xv= xv; 14 | particle.Pv= Pv; 15 | -------------------------------------------------------------------------------- /matlab/fastslam2r/predict.m: -------------------------------------------------------------------------------- 1 | function particle= predict(particle, V,G,Q, WB,dt, addrandom) 2 | % 3 | % INPUTS: 4 | % xv - vehicle pose sample 5 | % Pv - vehicle pose predict covariance 6 | % 7 | % Note: Pv must be zeroed after each observation. It accumulates the 8 | % vehicle pose uncertainty between measurements. 9 | 10 | xv= particle.xv; 11 | Pv= particle.Pv; 12 | 13 | % Jacobians 14 | phi= xv(3); 15 | Gv= [1 0 -V*dt*sin(G+phi); 16 | 0 1 V*dt*cos(G+phi); 17 | 0 0 1]; 18 | Gu= [dt*cos(G+phi) -V*dt*sin(G+phi); 19 | dt*sin(G+phi) V*dt*cos(G+phi); 20 | dt*sin(G)/WB V*dt*cos(G)/WB]; 21 | 22 | % predict covariance 23 | particle.Pv= Gv*Pv*Gv' + Gu*Q*Gu'; 24 | 25 | % optional: add random noise to predicted state 26 | if addrandom == 1 27 | VG= multivariate_gauss([V;G], Q, 1); 28 | V= VG(1); G= VG(2); 29 | end 30 | 31 | % predict state 32 | particle.xv= [xv(1) + V*dt*cos(G+xv(3,:)); 33 | xv(2) + V*dt*sin(G+xv(3,:)); 34 | pi_to_pi(xv(3) + V*dt*sin(G)/WB)]; 35 | 36 | % 37 | % 38 | 39 | function x= pi_to_pi(x) 40 | if x > pi 41 | x= x - 2*pi; 42 | elseif x < -pi 43 | x= x + 2*pi; 44 | end 45 | -------------------------------------------------------------------------------- /matlab/fastslam2r/readme.txt: -------------------------------------------------------------------------------- 1 | 2 | fastslam2r_sim is a slight variation on the normal FastSLAM 2 algorithm. It incorporates "reverse resampling" by computing sample weights and resampling *before* the proposal distribution is obtained. 3 | -------------------------------------------------------------------------------- /matlab/fastslam2r/sample_proposal.m: -------------------------------------------------------------------------------- 1 | function particle= sample_proposal(particle, z,idf, R, sampleproposal) 2 | % Compute proposal distribution and then sample from it 3 | 4 | xv= particle.xv; 5 | Pv= particle.Pv; 6 | 7 | % process each feature, incrementally refine proposal distribution 8 | for i=1:length(idf) 9 | j= idf(i); 10 | 11 | [zpi,Hvi,Hfi,Qfi]= compute_jacobians(particle, j, R); 12 | Qfi= inv(Qfi); % only need inverse below 13 | vi= z(:,i)-zpi; vi(2)= pi_to_pi(vi(2)); 14 | 15 | Pv= inv(Hvi' * Qfi * Hvi + inv(Pv)); % proposal covariance 16 | xv= xv + Pv * Hvi' * Qfi * vi; % proposal mean 17 | 18 | particle.xv= xv; 19 | particle.Pv= Pv; 20 | end 21 | 22 | % sample from proposal distribution 23 | if sampleproposal==1, particle.xv= multivariate_gauss(xv,Pv,1); end 24 | particle.Pv= zeros(3); 25 | -------------------------------------------------------------------------------- /matlab/feature_update.m: -------------------------------------------------------------------------------- 1 | function particle= feature_update(particle, z, idf, R) 2 | %function particle= feature_update(particle, z, idf, R) 3 | % 4 | % Having selected a new pose from the proposal distribution, this pose is assumed 5 | % perfect and each feature update may be computed independently and without pose uncertainty. 6 | 7 | xf= particle.xf(:,idf); 8 | Pf= particle.Pf(:,:,idf); 9 | 10 | [zp,Hv,Hf,Sf]= compute_jacobians(particle, idf, R); 11 | v= z-zp; v(2,:)= pi_to_pi(v(2,:)); 12 | 13 | for i=1:length(idf) 14 | vi= v(:,i); 15 | Hfi= Hf(:,:,i); 16 | Pfi= Pf(:,:,i); 17 | xfi= xf(:,i); 18 | 19 | [xf(:,i), Pf(:,:,i)]= KF_cholesky_update(xfi,Pfi, vi,R,Hfi); 20 | end 21 | 22 | particle.xf(:,idf)= xf; 23 | particle.Pf(:,:,idf)= Pf; 24 | -------------------------------------------------------------------------------- /matlab/frontend.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yglee/FastSLAM/fa3b24d789c18dad56f5c5bbb99f69f8c0ef6bdb/matlab/frontend.fig -------------------------------------------------------------------------------- /matlab/frontend.m: -------------------------------------------------------------------------------- 1 | function varargout = frontend(varargin) 2 | %EKF-SLAM environment-making GUI 3 | % 4 | % This program permits the graphical creation and manipulation 5 | % of an environment of point landmarks, and the specification of 6 | % vehicle path waypoints therein. 7 | % 8 | % USAGE: type 'frontend' to start. 9 | % 1. Click on the desired operation: , , or . 10 | % 2. Click on the type: or to commence the 11 | % operation. 12 | % 3. If entering new landmarks or waypoints, click with the left 13 | % mouse button to add new points. Click the right mouse button, or 14 | % hit key to finish. 15 | % 4. To move or delete a point, just click near the desired point. 16 | % 5. Saving maps and loading previous maps is accomplished via the 17 | % and buttons, respectively. 18 | % 19 | % Tim Bailey and Juan Nieto 2004. 20 | 21 | % FRONTEND Application M-file for frontend.fig 22 | % FIG = FRONTEND launch frontend GUI. 23 | % FRONTEND('callback_name', ...) invoke the named callback. 24 | global WAYPOINTS LANDMARKS FH 25 | 26 | if nargin == 0 % LAUNCH GUI 27 | 28 | %initialisation 29 | WAYPOINTS= [0;0]; 30 | LANDMARKS= []; 31 | 32 | % open figure 33 | fig = openfig(mfilename,'reuse'); 34 | hh= get(fig, 'children'); 35 | set(hh(3), 'value', 1) 36 | 37 | hold on 38 | FH.hl= plot(0,0,'g*'); plot(0,0,'w*') 39 | FH.hw= plot(0,0,0,0,'ro'); 40 | plotwaypoints(WAYPOINTS); 41 | 42 | % Use system color scheme for figure: 43 | set(fig,'Color',get(0,'defaultUicontrolBackgroundColor')); 44 | set(fig,'name', 'SLAM Map-Making GUI') 45 | 46 | % Generate a structure of handles to pass to callbacks, and store it. 47 | handles = guihandles(fig); 48 | guidata(fig, handles); 49 | 50 | if nargout > 0 51 | varargout{1} = fig; 52 | end 53 | 54 | elseif ischar(varargin{1}) % INVOKE NAMED SUBFUNCTION OR CALLBACK 55 | 56 | try 57 | [varargout{1:nargout}] = feval(varargin{:}); % FEVAL switchyard 58 | catch 59 | disp(lasterr); 60 | end 61 | 62 | end 63 | 64 | 65 | % -------------------------------------------------------------------- 66 | function varargout = waypoint_checkbox_Callback(h, eventdata, handles, varargin) 67 | global WAYPOINTS 68 | set(handles.landmark_checkbox, 'value', 0) 69 | WAYPOINTS= perform_task(WAYPOINTS, handles.waypoint_checkbox, handles); 70 | plotwaypoints(WAYPOINTS); 71 | 72 | % -------------------------------------------------------------------- 73 | function varargout = landmark_checkbox_Callback(h, eventdata, handles, varargin) 74 | global LANDMARKS 75 | set(handles.waypoint_checkbox, 'value', 0) 76 | LANDMARKS= perform_task(LANDMARKS, handles.landmark_checkbox, handles); 77 | plotlandmarks(LANDMARKS); 78 | 79 | % -------------------------------------------------------------------- 80 | function varargout = enter_checkbox_Callback(h, eventdata, handles, varargin) 81 | set(handles.enter_checkbox, 'value', 1) 82 | set(handles.move_checkbox, 'value', 0) 83 | set(handles.delete_checkbox, 'value', 0) 84 | 85 | % -------------------------------------------------------------------- 86 | function varargout = move_checkbox_Callback(h, eventdata, handles, varargin) 87 | set(handles.enter_checkbox, 'value', 0) 88 | set(handles.move_checkbox, 'value', 1) 89 | set(handles.delete_checkbox, 'value', 0) 90 | 91 | % -------------------------------------------------------------------- 92 | function varargout = delete_checkbox_Callback(h, eventdata, handles, varargin) 93 | set(handles.enter_checkbox, 'value', 0) 94 | set(handles.move_checkbox, 'value', 0) 95 | set(handles.delete_checkbox, 'value', 1) 96 | 97 | % -------------------------------------------------------------------- 98 | function varargout = load_button_Callback(h, eventdata, handles, varargin) 99 | global WAYPOINTS LANDMARKS 100 | seed = {'*.mat','MAT-files (*.mat)'}; 101 | [fn,pn] = uigetfile(seed, 'Load landmarks and waypoints'); 102 | if fn==0, return, end 103 | 104 | fnpn = strrep(fullfile(pn,fn), '''', ''''''); 105 | load(fnpn) 106 | WAYPOINTS= wp; LANDMARKS= lm; 107 | plotwaypoints(WAYPOINTS); 108 | plotlandmarks(LANDMARKS); 109 | 110 | % -------------------------------------------------------------------- 111 | function varargout = save_button_Callback(h, eventdata, handles, varargin) 112 | global WAYPOINTS LANDMARKS 113 | wp= WAYPOINTS; lm= LANDMARKS; 114 | seed = {'*.mat','MAT-files (*.mat)'}; 115 | [fn,pn] = uiputfile(seed, 'Save landmarks and waypoints'); 116 | if fn==0, return, end 117 | 118 | fnpn = strrep(fullfile(pn,fn), '''', ''''''); 119 | save(fnpn, 'wp', 'lm'); 120 | 121 | % -------------------------------------------------------------------- 122 | function plotwaypoints(x) 123 | global FH 124 | set(FH.hw(1), 'xdata', x(1,:), 'ydata', x(2,:)) 125 | set(FH.hw(2), 'xdata', x(1,:), 'ydata', x(2,:)) 126 | 127 | % -------------------------------------------------------------------- 128 | function plotlandmarks(x) 129 | global FH 130 | set(FH.hl, 'xdata', x(1,:), 'ydata', x(2,:)) 131 | 132 | % -------------------------------------------------------------------- 133 | function i= find_nearest(x) 134 | xp= ginput(1); 135 | d2= (x(1,:)-xp(1)).^2 + (x(2,:)-xp(2)).^2; 136 | i= find(d2 == min(d2)); 137 | i= i(1); 138 | 139 | % -------------------------------------------------------------------- 140 | function x= perform_task(x, h, handles) 141 | 142 | if get(h, 'value') == 1 143 | zoom off 144 | 145 | if get(handles.enter_checkbox, 'value') == 1 % enter points 146 | [xn,yn,bn]= ginput(1); 147 | while ~isempty(xn) & bn == 1 148 | x= [x [xn;yn]]; 149 | if h == handles.waypoint_checkbox 150 | plotwaypoints(x); 151 | else 152 | plotlandmarks(x); 153 | end 154 | [xn,yn,bn]= ginput(1); 155 | end 156 | else 157 | i= find_nearest(x); 158 | if get(handles.delete_checkbox, 'value') == 1 % delete nearest point 159 | x= [x(:,1:i-1) x(:,i+1:end)]; 160 | 161 | elseif get(handles.move_checkbox, 'value') == 1 % move nearest point 162 | xt= x(:,i); 163 | plot(xt(1), xt(2),'kx', 'markersize',10) 164 | x(:,i)= ginput(1)'; 165 | plot(xt(1), xt(2),'wx', 'markersize',10) 166 | end 167 | end 168 | 169 | set(h, 'value', 0) 170 | end 171 | -------------------------------------------------------------------------------- /matlab/get_observations.m: -------------------------------------------------------------------------------- 1 | function [z,idf]= get_observations(x, lm, idf, rmax) 2 | 3 | [lm,idf]= get_visible_landmarks(x,lm,idf,rmax); 4 | z= compute_range_bearing(x,lm); 5 | 6 | % 7 | % 8 | 9 | function [lm,idf]= get_visible_landmarks(x,lm,idf,rmax) 10 | % Select set of landmarks that are visible within vehicle's semi-circular field-of-view 11 | dx= lm(1,:) - x(1); 12 | dy= lm(2,:) - x(2); 13 | phi= x(3); 14 | 15 | % incremental tests for bounding semi-circle 16 | ii= find(abs(dx) < rmax & abs(dy) < rmax ... % bounding box 17 | & (dx*cos(phi) + dy*sin(phi)) > 0 ... % bounding line 18 | & (dx.^2 + dy.^2) < rmax^2); % bounding circle 19 | % Note: the bounding box test is unnecessary but illustrates a possible speedup technique 20 | % as it quickly eliminates distant points. Ordering the landmark set would make this operation 21 | % O(logN) rather that O(N). 22 | 23 | lm= lm(:,ii); 24 | idf= idf(ii); 25 | 26 | % 27 | % 28 | 29 | function z= compute_range_bearing(x,lm) 30 | % Compute exact observation 31 | dx= lm(1,:) - x(1); 32 | dy= lm(2,:) - x(2); 33 | phi= x(3); 34 | z= [sqrt(dx.^2 + dy.^2); 35 | atan2(dy,dx) - phi]; 36 | 37 | -------------------------------------------------------------------------------- /matlab/line_plot_conversion.m: -------------------------------------------------------------------------------- 1 | function p= line_plot_conversion (lne) 2 | %function p= line_plot_conversion (lne) 3 | % 4 | % INPUT: list of lines [x1;y1;x2;y2] 5 | % OUTPUT: list of points [x;y] 6 | % 7 | % Convert a list of lines so that they will be plotted as a set of 8 | % unconnected lines but only require a single handle to do so. This 9 | % is performed by converting the lines to a set of points, where a 10 | % NaN point is inserted between every point-pair: 11 | % 12 | % l= [x1a x1b x1c; 13 | % y1a y1b y1c; 14 | % x2a x2b x2c; 15 | % y2a y2b y2c]; 16 | % 17 | % becomes 18 | % 19 | % p= [x1a x2a NaN x1b x2b NaN x1c x2c; 20 | % y1a y2a NaN y1b y2b NaN y1c y2c]; 21 | % 22 | % Tim Bailey 2002. Thanks to Jose Guivant for this 'discrete lines' 23 | % plotting technique. 24 | 25 | len= size(lne,2)*3 - 1; 26 | p= zeros(2, len); 27 | 28 | p(:,1:3:end)= lne(1:2,:); 29 | p(:,2:3:end)= lne(3:4,:); 30 | p(:,3:3:end)= NaN; 31 | -------------------------------------------------------------------------------- /matlab/multivariate_gauss.m: -------------------------------------------------------------------------------- 1 | function s= multivariate_gauss(x,P,n) 2 | %function s= multivariate_gauss(x,P,n) 3 | % 4 | % INPUTS: 5 | % (x, P) mean vector and covariance matrix 6 | % obtain n samples 7 | % OUTPUT: 8 | % sample set 9 | % 10 | % Random sample from multivariate Gaussian distribution. 11 | % Adapted from MVNORMRND (c) 1998, Harvard University. 12 | % 13 | % Tim Bailey 2004. 14 | 15 | len= length(x); 16 | S= chol(P)'; 17 | X = randn(len,n); 18 | s = S*X + x*ones(1,n); 19 | -------------------------------------------------------------------------------- /matlab/pi_to_pi.m: -------------------------------------------------------------------------------- 1 | function angle = pi_to_pi(angle) 2 | 3 | %function angle = pi_to_pi(angle) 4 | % Input: array of angles. 5 | % Tim Bailey 2000 6 | 7 | %angle = mod(angle, 2*pi); 8 | % mod() not necessary as angles are (nearly) always bounded by -2*pi <= angle <= 2*pi 9 | % and, also, the mod() function is very slow. [Tim Bailey 2004] 10 | i= find(angle<-2*pi | angle>2*pi); % replace with a check 11 | if ~isempty(i) 12 | % warning('pi_to_pi() error: angle outside 2-PI bounds.') 13 | angle(i) = mod(angle(i), 2*pi); 14 | end 15 | 16 | i= find(angle>pi); 17 | angle(i)= angle(i)-2*pi; 18 | 19 | i= find(angle<-pi); 20 | angle(i)= angle(i)+2*pi; 21 | -------------------------------------------------------------------------------- /matlab/predict_true.m: -------------------------------------------------------------------------------- 1 | function xv= predict_true(xv, V,G, WB,dt) 2 | % 3 | % INPUTS: 4 | 5 | xv= [xv(1) + V*dt*cos(G+xv(3,:)); 6 | xv(2) + V*dt*sin(G+xv(3,:)); 7 | pi_to_pi(xv(3) + V*dt*sin(G)/WB)]; 8 | -------------------------------------------------------------------------------- /matlab/readme.txt: -------------------------------------------------------------------------------- 1 | FastSLAM Simulator (version 1.0) 2 | ------------------ 3 | 4 | This simulator demonstrates simple implementations of FastSLAM versions 1.0 and 2.0. 5 | It permits simple configuration via 'configfile.m' to perform SLAM with various 6 | control parameters, noises, etc. The simulator assumes known data association. 7 | 8 | The key files in this simulator are called 'fastslam1_sim.m' and 'fastslam2_sim.m' 9 | for FastSLAM 1.0 and 2.0, respectively. These files are in the two subdirectories 10 | in this collection. 11 | 12 | The runtime of this simulator is very slow due, in part, to the graphics of the 13 | simulator animations, and also to the use of 'for'-loops. Loops in Matlab are very 14 | inefficient, but it is difficult to avoid them for this task without making the 15 | code very obscure. Hopefully, however, the basic algorithms are clear enough to 16 | assist development in a more suitable language. 17 | 18 | Tim Bailey and Juan Nieto 19 | 2004. 20 | 21 | 22 | -------------------------------------------------------------------------------- /matlab/resample_particles.m: -------------------------------------------------------------------------------- 1 | function particles= resample_particles(particles, Nmin, doresample) 2 | %function particles= resample_particles(particles, Nmin, doresample) 3 | % 4 | % Resample particles if their weight variance is such that N-effective 5 | % is less than Nmin. 6 | % 7 | 8 | N= length(particles); 9 | w= zeros(1,N); 10 | for i=1:N, w(i)= particles(i).w; end 11 | ws= sum(w); w= w/ws; 12 | for i=1:N, particles(i).w= particles(i).w / ws; end 13 | 14 | [keep, Neff] = stratified_resample(w); 15 | if Neff < Nmin & doresample==1 16 | particles= particles(keep); 17 | for i=1:N, particles(i).w= 1/N; end 18 | end 19 | -------------------------------------------------------------------------------- /matlab/sqrtm_2by2.m: -------------------------------------------------------------------------------- 1 | function X = sqrtm_2by2(A) 2 | %SQRTM Matrix square root. 3 | % X = SQRTM_2by2(A) is the principal square root of the matrix A, i.e. X*X = A. 4 | % 5 | % X is the unique square root for which every eigenvalue has nonnegative 6 | % real part. If A has any eigenvalues with negative real parts then a 7 | % complex result is produced. If A is singular then A may not have a 8 | % square root. 9 | % 10 | % Adapted for speed for 2x2 matrices from the MathWorks sqrtm.m implementation. 11 | % Tim Bailey 2004. 12 | 13 | [Q, T] = schur(A); % T is real/complex according to A. 14 | %[Q, T] = rsf2csf(Q, T); % T is now complex Schur form. 15 | 16 | R = zeros(2); 17 | 18 | R(1,1) = sqrt(T(1,1)); 19 | R(2,2) = sqrt(T(2,2)); 20 | R(1,2) = T(1,2) / (R(1,1) + R(2,2)); 21 | 22 | X = Q*R*Q'; 23 | -------------------------------------------------------------------------------- /matlab/stratified_random.m: -------------------------------------------------------------------------------- 1 | function s = stratified_random(N) 2 | %function s = stratified_random(N) 3 | % 4 | % Generate N uniform-random numbers stratified within interval (0,1). 5 | % The set of samples, s, are in ascending order. 6 | % 7 | % Tim Bailey 2003. 8 | 9 | k = 1/N; 10 | di = (k/2):k:(1-k/2); % deterministic intervals 11 | s = di + rand(1,N) * k - (k/2); % dither within interval 12 | -------------------------------------------------------------------------------- /matlab/stratified_resample.m: -------------------------------------------------------------------------------- 1 | function [keep, Neff] = stratified_resample(w) 2 | %function [keep, Neff] = stratified_resample(w) 3 | % 4 | % INPUT: 5 | % w - set of N weights [w1, w2, ..] 6 | % 7 | % OUTPUTS: 8 | % keep - N indices of particles to keep 9 | % Neff - number of effective particles (measure of weight variance) 10 | % 11 | % Tim Bailey 2004. 12 | 13 | 14 | w= w / sum(w); % normalise 15 | Neff= 1 / sum(w .^ 2); 16 | 17 | len= length(w); 18 | keep= zeros(1,len); 19 | select = stratified_random(len); 20 | w= cumsum(w); 21 | 22 | ctr=1; 23 | for i=1:len 24 | while ctr<=len & select(ctr) 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace std; 12 | 13 | //////////////////////////////////////////////////////////////////////////////// 14 | 15 | struct ErrorMsg 16 | { 17 | cl_int code; 18 | const char *msg; 19 | }; 20 | 21 | ErrorMsg openCL_error_msgs[] = { 22 | {CL_SUCCESS, "CL_SUCCESS"}, 23 | {CL_DEVICE_NOT_FOUND, "CL_DEVICE_NOT_FOUND"}, 24 | {CL_DEVICE_NOT_AVAILABLE, "CL_DEVICE_NOT_AVAILABLE"}, 25 | {CL_COMPILER_NOT_AVAILABLE, "CL_COMPILER_NOT_AVAILABLE"}, 26 | {CL_MEM_OBJECT_ALLOCATION_FAILURE, "CL_MEM_OBJECT_ALLOCATION_FAILURE"}, 27 | {CL_OUT_OF_RESOURCES, "CL_OUT_OF_RESOURCES"}, 28 | {CL_OUT_OF_HOST_MEMORY, "CL_OUT_OF_HOST_MEMORY"}, 29 | {CL_PROFILING_INFO_NOT_AVAILABLE, "CL_PROFILING_INFO_NOT_AVAILABLE"}, 30 | {CL_MEM_COPY_OVERLAP, "CL_MEM_COPY_OVERLAP"}, 31 | {CL_IMAGE_FORMAT_MISMATCH, "CL_IMAGE_FORMAT_MISMATCH"}, 32 | {CL_IMAGE_FORMAT_NOT_SUPPORTED, "CL_IMAGE_FORMAT_NOT_SUPPORTED"}, 33 | {CL_BUILD_PROGRAM_FAILURE, "CL_BUILD_PROGRAM_FAILURE"}, 34 | {CL_MAP_FAILURE, "CL_MAP_FAILURE"}, 35 | {CL_MISALIGNED_SUB_BUFFER_OFFSET, "CL_MISALIGNED_SUB_BUFFER_OFFSET"}, 36 | {CL_EXEC_STATUS_ERROR_FOR_EVENTS_IN_WAIT_LIST, 37 | "CL_EXEC_STATUS_ERROR_FOR_EVENTS_IN_WAIT_LIST"}, 38 | 39 | {CL_INVALID_VALUE, "CL_INVALID_VALUE"}, 40 | {CL_INVALID_DEVICE_TYPE, "CL_INVALID_DEVICE_TYPE"}, 41 | {CL_INVALID_PLATFORM, "CL_INVALID_PLATFORM"}, 42 | {CL_INVALID_DEVICE, "CL_INVALID_DEVICE"}, 43 | {CL_INVALID_CONTEXT, "CL_INVALID_CONTEXT"}, 44 | {CL_INVALID_QUEUE_PROPERTIES, "CL_INVALID_QUEUE_PROPERTIES"}, 45 | {CL_INVALID_COMMAND_QUEUE, "CL_INVALID_COMMAND_QUEUE"}, 46 | {CL_INVALID_HOST_PTR, "CL_INVALID_HOST_PTR"}, 47 | {CL_INVALID_MEM_OBJECT, "CL_INVALID_MEM_OBJECT"}, 48 | {CL_INVALID_IMAGE_FORMAT_DESCRIPTOR, "CL_INVALID_IMAGE_FORMAT_DESCRIPTOR"}, 49 | {CL_INVALID_IMAGE_SIZE, "CL_INVALID_IMAGE_SIZE"}, 50 | {CL_INVALID_SAMPLER, "CL_INVALID_SAMPLER"}, 51 | {CL_INVALID_BINARY, "CL_INVALID_BINARY"}, 52 | {CL_INVALID_BUILD_OPTIONS, "CL_INVALID_BUILD_OPTIONS"}, 53 | {CL_INVALID_PROGRAM, "CL_INVALID_PROGRAM"}, 54 | {CL_INVALID_PROGRAM_EXECUTABLE, "CL_INVALID_PROGRAM_EXECUTABLE"}, 55 | {CL_INVALID_KERNEL_NAME, "CL_INVALID_KERNEL_NAME"}, 56 | {CL_INVALID_KERNEL_DEFINITION, "CL_INVALID_KERNEL_DEFINITION"}, 57 | {CL_INVALID_KERNEL, "CL_INVALID_KERNEL"}, 58 | {CL_INVALID_ARG_INDEX, "CL_INVALID_ARG_INDEX"}, 59 | {CL_INVALID_ARG_VALUE, "CL_INVALID_ARG_VALU"}, 60 | {CL_INVALID_ARG_SIZE, "CL_INVALID_ARG_SIZE"}, 61 | {CL_INVALID_KERNEL_ARGS, "CL_INVALID_KERNEL_ARGS"}, 62 | {CL_INVALID_WORK_DIMENSION, "CL_INVALID_WORK_DIMENSION"}, 63 | {CL_INVALID_WORK_GROUP_SIZE, "CL_INVALID_WORK_GROUP_SIZE"}, 64 | {CL_INVALID_WORK_ITEM_SIZE, "CL_INVALID_WORK_ITEM_SIZE"}, 65 | {CL_INVALID_GLOBAL_OFFSET, "CL_INVALID_GLOBAL_OFFSET"}, 66 | {CL_INVALID_EVENT_WAIT_LIST, "CL_INVALID_EVENT_WAIT_LIST"}, 67 | {CL_INVALID_EVENT, "CL_INVALID_EVENT"}, 68 | {CL_INVALID_OPERATION, "CL_INVALID_OPERATION"}, 69 | {CL_INVALID_BUFFER_SIZE, "CL_INVALID_BUFFER_SIZE"}, 70 | {CL_INVALID_MIP_LEVEL, "CL_INVALID_MIP_LEVEL"}, 71 | {CL_INVALID_GLOBAL_WORK_SIZE, "CL_INVALID_GLOBAL_WORK_SIZE"}, 72 | {CL_INVALID_PROPERTY, "CL_INVALID_PROPERTY"}, 73 | 74 | {0, NULL} // end marker 75 | }; 76 | 77 | const char *GetOpenCLErrorString(cl_int error) 78 | { 79 | int idx = 0; 80 | 81 | while (openCL_error_msgs[idx].msg != NULL) { 82 | if (openCL_error_msgs[idx].code == error) 83 | return openCL_error_msgs[idx].msg; 84 | 85 | ++idx; 86 | } 87 | 88 | cerr << "Warning: encountered unknown OpenCL error code: " << error << endl; 89 | 90 | return "Unknown OpenCL error"; 91 | } 92 | 93 | //////////////////////////////////////////////////////////////////////////////// 94 | 95 | OpenCLBuffer::OpenCLBuffer(OpenCLContext &context, 96 | size_t size, 97 | cl_mem_flags flags, 98 | bool create_ogl_buffer) 99 | : _context(context), _size(size) 100 | { 101 | cl_int error = 0; 102 | 103 | 104 | _buffer = clCreateBuffer(_context._context, flags, size, NULL, &error); 105 | 106 | if (error != CL_SUCCESS) { 107 | cerr << "Error allocating OpenCL buffer of size " << size 108 | << ", error : " << GetOpenCLErrorString(error) << endl; 109 | 110 | _buffer = NULL; 111 | _size = 0; 112 | } 113 | } 114 | 115 | OpenCLBuffer::~OpenCLBuffer() 116 | { 117 | if (_buffer) 118 | clReleaseMemObject(_buffer); 119 | } 120 | 121 | bool OpenCLBuffer::UploadData(const void *data, size_t size, size_t offset, 122 | bool blocking) 123 | { 124 | cl_int error = 0; 125 | 126 | if (offset + size > _size) 127 | return false; 128 | 129 | OpenCLMemory *ptr = this; 130 | 131 | error = clEnqueueWriteBuffer(_context._queue, _buffer, blocking, 132 | offset, size, data, 0, 0, 0); 133 | 134 | if (error != CL_SUCCESS) { 135 | cerr << "Error writing OpenCL buffer: " 136 | << GetOpenCLErrorString(error) << endl; 137 | return false; 138 | } 139 | 140 | return true; 141 | } 142 | 143 | bool OpenCLBuffer::DownloadData(void *data, size_t size, size_t offset, 144 | bool blocking) 145 | { 146 | cl_int error = 0; 147 | 148 | if (offset + size > _size) 149 | return false; 150 | 151 | OpenCLMemory *ptr = this; 152 | 153 | error = clEnqueueReadBuffer(_context._queue, _buffer, blocking, 154 | offset, size, data, 0, 0, 0); 155 | 156 | if (error != CL_SUCCESS) { 157 | cerr << "Error reading OpenCL buffer: " 158 | << GetOpenCLErrorString(error) << endl; 159 | return false; 160 | } 161 | 162 | return true; 163 | } 164 | 165 | //////////////////////////////////////////////////////////////////////////////// 166 | 167 | OpenCLKernel::~OpenCLKernel() 168 | { 169 | if (_kernel) 170 | clReleaseKernel(_kernel); 171 | } 172 | 173 | OpenCLKernel::OpenCLKernel(OpenCLContext &context, const OpenCLProgram &program, 174 | const std::string &name) 175 | : _context(context), _program(program), _kernel(NULL), _name(name) 176 | { 177 | cl_int error = 0; 178 | 179 | // Extract the kernel 180 | _kernel = clCreateKernel(_program._program, _name.c_str(), &error); 181 | if (error != CL_SUCCESS) { 182 | cerr << "Error creating OpenCL kernel ('" << _name << "' : " 183 | << GetOpenCLErrorString(error) << endl; 184 | 185 | _kernel = NULL; 186 | 187 | return; 188 | } 189 | } 190 | 191 | bool OpenCLKernel::SetBuf(int argNo, OpenCLBuffer *buf) 192 | { 193 | if (!_kernel) 194 | return false; 195 | 196 | cl_int error = clSetKernelArg(_kernel, argNo, 197 | sizeof(cl_mem), &buf->_buffer); 198 | if (error == CL_SUCCESS) 199 | return true; 200 | 201 | std::cerr << "Error setting kernel argument: " 202 | << GetOpenCLErrorString(error) << std::endl; 203 | 204 | return false; 205 | } 206 | 207 | bool OpenCLKernel::RunKernel(int dim, const size_t *workSize, 208 | const size_t *localWorkSize) 209 | { 210 | if (!_kernel) 211 | return false; 212 | 213 | cl_int error = clEnqueueNDRangeKernel(_context._queue, _kernel, 214 | dim, NULL, workSize, localWorkSize, 215 | 0, NULL, NULL); 216 | 217 | if (error == CL_SUCCESS) 218 | return true; 219 | 220 | std::cerr << "Error running kernel: " 221 | << GetOpenCLErrorString(error) << std::endl; 222 | 223 | return false; 224 | } 225 | 226 | //////////////////////////////////////////////////////////////////////////////// 227 | 228 | 229 | OpenCLProgram::OpenCLProgram(OpenCLContext &context, const std::string &code) 230 | : _context(context), _program(NULL) 231 | { 232 | cl_int error = 0; 233 | 234 | // Create the program 235 | size_t src_size = code.size(); 236 | const char *cptr = code.c_str(); 237 | _program = clCreateProgramWithSource(_context._context, 1, 238 | &cptr, &src_size, &error); 239 | if (error != CL_SUCCESS) { 240 | cerr << "Error creating OpenCL program: " 241 | << GetOpenCLErrorString(error) << endl; 242 | _program = NULL; 243 | return; 244 | } 245 | 246 | // Builds the program 247 | error = clBuildProgram(_program, 1, &_context._device, NULL, NULL, NULL); 248 | 249 | if (error != CL_SUCCESS) { 250 | cerr << "Error building OpenCL program: " 251 | << GetOpenCLErrorString(error) << endl; 252 | 253 | // Shows the log 254 | char* build_log; 255 | size_t log_size; 256 | // First call to know the proper size 257 | clGetProgramBuildInfo(_program, _context._device, 258 | CL_PROGRAM_BUILD_LOG, 0, NULL, &log_size); 259 | build_log = new char[log_size+1]; 260 | 261 | // Second call to get the log 262 | clGetProgramBuildInfo(_program, _context._device, 263 | CL_PROGRAM_BUILD_LOG, log_size, build_log, NULL); 264 | build_log[log_size] = '\0'; 265 | cerr << build_log << endl; 266 | delete [] build_log; 267 | 268 | clReleaseProgram(_program); 269 | _program = NULL; 270 | 271 | return; 272 | } 273 | 274 | _code = code; 275 | } 276 | 277 | OpenCLProgram::~OpenCLProgram() 278 | { 279 | if (_program) 280 | clReleaseProgram(_program); 281 | } 282 | 283 | OpenCLKernel *OpenCLProgram::CreateKernel(const std::string &name) 284 | { 285 | if (!_program) 286 | return NULL; 287 | 288 | OpenCLKernel *kernel = new OpenCLKernel(_context, *this, name); 289 | if (kernel->IsValid()) 290 | return kernel; 291 | 292 | delete kernel; 293 | 294 | return NULL; 295 | } 296 | 297 | //////////////////////////////////////////////////////////////////////////////// 298 | 299 | OpenCLContext::OpenCLContext(bool openGLSupport) 300 | : _platform(0), _device(0), _context(0), _queue(0), 301 | _supportsOpenGL(openGLSupport) 302 | { 303 | cl_int error = 0; 304 | 305 | // Platform 306 | error = clGetPlatformIDs(1, &_platform, 0); 307 | if (error != CL_SUCCESS) { 308 | cerr << "Error getting platform id: " 309 | << GetOpenCLErrorString(error) << endl; 310 | 311 | _platform = 0; 312 | 313 | return; 314 | } 315 | 316 | // Device 317 | error = clGetDeviceIDs(_platform, CL_DEVICE_TYPE_GPU, 1, &_device, NULL); 318 | if (error != CL_SUCCESS) { 319 | cerr << "Error getting device ids: " 320 | << GetOpenCLErrorString(error) << endl; 321 | 322 | _device = 0; 323 | _platform = 0; 324 | 325 | return; 326 | } 327 | 328 | // Context 329 | cl_context_properties props[] = { 330 | CL_CONTEXT_PLATFORM, (cl_context_properties)(_platform), 331 | 0 332 | }; 333 | 334 | _context = clCreateContext(props, 1, 335 | &_device, NULL, NULL, &error); 336 | if (error != CL_SUCCESS) { 337 | cerr << "Error creating context: " 338 | << GetOpenCLErrorString(error) << endl; 339 | 340 | _context = 0; 341 | _device = 0; 342 | _platform = 0; 343 | 344 | return; 345 | } 346 | 347 | // Default command-queue 348 | _queue = clCreateCommandQueue(_context, _device, 0, &error); 349 | if (error != CL_SUCCESS) { 350 | cerr << "Error creating command queue: " 351 | << GetOpenCLErrorString(error) << endl; 352 | 353 | // release allocated context 354 | clReleaseContext(_context); 355 | 356 | _queue = 0; 357 | _context = 0; 358 | _device = 0; 359 | _platform = 0; 360 | 361 | return; 362 | } 363 | } 364 | 365 | OpenCLContext::~OpenCLContext() 366 | { 367 | if (_queue) 368 | clReleaseCommandQueue(_queue); 369 | if (_context) 370 | clReleaseContext(_context); 371 | 372 | _device = 0; 373 | _platform = 0; 374 | } 375 | 376 | bool OpenCLContext::HasImagingSupport() const 377 | { 378 | if (!_context) 379 | return false; 380 | 381 | cl_bool bImageSupport; 382 | clGetDeviceInfo(_device, CL_DEVICE_IMAGE_SUPPORT, sizeof(bImageSupport), 383 | &bImageSupport, NULL); 384 | return bImageSupport; 385 | } 386 | 387 | size_t OpenCLContext::MaxWorkGroupSize() const 388 | { 389 | if (!_context) 390 | return 0; 391 | 392 | size_t mwgs = 0; 393 | clGetDeviceInfo(_device, CL_DEVICE_MAX_WORK_GROUP_SIZE, sizeof(size_t), 394 | &mwgs, NULL); 395 | return mwgs; 396 | } 397 | 398 | size_t OpenCLContext::MaxLocalMemory() const 399 | { 400 | if (!_context) 401 | return 0; 402 | 403 | cl_ulong mlm = 0; 404 | clGetDeviceInfo(_device, CL_DEVICE_LOCAL_MEM_SIZE, sizeof(cl_ulong), 405 | &mlm, NULL); 406 | return mlm; 407 | } 408 | 409 | OpenCLProgram *OpenCLContext::CreateProgram(const std::string &code) 410 | { 411 | if (!_context) 412 | return NULL; 413 | 414 | OpenCLProgram *prog = new OpenCLProgram(*this, code); 415 | if (prog->IsValid()) 416 | return prog; 417 | 418 | delete prog; 419 | 420 | return NULL; 421 | } 422 | 423 | OpenCLProgram *OpenCLContext::CreateProgramFromFile(const std::string &filename) 424 | { 425 | // read contents of text file into string 426 | ifstream file(filename.c_str()); 427 | if (!file.is_open()) { 428 | cerr << "Error loading program from file: " << filename << endl; 429 | return NULL; 430 | } 431 | 432 | string code; 433 | 434 | // find the size of the data in the file and 435 | // pre-allocate space in the string 436 | file.seekg(0, std::ios::end); 437 | code.reserve(file.tellg()); 438 | file.seekg(0, std::ios::beg); 439 | 440 | code.assign((std::istreambuf_iterator(file)), 441 | std::istreambuf_iterator()); 442 | 443 | return CreateProgram(code); 444 | } 445 | 446 | size_t OpenCLContext::MaxMemAllocSize() const 447 | { 448 | if (!_context) 449 | return 0; 450 | 451 | cl_ulong bsize = 0; 452 | clGetDeviceInfo(_device, CL_DEVICE_MAX_MEM_ALLOC_SIZE, sizeof(cl_ulong), 453 | &bsize, NULL); 454 | return bsize; 455 | } 456 | 457 | OpenCLBuffer *OpenCLContext::AllocMemBuffer(size_t size, cl_mem_flags flags) 458 | { 459 | if (!_context) 460 | return NULL; 461 | 462 | OpenCLBuffer *buf = new OpenCLBuffer(*this, size, flags, false); 463 | 464 | if (buf->IsValid()) 465 | return buf; 466 | 467 | delete buf; 468 | 469 | return NULL; 470 | } 471 | 472 | void OpenCLContext::Finish() 473 | { 474 | if (!_context) 475 | return; 476 | 477 | cl_int error = 0; 478 | 479 | error = clFinish(_queue); 480 | if (error != CL_SUCCESS) 481 | { 482 | cerr << "Error Finish: " << GetOpenCLErrorString(error) << endl; 483 | } 484 | } 485 | 486 | -------------------------------------------------------------------------------- /testcode/openCLWrapper.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | #ifndef OPENCL_WRAPPER_H 4 | #define OPENCL_WRAPPER_H 5 | 6 | #ifdef __APPLE__ 7 | #include 8 | #else 9 | #include 10 | #endif 11 | 12 | #ifdef __APPLE__ 13 | #include 14 | #else 15 | #include 16 | #endif 17 | 18 | #include 19 | #include 20 | 21 | // forward decl 22 | class OpenCLContext; 23 | class OpenCLProgram; 24 | 25 | // defines 26 | #define TEXTURE_TYPE_UBYTE 1 27 | #define TEXTURE_TYPE_HALF 2 28 | #define TEXTURE_TYPE_FLOAT 4 29 | 30 | //////////////////////////////////////////////////////////////////////////////// 31 | // OpenCL memory location abstraction 32 | // base class for OpenCLBuffer and OpenCLTexture 33 | 34 | class OpenCLMemory 35 | { 36 | friend class OpenCLContext; 37 | 38 | public: 39 | ~OpenCLMemory() 40 | { 41 | } 42 | 43 | protected: 44 | OpenCLMemory() 45 | : _buffer(NULL) 46 | { 47 | } 48 | 49 | cl_mem _buffer; 50 | }; 51 | 52 | //////////////////////////////////////////////////////////////////////////////// 53 | // OpenCL memory buffer abstraction 54 | 55 | class OpenCLBuffer : public OpenCLMemory 56 | { 57 | friend class OpenCLContext; 58 | friend class OpenCLKernel; 59 | 60 | public: 61 | ~OpenCLBuffer(); 62 | 63 | bool IsValid() const 64 | { 65 | return _buffer != NULL; 66 | } 67 | size_t GetSize() const 68 | { 69 | return _size; 70 | } 71 | 72 | // upload of data to the memory buffer 73 | // it checks that the transfer does not exceed the bounds of the buffer 74 | // if blocking is false the source buffer must be kept around until the 75 | // transfer completes 76 | bool UploadData(const void *data, size_t size, size_t offset = 0, 77 | bool blocking = true); 78 | 79 | // download data from the memory buffer 80 | // it checks that the transfer does not exceed the bounds of he buffer 81 | // if blocking is false the destination buffer must be kept around until 82 | // the transfer completes 83 | bool DownloadData(void *data, size_t size, size_t offset = 0, 84 | bool blocking = true); 85 | 86 | private: 87 | OpenCLBuffer(OpenCLContext &context, size_t size, 88 | cl_mem_flags flags, bool create_ogl_buffer); 89 | 90 | OpenCLContext &_context; 91 | size_t _size; 92 | //unsigned int _ogl_buffer; 93 | }; 94 | 95 | //////////////////////////////////////////////////////////////////////////////// 96 | // OpenCL kernel abstraction 97 | 98 | class OpenCLKernel 99 | { 100 | friend class OpenCLProgram; 101 | 102 | public: 103 | ~OpenCLKernel(); 104 | 105 | bool IsValid() const 106 | { 107 | return _kernel != NULL; 108 | } 109 | 110 | // set arguments 111 | template 112 | bool SetArg(int argNo, T arg); 113 | 114 | template 115 | bool SetArray(int argNo, T *arg, int numElem); 116 | 117 | bool SetBuf(int argNo, OpenCLBuffer *buf); 118 | 119 | // call the kernel 120 | bool RunKernel(int dim, const size_t *workSize, const size_t *localWorkSize = NULL); 121 | 122 | private: 123 | OpenCLKernel(OpenCLContext &context, const OpenCLProgram &program, 124 | const std::string &name); 125 | 126 | OpenCLContext &_context; 127 | const OpenCLProgram &_program; 128 | cl_kernel _kernel; 129 | std::string _name; 130 | }; 131 | 132 | template 133 | bool OpenCLKernel::SetArg(int argNo, T arg) 134 | { 135 | if (!_kernel) 136 | return false; 137 | 138 | cl_int error = clSetKernelArg(_kernel, argNo, sizeof(T), &arg); 139 | if (error == CL_SUCCESS) 140 | return true; 141 | 142 | std::cerr << "Error setting kernel argument " << argNo << " : " << error << std::endl; 143 | std::cerr << "Arg size: " << sizeof(T) << std::endl; 144 | 145 | 146 | return false; 147 | } 148 | 149 | template 150 | bool OpenCLKernel::SetArray(int argNo, T *arg, int numElem) 151 | { 152 | if (!_kernel) 153 | return false; 154 | 155 | cl_int error = clSetKernelArg(_kernel, argNo, sizeof(T)*numElem, arg); 156 | if (error == CL_SUCCESS) 157 | return true; 158 | 159 | std::cerr << "Error setting kernel argument " << argNo << " : " << error << std::endl; 160 | std::cerr << "Arg size: " << (sizeof(T)*numElem) << std::endl; 161 | 162 | 163 | return false; 164 | } 165 | 166 | 167 | //////////////////////////////////////////////////////////////////////////////// 168 | // OpenCL program abstraction 169 | 170 | class OpenCLProgram 171 | { 172 | friend class OpenCLContext; 173 | friend class OpenCLKernel; 174 | 175 | public: 176 | ~OpenCLProgram(); 177 | 178 | bool IsValid() const 179 | { 180 | return _program != NULL; 181 | } 182 | 183 | OpenCLKernel *CreateKernel(const std::string &name); 184 | 185 | private: 186 | OpenCLProgram(OpenCLContext &context, const std::string &code); 187 | 188 | OpenCLContext &_context; 189 | cl_program _program; 190 | std::string _code; 191 | }; 192 | 193 | //////////////////////////////////////////////////////////////////////////////// 194 | // OpenCL context abstraction 195 | // initializes the platform, the device and the context 196 | // as well as a default queue 197 | 198 | class OpenCLContext 199 | { 200 | friend class OpenCLBuffer; 201 | friend class OpenCLTexture; 202 | friend class OpenCLKernel; 203 | friend class OpenCLProgram; 204 | 205 | public: 206 | // the constructor initializes the OpenCL platform, device and context 207 | OpenCLContext(bool openGLSupport = false); 208 | ~OpenCLContext(); 209 | 210 | // To check successful creation of context 211 | bool IsValid() const 212 | { 213 | return _context != NULL; 214 | } 215 | 216 | // Queries to capabilities of the OpenCL device 217 | bool HasImagingSupport() const; 218 | size_t MaxWorkGroupSize() const; 219 | size_t MaxLocalMemory() const; 220 | size_t MaxMemAllocSize() const; 221 | 222 | // Create programs 223 | OpenCLProgram *CreateProgram(const std::string &code); 224 | OpenCLProgram *CreateProgramFromFile(const std::string &filename); 225 | 226 | // Create mem buffer 227 | OpenCLBuffer *AllocMemBuffer(size_t size, cl_mem_flags flags); 228 | 229 | // Flow control 230 | void Finish(); 231 | 232 | private: 233 | cl_platform_id _platform; 234 | cl_device_id _device; 235 | cl_context _context; 236 | cl_command_queue _queue; 237 | 238 | bool _supportsOpenGL; 239 | }; 240 | 241 | //////////////////////////////////////////////////////////////////////////////// 242 | 243 | #endif // OPENCL_WRAPPER_H 244 | 245 | -------------------------------------------------------------------------------- /testcode/test_main.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "openCLWrapper.h" 4 | #include 5 | #include 6 | #include 7 | #include "utilities.h" 8 | #include "vector_add_cpu.h" 9 | 10 | #define PROGRAM_FILE "vector_add_gpu.cl" 11 | 12 | using namespace std; 13 | 14 | 15 | const int NUM_SRC_PARAMS = 2; 16 | const int NUM_RET_PARAMS = 1; 17 | const int VEC_SIZE = 1234567; 18 | 19 | size_t shrRoundUp( const size_t group_size, const size_t global_size ) 20 | { 21 | size_t r = global_size % group_size; 22 | if( r == 0 ) 23 | return global_size; 24 | return global_size + group_size - r; 25 | } 26 | 27 | int main() 28 | { 29 | MyTimer Timer= MyTimer(); 30 | Timer.Start(); 31 | /* 32 | float *src1 = new float[VEC_SIZE]; 33 | float *src2 = new float[VEC_SIZE]; 34 | float *res = new float[VEC_SIZE]; 35 | 36 | for (int i=0; iIsValid()) { 49 | printf("CONTEXT IS NOT VALID\n"); 50 | assert(false); 51 | } 52 | 53 | //get kernel paths (hard coded for now) 54 | string kernelpaths = "/Users/ylee8/FastSLAM/testcode/"; 55 | 56 | //create program (CreateProgramFromFile also builds the prog) 57 | cout<<"kernel path: "<CreateProgramFromFile(kernelpaths + PROGRAM_FILE); 59 | if(!prog) { 60 | cerr<<"Error: couldn't load OpenCL program!"<CreateKernel("vecAdd_k"); 66 | if (!math_kernel) { 67 | cerr<<"Error: couldn't load OpenCL Kernel!"<AllocMemBuffer(mem_size,CL_MEM_READ_ONLY); 80 | src_params_2 = context->AllocMemBuffer(mem_size,CL_MEM_READ_ONLY); 81 | 82 | // Initialize both vectors 83 | float* src1_dataf = new float[VEC_SIZE]; 84 | float* src2_dataf = new float[VEC_SIZE]; 85 | for (int i = 0; i < VEC_SIZE; i++) { 86 | src1_dataf[i] = src2_dataf[i] = (float) i; 87 | } 88 | src_params_1->UploadData(src1_dataf, VEC_SIZE * sizeof(float)); 89 | src_params_2->UploadData(src2_dataf, VEC_SIZE * sizeof(float)); 90 | 91 | ////////////////////////// 92 | // allocate mem for result 93 | ////////////////////////// 94 | 95 | OpenCLBuffer *res_params; 96 | res_params = context->AllocMemBuffer(mem_size,CL_MEM_WRITE_ONLY); 97 | 98 | 99 | ///////////////////// 100 | // launching kernel 101 | ///////////////////// 102 | 103 | //set up the kernel args (calls clKernelArgs) 104 | math_kernel->SetBuf(0,src_params_1); //SetBuf(which argument, buffer) 105 | math_kernel->SetBuf(1,src_params_2); 106 | math_kernel->SetBuf(2,res_params); 107 | math_kernel->SetArg(3,VEC_SIZE); 108 | 109 | 110 | //context finish (blocks until everything in CL queue is processed 111 | context->Finish(); 112 | 113 | //run the kernel 114 | const size_t global_size_offset = NULL; // 115 | const size_t local_work_size = 512; //total number of work items 116 | const size_t global_work_size = shrRoundUp(local_work_size,VEC_SIZE); //num of work items per work group 117 | int dim = 1; 118 | 119 | 120 | math_kernel->RunKernel(dim, &global_work_size, &local_work_size); 121 | 122 | //Get the result 123 | float *result = new float[VEC_SIZE]; 124 | res_params->DownloadData(result, mem_size); 125 | 126 | /* 127 | for (size_t i =0; iFinish(); 133 | Timer.Stop(); 134 | Timer.Print("gpu vec add done"); 135 | return 0; 136 | } 137 | -------------------------------------------------------------------------------- /testcode/utilities.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | //////////////////////////////////////////////////////////////////////////////// 5 | // MyTimer 6 | //////////////////////////////////////////////////////////////////////////////// 7 | 8 | // little helper class to time stuff 9 | // this should be in a utilities header file 10 | class MyTimer 11 | { 12 | public: 13 | MyTimer() 14 | { 15 | } 16 | 17 | void Start() 18 | { 19 | gettimeofday(&tv1, NULL); 20 | } 21 | double Stop() 22 | { 23 | gettimeofday(&tv2, NULL); 24 | int sec = tv2.tv_sec - tv1.tv_sec; 25 | int usec = tv2.tv_usec - tv1.tv_usec; 26 | 27 | if (usec < 0) 28 | { 29 | sec--; 30 | usec = 1000000 + usec; 31 | } 32 | 33 | return (double)sec + (double)usec / 1000000.0; 34 | } 35 | void Print(const char *label) 36 | { 37 | int sec = tv2.tv_sec - tv1.tv_sec; 38 | int usec = tv2.tv_usec - tv1.tv_usec; 39 | 40 | if (usec < 0) 41 | { 42 | sec--; 43 | usec = 1000000 + usec; 44 | } 45 | printf("%s took %d sec %d usec\n", label, sec, usec); 46 | } 47 | 48 | private: 49 | struct timeval tv1, tv2; 50 | }; 51 | 52 | -------------------------------------------------------------------------------- /testcode/vector_add_cpu.cpp: -------------------------------------------------------------------------------- 1 | #include "vector_add_cpu.h" 2 | 3 | void vector_add_cpu (const float* src_a, 4 | const float* src_b, 5 | float* res, 6 | const int num) 7 | { 8 | for (int i = 0; i < num; i++) 9 | res[i] = src_a[i] + src_b[i]; 10 | } -------------------------------------------------------------------------------- /testcode/vector_add_cpu.h: -------------------------------------------------------------------------------- 1 | void vector_add_cpu (const float* src_a, 2 | const float* src_b, 3 | float* res, 4 | const int num); -------------------------------------------------------------------------------- /testcode/vector_add_gpu.cl: -------------------------------------------------------------------------------- 1 | //CL 2 | __kernel void 3 | vecAdd_k (__global const float* src_a, 4 | __global const float* src_b, 5 | __global float* res, 6 | const int num) 7 | { 8 | /* get_global_id(0) returns the ID of the thread in execution. 9 | As many threads are launched at the same time, executing the same kernel, 10 | each one will receive a different ID, and consequently perform a different computation.*/ 11 | const int idx = get_global_id(0); 12 | 13 | /* Now each work-item asks itself: "is my ID inside the vector's range?" 14 | If the answer is YES, the work-item performs the corresponding computation*/ 15 | if (idx < num) 16 | res[idx] = src_a[idx] + src_b[idx]; 17 | } 18 | -------------------------------------------------------------------------------- /thirdparty/gtest/CHANGES: -------------------------------------------------------------------------------- 1 | Changes for 1.5.0: 2 | 3 | * New feature: assertions can be safely called in multiple threads 4 | where the pthreads library is available. 5 | * New feature: predicates used inside EXPECT_TRUE() and friends 6 | can now generate custom failure messages. 7 | * New feature: Google Test can now be compiled as a DLL. 8 | * New feature: fused source files are included. 9 | * New feature: prints help when encountering unrecognized Google Test flags. 10 | * Experimental feature: CMake build script (requires CMake 2.6.4+). 11 | * Experimental feature: the Pump script for meta programming. 12 | * double values streamed to an assertion are printed with enough precision 13 | to differentiate any two different values. 14 | * Google Test now works on Solaris and AIX. 15 | * Build and test script improvements. 16 | * Bug fixes and implementation clean-ups. 17 | 18 | Potentially breaking changes: 19 | 20 | * Stopped supporting VC++ 7.1 with exceptions disabled. 21 | * Dropped support for 'make install'. 22 | 23 | Changes for 1.4.0: 24 | 25 | * New feature: the event listener API 26 | * New feature: test shuffling 27 | * New feature: the XML report format is closer to junitreport and can 28 | be parsed by Hudson now. 29 | * New feature: when a test runs under Visual Studio, its failures are 30 | integrated in the IDE. 31 | * New feature: /MD(d) versions of VC++ projects. 32 | * New feature: elapsed time for the tests is printed by default. 33 | * New feature: comes with a TR1 tuple implementation such that Boost 34 | is no longer needed for Combine(). 35 | * New feature: EXPECT_DEATH_IF_SUPPORTED macro and friends. 36 | * New feature: the Xcode project can now produce static gtest 37 | libraries in addition to a framework. 38 | * Compatibility fixes for Solaris, Cygwin, minGW, Windows Mobile, 39 | Symbian, gcc, and C++Builder. 40 | * Bug fixes and implementation clean-ups. 41 | 42 | Changes for 1.3.0: 43 | 44 | * New feature: death tests on Windows, Cygwin, and Mac. 45 | * New feature: ability to use Google Test assertions in other testing 46 | frameworks. 47 | * New feature: ability to run disabled test via 48 | --gtest_also_run_disabled_tests. 49 | * New feature: the --help flag for printing the usage. 50 | * New feature: access to Google Test flag values in user code. 51 | * New feature: a script that packs Google Test into one .h and one 52 | .cc file for easy deployment. 53 | * New feature: support for distributing test functions to multiple 54 | machines (requires support from the test runner). 55 | * Bug fixes and implementation clean-ups. 56 | 57 | Changes for 1.2.1: 58 | 59 | * Compatibility fixes for Linux IA-64 and IBM z/OS. 60 | * Added support for using Boost and other TR1 implementations. 61 | * Changes to the build scripts to support upcoming release of Google C++ 62 | Mocking Framework. 63 | * Added Makefile to the distribution package. 64 | * Improved build instructions in README. 65 | 66 | Changes for 1.2.0: 67 | 68 | * New feature: value-parameterized tests. 69 | * New feature: the ASSERT/EXPECT_(NON)FATAL_FAILURE(_ON_ALL_THREADS) 70 | macros. 71 | * Changed the XML report format to match JUnit/Ant's. 72 | * Added tests to the Xcode project. 73 | * Added scons/SConscript for building with SCons. 74 | * Added src/gtest-all.cc for building Google Test from a single file. 75 | * Fixed compatibility with Solaris and z/OS. 76 | * Enabled running Python tests on systems with python 2.3 installed, 77 | e.g. Mac OS X 10.4. 78 | * Bug fixes. 79 | 80 | Changes for 1.1.0: 81 | 82 | * New feature: type-parameterized tests. 83 | * New feature: exception assertions. 84 | * New feature: printing elapsed time of tests. 85 | * Improved the robustness of death tests. 86 | * Added an Xcode project and samples. 87 | * Adjusted the output format on Windows to be understandable by Visual Studio. 88 | * Minor bug fixes. 89 | 90 | Changes for 1.0.1: 91 | 92 | * Added project files for Visual Studio 7.1. 93 | * Fixed issues with compiling on Mac OS X. 94 | * Fixed issues with compiling on Cygwin. 95 | 96 | Changes for 1.0.0: 97 | 98 | * Initial Open Source release of Google Test 99 | -------------------------------------------------------------------------------- /thirdparty/gtest/CONTRIBUTORS: -------------------------------------------------------------------------------- 1 | # This file contains a list of people who've made non-trivial 2 | # contribution to the Google C++ Testing Framework project. People 3 | # who commit code to the project are encouraged to add their names 4 | # here. Please keep the list sorted by first names. 5 | 6 | Ajay Joshi 7 | Balázs Dán 8 | Bharat Mediratta 9 | Chandler Carruth 10 | Chris Prince 11 | Chris Taylor 12 | Dan Egnor 13 | Eric Roman 14 | Hady Zalek 15 | Jeffrey Yasskin 16 | Jói Sigurðsson 17 | Keir Mierle 18 | Keith Ray 19 | Kenton Varda 20 | Manuel Klimek 21 | Markus Heule 22 | Mika Raento 23 | Miklós Fazekas 24 | Patrick Hanna 25 | Patrick Riley 26 | Peter Kaminski 27 | Preston Jackson 28 | Rainer Klaffenboeck 29 | Russ Cox 30 | Russ Rufer 31 | Sean Mcafee 32 | Sigurður Ásgeirsson 33 | Tracy Bialik 34 | Vadim Berman 35 | Vlad Losev 36 | Zhanyong Wan 37 | -------------------------------------------------------------------------------- /thirdparty/gtest/COPYING: -------------------------------------------------------------------------------- 1 | Copyright 2008, Google Inc. 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are 6 | met: 7 | 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above 11 | copyright notice, this list of conditions and the following disclaimer 12 | in the documentation and/or other materials provided with the 13 | distribution. 14 | * Neither the name of Google Inc. nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 19 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 20 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 21 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 24 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 25 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 26 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /thirdparty/gtest/src/gtest_main.cc: -------------------------------------------------------------------------------- 1 | // __BEGIN_LICENSE__ 2 | // Copyright (C) 2006-2011 United States Government as represented by 3 | // the Administrator of the National Aeronautics and Space Administration. 4 | // All Rights Reserved. 5 | // __END_LICENSE__ 6 | 7 | 8 | // Copyright 2006, Google Inc. 9 | // All rights reserved. 10 | // 11 | // Redistribution and use in source and binary forms, with or without 12 | // modification, are permitted provided that the following conditions are 13 | // met: 14 | // 15 | // * Redistributions of source code must retain the above copyright 16 | // notice, this list of conditions and the following disclaimer. 17 | // * Redistributions in binary form must reproduce the above 18 | // copyright notice, this list of conditions and the following disclaimer 19 | // in the documentation and/or other materials provided with the 20 | // distribution. 21 | // * Neither the name of Google Inc. nor the names of its 22 | // contributors may be used to endorse or promote products derived from 23 | // this software without specific prior written permission. 24 | // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 26 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 27 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 28 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 29 | // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 30 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 31 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 32 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 33 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 34 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 35 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 36 | 37 | #include 38 | 39 | #include 40 | 41 | int main(int argc, char **argv) { 42 | std::cout << "Running main() from gtest_main.cc\n"; 43 | 44 | testing::InitGoogleTest(&argc, argv); 45 | return RUN_ALL_TESTS(); 46 | } 47 | --------------------------------------------------------------------------------