diff --git a/CMakeLists.txt b/CMakeLists.txt index ab89d891..c01d4022 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,24 +6,6 @@ ENABLE_TESTING() # enable CTest environment of subprojects ADD_SUBDIRECTORY( octomap ) ADD_SUBDIRECTORY( octovis ) ADD_SUBDIRECTORY( dynamicEDT3D ) - -# make complete package release from source -IF (NOT WIN32) - SET(PKG_NAME "${PROJECT_NAME}.tar.gz") - SET(DIST_DIR "${CMAKE_BINARY_DIR}/dist") - ADD_CUSTOM_TARGET("dist" - rm -rf "${DIST_DIR}" "${CMAKE_BINARY_DIR}/${PKG_NAME}" - COMMAND mkdir "${DIST_DIR}" - COMMAND svn export --force -q "${PROJECT_SOURCE_DIR}" "${DIST_DIR}/${PROJECT_NAME}" - COMMAND tar -czf "${CMAKE_BINARY_DIR}/${PKG_NAME}" -C "${DIST_DIR}" --exclude=".hidden" "${PROJECT_NAME}" - WORKING_DIRECTORY "${PROJECT_SOURCE_DIR}" - # cleanup so that there is no copy in the source dir - COMMAND rm -rf "${DIST_DIR}" - ) - - # also build subproject's dist for completeness - ADD_DEPENDENCIES(dist dist-octomap dist-octovis) -ENDIF() diff --git a/README.md b/README.md new file mode 100644 index 00000000..fa7958d6 --- /dev/null +++ b/README.md @@ -0,0 +1,64 @@ +OctoMap - A probabilistic, flexible, and compact 3D mapping library for robotic systems. +======================================================================================== + + +Authors: Kai M. Wurm and Armin Hornung, University of Freiburg, Copyright (C) 2009-2013. +http://octomap.github.com + +Further Contributors: +* C. Sprunk, University of Freiburg +* J. Mueller, University of Freiburg +* S. Osswald, University of Freiburg +* R. Schmitt, University of Freiburg +* R. Bogdan Rusu, Willow Garage Inc. +* C. Dornhege, University of Freiburg + +License: + * octomap: [New BSD License](octomap/LICENSE.txt) + * octovis and related libraries: [GPL](octovis/LICENSE.txt) + + +Download the latest releases: + https://github.com/octomap/octomap/tags + +API documentation: + http://octomap.github.com/octomap/doc/ + +Report bugs and request features in our tracker: + https://github.com/OctoMap/octomap/issues + +A list of changes is available in the [octomap changelog](octomap/CHANGELOG.txt) + + +OVERVIEW +-------- + +OctoMap consists of two separate libraries each in its own subfolder: +**octomap**, the actual library, and **octovis**, our visualization libraries and tools. +This README provides an overview of both, for details on compiling each please +see [octomap/README.md](octomap/README.md) and [octovis/README.md](octovis/README.md) respectively. +See http://www.ros.org/wiki/octomap and http://www.ros.org/wiki/octovis if you +want to use OctoMap in ROS; there are pre-compiled packages available. + +You can build each library separately with CMake by running it from the subdirectories, +or build octomap and octovis together from this top-level directory. E.g., to +only compile the library, run: + + cd octomap + mkdir build + cd build + cmake .. + make + +To compile the complete package, run: + + cd build + cmake .. + make + +Binaries and libs will end up in the directories `bin` and `lib` of the +top-level directory where you started the build. + + +See [octomap README](octomap/README.md) and [octovis README](octovis/README.md) for further +details and hints on compiling, especially under Windows. diff --git a/README.txt b/README.txt deleted file mode 100644 index 1345649c..00000000 --- a/README.txt +++ /dev/null @@ -1,62 +0,0 @@ - -Octomap -- A probabilistic, flexible, and compact 3D mapping library for robotic systems. - -Authors: Kai M. Wurm and Armin Hornung, University of Freiburg, Copyright (C) 2009-2013. -http://octomap.github.com - -Further Contributors: -C. Sprunk, University of Freiburg -J. Mueller, University of Freiburg -S. Osswald, University of Freiburg -R. Schmitt, University of Freiburg -R. Bogdan Rusu, Willow Garage Inc. - -License: - * New BSD License (see "octomap/LICENSE.txt") - * GPL for the viewer "octovis" and related libraries (see "octovis/LICENSE.txt"). - - -Download the latest releases: - https://github.com/octomap/octomap/tags - -API documentation: - http://octomap.github.com/octomap/doc/ - -Report bugs and request features in our tracker: - https://github.com/OctoMap/octomap/issues - -A list of changes is available in the file "octomap/CHANGELOG.txt" - - -OVERVIEW -############################ - -OctoMap now consists of two separate libraries each in its own subfolder: -octomap, the actual library, and octovis, our visualization libraries and tools. -This README provides an overview of both, for details on compiling each please -see "octomap/README.txt" and "octovis/README.txt" respectively. -See http://www.ros.org/wiki/octomap and http://www.ros.org/wiki/octovis if you -want to use OctoMap in ROS; there are pre-compiled packages available. - -You can build each library separately with CMake by running it from the subdirectories, -or build octomap and octovis together from this top-level directory. E.g., to -only compile the library, run: - - cd octomap - mkdir build - cd build - cmake .. - make - -To compile the complete package, run: - cd build - cmake .. - make - -Binaries and libs will end up in the directories "bin" and "lib" of the -top-level directory where you started the build. - - -See "octomap/README.txt" and "octovis/README.txt" for further -details and hints on compiling under Windows. \ No newline at end of file diff --git a/dynamicEDT3D/CMakeLists.txt b/dynamicEDT3D/CMakeLists.txt index fc7a2a34..92f3b1bd 100644 --- a/dynamicEDT3D/CMakeLists.txt +++ b/dynamicEDT3D/CMakeLists.txt @@ -43,7 +43,6 @@ find_package(octomap REQUIRED ${CMAKE_SOURCE_DIR}/../octomap/lib/cmake/octomap ) INCLUDE_DIRECTORIES(${OCTOMAP_INCLUDE_DIRS}) -LINK_DIRECTORIES(${OCTOMAP_LIBRARY_DIRS}) ADD_SUBDIRECTORY(src) diff --git a/dynamicEDT3D/include/dynamicEDT3D/bucketedqueue.h b/dynamicEDT3D/include/dynamicEDT3D/bucketedqueue.h index f36f2884..6658e9a3 100644 --- a/dynamicEDT3D/include/dynamicEDT3D/bucketedqueue.h +++ b/dynamicEDT3D/include/dynamicEDT3D/bucketedqueue.h @@ -1,5 +1,3 @@ -// $Id$ - /** * dynamicEDT3D: * A library for incrementally updatable Euclidean distance transforms in 3D. diff --git a/dynamicEDT3D/include/dynamicEDT3D/bucketedqueue.hxx b/dynamicEDT3D/include/dynamicEDT3D/bucketedqueue.hxx index 562e57c3..535b8e7f 100644 --- a/dynamicEDT3D/include/dynamicEDT3D/bucketedqueue.hxx +++ b/dynamicEDT3D/include/dynamicEDT3D/bucketedqueue.hxx @@ -1,5 +1,3 @@ -// $Id$ - /** * dynamicEDT3D: * A library for incrementally updatable Euclidean distance transforms in 3D. diff --git a/dynamicEDT3D/include/dynamicEDT3D/dynamicEDT3D.h b/dynamicEDT3D/include/dynamicEDT3D/dynamicEDT3D.h index 1c94dcb1..e6c4067a 100644 --- a/dynamicEDT3D/include/dynamicEDT3D/dynamicEDT3D.h +++ b/dynamicEDT3D/include/dynamicEDT3D/dynamicEDT3D.h @@ -1,5 +1,3 @@ -// $Id$ - /** * dynamicEDT3D: * A library for incrementally updatable Euclidean distance transforms in 3D. diff --git a/dynamicEDT3D/include/dynamicEDT3D/dynamicEDTOctomap.h b/dynamicEDT3D/include/dynamicEDT3D/dynamicEDTOctomap.h index 16fdeae7..6da5ed26 100644 --- a/dynamicEDT3D/include/dynamicEDT3D/dynamicEDTOctomap.h +++ b/dynamicEDT3D/include/dynamicEDT3D/dynamicEDTOctomap.h @@ -1,5 +1,3 @@ -// $Id$ - /** * dynamicEDT3D: * A library for incrementally updatable Euclidean distance transforms in 3D. diff --git a/dynamicEDT3D/include/dynamicEDT3D/point.h b/dynamicEDT3D/include/dynamicEDT3D/point.h index 02813629..8bf49e36 100644 --- a/dynamicEDT3D/include/dynamicEDT3D/point.h +++ b/dynamicEDT3D/include/dynamicEDT3D/point.h @@ -1,5 +1,3 @@ -// $Id$ - /** * dynamicEDT3D: * A library for incrementally updatable Euclidean distance transforms in 3D. diff --git a/dynamicEDT3D/src/CMakeLists.txt b/dynamicEDT3D/src/CMakeLists.txt index 95cc9bb0..ae29eca3 100644 --- a/dynamicEDT3D/src/CMakeLists.txt +++ b/dynamicEDT3D/src/CMakeLists.txt @@ -11,6 +11,13 @@ target_link_libraries(dynamicedt3d-static ${OCTOMAP_LIBRARIES}) SET_TARGET_PROPERTIES(dynamicedt3d-static PROPERTIES OUTPUT_NAME "dynamicedt3d") +# directly depend on the octomap library target when building the +# complete distribution, so it it recompiled as needed +if (CMAKE_PROJECT_NAME STREQUAL "octomap-distribution") + ADD_DEPENDENCIES(dynamicedt3d-static octomap-static) + ADD_DEPENDENCIES(dynamicedt3d octomap) +endif() + ADD_SUBDIRECTORY(examples) install(TARGETS diff --git a/dynamicEDT3D/src/dynamicEDT3D.cpp b/dynamicEDT3D/src/dynamicEDT3D.cpp index a86393f5..62d8c658 100644 --- a/dynamicEDT3D/src/dynamicEDT3D.cpp +++ b/dynamicEDT3D/src/dynamicEDT3D.cpp @@ -1,5 +1,3 @@ -// $Id$ - /** * dynamicEDT3D: * A library for incrementally updatable Euclidean distance transforms in 3D. diff --git a/dynamicEDT3D/src/dynamicEDTOctomap.cpp b/dynamicEDT3D/src/dynamicEDTOctomap.cpp index 3be31902..b66c8c3a 100644 --- a/dynamicEDT3D/src/dynamicEDTOctomap.cpp +++ b/dynamicEDT3D/src/dynamicEDTOctomap.cpp @@ -1,5 +1,3 @@ -// $Id$ - /** * dynamicEDT3D: * A library for incrementally updatable Euclidean distance transforms in 3D. diff --git a/dynamicEDT3D/src/examples/exampleEDT3D.cpp b/dynamicEDT3D/src/examples/exampleEDT3D.cpp index 685b2605..2a933423 100644 --- a/dynamicEDT3D/src/examples/exampleEDT3D.cpp +++ b/dynamicEDT3D/src/examples/exampleEDT3D.cpp @@ -1,5 +1,3 @@ -// $Id$ - /** * dynamicEDT3D: * A library for incrementally updatable Euclidean distance transforms in 3D. diff --git a/dynamicEDT3D/src/examples/exampleEDTOctomap.cpp b/dynamicEDT3D/src/examples/exampleEDTOctomap.cpp index 3ea69cbe..74f5a9ca 100644 --- a/dynamicEDT3D/src/examples/exampleEDTOctomap.cpp +++ b/dynamicEDT3D/src/examples/exampleEDTOctomap.cpp @@ -1,5 +1,3 @@ -// $Id$ - /** * dynamicEDT3D: * A library for incrementally updatable Euclidean distance transforms in 3D. diff --git a/octomap/CHANGELOG.txt b/octomap/CHANGELOG.txt index 48d6c557..20c86d8c 100644 --- a/octomap/CHANGELOG.txt +++ b/octomap/CHANGELOG.txt @@ -1,3 +1,21 @@ +v1.6.0: 2013-04-05 +================== +- Speedup: Pruning is now done only on affected nodes on each call to + updateNode(). You no longer have to call prune(), except when you manually + change nodes or use lazy_update. +- insertScan() renamed to insertPointCloud(), removed parameter "pruning" +- insertScanNaive() renamed to insertScanRays(), removed parameter "pruning" +- insertScan() / insertPointCloud() / insertScanRays() can now be parallelized + with OpenMP, currently disabled by default. Enable with CMake flag OCTOMAP_OMP +- Empty octrees no longer contain a root node (#11) +- Library names in CMake configs are now absolute paths, you should no longer + use OCTOMAP_LIBRARY_DIRS with FIND_PACKAGE (#14) +- graph2tree: pointcloud is transformed in batch before insertion, small changes + in command line options +- octovis: more map editing options, fixed setting nodes to free/occupied (thx + to C. Dornhege) +- Sample data is now in octomap/share + v1.5.4: 2013-02-27 ================== - Removed binvox binaries from sources for improved packaging compatibility diff --git a/octomap/CMakeLists.txt b/octomap/CMakeLists.txt index 9cd49cd1..37929f2b 100644 --- a/octomap/CMakeLists.txt +++ b/octomap/CMakeLists.txt @@ -5,8 +5,8 @@ ENABLE_TESTING() # version (e.g. for packaging) set(OCTOMAP_MAJOR_VERSION 1) -set(OCTOMAP_MINOR_VERSION 5) -set(OCTOMAP_PATCH_VERSION 4) +set(OCTOMAP_MINOR_VERSION 6) +set(OCTOMAP_PATCH_VERSION 0) set(OCTOMAP_VERSION ${OCTOMAP_MAJOR_VERSION}.${OCTOMAP_MINOR_VERSION}.${OCTOMAP_PATCH_VERSION}) if(COMMAND cmake_policy) cmake_policy(SET CMP0003 NEW) @@ -17,6 +17,18 @@ SET (CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/CMakeModules") # COMPILER SETTINGS (default: Release) and flags INCLUDE(CompilerSettings) +# OCTOMAP_OMP = enable OpenMP parallelization (experimental, defaults to OFF) +SET(OCTOMAP_OMP FALSE CACHE BOOL "Enable/disable OpenMP parallelization") +IF(DEFINED ENV{OCTOMAP_OMP}) + SET(OCTOMAP_OMP $ENV{OCTOMAP_OMP}) +ENDIF(DEFINED ENV{OCTOMAP_OMP}) +IF(OCTOMAP_OMP) + FIND_PACKAGE( OpenMP REQUIRED) + SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") +ENDIF(OCTOMAP_OMP) + # Set output directories for libraries and executables SET( BASE_DIR ${CMAKE_SOURCE_DIR} ) SET( CMAKE_LIBRARY_OUTPUT_DIRECTORY ${BASE_DIR}/lib ) @@ -44,6 +56,9 @@ install(FILES ${octomap_HDRS} DESTINATION include/octomap) file(GLOB octomap_math_HDRS ${PROJECT_SOURCE_DIR}/include/octomap/math/*.h) install(FILES ${octomap_math_HDRS} DESTINATION include/octomap/math) +# Install catkin package.xml +install(FILES package.xml DESTINATION share/octomap) + # uninstall target configure_file( "${PROJECT_SOURCE_DIR}/CMakeModules/CMakeUninstall.cmake.in" @@ -104,21 +119,6 @@ IF(DOXYGEN_FOUND) COMMENT "Generating documentation (Doxygen) at location ${PROJECT_SOURCE_DIR}/doc/html/") ENDIF(DOXYGEN_FOUND) -# make package release from source (must be done on the SVN source tree) -IF (NOT WIN32) - SET(OCTOMAP_PKG_NAME "${PROJECT_NAME}-${OCTOMAP_VERSION}.tar.gz") - SET(DIST_DIR "${CMAKE_BINARY_DIR}/dist-${PROJECT_NAME}") - ADD_CUSTOM_TARGET("dist-${PROJECT_NAME}" - rm -rf "${DIST_DIR}" "${CMAKE_BINARY_DIR}/${OCTOMAP_PKG_NAME}" - COMMAND mkdir "${DIST_DIR}" - COMMAND svn export --force -q "${PROJECT_SOURCE_DIR}" "${DIST_DIR}/${PROJECT_NAME}" - COMMAND tar -czf "${CMAKE_BINARY_DIR}/${OCTOMAP_PKG_NAME}" -C "${DIST_DIR}" --exclude=".hidden" "${PROJECT_NAME}" - WORKING_DIRECTORY "${PROJECT_SOURCE_DIR}" - # cleanup so that there is no copy in the source dir - COMMAND rm -rf "${DIST_DIR}" - ) -ENDIF() - # Needs to be last statement: INCLUDE(CPackSettings) diff --git a/octomap/CMakeModules/CompilerSettings.cmake b/octomap/CMakeModules/CompilerSettings.cmake index 51f1ebee..7c986e57 100644 --- a/octomap/CMakeModules/CompilerSettings.cmake +++ b/octomap/CMakeModules/CompilerSettings.cmake @@ -7,13 +7,6 @@ ENDIF(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) MESSAGE ("\n") MESSAGE (STATUS "${PROJECT_NAME} building as ${CMAKE_BUILD_TYPE}") -# OCTOMAP_OMP = enable OpenMP -# SET(OCTOMAP_OMP 1 CACHE BOOL "Enable/disable OpenMP") -# IF($ENV{OCTOMAP_OMP}) -# SET(OCTOMAP_OMP $ENV{OCTOMAP_OMP}) -# MESSAGE(STATUS "Found OCTOMAP_OMP=${OCTOMAP_OMP}") -# ENDIF($ENV{OCTOMAP_OMP}) - # COMPILER FLAGS IF (CMAKE_COMPILER_IS_GNUCC) SET (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wno-error ") @@ -21,11 +14,7 @@ IF (CMAKE_COMPILER_IS_GNUCC) SET (CMAKE_CXX_FLAGS_RELEASE "-O3 -funroll-loops -DNDEBUG") SET (CMAKE_CXX_FLAGS_DEBUG "-O0 -g") # Shared object compilation under 64bit (vtable) - ADD_DEFINITIONS(-fPIC) - # IF(OCTOMAP_OMP) - # SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fopenmp") - # SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -fopenmp") - # ENDIF(OCTOMAP_OMP) + ADD_DEFINITIONS(-fPIC) ENDIF() diff --git a/octomap/README.md b/octomap/README.md new file mode 100644 index 00000000..4862d979 --- /dev/null +++ b/octomap/README.md @@ -0,0 +1,164 @@ +Octomap - A probabilistic, flexible, and compact 3D mapping library for robotic systems +======================================================================================= + +Authors: Kai M. Wurm and Armin Hornung, University of Freiburg, Copyright (C) 2009-2013. +http://octomap.github.com + +Further Contributors: +* C. Sprunk, University of Freiburg +* J. Mueller, University of Freiburg +* S. Osswald, University of Freiburg +* R. Schmitt, University of Freiburg +* R. Bogdan Rusu, Willow Garage Inc. +* C. Dornhege, University of Freiburg + +License for octomap: [New BSD License](LICENSE.txt) + + +REQUIREMENTS +------------ + +* For only the octomap library: cmake and a regular build environment (gcc) +* For HTML documentation: doxygen (optional) +* For the viewer octovis: Qt4, OpenGL, QGLViewer (optional) + + +Skip to WINDOWS for tips on compilation under Windows. You can install all dependencies on Ubuntu by running: + + sudo apt-get install cmake doxygen libqt4-dev libqt4-opengl-dev libqglviewer-qt4-dev + + + +INSTALLATION +------------ + +See http://www.ros.org/wiki/octomap if you want to use OctoMap in ROS! +There are pre-compiled packages for octomap, octovis, and ROS integration available. + + +Build the complete project by changing into the "build" directory +and running cmake: + + mkdir build && cd build + cmake .. + +Type `make` to compile afterwards. This will create all CMake +files cleanly in the `build` folder (Out-of-source build). +Executables will end up in `bin`, libraries in `lib`. + + +A debug configuration can be created by running: + + cmake -DCMAKE_BUILD_TYPE=Debug .. + +in `build` or a different directory (e.g. `build-debug`). + +You can install the library by running `make install`, though it +is usually not necessary. Be sure to adjust `CMAKE_INSTALL_PREFIX` before. + +The target `make test` executes the unit tests for the octomap library, +if you are interested in verifying the functionality on your machine. + + +DOCUMENTATION +------------- + +The documentation for the latest stable release is available online: + http://octomap.github.com/octomap/doc/index.html + +You can build the most current HTML-Documentation for your current +source with Doxygen by running `make docs` +in the build directory. The documentation will end up in +`doc/html/index.html` in the main directory. + + +GETTING STARTED +--------------- + +Jump right in and have a look at the example +src/octomap/simple.cpp + +Or start the 3D viewer with `bin/octovis` + +You will find an example scan and binary tree to load in the directory `share`. +Further examples can be downloaded from the project website. + + +USE IN OTHER PROJECTS +--------------------- + +A CMake-project config is generated for OctoMap which allows OctoMap +to be used from other CMake-Projects easily. + +Point CMake to your octomap installation so that it finds the file +octomap/lib/cmake/octomap/octomap-config.cmake, e.g. by setting the environment +variable `octomap_DIR`to the directory containing it. + +Then add the following to your CMakeLists.txt: + + find_package(octomap REQUIRED) + include_directories(${OCTOMAP_INCLUDE_DIRS}) + link_libraries(${OCTOMAP_LIBRARIES}) + +In addition to this cmake-module we also provide a pkgconfig-file. + +For convenience, there is a minimal example project included in the file +share/example-project.tgz + + +ECLIPSE PROJECT FILES +--------------------- + +Eclipse project files can be generated (with some limitations, see: +http://www.vtk.org/Wiki/Eclipse_CDT4_Generator) by running: + + cmake -G"Eclipse CDT4 - Unix Makefiles" .. + +Import the project (existing project, root is the build folder, +do not copy contents) into Eclipse afterwards. For full Eclipse +compatibility, it might be necessary to build in the main source +directory. + + +WINDOWS +------- + +The octomap library and tools can be compiled and used +under Windows although this has not been tested in-depth. +Feedback is welcome. + +To compile the library you need cmake (http://www.cmake.org). + +### MinGW ### + +1. Download the MinGW distribution (http://www.mingw.org) +2. Install C++ compiler and add MingGW/bin to your system PATH +3. Start the cmake-gui and set the code directory to the + library root (e.g. `/octomap`) +4. Set the build directory to, e.g., `/octomap/build`. +5. Press "Generate", select the appropriate generator, "MinGW Makefiles". +6. Start a command shell and "make" the project: + + octomap> cd build + octomap/build> mingw32-make.exe + + +You can run the unit tests using ctest on the command prompt: + + octomap/build> ctest.exe + + +### Microsoft Visual Studio 2010 ### + +1. Start the cmake-gui and set the code directory to the + library root (e.g. `/octomap`) +2. Set the build directory to, e.g., /octomap/build. +3. Press "Generate", select the appropriate generator, e.g. "Visual Studio 10". + This generates a solution file octomap.sln +4. Load this file and build the project + + +You can run the unit tests using ctest on the command prompt: + + octomap/build> ctest.exe -C Release + diff --git a/octomap/README.txt b/octomap/README.txt deleted file mode 100644 index cf4e497d..00000000 --- a/octomap/README.txt +++ /dev/null @@ -1,179 +0,0 @@ - -Octomap -- A probabilistic, flexible, and compact 3D mapping library for robotic systems. - -Authors: Kai M. Wurm and Armin Hornung, University of Freiburg, Copyright (C) 2009-2013. -http://octomap.github.com - -Further Contributors: -C. Sprunk, University of Freiburg -J. Mueller, University of Freiburg -S. Osswald, University of Freiburg -R. Schmitt, University of Freiburg -R. Bogdan Rusu, Willow Garage Inc. - -License for octomap: New BSD License (see LICENSE.txt) - - -REQUIREMENTS -############################ - - * cmake - * regular build environment (gcc) - * skip to WINDOWS for tips on compilation under Windows - - viewer: - * Qt4 - * OpenGL - * (QGLViewer) - - HTML documentation: - * doxygen - - Hint: you can install all dependencies on Ubuntu by running: - - sudo apt-get install cmake doxygen libqt4-dev libqt4-opengl-dev libqglviewer-qt4-dev - - - -INSTALLATION -############################ - - * See http://www.ros.org/wiki/octomap if you want to use OctoMap in ROS! There * - * are pre-compiled packages for octomap, octovis, and ROS integration available * - -Build the complete project by changing into the "build" directory -and running cmake: - - mkdir build && cd build - cmake .. - -Type "make" to compile afterwards. This will create all CMake -files cleanly in the "build" folder (Out-of-source build). -Executables will end up in "bin", libraries in "lib". - - -A debug configuration can be created by running: - - cmake -DCMAKE_BUILD_TYPE=Debug .. - -in "build" or a different directory (e.g. "build-debug"). - -You can install the library by running "make install", though it -is not necessary. Be sure to adjust CMAKE_INSTALL_PREFIX before. - -The target "make test" executes the unit tests for the octomap library, -if you are interested in verifying the functionality on your machine. - - -DOCUMENTATION -############################ - -The documentation for the latest stable release is available at: - - http://octomap.github.com/octomap/doc/index.html - -You can build the most current HTML-Documentation for your current -source with Doxygen by running - - make docs - -in the build directory. The documentation will end up in -doc/html/index.html in the main directory. - - -GETTING STARTED -############################ - -Jump right in and have a look at -src/octomap/simple.cpp - -Or start the 3D viewer -bin/octovis - -You will find an example scan to load at -src/examples/scan.dat.bz2 (please bunzip2 it first) - -Further examples can be downloaded from the project website. - - -USE IN OTHER PROJECTS -############################ - -A CMake-project config is generated for OctoMap which allows OctoMap -to be used from other CMake-Projects easily. - -Point CMake to your octomap installation so that it finds: -[octomap]/lib/cmake/octomap/octomap-config.cmake - -Then add the following to your CMakeLists.txt: - -find_package(octomap REQUIRED) -include_directories(${OCTOMAP_INCLUDE_DIRS}) -link_directories(${OCTOMAP_LIBRARY_DIRS}) -link_libraries(${OCTOMAP_LIBRARIES}) - -In addition to this cmake-module we also provide a pkgconfig-file. - -For convenience, there is a minimal project included in the file -example-project.tgz - - -ECLIPSE PROJECT FILES -############################ - -Eclipse project files can be generated (with some limitations, see: -http://www.vtk.org/Wiki/Eclipse_CDT4_Generator) by running: - - cmake -G"Eclipse CDT4 - Unix Makefiles" .. - -Import the project (existing project, root is the build folder, -do not copy contents) into Eclipse afterwards. For full Eclipse -compatibility, it might be necessary to build in the main source -directory. - - -WINDOWS -############################ - -The octomap library and tools can be compiled and used -under Windows although this has not been tested in-depth. -Feedback is welcome. - -To compile the library you need cmake (http://www.cmake.org). - -MinGW ------------------------------- - -* Download the MinGW distribution (http://www.mingw.org) -* Install C++ compiler and add MingGW/bin to your system PATH - -* Start the cmake-gui and set the code directory to the - library root (e.g. /octomap) -* Set the build directory to, e.g., /octomap/build. -* Press "Generate", select the appropriate generator, - "MinGW Makefiles". - -* Start a command shell and "make" the project: - octomap> cd build - octomap/build> mingw32-make.exe - -* You can run the unit tests using ctest on the command prompt: - octomap/build> ctest.exe - - -Microsoft Visual Studio 2010 ------------------------------- - -* Start the cmake-gui and set the code directory to the - library root (e.g. /octomap) -* Set the build directory to, e.g., /octomap/build. -* Press "Generate", select the appropriate generator, - e.g. "Visual Studio 10". - -* This generates a solution file octomap.sln - Load this file and build the project. - -* You can run the unit tests using ctest on the command prompt: - octomap/build> ctest.exe -C Release - diff --git a/octomap/doxygen.h b/octomap/doxygen.h index 7413c28d..e5b515da 100644 --- a/octomap/doxygen.h +++ b/octomap/doxygen.h @@ -83,7 +83,7 @@ You can find an overview at http://octomap.github.com/ and the code repository a Jump right in and have a look at the main class \ref octomap::OcTree OcTree and the examples in src/octomap/simple.cpp. To integrate single measurements into the 3D map have a look at \ref octomap::OcTree::insertRay "OcTree::insertRay(...)", to insert full 3D scans (pointclouds) please have a look at - \ref octomap::OcTree::insertScan "OcTree::insertScan(...)". Queries can be performed e.g. with \ref octomap::OcTree::search "OcTree::search(...)" or + \ref octomap::OcTree::insertPointCloud "OcTree::insertPointCloud(...)". Queries can be performed e.g. with \ref octomap::OcTree::search "OcTree::search(...)" or \ref octomap::OcTree::castRay(...) "OcTree::castRay(...)". The preferred way to batch-access or process nodes in an Octree is with the iterators \ref leaf_iterator "leaf_iterator", \ref tree_iterator "tree_iterator", or \ref leaf_bbx_iterator "leaf_bbx_iterator".

