From ae3111229e5a211c8cf3e7a1c9ef3408bd99e354 Mon Sep 17 00:00:00 2001 From: Matt Spencer Date: Sat, 11 Jul 2026 20:40:47 +0100 Subject: [PATCH] use upstream slam_gmapping --- slam_gmapping/.gitignore | 29 - slam_gmapping/README.md | 17 - slam_gmapping/openslam_gmapping/CHANGELOG.rst | 25 - .../openslam_gmapping/CMakeLists.txt | 39 - slam_gmapping/openslam_gmapping/README | 2 - .../build_tools/Makefile.app | 80 -- .../Makefile.generic-shared-object | 109 --- .../build_tools/Makefile.subdirs | 16 - .../build_tools/generate_shared_object | 16 - .../openslam_gmapping/build_tools/message | 14 - .../build_tools/pretty_compiler | 26 - .../openslam_gmapping/build_tools/testlib | 26 - .../carmenwrapper/carmenwrapper.cpp | 490 ------------ .../carmenwrapper/carmenwrapper.h | 108 --- .../openslam_gmapping/gfs-carmen/Makefile | 21 - .../gfs-carmen/gfs-carmen.cpp | 243 ------ slam_gmapping/openslam_gmapping/grid/Makefile | 9 - .../openslam_gmapping/grid/graphmap.cpp | 59 -- .../openslam_gmapping/grid/map_test.cpp | 62 -- .../gridfastslam/CMakeLists.txt | 11 - .../gridfastslam/gfs2log.cpp | 64 -- .../gridfastslam/gfs2neff.cpp | 40 - .../gridfastslam/gfs2rec.cpp | 406 ---------- .../gridfastslam/gfs2stat.cpp | 58 -- .../gridfastslam/gfs2stream.cpp | 74 -- .../gridfastslam/gfsreader.cpp | 506 ------------ .../gridfastslam/gfsreader.h | 101 --- .../gridfastslam/gridslamprocessor.cpp | 518 ------------- .../gridfastslam/gridslamprocessor_tree.cpp | 263 ------- .../gridfastslam/motionmodel.cpp | 80 -- .../openslam_gmapping/gui/CMakeLists.txt | 38 - .../openslam_gmapping/gui/gfs2img.cpp | 258 ------- .../openslam_gmapping/gui/gfs_logplayer.cpp | 41 - .../openslam_gmapping/gui/gfs_nogui.cpp | 64 -- .../openslam_gmapping/gui/gfs_simplegui.cpp | 96 --- .../openslam_gmapping/gui/gsp_thread.cpp | 643 ---------------- .../openslam_gmapping/gui/gsp_thread.h | 177 ----- .../openslam_gmapping/gui/qgraphpainter.cpp | 142 ---- .../openslam_gmapping/gui/qgraphpainter.h | 67 -- .../openslam_gmapping/gui/qmappainter.cpp | 32 - .../openslam_gmapping/gui/qmappainter.h | 60 -- .../gui/qnavigatorwidget.cpp | 126 --- .../openslam_gmapping/gui/qnavigatorwidget.h | 35 - .../openslam_gmapping/gui/qparticleviewer.cpp | 453 ----------- .../openslam_gmapping/gui/qparticleviewer.h | 161 ---- .../openslam_gmapping/gui/qpixmapdumper.cpp | 33 - .../openslam_gmapping/gui/qpixmapdumper.h | 19 - .../gui/qslamandnavwidget.cpp | 128 ---- .../openslam_gmapping/gui/qslamandnavwidget.h | 38 - .../include/gmapping/grid/accessstate.h | 9 - .../include/gmapping/grid/array2d.h | 188 ----- .../include/gmapping/grid/harray2d.h | 200 ----- .../include/gmapping/grid/map.h | 265 ------- .../gmapping/gridfastslam/gridslamprocessor.h | 337 -------- .../gridfastslam/gridslamprocessor.hxx | 177 ----- .../gmapping/gridfastslam/motionmodel.h | 19 - .../include/gmapping/log/configuration.h | 17 - .../include/gmapping/log/sensorlog.h | 29 - .../gmapping/particlefilter/particlefilter.h | 328 -------- .../include/gmapping/scanmatcher/icp.h | 85 -- .../gmapping/scanmatcher/scanmatcher.h | 252 ------ .../include/gmapping/scanmatcher/smmap.h | 54 -- .../gmapping/sensor/sensor_base/sensor.h | 24 - .../sensor/sensor_base/sensorreading.h | 26 - .../sensor/sensor_odometry/odometryreading.h | 29 - .../sensor/sensor_odometry/odometrysensor.h | 20 - .../sensor/sensor_range/rangereading.h | 26 - .../sensor/sensor_range/rangesensor.h | 35 - .../include/gmapping/utils/autoptr.h | 97 --- .../include/gmapping/utils/commandline.h | 115 --- .../include/gmapping/utils/gvalues.h | 28 - .../include/gmapping/utils/macro_params.h | 38 - .../include/gmapping/utils/point.h | 207 ----- .../include/gmapping/utils/stat.h | 147 ---- .../openslam_gmapping/ini/gfs-LMS-10cm.ini | 78 -- .../openslam_gmapping/ini/gfs-LMS-20cm.ini | 76 -- .../openslam_gmapping/ini/gfs-LMS-5cm.ini | 78 -- .../openslam_gmapping/ini/gfs-PLS-10cm.ini | 78 -- .../openslam_gmapping/ini/gfs-PLS-5cm.ini | 78 -- slam_gmapping/openslam_gmapping/log/Makefile | 9 - .../log/carmenconfiguration.cpp | 463 ----------- .../log/carmenconfiguration.h | 22 - .../openslam_gmapping/log/configuration.cpp | 8 - .../openslam_gmapping/log/configuration.h | 17 - .../openslam_gmapping/log/log_plot.cpp | 71 -- .../openslam_gmapping/log/log_test.cpp | 47 -- .../openslam_gmapping/log/rdk2carmen.cpp | 58 -- .../log/scanstudio2carmen.cpp | 66 -- .../openslam_gmapping/log/sensorlog.cpp | 153 ---- .../openslam_gmapping/log/sensorlog.h | 29 - .../openslam_gmapping/log/sensorstream.cpp | 155 ---- .../openslam_gmapping/log/sensorstream.h | 47 -- slam_gmapping/openslam_gmapping/package.xml | 25 - .../particlefilter/particlefilter.cpp | 133 ---- .../particlefilter/particlefilter_test.cpp | 98 --- .../openslam_gmapping/particlefilter/pf.h | 175 ----- .../particlefilter/range_bearing.cpp | 99 --- .../scanmatcher/CMakeLists.txt | 4 - .../openslam_gmapping/scanmatcher/eig3.cpp | 270 ------- .../openslam_gmapping/scanmatcher/eig3.h | 11 - .../scanmatcher/gridlinetraversal.h | 128 ---- .../openslam_gmapping/scanmatcher/icptest.cpp | 91 --- .../openslam_gmapping/scanmatcher/lumiles.h | 50 -- .../scanmatcher/scanmatch_test.cpp | 186 ----- .../scanmatcher/scanmatcher.cpp | 725 ------------------ .../scanmatcher/scanmatcher.new.cpp | 426 ---------- .../scanmatcher/scanmatcherprocessor.cpp | 233 ------ .../scanmatcher/scanmatcherprocessor.h | 48 -- .../openslam_gmapping/scanmatcher/smmap.cpp | 15 - .../openslam_gmapping/sensor/CMakeLists.txt | 8 - .../openslam_gmapping/sensor/Makefile | 5 - .../sensor/sensor_base/CMakeLists.txt | 4 - .../sensor/sensor_base/sensor.cpp | 12 - .../sensor/sensor_base/sensoreading.h | 18 - .../sensor/sensor_base/sensorreading.cpp | 15 - .../sensor/sensor_odometry/CMakeLists.txt | 6 - .../sensor_odometry/odometryreading.cpp | 9 - .../sensor/sensor_odometry/odometrysensor.cpp | 9 - .../sensor/sensor_range/CMakeLists.txt | 7 - .../sensor/sensor_range/rangereading.cpp | 114 --- .../sensor/sensor_range/rangesensor.cpp | 30 - .../openslam_gmapping/utils/CMakeLists.txt | 5 - .../openslam_gmapping/utils/autoptr_test.cpp | 29 - .../openslam_gmapping/utils/datasmoother.h | 451 ----------- .../openslam_gmapping/utils/dmatrix.h | 232 ------ .../openslam_gmapping/utils/movement.cpp | 108 --- .../openslam_gmapping/utils/movement.h | 45 -- .../openslam_gmapping/utils/optimizer.h | 159 ---- .../utils/orientedboundingbox.h | 31 - .../utils/orientedboundingbox.hxx | 116 --- .../openslam_gmapping/utils/printmemusage.cpp | 27 - .../openslam_gmapping/utils/printmemusage.h | 13 - .../openslam_gmapping/utils/printpgm.h | 19 - .../openslam_gmapping/utils/stat.cpp | 279 ------- .../openslam_gmapping/utils/stat_test.cpp | 66 -- slam_gmapping/slam_gmapping/CMakeLists.txt | 63 -- .../include/slam_gmapping/slam_gmapping.h | 152 ---- .../launch/slam_gmapping.launch.py | 29 - slam_gmapping/slam_gmapping/package.xml | 36 - .../slam_gmapping/rviz/gmapping.rviz | 295 ------- .../slam_gmapping/src/slam_gmapping.cpp | 541 ------------- upstream.jazzy.repos | 4 + 142 files changed, 4 insertions(+), 16058 deletions(-) delete mode 100644 slam_gmapping/.gitignore delete mode 100644 slam_gmapping/README.md delete mode 100644 slam_gmapping/openslam_gmapping/CHANGELOG.rst delete mode 100644 slam_gmapping/openslam_gmapping/CMakeLists.txt delete mode 100644 slam_gmapping/openslam_gmapping/README delete mode 100644 slam_gmapping/openslam_gmapping/build_tools/Makefile.app delete mode 100644 slam_gmapping/openslam_gmapping/build_tools/Makefile.generic-shared-object delete mode 100644 slam_gmapping/openslam_gmapping/build_tools/Makefile.subdirs delete mode 100755 slam_gmapping/openslam_gmapping/build_tools/generate_shared_object delete mode 100755 slam_gmapping/openslam_gmapping/build_tools/message delete mode 100755 slam_gmapping/openslam_gmapping/build_tools/pretty_compiler delete mode 100755 slam_gmapping/openslam_gmapping/build_tools/testlib delete mode 100644 slam_gmapping/openslam_gmapping/carmenwrapper/carmenwrapper.cpp delete mode 100644 slam_gmapping/openslam_gmapping/carmenwrapper/carmenwrapper.h delete mode 100644 slam_gmapping/openslam_gmapping/gfs-carmen/Makefile delete mode 100644 slam_gmapping/openslam_gmapping/gfs-carmen/gfs-carmen.cpp delete mode 100644 slam_gmapping/openslam_gmapping/grid/Makefile delete mode 100644 slam_gmapping/openslam_gmapping/grid/graphmap.cpp delete mode 100644 slam_gmapping/openslam_gmapping/grid/map_test.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gridfastslam/CMakeLists.txt delete mode 100644 slam_gmapping/openslam_gmapping/gridfastslam/gfs2log.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gridfastslam/gfs2neff.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gridfastslam/gfs2rec.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gridfastslam/gfs2stat.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gridfastslam/gfs2stream.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gridfastslam/gfsreader.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gridfastslam/gfsreader.h delete mode 100644 slam_gmapping/openslam_gmapping/gridfastslam/gridslamprocessor.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gridfastslam/gridslamprocessor_tree.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gridfastslam/motionmodel.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gui/CMakeLists.txt delete mode 100644 slam_gmapping/openslam_gmapping/gui/gfs2img.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gui/gfs_logplayer.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gui/gfs_nogui.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gui/gfs_simplegui.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gui/gsp_thread.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gui/gsp_thread.h delete mode 100644 slam_gmapping/openslam_gmapping/gui/qgraphpainter.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gui/qgraphpainter.h delete mode 100644 slam_gmapping/openslam_gmapping/gui/qmappainter.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gui/qmappainter.h delete mode 100644 slam_gmapping/openslam_gmapping/gui/qnavigatorwidget.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gui/qnavigatorwidget.h delete mode 100644 slam_gmapping/openslam_gmapping/gui/qparticleviewer.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gui/qparticleviewer.h delete mode 100644 slam_gmapping/openslam_gmapping/gui/qpixmapdumper.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gui/qpixmapdumper.h delete mode 100644 slam_gmapping/openslam_gmapping/gui/qslamandnavwidget.cpp delete mode 100644 slam_gmapping/openslam_gmapping/gui/qslamandnavwidget.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/grid/accessstate.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/grid/array2d.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/grid/harray2d.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/grid/map.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.hxx delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/gridfastslam/motionmodel.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/log/configuration.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/log/sensorlog.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/particlefilter/particlefilter.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/scanmatcher/icp.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/scanmatcher/scanmatcher.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/scanmatcher/smmap.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_base/sensor.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_base/sensorreading.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometryreading.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometrysensor.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_range/rangereading.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_range/rangesensor.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/utils/autoptr.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/utils/commandline.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/utils/gvalues.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/utils/macro_params.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/utils/point.h delete mode 100644 slam_gmapping/openslam_gmapping/include/gmapping/utils/stat.h delete mode 100644 slam_gmapping/openslam_gmapping/ini/gfs-LMS-10cm.ini delete mode 100644 slam_gmapping/openslam_gmapping/ini/gfs-LMS-20cm.ini delete mode 100644 slam_gmapping/openslam_gmapping/ini/gfs-LMS-5cm.ini delete mode 100644 slam_gmapping/openslam_gmapping/ini/gfs-PLS-10cm.ini delete mode 100644 slam_gmapping/openslam_gmapping/ini/gfs-PLS-5cm.ini delete mode 100644 slam_gmapping/openslam_gmapping/log/Makefile delete mode 100644 slam_gmapping/openslam_gmapping/log/carmenconfiguration.cpp delete mode 100644 slam_gmapping/openslam_gmapping/log/carmenconfiguration.h delete mode 100644 slam_gmapping/openslam_gmapping/log/configuration.cpp delete mode 100644 slam_gmapping/openslam_gmapping/log/configuration.h delete mode 100644 slam_gmapping/openslam_gmapping/log/log_plot.cpp delete mode 100644 slam_gmapping/openslam_gmapping/log/log_test.cpp delete mode 100644 slam_gmapping/openslam_gmapping/log/rdk2carmen.cpp delete mode 100644 slam_gmapping/openslam_gmapping/log/scanstudio2carmen.cpp delete mode 100644 slam_gmapping/openslam_gmapping/log/sensorlog.cpp delete mode 100644 slam_gmapping/openslam_gmapping/log/sensorlog.h delete mode 100644 slam_gmapping/openslam_gmapping/log/sensorstream.cpp delete mode 100644 slam_gmapping/openslam_gmapping/log/sensorstream.h delete mode 100644 slam_gmapping/openslam_gmapping/package.xml delete mode 100644 slam_gmapping/openslam_gmapping/particlefilter/particlefilter.cpp delete mode 100644 slam_gmapping/openslam_gmapping/particlefilter/particlefilter_test.cpp delete mode 100644 slam_gmapping/openslam_gmapping/particlefilter/pf.h delete mode 100644 slam_gmapping/openslam_gmapping/particlefilter/range_bearing.cpp delete mode 100644 slam_gmapping/openslam_gmapping/scanmatcher/CMakeLists.txt delete mode 100644 slam_gmapping/openslam_gmapping/scanmatcher/eig3.cpp delete mode 100644 slam_gmapping/openslam_gmapping/scanmatcher/eig3.h delete mode 100644 slam_gmapping/openslam_gmapping/scanmatcher/gridlinetraversal.h delete mode 100644 slam_gmapping/openslam_gmapping/scanmatcher/icptest.cpp delete mode 100644 slam_gmapping/openslam_gmapping/scanmatcher/lumiles.h delete mode 100644 slam_gmapping/openslam_gmapping/scanmatcher/scanmatch_test.cpp delete mode 100644 slam_gmapping/openslam_gmapping/scanmatcher/scanmatcher.cpp delete mode 100644 slam_gmapping/openslam_gmapping/scanmatcher/scanmatcher.new.cpp delete mode 100644 slam_gmapping/openslam_gmapping/scanmatcher/scanmatcherprocessor.cpp delete mode 100644 slam_gmapping/openslam_gmapping/scanmatcher/scanmatcherprocessor.h delete mode 100644 slam_gmapping/openslam_gmapping/scanmatcher/smmap.cpp delete mode 100644 slam_gmapping/openslam_gmapping/sensor/CMakeLists.txt delete mode 100644 slam_gmapping/openslam_gmapping/sensor/Makefile delete mode 100644 slam_gmapping/openslam_gmapping/sensor/sensor_base/CMakeLists.txt delete mode 100644 slam_gmapping/openslam_gmapping/sensor/sensor_base/sensor.cpp delete mode 100644 slam_gmapping/openslam_gmapping/sensor/sensor_base/sensoreading.h delete mode 100644 slam_gmapping/openslam_gmapping/sensor/sensor_base/sensorreading.cpp delete mode 100644 slam_gmapping/openslam_gmapping/sensor/sensor_odometry/CMakeLists.txt delete mode 100644 slam_gmapping/openslam_gmapping/sensor/sensor_odometry/odometryreading.cpp delete mode 100644 slam_gmapping/openslam_gmapping/sensor/sensor_odometry/odometrysensor.cpp delete mode 100644 slam_gmapping/openslam_gmapping/sensor/sensor_range/CMakeLists.txt delete mode 100644 slam_gmapping/openslam_gmapping/sensor/sensor_range/rangereading.cpp delete mode 100644 slam_gmapping/openslam_gmapping/sensor/sensor_range/rangesensor.cpp delete mode 100644 slam_gmapping/openslam_gmapping/utils/CMakeLists.txt delete mode 100644 slam_gmapping/openslam_gmapping/utils/autoptr_test.cpp delete mode 100644 slam_gmapping/openslam_gmapping/utils/datasmoother.h delete mode 100644 slam_gmapping/openslam_gmapping/utils/dmatrix.h delete mode 100644 slam_gmapping/openslam_gmapping/utils/movement.cpp delete mode 100644 slam_gmapping/openslam_gmapping/utils/movement.h delete mode 100644 slam_gmapping/openslam_gmapping/utils/optimizer.h delete mode 100644 slam_gmapping/openslam_gmapping/utils/orientedboundingbox.h delete mode 100644 slam_gmapping/openslam_gmapping/utils/orientedboundingbox.hxx delete mode 100644 slam_gmapping/openslam_gmapping/utils/printmemusage.cpp delete mode 100644 slam_gmapping/openslam_gmapping/utils/printmemusage.h delete mode 100644 slam_gmapping/openslam_gmapping/utils/printpgm.h delete mode 100644 slam_gmapping/openslam_gmapping/utils/stat.cpp delete mode 100644 slam_gmapping/openslam_gmapping/utils/stat_test.cpp delete mode 100644 slam_gmapping/slam_gmapping/CMakeLists.txt delete mode 100644 slam_gmapping/slam_gmapping/include/slam_gmapping/slam_gmapping.h delete mode 100644 slam_gmapping/slam_gmapping/launch/slam_gmapping.launch.py delete mode 100644 slam_gmapping/slam_gmapping/package.xml delete mode 100644 slam_gmapping/slam_gmapping/rviz/gmapping.rviz delete mode 100644 slam_gmapping/slam_gmapping/src/slam_gmapping.cpp diff --git a/slam_gmapping/.gitignore b/slam_gmapping/.gitignore deleted file mode 100644 index 37e6331..0000000 --- a/slam_gmapping/.gitignore +++ /dev/null @@ -1,29 +0,0 @@ -*/build/ -*/build_isolated/ -*/devel/ -*/devel_isolated/ -*/build_isolated/ -*/install_isolated/ -*/logs/ -docs/ -*/.catkin_workspace -*/DISABLED -*.bag -*.pyc -cmake-build-debug/ -cmake-build-release/ -*.idea -*.swp -*.kate-swp -*.director -*.DS_STORE -*.catkin_tools -*.bag -*.bag.active -*.stl -*.directory -build -log -!openslam_gmapping/include/gmapping/log/ -!openslam_gmapping/log/ -install diff --git a/slam_gmapping/README.md b/slam_gmapping/README.md deleted file mode 100644 index c3babdb..0000000 --- a/slam_gmapping/README.md +++ /dev/null @@ -1,17 +0,0 @@ -# SLAM_GMAPPING - -SLAM(Simultaneous Localization and Mapping) is the computational problem of constructing or updating a map of an unknown environment while simultaneously keeping track of an agent's location within it. - -This contains package ```openslam_gmapping``` and ```slam_gmapping``` which is a ROS2 wrapper for OpenSlam's Gmapping. Using slam_gmapping, you can create a 2-D occupancy grid map (like a building floorplan) from laser and pose data collected by a mobile robot. - -## Launch: - -```bash -ros2 launch slam_gmapping slam_gmapping.launch.py -``` - -The node slam_gmapping subscribes to sensor_msgs/LaserScan on ros2 topic ``scan``. It also expects appropriate TF to be available. - -It publishes the nav_msgs/OccupancyGrid on ``map``. - -Map Meta Data and Entropy is published on ``map_metadata`` and ``entropy`` respectively. diff --git a/slam_gmapping/openslam_gmapping/CHANGELOG.rst b/slam_gmapping/openslam_gmapping/CHANGELOG.rst deleted file mode 100644 index 1649f0d..0000000 --- a/slam_gmapping/openslam_gmapping/CHANGELOG.rst +++ /dev/null @@ -1,25 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package openslam_gmapping -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.1.2 (2016-04-23) ------------------- -* better Windows compilation - This is taken from `#9 `_ which can now be closed. -* fix a few more graphics stuff for Qt5 -* get GUI back in shape for those interested -* use srand instead of srand48 - srand48 is non-standard and we are using a seed that is an - unsigned int so we might as well use srand -* Contributors: Vincent Rabaud - -0.1.1 (2015-06-25) ------------------- -* fix cppcheck warnings -* License from BSD to CC -* Contributors: Isaac IY Saito, Vincent Rabaud - -0.1.0 (2013-06-28 17:33:53 -0700) ---------------------------------- -- Forked from https://openslam.informatik.uni-freiburg.de/data/svn/gmapping/trunk/ -- Catkinized and prepared for release into the ROS ecosystem diff --git a/slam_gmapping/openslam_gmapping/CMakeLists.txt b/slam_gmapping/openslam_gmapping/CMakeLists.txt deleted file mode 100644 index b63faba..0000000 --- a/slam_gmapping/openslam_gmapping/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(openslam_gmapping) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-O3 -funroll-loops) -endif() - -add_compile_options(-fPIC) -find_package(ament_cmake REQUIRED) - -include_directories(include) - -add_subdirectory(gridfastslam) -add_subdirectory(scanmatcher) -add_subdirectory(sensor) -add_subdirectory(utils) - -ament_export_libraries(gridfastslam) -ament_export_libraries(scanmatcher) -ament_export_libraries(sensor_base) -ament_export_libraries(sensor_odometry) -ament_export_libraries(sensor_range) -ament_export_libraries(utils) -ament_export_include_directories(include) - -install(DIRECTORY include/ - DESTINATION include/) - -ament_package() diff --git a/slam_gmapping/openslam_gmapping/README b/slam_gmapping/openslam_gmapping/README deleted file mode 100644 index c5b275a..0000000 --- a/slam_gmapping/openslam_gmapping/README +++ /dev/null @@ -1,2 +0,0 @@ -This is a fork from gmapping at https://openslam.informatik.uni-freiburg.de/data/svn/gmapping/trunk/ -It includes a few patches that could be pushed upstream if needed diff --git a/slam_gmapping/openslam_gmapping/build_tools/Makefile.app b/slam_gmapping/openslam_gmapping/build_tools/Makefile.app deleted file mode 100644 index 9668830..0000000 --- a/slam_gmapping/openslam_gmapping/build_tools/Makefile.app +++ /dev/null @@ -1,80 +0,0 @@ -# Makefile generico per applicazione -# -# Variabili: -# APPS lista delle applicazioni -# OBJS lista degli oggetti -# QOBJS lista degli oggetti QT -# LIBS librerie -# -# Ogni applicazione viene linkata con tutti gli oggetti - -export VERBOSE - -ifeq ($(LINUX),1) -CPPFLAGS+=-DLINUX -endif - - -APPLICATIONS= $(foreach a, $(APPS),$(BINDIR)/$(a)) -all: $(APPLICATIONS) - -PACKAGE=$(notdir $(shell pwd)) - -.SECONDARY: $(OBJS) $(QOBJS) -.PHONY: all clean copy doc - -$(QOBJS): %.o: %.cpp moc_%.cpp - @$(MESSAGE) "Compiling (QT) $@" - @$(PRETTY) "$(CXX) $(CPPFLAGS) $(QT_INCLUDE) $(CXXFLAGS) -c $< -o $@" - -moc_%.cpp: %.h - @$(MESSAGE) "Generating MOC $@" - @$(PRETTY) "$(MOC) -i $< -o $@" - -# Generazione degli oggetti -%.o: %.cpp - @$(MESSAGE) "Compiling $@" - @$(PRETTY) "$(CXX) $(CPPFLAGS) $(CXXFLAGS) -c $< -o $@" - -# Generazione delle applicazioni -$(BINDIR)/%: %.cpp $(OBJS) $(QOBJS) - @$(MESSAGE) "Linking application `basename $@`" - @$(PRETTY) "$(CXX) $(CPPFLAGS) $(CXXFLAGS) $(OBJS) $(QOBJS) $< -L$(LIBDIR) $(LIBS) -o $@" - -#Regole per la generazione di tabelle o altri file creati automaticamente -table_%.cpp: gen_table_% - @$(MESSAGE) "Generating $@" - @$(PRETTY) "./$< > $@" - -gen_table_%: gen_table_%.cpp - @$(MESSAGE) "Generating $@" - @$(PRETTY) "$(CXX) $(CPPFLAGS) $(CXXFLAGS) $< -o $@" - -#Regole per la generazione delle dipendenze -OBJDEPS=$(foreach module,$(basename $(OBJS) $(QOBJS)),$(module).d) - -$(OBJDEPS): %.d: %.cpp # ci va o no? %.h - @$(MESSAGE) "Generating dependecies $@" - @$(PRETTY) "$(CXX) $(CPPFLAGS) -MM -MG -MF $@ $<" - -ifneq ($(MAKECMDGOALS),clean) -ifneq ($(MAKECMDGOALS),copy) --include $(OBJDEPS) -endif -endif - -doc: - rm -rf doc/$(PACKAGE) -ifeq ($(strip $(DOCTITLE)),) - kdoc -L doc -d doc/$(PACKAGE) -n "Package $(PACKAGE) (lib$(PACKAGE).so)" $(HEADERS) -else - kdoc -L doc -d doc/$(PACKAGE) -n "$(DOCTITLE) (lib$(PACKAGE).so)" $(HEADERS) -endif - -clean: - @$(MESSAGE) "Cleaning $(PACKAGE)" - @$(PRETTY) "rm -f *.d *.o moc_*.cpp *.d core *~ table_*.cpp gen_table*[^.][^c][^p][^p] $(APPLICATIONS)" - @$(PRETTY) "rm -rf doc/$(PACKAGE)" - -copy: clean - tar -C .. -cvzf `date +../$(PACKAGE)-%d%b%y.tgz` $(PACKAGE) diff --git a/slam_gmapping/openslam_gmapping/build_tools/Makefile.generic-shared-object b/slam_gmapping/openslam_gmapping/build_tools/Makefile.generic-shared-object deleted file mode 100644 index 8bb7ec6..0000000 --- a/slam_gmapping/openslam_gmapping/build_tools/Makefile.generic-shared-object +++ /dev/null @@ -1,109 +0,0 @@ -# Makefile generico per shared object -export VERBOSE -export CXX - -# Nome del package -PACKAGE=$(notdir $(shell pwd)) - -# Libreria da generare: -# Se non si setta la variabile LIBNAME la libreria si chiama -# come la directory -ifndef LIBNAME -LIBNAME=$(PACKAGE) -endif - -ifeq ($(MACOSX),1) -SONAME=$(LIBDIR)/lib$(LIBNAME).dylib -endif - -ifeq ($(LINUX),1) -SONAME=$(LIBDIR)/lib$(LIBNAME).so -endif - -APPLICATIONS= $(foreach a, $(APPS),$(BINDIR)/$(a)) -INSTALL_SCRIPTS=$(foreach a, $(SCRIPTS),$(BINDIR)/$(a)) - -all: $(SONAME) $(APPLICATIONS) $(INSTALL_SCRIPTS) - -.SECONDARY: $(OBJS) $(COBJS) -.PHONY: all clean copy doc - -# Generazione della libreria -$(SONAME): $(OBJS) $(COBJS) - @$(MESSAGE) "Creating library lib$(LIBNAME).so" -ifeq ($(MACOSX),1) - @$(PRETTY) "$(CXX) $(LDFLAGS) -dynamiclib $(OBJS) $(COBJS) -L$(LIBDIR) $(LIBS) -install_name $@ -o $@" -endif -ifeq ($(LINUX),1) - @$(PRETTY) "$(CXX) -fPIC -shared $(OBJS) $(COBJS) -L $(LIBDIR) $(LIBS) $(LDFLAGS) -o $@" - @if ! $(PRETTY) "$(TESTLIB) $(SONAME)"; then $(MESSAGE) "Testing of $(SONAME) failed."; rm $(SONAME); exit 1; fi; -endif - -# Generazione delle applicazioni -$(BINDIR)/%: %.o $(SONAME) - @$(MESSAGE) "Linking application `basename "$@"`" - @$(PRETTY) "$(CXX) $< -l$(LIBNAME) $(LDFLAGS) -L$(LIBDIR) $(LIBS) -o $@" - -#Generazione dei moc files -moc_%.cpp: %.h - @$(MESSAGE) "Compiling MOC $@" - @$(PRETTY) "$(MOC) -i $< -o $@" - -# Generazione degli oggetti -%.o: %.cpp - @$(MESSAGE) "Compiling $<" - @$(PRETTY) "$(CXX) -fPIC $(CPPFLAGS) $(CXXFLAGS) -c $< -o $@" - -%.o: %.c - @$(MESSAGE) "Compiling $<" - @$(PRETTY) "$(CC) -fPIC $(CPPFLAGS) $(CFLAGS) -c $< -o $@" - -#Regole per la generazione delle dipendenze -OBJDEPS= $(foreach module,$(basename $(OBJS)),$(module).d) $(foreach a, $(APPS),$(a).d) -COBJDEPS=$(foreach module,$(basename $(COBJS)),$(module).d) - -$(OBJDEPS): %.d: %.cpp - @$(MESSAGE) "Generating dependencies for $<" - @$(PRETTY) "$(CXX) $(CPPFLAGS) -MM -MG $< -MF $@" - -$(COBJDEPS): %.d: %.c - @$(MESSAGE) "Generating dependencies for $<" - @$(PRETTY) "$(CC) $(CPPFLAGS) -MM -MG $< -MF $@" - -#HEADERS=`ls *.h` -#PRECOMPILED_HEADERS=$(foreach file,$(basename $(HEADERS)), $(file).pch) - -ifneq ($(MAKECMDGOALS),clean) -ifneq ($(MAKECMDGOALS),copy) -ifneq ($(MAKECMDGOALS),dep) --include $(OBJDEPS) $(COBJDEPS) -endif -endif -endif - -dep: $(OBJDEPS) $(COBJDEPS) - - -# GLi script vengono semplicemente copiati -$(BINDIR)/%.sh: %.sh - @$(MESSAGE) "Installing script `basename "$@"`" - @$(PRETTY) "cp $< $@" - @$(PRETTY) "chmod +x $@" - - -#doc: -# rm -rf doc/$(PACKAGE) -#ifeq ($(strip $(DOCTITLE)),) -# kdoc -L doc -d doc/$(PACKAGE) -n "Package $(PACKAGE) (lib$(PACKAGE).so)" $(HEADERS) -#else -# kdoc -L doc -d doc/$(PACKAGE) -n "$(DOCTITLE) (lib$(PACKAGE).so)" $(HEADERS) -#endif - - -clean: - @$(MESSAGE) "Cleaning $(PACKAGE)" - @$(PRETTY) "rm -f $(SONAME) $(APPLICATIONS)" - @$(PRETTY) "rm -f *.o *.d core *~ moc_*.cpp" - -copy: clean - tar -C .. -cvzf `date +../$(PACKAGE)-%d%b%y.tgz` $(PACKAGE) diff --git a/slam_gmapping/openslam_gmapping/build_tools/Makefile.subdirs b/slam_gmapping/openslam_gmapping/build_tools/Makefile.subdirs deleted file mode 100644 index 879bb45..0000000 --- a/slam_gmapping/openslam_gmapping/build_tools/Makefile.subdirs +++ /dev/null @@ -1,16 +0,0 @@ -export VERBOSE - -.PHONY: clean, all - -ifeq ($(VERBOSE), 0) -QUIET=--no-print-directory -endif - -all: - @for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; if ! $(MAKE) $(QUIET) -C $$subdir; then $(MESSAGE) "Compilation in $$subdir failed."; exit 1; fi; done - -clean: - @for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; $(MAKE) $(QUIET) -C $$subdir clean; done - -dep: - @for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; $(MAKE) $(QUIET) -C $$subdir dep; done diff --git a/slam_gmapping/openslam_gmapping/build_tools/generate_shared_object b/slam_gmapping/openslam_gmapping/build_tools/generate_shared_object deleted file mode 100755 index 422be75..0000000 --- a/slam_gmapping/openslam_gmapping/build_tools/generate_shared_object +++ /dev/null @@ -1,16 +0,0 @@ -#!/bin/tcsh - -echo decompressing file $1 - -set FILELIST=`ar -t $1` -echo "Object files:" -foreach i ($FILELIST) - echo $i -end - -echo generating $1:r.so - -ar -x $1 -ld -shared -o $1:r.so $FILELIST - -rm $FILELIST diff --git a/slam_gmapping/openslam_gmapping/build_tools/message b/slam_gmapping/openslam_gmapping/build_tools/message deleted file mode 100755 index 219ca21..0000000 --- a/slam_gmapping/openslam_gmapping/build_tools/message +++ /dev/null @@ -1,14 +0,0 @@ -#!/bin/sh - -#echo "message: verbose = $VERBOSE" - -if ($VERBOSE) -then - exit 0; -fi - -a=$MAKELEVEL - -while ((0<$a)); do echo -n " "; let "a = $a - 1";done - -echo $1 diff --git a/slam_gmapping/openslam_gmapping/build_tools/pretty_compiler b/slam_gmapping/openslam_gmapping/build_tools/pretty_compiler deleted file mode 100755 index c57ce31..0000000 --- a/slam_gmapping/openslam_gmapping/build_tools/pretty_compiler +++ /dev/null @@ -1,26 +0,0 @@ -#!/bin/sh - - -#echo "pretty: verbose = $VERBOSE" - -if ($VERBOSE) -then - echo $1; - if ! eval $1 - then - echo "Failed command was:" - echo $1 - echo "in directory " `pwd` - exit 1 - fi -else - if ! eval $1 - then - echo "Failed command was:" - echo $1 - echo "in directory " `pwd` - exit 1 - fi -fi - -exit 0 diff --git a/slam_gmapping/openslam_gmapping/build_tools/testlib b/slam_gmapping/openslam_gmapping/build_tools/testlib deleted file mode 100755 index 28702e4..0000000 --- a/slam_gmapping/openslam_gmapping/build_tools/testlib +++ /dev/null @@ -1,26 +0,0 @@ -#!/bin/bash -if [ -z "$1" ]; then - echo "Syntax: rtestlib " - exit 1 -fi - -exit 0 - -FNAME=`mktemp rtestlibXXXXXX` -echo "int main() { return 0; }" > $FNAME.cpp - -g++ $1 $FNAME.cpp -o $FNAME -result=$? -rm -f $FNAME.cpp $FNAME - -exit $result - -#if g++ $1 $FNAME.cpp -o $FNAME -#then# -# rm -f $FNAME.cpp $FNAME -# exit 1 -#else -# rm -f $FNAME.cpp $FNAME -# exit 0 -#fi - diff --git a/slam_gmapping/openslam_gmapping/carmenwrapper/carmenwrapper.cpp b/slam_gmapping/openslam_gmapping/carmenwrapper/carmenwrapper.cpp deleted file mode 100644 index 9c193e7..0000000 --- a/slam_gmapping/openslam_gmapping/carmenwrapper/carmenwrapper.cpp +++ /dev/null @@ -1,490 +0,0 @@ -/***************************************************************** - * - * This file is part of the GMAPPING project - * - * GMAPPING Copyright (c) 2004 Giorgio Grisetti, - * Cyrill Stachniss, and Wolfram Burgard - * - * This software is licensed under the "Creative Commons - * License (Attribution-NonCommercial-ShareAlike 2.0)" - * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, - * and Wolfram Burgard. - * - * Further information on this license can be found at: - * http://creativecommons.org/licenses/by-nc-sa/2.0/ - * - * GMAPPING is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR - * PURPOSE. - * - *****************************************************************/ - - -#include "carmenwrapper.h" - -using namespace GMapping; -using namespace std; - -//static vars for the carmenwrapper -SensorMap CarmenWrapper::m_sensorMap; -deque CarmenWrapper::m_rangeDeque; -pthread_mutex_t CarmenWrapper::m_mutex; -sem_t CarmenWrapper::m_dequeSem; -pthread_mutex_t CarmenWrapper::m_lock; -pthread_t CarmenWrapper::m_readingThread; -RangeSensor* CarmenWrapper::m_frontLaser=0; -RangeSensor* CarmenWrapper::m_rearLaser=0; -bool CarmenWrapper::m_threadRunning=false; -OrientedPoint CarmenWrapper::m_truepos; -bool CarmenWrapper::stopped=true; - - -void CarmenWrapper::initializeIPC(const char* name) { - carmen_ipc_initialize(1,(char **)&name); -} - - - - -int CarmenWrapper::registerLocalizationMessages(){ - lock(); - IPC_RETURN_TYPE err; - - /* register globalpos message */ - err = IPC_defineMsg(CARMEN_LOCALIZE_GLOBALPOS_NAME, IPC_VARIABLE_LENGTH, - CARMEN_LOCALIZE_GLOBALPOS_FMT); - carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_GLOBALPOS_NAME); - - /* register robot particle message */ - err = IPC_defineMsg(CARMEN_LOCALIZE_PARTICLE_NAME, IPC_VARIABLE_LENGTH, - CARMEN_LOCALIZE_PARTICLE_FMT); - carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_PARTICLE_NAME); - -/* - carmen_localize_subscribe_initialize_placename_message(NULL, - (carmen_handler_t) - carmen_localize_initialize_placename_handler, - CARMEN_SUBSCRIBE_LATEST); - - // register map request message - err = IPC_defineMsg(CARMEN_LOCALIZE_MAP_QUERY_NAME, IPC_VARIABLE_LENGTH, - CARMEN_LOCALIZE_MAP_QUERY_FMT); - carmen_test_ipc_exit(err, "Could not define", - CARMEN_LOCALIZE_MAP_QUERY_NAME); - - err = IPC_defineMsg(CARMEN_LOCALIZE_MAP_NAME, IPC_VARIABLE_LENGTH, - CARMEN_LOCALIZE_MAP_FMT); - carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_MAP_NAME); - - // subscribe to map request message - err = IPC_subscribe(CARMEN_LOCALIZE_MAP_QUERY_NAME, map_query_handler, NULL); - carmen_test_ipc(err, "Could not subscribe", CARMEN_LOCALIZE_MAP_QUERY_NAME); - IPC_setMsgQueueLength(CARMEN_LOCALIZE_MAP_QUERY_NAME, 1); - - - // register globalpos request message - err = IPC_defineMsg(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME, - IPC_VARIABLE_LENGTH, - CARMEN_DEFAULT_MESSAGE_FMT); - carmen_test_ipc_exit(err, "Could not define", - CARMEN_LOCALIZE_MAP_QUERY_NAME); - - // subscribe to globalpos request message - err = IPC_subscribe(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME, - globalpos_query_handler, NULL); - carmen_test_ipc(err, "Could not subscribe", - CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME); - IPC_setMsgQueueLength(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME, 1); -*/ - unlock(); - return 0; -} - -bool CarmenWrapper::start(const char* name){ - if (m_threadRunning) - return false; - carmen_robot_subscribe_frontlaser_message(NULL, (carmen_handler_t)robot_frontlaser_handler, CARMEN_SUBSCRIBE_LATEST); - carmen_robot_subscribe_rearlaser_message(NULL, (carmen_handler_t)robot_rearlaser_handler, CARMEN_SUBSCRIBE_LATEST); - carmen_simulator_subscribe_truepos_message(NULL,(carmen_handler_t) simulator_truepos_handler, CARMEN_SUBSCRIBE_LATEST); - - IPC_RETURN_TYPE err; - - err = IPC_subscribe(CARMEN_NAVIGATOR_GO_NAME, navigator_go_handler, NULL); - carmen_test_ipc_exit(err, "Could not subscribe", - CARMEN_NAVIGATOR_GO_NAME); - IPC_setMsgQueueLength(CARMEN_NAVIGATOR_GO_NAME, 1); - - err = IPC_subscribe(CARMEN_NAVIGATOR_STOP_NAME, navigator_stop_handler, NULL); - carmen_test_ipc_exit(err, "Could not subscribe", - CARMEN_NAVIGATOR_STOP_NAME); - IPC_setMsgQueueLength(CARMEN_NAVIGATOR_STOP_NAME, 1); - - - - signal(SIGINT, shutdown_module); - pthread_mutex_init(&m_mutex, 0); - pthread_mutex_init(&m_lock, 0); - sem_init(&m_dequeSem, 0, 0); - m_threadRunning=true; - pthread_create (&m_readingThread,0,m_reading_function,0); - return true; -} - -void CarmenWrapper::lock(){ - //cerr <<"LOCK" << endl; - pthread_mutex_lock(&m_lock); -} - -void CarmenWrapper::unlock(){ - //cerr <<"UNLOCK" << endl; - pthread_mutex_unlock(&m_lock); -} - - -bool CarmenWrapper::sensorMapComputed(){ - pthread_mutex_lock(&m_mutex); - bool smok=m_frontLaser; - pthread_mutex_unlock(&m_mutex); - return smok; -} - -const SensorMap& CarmenWrapper::sensorMap(){ - return m_sensorMap; -} - -bool CarmenWrapper::isRunning(){ - return m_threadRunning; -} - -bool CarmenWrapper::isStopped(){ - return stopped; -} - -int CarmenWrapper::queueLength(){ - int ql=0; - pthread_mutex_lock(&m_mutex); - ql=m_rangeDeque.size(); - pthread_mutex_unlock(&m_mutex); - return ql; -} - -OrientedPoint CarmenWrapper::getTruePos(){ - return m_truepos; -} - -bool CarmenWrapper::getReading(RangeReading& reading){ - bool present=false; - sem_wait(&m_dequeSem); - pthread_mutex_lock(&m_mutex); - if (!m_rangeDeque.empty()){ -// cerr << __PRETTY_FUNCTION__ << ": queue size=" <num_readings, res, OrientedPoint(0,0,0), 0, 89.9); - m_sensorMap.insert(make_pair(string("FLASER"), m_rangeSensor)); - - cout << __PRETTY_FUNCTION__ - << ": FrontLaser configured." - << " Readings " << m_rangeSensor->beams().size() - << " Resolution " << res << endl; - } - - RangeReading reading(m_rangeSensor, frontlaser->timestamp); - reading.resize(m_rangeSensor->beams().size()); - for (unsigned int i=0; i< (unsigned int)frontlaser->num_readings; i++){ - reading[i]=(double)frontlaser->range[i]; - } - reading.setPose(OrientedPoint(frontlaser->x, frontlaser->y, frontlaser->theta)); -*/ - RangeReading reading=carmen2reading(*frontlaser); - addReading(reading); -} - -void CarmenWrapper::robot_rearlaser_handler(carmen_robot_laser_message* rearlaser) { -/* if (! m_rangeSensor){ - double res=0; - if (frontlaser->num_readings==180 || frontlaser->num_readings==181) - res=M_PI/180; - if (frontlaser->num_readings==360 || frontlaser->num_readings==361) - res=M_PI/360; - assert(res>0); - m_rangeSensor=new RangeSensor("FLASER",frontlaser->num_readings, res, OrientedPoint(0,0,0), 0, 89.9); - m_sensorMap.insert(make_pair(string("FLASER"), m_rangeSensor)); - - cout << __PRETTY_FUNCTION__ - << ": FrontLaser configured." - << " Readings " << m_rangeSensor->beams().size() - << " Resolution " << res << endl; - } - - RangeReading reading(m_rangeSensor, frontlaser->timestamp); - reading.resize(m_rangeSensor->beams().size()); - for (unsigned int i=0; i< (unsigned int)frontlaser->num_readings; i++){ - reading[i]=(double)frontlaser->range[i]; - } - reading.setPose(OrientedPoint(frontlaser->x, frontlaser->y, frontlaser->theta)); -*/ - RangeReading reading=carmen2reading(*rearlaser); - addReading(reading); -} - - - - -void CarmenWrapper:: navigator_go_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) { - carmen_navigator_go_message msg; - FORMATTER_PTR formatter; - IPC_RETURN_TYPE err; - - formatter = IPC_msgInstanceFormatter(msgRef); - err = IPC_unmarshallData(formatter, callData, &msg, - sizeof(carmen_navigator_go_message)); - IPC_freeByteArray(callData); - - carmen_test_ipc_return - (err, "Could not unmarshall", IPC_msgInstanceName(msgRef)); - cerr<<"go"<truepose.x; - m_truepos.y=truepos->truepose.y; - m_truepos.theta=truepos->truepose.theta; -} - -RangeReading CarmenWrapper::carmen2reading(const carmen_robot_laser_message& msg){ - //either front laser or rear laser - double dth=msg.laser_pose.theta-msg.robot_pose.theta; - dth=atan2(sin(dth), cos(dth)); - - if (msg.laser_pose.theta==msg.robot_pose.theta && !m_frontLaser){ - double res=0; - res = msg.config.angular_resolution; -// if (msg.num_readings==180 || msg.num_readings==181) -// res=M_PI/180; -// if (msg.num_readings==360 || msg.num_readings==361) -// res=M_PI/360; - assert(res>0); - string sensorName="FLASER"; - OrientedPoint rpose(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta); - OrientedPoint lpose(msg.laser_pose.x, msg.laser_pose.y, msg.laser_pose.theta); - OrientedPoint dp=absoluteDifference(lpose, rpose); - m_frontLaser=new RangeSensor(sensorName,msg.num_readings, res, OrientedPoint(0,0,msg.laser_pose.theta-msg.robot_pose.theta), 0, - msg.config.maximum_range); - m_frontLaser->updateBeamsLookup(); - m_sensorMap.insert(make_pair(sensorName, m_frontLaser)); - - cout << __PRETTY_FUNCTION__ - << ": " << sensorName <<" configured." - << " Readings " << m_frontLaser->beams().size() - << " Resolution " << res << endl; - } - if (msg.laser_pose.theta!=msg.robot_pose.theta && !m_rearLaser){ - double res=0; - res = msg.config.angular_resolution; -// if (msg.num_readings==180 || msg.num_readings==181) -// res=M_PI/180; -// if (msg.num_readings==360 || msg.num_readings==361) -// res=M_PI/360; - assert(res>0); - OrientedPoint rpose(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta); - OrientedPoint lpose(msg.laser_pose.x, msg.laser_pose.y, msg.laser_pose.theta); - OrientedPoint dp=absoluteDifference(lpose, rpose); - string sensorName="RLASER"; - m_rearLaser=new RangeSensor(sensorName,msg.num_readings, res, OrientedPoint(0,0,msg.laser_pose.theta-msg.robot_pose.theta), 0, - msg.config.maximum_range); - m_rearLaser->updateBeamsLookup(); - m_sensorMap.insert(make_pair(sensorName, m_rearLaser)); - - cout << __PRETTY_FUNCTION__ - << ": " << sensorName <<" configured." - << " Readings " << m_rearLaser->beams().size() - << " Resolution " << res << endl; - } - - const RangeSensor * rs=(msg.laser_pose.theta==msg.robot_pose.theta)?m_frontLaser:m_rearLaser; - RangeReading reading(rs, msg.timestamp); - reading.resize(rs->beams().size()); - for (unsigned int i=0; i< (unsigned int)msg.num_readings; i++){ - reading[i]=(double)msg.range[i]; - } - reading.setPose(OrientedPoint(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta)); - return reading; -} - -void CarmenWrapper::publish_globalpos(carmen_localize_summary_p summary) -{ - lock(); - static carmen_localize_globalpos_message globalpos; - IPC_RETURN_TYPE err; - - globalpos.timestamp = carmen_get_time(); - globalpos.host = carmen_get_host(); - globalpos.globalpos = summary->mean; - globalpos.globalpos_std = summary->std; - globalpos.globalpos_xy_cov = summary->xy_cov; - globalpos.odometrypos = summary->odometry_pos; - globalpos.converged = summary->converged; - err = IPC_publishData(CARMEN_LOCALIZE_GLOBALPOS_NAME, &globalpos); - carmen_test_ipc_exit(err, "Could not publish", - CARMEN_LOCALIZE_GLOBALPOS_NAME); - unlock(); -} - -/* publish a particle message */ - -void CarmenWrapper::publish_particles(carmen_localize_particle_filter_p filter, - carmen_localize_summary_p summary) -{ - lock(); - static carmen_localize_particle_message pmsg; - IPC_RETURN_TYPE err; - - pmsg.timestamp = carmen_get_time(); - pmsg.host = carmen_get_host(); - pmsg.globalpos = summary->mean; - pmsg.globalpos_std = summary->mean; - pmsg.num_particles = filter->param->num_particles; - pmsg.particles = (carmen_localize_particle_ipc_p)filter->particles; - err = IPC_publishData(CARMEN_LOCALIZE_PARTICLE_NAME, &pmsg); - carmen_test_ipc_exit(err, "Could not publish", - CARMEN_LOCALIZE_PARTICLE_NAME); - fprintf(stderr, "P"); - unlock(); -} - - - - - -void * CarmenWrapper::m_reading_function(void*){ - while (true) { - lock(); - IPC_listen(100); - unlock(); - usleep(20000); - } - return 0; -} - -void CarmenWrapper::shutdown_module(int sig){ - if(sig == SIGINT) { - carmen_ipc_disconnect(); - - fprintf(stderr, "\nDisconnecting (shutdown_module(%d) called).\n",sig); - exit(0); - } -} -/* -typedef struct { - int num_readings; - float *range; - char *tooclose; - double x, y, theta;//position of the laser on the robot - double odom_x, odom_y, odom_theta; //position of the center of the robot - double tv, rv; - double forward_safety_dist, side_safety_dist; - double turn_axis; - double timestamp; - char host[10]; -} carmen_robot_laser_message; -*/ - -carmen_robot_laser_message CarmenWrapper::reading2carmen(const RangeReading& reading){ - carmen_robot_laser_message frontlaser; - frontlaser.num_readings=reading.size(); - frontlaser.range = new float[frontlaser.num_readings]; - frontlaser.tooclose=0; - frontlaser.laser_pose.x=frontlaser.robot_pose.x=reading.getPose().x; - frontlaser.laser_pose.y=frontlaser.robot_pose.y=reading.getPose().y; - frontlaser.laser_pose.theta=frontlaser.robot_pose.theta=reading.getPose().theta; - frontlaser.tv=frontlaser.rv=0; - frontlaser.forward_safety_dist=frontlaser.side_safety_dist=0; - frontlaser.turn_axis=0; - frontlaser.timestamp=reading.getTime(); - for (unsigned int i=0; i< reading.size(); i++){ - frontlaser.range[i]=(float)reading[i]; - } - return frontlaser; -} - -carmen_point_t CarmenWrapper::point2carmen (const OrientedPoint& p){ - return (carmen_point_t){p.x,p.y,p.theta}; -} - -OrientedPoint CarmenWrapper::carmen2point (const carmen_point_t& p){ - return OrientedPoint(p.x, p.y, p.theta); -} - - -/* -int main (int argc, char** argv) { - CarmenWrapper::start(argc, argv); - while(1){ - sleep(2); - RangeReading reading(0,0); - while(CarmenWrapper::getReading(reading)){ - cout << "FLASER " << reading.size(); - for (int i=0; i -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace GMapping{ - -class CarmenWrapper { -public: - static void initializeIPC(const char* name); - static bool start(const char* name); - static bool isRunning(); - static void lock(); - static void unlock(); - static int registerLocalizationMessages(); - - static int queueLength(); - static OrientedPoint getTruePos(); - static bool getReading(RangeReading& reading); - static void addReading(RangeReading& reading); - static const SensorMap& sensorMap(); - static bool sensorMapComputed(); - static bool isStopped(); - -// conversion function - static carmen_robot_laser_message reading2carmen(const RangeReading& reading); - static RangeReading carmen2reading(const carmen_robot_laser_message& msg); - static carmen_point_t point2carmen (const OrientedPoint& p); - static OrientedPoint carmen2point (const carmen_point_t& p); - - -// carmen interaction - static void robot_frontlaser_handler(carmen_robot_laser_message* frontlaser); - static void robot_rearlaser_handler(carmen_robot_laser_message* frontlaser); - static void simulator_truepos_handler(carmen_simulator_truepos_message* truepos); - //babsi: - static void navigator_go_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) ; - static void navigator_stop_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) ; - - //babsi: - static void publish_globalpos(carmen_localize_summary_p summary); - static void publish_particles(carmen_localize_particle_filter_p filter, - carmen_localize_summary_p summary); - - static void shutdown_module(int sig); - - private: - static std::deque m_rangeDeque; - static sem_t m_dequeSem; - static pthread_mutex_t m_mutex, m_lock; - static pthread_t m_readingThread; - static void * m_reading_function(void*); - static bool m_threadRunning; - static SensorMap m_sensorMap; - static RangeSensor* m_frontLaser, *m_rearLaser; - static OrientedPoint m_truepos; - static bool stopped; -}; - -} //end namespace - - - -#endif -/* -int main (int argc, char** argv) { - - CarmenWrapper::init_carmen(argc, argv); - while (true) { - IPC_listenWait(100); - } - return 1; -} -*/ diff --git a/slam_gmapping/openslam_gmapping/gfs-carmen/Makefile b/slam_gmapping/openslam_gmapping/gfs-carmen/Makefile deleted file mode 100644 index 209b772..0000000 --- a/slam_gmapping/openslam_gmapping/gfs-carmen/Makefile +++ /dev/null @@ -1,21 +0,0 @@ -OBJS= -APPS= gfs-carmen - -LIBS+= -lcarmenwrapper -lgridfastslam -lconfigfile -CPPFLAGS+= -I ../sensor -I$(CARMEN_HOME)/include - --include ../global.mk -ifeq ($(CARMENSUPPORT), 0) -APPS= -.PHONY: clean all - -all: - -clean: - -else - -include ../build_tools/Makefile.app -endif - - - diff --git a/slam_gmapping/openslam_gmapping/gfs-carmen/gfs-carmen.cpp b/slam_gmapping/openslam_gmapping/gfs-carmen/gfs-carmen.cpp deleted file mode 100644 index 14deee4..0000000 --- a/slam_gmapping/openslam_gmapping/gfs-carmen/gfs-carmen.cpp +++ /dev/null @@ -1,243 +0,0 @@ -/***************************************************************** - * - * This file is part of the GMAPPING project - * - * GMAPPING Copyright (c) 2004 Giorgio Grisetti, - * Cyrill Stachniss, and Wolfram Burgard - * - * This software is licensed under the "Creative Commons - * License (Attribution-NonCommercial-ShareAlike 2.0)" - * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, - * and Wolfram Burgard. - * - * Further information on this license can be found at: - * http://creativecommons.org/licenses/by-nc-sa/2.0/ - * - * GMAPPING is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR - * PURPOSE. - * - *****************************************************************/ - - -#include -#include -#include -#include -#include - -#define DEBUG cout << __PRETTY_FUNCTION__ - -/* -Example file for interfacing carmen, and gfs. - -if you want to look for a specific topic search for one of the following keywords in the file comments - -KEYWORDS: - CREATION - INITIALIZATION - SENSOR MAP - BEST PARTICLE INDEX - PARTICLE VECTOR - PARTICLE TRAJECTORIES - BEST MAP - BOUNDING BOX -*/ - -using namespace GMapping; -using namespace std; - -int main(int argc, const char * const * argv){ - - std::string outfilename=""; - double xmin=-100.; - double ymin=-100.; - double xmax=100.; - double ymax=100.; - double delta=0.05; - - //scan matching parameters - double sigma=0.05; - double maxrange=80.; - double maxUrange=80.; - double regscore=1e4; - double lstep=.05; - double astep=.05; - int kernelSize=1; - int iterations=5; - double critscore=0.; - double maxMove=1.; - double lsigma=.075; - double ogain=3; - int lskip=0; - - //motion model parameters - double srr=0.01, srt=0.01, str=0.01, stt=0.01; - //particle parameters - int particles=30; - - - //gfs parameters - double angularUpdate=0.5; - double linearUpdate=1; - double resampleThreshold=0.5; - bool generateMap=true; - - std::string configfilename = ""; - - CMD_PARSE_BEGIN_SILENT(1,argc); - parseStringSilent("-cfg",configfilename); - CMD_PARSE_END_SILENT; - - if (configfilename.length()>0){ - ConfigFile cfg(configfilename); - outfilename = (std::string) cfg.value("gfs","outfilename",outfilename); - xmin = cfg.value("gfs","xmin", xmin); - xmax = cfg.value("gfs","xmax",xmax); - ymin = cfg.value("gfs","ymin",ymin); - ymax = cfg.value("gfs","ymax",ymax); - delta = cfg.value("gfs","delta",delta); - maxrange = cfg.value("gfs","maxrange",maxrange); - maxUrange = cfg.value("gfs","maxUrange",maxUrange); - regscore = cfg.value("gfs","regscore",regscore); - critscore = cfg.value("gfs","critscore",critscore); - kernelSize = cfg.value("gfs","kernelSize",kernelSize); - sigma = cfg.value("gfs","sigma",sigma); - iterations = cfg.value("gfs","iterations",iterations); - lstep = cfg.value("gfs","lstep",lstep); - astep = cfg.value("gfs","astep",astep); - maxMove = cfg.value("gfs","maxMove",maxMove); - srr = cfg.value("gfs","srr", srr); - srt = cfg.value("gfs","srt", srt); - str = cfg.value("gfs","str", str); - stt = cfg.value("gfs","stt", stt); - particles = cfg.value("gfs","particles",particles); - angularUpdate = cfg.value("gfs","angularUpdate", angularUpdate); - linearUpdate = cfg.value("gfs","linearUpdate", linearUpdate); - lsigma = cfg.value("gfs","lsigma", lsigma); - ogain = cfg.value("gfs","lobsGain", ogain); - lskip = (int)cfg.value("gfs","lskip", lskip); - // randseed = cfg.value("gfs","randseed", randseed); - resampleThreshold = cfg.value("gfs","resampleThreshold", resampleThreshold); - generateMap = cfg.value("gfs","generateMap", generateMap); - } - - - CMD_PARSE_BEGIN(1,argc); - parseString("-cfg",configfilename); - parseString("-outfilename",outfilename); - parseDouble("-xmin",xmin); - parseDouble("-xmax",xmax); - parseDouble("-ymin",ymin); - parseDouble("-ymax",ymax); - parseDouble("-delta",delta); - parseDouble("-maxrange",maxrange); - parseDouble("-maxUrange",maxUrange); - parseDouble("-regscore",regscore); - parseDouble("-critscore",critscore); - parseInt("-kernelSize",kernelSize); - parseDouble("-sigma",sigma); - parseInt("-iterations",iterations); - parseDouble("-lstep",lstep); - parseDouble("-astep",astep); - parseDouble("-maxMove",maxMove); - parseDouble("-srr", srr); - parseDouble("-srt", srt); - parseDouble("-str", str); - parseDouble("-stt", stt); - parseInt("-particles",particles); - parseDouble("-angularUpdate", angularUpdate); - parseDouble("-linearUpdate", linearUpdate); - parseDouble("-lsigma", lsigma); - parseDouble("-lobsGain", ogain); - parseInt("-lskip", lskip); - parseDouble("-resampleThreshold", resampleThreshold); - parseFlag("-generateMap", generateMap); - CMD_PARSE_END; - - cerr << "Parameter parsed, connecting to Carmen!"; - - CarmenWrapper::initializeIPC(argv[0]); - CarmenWrapper::start(argv[0]); - - while (! CarmenWrapper::sensorMapComputed()){ - usleep(500000); - cerr << "." << flush; - } - - //CREATION - - GridSlamProcessor* processor=new GridSlamProcessor; - - //SENSOR MAP - //loads from the carmen wrapper the laser and robot settings - SensorMap sensorMap=CarmenWrapper::sensorMap(); - cerr << "Connected " << endl; - processor->setSensorMap(sensorMap); - - //set the command line parameters - processor->setMatchingParameters(maxUrange, maxrange, sigma, kernelSize, lstep, astep, iterations, lsigma, ogain, lskip); - processor->setMotionModelParameters(srr, srt, str, stt); - processor->setUpdateDistances(linearUpdate, angularUpdate, resampleThreshold); - processor->setgenerateMap(generateMap); - OrientedPoint initialPose(xmin+xmax/2, ymin+ymax/2, 0); - - - //INITIALIZATION - processor->init(particles, xmin, ymin, xmax, ymax, delta, initialPose); - if (outfilename.length()>0) - processor->outputStream().open(outfilename.c_str()); - - bool running=true; - - GridSlamProcessor* ap, *copy=processor->clone(); - ap=processor; processor=copy; copy=ap; - - //this is the CORE LOOP; - RangeReading rr(0,0); - while (running){ - while (CarmenWrapper::getReading(rr)){ - - - bool processed=processor->processScan(rr); - - //this returns true when the algorithm effectively processes (the traveled path since the last processing is over a given threshold) - if (processed){ - cerr << "PROCESSED" << endl; - //for searching for the BEST PARTICLE INDEX - // unsigned int best_idx=processor->getBestParticleIndex(); - - //if you want to access to the PARTICLE VECTOR - const GridSlamProcessor::ParticleVector& particles = processor->getParticles(); - //remember to use a const reference, otherwise it copys the whole particles and maps - - //this is for recovering the tree of PARTICLE TRAJECTORIES (obtaining the ancestor of each particle) - cerr << "Particle reproduction story begin" << endl; - for (unsigned int i=0; i" << i << " "; - } - cerr << "Particle reproduction story end" << endl; -/* - //then if you want to access the BEST MAP, - //of course by copying it in a plain structure - Map* mymap = processor->getParticles()[best_idx].map.toDoubleMap(); - //at this point mymap is yours. Can do what you want. - - double best_weight=particles[best_idx].weightSum; - cerr << "Best Particle is " << best_idx << " with weight " << best_weight << endl; - -*/ - cerr << __PRETTY_FUNCTION__ << "CLONING... " << endl; - GridSlamProcessor* newProcessor=processor->clone(); - cerr << "DONE" << endl; - cerr << __PRETTY_FUNCTION__ << "DELETING... " << endl; - delete processor; - cerr << "DONE" << endl; - processor=newProcessor; - } - } - } - return 0; -} - diff --git a/slam_gmapping/openslam_gmapping/grid/Makefile b/slam_gmapping/openslam_gmapping/grid/Makefile deleted file mode 100644 index 22de055..0000000 --- a/slam_gmapping/openslam_gmapping/grid/Makefile +++ /dev/null @@ -1,9 +0,0 @@ -OBJS= -APPS= map_test - -LDFLAGS+= -CPPFLAGS+= -DNDEBUG - --include ../global.mk --include ../build_tools/Makefile.app - diff --git a/slam_gmapping/openslam_gmapping/grid/graphmap.cpp b/slam_gmapping/openslam_gmapping/grid/graphmap.cpp deleted file mode 100644 index a262d4c..0000000 --- a/slam_gmapping/openslam_gmapping/grid/graphmap.cpp +++ /dev/null @@ -1,59 +0,0 @@ -#ifndef GRAPHMAP_H -#define GRAPHMAP_H -#include -#include -#include -#include - -namespace GMapping { - -class RasterMap; - -struct GraphMapPatch{ - typedef typename std::list PointList; - /**Renders the map relatively to the center of the patch*/ - //void render(RenderMap rmap); - /**returns the lower left corner of the patch, relative to the center*/ - //Point minBoundary() const; - /**returns the upper right corner of the patch, relative to the center*/ - //Point maxBoundary() const; // - - OrientedPoint center; - PointList m_points; -}; - -struct Covariance3{ - double sxx, sxy, sxt, syy, syt ,stt; -}; - -struct GraphMapEdge{ - Covariance3 covariance; - GraphMapPatch* first, *second; - inline operator double() const{ - return sqrt((first->center-second->center)*(first->center-second->center)); - } -}; - - -struct GraphPatchGraph: public Graph{ - void addEdge(Vertex* v1, Vertex* v2, const Covariance3& covariance); -}; - -void GraphPatchGraph::addEdge(GraphPatchGraph::Vertex* v1, GraphPatchGraph::VertexVertex* v2, - const Covariance3& cov){ - GraphMapEdge gme; - gme.covariance=cov; - gme.first=v1; - gme.second=v2; - return Graph::addEdge(v1,v2,gme); -} - -struct GraphPatchDirectoryCell: public std::set { - GraphPatchDirectoryCell(double); -}; - -typedef Map, Array2D::set > - -}; - -#endif \ No newline at end of file diff --git a/slam_gmapping/openslam_gmapping/grid/map_test.cpp b/slam_gmapping/openslam_gmapping/grid/map_test.cpp deleted file mode 100644 index 41dbf5a..0000000 --- a/slam_gmapping/openslam_gmapping/grid/map_test.cpp +++ /dev/null @@ -1,62 +0,0 @@ -#include -#include "map.h" -#include "harray2d.h" - -using namespace std; -using namespace GMapping; - -struct SimpleCell{ - int value; - SimpleCell(int v=0){value=v;} - static const SimpleCell& Unknown(); - static SimpleCell* address; -}; - -SimpleCell* SimpleCell::address=0; - -const SimpleCell& SimpleCell::Unknown(){ - if (address) - return *address; - address=new SimpleCell(-1); - return *address; -} - -typedef Map< SimpleCell, HierarchicalArray2D > CGrid; - -int main (int argc, char ** argv){ - CGrid g1(Point(0.,0.), 200, 200, 0.1); - CGrid g2(Point(10.,10.), 200, 200, 0.1); - { - HierarchicalArray2D::PointSet ps; - IntPoint pp=g1.world2map(Point(5.1,5.1)); - cout << pp.x << " " << pp.y << endl; - ps.insert(pp); - g1.storage().setActiveArea(ps,false); - g1.storage().allocActiveArea(); - g1.cell(Point(5.1,5.1)).value=5; - cout << "cell value" << (int) g1.cell(Point(5.1,5.1)).value << endl; - g1.resize(-150, -150, 150, 150); - cout << "cell value" << (int) g1.cell(Point(5.1,5.1)).value << endl; - CGrid g3(g1); - g1=g2; - } - cerr << "copy and modify test" << endl; - CGrid *ap,* gp1=new CGrid(Point(0,0), 200, 200, 0.1); - CGrid* gp0=new CGrid(*gp1); - for (int i=1; i<10; i++){ - ap=new CGrid(*gp1); - delete gp1; - gp1=gp0; - gp0=ap; - IntPoint pp=gp0->world2map(Point(5.1,5.1)); - HierarchicalArray2D::PointSet ps; - ps.insert(pp); - gp1->storage().setActiveArea(ps,false); - gp1->storage().allocActiveArea(); - gp1->cell(Point(5.1,5.1)).value=i; - cout << "cell value" << (int) gp1->cell(Point(5.1,5.1)).value << endl; - } - delete gp0; - delete gp1; - return 0; -} diff --git a/slam_gmapping/openslam_gmapping/gridfastslam/CMakeLists.txt b/slam_gmapping/openslam_gmapping/gridfastslam/CMakeLists.txt deleted file mode 100644 index 8df8800..0000000 --- a/slam_gmapping/openslam_gmapping/gridfastslam/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -add_library(gridfastslam - gfsreader.cpp - gridslamprocessor.cpp - gridslamprocessor_tree.cpp - motionmodel.cpp - ) -target_link_libraries(gridfastslam scanmatcher sensor_range) - -install(TARGETS gridfastslam DESTINATION lib) - -#ament_export_libraries(gridfastslam) diff --git a/slam_gmapping/openslam_gmapping/gridfastslam/gfs2log.cpp b/slam_gmapping/openslam_gmapping/gridfastslam/gfs2log.cpp deleted file mode 100644 index 1324818..0000000 --- a/slam_gmapping/openslam_gmapping/gridfastslam/gfs2log.cpp +++ /dev/null @@ -1,64 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include "gfsreader.h" - -#define MAX_LINE_LENGHT (1000000) - -using namespace std; -using namespace GMapping; -using namespace GMapping::GFSReader; - -int main (int argc, const char * const * argv){ - if (argc<3){ - cout << "usage gfs2log [-err] [-neff] [-part] [-odom] " << endl; - cout << " -odom : dump raw odometry in ODOM message instead of inpolated corrected one" << endl; - return -1; - } - bool err=0; - bool neff=0; - bool part=0; - bool odom=0; - // int particle_num; - unsigned int c=1; - if (!strcmp(argv[c],"-err")){ - err=true; - c++; - } - if (!strcmp(argv[c],"-neff")){ - neff=true; - c++; - } - if (!strcmp(argv[c],"-part")){ - part=true; - c++; - } - if (!strcmp(argv[c],"-odom")){ - odom=true; - c++; - } - ifstream is(argv[c]); - if (!is){ - cout << "could read file "<< endl; - return -1; - } - c++; - RecordList rl; - rl.read(is); - unsigned int bestidx=rl.getBestIdx(); - cout << endl << "best index = " << bestidx<< endl; - ofstream os(argv[c]); - if (! os){ - cout << "could write file "<< endl; - return -1; - } - rl.printPath(os,bestidx,err,odom); - if(part) - rl.printLastParticles(os); - os.close(); - return 0; -} diff --git a/slam_gmapping/openslam_gmapping/gridfastslam/gfs2neff.cpp b/slam_gmapping/openslam_gmapping/gridfastslam/gfs2neff.cpp deleted file mode 100644 index 5faad99..0000000 --- a/slam_gmapping/openslam_gmapping/gridfastslam/gfs2neff.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#include -#include -#include -#include - -using namespace std; - -int main(int argc, char**argv){ - if (argc<3){ - cout << "usage gfs2neff " << endl; - return -1; - } - ifstream is(argv[1]); - if (!is){ - cout << "could read file "<< endl; - return -1; - } - ofstream os(argv[2]); - if (! os){ - cout << "could write file "<< endl; - return -1; - } - unsigned int frame=0; - double neff=0; - while(is){ - char buf[8192]; - is.getline(buf, 8192); - istringstream lineStream(buf); - string recordType; - lineStream >> recordType; - if (recordType=="FRAME"){ - lineStream>> frame; - } - if (recordType=="NEFF"){ - lineStream>> neff; - os << frame << " " << neff << endl; - } - } - os.close(); -} diff --git a/slam_gmapping/openslam_gmapping/gridfastslam/gfs2rec.cpp b/slam_gmapping/openslam_gmapping/gridfastslam/gfs2rec.cpp deleted file mode 100644 index 5eb65fa..0000000 --- a/slam_gmapping/openslam_gmapping/gridfastslam/gfs2rec.cpp +++ /dev/null @@ -1,406 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#define MAX_LINE_LENGHT (1000000) - -using namespace GMapping; -using namespace std; - -struct Record{ - unsigned int dim; - double time; - virtual ~Record(){} - virtual void read(istream& is)=0; - virtual void write(ostream& os){}; -}; - -struct CommentRecord: public Record{ - string text; - virtual void read(istream& is){ - char buf[MAX_LINE_LENGHT]; - memset(buf,0, MAX_LINE_LENGHT*sizeof(char)); - is.getline(buf, MAX_LINE_LENGHT); - text=string(buf); - } - virtual void write(ostream& os){ - os << "#GFS_COMMENT: " << text << endl; - } -}; - -struct PoseRecord: public Record{ - PoseRecord(bool ideal=false){ - truePos=ideal; - } - bool truePos; - OrientedPoint pose; - void read(istream& is){ - is >> pose.x >> pose.y >> pose.theta; - time = 0; - if (is) - is >> time; - } - virtual void write(ostream& os){ - if (truePos) - os << "POS-CORR"; - else - os << "POS "; - // FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": "; - os << "0 0: "; - os << pose.x*100 << " " << pose.y*100 << " " << 180/M_PI*pose.theta << endl; - } -}; - -struct NeffRecord: public Record{ - double neff; - void read(istream& is){ - is >> neff; - } - virtual void write(ostream& os){ - os << "NEFF " << neff << endl; - } -}; - - -struct OdometryRecord: public Record{ - vector poses; - virtual void read(istream& is){ - is >> dim; - for (unsigned int i=0; i< dim; i++){ - OrientedPoint p; - double w; - is >> p.x; - is >> p.y; - is >> p.theta; - is >> w; - poses.push_back(p); - } - time = 0; - if (is) - is >> time; - } -}; - - -struct ScanMatchRecord: public Record{ - vector poses; - vector weights; - virtual void read(istream& is){ - is >> dim; - for (unsigned int i=0; i< dim; i++){ - OrientedPoint p; - double w; - is >> p.x; - is >> p.y; - is >> p.theta; - is >> w; - poses.push_back(p); - weights.push_back(w); - } - } -}; - -struct LaserRecord: public Record{ - vector readings; - OrientedPoint pose; - virtual void read(istream& is){ - is >> dim; - for (unsigned int i=0; i< dim; i++){ - double r; - is >> r; - readings.push_back(r); - } - is >> pose.x; - is >> pose.y; - is >> pose.theta; - time = 0; - if (is) - is >> time; - } - - // dummy, &sec, &usec, &nLas, &nVal, &range ) == EOF) { - - virtual void write(ostream& os){ - os << "POS "; - // FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": "; - os << "0 0: "; - os << pose.x*100 << " " << pose.y*100 << " " << 180/M_PI*pose.theta << endl; - - os << "LASER-RANGE "; - // FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": "; - os << " 0 0 0 " << dim << " 180. : "; - for (unsigned int i=0; i< dim; i++){ - os <<" "<< readings[i]*100 ; - } - os << endl; - }; -}; - -struct ResampleRecord: public Record{ - vector indexes; - virtual void read(istream& is){ - is >> dim; - for (unsigned int i=0; i< dim; i++){ - unsigned int j; - is >> j; - indexes.push_back(j); - } - } -}; - -struct RecordList: public list{ - mutable int sampleSize; - - istream& read(istream& is){ - while(is){ - char buf[8192]; - is.getline(buf, 8192); - istringstream lineStream(buf); - string recordType; - lineStream >> recordType; - Record* rec=0; - if (recordType=="LASER_READING"){ - rec=new LaserRecord; - cout << "l" << flush; - } - if (recordType=="ODO_UPDATE"){ - rec=new OdometryRecord; - cout << "o" << flush; - } - if (recordType=="SM_UPDATE"){ - rec=new ScanMatchRecord; - cout << "m" << flush; - } - if (recordType=="SIMULATOR_POS"){ - rec=new PoseRecord(true); - cout << "t" << flush; - } - if (recordType=="RESAMPLE"){ - rec=new ResampleRecord; - cout << "r" << flush; - } - if (recordType=="NEFF"){ - rec=new NeffRecord; - cout << "n" << flush; - } - if (recordType=="COMMENT"){ - rec=new CommentRecord; - cout << "c" << flush; - } - if (rec){ - rec->read(lineStream); - push_back(rec); - } - } - return is; - } - - double getLogWeight(unsigned int i) const{ - double weight=0; - unsigned int currentIndex=i; - for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){ - ScanMatchRecord* scanmatch=dynamic_cast(*it); - if (scanmatch){ - weight+=scanmatch->weights[currentIndex]; - } - ResampleRecord* resample=dynamic_cast(*it); - if (resample){ - currentIndex=resample->indexes[currentIndex]; - } - } - return weight; - } - unsigned int getBestIdx() const { - if (empty()) - return 0; - const ScanMatchRecord* scanmatch=0; - const_reverse_iterator it=rbegin(); - while(!scanmatch){ - scanmatch=dynamic_cast(*it); - it++; - } - unsigned int dim=scanmatch->dim; - sampleSize=(int)dim; - double bestw=-1e200; - unsigned int best=scanmatch->dim+1; - for (unsigned i=0; ibestw){ - best=i; - bestw=w; - } - } - return best; - } - - void printPath(ostream& os, unsigned int i, bool err=false) const{ - unsigned int currentIndex=i; - OrientedPoint p(0,0,0); - - RecordList rl; - - //reconstruct a path - for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){ - const NeffRecord* neff=dynamic_cast(*it); - if (neff){ - NeffRecord* n=new NeffRecord(*neff); - rl.push_front(n); - } - const ScanMatchRecord* scanmatch=dynamic_cast(*it); - if (scanmatch){ - PoseRecord* pose=new PoseRecord; - pose->dim=0; - p=pose->pose=scanmatch->poses[currentIndex]; - rl.push_front(pose); - } - const OdometryRecord* odometry=dynamic_cast(*it); - if (odometry){ - PoseRecord* pose=new PoseRecord; - pose->dim=0; - p=pose->pose=odometry->poses[currentIndex]; - pose->time=odometry->time; - rl.push_front(pose); - } - const PoseRecord* tpose=dynamic_cast(*it); - if (tpose){ - PoseRecord* pose=new PoseRecord(*tpose); - rl.push_front(pose); - } - const LaserRecord* laser=dynamic_cast(*it); - if (laser){ - LaserRecord* claser=new LaserRecord(*laser); - claser->pose=p; - rl.push_front(claser); - } - const CommentRecord* comment=dynamic_cast(*it); - if (comment){ - CommentRecord* ccomment=new CommentRecord(*comment); - rl.push_front(ccomment); - } - const ResampleRecord* resample=dynamic_cast(*it); - if (resample){ - currentIndex=resample->indexes[currentIndex]; - ResampleRecord* r= new ResampleRecord(*resample); - rl.push_front(r); - } - } - bool started=false; - double ox=0, oy=0, rxx=0, rxy=0, ryx=0, ryy=0, rth=0; - bool computedTransformation=false; - bool truePosFound=false; - OrientedPoint truePose(0,0,0); - OrientedPoint currPose(0,0,0); - bool tpf=false; - double neff=0; - unsigned int count=0; - for(RecordList::iterator it=rl.begin(); it!=rl.end(); it++){ - NeffRecord* neffr=dynamic_cast(*it); - if (neffr) - neff=neffr->neff/(double)sampleSize; - started=started || dynamic_cast(*it)?true:false; - if (started && ! truePosFound){ - PoseRecord* tpose=dynamic_cast(*it); - if (tpose && tpose->truePos){ - truePosFound=true; - tpf=true; - truePose=tpose->pose; - os << "# "; - (*it)->write(os); - } - } - if (started && truePosFound && ! computedTransformation){ - PoseRecord* pos=dynamic_cast(*it); - if (pos && !pos->truePos){ - OrientedPoint pose=pos->pose; - rth=truePose.theta-pose.theta; - double s=sin(rth), c=cos(rth); - rxx=ryy=c; - rxy=-s; ryx=s; - ox=truePose.x-(rxx*pose.x+rxy*pose.y); - oy=truePose.y-(ryx*pose.x+ryy*pose.y); - computedTransformation=true; - os << "# "; - (*it)->write(os); - - } - } - ResampleRecord* resample=dynamic_cast(*it); - if(resample){ - os << "MARK-POS 0 0: " <(*it); - if (pos){ - if (pos->truePos){ - tpf=true; - truePose=pos->pose; - } else { - if (tpf){ - tpf=false; - OrientedPoint pose=pos->pose; - double ex, ey, eth=truePose.theta-pose.theta-rth; - ex=truePose.x-(ox+rxx*pose.x+rxy*pose.y); - ey=truePose.y-(oy+ryx*pose.x+ryy*pose.y); - eth=atan2(sin(eth), cos(eth)); - if (! err) - os << "# ERROR "; - os << neff << " " - << ex << " " << ey << " " << eth - << " " << sqrt(ex*ex+ey*ey) << " " << fabs(eth) << endl; - } - } - } - - } - PoseRecord* pos=dynamic_cast(*it); - if (pos) - currPose=pos->pose; - - if (! err) - (*it)->write(os); - delete *it; - } - } -}; - - - -int main (int argc, const char * const * argv){ - if (argc<3){ - cout << "usage gfs2rec [-err] " << endl; - return -1; - } - bool err=0; - bool neff=0; - unsigned int c=1; - if (!strcmp(argv[c],"-err")){ - err=true; - c++; - } - if (!strcmp(argv[c],"-neff")){ - neff=true; - c++; - } - ifstream is(argv[c]); - if (!is){ - cout << "could read file "<< endl; - return -1; - } - c++; - RecordList rl; - rl.read(is); - unsigned int bestidx=rl.getBestIdx(); - cout << endl << "best index = " << bestidx<< endl; - ofstream os(argv[c]); - if (! os){ - cout << "could write file "<< endl; - return -1; - } - rl.printPath(os,bestidx,err); - os.close(); - return 0; -} diff --git a/slam_gmapping/openslam_gmapping/gridfastslam/gfs2stat.cpp b/slam_gmapping/openslam_gmapping/gridfastslam/gfs2stat.cpp deleted file mode 100644 index 3346199..0000000 --- a/slam_gmapping/openslam_gmapping/gridfastslam/gfs2stat.cpp +++ /dev/null @@ -1,58 +0,0 @@ -#include -#include -#include -#include -#include "gfsreader.h" - -using namespace std; -using namespace GMapping; -using namespace GMapping::GFSReader; - - -int main(int argc, char ** argv){ - if (argc<2){ - cout << "usage gfs2stat " << endl; - return 0; - } - ifstream is(argv[1]); - if (!is){ - cout << "no file found: " << argv[1] << endl; - return 0; - } - ofstream os(argv[2]); - if (!os){ - cout << "cannot open file: " << argv[1] << endl; - return 0; - } - cout << "loading... "<< flush; - RecordList rl; - rl.read(is); - cout << " done" << endl; - int count=-1; - for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++){ - - count++; - const ScanMatchRecord* rec=dynamic_cast(*it); - if (!rec) - continue; - Gaussian3 gaussian; - /* - vector nweights; - cout << "N"<< flush; - back_insert_iterator< vector > out(nweights); - toNormalForm(out,rec->weights.begin(), rec->weights.end()); - cout << "G"<< flush; - gaussian.computeFromSamples(rec->poses, nweights); - */ - gaussian.computeFromSamples(rec->poses); - cout << "E"<< flush; - os << count <<" "; - os << gaussian.mean.x <<" "; - os << gaussian.mean.y <<" "; - os << gaussian.mean.theta <<" "; - os << gaussian.covariance.eval[0] <<" "; - os << gaussian.covariance.eval[1] <<" "; - os << gaussian.covariance.eval[2] < -#include -#include -#include -#include -#include -#include -#include "gfsreader.h" -#define MAX_LINE_LENGHT (1000000) - -using namespace std; -using namespace GMapping; -using namespace GMapping::GFSReader; - -computeBoundingBox() - - -int main (unsigned int argc, const char * const * argv) - double delta = 0.1; - double skip = 2; - double rotate = 0; - double maxrange = 0; - - if (argc<3){ - cout << "usage gfs2stream [-step Number] " << endl; - return -1; - } - - - CMD_PARSE_BEGIN(1,argc-2); - - CMD_PARSE_END; - - if (argc<3){ - cout << "usage gfs2stream [-step Number] " << endl; - return -1; - } - bool err=0; - bool neff=0; - bool part=0; - unsigned int c=1; - if (!strcmp(argv[c],"-err")){ - err=true; - c++; - } - if (!strcmp(argv[c],"-neff")){ - neff=true; - c++; - } - if (!strcmp(argv[c],"-part")){ - part=true; - c++; - } - ifstream is(argv[c]); - if (!is){ - cout << "could read file "<< endl; - return -1; - } - c++; - RecordList rl; - rl.read(is); - unsigned int bestidx=rl.getBestIdx(); - cout << endl << "best index = " << bestidx<< endl; - ofstream os(argv[c]); - if (! os){ - cout << "could write file "<< endl; - return -1; - } - rl.printPath(os,bestidx,err); - if(part) - rl.printLastParticles(os); - os.close(); - return 0; -} diff --git a/slam_gmapping/openslam_gmapping/gridfastslam/gfsreader.cpp b/slam_gmapping/openslam_gmapping/gridfastslam/gfsreader.cpp deleted file mode 100644 index 8b16427..0000000 --- a/slam_gmapping/openslam_gmapping/gridfastslam/gfsreader.cpp +++ /dev/null @@ -1,506 +0,0 @@ -#include -#include "gfsreader.h" -#include -#include - -namespace GMapping { - -namespace GFSReader{ - -Record::~Record(){} -void Record::write(ostream& os){}; - -void CommentRecord::read(istream& is){ - char buf[MAX_LINE_LENGHT]; - memset(buf,0, MAX_LINE_LENGHT*sizeof(char)); - is.getline(buf, MAX_LINE_LENGHT); - text=string(buf); -} - -void CommentRecord::write(ostream& os){ - os << "#GFS_COMMENT: " << text << endl; -} - -PoseRecord::PoseRecord(bool ideal){ - truePos=ideal; -} -void PoseRecord::read(istream& is){ - is >> pose.x >> pose.y >> pose.theta; - time = 0; - if (is) - is >> time; -} -void PoseRecord::write(ostream& os){ - if (truePos) - os << "TRUEPOS "; - else - os << "ODOM "; - os << setiosflags(ios::fixed) << setprecision(6); - os << pose.x << " " << pose.y << " " << pose.theta << " 0 0 0 "; - os << time << " pippo " << time << endl; -} - -void NeffRecord::read(istream& is){ - is >> neff; - time =0; - if (is) - is >> time; - -} - -void NeffRecord::write(ostream& os){ - os << "NEFF " << neff ; - os << setiosflags(ios::fixed) << setprecision(6); - os << " " << time << " pippo " << time << endl; -} - - -void OdometryRecord::read(istream& is){ - is >> dim; - for (unsigned int i=0; i< dim; i++){ - OrientedPoint p; - double w; - is >> p.x; - is >> p.y; - is >> p.theta; - is >> w; - poses.push_back(p); - } - time = 0; - if (is) - is >> time; -} - -void RawOdometryRecord::read(istream& is){ - is >> pose.x; - is >> pose.y; - is >> pose.theta; - time = 0; - assert(is); - is >> time; - -} - - -void EntropyRecord::read(istream& is){ - is >> poseEntropy >> trajectoryEntropy >> mapEntropy; - time =0; - if (is) - is >> time; -} - -void EntropyRecord::write(ostream& os){ - os << setiosflags(ios::fixed) << setprecision(6) << "ENTROPY " << poseEntropy << " " << trajectoryEntropy << " " << mapEntropy; - os << " " << time << " pippo " << time << endl; -} - -void ScanMatchRecord::read(istream& is){ - is >> dim; - for (unsigned int i=0; i< dim; i++){ - OrientedPoint p; - double w; - is >> p.x; - is >> p.y; - is >> p.theta; - is >> w; - poses.push_back(p); - weights.push_back(w); - } -} - -void LaserRecord::read(istream& is){ - is >> dim; - for (unsigned int i=0; i< dim; i++){ - double r; - is >> r; - readings.push_back(r); - } - is >> pose.x; - is >> pose.y; - is >> pose.theta; - time = 0; - if (is) - is >> time; -} - -void LaserRecord::write(ostream& os){ - os << "WEIGHT " << weight << endl; - os << "ROBOTLASER1 "; - - - if ((dim == 541)||(dim == 540)) { // S300 - os <<" 4"; // laser type - os <<" -2.351831"; // start_angle - os <<" 4.712389"; // fov - os <<" 0.008727"; // angular res - os <<" 30.0" ; // maxrange - } - else if ((dim == 180)||(dim == 181)) { // PLS - os <<" 0"; // laser type - os <<" -1.570796"; // start_angle - os <<" 3.141593"; // fov - os <<" 0.017453"; // angular res - os <<" 81.9" ; // maxrange - } - else if ((dim == 360)||(dim == 361)) { // LMS - os <<" 0"; // laser type - os <<" -1.570796"; // start_angle - os <<" 3.141593"; // fov - os <<" 0.008726"; // angular res - os <<" 81.9" ; // maxrange - } - else if ((dim == 682)||(dim == 683)) { // URG - os <<" 0"; // laser type - os <<" -2.094395"; // start_angle - os <<" 4.1887902"; // fov - os << " " << 360.0/1024.0/180.0*M_PI; // angular res - os <<" 5.5" ; // maxrange - } - else { // PLS - os <<" 0"; // laser type - os <<" -1.570796"; // start_angle - os <<" 3.141593"; // fov - os <<" 0.017453"; // angular res - os <<" 81.9" ; // maxrange - } - os <<" 0.01"; // accuracy - os <<" 0" ; // remission mode - os <<" "<< dim; // num readings - os << setiosflags(ios::fixed) << setprecision(2); - for (unsigned int i=0; i< dim; i++){ - os <<" "<< readings[i] ; - } - os << setiosflags(ios::fixed) << setprecision(6); - os <<" 0"; // num remession values - os <<" "<< pose.x; - os <<" "<< pose.y; - os <<" "<< pose.theta; - os <<" "<< pose.x; - os <<" "<< pose.y; - os <<" "<< pose.theta; - os <<" 0" ; // tv - os <<" 0" ; // rv - os <<" 0.55" ; // forward_safety_dist - os <<" 0.375" ; // sideward_safety_dist - os <<" 1000000.0" ; // turn_axis - os <<" "<< time << " localhost " << time << endl; -}; - -void ResampleRecord::read(istream& is){ - is >> dim; - for (unsigned int i=0; i< dim; i++){ - unsigned int j; - is >> j; - indexes.push_back(j); - } -} - -istream& RecordList::read(istream& is){ - while(is){ - char buf[MAX_LINE_LENGHT]; - is.getline(buf, MAX_LINE_LENGHT); - istringstream lineStream(buf); - string recordType; - lineStream >> recordType; - Record* rec=0; - if (recordType=="LASER_READING"){ - rec=new LaserRecord; -// cout << "l" << flush; - } - else if (recordType=="ODO_UPDATE"){ - rec=new OdometryRecord; -// cout << "o" << flush; - } - else if (recordType=="ODOM"){ - rec=new RawOdometryRecord; -// cout << "O" << flush; - } - else if (recordType=="SM_UPDATE"){ - rec=new ScanMatchRecord; -// cout << "m" << flush; - } - else if (recordType=="SIMULATOR_POS"){ - rec=new PoseRecord(true); -// cout << "t" << flush; - } - else if (recordType=="RESAMPLE"){ - rec=new ResampleRecord; -// cout << "r" << flush; - } - else if (recordType=="NEFF"){ - rec=new NeffRecord; -// cout << "n" << flush; - } - else if (recordType=="COMMENT" || recordType=="#COMMENT"){ - rec=new CommentRecord; -// cout << "c" << flush; - } - else if (recordType=="ENTROPY"){ - rec=new EntropyRecord; -// cout << "c" << flush; - } - - if (rec){ - rec->read(lineStream); - push_back(rec); - } - } - return is; -} - -double RecordList::getLogWeight(unsigned int i) const{ - double weight=0; - unsigned int currentIndex=i; - for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){ - ScanMatchRecord* scanmatch=dynamic_cast(*it); - if (scanmatch){ - weight+=scanmatch->weights[currentIndex]; - } - ResampleRecord* resample=dynamic_cast(*it); - if (resample){ - currentIndex=resample->indexes[currentIndex]; - } - } - return weight; -} - -double RecordList::getLogWeight(unsigned int i, RecordList::const_iterator frame) const{ - double weight=0; - unsigned int currentIndex=i; - for(RecordList::const_reverse_iterator it(frame); it!=rend(); it++){ - ScanMatchRecord* scanmatch=dynamic_cast(*it); - if (scanmatch){ - weight+=scanmatch->weights[currentIndex]; - } - ResampleRecord* resample=dynamic_cast(*it); - if (resample){ - currentIndex=resample->indexes[currentIndex]; - } - } - return weight; -} - -unsigned int RecordList::getBestIdx() const { - if (empty()) - return 0; - const ScanMatchRecord* scanmatch=0; - const_reverse_iterator it=rbegin(); - while(!scanmatch){ - scanmatch=dynamic_cast(*it); - it++; - } - unsigned int dim=scanmatch->dim; - sampleSize=(int)dim; - double bestw=-std::numeric_limits::max(); - unsigned int best=scanmatch->dim+1; - for (unsigned i=0; ibestw){ - best=i; - bestw=w; - } - } - return best; -} - -void RecordList::printLastParticles(ostream& os) const { - if (empty()) - return; - const ScanMatchRecord* scanmatch=0; - const_reverse_iterator it=rbegin(); - while(!scanmatch){ - scanmatch=dynamic_cast(*it); - it++; - } - if (! scanmatch) - return; - for (vector::const_iterator it=scanmatch->poses.begin(); it!=scanmatch->poses.end(); it++){ - os << "MARKER [color=black; circle=" << it->x*100 << "," << it->y*100 << ",10] 0 pippo 0" << endl; - } -} - -void RecordList::destroyReferences(){ - for(RecordList::iterator it=begin(); it!=end(); it++) - delete (*it); - -} - -RecordList RecordList::computePath(unsigned int i, RecordList::const_iterator frame) const{ - unsigned int currentIndex=i; - OrientedPoint p(0,0,0); - RecordList rl; - - //reconstruct a path - bool first=true; - for(RecordList::const_reverse_iterator it(frame); it!=rend(); it++){ - const ScanMatchRecord* scanmatch=dynamic_cast(*it); - if (scanmatch){ - p=scanmatch->poses[currentIndex]; - first=false; - } - const LaserRecord* laser=dynamic_cast(*it); - if (laser && !first){ - LaserRecord* claser=new LaserRecord(*laser); - claser->pose=p; - rl.push_front(claser); - } - const ResampleRecord* resample=dynamic_cast(*it); - if (resample){ - currentIndex=resample->indexes[currentIndex]; - } - } - return rl; -} - - -void RecordList::printPath(ostream& os, unsigned int i, bool err, bool rawodom) const{ - unsigned int currentIndex=i; - OrientedPoint p(0,0,0); - RecordList rl; - double oldWeight=0; - double w=0; - //reconstruct a path - for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){ - const NeffRecord* neff=dynamic_cast(*it); - if (neff){ - NeffRecord* n=new NeffRecord(*neff); - rl.push_front(n); - } - const EntropyRecord* entropy=dynamic_cast(*it); - if (entropy){ - EntropyRecord* n=new EntropyRecord(*entropy); - rl.push_front(n); - } - const ScanMatchRecord* scanmatch=dynamic_cast(*it); - if (scanmatch){ - PoseRecord* pose=new PoseRecord; - pose->dim=0; - p=pose->pose=scanmatch->poses[currentIndex]; - w=scanmatch->weights[currentIndex]-oldWeight; - oldWeight=scanmatch->weights[currentIndex]; - - if (!rawodom) { - rl.push_front(pose); - } - } - const OdometryRecord* odometry=dynamic_cast(*it); - if (odometry){ - PoseRecord* pose=new PoseRecord; - pose->dim=0; - p=pose->pose=odometry->poses[currentIndex]; - pose->time=odometry->time; - if (!rawodom) { - rl.push_front(pose); - } - } - const RawOdometryRecord* rawodometry=dynamic_cast(*it); - if (rawodometry){ - PoseRecord* pose=new PoseRecord; - pose->dim=0; - pose->pose=rawodometry->pose; - pose->time=rawodometry->time; - if (rawodom) { - rl.push_front(pose); - } - } - const PoseRecord* tpose=dynamic_cast(*it); - if (tpose){ - PoseRecord* pose=new PoseRecord(*tpose); - rl.push_front(pose); - } - const LaserRecord* laser=dynamic_cast(*it); - if (laser){ - LaserRecord* claser=new LaserRecord(*laser); - claser->pose=p; - claser->weight=w; - rl.push_front(claser); - } - const CommentRecord* comment=dynamic_cast(*it); - if (comment){ - CommentRecord* ccomment=new CommentRecord(*comment); - rl.push_front(ccomment); - } - const ResampleRecord* resample=dynamic_cast(*it); - if (resample){ - rl.push_front(new ResampleRecord(*resample)); - currentIndex=resample->indexes[currentIndex]; - } - - } - bool started=false; - bool computedTransformation=false; - bool truePosFound=false; - OrientedPoint truePose; - OrientedPoint oldPose; - OrientedPoint trueStart, realStart; - bool tpf=false; - double neff=0; - double totalError=0; - int count=0; - for(RecordList::iterator it=rl.begin(); it!=rl.end(); it++){ - NeffRecord* neffr=dynamic_cast(*it); - if (neffr) - neff=neffr->neff/(double)sampleSize; - started=started || dynamic_cast(*it)?true:false; - if (started && ! truePosFound){ - PoseRecord* tpose=dynamic_cast(*it); - if (tpose && tpose->truePos){ - truePosFound=true; - tpf=true; - truePose=tpose->pose; - os << "# "; - (*it)->write(os); - } - } - if (started && truePosFound && ! computedTransformation){ - PoseRecord* pos=dynamic_cast(*it); - if (pos && !pos->truePos){ - trueStart=truePose; - realStart=pos->pose; - os << "# "; - (*it)->write(os); - computedTransformation=true; - } - } - if (computedTransformation){ - os << setiosflags(ios::fixed) << setprecision(6); - PoseRecord* pos=dynamic_cast(*it); - if (pos){ - if (pos->truePos){ - tpf=true; - truePose=pos->pose; - } else { - if (tpf){ - tpf=false; - OrientedPoint realDelta=absoluteDifference(pos->pose,realStart); - OrientedPoint trueDelta=absoluteDifference(truePose,trueStart); - double ex=realDelta.x-trueDelta.x; - double ey=realDelta.y-trueDelta.y; - double eth=realDelta.theta-trueDelta.theta; - eth=atan2(sin(eth), cos(eth)); - if (! err) - os << "# ERROR "; - os << neff << " " - << ex << " " << ey << " " << eth - << " " << sqrt(ex*ex+ey*ey) << " " << fabs(eth) << endl; - totalError+=sqrt(ex*ex+ey*ey); - count++; - } - } - } - - } - PoseRecord* pos=dynamic_cast(*it); - if (pos) - oldPose=pos->pose; - if (! err) - (*it)->write(os); - delete *it; - } - if (err) - cout << "average error" << totalError/count << endl; -} - -}; //gfsreader - -}; //GMapping; diff --git a/slam_gmapping/openslam_gmapping/gridfastslam/gfsreader.h b/slam_gmapping/openslam_gmapping/gridfastslam/gfsreader.h deleted file mode 100644 index 16041f3..0000000 --- a/slam_gmapping/openslam_gmapping/gridfastslam/gfsreader.h +++ /dev/null @@ -1,101 +0,0 @@ -#ifndef GFSREADER_H -#define GFSREADER_H - -#include -#include -#include -#include -#include -#include - -#define MAX_LINE_LENGHT (1000000) - -namespace GMapping{ - -namespace GFSReader{ - -using namespace std; - -struct Record{ - unsigned int dim; - double time; - virtual ~Record(); - virtual void read(istream& is)=0; - virtual void write(ostream& os); -}; - -struct CommentRecord: public Record{ - string text; - virtual void read(istream& is); - virtual void write(ostream& os); -}; - -struct PoseRecord: public Record{ - PoseRecord(bool ideal=false); - void read(istream& is); - virtual void write(ostream& os); - bool truePos; - OrientedPoint pose; -}; - -struct NeffRecord: public Record{ - void read(istream& is); - virtual void write(ostream& os); - double neff; -}; - -struct EntropyRecord: public Record{ - void read(istream& is); - virtual void write(ostream& os); - double poseEntropy; - double trajectoryEntropy; - double mapEntropy; -}; - - -struct OdometryRecord: public Record{ - virtual void read(istream& is); - vector poses; -}; - -struct RawOdometryRecord: public Record{ - virtual void read(istream& is); - OrientedPoint pose; -}; - -struct ScanMatchRecord: public Record{ - virtual void read(istream& is); - vector poses; - vector weights; -}; - -struct LaserRecord: public Record{ - virtual void read(istream& is); - virtual void write(ostream& os); - vector readings; - OrientedPoint pose; - double weight; -}; - -struct ResampleRecord: public Record{ - virtual void read(istream& is); - vector indexes; -}; - -struct RecordList: public list{ - mutable int sampleSize; - istream& read(istream& is); - double getLogWeight(unsigned int i) const; - double getLogWeight(unsigned int i, RecordList::const_iterator frame) const; - unsigned int getBestIdx() const ; - void printLastParticles(ostream& os) const ; - void printPath(ostream& os, unsigned int i, bool err=false, bool rawodom=false) const; - RecordList computePath(unsigned int i, RecordList::const_iterator frame) const; - void destroyReferences(); -}; - -}; //end namespace GFSReader - -}; //end namespace GMapping - -#endif diff --git a/slam_gmapping/openslam_gmapping/gridfastslam/gridslamprocessor.cpp b/slam_gmapping/openslam_gmapping/gridfastslam/gridslamprocessor.cpp deleted file mode 100644 index a4a510e..0000000 --- a/slam_gmapping/openslam_gmapping/gridfastslam/gridslamprocessor.cpp +++ /dev/null @@ -1,518 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -//#define MAP_CONSISTENCY_CHECK -//#define GENERATE_TRAJECTORIES - -namespace GMapping { - -const double m_distanceThresholdCheck = 20; - -using namespace std; - - GridSlamProcessor::GridSlamProcessor(): m_infoStream(cout){ - - period_ = 5.0; - m_obsSigmaGain=1; - m_resampleThreshold=0.5; - m_minimumScore=0.; - } - - GridSlamProcessor::GridSlamProcessor(const GridSlamProcessor& gsp) - :last_update_time_(0.0), m_particles(gsp.m_particles), m_infoStream(cout){ - - period_ = 5.0; - - m_obsSigmaGain=gsp.m_obsSigmaGain; - m_resampleThreshold=gsp.m_resampleThreshold; - m_minimumScore=gsp.m_minimumScore; - - m_beams=gsp.m_beams; - m_indexes=gsp.m_indexes; - m_motionModel=gsp.m_motionModel; - m_resampleThreshold=gsp.m_resampleThreshold; - m_matcher=gsp.m_matcher; - - m_count=gsp.m_count; - m_readingCount=gsp.m_readingCount; - m_lastPartPose=gsp.m_lastPartPose; - m_pose=gsp.m_pose; - m_odoPose=gsp.m_odoPose; - m_linearDistance=gsp.m_linearDistance; - m_angularDistance=gsp.m_angularDistance; - m_neff=gsp.m_neff; - - cerr << "FILTER COPY CONSTRUCTOR" << endl; - cerr << "m_odoPose=" << m_odoPose.x << " " < >::reference* const, int> PointerMap; - PointerMap pmap; - for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - const ScanMatcherMap& m1(it->map); - const HierarchicalArray2D& h1(m1.storage()); - for (int x=0; x >& a1(h1.m_cells[x][y]); - if (a1.m_reference){ - PointerMap::iterator f=pmap.find(a1.m_reference); - if (f==pmap.end()) - pmap.insert(make_pair(a1.m_reference, 1)); - else - f->second++; - } - } - } - } - cerr << __PRETTY_FUNCTION__ << ": Number of allocated chunks" << pmap.size() << endl; - for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++) - assert(it->first->shares==(unsigned int)it->second); - - cerr << __PRETTY_FUNCTION__ << ": SUCCESS, the error is somewhere else" << endl; -# endif - GridSlamProcessor* cloned=new GridSlamProcessor(*this); - -# ifdef MAP_CONSISTENCY_CHECK - cerr << __PRETTY_FUNCTION__ << ": trajectories end" << endl; - cerr << __PRETTY_FUNCTION__ << ": performing afterclone_fit_test" << endl; - ParticleVector::const_iterator jt=cloned->m_particles.begin(); - for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - const ScanMatcherMap& m1(it->map); - const ScanMatcherMap& m2(jt->map); - const HierarchicalArray2D& h1(m1.storage()); - const HierarchicalArray2D& h2(m2.storage()); - jt++; - for (int x=0; x >& a1(h1.m_cells[x][y]); - const autoptr< Array2D >& a2(h2.m_cells[x][y]); - assert(a1.m_reference==a2.m_reference); - assert((!a1.m_reference) || !(a1.m_reference->shares%2)); - } - } - } - cerr << __PRETTY_FUNCTION__ << ": SUCCESS, the error is somewhere else" << endl; -# endif - return cloned; -} - - GridSlamProcessor::~GridSlamProcessor(){ - cerr << __PRETTY_FUNCTION__ << ": Start" << endl; - cerr << __PRETTY_FUNCTION__ << ": Deleting tree" << endl; - for (std::vector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ -#ifdef TREE_CONSISTENCY_CHECK - TNode* node=it->node; - while(node) - node=node->parent; - cerr << "@" << endl; -#endif - if (it->node) - delete it->node; - //cout << "l=" << it->weight<< endl; - } - -# ifdef MAP_CONSISTENCY_CHECK - cerr << __PRETTY_FUNCTION__ << ": performing predestruction_fit_test" << endl; - typedef std::map >::reference* const, int> PointerMap; - PointerMap pmap; - for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - const ScanMatcherMap& m1(it->map); - const HierarchicalArray2D& h1(m1.storage()); - for (int x=0; x >& a1(h1.m_cells[x][y]); - if (a1.m_reference){ - PointerMap::iterator f=pmap.find(a1.m_reference); - if (f==pmap.end()) - pmap.insert(make_pair(a1.m_reference, 1)); - else - f->second++; - } - } - } - } - cerr << __PRETTY_FUNCTION__ << ": Number of allocated chunks" << pmap.size() << endl; - for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++) - assert(it->first->shares>=(unsigned int)it->second); - cerr << __PRETTY_FUNCTION__ << ": SUCCESS, the error is somewhere else" << endl; -# endif - } - - - - void GridSlamProcessor::setMatchingParameters (double urange, double range, double sigma, int kernsize, double lopt, double aopt, - int iterations, double likelihoodSigma, double likelihoodGain, unsigned int likelihoodSkip){ - m_obsSigmaGain=likelihoodGain; - m_matcher.setMatchingParameters(urange, range, sigma, kernsize, lopt, aopt, iterations, likelihoodSigma, likelihoodSkip); - if (m_infoStream) - m_infoStream << " -maxUrange "<< urange - << " -maxUrange "<< range - << " -sigma "<< sigma - << " -kernelSize "<< kernsize - << " -lstep " << lopt - << " -lobsGain " << m_obsSigmaGain - << " -astep " << aopt << endl; - - - } - -void GridSlamProcessor::setMotionModelParameters -(double srr, double srt, double str, double stt){ - m_motionModel.srr=srr; - m_motionModel.srt=srt; - m_motionModel.str=str; - m_motionModel.stt=stt; - - if (m_infoStream) - m_infoStream << " -srr "<< srr << " -srt "<< srt - << " -str "<< str << " -stt "<< stt << endl; - -} - - void GridSlamProcessor::setUpdateDistances(double linear, double angular, double resampleThreshold){ - m_linearThresholdDistance=linear; - m_angularThresholdDistance=angular; - m_resampleThreshold=resampleThreshold; - if (m_infoStream) - m_infoStream << " -linearUpdate " << linear - << " -angularUpdate "<< angular - << " -resampleThreshold " << m_resampleThreshold << endl; - } - - //HERE STARTS THE BEEF - - GridSlamProcessor::Particle::Particle(const ScanMatcherMap& m): - map(m), pose(0,0,0), weight(0), weightSum(0), gweight(0), previousIndex(0){ - node=0; - } - - - void GridSlamProcessor::setSensorMap(const SensorMap& smap){ - - /* - Construct the angle table for the sensor - - FIXME For now detect the readings of only the front laser, and assume its pose is in the center of the robot - */ - - SensorMap::const_iterator laser_it=smap.find(std::string("FLASER")); - if (laser_it==smap.end()){ - cerr << "Attempting to load the new carmen log format" << endl; - laser_it=smap.find(std::string("ROBOTLASER1")); - assert(laser_it!=smap.end()); - } - const RangeSensor* rangeSensor=dynamic_cast((laser_it->second)); - assert(rangeSensor && rangeSensor->beams().size()); - - m_beams=static_cast(rangeSensor->beams().size()); - double* angles=new double[rangeSensor->beams().size()]; - for (unsigned int i=0; ibeams()[i].pose.theta; - } - m_matcher.setLaserParameters(m_beams, angles, rangeSensor->getPose()); - delete [] angles; - } - - void GridSlamProcessor::init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, OrientedPoint initialPose){ - m_xmin=xmin; - m_ymin=ymin; - m_xmax=xmax; - m_ymax=ymax; - m_delta=delta; - if (m_infoStream) - m_infoStream - << " -xmin "<< m_xmin - << " -xmax "<< m_xmax - << " -ymin "<< m_ymin - << " -ymax "<< m_ymax - << " -delta "<< m_delta - << " -particles "<< size << endl; - - - m_particles.clear(); - TNode* node=new TNode(initialPose, 0, 0, 0); - ScanMatcherMap lmap(Point(xmin+xmax, ymin+ymax)*.5, xmax-xmin, ymax-ymin, delta); - for (unsigned int i=0; i(o.getSensor()); - if (os && os->isIdeal() && m_outputStream){ - m_outputStream << setiosflags(ios::fixed) << setprecision(3); - m_outputStream << "SIMULATOR_POS " << o.getPose().x << " " << o.getPose().y << " " ; - m_outputStream << setiosflags(ios::fixed) << setprecision(6) << o.getPose().theta << " " << o.getTime() << endl; - } - } - - - bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles){ - - /**retireve the position from the reading, and compute the odometry*/ - OrientedPoint relPose=reading.getPose(); - if (!m_count){ - m_lastPartPose=m_odoPose=relPose; - } - - //write the state of the reading and update all the particles using the motion model - for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - OrientedPoint& pose(it->pose); - pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose); - } - - // update the output file - if (m_outputStream.is_open()){ - m_outputStream << setiosflags(ios::fixed) << setprecision(6); - m_outputStream << "ODOM "; - m_outputStream << setiosflags(ios::fixed) << setprecision(3) << m_odoPose.x << " " << m_odoPose.y << " "; - m_outputStream << setiosflags(ios::fixed) << setprecision(6) << m_odoPose.theta << " "; - m_outputStream << reading.getTime(); - m_outputStream << endl; - } - if (m_outputStream.is_open()){ - m_outputStream << setiosflags(ios::fixed) << setprecision(6); - m_outputStream << "ODO_UPDATE "<< m_particles.size() << " "; - for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - OrientedPoint& pose(it->pose); - m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " "; - m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " "; - } - m_outputStream << reading.getTime(); - m_outputStream << endl; - } - - //invoke the callback - onOdometryUpdate(); - - - // accumulate the robot translation and rotation - OrientedPoint move=relPose-m_odoPose; - move.theta=atan2(sin(move.theta), cos(move.theta)); - m_linearDistance+=sqrt(move*move); - m_angularDistance+=fabs(move.theta); - - // if the robot jumps throw a warning - if (m_linearDistance>m_distanceThresholdCheck){ - cerr << "***********************************************************************" << endl; - cerr << "********** Error: m_distanceThresholdCheck overridden!!!! *************" << endl; - cerr << "m_distanceThresholdCheck=" << m_distanceThresholdCheck << endl; - cerr << "Old Odometry Pose= " << m_odoPose.x << " " << m_odoPose.y - << " " <=m_linearThresholdDistance - || m_angularDistance>=m_angularThresholdDistance - || (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_)){ - last_update_time_ = reading.getTime(); - - if (m_outputStream.is_open()){ - m_outputStream << setiosflags(ios::fixed) << setprecision(6); - m_outputStream << "FRAME " << m_readingCount; - m_outputStream << " " << m_linearDistance; - m_outputStream << " " << m_angularDistance << endl; - } - - if (m_infoStream) - m_infoStream << "update frame " << m_readingCount << endl - << "update ld=" << m_linearDistance << " ad=" << m_angularDistance << endl; - - - cerr << "Laser Pose= " << reading.getPose().x << " " << reading.getPose().y - << " " << reading.getPose().theta << endl; - - - //this is for converting the reading in a scan-matcher feedable form - assert(reading.size()==m_beams); - double * plainReading = new double[m_beams]; - for(unsigned int i=0; i(reading.getSensor()), - reading.getTime()); - - if (m_count>0){ - scanMatch(plainReading); - if (m_outputStream.is_open()){ - m_outputStream << "LASER_READING "<< reading.size() << " "; - m_outputStream << setiosflags(ios::fixed) << setprecision(2); - for (RangeReading::const_iterator b=reading.begin(); b!=reading.end(); b++){ - m_outputStream << *b << " "; - } - OrientedPoint p=reading.getPose(); - m_outputStream << setiosflags(ios::fixed) << setprecision(6); - m_outputStream << p.x << " " << p.y << " " << p.theta << " " << reading.getTime()<< endl; - m_outputStream << "SM_UPDATE "<< m_particles.size() << " "; - for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - const OrientedPoint& pose=it->pose; - m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " "; - m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " "; - } - m_outputStream << endl; - } - onScanmatchUpdate(); - - updateTreeWeights(false); - - if (m_infoStream){ - m_infoStream << "neff= " << m_neff << endl; - } - if (m_outputStream.is_open()){ - m_outputStream << setiosflags(ios::fixed) << setprecision(6); - m_outputStream << "NEFF " << m_neff << endl; - } - resample(plainReading, adaptParticles, reading_copy); - - } else { - m_infoStream << "Registering First Scan"<< endl; - for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - m_matcher.invalidateActiveArea(); - m_matcher.computeActiveArea(it->map, it->pose, plainReading); - m_matcher.registerScan(it->map, it->pose, plainReading); - - // cyr: not needed anymore, particles refer to the root in the beginning! - TNode* node=new TNode(it->pose, 0., it->node, 0); - //node->reading=0; - node->reading = reading_copy; - it->node=node; - - } - } - // cerr << "Tree: normalizing, resetting and propagating weights at the end..." ; - updateTreeWeights(false); - // cerr << ".done!" <previousPose=it->pose; - } - - } - if (m_outputStream.is_open()) - m_outputStream << flush; - m_readingCount++; - return processed; - } - - - std::ofstream& GridSlamProcessor::outputStream(){ - return m_outputStream; - } - - std::ostream& GridSlamProcessor::infoStream(){ - return m_infoStream; - } - - - int GridSlamProcessor::getBestParticleIndex() const{ - unsigned int bi=0; - double bw=-std::numeric_limits::max(); - for (unsigned int i=0; i -#include -#include -#include -#include -#include -//#include - -#include -#include - -namespace GMapping { - -using namespace std; - -GridSlamProcessor::TNode::TNode(const OrientedPoint& p, double w, TNode* n, unsigned int c){ - pose=p; - weight=w; - childs=c; - parent=n; - reading=0; - gweight=0; - if (n){ - n->childs++; - } - flag=0; - accWeight=0; -} - - -GridSlamProcessor::TNode::~TNode(){ - if (parent && (--parent->childs)<=0) - delete parent; - assert(!childs); -} - - -//BEGIN State Save/Restore - -GridSlamProcessor::TNodeVector GridSlamProcessor::getTrajectories() const{ - TNodeVector v; - TNodeMultimap parentCache; - TNodeDeque border; - - for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - TNode* node=it->node; - while(node){ - node->flag=false; - node=node->parent; - } - } - - for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - TNode* newnode=new TNode(* (it->node) ); - - v.push_back(newnode); - assert(newnode->childs==0); - if (newnode->parent){ - parentCache.insert(make_pair(newnode->parent, newnode)); - //cerr << __PRETTY_FUNCTION__ << ": node " << newnode->parent << " flag=" << newnode->parent->flag<< endl; - if (! newnode->parent->flag){ - //cerr << __PRETTY_FUNCTION__ << ": node " << newnode->parent << " flag=" << newnode->parent->flag<< endl; - newnode->parent->flag=true; - border.push_back(newnode->parent); - } - } - } - - //cerr << __PRETTY_FUNCTION__ << ": border.size(INITIAL)=" << border.size() << endl; - //cerr << __PRETTY_FUNCTION__ << ": parentCache.size()=" << parentCache.size() << endl; - while (! border.empty()){ - //cerr << __PRETTY_FUNCTION__ << ": border.size(PREPROCESS)=" << border.size() << endl; - //cerr << __PRETTY_FUNCTION__ << ": parentCache.size(PREPROCESS)=" << parentCache.size() << endl; - const TNode* node=border.front(); - //cerr << __PRETTY_FUNCTION__ << ": node " << node << endl; - border.pop_front(); - if (! node) - continue; - - TNode* newnode=new TNode(*node); - node->flag=false; - - //update the parent of all of the referring childs - pair p=parentCache.equal_range(node); - double childs=0; - for (TNodeMultimap::iterator it=p.first; it!=p.second; it++){ - assert(it->second->parent==it->first); - (it->second)->parent=newnode; - //cerr << "PS(" << it->first << ", "<< it->second << ")"; - childs++; - } - ////cerr << endl; - parentCache.erase(p.first, p.second); - //cerr << __PRETTY_FUNCTION__ << ": parentCache.size(POSTERASE)=" << parentCache.size() << endl; - assert(childs==newnode->childs); - - //unmark the node - if ( node->parent ){ - parentCache.insert(make_pair(node->parent, newnode)); - if(! node->parent->flag){ - border.push_back(node->parent); - node->parent->flag=true; - } - } - //insert the parent in the cache - } - //cerr << __PRETTY_FUNCTION__ << " : checking cloned trajectories" << endl; - for (unsigned int i=0; iparent; - } - //cerr << endl; - } - - return v; - -} - -void GridSlamProcessor::integrateScanSequence(GridSlamProcessor::TNode* node){ - //reverse the list - TNode* aux=node; - TNode* reversed=0; - double count=0; - while(aux!=0){ - TNode * newnode=new TNode(*aux); - newnode->parent=reversed; - reversed=newnode; - aux=aux->parent; - count++; - } - - //attach the path to each particle and compute the map; - if (m_infoStream ) - m_infoStream << "Restoring State Nodes=" <pose; - first=false; - oldWeight=aux->weight; - } - - OrientedPoint dp=aux->pose-oldPose; - double dw=aux->weight-oldWeight; - oldPose=aux->pose; - - - double * plainReading = new double[m_beams]; - for(unsigned int i=0; ireading))[i]; - - for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - //compute the position relative to the path; - double s=sin(oldPose.theta-it->pose.theta), - c=cos(oldPose.theta-it->pose.theta); - - it->pose.x+=c*dp.x-s*dp.y; - it->pose.y+=s*dp.x+c*dp.y; - it->pose.theta+=dp.theta; - it->pose.theta=atan2(sin(it->pose.theta), cos(it->pose.theta)); - - //register the scan - m_matcher.invalidateActiveArea(); - m_matcher.computeActiveArea(it->map, it->pose, plainReading); - it->weight+=dw; - it->weightSum+=dw; - - // this should not work, since it->weight is not the correct weight! - // it->node=new TNode(it->pose, it->weight, it->node); - it->node=new TNode(it->pose, 0.0, it->node); - //update the weight - } - - delete [] plainReading; - aux=aux->parent; - } - - //destroy the path - aux=reversed; - while (reversed){ - aux=reversed; - reversed=reversed->parent; - delete aux; - } -} - -//END State Save/Restore - -//BEGIN - -void GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized){ - - if (!weightsAlreadyNormalized) { - normalize(); - } - resetTree(); - propagateWeights(); -} - -void GridSlamProcessor::resetTree(){ - // don't calls this function directly, use updateTreeWeights(..) ! - - for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - TNode* n=it->node; - while (n){ - n->accWeight=0; - n->visitCounter=0; - n=n->parent; - } - } -} - -double propagateWeight(GridSlamProcessor::TNode* n, double weight){ - if (!n) - return weight; - double w=0; - n->visitCounter++; - n->accWeight+=weight; - if (n->visitCounter==n->childs){ - w=propagateWeight(n->parent,n->accWeight); - } - assert(n->visitCounter<=n->childs); - return w; -} - -double GridSlamProcessor::propagateWeights(){ - // don't calls this function directly, use updateTreeWeights(..) ! - - // all nodes must be resetted to zero and weights normalized - - // the accumulated weight of the root - double lastNodeWeight=0; - // sum of the weights in the leafs - double aw=0; - - std::vector::iterator w=m_weights.begin(); - for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - double weight=*w; - aw+=weight; - TNode * n=it->node; - n->accWeight=weight; - lastNodeWeight+=propagateWeight(n->parent,n->accWeight); - w++; - } - - if (fabs(aw-1.0) > 0.0001 || fabs(lastNodeWeight-1.0) > 0.0001) { - cerr << "ERROR: "; - cerr << "root->accWeight=" << lastNodeWeight << " sum_leaf_weights=" << aw << endl; - assert(0); - } - return lastNodeWeight; -} - -}; - -//END diff --git a/slam_gmapping/openslam_gmapping/gridfastslam/motionmodel.cpp b/slam_gmapping/openslam_gmapping/gridfastslam/motionmodel.cpp deleted file mode 100644 index b625ab5..0000000 --- a/slam_gmapping/openslam_gmapping/gridfastslam/motionmodel.cpp +++ /dev/null @@ -1,80 +0,0 @@ -#include -#include -#include - -#define MotionModelConditioningLinearCovariance 0.01 -#define MotionModelConditioningAngularCovariance 0.001 - -namespace GMapping { - - - -OrientedPoint -MotionModel::drawFromMotion (const OrientedPoint& p, double linearMove, double angularMove) const{ - OrientedPoint n(p); - double lm=linearMove + fabs( linearMove ) * sampleGaussian( srr ) + fabs( angularMove ) * sampleGaussian( str ); - double am=angularMove + fabs( linearMove ) * sampleGaussian( srt ) + fabs( angularMove ) * sampleGaussian( stt ); - n.x+=lm*cos(n.theta+.5*am); - n.y+=lm*sin(n.theta+.5*am); - n.theta+=am; - n.theta=atan2(sin(n.theta), cos(n.theta)); - return n; -} - -OrientedPoint -MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{ - double sxy=0.3*srr; - OrientedPoint delta=absoluteDifference(pnew, pold); - OrientedPoint noisypoint(delta); - noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y)); - noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x)); - noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y)); - noisypoint.theta=fmod(noisypoint.theta, 2*M_PI); - if (noisypoint.theta>M_PI) - noisypoint.theta-=2*M_PI; - return absoluteSum(p,noisypoint); -} - - -/* -OrientedPoint -MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{ - - //compute the three stps needed for perfectly matching the two poses if the noise is absent - - OrientedPoint delta=pnew-pold; - double aoffset=atan2(delta.y, delta.x); - double alpha1=aoffset-pold.theta; - alpha1=atan2(sin(alpha1), cos(alpha1)); - double rho=sqrt(delta*delta); - double alpha2=pnew.theta-aoffset; - alpha2=atan2(sin(alpha2), cos(alpha2)); - - OrientedPoint pret=drawFromMotion(p, 0, alpha1); - pret=drawFromMotion(pret, rho, 0); - pret=drawFromMotion(pret, 0, alpha2); - return pret; -} -*/ - - -Covariance3 MotionModel::gaussianApproximation(const OrientedPoint& pnew, const OrientedPoint& pold) const{ - OrientedPoint delta=absoluteDifference(pnew,pold); - double linearMove=sqrt(delta.x*delta.x+delta.y*delta.y); - double angularMove=fabs(delta.x); - double s11=srr*srr*linearMove*linearMove; - double s22=stt*stt*angularMove*angularMove; - double s12=str*angularMove*srt*linearMove; - Covariance3 cov; - double s=sin(pold.theta),c=cos(pold.theta); - cov.xx=c*c*s11+MotionModelConditioningLinearCovariance; - cov.yy=s*s*s11+MotionModelConditioningLinearCovariance; - cov.tt=s22+MotionModelConditioningAngularCovariance; - cov.xy=s*c*s11; - cov.xt=c*s12; - cov.yt=s*s12; - return cov; -} - -}; - diff --git a/slam_gmapping/openslam_gmapping/gui/CMakeLists.txt b/slam_gmapping/openslam_gmapping/gui/CMakeLists.txt deleted file mode 100644 index c8fabda..0000000 --- a/slam_gmapping/openslam_gmapping/gui/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -find_package(Qt5Widgets) -if(Qt5Widgets_FOUND) -else() -find_package(Qt4) -include(${QT_USE_FILE}) -endif() - -include_directories(../include/gmapping ../ ../include/gmapping/log/) - -#add_executable(gfs_nogui gfs_nogui.cpp) - -add_executable(gfs_simplegui gfs_simplegui.cpp gsp_thread.cpp) -if(Qt5Widgets_FOUND) -target_link_libraries(gfs_simplegui gridfastslam Qt5::Widgets) -else() -target_link_libraries(gfs_simplegui gridfastslam ${QT_LIBRARIES}) -endif() - -#add_executable(gfs2img gfs2img.cpp gridfastslam) - -#-include ../global.mk - -#OBJS= gsp_thread.o qparticleviewer.o qgraphpainter.o qmappainter.o - -#APPS= gfs_nogui gfs_simplegui gfs2img -#LDFLAGS+= $(QT_LIB) $(KDE_LIB) -lgridfastslam -lscanmatcher -llog -lsensor_range -lsensor_odometry -lsensor_base -lconfigfile -lutils -lpthread - -#ifeq ($(CARMENSUPPORT),1) -#LDFLAGS+= -lcarmenwrapper -#endif - -#CPPFLAGS+= -I../sensor $(QT_INCLUDE) $(KDE_INCLUDE) -I$(CARMEN_HOME)/include - - -#-include ../build_tools/Makefile.generic-shared-object - - - diff --git a/slam_gmapping/openslam_gmapping/gui/gfs2img.cpp b/slam_gmapping/openslam_gmapping/gui/gfs2img.cpp deleted file mode 100644 index cdd654e..0000000 --- a/slam_gmapping/openslam_gmapping/gui/gfs2img.cpp +++ /dev/null @@ -1,258 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define MAX_LASER_BEAMS 1024 -#define MAX_FILENAME 1024 - -using namespace std; -using namespace GMapping; -using namespace GMapping::GFSReader; - -inline double min(double a, double b){ - return (ab)?a:b; -} - -void computeBoundingBox(double& xmin, double& ymin, double& xmax, double& ymax, const LaserRecord& laser, const OrientedPoint& pose, double maxrange){ - double theta=-M_PI/2+pose.theta; - double theta_step=(laser.readings.size()==180||laser.readings.size()==181)?M_PI/180:M_PI/360; - for (std::vector::const_iterator it=laser.readings.begin(); it!=laser.readings.end(); it++){ - if (*it(*it); - if (lr){ - lastLaser=lr; - continue; - } - const ScanMatchRecord* smr= dynamic_cast(*it); - if (smr && lastLaser){ - for (std::vector::const_iterator pit=smr->poses.begin(); pit!=smr->poses.end(); pit++){ - computeBoundingBox(xmin, ymin, xmax, ymax, *lastLaser, *pit, maxrange); - } - } - } -} -int main(int argc, char** argv){ - QApplication app(argc, argv); - double maxrange=50; - double delta=0.1; - int scanSkip=5; - const char* filename=0; - const char* format="PNG"; - CMD_PARSE_BEGIN(1, argc) - parseDouble("-maxrange", maxrange); - parseDouble("-delta", delta); - parseInt("-skip", scanSkip); - parseString("-filename",filename); - parseString("-format",format); - CMD_PARSE_END - - double maxUrange=maxrange; - if (! filename){ - cout << " supply a gfs file, please" << endl; - cout << " usage gfs2img [options] -filename " << endl; - cout << " [options]:" << endl; - cout << " -maxrange " << endl; - cout << " -delta " << endl; - cout << " -skip " << endl; - cout << " -format " << endl; - return -1; - } - ifstream is(filename); - if (!is){ - cout << " supply an EXISTING gfs file, please" << endl; - return -1; - } - RecordList rl; - rl.read(is); - - int particles=0; - int beams=0; - for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++){ - const OdometryRecord* odometry=dynamic_cast(*it); - if (odometry){ - particles=odometry->dim; - } - const LaserRecord* s=dynamic_cast(*it); - if (s){ - beams=s->readings.size(); - } - if (particles && beams) - break; - } - cout << "Particles from gfs=" << particles << endl; - if (! particles){ - cout << "no particles found, terminating" << endl; - return -1; - } - cout << "Laser beams from gfs=" << beams << endl; - if (! beams){ - cout << "0 beams found, terminating" << endl; - return -1; - } - - - double laserBeamStep=0; - if (beams==180||beams==181){ - laserBeamStep=M_PI/180; - } else if (beams==360||beams==361){ - laserBeamStep=M_PI/360; - } - cout << "Laser beam step" << laserBeamStep << endl; - if (laserBeamStep==0){ - cout << "Invalid Beam Step, terminating" << endl; - return -1; - } - double laserAngles[MAX_LASER_BEAMS]; - double theta=-M_PI/2; - for (int i=0; i(*it); - if (!s) - continue; - scanCount++; - if (scanCount%scanSkip) - continue; - cout << "Frame " << frame << " "; - std::vector paths(particles); - int bestIdx=0; - double bestWeight=-MAXDOUBLE; - for (int p=0; pbestWeight){ - bestWeight=w; - bestIdx=p; - } - } - cout << "bestIdx=" << bestIdx << " bestWeight=" << bestWeight << endl; - - cout << "computing best map" << endl; - ScanMatcherMap smap(center, xmin, ymin, xmax, ymax, delta); - int count=0; - for (RecordList::const_iterator mt=paths[bestIdx].begin(); mt!=paths[bestIdx].end(); mt++){ - const LaserRecord* s=dynamic_cast(*mt); - if (s){ - double rawreadings[MAX_LASER_BEAMS]; - for (uint i=0; ireadings.size(); i++) - rawreadings[i]=s->readings[i]; - matcher.invalidateActiveArea(); - matcher.computeActiveArea(smap, s->pose, rawreadings); -// matcher.allocActiveArea(smap, s->pose, rawreadings); - matcher.registerScan(smap, s->pose, rawreadings); - count++; - } - } - cout << "DONE " << count <=0){ - int grayValue=255-(int)(255.*v); - painter.setPen(QColor(grayValue, grayValue, grayValue)); - painter.drawPoint(x,smap.getMapSizeY()-y-1); - } - } - - /* - cout << "painting trajectories" << endl; - for (int p=0; p(*mt); - if (s){ - IntPoint ip=smap.world2map(s->pose); - ip.y=smap.getMapSizeY()-ip.y-1; - if (!first){ - painter.drawLine( oldPoint.x, oldPoint.y, ip.x, ip.y); - } - oldPoint=ip; - first=false; - } - } - paths[p].destroyReferences();; - } - painter.setPen(QColor(Qt::black)); - bool first=true; - IntPoint oldPoint(0,0); - for (RecordList::const_iterator mt=paths[bestIdx].begin(); mt!=paths[bestIdx].end(); mt++){ - const LaserRecord* s=dynamic_cast(*mt); - if (s){ - IntPoint ip=smap.world2map(s->pose); - ip.y=smap.getMapSizeY()-ip.y-1; - if (!first){ - painter.drawLine( oldPoint.x, oldPoint.y, ip.x, ip.y); - } - oldPoint=ip; - first=false; - } - } - paths[bestIdx].destroyReferences();; - */ - cout << " DONE" << endl; - cout << "writing image" << endl; - QImage img=pixmap.convertToImage(); - char ofilename[MAX_FILENAME]; - sprintf(ofilename,"%s-%.4d.%s",filename, frame, format); - cout << ofilename << endl; - img.save(QString(ofilename), format,0); - frame++; - - } - cout << "For Cyrill: \"The Evil is Outside\"" << endl; -} - diff --git a/slam_gmapping/openslam_gmapping/gui/gfs_logplayer.cpp b/slam_gmapping/openslam_gmapping/gui/gfs_logplayer.cpp deleted file mode 100644 index 674e59c..0000000 --- a/slam_gmapping/openslam_gmapping/gui/gfs_logplayer.cpp +++ /dev/null @@ -1,41 +0,0 @@ -/***************************************************************** - * - * This file is part of the GMAPPING project - * - * GMAPPING Copyright (c) 2004 Giorgio Grisetti, - * Cyrill Stachniss, and Wolfram Burgard - * - * This software is licensed under the "Creative Commons - * License (Attribution-NonCommercial-ShareAlike 2.0)" - * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, - * and Wolfram Burgard. - * - * Further information on this license can be found at: - * http://creativecommons.org/licenses/by-nc-sa/2.0/ - * - * GMAPPING is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR - * PURPOSE. - * - *****************************************************************/ - - -#include -#include "qparticleviewer.h" - -int main (int argc, char ** argv){ - QApplication app(argc, argv); - QParticleViewer * pviewer=new QParticleViewer(0); - app.setMainWidget(pviewer); - pviewer->show(); - FILE* f=fopen(argv[1], "r"); - if (!f) - return -1; - QTextIStream is(f); - pviewer->tis=&is; - pviewer->start(10); - return app.exec(); - std::cout << "DONE: " << argv[1] < -#include "gsp_thread.h" - -using namespace GMapping; - -int main (int argc, char ** argv){ - cerr << "GMAPPING copyright 2004 by Giorgio Grisetti, Cyrill Stachniss," << endl ; - cerr << "and Wolfram Burgard. To be published under the CreativeCommons license," << endl ; - cerr << "see: http://creativecommons.org/licenses/by-nc-sa/2.0/" << endl << endl; - - - GridSlamProcessorThread* gsp= new GridSlamProcessorThread; - if (gsp->init(argc, argv)){ - cout << "GSP INIT ERROR" << endl; - return -1; - } - cout <<"GSP INITIALIZED"<< endl; - if (gsp->loadFiles()){ - cout <<"GSP READFILE ERROR"<< endl; - return -2; - } - cout <<"FILES LOADED"<< endl; - gsp->setMapUpdateTime(1000000); - gsp->start(); - cout <<"THREAD STARTED"<< endl; - bool done=false; - while (!done){ - GridSlamProcessorThread::EventDeque events=gsp->getEvents(); - for (GridSlamProcessorThread::EventDeque::iterator it=events.begin(); it!=events.end(); it++){ - cout << flush; - GridSlamProcessorThread::DoneEvent* doneEvent=dynamic_cast(*it); - if (doneEvent){ - done=true; - cout <<"DONE!"<< endl; - gsp->stop(); - } - if (*it) - delete(*it); - } - } -} diff --git a/slam_gmapping/openslam_gmapping/gui/gfs_simplegui.cpp b/slam_gmapping/openslam_gmapping/gui/gfs_simplegui.cpp deleted file mode 100644 index ba0b1d7..0000000 --- a/slam_gmapping/openslam_gmapping/gui/gfs_simplegui.cpp +++ /dev/null @@ -1,96 +0,0 @@ -/***************************************************************** - * - * This file is part of the GMAPPING project - * - * GMAPPING Copyright (c) 2004 Giorgio Grisetti, - * Cyrill Stachniss, and Wolfram Burgard - * - * This software is licensed under the "Creative Commons - * License (Attribution-NonCommercial-ShareAlike 2.0)" - * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, - * and Wolfram Burgard. - * - * Further information on this license can be found at: - * http://creativecommons.org/licenses/by-nc-sa/2.0/ - * - * GMAPPING is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR - * PURPOSE. - * - *****************************************************************/ - - -#include "qparticleviewer.h" -#include "qgraphpainter.h" -#include -#include -#include -#include -#include -#include - -class GFSMainWindow: public QMainWindow{ -public: - GFSMainWindow(GridSlamProcessorThread* t){ - gsp_thread=t; - QVBoxLayout* layout=new QVBoxLayout(this); - pviewer=new QParticleViewer(this,0,0,gsp_thread); - pviewer->setGeometry(0,0,500,500); - pviewer->setFocusPolicy(Qt::ClickFocus); - layout->addWidget(pviewer); - - gpainter=new QGraphPainter(this); - gpainter->setFixedHeight(100); - layout->addWidget(gpainter); - gpainter->setRange(0,1); - gpainter->setTitle("Neff"); - - help = new QLabel(QString("+/- - zoom | b - show/hide best path | p - show/hide all paths | c - center robot "),this); - help->setMaximumHeight(30); - layout->addWidget(help); - - QObject::connect( pviewer, SIGNAL(neffChanged(double) ), gpainter, SLOT(valueAdded(double)) ); - setTabOrder(pviewer, pviewer); - } - - void start(int c){ - pviewer->start(c); - gpainter->start(c); - } - -protected: - GridSlamProcessorThread* gsp_thread; - QVBoxLayout* layout; - QParticleViewer* pviewer; - QGraphPainter* gpainter; - QLabel* help; -}; - - -int main (int argc, char ** argv){ - cerr << "GMAPPING copyright 2004 by Giorgio Grisetti, Cyrill Stachniss," << endl ; - cerr << "and Wolfram Burgard. To be published under the CreativeCommons license," << endl; - cerr << "see: http://creativecommons.org/licenses/by-nc-sa/2.0/" << endl << endl; - - - GridSlamProcessorThread* gsp= new GridSlamProcessorThread; - if (gsp->init(argc, argv)){ - cerr << "GridFastSlam: Initialization Error!" << endl; - cerr << "(Did you specified an input file for reading?)" << endl; - return -1; - } - if (gsp->loadFiles()){ - cerr <<"Error reading file!"<< endl; - return -2; - } - cerr <<"File successfully loaded!"<< endl; - QApplication app(argc, argv); - GFSMainWindow* mainWin=new GFSMainWindow(gsp); - mainWin->show(); - gsp->setEventBufferSize(10000); - gsp->start(); - mainWin->start(1000); - return app.exec(); -} - diff --git a/slam_gmapping/openslam_gmapping/gui/gsp_thread.cpp b/slam_gmapping/openslam_gmapping/gui/gsp_thread.cpp deleted file mode 100644 index 5d0022f..0000000 --- a/slam_gmapping/openslam_gmapping/gui/gsp_thread.cpp +++ /dev/null @@ -1,643 +0,0 @@ -/***************************************************************** - * - * This file is part of the GMAPPING project - * - * GMAPPING Copyright (c) 2004 Giorgio Grisetti, - * Cyrill Stachniss, and Wolfram Burgard - * - * This software is licensed under the "Creative Commons - * License (Attribution-NonCommercial-ShareAlike 2.0)" - * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, - * and Wolfram Burgard. - * - * Further information on this license can be found at: - * http://creativecommons.org/licenses/by-nc-sa/2.0/ - * - * GMAPPING is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR - * PURPOSE. - * - *****************************************************************/ - - -#include "gsp_thread.h" -#include -#include -#include - -#ifdef CARMEN_SUPPORT - #include -#endif - -#define DEBUG cout << __PRETTY_FUNCTION__ - -using namespace std; - -int GridSlamProcessorThread::init(int argc, const char * const * argv){ - m_argc=argc; - m_argv=argv; - std::string configfilename; - std::string ebuf="not_set"; - - CMD_PARSE_BEGIN_SILENT(1,argc); - parseStringSilent("-cfg",configfilename); - CMD_PARSE_END_SILENT; - - if (configfilename.length()>0){ - ConfigFile cfg(configfilename); - - filename = (std::string) cfg.value("gfs","filename",filename); - outfilename = (std::string) cfg.value("gfs","outfilename",outfilename); - xmin = cfg.value("gfs","xmin", xmin); - xmax = cfg.value("gfs","xmax",xmax); - ymin = cfg.value("gfs","ymin",ymin); - ymax = cfg.value("gfs","ymax",ymax); - delta = cfg.value("gfs","delta",delta); - maxrange = cfg.value("gfs","maxrange",maxrange); - maxUrange = cfg.value("gfs","maxUrange",maxUrange); - regscore = cfg.value("gfs","regscore",regscore); - critscore = cfg.value("gfs","critscore",critscore); - kernelSize = cfg.value("gfs","kernelSize",kernelSize); - sigma = cfg.value("gfs","sigma",sigma); - iterations = cfg.value("gfs","iterations",iterations); - lstep = cfg.value("gfs","lstep",lstep); - astep = cfg.value("gfs","astep",astep); - maxMove = cfg.value("gfs","maxMove",maxMove); - srr = cfg.value("gfs","srr", srr); - srt = cfg.value("gfs","srt", srt); - str = cfg.value("gfs","str", str); - stt = cfg.value("gfs","stt", stt); - particles = cfg.value("gfs","particles",particles); - angularUpdate = cfg.value("gfs","angularUpdate", angularUpdate); - linearUpdate = cfg.value("gfs","linearUpdate", linearUpdate); - lsigma = cfg.value("gfs","lsigma", lsigma); - ogain = cfg.value("gfs","lobsGain", ogain); - lskip = (int)cfg.value("gfs","lskip", lskip); - mapUpdateTime = cfg.value("gfs","mapUpdate", mapUpdateTime); - randseed = cfg.value("gfs","randseed", randseed); - autosize = cfg.value("gfs","autosize", autosize); - readFromStdin = cfg.value("gfs","stdin", readFromStdin); - resampleThreshold = cfg.value("gfs","resampleThreshold", resampleThreshold); - skipMatching = cfg.value("gfs","skipMatching", skipMatching); - onLine = cfg.value("gfs","onLine", onLine); - generateMap = cfg.value("gfs","generateMap", generateMap); - m_minimumScore = cfg.value("gfs","minimumScore", m_minimumScore); - llsamplerange = cfg.value("gfs","llsamplerange", llsamplerange); - lasamplerange = cfg.value("gfs","lasamplerange",lasamplerange ); - llsamplestep = cfg.value("gfs","llsamplestep", llsamplestep); - lasamplestep = cfg.value("gfs","lasamplestep", lasamplestep); - linearOdometryReliability = cfg.value("gfs","linearOdometryReliability",linearOdometryReliability); - angularOdometryReliability = cfg.value("gfs","angularOdometryReliability",angularOdometryReliability); - ebuf = (std::string) cfg.value("gfs","estrategy", ebuf); - considerOdometryCovariance = cfg.value("gfs","considerOdometryCovariance",considerOdometryCovariance); - - } - - - CMD_PARSE_BEGIN(1,argc); - parseString("-cfg",configfilename); /* to avoid the warning*/ - parseString("-filename",filename); - parseString("-outfilename",outfilename); - parseDouble("-xmin",xmin); - parseDouble("-xmax",xmax); - parseDouble("-ymin",ymin); - parseDouble("-ymax",ymax); - parseDouble("-delta",delta); - parseDouble("-maxrange",maxrange); - parseDouble("-maxUrange",maxUrange); - parseDouble("-regscore",regscore); - parseDouble("-critscore",critscore); - parseInt("-kernelSize",kernelSize); - parseDouble("-sigma",sigma); - parseInt("-iterations",iterations); - parseDouble("-lstep",lstep); - parseDouble("-astep",astep); - parseDouble("-maxMove",maxMove); - parseDouble("-srr", srr); - parseDouble("-srt", srt); - parseDouble("-str", str); - parseDouble("-stt", stt); - parseInt("-particles",particles); - parseDouble("-angularUpdate", angularUpdate); - parseDouble("-linearUpdate", linearUpdate); - parseDouble("-lsigma", lsigma); - parseDouble("-lobsGain", ogain); - parseInt("-lskip", lskip); - parseInt("-mapUpdate", mapUpdateTime); - parseInt("-randseed", randseed); - parseFlag("-autosize", autosize); - parseFlag("-stdin", readFromStdin); - parseDouble("-resampleThreshold", resampleThreshold); - parseFlag("-skipMatching", skipMatching); - parseFlag("-onLine", onLine); - parseFlag("-generateMap", generateMap); - parseDouble("-minimumScore", m_minimumScore); - parseDouble("-llsamplerange", llsamplerange); - parseDouble("-lasamplerange", lasamplerange); - parseDouble("-llsamplestep", llsamplestep); - parseDouble("-lasamplestep", lasamplestep); - parseDouble("-linearOdometryReliability",linearOdometryReliability); - parseDouble("-angularOdometryReliability",angularOdometryReliability); - parseString("-estrategy", ebuf); - - parseFlag("-considerOdometryCovariance",considerOdometryCovariance); - CMD_PARSE_END; - - if (filename.length() <=0){ - cout << "no filename specified" << endl; - return -1; - } - return 0; -} - -int GridSlamProcessorThread::loadFiles(const char * fn){ - if (onLine){ - cout << " onLineProcessing" << endl; - return 0; - } - ifstream is; - if (fn) - is.open(fn); - else - is.open(filename.c_str()); - if (! is){ - cout << "no file found" << endl; - return -1; - } - - CarmenConfiguration conf; - conf.load(is); - is.close(); - - sensorMap=conf.computeSensorMap(); - - if (input) - delete input; - - if (! readFromStdin){ - plainStream.open(filename.c_str()); - input=new InputSensorStream(sensorMap, plainStream); - cout << "Plain Stream opened="<< (bool) plainStream << endl; - } else { - input=new InputSensorStream(sensorMap, cin); - cout << "Plain Stream opened on stdin" << endl; - } - return 0; -} - -GridSlamProcessorThread::GridSlamProcessorThread(): GridSlamProcessor(cerr){ - //This are the processor parameters - filename=""; - outfilename=""; - xmin=-100.; - ymin=-100.; - xmax=100.; - ymax=100.; - delta=0.05; - - //scan matching parameters - sigma=0.05; - maxrange=80.; - maxUrange=80.; - regscore=1e4; - lstep=.05; - astep=.05; - kernelSize=1; - iterations=5; - critscore=0.; - maxMove=1.; - lsigma=.075; - ogain=3; - lskip=0; - autosize=false; - skipMatching=false; - - //motion model parameters - srr=0.1, srt=0.1, str=0.1, stt=0.1; - //particle parameters - particles=30; - randseed=0; - - //gfs parameters - angularUpdate=0.5; - linearUpdate=1; - resampleThreshold=0.5; - - input=0; - - pthread_mutex_init(&hp_mutex,0); - pthread_mutex_init(&ind_mutex,0); - pthread_mutex_init(&hist_mutex,0); - running=false; - eventBufferLength=0; - mapUpdateTime=5; - mapTimer=0; - readFromStdin=false; - onLine=false; - generateMap=false; - - // This are the dafault settings for a grid map of 5 cm - llsamplerange=0.01; - llsamplestep=0.01; - lasamplerange=0.005; - lasamplestep=0.005; - linearOdometryReliability=0.; - angularOdometryReliability=0.; - - considerOdometryCovariance=false; -/* - // This are the dafault settings for a grid map of 10 cm - m_llsamplerange=0.1; - m_llsamplestep=0.1; - m_lasamplerange=0.02; - m_lasamplestep=0.01; -*/ - // This are the dafault settings for a grid map of 20/25 cm -/* - m_llsamplerange=0.2; - m_llsamplestep=0.1; - m_lasamplerange=0.02; - m_lasamplestep=0.01; - m_generateMap=false; -*/ - - -} - -GridSlamProcessorThread::~GridSlamProcessorThread(){ - pthread_mutex_destroy(&hp_mutex); - pthread_mutex_destroy(&ind_mutex); - pthread_mutex_destroy(&hist_mutex); - - for (deque::const_iterator it=eventBuffer.begin(); it!=eventBuffer.end(); it++) - delete *it; -} - - -void * GridSlamProcessorThread::fastslamthread(GridSlamProcessorThread* gpt){ - if (! gpt->input && ! gpt->onLine) - return 0; - - - //if started online retrieve the settings from the connection -#ifdef CARMEN_SUPPORT - if (gpt->onLine){ - cout << "starting the process:" << endl; - CarmenWrapper::initializeIPC(gpt->m_argv[0]); - CarmenWrapper::start(gpt->m_argv[0]); - cout << "Waiting for retrieving the sensor map:" << endl; - while (! CarmenWrapper::sensorMapComputed()){ - usleep(500000); - cout << "." << flush; - } - gpt->sensorMap=CarmenWrapper::sensorMap(); - cout << "Connected " << endl; - } -#else - if (gpt->onLine){ - cout << "FATAL ERROR: cannot run online without the carmen support" << endl; - DoneEvent *done=new DoneEvent; - gpt->addEvent(done); - return 0; - } -#endif - - gpt->setSensorMap(gpt->sensorMap); - gpt->setMatchingParameters(gpt->maxUrange, gpt->maxrange, gpt->sigma, gpt->kernelSize, gpt->lstep, gpt->astep, gpt->iterations, gpt->lsigma, gpt->ogain, gpt->lskip); - - double xmin=gpt->xmin, - ymin=gpt->ymin, - xmax=gpt->xmax, - ymax=gpt->ymax; - - OrientedPoint initialPose(0,0,0); - - if (gpt->autosize){ - if (gpt->readFromStdin || gpt->onLine) - cout << "Error, cant autosize form stdin" << endl; - SensorLog * log=new SensorLog(gpt->sensorMap); - ifstream is(gpt->filename.c_str()); - log->load(is); - is.close(); - initialPose=gpt->boundingBox(log, xmin, ymin, xmax, ymax); - delete log; - } - - if( gpt->infoStream()){ - gpt->infoStream() << " initialPose=" << initialPose.x << " " << initialPose.y << " " << initialPose.theta - << cout << " xmin=" << xmin <<" ymin=" << ymin <<" xmax=" << xmax <<" ymax=" << ymax << endl; - } - gpt->setMotionModelParameters(gpt->srr, gpt->srt, gpt->str, gpt->stt); - gpt->setUpdateDistances(gpt->linearUpdate, gpt->angularUpdate, gpt->resampleThreshold); - gpt->setgenerateMap(gpt->generateMap); - gpt->GridSlamProcessor::init(gpt->particles, xmin, ymin, xmax, ymax, gpt->delta, initialPose); - gpt->setllsamplerange(gpt->llsamplerange); - gpt->setllsamplestep(gpt->llsamplestep); - gpt->setlasamplerange(gpt->llsamplerange); - gpt->setlasamplestep(gpt->llsamplestep); - -#define printParam(n)\ - gpt->outputStream() \ - << "PARAM " << \ - #n \ - << " " << gpt->n << endl - - if (gpt->outfilename.length()>0 ){ - gpt->outputStream().open(gpt->outfilename.c_str()); - printParam(filename); - printParam(outfilename); - printParam(xmin); - printParam(ymin); - printParam(xmax); - printParam(ymax); - printParam(delta); - - //scan matching parameters - printParam(sigma); - printParam(maxrange); - printParam(maxUrange); - printParam(regscore); - printParam(lstep); - printParam(astep); - printParam(kernelSize); - printParam(iterations); - printParam(critscore); - printParam(maxMove); - printParam(lsigma); - printParam(ogain); - printParam(lskip); - printParam(autosize); - printParam(skipMatching); - - //motion model parameters - printParam(srr); - printParam(srt); - printParam(str); - printParam(stt); - //particle parameters - printParam(particles); - printParam(randseed); - - //gfs parameters - printParam(angularUpdate); - printParam(linearUpdate); - printParam(resampleThreshold); - - printParam(llsamplerange); - printParam(lasamplerange); - printParam(llsamplestep); - printParam(lasamplestep); - } - #undef printParam - - if (gpt->randseed!=0) - sampleGaussian(1,gpt->randseed); - if (!gpt->infoStream()){ - cerr << "cant open info stream for writing by unuseful debug messages" << endl; - } else { - gpt->infoStream() << "setting randseed" << gpt->randseed<< endl; - } - - -#ifdef CARMEN_SUPPORT - list rrlist; - if (gpt->onLine){ - RangeReading rr(0,0); - while (1){ - while (CarmenWrapper::getReading(rr)){ - RangeReading* nr=new RangeReading(rr); - rrlist.push_back(nr); - gpt->processScan(*nr); - } - } - } -#endif - ofstream rawpath("rawpath.dat"); - if (!gpt->onLine){ - while(*(gpt->input) && gpt->running){ - const SensorReading* r; - (*(gpt->input)) >> r; - if (! r) - continue; - const RangeReading* rr=dynamic_cast(r); - if (rr && gpt->running){ - const RangeSensor* rs=dynamic_cast(rr->getSensor()); - assert (rs && rs->beams().size()==rr->size()); - - bool processed=gpt->processScan(*rr); - rawpath << rr->getPose().x << " " << rr->getPose().y << " " << rr->getPose().theta << endl; - if (0 && processed){ - cerr << "Retrieving state .. "; - TNodeVector trajetories=gpt->getTrajectories(); - cerr << "Done" << endl; - cerr << "Deleting Tree state .. "; - for (TNodeVector::iterator it=trajetories.begin(); it!=trajetories.end(); it++) - delete *it; - cerr << "Done" << endl; - } -// if (0 && processed){ -// cerr << "generating copy" << endl;; -// GridSlamProcessor* m_gsp=gpt->clone(); -// Map* pmap=m_gsp->getParticles()[0].map.toDoubleMap() ; -// cerr << "deleting" << endl; -// delete m_gsp; -// delete pmap; -// } - } - const OdometryReading* o=dynamic_cast(r); - if (o && gpt->running){ - gpt->processTruePos(*o); - TruePosEvent* truepos=new TruePosEvent; - truepos->pose=o->getPose(); - } - } - } - rawpath.close(); - - TNodeVector trajetories=gpt->getTrajectories(); - cerr << "WRITING WEIGHTS" << endl; - int pnumber=0; - for (TNodeVector::iterator it=trajetories.begin(); it!=trajetories.end(); it++){ - char buf[10]; - sprintf(buf, "w-%03d.dat",pnumber); - ofstream weightsStream(buf); - GridSlamProcessor::TNode* n=*it; - double oldWeight=0, oldgWeight=0; - while (n!=0){ - double w=n->weight-oldWeight; - double gw=n->gweight-oldgWeight; - oldWeight=n->weight; - oldgWeight=n->gweight; - weightsStream << w << " " << gw << endl; - n=n->parent; - } - weightsStream.close(); - pnumber++; - cerr << buf << endl; - } - - DoneEvent *done=new DoneEvent; - gpt->addEvent(done); - gpt->infoStream() << "Hallo, I am the gsp thread. I have finished. Do you think it is the case of checking for unlocked mutexes." << endl; - return 0; -} - -std::vector GridSlamProcessorThread::getHypotheses(){ - pthread_mutex_lock(&hp_mutex); - std::vector retval(hypotheses); - pthread_mutex_unlock(&hp_mutex); - return retval; -} - -std::vector GridSlamProcessorThread::getIndexes(){ - pthread_mutex_lock(&ind_mutex); - std::vector retval(indexes); - pthread_mutex_unlock(&ind_mutex); - return retval; -} - -void GridSlamProcessorThread::start(){ - if (running) - return; - running=true; - pthread_create(&gfs_thread, 0, (void * (*)(void *))fastslamthread, (void *) this); -} - -void GridSlamProcessorThread::stop(){ - if (! running){ - cout << "PORCO CAZZO" << endl; - return; - } - running=false; - void * retval; - pthread_join(gfs_thread, &retval); -} - -void GridSlamProcessorThread::onOdometryUpdate(){ - pthread_mutex_lock(&hp_mutex); - hypotheses.clear(); - weightSums.clear(); - for (GridSlamProcessor::ParticleVector::const_iterator part=getParticles().begin(); part!=getParticles().end(); part++ ){ - hypotheses.push_back(part->pose); - weightSums.push_back(part->weightSum); - } - - ParticleMoveEvent* event=new ParticleMoveEvent; - event->scanmatched=false; - event->hypotheses=hypotheses; - event->weightSums=weightSums; - event->neff=m_neff; - pthread_mutex_unlock(&hp_mutex); - - addEvent(event); - - syncOdometryUpdate(); -} - -void GridSlamProcessorThread::onResampleUpdate(){ - pthread_mutex_lock(&ind_mutex); - pthread_mutex_lock(&hp_mutex); - - indexes=GridSlamProcessor::getIndexes(); - - assert (indexes.size()==getParticles().size()); - ResampleEvent* event=new ResampleEvent; - event->indexes=indexes; - - pthread_mutex_unlock(&hp_mutex); - pthread_mutex_unlock(&ind_mutex); - - addEvent(event); - - syncResampleUpdate(); -} - -void GridSlamProcessorThread::onScanmatchUpdate(){ - pthread_mutex_lock(&hp_mutex); - hypotheses.clear(); - weightSums.clear(); - unsigned int bestIdx=0; - double bestWeight=-1e1000; - unsigned int idx=0; - for (GridSlamProcessor::ParticleVector::const_iterator part=getParticles().begin(); part!=getParticles().end(); part++ ){ - hypotheses.push_back(part->pose); - weightSums.push_back(part->weightSum); - if(part->weightSum>bestWeight){ - bestIdx=idx; - bestWeight=part->weightSum; - } - idx++; - } - - ParticleMoveEvent* event=new ParticleMoveEvent; - event->scanmatched=true; - event->hypotheses=hypotheses; - event->weightSums=weightSums; - event->neff=m_neff; - addEvent(event); - - if (! mapTimer){ - MapEvent* event=new MapEvent; - event->index=bestIdx; - event->pmap=new ScanMatcherMap(getParticles()[bestIdx].map); - event->pose=getParticles()[bestIdx].pose; - addEvent(event); - } - - mapTimer++; - mapTimer=mapTimer%mapUpdateTime; - - pthread_mutex_unlock(&hp_mutex); - - syncOdometryUpdate(); -} - -void GridSlamProcessorThread::syncOdometryUpdate(){ -} - -void GridSlamProcessorThread::syncResampleUpdate(){ -} - -void GridSlamProcessorThread::syncScanmatchUpdate(){ -} - -void GridSlamProcessorThread::addEvent(GridSlamProcessorThread::Event * e){ - pthread_mutex_lock(&hist_mutex); - while (eventBuffer.size()>eventBufferLength){ - Event* event=eventBuffer.front(); - delete event; - eventBuffer.pop_front(); - } - eventBuffer.push_back(e); - pthread_mutex_unlock(&hist_mutex); -} - -GridSlamProcessorThread::EventDeque GridSlamProcessorThread::getEvents(){ - pthread_mutex_lock(&hist_mutex); - EventDeque copy(eventBuffer); - eventBuffer.clear(); - pthread_mutex_unlock(&hist_mutex); - return copy; -} - -GridSlamProcessorThread::Event::~Event(){} - -GridSlamProcessorThread::MapEvent::~MapEvent(){ - if (pmap) - delete pmap; -} - -void GridSlamProcessorThread::setEventBufferSize(unsigned int length){ - eventBufferLength=length; -} - -OrientedPoint GridSlamProcessorThread::boundingBox(SensorLog* log, double& xmin, double& ymin, double& xmax, double& ymax) const{ - OrientedPoint initialPose(0,0,0); - initialPose=log->boundingBox(xmin, ymin, xmax, ymax); - xmin-=3*maxrange; - ymin-=3*maxrange; - xmax+=3*maxrange; - ymax+=3*maxrange; - return initialPose; -} diff --git a/slam_gmapping/openslam_gmapping/gui/gsp_thread.h b/slam_gmapping/openslam_gmapping/gui/gsp_thread.h deleted file mode 100644 index 51c0dc9..0000000 --- a/slam_gmapping/openslam_gmapping/gui/gsp_thread.h +++ /dev/null @@ -1,177 +0,0 @@ -/***************************************************************** - * - * This file is part of the GMAPPING project - * - * GMAPPING Copyright (c) 2004 Giorgio Grisetti, - * Cyrill Stachniss, and Wolfram Burgard - * - * This software is licensed under the "Creative Commons - * License (Attribution-NonCommercial-ShareAlike 2.0)" - * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, - * and Wolfram Burgard. - * - * Further information on this license can be found at: - * http://creativecommons.org/licenses/by-nc-sa/2.0/ - * - * GMAPPING is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR - * PURPOSE. - * - *****************************************************************/ - - -#ifndef GSP_THREAD_H -#define GSP_THREAD_H - -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace GMapping; - - -#define MAX_STRING_LENGTH 1024 - - -struct GridSlamProcessorThread : public GridSlamProcessor { - struct Event{ - virtual ~Event(); - }; - - struct ParticleMoveEvent: public Event{ - bool scanmatched; - double neff; - std::vector hypotheses; - std::vector weightSums; - }; - - struct TruePosEvent : public Event{ - OrientedPoint pose; - }; - - struct ResampleEvent: public Event{ - std::vector indexes; - }; - - struct MapEvent: public Event{ - ScanMatcherMap* pmap; - unsigned int index; - OrientedPoint pose; - virtual ~MapEvent(); - }; - - struct DoneEvent: public Event{ - }; - - typedef deque EventDeque; - - GridSlamProcessorThread(); - ~GridSlamProcessorThread(); - int init(int argc, const char * const * argv); - int loadFiles(const char * fn=0); - static void * fastslamthread(GridSlamProcessorThread* gpt); - std::vector getHypotheses(); - std::vector getIndexes(); - - EventDeque getEvents(); - - void start(); - void stop(); - - virtual void onOdometryUpdate(); - virtual void onResampleUpdate(); - virtual void onScanmatchUpdate(); - - virtual void syncOdometryUpdate(); - virtual void syncResampleUpdate(); - virtual void syncScanmatchUpdate(); - - void setEventBufferSize(unsigned int length); - inline void setMapUpdateTime(unsigned int ut) {mapUpdateTime=ut;} - inline bool isRunning() const {return running;} - OrientedPoint boundingBox(SensorLog* log, double& xmin, double& ymin, double& xmax, double& ymax) const; - private: - - void addEvent(Event *); - EventDeque eventBuffer; - - unsigned int eventBufferLength; - unsigned int mapUpdateTime; - unsigned int mapTimer; - - //thread interaction stuff - std::vector hypotheses; - std::vector indexes; - std::vector weightSums; - pthread_mutex_t hp_mutex, ind_mutex, hist_mutex; - pthread_t gfs_thread; - bool running; - - //This are the processor parameters - std::string filename; - std::string outfilename; - - double xmin; - double ymin; - double xmax; - double ymax; - bool autosize; - double delta; - double resampleThreshold; - - //scan matching parameters - double sigma; - double maxrange; - double maxUrange; - double regscore; - double lstep; - double astep; - int kernelSize; - int iterations; - double critscore; - double maxMove; - unsigned int lskip; - - //likelihood - double lsigma; - double ogain; - double llsamplerange, lasamplerange; - double llsamplestep, lasamplestep; - double linearOdometryReliability; - double angularOdometryReliability; - - - //motion model parameters - double srr, srt, str, stt; - //particle parameters - int particles; - bool skipMatching; - - //gfs parameters - double angularUpdate; - double linearUpdate; - - //robot config - SensorMap sensorMap; - //input stream - InputSensorStream* input; - std::ifstream plainStream; - bool readFromStdin; - bool onLine; - bool generateMap; - bool considerOdometryCovariance; - unsigned int randseed; - - //dirty carmen interface - const char* const * m_argv; - unsigned int m_argc; - -}; -#endif diff --git a/slam_gmapping/openslam_gmapping/gui/qgraphpainter.cpp b/slam_gmapping/openslam_gmapping/gui/qgraphpainter.cpp deleted file mode 100644 index 744e23b..0000000 --- a/slam_gmapping/openslam_gmapping/gui/qgraphpainter.cpp +++ /dev/null @@ -1,142 +0,0 @@ -/***************************************************************** - * - * This file is part of the GMAPPING project - * - * GMAPPING Copyright (c) 2004 Giorgio Grisetti, - * Cyrill Stachniss, and Wolfram Burgard - * - * This software is licensed under the "Creative Commons - * License (Attribution-NonCommercial-ShareAlike 2.0)" - * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, - * and Wolfram Burgard. - * - * Further information on this license can be found at: - * http://creativecommons.org/licenses/by-nc-sa/2.0/ - * - * GMAPPING is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR - * PURPOSE. - * - *****************************************************************/ - - -#include -#include "qgraphpainter.h" -#include "moc_qgraphpainter.cpp" -using namespace std; - -QGraphPainter::QGraphPainter( QWidget * parent, const char * name, WFlags f): - QWidget(parent, name, f|WRepaintNoErase|WResizeNoErase){ - m_pixmap=new QPixmap(size()); - m_pixmap->fill(Qt::white); - autoscale=false; - m_useYReference = false; -} - -void QGraphPainter::resizeEvent(QResizeEvent * sizeev){ - m_pixmap->resize(sizeev->size()); -} - -QGraphPainter::~QGraphPainter(){ - delete m_pixmap; -} - -void QGraphPainter::clear(){ - values.clear(); -} - -void QGraphPainter::valueAdded(double v){ - values.push_back(v); -} - -void QGraphPainter::valueAdded(double v, double _min, double _max){ - setRange(_min, _max); - values.push_back(v); -} - -void QGraphPainter::setYReference(double y){ - m_useYReference = true; - reference=y; -} - -void QGraphPainter::disableYReference(){ - m_useYReference = false; -} - - -void QGraphPainter::setTitle(const char* t){ - title=t; -} - -void QGraphPainter::setRange(double _min, double _max){ - min=_min; - max=_max; -} - -void QGraphPainter::setAutoscale(bool a) { - autoscale=a; -} - -bool QGraphPainter::getAutoscale() const { - return autoscale; -} - -void QGraphPainter::timerEvent(QTimerEvent * te) { - if (te->timerId()==timer) - update(); -} - -void QGraphPainter::start(int period){ - timer=startTimer(period); -} - - - -void QGraphPainter::paintEvent ( QPaintEvent * ){ - m_pixmap->fill(Qt::white); - QPainter painter(m_pixmap); - double _min=MAXDOUBLE, _max=-MAXDOUBLE; - if (autoscale){ - for (unsigned int i=0; i<(unsigned int)width() && ivalues[i]?_max:values[i]; - } - } else { - _min=min; - _max=max; - } - - - painter.setPen(Qt::black); - painter.drawRect(0, 0, width(), height()); - const int boundary=2; - int xoffset=40; - double scale=((double)height()-2*boundary-2)/(_max-_min); - - if (m_useYReference) { - painter.setPen(Qt::green); - painter.drawLine(xoffset+boundary/2, height()-(int)(scale*(reference-_min)), - width()-boundary/2, height()-(int)(scale*(reference-_min))); - } - painter.setPen(Qt::blue); - unsigned int start=0; - if (values.size()>(unsigned int)width()-2*boundary-xoffset) - start=values.size()-width()+2*boundary+xoffset; - int oldv=0; - if ((unsigned int)width()-2*boundary-xoffset>1 && values.size()>1) - oldv = (int)(scale*(values[1+start]-_min)) + boundary; - - for (unsigned int i=1; i<(unsigned int)width()-2*boundary-xoffset && iwidth(),m_pixmap->height(),CopyROP); -} - diff --git a/slam_gmapping/openslam_gmapping/gui/qgraphpainter.h b/slam_gmapping/openslam_gmapping/gui/qgraphpainter.h deleted file mode 100644 index feb633b..0000000 --- a/slam_gmapping/openslam_gmapping/gui/qgraphpainter.h +++ /dev/null @@ -1,67 +0,0 @@ -/***************************************************************** - * - * This file is part of the GMAPPING project - * - * GMAPPING Copyright (c) 2004 Giorgio Grisetti, - * Cyrill Stachniss, and Wolfram Burgard - * - * This software is licensed under the "Creative Commons - * License (Attribution-NonCommercial-ShareAlike 2.0)" - * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, - * and Wolfram Burgard. - * - * Further information on this license can be found at: - * http://creativecommons.org/licenses/by-nc-sa/2.0/ - * - * GMAPPING is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR - * PURPOSE. - * - *****************************************************************/ - - -#ifndef QGRAPHPAINTER_H -#define QGRAPHPAINTER_H - -#include -#include -#include -#include -#include -#include -#include - -typedef std::deque DoubleDeque; - -class QGraphPainter : public QWidget{ - Q_OBJECT - public: - QGraphPainter( QWidget * parent = 0, const char * name = 0, Qt::WindowFlags f = 0); - virtual ~QGraphPainter(); - public slots: - void clear(); - void valueAdded(double); - void valueAdded(double, double, double); - void setYReference(double y); - void disableYReference(); - void setRange(double min, double max); - void start(int period); - void setTitle(const char* title); - void setAutoscale(bool a); - bool getAutoscale() const; - protected: - virtual void timerEvent(QTimerEvent * te); - virtual void resizeEvent(QResizeEvent *); - double min, max, reference; - DoubleDeque values; - bool autoscale; - bool m_useYReference; - int timer; - virtual void paintEvent ( QPaintEvent *paintevent ); - QPixmap * m_pixmap; - QString title; -}; - -#endif - diff --git a/slam_gmapping/openslam_gmapping/gui/qmappainter.cpp b/slam_gmapping/openslam_gmapping/gui/qmappainter.cpp deleted file mode 100644 index 3aa2dfb..0000000 --- a/slam_gmapping/openslam_gmapping/gui/qmappainter.cpp +++ /dev/null @@ -1,32 +0,0 @@ -#include "qmappainter.h" -#include "moc_qmappainter.cpp" - -QMapPainter::QMapPainter( QWidget * parent, const char * name, WFlags f): - QWidget(parent, name, f|WRepaintNoErase|WResizeNoErase){ - m_pixmap=new QPixmap(size()); - m_pixmap->fill(Qt::white); -} - -void QMapPainter::resizeEvent(QResizeEvent * sizeev){ - m_pixmap->resize(sizeev->size()); -} - -QMapPainter::~QMapPainter(){ - delete m_pixmap; -} - - -void QMapPainter::timerEvent(QTimerEvent * te) { - if (te->timerId()==timer) - update(); -} - -void QMapPainter::start(int period){ - timer=startTimer(period); -} - - -void QMapPainter::paintEvent ( QPaintEvent * ){ - bitBlt(this,0,0,m_pixmap,0,0,m_pixmap->width(),m_pixmap->height(),CopyROP); -} - diff --git a/slam_gmapping/openslam_gmapping/gui/qmappainter.h b/slam_gmapping/openslam_gmapping/gui/qmappainter.h deleted file mode 100644 index d121d1b..0000000 --- a/slam_gmapping/openslam_gmapping/gui/qmappainter.h +++ /dev/null @@ -1,60 +0,0 @@ -#ifndef QMAPPAINTER_H -#define QMAPPAINTER_H - -#include -#include -#include -#include -#include -#include - -class QMapPainter : public QWidget{ - public: - QMapPainter( QWidget * parent = 0, const char * name = 0, WFlags f = 0); - virtual ~QMapPainter(); - public: - template < typename Cell > - void setPixmap(unsigned int xsize, unsigned int ysize, Cell** values); - template < typename Iterator > - void drawPoints(const Iterator& begin, const Iterator& end, unsigned char r, unsigned char g, unsigned char b); - void start(int period); - protected: - virtual void timerEvent(QTimerEvent * te); - virtual void resizeEvent(QResizeEvent *); - int timer; - virtual void paintEvent ( QPaintEvent *paintevent ); - QPixmap * m_pixmap; -}; - -template -void QMapPainter::setPixmap(unsigned int xsize, unsigned int ysize, Cell** values){ - QSize s(xsize, ysize); - m_pixmap->resize(s); - m_pixmap->fill(Qt::white); - QPainter painter(m_pixmap); - for (unsigned int x=0; x<(unsigned int)xsize; x++) - for (unsigned int y=0; y<(unsigned int)ysize; y++){ - double v=(double) values[x][y]; - - if (v>=0){ - unsigned int grayVal=(unsigned char) (255-(unsigned char)(255*v)); - painter.setPen(QColor(grayVal, grayVal, grayVal)); - } else { - painter.setPen(QColor(255, 100, 100)); - } - painter.drawPoint(x,ysize-y); - } -} - -template < typename Iterator > -void QMapPainter::drawPoints(const Iterator& begin, const Iterator& end, unsigned char r, unsigned char g, unsigned char b){ - QPainter painter(m_pixmap); - painter.setPen(QColor(r,g,b)); - for (Iterator it=begin; it!=end; it++){ - GMapping::IntPoint p=(GMapping::IntPoint)*it; - painter.drawPoint(p.x, height()-p.y); - } -} - -#endif - diff --git a/slam_gmapping/openslam_gmapping/gui/qnavigatorwidget.cpp b/slam_gmapping/openslam_gmapping/gui/qnavigatorwidget.cpp deleted file mode 100644 index d818554..0000000 --- a/slam_gmapping/openslam_gmapping/gui/qnavigatorwidget.cpp +++ /dev/null @@ -1,126 +0,0 @@ -#include "qnavigatorwidget.h" -#include -using namespace GMapping; - - -QNavigatorWidget::QNavigatorWidget( QWidget * parent, const char * name, WFlags f) -: QMapPainter(parent, name, f), dumper("navigator", 1){ - robotPose=IntPoint(0,0); - robotHeading=0; - confirmLocalization=false; - repositionRobot=false; - startWalker=false; - enableMotion=false; - goHome=false; - trajectorySent=false; - writeImages=false; - drawRobot=true; - wantsQuit=false; -} - -QNavigatorWidget::~QNavigatorWidget(){} - - -void QNavigatorWidget::mousePressEvent ( QMouseEvent * e ){ - QPoint p=e->pos(); - int mx=p.x(); - int my=height()-p.y(); - if (!(e->state()&Qt::ShiftButton) && e->button()==Qt::LeftButton) { - if (trajectorySent) - trajectoryPoints.clear(); - e->accept(); - IntPoint p=IntPoint(mx, my); - trajectoryPoints.push_back(p); - trajectorySent=false; - } - if (e->state()&Qt::ControlButton && e->button()==Qt::LeftButton){ - e->accept(); - robotPose=IntPoint(mx, my); - repositionRobot=true; - confirmLocalization=true; - } - if (e->state()&Qt::ControlButton && e->button()==Qt::RightButton){ - e->accept(); - IntPoint p(mx, my); - p=p-robotPose; - robotHeading=atan2(p.y, p.x); - repositionRobot=true; - confirmLocalization=true; - } -} - -void QNavigatorWidget::keyPressEvent ( QKeyEvent * e ){ - if (e->key()==Qt::Key_Delete){ - e->accept(); - if (!trajectoryPoints.empty()) - trajectoryPoints.pop_back(); - } - if (e->key()==Qt::Key_S){ - e->accept(); - enableMotion=!enableMotion; - } - if (e->key()==Qt::Key_W){ - e->accept(); - startWalker=!startWalker; - } - if (e->key()==Qt::Key_G){ - e->accept(); - startGlobalLocalization=true; - } - if (e->key()==Qt::Key_T){ - e->accept(); - trajectorySent=true; - } - if (e->key()==Qt::Key_R){ - e->accept(); - goHome=true; - } - if (e->key()==Qt::Key_C){ - e->accept(); - confirmLocalization=true; - - } - if (e->key()==Qt::Key_Q){ - e->accept(); - wantsQuit=true; - - } - if (e->key()==Qt::Key_D){ - e->accept(); - drawRobot=!drawRobot;; - - } -} - -void QNavigatorWidget::paintEvent ( QPaintEvent * ){ - QPixmap pixmap(*m_pixmap); - QPainter painter(&pixmap); - if (trajectorySent) - painter.setPen(Qt::red); - bool first=true; - int oldx=0, oldy=0; - //paint the path - for (std::list::const_iterator it=trajectoryPoints.begin(); it!=trajectoryPoints.end(); it++){ - int x=it->x; - int y=height()-it->y; - if (! first) - painter.drawLine(oldx, oldy, x,y); - oldx=x; - oldy=y; - first=false; - } - //paint the robot - if (drawRobot){ - painter.setPen(Qt::black); - int rx=robotPose.x; - int ry=height()-robotPose.y; - int robotSize=6; - painter.drawLine(rx, ry, - rx+(int)(robotSize*cos(robotHeading)), ry-(int)(robotSize*sin(robotHeading))); - painter.drawEllipse(rx-robotSize, ry-robotSize, 2*robotSize, 2*robotSize); - } - if (writeImages){ - dumper.dump(pixmap); - } - bitBlt(this,0,0,&pixmap,0,0,pixmap.width(),pixmap.height(),CopyROP); -} diff --git a/slam_gmapping/openslam_gmapping/gui/qnavigatorwidget.h b/slam_gmapping/openslam_gmapping/gui/qnavigatorwidget.h deleted file mode 100644 index 9766b9d..0000000 --- a/slam_gmapping/openslam_gmapping/gui/qnavigatorwidget.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef _QNAVIGATOR_WIDGET_H -#define _QNAVIGATOR_WIDGET_H - -#include "qmappainter.h" -#include "qpixmapdumper.h" -#include -#include - -class QNavigatorWidget : public QMapPainter{ - public: - QNavigatorWidget( QWidget * parent = 0, const char * name = 0, WFlags f = 0); - virtual ~QNavigatorWidget(); - std::list trajectoryPoints; - bool repositionRobot; - GMapping::IntPoint robotPose; - double robotHeading; - bool confirmLocalization; - bool enableMotion; - bool startWalker; - bool startGlobalLocalization; - bool trajectorySent; - bool goHome; - bool wantsQuit; - bool writeImages; - QPixmapDumper dumper; - bool drawRobot; - - protected: - virtual void paintEvent ( QPaintEvent *paintevent ); - virtual void mousePressEvent ( QMouseEvent * e ); - virtual void keyPressEvent ( QKeyEvent * e ); -}; - -#endif - diff --git a/slam_gmapping/openslam_gmapping/gui/qparticleviewer.cpp b/slam_gmapping/openslam_gmapping/gui/qparticleviewer.cpp deleted file mode 100644 index 633aee5..0000000 --- a/slam_gmapping/openslam_gmapping/gui/qparticleviewer.cpp +++ /dev/null @@ -1,453 +0,0 @@ -/***************************************************************** - * - * This file is part of the GMAPPING project - * - * GMAPPING Copyright (c) 2004 Giorgio Grisetti, - * Cyrill Stachniss, and Wolfram Burgard - * - * This software is licensed under the "Creative Commons - * License (Attribution-NonCommercial-ShareAlike 2.0)" - * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, - * and Wolfram Burgard. - * - * Further information on this license can be found at: - * http://creativecommons.org/licenses/by-nc-sa/2.0/ - * - * GMAPPING is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR - * PURPOSE. - * - *****************************************************************/ - - -#include "qparticleviewer.h" -#include "moc_qparticleviewer.cpp" -#include - - -using namespace GMapping; - -QParticleViewer::QParticleViewer( QWidget * parent, const char * name , WFlags f, GridSlamProcessorThread* thread): QWidget(parent, name, f|WRepaintNoErase|WResizeNoErase){ - viewCenter=Point(0.,0.); - setMinimumSize(500,500); - mapscale=10.; - m_pixmap=new QPixmap(500,500); - m_pixmap->fill(Qt::white); - gfs_thread=thread; - tis=0; - m_particleSize=0; - m_refresh=false; - bestMap=0; - dragging=false; - showPaths=0; - showBestPath=1; - count=0; - writeToFile=0; -} - -QParticleViewer::~QParticleViewer(){ - if (m_pixmap) - delete m_pixmap; -} - -void QParticleViewer::paintEvent ( QPaintEvent *paintevent ){ - if (! m_pixmap) - return; - bitBlt(this,0,0,m_pixmap,0,0,m_pixmap->width(),m_pixmap->height(),CopyROP); -} - -void QParticleViewer::mousePressEvent ( QMouseEvent *event ){ - if (event->button()==LeftButton){ - dragging=true; - draggingPos=event->pos(); - } -} -void QParticleViewer::mouseMoveEvent ( QMouseEvent *event ){ - if (dragging){ - QPoint delta=event->pos()-draggingPos; - draggingPos=event->pos(); - viewCenter.x-=delta.x()/mapscale; - viewCenter.y+=delta.y()/mapscale; - update(); - } -} - -void QParticleViewer::mouseReleaseEvent ( QMouseEvent *event ){ - if (event->button()==LeftButton){ - dragging=false; - } -} - -void QParticleViewer::keyPressEvent ( QKeyEvent* e ){ - switch (e->key()){ - case Qt::Key_B: showBestPath=!showBestPath; break; - case Qt::Key_P: showPaths=!showPaths; break; - case Qt::Key_Plus: mapscale *=1.25; break; - case Qt::Key_Minus: mapscale/=1.25; break; - case Qt::Key_C: viewCenter=bestParticlePose; break; - default:; - } -} - - -void QParticleViewer::resizeEvent(QResizeEvent * sizeev){ - if (!m_pixmap) - return; - cerr << "QParticleViewer::resizeEvent" << sizeev->size().width()<< " " << sizeev->size().height() << endl; - m_pixmap->resize(sizeev->size()); -} - -void QParticleViewer::drawParticleMove(const QParticleViewer::OrientedPointVector& oldPose, const QParticleViewer::OrientedPointVector& newPose){ - assert(oldPose.size()==newPose.size()); - QPainter painter(m_pixmap); - painter.setPen(Qt::red); - OrientedPointVector::const_iterator nit=newPose.begin(); - for(OrientedPointVector::const_iterator it=oldPose.begin(); it!=oldPose.end(); it++, nit++){ - IntPoint p0=map2pic(*it); - IntPoint p1=map2pic(*nit); - painter.drawLine( - (int)(p0.x), (int)(p0.y), (int)(p1.x), (int)(p1.y) - ); - } -} - -void QParticleViewer::drawFromFile(){ - if(! tis) - return; - if (tis->atEnd()) - return; - QTextIStream& is=*tis; - - string line=is.readLine(); - istringstream lineStream(line); - string recordType; - lineStream >> recordType; - if (recordType=="LASER_READING"){ - //do nothing with the laser - cout << "l" << flush; - } - if (recordType=="ODO_UPDATE"){ - //just move the particles - if (m_particleSize) - m_refresh=true; - m_oldPose=m_newPose; - m_newPose.clear(); - unsigned int size; - lineStream >> size; - if (!m_particleSize) - m_particleSize=size; - assert(m_particleSize==size); - for (unsigned int i=0; i< size; i++){ - OrientedPoint p; - double w; - lineStream >> p.x; - lineStream >> p.y; - lineStream >> p.theta; - lineStream >> w; - m_newPose.push_back(p); - } - cout << "o" << flush; - } - if (recordType=="SM_UPDATE"){ - if (m_particleSize) - m_refresh=true; - m_oldPose=m_newPose; - m_newPose.clear(); - unsigned int size; - lineStream >> size; - if (!m_particleSize) - m_particleSize=size; - assert(m_particleSize==size); - for (unsigned int i=0; i< size; i++){ - OrientedPoint p; - double w; - lineStream >> p.x; - lineStream >> p.y; - lineStream >> p.theta; - lineStream >> w; - m_newPose.push_back(p); - } - cout << "u" << flush; - } - if (recordType=="RESAMPLE"){ - unsigned int size; - lineStream >> size; - if (!m_particleSize) - m_particleSize=size; - assert(m_particleSize==size); - OrientedPointVector temp(size); - for (unsigned int i=0; i< size; i++){ - unsigned int ind; - lineStream >> ind; - temp[i]=m_newPose[ind]; - } - m_newPose=temp; - cout << "r" << flush; - } - if (m_refresh){ - drawParticleMove(m_oldPose, m_newPose); - m_refresh=false; - } -} - -void QParticleViewer::drawMap(const ScanMatcherMap& map){ - //cout << "Map received" << map.getMapSizeX() << " " << map.getMapSizeY() << endl; - QPainter painter(m_pixmap); - painter.setPen(Qt::black); - m_pixmap->fill(QColor(200,200,255)); - unsigned int count=0; - - Point wmin=Point(pic2map(IntPoint(-m_pixmap->width()/2,m_pixmap->height()/2))); - Point wmax=Point(pic2map(IntPoint(m_pixmap->width()/2,-m_pixmap->height()/2))); - IntPoint imin=map.world2map(wmin); - IntPoint imax=map.world2map(wmax); - /* cout << __PRETTY_FUNCTION__ << endl; - cout << " viewCenter=" << viewCenter.x << "," << viewCenter.y << endl; - cout << " wmin=" << wmin.x << "," << wmin.y << " wmax=" << wmax.x << "," << wmax.y << endl; - cout << " imin=" << imin.x << "," << imin.y << " imax=" << imax.x << "," << imax.y << endl; - cout << " mapSize=" << map.getMapSizeX() << "," << map.getMapSizeY() << endl;*/ - for(int x=0; xwidth(); x++) - for(int y=0; yheight(); y++){ - //IntPoint ip=IntPoint(x,y)+imin; - //Point p=map.map2world(ip); - Point p=pic2map(IntPoint(x-m_pixmap->width()/2, - y-m_pixmap->height()/2)); - - //if (map.storage().isInside(map.world2map(p))){ - double v=map.cell(p); - if (v>=0){ - int grayValue=255-(int)(255.*v); - painter.setPen(QColor(grayValue, grayValue, grayValue)); - painter.drawPoint(x,y); - count++; - } - } -} - - -void QParticleViewer::drawFromMemory(){ - if (! gfs_thread) - return; - m_pixmap->fill(Qt::white); - GridSlamProcessorThread::EventDeque events=gfs_thread->getEvents(); - for (GridSlamProcessorThread::EventDeque::const_iterator it=events.begin(); it!=events.end();it++){ - GridSlamProcessorThread::MapEvent* mapEvent= dynamic_cast(*it); - if (mapEvent){ - //cout << "Map: bestIdx=" << mapEvent->index <pmap; - mapEvent->pmap=0; - bestParticlePose=mapEvent->pose; - delete mapEvent; - }else{ - GridSlamProcessorThread::DoneEvent* doneEvent= dynamic_cast(*it); - if (doneEvent){ - gfs_thread->stop(); - delete doneEvent; - } else - history.push_back(*it); - } - - } - if (bestMap) - drawMap(*bestMap); - - unsigned int particleSize=0; - std::vector oldPose, newPose; - vector indexes; - - GridSlamProcessorThread::EventDeque::reverse_iterator it=history.rbegin(); - while (!particleSize && it!=history.rend()){ - GridSlamProcessorThread::ParticleMoveEvent* move= dynamic_cast(*it); - GridSlamProcessorThread::ResampleEvent* resample= dynamic_cast(*it); - if (move) - particleSize=move->hypotheses.size(); - if (resample) - particleSize=resample->indexes.size(); - it++; - } - - //check for the best index - double wmax=-1e2000; - unsigned int bestIdx=0; - bool emitted=false; - for (unsigned int i=0; i(*it); - if (move && move->scanmatched){ - double cw=move->weightSums[currentIndex]; - if (cw>wmax){ - wmax=cw; - bestIdx=currentIndex; - } - done=true; - if (! emitted){ - emit neffChanged(move->neff/particleSize); - emitted=true; - } - } - GridSlamProcessorThread::ResampleEvent* resample= dynamic_cast(*it); - if (resample){ - currentIndex=resample->indexes[currentIndex]; - } - } - } - //cout << "bestIdx=" << bestIdx << endl; - QPainter painter(m_pixmap); - - for (unsigned int i=0; i(*it); - if (move){ - OrientedPoint pold=move->hypotheses[currentIndex]; - IntPoint p0=map2pic(pold)+IntPoint(m_pixmap->width()/2,m_pixmap->height()/2); - IntPoint p1=map2pic(pnew)+IntPoint(m_pixmap->width()/2,m_pixmap->height()/2);; - if (first){ - painter.drawPoint(p0.x, p0.y); - } else { - painter.drawLine(p0.x, p0.y, p1.x, p1.y); - } - first=false; - if (!(showPaths || showBestPath&&i==particleSize)) - break; - pnew=pold; - } - GridSlamProcessorThread::ResampleEvent* resample= dynamic_cast(*it); - if (resample && ! first){ - currentIndex=resample->indexes[currentIndex]; - } - } - } - if (writeToFile && bestMap){ - if (! (count%writeToFile) ){ - char name[100]; - sprintf(name,"dump-%05d.png", count/writeToFile); - cout << " Writing " << name <<" ..." << flush; - QImage image=m_pixmap->convertToImage(); - bool rv=image.save(name,"PNG"); - if (rv) - cout << " Done"; - else - cout << " ERROR"; - cout << endl; - } - count++; - } -} - -void QParticleViewer::timerEvent(QTimerEvent * te) { - if (te->timerId()==timer) { - if ( tis) - drawFromFile(); - else{ - drawFromMemory(); - update(); - } - } -} - - -void QParticleViewer::start(int period){ - timer=startTimer(period); -} - -void QParticleViewer::refreshParameters(){ - //scanmatcher - matchingParameters.maxrange=gfs_thread->getlaserMaxRange(); - matchingParameters.urange=gfs_thread->getusableRange(); - matchingParameters.ssigma=gfs_thread->getgaussianSigma(); - matchingParameters.sreg=gfs_thread->getregScore(); - matchingParameters.scrit=gfs_thread->getcritScore(); - matchingParameters.ksize=gfs_thread->getkernelSize(); - matchingParameters.lstep=gfs_thread->getoptLinearDelta(); - matchingParameters.astep=gfs_thread->getoptAngularDelta(); - matchingParameters.iterations=gfs_thread->getoptRecursiveIterations(); - - //start - startParameters.srr=gfs_thread->getsrr(); - startParameters.stt=gfs_thread->getstt(); - startParameters.str=gfs_thread->getstr(); - startParameters.srt=gfs_thread->getsrt(); - - startParameters.xmin=gfs_thread->getxmin(); - startParameters.ymin=gfs_thread->getymin(); - startParameters.xmax=gfs_thread->getxmax(); - startParameters.ymax=gfs_thread->getymax(); - startParameters.delta=gfs_thread->getdelta(); - - startParameters.particles=gfs_thread->getParticles().size(); - startParameters.resampleThreshold=gfs_thread->getresampleThreshold(); - startParameters.outFileName=0; -} - -void QParticleViewer::start(){ - gfs_thread->setMatchingParameters( - matchingParameters.urange, - matchingParameters.maxrange, - matchingParameters.ssigma, - matchingParameters.ksize, - matchingParameters.lstep, - matchingParameters.astep, - matchingParameters.iterations, - startParameters.lsigma, - startParameters.lgain, - startParameters.lskip); - gfs_thread->setMotionModelParameters( - startParameters.srr, - startParameters.srt, - startParameters.srt, - startParameters.stt); - gfs_thread->setUpdateDistances( - startParameters.linearUpdate, - startParameters.angularUpdate, - startParameters.resampleThreshold - ); - ((GridSlamProcessor*)(gfs_thread))->init( - startParameters.particles, - startParameters.xmin, - startParameters.ymin, - startParameters.xmax, - startParameters.ymax, - startParameters.delta, - startParameters.initialPose); - gfs_thread->start(); -} - -void QParticleViewer::setMatchingParameters(const QParticleViewer::MatchingParameters& mp){ - matchingParameters=mp; -} - -void QParticleViewer::setStartParameters(const QParticleViewer::StartParameters& sp){ - startParameters=sp; -} - -void QParticleViewer::stop(){ - gfs_thread->stop(); -} - -void QParticleViewer::loadFile(const char * fn){ - gfs_thread->loadFiles(fn); - /* - startParameters.initialPose= - gfs_thread->boundingBox( - startParameters.xmin, - startParameters.ymin, - startParameters.xmax, - startParameters.ymax); - */ -} diff --git a/slam_gmapping/openslam_gmapping/gui/qparticleviewer.h b/slam_gmapping/openslam_gmapping/gui/qparticleviewer.h deleted file mode 100644 index 63ff4fd..0000000 --- a/slam_gmapping/openslam_gmapping/gui/qparticleviewer.h +++ /dev/null @@ -1,161 +0,0 @@ -/***************************************************************** - * - * This file is part of the GMAPPING project - * - * GMAPPING Copyright (c) 2004 Giorgio Grisetti, - * Cyrill Stachniss, and Wolfram Burgard - * - * This software is licensed under the "Creative Commons - * License (Attribution-NonCommercial-ShareAlike 2.0)" - * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, - * and Wolfram Burgard. - * - * Further information on this license can be found at: - * http://creativecommons.org/licenses/by-nc-sa/2.0/ - * - * GMAPPING is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR - * PURPOSE. - * - *****************************************************************/ - - -#ifndef QPARTICLEVIEWER_H -#define QPARTICLEVIEWER_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include "gsp_thread.h" - -namespace GMapping { - -class QParticleViewer : public QWidget{ - Q_OBJECT - public: - struct StartParameters{ - //motionmodel - double srr, srt, str, stt; - //map - double xmin, ymin, xmax, ymax, delta; - OrientedPoint initialPose; - //likelihood - double lsigma, lgain; - unsigned int lskip; - //update - double linearUpdate, angularUpdate; - //filter - unsigned int particles; - double resampleThreshold; - //mode - bool drawFromObservation; - //output - const char * outFileName; - }; - - struct MatchingParameters{ - //ranges - double maxrange, urange; - //score - double ssigma, sreg, scrit; - unsigned int ksize; - //search - double lstep, astep; - unsigned int iterations; - }; - - void refreshParameters(); //reads the parameters from the thread - inline void setGSP( GridSlamProcessorThread* thread){gfs_thread=thread;} - - - typedef std::vector OrientedPointVector; - QParticleViewer( QWidget * parent = 0, const char * name = 0, Qt::WindowFlags f = 0, GridSlamProcessorThread* thread=0 ); - virtual ~QParticleViewer(); - virtual void timerEvent(QTimerEvent * te); - virtual void resizeEvent(QResizeEvent *); - - void drawFromFile(); - void drawFromMemory(); - void drawMap(const ScanMatcherMap& map); - void start(int period); - QTextStream* tis; - - MatchingParameters matchingParameters; - StartParameters startParameters; - - int writeToFile; - public slots: - void setMatchingParameters(const MatchingParameters& mp); - void setStartParameters(const StartParameters& mp); - void start(); - void stop(); - void loadFile(const char *); - signals: - void neffChanged(double); - void poseEntropyChanged(double, double, double); - void trajectoryEntropyChanged(double, double, double); - void mapsEntropyChanged(double); - void mapsIGainChanged(double); - - protected: - ifstream inputStream; - ofstream outputStream; - - - protected: - inline Point pic2map(const IntPoint& p) - {return viewCenter+Point(p.x/mapscale, -p.y/mapscale); } - inline IntPoint map2pic(const Point& p) - {return IntPoint((int)((p.x-viewCenter.x)*mapscale),(int)((viewCenter.y-p.y)*mapscale)); } - - int timer; - virtual void paintEvent ( QPaintEvent *paintevent ); - void drawParticleMove(const OrientedPointVector& start, const OrientedPointVector& end); - QPixmap* m_pixmap; - - //thread interaction - GridSlamProcessorThread* gfs_thread; - GridSlamProcessorThread::EventDeque history; - - //mouse movement - virtual void mousePressEvent(QMouseEvent*); - virtual void mouseReleaseEvent(QMouseEvent*); - virtual void mouseMoveEvent(QMouseEvent*); - QPoint draggingPos; - bool dragging; - - //particle plotting - virtual void keyPressEvent ( QKeyEvent* e ); - - //map painting - double mapscale; - Point viewCenter; - Point bestParticlePose; - ScanMatcherMap * bestMap; - - // view mode - bool showPaths; - bool showBestPath; - - // file plotting - QParticleViewer::OrientedPointVector m_oldPose, m_newPose; - unsigned int m_particleSize; - bool m_refresh; - int count; -}; - -}; - -#endif - diff --git a/slam_gmapping/openslam_gmapping/gui/qpixmapdumper.cpp b/slam_gmapping/openslam_gmapping/gui/qpixmapdumper.cpp deleted file mode 100644 index 239b209..0000000 --- a/slam_gmapping/openslam_gmapping/gui/qpixmapdumper.cpp +++ /dev/null @@ -1,33 +0,0 @@ -#include "qpixmapdumper.h" -#include -#include - -QPixmapDumper::QPixmapDumper(std::string p, int c){ - format="PNG"; - prefix=p; - reset(); - cycles=c; -} - -void QPixmapDumper::reset(){ - cycles=0; - frame=0; - counter=0; -} - -#define filename_bufsize 1024 - -bool QPixmapDumper::dump(const QPixmap& pixmap){ - bool processed=false; - if (!(counter%cycles)){ - char buf[filename_bufsize]; - sprintf(buf,"%s-%05d.%s",prefix.c_str(), frame, format.c_str()); - QImage image=pixmap.convertToImage(); - image.save(QString(buf), format.c_str(),0); - frame ++; - } - counter++; - return processed; -} - - diff --git a/slam_gmapping/openslam_gmapping/gui/qpixmapdumper.h b/slam_gmapping/openslam_gmapping/gui/qpixmapdumper.h deleted file mode 100644 index 7193d12..0000000 --- a/slam_gmapping/openslam_gmapping/gui/qpixmapdumper.h +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef _QPIXMAPDUMPER_H_ -#define _QPIXMAPDUMPER_H_ -#include -#include -#include - - -struct QPixmapDumper{ - QPixmapDumper(std::string prefix, int cycles); - void reset(); - std::string prefix; - std::string format; - bool dump(const QPixmap& pixmap); - int counter; - int cycles; - int frame; -}; - -#endif diff --git a/slam_gmapping/openslam_gmapping/gui/qslamandnavwidget.cpp b/slam_gmapping/openslam_gmapping/gui/qslamandnavwidget.cpp deleted file mode 100644 index 9c81264..0000000 --- a/slam_gmapping/openslam_gmapping/gui/qslamandnavwidget.cpp +++ /dev/null @@ -1,128 +0,0 @@ -#include "qslamandnavwidget.h" -#include -using namespace GMapping; - - -QSLAMandNavWidget::QSLAMandNavWidget( QWidget * parent, const char * name, WFlags f) -: QMapPainter(parent, name, f), dumper("slamandnav", 1){ - robotPose=IntPoint(0,0); - robotHeading=0; - slamRestart=false; - slamFinished=false; - enableMotion=false; - startWalker=false; - trajectorySent=false; - goHome=false; - wantsQuit=false; - printHelp=false; - saveGoalPoints=false; - writeImages=false; - drawRobot=true; -} - -QSLAMandNavWidget::~QSLAMandNavWidget(){} - - -void QSLAMandNavWidget::mousePressEvent ( QMouseEvent * e ){ - QPoint p=e->pos(); - int mx=p.x(); - int my=height()-p.y(); - if ( e->state()&Qt::ShiftButton && e->button()==Qt::LeftButton) { - if (trajectorySent) - trajectoryPoints.clear(); - e->accept(); - IntPoint p=IntPoint(mx, my); - trajectoryPoints.push_back(p); - trajectorySent=false; - } -} - -void QSLAMandNavWidget::keyPressEvent ( QKeyEvent * e ){ - if (e->key()==Qt::Key_Delete){ - e->accept(); - if (!trajectoryPoints.empty()) - trajectoryPoints.pop_back(); - } - if (e->key()==Qt::Key_S){ - e->accept(); - enableMotion=!enableMotion; - } - if (e->key()==Qt::Key_W){ - e->accept(); - startWalker=!startWalker; - } - if (e->key()==Qt::Key_G){ - e->accept(); - slamRestart=true; - } - if (e->key()==Qt::Key_T){ - e->accept(); - trajectorySent=true; - } - if (e->key()==Qt::Key_R){ - e->accept(); - goHome=true; - } - if (e->key()==Qt::Key_C){ - e->accept(); - slamFinished=true; - - } - if (e->key()==Qt::Key_Q){ - e->accept(); - wantsQuit=true; - - } - if (e->key()==Qt::Key_H){ - e->accept(); - printHelp=true; - //BABSI - //insert the help here - } - if (e->key()==Qt::Key_Y){ - e->accept(); - saveGoalPoints=true; - //BABSI - //insert the help here - } - if (e->key()==Qt::Key_D){ - e->accept(); - drawRobot=!drawRobot; - //BABSI - //insert the help here - } -} - -void QSLAMandNavWidget::paintEvent ( QPaintEvent * ){ - QPixmap pixmap(*m_pixmap); - QPainter painter(&pixmap); - if (trajectorySent) - painter.setPen(Qt::red); - bool first=true; - int oldx=0, oldy=0; - //paint the path - for (std::list::const_iterator it=trajectoryPoints.begin(); it!=trajectoryPoints.end(); it++){ - int x=it->x; - int y=height()-it->y; - if (! first) - painter.drawLine(oldx, oldy, x,y); - oldx=x; - oldy=y; - first=false; - } - - //paint the robot - if (drawRobot){ - painter.setPen(Qt::black); - int rx=robotPose.x; - int ry=height()-robotPose.y; - int robotSize=6; - painter.drawLine(rx, ry, - rx+(int)(robotSize*cos(robotHeading)), ry-(int)(robotSize*sin(robotHeading))); - painter.drawEllipse(rx-robotSize, ry-robotSize, 2*robotSize, 2*robotSize); - } - if (writeImages){ - dumper.dump(pixmap); - } - bitBlt(this,0,0,&pixmap,0,0,pixmap.width(),pixmap.height(),CopyROP); -} diff --git a/slam_gmapping/openslam_gmapping/gui/qslamandnavwidget.h b/slam_gmapping/openslam_gmapping/gui/qslamandnavwidget.h deleted file mode 100644 index e33ca3f..0000000 --- a/slam_gmapping/openslam_gmapping/gui/qslamandnavwidget.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef _QSLAMANDNAV_WIDGET_H -#define _QSLAMANDNAV_WIDGET_H - -#include "qmappainter.h" -#include "qpixmapdumper.h" -#include -#include - -class QSLAMandNavWidget : public QMapPainter{ - public: - QSLAMandNavWidget( QWidget * parent = 0, const char * name = 0, WFlags f = 0); - virtual ~QSLAMandNavWidget(); - std::list trajectoryPoints; - GMapping::IntPoint robotPose; - double robotHeading; - - bool slamRestart; - bool slamFinished; - bool enableMotion; - bool startWalker; - bool trajectorySent; - bool goHome; - bool wantsQuit; - bool printHelp; - bool saveGoalPoints; - bool writeImages; - bool drawRobot; - QPixmapDumper dumper; - - - protected: - virtual void paintEvent ( QPaintEvent *paintevent ); - virtual void mousePressEvent ( QMouseEvent * e ); - virtual void keyPressEvent ( QKeyEvent * e ); -}; - -#endif - diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/grid/accessstate.h b/slam_gmapping/openslam_gmapping/include/gmapping/grid/accessstate.h deleted file mode 100644 index 43bd86a..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/grid/accessstate.h +++ /dev/null @@ -1,9 +0,0 @@ -#ifndef ACCESSTATE_H -#define ACCESSTATE_H - -namespace GMapping { -enum AccessibilityState{Outside=0x0, Inside=0x1, Allocated=0x2}; -}; - -#endif - diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/grid/array2d.h b/slam_gmapping/openslam_gmapping/include/gmapping/grid/array2d.h deleted file mode 100644 index 67c9388..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/grid/array2d.h +++ /dev/null @@ -1,188 +0,0 @@ -#ifndef ARRAY2D_H -#define ARRAY2D_H - -#include -#include -#include "accessstate.h" - -#include - -#ifndef __PRETTY_FUNCTION__ -#define __FUNCDNAME__ -#endif - -namespace GMapping { - -template class Array2D{ - public: - Array2D(int xsize=0, int ysize=0); - Array2D& operator=(const Array2D &); - Array2D(const Array2D &); - ~Array2D(); - void clear(); - void resize(int xmin, int ymin, int xmax, int ymax); - - - inline bool isInside(int x, int y) const; - inline const Cell& cell(int x, int y) const; - inline Cell& cell(int x, int y); - inline AccessibilityState cellState(int x, int y) const { return (AccessibilityState) (isInside(x,y)?(Inside|Allocated):Outside);} - - inline bool isInside(const IntPoint& p) const { return isInside(p.x, p.y);} - inline const Cell& cell(const IntPoint& p) const {return cell(p.x,p.y);} - inline Cell& cell(const IntPoint& p) {return cell(p.x,p.y);} - inline AccessibilityState cellState(const IntPoint& p) const { return cellState(p.x, p.y);} - - inline int getPatchSize() const{return 0;} - inline int getPatchMagnitude() const{return 0;} - inline int getXSize() const {return m_xsize;} - inline int getYSize() const {return m_ysize;} - inline Cell** cells() {return m_cells;} - Cell ** m_cells; - protected: - int m_xsize, m_ysize; -}; - - -template -Array2D::Array2D(int xsize, int ysize){ -// assert(xsize>0); -// assert(ysize>0); - m_xsize=xsize; - m_ysize=ysize; - if (m_xsize>0 && m_ysize>0){ - m_cells=new Cell*[m_xsize]; - for (int i=0; i -class HierarchicalArray2D: public Array2D > >{ - public: - typedef std::set< point, pointcomparator > PointSet; - HierarchicalArray2D(int xsize, int ysize, int patchMagnitude=5); - HierarchicalArray2D(const HierarchicalArray2D& hg); - HierarchicalArray2D& operator=(const HierarchicalArray2D& hg); - virtual ~HierarchicalArray2D(){} - void resize(int ixmin, int iymin, int ixmax, int iymax); - inline int getPatchSize() const {return m_patchMagnitude;} - inline int getPatchMagnitude() const {return m_patchMagnitude;} - - inline const Cell& cell(int x, int y) const; - inline Cell& cell(int x, int y); - inline bool isAllocated(int x, int y) const; - inline AccessibilityState cellState(int x, int y) const ; - inline IntPoint patchIndexes(int x, int y) const; - - inline const Cell& cell(const IntPoint& p) const { return cell(p.x,p.y); } - inline Cell& cell(const IntPoint& p) { return cell(p.x,p.y); } - inline bool isAllocated(const IntPoint& p) const { return isAllocated(p.x,p.y);} - inline AccessibilityState cellState(const IntPoint& p) const { return cellState(p.x,p.y); } - inline IntPoint patchIndexes(const IntPoint& p) const { return patchIndexes(p.x,p.y);} - - inline void setActiveArea(const PointSet&, bool patchCoords=false); - const PointSet& getActiveArea() const {return m_activeArea; } - inline void allocActiveArea(); - protected: - virtual Array2D * createPatch(const IntPoint& p) const; - PointSet m_activeArea; - int m_patchMagnitude; - int m_patchSize; -}; - -template -HierarchicalArray2D::HierarchicalArray2D(int xsize, int ysize, int patchMagnitude) - :Array2D > >::Array2D((xsize>>patchMagnitude), (ysize>>patchMagnitude)){ - m_patchMagnitude=patchMagnitude; - m_patchSize=1< -HierarchicalArray2D::HierarchicalArray2D(const HierarchicalArray2D& hg) - :Array2D > >::Array2D((hg.m_xsize>>hg.m_patchMagnitude), (hg.m_ysize>>hg.m_patchMagnitude)) // added by cyrill: if you have a resize error, check this again -{ - this->m_xsize=hg.m_xsize; - this->m_ysize=hg.m_ysize; - this->m_cells=new autoptr< Array2D >*[this->m_xsize]; - for (int x=0; xm_xsize; x++){ - this->m_cells[x]=new autoptr< Array2D >[this->m_ysize]; - for (int y=0; ym_ysize; y++) - this->m_cells[x][y]=hg.m_cells[x][y]; - } - this->m_patchMagnitude=hg.m_patchMagnitude; - this->m_patchSize=hg.m_patchSize; -} - -template -void HierarchicalArray2D::resize(int xmin, int ymin, int xmax, int ymax){ - int xsize=xmax-xmin; - int ysize=ymax-ymin; - autoptr< Array2D > ** newcells=new autoptr< Array2D > *[xsize]; - for (int x=0; x >[ysize]; - for (int y=0; y >(0); - } - } - int dx= xmin < 0 ? 0 : xmin; - int dy= ymin < 0 ? 0 : ymin; - int Dx=xmaxm_xsize?xmax:this->m_xsize; - int Dy=ymaxm_ysize?ymax:this->m_ysize; - for (int x=dx; xm_cells[x][y]; - } - delete [] this->m_cells[x]; - } - delete [] this->m_cells; - this->m_cells=newcells; - this->m_xsize=xsize; - this->m_ysize=ysize; -} - -template -HierarchicalArray2D& HierarchicalArray2D::operator=(const HierarchicalArray2D& hg){ -// Array2D > >::operator=(hg); - if (this->m_xsize!=hg.m_xsize || this->m_ysize!=hg.m_ysize){ - for (int i=0; im_xsize; i++) - delete [] this->m_cells[i]; - delete [] this->m_cells; - this->m_xsize=hg.m_xsize; - this->m_ysize=hg.m_ysize; - this->m_cells=new autoptr< Array2D >*[this->m_xsize]; - for (int i=0; im_xsize; i++) - this->m_cells[i]=new autoptr< Array2D > [this->m_ysize]; - } - for (int x=0; xm_xsize; x++) - for (int y=0; ym_ysize; y++) - this->m_cells[x][y]=hg.m_cells[x][y]; - - m_activeArea.clear(); - m_patchMagnitude=hg.m_patchMagnitude; - m_patchSize=hg.m_patchSize; - return *this; -} - - -template -void HierarchicalArray2D::setActiveArea(const typename HierarchicalArray2D::PointSet& aa, bool patchCoords){ - m_activeArea.clear(); - for (PointSet::const_iterator it= aa.begin(); it!=aa.end(); ++it) { - IntPoint p; - if (patchCoords) - p=*it; - else - p=patchIndexes(*it); - m_activeArea.insert(p); - } -} - -template -Array2D* HierarchicalArray2D::createPatch(const IntPoint& ) const{ - return new Array2D(1< -AccessibilityState HierarchicalArray2D::cellState(int x, int y) const { - if (this->isInside(patchIndexes(x,y))) { - if(isAllocated(x,y)) - return (AccessibilityState)((int)Inside|(int)Allocated); - else - return Inside; - } - return Outside; -} - -template -void HierarchicalArray2D::allocActiveArea(){ - for (PointSet::const_iterator it= m_activeArea.begin(); it!=m_activeArea.end(); ++it){ - const autoptr< Array2D >& ptr=this->m_cells[it->x][it->y]; - Array2D* patch=0; - if (!ptr){ - patch=createPatch(*it); - } else{ - patch=new Array2D(*ptr); - } - this->m_cells[it->x][it->y]=autoptr< Array2D >(patch); - } -} - -template -bool HierarchicalArray2D::isAllocated(int x, int y) const{ - IntPoint c=patchIndexes(x,y); - autoptr< Array2D >& ptr=this->m_cells[c.x][c.y]; - return (ptr != 0); -} - -template -IntPoint HierarchicalArray2D::patchIndexes(int x, int y) const{ - if (x>=0 && y>=0) - return IntPoint(x>>m_patchMagnitude, y>>m_patchMagnitude); - return IntPoint(-1, -1); -} - -template -Cell& HierarchicalArray2D::cell(int x, int y){ - IntPoint c=patchIndexes(x,y); - assert(this->isInside(c.x, c.y)); - if (!this->m_cells[c.x][c.y]){ - Array2D* patch=createPatch(IntPoint(x,y)); - this->m_cells[c.x][c.y]=autoptr< Array2D >(patch); - //cerr << "!!! FATAL: your dick is going to fall down" << endl; - } - autoptr< Array2D >& ptr=this->m_cells[c.x][c.y]; - return (*ptr).cell(IntPoint(x-(c.x< -const Cell& HierarchicalArray2D::cell(int x, int y) const{ - assert(isAllocated(x,y)); - IntPoint c=patchIndexes(x,y); - const autoptr< Array2D >& ptr=this->m_cells[c.x][c.y]; - return (*ptr).cell(IntPoint(x-(c.x< -#include -#include "accessstate.h" -#include "array2d.h" - -namespace GMapping { -/** -The cells have to define the special value Cell::Unknown to handle with the unallocated areas. -The cells have to define (int) constructor; -*/ -typedef Array2D DoubleArray2D; - -template -class Map{ - public: - Map(int mapSizeX, int mapSizeY, double delta); - Map(const Point& center, double worldSizeX, double worldSizeY, double delta); - Map(const Point& center, double xmin, double ymin, double xmax, double ymax, double delta); - /* the standard implementation works filen in this case*/ - //Map(const Map& g); - //Map& operator =(const Map& g); - void resize(double xmin, double ymin, double xmax, double ymax); - void grow(double xmin, double ymin, double xmax, double ymax); - inline IntPoint world2map(const Point& p) const; - inline Point map2world(const IntPoint& p) const; - inline IntPoint world2map(double x, double y) const - { return world2map(Point(x,y)); } - inline Point map2world(int x, int y) const - { return map2world(IntPoint(x,y)); } - - inline Point getCenter() const {return m_center;} - inline double getWorldSizeX() const {return m_worldSizeX;} - inline double getWorldSizeY() const {return m_worldSizeY;} - inline int getMapSizeX() const {return m_mapSizeX;} - inline int getMapSizeY() const {return m_mapSizeY;} - inline double getDelta() const { return m_delta;} - inline double getMapResolution() const { return m_delta;} - inline double getResolution() const { return m_delta;} - inline void getSize(double & xmin, double& ymin, double& xmax, double& ymax) const { - Point min=map2world(0,0), max=map2world(IntPoint(m_mapSizeX-1, m_mapSizeY-1)); - xmin=min.x, ymin=min.y, xmax=max.x, ymax=max.y; - } - - inline Cell& cell(int x, int y) { - return cell(IntPoint(x, y)); - } - inline Cell& cell(const IntPoint& p); - - inline const Cell& cell(int x, int y) const { - return cell(IntPoint(x, y)); - } - inline const Cell& cell(const IntPoint& p) const; - - inline Cell& cell(double x, double y) { - return cell(Point(x, y)); - } - inline Cell& cell(const Point& p); - - inline const Cell& cell(double x, double y) const { - return cell(Point(x, y)); - } - - inline bool isInside(int x, int y) const { - return m_storage.cellState(IntPoint(x,y))&Inside; - } - inline bool isInside(const IntPoint& p) const { - return m_storage.cellState(p)&Inside; - } - - inline bool isInside(double x, double y) const { - return m_storage.cellState(world2map(x,y))&Inside; - } - inline bool isInside(const Point& p) const { - return m_storage.cellState(world2map(p))&Inside; - } - - inline const Cell& cell(const Point& p) const; - - inline Storage& storage() { return m_storage; } - inline const Storage& storage() const { return m_storage; } - DoubleArray2D* toDoubleArray() const; - Map* toDoubleMap() const; - - protected: - Point m_center; - double m_worldSizeX, m_worldSizeY, m_delta; - Storage m_storage; - int m_mapSizeX, m_mapSizeY; - int m_sizeX2, m_sizeY2; - static const Cell m_unknown; -}; - -typedef Map DoubleMap; - -template - const Cell Map::m_unknown = Cell(-1); - -template -Map::Map(int mapSizeX, int mapSizeY, double delta): - m_storage(mapSizeX, mapSizeY){ - m_worldSizeX=mapSizeX * delta; - m_worldSizeY=mapSizeY * delta; - m_delta=delta; - m_center=Point(0.5*m_worldSizeX, 0.5*m_worldSizeY); - m_sizeX2=m_mapSizeX>>1; - m_sizeY2=m_mapSizeY>>1; -} - -template -Map::Map(const Point& center, double worldSizeX, double worldSizeY, double delta): - m_storage((int)ceil(worldSizeX/delta), (int)ceil(worldSizeY/delta)){ - m_center=center; - m_worldSizeX=worldSizeX; - m_worldSizeY=worldSizeY; - m_delta=delta; - m_mapSizeX=m_storage.getXSize()<>1; - m_sizeY2=m_mapSizeY>>1; -} - -template -Map::Map(const Point& center, double xmin, double ymin, double xmax, double ymax, double delta): - m_storage((int)ceil((xmax-xmin)/delta), (int)ceil((ymax-ymin)/delta)){ - m_center=center; - m_worldSizeX=xmax-xmin; - m_worldSizeY=ymax-ymin; - m_delta=delta; - m_mapSizeX=m_storage.getXSize()< -void Map::resize(double xmin, double ymin, double xmax, double ymax){ - IntPoint imin=world2map(xmin, ymin); - IntPoint imax=world2map(xmax, ymax); - int pxmin, pymin, pxmax, pymax; - pxmin=(int)floor((float)imin.x/(1< -void Map::grow(double xmin, double ymin, double xmax, double ymax){ - IntPoint imin=world2map(xmin, ymin); - IntPoint imax=world2map(xmax, ymax); - if (isInside(imin) && isInside(imax)) - return; - imin=min(imin, IntPoint(0,0)); - imax=max(imax, IntPoint(m_mapSizeX-1,m_mapSizeY-1)); - int pxmin, pymin, pxmax, pymax; - pxmin=(int)floor((float)imin.x/(1< -IntPoint Map::world2map(const Point& p) const{ - return IntPoint( (int)round((p.x-m_center.x)/m_delta)+m_sizeX2, (int)round((p.y-m_center.y)/m_delta)+m_sizeY2); -} - -template -Point Map::map2world(const IntPoint& p) const{ - return Point( (p.x-m_sizeX2)*m_delta, - (p.y-m_sizeY2)*m_delta)+m_center; -} - - -template -Cell& Map::cell(const IntPoint& p) { - AccessibilityState s=m_storage.cellState(p); - if (! (s&Inside)) - assert(0); - //if (s&Allocated) return m_storage.cell(p); assert(0); - - // this will never happend. Just to satify the compiler.. - return m_storage.cell(p); - -} - -template -Cell& Map::cell(const Point& p) { - IntPoint ip=world2map(p); - AccessibilityState s=m_storage.cellState(ip); - if (! (s&Inside)) - assert(0); - //if (s&Allocated) return m_storage.cell(ip); assert(0); - - // this will never happend. Just to satify the compiler.. - return m_storage.cell(ip); -} - -template - const Cell& Map::cell(const IntPoint& p) const { - AccessibilityState s=m_storage.cellState(p); - //if (! s&Inside) assert(0); - if (s&Allocated) - return m_storage.cell(p); - return m_unknown; -} - -template -const Cell& Map::cell(const Point& p) const { - IntPoint ip=world2map(p); - AccessibilityState s=m_storage.cellState(ip); - //if (! s&Inside) assert(0); - if (s&Allocated) - return m_storage.cell(ip); - return m_unknown; -} - - -//FIXME check why the last line of the map is corrupted. -template -DoubleArray2D* Map::toDoubleArray() const{ - DoubleArray2D* darr=new DoubleArray2D(getMapSizeX()-1, getMapSizeY()-1); - for(int x=0; xcell(p)=cell(p); - } - return darr; -} - - -template -Map* Map::toDoubleMap() const{ -//FIXME size the map so that m_center will be setted accordingly - Point pmin=map2world(IntPoint(0,0)); - Point pmax=map2world(getMapSizeX()-1,getMapSizeY()-1); - Point center=(pmax+pmin)*0.5; - Map* plainMap=new Map(center, (pmax-pmin).x, (pmax-pmin).y, getDelta()); - for(int x=0; xcell(p)=cell(p); - } - return plainMap; -} - -}; - -#endif - diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.h b/slam_gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.h deleted file mode 100644 index 9a859fb..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.h +++ /dev/null @@ -1,337 +0,0 @@ -#ifndef GRIDSLAMPROCESSOR_H -#define GRIDSLAMPROCESSOR_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "motionmodel.h" - - -namespace GMapping { - - /**This class defines the basic GridFastSLAM algorithm. It - implements a rao blackwellized particle filter. Each particle - has its own map and robot pose.
This implementation works - as follows: each time a new pair odometry/laser reading is - received, the particle's robot pose is updated according to the - motion model. This pose is subsequently used for initalizing a - scan matching algorithm. The scanmatcher performs a local - optimization for each particle. It is initialized with the - pose drawn from the motion model, and the pose is corrected - according to the each particle map.
- In order to avoid unnecessary computation the filter state is updated - only when the robot moves more than a given threshold. - */ - class GridSlamProcessor{ - public: - - - /**This class defines the the node of reversed tree in which the trajectories are stored. - Each node of a tree has a pointer to its parent and a counter indicating the number of childs of a node. - The tree is updated in a way consistent with the operation performed on the particles. - */ - struct TNode{ - /**Constructs a node of the trajectory tree. - @param pose: the pose of the robot in the trajectory - @param weight: the weight of the particle at that point in the trajectory - @param accWeight: the cumulative weight of the particle - @param parent: the parent node in the tree - @param childs: the number of childs - */ - TNode(const OrientedPoint& pose, double weight, TNode* parent=0, unsigned int childs=0); - - /**Destroys a tree node, and consistently updates the tree. If a node whose parent has only one child is deleted, - also the parent node is deleted. This because the parent will not be reacheable anymore in the trajectory tree.*/ - ~TNode(); - - /**The pose of the robot*/ - OrientedPoint pose; - - /**The weight of the particle*/ - double weight; - - /**The sum of all the particle weights in the previous part of the trajectory*/ - double accWeight; - - double gweight; - - - /**The parent*/ - TNode* parent; - - /**The range reading to which this node is associated*/ - const RangeReading* reading; - - /**The number of childs*/ - unsigned int childs; - - /**counter in visiting the node (internally used)*/ - mutable unsigned int visitCounter; - - /**visit flag (internally used)*/ - mutable bool flag; - }; - - typedef std::vector TNodeVector; - typedef std::deque TNodeDeque; - - /**This class defines a particle of the filter. Each particle has a map, a pose, a weight and retains the current node in the trajectory tree*/ - struct Particle{ - /**constructs a particle, given a map - @param map: the particle map - */ - Particle(const ScanMatcherMap& map); - - /** @returns the weight of a particle */ - inline operator double() const {return weight;} - /** @returns the pose of a particle */ - inline operator OrientedPoint() const {return pose;} - /** sets the weight of a particle - @param w the weight - */ - inline void setWeight(double w) {weight=w;} - /** The map */ - ScanMatcherMap map; - /** The pose of the robot */ - OrientedPoint pose; - - /** The pose of the robot at the previous time frame (used for computing thr odometry displacements) */ - OrientedPoint previousPose; - - /** The weight of the particle */ - double weight; - - /** The cumulative weight of the particle */ - double weightSum; - - double gweight; - - /** The index of the previous particle in the trajectory tree */ - int previousIndex; - - /** Entry to the trajectory tree */ - TNode* node; - }; - - - typedef std::vector ParticleVector; - - /** Constructs a GridSlamProcessor, initialized with the default parameters */ - GridSlamProcessor(); - - /** Constructs a GridSlamProcessor, whose output is routed to a stream. - @param infoStr: the output stream - */ - GridSlamProcessor(std::ostream& infoStr); - - /** @returns a deep copy of the grid slam processor with all the internal structures. - */ - GridSlamProcessor* clone() const; - - /**Deleted the gridslamprocessor*/ - virtual ~GridSlamProcessor(); - - //methods for accessing the parameters - void setSensorMap(const SensorMap& smap); - void init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, - OrientedPoint initialPose=OrientedPoint(0,0,0)); - void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt, - int iterations, double likelihoodSigma=1, double likelihoodGain=1, unsigned int likelihoodSkip=0); - void setMotionModelParameters(double srr, double srt, double str, double stt); - void setUpdateDistances(double linear, double angular, double resampleThreshold); - void setUpdatePeriod(double p) {period_=p;} - - //the "core" algorithm - void processTruePos(const OdometryReading& odometry); - bool processScan(const RangeReading & reading, int adaptParticles=0); - - /**This method copies the state of the filter in a tree. - The tree is represented through reversed pointers (each node has a pointer to its parent). - The leafs are stored in a vector, whose size is the same as the number of particles. - @returns the leafs of the tree - */ - TNodeVector getTrajectories() const; - void integrateScanSequence(TNode* node); - - /**the scanmatcher algorithm*/ - ScanMatcher m_matcher; - /**the stream used for writing the output of the algorithm*/ - std::ofstream& outputStream(); - /**the stream used for writing the info/debug messages*/ - std::ostream& infoStream(); - /**@returns the particles*/ - inline const ParticleVector& getParticles() const {return m_particles; } - - inline const std::vector& getIndexes() const{return m_indexes; } - int getBestParticleIndex() const; - //callbacks - virtual void onOdometryUpdate(); - virtual void onResampleUpdate(); - virtual void onScanmatchUpdate(); - - //accessor methods - /**the maxrange of the laser to consider */ - MEMBER_PARAM_SET_GET(m_matcher, double, laserMaxRange, protected, public, public); - - /**the maximum usable range of the laser. A beam is cropped to this value. [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, double, usableRange, protected, public, public); - - /**The sigma used by the greedy endpoint matching. [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher,double, gaussianSigma, protected, public, public); - - /**The sigma of a beam used for likelihood computation [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher,double, likelihoodSigma, protected, public, public); - - /**The kernel in which to look for a correspondence[scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, int, kernelSize, protected, public, public); - - /**The optimization step in rotation [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, double, optAngularDelta, protected, public, public); - - /**The optimization step in translation [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, double, optLinearDelta, protected, public, public); - - /**The number of iterations of the scanmatcher [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, unsigned int, optRecursiveIterations, protected, public, public); - - /**the beams to skip for computing the likelihood (consider a beam every likelihoodSkip) [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, unsigned int, likelihoodSkip, protected, public, public); - - /**translational sampling range for the likelihood [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, double, llsamplerange, protected, public, public); - - /**angular sampling range for the likelihood [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, double, lasamplerange, protected, public, public); - - /**translational sampling range for the likelihood [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, double, llsamplestep, protected, public, public); - - /**angular sampling step for the likelihood [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, double, lasamplestep, protected, public, public); - - /**generate an accupancy grid map [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, bool, generateMap, protected, public, public); - - /**enlarge the map when the robot goes out of the boundaries [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, bool, enlargeStep, protected, public, public); - - /**pose of the laser wrt the robot [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, OrientedPoint, laserPose, protected, public, public); - - - /**odometry error in translation as a function of translation (rho/rho) [motionmodel]*/ - STRUCT_PARAM_SET_GET(m_motionModel, double, srr, protected, public, public); - - /**odometry error in translation as a function of rotation (rho/theta) [motionmodel]*/ - STRUCT_PARAM_SET_GET(m_motionModel, double, srt, protected, public, public); - - /**odometry error in rotation as a function of translation (theta/rho) [motionmodel]*/ - STRUCT_PARAM_SET_GET(m_motionModel, double, str, protected, public, public); - - /**odometry error in rotation as a function of rotation (theta/theta) [motionmodel]*/ - STRUCT_PARAM_SET_GET(m_motionModel, double, stt, protected, public, public); - - /**minimum score for considering the outcome of the scanmatching good*/ - PARAM_SET_GET(double, minimumScore, protected, public, public); - - protected: - /**Copy constructor*/ - GridSlamProcessor(const GridSlamProcessor& gsp); - - /**the laser beams*/ - unsigned int m_beams; - double last_update_time_; - double period_; - - /**the particles*/ - ParticleVector m_particles; - - /**the particle indexes after resampling (internally used)*/ - std::vector m_indexes; - - /**the particle weights (internally used)*/ - std::vector m_weights; - - /**the motion model*/ - MotionModel m_motionModel; - - /**this sets the neff based resampling threshold*/ - PARAM_SET_GET(double, resampleThreshold, protected, public, public); - - //state - int m_count, m_readingCount; - OrientedPoint m_lastPartPose; - OrientedPoint m_odoPose; - OrientedPoint m_pose; - double m_linearDistance, m_angularDistance; - PARAM_GET(double, neff, protected, public); - - //processing parameters (size of the map) - PARAM_GET(double, xmin, protected, public); - PARAM_GET(double, ymin, protected, public); - PARAM_GET(double, xmax, protected, public); - PARAM_GET(double, ymax, protected, public); - //processing parameters (resolution of the map) - PARAM_GET(double, delta, protected, public); - - //registration score (if a scan score is above this threshold it is registered in the map) - PARAM_SET_GET(double, regScore, protected, public, public); - //registration score (if a scan score is below this threshold a scan matching failure is reported) - PARAM_SET_GET(double, critScore, protected, public, public); - //registration score maximum move allowed between consecutive scans - PARAM_SET_GET(double, maxMove, protected, public, public); - - //process a scan each time the robot translates of linearThresholdDistance - PARAM_SET_GET(double, linearThresholdDistance, protected, public, public); - - //process a scan each time the robot rotates more than angularThresholdDistance - PARAM_SET_GET(double, angularThresholdDistance, protected, public, public); - - //smoothing factor for the likelihood - PARAM_SET_GET(double, obsSigmaGain, protected, public, public); - - //stream in which to write the gfs file - std::ofstream m_outputStream; - - // stream in which to write the messages - std::ostream& m_infoStream; - - - // the functions below performs side effect on the internal structure, - //should be called only inside the processScan method - private: - - /**scanmatches all the particles*/ - inline void scanMatch(const double *plainReading); - /**normalizes the particle weights*/ - inline void normalize(); - - // return if a resampling occured or not - inline bool resample(const double* plainReading, int adaptParticles, - const RangeReading* rr=0); - - //tree utilities - - void updateTreeWeights(bool weightsAlreadyNormalized = false); - void resetTree(); - double propagateWeights(); - - }; - -typedef std::multimap TNodeMultimap; - - -#include "gridslamprocessor.hxx" - -}; - -#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.hxx b/slam_gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.hxx deleted file mode 100644 index 8a8b7a4..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.hxx +++ /dev/null @@ -1,177 +0,0 @@ - -#ifdef MACOSX -// This is to overcome a possible bug in Apple's GCC. -#define isnan(x) (x==FP_NAN) -#endif - -/**Just scan match every single particle. -If the scan matching fails, the particle gets a default likelihood.*/ -inline void GridSlamProcessor::scanMatch(const double* plainReading){ - // sample a new pose from each scan in the reference - - double sumScore=0; - for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - OrientedPoint corrected; - double score, l, s; - score=m_matcher.optimize(corrected, it->map, it->pose, plainReading); - // it->pose=corrected; - if (score>m_minimumScore){ - it->pose=corrected; - } else { - if (m_infoStream){ - m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l <map, it->pose, plainReading); - sumScore+=score; - it->weight+=l; - it->weightSum+=l; - - //set up the selective copy of the active area - //by detaching the areas that will be updated - m_matcher.invalidateActiveArea(); - m_matcher.computeActiveArea(it->map, it->pose, plainReading); - } - if (m_infoStream) - m_infoStream << "Average Scan Matching Score=" << sumScore/m_particles.size() << std::endl; -} - -inline void GridSlamProcessor::normalize(){ - //normalize the log m_weights - double gain=1./(m_obsSigmaGain*m_particles.size()); - double lmax= -std::numeric_limits::max(); - for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - lmax=it->weight>lmax?it->weight:lmax; - } - //cout << "!!!!!!!!!!! maxwaight= "<< lmax << endl; - - m_weights.clear(); - double wcum=0; - m_neff=0; - for (std::vector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - m_weights.push_back(exp(gain*(it->weight-lmax))); - wcum+=m_weights.back(); - //cout << "l=" << it->weight<< endl; - } - - m_neff=0; - for (std::vector::iterator it=m_weights.begin(); it!=m_weights.end(); it++){ - *it=*it/wcum; - double w=*it; - m_neff+=w*w; - } - m_neff=1./m_neff; - -} - -inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* reading){ - - bool hasResampled = false; - - TNodeVector oldGeneration; - for (unsigned int i=0; i resampler; - m_indexes=resampler.resampleIndexes(m_weights, adaptSize); - - if (m_outputStream.is_open()){ - m_outputStream << "RESAMPLE "<< m_indexes.size() << " "; - for (std::vector::const_iterator it=m_indexes.begin(); it!=m_indexes.end(); it++){ - m_outputStream << *it << " "; - } - m_outputStream << std::endl; - } - - onResampleUpdate(); - //BEGIN: BUILDING TREE - ParticleVector temp; - unsigned int j=0; - std::vector deletedParticles; //this is for deleteing the particles which have been resampled away. - - // cerr << "Existing Nodes:" ; - for (unsigned int i=0; i" << m_indexes[i] << "B("<childs <<") "; - node=new TNode(p.pose, 0, oldNode, 0); - //node->reading=0; - node->reading=reading; - // cerr << "A("<parent->childs <<") " <setWeight(0); - m_matcher.invalidateActiveArea(); - m_matcher.registerScan(it->map, it->pose, plainReading); - m_particles.push_back(*it); - } - std::cerr << " Done" <pose, 0.0, *node_it, 0); - - //node->reading=0; - node->reading=reading; - it->node=node; - - //END: BUILDING TREE - m_matcher.invalidateActiveArea(); - m_matcher.registerScan(it->map, it->pose, plainReading); - it->previousIndex=index; - index++; - node_it++; - - } - std::cerr << "Done" < -#include -#include - -namespace GMapping { - -struct MotionModel{ - OrientedPoint drawFromMotion(const OrientedPoint& p, double linearMove, double angularMove) const; - OrientedPoint drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const; - Covariance3 gaussianApproximation(const OrientedPoint& pnew, const OrientedPoint& pold) const; - double srr, str, srt, stt; -}; - -}; - -#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/log/configuration.h b/slam_gmapping/openslam_gmapping/include/gmapping/log/configuration.h deleted file mode 100644 index ce1c81c..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/log/configuration.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef CONFIGURATION_H -#define CONFIGURATION_H - -#include -#include - -namespace GMapping { - -class Configuration{ - public: - virtual ~Configuration(); - virtual SensorMap computeSensorMap() const=0; -}; - -}; -#endif - diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/log/sensorlog.h b/slam_gmapping/openslam_gmapping/include/gmapping/log/sensorlog.h deleted file mode 100644 index f45ddda..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/log/sensorlog.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef SENSORLOG_H -#define SENSORLOG_H - -#include -#include -#include -#include -#include -#include -#include -#include "configuration.h" - -namespace GMapping { - -class SensorLog : public std::list{ - public: - SensorLog(const SensorMap&); - ~SensorLog(); - std::istream& load(std::istream& is); - OrientedPoint boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const; - protected: - const SensorMap& m_sensorMap; - OdometryReading* parseOdometry(std::istream& is, const OdometrySensor* ) const; - RangeReading* parseRange(std::istream& is, const RangeSensor* ) const; -}; - -}; - -#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/particlefilter/particlefilter.h b/slam_gmapping/openslam_gmapping/include/gmapping/particlefilter/particlefilter.h deleted file mode 100644 index 2a8b8a9..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/particlefilter/particlefilter.h +++ /dev/null @@ -1,328 +0,0 @@ -#ifndef PARTICLEFILTER_H -#define PARTICLEFILTER_H -#include -#include -#include -#include -#include -#include -#include - - -/** -the particle class has to be convertible into numeric data type; -That means that a particle must define the Numeric conversion operator; - operator Numeric() const. -that returns the weight, and the method - setWeight(Numeric) -that sets the weight. - -*/ - -typedef std::pair UIntPair; - -template -double toNormalForm(OutputIterator& out, const Iterator & begin, const Iterator & end){ - //determine the maximum - double lmax = -std::numeric_limits::max(); - for (Iterator it=begin; it!=end; it++){ - lmax=lmax>((double)(*it))? lmax: (double)(*it); - } - //convert to raw form - for (Iterator it=begin; it!=end; it++){ - *out=exp((double)(*it)-lmax); - out++; - } - return lmax; -} - -template -void toLogForm(OutputIterator& out, const Iterator & begin, const Iterator & end, Numeric lmax){ - //determine the maximum - for (Iterator it=begin; it!=end; it++){ - *out=log((Numeric)(*it))-lmax; - out++; - } - return lmax; -} - -template -void resample(std::vector& indexes, const WeightVector& weights, unsigned int nparticles=0){ - double cweight=0; - - //compute the cumulative weights - unsigned int n=0; - for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it){ - cweight+=(double)*it; - n++; - } - - if (nparticles>0) - n=nparticles; - - //compute the interval - double interval=cweight/n; - - //compute the initial target weight - double target=interval*::drand48(); - //compute the resampled indexes - - cweight=0; - indexes.resize(n); - - n=0; - unsigned int i=0; - for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){ - cweight+=(double)* it; - while(cweight>target){ - indexes[n++]=i; - target+=interval; - } - } -} - -template -void repeatIndexes(Vector& dest, const std::vector& indexes, const Vector& particles){ - assert(indexes.size()==particles.size()); - dest.resize(particles.size()); - unsigned int i=0; - for (std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){ - dest[i]=particles[*it]; - i++; - } -} - - -template -double neff(const Iterator& begin, const Iterator& end){ - double sum=0; - for (Iterator it=begin; it!=end; ++it){ - sum+=*it; - } - double cum=0; - for (Iterator it=begin; it!=end; ++it){ - double w=*it/sum; - cum+=w*w; - } - return 1./cum; -} - -template -void normalize(const Iterator& begin, const Iterator& end){ - double sum=0; - for (Iterator it=begin; it!=end; ++it){ - sum+=*it; - } - for (Iterator it=begin; it!=end; ++it){ - *it=*it/sum; - } -} - -template -void rle(OutputIterator& out, const Iterator & begin, const Iterator & end){ - unsigned int current=0; - unsigned int count=0; - for (Iterator it=begin; it!=end; it++){ - if (it==begin){ - current=*it; - count=1; - continue; - } - if (((uint)*it) ==current) - count++; - if (((uint)*it)!=current){ - *out=std::make_pair(current,count); - out++; - current=*it; - count=1; - } - } - if (count>0) - *out=std::make_pair(current,count); - out++; -} - -//BEGIN legacy -template -struct uniform_resampler{ - std::vector resampleIndexes(const std::vector & particles, int nparticles=0) const; - std::vector resample(const std::vector & particles, int nparticles=0) const; - Numeric neff(const std::vector & particles) const; -}; - -/*Implementation of the above stuff*/ -template -std::vector uniform_resampler:: resampleIndexes(const std::vector& particles, int nparticles) const{ - Numeric cweight=0; - - //compute the cumulative weights - unsigned int n=0; - for (typename std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it){ - cweight+=(Numeric)*it; - n++; - } - - if (nparticles>0) - n=nparticles; - - //compute the interval - Numeric interval=cweight/n; - - //compute the initial target weight - Numeric target=interval*::drand48(); - //compute the resampled indexes - - cweight=0; - std::vector indexes(n); - n=0; - unsigned int i=0; - for (typename std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){ - cweight+=(Numeric)* it; - while(cweight>target){ - indexes[n++]=i; - target+=interval; - } - } - return indexes; -} - -template -std::vector uniform_resampler::resample - (const typename std::vector& particles, int nparticles) const{ - Numeric cweight=0; - - //compute the cumulative weights - unsigned int n=0; - for (typename std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it){ - cweight+=(Numeric)*it; - n++; - } - - if (nparticles>0) - n=nparticles; - - //weight of the particles after resampling - double uw=1./n; - - //compute the interval - Numeric interval=cweight/n; - - //compute the initial target weight - Numeric target=interval*::drand48(); - //compute the resampled indexes - - cweight=0; - std::vector resampled; - n=0; - unsigned int i=0; - for (typename std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){ - cweight+=(Numeric)*it; - while(cweight>target){ - resampled.push_back(*it); - resampled.back().setWeight(uw); - target+=interval; - } - } - return resampled; -} - -template -Numeric uniform_resampler::neff(const std::vector & particles) const{ - double cum=0; - double sum=0; - for (typename std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it){ - Numeric w=(Numeric)*it; - cum+=w*w; - sum+=w; - } - return sum*sum/cum; -} - - -/* - -The following are patterns for the evolution and the observation classes -The user should implement classes having the specified meaning - -template -struct observer{ - Observation& observation - Numeric observe(const class State&) const; -}; - -template -struct evolver{ - Input& input; - State& evolve(const State& s); -}; -*/ - - -template -struct evolver{ - EvolutionModel evolutionModel; - void evolve(std::vector& particles); - void evolve(std::vector& dest, const std::vector& src); -}; - -template -void evolver::evolve(std::vector& particles){ - for (typename std::vector::iterator it=particles.begin(); it!=particles.end(); ++it){ - *it=evolutionModel.evolve(*it); - } -} - -template -void evolver::evolve(std::vector& dest, const std::vector& src){ - dest.clear(); - for (typename std::vector::const_iterator it=src.begin(); it!=src.end(); ++it) - dest.push_back(evolutionModel.evolve(*it)); -} - - -template -struct auxiliary_evolver{ - EvolutionModel evolutionModel; - QualificationModel qualificationModel; - LikelyhoodModel likelyhoodModel; - void evolve(std::vector& particles); - void evolve(std::vector& dest, const std::vector& src); -}; - -template -void auxiliary_evolver::evolve - (std::vector&particles){ - std::vector observationWeights(particles.size()); - unsigned int i=0; - for (typename std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it, i++){ - observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it)); - } - uniform_resampler resampler; - std::vector indexes(resampler.resampleIndexes(observationWeights)); - for (typename std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){ - Particle & particle=particles[*it]; - particle=evolutionModel.evolve(particle); - particle.setWeight(likelyhoodModel.likelyhood(particle)/observationWeights[*it]); - } -} - -template -void auxiliary_evolver::evolve - (std::vector& dest, const std::vector& src){ - dest.clear(); - std::vector observationWeights(src.size()); - unsigned int i=0; - for (typename std::vector::const_iterator it=src.begin(); it!=src.end(); ++it, i++){ - observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it)); - } - uniform_resampler resampler; - std::vector indexes(resampler.resampleIndexes(observationWeights)); - for (typename std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){ - Particle & particle=src[*it]; - dest.push_back(evolutionModel.evolve(particle)); - dest.back().weight*=likelyhoodModel.likelyhood(particle)/observationWeights[*it]; - } -} -//END legacy - -#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/scanmatcher/icp.h b/slam_gmapping/openslam_gmapping/include/gmapping/scanmatcher/icp.h deleted file mode 100644 index 64c257c..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/scanmatcher/icp.h +++ /dev/null @@ -1,85 +0,0 @@ -#ifndef _ICP_H_ -#define _ICP_H_ - -#include -#include -#include -#include - -namespace GMapping{ -typedef std::pair PointPair; - -template -double icpStep(OrientedPoint & retval, const PointPairContainer& container){ - typedef typename PointPairContainer::const_iterator ContainerIterator; - PointPair mean=std::make_pair(Point(0.,0.), Point(0.,0.)); - int size=0; - for (ContainerIterator it=container.begin(); it!=container.end(); it++){ - mean.first=mean.first+it->first; - mean.second=mean.second+it->second; - size++; - } - mean.first=mean.first*(1./size); - mean.second=mean.second*(1./size); - double sxx=0, sxy=0, syx=0, syy=0; - - for (ContainerIterator it=container.begin(); it!=container.end(); it++){ - PointPair mf=std::make_pair(it->first-mean.first, it->second-mean.second); - sxx+=mf.first.x*mf.second.x; - sxy+=mf.first.x*mf.second.y; - syx+=mf.first.y*mf.second.x; - syy+=mf.first.y*mf.second.y; - } - retval.theta=atan2(sxy-syx, sxx+sxy); - double s=sin(retval.theta), c=cos(retval.theta); - retval.x=mean.second.x-(c*mean.first.x-s*mean.first.y); - retval.y=mean.second.y-(s*mean.first.x+c*mean.first.y); - - double error=0; - for (ContainerIterator it=container.begin(); it!=container.end(); it++){ - Point delta( - c*it->first.x-s*it->first.y+retval.x-it->second.x, s*it->first.x+c*it->first.y+retval.y-it->second.y); - error+=delta*delta; - } - return error; -} - -template -double icpNonlinearStep(OrientedPoint & retval, const PointPairContainer& container){ - typedef typename PointPairContainer::const_iterator ContainerIterator; - PointPair mean=std::make_pair(Point(0.,0.), Point(0.,0.)); - int size=0; - for (ContainerIterator it=container.begin(); it!=container.end(); it++){ - mean.first=mean.first+it->first; - mean.second=mean.second+it->second; - size++; - } - - mean.first=mean.first*(1./size); - mean.second=mean.second*(1./size); - - double ms=0,mc=0; - for (ContainerIterator it=container.begin(); it!=container.end(); it++){ - PointPair mf=std::make_pair(it->first-mean.first, it->second-mean.second); - double dalpha=atan2(mf.second.y, mf.second.x) - atan2(mf.first.y, mf.first.x); - double gain=sqrt(mean.first*mean.first); - ms+=gain*sin(dalpha); - mc+=gain*cos(dalpha); - } - retval.theta=atan2(ms, mc); - double s=sin(retval.theta), c=cos(retval.theta); - retval.x=mean.second.x-(c*mean.first.x-s*mean.first.y); - retval.y=mean.second.y-(s*mean.first.x+c*mean.first.y); - - double error=0; - for (ContainerIterator it=container.begin(); it!=container.end(); it++){ - Point delta( - c*it->first.x-s*it->first.y+retval.x-it->second.x, s*it->first.x+c*it->first.y+retval.y-it->second.y); - error+=delta*delta; - } - return error; -} - -}//end namespace - -#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/scanmatcher/scanmatcher.h b/slam_gmapping/openslam_gmapping/include/gmapping/scanmatcher/scanmatcher.h deleted file mode 100644 index 1f0a4d4..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/scanmatcher/scanmatcher.h +++ /dev/null @@ -1,252 +0,0 @@ -#ifndef SCANMATCHER_H -#define SCANMATCHER_H - -#include "icp.h" -#include "smmap.h" -#include -#include -#include -#include -#define LASER_MAXBEAMS 2048 - -namespace GMapping { - -class ScanMatcher{ - public: - typedef Covariance3 CovarianceMatrix; - - ScanMatcher(); - ~ScanMatcher(); - double icpOptimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const; - double optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const; - double optimize(OrientedPoint& mean, CovarianceMatrix& cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const; - - double registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings); - void setLaserParameters - (unsigned int beams, double* angles, const OrientedPoint& lpose); - void setMatchingParameters - (double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma=1, unsigned int likelihoodSkip=0 ); - void invalidateActiveArea(); - void computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings); - - inline double icpStep(OrientedPoint & pret, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const; - inline double score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const; - inline unsigned int likelihoodAndScore(double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const; - double likelihood(double& lmax, OrientedPoint& mean, CovarianceMatrix& cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings); - double likelihood(double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p, Gaussian3& odometry, const double* readings, double gain=180.); - inline const double* laserAngles() const { return m_laserAngles; } - inline unsigned int laserBeams() const { return m_laserBeams; } - - static const double nullLikelihood; - protected: - //state of the matcher - bool m_activeAreaComputed; - - /**laser parameters*/ - unsigned int m_laserBeams; - double m_laserAngles[LASER_MAXBEAMS]; - //OrientedPoint m_laserPose; - PARAM_SET_GET(OrientedPoint, laserPose, protected, public, public) - PARAM_SET_GET(double, laserMaxRange, protected, public, public) - /**scan_matcher parameters*/ - PARAM_SET_GET(double, usableRange, protected, public, public) - PARAM_SET_GET(double, gaussianSigma, protected, public, public) - PARAM_SET_GET(double, likelihoodSigma, protected, public, public) - PARAM_SET_GET(int, kernelSize, protected, public, public) - PARAM_SET_GET(double, optAngularDelta, protected, public, public) - PARAM_SET_GET(double, optLinearDelta, protected, public, public) - PARAM_SET_GET(unsigned int, optRecursiveIterations, protected, public, public) - PARAM_SET_GET(unsigned int, likelihoodSkip, protected, public, public) - PARAM_SET_GET(double, llsamplerange, protected, public, public) - PARAM_SET_GET(double, llsamplestep, protected, public, public) - PARAM_SET_GET(double, lasamplerange, protected, public, public) - PARAM_SET_GET(double, lasamplestep, protected, public, public) - PARAM_SET_GET(bool, generateMap, protected, public, public) - PARAM_SET_GET(double, enlargeStep, protected, public, public) - PARAM_SET_GET(double, fullnessThreshold, protected, public, public) - PARAM_SET_GET(double, angularOdometryReliability, protected, public, public) - PARAM_SET_GET(double, linearOdometryReliability, protected, public, public) - PARAM_SET_GET(double, freeCellRatio, protected, public, public) - PARAM_SET_GET(unsigned int, initialBeamsSkip, protected, public, public) - - // allocate this large array only once - IntPoint* m_linePoints; -}; - -inline double ScanMatcher::icpStep(OrientedPoint & pret, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{ - const double * angle=m_laserAngles+m_initialBeamsSkip; - OrientedPoint lp=p; - lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y; - lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y; - lp.theta+=m_laserPose.theta; - unsigned int skip=0; - double freeDelta=map.getDelta()*m_freeCellRatio; - std::list pairs; - - for (const double* r=readings+m_initialBeamsSkip; rm_likelihoodSkip?0:skip; - if (*r>m_usableRange||*r==0.0) continue; - if (skip) continue; - Point phit=lp; - phit.x+=*r*cos(lp.theta+*angle); - phit.y+=*r*sin(lp.theta+*angle); - IntPoint iphit=map.world2map(phit); - Point pfree=lp; - pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle); - pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle); - pfree=pfree-phit; - IntPoint ipfree=map.world2map(pfree); - bool found=false; - Point bestMu(0.,0.); - Point bestCell(0.,0.); - for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++) - for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){ - IntPoint pr=iphit+IntPoint(xx,yy); - IntPoint pf=pr+ipfree; - //AccessibilityState s=map.storage().cellState(pr); - //if (s&Inside && s&Allocated){ - const PointAccumulator& cell=map.cell(pr); - const PointAccumulator& fcell=map.cell(pf); - if (((double)cell )> m_fullnessThreshold && ((double)fcell )m_likelihoodSkip?0:skip; - if (skip||*r>m_usableRange||*r==0.0) continue; - Point phit=lp; - phit.x+=*r*cos(lp.theta+*angle); - phit.y+=*r*sin(lp.theta+*angle); - IntPoint iphit=map.world2map(phit); - Point pfree=lp; - pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle); - pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle); - pfree=pfree-phit; - IntPoint ipfree=map.world2map(pfree); - bool found=false; - Point bestMu(0.,0.); - for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++) - for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){ - IntPoint pr=iphit+IntPoint(xx,yy); - IntPoint pf=pr+ipfree; - //AccessibilityState s=map.storage().cellState(pr); - //if (s&Inside && s&Allocated){ - const PointAccumulator& cell=map.cell(pr); - const PointAccumulator& fcell=map.cell(pf); - if (((double)cell )> m_fullnessThreshold && ((double)fcell )m_likelihoodSkip?0:skip; - if (*r>m_usableRange) continue; - if (skip) continue; - Point phit=lp; - phit.x+=*r*cos(lp.theta+*angle); - phit.y+=*r*sin(lp.theta+*angle); - IntPoint iphit=map.world2map(phit); - Point pfree=lp; - pfree.x+=(*r-freeDelta)*cos(lp.theta+*angle); - pfree.y+=(*r-freeDelta)*sin(lp.theta+*angle); - pfree=pfree-phit; - IntPoint ipfree=map.world2map(pfree); - bool found=false; - Point bestMu(0.,0.); - for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++) - for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){ - IntPoint pr=iphit+IntPoint(xx,yy); - IntPoint pf=pr+ipfree; - //AccessibilityState s=map.storage().cellState(pr); - //if (s&Inside && s&Allocated){ - const PointAccumulator& cell=map.cell(pr); - const PointAccumulator& fcell=map.cell(pf); - if (((double)cell )>m_fullnessThreshold && ((double)fcell ) -#include -#include -#define SIGHT_INC 1 - -namespace GMapping { - -struct PointAccumulator{ - typedef point FloatPoint; - /* before - PointAccumulator(int i=-1): acc(0,0), n(0), visits(0){assert(i==-1);} - */ - /*after begin*/ - PointAccumulator(): acc(0,0), n(0), visits(0){} - PointAccumulator(int i): acc(0,0), n(0), visits(0){assert(i==-1);} - /*after end*/ - inline void update(bool value, const Point& p=Point(0,0)); - inline Point mean() const {return 1./n*Point(acc.x, acc.y);} - inline operator double() const { return visits?(double)n*SIGHT_INC/(double)visits:-1; } - inline void add(const PointAccumulator& p) {acc=acc+p.acc; n+=p.n; visits+=p.visits; } - static const PointAccumulator& Unknown(); - static PointAccumulator* unknown_ptr; - FloatPoint acc; - int n, visits; - inline double entropy() const; -}; - -void PointAccumulator::update(bool value, const Point& p){ - if (value) { - acc.x+= static_cast(p.x); - acc.y+= static_cast(p.y); - n++; - visits+=SIGHT_INC; - } else - visits++; -} - -double PointAccumulator::entropy() const{ - if (!visits) - return -log(.5); - if (n==visits || n==0) - return 0; - double x=(double)n*SIGHT_INC/(double)visits; - return -( x*log(x)+ (1-x)*log(1-x) ); -} - - -typedef Map > ScanMatcherMap; - -}; - -#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_base/sensor.h b/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_base/sensor.h deleted file mode 100644 index 4368809..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_base/sensor.h +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef SENSOR_H -#define SENSOR_H - -#include -#include - -namespace GMapping{ - -class Sensor{ - public: - Sensor(const std::string& name=""); - virtual ~Sensor(); - inline std::string getName() const {return m_name;} - inline void setName(const std::string& name) {m_name=name;} - protected: - std::string m_name; -}; - -typedef std::map SensorMap; - -}; //end namespace - -#endif - diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_base/sensorreading.h b/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_base/sensorreading.h deleted file mode 100644 index 66fa537..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_base/sensorreading.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef SENSORREADING_H -#define SENSORREADING_H - -#include "sensor.h" -namespace GMapping{ - -class SensorReading{ - public: - SensorReading(const Sensor* s, double time){ - m_sensor=s; - m_time=time; - }; - ~SensorReading(){}; - inline double getTime() const {return m_time;} - inline void setTime(double t) {m_time=t;} - inline const Sensor* getSensor() const {return m_sensor;} - protected: - double m_time; - const Sensor* m_sensor; - -}; - -}; //end namespace -#endif - - diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometryreading.h b/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometryreading.h deleted file mode 100644 index 1a6456c..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometryreading.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef ODOMETRYREADING_H -#define ODOMETRYREADING_H - -#include -#include -#include -#include "odometrysensor.h" - -namespace GMapping{ - -class OdometryReading: public SensorReading{ - public: - OdometryReading(const OdometrySensor* odo, double time=0); - inline const OrientedPoint& getPose() const {return m_pose;} - inline const OrientedPoint& getSpeed() const {return m_speed;} - inline const OrientedPoint& getAcceleration() const {return m_acceleration;} - inline void setPose(const OrientedPoint& pose) {m_pose=pose;} - inline void setSpeed(const OrientedPoint& speed) {m_speed=speed;} - inline void setAcceleration(const OrientedPoint& acceleration) {m_acceleration=acceleration;} - - protected: - OrientedPoint m_pose; - OrientedPoint m_speed; - OrientedPoint m_acceleration; -}; - -}; -#endif - diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometrysensor.h b/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometrysensor.h deleted file mode 100644 index 1d18bd3..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometrysensor.h +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef ODOMETRYSENSOR_H -#define ODOMETRYSENSOR_H - -#include -#include - -namespace GMapping{ - -class OdometrySensor: public Sensor{ - public: - OdometrySensor(const std::string& name, bool ideal=false); - inline bool isIdeal() const { return m_ideal; } - protected: - bool m_ideal; -}; - -}; - -#endif - diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_range/rangereading.h b/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_range/rangereading.h deleted file mode 100644 index 1582c67..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_range/rangereading.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef RANGEREADING_H -#define RANGEREADING_H - -#include -#include -#include - -namespace GMapping{ - -class RangeReading: public SensorReading, public std::vector{ - public: - RangeReading(const RangeSensor* rs, double time=0); - RangeReading(unsigned int n_beams, const double* d, const RangeSensor* rs, double time=0); - virtual ~RangeReading(); - inline const OrientedPoint& getPose() const {return m_pose;} - inline void setPose(const OrientedPoint& pose) {m_pose=pose;} - unsigned int rawView(double* v, double density=0.) const; - std::vector cartesianForm(double maxRange=1e6) const; - unsigned int activeBeams(double density=0.) const; - protected: - OrientedPoint m_pose; -}; - -}; - -#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_range/rangesensor.h b/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_range/rangesensor.h deleted file mode 100644 index 65feffb..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_range/rangesensor.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef RANGESENSOR_H -#define RANGESENSOR_H - -#include -#include -#include - -namespace GMapping{ - -class RangeSensor: public Sensor{ - friend class Configuration; - friend class CarmenConfiguration; - friend class CarmenWrapper; - public: - struct Beam{ - OrientedPoint pose; //pose relative to the center of the sensor - double span; //spam=0 indicates a line-like beam - double maxRange; //maximum range of the sensor - double s,c; //sinus and cosinus of the beam (optimization); - }; - RangeSensor(std::string name); - RangeSensor(std::string name, unsigned int beams, double res, const OrientedPoint& position=OrientedPoint(0,0,0), double span=0, double maxrange=89.0); - inline const std::vector& beams() const {return m_beams;} - inline std::vector& beams() {return m_beams;} - inline OrientedPoint getPose() const {return m_pose;} - void updateBeamsLookup(); - bool newFormat; - protected: - OrientedPoint m_pose; - std::vector m_beams; -}; - -}; - -#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/utils/autoptr.h b/slam_gmapping/openslam_gmapping/include/gmapping/utils/autoptr.h deleted file mode 100644 index abc647c..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/utils/autoptr.h +++ /dev/null @@ -1,97 +0,0 @@ -#ifndef AUTOPTR_H -#define AUTOPTR_H -#include - -namespace GMapping{ - -template -class autoptr{ - protected: - - public: - struct reference{ - X* data; - unsigned int shares; - }; - inline autoptr(X* p=(X*)(0)); - inline autoptr(const autoptr& ap); - inline autoptr& operator=(const autoptr& ap); - inline ~autoptr(); - inline operator int() const; - inline X& operator*(); - inline const X& operator*() const; - //p - reference * m_reference; - protected: -}; - -template -autoptr::autoptr(X* p){ - m_reference=0; - if (p){ - m_reference=new reference; - m_reference->data=p; - m_reference->shares=1; - } -} - -template -autoptr::autoptr(const autoptr& ap){ - m_reference=0; - reference* ref=ap.m_reference; - if (ap.m_reference){ - m_reference=ref; - m_reference->shares++; - } -} - -template -autoptr& autoptr::operator=(const autoptr& ap){ - reference* ref=ap.m_reference; - if (m_reference==ref){ - return *this; - } - if (m_reference && !(--m_reference->shares)){ - delete m_reference->data; - delete m_reference; - m_reference=0; - } - if (ref){ - m_reference=ref; - m_reference->shares++; - } -//20050802 nasty changes begin - else - m_reference=0; -//20050802 nasty changes end - return *this; -} - -template -autoptr::~autoptr(){ - if (m_reference && !(--m_reference->shares)){ - delete m_reference->data; - delete m_reference; - m_reference=0; - } -} - -template -autoptr::operator int() const{ - return m_reference && m_reference->shares && m_reference->data; -} - -template -X& autoptr::operator*(){ - assert(m_reference && m_reference->shares && m_reference->data); - return *(m_reference->data); -} - -template -const X& autoptr::operator*() const{ - assert(m_reference && m_reference->shares && m_reference->data); - return *(m_reference->data); -} - -}; -#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/utils/commandline.h b/slam_gmapping/openslam_gmapping/include/gmapping/utils/commandline.h deleted file mode 100644 index bbd1ea4..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/utils/commandline.h +++ /dev/null @@ -1,115 +0,0 @@ -/***************************************************************** - * - * This file is part of the GMAPPING project - * - * GMAPPING Copyright (c) 2004 Giorgio Grisetti, - * Cyrill Stachniss, and Wolfram Burgard - * - * This software is licensed under the "Creative Commons - * License (Attribution-NonCommercial-ShareAlike 2.0)" - * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, - * and Wolfram Burgard. - * - * Further information on this license can be found at: - * http://creativecommons.org/licenses/by-nc-sa/2.0/ - * - * GMAPPING is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR - * PURPOSE. - * - *****************************************************************/ - - -#ifndef COMMANDLINE_H -#define COMMANDLINE_H - - -#define parseFlag(name,value)\ -if (!strcmp(argv[c],name)){\ - value=true;\ - cout << name << " on"<< endl;\ - recognized=true;\ -}\ - -#define parseString(name,value)\ -if (!strcmp(argv[c],name) && c -#endif -#ifdef MACOSX - #include - #include - //#define isnan(x) (x==FP_NAN) -#endif -#ifdef _WIN32 - #include - #ifndef __DRAND48_DEFINED__ - #define __DRAND48_DEFINED__ - inline double drand48() { return double(rand()) / RAND_MAX;} - #endif - #ifndef M_PI - #define M_PI 3.1415926535897932384626433832795 - #endif - #define round(d) (floor((d) + 0.5)) - typedef unsigned int uint; - #define isnan(x) (_isnan(x)) -#endif - -#endif - diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/utils/macro_params.h b/slam_gmapping/openslam_gmapping/include/gmapping/utils/macro_params.h deleted file mode 100644 index 5051408..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/utils/macro_params.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef MACRO_PARAMS_H -#define MACRO_PARAMS_H - -#define PARAM_SET_GET(type, name, qualifier, setqualifier, getqualifier)\ -qualifier: type m_##name;\ -getqualifier: inline type get##name() const {return m_##name;}\ -setqualifier: inline void set##name(type name) {m_##name=name;} - -#define PARAM_SET(type, name, qualifier, setqualifier)\ -qualifier: type m_##name;\ -setqualifier: inline void set##name(type name) {m_##name=name;} - -#define PARAM_GET(type, name, qualifier, getqualifier)\ -qualifier: type m_##name;\ -getqualifier: inline type get##name() const {return m_##name;} - -#define MEMBER_PARAM_SET_GET(member, type, name, qualifier, setqualifier, getqualifier)\ -getqualifier: inline type get##name() const {return member.get##name();}\ -setqualifier: inline void set##name(type name) { member.set##name(name);} - -#define MEMBER_PARAM_SET(member, type, name, qualifier, setqualifier, getqualifier)\ -setqualifier: inline void set##name(type name) { member.set##name(name);} - -#define MEMBER_PARAM_GET(member, type, name, qualifier, setqualifier, getqualifier)\ -getqualifier: inline type get##name() const {return member.get##name();} - -#define STRUCT_PARAM_SET_GET(member, type, name, qualifier, setqualifier, getqualifier)\ -getqualifier: inline type get##name() const {return member.name;}\ -setqualifier: inline void set##name(type name) {member.name=name;} - -#define STRUCT_PARAM_SET(member, type, name, qualifier, setqualifier, getqualifier)\ -setqualifier: inline void set##name(type name) {member.name=name;} - -#define STRUCT_PARAM_GET(member, type, name, qualifier, setqualifier, getqualifier)\ -getqualifier: inline type get##name() const {return member.name;}\ - -#define convertStringArgument(var,val,buf) if (!strcmp(buf,#val)) var=val -#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/utils/point.h b/slam_gmapping/openslam_gmapping/include/gmapping/utils/point.h deleted file mode 100644 index 92bab48..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/utils/point.h +++ /dev/null @@ -1,207 +0,0 @@ -#ifndef _POINT_H_ -#define _POINT_H_ -#include -#include -#include -#include "gvalues.h" - -#define DEBUG_STREAM cerr << __PRETTY_FUNCTION__ << ":" //FIXME - -namespace GMapping { - -template -struct point{ - inline point():x(0),y(0) {} - inline point(T _x, T _y):x(_x),y(_y){} - T x, y; -}; - -template -inline point operator+(const point& p1, const point& p2){ - return point(p1.x+p2.x, p1.y+p2.y); -} - -template -inline point operator - (const point & p1, const point & p2){ - return point(p1.x-p2.x, p1.y-p2.y); -} - -template -inline point operator * (const point& p, const T& v){ - return point(p.x*v, p.y*v); -} - -template -inline point operator * (const T& v, const point& p){ - return point(p.x*v, p.y*v); -} - -template -inline T operator * (const point& p1, const point& p2){ - return p1.x*p2.x+p1.y*p2.y; -} - - -template -struct orientedpoint: public point{ - inline orientedpoint() : point(0,0), theta(0) {}; - inline orientedpoint(const point& p); - inline orientedpoint(T x, T y, A _theta): point(x,y), theta(_theta){} - inline void normalize(); - inline orientedpoint rotate(A alpha){ - T s=sin(alpha), c=cos(alpha); - A a=alpha+theta; - a=atan2(sin(a),cos(a)); - return orientedpoint( - c*this->x-s*this->y, - s*this->x+c*this->y, - a); - } - A theta; -}; - - -template -void orientedpoint::normalize() { - if (theta >= -M_PI && theta < M_PI) - return; - - int multiplier = (int)(theta / (2*M_PI)); - theta = theta - multiplier*2*M_PI; - if (theta >= M_PI) - theta -= 2*M_PI; - if (theta < -M_PI) - theta += 2*M_PI; -} - - -template -orientedpoint::orientedpoint(const point& p){ - this->x=p.x; - this->y=p.y; - this->theta=0.; -} - - -template -orientedpoint operator+(const orientedpoint& p1, const orientedpoint& p2){ - return orientedpoint(p1.x+p2.x, p1.y+p2.y, p1.theta+p2.theta); -} - -template -orientedpoint operator - (const orientedpoint & p1, const orientedpoint & p2){ - return orientedpoint(p1.x-p2.x, p1.y-p2.y, p1.theta-p2.theta); -} - -template -orientedpoint operator * (const orientedpoint& p, const T& v){ - return orientedpoint(p.x*v, p.y*v, p.theta*v); -} - -template -orientedpoint operator * (const T& v, const orientedpoint& p){ - return orientedpoint(p.x*v, p.y*v, p.theta*v); -} - -template -orientedpoint absoluteDifference(const orientedpoint& p1,const orientedpoint& p2){ - orientedpoint delta=p1-p2; - delta.theta=atan2(sin(delta.theta), cos(delta.theta)); - double s=sin(p2.theta), c=cos(p2.theta); - return orientedpoint(c*delta.x+s*delta.y, - -s*delta.x+c*delta.y, delta.theta); -} - -template -orientedpoint absoluteSum(const orientedpoint& p1,const orientedpoint& p2){ - double s=sin(p1.theta), c=cos(p1.theta); - return orientedpoint(c*p2.x-s*p2.y, - s*p2.x+c*p2.y, p2.theta) + p1; -} - -template -point absoluteSum(const orientedpoint& p1,const point& p2){ - double s=sin(p1.theta), c=cos(p1.theta); - return point(c*p2.x-s*p2.y, s*p2.x+c*p2.y) + (point) p1; -} - -template -struct pointcomparator{ - bool operator ()(const point& a, const point& b) const { - return a.x -struct pointradialcomparator{ - point origin; - bool operator ()(const point& a, const point& b) const { - point delta1=a-origin; - point delta2=b-origin; - return (atan2(delta1.y,delta1.x) -inline point max(const point& p1, const point& p2){ - point p=p1; - p.x=p.x>p2.x?p.x:p2.x; - p.y=p.y>p2.y?p.y:p2.y; - return p; -} - -template -inline point min(const point& p1, const point& p2){ - point p=p1; - p.x=p.x -inline point interpolate(const point& p1, const F& t1, const point& p2, const F& t2, const F& t3){ - F gain=(t3-t1)/(t2-t1); - point p=p1+(p2-p1)*gain; - return p; -} - -template -inline orientedpoint -interpolate(const orientedpoint& p1, const F& t1, const orientedpoint& p2, const F& t2, const F& t3){ - F gain=(t3-t1)/(t2-t1); - orientedpoint p; - p.x=p1.x+(p2.x-p1.x)*gain; - p.y=p1.y+(p2.y-p1.y)*gain; - double s=sin(p1.theta)+sin(p2.theta)*gain, - c=cos(p1.theta)+cos(p2.theta)*gain; - p.theta=atan2(s,c); - return p; -} - - -template -inline double euclidianDist(const point& p1, const point& p2){ - return hypot(p1.x-p2.x, p1.y-p2.y); -} -template -inline double euclidianDist(const orientedpoint& p1, const orientedpoint& p2){ - return hypot(p1.x-p2.x, p1.y-p2.y); -} -template -inline double euclidianDist(const orientedpoint& p1, const point& p2){ - return hypot(p1.x-p2.x, p1.y-p2.y); -} -template -inline double euclidianDist(const point& p1, const orientedpoint& p2 ){ - return hypot(p1.x-p2.x, p1.y-p2.y); -} - - - -typedef point IntPoint; -typedef point Point; -typedef orientedpoint OrientedPoint; - -}; //end namespace - -#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/utils/stat.h b/slam_gmapping/openslam_gmapping/include/gmapping/utils/stat.h deleted file mode 100644 index b5e6115..0000000 --- a/slam_gmapping/openslam_gmapping/include/gmapping/utils/stat.h +++ /dev/null @@ -1,147 +0,0 @@ -#ifndef STAT_H -#define STAT_H -#include "point.h" -#include -#include "gvalues.h" - -namespace GMapping { - -/**stupid utility function for drawing particles form a zero mean, sigma variance normal distribution -probably it should not go there*/ -double sampleGaussian(double sigma,unsigned int S=0); - -double evalGaussian(double sigmaSquare, double delta); -double evalLogGaussian(double sigmaSquare, double delta); -int sampleUniformInt(int max); -double sampleUniformDouble(double min, double max); - -struct Covariance3{ - Covariance3 operator + (const Covariance3 & cov) const; - static Covariance3 zero; - double xx, yy, tt, xy, xt, yt; -}; - -struct EigenCovariance3{ - EigenCovariance3(); - EigenCovariance3(const Covariance3& c); - EigenCovariance3 rotate(double angle) const; - OrientedPoint sample() const; - double eval[3]; - double evec[3][3]; -}; - -struct Gaussian3{ - OrientedPoint mean; - EigenCovariance3 covariance; - Covariance3 cov; - double eval(const OrientedPoint& p) const; - void computeFromSamples(const std::vector & poses); - void computeFromSamples(const std::vector & poses, const std::vector& weights ); -}; - -template -Gaussian3 computeGaussianFromSamples(PointIterator& pointBegin, PointIterator& pointEnd, WeightIterator& weightBegin, WeightIterator& weightEnd){ - Gaussian3 gaussian; - OrientedPoint mean=OrientedPoint(0,0,0); - double wcum=0; - double s=0, c=0; - WeightIterator wt=weightBegin; - double *w=new double(); - OrientedPoint *p=new OrientedPoint(); - for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){ - *w=*wt; - *p=*pt; - s+=*w*sin(p->theta); - c+=*w*cos(p->theta); - mean.x+=*w*p->x; - mean.y+=*w*p->y; - wcum+=*w; - wt++; - } - mean.x/=wcum; - mean.y/=wcum; - s/=wcum; - c/=wcum; - mean.theta=atan2(s,c); - - Covariance3 cov=Covariance3::zero; - wt=weightBegin; - for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){ - *w=*wt; - *p=*pt; - OrientedPoint delta=(*p)-mean; - delta.theta=atan2(sin(delta.theta),cos(delta.theta)); - cov.xx+=*w*delta.x*delta.x; - cov.yy+=*w*delta.y*delta.y; - cov.tt+=*w*delta.theta*delta.theta; - cov.xy+=*w*delta.x*delta.y; - cov.yt+=*w*delta.y*delta.theta; - cov.xt+=*w*delta.x*delta.theta; - wt++; - } - cov.xx/=wcum; - cov.yy/=wcum; - cov.tt/=wcum; - cov.xy/=wcum; - cov.yt/=wcum; - cov.xt/=wcum; - EigenCovariance3 ecov(cov); - gaussian.mean=mean; - gaussian.covariance=ecov; - gaussian.cov=cov; - delete w; - delete p; - return gaussian; -} - -template -Gaussian3 computeGaussianFromSamples(PointIterator& pointBegin, PointIterator& pointEnd){ - Gaussian3 gaussian; - OrientedPoint mean=OrientedPoint(0,0,0); - double wcum=1; - double s=0, c=0; - OrientedPoint *p=new OrientedPoint(); - for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){ - *p=*pt; - s+=sin(p->theta); - c+=cos(p->theta); - mean.x+=p->x; - mean.y+=p->y; - wcum+=1.; - } - mean.x/=wcum; - mean.y/=wcum; - s/=wcum; - c/=wcum; - mean.theta=atan2(s,c); - - Covariance3 cov=Covariance3::zero; - for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){ - *p=*pt; - OrientedPoint delta=(*p)-mean; - delta.theta=atan2(sin(delta.theta),cos(delta.theta)); - cov.xx+=delta.x*delta.x; - cov.yy+=delta.y*delta.y; - cov.tt+=delta.theta*delta.theta; - cov.xy+=delta.x*delta.y; - cov.yt+=delta.y*delta.theta; - cov.xt+=delta.x*delta.theta; - } - cov.xx/=wcum; - cov.yy/=wcum; - cov.tt/=wcum; - cov.xy/=wcum; - cov.yt/=wcum; - cov.xt/=wcum; - EigenCovariance3 ecov(cov); - gaussian.mean=mean; - gaussian.covariance=ecov; - gaussian.cov=cov; - delete p; - return gaussian; -} - - -}; //end namespace -#endif - diff --git a/slam_gmapping/openslam_gmapping/ini/gfs-LMS-10cm.ini b/slam_gmapping/openslam_gmapping/ini/gfs-LMS-10cm.ini deleted file mode 100644 index 544bf78..0000000 --- a/slam_gmapping/openslam_gmapping/ini/gfs-LMS-10cm.ini +++ /dev/null @@ -1,78 +0,0 @@ -### gfs dummy config file - -## WARNING: Changing these parameters, can -## increase of decrese the performance of the -## mapper! - - -[gfs] - -################################################# -## -## These are probably the most improtant parameters -## - -## gfs - number of particles -particles 30 - -## gfs measurement integration -angularUpdate 0.5 -linearUpdate 1 - -## map resolution -delta 0.1 - -## scan matcher -maxrange 81.0 # (maximum valid) for SICK LMS, 81m max, SICK PLS 50m -maxUrange 80.0 # (use up to) -sigma 0.05 # scan matcher cell sigma, for the greedy search -regscore 0.0004 # minimum score for regsistering a scan -iterations 5 # iterations -critscore 0.0 # critical score (leave this) -maxMove 1.0 # maximum move among two scans. This detects some corrupted logs -autosize off # determine te map size by pre readoing the log - - -## default settings for a 0.1 m map cell -lstep 0.1 # linear search step (choose delta) -astep 0.05 # angular search step, this is fine, depending on the odometry error and the update interval -lsigma 0.075 # sigma likelihood of 1 beam -lskip 0 # beams to skip in the likelihood computation -skipMatching off # do not perform scan matching before computing the statistics - -kernelSize 1 # the higher the value the slower the filter - # the better it can deal with noise, but the less precise and slower -ogain 3 # gain for smoothing the likelihood -resampleThreshold 0.5 # when neff is below this value a resampling occurs -randseed 0 # this is for the repeated experiments - -## likelihood sampling -llsamplerange 0.1 # linear range -llsamplestep 0.1 # linear step -lasamplerange 0.05 # angular range -lasamplestep 0.05 # angular step - -## motion model parameters -srr 0.1 # translation as a function of translation -srt 0.1 # translation as a function of rotation -str 0.1 # rotation as a function of translation -stt 0.1 # rotation as a function of rotation - -## odometry integration in proposal -linearOdometryReliability 0.0 -angularOdometryReliability 0.0 -considerOdometryCovariance off - - -## inital map params -xmin -100.0 -ymin -100.0 -xmax 100.0 -ymax 100.0 - -## file parameters - -readFromStdin off -onLine off -generateMap off - diff --git a/slam_gmapping/openslam_gmapping/ini/gfs-LMS-20cm.ini b/slam_gmapping/openslam_gmapping/ini/gfs-LMS-20cm.ini deleted file mode 100644 index e0962ce..0000000 --- a/slam_gmapping/openslam_gmapping/ini/gfs-LMS-20cm.ini +++ /dev/null @@ -1,76 +0,0 @@ -### gfs dummy config file - -## WARNING: Changing these parameters, can -## increase of decrese the performance of the -## mapper! - - -[gfs] - -################################################# -## -## These are probably the most improtant parameters -## - -## gfs - number of particles -particles 30 - -## gfs measurement integration -angularUpdate 0.5 -linearUpdate 1 - -## map resolution -delta 0.2 -## scan matcher -maxrange 80 # (maximum valid) for SICK LMS, 81m max, SICK PLS 50m -maxUrange 80 # (use up to) -sigma 0.05 # scan matcher cell sigma, for the greedy search -regscore 10000 # minimum score for regsistering a scan -iterations 5 # iterations -critscore 0.0 # critical score (leave this) -maxMove 1.0 # maximum move among two scans. This detects some corrupted logs -autosize off # determine te map size by pre readoing the log - - -lstep 0.2 # linear search step (choose delta) -astep 0.05 # angular search step, this is fine, depending on the odometry error and the update interval -lsigma 0.2 # sigma likelihood of 1 beam -lskip 1. # beams to skip in the likelihood computation -skipMatching off # do not perform scan matching before computing the statistics - -kernelSize 1 # the higher the value the slower the filter - # the better it can deal with noise, but the less precise and slower -ogain 3 # gain for smoothing the likelihood -resampleThreshold 0.5 # when neff is below this value a resampling occurs -randseed 0 # this is for the repeated experiments - -## likelihood sampling -llsamplerange 0.2 # linear range -llsamplestep 0.2 # linear step -lasamplerange 0.05 # angular range -lasamplestep 0.05 # angular step - -## motion model parameters -srr 0.1 # translation as a function of translation -srt 0.1 # translation as a function of rotation -str 0.1 # rotation as a function of translation -stt 0.1 # rotation as a function of rotation - -## odometry integration in proposal -linearOdometryReliability 0.0 -angularOdometryReliability 0.0 -considerOdometryCovariance off - - -## inital map params -xmin -150.0 -ymin -100.0 -xmax 100.0 -ymax 100.0 - -## file parameters - -readFromStdin off -onLine off -generateMap off - diff --git a/slam_gmapping/openslam_gmapping/ini/gfs-LMS-5cm.ini b/slam_gmapping/openslam_gmapping/ini/gfs-LMS-5cm.ini deleted file mode 100644 index c54fa02..0000000 --- a/slam_gmapping/openslam_gmapping/ini/gfs-LMS-5cm.ini +++ /dev/null @@ -1,78 +0,0 @@ -### gfs dummy config file - -## WARNING: Changing these parameters, can -## increase of decrese the performance of the -## mapper! - - -[gfs] - -################################################# -## -## These are probably the most improtant parameters -## - -## gfs - number of particles -particles 30 - -## gfs measurement integration -angularUpdate 0.5 -linearUpdate 1 - -## map resolution -delta 0.05 - -## scan matcher -maxrange 81.0 # (maximum valid) for SICK LMS, 81m max, SICK PLS 50m -maxUrange 80.0 # (use up to) -sigma 0.05 # scan matcher cell sigma, for the greedy search -regscore 0.0004 # minimum score for regsistering a scan -iterations 5 # iterations -critscore 0.0 # critical score (leave this) -maxMove 1.0 # maximum move among two scans. This detects some corrupted logs -autosize off # determine te map size by pre readoing the log - - -## default settings for a 0.1 m map cell -lstep 0.05 # linear search step (choose delta) -astep 0.05 # angular search step, this is fine, depending on the odometry error and the update interval -lsigma 0.05 # sigma likelihood of 1 beam -lskip 0 # beams to skip in the likelihood computation -skipMatching off # do not perform scan matching before computing the statistics - -kernelSize 1 # the higher the value the slower the filter - # the better it can deal with noise, but the less precise and slower -ogain 3 # gain for smoothing the likelihood -resampleThreshold 0.5 # when neff is below this value a resampling occurs -randseed 0 # this is for the repeated experiments - -## likelihood sampling -llsamplerange 0.05 # linear range -llsamplestep 0.05 # linear step -lasamplerange 0.05 # angular range -lasamplestep 0.05 # angular step - -## motion model parameters -srr 0.1 # translation as a function of translation -srt 0.1 # translation as a function of rotation -str 0.1 # rotation as a function of translation -stt 0.1 # rotation as a function of rotation - -## odometry integration in proposal -linearOdometryReliability 0.0 -angularOdometryReliability 0.0 -considerOdometryCovariance off - - -## inital map params -xmin -100.0 -ymin -100.0 -xmax 100.0 -ymax 100.0 - -## file parameters - -readFromStdin off -onLine off -generateMap off - diff --git a/slam_gmapping/openslam_gmapping/ini/gfs-PLS-10cm.ini b/slam_gmapping/openslam_gmapping/ini/gfs-PLS-10cm.ini deleted file mode 100644 index 1e6fb34..0000000 --- a/slam_gmapping/openslam_gmapping/ini/gfs-PLS-10cm.ini +++ /dev/null @@ -1,78 +0,0 @@ -### gfs dummy config file - -## WARNING: Changing these parameters, can -## increase of decrese the performance of the -## mapper! - - -[gfs] - -################################################# -## -## These are probably the most improtant parameters -## - -## gfs - number of particles -particles 30 - -## gfs measurement integration -angularUpdate 0.5 -linearUpdate 1 - -## map resolution -delta 0.1 - -## scan matcher -maxrange 50.0 # (maximum valid) for SICK LMS, 81m max, SICK PLS 50m -maxUrange 50.0 # (use up to) -sigma 0.075 # scan matcher cell sigma, for the greedy search -regscore 0.0004 # minimum score for regsistering a scan -iterations 5 # iterations -critscore 0.0 # critical score (leave this) -maxMove 1.0 # maximum move among two scans. This detects some corrupted logs -autosize off # determine te map size by pre readoing the log - - -## default settings for a 0.1 m map cell -lstep 0.1 # linear search step (choose delta) -astep 0.05 # angular search step, this is fine, depending on the odometry error and the update interval -lsigma 0.1 # sigma likelihood of 1 beam -lskip 0 # beams to skip in the likelihood computation -skipMatching off # do not perform scan matching before computing the statistics - -kernelSize 1 # the higher the value the slower the filter - # the better it can deal with noise, but the less precise and slower -ogain 3 # gain for smoothing the likelihood -resampleThreshold 0.5 # when neff is below this value a resampling occurs -randseed 0 # this is for the repeated experiments - -## likelihood sampling -llsamplerange 0.1 # linear range -llsamplestep 0.1 # linear step -lasamplerange 0.05 # angular range -lasamplestep 0.05 # angular step - -## motion model parameters -srr 0.1 # translation as a function of translation -srt 0.1 # translation as a function of rotation -str 0.1 # rotation as a function of translation -stt 0.1 # rotation as a function of rotation - -## odometry integration in proposal -linearOdometryReliability 0.0 # condition the scan matcher with odometry while serching for max [translation component] -angularOdometryReliability 0.0 # condition the scan matcher with odometry while serching for max [rotation component] -considerOdometryCovariance off - - -## inital map params -xmin -100.0 -ymin -100.0 -xmax 100.0 -ymax 100.0 - -## file parameters - -readFromStdin off -onLine off -generateMap off - diff --git a/slam_gmapping/openslam_gmapping/ini/gfs-PLS-5cm.ini b/slam_gmapping/openslam_gmapping/ini/gfs-PLS-5cm.ini deleted file mode 100644 index 871dcc5..0000000 --- a/slam_gmapping/openslam_gmapping/ini/gfs-PLS-5cm.ini +++ /dev/null @@ -1,78 +0,0 @@ -### gfs dummy config file - -## WARNING: Changing these parameters, can -## increase of decrese the performance of the -## mapper! - - -[gfs] - -################################################# -## -## These are probably the most improtant parameters -## - -## gfs - number of particles -particles 30 - -## gfs measurement integration -angularUpdate 0.5 -linearUpdate 1 - -## map resolution -delta 0.05 - -## scan matcher -maxrange 50.0 # (maximum valid) for SICK LMS, 81m max, SICK PLS 50m -maxUrange 50.0 # (use up to) -sigma 0.07 # scan matcher cell sigma, for the greedy search -regscore 0.0004 # minimum score for regsistering a scan -iterations 5 # iterations -critscore 0.0 # critical score (leave this) -maxMove 1.0 # maximum move among two scans. This detects some corrupted logs -autosize off # determine te map size by pre readoing the log - - -## default settings for a 0.1 m map cell -lstep 0.05 # linear search step (choose delta) -astep 0.05 # angular search step, this is fine, depending on the odometry error and the update interval -lsigma 0.05 # sigma likelihood of 1 beam -lskip 0 # beams to skip in the likelihood computation -skipMatching off # do not perform scan matching before computing the statistics - -kernelSize 1 # the higher the value the slower the filter - # the better it can deal with noise, but the less precise and slower -ogain 3 # gain for smoothing the likelihood -resampleThreshold 0.5 # when neff is below this value a resampling occurs -randseed 0 # this is for the repeated experiments - -## likelihood sampling -llsamplerange 0.05 # linear range -llsamplestep 0.05 # linear step -lasamplerange 0.05 # angular range -lasamplestep 0.05 # angular step - -## motion model parameters -srr 0.1 # translation as a function of translation -srt 0.1 # translation as a function of rotation -str 0.1 # rotation as a function of translation -stt 0.1 # rotation as a function of rotation - -## odometry integration in proposal -linearOdometryReliability 0.0 -angularOdometryReliability 0.0 -considerOdometryCovariance off - - -## inital map params -xmin -100.0 -ymin -100.0 -xmax 100.0 -ymax 100.0 - -## file parameters - -readFromStdin off -onLine off -generateMap off - diff --git a/slam_gmapping/openslam_gmapping/log/Makefile b/slam_gmapping/openslam_gmapping/log/Makefile deleted file mode 100644 index 02a0515..0000000 --- a/slam_gmapping/openslam_gmapping/log/Makefile +++ /dev/null @@ -1,9 +0,0 @@ -OBJS= configuration.o carmenconfiguration.o sensorlog.o sensorstream.o -APPS= log_test log_plot scanstudio2carmen rdk2carmen - -LDFLAGS+= -lsensor_range -lsensor_odometry -lsensor_base -CPPFLAGS+= -I../sensor - --include ../global.mk --include ../build_tools/Makefile.generic-shared-object - diff --git a/slam_gmapping/openslam_gmapping/log/carmenconfiguration.cpp b/slam_gmapping/openslam_gmapping/log/carmenconfiguration.cpp deleted file mode 100644 index 13784e3..0000000 --- a/slam_gmapping/openslam_gmapping/log/carmenconfiguration.cpp +++ /dev/null @@ -1,463 +0,0 @@ -#include -#include "carmenconfiguration.h" -#include -#include -#include -#include -#include -#include - - -#define LINEBUFFER_SIZE 10000 - -namespace GMapping { - -using namespace std; - -istream& CarmenConfiguration::load(istream& is){ - clear(); - char buf[LINEBUFFER_SIZE]; - bool laseron=false; - bool rlaseron=false; - bool rlaser1=false; - bool rlaser2=false; - - string beams; - string rbeams; - - while (is){ - is.getline(buf, LINEBUFFER_SIZE); - istringstream lis(buf); - - string qualifier; - string name; - - if (lis) - lis >> qualifier; - else - continue; - //this is a workaround for carmen log files - //the number lf laser beams should be specofoed in the config - //part of the log - if (qualifier=="FLASER"){ - laseron=true; - lis >> beams; - } - if (qualifier=="RLASER"){ - rlaseron=true; - lis >> rbeams; - } - if (qualifier=="ROBOTLASER1"){ - string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode; - lis >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode>> beams; - rlaser1=true; - } - if (qualifier=="ROBOTLASER2"){ - string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode; - lis >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode>> rbeams; - rlaser2=true; - } - if (qualifier!="PARAM") - continue; - if (lis) - lis >> name; - else continue; - - - vector v; - while (lis){ - string cparm; - lis >> cparm; - if (lis) - v.push_back(cparm); - } - insert(make_pair(name, v)); - } - if (laseron || rlaser1){ - vector v; - v.push_back(beams); - insert(make_pair("laser_beams", v)); - cerr << "FRONT LASER BEAMS FROM LOG: " << beams << endl; - v.clear(); - v.push_back("on"); - insert(make_pair("robot_use_laser", v)); - } - if (rlaseron || rlaser2){ - vector v; - v.push_back(rbeams); - insert(make_pair("rear_laser_beams", v)); - cerr << "REAR LASER BEAMS FROM LOG: " << beams << endl; - v.clear(); - v.push_back("on"); - insert(make_pair("robot_use_rear_laser", v)); - } - return is; -} - -SensorMap CarmenConfiguration::computeSensorMap() const{ - //this boring stuff is for retrieving the parameters from the loaded tokens - - SensorMap smap; - //odometry - OdometrySensor* odometry=new OdometrySensor("ODOM"); - OdometrySensor* truepos=new OdometrySensor("TRUEPOS", true); - - smap.insert(make_pair(odometry->getName(), odometry)); - smap.insert(make_pair(truepos->getName(), truepos)); - //sonars - const_iterator key=find("robot_use_sonar"); - if (key!=end() && key->second.front()=="on"){ - RangeSensor* sonar=new RangeSensor("SONAR"); - - //the center of the sonar is the center of the base - sonar->m_pose.x=sonar->m_pose.y=sonar->m_pose.theta=0; - - double maxrange=10.; - key=find("robot_max_sonar"); - if (key!=end()){ - maxrange=atof(key->second.front().c_str()); - cerr << "max sonar:" << maxrange << endl; - } - - unsigned int sonar_num=0; - key=find("robot_num_sonars"); - if (key!=end()){ - sonar_num=atoi(key->second.front().c_str()); - cerr << "robot_num_sonars" << sonar_num << endl; - } - - key=find("robot_sonar_offsets"); - if (key!=end()){ - const vector & soff=key->second; - - if( (soff.size()/3m_beams.push_back(beam); - cerr << "beam_x" << beam.pose.x; - cerr << " beam_y" << beam.pose.y; - cerr << " beam_theta" << beam.pose.theta << endl;; - } - } - sonar->updateBeamsLookup(); - smap.insert(make_pair(sonar->getName(), sonar)); - } - - //laser - key=find("robot_use_laser"); - - if (key!=end() && key->second.front()=="on"){ - RangeSensor* laser=new RangeSensor("FLASER"); - laser->newFormat=false; - //by default the center of the robot is the center of the laser - laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0; - key=find("robot_frontlaser_offset"); - if (key!=end()){ - laser->m_pose.x=atof(key->second.front().c_str()); - cerr << "FRONT OFFSET= " << laser->m_pose.x << endl; - } - - - - RangeSensor::Beam beam; - - //double angle=-.5*M_PI; - unsigned int beam_no=180; - - key=find("laser_beams"); - if (key!=end()){ - beam_no=atoi(key->second.front().c_str()); - cerr << "FRONT BEAMS="<< beam_no << endl; - } - - double maxrange=50; - double resolution=1.; - - - if (beam_no==180 || beam_no==181) - resolution =1.; - else if (beam_no==360 || beam_no==361) - resolution =.5; - else if (beam_no==540 || beam_no==541) - resolution =.5; - else if (beam_no==769) { - resolution =360./1024.; - maxrange = 4.1; - } - else if (beam_no==682) { - resolution =360./1024.; - maxrange = 4.1; - } - else if (beam_no==683) { - resolution =360./1024.; - maxrange = 5.5; - } - else { - key=find("laser_front_laser_resolution"); - if (key!=end()){ - resolution=atof(key->second.front().c_str()); - cerr << "FRONT RES " << resolution << endl; - } - } - - laser->m_beams.resize(beam_no); - double center_beam=(double)beam_no/2.; - uint low_index=(uint)floor(center_beam); - uint up_index=(uint)ceil(center_beam); - double step=resolution*M_PI/180.; - double angle=beam_no%2?0:step; - unsigned int i=beam_no%2?0:1; - for (; im_beams[low_index-i]=beam; - beam.pose.theta=angle; - laser->m_beams[up_index+i-1]=beam; - } - laser->updateBeamsLookup(); - smap.insert(make_pair(laser->getName(), laser)); - cerr << "front beams " << beam_no << endl; - cerr << "maxrange " << maxrange << endl; - } - - - key=find("robot_use_laser"); - if (key!=end() && key->second.front()=="on"){ - RangeSensor* laser=new RangeSensor("ROBOTLASER1"); - laser->newFormat=true; - cerr << "ROBOTLASER1 inserted" << endl; - //by default the center of the robot is the center of the laser - laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0; - key=find("robot_frontlaser_offset"); - if (key!=end()){ - laser->m_pose.x=atof(key->second.front().c_str()); - cerr << "FRONT OFFSET=" << laser->m_pose.x << endl; - } - - RangeSensor::Beam beam; - - //double angle=-.5*M_PI; - unsigned int beam_no=180; - - key=find("laser_beams"); - if (key!=end()){ - beam_no=atoi(key->second.front().c_str()); - cerr << "FRONT BEAMS="<< beam_no << endl; - } - - double maxrange=50; - double resolution=1.; - - - if (beam_no==180 || beam_no==181) - resolution =1.; - else if (beam_no==360 || beam_no==361) - resolution =.5; - else if (beam_no==540 || beam_no==541) - resolution =.5; - else if (beam_no==769) - resolution =360./1024.; - else if (beam_no==683) { - resolution =360./1024.; - maxrange=5.50; - } - else { - key=find("laser_front_laser_resolution"); - if (key!=end()){ - resolution=atof(key->second.front().c_str()); - cerr << "FRONT RES" << resolution << endl; - } - } - - laser->m_beams.resize(beam_no); - double center_beam=(double)beam_no/2.; - uint low_index=(uint)floor(center_beam); - uint up_index=(uint)ceil(center_beam); - double step=resolution*M_PI/180.; - double angle=beam_no%2?0:step; - unsigned int i=beam_no%2?0:1; - for (; im_beams[low_index-i]=beam; - beam.pose.theta=angle; - laser->m_beams[up_index+i-1]=beam; - } - laser->updateBeamsLookup(); - smap.insert(make_pair(laser->getName(), laser)); - cerr << "front beams" << beam_no << endl; - } - - - //vertical laser - key=find("robot_use_rear_laser"); - - if (key!=end() && key->second.front()=="on"){ - RangeSensor* laser=new RangeSensor("RLASER"); - - //by default the center of the robot is the center of the laser - laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0; - laser->m_pose.theta=M_PI; - key=find("robot_rearlaser_offset"); - if (key!=end()){ - laser->m_pose.x=atof(key->second.front().c_str()); - cerr << "REAR OFFSET = " << laser->m_pose.x << endl; - } - - - - RangeSensor::Beam beam; - - //double angle=-.5*M_PI; - unsigned int beam_no=180; - - key=find("rear_laser_beams"); - if (key!=end()){ - beam_no=atoi(key->second.front().c_str()); - cerr << "REAR BEAMS="<< beam_no << endl; - } - - double maxrange=89; - double resolution=1.; - - - if (beam_no==180 || beam_no==181) - resolution =1.; - else if (beam_no==360 || beam_no==361) - resolution =.5; - else if (beam_no==540 || beam_no==541) - resolution =.5; - else if (beam_no==769) - resolution =360./1024.; - else { - key=find("laser_rear_laser_resolution"); - if (key!=end()){ - resolution=atof(key->second.front().c_str()); - cerr << "REAR RES" << resolution << endl; - } - } - - laser->m_beams.resize(beam_no); - double center_beam=(double)beam_no/2.; - uint low_index=(uint)floor(center_beam); - uint up_index=(uint)ceil(center_beam); - double step=resolution*M_PI/180.; - double angle=beam_no%2?0:step; - unsigned int i=beam_no%2?0:1; - for (; im_beams[low_index-i]=beam; - beam.pose.theta=angle; - laser->m_beams[up_index+i-1]=beam; - } - laser->updateBeamsLookup(); - smap.insert(make_pair(laser->getName(), laser)); - cerr<< "rear beams" << beam_no << endl; - } - - key=find("robot_use_rear_laser"); - if (key!=end() && key->second.front()=="on"){ - RangeSensor* laser=new RangeSensor("ROBOTLASER2"); - laser->newFormat=true; - cerr << "ROBOTLASER2 inserted" << endl; - //by default the center of the robot is the center of the laser - laser->m_pose.x=laser->m_pose.y=0; - laser->m_pose.theta=M_PI; - key=find("robot_rearlaser_offset"); - if (key!=end()){ - // laser->m_pose.x==atof(key->second.front().c_str()); - cerr << "REAR OFFSET not used" << laser->m_pose.x << endl; - } - - RangeSensor::Beam beam; - - //double angle=-.5*M_PI; - unsigned int beam_no=180; - - key=find("rear_laser_beams"); - if (key!=end()){ - beam_no=atoi(key->second.front().c_str()); - cerr << "REAR BEAMS="<< beam_no << endl; - } - - double maxrange=50; - double resolution=1.; - - - if (beam_no==180 || beam_no==181) - resolution =1.; - else if (beam_no==360 || beam_no==361) - resolution =.5; - else if (beam_no==540 || beam_no==541) - resolution =.5; - else if (beam_no==769) - resolution =360./1024.; - else { - key=find("laser_rear_laser_resolution"); - if (key!=end()){ - resolution=atof(key->second.front().c_str()); - cerr << "REAR RES" << resolution << endl; - } - } - - laser->m_beams.resize(beam_no); - double center_beam=(double)beam_no/2.; - uint low_index=(uint)floor(center_beam); - uint up_index=(uint)ceil(center_beam); - double step=resolution*M_PI/180.; - double angle=beam_no%2?0:step; - unsigned int i=beam_no%2?0:1; - for (; im_beams[low_index-i]=beam; - beam.pose.theta=angle; - laser->m_beams[up_index+i-1]=beam; - } - laser->updateBeamsLookup(); - smap.insert(make_pair(laser->getName(), laser)); - cerr << "rear beams" << beam_no << endl; - } - - - return smap; -} - -}; - diff --git a/slam_gmapping/openslam_gmapping/log/carmenconfiguration.h b/slam_gmapping/openslam_gmapping/log/carmenconfiguration.h deleted file mode 100644 index 4cf9bfb..0000000 --- a/slam_gmapping/openslam_gmapping/log/carmenconfiguration.h +++ /dev/null @@ -1,22 +0,0 @@ -#ifndef CARMENCONFIGURATION_H -#define CARMENCONFIGURATION_H - -#include -#include -#include -#include -#include -#include "configuration.h" - -namespace GMapping { - -class CarmenConfiguration: public Configuration, public std::map >{ - public: - virtual std::istream& load(std::istream& is); - virtual SensorMap computeSensorMap() const; -}; - -}; - -#endif - diff --git a/slam_gmapping/openslam_gmapping/log/configuration.cpp b/slam_gmapping/openslam_gmapping/log/configuration.cpp deleted file mode 100644 index 9c81c5e..0000000 --- a/slam_gmapping/openslam_gmapping/log/configuration.cpp +++ /dev/null @@ -1,8 +0,0 @@ -#include "configuration.h" - -namespace GMapping { - -Configuration::~Configuration(){ -} - -}; diff --git a/slam_gmapping/openslam_gmapping/log/configuration.h b/slam_gmapping/openslam_gmapping/log/configuration.h deleted file mode 100644 index f02d17f..0000000 --- a/slam_gmapping/openslam_gmapping/log/configuration.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef CONFIGURATION_H -#define CONFIGURATION_H - -#include -#include - -namespace GMapping { - -class Configuration{ - public: - virtual ~Configuration(); - virtual SensorMap computeSensorMap() const=0; -}; - -}; -#endif - diff --git a/slam_gmapping/openslam_gmapping/log/log_plot.cpp b/slam_gmapping/openslam_gmapping/log/log_plot.cpp deleted file mode 100644 index ad42861..0000000 --- a/slam_gmapping/openslam_gmapping/log/log_plot.cpp +++ /dev/null @@ -1,71 +0,0 @@ -#include -#include -#include -#include -#include -#include - - -using namespace std; -using namespace GMapping; - -int main(int argc, char ** argv){ - double maxrange=2.; - if (argc<2){ - cout << "usage log_plot | gnuplot" << endl; - exit (-1); - } - ifstream is(argv[1]); - if (! is){ - cout << "no file " << argv[1] << " found" << endl; - exit (-1); - } - CarmenConfiguration conf; - conf.load(is); - - SensorMap m=conf.computeSensorMap(); - - //for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++) - // cout << it->first << " " << it->second->getName() << endl; - - SensorLog log(m); - is.close(); - - ifstream ls(argv[1]); - log.load(ls); - ls.close(); - int count=0; - int frame=0; - cerr << "log size" << log.size() << endl; - for (SensorLog::iterator it=log.begin(); it!=log.end(); it++){ - RangeReading* rr=dynamic_cast(*it); - if (rr){ - count++; - if (count%3) - continue; - std::vector points(rr->size()); - uint j=0; - for (uint i=0; isize(); i++){ - const RangeSensor * rs=dynamic_cast(rr->getSensor()); - double c=rs->beams()[i].c, s=rs->beams()[i].s; - double r=(*rr)[i]; - if (r>maxrange) - continue; - points[j++]=Point(r*c,r*s); - } - if (j){ - char buf[1024]; - sprintf(buf,"frame-%05d.gif",frame); - frame++; - cout << "set terminal gif" << endl; - cout << "set output \"" << buf << "\"" << endl; - cout << "set size ratio -1" << endl; - cout << "plot [-3:3][0:3] '-' w p ps 1" << endl; - for (uint i=0; i -#include -#include -#include -#include - - -using namespace std; -using namespace GMapping; - -int main(int argc, char ** argv){ - if (argc<2){ - cout << "usage log_test " << endl; - exit (-1); - } - ifstream is(argv[1]); - if (! is){ - cout << "no file " << argv[1] << " found" << endl; - exit (-1); - } - CarmenConfiguration conf; - conf.load(is); - - SensorMap m=conf.computeSensorMap(); - - //for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++) - // cout << it->first << " " << it->second->getName() << endl; - - SensorLog log(m); - is.close(); - - ifstream ls(argv[1]); - log.load(ls); - ls.close(); - cerr << "log size" << log.size() << endl; - for (SensorLog::iterator it=log.begin(); it!=log.end(); it++){ - RangeReading* rr=dynamic_cast(*it); - if (rr){ - //cerr << rr->getSensor()->getName() << " "; - //cerr << rr->size()<< " "; - //for (RangeReading::const_iterator it=rr->begin(); it!=rr->end(); it++){ - // cerr << *it << " "; - //} - cout<< rr->getPose().x << " " << rr->getPose().y << " " << rr->getPose().theta << " " << rr->getTime() << endl; - } - } -} diff --git a/slam_gmapping/openslam_gmapping/log/rdk2carmen.cpp b/slam_gmapping/openslam_gmapping/log/rdk2carmen.cpp deleted file mode 100644 index 199c6f7..0000000 --- a/slam_gmapping/openslam_gmapping/log/rdk2carmen.cpp +++ /dev/null @@ -1,58 +0,0 @@ -#include -#include -#include -#include -#include - - -using namespace std; -using namespace GMapping; - -int main(int argc, char ** argv){ - if (argc<2){ - cerr << "usage "< " << endl; - cerr << "or "< for standard output" << endl; - exit (-1); - } - ifstream is(argv[1]); - if (! is){ - cerr << "no file " << argv[1] << " found" << endl; - exit (-1); - } - ostream *os; - if (argc<3) - os=&cout; - else{ - os=new ofstream(argv[2]); - if (! os){ - cerr << "no file " << argv[1] << " found" << endl; - exit (-1); - } - } - CarmenConfiguration conf; - conf.load(is); - - SensorMap m=conf.computeSensorMap(); - - //for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++) - // cout << it->first << " " << it->second->getName() << endl; - - SensorLog log(m); - is.close(); - - ifstream ls(argv[1]); - log.load(ls); - ls.close(); - cerr << "log size" << log.size() << endl; - for (SensorLog::iterator it=log.begin(); it!=log.end(); it++){ - RangeReading* rr=dynamic_cast(*it); - if (rr){ - *os << rr->getSensor()->getName() << " "; - *os << rr->size()<< " "; - for (RangeReading::const_iterator it=rr->begin(); it!=rr->end(); it++){ - *os << (*it)*0.001 << " "; - } - *os<< rr->getPose().x*0.001 << " " << rr->getPose().y*0.001 << " " << rr->getPose().theta << endl; - } - } -} diff --git a/slam_gmapping/openslam_gmapping/log/scanstudio2carmen.cpp b/slam_gmapping/openslam_gmapping/log/scanstudio2carmen.cpp deleted file mode 100644 index 1fdde8a..0000000 --- a/slam_gmapping/openslam_gmapping/log/scanstudio2carmen.cpp +++ /dev/null @@ -1,66 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#define MAXLINELENGHT (10240) -#define MAXREADINGS (10240) - -using namespace std; -using namespace GMapping; - -int main (int argc, char** argv){ - if (argc<3){ - cout << "usage scanstudio2carmen scanfilename carmenfilename" << endl; - exit(1); - } - ifstream is(argv[1]); - if (!is){ - cout << "cannopt open file" << argv[1] << endl; - exit(1); - } - - ofstream os(argv[2]); - - double readings[MAXREADINGS]; - OrientedPoint pose; - int nbeams; - while (is){ - char buf[MAXLINELENGHT]; - is.getline(buf,MAXLINELENGHT); - istringstream st(buf); - string token; - st>>token; - if (token=="RobotPos:"){ - st >> pose.x >> pose.y >> pose.theta; - pose.x/=1000; - pose.y/=1000; - } else - if (token=="NumPoints:"){ - st >> nbeams; - assert(nbeams> angle; - is >> readings[c]; - readings[c]/=1000; - c++; - } - if (c==nbeams) - os << "FLASER " << nbeams << " "; - c=0; - while (c -#include -#include -#include -#include - -#define LINEBUFFER_SIZE 100000 - -namespace GMapping { - -using namespace std; - -SensorLog::SensorLog(const SensorMap& sm): m_sensorMap(sm){ -} - -SensorLog::~SensorLog(){ - for (iterator it=begin(); it!=end(); it++) - if (*it) delete (*it); -} - -istream& SensorLog::load(istream& is){ - for (iterator it=begin(); it!=end(); it++) - if (*it) delete (*it); - clear(); - - char buf[LINEBUFFER_SIZE]; - while (is){ - is.getline(buf, LINEBUFFER_SIZE); - istringstream lis(buf); - - string sensorname; - - if (lis) - lis >>sensorname; - else - continue; - - - - SensorMap::const_iterator it=m_sensorMap.find(sensorname); - if (it==m_sensorMap.end()){ - continue; - } - - Sensor* sensor=it->second; - - SensorReading* reading=0; - OdometrySensor* odometry=dynamic_cast(sensor); - if (odometry) - reading=parseOdometry(lis, odometry); - - RangeSensor* range=dynamic_cast(sensor); - if (range) - reading=parseRange(lis, range); - if (reading) - push_back(reading); - } - return is; - -} - -OdometryReading* SensorLog::parseOdometry(istream& is, const OdometrySensor* osen) const{ - OdometryReading* reading=new OdometryReading(osen); - OrientedPoint pose; - OrientedPoint speed; - OrientedPoint accel; - is >> pose.x >> pose.y >> pose.theta; - is >> speed.x >>speed.theta; - speed.y=0; - is >> accel.x; - accel.y=accel.theta=0; - reading->setPose(pose); reading->setSpeed(speed); reading->setAcceleration(accel); - return reading; -} - -RangeReading* SensorLog::parseRange(istream& is, const RangeSensor* rs) const{ - if(rs->newFormat){ - string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode; - is >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy >> remission_mode; - } - - unsigned int size; - is >> size; - assert(size==rs->beams().size()); - - RangeReading* reading=new RangeReading(rs); - //cerr << "#R=" << size << endl; - reading->resize(size); - for (unsigned int i=0; i> (*reading)[i]; - } - if (rs->newFormat){ - int reflectionBeams; - is >> reflectionBeams; - double reflection; - for (int i=0; i> reflection; - } - //FIXME XXX - OrientedPoint laserPose; - is >> laserPose.x >> laserPose.y >> laserPose.theta; - OrientedPoint pose; - is >> pose.x >> pose.y >> pose.theta; - reading->setPose(pose); - double a,b,c; - if (rs->newFormat){ - string laser_tv, laser_rv, forward_safety_dist, side_safty_dist, turn_axis; - is >> laser_tv >> laser_rv >> forward_safety_dist >> side_safty_dist >> turn_axis; - } else { - is >> a >> b >> c; - } - string s; - is >> a >> s; - is >> a; - reading->setTime(a); - return reading; -} - -OrientedPoint SensorLog::boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const { - xmin=ymin=1e6; - xmax=ymax=-1e6; - bool first=true; - OrientedPoint start; - for (const_iterator it=begin(); it!=end(); it++){ - double lxmin=0., lxmax=0., lymin=0., lymax=0.; - const SensorReading* reading=*it; - const OdometryReading* odometry=dynamic_cast (reading); - if (odometry){ - lxmin=lxmax=odometry->getPose().x; - lymin=lymax=odometry->getPose().y; - } - - const RangeReading* rangeReading=dynamic_cast (reading); - if (rangeReading){ - lxmin=lxmax=rangeReading->getPose().x; - lymin=lymax=rangeReading->getPose().y; - if (first){ - first=false; - start=rangeReading->getPose(); - } - } - xmin=xminlxmax?xmax:lxmax; - ymin=yminlymax?ymax:lymax; - } - return start; -} - -}; - diff --git a/slam_gmapping/openslam_gmapping/log/sensorlog.h b/slam_gmapping/openslam_gmapping/log/sensorlog.h deleted file mode 100644 index c6b380f..0000000 --- a/slam_gmapping/openslam_gmapping/log/sensorlog.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef SENSORLOG_H -#define SENSORLOG_H - -#include -#include -#include -#include -#include -#include -#include -#include "configuration.h" - -namespace GMapping { - -class SensorLog : public std::list{ - public: - SensorLog(const SensorMap&); - ~SensorLog(); - std::istream& load(std::istream& is); - OrientedPoint boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const; - protected: - const SensorMap& m_sensorMap; - OdometryReading* parseOdometry(std::istream& is, const OdometrySensor* ) const; - RangeReading* parseRange(std::istream& is, const RangeSensor* ) const; -}; - -}; - -#endif diff --git a/slam_gmapping/openslam_gmapping/log/sensorstream.cpp b/slam_gmapping/openslam_gmapping/log/sensorstream.cpp deleted file mode 100644 index 3c27b04..0000000 --- a/slam_gmapping/openslam_gmapping/log/sensorstream.cpp +++ /dev/null @@ -1,155 +0,0 @@ -#include -#include -#include "sensorstream.h" -//#define LINEBUFFER_SIZE 1000000 //for not Cyrill to unbless me, it is better to exagerate :-)) -// Can't declare a buffer that big on the stack. So we'll risk Cyrill's -// unblessing, and make it smaller. -#define LINEBUFFER_SIZE 8192 - -namespace GMapping { - -using namespace std; - -//SensorStream -SensorStream::SensorStream(const SensorMap& sensorMap) :m_sensorMap(sensorMap){} - -SensorStream::~SensorStream(){} - -SensorReading* SensorStream::parseReading(std::istream& is, const SensorMap& smap){ - SensorReading* reading=0; - if (is){ - char buf[LINEBUFFER_SIZE]; - is.getline(buf, LINEBUFFER_SIZE); - istringstream lis(buf); - - string sensorname; - - if (lis){ - lis >>sensorname; - } else - return 0; - - SensorMap::const_iterator it=smap.find(sensorname); - if (it==smap.end()){ - return 0; - } - - Sensor* sensor=it->second; - - OdometrySensor* odometry=dynamic_cast(sensor); - if (odometry) - reading=parseOdometry(lis, odometry); - - RangeSensor* range=dynamic_cast(sensor); - if (range) - reading=parseRange(lis, range); - } - return reading; -} - -OdometryReading* SensorStream::parseOdometry(std::istream& is, const OdometrySensor* osen ){ - OdometryReading* reading=new OdometryReading(osen); - OrientedPoint pose; - OrientedPoint speed; - OrientedPoint accel; - is >> pose.x >> pose.y >> pose.theta; - is >> speed.x >>speed.theta; - speed.y=0; - is >> accel.x; - accel.y=accel.theta=0; - reading->setPose(pose); reading->setSpeed(speed); reading->setAcceleration(accel); - double timestamp, reltimestamp; - string s; - is >> timestamp >>s >> reltimestamp; - reading->setTime(timestamp); - return reading; -} - -RangeReading* SensorStream::parseRange(std::istream& is, const RangeSensor* rs){ - //cerr << __PRETTY_FUNCTION__ << endl; - if(rs->newFormat){ - string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode; - is >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode; - //cerr << " New format laser msg" << endl; - } - unsigned int size; - is >> size; - assert(size==rs->beams().size()); - RangeReading* reading=new RangeReading(rs); - reading->resize(size); - for (unsigned int i=0; i> (*reading)[i]; - } - if (rs->newFormat){ - int reflectionBeams; - is >> reflectionBeams; - double reflection; - for (int i=0; i> reflection; - } - OrientedPoint laserPose; - is >> laserPose.x >> laserPose.y >> laserPose.theta; - OrientedPoint pose; - is >> pose.x >> pose.y >> pose.theta; - reading->setPose(pose); - - if (rs->newFormat){ - string laser_tv, laser_rv, forward_safety_dist, side_safty_dist, turn_axis; - is >> laser_tv >> laser_rv >> forward_safety_dist >> side_safty_dist >> turn_axis; - } -// else { -// double a,b,c; -// is >> a >> b >> c; -// } - double timestamp, reltimestamp; - string s; - is >> timestamp >>s >> reltimestamp; - reading->setTime(timestamp); - return reading; - -} - -//LogSensorStream -LogSensorStream::LogSensorStream(const SensorMap& sensorMap, const SensorLog* log): - SensorStream(sensorMap){ - m_log=log; - assert(m_log); - m_cursor=log->begin(); -} - -LogSensorStream::operator bool() const{ - return m_cursor==m_log->end(); -} - -bool LogSensorStream::rewind(){ - m_cursor=m_log->begin(); - return true; -} - -SensorStream& LogSensorStream::operator >>(const SensorReading*& rd){ - rd=*m_cursor; - m_cursor++; - return *this; -} - -//InputSensorStream -InputSensorStream::InputSensorStream(const SensorMap& sensorMap, std::istream& is): - SensorStream(sensorMap), m_inputStream(is){ -} - -InputSensorStream::operator bool() const{ - return (bool) m_inputStream; -} - -bool InputSensorStream::rewind(){ - //m_inputStream.rewind(); - return false; -} - -SensorStream& InputSensorStream::operator >>(const SensorReading*& reading){ - reading=parseReading(m_inputStream, m_sensorMap); - return *this; -} - -}; - diff --git a/slam_gmapping/openslam_gmapping/log/sensorstream.h b/slam_gmapping/openslam_gmapping/log/sensorstream.h deleted file mode 100644 index 5e25507..0000000 --- a/slam_gmapping/openslam_gmapping/log/sensorstream.h +++ /dev/null @@ -1,47 +0,0 @@ -#ifndef SENSORSTREAM_H -#define SENSORSTREAM_H - -#include -#include "sensorlog.h" - -namespace GMapping { -class SensorStream{ - public: - SensorStream(const SensorMap& sensorMap); - virtual ~SensorStream(); - virtual operator bool() const=0; - virtual bool rewind() = 0 ; - virtual SensorStream& operator >>(const SensorReading*&) = 0; - inline const SensorMap& getSensorMap() const {return m_sensorMap; } - protected: - const SensorMap& m_sensorMap; - static SensorReading* parseReading(std::istream& is, const SensorMap& smap); - static OdometryReading* parseOdometry(std::istream& is, const OdometrySensor* ); - static RangeReading* parseRange(std::istream& is, const RangeSensor* ); -}; - -class InputSensorStream: public SensorStream{ - public: - InputSensorStream(const SensorMap& sensorMap, std::istream& is); - virtual operator bool() const; - virtual bool rewind(); - virtual SensorStream& operator >>(const SensorReading*&); - - //virtual SensorStream& operator >>(SensorLog*& log); - protected: - std::istream& m_inputStream; -}; - -class LogSensorStream: public SensorStream{ - public: - LogSensorStream(const SensorMap& sensorMap, const SensorLog* log); - virtual operator bool() const; - virtual bool rewind(); - virtual SensorStream& operator >>(const SensorReading*&); - protected: - const SensorLog* m_log; - SensorLog::const_iterator m_cursor; -}; - -}; -#endif diff --git a/slam_gmapping/openslam_gmapping/package.xml b/slam_gmapping/openslam_gmapping/package.xml deleted file mode 100644 index 23ef119..0000000 --- a/slam_gmapping/openslam_gmapping/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - openslam_gmapping - 0.1.2 - ROS-ified version of gmapping SLAM. Forked from https://openslam.informatik.uni-freiburg.de/data/svn/gmapping/trunk/ - Vincent Rabaud - CreativeCommons-by-nc-sa-2.0 - - http://openslam.org/gmapping - https://github.com/ros-perception/openslam_gmapping - https://github.com/ros-perception/openslam_gmapping/issues - - Giorgio Grisetti - Cyrill Stachniss - Wolfram Burgard - - ament_lint_auto - ament_lint_common - - - ament_cmake - - - diff --git a/slam_gmapping/openslam_gmapping/particlefilter/particlefilter.cpp b/slam_gmapping/openslam_gmapping/particlefilter/particlefilter.cpp deleted file mode 100644 index e49999a..0000000 --- a/slam_gmapping/openslam_gmapping/particlefilter/particlefilter.cpp +++ /dev/null @@ -1,133 +0,0 @@ -std::vector sistematicResampler::resample(const vector& particles) const{ - Numeric cweight=0; - - //compute the cumulative weights - unsigned int n=0; - for (vector::const_iterator it=particles.begin(); it!=particles.end(); ++it){ - cweight+=it->weight; - n++; - } - - //compute the interval - Numeric interval=cweight/n; - - //compute the initial target weight - Numeric target= - //compute the resampled indexes - - cweight=0; - std::vector indexes(n); - n=0; - unsigned int i=0; - for (vector::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){ - cweight+=it->weight; - while(cweight>target){ - indexes[n++]=i; - target+=interval; - } - } - return indexes; - } - -template -std::vector indexResampler::resample(const vector >& weights) const{ - Numeric cweight=0; - - //compute the cumulative weights - unsigned int n=0; - for (vector::const_iterator it=weights.begin(); it!=weights.end(); ++it){ - cweight+=*it; - n++; - } - - //compute the interval - Numeric interval=cweight/n; - - //compute the initial target weight - Numeric target= - //compute the resampled indexes - - cweight=0; - std::vector indexes(n); - n=0; - unsigned int i=0; - for (vector::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){ - cweight+=it->weight; - while(cweight>target){ - indexes[n++]=i; - target+=interval; - } - } - return indexes; -} - -/* - -The following are patterns for the evolution and the observation classes -The user should implement classes having the specified meaning - -template -struct observer{ - Observation& observation - Numeric observe(const class State&) const; -}; - -template -struct evolver{ - Input& input; - State& evolve(const State& s); -}; -*/ - -template -void evolver::evolve(std::vector& particles) const{ - for (std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it) - *it=evolutionModel.evolve(*it); -} - -void evolver::evolve(std::vector& dest, const std::vector& src) const{ - dest.clear(); - for (std::vector::const_iterator it=src.begin(); it!=src.end(); ++it) - dest.push_back(evolutionModel.evolve(*it)); -} - -template -struct auxiliaryEvolver{ - typedef particle Particle; - - EvolutionModel evolutionModel; - QualificationModel qualificationModel; - LikelyhoodModel likelyhoodModel; - indexResampler resampler; - -void auxiliaryEvolver::evolve - (std::vector&particles){ - std::vector observationWeights(particles.size()); - unsigned int i=0; - for (std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it, i++){ - observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it)); - } - std::vector indexes(indexResampler.resample(observationWeights)); - for (std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); it++){ - Particle & particle=particles[*it]; - particle=evolutionModel.evolve(particle); - particle.weight*=lykelyhoodModel.lykelyhood(particle)/observationWeights[*it]; - } -} - -void auxiliaryEvolver::evolve - (std::vector& dest, const std::vector& src){ - dest.clear(); - std::vector observationWeights(particles.size()); - unsigned int i=0; - for (std::vector::const_iterator it=src.begin(); it!=src.end(); ++it, i++){ - observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it)); - } - std::vector indexes(indexResampler.resample(observationWeights)); - for (std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); it++){ - Particle & particle=src[*it]; - dest.push_back(evolutionModel.evolve(particle)); - dest.back().weight*=likelyhoodModel.lykelyhood(particle)/observationWeights[*it]; - } - return dest(); -} diff --git a/slam_gmapping/openslam_gmapping/particlefilter/particlefilter_test.cpp b/slam_gmapping/openslam_gmapping/particlefilter/particlefilter_test.cpp deleted file mode 100644 index 28defed..0000000 --- a/slam_gmapping/openslam_gmapping/particlefilter/particlefilter_test.cpp +++ /dev/null @@ -1,98 +0,0 @@ -#include -#include -#include -#include "particlefilter.h" - -using namespace std; - -#define test(s) {cout << s << " " << flush;} -#define testOk() {cout << "OK" << endl;} - -struct Particle{ - double p; - double w; - inline operator double() const {return w;} - inline void setWeight(double _w) {w=_w;} -}; - -ostream& printParticles(ostream& os, const vector& p) -{ - for (vector::const_iterator it=p.begin(); it!=p.end(); ++it) { - os << it->p<< " " << (double)*it << endl; - } - return os; -} - -struct EvolutionModel{ - Particle evolve(const Particle& p){ - Particle pn(p); - pn.p+=.5*(drand48()-.5); - return pn; - } -}; - -struct QualificationModel{ - Particle evolve(const Particle& p){ - return p; - } -}; - -struct LikelyhoodModel{ - double likelyhood(const Particle& p) const{ - double v = 1./(0.1+10*(p.p-2)*(p.p-2))+0.5/(0.1+10*(p.p-8)*(p.p-8)); - return v; - } -}; - -int main (unsigned int argc, const char * const * argv){ - int nparticles=100; - if (argc>1) - nparticles=atoi(argv[1]); - vector particles(nparticles); - LikelyhoodModel likelyhoodModel; - uniform_resampler resampler; - auxiliary_evolver auxevolver; - evolver evolver; - - for (vector::iterator it=particles.begin(); it!=particles.end(); it++){ - it->w=1; - it->p=10*(drand48()); - } - - vector sirparticles(particles); - vector auxparticles(particles); - - /*sir step*/ - while (1){ - char buf[2]; - cin.getline(buf,2); - vector newgeneration; - - cout << "# SIR step" << endl; - evolver.evolve(sirparticles); - for (vector::iterator it=sirparticles.begin(); it!=sirparticles.end(); it++){ - it->setWeight(likelyhoodModel.likelyhood(*it)); - } - ofstream os("sir.dat"); - printParticles(os, sirparticles); - os.close(); - newgeneration=resampler.resample(sirparticles); - sirparticles=newgeneration; - - cout << "# AUX step" << endl; - auxevolver.evolve(auxparticles); - for (vector::iterator it=auxparticles.begin(); it!=auxparticles.end(); it++){ - it->setWeight(likelyhoodModel.likelyhood(*it)); - } - os.open("aux.dat"); - printParticles(os, auxparticles); - os.close(); - newgeneration=resampler.resample(auxparticles); - auxparticles=newgeneration; - cout << "plot [0:10][0:10]\"sir.dat\" w impulses" << endl; - cout << "replot 1./(0.1+10*(x-2)*(x-2))+0.5/(0.1+10*(x-8)*(x-8))" << endl; - -// cout << "replot \"aux.dat\" w p" << endl; - } -} - diff --git a/slam_gmapping/openslam_gmapping/particlefilter/pf.h b/slam_gmapping/openslam_gmapping/particlefilter/pf.h deleted file mode 100644 index 374b0d8..0000000 --- a/slam_gmapping/openslam_gmapping/particlefilter/pf.h +++ /dev/null @@ -1,175 +0,0 @@ -#ifndef PARTICLEFILTER_H -#define PARTICLEFILTER_H -#include -#include -#include -#include - - - -/** -the particle class has to be convertible into numeric data type; -That means that a particle must define the Numeric conversion operator; - operator Numeric() const. -that returns the weight, and the method - setWeight(Numeric) -that sets the weight. - -*/ - -typedef std::pair UIntPair; - -template -double toNormalForm(OutputIterator& out, const Iterator & begin, const Iterator & end){ - //determine the maximum - double lmax=-MAXDOUBLE; - for (Iterator it=begin; it!=end; it++){ - lmax=lmax>((double)(*it))? lmax: (double)(*it); - } - //convert to raw form - for (Iterator it=begin; it!=end; it++){ - *out=exp((double)(*it)-lmax); - out++; - } - return lmax; -} - -template -void toLogForm(OutputIterator& out, const Iterator & begin, const Iterator & end, Numeric lmax){ - //determine the maximum - for (Iterator it=begin; it!=end; it++){ - *out=log((Numeric)(*it))-lmax; - out++; - } - return lmax; -} - -template -void resample(std::vector& indexes, const WeightVector& weights, unsigned int nparticles=0){ - double cweight=0; - - //compute the cumulative weights - unsigned int n=0; - for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it){ - cweight+=(double)*it; - n++; - } - - if (nparticles>0) - n=nparticles; - - //compute the interval - double interval=cweight/n; - - //compute the initial target weight - double target=interval*::drand48(); - //compute the resampled indexes - - cweight=0; - indexes.resize(n); - - n=0; - unsigned int i=0; - for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){ - cweight+=(double)* it; - while(cweight>target){ - indexes[n++]=i; - target+=interval; - } - } -} - -template -void normalizeWeights(WeightVector& weights, unsigned int size, double minWeight){ - double wmin=MAXDOUBLE; - double wmax=-MAXDOUBLE; - for (uint i=0; iweights[i]?wmax:weights[i]; - } - double min_normalized_value=log(minWeight); - double max_normalized_value=log(1.); - double dn=max_normalized_value-min_normalized_value; - double dw=wmax-wmin; - if (dw==0) dw=1; - double scale=dn/dw; - double offset=-wmax*scale; - for (uint i=0; i -void repeatIndexes(Vector& dest, const std::vector& indexes, const Vector& particles){ -/*<<<<<<< .mine - assert(indexes.size()==particles.size()); - if (dest.size()!=particles.size()) - dest.resize(particles.size()); -=======*/ - //assert(indexes.size()==particles.size()); //DIEGO non ne vedo il senso, anzi è sbagliata - //dest.resize(particles.size()); // è sbagliato anche questo - dest.resize(indexes.size()); -// >>>>>>> .r2534 - unsigned int i=0; - for (std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){ - dest[i]=particles[*it]; - i++; - } -} - -template -void repeatIndexes(Vector& dest, const std::vector& indexes2, const Vector& particles, const std::vector& indexes){ - // assert(indexes.size()==indexes2.size()); - dest=particles; - unsigned int i=0; - for (std::vector::const_iterator it=indexes2.begin(); it!=indexes2.end(); ++it){ - dest[indexes[i]]=particles[*it]; - i++; - } -} - - -template -double neff(const Iterator& begin, const Iterator& end){ - double sum=0; - for (Iterator it=begin; it!=end; ++it){ - sum+=*it; - } - double cum=0; - for (Iterator it=begin; it!=end; ++it){ - double w=*it/sum; - cum+=w*w; - } - return 1./cum; -} - - - -template -void rle(OutputIterator& out, const Iterator & begin, const Iterator & end){ - unsigned int current=0; - unsigned int count=0; - for (Iterator it=begin; it!=end; it++){ - if (it==begin){ - current=*it; - count=1; - continue; - } - if (((uint)*it) ==current) - count++; - if (((uint)*it)!=current){ - *out=std::make_pair(current,count); - out++; - current=*it; - count=1; - } - } - if (count>0) - *out=std::make_pair(current,count); - out++; -} - -#endif - diff --git a/slam_gmapping/openslam_gmapping/particlefilter/range_bearing.cpp b/slam_gmapping/openslam_gmapping/particlefilter/range_bearing.cpp deleted file mode 100644 index 44f99e0..0000000 --- a/slam_gmapping/openslam_gmapping/particlefilter/range_bearing.cpp +++ /dev/null @@ -1,99 +0,0 @@ -#include -#include -#include -#include -#include "particlefilter.h" - -using namespace std; -using namespace GMapping; - -#define test(s) {cout << s << " " << flush;} -#define testOk() {cout << "OK" << endl;} - -struct Particle{ - Particle(): p(0,0), w(0){} - Point p; - double w; - operator double() const {return w; } - void setWeight(double _w) {w=_w;} -}; - -ostream& printParticles(ostream& os, const vector& p) -{ - for (vector::const_iterator it=p.begin(); it!=p.end(); ++it) { - os << it->p.x << " " << it->p.y << endl; - } - return os; -} - -struct EvolutionModel{ - Particle evolve(const Particle& p){ - Particle pn(p); - pn.p.x+=10*(drand48()-.5); - pn.p.y+=10*(drand48()-.5); - return pn; - } -}; - - -struct LikelyhoodModel{ - std::vector observerVector; - std::vector observations; - double sigma; - double likelyhood(const Particle& p) const{ - double v=1; - std::vector::const_iterator oit=observations.begin(); - for (std::vector::const_iterator it=observerVector.begin(); it!=observerVector.end();it++){ - v*=exp(-pow(((p.p-*it)*(p.p-*it)-*oit*(*oit))/sigma, 2)); - oit++; - } - cout << "#v=" << v << endl; - return v; - } -}; - -int main (unsigned int argc, const char * const * argv){ - vector particles(1000); - LikelyhoodModel likelyhoodModel; - uniform_resampler resampler; - evolver evolver; - - for (vector::iterator it=particles.begin(); it!=particles.end(); it++){ - it->w=1; - it->p.x=400*(drand48()-.5); - it->p.y=400*(drand48()-.5); - } - - vector sensors; - - sensors.push_back(Point(-50,0)); - sensors.push_back(Point(50,0)); - sensors.push_back(Point(0,100)); - - likelyhoodModel.sigma=1000; - likelyhoodModel.observations.push_back(70); - likelyhoodModel.observations.push_back(70); - likelyhoodModel.observations.push_back(70); - - likelyhoodModel.observerVector=sensors; - while (1){ - char buf[2]; - cin.getline(buf,2); - vector newgeneration; - - cout << "# SIR step" << endl; - evolver.evolve(particles); - for (vector::iterator it=particles.begin(); it!=particles.end(); it++){ - it->w*=likelyhoodModel.likelyhood(*it); - } - - ofstream os("sir.dat"); - printParticles(os, particles); - os.close(); - vector newpart=resampler.resample(particles); - particles=newpart; - - cout << "plot [-200:200][-200:200]\"sir.dat\" w p" << endl; - } -} - diff --git a/slam_gmapping/openslam_gmapping/scanmatcher/CMakeLists.txt b/slam_gmapping/openslam_gmapping/scanmatcher/CMakeLists.txt deleted file mode 100644 index 14993b4..0000000 --- a/slam_gmapping/openslam_gmapping/scanmatcher/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -add_library(scanmatcher STATIC eig3.cpp scanmatcher.cpp scanmatcherprocessor.cpp smmap.cpp) -target_link_libraries(scanmatcher sensor_range utils) - -install(TARGETS scanmatcher DESTINATION lib) diff --git a/slam_gmapping/openslam_gmapping/scanmatcher/eig3.cpp b/slam_gmapping/openslam_gmapping/scanmatcher/eig3.cpp deleted file mode 100644 index 2c9e409..0000000 --- a/slam_gmapping/openslam_gmapping/scanmatcher/eig3.cpp +++ /dev/null @@ -1,270 +0,0 @@ - -/* Eigen decomposition code for symmetric 3x3 matrices, copied from the public - domain Java Matrix library JAMA. */ - -#include - -#ifndef MAX -#define MAX(a, b) ((a)>(b)?(a):(b)) -#endif - -#define n 3 - -static double hypot2(double x, double y) { - return sqrt(x*x+y*y); -} - -// Symmetric Householder reduction to tridiagonal form. - -static void tred2(double V[n][n], double d[n], double e[n]) { - -// This is derived from the Algol procedures tred2 by -// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for -// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding -// Fortran subroutine in EISPACK. - - int i,j,k; - double f,g,h,hh; - for (j = 0; j < n; j++) { - d[j] = V[n-1][j]; - } - - // Householder reduction to tridiagonal form. - - for (i = n-1; i > 0; i--) { - - // Scale to avoid under/overflow. - - double scale = 0.0; - double h = 0.0; - for (k = 0; k < i; k++) { - scale = scale + fabs(d[k]); - } - if (scale == 0.0) { - e[i] = d[i-1]; - for (j = 0; j < i; j++) { - d[j] = V[i-1][j]; - V[i][j] = 0.0; - V[j][i] = 0.0; - } - } else { - - // Generate Householder vector. - - for (k = 0; k < i; k++) { - d[k] /= scale; - h += d[k] * d[k]; - } - f = d[i-1]; - g = sqrt(h); - if (f > 0) { - g = -g; - } - e[i] = scale * g; - h = h - f * g; - d[i-1] = f - g; - for (j = 0; j < i; j++) { - e[j] = 0.0; - } - - // Apply similarity transformation to remaining columns. - - for (j = 0; j < i; j++) { - f = d[j]; - V[j][i] = f; - g = e[j] + V[j][j] * f; - for (k = j+1; k <= i-1; k++) { - g += V[k][j] * d[k]; - e[k] += V[k][j] * f; - } - e[j] = g; - } - f = 0.0; - for (j = 0; j < i; j++) { - e[j] /= h; - f += e[j] * d[j]; - } - hh = f / (h + h); - for (j = 0; j < i; j++) { - e[j] -= hh * d[j]; - } - for (j = 0; j < i; j++) { - f = d[j]; - g = e[j]; - for (k = j; k <= i-1; k++) { - V[k][j] -= (f * e[k] + g * d[k]); - } - d[j] = V[i-1][j]; - V[i][j] = 0.0; - } - } - d[i] = h; - } - - // Accumulate transformations. - - for (i = 0; i < n-1; i++) { - V[n-1][i] = V[i][i]; - V[i][i] = 1.0; - h = d[i+1]; - if (h != 0.0) { - for (k = 0; k <= i; k++) { - d[k] = V[k][i+1] / h; - } - for (j = 0; j <= i; j++) { - g = 0.0; - for (k = 0; k <= i; k++) { - g += V[k][i+1] * V[k][j]; - } - for (k = 0; k <= i; k++) { - V[k][j] -= g * d[k]; - } - } - } - for (k = 0; k <= i; k++) { - V[k][i+1] = 0.0; - } - } - for (j = 0; j < n; j++) { - d[j] = V[n-1][j]; - V[n-1][j] = 0.0; - } - V[n-1][n-1] = 1.0; - e[0] = 0.0; -} - -// Symmetric tridiagonal QL algorithm. - -static void tql2(double V[n][n], double d[n], double e[n]) { - -// This is derived from the Algol procedures tql2, by -// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for -// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding -// Fortran subroutine in EISPACK. - - int i,j,m,l,k; - double g,p,r,dl1,h,f,tst1,eps; - double c,c2,c3,el1,s,s2; - - for (i = 1; i < n; i++) { - e[i-1] = e[i]; - } - e[n-1] = 0.0; - - f = 0.0; - tst1 = 0.0; - eps = pow(2.0,-52.0); - for (l = 0; l < n; l++) { - - // Find small subdiagonal element - - tst1 = MAX(tst1,fabs(d[l]) + fabs(e[l])); - m = l; - while (m < n) { - if (fabs(e[m]) <= eps*tst1) { - break; - } - m++; - } - - // If m == l, d[l] is an eigenvalue, - // otherwise, iterate. - - if (m > l) { - int iter = 0; - do { - iter = iter + 1; // (Could check iteration count here.) - - // Compute implicit shift - - g = d[l]; - p = (d[l+1] - g) / (2.0 * e[l]); - r = hypot2(p,1.0); - if (p < 0) { - r = -r; - } - d[l] = e[l] / (p + r); - d[l+1] = e[l] * (p + r); - dl1 = d[l+1]; - h = g - d[l]; - for (i = l+2; i < n; i++) { - d[i] -= h; - } - f = f + h; - - // Implicit QL transformation. - - p = d[m]; - c = 1.0; - c2 = c; - c3 = c; - el1 = e[l+1]; - s = 0.0; - s2 = 0.0; - for (i = m-1; i >= l; i--) { - c3 = c2; - c2 = c; - s2 = s; - g = c * e[i]; - h = c * p; - r = hypot2(p,e[i]); - e[i+1] = s * r; - s = e[i] / r; - c = p / r; - p = c * d[i] - s * g; - d[i+1] = h + s * (c * g + s * d[i]); - - // Accumulate transformation. - - for (k = 0; k < n; k++) { - h = V[k][i+1]; - V[k][i+1] = s * V[k][i] + c * h; - V[k][i] = c * V[k][i] - s * h; - } - } - p = -s * s2 * c3 * el1 * e[l] / dl1; - e[l] = s * p; - d[l] = c * p; - - // Check for convergence. - - } while (fabs(e[l]) > eps*tst1); - } - d[l] = d[l] + f; - e[l] = 0.0; - } - - // Sort eigenvalues and corresponding vectors. - - for (i = 0; i < n-1; i++) { - k = i; - p = d[i]; - for (j = i+1; j < n; j++) { - if (d[j] < p) { - k = j; - p = d[j]; - } - } - if (k != i) { - d[k] = d[i]; - d[i] = p; - for (j = 0; j < n; j++) { - p = V[j][i]; - V[j][i] = V[j][k]; - V[j][k] = p; - } - } - } -} - -void eigen_decomposition(double A[n][n], double V[n][n], double d[n]) { - int i,j; - double e[n]; - for (i = 0; i < n; i++) { - for (j = 0; j < n; j++) { - V[i][j] = A[i][j]; - } - } - tred2(V, d, e); - tql2(V, d, e); -} diff --git a/slam_gmapping/openslam_gmapping/scanmatcher/eig3.h b/slam_gmapping/openslam_gmapping/scanmatcher/eig3.h deleted file mode 100644 index b708cb1..0000000 --- a/slam_gmapping/openslam_gmapping/scanmatcher/eig3.h +++ /dev/null @@ -1,11 +0,0 @@ - -/* Eigen-decomposition for symmetric 3x3 real matrices. - Public domain, copied from the public domain Java library JAMA. */ - -#ifndef _eig_h - -/* Symmetric matrix A => eigenvectors in columns of V, corresponding - eigenvalues in d. */ -void eigen_decomposition(double A[3][3], double V[3][3], double d[3]); - -#endif diff --git a/slam_gmapping/openslam_gmapping/scanmatcher/gridlinetraversal.h b/slam_gmapping/openslam_gmapping/scanmatcher/gridlinetraversal.h deleted file mode 100644 index 2eb34a8..0000000 --- a/slam_gmapping/openslam_gmapping/scanmatcher/gridlinetraversal.h +++ /dev/null @@ -1,128 +0,0 @@ -#ifndef GRIDLINETRAVERSAL_H -#define GRIDLINETRAVERSAL_H - -#include -#include - -namespace GMapping { - -typedef struct { - int num_points; - IntPoint* points; -} GridLineTraversalLine; - -struct GridLineTraversal { - inline static void gridLine( IntPoint start, IntPoint end, GridLineTraversalLine *line ) ; - inline static void gridLineCore( IntPoint start, IntPoint end, GridLineTraversalLine *line ) ; - -}; - -void GridLineTraversal::gridLineCore( IntPoint start, IntPoint end, GridLineTraversalLine *line ) -{ - int dx, dy, incr1, incr2, d, x, y, xend, yend, xdirflag, ydirflag; - int cnt = 0; - - dx = abs(end.x-start.x); dy = abs(end.y-start.y); - - if (dy <= dx) { - d = 2*dy - dx; incr1 = 2 * dy; incr2 = 2 * (dy - dx); - if (start.x > end.x) { - x = end.x; y = end.y; - ydirflag = (-1); - xend = start.x; - } else { - x = start.x; y = start.y; - ydirflag = 1; - xend = end.x; - } - line->points[cnt].x=x; - line->points[cnt].y=y; - cnt++; - if (((end.y - start.y) * ydirflag) > 0) { - while (x < xend) { - x++; - if (d <0) { - d+=incr1; - } else { - y++; d+=incr2; - } - line->points[cnt].x=x; - line->points[cnt].y=y; - cnt++; - } - } else { - while (x < xend) { - x++; - if (d <0) { - d+=incr1; - } else { - y--; d+=incr2; - } - line->points[cnt].x=x; - line->points[cnt].y=y; - cnt++; - } - } - } else { - d = 2*dx - dy; - incr1 = 2*dx; incr2 = 2 * (dx - dy); - if (start.y > end.y) { - y = end.y; x = end.x; - yend = start.y; - xdirflag = (-1); - } else { - y = start.y; x = start.x; - yend = end.y; - xdirflag = 1; - } - line->points[cnt].x=x; - line->points[cnt].y=y; - cnt++; - if (((end.x - start.x) * xdirflag) > 0) { - while (y < yend) { - y++; - if (d <0) { - d+=incr1; - } else { - x++; d+=incr2; - } - line->points[cnt].x=x; - line->points[cnt].y=y; - cnt++; - } - } else { - while (y < yend) { - y++; - if (d <0) { - d+=incr1; - } else { - x--; d+=incr2; - } - line->points[cnt].x=x; - line->points[cnt].y=y; - cnt++; - } - } - } - line->num_points = cnt; -} - -void GridLineTraversal::gridLine( IntPoint start, IntPoint end, GridLineTraversalLine *line ) { - int i,j; - int half; - IntPoint v; - gridLineCore( start, end, line ); - if ( start.x!=line->points[0].x || - start.y!=line->points[0].y ) { - half = line->num_points/2; - for (i=0,j=line->num_points - 1;ipoints[i]; - line->points[i] = line->points[j]; - line->points[j] = v; - } - } -} - -}; - -#endif diff --git a/slam_gmapping/openslam_gmapping/scanmatcher/icptest.cpp b/slam_gmapping/openslam_gmapping/scanmatcher/icptest.cpp deleted file mode 100644 index 41c240f..0000000 --- a/slam_gmapping/openslam_gmapping/scanmatcher/icptest.cpp +++ /dev/null @@ -1,91 +0,0 @@ -#include -#include -#include - -#include -#include - -using namespace GMapping; -using namespace std; - -typedef std::list PointPairList; - -PointPairList generateRandomPointPairs(int size, OrientedPoint t, double noise=0.){ - PointPairList ppl; - double s=sin(t.theta), c=cos(t.theta); - for (int i=0; i> size >> t.x >> t.y >> t.theta; - PointPairList ppl=generateRandomPointPairs(size, t, 3); - OrientedPoint tc; - OrientedPoint ttot(0.,0.,0.); - bool method=true; - while(1){ - char buf[10]; - cerr << "iterate?" << endl; - cin.getline(buf,10); - if (buf[0]=='n') - method=false; - else if (buf[0]=='l') - method=true; - else if (buf[0]!=char(0)) - break; - cout << "plot '-' w l, '-' w p, '-' w p" << endl; - for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){ - cout << it->first.x << " " << it->first.y<< endl; - cout << it->second.x << " " << it->second.y<< endl; - cout << endl; - } - cout << "e" << endl; - for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){ - cout << it->first.x << " " << it->first.y<< endl; - } - cout << "e" << endl; - for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){ - cout << it->second.x << " " << it->second.y<< endl; - } - cout << "e" << endl; - - double error; - if (!method){ - cerr << "Nonlinear Optimization" << endl; - error=icpNonlinearStep(tc,ppl); - }else { - cerr << "Linear Optimization" << endl; - error=icpStep(tc,ppl); - } - cerr << "ICP err=" << error << " t.x=" << tc.x << " t.y=" << tc.y << " t.theta=" << tc.theta << endl; - cerr << "\t" << error << " ttot.x=" << ttot.x << " ttot.y=" << ttot.y << " ttot.theta=" << ttot.theta << endl; - double s=sin(tc.theta), c=cos(tc.theta); - for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){ - Point p1(c*it->first.x-s*it->first.y+tc.x, - s*it->first.x+c*it->first.y+tc.y); - it->first=p1; - } - ttot.x+=tc.x; - ttot.y+=tc.y; - ttot.theta+=tc.theta; - ttot.theta=atan2(sin(ttot.theta), cos(ttot.theta)); - } - } - return 0; -} diff --git a/slam_gmapping/openslam_gmapping/scanmatcher/lumiles.h b/slam_gmapping/openslam_gmapping/scanmatcher/lumiles.h deleted file mode 100644 index 151dd81..0000000 --- a/slam_gmapping/openslam_gmapping/scanmatcher/lumiles.h +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef LUMILESPROCESSOR -#define LUMILESPROCESSOR - -namespace GMapping{ - -class LuMilesProcessor{ - typedef std:vector PointVector; - static OrientedPoint step(const PointVector& src, const PointVector& dest); -}; - -OrientedPoint LuMilesProcessors::step(const PointVector& src, const PointVector& dest){ - assert(src.size()==dest.size()); - unsigned int size=dest.size(); - double smx=0, smy=0, dmx=0, dmy=0; - for (PointVector::const_iterator it=src.begin(); it!=src.end(); it++){ - smx+=it->x; - smy+=it->y; - } - smx/=src.size(); - smy/=src.size(); - - for (PointVector::const_iterator it=dest.begin(); it!=dest.end(); it++){ - dmx+=it->x; - dmy+=it->y; - } - dmx/=src.size(); - dmy/=src.size(); - - double sxx=0, sxy=0; - double syx=0, syy=0; - for (unsigned int i=0; i -#include -#include -#include -#include -#include -#include -#include -#include "scanmatcherprocessor.h" - -using namespace std; -using namespace GMapping; - -#define DEBUG cout << __PRETTY_FUNCTION__ -#define MAX_STRING_LENGTH 1024 - -int main(int argc, const char * const * argv){ - string filename; - string outfilename; - double xmin=-100.; - double ymin=-100.; - double xmax=100.; - double ymax=100.; - double delta=1.; - double patchDelta=0.1; - double sigma=0.02; - double maxrange=81.9; - double maxUrange=81.9; - double regscore=1e4; - double lstep=.05; - double astep=.05; - int kernelSize=0; - int iterations=4; - double critscore=0.; - double maxMove=1.; - bool computeCovariance=false; - bool readFromStdin=false; - bool useICP=false; - double laserx=.0,lasery=.0,lasertheta=.0; -// bool headingOnly=false; - - - if (argc<2){ - cout << "usage main {arglist}" << endl; - cout << "where the arguments are: " << endl; - cout << "\t -xmin " << endl; - cout << "\t -xmax " << endl; - cout << "\t -ymin " << endl; - cout << "\t -ymax " << endl; - cout << "\t -maxrange : maxmimum preception range" << endl; - cout << "\t -delta : patch size" << endl; - cout << "\t -patchDelta : patch cell size" << endl; - cout << "\t -lstep : linear serach step" << endl; - cout << "\t -astep : ìangular search step" << endl; - cout << "\t -regscore : registration scan score" << endl; - cout << "\t -filename : log filename in carmen format" << endl; - cout << "\t -sigma : convolution kernel size" << endl; - cout << "Look the code for discovering another thousand of unuseful parameters" << endl; - return -1; - } - - CMD_PARSE_BEGIN(1,argc); - parseString("-filename",filename); - parseString("-outfilename",outfilename); - parseDouble("-xmin",xmin); - parseDouble("-xmax",xmax); - parseDouble("-ymin",ymin); - parseDouble("-ymax",ymax); - parseDouble("-delta",delta); - parseDouble("-patchDelta",patchDelta); - parseDouble("-maxrange",maxrange); - parseDouble("-maxUrange",maxUrange); - parseDouble("-regscore",regscore); - parseDouble("-critscore",critscore); - parseInt("-kernelSize",kernelSize); - parseDouble("-sigma",sigma); - parseInt("-iterations",iterations); - parseDouble("-lstep",lstep); - parseDouble("-astep",astep); - parseDouble("-maxMove",maxMove); - parseFlag("-computeCovariance",computeCovariance); - parseFlag("-stdin", readFromStdin); - parseFlag("-useICP", useICP); - parseDouble("-laserx",laserx); - parseDouble("-lasery",lasery); - parseDouble("-lasertheta",lasertheta); - CMD_PARSE_END; - - if (!filename.size()){ - cout << "no filename specified" << endl; - return -1; - } - - ifstream is; - is.open(filename.c_str()); - if (! is){ - cout << "no file found" << endl; - return -1; - } - - - DEBUG << "scanmatcher processor construction" << endl; - ScanMatcherProcessor scanmatcher(xmin, ymin, xmax, ymax, delta, patchDelta); - - //double range, double sigma, int kernsize, double lopt, double aopt, int iterations - scanmatcher.setMatchingParameters(maxUrange, maxrange, sigma, kernelSize, lstep, astep, iterations, computeCovariance); - scanmatcher.setRegistrationParameters(regscore, critscore); - scanmatcher.setmaxMove(maxMove); - scanmatcher.useICP=useICP; - scanmatcher.matcher().setlaserPose(OrientedPoint(laserx,lasery,lasertheta)); - - CarmenConfiguration conf; - conf.load(is); - is.close(); - - SensorMap sensorMap=conf.computeSensorMap(); - scanmatcher.setSensorMap(sensorMap); - - InputSensorStream* input=0; - - ifstream plainStream; - if (! readFromStdin){ - plainStream.open(filename.c_str()); - input=new InputSensorStream(sensorMap, plainStream); - cout << "Plain Stream opened="<< (bool) plainStream << endl; - } else { - input=new InputSensorStream(sensorMap, cin); - cout << "Plain Stream opened on stdin" << endl; - } - -/* - SensorLog log(sensorMap); - ifstream logstream(filename); - log.load(logstream); - logstream.close(); - cout << "Log loaded " << log.size() << " records" << endl; -*/ - ostream* output; - ofstream poseStream; - if (! readFromStdin){ - if (! outfilename.size()){ - outfilename=string("scanmatched")+filename; - } - poseStream.open(outfilename.c_str()); - output=&poseStream; - } else { - output=&cout; - } - scanmatcher.init(); - ofstream odopathStream("odopath.dat"); - while (*input){ - const SensorReading* r; - (*input) >> r; - if (! r) - continue; - const RangeReading* rr=dynamic_cast(r); - if (rr){ - const RangeSensor* s=dynamic_cast(r->getSensor()); - bool isFront= s->getPose().theta==0; - - if (! readFromStdin){ - cout << "." << flush; - } - const RangeSensor* rs=dynamic_cast(rr->getSensor()); - assert (rs && rs->beams().size()==rr->size()); - odopathStream << rr->getPose().x << " " << rr->getPose().y << endl; - scanmatcher.processScan(*rr); - OrientedPoint p=scanmatcher.getPose(); - if (isFront) - *output << "FLASER "<< rr->size() << " "; - else - *output << "RLASER "<< rr->size() << " "; - for (RangeReading::const_iterator b=rr->begin(); b!=rr->end(); b++){ - *output << *b << " "; - } - *output << p.x << " " << p.y << " " << p.theta << " "; - //p=rr->getPose(); - double t=rr->getTime(); //FIXME - *output << p.x << " " << p.y << " " << p.theta << " "; - *output << t << " nohost " << t << endl; - } - } - if (! readFromStdin){ - poseStream.close(); - } -} diff --git a/slam_gmapping/openslam_gmapping/scanmatcher/scanmatcher.cpp b/slam_gmapping/openslam_gmapping/scanmatcher/scanmatcher.cpp deleted file mode 100644 index 8f2c3ac..0000000 --- a/slam_gmapping/openslam_gmapping/scanmatcher/scanmatcher.cpp +++ /dev/null @@ -1,725 +0,0 @@ -#include -#include -#include -#include - -#include -#include "gridlinetraversal.h" -//#define GENERATE_MAPS - -namespace GMapping { - -using namespace std; - -const double ScanMatcher::nullLikelihood=-.5; - -ScanMatcher::ScanMatcher(): m_laserPose(0,0,0){ - //m_laserAngles=0; - m_laserBeams=0; - m_optRecursiveIterations=3; - m_activeAreaComputed=false; - - // This are the dafault settings for a grid map of 5 cm - m_llsamplerange=0.01; - m_llsamplestep=0.01; - m_lasamplerange=0.005; - m_lasamplestep=0.005; - m_enlargeStep=10.; - m_fullnessThreshold=0.1; - m_angularOdometryReliability=0.; - m_linearOdometryReliability=0.; - m_freeCellRatio=sqrt(2.); - m_initialBeamsSkip=0; - -/* - // This are the dafault settings for a grid map of 10 cm - m_llsamplerange=0.1; - m_llsamplestep=0.1; - m_lasamplerange=0.02; - m_lasamplestep=0.01; -*/ - // This are the dafault settings for a grid map of 20/25 cm -/* - m_llsamplerange=0.2; - m_llsamplestep=0.1; - m_lasamplerange=0.02; - m_lasamplestep=0.01; - m_generateMap=false; -*/ - - m_linePoints = new IntPoint[20000]; -} - -ScanMatcher::~ScanMatcher(){ - delete [] m_linePoints; -} - -void ScanMatcher::invalidateActiveArea(){ - m_activeAreaComputed=false; -} - -/* -void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){ - if (m_activeAreaComputed) - return; - HierarchicalArray2D::PointSet activeArea; - OrientedPoint lp=p; - lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y; - lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y; - lp.theta+=m_laserPose.theta; - IntPoint p0=map.world2map(lp); - const double * angle=m_laserAngles; - for (const double* r=readings; rm_laserMaxRange) - continue; - if (d>m_usableRange) - d=m_usableRange; - - Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle)); - IntPoint p1=map.world2map(phit); - - d+=map.getDelta(); - //Point phit2=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle)); - //IntPoint p2=map.world2map(phit2); - IntPoint linePoints[20000] ; - GridLineTraversalLine line; - line.points=linePoints; - //GridLineTraversal::gridLine(p0, p2, &line); - GridLineTraversal::gridLine(p0, p1, &line); - for (int i=0; im_laserMaxRange||*r>m_usableRange) continue; - Point phit=lp; - phit.x+=*r*cos(lp.theta+*angle); - phit.y+=*r*sin(lp.theta+*angle); - IntPoint p1=map.world2map(phit); - assert(p1.x>=0 && p1.y>=0); - IntPoint cp=map.storage().patchIndexes(p1); - assert(cp.x>=0 && cp.y>=0); - activeArea.insert(cp); - - } - //this allocates the unallocated cells in the active area of the map - //cout << "activeArea::size() " << activeArea.size() << endl; - map.storage().setActiveArea(activeArea, true); - m_activeAreaComputed=true; -} -*/ -void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){ - if (m_activeAreaComputed) - return; - OrientedPoint lp=p; - lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y; - lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y; - lp.theta+=m_laserPose.theta; - IntPoint p0=map.world2map(lp); - - Point min(map.map2world(0,0)); - Point max(map.map2world(map.getMapSizeX()-1,map.getMapSizeY()-1)); - - if (lp.xmax.x) max.x=lp.x; - if (lp.y>max.y) max.y=lp.y; - - /*determine the size of the area*/ - const double * angle=m_laserAngles+m_initialBeamsSkip; - for (const double* r=readings+m_initialBeamsSkip; rm_laserMaxRange||*r==0.0||isnan(*r)) continue; - double d=*r>m_usableRange?m_usableRange:*r; - Point phit=lp; - phit.x+=d*cos(lp.theta+*angle); - phit.y+=d*sin(lp.theta+*angle); - if (phit.xmax.x) max.x=phit.x; - if (phit.y>max.y) max.y=phit.y; - } - //min=min-Point(map.getDelta(),map.getDelta()); - //max=max+Point(map.getDelta(),map.getDelta()); - - if ( !map.isInside(min) || !map.isInside(max)){ - Point lmin(map.map2world(0,0)); - Point lmax(map.map2world(map.getMapSizeX()-1,map.getMapSizeY()-1)); - //cerr << "CURRENT MAP " << lmin.x << " " << lmin.y << " " << lmax.x << " " << lmax.y << endl; - //cerr << "BOUNDARY OVERRIDE " << min.x << " " << min.y << " " << max.x << " " << max.y << endl; - min.x=( min.x >= lmin.x )? lmin.x: min.x-m_enlargeStep; - max.x=( max.x <= lmax.x )? lmax.x: max.x+m_enlargeStep; - min.y=( min.y >= lmin.y )? lmin.y: min.y-m_enlargeStep; - max.y=( max.y <= lmax.y )? lmax.y: max.y+m_enlargeStep; - map.resize(min.x, min.y, max.x, max.y); - //cerr << "RESIZE " << min.x << " " << min.y << " " << max.x << " " << max.y << endl; - } - - HierarchicalArray2D::PointSet activeArea; - /*allocate the active area*/ - angle=m_laserAngles+m_initialBeamsSkip; - for (const double* r=readings+m_initialBeamsSkip; rm_laserMaxRange||d==0.0||isnan(d)) - continue; - if (d>m_usableRange) - d=m_usableRange; - Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle)); - IntPoint p0=map.world2map(lp); - IntPoint p1=map.world2map(phit); - - //IntPoint linePoints[20000] ; - GridLineTraversalLine line; - line.points=m_linePoints; - GridLineTraversal::gridLine(p0, p1, &line); - for (int i=0; i=0 && m_linePoints[i].y>=0); - } - if (d=0 && cp.y>=0); - activeArea.insert(cp); - } - } else { - if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue; - Point phit=lp; - phit.x+=*r*cos(lp.theta+*angle); - phit.y+=*r*sin(lp.theta+*angle); - IntPoint p1=map.world2map(phit); - assert(p1.x>=0 && p1.y>=0); - IntPoint cp=map.storage().patchIndexes(p1); - assert(cp.x>=0 && cp.y>=0); - activeArea.insert(cp); - } - - //this allocates the unallocated cells in the active area of the map - //cout << "activeArea::size() " << activeArea.size() << endl; -/* - cerr << "ActiveArea="; - for (HierarchicalArray2D::PointSet::const_iterator it=activeArea.begin(); it!= activeArea.end(); it++){ - cerr << "(" << it->x <<"," << it->y << ") "; - } - cerr << endl; -*/ - map.storage().setActiveArea(activeArea, true); - m_activeAreaComputed=true; -} - -double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){ - if (!m_activeAreaComputed) - computeActiveArea(map, p, readings); - - //this operation replicates the cells that will be changed in the registration operation - map.storage().allocActiveArea(); - - OrientedPoint lp=p; - lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y; - lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y; - lp.theta+=m_laserPose.theta; - IntPoint p0=map.world2map(lp); - - - const double * angle=m_laserAngles+m_initialBeamsSkip; - double esum=0; - for (const double* r=readings+m_initialBeamsSkip; rm_laserMaxRange||d==0.0||isnan(d)) - continue; - if (d>m_usableRange) - d=m_usableRange; - Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle)); - IntPoint p1=map.world2map(phit); - //IntPoint linePoints[20000] ; - GridLineTraversalLine line; - line.points=m_linePoints; - GridLineTraversal::gridLine(p0, p1, &line); - for (int i=0; im_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue; - Point phit=lp; - phit.x+=*r*cos(lp.theta+*angle); - phit.y+=*r*sin(lp.theta+*angle); - IntPoint p1=map.world2map(phit); - assert(p1.x>=0 && p1.y>=0); - map.cell(p1).update(true,phit); - } - //cout << "informationGain=" << -esum << endl; - return esum; -} - -/* -void ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){ - if (!m_activeAreaComputed) - computeActiveArea(map, p, readings); - - //this operation replicates the cells that will be changed in the registration operation - map.storage().allocActiveArea(); - - OrientedPoint lp=p; - lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y; - lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y; - lp.theta+=m_laserPose.theta; - IntPoint p0=map.world2map(lp); - const double * angle=m_laserAngles; - for (const double* r=readings; rm_laserMaxRange) - continue; - if (d>m_usableRange) - d=m_usableRange; - Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle)); - IntPoint p1=map.world2map(phit); - - IntPoint linePoints[20000] ; - GridLineTraversalLine line; - line.points=linePoints; - GridLineTraversal::gridLine(p0, p1, &line); - for (int i=0; im_laserMaxRange||*r>m_usableRange) continue; - Point phit=lp; - phit.x+=*r*cos(lp.theta+*angle); - phit.y+=*r*sin(lp.theta+*angle); - map.cell(phit).update(true,phit); - } -} - -*/ - -double ScanMatcher::icpOptimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{ - double currentScore; - double sc=score(map, init, readings);; - OrientedPoint start=init; - pnew=init; - int iterations=0; - do{ - currentScore=sc; - sc=icpStep(pnew, map, start, readings); - //cerr << "pstart=" << start.x << " " <currentScore); - cerr << "i="<< iterations << endl; - return currentScore; -} - -double ScanMatcher::optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{ - double bestScore=-1; - OrientedPoint currentPose=init; - double currentScore=score(map, currentPose, readings); - double adelta=m_optAngularDelta, ldelta=m_optLinearDelta; - unsigned int refinement=0; - enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done}; -/* cout << __PRETTY_FUNCTION__<< " readings: "; - for (int i=0; i=currentScore){ - refinement++; - adelta*=.5; - ldelta*=.5; - } - bestScore=currentScore; -// cout <<"score="<< currentScore << " refinement=" << refinement; -// cout << "pose=" << currentPose.x << " " << currentPose.y << " " << currentPose.theta << endl; - OrientedPoint bestLocalPose=currentPose; - OrientedPoint localPose=currentPose; - - Move move=Front; - do { - localPose=currentPose; - switch(move){ - case Front: - localPose.x+=ldelta; - move=Back; - break; - case Back: - localPose.x-=ldelta; - move=Left; - break; - case Left: - localPose.y-=ldelta; - move=Right; - break; - case Right: - localPose.y+=ldelta; - move=TurnLeft; - break; - case TurnLeft: - localPose.theta+=adelta; - move=TurnRight; - break; - case TurnRight: - localPose.theta-=adelta; - move=Done; - break; - default:; - } - - double odo_gain=1; - if (m_angularOdometryReliability>0.){ - double dth=init.theta-localPose.theta; dth=atan2(sin(dth), cos(dth)); dth*=dth; - odo_gain*=exp(-m_angularOdometryReliability*dth); - } - if (m_linearOdometryReliability>0.){ - double dx=init.x-localPose.x; - double dy=init.y-localPose.y; - double drho=dx*dx+dy*dy; - odo_gain*=exp(-m_linearOdometryReliability*drho); - } - double localScore=odo_gain*score(map, localPose, readings); - - if (localScore>currentScore){ - currentScore=localScore; - bestLocalPose=localPose; - } - c_iterations++; - } while(move!=Done); - currentPose=bestLocalPose; -// cout << "currentScore=" << currentScore<< endl; - //here we look for the best move; - }while (currentScore>bestScore || refinement0.){ - double dth=init.theta-localPose.theta; dth=atan2(sin(dth), cos(dth)); dth*=dth; - odo_gain*=exp(-m_angularOdometryReliability*dth); - } - if (m_linearOdometryReliability>0.){ - double dx=init.x-localPose.x; - double dy=init.y-localPose.y; - double drho=dx*dx+dy*dy; - odo_gain*=exp(-m_linearOdometryReliability*drho); - } - localScore=odo_gain*score(map, localPose, readings); - //update the score - count++; - matched=likelihoodAndScore(localScore, localLikelihood, map, localPose, readings); - if (localScore>currentScore){ - currentScore=localScore; - bestLocalPose=localPose; - } - sm.score=localScore; - sm.likelihood=localLikelihood;//+log(odo_gain); - sm.pose=localPose; - moveList.push_back(sm); - //update the move list - } while(move!=Done); - currentPose=bestLocalPose; - //cout << __PRETTY_FUNCTION__ << "currentScore=" << currentScore<< endl; - //here we look for the best move; - }while (currentScore>bestScore || refinement::PointSet activeArea; - OrientedPoint lp=p; - lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y; - lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y; - lp.theta+=m_laserPose.theta; - IntPoint p0=map.world2map(lp); - const double * angle=m_laserAngles; - for (const double* r=readings; rm_laserMaxRange) - continue; - if (d>m_usableRange) - d=m_usableRange; - - Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle)); - IntPoint p1=map.world2map(phit); - - d+=map.getDelta(); - //Point phit2=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle)); - //IntPoint p2=map.world2map(phit2); - IntPoint linePoints[20000] ; - GridLineTraversalLine line; - line.points=linePoints; - //GridLineTraversal::gridLine(p0, p2, &line); - GridLineTraversal::gridLine(p0, p1, &line); - for (int i=0; im_laserMaxRange||*r>m_usableRange) continue; - Point phit=lp; - phit.x+=*r*cos(lp.theta+*angle); - phit.y+=*r*sin(lp.theta+*angle); - IntPoint p1=map.world2map(phit); - assert(p1.x>=0 && p1.y>=0); - IntPoint cp=map.storage().patchIndexes(p1); - assert(cp.x>=0 && cp.y>=0); - activeArea.insert(cp); - - } - //this allocates the unallocated cells in the active area of the map - //cout << "activeArea::size() " << activeArea.size() << endl; - map.storage().setActiveArea(activeArea, true); - m_activeAreaComputed=true; -} - -void ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){ - if (!m_activeAreaComputed) - computeActiveArea(map, p, readings); - - //this operation replicates the cells that will be changed in the registration operation - map.storage().allocActiveArea(); - - OrientedPoint lp=p; - lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y; - lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y; - lp.theta+=m_laserPose.theta; - IntPoint p0=map.world2map(lp); - const double * angle=m_laserAngles; - for (const double* r=readings; rm_laserMaxRange) - continue; - if (d>m_usableRange) - d=m_usableRange; - Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle)); - IntPoint p1=map.world2map(phit); - - d+=map.getDelta(); - //Point phit2=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle)); - //IntPoint p2=map.world2map(phit2); - IntPoint linePoints[20000] ; - GridLineTraversalLine line; - line.points=linePoints; - //GridLineTraversal::gridLine(p0, p2, &line); - GridLineTraversal::gridLine(p0, p1, &line); - for (int i=0; im_laserMaxRange||*r>m_usableRange) continue; - Point phit=lp; - phit.x+=*r*cos(lp.theta+*angle); - phit.y+=*r*sin(lp.theta+*angle); - map.cell(phit).update(true,phit); - } -} - - - -double ScanMatcher::optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{ - double bestScore=-1; - OrientedPoint currentPose=init; - double currentScore=score(map, currentPose, readings); - double adelta=m_optAngularDelta, ldelta=m_optLinearDelta; - unsigned int refinement=0; - enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done}; - int c_iterations=0; - do{ - if (bestScore>=currentScore){ - refinement++; - adelta*=.5; - ldelta*=.5; - } - bestScore=currentScore; -// cout <<"score="<< currentScore << " refinement=" << refinement; -// cout << "pose=" << currentPose.x << " " << currentPose.y << " " << currentPose.theta << endl; - OrientedPoint bestLocalPose=currentPose; - OrientedPoint localPose=currentPose; - - Move move=Front; - do { - localPose=currentPose; - switch(move){ - case Front: - localPose.x+=ldelta; - move=Back; - break; - case Back: - localPose.x-=ldelta; - move=Left; - break; - case Left: - localPose.y-=ldelta; - move=Right; - break; - case Right: - localPose.y+=ldelta; - move=TurnLeft; - break; - case TurnLeft: - localPose.theta+=adelta; - move=TurnRight; - break; - case TurnRight: - localPose.theta-=adelta; - move=Done; - break; - default:; - } - double localScore=score(map, localPose, readings); - if (localScore>currentScore){ - currentScore=localScore; - bestLocalPose=localPose; - } - c_iterations++; - } while(move!=Done); - currentPose=bestLocalPose; - //cout << __PRETTY_FUNCTION__ << "currentScore=" << currentScore<< endl; - //here we look for the best move; - }while (currentScore>bestScore || refinementcurrentScore){ - currentScore=localScore; - bestLocalPose=localPose; - } - sm.score=localScore; - sm.likelihood=localLikelihood; - sm.pose=localPose; - moveList.push_back(sm); - //update the move list - } while(move!=Done); - currentPose=bestLocalPose; - //cout << __PRETTY_FUNCTION__ << "currentScore=" << currentScore<< endl; - //here we look for the best move; - }while (currentScore>bestScore || refinement((laser_it->second)); - assert(rangeSensor && rangeSensor->beams().size()); - - m_beams=static_cast(rangeSensor->beams().size()); - double* angles=new double[rangeSensor->beams().size()]; - for (unsigned int i=0; ibeams()[i].pose.theta; - } - m_matcher.setLaserParameters(m_beams, angles, rangeSensor->getPose()); - delete [] angles; - - -} - -void ScanMatcherProcessor::init(){ - m_first=true; - m_pose=OrientedPoint(0,0,0); - m_count=0; -} - -void ScanMatcherProcessor::processScan(const RangeReading & reading){ - /**retireve the position from the reading, and compute the odometry*/ - OrientedPoint relPose=reading.getPose(); - if (!m_count){ - m_odoPose=relPose; - } - - //compute the move in the scan m_matcher - //reference frame - - OrientedPoint move=relPose-m_odoPose; - - double dth=m_odoPose.theta-m_pose.theta; - // cout << "rel-move x="<< move.x << " y=" << move.y << " theta=" << move.theta << endl; - - double lin_move=move*move; - if (lin_move>m_maxMove){ - cerr << "Too big jump in the log file: " << lin_move << endl; - cerr << "relPose=" << relPose.x << " " < - -namespace GMapping { - -const PointAccumulator& PointAccumulator::Unknown(){ - if (! unknown_ptr) - unknown_ptr=new PointAccumulator; - return *unknown_ptr; -} - -PointAccumulator* PointAccumulator::unknown_ptr=0; - -}; - - diff --git a/slam_gmapping/openslam_gmapping/sensor/CMakeLists.txt b/slam_gmapping/openslam_gmapping/sensor/CMakeLists.txt deleted file mode 100644 index 93ddf7a..0000000 --- a/slam_gmapping/openslam_gmapping/sensor/CMakeLists.txt +++ /dev/null @@ -1,8 +0,0 @@ -add_subdirectory(sensor_base) -ament_export_libraries(sensor_base) - -add_subdirectory(sensor_odometry) -ament_export_libraries(sensor_odometry) - -add_subdirectory(sensor_range) -ament_export_libraries(sensor_range) \ No newline at end of file diff --git a/slam_gmapping/openslam_gmapping/sensor/Makefile b/slam_gmapping/openslam_gmapping/sensor/Makefile deleted file mode 100644 index 6b9f784..0000000 --- a/slam_gmapping/openslam_gmapping/sensor/Makefile +++ /dev/null @@ -1,5 +0,0 @@ --include ../global.mk - -SUBDIRS=sensor_base sensor_odometry sensor_range - --include ../build_tools/Makefile.subdirs diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_base/CMakeLists.txt b/slam_gmapping/openslam_gmapping/sensor/sensor_base/CMakeLists.txt deleted file mode 100644 index 1f23da6..0000000 --- a/slam_gmapping/openslam_gmapping/sensor/sensor_base/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -add_library(sensor_base sensor.cpp sensorreading.cpp) -install(TARGETS sensor_base DESTINATION lib) - -ament_export_libraries(sensor_base) \ No newline at end of file diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_base/sensor.cpp b/slam_gmapping/openslam_gmapping/sensor/sensor_base/sensor.cpp deleted file mode 100644 index d7a2ef7..0000000 --- a/slam_gmapping/openslam_gmapping/sensor/sensor_base/sensor.cpp +++ /dev/null @@ -1,12 +0,0 @@ -#include - -namespace GMapping{ - -Sensor::Sensor(const std::string& name){ - m_name=name; -} - -Sensor::~Sensor(){ -} - -};// end namespace diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_base/sensoreading.h b/slam_gmapping/openslam_gmapping/sensor/sensor_base/sensoreading.h deleted file mode 100644 index 217ec6f..0000000 --- a/slam_gmapping/openslam_gmapping/sensor/sensor_base/sensoreading.h +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef SENSORREADING_H -#define SENSORREADING_H - -#include "sensor.h" -namespace GMapping{ - -class SensorReading{ - public: - SensorReading(const Sensor* s=0, double time=0); - inline double getTime() const {return m_time;} - inline const Sensor* getSensor() const {return m_sensor;} - protected: - double m_time; - const Sensor* m_sensor; -}; - -}; //end namespace -#endif diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_base/sensorreading.cpp b/slam_gmapping/openslam_gmapping/sensor/sensor_base/sensorreading.cpp deleted file mode 100644 index aa703ac..0000000 --- a/slam_gmapping/openslam_gmapping/sensor/sensor_base/sensorreading.cpp +++ /dev/null @@ -1,15 +0,0 @@ -#include - -namespace GMapping{ - -//SensorReading::SensorReading(const Sensor* s, double t){ -// m_sensor=s; -// m_time=t; -//} -// -// -//SensorReading::~SensorReading(){ -//} - -}; - diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_odometry/CMakeLists.txt b/slam_gmapping/openslam_gmapping/sensor/sensor_odometry/CMakeLists.txt deleted file mode 100644 index ed4cc4b..0000000 --- a/slam_gmapping/openslam_gmapping/sensor/sensor_odometry/CMakeLists.txt +++ /dev/null @@ -1,6 +0,0 @@ -add_library(sensor_odometry odometryreading.cpp odometrysensor.cpp) -target_link_libraries(sensor_odometry sensor_base) - -install(TARGETS sensor_odometry DESTINATION lib) - -ament_export_libraries(sensor_odometry) diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_odometry/odometryreading.cpp b/slam_gmapping/openslam_gmapping/sensor/sensor_odometry/odometryreading.cpp deleted file mode 100644 index 1d76b8b..0000000 --- a/slam_gmapping/openslam_gmapping/sensor/sensor_odometry/odometryreading.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include - -namespace GMapping{ - -OdometryReading::OdometryReading(const OdometrySensor* odo, double time): - SensorReading(odo,time){} - -}; - diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_odometry/odometrysensor.cpp b/slam_gmapping/openslam_gmapping/sensor/sensor_odometry/odometrysensor.cpp deleted file mode 100644 index 1cd98e0..0000000 --- a/slam_gmapping/openslam_gmapping/sensor/sensor_odometry/odometrysensor.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include - -namespace GMapping{ - -OdometrySensor::OdometrySensor(const std::string& name, bool ideal): Sensor(name){ m_ideal=ideal;} - - -}; - diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_range/CMakeLists.txt b/slam_gmapping/openslam_gmapping/sensor/sensor_range/CMakeLists.txt deleted file mode 100644 index 5e1c9ba..0000000 --- a/slam_gmapping/openslam_gmapping/sensor/sensor_range/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -add_library(sensor_range rangereading.cpp rangesensor.cpp) -target_link_libraries(sensor_range sensor_base) -#ament_target_dependencies(sensor_range sensor_base) - -install(TARGETS sensor_range DESTINATION lib) - -ament_export_libraries(sensor_range) \ No newline at end of file diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_range/rangereading.cpp b/slam_gmapping/openslam_gmapping/sensor/sensor_range/rangereading.cpp deleted file mode 100644 index 6bb8946..0000000 --- a/slam_gmapping/openslam_gmapping/sensor/sensor_range/rangereading.cpp +++ /dev/null @@ -1,114 +0,0 @@ -#include -#include -#include -#include -#include -#include - -namespace GMapping{ - -using namespace std; - -RangeReading::RangeReading(const RangeSensor* rs, double time): - SensorReading(rs,time){} - -RangeReading::RangeReading(unsigned int n_beams, const double* d, const RangeSensor* rs, double time): - SensorReading(rs,time){ - assert(n_beams==rs->beams().size()); - resize(n_beams); - for (unsigned int i=0; i(getSensor()); - assert(rs); - Point lp( - cos(rs->beams()[i].pose.theta)*(*this)[i], - sin(rs->beams()[i].pose.theta)*(*this)[i]); - Point dp=lastPoint-lp; - double distance=sqrt(dp*dp); - if (distance::max(); - suppressed++; - } - else{ - lastPoint=lp; - v[i]=(*this)[i]; - } - //std::cerr<< __PRETTY_FUNCTION__ << std::endl; - //std::cerr<< "suppressed " << suppressed <<"/"<(size()); - -}; - -unsigned int RangeReading::activeBeams(double density) const{ - if (density==0.) - return size(); - int ab=0; - Point lastPoint(0,0); - uint suppressed=0; - for (unsigned int i=0; i(getSensor()); - assert(rs); - Point lp( - cos(rs->beams()[i].pose.theta)*(*this)[i], - sin(rs->beams()[i].pose.theta)*(*this)[i]); - Point dp=lastPoint-lp; - double distance=sqrt(dp*dp); - if (distance RangeReading::cartesianForm(double maxRange) const{ - const RangeSensor* rangeSensor=dynamic_cast(getSensor()); - assert(rangeSensor && rangeSensor->beams().size()); - // uint m_beams=rangeSensor->beams().size(); - uint m_beams=static_cast(rangeSensor->beams().size()); - std::vector cartesianPoints(m_beams); - double px,py,ps,pc; - px=rangeSensor->getPose().x; - py=rangeSensor->getPose().y; - ps=sin(rangeSensor->getPose().theta); - pc=cos(rangeSensor->getPose().theta); - for (unsigned int i=0; ibeams()[i].s; - const double& c=rangeSensor->beams()[i].c; - if (rho>=maxRange){ - cartesianPoints[i]=Point(0,0); - } else { - Point p=Point(rangeSensor->beams()[i].pose.x+c*rho, rangeSensor->beams()[i].pose.y+s*rho); - cartesianPoints[i].x=px+pc*p.x-ps*p.y; - cartesianPoints[i].y=py+ps*p.x+pc*p.y; - } - } - return cartesianPoints; -} - -}; - diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_range/rangesensor.cpp b/slam_gmapping/openslam_gmapping/sensor/sensor_range/rangesensor.cpp deleted file mode 100644 index dc7ff1a..0000000 --- a/slam_gmapping/openslam_gmapping/sensor/sensor_range/rangesensor.cpp +++ /dev/null @@ -1,30 +0,0 @@ -#include - -namespace GMapping{ - -RangeSensor::RangeSensor(std::string name): Sensor(name){} - -RangeSensor::RangeSensor(std::string name, unsigned int beams_num, double res, const OrientedPoint& position, double span, double maxrange):Sensor(name), - m_pose(position), m_beams(beams_num){ - double angle=-.5*res*beams_num; - for (unsigned int i=0; i -#include "autoptr.h" - -using namespace std; -using namespace GMapping; - -typedef autoptr DoubleAutoPtr; - -int main(int argc, const char * const * argv){ - double* d1=new double(10.); - double* d2=new double(20.); - cout << "Construction test" << endl; - DoubleAutoPtr pd1(d1); - DoubleAutoPtr pd2(d2); - cout << *pd1 << " " << *pd2 << endl; - cout << "Copy Construction" << endl; - DoubleAutoPtr pd3(pd1); - cout << *pd3 << endl; - cout << "assignment" << endl; - pd3=pd2; - pd1=pd2; - cout << *pd1 << " " << *pd2 << " " << *pd3 << " " << endl; - cout << "conversion operator" << endl; - DoubleAutoPtr nullPtr; - cout << "conversion operator " << !(nullPtr) << endl; - cout << "neg conversion operator " << nullPtr << endl; - cout << "conversion operator " << (int)pd1 << endl; - cout << "neg conversion operator " << !(pd1) << endl; -} diff --git a/slam_gmapping/openslam_gmapping/utils/datasmoother.h b/slam_gmapping/openslam_gmapping/utils/datasmoother.h deleted file mode 100644 index ddd8553..0000000 --- a/slam_gmapping/openslam_gmapping/utils/datasmoother.h +++ /dev/null @@ -1,451 +0,0 @@ -#ifndef DATASMOOTHER_H -#define DATASMOOTHER_H - -#include -#include -#include -#include -#include "stat.h" -#include - -namespace GMapping { - -class DataSmoother { - public: - struct DataPoint { - DataPoint(double _x=0.0, double _y=0.0) { x=_x;y=_y;} - double x; - double y; - }; - - typedef std::vector Data; - - DataSmoother(double parzenWindow) { - init(parzenWindow); - }; - - virtual ~DataSmoother() { - m_data.clear(); - m_cummulated.clear(); - }; - - void init(double parzenWindow) { - m_data.clear(); - m_cummulated.clear(); - m_int=-1; - m_parzenWindow = parzenWindow; - m_from = MAXDOUBLE; - m_to = -MAXDOUBLE; - m_lastStep = 0.001; - }; - - - double sqr(double x) { - return x*x; - } - - - void setMinToZero() { - double minval=MAXDOUBLE; - - for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { - const DataPoint& d = *it; - if (minval > d.y) - minval = d.y; - } - - for (Data::iterator it = m_data.begin(); it != m_data.end(); it++) { - DataPoint& d = *it; - d.y = d.y - minval; - } - - m_cummulated.clear(); - } - - void add(double x, double p) { - m_data.push_back(DataPoint(x,p)); - m_int=-1; - - if (x-3.0*m_parzenWindow < m_from) - m_from = x - 3.0*m_parzenWindow; - - if (x+3.0*m_parzenWindow > m_to) - m_to = x + 3.0*m_parzenWindow; - - m_cummulated.clear(); - } - - void integrate(double step) { - m_lastStep = step; - double sum=0; - for (double x=m_from; x<=m_to; x+=step) - sum += smoothedData(x)*step; - m_int = sum; - } - - double integral(double step, double xTo) { - double sum=0; - for (double x=m_from; x<=xTo; x+=step) - sum += smoothedData(x)*step; - return sum; - } - - - double smoothedData(double x) { - assert( m_data.size() > 0 ); - - double p=0; - double sum_y=0; - for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { - const DataPoint& d = *it; - double dist = fabs(x - d.x); - p += d.y * exp( -0.5 * sqr ( dist/m_parzenWindow ) ); - sum_y += d.y; - } - double denom = sqrt(2.0 * M_PI) * (sum_y) * m_parzenWindow; - p *= 1./denom; - - return p; - } - - double sampleNumeric(double step) { - - assert( m_data.size() > 0 ); - - if (m_int <0 || step != m_lastStep) - integrate(step); - - double r = sampleUniformDouble(0.0, m_int); - double sum2=0; - for (double x=m_from; x<=m_to; x+=step) { - sum2 += smoothedData(x)*step; - if (sum2 > r) - return x-0.5*step; - } - return m_to; - } - - void computeCummuated() { - assert( m_data.size() > 0 ); - m_cummulated.resize(m_data.size()); - std::vector::iterator cit = m_cummulated.begin(); - double sum=0; - for (Data::const_iterator it = m_data.begin(); it != m_data.end(); ++it) { - sum += it->y; - (*cit) = sum; - ++cit; - } - } - - double sample() { - - assert( m_data.size() > 0 ); - - if (m_cummulated.size() == 0) { - computeCummuated(); - } - double maxval = m_cummulated.back(); - - double random = sampleUniformDouble(0.0, maxval); - int nCum = (int) m_cummulated.size(); - double sum=0; - int i=0; - while (i= random) { - return m_data[i].x + sampleGaussian(m_parzenWindow); - } - i++; - } - assert(0); - } - - - void sampleMultiple(std::vector& samples, int num) { - - assert( m_data.size() > 0 ); - samples.clear(); - - if (m_cummulated.size() == 0) { - computeCummuated(); - } - double maxval = m_cummulated.back(); - - std::vector randoms(num); - for (int i=0; i= randoms[j] && j < num) { - samples.push_back( m_data[i].x + sampleGaussian(m_parzenWindow) ); - j++; - } - i++; - } - } - - - - void approxGauss(double step, double* mean, double* sigma) { - - assert( m_data.size() > 0 ); - - double sum=0; - double d=0; - - *mean=0; - for (double x=m_from; x<=m_to; x+=step) { - d = smoothedData(x); - sum += d; - *mean += x*d; - } - *mean /= sum; - - double var=0; - for (double x=m_from; x<=m_to; x+=step) { - d = smoothedData(x); - var += sqr(x-*mean) * d; - } - var /= sum; - - *sigma = sqrt(var); - } - - double gauss(double x, double mean, double sigma) { - return 1.0/(sqrt(2.0*M_PI)*sigma) * exp(-0.5 * sqr( (x-mean)/sigma)); - } - - double cramerVonMisesToGauss(double step, double mean, double sigma) { - - double p=0; - double s=0; - double g=0; - double sint=0; - double gint=0; - - for (double x=m_from; x<=m_to; x+=step) { - s = smoothedData(x); - sint += s * step; - - g = gauss(x, mean, sigma); - gint += g * step; - - p += sqr( (sint - gint) ); - } - - return p; - } - - double kldToGauss(double step, double mean, double sigma) { - - double p=0; - double d=0; - double g=0; - - double sd=0; - double sg=0; - - for (double x=m_from; x<=m_to; x+=step) { - - d = 1e-10 + smoothedData(x); - g = 1e-10 + gauss(x, mean, sigma); - - sd += d; - sg += g; - - p += d * log(d/g); - } - - sd *= step; - sg *= step; - - if (fabs(sd-sg) > 0.1) - assert(0); - - p *= step; - return p; - } - - - void gnuplotDumpData(FILE* fp) { - for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { - const DataPoint& d = *it; - fprintf(fp, "%f %f\n", d.x, d.y); - } - } - - void gnuplotDumpSmoothedData(FILE* fp, double step) { - for (double x=m_from; x<=m_to; x+=step) - fprintf(fp, "%f %f\n", x, smoothedData(x)); - } - - protected: - Data m_data; - std::vector m_cummulated; - double m_int; - double m_lastStep; - - double m_parzenWindow; - double m_from; - double m_to; - -}; - - - - -/* class DataSmoother3D { */ -/* public: */ -/* struct InputPoint { */ -/* InputPoint(double _x=0.0, double _y=0.0, double _t=0.0) { x=_x;y=_y;t=_t;} */ -/* double x; */ -/* double y; */ -/* double t; */ -/* }; */ - -/* struct DataPoint { */ -/* DataPoint(const InputPoint& _p, double _val=0.0;) { p=_p;val=_val;} */ -/* InputPoint p; */ -/* double val; */ -/* }; */ - -/* typedef std::list Data; */ - -/* DataSmoother(double parzenWindow) { */ -/* m_int=-1; */ -/* m_parzenWindow = parzenWindow; */ -/* m_from = InputPoint(MAXDOUBLE,MAXDOUBLE,MAXDOUBLE); */ -/* m_to = InputPoint(-MAXDOUBLE,-MAXDOUBLE,-MAXDOUBLE); */ -/* }; */ - -/* virtual ~DataSmoother() { */ -/* m_data.clear(); */ -/* }; */ - -/* double sqr(double x) { */ -/* return x*x; */ -/* } */ - - -/* void setMinToZero() { */ -/* double minval=MAXDOUBLE; */ -/* for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { */ -/* const DataPoint& d = *it; */ -/* if (minval > d.val) */ -/* minval = d.val; */ -/* } */ - -/* for (Data::iterator it = m_data.begin(); it != m_data.end(); it++) { */ -/* DataPoint& d = *it; */ -/* d.val = d.val - minval; */ -/* } */ - -/* } */ - -/* void add(double x, double y, double t, double v) { */ -/* m_data.push_back(DataPoint(InputPoint(x,y,t),v)); */ -/* m_int=-1; */ - -/* if (x-3.0*m_parzenWindow < m_from.x) */ -/* m_from.x = x - 3.0*m_parzenWindow.x; */ -/* if (x+3.0*m_parzenWindow.x > m_to.x) */ -/* m_to.x = x + 3.0*m_parzenWindow.x; */ - -/* if (y-3.0*m_parzenWindow < m_from.y) */ -/* m_from.y = y - 3.0*m_parzenWindow.y; */ -/* if (y+3.0*m_parzenWindow.y > m_to.y) */ -/* m_to.y = y + 3.0*m_parzenWindow.y; */ - -/* if (t-3.0*m_parzenWindow < m_from.t) */ -/* m_from.t = t - 3.0*m_parzenWindow.t; */ -/* if (t+3.0*m_parzenWindow.t > m_to.t) */ -/* m_to.t = t + 3.0*m_parzenWindow.t; */ -/* } */ - -/* void integrate(InputPoint step) { */ -/* m_lastStep = step; */ -/* double sum=0; */ -/* for (double x=m_from.x; x<=m_to.x; x+=step.x) { */ -/* for (double y=m_from.y; x<=m_to.y; y+=step.y) { */ -/* for (double t=m_from.t; t<=m_to.t; t+=step.t) { */ -/* sum += smoothedData(InputPoint(x,y,t)) * step.x * step.y * step.t; */ -/* } */ -/* } */ -/* } */ -/* m_int = sum; */ -/* } */ - - -/* double smoothedData(InputPoint pnt) { */ -/* assert( m_data.size() > 0 ); */ -/* double p=0; */ -/* double sum_y=0; */ -/* for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { */ -/* const DataPoint& d = *it; */ -/* double u = sqr( (pnt.x-d.x)/m_parzenWindow.x) + */ -/* sqr((pnt.y-d.y)/m_parzenWindow.y) + */ -/* sqr((pnt.t-d.t)/m_parzenWindow.t); */ -/* p += d.val * exp( -0.5 * u); */ -/* sum_y += d.y; */ -/* } */ -/* double denom = sqr(m_parzenWindow.x)*sqr(m_parzenWindow.x)*sqr(m_parzenWindow.x) * (sum_y) * */ -/* sqrt(sqr(m_parzenWindow.x) + sqr(m_parzenWindow.y) + sqr(m_parzenWindow.t)); */ -/* p *= 1./denom; */ - -/* return p; */ -/* } */ - -/* double sample(const InputPoint& step) { */ - -/* assert( m_data.size() > 0 ); */ - -/* if (m_int <0 || step != m_lastStep) */ -/* integrate(step); */ - -/* double r = sampleUniformDouble(0.0, m_int); */ -/* double sum2=0; */ -/* for (double x=m_from; x<=m_to; x+=step) { */ -/* sum2 += smoothedData(x)*step; */ -/* if (sum2 > r) */ -/* return x-0.5*step; */ -/* } */ -/* return m_to; */ -/* } */ - -/* void gnuplotDumpData(FILE* fp) { */ -/* for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { */ -/* const DataPoint& d = *it; */ -/* fprintf(fp, "%f %f %f %f\n", d.x, d.y, d.t, d.val); */ -/* } */ -/* } */ - -/* void gnuplotDumpSmoothedData(FILE* fp, double step) { */ -/* for (double x=m_from; x<=m_to; x+=step) */ -/* fprintf(fp, "%f %f %f %f\n", x, ,y, t, smoothedData(x,y,t)); */ -/* } */ - -/* protected: */ -/* Data m_data; */ -/* vector m_intdata; */ -/* double m_int; */ -/* double m_lastStep; */ - -/* double m_parzenWindow; */ -/* double m_from; */ -/* double m_to; */ - -/* }; */ - -} - -#endif diff --git a/slam_gmapping/openslam_gmapping/utils/dmatrix.h b/slam_gmapping/openslam_gmapping/utils/dmatrix.h deleted file mode 100644 index 708a1f5..0000000 --- a/slam_gmapping/openslam_gmapping/utils/dmatrix.h +++ /dev/null @@ -1,232 +0,0 @@ -#ifndef DMATRIX_HXX -#define DMATRIX_HXX - -#include -#include -namespace GMapping { - -class DNotInvertibleMatrixException: public std::exception {}; -class DIncompatibleMatrixException: public std::exception {}; -class DNotSquareMatrixException: public std::exception {}; - -template class DMatrix { - public: - DMatrix(int n=0,int m=0); - ~DMatrix(); - - DMatrix(const DMatrix&); - DMatrix& operator=(const DMatrix&); - - X * operator[](int i) { - if ((*shares)>1) detach(); - return mrows[i]; - } - - const X * operator[](int i) const { return mrows[i]; } - - const X det() const; - DMatrix inv() const; - DMatrix transpose() const; - DMatrix operator*(const DMatrix&) const; - DMatrix operator+(const DMatrix&) const; - DMatrix operator-(const DMatrix&) const; - DMatrix operator*(const X&) const; - - int rows() const { return nrows; } - int columns() const { return ncols; } - - void detach(); - - static DMatrix I(int); - - protected: - int nrows,ncols; - X * elems; - X ** mrows; - - int * shares; -}; - -template DMatrix::DMatrix(int n,int m) { - if (n<1) n=1; - if (m<1) m=1; - nrows=n; - ncols=m; - elems=new X[nrows*ncols]; - mrows=new X* [nrows]; - for (int i=0;i DMatrix::~DMatrix() { - if (--(*shares)) return; - delete [] elems; - delete [] mrows; - delete shares; -} - -template DMatrix::DMatrix(const DMatrix& m) { - shares=m.shares; - elems=m.elems; - nrows=m.nrows; - ncols=m.ncols; - mrows=m.mrows; - (*shares)++; -} - -template DMatrix& DMatrix::operator=(const DMatrix& m) { - if (!--(*shares)) { - delete [] elems; - delete [] mrows; - delete shares; - } - shares=m.shares; - elems=m.elems; - nrows=m.nrows; - ncols=m.ncols; - mrows=m.mrows; - (*shares)++; - return *this; -} - -template DMatrix DMatrix::inv() const { - if (nrows!=ncols) throw DNotInvertibleMatrixException(); - DMatrix aux1(*this),aux2(I(nrows)); - aux1.detach(); - for (int i=0;i=nrows) throw DNotInvertibleMatrixException(); - X val=aux1.mrows[k][i]; - for (int j=0;j const X DMatrix::det() const { - if (nrows!=ncols) throw DNotSquareMatrixException(); - DMatrix aux(*this); - X d=X(1); - aux.detach(); - for (int i=0;i=nrows) return X(0); - X val=aux.mrows[k][i]; - for (int j=0;j DMatrix DMatrix::transpose() const { - DMatrix aux(ncols, nrows); - for (int i=0; i DMatrix DMatrix::operator*(const DMatrix& m) const { - if (ncols!=m.nrows) throw DIncompatibleMatrixException(); - DMatrix aux(nrows,m.ncols); - for (int i=0;i DMatrix DMatrix::operator+(const DMatrix& m) const { - if (ncols!=m.ncols||nrows!=m.nrows) throw DIncompatibleMatrixException(); - DMatrix aux(nrows,ncols); - for (int i=0;i DMatrix DMatrix::operator-(const DMatrix& m) const { - if (ncols!=m.ncols||nrows!=m.nrows) throw DIncompatibleMatrixException(); - DMatrix aux(nrows,ncols); - for (int i=0;i DMatrix DMatrix::operator*(const X& e) const { - DMatrix aux(nrows,ncols); - for (int i=0;i void DMatrix::detach() { - DMatrix aux(nrows,ncols); - for (int i=0;i DMatrix DMatrix::I(int n) { - DMatrix aux(n,n); - for (int i=0;i std::ostream& operator<<(std::ostream& os, const DMatrix &m) { - os << "{"; - for (int i=0;i0) os << ","; - os << "{"; - for (int j=0;j0) os << ","; - os << m[i][j]; - } - os << "}"; - } - return os << "}"; -} - -}; //namespace GMapping -#endif diff --git a/slam_gmapping/openslam_gmapping/utils/movement.cpp b/slam_gmapping/openslam_gmapping/utils/movement.cpp deleted file mode 100644 index 6c66e3e..0000000 --- a/slam_gmapping/openslam_gmapping/utils/movement.cpp +++ /dev/null @@ -1,108 +0,0 @@ -#include "movement.h" -#include - -namespace GMapping { - - -FSRMovement::FSRMovement(double f, double s, double r) { - this->f = f; - this->s = s; - this->r = r; -} - -FSRMovement::FSRMovement(const FSRMovement& src) { - *this = src; -} - -FSRMovement::FSRMovement(const OrientedPoint& pt1, const OrientedPoint& pt2) { - *this = moveBetweenPoints(pt1, pt2); -} - - -FSRMovement::FSRMovement(const FSRMovement& move1, const FSRMovement& move2) { - *this = composeMoves(move1, move2); -} - -void FSRMovement::normalize() -{ - if (r >= -M_PI && r < M_PI) - return; - - int multiplier = (int)(r / (2*M_PI)); - r = r - multiplier*2*M_PI; - if (r >= M_PI) - r -= 2*M_PI; - if (r < -M_PI) - r += 2*M_PI; -} - -OrientedPoint FSRMovement::move(const OrientedPoint& pt) const { - return movePoint(pt, *this); -} - -void FSRMovement::invert() { - *this = invertMove(*this); -} - -void FSRMovement::compose(const FSRMovement& move2) { - *this = composeMoves(*this, move2); -} - - -FSRMovement FSRMovement::composeMoves(const FSRMovement& move1, - const FSRMovement& move2) { - FSRMovement comp; - comp.f = cos(move1.r) * move2.f - sin(move1.r) * move2.s + move1.f; - comp.s = sin(move1.r) * move2.f + cos(move1.r) * move2.s + move1.s; - comp.r = (move1.r + move2.r); - comp.normalize(); - return comp; -} - -OrientedPoint FSRMovement::movePoint(const OrientedPoint& pt, const FSRMovement& move1) { - OrientedPoint pt2(pt); - pt2.x += move1.f * cos(pt.theta) - move1.s * sin(pt.theta); - pt2.y += move1.f * sin(pt.theta) + move1.s * cos(pt.theta); - pt2.theta = (move1.r + pt.theta); - pt2.normalize(); - return pt2; -} - -FSRMovement FSRMovement::moveBetweenPoints(const OrientedPoint& pt1, - const OrientedPoint& pt2) { - FSRMovement move; - move.f = (pt2.y - pt1.y) * sin(pt1.theta) + (pt2.x - pt1.x) * cos(pt1.theta); - move.s = + (pt2.y - pt1.y) * cos(pt1.theta) - (pt2.x - pt1.x) * sin(pt1.theta); - move.r = (pt2.theta - pt1.theta); - move.normalize(); - return move; - -} - -FSRMovement FSRMovement::invertMove(const FSRMovement& move1) { - FSRMovement p_inv; - p_inv.f = - cos(move1.r) * move1.f - sin(move1.r) * move1.s; - p_inv.s = sin(move1.r) * move1.f - cos(move1.r) * move1.s; - p_inv.r = (-move1.r); - p_inv.normalize(); - return p_inv; -} - - -OrientedPoint FSRMovement::frameTransformation(const OrientedPoint& reference_pt_frame1, - const OrientedPoint& reference_pt_frame2, - const OrientedPoint& pt_frame1) { - OrientedPoint zero; - - FSRMovement itrans_refp1(zero, reference_pt_frame1); - itrans_refp1.invert(); - - FSRMovement trans_refp2(zero, reference_pt_frame2); - FSRMovement trans_pt(zero, pt_frame1); - - FSRMovement tmp = composeMoves( composeMoves(trans_refp2, itrans_refp1), trans_pt); - return tmp.move(zero); -} - - -} diff --git a/slam_gmapping/openslam_gmapping/utils/movement.h b/slam_gmapping/openslam_gmapping/utils/movement.h deleted file mode 100644 index 19b2200..0000000 --- a/slam_gmapping/openslam_gmapping/utils/movement.h +++ /dev/null @@ -1,45 +0,0 @@ -#ifndef FSRMOVEMENT_H -#define FSRMOVEMENT_H - -#include - -namespace GMapping { - - /** fsr-movement (forward, sideward, rotate) **/ -class FSRMovement { - public: - FSRMovement(double f=0.0, double s=0.0, double r=0.0); - FSRMovement(const FSRMovement& src); - FSRMovement(const OrientedPoint& pt1, const OrientedPoint& pt2); - FSRMovement(const FSRMovement& move1, const FSRMovement& move2); - - - void normalize(); - void invert(); - void compose(const FSRMovement& move2); - OrientedPoint move(const OrientedPoint& pt) const; - - - /* static members */ - - static OrientedPoint movePoint(const OrientedPoint& pt, const FSRMovement& move1); - - static FSRMovement composeMoves(const FSRMovement& move1, - const FSRMovement& move2); - - static FSRMovement moveBetweenPoints(const OrientedPoint& pt1, - const OrientedPoint& pt2); - - static FSRMovement invertMove(const FSRMovement& move1); - - static OrientedPoint frameTransformation(const OrientedPoint& reference_pt_frame1, - const OrientedPoint& reference_pt_frame2, - const OrientedPoint& pt_frame1); - - public: - double f; - double s; - double r; -}; -} -#endif diff --git a/slam_gmapping/openslam_gmapping/utils/optimizer.h b/slam_gmapping/openslam_gmapping/utils/optimizer.h deleted file mode 100644 index b6b1d57..0000000 --- a/slam_gmapping/openslam_gmapping/utils/optimizer.h +++ /dev/null @@ -1,159 +0,0 @@ -#ifndef _OPTIMIZER_H_ -#define _OPTIMIZER_H_ - -#include "point.h" - -namespace GMapping { - -struct OptimizerParams{ - double discretization; - double angularStep, linearStep; - int iterations; - double maxRange; -}; - -template -struct Optimizer { - Optimizer(const OptimizerParams& params); - OptimizerParams params; - Map lmap; - Likelihood likelihood; - OrientedPoint gradientDescent(const RangeReading& oldReading, const RangeReading& newReading); - OrientedPoint gradientDescent(const RangeReading& oldReading, const OrientedPoint& pose, OLocalMap& Map); - enum Move {Forward, Backward, Left, Right, TurnRight, TurnLeft}; -}; - -template -Optimizer::Optimizer(const OptimizerParams& p): - params(p), - lmap(p.discretization){} - -template -OrientedPoint Optimizer::gradientDescent(const RangeReading& oldReading, const RangeReading& newReading){ - lmap.clear(); - lmap.update(oldReading, OrientedPoint(0,0,0), params.maxRange); - OrientedPoint delta=absoluteDifference(newReading.getPose(), oldReading.getPose()); - OrientedPoint bestPose=delta; - double bestScore=likelihood(lmap, newReading, bestPose, params.maxRange); - int it=0; - double lstep=params.linearStep, astep=params.angularStep; - bool increase; -/* cerr << "bestScore=" << bestScore << endl;;*/ - do { - increase=false; - OrientedPoint itBestPose=bestPose; - double itBestScore=bestScore; - bool itIncrease; - do { - itIncrease=false; - OrientedPoint testBestPose=itBestPose; - double testBestScore=itBestScore; - for (Move move=Forward; move<=TurnLeft; move=(Move)((int)move+1)){ - OrientedPoint testPose=itBestPose; - switch(move){ - case Forward: testPose.x+=lstep; - break; - case Backward: testPose.x-=lstep; - break; - case Left: testPose.y+=lstep; - break; - case Right: testPose.y-=lstep; - break; - case TurnRight: testPose.theta-=astep; - break; - case TurnLeft: testPose.theta+=astep; - break; - } - double score=likelihood(lmap, newReading, testPose, params.maxRange); - if (score>testBestScore){ - testBestScore=score; - testBestPose=testPose; - } - } - if (testBestScore > itBestScore){ - itBestScore=testBestScore; - itBestPose=testBestPose; -/* cerr << "s=" << itBestScore << " ";*/ - itIncrease=true; - } - } while(itIncrease); - if (itBestScore > bestScore){ -/* cerr << "S(" << itBestScore << "," << bestScore<< ")";*/ - bestScore=itBestScore; - bestPose=itBestPose; - increase=true; - } else { - it++; - lstep*=0.5; - astep*=0.5; - } - } while (it -OrientedPoint Optimizer::gradientDescent(const RangeReading& reading, const OrientedPoint& pose, OLocalMap& lmap){ - OrientedPoint bestPose=pose; - double bestScore=likelihood(lmap, reading, bestPose, params.maxRange); - int it=0; - double lstep=params.linearStep, astep=params.angularStep; - bool increase; -/* cerr << "bestScore=" << bestScore << endl;;*/ - do { - increase=false; - OrientedPoint itBestPose=bestPose; - double itBestScore=bestScore; - bool itIncrease; - do { - itIncrease=false; - OrientedPoint testBestPose=itBestPose; - double testBestScore=itBestScore; - for (Move move=Forward; move<=TurnLeft; move=(Move)((int)move+1)){ - OrientedPoint testPose=itBestPose; - switch(move){ - case Forward: testPose.x+=lstep; - break; - case Backward: testPose.x-=lstep; - break; - case Left: testPose.y+=lstep; - break; - case Right: testPose.y-=lstep; - break; - case TurnRight: testPose.theta-=astep; - break; - case TurnLeft: testPose.theta+=astep; - break; - } - double score=likelihood(lmap, reading, testPose, params.maxRange); - if (score>testBestScore){ - testBestScore=score; - testBestPose=testPose; - } - } - if (testBestScore > itBestScore){ - itBestScore=testBestScore; - itBestPose=testBestPose; -/* cerr << "s=" << itBestScore << " ";*/ - itIncrease=true; - } - } while(itIncrease); - if (itBestScore > bestScore){ -/* cerr << "S(" << itBestScore << "," << bestScore<< ")";*/ - bestScore=itBestScore; - bestPose=itBestPose; - increase=true; - } else { - it++; - lstep*=0.5; - astep*=0.5; - } - } while (it -#include - -#include - -namespace GMapping{ - -template -class OrientedBoundingBox { - - public: - OrientedBoundingBox(std::vector< point > p); - double area(); - - protected: - Point ul; - Point ur; - Point ll; - Point lr; -}; - -#include "orientedboundingbox.hxx" - -};// end namespace - -#endif - diff --git a/slam_gmapping/openslam_gmapping/utils/orientedboundingbox.hxx b/slam_gmapping/openslam_gmapping/utils/orientedboundingbox.hxx deleted file mode 100644 index 2b66e6b..0000000 --- a/slam_gmapping/openslam_gmapping/utils/orientedboundingbox.hxx +++ /dev/null @@ -1,116 +0,0 @@ -template -double OrientedBoundingBox::area() { - return sqrt((ul.x - ll.x)*(ul.x - ll.x) + (ul.y - ll.y)*(ul.y - ll.y)) * - sqrt((ul.x - ur.x)*(ul.x - ur.x) + (ul.y - ur.y)*(ul.y - ur.y)) ; -} - -template -OrientedBoundingBox::OrientedBoundingBox(std::vector< point > p) { - - int nOfPoints = (int) p.size(); - - // calculate the center of all points (schwerpunkt) - // ------------------------------------------------- - double centerx = 0; - double centery = 0; - for (int i=0; i < nOfPoints; i++) { - centerx += p[i].x; - centery += p[i].y; - } - centerx /= (double) nOfPoints; - centery /= (double) nOfPoints; - - - - // calcutae the covariance matrix - // ------------------------------- - // covariance matrix (x1 x2, x3 x4) - double x1 = 0.0; - double x2 = 0.0; - double x3 = 0.0; - double x4 = 0.0; - - for (int i=0; i < nOfPoints; i++) { - double cix = p[i].x - centerx; - double ciy = p[i].y - centery; - - x1 += cix*cix; - x2 += cix*ciy; - x4 += ciy*ciy; - } - x1 /= (double) nOfPoints; - x2 /= (double) nOfPoints; - x3 = x2; - x4 /= (double) nOfPoints; - // covariance & center done - - - // calculate the eigenvectors - // --------------------------- - // catch 1/0 or sqrt(<0) - if ((x3 == 0) || (x2 == 0)|| (x4*x4-2*x1*x4+x1*x1+4*x2*x3 < 0 )) { - fprintf(stderr,"error computing the Eigenvectors (%s, line %d)\nx3=%lf, x2=%lf, term=%lf\n\n", - __FILE__, __LINE__, x3,x2, (x4*x4-2*x1*x4+x1*x1+4*x2*x3) ); - - ul.x = 0; - ul.y = 0; - ur.x = 0; - ur.y = 0; - ll.x = 0; - ll.y = 0; - lr.x = 0; - lr.y = 0; - } - - // eigenvalues - double lamda1 = 0.5* (x4 + x1 + sqrt(x4*x4 - 2.0*x1*x4 + x1*x1 + 4.0*x2*x3)); - double lamda2 = 0.5* (x4 + x1 - sqrt(x4*x4 - 2.0*x1*x4 + x1*x1 + 4.0*x2*x3)); - - // eigenvector 1 with (x,y) - double v1x = - (x4-lamda1) * (x4-lamda1) * (x1-lamda1) / (x2 * x3 * x3); - double v1y = (x4-lamda1) * (x1-lamda1) / (x2 * x3); - // eigenvector 2 with (x,y) - double v2x = - (x4-lamda2) * (x4-lamda2) * (x1-lamda2) / (x2 * x3 * x3); - double v2y = (x4-lamda2) * (x1-lamda2) / (x2 * x3); - - // norm the eigenvectors - double lv1 = sqrt ( (v1x*v1x) + (v1y*v1y) ); - double lv2 = sqrt ( (v2x*v2x) + (v2y*v2y) ); - v1x /= lv1; - v1y /= lv1; - v2x /= lv2; - v2y /= lv2; - // eigenvectors done - - // get the points with maximal dot-product - double x = 0.0; - double y = 0.0; - double xmin = 1e20; - double xmax = -1e20; - double ymin = 1e20; - double ymax = -1e20; - for(int i = 0; i< nOfPoints; i++) { - // dot-product of relativ coordinates of every point - x = (p[i].x - centerx) * v1x + (p[i].y - centery) * v1y; - y = (p[i].x - centerx) * v2x + (p[i].y - centery) * v2y; - - if( x > xmax) xmax = x; - if( x < xmin) xmin = x; - if( y > ymax) ymax = y; - if( y < ymin) ymin = y; - } - - // now we can compute the corners of the bounding box - ul.x = centerx + xmin * v1x + ymin * v2x; - ul.y = centery + xmin * v1y + ymin * v2y; - - ur.x = centerx + xmax * v1x + ymin * v2x; - ur.y = centery + xmax * v1y + ymin * v2y; - - ll.x = centerx + xmin * v1x + ymax * v2x; - ll.y = centery + xmin * v1y + ymax * v2y; - - lr.x = centerx + xmax * v1x + ymax * v2x; - lr.y = centery + xmax * v1y + ymax * v2y; - -} diff --git a/slam_gmapping/openslam_gmapping/utils/printmemusage.cpp b/slam_gmapping/openslam_gmapping/utils/printmemusage.cpp deleted file mode 100644 index 44f4664..0000000 --- a/slam_gmapping/openslam_gmapping/utils/printmemusage.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include "printmemusage.h" - -namespace GMapping{ - -using namespace std; -void printmemusage(){ - pid_t pid=getpid(); - char procfilename[1000]; - sprintf(procfilename, "/proc/%d/status", pid); - ifstream is(procfilename); - string line; - while (is){ - is >> line; - if (line=="VmData:"){ - is >> line; - cerr << "#VmData:\t" << line << endl; - } - if (line=="VmSize:"){ - is >> line; - cerr << "#VmSize:\t" << line << endl; - } - - } -} - -}; - diff --git a/slam_gmapping/openslam_gmapping/utils/printmemusage.h b/slam_gmapping/openslam_gmapping/utils/printmemusage.h deleted file mode 100644 index fc7fd91..0000000 --- a/slam_gmapping/openslam_gmapping/utils/printmemusage.h +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef PRINTMEMUSAGE_H -#define PRINTMEMUSAGE_H -#include -#include -#include -#include -#include - -namespace GMapping{ - void printmemusage(); -}; - -#endif diff --git a/slam_gmapping/openslam_gmapping/utils/printpgm.h b/slam_gmapping/openslam_gmapping/utils/printpgm.h deleted file mode 100644 index d9a3b8a..0000000 --- a/slam_gmapping/openslam_gmapping/utils/printpgm.h +++ /dev/null @@ -1,19 +0,0 @@ -#include -#include -#include -#include - - -using namespace std; -ostream& printpgm(ostream& os, int xsize, int ysize, const double * const * matrix){ - if (!os) - return os; - os<< "P5" << endl << xsize << endl << ysize << endl << 255 << endl; - for (int y=ysize-1; y>=0; y--){ - for (int x=0;x - -//#include -//#include -//#include -//#include -#include -#include -#include - -namespace GMapping { - -#if 0 - -int sampleUniformInt(int max) -{ - return (int)(max*(rand()/(RAND_MAX+1.0))); -} - -double sampleUniformDouble(double min, double max) -{ - return min + (rand() / (double)RAND_MAX) * (max - min); -} - - -#endif - -// Draw randomly from a zero-mean Gaussian distribution, with standard -// deviation sigma. -// We use the polar form of the Box-Muller transformation, explained here: -// http://www.taygeta.com/random/gaussian.html -double pf_ran_gaussian(double sigma) -{ - double x1, x2, w; - double r; - - do - { - do { r = drand48(); } while (r == 0.0); - x1 = 2.0 * r - 1.0; - do { r = drand48(); } while (r == 0.0); - x2 = 2.0 * drand48() - 1.0; - w = x1*x1 + x2*x2; - } while(w > 1.0 || w==0.0); - - return(sigma * x2 * sqrt(-2.0*log(w)/w)); -} - -double sampleGaussian(double sigma, unsigned int S) { - /* - static gsl_rng * r = NULL; - if(r==NULL) { - gsl_rng_env_setup(); - r = gsl_rng_alloc (gsl_rng_default); - } - */ - if (S!=0) - { - //gsl_rng_set(r, S); - srand(S); - } - if (sigma==0) - return 0; - //return gsl_ran_gaussian (r,sigma); - return pf_ran_gaussian (sigma); -} -#if 0 - -double evalGaussian(double sigmaSquare, double delta){ - if (sigmaSquare<=0) - sigmaSquare=1e-4; - return exp(-.5*delta*delta/sigmaSquare)/sqrt(2*M_PI*sigmaSquare); -} - -#endif -double evalLogGaussian(double sigmaSquare, double delta){ - if (sigmaSquare<=0) - sigmaSquare=1e-4; - return -.5*delta*delta/sigmaSquare-.5*log(2*M_PI*sigmaSquare); -} -#if 0 - - -Covariance3 Covariance3::zero={0.,0.,0.,0.,0.,0.}; - -Covariance3 Covariance3::operator + (const Covariance3 & cov) const{ - Covariance3 r(*this); - r.xx+=cov.xx; - r.yy+=cov.yy; - r.tt+=cov.tt; - r.xy+=cov.xy; - r.yt+=cov.yt; - r.xt+=cov.xt; - return r; -} - -EigenCovariance3::EigenCovariance3(){} - -EigenCovariance3::EigenCovariance3(const Covariance3& cov){ - static gsl_eigen_symmv_workspace * m_eigenspace=NULL; - static gsl_matrix * m_cmat=NULL; - static gsl_matrix * m_evec=NULL; - static gsl_vector * m_eval=NULL; - static gsl_vector * m_noise=NULL; - static gsl_vector * m_pnoise=NULL; - - if (m_eigenspace==NULL){ - m_eigenspace=gsl_eigen_symmv_alloc(3); - m_cmat=gsl_matrix_alloc(3,3); - m_evec=gsl_matrix_alloc(3,3); - m_eval=gsl_vector_alloc(3); - m_noise=gsl_vector_alloc(3); - m_pnoise=gsl_vector_alloc(3); - } - - gsl_matrix_set(m_cmat,0,0,cov.xx); gsl_matrix_set(m_cmat,0,1,cov.xy); gsl_matrix_set(m_cmat,0,2,cov.xt); - gsl_matrix_set(m_cmat,1,0,cov.xy); gsl_matrix_set(m_cmat,1,1,cov.yy); gsl_matrix_set(m_cmat,1,2,cov.yt); - gsl_matrix_set(m_cmat,2,0,cov.xt); gsl_matrix_set(m_cmat,2,1,cov.yt); gsl_matrix_set(m_cmat,2,2,cov.tt); - gsl_eigen_symmv (m_cmat, m_eval, m_evec, m_eigenspace); - for (int i=0; i<3; i++){ - eval[i]=gsl_vector_get(m_eval,i); - for (int j=0; j<3; j++) - evec[i][j]=gsl_matrix_get(m_evec,i,j); - } -} - -EigenCovariance3 EigenCovariance3::rotate(double angle) const{ - static gsl_matrix * m_rmat=NULL; - static gsl_matrix * m_vmat=NULL; - static gsl_matrix * m_result=NULL; - if (m_rmat==NULL){ - m_rmat=gsl_matrix_alloc(3,3); - m_vmat=gsl_matrix_alloc(3,3); - m_result=gsl_matrix_alloc(3,3); - } - - double c=cos(angle); - double s=sin(angle); - gsl_matrix_set(m_rmat,0,0, c ); gsl_matrix_set(m_rmat,0,1, -s); gsl_matrix_set(m_rmat,0,2, 0.); - gsl_matrix_set(m_rmat,1,0, s ); gsl_matrix_set(m_rmat,1,1, c); gsl_matrix_set(m_rmat,1,2, 0.); - gsl_matrix_set(m_rmat,2,0, 0.); gsl_matrix_set(m_rmat,2,1, 0.); gsl_matrix_set(m_rmat,2,2, 1.); - - for (unsigned int i=0; i<3; i++) - for (unsigned int j=0; j<3; j++) - gsl_matrix_set(m_vmat,i,j,evec[i][j]); - gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1., m_rmat, m_vmat, 0., m_result); - EigenCovariance3 ecov(*this); - for (int i=0; i<3; i++){ - for (int j=0; j<3; j++) - ecov.evec[i][j]=gsl_matrix_get(m_result,i,j); - } - return ecov; -} - -OrientedPoint EigenCovariance3::sample() const{ - static gsl_matrix * m_evec=NULL; - static gsl_vector * m_noise=NULL; - static gsl_vector * m_pnoise=NULL; - if (m_evec==NULL){ - m_evec=gsl_matrix_alloc(3,3); - m_noise=gsl_vector_alloc(3); - m_pnoise=gsl_vector_alloc(3); - } - for (int i=0; i<3; i++){ - for (int j=0; j<3; j++) - gsl_matrix_set(m_evec,i,j, evec[i][j]); - } - for (int i=0; i<3; i++){ - double v=sampleGaussian(sqrt(eval[i])); - if(isnan(v)) - v=0; - gsl_vector_set(m_pnoise,i, v); - } - gsl_blas_dgemv (CblasNoTrans, 1., m_evec, m_pnoise, 0, m_noise); - OrientedPoint ret(gsl_vector_get(m_noise,0),gsl_vector_get(m_noise,1),gsl_vector_get(m_noise,2)); - ret.theta=atan2(sin(ret.theta), cos(ret.theta)); - return ret; -} - -#endif - -double Gaussian3::eval(const OrientedPoint& p) const{ - OrientedPoint q=p-mean; - q.theta=atan2(sin(p.theta-mean.theta),cos(p.theta-mean.theta)); - double v1,v2,v3; - v1 = covariance.evec[0][0]*q.x+covariance.evec[1][0]*q.y+covariance.evec[2][0]*q.theta; - v2 = covariance.evec[0][1]*q.x+covariance.evec[1][1]*q.y+covariance.evec[2][1]*q.theta; - v3 = covariance.evec[0][2]*q.x+covariance.evec[1][2]*q.y+covariance.evec[2][2]*q.theta; - return evalLogGaussian(covariance.eval[0], v1)+evalLogGaussian(covariance.eval[1], v2)+evalLogGaussian(covariance.eval[2], v3); -} - -#if 0 -void Gaussian3::computeFromSamples(const std::vector & poses, const std::vector& weights ){ - OrientedPoint mean=OrientedPoint(0,0,0); - double wcum=0; - double s=0, c=0; - std::vector::const_iterator w=weights.begin(); - for (std::vector::const_iterator p=poses.begin(); p!=poses.end(); p++){ - s+=*w*sin(p->theta); - c+=*w*cos(p->theta); - mean.x+=*w*p->x; - mean.y+=*w*p->y; - wcum+=*w; - w++; - } - mean.x/=wcum; - mean.y/=wcum; - s/=wcum; - c/=wcum; - mean.theta=atan2(s,c); - - Covariance3 cov=Covariance3::zero; - w=weights.begin(); - for (std::vector::const_iterator p=poses.begin(); p!=poses.end(); p++){ - OrientedPoint delta=(*p)-mean; - delta.theta=atan2(sin(delta.theta),cos(delta.theta)); - cov.xx+=*w*delta.x*delta.x; - cov.yy+=*w*delta.y*delta.y; - cov.tt+=*w*delta.theta*delta.theta; - cov.xy+=*w*delta.x*delta.y; - cov.yt+=*w*delta.y*delta.theta; - cov.xt+=*w*delta.x*delta.theta; - w++; - } - cov.xx/=wcum; - cov.yy/=wcum; - cov.tt/=wcum; - cov.xy/=wcum; - cov.yt/=wcum; - cov.xt/=wcum; - EigenCovariance3 ecov(cov); - this->mean=mean; - this->covariance=ecov; - this->cov=cov; -} - -void Gaussian3::computeFromSamples(const std::vector & poses){ - OrientedPoint mean=OrientedPoint(0,0,0); - double wcum=1; - double s=0, c=0; - for (std::vector::const_iterator p=poses.begin(); p!=poses.end(); p++){ - s+=sin(p->theta); - c+=cos(p->theta); - mean.x+=p->x; - mean.y+=p->y; - wcum+=1.; - } - mean.x/=wcum; - mean.y/=wcum; - s/=wcum; - c/=wcum; - mean.theta=atan2(s,c); - - Covariance3 cov=Covariance3::zero; - for (std::vector::const_iterator p=poses.begin(); p!=poses.end(); p++){ - OrientedPoint delta=(*p)-mean; - delta.theta=atan2(sin(delta.theta),cos(delta.theta)); - cov.xx+=delta.x*delta.x; - cov.yy+=delta.y*delta.y; - cov.tt+=delta.theta*delta.theta; - cov.xy+=delta.x*delta.y; - cov.yt+=delta.y*delta.theta; - cov.xt+=delta.x*delta.theta; - } - cov.xx/=wcum; - cov.yy/=wcum; - cov.tt/=wcum; - cov.xy/=wcum; - cov.yt/=wcum; - cov.xt/=wcum; - EigenCovariance3 ecov(cov); - this->mean=mean; - this->covariance=ecov; - this->cov=cov; -} -#endif - -}// end namespace - diff --git a/slam_gmapping/openslam_gmapping/utils/stat_test.cpp b/slam_gmapping/openslam_gmapping/utils/stat_test.cpp deleted file mode 100644 index a49dccb..0000000 --- a/slam_gmapping/openslam_gmapping/utils/stat_test.cpp +++ /dev/null @@ -1,66 +0,0 @@ -#include -#include -#include -#include "stat.h" - -using namespace std; -using namespace GMapping; - -// struct Covariance3{ -// double xx, yy, tt, xy, xt, yt; -// }; - -#define SAMPLES_NUMBER 10000 - -int main(int argc, char** argv){ - Covariance3 cov={1.,0.01,0.01,0,0,0}; - EigenCovariance3 ecov(cov); - cout << "EigenValues: " << ecov.eval[0] << " "<< ecov.eval[1] << " " << ecov.eval[2] << endl; - - cout << "EigenVectors:" < points; - for (unsigned int i=0; i::iterator b = points.begin(); - std::vector::iterator e = points.end(); - Gaussian3 gaussian=computeGaussianFromSamples(b, e); - cov=gaussian.cov; - ecov=gaussian.covariance; - cout << "*************** Estimated with Templates ***************" << endl; - cout << "EigenValues: " << ecov.eval[0] << " "<< ecov.eval[1] << " " << ecov.eval[2] << endl; - cout << "EigenVectors:" < -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/laser_scan.hpp" -#include "std_msgs/msg/float64.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" -#include "nav_msgs/msg/map_meta_data.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2/utils.h" -#include "message_filters/subscriber.h" -#include "tf2_ros/message_filter.h" - -#include "gmapping/gridfastslam/gridslamprocessor.h" -#include "gmapping/sensor/sensor_base/sensor.h" -#include "gmapping/sensor/sensor_range/rangesensor.h" -#include "gmapping/sensor/sensor_odometry/odometrysensor.h" - -class SlamGmapping : public rclcpp::Node{ -public: - SlamGmapping(); - ~SlamGmapping() override; - - void init(); - void startLiveSlam(); - void publishTransform(); - void laserCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan); - void publishLoop(double transform_publish_period); - -private: - rclcpp::Node::SharedPtr node_; - rclcpp::Publisher::SharedPtr entropy_publisher_; - rclcpp::Publisher::SharedPtr sst_; - rclcpp::Publisher::SharedPtr sstm_; - - std::shared_ptr buffer_; - std::shared_ptr tfl_; - - std::shared_ptr> scan_filter_sub_; - std::shared_ptr> scan_filter_; - std::shared_ptr tfB_; - - GMapping::GridSlamProcessor* gsp_; - GMapping::RangeSensor* gsp_laser_; - // The angles in the laser, going from -x to x (adjustment is made to get the laser between - // symmetrical bounds as that's what gmapping expects) - std::vector laser_angles_; - // The pose, in the original laser frame, of the corresponding centered laser with z facing up - geometry_msgs::msg::PoseStamped centered_laser_pose_; - // Depending on the order of the elements in the scan and the orientation of the scan frame, - // We might need to change the order of the scan - bool do_reverse_range_; - unsigned int gsp_laser_beam_count_; - GMapping::OdometrySensor* gsp_odom_; - - bool got_first_scan_; - - bool got_map_; - nav_msgs::msg::OccupancyGrid map_; - - tf2::Duration map_update_interval_; - tf2::Transform map_to_odom_; - std::mutex map_to_odom_mutex_; - std::mutex map_mutex_; - - int laser_count_; - int throttle_scans_; - - std::shared_ptr transform_thread_; - - std::string base_frame_; - std::string laser_frame_; - std::string map_frame_; - std::string odom_frame_; - - void updateMap(sensor_msgs::msg::LaserScan::ConstSharedPtr scan); - bool getOdomPose(GMapping::OrientedPoint& gmap_pose, const rclcpp::Time& t); - bool initMapper(sensor_msgs::msg::LaserScan::ConstSharedPtr scan); - bool addScan(sensor_msgs::msg::LaserScan::ConstSharedPtr scan, GMapping::OrientedPoint& gmap_pose); - double computePoseEntropy(); - - // Parameters used by GMapping - double maxRange_; - double maxUrange_; - double maxrange_; - double minimum_score_; - double sigma_; - int kernelSize_; - double lstep_; - double astep_; - int iterations_; - double lsigma_; - double ogain_; - int lskip_; - double srr_; - double srt_; - double str_; - double stt_; - double linearUpdate_; - double angularUpdate_; - double temporalUpdate_; - double resampleThreshold_; - int particles_; - double xmin_; - double ymin_; - double xmax_; - double ymax_; - double delta_; - double occ_thresh_; - double llsamplerange_; - double llsamplestep_; - double lasamplerange_; - double lasamplestep_; - - unsigned long int seed_; - - double transform_publish_period_; - double tf_delay_; -}; - -#endif //SLAM_GMAPPING_SLAM_GMAPPING_H_ diff --git a/slam_gmapping/slam_gmapping/launch/slam_gmapping.launch.py b/slam_gmapping/slam_gmapping/launch/slam_gmapping.launch.py deleted file mode 100644 index dccafef..0000000 --- a/slam_gmapping/slam_gmapping/launch/slam_gmapping.launch.py +++ /dev/null @@ -1,29 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.substitutions import EnvironmentVariable -import launch.actions -import launch_ros.actions - - -def generate_launch_description(): - - rviz_config_dir = os.path.join( - get_package_share_directory('slam_gmapping'), - 'rviz', - 'gmapping.rviz') - - return LaunchDescription([ - launch_ros.actions.Node( - package='slam_gmapping', executable='slam_gmapping', output='screen'), - - launch_ros.actions.Node( - package='rviz2', - executable='rviz2', - name='rviz2', - arguments=['-d', rviz_config_dir], - output='screen'), - - ]) diff --git a/slam_gmapping/slam_gmapping/package.xml b/slam_gmapping/slam_gmapping/package.xml deleted file mode 100644 index 66e0ac7..0000000 --- a/slam_gmapping/slam_gmapping/package.xml +++ /dev/null @@ -1,36 +0,0 @@ - - - - slam_gmapping - 0.0.0 - This package contains a ROS2 Crystal Clemmys wrapper for OpenSlam's Gmapping. - The gmapping package provides laser-based SLAM (Simultaneous Localization and Mapping), - as a ROS node called slam_gmapping. Using slam_gmapping, you can create a 2-D occupancy - grid map (like a building floorplan) from laser and pose data collected by a mobile robot. - - Brian Gerkey - Vincent Rabaud - CreativeCommons-by-nc-sa-2.0 - - http://ros.org/wiki/gmapping - - ament_cmake - - std_msgs - nav_msgs - tf2 - tf2_ros - tf2_geometry_msgs - message_filters - rclcpp - sensor_msgs - visualization_msgs - openslam_gmapping - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/slam_gmapping/slam_gmapping/rviz/gmapping.rviz b/slam_gmapping/slam_gmapping/rviz/gmapping.rviz deleted file mode 100644 index 81a0256..0000000 --- a/slam_gmapping/slam_gmapping/rviz/gmapping.rviz +++ /dev/null @@ -1,295 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /RobotModel1 - - /LaserScan1 - Splitter Ratio: 0.5 - Tree Height: 627 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: LaserScan -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map_updates - Use Timestamp: false - Value: true - - Alpha: 1 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_front_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_rear_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_front_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_rear_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Mass Properties: - Inertia: false - Mass: false - Name: RobotModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - base_footprint: - Value: true - base_link: - Value: true - laser_link: - Value: true - left_front_wheel_link: - Value: true - left_rear_wheel_link: - Value: true - map: - Value: true - odom: - Value: true - right_front_wheel_link: - Value: true - right_rear_wheel_link: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - map: - odom: - base_footprint: - base_link: - laser_link: - {} - left_front_wheel_link: - {} - left_rear_wheel_link: - {} - right_front_wheel_link: - {} - right_rear_wheel_link: - {} - Update Interval: 0 - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/LaserScan - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 230 - Min Color: 0; 0; 0 - Min Intensity: 6 - Name: LaserScan - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /scan - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 8.487345695495605 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 1.5697963237762451 - Target Frame: - Value: Orbit (rviz) - Yaw: 3.1504018306732178 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 846 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1200 - X: 89 - Y: 85 diff --git a/slam_gmapping/slam_gmapping/src/slam_gmapping.cpp b/slam_gmapping/slam_gmapping/src/slam_gmapping.cpp deleted file mode 100644 index 193108f..0000000 --- a/slam_gmapping/slam_gmapping/src/slam_gmapping.cpp +++ /dev/null @@ -1,541 +0,0 @@ -/* - * slam_gmapping - * Copyright (c) 2008, Willow Garage, Inc. - * - * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE - * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY - * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS - * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. - * - * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO - * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS - * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND - * CONDITIONS. - * - */ - -/* Author: Brian Gerkey */ -/* Modified by: Charles DuHadway */ - -// -// Created by shivesh on 29/10/18. -// - -#include "slam_gmapping/slam_gmapping.h" - -#include "tf2_ros/create_timer_ros.h" - -#define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) - -using std::placeholders::_1; - -SlamGmapping::SlamGmapping(): - Node("slam_gmapping"), - scan_filter_sub_(nullptr), - scan_filter_(nullptr), - laser_count_(0), - transform_thread_(nullptr) -{ - buffer_ = std::make_shared(get_clock()); - auto timer_interface = std::make_shared( - get_node_base_interface(), - get_node_timers_interface()); - buffer_->setCreateTimerInterface(timer_interface); - tfl_ = std::make_shared(*buffer_); - node_ = std::shared_ptr(this, [](rclcpp::Node *) {}); - tfB_ = std::make_shared(node_); - map_to_odom_.setIdentity(); - seed_ = static_cast(time(nullptr)); - init(); - startLiveSlam(); -} - -void SlamGmapping::init() { - gsp_ = new GMapping::GridSlamProcessor(); - - gsp_laser_ = nullptr; - gsp_odom_ = nullptr; - got_first_scan_ = false; - got_map_ = false; - - throttle_scans_ = 1; - base_frame_ = "base_footprint"; - map_frame_ = "map"; - odom_frame_ = "odom"; - transform_publish_period_ = 0.05; - - map_update_interval_ = tf2::durationFromSec(0.5); - maxUrange_ = 80.0; maxRange_ = 0.0; - minimum_score_ = 0; - sigma_ = 0.05; - kernelSize_ = 1; - lstep_ = 0.05; - astep_ = 0.05; - iterations_ = 5; - lsigma_ = 0.075; - ogain_ = 3.0; - lskip_ = 0; - srr_ = 0.1; - srt_ = 0.2; - str_ = 0.1; - stt_ = 0.2; - linearUpdate_ = 1.0; - angularUpdate_ = 0.5; - temporalUpdate_ = 1.0; - resampleThreshold_ = 0.5; - particles_ = 30; - xmin_ = -10.0; - ymin_ = -10.0; - xmax_ = 10.0; - ymax_ = 10.0; - delta_ = 0.05; - occ_thresh_ = 0.25; - llsamplerange_ = 0.01; - llsamplestep_ = 0.01; - lasamplerange_ = 0.005; - lasamplestep_ = 0.005; - tf_delay_ = transform_publish_period_; -} - -void SlamGmapping::startLiveSlam() { - entropy_publisher_ = this->create_publisher("entropy", rclcpp::SystemDefaultsQoS()); - sst_ = this->create_publisher("map", rclcpp::SystemDefaultsQoS()); - sstm_ = this->create_publisher("map_metadata", rclcpp::SystemDefaultsQoS()); - scan_filter_sub_ = std::make_shared> - (node_, "scan", rclcpp::SensorDataQoS().get_rmw_qos_profile()); -// sub_ = this->create_subscription( -// "scan", rclcpp::SensorDataQoS(), -// std::bind(&SlamGmapping::laserCallback, this, std::placeholders::_1)); - scan_filter_ = std::make_shared> - (*scan_filter_sub_, *buffer_, odom_frame_, 10, node_); - scan_filter_->registerCallback(std::bind(&SlamGmapping::laserCallback, this, std::placeholders::_1)); - transform_thread_ = std::make_shared - (std::bind(&SlamGmapping::publishLoop, this, transform_publish_period_)); -} - -void SlamGmapping::publishLoop(double transform_publish_period){ - if (transform_publish_period == 0) - return; - rclcpp::Rate r(1.0 / transform_publish_period); - while (rclcpp::ok()) { - publishTransform(); - r.sleep(); - } -} - -SlamGmapping::~SlamGmapping() -{ - if(transform_thread_){ - transform_thread_->join(); - } - - delete gsp_; - delete gsp_laser_; - delete gsp_odom_; -} - -bool SlamGmapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const rclcpp::Time& t) -{ - // Get the pose of the centered laser at the right time - centered_laser_pose_.header.stamp = t; - // Get the laser's pose that is centered - geometry_msgs::msg::PoseStamped odom_pose; - try - { - buffer_->transform(centered_laser_pose_, odom_pose, odom_frame_, tf2::durationFromSec(1.0)); - } - catch(tf2::TransformException& e) - { - RCLCPP_WARN(this->get_logger(), "Failed to compute odom pose, skipping scan (%s)", e.what()); - return false; - } - - double yaw = tf2::getYaw(odom_pose.pose.orientation); - - gmap_pose = GMapping::OrientedPoint(odom_pose.pose.position.x, - odom_pose.pose.position.y, - yaw); - return true; -} - -bool SlamGmapping::initMapper(const sensor_msgs::msg::LaserScan::ConstSharedPtr scan) -{ - laser_frame_ = scan->header.frame_id; - // Get the laser's pose, relative to base. - geometry_msgs::msg::PoseStamped ident; - geometry_msgs::msg::PoseStamped laser_pose; - - try{ - ident.header.frame_id = laser_frame_; - ident.header.stamp = scan->header.stamp; - tf2::Transform transform; - transform.setIdentity(); - tf2::toMsg(transform, ident.pose); - buffer_->transform(ident, laser_pose, base_frame_); - } - catch (tf2::TransformException& e){ - RCLCPP_WARN(this->get_logger(), "Failed to compute laser pose, aborting initialization (%s)", e.what()); - return false; - } - - // create a point 1m above the laser position and transform it into the laser-frame - geometry_msgs::msg::PointStamped up; - up.header.stamp = scan->header.stamp; - up.header.frame_id = base_frame_; - up.point.x = up.point.y = 0; - up.point.z = 1 + laser_pose.pose.position.z; - try - { - buffer_->transform(up, up, laser_frame_); - } - catch(tf2::TransformException& e) - { - RCLCPP_WARN(this->get_logger(), "Unable to determine orientation of laser: %s", e.what()); - return false; - } - - // gmapping doesnt take roll or pitch into account. So check for correct sensor alignment. - if (fabs(fabs(up.point.z) - 1) > 0.001) - { - RCLCPP_INFO(this->get_logger(), - "Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f", up.point.z); - return false; - } - - gsp_laser_beam_count_ = static_cast(scan->ranges.size()); - - double angle_center = (scan->angle_min + scan->angle_max)/2; - - centered_laser_pose_.header.frame_id = laser_frame_; - centered_laser_pose_.header.stamp = get_clock()->now(); - tf2::Quaternion q; - - if (up.point.z > 0) - { - do_reverse_range_ = scan->angle_min > scan->angle_max; - q.setEuler(angle_center, 0, 0); - RCLCPP_INFO(this->get_logger(),"Laser is mounted upwards."); - } - else - { - do_reverse_range_ = scan->angle_min < scan->angle_max; - q.setEuler(-angle_center, 0, M_PI); - RCLCPP_INFO(this->get_logger(), "Laser is mounted upside down."); - } - - centered_laser_pose_.pose.position.x = 0; - centered_laser_pose_.pose.position.y = 0; - centered_laser_pose_.pose.position.z = 0; - - centered_laser_pose_.pose.orientation.w = q.getW(); - centered_laser_pose_.pose.orientation.x = q.getX(); - centered_laser_pose_.pose.orientation.y = q.getY(); - centered_laser_pose_.pose.orientation.z = q.getZ(); - - // Compute the angles of the laser from -x to x, basically symmetric and in increasing order - laser_angles_.resize(scan->ranges.size()); - // Make sure angles are started so that they are centered - double theta = - std::fabs(scan->angle_min - scan->angle_max)/2; - for(unsigned int i=0; iranges.size(); ++i) - { - laser_angles_[i]=theta; - theta += std::fabs(scan->angle_increment); - } - - RCLCPP_DEBUG(this->get_logger(), "Laser angles in laser-frame: min: %.3f max: %.3f inc: %.3f", - scan->angle_min, scan->angle_max, scan->angle_increment); - RCLCPP_DEBUG(this->get_logger(), "Laser angles in top-down centered laser-frame: min: %.3f max: %.3f inc: %.3f", - laser_angles_.front(), laser_angles_.back(), std::fabs(scan->angle_increment)); - - GMapping::OrientedPoint gmap_pose(0, 0, 0); - - // setting maxRange and maxUrange here so we can set a reasonable default - maxRange_ = scan->range_max - 0.01; - maxUrange_ = maxRange_; - - // The laser must be called "FLASER". - // We pass in the absolute value of the computed angle increment, on the - // assumption that GMapping requires a positive angle increment. If the - // actual increment is negative, we'll swap the order of ranges before - // feeding each scan to GMapping. - gsp_laser_ = new GMapping::RangeSensor("FLASER", gsp_laser_beam_count_, fabs(scan->angle_increment), gmap_pose, - 0.0, maxRange_); - - GMapping::SensorMap smap; - smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_)); - gsp_->setSensorMap(smap); - - gsp_odom_ = new GMapping::OdometrySensor(odom_frame_); - - /// @todo Expose setting an initial pose - GMapping::OrientedPoint initialPose; - if(!getOdomPose(initialPose, scan->header.stamp)) - { - RCLCPP_WARN(this->get_logger(), "Unable to determine inital pose of laser! Starting point will be set to zero."); - initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0); - } - - gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_, - kernelSize_, lstep_, astep_, iterations_, - lsigma_, ogain_, static_cast(lskip_)); - - gsp_->setMotionModelParameters(srr_, srt_, str_, stt_); - gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_); - gsp_->setUpdatePeriod(temporalUpdate_); - gsp_->setgenerateMap(false); - gsp_->GridSlamProcessor::init(static_cast(particles_), xmin_, ymin_, xmax_, ymax_, - delta_, initialPose); - gsp_->setllsamplerange(llsamplerange_); - gsp_->setllsamplestep(llsamplestep_); - /// @todo Check these calls; in the gmapping gui, they use - /// llsamplestep and llsamplerange intead of lasamplestep and - /// lasamplerange. It was probably a typo, but who knows. - gsp_->setlasamplerange(lasamplerange_); - gsp_->setlasamplestep(lasamplestep_); - gsp_->setminimumScore(minimum_score_); - - // Call the sampling function once to set the seed. - GMapping::sampleGaussian(1, static_cast(seed_)); - - RCLCPP_INFO(this->get_logger(), "Initialization complete"); - - return true; -} - -bool SlamGmapping::addScan(const sensor_msgs::msg::LaserScan::ConstSharedPtr scan, GMapping::OrientedPoint& gmap_pose) { - if (!getOdomPose(gmap_pose, scan->header.stamp)) - return false; - - if (scan->ranges.size() != gsp_laser_beam_count_) - return false; - - // GMapping wants an array of doubles... - auto *ranges_double = new double[scan->ranges.size()]; - // If the angle increment is negative, we have to invert the order of the readings. - if (do_reverse_range_) { - RCLCPP_DEBUG(this->get_logger(), "Inverting scan"); - int num_ranges = static_cast(scan->ranges.size()); - for (int i = 0; i < num_ranges; i++) { - // Must filter out short readings, because the mapper won't - if (scan->ranges[num_ranges - i - 1] < scan->range_min) - ranges_double[i] = (double) scan->range_max; - else - ranges_double[i] = (double) scan->ranges[num_ranges - i - 1]; - } - } else { - for (unsigned int i = 0; i < scan->ranges.size(); i++) { - // Must filter out short readings, because the mapper won't - if (scan->ranges[i] < scan->range_min) - ranges_double[i] = (double) scan->range_max; - else - ranges_double[i] = (double) scan->ranges[i]; - } - } - - GMapping::RangeReading reading(static_cast(scan->ranges.size()), - ranges_double, - gsp_laser_, - scan->header.stamp.sec); - - // ...but it deep copies them in RangeReading constructor, so we don't - // need to keep our array around. - delete[] ranges_double; - - reading.setPose(gmap_pose); - - RCLCPP_DEBUG(this->get_logger(), "processing scan"); - - return gsp_->processScan(reading); -} - - -void SlamGmapping::laserCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan) { - laser_count_++; - if ((laser_count_ % throttle_scans_) != 0) - return; - - tf2::TimePoint last_map_update = tf2::TimePointZero; - - // We can't initialize the mapper until we've got the first scan - if(!got_first_scan_) - { - if(!initMapper(scan)) - return; - got_first_scan_ = true; - } - - GMapping::OrientedPoint odom_pose; - - if(addScan(scan, odom_pose)) - { - GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose; - - tf2::Quaternion q; - q.setRPY(0, 0, mpose.theta); - tf2::Transform laser_to_map = tf2::Transform(q, tf2::Vector3(mpose.x, mpose.y, 0.0)).inverse(); - q.setRPY(0, 0, odom_pose.theta); - tf2::Transform odom_to_laser = tf2::Transform(q, tf2::Vector3(odom_pose.x, odom_pose.y, 0.0)); - - map_to_odom_mutex_.lock(); - map_to_odom_ = (odom_to_laser * laser_to_map).inverse(); - map_to_odom_mutex_.unlock(); - - tf2::TimePoint timestamp = tf2_ros::fromMsg(scan->header.stamp); - if(!got_map_ || (timestamp - last_map_update) > map_update_interval_) - { - updateMap(scan); - last_map_update = tf2_ros::fromMsg(scan->header.stamp); - } - } -} - -double SlamGmapping::computePoseEntropy() -{ - double weight_total=0.0; - for (const auto &it : gsp_->getParticles()) { - weight_total += it.weight; - } - double entropy = 0.0; - for (const auto &it : gsp_->getParticles()) { - if(it.weight/weight_total > 0.0) - entropy += it.weight/weight_total * log(it.weight/weight_total); - } - return -entropy; -} - -void SlamGmapping::updateMap(const sensor_msgs::msg::LaserScan::ConstSharedPtr scan) -{ - RCLCPP_DEBUG(this->get_logger(), "Update map"); - map_mutex_.lock(); - GMapping::ScanMatcher matcher; - - matcher.setLaserParameters(static_cast(scan->ranges.size()), &(laser_angles_[0]), - gsp_laser_->getPose()); - - matcher.setlaserMaxRange(maxRange_); - matcher.setusableRange(maxUrange_); - matcher.setgenerateMap(true); - - GMapping::GridSlamProcessor::Particle best = - gsp_->getParticles()[gsp_->getBestParticleIndex()]; - std_msgs::msg::Float64 entropy; - entropy.data = computePoseEntropy(); - if(entropy.data > 0.0) - entropy_publisher_->publish(entropy); - - if(!got_map_) { - map_.info.resolution = static_cast(delta_); - map_.info.origin.position.x = 0.0; - map_.info.origin.position.y = 0.0; - map_.info.origin.position.z = 0.0; - map_.info.origin.orientation.x = 0.0; - map_.info.origin.orientation.y = 0.0; - map_.info.origin.orientation.z = 0.0; - map_.info.origin.orientation.w = 1.0; - } - - GMapping::Point center; - center.x=(xmin_ + xmax_) / 2.0; - center.y=(ymin_ + ymax_) / 2.0; - - GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_, - delta_); - - RCLCPP_DEBUG(this->get_logger(), "Trajectory tree:"); - for(GMapping::GridSlamProcessor::TNode* n = best.node; - n; - n = n->parent) - { - RCLCPP_DEBUG(this->get_logger(), " %.3f %.3f %.3f", - n->pose.x, - n->pose.y, - n->pose.theta); - if(!n->reading) - { - RCLCPP_DEBUG(this->get_logger(), "Reading is NULL"); - continue; - } - matcher.invalidateActiveArea(); - matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0])); - matcher.registerScan(smap, n->pose, &((*n->reading)[0])); - } - - // the map may have expanded, so resize ros message as well - if(map_.info.width != (unsigned int) smap.getMapSizeX() || map_.info.height != (unsigned int) smap.getMapSizeY()) { - - // NOTE: The results of ScanMatcherMap::getSize() are different from the parameters given to the constructor - // so we must obtain the bounding box in a different way - GMapping::Point wmin = smap.map2world(GMapping::IntPoint(0, 0)); - GMapping::Point wmax = smap.map2world(GMapping::IntPoint(smap.getMapSizeX(), smap.getMapSizeY())); - xmin_ = wmin.x; ymin_ = wmin.y; - xmax_ = wmax.x; ymax_ = wmax.y; - - RCLCPP_DEBUG(this->get_logger(), "map size is now %dx%d pixels (%f,%f)-(%f, %f)", smap.getMapSizeX(), smap.getMapSizeY(), - xmin_, ymin_, xmax_, ymax_); - - map_.info.width = static_cast(smap.getMapSizeX()); - map_.info.height = static_cast(smap.getMapSizeY()); - map_.info.origin.position.x = xmin_; - map_.info.origin.position.y = ymin_; - map_.data.resize(map_.info.width * map_.info.height); - - RCLCPP_DEBUG(this->get_logger(), "map origin: (%f, %f)", map_.info.origin.position.x, map_.info.origin.position.y); - } - - for(int x=0; x < smap.getMapSizeX(); x++) - { - for(int y=0; y < smap.getMapSizeY(); y++) - { - /// @todo Sort out the unknown vs. free vs. obstacle thresholding - GMapping::IntPoint p(x, y); - double occ=smap.cell(p); - assert(occ <= 1.0); - if(occ < 0) - map_.data[MAP_IDX(map_.info.width, x, y)] = -1; - else if(occ > occ_thresh_) - { - //map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0); - map_.data[MAP_IDX(map_.info.width, x, y)] = 100; - } - else - map_.data[MAP_IDX(map_.info.width, x, y)] = 0; - } - } - got_map_ = true; - - //make sure to set the header information on the map - map_.header.stamp = get_clock()->now(); - map_.header.frame_id = map_frame_; - - sst_->publish(map_); - sstm_->publish(map_.info); - map_mutex_.unlock(); -} - -void SlamGmapping::publishTransform() -{ - map_to_odom_mutex_.lock(); - rclcpp::Time tf_expiration = get_clock()->now() + rclcpp::Duration( - static_cast(static_cast(tf_delay_)), 0); - geometry_msgs::msg::TransformStamped transform; - transform.header.frame_id = map_frame_; - transform.header.stamp = tf_expiration; - transform.child_frame_id = odom_frame_; - try { - transform.transform = tf2::toMsg(map_to_odom_); - tfB_->sendTransform(transform); - } - catch (tf2::LookupException& te){ - RCLCPP_INFO(this->get_logger(), te.what()); - } - map_to_odom_mutex_.unlock(); -} - -int main(int argc, char* argv[]) -{ - rclcpp::init(argc, argv); - - auto slam_gmapping_node = std::make_shared(); - rclcpp::spin(slam_gmapping_node); - return(0); -} diff --git a/upstream.jazzy.repos b/upstream.jazzy.repos index ac17fe8..1cf96b1 100644 --- a/upstream.jazzy.repos +++ b/upstream.jazzy.repos @@ -19,6 +19,10 @@ repositories: type: git url: https://github.com/dfloreaa/point_lio_ros2.git revision: main + slam_gmapping: + type: git + url: https://github.com/Project-MANAS/slam_gmapping.git + revision: eloquent-devel teleop_twist_keyboard: type: git url: https://github.com/ros2/teleop_twist_keyboard.git