diff --git a/CMakeLists.txt b/CMakeLists.txt index d482d56b5..86694de58 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,9 +3,14 @@ project(slam_toolbox) add_compile_options(-std=c++11) set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/CMake/") +list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/lib/KartoSDK-slam_toolbox/cmake) set(CMAKE_CXX_FLAGS "-fpermissive -std=c++11") find_package(Boost REQUIRED system serialization filesystem) +#open_karto lib +set(BUILD_SHARED_LIBS ON) +add_subdirectory(lib/KartoSDK-slam_toolbox) + find_package(catkin REQUIRED COMPONENTS cmake_modules @@ -139,7 +144,6 @@ set(SOURCE_FILES mapping_plugin/slam_toolbox_rviz_plugin.cpp ) add_library(SlamToolboxPlugin ${SOURCE_FILES}) add_dependencies(SlamToolboxPlugin ${PROJECT_NAME}_generate_messages_cpp) target_link_libraries(SlamToolboxPlugin ${rviz_DEFAULT_PLUGIN_LIBRARIES} ${QT_LIBRARIES} ${catkin_LIBRARIES} ) - #### Main tool for mapping add_executable(slam_toolbox src/slam_toolbox.cpp) add_dependencies(slam_toolbox ${PROJECT_NAME}_generate_messages_cpp) diff --git a/lib/KartoSDK-slam_toolbox/Authors b/lib/KartoSDK-slam_toolbox/Authors new file mode 100644 index 000000000..b1c0e8655 --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/Authors @@ -0,0 +1,6 @@ +Karto Open Source Library 1.0 + +Contributors: +------------------------------- +Michael A. Eriksen (eriksen@ai.sri.com) +Benson Limketkai (bensonl@ai.sri.com) diff --git a/lib/KartoSDK-slam_toolbox/CHANGELOG.rst b/lib/KartoSDK-slam_toolbox/CHANGELOG.rst new file mode 100644 index 000000000..4ef816f2f --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/CHANGELOG.rst @@ -0,0 +1,37 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package open_karto +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.1.4 (2016-03-02) +------------------ +* update build status badges +* Adds LocalizedRangeScanWithPoints range scan +* Contributors: Michael Ferguson, Russell Toris + +1.1.3 (2016-02-16) +------------------ +* Link against, and export depend on, boost +* Contributors: Hai Nguyen, Michael Ferguson + +1.1.2 (2015-07-13) +------------------ +* Added getters and setters for parameters inside Mapper so they can be changed via the ROS param server. +* Contributors: Luc Bettaieb, Michael Ferguson + +1.1.1 (2015-05-07) +------------------ +* Makes FindValidPoints robust to the first point in the scan being a NaN +* Bump minimum cmake version requirement +* Fix cppcheck warnings +* Fix newlines (dos2unix) & superfluous whitespace +* Protect functions that throw away const-ness (check dirty flag) with mutex +* Don't modify scan during loop closure check - work on a copy of it +* removed useless return to avoid cppcheck error +* Add Mapper::SetUseScanMatching +* Remove html entities from log output +* Fix NANs cause raytracing to take forever +* Contributors: Daniel Pinyol, Michael Ferguson, Paul Mathieu, Siegfried-A. Gevatter Pujals, liz-murphy + +1.1.0 (2014-06-15) +------------------ +* Release as a pure catkin ROS package diff --git a/lib/KartoSDK-slam_toolbox/CMakeLists.txt b/lib/KartoSDK-slam_toolbox/CMakeLists.txt new file mode 100644 index 000000000..42da907d8 --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 2.8.3) +project(open_karto) + +set(CMAKE_CXX_FLAGS "-ftemplate-backtrace-limit=0") +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/") + +find_package(catkin REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(Boost REQUIRED system serialization filesystem thread) +find_package(TBB REQUIRED) +add_compile_options(-std=c++11) + +catkin_package( + DEPENDS + Boost + TBB + INCLUDE_DIRS + include + ${TBB_INCLUDE_DIRS} + LIBRARIES + karto +) + +add_definitions(${EIGEN3_DEFINITIONS}) + +include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${TBB_INCLUDE_DIRS} ${Boost_INCLUDE_DIR}) +add_library(karto SHARED src/Karto.cpp src/Mapper.cpp) +target_link_libraries(karto ${Boost_LIBRARIES} ${TBB_LIBRARIES}) + +install(DIRECTORY include/ DESTINATION include) +install(TARGETS karto + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) diff --git a/lib/KartoSDK-slam_toolbox/LICENSE b/lib/KartoSDK-slam_toolbox/LICENSE new file mode 100644 index 000000000..65c5ca88a --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/LICENSE @@ -0,0 +1,165 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + + This version of the GNU Lesser General Public License incorporates +the terms and conditions of version 3 of the GNU General Public +License, supplemented by the additional permissions listed below. + + 0. Additional Definitions. + + As used herein, "this License" refers to version 3 of the GNU Lesser +General Public License, and the "GNU GPL" refers to version 3 of the GNU +General Public License. + + "The Library" refers to a covered work governed by this License, +other than an Application or a Combined Work as defined below. + + An "Application" is any work that makes use of an interface provided +by the Library, but which is not otherwise based on the Library. +Defining a subclass of a class defined by the Library is deemed a mode +of using an interface provided by the Library. + + A "Combined Work" is a work produced by combining or linking an +Application with the Library. The particular version of the Library +with which the Combined Work was made is also called the "Linked +Version". + + The "Minimal Corresponding Source" for a Combined Work means the +Corresponding Source for the Combined Work, excluding any source code +for portions of the Combined Work that, considered in isolation, are +based on the Application, and not on the Linked Version. + + The "Corresponding Application Code" for a Combined Work means the +object code and/or source code for the Application, including any data +and utility programs needed for reproducing the Combined Work from the +Application, but excluding the System Libraries of the Combined Work. + + 1. Exception to Section 3 of the GNU GPL. + + You may convey a covered work under sections 3 and 4 of this License +without being bound by section 3 of the GNU GPL. + + 2. Conveying Modified Versions. + + If you modify a copy of the Library, and, in your modifications, a +facility refers to a function or data to be supplied by an Application +that uses the facility (other than as an argument passed when the +facility is invoked), then you may convey a copy of the modified +version: + + a) under this License, provided that you make a good faith effort to + ensure that, in the event an Application does not supply the + function or data, the facility still operates, and performs + whatever part of its purpose remains meaningful, or + + b) under the GNU GPL, with none of the additional permissions of + this License applicable to that copy. + + 3. Object Code Incorporating Material from Library Header Files. + + The object code form of an Application may incorporate material from +a header file that is part of the Library. You may convey such object +code under terms of your choice, provided that, if the incorporated +material is not limited to numerical parameters, data structure +layouts and accessors, or small macros, inline functions and templates +(ten or fewer lines in length), you do both of the following: + + a) Give prominent notice with each copy of the object code that the + Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the object code with a copy of the GNU GPL and this license + document. + + 4. Combined Works. + + You may convey a Combined Work under terms of your choice that, +taken together, effectively do not restrict modification of the +portions of the Library contained in the Combined Work and reverse +engineering for debugging such modifications, if you also do each of +the following: + + a) Give prominent notice with each copy of the Combined Work that + the Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the Combined Work with a copy of the GNU GPL and this license + document. + + c) For a Combined Work that displays copyright notices during + execution, include the copyright notice for the Library among + these notices, as well as a reference directing the user to the + copies of the GNU GPL and this license document. + + d) Do one of the following: + + 0) Convey the Minimal Corresponding Source under the terms of this + License, and the Corresponding Application Code in a form + suitable for, and under terms that permit, the user to + recombine or relink the Application with a modified version of + the Linked Version to produce a modified Combined Work, in the + manner specified by section 6 of the GNU GPL for conveying + Corresponding Source. + + 1) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (a) uses at run time + a copy of the Library already present on the user's computer + system, and (b) will operate properly with a modified version + of the Library that is interface-compatible with the Linked + Version. + + e) Provide Installation Information, but only if you would otherwise + be required to provide such information under section 6 of the + GNU GPL, and only to the extent that such information is + necessary to install and execute a modified version of the + Combined Work produced by recombining or relinking the + Application with a modified version of the Linked Version. (If + you use option 4d0, the Installation Information must accompany + the Minimal Corresponding Source and Corresponding Application + Code. If you use option 4d1, you must provide the Installation + Information in the manner specified by section 6 of the GNU GPL + for conveying Corresponding Source.) + + 5. Combined Libraries. + + You may place library facilities that are a work based on the +Library side by side in a single library together with other library +facilities that are not Applications and are not covered by this +License, and convey such a combined library under terms of your +choice, if you do both of the following: + + a) Accompany the combined library with a copy of the same work based + on the Library, uncombined with any other library facilities, + conveyed under the terms of this License. + + b) Give prominent notice with the combined library that part of it + is a work based on the Library, and explaining where to find the + accompanying uncombined form of the same work. + + 6. Revised Versions of the GNU Lesser General Public License. + + The Free Software Foundation may publish revised and/or new versions +of the GNU Lesser General Public License from time to time. Such new +versions will be similar in spirit to the present version, but may +differ in detail to address new problems or concerns. + + Each version is given a distinguishing version number. If the +Library as you received it specifies that a certain numbered version +of the GNU Lesser General Public License "or any later version" +applies to it, you have the option of following the terms and +conditions either of that published version or of any later version +published by the Free Software Foundation. If the Library as you +received it does not specify a version number of the GNU Lesser +General Public License, you may choose any version of the GNU Lesser +General Public License ever published by the Free Software Foundation. + + If the Library as you received it specifies that a proxy can decide +whether future versions of the GNU Lesser General Public License shall +apply, that proxy's public statement of acceptance of any version is +permanent authorization for you to choose that version for the +Library. diff --git a/lib/KartoSDK-slam_toolbox/README.md b/lib/KartoSDK-slam_toolbox/README.md new file mode 100644 index 000000000..23844f904 --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/README.md @@ -0,0 +1,8 @@ +# Open Karto + +Catkinized ROS Package of the OpenKarto Library (LGPL3) + +# Status + + * Devel Job Status: [![Build Status](http://build.ros.org/buildStatus/icon?job=Idev__open_karto__ubuntu_trusty_amd64)](http://build.ros.org/job/Idev__open_karto__ubuntu_trusty_amd64/) + * AMD64 Debian Job Status: [![Build Status](http://build.ros.org/buildStatus/icon?job=Jbin_uT64__open_karto__ubuntu_trusty_amd64__binary)](http://build.ros.org/job/Jbin_uT64__open_karto__ubuntu_trusty_amd64__binary) diff --git a/lib/KartoSDK-slam_toolbox/cmake/FindTBB.cmake b/lib/KartoSDK-slam_toolbox/cmake/FindTBB.cmake new file mode 100644 index 000000000..2b41805b4 --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/cmake/FindTBB.cmake @@ -0,0 +1,292 @@ +# - Find ThreadingBuildingBlocks include dirs and libraries +# Use this module by invoking find_package with the form: +# find_package(TBB +# [REQUIRED] # Fail with error if TBB is not found +# ) # +# Once done, this will define +# +# TBB_FOUND - system has TBB +# TBB_INCLUDE_DIRS - the TBB include directories +# TBB_LIBRARIES - TBB libraries to be lined, doesn't include malloc or +# malloc proxy +# +# TBB_VERSION_MAJOR - Major Product Version Number +# TBB_VERSION_MINOR - Minor Product Version Number +# TBB_INTERFACE_VERSION - Engineering Focused Version Number +# TBB_COMPATIBLE_INTERFACE_VERSION - The oldest major interface version +# still supported. This uses the engineering +# focused interface version numbers. +# +# TBB_MALLOC_FOUND - system has TBB malloc library +# TBB_MALLOC_INCLUDE_DIRS - the TBB malloc include directories +# TBB_MALLOC_LIBRARIES - The TBB malloc libraries to be lined +# +# TBB_MALLOC_PROXY_FOUND - system has TBB malloc proxy library +# TBB_MALLOC_PROXY_INCLUDE_DIRS = the TBB malloc proxy include directories +# TBB_MALLOC_PROXY_LIBRARIES - The TBB malloc proxy libraries to be lined +# +# +# This module reads hints about search locations from variables: +# ENV TBB_ARCH_PLATFORM for windows only +# ENV TBB_ROOT or just TBB_ROOT +# +# +# +# Modified by Robert Maynard from the original OGRE source +# +#------------------------------------------------------------------- +# This file is part of the CMake build system for OGRE +# (Object-oriented Graphics Rendering Engine) +# For the latest info, see http://www.ogre3d.org/ +# +# The contents of this file are placed in the public domain. Feel +# free to make use of it in any way you like. +#------------------------------------------------------------------- +# +#============================================================================= +# Copyright 2010-2012 Kitware, Inc. +# Copyright 2012 Rolf Eike Beer +# +# Distributed under the OSI-approved BSD License (the "License"); +# see accompanying file Copyright.txt for details. +# +# This software is distributed WITHOUT ANY WARRANTY; without even the +# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the License for more information. +#============================================================================= +# (To distribute this file outside of CMake, substitute the full +# License text for the above reference.) + + +#============================================================================= +# FindTBB helper functions and macros +# + +#=============================================== +# Create search paths based on prefix path +#=============================================== +macro(create_search_paths PREFIX) + foreach(dir ${${PREFIX}_PREFIX_PATH}) + set(${PREFIX}_INC_SEARCH_PATH ${${PREFIX}_INC_SEARCH_PATH} + ${dir}/include ${dir}/Include ${dir}/include/${PREFIX}) + set(${PREFIX}_LIB_SEARCH_PATH ${${PREFIX}_LIB_SEARCH_PATH} + ${dir}/lib ${dir}/Lib ${dir}/lib/${PREFIX} ${dir}/Libs) + endforeach(dir) +endmacro(create_search_paths) + +#=============================================== +# Do the final processing for the package find. +#=============================================== +macro(findpkg_finish PREFIX) + # skip if already processed during this run + if (NOT ${PREFIX}_FOUND) + if (${PREFIX}_INCLUDE_DIR AND ${PREFIX}_LIBRARY) + set(${PREFIX}_FOUND TRUE) + set (${PREFIX}_INCLUDE_DIRS ${${PREFIX}_INCLUDE_DIR}) + set (${PREFIX}_LIBRARIES ${${PREFIX}_LIBRARY}) + else () + if (${PREFIX}_FIND_REQUIRED AND NOT ${PREFIX}_FIND_QUIETLY) + message(FATAL_ERROR "Required library ${PREFIX} not found.") + endif () + endif () + + #mark the following variables as internal variables + mark_as_advanced(${PREFIX}_INCLUDE_DIR + ${PREFIX}_LIBRARY + ${PREFIX}_LIBRARY_DEBUG + ${PREFIX}_LIBRARY_RELEASE) + endif () +endmacro(findpkg_finish) + +#=============================================== +# Generate debug names from given RELEASEease names +#=============================================== +macro(get_debug_names PREFIX) + foreach(i ${${PREFIX}}) + set(${PREFIX}_DEBUG ${${PREFIX}_DEBUG} ${i}d ${i}D ${i}_d ${i}_D ${i}_debug ${i}) + endforeach(i) +endmacro(get_debug_names) + +#=============================================== +# See if we have env vars to help us find tbb +#=============================================== +macro(getenv_path VAR) + set(ENV_${VAR} $ENV{${VAR}}) + # replace won't work if var is blank + if (ENV_${VAR}) + string( REGEX REPLACE "\\\\" "/" ENV_${VAR} ${ENV_${VAR}} ) + endif () +endmacro(getenv_path) + +#=============================================== +# Couple a set of RELEASEease AND debug libraries +#=============================================== +macro(make_library_set PREFIX) + if (${PREFIX}_RELEASE AND ${PREFIX}_DEBUG) + set(${PREFIX} optimized ${${PREFIX}_RELEASE} debug ${${PREFIX}_DEBUG}) + elseif (${PREFIX}_RELEASE) + set(${PREFIX} ${${PREFIX}_RELEASE}) + elseif (${PREFIX}_DEBUG) + set(${PREFIX} ${${PREFIX}_DEBUG}) + endif () +endmacro(make_library_set) + + +#============================================================================= +# Now to actually find TBB +# + +# Get path, convert backslashes as ${ENV_${var}} +getenv_path(TBB_ROOT) +# construct search paths +set(TBB_PREFIX_PATH ${TBB_ROOT} ${ENV_TBB_ROOT}) +create_search_paths(TBB) + +# get the arch, only used by windows +if($ENV{TBB_ARCH_PLATFORM}) + set(TBB_ARCH_PLATFORM $ENV{TBB_ARCH_PLATFORM}) +endif() + +# For Windows, let's assume that the user might be using the precompiled +# TBB packages from the main website. These use a rather awkward directory +# structure (at least for automatically finding the right files) depending +# on platform and compiler, but we'll do our best to accommodate it. +# Not adding the same effort for the precompiled linux builds, though. Those +# have different versions for CC compiler versions and linux kernels which +# will never adequately match the user's setup, so there is no feasible way +# to detect the "best" version to use. The user will have to manually +# select the right files. (Chances are the distributions are shipping their +# custom version of tbb, anyway, so the problem is probably nonexistant.) +if (WIN32 AND MSVC) + set(COMPILER_PREFIX "vc7.1") + if (MSVC_VERSION EQUAL 1400) + set(COMPILER_PREFIX "vc8") + elseif(MSVC_VERSION EQUAL 1500) + set(COMPILER_PREFIX "vc9") + elseif(MSVC_VERSION EQUAL 1600) + set(COMPILER_PREFIX "vc10") + elseif(MSVC_VERSION EQUAL 1700) + set(COMPILER_PREFIX "vc11") + elseif(MSVC_VERSION EQUAL 1800) + set(COMPILER_PREFIX "vc12") + endif () + + # for each prefix path, add ia32/64\${COMPILER_PREFIX}\lib to the lib search path + foreach (dir ${TBB_PREFIX_PATH}) + if (CMAKE_CL_64) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/ia64/${COMPILER_PREFIX}/lib) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/ia64/${COMPILER_PREFIX}) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/intel64/${COMPILER_PREFIX}/lib) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/intel64/${COMPILER_PREFIX}) + else () + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/ia32/${COMPILER_PREFIX}/lib) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/ia32/${COMPILER_PREFIX}) + endif () + endforeach () +endif () + +foreach (dir ${TBB_PREFIX_PATH}) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/${TBB_ARCH_PLATFORM}/lib) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/${TBB_ARCH_PLATFORM}) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib) +endforeach () + + +set(TBB_LIBRARY_NAMES tbb) +get_debug_names(TBB_LIBRARY_NAMES) + + +find_path(TBB_INCLUDE_DIR + NAMES tbb/tbb.h + PATHS ${TBB_INC_SEARCH_PATH}) + +find_library(TBB_LIBRARY_RELEASE + NAMES ${TBB_LIBRARY_NAMES} + PATHS ${TBB_LIB_SEARCH_PATH}) +find_library(TBB_LIBRARY_DEBUG + NAMES ${TBB_LIBRARY_NAMES_DEBUG} + PATHS ${TBB_LIB_SEARCH_PATH}) +make_library_set(TBB_LIBRARY) + +findpkg_finish(TBB) + +#on unix we need to also link to rt +if(UNIX AND NOT APPLE) + list(APPEND TBB_LIBRARIES rt) +endif() + +#if we haven't found TBB no point on going any further +if (NOT TBB_FOUND) + return() +endif () + +#============================================================================= +# Look for TBB's malloc package +set(TBB_MALLOC_LIBRARY_NAMES tbbmalloc) +get_debug_names(TBB_MALLOC_LIBRARY_NAMES) + +find_path(TBB_MALLOC_INCLUDE_DIR + NAMES tbb/tbb.h + PATHS ${TBB_INC_SEARCH_PATH}) + +find_library(TBB_MALLOC_LIBRARY_RELEASE + NAMES ${TBB_MALLOC_LIBRARY_NAMES} + PATHS ${TBB_LIB_SEARCH_PATH}) +find_library(TBB_MALLOC_LIBRARY_DEBUG + NAMES ${TBB_MALLOC_LIBRARY_NAMES_DEBUG} + PATHS ${TBB_LIB_SEARCH_PATH}) +make_library_set(TBB_MALLOC_LIBRARY) + +findpkg_finish(TBB_MALLOC) + +#============================================================================= +# Look for TBB's malloc proxy package +set(TBB_MALLOC_PROXY_LIBRARY_NAMES tbbmalloc_proxy) +get_debug_names(TBB_MALLOC_PROXY_LIBRARY_NAMES) + +find_path(TBB_MALLOC_PROXY_INCLUDE_DIR + NAMES tbb/tbbmalloc_proxy.h + PATHS ${TBB_INC_SEARCH_PATH}) + +find_library(TBB_MALLOC_PROXY_LIBRARY_RELEASE + NAMES ${TBB_MALLOC_PROXY_LIBRARY_NAMES} + PATHS ${TBB_LIB_SEARCH_PATH}) +find_library(TBB_MALLOC_PROXY_LIBRARY_DEBUG + NAMES ${TBB_MALLOC_PROXY_LIBRARY_NAMES_DEBUG} + PATHS ${TBB_LIB_SEARCH_PATH}) +make_library_set(TBB_MALLOC_PROXY_LIBRARY) + +findpkg_finish(TBB_MALLOC_PROXY) + +#----------------------------------------------------------------------------- +# setup timing libs we need to link too + + +#============================================================================= +#parse all the version numbers from tbb +if(NOT TBB_VERSION) + + #only read the start of the file + file(READ + "${TBB_INCLUDE_DIR}/tbb/tbb_stddef.h" + TBB_VERSION_CONTENTS + LIMIT 2048) + + string(REGEX REPLACE + ".*#define TBB_VERSION_MAJOR ([0-9]+).*" "\\1" + TBB_VERSION_MAJOR "${TBB_VERSION_CONTENTS}") + + string(REGEX REPLACE + ".*#define TBB_VERSION_MINOR ([0-9]+).*" "\\1" + TBB_VERSION_MINOR "${TBB_VERSION_CONTENTS}") + + string(REGEX REPLACE + ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1" + TBB_INTERFACE_VERSION "${TBB_VERSION_CONTENTS}") + + string(REGEX REPLACE + ".*#define TBB_COMPATIBLE_INTERFACE_VERSION ([0-9]+).*" "\\1" + TBB_COMPATIBLE_INTERFACE_VERSION "${TBB_VERSION_CONTENTS}") + +endif() + diff --git a/lib/KartoSDK-slam_toolbox/include/open_karto/Karto.h b/lib/KartoSDK-slam_toolbox/include/open_karto/Karto.h new file mode 100644 index 000000000..3e1fd64a8 --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/include/open_karto/Karto.h @@ -0,0 +1,7061 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#ifndef OPEN_KARTO_KARTO_H +#define OPEN_KARTO_KARTO_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef USE_POCO +#include +#endif + +#include +#include + +#define KARTO_Object(name) \ + virtual const char* GetClassName() const { return #name; } \ + virtual kt_objecttype GetObjectType() const { return ObjectType_##name; } + +typedef kt_int32u kt_objecttype; + +const kt_objecttype ObjectType_None = 0x00000000; +const kt_objecttype ObjectType_Sensor = 0x00001000; +const kt_objecttype ObjectType_SensorData = 0x00002000; +const kt_objecttype ObjectType_CustomData = 0x00004000; +const kt_objecttype ObjectType_Misc = 0x10000000; + +const kt_objecttype ObjectType_Drive = ObjectType_Sensor | 0x01; +const kt_objecttype ObjectType_LaserRangeFinder = ObjectType_Sensor | 0x02; +const kt_objecttype ObjectType_Camera = ObjectType_Sensor | 0x04; + +const kt_objecttype ObjectType_DrivePose = ObjectType_SensorData | 0x01; +const kt_objecttype ObjectType_LaserRangeScan = ObjectType_SensorData | 0x02; +const kt_objecttype ObjectType_LocalizedRangeScan = ObjectType_SensorData | 0x04; +const kt_objecttype ObjectType_CameraImage = ObjectType_SensorData | 0x08; +const kt_objecttype ObjectType_LocalizedRangeScanWithPoints = ObjectType_SensorData | 0x16; + +const kt_objecttype ObjectType_Header = ObjectType_Misc | 0x01; +const kt_objecttype ObjectType_Parameters = ObjectType_Misc | 0x02; +const kt_objecttype ObjectType_DatasetInfo = ObjectType_Misc | 0x04; +const kt_objecttype ObjectType_Module = ObjectType_Misc | 0x08; + +namespace karto +{ + + /** + * \defgroup OpenKarto OpenKarto Module + */ + /*@{*/ + + /** + * Exception class. All exceptions thrown from Karto will inherit from this class or be of this class + */ + class KARTO_EXPORT Exception + { + public: + /** + * Constructor with exception message + * @param rMessage exception message (default: "Karto Exception") + * @param errorCode error code (default: 0) + */ + Exception(const std::string& rMessage = "Karto Exception", kt_int32s errorCode = 0) + : m_Message(rMessage) + , m_ErrorCode(errorCode) + { + } + + /** + * Copy constructor + */ + Exception(const Exception& rException) + : m_Message(rException.m_Message) + , m_ErrorCode(rException.m_ErrorCode) + { + } + + /** + * Destructor + */ + virtual ~Exception() + { + } + + public: + /** + * Assignment operator + */ + Exception& operator = (const Exception& rException) + { + m_Message = rException.m_Message; + m_ErrorCode = rException.m_ErrorCode; + + return *this; + } + + public: + /** + * Gets the exception message + * @return error message as string + */ + const std::string& GetErrorMessage() const + { + return m_Message; + } + + /** + * Gets error code + * @return error code + */ + kt_int32s GetErrorCode() + { + return m_ErrorCode; + } + + public: + /** + * Write exception to output stream + * @param rStream output stream + * @param rException exception to write + */ + friend KARTO_EXPORT std::ostream& operator << (std::ostream& rStream, Exception& rException); + + private: + std::string m_Message; + kt_int32s m_ErrorCode; + }; // class Exception + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Subclass this class to make a non-copyable class (copy + * constructor and assignment operator are private) + */ + class KARTO_EXPORT NonCopyable + { + private: + NonCopyable(const NonCopyable&); + const NonCopyable& operator=(const NonCopyable&); + + protected: + NonCopyable() + { + } + + virtual ~NonCopyable() + { + } + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + } + }; // class NonCopyable + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Singleton class ensures only one instance of T is created + */ + template + class Singleton + { + public: + /** + * Constructor + */ + Singleton() + : m_pPointer(NULL) + { + } + + /** + * Destructor + */ + virtual ~Singleton() + { + delete m_pPointer; + } + + /** + * Gets the singleton + * @return singleton + */ + T* Get() + { +#ifdef USE_POCO + Poco::FastMutex::ScopedLock lock(m_Mutex); +#endif + if (m_pPointer == NULL) + { + m_pPointer = new T; + } + + return m_pPointer; + } + + private: + T* m_pPointer; + +#ifdef USE_POCO + Poco::FastMutex m_Mutex; +#endif + + private: + Singleton(const Singleton&); + const Singleton& operator=(const Singleton&); + }; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Functor + */ + class KARTO_EXPORT Functor + { + public: + /** + * Functor function + */ + virtual void operator() (kt_int32u) {}; + }; // Functor + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + class AbstractParameter; + + /** + * Type declaration of AbstractParameter vector + */ + typedef std::vector ParameterVector; + + /** + * Parameter manager. + */ + class KARTO_EXPORT ParameterManager : public NonCopyable + { + public: + /** + * Default constructor + */ + ParameterManager() + { + } + + /** + * Destructor + */ + virtual ~ParameterManager() + { + Clear(); + } + + public: + /** + * Adds the parameter to this manager + * @param pParameter + */ + void Add(AbstractParameter* pParameter); + + /** + * Gets the parameter of the given name + * @param rName + * @return parameter of given name + */ + AbstractParameter* Get(const std::string& rName) + { + if (m_ParameterLookup.find(rName) != m_ParameterLookup.end()) + { + return m_ParameterLookup[rName]; + } + + std::cout << "Unknown parameter: " << rName << std::endl; + + return NULL; + } + + /** + * Clears the manager of all parameters + */ + void Clear(); + + /** + * Gets all parameters + * @return vector of all parameters + */ + inline const ParameterVector& GetParameterVector() const + { + return m_Parameters; + } + + public: + /** + * Gets the parameter with the given name + * @param rName + * @return parameter of given name + */ + AbstractParameter* operator() (const std::string& rName) + { + return Get(rName); + } + + /** + * Serialization: class ParameterManager + */ + + private: + ParameterManager(const ParameterManager&); + const ParameterManager& operator=(const ParameterManager&); + + private: + ParameterVector m_Parameters; + std::map m_ParameterLookup; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(NonCopyable); + ar & BOOST_SERIALIZATION_NVP(m_Parameters); + ar & BOOST_SERIALIZATION_NVP(m_ParameterLookup); + } + + }; // ParameterManager + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + // valid names + // 'Test' -- no scope + // '/Test' -- no scope will be parsed to 'Test' + // '/scope/Test' - 'scope' scope and 'Test' name + // '/scope/name/Test' - 'scope/name' scope and 'Test' name + // + class Name + { + public: + /** + * Constructor + */ + Name() + { + } + + /** + * Constructor + */ + Name(const std::string& rName) + { + Parse(rName); + } + + /** + * Constructor + */ + Name(const Name& rOther) + : m_Name(rOther.m_Name) + , m_Scope(rOther.m_Scope) + { + } + + /** + * Destructor + */ + virtual ~Name() + { + } + + public: + /** + * Gets the name of this name + * @return name + */ + inline const std::string& GetName() const + { + return m_Name; + } + + /** + * Sets the name + * @param rName name + */ + inline void SetName(const std::string& rName) + { + std::string::size_type pos = rName.find_last_of('/'); + if (pos != 0 && pos != std::string::npos) + { + throw Exception("Name can't contain a scope!"); + } + + m_Name = rName; + } + + /** + * Gets the scope of this name + * @return scope + */ + inline const std::string& GetScope() const + { + return m_Scope; + } + + /** + * Sets the scope of this name + * @param rScope scope + */ + inline void SetScope(const std::string& rScope) + { + m_Scope = rScope; + } + + /** + * Returns a string representation of this name + * @return string representation of this name + */ + inline std::string ToString() const + { + if (m_Scope == "") + { + return m_Name; + } + else + { + std::string name; + name.append("/"); + name.append(m_Scope); + name.append("/"); + name.append(m_Name); + + return name; + } + } + + public: + /** + * Assignment operator. + */ + Name& operator = (const Name& rOther) + { + if (&rOther != this) + { + m_Name = rOther.m_Name; + m_Scope = rOther.m_Scope; + } + + return *this; + } + + /** + * Equality operator. + */ + kt_bool operator == (const Name& rOther) const + { + return (m_Name == rOther.m_Name) && (m_Scope == rOther.m_Scope); + } + + /** + * Inequality operator. + */ + kt_bool operator != (const Name& rOther) const + { + return !(*this == rOther); + } + + /** + * Less than operator. + */ + kt_bool operator < (const Name& rOther) const + { + return ToString() < rOther.ToString(); + } + + /** + * Write Name onto output stream + * @param rStream output stream + * @param rName to write + */ + friend inline std::ostream& operator << (std::ostream& rStream, const Name& rName) + { + rStream << rName.ToString(); + return rStream; + } + + private: + /** + * Parse the given string into a Name object + * @param rName name + */ + void Parse(const std::string& rName) + { + std::string::size_type pos = rName.find_last_of('/'); + + if (pos == std::string::npos) + { + m_Name = rName; + } + else + { + m_Scope = rName.substr(0, pos); + m_Name = rName.substr(pos+1, rName.size()); + + // remove '/' from m_Scope if first!! + if (m_Scope.size() > 0 && m_Scope[0] == '/') + { + m_Scope = m_Scope.substr(1, m_Scope.size()); + } + } + } + + /** + * Validates the given string as a Name + * @param rName name + */ + void Validate(const std::string& rName) + { + if (rName.empty()) + { + return; + } + + char c = rName[0]; + if (IsValidFirst(c)) + { + for (size_t i = 1; i < rName.size(); ++i) + { + c = rName[i]; + if (!IsValid(c)) + { + throw Exception("Invalid character in name. Valid characters must be within the ranges A-Z, a-z, 0-9, '/', '_' and '-'."); + } + } + } + else + { + throw Exception("Invalid first character in name. Valid characters must be within the ranges A-Z, a-z, and '/'."); + } + } + + /** + * Whether the character is valid as a first character (alphanumeric or /) + * @param c character + * @return true if the character is a valid first character + */ + inline kt_bool IsValidFirst(char c) + { + return (isalpha(c) || c == '/'); + } + + /** + * Whether the character is a valid character (alphanumeric, /, _, or -) + * @param c character + * @return true if the character is valid + */ + inline kt_bool IsValid(char c) + { + return (isalnum(c) || c == '/' || c == '_' || c == '-'); + } + + private: + std::string m_Name; + std::string m_Scope; + /** + * Serialization: class Name + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Name); + ar & BOOST_SERIALIZATION_NVP(m_Scope); + } + }; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Abstract base class for Karto objects. + */ + class KARTO_EXPORT Object : public NonCopyable + { + public: + /** + * Default constructor + */ + Object(); + + /** + * Constructs an object with the given name + * @param rName + */ + Object(const Name& rName); + + /** + * Default constructor + */ + virtual ~Object(); + + public: + /** + * Gets the name of this object + * @return name + */ + inline const Name& GetName() const + { + return m_Name; + } + + /** + * Gets the class name of this object + * @return class name + */ + virtual const char* GetClassName() const = 0; + + /** + * Gets the type of this object + * @return object type + */ + virtual kt_objecttype GetObjectType() const = 0; + + /** + * Gets the parameter manager of this dataset + * @return parameter manager + */ + virtual inline ParameterManager* GetParameterManager() + { + return m_pParameterManager; + } + + /** + * Gets the named parameter + * @param rName name of parameter + * @return parameter + */ + inline AbstractParameter* GetParameter(const std::string& rName) const + { + return m_pParameterManager->Get(rName); + } + + /** + * Sets the parameter with the given name with the given value + * @param rName name + * @param value value + */ + template + inline void SetParameter(const std::string& rName, T value); + + /** + * Gets all parameters + * @return parameters + */ + inline const ParameterVector& GetParameters() const + { + return m_pParameterManager->GetParameterVector(); + } + + Object(const Object&); + const Object& operator=(const Object&); + + private: + Name m_Name; + ParameterManager* m_pParameterManager; + /** + * Serialization: class Object + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(NonCopyable); + ar & BOOST_SERIALIZATION_NVP(m_pParameterManager); + ar & BOOST_SERIALIZATION_NVP(m_Name); + } + }; + BOOST_SERIALIZATION_ASSUME_ABSTRACT(Object) + + /** + * Type declaration of Object vector + */ + typedef std::vector ObjectVector; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Whether the object is a sensor + * @param pObject object + * @return whether the object is a sensor + */ + inline kt_bool IsSensor(Object* pObject) + { + return (pObject->GetObjectType() & ObjectType_Sensor) == ObjectType_Sensor; + } + + /** + * Whether the object is sensor data + * @param pObject object + * @return whether the object is sensor data + */ + inline kt_bool IsSensorData(Object* pObject) + { + return (pObject->GetObjectType() & ObjectType_SensorData) == ObjectType_SensorData; + } + + /** + * Whether the object is a laser range finder + * @param pObject object + * @return whether the object is a laser range finder + */ + inline kt_bool IsLaserRangeFinder(Object* pObject) + { + return (pObject->GetObjectType() & ObjectType_LaserRangeFinder) == ObjectType_LaserRangeFinder; + } + + /** + * Whether the object is a localized range scan + * @param pObject object + * @return whether the object is a localized range scan + */ + inline kt_bool IsLocalizedRangeScan(Object* pObject) + { + return (pObject->GetObjectType() & ObjectType_LocalizedRangeScan) == ObjectType_LocalizedRangeScan; + } + + /** + * Whether the object is a localized range scan with points + * @param pObject object + * @return whether the object is a localized range scan with points + */ + inline kt_bool IsLocalizedRangeScanWithPoints(Object* pObject) + { + return (pObject->GetObjectType() & ObjectType_LocalizedRangeScanWithPoints) == ObjectType_LocalizedRangeScanWithPoints; + } + + /** + * Whether the object is a Parameters object + * @param pObject object + * @return whether the object is a Parameters object + */ + inline kt_bool IsParameters(Object* pObject) + { + return (pObject->GetObjectType() & ObjectType_Parameters) == ObjectType_Parameters; + } + + /** + * Whether the object is a DatasetInfo object + * @param pObject object + * @return whether the object is a DatasetInfo object + */ + inline kt_bool IsDatasetInfo(Object* pObject) + { + return (pObject->GetObjectType() & ObjectType_DatasetInfo) == ObjectType_DatasetInfo; + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Abstract base class for Karto modules. + */ + class KARTO_EXPORT Module : public Object + { + public: + // @cond EXCLUDE + KARTO_Object(Module) + // @endcond + + public: + /** + * Construct a Module + * @param rName module name + */ + Module(const std::string& rName); + + /** + * Destructor + */ + virtual ~Module(); + + public: + /** + * Reset the module + */ + virtual void Reset() = 0; + + /** + * Process an Object + */ + virtual kt_bool Process(karto::Object*) + { + return false; + } + + private: + Module(const Module&); + const Module& operator=(const Module&); + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object); + } + }; + BOOST_SERIALIZATION_ASSUME_ABSTRACT(Module) + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Represents a size (width, height) in 2-dimensional real space. + */ + template + class Size2 + { + public: + /** + * Default constructor + */ + Size2() + : m_Width(0) + , m_Height(0) + { + } + + /** + * Constructor initializing point location + * @param width + * @param height + */ + Size2(T width, T height) + : m_Width(width) + , m_Height(height) + { + } + + /** + * Copy constructor + * @param rOther + */ + Size2(const Size2& rOther) + : m_Width(rOther.m_Width) + , m_Height(rOther.m_Height) + { + } + + public: + /** + * Gets the width + * @return the width + */ + inline const T GetWidth() const + { + return m_Width; + } + + /** + * Sets the width + * @param width + */ + inline void SetWidth(T width) + { + m_Width = width; + } + + /** + * Gets the height + * @return the height + */ + inline const T GetHeight() const + { + return m_Height; + } + + /** + * Sets the height + * @param height + */ + inline void SetHeight(T height) + { + m_Height = height; + } + + /** + * Assignment operator + */ + inline Size2& operator = (const Size2& rOther) + { + m_Width = rOther.m_Width; + m_Height = rOther.m_Height; + + return(*this); + } + + /** + * Equality operator + */ + inline kt_bool operator == (const Size2& rOther) const + { + return (m_Width == rOther.m_Width && m_Height == rOther.m_Height); + } + + /** + * Inequality operator + */ + inline kt_bool operator != (const Size2& rOther) const + { + return (m_Width != rOther.m_Width || m_Height != rOther.m_Height); + } + + /** + * Write Size2 onto output stream + * @param rStream output stream + * @param rSize to write + */ + friend inline std::ostream& operator << (std::ostream& rStream, const Size2& rSize) + { + rStream << "(" << rSize.m_Width << ", " << rSize.m_Height << ")"; + return rStream; + } + + private: + T m_Width; + T m_Height; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Width); + ar & BOOST_SERIALIZATION_NVP(m_Height); + } + }; // Size2 + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Represents a vector (x, y) in 2-dimensional real space. + */ + template + class Vector2 + { + public: + /** + * Default constructor + */ + Vector2() + { + m_Values[0] = 0; + m_Values[1] = 0; + } + + /** + * Constructor initializing vector location + * @param x + * @param y + */ + Vector2(T x, T y) + { + m_Values[0] = x; + m_Values[1] = y; + } + + public: + /** + * Gets the x-coordinate of this vector2 + * @return the x-coordinate of the vector2 + */ + inline const T& GetX() const + { + return m_Values[0]; + } + + /** + * Sets the x-coordinate of this vector2 + * @param x the x-coordinate of the vector2 + */ + inline void SetX(const T& x) + { + m_Values[0] = x; + } + + /** + * Gets the y-coordinate of this vector2 + * @return the y-coordinate of the vector2 + */ + inline const T& GetY() const + { + return m_Values[1]; + } + + /** + * Sets the y-coordinate of this vector2 + * @param y the y-coordinate of the vector2 + */ + inline void SetY(const T& y) + { + m_Values[1] = y; + } + + /** + * Floor point operator + * @param rOther + */ + inline void MakeFloor(const Vector2& rOther) + { + if ( rOther.m_Values[0] < m_Values[0] ) m_Values[0] = rOther.m_Values[0]; + if ( rOther.m_Values[1] < m_Values[1] ) m_Values[1] = rOther.m_Values[1]; + } + + /** + * Ceiling point operator + * @param rOther + */ + inline void MakeCeil(const Vector2& rOther) + { + if ( rOther.m_Values[0] > m_Values[0] ) m_Values[0] = rOther.m_Values[0]; + if ( rOther.m_Values[1] > m_Values[1] ) m_Values[1] = rOther.m_Values[1]; + } + + /** + * Returns the square of the length of the vector + * @return square of the length of the vector + */ + inline kt_double SquaredLength() const + { + return math::Square(m_Values[0]) + math::Square(m_Values[1]); + } + + /** + * Returns the length of the vector (x and y). + * @return length of the vector + */ + inline kt_double Length() const + { + return sqrt(SquaredLength()); + } + + /** + * Returns the square distance to the given vector + * @returns square distance to the given vector + */ + inline kt_double SquaredDistance(const Vector2& rOther) const + { + return (*this - rOther).SquaredLength(); + } + + /** + * Gets the distance to the other vector2 + * @param rOther + * @return distance to other vector2 + */ + inline kt_double Distance(const Vector2& rOther) const + { + return sqrt(SquaredDistance(rOther)); + } + + public: + /** + * In place Vector2 addition. + */ + inline void operator += (const Vector2& rOther) + { + m_Values[0] += rOther.m_Values[0]; + m_Values[1] += rOther.m_Values[1]; + } + + /** + * In place Vector2 subtraction. + */ + inline void operator -= (const Vector2& rOther) + { + m_Values[0] -= rOther.m_Values[0]; + m_Values[1] -= rOther.m_Values[1]; + } + + /** + * Addition operator + * @param rOther + * @return vector resulting from adding this vector with the given vector + */ + inline const Vector2 operator + (const Vector2& rOther) const + { + return Vector2(m_Values[0] + rOther.m_Values[0], m_Values[1] + rOther.m_Values[1]); + } + + /** + * Subtraction operator + * @param rOther + * @return vector resulting from subtracting this vector from the given vector + */ + inline const Vector2 operator - (const Vector2& rOther) const + { + return Vector2(m_Values[0] - rOther.m_Values[0], m_Values[1] - rOther.m_Values[1]); + } + + /** + * In place scalar division operator + * @param scalar + */ + inline void operator /= (T scalar) + { + m_Values[0] /= scalar; + m_Values[1] /= scalar; + } + + /** + * Divides a Vector2 + * @param scalar + * @return scalar product + */ + inline const Vector2 operator / (T scalar) const + { + return Vector2(m_Values[0] / scalar, m_Values[1] / scalar); + } + + /** + * Computes the dot product between the two vectors + * @param rOther + * @return dot product + */ + inline kt_double operator * (const Vector2& rOther) const + { + return m_Values[0] * rOther.m_Values[0] + m_Values[1] * rOther.m_Values[1]; + } + + /** + * Scales the vector by the given scalar + * @param scalar + */ + inline const Vector2 operator * (T scalar) const + { + return Vector2(m_Values[0] * scalar, m_Values[1] * scalar); + } + + /** + * Subtract the vector by the given scalar + * @param scalar + */ + inline const Vector2 operator - (T scalar) const + { + return Vector2(m_Values[0] - scalar, m_Values[1] - scalar); + } + + /** + * In place scalar multiplication operator + * @param scalar + */ + inline void operator *= (T scalar) + { + m_Values[0] *= scalar; + m_Values[1] *= scalar; + } + + /** + * Equality operator returns true if the corresponding x, y values of each Vector2 are the same values. + * @param rOther + */ + inline kt_bool operator == (const Vector2& rOther) const + { + return (m_Values[0] == rOther.m_Values[0] && m_Values[1] == rOther.m_Values[1]); + } + + /** + * Inequality operator returns true if any of the corresponding x, y values of each Vector2 not the same. + * @param rOther + */ + inline kt_bool operator != (const Vector2& rOther) const + { + return (m_Values[0] != rOther.m_Values[0] || m_Values[1] != rOther.m_Values[1]); + } + + /** + * Less than operator + * @param rOther + * @return true if left vector is less than right vector + */ + inline kt_bool operator < (const Vector2& rOther) const + { + if (m_Values[0] < rOther.m_Values[0]) + return true; + else if (m_Values[0] > rOther.m_Values[0]) + return false; + else + return (m_Values[1] < rOther.m_Values[1]); + } + + /** + * Write Vector2 onto output stream + * @param rStream output stream + * @param rVector to write + */ + friend inline std::ostream& operator << (std::ostream& rStream, const Vector2& rVector) + { + rStream << rVector.GetX() << " " << rVector.GetY(); + return rStream; + } + + /** + * Read Vector2 from input stream + * @param rStream input stream + */ + friend inline std::istream& operator >> (std::istream& rStream, const Vector2& /*rVector*/) + { + // Implement me!! TODO(lucbettaieb): What the what? Do I need to implement this? + return rStream; + } + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & boost::serialization::make_nvp("m_Values_0", m_Values[0]); + ar & boost::serialization::make_nvp("m_Values_1", m_Values[1]); + } + + private: + T m_Values[2]; + }; // Vector2 + + /** + * Type declaration of Vector2 vector + */ + typedef std::vector< Vector2 > PointVectorDouble; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Represents a vector (x, y, z) in 3-dimensional real space. + */ + template + class Vector3 + { + public: + /** + * Default constructor + */ + Vector3() + { + m_Values[0] = 0; + m_Values[1] = 0; + m_Values[2] = 0; + } + + /** + * Constructor initializing point location + * @param x + * @param y + * @param z + */ + Vector3(T x, T y, T z) + { + m_Values[0] = x; + m_Values[1] = y; + m_Values[2] = z; + } + + /** + * Copy constructor + * @param rOther + */ + Vector3(const Vector3& rOther) + { + m_Values[0] = rOther.m_Values[0]; + m_Values[1] = rOther.m_Values[1]; + m_Values[2] = rOther.m_Values[2]; + } + + public: + /** + * Gets the x-component of this vector + * @return x-component + */ + inline const T& GetX() const + { + return m_Values[0]; + } + + /** + * Sets the x-component of this vector + * @param x + */ + inline void SetX(const T& x) + { + m_Values[0] = x; + } + + /** + * Gets the y-component of this vector + * @return y-component + */ + inline const T& GetY() const + { + return m_Values[1]; + } + + /** + * Sets the y-component of this vector + * @param y + */ + inline void SetY(const T& y) + { + m_Values[1] = y; + } + + /** + * Gets the z-component of this vector + * @return z-component + */ + inline const T& GetZ() const + { + return m_Values[2]; + } + + /** + * Sets the z-component of this vector + * @param z + */ + inline void SetZ(const T& z) + { + m_Values[2] = z; + } + + /** + * Floor vector operator + * @param rOther Vector3d + */ + inline void MakeFloor(const Vector3& rOther) + { + if (rOther.m_Values[0] < m_Values[0]) m_Values[0] = rOther.m_Values[0]; + if (rOther.m_Values[1] < m_Values[1]) m_Values[1] = rOther.m_Values[1]; + if (rOther.m_Values[2] < m_Values[2]) m_Values[2] = rOther.m_Values[2]; + } + + /** + * Ceiling vector operator + * @param rOther Vector3d + */ + inline void MakeCeil(const Vector3& rOther) + { + if (rOther.m_Values[0] > m_Values[0]) m_Values[0] = rOther.m_Values[0]; + if (rOther.m_Values[1] > m_Values[1]) m_Values[1] = rOther.m_Values[1]; + if (rOther.m_Values[2] > m_Values[2]) m_Values[2] = rOther.m_Values[2]; + } + + /** + * Returns the square of the length of the vector + * @return square of the length of the vector + */ + inline kt_double SquaredLength() const + { + return math::Square(m_Values[0]) + math::Square(m_Values[1]) + math::Square(m_Values[2]); + } + + /** + * Returns the length of the vector. + * @return Length of the vector + */ + inline kt_double Length() const + { + return sqrt(SquaredLength()); + } + + /** + * Returns a string representation of this vector + * @return string representation of this vector + */ + inline std::string ToString() const + { + std::stringstream converter; + converter.precision(std::numeric_limits::digits10); + + converter << GetX() << " " << GetY() << " " << GetZ(); + + return converter.str(); + } + + public: + /** + * Assignment operator + */ + inline Vector3& operator = (const Vector3& rOther) + { + m_Values[0] = rOther.m_Values[0]; + m_Values[1] = rOther.m_Values[1]; + m_Values[2] = rOther.m_Values[2]; + + return *this; + } + + /** + * Binary vector add. + * @param rOther + * @return vector sum + */ + inline const Vector3 operator + (const Vector3& rOther) const + { + return Vector3(m_Values[0] + rOther.m_Values[0], + m_Values[1] + rOther.m_Values[1], + m_Values[2] + rOther.m_Values[2]); + } + + /** + * Binary vector add. + * @param scalar + * @return sum + */ + inline const Vector3 operator + (kt_double scalar) const + { + return Vector3(m_Values[0] + scalar, + m_Values[1] + scalar, + m_Values[2] + scalar); + } + + /** + * Binary vector subtract. + * @param rOther + * @return vector difference + */ + inline const Vector3 operator - (const Vector3& rOther) const + { + return Vector3(m_Values[0] - rOther.m_Values[0], + m_Values[1] - rOther.m_Values[1], + m_Values[2] - rOther.m_Values[2]); + } + + /** + * Binary vector subtract. + * @param scalar + * @return difference + */ + inline const Vector3 operator - (kt_double scalar) const + { + return Vector3(m_Values[0] - scalar, m_Values[1] - scalar, m_Values[2] - scalar); + } + + /** + * Scales the vector by the given scalar + * @param scalar + */ + inline const Vector3 operator * (T scalar) const + { + return Vector3(m_Values[0] * scalar, m_Values[1] * scalar, m_Values[2] * scalar); + } + + /** + * Equality operator returns true if the corresponding x, y, z values of each Vector3 are the same values. + * @param rOther + */ + inline kt_bool operator == (const Vector3& rOther) const + { + return (m_Values[0] == rOther.m_Values[0] && + m_Values[1] == rOther.m_Values[1] && + m_Values[2] == rOther.m_Values[2]); + } + + /** + * Inequality operator returns true if any of the corresponding x, y, z values of each Vector3 not the same. + * @param rOther + */ + inline kt_bool operator != (const Vector3& rOther) const + { + return (m_Values[0] != rOther.m_Values[0] || + m_Values[1] != rOther.m_Values[1] || + m_Values[2] != rOther.m_Values[2]); + } + + /** + * Write Vector3 onto output stream + * @param rStream output stream + * @param rVector to write + */ + friend inline std::ostream& operator << (std::ostream& rStream, const Vector3& rVector) + { + rStream << rVector.ToString(); + return rStream; + } + + /** + * Read Vector3 from input stream + * @param rStream input stream + */ + friend inline std::istream& operator >> (std::istream& rStream, const Vector3& /*rVector*/) + { + // Implement me!! + return rStream; + } + + private: + T m_Values[3]; + }; // Vector3 + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Defines an orientation as a quaternion rotation using the positive Z axis as the zero reference. + *
+ * Q = w + ix + jy + kz
+ * w = c_1 * c_2 * c_3 - s_1 * s_2 * s_3
+ * x = s_1 * s_2 * c_3 + c_1 * c_2 * s_3
+ * y = s_1 * c_2 * c_3 + c_1 * s_2 * s_3
+ * z = c_1 * s_2 * c_3 - s_1 * c_2 * s_3
+ * where
+ * c_1 = cos(theta/2)
+ * c_2 = cos(phi/2)
+ * c_3 = cos(psi/2)
+ * s_1 = sin(theta/2)
+ * s_2 = sin(phi/2)
+ * s_3 = sin(psi/2)
+ * and
+ * theta is the angle of rotation about the Y axis measured from the Z axis.
+ * phi is the angle of rotation about the Z axis measured from the X axis.
+ * psi is the angle of rotation about the X axis measured from the Y axis.
+ * (All angles are right-handed.) + */ + class Quaternion + { + public: + /** + * Create a quaternion with default (x=0, y=0, z=0, w=1) values + */ + inline Quaternion() + { + m_Values[0] = 0.0; + m_Values[1] = 0.0; + m_Values[2] = 0.0; + m_Values[3] = 1.0; + } + + /** + * Create a quaternion using x, y, z, w values. + * @param x + * @param y + * @param z + * @param w + */ + inline Quaternion(kt_double x, kt_double y, kt_double z, kt_double w) + { + m_Values[0] = x; + m_Values[1] = y; + m_Values[2] = z; + m_Values[3] = w; + } + + /** + * Copy constructor + */ + inline Quaternion(const Quaternion& rQuaternion) + { + m_Values[0] = rQuaternion.m_Values[0]; + m_Values[1] = rQuaternion.m_Values[1]; + m_Values[2] = rQuaternion.m_Values[2]; + m_Values[3] = rQuaternion.m_Values[3]; + } + + public: + /** + * Returns the X-value + * @return Return the X-value of the quaternion + */ + inline kt_double GetX() const + { + return m_Values[0]; + } + + /** + * Sets the X-value + * @param x X-value of the quaternion + */ + inline void SetX(kt_double x) + { + m_Values[0] = x; + } + + /** + * Returns the Y-value + * @return Return the Y-value of the quaternion + */ + inline kt_double GetY() const + { + return m_Values[1]; + } + + /** + * Sets the Y-value + * @param y Y-value of the quaternion + */ + inline void SetY(kt_double y) + { + m_Values[1] = y; + } + + /** + * Returns the Z-value + * @return Return the Z-value of the quaternion + */ + inline kt_double GetZ() const + { + return m_Values[2]; + } + + /** + * Sets the Z-value + * @param z Z-value of the quaternion + */ + inline void SetZ(kt_double z) + { + m_Values[2] = z; + } + + /** + * Returns the W-value + * @return Return the W-value of the quaternion + */ + inline kt_double GetW() const + { + return m_Values[3]; + } + + /** + * Sets the W-value + * @param w W-value of the quaternion + */ + inline void SetW(kt_double w) + { + m_Values[3] = w; + } + + /** + * Converts this quaternion into Euler angles + * Source: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm + * @param rYaw + * @param rPitch + * @param rRoll + */ + void ToEulerAngles(kt_double& rYaw, kt_double& rPitch, kt_double& rRoll) const + { + kt_double test = m_Values[0] * m_Values[1] + m_Values[2] * m_Values[3]; + + if (test > 0.499) + { + // singularity at north pole + rYaw = 2 * atan2(m_Values[0], m_Values[3]);; + rPitch = KT_PI_2; + rRoll = 0; + } + else if (test < -0.499) + { + // singularity at south pole + rYaw = -2 * atan2(m_Values[0], m_Values[3]); + rPitch = -KT_PI_2; + rRoll = 0; + } + else + { + kt_double sqx = m_Values[0] * m_Values[0]; + kt_double sqy = m_Values[1] * m_Values[1]; + kt_double sqz = m_Values[2] * m_Values[2]; + + rYaw = atan2(2 * m_Values[1] * m_Values[3] - 2 * m_Values[0] * m_Values[2], 1 - 2 * sqy - 2 * sqz); + rPitch = asin(2 * test); + rRoll = atan2(2 * m_Values[0] * m_Values[3] - 2 * m_Values[1] * m_Values[2], 1 - 2 * sqx - 2 * sqz); + } + } + + /** + * Set x,y,z,w values of the quaternion based on Euler angles. + * Source: http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm + * @param yaw + * @param pitch + * @param roll + */ + void FromEulerAngles(kt_double yaw, kt_double pitch, kt_double roll) + { + kt_double angle; + + angle = yaw * 0.5; + kt_double cYaw = cos(angle); + kt_double sYaw = sin(angle); + + angle = pitch * 0.5; + kt_double cPitch = cos(angle); + kt_double sPitch = sin(angle); + + angle = roll * 0.5; + kt_double cRoll = cos(angle); + kt_double sRoll = sin(angle); + + m_Values[0] = sYaw * sPitch * cRoll + cYaw * cPitch * sRoll; + m_Values[1] = sYaw * cPitch * cRoll + cYaw * sPitch * sRoll; + m_Values[2] = cYaw * sPitch * cRoll - sYaw * cPitch * sRoll; + m_Values[3] = cYaw * cPitch * cRoll - sYaw * sPitch * sRoll; + } + + /** + * Assignment operator + * @param rQuaternion + */ + inline Quaternion& operator = (const Quaternion& rQuaternion) + { + m_Values[0] = rQuaternion.m_Values[0]; + m_Values[1] = rQuaternion.m_Values[1]; + m_Values[2] = rQuaternion.m_Values[2]; + m_Values[3] = rQuaternion.m_Values[3]; + + return(*this); + } + + /** + * Equality operator returns true if the corresponding x, y, z, w values of each quaternion are the same values. + * @param rOther + */ + inline kt_bool operator == (const Quaternion& rOther) const + { + return (m_Values[0] == rOther.m_Values[0] && + m_Values[1] == rOther.m_Values[1] && + m_Values[2] == rOther.m_Values[2] && + m_Values[3] == rOther.m_Values[3]); + } + + /** + * Inequality operator returns true if any of the corresponding x, y, z, w values of each quaternion not the same. + * @param rOther + */ + inline kt_bool operator != (const Quaternion& rOther) const + { + return (m_Values[0] != rOther.m_Values[0] || + m_Values[1] != rOther.m_Values[1] || + m_Values[2] != rOther.m_Values[2] || + m_Values[3] != rOther.m_Values[3]); + } + + /** + * Write this quaternion onto output stream + * @param rStream output stream + * @param rQuaternion + */ + friend inline std::ostream& operator << (std::ostream& rStream, const Quaternion& rQuaternion) + { + rStream << rQuaternion.m_Values[0] << " " + << rQuaternion.m_Values[1] << " " + << rQuaternion.m_Values[2] << " " + << rQuaternion.m_Values[3]; + return rStream; + } + + private: + kt_double m_Values[4]; + }; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Stores x, y, width and height that represents the location and size of a rectangle + * (x, y) is at bottom left in mapper! + */ + template + class Rectangle2 + { + public: + /** + * Default constructor + */ + Rectangle2() + { + } + + /** + * Constructor initializing rectangle parameters + * @param x x-coordinate of left edge of rectangle + * @param y y-coordinate of bottom edge of rectangle + * @param width width of rectangle + * @param height height of rectangle + */ + Rectangle2(T x, T y, T width, T height) + : m_Position(x, y) + , m_Size(width, height) + { + } + + /** + * Constructor initializing rectangle parameters + * @param rPosition (x,y)-coordinate of rectangle + * @param rSize Size of the rectangle + */ + Rectangle2(const Vector2& rPosition, const Size2& rSize) + : m_Position(rPosition) + , m_Size(rSize) + { + } + + /** + * Copy constructor + */ + Rectangle2(const Rectangle2& rOther) + : m_Position(rOther.m_Position) + , m_Size(rOther.m_Size) + { + } + + public: + /** + * Gets the x-coordinate of the left edge of this rectangle + * @return the x-coordinate of the left edge of this rectangle + */ + inline T GetX() const + { + return m_Position.GetX(); + } + + /** + * Sets the x-coordinate of the left edge of this rectangle + * @param x the x-coordinate of the left edge of this rectangle + */ + inline void SetX(T x) + { + m_Position.SetX(x); + } + + /** + * Gets the y-coordinate of the bottom edge of this rectangle + * @return the y-coordinate of the bottom edge of this rectangle + */ + inline T GetY() const + { + return m_Position.GetY(); + } + + /** + * Sets the y-coordinate of the bottom edge of this rectangle + * @param y the y-coordinate of the bottom edge of this rectangle + */ + inline void SetY(T y) + { + m_Position.SetY(y); + } + + /** + * Gets the width of this rectangle + * @return the width of this rectangle + */ + inline T GetWidth() const + { + return m_Size.GetWidth(); + } + + /** + * Sets the width of this rectangle + * @param width the width of this rectangle + */ + inline void SetWidth(T width) + { + m_Size.SetWidth(width); + } + + /** + * Gets the height of this rectangle + * @return the height of this rectangle + */ + inline T GetHeight() const + { + return m_Size.GetHeight(); + } + + /** + * Sets the height of this rectangle + * @param height the height of this rectangle + */ + inline void SetHeight(T height) + { + m_Size.SetHeight(height); + } + + /** + * Gets the position of this rectangle + * @return the position of this rectangle + */ + inline const Vector2& GetPosition() const + { + return m_Position; + } + + /** + * Sets the position of this rectangle + * @param rX x + * @param rY y + */ + inline void SetPosition(const T& rX, const T& rY) + { + m_Position = Vector2(rX, rY); + } + + /** + * Sets the position of this rectangle + * @param rPosition position + */ + inline void SetPosition(const Vector2& rPosition) + { + m_Position = rPosition; + } + + /** + * Gets the size of this rectangle + * @return the size of this rectangle + */ + inline const Size2& GetSize() const + { + return m_Size; + } + + /** + * Sets the size of this rectangle + * @param rSize size + */ + inline void SetSize(const Size2& rSize) + { + m_Size = rSize; + } + + /** + * Gets the center of this rectangle + * @return the center of this rectangle + */ + inline const Vector2 GetCenter() const + { + return Vector2(m_Position.GetX() + m_Size.GetWidth() * 0.5, m_Position.GetY() + m_Size.GetHeight() * 0.5); + } + + public: + /** + * Assignment operator + */ + Rectangle2& operator = (const Rectangle2& rOther) + { + m_Position = rOther.m_Position; + m_Size = rOther.m_Size; + + return *this; + } + + /** + * Equality operator + */ + inline kt_bool operator == (const Rectangle2& rOther) const + { + return (m_Position == rOther.m_Position && m_Size == rOther.m_Size); + } + + /** + * Inequality operator + */ + inline kt_bool operator != (const Rectangle2& rOther) const + { + return (m_Position != rOther.m_Position || m_Size != rOther.m_Size); + } + + private: + Vector2 m_Position; + Size2 m_Size; + /** + * Serialization: class Rectangle2 + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Position); + ar & BOOST_SERIALIZATION_NVP(m_Size); + } + }; // Rectangle2 + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + class Pose3; + + /** + * Defines a position (x, y) in 2-dimensional space and heading. + */ + class Pose2 + { + public: + /** + * Default Constructor + */ + Pose2() + : m_Heading(0.0) + { + } + + /** + * Constructor initializing pose parameters + * @param rPosition position + * @param heading heading + **/ + Pose2(const Vector2& rPosition, kt_double heading) + : m_Position(rPosition) + , m_Heading(heading) + { + } + + /** + * Constructor initializing pose parameters + * @param x x-coordinate + * @param y y-coordinate + * @param heading heading + **/ + Pose2(kt_double x, kt_double y, kt_double heading) + : m_Position(x, y) + , m_Heading(heading) + { + } + + /** + * Constructs a Pose2 object from a Pose3. + */ + Pose2(const Pose3& rPose); + + /** + * Copy constructor + */ + Pose2(const Pose2& rOther) + : m_Position(rOther.m_Position) + , m_Heading(rOther.m_Heading) + { + } + + public: + /** + * Returns the x-coordinate + * @return the x-coordinate of the pose + */ + inline kt_double GetX() const + { + return m_Position.GetX(); + } + + /** + * Sets the x-coordinate + * @param x the x-coordinate of the pose + */ + inline void SetX(kt_double x) + { + m_Position.SetX(x); + } + + /** + * Returns the y-coordinate + * @return the y-coordinate of the pose + */ + inline kt_double GetY() const + { + return m_Position.GetY(); + } + + /** + * Sets the y-coordinate + * @param y the y-coordinate of the pose + */ + inline void SetY(kt_double y) + { + m_Position.SetY(y); + } + + /** + * Returns the position + * @return the position of the pose + */ + inline const Vector2& GetPosition() const + { + return m_Position; + } + + /** + * Sets the position + * @param rPosition of the pose + */ + inline void SetPosition(const Vector2& rPosition) + { + m_Position = rPosition; + } + + /** + * Returns the heading of the pose (in radians) + * @return the heading of the pose + */ + inline kt_double GetHeading() const + { + return m_Heading; + } + + /** + * Sets the heading + * @param heading of the pose + */ + inline void SetHeading(kt_double heading) + { + m_Heading = heading; + } + + /** + * Return the squared distance between two Pose2 + * @return squared distance + */ + inline kt_double SquaredDistance(const Pose2& rOther) const + { + return m_Position.SquaredDistance(rOther.m_Position); + } + + public: + /** + * Assignment operator + */ + inline Pose2& operator = (const Pose2& rOther) + { + m_Position = rOther.m_Position; + m_Heading = rOther.m_Heading; + + return *this; + } + + /** + * Equality operator + */ + inline kt_bool operator == (const Pose2& rOther) const + { + return (m_Position == rOther.m_Position && m_Heading == rOther.m_Heading); + } + + /** + * Inequality operator + */ + inline kt_bool operator != (const Pose2& rOther) const + { + return (m_Position != rOther.m_Position || m_Heading != rOther.m_Heading); + } + + /** + * In place Pose2 add. + */ + inline void operator += (const Pose2& rOther) + { + m_Position += rOther.m_Position; + m_Heading = math::NormalizeAngle(m_Heading + rOther.m_Heading); + } + + /** + * Binary Pose2 add + * @param rOther + * @return Pose2 sum + */ + inline Pose2 operator + (const Pose2& rOther) const + { + return Pose2(m_Position + rOther.m_Position, math::NormalizeAngle(m_Heading + rOther.m_Heading)); + } + + /** + * Binary Pose2 subtract + * @param rOther + * @return Pose2 difference + */ + inline Pose2 operator - (const Pose2& rOther) const + { + return Pose2(m_Position - rOther.m_Position, math::NormalizeAngle(m_Heading - rOther.m_Heading)); + } + + /** + * Read pose from input stream + * @param rStream input stream + */ + friend inline std::istream& operator >> (std::istream& rStream, const Pose2& /*rPose*/) + { + // Implement me!! + return rStream; + } + + /** + * Write this pose onto output stream + * @param rStream output stream + * @param rPose to read + */ + friend inline std::ostream& operator << (std::ostream& rStream, const Pose2& rPose) + { + rStream << rPose.m_Position.GetX() << " " << rPose.m_Position.GetY() << " " << rPose.m_Heading; + return rStream; + } + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Position); + ar & BOOST_SERIALIZATION_NVP(m_Heading); + } + + private: + Vector2 m_Position; + + kt_double m_Heading; + }; // Pose2 + + /** + * Type declaration of Pose2 vector + */ + typedef std::vector< Pose2 > Pose2Vector; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Defines a position and orientation in 3-dimensional space. + * Karto uses a right-handed coordinate system with X, Y as the 2-D ground plane and X is forward and Y is left. + * Values in Vector3 used to define position must have units of meters. + * The value of angle when defining orientation in two dimensions must be in units of radians. + * The definition of orientation in three dimensions uses quaternions. + */ + class Pose3 + { + public: + /** + * Default constructor + */ + Pose3() + { + } + + /** + * Create a new Pose3 object from the given position. + * @param rPosition position vector in three space. + */ + Pose3(const Vector3& rPosition) + : m_Position(rPosition) + { + } + + /** + * Create a new Pose3 object from the given position and orientation. + * @param rPosition position vector in three space. + * @param rOrientation quaternion orientation in three space. + */ + Pose3(const Vector3& rPosition, const karto::Quaternion& rOrientation) + : m_Position(rPosition) + , m_Orientation(rOrientation) + { + } + + /** + * Copy constructor + */ + Pose3(const Pose3& rOther) + : m_Position(rOther.m_Position) + , m_Orientation(rOther.m_Orientation) + { + } + + /** + * Constructs a Pose3 object from a Pose2. + */ + Pose3(const Pose2& rPose) + { + m_Position = Vector3(rPose.GetX(), rPose.GetY(), 0.0); + m_Orientation.FromEulerAngles(rPose.GetHeading(), 0.0, 0.0); + } + + public: + /** + * Get the position of the pose as a 3D vector as const. Values have units of meters. + * @return 3-dimensional position vector as const + */ + inline const Vector3& GetPosition() const + { + return m_Position; + } + + /** + * Set the position of the pose as a 3D vector. Values have units of meters. + * @return 3-dimensional position vector + */ + inline void SetPosition(const Vector3& rPosition) + { + m_Position = rPosition; + } + + /** + * Get the orientation quaternion of the pose as const. + * @return orientation quaternion as const + */ + inline const Quaternion& GetOrientation() const + { + return m_Orientation; + } + + /** + * Get the orientation quaternion of the pose. + * @return orientation quaternion + */ + inline void SetOrientation(const Quaternion& rOrientation) + { + m_Orientation = rOrientation; + } + + /** + * Returns a string representation of this pose + * @return string representation of this pose + */ + inline std::string ToString() + { + std::stringstream converter; + converter.precision(std::numeric_limits::digits10); + + converter << GetPosition() << " " << GetOrientation(); + + return converter.str(); + } + + public: + /** + * Assignment operator + */ + inline Pose3& operator = (const Pose3& rOther) + { + m_Position = rOther.m_Position; + m_Orientation = rOther.m_Orientation; + + return *this; + } + + /** + * Equality operator + */ + inline kt_bool operator == (const Pose3& rOther) const + { + return (m_Position == rOther.m_Position && m_Orientation == rOther.m_Orientation); + } + + /** + * Inequality operator + */ + inline kt_bool operator != (const Pose3& rOther) const + { + return (m_Position != rOther.m_Position || m_Orientation != rOther.m_Orientation); + } + + /** + * Write Pose3 onto output stream + * @param rStream output stream + * @param rPose to write + */ + friend inline std::ostream& operator << (std::ostream& rStream, const Pose3& rPose) + { + rStream << rPose.GetPosition() << ", " << rPose.GetOrientation(); + return rStream; + } + + /** + * Read Pose3 from input stream + * @param rStream input stream + */ + friend inline std::istream& operator >> (std::istream& rStream, const Pose3& /*rPose*/) + { + // Implement me!! + return rStream; + } + + private: + Vector3 m_Position; + Quaternion m_Orientation; + }; // Pose3 + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Defines a Matrix 3 x 3 class. + */ + class Matrix3 + { + public: + /** + * Default constructor + */ + Matrix3() + { + Clear(); + } + + /** + * Copy constructor + */ + inline Matrix3(const Matrix3& rOther) + { + memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double)); + } + + public: + /** + * Sets this matrix to identity matrix + */ + void SetToIdentity() + { + memset(m_Matrix, 0, 9*sizeof(kt_double)); + + for (kt_int32s i = 0; i < 3; i++) + { + m_Matrix[i][i] = 1.0; + } + } + + /** + * Sets this matrix to zero matrix + */ + void Clear() + { + memset(m_Matrix, 0, 9*sizeof(kt_double)); + } + + /** + * Sets this matrix to be the rotation matrix of rotation around given axis + * @param x x-coordinate of axis + * @param y y-coordinate of axis + * @param z z-coordinate of axis + * @param radians amount of rotation + */ + void FromAxisAngle(kt_double x, kt_double y, kt_double z, const kt_double radians) + { + kt_double cosRadians = cos(radians); + kt_double sinRadians = sin(radians); + kt_double oneMinusCos = 1.0 - cosRadians; + + kt_double xx = x * x; + kt_double yy = y * y; + kt_double zz = z * z; + + kt_double xyMCos = x * y * oneMinusCos; + kt_double xzMCos = x * z * oneMinusCos; + kt_double yzMCos = y * z * oneMinusCos; + + kt_double xSin = x * sinRadians; + kt_double ySin = y * sinRadians; + kt_double zSin = z * sinRadians; + + m_Matrix[0][0] = xx * oneMinusCos + cosRadians; + m_Matrix[0][1] = xyMCos - zSin; + m_Matrix[0][2] = xzMCos + ySin; + + m_Matrix[1][0] = xyMCos + zSin; + m_Matrix[1][1] = yy * oneMinusCos + cosRadians; + m_Matrix[1][2] = yzMCos - xSin; + + m_Matrix[2][0] = xzMCos - ySin; + m_Matrix[2][1] = yzMCos + xSin; + m_Matrix[2][2] = zz * oneMinusCos + cosRadians; + } + + /** + * Returns transposed version of this matrix + * @return transposed matrix + */ + Matrix3 Transpose() const + { + Matrix3 transpose; + + for (kt_int32u row = 0; row < 3; row++) + { + for (kt_int32u col = 0; col < 3; col++) + { + transpose.m_Matrix[row][col] = m_Matrix[col][row]; + } + } + + return transpose; + } + + /** + * Returns the inverse of the matrix + */ + Matrix3 Inverse() const + { + Matrix3 kInverse = *this; + kt_bool haveInverse = InverseFast(kInverse, 1e-14); + if (haveInverse == false) + { + assert(false); + } + return kInverse; + } + + /** + * Internal helper method for inverse matrix calculation + * This code is lifted from the OgreMatrix3 class!! + */ + kt_bool InverseFast(Matrix3& rkInverse, kt_double fTolerance = KT_TOLERANCE) const + { + // Invert a 3x3 using cofactors. This is about 8 times faster than + // the Numerical Recipes code which uses Gaussian elimination. + rkInverse.m_Matrix[0][0] = m_Matrix[1][1]*m_Matrix[2][2] - m_Matrix[1][2]*m_Matrix[2][1]; + rkInverse.m_Matrix[0][1] = m_Matrix[0][2]*m_Matrix[2][1] - m_Matrix[0][1]*m_Matrix[2][2]; + rkInverse.m_Matrix[0][2] = m_Matrix[0][1]*m_Matrix[1][2] - m_Matrix[0][2]*m_Matrix[1][1]; + rkInverse.m_Matrix[1][0] = m_Matrix[1][2]*m_Matrix[2][0] - m_Matrix[1][0]*m_Matrix[2][2]; + rkInverse.m_Matrix[1][1] = m_Matrix[0][0]*m_Matrix[2][2] - m_Matrix[0][2]*m_Matrix[2][0]; + rkInverse.m_Matrix[1][2] = m_Matrix[0][2]*m_Matrix[1][0] - m_Matrix[0][0]*m_Matrix[1][2]; + rkInverse.m_Matrix[2][0] = m_Matrix[1][0]*m_Matrix[2][1] - m_Matrix[1][1]*m_Matrix[2][0]; + rkInverse.m_Matrix[2][1] = m_Matrix[0][1]*m_Matrix[2][0] - m_Matrix[0][0]*m_Matrix[2][1]; + rkInverse.m_Matrix[2][2] = m_Matrix[0][0]*m_Matrix[1][1] - m_Matrix[0][1]*m_Matrix[1][0]; + + kt_double fDet = m_Matrix[0][0]*rkInverse.m_Matrix[0][0] + + m_Matrix[0][1]*rkInverse.m_Matrix[1][0] + + m_Matrix[0][2]*rkInverse.m_Matrix[2][0]; + + if (fabs(fDet) <= fTolerance) + { + return false; + } + + kt_double fInvDet = 1.0/fDet; + for (size_t row = 0; row < 3; row++) + { + for (size_t col = 0; col < 3; col++) + { + rkInverse.m_Matrix[row][col] *= fInvDet; + } + } + + return true; + } + + /** + * Returns a string representation of this matrix + * @return string representation of this matrix + */ + inline std::string ToString() const + { + std::stringstream converter; + converter.precision(std::numeric_limits::digits10); + + for (int row = 0; row < 3; row++) + { + for (int col = 0; col < 3; col++) + { + converter << m_Matrix[row][col] << " "; + } + } + + return converter.str(); + } + + public: + /** + * Assignment operator + */ + inline Matrix3& operator = (const Matrix3& rOther) + { + memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double)); + return *this; + } + + /** + * Matrix element access, allows use of construct mat(r, c) + * @param row + * @param column + * @return reference to mat(r,c) + */ + inline kt_double& operator()(kt_int32u row, kt_int32u column) + { + return m_Matrix[row][column]; + } + + /** + * Read-only matrix element access, allows use of construct mat(r, c) + * @param row + * @param column + * @return mat(r,c) + */ + inline kt_double operator()(kt_int32u row, kt_int32u column) const + { + return m_Matrix[row][column]; + } + + /** + * Binary Matrix3 multiplication. + * @param rOther + * @return Matrix3 product + */ + Matrix3 operator * (const Matrix3& rOther) const + { + Matrix3 product; + + for (size_t row = 0; row < 3; row++) + { + for (size_t col = 0; col < 3; col++) + { + product.m_Matrix[row][col] = m_Matrix[row][0]*rOther.m_Matrix[0][col] + + m_Matrix[row][1]*rOther.m_Matrix[1][col] + + m_Matrix[row][2]*rOther.m_Matrix[2][col]; + } + } + + return product; + } + + /** + * Matrix3 and Pose2 multiplication - matrix * pose [3x3 * 3x1 = 3x1] + * @param rPose2 + * @return Pose2 product + */ + inline Pose2 operator * (const Pose2& rPose2) const + { + Pose2 pose2; + + pose2.SetX(m_Matrix[0][0] * rPose2.GetX() + m_Matrix[0][1] * + rPose2.GetY() + m_Matrix[0][2] * rPose2.GetHeading()); + pose2.SetY(m_Matrix[1][0] * rPose2.GetX() + m_Matrix[1][1] * + rPose2.GetY() + m_Matrix[1][2] * rPose2.GetHeading()); + pose2.SetHeading(m_Matrix[2][0] * rPose2.GetX() + m_Matrix[2][1] * + rPose2.GetY() + m_Matrix[2][2] * rPose2.GetHeading()); + + return pose2; + } + + /** + * In place Matrix3 add. + * @param rkMatrix + */ + inline void operator += (const Matrix3& rkMatrix) + { + for (kt_int32u row = 0; row < 3; row++) + { + for (kt_int32u col = 0; col < 3; col++) + { + m_Matrix[row][col] += rkMatrix.m_Matrix[row][col]; + } + } + } + + /** + * Write Matrix3 onto output stream + * @param rStream output stream + * @param rMatrix to write + */ + friend inline std::ostream& operator << (std::ostream& rStream, const Matrix3& rMatrix) + { + rStream << rMatrix.ToString(); + return rStream; + } + + private: + kt_double m_Matrix[3][3]; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Matrix); + } + }; // Matrix3 + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Defines a general Matrix class. + */ + class Matrix + { + public: + /** + * Constructs a matrix of size rows x columns + */ + Matrix(kt_int32u rows, kt_int32u columns) + : m_Rows(rows) + , m_Columns(columns) + , m_pData(NULL) + { + Allocate(); + + Clear(); + } + + /** + * Destructor + */ + virtual ~Matrix() + { + delete [] m_pData; + } + + public: + /** + * Set all entries to 0 + */ + void Clear() + { + if (m_pData != NULL) + { + memset(m_pData, 0, sizeof(kt_double) * m_Rows * m_Columns); + } + } + + /** + * Gets the number of rows of the matrix + * @return nubmer of rows + */ + inline kt_int32u GetRows() const + { + return m_Rows; + } + + /** + * Gets the number of columns of the matrix + * @return nubmer of columns + */ + inline kt_int32u GetColumns() const + { + return m_Columns; + } + + /** + * Returns a reference to the entry at (row,column) + * @param row + * @param column + * @return reference to entry at (row,column) + */ + inline kt_double& operator()(kt_int32u row, kt_int32u column) + { + RangeCheck(row, column); + + return m_pData[row + column * m_Rows]; + } + + /** + * Returns a const reference to the entry at (row,column) + * @param row + * @param column + * @return const reference to entry at (row,column) + */ + inline const kt_double& operator()(kt_int32u row, kt_int32u column) const + { + RangeCheck(row, column); + + return m_pData[row + column * m_Rows]; + } + + private: + /** + * Allocate space for the matrix + */ + void Allocate() + { + try + { + if (m_pData != NULL) + { + delete[] m_pData; + } + + m_pData = new kt_double[m_Rows * m_Columns]; + } + catch (const std::bad_alloc& ex) + { + throw Exception("Matrix allocation error"); + } + + if (m_pData == NULL) + { + throw Exception("Matrix allocation error"); + } + } + + /** + * Checks if (row,column) is a valid entry into the matrix + * @param row + * @param column + */ + inline void RangeCheck(kt_int32u row, kt_int32u column) const + { + if (math::IsUpTo(row, m_Rows) == false) + { + throw Exception("Matrix - RangeCheck ERROR!!!!"); + } + + if (math::IsUpTo(column, m_Columns) == false) + { + throw Exception("Matrix - RangeCheck ERROR!!!!"); + } + } + + private: + kt_int32u m_Rows; + kt_int32u m_Columns; + + kt_double* m_pData; + }; // Matrix + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Defines a bounding box in 2-dimensional real space. + */ + class BoundingBox2 + { + public: + /* + * Default constructor + */ + BoundingBox2() + : m_Minimum(999999999999999999.99999, 999999999999999999.99999) + , m_Maximum(-999999999999999999.99999, -999999999999999999.99999) + { + } + + public: + /** + * Get bounding box minimum + */ + inline const Vector2& GetMinimum() const + { + return m_Minimum; + } + + /** + * Set bounding box minimum + */ + inline void SetMinimum(const Vector2& mMinimum) + { + m_Minimum = mMinimum; + } + + /** + * Get bounding box maximum + */ + inline const Vector2& GetMaximum() const + { + return m_Maximum; + } + + /** + * Set bounding box maximum + */ + inline void SetMaximum(const Vector2& rMaximum) + { + m_Maximum = rMaximum; + } + + /** + * Get the size of the bounding box + */ + inline Size2 GetSize() const + { + Vector2 size = m_Maximum - m_Minimum; + + return Size2(size.GetX(), size.GetY()); + } + + /** + * Add vector to bounding box + */ + inline void Add(const Vector2& rPoint) + { + m_Minimum.MakeFloor(rPoint); + m_Maximum.MakeCeil(rPoint); + } + + /** + * Add other bounding box to bounding box + */ + inline void Add(const BoundingBox2& rBoundingBox) + { + Add(rBoundingBox.GetMinimum()); + Add(rBoundingBox.GetMaximum()); + } + + /** + * Whether the given point is in the bounds of this box + * @param rPoint + * @return in bounds? + */ + inline kt_bool IsInBounds(const Vector2& rPoint) const + { + return (math::InRange(rPoint.GetX(), m_Minimum.GetX(), m_Maximum.GetX()) && + math::InRange(rPoint.GetY(), m_Minimum.GetY(), m_Maximum.GetY())); + } + + /** + * Serialization: class BoundingBox2 + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Minimum); + ar & BOOST_SERIALIZATION_NVP(m_Maximum); + } + + private: + Vector2 m_Minimum; + Vector2 m_Maximum; + }; // BoundingBox2 + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Implementation of a Pose2 transform + */ + class Transform + { + public: + /** + * Constructs a transformation from the origin to the given pose + * @param rPose pose + */ + Transform(const Pose2& rPose) + { + SetTransform(Pose2(), rPose); + } + + /** + * Constructs a transformation from the first pose to the second pose + * @param rPose1 first pose + * @param rPose2 second pose + */ + Transform(const Pose2& rPose1, const Pose2& rPose2) + { + SetTransform(rPose1, rPose2); + } + + public: + /** + * Transforms the pose according to this transform + * @param rSourcePose pose to transform from + * @return transformed pose + */ + inline Pose2 TransformPose(const Pose2& rSourcePose) + { + Pose2 newPosition = m_Transform + m_Rotation * rSourcePose; + kt_double angle = math::NormalizeAngle(rSourcePose.GetHeading() + m_Transform.GetHeading()); + + return Pose2(newPosition.GetPosition(), angle); + } + + /** + * Inverse transformation of the pose according to this transform + * @param rSourcePose pose to transform from + * @return transformed pose + */ + inline Pose2 InverseTransformPose(const Pose2& rSourcePose) + { + Pose2 newPosition = m_InverseRotation * (rSourcePose - m_Transform); + kt_double angle = math::NormalizeAngle(rSourcePose.GetHeading() - m_Transform.GetHeading()); + + // components of transform + return Pose2(newPosition.GetPosition(), angle); + } + + private: + /** + * Sets this to be the transformation from the first pose to the second pose + * @param rPose1 first pose + * @param rPose2 second pose + */ + void SetTransform(const Pose2& rPose1, const Pose2& rPose2) + { + if (rPose1 == rPose2) + { + m_Rotation.SetToIdentity(); + m_InverseRotation.SetToIdentity(); + m_Transform = Pose2(); + return; + } + + // heading transformation + m_Rotation.FromAxisAngle(0, 0, 1, rPose2.GetHeading() - rPose1.GetHeading()); + m_InverseRotation.FromAxisAngle(0, 0, 1, rPose1.GetHeading() - rPose2.GetHeading()); + + // position transformation + Pose2 newPosition; + if (rPose1.GetX() != 0.0 || rPose1.GetY() != 0.0) + { + newPosition = rPose2 - m_Rotation * rPose1; + } + else + { + newPosition = rPose2; + } + + m_Transform = Pose2(newPosition.GetPosition(), rPose2.GetHeading() - rPose1.GetHeading()); + } + + private: + // pose transformation + Pose2 m_Transform; + + Matrix3 m_Rotation; + Matrix3 m_InverseRotation; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Transform); + ar & BOOST_SERIALIZATION_NVP(m_Rotation); + ar & BOOST_SERIALIZATION_NVP(m_InverseRotation); + } + }; // Transform + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Enumerated type for valid LaserRangeFinder types + */ + typedef enum + { + LaserRangeFinder_Custom = 0, + + LaserRangeFinder_Sick_LMS100 = 1, + LaserRangeFinder_Sick_LMS200 = 2, + LaserRangeFinder_Sick_LMS291 = 3, + + LaserRangeFinder_Hokuyo_UTM_30LX = 4, + LaserRangeFinder_Hokuyo_URG_04LX = 5 + } LaserRangeFinderType; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Abstract base class for Parameters + */ + class AbstractParameter + { + + public: + AbstractParameter() + { + } + /** + * Constructs a parameter with the given name + * @param rName + * @param pParameterManger + */ + AbstractParameter(const std::string& rName, ParameterManager* pParameterManger = NULL) + : m_Name(rName) + { + // if parameter manager is provided add myself to it! + if (pParameterManger != NULL) + { + pParameterManger->Add(this); + } + } + + /** + * Constructs a parameter with the given name and description + * @param rName + * @param rDescription + * @param pParameterManger + */ + AbstractParameter(const std::string& rName, + const std::string& rDescription, + ParameterManager* pParameterManger = NULL) + : m_Name(rName) + , m_Description(rDescription) + { + // if parameter manager is provided add myself to it! + if (pParameterManger != NULL) + { + pParameterManger->Add(this); + } + } + + /** + * Destructor + */ + virtual ~AbstractParameter() + { + } + + public: + /** + * Gets the name of this object + * @return name + */ + inline const std::string& GetName() const + { + return m_Name; + } + + /** + * Returns the parameter description + * @return parameter description + */ + inline const std::string& GetDescription() const + { + return m_Description; + } + + /** + * Get parameter value as string. + * @return value as string + */ + virtual const std::string GetValueAsString() const = 0; + + /** + * Set parameter value from string. + * @param rStringValue value as string + */ + virtual void SetValueFromString(const std::string& rStringValue) = 0; + + /** + * Clones the parameter + * @return clone + */ + virtual AbstractParameter* Clone() = 0; + + public: + /** + * Write this parameter onto output stream + * @param rStream output stream + * @param rParameter + */ + friend std::ostream& operator << (std::ostream& rStream, const AbstractParameter& rParameter) + { + rStream.precision(6); + rStream.flags(std::ios::fixed); + + rStream << rParameter.GetName() << " = " << rParameter.GetValueAsString(); + return rStream; + } + + private: + std::string m_Name; + std::string m_Description; + /** + * Serialization: class AbstractParameter + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Name); + ar & BOOST_SERIALIZATION_NVP(m_Description); + } + }; // AbstractParameter + BOOST_SERIALIZATION_ASSUME_ABSTRACT(AbstractParameter) + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Parameter class + */ + template + class Parameter : public AbstractParameter + { + public: + /** + * Parameter with given name and value + * @param rName + * @param value + * @param pParameterManger + */ + Parameter() + { + } + Parameter(const std::string& rName, T value, ParameterManager* pParameterManger = NULL) + : AbstractParameter(rName, pParameterManger) + , m_Value(value) + { + } + + /** + * Parameter with given name, description and value + * @param rName + * @param rDescription + * @param value + * @param pParameterManger + */ + Parameter(const std::string& rName, + const std::string& rDescription, + T value, + ParameterManager* pParameterManger = NULL) + : AbstractParameter(rName, rDescription, pParameterManger) + , m_Value(value) + { + } + + /** + * Destructor + */ + virtual ~Parameter() + { + } + + public: + /** + * Gets value of parameter + * @return parameter value + */ + inline const T& GetValue() const + { + return m_Value; + } + + /** + * Sets value of parameter + * @param rValue + */ + inline void SetValue(const T& rValue) + { + m_Value = rValue; + } + + /** + * Gets value of parameter as string + * @return string version of value + */ + virtual const std::string GetValueAsString() const + { + std::stringstream converter; + converter << m_Value; + return converter.str(); + } + + /** + * Sets value of parameter from string + * @param rStringValue + */ + virtual void SetValueFromString(const std::string& rStringValue) + { + std::stringstream converter; + converter.str(rStringValue); + converter >> m_Value; + } + + /** + * Clone this parameter + * @return clone of this parameter + */ + virtual Parameter* Clone() + { + return new Parameter(GetName(), GetDescription(), GetValue()); + } + + public: + /** + * Assignment operator + */ + Parameter& operator = (const Parameter& rOther) + { + m_Value = rOther.m_Value; + + return *this; + } + + /** + * Sets the value of this parameter to given value + */ + T operator = (T value) + { + m_Value = value; + + return m_Value; + } + + /** + * Serialization: class Parameter + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(AbstractParameter); + ar & BOOST_SERIALIZATION_NVP(m_Value); + } + + protected: + /** + * Parameter value + */ + T m_Value; + }; // Parameter + BOOST_SERIALIZATION_ASSUME_ABSTRACT(Parameter) + + template<> + inline void Parameter::SetValueFromString(const std::string& rStringValue) + { + int precision = std::numeric_limits::digits10; + std::stringstream converter; + converter.precision(precision); + + converter.str(rStringValue); + + m_Value = 0.0; + converter >> m_Value; + } + + template<> + inline const std::string Parameter::GetValueAsString() const + { + std::stringstream converter; + converter.precision(std::numeric_limits::digits10); + converter << m_Value; + return converter.str(); + } + + template<> + inline void Parameter::SetValueFromString(const std::string& rStringValue) + { + if (rStringValue == "true" || rStringValue == "TRUE") + { + m_Value = true; + } + else + { + m_Value = false; + } + } + + template<> + inline const std::string Parameter::GetValueAsString() const + { + if (m_Value == true) + { + return "true"; + } + + return "false"; + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Parameter enum class + */ + class ParameterEnum : public Parameter + { + typedef std::map EnumMap; + + public: + /** + * Construct a Parameter object with name and value + * @param rName parameter name + * @param value of parameter + * @param pParameterManger + */ + ParameterEnum(const std::string& rName, kt_int32s value, ParameterManager* pParameterManger = NULL) + : Parameter(rName, value, pParameterManger) + { + } + ParameterEnum() + { + } + + /** + * Destructor + */ + virtual ~ParameterEnum() + { + } + + public: + /** + * Return a clone of this instance + * @return clone + */ + virtual Parameter* Clone() + { + ParameterEnum* pEnum = new ParameterEnum(GetName(), GetValue()); + + pEnum->m_EnumDefines = m_EnumDefines; + + return pEnum; + } + + /** + * Set parameter value from string. + * @param rStringValue value as string + */ + virtual void SetValueFromString(const std::string& rStringValue) + { + if (m_EnumDefines.find(rStringValue) != m_EnumDefines.end()) + { + m_Value = m_EnumDefines[rStringValue]; + } + else + { + std::string validValues; + + const_forEach(EnumMap, &m_EnumDefines) + { + validValues += iter->first + ", "; + } + + throw Exception("Unable to set enum: " + rStringValue + ". Valid values are: " + validValues); + } + } + + /** + * Get parameter value as string. + * @return value as string + */ + virtual const std::string GetValueAsString() const + { + const_forEach(EnumMap, &m_EnumDefines) + { + if (iter->second == m_Value) + { + return iter->first; + } + } + + throw Exception("Unable to lookup enum"); + } + + /** + * Defines the enum with the given name as having the given value + * @param value + * @param rName + */ + void DefineEnumValue(kt_int32s value, const std::string& rName) + { + if (m_EnumDefines.find(rName) == m_EnumDefines.end()) + { + m_EnumDefines[rName] = value; + } + else + { + std::cerr << "Overriding enum value: " << m_EnumDefines[rName] << " with " << value << std::endl; + + m_EnumDefines[rName] = value; + + assert(false); + } + } + + public: + /** + * Assignment operator + */ + ParameterEnum& operator = (const ParameterEnum& rOther) + { + SetValue(rOther.GetValue()); + + return *this; + } + + /** + * Assignment operator + */ + kt_int32s operator = (kt_int32s value) + { + SetValue(value); + + return m_Value; + } + + private: + EnumMap m_EnumDefines; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Parameter); + ar & BOOST_SERIALIZATION_NVP(m_EnumDefines); + } + }; // ParameterEnum + BOOST_SERIALIZATION_ASSUME_ABSTRACT(ParameterEnum) + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Set of parameters + */ + class Parameters : public Object + { + public: + // @cond EXCLUDE + KARTO_Object(Parameters) + // @endcond + + public: + /** + * Parameters + * @param rName + */ + Parameters() + { + } + Parameters(const std::string& rName) + : Object(rName) + { + } + + /** + * Destructor + */ + virtual ~Parameters() + { + } + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object); + } + + private: + Parameters(const Parameters&); + const Parameters& operator=(const Parameters&); + }; // Parameters + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + class SensorData; + + /** + * Abstract Sensor base class + */ + class KARTO_EXPORT Sensor : public Object + { + + /** + * Serialization: class Sensor + */ + public: + Sensor() + { + } + // @cond EXCLUDE + KARTO_Object(Sensor) + // @endcond + + protected: + /** + * Construct a Sensor + * @param rName sensor name + */ + Sensor(const Name& rName); + + public: + /** + * Destructor + */ + virtual ~Sensor(); + + public: + /** + * Gets this range finder sensor's offset + * @return offset pose + */ + inline const Pose2& GetOffsetPose() const + { + return m_pOffsetPose->GetValue(); + } + + /** + * Sets this range finder sensor's offset + * @param rPose + */ + inline void SetOffsetPose(const Pose2& rPose) + { + m_pOffsetPose->SetValue(rPose); + } + + /** + * Validates sensor + * @return true if valid + */ + virtual kt_bool Validate() = 0; + + /** + * Validates sensor data + * @param pSensorData sensor data + * @return true if valid + */ + virtual kt_bool Validate(SensorData* pSensorData) = 0; + + private: + /** + * Restrict the copy constructor + */ + Sensor(const Sensor&); + + /** + * Restrict the assignment operator + */ + const Sensor& operator=(const Sensor&); + + private: + /** + * Sensor offset pose + */ + Parameter* m_pOffsetPose; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object); + ar & BOOST_SERIALIZATION_NVP(m_pOffsetPose); + } + }; // Sensor + BOOST_SERIALIZATION_ASSUME_ABSTRACT(Sensor) + /** + * Type declaration of Sensor vector + */ + typedef std::vector SensorVector; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Type declaration of map + */ + typedef std::map SensorManagerMap; + + /** + * Manages sensors + */ + class KARTO_EXPORT SensorManager + { + public: + /** + * Constructor + */ + SensorManager() + { + } + + /** + * Destructor + */ + virtual ~SensorManager() + { + } + + public: + /** + * Get singleton instance of SensorManager + */ + static SensorManager* GetInstance(); + + public: + /** + * Registers a sensor by it's name. The Sensor name must be unique, if not sensor is not registered + * unless override is set to true + * @param pSensor sensor to register + * @param override + * @return true if sensor is registered with SensorManager, false if Sensor name is not unique + */ + void RegisterSensor(Sensor* pSensor, kt_bool override = false) + { + Validate(pSensor); + + if ((m_Sensors.find(pSensor->GetName()) != m_Sensors.end()) && !override) + { + throw Exception("Cannot register sensor: already registered: [" + + pSensor->GetName().ToString() + + "] (Consider setting 'override' to true)"); + } + + std::cout << "Registering sensor: [" << pSensor->GetName().ToString() << "]" << std::endl; + + m_Sensors[pSensor->GetName()] = pSensor; + } + + /** + * Unregisters the given sensor + * @param pSensor sensor to unregister + */ + void UnregisterSensor(Sensor* pSensor) + { + Validate(pSensor); + + if (m_Sensors.find(pSensor->GetName()) != m_Sensors.end()) + { + std::cout << "Unregistering sensor: " << pSensor->GetName().ToString() << std::endl; + + m_Sensors.erase(pSensor->GetName()); + } + else + { + throw Exception("Cannot unregister sensor: not registered: [" + pSensor->GetName().ToString() + "]"); + } + } + + /** + * Gets the sensor with the given name + * @param rName name of sensor + * @return sensor + */ + Sensor* GetSensorByName(const Name& rName) + { + if (m_Sensors.find(rName) != m_Sensors.end()) + { + return m_Sensors[rName]; + } + + throw Exception("Sensor not registered: [" + rName.ToString() + "] (Did you add the sensor to the Dataset?)"); + } + + /** + * Gets the sensor with the given name + * @param rName name of sensor + * @return sensor + */ + template + T* GetSensorByName(const Name& rName) + { + Sensor* pSensor = GetSensorByName(rName); + + return dynamic_cast(pSensor); + } + + /** + * Gets all registered sensors + * @return vector of all registered sensors + */ + SensorVector GetAllSensors() + { + SensorVector sensors; + + forEach(SensorManagerMap, &m_Sensors) + { + sensors.push_back(iter->second); + } + + return sensors; + } + + protected: + /** + * Checks that given sensor is not NULL and has non-empty name + * @param pSensor sensor to validate + */ + static void Validate(Sensor* pSensor) + { + if (pSensor == NULL) + { + throw Exception("Invalid sensor: NULL"); + } + else if (pSensor->GetName().ToString() == "") + { + throw Exception("Invalid sensor: nameless"); + } + } + + protected: + /** + * Sensor map + */ + SensorManagerMap m_Sensors; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Sensors); + } + }; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Sensor that provides pose information relative to world coordinates. + * + * The user can set the offset pose of the drive sensor. If no value is provided by the user the default is no offset, + * i.e, the sensor is initially at the world origin, oriented along the positive z axis. + */ + class Drive : public Sensor + { + public: + // @cond EXCLUDE + KARTO_Object(Drive) + // @endcond + + public: + /** + * Constructs a Drive object + */ + Drive(const std::string& rName) + : Sensor(rName) + { + } + /** + * Destructor + */ + virtual ~Drive() + { + } + + public: + virtual kt_bool Validate() + { + return true; + } + + virtual kt_bool Validate(SensorData* pSensorData) + { + if (pSensorData == NULL) + { + return false; + } + + return true; + } + + private: + Drive(const Drive&); + const Drive& operator=(const Drive&); + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Sensor); + } + }; // class Drive + + BOOST_SERIALIZATION_ASSUME_ABSTRACT(Drive) + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + class LocalizedRangeScan; + class CoordinateConverter; + + /** + * The LaserRangeFinder defines a laser sensor that provides the pose offset position of a localized range scan relative to the robot. + * The user can set an offset pose for the sensor relative to the robot coordinate system. If no value is provided + * by the user, the sensor is set to be at the origin of the robot coordinate system. + * The LaserRangeFinder contains parameters for physical laser sensor used by the mapper for scan matching + * Also contains information about the maximum range of the sensor and provides a threshold + * for limiting the range of readings. + * The optimal value for the range threshold depends on the angular resolution of the scan and + * the desired map resolution. RangeThreshold should be set as large as possible while still + * providing "solid" coverage between consecutive range readings. The diagram below illustrates + * the relationship between map resolution and the range threshold. + */ + class KARTO_EXPORT LaserRangeFinder : public Sensor + { + public: + LaserRangeFinder() + { + } + // @cond EXCLUDE + KARTO_Object(LaserRangeFinder) + // @endcond + + public: + /** + * Destructor + */ + virtual ~LaserRangeFinder() + { + } + + public: + /** + * Gets this range finder sensor's minimum range + * @return minimum range + */ + inline kt_double GetMinimumRange() const + { + return m_pMinimumRange->GetValue(); + } + + /** + * Sets this range finder sensor's minimum range + * @param minimumRange + */ + inline void SetMinimumRange(kt_double minimumRange) + { + m_pMinimumRange->SetValue(minimumRange); + + SetRangeThreshold(GetRangeThreshold()); + } + + /** + * Gets this range finder sensor's maximum range + * @return maximum range + */ + inline kt_double GetMaximumRange() const + { + return m_pMaximumRange->GetValue(); + } + + /** + * Sets this range finder sensor's maximum range + * @param maximumRange + */ + inline void SetMaximumRange(kt_double maximumRange) + { + m_pMaximumRange->SetValue(maximumRange); + + SetRangeThreshold(GetRangeThreshold()); + } + + /** + * Gets the range threshold + * @return range threshold + */ + inline kt_double GetRangeThreshold() const + { + return m_pRangeThreshold->GetValue(); + } + + /** + * Sets the range threshold + * @param rangeThreshold + */ + inline void SetRangeThreshold(kt_double rangeThreshold) + { + // make sure rangeThreshold is within laser range finder range + m_pRangeThreshold->SetValue(math::Clip(rangeThreshold, GetMinimumRange(), GetMaximumRange())); + + if (math::DoubleEqual(GetRangeThreshold(), rangeThreshold) == false) + { + std::cout << "Info: clipped range threshold to be within minimum and maximum range!" << std::endl; + } + } + + /** + * Gets this range finder sensor's minimum angle + * @return minimum angle + */ + inline kt_double GetMinimumAngle() const + { + return m_pMinimumAngle->GetValue(); + } + + /** + * Sets this range finder sensor's minimum angle + * @param minimumAngle + */ + inline void SetMinimumAngle(kt_double minimumAngle) + { + m_pMinimumAngle->SetValue(minimumAngle); + + Update(); + } + + /** + * Gets this range finder sensor's maximum angle + * @return maximum angle + */ + inline kt_double GetMaximumAngle() const + { + return m_pMaximumAngle->GetValue(); + } + + /** + * Sets this range finder sensor's maximum angle + * @param maximumAngle + */ + inline void SetMaximumAngle(kt_double maximumAngle) + { + m_pMaximumAngle->SetValue(maximumAngle); + + Update(); + } + + /** + * Gets this range finder sensor's angular resolution + * @return angular resolution + */ + inline kt_double GetAngularResolution() const + { + return m_pAngularResolution->GetValue(); + } + + /** + * Sets this range finder sensor's angular resolution + * @param angularResolution + */ + inline void SetAngularResolution(kt_double angularResolution) + { + if (m_pType->GetValue() == LaserRangeFinder_Custom) + { + m_pAngularResolution->SetValue(angularResolution); + } + else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS100) + { + if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25))) + { + m_pAngularResolution->SetValue(math::DegreesToRadians(0.25)); + } + else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50))) + { + m_pAngularResolution->SetValue(math::DegreesToRadians(0.50)); + } + else + { + std::stringstream stream; + stream << "Invalid resolution for Sick LMS100: "; + stream << angularResolution; + throw Exception(stream.str()); + } + } + else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS200 || + m_pType->GetValue() == LaserRangeFinder_Sick_LMS291) + { + if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25))) + { + m_pAngularResolution->SetValue(math::DegreesToRadians(0.25)); + } + else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50))) + { + m_pAngularResolution->SetValue(math::DegreesToRadians(0.50)); + } + else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(1.00))) + { + m_pAngularResolution->SetValue(math::DegreesToRadians(1.00)); + } + else + { + std::stringstream stream; + stream << "Invalid resolution for Sick LMS291: "; + stream << angularResolution; + throw Exception(stream.str()); + } + } + else + { + throw Exception("Can't set angular resolution, please create a LaserRangeFinder of type Custom"); + } + + Update(); + } + + /** + * Return Laser type + */ + inline kt_int32s GetType() + { + return m_pType->GetValue(); + } + + /** + * Gets the number of range readings each localized range scan must contain to be a valid scan. + * @return number of range readings + */ + inline kt_int32u GetNumberOfRangeReadings() const + { + return m_NumberOfRangeReadings; + } + + virtual kt_bool Validate() + { + Update(); + + if (math::InRange(GetRangeThreshold(), GetMinimumRange(), GetMaximumRange()) == false) + { + std::cout << "Please set range threshold to a value between [" + << GetMinimumRange() << ";" << GetMaximumRange() << "]" << std::endl; + return false; + } + + return true; + } + + virtual kt_bool Validate(SensorData* pSensorData); + + /** + * Get point readings (potentially scale readings if given coordinate converter is not null) + * @param pLocalizedRangeScan + * @param pCoordinateConverter + * @param ignoreThresholdPoints + * @param flipY + */ + const PointVectorDouble GetPointReadings(LocalizedRangeScan* pLocalizedRangeScan, + CoordinateConverter* pCoordinateConverter, + kt_bool ignoreThresholdPoints = true, + kt_bool flipY = false) const; + + public: + /** + * Create a laser range finder of the given type and ID + * @param type + * @param rName name of sensor - if no name is specified default name will be assigned + * @return laser range finder + */ + static LaserRangeFinder* CreateLaserRangeFinder(LaserRangeFinderType type, const Name& rName) + { + LaserRangeFinder* pLrf = NULL; + + switch (type) + { + // see http://www.hizook.com/files/publications/SICK_LMS100.pdf + // set range threshold to 18m + case LaserRangeFinder_Sick_LMS100: + { + pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 100")); + + // Sensing range is 18 meters (at 10% reflectivity, max range of 20 meters), with an error of about 20mm + pLrf->m_pMinimumRange->SetValue(0.0); + pLrf->m_pMaximumRange->SetValue(20.0); + + // 270 degree range, 50 Hz + pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-135)); + pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(135)); + + // 0.25 degree angular resolution + pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.25)); + + pLrf->m_NumberOfRangeReadings = 1081; + } + break; + + // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf + // set range threshold to 10m + case LaserRangeFinder_Sick_LMS200: + { + pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 200")); + + // Sensing range is 80 meters + pLrf->m_pMinimumRange->SetValue(0.0); + pLrf->m_pMaximumRange->SetValue(80.0); + + // 180 degree range, 75 Hz + pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90)); + pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90)); + + // 0.5 degree angular resolution + pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.5)); + + pLrf->m_NumberOfRangeReadings = 361; + } + break; + + // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf + // set range threshold to 30m + case LaserRangeFinder_Sick_LMS291: + { + pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 291")); + + // Sensing range is 80 meters + pLrf->m_pMinimumRange->SetValue(0.0); + pLrf->m_pMaximumRange->SetValue(80.0); + + // 180 degree range, 75 Hz + pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90)); + pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90)); + + // 0.5 degree angular resolution + pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.5)); + + pLrf->m_NumberOfRangeReadings = 361; + } + break; + + // see http://www.hizook.com/files/publications/Hokuyo_UTM_LaserRangeFinder_LIDAR.pdf + // set range threshold to 30m + case LaserRangeFinder_Hokuyo_UTM_30LX: + { + pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Hokuyo UTM-30LX")); + + // Sensing range is 30 meters + pLrf->m_pMinimumRange->SetValue(0.1); + pLrf->m_pMaximumRange->SetValue(30.0); + + // 270 degree range, 40 Hz + pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-135)); + pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(135)); + + // 0.25 degree angular resolution + pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.25)); + + pLrf->m_NumberOfRangeReadings = 1081; + } + break; + + // see http://www.hizook.com/files/publications/HokuyoURG_Datasheet.pdf + // set range threshold to 3.5m + case LaserRangeFinder_Hokuyo_URG_04LX: + { + pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Hokuyo URG-04LX")); + + // Sensing range is 4 meters. It has detection problems when + // scanning absorptive surfaces (such as black trimming). + pLrf->m_pMinimumRange->SetValue(0.02); + pLrf->m_pMaximumRange->SetValue(4.0); + + // 240 degree range, 10 Hz + pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-120)); + pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(120)); + + // 0.352 degree angular resolution + pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.352)); + + pLrf->m_NumberOfRangeReadings = 751; + } + break; + + case LaserRangeFinder_Custom: + { + pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("User-Defined LaserRangeFinder")); + + // Sensing range is 80 meters. + pLrf->m_pMinimumRange->SetValue(0.0); + pLrf->m_pMaximumRange->SetValue(80.0); + + // 180 degree range + pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90)); + pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90)); + + // 1.0 degree angular resolution + pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(1.0)); + + pLrf->m_NumberOfRangeReadings = 181; + } + break; + } + + if (pLrf != NULL) + { + pLrf->m_pType->SetValue(type); + + Pose2 defaultOffset; + pLrf->SetOffsetPose(defaultOffset); + } + + return pLrf; + } + + private: + /** + * Constructs a LaserRangeFinder object with given ID + */ + LaserRangeFinder(const Name& rName) + : Sensor(rName) + , m_NumberOfRangeReadings(0) + { + m_pMinimumRange = new Parameter("MinimumRange", 0.0, GetParameterManager()); + m_pMaximumRange = new Parameter("MaximumRange", 80.0, GetParameterManager()); + + m_pMinimumAngle = new Parameter("MinimumAngle", -KT_PI_2, GetParameterManager()); + m_pMaximumAngle = new Parameter("MaximumAngle", KT_PI_2, GetParameterManager()); + + m_pAngularResolution = new Parameter("AngularResolution", + math::DegreesToRadians(1), + GetParameterManager()); + + m_pRangeThreshold = new Parameter("RangeThreshold", 12.0, GetParameterManager()); + + m_pType = new ParameterEnum("Type", LaserRangeFinder_Custom, GetParameterManager()); + m_pType->DefineEnumValue(LaserRangeFinder_Custom, "Custom"); + m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS100, "Sick_LMS100"); + m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS200, "Sick_LMS200"); + m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS291, "Sick_LMS291"); + m_pType->DefineEnumValue(LaserRangeFinder_Hokuyo_UTM_30LX, "Hokuyo_UTM_30LX"); + m_pType->DefineEnumValue(LaserRangeFinder_Hokuyo_URG_04LX, "Hokuyo_URG_04LX"); + } + + /** + * Set the number of range readings based on the minimum and + * maximum angles of the sensor and the angular resolution + */ + void Update() + { + m_NumberOfRangeReadings = static_cast(math::Round((GetMaximumAngle() - + GetMinimumAngle()) + / GetAngularResolution()) + 1); + } + + private: + LaserRangeFinder(const LaserRangeFinder&); + const LaserRangeFinder& operator=(const LaserRangeFinder&); + + private: + // sensor m_Parameters + Parameter* m_pMinimumAngle; + Parameter* m_pMaximumAngle; + + Parameter* m_pAngularResolution; + + Parameter* m_pMinimumRange; + Parameter* m_pMaximumRange; + + Parameter* m_pRangeThreshold; + + ParameterEnum* m_pType; + + kt_int32u m_NumberOfRangeReadings; + + // static std::string LaserRangeFinderTypeNames[6]; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + if (Archive::is_loading::value) + { + m_pMinimumRange = new Parameter("MinimumRange", 0.0, GetParameterManager()); + m_pMaximumRange = new Parameter("MaximumRange", 80.0, GetParameterManager()); + + m_pMinimumAngle = new Parameter("MinimumAngle", -KT_PI_2, GetParameterManager()); + m_pMaximumAngle = new Parameter("MaximumAngle", KT_PI_2, GetParameterManager()); + + m_pAngularResolution = new Parameter("AngularResolution", + math::DegreesToRadians(1), + GetParameterManager()); + + m_pRangeThreshold = new Parameter("RangeThreshold", 12.0, GetParameterManager()); + + m_pType = new ParameterEnum("Type", LaserRangeFinder_Custom, GetParameterManager()); + } + + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Sensor); + ar & BOOST_SERIALIZATION_NVP(m_pMinimumAngle); + ar & BOOST_SERIALIZATION_NVP(m_pMaximumAngle); + ar & BOOST_SERIALIZATION_NVP(m_pAngularResolution); + ar & BOOST_SERIALIZATION_NVP(m_pMinimumRange); + ar & BOOST_SERIALIZATION_NVP(m_pMaximumRange); + ar & BOOST_SERIALIZATION_NVP(m_pRangeThreshold); + ar & BOOST_SERIALIZATION_NVP(m_pType); + ar & BOOST_SERIALIZATION_NVP(m_NumberOfRangeReadings); + } + }; // LaserRangeFinder + BOOST_SERIALIZATION_ASSUME_ABSTRACT(LaserRangeFinder) + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Enumerated type for valid grid cell states + */ + typedef enum + { + GridStates_Unknown = 0, + GridStates_Occupied = 100, + GridStates_Free = 255 + } GridStates; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * The CoordinateConverter class is used to convert coordinates between world and grid coordinates + * In world coordinates 1.0 = 1 meter where 1 in grid coordinates = 1 pixel! + * Default scale for coordinate converter is 20 that converters to 1 pixel = 0.05 meter + */ + class CoordinateConverter + { + public: + /** + * Default constructor + */ + CoordinateConverter() + : m_Scale(20.0) + { + } + + public: + /** + * Scales the value + * @param value + * @return scaled value + */ + inline kt_double Transform(kt_double value) + { + return value * m_Scale; + } + + /** + * Converts the point from world coordinates to grid coordinates + * @param rWorld world coordinate + * @param flipY + * @return grid coordinate + */ + inline Vector2 WorldToGrid(const Vector2& rWorld, kt_bool flipY = false) const + { + kt_double gridX = (rWorld.GetX() - m_Offset.GetX()) * m_Scale; + kt_double gridY = 0.0; + + if (flipY == false) + { + gridY = (rWorld.GetY() - m_Offset.GetY()) * m_Scale; + } + else + { + gridY = (m_Size.GetHeight() / m_Scale - rWorld.GetY() + m_Offset.GetY()) * m_Scale; + } + + return Vector2(static_cast(math::Round(gridX)), static_cast(math::Round(gridY))); + } + + /** + * Converts the point from grid coordinates to world coordinates + * @param rGrid world coordinate + * @param flipY + * @return world coordinate + */ + inline Vector2 GridToWorld(const Vector2& rGrid, kt_bool flipY = false) const + { + kt_double worldX = m_Offset.GetX() + rGrid.GetX() / m_Scale; + kt_double worldY = 0.0; + + if (flipY == false) + { + worldY = m_Offset.GetY() + rGrid.GetY() / m_Scale; + } + else + { + worldY = m_Offset.GetY() + (m_Size.GetHeight() - rGrid.GetY()) / m_Scale; + } + + return Vector2(worldX, worldY); + } + + /** + * Gets the scale + * @return scale + */ + inline kt_double GetScale() const + { + return m_Scale; + } + + /** + * Sets the scale + * @param scale + */ + inline void SetScale(kt_double scale) + { + m_Scale = scale; + } + + /** + * Gets the offset + * @return offset + */ + inline const Vector2& GetOffset() const + { + return m_Offset; + } + + /** + * Sets the offset + * @param rOffset + */ + inline void SetOffset(const Vector2& rOffset) + { + m_Offset = rOffset; + } + + /** + * Sets the size + * @param rSize + */ + inline void SetSize(const Size2& rSize) + { + m_Size = rSize; + } + + /** + * Gets the size + * @return size + */ + inline const Size2& GetSize() const + { + return m_Size; + } + + /** + * Gets the resolution + * @return resolution + */ + inline kt_double GetResolution() const + { + return 1.0 / m_Scale; + } + + /** + * Sets the resolution + * @param resolution + */ + inline void SetResolution(kt_double resolution) + { + m_Scale = 1.0 / resolution; + } + + /** + * Gets the bounding box + * @return bounding box + */ + inline BoundingBox2 GetBoundingBox() const + { + BoundingBox2 box; + + kt_double minX = GetOffset().GetX(); + kt_double minY = GetOffset().GetY(); + kt_double maxX = minX + GetSize().GetWidth() * GetResolution(); + kt_double maxY = minY + GetSize().GetHeight() * GetResolution(); + + box.SetMinimum(GetOffset()); + box.SetMaximum(Vector2(maxX, maxY)); + return box; + } + + private: + Size2 m_Size; + kt_double m_Scale; + + Vector2 m_Offset; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Size); + ar & BOOST_SERIALIZATION_NVP(m_Scale); + ar & BOOST_SERIALIZATION_NVP(m_Offset); + } + + }; // CoordinateConverter + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Defines a grid class + */ + template + class Grid + { + public: + /** + * Creates a grid of given size and resolution + * @param width + * @param height + * @param resolution + * @return grid pointer + */ + Grid() + { + } + static Grid* CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution) + { + Grid* pGrid = new Grid(width, height); + + pGrid->GetCoordinateConverter()->SetScale(1.0 / resolution); + + return pGrid; + } + + /** + * Destructor + */ + virtual ~Grid() + { + if (m_pData) + { + delete [] m_pData; + } + if (m_pCoordinateConverter) + { + delete m_pCoordinateConverter; + } + } + + public: + /** + * Clear out the grid data + */ + void Clear() + { + memset(m_pData, 0, GetDataSize() * sizeof(T)); + } + + /** + * Returns a clone of this grid + * @return grid clone + */ + Grid* Clone() + { + Grid* pGrid = CreateGrid(GetWidth(), GetHeight(), GetResolution()); + pGrid->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset()); + + memcpy(pGrid->GetDataPointer(), GetDataPointer(), GetDataSize()); + + return pGrid; + } + + /** + * Resizes the grid (deletes all old data) + * @param width + * @param height + */ + virtual void Resize(kt_int32s width, kt_int32s height) + { + m_Width = width; + m_Height = height; + m_WidthStep = math::AlignValue(width, 8); + + if (m_pData != NULL) + { + delete[] m_pData; + m_pData = NULL; + } + + try + { + m_pData = new T[GetDataSize()]; + + if (m_pCoordinateConverter == NULL) + { + m_pCoordinateConverter = new CoordinateConverter(); + } + + m_pCoordinateConverter->SetSize(Size2(width, height)); + } + catch(...) + { + m_pData = NULL; + + m_Width = 0; + m_Height = 0; + m_WidthStep = 0; + } + + Clear(); + } + + /** + * Checks whether the given coordinates are valid grid indices + * @param rGrid + */ + inline kt_bool IsValidGridIndex(const Vector2& rGrid) const + { + return (math::IsUpTo(rGrid.GetX(), m_Width) && math::IsUpTo(rGrid.GetY(), m_Height)); + } + + /** + * Gets the index into the data pointer of the given grid coordinate + * @param rGrid + * @param boundaryCheck default value is true + * @return grid index + */ + virtual kt_int32s GridIndex(const Vector2& rGrid, kt_bool boundaryCheck = true) const + { + if (boundaryCheck == true) + { + if (IsValidGridIndex(rGrid) == false) + { + std::stringstream error; + error << "Index " << rGrid << " out of range. Index must be between [0; " + << m_Width << ") and [0; " << m_Height << ")"; + throw Exception(error.str()); + } + } + + kt_int32s index = rGrid.GetX() + (rGrid.GetY() * m_WidthStep); + + if (boundaryCheck == true) + { + assert(math::IsUpTo(index, GetDataSize())); + } + + return index; + } + + /** + * Gets the grid coordinate from an index + * @param index + * @return grid coordinate + */ + Vector2 IndexToGrid(kt_int32s index) const + { + Vector2 grid; + + grid.SetY(index / m_WidthStep); + grid.SetX(index - grid.GetY() * m_WidthStep); + + return grid; + } + + /** + * Converts the point from world coordinates to grid coordinates + * @param rWorld world coordinate + * @param flipY + * @return grid coordinate + */ + inline Vector2 WorldToGrid(const Vector2& rWorld, kt_bool flipY = false) const + { + return GetCoordinateConverter()->WorldToGrid(rWorld, flipY); + } + + /** + * Converts the point from grid coordinates to world coordinates + * @param rGrid world coordinate + * @param flipY + * @return world coordinate + */ + inline Vector2 GridToWorld(const Vector2& rGrid, kt_bool flipY = false) const + { + return GetCoordinateConverter()->GridToWorld(rGrid, flipY); + } + + /** + * Gets pointer to data at given grid coordinate + * @param rGrid grid coordinate + * @return grid point + */ + T* GetDataPointer(const Vector2& rGrid) + { + kt_int32s index = GridIndex(rGrid, true); + return m_pData + index; + } + + /** + * Gets pointer to data at given grid coordinate + * @param rGrid grid coordinate + * @return grid point + */ + T* GetDataPointer(const Vector2& rGrid) const + { + kt_int32s index = GridIndex(rGrid, true); + return m_pData + index; + } + + /** + * Gets the width of the grid + * @return width of the grid + */ + inline kt_int32s GetWidth() const + { + return m_Width; + }; + + /** + * Gets the height of the grid + * @return height of the grid + */ + inline kt_int32s GetHeight() const + { + return m_Height; + }; + + /** + * Get the size as a Size2 + * @return size of the grid + */ + inline const Size2 GetSize() const + { + return Size2(m_Width, m_Height); + } + + /** + * Gets the width step in bytes + * @return width step + */ + inline kt_int32s GetWidthStep() const + { + return m_WidthStep; + } + + /** + * Gets the grid data pointer + * @return data pointer + */ + inline T* GetDataPointer() + { + return m_pData; + } + + /** + * Gets const grid data pointer + * @return data pointer + */ + inline T* GetDataPointer() const + { + return m_pData; + } + + /** + * Gets the allocated grid size in bytes + * @return data size + */ + inline kt_int32s GetDataSize() const + { + return m_WidthStep * m_Height; + } + + /** + * Get value at given grid coordinate + * @param rGrid grid coordinate + * @return value + */ + inline T GetValue(const Vector2& rGrid) const + { + kt_int32s index = GridIndex(rGrid); + return m_pData[index]; + } + + /** + * Gets the coordinate converter for this grid + * @return coordinate converter + */ + inline CoordinateConverter* GetCoordinateConverter() const + { + return m_pCoordinateConverter; + } + + /** + * Gets the resolution + * @return resolution + */ + inline kt_double GetResolution() const + { + return GetCoordinateConverter()->GetResolution(); + } + + /** + * Gets the grids bounding box + * @return bounding box + */ + inline BoundingBox2 GetBoundingBox() const + { + return GetCoordinateConverter()->GetBoundingBox(); + } + + /** + * Increments all the grid cells from (x0, y0) to (x1, y1); + * if applicable, apply f to each cell traced + * @param x0 + * @param y0 + * @param x1 + * @param y1 + * @param f + */ + void TraceLine(kt_int32s x0, kt_int32s y0, kt_int32s x1, kt_int32s y1, Functor* f = NULL) + { + kt_bool steep = abs(y1 - y0) > abs(x1 - x0); + if (steep) + { + std::swap(x0, y0); + std::swap(x1, y1); + } + if (x0 > x1) + { + std::swap(x0, x1); + std::swap(y0, y1); + } + + kt_int32s deltaX = x1 - x0; + kt_int32s deltaY = abs(y1 - y0); + kt_int32s error = 0; + kt_int32s ystep; + kt_int32s y = y0; + + if (y0 < y1) + { + ystep = 1; + } + else + { + ystep = -1; + } + + kt_int32s pointX; + kt_int32s pointY; + for (kt_int32s x = x0; x <= x1; x++) + { + if (steep) + { + pointX = y; + pointY = x; + } + else + { + pointX = x; + pointY = y; + } + + error += deltaY; + + if (2 * error >= deltaX) + { + y += ystep; + error -= deltaX; + } + + Vector2 gridIndex(pointX, pointY); + if (IsValidGridIndex(gridIndex)) + { + kt_int32s index = GridIndex(gridIndex, false); + T* pGridPointer = GetDataPointer(); + pGridPointer[index]++; + + if (f != NULL) + { + (*f)(index); + } + } + } + } + + protected: + /** + * Constructs grid of given size + * @param width + * @param height + */ + Grid(kt_int32s width, kt_int32s height) + : m_pData(NULL) + , m_pCoordinateConverter(NULL) + { + Resize(width, height); + } + + private: + kt_int32s m_Width; // width of grid + kt_int32s m_Height; // height of grid + kt_int32s m_WidthStep; // 8 bit aligned width of grid + T* m_pData; // grid data + + // coordinate converter to convert between world coordinates and grid coordinates + CoordinateConverter* m_pCoordinateConverter; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Width); + ar & BOOST_SERIALIZATION_NVP(m_Height); + ar & BOOST_SERIALIZATION_NVP(m_WidthStep); + ar & BOOST_SERIALIZATION_NVP(m_pCoordinateConverter); + + + if (Archive::is_loading::value) + { + m_pData = new T[m_WidthStep * m_Height]; + } + ar & boost::serialization::make_array(m_pData, m_WidthStep * m_Height); + } + + }; // Grid + BOOST_SERIALIZATION_ASSUME_ABSTRACT(Grid) + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * For making custom data + */ + class CustomData : public Object + { + public: + // @cond EXCLUDE + KARTO_Object(CustomData) + // @endcond + + public: + /** + * Constructor + */ + CustomData() + : Object() + { + } + + /** + * Destructor + */ + virtual ~CustomData() + { + } + + public: + /** + * Write out this custom data as a string + * @return string representation of this data object + */ + virtual const std::string Write() const = 0; + + /** + * Read in this custom data from a string + * @param rValue string representation of this data object + */ + virtual void Read(const std::string& rValue) = 0; + + private: + CustomData(const CustomData&); + const CustomData& operator=(const CustomData&); + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object); + } + }; + BOOST_SERIALIZATION_ASSUME_ABSTRACT(CustomData) + + /** + * Type declaration of CustomData vector + */ + typedef std::vector CustomDataVector; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * SensorData is a base class for all sensor data + */ + class KARTO_EXPORT SensorData : public Object + { + public: + // @cond EXCLUDE + KARTO_Object(SensorData) + // @endcond + + public: + + SensorData(){} + /** + * Destructor + */ + virtual ~SensorData(); + + public: + /** + * Gets sensor data id + * @return sensor id + */ + inline kt_int32s GetStateId() const + { + return m_StateId; + } + + /** + * Sets sensor data id + * @param stateId id + */ + inline void SetStateId(kt_int32s stateId) + { + m_StateId = stateId; + } + + /** + * Gets sensor unique id + * @return unique id + */ + inline kt_int32s GetUniqueId() const + { + return m_UniqueId; + } + + /** + * Sets sensor unique id + * @param uniqueId + */ + inline void SetUniqueId(kt_int32u uniqueId) + { + m_UniqueId = uniqueId; + } + + /** + * Gets sensor data time + * @return time + */ + inline kt_double GetTime() const + { + return m_Time; + } + + /** + * Sets sensor data time + * @param time + */ + inline void SetTime(kt_double time) + { + m_Time = time; + } + + /** + * Get the sensor that created this sensor data + * @return sensor + */ + inline const Name& GetSensorName() const + { + return m_SensorName; + } + + /** + * Add a CustomData object to sensor data + * @param pCustomData + */ + inline void AddCustomData(CustomData* pCustomData) + { + m_CustomData.push_back(pCustomData); + } + + /** + * Get all custom data objects assigned to sensor data + * @return CustomDataVector& + */ + inline const CustomDataVector& GetCustomData() const + { + return m_CustomData; + } + + protected: + /** + * Construct a SensorData object with a sensor name + */ + SensorData(const Name& rSensorName); + + private: + /** + * Restrict the copy constructor + */ + SensorData(const SensorData&); + + /** + * Restrict the assignment operator + */ + const SensorData& operator=(const SensorData&); + + private: + /** + * ID unique to individual sensor + */ + kt_int32s m_StateId; + + /** + * ID unique across all sensor data + */ + kt_int32s m_UniqueId; + + /** + * Sensor that created this sensor data + */ + Name m_SensorName; + + /** + * Time the sensor data was created + */ + kt_double m_Time; + + CustomDataVector m_CustomData; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_StateId); + ar & BOOST_SERIALIZATION_NVP(m_UniqueId); + ar & BOOST_SERIALIZATION_NVP(m_SensorName); + ar & BOOST_SERIALIZATION_NVP(m_Time); + ar & BOOST_SERIALIZATION_NVP(m_CustomData); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object); + } +}; + + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Type declaration of range readings vector + */ + typedef std::vector RangeReadingsVector; + + /** + * LaserRangeScan representing the range readings from a laser range finder sensor. + */ + class LaserRangeScan : public SensorData + { + public: + // @cond EXCLUDE + KARTO_Object(LaserRangeScan) + // @endcond + + public: + /** + * Constructs a scan from the given sensor with the given readings + * @param rSensorName + */ + LaserRangeScan(const Name& rSensorName) + : SensorData(rSensorName) + , m_pRangeReadings(NULL) + , m_NumberOfRangeReadings(0) + { + } + + LaserRangeScan() + { + } + + /** + * Constructs a scan from the given sensor with the given readings + * @param rSensorName + * @param rRangeReadings + */ + LaserRangeScan(const Name& rSensorName, const RangeReadingsVector& rRangeReadings) + : SensorData(rSensorName) + , m_pRangeReadings(NULL) + , m_NumberOfRangeReadings(0) + { + assert(rSensorName.ToString() != ""); + + SetRangeReadings(rRangeReadings); + } + + /** + * Destructor + */ + virtual ~LaserRangeScan() + { + delete [] m_pRangeReadings; + } + + public: + /** + * Gets the range readings of this scan + * @return range readings of this scan + */ + inline kt_double* GetRangeReadings() const + { + return m_pRangeReadings; + } + + inline RangeReadingsVector GetRangeReadingsVector() const + { + return RangeReadingsVector(m_pRangeReadings, m_pRangeReadings + m_NumberOfRangeReadings); + } + + /** + * Sets the range readings for this scan + * @param rRangeReadings + */ + inline void SetRangeReadings(const RangeReadingsVector& rRangeReadings) + { + // ignore for now! XXXMAE BUGUBUG 05/21/2010 << TODO(lucbettaieb): What the heck is this?? + // if (rRangeReadings.size() != GetNumberOfRangeReadings()) + // { + // std::stringstream error; + // error << "Given number of readings (" << rRangeReadings.size() + // << ") does not match expected number of range finder (" << GetNumberOfRangeReadings() << ")"; + // throw Exception(error.str()); + // } + + if (!rRangeReadings.empty()) + { + if (rRangeReadings.size() != m_NumberOfRangeReadings) + { + // delete old readings + delete [] m_pRangeReadings; + + // store size of array! + m_NumberOfRangeReadings = static_cast(rRangeReadings.size()); + + // allocate range readings + m_pRangeReadings = new kt_double[m_NumberOfRangeReadings]; + } + + // copy readings + kt_int32u index = 0; + const_forEach(RangeReadingsVector, &rRangeReadings) + { + m_pRangeReadings[index++] = *iter; + } + } + else + { + delete [] m_pRangeReadings; + m_pRangeReadings = NULL; + } + } + + /** + * Gets the laser range finder sensor that generated this scan + * @return laser range finder sensor of this scan + */ + inline LaserRangeFinder* GetLaserRangeFinder() const + { + return SensorManager::GetInstance()->GetSensorByName(GetSensorName()); + } + + /** + * Gets the number of range readings + * @return number of range readings + */ + inline kt_int32u GetNumberOfRangeReadings() const + { + return m_NumberOfRangeReadings; + } + + private: + LaserRangeScan(const LaserRangeScan&); + const LaserRangeScan& operator=(const LaserRangeScan&); + + private: + kt_double* m_pRangeReadings; + kt_int32u m_NumberOfRangeReadings; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_NumberOfRangeReadings); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(SensorData); + + if (Archive::is_loading::value) + { + m_pRangeReadings = new kt_double[m_NumberOfRangeReadings]; + } + ar & boost::serialization::make_array(m_pRangeReadings, m_NumberOfRangeReadings); + } + }; // LaserRangeScan + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * DrivePose representing the pose value of a drive sensor. + */ + class DrivePose : public SensorData + { + public: + // @cond EXCLUDE + KARTO_Object(DrivePose) + // @endcond + + public: + /** + * Constructs a pose of the given drive sensor + * @param rSensorName + */ + DrivePose(const Name& rSensorName) + : SensorData(rSensorName) + { + } + + /** + * Destructor + */ + virtual ~DrivePose() + { + } + + public: + /** + * Gets the odometric pose of this scan + * @return odometric pose of this scan + */ + inline const Pose3& GetOdometricPose() const + { + return m_OdometricPose; + } + + /** + * Sets the odometric pose of this scan + * @param rPose + */ + inline void SetOdometricPose(const Pose3& rPose) + { + m_OdometricPose = rPose; + } + + private: + DrivePose(const DrivePose&); + const DrivePose& operator=(const DrivePose&); + + private: + /** + * Odometric pose of robot + */ + Pose3 m_OdometricPose; + }; // class DrivePose + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * The LocalizedRangeScan contains range data from a single sweep of a laser range finder sensor + * in a two-dimensional space and position information. The odometer position is the position + * reported by the robot when the range data was recorded. The corrected position is the position + * calculated by the mapper (or localizer) + */ + class LocalizedRangeScan : public LaserRangeScan + { + public: + // @cond EXCLUDE + KARTO_Object(LocalizedRangeScan) + // @endcond + + public: + /** + * Constructs a range scan from the given range finder with the given readings + */ + LocalizedRangeScan(const Name& rSensorName, const RangeReadingsVector& rReadings) + : LaserRangeScan(rSensorName, rReadings) + , m_IsDirty(true) + { + } + + LocalizedRangeScan() + {} + + /** + * Destructor + */ + virtual ~LocalizedRangeScan() + { + } + + private: + mutable boost::shared_mutex m_Lock; + + public: + /** + * Gets the odometric pose of this scan + * @return odometric pose of this scan + */ + inline const Pose2& GetOdometricPose() const + { + return m_OdometricPose; + } + + /** + * Sets the odometric pose of this scan + * @param rPose + */ + inline void SetOdometricPose(const Pose2& rPose) + { + m_OdometricPose = rPose; + } + + /** + * Gets the (possibly corrected) robot pose at which this scan was taken. The corrected robot pose of the scan + * is usually set by an external module such as a localization or mapping module when it is determined + * that the original pose was incorrect. The external module will set the correct pose based on + * additional sensor data and any context information it has. If the pose has not been corrected, + * a call to this method returns the same pose as GetOdometricPose(). + * @return corrected pose + */ + inline const Pose2& GetCorrectedPose() const + { + return m_CorrectedPose; + } + + /** + * Moves the scan by moving the robot pose to the given location. + * @param rPose new pose of the robot of this scan + */ + inline void SetCorrectedPose(const Pose2& rPose) + { + m_CorrectedPose = rPose; + + m_IsDirty = true; + } + + /** + * Gets barycenter of point readings + */ + inline const Pose2& GetBarycenterPose() const + { + boost::shared_lock lock(m_Lock); + if (m_IsDirty) + { + // throw away constness and do an update! + lock.unlock(); + boost::unique_lock uniqueLock(m_Lock); + const_cast(this)->Update(); + } + + return m_BarycenterPose; + } + + inline void SetBarycenterPose(Pose2& bcenter) + { + m_BarycenterPose = bcenter; + } + + /** + * Gets barycenter if the given parameter is true, otherwise returns the scanner pose + * @param useBarycenter + * @return barycenter if parameter is true, otherwise scanner pose + */ + inline Pose2 GetReferencePose(kt_bool useBarycenter) const + { + boost::shared_lock lock(m_Lock); + if (m_IsDirty) + { + // throw away constness and do an update! + lock.unlock(); + boost::unique_lock uniqueLock(m_Lock); + const_cast(this)->Update(); + } + + return useBarycenter ? GetBarycenterPose() : GetSensorPose(); + } + + /** + * Computes the position of the sensor + * @return scan pose + */ + inline Pose2 GetSensorPose() const + { + return GetSensorAt(m_CorrectedPose); + } + + inline void SetIsDirty(kt_bool& rIsDirty) + { + m_IsDirty = rIsDirty; + } + + /** + * Computes the robot pose given the corrected scan pose + * @param rScanPose pose of the sensor + */ + void SetSensorPose(const Pose2& rScanPose) + { + Pose2 deviceOffsetPose2 = GetLaserRangeFinder()->GetOffsetPose(); + kt_double offsetLength = deviceOffsetPose2.GetPosition().Length(); + kt_double offsetHeading = deviceOffsetPose2.GetHeading(); + kt_double angleoffset = atan2(deviceOffsetPose2.GetY(), deviceOffsetPose2.GetX()); + kt_double correctedHeading = math::NormalizeAngle(rScanPose.GetHeading()); + Pose2 worldSensorOffset = Pose2(offsetLength * cos(correctedHeading + angleoffset - offsetHeading), + offsetLength * sin(correctedHeading + angleoffset - offsetHeading), + offsetHeading); + + m_CorrectedPose = rScanPose - worldSensorOffset; + + Update(); + } + + /** + * Computes the position of the sensor if the robot were at the given pose + * @param rPose + * @return sensor pose + */ + inline Pose2 GetSensorAt(const Pose2& rPose) const + { + return Transform(rPose).TransformPose(GetLaserRangeFinder()->GetOffsetPose()); + } + + /** + * Gets the bounding box of this scan + * @return bounding box of this scan + */ + inline const BoundingBox2& GetBoundingBox() const + { + boost::shared_lock lock(m_Lock); + if (m_IsDirty) + { + // throw away constness and do an update! + lock.unlock(); + boost::unique_lock uniqueLock(m_Lock); + const_cast(this)->Update(); + } + + return m_BoundingBox; + } + + inline void SetBoundingBox(BoundingBox2& bbox) + { + m_BoundingBox = bbox; + } + + /** + * Get point readings in local coordinates + */ + inline const PointVectorDouble& GetPointReadings(kt_bool wantFiltered = false) const + { + boost::shared_lock lock(m_Lock); + if (m_IsDirty) + { + // throw away constness and do an update! + lock.unlock(); + boost::unique_lock uniqueLock(m_Lock); + const_cast(this)->Update(); + } + + if (wantFiltered == true) + { + return m_PointReadings; + } + else + { + return m_UnfilteredPointReadings; + } + } + + inline void SetPointReadings(PointVectorDouble& points, kt_bool setFiltered = false) + { + if (setFiltered) + { + m_PointReadings = points; + } + else + { + m_UnfilteredPointReadings = points; + } + } + + private: + /** + * Compute point readings based on range readings + * Only range readings within [minimum range; range threshold] are returned + */ + virtual void Update() + { + LaserRangeFinder* pLaserRangeFinder = GetLaserRangeFinder(); + + if (pLaserRangeFinder != NULL) + { + m_PointReadings.clear(); + m_UnfilteredPointReadings.clear(); + + kt_double rangeThreshold = pLaserRangeFinder->GetRangeThreshold(); + kt_double minimumAngle = pLaserRangeFinder->GetMinimumAngle(); + kt_double angularResolution = pLaserRangeFinder->GetAngularResolution(); + Pose2 scanPose = GetSensorPose(); + + // compute point readings + Vector2 rangePointsSum; + kt_int32u beamNum = 0; + for (kt_int32u i = 0; i < pLaserRangeFinder->GetNumberOfRangeReadings(); i++, beamNum++) + { + kt_double rangeReading = GetRangeReadings()[i]; + if (!math::InRange(rangeReading, pLaserRangeFinder->GetMinimumRange(), rangeThreshold)) + { + kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution; + + Vector2 point; + point.SetX(scanPose.GetX() + (rangeReading * cos(angle))); + point.SetY(scanPose.GetY() + (rangeReading * sin(angle))); + + m_UnfilteredPointReadings.push_back(point); + continue; + } + + kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution; + + Vector2 point; + point.SetX(scanPose.GetX() + (rangeReading * cos(angle))); + point.SetY(scanPose.GetY() + (rangeReading * sin(angle))); + + m_PointReadings.push_back(point); + m_UnfilteredPointReadings.push_back(point); + + rangePointsSum += point; + } + + // compute barycenter + kt_double nPoints = static_cast(m_PointReadings.size()); + if (nPoints != 0.0) + { + Vector2 averagePosition = Vector2(rangePointsSum / nPoints); + m_BarycenterPose = Pose2(averagePosition, 0.0); + } + else + { + m_BarycenterPose = scanPose; + } + + // calculate bounding box of scan + m_BoundingBox = BoundingBox2(); + m_BoundingBox.Add(scanPose.GetPosition()); + forEach(PointVectorDouble, &m_PointReadings) + { + m_BoundingBox.Add(*iter); + } + } + + m_IsDirty = false; + } + + /** + * Serialization: class LocalizedRangeScan + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_OdometricPose); + ar & BOOST_SERIALIZATION_NVP(m_CorrectedPose); + ar & BOOST_SERIALIZATION_NVP(m_BarycenterPose); + ar & BOOST_SERIALIZATION_NVP(m_PointReadings); + ar & BOOST_SERIALIZATION_NVP(m_UnfilteredPointReadings); + ar & BOOST_SERIALIZATION_NVP(m_BoundingBox); + ar & BOOST_SERIALIZATION_NVP(m_IsDirty); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(LaserRangeScan); + } + + + private: + LocalizedRangeScan(const LocalizedRangeScan&); + const LocalizedRangeScan& operator=(const LocalizedRangeScan&); + + private: + /** + * Odometric pose of robot + */ + Pose2 m_OdometricPose; + + /** + * Corrected pose of robot calculated by mapper (or localizer) + */ + Pose2 m_CorrectedPose; + + protected: + /** + * Average of all the point readings + */ + Pose2 m_BarycenterPose; + + /** + * Vector of point readings + */ + PointVectorDouble m_PointReadings; + + /** + * Vector of unfiltered point readings + */ + PointVectorDouble m_UnfilteredPointReadings; + + /** + * Bounding box of localized range scan + */ + BoundingBox2 m_BoundingBox; + + /** + * Internal flag used to update point readings, barycenter and bounding box + */ + kt_bool m_IsDirty; + }; // LocalizedRangeScan + + /** + * Type declaration of LocalizedRangeScan vector + */ + typedef std::vector LocalizedRangeScanVector; +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// + + /** + * The LocalizedRangeScanWithPoints is an extension of the LocalizedRangeScan with precomputed points. + */ + class LocalizedRangeScanWithPoints : public LocalizedRangeScan + { + public: + // @cond EXCLUDE + KARTO_Object(LocalizedRangeScanWithPoints) + // @endcond + + public: + /** + * Constructs a range scan from the given range finder with the given readings. Precomptued points should be + * in the robot frame. + */ + LocalizedRangeScanWithPoints(const Name& rSensorName, const RangeReadingsVector& rReadings, + const PointVectorDouble& rPoints) + : m_Points(rPoints), + LocalizedRangeScan(rSensorName, rReadings) + { + } + + /** + * Destructor + */ + virtual ~LocalizedRangeScanWithPoints() + { + } + + private: + /** + * Update the points based on the latest sensor pose. + */ + void Update() + { + m_PointReadings.clear(); + m_UnfilteredPointReadings.clear(); + + Pose2 scanPose = GetSensorPose(); + Pose2 robotPose = GetCorrectedPose(); + + // update point readings + Vector2 rangePointsSum; + for (kt_int32u i = 0; i < m_Points.size(); i++) + { + // check if this has a NaN + if (!std::isfinite(m_Points[i].GetX()) || !std::isfinite(m_Points[i].GetY())) + { + Vector2 point(m_Points[i].GetX(), m_Points[i].GetY()); + m_UnfilteredPointReadings.push_back(point); + + continue; + } + + // transform into world coords + Pose2 pointPose(m_Points[i].GetX(), m_Points[i].GetY(), 0); + Pose2 result = Transform(robotPose).TransformPose(pointPose); + Vector2 point(result.GetX(), result.GetY()); + + m_PointReadings.push_back(point); + m_UnfilteredPointReadings.push_back(point); + + rangePointsSum += point; + } + + // compute barycenter + kt_double nPoints = static_cast(m_PointReadings.size()); + if (nPoints != 0.0) + { + Vector2 averagePosition = Vector2(rangePointsSum / nPoints); + m_BarycenterPose = Pose2(averagePosition, 0.0); + } + else + { + m_BarycenterPose = scanPose; + } + + // calculate bounding box of scan + m_BoundingBox = BoundingBox2(); + m_BoundingBox.Add(scanPose.GetPosition()); + forEach(PointVectorDouble, &m_PointReadings) + { + m_BoundingBox.Add(*iter); + } + + m_IsDirty = false; + } + + private: + LocalizedRangeScanWithPoints(const LocalizedRangeScanWithPoints&); + const LocalizedRangeScanWithPoints& operator=(const LocalizedRangeScanWithPoints&); + + private: + const PointVectorDouble m_Points; + }; // LocalizedRangeScanWithPoints + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + class OccupancyGrid; + + class KARTO_EXPORT CellUpdater : public Functor + { + public: + CellUpdater(OccupancyGrid* pGrid) + : m_pOccupancyGrid(pGrid) + { + } + + /** + * Updates the cell at the given index based on the grid's hits and pass counters + * @param index + */ + virtual void operator() (kt_int32u index); + + private: + OccupancyGrid* m_pOccupancyGrid; + }; // CellUpdater + + /** + * Occupancy grid definition. See GridStates for possible grid values. + */ + class OccupancyGrid : public Grid + { + friend class CellUpdater; + friend class IncrementalOccupancyGrid; + + public: + /** + * Constructs an occupancy grid of given size + * @param width + * @param height + * @param rOffset + * @param resolution + */ + OccupancyGrid(kt_int32s width, kt_int32s height, const Vector2& rOffset, kt_double resolution) + : Grid(width, height) + , m_pCellPassCnt(Grid::CreateGrid(0, 0, resolution)) + , m_pCellHitsCnt(Grid::CreateGrid(0, 0, resolution)) + , m_pCellUpdater(NULL) + { + m_pCellUpdater = new CellUpdater(this); + + if (karto::math::DoubleEqual(resolution, 0.0)) + { + throw Exception("Resolution cannot be 0"); + } + + m_pMinPassThrough = new Parameter("MinPassThrough", 2); + m_pOccupancyThreshold = new Parameter("OccupancyThreshold", 0.1); + + GetCoordinateConverter()->SetScale(1.0 / resolution); + GetCoordinateConverter()->SetOffset(rOffset); + } + + /** + * Destructor + */ + virtual ~OccupancyGrid() + { + delete m_pCellUpdater; + + delete m_pCellPassCnt; + delete m_pCellHitsCnt; + + delete m_pMinPassThrough; + delete m_pOccupancyThreshold; + } + + public: + /** + * Create an occupancy grid from the given scans using the given resolution + * @param rScans + * @param resolution + */ + static OccupancyGrid* CreateFromScans(const LocalizedRangeScanVector& rScans, kt_double resolution) + { + if (rScans.empty()) + { + return NULL; + } + + kt_int32s width, height; + Vector2 offset; + ComputeDimensions(rScans, resolution, width, height, offset); + OccupancyGrid* pOccupancyGrid = new OccupancyGrid(width, height, offset, resolution); + pOccupancyGrid->CreateFromScans(rScans); + + return pOccupancyGrid; + } + + /** + * Make a clone + * @return occupancy grid clone + */ + OccupancyGrid* Clone() const + { + OccupancyGrid* pOccupancyGrid = new OccupancyGrid(GetWidth(), + GetHeight(), + GetCoordinateConverter()->GetOffset(), + 1.0 / GetCoordinateConverter()->GetScale()); + memcpy(pOccupancyGrid->GetDataPointer(), GetDataPointer(), GetDataSize()); + + pOccupancyGrid->GetCoordinateConverter()->SetSize(GetCoordinateConverter()->GetSize()); + pOccupancyGrid->m_pCellPassCnt = m_pCellPassCnt->Clone(); + pOccupancyGrid->m_pCellHitsCnt = m_pCellHitsCnt->Clone(); + + return pOccupancyGrid; + } + + /** + * Check if grid point is free + * @param rPose + * @return whether the cell at the given point is free space + */ + virtual kt_bool IsFree(const Vector2& rPose) const + { + kt_int8u* pOffsets = reinterpret_cast(GetDataPointer(rPose)); + if (*pOffsets == GridStates_Free) + { + return true; + } + + return false; + } + + /** + * Casts a ray from the given point (up to the given max range) + * and returns the distance to the closest obstacle + * @param rPose2 + * @param maxRange + * @return distance to closest obstacle + */ + virtual kt_double RayCast(const Pose2& rPose2, kt_double maxRange) const + { + double scale = GetCoordinateConverter()->GetScale(); + + kt_double x = rPose2.GetX(); + kt_double y = rPose2.GetY(); + kt_double theta = rPose2.GetHeading(); + + kt_double sinTheta = sin(theta); + kt_double cosTheta = cos(theta); + + kt_double xStop = x + maxRange * cosTheta; + kt_double xSteps = 1 + fabs(xStop - x) * scale; + + kt_double yStop = y + maxRange * sinTheta; + kt_double ySteps = 1 + fabs(yStop - y) * scale; + + kt_double steps = math::Maximum(xSteps, ySteps); + kt_double delta = maxRange / steps; + kt_double distance = delta; + + for (kt_int32u i = 1; i < steps; i++) + { + kt_double x1 = x + distance * cosTheta; + kt_double y1 = y + distance * sinTheta; + + Vector2 gridIndex = WorldToGrid(Vector2(x1, y1)); + if (IsValidGridIndex(gridIndex) && IsFree(gridIndex)) + { + distance = (i + 1) * delta; + } + else + { + break; + } + } + + return (distance < maxRange)? distance : maxRange; + } + + /** + * Sets the minimum number of beams that must pass through a cell before it + * will be considered to be occupied or unoccupied. + * This prevents stray beams from messing up the map. + */ + void SetMinPassThrough(kt_int32u count) + { + m_pMinPassThrough->SetValue(count); + } + + /** + * Sets the minimum ratio of beams hitting cell to beams passing through + * cell for cell to be marked as occupied. + */ + void SetOccupancyThreshold(kt_double thresh) + { + m_pOccupancyThreshold->SetValue(thresh); + } + + protected: + /** + * Get cell hit grid + * @return Grid* + */ + virtual Grid* GetCellHitsCounts() + { + return m_pCellHitsCnt; + } + + /** + * Get cell pass grid + * @return Grid* + */ + virtual Grid* GetCellPassCounts() + { + return m_pCellPassCnt; + } + + protected: + /** + * Calculate grid dimensions from localized range scans + * @param rScans + * @param resolution + * @param rWidth + * @param rHeight + * @param rOffset + */ + static void ComputeDimensions(const LocalizedRangeScanVector& rScans, + kt_double resolution, + kt_int32s& rWidth, + kt_int32s& rHeight, + Vector2& rOffset) + { + BoundingBox2 boundingBox; + const_forEach(LocalizedRangeScanVector, &rScans) + { + boundingBox.Add((*iter)->GetBoundingBox()); + } + + kt_double scale = 1.0 / resolution; + Size2 size = boundingBox.GetSize(); + + rWidth = static_cast(math::Round(size.GetWidth() * scale)); + rHeight = static_cast(math::Round(size.GetHeight() * scale)); + rOffset = boundingBox.GetMinimum(); + } + + /** + * Create grid using scans + * @param rScans + */ + virtual void CreateFromScans(const LocalizedRangeScanVector& rScans) + { + m_pCellPassCnt->Resize(GetWidth(), GetHeight()); + m_pCellPassCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset()); + + m_pCellHitsCnt->Resize(GetWidth(), GetHeight()); + m_pCellHitsCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset()); + + const_forEach(LocalizedRangeScanVector, &rScans) + { + LocalizedRangeScan* pScan = *iter; + AddScan(pScan); + } + + Update(); + } + + /** + * Adds the scan's information to this grid's counters (optionally + * update the grid's cells' occupancy status) + * @param pScan + * @param doUpdate whether to update the grid's cell's occupancy status + * @return returns false if an endpoint fell off the grid, otherwise true + */ + virtual kt_bool AddScan(LocalizedRangeScan* pScan, kt_bool doUpdate = false) + { + kt_double rangeThreshold = pScan->GetLaserRangeFinder()->GetRangeThreshold(); + kt_double maxRange = pScan->GetLaserRangeFinder()->GetMaximumRange(); + kt_double minRange = pScan->GetLaserRangeFinder()->GetMinimumRange(); + + Vector2 scanPosition = pScan->GetSensorPose().GetPosition(); + // get scan point readings + const PointVectorDouble& rPointReadings = pScan->GetPointReadings(false); + + kt_bool isAllInMap = true; + + // draw lines from scan position to all point readings + int pointIndex = 0; + const_forEachAs(PointVectorDouble, &rPointReadings, pointsIter) + { + Vector2 point = *pointsIter; + kt_double rangeReading = pScan->GetRangeReadings()[pointIndex]; + kt_bool isEndPointValid = rangeReading < (rangeThreshold - KT_TOLERANCE); + + if (rangeReading <= minRange || rangeReading >= maxRange || std::isnan(rangeReading)) + { + // ignore these readings + pointIndex++; + continue; + } + else if (rangeReading >= rangeThreshold) + { + // trace up to range reading + kt_double ratio = rangeThreshold / rangeReading; + kt_double dx = point.GetX() - scanPosition.GetX(); + kt_double dy = point.GetY() - scanPosition.GetY(); + point.SetX(scanPosition.GetX() + ratio * dx); + point.SetY(scanPosition.GetY() + ratio * dy); + } + + kt_bool isInMap = RayTrace(scanPosition, point, isEndPointValid, doUpdate); + if (!isInMap) + { + isAllInMap = false; + } + + pointIndex++; + } + + return isAllInMap; + } + + /** + * Traces a beam from the start position to the end position marking + * the bookkeeping arrays accordingly. + * @param rWorldFrom start position of beam + * @param rWorldTo end position of beam + * @param isEndPointValid is the reading within the range threshold? + * @param doUpdate whether to update the cells' occupancy status immediately + * @return returns false if an endpoint fell off the grid, otherwise true + */ + virtual kt_bool RayTrace(const Vector2& rWorldFrom, + const Vector2& rWorldTo, + kt_bool isEndPointValid, + kt_bool doUpdate = false) + { + assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL); + + Vector2 gridFrom = m_pCellPassCnt->WorldToGrid(rWorldFrom); + Vector2 gridTo = m_pCellPassCnt->WorldToGrid(rWorldTo); + + CellUpdater* pCellUpdater = doUpdate ? m_pCellUpdater : NULL; + m_pCellPassCnt->TraceLine(gridFrom.GetX(), gridFrom.GetY(), gridTo.GetX(), gridTo.GetY(), pCellUpdater); + + // for the end point + if (isEndPointValid) + { + if (m_pCellPassCnt->IsValidGridIndex(gridTo)) + { + kt_int32s index = m_pCellPassCnt->GridIndex(gridTo, false); + + kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer(); + kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer(); + + // increment cell pass through and hit count + pCellPassCntPtr[index]++; + pCellHitCntPtr[index]++; + + if (doUpdate) + { + (*m_pCellUpdater)(index); + } + } + } + + return m_pCellPassCnt->IsValidGridIndex(gridTo); + } + + /** + * Updates a single cell's value based on the given counters + * @param pCell + * @param cellPassCnt + * @param cellHitCnt + */ + virtual void UpdateCell(kt_int8u* pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt) + { + if (cellPassCnt > m_pMinPassThrough->GetValue()) + { + kt_double hitRatio = static_cast(cellHitCnt) / static_cast(cellPassCnt); + + if (hitRatio > m_pOccupancyThreshold->GetValue()) + { + *pCell = GridStates_Occupied; + } + else + { + *pCell = GridStates_Free; + } + } + } + + /** + * Update the grid based on the values in m_pCellHitsCnt and m_pCellPassCnt + */ + virtual void Update() + { + assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL); + + // clear grid + Clear(); + + // set occupancy status of cells + kt_int8u* pDataPtr = GetDataPointer(); + kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer(); + kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer(); + + kt_int32u nBytes = GetDataSize(); + for (kt_int32u i = 0; i < nBytes; i++, pDataPtr++, pCellPassCntPtr++, pCellHitCntPtr++) + { + UpdateCell(pDataPtr, *pCellPassCntPtr, *pCellHitCntPtr); + } + } + + /** + * Resizes the grid (deletes all old data) + * @param width + * @param height + */ + virtual void Resize(kt_int32s width, kt_int32s height) + { + Grid::Resize(width, height); + m_pCellPassCnt->Resize(width, height); + m_pCellHitsCnt->Resize(width, height); + } + + protected: + /** + * Counters of number of times a beam passed through a cell + */ + Grid* m_pCellPassCnt; + + /** + * Counters of number of times a beam ended at a cell + */ + Grid* m_pCellHitsCnt; + + private: + /** + * Restrict the copy constructor + */ + OccupancyGrid(const OccupancyGrid&); + + /** + * Restrict the assignment operator + */ + const OccupancyGrid& operator=(const OccupancyGrid&); + + private: + CellUpdater* m_pCellUpdater; + + //////////////////////////////////////////////////////////// + // NOTE: These two values are dependent on the resolution. If the resolution is too small, + // then not many beams will hit the cell! + + // Number of beams that must pass through a cell before it will be considered to be occupied + // or unoccupied. This prevents stray beams from messing up the map. + Parameter* m_pMinPassThrough; + + // Minimum ratio of beams hitting cell to beams passing through cell for cell to be marked as occupied + Parameter* m_pOccupancyThreshold; + }; // OccupancyGrid + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Dataset info + * Contains title, author and other information about the dataset + */ + class DatasetInfo : public Object + { + public: + // @cond EXCLUDE + KARTO_Object(DatasetInfo) + // @endcond + + public: + DatasetInfo() + : Object() + { + m_pTitle = new Parameter("Title", "", GetParameterManager()); + m_pAuthor = new Parameter("Author", "", GetParameterManager()); + m_pDescription = new Parameter("Description", "", GetParameterManager()); + m_pCopyright = new Parameter("Copyright", "", GetParameterManager()); + } + + virtual ~DatasetInfo() + { + } + + public: + /** + * Dataset title + */ + const std::string& GetTitle() const + { + return m_pTitle->GetValue(); + } + + /** + * Dataset author(s) + */ + const std::string& GetAuthor() const + { + return m_pAuthor->GetValue(); + } + + /** + * Dataset description + */ + const std::string& GetDescription() const + { + return m_pDescription->GetValue(); + } + + /** + * Dataset copyrights + */ + const std::string& GetCopyright() const + { + return m_pCopyright->GetValue(); + } + + /** + * Serialization: class DatasetInfo + */ + + private: + DatasetInfo(const DatasetInfo&); + const DatasetInfo& operator=(const DatasetInfo&); + + private: + Parameter* m_pTitle; + Parameter* m_pAuthor; + Parameter* m_pDescription; + Parameter* m_pCopyright; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object); + ar & BOOST_SERIALIZATION_NVP(*m_pTitle); + ar & BOOST_SERIALIZATION_NVP(*m_pAuthor); + ar & BOOST_SERIALIZATION_NVP(*m_pDescription); + ar & BOOST_SERIALIZATION_NVP(*m_pCopyright); + } + }; // class DatasetInfo + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Karto dataset. Stores LaserRangeFinders and LocalizedRangeScans and manages memory of allocated LaserRangeFinders + * and LocalizedRangeScans + */ + class Dataset + { + public: + /** + * Default constructor + */ + Dataset() + : m_pDatasetInfo(NULL) + { + } + + /** + * Destructor + */ + virtual ~Dataset() + { + Clear(); + } + + public: + /** + * Adds object to this dataset + * @param pObject + */ + void Add(Object* pObject, kt_bool overrideSensorName = false) + { + if (pObject != NULL) + { + if (dynamic_cast(pObject)) + { + Sensor* pSensor = dynamic_cast(pObject); + if (pSensor != NULL) + { + m_SensorNameLookup[pSensor->GetName()] = pSensor; + + karto::SensorManager::GetInstance()->RegisterSensor(pSensor, overrideSensorName); + } + + m_Objects.push_back(pObject); + } + else if (dynamic_cast(pObject)) + { + SensorData* pSensorData = dynamic_cast(pObject); + m_Objects.push_back(pSensorData); + } + else if (dynamic_cast(pObject)) + { + m_pDatasetInfo = dynamic_cast(pObject); + } + else + { + m_Objects.push_back(pObject); + } + } + } + + /** + * Get sensor states + * @return sensor state + */ + inline const ObjectVector& GetObjects() const + { + return m_Objects; + } + + /** + * Get dataset info + * @return dataset info + */ + inline DatasetInfo* GetDatasetInfo() + { + return m_pDatasetInfo; + } + + /** + * Delete all stored data + */ + virtual void Clear() + { + for (std::map::iterator iter = m_SensorNameLookup.begin(); iter != m_SensorNameLookup.end(); ++iter) + { + karto::SensorManager::GetInstance()->UnregisterSensor(iter->second); + } + + forEach(ObjectVector, &m_Objects) + { + if(*iter) + { + delete *iter; + *iter = NULL; + } + } + + m_Objects.clear(); + + if (m_pDatasetInfo != NULL) + { + delete m_pDatasetInfo; + m_pDatasetInfo = NULL; + } + } + + private: + std::map m_SensorNameLookup; + ObjectVector m_Objects; + DatasetInfo* m_pDatasetInfo; + /** + * Serialization: class Dataset + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + std::cout<<"**Serializing Dataset**\n"; + std::cout<<"Dataset <- m_SensorNameLookup\n"; + ar & BOOST_SERIALIZATION_NVP(m_SensorNameLookup); + std::cout<<"Dataset <- m_Objects\n"; + ar & BOOST_SERIALIZATION_NVP(m_Objects); + std::cout<<"Dataset <- m_pDatasetInfo\n"; + ar & BOOST_SERIALIZATION_NVP(m_pDatasetInfo); + std::cout<<"**Finished serializing Dataset**\n"; + } + + }; // Dataset + BOOST_SERIALIZATION_ASSUME_ABSTRACT(Dataset) + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * An array that can be resized as long as the size + * does not exceed the initial capacity + */ + class LookupArray + { + public: + /** + * Constructs lookup array + */ + LookupArray() + : m_pArray(NULL) + , m_Capacity(0) + , m_Size(0) + { + } + + /** + * Destructor + */ + virtual ~LookupArray() + { + assert(m_pArray != NULL); + + delete[] m_pArray; + m_pArray = NULL; + } + + public: + /** + * Clear array + */ + void Clear() + { + memset(m_pArray, 0, sizeof(kt_int32s) * m_Capacity); + } + + /** + * Gets size of array + * @return array size + */ + kt_int32u GetSize() const + { + return m_Size; + } + + /** + * Sets size of array (resize if not big enough) + * @param size + */ + void SetSize(kt_int32u size) + { + assert(size != 0); + + if (size > m_Capacity) + { + if (m_pArray != NULL) + { + delete [] m_pArray; + } + m_Capacity = size; + m_pArray = new kt_int32s[m_Capacity]; + } + + m_Size = size; + } + + /** + * Gets reference to value at given index + * @param index + * @return reference to value at index + */ + inline kt_int32s& operator [] (kt_int32u index) + { + assert(index < m_Size); + + return m_pArray[index]; + } + + /** + * Gets value at given index + * @param index + * @return value at index + */ + inline kt_int32s operator [] (kt_int32u index) const + { + assert(index < m_Size); + + return m_pArray[index]; + } + + /** + * Gets array pointer + * @return array pointer + */ + inline kt_int32s* GetArrayPointer() + { + return m_pArray; + } + + /** + * Gets array pointer + * @return array pointer + */ + inline kt_int32s* GetArrayPointer() const + { + return m_pArray; + } + + private: + kt_int32s* m_pArray; + kt_int32u m_Capacity; + kt_int32u m_Size; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Capacity); + ar & BOOST_SERIALIZATION_NVP(m_Size); + if (Archive::is_loading::value) + { + m_pArray = new kt_int32s[m_Capacity]; + } + ar & boost::serialization::make_array(m_pArray, m_Capacity); + + + } + }; // LookupArray + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Create lookup tables for point readings at varying angles in grid. + * For each angle, grid indexes are calculated for each range reading. + * This is to speed up finding best angle/position for a localized range scan + * + * Used heavily in mapper and localizer. + * + * In the localizer, this is a huge speed up for calculating possible position. For each particle, + * a probability is calculated. The range scan is the same, but all grid indexes at all possible angles are + * calculated. So when calculating the particle probability at a specific angle, the index table is used + * to look up probability in probability grid! + * + */ + template + class GridIndexLookup + { + public: + /** + * Construct a GridIndexLookup with a grid + * @param pGrid + */ + GridIndexLookup() + { + } + GridIndexLookup(Grid* pGrid) + : m_pGrid(pGrid) + , m_Capacity(0) + , m_Size(0) + , m_ppLookupArray(NULL) + { + } + + /** + * Destructor + */ + virtual ~GridIndexLookup() + { + DestroyArrays(); + } + + public: + /** + * Gets the lookup array for a particular angle index + * @param index + * @return lookup array + */ + const LookupArray* GetLookupArray(kt_int32u index) const + { + assert(math::IsUpTo(index, m_Size)); + + return m_ppLookupArray[index]; + } + + /** + * Get angles + * @return std::vector& angles + */ + const std::vector& GetAngles() const + { + return m_Angles; + } + + /** + * Compute lookup table of the points of the given scan for the given angular space + * @param pScan the scan + * @param angleCenter + * @param angleOffset computes lookup arrays for the angles within this offset around angleStart + * @param angleResolution how fine a granularity to compute lookup arrays in the angular space + */ + void ComputeOffsets(LocalizedRangeScan* pScan, + kt_double angleCenter, + kt_double angleOffset, + kt_double angleResolution) + { + assert(angleOffset != 0.0); + assert(angleResolution != 0.0); + + kt_int32u nAngles = static_cast(math::Round(angleOffset * 2.0 / angleResolution) + 1); + SetSize(nAngles); + + ////////////////////////////////////////////////////// + // convert points into local coordinates of scan pose + + const PointVectorDouble& rPointReadings = pScan->GetPointReadings(); + + // compute transform to scan pose + Transform transform(pScan->GetSensorPose()); + + Pose2Vector localPoints; + const_forEach(PointVectorDouble, &rPointReadings) + { + // do inverse transform to get points in local coordinates + Pose2 vec = transform.InverseTransformPose(Pose2(*iter, 0.0)); + localPoints.push_back(vec); + } + + ////////////////////////////////////////////////////// + // create lookup array for different angles + kt_double angle = 0.0; + kt_double startAngle = angleCenter - angleOffset; + for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++) + { + angle = startAngle + angleIndex * angleResolution; + ComputeOffsets(angleIndex, angle, localPoints, pScan); + } + // assert(math::DoubleEqual(angle, angleCenter + angleOffset)); + } + + private: + /** + * Compute lookup value of points for given angle + * @param angleIndex + * @param angle + * @param rLocalPoints + */ + void ComputeOffsets(kt_int32u angleIndex, kt_double angle, const Pose2Vector& rLocalPoints, LocalizedRangeScan* pScan) + { + m_ppLookupArray[angleIndex]->SetSize(static_cast(rLocalPoints.size())); + m_Angles.at(angleIndex) = angle; + + // set up point array by computing relative offsets to points readings + // when rotated by given angle + + const Vector2& rGridOffset = m_pGrid->GetCoordinateConverter()->GetOffset(); + + kt_double cosine = cos(angle); + kt_double sine = sin(angle); + + kt_int32u readingIndex = 0; + + kt_int32s* pAngleIndexPointer = m_ppLookupArray[angleIndex]->GetArrayPointer(); + + kt_double maxRange = pScan->GetLaserRangeFinder()->GetMaximumRange(); + + const_forEach(Pose2Vector, &rLocalPoints) + { + const Vector2& rPosition = iter->GetPosition(); + + if (std::isnan(pScan->GetRangeReadings()[readingIndex]) || std::isinf(pScan->GetRangeReadings()[readingIndex])) + { + pAngleIndexPointer[readingIndex] = INVALID_SCAN; + readingIndex++; + continue; + } + + + // counterclockwise rotation and that rotation is about the origin (0, 0). + Vector2 offset; + offset.SetX(cosine * rPosition.GetX() - sine * rPosition.GetY()); + offset.SetY(sine * rPosition.GetX() + cosine * rPosition.GetY()); + + // have to compensate for the grid offset when getting the grid index + Vector2 gridPoint = m_pGrid->WorldToGrid(offset + rGridOffset); + + // use base GridIndex to ignore ROI + kt_int32s lookupIndex = m_pGrid->Grid::GridIndex(gridPoint, false); + + pAngleIndexPointer[readingIndex] = lookupIndex; + + readingIndex++; + } + assert(readingIndex == rLocalPoints.size()); + } + + /** + * Sets size of lookup table (resize if not big enough) + * @param size + */ + void SetSize(kt_int32u size) + { + assert(size != 0); + + if (size > m_Capacity) + { + if (m_ppLookupArray != NULL) + { + DestroyArrays(); + } + + m_Capacity = size; + m_ppLookupArray = new LookupArray*[m_Capacity]; + for (kt_int32u i = 0; i < m_Capacity; i++) + { + m_ppLookupArray[i] = new LookupArray(); + } + } + + m_Size = size; + + m_Angles.resize(size); + } + + /** + * Delete the arrays + */ + void DestroyArrays() + { + if (m_ppLookupArray) + { + for (kt_int32u i = 0; i < m_Capacity; i++) + { + delete m_ppLookupArray[i]; + } + } + if (m_ppLookupArray) + { + delete[] m_ppLookupArray; + m_ppLookupArray = NULL; + } + } + + private: + Grid* m_pGrid; + + kt_int32u m_Capacity; + kt_int32u m_Size; + + LookupArray **m_ppLookupArray; + + // for sanity check + std::vector m_Angles; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_pGrid); + ar & BOOST_SERIALIZATION_NVP(m_Capacity); + ar & BOOST_SERIALIZATION_NVP(m_Size); + ar & BOOST_SERIALIZATION_NVP(m_Angles); + if (Archive::is_loading::value) + { + m_ppLookupArray = new LookupArray*[m_Capacity]; + for (kt_int32u i = 0; i < m_Capacity; i++) + { + m_ppLookupArray[i] = new LookupArray(); + } + } + ar & boost::serialization::make_array(m_ppLookupArray, m_Capacity); + } + }; // class GridIndexLookup + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + inline Pose2::Pose2(const Pose3& rPose) + : m_Position(rPose.GetPosition().GetX(), rPose.GetPosition().GetY()) + { + kt_double t1, t2; + + // calculates heading from orientation + rPose.GetOrientation().ToEulerAngles(m_Heading, t1, t2); + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + // @cond EXCLUDE + + template + inline void Object::SetParameter(const std::string& rName, T value) + { + AbstractParameter* pParameter = GetParameter(rName); + if (pParameter != NULL) + { + std::stringstream stream; + stream << value; + pParameter->SetValueFromString(stream.str()); + } + else + { + throw Exception("Parameter does not exist: " + rName); + } + } + + template<> + inline void Object::SetParameter(const std::string& rName, kt_bool value) + { + AbstractParameter* pParameter = GetParameter(rName); + if (pParameter != NULL) + { + pParameter->SetValueFromString(value ? "true" : "false"); + } + else + { + throw Exception("Parameter does not exist: " + rName); + } + } + + // @endcond + + /*@}*/ +}; // namespace karto + +BOOST_CLASS_EXPORT_KEY(karto::NonCopyable); +BOOST_CLASS_EXPORT_KEY(karto::Object); +BOOST_CLASS_EXPORT_KEY(karto::Sensor); +BOOST_CLASS_EXPORT_KEY(karto::Name); +BOOST_CLASS_EXPORT_KEY(karto::SensorData); +BOOST_CLASS_EXPORT_KEY(karto::LocalizedRangeScan); +BOOST_CLASS_EXPORT_KEY(karto::LaserRangeScan); +BOOST_CLASS_EXPORT_KEY(karto::LaserRangeFinder); +BOOST_CLASS_EXPORT_KEY(karto::CustomData); +BOOST_CLASS_EXPORT_KEY(karto::Module); +BOOST_CLASS_EXPORT_KEY(karto::Rectangle2); +BOOST_CLASS_EXPORT_KEY(karto::CoordinateConverter); +BOOST_CLASS_EXPORT_KEY(karto::Dataset); +BOOST_CLASS_EXPORT_KEY(karto::SensorManager); +BOOST_CLASS_EXPORT_KEY(karto::Size2); +BOOST_CLASS_EXPORT_KEY(karto::GridIndexLookup); +BOOST_CLASS_EXPORT_KEY(karto::LookupArray); +BOOST_CLASS_EXPORT_KEY(karto::AbstractParameter); +BOOST_CLASS_EXPORT_KEY(karto::ParameterEnum); +BOOST_CLASS_EXPORT_KEY(karto::Parameters); +BOOST_CLASS_EXPORT_KEY(karto::ParameterManager); +BOOST_CLASS_EXPORT_KEY(karto::Parameter); +BOOST_CLASS_EXPORT_KEY(karto::Parameter); +BOOST_CLASS_EXPORT_KEY(karto::Parameter); +BOOST_CLASS_EXPORT_KEY(karto::Parameter); +BOOST_CLASS_EXPORT_KEY(karto::Parameter); +BOOST_CLASS_EXPORT_KEY(karto::Parameter); + +#endif // OPEN_KARTO_KARTO_H diff --git a/lib/KartoSDK-slam_toolbox/include/open_karto/Macros.h b/lib/KartoSDK-slam_toolbox/include/open_karto/Macros.h new file mode 100644 index 000000000..d2575813a --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/include/open_karto/Macros.h @@ -0,0 +1,127 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#ifndef OPEN_KARTO_MACROS_H +#define OPEN_KARTO_MACROS_H + +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// + +/** + * Karto defines for handling deprecated code + */ +#ifndef KARTO_DEPRECATED +# if defined(__GNUC__) && (__GNUC__ >= 4 || (__GNUC__ == 3 && __GNUC_MINOR__>=1)) +# define KARTO_DEPRECATED __attribute__((deprecated)) +# elif defined(__INTEL) || defined(_MSC_VER) +# define KARTO_DEPRECATED __declspec(deprecated) +# else +# define KARTO_DEPRECATED +# endif +#endif + +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// + +/** + * Karto defines for windows dynamic build + */ +#if defined(_MSC_VER) || defined(__CYGWIN__) || defined(__MINGW32__) || defined( __BCPLUSPLUS__) || defined( __MWERKS__) +# if defined( _LIB ) || defined( KARTO_STATIC ) || defined( STATIC_BUILD ) +# define KARTO_EXPORT +# else +# ifdef KARTO_DYNAMIC +# define KARTO_EXPORT __declspec(dllexport) +# else +# define KARTO_EXPORT __declspec(dllimport) +# endif // KARTO_DYNAMIC +# endif +#else +# define KARTO_EXPORT +#endif + +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// + +/** + * Helper defines for std iterator loops + */ +#define forEach( listtype, list ) \ + for ( listtype::iterator iter = (list)->begin(); iter != (list)->end(); ++iter ) + +#define forEachAs( listtype, list, iter ) \ + for ( listtype::iterator iter = (list)->begin(); iter != (list)->end(); ++iter ) + +#define const_forEach( listtype, list ) \ + for ( listtype::const_iterator iter = (list)->begin(); iter != (list)->end(); ++iter ) + +#define const_forEachAs( listtype, list, iter ) \ + for ( listtype::const_iterator iter = (list)->begin(); iter != (list)->end(); ++iter ) + +#define forEachR( listtype, list ) \ + for ( listtype::reverse_iterator iter = (list)->rbegin(); iter != (list)->rend(); ++iter ) + +#define const_forEachR( listtype, list ) \ + for ( listtype::const_reverse_iterator iter = (list)->rbegin(); iter != (list)->rend(); ++iter ) + + +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// + +/** + * Disable annoying compiler warnings + */ + +#if defined(__INTEL) || defined(_MSC_VER) + +// Disable the warning: 'identifier' : class 'type' needs to have dll-interface to be used by clients of class 'type2' +#pragma warning(disable:4251) + +#endif + +#ifdef __INTEL_COMPILER + +// Disable the warning: conditional expression is constant +#pragma warning(disable:4127) + +// Disable the warning: 'identifier' : unreferenced formal parameter +#pragma warning(disable:4100) + +// remark #383: value copied to temporary, reference to temporary used +#pragma warning(disable:383) + +// remark #981: operands are evaluated in unspecified order +// disabled -> completely pointless if the functions do not have side effects +#pragma warning(disable:981) + +// remark #1418: external function definition with no prior declaration +#pragma warning(disable:1418) + +// remark #1572: floating-point equality and inequality comparisons are unreliable +// disabled -> everyone knows it, the parser passes this problem deliberately to the user +#pragma warning(disable:1572) + +// remark #10121: +#pragma warning(disable:10121) + +#endif // __INTEL_COMPILER + +#endif // OPEN_KARTO_MACROS_H diff --git a/lib/KartoSDK-slam_toolbox/include/open_karto/Mapper.h b/lib/KartoSDK-slam_toolbox/include/open_karto/Mapper.h new file mode 100644 index 000000000..e6e98bbdd --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/include/open_karto/Mapper.h @@ -0,0 +1,2248 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#ifndef OPEN_KARTO_MAPPER_H +#define OPEN_KARTO_MAPPER_H + +#include +#include + +#include + +#include "tbb/parallel_for.h" +#include "tbb/blocked_range.h" +#include "tbb/tbb.h" +#include +#include + +#include + +namespace karto +{ + //////////////////////////////////////////////////////////////////////////////////////// + // Listener classes + + /** + * Abstract class to listen to mapper general messages + */ + class MapperListener + { + public: + /** + * Called with general message + */ + virtual void Info(const std::string& /*rInfo*/) {}; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + } + }; + BOOST_SERIALIZATION_ASSUME_ABSTRACT(MapperListener) + /** + * Abstract class to listen to mapper debug messages + */ + class MapperDebugListener + { + public: + /** + * Called with debug message + */ + virtual void Debug(const std::string& /*rInfo*/) {}; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + } + }; + BOOST_SERIALIZATION_ASSUME_ABSTRACT(MapperDebugListener) + /** + * Abstract class to listen to mapper loop closure messages + */ + class MapperLoopClosureListener : public MapperListener + { + public: + /** + * Called when checking for loop closures + */ + virtual void LoopClosureCheck(const std::string& /*rInfo*/) {}; + + /** + * Called when loop closure is starting + */ + virtual void BeginLoopClosure(const std::string& /*rInfo*/) {}; + + /** + * Called when loop closure is over + */ + virtual void EndLoopClosure(const std::string& /*rInfo*/) {}; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + } + }; // MapperLoopClosureListener + BOOST_SERIALIZATION_ASSUME_ABSTRACT(MapperLoopClosureListener) + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Class for edge labels + */ + class EdgeLabel + { + public: + /** + * Default constructor + */ + EdgeLabel() + { + } + + /** + * Destructor + */ + virtual ~EdgeLabel() + { + } + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + } + }; // EdgeLabel + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + // A LinkInfo object contains the requisite information for the "spring" + // that links two scans together--the pose difference and the uncertainty + // (represented by a covariance matrix). + class LinkInfo : public EdgeLabel + { + public: + /** + * Constructs a link between the given poses + * @param rPose1 + * @param rPose2 + * @param rCovariance + */ + LinkInfo() + { + } + LinkInfo(const Pose2& rPose1, const Pose2& rPose2, const Matrix3& rCovariance) + { + Update(rPose1, rPose2, rCovariance); + } + + /** + * Destructor + */ + virtual ~LinkInfo() + { + } + + public: + /** + * Changes the link information to be the given parameters + * @param rPose1 + * @param rPose2 + * @param rCovariance + */ + void Update(const Pose2& rPose1, const Pose2& rPose2, const Matrix3& rCovariance) + { + m_Pose1 = rPose1; + m_Pose2 = rPose2; + + // transform second pose into the coordinate system of the first pose + Transform transform(rPose1, Pose2()); + m_PoseDifference = transform.TransformPose(rPose2); + + // transform covariance into reference of first pose + Matrix3 rotationMatrix; + rotationMatrix.FromAxisAngle(0, 0, 1, -rPose1.GetHeading()); + + m_Covariance = rotationMatrix * rCovariance * rotationMatrix.Transpose(); + } + + /** + * Gets the first pose + * @return first pose + */ + inline const Pose2& GetPose1() + { + return m_Pose1; + } + + /** + * Gets the second pose + * @return second pose + */ + inline const Pose2& GetPose2() + { + return m_Pose2; + } + + /** + * Gets the pose difference + * @return pose difference + */ + inline const Pose2& GetPoseDifference() + { + return m_PoseDifference; + } + + /** + * Gets the link covariance + * @return link covariance + */ + inline const Matrix3& GetCovariance() + { + return m_Covariance; + } + + private: + Pose2 m_Pose1; + Pose2 m_Pose2; + Pose2 m_PoseDifference; + Matrix3 m_Covariance; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(EdgeLabel); + ar & BOOST_SERIALIZATION_NVP(m_Pose1); + ar & BOOST_SERIALIZATION_NVP(m_Pose2); + ar & BOOST_SERIALIZATION_NVP(m_PoseDifference); + ar & BOOST_SERIALIZATION_NVP(m_Covariance); + } + }; // LinkInfo + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + template + class Edge; + + /** + * Represents an object in a graph + */ + template + class Vertex + { + friend class Edge; + + public: + /** + * Constructs a vertex representing the given object + * @param pObject + */ + Vertex() + { + } + Vertex(T* pObject) + : m_pObject(pObject) + { + } + + /** + * Destructor + */ + virtual ~Vertex() + { + } + + /** + * Gets edges adjacent to this vertex + * @return adjacent edges + */ + inline const std::vector*>& GetEdges() const + { + return m_Edges; + } + + /** + * Gets the object associated with this vertex + * @return the object + */ + inline T* GetObject() const + { + return m_pObject; + } + + /** + * Gets a vector of the vertices adjacent to this vertex + * @return adjacent vertices + */ + std::vector*> GetAdjacentVertices() const + { + std::vector*> vertices; + + const_forEach(typename std::vector*>, &m_Edges) + { + Edge* pEdge = *iter; + + // check both source and target because we have a undirected graph + if (pEdge->GetSource() != this) + { + vertices.push_back(pEdge->GetSource()); + } + + if (pEdge->GetTarget() != this) + { + vertices.push_back(pEdge->GetTarget()); + } + } + + return vertices; + } + + private: + /** + * Adds the given edge to this vertex's edge list + * @param pEdge edge to add + */ + inline void AddEdge(Edge* pEdge) + { + m_Edges.push_back(pEdge); + } + + T* m_pObject; + std::vector*> m_Edges; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_pObject); + ar & BOOST_SERIALIZATION_NVP(m_Edges); + } + }; // Vertex + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Represents an edge in a graph + */ + template + class Edge + { + public: + /** + * Constructs an edge from the source to target vertex + * @param pSource + * @param pTarget + */ + Edge() + { + } + Edge(Vertex* pSource, Vertex* pTarget) + : m_pSource(pSource) + , m_pTarget(pTarget) + , m_pLabel(NULL) + { + m_pSource->AddEdge(this); + m_pTarget->AddEdge(this); + } + + /** + * Destructor + */ + virtual ~Edge() + { + m_pSource = NULL; + m_pTarget = NULL; + + if (m_pLabel != NULL) + { + delete m_pLabel; + m_pLabel = NULL; + } + } + + public: + /** + * Gets the source vertex + * @return source vertex + */ + inline Vertex* GetSource() const + { + return m_pSource; + } + + /** + * Gets the target vertex + * @return target vertex + */ + inline Vertex* GetTarget() const + { + return m_pTarget; + } + + /** + * Gets the link info + * @return link info + */ + inline EdgeLabel* GetLabel() + { + return m_pLabel; + } + + /** + * Sets the link payload + * @param pLabel + */ + inline void SetLabel(EdgeLabel* pLabel) + { + m_pLabel = pLabel; + } + + private: + Vertex* m_pSource; + Vertex* m_pTarget; + EdgeLabel* m_pLabel; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_pSource); + ar & BOOST_SERIALIZATION_NVP(m_pTarget); + ar & BOOST_SERIALIZATION_NVP(m_pLabel); + } + }; // class Edge + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Visitor class + */ + template + class Visitor + { + public: + /** + * Applies the visitor to the vertex + * @param pVertex + * @return true if the visitor accepted the vertex, false otherwise + */ + virtual kt_bool Visit(Vertex* pVertex) = 0; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + } + }; // Visitor + BOOST_SERIALIZATION_ASSUME_ABSTRACT(Visitor) + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + template + class Graph; + + /** + * Graph traversal algorithm + */ + template + class GraphTraversal + { + public: + /** + * Constructs a traverser for the given graph + * @param pGraph + */ + GraphTraversal() + { + } + GraphTraversal(Graph* pGraph) + : m_pGraph(pGraph) + { + } + + /** + * Destructor + */ + virtual ~GraphTraversal() + { + } + + public: + /** + * Traverse the graph starting at the given vertex and applying the visitor to all visited nodes + * @param pStartVertex + * @param pVisitor + */ + virtual std::vector Traverse(Vertex* pStartVertex, Visitor* pVisitor) = 0; + + protected: + /** + * Graph being traversed + */ + Graph* m_pGraph; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_pGraph); + } + }; // GraphTraversal + BOOST_SERIALIZATION_ASSUME_ABSTRACT(GraphTraversal) + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Graph + */ + template + class Graph + { + public: + /** + * Maps names to vector of vertices + */ + typedef std::map*> > VertexMap; + + public: + /** + * Default constructor + */ + Graph() + { + } + + /** + * Destructor + */ + virtual ~Graph() + { + Clear(); + } + + public: + /** + * Adds and indexes the given vertex into the map using the given name + * @param rName + * @param pVertex + */ + inline void AddVertex(const Name& rName, Vertex* pVertex) + { + m_Vertices[rName].push_back(pVertex); + } + + /** + * Adds an edge to the graph + * @param pEdge + */ + inline void AddEdge(Edge* pEdge) + { + m_Edges.push_back(pEdge); + } + + /** + * Deletes the graph data + */ + void Clear() + { + forEachAs(typename VertexMap, &m_Vertices, indexIter) + { + // delete each vertex + forEach(typename std::vector*>, &(indexIter->second)) + { + delete *iter; + } + } + m_Vertices.clear(); + + const_forEach(typename std::vector*>, &m_Edges) + { + delete *iter; + } + m_Edges.clear(); + } + + /** + * Gets the edges of this graph + * @return graph edges + */ + inline const std::vector*>& GetEdges() const + { + return m_Edges; + } + + /** + * Gets the vertices of this graph + * @return graph vertices + */ + inline const VertexMap& GetVertices() const + { + return m_Vertices; + } + + protected: + /** + * Map of names to vector of vertices + */ + VertexMap m_Vertices; + + /** + * Edges of this graph + */ + std::vector*> m_Edges; + /** + * Serialization: class Graph + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + std::cout<<"Graph <- m_Edges; "; + ar & BOOST_SERIALIZATION_NVP(m_Edges); + std::cout<<"Graph <- m_Vertices\n"; + ar & BOOST_SERIALIZATION_NVP(m_Vertices); + } + }; // Graph + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + class Mapper; + class ScanMatcher; + + /** + * Graph for graph SLAM algorithm + */ + class KARTO_EXPORT MapperGraph : public Graph + { + public: + /** + * Graph for graph SLAM + * @param pMapper + * @param rangeThreshold + */ + MapperGraph(Mapper* pMapper, kt_double rangeThreshold); + MapperGraph() + { + } + /** + * Destructor + */ + virtual ~MapperGraph(); + + public: + /** + * Adds a vertex representing the given scan to the graph + * @param pScan + */ + void AddVertex(LocalizedRangeScan* pScan); + + /** + * Creates an edge between the source scan vertex and the target scan vertex if it + * does not already exist; otherwise return the existing edge + * @param pSourceScan + * @param pTargetScan + * @param rIsNewEdge set to true if the edge is new + * @return edge between source and target scan vertices + */ + Edge* AddEdge(LocalizedRangeScan* pSourceScan, + LocalizedRangeScan* pTargetScan, + kt_bool& rIsNewEdge); + + /** + * Link scan to last scan and nearby chains of scans + * @param pScan + * @param rCovariance uncertainty of match + */ + void AddEdges(LocalizedRangeScan* pScan, const Matrix3& rCovariance); + + /** + * Tries to close a loop using the given scan with the scans from the given device + * @param pScan + * @param rSensorName + */ + kt_bool TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName); + + /** + * Optimizes scan poses + */ + void CorrectPoses(); + + /** + * Find "nearby" (no further than given distance away) scans through graph links + * @param pScan + * @param maxDistance + */ + LocalizedRangeScanVector FindNearLinkedScans(LocalizedRangeScan* pScan, kt_double maxDistance); + + /** + * Gets the graph's scan matcher + * @return scan matcher + */ + inline ScanMatcher* GetLoopScanMatcher() const + { + return m_pLoopScanMatcher; + } + + private: + /** + * Gets the vertex associated with the given scan + * @param pScan + * @return vertex of scan + */ + inline Vertex* GetVertex(LocalizedRangeScan* pScan) + { + return m_Vertices[pScan->GetSensorName()][pScan->GetStateId()]; + } + + /** + * Finds the closest scan in the vector to the given pose + * @param rScans + * @param rPose + */ + LocalizedRangeScan* GetClosestScanToPose(const LocalizedRangeScanVector& rScans, const Pose2& rPose) const; + + /** + * Adds an edge between the two scans and labels the edge with the given mean and covariance + * @param pFromScan + * @param pToScan + * @param rMean + * @param rCovariance + */ + void LinkScans(LocalizedRangeScan* pFromScan, + LocalizedRangeScan* pToScan, + const Pose2& rMean, + const Matrix3& rCovariance); + + /** + * Find nearby chains of scans and link them to scan if response is high enough + * @param pScan + * @param rMeans + * @param rCovariances + */ + void LinkNearChains(LocalizedRangeScan* pScan, Pose2Vector& rMeans, std::vector& rCovariances); + + /** + * Link the chain of scans to the given scan by finding the closest scan in the chain to the given scan + * @param rChain + * @param pScan + * @param rMean + * @param rCovariance + */ + void LinkChainToScan(const LocalizedRangeScanVector& rChain, + LocalizedRangeScan* pScan, + const Pose2& rMean, + const Matrix3& rCovariance); + + /** + * Find chains of scans that are close to given scan + * @param pScan + * @return chains of scans + */ + std::vector FindNearChains(LocalizedRangeScan* pScan); + + /** + * Compute mean of poses weighted by covariances + * @param rMeans + * @param rCovariances + * @return weighted mean + */ + Pose2 ComputeWeightedMean(const Pose2Vector& rMeans, const std::vector& rCovariances) const; + + /** + * Tries to find a chain of scan from the given device starting at the + * given scan index that could possibly close a loop with the given scan + * @param pScan + * @param rSensorName + * @param rStartNum + * @return chain that can possibly close a loop with given scan + */ + LocalizedRangeScanVector FindPossibleLoopClosure(LocalizedRangeScan* pScan, + const Name& rSensorName, + kt_int32u& rStartNum); + + private: + /** + * Mapper of this graph + */ + Mapper* m_pMapper; + + /** + * Scan matcher for loop closures + */ + ScanMatcher* m_pLoopScanMatcher; + + /** + * Traversal algorithm to find near linked scans + */ + GraphTraversal* m_pTraversal; + + /** + * Serialization: class MapperGraph + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + std::cout<<"MapperGraph <- Graph; "; + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Graph); + std::cout<<"MapperGraph <- m_pMapper; "; + ar & BOOST_SERIALIZATION_NVP(m_pMapper); + std::cout<<"MapperGraph <- m_pLoopScanMatcher; "; + ar & BOOST_SERIALIZATION_NVP(m_pLoopScanMatcher); + std::cout<<"MapperGraph <- m_pTraversal\n"; + ar & BOOST_SERIALIZATION_NVP(m_pTraversal); + } + + }; // MapperGraph + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Graph optimization algorithm + */ + class ScanSolver + { + public: + /** + * Vector of id-pose pairs + */ + typedef std::vector > IdPoseVector; + + /** + * Default constructor + */ + ScanSolver() + { + } + + /** + * Destructor + */ + virtual ~ScanSolver() + { + } + + public: + /** + * Solve! + */ + virtual void Compute() = 0; + + /** + * Get corrected poses after optimization + * @return optimized poses + */ + virtual const IdPoseVector& GetCorrections() const = 0; + + /** + * Adds a node to the solver + */ + virtual void AddNode(Vertex* /*pVertex*/) + { + } + + /** + * Removes a node from the solver + */ + virtual void RemoveNode(kt_int32s /*id*/) + { + } + + /** + * Adds a constraint to the solver + */ + virtual void AddConstraint(Edge* /*pEdge*/) + { + } + + /** + * Removes a constraint from the solver + */ + virtual void RemoveConstraint(kt_int32s /*sourceId*/, kt_int32s /*targetId*/) + { + } + + /** + * Resets the solver + */ + virtual void Clear(){}; + + /** + * Get graph stored + */ + virtual void getGraph(std::vector& ag) {}; + + /** + * Modify a node's pose + */ + virtual void ModifyNode(const int& unique_id, Eigen::Vector3d pose) + { + std::cout << "ModifyNode method not implemented for this solver type. Manual loop closure unavailable." << std::endl; + }; + /** + * Get node's yaw + */ + virtual void GetNodeOrientation(const int& unique_id, double& pose) + { + std::cout << "GetNodeOrientation method not implemented for this solver type." << std::endl; + }; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + } + }; // ScanSolver + BOOST_SERIALIZATION_ASSUME_ABSTRACT(ScanSolver) + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Implementation of a correlation grid used for scan matching + */ + class CorrelationGrid : public Grid + { + public: + /** + * Destructor + */ + virtual ~CorrelationGrid() + { + if (m_pKernel) + { + delete [] m_pKernel; + } + + } + + public: + /** + * Create a correlation grid of given size and parameters + * @param width + * @param height + * @param resolution + * @param smearDeviation + * @return correlation grid + */ + CorrelationGrid() + { + } + static CorrelationGrid* CreateGrid(kt_int32s width, + kt_int32s height, + kt_double resolution, + kt_double smearDeviation) + { + assert(resolution != 0.0); + + // +1 in case of roundoff + kt_int32u borderSize = GetHalfKernelSize(smearDeviation, resolution) + 1; + + CorrelationGrid* pGrid = new CorrelationGrid(width, height, borderSize, resolution, smearDeviation); + + return pGrid; + } + + /** + * Gets the index into the data pointer of the given grid coordinate + * @param rGrid + * @param boundaryCheck + * @return grid index + */ + virtual kt_int32s GridIndex(const Vector2& rGrid, kt_bool boundaryCheck = true) const + { + kt_int32s x = rGrid.GetX() + m_Roi.GetX(); + kt_int32s y = rGrid.GetY() + m_Roi.GetY(); + + return Grid::GridIndex(Vector2(x, y), boundaryCheck); + } + + /** + * Get the Region Of Interest (ROI) + * @return region of interest + */ + inline const Rectangle2& GetROI() const + { + return m_Roi; + } + + /** + * Sets the Region Of Interest (ROI) + * @param roi + */ + inline void SetROI(const Rectangle2& roi) + { + m_Roi = roi; + } + + /** + * Smear cell if the cell at the given point is marked as "occupied" + * @param rGridPoint + */ + inline void SmearPoint(const Vector2& rGridPoint) + { + assert(m_pKernel != NULL); + + int gridIndex = GridIndex(rGridPoint); + if (GetDataPointer()[gridIndex] != GridStates_Occupied) + { + return; + } + + kt_int32s halfKernel = m_KernelSize / 2; + + // apply kernel + for (kt_int32s j = -halfKernel; j <= halfKernel; j++) + { + kt_int8u* pGridAdr = GetDataPointer(Vector2(rGridPoint.GetX(), rGridPoint.GetY() + j)); + + kt_int32s kernelConstant = (halfKernel) + m_KernelSize * (j + halfKernel); + + // if a point is on the edge of the grid, there is no problem + // with running over the edge of allowable memory, because + // the grid has margins to compensate for the kernel size + for (kt_int32s i = -halfKernel; i <= halfKernel; i++) + { + kt_int32s kernelArrayIndex = i + kernelConstant; + + kt_int8u kernelValue = m_pKernel[kernelArrayIndex]; + if (kernelValue > pGridAdr[i]) + { + // kernel value is greater, so set it to kernel value + pGridAdr[i] = kernelValue; + } + } + } + } + + protected: + /** + * Constructs a correlation grid of given size and parameters + * @param width + * @param height + * @param borderSize + * @param resolution + * @param smearDeviation + */ + CorrelationGrid(kt_int32u width, kt_int32u height, kt_int32u borderSize, + kt_double resolution, kt_double smearDeviation) + : Grid(width + borderSize * 2, height + borderSize * 2) + , m_SmearDeviation(smearDeviation) + , m_pKernel(NULL) + { + GetCoordinateConverter()->SetScale(1.0 / resolution); + + // setup region of interest + m_Roi = Rectangle2(borderSize, borderSize, width, height); + + // calculate kernel + CalculateKernel(); + } + + /** + * Sets up the kernel for grid smearing. + */ + virtual void CalculateKernel() + { + kt_double resolution = GetResolution(); + + assert(resolution != 0.0); + assert(m_SmearDeviation != 0.0); + + // min and max distance deviation for smearing; + // will smear for two standard deviations, so deviation must be at least 1/2 of the resolution + const kt_double MIN_SMEAR_DISTANCE_DEVIATION = 0.5 * resolution; + const kt_double MAX_SMEAR_DISTANCE_DEVIATION = 10 * resolution; + + // check if given value too small or too big + if (!math::InRange(m_SmearDeviation, MIN_SMEAR_DISTANCE_DEVIATION, MAX_SMEAR_DISTANCE_DEVIATION)) + { + std::stringstream error; + error << "Mapper Error: Smear deviation too small: Must be between " + << MIN_SMEAR_DISTANCE_DEVIATION + << " and " + << MAX_SMEAR_DISTANCE_DEVIATION; + throw std::runtime_error(error.str()); + } + + // NOTE: Currently assumes a two-dimensional kernel + + // +1 for center + m_KernelSize = 2 * GetHalfKernelSize(m_SmearDeviation, resolution) + 1; + + // allocate kernel + m_pKernel = new kt_int8u[m_KernelSize * m_KernelSize]; + if (m_pKernel == NULL) + { + throw std::runtime_error("Unable to allocate memory for kernel!"); + } + + // calculate kernel + kt_int32s halfKernel = m_KernelSize / 2; + for (kt_int32s i = -halfKernel; i <= halfKernel; i++) + { + for (kt_int32s j = -halfKernel; j <= halfKernel; j++) + { +#ifdef WIN32 + kt_double distanceFromMean = _hypot(i * resolution, j * resolution); +#else + kt_double distanceFromMean = hypot(i * resolution, j * resolution); +#endif + kt_double z = exp(-0.5 * pow(distanceFromMean / m_SmearDeviation, 2)); + + kt_int32u kernelValue = static_cast(math::Round(z * GridStates_Occupied)); + assert(math::IsUpTo(kernelValue, static_cast(255))); + + int kernelArrayIndex = (i + halfKernel) + m_KernelSize * (j + halfKernel); + m_pKernel[kernelArrayIndex] = static_cast(kernelValue); + } + } + } + + /** + * Computes the kernel half-size based on the smear distance and the grid resolution. + * Computes to two standard deviations to get 95% region and to reduce aliasing. + * @param smearDeviation + * @param resolution + * @return kernel half-size based on the parameters + */ + static kt_int32s GetHalfKernelSize(kt_double smearDeviation, kt_double resolution) + { + assert(resolution != 0.0); + + return static_cast(math::Round(2.0 * smearDeviation / resolution)); + } + + private: + /** + * The point readings are smeared by this value in X and Y to create a smoother response. + * Default value is 0.03 meters. + */ + kt_double m_SmearDeviation; + + // Size of one side of the kernel + kt_int32s m_KernelSize; + + // Cached kernel for smearing + kt_int8u* m_pKernel; + + // region of interest + Rectangle2 m_Roi; + /** + * Serialization: class CorrelationGrid + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Grid); + ar & BOOST_SERIALIZATION_NVP(m_SmearDeviation); + ar & BOOST_SERIALIZATION_NVP(m_KernelSize); + if (Archive::is_loading::value) + { + m_pKernel = new kt_int8u[m_KernelSize * m_KernelSize]; + } + ar & boost::serialization::make_array(m_pKernel, m_KernelSize * m_KernelSize); + ar & BOOST_SERIALIZATION_NVP(m_Roi); + } + }; // CorrelationGrid + BOOST_SERIALIZATION_ASSUME_ABSTRACT(CorrelationGrid) + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Scan matcher + */ + class KARTO_EXPORT ScanMatcher + { + public: + ScanMatcher() + { + } + /** + * Destructor + */ + virtual ~ScanMatcher(); + + public: + /** + * Parallelize scan matching + */ + void operator() (const kt_double& y) const; + + /** + * Create a scan matcher with the given parameters + */ + static ScanMatcher* Create(Mapper* pMapper, + kt_double searchSize, + kt_double resolution, + kt_double smearDeviation, + kt_double rangeThreshold); + + /** + * Match given scan against set of scans + * @param pScan scan being scan-matched + * @param rBaseScans set of scans whose points will mark cells in grid as being occupied + * @param rMean output parameter of mean (best pose) of match + * @param rCovariance output parameter of covariance of match + * @param doPenalize whether to penalize matches further from the search center + * @param doRefineMatch whether to do finer-grained matching if coarse match is good (default is true) + * @return strength of response + */ + kt_double MatchScan(LocalizedRangeScan* pScan, + const LocalizedRangeScanVector& rBaseScans, + Pose2& rMean, Matrix3& rCovariance, + kt_bool doPenalize = true, + kt_bool doRefineMatch = true); + + /** + * Finds the best pose for the scan centering the search in the correlation grid + * at the given pose and search in the space by the vector and angular offsets + * in increments of the given resolutions + * @param pScan scan to match against correlation grid + * @param rSearchCenter the center of the search space + * @param rSearchSpaceOffset searches poses in the area offset by this vector around search center + * @param rSearchSpaceResolution how fine a granularity to search in the search space + * @param searchAngleOffset searches poses in the angles offset by this angle around search center + * @param searchAngleResolution how fine a granularity to search in the angular search space + * @param doPenalize whether to penalize matches further from the search center + * @param rMean output parameter of mean (best pose) of match + * @param rCovariance output parameter of covariance of match + * @param doingFineMatch whether to do a finer search after coarse search + * @return strength of response + */ + kt_double CorrelateScan(LocalizedRangeScan* pScan, + const Pose2& rSearchCenter, + const Vector2& rSearchSpaceOffset, + const Vector2& rSearchSpaceResolution, + kt_double searchAngleOffset, + kt_double searchAngleResolution, + kt_bool doPenalize, + Pose2& rMean, + Matrix3& rCovariance, + kt_bool doingFineMatch); + + /** + * Computes the positional covariance of the best pose + * @param rBestPose + * @param bestResponse + * @param rSearchCenter + * @param rSearchSpaceOffset + * @param rSearchSpaceResolution + * @param searchAngleResolution + * @param rCovariance + */ + void ComputePositionalCovariance(const Pose2& rBestPose, + kt_double bestResponse, + const Pose2& rSearchCenter, + const Vector2& rSearchSpaceOffset, + const Vector2& rSearchSpaceResolution, + kt_double searchAngleResolution, + Matrix3& rCovariance); + + /** + * Computes the angular covariance of the best pose + * @param rBestPose + * @param bestResponse + * @param rSearchCenter + * @param searchAngleOffset + * @param searchAngleResolution + * @param rCovariance + */ + void ComputeAngularCovariance(const Pose2& rBestPose, + kt_double bestResponse, + const Pose2& rSearchCenter, + kt_double searchAngleOffset, + kt_double searchAngleResolution, + Matrix3& rCovariance); + + /** + * Gets the correlation grid data (for debugging) + * @return correlation grid + */ + inline CorrelationGrid* GetCorrelationGrid() const + { + return m_pCorrelationGrid; + } + + private: + /** + * Marks cells where scans' points hit as being occupied + * @param rScans scans whose points will mark cells in grid as being occupied + * @param viewPoint do not add points that belong to scans "opposite" the view point + */ + void AddScans(const LocalizedRangeScanVector& rScans, Vector2 viewPoint); + + /** + * Marks cells where scans' points hit as being occupied. Can smear points as they are added. + * @param pScan scan whose points will mark cells in grid as being occupied + * @param viewPoint do not add points that belong to scans "opposite" the view point + * @param doSmear whether the points will be smeared + */ + void AddScan(LocalizedRangeScan* pScan, const Vector2& rViewPoint, kt_bool doSmear = true); + + /** + * Compute which points in a scan are on the same side as the given viewpoint + * @param pScan + * @param rViewPoint + * @return points on the same side + */ + PointVectorDouble FindValidPoints(LocalizedRangeScan* pScan, const Vector2& rViewPoint) const; + + /** + * Get response at given position for given rotation (only look up valid points) + * @param angleIndex + * @param gridPositionIndex + * @return response + */ + kt_double GetResponse(kt_int32u angleIndex, kt_int32s gridPositionIndex) const; + + protected: + /** + * Default constructor + */ + ScanMatcher(Mapper* pMapper) + : m_pMapper(pMapper) + , m_pCorrelationGrid(NULL) + , m_pSearchSpaceProbs(NULL) + , m_pGridLookup(NULL) + { + } + + private: + Mapper* m_pMapper; + CorrelationGrid* m_pCorrelationGrid; + Grid* m_pSearchSpaceProbs; + GridIndexLookup* m_pGridLookup; + std::pair* m_pPoseResponse; + std::vector m_xPoses; + std::vector m_yPoses; + Pose2 m_rSearchCenter; + kt_double m_searchAngleOffset; + kt_int32u m_nAngles; + kt_double m_searchAngleResolution; + kt_bool m_doPenalize; + + /** + * Serialization: class ScanMatcher + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_pMapper); + ar & BOOST_SERIALIZATION_NVP(m_pCorrelationGrid); + ar & BOOST_SERIALIZATION_NVP(m_pSearchSpaceProbs); + ar & BOOST_SERIALIZATION_NVP(m_pGridLookup); + ar & BOOST_SERIALIZATION_NVP(m_xPoses); + ar & BOOST_SERIALIZATION_NVP(m_yPoses); + ar & BOOST_SERIALIZATION_NVP(m_rSearchCenter); + ar & BOOST_SERIALIZATION_NVP(m_searchAngleResolution); + ar & BOOST_SERIALIZATION_NVP(m_nAngles); + ar & BOOST_SERIALIZATION_NVP(m_searchAngleResolution);; + ar & BOOST_SERIALIZATION_NVP(m_doPenalize); + kt_int32u poseResponseSize = static_cast(m_xPoses.size() * m_yPoses.size() * m_nAngles); + if (Archive::is_loading::value) + { + m_pPoseResponse = new std::pair[poseResponseSize]; + } + ar & boost::serialization::make_array>(m_pPoseResponse, poseResponseSize); + } + + }; // ScanMatcher + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + class ScanManager; + + /** + * Manages the devices for the mapper + */ + class KARTO_EXPORT MapperSensorManager //: public SensorManager + { + typedef std::map ScanManagerMap; + + public: + /** + * Constructor + */ + MapperSensorManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance) + : m_RunningBufferMaximumSize(runningBufferMaximumSize) + , m_RunningBufferMaximumDistance(runningBufferMaximumDistance) + , m_NextScanId(0) + { + } + + MapperSensorManager(){ + } + + /** + * Destructor + */ + virtual ~MapperSensorManager() + { + Clear(); + } + + public: + /** + * Registers a sensor (with given name); do + * nothing if device already registered + * @param rSensorName + */ + void RegisterSensor(const Name& rSensorName); + + /** + * Gets scan from given sensor with given ID + * @param rSensorName + * @param scanIndex + * @return localized range scan + */ + LocalizedRangeScan* GetScan(const Name& rSensorName, kt_int32s scanIndex); + + /** + * Gets names of all sensors + * @return sensor names + */ + inline std::vector GetSensorNames() + { + std::vector deviceNames; + const_forEach(ScanManagerMap, &m_ScanManagers) + { + deviceNames.push_back(iter->first); + } + + return deviceNames; + } + + /** + * Gets last scan of given sensor + * @param rSensorName + * @return last localized range scan of sensor + */ + LocalizedRangeScan* GetLastScan(const Name& rSensorName); + + /** + * Sets the last scan of device of given scan + * @param pScan + */ + inline void SetLastScan(LocalizedRangeScan* pScan); + + /** + * Gets the scan with the given unique id + * @param id + * @return scan + */ + inline LocalizedRangeScan* GetScan(kt_int32s id) + { + assert(math::IsUpTo(id, (kt_int32s)m_Scans.size())); + + return m_Scans[id]; + } + + /** + * Adds scan to scan vector of device that recorded scan + * @param pScan + */ + void AddScan(LocalizedRangeScan* pScan); + + /** + * Adds scan to running scans of device that recorded scan + * @param pScan + */ + void AddRunningScan(LocalizedRangeScan* pScan); + + /** + * Gets scans of device + * @param rSensorName + * @return scans of device + */ + LocalizedRangeScanVector& GetScans(const Name& rSensorName); + + /** + * Gets running scans of device + * @param rSensorName + * @return running scans of device + */ + LocalizedRangeScanVector& GetRunningScans(const Name& rSensorName); + + /** + * Gets all scans of all devices + * @return all scans of all devices + */ + LocalizedRangeScanVector GetAllScans(); + + /** + * Deletes all scan managers of all devices + */ + void Clear(); + + private: + /** + * Get scan manager for localized range scan + * @return ScanManager + */ + inline ScanManager* GetScanManager(LocalizedRangeScan* pScan) + { + return GetScanManager(pScan->GetSensorName()); + } + + /** + * Get scan manager for id + * @param rSensorName + * @return ScanManager + */ + inline ScanManager* GetScanManager(const Name& rSensorName) + { + if (m_ScanManagers.find(rSensorName) != m_ScanManagers.end()) + { + return m_ScanManagers[rSensorName]; + } + + return NULL; + } + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + std::cout<<"MapperSensorManager <- m_ScanManagers; "; + ar & BOOST_SERIALIZATION_NVP(m_ScanManagers); + ar & BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumSize); + ar & BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumDistance); + ar & BOOST_SERIALIZATION_NVP(m_NextScanId); + std::cout<<"MapperSensorManager <- m_Scans\n"; + ar & BOOST_SERIALIZATION_NVP(m_Scans); + } + + private: + // map from device ID to scan data + ScanManagerMap m_ScanManagers; + + kt_int32u m_RunningBufferMaximumSize; + kt_double m_RunningBufferMaximumDistance; + + kt_int32s m_NextScanId; + + std::vector m_Scans; + }; // MapperSensorManager + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Graph SLAM mapper. Creates a map given a set of LocalizedRangeScans + * The current Karto implementation is a proprietary, high-performance + * scan-matching algorithm that constructs a map from individual range scans and corrects for + * errors in the range and odometry data. + * + * The following parameters can be set on the Mapper. + * + * \a UseScanMatching (ParameterBool)\n + * When set to true, the mapper will use a scan matching algorithm. In most real-world situations + * this should be set to true so that the mapper algorithm can correct for noise and errors in + * odometry and scan data. In some simulator environments where the simulated scan and odometry + * data are very accurate, the scan matching algorithm can produce worse results. In those cases + * set to false to improve results. + * Default value is true. + * + * \a UseScanBarycenter (ParameterBool)\n + * + * \a UseResponseExpansion (ParameterBool)\n + * + * \a RangeThreshold (ParameterDouble - meters)\n + * The range threshold is used to truncate range scan distance measurement readings. The threshold should + * be set such that adjacent range readings in a scan will generally give "solid" coverage of objects. + * + * \image html doxygen/RangeThreshold.png + * \image latex doxygen/RangeThreshold.png "" width=3in + * + * Having solid coverage depends on the map resolution and the angular resolution of the range scan device. + * The following are the recommended threshold values for the corresponding map resolution and range finder + * resolution values: + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + *
Map ResolutionLaser Angular Resolution
1.0 degree0.5 degree0.25 degree
0.15.7m11.4m22.9m
0.052.8m5.7m11.4m
0.010.5m1.1m2.3m
+ * + * Note that the value of RangeThreshold should be adjusted taking into account the values of + * MinimumTravelDistance and MinimumTravelHeading (see also below). By incorporating scans + * into the map more frequently, the RangeThreshold value can be increased as the additional scans + * will "fill in" the gaps of objects at greater distances where there is less solid coverage. + * + * Default value is 12.0 (meters). + * + * \a MinimumTravelDistance (ParameterDouble - meters)\n + * Sets the minimum travel between scans. If a new scan's position is more than minimumDistance from + * the previous scan, the mapper will use the data from the new scan. Otherwise, it will discard the + * new scan if it also does not meet the minimum change in heading requirement. + * For performance reasons, generally it is a good idea to only process scans if the robot + * has moved a reasonable amount. + * Default value is 0.3 (meters). + * + * \a MinimumTravelHeading (ParameterDouble - radians)\n + * Sets the minimum heading change between scans. If a new scan's heading is more than minimumHeading from + * the previous scan, the mapper will use the data from the new scan. Otherwise, it will discard the + * new scan if it also does not meet the minimum travel distance requirement. + * For performance reasons, generally it is a good idea to only process scans if the robot + * has moved a reasonable amount. + * Default value is 0.08726646259971647 (radians) - 5 degrees. + * + * \a ScanBufferSize (ParameterIn32u - size)\n + * Scan buffer size is the length of the scan chain stored for scan matching. + * "ScanBufferSize" should be set to approximately "ScanBufferMaximumScanDistance" / "MinimumTravelDistance". + * The idea is to get an area approximately 20 meters long for scan matching. + * For example, if we add scans every MinimumTravelDistance = 0.3 meters, then "ScanBufferSize" + * should be 20 / 0.3 = 67.) + * Default value is 67. + * + * \a ScanBufferMaximumScanDistance (ParameterDouble - meters)\n + * Scan buffer maximum scan distance is the maximum distance between the first and last scans + * in the scan chain stored for matching. + * Default value is 20.0. + * + * \a CorrelationSearchSpaceDimension (ParameterDouble - meters)\n + * The size of the correlation grid used by the matcher. + * Default value is 0.3 meters which tells the matcher to use a 30cm x 30cm grid. + * + * \a CorrelationSearchSpaceResolution (ParameterDouble - meters)\n + * The resolution (size of a grid cell) of the correlation grid. + * Default value is 0.01 meters. + * + * \a CorrelationSearchSpaceSmearDeviation (ParameterDouble - meters)\n + * The robot position is smeared by this value in X and Y to create a smoother response. + * Default value is 0.03 meters. + * + * \a LinkMatchMinimumResponseFine (ParameterDouble - probability (>= 0.0, <= 1.0))\n + * Scans are linked only if the correlation response value is greater than this value. + * Default value is 0.4 + * + * \a LinkScanMaximumDistance (ParameterDouble - meters)\n + * Maximum distance between linked scans. Scans that are farther apart will not be linked + * regardless of the correlation response value. + * Default value is 6.0 meters. + * + * \a LoopSearchSpaceDimension (ParameterDouble - meters)\n + * Dimension of the correlation grid used by the loop closure detection algorithm + * Default value is 4.0 meters. + * + * \a LoopSearchSpaceResolution (ParameterDouble - meters)\n + * Coarse resolution of the correlation grid used by the matcher to determine a possible + * loop closure. + * Default value is 0.05 meters. + * + * \a LoopSearchSpaceSmearDeviation (ParameterDouble - meters)\n + * Smearing distance in the correlation grid used by the matcher to determine a possible + * loop closure match. + * Default value is 0.03 meters. + * + * \a LoopSearchMaximumDistance (ParameterDouble - meters)\n + * Scans less than this distance from the current position will be considered for a match + * in loop closure. + * Default value is 4.0 meters. + * + * \a LoopMatchMinimumChainSize (ParameterIn32s)\n + * When the loop closure detection finds a candidate it must be part of a large + * set of linked scans. If the chain of scans is less than this value we do not attempt + * to close the loop. + * Default value is 10. + * + * \a LoopMatchMaximumVarianceCoarse (ParameterDouble)\n + * The co-variance values for a possible loop closure have to be less than this value + * to consider a viable solution. This applies to the coarse search. + * Default value is 0.16. + * + * \a LoopMatchMinimumResponseCoarse (ParameterDouble - probability (>= 0.0, <= 1.0))\n + * If response is larger then this then initiate loop closure search at the coarse resolution. + * Default value is 0.7. + * + * \a LoopMatchMinimumResponseFine (ParameterDouble - probability (>= 0.0, <= 1.0))\n + * If response is larger then this then initiate loop closure search at the fine resolution. + * Default value is 0.5. + */ + class KARTO_EXPORT Mapper : public Module + { + friend class MapperGraph; + friend class ScanMatcher; + + public: + /** + * Default constructor + */ + Mapper(); + + /** + * Constructor a mapper with a name + * @param rName mapper name + */ + Mapper(const std::string& rName); + + /** + * Destructor + */ + virtual ~Mapper(); + + public: + /** + * Allocate memory needed for mapping + * @param rangeThreshold + */ + void Initialize(kt_double rangeThreshold); + + /** + * Save map to file + * @param filename + */ + void SaveToFile(const std::string& filename); + + /** + * Load map from file + * @param filename + */ + void LoadFromFile(const std::string& filename); + + /** + * Resets the mapper. + * Deallocate memory allocated in Initialize() + */ + void Reset(); + + /** + * Process a localized range scan for incorporation into the map. The scan must + * be identified with a range finder device. Once added to a map, the corrected pose information in the + * localized scan will be updated to the correct pose as determined by the mapper. + * + * @param pScan A localized range scan that has pose information associated directly with the scan data. The pose + * is that of the range device originating the scan. Note that the mapper will set corrected pose + * information in the scan object. + * + * @return true if the scan was added successfully, false otherwise + */ + virtual kt_bool Process(LocalizedRangeScan* pScan); + + /** + * Process an Object + */ + virtual kt_bool Process(Object* pObject); + + /** + * Returns all processed scans added to the mapper. + * NOTE: The returned scans have their corrected pose updated. + * @return list of scans received and processed by the mapper. If no scans have been processed, + * return an empty list. + */ + virtual const LocalizedRangeScanVector GetAllProcessedScans() const; + + /** + * Add a listener to mapper + * @param pListener + */ + void AddListener(MapperListener* pListener); + + /** + * Remove a listener to mapper + * @param pListener + */ + void RemoveListener(MapperListener* pListener); + + /** + * Set scan optimizer used by mapper when closing the loop + * @param pSolver + */ + void SetScanSolver(ScanSolver* pSolver); + + /** + * Get scan link graph + * @return graph + */ + virtual MapperGraph* GetGraph() const; + + /** + * Gets the sequential scan matcher + * @return sequential scan matcher + */ + virtual ScanMatcher* GetSequentialScanMatcher() const; + + /** + * Gets the loop scan matcher + * @return loop scan matcher + */ + virtual ScanMatcher* GetLoopScanMatcher() const; + + /** + * Gets the device manager + * @return device manager + */ + inline MapperSensorManager* GetMapperSensorManager() const + { + return m_pMapperSensorManager; + } + + /** + * Tries to close a loop using the given scan with the scans from the given sensor + * @param pScan + * @param rSensorName + */ + inline kt_bool TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName) + { + return m_pGraph->TryCloseLoop(pScan, rSensorName); + } + + inline void CorrectPoses() + { + m_pGraph->CorrectPoses(); + } + + private: + void InitializeParameters(); + + /** + * Test if the scan is "sufficiently far" from the last scan added. + * @param pScan scan to be checked + * @param pLastScan last scan added to mapper + * @return true if the scan is "sufficiently far" from the last scan added or + * the scan is the first scan to be added + */ + kt_bool HasMovedEnough(LocalizedRangeScan* pScan, LocalizedRangeScan* pLastScan) const; + + public: + ///////////////////////////////////////////// + // fire information for listeners!! + + /** + * Fire a general message to listeners + * @param rInfo + */ + void FireInfo(const std::string& rInfo) const; + + /** + * Fire a debug message to listeners + * @param rInfo + */ + void FireDebug(const std::string& rInfo) const; + + /** + * Fire a message upon checking for loop closure to listeners + * @param rInfo + */ + void FireLoopClosureCheck(const std::string& rInfo) const; + + /** + * Fire a message before loop closure to listeners + * @param rInfo + */ + void FireBeginLoopClosure(const std::string& rInfo) const; + + /** + * Fire a message after loop closure to listeners + * @param rInfo + */ + void FireEndLoopClosure(const std::string& rInfo) const; + + // FireRunningScansUpdated + + // FireCovarianceCalculated + + // FireLoopClosureChain + + private: + /** + * Restrict the copy constructor + */ + Mapper(const Mapper&); + + /** + * Restrict the assignment operator + */ + const Mapper& operator=(const Mapper&); + + public: + void SetUseScanMatching(kt_bool val) { m_pUseScanMatching->SetValue(val); } + + private: + kt_bool m_Initialized; + + ScanMatcher* m_pSequentialScanMatcher; + + MapperSensorManager* m_pMapperSensorManager; + + MapperGraph* m_pGraph; + ScanSolver* m_pScanOptimizer; + + std::vector m_Listeners; + + + /** + * When set to true, the mapper will use a scan matching algorithm. In most real-world situations + * this should be set to true so that the mapper algorithm can correct for noise and errors in + * odometry and scan data. In some simulator environments where the simulated scan and odometry + * data are very accurate, the scan matching algorithm can produce worse results. In those cases + * set this to false to improve results. + * Default value is true. + */ + Parameter* m_pUseScanMatching; + + /** + * Default value is true. + */ + Parameter* m_pUseScanBarycenter; + + /** + * Sets the minimum time between scans. If a new scan's time stamp is + * longer than MinimumTimeInterval from the previously processed scan, + * the mapper will use the data from the new scan. Otherwise, it will + * discard the new scan if it also does not meet the minimum travel + * distance and heading requirements. For performance reasons, it is + * generally it is a good idea to only process scans if a reasonable + * amount of time has passed. This parameter is particularly useful + * when there is a need to process scans while the robot is stationary. + * Default value is 3600 (seconds), effectively disabling this parameter. + */ + Parameter* m_pMinimumTimeInterval; + + /** + * Sets the minimum travel between scans. If a new scan's position is more than minimumTravelDistance + * from the previous scan, the mapper will use the data from the new scan. Otherwise, it will discard the + * new scan if it also does not meet the minimum change in heading requirement. + * For performance reasons, generally it is a good idea to only process scans if the robot + * has moved a reasonable amount. + * Default value is 0.2 (meters). + */ + Parameter* m_pMinimumTravelDistance; + + /** + * Sets the minimum heading change between scans. If a new scan's heading is more than minimumTravelHeading + * from the previous scan, the mapper will use the data from the new scan. Otherwise, it will discard the + * new scan if it also does not meet the minimum travel distance requirement. + * For performance reasons, generally it is a good idea to only process scans if the robot + * has moved a reasonable amount. + * Default value is 10 degrees. + */ + Parameter* m_pMinimumTravelHeading; + + /** + * Scan buffer size is the length of the scan chain stored for scan matching. + * "scanBufferSize" should be set to approximately "scanBufferMaximumScanDistance" / "minimumTravelDistance". + * The idea is to get an area approximately 20 meters long for scan matching. + * For example, if we add scans every minimumTravelDistance == 0.3 meters, then "scanBufferSize" + * should be 20 / 0.3 = 67.) + * Default value is 67. + */ + Parameter* m_pScanBufferSize; + + /** + * Scan buffer maximum scan distance is the maximum distance between the first and last scans + * in the scan chain stored for matching. + * Default value is 20.0. + */ + Parameter* m_pScanBufferMaximumScanDistance; + + /** + * Scans are linked only if the correlation response value is greater than this value. + * Default value is 0.4 + */ + Parameter* m_pLinkMatchMinimumResponseFine; + + /** + * Maximum distance between linked scans. Scans that are farther apart will not be linked + * regardless of the correlation response value. + * Default value is 6.0 meters. + */ + Parameter* m_pLinkScanMaximumDistance; + + /** + * Enable/disable loop closure. + * Default is enabled. + */ + Parameter* m_pDoLoopClosing; + + /** + * Scans less than this distance from the current position will be considered for a match + * in loop closure. + * Default value is 4.0 meters. + */ + Parameter* m_pLoopSearchMaximumDistance; + + /** + * When the loop closure detection finds a candidate it must be part of a large + * set of linked scans. If the chain of scans is less than this value we do not attempt + * to close the loop. + * Default value is 10. + */ + Parameter* m_pLoopMatchMinimumChainSize; + + /** + * The co-variance values for a possible loop closure have to be less than this value + * to consider a viable solution. This applies to the coarse search. + * Default value is 0.16. + */ + Parameter* m_pLoopMatchMaximumVarianceCoarse; + + /** + * If response is larger then this, then initiate loop closure search at the coarse resolution. + * Default value is 0.7. + */ + Parameter* m_pLoopMatchMinimumResponseCoarse; + + /** + * If response is larger then this, then initiate loop closure search at the fine resolution. + * Default value is 0.7. + */ + Parameter* m_pLoopMatchMinimumResponseFine; + + ////////////////////////////////////////////////////////////////////////////// + // CorrelationParameters correlationParameters; + + /** + * The size of the search grid used by the matcher. + * Default value is 0.3 meters which tells the matcher to use a 30cm x 30cm grid. + */ + Parameter* m_pCorrelationSearchSpaceDimension; + + /** + * The resolution (size of a grid cell) of the correlation grid. + * Default value is 0.01 meters. + */ + Parameter* m_pCorrelationSearchSpaceResolution; + + /** + * The point readings are smeared by this value in X and Y to create a smoother response. + * Default value is 0.03 meters. + */ + Parameter* m_pCorrelationSearchSpaceSmearDeviation; + + + ////////////////////////////////////////////////////////////////////////////// + // CorrelationParameters loopCorrelationParameters; + + /** + * The size of the search grid used by the matcher. + * Default value is 0.3 meters which tells the matcher to use a 30cm x 30cm grid. + */ + Parameter* m_pLoopSearchSpaceDimension; + + /** + * The resolution (size of a grid cell) of the correlation grid. + * Default value is 0.01 meters. + */ + Parameter* m_pLoopSearchSpaceResolution; + + /** + * The point readings are smeared by this value in X and Y to create a smoother response. + * Default value is 0.03 meters. + */ + Parameter* m_pLoopSearchSpaceSmearDeviation; + + ////////////////////////////////////////////////////////////////////////////// + // ScanMatcherParameters; + + // Variance of penalty for deviating from odometry when scan-matching. + // The penalty is a multiplier (less than 1.0) is a function of the + // delta of the scan position being tested and the odometric pose + Parameter* m_pDistanceVariancePenalty; + Parameter* m_pAngleVariancePenalty; + + // The range of angles to search during a coarse search and a finer search + Parameter* m_pFineSearchAngleOffset; + Parameter* m_pCoarseSearchAngleOffset; + + // Resolution of angles to search during a coarse search + Parameter* m_pCoarseAngleResolution; + + // Minimum value of the penalty multiplier so scores do not + // become too small + Parameter* m_pMinimumAnglePenalty; + Parameter* m_pMinimumDistancePenalty; + + // whether to increase the search space if no good matches are initially found + Parameter* m_pUseResponseExpansion; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + std::cout<<"**Serializing Mapper**\n"; + std::cout<<"Mapper <- Module\n"; + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Module); + ar & BOOST_SERIALIZATION_NVP(m_Initialized); + std::cout<<"Mapper <- m_pSequentialScanMatcher\n"; + ar & BOOST_SERIALIZATION_NVP(m_pSequentialScanMatcher); + std::cout<<"Mapper <- m_pGraph\n"; + ar & BOOST_SERIALIZATION_NVP(m_pGraph); + std::cout<<"Mapper <- m_pMapperSensorManager\n"; + ar & BOOST_SERIALIZATION_NVP(m_pMapperSensorManager); + std::cout<<"Mapper <- m_Listeners\n"; + ar & BOOST_SERIALIZATION_NVP(m_Listeners); + ar & BOOST_SERIALIZATION_NVP(m_pUseScanMatching); + ar & BOOST_SERIALIZATION_NVP(m_pUseScanBarycenter); + ar & BOOST_SERIALIZATION_NVP(m_pMinimumTimeInterval); + ar & BOOST_SERIALIZATION_NVP(m_pMinimumTravelDistance); + ar & BOOST_SERIALIZATION_NVP(m_pMinimumTravelHeading); + ar & BOOST_SERIALIZATION_NVP(m_pScanBufferSize); + ar & BOOST_SERIALIZATION_NVP(m_pScanBufferMaximumScanDistance); + ar & BOOST_SERIALIZATION_NVP(m_pLinkMatchMinimumResponseFine); + ar & BOOST_SERIALIZATION_NVP(m_pLinkScanMaximumDistance); + ar & BOOST_SERIALIZATION_NVP(m_pDoLoopClosing); + ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchMaximumDistance); + ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMinimumChainSize); + ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMaximumVarianceCoarse); + ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMinimumResponseCoarse); + ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMinimumResponseFine); + ar & BOOST_SERIALIZATION_NVP(m_pCorrelationSearchSpaceDimension); + ar & BOOST_SERIALIZATION_NVP(m_pCorrelationSearchSpaceResolution); + ar & BOOST_SERIALIZATION_NVP(m_pCorrelationSearchSpaceSmearDeviation); + ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchSpaceDimension); + ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchSpaceResolution); + ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchSpaceSmearDeviation); + ar & BOOST_SERIALIZATION_NVP(m_pDistanceVariancePenalty); + ar & BOOST_SERIALIZATION_NVP(m_pAngleVariancePenalty); + ar & BOOST_SERIALIZATION_NVP(m_pFineSearchAngleOffset); + ar & BOOST_SERIALIZATION_NVP(m_pCoarseSearchAngleOffset); + ar & BOOST_SERIALIZATION_NVP(m_pCoarseAngleResolution); + ar & BOOST_SERIALIZATION_NVP(m_pMinimumAnglePenalty); + ar & BOOST_SERIALIZATION_NVP(m_pMinimumDistancePenalty); + ar & BOOST_SERIALIZATION_NVP(m_pUseResponseExpansion); + std::cout<<"**Finished serializing Mapper**\n"; + } + public: + /* Abstract methods for parameter setters and getters */ + + /* Getters */ + // General Parameters + bool getParamUseScanMatching(); + bool getParamUseScanBarycenter(); + double getParamMinimumTimeInterval(); + double getParamMinimumTravelDistance(); + double getParamMinimumTravelHeading(); + int getParamScanBufferSize(); + double getParamScanBufferMaximumScanDistance(); + double getParamLinkMatchMinimumResponseFine(); + double getParamLinkScanMaximumDistance(); + double getParamLoopSearchMaximumDistance(); + bool getParamDoLoopClosing(); + int getParamLoopMatchMinimumChainSize(); + double getParamLoopMatchMaximumVarianceCoarse(); + double getParamLoopMatchMinimumResponseCoarse(); + double getParamLoopMatchMinimumResponseFine(); + + // Correlation Parameters - Correlation Parameters + double getParamCorrelationSearchSpaceDimension(); + double getParamCorrelationSearchSpaceResolution(); + double getParamCorrelationSearchSpaceSmearDeviation(); + + // Correlation Parameters - Loop Closure Parameters + double getParamLoopSearchSpaceDimension(); + double getParamLoopSearchSpaceResolution(); + double getParamLoopSearchSpaceSmearDeviation(); + + // Scan Matcher Parameters + double getParamDistanceVariancePenalty(); + double getParamAngleVariancePenalty(); + double getParamFineSearchAngleOffset(); + double getParamCoarseSearchAngleOffset(); + double getParamCoarseAngleResolution(); + double getParamMinimumAnglePenalty(); + double getParamMinimumDistancePenalty(); + bool getParamUseResponseExpansion(); + + /* Setters */ + // General Parameters + void setParamUseScanMatching(bool b); + void setParamUseScanBarycenter(bool b); + void setParamMinimumTimeInterval(double d); + void setParamMinimumTravelDistance(double d); + void setParamMinimumTravelHeading(double d); + void setParamScanBufferSize(int i); + void setParamScanBufferMaximumScanDistance(double d); + void setParamLinkMatchMinimumResponseFine(double d); + void setParamLinkScanMaximumDistance(double d); + void setParamLoopSearchMaximumDistance(double d); + void setParamDoLoopClosing(bool b); + void setParamLoopMatchMinimumChainSize(int i); + void setParamLoopMatchMaximumVarianceCoarse(double d); + void setParamLoopMatchMinimumResponseCoarse(double d); + void setParamLoopMatchMinimumResponseFine(double d); + + // Correlation Parameters - Correlation Parameters + void setParamCorrelationSearchSpaceDimension(double d); + void setParamCorrelationSearchSpaceResolution(double d); + void setParamCorrelationSearchSpaceSmearDeviation(double d); + + // Correlation Parameters - Loop Closure Parameters + void setParamLoopSearchSpaceDimension(double d); + void setParamLoopSearchSpaceResolution(double d); + void setParamLoopSearchSpaceSmearDeviation(double d); + + // Scan Matcher Parameters + void setParamDistanceVariancePenalty(double d); + void setParamAngleVariancePenalty(double d); + void setParamFineSearchAngleOffset(double d); + void setParamCoarseSearchAngleOffset(double d); + void setParamCoarseAngleResolution(double d); + void setParamMinimumAnglePenalty(double d); + void setParamMinimumDistancePenalty(double d); + void setParamUseResponseExpansion(bool b); + }; + BOOST_SERIALIZATION_ASSUME_ABSTRACT(Mapper) +} // namespace karto + +#endif // OPEN_KARTO_MAPPER_H diff --git a/lib/KartoSDK-slam_toolbox/include/open_karto/Math.h b/lib/KartoSDK-slam_toolbox/include/open_karto/Math.h new file mode 100644 index 000000000..c0b628550 --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/include/open_karto/Math.h @@ -0,0 +1,252 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#ifndef OPEN_KARTO_MATH_H +#define OPEN_KARTO_MATH_H + +#include +#include +#include + +#include + +namespace karto +{ + /** + * Platform independent pi definitions + */ + const kt_double KT_PI = 3.14159265358979323846; // The value of PI + const kt_double KT_2PI = 6.28318530717958647692; // 2 * PI + const kt_double KT_PI_2 = 1.57079632679489661923; // PI / 2 + const kt_double KT_PI_180 = 0.01745329251994329577; // PI / 180 + const kt_double KT_180_PI = 57.29577951308232087685; // 180 / PI + + /** + * Lets define a small number! + */ + const kt_double KT_TOLERANCE = 1e-06; + + /** + * Lets define max value of kt_int32s (int32_t) to use it to mark invalid scans + */ + + const kt_int32s INVALID_SCAN = std::numeric_limits::max(); + + namespace math + { + /** + * Converts degrees into radians + * @param degrees + * @return radian equivalent of degrees + */ + inline kt_double DegreesToRadians(kt_double degrees) + { + return degrees * KT_PI_180; + } + + /** + * Converts radians into degrees + * @param radians + * @return degree equivalent of radians + */ + inline kt_double RadiansToDegrees(kt_double radians) + { + return radians * KT_180_PI; + } + + /** + * Square function + * @param value + * @return square of value + */ + template + inline T Square(T value) + { + return (value * value); + } + + /** + * Round function + * @param value + * @return rounds value to the nearest whole number (as double) + */ + inline kt_double Round(kt_double value) + { + return value >= 0.0 ? floor(value + 0.5) : ceil(value - 0.5); + } + + /** + * Binary minimum function + * @param value1 + * @param value2 + * @return the lesser of value1 and value2 + */ + template + inline const T& Minimum(const T& value1, const T& value2) + { + return value1 < value2 ? value1 : value2; + } + + /** + * Binary maximum function + * @param value1 + * @param value2 + * @return the greater of value1 and value2 + */ + template + inline const T& Maximum(const T& value1, const T& value2) + { + return value1 > value2 ? value1 : value2; + } + + /** + * Clips a number to the specified minimum and maximum values. + * @param n number to be clipped + * @param minValue minimum value + * @param maxValue maximum value + * @return the clipped value + */ + template + inline const T& Clip(const T& n, const T& minValue, const T& maxValue) + { + return Minimum(Maximum(n, minValue), maxValue); + } + + /** + * Checks whether two numbers are equal within a certain tolerance. + * @param a + * @param b + * @return true if a and b differ by at most a certain tolerance. + */ + inline kt_bool DoubleEqual(kt_double a, kt_double b) + { + double delta = a - b; + return delta < 0.0 ? delta >= -KT_TOLERANCE : delta <= KT_TOLERANCE; + } + + /** + * Checks whether value is in the range [0;maximum) + * @param value + * @param maximum + */ + template + inline kt_bool IsUpTo(const T& value, const T& maximum) + { + return (value >= 0 && value < maximum); + } + + /** + * Checks whether value is in the range [0;maximum) + * Specialized version for unsigned int (kt_int32u) + * @param value + * @param maximum + */ + template<> + inline kt_bool IsUpTo(const kt_int32u& value, const kt_int32u& maximum) + { + return (value < maximum); + } + + + /** + * Checks whether value is in the range [a;b] + * @param value + * @param a + * @param b + */ + template + inline kt_bool InRange(const T& value, const T& a, const T& b) + { + return (value >= a && value <= b); + } + + /** + * Normalizes angle to be in the range of [-pi, pi] + * @param angle to be normalized + * @return normalized angle + */ + inline kt_double NormalizeAngle(kt_double angle) + { + while (angle < -KT_PI) + { + if (angle < -KT_2PI) + { + angle += (kt_int32u)(angle / -KT_2PI) * KT_2PI; + } + else + { + angle += KT_2PI; + } + } + + while (angle > KT_PI) + { + if (angle > KT_2PI) + { + angle -= (kt_int32u)(angle / KT_2PI) * KT_2PI; + } + else + { + angle -= KT_2PI; + } + } + + assert(math::InRange(angle, -KT_PI, KT_PI)); + + return angle; + } + + /** + * Returns an equivalent angle to the first parameter such that the difference + * when the second parameter is subtracted from this new value is an angle + * in the normalized range of [-pi, pi], i.e. abs(minuend - subtrahend) <= pi. + * @param minuend + * @param subtrahend + * @return normalized angle + */ + inline kt_double NormalizeAngleDifference(kt_double minuend, kt_double subtrahend) + { + while (minuend - subtrahend < -KT_PI) + { + minuend += KT_2PI; + } + + while (minuend - subtrahend > KT_PI) + { + minuend -= KT_2PI; + } + + return minuend; + } + + /** + * Align a value to the alignValue. + * The alignValue should be the power of two (2, 4, 8, 16, 32 and so on) + * @param value + * @param alignValue + * @return aligned value + */ + template + inline T AlignValue(size_t value, size_t alignValue = 8) + { + return static_cast ((value + (alignValue - 1)) & ~(alignValue - 1)); + } + } // namespace math + +} // namespace karto + +#endif // OPEN_KARTO_MATH_H diff --git a/lib/KartoSDK-slam_toolbox/include/open_karto/Types.h b/lib/KartoSDK-slam_toolbox/include/open_karto/Types.h new file mode 100644 index 000000000..b4b6fc5e4 --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/include/open_karto/Types.h @@ -0,0 +1,69 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#ifndef OPEN_KARTO_TYPES_H +#define OPEN_KARTO_TYPES_H + +#include + +/** + * Karto type definition + * Ensures platform independent types for windows, linux and mac + */ +#if defined(_MSC_VER) + + typedef signed __int8 kt_int8s; + typedef unsigned __int8 kt_int8u; + + typedef signed __int16 kt_int16s; + typedef unsigned __int16 kt_int16u; + + typedef signed __int32 kt_int32s; + typedef unsigned __int32 kt_int32u; + + typedef signed __int64 kt_int64s; + typedef unsigned __int64 kt_int64u; + +#else + + #include + + typedef int8_t kt_int8s; + typedef uint8_t kt_int8u; + + typedef int16_t kt_int16s; + typedef uint16_t kt_int16u; + + typedef int32_t kt_int32s; + typedef uint32_t kt_int32u; + +#if defined(__LP64__) + typedef signed long kt_int64s; + typedef unsigned long kt_int64u; +#else + typedef signed long long kt_int64s; + typedef unsigned long long kt_int64u; +#endif + +#endif + +typedef bool kt_bool; +typedef char kt_char; +typedef float kt_float; +typedef double kt_double; + +#endif // OPEN_KARTO_TYPES_H diff --git a/lib/KartoSDK-slam_toolbox/package.xml b/lib/KartoSDK-slam_toolbox/package.xml new file mode 100644 index 000000000..507fca19b --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/package.xml @@ -0,0 +1,19 @@ + + + open_karto + 1.1.4 + Catkinized ROS packaging of the OpenKarto library + + Michael Ferguson + Luc Bettaieb + LGPLv3 + + catkin + + boost + tbb + + boost + tbb + + diff --git a/lib/KartoSDK-slam_toolbox/samples/CMakeLists.txt b/lib/KartoSDK-slam_toolbox/samples/CMakeLists.txt new file mode 100644 index 000000000..a51b7ba69 --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/samples/CMakeLists.txt @@ -0,0 +1,71 @@ +# +# Copyright 2010 SRI International +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU Lesser General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with this program. If not, see . +# + +ADD_DEFINITIONS(-DUSE_SPA) + +if(WIN32) + ADD_DEFINITIONS(-DEIGEN_DONT_ALIGN) +endif() + +if(WIN32) + # turn off various warnings + foreach(warning 4251) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /wd${warning}") + endforeach(warning) +endif() + +include_directories(../source) + +############################################################################ +# tutorial 1 +add_executable( + Karto_Tutorial1 + tutorial1.cpp +) + +target_link_libraries(Karto_Tutorial1 OpenKarto) + +if (UNIX) + target_link_libraries(Karto_Tutorial1 "pthread") +endif() + +############################################################################ +# tutorial 2 + +include_directories(${CSPARSE_DIR}/CSparse/Include) +include_directories(${SBA_DIR}/sba/include) +include_directories(${EIGEN_DIR}/eigen) + +add_executable( + Karto_Tutorial2 + tutorial2.cpp + SpaSolver.h + SpaSolver.cpp +) + +target_link_libraries(Karto_Tutorial2 OpenKarto) +target_link_libraries(Karto_Tutorial2 spa2d) +target_link_libraries(Karto_Tutorial2 csparse) + +if(PNG_FOUND) + target_link_libraries(Karto_Tutorial2 ${ZLIB_LIBRARIES}) + target_link_libraries(Karto_Tutorial2 ${PNG_LIBRARIES}) +endif() + +if (UNIX) + target_link_libraries(Karto_Tutorial2 "pthread") +endif() diff --git a/lib/KartoSDK-slam_toolbox/samples/README.txt b/lib/KartoSDK-slam_toolbox/samples/README.txt new file mode 100644 index 000000000..80b307204 --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/samples/README.txt @@ -0,0 +1,10 @@ +The CMake file does not work out of the box, because you will need a few +external libraries (all available on ROS) if you want to use the SPA solver for +closing loops: + +CSparse +eigen +sba + + +Alternatively, you can just not compile Tutorial2. diff --git a/lib/KartoSDK-slam_toolbox/samples/SpaSolver.cpp b/lib/KartoSDK-slam_toolbox/samples/SpaSolver.cpp new file mode 100644 index 000000000..c74bcdda4 --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/samples/SpaSolver.cpp @@ -0,0 +1,84 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#include "SpaSolver.h" +#include + +SpaSolver::SpaSolver() +{ + +} + +SpaSolver::~SpaSolver() +{ + +} + +void SpaSolver::Clear() +{ + corrections.clear(); +} + +const karto::ScanSolver::IdPoseVector& SpaSolver::GetCorrections() const +{ + return corrections; +} + +void SpaSolver::Compute() +{ + corrections.clear(); + + typedef std::vector > NodeVector; + + printf("DO SPA BEGIN\n"); + m_Spa.doSPA(40); + printf("DO SPA END\n"); + NodeVector nodes = m_Spa.getNodes(); + forEach(NodeVector, &nodes) + { + karto::Pose2 pose(iter->trans(0), iter->trans(1), iter->arot); + corrections.push_back(std::make_pair(iter->nodeId, pose)); + } +} + +void SpaSolver::AddNode(karto::Vertex* pVertex) +{ + karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose(); + Eigen::Vector3d vector(pose.GetX(), pose.GetY(), pose.GetHeading()); + m_Spa.addNode2d(vector, pVertex->GetObject()->GetUniqueId()); +} + +void SpaSolver::AddConstraint(karto::Edge* pEdge) +{ + karto::LocalizedRangeScan* pSource = pEdge->GetSource()->GetObject(); + karto::LocalizedRangeScan* pTarget = pEdge->GetTarget()->GetObject(); + karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel()); + + karto::Pose2 diff = pLinkInfo->GetPoseDifference(); + Eigen::Vector3d mean(diff.GetX(), diff.GetY(), diff.GetHeading()); + + karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse(); + Eigen::Matrix m; + m(0,0) = precisionMatrix(0,0); + m(0,1) = m(1,0) = precisionMatrix(0,1); + m(0,2) = m(2,0) = precisionMatrix(0,2); + m(1,1) = precisionMatrix(1,1); + m(1,2) = m(2,1) = precisionMatrix(1,2); + m(2,2) = precisionMatrix(2,2); + + m_Spa.addConstraint2d(pSource->GetUniqueId(), pTarget->GetUniqueId(), mean, m); +} diff --git a/lib/KartoSDK-slam_toolbox/samples/SpaSolver.h b/lib/KartoSDK-slam_toolbox/samples/SpaSolver.h new file mode 100644 index 000000000..9fae93bde --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/samples/SpaSolver.h @@ -0,0 +1,55 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#ifndef __SPASOLVER__ +#define __SPASOLVER__ + +#include + +#ifndef EIGEN_USE_NEW_STDVECTOR +#define EIGEN_USE_NEW_STDVECTOR +#endif // EIGEN_USE_NEW_STDVECTOR + +#define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(10) +#include + +#include "sba/spa2d.h" + +typedef std::vector CovarianceVector; + +class SpaSolver : public karto::ScanSolver +{ +public: + SpaSolver(); + virtual ~SpaSolver(); + +public: + virtual void Clear(); + virtual void Compute(); + virtual const karto::ScanSolver::IdPoseVector& GetCorrections() const; + + virtual void AddNode(karto::Vertex* pVertex); + virtual void AddConstraint(karto::Edge* pEdge); + +private: + karto::ScanSolver::IdPoseVector corrections; + + sba::SysSPA2d m_Spa; +}; + +#endif // __SPASOLVER__ + diff --git a/lib/KartoSDK-slam_toolbox/samples/tutorial1.cpp b/lib/KartoSDK-slam_toolbox/samples/tutorial1.cpp new file mode 100644 index 000000000..167b8a06d --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/samples/tutorial1.cpp @@ -0,0 +1,182 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#include + +/** + * Sample code to demonstrate karto map creation + * Create a laser range finder device and three "dummy" range scans. + * Add the device and range scans to a karto Mapper. + */ +karto::Dataset* CreateMap(karto::Mapper* pMapper) +{ + karto::Dataset* pDataset = new karto::Dataset(); + + ///////////////////////////////////// + // Create a laser range finder device - use SmartPointer to let karto subsystem manage memory. + karto::Name name("laser0"); + karto::LaserRangeFinder* pLaserRangeFinder = karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, name); + pLaserRangeFinder->SetOffsetPose(karto::Pose2(1.0, 0.0, 0.0)); + pLaserRangeFinder->SetAngularResolution(karto::math::DegreesToRadians(0.5)); + pLaserRangeFinder->SetRangeThreshold(12.0); + + pDataset->Add(pLaserRangeFinder); + + ///////////////////////////////////// + // Create three localized range scans, all using the same range readings, but with different poses. + karto::LocalizedRangeScan* pLocalizedRangeScan = NULL; + + // 1. localized range scan + + // Create a vector of range readings. Simple example where all the measurements are the same value. + std::vector readings; + for (int i=0; i<361; i++) + { + readings.push_back(3.0); + } + + // create localized range scan + pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings); + pLocalizedRangeScan->SetOdometricPose(karto::Pose2(0.0, 0.0, 0.0)); + pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(0.0, 0.0, 0.0)); + + // Add the localized range scan to the mapper + pMapper->Process(pLocalizedRangeScan); + std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl; + + // Add the localized range scan to the dataset + pDataset->Add(pLocalizedRangeScan); + + // 2. localized range scan + + // create localized range scan + pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings); + pLocalizedRangeScan->SetOdometricPose(karto::Pose2(1.0, 0.0, 1.57)); + pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(1.0, 0.0, 1.57)); + + // Add the localized range scan to the mapper + pMapper->Process(pLocalizedRangeScan); + std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl; + + // Add the localized range scan to the dataset + pDataset->Add(pLocalizedRangeScan); + + // 3. localized range scan + + // create localized range scan + pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings); + pLocalizedRangeScan->SetOdometricPose(karto::Pose2(1.0, -1.0, 2.35619449)); + pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(1.0, -1.0, 2.35619449)); + + // Add the localized range scan to the mapper + pMapper->Process(pLocalizedRangeScan); + std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl; + + // Add the localized range scan to the dataset + pDataset->Add(pLocalizedRangeScan); + + return pDataset; +} + +/** + * Sample code to demonstrate basic occupancy grid creation and print occupancy grid. + */ +karto::OccupancyGrid* CreateOccupancyGrid(karto::Mapper* pMapper, kt_double resolution) +{ + std::cout << "Generating map..." << std::endl; + + // Create a map (occupancy grid) - time it + karto::OccupancyGrid* pOccupancyGrid = karto::OccupancyGrid::CreateFromScans(pMapper->GetAllProcessedScans(), resolution); + + return pOccupancyGrid; +} + +/** + * Sample code to print a basic occupancy grid + */ +void PrintOccupancyGrid(karto::OccupancyGrid* pOccupancyGrid) +{ + if (pOccupancyGrid != NULL) + { + // Output ASCII representation of map + kt_int32s width = pOccupancyGrid->GetWidth(); + kt_int32s height = pOccupancyGrid->GetHeight(); + karto::Vector2 offset = pOccupancyGrid->GetCoordinateConverter()->GetOffset(); + + std::cout << "width = " << width << ", height = " << height << ", scale = " << pOccupancyGrid->GetCoordinateConverter()->GetScale() << ", offset: " << offset.GetX() << ", " << offset.GetY() << std::endl; + for (kt_int32s y=height-1; y>=0; y--) + { + for (kt_int32s x=0; xGetValue(karto::Vector2(x, y)); + + switch (value) + { + case karto::GridStates_Unknown: + std::cout << "*"; + break; + case karto::GridStates_Occupied: + std::cout << "X"; + break; + case karto::GridStates_Free: + std::cout << " "; + break; + default: + std::cout << "?"; + } + } + std::cout << std::endl; + } + std::cout << std::endl; + } +} + +int main(int /*argc*/, char /***argv*/) +{ + // use try/catch to catch karto exceptions that might be thrown by the karto subsystem. + ///////////////////////////////////// + // Get karto default mapper + karto::Mapper* pMapper = new karto::Mapper(); + if (pMapper != NULL) + { + karto::OccupancyGrid* pOccupancyGrid = NULL; + + ///////////////////////////////////// + // sample code that creates a map from sample device and sample localized range scans + + std::cout << "Tutorial 1 ----------------" << std::endl << std::endl; + + // clear mapper + pMapper->Reset(); + + // create map from created dataset + karto::Dataset* pDataset = CreateMap(pMapper); + + // create occupancy grid at 0.1 resolution and print grid + pOccupancyGrid = CreateOccupancyGrid(pMapper, 0.1); + PrintOccupancyGrid(pOccupancyGrid); + delete pOccupancyGrid; + + // delete mapper + delete pMapper; + + delete pDataset; + } + + return 0; +} diff --git a/lib/KartoSDK-slam_toolbox/samples/tutorial2.cpp b/lib/KartoSDK-slam_toolbox/samples/tutorial2.cpp new file mode 100644 index 000000000..720c3cd4c --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/samples/tutorial2.cpp @@ -0,0 +1,190 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#include "SpaSolver.h" + +#include + +/** + * Sample code to demonstrate karto map creation + * Create a laser range finder device and three "dummy" range scans. + * Add the device and range scans to a karto Mapper. + */ +karto::Dataset* CreateMap(karto::Mapper* pMapper) +{ + karto::Dataset* pDataset = new karto::Dataset(); + + ///////////////////////////////////// + // Create a laser range finder device - use SmartPointer to let karto subsystem manage memory. + karto::Name name("laser0"); + karto::LaserRangeFinder* pLaserRangeFinder = karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, name); + pLaserRangeFinder->SetOffsetPose(karto::Pose2(1.0, 0.0, 0.0)); + pLaserRangeFinder->SetAngularResolution(karto::math::DegreesToRadians(0.5)); + pLaserRangeFinder->SetRangeThreshold(12.0); + + pDataset->Add(pLaserRangeFinder); + + ///////////////////////////////////// + // Create three localized range scans, all using the same range readings, but with different poses. + karto::LocalizedRangeScan* pLocalizedRangeScan = NULL; + + // 1. localized range scan + + // Create a vector of range readings. Simple example where all the measurements are the same value. + std::vector readings; + for (int i=0; i<361; i++) + { + readings.push_back(3.0); + } + + // create localized range scan + pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings); + pLocalizedRangeScan->SetOdometricPose(karto::Pose2(0.0, 0.0, 0.0)); + pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(0.0, 0.0, 0.0)); + + // Add the localized range scan to the mapper + pMapper->Process(pLocalizedRangeScan); + std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl; + + // Add the localized range scan to the dataset + pDataset->Add(pLocalizedRangeScan); + + // 2. localized range scan + + // create localized range scan + pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings); + pLocalizedRangeScan->SetOdometricPose(karto::Pose2(1.0, 0.0, 1.57)); + pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(1.0, 0.0, 1.57)); + + // Add the localized range scan to the mapper + pMapper->Process(pLocalizedRangeScan); + std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl; + + // Add the localized range scan to the dataset + pDataset->Add(pLocalizedRangeScan); + + // 3. localized range scan + + // create localized range scan + pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings); + pLocalizedRangeScan->SetOdometricPose(karto::Pose2(1.0, -1.0, 2.35619449)); + pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(1.0, -1.0, 2.35619449)); + + // Add the localized range scan to the mapper + pMapper->Process(pLocalizedRangeScan); + std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl; + + // Add the localized range scan to the dataset + pDataset->Add(pLocalizedRangeScan); + + return pDataset; +} + +/** + * Sample code to demonstrate basic occupancy grid creation and print occupancy grid. + */ +karto::OccupancyGrid* CreateOccupancyGrid(karto::Mapper* pMapper, kt_double resolution) +{ + std::cout << "Generating map..." << std::endl; + + // Create a map (occupancy grid) - time it + karto::OccupancyGrid* pOccupancyGrid = karto::OccupancyGrid::CreateFromScans(pMapper->GetAllProcessedScans(), resolution); + + return pOccupancyGrid; +} + +/** + * Sample code to print a basic occupancy grid + */ +void PrintOccupancyGrid(karto::OccupancyGrid* pOccupancyGrid) +{ + if (pOccupancyGrid != NULL) + { + // Output ASCII representation of map + kt_int32s width = pOccupancyGrid->GetWidth(); + kt_int32s height = pOccupancyGrid->GetHeight(); + karto::Vector2 offset = pOccupancyGrid->GetCoordinateConverter()->GetOffset(); + + std::cout << "width = " << width << ", height = " << height << ", scale = " << pOccupancyGrid->GetCoordinateConverter()->GetScale() << ", offset: " << offset.GetX() << ", " << offset.GetY() << std::endl; + for (kt_int32s y=height-1; y>=0; y--) + { + for (kt_int32s x=0; xGetValue(karto::Vector2(x, y)); + + switch (value) + { + case karto::GridStates_Unknown: + std::cout << "*"; + break; + case karto::GridStates_Occupied: + std::cout << "X"; + break; + case karto::GridStates_Free: + std::cout << " "; + break; + default: + std::cout << "?"; + } + } + std::cout << std::endl; + } + std::cout << std::endl; + } +} + +int main(int /*argc*/, char /***argv*/) +{ + // use try/catch to catch karto exceptions that might be thrown by the karto subsystem. + ///////////////////////////////////// + // Get karto default mapper + karto::Mapper* pMapper = new karto::Mapper(); + if (pMapper != NULL) + { + // set solver + SpaSolver* pSolver = new SpaSolver(); + pMapper->SetScanSolver(pSolver); + + karto::OccupancyGrid* pOccupancyGrid = NULL; + + ///////////////////////////////////// + // sample code that creates a map from sample device and sample localized range scans + + // clear mapper + pMapper->Reset(); + + // create map from created dataset + karto::Dataset* pDataset = CreateMap(pMapper); + + // create occupancy grid at 0.1 resolution and print grid + pOccupancyGrid = CreateOccupancyGrid(pMapper, 0.1); + PrintOccupancyGrid(pOccupancyGrid); + delete pOccupancyGrid; + + // delete solver + delete pSolver; + + // delete mapper + delete pMapper; + + // delete dataset and all allocated devices and device states + delete pDataset; + } + + return 0; +} diff --git a/lib/KartoSDK-slam_toolbox/src/Karto.cpp b/lib/KartoSDK-slam_toolbox/src/Karto.cpp new file mode 100644 index 000000000..79177825d --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/src/Karto.cpp @@ -0,0 +1,279 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#include +#include +#include +#include +#include +#include +#include +#include "open_karto/Karto.h" +#include +BOOST_CLASS_EXPORT_IMPLEMENT(karto::NonCopyable); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Object); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Sensor); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::SensorData); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Name); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::LaserRangeScan); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::LocalizedRangeScan); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::CustomData); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Module); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::LaserRangeFinder); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Dataset); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::LookupArray); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::SensorManager); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::AbstractParameter); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::ParameterEnum); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameters); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::ParameterManager); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameter); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameter); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameter); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameter); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameter); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameter); +namespace karto +{ + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + SensorManager* SensorManager::GetInstance() + { + static Singleton sInstance; + return sInstance.Get(); + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + Object::Object() + : m_pParameterManager(new ParameterManager()) + { + } + + Object::Object(const Name& rName) + : m_Name(rName) + , m_pParameterManager(new ParameterManager()) + { + } + + Object::~Object() + { + delete m_pParameterManager; + m_pParameterManager = NULL; + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + Module::Module(const std::string& rName) + : Object(rName) + { + } + + Module::~Module() + { + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + Sensor::Sensor(const Name& rName) + : Object(rName) + { + m_pOffsetPose = new Parameter("OffsetPose", Pose2(), GetParameterManager()); + } + + Sensor::~Sensor() + { + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + SensorData::SensorData(const Name& rSensorName) + : Object() + , m_StateId(-1) + , m_UniqueId(-1) + , m_SensorName(rSensorName) + , m_Time(0.0) + { + } + + SensorData::~SensorData() + { + forEach(CustomDataVector, &m_CustomData) + { + delete *iter; + } + + m_CustomData.clear(); + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + void CellUpdater::operator() (kt_int32u index) + { + kt_int8u* pDataPtr = m_pOccupancyGrid->GetDataPointer(); + kt_int32u* pCellPassCntPtr = m_pOccupancyGrid->m_pCellPassCnt->GetDataPointer(); + kt_int32u* pCellHitCntPtr = m_pOccupancyGrid->m_pCellHitsCnt->GetDataPointer(); + + m_pOccupancyGrid->UpdateCell(&pDataPtr[index], pCellPassCntPtr[index], pCellHitCntPtr[index]); + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + std::ostream& operator << (std::ostream& rStream, Exception& rException) + { + rStream << "Error detect: " << std::endl; + rStream << " ==> error code: " << rException.GetErrorCode() << std::endl; + rStream << " ==> error message: " << rException.GetErrorMessage() << std::endl; + + return rStream; + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + const PointVectorDouble LaserRangeFinder::GetPointReadings(LocalizedRangeScan* pLocalizedRangeScan, + CoordinateConverter* pCoordinateConverter, + kt_bool ignoreThresholdPoints, + kt_bool flipY) const + { + PointVectorDouble pointReadings; + + Pose2 scanPose = pLocalizedRangeScan->GetSensorPose(); + + // compute point readings + kt_int32u beamNum = 0; + kt_double* pRangeReadings = pLocalizedRangeScan->GetRangeReadings(); + for (kt_int32u i = 0; i < m_NumberOfRangeReadings; i++, beamNum++) + { + kt_double rangeReading = pRangeReadings[i]; + + if (ignoreThresholdPoints) + { + if (!math::InRange(rangeReading, GetMinimumRange(), GetRangeThreshold())) + { + continue; + } + } + else + { + rangeReading = math::Clip(rangeReading, GetMinimumRange(), GetRangeThreshold()); + } + + kt_double angle = scanPose.GetHeading() + GetMinimumAngle() + beamNum * GetAngularResolution(); + + Vector2 point; + + point.SetX(scanPose.GetX() + (rangeReading * cos(angle))); + point.SetY(scanPose.GetY() + (rangeReading * sin(angle))); + + if (pCoordinateConverter != NULL) + { + Vector2 gridPoint = pCoordinateConverter->WorldToGrid(point, flipY); + point.SetX(gridPoint.GetX()); + point.SetY(gridPoint.GetY()); + } + + pointReadings.push_back(point); + } + + return pointReadings; + } + + kt_bool LaserRangeFinder::Validate(SensorData* pSensorData) + { + LaserRangeScan* pLaserRangeScan = dynamic_cast(pSensorData); + + // verify number of range readings in LaserRangeScan matches the number of expected range readings + if (pLaserRangeScan->GetNumberOfRangeReadings() != GetNumberOfRangeReadings()) + { + std::cout << "LaserRangeScan contains " << pLaserRangeScan->GetNumberOfRangeReadings() + << " range readings, expected " << GetNumberOfRangeReadings() << std::endl; + return false; + } + + return true; + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + void ParameterManager::Clear() + { + forEach(karto::ParameterVector, &m_Parameters) + { + delete *iter; + } + + m_Parameters.clear(); + + m_ParameterLookup.clear(); + } + + void ParameterManager::Add(AbstractParameter* pParameter) + { + if (pParameter != NULL && pParameter->GetName() != "") + { + if (m_ParameterLookup.find(pParameter->GetName()) == m_ParameterLookup.end()) + { + m_Parameters.push_back(pParameter); + + m_ParameterLookup[pParameter->GetName()] = pParameter; + } + else + { + m_ParameterLookup[pParameter->GetName()]->SetValueFromString(pParameter->GetValueAsString()); + + assert(false); + } + } + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /* + std::string LaserRangeFinder::LaserRangeFinderTypeNames[6] = + { + "Custom", + + "Sick_LMS100", + "Sick_LMS200", + "Sick_LMS291", + + "Hokuyo_UTM_30LX", + "Hokuyo_URG_04LX" + }; + */ +} // namespace karto diff --git a/lib/KartoSDK-slam_toolbox/src/Mapper.cpp b/lib/KartoSDK-slam_toolbox/src/Mapper.cpp new file mode 100644 index 000000000..1b68cf0f7 --- /dev/null +++ b/lib/KartoSDK-slam_toolbox/src/Mapper.cpp @@ -0,0 +1,2545 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "open_karto/Mapper.h" + +BOOST_CLASS_EXPORT(karto::MapperGraph); +BOOST_CLASS_EXPORT(karto::Graph); +BOOST_CLASS_EXPORT(karto::EdgeLabel); +BOOST_CLASS_EXPORT(karto::LinkInfo); +BOOST_CLASS_EXPORT(karto::Edge); +BOOST_CLASS_EXPORT(karto::Vertex); +BOOST_CLASS_EXPORT(karto::MapperSensorManager) +namespace karto +{ + + // enable this for verbose debug information + // #define KARTO_DEBUG + + #define MAX_VARIANCE 500.0 + #define DISTANCE_PENALTY_GAIN 0.2 + #define ANGLE_PENALTY_GAIN 0.2 + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Manages the scan data for a device + */ + class ScanManager + { + public: + /** + * Default constructor + */ + ScanManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance) + : m_pLastScan(NULL) + , m_RunningBufferMaximumSize(runningBufferMaximumSize) + , m_RunningBufferMaximumDistance(runningBufferMaximumDistance) + { + } + + ScanManager(){} + + /** + * Destructor + */ + virtual ~ScanManager() + { + Clear(); + } + + public: + /** + * Adds scan to vector of processed scans tagging scan with given unique id + * @param pScan + */ + inline void AddScan(LocalizedRangeScan* pScan, kt_int32s uniqueId) + { + // assign state id to scan + pScan->SetStateId(static_cast(m_Scans.size())); + + // assign unique id to scan + pScan->SetUniqueId(uniqueId); + + // add it to scan buffer + m_Scans.push_back(pScan); + } + + /** + * Gets last scan + * @param deviceId + * @return last localized range scan + */ + inline LocalizedRangeScan* GetLastScan() + { + return m_pLastScan; + } + + /** + * Sets the last scan + * @param pScan + */ + inline void SetLastScan(LocalizedRangeScan* pScan) + { + m_pLastScan = pScan; + } + + /** + * Gets scans + * @return scans + */ + inline LocalizedRangeScanVector& GetScans() + { + return m_Scans; + } + + /** + * Gets running scans + * @return running scans + */ + inline LocalizedRangeScanVector& GetRunningScans() + { + return m_RunningScans; + } + + /** + * Adds scan to vector of running scans + * @param pScan + */ + void AddRunningScan(LocalizedRangeScan* pScan) + { + m_RunningScans.push_back(pScan); + + // vector has at least one element (first line of this function), so this is valid + Pose2 frontScanPose = m_RunningScans.front()->GetSensorPose(); + Pose2 backScanPose = m_RunningScans.back()->GetSensorPose(); + + // cap vector size and remove all scans from front of vector that are too far from end of vector + kt_double squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition()); + while (m_RunningScans.size() > m_RunningBufferMaximumSize || + squaredDistance > math::Square(m_RunningBufferMaximumDistance) - KT_TOLERANCE) + { + // remove front of running scans + m_RunningScans.erase(m_RunningScans.begin()); + + // recompute stats of running scans + frontScanPose = m_RunningScans.front()->GetSensorPose(); + backScanPose = m_RunningScans.back()->GetSensorPose(); + squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition()); + } + } + + /** + * Deletes data of this buffered device + */ + void Clear() + { + m_Scans.clear(); + m_RunningScans.clear(); + } + + private: + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Scans); + ar & BOOST_SERIALIZATION_NVP(m_RunningScans); + ar & BOOST_SERIALIZATION_NVP(m_pLastScan); + ar & BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumSize); + ar & BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumDistance); + } + + private: + LocalizedRangeScanVector m_Scans; + LocalizedRangeScanVector m_RunningScans; + LocalizedRangeScan* m_pLastScan; + + kt_int32u m_RunningBufferMaximumSize; + kt_double m_RunningBufferMaximumDistance; + }; // ScanManager + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + void MapperSensorManager::RegisterSensor(const Name& rSensorName) + { + if (GetScanManager(rSensorName) == NULL) + { + m_ScanManagers[rSensorName] = new ScanManager(m_RunningBufferMaximumSize, m_RunningBufferMaximumDistance); + } + } + + + /** + * Gets scan from given device with given ID + * @param rSensorName + * @param scanNum + * @return localized range scan + */ + LocalizedRangeScan* MapperSensorManager::GetScan(const Name& rSensorName, kt_int32s scanIndex) + { + ScanManager* pScanManager = GetScanManager(rSensorName); + if (pScanManager != NULL) + { + return pScanManager->GetScans().at(scanIndex); + } + + assert(false); + return NULL; + } + + /** + * Gets last scan of given device + * @param pLaserRangeFinder + * @return last localized range scan of device + */ + inline LocalizedRangeScan* MapperSensorManager::GetLastScan(const Name& rSensorName) + { + RegisterSensor(rSensorName); + + return GetScanManager(rSensorName)->GetLastScan(); + } + + /** + * Sets the last scan of device of given scan + * @param pScan + */ + inline void MapperSensorManager::SetLastScan(LocalizedRangeScan* pScan) + { + GetScanManager(pScan)->SetLastScan(pScan); + } + + /** + * Adds scan to scan vector of device that recorded scan + * @param pScan + */ + void MapperSensorManager::AddScan(LocalizedRangeScan* pScan) + { + GetScanManager(pScan)->AddScan(pScan, m_NextScanId); + m_Scans.push_back(pScan); + m_NextScanId++; + } + + /** + * Adds scan to running scans of device that recorded scan + * @param pScan + */ + inline void MapperSensorManager::AddRunningScan(LocalizedRangeScan* pScan) + { + GetScanManager(pScan)->AddRunningScan(pScan); + } + + /** + * Gets scans of device + * @param rSensorName + * @return scans of device + */ + inline LocalizedRangeScanVector& MapperSensorManager::GetScans(const Name& rSensorName) + { + return GetScanManager(rSensorName)->GetScans(); + } + + /** + * Gets running scans of device + * @param rSensorName + * @return running scans of device + */ + inline LocalizedRangeScanVector& MapperSensorManager::GetRunningScans(const Name& rSensorName) + { + return GetScanManager(rSensorName)->GetRunningScans(); + } + + /** + * Gets all scans of all devices + * @return all scans of all devices + */ + LocalizedRangeScanVector MapperSensorManager::GetAllScans() + { + LocalizedRangeScanVector scans; + + forEach(ScanManagerMap, &m_ScanManagers) + { + LocalizedRangeScanVector& rScans = iter->second->GetScans(); + + scans.insert(scans.end(), rScans.begin(), rScans.end()); + } + + return scans; + } + + /** + * Deletes all scan managers of all devices + */ + void MapperSensorManager::Clear() + { +// SensorManager::Clear(); + + forEach(ScanManagerMap, &m_ScanManagers) + { + delete iter->second; + } + + m_ScanManagers.clear(); + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + ScanMatcher::~ScanMatcher() + { + if (m_pCorrelationGrid) + { + delete m_pCorrelationGrid; + } + if (m_pSearchSpaceProbs) + { + delete m_pSearchSpaceProbs; + } + if (m_pGridLookup) + { + delete m_pGridLookup; + } + + } + + ScanMatcher* ScanMatcher::Create(Mapper* pMapper, kt_double searchSize, kt_double resolution, + kt_double smearDeviation, kt_double rangeThreshold) + { + // invalid parameters + if (resolution <= 0) + { + return NULL; + } + if (searchSize <= 0) + { + return NULL; + } + if (smearDeviation < 0) + { + return NULL; + } + if (rangeThreshold <= 0) + { + return NULL; + } + + assert(math::DoubleEqual(math::Round(searchSize / resolution), (searchSize / resolution))); + + // calculate search space in grid coordinates + kt_int32u searchSpaceSideSize = static_cast(math::Round(searchSize / resolution) + 1); + + // compute requisite size of correlation grid (pad grid so that scan points can't fall off the grid + // if a scan is on the border of the search space) + kt_int32u pointReadingMargin = static_cast(ceil(rangeThreshold / resolution)); + + kt_int32s gridSize = searchSpaceSideSize + 2 * pointReadingMargin; + + // create correlation grid + assert(gridSize % 2 == 1); + CorrelationGrid* pCorrelationGrid = CorrelationGrid::CreateGrid(gridSize, gridSize, resolution, smearDeviation); + + // create search space probabilities + Grid* pSearchSpaceProbs = Grid::CreateGrid(searchSpaceSideSize, + searchSpaceSideSize, resolution); + + ScanMatcher* pScanMatcher = new ScanMatcher(pMapper); + pScanMatcher->m_pCorrelationGrid = pCorrelationGrid; + pScanMatcher->m_pSearchSpaceProbs = pSearchSpaceProbs; + pScanMatcher->m_pGridLookup = new GridIndexLookup(pCorrelationGrid); + + return pScanMatcher; + } + + /** + * Match given scan against set of scans + * @param pScan scan being scan-matched + * @param rBaseScans set of scans whose points will mark cells in grid as being occupied + * @param rMean output parameter of mean (best pose) of match + * @param rCovariance output parameter of covariance of match + * @param doPenalize whether to penalize matches further from the search center + * @param doRefineMatch whether to do finer-grained matching if coarse match is good (default is true) + * @return strength of response + */ + kt_double ScanMatcher::MatchScan(LocalizedRangeScan* pScan, const LocalizedRangeScanVector& rBaseScans, Pose2& rMean, + Matrix3& rCovariance, kt_bool doPenalize, kt_bool doRefineMatch) + { + /////////////////////////////////////// + // set scan pose to be center of grid + + // 1. get scan position + Pose2 scanPose = pScan->GetSensorPose(); + + // scan has no readings; cannot do scan matching + // best guess of pose is based off of adjusted odometer reading + if (pScan->GetNumberOfRangeReadings() == 0) + { + rMean = scanPose; + + // maximum covariance + rCovariance(0, 0) = MAX_VARIANCE; // XX + rCovariance(1, 1) = MAX_VARIANCE; // YY + rCovariance(2, 2) = 4 * math::Square(m_pMapper->m_pCoarseAngleResolution->GetValue()); // TH*TH + + return 0.0; + } + + // 2. get size of grid + Rectangle2 roi = m_pCorrelationGrid->GetROI(); + + // 3. compute offset (in meters - lower left corner) + Vector2 offset; + offset.SetX(scanPose.GetX() - (0.5 * (roi.GetWidth() - 1) * m_pCorrelationGrid->GetResolution())); + offset.SetY(scanPose.GetY() - (0.5 * (roi.GetHeight() - 1) * m_pCorrelationGrid->GetResolution())); + + // 4. set offset + m_pCorrelationGrid->GetCoordinateConverter()->SetOffset(offset); + + /////////////////////////////////////// + + // set up correlation grid + AddScans(rBaseScans, scanPose.GetPosition()); + + // compute how far to search in each direction + Vector2 searchDimensions(m_pSearchSpaceProbs->GetWidth(), m_pSearchSpaceProbs->GetHeight()); + Vector2 coarseSearchOffset(0.5 * (searchDimensions.GetX() - 1) * m_pCorrelationGrid->GetResolution(), + 0.5 * (searchDimensions.GetY() - 1) * m_pCorrelationGrid->GetResolution()); + + // a coarse search only checks half the cells in each dimension + Vector2 coarseSearchResolution(2 * m_pCorrelationGrid->GetResolution(), + 2 * m_pCorrelationGrid->GetResolution()); + + // actual scan-matching + kt_double bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution, + m_pMapper->m_pCoarseSearchAngleOffset->GetValue(), + m_pMapper->m_pCoarseAngleResolution->GetValue(), + doPenalize, rMean, rCovariance, false); + + if (m_pMapper->m_pUseResponseExpansion->GetValue() == true) + { + if (math::DoubleEqual(bestResponse, 0.0)) + { +#ifdef KARTO_DEBUG + std::cout << "Mapper Info: Expanding response search space!" << std::endl; +#endif + // try and increase search angle offset with 20 degrees and do another match + kt_double newSearchAngleOffset = m_pMapper->m_pCoarseSearchAngleOffset->GetValue(); + for (kt_int32u i = 0; i < 3; i++) + { + newSearchAngleOffset += math::DegreesToRadians(20); + + bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution, + newSearchAngleOffset, m_pMapper->m_pCoarseAngleResolution->GetValue(), + doPenalize, rMean, rCovariance, false); + + if (math::DoubleEqual(bestResponse, 0.0) == false) + { + break; + } + } + +#ifdef KARTO_DEBUG + if (math::DoubleEqual(bestResponse, 0.0)) + { + std::cout << "Mapper Warning: Unable to calculate response!" << std::endl; + } +#endif + } + } + + if (doRefineMatch) + { + Vector2 fineSearchOffset(coarseSearchResolution * 0.5); + Vector2 fineSearchResolution(m_pCorrelationGrid->GetResolution(), m_pCorrelationGrid->GetResolution()); + bestResponse = CorrelateScan(pScan, rMean, fineSearchOffset, fineSearchResolution, + 0.5 * m_pMapper->m_pCoarseAngleResolution->GetValue(), + m_pMapper->m_pFineSearchAngleOffset->GetValue(), + doPenalize, rMean, rCovariance, true); + } + +#ifdef KARTO_DEBUG + std::cout << " BEST POSE = " << rMean << " BEST RESPONSE = " << bestResponse << ", VARIANCE = " + << rCovariance(0, 0) << ", " << rCovariance(1, 1) << std::endl; +#endif + assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI)); + + return bestResponse; + } + + void ScanMatcher::operator() (const kt_double& y) const + { + kt_int32u poseResponseCounter; + kt_int32u x_pose; + kt_int32u y_pose = std::find(m_yPoses.begin(), m_yPoses.end(), y) - m_yPoses.begin(); + + const kt_int32u size_x = m_xPoses.size(); + + kt_double newPositionY = m_rSearchCenter.GetY() + y; + kt_double squareY = math::Square(y); + + for ( std::vector::const_iterator xIter = m_xPoses.begin(); xIter != m_xPoses.end(); ++xIter ) + { + x_pose = std::distance(m_xPoses.begin(), xIter); + kt_double x = *xIter; + kt_double newPositionX = m_rSearchCenter.GetX() + x; + kt_double squareX = math::Square(x); + + Vector2 gridPoint = m_pCorrelationGrid->WorldToGrid(Vector2(newPositionX, newPositionY)); + kt_int32s gridIndex = m_pCorrelationGrid->GridIndex(gridPoint); + assert(gridIndex >= 0); + + kt_double angle = 0.0; + kt_double startAngle = m_rSearchCenter.GetHeading() - m_searchAngleOffset; + for (kt_int32u angleIndex = 0; angleIndex < m_nAngles; angleIndex++) + { + angle = startAngle + angleIndex * m_searchAngleResolution; + + kt_double response = GetResponse(angleIndex, gridIndex); + if (m_doPenalize && (math::DoubleEqual(response, 0.0) == false)) + { + // simple model (approximate Gaussian) to take odometry into account + kt_double squaredDistance = squareX + squareY; + kt_double distancePenalty = 1.0 - (DISTANCE_PENALTY_GAIN * + squaredDistance / m_pMapper->m_pDistanceVariancePenalty->GetValue()); + distancePenalty = math::Maximum(distancePenalty, m_pMapper->m_pMinimumDistancePenalty->GetValue()); + + kt_double squaredAngleDistance = math::Square(angle - m_rSearchCenter.GetHeading()); + kt_double anglePenalty = 1.0 - (ANGLE_PENALTY_GAIN * + squaredAngleDistance / m_pMapper->m_pAngleVariancePenalty->GetValue()); + anglePenalty = math::Maximum(anglePenalty, m_pMapper->m_pMinimumAnglePenalty->GetValue()); + + response *= (distancePenalty * anglePenalty); + } + + // store response and pose + poseResponseCounter = (y_pose*size_x + x_pose)*(m_nAngles) + angleIndex; + m_pPoseResponse[poseResponseCounter] = std::pair(response, Pose2(newPositionX, newPositionY, + math::NormalizeAngle(angle))); + } + } + return; + } + + /** + * Finds the best pose for the scan centering the search in the correlation grid + * at the given pose and search in the space by the vector and angular offsets + * in increments of the given resolutions + * @param rScan scan to match against correlation grid + * @param rSearchCenter the center of the search space + * @param rSearchSpaceOffset searches poses in the area offset by this vector around search center + * @param rSearchSpaceResolution how fine a granularity to search in the search space + * @param searchAngleOffset searches poses in the angles offset by this angle around search center + * @param searchAngleResolution how fine a granularity to search in the angular search space + * @param doPenalize whether to penalize matches further from the search center + * @param rMean output parameter of mean (best pose) of match + * @param rCovariance output parameter of covariance of match + * @param doingFineMatch whether to do a finer search after coarse search + * @return strength of response + */ + kt_double ScanMatcher::CorrelateScan(LocalizedRangeScan* pScan, const Pose2& rSearchCenter, + const Vector2& rSearchSpaceOffset, + const Vector2& rSearchSpaceResolution, + kt_double searchAngleOffset, kt_double searchAngleResolution, + kt_bool doPenalize, Pose2& rMean, Matrix3& rCovariance, kt_bool doingFineMatch) + { + assert(searchAngleResolution != 0.0); + + // setup lookup arrays + m_pGridLookup->ComputeOffsets(pScan, rSearchCenter.GetHeading(), searchAngleOffset, searchAngleResolution); + + // only initialize probability grid if computing positional covariance (during coarse match) + if (!doingFineMatch) + { + m_pSearchSpaceProbs->Clear(); + + // position search grid - finds lower left corner of search grid + Vector2 offset(rSearchCenter.GetPosition() - rSearchSpaceOffset); + m_pSearchSpaceProbs->GetCoordinateConverter()->SetOffset(offset); + } + + // calculate position arrays + + m_xPoses.clear(); + kt_int32u nX = static_cast(math::Round(rSearchSpaceOffset.GetX() * + 2.0 / rSearchSpaceResolution.GetX()) + 1); + kt_double startX = -rSearchSpaceOffset.GetX(); + for (kt_int32u xIndex = 0; xIndex < nX; xIndex++) + { + m_xPoses.push_back(startX + xIndex * rSearchSpaceResolution.GetX()); + } + assert(math::DoubleEqual(m_xPoses.back(), -startX)); + + m_yPoses.clear(); + kt_int32u nY = static_cast(math::Round(rSearchSpaceOffset.GetY() * + 2.0 / rSearchSpaceResolution.GetY()) + 1); + kt_double startY = -rSearchSpaceOffset.GetY(); + for (kt_int32u yIndex = 0; yIndex < nY; yIndex++) + { + m_yPoses.push_back(startY + yIndex * rSearchSpaceResolution.GetY()); + } + assert(math::DoubleEqual(m_yPoses.back(), -startY)); + + // calculate pose response array size + kt_int32u nAngles = static_cast(math::Round(searchAngleOffset * 2.0 / searchAngleResolution) + 1); + + kt_int32u poseResponseSize = static_cast(m_xPoses.size() * m_yPoses.size() * nAngles); + + // allocate array + m_pPoseResponse = new std::pair[poseResponseSize]; + + Vector2 startGridPoint = m_pCorrelationGrid->WorldToGrid(Vector2(rSearchCenter.GetX() + + startX, rSearchCenter.GetY() + startY)); + + // this isn't good but its the fastest way to iterate. Should clean up later. + m_rSearchCenter = rSearchCenter; + m_searchAngleOffset = searchAngleOffset; + m_nAngles = nAngles; + m_searchAngleResolution = searchAngleResolution; + m_doPenalize = doPenalize; + tbb::parallel_do(m_yPoses, (*this) ); + + // find value of best response (in [0; 1]) + kt_double bestResponse = -1; + for (kt_int32u i = 0; i < poseResponseSize; i++) + { + bestResponse = math::Maximum(bestResponse, m_pPoseResponse[i].first); + + // will compute positional covariance, save best relative probability for each cell + if (!doingFineMatch) + { + const Pose2& rPose = m_pPoseResponse[i].second; + Vector2 grid = m_pSearchSpaceProbs->WorldToGrid(rPose.GetPosition()); + kt_double* ptr; + + try + { + ptr = (kt_double*)(m_pSearchSpaceProbs->GetDataPointer(grid)); + } + catch(...) + { + throw std::runtime_error("Mapper FATAL ERROR - unable to get pointer in probability search!"); + } + + if (ptr == NULL) + { + throw std::runtime_error("Mapper FATAL ERROR - Index out of range in probability search!"); + } + + *ptr = math::Maximum(m_pPoseResponse[i].first, *ptr); + } + } + + // average all poses with same highest response + Vector2 averagePosition; + kt_double thetaX = 0.0; + kt_double thetaY = 0.0; + kt_int32s averagePoseCount = 0; + for (kt_int32u i = 0; i < poseResponseSize; i++) + { + if (math::DoubleEqual(m_pPoseResponse[i].first, bestResponse)) + { + averagePosition += m_pPoseResponse[i].second.GetPosition(); + + kt_double heading = m_pPoseResponse[i].second.GetHeading(); + thetaX += cos(heading); + thetaY += sin(heading); + + averagePoseCount++; + } + } + + Pose2 averagePose; + if (averagePoseCount > 0) + { + averagePosition /= averagePoseCount; + + thetaX /= averagePoseCount; + thetaY /= averagePoseCount; + + averagePose = Pose2(averagePosition, atan2(thetaY, thetaX)); + } + else + { + throw std::runtime_error("Mapper FATAL ERROR - Unable to find best position"); + } + + // delete pose response array + delete [] m_pPoseResponse; + +#ifdef KARTO_DEBUG + std::cout << "bestPose: " << averagePose << std::endl; + std::cout << "bestResponse: " << bestResponse << std::endl; +#endif + + if (!doingFineMatch) + { + ComputePositionalCovariance(averagePose, bestResponse, rSearchCenter, rSearchSpaceOffset, + rSearchSpaceResolution, searchAngleResolution, rCovariance); + } + else + { + ComputeAngularCovariance(averagePose, bestResponse, rSearchCenter, + searchAngleOffset, searchAngleResolution, rCovariance); + } + + rMean = averagePose; + +#ifdef KARTO_DEBUG + std::cout << "bestPose: " << averagePose << std::endl; +#endif + + if (bestResponse > 1.0) + { + bestResponse = 1.0; + } + + assert(math::InRange(bestResponse, 0.0, 1.0)); + assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI)); + + return bestResponse; + } + + /** + * Computes the positional covariance of the best pose + * @param rBestPose + * @param bestResponse + * @param rSearchCenter + * @param rSearchSpaceOffset + * @param rSearchSpaceResolution + * @param searchAngleResolution + * @param rCovariance + */ + void ScanMatcher::ComputePositionalCovariance(const Pose2& rBestPose, kt_double bestResponse, + const Pose2& rSearchCenter, + const Vector2& rSearchSpaceOffset, + const Vector2& rSearchSpaceResolution, + kt_double searchAngleResolution, Matrix3& rCovariance) + { + // reset covariance to identity matrix + rCovariance.SetToIdentity(); + + // if best response is vary small return max variance + if (bestResponse < KT_TOLERANCE) + { + rCovariance(0, 0) = MAX_VARIANCE; // XX + rCovariance(1, 1) = MAX_VARIANCE; // YY + rCovariance(2, 2) = 4 * math::Square(searchAngleResolution); // TH*TH + + return; + } + + kt_double accumulatedVarianceXX = 0; + kt_double accumulatedVarianceXY = 0; + kt_double accumulatedVarianceYY = 0; + kt_double norm = 0; + + kt_double dx = rBestPose.GetX() - rSearchCenter.GetX(); + kt_double dy = rBestPose.GetY() - rSearchCenter.GetY(); + + kt_double offsetX = rSearchSpaceOffset.GetX(); + kt_double offsetY = rSearchSpaceOffset.GetY(); + + kt_int32u nX = static_cast(math::Round(offsetX * 2.0 / rSearchSpaceResolution.GetX()) + 1); + kt_double startX = -offsetX; + assert(math::DoubleEqual(startX + (nX - 1) * rSearchSpaceResolution.GetX(), -startX)); + + kt_int32u nY = static_cast(math::Round(offsetY * 2.0 / rSearchSpaceResolution.GetY()) + 1); + kt_double startY = -offsetY; + assert(math::DoubleEqual(startY + (nY - 1) * rSearchSpaceResolution.GetY(), -startY)); + + for (kt_int32u yIndex = 0; yIndex < nY; yIndex++) + { + kt_double y = startY + yIndex * rSearchSpaceResolution.GetY(); + + for (kt_int32u xIndex = 0; xIndex < nX; xIndex++) + { + kt_double x = startX + xIndex * rSearchSpaceResolution.GetX(); + + Vector2 gridPoint = m_pSearchSpaceProbs->WorldToGrid(Vector2(rSearchCenter.GetX() + x, + rSearchCenter.GetY() + y)); + kt_double response = *(m_pSearchSpaceProbs->GetDataPointer(gridPoint)); + + // response is not a low response + if (response >= (bestResponse - 0.1)) + { + norm += response; + accumulatedVarianceXX += (math::Square(x - dx) * response); + accumulatedVarianceXY += ((x - dx) * (y - dy) * response); + accumulatedVarianceYY += (math::Square(y - dy) * response); + } + } + } + + if (norm > KT_TOLERANCE) + { + kt_double varianceXX = accumulatedVarianceXX / norm; + kt_double varianceXY = accumulatedVarianceXY / norm; + kt_double varianceYY = accumulatedVarianceYY / norm; + kt_double varianceTHTH = 4 * math::Square(searchAngleResolution); + + // lower-bound variances so that they are not too small; + // ensures that links are not too tight + kt_double minVarianceXX = 0.1 * math::Square(rSearchSpaceResolution.GetX()); + kt_double minVarianceYY = 0.1 * math::Square(rSearchSpaceResolution.GetY()); + varianceXX = math::Maximum(varianceXX, minVarianceXX); + varianceYY = math::Maximum(varianceYY, minVarianceYY); + + // increase variance for poorer responses + kt_double multiplier = 1.0 / bestResponse; + rCovariance(0, 0) = varianceXX * multiplier; + rCovariance(0, 1) = varianceXY * multiplier; + rCovariance(1, 0) = varianceXY * multiplier; + rCovariance(1, 1) = varianceYY * multiplier; + rCovariance(2, 2) = varianceTHTH; // this value will be set in ComputeAngularCovariance + } + + // if values are 0, set to MAX_VARIANCE + // values might be 0 if points are too sparse and thus don't hit other points + if (math::DoubleEqual(rCovariance(0, 0), 0.0)) + { + rCovariance(0, 0) = MAX_VARIANCE; + } + + if (math::DoubleEqual(rCovariance(1, 1), 0.0)) + { + rCovariance(1, 1) = MAX_VARIANCE; + } + } + + /** + * Computes the angular covariance of the best pose + * @param rBestPose + * @param bestResponse + * @param rSearchCenter + * @param rSearchAngleOffset + * @param searchAngleResolution + * @param rCovariance + */ + void ScanMatcher::ComputeAngularCovariance(const Pose2& rBestPose, + kt_double bestResponse, + const Pose2& rSearchCenter, + kt_double searchAngleOffset, + kt_double searchAngleResolution, + Matrix3& rCovariance) + { + // NOTE: do not reset covariance matrix + + // normalize angle difference + kt_double bestAngle = math::NormalizeAngleDifference(rBestPose.GetHeading(), rSearchCenter.GetHeading()); + + Vector2 gridPoint = m_pCorrelationGrid->WorldToGrid(rBestPose.GetPosition()); + kt_int32s gridIndex = m_pCorrelationGrid->GridIndex(gridPoint); + + kt_int32u nAngles = static_cast(math::Round(searchAngleOffset * 2 / searchAngleResolution) + 1); + + kt_double angle = 0.0; + kt_double startAngle = rSearchCenter.GetHeading() - searchAngleOffset; + + kt_double norm = 0.0; + kt_double accumulatedVarianceThTh = 0.0; + for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++) + { + angle = startAngle + angleIndex * searchAngleResolution; + kt_double response = GetResponse(angleIndex, gridIndex); + + // response is not a low response + if (response >= (bestResponse - 0.1)) + { + norm += response; + accumulatedVarianceThTh += (math::Square(angle - bestAngle) * response); + } + } + assert(math::DoubleEqual(angle, rSearchCenter.GetHeading() + searchAngleOffset)); + + if (norm > KT_TOLERANCE) + { + if (accumulatedVarianceThTh < KT_TOLERANCE) + { + accumulatedVarianceThTh = math::Square(searchAngleResolution); + } + + accumulatedVarianceThTh /= norm; + } + else + { + accumulatedVarianceThTh = 1000 * math::Square(searchAngleResolution); + } + + rCovariance(2, 2) = accumulatedVarianceThTh; + } + + /** + * Marks cells where scans' points hit as being occupied + * @param rScans scans whose points will mark cells in grid as being occupied + * @param viewPoint do not add points that belong to scans "opposite" the view point + */ + void ScanMatcher::AddScans(const LocalizedRangeScanVector& rScans, Vector2 viewPoint) + { + m_pCorrelationGrid->Clear(); + + // add all scans to grid + const_forEach(LocalizedRangeScanVector, &rScans) + { + AddScan(*iter, viewPoint); + } + } + + /** + * Marks cells where scans' points hit as being occupied. Can smear points as they are added. + * @param pScan scan whose points will mark cells in grid as being occupied + * @param viewPoint do not add points that belong to scans "opposite" the view point + * @param doSmear whether the points will be smeared + */ + void ScanMatcher::AddScan(LocalizedRangeScan* pScan, const Vector2& rViewPoint, kt_bool doSmear) + { + PointVectorDouble validPoints = FindValidPoints(pScan, rViewPoint); + + // put in all valid points + const_forEach(PointVectorDouble, &validPoints) + { + Vector2 gridPoint = m_pCorrelationGrid->WorldToGrid(*iter); + if (!math::IsUpTo(gridPoint.GetX(), m_pCorrelationGrid->GetROI().GetWidth()) || + !math::IsUpTo(gridPoint.GetY(), m_pCorrelationGrid->GetROI().GetHeight())) + { + // point not in grid + continue; + } + + int gridIndex = m_pCorrelationGrid->GridIndex(gridPoint); + + // set grid cell as occupied + if (m_pCorrelationGrid->GetDataPointer()[gridIndex] == GridStates_Occupied) + { + // value already set + continue; + } + + m_pCorrelationGrid->GetDataPointer()[gridIndex] = GridStates_Occupied; + + // smear grid + if (doSmear == true) + { + m_pCorrelationGrid->SmearPoint(gridPoint); + } + } + } + + /** + * Compute which points in a scan are on the same side as the given viewpoint + * @param pScan + * @param rViewPoint + * @return points on the same side + */ + PointVectorDouble ScanMatcher::FindValidPoints(LocalizedRangeScan* pScan, const Vector2& rViewPoint) const + { + const PointVectorDouble& rPointReadings = pScan->GetPointReadings(); + + // points must be at least 10 cm away when making comparisons of inside/outside of viewpoint + const kt_double minSquareDistance = math::Square(0.1); // in m^2 + + // this iterator lags from the main iterator adding points only when the points are on + // the same side as the viewpoint + PointVectorDouble::const_iterator trailingPointIter = rPointReadings.begin(); + PointVectorDouble validPoints; + + Vector2 firstPoint; + kt_bool firstTime = true; + const_forEach(PointVectorDouble, &rPointReadings) + { + Vector2 currentPoint = *iter; + + if (firstTime && !std::isnan(currentPoint.GetX()) && !std::isnan(currentPoint.GetY())) + { + firstPoint = currentPoint; + firstTime = false; + } + + Vector2 delta = firstPoint - currentPoint; + if (delta.SquaredLength() > minSquareDistance) + { + // This compute the Determinant (viewPoint FirstPoint, viewPoint currentPoint) + // Which computes the direction of rotation, if the rotation is counterclock + // wise then we are looking at data we should keep. If it's negative rotation + // we should not included in in the matching + // have enough distance, check viewpoint + double a = rViewPoint.GetY() - firstPoint.GetY(); + double b = firstPoint.GetX() - rViewPoint.GetX(); + double c = firstPoint.GetY() * rViewPoint.GetX() - firstPoint.GetX() * rViewPoint.GetY(); + double ss = currentPoint.GetX() * a + currentPoint.GetY() * b + c; + + // reset beginning point + firstPoint = currentPoint; + + if (ss < 0.0) // wrong side, skip and keep going + { + trailingPointIter = iter; + } + else + { + for (; trailingPointIter != iter; ++trailingPointIter) + { + validPoints.push_back(*trailingPointIter); + } + } + } + } + + return validPoints; + } + + /** + * Get response at given position for given rotation (only look up valid points) + * @param angleIndex + * @param gridPositionIndex + * @return response + */ + kt_double ScanMatcher::GetResponse(kt_int32u angleIndex, kt_int32s gridPositionIndex) const + { + kt_double response = 0.0; + + // add up value for each point + kt_int8u* pByte = m_pCorrelationGrid->GetDataPointer() + gridPositionIndex; + + const LookupArray* pOffsets = m_pGridLookup->GetLookupArray(angleIndex); + assert(pOffsets != NULL); + + // get number of points in offset list + kt_int32u nPoints = pOffsets->GetSize(); + if (nPoints == 0) + { + return response; + } + + // calculate response + kt_int32s* pAngleIndexPointer = pOffsets->GetArrayPointer(); + for (kt_int32u i = 0; i < nPoints; i++) + { + // ignore points that fall off the grid + kt_int32s pointGridIndex = gridPositionIndex + pAngleIndexPointer[i]; + if (!math::IsUpTo(pointGridIndex, m_pCorrelationGrid->GetDataSize()) || pAngleIndexPointer[i] == INVALID_SCAN) + { + continue; + } + + // uses index offsets to efficiently find location of point in the grid + response += pByte[pAngleIndexPointer[i]]; + } + + // normalize response + response /= (nPoints * GridStates_Occupied); + assert(fabs(response) <= 1.0); + + return response; + } + + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + template + class BreadthFirstTraversal : public GraphTraversal + { + public: + /** + * Constructs a breadth-first traverser for the given graph + */ + BreadthFirstTraversal() + { + } + BreadthFirstTraversal(Graph* pGraph) + : GraphTraversal(pGraph) + { + } + + /** + * Destructor + */ + virtual ~BreadthFirstTraversal() + { + } + + public: + /** + * Traverse the graph starting with the given vertex; applies the visitor to visited nodes + * @param pStartVertex + * @param pVisitor + * @return visited vertices + */ + virtual std::vector Traverse(Vertex* pStartVertex, Visitor* pVisitor) + { + std::queue*> toVisit; + std::set*> seenVertices; + std::vector*> validVertices; + + toVisit.push(pStartVertex); + seenVertices.insert(pStartVertex); + + do + { + Vertex* pNext = toVisit.front(); + toVisit.pop(); + + if (pVisitor->Visit(pNext)) + { + // vertex is valid, explore neighbors + validVertices.push_back(pNext); + + std::vector*> adjacentVertices = pNext->GetAdjacentVertices(); + forEach(typename std::vector*>, &adjacentVertices) + { + Vertex* pAdjacent = *iter; + + // adjacent vertex has not yet been seen, add to queue for processing + if (seenVertices.find(pAdjacent) == seenVertices.end()) + { + toVisit.push(pAdjacent); + seenVertices.insert(pAdjacent); + } + } + } + } while (toVisit.empty() == false); + + std::vector objects; + forEach(typename std::vector*>, &validVertices) + { + objects.push_back((*iter)->GetObject()); + } + + return objects; + } + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GraphTraversal); + } + }; // class BreadthFirstTraversal + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + class NearScanVisitor : public Visitor + { + public: + NearScanVisitor(LocalizedRangeScan* pScan, kt_double maxDistance, kt_bool useScanBarycenter) + : m_MaxDistanceSquared(math::Square(maxDistance)) + , m_UseScanBarycenter(useScanBarycenter) + { + m_CenterPose = pScan->GetReferencePose(m_UseScanBarycenter); + } + + virtual kt_bool Visit(Vertex* pVertex) + { + LocalizedRangeScan* pScan = pVertex->GetObject(); + + Pose2 pose = pScan->GetReferencePose(m_UseScanBarycenter); + + kt_double squaredDistance = pose.GetPosition().SquaredDistance(m_CenterPose.GetPosition()); + return (squaredDistance <= m_MaxDistanceSquared - KT_TOLERANCE); + } + + protected: + Pose2 m_CenterPose; + kt_double m_MaxDistanceSquared; + kt_bool m_UseScanBarycenter; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Visitor); + ar & BOOST_SERIALIZATION_NVP(m_CenterPose); + ar & BOOST_SERIALIZATION_NVP(m_MaxDistanceSquared); + ar & BOOST_SERIALIZATION_NVP(m_UseScanBarycenter); + } + }; // NearScanVisitor + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + MapperGraph::MapperGraph(Mapper* pMapper, kt_double rangeThreshold) + : m_pMapper(pMapper) + { + m_pLoopScanMatcher = ScanMatcher::Create(pMapper, m_pMapper->m_pLoopSearchSpaceDimension->GetValue(), + m_pMapper->m_pLoopSearchSpaceResolution->GetValue(), + m_pMapper->m_pLoopSearchSpaceSmearDeviation->GetValue(), rangeThreshold); + assert(m_pLoopScanMatcher); + + m_pTraversal = new BreadthFirstTraversal(this); + } + + MapperGraph::~MapperGraph() + { + if (m_pLoopScanMatcher) + { + delete m_pLoopScanMatcher; + m_pLoopScanMatcher = NULL; + } + if (m_pTraversal) + { + delete m_pTraversal; + m_pTraversal = NULL; + } + } + + void MapperGraph::AddVertex(LocalizedRangeScan* pScan) + { + assert(pScan); + + if (pScan != NULL) + { + Vertex* pVertex = new Vertex(pScan); + Graph::AddVertex(pScan->GetSensorName(), pVertex); + if (m_pMapper->m_pScanOptimizer != NULL) + { + m_pMapper->m_pScanOptimizer->AddNode(pVertex); + } + } + } + + void MapperGraph::AddEdges(LocalizedRangeScan* pScan, const Matrix3& rCovariance) + { + MapperSensorManager* pSensorManager = m_pMapper->m_pMapperSensorManager; + + const Name& rSensorName = pScan->GetSensorName(); + + // link to previous scan + kt_int32s previousScanNum = pScan->GetStateId() - 1; + if (pSensorManager->GetLastScan(rSensorName) != NULL) + { + assert(previousScanNum >= 0); + LinkScans(pSensorManager->GetScan(rSensorName, previousScanNum), pScan, pScan->GetSensorPose(), rCovariance); + } + + Pose2Vector means; + std::vector covariances; + + // first scan (link to first scan of other robots) + if (pSensorManager->GetLastScan(rSensorName) == NULL) + { + assert(pSensorManager->GetScans(rSensorName).size() == 1); + + std::vector deviceNames = pSensorManager->GetSensorNames(); + forEach(std::vector, &deviceNames) + { + const Name& rCandidateSensorName = *iter; + + // skip if candidate device is the same or other device has no scans + if ((rCandidateSensorName == rSensorName) || (pSensorManager->GetScans(rCandidateSensorName).empty())) + { + continue; + } + + Pose2 bestPose; + Matrix3 covariance; + kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan, + pSensorManager->GetScans(rCandidateSensorName), + bestPose, covariance); + LinkScans(pScan, pSensorManager->GetScan(rCandidateSensorName, 0), bestPose, covariance); + + // only add to means and covariances if response was high "enough" + if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue()) + { + means.push_back(bestPose); + covariances.push_back(covariance); + } + } + } + else + { + // link to running scans + Pose2 scanPose = pScan->GetSensorPose(); + means.push_back(scanPose); + covariances.push_back(rCovariance); + LinkChainToScan(pSensorManager->GetRunningScans(rSensorName), pScan, scanPose, rCovariance); + } + + // link to other near chains (chains that include new scan are invalid) + LinkNearChains(pScan, means, covariances); + + if (!means.empty()) + { + pScan->SetSensorPose(ComputeWeightedMean(means, covariances)); + } + } + + kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName) + { + kt_bool loopClosed = false; + + kt_int32u scanIndex = 0; + + LocalizedRangeScanVector candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex); + + while (!candidateChain.empty()) + { + Pose2 bestPose; + Matrix3 covariance; + kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain, + bestPose, covariance, false, false); + + std::stringstream stream; + stream << "COARSE RESPONSE: " << coarseResponse + << " (> " << m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue() << ")" + << std::endl; + stream << " var: " << covariance(0, 0) << ", " << covariance(1, 1) + << " (< " << m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue() << ")"; + + m_pMapper->FireLoopClosureCheck(stream.str()); + + if ((coarseResponse > m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) && + (covariance(0, 0) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) && + (covariance(1, 1) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue())) + { + LocalizedRangeScan tmpScan(pScan->GetSensorName(), pScan->GetRangeReadingsVector()); + tmpScan.SetUniqueId(pScan->GetUniqueId()); + tmpScan.SetTime(pScan->GetTime()); + tmpScan.SetStateId(pScan->GetStateId()); + tmpScan.SetCorrectedPose(pScan->GetCorrectedPose()); + tmpScan.SetSensorPose(bestPose); // This also updates OdometricPose. + kt_double fineResponse = m_pMapper->m_pSequentialScanMatcher->MatchScan(&tmpScan, candidateChain, + bestPose, covariance, false); + + std::stringstream stream1; + stream1 << "FINE RESPONSE: " << fineResponse << " (>" + << m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue() << ")" << std::endl; + m_pMapper->FireLoopClosureCheck(stream1.str()); + + if (fineResponse < m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue()) + { + m_pMapper->FireLoopClosureCheck("REJECTED!"); + } + else + { + m_pMapper->FireBeginLoopClosure("Closing loop..."); + + pScan->SetSensorPose(bestPose); + LinkChainToScan(candidateChain, pScan, bestPose, covariance); + CorrectPoses(); + + m_pMapper->FireEndLoopClosure("Loop closed!"); + + loopClosed = true; + } + } + + candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex); + } + + return loopClosed; + } + + LocalizedRangeScan* MapperGraph::GetClosestScanToPose(const LocalizedRangeScanVector& rScans, + const Pose2& rPose) const + { + LocalizedRangeScan* pClosestScan = NULL; + kt_double bestSquaredDistance = DBL_MAX; + + const_forEach(LocalizedRangeScanVector, &rScans) + { + Pose2 scanPose = (*iter)->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); + + kt_double squaredDistance = rPose.GetPosition().SquaredDistance(scanPose.GetPosition()); + if (squaredDistance < bestSquaredDistance) + { + bestSquaredDistance = squaredDistance; + pClosestScan = *iter; + } + } + + return pClosestScan; + } + + Edge* MapperGraph::AddEdge(LocalizedRangeScan* pSourceScan, + LocalizedRangeScan* pTargetScan, kt_bool& rIsNewEdge) + { + // check that vertex exists + assert(pSourceScan->GetStateId() < (kt_int32s)m_Vertices[pSourceScan->GetSensorName()].size()); + assert(pTargetScan->GetStateId() < (kt_int32s)m_Vertices[pTargetScan->GetSensorName()].size()); + + Vertex* v1 = m_Vertices[pSourceScan->GetSensorName()][pSourceScan->GetStateId()]; + Vertex* v2 = m_Vertices[pTargetScan->GetSensorName()][pTargetScan->GetStateId()]; + + // see if edge already exists + const_forEach(std::vector*>, &(v1->GetEdges())) + { + Edge* pEdge = *iter; + + if (pEdge->GetTarget() == v2) + { + rIsNewEdge = false; + return pEdge; + } + } + + Edge* pEdge = new Edge(v1, v2); + Graph::AddEdge(pEdge); + rIsNewEdge = true; + return pEdge; + } + + void MapperGraph::LinkScans(LocalizedRangeScan* pFromScan, LocalizedRangeScan* pToScan, + const Pose2& rMean, const Matrix3& rCovariance) + { + kt_bool isNewEdge = true; + Edge* pEdge = AddEdge(pFromScan, pToScan, isNewEdge); + + // only attach link information if the edge is new + if (isNewEdge == true) + { + pEdge->SetLabel(new LinkInfo(pFromScan->GetSensorPose(), rMean, rCovariance)); + if (m_pMapper->m_pScanOptimizer != NULL) + { + m_pMapper->m_pScanOptimizer->AddConstraint(pEdge); + } + } + } + + void MapperGraph::LinkNearChains(LocalizedRangeScan* pScan, Pose2Vector& rMeans, std::vector& rCovariances) + { + const std::vector nearChains = FindNearChains(pScan); + const_forEach(std::vector, &nearChains) + { + if (iter->size() < m_pMapper->m_pLoopMatchMinimumChainSize->GetValue()) + { + continue; + } + + Pose2 mean; + Matrix3 covariance; + // match scan against "near" chain + kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan, *iter, mean, covariance, false); + if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue() - KT_TOLERANCE) + { + rMeans.push_back(mean); + rCovariances.push_back(covariance); + LinkChainToScan(*iter, pScan, mean, covariance); + } + } + } + + void MapperGraph::LinkChainToScan(const LocalizedRangeScanVector& rChain, LocalizedRangeScan* pScan, + const Pose2& rMean, const Matrix3& rCovariance) + { + Pose2 pose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); + + LocalizedRangeScan* pClosestScan = GetClosestScanToPose(rChain, pose); + assert(pClosestScan != NULL); + + Pose2 closestScanPose = pClosestScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); + + kt_double squaredDistance = pose.GetPosition().SquaredDistance(closestScanPose.GetPosition()); + if (squaredDistance < math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE) + { + LinkScans(pClosestScan, pScan, rMean, rCovariance); + } + } + + std::vector MapperGraph::FindNearChains(LocalizedRangeScan* pScan) + { + std::vector nearChains; + + Pose2 scanPose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); + + // to keep track of which scans have been added to a chain + LocalizedRangeScanVector processed; + + const LocalizedRangeScanVector nearLinkedScans = FindNearLinkedScans(pScan, + m_pMapper->m_pLinkScanMaximumDistance->GetValue()); + const_forEach(LocalizedRangeScanVector, &nearLinkedScans) + { + LocalizedRangeScan* pNearScan = *iter; + + if (pNearScan == pScan) + { + continue; + } + + // scan has already been processed, skip + if (find(processed.begin(), processed.end(), pNearScan) != processed.end()) + { + continue; + } + + processed.push_back(pNearScan); + + // build up chain + kt_bool isValidChain = true; + std::list chain; + + // add scans before current scan being processed + for (kt_int32s candidateScanNum = pNearScan->GetStateId() - 1; candidateScanNum >= 0; candidateScanNum--) + { + LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(), + candidateScanNum); + + // chain is invalid--contains scan being added + if (pCandidateScan == pScan) + { + isValidChain = false; + } + + Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); + kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition()); + + if (squaredDistance < math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE) + { + chain.push_front(pCandidateScan); + processed.push_back(pCandidateScan); + } + else + { + break; + } + } + + chain.push_back(pNearScan); + + // add scans after current scan being processed + kt_int32u end = static_cast(m_pMapper->m_pMapperSensorManager->GetScans(pNearScan->GetSensorName()).size()); + for (kt_int32u candidateScanNum = pNearScan->GetStateId() + 1; candidateScanNum < end; candidateScanNum++) + { + LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(), + candidateScanNum); + + if (pCandidateScan == pScan) + { + isValidChain = false; + } + + Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());; + kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition()); + + if (squaredDistance < math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE) + { + chain.push_back(pCandidateScan); + processed.push_back(pCandidateScan); + } + else + { + break; + } + } + + if (isValidChain) + { + // change list to vector + LocalizedRangeScanVector tempChain; + std::copy(chain.begin(), chain.end(), std::inserter(tempChain, tempChain.begin())); + // add chain to collection + nearChains.push_back(tempChain); + } + } + + return nearChains; + } + + LocalizedRangeScanVector MapperGraph::FindNearLinkedScans(LocalizedRangeScan* pScan, kt_double maxDistance) + { + NearScanVisitor* pVisitor = new NearScanVisitor(pScan, maxDistance, m_pMapper->m_pUseScanBarycenter->GetValue()); + LocalizedRangeScanVector nearLinkedScans = m_pTraversal->Traverse(GetVertex(pScan), pVisitor); + delete pVisitor; + + return nearLinkedScans; + } + + Pose2 MapperGraph::ComputeWeightedMean(const Pose2Vector& rMeans, const std::vector& rCovariances) const + { + assert(rMeans.size() == rCovariances.size()); + + // compute sum of inverses and create inverse list + std::vector inverses; + inverses.reserve(rCovariances.size()); + + Matrix3 sumOfInverses; + const_forEach(std::vector, &rCovariances) + { + Matrix3 inverse = iter->Inverse(); + inverses.push_back(inverse); + + sumOfInverses += inverse; + } + Matrix3 inverseOfSumOfInverses = sumOfInverses.Inverse(); + + // compute weighted mean + Pose2 accumulatedPose; + kt_double thetaX = 0.0; + kt_double thetaY = 0.0; + + Pose2Vector::const_iterator meansIter = rMeans.begin(); + const_forEach(std::vector, &inverses) + { + Pose2 pose = *meansIter; + kt_double angle = pose.GetHeading(); + thetaX += cos(angle); + thetaY += sin(angle); + + Matrix3 weight = inverseOfSumOfInverses * (*iter); + accumulatedPose += weight * pose; + + ++meansIter; + } + + thetaX /= rMeans.size(); + thetaY /= rMeans.size(); + accumulatedPose.SetHeading(atan2(thetaY, thetaX)); + + return accumulatedPose; + } + + LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure(LocalizedRangeScan* pScan, + const Name& rSensorName, + kt_int32u& rStartNum) + { + LocalizedRangeScanVector chain; // return value + + Pose2 pose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); + + // possible loop closure chain should not include close scans that have a + // path of links to the scan of interest + const LocalizedRangeScanVector nearLinkedScans = + FindNearLinkedScans(pScan, m_pMapper->m_pLoopSearchMaximumDistance->GetValue()); + + kt_int32u nScans = static_cast(m_pMapper->m_pMapperSensorManager->GetScans(rSensorName).size()); + for (; rStartNum < nScans; rStartNum++) + { + LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(rSensorName, rStartNum); + + Pose2 candidateScanPose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); + + kt_double squaredDistance = candidateScanPose.GetPosition().SquaredDistance(pose.GetPosition()); + if (squaredDistance < math::Square(m_pMapper->m_pLoopSearchMaximumDistance->GetValue()) + KT_TOLERANCE) + { + // a linked scan cannot be in the chain + if (find(nearLinkedScans.begin(), nearLinkedScans.end(), pCandidateScan) != nearLinkedScans.end()) + { + chain.clear(); + } + else + { + chain.push_back(pCandidateScan); + } + } + else + { + // return chain if it is long "enough" + if (chain.size() >= m_pMapper->m_pLoopMatchMinimumChainSize->GetValue()) + { + return chain; + } + else + { + chain.clear(); + } + } + } + + return chain; + } + + void MapperGraph::CorrectPoses() + { + // optimize scans! + ScanSolver* pSolver = m_pMapper->m_pScanOptimizer; + if (pSolver != NULL) + { + pSolver->Compute(); + + const_forEach(ScanSolver::IdPoseVector, &pSolver->GetCorrections()) + { + m_pMapper->m_pMapperSensorManager->GetScan(iter->first)->SetSensorPose(iter->second); + } + + pSolver->Clear(); + } + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Default constructor + */ + Mapper::Mapper() + : Module("Mapper") + , m_Initialized(false) + , m_pSequentialScanMatcher(NULL) + , m_pMapperSensorManager(NULL) + , m_pGraph(NULL) + , m_pScanOptimizer(NULL) + { + InitializeParameters(); + } + + /** + * Default constructor + */ + Mapper::Mapper(const std::string& rName) + : Module(rName) + , m_Initialized(false) + , m_pSequentialScanMatcher(NULL) + , m_pMapperSensorManager(NULL) + , m_pGraph(NULL) + , m_pScanOptimizer(NULL) + { + InitializeParameters(); + } + + /** + * Destructor + */ + Mapper::~Mapper() + { + Reset(); + + delete m_pMapperSensorManager; + } + + void Mapper::InitializeParameters() + { + m_pUseScanMatching = new Parameter( + "UseScanMatching", + "When set to true, the mapper will use a scan matching algorithm. " + "In most real-world situations this should be set to true so that the " + "mapper algorithm can correct for noise and errors in odometry and " + "scan data. In some simulator environments where the simulated scan " + "and odometry data are very accurate, the scan matching algorithm can " + "produce worse results. In those cases set this to false to improve " + "results.", + true, + GetParameterManager()); + + m_pUseScanBarycenter = new Parameter( + "UseScanBarycenter", + "Use the barycenter of scan endpoints to define distances between " + "scans.", + true, GetParameterManager()); + + m_pMinimumTimeInterval = new Parameter( + "MinimumTimeInterval", + "Sets the minimum time between scans. If a new scan's time stamp is " + "longer than MinimumTimeInterval from the previously processed scan, " + "the mapper will use the data from the new scan. Otherwise, it will " + "discard the new scan if it also does not meet the minimum travel " + "distance and heading requirements. For performance reasons, it is " + "generally it is a good idea to only process scans if a reasonable " + "amount of time has passed. This parameter is particularly useful " + "when there is a need to process scans while the robot is stationary.", + 3600, GetParameterManager()); + + m_pMinimumTravelDistance = new Parameter( + "MinimumTravelDistance", + "Sets the minimum travel between scans. If a new scan's position is " + "more than minimumTravelDistance from the previous scan, the mapper " + "will use the data from the new scan. Otherwise, it will discard the " + "new scan if it also does not meet the minimum change in heading " + "requirement. For performance reasons, generally it is a good idea to " + "only process scans if the robot has moved a reasonable amount.", + 0.2, GetParameterManager()); + + m_pMinimumTravelHeading = new Parameter( + "MinimumTravelHeading", + "Sets the minimum heading change between scans. If a new scan's " + "heading is more than MinimumTravelHeading from the previous scan, the " + "mapper will use the data from the new scan. Otherwise, it will " + "discard the new scan if it also does not meet the minimum travel " + "distance requirement. For performance reasons, generally it is a good " + "idea to only process scans if the robot has moved a reasonable " + "amount.", + math::DegreesToRadians(10), GetParameterManager()); + + m_pScanBufferSize = new Parameter( + "ScanBufferSize", + "Scan buffer size is the length of the scan chain stored for scan " + "matching. \"ScanBufferSize\" should be set to approximately " + "\"ScanBufferMaximumScanDistance\" / \"MinimumTravelDistance\". The " + "idea is to get an area approximately 20 meters long for scan " + "matching. For example, if we add scans every MinimumTravelDistance == " + "0.3 meters, then \"scanBufferSize\" should be 20 / 0.3 = 67.)", + 70, GetParameterManager()); + + m_pScanBufferMaximumScanDistance = new Parameter( + "ScanBufferMaximumScanDistance", + "Scan buffer maximum scan distance is the maximum distance between the " + "first and last scans in the scan chain stored for matching.", + 20.0, GetParameterManager()); + + m_pLinkMatchMinimumResponseFine = new Parameter( + "LinkMatchMinimumResponseFine", + "Scans are linked only if the correlation response value is greater " + "than this value.", + 0.8, GetParameterManager()); + + m_pLinkScanMaximumDistance = new Parameter( + "LinkScanMaximumDistance", + "Maximum distance between linked scans. Scans that are farther apart " + "will not be linked regardless of the correlation response value.", + 10.0, GetParameterManager()); + + m_pLoopSearchMaximumDistance = new Parameter( + "LoopSearchMaximumDistance", + "Scans less than this distance from the current position will be " + "considered for a match in loop closure.", + 4.0, GetParameterManager()); + + m_pDoLoopClosing = new Parameter( + "DoLoopClosing", + "Enable/disable loop closure.", + true, GetParameterManager()); + + m_pLoopMatchMinimumChainSize = new Parameter( + "LoopMatchMinimumChainSize", + "When the loop closure detection finds a candidate it must be part of " + "a large set of linked scans. If the chain of scans is less than this " + "value we do not attempt to close the loop.", + 10, GetParameterManager()); + + m_pLoopMatchMaximumVarianceCoarse = new Parameter( + "LoopMatchMaximumVarianceCoarse", + "The co-variance values for a possible loop closure have to be less " + "than this value to consider a viable solution. This applies to the " + "coarse search.", + math::Square(0.4), GetParameterManager()); + + m_pLoopMatchMinimumResponseCoarse = new Parameter( + "LoopMatchMinimumResponseCoarse", + "If response is larger then this, then initiate loop closure search at " + "the coarse resolution.", + 0.8, GetParameterManager()); + + m_pLoopMatchMinimumResponseFine = new Parameter( + "LoopMatchMinimumResponseFine", + "If response is larger then this, then initiate loop closure search at " + "the fine resolution.", + 0.8, GetParameterManager()); + + ////////////////////////////////////////////////////////////////////////////// + // CorrelationParameters correlationParameters; + + m_pCorrelationSearchSpaceDimension = new Parameter( + "CorrelationSearchSpaceDimension", + "The size of the search grid used by the matcher. The search grid will " + "have the size CorrelationSearchSpaceDimension * " + "CorrelationSearchSpaceDimension", + 0.3, GetParameterManager()); + + m_pCorrelationSearchSpaceResolution = new Parameter( + "CorrelationSearchSpaceResolution", + "The resolution (size of a grid cell) of the correlation grid.", + 0.01, GetParameterManager()); + + m_pCorrelationSearchSpaceSmearDeviation = new Parameter( + "CorrelationSearchSpaceSmearDeviation", + "The point readings are smeared by this value in X and Y to create a " + "smoother response.", + 0.03, GetParameterManager()); + + + ////////////////////////////////////////////////////////////////////////////// + // CorrelationParameters loopCorrelationParameters; + + m_pLoopSearchSpaceDimension = new Parameter( + "LoopSearchSpaceDimension", + "The size of the search grid used by the matcher.", + 8.0, GetParameterManager()); + + m_pLoopSearchSpaceResolution = new Parameter( + "LoopSearchSpaceResolution", + "The resolution (size of a grid cell) of the correlation grid.", + 0.05, GetParameterManager()); + + m_pLoopSearchSpaceSmearDeviation = new Parameter( + "LoopSearchSpaceSmearDeviation", + "The point readings are smeared by this value in X and Y to create a " + "smoother response.", + 0.03, GetParameterManager()); + + ////////////////////////////////////////////////////////////////////////////// + // ScanMatcherParameters; + + m_pDistanceVariancePenalty = new Parameter( + "DistanceVariancePenalty", + "Variance of penalty for deviating from odometry when scan-matching. " + "The penalty is a multiplier (less than 1.0) is a function of the " + "delta of the scan position being tested and the odometric pose.", + math::Square(0.3), GetParameterManager()); + + m_pAngleVariancePenalty = new Parameter( + "AngleVariancePenalty", + "See DistanceVariancePenalty.", + math::Square(math::DegreesToRadians(20)), GetParameterManager()); + + m_pFineSearchAngleOffset = new Parameter( + "FineSearchAngleOffset", + "The range of angles to search during a fine search.", + math::DegreesToRadians(0.2), GetParameterManager()); + + m_pCoarseSearchAngleOffset = new Parameter( + "CoarseSearchAngleOffset", + "The range of angles to search during a coarse search.", + math::DegreesToRadians(20), GetParameterManager()); + + m_pCoarseAngleResolution = new Parameter( + "CoarseAngleResolution", + "Resolution of angles to search during a coarse search.", + math::DegreesToRadians(2), GetParameterManager()); + + m_pMinimumAnglePenalty = new Parameter( + "MinimumAnglePenalty", + "Minimum value of the angle penalty multiplier so scores do not become " + "too small.", + 0.9, GetParameterManager()); + + m_pMinimumDistancePenalty = new Parameter( + "MinimumDistancePenalty", + "Minimum value of the distance penalty multiplier so scores do not " + "become too small.", + 0.5, GetParameterManager()); + + m_pUseResponseExpansion = new Parameter( + "UseResponseExpansion", + "Whether to increase the search space if no good matches are initially " + "found.", + false, GetParameterManager()); + } + /* Adding in getters and setters here for easy parameter access */ + + // General Parameters + + bool Mapper::getParamUseScanMatching() + { + return static_cast(m_pUseScanMatching->GetValue()); + } + + bool Mapper::getParamUseScanBarycenter() + { + return static_cast(m_pUseScanBarycenter->GetValue()); + } + + double Mapper::getParamMinimumTimeInterval() + { + return static_cast(m_pMinimumTimeInterval->GetValue()); + } + + double Mapper::getParamMinimumTravelDistance() + { + return static_cast(m_pMinimumTravelDistance->GetValue()); + } + + double Mapper::getParamMinimumTravelHeading() + { + return math::RadiansToDegrees(static_cast(m_pMinimumTravelHeading->GetValue())); + } + + int Mapper::getParamScanBufferSize() + { + return static_cast(m_pScanBufferSize->GetValue()); + } + + double Mapper::getParamScanBufferMaximumScanDistance() + { + return static_cast(m_pScanBufferMaximumScanDistance->GetValue()); + } + + double Mapper::getParamLinkMatchMinimumResponseFine() + { + return static_cast(m_pLinkMatchMinimumResponseFine->GetValue()); + } + + double Mapper::getParamLinkScanMaximumDistance() + { + return static_cast(m_pLinkScanMaximumDistance->GetValue()); + } + + double Mapper::getParamLoopSearchMaximumDistance() + { + return static_cast(m_pLoopSearchMaximumDistance->GetValue()); + } + + bool Mapper::getParamDoLoopClosing() + { + return static_cast(m_pDoLoopClosing->GetValue()); + } + + int Mapper::getParamLoopMatchMinimumChainSize() + { + return static_cast(m_pLoopMatchMinimumChainSize->GetValue()); + } + + double Mapper::getParamLoopMatchMaximumVarianceCoarse() + { + return static_cast(std::sqrt(m_pLoopMatchMaximumVarianceCoarse->GetValue())); + } + + double Mapper::getParamLoopMatchMinimumResponseCoarse() + { + return static_cast(m_pLoopMatchMinimumResponseCoarse->GetValue()); + } + + double Mapper::getParamLoopMatchMinimumResponseFine() + { + return static_cast(m_pLoopMatchMinimumResponseFine->GetValue()); + } + + // Correlation Parameters - Correlation Parameters + + double Mapper::getParamCorrelationSearchSpaceDimension() + { + return static_cast(m_pCorrelationSearchSpaceDimension->GetValue()); + } + + double Mapper::getParamCorrelationSearchSpaceResolution() + { + return static_cast(m_pCorrelationSearchSpaceResolution->GetValue()); + } + + double Mapper::getParamCorrelationSearchSpaceSmearDeviation() + { + return static_cast(m_pCorrelationSearchSpaceSmearDeviation->GetValue()); + } + + // Correlation Parameters - Loop Correlation Parameters + + double Mapper::getParamLoopSearchSpaceDimension() + { + return static_cast(m_pLoopSearchSpaceDimension->GetValue()); + } + + double Mapper::getParamLoopSearchSpaceResolution() + { + return static_cast(m_pLoopSearchSpaceResolution->GetValue()); + } + + double Mapper::getParamLoopSearchSpaceSmearDeviation() + { + return static_cast(m_pLoopSearchSpaceSmearDeviation->GetValue()); + } + + // ScanMatcher Parameters + + double Mapper::getParamDistanceVariancePenalty() + { + return std::sqrt(static_cast(m_pDistanceVariancePenalty->GetValue())); + } + + double Mapper::getParamAngleVariancePenalty() + { + return std::sqrt(static_cast(m_pAngleVariancePenalty->GetValue())); + } + + double Mapper::getParamFineSearchAngleOffset() + { + return static_cast(m_pFineSearchAngleOffset->GetValue()); + } + + double Mapper::getParamCoarseSearchAngleOffset() + { + return static_cast(m_pCoarseSearchAngleOffset->GetValue()); + } + + double Mapper::getParamCoarseAngleResolution() + { + return static_cast(m_pCoarseAngleResolution->GetValue()); + } + + double Mapper::getParamMinimumAnglePenalty() + { + return static_cast(m_pMinimumAnglePenalty->GetValue()); + } + + double Mapper::getParamMinimumDistancePenalty() + { + return static_cast(m_pMinimumDistancePenalty->GetValue()); + } + + bool Mapper::getParamUseResponseExpansion() + { + return static_cast(m_pUseResponseExpansion->GetValue()); + } + + /* Setters for parameters */ + // General Parameters + void Mapper::setParamUseScanMatching(bool b) + { + m_pUseScanMatching->SetValue((kt_bool)b); + } + + void Mapper::setParamUseScanBarycenter(bool b) + { + m_pUseScanBarycenter->SetValue((kt_bool)b); + } + + void Mapper::setParamMinimumTimeInterval(double d) + { + m_pMinimumTimeInterval->SetValue((kt_double)d); + } + + void Mapper::setParamMinimumTravelDistance(double d) + { + m_pMinimumTravelDistance->SetValue((kt_double)d); + } + + void Mapper::setParamMinimumTravelHeading(double d) + { + m_pMinimumTravelHeading->SetValue((kt_double)d); + } + + void Mapper::setParamScanBufferSize(int i) + { + m_pScanBufferSize->SetValue((kt_int32u)i); + } + + void Mapper::setParamScanBufferMaximumScanDistance(double d) + { + m_pScanBufferMaximumScanDistance->SetValue((kt_double)d); + } + + void Mapper::setParamLinkMatchMinimumResponseFine(double d) + { + m_pLinkMatchMinimumResponseFine->SetValue((kt_double)d); + } + + void Mapper::setParamLinkScanMaximumDistance(double d) + { + m_pLinkScanMaximumDistance->SetValue((kt_double)d); + } + + void Mapper::setParamLoopSearchMaximumDistance(double d) + { + m_pLoopSearchMaximumDistance->SetValue((kt_double)d); + } + + void Mapper::setParamDoLoopClosing(bool b) + { + m_pDoLoopClosing->SetValue((kt_bool)b); + } + + void Mapper::setParamLoopMatchMinimumChainSize(int i) + { + m_pLoopMatchMinimumChainSize->SetValue((kt_int32u)i); + } + + void Mapper::setParamLoopMatchMaximumVarianceCoarse(double d) + { + m_pLoopMatchMaximumVarianceCoarse->SetValue((kt_double)math::Square(d)); + } + + void Mapper::setParamLoopMatchMinimumResponseCoarse(double d) + { + m_pLoopMatchMinimumResponseCoarse->SetValue((kt_double)d); + } + + void Mapper::setParamLoopMatchMinimumResponseFine(double d) + { + m_pLoopMatchMinimumResponseFine->SetValue((kt_double)d); + } + + // Correlation Parameters - Correlation Parameters + void Mapper::setParamCorrelationSearchSpaceDimension(double d) + { + m_pCorrelationSearchSpaceDimension->SetValue((kt_double)d); + } + + void Mapper::setParamCorrelationSearchSpaceResolution(double d) + { + m_pCorrelationSearchSpaceResolution->SetValue((kt_double)d); + } + + void Mapper::setParamCorrelationSearchSpaceSmearDeviation(double d) + { + m_pCorrelationSearchSpaceSmearDeviation->SetValue((kt_double)d); + } + + + // Correlation Parameters - Loop Closure Parameters + void Mapper::setParamLoopSearchSpaceDimension(double d) + { + m_pLoopSearchSpaceDimension->SetValue((kt_double)d); + } + + void Mapper::setParamLoopSearchSpaceResolution(double d) + { + m_pLoopSearchSpaceResolution->SetValue((kt_double)d); + } + + void Mapper::setParamLoopSearchSpaceSmearDeviation(double d) + { + m_pLoopSearchSpaceSmearDeviation->SetValue((kt_double)d); + } + + + // Scan Matcher Parameters + void Mapper::setParamDistanceVariancePenalty(double d) + { + m_pDistanceVariancePenalty->SetValue((kt_double)math::Square(d)); + } + + void Mapper::setParamAngleVariancePenalty(double d) + { + m_pAngleVariancePenalty->SetValue((kt_double)math::Square(d)); + } + + void Mapper::setParamFineSearchAngleOffset(double d) + { + m_pFineSearchAngleOffset->SetValue((kt_double)d); + } + + void Mapper::setParamCoarseSearchAngleOffset(double d) + { + m_pCoarseSearchAngleOffset->SetValue((kt_double)d); + } + + void Mapper::setParamCoarseAngleResolution(double d) + { + m_pCoarseAngleResolution->SetValue((kt_double)d); + } + + void Mapper::setParamMinimumAnglePenalty(double d) + { + m_pMinimumAnglePenalty->SetValue((kt_double)d); + } + + void Mapper::setParamMinimumDistancePenalty(double d) + { + m_pMinimumDistancePenalty->SetValue((kt_double)d); + } + + void Mapper::setParamUseResponseExpansion(bool b) + { + m_pUseResponseExpansion->SetValue((kt_bool)b); + } + + + + void Mapper::Initialize(kt_double rangeThreshold) + { + if (m_Initialized == false) + { + // create sequential scan and loop matcher + m_pSequentialScanMatcher = ScanMatcher::Create(this, + m_pCorrelationSearchSpaceDimension->GetValue(), + m_pCorrelationSearchSpaceResolution->GetValue(), + m_pCorrelationSearchSpaceSmearDeviation->GetValue(), + rangeThreshold); + assert(m_pSequentialScanMatcher); + + m_pMapperSensorManager = new MapperSensorManager(m_pScanBufferSize->GetValue(), + m_pScanBufferMaximumScanDistance->GetValue()); + + m_pGraph = new MapperGraph(this, rangeThreshold); + + m_Initialized = true; + } + } + + void Mapper::SaveToFile(const std::string& filename) + { + printf("Save To File\n"); + std::ofstream ofs(filename.c_str()); + boost::archive::binary_oarchive oa(ofs, boost::archive::no_codecvt); + oa << BOOST_SERIALIZATION_NVP(*this); + } + + void Mapper::LoadFromFile(const std::string& filename) + { + printf("Load From File\n"); + std::ifstream ifs(filename.c_str()); + boost::archive::binary_iarchive ia(ifs, boost::archive::no_codecvt); + ia >> BOOST_SERIALIZATION_NVP(*this); + } + + void Mapper::Reset() + { + if (m_pSequentialScanMatcher) + { + delete m_pSequentialScanMatcher; + m_pSequentialScanMatcher = NULL; + } + if (m_pGraph) + { + delete m_pGraph; + m_pGraph = NULL; + } + if (m_pMapperSensorManager) + { + delete m_pMapperSensorManager; + m_pMapperSensorManager = NULL; + } + m_Initialized = false; + } + + kt_bool Mapper::Process(Object* /*pObject*/) + { + return true; + } + + kt_bool Mapper::Process(LocalizedRangeScan* pScan) + { + if (pScan != NULL) + { + karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder(); + + // validate scan + if (pLaserRangeFinder == NULL || pScan == NULL || pLaserRangeFinder->Validate(pScan) == false) + { + return false; + } + + if (m_Initialized == false) + { + // initialize mapper with range threshold from device + Initialize(pLaserRangeFinder->GetRangeThreshold()); + } + + // get last scan + LocalizedRangeScan* pLastScan = m_pMapperSensorManager->GetLastScan(pScan->GetSensorName()); + + // update scans corrected pose based on last correction + if (pLastScan != NULL) + { + Transform lastTransform(pLastScan->GetOdometricPose(), pLastScan->GetCorrectedPose()); + pScan->SetCorrectedPose(lastTransform.TransformPose(pScan->GetOdometricPose())); + } + + // test if scan is outside minimum boundary or if heading is larger then minimum heading + if (!HasMovedEnough(pScan, pLastScan)) + { + return false; + } + + Matrix3 covariance; + covariance.SetToIdentity(); + + // correct scan (if not first scan) + if (m_pUseScanMatching->GetValue() && pLastScan != NULL) + { + Pose2 bestPose; + m_pSequentialScanMatcher->MatchScan(pScan, + m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), + bestPose, + covariance); + pScan->SetSensorPose(bestPose); + } + + // add scan to buffer and assign id + m_pMapperSensorManager->AddScan(pScan); + + if (m_pUseScanMatching->GetValue()) + { + // add to graph + m_pGraph->AddVertex(pScan); + m_pGraph->AddEdges(pScan, covariance); + + m_pMapperSensorManager->AddRunningScan(pScan); + + if (m_pDoLoopClosing->GetValue()) + { + std::vector deviceNames = m_pMapperSensorManager->GetSensorNames(); + const_forEach(std::vector, &deviceNames) + { + m_pGraph->TryCloseLoop(pScan, *iter); + } + } + } + + m_pMapperSensorManager->SetLastScan(pScan); + + return true; + } + + return false; + } + + /** + * Is the scan sufficiently far from the last scan? + * @param pScan + * @param pLastScan + * @return true if the scans are sufficiently far + */ + kt_bool Mapper::HasMovedEnough(LocalizedRangeScan* pScan, LocalizedRangeScan* pLastScan) const + { + // test if first scan + if (pLastScan == NULL) + { + return true; + } + + // test if enough time has passed + kt_double timeInterval = pScan->GetTime() - pLastScan->GetTime(); + if (timeInterval >= m_pMinimumTimeInterval->GetValue()) + { + return true; + } + + Pose2 lastScannerPose = pLastScan->GetSensorAt(pLastScan->GetOdometricPose()); + Pose2 scannerPose = pScan->GetSensorAt(pScan->GetOdometricPose()); + + // test if we have turned enough + kt_double deltaHeading = math::NormalizeAngle(scannerPose.GetHeading() - lastScannerPose.GetHeading()); + if (fabs(deltaHeading) >= m_pMinimumTravelHeading->GetValue()) + { + return true; + } + + // test if we have moved enough + kt_double squaredTravelDistance = lastScannerPose.GetPosition().SquaredDistance(scannerPose.GetPosition()); + if (squaredTravelDistance >= math::Square(m_pMinimumTravelDistance->GetValue()) - KT_TOLERANCE) + { + return true; + } + + return false; + } + + /** + * Gets all the processed scans + * @return all scans + */ + const LocalizedRangeScanVector Mapper::GetAllProcessedScans() const + { + LocalizedRangeScanVector allScans; + + if (m_pMapperSensorManager != NULL) + { + allScans = m_pMapperSensorManager->GetAllScans(); + } + + return allScans; + } + + /** + * Adds a listener + * @param pListener + */ + void Mapper::AddListener(MapperListener* pListener) + { + m_Listeners.push_back(pListener); + } + + /** + * Removes a listener + * @param pListener + */ + void Mapper::RemoveListener(MapperListener* pListener) + { + std::vector::iterator iter = std::find(m_Listeners.begin(), m_Listeners.end(), pListener); + if (iter != m_Listeners.end()) + { + m_Listeners.erase(iter); + } + } + + void Mapper::FireInfo(const std::string& rInfo) const + { + const_forEach(std::vector, &m_Listeners) + { + (*iter)->Info(rInfo); + } + } + + void Mapper::FireDebug(const std::string& rInfo) const + { + const_forEach(std::vector, &m_Listeners) + { + MapperDebugListener* pListener = dynamic_cast(*iter); + + if (pListener != NULL) + { + pListener->Debug(rInfo); + } + } + } + + void Mapper::FireLoopClosureCheck(const std::string& rInfo) const + { + const_forEach(std::vector, &m_Listeners) + { + MapperLoopClosureListener* pListener = dynamic_cast(*iter); + + if (pListener != NULL) + { + pListener->LoopClosureCheck(rInfo); + } + } + } + + void Mapper::FireBeginLoopClosure(const std::string& rInfo) const + { + const_forEach(std::vector, &m_Listeners) + { + MapperLoopClosureListener* pListener = dynamic_cast(*iter); + + if (pListener != NULL) + { + pListener->BeginLoopClosure(rInfo); + } + } + } + + void Mapper::FireEndLoopClosure(const std::string& rInfo) const + { + const_forEach(std::vector, &m_Listeners) + { + MapperLoopClosureListener* pListener = dynamic_cast(*iter); + + if (pListener != NULL) + { + pListener->EndLoopClosure(rInfo); + } + } + } + + void Mapper::SetScanSolver(ScanSolver* pScanOptimizer) + { + m_pScanOptimizer = pScanOptimizer; + } + + MapperGraph* Mapper::GetGraph() const + { + return m_pGraph; + } + + ScanMatcher* Mapper::GetSequentialScanMatcher() const + { + return m_pSequentialScanMatcher; + } + + ScanMatcher* Mapper::GetLoopScanMatcher() const + { + return m_pGraph->GetLoopScanMatcher(); + } +} // namespace karto +BOOST_CLASS_EXPORT(karto::BreadthFirstTraversal) diff --git a/snap/generate_snap b/snap/generate_snap index fc65d4feb..1a1528f89 100755 --- a/snap/generate_snap +++ b/snap/generate_snap @@ -7,7 +7,6 @@ cd src # add all the necessary things to it git clone -b kinetic-devel https://github.com/SteveMacenski/slam_toolbox.git -git clone -b kinetic-devel https://github.com/SteveMacenski/KartoSDK-slam_toolbox.git git clone -b kinetic-devel https://github.com/ros-visualization/rviz.git cd ../ diff --git a/snap/snapcraft.yaml b/snap/snapcraft.yaml index ad07c59de..8221be396 100644 --- a/snap/snapcraft.yaml +++ b/snap/snapcraft.yaml @@ -45,6 +45,6 @@ parts: include-roscore: true rosdistro: kinetic catkin-ros-master-uri: http://localhost:11311 - catkin-packages: [open_karto, slam_toolbox] #rviz + catkin-packages: [slam_toolbox] #rviz recursive-rosinstall: false #after: [desktop-qt5]