├── .catkin_tools ├── CATKIN_IGNORE ├── README └── profiles │ └── default │ ├── build.yaml │ └── config.yaml ├── .circleci └── config.yml ├── .dockerignore ├── .gitignore ├── Dockerfile ├── README.md ├── docker-compose.yml ├── ros_entrypoint.sh └── src ├── CMakeLists.txt ├── backward_ros ├── .gitignore ├── .travis.yml ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE.txt ├── README.md ├── builds.sh ├── cmake │ ├── BackwardConfig.cmake │ └── BackwardConfigCatkin.cmake ├── conanfile.py ├── doc │ ├── nice.png │ ├── pretty.png │ └── rude.png ├── include │ └── backward_ros │ │ └── backward.hpp ├── package.xml ├── src │ └── backward.cpp ├── test │ ├── _test_main.cpp │ ├── rectrace.cpp │ ├── select_signals.cpp │ ├── stacktrace.cpp │ ├── suicide.cpp │ ├── test.cpp │ └── test.hpp └── test_package │ ├── CMakeLists.txt │ ├── conanfile.py │ └── main.cpp ├── occupancy_mapping_benchmarks ├── .gitignore ├── CMakeLists.txt ├── README.md ├── cmake │ ├── FindOpenVDB.cmake │ └── FindTBB.cmake ├── launch │ └── octomap_server.launch ├── package.xml ├── scripts │ └── make_plots.py └── src │ └── benchmark.cc ├── octomap ├── .gitignore ├── .travis.yml ├── CATKIN_IGNORE ├── CMakeLists.txt ├── README.md ├── dynamicEDT3D │ ├── CHANGELOG.txt │ ├── CMakeLists.txt │ ├── CMakeModules │ │ ├── CMakeUninstall.cmake.in │ │ ├── CPackSettings.cmake │ │ ├── CompilerSettings.cmake │ │ └── InstallPkgConfigFile.cmake │ ├── LICENSE.txt │ ├── README.txt │ ├── doxygen.h │ ├── dynamicEDT3D.dox │ ├── dynamicEDT3DConfig.cmake.in │ ├── include │ │ └── dynamicEDT3D │ │ │ ├── bucketedqueue.h │ │ │ ├── bucketedqueue.hxx │ │ │ ├── dynamicEDT3D.h │ │ │ ├── dynamicEDTOctomap.h │ │ │ ├── dynamicEDTOctomap.hxx │ │ │ └── point.h │ ├── package.xml │ └── src │ │ ├── CMakeLists.txt │ │ ├── dynamicEDT3D.cpp │ │ └── examples │ │ ├── CMakeLists.txt │ │ ├── exampleEDT3D.cpp │ │ ├── exampleEDTOctomap.cpp │ │ └── exampleEDTOctomapStamped.cpp ├── octomap │ ├── AUTHORS.txt │ ├── CHANGELOG.txt │ ├── CMakeLists.txt │ ├── CMakeModules │ │ ├── CMakeUninstall.cmake.in │ │ ├── CPackSettings.cmake │ │ ├── CompilerSettings.cmake │ │ └── InstallPkgConfigFile.cmake │ ├── LICENSE.txt │ ├── README.md │ ├── doxygen.h │ ├── extern │ │ └── README_BINVOX │ ├── include │ │ └── octomap │ │ │ ├── AbstractOcTree.h │ │ │ ├── AbstractOccupancyOcTree.h │ │ │ ├── ColorOcTree.h │ │ │ ├── CountingOcTree.h │ │ │ ├── MCTables.h │ │ │ ├── MapCollection.h │ │ │ ├── MapCollection.hxx │ │ │ ├── MapNode.h │ │ │ ├── MapNode.hxx │ │ │ ├── OcTree.h │ │ │ ├── OcTreeBase.h │ │ │ ├── OcTreeBaseImpl.h │ │ │ ├── OcTreeBaseImpl.hxx │ │ │ ├── OcTreeDataNode.h │ │ │ ├── OcTreeDataNode.hxx │ │ │ ├── OcTreeIterator.hxx │ │ │ ├── OcTreeKey.h │ │ │ ├── OcTreeNode.h │ │ │ ├── OcTreeStamped.h │ │ │ ├── OccupancyOcTreeBase.h │ │ │ ├── OccupancyOcTreeBase.hxx │ │ │ ├── Pointcloud.h │ │ │ ├── ScanGraph.h │ │ │ ├── math │ │ │ ├── Pose6D.h │ │ │ ├── Quaternion.h │ │ │ ├── Utils.h │ │ │ └── Vector3.h │ │ │ ├── octomap.h │ │ │ ├── octomap_deprecated.h │ │ │ ├── octomap_timing.h │ │ │ ├── octomap_types.h │ │ │ └── octomap_utils.h │ ├── octomap-config.cmake.in │ ├── octomap.dox.in │ ├── package.xml │ ├── share │ │ ├── data │ │ │ ├── mapcoll.txt │ │ │ └── scan.dat.bz2 │ │ ├── example-project.tgz │ │ └── images │ │ │ └── uml_overview.png │ ├── src │ │ ├── AbstractOcTree.cpp │ │ ├── AbstractOccupancyOcTree.cpp │ │ ├── CMakeLists.txt │ │ ├── ColorOcTree.cpp │ │ ├── CountingOcTree.cpp │ │ ├── OcTree.cpp │ │ ├── OcTreeNode.cpp │ │ ├── OcTreeStamped.cpp │ │ ├── Pointcloud.cpp │ │ ├── ScanGraph.cpp │ │ ├── binvox2bt.cpp │ │ ├── bt2vrml.cpp │ │ ├── compare_octrees.cpp │ │ ├── convert_octree.cpp │ │ ├── edit_octree.cpp │ │ ├── eval_octree_accuracy.cpp │ │ ├── graph2tree.cpp │ │ ├── intersection_example.cpp │ │ ├── log2graph.cpp │ │ ├── math │ │ │ ├── CMakeLists.txt │ │ │ ├── Pose6D.cpp │ │ │ ├── Quaternion.cpp │ │ │ └── Vector3.cpp │ │ ├── normals_example.cpp │ │ ├── octree2pointcloud.cpp │ │ ├── offset_graph.cpp │ │ ├── simple_example.cpp │ │ └── testing │ │ │ ├── CMakeLists.txt │ │ │ ├── color_tree_histogram.cpp │ │ │ ├── test_changedkeys.cpp │ │ │ ├── test_color_tree.cpp │ │ │ ├── test_io.cpp │ │ │ ├── test_iterators.cpp │ │ │ ├── test_mapcollection.cpp │ │ │ ├── test_pruning.cpp │ │ │ ├── test_raycasting.cpp │ │ │ ├── test_scans.cpp │ │ │ ├── testing.h │ │ │ └── unit_tests.cpp │ └── valgrind_memcheck.supp ├── octovis │ ├── CMakeLists.txt │ ├── CMakeLists_src.txt │ ├── CMakeModules │ │ ├── CMakeUninstall.cmake.in │ │ ├── CompilerSettings.cmake │ │ └── FindQGLViewer.cmake │ ├── LICENSE.txt │ ├── README.md │ ├── include │ │ └── octovis │ │ │ ├── CameraFollowMode.h │ │ │ ├── ColorOcTreeDrawer.h │ │ │ ├── OcTreeDrawer.h │ │ │ ├── OcTreeRecord.h │ │ │ ├── PointcloudDrawer.h │ │ │ ├── SceneObject.h │ │ │ ├── SelectionBox.h │ │ │ ├── TrajectoryDrawer.h │ │ │ ├── ViewerGui.h │ │ │ ├── ViewerGui.ui │ │ │ ├── ViewerSettings.h │ │ │ ├── ViewerSettings.ui │ │ │ ├── ViewerSettingsPanel.h │ │ │ ├── ViewerSettingsPanel.ui │ │ │ ├── ViewerSettingsPanelCamera.h │ │ │ ├── ViewerSettingsPanelCamera.ui │ │ │ └── ViewerWidget.h │ ├── octovis-config.cmake.in │ ├── package.xml │ └── src │ │ ├── CameraFollowMode.cpp │ │ ├── ColorOcTreeDrawer.cpp │ │ ├── OcTreeDrawer.cpp │ │ ├── PointcloudDrawer.cpp │ │ ├── SceneObject.cpp │ │ ├── SelectionBox.cpp │ │ ├── TrajectoryDrawer.cpp │ │ ├── ViewerGui.cpp │ │ ├── ViewerSettings.cpp │ │ ├── ViewerSettingsPanel.cpp │ │ ├── ViewerSettingsPanelCamera.cpp │ │ ├── ViewerWidget.cpp │ │ ├── extern │ │ └── QGLViewer │ │ │ ├── .gitignore │ │ │ ├── CHANGELOG │ │ │ ├── GPL_EXCEPTION │ │ │ ├── INSTALL │ │ │ ├── ImageInterface.ui │ │ │ ├── LICENCE │ │ │ ├── QGLViewer.pro │ │ │ ├── README │ │ │ ├── VRender │ │ │ ├── AxisAlignedBox.h │ │ │ ├── BSPSortMethod.cpp │ │ │ ├── BackFaceCullingOptimizer.cpp │ │ │ ├── EPSExporter.cpp │ │ │ ├── Exporter.cpp │ │ │ ├── Exporter.h │ │ │ ├── FIGExporter.cpp │ │ │ ├── NVector3.cpp │ │ │ ├── NVector3.h │ │ │ ├── Optimizer.h │ │ │ ├── ParserGL.cpp │ │ │ ├── ParserGL.h │ │ │ ├── Primitive.cpp │ │ │ ├── Primitive.h │ │ │ ├── PrimitivePositioning.cpp │ │ │ ├── PrimitivePositioning.h │ │ │ ├── SortMethod.h │ │ │ ├── TopologicalSortMethod.cpp │ │ │ ├── Types.h │ │ │ ├── VRender.cpp │ │ │ ├── VRender.h │ │ │ ├── Vector2.cpp │ │ │ ├── Vector2.h │ │ │ ├── Vector3.cpp │ │ │ ├── Vector3.h │ │ │ ├── VisibilityOptimizer.cpp │ │ │ ├── gpc.cpp │ │ │ └── gpc.h │ │ │ ├── VRenderInterface.ui │ │ │ ├── camera.cpp │ │ │ ├── camera.h │ │ │ ├── config.h │ │ │ ├── constraint.cpp │ │ │ ├── constraint.h │ │ │ ├── domUtils.h │ │ │ ├── frame.cpp │ │ │ ├── frame.h │ │ │ ├── keyFrameInterpolator.cpp │ │ │ ├── keyFrameInterpolator.h │ │ │ ├── manipulatedCameraFrame.cpp │ │ │ ├── manipulatedCameraFrame.h │ │ │ ├── manipulatedFrame.cpp │ │ │ ├── manipulatedFrame.h │ │ │ ├── mouseGrabber.cpp │ │ │ ├── mouseGrabber.h │ │ │ ├── qglviewer-icon.xpm │ │ │ ├── qglviewer.cpp │ │ │ ├── qglviewer.h │ │ │ ├── qglviewer.icns │ │ │ ├── qglviewer_fr.qm │ │ │ ├── qglviewer_fr.ts │ │ │ ├── quaternion.cpp │ │ │ ├── quaternion.h │ │ │ ├── saveSnapshot.cpp │ │ │ ├── vec.cpp │ │ │ └── vec.h │ │ ├── icons.qrc │ │ ├── icons │ │ ├── LICENSE.txt │ │ ├── application-exit.png │ │ ├── configure.png │ │ ├── document-open-folder.png │ │ ├── document-open.png │ │ ├── document-save-as.png │ │ ├── document-save.png │ │ ├── edit-clear-list.png │ │ ├── edit-undo.png │ │ ├── go-bottom.png │ │ ├── go-top.png │ │ ├── help-contents.png │ │ ├── lgpl-3.0.txt │ │ ├── list-add.png │ │ ├── list-remove.png │ │ ├── media-playback-start.png │ │ ├── media-seek-backward.png │ │ ├── media-seek-forward.png │ │ ├── media-skip-backward.png │ │ ├── media-skip-forward.png │ │ ├── system-restart.png │ │ ├── view-preview.png │ │ └── view-refresh.png │ │ └── main.cpp └── scripts │ ├── increase_version.py │ └── travis_build_jobs.sh ├── octomap_catkin ├── CMakeLists.txt ├── lib_wrap.cc └── package.xml ├── octomap_mapping ├── .gitignore ├── .travis.yml ├── README.md ├── octomap_mapping │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ └── package.xml └── octomap_server │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── cfg │ └── OctomapServer.cfg │ ├── include │ └── octomap_server │ │ ├── OctomapServer.h │ │ ├── OctomapServerMultilayer.h │ │ └── TrackingOctomapServer.h │ ├── launch │ ├── octomap_mapping.launch │ ├── octomap_mapping_nodelet.launch │ ├── octomap_tracking_client.launch │ └── octomap_tracking_server.launch │ ├── mainpage.dox │ ├── nodelet_plugins.xml │ ├── package.xml │ ├── params │ └── default.yaml │ ├── scripts │ └── octomap_eraser_cli.py │ └── src │ ├── OctomapServer.cpp │ ├── OctomapServerMultilayer.cpp │ ├── TrackingOctomapServer.cpp │ ├── octomap_saver.cpp │ ├── octomap_server_multilayer.cpp │ ├── octomap_server_node.cpp │ ├── octomap_server_nodelet.cpp │ ├── octomap_server_static.cpp │ └── octomap_tracking_server_node.cpp ├── rosfmt ├── .gitignore ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE.txt ├── README.md ├── include │ └── rosfmt │ │ ├── macros_generated.h │ │ └── rosfmt.h ├── package.xml ├── scripts │ └── generate_macros.py └── test │ └── test.cpp └── skimap_ros ├── .gitignore ├── .vscode ├── c_cpp_properties.json ├── settings.json └── tasks.json ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake ├── CMakeParseArguments.cmake ├── Modules │ ├── FindEigen3.cmake │ ├── FindFLANN.cmake │ └── FindG2O.cmake └── cvlab_macros.cmake ├── include ├── skimap │ ├── KDSkipList.hpp │ ├── SkiMap.hpp │ ├── SkipList.hpp │ ├── SkipListDense.hpp │ ├── SkipListGrid.hpp │ ├── SkipListMap.hpp │ ├── SkipListMapV2.hpp │ ├── utils │ │ ├── ArgParse.hpp │ │ ├── CameraUtils.hpp │ │ └── TimingsUtils.hpp │ └── voxels │ │ ├── GenericTile2D.hpp │ │ ├── GenericVoxel3D.hpp │ │ ├── GenericVoxelKD.hpp │ │ ├── Voxel6DPose.hpp │ │ ├── VoxelDataContainer.hpp │ │ ├── VoxelDataMatrix.hpp │ │ ├── VoxelDataMultiLabel.hpp │ │ ├── VoxelDataOccupancy.hpp │ │ └── VoxelDataRGBW.hpp ├── skimap_ros │ └── SkiMapServiceClient.hpp ├── slamdunk │ ├── camera_tracker.h │ ├── data.h │ ├── edge_se3_xyzpair.h │ ├── feature_matcher.h │ ├── gpu_features.h │ ├── graph_backend.h │ ├── graph_utils.h │ ├── impl │ │ └── ratio_matcher.hpp │ ├── pretty_printer.h │ ├── quadtree.h │ ├── slam_dunk.h │ ├── slamdunk_defines.h │ └── transformation_estimation.h └── slamdunk_extension │ ├── FIROrder2.h │ ├── SlamDunkScene.h │ └── Utility.h ├── launch ├── skimap │ ├── skimap_live.launch │ └── skimap_map_service.launch ├── skimap_live_experiment.launch └── slamdunk_tracker.launch ├── package.xml ├── rviz ├── skimap_live.rviz └── slamdunk_tracker.rviz ├── src ├── nodes │ ├── experiments │ │ ├── debug.cpp │ │ ├── grid2d.cpp │ │ ├── kd_test.cpp │ │ ├── octree_test.cpp │ │ └── skimap_grid_test_python │ │ │ ├── bench.py │ │ │ ├── bench_kd.py │ │ │ ├── bench_octree.py │ │ │ ├── bench_people.py │ │ │ ├── grid.csv │ │ │ ├── matlab │ │ │ ├── bench_dimensions │ │ │ │ ├── kdskip_dim2.csv │ │ │ │ ├── kdskip_r10.csv │ │ │ │ ├── kdskip_r32.csv │ │ │ │ └── kdtree.csv │ │ │ ├── bench_octree_points │ │ │ │ ├── kdskip.csv │ │ │ │ ├── kdtree.csv │ │ │ │ ├── octree.csv │ │ │ │ └── skimap.csv │ │ │ ├── bench_points_2d │ │ │ │ ├── kdskip_r1.csv │ │ │ │ ├── kdskip_r10.csv │ │ │ │ ├── kdskip_r20.csv │ │ │ │ ├── kdskip_r5.csv │ │ │ │ └── kdtree.csv │ │ │ ├── bench_points_3d │ │ │ │ ├── kdskip_r1.csv │ │ │ │ ├── kdskip_r10.csv │ │ │ │ ├── kdskip_r5.csv │ │ │ │ └── kdtree.csv │ │ │ ├── bench_points_5d │ │ │ │ ├── kdskip_r10.csv │ │ │ │ ├── kdskip_r25.csv │ │ │ │ ├── kdskip_r32.csv │ │ │ │ └── kdtree.csv │ │ │ ├── bench_test.m │ │ │ ├── importfile.m │ │ │ ├── skigrid_omp.csv │ │ │ └── skigrid_single.csv │ │ │ ├── plots │ │ │ ├── plot_testing.py │ │ │ └── plotutils.py │ │ │ └── skigrid.csv │ ├── skimap_live.cpp │ ├── skimap_map_service.cpp │ ├── skimap_pose_filtering.cpp │ ├── slamdunk_tracker.cpp │ ├── test.cpp │ └── tutorials │ │ └── integration_of_random_points.cpp ├── skimap_ros │ └── skimap_ros_void_library.cpp ├── slamdunk │ ├── data.cpp │ ├── edge_se3_xyzpair.cpp │ ├── feature_matcher.cpp │ ├── feature_tracker.cpp │ ├── gpu_features.cpp │ ├── graph_backend.cpp │ ├── graph_utils.cpp │ ├── internal_timers.hpp │ ├── slam_dunk.cpp │ └── transformation_estimation.cpp └── slamdunk_extension │ ├── FIROrder2.cpp │ ├── SlamDunkScene.cpp │ └── Utility.cpp └── srv └── SkimapIntegrationService.srv /.catkin_tools/CATKIN_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/.catkin_tools/CATKIN_IGNORE -------------------------------------------------------------------------------- /.catkin_tools/README: -------------------------------------------------------------------------------- 1 | # Catkin Tools Metadata 2 | 3 | This directory was generated by catkin_tools and it contains persistent 4 | configuration information used by the `catkin` command and its sub-commands. 5 | 6 | Each subdirectory of the `profiles` directory contains a set of persistent 7 | configuration options for separate profiles. The default profile is called 8 | `default`. If another profile is desired, it can be described in the 9 | `profiles.yaml` file in this directory. 10 | 11 | Please see the catkin_tools documentation before editing any files in this 12 | directory. Most actions can be performed with the `catkin` command-line 13 | program. 14 | -------------------------------------------------------------------------------- /.catkin_tools/profiles/default/build.yaml: -------------------------------------------------------------------------------- 1 | blacklist: [] 2 | build_space: build 3 | catkin_make_args: [] 4 | cmake_args: 5 | - -DCMAKE_BUILD_TYPE=RelWithDebInfo 6 | devel_layout: merged 7 | devel_space: devel 8 | extend_path: /opt/ros/melodic 9 | install: false 10 | install_space: install 11 | isolate_install: false 12 | jobs_args: [] 13 | log_space: logs 14 | make_args: [] 15 | source_space: src 16 | use_env_cache: false 17 | use_internal_make_jobserver: true 18 | whitelist: [] 19 | -------------------------------------------------------------------------------- /.catkin_tools/profiles/default/config.yaml: -------------------------------------------------------------------------------- 1 | blacklist: [] 2 | build_space: build 3 | catkin_make_args: [] 4 | cmake_args: 5 | - -DCMAKE_BUILD_TYPE=RelWithDebInfo 6 | devel_layout: merged 7 | devel_space: devel 8 | extend_path: /opt/ros/melodic 9 | install: false 10 | install_space: install 11 | isolate_install: false 12 | jobs_args: [] 13 | log_space: logs 14 | make_args: [] 15 | source_space: src 16 | use_env_cache: false 17 | use_internal_make_jobserver: true 18 | whitelist: [] 19 | -------------------------------------------------------------------------------- /.circleci/config.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | 3 | jobs: 4 | build: 5 | docker: 6 | - image: nicolov/occupancy_mapping_benchmarks:0.0.1 7 | working_directory: /catkin_ws 8 | steps: 9 | - checkout: 10 | - restore_cache: 11 | keys: 12 | - build-{{ .Branch }}- 13 | - build- 14 | paths: 15 | - ~/.ccache 16 | 17 | - run: 18 | name: ccache setup 19 | command: | 20 | ccache --show-stats 21 | ccache --zero-stats 22 | ccache --max-size=2G 23 | 24 | - run: CC=/usr/lib/ccache/gcc CXX=/usr/lib/ccache/g++ catkin build -j3 25 | - run: ccache --show-stats 26 | 27 | - save_cache: 28 | key: build-{{ .Branch }}-{{ epoch }} 29 | paths: 30 | - ~/.ccache 31 | when: always 32 | 33 | # Download dataset 34 | - run: 35 | name: download dataset 36 | command: | 37 | cd /tmp && wget https://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household-2hz-with-pointclouds.bag 38 | 39 | - run: 40 | name: run benchmark 41 | command: | 42 | source devel/setup.bash && 43 | mkdir /tmp/results && 44 | cd /tmp/results && 45 | rosrun occupancy_mapping_benchmarks benchmark /tmp/*.bag > bench_output.txt && 46 | rosrun occupancy_mapping_benchmarks make_plots.py 47 | 48 | - store_artifacts: 49 | path: /tmp/results 50 | -------------------------------------------------------------------------------- /.dockerignore: -------------------------------------------------------------------------------- 1 | # All files not needed for the setup are ignored as we volume-mount the source code. 2 | 3 | * 4 | *.* 5 | !/ros_entrypoint.sh 6 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /.catkin_tools 2 | /logs 3 | /devel 4 | /build 5 | 6 | /src/cmake-build-debug 7 | /src/.idea 8 | 9 | *.bt 10 | *.vdb 11 | 12 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Occupancy mapping benchmarks 2 | 3 | 5 | 6 | Code to benchmark three open source Occupancy Mapping libraries: 7 | [OctoMap](https://octomap.github.io/), 8 | [SkiMap](https://github.com/m4nh/skimap_ros), and 9 | [OpenVDB](https://www.openvdb.org/) for an RGBD mapping task based on the TUM 10 | RGBD dataset. 11 | 12 | For discussion and instructions, read the [article on my blog](occupancy-mapping-benchmarks.html). 13 | 14 | ``` 15 | catkin init 16 | catkin config --extend /opt/ros/melodic --merge-devel --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo 17 | catkin build 18 | 19 | rosrun occupancy_mapping_benchmarks benchmark \ 20 | src/occupancy_mapping_benchmarks/rgbd_dataset_freiburg3_long_office_household-2hz-with-pointclouds.bag > bench_output.txt 21 | 22 | rosrun occupancy_mapping_benchmarks make_plots.py 23 | ``` 24 | 25 | ## Maintenance 26 | 27 | To upgrade the Docker image: 28 | 29 | ``` 30 | ( export TAG=nicolov/occupancy_mapping_benchmarks:0.0.1 && docker build -t $TAG . && docker push $TAG ) 31 | ``` 32 | -------------------------------------------------------------------------------- /docker-compose.yml: -------------------------------------------------------------------------------- 1 | version: '3' 2 | services: 3 | dev: 4 | build: . 5 | volumes: 6 | - .:/catkin_ws 7 | working_dir: /catkin_ws 8 | -------------------------------------------------------------------------------- /ros_entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | # setup ros environment 5 | source "/opt/ros/$ROS_DISTRO/setup.bash" 6 | exec "$@" 7 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # toplevel CMakeLists.txt for a catkin workspace 2 | # catkin/cmake/toplevel.cmake 3 | 4 | cmake_minimum_required(VERSION 2.8.3) 5 | 6 | set(CATKIN_TOPLEVEL TRUE) 7 | 8 | # search for catkin within the workspace 9 | set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}") 10 | execute_process(COMMAND ${_cmd} 11 | RESULT_VARIABLE _res 12 | OUTPUT_VARIABLE _out 13 | ERROR_VARIABLE _err 14 | OUTPUT_STRIP_TRAILING_WHITESPACE 15 | ERROR_STRIP_TRAILING_WHITESPACE 16 | ) 17 | if(NOT _res EQUAL 0 AND NOT _res EQUAL 2) 18 | # searching fot catkin resulted in an error 19 | string(REPLACE ";" " " _cmd_str "${_cmd}") 20 | message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}") 21 | endif() 22 | 23 | # include catkin from workspace or via find_package() 24 | if(_res EQUAL 0) 25 | set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake") 26 | # include all.cmake without add_subdirectory to let it operate in same scope 27 | include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE) 28 | add_subdirectory("${_out}") 29 | 30 | else() 31 | # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument 32 | # or CMAKE_PREFIX_PATH from the environment 33 | if(NOT DEFINED CMAKE_PREFIX_PATH) 34 | if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "") 35 | string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) 36 | endif() 37 | endif() 38 | 39 | # list of catkin workspaces 40 | set(catkin_search_path "") 41 | foreach(path ${CMAKE_PREFIX_PATH}) 42 | if(EXISTS "${path}/.catkin") 43 | list(FIND catkin_search_path ${path} _index) 44 | if(_index EQUAL -1) 45 | list(APPEND catkin_search_path ${path}) 46 | endif() 47 | endif() 48 | endforeach() 49 | 50 | # search for catkin in all workspaces 51 | set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE) 52 | find_package(catkin QUIET 53 | NO_POLICY_SCOPE 54 | PATHS ${catkin_search_path} 55 | NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) 56 | unset(CATKIN_TOPLEVEL_FIND_PACKAGE) 57 | 58 | if(NOT catkin_FOUND) 59 | message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.") 60 | endif() 61 | endif() 62 | 63 | catkin_workspace() 64 | -------------------------------------------------------------------------------- /src/backward_ros/.gitignore: -------------------------------------------------------------------------------- 1 | build*/ 2 | *.pyc 3 | -------------------------------------------------------------------------------- /src/backward_ros/.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | compiler: 3 | - gcc 4 | - clang 5 | 6 | addons: 7 | apt: 8 | packages: 9 | - valgrind 10 | 11 | install: 12 | - DEPS_DIR="${TRAVIS_BUILD_DIR}/deps" 13 | - mkdir ${DEPS_DIR} && cd ${DEPS_DIR} 14 | - CMAKE_URL="http://www.cmake.org/files/v3.3/cmake-3.3.2-Linux-x86_64.tar.gz" 15 | - mkdir cmake && travis_retry wget --no-check-certificate --quiet -O - ${CMAKE_URL} | tar --strip-components=1 -xz -C cmake 16 | - export PATH=${DEPS_DIR}/cmake/bin:${PATH} 17 | - pip install --user conan && export PATH=$PATH:$HOME/.local/bin 18 | - cd ${TRAVIS_BUILD_DIR} 19 | - mkdir build && cd build 20 | - cmake .. -DBACKWARD_TESTS=ON 21 | - cmake --build . 22 | 23 | script: 24 | - valgrind ctest .. --verbose 25 | - cd ${TRAVIS_BUILD_DIR} && conan test_package --build=outdated 26 | -------------------------------------------------------------------------------- /src/backward_ros/LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright 2013 Google Inc. All Rights Reserved. 2 | 3 | The MIT License (MIT) 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 9 | of the Software, and to permit persons to whom the Software is furnished to do 10 | so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /src/backward_ros/README.md: -------------------------------------------------------------------------------- 1 | Backward ROS 2 | ============ 3 | 4 | For info about the backward-cpp project check https://github.com/bombela/backward-cpp 5 | 6 | This wrapper should make it very easy to integrate backward_cpp into your ROS packages. 7 | 8 | ## Integration of backward_ros 9 | 10 | Add backward\_ros to your package.xml `backward_ros` 11 | 12 | Add backward\_ros to your CMakeLists.txt `find_package(catkin REQUIRED COMPONENTS your_dependencies backward_ros)` 13 | 14 | 15 | To get line numbers and more details, you need to build with debug information enabled (CMAKE_BUILD_TYPE = Debug or RelWithDebInfo) 16 | 17 | You're done, it should automatically add a library to your executables, when they crash, they should print a nice stacktrace like this: 18 | ![pretty stackstrace](doc/pretty.png) 19 | -------------------------------------------------------------------------------- /src/backward_ros/builds.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | COMPILERS_CXX98=`cat</dev/null 28 | ( 29 | cd "$builddir" 30 | cmake -DCMAKE_BUILD_TYPE=$buildtype -DBACKWARD_TESTS=ON .. 31 | ) 32 | } 33 | 34 | function build() { 35 | local builddir=$1 36 | shift 37 | make -C "$builddir" $@ 38 | } 39 | 40 | function dotest() { 41 | local builddir=$1 42 | shift 43 | make -C "$builddir" test $@ 44 | return 0 45 | } 46 | 47 | function do_action() { 48 | local lang=$1 49 | local action=$2 50 | shift 2 51 | 52 | for compiler in $COMPILERS; do 53 | local builddir="build_${lang}_${compiler}" 54 | 55 | if [[ $action == "cmake" ]]; then 56 | buildtype=$1 57 | mkbuild $compiler $lang "$buildtype" "$builddir" 58 | [[ $? != 0 ]] && exit 59 | elif [[ $action == "make" ]]; then 60 | build "$builddir" $@ 61 | [[ $? != 0 ]] && exit 62 | elif [[ $action == "test" ]]; then 63 | dotest "$builddir" $@ 64 | [[ $? != 0 ]] && exit 65 | elif [[ $action == "clean" ]]; then 66 | rm -r "$builddir" 67 | else 68 | echo "usage: $0 cmake [debug|release|relwithdbg]|make|test|clean" 69 | exit 255 70 | fi 71 | done 72 | } 73 | 74 | COMPILERS=$COMPILERS_CXX98 75 | do_action c++98 $@ 76 | COMPILERS=$COMPILERS_CXX11 77 | do_action c++11 $@ 78 | -------------------------------------------------------------------------------- /src/backward_ros/cmake/BackwardConfigCatkin.cmake: -------------------------------------------------------------------------------- 1 | if (LIBDW_FOUND) 2 | add_definitions(-DBACKWARD_HAS_DW=1) 3 | set(backward_ros_forced_LIBRARIES "${backward_ros_LIBRARIES};${LIBDW_LIBRARIES}") 4 | elseif(LIBBFD_FOUND) 5 | add_definitions(-DBACKWARD_HAS_BFD=1) 6 | set(backward_ros_forced_LIBRARIES "${backward_ros_LIBRARIES};${LIBBFD_LIBRARIES}") 7 | else() 8 | set(backward_ros_forced_LIBRARIES "${backward_ros_LIBRARIES}") 9 | endif() 10 | if (ROSCPP_FOUND) 11 | add_definitions(-DBACKWARD_HAS_ROSCPP=1) 12 | endif() 13 | set(backward_ros_LIBRARIES "") #This is used by catkin, but we don't need it since we force it below 14 | set(backward_ros_full_path_LIBRARIES "") #This is used by catkin, but we don't need it since we force it below 15 | 16 | #Hack to find absolute path to libraries, won't work if library is not compiled yet 17 | foreach(lib ${backward_ros_forced_LIBRARIES}) 18 | if(NOT EXISTS ${lib}) 19 | message("${lib} doesn't exist, trying to find it in ${backward_ros_PREFIX}") 20 | find_library(backward_ros_lib_path 21 | NAMES ${lib} 22 | PATHS ${backward_ros_PREFIX}) 23 | if(NOT ${backward_ros_lib_path}) 24 | message("${lib} not found") 25 | else() 26 | message("${backward_ros_lib_path} found") 27 | set(backward_ros_full_path_LIBRARIES "${backward_ros_full_path_LIBRARIES} ${backward_ros_lib_path}") 28 | endif() 29 | else() 30 | set(backward_ros_full_path_LIBRARIES "${backward_ros_full_path_LIBRARIES} ${lib}") 31 | endif() 32 | endforeach() 33 | SET(CMAKE_EXE_LINKER_FLAGS "-Wl,--no-as-needed ${backward_ros_full_path_LIBRARIES} -Wl,--as-needed ${CMAKE_EXE_LINKER_FLAGS}") 34 | 35 | -------------------------------------------------------------------------------- /src/backward_ros/conanfile.py: -------------------------------------------------------------------------------- 1 | from conans import ConanFile, CMake 2 | import os 3 | 4 | class BackwardCpp(ConanFile): 5 | settings = 'os', 'compiler', 'build_type', 'arch' 6 | name = 'backward' 7 | url = 'https://github.com/bombela/backward-cpp' 8 | license = 'MIT' 9 | version = '1.3.0' 10 | options = { 11 | 'stack_walking_unwind': [True, False], 12 | 'stack_walking_backtrace': [True, False], 13 | 'stack_details_auto_detect': [True, False], 14 | 'stack_details_backtrace_symbol': [True, False], 15 | 'stack_details_dw': [True, False], 16 | 'stack_details_bfd': [True, False], 17 | 'shared': [True, False] 18 | } 19 | default_options = ( 20 | 'stack_walking_unwind=True', 21 | 'stack_walking_backtrace=False', 22 | 'stack_details_auto_detect=True', 23 | 'stack_details_backtrace_symbol=False', 24 | 'stack_details_dw=False', 25 | 'stack_details_bfd=False', 26 | 'shared=False' 27 | ) 28 | exports = 'backward.cpp', 'backward.hpp', 'test/*', 'CMakeLists.txt', 'BackwardConfig.cmake' 29 | generators = 'cmake' 30 | 31 | def cmake_option(self, option, prefix = ''): 32 | return '-D{}{}={}'.format(prefix, option.upper(), getattr(self.options, option)) 33 | 34 | def build(self): 35 | cmake = CMake(self.settings) 36 | 37 | options = '' 38 | options += self.cmake_option('stack_walking_unwind') 39 | options += self.cmake_option('stack_walking_backtrace') 40 | options += self.cmake_option('stack_details_auto_detect') 41 | options += self.cmake_option('stack_details_backtrace_symbol') 42 | options += self.cmake_option('stack_details_dw') 43 | options += self.cmake_option('stack_details_bfd') 44 | options += self.cmake_option('shared', prefix = 'BACKWARD_') 45 | 46 | self.run('cmake {} {} {} -DBACKWARD_TESTS=OFF'.format(self.conanfile_directory, cmake.command_line, options)) 47 | self.run('cmake --build . {}'.format(cmake.build_config)) 48 | 49 | def package(self): 50 | self.copy('backward.hpp', os.path.join('include', 'backward')) 51 | self.copy('*.a', dst='lib') 52 | self.copy('*.so', dst='lib') 53 | self.copy('*.lib', dst='lib') 54 | self.copy('*.dll', dst='lib') 55 | 56 | def package_info(self): 57 | self.cpp_info.libs = ['backward'] 58 | -------------------------------------------------------------------------------- /src/backward_ros/doc/nice.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/backward_ros/doc/nice.png -------------------------------------------------------------------------------- /src/backward_ros/doc/pretty.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/backward_ros/doc/pretty.png -------------------------------------------------------------------------------- /src/backward_ros/doc/rude.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/backward_ros/doc/rude.png -------------------------------------------------------------------------------- /src/backward_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | backward_ros 4 | 0.1.7 5 | The backward_ros package is a ros wrapper of backward-cpp from https://github.com/bombela/backward-cpp 6 | 7 | Victor López 8 | 9 | MIT 10 | 11 | catkin 12 | roscpp 13 | libdw-dev 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /src/backward_ros/src/backward.cpp: -------------------------------------------------------------------------------- 1 | // Pick your poison. 2 | // 3 | // On GNU/Linux, you have few choices to get the most out of your stack trace. 4 | // 5 | // By default you get: 6 | // - object filename 7 | // - function name 8 | // 9 | // In order to add: 10 | // - source filename 11 | // - line and column numbers 12 | // - source code snippet (assuming the file is accessible) 13 | 14 | // Install one of the following library then uncomment one of the macro (or 15 | // better, add the detection of the lib and the macro definition in your build 16 | // system) 17 | 18 | // - apt-get install libdw-dev ... 19 | // - g++/clang++ -ldw ... 20 | // #define BACKWARD_HAS_DW 1 21 | 22 | // - apt-get install binutils-dev ... 23 | // - g++/clang++ -lbfd ... 24 | // #define BACKWARD_HAS_BFD 1 25 | 26 | #include 27 | 28 | namespace backward { 29 | 30 | backward::SignalHandling sh; 31 | 32 | } // namespace backward 33 | -------------------------------------------------------------------------------- /src/backward_ros/test/select_signals.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test/segfault.cpp 3 | * Copyright 2013 Google Inc. All Rights Reserved. 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy 6 | * of this software and associated documentation files (the "Software"), to deal 7 | * in the Software without restriction, including without limitation the rights 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | * copies of the Software, and to permit persons to whom the Software is 10 | * furnished to do so, subject to the following conditions: 11 | * 12 | * The above copyright notice and this permission notice shall be included in 13 | * all copies or substantial portions of the Software. 14 | * 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | * SOFTWARE. 22 | */ 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | #include "test.hpp" 29 | 30 | using namespace backward; 31 | 32 | void badass_function() { 33 | char* ptr = (char*)42; 34 | *ptr = 42; 35 | } 36 | 37 | TEST_SEGFAULT (pprint_sigsev) { 38 | std::vector signals; 39 | signals.push_back(SIGSEGV); 40 | SignalHandling sh(signals); 41 | std::cout << std::boolalpha << "sh.loaded() == " << sh.loaded() << std::endl; 42 | badass_function(); 43 | } 44 | 45 | TEST_SEGFAULT (wont_pprint) { 46 | std::vector signals; 47 | signals.push_back(SIGABRT); 48 | SignalHandling sh(signals); 49 | std::cout << std::boolalpha << "sh.loaded() == " << sh.loaded() << std::endl; 50 | badass_function(); 51 | } 52 | -------------------------------------------------------------------------------- /src/backward_ros/test/stacktrace.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test/stacktrace.cpp 3 | * Copyright 2013 Google Inc. All Rights Reserved. 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy 6 | * of this software and associated documentation files (the "Software"), to deal 7 | * in the Software without restriction, including without limitation the rights 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | * copies of the Software, and to permit persons to whom the Software is 10 | * furnished to do so, subject to the following conditions: 11 | * 12 | * The above copyright notice and this permission notice shall be included in 13 | * all copies or substantial portions of the Software. 14 | * 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | * SOFTWARE. 22 | */ 23 | 24 | #include 25 | #include 26 | #include "test.hpp" 27 | 28 | using namespace backward; 29 | 30 | void collect_trace(StackTrace& st) { 31 | st.load_here(); 32 | } 33 | 34 | TEST (minitrace) { 35 | StackTrace st; 36 | collect_trace(st); 37 | 38 | Printer printer; 39 | printer.print(st, stdout); 40 | } 41 | 42 | void d(StackTrace& st) { 43 | st.load_here(); 44 | } 45 | 46 | void c(StackTrace& st) { 47 | return d(st); 48 | } 49 | 50 | void b(StackTrace& st) { 51 | return c(st); 52 | } 53 | 54 | __attribute__ ((noinline)) 55 | void a(StackTrace& st) { 56 | return b(st); 57 | } 58 | 59 | TEST (smalltrace) { 60 | StackTrace st; 61 | a(st); 62 | 63 | Printer printer; 64 | printer.print(st, stdout); 65 | } 66 | -------------------------------------------------------------------------------- /src/backward_ros/test/test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test/test.cpp 3 | * Copyright 2013 Google Inc. All Rights Reserved. 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy 6 | * of this software and associated documentation files (the "Software"), to deal 7 | * in the Software without restriction, including without limitation the rights 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | * copies of the Software, and to permit persons to whom the Software is 10 | * furnished to do so, subject to the following conditions: 11 | * 12 | * The above copyright notice and this permission notice shall be included in 13 | * all copies or substantial portions of the Software. 14 | * 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | * SOFTWARE. 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | #include "test.hpp" 28 | 29 | TEST (empty_test) { } 30 | 31 | TEST_FAIL_ASSERT (fail_assert) { 32 | ASSERT(1 == 2); 33 | } 34 | 35 | TEST_FAIL_ASSERT (fail_assert_ge) { 36 | ASSERT_GE(4, 5); 37 | } 38 | 39 | TEST_UNCAUGHT_EXCEPTION (uncaught_exception) { 40 | throw std::runtime_error("some random runtime error"); 41 | } 42 | 43 | TEST_UNCAUGHT_EXCEPTION (uncaught_exception_int) { 44 | throw 42; 45 | } 46 | 47 | TEST_SEGFAULT (segfault) { 48 | char* a = 0; 49 | char b = a[42]; 50 | std::cout << "result: " << b << std::endl; 51 | } 52 | 53 | TEST_ABORT (abort) { 54 | abort(); 55 | } 56 | 57 | TEST (catch_int) { 58 | ASSERT_THROW({throw 42;}, int); 59 | } 60 | 61 | TEST_FAIL_ASSERT (fail_catch_int) { 62 | ASSERT_THROW({}, int); 63 | } 64 | 65 | TEST_FAIL_ASSERT (fail_no_throw) { 66 | ASSERT_NO_THROW({throw 42;}); 67 | } 68 | 69 | TEST (any_throw) { 70 | ASSERT_ANY_THROW({throw 42;}); 71 | } 72 | 73 | TEST_FAIL_ASSERT (fail_any_throw) { 74 | ASSERT_ANY_THROW({}); 75 | } 76 | -------------------------------------------------------------------------------- /src/backward_ros/test_package/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(backward-package-test) 2 | cmake_minimum_required(VERSION 2.8) 3 | 4 | include(${CMAKE_BINARY_DIR}/conanbuildinfo.cmake) 5 | include(${CONAN_CMAKE-UTILS_ROOT}/conan.cmake) 6 | conan_basic_setup() 7 | 8 | add_conan_library(backward) 9 | 10 | add_executable(example main.cpp) 11 | target_link_libraries(example PRIVATE backward-conan) 12 | -------------------------------------------------------------------------------- /src/backward_ros/test_package/conanfile.py: -------------------------------------------------------------------------------- 1 | from conans import ConanFile, CMake 2 | import os 3 | 4 | class TestBackward(ConanFile): 5 | settings = 'os', 'compiler', 'build_type', 'arch' 6 | requires = 'cmake-utils/0.0.0@Manu343726/testing', 'backward/1.3.0@Manu343726/testing' 7 | generators = 'cmake' 8 | 9 | def build(self): 10 | cmake = CMake(self.settings) 11 | self.run('cmake {} {}'.format(self.conanfile_directory, cmake.command_line)) 12 | self.run('cmake --build . {}'.format(cmake.build_config)) 13 | 14 | def test(self): 15 | self.run(os.path.join('.', 'bin', 'example')) 16 | -------------------------------------------------------------------------------- /src/backward_ros/test_package/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace backward; 7 | 8 | class TracedException : public std::runtime_error 9 | { 10 | public: 11 | TracedException() : 12 | std::runtime_error(_get_trace()) 13 | {} 14 | 15 | private: 16 | std::string _get_trace() 17 | { 18 | std::ostringstream ss; 19 | 20 | StackTrace stackTrace; 21 | TraceResolver resolver; 22 | stackTrace.load_here(); 23 | resolver.load_stacktrace(stackTrace); 24 | 25 | for(std::size_t i = 0; i < stackTrace.size(); ++i) 26 | { 27 | const ResolvedTrace trace = resolver.resolve(stackTrace[i]); 28 | 29 | ss << "#" << i << " at " << trace.object_function << "\n"; 30 | } 31 | 32 | return ss.str(); 33 | } 34 | }; 35 | 36 | void f(int i) 37 | { 38 | if(i >= 42) 39 | { 40 | throw TracedException(); 41 | } 42 | else 43 | { 44 | std::cout << "i=" << i << "\n"; 45 | f(i + 1); 46 | } 47 | } 48 | 49 | int main() 50 | { 51 | try 52 | { 53 | f(0); 54 | } catch (const TracedException& ex) 55 | { 56 | std::cout << ex.what(); 57 | } 58 | } 59 | 60 | 61 | -------------------------------------------------------------------------------- /src/occupancy_mapping_benchmarks/.gitignore: -------------------------------------------------------------------------------- 1 | /cmake-build-debug/ 2 | *.bag 3 | /.idea 4 | /build 5 | -------------------------------------------------------------------------------- /src/occupancy_mapping_benchmarks/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(occupancy_mapping_benchmarks) 3 | 4 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/cmake/") 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | rosbag 8 | rosfmt 9 | backward_ros 10 | sensor_msgs 11 | tf 12 | pcl_ros 13 | pcl_conversions 14 | octomap_ros 15 | tf2_ros 16 | skimap_ros 17 | ) 18 | 19 | find_package(octomap REQUIRED) 20 | find_package(OpenVDB REQUIRED) 21 | find_package(TBB REQUIRED) 22 | 23 | catkin_package() 24 | 25 | add_compile_options (-fdiagnostics-color=always) 26 | 27 | add_executable(benchmark 28 | src/benchmark.cc 29 | ) 30 | target_include_directories(benchmark PUBLIC 31 | ${catkin_INCLUDE_DIRS} 32 | ${OCTOMAP_INCLUDE_DIRS} 33 | ${OpenVDB_INCLUDE_DIR} 34 | ) 35 | target_link_libraries(benchmark 36 | ${catkin_LIBRARIES} 37 | ${OCTOMAP_LIBRARIES} 38 | ${OpenVDB_LIBRARIES} 39 | ${TBB_LIBRARIES} 40 | # Sad times.. 41 | -lHalf 42 | ) 43 | -------------------------------------------------------------------------------- /src/occupancy_mapping_benchmarks/README.md: -------------------------------------------------------------------------------- 1 | ## Installation 2 | 3 | ``` 4 | mkdir -p ~/mapping_benchmark_ws/src 5 | cd ~/mapping_benchmark_ws 6 | catkin init 7 | catkin config --extend /opt/ros/melodic --merge-devel --cmake-args -DCMAKE_BUILD_TYPE=Release 8 | 9 | cd src 10 | git clone https://github.com/nicolov/mapping-benchmarks 11 | wstool init . ./occupancy_mapping_benchmarks/ws_https.rosinstall 12 | wstool update 13 | 14 | catkin build 15 | ``` 16 | -------------------------------------------------------------------------------- /src/occupancy_mapping_benchmarks/launch/octomap_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /src/occupancy_mapping_benchmarks/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | occupancy_mapping_benchmarks 4 | 0.0.1 5 | occupancy_mapping_benchmarks 6 | nicolov 7 | BSD 8 | catkin 9 | 10 | rosbag 11 | octomap 12 | octomap_ros 13 | rosfmt 14 | backward_ros 15 | sensor_msgs 16 | tf 17 | pcl_ros 18 | pcl_conversions 19 | tf2_ros 20 | skimap_ros 21 | 22 | 23 | -------------------------------------------------------------------------------- /src/occupancy_mapping_benchmarks/scripts/make_plots.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Draw pretty plots. 5 | """ 6 | 7 | import os 8 | 9 | import matplotlib 10 | matplotlib.use('Agg') 11 | 12 | from matplotlib import pyplot as plt 13 | from tabulate import tabulate 14 | import numpy as np 15 | import pandas as pd 16 | import seaborn as sns 17 | 18 | def main(): 19 | df = pd.read_csv('bench_output.txt', header=None, names=['name', 'Time']) 20 | 21 | # table 22 | 23 | with open('bench_table.md', 'w') as f: 24 | f.write(tabulate(df, tablefmt='pipe', showindex=False, headers=['Library', 'Runtime [sec]'])) 25 | 26 | # plot 27 | 28 | sns.set() 29 | f, ax = plt.subplots(figsize=(5, 3.75)) 30 | df.plot.bar(ax=ax, x='name', y='Time', rot=0, legend=False, color=sns.color_palette()[0]) 31 | ax.set_title('Map building time') 32 | ax.set_xlabel('') 33 | ax.set_ylabel('Runtime [sec]') 34 | 35 | plt.tight_layout() 36 | f.savefig('bench_plot.png', bbox_inches='tight') 37 | 38 | 39 | if __name__ == '__main__': 40 | main() 41 | -------------------------------------------------------------------------------- /src/octomap/.gitignore: -------------------------------------------------------------------------------- 1 | # temp. editor backups: 2 | *~ 3 | 4 | # generated content / project files: 5 | .project 6 | .cproject 7 | doc 8 | html 9 | .qglviewer.xml 10 | 11 | # build output: 12 | lib 13 | bin 14 | build 15 | callgrind.out* 16 | 17 | # in case someone runs an in-source build: 18 | CMakeCache.txt 19 | cmake_install.cmake 20 | CPackConfig.cmake 21 | CMakeFiles 22 | CPackSourceConfig.cmake 23 | install_manifest.txt 24 | Makefile 25 | 26 | # our binary files, in case someone forgets them in the source dir: 27 | *.bt 28 | *.ot 29 | *.graph -------------------------------------------------------------------------------- /src/octomap/.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | sudo: required 3 | dist: trusty 4 | compiler: 5 | - gcc 6 | - clang 7 | before_install: 8 | - sudo apt-get update -qq 9 | - sudo apt-get install -qq libqt4-dev libqt4-opengl-dev libqglviewer-dev 10 | before_script: 11 | script: ./scripts/travis_build_jobs.sh $VARIANT 12 | env: 13 | - VARIANT=dist 14 | - VARIANT=components 15 | matrix: 16 | exclude: 17 | - compiler: clang 18 | env: VARIANT=components 19 | 20 | -------------------------------------------------------------------------------- /src/octomap/CATKIN_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/CATKIN_IGNORE -------------------------------------------------------------------------------- /src/octomap/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 2.8.8) 2 | PROJECT( octomap-distribution ) 3 | 4 | ENABLE_TESTING() # enable CTest environment of subprojects 5 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) # enables -fPIC in applicable compilers (required to avoid link errors in some cases) 6 | 7 | option(BUILD_OCTOVIS_SUBPROJECT "Build targets from subproject octovis" ON) 8 | option(BUILD_DYNAMICETD3D_SUBPROJECT "Build targets from subproject dynamicEDT3D" ON) 9 | option(OCTOVIS_QT5 "Link Octovis against Qt5?" NO) 10 | 11 | if(OCTOVIS_QT5) 12 | # Compiling against QT5 requires C++11. 13 | set(CMAKE_CXX_STANDARD 11) 14 | endif(OCTOVIS_QT5) 15 | 16 | ADD_SUBDIRECTORY( octomap ) 17 | 18 | if(BUILD_OCTOVIS_SUBPROJECT) 19 | ADD_SUBDIRECTORY( octovis ) 20 | endif() 21 | 22 | if(BUILD_DYNAMICETD3D_SUBPROJECT) 23 | ADD_SUBDIRECTORY( dynamicEDT3D ) 24 | endif() 25 | 26 | 27 | -------------------------------------------------------------------------------- /src/octomap/README.md: -------------------------------------------------------------------------------- 1 | OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees. 2 | =========================================================================== 3 | 4 | http://octomap.github.io 5 | 6 | Originally developed by Kai M. Wurm and Armin Hornung, University of Freiburg, Copyright (C) 2009-2014. 7 | Currently maintained by [Armin Hornung](https://github.com/ahornung). 8 | See the [list of contributors](octomap/AUTHORS.txt) for further authors. 9 | 10 | License: 11 | * octomap: [New BSD License](octomap/LICENSE.txt) 12 | * octovis and related libraries: [GPL](octovis/LICENSE.txt) 13 | 14 | 15 | Download the latest releases: 16 | https://github.com/octomap/octomap/releases 17 | 18 | API documentation: 19 | http://octomap.github.com/octomap/doc/ 20 | 21 | Build status: 22 | [![Build Status](https://travis-ci.org/OctoMap/octomap.png?branch=devel)](https://travis-ci.org/OctoMap/octomap) 23 | 24 | Report bugs and request features in our tracker: 25 | https://github.com/OctoMap/octomap/issues 26 | 27 | A list of changes is available in the [octomap changelog](octomap/CHANGELOG.txt) 28 | 29 | 30 | OVERVIEW 31 | -------- 32 | 33 | OctoMap consists of two separate libraries each in its own subfolder: 34 | **octomap**, the actual library, and **octovis**, our visualization libraries and tools. 35 | This README provides an overview of both, for details on compiling each please 36 | see [octomap/README.md](octomap/README.md) and [octovis/README.md](octovis/README.md) respectively. 37 | See http://www.ros.org/wiki/octomap and http://www.ros.org/wiki/octovis if you 38 | want to use OctoMap in ROS; there are pre-compiled packages available. 39 | 40 | You can build each library separately with CMake by running it from the subdirectories, 41 | or build octomap and octovis together from this top-level directory. E.g., to 42 | only compile the library, run: 43 | 44 | cd octomap 45 | mkdir build 46 | cd build 47 | cmake .. 48 | make 49 | 50 | To compile the complete package, run: 51 | 52 | cd build 53 | cmake .. 54 | make 55 | 56 | Binaries and libs will end up in the directories `bin` and `lib` of the 57 | top-level directory where you started the build. 58 | 59 | 60 | See [octomap README](octomap/README.md) and [octovis README](octovis/README.md) for further 61 | details and hints on compiling, especially under Windows. 62 | -------------------------------------------------------------------------------- /src/octomap/dynamicEDT3D/CHANGELOG.txt: -------------------------------------------------------------------------------- 1 | v1.6.8: 2014-09-06 2 | ================== 3 | - fixing package.xml installation location, now in /share/dynamic_edt_3d 4 | 5 | v1.6.7: 2014-08-31 6 | ================== 7 | - Support for SOVERSION in the library for better packaging 8 | - fixing ${DYNAMICEDT3D_LIBRARIES} to have full path 9 | - fixing location of dynamicEDT3DConfig.cmake 10 | - fixing package.xml installation 11 | -------------------------------------------------------------------------------- /src/octomap/dynamicEDT3D/CMakeModules/CMakeUninstall.cmake.in: -------------------------------------------------------------------------------- 1 | if (NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") 2 | message(FATAL_ERROR "Cannot find install manifest: \"@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt\"") 3 | endif(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") 4 | 5 | file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files) 6 | string(REGEX REPLACE "\n" ";" files "${files}") 7 | list(REVERSE files) 8 | foreach (file ${files}) 9 | message(STATUS "Uninstalling \"$ENV{DESTDIR}${file}\"") 10 | if (EXISTS "$ENV{DESTDIR}${file}") 11 | execute_process( 12 | COMMAND @CMAKE_COMMAND@ -E remove "$ENV{DESTDIR}${file}" 13 | OUTPUT_VARIABLE rm_out 14 | RESULT_VARIABLE rm_retval 15 | ) 16 | if(NOT ${rm_retval} EQUAL 0) 17 | message(FATAL_ERROR "Problem when removing \"$ENV{DESTDIR}${file}\"") 18 | endif (NOT ${rm_retval} EQUAL 0) 19 | else (EXISTS "$ENV{DESTDIR}${file}") 20 | message(STATUS "File \"$ENV{DESTDIR}${file}\" does not exist.") 21 | endif (EXISTS "$ENV{DESTDIR}${file}") 22 | endforeach(file) 23 | -------------------------------------------------------------------------------- /src/octomap/dynamicEDT3D/CMakeModules/CPackSettings.cmake: -------------------------------------------------------------------------------- 1 | # Package building stuff - Experimental! 2 | SET(CPACK_PACKAGE_VERSION "${OCTOMAP_VERSION}") 3 | SET(CPACK_PACKAGE_VERSION_MAJOR "${OCTOMAP_MAJOR_VERSION}") 4 | SET(CPACK_PACKAGE_VERSION_MINOR "${OCTOMAP_MINOR_VERSION}") 5 | SET(CPACK_PACKAGE_VERSION_PATCH "${OCTOMAP_PATCH_VERSION}") 6 | SET(CPACK_PACKAGE_INSTALL_DIRECTORY "CMake ${V_MAJOR}.${V_MINOR}") 7 | SET(CPACK_PACKAGE_CONTACT "K.M. Wurm and A. Hornung") 8 | SET(CPACK_PACKAGE_VENDOR "University of Freiburg") 9 | SET(CPACK_GENERATOR "DEB") 10 | SET(CPACK_SOURCE_GENERATOR "TGZ") 11 | SET(CPACK_SOURCE_PACKAGE_FILE_NAME 12 | "${PROJECT_NAME}-${CPACK_PACKAGE_VERSION_MAJOR}.${CPACK_PACKAGE_VERSION_MINOR}.${CPACK_PACKAGE_VERSION_PATCH}") 13 | 14 | # This must always be last statement! 15 | INCLUDE(CPack) 16 | 17 | -------------------------------------------------------------------------------- /src/octomap/dynamicEDT3D/CMakeModules/CompilerSettings.cmake: -------------------------------------------------------------------------------- 1 | # COMPILER SETTINGS (default: Release) 2 | # use "-DCMAKE_BUILD_TYPE=Debug" in cmake for a Debug-build 3 | IF(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) 4 | SET(CMAKE_BUILD_TYPE Release) 5 | ENDIF(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) 6 | 7 | MESSAGE ("\n") 8 | MESSAGE (STATUS "${PROJECT_NAME} building as ${CMAKE_BUILD_TYPE}") 9 | 10 | # OCTOMAP_OMP = enable OpenMP 11 | # SET(OCTOMAP_OMP 1 CACHE BOOL "Enable/disable OpenMP") 12 | # IF($ENV{OCTOMAP_OMP}) 13 | # SET(OCTOMAP_OMP $ENV{OCTOMAP_OMP}) 14 | # MESSAGE(STATUS "Found OCTOMAP_OMP=${OCTOMAP_OMP}") 15 | # ENDIF($ENV{OCTOMAP_OMP}) 16 | 17 | # COMPILER FLAGS 18 | IF (CMAKE_COMPILER_IS_GNUCC) 19 | SET (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wno-error ") 20 | SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-error ") 21 | SET (CMAKE_CXX_FLAGS_RELEASE "-O3 -funroll-loops -DNDEBUG") 22 | SET (CMAKE_CXX_FLAGS_DEBUG "-O0 -g") 23 | # Shared object compilation under 64bit (vtable) 24 | ADD_DEFINITIONS(-fPIC) 25 | # IF(OCTOMAP_OMP) 26 | # SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fopenmp") 27 | # SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -fopenmp") 28 | # ENDIF(OCTOMAP_OMP) 29 | ENDIF() 30 | 31 | 32 | # Set full rpath http://www.paraview.org/Wiki/CMake_RPATH_handling 33 | # (good to have and required with ROS) 34 | set(CMAKE_SKIP_BUILD_RPATH FALSE) 35 | set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) 36 | set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") 37 | set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) 38 | 39 | # no prefix needed for python modules 40 | set(CMAKE_SHARED_MODULE_PREFIX "") 41 | -------------------------------------------------------------------------------- /src/octomap/dynamicEDT3D/LICENSE.txt: -------------------------------------------------------------------------------- 1 | 2 | dynamicEDT3D: 3 | A library for incrementally updatable Euclidean distance transforms in 3D. 4 | 5 | License for the "dynamicEDT3D" library: New BSD License. 6 | 7 | Copyright (c) 2011-2012, C. Sprunk, B. Lau, W. Burgard, University of Freiburg 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright 14 | notice, this list of conditions and the following disclaimer. 15 | * Redistributions in binary form must reproduce the above copyright 16 | notice, this list of conditions and the following disclaimer in the 17 | documentation and/or other materials provided with the distribution. 18 | * Neither the name of the University of Freiburg nor the names of its 19 | contributors may be used to endorse or promote products derived from 20 | this software without specific prior written permission. 21 | 22 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 25 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 26 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 27 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 28 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 29 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 30 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | POSSIBILITY OF SUCH DAMAGE. 33 | -------------------------------------------------------------------------------- /src/octomap/dynamicEDT3D/README.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/dynamicEDT3D/README.txt -------------------------------------------------------------------------------- /src/octomap/dynamicEDT3D/doxygen.h: -------------------------------------------------------------------------------- 1 | 2 | /** 3 | * \namespace dynamicEDT3D Namespace of the dynamicEDT3D library 4 | * 5 | */ 6 | 7 | 8 | /** \mainpage dynamicEDT3D 9 | 10 | \section intro_sec Introduction 11 | 12 | The dynamicEDT3D library implements an inrementally updatable Euclidean distance transform (EDT) in 3D. It comes with a wrapper to use the OctoMap 3D representation and hooks into the change detection of the OctoMap library to propagate changes to the EDT. 13 | 14 | \section install_sec Installation 15 |

