├── src ├── empty.cpp ├── maps │ └── CMakeLists.txt ├── models │ └── CMakeLists.txt ├── CMakeLists.txt ├── utils │ ├── VersionInfo.h │ ├── CMakeLists.txt │ ├── Convenience.cpp │ └── Version.cpp ├── io │ ├── CMakeLists.txt │ └── carmen │ │ ├── CarmenGtReader.cpp │ │ └── CarmenReader.cpp └── viz │ └── CMakeLists.txt ├── images ├── intel.png └── cartesium.png ├── cmake_modules └── CompilerSettings.cmake ├── LICENSE ├── scripts └── create_version_info.sh ├── include └── efslam │ ├── slam │ ├── SLAMTypes.h │ ├── BayesFastSLAM2D.h │ ├── BeliefFastSLAM2D.h │ ├── Particle.hpp │ ├── TrajectoryNode.hpp │ ├── Particle.h │ └── TrajectoryNode.h │ ├── maps │ ├── BayesMap2D.h │ ├── BeliefMap2D.h │ ├── Map2D.h │ ├── storages │ │ ├── HArray2D.h │ │ ├── HStorageHelper.h │ │ ├── HashMap.h │ │ ├── HStorageHelper.hpp │ │ ├── HStorage.h │ │ └── Array2D.h │ ├── AccessState.h │ └── cells │ │ ├── CellBase.h │ │ ├── BayesCell.h │ │ ├── BeliefCell.h │ │ ├── PointAccumulator.h │ │ ├── BayesCell.hpp │ │ └── PointAccumulator.hpp │ ├── models │ ├── MotionModel.h │ ├── SensorModel.h │ ├── MotionModel2D.h │ ├── MotionModel2D.hpp │ ├── SensorModelBayes.hpp │ ├── SensorModelBayes.h │ └── MapScanMatcher.h │ ├── viz │ ├── Color.h │ ├── Color.hpp │ ├── CellColor.h │ ├── NeffView.h │ └── CellColor.hpp │ ├── utils │ ├── ConditionalMutex.h │ ├── ConditionalMutex.hpp │ ├── Line.h │ ├── Stopwatch.hpp │ ├── CovarianceEllipse.h │ ├── PointIterator.h │ ├── PointCrossIterator.h │ ├── Scan.h │ ├── Stopwatch.h │ ├── Version.h │ ├── PoseBase.hpp │ ├── rawptr.h │ ├── PoseBase.h │ ├── PointIterator.hpp │ ├── BeliefFunction.h │ ├── Log.h │ ├── PointComp.h │ ├── CovarianceEllipse.hpp │ ├── PointCrossIterator.hpp │ ├── BeliefFunction.hpp │ ├── shareduniqueptr.hpp │ └── Config.h │ └── io │ └── carmen │ ├── CarmenGtReader.h │ ├── CarmenProcessor.h │ └── CarmenReader.h ├── CMakeLists.txt └── README.md /src/empty.cpp: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /src/maps/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /src/models/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /images/intel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JoachimClemens/Evidential-FastSLAM/HEAD/images/intel.png -------------------------------------------------------------------------------- /images/cartesium.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JoachimClemens/Evidential-FastSLAM/HEAD/images/cartesium.png -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | ADD_SUBDIRECTORY( maps ) 3 | ADD_SUBDIRECTORY( models ) 4 | ADD_SUBDIRECTORY( utils ) 5 | ADD_SUBDIRECTORY( viz ) 6 | ADD_SUBDIRECTORY( io ) 7 | 8 | ADD_LIBRARY( efslam SHARED empty.cpp ) 9 | SET_TARGET_PROPERTIES( efslam PROPERTIES 10 | VERSION ${EFSLAM_VERSION} 11 | SOVERSION ${EFSLAM_VERSION} 12 | ) 13 | 14 | TARGET_LINK_LIBRARIES( efslam efslam-utils efslam-io ) 15 | -------------------------------------------------------------------------------- /src/utils/VersionInfo.h: -------------------------------------------------------------------------------- 1 | /* This file was automatically generated during make process on Wed Apr 27 10:44:58 CEST 2016 */ 2 | /* All changes will be overwritten! */ 3 | 4 | #ifndef _VERSION_INFO__H 5 | #define _VERSION_INFO__H 6 | 7 | const char *versionStr = ""; 8 | 9 | const char *buildDateStr = "2016-04-27 10:44:58 +0200"; 10 | 11 | const char *commitHashStr = "d1994d0d3d071ee15504f0e5cd1eb140372e6201"; 12 | const char *commitDateStr = "2016-02-16 09:03:14 +0100"; 13 | 14 | const char *branchNameStr = "master"; 15 | 16 | #endif /* _VERSION_INFO_H */ 17 | -------------------------------------------------------------------------------- /src/io/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET( EFSLAM_IO_SRSC 2 | carmen/CarmenReader.cpp 3 | carmen/CarmenGtReader.cpp 4 | ) 5 | 6 | IF( catkin_FOUND ) 7 | ADD_SUBDIRECTORY( ros ) 8 | ENDIF() 9 | 10 | ADD_LIBRARY( efslam-io SHARED ${EFSLAM_IO_SRSC} ) 11 | #ADD_LIBRARY( efslam-io-static STATIC ${EFSLAM_IO_SRSC} ) 12 | 13 | IF( catkin_FOUND ) 14 | target_link_libraries(efslam-io efslam-ros) 15 | #target_link_libraries(efslam-io-static efslam-ros-static) 16 | ENDIF() 17 | 18 | ADD_EXECUTABLE( EFSlamCarmenGui carmen/EFSlamCarmenGui.cpp ) 19 | TARGET_LINK_LIBRARIES( EFSlamCarmenGui efslam efslam-viz ${PCL_LIBRARIES}) -------------------------------------------------------------------------------- /src/viz/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET( EFSLAM_VIZ_SRSC 2 | MapView2D.cpp 3 | NeffView.cpp 4 | ) 5 | 6 | SET( EFSLAM_VIZ_INCLUDE_DIR ../../include/efslam/viz ) 7 | 8 | SET( EFSLAM_VIZ_MOC_HDRS 9 | ${EFSLAM_VIZ_INCLUDE_DIR}/MapView2D.h 10 | ${EFSLAM_VIZ_INCLUDE_DIR}/NeffView.h 11 | ) 12 | 13 | QT4_WRAP_CPP( EFSLAM_VIZ_MOC_SRCS ${EFSLAM_VIZ_MOC_HDRS} ) 14 | 15 | ADD_LIBRARY( efslam-viz SHARED ${EFSLAM_VIZ_SRSC} ${EFSLAM_VIZ_MOC_SRCS} ) 16 | #ADD_LIBRARY( efslam-viz-static STATIC ${EFSLAM_VIZ_SRSC} ${EFSLAM_VIZ_MOC_SRCS} ) 17 | 18 | target_link_libraries( efslam-viz ${VTK_LIBRARIES} ${QT_LIBRARIES} ) 19 | #target_link_libraries( efslam-viz-static ${VTK_LIBRARIES} ${QT_LIBRARIES} ) 20 | 21 | -------------------------------------------------------------------------------- /src/utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET( EFSLAM_UTILS_SRSC 2 | Config.cpp 3 | Convenience.cpp 4 | Version.cpp 5 | ) 6 | 7 | ADD_CUSTOM_COMMAND( OUTPUT VersionInfo.h 8 | COMMAND ${CMAKE_CURRENT_SOURCE_DIR}/../../scripts/create_version_info.sh ${CMAKE_CURRENT_SOURCE_DIR} Version.cpp 9 | ) 10 | ADD_CUSTOM_TARGET( VersionInfo DEPENDS VersionInfo.h ) 11 | 12 | ADD_LIBRARY( efslam-utils SHARED ${EFSLAM_UTILS_SRSC} ) 13 | #ADD_LIBRARY( efslam-utils-static STATIC ${EFSLAM_UTILS_SRSC} ) 14 | 15 | ADD_DEPENDENCIES( efslam-utils VersionInfo ) 16 | #ADD_DEPENDENCIES( efslam-utils-static VersionInfo ) 17 | 18 | target_link_libraries( efslam-utils ${EFSLAM_ROS_LIBS} ) 19 | #target_link_libraries( efslam-utils-static ${EFSLAM_ROS_LIBS} ) -------------------------------------------------------------------------------- /cmake_modules/CompilerSettings.cmake: -------------------------------------------------------------------------------- 1 | # COMPILER SETTINGS (default: Release) 2 | # use "-DCMAKE_BUILD_TYPE=Debug" in cmake for a Debug-build 3 | IF(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) 4 | SET(CMAKE_BUILD_TYPE Release) 5 | ENDIF(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) 6 | 7 | MESSAGE ("\n") 8 | MESSAGE (STATUS "${PROJECT_NAME} building as ${CMAKE_BUILD_TYPE}") 9 | 10 | # COMPILER FLAGS 11 | IF (CMAKE_COMPILER_IS_GNUCC) 12 | SET (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wno-error -ffast-math -fno-finite-math-only") 13 | SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -ffast-math -fno-finite-math-only") 14 | SET (CMAKE_CXX_FLAGS_RELEASE "-O3 -funroll-loops -DNDEBUG") 15 | SET (CMAKE_CXX_FLAGS_DEBUG "-O3 -g -DDEBUG") 16 | # Shared object compilation under 64bit (vtable) 17 | ADD_DEFINITIONS(-fPIC) 18 | ENDIF() 19 | 20 | 21 | # Set full rpath http://www.paraview.org/Wiki/CMake_RPATH_handling 22 | # (good to have and required with ROS) 23 | set(CMAKE_SKIP_BUILD_RPATH FALSE) 24 | set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) 25 | set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") 26 | set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) 27 | 28 | # no prefix needed for python modules 29 | set(CMAKE_SHARED_MODULE_PREFIX "") 30 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Evidential FastSLAM - An evidential approach to SLAM 4 | Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | 10 | * Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | * Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | * Neither the name of Evidential FastSLAM nor the names of its 18 | contributors may be used to endorse or promote products derived from 19 | this software without specific prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | -------------------------------------------------------------------------------- /scripts/create_version_info.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # 3 | # from http://stackoverflow.com/questions/3442874/in-git-how-can-i-write-the-current-commit-hash-to-a-file-in-the-same-commit 4 | 5 | version=$(git describe) 6 | 7 | commit=$(git log -1 --pretty="%H%n%ci") # hash \n date 8 | commit_hash=$(echo "$commit" | head -1) 9 | commit_date=$(echo "$commit" | head -2 | tail -1) # 2010-12-28 05:16:23 +0300 10 | 11 | branch_name=$(git symbolic-ref -q HEAD) # http://stackoverflow.com/questions/1593051/#1593487 12 | branch_name=${branch_name##refs/heads/} 13 | branch_name=${branch_name:-HEAD} # 'HEAD' indicates detached HEAD situation 14 | 15 | 16 | dirname=$1 17 | filename=$dirname/VersionInfo.h 18 | tmpfile=/tmp/`basename $filename` 19 | shift 20 | 21 | if [ -e $filename ]; then 22 | cp $filename $tmpfile 23 | else 24 | touch $tmpfile 25 | fi 26 | 27 | echo "/* This file was automatically generated during make process on `date` */" > $filename 28 | echo "/* All changes will be overwritten! */" >> $filename 29 | echo "" >> $filename 30 | echo "#ifndef _VERSION_INFO__H" >> $filename 31 | echo "#define _VERSION_INFO__H" >> $filename 32 | echo "" >> $filename 33 | echo "const char *versionStr = \"$version\";" >> $filename 34 | echo "" >> $filename 35 | echo "const char *buildDateStr = \"`date +'%F %T %z'`\";" >> $filename 36 | echo "" >> $filename 37 | echo "const char *commitHashStr = \"$commit_hash\";" >> $filename 38 | echo "const char *commitDateStr = \"$commit_date\";" >> $filename 39 | echo "" >> $filename 40 | echo "const char *branchNameStr = \"$branch_name\";" >> $filename 41 | echo "" >> $filename 42 | echo "#endif /* _VERSION_INFO_H */" >> $filename 43 | 44 | diff $filename $tmpfile > /dev/null 2> /dev/null 45 | 46 | # if the file changes, touch dependency files 47 | # (it contains the time, so we have always to touch the files) 48 | if [ $? -ne 0 ]; then 49 | for i in $*; do 50 | touch $dirname/$i 51 | done 52 | fi 53 | 54 | rm -f $tmpfile 55 | 56 | -------------------------------------------------------------------------------- /include/efslam/slam/SLAMTypes.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_SLAMTYPES_H_ 35 | #define EFS_SLAMTYPES_H_ 36 | 37 | 38 | namespace efs { 39 | 40 | enum SLAMType { 41 | SLAM_FAST = 0 42 | }; 43 | 44 | } 45 | 46 | 47 | #endif /* EFS_SLAMTYPES_H_ */ 48 | -------------------------------------------------------------------------------- /include/efslam/maps/BayesMap2D.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_BAYESMAP2D_H_ 35 | #define EFS_BAYESMAP2D_H_ 36 | 37 | #include "Map2D.h" 38 | #include "cells/BayesCell.h" 39 | 40 | 41 | namespace efs { 42 | 43 | using BayesMap2D = Map2D< BayesCell<2> >; 44 | 45 | } /* namespace efs */ 46 | 47 | 48 | #endif /* EFS_BAYESMAP2D_H_ */ 49 | -------------------------------------------------------------------------------- /include/efslam/maps/BeliefMap2D.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_BELIEFMAP2D_H_ 35 | #define EFS_BELIEFMAP2D_H_ 36 | 37 | #include "Map2D.h" 38 | #include "cells/BeliefCell.h" 39 | 40 | 41 | namespace efs { 42 | 43 | using BeliefMap2D = Map2D< BeliefCell<2> >; 44 | 45 | } /* namespace efs */ 46 | 47 | 48 | #endif /* EFS_BELIEFMAP2D_H_ */ 49 | -------------------------------------------------------------------------------- /include/efslam/models/MotionModel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_MOTIONMODEL_H_ 35 | #define EFS_MOTIONMODEL_H_ 36 | 37 | namespace efs { 38 | 39 | // Only basic template definition for further specialization 40 | template 41 | class MotionModel { 42 | 43 | }; 44 | 45 | } /* namespace efs */ 46 | 47 | #endif /* EFS_MOTIONMODEL_H_ */ 48 | -------------------------------------------------------------------------------- /include/efslam/maps/Map2D.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_MAP2D_H_ 35 | #define EFS_MAP2D_H_ 36 | 37 | #include "Map.h" 38 | #include "storages/HArray2D.h" 39 | 40 | 41 | namespace efs { 42 | 43 | template 44 | using Map2D = Map< Cell, HierarchicalArray2D< Cell, Magnitude > >; 45 | 46 | } /* namespace efs */ 47 | 48 | 49 | #endif /* EFS_MAP2D_H_ */ 50 | -------------------------------------------------------------------------------- /include/efslam/maps/storages/HArray2D.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_HARRAY2D_H_ 35 | #define EFS_HARRAY2D_H_ 36 | 37 | #include "Array2D.h" 38 | #include "HStorage.h" 39 | 40 | 41 | namespace efs { 42 | 43 | template 44 | using HierarchicalArray2D = HierarchicalStorage< Cell, Array2D, Magnitude >; 45 | 46 | } /* namespace efs */ 47 | 48 | 49 | #endif /* EFS_HARRAY2D_H_ */ 50 | 51 | -------------------------------------------------------------------------------- /include/efslam/models/SensorModel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_SENSORMODEL_H_ 35 | #define EFS_SENSORMODEL_H_ 36 | 37 | 38 | #ifndef CALC_ENTROPY 39 | # define CALC_ENTROPY 0 40 | #endif 41 | 42 | 43 | namespace efs { 44 | 45 | // Only basic template definition for further specialization 46 | template 47 | class SensorModel { 48 | 49 | }; 50 | 51 | 52 | } /* namespace efs */ 53 | 54 | 55 | #endif /* EFS_SENSORMODEL_H_ */ 56 | -------------------------------------------------------------------------------- /include/efslam/slam/BayesFastSLAM2D.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_BAYESFASTSLAM2D_H_ 35 | #define EFS_BAYESFASTSLAM2D_H_ 36 | 37 | #include "FastSLAM.h" 38 | #include "efslam/utils/PoseSE2.h" 39 | #include "efslam/maps/BayesMap2D.h" 40 | #include "efslam/models/MotionModel2D.h" 41 | 42 | namespace efs { 43 | 44 | using BayesFastSLAM2D = FastSLAM; 45 | 46 | } /* namespace efs */ 47 | 48 | #endif /* EFS_BAYESFASTSLAM2D_H_ */ 49 | -------------------------------------------------------------------------------- /include/efslam/slam/BeliefFastSLAM2D.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_BELIEFFASTSLAM2D_H_ 35 | #define EFS_BELIEFFASTSLAM2D_H_ 36 | 37 | #include "FastSLAM.h" 38 | #include "efslam/utils/PoseSE2.h" 39 | #include "efslam/maps/BeliefMap2D.h" 40 | #include "efslam/models/MotionModel2D.h" 41 | 42 | namespace efs { 43 | 44 | using BeliefFastSLAM2D = FastSLAM; 45 | 46 | } /* namespace efs */ 47 | 48 | #endif /* EFS_BELIEFFASTSLAM2D_H_ */ 49 | -------------------------------------------------------------------------------- /include/efslam/viz/Color.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_COLOR_H_ 35 | #define EFS_COLOR_H_ 36 | 37 | namespace efs { 38 | 39 | struct Color { 40 | uint8_t r, 41 | g, 42 | b; 43 | 44 | /** 45 | * @h 0 <= h < 360 46 | * @s 0 <= s <= 1 47 | * @v 0 <= v <= 1 48 | */ 49 | static inline Color hsv2rgb( double h, double s, double v ); 50 | }; 51 | 52 | 53 | } /* namespace efs */ 54 | 55 | #include "Color.hpp" 56 | 57 | #endif /* EFS_COLOR_H_ */ 58 | -------------------------------------------------------------------------------- /include/efslam/utils/ConditionalMutex.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_CONDITIONALMUTEX_H_ 35 | #define EFS_CONDITIONALMUTEX_H_ 36 | 37 | #include 38 | 39 | namespace efs { 40 | 41 | template 42 | class ConditionalMutex { 43 | public: 44 | inline void lock(); 45 | inline void unlock(); 46 | inline bool try_lock(); 47 | 48 | private: 49 | std::mutex m_mutex; 50 | }; 51 | 52 | } /* namespace efs */ 53 | 54 | #include "ConditionalMutex.hpp" 55 | 56 | #endif /* EFS_CONDITIONALMUTEX_H_ */ 57 | -------------------------------------------------------------------------------- /include/efslam/maps/storages/HStorageHelper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_HSTORAGEHELPER_H_ 35 | #define EFS_HSTORAGEHELPER_H_ 36 | 37 | namespace efs { 38 | 39 | 40 | template 41 | class HierarchicalStorageHelper { 42 | public: 43 | static inline size_t patchIndex( const Pointm &p, const Pointm &size, int magnitude ); 44 | static inline Pointm cellIndex( const Pointm &p, int magnitude ); 45 | }; 46 | 47 | 48 | } /* namespace efs */ 49 | 50 | 51 | #include "HStorageHelper.hpp" 52 | 53 | 54 | #endif /* EFS_HSTORAGEHELPER_H_ */ 55 | -------------------------------------------------------------------------------- /include/efslam/maps/AccessState.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * This file is a based on the GMapping AccessibilityStates 9 | * Copyright (c) 2004-2007, Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard 10 | * Originally licensed under the Creative Commons (Attribution-NonCommercial-ShareAlike). 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions are met: 14 | * 15 | * * Redistributions of source code must retain the above copyright notice, this 16 | * list of conditions and the following disclaimer. 17 | * 18 | * * Redistributions in binary form must reproduce the above copyright notice, 19 | * this list of conditions and the following disclaimer in the documentation 20 | * and/or other materials provided with the distribution. 21 | * 22 | * * Neither the name of Evidential FastSLAM nor the names of its 23 | * contributors may be used to endorse or promote products derived from 24 | * this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 28 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 29 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 30 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 31 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 32 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 33 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 34 | * OR TORT (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 | 38 | #ifndef EFS_ACCESSSTATE_H_ 39 | #define EFS_ACCESSSTATE_H_ 40 | 41 | namespace efs { 42 | 43 | enum AccessibilityState { 44 | AS_OUTSIDE = 0x0, 45 | AS_INSIDE = 0x1, 46 | AS_ALLOCATED = 0x2 47 | }; 48 | 49 | } /* namespace efs */ 50 | 51 | #endif /* EFS_ACCESSSTATE_H_ */ 52 | -------------------------------------------------------------------------------- /include/efslam/utils/ConditionalMutex.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | namespace efs { 35 | 36 | template 37 | void 38 | ConditionalMutex::lock() { 39 | if( ThreadSafe ) 40 | m_mutex.lock(); 41 | } 42 | 43 | 44 | template 45 | void 46 | ConditionalMutex::unlock() { 47 | if( ThreadSafe ) 48 | m_mutex.unlock(); 49 | } 50 | 51 | 52 | template 53 | bool 54 | ConditionalMutex::try_lock() { 55 | if( ThreadSafe ) 56 | return m_mutex.try_lock(); 57 | 58 | return true; 59 | } 60 | 61 | } /* namespace efs */ 62 | 63 | -------------------------------------------------------------------------------- /src/utils/Convenience.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #include 35 | #include 36 | 37 | #include "efslam/utils/Convenience.h" 38 | 39 | namespace efs { 40 | 41 | std::vector 42 | split( const std::string &s, char delim ) { 43 | // http://stackoverflow.com/questions/236129/how-to-split-a-string-in-c 44 | std::vector elems; 45 | std::istringstream ss( s ); 46 | std::string item; 47 | 48 | while( std::getline( ss, item, delim ) ) 49 | if( !item.empty() ) 50 | elems.push_back( item ); 51 | 52 | return elems; 53 | } 54 | 55 | } 56 | -------------------------------------------------------------------------------- /include/efslam/io/carmen/CarmenGtReader.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_CARMENGTREADER_H_ 35 | #define EFS_CARMENGTREADER_H_ 36 | 37 | #include "efslam/utils/PoseSE2.h" 38 | 39 | 40 | namespace efs { 41 | 42 | class CarmenGtReader { 43 | public: 44 | CarmenGtReader( std::istream &inputStream ); 45 | virtual ~CarmenGtReader(); 46 | 47 | bool nextPose( PoseSE2 *pose, std::string *timestamp = nullptr ); 48 | 49 | protected: 50 | enum { 51 | GT_TIMESTAMP = 0, 52 | GT_POSE_X, 53 | GT_POSE_Y, 54 | GT_POSE_PHI 55 | }; 56 | 57 | std::istream &m_in; 58 | }; 59 | 60 | } /* namespace efs */ 61 | 62 | #endif /* EFS_CARMENGTREADER_H_ */ 63 | -------------------------------------------------------------------------------- /include/efslam/utils/Line.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_LINE_H_ 35 | #define EFS_LINE_H_ 36 | 37 | #include 38 | 39 | #include "efslam/utils/Point.h" 40 | 41 | 42 | namespace efs { 43 | 44 | template 45 | class Line : public std::vector< Pointm > { 46 | public: 47 | Line( const Pointm &start, const Pointm &end ); 48 | virtual ~Line(); 49 | 50 | virtual void extend( const Pointm &end ); // extend the line from old end to end 51 | 52 | private: 53 | inline void bresenham( const Pointm &start, const Pointm &end ); 54 | }; 55 | 56 | 57 | } /* namespace efs */ 58 | 59 | 60 | #include "Line.hpp" 61 | 62 | #endif /* EFS_LINE_H_ */ 63 | -------------------------------------------------------------------------------- /include/efslam/slam/Particle.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | namespace efs { 35 | 36 | template 37 | Particle::Particle( const Pose &p, const Map &m, double w, TrajectoryNodePtr n ) : 38 | pose( p ), 39 | map( m ), 40 | weight( w ), 41 | logWeight( 0 ), 42 | logWeightSum( 0 ), 43 | node( n ) 44 | { 45 | // Nothing else to do here 46 | } 47 | 48 | 49 | template 50 | Particle::~Particle() { 51 | } 52 | 53 | 54 | template 55 | void 56 | Particle::newNode() { 57 | node = std::make_shared< TrajectoryNode >( pose, node, 0 ); 58 | } 59 | 60 | } /* namespace efs */ 61 | 62 | -------------------------------------------------------------------------------- /include/efslam/slam/TrajectoryNode.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * This file is partially based on the GMapping TNode class 9 | * Copyright (c) 2004-2007, Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard 10 | * Originally licensed under the Creative Commons (Attribution-NonCommercial-ShareAlike). 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions are met: 14 | * 15 | * * Redistributions of source code must retain the above copyright notice, this 16 | * list of conditions and the following disclaimer. 17 | * 18 | * * Redistributions in binary form must reproduce the above copyright notice, 19 | * this list of conditions and the following disclaimer in the documentation 20 | * and/or other materials provided with the distribution. 21 | * 22 | * * Neither the name of Evidential FastSLAM nor the names of its 23 | * contributors may be used to endorse or promote products derived from 24 | * this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 28 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 29 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 30 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 31 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 32 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 33 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 34 | * OR TORT (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 | 38 | #include 39 | 40 | namespace efs { 41 | 42 | template 43 | TrajectoryNode::TrajectoryNode( const Pose &p, Ptr n, size_t c ) : 44 | pose( p ), 45 | parent( n ), 46 | childs( c ), 47 | visitCount( 0 ) 48 | { 49 | if( parent ) 50 | parent->childs++; 51 | } 52 | 53 | 54 | template 55 | TrajectoryNode::~TrajectoryNode() 56 | { 57 | if( parent ) 58 | parent->childs--; 59 | assert( !childs ); 60 | } 61 | 62 | } /* namespace efs */ 63 | 64 | -------------------------------------------------------------------------------- /include/efslam/utils/Stopwatch.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #include "Stopwatch.h" 35 | 36 | namespace efs { 37 | 38 | Stopwatch::Stopwatch() : 39 | m_startTime( clock::now() ) 40 | { 41 | // Nothing else to do here 42 | } 43 | 44 | Stopwatch::time_point 45 | Stopwatch::now() const noexcept { 46 | return clock::now(); 47 | } 48 | 49 | 50 | void 51 | Stopwatch::reset( const time_point &time ) noexcept { 52 | m_startTime = time; 53 | } 54 | 55 | 56 | void 57 | Stopwatch::reset() noexcept { 58 | reset( now() ); 59 | } 60 | 61 | 62 | Stopwatch::milliseconds 63 | Stopwatch::timePast( const time_point &time ) const noexcept { 64 | return std::chrono::duration_cast( clock::now() - time ); 65 | } 66 | 67 | 68 | Stopwatch::milliseconds 69 | Stopwatch::timePast() const noexcept { 70 | return timePast( m_startTime ); 71 | } 72 | 73 | } /* namespace efs */ 74 | -------------------------------------------------------------------------------- /include/efslam/slam/Particle.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_PARTICLE_H_ 35 | #define EFS_PARTICLE_H_ 36 | 37 | #include "TrajectoryNode.h" 38 | 39 | namespace efs { 40 | 41 | template 42 | class Particle { 43 | public: 44 | using TrajectoryNodePtr = typename TrajectoryNode::Ptr; 45 | 46 | inline Particle( const Pose &p, const Map &m, double weight, TrajectoryNodePtr n = nullptr ); 47 | inline ~Particle(); 48 | 49 | inline void newNode(); 50 | 51 | Pose pose; 52 | Map map; 53 | 54 | double weight, // normalized weight (sums to 1 over all particles) 55 | logWeight, // sum of log weights since last resample 56 | logWeightSum; // sum of log weights over complete time 57 | 58 | TrajectoryNodePtr node; 59 | }; 60 | 61 | } /* namespace efs */ 62 | 63 | #include "Particle.hpp" 64 | 65 | #endif /* EFS_PARTICLE_H_ */ 66 | -------------------------------------------------------------------------------- /include/efslam/utils/CovarianceEllipse.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * This file is partially based on the ROS CovarianceEllipsoid class 9 | * Copyright (c) 2009, Daniel Stonier 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright notice, this 15 | * list of conditions and the following disclaimer. 16 | * 17 | * * Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * * Neither the name of Evidential FastSLAM 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 "AS IS" 26 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | #ifndef EFS_COVARIANCEELLIPSE_H_ 38 | #define EFS_COVARIANCEELLIPSE_H_ 39 | 40 | #include "Point.h" 41 | #include "Convenience.h" 42 | 43 | namespace efs { 44 | 45 | class CovarianceEllipse { 46 | public: 47 | using Matrix2w = Eigen::Matrix; 48 | 49 | inline CovarianceEllipse(); 50 | inline CovarianceEllipse( const Matrix2w &cov ); 51 | 52 | inline void compute( const Matrix2w &cov ); 53 | inline float rotation() const; 54 | 55 | protected: 56 | Matrix2w m_axes; 57 | Point2w m_lengths; 58 | 59 | public: 60 | GETTER( axes ); 61 | GETTER( lengths ); 62 | }; 63 | 64 | } /* namespace efs */ 65 | 66 | 67 | #include "CovarianceEllipse.hpp" 68 | 69 | #endif /* EFS_COVARIANCEELLIPSE_H_ */ 70 | -------------------------------------------------------------------------------- /include/efslam/utils/PointIterator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_POINTITERATOR_H_ 35 | #define EFS_POINTITERATOR_H_ 36 | 37 | #include "Point.h" 38 | 39 | namespace efs { 40 | 41 | template 42 | class PointIterator { 43 | public: 44 | PointIterator( const Pointm &end ); 45 | PointIterator( const Pointm &start, const Pointm &end ); 46 | PointIterator( const Pointm &start, const Pointm &end, const Pointm &inc ); 47 | 48 | inline operator bool() const { return !m_endReached; } 49 | inline void operator++(int); 50 | inline void operator++(); 51 | inline const Pointm& operator*() const { return m_cur; } 52 | 53 | private: 54 | Pointm m_start, 55 | m_cur, 56 | m_end, 57 | m_inc; 58 | bool m_endReached; 59 | }; 60 | 61 | } /* namespace efs */ 62 | 63 | #include "PointIterator.hpp" 64 | 65 | #endif /* EFS_POINTITERATOR_H_ */ 66 | -------------------------------------------------------------------------------- /include/efslam/slam/TrajectoryNode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * This file is partially based on the GMapping TNode class 9 | * Copyright (c) 2004-2007, Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard 10 | * Originally licensed under the Creative Commons (Attribution-NonCommercial-ShareAlike). 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions are met: 14 | * 15 | * * Redistributions of source code must retain the above copyright notice, this 16 | * list of conditions and the following disclaimer. 17 | * 18 | * * Redistributions in binary form must reproduce the above copyright notice, 19 | * this list of conditions and the following disclaimer in the documentation 20 | * and/or other materials provided with the distribution. 21 | * 22 | * * Neither the name of Evidential FastSLAM nor the names of its 23 | * contributors may be used to endorse or promote products derived from 24 | * this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 28 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 29 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 30 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 31 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 32 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 33 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 34 | * OR TORT (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 | 38 | #ifndef EFS_TRAJECTORYNODE_H_ 39 | #define EFS_TRAJECTORYNODE_H_ 40 | 41 | #include 42 | #include 43 | 44 | namespace efs { 45 | 46 | template 47 | class TrajectoryNode { 48 | public: 49 | using Ptr = std::shared_ptr< TrajectoryNode >; 50 | 51 | TrajectoryNode( const Pose &pose, Ptr parent = nullptr, size_t childs = 0 ); 52 | ~TrajectoryNode(); 53 | 54 | Pose pose; 55 | 56 | Ptr parent; 57 | size_t childs, 58 | visitCount; 59 | }; 60 | 61 | } /* namespace efs */ 62 | 63 | 64 | #include "TrajectoryNode.hpp" 65 | 66 | #endif /* EFS_TRAJECTORYNODE_H_ */ 67 | -------------------------------------------------------------------------------- /include/efslam/viz/Color.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #include 35 | 36 | namespace efs { 37 | 38 | Color 39 | Color::hsv2rgb( double h, double s, double v ) { 40 | assert( h >= 0 && h < 360 && s >= 0 && s <= 1.0 && v >= 0 && v <= 1.0 ); 41 | 42 | double c = v * s, 43 | x = c * ( 1 - fabs( fmod( h / 60.0, 2 ) - 1 ) ), 44 | m = v - c, 45 | r = 0, 46 | g = 0, 47 | b = 0; 48 | 49 | if( h < 60 ) { 50 | r = c; 51 | g = x; 52 | } else if( h < 120 ) { 53 | r = x; 54 | g = c; 55 | } else if( h < 180 ) { 56 | g = c; 57 | b = x; 58 | } else if( h < 240 ) { 59 | g = x; 60 | b = c; 61 | } else if( h < 300 ) { 62 | r = x; 63 | b = c; 64 | } else if( h < 360 ) { 65 | r = c; 66 | b = x; 67 | } else { 68 | throw std::invalid_argument( "H larger than 360" ); 69 | } 70 | 71 | return { (uint8_t)( (r + m) * 255 ), (uint8_t)((g + m) * 255), (uint8_t)( (b + m) * 255 ) }; 72 | } 73 | 74 | } /* namespace efs */ 75 | -------------------------------------------------------------------------------- /include/efslam/utils/PointCrossIterator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_POINTCROSSITERATOR_H_ 35 | #define EFS_POINTCROSSITERATOR_H_ 36 | 37 | #include "Point.h" 38 | 39 | namespace efs { 40 | 41 | template 42 | class PointCrossIterator { 43 | public: 44 | PointCrossIterator( const Pointm &start, const Pointm &end ); 45 | PointCrossIterator( const Pointm &start, const Pointm &end, const Pointm &inc ); 46 | 47 | inline operator bool() const { return !m_endReached; } 48 | inline void operator++(int); 49 | inline void operator++(); 50 | inline const Pointm& operator*() const { return m_cur; } 51 | 52 | private: 53 | Pointm m_start, 54 | m_cur, 55 | m_end, 56 | m_inc, 57 | m_mid; 58 | bool m_endReached; 59 | int m_idx, 60 | m_midIdx; 61 | }; 62 | 63 | } /* namespace efs */ 64 | 65 | #include "PointCrossIterator.hpp" 66 | 67 | #endif /* EFS_POINTCROSSITERATOR_H_ */ 68 | -------------------------------------------------------------------------------- /src/io/carmen/CarmenGtReader.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #include 35 | #include 36 | 37 | #include "efslam/io/carmen/CarmenGtReader.h" 38 | 39 | namespace efs { 40 | 41 | CarmenGtReader::CarmenGtReader( std::istream &inputStream ) : 42 | m_in( inputStream ) 43 | { 44 | 45 | } 46 | 47 | 48 | CarmenGtReader::~CarmenGtReader() { 49 | 50 | } 51 | 52 | 53 | bool 54 | CarmenGtReader::nextPose( PoseSE2 *pose, std::string *timestamp ) { 55 | bool found = false; 56 | 57 | while( m_in.good() && !found ) { 58 | std::string line; 59 | std::getline( m_in, line ); 60 | 61 | auto splitted = split( line, ' ' ); 62 | 63 | if( splitted.size() != 4 ) 64 | continue; 65 | 66 | found = true; 67 | 68 | pose->x() = std::stof( splitted[GT_POSE_X] ); 69 | pose->y() = std::stof( splitted[GT_POSE_Y] ); 70 | pose->phi() = std::stof( splitted[GT_POSE_PHI] ); 71 | 72 | if( timestamp ) 73 | *timestamp = splitted[GT_TIMESTAMP]; 74 | } 75 | 76 | return found; 77 | } 78 | 79 | 80 | } /* namespace efs */ 81 | -------------------------------------------------------------------------------- /include/efslam/utils/Scan.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_SCAN_H_ 35 | #define EFS_SCAN_H_ 36 | 37 | #include 38 | #include 39 | 40 | namespace efs { 41 | 42 | 43 | template 44 | class Scan : public std::vector { 45 | public: 46 | static constexpr int Dimension = N; 47 | using Ptr = std::shared_ptr; 48 | using ConstPtr = std::shared_ptr; 49 | 50 | inline void updateCenterAndRadius() { 51 | m_center.setZero(); 52 | m_radius = 0; 53 | for( const auto &p : *this ) { 54 | m_center += p; 55 | m_radius += p.norm(); 56 | } 57 | m_center /= std::vector::size(); 58 | m_radius /= std::vector::size(); 59 | } 60 | 61 | inline const PointT& center() const { return m_center; } 62 | inline const world_t& radius() const { return m_radius; } 63 | 64 | protected: 65 | PointT m_center; 66 | world_t m_radius; 67 | }; 68 | 69 | 70 | 71 | } /* namespace efs */ 72 | 73 | 74 | #endif /* EFS_SCAN_H_ */ 75 | -------------------------------------------------------------------------------- /include/efslam/utils/Stopwatch.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_STOPWATCH_H_ 35 | #define EFS_STOPWATCH_H_ 36 | 37 | #include 38 | 39 | #include "Log.h" 40 | 41 | namespace efs { 42 | 43 | class Stopwatch { 44 | public: 45 | using clock = std::chrono::high_resolution_clock; 46 | using time_point = clock::time_point; 47 | using milliseconds = std::chrono::milliseconds; 48 | 49 | inline Stopwatch(); 50 | 51 | inline time_point now() const noexcept; 52 | inline void reset( const time_point &time ) noexcept; 53 | inline void reset() noexcept; 54 | 55 | inline milliseconds timePast( const time_point &time ) const noexcept; 56 | inline milliseconds timePast() const noexcept; 57 | 58 | protected: 59 | time_point m_startTime; 60 | }; 61 | 62 | 63 | std::ostream& operator<<( std::ostream& ostr, const Stopwatch::milliseconds &ms ){ 64 | return ostr << BOLD << ms.count() / 1000.0 << "s" NORMAL; 65 | } 66 | 67 | } /* namespace efs */ 68 | 69 | #include "Stopwatch.hpp" 70 | 71 | #endif /* EFS_STOPWATCH_H_ */ 72 | -------------------------------------------------------------------------------- /include/efslam/utils/Version.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_VERSION_H_ 35 | #define EFS_VERSION_H_ 36 | 37 | #include 38 | #include 39 | 40 | namespace efs { 41 | 42 | class Version { 43 | public: 44 | static std::string name(); 45 | static std::string description(); 46 | static std::string nameDescription(); 47 | 48 | static std::string author(); 49 | static std::string email(); 50 | static std::string coauthors(); 51 | static std::string contact(); 52 | 53 | static std::string company(); 54 | static std::string department(); 55 | 56 | static std::string copyright(); 57 | 58 | static std::string version(); 59 | static std::string buildDate(); 60 | 61 | static std::string commitHash(); 62 | static std::string commitDate(); 63 | 64 | static std::string branchName(); 65 | 66 | static std::string info(); 67 | static std::vector infoLines(); 68 | }; 69 | 70 | } /* namespace efs */ 71 | 72 | #endif /* EFS_VERSION_H_ */ 73 | -------------------------------------------------------------------------------- /include/efslam/models/MotionModel2D.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * This file is partially based on the GMapping MotionModel class 9 | * Copyright (c) 2004-2007, Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard 10 | * Originally licensed under the Creative Commons (Attribution-NonCommercial-ShareAlike). 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions are met: 14 | * 15 | * * Redistributions of source code must retain the above copyright notice, this 16 | * list of conditions and the following disclaimer. 17 | * 18 | * * Redistributions in binary form must reproduce the above copyright notice, 19 | * this list of conditions and the following disclaimer in the documentation 20 | * and/or other materials provided with the distribution. 21 | * 22 | * * Neither the name of Evidential FastSLAM nor the names of its 23 | * contributors may be used to endorse or promote products derived from 24 | * this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 28 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 29 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 30 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 31 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 32 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 33 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 34 | * OR TORT (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 | 38 | #ifndef EFS_MOTIONMODEL2D_H_ 39 | #define EFS_MOTIONMODEL2D_H_ 40 | 41 | #include "MotionModel.h" 42 | #include "efslam/utils/PoseSE2.h" 43 | 44 | namespace efs { 45 | 46 | template<> 47 | class MotionModel { 48 | public: 49 | MotionModel(); 50 | virtual ~MotionModel(); 51 | 52 | void setParamsConfig(); 53 | 54 | inline PoseSE2 draw( const PoseSE2 &pose, double linearMove, double angularMove ) const; 55 | inline PoseSE2 draw( const PoseSE2 &pose, const PoseSE2 &oldPose, const PoseSE2 &newPose ) const; 56 | 57 | protected: 58 | double m_transTransSigma, 59 | m_transRotSigma, 60 | m_rotRotSigma, 61 | m_rotTransSigma; 62 | }; 63 | 64 | } /* namespace efs */ 65 | 66 | #include "MotionModel2D.hpp" 67 | 68 | #endif /* EFS_MOTIONMODEL2D_H_ */ 69 | -------------------------------------------------------------------------------- /include/efslam/utils/PoseBase.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_POSEBASE_HPP_ 35 | #define EFS_POSEBASE_HPP_ 36 | 37 | #include "PoseSE2.h" 38 | 39 | 40 | namespace efs { 41 | 42 | // Declaration of specialized methods 43 | template<> 44 | inline Pointw<2> 45 | PoseBase::pos<2>() const; 46 | 47 | 48 | template 49 | typename PoseSE::Type 50 | PoseBase::toPoseSE() const { 51 | static_assert( N == 2, "N must be 2." ); 52 | } 53 | 54 | 55 | template<> 56 | inline typename PoseSE<2>::Type 57 | PoseBase::toPoseSE<2>() const { 58 | return this->toPoseSE2(); 59 | } 60 | 61 | 62 | PoseBase::operator PoseSE2() const { 63 | return this->toPoseSE2(); 64 | } 65 | 66 | 67 | PoseBase::operator Eigen::Isometry2d() const { 68 | return EigenIsometry2w( *this ).cast(); 69 | } 70 | 71 | 72 | template 73 | Pointw 74 | PoseBase::pos() const { 75 | static_assert( N == 2, "N must be 2." ); 76 | } 77 | 78 | 79 | template<> 80 | inline Pointw<2> 81 | PoseBase::pos<2>() const { 82 | return this->pos2D(); 83 | } 84 | 85 | 86 | } /* namespace efs */ 87 | 88 | #endif /* EFS_POSEBASE_HPP_ */ 89 | -------------------------------------------------------------------------------- /include/efslam/maps/cells/CellBase.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_CELLBASE_H_ 35 | #define EFS_CELLBASE_H_ 36 | 37 | #include "efslam/utils/Convenience.h" 38 | 39 | namespace efs { 40 | 41 | class CellBase { 42 | public: 43 | enum TypeE { 44 | _CELL_TYPE_MIN_ = -1, 45 | CELL_POINT_ACCUMULATOR, 46 | CELL_BAYES, 47 | CELL_BELIEF, 48 | _CELL_TYPE_MAX_ 49 | }; 50 | 51 | virtual inline constexpr TypeE type() const { return TypeE::_CELL_TYPE_MIN_; } 52 | static inline constexpr TypeE staticType() { return TypeE::_CELL_TYPE_MIN_; } 53 | 54 | static inline TypeE getMapType( std::istream &f ); 55 | }; 56 | 57 | 58 | CellBase::TypeE 59 | CellBase::getMapType( std::istream &f ) { 60 | TypeE mapType = _CELL_TYPE_MIN_; 61 | std::string line; 62 | 63 | while( std::getline( f, line ) ) { 64 | auto splitted = split( line, ':' ); 65 | if( splitted[0] == "cell" ) { 66 | mapType = (CellBase::TypeE) stol( splitted[1] ); 67 | break; 68 | } 69 | } 70 | f.clear(); 71 | f.seekg( 0, std::ios::beg ); 72 | 73 | return mapType; 74 | } 75 | 76 | 77 | } /* namespace efs */ 78 | 79 | #endif /* EFS_CELLBASE_H_ */ 80 | -------------------------------------------------------------------------------- /include/efslam/utils/rawptr.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_RAWPTR_H_ 35 | #define EFS_RAWPTR_H_ 36 | 37 | namespace efs { 38 | 39 | template 40 | class raw_ptr { 41 | public: 42 | inline raw_ptr() : ptr( nullptr ) {} 43 | inline raw_ptr( T *p ) : ptr( p ) {}; 44 | inline raw_ptr( const raw_ptr &other ) : ptr( other.ptr ) {} 45 | inline raw_ptr( raw_ptr &&other ) : ptr( std::move( other.ptr ) ) {} 46 | inline ~raw_ptr() {}; 47 | 48 | inline raw_ptr& operator=( const raw_ptr &other ) { if( this != &other ) { ptr = other.ptr; }; return *this; } 49 | inline raw_ptr& operator=( raw_ptr &&other ) { ptr = std::move( other.ptr ); return *this; } 50 | 51 | inline T& operator*() noexcept { return *ptr; } 52 | inline T* operator->() noexcept { return ptr; } 53 | inline T* get() noexcept { return ptr; } 54 | 55 | inline const T& operator*() const noexcept { return *ptr; } 56 | inline const T* operator->() const noexcept { return ptr; } 57 | inline const T* get() const noexcept { return ptr; } 58 | 59 | inline operator bool() const noexcept { return (bool) ptr; } 60 | 61 | private: 62 | T *ptr; 63 | }; 64 | 65 | } /* namespace efs */ 66 | 67 | #endif /* EFS_RAWPTR_H_ */ 68 | -------------------------------------------------------------------------------- /include/efslam/utils/PoseBase.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_POSEBASE_H_ 35 | #define EFS_POSEBASE_H_ 36 | 37 | #include "Point.h" 38 | 39 | 40 | namespace efs { 41 | 42 | // Forward declaration 43 | class PoseSE2; 44 | 45 | template 46 | struct PoseSE { 47 | }; 48 | 49 | template<> 50 | struct PoseSE<2> { 51 | using Type = PoseSE2; 52 | }; 53 | 54 | 55 | class PoseBase { 56 | public: 57 | using EigenIsometry2w = Eigen::Transform; 58 | 59 | inline PoseBase() {}; 60 | virtual inline ~PoseBase() {}; 61 | 62 | template 63 | inline typename PoseSE::Type toPoseSE() const; 64 | inline operator PoseSE2() const; 65 | virtual PoseSE2 toPoseSE2() const = 0; 66 | 67 | virtual operator EigenIsometry2w() const = 0; 68 | virtual inline operator Eigen::Isometry2d() const; 69 | 70 | template 71 | inline Pointw pos() const; 72 | virtual Point2w pos2D() const = 0; 73 | 74 | virtual double posNorm() const = 0; 75 | virtual double angNorm() const = 0; 76 | 77 | 78 | virtual inline int dimension() const noexcept { return 0; } 79 | }; 80 | 81 | } /* namespace efs */ 82 | 83 | #ifndef NO_HPP 84 | #include "PoseBase.hpp" 85 | #endif 86 | 87 | #endif /* EFS_POSEBASE_H_ */ 88 | -------------------------------------------------------------------------------- /include/efslam/viz/CellColor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_CELLCOLOR_H_ 35 | #define EFS_CELLCOLOR_H_ 36 | 37 | #include "Color.h" 38 | 39 | #include "efslam/maps/cells/BayesCell.h" 40 | #include "efslam/maps/cells/BeliefCell.h" 41 | 42 | 43 | namespace efs { 44 | 45 | enum ColorModeE { 46 | _COLOR_MODE_MIN_ = -1, 47 | COLOR_MODE_DEFAULT, 48 | COLOR_MODE_ALPHA_EMPTY, 49 | COLOR_MODE_ALPHA_THETA, 50 | COLOR_MODE_ALPHA_NOT_OCCUPIED, 51 | _COLOR_MODE_MAX_ 52 | }; 53 | 54 | 55 | template 56 | class CellColor { 57 | }; 58 | 59 | 60 | template 61 | class CellColor< BayesCell > { 62 | public: 63 | static inline Color getColor( const BayesCell &cell, bool normalize = false, uint8_t *alpha = nullptr, ColorModeE = COLOR_MODE_DEFAULT, double alphaScale = 1.0 ); 64 | static inline constexpr Color defaultColor(); 65 | }; 66 | 67 | 68 | template 69 | class CellColor< BeliefCell > { 70 | public: 71 | static inline Color getColor( const BeliefCell &cell, bool normalize = false, uint8_t *alpha = nullptr, ColorModeE = COLOR_MODE_DEFAULT, double alphaScale = 1.0 ); 72 | static inline constexpr Color defaultColor(); 73 | }; 74 | 75 | } /* namespace efs */ 76 | 77 | #include "CellColor.hpp" 78 | 79 | #endif /* EFS_CELLCOLOR_H_ */ 80 | -------------------------------------------------------------------------------- /include/efslam/utils/PointIterator.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | namespace efs { 35 | 36 | 37 | template 38 | PointIterator::PointIterator( const Pointm &end ) : 39 | m_start( Pointm::Zero() ), 40 | m_cur( m_start ), 41 | m_end( end ), 42 | m_inc( Pointm::Ones() ), 43 | m_endReached( m_start == end ) 44 | { 45 | } 46 | 47 | 48 | template 49 | PointIterator::PointIterator( const Pointm &start, const Pointm &end ) : 50 | m_start( start ), 51 | m_cur( start ), 52 | m_end( end ), 53 | m_inc( Pointm::Ones() ), 54 | m_endReached( start == end ) 55 | { 56 | } 57 | 58 | 59 | template 60 | PointIterator::PointIterator( const Pointm &start, const Pointm &end, const Pointm &inc ) : 61 | m_start( start ), 62 | m_cur( start ), 63 | m_end( end ), 64 | m_inc( inc ), 65 | m_endReached( start == end ) 66 | { 67 | } 68 | 69 | 70 | template 71 | void 72 | PointIterator::operator++( int ) { 73 | ++(*this); 74 | } 75 | 76 | 77 | template 78 | void 79 | PointIterator::operator++() { 80 | if( m_endReached ) 81 | return; 82 | 83 | for( int i = N-1; i >= 0; i-- ) { 84 | m_cur[i] += m_inc[i]; 85 | if( m_cur[i] < m_end[i] ) { 86 | break; 87 | } else { 88 | if( i == 0 ) 89 | m_endReached = true; 90 | m_cur[i] = m_start[i]; 91 | } 92 | } 93 | } 94 | 95 | } /* namespace efs */ 96 | -------------------------------------------------------------------------------- /include/efslam/io/carmen/CarmenProcessor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_CARMENPROCESSOR_H_ 35 | #define EFS_CARMENPROCESSOR_H_ 36 | 37 | #include 38 | 39 | #include "CarmenReader.h" 40 | #include "CarmenGtReader.h" 41 | 42 | namespace efs { 43 | 44 | template 45 | class CarmenProcessor { 46 | public: 47 | using PointType = typename SLAM::PointType; 48 | using Scan = typename SLAM::ScanType; 49 | using Trajectory = typename SLAM::Trajectory; 50 | 51 | CarmenProcessor( SLAM *slam, std::istream &inputStream, std::istream *gtInputStream = nullptr ); 52 | ~CarmenProcessor(); 53 | 54 | inline uint64_t processAll(); 55 | inline bool processNext( bool *scanProcessed = nullptr ); 56 | 57 | protected: 58 | SLAM *m_slam; 59 | CarmenReader m_reader; 60 | CarmenGtReader *m_gtReader; 61 | 62 | double m_laserStartAngle, 63 | m_laserAngularRes; 64 | bool m_first; 65 | PoseSE2 m_lastPose, 66 | m_lastGtPose, 67 | m_lastEstPose; 68 | 69 | std::list m_timestamps; 70 | std::list m_gtTrajectory; 71 | std::list m_poseError; 72 | 73 | public: 74 | GETTER( timestamps ); 75 | GETTER( gtTrajectory ); 76 | GETTER( poseError ); 77 | 78 | private: 79 | CarmenProcessor( const CarmenProcessor &other ) {}; // shall not be used 80 | }; 81 | 82 | } /* namespace efs */ 83 | 84 | #include "CarmenProcessor.hpp" 85 | 86 | #endif /* EFS_CARMENPROCESSOR_H_ */ 87 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 2.8) 2 | PROJECT( EFSLAM ) 3 | 4 | set(EFSLAM_MAJOR_VERSION 3) 5 | set(EFSLAM_MINOR_VERSION 0) 6 | set(EFSLAM_PATCH_VERSION 0) 7 | set(EFSLAM_VERSION ${EFSLAM_MAJOR_VERSION}.${EFSLAM_MINOR_VERSION}.${EFSLAM_PATCH_VERSION}) 8 | set(EFSLAM_SOVERSION ${EFSLAM_MAJOR_VERSION}.${EFSLAM_MINOR_VERSION}) 9 | 10 | if(COMMAND cmake_policy) 11 | cmake_policy(SET CMP0003 NEW) 12 | endif(COMMAND cmake_policy) 13 | 14 | set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") 15 | 16 | # compiler settings and flags 17 | include(CompilerSettings) 18 | 19 | 20 | #------------- 21 | # OpenMP 22 | #------------- 23 | set(EFSLAM_OMP TRUE CACHE BOOL "Enable/disable OpenMP parallelization") 24 | 25 | if(DEFINED ENV{EFSLAM_OMP}) 26 | set(EFSLAM_OMP $ENV{EFSLAM_OMP}) 27 | endif(DEFINED ENV{EFSLAM_OMP}) 28 | 29 | if(EFSLAM_OMP) 30 | find_package(OpenMP REQUIRED) 31 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 32 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 33 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 34 | endif(EFSLAM_OMP) 35 | 36 | 37 | #------------- 38 | # std::thread 39 | #------------- 40 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") 41 | 42 | 43 | #------------- 44 | # Qt4 45 | #------------- 46 | #set(CMAKE_AUTOMOC ON) 47 | #set(CMAKE_INCLUDE_CURRENT_DIR ON) 48 | find_package(Qt4 REQUIRED QtGui QtCore) 49 | include( ${QT_USE_FILE} ) 50 | 51 | 52 | #------------- 53 | # Eigen 54 | #------------- 55 | find_package(Eigen3) 56 | add_definitions( -DHAVE_EIGEN ) 57 | if(NOT EIGEN3_FOUND) 58 | FIND_PACKAGE(PkgConfig) 59 | pkg_check_modules(EIGEN3 eigen3 REQUIRED) 60 | endif() 61 | if(NOT EIGEN3_FOUND) 62 | message(FATAL_ERROR "Please install Eigen3.") 63 | endif() 64 | 65 | 66 | #------------- 67 | # Directories 68 | #------------- 69 | include_directories( 70 | ${INCLUDE_DIRS} 71 | ${CMAKE_CURRENT_BINARY_DIR} 72 | ${CMAKE_CURRENT_SOURCE_DIR} 73 | ${EIGEN3_INCLUDE_DIR} 74 | ${CMAKE_CURRENT_SOURCE_DIR}/include 75 | ) 76 | 77 | 78 | link_directories( 79 | ${CMAKE_LIBRARY_OUTPUT_DIRECTORY} 80 | ) 81 | 82 | #-------------- 83 | # Installation 84 | #-------------- 85 | set( BASE_DIR ${CMAKE_SOURCE_DIR} ) 86 | set( CMAKE_LIBRARY_OUTPUT_DIRECTORY ${BASE_DIR}/lib ) 87 | set( CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${BASE_DIR}/lib ) 88 | set( CMAKE_RUNTIME_OUTPUT_DIRECTORY ${BASE_DIR}/bin ) 89 | 90 | set(INSTALL_TARGETS_DEFAULT_ARGS 91 | RUNTIME DESTINATION bin 92 | LIBRARY DESTINATION lib 93 | ARCHIVE DESTINATION lib 94 | ) 95 | 96 | file(MAKE_DIRECTORY ${BASE_DIR}/results) 97 | 98 | install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/efslam/ 99 | DESTINATION include/efslam/ 100 | FILES_MATCHING PATTERN "*.h") 101 | install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/efslam/ 102 | DESTINATION include/efslam/ 103 | FILES_MATCHING PATTERN "*.hpp") 104 | 105 | 106 | add_subdirectory( src ) 107 | 108 | 109 | # Finished 110 | message("\n") 111 | message(STATUS "Compile EFSLAM using: make") 112 | message(STATUS "Install EFSLAM using: make install") 113 | message(STATUS " (be sure to set the correct CMAKE_INSTALL_PREFIX before)") 114 | message("\n") 115 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Evidential FastSLAM 2 | An evidential approach to simultaneous localization and mapping 3 | 4 | ### Authors 5 | 6 | Joachim Clemens, Thomas Reineking, Tobias Kluth 7 | 8 | ### Description 9 | 10 | The Evidential FastSLAM algorithm represents the world using a grid map and utilizes a Rao-Blackwellized particle filter in order to approximate the joint distribution of path and map. 11 | In contrast to other grid-map-based SLAM approaches, a belief function is used instead of a single occupancy probability to model the state of a grid cell. 12 | It allows one to assign mass not only to the singletons of the hypotheses space, but also to all subsets. 13 | As a consequence, the algorithm is able to express the uncertainty in the map more explicitly and one can distinguish between different uncertainty dimensions that are indistinguishable in a probabilistic grid map. 14 | This additional information can be used for navigation tasks like path planning or active exploration. 15 | 16 | ### Paper Describing the Approach 17 | 18 | Joachim Clemens, Thomas Reineking, Tobias Kluth, *An evidential approach to SLAM, path planning, and active exploration*, International Journal of Approximate Reasoning, volume 73, 2016, pages 1-26, [doi:10.1016/j.ijar.2016.02.003](http://dx.doi.org/10.1016/j.ijar.2016.02.003). 19 | 20 | 21 | ### Example Maps 22 | 23 | Resulting maps for the Intel Research Lab (raw dataset recorded by Dirk Hähnel) and Cartesium building, University of Bremen (raw dataset recorded by Cyrill Stachniss): 24 | 25 | Intel 26 | Cartesium 27 | 28 | The color coding is red for occupied, green for free, blue for the superset (corresponding to unknown areas), and black for empty set (corresponding to conflicts). 29 | The estimated path is shown in yellow. 30 | 31 | ### Software Requirements 32 | 33 | The software is developed and tested on Ubuntu Linux 14.04. 34 | It should run on other recent Linux and UNIX systems as well, while some modifications may be required to run it on Windows. 35 | Furthermore, the following software is required: 36 | 37 | * CMake (package `cmake`) 38 | * Qt4 (package `libqt4-dev`) 39 | * Eigen3 (package `libeigen3-dev`) 40 | 41 | ### Compilation and Running 42 | 43 | Once all required software is installed, the code can be compiled and executed as follows: 44 | 45 | ``` 46 | git clone https://github.com/JoachimClemens/Evidential-FastSLAM.git 47 | cd Evidential-FastSLAM 48 | mkdir build 49 | cd build 50 | cmake .. 51 | make 52 | cd ../bin 53 | ./EFSlamCarmenGui --filename 54 | ``` 55 | 56 | The input file has to be in CARMEN log format. 57 | Be sure that you configure the sensor parameters with `--laser-start-angle` (default: -90.0) and `--laser-angular-res` (default: 1.0) correctly, which are both given in degrees. 58 | Datasets are, e.g., provided by [Cyrill Stachniss](http://www2.informatik.uni-freiburg.de/~stachnis/datasets.html), while one of the most popular datasets might be the one of the Intel Research Lab recorded by Dirk Hähnel (http://www2.informatik.uni-freiburg.de/~stachnis/datasets/datasets/intel-lab/intel.log.gz, see above). 59 | 60 | 61 | ### License Information 62 | 63 | Evidential FastSLAM is published under the BSD License. See [LICENSE](LICENSE) for further information. 64 | -------------------------------------------------------------------------------- /include/efslam/utils/BeliefFunction.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_BELIEFFUNCTION_H_ 35 | #define EFS_BELIEFFUNCTION_H_ 36 | 37 | namespace efs { 38 | 39 | struct BeliefFunction { 40 | inline BeliefFunction(); 41 | inline BeliefFunction( float Theta, float o, float e ); 42 | 43 | inline float& operator[]( size_t i ) { return m[i]; }; 44 | inline const float& operator[]( size_t i ) const { return m[i]; }; 45 | 46 | inline float emptyset() const; 47 | 48 | inline float normalize(); 49 | inline BeliefFunction normalized() const; 50 | 51 | inline void conjunctive( const BeliefFunction &other ); 52 | inline void dempster( const BeliefFunction &other ); 53 | 54 | inline float pignistic() const; 55 | inline float internalConflict() const; 56 | 57 | inline void set( float Theta, float o, float e ); 58 | 59 | inline bool operator==( const BeliefFunction &other ) const noexcept; 60 | inline bool operator!=( const BeliefFunction &other ) const noexcept; 61 | 62 | // TODO: Implement other uncertainty measures 63 | 64 | union { 65 | float m[3]; 66 | struct { 67 | float Theta, 68 | o, 69 | e; 70 | }; 71 | }; 72 | 73 | friend std::ostream& operator<<( std::ostream& ostr, const BeliefFunction* b ) { 74 | return operator<<( ostr, *b ); 75 | } 76 | friend std::ostream& operator<<( std::ostream& ostr, const BeliefFunction& b ) { 77 | ostr << "T: " << b.Theta << " o: " << b.o << " e: " << b.e; 78 | return ostr; 79 | } 80 | 81 | }; 82 | 83 | } /* namespace efs */ 84 | 85 | #include "BeliefFunction.hpp" 86 | 87 | #endif /* EFS_BELIEFFUNCTION_H_ */ 88 | -------------------------------------------------------------------------------- /include/efslam/maps/storages/HashMap.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_HASHMAP_H_ 35 | #define EFS_HASHMAP_H_ 36 | 37 | #include 38 | #include 39 | 40 | #include "efslam/utils/Point.h" 41 | 42 | 43 | namespace efs { 44 | 45 | template 46 | class HashMap { 47 | public: 48 | static constexpr int Dimension = N; 49 | template 50 | using CellList = std::list< std::pair, const CellOut &> >; 51 | 52 | HashMap( const Pointm &size ); 53 | HashMap( const HashMap &other ); 54 | HashMap( HashMap &&other ); 55 | ~HashMap(); 56 | 57 | inline void clear(); 58 | 59 | inline HashMap& operator=( const HashMap &other ); 60 | inline HashMap& operator=( HashMap &&other ); 61 | 62 | inline const Cell& cell( const Pointm& p ) const; 63 | inline Cell& cell( const Pointm& p ); 64 | 65 | template 66 | inline CellList cells( const Pointm &offset = Pointm::Zero() ) const; 67 | 68 | inline uint32_t prune() { return 0; } // nothing to prune 69 | 70 | inline size_t bytes() const; 71 | 72 | inline AccessibilityState cellState( const Pointm& p ) const; 73 | inline bool isInside( const Pointm& p ) const; 74 | inline bool isAllocated( const Pointm &p ) const; 75 | 76 | inline Pointm size() const { return m_size; } 77 | 78 | protected: 79 | Pointm m_size; 80 | std::unordered_map, Cell> m_map; 81 | static const Cell sm_emptyValue; 82 | }; 83 | 84 | } /* namespace efs */ 85 | 86 | #include "HashMap.hpp" 87 | 88 | #endif /* EFS_HASHMAP_H_ */ 89 | -------------------------------------------------------------------------------- /include/efslam/utils/Log.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_LOG_H_ 35 | #define EFS_LOG_H_ 36 | 37 | 38 | #ifndef NCOLOR 39 | # define BLACK "\033[30m" 40 | # define RED "\033[31m" 41 | # define GREEN "\033[32m" 42 | # define YELLOW "\033[33m" 43 | # define BLUE "\033[34m" 44 | # define PINK "\033[35m" 45 | # define TURQ "\033[36m" 46 | # define WHITE "\033[37m" 47 | # define UNDERLINE "\033[4m" 48 | # define DIM "\033[2m]" 49 | # define BOLD "\033[1m" 50 | # define NORMAL "\033[0m" 51 | #else 52 | # define BLACK "" 53 | # define RED "" 54 | # define GREEN "" 55 | # define YELLOW "" 56 | # define BLUE "" 57 | # define PINK "" 58 | # define TURQ "" 59 | # define WHITE "" 60 | # define UNDERLINE "" 61 | # define DIM "" 62 | # define BOLD "" 63 | # define NORMAL "" 64 | #endif 65 | 66 | #include 67 | // no debug output if not in debug mode: 68 | #ifdef NDEBUG 69 | # ifndef B3LSAM_NDEBUG_OUT 70 | # define B3LSAM_NDEBUG_OUT 71 | # endif 72 | #endif 73 | #ifdef B3LSAM_NDEBUG_OUT 74 | # define l_dbg( ... ) (void)0 75 | #else 76 | # define l_dbg( args ) std::cout << BLUE BOLD "DEBUG:" << __FILE__ << "," << __LINE__ << NORMAL ": " << args << std::endl 77 | #endif 78 | 79 | #define l_inf( args ) std::cout << GREEN BOLD "INFO" NORMAL ": " << args << std::endl 80 | #define l_wrn( args ) std::cerr << YELLOW BOLD "WARNING" NORMAL ": " << args << std::endl 81 | #define l_err( args ) std::cerr << RED BOLD "ERROR" NORMAL ": " << args << std::endl 82 | #define l_ftl( args ) std::cerr << RED BOLD "FATAL" NORMAL ": " << args << std::endl 83 | 84 | 85 | #define DEBUG_PRINT( x ) l_dbg( #x"=" << x ) 86 | #define DEBUG_PRINT_COUT( x ) std::cout << #x"=" << x << std::endl 87 | 88 | 89 | #endif /* EFS_LOG_H_ */ 90 | -------------------------------------------------------------------------------- /include/efslam/utils/PointComp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_POINTCOMP_H_ 35 | #define EFS_POINTCOMP_H_ 36 | 37 | #include "Point.h" 38 | 39 | namespace efs { 40 | 41 | // general version 42 | template 43 | struct PointComperator { 44 | bool operator()( const Point& a, const Point& b ) const { 45 | for( int i = 0; i < N; i++ ) { 46 | if( a[i] < b[i] ) 47 | return true; 48 | else if( a[i] > b[i] ) 49 | return false; 50 | } 51 | return false; 52 | } 53 | }; 54 | 55 | // partially specialized for points of size 2 56 | template 57 | struct PointComperator { 58 | bool operator()( const Point& a, const Point& b ) const { 59 | return (a[0] < b[0]) || ( (a[0] == b[0]) && (a[1] < b[1]) ); 60 | } 61 | }; 62 | 63 | // partially specialized for points of size 3 64 | template 65 | struct PointComperator { 66 | bool operator()( const Point& a, const Point& b ) const { 67 | return (a[0] < b[0]) || ( (a[0] == b[0]) && (a[1] < b[1]) ) || ( (a[0] == b[0]) && (a[1] == b[1]) && (a[2] < b[2]) ); 68 | } 69 | }; 70 | 71 | 72 | using PointComperator2i = PointComperator; 73 | using PointComperator2d = PointComperator; 74 | using PointComperator2f = PointComperator; 75 | using PointComperator2w = PointComperator; 76 | using PointComperator2m = PointComperator; 77 | 78 | using PointComperator3i = PointComperator; 79 | using PointComperator3d = PointComperator; 80 | using PointComperator3f = PointComperator; 81 | using PointComperator3w = PointComperator; 82 | using PointComperator3m = PointComperator; 83 | 84 | } /* namespace efs */ 85 | 86 | #endif /* EFS_POINTCOMP_H_ */ 87 | -------------------------------------------------------------------------------- /include/efslam/maps/cells/BayesCell.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_BAYES_CELL_H_ 35 | #define EFS_BAYES_CELL_H_ 36 | 37 | #include "PointAccumulator.h" 38 | 39 | namespace efs { 40 | 41 | 42 | template 43 | class BayesCell : public PointAccumulator { 44 | public: 45 | using typename PointAccumulator::PointNw; 46 | 47 | inline BayesCell( int i = 0 ); 48 | inline BayesCell( const std::string &str ); 49 | inline virtual ~BayesCell(); 50 | 51 | inline void updateNoHit(); 52 | inline void updateHit( const PointNw &p ); 53 | inline void integrate( const BayesCell &c ); 54 | 55 | inline double fullness() const; 56 | inline double entropy() const; 57 | 58 | inline bool operator==( const BayesCell &other ) const noexcept; 59 | 60 | inline bool pruneEqual( const BayesCell &other ) const noexcept; 61 | 62 | inline void fromStr( const std::string &str ); 63 | inline std::string toStr() const; 64 | 65 | virtual inline size_t bytes() const noexcept; 66 | 67 | virtual inline constexpr CellBase::TypeE type() const { return CellBase::TypeE::CELL_BAYES; } 68 | static inline constexpr CellBase::TypeE staticType() { return CellBase::TypeE::CELL_BAYES; } 69 | 70 | friend std::ostream& operator<<( std::ostream& ostr, const BayesCell* c ){ 71 | return operator<<( ostr,(*c) ); 72 | } 73 | 74 | friend std::ostream& operator<<( std::ostream& ostr, const BayesCell& c ){ 75 | if( c.m_visits ) 76 | ostr << c.fullness(); 77 | else 78 | ostr << "0.5"; 79 | return ostr; 80 | } 81 | 82 | protected: 83 | using Acc = typename PointAccumulator::Acc; 84 | 85 | uint32_t m_visits; 86 | 87 | public: 88 | GETTER( visits ); 89 | }; 90 | 91 | 92 | } /* namespace efs */ 93 | 94 | #include "BayesCell.hpp" 95 | 96 | #endif /* EFS_BAYES_CELL_H_ */ 97 | -------------------------------------------------------------------------------- /include/efslam/utils/CovarianceEllipse.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * This file is partially based on the ROS CovarianceEllipsoid class 9 | * Copyright (c) 2009, Daniel Stonier 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright notice, this 15 | * list of conditions and the following disclaimer. 16 | * 17 | * * Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * * Neither the name of Evidential FastSLAM 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 "AS IS" 26 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | namespace efs { 38 | 39 | CovarianceEllipse::CovarianceEllipse() : 40 | m_axes( Matrix2w::Identity() ), 41 | m_lengths( Point2w::Ones() ) 42 | { 43 | 44 | } 45 | 46 | 47 | CovarianceEllipse::CovarianceEllipse( const Matrix2w &cov ) { 48 | compute( cov ); 49 | } 50 | 51 | 52 | void 53 | CovarianceEllipse::compute( const Matrix2w &cov ) { 54 | // Based on ROS' ecl::CovarianceEllipsoid2f 55 | 56 | // Eigenvalues 57 | float a = cov( 0, 0 ), 58 | b = cov( 0, 1 ), 59 | c = cov( 1, 0 ), 60 | d = cov( 1, 1 ); 61 | 62 | float tmp = sqrtf( (a+d)*(a+d) / 4 - a*d + b*c ); 63 | m_lengths << sqrtf( (a+d)/2 + tmp ), sqrtf( (a+d)/2 - tmp ); 64 | 65 | 66 | // Eigenvectors 67 | if( c != 0 ) { 68 | m_axes( 0, 0 ) = m_lengths( 0 )*m_lengths( 0 ) - d; 69 | m_axes( 1, 0 ) = c; 70 | m_axes( 0, 1 ) = m_lengths( 1 )*m_lengths( 1 ) - d; 71 | m_axes( 1, 1 ) = c; 72 | } else if( b != 0 ) { 73 | m_axes( 0, 0 ) = b; 74 | m_axes( 1, 0 ) = m_lengths( 0 )*m_lengths( 0 ) - a; 75 | m_axes( 0, 1 ) = b; 76 | m_axes( 1, 1 ) = m_lengths( 1 )*m_lengths( 1 ) - a; 77 | } else { 78 | if( a > d ) { 79 | m_axes << 1, 0, 80 | 0, 1; 81 | } else { 82 | m_axes << 0, 1, 83 | 1, 0; 84 | } 85 | } 86 | 87 | // Normalize Evectors 88 | m_axes.block<2,1>( 0, 0 ).normalize(); 89 | m_axes.block<2,1>( 0, 1 ).normalize(); 90 | } 91 | 92 | 93 | float 94 | CovarianceEllipse::rotation() const { 95 | return atan2f( (float) m_axes( 1, 0 ), (float) m_axes( 0, 0 ) ); 96 | } 97 | 98 | 99 | } /* namespace efs */ 100 | 101 | -------------------------------------------------------------------------------- /include/efslam/utils/PointCrossIterator.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | namespace efs { 35 | 36 | template 37 | PointCrossIterator::PointCrossIterator( const Pointm &start, const Pointm &end ) : 38 | m_start( start ), 39 | m_end( end ), 40 | m_inc( Pointm::Ones() ), 41 | m_endReached( start == end ), 42 | m_idx( N-1 ), 43 | m_midIdx( round( m_idx / 2.0 ) ) 44 | { 45 | for( int i = 0; i < N; i++ ) 46 | m_mid[i] = m_start[i] + round( (m_end[i] - m_start[i] - 1) / 2.0 ); // -1 is because m_end is behind the range 47 | m_cur = m_mid; 48 | m_cur[N-1] = m_start[N-1]; 49 | } 50 | 51 | 52 | template 53 | PointCrossIterator::PointCrossIterator( const Pointm &start, const Pointm &end, const Pointm &inc ) : 54 | m_start( start ), 55 | m_end( end ), 56 | m_inc( inc ), 57 | m_endReached( start == end ), 58 | m_idx( N-1 ), 59 | m_midIdx( round( m_idx / 2.0 ) ) 60 | { 61 | for( int i = 0; i < N; i++ ) 62 | m_mid[i] = m_start[i] + round( (m_end[i] - m_start[i] - 1) / 2.0 ); // -1 is because m_end is behind the range 63 | m_cur = m_mid; 64 | m_cur[N-1] = m_start[N-1]; 65 | } 66 | 67 | 68 | template 69 | void 70 | PointCrossIterator::operator++( int ) { 71 | ++(*this); 72 | } 73 | 74 | 75 | template 76 | void 77 | PointCrossIterator::operator++() { 78 | if( m_endReached ) 79 | return; 80 | 81 | m_cur[m_idx] += m_inc[m_idx]; 82 | 83 | // consider the center (m_cur == m_mid) only once 84 | if( m_idx != m_midIdx && m_cur[m_idx] == m_mid[m_idx] ) 85 | m_cur[m_idx] += m_inc[m_idx]; 86 | 87 | // end for this idx reached? 88 | if( m_cur[m_idx] >= m_end[m_idx] ) { 89 | m_cur[m_idx] = m_mid[m_idx]; 90 | m_idx--; 91 | if( m_idx >= 0 ) { 92 | m_cur[m_idx] = m_start[m_idx]; 93 | } else { 94 | m_endReached = true; 95 | } 96 | } 97 | } 98 | 99 | } /* namespace efs */ 100 | -------------------------------------------------------------------------------- /include/efslam/maps/cells/BeliefCell.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_BELIEF_CELL_H_ 35 | #define EFS_BELIEF_CELL_H_ 36 | 37 | #include "BayesCell.h" 38 | 39 | #include "efslam/utils/BeliefFunction.h" 40 | 41 | namespace efs { 42 | 43 | 44 | template 45 | class BeliefCell : public BayesCell { 46 | public: 47 | using typename BayesCell::PointNw; 48 | 49 | inline BeliefCell( int i = 0 ); 50 | inline BeliefCell( const BeliefFunction &bf ); 51 | inline BeliefCell( const std::string &str ); 52 | inline virtual ~BeliefCell(); 53 | 54 | inline void updateNoHit( const BeliefFunction &bf, bool behind = false ); 55 | inline void updateHit( const BeliefFunction &bf, const PointNw &p = PointNw::Zero() ); 56 | inline void integrate( const BeliefCell &c ); 57 | 58 | inline double entropy() const; 59 | 60 | inline bool operator==( const BeliefCell &other ) const noexcept; 61 | 62 | inline bool prune() const noexcept; 63 | inline bool pruneEqual( const BeliefCell &other ) const noexcept; 64 | 65 | virtual inline size_t bytes() const noexcept; 66 | 67 | inline void fromStr( const std::string &str ); 68 | inline std::string toStr() const; 69 | 70 | virtual inline constexpr CellBase::TypeE type() const { return CellBase::TypeE::CELL_BELIEF; } 71 | static inline constexpr CellBase::TypeE staticType() { return CellBase::TypeE::CELL_BELIEF; } 72 | 73 | friend std::ostream& operator<<( std::ostream& ostr, const BeliefCell* c ){ 74 | return operator<<( ostr,(*c) ); 75 | } 76 | 77 | friend std::ostream& operator<<( std::ostream& ostr, const BeliefCell& c ){ 78 | ostr << c.m_mass; 79 | return ostr; 80 | } 81 | 82 | protected: 83 | using Acc = typename PointAccumulator::Acc; 84 | 85 | BeliefFunction m_mass; 86 | 87 | public: 88 | GETTER( mass ); 89 | SETTER( mass ); 90 | }; 91 | 92 | 93 | } /* namespace efs */ 94 | 95 | #include "BeliefCell.hpp" 96 | 97 | #endif /* EFS_BELIEF_CELL_H_ */ 98 | -------------------------------------------------------------------------------- /include/efslam/io/carmen/CarmenReader.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_CARMENREADER_H_ 35 | #define EFS_CARMENREADER_H_ 36 | 37 | #include 38 | 39 | #include "efslam/utils/PoseSE2.h" 40 | 41 | namespace efs { 42 | 43 | class CarmenReader { 44 | public: 45 | CarmenReader( std::istream &inputStream ); 46 | virtual ~CarmenReader(); 47 | 48 | bool nextReading( PoseSE2 *pose, std::vector *scan, std::string *timestamp = nullptr ); 49 | 50 | private: 51 | static constexpr char LASER_OLD_STR[] = "FLASER"; 52 | static constexpr char LASER_NEW_STR[] = "ROBOTLASER"; 53 | 54 | enum LineTypeE { 55 | TYPE_UNKNOWN = -1, 56 | LASER_OLD, 57 | LASER_NEW 58 | }; 59 | 60 | enum { 61 | LASER_OLD_NUM_READINGS = 1, 62 | LASER_OLD_READINGS = 2, // if numReadings == 0, than LASER_OLD_POSE_X is at this position 63 | LASER_OLD_POSE_X = 2, // + numReadings 64 | LASER_OLD_POSE_Y = 3, // + numReadings 65 | LASER_OLD_POSE_PHI = 4, // + numReadings 66 | LASER_OLD_TIMESTAMP = 8, // + numReadings 67 | LASER_OLD_MIN_VALUES // + numReadings 68 | }; 69 | 70 | enum { 71 | // LASER_NEW_TYPE 72 | LASER_NEW_START_ANGLE = 2, 73 | LASER_NEW_FOV = 3, 74 | LASER_NEW_ANGULAR_RES = 4, 75 | LASER_NEW_MAX_RANGE = 5, 76 | LASER_NEW_ACCURANCY = 6, 77 | // LASER_NEW_REMISSION_MODE 78 | LASER_NEW_NUM_READINGS = 8, 79 | LASER_NEW_READINGS = 9, // if numReadings == 0, than LASER_NEW_NUM_REMISSIONS is at this position 80 | LASER_NEW_NUM_REMISSIONS = 9, // + numReadings 81 | // LASER_NEW_LASER_X 82 | // LASER_NEW_LASER_Y 83 | // LASER_NEW_LASER_PHI 84 | LASER_NEW_POSE_X = 13, // + numReadings + numRemissions 85 | LASER_NEW_POSE_Y = 14, // + numReadings + numRemissions 86 | LASER_NEW_POSE_PHI = 15, // + numReadings + numRemissions 87 | LASER_NEW_TIMESTAMP = 21, // + numReadings + numRemissions 88 | LASER_NEW_MIN_VALUES 89 | }; 90 | std::istream &m_in; 91 | }; 92 | 93 | } /* namespace efs */ 94 | 95 | #endif /* EFS_CARMENREADER_H_ */ 96 | -------------------------------------------------------------------------------- /include/efslam/maps/storages/HStorageHelper.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | namespace efs { 35 | 36 | // generic methods 37 | 38 | template 39 | inline size_t 40 | HierarchicalStorageHelper::patchIndex( const Pointm &p, const Pointm &size, int magnitude ) { 41 | size_t sum = 0, 42 | prod; 43 | 44 | for( size_t i = 0; i < N; i++ ) { 45 | prod = p[i] >> magnitude; 46 | for( size_t j = i+1; j < N; j++ ) 47 | prod *= size[j]; 48 | sum += prod; 49 | } 50 | 51 | return sum; 52 | } 53 | 54 | 55 | template 56 | inline Pointm 57 | HierarchicalStorageHelper::cellIndex( const Pointm &p, int magnitude ) { 58 | Pointm res; 59 | 60 | for( size_t i = 0; i < N; i++ ) 61 | res[i] = p[i] - ((p[i] >> magnitude) << magnitude); 62 | 63 | return res; 64 | } 65 | 66 | 67 | // specialization for N=2 68 | 69 | template<> 70 | inline size_t 71 | HierarchicalStorageHelper<2>::patchIndex( const Point2m &p, const Point2m &size, int magnitude ) { 72 | // x*size_y + y 73 | return (p[0] >> magnitude)*size[1] + (p[1] >> magnitude); 74 | } 75 | 76 | 77 | template<> 78 | inline Point2m 79 | HierarchicalStorageHelper<2>::cellIndex( const Point2m &p, int magnitude ) { 80 | return Point2m( p[0] - ((p[0] >> magnitude) << magnitude), p[1] - ((p[1] >> magnitude) << magnitude) ); 81 | } 82 | 83 | 84 | // specialization for N=3 85 | 86 | template<> 87 | inline size_t 88 | HierarchicalStorageHelper<3>::patchIndex( const Point3m &p, const Point3m &size, int magnitude ) { 89 | // x*size_y*size_z + y*size_z + z 90 | return (p[0] >> magnitude)*size[1]*size[2] + (p[1] >> magnitude)*size[2] + (p[2] >> magnitude); 91 | } 92 | 93 | 94 | template<> 95 | inline Point3m 96 | HierarchicalStorageHelper<3>::cellIndex( const Point3m &p, int magnitude ) { 97 | return Point3m( p[0] - ((p[0] >> magnitude) << magnitude), p[1] - ((p[1] >> magnitude) << magnitude), p[2] - ((p[2] >> magnitude) << magnitude) ); 98 | } 99 | 100 | } /* namespace efs */ 101 | -------------------------------------------------------------------------------- /include/efslam/viz/NeffView.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * This file is partially based on the GMapping QGraphPainter class 9 | * Copyright (c) 2004-2007, Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard 10 | * Originally licensed under the Creative Commons (Attribution-NonCommercial-ShareAlike). 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions are met: 14 | * 15 | * * Redistributions of source code must retain the above copyright notice, this 16 | * list of conditions and the following disclaimer. 17 | * 18 | * * Redistributions in binary form must reproduce the above copyright notice, 19 | * this list of conditions and the following disclaimer in the documentation 20 | * and/or other materials provided with the distribution. 21 | * 22 | * * Neither the name of Evidential FastSLAM nor the names of its 23 | * contributors may be used to endorse or promote products derived from 24 | * this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 28 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 29 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 30 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 31 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 32 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 33 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 34 | * OR TORT (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 | 38 | #ifndef EFS_NEFFVIEW_H_ 39 | #define EFS_NEFFVIEW_H_ 40 | 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | 47 | namespace efs { 48 | 49 | class NeffView : public QWidget { 50 | Q_OBJECT 51 | 52 | public: 53 | using NeffFunction = std::function; 54 | using DequeDouble = std::deque; 55 | using NumMeasurementsFunction = std::function; 56 | 57 | NeffView( QWidget *parent = nullptr, Qt::WindowFlags f = 0 ); 58 | virtual ~NeffView(); 59 | 60 | void start( int drawInterval, int pollInterval = 10 ); 61 | void setNeffFunction( NeffFunction f ) { m_getNeff = f; } 62 | void setNumMeasurementsFunction( NumMeasurementsFunction f ) { m_numMeasurements = f; } 63 | 64 | void setYReference( double y ) { m_reference = y; m_useYReference = true; } 65 | void disableYReference() { m_useYReference = false; } 66 | 67 | void setTitle( const QString &t ) { m_title = t; } 68 | void setRange( double min, double max ) { m_min = min; m_max = max; } 69 | void setNormalization( double n ) { m_normalization = n; } 70 | 71 | protected: 72 | void timerEvent( QTimerEvent *re ); 73 | void keyPressEvent( QKeyEvent *e ); 74 | void resizeEvent( QResizeEvent *sizeev ); 75 | void paintEvent( QPaintEvent * ); 76 | 77 | bool m_autoscale; 78 | int m_timer, 79 | m_count, 80 | m_drawInterval; 81 | QPixmap *m_pixmap; 82 | NeffFunction m_getNeff; 83 | NumMeasurementsFunction m_numMeasurements; 84 | DequeDouble m_values; 85 | double m_min, 86 | m_max, 87 | m_normalization, 88 | m_reference; 89 | bool m_useYReference; 90 | QString m_title; 91 | uint64_t m_lastMeasurements; 92 | }; 93 | 94 | } /* namespace efs */ 95 | 96 | #endif /* EFS_NEFFVIEW_H_ */ 97 | -------------------------------------------------------------------------------- /src/utils/Version.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #include "efslam/utils/Version.h" 35 | #include "VersionInfo.h" 36 | 37 | namespace efs { 38 | 39 | std::string 40 | Version::name() { 41 | return "Evidential FastSLAM"; 42 | } 43 | 44 | 45 | std::string 46 | Version::description() { 47 | return "An evidential approach to SLAM"; 48 | } 49 | 50 | 51 | std::string 52 | Version::nameDescription() { 53 | return name() + " - " + description(); 54 | } 55 | 56 | 57 | std::string 58 | Version::author() { 59 | return "Joachim Clemens"; 60 | } 61 | 62 | 63 | std::string 64 | Version::email() { 65 | return "jaycee@informatik.uni-bremen.de"; 66 | } 67 | 68 | 69 | std::string 70 | Version::coauthors() { 71 | return "Thomas Reineking, Tobias Kluth"; 72 | } 73 | 74 | 75 | std::string 76 | Version::contact() { 77 | return author() + " <" + email() + ">"; 78 | } 79 | 80 | 81 | std::string 82 | Version::company() { 83 | return "University of Bremen"; 84 | } 85 | 86 | 87 | std::string 88 | Version::department() { 89 | return "Cognitive Neuroinformatics"; 90 | } 91 | 92 | 93 | std::string 94 | Version::copyright() { 95 | return "Copyright (c) 2013-2016, " + author() + ", " + coauthors(); 96 | } 97 | 98 | 99 | std::string 100 | Version::version() { 101 | return versionStr; 102 | } 103 | 104 | 105 | std::string 106 | Version::buildDate() { 107 | return buildDateStr; 108 | } 109 | 110 | 111 | std::string 112 | Version::commitHash() { 113 | return commitHashStr; 114 | } 115 | 116 | 117 | std::string 118 | Version::commitDate() { 119 | return commitDateStr; 120 | } 121 | 122 | 123 | std::string 124 | Version::branchName() { 125 | return branchNameStr; 126 | } 127 | 128 | 129 | std::string 130 | Version::info() { 131 | std::string res; 132 | 133 | for( auto &line : infoLines() ) 134 | res += line; 135 | 136 | return res; 137 | } 138 | 139 | 140 | std::vector 141 | Version::infoLines() { 142 | std::vector res; 143 | 144 | res.push_back( nameDescription() ); 145 | res.push_back( "Version " + version() + " from " + commitDate() + " build on " + buildDate() ); 146 | res.push_back( copyright() ); 147 | res.push_back( company() + ", " + department() ); 148 | res.push_back( "Contact: " + contact() ); 149 | 150 | return res; 151 | } 152 | 153 | } /* namespace efs */ 154 | -------------------------------------------------------------------------------- /include/efslam/utils/BeliefFunction.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #include 35 | 36 | 37 | namespace efs { 38 | 39 | BeliefFunction::BeliefFunction() : 40 | Theta(1), 41 | o(0), 42 | e(0) 43 | { 44 | // Nothing else to do here 45 | }; 46 | 47 | 48 | BeliefFunction::BeliefFunction( float _t, float _o, float _e ) : 49 | Theta(_t), 50 | o(_o), 51 | e(_e) 52 | { 53 | // Nothing else to do here 54 | }; 55 | 56 | 57 | float 58 | BeliefFunction::emptyset() const { 59 | float sum = 0; 60 | for( size_t i = 0; i < 3; i++ ) 61 | sum += m[i]; 62 | return 1 - sum; 63 | } 64 | 65 | 66 | float 67 | BeliefFunction::normalize() { 68 | float es = emptyset(); 69 | if( es != 0 ) 70 | for( size_t i = 0; i < 3; i++ ) 71 | m[i] /= (1 - es); 72 | return es; 73 | } 74 | 75 | 76 | BeliefFunction 77 | BeliefFunction::normalized() const { 78 | BeliefFunction res = *this; 79 | res.normalize(); 80 | return res; 81 | } 82 | 83 | 84 | void 85 | BeliefFunction::conjunctive( const BeliefFunction &other ) { 86 | o = o*other.o + o*other.Theta + Theta*other.o; 87 | e = e*other.e + e*other.Theta + Theta*other.e; 88 | Theta = Theta*other.Theta; 89 | } 90 | 91 | 92 | void 93 | BeliefFunction::dempster( const BeliefFunction &other ) { 94 | conjunctive( other ); 95 | normalize(); 96 | } 97 | 98 | 99 | float 100 | BeliefFunction::pignistic() const { 101 | // returns the value for occupied. The value for empty is implicitly given by 1-o 102 | return (o + Theta*0.5) / (1 - emptyset()); 103 | } 104 | 105 | 106 | float 107 | BeliefFunction::internalConflict() const { 108 | // Internal conflict, according to Reineking & Clemens 2014, equations (15) and (16) 109 | BeliefFunction mn = *this; 110 | double emptyset = mn.normalize(), // normalize mn and emptyset is returned 111 | ic = 0; 112 | 113 | if( mn.o != 0 ) 114 | ic += mn.o * log2(mn.o + mn.Theta); 115 | if( mn.e != 0 ) 116 | ic += mn.e * log2(mn.e + mn.Theta); 117 | 118 | return -(1 - emptyset) * ic; 119 | } 120 | 121 | 122 | void 123 | BeliefFunction::set( float Theta_, float o_, float e_ ) { 124 | Theta = Theta_; 125 | o = o_; 126 | e = e_; 127 | } 128 | 129 | 130 | bool 131 | BeliefFunction::operator==( const BeliefFunction &other ) const noexcept { 132 | return Theta == other.Theta && o == other.o && e == other.e; 133 | } 134 | 135 | 136 | bool 137 | BeliefFunction::operator!=( const BeliefFunction &other ) const noexcept { 138 | return !(*this == other); 139 | } 140 | 141 | 142 | } /* namespace efs */ 143 | -------------------------------------------------------------------------------- /include/efslam/maps/cells/PointAccumulator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * This file is partially based on the GMapping PointAccumulator class 9 | * Copyright (c) 2004-2007, Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard 10 | * Originally licensed under the Creative Commons (Attribution-NonCommercial-ShareAlike). 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions are met: 14 | * 15 | * * Redistributions of source code must retain the above copyright notice, this 16 | * list of conditions and the following disclaimer. 17 | * 18 | * * Redistributions in binary form must reproduce the above copyright notice, 19 | * this list of conditions and the following disclaimer in the documentation 20 | * and/or other materials provided with the distribution. 21 | * 22 | * * Neither the name of Evidential FastSLAM nor the names of its 23 | * contributors may be used to endorse or promote products derived from 24 | * this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 28 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 29 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 30 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 31 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 32 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 33 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 34 | * OR TORT (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 | 38 | #ifndef EFS_POINT_ACCUMULATOR_H_ 39 | #define EFS_POINT_ACCUMULATOR_H_ 40 | 41 | #include 42 | 43 | #include "CellBase.h" 44 | 45 | #include "efslam/utils/Point.h" 46 | #include "efslam/utils/Convenience.h" 47 | 48 | namespace efs { 49 | 50 | 51 | template 52 | class PointAccumulator : public CellBase { 53 | public: 54 | static constexpr int Dimension = N; 55 | using PointNw = Pointw; 56 | 57 | inline PointAccumulator( int i = 0 ); 58 | inline PointAccumulator( const PointAccumulator &p ); 59 | inline PointAccumulator( PointAccumulator &&p ); 60 | inline virtual ~PointAccumulator(); 61 | 62 | inline PointAccumulator& operator=( const PointAccumulator &p ); 63 | inline PointAccumulator& operator=( PointAccumulator &&p ); 64 | 65 | inline void update( const PointNw &p ); 66 | inline void integrate( const PointAccumulator &pa ); 67 | 68 | inline PointNw mean() const; 69 | inline uint32_t hits() const; 70 | 71 | inline bool operator==( const PointAccumulator &other ) const noexcept; 72 | 73 | inline bool prune() const noexcept; 74 | inline bool pruneEqual( const PointAccumulator &other ) const noexcept; 75 | 76 | virtual inline size_t bytes() const noexcept; 77 | 78 | virtual inline constexpr CellBase::TypeE type() const { return CellBase::TypeE::CELL_POINT_ACCUMULATOR; } 79 | static inline constexpr CellBase::TypeE staticType() { return CellBase::TypeE::CELL_POINT_ACCUMULATOR; } 80 | 81 | protected: 82 | struct Acc { 83 | inline Acc() : sum( PointNw::Zero() ), hits( 0 ) {}; 84 | inline Acc( const PointNw &p ) : sum( p ), hits( 1 ) {}; 85 | inline ~Acc() {}; 86 | 87 | inline Acc& operator+=( const PointNw &p ) noexcept { 88 | sum += p; 89 | hits++; 90 | return *this; 91 | } 92 | 93 | inline Acc& operator+=( const Acc &a ) noexcept { 94 | sum += a.sum; 95 | hits += a.hits; 96 | return *this; 97 | } 98 | 99 | PointNw sum; 100 | uint32_t hits; 101 | }; 102 | 103 | std::unique_ptr m_acc; 104 | }; 105 | 106 | 107 | } /* namespace efs */ 108 | 109 | 110 | #include "PointAccumulator.hpp" 111 | 112 | #endif /* EFS_POINT_ACCUMULATOR_H_ */ 113 | -------------------------------------------------------------------------------- /include/efslam/models/MotionModel2D.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * This file is partially based on the GMapping MotionModel class 9 | * Copyright (c) 2004-2007, Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard 10 | * Originally licensed under the Creative Commons (Attribution-NonCommercial-ShareAlike). 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions are met: 14 | * 15 | * * Redistributions of source code must retain the above copyright notice, this 16 | * list of conditions and the following disclaimer. 17 | * 18 | * * Redistributions in binary form must reproduce the above copyright notice, 19 | * this list of conditions and the following disclaimer in the documentation 20 | * and/or other materials provided with the distribution. 21 | * 22 | * * Neither the name of Evidential FastSLAM nor the names of its 23 | * contributors may be used to endorse or promote products derived from 24 | * this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 28 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 29 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 30 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 31 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 32 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 33 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 34 | * OR TORT (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 | 38 | #include "efslam/utils/Config.h" 39 | #include "efslam/utils/NormalDistribution.h" 40 | 41 | 42 | namespace efs { 43 | 44 | MotionModel::MotionModel() { 45 | setParamsConfig(); 46 | } 47 | 48 | 49 | MotionModel::~MotionModel() { 50 | // Nothing to do here 51 | } 52 | 53 | 54 | void 55 | MotionModel::setParamsConfig() { 56 | m_transTransSigma = Config::get( "TRANS_TRANS_SIGMA", 0.1 ); 57 | m_transRotSigma = Config::get( "TRANS_ROT_SIGMA", 0.1 ); 58 | m_rotRotSigma = Config::get( "ROT_ROT_SIGMA", 0.1 ); 59 | m_rotTransSigma = Config::get( "ROT_TRANS_SIGMA", 0.1 ); 60 | } 61 | 62 | 63 | PoseSE2 64 | MotionModel::draw( const PoseSE2 &pose, double linearMove, double angularMove ) const { 65 | PoseSE2 res( pose ); 66 | 67 | double lin = linearMove + fabs( linearMove ) * NormalDistribution::drawStddev( m_transTransSigma ) 68 | + fabs( angularMove ) * NormalDistribution::drawStddev( m_transRotSigma ), 69 | ang = angularMove + fabs( linearMove ) * NormalDistribution::drawStddev( m_rotTransSigma ) 70 | + fabs( angularMove ) * NormalDistribution::drawStddev( m_rotRotSigma ); 71 | 72 | res.x() += lin * cos( res.phi() + .5 * ang ); 73 | res.y() += lin * sin( res.phi() + .5 * ang ); 74 | res.phi() += ang; 75 | res.normalizeRotation(); 76 | 77 | return res; 78 | } 79 | 80 | 81 | PoseSE2 82 | MotionModel::draw( const PoseSE2 &pose, const PoseSE2 &oldPose, const PoseSE2 &newPose ) const { 83 | double xySigma = 0.3 * m_transTransSigma; 84 | 85 | PoseSE2 delta = newPose.compoundInv( oldPose ), 86 | noisy = delta; 87 | 88 | noisy.x() += NormalDistribution::drawStddev( m_transTransSigma * fabs( (world_t) delta.x() ) 89 | + m_transRotSigma * fabs( delta.phi() ) 90 | + xySigma * fabs( (world_t) delta.y() ) ); 91 | 92 | noisy.y() += NormalDistribution::drawStddev( m_transTransSigma * fabs( (world_t) delta.y() ) 93 | + m_transRotSigma * fabs( delta.phi() ) 94 | + xySigma * fabs( (world_t) delta.x() ) ); 95 | 96 | noisy.phi() += NormalDistribution::drawStddev( m_rotRotSigma * fabs( delta.phi() ) 97 | + m_rotTransSigma * delta.pos().norm() ); 98 | noisy.normalizeRotation(); 99 | 100 | return pose.compound( noisy ); 101 | } 102 | 103 | 104 | } /* namespace efs */ 105 | -------------------------------------------------------------------------------- /include/efslam/utils/shareduniqueptr.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | namespace efs { 35 | 36 | template 37 | shared_unique_ptr::shared_unique_ptr() { 38 | // Nothing to do here 39 | } 40 | 41 | 42 | template 43 | shared_unique_ptr::shared_unique_ptr( T *p ) : 44 | ptr( std::make_shared( p ) ) 45 | { 46 | // Nothing else to do here 47 | } 48 | 49 | 50 | template 51 | shared_unique_ptr::shared_unique_ptr( const shared_unique_ptr &other ) : 52 | ptr( other.ptr ) 53 | { 54 | // Nothing else to do here 55 | } 56 | 57 | 58 | template 59 | shared_unique_ptr::shared_unique_ptr( shared_unique_ptr &&other ) : 60 | ptr( std::move( other.ptr ) ) 61 | { 62 | // Nothing else to do here 63 | } 64 | 65 | 66 | template 67 | shared_unique_ptr::~shared_unique_ptr() { 68 | // Nothing to do here 69 | } 70 | 71 | 72 | template 73 | shared_unique_ptr& 74 | shared_unique_ptr::operator=( const shared_unique_ptr &other ) { 75 | ptr = other.ptr; 76 | return *this; 77 | } 78 | 79 | 80 | template 81 | shared_unique_ptr& 82 | shared_unique_ptr::operator=( shared_unique_ptr &&other ) { 83 | ptr = std::move( other.ptr ); 84 | return *this; 85 | } 86 | 87 | /* 88 | template 89 | void 90 | shared_unique_ptr::make_unique() { 91 | if( ptr ) { 92 | std::shared_ptr mutex = ptr->mutex; 93 | 94 | mutex->lock(); 95 | if( !unique() ) 96 | ptr = std::make_shared( *ptr ); // create a copy of the content 97 | mutex->unlock(); 98 | } 99 | } 100 | 101 | template 102 | void 103 | shared_unique_ptr::make_unique_clone() { 104 | if( ptr ) { 105 | std::shared_ptr mutex = ptr->mutex; 106 | 107 | mutex->lock(); 108 | if( !unique() ) 109 | ptr = std::make_shared( ptr->obj->clone() ); // create a copy of the content 110 | mutex->unlock(); 111 | } 112 | } 113 | */ 114 | 115 | template 116 | void 117 | shared_unique_ptr::make_unique() { 118 | if( ptr ) { 119 | std::mutex *mutex = &ptr->mutex; 120 | 121 | mutex->lock(); 122 | if( !unique() ) 123 | ptr = std::make_shared( *ptr ); // create a copy of the content 124 | mutex->unlock(); 125 | } 126 | } 127 | 128 | template 129 | void 130 | shared_unique_ptr::make_unique_clone() { 131 | if( ptr ) { 132 | std::mutex *mutex = &ptr->mutex; 133 | 134 | mutex->lock(); 135 | if( !unique() ) 136 | ptr = std::make_shared( ptr->obj->clone() ); // create a copy of the content 137 | mutex->unlock(); 138 | } 139 | } 140 | 141 | } /* namespace efs */ 142 | -------------------------------------------------------------------------------- /include/efslam/viz/CellColor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | namespace efs { 35 | 36 | 37 | /********************************* 38 | * BayesCell 39 | *********************************/ 40 | 41 | template 42 | Color 43 | CellColor< BayesCell >::getColor( const BayesCell &cell, bool normalize, uint8_t *alpha, ColorModeE colorMode, double alphaScale ) { 44 | double v = cell.fullness(); 45 | 46 | if( v < 0 ) { 47 | if( alpha ) 48 | *alpha = 0; 49 | return defaultColor(); 50 | } 51 | 52 | if( alpha ) { 53 | switch( colorMode ) { 54 | case COLOR_MODE_ALPHA_EMPTY: 55 | *alpha = MAX( 0.0, (alphaScale - 1.0+v) ) * 255; 56 | break; 57 | 58 | case COLOR_MODE_ALPHA_NOT_OCCUPIED: 59 | *alpha = v * 255; 60 | break; 61 | 62 | case COLOR_MODE_ALPHA_THETA: 63 | case COLOR_MODE_DEFAULT: 64 | default: 65 | *alpha = 255; 66 | break; 67 | } 68 | } 69 | 70 | uint8_t grayVal = (uint8_t) (255 - (uint8_t) (255 * v)); 71 | return { grayVal, grayVal, grayVal }; 72 | } 73 | 74 | 75 | template 76 | constexpr Color 77 | CellColor< BayesCell >::defaultColor() { 78 | return { 200, 200, 255 }; 79 | } 80 | 81 | 82 | /********************************* 83 | * BeliefCell 84 | *********************************/ 85 | 86 | template 87 | Color 88 | CellColor< BeliefCell >::getColor( const BeliefCell &cell, bool normalize, uint8_t *alpha, ColorModeE colorMode, double alphaScale ) { 89 | const BeliefFunction *m; 90 | BeliefFunction normalized; 91 | 92 | if( !normalize ) { 93 | m = &cell.mass(); 94 | } else { 95 | normalized = cell.mass().normalized(); 96 | m = &normalized; 97 | } 98 | 99 | if( alpha ) { 100 | switch( colorMode ) { 101 | case COLOR_MODE_ALPHA_THETA: 102 | *alpha = (alphaScale - m->Theta) * 255; // 1 - Theta -> Alpha channel 103 | break; 104 | 105 | case COLOR_MODE_ALPHA_EMPTY: 106 | *alpha = MAX( 0.0, (alphaScale - m->e) ) * 255; // 1 - Empty -> Alpha channel 107 | break; 108 | 109 | case COLOR_MODE_ALPHA_NOT_OCCUPIED: 110 | *alpha = m->o * 255; // o -> Alpha channel 111 | break; 112 | 113 | case COLOR_MODE_DEFAULT: 114 | default: 115 | *alpha = 255; // Full Alpha (Emptyset will be black) 116 | break; 117 | } 118 | } 119 | 120 | return { (uint8_t)(m->o * 255), // Occupied -> Red 121 | (uint8_t)(m->e * 255), // Empty -> Green 122 | (uint8_t)(m->Theta * 255) }; // Theta -> Blue 123 | } 124 | 125 | 126 | template 127 | constexpr Color 128 | CellColor< BeliefCell >::defaultColor() { 129 | return { 0, 0, 255 }; 130 | } 131 | 132 | } /* namespace efs */ 133 | 134 | -------------------------------------------------------------------------------- /include/efslam/models/SensorModelBayes.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #include "efslam/utils/Log.h" 35 | #include "efslam/utils/Config.h" 36 | #include "efslam/utils/NormalDistribution.h" 37 | 38 | 39 | namespace efs { 40 | 41 | 42 | /************************************ 43 | * Bayes sensor model 44 | ************************************/ 45 | 46 | template 47 | SensorModel< BayesCell >::SensorModel( bool unused ) 48 | { 49 | setParamsConfig(); 50 | } 51 | 52 | 53 | template 54 | void 55 | SensorModel< BayesCell >::setParamsConfig() { 56 | m_bayesSigmaSqr = Config::getDouble( "BAYES_SIGMA", 0.25 ); 57 | m_bayesSigmaSqr *= m_bayesSigmaSqr; 58 | } 59 | 60 | 61 | template 62 | template 63 | double 64 | SensorModel< BayesCell >::inverseModel( const typename Map::WorldPoint &pHit, const Line &scanLine, size_t hitIdx, ScanMap *scanMap, Map *map ) const { 65 | static_assert( std::is_same< typename Map::CellType, BayesCell >(), "Map cell type must be BayesCell." ); 66 | 67 | double entropy = 0.0; 68 | 69 | for( size_t i = 0; i < scanLine.size(); i++ ) { 70 | BayesCell &mapCell = map->cell( scanLine[i] ); 71 | 72 | #if CALC_ENTROPY 73 | entropy -= mapCell.entropy(); 74 | #endif 75 | 76 | if( i == hitIdx ) 77 | mapCell.updateHit( pHit ); 78 | else 79 | mapCell.updateNoHit(); 80 | 81 | #if CALC_ENTROPY 82 | entropy += mapCell.entropy(); 83 | #endif 84 | } 85 | 86 | return entropy; 87 | } 88 | 89 | 90 | template 91 | template 92 | double 93 | SensorModel< BayesCell >::forwardModel( world_t dist, const typename Map::WorldPoint &bestMu, const typename Map::MapPoint &bestMuMap, const typename Map::WorldPoint &sensorOrigin, const typename Map::MapPoint &sensorOriginMap, const Map &map ) const { 94 | static_assert( std::is_same< typename Map::CellType, BayesCell >(), "Map cell type must be BayesCell." ); 95 | return NormalDistribution::logPlFromSqr( bestMu.dot( bestMu ), m_bayesSigmaSqr ); // TODO: Save this in a LUT as well? (see Belief sensor model) 96 | } 97 | 98 | 99 | template 100 | double 101 | SensorModel< BayesCell >::sigmaSqr() const { 102 | return m_bayesSigmaSqr; 103 | } 104 | 105 | 106 | template 107 | double 108 | SensorModel< BayesCell >::nullLogLikelihood() const { 109 | return -0.5 / (2.0 * m_bayesSigmaSqr); 110 | } 111 | 112 | 113 | template 114 | constexpr bool 115 | SensorModel< BayesCell >::excludeNoHit() const { 116 | return true; // always exclude for bayes model 117 | } 118 | 119 | 120 | template 121 | constexpr int 122 | SensorModel< BayesCell >::cellsBehind() const { 123 | return 0; 124 | } 125 | 126 | 127 | } /* namespace efs */ 128 | -------------------------------------------------------------------------------- /include/efslam/models/SensorModelBayes.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_SENSORMODELBAYES_H_ 35 | #define EFS_SENSORMODELBAYES_H_ 36 | 37 | #include "SensorModel.h" 38 | 39 | #include "efslam/utils/Line.h" 40 | #include "efslam/maps/cells/BayesCell.h" 41 | 42 | 43 | namespace efs { 44 | 45 | 46 | template 47 | class SensorModel< BayesCell > { 48 | public: 49 | using ScanMap = char; // we need no cell container for Bayes cells, so just use something that does not require much space 50 | 51 | SensorModel( bool unused = false ); 52 | 53 | void setParamsConfig(); 54 | 55 | /** 56 | * @param pHit end point of measurement in world coordinates 57 | * @param scanLine line from sensor origin to hit cell (up to usable range) in map coordinates 58 | * @param hitIdx index of the hit cell in scanLine, number >= scanLine.size() if hit cell is not part of scanLine (e.g. not in usable range) 59 | * @param scanMap cells updated in the current scan (only used in Belief model) 60 | * @param map the map 61 | * 62 | * @return information gained in this step (if CALC_ENTROPY = 1) 63 | */ 64 | template 65 | inline double inverseModel( const typename Map::WorldPoint &pHit, const Line &scanLine, size_t hitIdx, ScanMap *scanMap, Map *map ) const; 66 | 67 | /** 68 | * @param dist distance between measurement and sensor origin (measured range) 69 | * @param bestMu distance between measurement and best matching cell 70 | * @param bestMuMap index of the best matching cell in the map 71 | * @param sensorOrigin origin of sensor in world coordinates 72 | * @param sensorOriginMap origin of sensor in map coordinates 73 | * @param map the map 74 | * 75 | * @return log-likelihood 76 | */ 77 | template 78 | inline double forwardModel( world_t dist, const typename Map::WorldPoint &bestMu, const typename Map::MapPoint &bestMuMap, const typename Map::WorldPoint &sensorOrigin, const typename Map::MapPoint &sensorOriginMap, const Map &map ) const; 79 | 80 | /** 81 | * @param scanMap cells updated in the current scan (only used in Belief model, container will be cleared) 82 | * @param map the map 83 | * 84 | * @return information gained in this step (if CALC_ENTROPY = 1) 85 | */ 86 | template 87 | inline double integrate( ScanMap *scanMap, Map *map ) const { return 0.0; }; 88 | 89 | inline double sigmaSqr() const; 90 | inline double nullLogLikelihood() const; 91 | inline constexpr bool excludeNoHit() const; 92 | inline constexpr int cellsBehind() const; 93 | 94 | private: 95 | double m_bayesSigmaSqr; 96 | }; 97 | 98 | 99 | } /* namespace efs */ 100 | 101 | 102 | #include "SensorModelBayes.hpp" 103 | 104 | 105 | #endif /* EFS_SENSORMODELBAYES_H_ */ 106 | -------------------------------------------------------------------------------- /include/efslam/models/MapScanMatcher.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * This file is partially based on the GMapping ScanMatcher class 9 | * Copyright (c) 2004-2007, Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard 10 | * Originally licensed under the Creative Commons (Attribution-NonCommercial-ShareAlike). 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions are met: 14 | * 15 | * * Redistributions of source code must retain the above copyright notice, this 16 | * list of conditions and the following disclaimer. 17 | * 18 | * * Redistributions in binary form must reproduce the above copyright notice, 19 | * this list of conditions and the following disclaimer in the documentation 20 | * and/or other materials provided with the distribution. 21 | * 22 | * * Neither the name of Evidential FastSLAM nor the names of its 23 | * contributors may be used to endorse or promote products derived from 24 | * this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 28 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 29 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 30 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 31 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 32 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 33 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 34 | * OR TORT (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 | 38 | #ifndef EFS_MAP_SCANMATCHER_H_ 39 | #define EFS_MAP_SCANMATCHER_H_ 40 | 41 | #include 42 | #include 43 | 44 | #ifndef CALC_ENTROPY 45 | # define CALC_ENTROPY 0 46 | #endif 47 | 48 | #ifndef USE_MAP_CACHE 49 | # define USE_MAP_CACHE 0 // Cache seams to be slower because of dynamic allocations TODO: Test this again with larger maps 50 | #endif 51 | 52 | 53 | #include "SensorModelBayes.h" 54 | #include "SensorModelBelief.h" 55 | 56 | #if USE_MAP_CACHE 57 | # include "maps/MapCache.h" 58 | #endif 59 | 60 | #include "efslam/utils/PoseBase.h" 61 | 62 | 63 | namespace efs { 64 | 65 | 66 | template 67 | class MapScanMatcher { 68 | public: 69 | static constexpr int Dimension = Map::Dimension; 70 | using WorldPoint = typename Map::WorldPoint; 71 | using MapPoint = typename Map::MapPoint; 72 | using CellType = typename Map::CellType; 73 | using ScanConstPtr = typename Scan::ConstPtr; 74 | 75 | MapScanMatcher( const PoseSE2 &sensorPose = PoseSE2() ); 76 | 77 | void setParamsConfig(); 78 | inline void setSensorPose( const PoseSE2 &sensorPose, size_t sensorId = 0 ); 79 | 80 | inline double score( const ScanConstPtr &z, const PoseBase &pose, const Map &map, size_t sensorId = 0, double *logLikelihood = nullptr, uint32_t *count = nullptr ) const; 81 | double registerScan( const ScanConstPtr &z, const PoseBase &pose, Map *map, size_t sensorId = 0 ) const; 82 | 83 | PoseSE2 optimize( const ScanConstPtr &z, const PoseSE2 &pose, const Map &map, size_t sensorId = 0, double *bestScore = nullptr ) const; 84 | 85 | protected: 86 | template 87 | double scoreInternal( const ScanConstPtr &z, const PoseBase &pose, const MapT &map, size_t sensorId = 0, double *logLikelihood = nullptr, uint32_t *count = nullptr ) const; 88 | 89 | using SensorModelType = SensorModel< typename Map::CellType >; 90 | 91 | SensorModelType m_sensorModel; 92 | std::vector< PoseSE2 > m_sensor2robot; 93 | std::vector< MapPoint > m_kernelPoints; 94 | 95 | double m_freeCellRatio, 96 | m_fullnessThreshold, 97 | m_scoreSigmaSqr; 98 | world_t m_optAngularStep, 99 | m_optLinearStep, 100 | m_usableRange, 101 | m_maxRange, 102 | m_relevantDist; 103 | int m_optIterations, 104 | m_likelihoodNumMeasurements; 105 | bool m_likelihoodLikeScore; 106 | }; 107 | 108 | 109 | } /* namespace efs */ 110 | 111 | 112 | #include "MapScanMatcher.hpp" 113 | 114 | 115 | #endif /* EFS_MAP_SCANMATCHER_H_ */ 116 | -------------------------------------------------------------------------------- /include/efslam/maps/storages/HStorage.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * This file is partially based on the GMapping HArray2D class 9 | * Copyright (c) 2004-2007, Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard 10 | * Originally licensed under the Creative Commons (Attribution-NonCommercial-ShareAlike). 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions are met: 14 | * 15 | * * Redistributions of source code must retain the above copyright notice, this 16 | * list of conditions and the following disclaimer. 17 | * 18 | * * Redistributions in binary form must reproduce the above copyright notice, 19 | * this list of conditions and the following disclaimer in the documentation 20 | * and/or other materials provided with the distribution. 21 | * 22 | * * Neither the name of Evidential FastSLAM nor the names of its 23 | * contributors may be used to endorse or promote products derived from 24 | * this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 28 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 29 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 30 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 31 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 32 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 33 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 34 | * OR TORT (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 | 38 | #ifndef EFS_HSTORAGE_H_ 39 | #define EFS_HSTORAGE_H_ 40 | 41 | #include 42 | 43 | #include "efslam/utils/shareduniqueptr.h" 44 | #include "efslam/utils/Point.h" 45 | 46 | 47 | namespace efs { 48 | 49 | 50 | /** 51 | * Meta-Storage, that devides the map into equal-sized patches, 52 | * that are represented by the actual storage and hold as smart-pointers 53 | * to save memory when the maps are copied during resampling step. 54 | * The actual storages a replicated on demand. 55 | */ 56 | template // patches will have the size 2^Magnitude 57 | class HierarchicalStorage { 58 | public: 59 | static constexpr int Dimension = Storage::Dimension; 60 | using PointNm = Pointm; 61 | template 62 | using CellList = typename Storage::template CellList; 63 | 64 | HierarchicalStorage( const PointNm &size ); 65 | HierarchicalStorage( const HierarchicalStorage &hs ); 66 | ~HierarchicalStorage(); 67 | 68 | inline void clear(); 69 | 70 | inline HierarchicalStorage& 71 | operator=( const HierarchicalStorage &hs ); 72 | 73 | inline Cell& cell( const PointNm &p ); 74 | inline const Cell& cell( const PointNm &p ) const; 75 | 76 | template 77 | inline CellList cells() const; 78 | 79 | inline uint32_t prune(); 80 | 81 | // the size of the shared parts is given relative to the number of shares 82 | inline size_t bytes() const; 83 | 84 | inline AccessibilityState cellState( const PointNm& p ) const; 85 | inline bool isInside( const PointNm& p ) const; 86 | inline bool isAllocated( const PointNm &p ) const; 87 | inline bool isPatchAllocated( const PointNm &p ) const; 88 | 89 | inline PointNm size() const { return m_totalSize; } 90 | inline PointNm patchSize() const { return m_patchSize; } 91 | inline PointNm numPatches() const { return m_size; } 92 | 93 | 94 | // TODO: If having memory problems, hold only the currently needed patches in memory 95 | 96 | protected: 97 | PointNm m_size, // size of the hierarchical storage 98 | m_totalSize, // total/usable size 99 | m_patchSize; // size of one patch (always quadratic) 100 | 101 | std::vector< shared_unique_ptr > m_patches; 102 | 103 | static const Cell sm_unknown; 104 | }; 105 | 106 | 107 | } /* namespace efs */ 108 | 109 | #include "HStorage.hpp" 110 | 111 | #endif /* EFS_HSTORAGE_H_ */ 112 | -------------------------------------------------------------------------------- /include/efslam/maps/storages/Array2D.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * This file is partially based on the GMapping Array2D class 9 | * Copyright (c) 2004-2007, Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard 10 | * Originally licensed under the Creative Commons (Attribution-NonCommercial-ShareAlike). 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions are met: 14 | * 15 | * * Redistributions of source code must retain the above copyright notice, this 16 | * list of conditions and the following disclaimer. 17 | * 18 | * * Redistributions in binary form must reproduce the above copyright notice, 19 | * this list of conditions and the following disclaimer in the documentation 20 | * and/or other materials provided with the distribution. 21 | * 22 | * * Neither the name of Evidential FastSLAM nor the names of its 23 | * contributors may be used to endorse or promote products derived from 24 | * this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 28 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 29 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 30 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 31 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 32 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 33 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 34 | * OR TORT (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 | 38 | #ifndef EFS_ARRAY2D_H_ 39 | #define EFS_ARRAY2D_H_ 40 | 41 | #include 42 | #include 43 | 44 | #include 45 | 46 | #include "efslam/utils/Point.h" 47 | #include "efslam/maps/AccessState.h" 48 | 49 | 50 | namespace efs { 51 | 52 | template 53 | class Array2D { 54 | public: 55 | static constexpr int Dimension = 2; 56 | template 57 | using CellList = std::list< std::pair >; 58 | 59 | Array2D( const Point2m &size ); 60 | Array2D( map_t xsize = 0, map_t ysize = 0 ); 61 | Array2D( const Array2D &other ); 62 | Array2D( Array2D &&other ); 63 | ~Array2D(); 64 | 65 | void clear(); 66 | void resize( map_t xmin, map_t ymin, map_t xmax, map_t ymax ); 67 | 68 | Array2D& operator=( const Array2D &other ); 69 | Array2D& operator=( Array2D &&other ); 70 | 71 | inline const Cell& cell( map_t x, map_t y ) const; 72 | inline Cell& cell( map_t x, map_t y ); 73 | inline const Cell& cell( const Point2m& p ) const { return cell( (map_t) p[0], (map_t) p[1] ); } 74 | inline Cell& cell( const Point2m& p ) { return cell( (map_t) p[0], (map_t) p[1] ); } 75 | 76 | template 77 | inline CellList cells( const Point2m &offset = Point2m::Zero() ) const; 78 | 79 | inline constexpr uint32_t prune() const { return 0; } // Array2D can't be pruned 80 | 81 | inline size_t bytes() const { return sizeof(*this) + sizeof(Cell*) * m_size[0] + sizeof(Cell) * m_size[0] * m_size[1]; } 82 | 83 | inline bool isInside( map_t x, map_t y ) const; 84 | inline bool isInside( const Point2m &p ) const { return isInside( (map_t) p[0], (map_t) p[1] ); } 85 | 86 | inline AccessibilityState cellState( map_t x, map_t y ) const { return (AccessibilityState) (isInside( x, y ) ? (AS_INSIDE | AS_ALLOCATED) : AS_OUTSIDE); } 87 | inline AccessibilityState cellState( const Point2m &p ) const { return cellState( (map_t) p[0], (map_t) p[1] ); } 88 | 89 | inline int getXSize() const { return m_size[0]; } 90 | inline int getYSize() const { return m_size[1]; } 91 | inline Point2m size() const { return m_size; } 92 | inline Point2m patchSize() const { return size(); } 93 | inline Point2m numPatches() const { return Point3m::Ones(); } 94 | 95 | //inline Cell** cells() { return m_cells; } 96 | 97 | protected: 98 | Cell **m_cells; 99 | Point2m m_size; 100 | }; 101 | 102 | } /* namespace efs */ 103 | 104 | #include "Array2D.hpp" 105 | 106 | #endif /* EFS_ARRAY_2D_H_ */ 107 | 108 | -------------------------------------------------------------------------------- /src/io/carmen/CarmenReader.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | #include "efslam/io/carmen/CarmenReader.h" 39 | #include "efslam/utils/Convenience.h" 40 | #include "efslam/utils/Log.h" 41 | 42 | namespace efs { 43 | 44 | constexpr char CarmenReader::LASER_OLD_STR[]; 45 | constexpr char CarmenReader::LASER_NEW_STR[]; 46 | 47 | 48 | CarmenReader::CarmenReader( std::istream &inputStream ) : 49 | m_in( inputStream ) 50 | { 51 | } 52 | 53 | CarmenReader::~CarmenReader() { 54 | } 55 | 56 | 57 | bool 58 | CarmenReader::nextReading( PoseSE2 *pose, std::vector *scan, std::string *timestamp ) { 59 | bool found = false; 60 | 61 | scan->clear(); 62 | while( m_in.good() && !found ) { 63 | std::string line; 64 | std::getline( m_in, line ); 65 | 66 | int numReadingIdx = LASER_OLD_NUM_READINGS, 67 | readingsIdx = LASER_OLD_READINGS, 68 | poseXIdx = LASER_OLD_POSE_X, 69 | poseYIdx = LASER_OLD_POSE_Y, 70 | posePhiIdx = LASER_OLD_POSE_PHI, 71 | timestampIdx = LASER_OLD_TIMESTAMP, 72 | minValues = LASER_OLD_MIN_VALUES; 73 | 74 | LineTypeE lineType = TYPE_UNKNOWN; 75 | 76 | if( line.compare( 0, strlen( LASER_OLD_STR ), LASER_OLD_STR ) == 0 ) { 77 | lineType = LASER_OLD; 78 | } else if( line.compare( 0, strlen( LASER_NEW_STR ), LASER_NEW_STR ) == 0 ) { 79 | lineType = LASER_NEW; 80 | 81 | numReadingIdx = LASER_NEW_NUM_READINGS, 82 | readingsIdx = LASER_NEW_READINGS, 83 | poseXIdx = LASER_NEW_POSE_X, 84 | poseYIdx = LASER_NEW_POSE_Y, 85 | posePhiIdx = LASER_NEW_POSE_PHI, 86 | timestampIdx = LASER_NEW_TIMESTAMP, 87 | minValues = LASER_NEW_MIN_VALUES; 88 | } 89 | 90 | 91 | if( lineType != TYPE_UNKNOWN ) { 92 | found = true; 93 | 94 | auto splitted = split( line, ' ' ); 95 | 96 | int numReadings = std::stoi( splitted[numReadingIdx] ), 97 | numRemissions = 0; 98 | 99 | if( lineType == LASER_NEW ) { 100 | numRemissions = std::stoi( splitted[numReadings + LASER_NEW_NUM_REMISSIONS] ); 101 | } 102 | 103 | int offset = numReadings + numRemissions; 104 | 105 | if( (int) splitted.size() < offset + minValues ) { 106 | l_wrn( "Wrong number of fields: Expected at least " << (offset + minValues) << " got " << splitted.size() ); 107 | found = false; 108 | continue; 109 | } 110 | 111 | scan->reserve( numReadings ); 112 | for( int i = 0; i < numReadings; i++ ) 113 | scan->push_back( std::stod( splitted[readingsIdx + i] ) ); 114 | 115 | /* 116 | for( int i = numReadings; i < splitted.size(); i++ ) 117 | std::cout << splitted[i] << std::endl; 118 | */ 119 | 120 | pose->x() = std::stof( splitted[poseXIdx + offset] ); 121 | pose->y() = std::stof( splitted[poseYIdx + offset] ); 122 | pose->phi() = std::stof( splitted[posePhiIdx + offset] ); 123 | //std::cout << pose << std::endl; 124 | 125 | if( timestamp ) 126 | *timestamp = splitted[timestampIdx + offset]; 127 | //std::cout << splitted[timestampIdx + offset] << std::endl; 128 | } 129 | 130 | } 131 | 132 | return found; 133 | } 134 | 135 | 136 | } /* namespace efs */ 137 | -------------------------------------------------------------------------------- /include/efslam/utils/Config.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef EFS_CONFIG_H_ 35 | #define EFS_CONFIG_H_ 36 | 37 | #include 38 | #include 39 | 40 | 41 | namespace efs { 42 | 43 | class Config { 44 | public: 45 | static void loadDefaults( bool clearBeforeLoad = false ); 46 | 47 | static void loadParams( int argc, char *argv[], bool clearBeforeLoad = false, bool warnUnknown = true ); 48 | 49 | static void loadStream( std::istream &f, bool clearBeforeLoad = false, bool warnUnknown = true ); 50 | static void saveStream( std::ostream &f ); 51 | 52 | static void loadFile( char const *filename, bool clearBeforeLoad = false, bool warnUnknown = true ); 53 | static void loadFile( const std::string &filename, bool clearBeforeLoad = false, bool warnUnknown = true ); 54 | static void saveFile( char const *filename ); 55 | static void saveFile( const std::string &filename ); 56 | 57 | static void loadDefaultFile( bool clearBeforeLoad = false, bool warnUnknown = true ); 58 | 59 | #ifndef WIN32 60 | static void createOutputDir( const std::string &suffix = "" ); 61 | #endif 62 | 63 | static std::string getString( const std::string &key ); 64 | static std::string getString( const std::string &key, const char *defaultVal ); 65 | static std::string getString( const std::string &key, const std::string defaultVal ); 66 | static double getDouble( const std::string &key ); 67 | static double getDouble( const std::string &key, const double defaultVal ); 68 | static int getInt( const std::string &key ); 69 | static int getInt( const std::string &key, const int defaultVal ); 70 | 71 | static std::string get( const std::string &key, const std::string defaultVal ); 72 | static double get( const std::string &key, const double defaultVal ); 73 | static int get( const std::string &key, const int defaultVal ); 74 | 75 | static void set( const std::string &key, const std::string val ); 76 | static void set( const std::string &key, const double val ); 77 | static void set( const std::string &key, int val ); 78 | 79 | static bool hasStringValue( const std::string &key ); 80 | static bool hasDoubleValue( const std::string &key ); 81 | static bool hasIntValue( const std::string &key ); 82 | 83 | static bool isValidKey( const std::string &key ); 84 | 85 | static std::string toString(); 86 | 87 | private: 88 | Config(); // we don't want any instance of this class 89 | 90 | enum ValueType { 91 | VALUE_TYPE_STRING, 92 | VALUE_TYPE_INT, 93 | VALUE_TYPE_DOUBLE 94 | }; 95 | 96 | static void clear(); 97 | 98 | static bool isAllowedKeyChar( char c ); 99 | static ValueType getValueType( const std::string &str ); 100 | static void addValue( const std::string &key, const std::string &value, bool wasQuoted = false, bool warnUnknown = true ); 101 | 102 | static std::string toLower( const std::string &in ); 103 | static std::string toUpper( const std::string &in ); 104 | 105 | static std::unordered_map sm_stringVals; 106 | static std::unordered_map sm_doubleVals; 107 | static std::unordered_map sm_intVals; 108 | }; 109 | 110 | } /* namespace efs */ 111 | 112 | #endif /* EFS_CONFIG_H_ */ 113 | 114 | -------------------------------------------------------------------------------- /include/efslam/maps/cells/BayesCell.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of Evidential FastSLAM nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | 39 | namespace efs { 40 | 41 | template 42 | BayesCell::BayesCell( int i ) : 43 | PointAccumulator( i ), 44 | m_visits( 0 ) 45 | { 46 | // nothing else to do here 47 | } 48 | 49 | 50 | template 51 | BayesCell::BayesCell( const std::string &str ) { 52 | fromStr( str ); 53 | } 54 | 55 | 56 | template 57 | BayesCell::~BayesCell() { 58 | // Nothing to do here 59 | } 60 | 61 | 62 | template 63 | void 64 | BayesCell::updateNoHit() { 65 | m_visits++; 66 | } 67 | 68 | 69 | template 70 | void 71 | BayesCell::updateHit( const PointNw &p ) { 72 | PointAccumulator::update( p ); 73 | m_visits++; 74 | } 75 | 76 | 77 | template 78 | void 79 | BayesCell::integrate( const BayesCell &c ) { 80 | PointAccumulator::integrate( c ); 81 | m_visits += c.m_visits; 82 | } 83 | 84 | 85 | template 86 | double 87 | BayesCell::fullness() const { 88 | if( !m_visits ) 89 | return -1; 90 | return (double) this->hits() / (double) m_visits; 91 | } 92 | 93 | 94 | template 95 | double 96 | BayesCell::entropy() const { 97 | if( !m_visits ) 98 | return -log2( 0.5 ); 99 | 100 | auto hits = this->hits(); 101 | if( hits == 0 || hits == m_visits ) 102 | return 0; 103 | 104 | double x = fullness(); 105 | return -(x * log2( x ) + (1 - x) * log2( 1 - x )); 106 | } 107 | 108 | 109 | template 110 | bool 111 | BayesCell::operator==( const BayesCell &other ) const noexcept { 112 | return m_visits == other.m_visits && PointAccumulator::operator==( other ); 113 | } 114 | 115 | 116 | template 117 | bool 118 | BayesCell::pruneEqual( const BayesCell &other ) const noexcept { 119 | return m_visits == other.m_visits && PointAccumulator::pruneEqual( other ); 120 | } 121 | 122 | 123 | template 124 | size_t 125 | BayesCell::bytes() const noexcept { 126 | return sizeof( BayesCell ) + (this->m_acc ? sizeof( Acc ) : 0); 127 | } 128 | 129 | 130 | template 131 | void 132 | BayesCell::fromStr( const std::string &str ) { 133 | auto splitted = split( str, ' ' ); 134 | 135 | if( splitted.size() != N + 2 ) 136 | throw std::runtime_error( to_string( "Wrong number of values, expected " ) + to_string( N + 2 ) + ", got " + to_string( splitted.size() ) ); 137 | 138 | uint32_t hits = std::stol( splitted[N] ); 139 | if( hits ) { 140 | if( !this->m_acc ) 141 | this->m_acc = std::unique_ptr( new Acc() ); 142 | 143 | this->m_acc->hits = hits; 144 | for( int i = 0; i < N; i++ ) 145 | this->m_acc->sum[i] = std::stod( splitted[i] ); 146 | } else { 147 | this->m_acc = nullptr; 148 | } 149 | 150 | m_visits = std::stol( splitted[N+1] ); 151 | } 152 | 153 | 154 | template 155 | std::string 156 | BayesCell::toStr() const { 157 | std::ostringstream sstr; 158 | 159 | sstr << std::setprecision( 30 ); 160 | 161 | if( this->m_acc ) 162 | sstr << this->m_acc->sum.transpose() << " " << this->m_acc->hits << " "; 163 | else 164 | for( int i = 0; i < N + 1; i++ ) 165 | sstr << "0 "; 166 | 167 | sstr << this->m_visits; 168 | 169 | return sstr.str(); 170 | } 171 | 172 | 173 | 174 | } /* namespace efs */ 175 | -------------------------------------------------------------------------------- /include/efslam/maps/cells/PointAccumulator.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Evidential FastSLAM - An evidential approach to SLAM 5 | * Copyright (c) 2013-2016, Joachim Clemens, Thomas Reineking, Tobias Kluth 6 | * All rights reserved. 7 | * 8 | * This file is partially based on the GMapping PointAccumulator class 9 | * Copyright (c) 2004-2007, Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard 10 | * Originally licensed under the Creative Commons (Attribution-NonCommercial-ShareAlike). 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions are met: 14 | * 15 | * * Redistributions of source code must retain the above copyright notice, this 16 | * list of conditions and the following disclaimer. 17 | * 18 | * * Redistributions in binary form must reproduce the above copyright notice, 19 | * this list of conditions and the following disclaimer in the documentation 20 | * and/or other materials provided with the distribution. 21 | * 22 | * * Neither the name of Evidential FastSLAM nor the names of its 23 | * contributors may be used to endorse or promote products derived from 24 | * this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 28 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 29 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 30 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 31 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 32 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 33 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 34 | * OR TORT (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 | 38 | #include 39 | namespace efs { 40 | 41 | 42 | template 43 | PointAccumulator::PointAccumulator( int i ) : 44 | m_acc( nullptr ) 45 | { 46 | // the i is just for compatibility with the map storages 47 | } 48 | 49 | 50 | template 51 | PointAccumulator::PointAccumulator( const PointAccumulator &p ) : 52 | m_acc( p.m_acc ? std::unique_ptr( new Acc( *p.m_acc ) ) : nullptr ) 53 | { 54 | // Nothing else to do here 55 | } 56 | 57 | 58 | template 59 | PointAccumulator::PointAccumulator( PointAccumulator &&p ) : 60 | m_acc( std::move( p.m_acc ) ) 61 | { 62 | // Nothing else to do here 63 | } 64 | 65 | 66 | template 67 | PointAccumulator::~PointAccumulator() { 68 | // Nothing to do here 69 | } 70 | 71 | 72 | template 73 | PointAccumulator& 74 | PointAccumulator::operator=( const PointAccumulator &p ) { 75 | if( this != &p ) { 76 | if( p.m_acc ) { 77 | if( !m_acc ) 78 | m_acc = std::unique_ptr( new Acc( *p.m_acc ) ); 79 | else 80 | *m_acc = *p.m_acc; 81 | } else { 82 | m_acc = nullptr; 83 | } 84 | } 85 | return *this; 86 | } 87 | 88 | 89 | template 90 | PointAccumulator& 91 | PointAccumulator::operator=( PointAccumulator &&p ) { 92 | if( this != &p ) { 93 | m_acc = std::move( p.m_acc ); 94 | } 95 | return *this; 96 | } 97 | 98 | 99 | template 100 | void 101 | PointAccumulator::update( const PointNw &p ) { 102 | if( !m_acc ) 103 | m_acc = std::unique_ptr( new Acc( p ) ); 104 | else 105 | *m_acc += p; 106 | } 107 | 108 | 109 | template 110 | void 111 | PointAccumulator::integrate( const PointAccumulator &pa ) { 112 | if( pa.m_acc ) { 113 | if( !m_acc ) 114 | m_acc = std::unique_ptr( new Acc( *pa.m_acc ) ); 115 | else 116 | *m_acc += *pa.m_acc; 117 | } 118 | } 119 | 120 | 121 | template 122 | typename PointAccumulator::PointNw 123 | PointAccumulator::mean() const { 124 | assert( m_acc ); 125 | assert( m_acc->hits ); 126 | return m_acc->sum / m_acc->hits; 127 | } 128 | 129 | 130 | template 131 | uint32_t 132 | PointAccumulator::hits() const { 133 | return m_acc ? m_acc->hits : 0; 134 | } 135 | 136 | 137 | template 138 | bool 139 | PointAccumulator::operator==( const PointAccumulator &other ) const noexcept { 140 | if( (bool) m_acc != (bool) other.m_acc ) 141 | return false; 142 | 143 | return !m_acc || (m_acc->hits == other.m_acc->hits && m_acc->sum == other.m_acc->sum); 144 | } 145 | 146 | 147 | template 148 | bool 149 | PointAccumulator::prune() const noexcept { 150 | return !m_acc; 151 | } 152 | 153 | 154 | template 155 | bool 156 | PointAccumulator::pruneEqual( const PointAccumulator &other ) const noexcept { 157 | return !other.m_acc; 158 | } 159 | 160 | 161 | template 162 | size_t 163 | PointAccumulator::bytes() const noexcept { 164 | return sizeof( PointAccumulator ) + (m_acc ? sizeof( Acc ) : 0); 165 | } 166 | 167 | 168 | } /* namespace efs */ 169 | --------------------------------------------------------------------------------