├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── ceres.patch ├── cmake ├── CeresConfig.cmake ├── FindCXSparse.cmake ├── FindEigen.cmake ├── FindGlog.cmake ├── FindSuiteSparse.cmake ├── FindVISensor.cmake ├── okvisConfig.cmake.in ├── okvisConfig.hpp.in ├── okvisConfigVersion.cmake.in └── opengv │ ├── CMakeLists.txt │ └── opengvConfig.cmake.in ├── config ├── config_DOC1357.yaml └── config_fpga_p2_euroc.yaml ├── contributors.txt ├── doxygen.config ├── okvis_apps └── src │ └── okvis_app_synchronous.cpp ├── okvis_ceres ├── CMakeLists.txt ├── include │ └── okvis │ │ ├── Estimator.hpp │ │ ├── IdProvider.hpp │ │ ├── ceres │ │ ├── CeresIterationCallback.hpp │ │ ├── ErrorInterface.hpp │ │ ├── HomogeneousPointError.hpp │ │ ├── HomogeneousPointLocalParameterization.hpp │ │ ├── HomogeneousPointParameterBlock.hpp │ │ ├── ImuError.hpp │ │ ├── LocalParamizationAdditionalInterfaces.hpp │ │ ├── Map.hpp │ │ ├── MarginalizationError.hpp │ │ ├── ParameterBlock.hpp │ │ ├── ParameterBlockSized.hpp │ │ ├── PoseError.hpp │ │ ├── PoseLocalParameterization.hpp │ │ ├── PoseParameterBlock.hpp │ │ ├── RelativePoseError.hpp │ │ ├── ReprojectionError.hpp │ │ ├── ReprojectionErrorBase.hpp │ │ ├── SpeedAndBiasError.hpp │ │ ├── SpeedAndBiasParameterBlock.hpp │ │ ├── implementation │ │ │ ├── MarginalizationError.hpp │ │ │ └── ReprojectionError.hpp │ │ └── ode │ │ │ └── ode.hpp │ │ └── implementation │ │ └── Estimator.hpp ├── src │ ├── Estimator.cpp │ ├── HomogeneousPointError.cpp │ ├── HomogeneousPointLocalParameterization.cpp │ ├── HomogeneousPointParameterBlock.cpp │ ├── IdProvider.cpp │ ├── ImuError.cpp │ ├── LocalParamizationAdditionalInterfaces.cpp │ ├── Map.cpp │ ├── MarginalizationError.cpp │ ├── PoseError.cpp │ ├── PoseLocalParameterization.cpp │ ├── PoseParameterBlock.cpp │ ├── RelativePoseError.cpp │ ├── SpeedAndBiasError.cpp │ └── SpeedAndBiasParameterBlock.cpp └── test │ ├── TestEstimator.cpp │ ├── TestHomogeneousPointError.cpp │ ├── TestImuError.cpp │ ├── TestMap.cpp │ ├── TestMarginalization.cpp │ ├── TestReprojectionError.cpp │ └── test_main.cpp ├── okvis_common ├── CMakeLists.txt ├── include │ └── okvis │ │ ├── FrameTypedefs.hpp │ │ ├── Measurements.hpp │ │ ├── Parameters.hpp │ │ ├── Variables.hpp │ │ ├── VioBackendInterface.hpp │ │ ├── VioFrontendInterface.hpp │ │ ├── VioInterface.hpp │ │ └── VioParametersReader.hpp └── src │ ├── VioInterface.cpp │ └── VioParametersReader.cpp ├── okvis_cv ├── CMakeLists.txt ├── include │ └── okvis │ │ ├── Frame.hpp │ │ ├── MultiFrame.hpp │ │ ├── cameras │ │ ├── CameraBase.hpp │ │ ├── DistortionBase.hpp │ │ ├── EquidistantDistortion.hpp │ │ ├── NCameraSystem.hpp │ │ ├── NoDistortion.hpp │ │ ├── PinholeCamera.hpp │ │ ├── RadialTangentialDistortion.hpp │ │ ├── RadialTangentialDistortion8.hpp │ │ └── implementation │ │ │ ├── CameraBase.hpp │ │ │ ├── EquidistantDistortion.hpp │ │ │ ├── NCameraSystem.hpp │ │ │ ├── PinholeCamera.hpp │ │ │ ├── RadialTangentialDistortion.hpp │ │ │ └── RadialTangentialDistortion8.hpp │ │ └── implementation │ │ ├── Frame.hpp │ │ └── MultiFrame.hpp ├── src │ ├── CameraBase.cpp │ └── NCameraSystem.cpp └── test │ ├── TestFrame.cpp │ ├── TestMultiFrame.cpp │ ├── TestNCameraSystem.cpp │ ├── TestPinholeCamera.cpp │ └── runTests.cpp ├── okvis_frontend ├── CMakeLists.txt ├── include │ ├── okvis │ │ ├── Frontend.hpp │ │ ├── VioKeyframeWindowMatchingAlgorithm.hpp │ │ └── triangulation │ │ │ ├── ProbabilisticStereoTriangulator.hpp │ │ │ └── stereo_triangulation.hpp │ └── opengv │ │ ├── absolute_pose │ │ └── FrameNoncentralAbsoluteAdapter.hpp │ │ ├── relative_pose │ │ └── FrameRelativeAdapter.hpp │ │ └── sac_problems │ │ ├── absolute_pose │ │ └── FrameAbsolutePoseSacProblem.hpp │ │ └── relative_pose │ │ ├── FrameRelativePoseSacProblem.hpp │ │ └── FrameRotationOnlySacProblem.hpp └── src │ ├── FrameNoncentralAbsoluteAdapter.cpp │ ├── FrameRelativeAdapter.cpp │ ├── Frontend.cpp │ ├── ProbabilisticStereoTriangulator.cpp │ ├── VioKeyframeWindowMatchingAlgorithm.cpp │ └── stereo_triangulation.cpp ├── okvis_kinematics ├── CMakeLists.txt ├── include │ └── okvis │ │ └── kinematics │ │ ├── Transformation.hpp │ │ ├── implementation │ │ └── Transformation.hpp │ │ └── operators.hpp ├── src │ └── dependency-tracker.cc └── test │ ├── TestTransformation.cpp │ └── runTests.cpp ├── okvis_matcher ├── CMakeLists.txt ├── include │ └── okvis │ │ ├── DenseMatcher.hpp │ │ ├── MatchingAlgorithm.hpp │ │ ├── ThreadPool.hpp │ │ └── implementation │ │ └── DenseMatcher.hpp ├── src │ ├── DenseMatcher.cpp │ ├── MatchingAlgorithm.cpp │ └── ThreadPool.cpp └── test │ ├── testMatcher.cpp │ └── test_main.cpp ├── okvis_multisensor_processing ├── CMakeLists.txt ├── include │ └── okvis │ │ ├── FrameSynchronizer.hpp │ │ ├── ImuFrameSynchronizer.hpp │ │ ├── ThreadedKFVio.hpp │ │ ├── VioVisualizer.hpp │ │ └── threadsafe │ │ └── ThreadsafeQueue.hpp ├── src │ ├── FrameSynchronizer.cpp │ ├── ImuFrameSynchronizer.cpp │ ├── ThreadedKFVio.cpp │ └── VioVisualizer.cpp └── test │ ├── FrameSynchronizer_test.cpp │ ├── ImuFrameSynchronizer_test.cpp │ ├── MockVioBackendInterface.hpp │ ├── MockVioFrontendInterface.hpp │ ├── VioVisualizer_test.cpp │ ├── testDataFlow.cpp │ ├── testDataGenerators.hpp │ ├── testImage.jpg │ ├── testSynchronizer.cpp │ ├── testThreading.cpp │ └── test_main.cpp ├── okvis_time ├── CMakeLists.txt ├── include │ └── okvis │ │ ├── Duration.hpp │ │ ├── Time.hpp │ │ └── implementation │ │ ├── Duration.hpp │ │ └── Time.hpp └── src │ ├── Duration.cpp │ └── Time.cpp ├── okvis_timing ├── CMakeLists.txt ├── include │ └── okvis │ │ └── timing │ │ ├── NsecTimeUtilities.hpp │ │ └── Timer.hpp ├── src │ ├── NsecTimeUtilities.cpp │ └── Timer.cpp └── test │ ├── TestNsecTimeUtilities.cpp │ └── test_main.cpp └── okvis_util ├── CMakeLists.txt ├── include └── okvis │ ├── assert_macros.hpp │ └── source_file_pos.hpp └── src └── dependency-tracker.cc /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | 19 | # Compiled Static libraries 20 | *.lai 21 | *.la 22 | *.a 23 | *.lib 24 | 25 | # Executables 26 | *.exe 27 | *.out 28 | *.app 29 | 30 | # Docu 31 | documentation/html/ 32 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | OKVIS - Open Keyframe-based Visual-Inertial SLAM 2 | Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 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 | * Redistributions of source code must retain the above copyright notice, 8 | this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright notice, 10 | this list of conditions and the following disclaimer in the documentation 11 | and/or other materials provided with the distribution. 12 | * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 13 | its contributors may be used to endorse or promote products derived from 14 | this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | POSSIBILITY OF SUCH DAMAGE. 27 | 28 | Authors and contributors: 29 | - Stefan Leutenegger 30 | - Andreas Forster 31 | - Paul Furgale 32 | - Pascal Gohl 33 | - Simon Lynen 34 | -------------------------------------------------------------------------------- /ceres.patch: -------------------------------------------------------------------------------- 1 | 268c268 2 | < Chunk() : size(0) {} 3 | --- 4 | > Chunk() : size(0), start(0) {} 5 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /cmake/FindVISensor.cmake: -------------------------------------------------------------------------------- 1 | find_package(PkgConfig) 2 | pkg_check_modules(PC_VISENSOR visensor) 3 | #message("pkg: ${PC_VISENSOR_INCLUDEDIRS}") 4 | set(VISENSOR_DEFINITIONS ${PC_VISENSOR_CFLAGS_OTHER}) 5 | 6 | find_path(VISensorDriver_INCLUDE_DIR visensor.hpp 7 | HINTS ${PC_VISENSOR_INCLUDEDIR} ${PC_VISENSOR_INCLUDE_DIRS} 8 | PATH_SUFFIXES visensor ) 9 | 10 | find_library(VISensorDriver_LIBRARY NAMES libvisensor.so 11 | HINTS ${PC_VISENSOR_LIBDIR} ${PC_VISENSOR_LIBRARY_DIRS} /usr/local/lib) 12 | 13 | set(VISENSOR_LIBRARIES ${VISENSOR_LIBRARY} ) 14 | set(VISENSOR_INCLUDE_DIRS ${VISENSOR_INCLUDE_DIR} ) 15 | 16 | 17 | include(FindPackageHandleStandardArgs) 18 | # handle the QUIETLY and REQUIRED arguments and set LIBXML2_FOUND to TRUE 19 | # if all listed variables are TRUE 20 | find_package_handle_standard_args(VISensorDriver DEFAULT_MSG 21 | VISensorDriver_LIBRARY VISensorDriver_INCLUDE_DIR) 22 | 23 | mark_as_advanced(VISensorDriver_INCLUDE_DIR VISensorDriver_LIBRARY ) 24 | -------------------------------------------------------------------------------- /cmake/okvisConfig.cmake.in: -------------------------------------------------------------------------------- 1 | # - Config file for the OKVIS package 2 | # It defines the following variables 3 | # OKVIS_INCLUDE_DIRS - include directories for FooBar 4 | # OKVIS_LIBRARIES - libraries to link against 5 | # OKVIS_EXECUTABLE - the okvis_app_synchronous executable 6 | # OKVIS_CERES_CONFIG - path to CeresConfig.cmake, to use find_package(ceres) 7 | 8 | set(OKVIS_CERES_CONFIG "@OKVIS_CERES_CONFIG@") 9 | 10 | # Compute paths 11 | get_filename_component(OKVIS_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) 12 | set(OKVIS_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@") 13 | 14 | # Our library dependencies (contains definitions for IMPORTED targets) 15 | if(NOT TARGET okvis AND NOT OKVIS_BINARY_DIR) 16 | include("${OKVIS_CMAKE_DIR}/okvisTargets.cmake") 17 | endif() 18 | 19 | # These are IMPORTED targets created by okvisTargets.cmake 20 | set(OKVIS_LIBRARIES 21 | okvis_util 22 | okvis_kinematics 23 | okvis_time 24 | okvis_cv 25 | okvis_common 26 | okvis_ceres 27 | okvis_timing 28 | okvis_matcher 29 | okvis_frontend 30 | okvis_multisensor_processing ) 31 | set(OKVIS_EXECUTABLE okvis_app_synchronous) 32 | -------------------------------------------------------------------------------- /cmake/okvisConfig.hpp.in: -------------------------------------------------------------------------------- 1 | #define OKVIS_VERSION_MAJOR @OKVIS_MAJOR_VERSION@ 2 | #define OKVIS_VERSION_MINOR @OKVIS_MINOR_VERSION@ 3 | #define OKVIS_PATCH_VERSION @OKVIS_PATCH_VERSION@ 4 | -------------------------------------------------------------------------------- /cmake/okvisConfigVersion.cmake.in: -------------------------------------------------------------------------------- 1 | set(PACKAGE_VERSION "@OKVIS_VERSION@") 2 | 3 | # Check whether the requested PACKAGE_FIND_VERSION is compatible 4 | if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") 5 | set(PACKAGE_VERSION_COMPATIBLE FALSE) 6 | else() 7 | set(PACKAGE_VERSION_COMPATIBLE TRUE) 8 | if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}") 9 | set(PACKAGE_VERSION_EXACT TRUE) 10 | endif() 11 | endif() 12 | -------------------------------------------------------------------------------- /cmake/opengv/opengvConfig.cmake.in: -------------------------------------------------------------------------------- 1 | # - Config file for the OPENGV package 2 | # It defines the following variables 3 | # OPENGV_INCLUDE_DIRS - include directories for FooBar 4 | # OPENGV_LIBRARIES - libraries to link against 5 | # OPENGV_EXECUTABLE - the opengv executable - none available 6 | 7 | # Compute paths 8 | get_filename_component(OPENGV_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) 9 | 10 | # Our library dependencies (contains definitions for IMPORTED targets) 11 | if(NOT TARGET opengv AND NOT OPENGV_BINARY_DIR) 12 | include("${OPENGV_CMAKE_DIR}/opengvTargets.cmake") 13 | endif() 14 | 15 | set(OPENGV_LIBRARIES opengv) 16 | set(OPENGV_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@") 17 | -------------------------------------------------------------------------------- /config/config_DOC1357.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | # 'cameras' sequence contains for each camera: T_SC, image_dimension, distortion_coefficients, distortion_type, focal_length and principal_point 3 | # if this sequence or one of the parameters of an entry is missing the calibration will not be used. Depending on 'useDriver' it will try to 4 | # get the calibration directly from the sensor. If useDriver==false it will first try to get the calibration via the visensor calibration service 5 | # and then as a last resort the calibration topic is tried. 6 | cameras: 7 | - {T_SC: 8 | [0.999983871478707, -0.003378272078458, 0.004565529566263, 0.036327130103992, 9 | 0.003394964456101, 0.999987563420818, -0.003653384737134, -0.009239753497739, 10 | -0.004553130659044, 0.003668825624041, 0.999982904213738, 0.000422652652043, 11 | 0.0, 0.0, 0.0, 1.0], 12 | image_dimension: [752, 480], 13 | distortion_coefficients: [-0.29370123496555756, 0.08730871667303616, 0.00010038160473754507, 0.00013680373784768344], 14 | distortion_type: radialtangential, 15 | focal_length: [466.8586127874696, 466.7725034368207], 16 | principal_point: [368.87847654026825, 240.78852760520198]} 17 | 18 | - {T_SC: 19 | [0.999970407015966, -0.005254557938991, 0.005619138117969, -0.072440830443425, 20 | 0.005282751748116, 0.999973473583788, -0.005014445407151, -0.009075617855002, 21 | -0.005592640368449, 0.005043981526464, 0.999971639909887, 0.001012817402609, 22 | 0.0, 0.0, 0.0, 1.0], 23 | image_dimension: [752, 480], 24 | distortion_coefficients: [-0.28861728460359526, 0.0822799844561439, -0.00015343044028117667, 3.3302619927644084e-05], 25 | distortion_type: radialtangential, 26 | focal_length: [467.7233457288314, 467.72122883849744], 27 | principal_point: [368.8077608831562, 213.90503557806537]} 28 | 29 | camera_params: 30 | camera_rate: 20 # just to manage the expectations of when there should be frames arriving 31 | sigma_absolute_translation: 0.0 # The standard deviation of the camera extrinsics translation, e.g. 1.0e-10 for online-calib [m]. 32 | sigma_absolute_orientation: 0.0 # The standard deviation of the camera extrinsics orientation, e.g. 1.0e-3 for online-calib [rad]. 33 | sigma_c_relative_translation: 0.0 # The std. dev. of the cam. extr. transl. change between frames, e.g. 1.0e-6 for adaptive online calib (not less for numerics) [m]. 34 | sigma_c_relative_orientation: 0.0 # The std. dev. of the cam. extr. orient. change between frames, e.g. 1.0e-6 for adaptive online calib (not less for numerics) [rad]. 35 | timestamp_tolerance: 0.005 # [s] stereo frame out-of-sync tolerance 36 | 37 | imu_params: 38 | a_max: 176.0 # acceleration saturation [m/s^2] 39 | g_max: 7.8 # gyro saturation [rad/s] 40 | sigma_g_c: 12.0e-4 # gyro noise density [rad/s/sqrt(Hz)] 41 | sigma_a_c: 8.0e-3 # accelerometer noise density [m/s^2/sqrt(Hz)] 42 | sigma_bg: 0.03 # gyro bias prior [rad/s] 43 | sigma_ba: 0.1 # accelerometer bias prior [m/s^2] 44 | sigma_gw_c: 4.0e-6 # gyro drift noise density [rad/s^s/sqrt(Hz)] 45 | sigma_aw_c: 4.0e-5 # accelerometer drift noise density [m/s^2/sqrt(Hz)] 46 | tau: 3600.0 # reversion time constant, currently not in use [s] 47 | g: 9.81007 # Earth's acceleration due to gravity [m/s^2] 48 | a0: [ 0.0, 0.0, 0.0 ] # Accelerometer bias [m/s^2] 49 | imu_rate: 200 50 | # tranform Body-Sensor (IMU) 51 | T_BS: 52 | [1.0000, 0.0000, 0.0000, 0.0000, 53 | 0.0000, 1.0000, 0.0000, 0.0000, 54 | 0.0000, 0.0000, 1.0000, 0.0000, 55 | 0.0000, 0.0000, 0.0000, 1.0000] 56 | 57 | # Estimator parameters 58 | numKeyframes: 5 # number of keyframes in optimisation window 59 | numImuFrames: 3 # number of frames linked by most recent nonlinear IMU error terms 60 | 61 | # ceres optimization options 62 | ceres_options: 63 | minIterations: 3 # minimum number of iterations always performed 64 | maxIterations: 10 # never do more than these, even if not converged 65 | timeLimit: 0.035 # [s] negative values will set the an unlimited time limit 66 | 67 | # detection 68 | detection_options: 69 | threshold: 40.0 # detection threshold. By default the uniformity radius in pixels 70 | octaves: 0 # number of octaves for detection. 0 means single-scale at highest resolution 71 | maxNoKeypoints: 400 # restrict to a maximum of this many keypoints per image (strongest ones) 72 | 73 | # delay of images [s]: 74 | imageDelay: 0.0 # in case you are using a custom setup, you will have to calibrate this. 0 for the VISensor. 75 | 76 | # display debug images? 77 | displayImages: true # displays debug video and keyframe matches. May be slow. 78 | 79 | # use direct driver 80 | useDriver: true 81 | 82 | # some options for how and what to publish -- optional in ROS-free version 83 | publishing_options: 84 | publish_rate: 200 # rate at which odometry updates are published only works properly if imu_rate/publish_rate is an integer!! 85 | publishLandmarks: true # select, if you want to publish landmarks at all 86 | landmarkQualityThreshold: 1.0e-2 # landmark with lower quality will not be published 87 | maximumLandmarkQuality: 0.05 # landmark with higher quality will be published with the maximum colour intensity 88 | maxPathLength: 20 # maximum length of the published path 89 | publishImuPropagatedState: true # Should the state that is propagated with IMU messages be published? Or just the optimized ones? 90 | # provide custom World frame Wc 91 | T_Wc_W: 92 | [1.0000, 0.0000, 0.0000, 0.0000, 93 | 0.0000, 1.0000, 0.0000, 0.0000, 94 | 0.0000, 0.0000, 1.0000, 0.0000, 95 | 0.0000, 0.0000, 0.0000, 1.0000] 96 | trackedBodyFrame: B # B or S, the frame of reference that will be expressed relative to the selected worldFrame 97 | velocitiesFrame: Wc # Wc, B or S, the frames in which the velocities of the selected trackedBodyFrame will be expressed in 98 | -------------------------------------------------------------------------------- /config/config_fpga_p2_euroc.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | cameras: 3 | - {T_SC: 4 | [ 0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, 5 | 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, 6 | -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, 7 | 0.0, 0.0, 0.0, 1.0], 8 | image_dimension: [752, 480], 9 | distortion_coefficients: [-0.28340811217, 0.0739590738929, 0.000193595028569, 1.76187114545e-05], 10 | distortion_type: radialtangential, 11 | focal_length: [458.654880721, 457.296696463], 12 | principal_point: [367.215803962, 248.37534061]} 13 | 14 | - {T_SC: 15 | [ 0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556, 16 | 0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024, 17 | -0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038, 18 | 0.0, 0.0, 0.0, 1.0], 19 | image_dimension: [752, 480], 20 | distortion_coefficients: [-0.283683654496, 0.0745128430929, -0.000104738949098, -3.55590700274e-05], 21 | distortion_type: radialtangential, 22 | focal_length: [457.587426604, 456.13442556], 23 | principal_point: [379.99944652, 255.238185386]} 24 | 25 | 26 | camera_params: 27 | camera_rate: 20 # just to manage the expectations of when there should be frames arriving 28 | sigma_absolute_translation: 0.0 # The standard deviation of the camera extrinsics translation, e.g. 1.0e-10 for online-calib [m]. 29 | sigma_absolute_orientation: 0.0 # The standard deviation of the camera extrinsics orientation, e.g. 1.0e-3 for online-calib [rad]. 30 | sigma_c_relative_translation: 0.0 # The std. dev. of the cam. extr. transl. change between frames, e.g. 1.0e-6 for adaptive online calib (not less for numerics) [m]. 31 | sigma_c_relative_orientation: 0.0 # The std. dev. of the cam. extr. orient. change between frames, e.g. 1.0e-6 for adaptive online calib (not less for numerics) [rad]. 32 | timestamp_tolerance: 0.005 # [s] stereo frame out-of-sync tolerance 33 | 34 | imu_params: 35 | a_max: 176.0 # acceleration saturation [m/s^2] 36 | g_max: 7.8 # gyro saturation [rad/s] 37 | sigma_g_c: 12.0e-4 # gyro noise density [rad/s/sqrt(Hz)] 38 | sigma_a_c: 8.0e-3 # accelerometer noise density [m/s^2/sqrt(Hz)] 39 | sigma_bg: 0.03 # gyro bias prior [rad/s] 40 | sigma_ba: 0.1 # accelerometer bias prior [m/s^2] 41 | sigma_gw_c: 4.0e-6 # gyro drift noise density [rad/s^s/sqrt(Hz)] 42 | sigma_aw_c: 4.0e-5 # accelerometer drift noise density [m/s^2/sqrt(Hz)] 43 | tau: 3600.0 # reversion time constant, currently not in use [s] 44 | g: 9.81007 # Earth's acceleration due to gravity [m/s^2] 45 | a0: [ 0.0, 0.0, 0.0 ] # Accelerometer bias [m/s^2] 46 | imu_rate: 200 47 | # tranform Body-Sensor (IMU) 48 | T_BS: 49 | [1.0000, 0.0000, 0.0000, 0.0000, 50 | 0.0000, 1.0000, 0.0000, 0.0000, 51 | 0.0000, 0.0000, 1.0000, 0.0000, 52 | 0.0000, 0.0000, 0.0000, 1.0000] 53 | 54 | # Estimator parameters 55 | numKeyframes: 5 # number of keyframes in optimisation window 56 | numImuFrames: 3 # number of frames linked by most recent nonlinear IMU error terms 57 | 58 | # ceres optimization options 59 | ceres_options: 60 | minIterations: 3 # minimum number of iterations always performed 61 | maxIterations: 10 # never do more than these, even if not converged 62 | timeLimit: 0.035 # [s] negative values will set the an unlimited time limit 63 | 64 | # detection 65 | detection_options: 66 | threshold: 40.0 # detection threshold. By default the uniformity radius in pixels 67 | octaves: 0 # number of octaves for detection. 0 means single-scale at highest resolution 68 | maxNoKeypoints: 400 # restrict to a maximum of this many keypoints per image (strongest ones) 69 | 70 | # delay of images [s]: 71 | imageDelay: 0.0 # in case you are using a custom setup, you will have to calibrate this. 0 for the VISensor. 72 | 73 | # display debug images? 74 | displayImages: true # displays debug video and keyframe matches. May be slow. 75 | 76 | # use direct driver 77 | useDriver: true 78 | 79 | # some options for how and what to publish -- optional in ROS-free version 80 | publishing_options: 81 | publish_rate: 200 # rate at which odometry updates are published only works properly if imu_rate/publish_rate is an integer!! 82 | publishLandmarks: treu # select, if you want to publish landmarks at all 83 | landmarkQualityThreshold: 1.0e-2 # landmark with lower quality will not be published 84 | maximumLandmarkQuality: 0.05 # landmark with higher quality will be published with the maximum colour intensity 85 | maxPathLength: 20 # maximum length of the published path 86 | publishImuPropagatedState: true # Should the state that is propagated with IMU messages be published? Or just the optimized ones? 87 | # provide custom World frame Wc 88 | T_Wc_W: 89 | [1.0000, 0.0000, 0.0000, 0.0000, 90 | 0.0000, 1.0000, 0.0000, 0.0000, 91 | 0.0000, 0.0000, 1.0000, 0.0000, 92 | 0.0000, 0.0000, 0.0000, 1.0000] 93 | trackedBodyFrame: B # B or S, the frame of reference that will be expressed relative to the selected worldFrame 94 | velocitiesFrame: Wc # Wc, B or S, the frames in which the velocities of the selected trackedBodyFrame will be expressed in 95 | 96 | -------------------------------------------------------------------------------- /contributors.txt: -------------------------------------------------------------------------------- 1 | Stefan Leutenegger 2 | -------------------------------------------------------------------------------- /okvis_ceres/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | project(okvis_ceres) 3 | 4 | # require Eigen 5 | find_package( Eigen REQUIRED ) 6 | include_directories(${EIGEN_INCLUDE_DIR}) 7 | 8 | # build the library 9 | add_library(${PROJECT_NAME} 10 | src/PoseParameterBlock.cpp 11 | src/SpeedAndBiasParameterBlock.cpp 12 | src/HomogeneousPointParameterBlock.cpp 13 | src/HomogeneousPointLocalParameterization.cpp 14 | src/PoseLocalParameterization.cpp 15 | src/ImuError.cpp 16 | src/PoseError.cpp 17 | src/RelativePoseError.cpp 18 | src/SpeedAndBiasError.cpp 19 | src/IdProvider.cpp 20 | src/Map.cpp 21 | src/MarginalizationError.cpp 22 | src/HomogeneousPointError.cpp 23 | src/Estimator.cpp 24 | src/LocalParamizationAdditionalInterfaces.cpp 25 | include/okvis/Estimator.hpp 26 | include/okvis/ceres/CeresIterationCallback.hpp 27 | ) 28 | 29 | # and link it 30 | target_link_libraries(${PROJECT_NAME} 31 | PUBLIC okvis_util 32 | PUBLIC okvis_cv 33 | PUBLIC okvis_common 34 | PRIVATE ${CERES_LIBRARIES} 35 | PRIVATE ${OpenCV_LIBRARIES} 36 | ) 37 | 38 | # installation if required 39 | install(TARGETS ${PROJECT_NAME} 40 | EXPORT okvisTargets 41 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 42 | ) 43 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 44 | 45 | # testing 46 | if(BUILD_TESTS) 47 | if(APPLE) 48 | add_definitions(-DGTEST_HAS_TR1_TUPLE=1) 49 | else() 50 | add_definitions(-DGTEST_HAS_TR1_TUPLE=0) 51 | endif(APPLE) 52 | enable_testing() 53 | set(PROJECT_TEST_NAME ${PROJECT_NAME}_test) 54 | add_executable(${PROJECT_TEST_NAME} 55 | test/test_main.cpp 56 | test/TestEstimator.cpp 57 | test/TestHomogeneousPointError.cpp 58 | test/TestReprojectionError.cpp 59 | test/TestImuError.cpp 60 | test/TestMap.cpp 61 | test/TestMarginalization.cpp 62 | ) 63 | target_link_libraries(${PROJECT_TEST_NAME} 64 | ${PROJECT_NAME} 65 | ${GTEST_LIBRARY} 66 | pthread) 67 | add_test(test ${PROJECT_TEST_NAME}) 68 | endif() 69 | -------------------------------------------------------------------------------- /okvis_ceres/include/okvis/IdProvider.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Mar 10, 2013 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file IdProvider.hpp 35 | * @brief Header file for the IdProvider struct emulating a singleton. This provides 36 | Unique IDs. 37 | * @author Stefan Leutenegger 38 | */ 39 | 40 | #ifndef INCLUDE_OKVIS_IDPROVIDER_HPP_ 41 | #define INCLUDE_OKVIS_IDPROVIDER_HPP_ 42 | 43 | #include 44 | #include 45 | #include 46 | 47 | /// \brief okvis Main namespace of this package. 48 | namespace okvis { 49 | 50 | /// \brief Provides IDs. 51 | namespace IdProvider { 52 | // emulating singleton syntax 53 | struct instance 54 | { 55 | /// \brief get a unique new ID. 56 | static uint64_t newId(); 57 | 58 | /** 59 | * \brief Get the last generated ID. 60 | * \warning Use with caution. This ID is almost surely in use. 61 | */ 62 | static uint64_t currentId(); 63 | }; 64 | 65 | } // namespace IdProvider 66 | } // namespace okvis 67 | 68 | #endif /* INCLUDE_OKVIS_IDPROVIDER_HPP_ */ 69 | -------------------------------------------------------------------------------- /okvis_ceres/include/okvis/ceres/CeresIterationCallback.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: May 22, 2015 30 | * Author: Andreas Forster (an.forster@gmail.com) 31 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 32 | *********************************************************************************/ 33 | 34 | /** 35 | * @file CeresIterationCallback.hpp 36 | * @brief Header file for the CeresIterationCallback class. 37 | * Used to enforce a time limit on the ceres optimization. 38 | * @author Andreas Forster 39 | * @author Stefan Leutenegger 40 | */ 41 | 42 | #ifndef INCLUDE_OKVIS_CERES_CERESITERATIONCALLBACK_HPP 43 | #define INCLUDE_OKVIS_CERES_CERESITERATIONCALLBACK_HPP 44 | 45 | #include 46 | 47 | /// \brief okvis Main namespace of this package. 48 | namespace okvis { 49 | /// \brief ceres Namespace for ceres-related functionality implemented in okvis. 50 | namespace ceres { 51 | 52 | /** 53 | * @brief The CeresIterationCallback class tries to enforce a time limit on the 54 | * optimization. It does not guarantee to stay within the time budget as 55 | * it assumes the next iteration takes as long as the previous iteration. 56 | */ 57 | class CeresIterationCallback : public ::ceres::IterationCallback { 58 | public: 59 | 60 | /** 61 | * @brief The constructor. 62 | * @param[in] timeLimit Time budget for the optimization. 63 | * @param[in] iterationMinimum Minimum iterations the optimization should perform 64 | * disregarding the time. 65 | */ 66 | CeresIterationCallback(double timeLimit, int iterationMinimum) 67 | : timeLimit_(timeLimit), 68 | iterationMinimum_(iterationMinimum) { 69 | } 70 | 71 | /// \brief Trivial Destructor. 72 | ~CeresIterationCallback() { 73 | } 74 | 75 | /// @brief This method is called after every iteration in ceres. 76 | /// @param[in] summary The iteration summary. 77 | ::ceres::CallbackReturnType operator()( 78 | const ::ceres::IterationSummary& summary) { 79 | // assume next iteration takes the same time as current iteration 80 | if (summary.iteration >= iterationMinimum_ 81 | && summary.cumulative_time_in_seconds 82 | + summary.iteration_time_in_seconds > timeLimit_) { 83 | return ::ceres::SOLVER_TERMINATE_SUCCESSFULLY; 84 | } 85 | return ::ceres::SOLVER_CONTINUE; 86 | } 87 | 88 | /** 89 | * @brief setTimeLimit changes time limit of optimization. 90 | * If you want to disable the time limit, either set it to a large value, 91 | * delete the callback in the ceres options or set the minimum iterations 92 | * to the maximum iteration. 93 | * @param[in] timeLimit desired time limit in seconds 94 | */ 95 | void setTimeLimit(double timeLimit) { 96 | timeLimit_ = timeLimit; 97 | } 98 | 99 | /** 100 | * @brief setMinimumIterations changes the minimum iterations the optimization 101 | * goes through disregarding the time limit 102 | * @param iterationMinimum 103 | */ 104 | void setMinimumIterations(int iterationMinimum) { 105 | iterationMinimum_ = iterationMinimum; 106 | } 107 | 108 | private: 109 | double timeLimit_; ///< The set time limit. 110 | int iterationMinimum_; ///< The set maximum no. iterations. 111 | }; 112 | 113 | } // namespace ceres 114 | } // namespace okvis 115 | 116 | #endif /* INCLUDE_OKVIS_CERES_CERESITERATIONCALLBACK_HPP */ 117 | -------------------------------------------------------------------------------- /okvis_ceres/include/okvis/ceres/ErrorInterface.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Sep 12, 2013 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file ErrorInterface.hpp 35 | * @brief Header file for the ErrorInterface class. A simple interface class that 36 | other error classes should inherit from. 37 | * @author Stefan Leutenegger 38 | */ 39 | 40 | #ifndef INCLUDE_OKVIS_CERES_ERRORINTERFACE_HPP_ 41 | #define INCLUDE_OKVIS_CERES_ERRORINTERFACE_HPP_ 42 | 43 | #include 44 | #include 45 | #include 46 | 47 | /// \brief okvis Main namespace of this package. 48 | namespace okvis { 49 | /// \brief ceres Namespace for ceres-related functionality implemented in okvis. 50 | namespace ceres { 51 | 52 | /// @brief Simple interface class the errors implemented here should inherit from. 53 | class ErrorInterface { 54 | public: 55 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 56 | OKVIS_DEFINE_EXCEPTION(Exception, std::runtime_error) 57 | 58 | /// @brief Constructor 59 | ErrorInterface() { 60 | } 61 | /// @brief Destructor (does nothing). 62 | virtual ~ErrorInterface() { 63 | } 64 | 65 | /// @name Sizes 66 | /// @{ 67 | 68 | /// @brief Get dimension of residuals. 69 | /// @return The residual dimension. 70 | virtual size_t residualDim() const = 0; 71 | 72 | /// @brief Get the number of parameter blocks this is connected to. 73 | /// @return The number of parameter blocks. 74 | virtual size_t parameterBlocks() const = 0; 75 | 76 | /** 77 | * @brief get the dimension of a parameter block this is connected to. 78 | * @param parameterBlockId The ID of the parameter block of interest. 79 | * @return Its dimension. 80 | */ 81 | virtual size_t parameterBlockDim(size_t parameterBlockId) const = 0; 82 | 83 | /// @} 84 | // Error and Jacobian computation 85 | /** 86 | * @brief This evaluates the error term and additionally computes 87 | * the Jacobians in the minimal internal representation. 88 | * @param parameters Pointer to the parameters (see ceres) 89 | * @param residuals Pointer to the residual vector (see ceres) 90 | * @param jacobians Pointer to the Jacobians (see ceres) 91 | * @param jacobiansMinimal Pointer to the minimal Jacobians (equivalent to jacobians). 92 | * @return Success of the evaluation. 93 | */ 94 | virtual bool EvaluateWithMinimalJacobians( 95 | double const* const * parameters, double* residuals, double** jacobians, 96 | double** jacobiansMinimal) const = 0; 97 | 98 | /// @brief Residual block type as string 99 | virtual std::string typeInfo() const = 0; 100 | }; 101 | 102 | } // namespace ceres 103 | } // namespace okvis 104 | 105 | #endif /* INCLUDE_OKVIS_CERES_ERRORINTERFACE_HPP_ */ 106 | -------------------------------------------------------------------------------- /okvis_ceres/include/okvis/ceres/LocalParamizationAdditionalInterfaces.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Feb 2, 2014 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file LocalParamizationAdditionalInterfaces.hpp 35 | * @brief Header file for the LocalParamizationAdditionalInterfaces class. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #ifndef INCLUDE_OKVIS_CERES_LOCALPARAMIZATIONADDITIONALINTERFACES_HPP_ 40 | #define INCLUDE_OKVIS_CERES_LOCALPARAMIZATIONADDITIONALINTERFACES_HPP_ 41 | 42 | #include "ceres/ceres.h" 43 | #include 44 | 45 | /// \brief okvis Main namespace of this package. 46 | namespace okvis { 47 | /// \brief ceres Namespace for ceres-related functionality implemented in okvis. 48 | namespace ceres { 49 | 50 | /// \brief Provides some additional interfaces to ceres' LocalParamization 51 | /// than are needed in the generic marginalisation okvis::ceres::MarginalizationError. 52 | class LocalParamizationAdditionalInterfaces { 53 | public: 54 | 55 | /// \brief Trivial destructor. 56 | virtual ~LocalParamizationAdditionalInterfaces() { 57 | } 58 | 59 | /// \brief Computes the minimal difference between a variable x and a perturbed variable x_plus_delta 60 | /// @param[in] x Variable. 61 | /// @param[in] x_plus_delta Perturbed variable. 62 | /// @param[out] delta minimal difference. 63 | /// \return True on success. 64 | virtual bool Minus(const double* x, const double* x_plus_delta, 65 | double* delta) const = 0; 66 | 67 | /// \brief Computes the Jacobian from minimal space to naively overparameterised space as used by ceres. 68 | /// @param[in] x Variable. 69 | /// @param[out] jacobian the Jacobian (dimension minDim x dim). 70 | /// \return True on success. 71 | virtual bool ComputeLiftJacobian(const double* x, double* jacobian) const = 0; 72 | 73 | /// \brief Verifies the correctness of an inplementation by means of numeric Jacobians. 74 | /// @param[in] x_raw Linearisation point of the variable. 75 | /// @param[in] purturbation_magnitude Magnitude of the delta used for numeric Jacobians. 76 | /// \return True on success. 77 | virtual bool verify(const double* x_raw, double purturbation_magnitude = 1.0e-6) const ; 78 | }; 79 | 80 | } // namespace ceres 81 | } // namespace okvis 82 | 83 | #endif /* INCLUDE_OKVIS_CERES_LOCALPARAMIZATIONADDITIONALINTERFACES_HPP_ */ 84 | -------------------------------------------------------------------------------- /okvis_ceres/include/okvis/ceres/ParameterBlockSized.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Sep 8, 2013 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file ParameterBlockSized.hpp 35 | * @brief Header file for the ParameterBlockSized class. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #ifndef INCLUDE_OKVIS_CERES_PARAMETERBLOCKSIZED_HPP_ 40 | #define INCLUDE_OKVIS_CERES_PARAMETERBLOCKSIZED_HPP_ 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | /// \brief okvis Main namespace of this package. 50 | namespace okvis { 51 | /// \brief ceres Namespace for ceres-related functionality implemented in okvis. 52 | namespace ceres { 53 | 54 | /// @brief Base class providing the interface for parameter blocks. 55 | /// @tparam Dim Dimension of parameter block 56 | /// @tparam MinDim Minimal dimension of parameter block 57 | /// @tparam T The type of the estimate 58 | template 59 | class ParameterBlockSized : public okvis::ceres::ParameterBlock { 60 | public: 61 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 62 | OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error) 63 | 64 | /// @brief Dimension of the parameter block. 65 | static const int Dimension = Dim; 66 | 67 | /// @brief Internal (minimal) dimension. 68 | static const int MinimalDimension = MinDim; 69 | 70 | /// \brief Make the parameter type accessible. 71 | typedef T parameter_t; 72 | 73 | /// \brief Default constructor -- initialises elements in parametes_ to zero. 74 | ParameterBlockSized() { 75 | for (int i = 0; i < Dimension; ++i) 76 | parameters_[i] = 0; 77 | } 78 | 79 | /// \brief Trivial destructor. 80 | virtual ~ParameterBlockSized() { 81 | } 82 | 83 | /// @name Setters 84 | /// @{ 85 | 86 | /// @brief Set estimate of this parameter block. 87 | /// @param[in] estimate The estimate to set this to. 88 | virtual void setEstimate(const parameter_t& estimate)=0; 89 | 90 | /// @brief Set exact parameters of this parameter block. 91 | /// @param[in] parameters The parameters to set this to. 92 | virtual void setParameters(const double* parameters) { 93 | OKVIS_ASSERT_TRUE_DBG(Exception, parameters != 0, "Null pointer"); 94 | memcpy(parameters_, parameters, Dimension * sizeof(double)); 95 | } 96 | 97 | /// @} 98 | 99 | /// @name Getters 100 | /// @{ 101 | 102 | /// @brief Get estimate. 103 | /// \return The estimate. 104 | virtual parameter_t estimate() const = 0; 105 | 106 | /// @brief Get parameters -- as a pointer. 107 | /// \return Pointer to the parameters allocated in here. 108 | virtual double* parameters() { 109 | return parameters_; 110 | } 111 | 112 | /// @brief Get parameters -- as a pointer. 113 | /// \return Pointer to the parameters allocated in here. 114 | virtual const double* parameters() const { 115 | return parameters_; 116 | } 117 | 118 | /// @brief Get the parameter dimension. 119 | /// \return The parameter dimension. 120 | virtual size_t dimension() const { 121 | return Dimension; 122 | } 123 | 124 | /// @brief Get the internal minimal parameter dimension. 125 | /// \return The internal minimal parameter dimension. 126 | virtual size_t minimalDimension() const { 127 | return MinimalDimension; 128 | } 129 | 130 | /// @} 131 | 132 | /// @name File read/write - implement in derived class, if needed 133 | /// @{ 134 | /// \brief Reading from file -- not implemented 135 | virtual bool read(std::istream& /*not implemented: is*/) { 136 | return false; 137 | } 138 | 139 | /// \brief Writing to file -- not implemented 140 | virtual bool write(std::ostream& /*not implemented: os*/) const { 141 | return false; 142 | } 143 | /// @} 144 | 145 | protected: 146 | /// @brief Parameters 147 | double parameters_[Dimension]; 148 | }; 149 | 150 | } // namespace ceres 151 | } // namespace okvis 152 | 153 | #endif /* INCLUDE_OKVIS_CERES_PARAMETERBLOCKSIZED_HPP_ */ 154 | -------------------------------------------------------------------------------- /okvis_ceres/include/okvis/ceres/ReprojectionErrorBase.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Jan 4, 2014 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file ReprojectionErrorBase.hpp 35 | * @brief Header file for the ReprojectionErrorBase class. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #ifndef INCLUDE_OKVIS_CERES_REPROJECTIONERRORBASE_HPP_ 40 | #define INCLUDE_OKVIS_CERES_REPROJECTIONERRORBASE_HPP_ 41 | 42 | #include "ceres/ceres.h" 43 | 44 | /// \brief okvis Main namespace of this package. 45 | namespace okvis { 46 | /// \brief ceres Namespace for ceres-related functionality implemented in okvis. 47 | namespace ceres { 48 | 49 | /// \brief Reprojection error base class. 50 | class ReprojectionErrorBase : 51 | public ::ceres::SizedCostFunction<2 /* number of residuals */, 52 | 7 /* size of first parameter */, 4 /* size of second parameter */, 7 /* size of third parameter (camera extrinsics) */>, 53 | public ErrorInterface { 54 | public: 55 | 56 | /// \brief Camera ID. 57 | uint64_t cameraId() const { 58 | return cameraId_; 59 | } 60 | 61 | /// \brief Set camera ID. 62 | /// @param[in] cameraId ID of the camera. 63 | void setCameraId(uint64_t cameraId) { 64 | cameraId_ = cameraId; 65 | } 66 | 67 | uint64_t cameraId_; ///< ID of the camera. 68 | }; 69 | 70 | /// \brief 2D keypoint reprojection error base class. 71 | class ReprojectionError2dBase : public ReprojectionErrorBase { 72 | public: 73 | 74 | /// \brief Measurement type (2D). 75 | typedef Eigen::Vector2d measurement_t; 76 | 77 | /// \brief Covariance / information matrix type (2x2). 78 | typedef Eigen::Matrix2d covariance_t; 79 | 80 | /// \brief Set the measurement. 81 | /// @param[in] measurement The measurement vector (2d). 82 | virtual void setMeasurement(const measurement_t& measurement) = 0; 83 | 84 | /// \brief Set the information. 85 | /// @param[in] information The information (weight) matrix (2x2). 86 | virtual void setInformation(const covariance_t& information) = 0; 87 | 88 | // getters 89 | /// \brief Get the measurement. 90 | /// \return The measurement vector (2d). 91 | virtual const measurement_t& measurement() const = 0; 92 | 93 | /// \brief Get the information matrix. 94 | /// \return The information (weight) matrix (2x2). 95 | virtual const covariance_t& information() const = 0; 96 | 97 | /// \brief Get the covariance matrix. 98 | /// \return The inverse information (covariance) matrix. 99 | virtual const covariance_t& covariance() const = 0; 100 | 101 | }; 102 | 103 | } 104 | 105 | } 106 | 107 | #endif /* INCLUDE_OKVIS_CERES_REPROJECTIONERRORBASE_HPP_ */ 108 | -------------------------------------------------------------------------------- /okvis_ceres/include/okvis/implementation/Estimator.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Jan 10, 2015 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file implementation/Estimator.hpp 35 | * @brief Header implementation file for the Estimator class. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | /// \brief okvis Main namespace of this package. 40 | namespace okvis { 41 | 42 | // Add an observation to a landmark. 43 | template 44 | ::ceres::ResidualBlockId Estimator::addObservation(uint64_t landmarkId, 45 | uint64_t poseId, 46 | size_t camIdx, 47 | size_t keypointIdx) { 48 | OKVIS_ASSERT_TRUE_DBG(Exception, isLandmarkAdded(landmarkId), 49 | "landmark not added"); 50 | 51 | // avoid double observations 52 | okvis::KeypointIdentifier kid(poseId, camIdx, keypointIdx); 53 | if (landmarksMap_.at(landmarkId).observations.find(kid) 54 | != landmarksMap_.at(landmarkId).observations.end()) { 55 | return NULL; 56 | } 57 | 58 | // get the keypoint measurement 59 | okvis::MultiFramePtr multiFramePtr = multiFramePtrMap_.at(poseId); 60 | Eigen::Vector2d measurement; 61 | multiFramePtr->getKeypoint(camIdx, keypointIdx, measurement); 62 | Eigen::Matrix2d information = Eigen::Matrix2d::Identity(); 63 | double size = 1.0; 64 | multiFramePtr->getKeypointSize(camIdx, keypointIdx, size); 65 | information *= 64.0 / (size * size); 66 | 67 | // create error term 68 | std::shared_ptr < ceres::ReprojectionError 69 | < GEOMETRY_TYPE 70 | >> reprojectionError( 71 | new ceres::ReprojectionError( 72 | multiFramePtr->template geometryAs(camIdx), 73 | camIdx, measurement, information)); 74 | 75 | ::ceres::ResidualBlockId retVal = mapPtr_->addResidualBlock( 76 | reprojectionError, 77 | cauchyLossFunctionPtr_ ? cauchyLossFunctionPtr_.get() : NULL, 78 | mapPtr_->parameterBlockPtr(poseId), 79 | mapPtr_->parameterBlockPtr(landmarkId), 80 | mapPtr_->parameterBlockPtr( 81 | statesMap_.at(poseId).sensors.at(SensorStates::Camera).at(camIdx).at( 82 | CameraSensorStates::T_SCi).id)); 83 | 84 | // remember 85 | landmarksMap_.at(landmarkId).observations.insert( 86 | std::pair( 87 | kid, reinterpret_cast(retVal))); 88 | 89 | return retVal; 90 | } 91 | 92 | } // namespace okvis 93 | -------------------------------------------------------------------------------- /okvis_ceres/src/HomogeneousPointParameterBlock.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Aug 30, 2013 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file HomogeneousPointParameterBlock.cpp 35 | * @brief Source file for the HomogeneousPointParameterBlock class. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #include 40 | 41 | /// \brief okvis Main namespace of this package. 42 | namespace okvis { 43 | /// \brief ceres Namespace for ceres-related functionality implemented in okvis. 44 | namespace ceres { 45 | 46 | // Default constructor (assumes not fixed). 47 | HomogeneousPointParameterBlock::HomogeneousPointParameterBlock() 48 | : base_t::ParameterBlockSized(), 49 | initialized_(false) { 50 | setFixed(false); 51 | } 52 | // Trivial destructor. 53 | HomogeneousPointParameterBlock::~HomogeneousPointParameterBlock() { 54 | } 55 | 56 | // Constructor with estimate and time. 57 | HomogeneousPointParameterBlock::HomogeneousPointParameterBlock( 58 | const Eigen::Vector4d& point, uint64_t id, bool initialized) { 59 | setEstimate(point); 60 | setId(id); 61 | setInitialized(initialized); 62 | setFixed(false); 63 | } 64 | 65 | // Constructor with estimate and time. 66 | HomogeneousPointParameterBlock::HomogeneousPointParameterBlock( 67 | const Eigen::Vector3d& point, uint64_t id, bool initialized) { 68 | setEstimate(Eigen::Vector4d(point[0], point[1], point[2], 1.0)); 69 | setId(id); 70 | setInitialized(initialized); 71 | setFixed(false); 72 | } 73 | 74 | // setters 75 | // Set estimate of this parameter block. 76 | void HomogeneousPointParameterBlock::setEstimate(const Eigen::Vector4d& point) { 77 | // hack: only do "Euclidean" points for now... 78 | for (int i = 0; i < base_t::Dimension; ++i) 79 | parameters_[i] = point[i]; 80 | } 81 | 82 | // getters 83 | // Get estimate. 84 | Eigen::Vector4d HomogeneousPointParameterBlock::estimate() const { 85 | return Eigen::Vector4d( 86 | Eigen::Vector4d(parameters_[0], parameters_[1], parameters_[2], 87 | parameters_[3])); 88 | } 89 | 90 | } // namespace ceres 91 | } // namespace okvis 92 | -------------------------------------------------------------------------------- /okvis_ceres/src/IdProvider.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Mar 10, 2013 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file IdProvider.cpp 35 | * @brief Source file for the IdProvider struct. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #include 40 | 41 | /// \brief okvis Main namespace of this package. 42 | namespace okvis { 43 | 44 | /// \brief Provides IDs. 45 | namespace IdProvider { 46 | std::atomic _id(0); 47 | 48 | // get a unique new ID. 49 | uint64_t instance::newId() { 50 | const uint64_t retVal = ++_id; 51 | return retVal; 52 | } 53 | 54 | // Get the last generated ID. 55 | uint64_t instance::currentId() { 56 | return _id; 57 | } 58 | } 59 | } 60 | -------------------------------------------------------------------------------- /okvis_ceres/src/LocalParamizationAdditionalInterfaces.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LocalParamizationAdditionalInterfaces.cpp 3 | * 4 | * Created on: 27 Jul 2015 5 | * Author: sleutene 6 | */ 7 | 8 | #include 9 | 10 | namespace okvis { 11 | namespace ceres { 12 | 13 | // Verifies the correctness of a inplementation. 14 | bool LocalParamizationAdditionalInterfaces::verify( 15 | const double* x_raw, double purturbation_magnitude) const 16 | { 17 | const ::ceres::LocalParameterization* casted = 18 | dynamic_cast(this); 19 | if (!casted) { 20 | return false; 21 | } 22 | // verify plus/minus 23 | Eigen::VectorXd x(casted->GlobalSize()); 24 | memcpy(x.data(), x_raw, sizeof(double) * casted->GlobalSize()); 25 | Eigen::VectorXd delta_x(casted->LocalSize()); 26 | Eigen::VectorXd x_plus_delta(casted->GlobalSize()); 27 | Eigen::VectorXd delta_x2(casted->LocalSize()); 28 | delta_x.setRandom(); 29 | delta_x *= purturbation_magnitude; 30 | casted->Plus(x.data(), delta_x.data(), x_plus_delta.data()); 31 | this->Minus(x.data(), x_plus_delta.data(), delta_x2.data()); 32 | if ((delta_x2 - delta_x).norm() > 1.0e-12) { 33 | return false; 34 | } 35 | 36 | // plusJacobian numDiff 37 | Eigen::Matrix J_plus_num_diff( 38 | casted->GlobalSize(), casted->LocalSize()); 39 | const double dx = 1.0e-9; 40 | for (int i = 0; i < casted->LocalSize(); ++i) { 41 | Eigen::VectorXd delta_p(casted->LocalSize()); 42 | delta_p.setZero(); 43 | delta_p[i] = dx; 44 | Eigen::VectorXd delta_m(casted->LocalSize()); 45 | delta_m.setZero(); 46 | delta_m[i] = -dx; 47 | 48 | // reset 49 | Eigen::VectorXd x_p(casted->GlobalSize()); 50 | Eigen::VectorXd x_m(casted->GlobalSize()); 51 | memcpy(x_p.data(), x_raw, sizeof(double) * casted->GlobalSize()); 52 | memcpy(x_m.data(), x_raw, sizeof(double) * casted->GlobalSize()); 53 | casted->Plus(x.data(), delta_p.data(), x_p.data()); 54 | casted->Plus(x.data(), delta_m.data(), x_m.data()); 55 | J_plus_num_diff.col(i) = (x_p - x_m) / (2 * dx); 56 | } 57 | 58 | // verify lift 59 | Eigen::Matrix J_plus(casted->GlobalSize(), 60 | casted->LocalSize()); 61 | Eigen::Matrix J_lift(casted->LocalSize(), 62 | casted->GlobalSize()); 63 | casted->ComputeJacobian(x_raw, J_plus.data()); 64 | ComputeLiftJacobian(x_raw, J_lift.data()); 65 | Eigen::MatrixXd identity(casted->LocalSize(), casted->LocalSize()); 66 | identity.setIdentity(); 67 | if (((J_lift * J_plus) - identity).norm() > 1.0e-6) { 68 | return false; 69 | } 70 | 71 | // verify numDiff jacobian 72 | if ((J_plus - J_plus_num_diff).norm() > 1.0e-6) { 73 | return false; 74 | } 75 | 76 | // everything fine... 77 | return true; 78 | } 79 | 80 | } // namespace ceres 81 | } // namespace okvis 82 | 83 | -------------------------------------------------------------------------------- /okvis_ceres/src/PoseParameterBlock.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Aug 30, 2013 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file PoseParameterBlock.cpp 35 | * @brief Source file for the PoseParameterBlock class. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #include 40 | 41 | /// \brief okvis Main namespace of this package. 42 | namespace okvis { 43 | /// \brief ceres Namespace for ceres-related functionality implemented in okvis. 44 | namespace ceres { 45 | 46 | // Default constructor (assumes not fixed). 47 | PoseParameterBlock::PoseParameterBlock() 48 | : base_t::ParameterBlockSized() { 49 | setFixed(false); 50 | } 51 | 52 | // Trivial destructor. 53 | PoseParameterBlock::~PoseParameterBlock() { 54 | } 55 | 56 | // Constructor with estimate and time. 57 | PoseParameterBlock::PoseParameterBlock( 58 | const okvis::kinematics::Transformation& T_WS, uint64_t id, 59 | const okvis::Time& timestamp) { 60 | setEstimate(T_WS); 61 | setId(id); 62 | setTimestamp(timestamp); 63 | setFixed(false); 64 | } 65 | 66 | // setters 67 | // Set estimate of this parameter block. 68 | void PoseParameterBlock::setEstimate( 69 | const okvis::kinematics::Transformation& T_WS) { 70 | const Eigen::Vector3d r = T_WS.r(); 71 | const Eigen::Vector4d q = T_WS.q().coeffs(); 72 | parameters_[0] = r[0]; 73 | parameters_[1] = r[1]; 74 | parameters_[2] = r[2]; 75 | parameters_[3] = q[0]; 76 | parameters_[4] = q[1]; 77 | parameters_[5] = q[2]; 78 | parameters_[6] = q[3]; 79 | } 80 | 81 | // getters 82 | // Get estimate. 83 | okvis::kinematics::Transformation PoseParameterBlock::estimate() const { 84 | return okvis::kinematics::Transformation( 85 | Eigen::Vector3d(parameters_[0], parameters_[1], parameters_[2]), 86 | Eigen::Quaterniond(parameters_[6], parameters_[3], parameters_[4], 87 | parameters_[5])); 88 | } 89 | 90 | } // namespace ceres 91 | } // namespace okvis 92 | -------------------------------------------------------------------------------- /okvis_ceres/src/SpeedAndBiasError.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Sep 10, 2013 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file SpeedAndBiasError.cpp 35 | * @brief Source file for the SpeedAndBiasError class. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #include 40 | 41 | /// \brief okvis Main namespace of this package. 42 | namespace okvis { 43 | /// \brief ceres Namespace for ceres-related functionality implemented in okvis. 44 | namespace ceres { 45 | 46 | // Construct with measurement and information matrix 47 | SpeedAndBiasError::SpeedAndBiasError(const okvis::SpeedAndBias & measurement, 48 | const information_t & information) { 49 | setMeasurement(measurement); 50 | setInformation(information); 51 | } 52 | 53 | // Construct with measurement and variance. 54 | SpeedAndBiasError::SpeedAndBiasError(const okvis::SpeedAndBiases& measurement, 55 | double speedVariance, 56 | double gyrBiasVariance, 57 | double accBiasVariance) { 58 | setMeasurement(measurement); 59 | 60 | information_t information; 61 | information.setZero(); 62 | information.topLeftCorner<3, 3>() = Eigen::Matrix3d::Identity() * 1.0 63 | / speedVariance; 64 | information.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity() * 1.0 65 | / gyrBiasVariance; 66 | information.bottomRightCorner<3, 3>() = Eigen::Matrix3d::Identity() * 1.0 67 | / accBiasVariance; 68 | 69 | setInformation(information); 70 | } 71 | 72 | // Set the information. 73 | void SpeedAndBiasError::setInformation(const information_t & information) { 74 | information_ = information; 75 | covariance_ = information.inverse(); 76 | // perform the Cholesky decomposition on order to obtain the correct error weighting 77 | Eigen::LLT lltOfInformation(information_); 78 | squareRootInformation_ = lltOfInformation.matrixL().transpose(); 79 | } 80 | 81 | // This evaluates the error term and additionally computes the Jacobians. 82 | bool SpeedAndBiasError::Evaluate(double const* const * parameters, 83 | double* residuals, double** jacobians) const { 84 | return EvaluateWithMinimalJacobians(parameters, residuals, jacobians, NULL); 85 | } 86 | 87 | // This evaluates the error term and additionally computes 88 | // the Jacobians in the minimal internal representation. 89 | bool SpeedAndBiasError::EvaluateWithMinimalJacobians( 90 | double const* const * parameters, double* residuals, double** jacobians, 91 | double** jacobiansMinimal) const { 92 | 93 | // compute error 94 | Eigen::Map estimate(parameters[0]); 95 | okvis::SpeedAndBias error = measurement_ - estimate; 96 | 97 | // weigh it 98 | Eigen::Map > weighted_error(residuals); 99 | weighted_error = squareRootInformation_ * error; 100 | 101 | // compute Jacobian - this is rather trivial in this case... 102 | if (jacobians != NULL) { 103 | if (jacobians[0] != NULL) { 104 | Eigen::Map > J0( 105 | jacobians[0]); 106 | J0 = -squareRootInformation_ * Eigen::Matrix::Identity(); 107 | } 108 | } 109 | if (jacobiansMinimal != NULL) { 110 | if (jacobiansMinimal[0] != NULL) { 111 | Eigen::Map > J0min( 112 | jacobiansMinimal[0]); 113 | J0min = -squareRootInformation_ * Eigen::Matrix::Identity(); 114 | } 115 | } 116 | 117 | return true; 118 | } 119 | 120 | } // namespace ceres 121 | } // namespace okvis 122 | -------------------------------------------------------------------------------- /okvis_ceres/src/SpeedAndBiasParameterBlock.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Aug 30, 2013 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file SpeedAndBiasParameterBlock.cpp 35 | * @brief Source file for the SpeedAndBiasParameterBlock class. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #include 40 | 41 | /// \brief okvis Main namespace of this package. 42 | namespace okvis { 43 | /// \brief ceres Namespace for ceres-related functionality implemented in okvis. 44 | namespace ceres { 45 | 46 | // Default constructor (assumes not fixed). 47 | SpeedAndBiasParameterBlock::SpeedAndBiasParameterBlock() 48 | : base_t::ParameterBlockSized() { 49 | setFixed(false); 50 | } 51 | 52 | // Trivial destructor. 53 | SpeedAndBiasParameterBlock::~SpeedAndBiasParameterBlock() { 54 | } 55 | 56 | // Constructor with estimate and time. 57 | SpeedAndBiasParameterBlock::SpeedAndBiasParameterBlock( 58 | const SpeedAndBias& speedAndBias, uint64_t id, 59 | const okvis::Time& timestamp) { 60 | setEstimate(speedAndBias); 61 | setId(id); 62 | setTimestamp(timestamp); 63 | setFixed(false); 64 | } 65 | 66 | // setters 67 | // Set estimate of this parameter block. 68 | void SpeedAndBiasParameterBlock::setEstimate(const SpeedAndBias& speedAndBias) { 69 | for (int i = 0; i < base_t::Dimension; ++i) 70 | parameters_[i] = speedAndBias[i]; 71 | } 72 | 73 | // getters 74 | // Get estimate. 75 | SpeedAndBias SpeedAndBiasParameterBlock::estimate() const { 76 | SpeedAndBias speedAndBias; 77 | for (int i = 0; i < base_t::Dimension; ++i) 78 | speedAndBias[i] = parameters_[i]; 79 | return speedAndBias; 80 | } 81 | 82 | } // namespace ceres 83 | } // namespace okvis 84 | -------------------------------------------------------------------------------- /okvis_ceres/test/TestHomogeneousPointError.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Dec 30, 2014 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | TEST(okvisTestSuite, HomogeneousPointError) { 49 | // initialize random number generator 50 | //srand((unsigned int) time(0)); // disabled: make unit tests deterministic... 51 | 52 | // Build the problem. 53 | okvis::ceres::Map map; 54 | 55 | OKVIS_DEFINE_EXCEPTION(Exception, std::runtime_error); 56 | 57 | for (size_t i = 0; i < 100; ++i) { 58 | // create point 59 | Eigen::Vector4d point; 60 | point.head<3>().setRandom(); 61 | point *= 100; 62 | point[3] = 1.0; 63 | 64 | // create parameter block 65 | std::shared_ptr homogeneousPointParameterBlock( 66 | new okvis::ceres::HomogeneousPointParameterBlock(point, i)); 67 | // add it as optimizable thing. 68 | map.addParameterBlock(homogeneousPointParameterBlock, 69 | okvis::ceres::Map::HomogeneousPoint); 70 | map.setParameterBlockVariable(i); 71 | 72 | // invent a point error 73 | std::shared_ptr homogeneousPointError( 74 | new okvis::ceres::HomogeneousPointError( 75 | homogeneousPointParameterBlock->estimate(), 0.1)); 76 | 77 | // add it 78 | ::ceres::ResidualBlockId id = map.addResidualBlock( 79 | homogeneousPointError, NULL, homogeneousPointParameterBlock); 80 | 81 | // disturb 82 | Eigen::Vector4d point_disturbed = point; 83 | point_disturbed.head<3>() += 0.2 * Eigen::Vector3d::Random(); 84 | homogeneousPointParameterBlock->setEstimate(point_disturbed); 85 | 86 | // check Jacobian 87 | OKVIS_ASSERT_TRUE(Exception, map.isJacobianCorrect(id), 88 | "Jacobian verification on homogeneous point error failed."); 89 | } 90 | 91 | // Run the solver! 92 | map.options.minimizer_progress_to_stdout = false; 93 | std::cout << "run the solver... " << std::endl; 94 | map.solve(); 95 | 96 | // print some infos about the optimization 97 | //std::cout << map.summary.BriefReport() << "\n"; 98 | 99 | // check convergence. this must converge to zero, since it is not an overdetermined system. 100 | OKVIS_ASSERT_TRUE( 101 | Exception, 102 | map.summary.final_cost < 1.0e-10, 103 | "No convergence. this must converge to zero, since it is not an overdetermined system."); 104 | } 105 | -------------------------------------------------------------------------------- /okvis_ceres/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Aug 30, 2013 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | #include 34 | #include "glog/logging.h" 35 | 36 | /// Run all the tests that were declared with TEST() 37 | int main(int argc, char **argv){ 38 | testing::InitGoogleTest(&argc, argv); 39 | google::InitGoogleLogging(argv[0]); 40 | return RUN_ALL_TESTS(); 41 | } 42 | -------------------------------------------------------------------------------- /okvis_common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | 3 | project(okvis_common) 4 | 5 | IF(libvisensor_FOUND) 6 | add_definitions(-DHAVE_LIBVISENSOR) 7 | MESSAGE(STATUS "Found libvisensor. Setting HAVE_LIBVISENSOR flag.") 8 | ENDIF() 9 | 10 | # require Eigen 11 | find_package( Eigen REQUIRED ) 12 | include_directories(${EIGEN_INCLUDE_DIR}) 13 | 14 | # build the library 15 | add_library(${PROJECT_NAME} STATIC 16 | src/VioInterface.cpp 17 | src/VioParametersReader.cpp 18 | include/okvis/FrameTypedefs.hpp 19 | include/okvis/Measurements.hpp 20 | include/okvis/Parameters.hpp 21 | include/okvis/Variables.hpp 22 | include/okvis/VioBackendInterface.hpp 23 | include/okvis/VioFrontendInterface.hpp 24 | include/okvis/VioInterface.hpp 25 | include/okvis/VioParametersReader.hpp) 26 | 27 | # and link it 28 | target_link_libraries(${PROJECT_NAME} 29 | PUBLIC okvis_util 30 | PUBLIC okvis_kinematics 31 | PUBLIC okvis_time 32 | PUBLIC okvis_cv) 33 | 34 | # installation if required 35 | install(TARGETS ${PROJECT_NAME} 36 | EXPORT okvisTargets 37 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 38 | ) 39 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 40 | -------------------------------------------------------------------------------- /okvis_common/include/okvis/Variables.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Apr 22, 2012 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file Variables.hpp 35 | * @brief This file contains some typedefs related to state variables. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #ifndef INCLUDE_OKVIS_VARIABLES_HPP_ 40 | #define INCLUDE_OKVIS_VARIABLES_HPP_ 41 | 42 | #include 43 | 44 | /// \brief okvis Main namespace of this package. 45 | namespace okvis { 46 | 47 | typedef Eigen::Matrix SpeedAndBias; 48 | 49 | typedef Eigen::Matrix Speed; 50 | 51 | typedef Eigen::Matrix GyroBias; 52 | typedef Eigen::Matrix AccBias; 53 | typedef Eigen::Matrix ImuBias; 54 | 55 | typedef Eigen::Matrix MagnetometerBias; 56 | typedef Eigen::Matrix MagnetometerWorldZBias; 57 | 58 | typedef Eigen::Matrix Wind; 59 | typedef Eigen::Matrix Qff; 60 | 61 | 62 | 63 | } // namespace okvis 64 | 65 | 66 | #endif /* INCLUDE_OKVIS_VARIABLES_HPP_ */ 67 | -------------------------------------------------------------------------------- /okvis_cv/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | 3 | project(okvis_cv) 4 | 5 | # require Eigen 6 | find_package( Eigen REQUIRED ) 7 | include_directories(${EIGEN_INCLUDE_DIR}) 8 | 9 | # require OpenCV 10 | find_package( OpenCV COMPONENTS core highgui imgproc features2d REQUIRED ) 11 | include_directories(BEFORE ${OpenCV_INCLUDE_DIRS}) 12 | 13 | # build the library 14 | add_library(${PROJECT_NAME} STATIC 15 | src/CameraBase.cpp 16 | src/NCameraSystem.cpp 17 | ) 18 | 19 | # and link it 20 | target_link_libraries(${PROJECT_NAME} 21 | PUBLIC okvis_util 22 | PUBLIC okvis_kinematics 23 | PUBLIC okvis_time 24 | PRIVATE ${OpenCV_LIBRARIES} 25 | PRIVATE ${BRISK_LIBRARIES} 26 | ) 27 | 28 | # installation if required 29 | install(TARGETS ${PROJECT_NAME} 30 | EXPORT okvisTargets 31 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 32 | ) 33 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 34 | 35 | # testing 36 | if(BUILD_TESTS) 37 | if(APPLE) 38 | add_definitions(-DGTEST_HAS_TR1_TUPLE=1) 39 | else() 40 | add_definitions(-DGTEST_HAS_TR1_TUPLE=0) 41 | endif(APPLE) 42 | enable_testing() 43 | set(PROJECT_TEST_NAME ${PROJECT_NAME}_test) 44 | add_executable(${PROJECT_TEST_NAME} 45 | test/runTests.cpp 46 | test/TestPinholeCamera.cpp 47 | test/TestFrame.cpp 48 | test/TestNCameraSystem.cpp 49 | test/TestMultiFrame.cpp 50 | ) 51 | target_link_libraries(${PROJECT_TEST_NAME} 52 | ${PROJECT_NAME} 53 | ${GTEST_LIBRARY} 54 | pthread) 55 | add_test(test ${PROJECT_TEST_NAME}) 56 | endif() 57 | -------------------------------------------------------------------------------- /okvis_cv/include/okvis/cameras/implementation/CameraBase.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Feb 3, 2015 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file implementation/CameraBase.hpp 35 | * @brief Header implementation file for the CameraBase class. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #include 40 | 41 | /// \brief okvis Main namespace of this package. 42 | namespace okvis { 43 | /// \brief cameras Namespace for camera-related functionality. 44 | namespace cameras { 45 | 46 | // Set the mask. It must be the same size as the image and 47 | bool CameraBase::setMask(const cv::Mat & mask) 48 | { 49 | // check type 50 | if (mask.type() != CV_8UC1) { 51 | return false; 52 | } 53 | // check size 54 | if (mask.rows != imageHeight_) { 55 | return false; 56 | } 57 | if (mask.cols != imageWidth_) { 58 | return false; 59 | } 60 | mask_ = mask; 61 | return true; 62 | } 63 | 64 | /// Was a nonzero mask set? 65 | bool CameraBase::removeMask() 66 | { 67 | mask_.resize(0); 68 | return true; 69 | } 70 | 71 | // Was a nonzero mask set? 72 | bool CameraBase::hasMask() const 73 | { 74 | return (mask_.data); 75 | } 76 | 77 | // Get the mask. 78 | const cv::Mat & CameraBase::mask() const 79 | { 80 | return mask_; 81 | } 82 | 83 | bool CameraBase::isMasked(const Eigen::Vector2d& imagePoint) const 84 | { 85 | if (!isInImage(imagePoint)) { 86 | return true; 87 | } 88 | if (!hasMask()) { 89 | return false; 90 | } 91 | return mask_.at(int(imagePoint[1]), int(imagePoint[0])); 92 | } 93 | 94 | // Check if the keypoint is in the image. 95 | bool CameraBase::isInImage(const Eigen::Vector2d& imagePoint) const 96 | { 97 | if (imagePoint[0] < 0.0 || imagePoint[1] < 0.0) { 98 | return false; 99 | } 100 | if (imagePoint[0] >= imageWidth_ || imagePoint[1] >= imageHeight_) { 101 | return false; 102 | } 103 | return true; 104 | } 105 | 106 | } // namespace cameras 107 | } // namespace okvis 108 | 109 | -------------------------------------------------------------------------------- /okvis_cv/src/CameraBase.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Feb 2, 2015 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file CameraBase.cpp 35 | * @brief Source file for the CameraBase class. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #include 40 | 41 | /// \brief okvis Main namespace of this package. 42 | namespace okvis { 43 | /// \brief cameras Namespace for camera-related functionality. 44 | namespace cameras { 45 | 46 | // Creates a random (uniform distribution) image point. 47 | Eigen::Vector2d CameraBase::createRandomImagePoint() const 48 | { 49 | // Uniform random sample in image coordinates. 50 | // Add safety boundary for later inaccurate backprojection 51 | Eigen::Vector2d outPoint = Eigen::Vector2d::Random(); 52 | outPoint += Eigen::Vector2d::Ones(); 53 | outPoint *= 0.5; 54 | outPoint[0] *= double(imageWidth_-0.022); 55 | outPoint[0] += 0.011; 56 | outPoint[1] *= double(imageHeight_-0.022); 57 | outPoint[1] += 0.011; 58 | return outPoint; 59 | } 60 | 61 | // Creates a random visible point in Euclidean coordinates. 62 | Eigen::Vector3d CameraBase::createRandomVisiblePoint(double minDist, 63 | double maxDist) const 64 | { 65 | // random image point first: 66 | Eigen::Vector2d imagePoint = createRandomImagePoint(); 67 | // now sample random depth: 68 | Eigen::Vector2d depth = Eigen::Vector2d::Random(); 69 | Eigen::Vector3d ray; 70 | backProject(imagePoint, &ray); 71 | ray.normalize(); 72 | ray *= (0.5 * (maxDist - minDist) * (depth[0] + 1.0) + minDist); // rescale and offset 73 | return ray; 74 | } 75 | 76 | // Creates a random visible point in homogeneous coordinates. 77 | Eigen::Vector4d CameraBase::createRandomVisibleHomogeneousPoint( 78 | double minDist, double maxDist) const 79 | { 80 | Eigen::Vector3d point = createRandomVisiblePoint(minDist, maxDist); 81 | return Eigen::Vector4d(point[0], point[1], point[2], 1.0); 82 | } 83 | 84 | } // namespace cameras 85 | } // namespace okvis 86 | -------------------------------------------------------------------------------- /okvis_cv/test/TestFrame.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Mar 31, 2015 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #pragma GCC diagnostic push 38 | #pragma GCC diagnostic ignored "-Wnon-virtual-dtor" 39 | #include 40 | #pragma GCC diagnostic pop 41 | #include "okvis/cameras/PinholeCamera.hpp" 42 | #include "okvis/cameras/NoDistortion.hpp" 43 | #include "okvis/cameras/RadialTangentialDistortion.hpp" 44 | #include "okvis/cameras/EquidistantDistortion.hpp" 45 | #include "okvis/Frame.hpp" 46 | 47 | TEST(Frame, functions) 48 | { 49 | 50 | // instantiate all possible versions of test cameras 51 | std::vector > cameras; 52 | cameras.push_back( 53 | okvis::cameras::PinholeCamera::createTestObject()); 54 | cameras.push_back( 55 | okvis::cameras::PinholeCamera::createTestObject()); 56 | cameras.push_back( 57 | okvis::cameras::PinholeCamera::createTestObject()); 58 | 59 | for (size_t c = 0; c < cameras.size(); ++c) { 60 | 61 | #ifdef __ARM_NEON__ 62 | std::shared_ptr detector( 63 | new brisk::BriskFeatureDetector(34, 2)); 64 | #else 65 | std::shared_ptr detector( 66 | new brisk::ScaleSpaceFeatureDetector( 67 | 34, 2, 800, 450)); 68 | #endif 69 | 70 | std::shared_ptr extractor( 71 | new cv::BriskDescriptorExtractor(true, false)); 72 | 73 | // create a stupid random image 74 | Eigen::Matrix eigenImage(752,480); 75 | eigenImage.setRandom(); 76 | cv::Mat image(480, 752, CV_8UC1, eigenImage.data()); 77 | okvis::Frame frame(image, cameras.at(c), detector, extractor); 78 | 79 | // run 80 | frame.detect(); 81 | frame.describe(); 82 | } 83 | } 84 | 85 | -------------------------------------------------------------------------------- /okvis_cv/test/TestMultiFrame.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Mar 31, 2015 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #pragma GCC diagnostic push 38 | #pragma GCC diagnostic ignored "-Wnon-virtual-dtor" 39 | #include 40 | #pragma GCC diagnostic pop 41 | #include "okvis/cameras/PinholeCamera.hpp" 42 | #include "okvis/cameras/NoDistortion.hpp" 43 | #include "okvis/cameras/RadialTangentialDistortion.hpp" 44 | #include "okvis/cameras/EquidistantDistortion.hpp" 45 | #include "okvis/MultiFrame.hpp" 46 | 47 | TEST(MulitFrame, functions) 48 | { 49 | 50 | // instantiate all possible versions of test cameras 51 | std::vector > cameras; 52 | std::vector distortions; 53 | cameras.push_back( 54 | okvis::cameras::PinholeCamera::createTestObject()); 55 | distortions.push_back(okvis::cameras::NCameraSystem::NoDistortion); 56 | cameras.push_back( 57 | okvis::cameras::PinholeCamera::createTestObject()); 58 | distortions.push_back(okvis::cameras::NCameraSystem::RadialTangential); 59 | cameras.push_back( 60 | okvis::cameras::PinholeCamera::createTestObject()); 61 | distortions.push_back(okvis::cameras::NCameraSystem::Equidistant); 62 | 63 | // the mounting transformations. The third one is opposite direction 64 | std::vector> T_SC; 65 | T_SC.push_back( 66 | std::shared_ptr( 67 | new okvis::kinematics::Transformation(Eigen::Vector3d(0.1, 0.1, 0.1), 68 | Eigen::Quaterniond(1, 0, 0, 0)))); 69 | T_SC.push_back( 70 | std::shared_ptr( 71 | new okvis::kinematics::Transformation( 72 | Eigen::Vector3d(0.1, -0.1, -0.1), Eigen::Quaterniond(1, 0, 0, 0)))); 73 | T_SC.push_back( 74 | std::shared_ptr( 75 | new okvis::kinematics::Transformation( 76 | Eigen::Vector3d(0.1, -0.1, -0.1), Eigen::Quaterniond(0, 0, 1, 0)))); 77 | 78 | okvis::cameras::NCameraSystem nCameraSystem(T_SC, cameras, distortions, true); // computes overlaps 79 | okvis::MultiFrame multiFrame(nCameraSystem, okvis::Time::now(), 1); 80 | 81 | for (size_t c = 0; c < cameras.size(); ++c) { 82 | //std::cout << "Testing MultiFrame with " << cameras.at(c)->type() << std::endl; 83 | 84 | #ifdef __ARM_NEON__ 85 | std::shared_ptr detector( 86 | new brisk::BriskFeatureDetector(34, 2)); 87 | #else 88 | std::shared_ptr detector( 89 | new brisk::ScaleSpaceFeatureDetector( 90 | 34, 2, 800, 450)); 91 | #endif 92 | 93 | std::shared_ptr extractor( 94 | new cv::BriskDescriptorExtractor(true, false)); 95 | 96 | // create a stupid random image 97 | Eigen::Matrix eigenImage( 98 | 752, 480); 99 | eigenImage.setRandom(); 100 | cv::Mat image(480, 752, CV_8UC1, eigenImage.data()); 101 | 102 | // setup multifrmae 103 | multiFrame.setDetector(c,detector); 104 | multiFrame.setExtractor(c,extractor); 105 | multiFrame.setImage(c,image); 106 | 107 | // run 108 | multiFrame.detect(c); 109 | multiFrame.describe(c); 110 | } 111 | } 112 | 113 | -------------------------------------------------------------------------------- /okvis_cv/test/TestNCameraSystem.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Mar 31, 2015 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #pragma GCC diagnostic push 38 | #pragma GCC diagnostic ignored "-Wnon-virtual-dtor" 39 | #include 40 | #pragma GCC diagnostic pop 41 | #include "okvis/cameras/PinholeCamera.hpp" 42 | #include "okvis/cameras/NoDistortion.hpp" 43 | #include "okvis/cameras/RadialTangentialDistortion.hpp" 44 | #include "okvis/cameras/EquidistantDistortion.hpp" 45 | #include "okvis/cameras/NCameraSystem.hpp" 46 | 47 | TEST(NCameraSystem, functions) 48 | { 49 | 50 | // instantiate all possible versions of test cameras 51 | std::vector > cameras; 52 | std::vector distortions; 53 | cameras.push_back( 54 | okvis::cameras::PinholeCamera::createTestObject()); 55 | distortions.push_back(okvis::cameras::NCameraSystem::NoDistortion); 56 | cameras.push_back( 57 | okvis::cameras::PinholeCamera::createTestObject()); 58 | distortions.push_back(okvis::cameras::NCameraSystem::RadialTangential); 59 | cameras.push_back( 60 | okvis::cameras::PinholeCamera::createTestObject()); 61 | distortions.push_back(okvis::cameras::NCameraSystem::Equidistant); 62 | 63 | // the mounting transformations. The third one is opposite direction 64 | std::vector> T_SC; 65 | T_SC.push_back( 66 | std::shared_ptr( 67 | new okvis::kinematics::Transformation(Eigen::Vector3d(0.1, 0.1, 0.1), 68 | Eigen::Quaterniond(1, 0, 0, 0)))); 69 | T_SC.push_back( 70 | std::shared_ptr( 71 | new okvis::kinematics::Transformation( 72 | Eigen::Vector3d(0.1, -0.1, -0.1), Eigen::Quaterniond(1, 0, 0, 0)))); 73 | T_SC.push_back( 74 | std::shared_ptr( 75 | new okvis::kinematics::Transformation( 76 | Eigen::Vector3d(0.1, -0.1, -0.1), Eigen::Quaterniond(0, 0, 1, 0)))); 77 | 78 | okvis::cameras::NCameraSystem nCameraSystem(T_SC, cameras, distortions, true); // computes overlaps 79 | 80 | // verify self overlaps 81 | OKVIS_ASSERT_TRUE(std::runtime_error, nCameraSystem.hasOverlap(0, 0), 82 | "No self overlap?"); 83 | OKVIS_ASSERT_TRUE(std::runtime_error, nCameraSystem.hasOverlap(1, 1), 84 | "No self overlap?"); 85 | OKVIS_ASSERT_TRUE(std::runtime_error, nCameraSystem.hasOverlap(2, 2), 86 | "No self overlap?"); 87 | 88 | // verify 0 and 1 overlap 89 | OKVIS_ASSERT_TRUE(std::runtime_error, nCameraSystem.hasOverlap(0, 1), 90 | "No overlap?"); 91 | OKVIS_ASSERT_TRUE(std::runtime_error, nCameraSystem.hasOverlap(1, 0), 92 | "No overlap?"); 93 | 94 | // verify 1 and 2 do not overlap 95 | OKVIS_ASSERT_TRUE(std::runtime_error, !nCameraSystem.hasOverlap(1, 2), 96 | "Overlap?"); 97 | OKVIS_ASSERT_TRUE(std::runtime_error, !nCameraSystem.hasOverlap(2, 1), 98 | "Overlap?"); 99 | 100 | // verify 0 and 2 do not overlap 101 | OKVIS_ASSERT_TRUE(std::runtime_error, !nCameraSystem.hasOverlap(0, 2), 102 | "Overlap?"); 103 | OKVIS_ASSERT_TRUE(std::runtime_error, !nCameraSystem.hasOverlap(2, 0), 104 | "Overlap?"); 105 | 106 | } 107 | 108 | -------------------------------------------------------------------------------- /okvis_cv/test/runTests.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Feb 3, 2015 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | #include 34 | #include "gtest/gtest.h" 35 | 36 | int main(int argc, char **argv) { 37 | testing::InitGoogleTest(&argc, argv); 38 | return RUN_ALL_TESTS(); 39 | } 40 | 41 | -------------------------------------------------------------------------------- /okvis_frontend/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | project(okvis_frontend) 3 | 4 | if(NOT DO_TIMING) 5 | add_definitions(-DDEACTIVATE_TIMERS) 6 | message(STATUS "Deactivating timers.") 7 | endif() 8 | 9 | # require Eigen 10 | find_package( Eigen REQUIRED ) 11 | include_directories(${EIGEN_INCLUDE_DIR}) 12 | 13 | # build the library 14 | add_library(${PROJECT_NAME} 15 | src/Frontend.cpp 16 | src/VioKeyframeWindowMatchingAlgorithm.cpp 17 | src/stereo_triangulation.cpp 18 | src/ProbabilisticStereoTriangulator.cpp 19 | src/FrameNoncentralAbsoluteAdapter.cpp 20 | src/FrameRelativeAdapter.cpp 21 | include/okvis/Frontend.hpp 22 | include/okvis/VioKeyframeWindowMatchingAlgorithm.hpp 23 | include/okvis/triangulation/stereo_triangulation.hpp 24 | include/okvis/triangulation/ProbabilisticStereoTriangulator.hpp 25 | include/opengv/absolute_pose/FrameNoncentralAbsoluteAdapter.hpp 26 | include/opengv/relative_pose/FrameRelativeAdapter.hpp 27 | include/opengv/sac_problems/absolute_pose/FrameAbsolutePoseSacProblem.hpp 28 | include/opengv/sac_problems/relative_pose/FrameRelativePoseSacProblem.hpp 29 | include/opengv/sac_problems/relative_pose/FrameRotationOnlySacProblem.hpp) 30 | 31 | # and link it 32 | target_link_libraries(${PROJECT_NAME} 33 | PRIVATE ${BRISK_LIBRARIES} 34 | PRIVATE ${OpenGV_LIBRARIES} 35 | PRIVATE ${CERES_LIBRARIES} 36 | PUBLIC okvis_util 37 | PUBLIC okvis_cv 38 | PUBLIC okvis_ceres 39 | PUBLIC okvis_timing 40 | PUBLIC okvis_matcher) 41 | 42 | # installation if required 43 | install(TARGETS ${PROJECT_NAME} 44 | EXPORT okvisTargets 45 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 46 | ) 47 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 48 | -------------------------------------------------------------------------------- /okvis_frontend/include/okvis/triangulation/stereo_triangulation.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Mar 10, 2013 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file stereo_triangulation.hpp 35 | * @brief File containing the triangulateFast method definition. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | 40 | #ifndef INCLUDE_OKVIS_TRIANGULATION_STEREO_TRIANGULATION_HPP_ 41 | #define INCLUDE_OKVIS_TRIANGULATION_STEREO_TRIANGULATION_HPP_ 42 | 43 | #include 44 | 45 | /// \brief okvis Main namespace of this package. 46 | namespace okvis { 47 | /// \brief 48 | namespace triangulation { 49 | 50 | /** 51 | * @brief Triangulate the intersection of two rays. 52 | * @warning The rays e1 and e2 need to be normalized! 53 | * @param[in] p1 Camera center position of frame A in coordinate frame A 54 | * @param[in] e1 Ray through keypoint of frame A in coordinate frame A. 55 | * @param[in] p2 Camera center position of frame B in coordinate frame A. 56 | * @param[in] e2 Ray through keypoint of frame B in coordinate frame A. 57 | * @param[in] sigma Ray uncertainty. 58 | * @param[out] isValid Is the triangulation valid. 59 | * @param[out] isParallel Are the rays parallel? 60 | * @return Homogeneous coordinates of triangulated point. 61 | */ 62 | Eigen::Vector4d triangulateFast(const Eigen::Vector3d & p1, 63 | const Eigen::Vector3d & e1, 64 | const Eigen::Vector3d & p2, 65 | const Eigen::Vector3d & e2, double sigma, 66 | bool & isValid, bool & isParallel); 67 | } 68 | } 69 | 70 | #endif /* INCLUDE_OKVIS_TRIANGULATION_STEREO_TRIANGULATION_HPP_ */ 71 | -------------------------------------------------------------------------------- /okvis_frontend/src/stereo_triangulation.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Mar 10, 2013 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file stereo_triangulation.cpp 35 | * @brief Implementation of the triangulateFast function. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | /// \brief okvis Main namespace of this package. 45 | namespace okvis { 46 | 47 | /// \brief triangulation A namespace for operations related to triangulation. 48 | namespace triangulation { 49 | 50 | // Triangulate the intersection of two rays. 51 | Eigen::Vector4d triangulateFast(const Eigen::Vector3d& p1, 52 | const Eigen::Vector3d& e1, 53 | const Eigen::Vector3d& p2, 54 | const Eigen::Vector3d& e2, double sigma, 55 | bool& isValid, bool& isParallel) { 56 | isParallel = false; // This should be the default. 57 | // But parallel and invalid is not the same. Points at infinity are valid and parallel. 58 | isValid = false; // hopefully this will be reset to true. 59 | 60 | // stolen and adapted from the Kneip toolchain geometric_vision/include/geometric_vision/triangulation/impl/triangulation.hpp 61 | Eigen::Vector3d t12 = p2 - p1; 62 | 63 | // check parallel 64 | /*if (t12.dot(e1) - t12.dot(e2) < 1.0e-12) { 65 | if ((e1.cross(e2)).norm() < 6 * sigma) { 66 | isValid = true; // check parallel 67 | isParallel = true; 68 | return (Eigen::Vector4d((e1[0] + e2[0]) / 2.0, (e1[1] + e2[1]) / 2.0, 69 | (e1[2] + e2[2]) / 2.0, 1e-2).normalized()); 70 | } 71 | }*/ 72 | 73 | Eigen::Vector2d b; 74 | b[0] = t12.dot(e1); 75 | b[1] = t12.dot(e2); 76 | Eigen::Matrix2d A; 77 | A(0, 0) = e1.dot(e1); 78 | A(1, 0) = e1.dot(e2); 79 | A(0, 1) = -A(1, 0); 80 | A(1, 1) = -e2.dot(e2); 81 | 82 | if (A(1, 0) < 0.0) { 83 | A(1, 0) = -A(1, 0); 84 | A(0, 1) = -A(0, 1); 85 | // wrong viewing direction 86 | }; 87 | 88 | bool invertible; 89 | Eigen::Matrix2d A_inverse; 90 | A.computeInverseWithCheck(A_inverse, invertible, 1.0e-6); 91 | Eigen::Vector2d lambda = A_inverse * b; 92 | if (!invertible) { 93 | isParallel = true; // let's note this. 94 | // parallel. that's fine. but A is not invertible. so handle it separately. 95 | if ((e1.cross(e2)).norm() < 6 * sigma){ 96 | isValid = true; // check parallel 97 | } 98 | return (Eigen::Vector4d((e1[0] + e2[0]) / 2.0, (e1[1] + e2[1]) / 2.0, 99 | (e1[2] + e2[2]) / 2.0, 1e-3).normalized()); 100 | } 101 | 102 | Eigen::Vector3d xm = lambda[0] * e1 + p1; 103 | Eigen::Vector3d xn = lambda[1] * e2 + p2; 104 | Eigen::Vector3d midpoint = (xm + xn) / 2.0; 105 | 106 | // check it 107 | Eigen::Vector3d error = midpoint - xm; 108 | Eigen::Vector3d diff = midpoint - (p1 + 0.5 * t12); 109 | const double diff_sq = diff.dot(diff); 110 | const double chi2 = error.dot(error) * (1.0 / (diff_sq * sigma * sigma)); 111 | 112 | isValid = true; 113 | if (chi2 > 9) { 114 | isValid = false; // reject large chi2-errors 115 | } 116 | 117 | // flip if necessary 118 | if (diff.dot(e1) < 0) { 119 | midpoint = (p1 + 0.5 * t12) - diff; 120 | } 121 | 122 | return Eigen::Vector4d(midpoint[0], midpoint[1], midpoint[2], 1.0).normalized(); 123 | } 124 | 125 | } 126 | 127 | } 128 | 129 | -------------------------------------------------------------------------------- /okvis_kinematics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | 3 | project(okvis_kinematics) 4 | 5 | # require Eigen3 6 | find_package( Eigen REQUIRED ) 7 | include_directories(${EIGEN_INCLUDE_DIR}) 8 | 9 | # build the library 10 | add_library(${PROJECT_NAME} STATIC src/dependency-tracker.cc) 11 | 12 | # and link it 13 | target_link_libraries(${PROJECT_NAME} 14 | PUBLIC okvis_util 15 | ) 16 | 17 | # installation if required 18 | install(TARGETS ${PROJECT_NAME} 19 | EXPORT okvisTargets 20 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 21 | ) 22 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 23 | 24 | # testing 25 | if(BUILD_TESTS) 26 | if(APPLE) 27 | add_definitions(-DGTEST_HAS_TR1_TUPLE=1) 28 | else() 29 | add_definitions(-DGTEST_HAS_TR1_TUPLE=0) 30 | endif(APPLE) 31 | enable_testing() 32 | set(PROJECT_TEST_NAME ${PROJECT_NAME}_test) 33 | add_executable(${PROJECT_TEST_NAME} 34 | test/runTests.cpp 35 | test/TestTransformation.cpp 36 | ) 37 | target_link_libraries(${PROJECT_TEST_NAME} 38 | ${PROJECT_NAME} 39 | ${GTEST_LIBRARY} 40 | pthread) 41 | add_test(test ${PROJECT_TEST_NAME}) 42 | endif() 43 | -------------------------------------------------------------------------------- /okvis_kinematics/include/okvis/kinematics/operators.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Mar 11, 2015 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file operators.hpp 35 | * @brief File with some helper functions related to matrix/vector operations. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #ifndef INCLUDE_OKVIS_KINEMATICS_OPERATORS_HPP_ 40 | #define INCLUDE_OKVIS_KINEMATICS_OPERATORS_HPP_ 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | /// \brief okvis Main namespace of this package. 48 | namespace okvis { 49 | 50 | /// \brief kinematics Namespace for kinematics functionality, i.e. transformations and stuff. 51 | namespace kinematics { 52 | 53 | // some free helper functions 54 | 55 | /// \brief Cross matrix of a vector [x,y,z]. 56 | /// Adopted from Schweizer-Messer by Paul Furgale. 57 | /// \tparam Scalar_T The scalar type, auto-deducible (typically double). 58 | /// @param[in] x First vector element. 59 | /// @param[in] y Second vector element. 60 | /// @param[in] z Third vector element. 61 | template 62 | inline Eigen::Matrix crossMx(Scalar_T x, Scalar_T y, Scalar_T z) 63 | { 64 | Eigen::Matrix C; 65 | C(0, 0) = 0.0; 66 | C(0, 1) = -z; 67 | C(0, 2) = y; 68 | C(1, 0) = z; 69 | C(1, 1) = 0.0; 70 | C(1, 2) = -x; 71 | C(2, 0) = -y; 72 | C(2, 1) = x; 73 | C(2, 2) = 0.0; 74 | return C; 75 | } 76 | 77 | /// \brief Cross matrix of a vector v. 78 | /// Adopted from Schweizer-Messer by Paul Furgale. 79 | /// \tparam Derived_T The vector type, auto-deducible. 80 | /// @param[in] v The vector. 81 | template 82 | inline Eigen::Matrix::Scalar, 3, 3> crossMx( 83 | Eigen::MatrixBase const & v) 84 | { 85 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase, 3); 86 | assert((v.cols()==3 && v.rows()==1)||(v.rows()==3 && v.cols()==1)); 87 | return crossMx(v(0, 0), v(1, 0), v(2, 0)); 88 | } 89 | 90 | /// \brief Plus matrix of a quaternion, i.e. q_AB*q_BC = plus(q_AB)*q_BC.coeffs(). 91 | /// @param[in] q_AB A Quaternion. 92 | inline Eigen::Matrix4d plus(const Eigen::Quaterniond & q_AB) { 93 | Eigen::Vector4d q = q_AB.coeffs(); 94 | Eigen::Matrix4d Q; 95 | Q(0,0) = q[3]; Q(0,1) = -q[2]; Q(0,2) = q[1]; Q(0,3) = q[0]; 96 | Q(1,0) = q[2]; Q(1,1) = q[3]; Q(1,2) = -q[0]; Q(1,3) = q[1]; 97 | Q(2,0) = -q[1]; Q(2,1) = q[0]; Q(2,2) = q[3]; Q(2,3) = q[2]; 98 | Q(3,0) = -q[0]; Q(3,1) = -q[1]; Q(3,2) = -q[2]; Q(3,3) = q[3]; 99 | return Q; 100 | } 101 | 102 | /// \brief Oplus matrix of a quaternion, i.e. q_AB*q_BC = oplus(q_BC)*q_AB.coeffs(). 103 | /// @param[in] q_BC A Quaternion. 104 | inline Eigen::Matrix4d oplus(const Eigen::Quaterniond & q_BC) { 105 | Eigen::Vector4d q = q_BC.coeffs(); 106 | Eigen::Matrix4d Q; 107 | Q(0,0) = q[3]; Q(0,1) = q[2]; Q(0,2) = -q[1]; Q(0,3) = q[0]; 108 | Q(1,0) = -q[2]; Q(1,1) = q[3]; Q(1,2) = q[0]; Q(1,3) = q[1]; 109 | Q(2,0) = q[1]; Q(2,1) = -q[0]; Q(2,2) = q[3]; Q(2,3) = q[2]; 110 | Q(3,0) = -q[0]; Q(3,1) = -q[1]; Q(3,2) = -q[2]; Q(3,3) = q[3]; 111 | return Q; 112 | } 113 | 114 | } // namespace kinematics 115 | } // namespace okvis 116 | 117 | 118 | 119 | #endif /* INCLUDE_OKVIS_KINEMATICS_OPERATORS_HPP_ */ 120 | -------------------------------------------------------------------------------- /okvis_kinematics/src/dependency-tracker.cc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/okvis/f0c9ba472b89f85030cd58dfbae03ceec7817679/okvis_kinematics/src/dependency-tracker.cc -------------------------------------------------------------------------------- /okvis_kinematics/test/runTests.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Feb 3, 2015 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | #include 34 | #include "gtest/gtest.h" 35 | 36 | int main(int argc, char **argv) { 37 | testing::InitGoogleTest(&argc, argv); 38 | return RUN_ALL_TESTS(); 39 | } 40 | 41 | -------------------------------------------------------------------------------- /okvis_matcher/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(okvis_matcher) 3 | 4 | # build the library 5 | add_library(${PROJECT_NAME} 6 | src/DenseMatcher.cpp 7 | src/MatchingAlgorithm.cpp 8 | src/ThreadPool.cpp 9 | include/okvis/DenseMatcher.hpp 10 | include/okvis/MatchingAlgorithm.hpp 11 | include/okvis/ThreadPool.hpp 12 | ) 13 | 14 | # and link it 15 | target_link_libraries(${PROJECT_NAME} 16 | PUBLIC okvis_util 17 | ) 18 | 19 | # installation if required 20 | install(TARGETS ${PROJECT_NAME} 21 | EXPORT okvisTargets 22 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 23 | PUBLIC_HEADER DESTINATION "${INSTALL_INCLUDE_DIR}/okvis" COMPONENT dev 24 | ) 25 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 26 | 27 | # testing 28 | if(BUILD_TESTS) 29 | if(APPLE) 30 | add_definitions(-DGTEST_HAS_TR1_TUPLE=1) 31 | else() 32 | add_definitions(-DGTEST_HAS_TR1_TUPLE=0) 33 | endif(APPLE) 34 | enable_testing() 35 | set(PROJECT_TEST_NAME ${PROJECT_NAME}_test) 36 | add_executable(${PROJECT_TEST_NAME} 37 | test/test_main.cpp 38 | test/testMatcher.cpp 39 | ) 40 | target_link_libraries(${PROJECT_TEST_NAME} 41 | ${PROJECT_NAME} 42 | ${GTEST_LIBRARY} 43 | ${GLOG_LIBRARY} 44 | pthread 45 | ) 46 | add_test(test ${PROJECT_TEST_NAME}) 47 | endif() 48 | -------------------------------------------------------------------------------- /okvis_matcher/include/okvis/ThreadPool.hpp: -------------------------------------------------------------------------------- 1 | // Adapted from https://github.com/progschj/ThreadPool on September 3, 2014 2 | // Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 3 | 4 | // Original copyright: 5 | // Copyright (c) 2012 Jakob Progsch 6 | // 7 | // This software is provided 'as-is', without any express or implied 8 | // warranty. In no event will the authors be held liable for any damages 9 | // arising from the use of this software. 10 | // 11 | // Permission is granted to anyone to use this software for any purpose, 12 | // including commercial applications, and to alter it and redistribute it 13 | // freely, subject to the following restrictions: 14 | // 15 | // 1. The origin of this software must not be misrepresented; you must not 16 | // claim that you wrote the original software. If you use this software 17 | // in a product, an acknowledgment in the product documentation would be 18 | // appreciated but is not required. 19 | // 20 | // 2. Altered source versions must be plainly marked as such, and must not be 21 | // misrepresented as being the original software. 22 | // 23 | // 3. This notice may not be removed or altered from any source 24 | // distribution. 25 | 26 | /* 27 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 28 | */ 29 | 30 | /** 31 | * @file ThreadPool.hpp 32 | * @brief Header file for the ThreadPool class. 33 | * @author Jakob Progsch 34 | * @author Stefan Leutenegger 35 | */ 36 | 37 | 38 | #ifndef INCLUDE_OKVIS_THREAD_POOL_HPP_ 39 | #define INCLUDE_OKVIS_THREAD_POOL_HPP_ 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | /// \brief okvis Main namespace of this package. 53 | namespace okvis { 54 | 55 | /** 56 | * @brief This class manages multiple threads and fills them with work. 57 | */ 58 | class ThreadPool 59 | { 60 | public: 61 | /// \brief Constructor. Launches some amount of workers. 62 | /// \param[in] numThreads The number of threads in the pool. 63 | ThreadPool(size_t numThreads); 64 | 65 | /// \brief Destructor. This joins all threads. 66 | ~ThreadPool(); 67 | 68 | /// \brief Enqueue work for the thread pool 69 | /// 70 | /// Pass in a function and its arguments to enqueue work in the thread pool 71 | /// \param[in] function A function pointer to be called by a thread. 72 | /// \param args Function arguments. 73 | /// \returns A std::future that will return the result of calling function. 74 | /// If this function is called after the thread pool has been stopped, 75 | /// it will return an uninitialized future that will return 76 | /// future.valid() == false 77 | template 78 | std::future::type> 79 | enqueue(Function&& function, Args&&... args); 80 | 81 | /// \brief Stop the thread pool. This method is non-blocking. 82 | void stop() 83 | { 84 | stop_ = true; 85 | } 86 | 87 | /// \brief This method blocks until the queue is empty. 88 | void waitForEmptyQueue() const; 89 | private: 90 | /// \brief Run a single thread. 91 | void run(); 92 | /// Need to keep track of threads so we can join them. 93 | std::vector workers_; 94 | /// The task queue. 95 | std::queue> tasks_; 96 | /// A mutex to protect the list of tasks. 97 | mutable std::mutex tasks_mutex_; 98 | /// A condition variable for worker threads. 99 | mutable std::condition_variable tasks_condition_; 100 | /// A condition variable to support waitForEmptyQueue(). 101 | mutable std::condition_variable wait_condition_; 102 | /// A counter of active threads 103 | unsigned active_threads_; 104 | /// A signal to stop the threads. 105 | volatile bool stop_; 106 | }; 107 | 108 | // Enqueue work for the thread pool. 109 | template 110 | std::future::type> ThreadPool::enqueue( 111 | Function&& function, Args&&... args) 112 | { 113 | typedef typename std::result_of::type return_type; 114 | // Don't allow enqueueing after stopping the pool. 115 | if (stop_) { 116 | LOG(ERROR)<< "enqueue() called on stopped ThreadPool"; 117 | // An empty future will return valid() == false. 118 | return std::future::type>(); 119 | } 120 | 121 | auto task = std::make_shared>( 122 | std::bind(std::forward(function), std::forward(args)...)); 123 | 124 | std::future res = task->get_future(); 125 | { 126 | std::unique_lock lock(tasks_mutex_); 127 | tasks_.push([task]() {(*task)();}); 128 | } 129 | tasks_condition_.notify_one(); 130 | return res; 131 | } 132 | 133 | } // namespace okvis 134 | 135 | #endif // INCLUDE_OKVIS_THREAD_POOL_HPP_ 136 | -------------------------------------------------------------------------------- /okvis_matcher/src/DenseMatcher.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: 2013 30 | * Author: Simon Lynen 31 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 32 | *********************************************************************************/ 33 | 34 | /** 35 | * @file DenseMatcher.cpp 36 | * @brief Source file for the DenseMatcher class. 37 | * @author Simon Lynen 38 | * @author Stefan Leutenegger 39 | */ 40 | 41 | 42 | #include 43 | 44 | /// \brief okvis Main namespace of this package. 45 | namespace okvis { 46 | 47 | // Initialize the dense matcher. 48 | DenseMatcher::DenseMatcher(unsigned char numMatcherThreads, 49 | unsigned char numBest, 50 | bool useDistanceRatioThreshold) 51 | : numMatcherThreads_(numMatcherThreads), 52 | numBest_(numBest), 53 | useDistanceRatioThreshold_(useDistanceRatioThreshold) { 54 | matcherThreadPool_.reset(new okvis::ThreadPool(numMatcherThreads_)); 55 | } 56 | 57 | DenseMatcher::~DenseMatcher() { 58 | matcherThreadPool_->stop(); 59 | } 60 | 61 | // Execute a matching algorithm. This is the slow, runtime polymorphic version. Don't use this. 62 | void DenseMatcher::matchSlow(MatchingAlgorithm & matchingAlgorithm) 63 | 64 | { 65 | match(matchingAlgorithm); 66 | } 67 | 68 | // A recursive function that reassigns weak matches, if a stronger match is found for a particular point 69 | void DenseMatcher::assignbest(int indexToAssignFromListA, 70 | pairing_list_t& vPairsWithScore, 71 | std::vector >& aiBestList, 72 | std::mutex* mutexes, int startidx) { 73 | //the top matches for the current index 74 | const std::vector& aiBest = aiBestList[indexToAssignFromListA]; 75 | for (int index = startidx; index < numBest_ && aiBest[index].indexA != -1; 76 | ++index) { 77 | //fetch index to pair with myidx 78 | const int pairIndexFromListB = aiBest[index].indexA; 79 | // synchronize this 80 | mutexes[pairIndexFromListB].lock(); 81 | if (vPairsWithScore[pairIndexFromListB].indexA == -1) { 82 | // if we are not paired yet 83 | // pair with me 84 | vPairsWithScore[pairIndexFromListB].indexA = indexToAssignFromListA; 85 | // set my distance 86 | vPairsWithScore[pairIndexFromListB].distance = aiBest[index].distance; 87 | mutexes[pairIndexFromListB].unlock(); 88 | return; 89 | } else { 90 | //already paired, so check the score of that pairing 91 | if (aiBest[index].distance 92 | < vPairsWithScore[pairIndexFromListB].distance) { 93 | // My distance is better! 94 | // save vals of old pairing 95 | const int oldPairIndexFromListA = vPairsWithScore[pairIndexFromListB] 96 | .indexA; 97 | // pair with me 98 | vPairsWithScore[pairIndexFromListB] = pairing_t(indexToAssignFromListA, 99 | aiBest[index].distance); 100 | mutexes[pairIndexFromListB].unlock(); 101 | // now reassign the old paring recursivly 102 | // note: skip element at position zero since we just assigned a match with better score, 103 | // so we start the reassignment at position 1 104 | assignbest(oldPairIndexFromListA, vPairsWithScore, aiBestList, mutexes, 105 | 1); 106 | return; 107 | } //else test next alternative 108 | mutexes[pairIndexFromListB].unlock(); 109 | } 110 | } 111 | } 112 | 113 | } // namespace okvis 114 | -------------------------------------------------------------------------------- /okvis_matcher/src/MatchingAlgorithm.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: 2013 30 | * Author: Simon Lynen 31 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 32 | *********************************************************************************/ 33 | 34 | /** 35 | * @file MatchingAlgorithm.cpp 36 | * @brief Source file for the MatchingAlgorithm class. 37 | * @author Simon Lynen 38 | * @author Stefan Leutenegger 39 | */ 40 | 41 | 42 | #include 43 | #include 44 | #include 45 | 46 | /// \brief okvis Main namespace of this package. 47 | namespace okvis { 48 | 49 | MatchingAlgorithm::MatchingAlgorithm() { 50 | } 51 | MatchingAlgorithm::~MatchingAlgorithm() { 52 | } 53 | 54 | MatchingAlgorithm::listB_tree_structure_t::iterator MatchingAlgorithm::getListBStartIterator( 55 | size_t /* indexA */) { 56 | OKVIS_THROW( 57 | std::runtime_error, 58 | "To use the listB iterators, implement this interface in your matching algorithm subclass."); 59 | return dummy_.end(); 60 | } 61 | MatchingAlgorithm::listB_tree_structure_t::iterator MatchingAlgorithm::getListBEndIterator( 62 | size_t /* indexA */) { 63 | OKVIS_THROW( 64 | std::runtime_error, 65 | "To use the listB iterators, implement this interface in your matching algorithm subclass."); 66 | return dummy_.end(); 67 | } 68 | 69 | } // namespace okvis 70 | -------------------------------------------------------------------------------- /okvis_matcher/src/ThreadPool.cpp: -------------------------------------------------------------------------------- 1 | // Adapted from https://github.com/progschj/ThreadPool on September 3, 2014 2 | // Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 3 | 4 | // Original copyright: 5 | // Copyright (c) 2012 Jakob Progsch 6 | // 7 | // This software is provided 'as-is', without any express or implied 8 | // warranty. In no event will the authors be held liable for any damages 9 | // arising from the use of this software. 10 | // 11 | // Permission is granted to anyone to use this software for any purpose, 12 | // including commercial applications, and to alter it and redistribute it 13 | // freely, subject to the following restrictions: 14 | // 15 | // 1. The origin of this software must not be misrepresented; you must not 16 | // claim that you wrote the original software. If you use this software 17 | // in a product, an acknowledgment in the product documentation would be 18 | // appreciated but is not required. 19 | // 20 | // 2. Altered source versions must be plainly marked as such, and must not be 21 | // misrepresented as being the original software. 22 | // 23 | // 3. This notice may not be removed or altered from any source 24 | // distribution. 25 | 26 | /* 27 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 28 | */ 29 | 30 | /** 31 | * @file ThreadPool.cpp 32 | * @brief Source file for the ThreadPool class. 33 | * @author Jakob Progsch 34 | * @author Stefan Leutenegger 35 | */ 36 | 37 | 38 | #include 39 | 40 | /// \brief okvis Main namespace of this package. 41 | namespace okvis { 42 | 43 | // The constructor just launches some amount of workers. 44 | ThreadPool::ThreadPool(size_t threads) 45 | : active_threads_(0), 46 | stop_(false) 47 | { 48 | for (size_t i = 0; i < threads; ++i) 49 | workers_.emplace_back(std::bind(&ThreadPool::run, this)); 50 | } 51 | 52 | // Destructor. This joins all threads. 53 | ThreadPool::~ThreadPool() 54 | { 55 | { 56 | std::unique_lock lock(tasks_mutex_); 57 | stop_ = true; 58 | } 59 | tasks_condition_.notify_all(); 60 | for (size_t i = 0; i < workers_.size(); ++i) { 61 | workers_[i].join(); 62 | } 63 | } 64 | 65 | // Run a single thread. 66 | void ThreadPool::run() 67 | { 68 | while (true) { 69 | std::unique_lock lock(this->tasks_mutex_); 70 | while (!this->stop_ && this->tasks_.empty()) { 71 | this->tasks_condition_.wait(lock); 72 | } 73 | if (this->stop_ && this->tasks_.empty()) { 74 | return; 75 | } 76 | std::function task(this->tasks_.front()); 77 | this->tasks_.pop(); 78 | ++active_threads_; 79 | // Unlock the queue while we execute the task. 80 | lock.unlock(); 81 | task(); 82 | lock.lock(); 83 | --active_threads_; 84 | // This is the secret to making the waitForEmptyQueue() function work. 85 | // After finishing a task, notify that this work is done. 86 | wait_condition_.notify_all(); 87 | } 88 | } 89 | 90 | // This method blocks until the queue is empty. 91 | void ThreadPool::waitForEmptyQueue() const 92 | { 93 | std::unique_lock lock(this->tasks_mutex_); 94 | // Only exit if all tasks are complete by tracking the number of 95 | // active threads. 96 | while (active_threads_ || !tasks_.empty()) { 97 | this->wait_condition_.wait(lock); 98 | } 99 | } 100 | } // namespace okvis 101 | -------------------------------------------------------------------------------- /okvis_matcher/test/testMatcher.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | class TestMatchingAlgorithm : public okvis::MatchingAlgorithm { 6 | public: 7 | TestMatchingAlgorithm() { 8 | } 9 | virtual ~TestMatchingAlgorithm() { 10 | } 11 | 12 | /// \brief this will be called exactly once for each call to DenseMatcher::match() 13 | virtual void doSetup() { 14 | } 15 | 16 | /// \brief what is the size of list A? 17 | virtual size_t sizeA() const { 18 | return listA.size(); 19 | } 20 | /// \brief what is the size of list B? 21 | virtual size_t sizeB() const { 22 | return listB.size(); 23 | } 24 | 25 | /// distances above this threshold will not be returned as matches. 26 | virtual float distanceThreshold() const { 27 | return 4.0f; 28 | } 29 | 30 | /// by which factor does the first best match has to be better than the second best one. 31 | virtual float distanceRatioThreshold() const { 32 | return 3.0f; 33 | } 34 | 35 | /// \brief Should we skip the item in list A? This will be called once for each item in the list 36 | virtual bool skipA(size_t indexA) const { 37 | return indexA == 0; 38 | } 39 | 40 | /// \brief Should we skip the item in list B? This will be called many times. 41 | virtual bool skipB(size_t /* indexB */) const { 42 | return false; 43 | } 44 | 45 | /// \brief the "distance" between the two points. 46 | /// For points that absolutely don't match. Please use float max. 47 | virtual float distance(size_t indexA, size_t indexB) const { 48 | double diff = listA[indexA] - listB[indexB]; 49 | return fabs(diff); 50 | } 51 | 52 | /// \brief a function that tells you how many times setMatching() will be called. 53 | virtual void reserveMatches(size_t numMatches) { 54 | matches.clear(); 55 | matches.reserve(numMatches); 56 | } 57 | 58 | /// \brief At the end of the matching step, this function is called once 59 | /// for each pair of matches discovered. 60 | virtual void setBestMatch(size_t indexA, size_t indexB, double /* distance */) { 61 | matches.push_back(std::make_pair(indexA, indexB)); 62 | } 63 | 64 | std::vector listA; 65 | std::vector listB; 66 | std::vector > matches; 67 | }; 68 | 69 | TEST(DenseMatcherTestSuite, denseMatcherTest) 70 | { 71 | TestMatchingAlgorithm tma; 72 | 73 | tma.listA.push_back(1.0); 74 | tma.listA.push_back(3.0); 75 | tma.listA.push_back(2.0); 76 | tma.listA.push_back(0.9); 77 | 78 | /// This shouldn't be matched because 18 - 4 > 4.0 79 | tma.listB.push_back(18.0); 80 | tma.listB.push_back(2.1); 81 | tma.listB.push_back(4.0); 82 | // This shouldn't be matched with listA[0] because of skipping listA[0] 83 | // So, it will be matches with listA[3] 84 | tma.listB.push_back(1.0); 85 | 86 | // We should have 1 --> 2 and 2 --> 1 87 | 88 | okvis::DenseMatcher matcher; 89 | 90 | matcher.match(tma); 91 | 92 | ASSERT_EQ(3u, tma.matches.size()); 93 | 94 | for(size_t i = 0; i < tma.matches.size(); ++i) 95 | { 96 | switch(tma.matches[i].first) 97 | { 98 | case 1: 99 | ASSERT_EQ(2, tma.matches[i].second); 100 | break; 101 | case 2: 102 | ASSERT_EQ(1, tma.matches[i].second); 103 | break; 104 | case 3: 105 | ASSERT_EQ(3, tma.matches[i].second); 106 | break; 107 | default: 108 | FAIL() << "Unexpected match " << tma.matches[i].first << " --> " << tma.matches[i].second; 109 | } 110 | } 111 | } 112 | 113 | TEST(DenseMatcherTestSuite, denseMatcherDistanceRatioTest) 114 | { 115 | TestMatchingAlgorithm tma; 116 | 117 | tma.listA.push_back(8.0); 118 | tma.listA.push_back(1.0); 119 | tma.listA.push_back(3.0); 120 | tma.listA.push_back(2.0); 121 | tma.listA.push_back(0.9); 122 | 123 | /// This shouldn't be matched because 16/15 < 4 124 | tma.listB.push_back(18.0); 125 | tma.listB.push_back(2.1); 126 | /// This shouldn't be matched because 2/1 < 4 127 | tma.listB.push_back(4.0); 128 | tma.listB.push_back(1.0); 129 | 130 | // This shouldn't be matched with listA[0] because of skipping listA[0] 131 | tma.listB.push_back(7.0); 132 | 133 | // We should have 1 --> 2 and 2 --> 1 134 | bool useDistanceRatioThreshold = true; 135 | okvis::DenseMatcher matcher(4, 4, useDistanceRatioThreshold); 136 | 137 | matcher.match(tma); 138 | 139 | ASSERT_EQ(2u, tma.matches.size()); 140 | 141 | for(size_t i = 0; i < tma.matches.size(); ++i) 142 | { 143 | switch(tma.matches[i].first) 144 | { 145 | case 1: 146 | ASSERT_EQ(3, tma.matches[i].second); 147 | break; 148 | case 3: 149 | ASSERT_EQ(1, tma.matches[i].second); 150 | break; 151 | default: 152 | FAIL() << "Unexpected match " << tma.matches[i].first << " --> " << tma.matches[i].second; 153 | } 154 | } 155 | } 156 | -------------------------------------------------------------------------------- /okvis_matcher/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 | 9 | -------------------------------------------------------------------------------- /okvis_multisensor_processing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | project(okvis_multisensor_processing) 3 | 4 | if(NOT DO_TIMING) 5 | add_definitions(-DDEACTIVATE_TIMERS) 6 | message(STATUS "Deactivating timers.") 7 | endif() 8 | 9 | # generate the configure header file 10 | configure_file(../cmake/okvisConfig.hpp.in "${CMAKE_CURRENT_BINARY_DIR}/okvisConfig.hpp" @ONLY) 11 | message(STATUS ${CMAKE_CURRENT_BINARY_DIR}/okvisConfig.hpp) 12 | 13 | # require Eigen 14 | find_package( Eigen REQUIRED ) 15 | include_directories(${EIGEN_INCLUDE_DIR}) 16 | 17 | # require OpenCV 18 | find_package( OpenCV COMPONENTS core highgui imgproc features2d REQUIRED ) 19 | include_directories(BEFORE ${OpenCV_INCLUDE_DIRS}) 20 | 21 | add_library(${PROJECT_NAME} 22 | src/ThreadedKFVio.cpp 23 | src/ImuFrameSynchronizer.cpp 24 | src/FrameSynchronizer.cpp 25 | src/VioVisualizer.cpp 26 | include/okvis/ThreadedKFVio.hpp 27 | include/okvis/ImuFrameSynchronizer.hpp 28 | include/okvis/FrameSynchronizer.hpp 29 | include/okvis/VioVisualizer.hpp 30 | include/okvis/threadsafe/ThreadsafeQueue.hpp 31 | ../cmake/okvisConfig.hpp.in 32 | okvisConfig.hpp 33 | ) 34 | 35 | # and link it 36 | target_link_libraries(${PROJECT_NAME} 37 | PUBLIC okvis_time 38 | PUBLIC okvis_util 39 | PUBLIC okvis_kinematics 40 | PUBLIC okvis_cv 41 | PUBLIC okvis_common 42 | PUBLIC okvis_ceres 43 | PUBLIC okvis_frontend 44 | PRIVATE ${GLOG_LIBRARIES} 45 | ) 46 | 47 | # export config 48 | set_target_properties(${PROJECT_NAME} PROPERTIES PUBLIC_HEADER ${CMAKE_CURRENT_BINARY_DIR}/okvisConfig.hpp) 49 | 50 | # installation if required 51 | install(TARGETS ${PROJECT_NAME} 52 | EXPORT okvisTargets 53 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 54 | PUBLIC_HEADER DESTINATION "${INSTALL_INCLUDE_DIR}/okvis" COMPONENT dev 55 | ) 56 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 57 | 58 | # testing 59 | if(BUILD_TESTS) 60 | if(APPLE) 61 | add_definitions(-DGTEST_HAS_TR1_TUPLE=1) 62 | else() 63 | add_definitions(-DGTEST_HAS_TR1_TUPLE=1) 64 | enable_testing() 65 | set(PROJECT_TEST_NAME ${PROJECT_NAME}_test) 66 | add_executable(${PROJECT_TEST_NAME} 67 | test/test_main.cpp 68 | test/FrameSynchronizer_test.cpp 69 | test/ImuFrameSynchronizer_test.cpp 70 | test/test_main.cpp 71 | test/testThreading.cpp 72 | test/testDataFlow.cpp 73 | test/testSynchronizer.cpp 74 | ) 75 | target_link_libraries(${PROJECT_TEST_NAME} 76 | ${GTEST_LIBRARY} 77 | ${GMOCK_LIBRARY} 78 | ${PROJECT_NAME} 79 | ${CERES_LIBRARIES} 80 | ${OpenCV_LIBRARIES} 81 | pthread) 82 | add_test(test ${PROJECT_TEST_NAME} ) 83 | endif(APPLE) 84 | endif() 85 | 86 | -------------------------------------------------------------------------------- /okvis_multisensor_processing/include/okvis/ImuFrameSynchronizer.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Sep 13, 2014 30 | * Author: Pascal Gohl 31 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 32 | * Modified: Andreas Forster (an.forster@gmail.com) 33 | *********************************************************************************/ 34 | 35 | /** 36 | * @file ImuFrameSynchronizer.hpp 37 | * @brief Header file for the ImuFrameSynchronizer class. 38 | * @author Pascal Gohl 39 | * @author Stefan Leutenegger 40 | * @author Andreas Forster 41 | */ 42 | 43 | #ifndef INCLUDE_OKVIS_IMUFRAMESYNCHRONIZER_H_ 44 | #define INCLUDE_OKVIS_IMUFRAMESYNCHRONIZER_H_ 45 | 46 | #include 47 | #include 48 | #include 49 | 50 | #include 51 | 52 | /// \brief okvis Main namespace of this package. 53 | namespace okvis { 54 | 55 | /** 56 | * @brief This class is to safely notify different threads whether IMU measurements 57 | * up to a timestamp (e.g. the one of a camera frame) have already been registered. 58 | */ 59 | class ImuFrameSynchronizer { 60 | public: 61 | /// @brief Constructor. 62 | ImuFrameSynchronizer(); 63 | /// @brief Destructor. 64 | ~ImuFrameSynchronizer(); 65 | 66 | /** 67 | * @brief Tell the synchronizer that a new IMU measurement has been registered. 68 | * @param stamp Timestamp of the new IMU mewasurement. 69 | */ 70 | void gotImuData(const okvis::Time& stamp); 71 | 72 | /** 73 | * @brief Wait until a IMU measurement with a timestamp equal or newer to the supplied one is registered. 74 | * @param frame_stamp Timestamp until you want to have IMU measurements for. 75 | * @return False if a shutdown signal has been received. Otherwise true. 76 | */ 77 | bool waitForUpToDateImuData(const okvis::Time& frame_stamp); 78 | 79 | /// @brief Tell the synchronizer to shutdown. This will notify all waiting threads to wake up. 80 | void shutdown(); 81 | 82 | private: 83 | okvis::Time newestImuDataStamp_; ///< Newest IMU data timestamp. 84 | okvis::Time imuDataNeededUntil_; ///< A thread is waiting for IMU data newer or equal to this timestamp. 85 | std::condition_variable gotNeededImuData_; ///< Condition variable for waiting and notyfing. 86 | std::mutex mutex_; ///< Mutex. 87 | std::atomic_bool shutdown_; ///< True if shutdown() was called. 88 | }; 89 | 90 | } /* namespace okvis */ 91 | 92 | #endif /* INCLUDE_OKVIS_IMUFRAMESYNCHRONIZER_H_ */ 93 | -------------------------------------------------------------------------------- /okvis_multisensor_processing/include/okvis/VioVisualizer.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Sep 15, 2014 30 | * Author: Pascal Gohl 31 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 32 | * Modified: Andreas Forster (an.forster@gmail.com) 33 | *********************************************************************************/ 34 | 35 | /** 36 | * @file VioVisualizer.hpp 37 | * @brief Header file for the VioVisualizer class. 38 | * @author Pascal Gohl 39 | * @author Stefan Leutenegger 40 | * @author Andreas Forster 41 | */ 42 | 43 | #ifndef INCLUDE_OKVIS_VIOVISUALIZER_HPP_ 44 | #define INCLUDE_OKVIS_VIOVISUALIZER_HPP_ 45 | 46 | #include 47 | #include 48 | 49 | #include 50 | #include 51 | #include 52 | 53 | /// \brief okvis Main namespace of this package. 54 | namespace okvis { 55 | 56 | /** 57 | * @brief This class is responsible to visualize the matching results 58 | */ 59 | class VioVisualizer { 60 | public: 61 | 62 | /// @brief This struct contains the relevant data for visualizing 63 | struct VisualizationData { 64 | typedef std::shared_ptr Ptr; 65 | okvis::ObservationVector observations; ///< Vector containing all the keypoint observations. 66 | std::shared_ptr currentFrames; ///< Current multiframe. 67 | std::shared_ptr keyFrames; ///< Current keyframe. 68 | okvis::kinematics::Transformation T_WS_keyFrame; ///< Pose of the current keyframe 69 | }; 70 | 71 | OKVIS_DEFINE_EXCEPTION(Exception, std::runtime_error) 72 | 73 | /** 74 | * @brief Constructor. 75 | * @param parameters Parameters and settings. 76 | */ 77 | VioVisualizer(okvis::VioParameters& parameters); 78 | virtual ~VioVisualizer(); 79 | 80 | /** 81 | * @brief Initialise parameters. Called in constructor. 82 | * @param parameters Parameters and settings. 83 | */ 84 | void init(okvis::VioParameters& parameters); 85 | /** 86 | * @brief Show the current frames with the current keyframe and all its matches. 87 | * @param data Visualization data containing all the info. 88 | */ 89 | void showDebugImages(VisualizationData::Ptr& data); 90 | 91 | /** 92 | * @brief Circles all keypoints in the current frame, links the matching ones to 93 | * the current keyframe and returns the result. 94 | * @param data Visualization data. 95 | * @param image_number Index of the frame to display. 96 | * @return OpenCV matrix with the resulting image. 97 | */ 98 | cv::Mat drawMatches(VisualizationData::Ptr& data, size_t image_number); 99 | 100 | private: 101 | /** 102 | * @brief Circles all keypoints in the current frame and returns the result. 103 | * @param data Visualization data. 104 | * @param cameraIndex Index of the frame to display. 105 | * @return OpenCV matrix with the resulting image. 106 | */ 107 | cv::Mat drawKeypoints(VisualizationData::Ptr& data, size_t cameraIndex); 108 | 109 | /// Parameters and settings. 110 | okvis::VioParameters parameters_; 111 | }; 112 | 113 | } /* namespace okvis */ 114 | 115 | #endif /* INCLUDE_OKVIS_VIOVISUALIZER_HPP_ */ 116 | -------------------------------------------------------------------------------- /okvis_multisensor_processing/src/ImuFrameSynchronizer.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Sep 13, 2014 30 | * Author: Pascal Gohl 31 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 32 | * Modified: Andreas Forster (an.forster@gmail.com) 33 | *********************************************************************************/ 34 | 35 | /** 36 | * @file ImuFrameSynchronizer.cpp 37 | * @brief Source file for the ImuFrameSynchronizer class. 38 | * @author Pascal Gohl 39 | * @author Stefan Leutenegger 40 | * @author Andreas Forster 41 | */ 42 | 43 | #include "okvis/ImuFrameSynchronizer.hpp" 44 | 45 | /// \brief okvis Main namespace of this package. 46 | namespace okvis { 47 | 48 | ImuFrameSynchronizer::ImuFrameSynchronizer() 49 | : shutdown_(false) {} 50 | 51 | ImuFrameSynchronizer::~ImuFrameSynchronizer() { 52 | if(!shutdown_) 53 | shutdown(); 54 | } 55 | 56 | // Tell the synchronizer that a new IMU measurement has been registered. 57 | void ImuFrameSynchronizer::gotImuData(const okvis::Time& stamp) { 58 | newestImuDataStamp_ = stamp; 59 | if(imuDataNeededUntil_ < stamp) 60 | gotNeededImuData_.notify_all(); 61 | } 62 | 63 | // Wait until a IMU measurement with a timestamp equal or newer to the supplied one is registered. 64 | bool ImuFrameSynchronizer::waitForUpToDateImuData(const okvis::Time& frame_stamp) { 65 | // if the newest imu data timestamp is smaller than frame_stamp, wait until 66 | // imu_data newer than frame_stamp arrives 67 | if(newestImuDataStamp_ <= frame_stamp && !shutdown_) { 68 | imuDataNeededUntil_ = frame_stamp; 69 | std::unique_lock lock(mutex_); 70 | gotNeededImuData_.wait(lock); 71 | } 72 | if(shutdown_) 73 | return false; 74 | return true; 75 | } 76 | 77 | // Tell the synchronizer to shutdown. This will notify all waiting threads to wake up. 78 | void ImuFrameSynchronizer::shutdown() { 79 | shutdown_ = true; 80 | gotNeededImuData_.notify_all(); 81 | } 82 | 83 | } /* namespace okvis */ 84 | -------------------------------------------------------------------------------- /okvis_multisensor_processing/test/ImuFrameSynchronizer_test.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Sep 13, 2014 30 | * Author: Pascal Gohl 31 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 32 | *********************************************************************************/ 33 | 34 | #include "okvis/ImuFrameSynchronizer.hpp" 35 | 36 | /// \brief okvis Main namespace of this package. 37 | namespace okvis { 38 | 39 | } /* namespace okvis */ 40 | -------------------------------------------------------------------------------- /okvis_multisensor_processing/test/MockVioFrontendInterface.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Aug 21, 2014 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | #ifndef MOCK_FRONTEND_HPP_ 34 | #define MOCK_FRONTEND_HPP_ 35 | 36 | #define GTEST_USE_OWN_TR1_TUPLE 0 37 | #include "gmock/gmock.h" 38 | #include "gtest/gtest.h" 39 | 40 | /// \brief okvis Main namespace of this package. 41 | namespace okvis { 42 | 43 | class MockVioFrontendInterface{ 44 | public: 45 | MOCK_METHOD4(detectAndDescribe, 46 | bool(size_t cameraIndex, std::shared_ptr frameOut, const okvis::kinematics::Transformation& T_WC, const std::vector * keypoints)); 47 | MOCK_METHOD6(dataAssociationAndInitialization, 48 | bool(okvis::VioBackendInterface& estimator, okvis::kinematics::Transformation& T_WS_propagated, const okvis::VioParameters & params, const std::shared_ptr map, std::shared_ptr framesInOut, bool* asKeyframe)); 49 | MOCK_CONST_METHOD8(propagation, 50 | bool(const okvis::ImuMeasurementDeque & imuMeasurements, const okvis::ImuParameters & imuParams, okvis::kinematics::Transformation& T_WS_propagated, okvis::SpeedAndBias & speedAndBiases, const okvis::Time& t_start, const okvis::Time& t_end, Eigen::Matrix* covariance, Eigen::Matrix* jacobian)); 51 | MOCK_METHOD1(setBriskDetectionOctaves, 52 | void(size_t octaves)); 53 | MOCK_METHOD1(setBriskDetectionThreshold, 54 | void(double threshold)); 55 | }; 56 | 57 | } // namespace okvis 58 | 59 | 60 | #endif /* MOCK_DUMMYVIO_HPP_ */ 61 | -------------------------------------------------------------------------------- /okvis_multisensor_processing/test/VioVisualizer_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * VioVisualizer_test.cpp 3 | * 4 | * Created on: Sep 15, 2014 5 | * Author: pascal 6 | */ 7 | 8 | #include "VioVisualizer.hpp" 9 | 10 | /// \brief okvis Main namespace of this package. 11 | namespace okvis { 12 | 13 | } /* namespace okvis */ 14 | -------------------------------------------------------------------------------- /okvis_multisensor_processing/test/testDataFlow.cpp: -------------------------------------------------------------------------------- 1 | #define GTEST_USE_OWN_TR1_TUPLE 0 2 | 3 | #include "gmock/gmock.h" 4 | #include "gtest/gtest.h" 5 | 6 | #pragma GCC diagnostic push 7 | #pragma GCC diagnostic ignored "-Wnon-virtual-dtor" 8 | #include 9 | #pragma GCC diagnostic pop 10 | #include 11 | 12 | #include 13 | #include "MockVioBackendInterface.hpp" 14 | #include "MockVioFrontendInterface.hpp" 15 | 16 | #include "testDataGenerators.hpp" 17 | 18 | using ::testing::AtLeast; 19 | using ::testing::AnyNumber; 20 | using ::testing::Between; 21 | using ::testing::Return; 22 | using ::testing::_; 23 | 24 | TEST(OkvisVioInterfaces, testDataFlow) 25 | { 26 | using namespace okvis; 27 | 28 | okvis::VioParameters parameters; 29 | parameters.nCameraSystem = TestDataGenerator::getTestCameraSystem(2); 30 | parameters.visualization.displayImages = false; 31 | parameters.imu.a_max = 1; 32 | parameters.imu.g_max = 1; 33 | parameters.optimization.numImuFrames = 2; 34 | parameters.publishing.publishImuPropagatedState = true; 35 | 36 | // configure mock object 37 | MockVioBackendInterface dummy; 38 | EXPECT_CALL(dummy, optimize(_,_,_)) 39 | .Times(Between(5, 10)); 40 | EXPECT_CALL(dummy, get_T_WS(_,_)) 41 | .Times(Between(12, 21)); // 1 per matching, 1 per optimization and in destructor 42 | EXPECT_CALL(dummy, addCamera(_)) 43 | .Times(2); 44 | EXPECT_CALL(dummy, addImu(_)) 45 | .Times(1); 46 | EXPECT_CALL(dummy, addStates(_,_,_)) 47 | .Times(Between(5, 10)); 48 | EXPECT_CALL(dummy, applyMarginalizationStrategy(_,_,_)) 49 | .Times(Between(5,10)); 50 | EXPECT_CALL(dummy, setOptimizationTimeLimit(_,_)) 51 | .Times(1); 52 | 53 | EXPECT_CALL(dummy, multiFrame(_)) 54 | .Times(AnyNumber()); 55 | EXPECT_CALL(dummy, getSpeedAndBias(_,_,_)) 56 | .Times(AnyNumber()); 57 | EXPECT_CALL(dummy, numFrames()) 58 | .Times(AnyNumber()); 59 | 60 | ON_CALL(dummy, numFrames()) 61 | .WillByDefault(Return(1)); 62 | ON_CALL(dummy, addStates(_,_,_)) 63 | .WillByDefault(Return(true)); 64 | // to circumvent segfault 65 | ON_CALL(dummy, multiFrame(_)) 66 | .WillByDefault(Return(std::shared_ptr(new okvis::MultiFrame(parameters.nCameraSystem, 67 | okvis::Time::now())))); 68 | 69 | MockVioFrontendInterface mock_frontend; 70 | EXPECT_CALL(mock_frontend, detectAndDescribe(_,_,_,_)) 71 | .Times(Between(18, 20)); 72 | EXPECT_CALL(mock_frontend, dataAssociationAndInitialization(_,_,_,_,_,_)) 73 | .Times(Between(7, 10)); 74 | EXPECT_CALL(mock_frontend, propagation(_,_,_,_,_,_,_,_)) 75 | .Times(Between(99, 100)); 76 | EXPECT_CALL(mock_frontend, setBriskDetectionOctaves(_)) 77 | .Times(1); 78 | EXPECT_CALL(mock_frontend, setBriskDetectionThreshold(_)) 79 | .Times(1); 80 | 81 | // start with mock 82 | ThreadedKFVio vio(parameters); 83 | vio.setBlocking(true); 84 | 85 | double now = okvis::Time::now().toSec(); 86 | 87 | // test Observation 88 | cv::Mat image_cam; 89 | image_cam = cv::imread("testImage.jpg", 0); 90 | 91 | if (image_cam.data == NULL) { 92 | std::cout << "testImage.jpg not found" << std::endl; 93 | } 94 | 95 | ASSERT_TRUE(image_cam.data != NULL); 96 | 97 | okvis::ImuMeasurement imu_data; 98 | imu_data.measurement.accelerometers.Zero(); imu_data.measurement.gyroscopes.Zero(); 99 | 100 | // simulate 100Hz IMU and 10Hz images 101 | for (int i = 0; i < 10; ++i) { 102 | for (int j = 0; j < 5; ++j) { 103 | vio.addImuMeasurement(okvis::Time(now), imu_data.measurement.accelerometers, imu_data.measurement.gyroscopes); 104 | now += 0.01; 105 | } 106 | vio.addImage(okvis::Time(now), 0, image_cam); 107 | vio.addImage(okvis::Time(now), 1, image_cam); 108 | for (int j = 0; j < 5; ++j) { 109 | vio.addImuMeasurement(okvis::Time(now), imu_data.measurement.accelerometers, imu_data.measurement.gyroscopes); 110 | now += 0.01; 111 | } 112 | } 113 | } 114 | -------------------------------------------------------------------------------- /okvis_multisensor_processing/test/testDataGenerators.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TEST_DATA_GENERATOR_HPP 2 | #define TEST_DATA_GENERATOR_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | /// \brief okvis Main namespace of this package. 9 | namespace okvis { 10 | class TestDataGenerator { 11 | public: 12 | static const okvis::cameras::NCameraSystem getTestCameraSystem(int num_cams) { 13 | okvis::cameras::NCameraSystem nCameraSystem; 14 | for (int i = 0; i < num_cams; ++i) { 15 | Eigen::Matrix4d T_SC_e; 16 | T_SC_e << 1, 0, 0, i*0.01, // TODO(gohlp): does that make sense? 17 | 0, 1, 0, 0, 18 | 0, 0, 1, 0, 19 | 0, 0, 0, 0; 20 | std::shared_ptr T_SC_ok_ptr( 21 | new okvis::kinematics::Transformation(T_SC_e)); 22 | nCameraSystem.addCamera( 23 | T_SC_ok_ptr, 24 | std::shared_ptr( 25 | new okvis::cameras::PinholeCamera< 26 | okvis::cameras::RadialTangentialDistortion>( 27 | 752,480, 28 | 615.14, 613.03, 29 | 378.27, 220.05, 30 | okvis::cameras::RadialTangentialDistortion( 31 | -0.40675345816443564, 0.1685454248925922, -0.00046944024734783504, 32 | 0.00026638950478762756))), 33 | okvis::cameras::NCameraSystem::RadialTangential); 34 | } 35 | return nCameraSystem; 36 | } 37 | }; 38 | } 39 | 40 | #endif// TEST_DATA_GENERATOR_HPP 41 | -------------------------------------------------------------------------------- /okvis_multisensor_processing/test/testImage.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/okvis/f0c9ba472b89f85030cd58dfbae03ceec7817679/okvis_multisensor_processing/test/testImage.jpg -------------------------------------------------------------------------------- /okvis_multisensor_processing/test/testSynchronizer.cpp: -------------------------------------------------------------------------------- 1 | #define GTEST_USE_OWN_TR1_TUPLE 0 2 | 3 | #include "gmock/gmock.h" 4 | #include "gtest/gtest.h" 5 | 6 | #pragma GCC diagnostic push 7 | #pragma GCC diagnostic ignored "-Wnon-virtual-dtor" 8 | #include 9 | #pragma GCC diagnostic pop 10 | #include 11 | 12 | #include 13 | 14 | #include "MockVioBackendInterface.hpp" 15 | #include "MockVioFrontendInterface.hpp" 16 | #include "testDataGenerators.hpp" 17 | 18 | using ::testing::AtLeast; 19 | using ::testing::AnyNumber; 20 | using ::testing::Between; 21 | using ::testing::Return; 22 | using ::testing::_; 23 | 24 | TEST(OkvisVioInterfaces, testFrameSync) 25 | { 26 | using namespace okvis; 27 | 28 | okvis::VioParameters parameters; 29 | parameters.nCameraSystem = TestDataGenerator::getTestCameraSystem(2); 30 | parameters.optimization.numImuFrames = 2; 31 | parameters.publishing.publishImuPropagatedState = true; 32 | 33 | // configure mock object 34 | MockVioBackendInterface dummy; 35 | EXPECT_CALL(dummy, optimize(_,_,_)) 36 | .Times(Between(3, 6)); 37 | EXPECT_CALL(dummy, get_T_WS(_,_)) 38 | .Times(Between(8, 13)); 39 | EXPECT_CALL(dummy, addStates(_,_,_)) 40 | .Times(Between(3,6)); 41 | EXPECT_CALL(dummy, applyMarginalizationStrategy(_,_,_)) 42 | .Times(Between(3,6)); 43 | EXPECT_CALL(dummy, setOptimizationTimeLimit(_,_)) 44 | .Times(1); 45 | EXPECT_CALL(dummy, addCamera(_)) 46 | .Times(2); 47 | EXPECT_CALL(dummy, addImu(_)) 48 | .Times(1); 49 | 50 | EXPECT_CALL(dummy, multiFrame(_)) 51 | .Times(AnyNumber()); 52 | EXPECT_CALL(dummy, getSpeedAndBias(_,_,_)) 53 | .Times(AnyNumber()); 54 | EXPECT_CALL(dummy, numFrames()) 55 | .Times(AnyNumber()); 56 | 57 | ON_CALL(dummy, numFrames()) 58 | .WillByDefault(Return(1)); 59 | ON_CALL(dummy, addStates(_,_,_)) 60 | .WillByDefault(Return(true)); 61 | // to circumvent segfault 62 | ON_CALL(dummy, multiFrame(_)) 63 | .WillByDefault(Return(std::shared_ptr(new okvis::MultiFrame(parameters.nCameraSystem, 64 | okvis::Time::now())))); 65 | 66 | MockVioFrontendInterface mock_frontend; 67 | EXPECT_CALL(mock_frontend, detectAndDescribe(_,_,_,_)) 68 | .Times(Between(12, 17)); 69 | EXPECT_CALL(mock_frontend, dataAssociationAndInitialization(_,_,_,_,_,_)) 70 | .Times(Between(4, 6)); 71 | EXPECT_CALL(mock_frontend, propagation(_,_,_,_,_,_,_,_)) 72 | .Times(Between(114, 115)); 73 | EXPECT_CALL(mock_frontend, setBriskDetectionOctaves(_)) 74 | .Times(1); 75 | EXPECT_CALL(mock_frontend, setBriskDetectionThreshold(_)) 76 | .Times(1); 77 | 78 | // start with mock 79 | ThreadedKFVio vio(parameters); 80 | vio.setBlocking(true); 81 | 82 | double now = okvis::Time::now().toSec(); 83 | 84 | // test Observation 85 | cv::Mat image_cam; 86 | image_cam = cv::imread("testImage.jpg", 0); 87 | 88 | if (image_cam.data == NULL) { 89 | std::cout << "testImage.jpg not found" << std::endl; 90 | } 91 | ASSERT_TRUE(image_cam.data != NULL); 92 | 93 | okvis::ImuMeasurement imu_data; 94 | imu_data.measurement.accelerometers.Zero(); imu_data.measurement.gyroscopes.Zero(); 95 | 96 | // simulate 100Hz IMU and 10Hz images 97 | for (int j = 0; j < 5; ++j) { 98 | vio.addImuMeasurement(okvis::Time(now), imu_data.measurement.accelerometers, imu_data.measurement.gyroscopes); 99 | now += 0.01; 100 | } 101 | for (int i = 0; i < 3; ++i) { 102 | vio.addImage(okvis::Time(now), 0, image_cam); 103 | vio.addImage(okvis::Time(now), 1, image_cam); 104 | for (int j = 0; j < 10; ++j) { 105 | vio.addImuMeasurement(okvis::Time(now), imu_data.measurement.accelerometers, imu_data.measurement.gyroscopes); 106 | now += 0.01; 107 | } 108 | } 109 | // feed only one image 110 | for (int i = 0; i < 5; ++i) { 111 | vio.addImage(okvis::Time(now), 0, image_cam); 112 | for (int j = 0; j < 10; ++j) { 113 | vio.addImuMeasurement(okvis::Time(now), imu_data.measurement.accelerometers, imu_data.measurement.gyroscopes); 114 | now += 0.01; 115 | } 116 | } 117 | // continue with pairs 118 | for (int i = 0; i < 3; ++i) { 119 | vio.addImage(okvis::Time(now), 0, image_cam); 120 | vio.addImage(okvis::Time(now), 1, image_cam); 121 | for (int j = 0; j < 10; ++j) { 122 | vio.addImuMeasurement(okvis::Time(now), imu_data.measurement.accelerometers, imu_data.measurement.gyroscopes); 123 | now += 0.01; 124 | } 125 | } 126 | } 127 | 128 | TEST(OkvisVioInterfaces, testImuFrameSync) 129 | { 130 | using namespace okvis; 131 | 132 | // configure mock object 133 | MockVioBackendInterface dummy; 134 | // EXPECT_CALL(dummy, detectAndDescribe(_,_,_,_,_,_)) 135 | // .Times(AtLeast(2)); 136 | // EXPECT_CALL(dummy, backendProcessing()) 137 | // .Times(AtLeast(1)); 138 | // EXPECT_CALL(dummy, propagation(_,_,_,_,_,_,_,_)) 139 | // .Times(Between(99, 100)); 140 | // 141 | // ImuFrameSynchronizer syncer(); 142 | // 143 | // std::thread test_consumer(); 144 | 145 | } 146 | -------------------------------------------------------------------------------- /okvis_multisensor_processing/test/testThreading.cpp: -------------------------------------------------------------------------------- 1 | #define GTEST_USE_OWN_TR1_TUPLE 0 2 | 3 | #include "gmock/gmock.h" 4 | #include "gtest/gtest.h" 5 | 6 | #pragma GCC diagnostic push 7 | #pragma GCC diagnostic ignored "-Wnon-virtual-dtor" 8 | #include 9 | #pragma GCC diagnostic pop 10 | 11 | #include 12 | 13 | #include 14 | 15 | #include "MockVioBackendInterface.hpp" 16 | #include "MockVioFrontendInterface.hpp" 17 | #include "testDataGenerators.hpp" 18 | 19 | using ::testing::AtLeast; 20 | using ::testing::Between; 21 | using ::testing::Return; 22 | using ::testing::_; 23 | 24 | TEST(OkvisVioInterfaces, testConstructionDestruction) 25 | { 26 | using namespace okvis; 27 | 28 | MockVioBackendInterface dummy; 29 | EXPECT_CALL(dummy, addCamera(_)) 30 | .Times(2); 31 | EXPECT_CALL(dummy, addImu(_)) 32 | .Times(1); 33 | EXPECT_CALL(dummy, currentFrameId()) 34 | .Times(1); 35 | EXPECT_CALL(dummy, get_T_WS(_,_)) 36 | .Times(1); 37 | 38 | MockVioFrontendInterface mock_frontend; 39 | okvis::VioParameters parameters; 40 | parameters.nCameraSystem = TestDataGenerator::getTestCameraSystem(2); 41 | // start with mock 42 | 43 | EXPECT_CALL(mock_frontend, setBriskDetectionOctaves(_)) 44 | .Times(1); 45 | EXPECT_CALL(mock_frontend, setBriskDetectionThreshold(_)) 46 | .Times(1); 47 | 48 | ThreadedKFVio vio(parameters); 49 | } 50 | 51 | /* ThreadedKFVio needs IMU data to work 52 | TEST(OkvisVioInterfaces, testDestructionWithImageData) 53 | { 54 | using namespace okvis; 55 | 56 | MockVioBackendInterface dummy; 57 | EXPECT_CALL(dummy, optimize(_,_,_)) 58 | .Times(0); 59 | 60 | MockVioFrontendInterface mock_frontend; 61 | EXPECT_CALL(mock_frontend, detectAndDescribe(_,_,_,_)) 62 | .Times(AtLeast(1)); 63 | EXPECT_CALL(mock_frontend, dataAssociationAndInitialization(_,_,_,_,_,_)) 64 | .Times(Between(0, 10)); 65 | EXPECT_CALL(mock_frontend, propagation(_,_,_,_,_,_,_,_)) 66 | .Times(0); 67 | 68 | okvis::VioParameters parameters; 69 | parameters.nCameraSystem = TestDataGenerator::getTestCameraSystem(2); 70 | 71 | // start with mock 72 | ThreadedKFVio vio(parameters, dummy, mock_frontend); 73 | vio.setBlocking(false); 74 | 75 | double now = okvis::Time::now().toSec(); 76 | 77 | // test Observation 78 | cv::Mat image_cam0; 79 | cv::Mat image_cam1; 80 | image_cam0 = cv::imread("testImage.jpg", 0); 81 | image_cam1 = cv::imread("testImage.jpg", 0); 82 | 83 | if (image_cam0.data == NULL) { 84 | std::cout << "testImage.jpg not found" << std::endl; 85 | } 86 | 87 | ASSERT_TRUE(image_cam0.data != NULL); 88 | ASSERT_TRUE(image_cam1.data != NULL); 89 | 90 | // simulate 100Hz IMU and 10Hz images 91 | for (int i = 0; i < 3; ++i) { 92 | vio.addImage(okvis::Time(now), 0, image_cam0); 93 | vio.addImage(okvis::Time(now), 1, image_cam1); 94 | now += 0.1; 95 | } 96 | } 97 | */ 98 | 99 | TEST(OkvisVioInterfaces, testDestructionWithIMUData) 100 | { 101 | using namespace okvis; 102 | 103 | MockVioBackendInterface dummy; 104 | EXPECT_CALL(dummy, addCamera(_)) 105 | .Times(2); 106 | EXPECT_CALL(dummy, addImu(_)) 107 | .Times(1); 108 | EXPECT_CALL(dummy, optimize(_,_,_)) 109 | .Times(0); 110 | EXPECT_CALL(dummy, currentFrameId()) 111 | .Times(1); 112 | EXPECT_CALL(dummy, get_T_WS(_,_)) 113 | .Times(1); 114 | EXPECT_CALL(dummy, setOptimizationTimeLimit(_,_)) 115 | .Times(1); 116 | 117 | MockVioFrontendInterface mock_frontend; 118 | EXPECT_CALL(mock_frontend, detectAndDescribe(_,_,_,_)) 119 | .Times(0); 120 | EXPECT_CALL(mock_frontend, propagation(_,_,_,_,_,_,_,_)) 121 | .Times(Between(9, 10)); 122 | EXPECT_CALL(mock_frontend, setBriskDetectionOctaves(_)) 123 | .Times(1); 124 | EXPECT_CALL(mock_frontend, setBriskDetectionThreshold(_)) 125 | .Times(1); 126 | 127 | okvis::VioParameters parameters; 128 | parameters.nCameraSystem = TestDataGenerator::getTestCameraSystem(2); 129 | parameters.publishing.publishImuPropagatedState = true; 130 | 131 | // start with mock 132 | ThreadedKFVio vio(parameters); 133 | vio.setBlocking(true); 134 | 135 | double now = okvis::Time::now().toSec(); 136 | 137 | // test Observation 138 | okvis::ImuMeasurement imu_data; 139 | imu_data.measurement.accelerometers.Zero(); 140 | imu_data.measurement.gyroscopes.Zero(); 141 | 142 | for (int j = 0; j < 10; ++j) { 143 | vio.addImuMeasurement(okvis::Time(now + j * 0.01), imu_data.measurement.accelerometers, imu_data.measurement.gyroscopes); 144 | } 145 | } 146 | -------------------------------------------------------------------------------- /okvis_multisensor_processing/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "gtest/gtest.h" 3 | #include "gmock/gmock.h" 4 | #include 5 | 6 | /// Run all the tests that were declared with TEST() 7 | int main(int argc, char **argv) { 8 | testing::InitGoogleTest(&argc, argv); 9 | 10 | google::InitGoogleLogging(argv[0]); 11 | FLAGS_stderrthreshold = 0; // INFO: 0, WARNING: 1, ERROR: 2, FATAL: 3 12 | FLAGS_colorlogtostderr = 1; 13 | 14 | return RUN_ALL_TESTS(); 15 | } 16 | -------------------------------------------------------------------------------- /okvis_time/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | 3 | project(okvis_time) 4 | 5 | IF(libvisensor_FOUND) 6 | add_definitions(-DHAVE_LIBVISENSOR) 7 | MESSAGE(STATUS "Found libvisensor. Setting HAVE_LIBVISENSOR flag.") 8 | ENDIF() 9 | 10 | # build the library 11 | add_library(${PROJECT_NAME} 12 | src/Time.cpp 13 | src/Duration.cpp) 14 | 15 | # installation if required 16 | install(TARGETS ${PROJECT_NAME} 17 | EXPORT okvisTargets 18 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 19 | ) 20 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 21 | -------------------------------------------------------------------------------- /okvis_time/include/okvis/implementation/Duration.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Modified and adapted by 5 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 6 | * 7 | * Original Copyright (c) 2010, Willow Garage, Inc. 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of Willow Garage, Inc. nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | *********************************************************************/ 37 | /* 38 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 39 | * Modified: Andreas Forster (an.forster@gmail.com) 40 | */ 41 | 42 | /** 43 | * @file implementation/Duration.hpp 44 | * @brief Header implementation file for the DurationBase class. 45 | * @author Willow Garage Inc. 46 | * @author Stefan Leutenegger 47 | * @author Andreas Forster 48 | */ 49 | 50 | #ifndef INCLUDE_OKVIS_IMPLEMENTATION_DURATION_HPP_ 51 | #define INCLUDE_OKVIS_IMPLEMENTATION_DURATION_HPP_ 52 | 53 | #include 54 | //#include 55 | 56 | /// \brief okvis Main namespace of this package. 57 | namespace okvis { 58 | // 59 | // DurationBase template member function implementation 60 | // 61 | template 62 | DurationBase::DurationBase(int32_t _sec, int32_t _nsec) 63 | : sec(_sec), 64 | nsec(_nsec) { 65 | normalizeSecNSecSigned(sec, nsec); 66 | } 67 | 68 | template 69 | T& DurationBase::fromSec(double d) { 70 | #ifdef HAVE_TRUNC 71 | sec = (int32_t)trunc(d); 72 | #else 73 | // (morgan: why doesn't win32 provide trunc? argh. hacked this together 74 | // without much thought. need to test this conversion. 75 | if (d >= 0.0) 76 | sec = (int32_t) floor(d); 77 | else 78 | sec = (int32_t) floor(d) + 1; 79 | #endif 80 | nsec = (int32_t)((d - (double) sec) * 1000000000); 81 | return *static_cast(this); 82 | } 83 | 84 | template 85 | T& DurationBase::fromNSec(int64_t t) { 86 | sec = (int32_t)(t / 1000000000); 87 | nsec = (int32_t)(t % 1000000000); 88 | 89 | normalizeSecNSecSigned(sec, nsec); 90 | 91 | return *static_cast(this); 92 | } 93 | 94 | template 95 | T DurationBase::operator+(const T &rhs) const { 96 | return T(sec + rhs.sec, nsec + rhs.nsec); 97 | } 98 | 99 | template 100 | T DurationBase::operator*(double scale) const { 101 | return T(toSec() * scale); 102 | } 103 | 104 | template 105 | T DurationBase::operator-(const T &rhs) const { 106 | return T(sec - rhs.sec, nsec - rhs.nsec); 107 | } 108 | 109 | template 110 | T DurationBase::operator-() const { 111 | return T(-sec, -nsec); 112 | } 113 | 114 | template 115 | T& DurationBase::operator+=(const T &rhs) { 116 | *this = *this + rhs; 117 | return *static_cast(this); 118 | } 119 | 120 | template 121 | T& DurationBase::operator-=(const T &rhs) { 122 | *this += (-rhs); 123 | return *static_cast(this); 124 | } 125 | 126 | template 127 | T& DurationBase::operator*=(double scale) { 128 | fromSec(toSec() * scale); 129 | return *static_cast(this); 130 | } 131 | 132 | template 133 | bool DurationBase::operator<(const T &rhs) const { 134 | if (sec < rhs.sec) 135 | return true; 136 | else if (sec == rhs.sec && nsec < rhs.nsec) 137 | return true; 138 | return false; 139 | } 140 | 141 | template 142 | bool DurationBase::operator>(const T &rhs) const { 143 | if (sec > rhs.sec) 144 | return true; 145 | else if (sec == rhs.sec && nsec > rhs.nsec) 146 | return true; 147 | return false; 148 | } 149 | 150 | template 151 | bool DurationBase::operator<=(const T &rhs) const { 152 | if (sec < rhs.sec) 153 | return true; 154 | else if (sec == rhs.sec && nsec <= rhs.nsec) 155 | return true; 156 | return false; 157 | } 158 | 159 | template 160 | bool DurationBase::operator>=(const T &rhs) const { 161 | if (sec > rhs.sec) 162 | return true; 163 | else if (sec == rhs.sec && nsec >= rhs.nsec) 164 | return true; 165 | return false; 166 | } 167 | 168 | template 169 | bool DurationBase::operator==(const T &rhs) const { 170 | return sec == rhs.sec && nsec == rhs.nsec; 171 | } 172 | 173 | template 174 | bool DurationBase::isZero() { 175 | return sec == 0 && nsec == 0; 176 | } 177 | } 178 | 179 | #endif // INCLUDE_OKVIS_IMPLEMENTATION_DURATION_HPP_ 180 | -------------------------------------------------------------------------------- /okvis_time/include/okvis/implementation/Time.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Modified and adapted by 5 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 6 | * 7 | * Original Copyright (c) 2008, Willow Garage, Inc. 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of Willow Garage, Inc. nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | *********************************************************************/ 37 | /* 38 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 39 | * Modified: Andreas Forster (an.forster@gmail.com) 40 | */ 41 | 42 | /** 43 | * @file implementation/Time.hpp 44 | * @brief Header file for the TimeBase class. 45 | * @author Willow Garage Inc. 46 | * @author Stefan Leutenegger 47 | * @author Andreas Forster 48 | */ 49 | 50 | 51 | #ifndef INCLUDE_OKVIS_IMPLEMENTATION_TIME_HPP_ 52 | #define INCLUDE_OKVIS_IMPLEMENTATION_TIME_HPP_ 53 | 54 | /********************************************************************* 55 | ** Headers 56 | *********************************************************************/ 57 | 58 | //#include 59 | #include 60 | #include 61 | //#include 62 | 63 | /********************************************************************* 64 | ** Cross Platform Headers 65 | *********************************************************************/ 66 | 67 | #ifdef WIN32 68 | #include 69 | #else 70 | #include 71 | #endif 72 | 73 | /// \brief okvis Main namespace of this package. 74 | namespace okvis { 75 | 76 | template 77 | T& TimeBase::fromNSec(uint64_t t) { 78 | sec = (int32_t)(t / 1000000000); 79 | nsec = (int32_t)(t % 1000000000); 80 | 81 | normalizeSecNSec(sec, nsec); 82 | 83 | return *static_cast(this); 84 | } 85 | 86 | template 87 | D TimeBase::operator-(const T &rhs) const { 88 | return D((int32_t) sec - (int32_t) rhs.sec, 89 | (int32_t) nsec - (int32_t) rhs.nsec); // carry handled in ctor 90 | } 91 | 92 | template 93 | T TimeBase::operator-(const D &rhs) const { 94 | return *static_cast(this) + (-rhs); 95 | } 96 | 97 | template 98 | T TimeBase::operator+(const D &rhs) const { 99 | int64_t sec_sum = (int64_t) sec + (int64_t) rhs.sec; 100 | int64_t nsec_sum = (int64_t) nsec + (int64_t) rhs.nsec; 101 | 102 | // Throws an exception if we go out of 32-bit range 103 | normalizeSecNSecUnsigned(sec_sum, nsec_sum); 104 | 105 | // now, it's safe to downcast back to uint32 bits 106 | return T((uint32_t) sec_sum, (uint32_t) nsec_sum); 107 | } 108 | 109 | template 110 | T& TimeBase::operator+=(const D &rhs) { 111 | *this = *this + rhs; 112 | return *static_cast(this); 113 | } 114 | 115 | template 116 | T& TimeBase::operator-=(const D &rhs) { 117 | *this += (-rhs); 118 | return *static_cast(this); 119 | } 120 | 121 | template 122 | bool TimeBase::operator==(const T &rhs) const { 123 | return sec == rhs.sec && nsec == rhs.nsec; 124 | } 125 | 126 | template 127 | bool TimeBase::operator<(const T &rhs) const { 128 | if (sec < rhs.sec) 129 | return true; 130 | else if (sec == rhs.sec && nsec < rhs.nsec) 131 | return true; 132 | return false; 133 | } 134 | 135 | template 136 | bool TimeBase::operator>(const T &rhs) const { 137 | if (sec > rhs.sec) 138 | return true; 139 | else if (sec == rhs.sec && nsec > rhs.nsec) 140 | return true; 141 | return false; 142 | } 143 | 144 | template 145 | bool TimeBase::operator<=(const T &rhs) const { 146 | if (sec < rhs.sec) 147 | return true; 148 | else if (sec == rhs.sec && nsec <= rhs.nsec) 149 | return true; 150 | return false; 151 | } 152 | 153 | template 154 | bool TimeBase::operator>=(const T &rhs) const { 155 | if (sec > rhs.sec) 156 | return true; 157 | else if (sec == rhs.sec && nsec >= rhs.nsec) 158 | return true; 159 | return false; 160 | } 161 | 162 | } 163 | 164 | #endif // INCLUDE_OKVIS_IMPLEMENTATION_TIME_HPP_ 165 | 166 | -------------------------------------------------------------------------------- /okvis_time/src/Duration.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Modified and adapted by 5 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 6 | * 7 | * Original Copyright (c) 2008, Willow Garage, Inc. 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of Willow Garage, Inc. nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | *********************************************************************/ 37 | /* 38 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 39 | */ 40 | 41 | /** 42 | * @file Duration.cpp 43 | * @brief This file contains osme helper function for the Duration class. 44 | * @author Willow Garage Inc. 45 | * @author Stefan Leutenegger 46 | * @author Andreas Forster 47 | */ 48 | 49 | #include 50 | #include 51 | 52 | /// \brief okvis Main namespace of this package. 53 | namespace okvis { 54 | 55 | void normalizeSecNSecSigned(int64_t& sec, int64_t& nsec) { 56 | int64_t nsec_part = nsec; 57 | int64_t sec_part = sec; 58 | 59 | while (nsec_part > 1000000000L) { 60 | nsec_part -= 1000000000L; 61 | ++sec_part; 62 | } 63 | while (nsec_part < 0) { 64 | nsec_part += 1000000000L; 65 | --sec_part; 66 | } 67 | 68 | if (sec_part < INT_MIN || sec_part > INT_MAX) 69 | throw std::runtime_error("Duration is out of dual 32-bit range"); 70 | 71 | sec = sec_part; 72 | nsec = nsec_part; 73 | } 74 | 75 | void normalizeSecNSecSigned(int32_t& sec, int32_t& nsec) { 76 | int64_t sec64 = sec; 77 | int64_t nsec64 = nsec; 78 | 79 | normalizeSecNSecSigned(sec64, nsec64); 80 | 81 | sec = (int32_t) sec64; 82 | nsec = (int32_t) nsec64; 83 | } 84 | 85 | template class DurationBase ; 86 | template class DurationBase ; 87 | } 88 | 89 | -------------------------------------------------------------------------------- /okvis_timing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | project(okvis_timing) 3 | 4 | add_library(${PROJECT_NAME} 5 | src/Timer.cpp 6 | src/NsecTimeUtilities.cpp 7 | ) 8 | 9 | target_link_libraries(${PROJECT_NAME} PUBLIC okvis_util) 10 | 11 | # installation if required 12 | install(TARGETS ${PROJECT_NAME} 13 | EXPORT okvisTargets 14 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 15 | ) 16 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 17 | -------------------------------------------------------------------------------- /okvis_timing/include/okvis/timing/NsecTimeUtilities.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Copyright (c) 2011-2013, Paul Furgale and others. 6 | * All rights reserved. 7 | * 8 | * Unlike otherwise stated in source files, the code in this repository is 9 | * published under the Revised BSD (New BSD) license. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above copyright 16 | * notice, this list of conditions and the following disclaimer in the 17 | * documentation and/or other materials provided with the distribution. 18 | * * Neither the name of the nor the 19 | * names of its contributors may be used to endorse or promote products 20 | * derived from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 23 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 24 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 26 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 29 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 31 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *********************************************************************************/ 33 | 34 | /** 35 | * @file NsecTimeUtilities.hpp 36 | * @author Paul Furgale 37 | * @date Sat Jul 20 12:39:54 2013 38 | * 39 | * @brief Functions to support the use of nanosecond epoch time. 40 | * 41 | */ 42 | #ifndef INCLUDE_OKVIS_TIMING_NSECTIMEUTILITIES_HPP_ 43 | #define INCLUDE_OKVIS_TIMING_NSECTIMEUTILITIES_HPP_ 44 | #include 45 | #include 46 | 47 | namespace okvis { 48 | namespace timing { 49 | 50 | /// \brief Nanoseconds since the epoch. 51 | typedef boost::int64_t NsecTime; 52 | 53 | /// \brief Convert nanoseconds since the epoch to std::chrono 54 | std::chrono::system_clock::time_point nsecToChrono( const NsecTime & time ); 55 | 56 | /// \brief Convert std::chrono to nanoseconds since the epoch. 57 | NsecTime chronoToNsec( const std::chrono::system_clock::time_point & time ); 58 | 59 | /// \brief Get the epoch time as nanoseconds since the epoch. 60 | NsecTime nsecNow(); 61 | 62 | /// \brief Convert the time (in integer nanoseconds) to decimal seconds. 63 | double nsecToSec( const NsecTime & time ); 64 | 65 | /// \brief Convert the time (in seconds) to integer nanoseconds 66 | NsecTime secToNsec( const double & time ); 67 | 68 | /// \brief return a magic number representing an invalid timestamp 69 | constexpr NsecTime getInvalidTime(); 70 | 71 | /// \brief Is the time valid? This uses a magic number 72 | /// std::numeric_limits::min() to represent an invalid time 73 | bool isValid(const NsecTime & time); 74 | 75 | } // namespace timing 76 | } // namespace okvis 77 | 78 | #endif /* INCLUDE_OKVIS_TIMING_NSECTIMEUTILITIES_HPP_ */ 79 | -------------------------------------------------------------------------------- /okvis_timing/src/NsecTimeUtilities.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Copyright (c) 2011-2013, Paul Furgale and others. 6 | * All rights reserved. 7 | * 8 | * Unlike otherwise stated in source files, the code in this repository is 9 | * published under the Revised BSD (New BSD) license. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above copyright 16 | * notice, this list of conditions and the following disclaimer in the 17 | * documentation and/or other materials provided with the distribution. 18 | * * Neither the name of the nor the 19 | * names of its contributors may be used to endorse or promote products 20 | * derived from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 23 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 24 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 26 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 29 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 31 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *********************************************************************************/ 33 | 34 | #include 35 | 36 | namespace okvis { 37 | namespace timing { 38 | 39 | /// \brief Convert nanoseconds since the epoch to std::chrono 40 | std::chrono::system_clock::time_point nsecToChrono( const NsecTime & time ) { 41 | std::chrono::nanoseconds tt(time); 42 | std::chrono::system_clock::time_point tp(std::chrono::duration_cast(tt)); 43 | return tp; 44 | } 45 | 46 | 47 | /// \brief Convert std::chrono to nanoseconds since the epoch. 48 | NsecTime chronoToNsec( const std::chrono::system_clock::time_point & time ) { 49 | return std::chrono::duration_cast( time.time_since_epoch()).count(); 50 | } 51 | 52 | 53 | /// \brief Get the epoch time as nanoseconds since the epoch. 54 | NsecTime nsecNow() { 55 | return std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch() ).count(); 56 | 57 | } 58 | 59 | 60 | /// \brief Convert the time (in integer nanoseconds) to decimal seconds. 61 | double nsecToSec( const NsecTime & time ) { 62 | return (double) time * 1e-9; 63 | } 64 | 65 | 66 | /// \brief Convert the time (in seconds) to integer nanoseconds 67 | NsecTime secToNsec( const double & time ) { 68 | return boost::int64_t( time * 1e9 ); 69 | } 70 | 71 | constexpr NsecTime getInvalidTime() { 72 | return std::numeric_limits::min(); 73 | } 74 | 75 | bool isValid(const NsecTime& time) { 76 | return getInvalidTime() == time; 77 | } 78 | 79 | } // namespace timing 80 | } // namespace okvis 81 | -------------------------------------------------------------------------------- /okvis_timing/test/TestNsecTimeUtilities.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | TEST( NsetTimeTestSuite, testChronoConversion ) { 5 | 6 | std::chrono::system_clock::time_point tp1 = std::chrono::system_clock::now(); 7 | okvis::timing::NsecTime ns1 = okvis::timing::chronoToNsec( tp1 ); 8 | std::chrono::system_clock::time_point tp2 = okvis::timing::nsecToChrono( ns1 ); 9 | ASSERT_TRUE(tp1 == tp2); 10 | 11 | } 12 | 13 | 14 | TEST( NsetTimeTestSuite, testSecConversion ) { 15 | 16 | okvis::timing::NsecTime ns1 = okvis::timing::nsecNow(); 17 | double s2 = okvis::timing::nsecToSec(ns1); 18 | okvis::timing::NsecTime ns2 = okvis::timing::secToNsec(s2); 19 | 20 | ASSERT_LT(abs(ns1-ns2), 1000000); 21 | 22 | } 23 | -------------------------------------------------------------------------------- /okvis_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 | -------------------------------------------------------------------------------- /okvis_util/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | project(okvis_util) 3 | 4 | # nothing to build for now... 5 | add_library(${PROJECT_NAME} STATIC src/dependency-tracker.cc) 6 | 7 | # installation if required 8 | install(TARGETS ${PROJECT_NAME} 9 | EXPORT okvisTargets 10 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 11 | PUBLIC_HEADER DESTINATION "${INSTALL_INCLUDE_DIR}/okvis" COMPONENT dev 12 | ) 13 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 14 | -------------------------------------------------------------------------------- /okvis_util/include/okvis/source_file_pos.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: 30 | * Author: Paul Furgale 31 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 32 | *********************************************************************************/ 33 | 34 | /** 35 | * @file source_file_pos.hpp 36 | * @brief This file contains some helper functions for the assert macros. 37 | * @author Paul Furgale 38 | * @author Stefan Leutenegger 39 | */ 40 | 41 | #ifndef OKVIS_SOURCE_FILE_POS_HPP 42 | #define OKVIS_SOURCE_FILE_POS_HPP 43 | 44 | #include 45 | #include 46 | #include 47 | // A class and macro that gives you the current file position. 48 | 49 | /// \brief okvis Main namespace of this package. 50 | namespace okvis { 51 | 52 | class source_file_pos 53 | { 54 | public: 55 | std::string function; 56 | std::string file; 57 | int line; 58 | 59 | source_file_pos(std::string function, std::string file, int line) : 60 | function(function), file(file), line(line) {} 61 | 62 | operator std::string() 63 | { 64 | return toString(); 65 | } 66 | 67 | std::string toString() const 68 | { 69 | std::stringstream s; 70 | s << file << ":" << line << ": " << function << "()"; 71 | return s.str(); 72 | } 73 | 74 | }; 75 | 76 | }// namespace okvis 77 | 78 | inline std::ostream & operator<<(std::ostream & out, const okvis::source_file_pos & sfp) 79 | { 80 | out << sfp.file << ":" << sfp.line << ": " << sfp.function << "()"; 81 | return out; 82 | } 83 | 84 | 85 | #define OKVIS_SOURCE_FILE_POS okvis::source_file_pos(__FUNCTION__,__FILE__,__LINE__) 86 | 87 | #endif // OKVIS_SOURCE_FILE_POS_HPP 88 | -------------------------------------------------------------------------------- /okvis_util/src/dependency-tracker.cc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/okvis/f0c9ba472b89f85030cd58dfbae03ceec7817679/okvis_util/src/dependency-tracker.cc --------------------------------------------------------------------------------