@@ -101,7 +101,7 @@ You can find an overview at http://octomap.github.com/ and the code repository a

- You will find an example scan to load at src/examples/scan.dat.bz2 (please bunzip2 it first) +You will find an example 3D scan (please bunzip2 first) and an example OctoMap .bt file in the directory share/data to try. More data sets are available at http://ais.informatik.uni-freiburg.de/projects/datasets/octomap/.

diff --git a/octomap/include/octomap/OcTreeBaseImpl.h b/octomap/include/octomap/OcTreeBaseImpl.h index 23582def..dc2f2480 100644 --- a/octomap/include/octomap/OcTreeBaseImpl.h +++ b/octomap/include/octomap/OcTreeBaseImpl.h @@ -100,7 +100,7 @@ namespace octomap { /** * \return Pointer to the root node of the tree. This pointer * should not be modified or deleted externally, the OcTree - * manages its memory itself. + * manages its memory itself. In an empty tree, root is NULL. */ inline NODE* getRoot() const { return root; } @@ -143,7 +143,7 @@ namespace octomap { */ bool deleteNode(const OcTreeKey& key, unsigned int depth = 0); - /// Deletes the complete tree structure (only the root node will remain) + /// Deletes the complete tree structure void clear(); OcTreeBaseImpl deepCopy() const; @@ -422,71 +422,14 @@ namespace octomap { return point3d(float(keyToCoord(key[0], depth)), float(keyToCoord(key[1], depth)), float(keyToCoord(key[2], depth))); } - /// @deprecated, replaced with coordToKeyChecked() - DEPRECATED( bool genKeyValue(double coordinate, unsigned short int& keyval) const) { - return coordToKeyChecked(coordinate, keyval); - } - - /// @deprecated, replaced with coordToKeyChecked() - DEPRECATED( bool genKey(const point3d& point, OcTreeKey& key) const ) { - return coordToKeyChecked(point, key); - } - - /// @deprecated, replaced by adjustKeyAtDepth() or coordToKey() with depth parameter - DEPRECATED( bool genKeyValueAtDepth(const unsigned short int keyval, unsigned int depth, unsigned short int &out_keyval) const ); - - /// @deprecated, replaced by adjustKeyAtDepth() or coordToKey() with depth parameter - DEPRECATED( bool genKeyAtDepth(const OcTreeKey& key, unsigned int depth, OcTreeKey& out_key) const ); - - /// @deprecated, replaced by keyToCoord() - /// Will always return true, there is no more boundary check here - DEPRECATED( bool genCoordFromKey(const unsigned short int& key, unsigned depth, float& coord) const ){ - coord = float(keyToCoord(key, depth)); - return true; - } - - /// @deprecated, replaced by keyToCoord() - /// Will always return true, there is no more boundary check here - DEPRECATED( inline bool genCoordFromKey(const unsigned short int& key, float& coord, unsigned depth) const) { - coord = float(keyToCoord(key, depth)); - return true; - } - - /// @deprecated, replaced by keyToCoord() - /// Will always return true, there is no more boundary check here - DEPRECATED( inline bool genCoordFromKey(const unsigned short int& key, float& coord) const) { - coord = float(keyToCoord(key)); - return true; - } - - /// @deprecated, replaced by keyToCoord() - DEPRECATED( double genCoordFromKey(const unsigned short int& key, unsigned depth) const) { - return keyToCoord(key, depth); - } - - /// @deprecated, replaced by keyToCoord() - DEPRECATED( inline double genCoordFromKey(const unsigned short int& key) const) { - return keyToCoord(key); - } - - /// @deprecated, replaced by keyToCoord(). - /// Will always return true, there is no more boundary check here - DEPRECATED( inline bool genCoords(const OcTreeKey& key, unsigned int depth, point3d& point) const){ - point = keyToCoord(key, depth); - return true; - } - - /// generate child index (between 0 and 7) from key at given tree depth - /// DEPRECATED - DEPRECATED( inline void genPos(const OcTreeKey& key, int depth, unsigned int& pos) const) { - pos = computeChildIdx(key, depth); - } - protected: /// Constructor to enable derived classes to change tree constants. /// This usually requires a re-implementation of some core tree-traversal functions as well! OcTreeBaseImpl(double resolution, unsigned int tree_depth, unsigned int tree_max_val); + /// initialize non-trivial members, helper for constructors + void init(); + /// recalculates min and max in x, y, z. Does nothing when tree size didn't change. void calcMinMax(); @@ -510,7 +453,7 @@ namespace octomap { protected: - NODE* root; + NODE* root; ///< Pointer to the root NODE, NULL for empty tree // constants of the tree const unsigned int tree_depth; ///< Maximum tree depth is fixed to 16 currently @@ -529,7 +472,8 @@ namespace octomap { /// contains the size of a voxel at level i (0: root node). tree_depth+1 levels (incl. 0) std::vector sizeLookupTable; - KeyRay keyray; // data structure for ray casting + /// data structure for ray casting, array for multithreading + std::vector keyrays; const leaf_iterator leaf_iterator_end; const leaf_bbx_iterator leaf_iterator_bbx_end; diff --git a/octomap/include/octomap/OcTreeBaseImpl.hxx b/octomap/include/octomap/OcTreeBaseImpl.hxx index 7291fe11..b762c94a 100644 --- a/octomap/include/octomap/OcTreeBaseImpl.hxx +++ b/octomap/include/octomap/OcTreeBaseImpl.hxx @@ -35,6 +35,10 @@ #undef min #include +#ifdef _OPENMP + #include +#endif + namespace octomap { @@ -44,16 +48,9 @@ namespace octomap { resolution(resolution), tree_size(0) { - this->setResolution(resolution); - for (unsigned i = 0; i< 3; i++){ - max_value[i] = -(std::numeric_limits::max( )); - min_value[i] = std::numeric_limits::max( ); - } - size_changed = true; + init(); - // init root node: - root = new NODE(); - tree_size++; + // no longer create an empty root node - only on demand } template @@ -61,22 +58,18 @@ namespace octomap { I(), root(NULL), tree_depth(tree_depth), tree_max_val(tree_max_val), resolution(resolution), tree_size(0) { - this->setResolution(resolution); - for (unsigned i = 0; i< 3; i++){ - max_value[i] = -(std::numeric_limits::max( )); - min_value[i] = std::numeric_limits::max( ); - } - size_changed = true; + init(); - // init root node: - root = new NODE(); - tree_size++; + // no longer create an empty root node - only on demand } template OcTreeBaseImpl::~OcTreeBaseImpl(){ - delete root; + if (root) + delete root; + + root = NULL; } @@ -85,15 +78,38 @@ namespace octomap { root(NULL), tree_depth(rhs.tree_depth), tree_max_val(rhs.tree_max_val), resolution(rhs.resolution), tree_size(rhs.tree_size) { - this->setResolution(resolution); + init(); + + // copy nodes recursively: + if (rhs.root) + root = new NODE(*(rhs.root)); + + } + + template + void OcTreeBaseImpl::init(){ + + this->setResolution(this->resolution); for (unsigned i = 0; i< 3; i++){ - max_value[i] = rhs.max_value[i]; - min_value[i] = rhs.min_value[i]; + max_value[i] = -(std::numeric_limits::max( )); + min_value[i] = std::numeric_limits::max( ); } + size_changed = true; - // copy nodes recursively: - root = new NODE(*(rhs.root)); + // create as many KeyRays as there are OMP_THREADS defined, + // one buffer for each thread +#ifdef _OPENMP + #pragma omp parallel + #pragma omp critical + { + if (omp_get_thread_num() == 0){ + this->keyrays.resize(omp_get_num_threads()); + } + } +#else + this->keyrays.resize(1); +#endif } @@ -256,30 +272,6 @@ namespace octomap { } } - template - bool OcTreeBaseImpl::genKeyValueAtDepth(const unsigned short int keyval, unsigned int depth, unsigned short int &out_keyval) const { - - if (keyval >= 2*tree_max_val) - return false; - - unsigned int diff = tree_depth - depth; - if(!diff) { - out_keyval = keyval; - } - else { - out_keyval = (((keyval-tree_max_val) >> diff) << diff) + (1 << (diff-1)) + tree_max_val; - } - return true; - } - - template - bool OcTreeBaseImpl::genKeyAtDepth(const OcTreeKey& key, unsigned int depth, OcTreeKey& out_key) const { - for (unsigned int i=0;i<3;i++) { - if (!genKeyValueAtDepth( key[i], depth, out_key[i])) return false; - } - return true; - } - template NODE* OcTreeBaseImpl::search(const point3d& value, unsigned int depth) const { OcTreeKey key; @@ -309,6 +301,8 @@ namespace octomap { template NODE* OcTreeBaseImpl::search (const OcTreeKey& key, unsigned int depth) const { assert(depth <= tree_depth); + if (root == NULL) + return NULL; if (depth == 0) depth = tree_depth; @@ -373,6 +367,9 @@ namespace octomap { template bool OcTreeBaseImpl::deleteNode(const OcTreeKey& key, unsigned int depth) { + if (root == NULL) + return true; + if (depth == 0) depth = tree_depth; @@ -381,29 +378,33 @@ namespace octomap { template void OcTreeBaseImpl::clear() { - // don't clear if the tree is empty: - if (this->root->hasChildren()) { + if (this->root){ delete this->root; - this->root = new NODE(); + this->root = NULL; + this->tree_size = 0; + // max extent of tree changed: + this->size_changed = true; } - this->tree_size = 1; - // max extent of tree changed: - this->size_changed = true; } template void OcTreeBaseImpl::prune() { - for (unsigned int depth=tree_depth-1; depth>0; depth--) { + if (root == NULL) + return; + + for (unsigned int depth=tree_depth-1; depth > 0; --depth) { unsigned int num_pruned = 0; pruneRecurs(this->root, 0, depth, num_pruned); - if (num_pruned == 0) break; + if (num_pruned == 0) + break; } } template void OcTreeBaseImpl::expand() { - expandRecurs(root,0, tree_depth); + if (root) + expandRecurs(root,0, tree_depth); } template @@ -425,7 +426,8 @@ namespace octomap { } - if (key_origin == key_end) return true; // same tree cell, we're done. + if (key_origin == key_end) + return true; // same tree cell, we're done. ray.addKey(key_origin); @@ -526,8 +528,8 @@ namespace octomap { bool OcTreeBaseImpl::computeRay(const point3d& origin, const point3d& end, std::vector& _ray) { _ray.clear(); - if (!computeRayKeys(origin, end, keyray)) return false; - for (KeyRay::const_iterator it = keyray.begin(); it != keyray.end(); ++it) { + if (!computeRayKeys(origin, end, keyrays.at(0))) return false; + for (KeyRay::const_iterator it = keyrays[0].begin(); it != keyrays[0].end(); ++it) { _ray.push_back(keyToCoord(*it)); } return true; @@ -538,6 +540,8 @@ namespace octomap { if (depth >= max_depth) // on last level: delete child when going up return true; + assert(node); + unsigned int pos = computeChildIdx(key, this->tree_depth-1-depth); if (!node->childExists(pos)) { @@ -575,6 +579,8 @@ namespace octomap { void OcTreeBaseImpl::pruneRecurs(NODE* node, unsigned int depth, unsigned int max_depth, unsigned int& num_pruned) { + assert(node); + if (depth < max_depth) { for (unsigned int i=0; i<8; i++) { if (node->childExists(i)) { @@ -597,7 +603,10 @@ namespace octomap { template void OcTreeBaseImpl::expandRecurs(NODE* node, unsigned int depth, unsigned int max_depth) { - if (depth >= max_depth) return; + if (depth >= max_depth) + return; + + assert(node); // current node has no children => can be expanded if (!node->hasChildren()){ @@ -616,7 +625,9 @@ namespace octomap { template std::ostream& OcTreeBaseImpl::writeData(std::ostream &s) const{ - root->writeValue(s); + if (root) + root->writeValue(s); + return s; } @@ -630,12 +641,13 @@ namespace octomap { this->tree_size = 0; size_changed = true; - // tree needs to be newly created or cleared externally! - if (root->hasChildren()) { + // tree needs to be newly created or cleared externally + if (root) { OCTOMAP_ERROR_STR("Trying to read into an existing tree."); return s; } + root = new NODE(); root->readValue(s); tree_size = calcNumNodes(); // compute number of nodes return s; @@ -646,6 +658,9 @@ namespace octomap { template unsigned long long OcTreeBaseImpl::memoryFullGrid() const{ + if (root == NULL) + return 0; + double size_x, size_y, size_z; this->getMetricSize(size_x, size_y,size_z); @@ -696,8 +711,8 @@ namespace octomap { if (!size_changed) return; - // workaround for "empty" tree (only root) - if (!this->root->hasChildren()){ + // empty tree + if (root == NULL){ min_value[0] = min_value[1] = min_value[2] = 0.0; max_value[0] = max_value[1] = max_value[2] = 0.0; size_changed = false; @@ -755,8 +770,8 @@ namespace octomap { void OcTreeBaseImpl::getMetricMin(double& mx, double& my, double& mz) const { mx = my = mz = std::numeric_limits::max( ); if (size_changed) { - // workaround for "empty" tree (only root) - if (!this->root->hasChildren()){ + // empty tree + if (root == NULL){ mx = my = mz = 0.0; return; } @@ -783,8 +798,8 @@ namespace octomap { void OcTreeBaseImpl::getMetricMax(double& mx, double& my, double& mz) const { mx = my = mz = -std::numeric_limits::max( ); if (size_changed) { - // workaround for "empty" tree (only root) - if (!this->root->hasChildren()){ + // empty tree + if (root == NULL){ mx = my = mz = 0.0; return; } @@ -809,14 +824,17 @@ namespace octomap { template size_t OcTreeBaseImpl::calcNumNodes() const { - size_t retval = 1; // root node - calcNumNodesRecurs(root, retval); + size_t retval = 0; // root node + if (root){ + retval++; + calcNumNodesRecurs(root, retval); + } return retval; } template void OcTreeBaseImpl::calcNumNodesRecurs(NODE* node, size_t& num_nodes) const { - assert (node != NULL); + assert (node); if (node->hasChildren()) { for (unsigned int i=0; i<8; ++i) { if (node->childExists(i)) { @@ -868,14 +886,19 @@ namespace octomap { template size_t OcTreeBaseImpl::getNumLeafNodes() const { + if (root == NULL) + return 0; + return getNumLeafNodesRecurs(root); } template size_t OcTreeBaseImpl::getNumLeafNodesRecurs(const NODE* parent) const { + assert(parent); - if (!parent->hasChildren()) return 1; // this is a leaf -> terminate + if (!parent->hasChildren()) // this is a leaf -> terminate + return 1; size_t sum_leafs_children = 0; for (unsigned int i=0; i<8; ++i) { diff --git a/octomap/include/octomap/OcTreeDataNode.h b/octomap/include/octomap/OcTreeDataNode.h index ed4753d0..8ff6c530 100644 --- a/octomap/include/octomap/OcTreeDataNode.h +++ b/octomap/include/octomap/OcTreeDataNode.h @@ -74,6 +74,8 @@ namespace octomap { /// initialize i-th child, allocate children array if needed bool createChild(unsigned int i); + /// Safe test to check of the i-th child exists, + /// first tests if there are any children. /// \return true if the i-th child exists bool childExists(unsigned int i) const; diff --git a/octomap/include/octomap/OcTreeDataNode.hxx b/octomap/include/octomap/OcTreeDataNode.hxx index 4a376b4d..267b875b 100644 --- a/octomap/include/octomap/OcTreeDataNode.hxx +++ b/octomap/include/octomap/OcTreeDataNode.hxx @@ -62,7 +62,6 @@ namespace octomap { } - template OcTreeDataNode::~OcTreeDataNode() { @@ -127,9 +126,13 @@ namespace octomap { template bool OcTreeDataNode::hasChildren() const { - if (children == NULL) return false; - for (unsigned int i = 0; i<8; i++) - if (childExists(i)) return true; + if (children == NULL) + return false; + for (unsigned int i = 0; i<8; i++){ + // fast check, we know children != NULL + if (children[i] != NULL) + return true; + } return false; } @@ -145,12 +148,11 @@ namespace octomap { if (!childExists(0) || getChild(0)->hasChildren()) return false; - T childValue = getChild(0)->getValue(); - for (unsigned int i = 1; i<8; i++) { - if (!childExists(i)) return false; - else if (getChild(i)->hasChildren()) return false; - else if (! (getChild(i)->getValue() == childValue)) return false; + // comparison via getChild so that casts of derived classes ensure + // that the right == operator gets called + if (!childExists(i) || getChild(i)->hasChildren() || !(*(getChild(i)) == *(getChild(0)))) + return false; } return true; } diff --git a/octomap/include/octomap/OcTreeIterator.hxx b/octomap/include/octomap/OcTreeIterator.hxx index 5b84bad4..5a7c0920 100644 --- a/octomap/include/octomap/OcTreeIterator.hxx +++ b/octomap/include/octomap/OcTreeIterator.hxx @@ -46,22 +46,27 @@ iterator_base() : tree(NULL), maxDepth(0){} /** - * Constructor of the iterator. + * Constructor of the iterator. Initializes the iterator with the default + * constructor (= end() iterator) if tree is empty or NULL. * * @param tree OcTreeBaseImpl on which the iterator is used on * @param depth Maximum depth to traverse the tree. 0 (default): unlimited */ iterator_base(OcTreeBaseImpl const* tree, unsigned char depth=0) - : tree(tree), maxDepth(depth) + : tree((tree && tree->root) ? tree : NULL), maxDepth(depth) { - if (maxDepth == 0) + if (tree && maxDepth == 0) maxDepth = tree->getTreeDepth(); - StackElement s; - s.node = tree->root; - s.depth = 0; - s.key[0] = s.key[1] = s.key[2] = tree->tree_max_val; - stack.push(s); + if (tree && tree->root){ // tree is not empty + StackElement s; + s.node = tree->root; + s.depth = 0; + s.key[0] = s.key[1] = s.key[2] = tree->tree_max_val; + stack.push(s); + } else{ // construct the same as "end", tree must already be NULL + this->maxDepth = 0; + } } /// Copy constructor of the iterator @@ -160,7 +165,7 @@ StackElement s; s.depth = top.depth +1; - unsigned short int center_offset_key = tree->tree_max_val >> (top.depth +1); + unsigned short int center_offset_key = tree->tree_max_val >> s.depth; // push on stack in reverse order for (int i=7; i>=0; --i) { if (top.node->childExists(i)) { @@ -263,10 +268,13 @@ * @param depth Maximum depth to traverse the tree. 0 (default): unlimited */ leaf_iterator(OcTreeBaseImpl const* tree, unsigned char depth=0) : iterator_base(tree, depth) { - // skip forward to next valid leaf node: - // add root another time (one will be removed) and ++ - this->stack.push(this->stack.top()); - operator ++(); + // tree could be empty (= no stack) + if (this->stack.size() > 0){ + // skip forward to next valid leaf node: + // add root another time (one will be removed) and ++ + this->stack.push(this->stack.top()); + operator ++(); + } } leaf_iterator(const leaf_iterator& other) : iterator_base(other) {}; @@ -417,7 +425,7 @@ typename iterator_base::StackElement s; s.depth = top.depth +1; - unsigned short int center_offset_key = this->tree->tree_max_val >> (top.depth +1); + unsigned short int center_offset_key = this->tree->tree_max_val >> s.depth; // push on stack in reverse order for (int i=7; i>=0; --i) { if (top.node->childExists(i)) { diff --git a/octomap/include/octomap/OcTreeKey.h b/octomap/include/octomap/OcTreeKey.h index 71db261f..37e64209 100644 --- a/octomap/include/octomap/OcTreeKey.h +++ b/octomap/include/octomap/OcTreeKey.h @@ -153,7 +153,7 @@ namespace octomap { */ inline void computeChildKey (const unsigned int& pos, const unsigned short int& center_offset_key, const OcTreeKey& parent_key, OcTreeKey& child_key) { - + // x-axis if (pos & 1) child_key[0] = parent_key[0] + center_offset_key; else child_key[0] = parent_key[0] - center_offset_key - (center_offset_key ? 0 : 1); // y-axis @@ -181,12 +181,16 @@ namespace octomap { * @return key corresponding to the input key at the given level */ inline OcTreeKey computeIndexKey(unsigned short int level, const OcTreeKey& key) { - unsigned short int mask = 65535 << level; - OcTreeKey result = key; - result[0] &= mask; - result[1] &= mask; - result[2] &= mask; - return result; + if (level == 0) + return key; + else { + unsigned short int mask = 65535 << level; + OcTreeKey result = key; + result[0] &= mask; + result[1] &= mask; + result[2] &= mask; + return result; + } } } // namespace diff --git a/octomap/include/octomap/OccupancyOcTreeBase.h b/octomap/include/octomap/OccupancyOcTreeBase.h index b8344d41..238daff5 100644 --- a/octomap/include/octomap/OccupancyOcTreeBase.h +++ b/octomap/include/octomap/OccupancyOcTreeBase.h @@ -75,83 +75,124 @@ namespace octomap { /// Copy constructor OccupancyOcTreeBase(const OccupancyOcTreeBase& rhs); - /** - * Integrate a Pointcloud (in global reference frame) - * - * @param scan Pointcloud (measurement endpoints), in global reference frame - * @param sensor_origin measurement origin in global reference frame - * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam) - * @param pruning whether the tree is (losslessly) pruned after insertion (default: true) - * @param lazy_eval whether update of inner nodes is omitted after the update (default: false). - * This speeds up the insertion, but you need to call updateInnerOccupancy() when done. - */ - virtual void insertScan(const Pointcloud& scan, const octomap::point3d& sensor_origin, - double maxrange=-1., bool pruning=true, bool lazy_eval = false); - - /** - * Integrate a 3d scan, transform scan before tree update - * - * @param scan Pointcloud (measurement endpoints) relative to frame origin - * @param sensor_origin origin of sensor relative to frame origin - * @param frame_origin origin of reference frame, determines transform to be applied to cloud and sensor origin - * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam) - * @param pruning whether the tree is (losslessly) pruned after insertion (default: true) - * @param lazy_eval whether update of inner nodes is omitted after the update (default: false). - * This speeds up the insertion, but you need to call updateInnerOccupancy() when done. - */ - virtual void insertScan(const Pointcloud& scan, const point3d& sensor_origin, const pose6d& frame_origin, - double maxrange=-1., bool pruning = true, bool lazy_eval = false); - /** - * Insert a 3d scan (given as a ScanNode) into the tree. - * - * @param scan ScanNode contains Pointcloud data and frame/sensor origin - * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam) - * @param pruning whether the tree is (losslessly) pruned after insertion (default: true) - * @param lazy_eval whether the tree is left 'dirty' after the update (default: false). - * This speeds up the insertion by not updating inner nodes, but you need to call updateInnerOccupancy() when done. - */ - virtual void insertScan(const ScanNode& scan, double maxrange=-1., bool pruning = true, bool lazy_eval = false); - - /// for testing only - virtual void insertScanNaive(const Pointcloud& pc, const point3d& origin, double maxrange, bool pruning = true, bool lazy_eval = false); + * Integrate a Pointcloud (in global reference frame), parallelized with OpenMP. + * Special care is taken that each voxel + * in the map is updated only once, and occupied nodes have a preference over free ones. + * This avoids holes in the floor from mutual deletion and is more efficient than the plain + * ray insertion in insertPointCloudRays(). + * + * @param scan Pointcloud (measurement endpoints), in global reference frame + * @param sensor_origin measurement origin in global reference frame + * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam) + * @param lazy_eval whether update of inner nodes is omitted after the update (default: false). + * This speeds up the insertion, but you need to call updateInnerOccupancy() when done. + */ + virtual void insertPointCloud(const Pointcloud& scan, const octomap::point3d& sensor_origin, + double maxrange=-1., bool lazy_eval = false); /** - * Manipulate log_odds value of voxel directly - * - * @param key OcTreeKey of the NODE that is to be updated - * @param log_odds_update value to be added (+) to log_odds value of node - * @param lazy_eval whether update of inner nodes is omitted after the update (default: false). - * This speeds up the insertion, but you need to call updateInnerOccupancy() when done. - * @return pointer to the updated NODE - */ - virtual NODE* updateNode(const OcTreeKey& key, float log_odds_update, bool lazy_eval = false); + * Integrate a 3d scan (transform scan before tree update), parallelized with OpenMP. + * Special care is taken that each voxel + * in the map is updated only once, and occupied nodes have a preference over free ones. + * This avoids holes in the floor from mutual deletion and is more efficient than the plain + * ray insertion in insertPointCloudRays(). + * + * @param scan Pointcloud (measurement endpoints) relative to frame origin + * @param sensor_origin origin of sensor relative to frame origin + * @param frame_origin origin of reference frame, determines transform to be applied to cloud and sensor origin + * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam) + * @param lazy_eval whether update of inner nodes is omitted after the update (default: false). + * This speeds up the insertion, but you need to call updateInnerOccupancy() when done. + */ + virtual void insertPointCloud(const Pointcloud& scan, const point3d& sensor_origin, const pose6d& frame_origin, + double maxrange=-1., bool lazy_eval = false); /** - * Manipulate log_odds value of voxel directly. - * Looks up the OcTreeKey corresponding to the coordinate and then calls udpateNode() with it. - * - * @param value 3d coordinate of the NODE that is to be updated - * @param log_odds_update value to be added (+) to log_odds value of node - * @param lazy_eval whether update of inner nodes is omitted after the update (default: false). - * This speeds up the insertion, but you need to call updateInnerOccupancy() when done. - * @return pointer to the updated NODE - */ - virtual NODE* updateNode(const point3d& value, float log_odds_update, bool lazy_eval = false); + * Insert a 3d scan (given as a ScanNode) into the tree, parallelized with OpenMP. + * + * @param scan ScanNode contains Pointcloud data and frame/sensor origin + * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam) + * @param lazy_eval whether the tree is left 'dirty' after the update (default: false). + * This speeds up the insertion by not updating inner nodes, but you need to call updateInnerOccupancy() when done. + */ + virtual void insertPointCloud(const ScanNode& scan, double maxrange=-1., bool lazy_eval = false); + + /// @note Deprecated, use insertPointCloud() instead. pruning is now done automatically. + OCTOMAP_DEPRECATED(virtual void insertScan(const Pointcloud& scan, const octomap::point3d& sensor_origin, + double maxrange=-1., bool pruning=true, bool lazy_eval = false)) + { + this->insertPointCloud(scan, sensor_origin, maxrange, lazy_eval); + } + + /// @note Deprecated, use insertPointCloud() instead. pruning is now done automatically. + OCTOMAP_DEPRECATED(virtual void insertScan(const Pointcloud& scan, const point3d& sensor_origin, + const pose6d& frame_origin, double maxrange=-1., bool pruning = true, bool lazy_eval = false)) + { + this->insertPointCloud(scan, sensor_origin, frame_origin, maxrange, lazy_eval); + } + + /// @note Deprecated, use insertPointCloud() instead. pruning is now done automatically. + OCTOMAP_DEPRECATED(virtual void insertScan(const ScanNode& scan, double maxrange=-1., bool pruning = true, bool lazy_eval = false)){ + this->insertPointCloud(scan, maxrange, lazy_eval); + } + + /// @note Deprecated, use insertPointCloudRays instead. pruning is now done automatically. + OCTOMAP_DEPRECATED( virtual void insertScanNaive(const Pointcloud& scan, const point3d& sensor_origin, double maxrange, bool lazy_eval = false)){ + this->insertPointCloudRays(scan, sensor_origin, maxrange, lazy_eval); + } /** - * Manipulate log_odds value of voxel directly. - * Looks up the OcTreeKey corresponding to the coordinate and then calls udpateNode() with it. + * Integrate a Pointcloud (in global reference frame), parallelized with OpenMP. + * This function simply inserts all rays of the point clouds as batch operation. + * Discretization effects can lead to the deletion of occupied space, it is + * usually recommended to use insertPointCloud() instead. * - * @param x - * @param y - * @param z - * @param log_odds_update value to be added (+) to log_odds value of node + * @param scan Pointcloud (measurement endpoints), in global reference frame + * @param sensor_origin measurement origin in global reference frame + * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam) * @param lazy_eval whether update of inner nodes is omitted after the update (default: false). * This speeds up the insertion, but you need to call updateInnerOccupancy() when done. - * @return pointer to the updated NODE */ - virtual NODE* updateNode(double x, double y, double z, float log_odds_update, bool lazy_eval = false); + virtual void insertPointCloudRays(const Pointcloud& scan, const point3d& sensor_origin, double maxrange = -1., bool lazy_eval = false); + + /** + * Manipulate log_odds value of a voxel directly. This only works if key is at the lowest + * octree level + * + * @param key OcTreeKey of the NODE that is to be updated + * @param log_odds_update value to be added (+) to log_odds value of node + * @param lazy_eval whether update of inner nodes is omitted after the update (default: false). + * This speeds up the insertion, but you need to call updateInnerOccupancy() when done. + * @return pointer to the updated NODE + */ + virtual NODE* updateNode(const OcTreeKey& key, float log_odds_update, bool lazy_eval = false); + + /** + * Manipulate log_odds value of voxel directly. + * Looks up the OcTreeKey corresponding to the coordinate and then calls udpateNode() with it. + * + * @param value 3d coordinate of the NODE that is to be updated + * @param log_odds_update value to be added (+) to log_odds value of node + * @param lazy_eval whether update of inner nodes is omitted after the update (default: false). + * This speeds up the insertion, but you need to call updateInnerOccupancy() when done. + * @return pointer to the updated NODE + */ + virtual NODE* updateNode(const point3d& value, float log_odds_update, bool lazy_eval = false); + + /** + * Manipulate log_odds value of voxel directly. + * Looks up the OcTreeKey corresponding to the coordinate and then calls udpateNode() with it. + * + * @param x + * @param y + * @param z + * @param log_odds_update value to be added (+) to log_odds value of node + * @param lazy_eval whether update of inner nodes is omitted after the update (default: false). + * This speeds up the insertion, but you need to call updateInnerOccupancy() when done. + * @return pointer to the updated NODE + */ + virtual NODE* updateNode(double x, double y, double z, float log_odds_update, bool lazy_eval = false); /** * Integrate occupancy measurement. @@ -201,6 +242,8 @@ namespace octomap { /** * Insert one ray between origin and end into the tree. * integrateMissOnRay() is called for the ray, the end point is updated as occupied. + * It is usually more efficient to insert complete pointcloudsm with insertPointCloud() or + * insertPointCloudRays(). * * @param origin origin of sensor in global coordinates * @param end endpoint of measurement in global coordinates @@ -212,7 +255,8 @@ namespace octomap { virtual bool insertRay(const point3d& origin, const point3d& end, double maxrange=-1.0, bool lazy_eval = false); /** - * Performs raycasting in 3d, similar to computeRay(). + * Performs raycasting in 3d, similar to computeRay(). Can be called in parallel e.g. with OpenMP + * for a speedup. * * A ray is cast from origin with a given direction, the first occupied * cell is returned (as center coordinate). If the starting coordinate is already @@ -227,75 +271,9 @@ namespace octomap { */ virtual bool castRay(const point3d& origin, const point3d& direction, point3d& end, bool ignoreUnknownCells=false, double maxRange=-1.0) const; - - /** - * Convenience function to return all occupied nodes in the OcTree. - * @note Deprecated, will be removed in the future. - * Direcly access the nodes with iterators instead! - * - * @param node_centers list of occpupied nodes (as point3d) - * @param max_depth Depth limit of query. 0 (default): no depth limit - */ - DEPRECATED( void getOccupied(point3d_list& node_centers, unsigned int max_depth = 0) const); - - /** - * Convenience function to return all occupied nodes in the OcTree. - * @note Deprecated, will be removed in the future. - * Direcly access the nodes with iterators instead! * - * - * @param occupied_volumes list of occpupied nodes (as point3d and size of the volume) - * @param max_depth Depth limit of query. 0 (default): no depth limit - */ - DEPRECATED(void getOccupied(std::list& occupied_volumes, unsigned int max_depth = 0) const); - - /** - * Traverses the tree and collects all OcTreeVolumes regarded as occupied. - * Inner nodes with both occupied and free children are regarded as occupied. - * This should be for internal use only, use getOccupied(occupied_volumes) instead. - * @note Deprecated, will be removed in the future. - * Direcly access the nodes with iterators instead! * - * - * @param binary_nodes list of binary OcTreeVolumes which are occupied - * @param delta_nodes list of delta OcTreeVolumes which are occupied - * @param max_depth Depth limit of query. 0 (default): no depth limit - */ - DEPRECATED(void getOccupied(std::list& binary_nodes, std::list& delta_nodes, - unsigned int max_depth = 0) const); - /** - * returns occupied leafs within a bounding box defined by min and max. - * @note Deprecated, will be removed in the future. - * Direcly access the nodes with iterators instead! - */ - DEPRECATED(void getOccupiedLeafsBBX(point3d_list& node_centers, point3d min, point3d max) const); - - /** - * Convenience function to return all free nodes in the OcTree. - * @note Deprecated, will be removed in the future. - * Direcly access the nodes with iterators instead! - * - * @param free_volumes list of free nodes (as point3d and size of the volume) - * @param max_depth Depth limit of query. 0 (default): no depth limit - */ - DEPRECATED(void getFreespace(std::list& free_volumes, unsigned int max_depth = 0) const); - - /** - * Traverses the tree and collects all OcTreeVolumes regarded as free. - * Inner nodes with both occupied and free children are regarded as occupied. - * @note Deprecated, will be removed in the future. - * Direcly access the nodes with iterators instead! - * - * @param binary_nodes list of binary OcTreeVolumes which are free - * @param delta_nodes list of delta OcTreeVolumes which are free - * @param max_depth Depth limit of query. 0 (default): no depth limit - */ - DEPRECATED(void getFreespace(std::list& binary_nodes, std::list& delta_nodes, - unsigned int max_depth = 0) const); - - - - //-- set BBX limit (limits tree updates to this bounding box + //-- set BBX limit (limits tree updates to this bounding box) /// use or ignore BBX limit (default: ignore) void useBBXLimit(bool enable) { use_bbx_limit = enable; } @@ -333,7 +311,7 @@ namespace octomap { /** - * Helper for insertScan. Computes all octree nodes affected by the point cloud + * Helper for insertPointCloud(). Computes all octree nodes affected by the point cloud * integration at once. Here, occupied nodes have a preference over free * ones. * @@ -351,7 +329,7 @@ namespace octomap { // -- I/O ----------------------------------------- /** - * Reads only the data (=tree structure) from the input stream. + * Reads only the data (=complete tree structure) from the input stream. * The tree needs to be constructed with the proper header information * beforehand, see readBinary(). */ @@ -363,9 +341,6 @@ namespace octomap { * * This will set the log_odds_occupancy value of * all leaves to either free or occupied. - * - * @param s - * @return */ std::istream& readBinaryNode(std::istream &s, NODE* node) const; @@ -389,8 +364,6 @@ namespace octomap { std::ostream& writeBinaryData(std::ostream &s) const; - void calcNumThresholdedNodes(unsigned int& num_thresholded, unsigned int& num_other) const; - /** * Updates the occupancy of all inner nodes to reflect their children's occupancy. * If you performed batch-updates with lazy evaluation enabled, you must call this @@ -429,16 +402,9 @@ namespace octomap { unsigned int depth, const float& log_odds_update, bool lazy_eval = false); void updateInnerOccupancyRecurs(NODE* node, unsigned int depth); - - void getOccupiedLeafsBBXRecurs( point3d_list& node_centers, unsigned int max_depth, NODE* node, - unsigned int depth, const OcTreeKey& parent_key, - const OcTreeKey& min, const OcTreeKey& max) const; void toMaxLikelihoodRecurs(NODE* node, unsigned int depth, unsigned int max_depth); - void calcNumThresholdedNodesRecurs (NODE* node, - unsigned int& num_thresholded, - unsigned int& num_other) const; protected: bool use_bbx_limit; ///< use bounding box for queries (needs to be set)? diff --git a/octomap/include/octomap/OccupancyOcTreeBase.hxx b/octomap/include/octomap/OccupancyOcTreeBase.hxx index 7c0db0f5..adb64c6c 100644 --- a/octomap/include/octomap/OccupancyOcTreeBase.hxx +++ b/octomap/include/octomap/OccupancyOcTreeBase.hxx @@ -69,19 +69,19 @@ namespace octomap { } - // performs transformation to data and sensor origin first template - void OccupancyOcTreeBase::insertScan(const ScanNode& scan, double maxrange, bool pruning, bool lazy_eval) { + void OccupancyOcTreeBase::insertPointCloud(const ScanNode& scan, double maxrange, bool lazy_eval) { + // performs transformation to data and sensor origin first Pointcloud& cloud = *(scan.scan); pose6d frame_origin = scan.pose; point3d sensor_origin = frame_origin.inv().transform(scan.pose.trans()); - insertScan(cloud, sensor_origin, frame_origin, maxrange, pruning, lazy_eval); + insertPointCloud(cloud, sensor_origin, frame_origin, maxrange, lazy_eval); } template - void OccupancyOcTreeBase::insertScan(const Pointcloud& scan, const octomap::point3d& sensor_origin, - double maxrange, bool pruning, bool lazy_eval) { + void OccupancyOcTreeBase::insertPointCloud(const Pointcloud& scan, const octomap::point3d& sensor_origin, + double maxrange, bool lazy_eval) { KeySet free_cells, occupied_cells; computeUpdate(scan, sensor_origin, free_cells, occupied_cells, maxrange); @@ -93,65 +93,102 @@ namespace octomap { for (KeySet::iterator it = occupied_cells.begin(); it != occupied_cells.end(); ++it) { updateNode(*it, true, lazy_eval); } + } - // TODO: does pruning make sense if we used "lazy_eval"? - if (pruning) this->prune(); - } - - // performs transformation to data and sensor origin first template - void OccupancyOcTreeBase::insertScan(const Pointcloud& pc, const point3d& sensor_origin, const pose6d& frame_origin, - double maxrange, bool pruning, bool lazy_eval) { + void OccupancyOcTreeBase::insertPointCloud(const Pointcloud& pc, const point3d& sensor_origin, const pose6d& frame_origin, + double maxrange, bool lazy_eval) { + // performs transformation to data and sensor origin first Pointcloud transformed_scan (pc); transformed_scan.transform(frame_origin); point3d transformed_sensor_origin = frame_origin.transform(sensor_origin); - insertScan(transformed_scan, transformed_sensor_origin, maxrange, pruning, lazy_eval); + insertPointCloud(transformed_scan, transformed_sensor_origin, maxrange, lazy_eval); } template - void OccupancyOcTreeBase::insertScanNaive(const Pointcloud& pc, const point3d& origin, double maxrange, bool pruning, bool lazy_eval) { + void OccupancyOcTreeBase::insertPointCloudRays(const Pointcloud& pc, const point3d& origin, double maxrange, bool lazy_eval) { if (pc.size() < 1) return; - // integrate each single beam - octomap::point3d p; - for (octomap::Pointcloud::const_iterator point_it = pc.begin(); - point_it != pc.end(); point_it++) { - this->insertRay(origin, *point_it, maxrange, lazy_eval); - } +#ifdef _OPENMP + omp_set_num_threads(this->keyrays.size()); + #pragma omp parallel for +#endif + for (int i = 0; i < (int)pc.size(); ++i) { + const point3d& p = pc[i]; + unsigned threadIdx = 0; +#ifdef _OPENMP + threadIdx = omp_get_thread_num(); +#endif + KeyRay* keyray = &(this->keyrays.at(threadIdx)); + + if (this->computeRayKeys(origin, p, *keyray)){ +#ifdef _OPENMP + #pragma omp critical +#endif + { + for(KeyRay::iterator it=keyray->begin(); it != keyray->end(); it++) { + updateNode(*it, false, lazy_eval); // insert freespace measurement + } + updateNode(p, true, lazy_eval); // update endpoint to be occupied + } + } - if (pruning) - this->prune(); + } } template void OccupancyOcTreeBase::computeUpdate(const Pointcloud& scan, const octomap::point3d& origin, KeySet& free_cells, KeySet& occupied_cells, - double maxrange) { - - //#pragma omp parallel private (local_key_ray, point_it) - for (Pointcloud::const_iterator point_it = scan.begin(); point_it != scan.end(); point_it++) { - const point3d& p = *point_it; + double maxrange) + { +#ifdef _OPENMP + omp_set_num_threads(this->keyrays.size()); + #pragma omp parallel for schedule(guided) +#endif + for (int i = 0; i < (int)scan.size(); ++i) { + const point3d& p = scan[i]; + unsigned threadIdx = 0; +#ifdef _OPENMP + threadIdx = omp_get_thread_num(); +#endif + KeyRay* keyray = &(this->keyrays.at(threadIdx)); if (!use_bbx_limit) { // -------------- no BBX specified --------------- if ((maxrange < 0.0) || ((p - origin).norm() <= maxrange) ) { // is not maxrange meas. // free cells - if (this->computeRayKeys(origin, p, this->keyray)){ - free_cells.insert(this->keyray.begin(), this->keyray.end()); + if (this->computeRayKeys(origin, p, *keyray)){ +#ifdef _OPENMP + #pragma omp critical (free_insert) +#endif + { + free_cells.insert(keyray->begin(), keyray->end()); + } } // occupied endpoint OcTreeKey key; - if (this->coordToKeyChecked(p, key)) - occupied_cells.insert(key); + if (this->coordToKeyChecked(p, key)){ +#ifdef _OPENMP + #pragma omp critical (occupied_insert) +#endif + { + occupied_cells.insert(key); + } + } } // end if NOT maxrange else { // user set a maxrange and this is reached point3d direction = (p - origin).normalized (); point3d new_end = origin + direction * (float) maxrange; - if (this->computeRayKeys(origin, new_end, this->keyray)){ - free_cells.insert(this->keyray.begin(), this->keyray.end()); + if (this->computeRayKeys(origin, new_end, *keyray)){ +#ifdef _OPENMP + #pragma omp critical (free_insert) +#endif + { + free_cells.insert(keyray->begin(), keyray->end()); + } } } // end if maxrange } @@ -163,14 +200,25 @@ namespace octomap { // occupied endpoint OcTreeKey key; - if (this->coordToKeyChecked(p, key)) - occupied_cells.insert(key); + if (this->coordToKeyChecked(p, key)){ +#ifdef _OPENMP + #pragma omp critical (occupied_insert) +#endif + { + occupied_cells.insert(key); + } + } // update freespace, break as soon as bbx limit is reached - if (this->computeRayKeys(origin, p, this->keyray)){ - for(KeyRay::reverse_iterator rit=this->keyray.rbegin(); rit != this->keyray.rend(); rit++) { + if (this->computeRayKeys(origin, p, *keyray)){ + for(KeyRay::reverse_iterator rit=keyray->rbegin(); rit != keyray->rend(); rit++) { if (inBBX(*rit)) { - free_cells.insert(*rit); +#ifdef _OPENMP + #pragma omp critical (free_insert) +#endif + { + free_cells.insert(*rit); + } } else break; } @@ -178,7 +226,7 @@ namespace octomap { } // end if in BBX and not maxrange } // end bbx case - } // end for all points + } // end for all points, end of parallel OMP loop // prefer occupied cells over free ones (and make sets disjunct) for(KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ){ @@ -192,7 +240,25 @@ namespace octomap { template NODE* OccupancyOcTreeBase::updateNode(const OcTreeKey& key, float log_odds_update, bool lazy_eval) { - return updateNodeRecurs(this->root, false, key, 0, log_odds_update, lazy_eval); + // early abort (no change will happen). + // may cause an overhead in some configuration, but more often helps + NODE* leaf = this->search(key); + // no change: node already at threshold + if (leaf + && ((log_odds_update >= 0 && leaf->getLogOdds() >= this->clamping_thres_max) + || ( log_odds_update <= 0 && leaf->getLogOdds() <= this->clamping_thres_min))) + { + return leaf; + } + + bool createdRoot = false; + if (this->root == NULL){ + this->root = new NODE(); + this->tree_size++; + createdRoot = true; + } + + return updateNodeRecurs(this->root, createdRoot, key, 0, log_odds_update, lazy_eval); } template @@ -215,13 +281,11 @@ namespace octomap { template NODE* OccupancyOcTreeBase::updateNode(const OcTreeKey& key, bool occupied, bool lazy_eval) { - NODE* leaf = this->search(key); - // no change: node already at threshold - if (leaf && (this->isNodeAtThreshold(leaf)) && (this->isNodeOccupied(leaf) == occupied)) { - return leaf; - } - if (occupied) return updateNodeRecurs(this->root, false, key, 0, this->prob_hit_log, lazy_eval); - else return updateNodeRecurs(this->root, false, key, 0, this->prob_miss_log, lazy_eval); + float logOdds = this->prob_miss_log; + if (occupied) + logOdds = this->prob_hit_log; + + return updateNode(key, logOdds, lazy_eval); } template @@ -243,16 +307,17 @@ namespace octomap { template NODE* OccupancyOcTreeBase::updateNodeRecurs(NODE* node, bool node_just_created, const OcTreeKey& key, unsigned int depth, const float& log_odds_update, bool lazy_eval) { - unsigned int pos = computeChildIdx(key, this->tree_depth-1-depth); + unsigned int pos = computeChildIdx(key, this->tree_depth -1 - depth); bool created_node = false; + assert(node); + // follow down to last level if (depth < this->tree_depth) { if (!node->childExists(pos)) { // child does not exist, but maybe it's a pruned node? - if ((!node->hasChildren()) && !node_just_created && (node != this->root)) { + if ((!node->hasChildren()) && !node_just_created ) { // current node does not have children AND it is not a new node - // AND its not the root node // -> expand pruned node node->expandNode(); this->tree_size+=8; @@ -271,8 +336,13 @@ namespace octomap { return updateNodeRecurs(node->getChild(pos), created_node, key, depth+1, log_odds_update, lazy_eval); else { NODE* retval = updateNodeRecurs(node->getChild(pos), created_node, key, depth+1, log_odds_update, lazy_eval); - // set own probability according to prob of children - node->updateOccupancyChildren(); + // prune node if possible, otherwise set own probability + // note: combining both did not lead to a speedup! + if (node->pruneNode()) + this->tree_size -= 8; + else + node->updateOccupancyChildren(); + return retval; } } @@ -282,6 +352,7 @@ namespace octomap { if (use_change_detection) { bool occBefore = this->isNodeOccupied(node); updateNodeLogOdds(node, log_odds_update); + if (node_just_created){ // new node changed_keys.insert(std::pair(key, true)); } else if (occBefore != this->isNodeOccupied(node)) { // occupancy changed, track it @@ -291,48 +362,23 @@ namespace octomap { else if (it->second == false) changed_keys.erase(it); } - } - else { + } else { updateNodeLogOdds(node, log_odds_update); } return node; } } - - template - void OccupancyOcTreeBase::calcNumThresholdedNodes(unsigned int& num_thresholded, - unsigned int& num_other) const { - num_thresholded = 0; - num_other = 0; - // TODO: The recursive call could be completely replaced with the new iterators - calcNumThresholdedNodesRecurs(this->root, num_thresholded, num_other); - } - - template - void OccupancyOcTreeBase::calcNumThresholdedNodesRecurs (NODE* node, - unsigned int& num_thresholded, - unsigned int& num_other) const { - assert(node != NULL); - for (unsigned int i=0; i<8; i++) { - if (node->childExists(i)) { - NODE* child_node = node->getChild(i); - if (this->isNodeAtThreshold(child_node)) - num_thresholded++; - else - num_other++; - calcNumThresholdedNodesRecurs(child_node, num_thresholded, num_other); - } // end if child - } // end for children - } - template void OccupancyOcTreeBase::updateInnerOccupancy(){ - this->updateInnerOccupancyRecurs(this->root, 0); + if (this->root) + this->updateInnerOccupancyRecurs(this->root, 0); } template void OccupancyOcTreeBase::updateInnerOccupancyRecurs(NODE* node, unsigned int depth){ + assert(node); + // only recurse and update for inner nodes: if (node->hasChildren()){ // return early for last level: @@ -349,6 +395,8 @@ namespace octomap { template void OccupancyOcTreeBase::toMaxLikelihood() { + if (this->root == NULL) + return; // convert bottom up for (unsigned int depth=this->tree_depth; depth>0; depth--) { @@ -363,6 +411,8 @@ namespace octomap { void OccupancyOcTreeBase::toMaxLikelihoodRecurs(NODE* node, unsigned int depth, unsigned int max_depth) { + assert(node); + if (depth < max_depth) { for (unsigned int i=0; i<8; i++) { if (node->childExists(i)) { @@ -505,11 +555,11 @@ namespace octomap { template inline bool OccupancyOcTreeBase::integrateMissOnRay(const point3d& origin, const point3d& end, bool lazy_eval) { - if (!this->computeRayKeys(origin, end, this->keyray)) { + if (!this->computeRayKeys(origin, end, this->keyrays.at(0))) { return false; } - for(KeyRay::iterator it=this->keyray.begin(); it != this->keyray.end(); it++) { + for(KeyRay::iterator it=this->keyrays[0].begin(); it != this->keyrays[0].end(); it++) { updateNode(*it, false, lazy_eval); // insert freespace measurement } @@ -536,96 +586,10 @@ namespace octomap { } } - template - void OccupancyOcTreeBase::getOccupied(point3d_list& node_centers, unsigned int max_depth) const { - - if (max_depth == 0) - max_depth = this->tree_depth; - - for(typename OccupancyOcTreeBase::leaf_iterator it = this->begin(max_depth), - end=this->end(); it!= end; ++it) - { - if(this->isNodeOccupied(*it)) - node_centers.push_back(it.getCoordinate()); - } - } - - - template - void OccupancyOcTreeBase::getOccupied(std::list& occupied_nodes, unsigned int max_depth) const{ - - if (max_depth == 0) - max_depth = this->tree_depth; - - for(typename OccupancyOcTreeBase::leaf_iterator it = this->begin(max_depth), - end=this->end(); it!= end; ++it) - { - if(this->isNodeOccupied(*it)) - occupied_nodes.push_back(OcTreeVolume(it.getCoordinate(), it.getSize())); - } - - } - - - template - void OccupancyOcTreeBase::getOccupied(std::list& binary_nodes, - std::list& delta_nodes, - unsigned int max_depth) const{ - if (max_depth == 0) - max_depth = this->tree_depth; - - for(typename OccupancyOcTreeBase::leaf_iterator it = this->begin(max_depth), - end=this->end(); it!= end; ++it) - { - if(this->isNodeOccupied(*it)){ - if (it->getLogOdds() >= this->clamping_thres_max) - binary_nodes.push_back(OcTreeVolume(it.getCoordinate(), it.getSize())); - else - delta_nodes.push_back(OcTreeVolume(it.getCoordinate(), it.getSize())); - } - } - } - - template - void OccupancyOcTreeBase::getFreespace(std::list& free_nodes, unsigned int max_depth) const{ - - if (max_depth == 0) - max_depth = this->tree_depth; - - for(typename OccupancyOcTreeBase::leaf_iterator it = this->begin(max_depth), - end=this->end(); it!= end; ++it) - { - if(!this->isNodeOccupied(*it)) - free_nodes.push_back(OcTreeVolume(it.getCoordinate(), it.getSize())); - } - } - - - template - void OccupancyOcTreeBase::getFreespace(std::list& binary_nodes, - std::list& delta_nodes, - unsigned int max_depth) const{ - - if (max_depth == 0) - max_depth = this->tree_depth; - - for(typename OccupancyOcTreeBase::leaf_iterator it = this->begin(max_depth), - end=this->end(); it!= end; ++it) - { - if(!this->isNodeOccupied(*it)){ - if (it->getLogOdds() <= this->clamping_thres_min) - binary_nodes.push_back(OcTreeVolume(it.getCoordinate(), it.getSize())); - else - delta_nodes.push_back(OcTreeVolume(it.getCoordinate(), it.getSize())); - } - } - } - - template void OccupancyOcTreeBase::setBBXMin (point3d& min) { bbx_min = min; - if (!this->genKey(bbx_min, bbx_min_key)) { + if (!this->coordToKeyChecked(bbx_min, bbx_min_key)) { OCTOMAP_ERROR("ERROR while generating bbx min key.\n"); } } @@ -633,7 +597,7 @@ namespace octomap { template void OccupancyOcTreeBase::setBBXMax (point3d& max) { bbx_max = max; - if (!this->genKey(bbx_max, bbx_max_key)) { + if (!this->coordToKeyChecked(bbx_max, bbx_max_key)) { OCTOMAP_ERROR("ERROR while generating bbx max key.\n"); } } @@ -666,58 +630,17 @@ namespace octomap { return bbx_min + obj_bounds; } - - template - void OccupancyOcTreeBase::getOccupiedLeafsBBX(point3d_list& node_centers, point3d min, point3d max) const { - - OcTreeKey root_key, min_key, max_key; - root_key[0] = root_key[1] = root_key[2] = this->tree_max_val; - if (!this->genKey(min, min_key)) return; - if (!this->genKey(max, max_key)) return; - getOccupiedLeafsBBXRecurs(node_centers, this->tree_depth, this->root, 0, root_key, min_key, max_key); - } - - - template - void OccupancyOcTreeBase::getOccupiedLeafsBBXRecurs( point3d_list& node_centers, unsigned int max_depth, - NODE* node, unsigned int depth, const OcTreeKey& parent_key, - const OcTreeKey& min, const OcTreeKey& max) const { - if (depth == max_depth) { // max level reached - if (this->isNodeOccupied(node)) { - node_centers.push_back(this->keyToCoords(parent_key, depth)); - } - } - - if (!node->hasChildren()) return; - - unsigned short int center_offset_key = this->tree_max_val >> (depth + 1); - - OcTreeKey child_key; - - for (unsigned int i=0; i<8; ++i) { - if (node->childExists(i)) { - - computeChildKey(i, center_offset_key, parent_key, child_key); - - // overlap of query bbx and child bbx? - if (!( - ( min[0] > (child_key[0] + center_offset_key)) || - ( max[0] < (child_key[0] - center_offset_key)) || - ( min[1] > (child_key[1] + center_offset_key)) || - ( max[1] < (child_key[1] - center_offset_key)) || - ( min[2] > (child_key[2] + center_offset_key)) || - ( max[2] < (child_key[2] - center_offset_key)) - )) { - getOccupiedLeafsBBXRecurs(node_centers, max_depth, node->getChild(i), depth+1, child_key, min, max); - } - } - } - } - // -- I/O ----------------------------------------- template std::istream& OccupancyOcTreeBase::readBinaryData(std::istream &s){ + // tree needs to be newly created or cleared externally + if (this->root) { + OCTOMAP_ERROR_STR("Trying to read into an existing tree."); + return s; + } + + this->root = new NODE(); this->readBinaryNode(s, this->root); this->size_changed = true; this->tree_size = OcTreeBaseImpl::calcNumNodes(); // compute number of nodes @@ -727,13 +650,16 @@ namespace octomap { template std::ostream& OccupancyOcTreeBase::writeBinaryData(std::ostream &s) const{ OCTOMAP_DEBUG("Writing %zu nodes to output stream...", this->size()); - this->writeBinaryNode(s, this->root); + if (this->root) + this->writeBinaryNode(s, this->root); return s; } template std::istream& OccupancyOcTreeBase::readBinaryNode(std::istream &s, NODE* node) const { + assert(node); + char child1to4_char; char child5to8_char; s.read((char*)&child1to4_char, sizeof(char)); @@ -803,6 +729,8 @@ namespace octomap { template std::ostream& OccupancyOcTreeBase::writeBinaryNode(std::ostream &s, const NODE* node) const{ + assert(node); + // 2 bits for each children, 8 children per node -> 16 bits std::bitset<8> child1to4; std::bitset<8> child5to8; diff --git a/octomap/include/octomap/Pointcloud.h b/octomap/include/octomap/Pointcloud.h index 58cc4b5c..fbd6dfc3 100644 --- a/octomap/include/octomap/Pointcloud.h +++ b/octomap/include/octomap/Pointcloud.h @@ -102,7 +102,12 @@ namespace octomap { const_iterator begin() const { return points.begin(); } const_iterator end() const { return points.end(); } point3d back() { return points.back(); } - point3d getPoint(unsigned int i); // may return NULL + /// Returns a copy of the ith point in point cloud. + /// Use operator[] for direct access to point reference. + point3d getPoint(unsigned int i) const; // may return NULL + + inline const point3d& operator[] (size_t i) const { return points[i]; } + inline point3d& operator[] (size_t i) { return points[i]; } // I/O methods diff --git a/octomap/include/octomap/ScanGraph.h b/octomap/include/octomap/ScanGraph.h index b3f0a480..81970fd8 100644 --- a/octomap/include/octomap/ScanGraph.h +++ b/octomap/include/octomap/ScanGraph.h @@ -213,7 +213,7 @@ namespace octomap { * all 3D point following until the next NODE keyword (or end of file) are * inserted into that scan node as pointcloud in its local coordinate frame * - * @param input stream to read from + * @param s input stream to read from * @return read stream */ std::istream& readPlainASCII(std::istream& s); diff --git a/octomap/include/octomap/math/Vector3.h b/octomap/include/octomap/math/Vector3.h index 14c2dfa6..49a4a73b 100644 --- a/octomap/include/octomap/math/Vector3.h +++ b/octomap/include/octomap/math/Vector3.h @@ -53,7 +53,7 @@ namespace octomath { /*! * \brief Default constructor */ - Vector3 () { data[0] = data[1] = data[2] = 0; } + Vector3 () { data[0] = data[1] = data[2] = 0.0; } /*! * \brief Copy constructor @@ -113,6 +113,7 @@ namespace octomath { x()*other.y() - y()*other.x()); } + /// dot product inline double dot (const Vector3& other) const { return x()*other.x() + y()*other.y() + z()*other.z(); @@ -255,21 +256,25 @@ namespace octomath { return true; } + /// @return length of the vector ("L2 norm") inline double norm () const { - double n = 0; - for (unsigned int i=0; i<3; i++) { - n += operator()(i) * operator()(i); - } - return sqrt(n); + return sqrt(norm_sq()); + } + + /// @return squared length ("L2 norm") of the vector + inline double norm_sq() const { + return (x()*x() + y()*y() + z()*z()); } + /// normalizes this vector, so that it has norm=1.0 inline Vector3& normalize () { - double len = norm (); + double len = norm(); if (len > 0) *this /= (float) len; return *this; } + /// @return normalized vector, this one remains unchanged inline Vector3 normalized () const { Vector3 result(*this); result.normalize (); diff --git a/octomap/include/octomap/octomap_deprecated.h b/octomap/include/octomap/octomap_deprecated.h index 2f6a7a1c..2e7fc1e2 100644 --- a/octomap/include/octomap/octomap_deprecated.h +++ b/octomap/include/octomap/octomap_deprecated.h @@ -35,14 +35,14 @@ #define OCTOMAP_DEPRECATED_H // define multi-platform deprecation mechanism -#ifndef DEPRECATED +#ifndef OCTOMAP_DEPRECATED #ifdef __GNUC__ - #define DEPRECATED(func) func __attribute__ ((deprecated)) + #define OCTOMAP_DEPRECATED(func) func __attribute__ ((deprecated)) #elif defined(_MSC_VER) - #define DEPRECATED(func) __declspec(deprecated) func + #define OCTOMAP_DEPRECATED(func) __declspec(deprecated) func #else - #pragma message("WARNING: You need to implement DEPRECATED for this compiler") - #define DEPRECATED(func) func + #pragma message("WARNING: You need to implement OCTOMAP_DEPRECATED for this compiler") + #define OCTOMAP_DEPRECATED(func) func #endif #endif diff --git a/octomap/include/octomap/octomap_types.h b/octomap/include/octomap/octomap_types.h index f9125c4a..8fb54d34 100644 --- a/octomap/include/octomap/octomap_types.h +++ b/octomap/include/octomap/octomap_types.h @@ -34,6 +34,7 @@ #ifndef OCTOMAP_TYPES_H #define OCTOMAP_TYPES_H +#include #include #include diff --git a/octomap/octomap-config.cmake.in b/octomap/octomap-config.cmake.in index c36022c6..30aaf4ea 100644 --- a/octomap/octomap-config.cmake.in +++ b/octomap/octomap-config.cmake.in @@ -1,17 +1,34 @@ -# - Config file for the OctoMap package -# (example from http://www.vtk.org/Wiki/CMake/Tutorials/How_to_create_a_ProjectConfig.cmake_file) -# It defines the following variables -# OCTOMAP_INCLUDE_DIRS - include directories for OctoMap -# OCTOMAP_LIBRARY_DIRS - library directories for OctoMap (normally not used!) -# OCTOMAP_LIBRARIES - libraries to link against +# =================================================================================== +# The OctoMap CMake configuration file +# +# ** File generated automatically, do not modify ** +# +# Usage from an external project: +# In your CMakeLists.txt, add these lines: +# +# FIND_PACKAGE(octomap REQUIRED ) +# TARGET_LINK_LIBRARIES(MY_TARGET_NAME ${OCTOMAP_LIBRARIES}) +# +# This file will define the following variables: +# - OCTOMAP_LIBRARIES : The list of libraries to links against. +# - OCTOMAP_LIBRARY_DIRS : The directory where lib files are. Calling +# LINK_DIRECTORIES with this path is NOT needed. +# - OCTOMAP_INCLUDE_DIRS : The OpenCV include directories. +# +# Based on the example CMake Tutorial +# http://www.vtk.org/Wiki/CMake/Tutorials/How_to_create_a_ProjectConfig.cmake_file +# and OpenCVConfig.cmake.in from OpenCV +# =================================================================================== + -# Tell the user project where to find our headers and libraries set(OCTOMAP_INCLUDE_DIRS @OCTOMAP_INCLUDE_DIRS@) set(OCTOMAP_LIBRARY_DIRS @OCTOMAP_LIB_DIR@) # Our library dependencies (contains definitions for IMPORTED targets) # include("@FOOBAR_CMAKE_DIR@/FooBarLibraryDepends.cmake") - -set(OCTOMAP_LIBRARIES octomap octomath) -#set(FOOBAR_EXECUTABLE bar) \ No newline at end of file +# Set library names as absolute paths: +set(OCTOMAP_LIBRARIES + @OCTOMAP_LIB_DIR@/liboctomap.so + @OCTOMAP_LIB_DIR@/liboctomath.so +) \ No newline at end of file diff --git a/octomap/package.xml b/octomap/package.xml new file mode 100644 index 00000000..5875aedd --- /dev/null +++ b/octomap/package.xml @@ -0,0 +1,22 @@ + + octomap + 1.6.0 + The OctoMap library implements a 3D occupancy grid mapping approach, providing data structures and mapping algorithms in C++. The map implementation is based on an octree. See + http://octomap.github.com for details. + + Kai M. Wurm + Armin Hornung + Armin Hornung + BSD + + http://octomap.github.com + https://github.com/OctoMap/octomap/issues + + catkin + cmake + + + cmake + + + diff --git a/octomap/src/testing/geb079.bt b/octomap/share/data/geb079.bt similarity index 100% rename from octomap/src/testing/geb079.bt rename to octomap/share/data/geb079.bt diff --git a/octomap/src/testing/mapcoll.txt b/octomap/share/data/mapcoll.txt similarity index 100% rename from octomap/src/testing/mapcoll.txt rename to octomap/share/data/mapcoll.txt diff --git a/data/scan.dat.bz2 b/octomap/share/data/scan.dat.bz2 similarity index 100% rename from data/scan.dat.bz2 rename to octomap/share/data/scan.dat.bz2 diff --git a/octomap/example-project.tgz b/octomap/share/example-project.tgz similarity index 100% rename from octomap/example-project.tgz rename to octomap/share/example-project.tgz diff --git a/octomap/src/AbstractOcTree.cpp b/octomap/src/AbstractOcTree.cpp index 58cbe581..d1d7f627 100644 --- a/octomap/src/AbstractOcTree.cpp +++ b/octomap/src/AbstractOcTree.cpp @@ -107,7 +107,9 @@ namespace octomap { AbstractOcTree* tree = createTree(id, res); if (tree){ - tree->readData(s); + if (size > 0) + tree->readData(s); + OCTOMAP_DEBUG_STR("Done ("<< tree->size() << " nodes)"); } diff --git a/octomap/src/AbstractOccupancyOcTree.cpp b/octomap/src/AbstractOccupancyOcTree.cpp index 39647392..ee01b341 100644 --- a/octomap/src/AbstractOccupancyOcTree.cpp +++ b/octomap/src/AbstractOccupancyOcTree.cpp @@ -166,7 +166,8 @@ namespace octomap { this->clear(); this->setResolution(res); - this->readBinaryData(s); + if (size > 0) + this->readBinaryData(s); if (size != this->size()){ OCTOMAP_ERROR("Tree size mismatch: # read nodes (%zu) != # expected nodes (%d)\n",this->size(), size); diff --git a/octomap/src/OcTreeNode.cpp b/octomap/src/OcTreeNode.cpp index 32b33385..a6e43193 100644 --- a/octomap/src/OcTreeNode.cpp +++ b/octomap/src/OcTreeNode.cpp @@ -73,7 +73,9 @@ namespace octomap { c++; } } - if (c) mean /= (double) c; + if (c) + mean /= (double) c; + return log(mean/(1-mean)); } @@ -82,7 +84,8 @@ namespace octomap { for (unsigned int i=0; i<8; i++) { if (childExists(i)) { float l = getChild(i)->getLogOdds(); - if (l > max) max = l; + if (l > max) + max = l; } } return max; diff --git a/octomap/src/Pointcloud.cpp b/octomap/src/Pointcloud.cpp index bf25daf7..30639d29 100644 --- a/octomap/src/Pointcloud.cpp +++ b/octomap/src/Pointcloud.cpp @@ -80,8 +80,9 @@ namespace octomap { } } - point3d Pointcloud::getPoint(unsigned int i) { - if (i 0) cout << "("<insertScan(**scan_it, maxrange, false); + tree->insertPointCloud(**scan_it, maxrange); } else cout << "(SKIP) " << flush; @@ -118,6 +118,8 @@ int main(int argc, char** argv) { currentScan++; } + tree->expand(); + cout << "\nEvaluating scans\n===========================\n"; currentScan = 1; diff --git a/octomap/src/graph2tree.cpp b/octomap/src/graph2tree.cpp index 5528b5d1..3e9bcb26 100644 --- a/octomap/src/graph2tree.cpp +++ b/octomap/src/graph2tree.cpp @@ -57,8 +57,8 @@ void printUsage(char* self){ " -m (optional) \n" " -n (optional) \n" " -log (enable a detailed log file with statistics) \n" - " -compress (enable lossless compression after every scan)\n" " -compressML (enable maximum-likelihood compression (lossy) after every scan)\n" + " -simple (simple scan insertion ray by ray instead of optimized) \n" " -clamping (override default sensor model clamping probabilities between 0..1)\n" " -sensor (override default sensor model hit and miss probabilities between 0..1)" "\n"; @@ -69,9 +69,24 @@ void printUsage(char* self){ exit(0); } +void calcThresholdedNodes(const OcTree* tree, + unsigned int& num_thresholded, + unsigned int& num_other) +{ + num_thresholded = 0; + num_other = 0; + + for(OcTree::tree_iterator it = tree->begin_tree(), end=tree->end_tree(); it!= end; ++it){ + if (tree->isNodeAtThreshold(*it)) + num_thresholded++; + else + num_other++; + } +} + void outputStatistics(const OcTree* tree){ unsigned int numThresholded, numOther; - tree->calcNumThresholdedNodes(numThresholded, numOther); + calcThresholdedNodes(tree, numThresholded, numOther); size_t memUsage = tree->memoryUsage(); unsigned long long memFullGrid = tree->memoryFullGrid(); size_t numLeafNodes = tree->getNumLeafNodes(); @@ -85,6 +100,7 @@ void outputStatistics(const OcTree* tree){ cout << endl; } + int main(int argc, char** argv) { // default values: double res = 0.1; @@ -93,7 +109,8 @@ int main(int argc, char** argv) { double maxrange = -1; int max_scan_no = -1; bool detailedLog = false; - unsigned char compression = 0; + bool simpleUpdate = false; + unsigned char compression = 1; // get default sensor model values: OcTree emptyTree(0.1); @@ -118,8 +135,10 @@ int main(int argc, char** argv) { res = atof(argv[++arg]); else if (! strcmp(argv[arg], "-log")) detailedLog = true; + else if (! strcmp(argv[arg], "-simple")) + simpleUpdate = true; else if (! strcmp(argv[arg], "-compress")) - compression = 1; + OCTOMAP_WARNING("Argument -compress no longer has an effect, incremental pruning is done during each insertion.\n"); else if (! strcmp(argv[arg], "-compressML")) compression = 2; else if (! strcmp(argv[arg], "-m")) @@ -180,6 +199,20 @@ int main(int argc, char** argv) { num_points_in_graph = graph->getNumPoints(); cout << "\n Data points in graph: " << num_points_in_graph << endl; } + + // transform pointclouds first, so we can directly operate on them later + for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) { + + pose6d frame_origin = (*scan_it)->pose; + point3d sensor_origin = frame_origin.inv().transform((*scan_it)->pose.trans()); + + (*scan_it)->scan->transform(frame_origin); + point3d transformed_sensor_origin = frame_origin.transform(sensor_origin); + (*scan_it)->pose = pose6d(transformed_sensor_origin, octomath::Quaternion()); + + } + + std::ofstream logfile; if (detailedLog){ logfile.open((treeFilename+".log").c_str()); @@ -188,6 +221,8 @@ int main(int argc, char** argv) { logfile << "# [scan number] [bytes octree] [bytes full 3D grid]\n"; } + + cout << "\nCreating tree\n===========================\n"; OcTree* tree = new OcTree(res); @@ -204,7 +239,11 @@ int main(int argc, char** argv) { if (max_scan_no > 0) cout << "("<insertScan(**scan_it, maxrange, (compression==1)); + if (simpleUpdate) + tree->insertPointCloudRays((*scan_it)->scan, (*scan_it)->pose.trans(), maxrange); + else + tree->insertPointCloud((*scan_it)->scan, (*scan_it)->pose.trans(), maxrange); + if (compression == 2){ tree->toMaxLikelihood(); tree->prune(); @@ -234,11 +273,7 @@ int main(int argc, char** argv) { cout << "time to insert 100.000 points took: " << time_to_insert/ ((double) num_points_in_graph / 100000) << " sec (avg)" << endl << endl; - std::cout << "Full tree\n" << "===========================\n"; - outputStatistics(tree); - std::cout << "Pruned tree (lossless compression)\n" << "===========================\n"; - tree->prune(); outputStatistics(tree); tree->write(treeFilenameOT); diff --git a/octomap/src/math/Pose6D.cpp b/octomap/src/math/Pose6D.cpp index 060ff86c..2a031848 100644 --- a/octomap/src/math/Pose6D.cpp +++ b/octomap/src/math/Pose6D.cpp @@ -1,16 +1,10 @@ -// $Id$ - -/** -* OctoMap: -* A probabilistic, flexible, and compact 3D mapping library for robotic systems. -* @author K. M. Wurm, A. Hornung, University of Freiburg, Copyright (C) 2009. -* @see http://octomap.sourceforge.net/ -* License: New BSD License -*/ - /* - * Copyright (c) 2009-2011, K. M. Wurm, A. Hornung, University of Freiburg + * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees + * http://octomap.github.com/ + * + * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg * All rights reserved. + * License: New BSD * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/octomap/src/math/Quaternion.cpp b/octomap/src/math/Quaternion.cpp index 2c6bae76..0aa57fde 100644 --- a/octomap/src/math/Quaternion.cpp +++ b/octomap/src/math/Quaternion.cpp @@ -1,16 +1,10 @@ -// $Id$ - -/** -* OctoMap: -* A probabilistic, flexible, and compact 3D mapping library for robotic systems. -* @author K. M. Wurm, A. Hornung, University of Freiburg, Copyright (C) 2009. -* @see http://octomap.sourceforge.net/ -* License: New BSD License -*/ - /* - * Copyright (c) 2009-2011, K. M. Wurm, A. Hornung, University of Freiburg + * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees + * http://octomap.github.com/ + * + * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg * All rights reserved. + * License: New BSD * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/octomap/src/math/Vector3.cpp b/octomap/src/math/Vector3.cpp index f1110c33..d1de3f2d 100644 --- a/octomap/src/math/Vector3.cpp +++ b/octomap/src/math/Vector3.cpp @@ -1,16 +1,10 @@ -// $Id$ - -/** -* OctoMap: -* A probabilistic, flexible, and compact 3D mapping library for robotic systems. -* @author K. M. Wurm, A. Hornung, University of Freiburg, Copyright (C) 2009. -* @see http://octomap.sourceforge.net/ -* License: New BSD License -*/ - /* - * Copyright (c) 2009-2011, K. M. Wurm, A. Hornung, University of Freiburg + * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees + * http://octomap.github.com/ + * + * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg * All rights reserved. + * License: New BSD * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/octomap/src/testing/CMakeLists.txt b/octomap/src/testing/CMakeLists.txt index 005ef33b..f32e4082 100644 --- a/octomap/src/testing/CMakeLists.txt +++ b/octomap/src/testing/CMakeLists.txt @@ -45,7 +45,7 @@ ADD_TEST (NAME StampedTree COMMAND unit_tests StampedTree ) ADD_TEST (NAME OcTreeKey COMMAND unit_tests OcTreeKey ) ADD_TEST (NAME OcTreeIterator COMMAND unit_tests OcTreeIterator ) ADD_TEST (NAME test_pruning COMMAND test_pruning ) -ADD_TEST (NAME test_iterators COMMAND test_iterators ${CMAKE_CURRENT_SOURCE_DIR}/geb079.bt) -ADD_TEST (NAME test_io COMMAND test_io ${CMAKE_CURRENT_SOURCE_DIR}/geb079.bt) -ADD_TEST (NAME test_mapcollection COMMAND test_mapcollection ${CMAKE_CURRENT_SOURCE_DIR}/mapcoll.txt) +ADD_TEST (NAME test_iterators COMMAND test_iterators ${PROJECT_SOURCE_DIR}/share/data/geb079.bt) +ADD_TEST (NAME test_io COMMAND test_io ${PROJECT_SOURCE_DIR}/share/data/geb079.bt) +ADD_TEST (NAME test_mapcollection COMMAND test_mapcollection ${PROJECT_SOURCE_DIR}/share/data/mapcoll.txt) ADD_TEST (NAME test_color_tree COMMAND test_color_tree) diff --git a/octomap/src/testing/test_changedkeys.cpp b/octomap/src/testing/test_changedkeys.cpp index 1e8dfcd9..20499879 100644 --- a/octomap/src/testing/test_changedkeys.cpp +++ b/octomap/src/testing/test_changedkeys.cpp @@ -81,7 +81,7 @@ int main(int argc, char** argv) { } // insert in global coordinates: - tree.insertScan(cloud, origin, -1, false); + tree.insertPointCloud(cloud, origin, -1); } printChanges(tree); diff --git a/octomap/src/testing/test_io.cpp b/octomap/src/testing/test_io.cpp index 0011f4cc..ac8627e7 100644 --- a/octomap/src/testing/test_io.cpp +++ b/octomap/src/testing/test_io.cpp @@ -17,6 +17,23 @@ int main(int argc, char** argv) { return 1; // exit 1 means failure } + //empty tree + OcTree emptyTree(0.999); + EXPECT_EQ(emptyTree.size(), 0); + EXPECT_TRUE(emptyTree.writeBinary("empty.bt")); + EXPECT_TRUE(emptyTree.write("empty.ot")); + + OcTree emptyReadTree(0.2); + EXPECT_TRUE(emptyReadTree.readBinary("empty.bt")); + EXPECT_EQ(emptyReadTree.size(), 0); + EXPECT_TRUE(emptyTree == emptyReadTree); + + EXPECT_TRUE(emptyReadTree.read("empty.ot")); + EXPECT_EQ(emptyReadTree.size(), 0); + EXPECT_TRUE(emptyTree == emptyReadTree); + + + string filename = string(argv[1]); string filenameOt = "test_io_file.ot"; @@ -30,7 +47,7 @@ int main(int argc, char** argv) { // test copy constructor / assignment: OcTree* treeCopy = new OcTree(tree); EXPECT_TRUE(tree == *treeCopy); - treeCopy->writeBinary(filenameBtCopyOut); + EXPECT_TRUE(treeCopy->writeBinary(filenameBtCopyOut)); // change a tree property, trees must be different afterwards treeCopy->setResolution(tree.getResolution()*2.0); diff --git a/octomap/src/testing/test_iterators.cpp b/octomap/src/testing/test_iterators.cpp index 676e4efc..f9f41b6a 100644 --- a/octomap/src/testing/test_iterators.cpp +++ b/octomap/src/testing/test_iterators.cpp @@ -152,6 +152,29 @@ int main(int argc, char** argv) { if (maxDepth== 0) maxDepth = tree_depth; + // iterate over empty tree: + OcTree emptyTree(0.2); + EXPECT_EQ(emptyTree.size(), 0); + EXPECT_EQ(emptyTree.calcNumNodes(), 0); + + size_t iteratedNodes = 0; + OcTree::tree_iterator t_it = emptyTree.begin_tree(maxDepth); + OcTree::tree_iterator t_end = emptyTree.end_tree(); + EXPECT_TRUE (t_it == t_end); + for( ; t_it != t_end; ++t_it){ + iteratedNodes++; + } + EXPECT_EQ(iteratedNodes, 0); + + + for(OcTree::leaf_iterator l_it = emptyTree.begin_leafs(maxDepth), l_end=emptyTree.end_leafs(); l_it!= l_end; ++l_it){ + iteratedNodes++; + } + EXPECT_EQ(iteratedNodes, 0); + + + + cout << "\nReading OcTree file\n===========================\n"; OcTree* tree = new OcTree(btFilename); @@ -374,10 +397,10 @@ int main(int argc, char** argv) { } - // test empty tree: - OcTree emptyTree(0.01); - emptyTree.updateNode(point3d(10, 10, 10), 5.0f); - for(OcTree::leaf_iterator it = emptyTree.begin_leafs(maxDepth), end=emptyTree.end_leafs(); it!= end; ++it) { + // test tree with one node: + OcTree simpleTree(0.01); + simpleTree.updateNode(point3d(10, 10, 10), 5.0f); + for(OcTree::leaf_iterator it = simpleTree.begin_leafs(maxDepth), end=simpleTree.end_leafs(); it!= end; ++it) { std::cout << it.getDepth() << " " << " "< #include #include +#include "testing.h" using namespace std; using namespace octomap; @@ -50,6 +51,7 @@ int main(int argc, char** argv) { //Read MapCollection from command line std::string filename(argv[1]); MapCollection > collection(filename); + EXPECT_TRUE(collection.size() > 0); //Write it to file collection.write("writeout.txt"); diff --git a/octomap/src/testing/test_pruning.cpp b/octomap/src/testing/test_pruning.cpp index 6c8ebc24..4f9b231d 100644 --- a/octomap/src/testing/test_pruning.cpp +++ b/octomap/src/testing/test_pruning.cpp @@ -86,7 +86,8 @@ int main(int argc, char** argv) { } } EXPECT_EQ(tree.calcNumNodes(), tree.size()); - EXPECT_EQ(37477, tree.size()); + EXPECT_EQ(29, tree.size()); + // TODO: replace with test for lazy eval? tree.prune(); EXPECT_EQ(tree.calcNumNodes(), tree.size()); EXPECT_EQ(29, tree.size()); @@ -94,6 +95,7 @@ int main(int argc, char** argv) { EXPECT_EQ(tree.calcNumNodes(), tree.size()); EXPECT_EQ(37477, tree.size()); tree.prune(); + EXPECT_EQ(29, tree.size()); // test expansion: for (float x=0.005; x <= 0.32; x+=res){ for (float y=0.005; y <= 0.32; y+=res){ diff --git a/octomap/src/testing/test_scans.cpp b/octomap/src/testing/test_scans.cpp index 2dfbd467..733c4093 100644 --- a/octomap/src/testing/test_scans.cpp +++ b/octomap/src/testing/test_scans.cpp @@ -31,7 +31,7 @@ int main(int argc, char** argv) { } // insert in global coordinates: - tree.insertScan(cloud, origin); + tree.insertPointCloud(cloud, origin); cout << "done." << endl; cout << "writing to spherical_scan.bt..." << endl; diff --git a/octomap/src/testing/unit_tests.cpp b/octomap/src/testing/unit_tests.cpp index 4a0620d8..3e73a3df 100644 --- a/octomap/src/testing/unit_tests.cpp +++ b/octomap/src/testing/unit_tests.cpp @@ -249,7 +249,7 @@ int main(int argc, char** argv) { } OcTree tree (0.05); - tree.insertScan(*measurement, origin); + tree.insertPointCloud(*measurement, origin); EXPECT_EQ ((int) tree.size(), 54076); ScanGraph* graph = new ScanGraph(); @@ -279,6 +279,7 @@ int main(int argc, char** argv) { EXPECT_TRUE (result); unsigned int tree_time = stamped_tree.getLastUpdateTime(); unsigned int node_time = result->getTimestamp(); + std::cout << "After 1st update (cube): Tree time " <getTimestamp() << std::endl; EXPECT_TRUE (tree_time > 0); #ifdef _WIN32 Sleep(1000); @@ -286,12 +287,18 @@ int main(int argc, char** argv) { sleep(1); #endif stamped_tree.integrateMissNoTime(result); // reduce occupancy, no time update + std::cout << "After 2nd update (single miss): Tree time " <getTimestamp()); // node time updated? - query = point3d (0.1f, 0.1f, 0.3f); - stamped_tree.updateNode(query, true); // integrate 'occupied' measurement - OcTreeNodeStamped* result2 = stamped_tree.search (query); + point3d query2 = point3d (0.1f, 0.1f, 0.3f); + stamped_tree.updateNode(query2, true); // integrate 'occupied' measurement + OcTreeNodeStamped* result2 = stamped_tree.search (query2); EXPECT_TRUE (result2); + result = stamped_tree.search (query); + EXPECT_TRUE (result); + std::cout << "After 3rd update (single hit at (0.1, 0.1, 0.3): Tree time " << stamped_tree.getLastUpdateTime() << "; node(0.1, 0.1, 0.1) time " << result->getTimestamp() + << "; node(0.1, 0.1, 0.3) time " << result2->getTimestamp() << std::endl; EXPECT_TRUE (result->getTimestamp() < result2->getTimestamp()); // result2 has been updated + EXPECT_EQ(result2->getTimestamp(), stamped_tree.getLastUpdateTime()); // ------------------------------------------------------------ } else if (test_name == "OcTreeKey") { OcTree tree (0.05); diff --git a/octovis/CMakeLists.txt b/octovis/CMakeLists.txt index d101e963..62222a18 100644 --- a/octovis/CMakeLists.txt +++ b/octovis/CMakeLists.txt @@ -3,8 +3,8 @@ PROJECT( octovis ) # # version (e.g. for packaging) set(OCTOVIS_MAJOR_VERSION 1) -set(OCTOVIS_MINOR_VERSION 5) -set(OCTOVIS_PATCH_VERSION 3) +set(OCTOVIS_MINOR_VERSION 6) +set(OCTOVIS_PATCH_VERSION 0) set(OCTOVIS_VERSION ${OCTOVIS_MAJOR_VERSION}.${OCTOVIS_MINOR_VERSION}.${OCTOVIS_PATCH_VERSION}) # get rid of a useless warning: if(COMMAND cmake_policy) @@ -22,7 +22,7 @@ SET( CMAKE_LIBRARY_OUTPUT_DIRECTORY ${BASE_DIR}/lib ) SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${BASE_DIR}/lib ) SET( CMAKE_RUNTIME_OUTPUT_DIRECTORY ${BASE_DIR}/bin ) -# We need the main octomap library to link againt. +# We need the main octomap library to link against. # Look at parent directory by default, in case the complete distribution # (octomap & octovis together) is built. # Otherwise you need to export octomap_DIR to find the CMakeConfig. @@ -31,7 +31,6 @@ find_package(octomap REQUIRED ${CMAKE_SOURCE_DIR}/../octomap/lib/cmake/octomap ) INCLUDE_DIRECTORIES(${OCTOMAP_INCLUDE_DIRS}) -LINK_DIRECTORIES(${OCTOMAP_LIBRARY_DIRS}) # Export the package for use from the build-tree # (this registers the build-tree with a global CMake-registry) @@ -96,25 +95,14 @@ IF(BUILD_VIEWER) "${PROJECT_BINARY_DIR}/InstallFiles/octovis-config-version.cmake" DESTINATION share/octovis/) - # make package release from source - IF (NOT WIN32) - SET(OCTOVIS_PKG_NAME "${PROJECT_NAME}-${OCTOVIS_VERSION}.tar.gz") - SET(DIST_DIR "${CMAKE_BINARY_DIR}/dist-${PROJECT_NAME}") - ADD_CUSTOM_TARGET("dist-${PROJECT_NAME}" - rm -rf "${DIST_DIR}" "${CMAKE_BINARY_DIR}/${OCTOVIS_PKG_NAME}" - COMMAND mkdir "${DIST_DIR}" - COMMAND svn export --force -q "${PROJECT_SOURCE_DIR}" "${DIST_DIR}/${PROJECT_NAME}" - COMMAND tar -czf "${CMAKE_BINARY_DIR}/${OCTOVIS_PKG_NAME}" -C "${DIST_DIR}" --exclude=".hidden" "${PROJECT_NAME}" - WORKING_DIRECTORY "${PROJECT_SOURCE_DIR}" - # cleanup so that there is no copy in the source dir - COMMAND rm -rf "${DIST_DIR}" - ) - ENDIF () # #installation: # # store all header files to install: file(GLOB octovis_HDRS *.h *.hxx *.hpp) install(FILES ${octovis_HDRS} DESTINATION include/octovis) + + # Install catkin package.xml + install(FILES package.xml DESTINATION share/octovis) ELSE() MESSAGE ( "Unfortunately, the viewer (octovis) can not be built because some requirements are missing.") diff --git a/octovis/CMakeLists_src.txt b/octovis/CMakeLists_src.txt index 1c3896dc..02046328 100644 --- a/octovis/CMakeLists_src.txt +++ b/octovis/CMakeLists_src.txt @@ -68,6 +68,7 @@ QT4_WRAP_UI(viewer_UIS_H ${viewer_UIS}) # the UI file won't be wrapped! include_directories(${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_BINARY_DIR}) + # Library target add_library(octovis-static STATIC ${viewerlib_SRCS}) target_link_libraries(octovis-static) @@ -82,6 +83,13 @@ target_link_libraries(octovis-shared ) set_target_properties(octovis-shared PROPERTIES OUTPUT_NAME octovis) +# directly depend on the octomap library target when building the +# complete distribution, so it is recompiled as needed +if (CMAKE_PROJECT_NAME STREQUAL "octomap-distribution") + ADD_DEPENDENCIES(octovis-static octomap-static) + ADD_DEPENDENCIES(octovis-shared octomap) +endif() + # Now add these generated files to the ADD_EXECUTABLE step # If this is NOT done, then the ui_*.h files will not be generated diff --git a/octovis/README.md b/octovis/README.md new file mode 100644 index 00000000..19c7e148 --- /dev/null +++ b/octovis/README.md @@ -0,0 +1,121 @@ +Octomap - A probabilistic, flexible, and compact 3D mapping library for robotic systems +======================================================================================= + +Authors: K. M. Wurm, A. Hornung, University of Freiburg, Copyright (C) 2009-2013. +http://octomap.github.com + +Octovis is a visualization tool and library for OctoMap. +It is distributed under the GPL license (see "LICENSE.txt"). + +Octovis is based on [QGLViewer](http://www.libqglviewer.com/), distributed under the +[GPL license](src/extern/QGLViewer/LICENSE) with an [exception by the author] +(octovis/src/extern/QGLViewer/GPL_EXCEPTION). + + +LINUX +----- + +You can build octovis independently of octomap by following +the these steps: + + cd octovis + mkdir build + cd build + cmake .. + make + + +You can manually set the location of the octomap library with the +`octomap_DIR` variable in CMake. + +Note: If you get an error such as + +> CMake Error at /usr/share/cmake-2.8/Modules/FindQt4.cmake:1148 (MESSAGE): +> Qt qmake not found! + +but you have Qt4 installed, this probably means that both Qt3 and Qt4 +are installed. In Ubuntu this can be resolved by executing: + + sudo update-alternatives --config qmake + +and choosing Qt4. + + +WINDOWS +------- + +The octomap viewer **octovis** can be compiled and used under +Windows although this has not been tested in-depth. Feedback +is welcome ("it works" is nice too :-)) + +To compile the library you need: + +* OpenGL +* cmake (http://www.cmake.org) +* QT development environment (see below) + + +### MinGW ### + +1. Download the MinGW distribution (http://www.mingw.org) +2. Install C++ compiler and add MingGW/bin to your system PATH +3. Download the QT library with MinGW support + (http://qt.nokia.com/downloads) +4. First build the GQLViewer library. Open a windows shell + (e.g., from the START-Menu -> QT) and execute: + + cd octovis/src/extern/QGLViewer + qmake + mingw32-make + This will generate QGLViewer2.dll and libQGLViewer2.a + +5. The viewer should be built along with the rest of the octomap package. + From a shell execute: + + cd octomap/build + cmake -G "MinGW Makefiles" .. + mingw32-make + + +### Microsoft Visual Studio 2010 ### + +1. Download the QT library with Visual Studio 20xx support (currently 2008) + (http://qt.nokia.com/downloads) +2. To build the qglviewer library + - open a windows shell (e.g., from the START-Menu -> QT) + + cd octovis/src/extern/QGLViewer + qmake -t vclib QGLViewer.pro -spec win32-msvc2010 (ignore any warnings) + - Load the generated file QGLViewer.vcxproj and build the project. + This will give you the needed files QGLViewer2.(dll,lib). + +3. The viewer should be built along with the rest of the octomap package. + These steps will create a solution file for the library and the viewer: + - Start the cmake-gui and set the code directory to the octomap main directory. + - Set the build directory to, e.g., `/build` + - Press "Generate", select the appropriate generator, e. g. "Visual Studio 10". + This generates a solution file octomap-distribution.sln + - Load this file and build the project. + +Some more hints on compiling with Visual Studio (these may be necessary depending +on the VS version and CMake version): +* When compiling QGLViewer, modify the output path in "Properties->Linker-> + General->Output". Remove the "debug" and "release" prefix so the libs are + installed in the base dir. +* For the octivis-shared target, add the Qt lib path ("C:\path\to\Qt\4.7.2\lib") + to "Properties->Linker->General->Additional Library Directories", and add + the following Qt libs as dependencies in "Properties->Linker->Input-> + Additional Dependencies": QtCore4.lib, QtOpenGL4.lib, QtGui4.lib and + QtXml4.lib (and the debug versions of them to the Debug configuration) +* If the debug version of octovis throws this error: "QWidget: Must construct a + QApplication before a QPaintDevice", it is linking to the release version of + QGLViewer. Change the library dependency of octovis and octovis-shared to the + debug version QGLViewerd2.lib in "Properties->Linker->Input->Additional + Dependencies". + + +When executing octovis.exe, Windows needs to find the following +libraries, so make sure they are on the PATH or in the same +directory: QGLViewer2.dll, QtOpenGL4.dll, QTGui4.dll, QTCore4.dll + + diff --git a/octovis/README.txt b/octovis/README.txt deleted file mode 100644 index ca113157..00000000 --- a/octovis/README.txt +++ /dev/null @@ -1,124 +0,0 @@ -Octomap -- A probabilistic, flexible, and compact 3D mapping library for robotic systems. - -Authors: K. M. Wurm, A. Hornung, University of Freiburg, Copyright (C) 2009-2013. -http://octomap.github.com - -Octovis is a visualization tool and library for OctoMap. -It is distributed under the GPL license (see "LICENSE.txt"). - -Octovis is based on QGLViewer,distributed under the GPL license (see -"octovis/src/extern/QGLViewer/LICENSE" and the author's exception -"octovis/src/extern/QGLViewer/GPL_EXCEPTION") - - -LINUX -############################ - -You can build octovis independently of octomap by following -the these steps: - - cd octovis - mkdir build - cd build - cmake .. - make - - -You can manually set the location of the octomap library with the -octomap_DIR variable in CMake. - -Note: If you get an error such as - -"CMake Error at /usr/share/cmake-2.8/Modules/FindQt4.cmake:1148 (MESSAGE): - Qt qmake not found!" - -but you have Qt4 installed, this probably means that both Qt3 and Qt4 -are installed. In Ubuntu this can be resolved using: -$ sudo update-alternatives --config qmake" - - -WINDOWS -############################ - -The octomap viewer "octovis" can be compiled and used under -Windows although this has not been tested in-depth. Feedback -is welcome ("it works" is nice too :-) - -To compile the library you need: - - - OpenGL - - cmake (http://www.cmake.org) - - QT development environment (see below) - - -MinGW ------------------------------- - -* Download the MinGW distribution (http://www.mingw.org) -* Install C++ compiler and add MingGW/bin to your system PATH - -* Download the QT library with MinGW support - (http://qt.nokia.com/downloads) - -* First build the GQLViewer library - - Open a windows shell (e.g., from the START-Menu -> QT) - - cd octovis/src/extern/QGLViewer - - qmake - (I had to fix a small bug in the Makefile) - - mingw32-make - - This will generate QGLViewer2.dll and libQGLViewer2.a - -The viewer should be build along with the rest of the octomap package: - -* from a shell execute: - - cd octomap/build - - cmake -G "MinGW Makefiles" .. - - mingw32-make - - -Microsoft Visual Studio 2010 ------------------------------- - -* Download the QT library with Visual Studio 20xx support (currently 2008) - (http://qt.nokia.com/downloads) - -* To build the qglviewer library - - open a windows shell (e.g., from the START-Menu -> QT) - - cd octovis/src/extern/QGLViewer - - qmake -t vclib QGLViewer.pro -spec win32-msvc2010 - (ignore any warnings) - - Load the generated file QGLViewer.vcxproj and build the project. - This will give you the files QGLViewer2.(dll,lib) that we will need. - -The viewer should be build along with the rest of the octomap package. -These steps will create a solution file for the library and the viewer: - -* Start the cmake-gui and set the code directory to the octomap main directory. -* Set the build directory to, e.g., /build -* Press "Generate", select the appropriate generator, e. g. "Visual Studio 10". -* This generates a solution file octomap-distribution.sln - Load this file and build the project. - -Some more hints on compiling with Visual Studio (these may be necessary depending -on the VS version and CMake version): -* When compiling QGLViewer, modify the output path in "Properties->Linker-> - General->Output". Remove the "debug" and "release" prefix so the libs are - installed in the base dir. -* For the octivis-shared target, add the Qt lib path ("C:\path\to\Qt\4.7.2\lib") - to "Properties->Linker->General->Additional Library Directories", and add - the following Qt libs as dependencies in "Properties->Linker->Input-> - Additional Dependencies": QtCore4.lib, QtOpenGL4.lib, QtGui4.lib and - QtXml4.lib (and the debug versions of them to the Debug configuration) -* If the debug version of octovis throws this error: "QWidget: Must construct a - QApplication before a QPaintDevice", it is linking to the release version of - QGLViewer. Change the library dependency of octovis and octovis-shared to the - debug version QGLViewerd2.lib in "Properties->Linker->Input->Additional - Dependencies". - - -When executing octovis.exe, Windows needs to find the following -libraries, so make sure they are on the PATH or in the same -directory: QGLViewer2.dll, QtOpenGL4.dll, QTGui4.dll, QTCore4.dll - - diff --git a/octovis/include/octovis/ViewerGui.h b/octovis/include/octovis/ViewerGui.h index eac4fed6..364b2ec1 100644 --- a/octovis/include/octovis/ViewerGui.h +++ b/octovis/include/octovis/ViewerGui.h @@ -67,9 +67,6 @@ namespace octomap { void addNextScans(unsigned scans); void gotoFirstScan(); - /* void handleOctomapBinaryMsg(const octomap2::OctomapBinary::ConstPtr& msg); */ - /* void handleMoveMapMsg(const octomap2::MoveMap::ConstPtr& msg); */ - bool isShown(); private slots: @@ -84,6 +81,8 @@ namespace octomap { void on_actionExport_sequence_triggered(bool checked); void on_actionClear_selection_triggered(); void on_actionFill_selection_triggered(); + void on_actionClear_unknown_in_selection_triggered(); + void on_actionFill_unknown_in_selection_triggered(); void on_actionClear_nodes_in_selection_triggered(); void on_actionFill_nodes_in_selection_triggered(); void on_actionDelete_nodes_in_selection_triggered(); @@ -189,10 +188,9 @@ namespace octomap { void saveCameraPosition(const char* filename) const; void loadCameraPosition(const char* filename); - void updateNodesInBBX(const point3d& min, const point3d& max, float logodds); void updateNodesInBBX(const point3d& min, const point3d& max, bool occupied); - void setNodesInBBX(const point3d& min, const point3d& max, float logodds); void setNodesInBBX(const point3d& min, const point3d& max, bool occupied); + void setNonNodesInBBX(const point3d& min, const point3d& max, bool occupied); std::map m_octrees; diff --git a/octovis/include/octovis/ViewerGui.ui b/octovis/include/octovis/ViewerGui.ui index ad9f83c0..8016b742 100644 --- a/octovis/include/octovis/ViewerGui.ui +++ b/octovis/include/octovis/ViewerGui.ui @@ -88,17 +88,47 @@ Edit + + + false + + + Delete nodes + + + + + + + false + + + Fill selection + + + + + + + + + + false + + + Change nodes in selection + + + + - - - - - - + + + @@ -449,50 +479,66 @@ - false + true - Fill selection with "free" + With "free" (all) - false + true - Fill selection with "occupied" + With "occupied" (all) - false + true - Set nodes in selection to "free" + To "free" - false + true - Set nodes in selection to "occupied" + To "occupied" - false + true - Delete nodes in selection + In selection - false + true + + + Outside of selection (crop) + + + + + true + + + With "free" (only unknown) + + + + + true - Delete nodes outside of selection (crop) + With "occupied" (only unknown) diff --git a/octovis/octovis-config.cmake.in b/octovis/octovis-config.cmake.in index 0707e2dc..2a78844f 100644 --- a/octovis/octovis-config.cmake.in +++ b/octovis/octovis-config.cmake.in @@ -7,5 +7,11 @@ set(OCTOVIS_INCLUDE_DIRS @QGLViewer_INCLUDE_DIR@ @OCTOVIS_INCLUDE_DIR@) set(OCTOVIS_LIBRARY_DIRS @QGLViewer_LIBRARY_DIR@ @OCTOVIS_LIB_DIR@) - -set(OCTOVIS_LIBRARIES @QGLViewer_LIBRARIES@ @QT_LIBRARIES@ octovis) + + +# Set library names as absolute paths: +set(OCTOVIS_LIBRARIES + @QGLViewer_LIBRARIES@ + @QT_LIBRARIES@ + @OCTOVIS_LIB_DIR@/liboctovis.so +) diff --git a/octovis/package.xml b/octovis/package.xml new file mode 100644 index 00000000..fb549abb --- /dev/null +++ b/octovis/package.xml @@ -0,0 +1,33 @@ + + octovis + 1.6.0 + octovis is visualization tool for the OctoMap library based on Qt and libQGLViewer. See + http://octomap.github.com for details. + + Kai M. Wurm + Armin Hornung + Armin Hornung + GPLv2 + + http://octomap.github.com + https://github.com/OctoMap/octomap/issues + + + catkin + cmake + + + cmake + + + octomap + libqglviewer-qt4-dev + libqt4-dev + libqt4-opengl-dev + + octomap + libqglviewer-qt4 + libqtgui4 + libqt4-opengl + + diff --git a/octovis/src/ViewerGui.cpp b/octovis/src/ViewerGui.cpp index 0fea3165..8bb9ddf5 100644 --- a/octovis/src/ViewerGui.cpp +++ b/octovis/src/ViewerGui.cpp @@ -314,7 +314,7 @@ namespace octomap{ unsigned numScans = m_scanGraph->size(); unsigned currentScan = 1; for (it = m_scanGraph->begin(); it != m_nextScanToAdd; it++) { - tree->insertScan(**it, m_laserMaxRange); + tree->insertPointCloud(**it, m_laserMaxRange); fprintf(stderr, "generateOctree:: inserting scan node with %d points, origin: %.2f ,%.2f , %.2f.\n", (unsigned int) (*it)->scan->size(), (*it)->pose.x(), (*it)->pose.y(), (*it)->pose.z() ); @@ -334,92 +334,6 @@ namespace octomap{ } - - // void ViewerGui::handleOctomapBinaryMsg(const octomap2::OctomapBinary::ConstPtr& msg) { - - // printf("received octomap, tree id=%d, origin: <%.2f , %.2f , %.2f>, <%.2f , %.2f , %.2f , %.2f>\n", - // msg->id, msg->origin.position.x, msg->origin.position.y, msg->origin.position.z, - // msg->origin.orientation.x, msg->origin.orientation.y, msg->origin.orientation.z, - // msg->origin.orientation.w - // ); - - // OcTree* tree = new octomap::OcTree(0.05); - // octomap::octomapMsgToMap(*msg, *tree); - - // octomath::Vector3 trans (msg->origin.position.x, msg->origin.position.y, msg->origin.position.z); - // octomath::Quaternion rot (msg->origin.orientation.w, msg->origin.orientation.x, msg->origin.orientation.y, msg->origin.orientation.z); - // std::cout << rot << std::endl; - // octomap::pose6d origin (trans, rot); - - // this->addOctree(tree, msg->id, origin); - - // m_octreeResolution = tree->getResolution(); - // emit changeResolution(m_octreeResolution); - - // ui.actionPointcloud->setChecked(false); - // ui.actionPointcloud->setEnabled(false); - // ui.actionOctree_cells->setChecked(true); - // ui.actionOctree_cells->setEnabled(true); - // ui.actionFree->setChecked(false); - // ui.actionFree->setEnabled(true); - // ui.actionOctree_structure->setEnabled(true); - // ui.actionOctree_structure->setChecked(false); - // ui.actionTrajectory->setEnabled(false); - // ui.actionConvert_ml_tree->setEnabled(false); - // ui.actionReload_Octree->setEnabled(false); - // ui.actionSettings->setEnabled(false); - - // showOcTree(); - // // m_glwidget->resetView(); - // } - - - // void ViewerGui::handleMoveMapMsg(const octomap2::MoveMap::ConstPtr& msg) { - - // octomath::Vector3 trans (msg->transform.position.x, msg->transform.position.y, msg->transform.position.z); - // octomath::Quaternion rot (msg->transform.orientation.w, msg->transform.orientation.x, msg->transform.orientation.y, msg->transform.orientation.z); - // octomap::pose6d transform (trans, rot); - // std::cout << transform << std::endl; - - // OcTreeRecord* r; - // if (getOctreeRecord(msg->id, r)) { - - // if (msg->absolute) { // absolute movement -> set origin - - // r->origin = transform; - // r->octree_drawer->setOrigin(transform); - - // fprintf(stderr, "received new origin for map id=%d: <%.2f , %.2f , %.2f>, <%.2f , %.2f , %.2f , %.2f>\n", - // msg->id, msg->transform.position.x, msg->transform.position.y, msg->transform.position.z, - // msg->transform.orientation.x, msg->transform.orientation.y, msg->transform.orientation.z, - // msg->transform.orientation.w - // ); - // } - - // else { // relative movement -> transform pose - - // octomap::pose6d T = r->origin; - // octomath::Quaternion rot_new = T.rot() * transform.rot(); - // octomap::point3d trans_new = T.trans() + transform.trans(); - - // r->origin = octomap::pose6d(trans_new, rot_new); - // r->octree_drawer->setOrigin(r->origin); - - // fprintf(stderr, "received relative transform for map id=%d: <%.2f , %.2f , %.2f>, <%.2f , %.2f , %.2f , %.2f>\n", - // msg->id, msg->transform.position.x, msg->transform.position.y, msg->transform.position.z, - // msg->transform.orientation.x, msg->transform.orientation.y, msg->transform.orientation.z, - // msg->transform.orientation.w - // ); - // } - // m_glwidget->updateGL(); - // } - // else { - // fprintf(stderr, "ERROR: map %d does not exist\n", msg->id); - // } - // } - - - // == incremental graph generation ======================= void ViewerGui::gotoFirstScan(){ @@ -454,7 +368,7 @@ namespace octomap{ return; } // not used with ColorOcTrees, omitting casts - ((OcTree*) r->octree)->insertScan(**m_nextScanToAdd, m_laserMaxRange); + ((OcTree*) r->octree)->insertPointCloud(**m_nextScanToAdd, m_laserMaxRange); m_nextScanToAdd++; } @@ -472,7 +386,6 @@ namespace octomap{ } - // == file I/O =========================================== void ViewerGui::openFile(){ @@ -807,7 +720,6 @@ namespace octomap{ AbstractOcTree* t = r->octree; - // TODO: get rid of casts (requires occupancy tree interface) if (fileinfo.suffix() == "bt") { AbstractOccupancyOcTree* ot = dynamic_cast (t); if (ot) @@ -848,6 +760,24 @@ namespace octomap{ setNodesInBBX(min, max, false); } + void ViewerGui::on_actionClear_unknown_in_selection_triggered() + { + point3d min, max; + m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z()); + m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z()); + + setNonNodesInBBX(min, max, false); + } + + void ViewerGui::on_actionFill_unknown_in_selection_triggered() + { + point3d min, max; + m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z()); + m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z()); + + setNonNodesInBBX(min, max, true); + } + void ViewerGui::on_actionFill_selection_triggered(){ point3d min, max; m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z()); @@ -920,56 +850,46 @@ namespace octomap{ showOcTree(); } + void ViewerGui::updateNodesInBBX(const point3d& min, const point3d& max, bool occupied){ + for (std::map::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) { + OcTree* octree = dynamic_cast(t_it->second.octree); + if (octree){ + float logodds; + if (occupied) + logodds = octree->getClampingThresMaxLog(); + else + logodds = octree->getClampingThresMinLog(); - void ViewerGui::updateNodesInBBX(const point3d& min, const point3d& max, float logodds){ - for (std::map::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) { - OcTree* octree = dynamic_cast(t_it->second.octree); - - if (octree){ - - for(OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(min,max), - end=octree->end_leafs_bbx(); it!= end; ++it){ - octree->updateNode(it.getKey(), logodds); - } - } else - QMessageBox::warning(this, "Not implemented", "Functionality not yet implemented for this octree type", - QMessageBox::Ok); - - } - - showOcTree(); - } + for(OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(min,max), + end=octree->end_leafs_bbx(); it!= end; ++it) + { + // directly set values of leafs: + it->setLogOdds(logodds); + } + // update inner nodes to make tree consistent: + octree->updateInnerOccupancy(); - void ViewerGui::updateNodesInBBX(const point3d& min, const point3d& max, bool occupied){ - for (std::map::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) { - OcTree* octree = dynamic_cast(t_it->second.octree); - - if (octree){ - float logodds = octree->getClampingThresMaxLog() - octree->getClampingThresMinLog(); - if (!occupied) - logodds *= -1; - - for(OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(min,max), - end=octree->end_leafs_bbx(); it!= end; ++it){ - octree->updateNode(it.getKey(), logodds); - } - } else + } else QMessageBox::warning(this, "Not implemented", "Functionality not yet implemented for this octree type", - QMessageBox::Ok); + QMessageBox::Ok); - } + } - showOcTree(); + showOcTree(); } - void ViewerGui::setNodesInBBX(const point3d& min, const point3d& max, float logodds){ + void ViewerGui::setNodesInBBX(const point3d& min, const point3d& max, bool occupied){ for (std::map::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) { OcTree* octree = dynamic_cast(t_it->second.octree); if (octree){ + float logodds = octree->getClampingThresMaxLog() - octree->getClampingThresMinLog(); + if (!occupied) + logodds *= -1; + OcTreeKey minKey(0,0,0); OcTreeKey maxKey(0,0,0); octree->coordToKeyChecked(min, minKey); @@ -989,7 +909,7 @@ namespace octomap{ showOcTree(); } - void ViewerGui::setNodesInBBX(const point3d& min, const point3d& max, bool occupied){ + void ViewerGui::setNonNodesInBBX(const point3d& min, const point3d& max, bool occupied) { for (std::map::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) { OcTree* octree = dynamic_cast(t_it->second.octree); @@ -1006,7 +926,9 @@ namespace octomap{ for (k[0] = minKey[0]; k[0] < maxKey[0]; ++k[0]){ for (k[1] = minKey[1]; k[1] < maxKey[1]; ++k[1]){ for (k[2] = minKey[2]; k[2] < maxKey[2]; ++k[2]){ - octree->updateNode(k, logodds); + OcTreeNode* n = octree->search(k); + if(!n) + octree->updateNode(k, logodds); } } } @@ -1046,12 +968,11 @@ namespace octomap{ } void ViewerGui::on_actionSelection_box_toggled(bool checked){ - ui.actionClear_selection->setEnabled(checked); - ui.actionFill_selection->setEnabled(checked); - ui.actionClear_nodes_in_selection->setEnabled(checked); - ui.actionFill_nodes_in_selection->setEnabled(checked); - ui.actionDelete_nodes_in_selection->setEnabled(checked); - ui.actionDelete_nodes_outside_of_selection->setEnabled(checked); + + ui.menuDelete_nodes->setEnabled(checked); + ui.menuFill_selection->setEnabled(checked); + ui.menuChange_nodes_in_selection->setEnabled(checked); + m_glwidget->enableSelectionBox(checked); diff --git a/octovis/src/ViewerWidget.cpp b/octovis/src/ViewerWidget.cpp index 16372663..3e7403eb 100644 --- a/octovis/src/ViewerWidget.cpp +++ b/octovis/src/ViewerWidget.cpp @@ -84,7 +84,7 @@ namespace octomap { "It provides data structures and mapping algorithms. The map is implemented " "using an octree. 3D maps can be viewed an built using this 3D viewer." "

" - "Octomap is available at http://octomap.sourceforge.net/, and is actively " + "Octomap is available at http://octomap.github.com, and is actively " "maintained by Kai M. Wurm and Armin Hornung. This 3D viewer is based on " "libQGLViewer, available at http://www.libqglviewer.com/." "

"