├── .gitignore ├── CMakeLists.txt ├── Doxyfile.in ├── LICENSE ├── README.asciidoc ├── manifest.xml ├── src ├── CMakeLists.txt ├── LocalMap.hpp ├── geometric │ ├── ContourMap.hpp │ ├── GeometricMap.hpp │ ├── LineSegment.hpp │ └── Point.hpp ├── grid │ ├── AccessIterator.hpp │ ├── DiscreteTree.hpp │ ├── ElevationMap.cpp │ ├── ElevationMap.hpp │ ├── GridAccessInterface.hpp │ ├── GridFacade.hpp │ ├── GridMap.hpp │ ├── Index.hpp │ ├── LayeredGridMap.hpp │ ├── LevelList.hpp │ ├── MLSConfig.hpp │ ├── MLSMap.hpp │ ├── MultiLevelGridMap.hpp │ ├── OccupancyConfiguration.hpp │ ├── OccupancyGridMap.cpp │ ├── OccupancyGridMap.hpp │ ├── OccupancyGridMapBase.hpp │ ├── OccupancyPatch.hpp │ ├── SurfacePatches.hpp │ ├── TSDFPatch.hpp │ ├── TSDFVolumetricMap.cpp │ ├── TSDFVolumetricMap.hpp │ ├── TraversabilityCell.cpp │ ├── TraversabilityCell.hpp │ ├── TraversabilityClass.cpp │ ├── TraversabilityClass.hpp │ ├── TraversabilityGrid.cpp │ ├── TraversabilityGrid.hpp │ ├── TraversabilityMap3d.cpp │ ├── TraversabilityMap3d.hpp │ ├── VectorGrid.hpp │ ├── VectorGridAccess.hpp │ └── VoxelGridMap.hpp ├── maps.pc.in ├── operations │ ├── CoverageMapGeneration.cpp │ ├── CoverageMapGeneration.hpp │ └── GridInterpolation.hpp └── tools │ ├── BresenhamLine.cpp │ ├── BresenhamLine.hpp │ ├── MLSToSlopes.cpp │ ├── MLSToSlopes.hpp │ ├── MarchingCubes.hpp │ ├── Overlap.hpp │ ├── SimpleTraversability.cpp │ ├── SimpleTraversability.hpp │ ├── SimpleTraversabilityRadialLUT.cpp │ ├── SimpleTraversabilityRadialLUT.hpp │ ├── SurfaceIntersection.hpp │ ├── TSDFPolygonMeshReconstruction.cpp │ ├── TSDFPolygonMeshReconstruction.hpp │ ├── TSDFSurfaceReconstruction.hpp │ ├── TSDF_MLSMapReconstruction.cpp │ ├── TSDF_MLSMapReconstruction.hpp │ ├── TraversabilityGrassFireSearchItem.hpp │ ├── TraversabilityGrassfire.cpp │ ├── TraversabilityGrassfire.hpp │ ├── TraversabilityGrassfireConfig.hpp │ ├── VoxelTraversal.cpp │ └── VoxelTraversal.hpp ├── test ├── CMakeLists.txt ├── common │ └── GenerateMLS.hpp ├── geometric │ ├── CMakeLists.txt │ ├── test_ContourMap.cpp │ ├── test_GeometricMap.cpp │ ├── test_LineSegment.cpp │ ├── test_Point.cpp │ └── test_closest_MLS.cpp ├── grid │ ├── CMakeLists.txt │ ├── test_GridMap.cpp │ ├── test_Index.cpp │ ├── test_LayeredGridMap.cpp │ ├── test_LocalMap.cpp │ ├── test_MLGrid.cpp │ ├── test_TravMap3d.cpp │ ├── test_TraversabilityGrid.cpp │ └── test_VectorGrid.cpp ├── serialization │ ├── CMakeLists.txt │ ├── DiscreteTree_v0.bin │ ├── TravMap_v00.bin │ ├── test_serialization_DiscreteTree.cpp │ ├── test_serialization_Geometric.cpp │ ├── test_serialization_Grid.cpp │ ├── test_serialization_LocalMap.cpp │ ├── test_serialization_MLS.cpp │ └── test_serialization_TravMap.cpp ├── suite.cpp ├── tools │ ├── CMakeLists.txt │ ├── GeneratePointclouds.hpp │ ├── test_surface_intersection.cpp │ ├── test_tools_GeneratePoints.cpp │ ├── test_tools_MLSToSlopes.cpp │ ├── test_tools_SimpleTraversability.cpp │ ├── test_tools_TraversabilityGrassfire.cpp │ └── test_voxel_traversal.cpp └── viz │ ├── CMakeLists.txt │ ├── DeserializeVizTest.cpp │ ├── mlsFromImageHeightmap.cpp │ ├── test_viz_ContourMap.cpp │ ├── test_viz_Coverage.cpp │ ├── test_viz_ElevationMap.cpp │ ├── test_viz_GridMap.cpp │ ├── test_viz_MLSLIDAR.cpp │ ├── test_viz_MLSMap.cpp │ ├── test_viz_OccupancyLidar.cpp │ ├── test_viz_TSDFMap.cpp │ ├── test_viz_TraversabilityGrid.cpp │ └── test_viz_TraversabilityMap3d.cpp └── viz ├── CMakeLists.txt ├── ColorGradient.hpp ├── ContourMapVisualization.cpp ├── ContourMapVisualization.hpp ├── ElevationMapVisualization.cpp ├── ElevationMapVisualization.hpp ├── ExtentsRectangle.hpp ├── GridMapVisualization.cpp ├── GridMapVisualization.hpp ├── MLSMapVisualization.cpp ├── MLSMapVisualization.hpp ├── MapVisualization.hpp ├── OccupancyGridMapVisualization.cpp ├── OccupancyGridMapVisualization.hpp ├── PatchesGeode.cpp ├── PatchesGeode.hpp ├── PluginLoader.cpp ├── StandaloneVisualizer.cpp ├── StandaloneVisualizer.hpp ├── SurfaceGeode.cpp ├── SurfaceGeode.hpp ├── TraversabilityGridVisualization.cpp ├── TraversabilityGridVisualization.hpp ├── TraversabilityMap3dVisualization.cpp ├── TraversabilityMap3dVisualization.hpp ├── maps-viz.pc.in └── vizkit_plugin.rb /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | .cproject 3 | .project 4 | .settings/ 5 | MLSMap*.bin 6 | test/test*.boost.xml -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # CMakeLists.txt has to be located in the project folder and cmake has to be 2 | # executed from 'project/build' with 'cmake ../'. 3 | cmake_minimum_required(VERSION 3.0) 4 | project(maps VERSION 0.1) 5 | set(CMAKE_CXX_STANDARD 14) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | find_package(Rock) 8 | 9 | if(COVERAGE) 10 | if(CMAKE_BUILD_TYPE MATCHES Debug) 11 | add_definitions(-fprofile-arcs -ftest-coverage) 12 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fprofile-arcs -ftest-coverage -lgcov") 13 | set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -fprofile-arcs -ftest-coverage -lgcov") 14 | set(CMAKE_MODULE_LINKER_FLAGS "${CMAKE_MODULE_LINKER_FLAGS} -fprofile-arcs -ftest-coverage -lgcov") 15 | 16 | add_custom_target(coverage 17 | COMMAND lcov --directory . --capture --output-file cov.info 18 | COMMAND lcov --remove cov.info 'tests/*' '/usr/*' '*/install/include/*' --output-file cov.info.cleaned 19 | COMMAND genhtml -o ./cov cov.info.cleaned 20 | COMMAND cmake -E remove cov.info cov.info.cleaned 21 | WORKING_DIRECTORY ${CMAKE_BINARY_DIR} 22 | ) 23 | else() 24 | message(FATAL_ERROR "Code coverage only works in Debug versions" ) 25 | endif() 26 | endif() 27 | 28 | if(CMAKE_BUILD_TYPE MATCHES Debug) 29 | add_definitions(-O1) # without any optimization, code is extreamly slow 30 | endif() 31 | 32 | add_definitions(-DNUMERIC_DEPRECATE=1 ) 33 | 34 | rock_init() 35 | rock_standard_layout() 36 | 37 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 4 | Copyright (c) 2015-2017, University of Bremen 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | 10 | * Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | * Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | 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 | -------------------------------------------------------------------------------- /manifest.xml: -------------------------------------------------------------------------------- 1 | 22 | 23 | 24 | 25 | Standard Maps library following the IEEE 1873 standard for mapa data representation with extensions (i.e: MLS, 3D, Operators, etc..) 26 | 27 | Autonomy Team/ric-team8@dfki.de 28 | Anna Born/anna.born@dfki.de 29 | Sascha Arnold/sascha.arnold@dfki.de 30 | BSD 31 | http://rock-robotics.org/ 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | Mapping 47 | Representation 48 | Environment 49 | 50 | 51 | 1 52 | multi project 53 | active 54 | 55 | needs_opt 56 | 57 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(Boost REQUIRED COMPONENTS system filesystem serialization) 2 | find_package(CGAL REQUIRED COMPONENTS Core) 3 | find_package(PCL 1.7 REQUIRED COMPONENTS io) 4 | 5 | rock_library(maps 6 | SOURCES 7 | grid/ElevationMap.cpp 8 | grid/TraversabilityMap3d.cpp 9 | grid/OccupancyGridMap.cpp 10 | grid/TraversabilityCell.cpp 11 | grid/TraversabilityClass.cpp 12 | grid/TraversabilityGrid.cpp 13 | grid/TSDFVolumetricMap.cpp 14 | tools/BresenhamLine.cpp 15 | tools/VoxelTraversal.cpp 16 | tools/TSDFPolygonMeshReconstruction.cpp 17 | tools/TSDF_MLSMapReconstruction.cpp 18 | operations/CoverageMapGeneration.cpp 19 | tools/MLSToSlopes.cpp 20 | tools/SimpleTraversability.cpp 21 | tools/SimpleTraversabilityRadialLUT.cpp 22 | tools/TraversabilityGrassfire.cpp 23 | HEADERS 24 | LocalMap.hpp 25 | grid/Index.hpp 26 | grid/GridMap.hpp 27 | grid/LevelList.hpp 28 | grid/LayeredGridMap.hpp 29 | grid/MultiLevelGridMap.hpp 30 | grid/ElevationMap.hpp 31 | grid/SurfacePatches.hpp 32 | grid/MLSConfig.hpp 33 | grid/MLSMap.hpp 34 | grid/TraversabilityMap3d.hpp 35 | grid/AccessIterator.hpp 36 | grid/GridAccessInterface.hpp 37 | grid/GridFacade.hpp 38 | grid/VectorGrid.hpp 39 | grid/VectorGridAccess.hpp 40 | grid/DiscreteTree.hpp 41 | grid/VoxelGridMap.hpp 42 | grid/OccupancyGridMapBase.hpp 43 | grid/OccupancyGridMap.hpp 44 | grid/OccupancyConfiguration.hpp 45 | grid/OccupancyPatch.hpp 46 | grid/TraversabilityCell.hpp 47 | grid/TraversabilityClass.hpp 48 | grid/TraversabilityGrid.hpp 49 | grid/TSDFPatch.hpp 50 | grid/TSDFVolumetricMap.hpp 51 | geometric/Point.hpp 52 | geometric/LineSegment.hpp 53 | geometric/GeometricMap.hpp 54 | geometric/ContourMap.hpp 55 | tools/BresenhamLine.hpp 56 | tools/Overlap.hpp 57 | tools/VoxelTraversal.hpp 58 | tools/TSDFSurfaceReconstruction.hpp 59 | tools/TSDFPolygonMeshReconstruction.hpp 60 | tools/TSDF_MLSMapReconstruction.hpp 61 | tools/MarchingCubes.hpp 62 | tools/SurfaceIntersection.hpp 63 | tools/MLSToSlopes.hpp 64 | tools/SimpleTraversability.hpp 65 | tools/SimpleTraversabilityRadialLUT.hpp 66 | tools/TraversabilityGrassfire.hpp 67 | tools/TraversabilityGrassfireConfig.hpp 68 | tools/TraversabilityGrassFireSearchItem.hpp 69 | operations/GridInterpolation.hpp 70 | operations/CoverageMapGeneration.hpp 71 | DEPS_PKGCONFIG 72 | base-types 73 | base-logging 74 | boost_serialization 75 | pcl_io-${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR} 76 | DEPS 77 | Boost::system 78 | Boost::filesystem 79 | Boost::serialization 80 | ) 81 | 82 | -------------------------------------------------------------------------------- /src/geometric/ContourMap.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #ifndef __MAPS_CONTOURMAP_HPP_ 28 | #define __MAPS_CONTOURMAP_HPP_ 29 | 30 | #pragma once 31 | 32 | /** Maps classes **/ 33 | #include 34 | #include 35 | 36 | namespace maps { namespace geometric 37 | { 38 | typedef GeometricMap ContourMap; 39 | }} 40 | 41 | #endif /* __MAPS_CONTOURMAP_HPP_ */ 42 | -------------------------------------------------------------------------------- /src/geometric/Point.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #ifndef __MAPS_POINT_HPP__ 28 | #define __MAPS_POINT_HPP__ 29 | 30 | /** Boost serialization **/ 31 | #include 32 | 33 | /** Eigen **/ 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | namespace maps { namespace geometric 40 | { 41 | /**@brief Point class IEEE 1873 standard 42 | * adapted to point in D-space. 43 | * T type (e.g. float or double) 44 | * D int specification for the dimensional space 45 | * **/ 46 | template 47 | class Point: public Eigen::Matrix 48 | { 49 | 50 | public: 51 | template 52 | Point(Ts&&... args) : Eigen::Matrix (std::forward(args)...) 53 | { 54 | } 55 | }; 56 | 57 | typedef Point Point2d; 58 | typedef Point Point3d; 59 | typedef Point Point2f; 60 | typedef Point Point3f; 61 | }} 62 | 63 | namespace boost { namespace serialization 64 | { 65 | template 66 | void serialize(Archive & ar, ::maps::geometric::Point & t, const unsigned int version) 67 | { 68 | for(size_t i=0; i Ptr; 42 | static const float ELEVATION_DEFAULT; 43 | public: 44 | ElevationMap(); 45 | ElevationMap(const ElevationMap &elevation_map); 46 | ElevationMap(const GridMapF &grid_map); 47 | ElevationMap(const Vector2ui &num_cells, const Vector2d &resolution); 48 | ElevationMap(const Vector2ui &num_cells, const Vector2d &resolution, const float &default_value); 49 | 50 | ~ElevationMap(); 51 | 52 | Vector3d getNormal(const Index& pos) const; 53 | 54 | /** @brief get the normal vector at the given position 55 | */ 56 | Vector3d getNormal(const Vector3d& pos) const; 57 | 58 | /** @brief get the elevation at the given point 59 | * 60 | * The underlying model assumes the height value to be at 61 | * the center of the cell, and a surface is approximated 62 | * using the getNormal. The Height value is the value of the 63 | * plane at that point. 64 | */ 65 | float getMeanElevation(const Vector3d& pos) const; 66 | 67 | std::pair getElevationRange() const; 68 | }; 69 | }} 70 | 71 | 72 | #endif // __MAPS_ELEVATION_MAP_HPP__ 73 | -------------------------------------------------------------------------------- /src/grid/GridAccessInterface.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #pragma once 28 | 29 | #include "AccessIterator.hpp" 30 | #include "Index.hpp" 31 | 32 | namespace maps { namespace grid 33 | { 34 | template 35 | class GridAccessInterface 36 | { 37 | public: 38 | 39 | typedef AccessIterator iterator; 40 | typedef ConstAccessIterator const_iterator; 41 | 42 | GridAccessInterface() {} 43 | 44 | virtual ~GridAccessInterface() {} 45 | 46 | virtual const CellBaseT &getDefaultValue() const = 0; 47 | 48 | virtual iterator begin() = 0; 49 | 50 | virtual iterator end() = 0; 51 | 52 | virtual const_iterator begin() const = 0; 53 | 54 | virtual const_iterator end() const = 0; 55 | 56 | virtual void resize(Vector2ui new_number_cells) = 0; 57 | 58 | virtual void moveBy(Index idx) = 0; 59 | 60 | virtual const CellBaseT& at(Index idx) const = 0; 61 | 62 | virtual CellBaseT& at(Index idx) = 0; 63 | 64 | virtual const Vector2ui &getNumCells() const = 0; 65 | 66 | virtual void clear() = 0; 67 | }; 68 | }} 69 | -------------------------------------------------------------------------------- /src/grid/GridFacade.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #pragma once 28 | 29 | #include "GridAccessInterface.hpp" 30 | 31 | namespace maps { namespace grid 32 | { 33 | template 34 | class GridFacade 35 | { 36 | GridAccessInterface *impl; 37 | public: 38 | 39 | typedef AccessIterator iterator; 40 | typedef ConstAccessIterator const_iterator; 41 | 42 | GridFacade(GridAccessInterface *impl) : impl(impl) 43 | { 44 | } 45 | 46 | const CellBaseT &getDefaultValue() const 47 | { 48 | return impl->getDefaultValue(); 49 | } 50 | 51 | iterator begin() 52 | { 53 | return impl->begin(); 54 | } 55 | 56 | iterator end() 57 | { 58 | return impl->end(); 59 | } 60 | 61 | const_iterator begin() const 62 | { 63 | return impl->begin(); 64 | } 65 | 66 | const_iterator end() const 67 | { 68 | return impl->end(); 69 | } 70 | 71 | void resize(const Vector2ui &new_number_cells) 72 | { 73 | impl->resize(new_number_cells); 74 | }; 75 | 76 | void moveBy(const Index &idx) 77 | { 78 | impl->moveBy(idx); 79 | } 80 | 81 | const CellBaseT& at(const Index &idx) const 82 | { 83 | return impl->at(idx); 84 | } 85 | 86 | CellBaseT& at(const Index &idx) 87 | { 88 | return impl->at(idx); 89 | } 90 | 91 | const CellBaseT& at(size_t x, size_t y) const 92 | { 93 | return impl->at(Index(x,y)); 94 | } 95 | 96 | CellBaseT& at(size_t x, size_t y) 97 | { 98 | return impl->at(Index(x,y)); 99 | } 100 | 101 | const Index &getNumCells() const 102 | { 103 | return impl->getNumCells(); 104 | }; 105 | 106 | void clear() 107 | { 108 | impl->clear(); 109 | }; 110 | }; 111 | 112 | }} 113 | -------------------------------------------------------------------------------- /src/grid/MLSConfig.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #ifndef __MAPS_MLS_CONFIG_HPP__ 28 | #define __MAPS_MLS_CONFIG_HPP__ 29 | 30 | #include 31 | #include 32 | 33 | namespace maps { namespace grid 34 | { 35 | 36 | /** 37 | * Configuration struct which hold information on the different 38 | * options and parameters of the MLS. 39 | */ 40 | struct MLSConfig 41 | { 42 | MLSConfig() 43 | : gapSize( 1.0 ) 44 | , thickness( 0.05 ) 45 | , useColor( false ) 46 | , updateModel( KALMAN ) 47 | , useNegativeInformation( false ) 48 | {} 49 | 50 | enum update_model 51 | { 52 | KALMAN, 53 | SLOPE 54 | , PRECALCULATED //! Patches with precalculated normal vector (can't be updated) 55 | , BASE 56 | }; 57 | 58 | float gapSize; 59 | float thickness; 60 | bool useColor; 61 | update_model updateModel; 62 | bool useNegativeInformation; 63 | 64 | protected: 65 | /** Grants access to boost serialization */ 66 | friend class boost::serialization::access; 67 | 68 | /** Serializes the members of this class*/ 69 | template 70 | void serialize(Archive &ar, const unsigned int version) 71 | { 72 | ar & BOOST_SERIALIZATION_NVP(gapSize); 73 | ar & BOOST_SERIALIZATION_NVP(thickness); 74 | ar & BOOST_SERIALIZATION_NVP(useColor); 75 | ar & BOOST_SERIALIZATION_NVP(updateModel); 76 | ar & BOOST_SERIALIZATION_NVP(useNegativeInformation); 77 | } 78 | }; 79 | 80 | }} 81 | 82 | #endif // __MAPS_MLS_CONFIG_HPP__ 83 | -------------------------------------------------------------------------------- /src/grid/OccupancyConfiguration.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #pragma once 28 | 29 | #include "OccupancyPatch.hpp" 30 | 31 | #include 32 | #include 33 | 34 | namespace maps { namespace grid 35 | { 36 | 37 | struct OccupancyConfiguration 38 | { 39 | OccupancyConfiguration(double hit_probability = 0.7, double miss_probability = 0.4, 40 | double occupied_probability = 0.8, double free_space_probability = 0.3, 41 | double max_probability = 0.971, double min_probability = 0.1192, 42 | float uncertainty_threshold = 0.25) : 43 | hit_logodds(OccupancyPatch::logodds(hit_probability)), 44 | miss_logodds(OccupancyPatch::logodds(miss_probability)), 45 | occupied_logodds(OccupancyPatch::logodds(occupied_probability)), 46 | free_space_logodds(OccupancyPatch::logodds(free_space_probability)), 47 | max_logodds(OccupancyPatch::logodds(max_probability)) , 48 | min_logodds(OccupancyPatch::logodds(min_probability)), 49 | uncertainty_threshold(uncertainty_threshold) {} 50 | 51 | float hit_logodds; 52 | float miss_logodds; 53 | float occupied_logodds; 54 | float free_space_logodds; 55 | float max_logodds; 56 | float min_logodds; 57 | float uncertainty_threshold; 58 | 59 | protected: 60 | /** Grants access to boost serialization */ 61 | friend class boost::serialization::access; 62 | 63 | /** Serializes the members of this class*/ 64 | template 65 | void serialize(Archive &ar, const unsigned int version) 66 | { 67 | ar & BOOST_SERIALIZATION_NVP(hit_logodds); 68 | ar & BOOST_SERIALIZATION_NVP(miss_logodds); 69 | ar & BOOST_SERIALIZATION_NVP(occupied_logodds); 70 | ar & BOOST_SERIALIZATION_NVP(free_space_logodds); 71 | ar & BOOST_SERIALIZATION_NVP(max_logodds); 72 | ar & BOOST_SERIALIZATION_NVP(min_logodds); 73 | ar & BOOST_SERIALIZATION_NVP(uncertainty_threshold); 74 | } 75 | }; 76 | 77 | }} 78 | -------------------------------------------------------------------------------- /src/grid/OccupancyGridMapBase.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #pragma once 28 | 29 | #include "OccupancyConfiguration.hpp" 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | namespace maps { namespace grid 37 | { 38 | 39 | class OccupancyGridMapBase 40 | { 41 | public: 42 | OccupancyGridMapBase(const OccupancyConfiguration& config) : config(config) {} 43 | virtual ~OccupancyGridMapBase() {} 44 | 45 | virtual void mergePoint(const Eigen::Vector3d& sensor_origin, const Eigen::Vector3d& measurement) = 0; 46 | virtual bool isOccupied(const Eigen::Vector3d& point) const = 0; 47 | virtual bool isOccupied(Index idx, float z) const = 0; 48 | virtual bool isFreeSpace(const Eigen::Vector3d& point) const = 0; 49 | virtual bool isFreeSpace(Index idx, float z) const = 0; 50 | virtual bool hasSameFrame(const base::Transform3d& local_frame, const Vector2ui &num_cells, const Vector2d &resolution) const = 0; 51 | 52 | const OccupancyConfiguration& getConfig() const {return config;} 53 | 54 | protected: 55 | /** Grants access to boost serialization */ 56 | friend class boost::serialization::access; 57 | 58 | /** Serializes the members of this class*/ 59 | template 60 | void serialize(Archive &ar, const unsigned int version) 61 | { 62 | ar & BOOST_SERIALIZATION_NVP(config); 63 | } 64 | 65 | OccupancyConfiguration config; 66 | }; 67 | 68 | }} 69 | 70 | BOOST_SERIALIZATION_ASSUME_ABSTRACT(maps::grid::OccupancyGridMapBase); 71 | -------------------------------------------------------------------------------- /src/grid/OccupancyPatch.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #ifndef __MAPS_OCCUPANCY_PATCH_HPP_ 28 | #define __MAPS_OCCUPANCY_PATCH_HPP_ 29 | 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | 37 | namespace maps { namespace grid 38 | { 39 | 40 | class OccupancyPatch 41 | { 42 | float log_odds; 43 | 44 | public: 45 | OccupancyPatch(double initial_probability) : log_odds(logodds(initial_probability)) {} 46 | OccupancyPatch(float initial_log_odds = 0.f) : log_odds(initial_log_odds) {} 47 | virtual ~OccupancyPatch() {} 48 | 49 | double getPropability() const 50 | { 51 | return probability(log_odds); 52 | } 53 | 54 | float getLogOdds() const 55 | { 56 | return log_odds; 57 | } 58 | 59 | bool isOccupied(double occupied_tresshold = 0.8) const 60 | { 61 | return probability(log_odds) >= occupied_tresshold; 62 | } 63 | 64 | bool isFreeSpace(double not_occupied_tresshold = 0.3) const 65 | { 66 | return probability(log_odds) < not_occupied_tresshold; 67 | } 68 | 69 | void updatePropability(double update_prob, double min_prob = 0.1192, double max_prob = 0.971) 70 | { 71 | updateLogOdds(logodds(update_prob), logodds(min_prob), logodds(max_prob)); 72 | } 73 | 74 | void updateLogOdds(float update_logodds, float min = -2.f, float max = 3.5f) 75 | { 76 | log_odds += update_logodds; 77 | if(log_odds < min) 78 | log_odds = min; 79 | else if(log_odds > max) 80 | log_odds = max; 81 | } 82 | 83 | bool operator==(const OccupancyPatch& other) const 84 | { 85 | return this == &other; 86 | } 87 | 88 | // compute log-odds from probability 89 | static inline float logodds(double probability) 90 | { 91 | return (float)log(probability / (1.0 - probability)); 92 | } 93 | 94 | // compute probability from log-odds 95 | static inline double probability(double logodds) 96 | { 97 | return 1.0 - ( 1.0 / (1.0 + exp(logodds))); 98 | } 99 | 100 | protected: 101 | 102 | /** Grants access to boost serialization */ 103 | friend class boost::serialization::access; 104 | 105 | /** Serializes the members of this class*/ 106 | template 107 | void serialize(Archive &ar, const unsigned int version) 108 | { 109 | ar & BOOST_SERIALIZATION_NVP(log_odds); 110 | } 111 | }; 112 | 113 | } //namespace grid 114 | } //namespace maps 115 | 116 | #endif //__MAPS_OCCUPANCY_PATCH_HPP_ -------------------------------------------------------------------------------- /src/grid/TSDFPatch.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #ifndef __MAPS_TSDF_PATCH_HPP_ 28 | #define __MAPS_TSDF_PATCH_HPP_ 29 | 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | 37 | namespace maps { namespace grid 38 | { 39 | 40 | class TSDFPatch 41 | { 42 | float distance; 43 | float var; 44 | 45 | template static inline void kalman_update( T& mean, T& var, T m_mean, T m_var ) 46 | { 47 | float gain = var / (var + m_var); 48 | if( gain != gain ) 49 | gain = 0.5f; // this happens when both vars are 0. 50 | mean = mean + gain * (m_mean - mean); 51 | var = (1.0f - gain) * var; 52 | } 53 | 54 | public: 55 | TSDFPatch() : distance(base::NaN()), var(1.f) {} 56 | TSDFPatch(float distance, float var) : distance(distance), var(var) {} 57 | virtual ~TSDFPatch() {} 58 | 59 | void update(float distance, float var, float truncation = 1.f, float min_var = 0.001f) 60 | { 61 | if(base::isNaN(this->distance)) 62 | this->distance = distance; 63 | 64 | kalman_update(this->distance, this->var, distance, var); 65 | 66 | if(this->var < min_var) 67 | this->var = min_var; 68 | 69 | if(distance > truncation) 70 | distance = truncation; 71 | else if(distance < -truncation) 72 | distance = -truncation; 73 | } 74 | 75 | float getDistance() const 76 | { 77 | return distance; 78 | } 79 | 80 | float getVariance() const 81 | { 82 | return var; 83 | } 84 | 85 | float getStandardDeviation() const 86 | { 87 | return std::sqrt(var); 88 | } 89 | 90 | bool operator==(const TSDFPatch& other) const 91 | { 92 | return this == &other; 93 | } 94 | 95 | protected: 96 | 97 | /** Grants access to boost serialization */ 98 | friend class boost::serialization::access; 99 | 100 | /** Serializes the members of this class*/ 101 | template 102 | void serialize(Archive &ar, const unsigned int version) 103 | { 104 | ar & BOOST_SERIALIZATION_NVP(distance); 105 | ar & BOOST_SERIALIZATION_NVP(var); 106 | } 107 | }; 108 | 109 | } //namespace grid 110 | } //namespace maps 111 | 112 | #endif //__MAPS_TSDF_PATCH_HPP_ -------------------------------------------------------------------------------- /src/grid/TraversabilityCell.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #include "TraversabilityCell.hpp" 28 | 29 | 30 | using namespace maps::grid; 31 | 32 | TraversabilityCell::TraversabilityCell(): traversabilityClassId(0), probability(0) 33 | { 34 | } 35 | 36 | TraversabilityCell::TraversabilityCell(uint8_t traversabilityClass, uint8_t probability): traversabilityClassId(traversabilityClass), probability(probability) 37 | { 38 | } 39 | 40 | TraversabilityCell::~TraversabilityCell() 41 | { 42 | } 43 | 44 | void TraversabilityCell::setTraversabilityClassId(uint8_t traversabilityClassId) 45 | { 46 | this->traversabilityClassId = traversabilityClassId; 47 | } 48 | 49 | uint8_t TraversabilityCell::getTraversabilityClassId() const 50 | { 51 | return traversabilityClassId; 52 | } 53 | 54 | void TraversabilityCell::setProbability(uint8_t probability) 55 | { 56 | this->probability = probability; 57 | } 58 | 59 | uint8_t TraversabilityCell::getProbability() const 60 | { 61 | return probability; 62 | } 63 | 64 | bool TraversabilityCell::operator==(const TraversabilityCell& other) const 65 | { 66 | bool isequal = traversabilityClassId == other.getTraversabilityClassId() && probability == other.getProbability(); 67 | return isequal; 68 | } 69 | 70 | bool TraversabilityCell::operator!=(const TraversabilityCell& other) const 71 | { 72 | bool isinequal = traversabilityClassId != other.getTraversabilityClassId() || probability != other.getProbability(); 73 | return isinequal; 74 | } 75 | -------------------------------------------------------------------------------- /src/grid/TraversabilityCell.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #ifndef __MAPS_TRAVERSABILITY_CELL_HPP_ 28 | #define __MAPS_TRAVERSABILITY_CELL_HPP_ 29 | 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | namespace maps { namespace grid 37 | { 38 | /** 39 | * Type used in TraversabilityGrid as a cell type to hold the values of traversabilityClassId and probability for the respective cell of the grid. 40 | * @details: traversabilityClassId is cooresponds to a TraversabilityClass holding the actual traversability value, 41 | * while probability is used as a measure of certainty for the traversability value. 42 | * */ 43 | class TraversabilityCell 44 | { 45 | private: 46 | uint8_t traversabilityClassId; 47 | uint8_t probability; 48 | 49 | public: 50 | // Default constructor, sets traversability and probability to zero. 51 | TraversabilityCell(); 52 | 53 | // Initializes the TraversabilityCell with the given values for traversabilityClassId and probability. 54 | TraversabilityCell(uint8_t traversabilityClassId, uint8_t probability); 55 | 56 | ~TraversabilityCell(); 57 | 58 | void setTraversabilityClassId(uint8_t traversabilityClassId); 59 | uint8_t getTraversabilityClassId() const; 60 | 61 | void setProbability(uint8_t probability); 62 | uint8_t getProbability() const; 63 | 64 | bool operator==(const maps::grid::TraversabilityCell& other) const; 65 | bool operator!=(const TraversabilityCell& other) const; 66 | 67 | protected: 68 | 69 | /** Grants access to boost serialization */ 70 | friend class boost::serialization::access; 71 | 72 | /** Serializes the members of this class*/ 73 | template 74 | void serialize(Archive &ar, const unsigned int version) 75 | { 76 | ar & BOOST_SERIALIZATION_NVP(traversabilityClassId); 77 | ar & BOOST_SERIALIZATION_NVP(probability); 78 | } 79 | }; 80 | 81 | } //end namespace grid 82 | } //end namespace maps 83 | 84 | #endif //__MAPS_TRAVERSABILITY_CELL_HPP_ 85 | -------------------------------------------------------------------------------- /src/grid/TraversabilityClass.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #include "TraversabilityClass.hpp" 28 | 29 | 30 | using namespace maps::grid; 31 | 32 | TraversabilityClass::TraversabilityClass(float drivability): drivability(drivability) 33 | { 34 | } 35 | 36 | TraversabilityClass::TraversabilityClass(): drivability(-1) 37 | { 38 | } 39 | 40 | bool TraversabilityClass::isClassDefined() const 41 | { 42 | return drivability >= 0; 43 | } 44 | 45 | bool TraversabilityClass::isTraversable() const 46 | { 47 | return drivability > 0; 48 | } 49 | 50 | float TraversabilityClass::getDrivability() const 51 | { 52 | if (drivability < 0) 53 | return 0; 54 | 55 | return drivability; 56 | } 57 | -------------------------------------------------------------------------------- /src/grid/TraversabilityClass.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #ifndef __MAPS_TAVERSABILITY_CLASS_HPP_ 28 | #define __MAPS_TAVERSABILITY_CLASS_HPP_ 29 | 30 | 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | namespace maps { namespace grid 37 | { 38 | 39 | /** 40 | * @brief Type holding the actual traversability value used in TraversabilityGrid. 41 | **/ 42 | class TraversabilityClass 43 | { 44 | public: 45 | 46 | /** 47 | * Default constructor. 48 | * Drivability must be given in the range of [0,1] (0 - 100%). 49 | * */ 50 | TraversabilityClass(float drivability); 51 | 52 | TraversabilityClass(); 53 | 54 | /** 55 | * Return whether this class is defined 56 | * or if it is an empty placeholder. 57 | * */ 58 | bool isClassDefined() const; 59 | 60 | /** 61 | * Returns whether the terrain is drivable. 62 | * */ 63 | bool isTraversable() const; 64 | 65 | /** 66 | * Returns a value in the interval [0, 1]. 67 | * Zero means not drivable at all and 68 | * one means perfect ground for driving. 69 | * */ 70 | float getDrivability() const; 71 | 72 | private: 73 | float drivability; 74 | 75 | protected: 76 | 77 | /** Grants access to boost serialization */ 78 | friend class boost::serialization::access; 79 | 80 | /** Serializes the members of this class*/ 81 | template 82 | void serialize(Archive &ar, const unsigned int version) 83 | {; 84 | ar & BOOST_SERIALIZATION_NVP(drivability); 85 | } 86 | }; 87 | 88 | } //end namespace grid 89 | } //end namespace maps 90 | 91 | #endif //__MAPS_TAVERSABILITY_CLASS_HPP_ 92 | -------------------------------------------------------------------------------- /src/grid/VectorGridAccess.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #pragma once 28 | 29 | #include "GridAccessInterface.hpp" 30 | 31 | namespace maps { namespace grid 32 | { 33 | template 34 | class VectorGridAccess : public GridAccessInterface 35 | { 36 | VectorGrid *grid; 37 | public: 38 | VectorGridAccess(VectorGrid *grid) : grid(grid) 39 | { 40 | } 41 | 42 | virtual ~VectorGridAccess() 43 | { 44 | } 45 | 46 | virtual const CellBaseT &getDefaultValue() const 47 | { 48 | return grid->getDefaultValue(); 49 | } 50 | 51 | virtual typename GridAccessInterface::iterator begin() 52 | { 53 | return AccessIteratorImpl >(grid->begin()); 54 | } 55 | 56 | virtual typename GridAccessInterface::iterator end() 57 | { 58 | return AccessIteratorImpl >(grid->end()); 59 | } 60 | 61 | virtual typename GridAccessInterface::const_iterator begin() const 62 | { 63 | return ConstAccessIteratorImpl >(grid->begin()); 64 | } 65 | 66 | virtual typename GridAccessInterface::const_iterator end() const 67 | { 68 | return ConstAccessIteratorImpl >(grid->end()); 69 | } 70 | 71 | virtual void resize(Vector2ui new_number_cells) 72 | { 73 | grid->resize(new_number_cells); 74 | }; 75 | 76 | virtual void moveBy(Index idx) 77 | { 78 | grid->moveBy(idx); 79 | } 80 | 81 | virtual const CellBaseT& at(Index idx) const 82 | { 83 | return static_cast(grid->at(idx)); 84 | } 85 | 86 | virtual CellBaseT& at(Index idx) 87 | { 88 | return static_cast(grid->at(idx)); 89 | } 90 | 91 | virtual const CellBaseT& at(size_t x, size_t y) const 92 | { 93 | return static_cast(grid->at(x, y)); 94 | } 95 | 96 | virtual CellBaseT& at(size_t x, size_t y) 97 | { 98 | return static_cast(grid->at(x, y)); 99 | } 100 | 101 | virtual const Vector2ui &getNumCells() const 102 | { 103 | return grid->getNumCells(); 104 | }; 105 | 106 | virtual void clear() 107 | { 108 | grid->clear(); 109 | }; 110 | }; 111 | }} 112 | -------------------------------------------------------------------------------- /src/maps.pc.in: -------------------------------------------------------------------------------- 1 | prefix=@CMAKE_INSTALL_PREFIX@ 2 | exec_prefix=@CMAKE_INSTALL_PREFIX@ 3 | libdir=${prefix}/lib 4 | includedir=${prefix}/include 5 | 6 | Name: @TARGET_NAME@ 7 | Description: @PROJECT_DESCRIPTION@ 8 | Version: @PROJECT_VERSION@ 9 | Requires: @PKGCONFIG_REQUIRES@ 10 | Libs: -L${libdir} -l@TARGET_NAME@ @PKGCONFIG_LIBS@ 11 | Cflags: -I${includedir} @PKGCONFIG_CFLAGS@ 12 | 13 | -------------------------------------------------------------------------------- /src/operations/CoverageMapGeneration.cpp: -------------------------------------------------------------------------------- 1 | #include "CoverageMapGeneration.hpp" 2 | 3 | namespace maps { 4 | 5 | namespace operations { 6 | 7 | bool CoverageTracker::frameChanged(const grid::MLSMapKalman& mls_) const 8 | { 9 | return ! (coverage.getLocalFrame().isApprox(mls_.getLocalFrame()) 10 | && coverage.getResolution().isApprox(mls_.getResolution()) 11 | && coverage.getNumCells() == mls_.getNumCells()); 12 | } 13 | 14 | 15 | void CoverageTracker::updateMLS(const grid::MLSMapKalman& mls_) { 16 | mls = &mls_; 17 | return; // FIXME shift local frame? 18 | if(frameChanged(*mls)) 19 | { 20 | std::cout << "Frame changed. Resetting coverage. New Frame\n" << mls->getLocalFrame().matrix().topRows<3>() << "\nResolution: " << mls->getResolution().transpose() << ", NumCells: " << mls->getNumCells() << "\n"; 21 | grid::MLSConfig cfg = mls->getConfig(); 22 | cfg.updateModel = grid::MLSConfig::BASE; 23 | 24 | // Just reset the coverage map for now 25 | // TODO FIXME properly move the current contents of the coverage map 26 | coverage = CoverageMap3d(mls->getNumCells(), mls->getResolution(), cfg); 27 | coverage.getLocalFrame() = mls->getLocalFrame(); 28 | } 29 | } 30 | 31 | void CoverageTracker::addCoverage(const double &radius, const base::AngleSegment& range /* ignored */, const base::Pose& pose_in_map) 32 | { 33 | if(false && frameChanged(*mls) ) // FIXME temporarily disabled (MLS frame is irrelevant at the moment) 34 | { 35 | std::cerr << "Local Frames and resolution of coverage map and MLS map must be the same.\n"; 36 | updateMLS(*mls); // HACK should not be necessary 37 | // throw std::runtime_error( "Local Frames and resolution of coverage map and MLS map must be the same."); 38 | } 39 | 40 | const Eigen::Array2d &res = coverage.getResolution(); 41 | const Eigen::Affine3d pose_in_grid = coverage.getLocalFrame() * pose_in_map.toTransform(); 42 | const Eigen::Array2d pos2d = pose_in_grid.translation().head<2>().array(); 43 | const double z = pose_in_grid.translation().z(); 44 | 45 | const Eigen::Array2i minIdx = ((pos2d - radius) / res).cast().max(0); 46 | const Eigen::Array2i maxIdx = ((pos2d + radius) / res).cast().min(coverage.getNumCells().array().cast()); 47 | 48 | for(int x=minIdx.x(); x < maxIdx.x(); ++x) 49 | { 50 | for(int y=minIdx.y(); y< maxIdx.y(); ++y) 51 | { 52 | // TODO here we need to ray-trace through the MLS and consider the angle-range 53 | 54 | Eigen::Array2i idx(x,y); 55 | 56 | const double z_diff_2 = radius*radius - (idx.cast() * res - pos2d).matrix().squaredNorm(); 57 | if(z_diff_2 <= 0) continue; 58 | 59 | const double z_diff = std::sqrt(z_diff_2); 60 | coverage.mergePatch(grid::Index(idx.matrix()), CoverageMap3d::Patch(z+z_diff, 2*z_diff)); 61 | } 62 | } 63 | } 64 | 65 | 66 | 67 | } // namespace operations 68 | } // namespace maps 69 | -------------------------------------------------------------------------------- /src/operations/CoverageMapGeneration.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | // TODO make configurable? 6 | // TODO MLSMap is currently ignored 7 | 8 | namespace maps { 9 | 10 | // the simplest ML-Map (only vertical intervals): 11 | typedef grid::MLSMapBase CoverageMap3d; 12 | 13 | namespace operations { 14 | 15 | class CoverageTracker { 16 | CoverageMap3d coverage; 17 | const grid::MLSMapKalman* mls; 18 | 19 | bool frameChanged(const grid::MLSMapKalman& mls_) const; 20 | 21 | 22 | public: 23 | CoverageTracker() : mls(nullptr) 24 | { 25 | 26 | } 27 | 28 | void updateMLS(const grid::MLSMapKalman& mls_); 29 | 30 | template 31 | void setFrame(const grid::GridMap& map) 32 | { 33 | coverage.resize(map.getNumCells()); 34 | coverage.setResolution(map.getResolution()); 35 | coverage.getLocalFrame() = map.getLocalFrame(); 36 | } 37 | 38 | void addCoverage(const double &radius, const base::AngleSegment& range /* ignored */, const base::Pose& pose_in_map); 39 | const CoverageMap3d& getCoverage() const { return coverage; } 40 | }; 41 | 42 | } // namespace operations 43 | } // namespace maps 44 | -------------------------------------------------------------------------------- /src/tools/BresenhamLine.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #include "BresenhamLine.hpp" 28 | 29 | namespace maps { namespace tools 30 | { 31 | 32 | 33 | Bresenham::Bresenham(const Eigen::Vector2i& start, const Eigen::Vector2i& end) 34 | { 35 | init(start.x(), start.y(), end.x(), end.y()); 36 | } 37 | 38 | 39 | void Bresenham::init(const Point& start, const Point& end) 40 | { 41 | init(start.x(), start.y(), end.x(), end.y()); 42 | } 43 | 44 | void Bresenham::init(int startX, int startY, int endX, int endY) 45 | { 46 | x0 = startX; 47 | y0 = startY; 48 | x1 = endX; 49 | y1 = endY; 50 | dx = abs(x1-x0), sx = x0 dy) { err += dy; x0 += sx; } /* e_xy+e_x > 0 */ 71 | if (e2 < dx) { err += dx; y0 += sy; } /* e_xy+e_y < 0 */ 72 | 73 | return true; 74 | } 75 | 76 | 77 | 78 | } // namespace tools 79 | } // namespace maps 80 | -------------------------------------------------------------------------------- /src/tools/BresenhamLine.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #ifndef __MAPS_BRESENHAM_LINE__ 28 | #define __MAPS_BRESENHAM_LINE__ 29 | 30 | #include 31 | 32 | namespace maps { namespace tools 33 | { 34 | 35 | 36 | /** 37 | * This class use useful for calculation 38 | * straight lines in grid. The Bresenham Algorithm 39 | * is a fast but inaccurate way to do this. 40 | * Inaccurate means, that the result will have 41 | * aliasing artifacts. 42 | * 43 | * The implementation of the core algorithm was taken from an wikipedia article. 44 | * */ 45 | class Bresenham { 46 | public: 47 | typedef Eigen::Vector2i Point; 48 | Bresenham(const Point& start, const Point& end); 49 | 50 | /** 51 | * Inits the algorithm. 52 | * The method may be used to 'reinit' the class 53 | * after a line was already interpolated. 54 | * */ 55 | void init(const Point& start, const Point& end); 56 | void init(int startX, int startY, int endX, int endY); 57 | 58 | /** 59 | * Calculates the next point in the line 60 | * and returns it over the given parameters. 61 | * 62 | * returns false if the end point is reached an 63 | * no further point can be calculated. 64 | * 65 | * */ 66 | bool getNextPoint(Point& next); 67 | 68 | private: 69 | bool hasP; 70 | int x0,y0, x1, y1, dx, sx, dy, sy, err; 71 | }; 72 | 73 | 74 | }} // namespace maps 75 | 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /src/tools/MLSToSlopes.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #ifndef __MAPS_MLS_TO_SLOPES_HPP_ 28 | #define __MAPS_MLS_TO_SLOPES_HPP_ 29 | 30 | #include "../grid/GridMap.hpp" 31 | #include "../grid/MLSMap.hpp" 32 | 33 | namespace maps { namespace tools 34 | { 35 | /** This operator computes local slopes on a MLS map 36 | * 37 | * It acts on an MLSMapKalman and updates a GridMap with the maximum local 38 | * slope angles in radians. 39 | * It also returns the absolute value of the maximal step between each cell and its direct 40 | * neighbours. 41 | * 42 | * The default operation will compute the maximum slope between the topmost 43 | * surfaces of a MLS. In practice, it means that it works only on MLS that 44 | * have one patch per cell. 45 | */ 46 | class MLSToSlopes 47 | { 48 | 49 | public: 50 | 51 | /** 52 | * @brief: Compute maxSteps from MLSMapKalman. 53 | * @details: The output GridMap will be fitted to match the size and resolution of the input. 54 | * If not enough information is available for a cell all ouput values are set to -inf. 55 | * @param useStdDev : Set to true to include standard deviation in the calculation of max steps. 56 | * @param correctSteps : Set to true to compute corrected max steps instead. 57 | * @param correctedStepThreshold : If the value for a max step falls under the threshold, it will be corrected. 58 | * */ 59 | static bool computeMaxSteps(const maps::grid::MLSMapKalman& mlsIn, maps::grid::GridMapF& maxStepsOut, 60 | bool useStdDev = false, bool correctSteps = false, float correctedStepThreshold = 0); 61 | 62 | /** 63 | * @brief Compute slopes from MLSMapKalman. 64 | * @param windowSize: The slope for each cell is computed from -windowSize to windowSize around the cell. Has to be >= 1. 65 | **/ 66 | static bool computeSlopes(const maps::grid::MLSMapKalman& mlsIn, maps::grid::GridMapF& slopesOut, int windowSize = 1); 67 | 68 | }; 69 | 70 | } // end namespace tools 71 | } // end namespace maps 72 | 73 | #endif // end __MAPS_MLS_TO_SLOPES_HPP_ 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | -------------------------------------------------------------------------------- /src/tools/Overlap.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #pragma once 28 | 29 | namespace maps { namespace tools 30 | { 31 | inline bool overlap( double a_min, double a_max, double b_min, double b_max ) 32 | { 33 | return 34 | ((b_max >= a_max) && (b_min <= a_max)) || 35 | ((b_max >= a_min) && (b_max <= a_max)) ; 36 | } 37 | 38 | template 39 | inline typename std::enable_if::value, bool>::type overlap(const Patch p, double b_min, double b_max) 40 | { 41 | return overlap(p.getMin(), p.getMax(), b_min, b_max); 42 | } 43 | template 44 | inline typename std::enable_if::value, bool>::type overlap(const Patch p, double b_min, double b_max) 45 | { 46 | return overlap(p->getMin(), p->getMax(), b_min, b_max); 47 | } 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /src/tools/SimpleTraversabilityRadialLUT.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #ifndef __MAPS_SIMPLE_TRAVERSABILITY_RADIAL_LUT_HPP_ 28 | #define __MAPS_SIMPLE_TRAVERSABILITY_RADIAL_LUT_HPP_ 29 | 30 | #include "maps/grid/TraversabilityGrid.hpp" 31 | #include "boost/multi_array.hpp" 32 | 33 | namespace maps { namespace tools 34 | { 35 | 36 | /** 37 | * Radial lookup table used in SimpleTraversability::closeNarrowPassages. 38 | **/ 39 | class SimpleTraversabilityRadialLUT 40 | { 41 | private: 42 | int centerX, centerY; 43 | unsigned int width, height; 44 | boost::multi_array, 2> parents; 45 | boost::multi_array inDistance; 46 | 47 | public: 48 | /** 49 | * Sets up parents and inDistance so they cover a grid of 50 | * (@a distance * 2) x (@a distance * 2) with an additional 51 | * Cell as the center. 52 | */ 53 | void precompute(double distance, double resolutionX, double resolutionY); 54 | 55 | /** 56 | * Checks for the targetedValue within the in distance around the given center and calls markSingleRadius on them. 57 | */ 58 | void markAllRadius(grid::TraversabilityGrid& traversabilityGrid, int centerX, int centerY, int targetedValue) const; 59 | 60 | /** 61 | * Marks all cells between target and center with the @a markValue using the @a parents table. 62 | */ 63 | void markSingleRadius(grid::TraversabilityGrid& traversabilityGrid, int centerX, int centerY, int targetX, int targetY, int expectedValue, int markValue) const; 64 | 65 | }; 66 | 67 | } // End namespace tools. 68 | } // End namespace maps. 69 | 70 | #endif // __MAPS_SIMPLE_TRAVERSABILITY_RADIAL_LUT_HPP_ 71 | -------------------------------------------------------------------------------- /src/tools/SurfaceIntersection.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #pragma once 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | namespace maps { namespace tools { 35 | 36 | class SurfaceIntersection 37 | { 38 | public: 39 | 40 | /** 41 | * Computes the intersection points between a plane and an axis aligned box. 42 | * Note: This method doesn't check if the plane intersects with the box. 43 | * Note: The intersection points are appended to the vector, i.e., the caller must make sure it is empty. 44 | */ 45 | template 46 | static void computeIntersections(const Eigen::Hyperplane& plane, const Eigen::AlignedBox& box, 47 | std::vector< Eigen::Matrix, Allocator >& intersections) 48 | { 49 | typedef Eigen::Matrix Vec; 50 | Vec normal = plane.normal(); 51 | Vec box_center = box.center(); 52 | Vec extents = box.max() - box_center; 53 | const Scalar dist = -plane.signedDistance(box_center); 54 | 55 | // find the max coefficient of the normal: 56 | int i=0, j=1, k=2; 57 | if(std::abs(normal[i]) < std::abs(normal[j])) std::swap(i,j); 58 | if(std::abs(normal[i]) < std::abs(normal[k])) std::swap(i,k); 59 | 60 | const Scalar dotj = extents[j] * normal[j]; 61 | const Scalar dotk = extents[k] * normal[k]; 62 | 63 | Vec prev_p; 64 | enum { NONE, LOW, BOX, HIGH } prev_pos = NONE, pos; 65 | // calculate intersections in direction i: 66 | for(int n=0; n<5; ++n) 67 | { 68 | Vec p(0,0,0); 69 | Scalar dotp = Scalar(0.0); 70 | if((n+1)&2) 71 | dotp += dotj, p[j] = extents[j]; 72 | else 73 | dotp -= dotj, p[j] = -extents[j]; 74 | if(n&2) 75 | dotp += dotk, p[k] = extents[k]; 76 | else 77 | dotp -= dotk, p[k] = -extents[k]; 78 | 79 | p[i] = (dist - dotp) / normal[i]; 80 | 81 | if( p[i] < -extents[i]) 82 | pos = LOW; 83 | else if( p[i] > extents[i]) 84 | pos = HIGH; 85 | else 86 | pos = BOX; 87 | 88 | if( (prev_pos == LOW || prev_pos == HIGH) && pos != prev_pos ) 89 | { 90 | // clipping in 91 | const Scalar h = prev_pos == LOW ? -extents[i] : extents[i]; 92 | const Scalar s = (h - prev_p[i]) / (p[i] - prev_p[i]); 93 | intersections.push_back(box_center + prev_p + (p - prev_p) * s); 94 | } 95 | if( pos == BOX ) 96 | { 97 | if( n==4 ) break; // n==4 is only for clipping in 98 | intersections.push_back(box_center + p); 99 | } 100 | else if( pos != prev_pos && prev_pos != NONE ) 101 | { 102 | // clipping out 103 | const Scalar h = pos == LOW ? -extents[i] : extents[i]; 104 | const Scalar s = (h - prev_p[i]) / (p[i] - prev_p[i]); 105 | intersections.push_back(box_center + prev_p + (p - prev_p) * s); 106 | } 107 | 108 | prev_pos = pos; 109 | prev_p = p; 110 | } 111 | } 112 | }; 113 | 114 | }} 115 | -------------------------------------------------------------------------------- /src/tools/TSDFPolygonMeshReconstruction.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #include "TSDFPolygonMeshReconstruction.hpp" 28 | 29 | #include 30 | #include 31 | 32 | using namespace maps::tools; 33 | using namespace maps::grid; 34 | 35 | void TSDFPolygonMeshReconstruction::reconstruct(pcl::PolygonMesh& output) 36 | { 37 | std::vector< Eigen::Vector3f, Eigen::aligned_allocator > surfaces; 38 | std::vector intensities; 39 | reconstructSurfaces(surfaces, intensities); 40 | 41 | pcl::PointCloud cloud_with_intensity; 42 | cloud_with_intensity.resize(surfaces.size()); 43 | for(unsigned i = 0; i < surfaces.size(); i++) 44 | { 45 | cloud_with_intensity[i].getVector3fMap() = surfaces[i]; 46 | cloud_with_intensity[i].intensity = intensities[i]; 47 | } 48 | 49 | pcl::toPCLPointCloud2(cloud_with_intensity, output.cloud); 50 | 51 | output.polygons.resize (cloud_with_intensity.size() / 3); 52 | for(size_t i = 0; i < output.polygons.size(); ++i) 53 | { 54 | pcl::Vertices v; 55 | v.vertices.resize (3); 56 | for(int j = 0; j < 3; ++j) 57 | { 58 | v.vertices[j] = static_cast (i) * 3 + j; 59 | } 60 | output.polygons[i] = v; 61 | } 62 | } 63 | -------------------------------------------------------------------------------- /src/tools/TSDFPolygonMeshReconstruction.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #pragma once 28 | 29 | #include 30 | #include "TSDFSurfaceReconstruction.hpp" 31 | 32 | namespace maps { namespace tools 33 | { 34 | 35 | class TSDFPolygonMeshReconstruction : public TSDFSurfaceReconstruction 36 | { 37 | public: 38 | TSDFPolygonMeshReconstruction() : TSDFSurfaceReconstruction() {} 39 | 40 | void reconstruct(pcl::PolygonMesh &output); 41 | }; 42 | 43 | }} 44 | -------------------------------------------------------------------------------- /src/tools/TSDF_MLSMapReconstruction.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #include "TSDF_MLSMapReconstruction.hpp" 28 | 29 | using namespace maps::tools; 30 | using namespace maps::grid; 31 | 32 | void TSDF_MLSMapReconstruction::reconstruct(MLSMapPrecalculated& output) 33 | { 34 | std::vector< Eigen::Vector3f, Eigen::aligned_allocator > surfaces; 35 | std::vector intensities; 36 | reconstructSurfaces(surfaces, intensities, false); 37 | 38 | output = MLSMapPrecalculated(tsdf_map->getNumCells() + maps::grid::Vector2ui(1,1), tsdf_map->getResolution(), MLSConfig()); 39 | output.getLocalFrame().translation() << 0.5*tsdf_map->getResolution(), 0; 40 | 41 | Eigen::Vector3f p1, p2, p3; 42 | Index idx; 43 | Eigen::Vector3f center; 44 | Eigen::Vector3f normal; 45 | float min_z, max_z; 46 | for(unsigned i = 0; i < surfaces.size(); i += 3) 47 | { 48 | p1 = surfaces[i]; 49 | p2 = surfaces[i+1]; 50 | p3 = surfaces[i+2]; 51 | normal = (p2 - p1).cross(p3 - p1); 52 | normal.normalize(); 53 | 54 | // in case the vectors are zero or linearly dependent 55 | if(!normal.allFinite()) 56 | continue; 57 | 58 | center = (p1 + p2 + p3) / 3.f; 59 | if(!center.allFinite()) 60 | continue; 61 | 62 | min_z = std::min(p1.z(), std::min(p2.z(), p3.z())); 63 | max_z = std::max(p1.z(), std::max(p2.z(), p3.z())); 64 | 65 | Eigen::Vector3d pos_diff; 66 | if(output.toGrid(center.cast(), idx, pos_diff)) 67 | { 68 | SurfacePatch< MLSConfig::PRECALCULATED > patch(pos_diff.cast(), normal, min_z, max_z); 69 | // insert as new patch 70 | output.at(idx).insert(patch); 71 | } 72 | else 73 | { 74 | std::stringstream stream; 75 | stream << center.transpose(); 76 | stream << " is outside of grid! This should never happen."; 77 | throw std::runtime_error(stream.str()); 78 | } 79 | } 80 | 81 | // set local frame 82 | output.getLocalFrame() = output.getLocalFrame() * tsdf_map->getLocalFrame(); 83 | } 84 | -------------------------------------------------------------------------------- /src/tools/TSDF_MLSMapReconstruction.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #pragma once 28 | 29 | #include 30 | #include 31 | 32 | #include "TSDFSurfaceReconstruction.hpp" 33 | 34 | namespace maps { namespace tools 35 | { 36 | 37 | class TSDF_MLSMapReconstruction : public TSDFSurfaceReconstruction 38 | { 39 | public: 40 | TSDF_MLSMapReconstruction() : TSDFSurfaceReconstruction() {} 41 | 42 | void reconstruct(maps::grid::MLSMapPrecalculated &output); 43 | }; 44 | 45 | }} 46 | -------------------------------------------------------------------------------- /src/tools/TraversabilityGrassFireSearchItem.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #ifndef __MAPS_TRAVERSABILITY_GRASSFIRE_SEARCHITEM_HPP_ 28 | #define __MAPS_TRAVERSABILITY_GRASSFIRE_SEARCHITEM_HPP_ 29 | 30 | #include 31 | 32 | namespace maps { namespace tools 33 | { 34 | 35 | class TraversabilityGrassfireSearchItem 36 | { 37 | public: 38 | TraversabilityGrassfireSearchItem(size_t x, size_t y, 39 | const grid::SurfacePatch* origin) 40 | : x(x) 41 | , y(y) 42 | , origin(origin) 43 | { 44 | }; 45 | 46 | size_t x; 47 | size_t y; 48 | const grid::SurfacePatch* origin; 49 | }; 50 | 51 | } // End namespace tools. 52 | } // End namespace maps. 53 | 54 | #endif // __MAPS_TRAVERSABILITY_GRASSFIRE_SEARCHITEM_HPP_ 55 | -------------------------------------------------------------------------------- /src/tools/TraversabilityGrassfire.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #ifndef __MAPS_TRAVERSABILITY_GRASSFIRE_HPP_ 28 | #define __MAPS_TRAVERSABILITY_GRASSFIRE_HPP_ 29 | 30 | #include 31 | 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | namespace maps { namespace tools 45 | { 46 | /** 47 | * @brief: TraversabilityGrassfire is a tool to calvulate the traversability 48 | * of an area based on maxSteps and slopes. 49 | * @details: TraversabilityGrassfire iterates recursively over the given mls 50 | * from the @param startPos. Any unreachable areas will not be 51 | * included. 52 | * Always set a @param config before calling @fn calculateTraversability. 53 | */ 54 | class TraversabilityGrassfire 55 | { 56 | typedef grid::SurfacePatch SurfacePatchKalman; 57 | typedef TraversabilityGrassfireSearchItem SearchItem; 58 | 59 | public: 60 | TraversabilityGrassfire(); 61 | TraversabilityGrassfire(const TraversabilityGrassfireConfig& config); 62 | 63 | void setConfig(const TraversabilityGrassfireConfig& config); 64 | bool calculateTraversability(grid::TraversabilityGrid& traversabilityGridOut, const grid::MLSMapKalman& mlsIn, const Eigen::Vector3d& startPos); 65 | 66 | private: 67 | bool determineDrivePlane(const grid::MLSMapKalman& mlsIn, const base::Vector3d& startPos, bool searchSurrounding = true); 68 | const SurfacePatchKalman* getNearestPatchWhereRobotFits(const grid::MLSMapKalman& mlsIn, size_t x, size_t y, double height, bool& isObstacle); 69 | void addNeighboursToSearchList(const grid::MLSMapKalman& mlsIn, std::size_t x, std::size_t y, const SurfacePatchKalman* patch); 70 | void checkRecursive(const grid::MLSMapKalman& mlsIn, size_t x, size_t y, const SurfacePatchKalman* origin); 71 | 72 | void computeTraversability(grid::TraversabilityGrid& traversabilityGridOut, const grid::MLSMapKalman& mlsIn) const; 73 | void setProbability(grid::TraversabilityGrid& traversabilityGridOut, size_t x, size_t y) const; 74 | void setTraversability(grid::TraversabilityGrid& traversabilityGridOut, const grid::MLSMapKalman& mlsIn, size_t x, size_t y) const; 75 | 76 | double getStepHeight(const SurfacePatchKalman* from, const SurfacePatchKalman* to); 77 | void markAsObstacle(size_t x, size_t y); 78 | 79 | TraversabilityGrassfireConfig config; 80 | 81 | std::queue searchList; 82 | 83 | boost::multi_array visited; 84 | boost::multi_array bestPatchMap; 85 | 86 | enum TRCLASSES 87 | { 88 | UNKNOWN = 0, 89 | OBSTACLE = 1, 90 | }; 91 | }; 92 | 93 | } // End namespace tools 94 | } // End namespace maps 95 | 96 | #endif // __MAPS_TRAVERSABILITY_GRASSFIRE_HPP_ 97 | -------------------------------------------------------------------------------- /src/tools/TraversabilityGrassfireConfig.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #ifndef __MAPS_TRAVERSABILITY_GRASSFIRE_CONFIG_HPP_ 28 | #define __MAPS_TRAVERSABILITY_GRASSFIRE_CONFIG_HPP_ 29 | 30 | namespace maps { namespace tools 31 | { 32 | class TraversabilityGrassfireConfig 33 | { 34 | public: 35 | TraversabilityGrassfireConfig() 36 | : maxStepHeight(0) 37 | , maxSlope(0) 38 | , robotHeight(0) 39 | , numTraversabilityClasses(0) 40 | , nominalStdDev(1) 41 | , outlierFilterMaxStdDev(0.0) 42 | { 43 | }; 44 | 45 | TraversabilityGrassfireConfig(double maxStepHeight, 46 | double maxSlope, 47 | double robotHeight, 48 | int numTraversabilityClasses, 49 | double nominalStdDev, 50 | double outlierFilterMaxStdDev) 51 | : maxStepHeight(maxStepHeight) 52 | , maxSlope(maxSlope) 53 | , robotHeight(robotHeight) 54 | , numTraversabilityClasses(numTraversabilityClasses) 55 | , nominalStdDev(nominalStdDev) 56 | , outlierFilterMaxStdDev(outlierFilterMaxStdDev) 57 | { 58 | }; 59 | 60 | double maxStepHeight; 61 | double maxSlope; 62 | double robotHeight; 63 | int numTraversabilityClasses; 64 | 65 | /** 66 | * NOTE: CHANGED FROM ENVIRE: Used to use both stddev and a min measurement count. 67 | * Since "Kalmanpatches" do not store their measurement count anymore, 68 | * this is no longer possible. 69 | * 70 | * The standard deviation a MLS-Patch needs 71 | * to get a probability of 1.0 (lower == better). 72 | */ 73 | double nominalStdDev; 74 | 75 | double outlierFilterMaxStdDev; 76 | }; 77 | } // End namespace tools. 78 | } // End namespace maps. 79 | 80 | #endif // __MAPS_TRAVERSABILITY_GRASSFIRE_CONFIG_HPP_ 81 | -------------------------------------------------------------------------------- /src/tools/VoxelTraversal.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #include "VoxelTraversal.hpp" 28 | 29 | using namespace maps::tools; 30 | 31 | void VoxelTraversal::computeRay(const Eigen::Vector3d& grid_res, const Eigen::Vector3d& origin, 32 | const Eigen::Vector3d& measurement, std::vector& voxel_indices) 33 | { 34 | voxel_indices.clear(); 35 | Eigen::Vector3i current_voxel, last_voxel, diff; 36 | Eigen::Vector3d ray, step, voxel_border, t_max, t_delta; 37 | diff = Eigen::Vector3i::Zero(); 38 | bool neg_ray = false; 39 | 40 | for(unsigned i = 0; i < 3; i++) 41 | { 42 | current_voxel(i) = std::floor(origin(i) / grid_res(i)); 43 | last_voxel(i) = std::floor(measurement(i) / grid_res(i)); 44 | ray(i) = measurement(i) - origin(i); 45 | 46 | // compute initial coefficients 47 | step(i) = (ray(i) >= 0.) ? 1. : -1.; 48 | voxel_border(i) = (current_voxel(i) + step(i)) * grid_res(i); 49 | t_max(i) = (ray(i) != 0.) ? (voxel_border(i) - origin(i)) / ray(i) : std::numeric_limits< double >::max(); 50 | t_delta(i) = (ray(i) != 0.) ? grid_res(i) / ray(i) * step(i) : std::numeric_limits< double >::max(); 51 | 52 | if(current_voxel(i) != last_voxel(i) && ray(i) < 0.) 53 | { 54 | diff(i)--; 55 | neg_ray = true; 56 | } 57 | } 58 | 59 | voxel_indices.push_back(current_voxel); 60 | if (neg_ray) 61 | { 62 | current_voxel += diff; 63 | voxel_indices.push_back(current_voxel); 64 | } 65 | 66 | // traverse ray 67 | while(last_voxel != current_voxel) 68 | { 69 | // identify axis to increase 70 | int axis = 0; 71 | if(t_max.x() < t_max.y()) 72 | axis = t_max.x() < t_max.z() ? 0 : 2; 73 | else 74 | axis = t_max.y() < t_max.z() ? 1 : 2; 75 | 76 | // increase index 77 | current_voxel[axis] += step[axis]; 78 | t_max[axis] += t_delta[axis]; 79 | voxel_indices.push_back(current_voxel); 80 | } 81 | } 82 | 83 | void VoxelTraversal::computeRay(const Eigen::Vector3d& grid_res, const Eigen::Vector3d& origin, 84 | const Eigen::Vector3d& measurement, std::vector< RayElement >& ray) 85 | { 86 | std::vector visited_voxels; 87 | computeRay(grid_res, origin, measurement, visited_voxels); 88 | 89 | ray.clear(); 90 | for(unsigned i = 0; i < visited_voxels.size(); i++) 91 | { 92 | if(!ray.empty() && ray.back().idx == visited_voxels[i].head<2>()) 93 | { 94 | ray.back().z_step = visited_voxels[i].z() - ray.back().z_last; 95 | ray.back().z_last = visited_voxels[i].z(); 96 | } 97 | else 98 | { 99 | ray.push_back(RayElement(visited_voxels[i], 0)); 100 | } 101 | } 102 | } -------------------------------------------------------------------------------- /src/tools/VoxelTraversal.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #pragma once 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | namespace maps { namespace tools 34 | { 35 | 36 | class VoxelTraversal 37 | { 38 | public: 39 | 40 | struct RayElement 41 | { 42 | RayElement(const Eigen::Vector3i& idx, int z_step) : idx(idx.x(), idx.y()), z_first(idx.z()), z_last(idx.z()), z_step(z_step) {} 43 | 44 | maps::grid::Index idx; 45 | int32_t z_first; 46 | int32_t z_last; 47 | int32_t z_step; 48 | }; 49 | 50 | /** 51 | * Voxel traversal algorithm implemented according to: 52 | * Amanatides, John, and Andrew Woo. "A fast voxel traversal algorithm for ray tracing." Eurographics. Vol. 87. No. 3. 1987. 53 | * 54 | * Origin and measurement must be expressed in the local map frame. 55 | */ 56 | static void computeRay(const Eigen::Vector3d& grid_res, const Eigen::Vector3d& origin, 57 | const Eigen::Vector3d& measurement, std::vector& ray); 58 | 59 | /** 60 | * Voxel traversal algorithm implemented according to: 61 | * Amanatides, John, and Andrew Woo. "A fast voxel traversal algorithm for ray tracing." Eurographics. Vol. 87. No. 3. 1987. 62 | * 63 | * Origin and measurement must be expressed in the local map frame. 64 | */ 65 | static void computeRay(const Eigen::Vector3d& grid_res, const Eigen::Vector3d& origin, 66 | const Eigen::Vector3d& measurement, std::vector& voxel_indices); 67 | 68 | }; 69 | 70 | 71 | 72 | }} 73 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # TEST GRID MAPS 2 | # 3 | add_subdirectory(grid) 4 | 5 | # TEST GEOMETRIC MAPS 6 | # 7 | add_subdirectory(geometric) 8 | 9 | # TEST SERIALIZATION 10 | # 11 | add_subdirectory(serialization) 12 | 13 | # TEST TOOLS 14 | # 15 | add_subdirectory(tools) 16 | 17 | # TEST VISUALIZATION 18 | # 19 | if( vizkit3d_FOUND AND OSGVIZ_PRIMITIVES_FOUND) 20 | add_subdirectory(viz) 21 | endif() 22 | -------------------------------------------------------------------------------- /test/common/GenerateMLS.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | 28 | /** Based local map **/ 29 | #include 30 | 31 | 32 | namespace maps { 33 | namespace grid { 34 | 35 | 36 | 37 | 38 | 39 | 40 | MLSMapSloped generateWaves() 41 | { 42 | // GridConfig conf(300, 300, 0.05, 0.05, -7.5, -7.5); 43 | Vector2d res(0.05, 0.05); 44 | Vector2ui numCells(300, 300); 45 | 46 | MLSConfig mls_config; 47 | mls_config.updateModel = MLSConfig::SLOPE; 48 | //mls_config.updateModel = MLSConfig::KALMAN; 49 | MLSMapSloped mls = MLSMapSloped(numCells, res, mls_config); 50 | 51 | /** Translate the local frame (offset) **/ 52 | mls.getLocalFrame().translation() << 0.5*mls.getSize(), 0; 53 | 54 | 55 | Eigen::Vector2d max = 0.5 * mls.getSize(); 56 | Eigen::Vector2d min = -0.5 * mls.getSize(); 57 | for (double x = min.x(); x < max.x(); x += 0.00625) 58 | { 59 | double cs = std::cos(x * M_PI/2.5); 60 | for (double y = min.y(); y < max.y(); y += 0.00625) 61 | { 62 | double sn = std::sin(y * M_PI/2.5); 63 | mls.mergePoint(Eigen::Vector3d(x, y, cs*sn)); 64 | } 65 | } 66 | return mls; 67 | } 68 | 69 | } // namespace grid 70 | } // namespace maps 71 | -------------------------------------------------------------------------------- /test/geometric/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # TEST GEOMETRIC MAPS 2 | # 3 | rock_testsuite(test_geometricmap 4 | test_GeometricMap.cpp 5 | DEPS maps) 6 | 7 | rock_testsuite(test_point 8 | test_Point.cpp 9 | DEPS maps) 10 | 11 | rock_testsuite(test_linesegment 12 | test_LineSegment.cpp 13 | DEPS maps) 14 | 15 | rock_testsuite(test_contourmap 16 | test_ContourMap.cpp 17 | DEPS maps) 18 | 19 | rock_testsuite(test_closest_MLS 20 | test_closest_MLS.cpp 21 | DEPS maps) 22 | -------------------------------------------------------------------------------- /test/geometric/test_ContourMap.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #define BOOST_TEST_MODULE GeometricTest 28 | #include 29 | 30 | /** Geometric Contour Map **/ 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | #include 37 | 38 | using namespace ::maps::geometric; 39 | 40 | BOOST_AUTO_TEST_CASE(test_contour_map_constructor) 41 | { 42 | /** Check empty map **/ 43 | ContourMap *contour = new ContourMap(); 44 | BOOST_CHECK_EQUAL(contour->getNumElements(), 0); 45 | 46 | /** Check local map members **/ 47 | BOOST_CHECK_EQUAL(contour->getId(), ::maps::UNKNOWN_MAP_ID); 48 | BOOST_CHECK_EQUAL(contour->getMapType(), ::maps::LocalMapType::GEOMETRIC_MAP); 49 | BOOST_CHECK_EQUAL(contour->getEPSGCode(), ::maps::UNKNOWN_EPSG_CODE); 50 | BOOST_CHECK_EQUAL(contour->getLocalFrame().translation(), base::Transform3d::Identity().translation()); 51 | BOOST_CHECK_EQUAL(contour->getLocalFrame().rotation(), base::Transform3d::Identity().rotation()); 52 | 53 | delete contour; 54 | } 55 | 56 | 57 | BOOST_AUTO_TEST_CASE(test_contour_map_copy) 58 | { 59 | /** Check empty map **/ 60 | ContourMap *contour = new ContourMap(); 61 | BOOST_CHECK_EQUAL(contour->getNumElements(), 0); 62 | 63 | Point3d p_a, p_b; 64 | p_a << 0.00, 0.00, 0.00; p_b << 1.00, 1.00, 1.00; 65 | LineSegment3d my_line(p_a, p_b); 66 | 67 | for (register unsigned int i=0; i < 99 ; ++i) 68 | { 69 | contour->push_back(my_line); 70 | my_line.psi_a() << p_a.x()++, p_a.y(), p_a.z(); 71 | } 72 | 73 | ContourMap *contour_copy = new ContourMap(*contour); 74 | 75 | // check configuration 76 | BOOST_CHECK_EQUAL(contour_copy->getNumElements(), contour->getNumElements()); 77 | BOOST_CHECK_EQUAL(contour_copy->getMapType(), contour->getMapType()); 78 | BOOST_CHECK_EQUAL(contour->getEPSGCode(), contour->getEPSGCode()); 79 | BOOST_CHECK_EQUAL(contour_copy->getLocalFrame().translation(), contour->getLocalFrame().translation()); 80 | BOOST_CHECK_EQUAL(contour_copy->getLocalFrame().rotation(), contour->getLocalFrame().rotation()); 81 | 82 | for (register unsigned int i=0; i < contour->getNumElements() ; ++i) 83 | { 84 | BOOST_CHECK_EQUAL((*contour_copy)[i].psi_a(), (*contour)[i].psi_a()); 85 | BOOST_CHECK_EQUAL((*contour_copy)[i].psi_b(), (*contour)[i].psi_b()); 86 | } 87 | 88 | delete contour; 89 | delete contour_copy; 90 | } 91 | 92 | -------------------------------------------------------------------------------- /test/geometric/test_LineSegment.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #define BOOST_TEST_MODULE GeometricTest 28 | #include 29 | 30 | /** Element type **/ 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | #include 37 | 38 | using namespace ::maps::geometric; 39 | 40 | BOOST_AUTO_TEST_CASE(test_linesegment_constructor) 41 | { 42 | Point p_a(-2, -3); 43 | Point p_b(4, 2); 44 | LineSegment line(p_a, p_b); 45 | 46 | BOOST_CHECK_EQUAL(line.psi_a(), p_a); 47 | BOOST_CHECK_EQUAL(line.psi_b(), p_b); 48 | 49 | } 50 | 51 | BOOST_AUTO_TEST_CASE(test_linesegment_2d) 52 | { 53 | Point p_a(0, 6); 54 | Point p_b(6, 6); 55 | LineSegment line_0(p_a, p_b); 56 | 57 | BOOST_CHECK_EQUAL(line_0.psi_a(), p_a); 58 | BOOST_CHECK_EQUAL(line_0.psi_b(), p_b); 59 | BOOST_CHECK_EQUAL(line_0.rho(), 6); 60 | BOOST_CHECK_EQUAL(line_0.alpha(), M_PI/2.0); 61 | 62 | p_a << 0.00, 0.00; p_b << 6.00, 6.00; 63 | LineSegment line_1(p_a, p_b); 64 | 65 | BOOST_CHECK_EQUAL(line_1.psi_a(), p_a); 66 | BOOST_CHECK_EQUAL(line_1.psi_b(), p_b); 67 | BOOST_CHECK_EQUAL(line_1.rho(), 0); 68 | BOOST_CHECK_EQUAL(line_1.alpha(), 3.0*(M_PI/4.0)); 69 | 70 | p_a << 0.00, 6.00; p_b << 6.00, 0.00; 71 | LineSegment line_2(p_a, p_b); 72 | 73 | BOOST_CHECK_EQUAL(line_2.psi_a(), p_a); 74 | BOOST_CHECK_EQUAL(line_2.psi_b(), p_b); 75 | BOOST_CHECK_EQUAL(line_2.rho(), 4.2426406871192848); 76 | BOOST_CHECK_EQUAL(line_2.alpha(), M_PI/4.0); 77 | 78 | p_a << -6.00, 0.00; p_b << 0.00, 6.00; 79 | LineSegment line_3(p_a, p_b); 80 | 81 | BOOST_CHECK_EQUAL(line_3.psi_a(), p_a); 82 | BOOST_CHECK_EQUAL(line_3.psi_b(), p_b); 83 | BOOST_CHECK_EQUAL(line_3.rho(), 4.2426406871192848); 84 | BOOST_CHECK_EQUAL(line_3.alpha(), 3.0*(M_PI/4.0)); 85 | 86 | //std::cout<<"rho = "< p_a(0, 6, 0); 95 | Point p_b(6, 6, 0); 96 | LineSegment line_0(p_a, p_b); 97 | 98 | BOOST_CHECK_EQUAL(line_0.psi_a(), p_a); 99 | BOOST_CHECK_EQUAL(line_0.psi_b(), p_b); 100 | BOOST_CHECK_EQUAL(line_0.rho(), 6); 101 | BOOST_CHECK_EQUAL(boost::math::isnan(line_0.alpha()), true); 102 | } 103 | 104 | BOOST_AUTO_TEST_CASE(test_linesegment_typedefs) 105 | { 106 | Point p_a_2d(0, 6); 107 | Point p_b_2d(6, 6); 108 | 109 | Point p_a_3d(0, 6, 0); 110 | Point p_b_3d(6, 6, 0); 111 | 112 | LineSegment2d line_2d(p_a_2d, p_b_2d); 113 | LineSegment3d line_3d(p_a_3d, p_b_3d); 114 | 115 | Point p_a_2f(0, 6); 116 | Point p_b_2f(6, 6); 117 | 118 | Point p_a_3f(0, 6, 0); 119 | Point p_b_3f(6, 6, 0); 120 | 121 | LineSegment2f line_2f(p_a_2f, p_b_2f); 122 | LineSegment3f line_3f(p_a_3f, p_b_3f); 123 | } 124 | 125 | 126 | -------------------------------------------------------------------------------- /test/geometric/test_Point.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #define BOOST_TEST_MODULE GeometricTest 28 | #include 29 | 30 | /** Element type **/ 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | 37 | using namespace ::maps::geometric; 38 | 39 | BOOST_AUTO_TEST_CASE(test_point_constructor) 40 | { 41 | Point p_a(-2, -3); 42 | Point p_b(4, 2); 43 | 44 | BOOST_CHECK_EQUAL(p_a.x(), -2); 45 | BOOST_CHECK_EQUAL(p_a.y(), -3); 46 | 47 | BOOST_CHECK_EQUAL(p_b.x(), 4); 48 | BOOST_CHECK_EQUAL(p_b.y(), 2); 49 | 50 | Point v((p_b - p_a).normalized()); 51 | BOOST_CHECK_EQUAL(v.x(), 0.76822127959737585); 52 | BOOST_CHECK_EQUAL(v.y(), 0.64018439966447993); 53 | BOOST_CHECK_EQUAL(1.2, v[0]/v[1]); 54 | BOOST_CHECK_EQUAL(0.87605805059819342, atan(v[0]/v[1])); 55 | 56 | } 57 | 58 | BOOST_AUTO_TEST_CASE(test_point_typedefs) 59 | { 60 | Point2d point_2d(0.00, 0.00); 61 | Point3d point_3d(0.00, 0.00, 0.00); 62 | 63 | Point2f point_2f(0.00, 0.00); 64 | Point3f point_3f(0.00, 0.00, 0.00); 65 | } 66 | -------------------------------------------------------------------------------- /test/geometric/test_closest_MLS.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #define BOOST_TEST_MODULE SerializationTest 28 | #include 29 | 30 | 31 | /** Based local map **/ 32 | #include "../common/GenerateMLS.hpp" 33 | 34 | using namespace ::maps::grid; 35 | 36 | 37 | 38 | 39 | BOOST_AUTO_TEST_CASE(test_mls_serialization) 40 | { 41 | MLSMapSloped mls = generateWaves(); 42 | 43 | 44 | 45 | Eigen::Vector2d max = 0.5 * mls.getSize(); 46 | Eigen::Vector2d min = -0.5 * mls.getSize(); 47 | double max_error = -1; 48 | for (double x = min.x(); x < max.x(); x += 0.00625*16) 49 | { 50 | std::cout << "\n\n\nx:" << x << '\n'; 51 | double cs = std::cos(x * M_PI/2.5); 52 | for (double y = min.y(); y < max.y(); y += 0.00625*16) 53 | { 54 | double sn = std::sin(y * M_PI/2.5); 55 | double z =-10; 56 | mls.getClosestSurfacePos(Eigen::Vector3d(x,y,z), z); 57 | max_error = std::max(std::abs(z-sn*cs), max_error ); 58 | std::cout << "y: " << y << ", z: " << z << ", expected: " << sn*cs << '\n'; 59 | } 60 | } 61 | std::cout << "\nMaxError: " << max_error << '\n'; 62 | 63 | 64 | 65 | } 66 | -------------------------------------------------------------------------------- /test/grid/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # TEST GRID MAPS 2 | # 3 | rock_testsuite(test_index 4 | test_Index.cpp 5 | DEPS maps) 6 | 7 | rock_testsuite(test_vectorgrid 8 | test_VectorGrid.cpp 9 | DEPS maps) 10 | 11 | rock_testsuite(test_localmap 12 | test_LocalMap.cpp 13 | DEPS maps) 14 | 15 | rock_testsuite(test_gridmap 16 | test_GridMap.cpp 17 | DEPS maps) 18 | 19 | rock_testsuite(test_mlgrid 20 | test_MLGrid.cpp 21 | DEPS maps) 22 | 23 | rock_testsuite(test_travMap3d 24 | test_TravMap3d.cpp 25 | DEPS maps) 26 | 27 | rock_testsuite(test_layeredgridmap 28 | test_LayeredGridMap.cpp 29 | DEPS maps) 30 | 31 | rock_testsuite(test_traversabilitygrid 32 | test_TraversabilityGrid.cpp 33 | DEPS maps) 34 | -------------------------------------------------------------------------------- /test/grid/test_TravMap3d.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #define BOOST_TEST_MODULE GridTest 28 | #include 29 | 30 | #include 31 | 32 | using namespace ::maps::grid; 33 | 34 | BOOST_AUTO_TEST_CASE(test_copyCast) 35 | { 36 | boost::shared_ptr data(new maps::LocalMapData()); 37 | TraversabilityMap3d *> map(Vector2ui(2,2), Eigen::Vector2d(1,1), data); 38 | 39 | Index idx(0,0); 40 | map.at(idx).insert(new TraversabilityNode(5.0, idx)); 41 | 42 | //fun fact, here the move assign ist optimized out. 43 | //smart compiler... 44 | TraversabilityBaseMap3d baseMap = map.copyCast(); 45 | 46 | BOOST_CHECK_EQUAL(map.at(idx).size(), 1); 47 | BOOST_CHECK_EQUAL(baseMap.at(idx).size(), 1); 48 | 49 | } 50 | 51 | BOOST_AUTO_TEST_CASE(test_copyCastNonStack) 52 | { 53 | boost::shared_ptr data(new maps::LocalMapData()); 54 | TraversabilityMap3d *> *map = new TraversabilityMap3d *>(Vector2ui(2,2), Eigen::Vector2d(1,1), data); 55 | 56 | Index idx(0,0); 57 | map->at(idx).insert(new TraversabilityNode(5.0, idx)); 58 | 59 | TraversabilityBaseMap3d * baseMap = new TraversabilityBaseMap3d(); 60 | //test move assignment 61 | *baseMap = map->copyCast(); 62 | 63 | BOOST_CHECK_EQUAL(map->at(idx).size(), 1); 64 | BOOST_CHECK_EQUAL(baseMap->at(idx).size(), 1); 65 | 66 | //move constructor 67 | TraversabilityBaseMap3d *baseMapCA(new TraversabilityBaseMap3d(map->copyCast())); 68 | 69 | BOOST_CHECK_EQUAL(map->at(idx).size(), 1); 70 | BOOST_CHECK_EQUAL(baseMapCA->at(idx).size(), 1); 71 | 72 | delete map; 73 | delete baseMap; 74 | delete baseMapCA; 75 | } 76 | 77 | BOOST_AUTO_TEST_CASE(test_deepCopy) 78 | { 79 | boost::shared_ptr data(new maps::LocalMapData()); 80 | TraversabilityMap3d *> map(Vector2ui(2,2), Eigen::Vector2d(1,1), data); 81 | 82 | Index idx(0,0); 83 | 84 | map.at(idx).insert(new TraversabilityNode(5.0, idx)); 85 | 86 | //copy constructor case 87 | TraversabilityMap3d *> copy(map); 88 | 89 | BOOST_CHECK_EQUAL(map.at(idx).size(), 1); 90 | BOOST_CHECK_EQUAL(copy.at(idx).size(), 1); 91 | 92 | BOOST_CHECK(*map.at(idx).begin() != *copy.at(idx).begin()); 93 | 94 | copy.clear(); 95 | BOOST_CHECK_EQUAL(copy.at(idx).size(), 0); 96 | BOOST_CHECK_EQUAL(map.at(idx).size(), 1); 97 | 98 | //assignment operator 99 | copy = map; 100 | BOOST_CHECK_EQUAL(copy.at(idx).size(), 1); 101 | 102 | BOOST_CHECK(*map.at(idx).begin() != *copy.at(idx).begin()); 103 | } 104 | 105 | -------------------------------------------------------------------------------- /test/grid/test_VectorGrid.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #define BOOST_TEST_MODULE GridTest 28 | #include 29 | 30 | #include 31 | 32 | using namespace ::maps::grid; 33 | 34 | template 35 | class VectorGridTest : public VectorGrid 36 | { 37 | public: 38 | using VectorGrid::VectorGrid; 39 | 40 | std::pair::const_iterator, typename VectorGrid::const_iterator> getRangeTest() 41 | { 42 | return this->getRange(); 43 | } 44 | }; 45 | 46 | BOOST_AUTO_TEST_CASE(test_get_range) 47 | { 48 | VectorGridTest vector_grid(Vector2ui(2, 3), -5); 49 | 50 | std::pair::const_iterator, VectorGridTest::const_iterator> range; 51 | 52 | // No elements 53 | range = vector_grid.getRangeTest(); 54 | 55 | BOOST_CHECK(range.first == vector_grid.end()); 56 | BOOST_CHECK(range.second == vector_grid.end()); 57 | 58 | // First element 59 | vector_grid.at(0, 0) = 0; 60 | 61 | range = vector_grid.getRangeTest(); 62 | 63 | BOOST_CHECK_EQUAL(*range.first, 0); 64 | BOOST_CHECK_EQUAL(*(range.second-1), 0); 65 | BOOST_CHECK(range.first == vector_grid.begin()); 66 | BOOST_CHECK(range.second == vector_grid.begin() + 1); 67 | 68 | // Last element 69 | vector_grid.at(0, 0) = -5; 70 | vector_grid.at(1, 2) = 5; 71 | 72 | range = vector_grid.getRangeTest(); 73 | 74 | BOOST_CHECK_EQUAL(*range.first, 5); 75 | BOOST_CHECK_EQUAL(*(range.second-1), 5); 76 | BOOST_CHECK(range.first == (vector_grid.end() - 1)); 77 | BOOST_CHECK(range.second == vector_grid.end()); 78 | 79 | // First and last element 80 | vector_grid.at(0, 0) = 0; 81 | 82 | range = vector_grid.getRangeTest(); 83 | 84 | BOOST_CHECK_EQUAL(*range.first, 0); 85 | BOOST_CHECK_EQUAL(*(range.second-1), 5); 86 | BOOST_CHECK(range.first == vector_grid.begin()); 87 | BOOST_CHECK(range.second == vector_grid.end()); 88 | 89 | // In the middle 90 | vector_grid.at(0, 0) = -5; 91 | vector_grid.at(1, 2) = -5; 92 | 93 | vector_grid.at(1, 0) = 1; 94 | vector_grid.at(0, 2) = 4; 95 | 96 | range = vector_grid.getRangeTest(); 97 | 98 | BOOST_CHECK_EQUAL(*range.first, 1); 99 | BOOST_CHECK_EQUAL(*(range.second-1), 4); 100 | BOOST_CHECK(range.first == (vector_grid.begin() + 1)); 101 | BOOST_CHECK(range.second == (vector_grid.end() - 1)); 102 | } 103 | -------------------------------------------------------------------------------- /test/serialization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # TEST SERIALIZATION 2 | # 3 | rock_testsuite(test_serialization_localmap 4 | test_serialization_LocalMap.cpp 5 | DEPS maps) 6 | 7 | rock_testsuite(test_serialization_grid 8 | test_serialization_Grid.cpp 9 | DEPS maps) 10 | 11 | rock_testsuite(test_serialization_geometric 12 | test_serialization_Geometric.cpp 13 | DEPS maps) 14 | 15 | rock_testsuite(test_serialization_mlsmap 16 | test_serialization_MLS.cpp 17 | DEPS maps) 18 | 19 | rock_testsuite(test_serialization_travmap 20 | test_serialization_TravMap.cpp 21 | DEPS maps) 22 | 23 | rock_testsuite(test_serialization_discreteTree 24 | test_serialization_DiscreteTree.cpp 25 | DEPS maps) 26 | 27 | # Testfiles. 28 | configure_file(${CMAKE_CURRENT_SOURCE_DIR}/DiscreteTree_v0.bin 29 | ${CMAKE_CURRENT_BINARY_DIR}/DiscreteTree_v0.bin COPYONLY) 30 | 31 | configure_file(${CMAKE_CURRENT_SOURCE_DIR}/TravMap_v00.bin 32 | ${CMAKE_CURRENT_BINARY_DIR}/TravMap_v00.bin COPYONLY) 33 | -------------------------------------------------------------------------------- /test/serialization/DiscreteTree_v0.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/envire/slam-maps/bb5c125cb264e21e8c951ddd1196256a2f1a9168/test/serialization/DiscreteTree_v0.bin -------------------------------------------------------------------------------- /test/serialization/TravMap_v00.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/envire/slam-maps/bb5c125cb264e21e8c951ddd1196256a2f1a9168/test/serialization/TravMap_v00.bin -------------------------------------------------------------------------------- /test/serialization/test_serialization_DiscreteTree.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #define BOOST_TEST_MODULE SerializationTest 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | #include 41 | #include 42 | 43 | 44 | using namespace maps::grid; 45 | 46 | BOOST_AUTO_TEST_CASE(test_serialization_discrete_tree) 47 | { 48 | std::string filename = "DiscreteTree_v1.bin"; 49 | 50 | // Save. 51 | std::ofstream output(filename, std::ios::binary); 52 | if (output.is_open()) 53 | { 54 | boost::archive::binary_oarchive oa(output); 55 | 56 | DiscreteTree tree = DiscreteTree(0.1); 57 | 58 | for (uint i = 0; i < 5; ++i) 59 | { 60 | tree.insert(std::pair(i, OccupancyPatch(i / 10.0))); 61 | } 62 | BOOST_CHECK_EQUAL(tree.size(), 5); 63 | 64 | oa << tree; 65 | } 66 | 67 | // Load. 68 | std::ifstream input(filename, std::ios::binary); 69 | if (input.is_open()) 70 | { 71 | boost::archive::binary_iarchive ia(input); 72 | 73 | DiscreteTree tree(1.0); 74 | 75 | ia >> tree; 76 | 77 | BOOST_CHECK_EQUAL(tree.size(), 5); 78 | BOOST_CHECK_CLOSE(tree.getResolution(), 0.1, 0.1); 79 | 80 | for (uint i = 0; i < 5; ++i) 81 | { 82 | BOOST_CHECK_CLOSE(tree.getCellAt(i).getPropability(), i / 10.0, 0.1); 83 | } 84 | } 85 | } 86 | 87 | BOOST_AUTO_TEST_CASE(test_serialization_discrete_tree_backwards_compatibility_v0) 88 | { 89 | // Load v0 discrete tree with: 90 | int size = 5; 91 | float resolution = 0.1; 92 | // OccupancyPatch.getPropability() is 1/10 of the index. 93 | 94 | std::string filename = "DiscreteTree_v0.bin"; 95 | 96 | std::ifstream input(filename, std::ios::binary); 97 | if (input.is_open()) 98 | { 99 | boost::archive::binary_iarchive ia(input); 100 | 101 | DiscreteTree tree(1.0); 102 | 103 | ia >> tree; 104 | 105 | BOOST_CHECK_EQUAL(tree.size(), size); 106 | BOOST_CHECK_CLOSE(tree.getResolution(), resolution, 0.1); 107 | 108 | for (uint i = 0; i < 5; ++i) 109 | { 110 | BOOST_CHECK_CLOSE(tree.getCellAt(i).getPropability(), i / 10.0, 0.1); 111 | } 112 | } 113 | } 114 | 115 | 116 | 117 | -------------------------------------------------------------------------------- /test/suite.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | 28 | // Do NOT add anything to this file 29 | // This header from boost takes ages to compile, so we make sure it is compiled 30 | // only once (here) 31 | //#define BOOST_TEST_DYN_LINK 32 | #define BOOST_TEST_MAIN 33 | #include 34 | -------------------------------------------------------------------------------- /test/tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # TEST TOOLS 2 | # 3 | rock_testsuite(test_tools_generatepoints 4 | test_tools_GeneratePoints.cpp 5 | DEPS maps) 6 | 7 | rock_testsuite(test_voxel_traversal 8 | test_voxel_traversal.cpp 9 | DEPS maps) 10 | 11 | rock_testsuite(test_surface_intersection 12 | test_surface_intersection.cpp 13 | DEPS maps) 14 | 15 | rock_testsuite(test_MLSToSlopes 16 | test_tools_MLSToSlopes.cpp 17 | DEPS maps) 18 | 19 | rock_testsuite(test_SimpleTraversability 20 | test_tools_SimpleTraversability.cpp 21 | DEPS maps) 22 | 23 | rock_testsuite(test_traversability_grassfire 24 | test_tools_TraversabilityGrassfire.cpp 25 | DEPS maps) 26 | -------------------------------------------------------------------------------- /test/tools/test_surface_intersection.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #define BOOST_TEST_MODULE ToolsTest 28 | #include 29 | 30 | #include 31 | 32 | #include 33 | 34 | #include 35 | 36 | inline bool current_test_passing() 37 | { 38 | using namespace boost::unit_test; 39 | test_case::id_t id = framework::current_test_case().p_id; 40 | test_results rez = results_collector.results(id); 41 | return rez.passed(); 42 | } 43 | 44 | 45 | BOOST_AUTO_TEST_CASE(test_surface_intersection) 46 | { 47 | typedef Eigen::Vector3d Vec; 48 | typedef Eigen::AlignedBox3d Box; 49 | typedef Eigen::Hyperplane Plane; 50 | for(int i=0; i<100000; ++i) 51 | { 52 | // generate random box: 53 | Box box(Vec::Random()); 54 | box.extend(Vec::Random()); 55 | 56 | Vec inner = box.sample(); 57 | BOOST_CHECK_SMALL(box.squaredExteriorDistance(inner), 1e-4); 58 | Vec normal = Vec::Random().normalized(); 59 | BOOST_CHECK_CLOSE(normal.squaredNorm(), 1.0, 1e-6); 60 | Plane plane(normal, inner); 61 | 62 | std::vector intersections; 63 | maps::tools::SurfaceIntersection::computeIntersections(plane, box, intersections); 64 | BOOST_CHECK_GE(intersections.size(), 3); 65 | BOOST_CHECK_LE(intersections.size(), 6); 66 | for(size_t j=0; j 29 | 30 | #include "GeneratePointclouds.hpp" 31 | 32 | 33 | using namespace ::maps; 34 | 35 | BOOST_AUTO_TEST_CASE(test_generate_PC) 36 | { 37 | typedef Eigen::Hyperplane Plane; 38 | LIDARSimulator lidar(Eigen::VectorXd::LinSpaced(20, -1.0, +1.0), Eigen::VectorXd::LinSpaced(30, -M_PI, +M_PI)); 39 | // LIDARSimulator lidar(Eigen::VectorXd::Constant(2, 0.0), Eigen::VectorXd::LinSpaced(30, -M_PI, +M_PI)); 40 | std::vector scene; 41 | for(int i=0; i<3; ++i) 42 | { 43 | scene.push_back(Plane(Eigen::Vector3d::Unit(i), 2.0)); 44 | scene.push_back(Plane(Eigen::Vector3d::Unit(i), -2.0)); 45 | } 46 | Eigen::ArrayXXd ranges; 47 | PointCloud pointcloud; 48 | Eigen::Affine3d trafo; 49 | trafo.setIdentity(); 50 | for(int k=0; k<5; ++k) 51 | { 52 | trafo.translation().setRandom(); 53 | Eigen::Quaterniond q; q.coeffs().setRandom(); q.normalize(); 54 | trafo.linear() = q.toRotationMatrix(); 55 | // std::cout << trafo.matrix() << std::endl; 56 | 57 | lidar.getRanges(ranges, scene, trafo, &pointcloud); 58 | 59 | ranges.transposeInPlace(); 60 | 61 | for(PointCloud::const_iterator it = pointcloud.begin(); it != pointcloud.end(); ++it) 62 | { 63 | // std::cout << ranges(it - pointcloud.begin()) << ",\t" << it->getVector3fMap().transpose() << std::endl; 64 | BOOST_CHECK_CLOSE((trafo * it->getVector3fMap().cast()).lpNorm(), 2.0, 0.01); 65 | } 66 | 67 | } 68 | } 69 | 70 | 71 | -------------------------------------------------------------------------------- /test/tools/test_voxel_traversal.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #define BOOST_TEST_MODULE ToolsTest 28 | #include 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | 37 | template 38 | bool checkRayElements(const Iterator& ray_start, const Iterator& ray_end, const Eigen::Vector3i& next_idx) 39 | { 40 | if(ray_start == ray_end) 41 | return true; 42 | 43 | if(next_idx.x() != ray_start->x()) 44 | return false; 45 | 46 | if(next_idx.y() != ray_start->y()) 47 | return false; 48 | 49 | if(next_idx.z() != ray_start->z()) 50 | return false; 51 | 52 | if(checkRayElements(ray_start + 1, ray_end, next_idx + Eigen::Vector3i(1,0,0)) || 53 | checkRayElements(ray_start + 1, ray_end, next_idx + Eigen::Vector3i(0,1,0)) || 54 | checkRayElements(ray_start + 1, ray_end, next_idx + Eigen::Vector3i(0,0,1)) || 55 | checkRayElements(ray_start + 1, ray_end, next_idx + Eigen::Vector3i(-1,0,0)) || 56 | checkRayElements(ray_start + 1, ray_end, next_idx + Eigen::Vector3i(0,-1,0)) || 57 | checkRayElements(ray_start + 1, ray_end, next_idx + Eigen::Vector3i(0,0,-1))) 58 | { 59 | return true; 60 | } 61 | 62 | std::cerr << "Cant find a following element from " << next_idx << std::endl; 63 | 64 | return false; 65 | } 66 | 67 | bool checkRay(const std::vector &ray) 68 | { 69 | if(ray.empty()) 70 | return false; 71 | 72 | return checkRayElements(ray.begin()+1, ray.end(), *(ray.begin()+1)); 73 | } 74 | 75 | 76 | 77 | BOOST_AUTO_TEST_CASE(test_voxel_traversal_continuity) 78 | { 79 | Eigen::Vector3d resolution(0.1,0.07367,0.05); 80 | 81 | unsigned runs = 0; 82 | Eigen::Affine3d transformation; 83 | Eigen::Quaterniond q; 84 | std::vector ray; 85 | while(true) 86 | { 87 | // generate new transformation 88 | transformation.translation().setRandom(); 89 | q.coeffs().setRandom(); 90 | q.normalize(); 91 | transformation.linear() = q.toRotationMatrix(); 92 | 93 | Eigen::Vector3d origin = transformation.translation(); 94 | Eigen::Vector3d measurement = transformation * (Eigen::Vector3d::UnitX() * 10.0); 95 | 96 | maps::tools::VoxelTraversal::computeRay(resolution, origin, measurement, ray); 97 | 98 | if(ray.empty()) 99 | { 100 | std::cerr << "Ray is empty, probably missed the last element while ray tracing." << std::endl; 101 | continue; 102 | } 103 | 104 | BOOST_CHECK(checkRay(ray)); 105 | 106 | runs++; 107 | if(runs > 1000000) 108 | break; 109 | } 110 | } 111 | -------------------------------------------------------------------------------- /test/viz/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(BEFORE ${PROJECT_SOURCE_DIR}/viz) 2 | rock_find_qt4(QtCore QtGui) 3 | 4 | rock_executable(heightmap_to_mls 5 | SOURCES 6 | mlsFromImageHeightmap.cpp 7 | HEADERS 8 | DEPS_PKGCONFIG QtCore QtGui vizkit3d vizkit3d-viz 9 | DEPS maps-viz maps) 10 | 11 | 12 | rock_testsuite(test_viz_elevationmap 13 | test_viz_ElevationMap.cpp 14 | DEPS maps-viz 15 | DEPS_PKGCONFIG vizkit3d vizkit3d-viz) 16 | 17 | rock_testsuite(test_viz_mlsmap 18 | test_viz_MLSMap.cpp 19 | DEPS maps-viz 20 | DEPS_PKGCONFIG vizkit3d vizkit3d-viz) 21 | 22 | rock_testsuite(test_viz_mlslidar 23 | test_viz_MLSLIDAR.cpp 24 | DEPS maps-viz 25 | DEPS_PKGCONFIG vizkit3d vizkit3d-viz) 26 | 27 | rock_testsuite(test_viz_countourmap 28 | test_viz_ContourMap.cpp 29 | DEPS maps-viz 30 | DEPS_PKGCONFIG vizkit3d vizkit3d-viz) 31 | 32 | rock_testsuite(test_viz_gridmap 33 | test_viz_GridMap.cpp 34 | DEPS maps-viz 35 | DEPS_PKGCONFIG vizkit3d vizkit3d-viz) 36 | 37 | rock_executable(deserialize_test 38 | DeserializeVizTest.cpp 39 | DEPS_CMAKE Boost 40 | DEPS maps-viz 41 | DEPS_PKGCONFIG vizkit3d vizkit3d-viz) 42 | 43 | rock_testsuite(test_viz_tsdf_map 44 | test_viz_TSDFMap.cpp 45 | DEPS maps-viz 46 | DEPS_PKGCONFIG vizkit3d vizkit3d-viz) 47 | 48 | rock_testsuite(test_viz_occupancylidar 49 | test_viz_OccupancyLidar.cpp 50 | DEPS maps-viz 51 | DEPS_PKGCONFIG vizkit3d vizkit3d-viz) 52 | 53 | rock_testsuite(test_viz_coverage 54 | test_viz_Coverage.cpp 55 | DEPS maps-viz 56 | DEPS_PKGCONFIG vizkit3d vizkit3d-viz) 57 | rock_testsuite(test_viz_traversabilitygrid 58 | test_viz_TraversabilityGrid.cpp 59 | DEPS maps-viz 60 | DEPS_PKGCONFIG vizkit3d vizkit3d-viz) 61 | 62 | rock_testsuite(test_viz_traversability_map3d 63 | test_viz_TraversabilityMap3d.cpp 64 | DEPS maps-viz 65 | DEPS_PKGCONFIG vizkit3d vizkit3d-viz) 66 | -------------------------------------------------------------------------------- /test/viz/DeserializeVizTest.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | int main(int argc, char **argv) { 33 | if(argc < 2) 34 | { 35 | std::cerr << "Usage " << argv[0] << " filename\n"; 36 | exit(1); 37 | } 38 | 39 | std::ifstream input(argv[1], std::ios::binary); 40 | boost::archive::binary_iarchive ia(input); 41 | maps::grid::MLSMapSloped mls; 42 | 43 | ia >> mls; 44 | 45 | maps::grid::StandaloneVisualizer viz; 46 | 47 | viz.updateData(mls); 48 | 49 | while(viz.wait(1000)) 50 | { 51 | // waiting 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /test/viz/mlsFromImageHeightmap.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "maps/grid/MLSConfig.hpp" 3 | #include "maps/grid/MLSMap.hpp" 4 | #include 5 | #include 6 | #include "StandaloneVisualizer.hpp" 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace ::maps::grid; 12 | 13 | template 14 | static void show_MLS(const MLSMap& mls) 15 | { 16 | std::cout << "update finish" << std::endl; 17 | StandaloneVisualizer app; 18 | app.updateData(mls); 19 | 20 | while (app.wait(1000)) 21 | { 22 | } 23 | } 24 | 25 | int main(int argc, char *argv[]) 26 | { 27 | 28 | if(argc <= 3) 29 | { 30 | std::cout << "Usage: heightmap_to_mls \n input = any grayscale image" << std::endl; 31 | exit(-1); 32 | } 33 | 34 | const double maxHeight = boost::lexical_cast(argv[3]); 35 | QImage img(argv[1]); 36 | 37 | const int width = img.width(); 38 | const int height = img.height(); 39 | std::cout << "Image: width: " << width << ", height: " << height << std::endl; 40 | 41 | const Eigen::Vector2d res(0.1, 0.1); 42 | const Vector2ui numCells(width, height); 43 | 44 | MLSConfig mls_config; 45 | // mls_config.updateModel = MLSConfig::SLOPE; 46 | mls_config.updateModel = MLSConfig::KALMAN; 47 | mls_config.gapSize = 0.3f; 48 | mls_config.useNegativeInformation = false; 49 | MLSMapKalman mls(numCells, res, mls_config); 50 | // 51 | for(int x = 0; x < width; ++x) 52 | { 53 | for(int y = 0; y < height; ++y) 54 | { 55 | const QRgb pixel = img.pixel(x, y); 56 | const int brightness = qGray(pixel); 57 | const double topHeight = maxHeight * brightness / 255.0; 58 | 59 | for(double height = topHeight; height > -0.3; height -= 0.2) 60 | { 61 | base::Vector3d point(res.x() / 2.0 + x * res.x(), res.y() / 2.0 + y * res.y(), height); 62 | mls.mergePoint(point); 63 | } 64 | } 65 | } 66 | 67 | std::ofstream of(argv[2], std::ios::binary); 68 | boost::archive::binary_oarchive oa(of); 69 | oa << mls; 70 | of.flush(); 71 | 72 | show_MLS(mls); 73 | 74 | 75 | 76 | return 0; 77 | } 78 | -------------------------------------------------------------------------------- /test/viz/test_viz_Coverage.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #define BOOST_TEST_MODULE VizTest 28 | #include 29 | #include 30 | 31 | #include 32 | 33 | #include "StandaloneVisualizer.hpp" 34 | #include 35 | 36 | #include 37 | 38 | using namespace ::maps::grid; 39 | 40 | template 41 | static void show_MLS(const MLSMap& mls) 42 | { 43 | std::cout << "update finish" << std::endl; 44 | StandaloneVisualizer app; 45 | app.updateData(mls); 46 | 47 | while (app.wait(1000)) 48 | { 49 | } 50 | } 51 | 52 | 53 | 54 | BOOST_AUTO_TEST_CASE(mls_coverage) 55 | { 56 | // GridConfig conf(150, 150, 0.1, 0.1, -7.5, -7.5); 57 | Eigen::Vector2d res(0.1, 0.1); 58 | Vector2ui numCells(150, 150); 59 | 60 | MLSConfig mls_config; 61 | mls_config.updateModel = MLSConfig::KALMAN; 62 | mls_config.gapSize = 0.05f; 63 | mls_config.useNegativeInformation = false; 64 | MLSMapKalman mls(numCells, res, mls_config); 65 | 66 | maps::operations::CoverageTracker coverage; 67 | 68 | 69 | mls.getLocalFrame().translation() << 0.5*mls.getSize(), 0; 70 | 71 | coverage.setFrame(mls); 72 | 73 | StandaloneVisualizer app; 74 | 75 | Eigen::ArrayXXd ranges; 76 | PointCloud pointcloud; 77 | base::Pose trafo; 78 | int loop = 0; 79 | while (app.wait(1000)) 80 | { 81 | if(++loop & 1023) continue; 82 | trafo.position +=Eigen::Vector3d::Random() * 0.5; 83 | Eigen::Quaterniond q; q.coeffs().setRandom(); q.normalize(); 84 | trafo.orientation = q; 85 | 86 | coverage.addCoverage(2.0, base::AngleSegment(), trafo); 87 | 88 | app.updateData(coverage.getCoverage()); 89 | } 90 | } 91 | 92 | -------------------------------------------------------------------------------- /test/viz/test_viz_ElevationMap.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #define BOOST_TEST_MODULE VizTest 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include "ElevationMapVisualization.hpp" 36 | #include 37 | 38 | using namespace ::maps::grid; 39 | 40 | static void showElevationMap(const ElevationMap &elev_map) 41 | { 42 | // set up test environment 43 | QtThreadedWidget app; 44 | app.start(); 45 | 46 | //create vizkit3d plugin 47 | vizkit3d::ElevationMapVisualization *elev_viz = new vizkit3d::ElevationMapVisualization(); 48 | elev_viz->updateData(elev_map); 49 | 50 | //create vizkit3d widget 51 | vizkit3d::Vizkit3DWidget *widget = app.getWidget(); 52 | // grid plugin 53 | vizkit3d::GridVisualization *grid_viz = new vizkit3d::GridVisualization(); 54 | widget->addPlugin(grid_viz); 55 | // add plugin 56 | widget->addPlugin(elev_viz); 57 | 58 | while (app.isRunning()) 59 | { 60 | usleep(1000); 61 | } 62 | } 63 | 64 | BOOST_AUTO_TEST_CASE(elevviz_test) 65 | { 66 | ElevationMap elev_map(Vector2ui(150, 250), Vector2d(0.1, 0.1)); 67 | elev_map.getLocalFrame().translation() << 0.5 * elev_map.getSize(), 0; 68 | 69 | for (unsigned int x = 10; x < elev_map.getNumCells().x() - 10; ++x) 70 | { 71 | float cs = std::cos(x * M_PI/50); 72 | for (unsigned int y = 10; y < elev_map.getNumCells().y() - 10; ++y) 73 | { 74 | float sn = std::sin(y * M_PI/50); 75 | 76 | elev_map.at(x,y) = cs * sn; 77 | //elev_map.at(x,y).elevation_min = -1 * cs * sn; 78 | } 79 | } 80 | } 81 | 82 | BOOST_AUTO_TEST_CASE(elevviz_loop) 83 | { 84 | ElevationMap elev_map(Vector2ui(150, 250), Vector2d(0.1, 0.1)); 85 | 86 | /** Translate the local frame (offset) **/ 87 | elev_map.getLocalFrame().translation() << 0.5 * elev_map.getSize(), 0; 88 | 89 | /** Equivalent to translate the grid in opposite direction **/ 90 | // Eigen::Vector3d offset_grid; 91 | // offset_grid << -0.5*elev_map.getSize(), 0.00; 92 | // elev_map.translate(offset_grid); 93 | 94 | for(unsigned int x=0; x< elev_map.getNumCells().x(); ++x ) 95 | for(unsigned int y=0; y= 0) 100 | { 101 | elev_map.at(x,y) = std::sqrt(r2); 102 | } 103 | else 104 | elev_map.at(x,y) = 0.0; 105 | } 106 | 107 | showElevationMap(elev_map); 108 | } 109 | -------------------------------------------------------------------------------- /test/viz/test_viz_MLSLIDAR.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #define BOOST_TEST_MODULE VizTest 28 | #include 29 | 30 | #include 31 | 32 | #include "StandaloneVisualizer.hpp" 33 | #include 34 | 35 | #include "../tools/GeneratePointclouds.hpp" 36 | using namespace ::maps::grid; 37 | #include 38 | 39 | 40 | BOOST_AUTO_TEST_CASE(mls_simulate_LIDAR) 41 | { 42 | // GridConfig conf(150, 150, 0.1, 0.1, -7.5, -7.5); 43 | Eigen::Vector2d res(0.125, 0.125); 44 | Vector2ui numCells(200, 200); 45 | 46 | MLSConfig mls_config; 47 | mls_config.updateModel = MLSConfig::SLOPE; 48 | // mls_config.updateModel = MLSConfig::KALMAN; 49 | mls_config.gapSize = 0.125f; 50 | mls_config.useNegativeInformation = true; 51 | MLSMapSloped *mls = new MLSMapSloped(numCells, res, mls_config); 52 | 53 | /** Translate the local frame (offset) **/ 54 | mls->getLocalFrame().translation() << 0.5*mls->getSize(), 0; 55 | 56 | /** Equivalent to translate the grid in opposite direction **/ 57 | //Eigen::Vector3d offset_grid; 58 | //offset_grid << -0.5*mls->getSize(), 0.00; 59 | //mls->translate(offset_grid); 60 | 61 | typedef Eigen::Hyperplane Plane; 62 | maps::LIDARSimulator lidar(Eigen::VectorXd::LinSpaced(32, -16*M_PI/180, +16*M_PI/180), Eigen::VectorXd::LinSpaced(360, -M_PI, +M_PI)); 63 | // maps::LIDARSimulator lidar(Eigen::VectorXd::Constant(2, 0.0), Eigen::VectorXd::LinSpaced(30, -M_PI, +M_PI)); 64 | std::vector scene; 65 | { 66 | Eigen::Quaterniond q; q.coeffs().setRandom(); q.normalize(); 67 | 68 | for(int i=0; i<3; ++i) 69 | { 70 | scene.push_back(Plane(q * Eigen::Vector3d::Unit(i), -2.0)); 71 | scene.push_back(Plane(q * Eigen::Vector3d::Unit(i), 2.0)); 72 | } 73 | } 74 | 75 | StandaloneVisualizer app; 76 | 77 | Eigen::ArrayXXd ranges; 78 | PointCloud pointcloud; 79 | Eigen::Affine3d trafo; 80 | trafo.setIdentity(); 81 | int loop = 0; 82 | while (app.wait(1000)) 83 | { 84 | if(++loop & 1023) continue; 85 | trafo.translation().setRandom(); 86 | Eigen::Quaterniond q; q.coeffs().setRandom(); q.normalize(); 87 | trafo.linear() = q.toRotationMatrix(); 88 | // std::cout << trafo.matrix() << std::endl; 89 | 90 | lidar.getRanges(ranges, scene, trafo, &pointcloud); 91 | 92 | base::TimeMark timer("mls->mergePointCloud"); 93 | mls->mergePointCloud(pointcloud, trafo); 94 | std::cout << timer << std::endl; 95 | app.updateData(*mls); 96 | } 97 | 98 | } 99 | -------------------------------------------------------------------------------- /test/viz/test_viz_OccupancyLidar.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #define BOOST_TEST_MODULE VizTest 28 | #include 29 | 30 | #include 31 | 32 | #include "StandaloneVisualizer.hpp" 33 | #include 34 | 35 | #include "../tools/GeneratePointclouds.hpp" 36 | using namespace ::maps::grid; 37 | #include 38 | 39 | 40 | BOOST_AUTO_TEST_CASE(occupancy_simulate_LIDAR) 41 | { 42 | Eigen::Vector3d res(0.125, 0.125, 0.05); 43 | Vector2ui numCells(200, 200); 44 | 45 | OccupancyConfiguration config; 46 | OccupancyGridMap *grid = new OccupancyGridMap(numCells, res, config); 47 | 48 | /** Translate the local frame (offset) **/ 49 | grid->getLocalFrame().translation() << 0.5*grid->getSize(), 0; 50 | 51 | typedef Eigen::Hyperplane Plane; 52 | maps::LIDARSimulator lidar(Eigen::VectorXd::LinSpaced(32, -16*M_PI/180, +16*M_PI/180), Eigen::VectorXd::LinSpaced(360, -M_PI, +M_PI)); 53 | // maps::LIDARSimulator lidar(Eigen::VectorXd::Constant(2, 0.0), Eigen::VectorXd::LinSpaced(30, -M_PI, +M_PI)); 54 | std::vector scene; 55 | { 56 | Eigen::Quaterniond q; q.coeffs().setRandom(); q.normalize(); 57 | 58 | for(int i=0; i<3; ++i) 59 | { 60 | scene.push_back(Plane(q * Eigen::Vector3d::Unit(i), -2.0)); 61 | scene.push_back(Plane(q * Eigen::Vector3d::Unit(i), 2.0)); 62 | } 63 | } 64 | 65 | StandaloneVisualizer app; 66 | 67 | Eigen::ArrayXXd ranges; 68 | PointCloud pointcloud; 69 | pointcloud.sensor_origin_ = Eigen::Vector4f::Zero(); 70 | Eigen::Affine3d trafo; 71 | trafo.setIdentity(); 72 | int loop = 0; 73 | while (app.wait(1000)) 74 | { 75 | if(++loop & 1023) continue; 76 | trafo.translation().setRandom(); 77 | Eigen::Quaterniond q; q.coeffs().setRandom(); q.normalize(); 78 | trafo.linear() = q.toRotationMatrix(); 79 | // std::cout << trafo.matrix() << std::endl; 80 | 81 | lidar.getRanges(ranges, scene, trafo, &pointcloud); 82 | 83 | base::TimeMark timer("grid->mergePointCloud"); 84 | grid->mergePointCloud(pointcloud, trafo); 85 | std::cout << timer << std::endl; 86 | app.updateData(*grid); 87 | } 88 | 89 | delete grid; 90 | } 91 | -------------------------------------------------------------------------------- /test/viz/test_viz_TSDFMap.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #define BOOST_TEST_MODULE VizTest 28 | #include 29 | 30 | #include 31 | 32 | #include "StandaloneVisualizer.hpp" 33 | #include 34 | #include 35 | 36 | using namespace ::maps::grid; 37 | 38 | BOOST_AUTO_TEST_CASE(tsdf_moving_waves) 39 | { 40 | Vector3d res(0.2, 0.2, 0.2); 41 | Vector2ui numCells(40, 40); 42 | 43 | StandaloneVisualizer app; 44 | 45 | TSDFVolumetricMap::Ptr grid(new TSDFVolumetricMap(numCells, res)); 46 | 47 | /** Translate the local frame (offset) **/ 48 | grid->getLocalFrame().translation() << 0.5*grid->getSize(), 0; 49 | 50 | maps::tools::TSDF_MLSMapReconstruction mls_reconstruction; 51 | mls_reconstruction.setTSDFMap(grid); 52 | 53 | Eigen::Vector2d max = 0.375 * grid->getSize(); 54 | Eigen::Vector2d min = -0.375 * grid->getSize(); 55 | 56 | double x_shift = 0.; 57 | while (app.wait(200)) 58 | { 59 | x_shift += 0.01; 60 | for (double x = min.x(); x < max.x(); x += 0.05) 61 | { 62 | double cs = std::cos(x_shift + x * M_PI/2.5); 63 | for (double y = min.y(); y < max.y(); y += 0.05) 64 | { 65 | double sn = std::sin(y * M_PI/2.5); 66 | try 67 | { 68 | grid->mergePoint(Eigen::Vector3d(x, y, 4.) + Eigen::Vector3d::Random(), Eigen::Vector3d(x, y, cs*sn)); 69 | } 70 | catch(std::runtime_error &e) 71 | { 72 | std::cerr << "Caught exception: " << e.what() << std::endl; 73 | } 74 | } 75 | } 76 | 77 | maps::grid::MLSMapPrecalculated mls; 78 | mls_reconstruction.reconstruct(mls); 79 | app.updateData(mls); 80 | } 81 | } 82 | -------------------------------------------------------------------------------- /test/viz/test_viz_TraversabilityMap3d.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #define BOOST_TEST_MODULE VizTest 28 | #include 29 | 30 | #include 31 | 32 | #include "StandaloneVisualizer.hpp" 33 | #include 34 | 35 | using namespace ::maps::grid; 36 | 37 | BOOST_AUTO_TEST_CASE(traversability_map3d_viz_test) 38 | { 39 | StandaloneVisualizer app; 40 | 41 | TraversabilityMap3d *> map; 42 | 43 | map.setResolution(Eigen::Vector2d(0.5, 0.5)); 44 | map.resize(Vector2ui(20, 20)); 45 | 46 | map.getLocalFrame().translation() << 0.5*map.getSize(), 0; 47 | 48 | TraversabilityNode *child1 = new TraversabilityNode(0.0, Index(1,2)); 49 | map.at(child1->getIndex()).insert(child1); 50 | TraversabilityNode *child2 = new TraversabilityNode(1.0, Index(1,3)); 51 | map.at(child2->getIndex()).insert(child2); 52 | TraversabilityNode *child3 = new TraversabilityNode(2.0, Index(1,4)); 53 | map.at(child3->getIndex()).insert(child3); 54 | 55 | TraversabilityNode *parentNode = new TraversabilityNode(12.0, Index(1,10)); 56 | map.at(parentNode->getIndex()).insert(parentNode); 57 | 58 | child1->setType(TraversabilityNodeBase::FRONTIER); 59 | child1->setType(TraversabilityNodeBase::HOLE); 60 | child1->setType(TraversabilityNodeBase::OBSTACLE); 61 | parentNode->setType(TraversabilityNodeBase::TRAVERSABLE); 62 | 63 | parentNode->addConnection(child1); 64 | child1->addConnection(parentNode); 65 | 66 | parentNode->addConnection(child2); 67 | child2->addConnection(parentNode); 68 | 69 | parentNode->addConnection(child3); 70 | child3->addConnection(parentNode); 71 | 72 | TraversabilityMap3d base_map = map.copyCast(); 73 | 74 | while (app.wait(1000)) 75 | { 76 | app.updateData(base_map); 77 | } 78 | } 79 | -------------------------------------------------------------------------------- /viz/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | pkg_check_modules(OSGVIZ_PRIMITIVES PrimitivesFactory) 2 | pkg_check_modules(OSG REQUIRED openscenegraph) 3 | 4 | if(${OSG_VERSION} VERSION_GREATER_EQUAL 3.6.4) 5 | add_compile_definitions(NEW_OSG) 6 | endif() 7 | 8 | if (vizkit3d_FOUND AND OSGVIZ_PRIMITIVES_FOUND) 9 | rock_find_qt4() 10 | rock_vizkit_plugin(maps-viz 11 | PluginLoader.cpp 12 | PatchesGeode.cpp 13 | SurfaceGeode.cpp 14 | StandaloneVisualizer.cpp 15 | MOC 16 | GridMapVisualization.cpp 17 | ElevationMapVisualization.cpp 18 | MLSMapVisualization.cpp 19 | TraversabilityMap3dVisualization.cpp 20 | ContourMapVisualization.cpp 21 | OccupancyGridMapVisualization.cpp 22 | TraversabilityGridVisualization.cpp 23 | HEADERS 24 | ColorGradient.hpp 25 | ExtentsRectangle.hpp 26 | MapVisualization.hpp 27 | GridMapVisualization.hpp 28 | ElevationMapVisualization.hpp 29 | MLSMapVisualization.hpp 30 | TraversabilityMap3dVisualization.hpp 31 | PatchesGeode.hpp 32 | SurfaceGeode.hpp 33 | StandaloneVisualizer.hpp 34 | ContourMapVisualization.hpp 35 | OccupancyGridMapVisualization.hpp 36 | TraversabilityGridVisualization.hpp 37 | DEPS 38 | maps 39 | DEPS_PKGCONFIG 40 | base-logging 41 | base-viz vizkit3d vizkit3d-viz 42 | PrimitivesFactory 43 | ) 44 | else() 45 | message(STATUS "osgviz not found ... NOT building the maps-viz plugins") 46 | endif() 47 | 48 | -------------------------------------------------------------------------------- /viz/ContourMapVisualization.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #pragma once 28 | #include 29 | 30 | /** Base types **/ 31 | #include 32 | 33 | /** OSG **/ 34 | #include 35 | 36 | /** Boost **/ 37 | #include 38 | 39 | /** The type to visualize **/ 40 | #if QT_VERSION >= 0x050000 || !defined(Q_MOC_RUN) 41 | #include 42 | #include 43 | #include 44 | #endif 45 | 46 | /** Std **/ 47 | #include 48 | 49 | namespace vizkit3d 50 | { 51 | 52 | class ContourMapVisualization 53 | : public vizkit3d::MapVisualization<::maps::geometric::ContourMap> 54 | { 55 | Q_OBJECT 56 | Q_PROPERTY(double LineWidth READ getLineWidth WRITE setLineWidth) 57 | Q_PROPERTY(QColor Color READ getColor WRITE setColor) 58 | 59 | public: 60 | ContourMapVisualization(); 61 | virtual ~ContourMapVisualization(); 62 | 63 | Q_INVOKABLE void updateData(::maps::geometric::ContourMap const &sample) 64 | { 65 | vizkit3d::Vizkit3DPlugin<::maps::geometric::ContourMap>::updateData(sample); 66 | } 67 | 68 | void setColor(const base::Vector3d& color); 69 | void setColor(double r, double g, double b, double a); 70 | 71 | public slots: 72 | double getLineWidth(); 73 | void setLineWidth(double line_width); 74 | void setColor(QColor color); 75 | QColor getColor() const; 76 | 77 | protected: 78 | virtual osg::ref_ptr createMainNode(); 79 | virtual void updateMainNode( osg::Node* node ); 80 | virtual void updateDataIntern( const ::maps::geometric::ContourMap & data ); 81 | 82 | private: 83 | /** Internal storage **/ 84 | ::maps::geometric::ContourMap contour_lines; 85 | 86 | /** OSG drawable storage **/ 87 | osg::ref_ptr line_points; 88 | osg::ref_ptr draw_arrays; 89 | osg::ref_ptr geom; 90 | osg::ref_ptr geode; 91 | osg::ref_ptr color_array; 92 | 93 | /** Property color attribute **/ 94 | osg::Vec4 color; 95 | 96 | /** Property line width attribute **/ 97 | double line_width; 98 | 99 | }; 100 | } 101 | -------------------------------------------------------------------------------- /viz/ElevationMapVisualization.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #ifndef maps_ElevationGridVisualization_H 28 | #define maps_ElevationGridVisualization_H 29 | 30 | #include 31 | #include 32 | 33 | #include "ColorGradient.hpp" 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #if QT_VERSION >= 0x050000 || !defined(Q_MOC_RUN) 40 | #include 41 | #endif 42 | 43 | // TODO: should be replace by GridMapVisualization, 44 | // since the ElevationMap is specification of GridMap 45 | 46 | namespace vizkit3d 47 | { 48 | class ElevationMapVisualization 49 | : public vizkit3d::MapVisualization<::maps::grid::ElevationMap> 50 | { 51 | Q_OBJECT 52 | 53 | Q_PROPERTY(bool showMapExtents READ areMapExtentsShown WRITE setShowMapExtents) 54 | 55 | public: 56 | ElevationMapVisualization(); 57 | ~ElevationMapVisualization(); 58 | 59 | Q_INVOKABLE void updateData(::maps::grid::ElevationMap const &sample) 60 | {vizkit3d::Vizkit3DPlugin<::maps::grid::ElevationMap>::updateData(sample);} 61 | 62 | protected: 63 | virtual osg::ref_ptr createMainNode(); 64 | virtual void updateMainNode(osg::Node* node); 65 | virtual void updateDataIntern(::maps::grid::ElevationMap const& plan); 66 | 67 | private: 68 | struct Data; 69 | Data* p; 70 | 71 | osg::ref_ptr geode; 72 | 73 | osg::HeightField* createHeighField(); 74 | 75 | osg::Image* createTextureImage(); 76 | 77 | ColorGradient heatMapGradient; 78 | 79 | public slots: 80 | }; 81 | } 82 | #endif 83 | -------------------------------------------------------------------------------- /viz/ExtentsRectangle.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #ifndef EXTENTSRECTANGLE_H 28 | #define EXTENTSRECTANGLE_H 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | 37 | class ExtentsRectangle : public osg::Geode 38 | { 39 | osg::ref_ptr geom; 40 | osg::ref_ptr color; 41 | osg::ref_ptr vertices; 42 | 43 | public: 44 | ExtentsRectangle( const Eigen::Vector2d& min, const Eigen::Vector2d& max, 45 | const osg::Vec4& col = osg::Vec4( 0.0f, 0.9f, 0.1f, 0.8f ) ) 46 | : geom( new osg::Geometry() ), 47 | color( new osg::Vec4Array() ), 48 | vertices( new osg::Vec3Array() ) 49 | { 50 | vertices->push_back( osg::Vec3( min.x(), min.y(), 0 )); 51 | vertices->push_back( osg::Vec3( min.x(), max.y(), 0 )); 52 | vertices->push_back( osg::Vec3( max.x(), max.y(), 0 )); 53 | vertices->push_back( osg::Vec3( max.x(), min.y(), 0 )); 54 | 55 | geom->setVertexArray(vertices); 56 | osg::ref_ptr drawArrays = 57 | new osg::DrawArrays( osg::PrimitiveSet::LINE_LOOP, 0, vertices->size() ); 58 | geom->addPrimitiveSet(drawArrays.get()); 59 | 60 | color->push_back( col ); 61 | geom->setColorArray(color.get()); 62 | geom->setColorBinding( osg::Geometry::BIND_OVERALL ); 63 | 64 | addDrawable(geom.get()); 65 | 66 | osg::StateSet* ss = getOrCreateStateSet(); 67 | ss->setMode( GL_LIGHTING, osg::StateAttribute::OFF ); 68 | ss->setAttribute( new osg::LineWidth( 3.0 ) ); 69 | } 70 | 71 | void update(const Eigen::Vector2d& min, const Eigen::Vector2d& max) 72 | { 73 | vertices->at(0) = osg::Vec3( min.x(), min.y(), 0 ); 74 | vertices->at(1) = osg::Vec3( min.x(), max.y(), 0 ); 75 | vertices->at(2) = osg::Vec3( max.x(), max.y(), 0 ); 76 | vertices->at(3) = osg::Vec3( max.x(), min.y(), 0 ); 77 | 78 | geom->setVertexArray(vertices); 79 | } 80 | }; 81 | 82 | #endif // EXTENTSRECTANGLE_H 83 | -------------------------------------------------------------------------------- /viz/OccupancyGridMapVisualization.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #pragma once 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #if QT_VERSION >= 0x050000 || !defined(Q_MOC_RUN) 36 | #include 37 | #endif 38 | 39 | namespace vizkit3d 40 | { 41 | 42 | class OccupancyGridMapVisualization : public vizkit3d::MapVisualization< ::maps::grid::OccupancyGridMap > 43 | { 44 | Q_OBJECT 45 | 46 | Q_PROPERTY(bool showMapExtents READ areMapExtentsShown WRITE setShowMapExtents) 47 | Q_PROPERTY(bool ShowOccupied READ getShowOccupied WRITE setShowOccupied) 48 | Q_PROPERTY(bool ShowFreespace READ getShowFreespace WRITE setShowFreespace) 49 | Q_PROPERTY(QColor OccupiedCellColor READ getOccupiedCellColor WRITE setOccupiedCellColor) 50 | Q_PROPERTY(QColor FreeSpaceCellColor READ getFreeSpaceCellColor WRITE setFreeSpaceCellColor) 51 | 52 | public: 53 | OccupancyGridMapVisualization(); 54 | ~OccupancyGridMapVisualization(); 55 | 56 | Q_INVOKABLE void updateData(::maps::grid::OccupancyGridMap const &sample) 57 | {vizkit3d::Vizkit3DPlugin<::maps::grid::OccupancyGridMap>::updateData(sample);} 58 | 59 | public slots: 60 | bool getShowOccupied() {return show_occupied;} 61 | void setShowOccupied(bool b) {show_occupied = b; emit propertyChanged("ShowOccupied"); setDirty();} 62 | bool getShowFreespace() {return show_freespace;} 63 | void setShowFreespace(bool b) {show_freespace = b; emit propertyChanged("ShowFreespace"); setDirty();} 64 | QColor getOccupiedCellColor() const {return occupied_color;} 65 | void setOccupiedCellColor(QColor color) {occupied_color = color; emit propertyChanged("OccupiedCellColor");} 66 | QColor getFreeSpaceCellColor() const {return free_space_color;} 67 | void setFreeSpaceCellColor(QColor color) {free_space_color = color; emit propertyChanged("FreeSpaceCellColor");} 68 | 69 | protected: 70 | virtual osg::ref_ptr createMainNode(); 71 | virtual void updateMainNode(osg::Node* node); 72 | virtual void updateDataIntern(::maps::grid::OccupancyGridMap const& grid); 73 | 74 | private: 75 | osg::ref_ptr localNode; 76 | 77 | maps::grid::OccupancyGridMap grid; 78 | bool show_occupied; 79 | bool show_freespace; 80 | QColor occupied_color; 81 | QColor free_space_color; 82 | }; 83 | 84 | } 85 | -------------------------------------------------------------------------------- /viz/PluginLoader.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #include 28 | #include "ContourMapVisualization.hpp" 29 | #include "ElevationMapVisualization.hpp" 30 | #include "GridMapVisualization.hpp" 31 | #include "MLSMapVisualization.hpp" 32 | #include "TraversabilityMap3dVisualization.hpp" 33 | #include "OccupancyGridMapVisualization.hpp" 34 | #include "TraversabilityGridVisualization.hpp" 35 | 36 | namespace vizkit3d { 37 | class QtPluginVizkitMaps : public vizkit3d::VizkitPluginFactory 38 | { 39 | public: 40 | 41 | QtPluginVizkitMaps() {} 42 | 43 | /** 44 | * Returns a list of all available visualization plugins. 45 | * @return list of plugin names 46 | */ 47 | virtual QStringList* getAvailablePlugins() const 48 | { 49 | QStringList *pluginNames = new QStringList(); 50 | pluginNames->push_back("ContourMapVisualization"); 51 | pluginNames->push_back("ElevationMapVisualization"); 52 | pluginNames->push_back("GridMapVisualization"); 53 | pluginNames->push_back("MLSMapVisualization"); 54 | pluginNames->push_back("TraversabilityMap3dVisualization"); 55 | pluginNames->push_back("OccupancyGridMapVisualization"); 56 | pluginNames->push_back("TraversabilityGridVisualization"); 57 | return pluginNames; 58 | } 59 | 60 | virtual QObject* createPlugin(QString const& pluginName) 61 | { 62 | vizkit3d::VizPluginBase* plugin = 0; 63 | if (pluginName == "ContourMapVisualization") 64 | { 65 | plugin = new vizkit3d::ContourMapVisualization(); 66 | } 67 | else if (pluginName == "ElevationMapVisualization") 68 | { 69 | plugin = new vizkit3d::ElevationMapVisualization(); 70 | } 71 | else if (pluginName == "GridMapVisualization") 72 | { 73 | plugin = new vizkit3d::GridMapVisualization(); 74 | } 75 | else if (pluginName == "MLSMapVisualization") 76 | { 77 | plugin = new vizkit3d::MLSMapVisualization(); 78 | } 79 | else if (pluginName == "TraversabilityMap3dVisualization") 80 | { 81 | plugin = new vizkit3d::TraversabilityMap3dVisualization(); 82 | } 83 | else if (pluginName == "OccupancyGridMapVisualization") 84 | { 85 | plugin = new OccupancyGridMapVisualization(); 86 | } 87 | else if (pluginName == "TraversabilityGridVisualization") 88 | { 89 | plugin = new TraversabilityGridVisualization(); 90 | } 91 | 92 | if (plugin) 93 | { 94 | return plugin; 95 | } 96 | return NULL; 97 | }; 98 | }; 99 | Q_EXPORT_PLUGIN2(QtPluginVizkitMaps, QtPluginVizkitMaps) 100 | } 101 | -------------------------------------------------------------------------------- /viz/StandaloneVisualizer.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | 28 | #include 29 | #include 30 | #include "MLSMapVisualization.hpp" 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include "StandaloneVisualizer.hpp" 36 | 37 | 38 | namespace maps { namespace grid 39 | { 40 | 41 | class StandaloneVisualizer::Impl 42 | { 43 | friend class StandaloneVisualizer; 44 | QtThreadedWidget app; 45 | vizkit3d::MLSMapVisualization *mls_viz; 46 | vizkit3d::OccupancyGridMapVisualization *occ_viz; 47 | vizkit3d::TraversabilityGridVisualization *trav_viz; 48 | vizkit3d::TraversabilityMap3dVisualization *trav_3d_viz; 49 | 50 | Impl() 51 | { 52 | app.start(); 53 | 54 | //create vizkit3d plugin 55 | mls_viz = new vizkit3d::MLSMapVisualization(); 56 | 57 | //create vizkit3d widget 58 | vizkit3d::Vizkit3DWidget *widget = app.getWidget(); 59 | // grid plugin 60 | vizkit3d::GridVisualization *grid_viz = new vizkit3d::GridVisualization(); 61 | widget->addPlugin(grid_viz); 62 | // add plugin 63 | widget->addPlugin(mls_viz); 64 | 65 | occ_viz = new vizkit3d::OccupancyGridMapVisualization(); 66 | widget->addPlugin(occ_viz); 67 | 68 | trav_viz = new vizkit3d::TraversabilityGridVisualization(); 69 | widget->addPlugin(trav_viz); 70 | 71 | trav_3d_viz = new vizkit3d::TraversabilityMap3dVisualization(); 72 | widget->addPlugin(trav_3d_viz); 73 | } 74 | }; 75 | 76 | 77 | StandaloneVisualizer::StandaloneVisualizer() 78 | : impl(new Impl()) 79 | { 80 | 81 | } 82 | 83 | StandaloneVisualizer::~StandaloneVisualizer() 84 | { 85 | // TODO Auto-generated destructor stub 86 | } 87 | 88 | void StandaloneVisualizer::updateData(const MLSMapKalman& mls) 89 | { 90 | impl->mls_viz->updateData(mls); 91 | } 92 | void StandaloneVisualizer::updateData(const MLSMapSloped& mls) 93 | { 94 | impl->mls_viz->updateMLSSloped(mls); 95 | } 96 | void StandaloneVisualizer::updateData(const MLSMapPrecalculated& mls) 97 | { 98 | impl->mls_viz->updateMLSPrecalculated(mls); 99 | } 100 | 101 | void StandaloneVisualizer::updateData(const MLSMapBase& mls) 102 | { 103 | impl->mls_viz->updateMLSBase(mls); 104 | } 105 | 106 | void StandaloneVisualizer::updateData(const OccupancyGridMap& grid) 107 | { 108 | impl->occ_viz->updateData(grid); 109 | } 110 | 111 | void StandaloneVisualizer::updateData(const TraversabilityGrid& travGrid) 112 | { 113 | impl->trav_viz->updateData(travGrid); 114 | } 115 | 116 | void StandaloneVisualizer::updateData(const maps::grid::TraversabilityMap3d< maps::grid::TraversabilityNodeBase* >& travGrid) 117 | { 118 | impl->trav_3d_viz->updateData(travGrid); 119 | } 120 | 121 | bool StandaloneVisualizer::wait(int usecs) 122 | { 123 | return impl->app.isRunning() && (!usleep(usecs)); 124 | } 125 | 126 | } /* namespace grid */ 127 | } /* namespace maps */ 128 | -------------------------------------------------------------------------------- /viz/StandaloneVisualizer.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #ifndef __MAPS_VIZ_STANDALONEVISUALIZER_HPP_ 28 | #define __MAPS_VIZ_STANDALONEVISUALIZER_HPP_ 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | namespace maps{ namespace grid 38 | { 39 | 40 | class StandaloneVisualizer 41 | { 42 | class Impl; 43 | boost::scoped_ptr impl; 44 | public: 45 | StandaloneVisualizer(); 46 | ~StandaloneVisualizer(); 47 | 48 | bool wait(int usecs = 1000); 49 | 50 | void updateData(const ::maps::grid::MLSMapKalman& mls); 51 | void updateData(const ::maps::grid::MLSMapSloped& mls); 52 | void updateData(const ::maps::grid::MLSMapPrecalculated& mls); 53 | void updateData(const ::maps::grid::MLSMapBase& mls); 54 | void updateData(const ::maps::grid::OccupancyGridMap& grid); 55 | void updateData(const ::maps::grid::TraversabilityGrid& travGrid); 56 | void updateData(const ::maps::grid::TraversabilityMap3d& travGrid); 57 | 58 | }; 59 | 60 | } /* namespace grid */ 61 | } /* namespace maps */ 62 | 63 | #endif /* __MAPS_VIZ_STANDALONEVISUALIZER_HPP_ */ 64 | -------------------------------------------------------------------------------- /viz/SurfaceGeode.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #ifndef __VIZKIT_SURFACEGEODE_HPP__ 28 | #define __VIZKIT_SURFACEGEODE_HPP__ 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | namespace vizkit3d { 38 | 39 | class SurfaceGeode : public osg::Geode 40 | { 41 | public: 42 | SurfaceGeode(float x_res, float y_res); 43 | 44 | void setColor(const osg::Vec4& color); 45 | void setColorHSVA(float hue, float sat, float lum, float alpha); 46 | 47 | void showCycleColor(bool cycle_color); 48 | void setCycleColorInterval(float cycle_color_interval); 49 | void setUncertaintyScale(double uncertainty_scale); 50 | 51 | void setShowPatchExtents(bool enable = true) { showPatchExtents = enable; }; 52 | void setShowNormals(bool enable = true) { showNormals = enable; }; 53 | void setShowUncertainty(bool enable = true) { showUncertainty = enable; } 54 | void drawLines(); 55 | 56 | 57 | void addVertex(const osg::Vec3& p, const osg::Vec3& n, const float & stdev = 0.f); 58 | 59 | void closeTriangleStrip(){ 60 | geom->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::TRIANGLE_STRIP,vertex_index,vertices->size()-vertex_index)); 61 | vertex_index = vertices->size(); 62 | } 63 | #ifdef NEW_OSG 64 | inline osg::ref_ptr getGeometry(){ 65 | return geom; 66 | } 67 | #endif 68 | 69 | private: 70 | osg::ref_ptr vertices; 71 | osg::ref_ptr normals; 72 | osg::ref_ptr colors; 73 | osg::ref_ptr geom; 74 | 75 | osg::ref_ptr var_vertices; 76 | 77 | size_t vertex_index; 78 | 79 | float xp, yp; // position of current patch 80 | float xs, ys; // grid resolution 81 | 82 | float hue; 83 | float sat; 84 | float alpha; 85 | float lum; 86 | osg::Vec4 color; 87 | 88 | bool showNormals; 89 | bool showPatchExtents; 90 | bool showUncertainty; 91 | bool cycle_color; 92 | float cycle_color_interval; 93 | double uncertaintyScale; 94 | 95 | 96 | void updateColor(); 97 | 98 | }; 99 | 100 | } 101 | 102 | #endif // __VIZKIT_PATCHESGEODE_HPP__ 103 | -------------------------------------------------------------------------------- /viz/TraversabilityMap3dVisualization.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH. 3 | // Copyright (c) 2015-2017, University of Bremen 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | // 27 | #pragma once 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #if QT_VERSION >= 0x050000 || !defined(Q_MOC_RUN) 36 | #include "maps/grid/TraversabilityMap3d.hpp" 37 | #endif 38 | 39 | namespace vizkit3d 40 | { 41 | 42 | class TraversabilityMap3dVisualization 43 | : public vizkit3d::MapVisualization<::maps::grid::TraversabilityMap3d> 44 | { 45 | Q_OBJECT 46 | 47 | Q_PROPERTY(bool showMapExtents READ areMapExtentsShown WRITE setShowMapExtents) 48 | Q_PROPERTY(double isoline_interval READ getIsolineInterval WRITE setIsolineInterval) 49 | Q_PROPERTY(bool show_connections READ getShowConnections WRITE setShowConnections) 50 | 51 | protected: 52 | virtual void updateDataIntern(const ::maps::grid::TraversabilityMap3d& data); 53 | virtual void updateMainNode(osg::Node* node); 54 | 55 | virtual osg::ref_ptr< osg::Node > createMainNode(); 56 | 57 | ::maps::grid::TraversabilityMap3d map; 58 | 59 | void addNodeList(const ::maps::grid::LevelList<::maps::grid::TraversabilityNodeBase *> &l, osg::Group* group); 60 | 61 | void visualizeNode(const ::maps::grid::TraversabilityNodeBase *node); 62 | void visualizeConnection(const ::maps::grid::TraversabilityNodeBase *from, const ::maps::grid::TraversabilityNodeBase *to); 63 | 64 | osg::ref_ptr nodeGeode; 65 | osg::ref_ptr linesNode; 66 | 67 | osg::Group* connectionGroup; 68 | 69 | double isoline_interval; 70 | bool show_connections; 71 | 72 | public: 73 | TraversabilityMap3dVisualization(); 74 | virtual ~TraversabilityMap3dVisualization(); 75 | 76 | Q_INVOKABLE void updateData(::maps::grid::TraversabilityBaseMap3d const &sample) 77 | { 78 | vizkit3d::Vizkit3DPlugin<::maps::grid::TraversabilityMap3d>::updateData(sample); 79 | } 80 | 81 | Q_INVOKABLE void updateTrMap(::maps::grid::TraversabilityBaseMap3d const &sample) 82 | { 83 | vizkit3d::Vizkit3DPlugin<::maps::grid::TraversabilityMap3d>::updateData(sample); 84 | } 85 | 86 | double getIsolineInterval() const { return isoline_interval; } 87 | void setIsolineInterval(const double& val); 88 | 89 | bool getShowConnections(); 90 | void setShowConnections(bool val); 91 | 92 | private: 93 | osg::ref_ptr localNode; 94 | }; 95 | 96 | } 97 | -------------------------------------------------------------------------------- /viz/maps-viz.pc.in: -------------------------------------------------------------------------------- 1 | prefix=@CMAKE_INSTALL_PREFIX@ 2 | exec_prefix=@CMAKE_INSTALL_PREFIX@ 3 | libdir=${prefix}/lib 4 | includedir=${prefix}/include 5 | 6 | Name: maps-viz 7 | Description: vizKit plugin for the maps library 8 | Requires: maps @PKGCONFIG_DEPS@ 9 | Version: @PROJECT_VERSION@ 10 | Libs: -L${libdir} -lmaps-viz 11 | Cflags: -I${includedir} 12 | 13 | --------------------------------------------------------------------------------