See the file README.txt in the main folder. 16 |

17 | 18 | \section gettingstarted_sec Getting Started 19 |

There are two example programs in the src/examples directory that show the basic functionality of the library.

20 | 21 | 22 | \section changelog_sec Changelog 23 |

See the file CHANGELOG.txt in the main folder or the latest version online 24 |

25 | 26 | \section using_sec Using dynamicEDT3D? 27 |

Please let us know if you are using dynamicEDT3D, as we are curious to find out how it enables other people's work or research. Additionally, please cite our paper if you use dynamicEDT3D in your research:

28 |

B. Lau, C. Sprunk, and W. Burgard, 29 | "Efficient Grid-based Spatial Representations for Robot Navigation in Dynamic Environments" in 30 | Robotics and Autonomous Systems, 2012. Accepted for publication. 31 | Software available at http://octomap.sf.net/. 32 |

33 |

BibTeX:

34 | 
@@article{lau12ras,
35 |   author = {Boris Lau and Christoph Sprunk and Wolfram Burgard},
36 |   title = {Efficient Grid-based Spatial Representations for Robot Navigation in Dynamic Environments},
37 |   journal = {Robotics and Autonomous Systems (RAS)},
38 |   year = {2012},
39 |   url = {http://octomap.sf.net/},
40 |   note = {Accepted for publication. Software available at \url{http://octomap.sf.net/}}
41 | }
42 | 43 | **/ 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /src/octomap/dynamicEDT3D/dynamicEDT3DConfig.cmake.in: -------------------------------------------------------------------------------- 1 | # - Config file for the dynamicEDT3D package 2 | # 3 | # Usage from an external project: 4 | # In your CMakeLists.txt, add these lines: 5 | # 6 | # FIND_PACKAGE(dynamicedt3d REQUIRED ) 7 | # INCLUDE_DIRECTORIES(${DYNAMICEDT3D_INCLUDE_DIRS}) 8 | # TARGET_LINK_LIBRARIES(MY_TARGET_NAME ${DYNAMICEDT3D_LIBRARIES}) 9 | # 10 | # It defines the following variables 11 | # DYNAMICEDT3D_INCLUDE_DIRS - include directories for dynamicEDT3D 12 | # DYNAMICEDT3D_LIBRARY_DIRS - library directories for dynamicEDT3D (normally not used!) 13 | # DYNAMICEDT3D_LIBRARIES - libraries to link against 14 | # DYNAMICEDT3D_MAJOR_VERSION - major version 15 | # DYNAMICEDT3D_MINOR_VERSION - minor version 16 | # DYNAMICEDT3D_PATCH_VERSION - patch version 17 | # DYNAMICEDT3D_VERSION - major.minor.patch version 18 | 19 | @PACKAGE_INIT@ 20 | 21 | set(DYNAMICEDT3D_MAJOR_VERSION "@DYNAMICEDT3D_MAJOR_VERSION@") 22 | set(DYNAMICEDT3D_MINOR_VERSION "@DYNAMICEDT3D_MINOR_VERSION@") 23 | set(DYNAMICEDT3D_PATCH_VERSION "@DYNAMICEDT3D_PATCH_VERSION@") 24 | set(DYNAMICEDT3D_VERSION "@DYNAMICEDT3D_VERSION@") 25 | 26 | # Tell the user project where to find our headers and libraries 27 | set_and_check(DYNAMICEDT3D_INCLUDE_DIRS "@PACKAGE_DYNAMICEDT3D_INCLUDE_DIRS@") 28 | set_and_check(DYNAMICEDT3D_LIBRARY_DIRS "@PACKAGE_DYNAMICEDT3D_LIB_DIR@") 29 | 30 | set(DYNAMICEDT3D_LIBRARIES "@PACKAGE_DYNAMICEDT3D_LIB_DIR@/@DYNAMICEDT3D_LIBRARY@") 31 | 32 | @DYNAMICEDT3D_INCLUDE_TARGETS@ 33 | -------------------------------------------------------------------------------- /src/octomap/dynamicEDT3D/package.xml: -------------------------------------------------------------------------------- 1 | 2 | dynamic_edt_3d 3 | 1.9.0 4 | The dynamicEDT3D library implements an inrementally updatable Euclidean distance transform (EDT) in 3D. It comes with a wrapper to use the OctoMap 3D representation and hooks into the change detection of the OctoMap library to propagate changes to the EDT. 5 | 6 | Christoph Sprunk 7 | Christoph Sprunk 8 | BSD 9 | 10 | http://octomap.github.io 11 | https://github.com/OctoMap/octomap/issues 12 | 13 | octomap 14 | 15 | octomap 16 | catkin 17 | 18 | cmake 19 | 20 | 21 | 22 | cmake 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /src/octomap/dynamicEDT3D/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET( dynamicEDT3D_SRCS 2 | dynamicEDT3D.cpp 3 | ) 4 | 5 | add_library(dynamicedt3d SHARED ${dynamicEDT3D_SRCS}) 6 | set_target_properties(dynamicedt3d PROPERTIES 7 | VERSION ${DYNAMICEDT3D_VERSION} 8 | SOVERSION ${DYNAMICEDT3D_SOVERSION} 9 | ) 10 | target_link_libraries(dynamicedt3d ${OCTOMAP_LIBRARIES}) 11 | 12 | add_library(dynamicedt3d-static STATIC ${dynamicEDT3D_SRCS}) 13 | target_link_libraries(dynamicedt3d-static ${OCTOMAP_LIBRARIES}) 14 | 15 | SET_TARGET_PROPERTIES(dynamicedt3d-static PROPERTIES OUTPUT_NAME "dynamicedt3d") 16 | 17 | if(NOT EXISTS "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}/cmake/dynamicEDT3D") 18 | file(MAKE_DIRECTORY "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}/cmake/dynamicEDT3D") 19 | endif() 20 | 21 | export(TARGETS dynamicedt3d dynamicedt3d-static 22 | FILE "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}/cmake/dynamicEDT3D/dynamicEDT3DTargets.cmake") 23 | 24 | # directly depend on the octomap library target when building the 25 | # complete distribution, so it it recompiled as needed 26 | if (CMAKE_PROJECT_NAME STREQUAL "octomap-distribution") 27 | ADD_DEPENDENCIES(dynamicedt3d-static octomap-static) 28 | ADD_DEPENDENCIES(dynamicedt3d octomap) 29 | endif() 30 | 31 | ADD_SUBDIRECTORY(examples) 32 | 33 | install(TARGETS dynamicedt3d dynamicedt3d-static 34 | EXPORT dynamicEDT3DTargets 35 | INCLUDES DESTINATION include 36 | ${INSTALL_TARGETS_DEFAULT_ARGS} 37 | ) 38 | install(EXPORT dynamicEDT3DTargets DESTINATION share/dynamicEDT3D/) 39 | -------------------------------------------------------------------------------- /src/octomap/dynamicEDT3D/src/examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(exampleEDT3D exampleEDT3D.cpp) 2 | target_link_libraries(exampleEDT3D dynamicedt3d) 3 | 4 | add_executable(exampleEDTOctomap exampleEDTOctomap.cpp) 5 | target_link_libraries(exampleEDTOctomap dynamicedt3d) 6 | 7 | add_executable(exampleEDTOctomapStamped exampleEDTOctomapStamped.cpp) 8 | target_link_libraries(exampleEDTOctomapStamped dynamicedt3d) -------------------------------------------------------------------------------- /src/octomap/octomap/AUTHORS.txt: -------------------------------------------------------------------------------- 1 | OctoMap was originally developed by Kai M. Wurm and Armin Hornung, 2 | University of Freiburg 3 | 4 | Further contributors: 5 | * C. Sprunk, University of Freiburg 6 | * J. Mueller, University of Freiburg 7 | * S. Osswald, University of Freiburg 8 | * R. Schmitt, University of Freiburg 9 | * R. Bogdan Rusu, Willow Garage Inc. 10 | * C. Dornhege, University of Freiburg 11 | * F-M. de Rainville, Universite Laval Quebec 12 | * B. Jensen, TU Munich 13 | * A. Ecins, University of Maryland 14 | * C. Brew 15 | * F. Boniardi, University of Freiburg -------------------------------------------------------------------------------- /src/octomap/octomap/CMakeModules/CMakeUninstall.cmake.in: -------------------------------------------------------------------------------- 1 | # Ignore empty list items. 2 | cmake_policy(SET CMP0007 OLD) 3 | 4 | if (NOT EXISTS "@CMAKE_BINARY_DIR@/install_manifest.txt") 5 | message(FATAL_ERROR "Cannot find install manifest: \"@CMAKE_BINARY_DIR@/install_manifest.txt\"") 6 | endif(NOT EXISTS "@CMAKE_BINARY_DIR@/install_manifest.txt") 7 | 8 | file(READ "@CMAKE_BINARY_DIR@/install_manifest.txt" files) 9 | string(REGEX REPLACE "\n" ";" files "${files}") 10 | list(REVERSE files) 11 | foreach (file ${files}) 12 | message(STATUS "Uninstalling \"$ENV{DESTDIR}${file}\"") 13 | if (EXISTS "$ENV{DESTDIR}${file}") 14 | execute_process( 15 | COMMAND @CMAKE_COMMAND@ -E remove "$ENV{DESTDIR}${file}" 16 | OUTPUT_VARIABLE rm_out 17 | RESULT_VARIABLE rm_retval 18 | ) 19 | if(NOT ${rm_retval} EQUAL 0) 20 | message(FATAL_ERROR "Problem when removing \"$ENV{DESTDIR}${file}\"") 21 | endif (NOT ${rm_retval} EQUAL 0) 22 | else (EXISTS "$ENV{DESTDIR}${file}") 23 | message(STATUS "File \"$ENV{DESTDIR}${file}\" does not exist.") 24 | endif (EXISTS "$ENV{DESTDIR}${file}") 25 | endforeach(file) 26 | -------------------------------------------------------------------------------- /src/octomap/octomap/CMakeModules/CPackSettings.cmake: -------------------------------------------------------------------------------- 1 | # Package building stuff - Experimental! 2 | SET(CPACK_PACKAGE_VERSION "${OCTOMAP_VERSION}") 3 | SET(CPACK_PACKAGE_VERSION_MAJOR "${OCTOMAP_MAJOR_VERSION}") 4 | SET(CPACK_PACKAGE_VERSION_MINOR "${OCTOMAP_MINOR_VERSION}") 5 | SET(CPACK_PACKAGE_VERSION_PATCH "${OCTOMAP_PATCH_VERSION}") 6 | SET(CPACK_PACKAGE_INSTALL_DIRECTORY "CMake ${V_MAJOR}.${V_MINOR}") 7 | SET(CPACK_PACKAGE_CONTACT "K.M. Wurm and A. Hornung") 8 | SET(CPACK_PACKAGE_VENDOR "University of Freiburg") 9 | SET(CPACK_GENERATOR "DEB") 10 | SET(CPACK_SOURCE_GENERATOR "TGZ") 11 | SET(CPACK_SOURCE_PACKAGE_FILE_NAME 12 | "${PROJECT_NAME}-${CPACK_PACKAGE_VERSION_MAJOR}.${CPACK_PACKAGE_VERSION_MINOR}.${CPACK_PACKAGE_VERSION_PATCH}") 13 | 14 | # This must always be last statement! 15 | INCLUDE(CPack) 16 | 17 | -------------------------------------------------------------------------------- /src/octomap/octomap/CMakeModules/CompilerSettings.cmake: -------------------------------------------------------------------------------- 1 | # COMPILER SETTINGS (default: Release) 2 | # use "-DCMAKE_BUILD_TYPE=Debug" in cmake for a Debug-build 3 | IF(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) 4 | SET(CMAKE_BUILD_TYPE Release) 5 | ENDIF(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) 6 | 7 | MESSAGE ("\n") 8 | MESSAGE (STATUS "${PROJECT_NAME} building as ${CMAKE_BUILD_TYPE}") 9 | 10 | # COMPILER FLAGS 11 | IF (CMAKE_COMPILER_IS_GNUCC) 12 | SET (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wno-error ") 13 | SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-error ") 14 | SET (CMAKE_CXX_FLAGS_RELEASE "-O3 -funroll-loops -DNDEBUG") 15 | SET (CMAKE_CXX_FLAGS_DEBUG "-O0 -g") 16 | # Shared object compilation under 64bit (vtable) 17 | ADD_DEFINITIONS(-fPIC) 18 | ENDIF() 19 | 20 | 21 | # Set full rpath http://www.paraview.org/Wiki/CMake_RPATH_handling 22 | # (good to have and required with ROS) 23 | set(CMAKE_SKIP_BUILD_RPATH FALSE) 24 | set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) 25 | set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") 26 | set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) 27 | 28 | # no prefix needed for python modules 29 | set(CMAKE_SHARED_MODULE_PREFIX "") 30 | -------------------------------------------------------------------------------- /src/octomap/octomap/LICENSE.txt: -------------------------------------------------------------------------------- 1 | 2 | OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees 3 | 4 | License for the "octomap" library: New BSD License. 5 | 6 | Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg 7 | All rights reserved. 8 | 9 | Redistribution and use in source and binary forms, with or without 10 | modification, are permitted provided that the following conditions are met: 11 | 12 | * Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | * Redistributions in binary form must reproduce the above copyright 15 | notice, this list of conditions and the following disclaimer in the 16 | documentation and/or other materials provided with the distribution. 17 | * Neither the name of the University of Freiburg nor the names of its 18 | contributors may be used to endorse or promote products derived from 19 | this software without specific prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | POSSIBILITY OF SUCH DAMAGE. 32 | -------------------------------------------------------------------------------- /src/octomap/octomap/extern/README_BINVOX: -------------------------------------------------------------------------------- 1 | binvox (and viewvox) binaries are no longer provided in the 2 | OctoMap source. Please obtain the latest versions from: 3 | 4 | http://www.cs.princeton.edu/~min/binvox 5 | 6 | -------------------------------------------------------------------------------- /src/octomap/octomap/include/octomap/math/Utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees 3 | * http://octomap.github.com/ 4 | * 5 | * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg 6 | * All rights reserved. 7 | * License: New BSD 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in the 16 | * documentation and/or other materials provided with the distribution. 17 | * * Neither the name of the University of Freiburg nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef OCTOMATH_UTILS_H 35 | #define OCTOMATH_UTILS_H 36 | 37 | #ifndef M_PI 38 | #define M_PI 3.14159265358979323846 39 | #endif 40 | 41 | #ifndef M_PI_2 42 | #define M_PI_2 1.570796326794896619 43 | #endif 44 | 45 | 46 | #ifndef DEG2RAD 47 | #define DEG2RAD(x) ((x) * 0.01745329251994329575) 48 | #endif 49 | 50 | #ifndef RAD2DEG 51 | #define RAD2DEG(x) ((x) * 57.29577951308232087721) 52 | #endif 53 | 54 | 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /src/octomap/octomap/include/octomap/octomap.h: -------------------------------------------------------------------------------- 1 | /* 2 | * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees 3 | * http://octomap.github.com/ 4 | * 5 | * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg 6 | * All rights reserved. 7 | * License: New BSD 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in the 16 | * documentation and/or other materials provided with the distribution. 17 | * * Neither the name of the University of Freiburg nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #include "octomap_types.h" 35 | #include "Pointcloud.h" 36 | #include "ScanGraph.h" 37 | #include "OcTree.h" 38 | 39 | -------------------------------------------------------------------------------- /src/octomap/octomap/include/octomap/octomap_deprecated.h: -------------------------------------------------------------------------------- 1 | /* 2 | * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees 3 | * http://octomap.github.com/ 4 | * 5 | * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg 6 | * All rights reserved. 7 | * License: New BSD 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in the 16 | * documentation and/or other materials provided with the distribution. 17 | * * Neither the name of the University of Freiburg nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef OCTOMAP_DEPRECATED_H 35 | #define OCTOMAP_DEPRECATED_H 36 | 37 | // define multi-platform deprecation mechanism 38 | #ifndef OCTOMAP_DEPRECATED 39 | #ifdef __GNUC__ 40 | #define OCTOMAP_DEPRECATED(func) func __attribute__ ((deprecated)) 41 | #elif defined(_MSC_VER) 42 | #define OCTOMAP_DEPRECATED(func) __declspec(deprecated) func 43 | #else 44 | #pragma message("WARNING: You need to implement OCTOMAP_DEPRECATED for this compiler") 45 | #define OCTOMAP_DEPRECATED(func) func 46 | #endif 47 | #endif 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /src/octomap/octomap/include/octomap/octomap_timing.h: -------------------------------------------------------------------------------- 1 | /* 2 | * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees 3 | * http://octomap.github.com/ 4 | * 5 | * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg 6 | * All rights reserved. 7 | * License: New BSD 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in the 16 | * documentation and/or other materials provided with the distribution. 17 | * * Neither the name of the University of Freiburg nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef OCTOMAP_TIMING_H_ 35 | #define OCTOMAP_TIMING_H_ 36 | 37 | #ifdef _MSC_VER 38 | // MS compilers 39 | #include 40 | #include 41 | #include 42 | void gettimeofday(struct timeval* t, void* timezone) { 43 | struct _timeb timebuffer; 44 | _ftime64_s( &timebuffer ); 45 | t->tv_sec= (long) timebuffer.time; 46 | t->tv_usec=1000*timebuffer.millitm; 47 | } 48 | #else 49 | // GCC and minGW 50 | #include 51 | #endif 52 | 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /src/octomap/octomap/include/octomap/octomap_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees 3 | * http://octomap.github.com/ 4 | * 5 | * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg 6 | * All rights reserved. 7 | * License: New BSD 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in the 16 | * documentation and/or other materials provided with the distribution. 17 | * * Neither the name of the University of Freiburg nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef OCTOMAP_UTILS_H_ 35 | #define OCTOMAP_UTILS_H_ 36 | 37 | #include 38 | 39 | namespace octomap{ 40 | 41 | /// compute log-odds from probability: 42 | inline float logodds(double probability){ 43 | return (float) log(probability/(1-probability)); 44 | } 45 | 46 | /// compute probability from logodds: 47 | inline double probability(double logodds){ 48 | return 1. - ( 1. / (1. + exp(logodds))); 49 | 50 | } 51 | } 52 | 53 | 54 | 55 | #endif /* OCTOMAP_UTILS_H_ */ 56 | -------------------------------------------------------------------------------- /src/octomap/octomap/octomap-config.cmake.in: -------------------------------------------------------------------------------- 1 | # =================================================================================== 2 | # The OctoMap CMake configuration file 3 | # 4 | # ** File generated automatically, do not modify ** 5 | # 6 | # Usage from an external project: 7 | # In your CMakeLists.txt, add these lines: 8 | # 9 | # FIND_PACKAGE(OCTOMAP REQUIRED ) 10 | # INCLUDE_DIRECTORIES(${OCTOMAP_INCLUDE_DIRS}) 11 | # TARGET_LINK_LIBRARIES(MY_TARGET_NAME ${OCTOMAP_LIBRARIES}) 12 | # 13 | # 14 | # This file will define the following variables: 15 | # - OCTOMAP_LIBRARIES : The list of libraries to links against. 16 | # - OCTOMAP_LIBRARY_DIRS : The directory where lib files are. Calling 17 | # LINK_DIRECTORIES with this path is NOT needed. 18 | # - OCTOMAP_INCLUDE_DIRS : The OctoMap include directories. 19 | # - OCTOMAP_MAJOR_VERSION : Major version. 20 | # - OCTOMAP_MINOR_VERSION : Minor version. 21 | # - OCTOMAP_PATCH_VERSION : Patch version. 22 | # - OCTOMAP_VERSION : Major.Minor.Patch version. 23 | # 24 | # =================================================================================== 25 | 26 | @PACKAGE_INIT@ 27 | 28 | set(OCTOMAP_MAJOR_VERSION "@OCTOMAP_MAJOR_VERSION@") 29 | set(OCTOMAP_MINOR_VERSION "@OCTOMAP_MINOR_VERSION@") 30 | set(OCTOMAP_PATCH_VERSION "@OCTOMAP_PATCH_VERSION@") 31 | set(OCTOMAP_VERSION "@OCTOMAP_VERSION@") 32 | 33 | set_and_check(OCTOMAP_INCLUDE_DIRS "@PACKAGE_OCTOMAP_INCLUDE_DIRS@") 34 | set_and_check(OCTOMAP_LIBRARY_DIRS "@PACKAGE_OCTOMAP_LIB_DIR@") 35 | 36 | # Set library names 37 | set(OCTOMAP_LIBRARIES 38 | "@PACKAGE_OCTOMAP_LIB_DIR@/@OCTOMAP_LIBRARY@" 39 | "@PACKAGE_OCTOMAP_LIB_DIR@/@OCTOMATH_LIBRARY@" 40 | ) 41 | 42 | @OCTOMAP_INCLUDE_TARGETS@ 43 | -------------------------------------------------------------------------------- /src/octomap/octomap/package.xml: -------------------------------------------------------------------------------- 1 | 2 | octomap 3 | 1.9.0 4 | The OctoMap library implements a 3D occupancy grid mapping approach, providing data structures and mapping algorithms in C++. The map implementation is based on an octree. See 5 | http://octomap.github.io for details. 6 | 7 | Kai M. Wurm 8 | Armin Hornung 9 | Armin Hornung 10 | BSD 11 | 12 | http://octomap.github.io 13 | https://github.com/OctoMap/octomap/issues 14 | 15 | catkin 16 | cmake 17 | 18 | 19 | cmake 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /src/octomap/octomap/share/data/mapcoll.txt: -------------------------------------------------------------------------------- 1 | #a comment 2 | #a comment 3 | MAPNODEID cup0815 4 | #a further comment 5 | #a further comment 6 | MAPNODEFILENAME geb079.bt 7 | #a further comment 8 | MAPNODEPOSE 1 2 3 4.56 67.3 1.24 9 | #a further comment 10 | MAPNODEID glass 11 | MAPNODEFILENAME geb079.bt 12 | MAPNODEPOSE 1 2 3 4.56 67.3 1.24 13 | MAPNODEID cup08w5 14 | MAPNODEFILENAME geb079.bt 15 | MAPNODEPOSE 1 2 3 4.56 67.3 1.24 -------------------------------------------------------------------------------- /src/octomap/octomap/share/data/scan.dat.bz2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octomap/share/data/scan.dat.bz2 -------------------------------------------------------------------------------- /src/octomap/octomap/share/example-project.tgz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octomap/share/example-project.tgz -------------------------------------------------------------------------------- /src/octomap/octomap/share/images/uml_overview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octomap/share/images/uml_overview.png -------------------------------------------------------------------------------- /src/octomap/octomap/src/OcTree.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees 3 | * http://octomap.github.com/ 4 | * 5 | * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg 6 | * All rights reserved. 7 | * License: New BSD 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in the 16 | * documentation and/or other materials provided with the distribution. 17 | * * Neither the name of the University of Freiburg nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #include 35 | 36 | 37 | namespace octomap { 38 | 39 | OcTree::OcTree(double in_resolution) 40 | : OccupancyOcTreeBase(in_resolution) { 41 | ocTreeMemberInit.ensureLinking(); 42 | }; 43 | 44 | OcTree::OcTree(std::string _filename) 45 | : OccupancyOcTreeBase (0.1) { // resolution will be set according to tree file 46 | readBinary(_filename); 47 | } 48 | 49 | OcTree::StaticMemberInitializer OcTree::ocTreeMemberInit; 50 | 51 | 52 | 53 | 54 | } // namespace 55 | -------------------------------------------------------------------------------- /src/octomap/octomap/src/math/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET (octomath_SRCS 2 | Vector3.cpp 3 | Quaternion.cpp 4 | Pose6D.cpp 5 | ) 6 | 7 | 8 | ADD_LIBRARY( octomath SHARED ${octomath_SRCS}) 9 | 10 | SET_TARGET_PROPERTIES( octomath PROPERTIES 11 | VERSION ${OCTOMAP_VERSION} 12 | SOVERSION ${OCTOMAP_SOVERSION} 13 | INSTALL_NAME_DIR ${CMAKE_ARCHIVE_OUTPUT_DIRECTORY} # this seems to be necessary for MacOS X 14 | ) 15 | # INSTALL_NAME_DIR seems to be necessary for MacOS X 16 | 17 | ADD_LIBRARY( octomath-static STATIC ${octomath_SRCS}) 18 | SET_TARGET_PROPERTIES(octomath-static PROPERTIES OUTPUT_NAME "octomath") 19 | 20 | if(NOT EXISTS "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}/cmake/octomap") 21 | file(MAKE_DIRECTORY "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}/cmake/octomap") 22 | endif() 23 | 24 | export(TARGETS octomath octomath-static 25 | APPEND FILE "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}/cmake/octomap/octomap-targets.cmake") 26 | 27 | install(TARGETS octomath octomath-static 28 | EXPORT octomap-targets 29 | INCLUDES DESTINATION include 30 | ${INSTALL_TARGETS_DEFAULT_ARGS} 31 | ) 32 | -------------------------------------------------------------------------------- /src/octomap/octomap/src/testing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(BUILD_TESTING) 2 | ADD_EXECUTABLE(test_raycasting test_raycasting.cpp) 3 | TARGET_LINK_LIBRARIES(test_raycasting octomap) 4 | 5 | ADD_EXECUTABLE(test_iterators test_iterators.cpp) 6 | TARGET_LINK_LIBRARIES(test_iterators octomap) 7 | 8 | ADD_EXECUTABLE(test_io test_io.cpp) 9 | TARGET_LINK_LIBRARIES(test_io octomap) 10 | 11 | ADD_EXECUTABLE(test_changedkeys test_changedkeys.cpp) 12 | TARGET_LINK_LIBRARIES(test_changedkeys octomap) 13 | 14 | ADD_EXECUTABLE(test_scans test_scans.cpp) 15 | TARGET_LINK_LIBRARIES(test_scans octomap) 16 | 17 | ADD_EXECUTABLE(test_color_tree test_color_tree.cpp) 18 | TARGET_LINK_LIBRARIES(test_color_tree octomap) 19 | 20 | ADD_EXECUTABLE(color_tree_histogram color_tree_histogram.cpp) 21 | TARGET_LINK_LIBRARIES(color_tree_histogram octomap) 22 | 23 | ADD_EXECUTABLE(test_mapcollection test_mapcollection.cpp) 24 | TARGET_LINK_LIBRARIES(test_mapcollection octomap octomath) 25 | 26 | ADD_EXECUTABLE(test_pruning test_pruning.cpp) 27 | TARGET_LINK_LIBRARIES(test_pruning octomap octomath) 28 | 29 | 30 | # CTest tests below 31 | 32 | ADD_EXECUTABLE(unit_tests unit_tests.cpp) 33 | TARGET_LINK_LIBRARIES(unit_tests octomap) 34 | 35 | ADD_TEST (NAME MathVector COMMAND unit_tests MathVector ) 36 | ADD_TEST (NAME MathPose COMMAND unit_tests MathPose ) 37 | ADD_TEST (NAME InsertRay COMMAND unit_tests InsertRay ) 38 | ADD_TEST (NAME InsertScan COMMAND unit_tests InsertScan ) 39 | ADD_TEST (NAME ReadGraph COMMAND unit_tests ReadGraph ) 40 | ADD_TEST (NAME StampedTree COMMAND unit_tests StampedTree ) 41 | ADD_TEST (NAME OcTreeKey COMMAND unit_tests OcTreeKey ) 42 | ADD_TEST (NAME test_scans COMMAND test_scans ${PROJECT_SOURCE_DIR}/share/data/spherical_scan.graph) 43 | ADD_TEST (NAME test_raycasting COMMAND test_raycasting) 44 | ADD_TEST (NAME test_io COMMAND test_io ${PROJECT_SOURCE_DIR}/share/data/geb079.bt) 45 | ADD_TEST (NAME test_pruning COMMAND test_pruning ) 46 | ADD_TEST (NAME test_iterators COMMAND test_iterators ${PROJECT_SOURCE_DIR}/share/data/geb079.bt) 47 | ADD_TEST (NAME test_mapcollection COMMAND test_mapcollection ${PROJECT_SOURCE_DIR}/share/data/mapcoll.txt) 48 | ADD_TEST (NAME test_color_tree COMMAND test_color_tree) 49 | endif() 50 | -------------------------------------------------------------------------------- /src/octomap/octomap/src/testing/color_tree_histogram.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace std; 5 | using namespace octomap; 6 | 7 | 8 | int main(int argc, char** argv) { 9 | 10 | std::string filename(argv[1]); 11 | 12 | std::ifstream infile(filename.c_str(), std::ios_base::in |std::ios_base::binary); 13 | if (!infile.is_open()) { 14 | cout << "file "<< filename << " could not be opened for reading.\n"; 15 | return -1; 16 | } 17 | 18 | ColorOcTree tree (0.1); 19 | tree.readData(infile); 20 | infile.close(); 21 | cout << "color tree read from "<< filename <<"\n"; 22 | 23 | tree.writeColorHistogram("histogram.eps"); 24 | 25 | return 0; 26 | } 27 | -------------------------------------------------------------------------------- /src/octomap/octomap/src/testing/testing.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | // this is mimicing gtest expressions 5 | 6 | #define EXPECT_TRUE(args) { \ 7 | if (!(args)) { fprintf(stderr, "test failed (EXPECT_TRUE) in %s, line %d\n", __FILE__, __LINE__); \ 8 | exit(1); \ 9 | } } 10 | 11 | #define EXPECT_FALSE(args) { \ 12 | if (args) { fprintf(stderr, "test failed (EXPECT_FALSE) in %s, line %d\n", __FILE__, __LINE__); \ 13 | exit(1); \ 14 | } } 15 | 16 | #define EXPECT_EQ(a,b) { \ 17 | if (!(a == b)) { std::cerr << "test failed: " < %f in %s, line %d\n", a, b, prec, __FILE__, __LINE__); \ 29 | exit(1); \ 30 | } } 31 | 32 | -------------------------------------------------------------------------------- /src/octomap/octovis/CMakeModules/CMakeUninstall.cmake.in: -------------------------------------------------------------------------------- 1 | if (NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") 2 | message(FATAL_ERROR "Cannot find install manifest: \"@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt\"") 3 | endif(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") 4 | 5 | file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files) 6 | string(REGEX REPLACE "\n" ";" files "${files}") 7 | list(REVERSE files) 8 | foreach (file ${files}) 9 | message(STATUS "Uninstalling \"$ENV{DESTDIR}${file}\"") 10 | if (EXISTS "$ENV{DESTDIR}${file}") 11 | execute_process( 12 | COMMAND @CMAKE_COMMAND@ -E remove "$ENV{DESTDIR}${file}" 13 | OUTPUT_VARIABLE rm_out 14 | RESULT_VARIABLE rm_retval 15 | ) 16 | if(NOT ${rm_retval} EQUAL 0) 17 | message(FATAL_ERROR "Problem when removing \"$ENV{DESTDIR}${file}\"") 18 | endif (NOT ${rm_retval} EQUAL 0) 19 | else (EXISTS "$ENV{DESTDIR}${file}") 20 | message(STATUS "File \"$ENV{DESTDIR}${file}\" does not exist.") 21 | endif (EXISTS "$ENV{DESTDIR}${file}") 22 | endforeach(file) 23 | -------------------------------------------------------------------------------- /src/octomap/octovis/CMakeModules/CompilerSettings.cmake: -------------------------------------------------------------------------------- 1 | # COMPILER SETTINGS (default: Release) 2 | # use "-DCMAKE_BUILD_TYPE=Debug" in cmake for a Debug-build 3 | IF(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) 4 | SET(CMAKE_BUILD_TYPE Release) 5 | ENDIF(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) 6 | 7 | MESSAGE ("\n") 8 | MESSAGE (STATUS "${PROJECT_NAME} building as ${CMAKE_BUILD_TYPE}") 9 | 10 | # OCTOMAP_OMP = enable OpenMP 11 | # SET(OCTOMAP_OMP 1 CACHE BOOL "Enable/disable OpenMP") 12 | # IF($ENV{OCTOMAP_OMP}) 13 | # SET(OCTOMAP_OMP $ENV{OCTOMAP_OMP}) 14 | # MESSAGE(STATUS "Found OCTOMAP_OMP=${OCTOMAP_OMP}") 15 | # ENDIF($ENV{OCTOMAP_OMP}) 16 | 17 | # COMPILER FLAGS 18 | IF (CMAKE_COMPILER_IS_GNUCC) 19 | SET (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wno-error ") 20 | SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-error ") 21 | SET (CMAKE_CXX_FLAGS_RELEASE "-O3 -funroll-loops -DNDEBUG") #sse3 disabled for compatibility 22 | # SET (CMAKE_CXX_FLAGS_RELEASE "-O3 -funroll-loops -DNDEBUG -msse3 -mssse3") 23 | SET (CMAKE_CXX_FLAGS_DEBUG "-O0 -g") 24 | # Shared object compilation under 64bit (vtable) 25 | ADD_DEFINITIONS(-fPIC) 26 | # IF(OCTOMAP_OMP) 27 | # SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fopenmp") 28 | # SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -fopenmp") 29 | # ENDIF(OCTOMAP_OMP) 30 | ENDIF() 31 | 32 | # Set full rpath http://www.paraview.org/Wiki/CMake_RPATH_handling 33 | # (good to have and required with ROS) 34 | set(CMAKE_SKIP_BUILD_RPATH FALSE) 35 | set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) 36 | set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") 37 | set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) 38 | 39 | # no prefix needed for python modules 40 | set(CMAKE_SHARED_MODULE_PREFIX "") 41 | -------------------------------------------------------------------------------- /src/octomap/octovis/include/octovis/ColorOcTreeDrawer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of OctoMap - An Efficient Probabilistic 3D Mapping 3 | * Framework Based on Octrees 4 | * http://octomap.github.io 5 | * 6 | * Copyright (c) 2009-2014, K.M. Wurm and A. Hornung, University of Freiburg 7 | * All rights reserved. License for the viewer octovis: GNU GPL v2 8 | * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt 9 | * 10 | * 11 | * This program is free software; you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation; either version 2 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, but 17 | * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 18 | * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 19 | * for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see http://www.gnu.org/licenses/. 23 | */ 24 | 25 | #ifndef OCTOVIS_COLOR_OCTREEDRAWER_H_ 26 | #define OCTOVIS_COLOR_OCTREEDRAWER_H_ 27 | 28 | #include 29 | #include 30 | 31 | namespace octomap { 32 | 33 | class ColorOcTreeDrawer : public OcTreeDrawer { 34 | public: 35 | ColorOcTreeDrawer(); 36 | virtual ~ColorOcTreeDrawer(); 37 | 38 | virtual void setOcTree(const AbstractOcTree& tree_pnt, const pose6d& origin, int map_id_); 39 | 40 | protected: 41 | 42 | 43 | }; 44 | 45 | 46 | } // end namespace 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /src/octomap/octovis/include/octovis/OcTreeRecord.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of OctoMap - An Efficient Probabilistic 3D Mapping 3 | * Framework Based on Octrees 4 | * http://octomap.github.io 5 | * 6 | * Copyright (c) 2009-2014, K.M. Wurm and A. Hornung, University of Freiburg 7 | * All rights reserved. License for the viewer octovis: GNU GPL v2 8 | * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt 9 | * 10 | * 11 | * This program is free software; you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation; either version 2 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, but 17 | * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 18 | * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 19 | * for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see http://www.gnu.org/licenses/. 23 | */ 24 | 25 | #ifndef OCTOVIS_OC_TREE_RECORD 26 | #define OCTOVIS_OC_TREE_RECORD 27 | 28 | #include 29 | 30 | namespace octomap { 31 | 32 | class OcTreeRecord { 33 | public: 34 | AbstractOcTree* octree; 35 | OcTreeDrawer* octree_drawer; 36 | unsigned int id; 37 | pose6d origin; 38 | }; 39 | 40 | } // namespace 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /src/octomap/octovis/include/octovis/PointcloudDrawer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of OctoMap - An Efficient Probabilistic 3D Mapping 3 | * Framework Based on Octrees 4 | * http://octomap.github.io 5 | * 6 | * Copyright (c) 2009-2014, K.M. Wurm and A. Hornung, University of Freiburg 7 | * All rights reserved. License for the viewer octovis: GNU GPL v2 8 | * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt 9 | * 10 | * 11 | * This program is free software; you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation; either version 2 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, but 17 | * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 18 | * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 19 | * for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see http://www.gnu.org/licenses/. 23 | */ 24 | 25 | #ifndef POINTCLOUDDRAWER_H_ 26 | #define POINTCLOUDDRAWER_H_ 27 | 28 | #include "SceneObject.h" 29 | 30 | namespace octomap { 31 | 32 | /** 33 | * Drawer which visualizes Pointclouds 34 | */ 35 | class PointcloudDrawer: public ScanGraphDrawer { 36 | public: 37 | PointcloudDrawer(); 38 | PointcloudDrawer(const ScanGraph& graph); 39 | virtual ~PointcloudDrawer(); 40 | 41 | virtual void draw() const; 42 | virtual void clear(); 43 | virtual void setScanGraph(const ScanGraph& graph); 44 | 45 | protected: 46 | GLfloat* m_pointsArray; 47 | unsigned m_numberPoints; 48 | 49 | }; 50 | 51 | } 52 | 53 | #endif /* POINTCLOUDDRAWER_H_ */ 54 | -------------------------------------------------------------------------------- /src/octomap/octovis/include/octovis/SelectionBox.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of OctoMap - An Efficient Probabilistic 3D Mapping 3 | * Framework Based on Octrees 4 | * http://octomap.github.io 5 | * 6 | * Copyright (c) 2009-2014, K.M. Wurm and A. Hornung, University of Freiburg 7 | * All rights reserved. License for the viewer octovis: GNU GPL v2 8 | * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt 9 | * 10 | * 11 | * This program is free software; you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation; either version 2 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, but 17 | * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 18 | * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 19 | * for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see http://www.gnu.org/licenses/. 23 | */ 24 | 25 | #ifndef SELECTIONBOX_H_ 26 | #define SELECTIONBOX_H_ 27 | 28 | #include 29 | 30 | namespace octomap { 31 | class SelectionBox{ 32 | 33 | public: 34 | SelectionBox(); 35 | virtual ~SelectionBox(); 36 | void draw(bool withNames = false); 37 | const qglviewer::ManipulatedFrame* frame (unsigned short i) const { return m_frames.at(i); } 38 | qglviewer::ManipulatedFrame* frame (unsigned short i) { return m_frames.at(i); } 39 | void getBBXMin(float& x, float& y, float& z) const; 40 | void getBBXMax(float& x, float& y, float& z) const; 41 | int getGrabbedFrame() const; 42 | 43 | protected: 44 | void drawAxis(float length = 0.2f) const; 45 | 46 | bool m_visible; 47 | std::vector m_frames; 48 | unsigned short m_selectedFrame; 49 | qglviewer::Vec m_minPt; 50 | qglviewer::Vec m_maxPt; 51 | float m_arrowLength; 52 | 53 | }; 54 | 55 | 56 | 57 | } 58 | 59 | 60 | 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /src/octomap/octovis/include/octovis/TrajectoryDrawer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of OctoMap - An Efficient Probabilistic 3D Mapping 3 | * Framework Based on Octrees 4 | * http://octomap.github.io 5 | * 6 | * Copyright (c) 2009-2014, K.M. Wurm and A. Hornung, University of Freiburg 7 | * All rights reserved. License for the viewer octovis: GNU GPL v2 8 | * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt 9 | * 10 | * 11 | * This program is free software; you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation; either version 2 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, but 17 | * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 18 | * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 19 | * for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see http://www.gnu.org/licenses/. 23 | */ 24 | 25 | #ifndef TRAJECTORYDRAWER_H_ 26 | #define TRAJECTORYDRAWER_H_ 27 | 28 | #include "SceneObject.h" 29 | #include 30 | 31 | namespace octomap { 32 | 33 | class TrajectoryDrawer : public ScanGraphDrawer{ 34 | public: 35 | TrajectoryDrawer(); 36 | TrajectoryDrawer(const ScanGraph& graph); 37 | virtual ~TrajectoryDrawer(); 38 | virtual void draw() const; 39 | virtual void clear(); 40 | virtual void setScanGraph(const octomap::ScanGraph& graph); 41 | 42 | protected: 43 | GLfloat* m_trajectoryVertexArray; 44 | GLfloat* m_trajectoryColorArray; 45 | unsigned int m_trajectorySize; //!< number of nodes in the ScanGraph 46 | }; 47 | 48 | } 49 | 50 | #endif /* TRAJECTORYDRAWER_H_ */ 51 | -------------------------------------------------------------------------------- /src/octomap/octovis/include/octovis/ViewerSettings.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of OctoMap - An Efficient Probabilistic 3D Mapping 3 | * Framework Based on Octrees 4 | * http://octomap.github.io 5 | * 6 | * Copyright (c) 2009-2014, K.M. Wurm and A. Hornung, University of Freiburg 7 | * All rights reserved. License for the viewer octovis: GNU GPL v2 8 | * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt 9 | * 10 | * 11 | * This program is free software; you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation; either version 2 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, but 17 | * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 18 | * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 19 | * for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see http://www.gnu.org/licenses/. 23 | */ 24 | 25 | #ifndef VIEWERSETTINGS_H 26 | #define VIEWERSETTINGS_H 27 | 28 | #include 29 | #if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) 30 | #include 31 | #else // QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) 32 | #include 33 | #endif // QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) 34 | #include "ui_ViewerSettings.h" 35 | 36 | class ViewerSettings : public QDialog 37 | { 38 | Q_OBJECT 39 | 40 | public: 41 | ViewerSettings(QWidget *parent = 0); 42 | ~ViewerSettings(); 43 | double getResolution(){return ui.resolution->value(); }; 44 | void setResolution(double resolution){ui.resolution->setValue(resolution);}; 45 | unsigned int getLaserType(){return ui.laserType->currentIndex(); }; 46 | void setLaserType(int type){ui.laserType->setCurrentIndex(type); }; 47 | double getMaxRange(){return ui.maxRange->value(); }; 48 | void setMaxRange(double range){ui.maxRange->setValue(range); }; 49 | 50 | private: 51 | Ui::ViewerSettingsClass ui; 52 | }; 53 | 54 | #endif // VIEWERSETTINGS_H 55 | -------------------------------------------------------------------------------- /src/octomap/octovis/include/octovis/ViewerSettingsPanel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of OctoMap - An Efficient Probabilistic 3D Mapping 3 | * Framework Based on Octrees 4 | * http://octomap.github.io 5 | * 6 | * Copyright (c) 2009-2014, K.M. Wurm and A. Hornung, University of Freiburg 7 | * All rights reserved. License for the viewer octovis: GNU GPL v2 8 | * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt 9 | * 10 | * 11 | * This program is free software; you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation; either version 2 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, but 17 | * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 18 | * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 19 | * for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see http://www.gnu.org/licenses/. 23 | */ 24 | 25 | #ifndef VIEWERSETTINGSPANEL_H 26 | #define VIEWERSETTINGSPANEL_H 27 | 28 | #include 29 | #include 30 | #include "ui_ViewerSettingsPanel.h" 31 | 32 | #define _TREE_MAX_DEPTH 16 33 | 34 | class ViewerSettingsPanel : public QWidget 35 | { 36 | Q_OBJECT 37 | 38 | public: 39 | ViewerSettingsPanel(QWidget *parent = 0); 40 | ~ViewerSettingsPanel(); 41 | 42 | public slots: 43 | void setNumberOfScans(unsigned scans); 44 | void setCurrentScan(unsigned scan); 45 | void setResolution(double resolution); 46 | void setTreeDepth(int depth); 47 | 48 | private slots: 49 | void on_firstScanButton_clicked(); 50 | void on_lastScanButton_clicked(); 51 | void on_nextScanButton_clicked(); 52 | void on_fastFwdScanButton_clicked(); 53 | 54 | signals: 55 | void treeDepthChanged(int depth); 56 | void addNextScans(unsigned scans); 57 | void gotoFirstScan(); 58 | 59 | 60 | private: 61 | void scanProgressChanged(); 62 | void leafSizeChanged(); 63 | Ui::ViewerSettingsPanelClass ui; 64 | unsigned m_currentScan; 65 | unsigned m_numberScans; 66 | unsigned m_treeDepth; 67 | double m_resolution; 68 | }; 69 | 70 | #endif // VIEWERSETTINGSPANEL_H 71 | -------------------------------------------------------------------------------- /src/octomap/octovis/octovis-config.cmake.in: -------------------------------------------------------------------------------- 1 | # It defines the following variables 2 | # OCTOVIS_INCLUDE_DIRS - include directories for OctoMap viewer 3 | # OCTOVIS_LIBRARY_DIRS - library directories for OctoMap viewer 4 | # OCTOVIS_LIBRARIES - libraries to link against 5 | # OCTOVIS_MAJOR_VERSION - major version 6 | # OCTOVIS_MINOR_VERSION - minor version 7 | # OCTOVIS_PATCH_VERSION - patch version 8 | # OCTOVIS_VERSION - major.minor.patch version 9 | 10 | @PACKAGE_INIT@ 11 | 12 | set(OCTOVIS_MAJOR_VERSION "@OCTOVIS_MAJOR_VERSION@") 13 | set(OCTOVIS_MINOR_VERSION "@OCTOVIS_MINOR_VERSION@") 14 | set(OCTOVIS_PATCH_VERSION "@OCTOVIS_PATCH_VERSION@") 15 | set(OCTOVIS_VERSION "@OCTOVIS_VERSION@") 16 | 17 | set_and_check(OCTOVIS_INCLUDE_DIRS "@PACKAGE_OCTOVIS_INCLUDE_DIRS@" "@QGLViewer_INCLUDE_DIR@") 18 | set_and_check(OCTOVIS_LIBRARY_DIRS "@PACKAGE_OCTOVIS_LIB_DIR@" "@QGLViewer_LIBRARY_DIR@") 19 | 20 | # Set library names as absolute paths: 21 | set(OCTOVIS_LIBRARIES 22 | "@QGLViewer_LIBRARIES@" 23 | "@QT_LIBRARIES@" 24 | "@PACKAGE_OCTOVIS_LIB_DIR@/@OCTOVIS_LIBRARY@" 25 | ) 26 | 27 | @OCTOVIS_INCLUDE_TARGETS@ 28 | -------------------------------------------------------------------------------- /src/octomap/octovis/package.xml: -------------------------------------------------------------------------------- 1 | 2 | octovis 3 | 1.9.0 4 | octovis is visualization tool for the OctoMap library based on Qt and libQGLViewer. See 5 | http://octomap.github.io for details. 6 | 7 | Kai M. Wurm 8 | Armin Hornung 9 | Armin Hornung 10 | GPLv2 11 | 12 | http://octomap.github.io 13 | https://github.com/OctoMap/octomap/issues 14 | 15 | 16 | catkin 17 | cmake 18 | 19 | 20 | cmake 21 | 22 | 23 | octomap 24 | libqglviewer-qt4-dev 25 | libqt4-dev 26 | libqt4-opengl-dev 27 | 28 | octomap 29 | libqglviewer-qt4 30 | libqtgui4 31 | libqt4-opengl 32 | 33 | 34 | -------------------------------------------------------------------------------- /src/octomap/octovis/src/ViewerSettings.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of OctoMap - An Efficient Probabilistic 3D Mapping 3 | * Framework Based on Octrees 4 | * http://octomap.github.io 5 | * 6 | * Copyright (c) 2009-2014, K.M. Wurm and A. Hornung, University of Freiburg 7 | * All rights reserved. License for the viewer octovis: GNU GPL v2 8 | * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt 9 | * 10 | * 11 | * This program is free software; you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation; either version 2 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, but 17 | * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 18 | * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 19 | * for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see http://www.gnu.org/licenses/. 23 | */ 24 | 25 | #include 26 | 27 | ViewerSettings::ViewerSettings(QWidget *parent) 28 | : QDialog(parent) 29 | { 30 | ui.setupUi(this); 31 | } 32 | 33 | ViewerSettings::~ViewerSettings() 34 | { 35 | 36 | } 37 | -------------------------------------------------------------------------------- /src/octomap/octovis/src/extern/QGLViewer/.gitignore: -------------------------------------------------------------------------------- 1 | .moc 2 | .obj 3 | *.so 4 | *.so.* 5 | ui_*.h 6 | libQGLViewer.prl 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/octomap/octovis/src/extern/QGLViewer/CHANGELOG: -------------------------------------------------------------------------------- 1 | This is libQGLViewer version 2.6.3. Packaged on July 10, 2015. 2 | 3 | The complete change log is available in doc/changeLog.html 4 | 5 | The latest releases and changeLog are available at: 6 | http://www.libqglviewer.com/changeLog.html 7 | -------------------------------------------------------------------------------- /src/octomap/octovis/src/extern/QGLViewer/INSTALL: -------------------------------------------------------------------------------- 1 | 2 | l i b Q G L V i e w e r I n s t a l l a t i o n 3 | 4 | 5 | 6 | libQGLViewer requires the Qt library, available from Digia. 7 | 8 | In order to compile the library from its sources: 9 | 10 | - On UNIX platforms, simply type (see doc/installUnix.html for details): 11 | 12 | > qmake 13 | > make 14 | > make install [optional] 15 | 16 | - For Windows installation, see doc/installWindows.html. 17 | 18 | 19 | 20 | See doc/compilation.html for details on compiling programs that use libQGLViewer. 21 | -------------------------------------------------------------------------------- /src/octomap/octovis/src/extern/QGLViewer/README: -------------------------------------------------------------------------------- 1 | 2 | l i b Q G L V i e w e r 3 | 4 | Version 2.6.3. Packaged on July 10, 2015 5 | 6 | 7 | Copyright (C) 2002-2014 Gilles Debunne. All rights reserved. 8 | http://www.libqglviewer.com 9 | Send e-mail to contact@libqglviewer.com 10 | 11 | 12 | libQGLViewer is a C++ library based on Qt that eases the creation of OpenGL 3D viewers. 13 | 14 | It provides some of the typical 3D viewer functionalities, such as the possibility to 15 | move the camera using the mouse, which lacks in most of the other APIs. Other features 16 | include mouse manipulated frames, interpolated keyFrames, object selection, stereo 17 | display, screenshot saving and much more. It can be used by OpenGL beginners as well as 18 | to create complex applications, being fully customizable and easy to extend. 19 | 20 | Based on the Qt toolkit, it compiles on any architecture (Unix-Linux, Mac, Windows). 21 | Full reference documentation and many examples are provided. libQGLViewer does not 22 | display 3D scenes in various formats, but it can be the base for the coding of such a 23 | viewer. 24 | 25 | libQGLViewer uses dual licensing: it is freely available under the terms of the GNU-GPL 26 | license for open source software development, while commercial applications can apply 27 | for a commercial license. 28 | 29 | 30 | The library's main functionalities are: 31 | 32 | - A camera trackball to intuitively move the camera in the scene. 33 | - Screenshot saving in different file formats (JPG, PNG, EPS...). 34 | - Easy scene object selection and manipulation using the mouse. 35 | - Definition and replay of keyFrame paths. 36 | - Stereo display (provided that your hardware supports it). 37 | - Clean, well-designed and easily extendable API. 38 | - Many examples and a complete reference documentation. 39 | 40 | See the doc/index.html page for details. 41 | 42 | -------------------------------------------------------------------------------- /src/octomap/octovis/src/extern/QGLViewer/qglviewer.icns: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/extern/QGLViewer/qglviewer.icns -------------------------------------------------------------------------------- /src/octomap/octovis/src/extern/QGLViewer/qglviewer_fr.qm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/extern/QGLViewer/qglviewer_fr.qm -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons.qrc: -------------------------------------------------------------------------------- 1 | 2 | 3 | icons/media-playback-start.png 4 | icons/media-seek-backward.png 5 | icons/media-seek-forward.png 6 | icons/media-skip-backward.png 7 | icons/media-skip-forward.png 8 | icons/application-exit.png 9 | icons/configure.png 10 | icons/document-open-folder.png 11 | icons/document-open.png 12 | icons/document-save.png 13 | icons/edit-undo.png 14 | icons/help-contents.png 15 | icons/system-restart.png 16 | icons/view-preview.png 17 | icons/document-save-as.png 18 | icons/go-bottom.png 19 | icons/go-top.png 20 | icons/view-refresh.png 21 | icons/edit-clear-list.png 22 | icons/list-add.png 23 | icons/list-remove.png 24 | 25 | 26 | -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/LICENSE.txt: -------------------------------------------------------------------------------- 1 | These are the KDE4 icons from http://www.oxygen-icons.org/. 2 | All icons are released under GNU Library General Public License, 3 | for details please see http://www.gnu.org/licenses/lgpl-3.0.txt 4 | or "lgpl-3.0.txt" in this directory. -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/application-exit.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/application-exit.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/configure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/configure.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/document-open-folder.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/document-open-folder.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/document-open.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/document-open.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/document-save-as.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/document-save-as.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/document-save.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/document-save.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/edit-clear-list.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/edit-clear-list.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/edit-undo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/edit-undo.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/go-bottom.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/go-bottom.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/go-top.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/go-top.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/help-contents.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/help-contents.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/list-add.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/list-add.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/list-remove.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/list-remove.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/media-playback-start.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/media-playback-start.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/media-seek-backward.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/media-seek-backward.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/media-seek-forward.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/media-seek-forward.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/media-skip-backward.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/media-skip-backward.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/media-skip-forward.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/media-skip-forward.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/system-restart.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/system-restart.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/view-preview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/view-preview.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/icons/view-refresh.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap/octovis/src/icons/view-refresh.png -------------------------------------------------------------------------------- /src/octomap/octovis/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of OctoMap - An Efficient Probabilistic 3D Mapping 3 | * Framework Based on Octrees 4 | * http://octomap.github.io 5 | * 6 | * Copyright (c) 2009-2014, K.M. Wurm and A. Hornung, University of Freiburg 7 | * All rights reserved. License for the viewer octovis: GNU GPL v2 8 | * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt 9 | * 10 | * 11 | * This program is free software; you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation; either version 2 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * This program is distributed in the hope that it will be useful, but 17 | * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 18 | * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 19 | * for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see http://www.gnu.org/licenses/. 23 | */ 24 | 25 | #include 26 | #include 27 | #include 28 | #include //strtol 29 | 30 | int main(int argc, char *argv[]) { 31 | 32 | std::string filename = ""; 33 | int depth = 0; 34 | if (argc == 1) { 35 | std::cout << "Usage: " << argv[0] << " [mapfile] [tree depth cutoff]\n"; 36 | std::cout << "Where the optional [tree depth cutoff] is an integer from 1 to 16\n"; 37 | } 38 | if (argc >= 2) { filename = std::string(argv[1]); } 39 | if (argc >= 3) { depth = std::strtol(argv[2], NULL, 10); }//zero on parse error 40 | 41 | QApplication app(argc, argv); 42 | 43 | octomap::ViewerGui gui(filename, NULL, depth); 44 | gui.show(); 45 | return app.exec(); 46 | } 47 | -------------------------------------------------------------------------------- /src/octomap/scripts/travis_build_jobs.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # travis build script for test compilations 4 | 5 | set -e 6 | 7 | function build { 8 | cd $1 9 | mkdir build 10 | cd build 11 | cmake .. -DCMAKE_INSTALL_PREFIX=/tmp/octomap/$1 12 | make -j4 13 | cd .. 14 | } 15 | 16 | case "$1" in 17 | "dist") 18 | build . 19 | cd build && make test 20 | make install 21 | ;; 22 | "components") 23 | build octomap 24 | cd build && make test 25 | make install 26 | cd ../.. 27 | build dynamicEDT3D 28 | cd .. 29 | build octovis 30 | cd .. 31 | ;; 32 | *) 33 | echo "Invalid build variant" 34 | exit 1 35 | esac 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /src/octomap_catkin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | cmake_minimum_required(VERSION 2.8.3) 3 | project(octomap_catkin) 4 | 5 | find_package(catkin REQUIRED) 6 | 7 | include(ExternalProject) 8 | file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/include) 9 | ExternalProject_Add(octomap_src 10 | URL "${CMAKE_CURRENT_LIST_DIR}/../octomap" 11 | CMAKE_ARGS 12 | -DCMAKE_INSTALL_PREFIX:PATH=${CATKIN_DEVEL_PREFIX} 13 | ) 14 | 15 | add_library(octomap_catkin lib_wrap.cc) 16 | add_dependencies(octomap_catkin octomap_src) 17 | 18 | catkin_package( 19 | LIBRARIES ${PROJECT_NAME}_catkin 20 | ) 21 | -------------------------------------------------------------------------------- /src/octomap_catkin/lib_wrap.cc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/occupancy_mapping_benchmarks/4beec0be4d9bbc0403738f021b069fceecd9ff27/src/octomap_catkin/lib_wrap.cc -------------------------------------------------------------------------------- /src/octomap_catkin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | octomap_catkin 4 | 0.0.1 5 | catkin wrapper for octomap 6 | nicolov 7 | BSD 8 | catkin 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/octomap_mapping/.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | 6 | # Compiled Dynamic libraries 7 | *.so 8 | *.dylib 9 | 10 | # Compiled Static libraries 11 | *.lai 12 | *.la 13 | *.a 14 | 15 | octomap_server/docs 16 | octomap_server/cfg 17 | octomap_server/src/octomap_server 18 | -------------------------------------------------------------------------------- /src/octomap_mapping/.travis.yml: -------------------------------------------------------------------------------- 1 | # This config file for Travis CI utilizes ros-industrial/industrial_ci package. 2 | # For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst 3 | sudo: required 4 | language: generic 5 | env: 6 | global: 7 | - ROS_PARALLEL_JOBS=-j4 # Don't exhaust virtual memory 8 | matrix: 9 | - ROS_DISTRO="kinetic" CATKIN_CONFIG='-DCMAKE_BUILD_TYPE=Debug' 10 | - ROS_DISTRO="kinetic" CATKIN_CONFIG='-DCMAKE_BUILD_TYPE=Release' 11 | - ROS_DISTRO="kinetic" PRERELEASE=true 12 | - ROS_DISTRO="melodic" CATKIN_CONFIG='-DCMAKE_BUILD_TYPE=Debug' 13 | - ROS_DISTRO="melodic" CATKIN_CONFIG='-DCMAKE_BUILD_TYPE=Release' 14 | - ROS_DISTRO="melodic" PRERELEASE=true 15 | - ROS_DISTRO="melodic" OS_NAME="debian" OS_CODE_NAME="stretch" 16 | matrix: 17 | allow_failures: 18 | - env: ROS_DISTRO="kinetic" PRERELEASE=true 19 | - env: ROS_DISTRO="melodic" PRERELEASE=true 20 | install: 21 | - git clone --quiet --depth 1 https://github.com/ros-industrial/industrial_ci.git .ci_config 22 | script: 23 | - source .ci_config/travis.sh 24 | -------------------------------------------------------------------------------- /src/octomap_mapping/README.md: -------------------------------------------------------------------------------- 1 | octomap_mapping 2 | =============== 3 | 4 | ROS stack for mapping with OctoMap, contains the octomap_server package. 5 | 6 | The `master` branch tracks the latest (stable) releases. Please send pull requests against the latest development branch, e.g. `indigo-devel`. 7 | 8 | Indigo: [![Build Status](https://travis-ci.org/OctoMap/octomap_mapping.svg?branch=indigo-devel)](https://travis-ci.org/OctoMap/octomap_mapping) 9 | 10 | Kinetic/Melodic: [![Build Status](https://travis-ci.org/OctoMap/octomap_mapping.svg?branch=kinetic-devel)](https://travis-ci.org/OctoMap/octomap_mapping) 11 | 12 | 13 | 14 | Imported from SVN, see https://code.google.com/p/alufr-ros-pkg/ for the previous versions. 15 | -------------------------------------------------------------------------------- /src/octomap_mapping/octomap_mapping/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package octomap_mapping 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.6.3 (2019-01-28) 6 | ------------------ 7 | 8 | 0.6.2 (2019-01-27) 9 | ------------------ 10 | * Update maintainer email (Wolfgang Merkt) 11 | * Contributors: Wolfgang Merkt 12 | 13 | 0.6.1 (2016-10-19) 14 | ------------------ 15 | * Adjust maintainer email 16 | * Contributors: Armin Hornung 17 | 18 | 0.6.0 (2016-03-25) 19 | ------------------ 20 | 21 | 0.5.3 (2014-01-09) 22 | ------------------ 23 | 24 | 0.5.2 (2014-01-09) 25 | ------------------ 26 | 27 | 0.5.1 (2013-11-25) 28 | ------------------ 29 | 30 | 0.5.0 (2013-10-24) 31 | ------------------ 32 | * URLs in package.xml 33 | * Catkinization, remove support for arm_navigation 34 | 35 | 0.4.9 (2013-06-27) 36 | ------------------ 37 | 38 | 0.4.8 (2013-01-08) 39 | ------------------ 40 | 41 | 0.4.6 (2013-01-28) 42 | ------------------ 43 | 44 | 0.4.5 (2012-06-18) 45 | ------------------ 46 | 47 | 0.4.4 (2012-04-20) 48 | ------------------ 49 | 50 | 0.4.3 (2012-04-17) 51 | ------------------ 52 | 53 | 0.4.2 (2012-03-16) 54 | ------------------ 55 | 56 | 0.4.1 (2012-02-21 16:50) 57 | ------------------------ 58 | 59 | 0.4.0 (2012-02-21 15:37) 60 | ------------------------ 61 | 62 | 0.3.8 (2012-04-26) 63 | ------------------ 64 | 65 | 0.3.7 (2012-02-22) 66 | ------------------ 67 | 68 | 0.3.6 (2012-01-09) 69 | ------------------ 70 | 71 | 0.3.5 (2011-10-30) 72 | ------------------ 73 | 74 | 0.3.4 (2011-10-12) 75 | ------------------ 76 | 77 | 0.3.3 (2011-08-17 07:41) 78 | ------------------------ 79 | 80 | 0.3.2 (2011-08-09) 81 | ------------------ 82 | 83 | 0.3.1 (2011-07-15) 84 | ------------------ 85 | 86 | 0.3.0 (2011-06-28) 87 | ------------------ 88 | 89 | 0.2.0 (2011-03-16) 90 | ------------------ 91 | 92 | 0.1.2 (2010-11-23) 93 | ------------------ 94 | 95 | 0.1.1 (2010-11-17) 96 | ------------------ 97 | 98 | 0.1.0 (2010-11-16) 99 | ------------------ 100 | -------------------------------------------------------------------------------- /src/octomap_mapping/octomap_mapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(octomap_mapping) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /src/octomap_mapping/octomap_mapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | octomap_mapping 3 | 0.6.3 4 | 5 | Mapping tools to be used with the OctoMap library, implementing a 3D occupancy grid mapping. 6 | 7 | Armin Hornung 8 | Wolfgang Merkt 9 | BSD 10 | 11 | http://ros.org/wiki/octomap_mapping 12 | https://github.com/OctoMap/octomap_mapping/issues 13 | https://github.com/OctoMap/octomap_mapping 14 | 15 | catkin 16 | octomap_server 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /src/octomap_mapping/octomap_server/cfg/OctomapServer.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "octomap_server" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("compress_map", bool_t, 0, "Compresses the map losslessly", True) 9 | gen.add("incremental_2D_projection", bool_t, 0, "Incremental 2D projection", False) 10 | gen.add("filter_speckles", bool_t, 0, "Filter speckle nodes (with no neighbors)", False) 11 | gen.add("max_depth", int_t, 0, "Maximum depth when traversing the octree to send out markers. 16: full depth / max. resolution", 16, 1, 16) 12 | gen.add("pointcloud_min_z", double_t, 0, "Minimum height of points to consider for insertion", -100, -100, 100) 13 | gen.add("pointcloud_max_z", double_t, 0, "Maximum height of points to consider for insertion", 100, -100, 100) 14 | gen.add("occupancy_min_z", double_t, 0, "Minimum height of occupied cells to consider in the final map", -100, -100, 100) 15 | gen.add("occupancy_max_z", double_t, 0, "Maximum height of occupied cells to consider in the final map", 100, -100, 100) 16 | gen.add("sensor_model_max_range", double_t, 0, "Sensor maximum range", -1.0, -1.0, 100) 17 | gen.add("sensor_model_hit", double_t, 0, "Probabilities for hits in the sensor model when dynamically building a map", 0.7, 0.5, 1.0) 18 | gen.add("sensor_model_miss", double_t, 0, "Probabilities for misses in the sensor model when dynamically building a map", 0.4, 0.0, 0.5) 19 | gen.add("sensor_model_min", double_t, 0, "Minimum probability for clamping when dynamically building a map", 0.12, 0.0, 1.0) 20 | gen.add("sensor_model_max", double_t, 0, "Maximum probability for clamping when dynamically building a map", 0.97, 0.0, 1.0) 21 | gen.add("filter_ground", bool_t, 0, "Filter ground plane", False) 22 | gen.add("ground_filter_distance", double_t, 0, "Distance threshold to consider a point as ground", 0.04, 0.001, 1) 23 | gen.add("ground_filter_angle", double_t, 0, "Angular threshold of the detected plane from the horizontal plane to be detected as ground ", 0.15, 0.001, 15) 24 | gen.add("ground_filter_plane_distance", double_t, 0, "Distance threshold from z=0 for a plane to be detected as ground", 0.07, 0.001, 1) 25 | 26 | exit(gen.generate(PACKAGE, "octomap_server_node", "OctomapServer")) 27 | -------------------------------------------------------------------------------- /src/octomap_mapping/octomap_server/launch/octomap_mapping.launch: -------------------------------------------------------------------------------- 1 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /src/octomap_mapping/octomap_server/launch/octomap_mapping_nodelet.launch: -------------------------------------------------------------------------------- 1 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /src/octomap_mapping/octomap_server/launch/octomap_tracking_client.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/octomap_mapping/octomap_server/launch/octomap_tracking_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/octomap_mapping/octomap_server/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b octomap_server is ... 6 | 7 | 14 | 15 | 16 | \section codeapi Code API 17 | 18 | 28 | 29 | 30 | */ -------------------------------------------------------------------------------- /src/octomap_mapping/octomap_server/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Nodelet for running the Octomap server 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/octomap_mapping/octomap_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | octomap_server 3 | 0.6.3 4 | 5 | octomap_server loads a 3D map (as Octree-based OctoMap) and distributes it to other nodes in a compact binary format. It also allows to incrementally build 3D OctoMaps, and provides map saving in the node octomap_saver. 6 | 7 | Armin Hornung 8 | Wolfgang Merkt 9 | BSD 10 | http://www.ros.org/wiki/octomap_server 11 | https://github.com/OctoMap/octomap_mapping/issues 12 | https://github.com/OctoMap/octomap_mapping 13 | 14 | 15 | 16 | 17 | 18 | catkin 19 | 20 | roscpp 21 | visualization_msgs 22 | sensor_msgs 23 | pcl_ros 24 | pcl_conversions 25 | nav_msgs 26 | std_msgs 27 | std_srvs 28 | octomap 29 | octomap_msgs 30 | octomap_ros 31 | dynamic_reconfigure 32 | nodelet 33 | libpcl-all-dev 34 | 35 | roscpp 36 | visualization_msgs 37 | sensor_msgs 38 | pcl_ros 39 | pcl_conversions 40 | nav_msgs 41 | std_msgs 42 | std_srvs 43 | octomap 44 | octomap_msgs 45 | octomap_ros 46 | dynamic_reconfigure 47 | nodelet 48 | libpcl-all 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /src/octomap_mapping/octomap_server/params/default.yaml: -------------------------------------------------------------------------------- 1 | # Empty file to load default parameters. 2 | -------------------------------------------------------------------------------- /src/octomap_mapping/octomap_server/scripts/octomap_eraser_cli.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """Clear a region specified by a global axis-aligned bounding box in stored 4 | OctoMap. 5 | 6 | """ 7 | 8 | import sys 9 | from time import sleep 10 | 11 | import roslib 12 | roslib.load_manifest('octomap_server') 13 | from geometry_msgs.msg import Point 14 | import octomap_msgs.srv 15 | import rospy 16 | 17 | 18 | SRV_NAME = '/octomap_server/clear_bbx' 19 | SRV_INTERFACE = octomap_msgs.srv.BoundingBoxQuery 20 | 21 | 22 | if __name__ == '__main__': 23 | min = Point(*[float(x) for x in sys.argv[1:4]]) 24 | max = Point(*[float(x) for x in sys.argv[4:7]]) 25 | 26 | rospy.init_node('octomap_eraser_cli', anonymous=True) 27 | sleep(1) 28 | service = rospy.ServiceProxy(SRV_NAME, SRV_INTERFACE) 29 | rospy.loginfo("Connected to %s service." % SRV_NAME) 30 | 31 | service(min, max) 32 | -------------------------------------------------------------------------------- /src/rosfmt/.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | .* 3 | !.gitignore 4 | *.kdev4 5 | -------------------------------------------------------------------------------- /src/rosfmt/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rosfmt 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 6.0.0 (2018-11-14) 6 | ------------------ 7 | * Major release bump to decouple rosfmt and fmt versions 8 | * rosfmt.h: produce nicer error message if C++11 is not available 9 | * cmake: add transitive rosconsole / roscpp deps 10 | * README: add CMake example 11 | * Contributors: Max Schwarz 12 | 13 | 5.2.2 (2018-10-29) 14 | ------------------ 15 | * README: syntax highlighting 16 | * add README.md, LICENSE.txt 17 | * initial commit - basics are working 18 | * Contributors: Max Schwarz 19 | -------------------------------------------------------------------------------- /src/rosfmt/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | cmake_minimum_required(VERSION 3.0) 3 | project(rosfmt) 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | roscpp 7 | rosconsole 8 | ) 9 | 10 | # Get fmt from an official release 11 | include(ExternalProject) 12 | ExternalProject_Add( 13 | fmt 14 | URL https://github.com/fmtlib/fmt/releases/download/5.2.1/fmt-5.2.1.zip 15 | URL_HASH SHA256=43894ab8fe561fc9e523a8024efc23018431fa86b95d45b06dbe6ddb29ffb6cd 16 | CONFIGURE_COMMAND "" 17 | BUILD_COMMAND "" 18 | INSTALL_COMMAND "" 19 | ) 20 | 21 | # Now extract the needed files. We need to copy headers to the devel space 22 | # b/c of catkin policies (they count as "generated headers"). 23 | ExternalProject_Get_property(fmt SOURCE_DIR) 24 | set(fmt_SOURCE_DIR "${SOURCE_DIR}") 25 | set(fmt_HEADER_DEST "${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}") 26 | 27 | file(MAKE_DIRECTORY ${fmt_HEADER_DEST}) 28 | foreach(FILE color.h core.h format-inl.h format.h ostream.h posix.h printf.h ranges.h time.h) 29 | set(in "${fmt_SOURCE_DIR}/include/fmt/${FILE}") 30 | set(out "${fmt_HEADER_DEST}/fmt/${FILE}") 31 | set(fmt_HEADERS_OUT ${fmt_HEADERS_OUT} ${out}) 32 | 33 | set_source_files_properties(${in} PROPERTIES GENERATED TRUE) 34 | 35 | add_custom_command( 36 | OUTPUT ${out} 37 | DEPENDS ${in} 38 | COMMAND cmake -E copy_if_different "${in}" "${out}" 39 | ) 40 | endforeach() 41 | 42 | catkin_package( 43 | INCLUDE_DIRS include ${fmt_HEADER_DEST} 44 | CATKIN_DEPENDS roscpp rosconsole 45 | LIBRARIES rosfmt 46 | ) 47 | 48 | include_directories( 49 | ${catkin_INCLUDE_DIRS} 50 | include ${fmt_HEADER_DEST} 51 | ) 52 | 53 | set(CMAKE_CXX_STANDARD 11) 54 | 55 | set_source_files_properties( 56 | ${fmt_SOURCE_DIR}/src/format.cc 57 | ${fmt_SOURCE_DIR}/src/posix.cc 58 | PROPERTIES GENERATED TRUE 59 | ) 60 | 61 | add_library(rosfmt 62 | ${fmt_HEADERS_OUT} 63 | ${fmt_SOURCE_DIR}/src/format.cc 64 | ${fmt_SOURCE_DIR}/src/posix.cc 65 | ) 66 | target_link_libraries(rosfmt 67 | ${catkin_LIBRARIES} 68 | ) 69 | add_dependencies(rosfmt fmt) 70 | 71 | add_executable(simple_test 72 | test/test.cpp 73 | ) 74 | target_link_libraries(simple_test rosfmt) 75 | 76 | install( 77 | DIRECTORY ${fmt_HEADER_DEST}/fmt/ 78 | DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}/fmt 79 | ) 80 | install( 81 | DIRECTORY include/rosfmt/ 82 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 83 | ) 84 | install( 85 | TARGETS rosfmt 86 | DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 87 | ) 88 | -------------------------------------------------------------------------------- /src/rosfmt/LICENSE.txt: -------------------------------------------------------------------------------- 1 | 2 | BSD 2-Clause License 3 | 4 | Copyright (c) 2018, Max Schwarz 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | 10 | 1. Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | 2. Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 21 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 22 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 23 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 24 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 25 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | -------------------------------------------------------------------------------- /src/rosfmt/README.md: -------------------------------------------------------------------------------- 1 | 2 | rosfmt 3 | ====== 4 | 5 | `rosfmt` is a ROS wrapper around the awesome [fmt] library, which offers 6 | modern C++11 type-safe formatting strings. 7 | 8 | TLDR: Instead of 9 | 10 | ```C++ 11 | #include 12 | 13 | ROS_INFO("Here is my string: %s. And a number: %llu", 14 | my_string.c_str(), my_number 15 | ); 16 | ``` 17 | 18 | you can now write: 19 | 20 | ```C++ 21 | #include 22 | 23 | ROSFMT_INFO("Here is my string: {}. And a number: {}", 24 | my_string, my_number 25 | ); 26 | ``` 27 | 28 | For more complicated messages, you can use named arguments: 29 | 30 | ```C++ 31 | ROSFMT_INFO("Here is my string: {str}. And a number: {num}", 32 | fmt::arg("str", my_string), 33 | fmt::arg("num", my_number) 34 | ); 35 | ``` 36 | 37 | Of course, you can also use fmt's API directly: 38 | 39 | ```C++ 40 | std::string str = fmt::format("my string: {}", my_string); 41 | ``` 42 | 43 | See the [fmt documentation] for more details about fmt's features. For example, 44 | you can easily define printing routines for your own data structures. 45 | 46 | [fmt]: https://github.com/fmtlib/fmt 47 | [fmt documentation]: http://fmtlib.net/ 48 | 49 | Usage 50 | ----- 51 | 52 | Just depend on the `rosfmt` catkin package as usual. One catch is that `fmt` 53 | requires C++11, so you need to enable that: 54 | 55 | ```CMake 56 | cmake_minimum_required(3.0) 57 | project(my_package) 58 | 59 | find_package(catkin REQUIRED COMPONENTS 60 | rosfmt 61 | roscpp 62 | rosconsole # might be required in older versions of rosfmt 63 | ) 64 | 65 | catkin_package() 66 | include_directories(${catkin_INCLUDE_DIRS}) 67 | 68 | # Important: enable C++11 69 | set(CMAKE_CXX_STANDARD 11) 70 | 71 | add_executable(my_node 72 | src/my_node.cpp 73 | ) 74 | target_link_libraries(my_node 75 | ${catkin_LIBRARIES} 76 | ) 77 | ``` 78 | 79 | License 80 | ------- 81 | 82 | `rosfmt` and the underlying `fmt` library are licensed under the BSD-2 license. 83 | -------------------------------------------------------------------------------- /src/rosfmt/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rosfmt 3 | 4 | fmt is an open-source formatting library for C++. 5 | It can be used as a safe and fast alternative to (s)printf and IOStreams. 6 | 7 | BSD 8 | 6.0.0 9 | 10 | Max Schwarz 11 | 12 | catkin 13 | roscpp 14 | rosconsole 15 | 16 | -------------------------------------------------------------------------------- /src/rosfmt/test/test.cpp: -------------------------------------------------------------------------------- 1 | // Simple compilation test 2 | // Author: Max Schwarz 3 | 4 | #include 5 | 6 | int main(int argc, char** argv) 7 | { 8 | ROSFMT_INFO("Hello world"); 9 | ROSFMT_INFO("This is five: {}", 5); 10 | 11 | return 0; 12 | } 13 | -------------------------------------------------------------------------------- /src/skimap_ros/.gitignore: -------------------------------------------------------------------------------- 1 | nbproject 2 | -------------------------------------------------------------------------------- /src/skimap_ros/.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Mac", 5 | "includePath": [ 6 | "/usr/include", 7 | "/usr/local/include", 8 | "/opt/ros/indigo/include", 9 | "../devel/include", 10 | "${workspaceRoot}" 11 | ], 12 | "browse": { 13 | "limitSymbolsToIncludedHeaders": true, 14 | "databaseFilename": "", 15 | "path": [ 16 | "/usr/include", 17 | "/usr/local/include", 18 | "/opt/ros/indigo/include", 19 | "../devel/include", 20 | "${workspaceRoot}" 21 | ] 22 | }, 23 | "intelliSenseMode": "clang-x64", 24 | "macFrameworkPath": [ 25 | "/System/Library/Frameworks", 26 | "/Library/Frameworks" 27 | ] 28 | }, 29 | { 30 | "name": "Linux", 31 | "includePath": [ 32 | "/usr/include", 33 | "/usr/local/include", 34 | "/opt/ros/indigo/include", 35 | "../devel/include", 36 | "${workspaceRoot}" 37 | ], 38 | "browse": { 39 | "limitSymbolsToIncludedHeaders": true, 40 | "databaseFilename": "", 41 | "path": [ 42 | "/usr/include", 43 | "/usr/local/include", 44 | "/opt/ros/indigo/include", 45 | "../devel/include", 46 | "${workspaceRoot}" 47 | ] 48 | }, 49 | "intelliSenseMode": "clang-x64", 50 | "compilerPath": "/usr/bin/gcc", 51 | "cStandard": "c11", 52 | "cppStandard": "c++17" 53 | }, 54 | { 55 | "name": "Win32", 56 | "includePath": [ 57 | "C:/Program Files (x86)/Microsoft Visual Studio 14.0/VC/include/*", 58 | "${workspaceRoot}" 59 | ], 60 | "browse": { 61 | "limitSymbolsToIncludedHeaders": true, 62 | "databaseFilename": "", 63 | "path": [ 64 | "C:/Program Files (x86)/Microsoft Visual Studio 14.0/VC/include/*", 65 | "${workspaceRoot}" 66 | ] 67 | }, 68 | "intelliSenseMode": "msvc-x64" 69 | } 70 | ], 71 | "version": 3 72 | } -------------------------------------------------------------------------------- /src/skimap_ros/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "*.urdf": "html", 4 | "*.xacro": "html", 5 | "*.launch": "html", 6 | "cctype": "cpp", 7 | "cmath": "cpp", 8 | "cstdarg": "cpp", 9 | "cstddef": "cpp", 10 | "cstdio": "cpp", 11 | "cstdlib": "cpp", 12 | "cstring": "cpp", 13 | "ctime": "cpp", 14 | "cwchar": "cpp", 15 | "cwctype": "cpp", 16 | "cstdint": "cpp", 17 | "initializer_list": "cpp", 18 | "slist": "cpp", 19 | "vector": "cpp", 20 | "clocale": "cpp", 21 | "csignal": "cpp", 22 | "array": "cpp", 23 | "atomic": "cpp", 24 | "*.tcc": "cpp", 25 | "chrono": "cpp", 26 | "exception": "cpp", 27 | "functional": "cpp", 28 | "iosfwd": "cpp", 29 | "limits": "cpp", 30 | "new": "cpp", 31 | "ratio": "cpp", 32 | "stdexcept": "cpp", 33 | "thread": "cpp", 34 | "tuple": "cpp", 35 | "type_traits": "cpp", 36 | "typeinfo": "cpp", 37 | "utility": "cpp", 38 | "*.ipp": "cpp", 39 | "algorithm": "cpp", 40 | "codecvt": "cpp", 41 | "condition_variable": "cpp", 42 | "memory": "cpp", 43 | "mutex": "cpp", 44 | "system_error": "cpp" 45 | } 46 | } -------------------------------------------------------------------------------- /src/skimap_ros/.vscode/tasks.json: -------------------------------------------------------------------------------- 1 | { 2 | // See https://go.microsoft.com/fwlink/?LinkId=733558 3 | // for the documentation about the tasks.json format 4 | "version": "0.1.0", 5 | "command": "ll", 6 | "isShellCommand": true, 7 | "args": [], 8 | "showOutput": "always", 9 | "tasks": [{ 10 | "taskName": "cwd", 11 | "command": "catkinRootBuild", 12 | "isBuildCommand": true, 13 | "isShellCommand": true, 14 | "args": ["."] 15 | }, { 16 | "taskName": "done", 17 | "command": "echo", 18 | "isShellCommand": true, 19 | "args": ["\"Ready!\""] 20 | }] 21 | 22 | } -------------------------------------------------------------------------------- /src/skimap_ros/include/skimap/utils/CameraUtils.hpp: -------------------------------------------------------------------------------- 1 | //OPENCV 2 | #include 3 | 4 | using namespace cv; 5 | 6 | namespace skimap 7 | { 8 | namespace utils 9 | { 10 | 11 | /** 12 | */ 13 | struct PointRGB 14 | { 15 | cv::Point3f point; 16 | cv::Vec3b color; 17 | float w; 18 | }; 19 | 20 | /** 21 | */ 22 | struct Camera 23 | { 24 | double fx, fy, cx, cy; 25 | int width, height; 26 | double min_distance; 27 | double max_distance; 28 | int point_cloud_downscale; 29 | cv::Mat camera_matrix; 30 | 31 | void build3DTensor(cv::Mat &rgb, cv::Mat &depth, cv::Mat &tensor, int downsample_factor = 1) 32 | { 33 | downsample_factor = downsample_factor > 1 ? downsample_factor : 1; 34 | tensor = cv::Mat(rgb.rows, rgb.cols, CV_32FC3); 35 | 36 | for (int y = 0; y < depth.rows; y += downsample_factor) 37 | { 38 | for (int x = 0; x < depth.cols; x += downsample_factor) 39 | { 40 | float d = depth.at(y, x); 41 | cv::Vec3f p3d; 42 | p3d[0] = (d / this->fx) * (x - this->cx); 43 | p3d[1] = (d / this->fy) * (y - this->cy); 44 | p3d[2] = d; 45 | //printf("(%f,%f,%f,%f) %d,%d,%f = %f,%f,%f\n", this->fx, this->fy, this->cx, this->cy, y, x, d, p3d[0], p3d[1], p3d[2]); 46 | if (p3d[2] != p3d[2]) 47 | p3d[2] = 0.0f; 48 | if (p3d[2] < this->min_distance && p3d[2] > this->max_distance) 49 | p3d[2] = 0.0f; 50 | 51 | tensor.at(y, x) = p3d; 52 | } 53 | } 54 | } 55 | 56 | /** 57 | * Load Camera Object from YAML file 58 | */ 59 | static Camera loadFromFile(std::string yaml_file) 60 | { 61 | Camera camera; 62 | FileStorage fs(yaml_file, FileStorage::READ); 63 | camera.width = fs["image_width"]; 64 | camera.height = fs["image_height"]; 65 | fs["camera_matrix"] >> camera.camera_matrix; 66 | std::cout << camera.camera_matrix.type() << std::endl; 67 | camera.fx = camera.camera_matrix.at(0, 0); 68 | camera.fy = camera.camera_matrix.at(1, 1); 69 | camera.cx = camera.camera_matrix.at(0, 2); 70 | camera.cy = camera.camera_matrix.at(1, 2); 71 | camera.min_distance = fs["min_distance"]; 72 | camera.max_distance = fs["max_distance"]; 73 | return camera; 74 | } 75 | 76 | } camera; 77 | } 78 | } 79 | -------------------------------------------------------------------------------- /src/skimap_ros/include/skimap/utils/TimingsUtils.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | /** 10 | */ 11 | struct Timings 12 | { 13 | typedef std::chrono::high_resolution_clock Time; 14 | typedef std::chrono::milliseconds ms; 15 | typedef std::chrono::microseconds us; 16 | typedef std::chrono::duration fsec; 17 | 18 | std::map> times; 19 | 20 | void startTimer(std::string name) 21 | { 22 | times[name] = Time::now(); //IS NOT ROS TIME! 23 | } 24 | 25 | us elapsedMicroseconds(std::string name) 26 | { 27 | fsec elaps = Time::now() - times[name]; 28 | return std::chrono::duration_cast(elaps); 29 | } 30 | 31 | ms elapsedMilliseconds(std::string name) 32 | { 33 | fsec elaps = Time::now() - times[name]; 34 | return std::chrono::duration_cast(elaps); 35 | } 36 | 37 | void printTime(std::string name) 38 | { 39 | printf("Time for %s: %f ms\n", name.c_str(), float(elapsedMicroseconds(name).count()) / 1000.0f); 40 | } 41 | } DEBUG_TIMINGS; -------------------------------------------------------------------------------- /src/skimap_ros/include/skimap/voxels/GenericTile2D.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef GENERICTILE2D_HPP 10 | #define GENERICTILE2D_HPP 11 | #include 12 | 13 | namespace skimap 14 | { 15 | 16 | /** 17 | * Generic 3D Voxel representation. 18 | * V template represents datatype for user data. 19 | * D template represents datatype for coordinates. 20 | */ 21 | template 22 | struct GenericTile2D : public GenericVoxel3D 23 | { 24 | 25 | /** 26 | * Void constructor. Sets NULL for user data. 27 | */ 28 | GenericTile2D() : GenericVoxel3D(0, 0, 0, NULL) 29 | { 30 | } 31 | 32 | /** 33 | * Full Constructor 34 | * @param x X coordinate 35 | * @param y Y coordinate 36 | * @param z Z coordinate 37 | * @param data User Data pointer 38 | */ 39 | GenericTile2D(D x, D y, D z, V *data) : GenericVoxel3D(x, y, z, data) 40 | { 41 | } 42 | 43 | /** 44 | * Serializes object into stream. 45 | */ 46 | friend std::ostream &operator<<(std::ostream &os, const GenericTile2D &voxel) 47 | { 48 | os << std::setprecision(std::numeric_limits::digits10 + 2); 49 | os << voxel.x << " "; 50 | os << voxel.y << " "; 51 | os << voxel.z << " "; 52 | os << 1 << " "; 53 | if (voxel.data != NULL) 54 | os << *(voxel.data); 55 | return os; 56 | } 57 | 58 | /** 59 | * Hydrates object from stream. 60 | */ 61 | friend std::istream &operator>>(std::istream &is, GenericTile2D &voxel) 62 | { 63 | double x, y, z; 64 | int t; 65 | is >> x; 66 | is >> y; 67 | is >> z; 68 | is >> t; 69 | voxel.x = D(x); 70 | voxel.y = D(y); 71 | voxel.z = D(z); 72 | voxel.data = new V(); 73 | is >> (*voxel.data); 74 | return is; 75 | } 76 | }; 77 | } 78 | 79 | #endif /* GENERICTILE2D_HPP */ 80 | -------------------------------------------------------------------------------- /src/skimap_ros/include/skimap/voxels/GenericVoxelKD.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef GENERICVOXELKD_HPP 10 | #define GENERICVOXELKD_HPP 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace skimap 21 | { 22 | 23 | /** 24 | * Generic KD Voxel representation. 25 | * V template represents datatype for user data. 26 | * D template represents datatype for coordinates. 27 | */ 28 | template 29 | struct GenericVoxelKD 30 | { 31 | std::vector coordinates; 32 | V *data; 33 | 34 | GenericVoxelKD() : data(NULL) 35 | { 36 | } 37 | 38 | GenericVoxelKD(std::vector coordinates, V *data) : coordinates(coordinates), data(data) 39 | { 40 | } 41 | 42 | /** 43 | * Serializes object into stream. 44 | */ 45 | friend std::ostream &operator<<(std::ostream &os, const GenericVoxelKD &voxel) 46 | { 47 | /* os << std::setprecision(std::numeric_limits::digits10 + 2); 48 | os << voxel.x << " "; 49 | os << voxel.y << " "; 50 | os << voxel.z << " "; 51 | if (voxel.data != NULL) 52 | os << *(voxel.data); 53 | return os;*/ 54 | } 55 | 56 | /** 57 | * Hydrates object from stream. 58 | */ 59 | friend std::istream &operator>>(std::istream &is, GenericVoxelKD &voxel) 60 | { 61 | /* double x, y, z; 62 | is >> x; 63 | is >> y; 64 | is >> z; 65 | voxel.x = D(x); 66 | voxel.y = D(y); 67 | voxel.z = D(z); 68 | voxel.data = new V(); 69 | is >> (*voxel.data); 70 | return is;*/ 71 | } 72 | }; 73 | } 74 | 75 | #endif /* GenericVoxelKD_HPP */ 76 | -------------------------------------------------------------------------------- /src/skimap_ros/include/slamdunk/gpu_features.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef SLAM_DUNK_GPU_FEATURES_H 10 | #define SLAM_DUNK_GPU_FEATURES_H 11 | 12 | #include "slamdunk/slamdunk_defines.h" 13 | 14 | #include "opencv2/features2d/features2d.hpp" 15 | #include "opencv2/gpu/gpu.hpp" 16 | #include "opencv2/nonfree/gpu.hpp" 17 | 18 | namespace slamdunk 19 | { 20 | 21 | class SLAM_DUNK_API GPUSurf : public cv::Feature2D 22 | { 23 | public: 24 | explicit GPUSurf(double hessianThreshold, int nOctaves = 4, 25 | int nOctaveLayers = 2, bool extended = false, float keypointsRatio = 0.01f, bool upright = false); 26 | 27 | //! returns the descriptor size in floats (64 or 128) 28 | inline int descriptorSize() const { return m_surf_gpu.descriptorSize(); } 29 | 30 | //! returns the descriptor type 31 | inline int descriptorType() const { return CV_32F; } 32 | 33 | //! finds the keypoints using SIFT algorithm 34 | void operator()(cv::InputArray img, cv::InputArray mask, 35 | std::vector &keypoints) const; 36 | //! finds the keypoints and computes descriptors for them using SIFT algorithm. 37 | //! Optionally it can compute descriptors for the user-provided keypoints 38 | void operator()(cv::InputArray img, cv::InputArray mask, 39 | std::vector &keypoints, 40 | cv::OutputArray descriptors, 41 | bool useProvidedKeypoints = false) const; 42 | 43 | // This function has little sense outside of an OpenCV module 44 | cv::AlgorithmInfo *info() const { return NULL; }; 45 | 46 | protected: 47 | void detectImpl(const cv::Mat &image, std::vector &keypoints, const cv::Mat &mask = cv::Mat()) const; 48 | void computeImpl(const cv::Mat &image, std::vector &keypoints, cv::Mat &descriptors) const; 49 | 50 | mutable cv::gpu::SURF_GPU m_surf_gpu; // cv::SURF_GPU needs to change its state during feature extraction 51 | mutable cv::gpu::GpuMat m_image_gpu, m_mask_gpu; // we need to upload the image onto the device before actual computation 52 | }; 53 | 54 | typedef GPUSurf GPUSurfFeatureDetector; 55 | typedef GPUSurf GPUSurfDescriptorExtractor; 56 | } 57 | 58 | #endif // SLAM_DUNK_GPU_FEATURES_H 59 | -------------------------------------------------------------------------------- /src/skimap_ros/include/slamdunk/pretty_printer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef SLAM_DUNK_PRETTY_PRINTER_H 10 | #define SLAM_DUNK_PRETTY_PRINTER_H 11 | 12 | #ifdef _MSC_VER 13 | #define SLAM_DUNK_FUNCTION_STR __FUNCSIG__ 14 | #else 15 | #ifdef __GNUC__ 16 | #define SLAM_DUNK_FUNCTION_STR __PRETTY_FUNCTION__ 17 | #else 18 | #define SLAM_DUNK_FUNCTION_STR __FUNCTION__ 19 | #endif 20 | #endif 21 | 22 | // std logging string 23 | #define SLAM_DUNK_DEBUG_STR(s) "\033[1;34m[DEBUG]\033[0m" \ 24 | << "\033[1m[" << SLAM_DUNK_FUNCTION_STR << "]\033[0m\n\t" << s 25 | #define SLAM_DUNK_INFO_STR(s) "\033[1;32m[INFO] \033[0m" \ 26 | << "\033[1m[" << SLAM_DUNK_FUNCTION_STR << "]\033[0m\n\t" << s 27 | #define SLAM_DUNK_WARNING_STR(s) "\033[1;33m[WARN] \033[0m" \ 28 | << "\033[1m[" << SLAM_DUNK_FUNCTION_STR << "]\033[0m\n\t" << s 29 | #define SLAM_DUNK_ERROR_STR(s) "\033[1;31m[ERROR]\033[0m" \ 30 | << "\033[1m[" << SLAM_DUNK_FUNCTION_STR << "]\033[0m\n\t" << s 31 | #define SLAM_DUNK_FATAL_STR(s) "\033[1;31m[FATAL]\033[0m" \ 32 | << "\033[1m[" << SLAM_DUNK_FUNCTION_STR << "]\033[0m\n\t" << s 33 | 34 | #endif // SLAM_DUNK_PRETTY_PRINTER_H 35 | -------------------------------------------------------------------------------- /src/skimap_ros/include/slamdunk/slamdunk_defines.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef SLAM_DUNK_SLAMDUNK_DEFINES_H 10 | #define SLAM_DUNK_SLAMDUNK_DEFINES_H 11 | 12 | // The following ifdef block is the standard way of creating macros which make exporting 13 | // from a DLL simpler. All files within this DLL are compiled with the SLAM_DUNK_EXPORTS 14 | // symbol defined on the command line. This symbol should not be defined on any project 15 | // that uses this DLL. This way any other project whose source files include this file see 16 | // SEBA_API functions as being imported from a DLL, whereas this DLL sees symbols 17 | // defined with this macro as being exported. 18 | #ifdef _MSC_VER 19 | #ifdef SLAM_DUNK_EXPORTS 20 | #define SLAM_DUNK_API __declspec(dllexport) 21 | #else 22 | #define SLAM_DUNK_API __declspec(dllimport) 23 | #endif 24 | #else 25 | #define SLAM_DUNK_API 26 | #endif 27 | 28 | #ifdef _MSC_VER 29 | #ifndef _CRT_SECURE_NO_WARNINGS 30 | #define _CRT_SECURE_NO_WARNINGS 31 | #endif 32 | #ifndef NOMINMAX 33 | #define NOMINMAX 34 | #endif 35 | 36 | #include "windows.h" 37 | 38 | #ifndef M_PI 39 | #define M_PI 3.14159265358979323846 40 | #endif 41 | #endif 42 | 43 | #if defined(_MSC_VER) && _MSC_VER <= 1600 // 1400 == VC++ 8.0, 1600 == VC++ 10.0 44 | #pragma warning(disable : 4251 4231 4660) 45 | #endif 46 | 47 | // Automatically generated part 48 | //#define SLAMDUNK_TIMERS_ENABLED false 49 | #define SLAMDUNK_FLANN_ENABLED 50 | #define SLAMDUNK_ICP_ENABLED 51 | 52 | #endif // SLAM_DUNK_SLAMDUNK_DEFINES_H 53 | -------------------------------------------------------------------------------- /src/skimap_ros/include/slamdunk_extension/FIROrder2.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #ifndef FIRORDER2_H 19 | #define FIRORDER2_H 20 | 21 | class FIROrder2 22 | { 23 | public: 24 | FIROrder2(int data_size, double cutoff, double quality = 0.5); 25 | virtual ~FIROrder2(); 26 | void updateParameters(double cutoff); 27 | void setSampleTime(double sample_time); 28 | 29 | void setInput(int index, double X); 30 | void setInputs(double *Xs); 31 | void setInput(Eigen::Isometry3d &iso); 32 | void setInitialData(double *Xs); 33 | void setInitialData(Eigen::Isometry3d &iso); 34 | 35 | void getOutput(Eigen::Isometry3d &iso); 36 | 37 | double Fc; 38 | double Fs; 39 | double Q; 40 | double W; 41 | double N; 42 | double B0; 43 | double B1; 44 | double B2; 45 | double A1; 46 | double A2; 47 | 48 | std::vector output; 49 | std::vector> data; 50 | bool ready; 51 | 52 | private: 53 | void conversion(Eigen::Isometry3d &iso, double *&data, bool reverse = false); 54 | int data_size; 55 | }; 56 | 57 | #endif /* FIRORDER2_H */ -------------------------------------------------------------------------------- /src/skimap_ros/launch/skimap/skimap_live.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /src/skimap_ros/launch/skimap/skimap_map_service.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /src/skimap_ros/launch/skimap_live_experiment.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /src/skimap_ros/launch/slamdunk_tracker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /src/skimap_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | skimap_ros 4 | 0.0.0 5 | The lar_slam package 6 | 7 | 8 | 9 | 10 | lar 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | cv_bridge 36 | 37 | 38 | 39 | cv_bridge 40 | message_generation 41 | message_runtime 42 | 43 | 44 | catkin 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/bench.py: -------------------------------------------------------------------------------- 1 | import subprocess 2 | from subprocess import check_output 3 | import sys 4 | 5 | proc = './../../../work/ros/skimap_ws/devel/lib/skimap_ros/grid2d' 6 | 7 | step = 2000 8 | coords = 20000 9 | coords_max = coords * coords 10 | 11 | points = [1000, 10000, 20000, 50000, 100000, 150000, 12 | 200000, 300000, 500000, 1000000, 1500000, 2000000, 3000000, 3500000, 5000000, 10000000, 20000000] # , 1000000, 2000000, 5000000, 10000000, 20000000, 25000000] 13 | 14 | 15 | for i in points: 16 | n_points = i 17 | coords = coords 18 | algo = sys.argv[1] 19 | command = proc + " {} {} {} 0".format(n_points, coords, algo) 20 | p = subprocess.Popen(command.split(' '), stdout=subprocess.PIPE) 21 | out, err = p.communicate() 22 | print out 23 | -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/bench_people.py: -------------------------------------------------------------------------------- 1 | import subprocess 2 | from subprocess import check_output 3 | import sys 4 | 5 | proc = './../../../work/ros/skimap_ws/devel/lib/skimap_ros/grid2d' 6 | 7 | step = 2000 8 | coords = 24000 9 | coords_max = coords * coords 10 | 11 | points = [60000000] 12 | 13 | 14 | for i in points: 15 | n_points = i 16 | coords = coords 17 | algo = sys.argv[1] 18 | command = proc + " {} {} {} 0".format(n_points, coords, algo) 19 | p = subprocess.Popen(command.split(' '), stdout=subprocess.PIPE) 20 | out, err = p.communicate() 21 | print out 22 | -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_dimensions/kdskip_dim2.csv: -------------------------------------------------------------------------------- 1 | kdskip 1 1000000 5000.000000 4.000000 0.697775 0.125374 530220.000000 2 | kdskip 2 1000000 5000.000000 4.000000 0.697775 0.125374 530220.000000 3 | kdskip 3 1000000 5000.000000 9.000000 0.680560 0.180693 683368.000000 4 | kdskip 4 1000000 5000.000000 16.000000 0.774712 0.214544 973404.000000 5 | kdskip 5 1000000 5000.000000 25.000000 1.018543 0.236273 1308892.000000 6 | kdskip 6 1000000 5000.000000 36.000000 1.294357 0.247673 1641496.000000 7 | kdskip 7 1000000 5000.000000 49.000000 1.582677 0.295864 2022220.000000 8 | kdskip 8 1000000 5000.000000 64.000000 1.830637 0.320213 2319600.000000 9 | kdskip 9 1000000 5000.000000 81.000000 2.084006 0.351131 2625728.000000 10 | kdskip 10 1000000 5000.000000 100.000000 2.369433 0.393254 2961316.000000 11 | kdskip 11 1000000 5000.000000 121.000000 2.654375 0.420219 3355016.000000 12 | kdskip 12 1000000 5000.000000 144.000000 2.931303 0.453290 3693100.000000 13 | kdskip 13 1000000 5000.000000 169.000000 3.149653 0.458285 3996056.000000 14 | kdskip 14 1000000 5000.000000 196.000000 3.400766 0.502937 4306952.000000 15 | kdskip 15 1000000 5000.000000 225.000000 3.674220 0.530242 4716800.000000 16 | kdskip 16 1000000 5000.000000 256.000000 3.989024 0.554386 5024088.000000 17 | kdskip 17 1000000 5000.000000 289.000000 4.149026 0.614230 5352828.000000 18 | kdskip 18 1000000 5000.000000 324.000000 4.386562 0.552536 5668984.000000 19 | kdskip 19 1000000 5000.000000 361.000000 4.621640 0.589713 6009448.000000 20 | kdskip 20 1000000 5000.000000 400.000000 4.836186 0.727907 6349464.000000 21 | kdskip 21 1000000 5000.000000 441.000000 5.047706 0.748532 6655412.000000 22 | kdskip 22 1000000 5000.000000 484.000000 5.241879 0.869755 6977936.000000 23 | kdskip 23 1000000 5000.000000 529.000000 5.459003 0.842341 7316388.000000 24 | kdskip 24 1000000 5000.000000 576.000000 5.620885 0.943085 7640936.000000 25 | kdskip 25 1000000 5000.000000 625.000000 5.717347 0.778699 7892008.000000 26 | kdskip 26 1000000 5000.000000 676.000000 5.988459 0.755794 8221876.000000 27 | kdskip 27 1000000 5000.000000 729.000000 6.174693 0.872088 8579468.000000 28 | kdskip 28 1000000 5000.000000 784.000000 6.338548 0.940821 8915220.000000 29 | kdskip 29 1000000 5000.000000 841.000000 6.344337 0.804551 9050992.000000 30 | kdskip 30 1000000 5000.000000 900.000000 6.586970 0.947081 9438672.000000 31 | kdskip 31 1000000 5000.000000 961.000000 6.724866 1.153238 9626052.000000 32 | kdskip 32 1000000 5000.000000 1024.000000 6.773105 1.276301 9727748.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_dimensions/kdskip_r10.csv: -------------------------------------------------------------------------------- 1 | kdskip 1 1000000 5000.000000 10.000000 0.674344 0.048984 381336.000000 2 | kdskip 2 1000000 5000.000000 10.000000 0.674344 0.048984 381336.000000 3 | kdskip 3 1000000 5000.000000 10.000000 0.648449 0.178386 667528.000000 4 | kdskip 4 1000000 5000.000000 10.000000 0.802086 0.218102 1030156.000000 5 | kdskip 5 1000000 5000.000000 10.000000 1.097474 0.248795 1410780.000000 6 | kdskip 6 1000000 5000.000000 10.000000 1.439023 0.292872 1793048.000000 7 | kdskip 7 1000000 5000.000000 10.000000 1.439023 0.292872 1793048.000000 8 | kdskip 8 1000000 5000.000000 10.000000 2.095330 0.343382 2644444.000000 9 | kdskip 9 1000000 5000.000000 10.000000 2.453285 0.371234 3037140.000000 10 | kdskip 10 1000000 5000.000000 10.000000 2.779600 0.405413 3428652.000000 11 | kdskip 11 1000000 5000.000000 10.000000 3.116769 0.446933 3870876.000000 12 | kdskip 12 1000000 5000.000000 10.000000 3.116769 0.446933 3870876.000000 13 | kdskip 13 1000000 5000.000000 10.000000 3.827201 0.514513 4652716.000000 14 | kdskip 14 1000000 5000.000000 10.000000 4.127972 0.529756 5046532.000000 15 | kdskip 15 1000000 5000.000000 10.000000 4.520226 0.555738 5509820.000000 16 | kdskip 16 1000000 5000.000000 10.000000 4.866411 0.607717 5900076.000000 17 | kdskip 17 1000000 5000.000000 10.000000 5.201311 0.626816 6292120.000000 18 | kdskip 18 1000000 5000.000000 10.000000 5.549316 0.665598 6682628.000000 19 | kdskip 19 1000000 5000.000000 10.000000 5.929376 0.707439 7121148.000000 20 | kdskip 20 1000000 5000.000000 10.000000 6.202403 0.725731 7513032.000000 21 | kdskip 21 1000000 5000.000000 10.000000 6.546089 0.766790 7902424.000000 22 | kdskip 22 1000000 5000.000000 10.000000 6.938184 0.803397 8295404.000000 23 | kdskip 23 1000000 5000.000000 10.000000 6.938184 0.803397 8295404.000000 24 | kdskip 24 1000000 5000.000000 10.000000 7.597998 0.849797 9138388.000000 25 | kdskip 25 1000000 5000.000000 10.000000 7.968272 0.892223 9527388.000000 26 | kdskip 26 1000000 5000.000000 10.000000 8.250544 0.909905 9919896.000000 27 | kdskip 27 1000000 5000.000000 10.000000 8.576606 0.937918 10382252.000000 28 | kdskip 28 1000000 5000.000000 10.000000 8.941732 0.991277 10773444.000000 29 | kdskip 29 1000000 5000.000000 10.000000 9.310804 1.024069 11164484.000000 30 | kdskip 30 1000000 5000.000000 10.000000 9.704137 1.057759 11553432.000000 31 | kdskip 31 1000000 5000.000000 10.000000 9.704137 1.057759 11553432.000000 32 | kdskip 32 1000000 5000.000000 10.000000 10.410001 1.156654 12378372.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_dimensions/kdskip_r32.csv: -------------------------------------------------------------------------------- 1 | kdskip 1 1000000 5000.000000 32.000000 1.447217 0.023413 355004.000000 2 | kdskip 2 1000000 5000.000000 32.000000 1.447217 0.023413 355004.000000 3 | kdskip 3 1000000 5000.000000 32.000000 0.596022 0.147722 550032.000000 4 | kdskip 4 1000000 5000.000000 32.000000 0.702250 0.198724 903644.000000 5 | kdskip 5 1000000 5000.000000 32.000000 1.013412 0.234716 1276008.000000 6 | kdskip 6 1000000 5000.000000 32.000000 1.338032 0.251149 1662172.000000 7 | kdskip 7 1000000 5000.000000 32.000000 1.646094 0.301473 2119988.000000 8 | kdskip 8 1000000 5000.000000 32.000000 1.993549 0.322553 2512464.000000 9 | kdskip 9 1000000 5000.000000 32.000000 2.333649 0.355499 2903852.000000 10 | kdskip 10 1000000 5000.000000 32.000000 2.692977 0.383272 3295484.000000 11 | kdskip 11 1000000 5000.000000 32.000000 3.030865 0.422675 3740868.000000 12 | kdskip 12 1000000 5000.000000 32.000000 3.330562 0.457584 4130468.000000 13 | kdskip 13 1000000 5000.000000 32.000000 3.695394 0.482436 4521232.000000 14 | kdskip 14 1000000 5000.000000 32.000000 4.035857 0.535102 4912068.000000 15 | kdskip 15 1000000 5000.000000 32.000000 4.035857 0.535102 4912068.000000 16 | kdskip 16 1000000 5000.000000 32.000000 4.695685 0.593909 5769512.000000 17 | kdskip 17 1000000 5000.000000 32.000000 5.043141 0.624753 6160932.000000 18 | kdskip 18 1000000 5000.000000 32.000000 5.395551 0.650483 6551100.000000 19 | kdskip 19 1000000 5000.000000 32.000000 5.772160 0.699420 6989360.000000 20 | kdskip 20 1000000 5000.000000 32.000000 6.091953 0.742820 7380752.000000 21 | kdskip 21 1000000 5000.000000 32.000000 6.614057 0.754936 7773284.000000 22 | kdskip 22 1000000 5000.000000 32.000000 6.737995 0.819610 8164240.000000 23 | kdskip 23 1000000 5000.000000 32.000000 7.090939 0.814002 8614660.000000 24 | kdskip 24 1000000 5000.000000 32.000000 7.440571 0.861760 9006744.000000 25 | kdskip 25 1000000 5000.000000 32.000000 7.779695 0.892008 9397812.000000 26 | kdskip 26 1000000 5000.000000 32.000000 8.098206 0.913954 9786936.000000 27 | kdskip 27 1000000 5000.000000 32.000000 8.423368 0.962186 10249636.000000 28 | kdskip 28 1000000 5000.000000 32.000000 8.764888 0.980215 10640868.000000 29 | kdskip 29 1000000 5000.000000 32.000000 9.169572 1.020895 11031444.000000 30 | kdskip 30 1000000 5000.000000 32.000000 9.486545 1.047982 11422720.000000 31 | kdskip 31 1000000 5000.000000 32.000000 9.846071 1.104355 11867968.000000 32 | kdskip 32 1000000 5000.000000 32.000000 10.221925 1.148831 12257568.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_octree_points/kdskip.csv: -------------------------------------------------------------------------------- 1 | kdskip 3 100000 365.000000 0.050000 0.093395 0.007467 87244.000000 2 | kdskip 3 150000 365.000000 0.050000 0.154709 0.010184 118844.000000 3 | kdskip 3 200000 365.000000 0.050000 0.191743 0.012871 151332.000000 4 | kdskip 3 250000 365.000000 0.050000 0.246193 0.015747 180536.000000 5 | kdskip 3 300000 365.000000 0.050000 0.298760 0.018668 218672.000000 6 | kdskip 3 350000 365.000000 0.050000 0.335976 0.022016 249112.000000 7 | kdskip 3 400000 365.000000 0.050000 0.335976 0.022016 249112.000000 8 | kdskip 3 450000 365.000000 0.050000 0.440828 0.026456 308936.000000 9 | kdskip 3 500000 365.000000 0.050000 0.486196 0.029746 341636.000000 10 | kdskip 3 550000 365.000000 0.050000 0.523022 0.035309 376448.000000 11 | kdskip 3 600000 365.000000 0.050000 0.586299 0.039612 408640.000000 12 | kdskip 3 650000 365.000000 0.050000 0.622749 0.036346 436128.000000 13 | kdskip 3 700000 365.000000 0.050000 0.700533 0.039648 470996.000000 14 | kdskip 3 750000 365.000000 0.050000 0.721011 0.044963 499268.000000 15 | kdskip 3 800000 365.000000 0.050000 0.800587 0.045966 534252.000000 16 | kdskip 3 850000 365.000000 0.050000 0.830620 0.047785 559880.000000 17 | kdskip 3 900000 365.000000 0.050000 0.875534 0.049676 601436.000000 18 | kdskip 3 950000 365.000000 0.050000 0.924537 0.053269 622484.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_octree_points/kdtree.csv: -------------------------------------------------------------------------------- 1 | flann_kdtree 3 100000 365.000000 0.001000 0.053696 0.004566 46920.000000 2 | flann_kdtree 3 150000 365.000000 0.001000 0.081967 0.006693 63648.000000 3 | flann_kdtree 3 200000 365.000000 0.001000 0.110485 0.009045 74600.000000 4 | flann_kdtree 3 250000 365.000000 0.001000 0.140574 0.010506 88920.000000 5 | flann_kdtree 3 300000 365.000000 0.001000 0.167284 0.012076 104200.000000 6 | flann_kdtree 3 350000 365.000000 0.001000 0.199567 0.014194 117096.000000 7 | flann_kdtree 3 400000 365.000000 0.001000 0.226392 0.016083 132016.000000 8 | flann_kdtree 3 450000 365.000000 0.001000 0.261273 0.018341 144056.000000 9 | flann_kdtree 3 500000 365.000000 0.001000 0.289858 0.019501 158184.000000 10 | flann_kdtree 3 550000 365.000000 0.001000 0.322065 0.021387 171984.000000 11 | flann_kdtree 3 600000 365.000000 0.001000 0.348963 0.023333 186440.000000 12 | flann_kdtree 3 650000 365.000000 0.001000 0.384814 0.025017 200328.000000 13 | flann_kdtree 3 700000 365.000000 0.001000 0.422366 0.027067 214776.000000 14 | flann_kdtree 3 750000 365.000000 0.001000 0.457865 0.029708 227768.000000 15 | flann_kdtree 3 800000 365.000000 0.001000 0.486370 0.030666 241588.000000 16 | flann_kdtree 3 850000 365.000000 0.001000 0.526671 0.033729 254568.000000 17 | flann_kdtree 3 900000 365.000000 0.001000 0.559139 0.036446 269016.000000 18 | flann_kdtree 3 950000 365.000000 0.001000 0.598645 0.039654 283704.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_octree_points/octree.csv: -------------------------------------------------------------------------------- 1 | octree 3 100000 365.000000 0.001000 0.086353 0.008081 153364.000000 2 | octree 3 150000 365.000000 0.001000 0.128280 0.011317 218352.000000 3 | octree 3 200000 365.000000 0.001000 0.170343 0.013679 281052.000000 4 | octree 3 250000 365.000000 0.001000 0.215232 0.017047 343220.000000 5 | octree 3 300000 365.000000 0.001000 0.261374 0.020370 407024.000000 6 | octree 3 350000 365.000000 0.001000 0.307148 0.024205 469240.000000 7 | octree 3 400000 365.000000 0.001000 0.350118 0.028330 530804.000000 8 | octree 3 450000 365.000000 0.001000 0.397585 0.031959 590236.000000 9 | octree 3 500000 365.000000 0.001000 0.440896 0.034976 651468.000000 10 | octree 3 550000 365.000000 0.001000 0.485383 0.038075 712832.000000 11 | octree 3 600000 365.000000 0.001000 0.533736 0.041620 772652.000000 12 | octree 3 650000 365.000000 0.001000 0.572267 0.044646 835064.000000 13 | octree 3 700000 365.000000 0.001000 0.637531 0.048530 894292.000000 14 | octree 3 750000 365.000000 0.001000 0.666899 0.051920 953460.000000 15 | octree 3 800000 365.000000 0.001000 0.715680 0.055405 1016012.000000 16 | octree 3 850000 365.000000 0.001000 0.765032 0.059372 1075952.000000 17 | octree 3 900000 365.000000 0.001000 0.817587 0.062195 1135144.000000 18 | octree 3 950000 365.000000 0.001000 0.872649 0.065883 1193956.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_octree_points/skimap.csv: -------------------------------------------------------------------------------- 1 | skimap 3 100000 365.000000 0.050000 0.095232 0.002332 107844.000000 2 | skimap 3 150000 365.000000 0.050000 0.095232 0.002332 107844.000000 3 | skimap 3 200000 365.000000 0.050000 0.205925 0.003808 192648.000000 4 | skimap 3 250000 365.000000 0.050000 0.250978 0.004408 233900.000000 5 | skimap 3 300000 365.000000 0.050000 0.300703 0.005213 277652.000000 6 | skimap 3 350000 365.000000 0.050000 0.348785 0.010129 319280.000000 7 | skimap 3 400000 365.000000 0.050000 0.388460 0.006731 360164.000000 8 | skimap 3 450000 365.000000 0.050000 0.447114 0.009194 402908.000000 9 | skimap 3 500000 365.000000 0.050000 0.481272 0.008451 444204.000000 10 | skimap 3 550000 365.000000 0.050000 0.544395 0.009294 487808.000000 11 | skimap 3 600000 365.000000 0.050000 0.595937 0.009820 530036.000000 12 | skimap 3 650000 365.000000 0.050000 0.653879 0.010961 573012.000000 13 | skimap 3 700000 365.000000 0.050000 0.690596 0.011423 614056.000000 14 | skimap 3 750000 365.000000 0.050000 0.739642 0.011914 655872.000000 15 | skimap 3 800000 365.000000 0.050000 0.802050 0.012861 697732.000000 16 | skimap 3 850000 365.000000 0.050000 0.839659 0.013433 739988.000000 17 | skimap 3 900000 365.000000 0.050000 0.897173 0.014868 779924.000000 18 | skimap 3 950000 365.000000 0.050000 0.945125 0.015188 823224.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_2d/kdskip_r1.csv: -------------------------------------------------------------------------------- 1 | kdskip 2 1000000 5000.000000 1.000000 0.814965 0.172397 611040.000000 2 | kdskip 2 1500000 5000.000000 1.000000 1.276408 0.260442 795564.000000 3 | kdskip 2 2000000 5000.000000 1.000000 1.683748 0.335462 1001332.000000 4 | kdskip 2 2500000 5000.000000 1.000000 2.127634 0.426662 1159448.000000 5 | kdskip 2 3000000 5000.000000 1.000000 2.542711 0.507590 1345488.000000 6 | kdskip 2 3500000 5000.000000 1.000000 2.991633 0.583608 1449908.000000 7 | kdskip 2 4000000 5000.000000 1.000000 3.478163 0.682334 1704896.000000 8 | kdskip 2 4500000 5000.000000 1.000000 3.937203 0.762582 1848648.000000 9 | kdskip 2 5000000 5000.000000 1.000000 4.396502 0.865608 1969524.000000 10 | kdskip 2 5500000 5000.000000 1.000000 4.871279 0.925383 2092820.000000 11 | kdskip 2 6000000 5000.000000 1.000000 5.298679 1.004504 2238236.000000 12 | kdskip 2 6500000 5000.000000 1.000000 5.769029 1.081194 2398952.000000 13 | kdskip 2 7000000 5000.000000 1.000000 6.207895 1.144101 2550196.000000 14 | kdskip 2 7500000 5000.000000 1.000000 6.812027 1.278237 2681732.000000 15 | kdskip 2 8000000 5000.000000 1.000000 7.123045 1.346384 2829648.000000 16 | kdskip 2 8500000 5000.000000 1.000000 7.729137 1.421366 2964756.000000 17 | kdskip 2 9000000 5000.000000 1.000000 8.280959 1.445313 3125180.000000 18 | kdskip 2 9500000 5000.000000 1.000000 8.870138 1.517883 3297400.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_2d/kdskip_r10.csv: -------------------------------------------------------------------------------- 1 | kdskip 2 1000000 5000.000000 10.000000 0.673127 0.048068 387704.000000 2 | kdskip 2 1500000 5000.000000 10.000000 1.088257 0.059753 451092.000000 3 | kdskip 2 2000000 5000.000000 10.000000 1.533063 0.091017 527624.000000 4 | kdskip 2 2500000 5000.000000 10.000000 1.975446 0.091328 597016.000000 5 | kdskip 2 3000000 5000.000000 10.000000 2.478252 0.119050 680096.000000 6 | kdskip 2 3500000 5000.000000 10.000000 3.426770 0.116500 740484.000000 7 | kdskip 2 4000000 5000.000000 10.000000 3.701359 0.128698 841480.000000 8 | kdskip 2 4500000 5000.000000 10.000000 4.290214 0.129612 915384.000000 9 | kdskip 2 5000000 5000.000000 10.000000 4.969801 0.148042 1009252.000000 10 | kdskip 2 5500000 5000.000000 10.000000 5.779322 0.156280 1090892.000000 11 | kdskip 2 6000000 5000.000000 10.000000 6.893417 0.164418 1186212.000000 12 | kdskip 2 6500000 5000.000000 10.000000 7.834441 0.170382 1269364.000000 13 | kdskip 2 7000000 5000.000000 10.000000 9.339149 0.181571 1361980.000000 14 | kdskip 2 7500000 5000.000000 10.000000 10.688451 0.196093 1467256.000000 15 | kdskip 2 8000000 5000.000000 10.000000 11.787132 0.202779 1559204.000000 16 | kdskip 2 8500000 5000.000000 10.000000 11.690665 0.217115 1677724.000000 17 | kdskip 2 9000000 5000.000000 10.000000 13.739477 0.258461 1754236.000000 18 | kdskip 2 9500000 5000.000000 10.000000 14.940938 0.240768 1865908.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_2d/kdskip_r20.csv: -------------------------------------------------------------------------------- 1 | kdskip 2 1000000 5000.000000 20.000000 0.839913 0.029197 334032.000000 2 | kdskip 2 1500000 5000.000000 20.000000 1.559849 0.035840 424984.000000 3 | kdskip 2 2000000 5000.000000 20.000000 2.487401 0.044774 522940.000000 4 | kdskip 2 2500000 5000.000000 20.000000 3.656181 0.057829 623900.000000 5 | kdskip 2 3000000 5000.000000 20.000000 5.031625 0.081021 731004.000000 6 | kdskip 2 3500000 5000.000000 20.000000 6.629939 0.094072 839408.000000 7 | kdskip 2 4000000 5000.000000 20.000000 8.530534 0.110229 946548.000000 8 | kdskip 2 4500000 5000.000000 20.000000 10.545138 0.109528 1067200.000000 9 | kdskip 2 5000000 5000.000000 20.000000 12.764492 0.104058 1179316.000000 10 | kdskip 2 5500000 5000.000000 20.000000 15.375516 0.081756 1300068.000000 11 | kdskip 2 6000000 5000.000000 20.000000 18.221374 0.086772 1413200.000000 12 | kdskip 2 6500000 5000.000000 20.000000 21.291247 0.035767 1550408.000000 13 | kdskip 2 7000000 5000.000000 20.000000 24.212804 0.035050 1656924.000000 14 | kdskip 2 7500000 5000.000000 20.000000 27.780371 0.044177 1769356.000000 15 | kdskip 2 8000000 5000.000000 20.000000 31.384978 0.039121 1903648.000000 16 | kdskip 2 8500000 5000.000000 20.000000 35.256120 0.038144 2024208.000000 17 | kdskip 2 9000000 5000.000000 20.000000 39.097930 0.048569 2134400.000000 18 | kdskip 2 9500000 5000.000000 20.000000 43.659300 0.030082 2268652.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_2d/kdskip_r5.csv: -------------------------------------------------------------------------------- 1 | kdskip 2 1000000 5000.000000 5.000000 0.685599 0.111093 493976.000000 2 | kdskip 2 1500000 5000.000000 5.000000 1.026361 0.141201 623828.000000 3 | kdskip 2 2000000 5000.000000 5.000000 1.365625 0.157038 718500.000000 4 | kdskip 2 2500000 5000.000000 5.000000 1.748177 0.171963 770384.000000 5 | kdskip 2 3000000 5000.000000 5.000000 2.113779 0.181830 833284.000000 6 | kdskip 2 3500000 5000.000000 5.000000 2.635059 0.223364 900488.000000 7 | kdskip 2 4000000 5000.000000 5.000000 3.141288 0.202865 972956.000000 8 | kdskip 2 4500000 5000.000000 5.000000 3.667826 0.218306 1087428.000000 9 | kdskip 2 5000000 5000.000000 5.000000 3.993233 0.192159 1152196.000000 10 | kdskip 2 5500000 5000.000000 5.000000 4.145108 0.235567 1220760.000000 11 | kdskip 2 6000000 5000.000000 5.000000 4.506435 0.245193 1287676.000000 12 | kdskip 2 6500000 5000.000000 5.000000 4.995985 0.260913 1350308.000000 13 | kdskip 2 7000000 5000.000000 5.000000 5.295985 0.260913 1350308.000000 14 | kdskip 2 7500000 5000.000000 5.000000 5.801158 0.290520 1496948.000000 15 | kdskip 2 8000000 5000.000000 5.000000 6.487572 0.302891 1549824.000000 16 | kdskip 2 8500000 5000.000000 5.000000 7.016074 0.299087 1612472.000000 17 | kdskip 2 9000000 5000.000000 5.000000 7.397598 0.312716 1686752.000000 18 | kdskip 2 9500000 5000.000000 5.000000 8.137543 0.395591 1751644.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_2d/kdtree.csv: -------------------------------------------------------------------------------- 1 | flann_kdtree 2 1000000 5000.000000 10.000000 0.539054 0.231887 449056.000000 2 | flann_kdtree 2 1500000 5000.000000 10.000000 0.851489 0.351304 593224.000000 3 | flann_kdtree 2 2000000 5000.000000 10.000000 1.216012 0.525777 731408.000000 4 | flann_kdtree 2 2500000 5000.000000 10.000000 1.540171 0.651280 870508.000000 5 | flann_kdtree 2 3000000 5000.000000 10.000000 1.897952 0.824892 1010260.000000 6 | flann_kdtree 2 3500000 5000.000000 10.000000 2.278977 1.025024 1151952.000000 7 | flann_kdtree 2 4000000 5000.000000 10.000000 2.637600 1.109519 1290068.000000 8 | flann_kdtree 2 4500000 5000.000000 10.000000 3.011912 1.355155 1434136.000000 9 | flann_kdtree 2 5000000 5000.000000 10.000000 3.419225 1.517113 1571812.000000 10 | flann_kdtree 2 5500000 5000.000000 10.000000 3.859852 1.736507 1713196.000000 11 | flann_kdtree 2 6000000 5000.000000 10.000000 4.169820 1.830540 1851520.000000 12 | flann_kdtree 2 6500000 5000.000000 10.000000 4.661719 2.072784 1991780.000000 13 | flann_kdtree 2 7000000 5000.000000 10.000000 5.025375 2.172728 2132124.000000 14 | flann_kdtree 2 7500000 5000.000000 10.000000 5.504341 2.414933 2271692.000000 15 | flann_kdtree 2 8000000 5000.000000 10.000000 6.069174 2.547094 2412124.000000 16 | flann_kdtree 2 8500000 5000.000000 10.000000 6.216623 2.638016 2550852.000000 17 | flann_kdtree 2 9000000 5000.000000 10.000000 6.559452 2.936697 2694860.000000 18 | flann_kdtree 2 9500000 5000.000000 10.000000 6.993293 3.091916 2833060.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_3d/kdskip_r1.csv: -------------------------------------------------------------------------------- 1 | kdskip 3 1000000 5000.000000 1.000000 0.934796 0.207735 951944.000000 2 | kdskip 3 1500000 5000.000000 1.000000 1.403396 0.304142 1338764.000000 3 | kdskip 3 2000000 5000.000000 1.000000 1.901982 0.410598 1713148.000000 4 | kdskip 3 2500000 5000.000000 1.000000 2.395175 0.523172 2078928.000000 5 | kdskip 3 3000000 5000.000000 1.000000 2.911742 0.645338 2421912.000000 6 | kdskip 3 3500000 5000.000000 1.000000 3.373410 0.752494 2792212.000000 7 | kdskip 3 4000000 5000.000000 1.000000 3.842186 0.861975 3167372.000000 8 | kdskip 3 4500000 5000.000000 1.000000 4.328054 0.972207 3410668.000000 9 | kdskip 3 5000000 5000.000000 1.000000 4.827996 1.080582 3733096.000000 10 | kdskip 3 5500000 5000.000000 1.000000 5.227996 1.180582 3933096.000000 11 | kdskip 3 6000000 5000.000000 1.000000 5.833935 1.293358 4400688.000000 12 | kdskip 3 6500000 5000.000000 1.000000 6.417843 1.405188 4681160.000000 13 | kdskip 3 7000000 5000.000000 1.000000 6.900629 1.575579 5017512.000000 14 | kdskip 3 7500000 5000.000000 1.000000 7.445980 1.637557 5326772.000000 15 | kdskip 3 8000000 5000.000000 1.000000 8.106903 1.748545 5650964.000000 16 | kdskip 3 8500000 5000.000000 1.000000 8.602430 1.911174 5955332.000000 17 | kdskip 3 9000000 5000.000000 1.000000 9.126322 2.012947 6212988.000000 18 | kdskip 3 9500000 5000.000000 1.000000 9.687495 2.161250 6542368.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_3d/kdskip_r10.csv: -------------------------------------------------------------------------------- 1 | kdskip 3 1000000 5000.000000 10.000000 0.644240 0.183188 666248.000000 2 | kdskip 3 1500000 5000.000000 10.000000 0.941575 0.264472 883840.000000 3 | kdskip 3 2000000 5000.000000 10.000000 1.280701 0.354211 1071956.000000 4 | kdskip 3 2500000 5000.000000 10.000000 1.665522 0.426708 1236388.000000 5 | kdskip 3 3000000 5000.000000 10.000000 1.929029 0.530189 1425676.000000 6 | kdskip 3 3500000 5000.000000 10.000000 2.249303 0.602678 1597568.000000 7 | kdskip 3 4000000 5000.000000 10.000000 2.572607 0.677135 1782020.000000 8 | kdskip 3 4500000 5000.000000 10.000000 2.969957 0.766479 1879960.000000 9 | kdskip 3 5000000 5000.000000 10.000000 3.266602 0.865146 2033924.000000 10 | kdskip 3 5500000 5000.000000 10.000000 3.579220 0.926121 2233064.000000 11 | kdskip 3 6000000 5000.000000 10.000000 3.906046 1.027036 2382956.000000 12 | kdskip 3 6500000 5000.000000 10.000000 4.235451 1.101316 2532164.000000 13 | kdskip 3 7000000 5000.000000 10.000000 4.716498 1.167795 2673076.000000 14 | kdskip 3 7500000 5000.000000 10.000000 4.953907 1.275503 2798720.000000 15 | kdskip 3 8000000 5000.000000 10.000000 5.395500 1.334778 3020588.000000 16 | kdskip 3 8500000 5000.000000 10.000000 5.594637 1.381279 3165584.000000 17 | kdskip 3 9000000 5000.000000 10.000000 6.119115 1.524743 3312784.000000 18 | kdskip 3 9500000 5000.000000 10.000000 6.302468 1.677834 3449856.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_3d/kdskip_r5.csv: -------------------------------------------------------------------------------- 1 | kdskip 3 1000000 5000.000000 5.000000 0.712404 0.195313 819528.000000 2 | kdskip 3 1500000 5000.000000 5.000000 1.084312 0.298743 1096376.000000 3 | kdskip 3 2000000 5000.000000 5.000000 1.441557 0.400683 1283716.000000 4 | kdskip 3 2500000 5000.000000 5.000000 1.750100 0.460958 1496384.000000 5 | kdskip 3 3000000 5000.000000 5.000000 2.101258 0.552551 1713960.000000 6 | kdskip 3 3500000 5000.000000 5.000000 2.498248 0.626122 1893536.000000 7 | kdskip 3 4000000 5000.000000 5.000000 2.974212 0.749216 2084336.000000 8 | kdskip 3 4500000 5000.000000 5.000000 3.352105 0.811671 2180264.000000 9 | kdskip 3 5000000 5000.000000 5.000000 3.619260 0.912979 2362256.000000 10 | kdskip 3 5500000 5000.000000 5.000000 4.031062 1.001055 2545928.000000 11 | kdskip 3 6000000 5000.000000 5.000000 4.324799 1.098252 2666240.000000 12 | kdskip 3 6500000 5000.000000 5.000000 4.619308 1.204433 2847788.000000 13 | kdskip 3 7000000 5000.000000 5.000000 5.108564 1.310509 3023528.000000 14 | kdskip 3 7500000 5000.000000 5.000000 5.344661 1.349272 3170708.000000 15 | kdskip 3 8000000 5000.000000 5.000000 5.646024 1.468644 3298796.000000 16 | kdskip 3 8500000 5000.000000 5.000000 6.143752 1.519247 3520536.000000 17 | kdskip 3 9000000 5000.000000 5.000000 6.440595 1.650995 3663900.000000 18 | kdskip 3 9500000 5000.000000 5.000000 7.105001 1.757049 3826516.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_3d/kdtree.csv: -------------------------------------------------------------------------------- 1 | flann_kdtree 3 1000000 5000.000000 10.000000 0.642494 0.204250 449920.000000 2 | flann_kdtree 3 1500000 5000.000000 10.000000 1.033402 0.336263 593456.000000 3 | flann_kdtree 3 2000000 5000.000000 10.000000 1.503326 0.484609 735964.000000 4 | flann_kdtree 3 2500000 5000.000000 10.000000 1.782842 0.576000 875812.000000 5 | flann_kdtree 3 3000000 5000.000000 10.000000 2.255901 0.725650 1017044.000000 6 | flann_kdtree 3 3500000 5000.000000 10.000000 2.773111 0.877424 1158928.000000 7 | flann_kdtree 3 4000000 5000.000000 10.000000 3.322972 1.005480 1298624.000000 8 | flann_kdtree 3 4500000 5000.000000 10.000000 3.535072 1.121170 1440668.000000 9 | flann_kdtree 3 5000000 5000.000000 10.000000 4.056507 1.299342 1582872.000000 10 | flann_kdtree 3 5500000 5000.000000 10.000000 4.663234 1.451825 1722588.000000 11 | flann_kdtree 3 6000000 5000.000000 10.000000 5.285430 1.572314 1864396.000000 12 | flann_kdtree 3 6500000 5000.000000 10.000000 5.793652 1.785578 2005236.000000 13 | flann_kdtree 3 7000000 5000.000000 10.000000 6.302779 1.821034 2143720.000000 14 | flann_kdtree 3 7500000 5000.000000 10.000000 6.884936 2.107053 2288116.000000 15 | flann_kdtree 3 8000000 5000.000000 10.000000 7.382275 2.266811 2429360.000000 16 | flann_kdtree 3 8500000 5000.000000 10.000000 7.321527 2.343101 2569704.000000 17 | flann_kdtree 3 9000000 5000.000000 10.000000 7.933009 2.492354 2710308.000000 18 | flann_kdtree 3 9500000 5000.000000 10.000000 8.579343 2.592515 2849964.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_5d/kdskip_r10.csv: -------------------------------------------------------------------------------- 1 | kdskip 5 1000000 5000.000000 10.000000 1.082161 0.242113 1413864.000000 2 | kdskip 5 1500000 5000.000000 10.000000 1.382161 0.242113 1413864.000000 3 | kdskip 5 2000000 5000.000000 10.000000 2.103406 0.480468 2565480.000000 4 | kdskip 5 2500000 5000.000000 10.000000 2.603923 0.592346 3128328.000000 5 | kdskip 5 3000000 5000.000000 10.000000 3.078191 0.699455 3693296.000000 6 | kdskip 5 3500000 5000.000000 10.000000 3.606175 0.812973 4249120.000000 7 | kdskip 5 4000000 5000.000000 10.000000 4.143444 0.931216 4824664.000000 8 | kdskip 5 4500000 5000.000000 10.000000 4.596879 1.039901 5261816.000000 9 | kdskip 5 5000000 5000.000000 10.000000 5.105001 1.130851 5792700.000000 10 | kdskip 5 5500000 5000.000000 10.000000 5.611607 1.240321 6355552.000000 11 | kdskip 5 6000000 5000.000000 10.000000 6.173194 1.353181 6906396.000000 12 | kdskip 5 6500000 5000.000000 10.000000 6.662795 1.466827 7420856.000000 13 | kdskip 5 7000000 5000.000000 10.000000 7.155359 1.574538 7970816.000000 14 | kdskip 5 7500000 5000.000000 10.000000 7.622937 1.707257 8520076.000000 15 | kdskip 5 8000000 5000.000000 10.000000 8.082560 1.819948 9028824.000000 16 | kdskip 5 8500000 5000.000000 10.000000 8.082560 1.819948 9028824.000000 17 | kdskip 5 9000000 5000.000000 10.000000 9.311059 2.065423 10109948.000000 18 | kdskip 5 9500000 5000.000000 10.000000 9.754819 2.163505 10662744.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_5d/kdskip_r25.csv: -------------------------------------------------------------------------------- 1 | kdskip 5 1000000 5000.000000 25.000000 1.019368 0.226223 1308316.000000 2 | kdskip 5 1500000 5000.000000 25.000000 1.492909 0.328665 1868024.000000 3 | kdskip 5 2000000 5000.000000 25.000000 1.983522 0.451544 2379328.000000 4 | kdskip 5 2500000 5000.000000 25.000000 2.445511 0.562688 2925288.000000 5 | kdskip 5 3000000 5000.000000 25.000000 2.930460 0.682281 3438624.000000 6 | kdskip 5 3500000 5000.000000 25.000000 3.396834 0.767365 3925224.000000 7 | kdskip 5 4000000 5000.000000 25.000000 3.867604 0.888346 4429848.000000 8 | kdskip 5 4500000 5000.000000 25.000000 4.287047 0.982466 4806652.000000 9 | kdskip 5 5500000 5000.000000 25.000000 5.201368 1.207174 5707220.000000 10 | kdskip 5 5500000 5000.000000 25.000000 5.201368 1.207174 5707220.000000 11 | kdskip 5 6000000 5000.000000 25.000000 5.660080 1.296813 6157944.000000 12 | kdskip 5 6500000 5000.000000 25.000000 6.105823 1.406147 6635108.000000 13 | kdskip 5 7000000 5000.000000 25.000000 6.105823 1.406147 6635108.000000 14 | kdskip 5 7500000 5000.000000 25.000000 6.948942 1.593570 7484020.000000 15 | kdskip 5 8000000 5000.000000 25.000000 7.461763 1.733698 7879124.000000 16 | kdskip 5 8500000 5000.000000 25.000000 7.849881 1.848185 8316948.000000 17 | kdskip 5 9000000 5000.000000 25.000000 8.201984 1.944721 8790700.000000 18 | kdskip 5 9500000 5000.000000 25.000000 8.659590 2.094784 9177316.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_5d/kdskip_r32.csv: -------------------------------------------------------------------------------- 1 | kdskip 5 1000000 5000.000000 32.000000 0.993209 0.223567 1276636.000000 2 | kdskip 5 1500000 5000.000000 32.000000 1.471200 0.325596 1824732.000000 3 | kdskip 5 2000000 5000.000000 32.000000 1.978820 0.508869 2309292.000000 4 | kdskip 5 2500000 5000.000000 32.000000 2.654605 0.642672 2784320.000000 5 | kdskip 5 3000000 5000.000000 32.000000 2.930228 0.637649 3280288.000000 6 | kdskip 5 3500000 5000.000000 32.000000 3.245092 0.780027 3716396.000000 7 | kdskip 5 4000000 5000.000000 32.000000 3.699430 0.862086 4144148.000000 8 | kdskip 5 4500000 5000.000000 32.000000 4.136155 0.958633 4465472.000000 9 | kdskip 5 5000000 5000.000000 32.000000 4.537896 1.082811 4890948.000000 10 | kdskip 5 5500000 5000.000000 32.000000 4.902130 1.171746 5300256.000000 11 | kdskip 5 6000000 5000.000000 32.000000 5.424516 1.270328 5673624.000000 12 | kdskip 5 6500000 5000.000000 32.000000 5.835254 1.349033 6084372.000000 13 | kdskip 5 7000000 5000.000000 32.000000 6.236539 1.464556 6442912.000000 14 | kdskip 5 7500000 5000.000000 32.000000 6.615577 1.555155 6815020.000000 15 | kdskip 5 8000000 5000.000000 32.000000 6.983298 1.661390 7226152.000000 16 | kdskip 5 8500000 5000.000000 32.000000 7.373869 1.741971 7618112.000000 17 | kdskip 5 9000000 5000.000000 32.000000 7.758641 1.883333 7965808.000000 18 | kdskip 5 9500000 5000.000000 32.000000 8.208988 2.008831 8344048.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_points_5d/kdtree.csv: -------------------------------------------------------------------------------- 1 | flann_kdtree 5 1000000 5000.000000 10.000000 0.835496 0.167531 452812.000000 2 | flann_kdtree 5 1500000 5000.000000 10.000000 1.305427 0.253833 601536.000000 3 | flann_kdtree 5 2000000 5000.000000 10.000000 1.776717 0.345364 741984.000000 4 | flann_kdtree 5 2500000 5000.000000 10.000000 2.250851 0.432557 887272.000000 5 | flann_kdtree 5 3000000 5000.000000 10.000000 2.749457 0.530664 1030004.000000 6 | flann_kdtree 5 3500000 5000.000000 10.000000 3.260509 0.633973 1173592.000000 7 | flann_kdtree 5 4000000 5000.000000 10.000000 3.766719 0.756192 1317832.000000 8 | flann_kdtree 5 4500000 5000.000000 10.000000 4.298073 0.828301 1461492.000000 9 | flann_kdtree 5 5000000 5000.000000 10.000000 4.821063 0.948965 1607100.000000 10 | flann_kdtree 5 5500000 5000.000000 10.000000 5.354093 1.012151 1747364.000000 11 | flann_kdtree 5 6000000 5000.000000 10.000000 5.866918 1.133715 1892640.000000 12 | flann_kdtree 5 6500000 5000.000000 10.000000 6.422771 1.215078 2036512.000000 13 | flann_kdtree 5 7000000 5000.000000 10.000000 6.968018 1.414152 2181124.000000 14 | flann_kdtree 5 7500000 5000.000000 10.000000 7.481058 1.458921 2323892.000000 15 | flann_kdtree 5 8000000 5000.000000 10.000000 8.040413 1.535670 2468292.000000 16 | flann_kdtree 5 8500000 5000.000000 10.000000 8.582489 1.641569 2610936.000000 17 | flann_kdtree 5 9000000 5000.000000 10.000000 9.131442 1.769813 2754832.000000 18 | flann_kdtree 5 9500000 5000.000000 10.000000 9.657458 1.871380 2898104.000000 -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_test.m: -------------------------------------------------------------------------------- 1 | 2 | 3 | kdtree = importfile('bench_points/kdtree.csv'); 4 | kdskip_1 = importfile('bench_points/kdskip_r1.csv'); 5 | kdskip_5 = importfile('bench_points/kdskip_r5.csv'); 6 | kdskip_10 = importfile('bench_points/kdskip_r10.csv'); 7 | kdskip_20 = importfile('bench_points/kdskip_r20.csv'); 8 | 9 | 10 | hold on; 11 | c = 7; 12 | plot(kdtree(:,c)); 13 | plot(kdskip_1(:,c)); 14 | plot(kdskip_5(:,c)); 15 | plot(kdskip_10(:,c)); 16 | plot(kdskip_20(:,c)); 17 | legend('kdtree','kdskip r1','kdskip r5','kdskip r10','kdskip r20'); -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/skigrid_omp.csv: -------------------------------------------------------------------------------- 1 | skigrid 1000 20000.000000 0.000000 0.011538 0.000041 1189992.000000 2 | skigrid 10000 20000.000000 0.000000 0.021136 0.000364 1197204.000000 3 | skigrid 20000 20000.000000 0.000000 0.030117 0.000896 1201880.000000 4 | skigrid 50000 20000.000000 0.000000 0.063181 0.002352 1214112.000000 5 | skigrid 100000 20000.000000 0.000000 0.106405 0.004357 1235196.000000 6 | skigrid 150000 20000.000000 0.000000 0.157110 0.005972 1246076.000000 7 | skigrid 200000 20000.000000 0.000000 0.208292 0.007826 1261352.000000 8 | skigrid 300000 20000.000000 0.000000 0.319287 0.011021 1291248.000000 9 | skigrid 500000 20000.000000 0.000000 0.532085 0.017489 1351308.000000 10 | skigrid 1000000 20000.000000 0.000000 1.063082 0.032430 1503176.000000 11 | skigrid 1500000 20000.000000 0.000000 1.628623 0.049630 1660568.000000 12 | skigrid 2000000 20000.000000 0.000000 2.191867 0.063446 1808496.000000 13 | skigrid 3000000 20000.000000 0.000000 3.268287 0.103148 2122808.000000 14 | skigrid 3500000 20000.000000 0.000000 3.929934 0.115048 2282760.000000 15 | skigrid 5000000 20000.000000 0.000000 5.619936 0.181303 2735304.000000 16 | skigrid 10000000 20000.000000 0.000000 11.480747 0.385266 4227296.000000 17 | skigrid 20000000 20000.000000 0.000000 24.298524 0.855037 7101572.000000 18 | -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/skigrid_single.csv: -------------------------------------------------------------------------------- 1 | skigrid 1000 20000.000000 0.000000 0.000485 0.010502 1190332.000000 2 | skigrid 10000 20000.000000 0.000000 0.005496 0.010771 1195868.000000 3 | skigrid 20000 20000.000000 0.000000 0.011367 0.011342 1201376.000000 4 | skigrid 50000 20000.000000 0.000000 0.033307 0.002771 1212544.000000 5 | skigrid 100000 20000.000000 0.000000 0.074049 0.004635 1228368.000000 6 | skigrid 150000 20000.000000 0.000000 0.119242 0.006751 1245736.000000 7 | skigrid 200000 20000.000000 0.000000 0.163327 0.009036 1259316.000000 8 | skigrid 300000 20000.000000 0.000000 0.277160 0.021380 1289952.000000 9 | skigrid 500000 20000.000000 0.000000 0.498934 0.027800 1350480.000000 10 | skigrid 1000000 20000.000000 0.000000 1.228157 0.042421 1506032.000000 11 | skigrid 1500000 20000.000000 0.000000 1.911463 0.049432 1665640.000000 12 | skigrid 2000000 20000.000000 0.000000 2.802714 0.062480 1809340.000000 13 | skigrid 3000000 20000.000000 0.000000 4.392162 0.103948 2136248.000000 14 | skigrid 3500000 20000.000000 0.000000 5.315917 0.117934 2281736.000000 15 | skigrid 5000000 20000.000000 0.000000 8.146614 0.164918 2721200.000000 16 | skigrid 10000000 20000.000000 0.000000 19.486182 0.371671 4225272.000000 17 | skigrid 20000000 20000.000000 0.000000 45.678550 0.828911 7103180.000000 18 | -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/plots/plot_testing.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import glob 4 | import os 5 | import csv 6 | from plotutils import Bench 7 | 8 | folder = "/home/daniele/work/ros/skimap_ws/src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/matlab/bench_octree_points" 9 | 10 | files = glob.glob(os.path.join(folder, "*.csv")) 11 | 12 | name_composition = "" # "0_str:1_int:4_int" 13 | columns = "DIMENSIONS:POINTS:MAX_COORDINATES:RESOLUTION:TIME_INTEGRATION:TIME_RADIUS_SEARCH:MEMORY" 14 | 15 | 16 | benchs = [] 17 | for f in files: 18 | bench = Bench(f, name_composition=name_composition, 19 | columns=columns, start_column=1) 20 | benchs.append(bench) 21 | print bench.name 22 | 23 | benchs.sort(key=lambda x: x.name) 24 | 25 | x_data = benchs[0].getDataByName('POINTS') 26 | 27 | for i in range(0, len(benchs)): 28 | y_data = benchs[i].getDataByName('TIME_RADIUS_SEARCH') 29 | plt.plot(x_data, y_data, label=benchs[i].name) 30 | plt.legend() 31 | plt.show() 32 | -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/experiments/skimap_grid_test_python/plots/plotutils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | 4 | 5 | class Bench(object): 6 | NAME_SEPARATOR = '_' 7 | 8 | def __init__(self, filename, name_composition='0_str', columns=[], start_column=0): 9 | self.filename = filename 10 | self.raw_data = np.genfromtxt(filename, dtype=object, delimiter=' ') 11 | self.data = np.array(self.raw_data[:, start_column:], dtype=float) 12 | 13 | col_counter = 0 14 | self.columns = {} 15 | for c in columns.split(":"): 16 | self.columns[c] = col_counter 17 | col_counter += 1 18 | 19 | raw_data_row = self.raw_data[0, :] 20 | self.name = "" 21 | if name_composition == None or len(name_composition) == 0: 22 | self.name = os.path.basename(filename).split(".")[0] 23 | else: 24 | name_chunks = name_composition.split(":") 25 | for i in range(0, len(name_chunks)): 26 | chunk = name_chunks[i] 27 | index = int(chunk.split("_")[0]) 28 | type = str(chunk.split("_")[1]) 29 | self.name += self._buildNameChunk(raw_data_row, index, type) 30 | if i != len(name_chunks) - 1: 31 | self.name += Bench.NAME_SEPARATOR 32 | 33 | def getDataByName(self, column_name): 34 | index = self.columns[column_name] 35 | return self.data[:, index] 36 | 37 | def _buildNameChunk(self, raw_data, index, type): 38 | print "BUILD CHUNK", index, type, raw_data[index] 39 | if type == 'float': 40 | return "{:.2f}".format(float(raw_data[index])) 41 | if type == 'int': 42 | return "{:d}".format(int(float(raw_data[index]))) 43 | if type == 'str': 44 | return "{}".format(str(raw_data[index])) 45 | -------------------------------------------------------------------------------- /src/skimap_ros/src/nodes/test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | //ROS 15 | #include 16 | 17 | #include 18 | 19 | ros::NodeHandle *nh; 20 | 21 | /** MAIN NODE **/ 22 | int main(int argc, char **argv) 23 | { 24 | 25 | // Initialize ROS 26 | ros::init(argc, argv, "slam_test"); 27 | nh = new ros::NodeHandle("~"); 28 | 29 | // Spin 30 | while (nh->ok()) 31 | { 32 | 33 | ros::spinOnce(); 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /src/skimap_ros/src/skimap_ros/skimap_ros_void_library.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | //ROS 15 | #include 16 | 17 | ros::NodeHandle *nh; 18 | 19 | /** MAIN NODE **/ 20 | int main(int argc, char **argv) 21 | { 22 | 23 | // Initialize ROS 24 | ros::init(argc, argv, "skimap_ros_void_file"); 25 | nh = new ros::NodeHandle("~"); 26 | 27 | // Spin 28 | while (nh->ok()) 29 | { 30 | 31 | ros::spinOnce(); 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /src/skimap_ros/src/slamdunk/graph_utils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #include "slamdunk/graph_utils.h" 10 | #include "slamdunk/graph_backend.h" 11 | #include 12 | 13 | std::string slamdunk::graphToDot(const GraphBackend *gb, int root_id, 14 | const g2o::HyperGraph::VertexSet &colored, 15 | const g2o::HyperGraph::VertexSet &highlighted, 16 | const std::string &graph_name) 17 | { 18 | double buf[7]; 19 | std::ostringstream sstream; 20 | sstream << "digraph " << graph_name << " { root=" << root_id << ";"; 21 | 22 | for (g2o::HyperGraph::VertexSet::const_iterator vit = colored.begin(); vit != colored.end(); ++vit) 23 | { 24 | static_cast(*vit)->getEstimateData(buf); 25 | sstream << " " << (*vit)->id() << " [style=filled,fillcolor=" << ((*vit)->id() == root_id ? "yellow" : "orange") 26 | //<< ",pos=\"" << (int)(buf[0]*1000) << "," << (int)(buf[2]*1000) << "!\"];"; 27 | << "];"; 28 | } 29 | 30 | for (g2o::HyperGraph::VertexSet::const_iterator vit = highlighted.begin(); vit != highlighted.end(); ++vit) 31 | if (!colored.count(*vit)) 32 | { 33 | static_cast(*vit)->getEstimateData(buf); 34 | sstream << " " << (*vit)->id() << " [style=filled,fillcolor=" << ((*vit)->id() == root_id ? "yellow" : "lightblue") 35 | //<< ",pos=\"" << (int)(buf[0]*1000) << "," << (int)(buf[2]*1000) << "!\"];"; 36 | << "];"; 37 | } 38 | 39 | std::set> edges; 40 | const g2o::SparseOptimizer::EdgeSet &eset = gb->getGraph().edges(); 41 | for (g2o::SparseOptimizer::EdgeSet::const_iterator eit = eset.begin(); eit != eset.end(); ++eit) 42 | if (edges.insert(std::make_pair((*eit)->vertices()[0]->id(), (*eit)->vertices()[1]->id())).second) 43 | sstream << " " << (*eit)->vertices()[0]->id() << " -> " << (*eit)->vertices()[1]->id() << ";"; 44 | 45 | sstream << " }" << std::endl; 46 | return sstream.str(); 47 | } 48 | -------------------------------------------------------------------------------- /src/skimap_ros/src/slamdunk/internal_timers.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 daniele de gregorio, University of Bologna - All Rights Reserved 3 | * You may use, distribute and modify this code under the 4 | * terms of the GNU GPLv3 license. 5 | * 6 | * please write to: d.degregorio@unibo.it 7 | */ 8 | 9 | #ifndef SLAM_DUNK_INTERNAL_TIMERS_HPP 10 | #define SLAM_DUNK_INTERNAL_TIMERS_HPP 11 | 12 | #ifdef SLAMDUNK_TIMERS_ENABLED 13 | #include 14 | #include 15 | #include 16 | namespace 17 | { 18 | static std::map> g_timings_; 19 | static boost::posix_time::ptime g_base_time_; 20 | class slam_dunk_cpu_timer : public boost::timer::cpu_timer 21 | { 22 | public: 23 | explicit slam_dunk_cpu_timer(const std::string &title) 24 | : title_(title) 25 | { 26 | start(); 27 | } 28 | ~slam_dunk_cpu_timer() 29 | { 30 | stop(); 31 | std::map>::iterator tIt = 32 | g_timings_.insert(std::make_pair(title_, std::pair(0, 0))).first; 33 | tIt->second.first += 1; 34 | tIt->second.second += elapsed().wall; 35 | 36 | // Print average timings every N seconds 37 | boost::posix_time::ptime current_time = boost::posix_time::second_clock::local_time(); 38 | if (g_base_time_.is_not_a_date_time()) 39 | g_base_time_ = current_time; 40 | if ((current_time - g_base_time_).total_seconds() >= 5) 41 | { 42 | std::ostringstream outstr; 43 | outstr << "[SLAM DUNK TIMERS]\n"; 44 | for (tIt = g_timings_.begin(); tIt != g_timings_.end(); ++tIt) 45 | { 46 | outstr << ("- " + tIt->first + ": average speed (msec) ") << ((double(tIt->second.second) * 1e-6) / double(tIt->second.first)) << std::endl; 47 | tIt->second.first = 0; 48 | tIt->second.second = 0; 49 | } 50 | std::cout << outstr.str(); 51 | g_base_time_ = current_time; 52 | } 53 | } 54 | 55 | private: 56 | const std::string title_; 57 | }; 58 | } 59 | #define SLAM_DUNK_AUTO_CPU_TIMER(title) slam_dunk_cpu_timer scoped_timer(title) 60 | #else // SLAMDUNK_TIMERS_ENABLED 61 | #define SLAM_DUNK_AUTO_CPU_TIMER(title) 62 | #endif // SLAMDUNK_TIMERS_ENABLED 63 | 64 | #endif // SLAM_DUNK_INTERNAL_TIMERS_HPP 65 | -------------------------------------------------------------------------------- /src/skimap_ros/srv/SkimapIntegrationService.srv: -------------------------------------------------------------------------------- 1 | string map_name 2 | geometry_msgs/Pose sensor_pose 3 | geometry_msgs/Point[] points 4 | std_msgs/ColorRGBA[] colors 5 | float32[] weights 6 | int8 action 7 | 8 | int8 ACTION_INTEGRATE = 0 9 | 10 | --- 11 | int32 integrated_points 12 | int8 status 13 | 14 | 15 | int8 STATUS_OK=0 16 | int8 STATUS_FAIL=10 --------------------------------------------------------------------------------