├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── Readme.md ├── cmake ├── FindEigen.cmake └── FindMatlab.cmake ├── dependencies.rosinstall ├── numpy_eigen └── README.md ├── python_module ├── CMakeLists.txt ├── cmake │ └── add_python_export_library.cmake └── package.xml ├── schweizer_messer_root ├── sm_boost ├── CMakeLists.txt ├── include │ └── sm │ │ └── boost │ │ ├── JobQueue.hpp │ │ ├── implementation │ │ └── JobQueue.hpp │ │ ├── null_deleter.hpp │ │ ├── ptime.hpp │ │ └── serialization.hpp ├── mainpage.dox ├── package.xml ├── src │ └── JobQueue.cpp └── test │ ├── testFuture.cpp │ └── test_main.cpp ├── sm_common ├── CMakeLists.txt ├── cmake │ └── export_flags.cmake ├── include │ └── sm │ │ ├── Id.hpp │ │ ├── aligned_allocation.h │ │ ├── assert_macros.hpp │ │ ├── hash_id.hpp │ │ ├── is_binary_equal.hpp │ │ ├── maths.hpp │ │ ├── numerical_comparisons.hpp │ │ ├── progress_info.hpp │ │ ├── round.hpp │ │ ├── serialization_macros.hpp │ │ ├── source_file_pos.hpp │ │ ├── string_routines.hpp │ │ └── typetraits.hpp ├── mainpage.dox ├── package.xml ├── src │ ├── hash_id.cpp │ └── progress_info.cpp └── test │ ├── assert_macros.cpp │ ├── assert_macros_always_assert.cpp │ ├── assert_macros_dbg.cpp │ ├── hash_id.cpp │ ├── maths.cpp │ ├── numerical_comparisons.cpp │ ├── serialization_macros.cpp │ └── test_main.cpp ├── sm_deprecation ├── CMakeLists.txt ├── include │ └── sm │ │ └── deprecation.h ├── package.xml └── test │ ├── CMakeLists.txt │ └── test_deprecation.cpp ├── sm_doc ├── CMakeLists.txt ├── doc │ └── doxygen.config.in ├── mainpage.dox └── package.xml ├── sm_eigen ├── CMakeLists.txt ├── include │ └── sm │ │ └── eigen │ │ ├── NumericalDiff.hpp │ │ ├── assert_macros.hpp │ │ ├── gtest.hpp │ │ ├── matrix_sqrt.hpp │ │ ├── property_tree.hpp │ │ ├── random.hpp │ │ ├── serialization.hpp │ │ ├── static_assert.hpp │ │ └── traits.hpp ├── mainpage.dox ├── package.xml ├── src │ └── random.cpp └── test │ ├── EigenSerializationTests.cpp │ ├── MatrixSqrtTest.cpp │ └── test_main.cpp ├── sm_kinematics ├── CMakeLists.txt ├── include │ └── sm │ │ └── kinematics │ │ ├── EulerAnglesYawPitchRoll.hpp │ │ ├── EulerAnglesZXY.h │ │ ├── EulerAnglesZYX.hpp │ │ ├── EulerRodriguez.hpp │ │ ├── HomogeneousPoint.hpp │ │ ├── RotationVector.hpp │ │ ├── RotationalKinematics.hpp │ │ ├── Transformation.hpp │ │ ├── UncertainHomogeneousPoint.hpp │ │ ├── UncertainTransformation.hpp │ │ ├── UncertainVector.hpp │ │ ├── homogeneous_coordinates.hpp │ │ ├── implementation │ │ └── UncertainVector.hpp │ │ ├── property_tree.hpp │ │ ├── quaternion_algebra.hpp │ │ ├── rotations.hpp │ │ ├── three_point_methods.hpp │ │ └── transformations.hpp ├── mainpage.dox ├── package.xml ├── src │ ├── EulerAnglesYawPitchRoll.cpp │ ├── EulerAnglesZXY.cpp │ ├── EulerAnglesZYX.cpp │ ├── EulerRodriguez.cpp │ ├── HomogeneousPoint.cpp │ ├── RotationVector.cpp │ ├── RotationalKinematics.cpp │ ├── Transformation.cpp │ ├── UncertainHomogeneousPoint.cpp │ ├── UncertainTransformation.cpp │ ├── homogeneous_coordinates.cpp │ ├── quaternion_algebra.cpp │ ├── rotations.cpp │ ├── three_point_methods.cpp │ └── transformations.cpp └── test │ ├── HomogeneousPoint.cpp │ ├── PointTestHarness.hpp │ ├── QuaternionTests.cpp │ ├── RotationalKinematicsTests.cpp │ ├── TransformationTests.cpp │ ├── UncertainHomogeneousPoint.cpp │ ├── UncertainTransformationTests.cpp │ ├── homogeneous_coordinates.cpp │ ├── test_main.cpp │ ├── three_point_methods.cpp │ └── transformations.cpp ├── sm_logging ├── CMakeLists.txt ├── cmake │ └── sm_logging-extras.cmake.in ├── include │ └── sm │ │ ├── logging.hpp │ │ └── logging │ │ ├── Formatter.hpp │ │ ├── Levels.hpp │ │ ├── LogLocation.hpp │ │ ├── Logger.hpp │ │ ├── LoggingEvent.hpp │ │ ├── LoggingGlobals.hpp │ │ ├── StdOutLogger.hpp │ │ ├── Tokens.hpp │ │ ├── assert.h │ │ ├── console.hpp │ │ ├── macros.h │ │ └── macros_generated.hpp ├── mainpage.dox ├── package.xml ├── scripts │ ├── generate_macros.py │ └── generate_speed_test.py ├── src │ ├── Formatter.cpp │ ├── Levels.cpp │ ├── Logger.cpp │ ├── LoggingEvent.cpp │ ├── LoggingGlobals.cpp │ ├── StdOutLogger.cpp │ ├── Tokens.cpp │ └── gettimeofday.hpp └── test │ ├── logTest.cpp │ └── test_main.cpp ├── sm_matlab ├── CMakeLists.txt ├── Readme.txt ├── demo │ └── demo.cpp ├── include │ └── sm │ │ └── matlab │ │ └── Engine.hpp ├── mainpage.dox ├── package.xml ├── src │ └── Engine.cpp └── test │ ├── MatlabInterfaceTests.cpp │ └── test_main.cpp ├── sm_matrix_archive ├── .gitignore ├── CMakeLists.txt ├── include │ └── sm │ │ └── MatrixArchive.hpp ├── mainpage.dox ├── matlab │ ├── loadMatrixArchive.m │ └── saveMatrixArchive.m ├── package.xml ├── src │ ├── MatrixArchive.cpp │ └── lsma.cpp └── test │ ├── TestMatrixArchive.cpp │ └── test_main.cpp ├── sm_opencv ├── CMakeLists.txt ├── include │ └── sm │ │ └── opencv │ │ └── serialization.hpp ├── mainpage.dox └── package.xml ├── sm_property_tree ├── CMakeLists.txt ├── include │ └── sm │ │ ├── BoostPropertyTree.hpp │ │ ├── BoostPropertyTreeImplementation.hpp │ │ ├── BoostPropertyTreeLoader.hpp │ │ ├── BoostPropertyTreeSupport.hpp │ │ ├── PropertyTree.hpp │ │ └── PropertyTreeImplementation.hpp ├── mainpage.dox ├── package.xml ├── src │ ├── BoostPropertyTree.cpp │ ├── BoostPropertyTreeImplementation.cpp │ ├── BoostPropertyTreeLoader.cpp │ ├── BoostPropertyTreeSupport.cpp │ ├── PropertyTree.cpp │ └── PropertyTreeImplementation.cpp └── test │ ├── BoostPropertyTreeImplementation.cpp │ └── test_main.cpp ├── sm_python ├── CMakeLists.txt ├── include │ └── sm │ │ └── python │ │ ├── Id.hpp │ │ ├── boost_serialization_pickle.hpp │ │ └── stl_converters.hpp ├── mainpage.dox ├── package.xml ├── python │ └── sm │ │ ├── PlotCollection.py │ │ ├── Progress.py │ │ ├── __init__.py │ │ ├── experiments │ │ └── __init__.py │ │ ├── plotCoordinateFrame.py │ │ └── saveFigTight.py ├── setup.py ├── src │ ├── Logging.cpp │ ├── Timing.cpp │ ├── exportEigen.cpp │ ├── exportHomogeneousPoint.cpp │ ├── exportMatrixArchive.cpp │ ├── exportNsecTime.cpp │ ├── exportPropertyTree.cpp │ ├── exportTimestampCorrector.cpp │ ├── exportTransformation.cpp │ ├── exportUncertainVector.cpp │ ├── exportValueStoreRef.cpp │ ├── export_eigen_property_tree.cpp │ ├── export_homogeneous.cpp │ ├── export_kinematics_property_tree.cpp │ ├── export_quaternion_algebra.cpp │ ├── export_rotational_kinematics.cpp │ ├── export_rotations.cpp │ ├── export_transformations.cpp │ ├── module.cpp │ └── random.cpp └── test │ └── Test.py ├── sm_random ├── CMakeLists.txt ├── include │ └── sm │ │ └── random.hpp ├── mainpage.dox ├── package.xml └── src │ └── random.cpp ├── sm_timing ├── CMakeLists.txt ├── include │ └── sm │ │ └── timing │ │ ├── NsecTimeUtilities.hpp │ │ ├── Timer.hpp │ │ ├── TimestampCorrector.hpp │ │ └── implementation │ │ └── TimestampCorrector.hpp ├── mainpage.dox ├── package.xml ├── src │ ├── NsecTimeUtilities.cpp │ └── Timer.cpp └── test │ ├── TestNsecTimeUtilities.cpp │ ├── TestTimestampCorrector.cpp │ └── test_main.cpp ├── sm_value_store ├── CMakeLists.txt ├── include │ └── sm │ │ └── value_store │ │ ├── LayeredValueStore.hpp │ │ ├── PrefixedValueStore.hpp │ │ ├── PropertyTreeValueStore.hpp │ │ ├── ValueStore.hpp │ │ └── VerboseValueStore.hpp ├── package.xml ├── src │ ├── PrefixedValueStore.cpp │ ├── PropertyTreeValueStore.cpp │ ├── ValueStore.cpp │ └── VerboseValueStore.cpp └── test │ ├── ValueStore.cpp │ └── test_main.cpp └── tutorial.txt /.gitignore: -------------------------------------------------------------------------------- 1 | sm_eigen/lib 2 | *~ 3 | *.so 4 | *.pyc 5 | SFM 6 | build 7 | sm_boost/bin/ 8 | sm_boost/lib/ 9 | sm_matlab/Makefile 10 | sm_matrix_archive/build/ 11 | sm_matrix_archive/lib/ 12 | sm_console/build/ 13 | sm_console/lib/ 14 | test.ba 15 | test.xml 16 | sm_doc/doc/html 17 | sm_doc/doc/doxygen.config 18 | bblos-driver/ 19 | numpy_eigen/build/ 20 | numpy_eigen/lib/ 21 | sm_eigen/bin/ 22 | sm_eigen/build/ 23 | sm_eigen/test.ba 24 | sm_common/build 25 | sm_common/bin 26 | sm_common/lib 27 | sm_kinematics/build 28 | sm_kinematics/bin 29 | sm_kinematics/lib 30 | sm_database/bin/ 31 | sm_database/build/ 32 | sm_database/test.db 33 | sm_omnicam/ 34 | sm_property_tree/bin/sm_property_tree_test 35 | sm_property_tree/build/ 36 | sm_matlab/bin/ 37 | sm_matlab/build/ 38 | sm_matlab/lib/ 39 | sm_pose_graph/bin/ 40 | sm_pose_graph/lib/ 41 | sm_pose_graph/test.graph 42 | sm_property_tree/lib/ 43 | sm_property_tree/src/#BoostPropertyTreeImplementation.cpp# 44 | sm_property_tree/test.xml 45 | sm_python/build/ 46 | sm_python/lib/ 47 | sm_random/build/ 48 | sm_random/lib/ 49 | sm_timing/bin/ 50 | sm_timing/build/ 51 | sm_timing/doc/ 52 | sm_timing/lib/ 53 | sm_boost/build/ 54 | sm_opencv/build/ 55 | numpy_eigen2/ 56 | sm_doc/build 57 | .project 58 | .cproject 59 | .pydevproject 60 | */.settings/language.settings.xml 61 | */cmake_install.cmake 62 | *.orig 63 | sm_pose_graph/build/ 64 | sm_pose_graph/lib/ 65 | sm_database/lib 66 | 67 | sm_logging/bin/sm_logging_test 68 | sm_logging/lib/ 69 | 70 | *.dylib 71 | opencv2 72 | *~ 73 | #* 74 | bin 75 | build 76 | lib 77 | *.o 78 | *.so 79 | *.pyc 80 | *.vscode 81 | log_test 82 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | project(schweizer_messer) 3 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2011-2019, Autonomous Systems Lab, ETH Zurich 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | 10 | 2. Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | 14 | 3. Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 15 | names of its contributors may be used to endorse or promote products 16 | derived from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /Readme.md: -------------------------------------------------------------------------------- 1 | # Schweizer Messer 2 | 3 | Schweizer Messer is a grab bag of tools for programming mainly focussed on work 4 | in robotics research requiring rapid prototyping. 5 | 6 | That's it. 7 | 8 | [![Build Status](https://jenkins.asl.ethz.ch/buildStatus/icon?job=schweizer_messer)](https://jenkins.asl.ethz.ch/job/schweizer_messer/) 9 | 10 | ## Recent BREAKING changes 11 | 12 | * `numpy_eigen` moved into its [own package](https://github.com/ethz-asl/numpy_eigen/). The last revision of this repository still containing it is tagged as `with_numpy_eigen`. 13 | 14 | ## Dependencies 15 | 16 | ... can be found here [dependencies.rosinstall](dependencies.rosinstall) 17 | 18 | ## If you have trouble compiling 19 | 20 | ... make sure to: 21 | 22 | * Use gcc 4.7 ([How to install on Ubuntu 12.04](http://charette.no-ip.com:81/programming/2011-12-24_GCCv47/)) 23 | * Use a boost version that is compatible with gcc 4.7 (>= 1.51) 24 | 25 | On Ubuntu 12.04, if your ubuntu-installed ROS depends on the ubuntu-installed 26 | boost you can simply download a later boost and install it from source - it 27 | will install to "/usr/local" per default (verify this though), where the 28 | Schweizer Messer build will look for it. 29 | 30 | -------------------------------------------------------------------------------- /cmake/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN_FOUND - system has eigen lib with correct version 10 | # EIGEN_INCLUDE_DIR - the eigen include directory 11 | # EIGEN_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen_FIND_VERSION) 19 | if(NOT Eigen_FIND_VERSION_MAJOR) 20 | set(Eigen_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen_FIND_VERSION_MAJOR) 22 | if(NOT Eigen_FIND_VERSION_MINOR) 23 | set(Eigen_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen_FIND_VERSION_MINOR) 25 | if(NOT Eigen_FIND_VERSION_PATCH) 26 | set(Eigen_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen_FIND_VERSION_PATCH) 28 | 29 | set(Eigen_FIND_VERSION "${Eigen_FIND_VERSION_MAJOR}.${Eigen_FIND_VERSION_MINOR}.${Eigen_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}) 43 | if(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 44 | set(EIGEN_VERSION_OK FALSE) 45 | else(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 46 | set(EIGEN_VERSION_OK TRUE) 47 | endif(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 48 | 49 | if(NOT EIGEN_VERSION_OK) 50 | 51 | message(STATUS "Eigen version ${EIGEN_VERSION} found in ${EIGEN_INCLUDE_DIR}, " 52 | "but at least version ${Eigen_FIND_VERSION} is required") 53 | endif(NOT EIGEN_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN_INCLUDE_DIRS) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN_FOUND ${EIGEN_VERSION_OK}) 61 | 62 | else () 63 | 64 | find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR EIGEN_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN_INCLUDE_DIR) 79 | SET(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR} CACHE PATH "The Eigen include path.") 80 | 81 | endif() 82 | 83 | 84 | -------------------------------------------------------------------------------- /dependencies.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: catkin_simple 3 | uri: https://github.com/catkin/catkin_simple.git 4 | - git: 5 | local-name: eigen_catkin 6 | uri: https://github.com/ethz-asl/eigen_catkin.git 7 | - git: 8 | local-name: catkin_boost_python_buildtool 9 | uri: https://github.com/ethz-asl/catkin_boost_python_buildtool.git 10 | - git: 11 | local-name: numpy_eigen 12 | uri: https://github.com/ethz-asl/numpy_eigen.git 13 | -------------------------------------------------------------------------------- /numpy_eigen/README.md: -------------------------------------------------------------------------------- 1 | ## Where did numpy_eigen go? 2 | 3 | It moved [here](https://github.com/ethz-asl/numpy_eigen/) 4 | -------------------------------------------------------------------------------- /python_module/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(python_module) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package( 7 | CFG_EXTRAS add_python_export_library.cmake 8 | ) -------------------------------------------------------------------------------- /python_module/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | python_module 4 | 0.0.1 5 | python_module 6 | 7 | Paul Furgale 8 | New BSD 9 | catkin 10 | 11 | -------------------------------------------------------------------------------- /schweizer_messer_root: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/schweizer_messer/cb9d52841cebd965bb201ba83031e830980d8016/schweizer_messer_root -------------------------------------------------------------------------------- /sm_boost/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sm_boost) 3 | 4 | 5 | find_package(catkin_simple REQUIRED) 6 | catkin_simple() 7 | 8 | find_package(Boost REQUIRED COMPONENTS system serialization thread) 9 | include_directories(include ${Boost_INCLUDE_DIRS}) 10 | 11 | ############## 12 | ## Building ## 13 | ############## 14 | 15 | cs_add_library(${PROJECT_NAME} 16 | src/JobQueue.cpp 17 | ) 18 | 19 | target_link_libraries(${PROJECT_NAME} 20 | ${Boost_LIBRARIES}) 21 | 22 | ############# 23 | ## Testing ## 24 | ############# 25 | # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 26 | add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) 27 | 28 | ## Add gtest based cpp test target and link libraries 29 | catkin_add_gtest(${PROJECT_NAME}-test 30 | test/test_main.cpp 31 | test/testFuture.cpp) 32 | if(TARGET ${PROJECT_NAME}-test) 33 | target_link_libraries(${PROJECT_NAME}-test 34 | ${PROJECT_NAME} 35 | pthread 36 | ) 37 | endif() 38 | 39 | 40 | cs_install() 41 | cs_export() 42 | -------------------------------------------------------------------------------- /sm_boost/include/sm/boost/JobQueue.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_JOB_QUEUE_HPP 2 | #define SM_JOB_QUEUE_HPP 3 | 4 | // Copyright (c) 2007 Braddock Gaskill Distributed under the Boost 5 | // Software License, Version 1.0. (See accompanying file 6 | // LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt) 7 | // exception_ptr.hpp/cpp copyright Peter Dimov 8 | // Adapted by Paul Furgale (2010/2011/2012) 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | namespace sm { 17 | 18 | /** 19 | * \class JobQueue 20 | * 21 | * A thread pool to execute work in a multithreaded way. The queue 22 | * isn't highly optimized so the units of work should be big. 23 | * 24 | */ 25 | class JobQueue { 26 | public: 27 | 28 | JobQueue(); 29 | 30 | ~JobQueue(); 31 | 32 | /// \brief start the queue processing with n threads. 33 | void start(int nThreads); 34 | 35 | /// \brief stop the queue processing (but don't block and wait) 36 | /// Currently processing items will still finish but 37 | /// items not started will remin unprocessed 38 | void stop(); 39 | 40 | /// \brief stop the queue processing and block until currently processing items are complete 41 | /// Currently processing items will still finish but 42 | /// items not started will remin unprocessed 43 | void join(); 44 | 45 | /// \brief is the queue empty? 46 | bool empty(); 47 | 48 | /// \brief block untill all the items in the queue have been processed. 49 | /// Be careful! If the queue has not been started with some threads, this will never exit. 50 | void waitForEmptyQueue(); 51 | 52 | 53 | 54 | /// \brief schedule a future for processing. 55 | template 56 | void scheduleFuture(boost::function const& fn, boost::unique_future & outFuture); 57 | 58 | /// \brief submit a unit of work to be processed. 59 | void scheduleWork(boost::function const & fn); 60 | protected: 61 | void exec_loop(); 62 | 63 | volatile bool killWorkers_; 64 | bool started_; 65 | int threadsInPool_; 66 | int activeThreads_; 67 | std::deque > q_; 68 | boost::mutex mutex_; 69 | boost::condition condition_; // signal we wait for when queue is empty 70 | boost::condition workCondition_; // signal we wait for when waiting for the queue to complete 71 | boost::shared_ptr work_; 72 | }; 73 | 74 | } // end namespace sm 75 | 76 | #include "implementation/JobQueue.hpp" 77 | 78 | #endif /* SM_JOB_QUEUE_HPP */ 79 | -------------------------------------------------------------------------------- /sm_boost/include/sm/boost/implementation/JobQueue.hpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | namespace sm { 4 | 5 | namespace detail { 6 | template 7 | struct taskWrapper 8 | { 9 | boost::shared_ptr > _task; 10 | 11 | taskWrapper( boost::function const & fn ) : 12 | _task( new boost::packaged_task(fn) ) {} 13 | 14 | ~taskWrapper() { 15 | } 16 | 17 | void operator()(){ (*_task)(); } 18 | }; 19 | 20 | 21 | } // namespace detail 22 | 23 | template 24 | void JobQueue::scheduleFuture(boost::function const& fn, boost::unique_future & outFuture) 25 | { 26 | 27 | detail::taskWrapper task(fn); 28 | outFuture = task._task->get_future(); 29 | scheduleWork(task); 30 | } 31 | 32 | 33 | } // namespace sm 34 | -------------------------------------------------------------------------------- /sm_boost/include/sm/boost/null_deleter.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_BOOST_NULL_DELETER_HPP 2 | #define SM_BOOST_NULL_DELETER_HPP 3 | 4 | namespace sm { 5 | 6 | struct null_deleter 7 | { 8 | template 9 | void operator()(const T * ){} 10 | }; 11 | 12 | } // namespace sm 13 | 14 | 15 | #endif /* SM_BOOST_NULL_DELETER_HPP */ 16 | -------------------------------------------------------------------------------- /sm_boost/include/sm/boost/ptime.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_PTIME_HPP 2 | #define SM_PTIME_HPP 3 | #include 4 | #include 5 | 6 | namespace sm { 7 | 8 | timeval ptimeToTimeval(const boost::posix_time::ptime time) 9 | { 10 | static const boost::posix_time::ptime UNIX_EPOCH(boost::gregorian::date(1970,1,1)); 11 | 12 | // \todo: check for special values. 13 | boost::posix_time::time_duration duration( time - UNIX_EPOCH ); 14 | timeval tv; 15 | tv.tv_sec = duration.total_seconds(); 16 | tv.tv_usec = duration.fractional_seconds(); 17 | 18 | 19 | 20 | } 21 | 22 | 23 | 24 | } // namespace sm 25 | 26 | 27 | #endif /* SM_PTIME_HPP */ 28 | -------------------------------------------------------------------------------- /sm_boost/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b sm_boost is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /sm_boost/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sm_boost 4 | 0.0.1 5 | sm_boost 6 | 7 | Paul Furgale 8 | New BSD 9 | catkin 10 | catkin_simple 11 | 12 | sm_common 13 | sm_common 14 | gtest 15 | 16 | -------------------------------------------------------------------------------- /sm_boost/src/JobQueue.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace sm { 4 | 5 | JobQueue::JobQueue() : killWorkers_(false), started_(false), threadsInPool_(0), activeThreads_(0) {} 6 | JobQueue::~JobQueue() { // we must kill thread before we're dead 7 | boost::mutex::scoped_lock lck(mutex_); 8 | killWorkers_ = true; // flag to tell thread to die 9 | condition_.notify_all(); 10 | while (threadsInPool_) // all threads must exit 11 | condition_.wait(lck); 12 | } 13 | 14 | void JobQueue::scheduleWork(boost::function const & fn) 15 | { 16 | boost::mutex::scoped_lock lck(mutex_); 17 | q_.push_back(fn); //queue the job 18 | condition_.notify_all(); // wake worker thread(s) 19 | } 20 | 21 | void JobQueue::start(int nThreads) { 22 | if(!work_) 23 | { 24 | work_.reset(new boost::thread_group); 25 | for(int i = 0; i < nThreads; ++i) 26 | { 27 | work_->create_thread(boost::bind(&JobQueue::exec_loop,this)); 28 | } 29 | } 30 | } 31 | 32 | void JobQueue::stop() { 33 | killWorkers_ = true; 34 | } 35 | 36 | void JobQueue::join() 37 | { 38 | stop(); 39 | condition_.notify_all(); 40 | work_->join_all(); 41 | } 42 | 43 | bool JobQueue::empty() 44 | { 45 | boost::mutex::scoped_lock lck(mutex_); 46 | return q_.empty(); 47 | } 48 | 49 | void JobQueue::waitForEmptyQueue() 50 | { 51 | boost::mutex::scoped_lock lck(mutex_); 52 | while( activeThreads_ || !q_.empty() ) 53 | { 54 | workCondition_.wait(lck); 55 | } 56 | } 57 | 58 | void JobQueue::exec_loop() { 59 | boost::mutex::scoped_lock lck(mutex_); 60 | ++threadsInPool_; 61 | while (true) { 62 | while ((q_.empty()) && !killWorkers_) 63 | condition_.wait(lck); // wait for a job to be added to queue 64 | if (killWorkers_) { 65 | --threadsInPool_; 66 | condition_.notify_all(); 67 | return; 68 | } 69 | boost::function f = q_.front(); 70 | q_.pop_front(); 71 | ++activeThreads_; 72 | // unlock the queue while we exec the job 73 | lck.unlock(); 74 | // call the work function 75 | f(); 76 | lck.lock(); 77 | --activeThreads_; 78 | workCondition_.notify_all(); 79 | } 80 | } 81 | 82 | 83 | } // namespace sm 84 | -------------------------------------------------------------------------------- /sm_boost/test/testFuture.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | void setTrue(bool * val) 6 | { 7 | boost::this_thread::sleep(boost::posix_time::seconds(1)); 8 | *val = true; 9 | } 10 | 11 | int work(int n) 12 | { 13 | boost::this_thread::sleep(boost::posix_time::seconds(1)); 14 | int sum = 0; 15 | for(int i = 0; i < n; ++i) 16 | { 17 | sum += i; 18 | } 19 | return sum; 20 | } 21 | 22 | void testQueue(int nThreads) 23 | { 24 | 25 | sm::JobQueue queue; 26 | // Start the queue with n threads. 27 | queue.start(nThreads); 28 | 29 | 30 | boost::unique_future job1; 31 | queue.scheduleFuture(boost::bind(&work,2000), job1); 32 | 33 | boost::unique_future job2; 34 | queue.scheduleFuture(boost::bind(&work,2000), job2); 35 | 36 | boost::unique_future job3; 37 | queue.scheduleFuture(boost::bind(&work,2000), job3); 38 | 39 | boost::unique_future job4; 40 | queue.scheduleFuture(boost::bind(&work,2000), job4); 41 | 42 | bool complete = false; 43 | queue.scheduleWork( boost::bind(&setTrue, &complete) ); 44 | ASSERT_FALSE(complete); 45 | 46 | std::cout << "Job 1: " << job1.get() << std::endl; 47 | 48 | queue.waitForEmptyQueue(); 49 | ASSERT_TRUE(complete); 50 | ASSERT_TRUE(queue.empty()); 51 | 52 | // boost::unique_future job2 = queue.scheduleBoost(boost::bind(&work,200000)); 53 | // boost::unique_future job3 = queue.scheduleBoost(boost::bind(&work,200000)); 54 | // boost::unique_future job4 = queue.scheduleBoost(boost::bind(&work,200000)); 55 | // boost::unique_future job5 = queue.scheduleBoost(boost::bind(&work,200000)); 56 | // boost::unique_future job6 = queue.scheduleBoost(boost::bind(&work,200000)); 57 | // boost::unique_future job7 = queue.scheduleBoost(boost::bind(&work,200000)); 58 | // boost::unique_future job8 = queue.scheduleBoost(boost::bind(&work,200000)); 59 | // boost::unique_future job9 = queue.scheduleBoost(boost::bind(&work,200000)); 60 | 61 | std::cout << "Job 2: " << job2.get() << std::endl; 62 | std::cout << "Job 3: " << job3.get() << std::endl; 63 | std::cout << "Job 4: " << job4.get() << std::endl; 64 | //std::cout << "Job 2: " << job2.get() << std::endl; 65 | //std::cout << "Job 9: " << job9.get() << std::endl; 66 | 67 | queue.join(); 68 | } 69 | 70 | 71 | TEST(FuturesTestSuite, testJobQueue1) 72 | { 73 | testQueue(1); 74 | } 75 | 76 | TEST(FuturesTestSuite, testJobQueue2) 77 | { 78 | testQueue(2); 79 | } 80 | 81 | TEST(FuturesTestSuite, testJobQueue8) 82 | { 83 | testQueue(8); 84 | } 85 | 86 | -------------------------------------------------------------------------------- /sm_boost/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /// Run all the tests that were declared with TEST() 4 | int main(int argc, char **argv){ 5 | testing::InitGoogleTest(&argc, argv); 6 | return RUN_ALL_TESTS(); 7 | } 8 | -------------------------------------------------------------------------------- /sm_common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sm_common) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | find_package(Boost REQUIRED COMPONENTS system) 8 | include_directories(${Boost_INCLUDE_DIRS}) 9 | 10 | # enable warnings 11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") 12 | 13 | # enable C++11 support 14 | if(CMAKE_VERSION VERSION_LESS "3.1") 15 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 16 | else() 17 | set(CMAKE_CXX_STANDARD 11) 18 | endif() 19 | 20 | cs_add_library(${PROJECT_NAME} 21 | src/hash_id.cpp 22 | src/progress_info.cpp 23 | ) 24 | target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) 25 | 26 | ############# 27 | ## Testing ## 28 | ############# 29 | 30 | catkin_add_gtest(${PROJECT_NAME}-test 31 | test/test_main.cpp 32 | test/assert_macros.cpp 33 | test/assert_macros_dbg.cpp 34 | test/assert_macros_always_assert.cpp 35 | test/maths.cpp 36 | test/serialization_macros.cpp 37 | test/numerical_comparisons.cpp 38 | test/hash_id.cpp 39 | ) 40 | target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 41 | 42 | cs_install() 43 | cs_export(CFG_EXTRAS export_flags.cmake) 44 | -------------------------------------------------------------------------------- /sm_common/cmake/export_flags.cmake: -------------------------------------------------------------------------------- 1 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -std=c++0x") 2 | 3 | 4 | -------------------------------------------------------------------------------- /sm_common/include/sm/aligned_allocation.h: -------------------------------------------------------------------------------- 1 | #ifndef SM_ALIGNED_ALLOCATION_H_ 2 | #define SM_ALIGNED_ALLOCATION_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | template class Container, typename Type> 15 | struct Aligned { 16 | typedef Container > type; 17 | }; 18 | 19 | template 20 | struct AlignedUnorderedMap { 21 | typedef std::unordered_map, std::equal_to, 23 | Eigen::aligned_allocator > > type; 24 | }; 25 | 26 | template 27 | struct AlignedMap { 28 | typedef std::map, 30 | Eigen::aligned_allocator > > type; 31 | }; 32 | 33 | template 34 | inline std::shared_ptr aligned_shared(Arguments&&... arguments) { 35 | typedef typename std::remove_const::type TypeNonConst; 36 | return std::allocate_shared(Eigen::aligned_allocator(), 37 | std::forward(arguments)...); 38 | } 39 | #endif // SM_ALIGNED_ALLOCATION_H_ 40 | -------------------------------------------------------------------------------- /sm_common/include/sm/is_binary_equal.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_IS_BINARY_EQUAL_HPP 2 | #define SM_IS_BINARY_EQUAL_HPP 3 | 4 | # warning "This header is deprecated. Please use: the macros in sm/serialization_macros.hpp" 5 | 6 | #include 7 | 8 | namespace sm { 9 | 10 | template 11 | inline bool isBinaryEqual(const T * t1, const T * t2) 12 | { 13 | return SM_CHECKSAME(t1, t2); 14 | } 15 | 16 | template 17 | inline bool isBinaryEqual(const boost::shared_ptr & t1, const boost::shared_ptr & t2) 18 | { 19 | return SM_CHECKSAME(t1, t2); 20 | } 21 | 22 | 23 | } // namespace sm 24 | 25 | 26 | #endif /* SM_IS_BINARY_EQUAL_HPP */ 27 | -------------------------------------------------------------------------------- /sm_common/include/sm/maths.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_MATHS_HPP 2 | #define SM_MATHS_HPP 3 | 4 | #include 5 | 6 | #include // std::pair, if you can believe that. 7 | #include "assert_macros.hpp" 8 | namespace sm { 9 | 10 | /** 11 | * \brief Solve a quadratic equation of the form \f$ a x^2 + b x + c = 0 \f$ 12 | * 13 | * This uses the numerically stable algorithm described here: 14 | * http://en.wikipedia.org/wiki/Loss_of_significance#Instability_of_the_quadratic_equation 15 | * 16 | * The function does not support complex solutions. If \f$ b*b - 4*a*c < 0\f$, the function 17 | * will throw an exception 18 | * 19 | * @return a 2x1 column with the solutions 20 | */ 21 | inline std::pair solveQuadratic(double a, double b, double c) 22 | { 23 | double bb_m_4ac = b*b - 4*c*a; 24 | 25 | SM_ASSERT_GE(std::runtime_error, bb_m_4ac, 0.0, "b^2 - 4ac < 0. Imaginary solutions are not supported"); 26 | 27 | double n; 28 | if(b < 0.f) 29 | { 30 | n = sqrt(bb_m_4ac) - b; 31 | } 32 | else 33 | { 34 | n = -sqrt(bb_m_4ac) - b; 35 | } 36 | 37 | return std::make_pair(n/(2*a),(2*c)/n); 38 | 39 | } 40 | 41 | 42 | /** 43 | * \brief An implementation of the signum function 44 | * 45 | * @param val the variable to check 46 | * 47 | * @return -1 if the variable is negative, +1 if the variable is positive, and 0 if the variable is 0 48 | */ 49 | template int sgn(T val) { 50 | return (T(0) < val) - (val < T(0)); 51 | } 52 | 53 | } // namespace sm 54 | 55 | 56 | #endif /* SM_MATHS_HPP */ 57 | -------------------------------------------------------------------------------- /sm_common/include/sm/progress_info.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PROGRESS_INFO_HPP_ 2 | #define PROGRESS_INFO_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace sm { 10 | 11 | void showProgress(double progress); 12 | 13 | template 14 | void showProgress(T1 done, T2 all) 15 | { 16 | SM_ASSERT_GT(std::runtime_error, all, 0, "#DIV0"); 17 | showProgress(static_cast(done) / static_cast(all)); 18 | } 19 | 20 | } /* namespace sm */ 21 | 22 | #endif /* PROGRESS_INFO_HPP_ */ 23 | -------------------------------------------------------------------------------- /sm_common/include/sm/round.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_ROUND_HPP 2 | #define SM_ROUND_HPP 3 | 4 | namespace sm { 5 | 6 | inline double round (double number) 7 | { 8 | return (number < 0.0 ? ceil (number - 0.5) : floor (number + 0.5)); 9 | } 10 | 11 | inline float round (float number) 12 | { 13 | return (number < 0.0f ? ceilf (number - 0.5f) : floorf (number + 0.5f)); 14 | } 15 | 16 | inline int rint(double number) 17 | { 18 | return static_cast( round(number) ); 19 | } 20 | 21 | inline int rint(float number) 22 | { 23 | return static_cast( round(number) ); 24 | } 25 | 26 | inline long int lrint(double number) 27 | { 28 | return static_cast( round(number) ); 29 | } 30 | 31 | inline long int lrint(float number) 32 | { 33 | return static_cast( round(number) ); 34 | } 35 | 36 | } // namespace sm 37 | 38 | 39 | #endif /* SM_ROUND_HPP */ 40 | -------------------------------------------------------------------------------- /sm_common/include/sm/source_file_pos.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_SOURCE_FILE_POS_HPP 2 | #define SM_SOURCE_FILE_POS_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | // A class and macro that gives you the current file position. 8 | 9 | namespace sm { 10 | 11 | class source_file_pos 12 | { 13 | public: 14 | std::string function; 15 | std::string file; 16 | int line; 17 | 18 | source_file_pos(std::string function, std::string file, int line) : 19 | function(function), file(file), line(line) {} 20 | 21 | operator std::string() 22 | { 23 | return toString(); 24 | } 25 | 26 | std::string toString() const 27 | { 28 | std::stringstream s; 29 | s << file << ":" << line << ": " << function << "()"; 30 | return s.str(); 31 | } 32 | 33 | }; 34 | 35 | }// namespace sm 36 | 37 | inline std::ostream & operator<<(std::ostream & out, const sm::source_file_pos & sfp) 38 | { 39 | out << sfp.file << ":" << sfp.line << ": " << sfp.function << "()"; 40 | return out; 41 | } 42 | 43 | 44 | #define SM_SOURCE_FILE_POS sm::source_file_pos(__FUNCTION__,__FILE__,__LINE__) 45 | 46 | #endif 47 | 48 | -------------------------------------------------------------------------------- /sm_common/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b sm_common is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /sm_common/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sm_common 4 | 0.0.1 5 | sm_common 6 | 7 | Paul Furgale 8 | New BSD 9 | catkin 10 | catkin_simple 11 | 12 | gtest 13 | 14 | -------------------------------------------------------------------------------- /sm_common/src/hash_id.cpp: -------------------------------------------------------------------------------- 1 | #include "sm/hash_id.hpp" 2 | 3 | namespace sm { 4 | 5 | const char HashId::kHexConversion[] = "0123456789abcdef"; 6 | 7 | } // namespace sm 8 | -------------------------------------------------------------------------------- /sm_common/src/progress_info.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * show_progress.cpp 3 | * 4 | * Created on: Jan 29, 2014 5 | * Author: mbuerki 6 | */ 7 | 8 | #include 9 | 10 | #include // std::cout, std::fixed 11 | #include // std::setprecision 12 | 13 | namespace sm { 14 | 15 | void showProgress(double progress) 16 | { 17 | SM_ASSERT_GE_DBG(std::runtime_error, progress, 0.0, "negative progress! shame on you!"); 18 | SM_ASSERT_LE_DBG(std::runtime_error, progress, 1.0, "progress out of bounds!"); 19 | 20 | std::cout << std::fixed << std::setprecision(2) << std::showpoint; 21 | if(progress >= 1.0) 22 | { 23 | std::cout << "\r" << "Complete! " << std::endl; 24 | } else 25 | { 26 | std::cout << "\r" << 100 * progress << "% complete "; 27 | std::cout.flush(); 28 | } 29 | } 30 | 31 | } /* namespace sm */ 32 | -------------------------------------------------------------------------------- /sm_common/test/assert_macros.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | TEST(SmCommonTestSuite,testAssertMacros) 6 | { 7 | 8 | SM_DEFINE_EXCEPTION(Exception, std::runtime_error); 9 | 10 | { 11 | double* val = new double; 12 | EXPECT_NO_THROW( SM_ASSERT_TRUE(Exception, true, "") ); 13 | EXPECT_NO_THROW( SM_ASSERT_FALSE(Exception, false, "") ); 14 | EXPECT_NO_THROW( SM_ASSERT_GE_LT(Exception, 0.0, 0.0, 1.0, "") ); 15 | EXPECT_NO_THROW( SM_ASSERT_GT_LE(Exception, 0.1, 0.0, 1.0, "") ); 16 | EXPECT_NO_THROW( SM_ASSERT_GE_LE(Exception, 0.0, 0.0, 1.0, "") ); 17 | EXPECT_NO_THROW( SM_ASSERT_GE_LE(Exception, 1.0, 0.0, 1.0, "") ); 18 | EXPECT_NO_THROW( SM_ASSERT_LT(Exception, 0.0, 1.0, "") ); 19 | EXPECT_NO_THROW( SM_ASSERT_GT(Exception, 1.0, 0.0, "") ); 20 | EXPECT_NO_THROW( SM_ASSERT_POSITIVE(Exception, 1.0, "") ); 21 | EXPECT_NO_THROW( SM_ASSERT_NONNEGATIVE(Exception, 0.0, "") ); 22 | EXPECT_NO_THROW( SM_ASSERT_NEGATIVE(Exception, -1.0, "") ); 23 | EXPECT_NO_THROW( SM_ASSERT_NONPOSITIVE(Exception, 0.0, "") ); 24 | EXPECT_NO_THROW( SM_ASSERT_ZERO(Exception, 0.0, "") ); 25 | EXPECT_NO_THROW( SM_ASSERT_NOTNULL(Exception, val, "") ); 26 | EXPECT_NO_THROW( SM_ASSERT_LE(Exception, 0.0, 0.0, "") ); 27 | EXPECT_NO_THROW( SM_ASSERT_GE(Exception, 0.0, 0.0, "") ); 28 | EXPECT_NO_THROW( SM_ASSERT_NE(Exception, 0.0, 1.0, "") ); 29 | EXPECT_NO_THROW( SM_ASSERT_EQ(Exception, 0.0, 0.0, "") ); 30 | EXPECT_NO_THROW( SM_ASSERT_NEAR(Exception, 0.0, 1.0, 2.0, "") ); 31 | EXPECT_NO_THROW( SM_ASSERT_FINITE(Exception, 0.0, "") ); 32 | EXPECT_NO_THROW( SM_ASSERT_NOTNAN(Exception, 0.0, "") ); 33 | delete val; 34 | } 35 | 36 | { 37 | double* val = NULL; 38 | EXPECT_THROW( SM_ASSERT_TRUE(Exception, false, ""), Exception); 39 | EXPECT_THROW( SM_ASSERT_FALSE(Exception, true, ""), Exception ); 40 | EXPECT_THROW( SM_ASSERT_GE_LT(Exception, 1.0, 0.0, 1.0, ""), Exception ); 41 | EXPECT_THROW( SM_ASSERT_GT_LE(Exception, 0.0, 0.0, 1.0, ""), Exception ); 42 | EXPECT_THROW( SM_ASSERT_GE_LE(Exception, -0.1, 0.0, 1.0, ""), Exception ); 43 | EXPECT_THROW( SM_ASSERT_GE_LE(Exception, 1.1, 0.0, 1.0, ""), Exception ); 44 | EXPECT_THROW( SM_ASSERT_LT(Exception, 1.0, 1.0, ""), Exception ); 45 | EXPECT_THROW( SM_ASSERT_GT(Exception, 0.0, 0.0, ""), Exception ); 46 | EXPECT_THROW( SM_ASSERT_POSITIVE(Exception, 0.0, ""), Exception ); 47 | EXPECT_THROW( SM_ASSERT_NONNEGATIVE(Exception, -1.0, ""), Exception ); 48 | EXPECT_THROW( SM_ASSERT_NEGATIVE(Exception, 0.0, ""), Exception ); 49 | EXPECT_THROW( SM_ASSERT_NONPOSITIVE(Exception, 1.0, ""), Exception ); 50 | EXPECT_THROW( SM_ASSERT_ZERO(Exception, 1.0, ""), Exception ); 51 | EXPECT_THROW( SM_ASSERT_NOTNULL(Exception, val, ""), Exception ); 52 | EXPECT_THROW( SM_ASSERT_LE(Exception, 1.0, 0.0, ""), Exception ); 53 | EXPECT_THROW( SM_ASSERT_GE(Exception, -1.0, 0.0, ""), Exception ); 54 | EXPECT_THROW( SM_ASSERT_NE(Exception, 0.0, 0.0, ""), Exception ); 55 | EXPECT_THROW( SM_ASSERT_EQ(Exception, 1.0, 0.0, ""), Exception ); 56 | EXPECT_THROW( SM_ASSERT_NEAR(Exception, 0.0, 1.0, 0.5, ""), Exception ); 57 | EXPECT_THROW( SM_ASSERT_FINITE(Exception, std::numeric_limits::infinity(), ""), Exception ); 58 | EXPECT_THROW( SM_ASSERT_NOTNAN(Exception, std::numeric_limits::signaling_NaN(), ""), Exception ); 59 | } 60 | } 61 | -------------------------------------------------------------------------------- /sm_common/test/assert_macros_always_assert.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef NDEBUG 4 | #define NDEBUG 5 | #endif 6 | #ifndef SM_ALWAYS_ASSERT 7 | #define SM_ALWAYS_ASSERT 8 | #endif 9 | #include 10 | 11 | TEST(SmCommonTestSuite,testAssertMacrosAlwaysAssert) 12 | { 13 | SM_DEFINE_EXCEPTION(Exception, std::runtime_error); 14 | EXPECT_THROW( SM_ASSERT_TRUE_DBG(Exception, false, ""), Exception); 15 | } 16 | -------------------------------------------------------------------------------- /sm_common/test/hash_id.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include "sm/hash_id.hpp" 7 | 8 | using namespace sm; 9 | 10 | TEST(SmCommonTestSuite, hashId_different) { 11 | HashId a(HashId::random()), b(HashId::random()); 12 | EXPECT_NE(a, b); 13 | } 14 | 15 | TEST(SmCommonTestSuite, hashId_validity) { 16 | HashId a, b; 17 | EXPECT_FALSE(a.isValid()); 18 | EXPECT_FALSE(b.isValid()); 19 | EXPECT_EQ(a,b); 20 | a.randomize(); 21 | EXPECT_TRUE(a.isValid()); 22 | } 23 | 24 | TEST(SmCommonTestSuite, hashId_string) { 25 | HashId a(HashId::random()), b(HashId::random()); 26 | std::string as(a.hexString()), bs(b.hexString()); 27 | EXPECT_NE(as, bs); 28 | EXPECT_EQ(as.length(), 32); 29 | EXPECT_EQ(bs.length(), 32); 30 | } 31 | 32 | TEST(SmCommonTestSuite, hashId_deserialize) { 33 | HashId a; 34 | std::string as(a.hexString()); 35 | HashId b; 36 | EXPECT_TRUE(b.fromHexString(as)); 37 | EXPECT_EQ(a, b); 38 | } 39 | 40 | TEST(SmCommonTestSuite, hashId_stdHash) { 41 | std::unordered_set hashes; 42 | HashId needle(HashId::random()); 43 | hashes.insert(needle); 44 | hashes.insert(HashId::random()); 45 | std::unordered_set::iterator found = hashes.find(needle); 46 | EXPECT_TRUE(found != hashes.end()); 47 | EXPECT_EQ(*found, needle); 48 | } 49 | -------------------------------------------------------------------------------- /sm_common/test/maths.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | TEST(SmCommonTestSuite,testSolveQuadratic) 6 | { 7 | // ( x - 1) ( x + 2 ) = x^2 + x - 2 8 | // x = 1, x = -2 9 | std::pair r = sm::solveQuadratic(1,1,-2); 10 | if(r.first < r.second) 11 | { 12 | std::swap(r.first,r.second); 13 | } 14 | 15 | ASSERT_NEAR(r.first,1,1e-10); 16 | ASSERT_NEAR(r.second,-2,1e-10); 17 | 18 | 19 | // (2x - 2)(x - 4) = 2x^2 - 10 x + 8 20 | r = sm::solveQuadratic(2, -10, 8); 21 | if(r.first < r.second) 22 | { 23 | std::swap(r.first,r.second); 24 | } 25 | 26 | ASSERT_NEAR(r.first,4,1e-10); 27 | ASSERT_NEAR(r.second,1,1e-10); 28 | 29 | 30 | } 31 | 32 | TEST(SmCommonTestSuite,testSolveQuadraticImaginary) 33 | { 34 | 35 | // b*b - 4*a*c = 0 36 | ASSERT_NO_THROW(sm::solveQuadratic(1,2,1)); 37 | 38 | // b*b - 4*a*c = -3 39 | ASSERT_THROW(sm::solveQuadratic(1,1,1),std::runtime_error); 40 | 41 | } 42 | -------------------------------------------------------------------------------- /sm_common/test/numerical_comparisons.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | TEST(SmCommonTestSuite,testApproximatelyEqual) 5 | { 6 | ASSERT_TRUE(sm::approximatelyEqual(1.0, 1.0, 1e-10)); 7 | ASSERT_FALSE(sm::approximatelyEqual(-1.0, 1.0)); 8 | ASSERT_TRUE(sm::approximatelyEqual(-1.0, -1.0)); 9 | ASSERT_TRUE(sm::approximatelyEqual(1.0, 2.0/2.0)); 10 | ASSERT_TRUE(sm::approximatelyEqual(1000.0, 999.0, 1e-3)); 11 | ASSERT_FALSE(sm::approximatelyEqual(1000.0, 998.0, 1e-3)); 12 | ASSERT_THROW(sm::approximatelyEqual(1.0, 1.0, 0.0), std::invalid_argument); 13 | ASSERT_THROW(sm::approximatelyEqual(1.0, 1.0, -0.1), std::invalid_argument); 14 | } 15 | 16 | TEST(SmCommonTestSuite,testDefinitelyGreaterThan) 17 | { 18 | ASSERT_TRUE(sm::definitelyGreaterThan(2.0, 1.0, 1e-10)); 19 | ASSERT_FALSE(sm::definitelyGreaterThan(1.0, 1.0)); 20 | ASSERT_FALSE(sm::definitelyGreaterThan(-2.0, 1.0)); 21 | ASSERT_TRUE(sm::definitelyGreaterThan(-0.5, -1.0)); 22 | ASSERT_FALSE(sm::definitelyGreaterThan(1000.0, 999.0, 1e-3)); 23 | ASSERT_TRUE(sm::definitelyGreaterThan(1000.0, 998.0, 1e-3)); 24 | ASSERT_THROW(sm::definitelyGreaterThan(2.0, 1.0, 0.0), std::invalid_argument); 25 | ASSERT_THROW(sm::definitelyGreaterThan(2.0, 1.0, -0.1), std::invalid_argument); 26 | } 27 | 28 | TEST(SmCommonTestSuite,testDefinitelyLessThan) 29 | { 30 | ASSERT_TRUE(sm::definitelyLessThan(1.0, 2.0, 1e-10)); 31 | ASSERT_FALSE(sm::definitelyLessThan(1.0, 1.0)); 32 | ASSERT_FALSE(sm::definitelyLessThan(1.0, -2.0)); 33 | ASSERT_TRUE(sm::definitelyLessThan(-1.0, -0.5)); 34 | ASSERT_FALSE(sm::definitelyLessThan(999.0, 1000.0, 1e-3)); 35 | ASSERT_TRUE(sm::definitelyLessThan(998.0, 1000.0, 1e-3)); 36 | ASSERT_THROW(sm::definitelyLessThan(1.0, 2.0, 0.0), std::invalid_argument); 37 | ASSERT_THROW(sm::definitelyLessThan(1.0, 2.0, -0.1), std::invalid_argument); 38 | } 39 | -------------------------------------------------------------------------------- /sm_common/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /// Run all the tests that were declared with TEST() 4 | int main(int argc, char **argv){ 5 | testing::InitGoogleTest(&argc, argv); 6 | return RUN_ALL_TESTS(); 7 | } 8 | -------------------------------------------------------------------------------- /sm_deprecation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sm_deprecation) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | set(CPP14_SUPPORTED OFF) 8 | 9 | MESSAGE(STATUS "Using ${CMAKE_CXX_COMPILER_ID} compiler with version ${CMAKE_CXX_COMPILER_VERSION}.") 10 | 11 | # Most versions are taken from : http://en.cppreference.com/w/cpp/compiler_support 12 | 13 | if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang") 14 | # using Clang 15 | if (CMAKE_CXX_COMPILER_VERSION VERSION_LESS "3.3") 16 | message(FATAL_ERROR "Your clang compiler has version ${CMAKE_CXX_COMPILER_VERSION}, while version 3.3 or later is required") 17 | endif () 18 | if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS "3.5") # 3.4 supports c++14 but with the --std=c++1y flag. 19 | set(CPP14_SUPPORTED ON) 20 | endif () 21 | elseif (CMAKE_CXX_COMPILER_ID STREQUAL "AppleClang") 22 | # using AppleClang 23 | if (CMAKE_CXX_COMPILER_VERSION VERSION_LESS "5.1") 24 | message(FATAL_ERROR "Your XCode environment has version ${CMAKE_CXX_COMPILER_VERSION}, while version 5.1 or later is required") 25 | endif () 26 | if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS "5.2") # TODO replace this guesswork with a correct number 27 | set(CPP14_SUPPORTED ON) 28 | endif () 29 | elseif (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") 30 | # using GCC 31 | if (CMAKE_CXX_COMPILER_VERSION VERSION_LESS "4.8.2") 32 | message(FATAL_ERROR "Your g++ compiler has version ${CMAKE_CXX_COMPILER_VERSION}, while version 4.8.2 or later is required") 33 | endif () 34 | if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS "4.9") 35 | set(CPP14_SUPPORTED ON) 36 | endif () 37 | elseif (CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") 38 | # using MSVC 39 | if (CMAKE_CXX_COMPILER_VERSION VERSION_LESS "19.0.23506") 40 | message(FATAL_ERROR "Your Microsoft Visual C++ compiler has version ${CMAKE_CXX_COMPILER_VERSION}, while version MSVC 2015 Update 1+ is required") 41 | endif () 42 | if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS "19.1") # TODO replace this guesswork with a correct number 43 | set(CPP14_SUPPORTED ON) 44 | endif () 45 | endif () 46 | 47 | if(CPP14_SUPPORTED) 48 | MESSAGE(STATUS "Found compiler with c++14 support! Going to test std c++14 deprecation!") 49 | endif() 50 | 51 | add_subdirectory(test) 52 | 53 | cs_install() 54 | cs_export() 55 | -------------------------------------------------------------------------------- /sm_deprecation/include/sm/deprecation.h: -------------------------------------------------------------------------------- 1 | #ifndef H6F1EF34F_B147_4BB3_8FE9_95D9ACF6C640 2 | #define H6F1EF34F_B147_4BB3_8FE9_95D9ACF6C640 3 | 4 | 5 | #if __cplusplus >= 201402L 6 | #define SM_DEPRECATED(msg) [[deprecated(msg)]] 7 | #define SM_DEPRECATION_SUPPORTED 8 | #elif defined(__GNUC__) 9 | #define SM_DEPRECATED(msg) __attribute__((deprecated)) 10 | #define SM_DEPRECATION_SUPPORTED 11 | #elif defined(_MSC_VER) 12 | #define SM_DEPRECATED(msg) __declspec(deprecated) 13 | #define SM_DEPRECATION_SUPPORTED 14 | #else 15 | #define SM_DEPRECATED(msg) 16 | #endif 17 | 18 | #endif /* H6F1EF34F_B147_4BB3_8FE9_95D9ACF6C640 */ 19 | -------------------------------------------------------------------------------- /sm_deprecation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sm_deprecation 4 | 0.0.1 5 | sm_deprecation 6 | 7 | Hannes Sommer 8 | BSD-3-Clause 9 | catkin 10 | catkin_simple 11 | 12 | 13 | -------------------------------------------------------------------------------- /sm_deprecation/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | macro(compile_and_require_output FILE EXPECT IGNORE) 2 | try_compile(COMPILE_SUCCEEDED 3 | "${CMAKE_CURRENT_BINARY_DIR}" 4 | "${CMAKE_CURRENT_SOURCE_DIR}/${FILE}" 5 | COMPILE_DEFINITIONS "${CMAKE_CXX_FLAGS} -Wall -I${CMAKE_CURRENT_SOURCE_DIR}/../include" 6 | CMAKE_FLAGS "-DCMAKE_CXX_LINK_EXECUTABLE='echo not linking now...'" 7 | OUTPUT_VARIABLE OUTPUT) 8 | if (NOT "${OUTPUT}" MATCHES "${EXPECT}" AND NOT "${OUTPUT}" MATCHES "${IGNORE}") 9 | message( FATAL_ERROR "Output when compiling ${FILE} did not match expected ${EXPECT}. OUTPUT: ${OUTPUT}" ) 10 | endif () 11 | endmacro() 12 | 13 | if(CPP14_SUPPORTED) 14 | MESSAGE(STATUS "Testing c++14 behavior") 15 | set(CMAKE_CXX_FLAGS "-std=c++14") 16 | compile_and_require_output(test_deprecation.cpp "DEP_TEST_MESSAGE" "SM_DEPRECATION_NOT_SUPPORTED") 17 | endif() 18 | 19 | MESSAGE(STATUS "Testing c++11 behavior") 20 | set(CMAKE_CXX_FLAGS "-std=c++11") 21 | compile_and_require_output(test_deprecation.cpp "warning.*foo.*deprecated" "SM_DEPRECATION_NOT_SUPPORTED") 22 | -------------------------------------------------------------------------------- /sm_deprecation/test/test_deprecation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | SM_DEPRECATED("DEP_TEST_MESSAGE") void foo(); 4 | 5 | 6 | 7 | void bar() { 8 | #ifndef SM_DEPRECATION_SUPPORTED 9 | int SM_DEPRECATION_NOT_SUPPORTED; // cause warning about unused variable! 10 | #endif 11 | foo(); 12 | } 13 | -------------------------------------------------------------------------------- /sm_doc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sm_doc) 3 | 4 | find_package(catkin REQUIRED) 5 | find_package(Boost REQUIRED COMPONENTS system) 6 | 7 | catkin_package( 8 | INCLUDE_DIRS 9 | LIBRARIES ${PROJECT_NAME} 10 | CATKIN_DEPENDS 11 | DEPENDS 12 | ) 13 | 14 | ############## 15 | ## Building ## 16 | ############## 17 | 18 | #--------------------------# 19 | # Documentation 20 | SET(DOCSOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/doc) 21 | 22 | 23 | FIND_PACKAGE(Doxygen REQUIRED) 24 | IF(DOXYGEN) 25 | Set(DOCSOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/doc) 26 | SET(DOXYGEN_IMAGE_DIRS "\"${DOCSOURCE_DIR}/figs/\"") 27 | SET(DOXYGEN_SOURCE_DIRS "\"${CMAKE_CURRENT_SOURCE_DIR}/../sm_boost/include/\" \"${CMAKE_CURRENT_SOURCE_DIR}/../sm_common/include/\" \"${CMAKE_CURRENT_SOURCE_DIR}/../sm_database/include/\" \"${CMAKE_CURRENT_SOURCE_DIR}/../sm_eigen/include/\" \"${CMAKE_CURRENT_SOURCE_DIR}/../sm_kinematics/include/\" \"${CMAKE_CURRENT_SOURCE_DIR}/../sm_opencv/include/\" \"${CMAKE_CURRENT_SOURCE_DIR}/../sm_pose_graph/include/\" \"${CMAKE_CURRENT_SOURCE_DIR}/../sm_property_tree/include/\" \"${CMAKE_CURRENT_SOURCE_DIR}/../sm_python/include/\" \"${CMAKE_CURRENT_SOURCE_DIR}/../sm_random/include/\" \"${CMAKE_CURRENT_SOURCE_DIR}/../sm_timing/include/\"") 28 | 29 | Set(HTML_DIR ${CMAKE_CURRENT_SOURCE_DIR}/doc/html) 30 | SET(DOXYGEN_QUIET YES) 31 | MAKE_DIRECTORY(${HTML_DIR}) 32 | SET( HAVE_DOT YES ) 33 | 34 | CONFIGURE_FILE(${DOCSOURCE_DIR}/doxygen.config.in ${DOCSOURCE_DIR}/doxygen.config IMMEDIATE) 35 | IF(DOXYGEN_VERSION VERSION_GREATER 1.8.0) # This version bound might be wrong. But I could not find the precise version transition when this option vanished - not mentioned in the doxygen changelog at all! 36 | SET(DOX_FLAGS) 37 | ELSE() 38 | SET(DOX_FLAGS --silent) 39 | ENDIF() 40 | ADD_CUSTOM_TARGET(doc ALL ${DOXYGEN} ${DOX_FLAGS} ${DOCSOURCE_DIR}/doxygen.config) 41 | ENDIF(DOXYGEN) 42 | 43 | #-------------------------# -------------------------------------------------------------------------------- /sm_doc/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b sm_doc is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /sm_doc/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sm_doc 4 | 0.0.1 5 | sm_doc 6 | 7 | Paul Furgale 8 | New BSD 9 | catkin 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /sm_eigen/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sm_eigen) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | find_package(Boost REQUIRED COMPONENTS system serialization) 8 | 9 | include_directories(${Boost_INCLUDE_DIRS}) 10 | 11 | ############## 12 | ## Building ## 13 | ############## 14 | 15 | cs_add_library(${PROJECT_NAME} 16 | src/random.cpp 17 | ) 18 | 19 | target_link_libraries(${PROJECT_NAME} 20 | ${Boost_LIBRARIES}) 21 | 22 | ############# 23 | ## Testing ## 24 | ############# 25 | # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 26 | add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) 27 | 28 | ## Add gtest based cpp test target and link libraries 29 | catkin_add_gtest(${PROJECT_NAME}-test 30 | test/test_main.cpp 31 | test/EigenSerializationTests.cpp 32 | test/MatrixSqrtTest.cpp 33 | ) 34 | if(TARGET ${PROJECT_NAME}-test) 35 | target_link_libraries(${PROJECT_NAME}-test 36 | ${PROJECT_NAME} 37 | ) 38 | endif() 39 | 40 | cs_install() 41 | cs_export(LIBRARIES ${Boost_LIBRARIES}) 42 | -------------------------------------------------------------------------------- /sm_eigen/include/sm/eigen/NumericalDiff.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_NUMERICAL_DIFF_HPP 2 | #define SM_NUMERICAL_DIFF_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace sm { namespace eigen { 10 | 11 | template 12 | struct NumericalDiffFunctor 13 | { 14 | typedef RESULT_VEC_T value_t; 15 | typedef typename value_t::Scalar scalar_t; 16 | typedef INPUT_VEC_T input_t; 17 | typedef JACOBIAN_T jacobian_t; 18 | 19 | 20 | NumericalDiffFunctor( boost::function< value_t(input_t) > f) : _f(f){} 21 | 22 | value_t operator()(const input_t & x) { return _f(x); } 23 | 24 | input_t update(const input_t & x, int c, scalar_t delta) { input_t xnew = x; xnew[c] += delta; return xnew; } 25 | boost::function _f; 26 | }; 27 | 28 | // A simple implementation of central differences to estimate a Jacobian matrix 29 | template 30 | struct NumericalDiff 31 | { 32 | typedef FUNCTOR_T functor_t; 33 | typedef typename functor_t::input_t input_t; 34 | typedef typename functor_t::value_t value_t; 35 | typedef typename functor_t::scalar_t scalar_t; 36 | typedef typename functor_t::jacobian_t jacobian_t; 37 | 38 | NumericalDiff(functor_t f, scalar_t eps = sqrt(std::numeric_limits::epsilon())) : functor(f), eps(eps) {} 39 | 40 | jacobian_t estimateJacobian(input_t const & x0) 41 | { 42 | // evaluate the function at the operating point: 43 | value_t fx0 = functor(x0); 44 | size_t N = x0.size(); 45 | size_t M = fx0.size(); 46 | 47 | //std::cout << "Size: " << M << ", " << N << std::endl; 48 | jacobian_t J; 49 | J.resize(M, N); 50 | 51 | SM_ASSERT_EQ(std::runtime_error,x0.size(),J.cols(),"Unexpected number of columns for input size"); 52 | SM_ASSERT_EQ(std::runtime_error,fx0.size(),J.rows(),"Unexpected number of columns for output size"); 53 | 54 | for(unsigned c = 0; c < N; c++) { 55 | // Calculate a central difference. 56 | // This step size was stolen from cminpack: temp = eps * fabs(x[j]); 57 | scalar_t rcEps = std::max(static_cast(fabs(x0(c))) * eps,eps); 58 | 59 | value_t fxp = functor(functor.update(x0,c,rcEps)); 60 | value_t fxm = functor(functor.update(x0,c,-rcEps)); 61 | J.block(0, c, M, 1) = (fxp - fxm).template cast()/(typename jacobian_t::Scalar)(rcEps*(scalar_t)2.0); 62 | } 63 | return J; 64 | } 65 | 66 | functor_t functor; 67 | scalar_t eps; 68 | }; 69 | 70 | 71 | template < typename ValueType_, typename InputType_> 72 | Eigen::MatrixXd numericalDiff(std::function function, InputType_ const & input, double eps = sqrt(std::numeric_limits::scalar_t>::epsilon())){ 73 | typedef NumericalDiffFunctor Functor; 74 | 75 | NumericalDiff numDiff(Functor(function), eps); 76 | return numDiff.estimateJacobian(input); 77 | } 78 | 79 | }} 80 | 81 | 82 | #endif /* SM_NUMERICAL_DIFF_HPP */ 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /sm_eigen/include/sm/eigen/assert_macros.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file assert_macros.hpp 3 | * @author Paul Furgale 4 | * @date Mon Dec 12 11:53:43 2011 5 | * 6 | * @brief Code for checking if Eigen matrices are finite. It is slow. Use sparingly. 7 | * 8 | * 9 | */ 10 | 11 | 12 | #ifndef SM_EIGEN_ISNAN_HPP 13 | #define SM_EIGEN_ISNAN_HPP 14 | 15 | #include 16 | #include 17 | 18 | 19 | #define SM_ASSERT_MAT_IS_FINITE(exceptionType, matrix, message) \ 20 | { \ 21 | for(int r = 0; r < matrix.rows(); ++r) \ 22 | { \ 23 | for(int c = 0; c < matrix.cols(); ++c) \ 24 | { \ 25 | if(!std::isfinite(matrix(r,c))) \ 26 | { \ 27 | std::stringstream sm_assert_stringstream; \ 28 | sm_assert_stringstream << "debug assert( isfinite(" << #matrix << "(" << r << ", " << c << ") ) failed. [ isfinite(" << matrix(r,c) << " ) ]" << message << std::endl << matrix; \ 29 | sm::detail::sm_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,sm_assert_stringstream.str()); \ 30 | } \ 31 | } \ 32 | } \ 33 | } 34 | 35 | 36 | #ifndef NDEBUG 37 | 38 | #define SM_ASSERT_MAT_IS_FINITE_DBG(exceptionType, matrix, message) \ 39 | { \ 40 | for(int r = 0; r < matrix.rows(); ++r) \ 41 | { \ 42 | for(int c = 0; c < matrix.cols(); ++c) \ 43 | { \ 44 | if(!std::isfinite(matrix(r,c))) \ 45 | { \ 46 | std::stringstream sm_assert_stringstream; \ 47 | sm_assert_stringstream << "assert( isfinite(" << #matrix << "(" << r << ", " << c << ") ) failed. [ isfinite(" << matrix(r,c) << " ) ]" << message << std::endl << matrix; \ 48 | sm::detail::sm_throw_exception("[" #exceptionType "] ", __FUNCTION__,__FILE__,__LINE__,sm_assert_stringstream.str()); \ 49 | } \ 50 | } \ 51 | } \ 52 | } 53 | 54 | 55 | #else 56 | 57 | #define SM_ASSERT_MAT_IS_FINITE_DBG(exceptionType, matrix, message) 58 | 59 | #endif 60 | 61 | 62 | #endif /* SM_EIGEN_ISNAN_HPP */ 63 | -------------------------------------------------------------------------------- /sm_eigen/include/sm/eigen/matrix_sqrt.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_EIGEN_MATRIX_SQRT_HPP 2 | #define SM_EIGEN_MATRIX_SQRT_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace sm { 8 | namespace eigen { 9 | 10 | /** 11 | * \brief Compute the square root of a matrix using the LDLt decomposition for square, positive semidefinite matrices 12 | * 13 | * To reconstruct the input matrix, \f$ \mathbf A \f$, from the returned matrix, \f$ \mathbf S \f$, 14 | * use, \f$ \mathbf A = \mathbf S \mathbf S^T \f$. 15 | * 16 | * 17 | * @param inMatrix The square matrix whose square root should be computed. 18 | * @param outMatrixSqrt The output square root. 19 | */ 20 | template 21 | /*Eigen::ComputationInfo*/ void computeMatrixSqrt(const Eigen::MatrixBase & inMatrix, 22 | const Eigen::MatrixBase & outMatrixSqrt) 23 | { 24 | SM_ASSERT_EQ_DBG(std::runtime_error, inMatrix.rows(), inMatrix.cols(), "This method is only valid for square input matrices"); 25 | 26 | DERIVED2 & result = const_cast(outMatrixSqrt.derived()); 27 | 28 | // This is tricky. Using the output matrix type causes the input matrix 29 | // type to be upgraded to a real numeric matrix. This is useful if, 30 | // for example, the inMatrix is something like Eigen::Matrix3d::Identity(), 31 | // which is not an actual matrix. Using DERIVED1 as the template argument 32 | // in that case will cause a firestorm of compiler errors. 33 | Eigen::LDLT< DERIVED2 > ldlt(inMatrix.derived()); 34 | result = ldlt.matrixL(); 35 | result = ldlt.transpositionsP().transpose() * result; 36 | result *= ldlt.vectorD().array().sqrt().matrix().asDiagonal(); 37 | 38 | //return ldlt.info(); 39 | 40 | } 41 | 42 | } // namespace eigen 43 | } // namespace sm 44 | 45 | #endif /* SM_EIGEN_MATRIX_SQRT_HPP */ 46 | -------------------------------------------------------------------------------- /sm_eigen/include/sm/eigen/property_tree.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_EIGEN_PROPERTY_TREE_HPP 2 | #define SM_EIGEN_PROPERTY_TREE_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace sm { 8 | namespace eigen { 9 | 10 | inline Eigen::Vector3d vector3FromPropertyTree(const sm::ConstPropertyTree & config) 11 | { 12 | Eigen::Vector3d rval; 13 | rval[0] = config.getDouble("x"); 14 | rval[1] = config.getDouble("y"); 15 | rval[2] = config.getDouble("z"); 16 | 17 | return rval; 18 | } 19 | 20 | 21 | inline Eigen::Vector4d quaternionFromPropertyTree(const sm::ConstPropertyTree & config) 22 | { 23 | Eigen::Vector4d rval; 24 | rval[0] = config.getDouble("x"); 25 | rval[1] = config.getDouble("y"); 26 | rval[2] = config.getDouble("z"); 27 | rval[3] = config.getDouble("w"); 28 | return rval; 29 | } 30 | 31 | 32 | } // namespace eigen 33 | } // namespace sm 34 | 35 | 36 | #endif /* SM_EIGEN_PROPERTY_TREE_HPP */ 37 | -------------------------------------------------------------------------------- /sm_eigen/include/sm/eigen/random.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_EIGEN_RANDOM_HPP 2 | #define SM_EIGEN_RANDOM_HPP 3 | #include 4 | 5 | namespace sm { 6 | namespace eigen { 7 | 8 | Eigen::VectorXd randn(unsigned dim); 9 | 10 | } // namespace eigen 11 | } // namespace sm 12 | 13 | 14 | #endif /* SM_EIGEN_RANDOM_HPP */ 15 | -------------------------------------------------------------------------------- /sm_eigen/include/sm/eigen/static_assert.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_EIGEN_STATIC_ASSERT_HPP 2 | #define SM_EIGEN_STATIC_ASSERT_HPP 3 | 4 | #include 5 | 6 | // static assertion failing if the type \a TYPE is not a vector type of the given size 7 | #define EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(TYPE, SIZE) \ 8 | EIGEN_STATIC_ASSERT(TYPE::IsVectorAtCompileTime && (TYPE::SizeAtCompileTime==SIZE || TYPE::SizeAtCompileTime==Eigen::Dynamic), \ 9 | THIS_METHOD_IS_ONLY_FOR_VECTORS_OF_A_SPECIFIC_SIZE) 10 | 11 | // static assertion failing if the type \a TYPE is not a vector type of the given size 12 | #define EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE_OR_DYNAMIC(TYPE, ROWS, COLS) \ 13 | EIGEN_STATIC_ASSERT( (TYPE::RowsAtCompileTime==ROWS || TYPE::RowsAtCompileTime==Eigen::Dynamic) && (TYPE::ColsAtCompileTime==COLS || TYPE::RowsAtCompileTime==Eigen::Dynamic), \ 14 | THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE) 15 | 16 | 17 | 18 | #endif /* SM_EIGEN_STATIC_ASSERT_HPP */ 19 | -------------------------------------------------------------------------------- /sm_eigen/include/sm/eigen/traits.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_EIGEN_TRAITS 2 | #define SM_EIGEN_TRAITS 3 | 4 | #include 5 | #include 6 | 7 | namespace sm { 8 | namespace eigen { 9 | 10 | template 11 | struct Traits 12 | { 13 | typedef std::vector< T, Eigen::aligned_allocator > vector_t; 14 | }; 15 | 16 | template 17 | struct MapTraits 18 | { 19 | 20 | typedef std::map, 21 | Eigen::aligned_allocator > > map_t; 22 | 23 | }; 24 | 25 | } // namespace eigen 26 | } // namespace sm 27 | 28 | 29 | #endif /* SM_EIGEN_TRAITS */ 30 | -------------------------------------------------------------------------------- /sm_eigen/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b sm_eigen is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /sm_eigen/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sm_eigen 4 | 0.0.1 5 | sm_eigen 6 | 7 | Paul Furgale 8 | New BSD 9 | catkin 10 | catkin_simple 11 | 12 | eigen_catkin 13 | sm_common 14 | sm_random 15 | 16 | sm_common 17 | sm_random 18 | eigen_catkin 19 | 20 | gtest 21 | 22 | -------------------------------------------------------------------------------- /sm_eigen/src/random.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace sm { 5 | namespace eigen { 6 | 7 | 8 | Eigen::VectorXd randn(unsigned dim) { 9 | Eigen::VectorXd v(dim); 10 | for(unsigned i = 0; i < dim; ++i) { 11 | v[i] = sm::random::randn(); 12 | } 13 | return v; 14 | } 15 | 16 | } // namespace eigen 17 | } // namespace sm 18 | -------------------------------------------------------------------------------- /sm_eigen/test/MatrixSqrtTest.cpp: -------------------------------------------------------------------------------- 1 | // Bring in my package's API, which is what I'm testing 2 | 3 | #include 4 | // Bring in gtest 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | 12 | TEST(EigenMatrixSqrtTest, testMatrixSqrt) 13 | { 14 | 15 | for(int i = 0; i < 100; ++i) 16 | { 17 | Eigen::MatrixXd R = sm::eigen::randomCovariance<3>(); 18 | Eigen::MatrixXd sqrtR; 19 | sm::eigen::computeMatrixSqrt(R,sqrtR); 20 | Eigen::MatrixXd reconstructedR = sqrtR * sqrtR.transpose(); 21 | ASSERT_DOUBLE_MX_EQ(R, reconstructedR, 1e-9, "Reconstructing the matrix from the square root in iteration " << i); 22 | } 23 | 24 | 25 | } 26 | -------------------------------------------------------------------------------- /sm_eigen/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /// Run all the tests that were declared with TEST() 4 | int main(int argc, char **argv){ 5 | testing::InitGoogleTest(&argc, argv); 6 | return RUN_ALL_TESTS(); 7 | } 8 | -------------------------------------------------------------------------------- /sm_kinematics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sm_kinematics) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | find_package(Boost REQUIRED COMPONENTS system serialization filesystem) 8 | 9 | add_definitions(-Wall -std=c++0x) 10 | include_directories(include ${Boost_INCLUDE_DIRS}) 11 | 12 | ############## 13 | ## Building ## 14 | ############## 15 | 16 | cs_add_library(${PROJECT_NAME} 17 | src/quaternion_algebra.cpp 18 | src/rotations.cpp 19 | src/transformations.cpp 20 | src/RotationalKinematics.cpp 21 | src/RotationVector.cpp 22 | src/EulerAnglesZYX.cpp 23 | src/EulerAnglesYawPitchRoll.cpp 24 | src/EulerRodriguez.cpp 25 | src/Transformation.cpp 26 | src/homogeneous_coordinates.cpp 27 | src/HomogeneousPoint.cpp 28 | src/UncertainTransformation.cpp 29 | src/UncertainHomogeneousPoint.cpp 30 | src/three_point_methods.cpp 31 | src/EulerAnglesZXY.cpp 32 | ) 33 | 34 | target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) 35 | 36 | ############# 37 | ## Testing ## 38 | ############# 39 | # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 40 | add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) 41 | 42 | ## Add gtest based cpp test target and link libraries 43 | catkin_add_gtest(${PROJECT_NAME}-test 44 | test/RotationalKinematicsTests.cpp 45 | test/QuaternionTests.cpp 46 | test/TransformationTests.cpp 47 | test/transformations.cpp 48 | test/HomogeneousPoint.cpp 49 | test/UncertainHomogeneousPoint.cpp 50 | test/test_main.cpp 51 | test/UncertainTransformationTests.cpp 52 | test/homogeneous_coordinates.cpp 53 | test/three_point_methods.cpp 54 | ) 55 | if(TARGET ${PROJECT_NAME}-test) 56 | target_link_libraries(${PROJECT_NAME}-test 57 | ${PROJECT_NAME}) 58 | endif() 59 | 60 | cs_install() 61 | cs_export() 62 | -------------------------------------------------------------------------------- /sm_kinematics/include/sm/kinematics/EulerAnglesYawPitchRoll.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _EULERANGLESYAWPITCHROLL_H_ 2 | #define _EULERANGLESYAWPITCHROLL_H_ 3 | 4 | #include "RotationalKinematics.hpp" 5 | 6 | namespace sm { namespace kinematics { 7 | 8 | /** 9 | * @class EulerAnglesYawPitchRoll 10 | * This is the standard Rotation sequence for aerospace vehicles. 11 | * 12 | * Passing this the parameters [yaw, pitch, roll] of a vehicle frame, F_v, 13 | * with respect to an inertial frame, F_i, returns the rotation matrix, 14 | * C_iv, the rotation that takes vectors from F_v, to F_i. 15 | * 16 | * Similarly passing the rotation matrix C_iv to this class will cause 17 | * return the vector [heading, pitch, roll] where these represent the usual 18 | * meaning with respect to aerospace vehicles. 19 | * 20 | * This assumes that the vehicle frame has: 21 | * x-axis pointing to the front. 22 | * y-axis pointing to the left. 23 | * z-axis pointing up. 24 | * 25 | * heading: positive is vehicle nose left (counter clockwise). 26 | * pitch: positive is vehicle nose down. 27 | * roll: positive is left wing up. 28 | * 29 | */ 30 | class EulerAnglesYawPitchRoll : public RotationalKinematics 31 | { 32 | public: 33 | ~EulerAnglesYawPitchRoll() override; 34 | // Three functions that must be implemented. 35 | Eigen::Matrix3d parametersToRotationMatrix(const Eigen::Vector3d & parameters, Eigen::Matrix3d * S = NULL) const override; 36 | Eigen::Vector3d rotationMatrixToParameters(const Eigen::Matrix3d & rotationMatrix) const override; 37 | Eigen::Matrix3d parametersToSMatrix(const Eigen::Vector3d & parameters) const override; 38 | Eigen::Vector3d angularVelocityAndJacobian(const Eigen::Vector3d & p, const Eigen::Vector3d & pdot, Eigen::Matrix * Jacobian) const override; 39 | private: 40 | // A helper function 41 | Eigen::Matrix3d buildSMatrix(double sz, double cz, double sy, double cy) const; 42 | }; 43 | 44 | }} // namespace sm::kinematics 45 | 46 | 47 | #endif /* _EULERANGLESYAWPITCHROLL_H_ */ 48 | -------------------------------------------------------------------------------- /sm_kinematics/include/sm/kinematics/EulerAnglesZYX.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_ROTATIONAL_KINEMATICS_EXYZ_HPP 2 | #define SM_ROTATIONAL_KINEMATICS_EXYZ_HPP 3 | 4 | #include "RotationalKinematics.hpp" 5 | 6 | namespace sm { namespace kinematics { 7 | 8 | class EulerAnglesZYX : public RotationalKinematics 9 | { 10 | public: 11 | ~EulerAnglesZYX() override; 12 | // Three functions that must be implemented. 13 | Eigen::Matrix3d parametersToRotationMatrix(const Eigen::Vector3d & parameters, Eigen::Matrix3d * S = NULL) const override; 14 | Eigen::Vector3d rotationMatrixToParameters(const Eigen::Matrix3d & rotationMatrix) const override; 15 | Eigen::Matrix3d parametersToSMatrix(const Eigen::Vector3d & parameters) const override; 16 | Eigen::Vector3d angularVelocityAndJacobian(const Eigen::Vector3d & p, const Eigen::Vector3d & pdot, Eigen::Matrix * Jacobian) const override; 17 | 18 | private: 19 | // A helper function 20 | Eigen::Matrix3d buildSMatrix(double sz, double cz, double sy, double cy) const; 21 | }; 22 | 23 | }} // namespace sm::kinematics 24 | 25 | 26 | #endif 27 | -------------------------------------------------------------------------------- /sm_kinematics/include/sm/kinematics/EulerRodriguez.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_ROTATIONAL_KINEMATICS_ER_HPP 2 | #define SM_ROTATIONAL_KINEMATICS_ER_HPP 3 | 4 | #include "RotationalKinematics.hpp" 5 | 6 | namespace sm { namespace kinematics { 7 | 8 | class EulerRodriguez : public RotationalKinematics 9 | { 10 | public: 11 | ~EulerRodriguez() override; 12 | // Three functions that must be implemented. 13 | Eigen::Matrix3d parametersToRotationMatrix(const Eigen::Vector3d & parameters, Eigen::Matrix3d * S = NULL) const override; 14 | Eigen::Vector3d rotationMatrixToParameters(const Eigen::Matrix3d & rotationMatrix) const override; 15 | Eigen::Matrix3d parametersToSMatrix(const Eigen::Vector3d & parameters) const override; 16 | Eigen::Vector3d angularVelocityAndJacobian(const Eigen::Vector3d & p, const Eigen::Vector3d & pdot, Eigen::Matrix * Jacobian) const override; 17 | }; 18 | 19 | 20 | }} // namespace sm::kinematics 21 | 22 | 23 | #endif /* SM_ROTATIONAL_KINEMATICS_HPP */ 24 | -------------------------------------------------------------------------------- /sm_kinematics/include/sm/kinematics/RotationVector.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _ROTATIONVECTOR_H_ 2 | #define _ROTATIONVECTOR_H_ 3 | 4 | #include "RotationalKinematics.hpp" 5 | 6 | namespace sm { namespace kinematics { 7 | 8 | class RotationVector : public RotationalKinematics 9 | { 10 | public: 11 | ~RotationVector() override; 12 | Eigen::Matrix3d parametersToRotationMatrix(const Eigen::Vector3d & parameters, Eigen::Matrix3d * S = NULL) const override; 13 | Eigen::Vector3d rotationMatrixToParameters(const Eigen::Matrix3d & rotationMatrix) const override; 14 | Eigen::Matrix3d parametersToSMatrix(const Eigen::Vector3d & parameters) const override; 15 | Eigen::Vector3d angularVelocityAndJacobian(const Eigen::Vector3d & p, const Eigen::Vector3d & pdot, Eigen::Matrix * Jacobian) const override; 16 | // Fabio: 17 | Eigen::Matrix3d parametersToInverseSMatrix(const Eigen::Vector3d & parameters) const; 18 | }; 19 | 20 | }} // sm::kinematics 21 | 22 | #endif /* _ROTATIONVECTOR_H_ */ 23 | -------------------------------------------------------------------------------- /sm_kinematics/include/sm/kinematics/RotationalKinematics.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_ROTATIONAL_KINEMATICS_HPP 2 | #define SM_ROTATIONAL_KINEMATICS_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include "rotations.hpp" 8 | 9 | namespace sm { namespace kinematics { 10 | class RotationalKinematics 11 | { 12 | public: 13 | SM_DEFINE_EXCEPTION(Exception,std::runtime_error); 14 | virtual ~RotationalKinematics(); 15 | typedef ::boost::shared_ptr Ptr; 16 | typedef ::boost::shared_ptr ConstPtr; 17 | 18 | // Three functions that must be implemented. 19 | virtual Eigen::Matrix3d parametersToRotationMatrix(const Eigen::Vector3d & parameters, Eigen::Matrix3d * S = NULL) const = 0; 20 | virtual Eigen::Vector3d rotationMatrixToParameters(const Eigen::Matrix3d & rotationMatrix) const = 0; 21 | virtual Eigen::Matrix3d parametersToSMatrix(const Eigen::Vector3d & parameters) const = 0; 22 | virtual Eigen::Vector3d angularVelocityAndJacobian(const Eigen::Vector3d & /* p */, const Eigen::Vector3d & /* pdot */, Eigen::Matrix * /* Jacobian */) const {SM_THROW(Exception,"Not implemented"); return Eigen::Vector3d::Zero();} 23 | }; 24 | 25 | 26 | 27 | }} // namespace sm::kinematics 28 | 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /sm_kinematics/include/sm/kinematics/homogeneous_coordinates.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_MATH_HOMOGENEOUS_COORDINATES_HPP 2 | #define SM_MATH_HOMOGENEOUS_COORDINATES_HPP 3 | 4 | #include 5 | 6 | namespace sm { namespace kinematics { 7 | 8 | Eigen::Matrix toHomogeneousJacobian(const Eigen::Vector3d & v); 9 | Eigen::Vector4d toHomogeneous(const Eigen::Vector3d & v, Eigen::Matrix * jacobian = NULL); 10 | 11 | 12 | Eigen::Matrix fromHomogeneousJacobian(const Eigen::Vector4d & v); 13 | Eigen::Vector3d fromHomogeneous(const Eigen::Vector4d & v, Eigen::Matrix * jacobian = NULL); 14 | 15 | 16 | Eigen::MatrixXd toHomogeneousColumns(const Eigen::MatrixXd & M); 17 | Eigen::MatrixXd fromHomogeneousColumns(const Eigen::MatrixXd & M); 18 | 19 | template 20 | Eigen::Matrix normalize(const Eigen::Matrix & v) 21 | { 22 | return v/v.norm(); 23 | } 24 | 25 | template 26 | Eigen::Matrix normalizeAndJacobian(const Eigen::Matrix & v, Eigen::Matrix & outJacobian) 27 | { 28 | double recip_s_vtv = 1.0/v.norm(); 29 | 30 | outJacobian = (Eigen::Matrix::Identity() * recip_s_vtv) - (recip_s_vtv*recip_s_vtv*recip_s_vtv)*v*v.transpose(); 31 | 32 | return v*recip_s_vtv; 33 | } 34 | 35 | /// \brief to the matrix that implements "plus" for homogeneous coordinates. 36 | /// this operation has the same result as adding the corresponding Euclidean points. 37 | Eigen::Matrix4d toHomogeneousPlus(const Eigen::Vector4d & ph); 38 | 39 | /// \brief to the matrix that implements "minus" for homogeneous coordinates. 40 | /// this operation has the same result as adding the corresponding Euclidean points. 41 | Eigen::Matrix4d toHomogeneousMinus(const Eigen::Vector4d & ph); 42 | 43 | }} // namespace sm::kinematics 44 | 45 | #endif /* SM_MATH_HOMOGENEOUS_COORDINATES_HPP */ 46 | -------------------------------------------------------------------------------- /sm_kinematics/include/sm/kinematics/property_tree.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_KINEMATICS_PROPERTY_TREE_HPP 2 | #define SM_KINEMATICS_PROPERTY_TREE_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace sm { 8 | namespace kinematics { 9 | 10 | inline sm::kinematics::Transformation transformationFromPropertyTree(const sm::ConstPropertyTree & config) 11 | { 12 | Eigen::Vector4d q = sm::eigen::quaternionFromPropertyTree( sm::ConstPropertyTree(config, "q_a_b") ); 13 | q = q / q.norm(); 14 | Eigen::Vector3d t = sm::eigen::vector3FromPropertyTree( sm::ConstPropertyTree(config, "t_a_b_a" ) ); 15 | return sm::kinematics::Transformation(q,t); 16 | } 17 | 18 | 19 | } // namespace kinematics 20 | } // namespace sm 21 | 22 | 23 | #endif /* SM_KINEMATICS_PROPERTY_TREE_HPP */ 24 | -------------------------------------------------------------------------------- /sm_kinematics/include/sm/kinematics/rotations.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file rotations.hpp 3 | * @author Paul Furgale 4 | * @date Mon Nov 22 21:45:54 2010 5 | * 6 | * @brief 7 | * 8 | * 9 | */ 10 | 11 | #ifndef SM_ROTATIONS_HPP 12 | #define SM_ROTATIONS_HPP 13 | 14 | #define SM_2_PI 0.6366197723675814 // 2/pi 15 | #define SM_PI 3.141592653589793 // pi 16 | #define SM_PI_2 1.5707963267948966 // pi/2 17 | #define SM_PI_4 0.7853981633974483 // pi/4 18 | #define SM_2PI 6.283185307179586 // 2*pi 19 | #define SM_DEG2RAD 0.017453292519943 // pi/180 20 | #define SM_RAD2DEG 57.295779513082323 // 180/pi 21 | 22 | #define SM_1_PI_F 0.3183098861837907f 23 | #define SM_2_PI_F 0.6366197723675814f 24 | #define SM_PI_F 3.141592653589793f 25 | #define SM_PI_2_F 1.5707963267948966f 26 | #define SM_PI_4_F 0.7853981633974483f 27 | #define SM_2PI_F 6.283185307179586f 28 | #define SM_DEG2RAD_F 0.017453292519943f 29 | #define SM_RAD2DEG_F 57.295779513082323f 30 | 31 | #include 32 | 33 | namespace sm { namespace kinematics { 34 | 35 | // Principle axis rotations. 36 | Eigen::Matrix3d Rx(double radians); 37 | Eigen::Matrix3d Ry(double radians); 38 | Eigen::Matrix3d Rz(double radians); 39 | 40 | Eigen::Matrix3d rph2R(double x, double y, double z); 41 | Eigen::Matrix3d rph2R(Eigen::Vector3d const & x); 42 | Eigen::Vector3d R2rph(Eigen::Matrix3d const & C); 43 | 44 | // The C rotations are more standard. They go the other way 45 | Eigen::Matrix3d Cx(double radians); 46 | Eigen::Matrix3d Cy(double radians); 47 | Eigen::Matrix3d Cz(double radians); 48 | 49 | Eigen::Matrix3d rph2C(double x, double y, double z); 50 | Eigen::Matrix3d rph2C(Eigen::Vector3d const & x); 51 | Eigen::Vector3d C2rph(Eigen::Matrix3d const & C); 52 | 53 | // Small angle approximation. 54 | template 55 | Eigen::Matrix crossMx(Scalar_ x, Scalar_ y, Scalar_ z); 56 | 57 | template 58 | Eigen::Matrix::Scalar, 3, 3> crossMx(Eigen::MatrixBase const & x){ return crossMx(x(0, 0),x(1, 0),x(2, 0)); } 59 | 60 | 61 | 62 | // Axis Angle rotation. 63 | Eigen::Matrix3d axisAngle2R(double a, double ax, double ay, double az); 64 | Eigen::Matrix3d axisAngle2R(double x, double y, double z); 65 | Eigen::Matrix3d axisAngle2R(Eigen::Vector3d const & x); 66 | Eigen::Vector3d R2AxisAngle(Eigen::Matrix3d const & C); 67 | 68 | // Utility functions 69 | // Moves a value in radians to within -pi, pi 70 | double angleMod(double radians); 71 | double deg2rad(double degrees); 72 | double rad2deg(double radians); 73 | 74 | extern template Eigen::Matrix crossMx(double x, double y, double z); 75 | extern template Eigen::Matrix crossMx(float x, float y, float z); 76 | extern template Eigen::Matrix crossMx(const Eigen::MatrixBase > &); 77 | extern template Eigen::Matrix crossMx(const Eigen::MatrixBase > &); 78 | extern template Eigen::Matrix crossMx(const Eigen::MatrixBase > &); 79 | extern template Eigen::Matrix crossMx(const Eigen::MatrixBase > &); 80 | }} // namespace sm::kinematics 81 | 82 | #endif // ndef SM_ROTATIONS_HPP 83 | -------------------------------------------------------------------------------- /sm_kinematics/include/sm/kinematics/three_point_methods.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_THREE_POINT_METHODS 2 | #define SM_THREE_POINT_METHODS 3 | 4 | #include 5 | 6 | namespace sm { namespace kinematics { 7 | 8 | /** 9 | * Use a three-point SVD method to compute the transformation between two sets of points. 10 | * 11 | * @param p0 a 3xK matrix where each column is a point expressed in frame zero 12 | * @param p1 a 3xK matrix where each column is a point expressed in frame one 13 | * @return the 4x4 transformation matrix that is the best fit transformation \f$p_0 = \mathbf T_{0,1} p_1\f$ between the two point sets. 14 | */ 15 | Eigen::Matrix4d threePointSvd(Eigen::MatrixXd const & p0, Eigen::MatrixXd const & p1); 16 | 17 | /** 18 | * Compute the rotation between two sets of unit vectors using Davenport's q-method 19 | * P. Davenport, “A vector approach to the algebra of rotations with applications,” 20 | * NASA Technical Note TN D-4696, Aug. 1968. 21 | * 22 | * @param p0 a 3xK matrix where each column is a unit vector expressed in frame zero 23 | * @param p1 a 3xK matrix where each column is a unit vector expressed in frame one 24 | * @param w a Kx1 vector where each entry is a scalar weight for the corresponding pair of unit vectors 25 | * 26 | * @return the 3x3 transformation matrix that is the best fit rotation \f$p_0 = \mathbf C_{0,1} p_1\f$ between the two point sets. 27 | */ 28 | Eigen::Matrix3d qMethod(Eigen::MatrixXd const & p0, Eigen::MatrixXd const & p1, const Eigen::VectorXd & w); 29 | 30 | /** 31 | * Compute the rotation between two sets of unit vectors using Davenport's q-method 32 | * P. Davenport, “A vector approach to the algebra of rotations with applications,” 33 | * NASA Technical Note TN D-4696, Aug. 1968. 34 | * 35 | * In this overloaded version, all points are weighted equally 36 | * 37 | * @param p0 a 3xK matrix where each column is a unit vector expressed in frame zero 38 | * @param p1 a 3xK matrix where each column is a unit vector expressed in frame one 39 | * 40 | * @return the 3x3 transformation matrix that is the best fit rotation \f$p_0 = \mathbf C_{0,1} p_1\f$ between the two point sets. 41 | */ 42 | Eigen::Matrix3d qMethod(Eigen::MatrixXd const & p0, Eigen::MatrixXd const & p1); 43 | 44 | 45 | }} // namespace sm::kinematics 46 | 47 | #endif /* SM_THREE_POINT_METHODS */ 48 | -------------------------------------------------------------------------------- /sm_kinematics/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b sm_kinematics is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /sm_kinematics/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sm_kinematics 4 | 0.0.1 5 | sm_kinematics 6 | 7 | Paul Furgale 8 | New BSD 9 | catkin 10 | catkin_simple 11 | 12 | eigen_catkin 13 | sm_boost 14 | sm_common 15 | sm_eigen 16 | sm_random 17 | 18 | sm_boost 19 | sm_common 20 | sm_eigen 21 | sm_random 22 | 23 | gtest 24 | 25 | -------------------------------------------------------------------------------- /sm_kinematics/src/EulerAnglesZYX.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace sm { namespace kinematics { 4 | 5 | 6 | EulerAnglesZYX::~EulerAnglesZYX() 7 | { 8 | 9 | } 10 | 11 | 12 | Eigen::Matrix3d EulerAnglesZYX::parametersToRotationMatrix(const Eigen::Vector3d & parameters, Eigen::Matrix3d * S) const 13 | { 14 | Eigen::Matrix3d C; 15 | double cx = cos(-parameters[2]); 16 | double sx = sin(-parameters[2]); 17 | double cy = cos(-parameters[1]); 18 | double sy = sin(-parameters[1]); 19 | double cz = cos(-parameters[0]); 20 | double sz = sin(-parameters[0]); 21 | //[cos(z)*cos(y), -sin(z)*cos(x)+cos(z)*sin(y)*sin(x), sin(z)*sin(x)+cos(z)*sin(y)*cos(x)] 22 | //[sin(z)*cos(y), cos(z)*cos(x)+sin(z)*sin(y)*sin(x), -cos(z)*sin(x)+sin(z)*sin(y)*cos(x)] 23 | //[ -sin(y), cos(y)*sin(x), cos(y)*cos(x)] 24 | C << 25 | cz*cy, -sz*cx+cz*sy*sx, sz*sx+cz*sy*cx, 26 | sz*cy, cz*cx+sz*sy*sx, -cz*sx+sz*sy*cx, 27 | -sy, cy*sx, cy*cx; 28 | 29 | if(S) 30 | { 31 | *S << buildSMatrix(sz, cz, sy, cy); 32 | } 33 | 34 | return C; 35 | } 36 | 37 | Eigen::Vector3d EulerAnglesZYX::rotationMatrixToParameters(const Eigen::Matrix3d & rotationMatrix) const 38 | { 39 | double phi = asin(rotationMatrix(2,0)); 40 | double theta = atan2(rotationMatrix(2,1),rotationMatrix(2,2)); 41 | double psi = atan2(rotationMatrix(1,0), rotationMatrix(0,0)); 42 | 43 | Eigen::Vector3d ret; 44 | ret[2] = -theta; 45 | ret[1] = phi; 46 | ret[0] = -psi; 47 | 48 | return ret; 49 | } 50 | 51 | Eigen::Matrix3d EulerAnglesZYX::parametersToSMatrix(const Eigen::Vector3d & parameters) const 52 | { 53 | double cy = cos(-parameters[1]); 54 | double sy = sin(-parameters[1]); 55 | double cz = cos(-parameters[0]); 56 | double sz = sin(-parameters[0]); 57 | return buildSMatrix(sz, cz, sy, cy); 58 | } 59 | 60 | Eigen::Matrix3d EulerAnglesZYX::buildSMatrix(double sz, double cz, double sy, double cy) const 61 | { 62 | Eigen::Matrix3d S; 63 | S << 64 | 0.0, -sz, cy*cz, 65 | 0.0, cz, cy*sz, 66 | 1.0, 0.0, -sy; 67 | 68 | return S; 69 | } 70 | 71 | Eigen::Vector3d EulerAnglesZYX::angularVelocityAndJacobian(const Eigen::Vector3d & p, const Eigen::Vector3d & pdot, Eigen::Matrix * Jacobian) const 72 | { 73 | //double cx = cos(-p[2]); 74 | //double sx = sin(-p[2]); 75 | double cy = cos(-p[1]); 76 | double sy = sin(-p[1]); 77 | double cz = cos(-p[0]); 78 | double sz = sin(-p[0]); 79 | 80 | Eigen::Matrix3d S = buildSMatrix(sz, cz, sy, cy); 81 | 82 | Eigen::Vector3d omega = S * pdot; 83 | 84 | if(Jacobian) 85 | { 86 | *Jacobian = Eigen::Matrix::Zero(); 87 | (*Jacobian)(0,0) = cz*pdot[1] + cy*sz*pdot[2]; 88 | (*Jacobian)(0,1) = cz*sy*pdot[2]; 89 | (*Jacobian)(1,0) = sz*pdot[1] - cz*cy*pdot[2]; 90 | (*Jacobian)(1,1) = sz*sy*pdot[2]; 91 | (*Jacobian)(2,1) = cy*pdot[2]; 92 | Jacobian->block(0,3,3,3) = S; 93 | } 94 | 95 | return omega; 96 | } 97 | 98 | 99 | }} // sm::kinematics 100 | -------------------------------------------------------------------------------- /sm_kinematics/src/EulerRodriguez.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace sm { namespace kinematics { 4 | 5 | 6 | EulerRodriguez::~EulerRodriguez() 7 | { 8 | 9 | } 10 | 11 | Eigen::Matrix3d EulerRodriguez::parametersToRotationMatrix(const Eigen::Vector3d & parameters, Eigen::Matrix3d * S) const 12 | { 13 | Eigen::Matrix3d C; 14 | 15 | Eigen::Matrix3d crossp = crossMx(parameters); 16 | 17 | C = Eigen::Matrix3d::Identity() + (2.0/(1.0 + parameters.squaredNorm()))*(crossp * crossp - crossp); 18 | 19 | if(S) 20 | { 21 | *S = parametersToSMatrix(parameters); 22 | } 23 | 24 | return C; 25 | } 26 | 27 | Eigen::Vector3d EulerRodriguez::rotationMatrixToParameters(const Eigen::Matrix3d & C) const 28 | { 29 | Eigen::Vector3d p; 30 | double a = acos( (C(0,0) + C(1,1) + C(2,2) - double(1)) * double(0.5)); 31 | 32 | if(fabs(a) < 1e-14){ 33 | return Eigen::Vector3d::Zero(); 34 | } 35 | 36 | p[0] = (C(2,1) - C(1,2)); 37 | p[1] = (C(0,2) - C(2,0)); 38 | p[2] = (C(1,0) - C(0,1)); 39 | 40 | double n2 = p.norm(); 41 | if(n2 < 1e-14) 42 | { 43 | return Eigen::Vector3d::Zero(); 44 | } 45 | 46 | double scale = -tan(0.5*a)/n2; 47 | p = scale * p; 48 | 49 | 50 | return p; 51 | } 52 | 53 | Eigen::Matrix3d EulerRodriguez::parametersToSMatrix(const Eigen::Vector3d & parameters) const 54 | { 55 | Eigen::Matrix3d S; 56 | Eigen::Matrix3d crossp = crossMx(parameters); 57 | S = (2.0/(1.0 + parameters.squaredNorm()))*(Eigen::Matrix3d::Identity() - crossp); 58 | 59 | return S; 60 | } 61 | 62 | Eigen::Vector3d EulerRodriguez::angularVelocityAndJacobian(const Eigen::Vector3d & p, const Eigen::Vector3d & pdot, Eigen::Matrix * Jacobian) const 63 | { 64 | Eigen::Vector3d omega; 65 | Eigen::Matrix3d S; 66 | Eigen::Matrix3d crossp = crossMx(p); 67 | double recip_denom = 1.0/(1.0 + p.squaredNorm()); 68 | double f = 2 * recip_denom; 69 | S = f * (Eigen::Matrix3d::Identity() - crossp); 70 | omega = S*pdot; 71 | 72 | 73 | 74 | if(Jacobian) 75 | { 76 | Eigen::Vector3d a = -4.0 * recip_denom * recip_denom * p; 77 | *Jacobian = Eigen::Matrix::Zero(); 78 | Jacobian->block(0,0,3,3) = pdot * a.transpose() - crossp * pdot * a.transpose() + f * crossMx(pdot); 79 | Jacobian->block(0,3,3,3) = S; 80 | } 81 | 82 | return omega; 83 | 84 | } 85 | 86 | 87 | 88 | 89 | 90 | }} // sm::kinematics 91 | -------------------------------------------------------------------------------- /sm_kinematics/src/RotationalKinematics.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace sm { namespace kinematics { 4 | 5 | 6 | RotationalKinematics::~RotationalKinematics() 7 | { 8 | 9 | } 10 | 11 | 12 | }} // sm::kinematics 13 | -------------------------------------------------------------------------------- /sm_kinematics/src/homogeneous_coordinates.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace sm { namespace kinematics{ 5 | 6 | Eigen::Matrix toHomogeneousJacobian(const Eigen::Vector3d &) 7 | { 8 | return Eigen::Matrix::Identity(); 9 | } 10 | 11 | Eigen::Vector4d toHomogeneous(const Eigen::Vector3d & v, Eigen::Matrix * jacobian) 12 | { 13 | if(jacobian) 14 | { 15 | *jacobian = toHomogeneousJacobian(v); 16 | } 17 | return Eigen::Vector4d(v[0],v[1],v[2],1.0); 18 | } 19 | 20 | Eigen::Matrix fromHomogeneousJacobian(const Eigen::Vector4d & v) 21 | { 22 | Eigen::Matrix J; 23 | 24 | fromHomogeneous(v,&J); 25 | return J; 26 | 27 | } 28 | 29 | Eigen::Vector3d fromHomogeneous(const Eigen::Vector4d & v, Eigen::Matrix * jacobian) 30 | { 31 | double rv3 = 1.0/v[3]; 32 | 33 | Eigen::Vector3d v3; 34 | v3 = v.head<3>() * rv3; 35 | 36 | if(jacobian) 37 | { 38 | jacobian->topRightCorner<3,1>() = -v3 * rv3; 39 | jacobian->topLeftCorner<3,3>() = Eigen::Matrix3d::Identity() * rv3; 40 | } 41 | 42 | return v3; 43 | } 44 | 45 | 46 | Eigen::MatrixXd toHomogeneousColumns(const Eigen::MatrixXd & M) 47 | { 48 | Eigen::MatrixXd hM(Eigen::MatrixXd::Ones(M.rows() + 1, M.cols())); 49 | hM.topRows(M.rows()) = M; 50 | return hM; 51 | } 52 | 53 | Eigen::MatrixXd fromHomogeneousColumns(const Eigen::MatrixXd & hM) 54 | { 55 | SM_ASSERT_GE_DBG(std::runtime_error,hM.rows(),2,"A homogeneous matrix must have at least 2 rows"); 56 | 57 | Eigen::MatrixXd M(hM.topRows(hM.rows() - 1)); 58 | Eigen::Matrix denom(Eigen::Matrix::Ones(hM.cols()).array()/hM.row(hM.rows()-1).array()); 59 | 60 | 61 | for(int i = 0; i < M.rows(); i++) 62 | { 63 | M.row(i).array() *= denom.array(); 64 | } 65 | 66 | return M; 67 | } 68 | 69 | /// \brief to the matrix that implements "plus" for homogeneous coordinates. 70 | /// this operation has the same result as adding the corresponding Euclidean points. 71 | Eigen::Matrix4d toHomogeneousPlus(const Eigen::Vector4d & ph) 72 | { 73 | Eigen::Matrix4d rval; 74 | rval << 75 | ph[3], 0.0, 0.0, ph[0], 76 | 0.0 , ph[3], 0.0, ph[1], 77 | 0.0 , 0.0, ph[3], ph[2], 78 | 0.0 , 0.0, 0.0, ph[3]; 79 | 80 | return rval; 81 | 82 | } 83 | 84 | /// \brief to the matrix that implements "minus" for homogeneous coordinates. 85 | /// this operation has the same result as adding the corresponding Euclidean points. 86 | Eigen::Matrix4d toHomogeneousMinus(const Eigen::Vector4d & ph) 87 | { 88 | Eigen::Matrix4d rval; 89 | rval << 90 | ph[3], 0.0, 0.0, -ph[0], 91 | 0.0 , ph[3], 0.0, -ph[1], 92 | 0.0 , 0.0, ph[3], -ph[2], 93 | 0.0 , 0.0, 0.0, ph[3]; 94 | 95 | return rval; 96 | } 97 | 98 | 99 | 100 | }} // namespace sm::kinematics 101 | -------------------------------------------------------------------------------- /sm_kinematics/test/HomogeneousPoint.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "PointTestHarness.hpp" 5 | 6 | TEST(HomogeneousPointTestSuite, testHpoints) 7 | { 8 | sm::PointTestHarness harness(1e-6); 9 | harness.testAll(); 10 | } 11 | -------------------------------------------------------------------------------- /sm_kinematics/test/UncertainHomogeneousPoint.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "PointTestHarness.hpp" 5 | 6 | TEST(UncertainHomogeneousPointTestSuite, testUHpoints) 7 | { 8 | sm::PointTestHarness harness(1e-7); 9 | harness.testAll(); 10 | } 11 | 12 | 13 | TEST(UncertainHomogeneousPointTestSuite, testU3) 14 | { 15 | using namespace sm::kinematics; 16 | Eigen::Vector3d p; 17 | p.setRandom(); 18 | Eigen::Matrix3d U; 19 | U = sm::eigen::randomCovariance<3>(); 20 | 21 | UncertainHomogeneousPoint uhp(p,U); 22 | 23 | sm::eigen::assertNear(U, uhp.U3(), 1e-10, SM_SOURCE_FILE_POS, "Checking that I can recover the Euclideam point uncertainty"); 24 | } 25 | 26 | TEST(UncertainHomogeneousPointTestSuite, testUav) 27 | { 28 | try { 29 | using namespace sm::kinematics; 30 | Eigen::Vector4d p; 31 | p.setRandom(); 32 | p = p/p.norm(); 33 | Eigen::Matrix3d U; 34 | U = sm::eigen::randomCovariance<3>(); 35 | 36 | UncertainHomogeneousPoint uhp(p,U); 37 | 38 | sm::eigen::assertNear(U, uhp.U_av_form(), 1e-10, SM_SOURCE_FILE_POS, "Checking that I can recover the av/form point uncertainty"); 39 | } 40 | catch(std::exception const & e) 41 | { 42 | FAIL() << e.what(); 43 | } 44 | } 45 | 46 | TEST(UncertainHomogeneousPointTestSuite, testU3normalized) 47 | { 48 | using namespace sm::kinematics; 49 | Eigen::Vector3d p; 50 | p.setRandom(); 51 | Eigen::Matrix3d U; 52 | U = sm::eigen::randomCovariance<3>(); 53 | 54 | UncertainHomogeneousPoint uhp(p,U); 55 | 56 | uhp.normalize(); 57 | 58 | ASSERT_NEAR(uhp.toHomogeneous().norm(),1.0,1e-14); 59 | sm::eigen::assertNear(p, uhp.toEuclidean(),1e-10, SM_SOURCE_FILE_POS, "Checking that I can recover the Euclidean point after normalization"); 60 | sm::eigen::assertNear(U, uhp.U3(), 1e-10, SM_SOURCE_FILE_POS, "Checking that I can recover the Euclideam point uncertainty after normalization."); 61 | } 62 | 63 | TEST(UncertainHomogeneousPointTestSuite, testU3normalizedTwice) 64 | { 65 | using namespace sm::kinematics; 66 | Eigen::Vector3d p; 67 | p.setRandom(); 68 | Eigen::Matrix3d U; 69 | U = sm::eigen::randomCovariance<3>(); 70 | 71 | UncertainHomogeneousPoint uhp(p,U); 72 | 73 | uhp.normalize(); 74 | uhp.normalize(); 75 | 76 | ASSERT_NEAR(uhp.toHomogeneous().norm(),1.0,1e-14); 77 | sm::eigen::assertNear(p, uhp.toEuclidean(),1e-8, SM_SOURCE_FILE_POS, "Checking that I can recover the Euclidean point after normalization"); 78 | sm::eigen::assertNear(U, uhp.U3(), 1e-8, SM_SOURCE_FILE_POS, "Checking that I can recover the Euclideam point uncertainty after normalization."); 79 | } 80 | -------------------------------------------------------------------------------- /sm_kinematics/test/homogeneous_coordinates.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | 6 | template 7 | struct NormalizeJacobianFunctor 8 | { 9 | typedef Eigen::Matrix value_t; 10 | typedef double scalar_t; 11 | typedef value_t input_t; 12 | typedef Eigen::Matrix jacobian_t; 13 | 14 | input_t update(input_t pp, int dimension, double dpd) 15 | { 16 | pp[dimension] += dpd; 17 | return pp; 18 | } 19 | 20 | value_t operator()(const input_t & p) 21 | { 22 | return sm::kinematics::normalize(p); 23 | } 24 | 25 | }; 26 | 27 | TEST(HomogeneousCoordinatesTestSuite, testNormalize) 28 | { 29 | 30 | for(int i = 0; i < 10; i++) 31 | { 32 | NormalizeJacobianFunctor<3> nj3; 33 | sm::eigen::NumericalDiff< NormalizeJacobianFunctor<3> > nd3(nj3); 34 | Eigen::Vector3d p3; 35 | p3.setRandom(); 36 | Eigen::Matrix3d J3est = nd3.estimateJacobian(p3); 37 | Eigen::Matrix3d J3; 38 | sm::kinematics::normalizeAndJacobian(p3,J3); 39 | sm::eigen::assertNear(J3,J3est,1e-6,SM_SOURCE_FILE_POS, "Checking the normalize jacobian"); 40 | } 41 | 42 | for(int i = 0; i < 10; i++) 43 | { 44 | NormalizeJacobianFunctor<4> nj4; 45 | sm::eigen::NumericalDiff< NormalizeJacobianFunctor<4> > nd4(nj4); 46 | Eigen::Vector4d p4; 47 | p4.setRandom(); 48 | Eigen::Matrix4d J4est = nd4.estimateJacobian(p4); 49 | Eigen::Matrix4d J4; 50 | sm::kinematics::normalizeAndJacobian(p4,J4); 51 | sm::eigen::assertNear(J4,J4est,1e-6,SM_SOURCE_FILE_POS, "Checking the normalize jacobian"); 52 | } 53 | 54 | 55 | } 56 | 57 | TEST(HomogeneousCoordinatesTestSuite, testPlus) 58 | { 59 | using namespace sm::kinematics; 60 | for(int i = 0; i < 10; ++i) 61 | { 62 | Eigen::Vector4d ph1, ph2; 63 | ph1.setRandom(); 64 | ph2.setRandom(); 65 | ph1.array() -= 0.5; 66 | ph2.array() -= 0.5; 67 | 68 | Eigen::Vector4d ph12 = toHomogeneousPlus(ph1) * ph2; 69 | 70 | Eigen::Vector3d p1, p2, p12; 71 | p1 = fromHomogeneous(ph1); 72 | p2 = fromHomogeneous(ph2); 73 | p12 = fromHomogeneous(ph12); 74 | 75 | sm::eigen::assertNear(p12,p1 + p2,1e-6,SM_SOURCE_FILE_POS, "the addition of homogeneous points by matrix multiplication"); 76 | } 77 | } 78 | 79 | 80 | TEST(HomogeneousCoordinatesTestSuite, testMinus) 81 | { 82 | using namespace sm::kinematics; 83 | for(int i = 0; i < 10; ++i) 84 | { 85 | Eigen::Vector4d ph1, ph2; 86 | ph1.setRandom(); 87 | ph2.setRandom(); 88 | ph1.array() -= 0.5; 89 | ph2.array() -= 0.5; 90 | 91 | Eigen::Vector4d ph12 = toHomogeneousMinus(ph1) * ph2; 92 | 93 | Eigen::Vector3d p1, p2, p12; 94 | p1 = fromHomogeneous(ph1); 95 | p2 = fromHomogeneous(ph2); 96 | p12 = fromHomogeneous(ph12); 97 | 98 | sm::eigen::assertNear(p12,p2 - p1,1e-6,SM_SOURCE_FILE_POS, "the subtraction of homogeneous points by matrix multiplication"); 99 | } 100 | } 101 | 102 | -------------------------------------------------------------------------------- /sm_kinematics/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /// Run all the tests that were declared with TEST() 4 | int main(int argc, char **argv){ 5 | testing::InitGoogleTest(&argc, argv); 6 | return RUN_ALL_TESTS(); 7 | } 8 | -------------------------------------------------------------------------------- /sm_logging/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sm_logging) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | find_package(Boost REQUIRED COMPONENTS system thread regex) 8 | include_directories(include ${Boost_INCLUDE_DIRS}) 9 | add_definitions("-std=c++0x") 10 | 11 | cs_add_library(${PROJECT_NAME} 12 | src/Logger.cpp 13 | src/StdOutLogger.cpp 14 | src/LoggingEvent.cpp 15 | src/LoggingGlobals.cpp 16 | src/Formatter.cpp 17 | src/Tokens.cpp 18 | src/Levels.cpp 19 | ) 20 | 21 | target_link_libraries(${PROJECT_NAME} 22 | ${Boost_LIBRARIES}) 23 | 24 | # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 25 | add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) 26 | 27 | catkin_add_gtest(${PROJECT_NAME}-test 28 | test/test_main.cpp 29 | test/logTest.cpp) 30 | if(TARGET ${PROJECT_NAME}-test) 31 | target_link_libraries(${PROJECT_NAME}-test 32 | ${PROJECT_NAME} 33 | pthread 34 | ) 35 | endif() 36 | 37 | cs_install() 38 | cs_export( 39 | LIBRARIES ${Boost_LIBRARIES} 40 | CFG_EXTRAS ${PROJECT_NAME}-extras.cmake 41 | ) 42 | -------------------------------------------------------------------------------- /sm_logging/cmake/sm_logging-extras.cmake.in: -------------------------------------------------------------------------------- 1 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") -------------------------------------------------------------------------------- /sm_logging/include/sm/logging.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_LOGGING_HPP 2 | #define SM_LOGGING_HPP 3 | 4 | #include 5 | 6 | 7 | namespace sm { 8 | namespace logging { 9 | 10 | class Logger; 11 | 12 | sm::logging::levels::Level getLevel(); 13 | void setLevel( sm::logging::levels::Level level ); 14 | void setLogger( boost::shared_ptr logger ); 15 | boost::shared_ptr getLogger(); 16 | bool isNamedStreamEnabled( const std::string & name ); 17 | void enableNamedStream( const std::string & name ); 18 | void disableNamedStream( const std::string & name ); 19 | 20 | 21 | 22 | } // namespace logging 23 | } // namespace sm 24 | 25 | #define SM_INIT_LOGGING() \ 26 | sm::logging::enableNamedStream( SMCONSOLE_NAME_PREFIX ); 27 | 28 | 29 | 30 | #endif /* SM_LOGGING_HPP */ 31 | -------------------------------------------------------------------------------- /sm_logging/include/sm/logging/Formatter.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_FORMATTER_HPP 2 | #define SM_FORMATTER_HPP 3 | 4 | #include "Tokens.hpp" 5 | #include "LoggingEvent.hpp" 6 | 7 | namespace sm { 8 | namespace logging { 9 | 10 | 11 | struct Formatter 12 | { 13 | 14 | Formatter(); 15 | virtual ~Formatter(); 16 | 17 | 18 | TokenPtr createTokenFromType(const std::string& type); 19 | void init(const char* fmt); 20 | void print(const ::sm::logging::LoggingEvent& event, std::ostream & ss); 21 | 22 | std::string format_; 23 | V_Token tokens_; 24 | bool doColor_; 25 | typedef std::map M_string; 26 | M_string extra_fixed_tokens_; 27 | 28 | 29 | }; 30 | 31 | 32 | } // namespace logging 33 | } // namespace sm 34 | 35 | 36 | #endif /* SM_FORMATTER_HPP */ 37 | -------------------------------------------------------------------------------- /sm_logging/include/sm/logging/Levels.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_LOGGING_LEVELS_HPP 2 | #define SM_LOGGING_LEVELS_HPP 3 | 4 | // These allow you to compile-out everything below a certain severity level if necessary 5 | #define SMCONSOLE_SEVERITY_ALL 0 6 | #define SMCONSOLE_SEVERITY_FINEST 1 7 | #define SMCONSOLE_SEVERITY_VERBOSE 2 8 | #define SMCONSOLE_SEVERITY_FINER 3 9 | #define SMCONSOLE_SEVERITY_TRACE 4 10 | #define SMCONSOLE_SEVERITY_FINE 5 11 | #define SMCONSOLE_SEVERITY_DEBUG 6 12 | #define SMCONSOLE_SEVERITY_INFO 7 13 | #define SMCONSOLE_SEVERITY_WARN 8 14 | #define SMCONSOLE_SEVERITY_ERROR 9 15 | #define SMCONSOLE_SEVERITY_FATAL 10 16 | #define SMCONSOLE_SEVERITY_NONE 11 17 | 18 | #include 19 | 20 | namespace sm { 21 | namespace logging { 22 | 23 | namespace levels 24 | { 25 | enum Level 26 | { 27 | All, 28 | Finest, 29 | Verbose, 30 | Finer, 31 | Trace, 32 | Fine, 33 | Debug, 34 | Info, 35 | Warn, 36 | Error, 37 | Fatal, 38 | 39 | Count 40 | }; 41 | 42 | Level fromString(std::string level); 43 | 44 | } 45 | 46 | typedef levels::Level Level; 47 | 48 | } // namespace logging 49 | } // namespace sm 50 | 51 | 52 | #endif /* SM_LOGGING_LEVELS_HPP */ 53 | -------------------------------------------------------------------------------- /sm_logging/include/sm/logging/LogLocation.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_LOGGING_LOG_LOCATION_HPP 2 | #define SM_LOGGING_LOG_LOCATION_HPP 3 | 4 | namespace sm { 5 | namespace logging { 6 | struct LogLocation 7 | { 8 | LogLocation() : _initialized(false), _loggerEnabled(false), 9 | _level(::sm::logging::levels::Count) {} 10 | 11 | bool _initialized; 12 | bool _loggerEnabled; 13 | ::sm::logging::Level _level; 14 | std::string _streamName; 15 | }; 16 | 17 | } // namespace logging 18 | } // namespace sm 19 | 20 | 21 | #endif /* SM_LOGGING_LOG_LOCATION_HPP */ 22 | -------------------------------------------------------------------------------- /sm_logging/include/sm/logging/Logger.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_LOGGER_HPP 2 | #define SM_LOGGER_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace sm { 8 | namespace logging { 9 | 10 | struct LoggingEvent; 11 | 12 | class Logger 13 | { 14 | public: 15 | typedef std::chrono::system_clock Clock; 16 | typedef Clock::time_point Time; 17 | typedef Clock::duration Duration; 18 | 19 | 20 | Logger(); 21 | virtual ~Logger(); 22 | 23 | double currentTimeSecondsUtc() const; 24 | std::string currentTimeString() const; 25 | Time currentTime() const; 26 | 27 | void log(const LoggingEvent & event); 28 | protected: 29 | virtual Time currentTimeImplementation() const; 30 | virtual void logImplementation(const LoggingEvent & event) = 0; 31 | }; 32 | 33 | } // namespace logging 34 | } // namespace sm 35 | 36 | 37 | #endif /* SM_LOGGER_HPP */ 38 | -------------------------------------------------------------------------------- /sm_logging/include/sm/logging/LoggingEvent.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_LOGGING_EVENT_HPP 2 | #define SM_LOGGING_EVENT_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | namespace sm { 10 | namespace logging { 11 | 12 | struct LoggingEvent 13 | { 14 | LoggingEvent(const char* streamName, 15 | Level level, 16 | const char* file, int line, 17 | const char* function, 18 | const char * message, 19 | const std::string & timestring) : 20 | streamName(streamName), 21 | level(level), 22 | file(file), 23 | line(line), 24 | function(function), 25 | message(message), 26 | timestring(timestring) 27 | {} 28 | 29 | const char * streamName; 30 | Level level; 31 | const char * file; 32 | int line; 33 | const char * function; 34 | const char * message; 35 | std::string timestring; 36 | }; 37 | 38 | 39 | } // namespace logging 40 | } // namespace sm 41 | 42 | 43 | #endif /* SM_LOGGING_EVENT_HPP */ 44 | -------------------------------------------------------------------------------- /sm_logging/include/sm/logging/StdOutLogger.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_LOGGING_STDOUT_LOGGER_HPP 2 | #define SM_LOGGING_STDOUT_LOGGER_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace sm { 8 | namespace logging { 9 | 10 | class StdOutLogger : public Logger 11 | { 12 | public: 13 | StdOutLogger(); 14 | ~StdOutLogger() override; 15 | 16 | Formatter formatter; 17 | protected: 18 | void logImplementation(const LoggingEvent & event) override; 19 | }; 20 | 21 | } // namespace logger 22 | } // namespace sm 23 | 24 | 25 | #endif /* SM_LOGGING_STDOUT_LOGGER_HPP */ 26 | -------------------------------------------------------------------------------- /sm_logging/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b sm_logging is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /sm_logging/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sm_logging 4 | 0.0.1 5 | sm_logging 6 | 7 | Paul Furgale 8 | New BSD 9 | catkin 10 | catkin_simple 11 | gtest 12 | 13 | -------------------------------------------------------------------------------- /sm_logging/src/Levels.cpp: -------------------------------------------------------------------------------- 1 | #include "sm/logging.hpp" 2 | 3 | #include 4 | #include // for case-insensitive string comparison 5 | #include 6 | 7 | /** 8 | * \brief Returns a Level enum from string 9 | * \param level level as string 10 | * \return logging level enum 11 | */ 12 | sm::logging::Level sm::logging::levels::fromString(std::string level) { 13 | 14 | std::transform(level.begin(), level.end(), level.begin(), ::tolower); 15 | 16 | if (level == "all") { 17 | return Level::All; 18 | } else if (level == "finest") { 19 | return Level::Finest; 20 | } else if (level == "verbose") { 21 | return Level::Verbose; 22 | } else if (level == "finer") { 23 | return Level::Finer; 24 | } else if (level == "trace") { 25 | return Level::Trace; 26 | } else if (level == "fine") { 27 | return Level::Fine; 28 | } else if (level == "debug") { 29 | return Level::Debug; 30 | } else if (level == "info") { 31 | return Level::Info; 32 | } else if (level == "warn") { 33 | return Level::Warn; 34 | } else if (level == "error") { 35 | return Level::Error; 36 | } else if (level == "fatal") { 37 | return Level::Fatal; 38 | } else { 39 | SM_ERROR_STREAM("Invalid logging level " << level << ", setting to Level::Info"); 40 | return Level::Info; 41 | } 42 | 43 | } 44 | -------------------------------------------------------------------------------- /sm_logging/src/Logger.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace sm { 5 | namespace logging { 6 | 7 | Logger::Logger(){} 8 | Logger::~Logger(){} 9 | 10 | 11 | double Logger::currentTimeSecondsUtc() const 12 | { 13 | Time t = currentTimeImplementation(); 14 | boost::int64_t us = std::chrono::duration_cast( t.time_since_epoch() ).count(); 15 | return (double)us/1000000.0; 16 | } 17 | 18 | std::string Logger::currentTimeString() const 19 | { 20 | double cts = currentTimeSecondsUtc(); 21 | std::stringstream ss; 22 | ss.fill(' '); 23 | ss.setf(std::ios::fixed,std::ios::floatfield); // floatfield set to fixed 24 | ss.precision(6); 25 | 26 | ss << cts; 27 | return ss.str(); 28 | 29 | } 30 | 31 | Logger::Time Logger::currentTime() const { 32 | return currentTimeImplementation(); 33 | } 34 | 35 | 36 | void Logger::log(const LoggingEvent & event) 37 | { 38 | logImplementation(event); 39 | } 40 | 41 | 42 | Logger::Time Logger::currentTimeImplementation() const 43 | { 44 | return Clock::now(); 45 | 46 | } 47 | 48 | 49 | 50 | } // namespace logging 51 | } // namespace sm 52 | -------------------------------------------------------------------------------- /sm_logging/src/LoggingEvent.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace sm { 4 | namespace logging { 5 | 6 | 7 | 8 | } // namespace logging 9 | } // namespace sm 10 | -------------------------------------------------------------------------------- /sm_logging/src/StdOutLogger.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #ifdef WIN32 5 | #include 6 | #endif 7 | #include 8 | #include 9 | #include 10 | 11 | namespace sm { 12 | namespace logging { 13 | 14 | StdOutLogger::StdOutLogger(){} 15 | StdOutLogger::~StdOutLogger(){} 16 | 17 | 18 | void StdOutLogger::logImplementation(const LoggingEvent & event) 19 | { 20 | formatter.print(event,std::cout); 21 | 22 | } 23 | 24 | 25 | } // namespace logging 26 | } // namespace sm 27 | -------------------------------------------------------------------------------- /sm_logging/src/Tokens.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace sm { 4 | namespace logging { 5 | 6 | 7 | 8 | } // namespace logging 9 | } // namespace sm 10 | -------------------------------------------------------------------------------- /sm_logging/src/gettimeofday.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _GETTIMEOFDAY_H_ 2 | #define _GETTIMEOFDAY_H_ 3 | 4 | #ifdef WIN32 5 | #include 6 | #include 7 | #else 8 | #include 9 | #endif 10 | 11 | // Code from: http://www.suacommunity.com/dictionary/gettimeofday-entry.php 12 | 13 | 14 | namespace sm { 15 | 16 | #ifdef WIN32 17 | #if defined(_MSC_VER) || defined(_MSC_EXTENSIONS) 18 | #define DELTA_EPOCH_IN_MICROSECS 11644473600000000Ui64 19 | #else 20 | #define DELTA_EPOCH_IN_MICROSECS 11644473600000000ULL 21 | #endif 22 | 23 | struct timezone 24 | { 25 | int tz_minuteswest; /* minutes W of Greenwich */ 26 | int tz_dsttime; /* type of dst correction */ 27 | }; 28 | 29 | // Definition of a gettimeofday function 30 | 31 | int gettimeofday(struct timeval *tv, struct timezone *tz) 32 | { 33 | // Define a structure to receive the current Windows filetime 34 | FILETIME ft; 35 | 36 | // Initialize the present time to 0 and the timezone to UTC 37 | unsigned __int64 tmpres = 0; 38 | static int tzflag = 0; 39 | 40 | if (NULL != tv) 41 | { 42 | GetSystemTimeAsFileTime(&ft); 43 | 44 | // The GetSystemTimeAsFileTime returns the number of 100 nanosecond 45 | // intervals since Jan 1, 1601 in a structure. Copy the high bits to 46 | // the 64 bit tmpres, shift it left by 32 then or in the low 32 bits. 47 | tmpres |= ft.dwHighDateTime; 48 | tmpres <<= 32; 49 | tmpres |= ft.dwLowDateTime; 50 | 51 | // Convert to microseconds by dividing by 10 52 | tmpres /= 10; 53 | 54 | // The Unix epoch starts on Jan 1 1970. Need to subtract the difference 55 | // in seconds from Jan 1 1601. 56 | tmpres -= DELTA_EPOCH_IN_MICROSECS; 57 | 58 | // Finally change microseconds to seconds and place in the seconds value. 59 | // The modulus picks up the microseconds. 60 | tv->tv_sec = (long)(tmpres / 1000000UL); 61 | tv->tv_usec = (long)(tmpres % 1000000UL); 62 | } 63 | 64 | if (NULL != tz) 65 | { 66 | if (!tzflag) 67 | { 68 | _tzset(); 69 | tzflag++; 70 | } 71 | 72 | // Adjust for the timezone west of Greenwich 73 | tz->tz_minuteswest = _timezone / 60; 74 | tz->tz_dsttime = _daylight; 75 | } 76 | 77 | return 0; 78 | } 79 | #else 80 | int gettimeofday(struct timeval *tv, struct timezone *tz) 81 | { 82 | return ::gettimeofday(tv,tz); 83 | } 84 | #endif // WIN32 85 | 86 | 87 | } // namespace sm 88 | 89 | 90 | #endif /* _GETTIMEOFDAY_H_ */ 91 | -------------------------------------------------------------------------------- /sm_logging/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /// Run all the tests that were declared with TEST() 4 | int main(int argc, char **argv){ 5 | testing::InitGoogleTest(&argc, argv); 6 | return RUN_ALL_TESTS(); 7 | } 8 | -------------------------------------------------------------------------------- /sm_matlab/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sm_matlab) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | find_package(Boost REQUIRED COMPONENTS system) 8 | include_directories(include ${Boost_INCLUDE_DIRS}) 9 | 10 | list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../cmake) 11 | find_package(Eigen REQUIRED) 12 | include_directories(${EIGEN_INCLUDE_DIRS}) 13 | 14 | find_package(Matlab) # Don't search with required, so we can continue. 15 | 16 | if(MATLAB_FOUND) 17 | cs_add_library(${PROJECT_NAME} 18 | src/Engine.cpp 19 | ) 20 | include_directories(${MATLAB_INCLUDE_DIR}) 21 | 22 | target_link_libraries(${PROJECT_NAME} ${MATLAB_LIBRARIES} 23 | ${Boost_LIBRARIES}) 24 | 25 | add_executable(sm_matlab_demo demo/demo.cpp) 26 | target_link_libraries(sm_matlab_demo 27 | ${PROJECT_NAME}) 28 | 29 | ############# 30 | ## Testing ## 31 | ############# 32 | # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 33 | add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) 34 | 35 | ## Add gtest based cpp test target and link libraries 36 | catkin_add_gtest(${PROJECT_NAME}-test 37 | test/test_main.cpp 38 | test/MatlabInterfaceTests.cpp 39 | ) 40 | if(TARGET ${PROJECT_NAME}-test) 41 | target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 42 | endif() 43 | else() 44 | message("Matlab not found, not building sm_matlab") 45 | endif() 46 | 47 | cs_install() 48 | cs_export() 49 | -------------------------------------------------------------------------------- /sm_matlab/Readme.txt: -------------------------------------------------------------------------------- 1 | C++ Matlab Interface (written by Michael Neunert, neunertm@gmail.com) 2 | ==================== 3 | 4 | This is a basic C++ Wrapper for Matlab. You can use it to send and read data/variables from Matlab and execute Matlab commands. It is less efficient than directly working with mxArrays but it provides a very intuitive and easy to use interface. 5 | 6 | 1. Installation 7 | 8 | 1a. Schweizer-Messer 9 | If you do not build it within the Schweizer-Messer written by Paul Furgale, you will have to remove SM specific types, manually compile it and then link against libeng and libmx, which should either be in your Matlab folder or common library directories. 10 | 11 | 1b. Packages 12 | Under linux you have to install csh. Under Ubuntu/Mint you can simply type 13 | > sudo apt-get install csh 14 | 15 | 16 | 2. Usage 17 | 18 | 2a. Initialization 19 | First you will have to create an object of the type sm::matlab::Engine, e.g.: 20 | > sm::matlab::Engine myMatlabEngine; 21 | Once the object is created, Matlab will launch in the background. Hence, this command might take some time. Even though run in the background, you have full access to almost all Matlab commands including opening windows e.g. for plotting. 22 | 23 | 2b. Execute a command or launch an m-file 24 | Just hit executeCommand("command"), e.g. 25 | > myMatlabEngine.executeCommand("plot([1 2 3]"); 26 | The return value is the Matlab output of the command 27 | 28 | 2c. Send data to Matlab 29 | You can simply send data to Matlab using put(variableName, value) (for standard types) or putEigen() (for Eigen Matrices and Vectors), e.g. 30 | > myMatlabEngine.put("a", 4); 31 | > Eigen::Vector2d B; 32 | > B << 2.0, 4.2; 33 | > myMatlabEngine.putEigen("B", B); 34 | Now, you can process the data in Matlab 35 | 36 | 2d. Get data from Matlab 37 | Essentially, it is the same as using the put() function: Just use get(variableName, value) or getEigen(variableName, value). Please note that Matlab normally stores all data types as doubles, so use doubles or Eigen double matrices/vectors as return containers, e.g.: 38 | > double a; 39 | > myMatlabEngine.get("a", a); 40 | > Eigen::Matrix4d C; 41 | > myMatlabEngine.getEigen("C", C); 42 | 43 | 2e. Interactive Console 44 | You can also open up an interactive console which almost behaves like the regular Matlab command line. Just enter 45 | > myMatlabEngine.openCommandLine(); 46 | To exit, type "quit" or "exit". This intentionally blocks the Matlab commands "quit" and "exit". In case you want to close Matlab early, use executeCommand(). However, calling the Engine class constructor is recommended as early exit might make the object unusable. 47 | Under Windows, you can also use setVisibility() to show/hide a Matlab console. Unfortunately, Matlab does not provide this functionality under Linux/MacOSX but openCommandLine() should be a good alternative. 48 | 49 | 2f. More 50 | For more options, take a look at the Engine.hpp header which shows all available functions. 51 | 52 | 53 | 3. Example 54 | 55 | For an example see demo.cpp 56 | 57 | 58 | ENJOY!! 59 | 60 | -------------------------------------------------------------------------------- /sm_matlab/demo/demo.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | // Bring in the Matlab Interface 5 | #include 6 | 7 | 8 | int main(int argc, char **argv) 9 | { 10 | // create a new engine 11 | sm::matlab::Engine engine; 12 | 13 | if (!engine.initialize()) 14 | { 15 | std::cout<<"Could not start Matlab"< 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /sm_matlab/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sm_matlab 4 | 0.0.1 5 | sm_matlab 6 | 7 | Paul Furgale 8 | New BSD 9 | catkin 10 | catkin_simple 11 | sm_common 12 | sm_random 13 | 14 | sm_common 15 | sm_random 16 | 17 | -------------------------------------------------------------------------------- /sm_matlab/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /// Run all the tests that were declared with TEST() 4 | int main(int argc, char **argv){ 5 | testing::InitGoogleTest(&argc, argv); 6 | return RUN_ALL_TESTS(); 7 | } 8 | -------------------------------------------------------------------------------- /sm_matrix_archive/.gitignore: -------------------------------------------------------------------------------- 1 | /bin 2 | -------------------------------------------------------------------------------- /sm_matrix_archive/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sm_matrix_archive) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | find_package(Boost REQUIRED COMPONENTS system filesystem) 8 | 9 | add_definitions(-std=c++0x -D__STRICT_ANSI__) 10 | 11 | include_directories(include ${Boost_INCLUDE_DIRS}) 12 | 13 | cs_add_library(${PROJECT_NAME} 14 | src/MatrixArchive.cpp 15 | ) 16 | 17 | target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) 18 | 19 | cs_add_executable(lsma 20 | src/lsma.cpp 21 | ) 22 | target_link_libraries(lsma ${PROJECT_NAME}) 23 | 24 | set_target_properties(lsma 25 | PROPERTIES 26 | RUNTIME_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/bin" 27 | ) 28 | install(TARGETS lsma 29 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/${CATKIN_GLOBAL_BIN_DESTINATION} 30 | ) 31 | 32 | # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 33 | add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) 34 | 35 | ## Add gtest based cpp test target and link libraries 36 | catkin_add_gtest(${PROJECT_NAME}-test 37 | test/test_main.cpp 38 | test/TestMatrixArchive.cpp 39 | ) 40 | if(TARGET ${PROJECT_NAME}-test) 41 | target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 42 | endif() 43 | 44 | cs_install() 45 | cs_export() 46 | -------------------------------------------------------------------------------- /sm_matrix_archive/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b sm_matrix_archive is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /sm_matrix_archive/matlab/loadMatrixArchive.m: -------------------------------------------------------------------------------- 1 | function [ ama ] = loadMatrixArchive( filename ) 2 | %LOADMATRIXARCHIVE Loads an matrix archive 3 | % 4 | % Input: 5 | % Filename - the filename of the matrix archive. 6 | % 7 | % Output: 8 | % ma - a struct with fields named according to the names loaded from the 9 | % matrix archive 10 | % 11 | 12 | startMagicMatrix = 'A'; 13 | startMagicString = 'S'; 14 | endMagic = 'B'; 15 | nameFixedSize = 32; 16 | 17 | [fid, message] = fopen(filename,'r','ieee-le'); 18 | if fid < 0 19 | error('unable to open file %s for reading: %s',filename, message); 20 | end 21 | 22 | try 23 | 24 | ama = struct(); 25 | 26 | 27 | % Read the start magic character 28 | start = fread(fid,1,'uint8=>char'); 29 | while ~feof(fid) 30 | if start ~= startMagicString && start ~= startMagicMatrix 31 | error('The start of a matrix block did not have the expected character. Wanted %s or %s, got %s', startMagicMatrix, startMagicString, start); 32 | end 33 | 34 | % Read the name 35 | name = strtrim(fread(fid,nameFixedSize,'uint8=>char')'); 36 | if isempty(name) 37 | error('Failed to read the matrix name'); 38 | end 39 | 40 | if(start == startMagicString) 41 | % Read the data size 42 | mxSize = fread(fid,1,'uint32'); 43 | 44 | % Read the data 45 | M = fread(fid, [1, mxSize(1)],'uint8=>char'); 46 | else 47 | % Read the data size 48 | mxSize = fread(fid,2,'uint32'); 49 | 50 | % Read the data 51 | M = fread(fid,mxSize(1)*mxSize(2),'double'); 52 | M = reshape(M,mxSize(1),mxSize(2)); 53 | end 54 | 55 | % Read the end magic character 56 | endchar = fread(fid,1,'uint8=>char'); 57 | if endchar ~= endMagic 58 | error('The end of a matrix block for matrix named %s did not have the expected character. Wanted %s, got %s', name, endMagic, endchar); 59 | end 60 | 61 | % Set the field on the return struct 62 | ama.(name) = M; 63 | 64 | % Reprime the loop (this will cause feof to evaluate to true if the 65 | % file has been finished) 66 | % Read the start magic character 67 | start = fread(fid,1,'uint8=>char'); 68 | 69 | end 70 | 71 | fclose(fid); 72 | catch ME 73 | fclose(fid); 74 | rethrow(ME); 75 | end 76 | 77 | 78 | end 79 | 80 | -------------------------------------------------------------------------------- /sm_matrix_archive/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sm_matrix_archive 4 | 0.0.1 5 | sm_matrix_archive 6 | 7 | Paul Furgale 8 | New BSD 9 | catkin 10 | catkin_simple 11 | 12 | eigen_catkin 13 | sm_common 14 | 15 | eigen_catkin 16 | sm_common 17 | 18 | -------------------------------------------------------------------------------- /sm_matrix_archive/src/lsma.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | void usage(const char * cmd) { 5 | std::cerr << "USAGE: " << cmd << " " << std::endl; 6 | } 7 | 8 | int main(int argc, char **argv) { 9 | if(argc < 2){ 10 | usage(argv[0]); 11 | return -1; 12 | } 13 | using sm::MatrixArchive; 14 | for(int i = 1; i < argc; i++){ 15 | const std::string path = std::string(argv[i]); 16 | MatrixArchive ma; 17 | ma.load(path); 18 | const std::string namePrefix = (argc > 2) ? path + ":" : std::string(); 19 | for (auto & s : ma.getStrings()){ 20 | std::cout << namePrefix << s.first << " : " << s.second << std::endl; 21 | } 22 | for (auto & m : ma){ 23 | std::cout << namePrefix << m.first << " :\n" << m.second << std::endl; 24 | } 25 | } 26 | return 0; 27 | } 28 | -------------------------------------------------------------------------------- /sm_matrix_archive/test/TestMatrixArchive.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * TestCpp.cpp 3 | * 4 | * Created on: May 17, 2013 5 | * Author: hannes 6 | */ 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | TEST(MatrixArchive, testMatrixLoadAndSaveWorkTogether) { 13 | try { 14 | SCOPED_TRACE(""); 15 | Eigen::MatrixXd testMat = Eigen::MatrixXd::Random(1, 1); 16 | std::string testString = "testString"; 17 | sm::MatrixArchive archive; 18 | std::string tempfile("/tmp/testMatrixArchive.ama"); 19 | 20 | archive.setMatrix("m", testMat); 21 | archive.setString("s", testString); 22 | 23 | archive.save(tempfile); 24 | 25 | archive.clear(); 26 | ASSERT_EQ(0, archive.size()); 27 | 28 | archive.load(tempfile); 29 | 30 | ASSERT_EQ(testString, archive.getString("s")); 31 | auto & resultMat = archive.getMatrix("m"); 32 | 33 | ASSERT_EQ(testMat.rows(), resultMat.rows()); 34 | ASSERT_EQ(testMat.cols(), resultMat.cols()); 35 | ASSERT_EQ(testMat(0, 0), resultMat(0,0)); 36 | 37 | unlink(tempfile.c_str()); 38 | } catch (const std::exception & e) { 39 | FAIL()<< e.what(); 40 | } 41 | } 42 | -------------------------------------------------------------------------------- /sm_matrix_archive/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /// Run all the tests that were declared with TEST() 4 | int main(int argc, char **argv){ 5 | testing::InitGoogleTest(&argc, argv); 6 | return RUN_ALL_TESTS(); 7 | } 8 | -------------------------------------------------------------------------------- /sm_opencv/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sm_opencv) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | find_package(Boost REQUIRED COMPONENTS system) 8 | 9 | add_definitions(-std=c++0x -D__STRICT_ANSI__) 10 | 11 | include_directories(${Boost_INCLUDE_DIRS}) 12 | 13 | cs_install() 14 | cs_export() 15 | -------------------------------------------------------------------------------- /sm_opencv/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b sm_opencv is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /sm_opencv/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sm_opencv 4 | 0.0.1 5 | sm_opencv 6 | 7 | Paul Furgale 8 | New BSD 9 | catkin 10 | catkin_simple 11 | sm_common 12 | 13 | sm_common 14 | 15 | -------------------------------------------------------------------------------- /sm_property_tree/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sm_property_tree) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | find_package(Boost REQUIRED COMPONENTS system filesystem) 8 | 9 | add_definitions(-D__STRICT_ANSI__) 10 | remove_definitions(-std=c++0x -std-c++11) 11 | 12 | include_directories(include ${Boost_INCLUDE_DIRS}) 13 | 14 | ############## 15 | ## Building ## 16 | ############## 17 | 18 | cs_add_library(${PROJECT_NAME} 19 | src/PropertyTree.cpp 20 | src/BoostPropertyTreeImplementation.cpp 21 | src/BoostPropertyTree.cpp 22 | src/PropertyTreeImplementation.cpp 23 | src/BoostPropertyTreeSupport.cpp 24 | src/BoostPropertyTreeLoader.cpp 25 | ) 26 | 27 | target_link_libraries(${PROJECT_NAME} 28 | ${Boost_LIBRARIES}) 29 | 30 | cs_install() 31 | cs_export(LIBRARIES ${Boost_LIBRARIES}) 32 | 33 | ############# 34 | ## Testing ## 35 | ############# 36 | 37 | if(CATKIN_ENABLE_TESTING) 38 | catkin_add_gtest(${PROJECT_NAME}-test 39 | test/test_main.cpp 40 | test/BoostPropertyTreeImplementation.cpp 41 | ) 42 | if(TARGET ${PROJECT_NAME}-test) 43 | target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 44 | endif() 45 | endif() 46 | 47 | -------------------------------------------------------------------------------- /sm_property_tree/include/sm/BoostPropertyTreeLoader.hpp: -------------------------------------------------------------------------------- 1 | #ifndef H050B8D6A_0312_469D_90C1_CC9482370CBF 2 | #define H050B8D6A_0312_469D_90C1_CC9482370CBF 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace sm { 9 | 10 | class BoostPropertyTreeLoader { 11 | public: 12 | sm::BoostPropertyTree readFilesAndMergeIntoPropertyTree(const std::string& commaSeparatedFileList); 13 | sm::BoostPropertyTree readFilesAndMergeIntoPropertyTree(const std::vector& files); 14 | 15 | std::string resolveFullFilePath(const std::string& path, const std::string& relativeToFile); 16 | 17 | std::vector & getSearchPaths() { 18 | return searchPaths; 19 | } 20 | 21 | std::vector & getFileNameInfixes() { 22 | return fileNameInfixes; 23 | } 24 | 25 | virtual ~BoostPropertyTreeLoader(){} 26 | 27 | protected: 28 | virtual bool fileExists(const std::string& path) const; 29 | private: 30 | virtual void goingToMergeIn(const std::string & updatePath); 31 | virtual void startingWith(const std::string & path); 32 | virtual void warn(const std::string & message); 33 | virtual void info(const std::string & message); 34 | virtual void verbose(const std::string & message); 35 | 36 | sm::BoostPropertyTree readFiles(const std::vector& configFiles, std::set& ignore); 37 | 38 | std::vector searchPaths; 39 | std::vector fileNameInfixes; 40 | friend class UpdateablePropertyTree; 41 | }; 42 | 43 | } // namespace sm 44 | 45 | #endif /* H050B8D6A_0312_469D_90C1_CC9482370CBF */ 46 | -------------------------------------------------------------------------------- /sm_property_tree/include/sm/BoostPropertyTreeSupport.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_BOOST_PROPERTY_TREE_SUPPORT_HPP 2 | #define SM_BOOST_PROPERTY_TREE_SUPPORT_HPP 3 | 4 | #include 5 | 6 | namespace sm { 7 | 8 | boost::filesystem::path findFile(const std::string& filenameToFind, const std::string& envVarNameContainingSearchDir); 9 | 10 | } 11 | 12 | #endif /* SM_BOOST_PROPERTY_TREE_SUPPORT_HPP */ 13 | -------------------------------------------------------------------------------- /sm_property_tree/include/sm/PropertyTreeImplementation.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_PROPERTY_TREE_IMPLEMENTATION_HPP 2 | #define SM_PROPERTY_TREE_IMPLEMENTATION_HPP 3 | 4 | #include "PropertyTree.hpp" 5 | #include 6 | 7 | namespace sm { 8 | 9 | class PropertyTreeImplementation 10 | { 11 | public: 12 | PropertyTreeImplementation(); 13 | virtual ~PropertyTreeImplementation(); 14 | 15 | virtual double getDouble(const std::string & key) const = 0; 16 | virtual double getDouble(const std::string & key, double defaultValue) const = 0; 17 | // The non-const version will call "set" if the value is not already set. 18 | virtual double getDouble(const std::string & key, double defaultValue) = 0; 19 | 20 | virtual int getInt(const std::string & key) const = 0; 21 | virtual int getInt(const std::string & key, int defaultValue) const = 0; 22 | // The non-const version will call "set" if the value is not already set. 23 | virtual int getInt(const std::string & key, int defaultValue) = 0; 24 | 25 | virtual bool getBool(const std::string & key) const = 0; 26 | virtual bool getBool(const std::string & key, bool defaultValue) const = 0; 27 | // The non-const version will call setBool if the value is not already set. 28 | virtual bool getBool(const std::string & key, bool defaultValue) = 0; 29 | 30 | virtual std::string getString(const std::string & key) const = 0; 31 | virtual std::string getString(const std::string & key, const std::string & defaultValue) const = 0; 32 | // The non-const version will call setBool if the value is not already set. 33 | virtual std::string getString(const std::string & key, const std::string & defaultValue) = 0; 34 | 35 | virtual void setDouble(const std::string & key, double value) = 0; 36 | virtual void setInt(const std::string & key, int value) = 0; 37 | virtual void setBool(const std::string & key, bool value) = 0; 38 | virtual void setString(const std::string & key, const std::string & value) = 0; 39 | 40 | virtual bool doesKeyExist(const std::string & key) const = 0; 41 | 42 | virtual std::vector getChildren(const std::string & key) const = 0; 43 | }; 44 | 45 | } // namespace sm 46 | 47 | 48 | #endif /* SM_PROPERTY_TREE_IMPLEMENTATION_HPP */ 49 | -------------------------------------------------------------------------------- /sm_property_tree/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b sm_property_tree is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /sm_property_tree/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sm_property_tree 4 | 0.0.1 5 | sm_property_tree 6 | 7 | Paul Furgale 8 | New BSD 9 | catkin 10 | catkin_simple 11 | sm_common 12 | 13 | sm_common 14 | 15 | -------------------------------------------------------------------------------- /sm_property_tree/src/BoostPropertyTreeSupport.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace sm { 9 | 10 | /** 11 | * @brief Helper to find a file with name filenameToFind in the directory specified by an environment variable envVarNameContainingSearchDir 12 | **/ 13 | boost::filesystem::path findFile(const std::string& filenameToFind, const std::string& envVarNameContainingSearchDir) { 14 | 15 | boost::filesystem::path fileFound; 16 | char* dir = std::getenv(envVarNameContainingSearchDir.c_str()); 17 | SM_ASSERT_TRUE( std::runtime_error, dir, "Environment variable " << envVarNameContainingSearchDir << " could not be found"); 18 | 19 | std::string searchDirectory = std::string(dir); 20 | searchDirectory = sm::ensureTrailingBackslash(searchDirectory); 21 | 22 | std::string fullPath = searchDirectory + filenameToFind; 23 | SM_ASSERT_TRUE( std::runtime_error, boost::filesystem::exists(fullPath), 24 | "Environment variable " << envVarNameContainingSearchDir << " exists, but file " << filenameToFind << " could not be found in " << searchDirectory); 25 | 26 | fileFound = boost::filesystem::path(fullPath.c_str()); 27 | return fileFound; 28 | } 29 | 30 | } /* namespace sm */ 31 | -------------------------------------------------------------------------------- /sm_property_tree/src/PropertyTreeImplementation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace sm { 4 | 5 | PropertyTreeImplementation::PropertyTreeImplementation() 6 | { 7 | 8 | } 9 | PropertyTreeImplementation::~PropertyTreeImplementation() 10 | { 11 | 12 | } 13 | 14 | 15 | } // namespace sm 16 | -------------------------------------------------------------------------------- /sm_property_tree/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /// Run all the tests that were declared with TEST() 4 | int main(int argc, char **argv){ 5 | testing::InitGoogleTest(&argc, argv); 6 | return RUN_ALL_TESTS(); 7 | } 8 | -------------------------------------------------------------------------------- /sm_python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sm_python) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | add_definitions(-std=c++0x -D__STRICT_ANSI__) 8 | 9 | # Set up the python exports. 10 | SET(PY_PROJECT_NAME sm_python) 11 | SET(PY_PACKAGE_DIR python/sm) 12 | 13 | add_python_export_library(${PY_PROJECT_NAME} ${PY_PACKAGE_DIR} 14 | src/module.cpp 15 | src/export_rotational_kinematics.cpp 16 | src/export_rotations.cpp 17 | src/export_transformations.cpp 18 | src/export_quaternion_algebra.cpp 19 | src/export_homogeneous.cpp 20 | src/exportTransformation.cpp 21 | src/exportHomogeneousPoint.cpp 22 | src/exportTimestampCorrector.cpp 23 | src/exportPropertyTree.cpp 24 | src/exportEigen.cpp 25 | src/exportUncertainVector.cpp 26 | src/exportMatrixArchive.cpp 27 | src/export_kinematics_property_tree.cpp 28 | src/export_eigen_property_tree.cpp 29 | src/Logging.cpp 30 | src/Timing.cpp 31 | src/exportNsecTime.cpp 32 | src/random.cpp 33 | src/exportValueStoreRef.cpp 34 | ) 35 | 36 | cs_install() 37 | cs_export() 38 | 39 | catkin_add_nosetests( 40 | test/Test.py 41 | DEPENDENCIES ${PROJECT_NAME} 42 | ) 43 | -------------------------------------------------------------------------------- /sm_python/include/sm/python/Id.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_PYTHON_ID_HPP 2 | #define SM_PYTHON_ID_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace sm { namespace python { 10 | 11 | // to use: sm::python::Id_python_converter::register_converter(); 12 | 13 | 14 | // adapted from http://misspent.wordpress.com/2009/09/27/how-to-write-boost-python-converters/ 15 | 16 | template 17 | struct Id_python_converter 18 | { 19 | typedef ID_T id_t; 20 | 21 | // The "Convert from C to Python" API 22 | static PyObject * convert(id_t const & id){ 23 | PyObject * i = PyInt_FromLong(id.getId()); 24 | // It seems that the call to "incref(.)" caused a memory leak! 25 | // I will check this in hoping it doesn't cause any instability. 26 | return i;//boost::python::incref(i); 27 | } 28 | 29 | // The "Convert from Python to C" API 30 | // Two functions: convertible() and construct() 31 | static void * convertible(PyObject* obj_ptr) 32 | { 33 | if (!(PyInt_Check(obj_ptr) || PyLong_Check(obj_ptr))) 34 | return 0; 35 | 36 | return obj_ptr; 37 | } 38 | 39 | static void construct( 40 | PyObject* obj_ptr, 41 | boost::python::converter::rvalue_from_python_stage1_data* data) 42 | { 43 | 44 | // Get the value. 45 | boost::uint64_t value; 46 | if ( PyInt_Check(obj_ptr) ) { 47 | value = PyInt_AsUnsignedLongLongMask(obj_ptr); 48 | } else { 49 | value = PyLong_AsUnsignedLongLong(obj_ptr); 50 | } 51 | 52 | void* storage = ((boost::python::converter::rvalue_from_python_storage*) 53 | data)->storage.bytes; 54 | 55 | 56 | new (storage) id_t(value); 57 | 58 | // Stash the memory chunk pointer for later use by boost.python 59 | data->convertible = storage; 60 | } 61 | 62 | 63 | 64 | 65 | // The registration function. 66 | static void register_converter() 67 | { 68 | 69 | // This code checks if the type has already been registered. 70 | // http://stackoverflow.com/questions/9888289/checking-whether-a-converter-has-already-been-registered 71 | boost::python::type_info info = boost::python::type_id(); 72 | const boost::python::converter::registration* reg = boost::python::converter::registry::query(info); 73 | if (reg == NULL || reg->m_to_python == NULL) { 74 | // This is the code to register the type. 75 | boost::python::to_python_converter(); 76 | boost::python::converter::registry::push_back( 77 | &convertible, 78 | &construct, 79 | boost::python::type_id()); 80 | 81 | } 82 | 83 | 84 | } 85 | 86 | }; 87 | 88 | }} 89 | 90 | #endif /* SM_PYTHON_ID_HPP */ 91 | -------------------------------------------------------------------------------- /sm_python/include/sm/python/boost_serialization_pickle.hpp: -------------------------------------------------------------------------------- 1 | /// 2 | /// @file boost_serialization_pickle.hpp 3 | /// @author Paul Furgale 4 | /// @date Tue Jul 23 21:22:15 2013 5 | /// 6 | /// @brief A class to leverage boost::serialization for Python pickle support 7 | /// 8 | /// 9 | /// 10 | 11 | #ifndef SM_BOOST_SERIALIZATION_PICKLE_HPP 12 | #define SM_BOOST_SERIALIZATION_PICKLE_HPP 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | 21 | namespace sm { 22 | namespace python { 23 | 24 | 25 | /// 26 | /// \class pickle_suite 27 | /// 28 | /// \brief A class to leverage boost::serialization for Python pickle support 29 | /// 30 | /// This class provides pickle serialization support for classes that have: 31 | /// (1) a default constructor that takes no arguments, and 32 | /// (2) boost serialization implemented. 33 | /// 34 | /// To include this support in your class, simply add a single line to 35 | /// the list of your classes member functions when exporting the 36 | /// boost::python wrapper: 37 | /// 38 | /// class_("MyClass", init<>()) 39 | /// // other .def(...) statements 40 | /// .def_pickle( sm::python::pickle_suite< MyClass >() ) 41 | /// ; 42 | /// 43 | /// 44 | /// Then, in Python, you should be able to pickle the object. See the very 45 | /// simple example here: http://wiki.python.org/moin/UsingPickle 46 | /// 47 | template 48 | struct pickle_suite : boost::python::pickle_suite 49 | { 50 | /// \brief This class assumes the object has a default constructor that takes 51 | /// no arguments. 52 | /// 53 | /// From the documentation: 54 | /// http://www.boost.org/doc/libs/1_35_0/libs/python/doc/v2/pickle.html 55 | /// 56 | /// "When an instance of a Boost.Python 57 | /// extension class is pickled, the pickler tests if the instance 58 | /// has a __getinitargs__ method. This method must return a Python 59 | /// tuple (it is most convenient to use a 60 | /// boost::python::tuple). When the instance is restored by the 61 | /// unpickler, the contents of this tuple are used as the 62 | /// arguments for the class constructor." 63 | // 64 | static 65 | boost::python::tuple 66 | getinitargs(const T & /* val */) 67 | { 68 | return boost::python::tuple(); 69 | } 70 | 71 | static 72 | boost::python::tuple 73 | getstate(const T & val) 74 | { 75 | // Serialize to a string stream then return the string. 76 | std::ostringstream out; 77 | ::boost::archive::text_oarchive oa(out); 78 | oa << val; 79 | return boost::python::make_tuple( out.str() ); 80 | } 81 | 82 | static 83 | void 84 | setstate(T & val, boost::python::tuple state) 85 | { 86 | boost::python::extract str(state[0]); 87 | SM_ASSERT_TRUE(std::runtime_error, str.check(), "Error during de-serialization. Unable to convert the tuple object to a string"); 88 | std::istringstream in(str()); 89 | ::boost::archive::text_iarchive ia(in); 90 | ia >> val; 91 | } 92 | }; 93 | 94 | 95 | } // namespace python 96 | } // namespace sm 97 | 98 | 99 | 100 | #endif /* SM_BOOST_SERIALIZATION_PICKLE_HPP */ 101 | -------------------------------------------------------------------------------- /sm_python/include/sm/python/stl_converters.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_PYTHON_STL_CONVERTERS_HPP 2 | #define SM_PYTHON_STL_CONVERTERS_HPP 3 | 4 | #include 5 | 6 | namespace sm { 7 | namespace python { 8 | 9 | /// \brief Convert an STL list to a boost::python list 10 | template 11 | inline void stlToList(const LIST_ITERATOR_T & begin, const LIST_ITERATOR_T & end, boost::python::list & outlist) 12 | { 13 | LIST_ITERATOR_T it = begin; 14 | 15 | for( ; it != end; ++it) 16 | { 17 | outlist.append(*it); 18 | } 19 | 20 | 21 | } 22 | 23 | template 24 | inline void stlToDict(const MAP_ITERATOR_T & begin, const MAP_ITERATOR_T & end, boost::python::dict & out) 25 | { 26 | MAP_ITERATOR_T it = begin; 27 | for( ; it != end; ++it) 28 | { 29 | out[it->first] = it->second; 30 | } 31 | } 32 | 33 | } // namespace python 34 | } // namespace sm 35 | 36 | #endif // SM_PYTHON_STL_CONVERTERS_HPP 37 | -------------------------------------------------------------------------------- /sm_python/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b sm_python is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /sm_python/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sm_python 4 | 0.0.1 5 | sm_python 6 | 7 | Paul Furgale 8 | New BSD 9 | catkin 10 | catkin_simple 11 | 12 | sm_common 13 | numpy_eigen 14 | catkin_boost_python_buildtool 15 | sm_kinematics 16 | sm_timing 17 | sm_logging 18 | sm_matrix_archive 19 | sm_property_tree 20 | sm_value_store 21 | 22 | python-tk 23 | sm_common 24 | numpy_eigen 25 | catkin_boost_python_buildtool 26 | sm_kinematics 27 | sm_timing 28 | sm_logging 29 | sm_matrix_archive 30 | sm_property_tree 31 | python3-matplotlib 32 | 33 | -------------------------------------------------------------------------------- /sm_python/python/sm/PlotCollection.py: -------------------------------------------------------------------------------- 1 | import wxversion 2 | wxversion.ensureMinimal('2.8') 3 | 4 | import wx 5 | import wx.aui 6 | import matplotlib as mpl 7 | from matplotlib.backends.backend_wxagg import FigureCanvasWxAgg as Canvas 8 | from matplotlib.backends.backend_wxagg import NavigationToolbar2Wx as Toolbar 9 | import collections 10 | 11 | class PlotCollection: 12 | def __init__(self, window_name="", window_size=(800,600)): 13 | """ 14 | This class places matplot figures in tabs on a wx window 15 | (make sure to use unique figure ids between different PlotCollection instances 16 | or wxwidget may segfault) 17 | e.g. usage: 18 | from sm import PlotCollection 19 | import pylab as pl 20 | 21 | #create the plot as usual 22 | fig1=pl.figure() 23 | pl.plot([1,2],[2,3]) 24 | fig2=pl.figure() 25 | pl.plot([3,1],[4,5]) 26 | 27 | #add to collection 28 | plotter = PlotCollection.PlotCollection("My window name") 29 | plotter.add_figure("My plot1 name", fig1) 30 | plotter.add_figure("My plot2 name", fig2) 31 | 32 | #show collection 33 | plotter.show() 34 | """ 35 | self.frame_name = window_name 36 | self.window_size = window_size 37 | self.figureList = collections.OrderedDict() 38 | 39 | def add_figure(self, tabname, fig): 40 | """ 41 | Add a matplotlib figure to the collection 42 | """ 43 | self.figureList[tabname] = fig 44 | 45 | def delete_figure(self, name): 46 | """ 47 | Delete a figure from the collection given the tab name. 48 | """ 49 | self.figureList.pop(name, None) 50 | 51 | def show(self): 52 | """ 53 | Show the window on screen 54 | """ 55 | if len(self.figureList.keys()) == 0: 56 | return 57 | app = wx.PySimpleApp() 58 | frame = wx.Frame(None,-1,self.frame_name, size=self.window_size) 59 | plotter = self.PlotNotebook(frame) 60 | for name in self.figureList.keys(): 61 | plotter.add(name, self.figureList[name]) 62 | frame.Show() 63 | app.MainLoop() 64 | 65 | class Plot(wx.Panel): 66 | def __init__(self, parent, fig, id = -1, dpi = None, **kwargs): 67 | wx.Panel.__init__(self, parent, id=id, **kwargs) 68 | fig.set_figheight(2) 69 | fig.set_figwidth(2) 70 | self.canvas = Canvas(self, -1, fig) 71 | self.toolbar = Toolbar(self.canvas) 72 | self.toolbar.Realize() 73 | 74 | sizer = wx.BoxSizer(wx.VERTICAL) 75 | sizer.Add(self.canvas,1,wx.EXPAND) 76 | sizer.Add(self.toolbar, 0 , wx.LEFT | wx.EXPAND) 77 | self.SetSizer(sizer) 78 | 79 | class PlotNotebook(wx.Panel): 80 | def __init__(self, parent, id = -1): 81 | wx.Panel.__init__(self, parent, id=id) 82 | self.nb = wx.aui.AuiNotebook(self) 83 | sizer = wx.BoxSizer() 84 | sizer.Add(self.nb, 1, wx.EXPAND) 85 | self.SetSizer(sizer) 86 | 87 | def add(self,name, fig): 88 | page = PlotCollection.Plot(self.nb, fig) 89 | self.nb.AddPage(page,name) 90 | -------------------------------------------------------------------------------- /sm_python/python/sm/Progress.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | import time 4 | import datetime 5 | import sys 6 | 7 | class Progress(object): 8 | def __init__(self, numIterations): 9 | self.started = False 10 | self.elapsed = 0 11 | self.startTime = 0 12 | self.numIterations = numIterations 13 | self.iteration = 0 14 | 15 | def sample(self): 16 | if self.started: 17 | self.iteration = self.iteration + 1 18 | self.elapsed = time.time() - self.startTime 19 | timePerRun = self.elapsed / self.iteration 20 | totalTime = self.numIterations * timePerRun 21 | 22 | print("Progress %d / %d" % (self.iteration, self.numIterations)) 23 | print("Time %s / %s (%s * %d) " % (datetime.timedelta(seconds=self.elapsed), datetime.timedelta(seconds=totalTime), datetime.timedelta(seconds=timePerRun), self.numIterations)) 24 | 25 | else: 26 | self.startTime = time.time() 27 | self.iteration = 0 28 | self.started = True 29 | 30 | 31 | class Progress2(object): 32 | def __init__(self, numIterations): 33 | """ 34 | Progress tracker that calculates and prints the time until a process is finished. 35 | 36 | example usage: 37 | import sm 38 | import time 39 | 40 | numIter = 10 41 | progress = sm.Progress2(numIter) 42 | for iter in range(0, numIter): 43 | progress.sample() 44 | time.sleep(1) 45 | """ 46 | self.started = False 47 | self.elapsed = 0 48 | self.startTime = 0 49 | self.numIterations = numIterations 50 | self.iteration = 0 51 | 52 | def sample(self, steps=1): 53 | """ 54 | Call this function at each iteration. It prints the remaining steps and time. 55 | """ 56 | if self.started: 57 | self.iteration = self.iteration + steps 58 | self.elapsed = time.time() - self.startTime 59 | timePerRun = self.elapsed / self.iteration 60 | totalTime = self.numIterations * timePerRun 61 | 62 | m, s = divmod(totalTime-self.elapsed, 60) 63 | h, m = divmod(m, 60) 64 | t_remaining_str = "" 65 | if h > 0: t_remaining_str = "%d h " % h 66 | if m > 0: t_remaining_str = t_remaining_str + "%dm " % m 67 | if s > 0: t_remaining_str = t_remaining_str + "%ds" % s 68 | print("\r Progress {0} / {1} \t Time remaining: {2} ".format(self.iteration, self.numIterations, t_remaining_str), 69 | sys.stdout.flush()) 70 | else: 71 | self.startTime = time.time() 72 | self.iteration = 0 73 | self.started = True 74 | 75 | def reset(self, numIterations = -1): 76 | """ 77 | Reset the progress tracker 78 | """ 79 | self.started = False 80 | self.elapsed = 0 81 | self.startTime = 0 82 | self.iteration = 0 83 | if numIterations != -1: 84 | self.numIterations = numIterations -------------------------------------------------------------------------------- /sm_python/python/sm/experiments/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/schweizer_messer/cb9d52841cebd965bb201ba83031e830980d8016/sm_python/python/sm/experiments/__init__.py -------------------------------------------------------------------------------- /sm_python/python/sm/saveFigTight.py: -------------------------------------------------------------------------------- 1 | 2 | def saveFigTight(fig, filename): 3 | ax = fig.gca() 4 | extent = ax.get_window_extent().transformed(fig.dpi_scale_trans.inverted()) 5 | fig.savefig(filename, bbox_inches=extent) 6 | -------------------------------------------------------------------------------- /sm_python/setup.py: -------------------------------------------------------------------------------- 1 | 2 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 3 | 4 | from distutils.core import setup 5 | from catkin_pkg.python_setup import generate_distutils_setup 6 | 7 | # fetch values from package.xml 8 | setup_args = generate_distutils_setup( 9 | packages=['sm'], 10 | package_dir={'':'python'}) 11 | 12 | setup(**setup_args) 13 | -------------------------------------------------------------------------------- /sm_python/src/Logging.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | 6 | void smLogNamed(const std::string & name, sm::logging::Level level, 7 | const std::string & file, int line, const std::string & function, 8 | const std::string & message) 9 | { 10 | 11 | if (!sm::logging::isNamedStreamEnabled(name)) return; 12 | 13 | boost::shared_ptr logger = sm::logging::getLogger(); 14 | sm::logging::LoggingEvent event(name.c_str(), 15 | level, 16 | file.c_str(), 17 | line, 18 | function.c_str(), 19 | message.c_str(), 20 | logger->currentTimeString()); 21 | 22 | logger->log(event); 23 | 24 | } 25 | 26 | void smLog(sm::logging::Level level, const std::string & file, int line, const std::string & function, 27 | const std::string & message) 28 | { 29 | smLogNamed(SMCONSOLE_DEFAULT_NAME,level,file,line,function,message); 30 | 31 | } 32 | 33 | void exportLogging() 34 | { 35 | 36 | using namespace boost::python; 37 | using namespace sm::logging; 38 | 39 | enum_("LoggingLevel") 40 | .value("All",levels::All) 41 | .value("Finest",levels::Finest) 42 | .value("Verbose",levels::Verbose) 43 | .value("Finer",levels::Finer) 44 | .value("Trace",levels::Trace) 45 | .value("Fine",levels::Fine) 46 | .value("Debug",levels::Debug) 47 | .value("Info",levels::Info) 48 | .value("Warn",levels::Warn) 49 | .value("Error",levels::Error) 50 | .value("Fatal",levels::Fatal) 51 | ; 52 | 53 | // sm::logging::levels::Level getLevel(); 54 | def("getLoggingLevel",&getLevel); 55 | // void setLevel( sm::logging::levels::Level level ); 56 | def("setLoggingLevel", &setLevel); 57 | def("loggingLevelFromString", &levels::fromString); 58 | // void setLogger( boost::shared_ptr logger ); 59 | def("setLogger", &setLogger); 60 | // boost::shared_ptr getLogger();z 61 | def("getLogger", &getLogger); 62 | // bool isNamedStreamEnabled( const std::string & name ); 63 | def("isNamedLoggingStreamEnabled", &isNamedStreamEnabled); 64 | // void enableNamedStream( const std::string & name ); 65 | def("enableNamedLoggingStream", &enableNamedStream); 66 | // void disableNamedStream( const std::string & name ); 67 | def("disableNamedLoggingStream", &disableNamedStream); 68 | def("rawLog",&smLog,"log(level,file,line,function,message)"); 69 | def("rawLogNamed", &smLogNamed, "logNamed(level, file, line, function, message)"); 70 | 71 | 72 | 73 | // Logging Event 74 | 75 | // Log 76 | 77 | } 78 | -------------------------------------------------------------------------------- /sm_python/src/Timing.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Timing.cpp 3 | * 4 | * Created on: 18.12.2015 5 | * Author: Ulrich Schwesinger 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | void printTiming() 12 | { 13 | sm::timing::Timing::print(std::cout); 14 | } 15 | 16 | void printTimingSorted(const sm::timing::SortType sortType) 17 | { 18 | sm::timing::Timing::print(std::cout, sortType); 19 | } 20 | 21 | void exportTiming() 22 | { 23 | using namespace boost::python; 24 | using namespace sm::timing; 25 | 26 | enum_("TimingSortType") 27 | .value("SORT_BY_TOTAL", SortType::SORT_BY_TOTAL) 28 | .value("SORT_BY_MEAN", SortType::SORT_BY_MEAN) 29 | .value("SORT_BY_STD", SortType::SORT_BY_STD) 30 | .value("SORT_BY_MIN", SortType::SORT_BY_MIN) 31 | .value("SORT_BY_MAX", SortType::SORT_BY_MAX) 32 | .value("SORT_BY_NUM_SAMPLES", SortType::SORT_BY_NUM_SAMPLES) 33 | ; 34 | 35 | def("printTiming", &printTiming); 36 | def("printTimingSorted", &printTimingSorted); 37 | } 38 | -------------------------------------------------------------------------------- /sm_python/src/exportEigen.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | 6 | Eigen::MatrixXd matrixSqrt(const Eigen::MatrixXd & M) 7 | { 8 | SM_ASSERT_EQ(std::runtime_error, M.rows(), M.cols(), "The matrix must be square"); 9 | Eigen::MatrixXd sqrtM; 10 | 11 | /*Eigen::ComputationInfo result =*/ sm::eigen::computeMatrixSqrt(M, sqrtM); 12 | //SM_ASSERT_EQ(std::runtime_error, result, Eigen::Success, "The matrix square root was not successful") 13 | return sqrtM; 14 | } 15 | 16 | void exportEigen() 17 | { 18 | boost::python::def("computeMatrixSqrt", &matrixSqrt); 19 | } 20 | -------------------------------------------------------------------------------- /sm_python/src/exportHomogeneousPoint.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace boost::python; 6 | using namespace sm::kinematics; 7 | 8 | void exportHomogeneousPoint() 9 | { 10 | 11 | class_ >("HomogeneousPoint", init<>()) 12 | .def(init()) 13 | .def(init()) 14 | .def("toEuclidean",&HomogeneousPoint::toEuclidean) 15 | // Eigen::Vector3d toEuclideanAndJacobian(euclidean_jacobian_t & J) const; 16 | .def("toHomogeneous",&HomogeneousPoint::toHomogeneous, return_value_policy()) 17 | // const Eigen::Vector4d & toHomogeneousAndJacobian(homogeneous_jacobian_t & J) const; 18 | .def(self + self) 19 | .def(self + UncertainHomogeneousPoint()) 20 | .def(self - self) 21 | .def(self - UncertainHomogeneousPoint()) 22 | .def("setRandom", &HomogeneousPoint::setRandom) 23 | .def("setZero", &HomogeneousPoint::setZero) 24 | .def("normalize", &HomogeneousPoint::normalize) 25 | ; 26 | 27 | class_, bases >("UncertainHomogeneousPoint", init<>()) 28 | .def(init()) 29 | .def(init()) 30 | .def(init()) 31 | .def(init()) 32 | .def(init()) 33 | .def(init()) 34 | .def(init()) 35 | .def(init()) 36 | .def("setRandom", &UncertainHomogeneousPoint::setRandom) 37 | .def(self + HomogeneousPoint()) 38 | .def(self + UncertainHomogeneousPoint()) 39 | .def(self - HomogeneousPoint()) 40 | .def(self - UncertainHomogeneousPoint()) 41 | .def("U4", &UncertainHomogeneousPoint::U4, return_value_policy()) 42 | .def("U3", &UncertainHomogeneousPoint::U3) 43 | .def("U_av_form", &UncertainHomogeneousPoint::U_av_form) 44 | .def("setU", &UncertainHomogeneousPoint::setU) 45 | .def("setUOplus", &UncertainHomogeneousPoint::setUOplus) 46 | .def("normalize", &UncertainHomogeneousPoint::normalize) 47 | ; 48 | 49 | } 50 | -------------------------------------------------------------------------------- /sm_python/src/exportNsecTime.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | void exportNsecTime() { 6 | using namespace boost::python; 7 | using namespace sm::timing; 8 | 9 | def("nsecNow", &nsecNow); 10 | /// \brief Get the epoch time as nanoseconds since the epoch. 11 | ///NsecTime nsecNow(); 12 | 13 | /// \brief Convert the time (in integer nanoseconds) to decimal seconds. 14 | ///double nsecToSec( const NsecTime & time ); 15 | def("nsecToSec", &nsecToSec); 16 | 17 | /// \brief Convert the time (in seconds) to integer nanoseconds 18 | ///NsecTime secToNsec( const double & time ); 19 | def("secToNsec", &secToNsec); 20 | } 21 | -------------------------------------------------------------------------------- /sm_python/src/exportTimestampCorrector.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | template 6 | void exportTimestampCorrector(const std::string & className) 7 | { 8 | using namespace boost::python; 9 | using namespace sm::timing; 10 | 11 | typedef typename TimestampCorrector::time_t time_t; 12 | 13 | class_< TimestampCorrector >( className.c_str(), init<>() ) 14 | .def("correctTimestamp", (time_t (TimestampCorrector::*) (const time_t&, const time_t&))&TimestampCorrector::correctTimestamp, "correctedEventLocalTime = correctTimestamp(eventRemoteTime, eventLocalTimes).\nNote: This function must be called with monotonically increasing remote timestamps.") 15 | .def("correctTimestampPeriodic", (time_t (TimestampCorrector::*) (const time_t&, const time_t&, const time_t&))&TimestampCorrector::correctTimestamp, "correctedEventLocalTime = correctTimestamp(eventRemoteTime, eventLocalTimes, switchingPeriod).\nNote: This function must be called with monotonically increasing remote timestamps.") 16 | .def("getLocalTime", &TimestampCorrector::getLocalTime, "eventLocalTime = getLocalTime(eventRemoteTime)") 17 | .def("convexHullSize", &TimestampCorrector::convexHullSize) 18 | .def("printHullPoints", &TimestampCorrector::printHullPoints) 19 | .def("getSlope", &TimestampCorrector::getSlope) 20 | .def("getOffset", &TimestampCorrector::getOffset) 21 | ; 22 | 23 | } 24 | 25 | 26 | void exportTimestampCorrectors() 27 | { 28 | exportTimestampCorrector("DoubleTimestampCorrector"); 29 | exportTimestampCorrector("LongTimestampCorrector"); 30 | } 31 | -------------------------------------------------------------------------------- /sm_python/src/export_eigen_property_tree.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | void export_eigen_property_tree() 5 | { 6 | boost::python::def("vector3FromPropertyTree", &sm::eigen::vector3FromPropertyTree); 7 | boost::python::def("quaternionFromPropertyTree", &sm::eigen::quaternionFromPropertyTree); 8 | } 9 | -------------------------------------------------------------------------------- /sm_python/src/export_homogeneous.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | using namespace boost::python; 6 | using namespace sm::kinematics; 7 | 8 | Eigen::Vector4d toHomogeneousWrap(const Eigen::Vector3d & v) 9 | { 10 | return sm::kinematics::toHomogeneous(v); 11 | } 12 | 13 | 14 | Eigen::Vector3d fromHomogeneousWrap(const Eigen::Vector4d & v) 15 | { 16 | return sm::kinematics::fromHomogeneous(v); 17 | } 18 | 19 | void export_homogeneous_coordinates() 20 | { 21 | 22 | def("toHomogeneousJacobian",&toHomogeneousJacobian); 23 | //Eigen::Matrix toHomogeneousJacobian(const Eigen::Vector3d & v); 24 | //Eigen::Vector4d toHomogeneous(const Eigen::Vector3d & v, Eigen::Matrix * jacobian = NULL); 25 | def("toHomogeneous",&toHomogeneousWrap); 26 | //Eigen::Matrix fromHomogeneousJacobian(const Eigen::Vector4d & v); 27 | def("fromHomogeneousJacobian", &fromHomogeneousJacobian); 28 | //Eigen::Vector3d fromHomogeneous(const Eigen::Vector4d & v, Eigen::Matrix * jacobian = NULL); 29 | def("fromHomogeneous", &fromHomogeneousWrap); 30 | //Eigen::MatrixXd toHomogeneousColumns(const Eigen::MatrixXd & M); 31 | def("toHomogeneousColumns", &toHomogeneousColumns); 32 | //Eigen::MatrixXd fromHomogeneousColumns(const Eigen::MatrixXd & M); 33 | def("fromHomogeneousColumns", &fromHomogeneousColumns); 34 | } 35 | -------------------------------------------------------------------------------- /sm_python/src/export_kinematics_property_tree.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | void export_kinematics_property_tree() 5 | { 6 | boost::python::def("transformationFromPropertyTree", &sm::kinematics::transformationFromPropertyTree); 7 | } 8 | -------------------------------------------------------------------------------- /sm_python/src/export_quaternion_algebra.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | using namespace boost::python; 6 | using namespace sm::kinematics; 7 | 8 | void export_quaternion_algebra() 9 | { 10 | using namespace boost::python; 11 | using namespace sm; 12 | // Eigen::Matrix3d quat2r(Eigen::Vector4d const & q); 13 | def("quat2r",quat2r,"Build a unit-length quaternion from a rotation matrix"); 14 | // Eigen::Vector4d r2quat(Eigen::Matrix3d const & C); 15 | def("r2quat",r2quat,"Build a rotation matrix from unit-length quaternion"); 16 | // Eigen::Vector4d axisAngle2quat(Eigen::Vector3d const & a); 17 | def("axisAngle2quat",axisAngle2quat, "Build a quaternion from a axis/angle representation (the input is the unit-length axis times the angle of rotation about that axis)"); 18 | // Eigen::Vector3d quat2AxisAngle(Eigen::Vector4d const & q); 19 | def("quat2AxisAngle",quat2AxisAngle, "Build an axis angle from a quaternion. The output is the unit-length axis times the angle of rotation about that axis"); 20 | // Eigen::Matrix4d quatPlus(Eigen::Vector4d const & q); 21 | def("quatPlus",quatPlus, "Build a q-plus matrix"); 22 | // Eigen::Matrix4d quatOPlus(Eigen::Vector4d const & q); 23 | def("quatOPlus",quatOPlus, "Build a q-oplus matrix"); 24 | // Eigen::Vector4d quatInv(Eigen::Vector4d const & q); 25 | def("quatInv",quatInv, "Return the inverse of the quaternion input"); 26 | // void invertQuat(Eigen::Vector4d & q); 27 | // Eigen::Vector3d qeps(Eigen::Vector4d const & q); 28 | def("qeps",qeps, "Returns the vector part of the quaternion"); 29 | // double qeta(Eigen::Vector4d const & q); 30 | def("qeta",qeta, "Returns the scalar part of the quaternion"); 31 | // // For estimation functions to handle a constraint-sensitive minimal parameterization for a quaternion update 32 | // Eigen::Matrix quatJacobian(Eigen::Vector4d const & q); 33 | def("quatJacobian",quatJacobian); 34 | // Eigen::Vector4d updateQuat(Eigen::Vector4d const & q, Eigen::Vector3d const & dq); 35 | def("updateQuat",updateQuat); 36 | def("quatIdentity",&quatIdentity, "Return the identity quaternion"); 37 | def("quatRandom",&quatRandom, "Return a random quaternion"); 38 | def("quatS",&quatS); 39 | def("quatInvS",&quatInvS); 40 | // Eigen::Vector3d quatRotate(Eigen::Vector4d const & q_a_b, Eigen::Vector3d const & v_b); 41 | def("quatRotate", &quatRotate); 42 | // Eigen::Vector4d qoplus(Eigen::Vector4d const & q, Eigen::Vector4d const & p); 43 | def("qoplus", &qoplus); 44 | // Eigen::Vector4d qplus(Eigen::Vector4d const & q, Eigen::Vector4d const & p); 45 | def("qplus", &qplus); 46 | def("qlog", &qlog); 47 | def("qexp", &qexp); 48 | def("qslerp", &qslerp); 49 | def("lerp", &lerp); 50 | ; 51 | } 52 | -------------------------------------------------------------------------------- /sm_python/src/export_rotational_kinematics.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | // A wrapper that gets rid of the second parameter in the parametersToRotationMatrix() function. 10 | template 11 | Eigen::Matrix3d parametersToRotationMatrixWrapper(const ROTATION_TYPE * rt, const Eigen::Vector3d & parameters) 12 | { 13 | return rt->parametersToRotationMatrix(parameters); 14 | } 15 | 16 | template 17 | boost::python::tuple angularVelocityAndJacobianWrapper(const ROTATION_TYPE * rt, const Eigen::Vector3d & p, const Eigen::Vector3d pdot) 18 | { 19 | Eigen::Matrix J; 20 | Eigen::Vector3d omega = rt->angularVelocityAndJacobian(p,pdot,&J); 21 | 22 | return boost::python::make_tuple(omega,J); 23 | } 24 | 25 | 26 | 27 | template 28 | class RotationalKinematicsPythonWrapper 29 | { 30 | public: 31 | typedef ROTATION_TYPE rotation_t; 32 | 33 | static void exportToPython(const std::string & className) 34 | { 35 | using namespace boost::python; 36 | 37 | class_ >(className.c_str()) 38 | .def("parametersToRotationMatrix",¶metersToRotationMatrixWrapper) 39 | .def("rotationMatrixToParameters",&rotation_t::rotationMatrixToParameters) 40 | .def("parametersToSMatrix",&rotation_t::parametersToSMatrix) 41 | .def("angularVelocityAndJacobian",&angularVelocityAndJacobianWrapper); 42 | } 43 | }; 44 | 45 | 46 | void import_rotational_kinematics_python() 47 | { 48 | using namespace boost::python; 49 | // The no_init gets us a pure virtual base class. 50 | class_("RotationalKinematics", no_init); 51 | 52 | RotationalKinematicsPythonWrapper::exportToPython("EulerAnglesZYX"); 53 | RotationalKinematicsPythonWrapper::exportToPython("EulerAnglesYawPitchRoll"); 54 | RotationalKinematicsPythonWrapper::exportToPython("RotationVector"); 55 | RotationalKinematicsPythonWrapper::exportToPython("EulerRodriguez"); 56 | } 57 | -------------------------------------------------------------------------------- /sm_python/src/export_rotations.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | using namespace boost::python; 6 | using namespace sm::kinematics; 7 | 8 | Eigen::Matrix3d crossMxWrap( const Eigen::Vector3d & v ) { 9 | return crossMx(v[0],v[1],v[2]); 10 | } 11 | 12 | 13 | void export_rotations() 14 | { 15 | using namespace boost::python; 16 | 17 | // Euler angle rotations. 18 | // Eigen::Matrix3d Rx(double radians); 19 | def("Rx", Rx); 20 | // Eigen::Matrix3d Ry(double radians); 21 | def("Ry", Ry); 22 | // Eigen::Matrix3d Rz(double radians); 23 | def("Rz",Rz); 24 | 25 | def("Cx", Cx); 26 | def("Cy", Cy); 27 | def("Cz", Cz); 28 | 29 | Eigen::Matrix3d (*rph2r1)(double, double, double) = &rph2R; 30 | // Eigen::Matrix3d rph2r(double x, double y, double z); 31 | def("rph2R",rph2r1); 32 | 33 | Eigen::Matrix3d (*rph2r2)(Eigen::Vector3d const &) = &rph2R; 34 | // Eigen::Matrix3d rph2r(Eigen::Vector3d const & x); 35 | def("rph2R",rph2r2); 36 | 37 | Eigen::Matrix3d (*rph2C1)(double , double , double ) = &rph2C; 38 | def("rph2C",rph2C1); 39 | 40 | Eigen::Matrix3d (*rph2C2)(Eigen::Vector3d const &) = &rph2C; 41 | def("rph2C",rph2C2); 42 | 43 | def("C2rph",C2rph); 44 | 45 | Eigen::Vector3d (*r2rph1)(Eigen::Matrix3d const &) = &R2rph; 46 | // Eigen::Vector3d r2rph(Eigen::Matrix3d const & C); 47 | def("R2rph",r2rph1); 48 | // Eigen::Matrix3d rph_S(Eigen::Vector3d const & rph); 49 | 50 | 51 | // // Small angle approximation. 52 | // Eigen::Matrix3d crossMx(double x, double y, double z); 53 | Eigen::Matrix3d (*crossMx1)(double , double , double ) = &crossMx; 54 | def("crossMx",crossMx1); 55 | 56 | def("crossMx",&crossMxWrap); 57 | 58 | 59 | 60 | 61 | // Eigen::Matrix3d crossMx(Eigen::Vector3d const & x); 62 | //Eigen::Matrix3d (*crossMx2)(Eigen::Vector3d const & ) = &crossMx; 63 | //def("crossMx",crossMx2); 64 | // // Axis Angle rotation. 65 | 66 | // Eigen::Matrix3d axisAngle2r(double x, double y, double z); 67 | Eigen::Matrix3d (*axisAngle2r1)(double , double , double ) = &axisAngle2R; 68 | def("axisAngle2R",axisAngle2r1); 69 | // Eigen::Matrix3d axisAngle2r(Eigen::Vector3d const & x); 70 | Eigen::Matrix3d (*axisAngle2r2)(Eigen::Vector3d const & ) = &axisAngle2R; 71 | def("axisAngle2R",axisAngle2r2); 72 | 73 | // Eigen::Vector3d r2AxisAngle(Eigen::Matrix3d const & C); 74 | Eigen::Vector3d (*r2AxisAngle1)(Eigen::Matrix3d const & ) = &R2AxisAngle; 75 | def("R2AxisAngle",r2AxisAngle1); 76 | // // quaternion rotation 77 | // vector4 r2quat(Eigen::Matrix3d R); 78 | //def("r2quat",r2quat); 79 | // Eigen::Matrix3d quat2r(vector4 q); 80 | //def("quat2r",quat2r); 81 | // // Utility functions 82 | // // Moves a value in radians to within -pi, pi 83 | // double angleMod(double radians); 84 | def("angleMod",angleMod); 85 | // double deg2rad(double degrees); 86 | def("deg2rad",deg2rad); 87 | // double rad2deg(double radians); 88 | def("rad2deg",rad2deg); 89 | } 90 | 91 | -------------------------------------------------------------------------------- /sm_python/src/export_transformations.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | using namespace boost::python; 6 | using namespace sm::kinematics; 7 | 8 | tuple transformationAndJacobianTuple(Eigen::Matrix4d const & T_a_b, 9 | Eigen::Vector4d const & v_b) 10 | { 11 | Eigen::Vector4d out_v_a; 12 | Eigen::Matrix out_B; 13 | transformationAndJacobian(T_a_b, v_b, out_v_a, out_B); 14 | return make_tuple(out_v_a, out_B); 15 | 16 | } 17 | 18 | tuple inverseTransformationAndJacobianTuple(Eigen::Matrix4d const & T_ba, 19 | Eigen::Vector4d const & v_b) 20 | { 21 | Eigen::Vector4d out_v_a; 22 | Eigen::Matrix out_B; 23 | inverseTransformationAndJacobian(T_ba, v_b, out_v_a, out_B); 24 | 25 | return make_tuple(out_v_a, out_B); 26 | } 27 | 28 | 29 | 30 | void export_transformations() 31 | { 32 | using namespace boost::python; 33 | using namespace sm; 34 | 35 | 36 | def("rt2Transform", rt2Transform, "creates a transformation matrix T_ba from the rotation matrix C_ba and the translation rho_b_ab"); 37 | def("transform2C", transform2C, "returns the rotation matrix C_ba from the transformation matrix T_ba"); 38 | def("transform2rho", transform2rho, "returns the translation p_b_ab from the transformation matrix T_ba"); 39 | def("transform2rhoHomogeneous", transform2rhoHomogeneous, "returns the translation p_b_ab from the transformation matrix T_ba"); 40 | def("boxPlus", boxPlus, "Builds the 4x4 box plus matrix from a 6x1 perturbation"); 41 | def("boxMinus", boxMinus, "Builds the 6x6 box minus matrix from a 4x1 homogeneous point"); 42 | def("boxTimes", boxTimes, "Builds the 6x6 box times matrix from a 4x4 transformation matrix"); 43 | def("inverseTransform",inverseTransform, "Invert a 4x4 transformation matrix"); 44 | 45 | Eigen::Matrix4d (*toTEuler1)(double, double, double, double, double, double) = &toTEuler; 46 | def("toTEuler",toTEuler1,"Create a 4x4 transformation matrix from 6 parameters"); 47 | 48 | Eigen::Matrix4d (*toTEuler2)(Eigen::Matrix const &) = &toTEuler; 49 | def("toTEuler",toTEuler2,"Create a 4x4 transformation matrix from a 6x1 column of parameters"); 50 | def("fromTEuler",fromTEuler,"Create 6x1 column of parameters from a 4x4 transformation matrix"); 51 | def("transformationAndJacobian",transformationAndJacobianTuple, "Transform a point and return the point and jacobian with respect to local perturbations of the transformation matrix. (v_a, H) = transformationAndJacobian(T_ab, v_b)"); 52 | def("inverseTransformationAndJacobian",inverseTransformationAndJacobianTuple, "Transform a point (using the inverse of the supplied transformation matrix) and return the point and jacobian with respect to local perturbations of the transformation matrix. (v_b, H) = inverseTransformationAndJacobian(T_ab, v_a)"); 53 | } 54 | -------------------------------------------------------------------------------- /sm_python/src/module.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | using namespace boost::python; 4 | 5 | //typedef UniformCubicBSpline UniformCubicBSplineX; 6 | 7 | 8 | void import_rotational_kinematics_python(); 9 | void export_rotations(); 10 | void export_transformations(); 11 | void export_quaternion_algebra(); 12 | void export_homogeneous_coordinates(); 13 | void exportTransformation(); 14 | void exportHomogeneousPoint(); 15 | void exportTimestampCorrectors(); 16 | void exportPropertyTree(); 17 | void exportPropertyTreeLoader(); 18 | void exportEigen(); 19 | void exportUncertainVector(); 20 | void exportMatrixArchive(); 21 | void exportLogging(); 22 | void exportTiming(); 23 | void exportNsecTime(); 24 | void exportRandom(); 25 | void export_eigen_property_tree(); 26 | void export_kinematics_property_tree(); 27 | void exportValueStoreRef(); 28 | void exportKeyValueStorePair(); 29 | void exportExtendibleValueStoreRef(); 30 | void exportExtendibleKeyValueStorePair(); 31 | void exportValueStore(); 32 | void exportLayeredValueStore(); 33 | void exportPrefixedValueStore(); 34 | 35 | BOOST_PYTHON_MODULE(libsm_python) 36 | { 37 | import_rotational_kinematics_python(); 38 | export_rotations(); 39 | export_transformations(); 40 | export_quaternion_algebra(); 41 | export_homogeneous_coordinates(); 42 | exportTransformation(); 43 | exportHomogeneousPoint(); 44 | exportTimestampCorrectors(); 45 | exportPropertyTree(); 46 | exportPropertyTreeLoader(); 47 | exportEigen(); 48 | exportUncertainVector(); 49 | exportMatrixArchive(); 50 | exportLogging(); 51 | exportTiming(); 52 | exportNsecTime(); 53 | exportRandom(); 54 | export_eigen_property_tree(); 55 | export_kinematics_property_tree(); 56 | exportValueStoreRef(); 57 | exportKeyValueStorePair(); 58 | exportExtendibleValueStoreRef(); 59 | exportExtendibleKeyValueStorePair(); 60 | exportValueStore(); 61 | exportLayeredValueStore(); 62 | exportPrefixedValueStore(); 63 | } 64 | -------------------------------------------------------------------------------- /sm_python/src/random.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | void exportRandom() { 6 | using namespace boost::python; 7 | using namespace sm::random; 8 | 9 | /// \brief Return a sample from a normally distributed random distribution with zero mean and standard deviation 1. 10 | def("normal",&normal); 11 | /// \brief Return a sample from a normally distributed random distribution with zero mean and standard deviation 1. 12 | def("randn",&randn); 13 | 14 | /// \brief Return a sample from a uniform distribution between 0.0 and 1.0 15 | def("uniform",&uniform); 16 | 17 | /// \brief Return a sample from a uniform distribution between 0.0 and 1.0 18 | def("rand",&sm::random::rand); 19 | 20 | /// \brief Return a sample from a uniform distribution in the half-open range [lowerBound, upperBound) 21 | def("randLU",&randLU); 22 | 23 | /// \brief Return a sample from a uniform distribution in the half-open range [lowerBound, upperBound) 24 | def("randLUi",&randLUi); 25 | 26 | /// \brief Seed the random number generator. 27 | def("seed",&seed); 28 | 29 | 30 | } 31 | -------------------------------------------------------------------------------- /sm_python/test/Test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import os 3 | import sm 4 | import numpy as np 5 | 6 | import unittest 7 | 8 | from nose import SkipTest 9 | 10 | 11 | class TestMatrixArchive(unittest.TestCase): 12 | def test_saveLoad(self): 13 | if sm.numpy_eigen.IsBroken: 14 | self.skipTest("numpy_eigen is broken (see #137)") 15 | ma = sm.MatrixArchive() 16 | ma.setMatrix("testM", np.array([[0, 0], [1, 1], [2, 2]])) 17 | testSValue = "testStringValue" 18 | ma.setString("testS", testSValue) 19 | self.assertTrue("testM" in ma.getNameList()) 20 | self.assertEqual(ma.getMatrix("testM").size, 6) 21 | self.assertEqual(ma.getString("testS"), testSValue) 22 | 23 | testAma = "TestMatrixArchive-test.ama" 24 | ma.save(testAma) 25 | 26 | ma = sm.MatrixArchive() 27 | self.assertFalse("testM" in ma.getNameList()) 28 | ma.load(testAma) 29 | self.assertTrue("testM" in ma.getNameList()) 30 | self.assertEqual(ma.getMatrix("testM").size, 6) 31 | self.assertEqual(ma.getString("testS"), testSValue) 32 | os.unlink(testAma) 33 | 34 | 35 | class TestNsecTime(unittest.TestCase): 36 | def test_secToNsec(self): 37 | self.assertEqual(sm.secToNsec(1), 1.0e9) 38 | 39 | 40 | class TestPropertyTree(unittest.TestCase): 41 | def test_boostpropertyTree(self): 42 | bpt = sm.BoostPropertyTree() 43 | bpt.setDouble("bla", 0.3) 44 | self.assertEqual(bpt.getDouble("bla"), 0.3) 45 | 46 | 47 | class TestValueStore(unittest.TestCase): 48 | def test_fromString(self): 49 | vs = sm.ValueStoreRef.fromString("bla 3.0, a test") 50 | self.assertEqual(vs.getDouble("bla"), 3.0) 51 | self.assertEqual(vs.getString("a"), "test") 52 | 53 | 54 | if __name__ == '__main__': 55 | unittest.main() 56 | -------------------------------------------------------------------------------- /sm_random/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sm_random) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | cs_add_library(${PROJECT_NAME} 8 | src/random.cpp 9 | ) 10 | 11 | cs_install() 12 | cs_export() -------------------------------------------------------------------------------- /sm_random/include/sm/random.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SM_RANDOM_HPP 2 | #define SM_RANDOM_HPP 3 | 4 | #include 5 | 6 | namespace sm { 7 | namespace random { 8 | 9 | 10 | /// \brief Return a sample from a normally distributed random distribution with zero mean and standard deviation 1. 11 | double normal(); 12 | /// \brief Return a sample from a normally distributed random distribution with zero mean and standard deviation 1. 13 | double randn(); 14 | 15 | /// \brief Return a sample from a uniform distribution between 0.0 and 1.0 16 | double uniform(); 17 | 18 | /// \brief Return a sample from a uniform distribution between 0.0 and 1.0 19 | double rand(); 20 | 21 | /// \brief Return a sample from a uniform distribution in the half-open range [lowerBound, upperBound) 22 | double randLU(double lowerBoundInclusive, double upperBoundExclusive); 23 | 24 | /// \brief Return a sample from a uniform distribution in the half-open range [lowerBound, upperBound) 25 | int randLUi(int lowerBoundInclusive, int upperBoundExclusive); 26 | 27 | /// \brief Seed the random number generator. 28 | void seed(boost::uint64_t s); 29 | 30 | } // namespace random 31 | } // namespace sm 32 | 33 | 34 | #endif /* SM_RANDOM_HPP */ 35 | -------------------------------------------------------------------------------- /sm_random/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b sm_random is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /sm_random/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sm_random 4 | 0.0.1 5 | sm_random 6 | 7 | Paul Furgale 8 | New BSD 9 | catkin 10 | catkin_simple 11 | gtest 12 | 13 | -------------------------------------------------------------------------------- /sm_random/src/random.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | namespace sm { 11 | 12 | namespace random { 13 | 14 | class Random 15 | { 16 | public: 17 | // http://www.boost.org/doc/libs/1_49_0/doc/html/boost_random/reference.html#boost_random.reference.generators 18 | typedef boost::mt19937 base_generator_type; 19 | 20 | base_generator_type _generator; 21 | // Define a uniform random number distribution which produces "double" 22 | // values between 0 and 1 (0 inclusive, 1 exclusive). 23 | boost::uniform_real<> _uni_dist; 24 | boost::variate_generator > _uniform; 25 | boost::normal_distribution<> _normal_dist; 26 | boost::variate_generator > _normal; 27 | 28 | Random() : _uni_dist(0.0,1.0), _uniform(_generator, _uni_dist), _normal_dist(), _normal(_generator, _normal_dist) 29 | { 30 | // 0 31 | } 32 | 33 | double normal() 34 | { 35 | return _normal(); 36 | } 37 | 38 | double uniform() 39 | { 40 | return _uniform(); 41 | } 42 | 43 | void seed(boost::uint64_t s) 44 | { 45 | _normal.engine().seed(s); 46 | _normal.distribution().reset(); 47 | _uniform.engine().seed(s); 48 | _uniform.distribution().reset(); 49 | } 50 | 51 | static Random & instance() 52 | { 53 | static Random random; 54 | return random; 55 | } 56 | }; 57 | 58 | double normal() 59 | { 60 | return Random::instance().normal(); 61 | } 62 | 63 | double randn() 64 | { 65 | return Random::instance().normal(); 66 | } 67 | 68 | double uniform() 69 | { 70 | return Random::instance().uniform(); 71 | } 72 | 73 | double rand() 74 | { 75 | return Random::instance().uniform(); 76 | } 77 | 78 | double randLU(double lowerBoundInclusive, double upperBoundExclusive) 79 | { 80 | return Random::instance().uniform() * (upperBoundExclusive - lowerBoundInclusive) + lowerBoundInclusive; 81 | } 82 | 83 | int randLUi(int lowerBoundInclusive, int upperBoundExclusive) 84 | { 85 | return (int)floor(Random::instance().uniform() * (upperBoundExclusive - lowerBoundInclusive)) + lowerBoundInclusive; 86 | } 87 | 88 | void seed(boost::uint64_t s) 89 | { 90 | Random::instance().seed(s); 91 | } 92 | 93 | 94 | } // namespace random 95 | 96 | } // namespace sm 97 | -------------------------------------------------------------------------------- /sm_timing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sm_timing) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | find_package(Boost REQUIRED COMPONENTS system) 8 | 9 | add_definitions(-std=c++0x -D__STRICT_ANSI__) 10 | 11 | include_directories(${Boost_INCLUDE_DIRS}) 12 | 13 | ############## 14 | ## Building ## 15 | ############## 16 | 17 | cs_add_library(${PROJECT_NAME} 18 | src/Timer.cpp 19 | src/NsecTimeUtilities.cpp 20 | ) 21 | target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) 22 | 23 | ############# 24 | ## Testing ## 25 | ############# 26 | 27 | if(CATKIN_ENABLE_TESTING) 28 | catkin_add_gtest(${PROJECT_NAME}-test 29 | test/test_main.cpp 30 | test/TestTimestampCorrector.cpp 31 | test/TestNsecTimeUtilities.cpp 32 | ) 33 | if(TARGET ${PROJECT_NAME}-test) 34 | target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 35 | endif() 36 | endif() 37 | 38 | cs_install() 39 | cs_export(LIBRARIES ${Boost_LIBRARIES}) 40 | -------------------------------------------------------------------------------- /sm_timing/include/sm/timing/NsecTimeUtilities.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file NsecTimeUtilities.hpp 3 | * @author Paul Furgale 4 | * @date Sat Jul 20 12:39:54 2013 5 | * 6 | * @brief Functions to support the use of nanosecond epoch time. 7 | * 8 | */ 9 | 10 | #ifndef SM_NSEC_TIME_UTILITIES 11 | #define SM_NSEC_TIME_UTILITIES 12 | 13 | #include 14 | #include 15 | 16 | namespace sm { 17 | namespace timing { 18 | 19 | /// \brief Nanoseconds since the epoch. 20 | typedef boost::int64_t NsecTime; 21 | 22 | /// \brief Convert nanoseconds since the epoch to std::chrono 23 | std::chrono::system_clock::time_point nsecToChrono( const NsecTime & time ); 24 | 25 | /// \brief Convert std::chrono to nanoseconds since the epoch. 26 | NsecTime chronoToNsec( const std::chrono::system_clock::time_point & time ); 27 | 28 | /// \brief Get the epoch time as nanoseconds since the epoch. 29 | NsecTime nsecNow(); 30 | 31 | /// \brief Convert the time (in integer nanoseconds) to decimal seconds. 32 | double nsecToSec( const NsecTime & time ); 33 | 34 | /// \brief Convert the time (in seconds) to integer nanoseconds 35 | NsecTime secToNsec( const double & time ); 36 | 37 | /// \brief return a magic number representing an invalid timestamp 38 | constexpr NsecTime getInvalidTime(); 39 | 40 | /// \brief Is the time valid? This uses a magic number 41 | /// std::numeric_limits::min() to represent an invalid time 42 | bool isValid(const NsecTime & time); 43 | 44 | } // namespace timing 45 | } // namespace sm 46 | 47 | #endif /* SM_NSEC_TIME_UTILITIES */ 48 | -------------------------------------------------------------------------------- /sm_timing/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b sm_timing is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /sm_timing/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sm_timing 4 | 0.0.1 5 | sm_timing 6 | 7 | Paul Furgale 8 | New BSD 9 | catkin 10 | catkin_simple 11 | sm_common 12 | sm_random 13 | 14 | sm_common 15 | sm_random 16 | 17 | -------------------------------------------------------------------------------- /sm_timing/src/NsecTimeUtilities.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace sm { 4 | namespace timing { 5 | 6 | /// \brief Convert nanoseconds since the epoch to std::chrono 7 | std::chrono::system_clock::time_point nsecToChrono( const NsecTime & time ) { 8 | std::chrono::nanoseconds tt(time); 9 | std::chrono::system_clock::time_point tp(std::chrono::duration_cast(tt)); 10 | return tp; 11 | } 12 | 13 | 14 | /// \brief Convert std::chrono to nanoseconds since the epoch. 15 | NsecTime chronoToNsec( const std::chrono::system_clock::time_point & time ) { 16 | return std::chrono::duration_cast( time.time_since_epoch()).count(); 17 | } 18 | 19 | 20 | /// \brief Get the epoch time as nanoseconds since the epoch. 21 | NsecTime nsecNow() { 22 | return std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch() ).count(); 23 | 24 | } 25 | 26 | 27 | /// \brief Convert the time (in integer nanoseconds) to decimal seconds. 28 | double nsecToSec( const NsecTime & time ) { 29 | return (double) time * 1e-9; 30 | } 31 | 32 | 33 | /// \brief Convert the time (in seconds) to integer nanoseconds 34 | NsecTime secToNsec( const double & time ) { 35 | return boost::int64_t( time * 1e9 ); 36 | } 37 | 38 | constexpr NsecTime getInvalidTime() { 39 | return std::numeric_limits::min(); 40 | } 41 | 42 | bool isValid(const NsecTime& time) { 43 | return getInvalidTime() == time; 44 | } 45 | 46 | } // namespace timing 47 | } // namespace sm 48 | -------------------------------------------------------------------------------- /sm_timing/test/TestNsecTimeUtilities.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | TEST( NsetTimeTestSuite, testChronoConversion ) { 6 | 7 | std::chrono::system_clock::time_point tp1 = std::chrono::system_clock::now(); 8 | sm::timing::NsecTime ns1 = sm::timing::chronoToNsec( tp1 ); 9 | std::chrono::system_clock::time_point tp2 = sm::timing::nsecToChrono( ns1 ); 10 | ASSERT_TRUE(tp1 == tp2); 11 | 12 | } 13 | 14 | 15 | TEST( NsetTimeTestSuite, testSecConversion ) { 16 | 17 | sm::timing::NsecTime ns1 = sm::timing::nsecNow(); 18 | double s2 = sm::timing::nsecToSec(ns1); 19 | sm::timing::NsecTime ns2 = sm::timing::secToNsec(s2); 20 | 21 | ASSERT_LT(std::abs(ns1-ns2), 1000000); 22 | 23 | } 24 | -------------------------------------------------------------------------------- /sm_timing/test/TestTimestampCorrector.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | 6 | TEST(TimestampCorrectorTestSuite, testTimestampCorrector1) 7 | { 8 | 9 | try { 10 | using namespace sm::timing; 11 | 12 | TimestampCorrector tc; 13 | 14 | for(int i = 0; i < 1000; ++i) 15 | { 16 | double localTime = i + sm::random::uniform(); 17 | double remoteTime = i; 18 | 19 | double estLocalTime = tc.correctTimestamp(remoteTime,localTime); 20 | 21 | std::cout << remoteTime << ", " << localTime << ", " << estLocalTime << ", " << tc.convexHullSize() << std::endl; 22 | 23 | } 24 | 25 | for(int i = 0; i < 1000; ++i) 26 | { 27 | double remoteTime = i; 28 | 29 | std::cout << remoteTime << ", " << tc.getLocalTime(remoteTime) << std::endl; 30 | } 31 | 32 | } 33 | catch(const std::exception & e) 34 | { 35 | FAIL() << e.what(); 36 | } 37 | 38 | } 39 | 40 | 41 | TEST(TimestampCorrectorTestSuite, testTimestampCorrectorPeriodic) 42 | { 43 | 44 | try { 45 | using namespace sm::timing; 46 | 47 | TimestampCorrector tc; 48 | double switchingPeriod = 100.0; 49 | 50 | for(int i = 0; i < 1000; ++i) 51 | { 52 | double localTime = i > 500 ? 1.2*i : i + sm::random::uniform(); 53 | double remoteTime = i; 54 | 55 | double estLocalTime = tc.correctTimestamp(remoteTime, localTime, switchingPeriod); 56 | 57 | if (i % int(switchingPeriod) == 0) 58 | { 59 | ASSERT_NEAR(localTime, estLocalTime, 1); 60 | } 61 | 62 | } 63 | 64 | } 65 | catch(const std::exception & e) 66 | { 67 | FAIL() << e.what(); 68 | } 69 | 70 | } 71 | 72 | 73 | -------------------------------------------------------------------------------- /sm_timing/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /// Run all the tests that were declared with TEST() 4 | int main(int argc, char **argv){ 5 | testing::InitGoogleTest(&argc, argv); 6 | return RUN_ALL_TESTS(); 7 | } 8 | -------------------------------------------------------------------------------- /sm_value_store/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.1) 2 | project(sm_value_store) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | 6 | set(CMAKE_CXX_STANDARD 14) 7 | 8 | catkin_simple() 9 | 10 | cs_add_library(${PROJECT_NAME} 11 | src/ValueStore.cpp 12 | src/VerboseValueStore.cpp 13 | src/PrefixedValueStore.cpp 14 | src/PropertyTreeValueStore.cpp 15 | ) 16 | 17 | cs_install() 18 | cs_export() 19 | 20 | if(CATKIN_ENABLE_TESTING) 21 | catkin_add_gtest(${PROJECT_NAME}-test 22 | test/test_main.cpp 23 | test/ValueStore.cpp 24 | ) 25 | if(TARGET ${PROJECT_NAME}-test) 26 | target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 27 | endif() 28 | endif() 29 | -------------------------------------------------------------------------------- /sm_value_store/include/sm/value_store/LayeredValueStore.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VALUE_STORE_LAYEREDVALUESTORE_HPP_ 2 | #define VALUE_STORE_LAYEREDVALUESTORE_HPP_ 3 | 4 | #include 5 | 6 | #include "ValueStore.hpp" 7 | 8 | namespace sm { 9 | namespace value_store { 10 | 11 | class LayeredValueStore : public ValueStore { 12 | public: 13 | template 14 | LayeredValueStore(ValueStores && ... vss) { 15 | add(vss...); 16 | } 17 | template 18 | void add(ValueStoreA && vsA, ValueStoreB && vsB, ValueStores && ... vss) { 19 | add(vsA); 20 | add(vsB, vss...); 21 | } 22 | void add(const std::shared_ptr & p){ if(p) valuestores.push_back(p); } 23 | void add(ValueStoreRef & p){ if(p) valuestores.push_back(p.getValueStoreSharedPtr()); } 24 | 25 | ValueHandle getBool(const std::string & path, boost::optional def = boost::optional()) const override; 26 | ValueHandle getInt(const std::string & path, boost::optional def = boost::optional()) const override; 27 | ValueHandle getDouble(const std::string & path, boost::optional def = boost::optional()) const override; 28 | ValueHandle getString(const std::string & path, boost::optional def = boost::optional()) const override; 29 | 30 | bool hasKey(const std::string & path) const override; 31 | 32 | bool isChildSupported() const override; 33 | KeyValueStorePair getChild(const std::string & key) const override; 34 | std::vector getChildren() const override; 35 | private: 36 | void add(){} 37 | template (ValueStore::* Func)(const std::string &, boost::optional) const> 38 | ValueHandle getFromLayers(const std::string & path, boost::optional def) const; 39 | 40 | std::vector > valuestores; 41 | }; 42 | 43 | } 44 | } 45 | 46 | 47 | #endif /* VALUE_STORE_LAYEREDVALUESTORE_HPP_ */ 48 | -------------------------------------------------------------------------------- /sm_value_store/include/sm/value_store/PrefixedValueStore.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VALUE_STORE_PREFIXEDVALUESTORE_HPP_ 2 | #define VALUE_STORE_PREFIXEDVALUESTORE_HPP_ 3 | 4 | 5 | #include "ValueStore.hpp" 6 | 7 | namespace sm { 8 | namespace value_store { 9 | 10 | class PrefixedValueStore : public ValueStore { 11 | public: 12 | enum class PrefixMode { 13 | ADD, REMOVE 14 | }; 15 | PrefixedValueStore(ValueStore::SharedPtr vs, PrefixMode mode, const std::string & prefix) : vs_(vs), prefix_(prefix + "/"), mode_(mode) {} 16 | PrefixedValueStore(const ValueStoreRef & vs, PrefixMode mode, const std::string & prefix) : PrefixedValueStore(vs.getValueStoreSharedPtr(), mode, prefix){} 17 | 18 | ValueHandle getBool(const std::string & path, boost::optional def) const override; 19 | ValueHandle getInt(const std::string & path, boost::optional def) const override; 20 | ValueHandle getDouble(const std::string & path, boost::optional def) const override; 21 | ValueHandle getString(const std::string & path, boost::optional def) const override; 22 | 23 | bool hasKey(const std::string & path) const override; 24 | 25 | KeyValueStorePair getChild(const std::string & key) const override; 26 | std::vector getChildren() const override; 27 | bool isChildSupported() const override; 28 | private: 29 | std::string applyPrefix(const std::string & path, bool* isPossiblePtr = nullptr) const; 30 | ValueStore::SharedPtr vs_; 31 | std::string prefix_; 32 | const PrefixMode mode_; 33 | }; 34 | 35 | } 36 | } 37 | 38 | 39 | #endif /* VALUE_STORE_PREFIXEDVALUESTORE_HPP_ */ 40 | -------------------------------------------------------------------------------- /sm_value_store/include/sm/value_store/PropertyTreeValueStore.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VALUE_STORE_PROPERTYTREEVALUESTORE_HPP_ 2 | #define VALUE_STORE_PROPERTYTREEVALUESTORE_HPP_ 3 | 4 | #include "ValueStore.hpp" 5 | 6 | #include 7 | 8 | namespace sm { 9 | namespace value_store { 10 | 11 | class PropertyTreeValueStore : public ExtendibleValueStore, public sm::PropertyTree { 12 | public: 13 | PropertyTreeValueStore(const sm::PropertyTree & pt) : sm::PropertyTree(pt) {} 14 | ValueHandle getBool(const std::string & path, boost::optional def = boost::optional()) const override; 15 | ValueHandle getInt(const std::string & path, boost::optional def = boost::optional()) const override; 16 | ValueHandle getDouble(const std::string & path, boost::optional def = boost::optional()) const override; 17 | ValueHandle getString(const std::string & path, boost::optional def = boost::optional()) const override; 18 | 19 | bool hasKey(const std::string& path) const override; 20 | 21 | KeyValueStorePair getChild(const std::string & key) const override; 22 | std::vector getChildren() const override; 23 | 24 | ValueHandle addBool(const std::string & path, bool initialValue) override; 25 | ValueHandle addInt(const std::string & path, int initialValue) override; 26 | ValueHandle addDouble(const std::string & path, double initialValue) override; 27 | ValueHandle addString(const std::string & path, std::string initialValue) override; 28 | 29 | ExtendibleKeyValueStorePair getExtendibleChild(const std::string & key) const override; 30 | std::vector getExtendibleChildren() const override; 31 | 32 | bool isEmpty() const override; 33 | private: 34 | friend class ValueStoreRef; 35 | template ValueHandle getFromPt(const std::string & path, boost::optional def) const; 36 | }; 37 | 38 | } 39 | } 40 | 41 | #endif /* VALUE_STORE_PROPERTYTREEVALUESTORE_HPP_ */ 42 | -------------------------------------------------------------------------------- /sm_value_store/include/sm/value_store/VerboseValueStore.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VALUE_STORE_VERBOSEVALUESTORE_HPP_ 2 | #define VALUE_STORE_VERBOSEVALUESTORE_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | #include "ValueStore.hpp" 8 | 9 | namespace sm { 10 | namespace value_store { 11 | namespace internal { 12 | template 13 | class VerboseValueStoreT : public Base { 14 | public: 15 | VerboseValueStoreT(std::shared_ptr vs, std::function log) : vs_(vs), log_(log) {} 16 | ValueHandle getBool(const std::string & path, boost::optional def = boost::optional()) const override; 17 | ValueHandle getInt(const std::string & path, boost::optional def = boost::optional()) const override; 18 | ValueHandle getDouble(const std::string & path, boost::optional def = boost::optional()) const override; 19 | ValueHandle getString(const std::string & path, boost::optional def = boost::optional()) const override; 20 | 21 | bool hasKey(const std::string & path) const override; 22 | 23 | bool isChildSupported() const override; 24 | KeyValueStorePair getChild(const std::string & key) const override; 25 | std::vector getChildren() const override; 26 | 27 | std::shared_ptr getUnderlyingValueStore() const { 28 | return vs_; 29 | } 30 | protected: 31 | std::shared_ptr vs_; 32 | std::function log_; 33 | private: 34 | template 35 | T logValue(const char * func, const std::string & path, T && v, O o = false) const; 36 | }; 37 | 38 | extern template class VerboseValueStoreT; 39 | extern template class VerboseValueStoreT; 40 | } 41 | 42 | using VerboseValueStore = internal::VerboseValueStoreT; 43 | 44 | class VerboseExtendibleValueStore : public internal::VerboseValueStoreT { 45 | public: 46 | using internal::VerboseValueStoreT::VerboseValueStoreT; 47 | ValueHandle addBool(const std::string & path, bool initialValue) override; 48 | ValueHandle addInt(const std::string & path, int initialValue) override; 49 | ValueHandle addDouble(const std::string & path, double initialValue) override; 50 | ValueHandle addString(const std::string & path, std::string initialValue) override; 51 | 52 | ExtendibleKeyValueStorePair getExtendibleChild(const std::string & key) const override; 53 | std::vector getExtendibleChildren() const override; 54 | 55 | private: 56 | template 57 | T logAddValue(const char * func, const std::string & path, T && v) const; 58 | }; 59 | 60 | } 61 | } 62 | 63 | 64 | #endif /* VALUE_STORE_VERBOSEVALUESTORE_HPP_ */ 65 | -------------------------------------------------------------------------------- /sm_value_store/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sm_value_store 4 | 0.0.0 5 | The sm_value_store is an experimental package to experiment with the idea of an abstract source of parameters / values organized as tree. This is very similar to the PropertyTree, on which the only so far existing implementation is also based on, but a bit more abstract and with a different mutation concept : values come with a handle through which one can update the value (if supported by the backend). 6 | Adding new values is not supported yet. 7 | 8 | Hannes Sommer 9 | Hannes Sommer 10 | New BSD 11 | 12 | catkin 13 | catkin_simple 14 | 15 | sm_common 16 | sm_property_tree 17 | gtest 18 | 19 | 20 | -------------------------------------------------------------------------------- /sm_value_store/src/PrefixedValueStore.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | namespace sm { 9 | namespace value_store { 10 | 11 | ValueHandle PrefixedValueStore::getBool(const std::string & path, boost::optional def) const { 12 | return vs_->getBool(applyPrefix(path), def); 13 | } 14 | ValueHandle PrefixedValueStore::getInt(const std::string & path, boost::optional def) const { 15 | return vs_->getInt(applyPrefix(path), def); 16 | } 17 | ValueHandle PrefixedValueStore::getDouble(const std::string & path, boost::optional def) const { 18 | return vs_->getDouble(applyPrefix(path), def); 19 | } 20 | ValueHandle PrefixedValueStore::getString(const std::string & path, boost::optional def) const { 21 | return vs_->getString(applyPrefix(path), def); 22 | } 23 | bool PrefixedValueStore::hasKey(const std::string& path) const { 24 | bool isPossible; 25 | auto && prefixedPath = applyPrefix(path, &isPossible); 26 | return isPossible && vs_->hasKey(prefixedPath); 27 | } 28 | 29 | KeyValueStorePair PrefixedValueStore::getChild(const std::string & key) const { 30 | bool isPossible; 31 | auto && prefixedKey = applyPrefix(key, &isPossible); 32 | if(isPossible){ 33 | if(PrefixMode::REMOVE == mode_ && (prefixedKey.empty() || prefixedKey == "/")){ 34 | return KeyValueStorePair(key, vs_); 35 | } 36 | return KeyValueStorePair(key, std::make_shared(vs_, mode_, prefixedKey)); 37 | } else { 38 | return KeyValueStorePair(key, ValueStoreRef()); 39 | } 40 | } 41 | std::vector PrefixedValueStore::getChildren() const { 42 | SM_THROW(std::runtime_error, "Not implemented, yet"); 43 | } 44 | 45 | std::string PrefixedValueStore::applyPrefix(const std::string& path, bool* isPossiblePtr) const { 46 | switch(mode_){ 47 | case PrefixMode::ADD: 48 | return prefix_ + path; 49 | case PrefixMode::REMOVE: 50 | if (path.size() >= prefix_.size() && path.substr(0, prefix_.size()) == prefix_) { 51 | if(isPossiblePtr) { 52 | *isPossiblePtr = true; 53 | } 54 | return path.substr(prefix_.size()); 55 | } else { 56 | if(isPossiblePtr) { 57 | *isPossiblePtr = path.size() == prefix_.size() - 1 && path + "/" == prefix_; 58 | return std::string(); 59 | } else { 60 | SM_THROW(std::runtime_error, "path does not contain the prefix (" << prefix_ << "): " << path); 61 | } 62 | } 63 | default: 64 | SM_THROW(std::runtime_error, "Unknown mode " << size_t(mode_)); 65 | } 66 | } 67 | 68 | bool PrefixedValueStore::isChildSupported() const { 69 | switch(mode_){ 70 | case PrefixMode::REMOVE: 71 | return vs_->isChildSupported(); 72 | default: 73 | return true; 74 | } 75 | } 76 | 77 | } 78 | } 79 | -------------------------------------------------------------------------------- /sm_value_store/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /// Run all the tests that were declared with TEST() 4 | int main(int argc, char **argv){ 5 | testing::InitGoogleTest(&argc, argv); 6 | return RUN_ALL_TESTS(); 7 | } 8 | -------------------------------------------------------------------------------- /tutorial.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/schweizer_messer/cb9d52841cebd965bb201ba83031e830980d8016/tutorial.txt --------------------------------------------------------------------------------