├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── Doxyfile.in ├── LICENSE ├── README.md ├── cfg ├── euroc_cam0.yaml ├── euroc_cam1.yaml ├── p22021_equidist.yaml └── rovio.info ├── cmake ├── DetectSIMD.cmake └── FindEigen.cmake ├── include └── rovio │ ├── Camera.hpp │ ├── CameraCalibration.hpp │ ├── CoordinateTransform │ ├── FeatureOutput.hpp │ ├── FeatureOutputReadable.hpp │ ├── LandmarkOutput.hpp │ ├── PixelOutput.hpp │ ├── RovioOutput.hpp │ └── YprOutput.hpp │ ├── FeatureCoordinates.hpp │ ├── FeatureDistance.hpp │ ├── FeatureManager.hpp │ ├── FeatureStatistics.hpp │ ├── FilterConfiguration.hpp │ ├── FilterStates.hpp │ ├── ImagePyramid.hpp │ ├── ImgUpdate.hpp │ ├── ImuPrediction.hpp │ ├── LocalizationLandmarkUpdate.hpp │ ├── Memory.hpp │ ├── MultiCamera.hpp │ ├── MultilevelPatch.hpp │ ├── MultilevelPatchAlignment.hpp │ ├── Patch.hpp │ ├── PoseUpdate.hpp │ ├── RobocentricFeatureElement.hpp │ ├── RovioFilter.hpp │ ├── RovioInterface.hpp │ ├── RovioInterfaceBuilder.hpp │ ├── RovioInterfaceImpl.hpp │ ├── RovioInterfaceImplInl.hpp │ ├── RovioInterfaceStates.hpp │ ├── RovioInterfaceStatesImpl.hpp │ ├── RovioRosNode.hpp │ ├── RovioScene.hpp │ ├── Scene.hpp │ ├── VelocityUpdate.hpp │ ├── ZeroVelocityUpdate.hpp │ ├── exceptions.hpp │ └── featureTracker.hpp ├── launch ├── rovio_node.launch ├── rovio_rosbag_node.launch └── valgrind_rovio.launch ├── mainpage.dox ├── package.xml ├── shaders ├── shader.fs └── shader.vs ├── src ├── Camera.cpp ├── CameraCalibration.cpp ├── FeatureCoordinates.cpp ├── FeatureDistance.cpp ├── RovioInterfaces.cpp ├── Scene.cpp ├── feature_tracker_node.cpp ├── rovio_node.cpp ├── rovio_rosbag_loader.cpp ├── test_localization_landmark_update.cpp ├── test_mlp.cpp └── test_patch.cpp └── srv └── SrvResetToPose.srv /.gitignore: -------------------------------------------------------------------------------- 1 | # CMake 2 | CMakeCache.txt 3 | CMakeFiles 4 | Makefile 5 | cmake_install.cmake 6 | install_manifest.txt 7 | gtest 8 | rovio_custom.info 9 | *.bag 10 | *.active 11 | 12 | # Compiled Object files 13 | *.slo 14 | *.lo 15 | *.o 16 | 17 | # Compiled Dynamic libraries 18 | *.so 19 | *.dylib 20 | 21 | # Compiled Static libraries 22 | *.lai 23 | *.la 24 | *.a 25 | 26 | *~ 27 | .* 28 | 29 | html/ 30 | latex/ 31 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "lightweight_filtering"] 2 | path = lightweight_filtering 3 | url = https://github.com/ethz-asl/maplab_lightweight_filtering.git 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.6) 2 | project(rovio) 3 | include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/DetectSIMD.cmake) 4 | set(ROVIO_NMAXFEATURE 25 CACHE STRING "Number of features for ROVIO") 5 | set(ROVIO_NCAM 1 CACHE STRING "Number of enabled cameras") 6 | set(ROVIO_NLEVELS 4 CACHE STRING "Number of image leavels for the features") 7 | set(ROVIO_PATCHSIZE 6 CACHE STRING "Size of patch (edge length in pixel)") 8 | set(ROVIO_NPOSE 0 CACHE STRING "Additional estimated poses for external pose measurements") 9 | add_definitions(-DROVIO_NMAXFEATURE=${ROVIO_NMAXFEATURE}) 10 | add_definitions(-DROVIO_NCAM=${ROVIO_NCAM}) 11 | add_definitions(-DROVIO_NLEVELS=${ROVIO_NLEVELS}) 12 | add_definitions(-DROVIO_PATCHSIZE=${ROVIO_PATCHSIZE}) 13 | add_definitions(-DROVIO_NPOSE=${ROVIO_NPOSE}) 14 | add_definitions(-std=c++11) 15 | 16 | add_subdirectory(lightweight_filtering) 17 | 18 | ##################### Find, include, and compile library ##################### 19 | set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH}) 20 | if(0) 21 | find_package(OpenMP REQUIRED) 22 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 23 | endif() 24 | 25 | find_package(Eigen REQUIRED) 26 | include_directories(${EIGEN_INCLUDE_DIR}) 27 | 28 | if(MAKE_SCENE) 29 | message(STATUS "Building ROVIO with openGL Scene Visualization") 30 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAKE_SCENE=1") 31 | find_package(GLEW REQUIRED) 32 | include_directories(${GLEW_INCLUDE_DIRS}) 33 | 34 | find_package(GLUT REQUIRED) 35 | include_directories(${GLUT_INCLUDE_DIRS}) 36 | link_directories(${GLUT_LIBRARY_DIRS}) 37 | add_definitions(${GLUT_DEFINITIONS}) 38 | 39 | find_package(OpenGL REQUIRED) 40 | include_directories(${OpenGL_INCLUDE_DIRS}) 41 | link_directories(${OpenGL_LIBRARY_DIRS}) 42 | add_definitions(${OpenGL_DEFINITIONS}) 43 | endif() 44 | 45 | find_package(PkgConfig) 46 | 47 | find_package(glog_catkin REQUIRED) 48 | 49 | find_package(opencv3_catkin QUIET) 50 | if(opencv3_catkin_FOUND) 51 | message(STATUS "Found opencv3_catkin!") 52 | set(OPENCV_PACKAGE "opencv3_catkin") 53 | else() 54 | message(STATUS "Use the systems opencv installation!") 55 | set(OPENCV_PACKAGE "") 56 | endif() 57 | 58 | find_package(yaml_cpp_catkin QUIET) 59 | if(${yaml_cpp_catkin_FOUND}) 60 | message(STATUS "Found yaml_cpp_catkin, using instead of system library.") 61 | set(YamlCpp_LIBRARIES ${yaml_cpp_catkin_LIBRARIES}) 62 | set(YamlCpp_INCLUDE_DIRS ${yaml_cpp_catkin_INCLUDE_DIRS}) 63 | else() 64 | message(STATUS "No yaml_cpp_catkin, using yaml-cpp system library instead.") 65 | pkg_check_modules(YamlCpp REQUIRED yaml-cpp>=0.5) 66 | endif() 67 | 68 | 69 | ##################### Install ROS stuff ##################### 70 | find_package(catkin REQUIRED COMPONENTS 71 | ${OPENCV_PACKAGE} 72 | cv_bridge 73 | geometry_msgs 74 | kindr 75 | lightweight_filtering 76 | message_generation 77 | nav_msgs 78 | rosbag 79 | roscpp 80 | roslib 81 | sensor_msgs 82 | std_msgs 83 | tf 84 | ) 85 | 86 | add_service_files( 87 | DIRECTORY srv 88 | FILES SrvResetToPose.srv 89 | ) 90 | 91 | generate_messages(DEPENDENCIES 92 | std_msgs 93 | geometry_msgs 94 | ) 95 | 96 | catkin_package( 97 | INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} 98 | LIBRARIES ${PROJECT_NAME} 99 | CATKIN_DEPENDS 100 | ${OPENCV_PACKAGE} 101 | cv_bridge 102 | geometry_msgs 103 | kindr 104 | lightweight_filtering 105 | message_runtime 106 | nav_msgs 107 | rosbag 108 | roscpp 109 | roslib 110 | sensor_msgs 111 | std_msgs 112 | tf 113 | yaml_cpp_catkin 114 | ) 115 | 116 | include_directories(include ${catkin_INCLUDE_DIRS} ${YamlCpp_INCLUDE_DIRS} ${glog_catkin_INCLUDE_DIRS}) 117 | 118 | set(ROVIO_SRCS 119 | src/Camera.cpp 120 | src/CameraCalibration.cpp 121 | src/FeatureCoordinates.cpp 122 | src/FeatureDistance.cpp 123 | src/RovioInterfaces.cpp 124 | ) 125 | 126 | # Optionally also build the SCENE files. 127 | if(MAKE_SCENE) 128 | set(ROVIO_SRCS ${ROVIO_SRCS} src/Scene.cpp) 129 | endif() 130 | 131 | add_library(${PROJECT_NAME} ${ROVIO_SRCS}) 132 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${YamlCpp_LIBRARIES} ${OpenMP_EXE_LINKER_FLAGS} ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} ${GLEW_LIBRARY} ${glog_catkin_LIBRARIES}) 133 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} rovio_generate_messages_cpp) 134 | 135 | add_executable(rovio_node src/rovio_node.cpp) 136 | target_link_libraries(rovio_node ${PROJECT_NAME}) 137 | 138 | add_executable(rovio_rosbag_loader src/rovio_rosbag_loader.cpp) 139 | target_link_libraries(rovio_rosbag_loader ${PROJECT_NAME}) 140 | add_dependencies(rovio_rosbag_loader ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 141 | 142 | add_executable(feature_tracker_node src/feature_tracker_node.cpp) 143 | target_link_libraries(feature_tracker_node ${PROJECT_NAME}) 144 | add_dependencies(feature_tracker_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 145 | 146 | if(CATKIN_ENABLE_TESTING) 147 | message(STATUS "Building GTests!") 148 | option(BUILD_GTEST "build gtest" ON) 149 | #add_subdirectory(gtest gtest) 150 | enable_testing() 151 | include_directories(${gtest_SOURCE_DIR}/include ${gtest_SOURCE_DIR}) 152 | add_executable(test_patch src/test_patch.cpp src/Camera.cpp src/FeatureCoordinates.cpp src/FeatureDistance.cpp) 153 | target_link_libraries(test_patch gtest_main gtest pthread ${PROJECT_NAME} ${catkin_LIBRARIES} ${YamlCpp_LIBRARIES} ${glog_catkin_LIBRARIES} ) 154 | add_test(test_patch test_patch) 155 | 156 | add_executable(test_mlp src/test_mlp.cpp src/Camera.cpp src/FeatureCoordinates.cpp src/FeatureDistance.cpp) 157 | target_link_libraries(test_mlp gtest_main gtest pthread ${PROJECT_NAME} ${catkin_LIBRARIES} ${YamlCpp_LIBRARIES} ${glog_catkin_LIBRARIES}) 158 | add_test(test_mlp test_mlp) 159 | 160 | add_executable(test_llu src/test_localization_landmark_update.cpp) 161 | target_link_libraries(test_llu gtest_main gtest pthread ${PROJECT_NAME} ${catkin_LIBRARIES} ${YamlCpp_LIBRARIES} ${glog_catkin_LIBRARIES}) 162 | add_test(test_llu test_llu) 163 | endif() 164 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2014, Autonomous Systems Lab 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | * Redistributions of source code must retain the above copyright 7 | notice, this list of conditions and the following disclaimer. 8 | * Redistributions in binary form must reproduce the above copyright 9 | notice, this list of conditions and the following disclaimer in the 10 | documentation and/or other materials provided with the distribution. 11 | * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 12 | names of its contributors may be used to endorse or promote products 13 | derived from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 19 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 21 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 22 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 23 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 24 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | **This repository is a hard-fork of https://github.com/ethz-asl/rovio to integrate it with maplab and provide localization and map-building capabilities.** 2 | 3 | # README # 4 | 5 | This repository contains the ROVIO (Robust Visual Inertial Odometry) framework. The code is open-source (BSD License). Please remember that it is strongly coupled to on-going research and thus some parts are not fully mature yet. Furthermore, the code will also be subject to changes in the future which could include greater re-factoring of some parts. 6 | 7 | Author: Michael Bloesch 8 | 9 | Video: https://youtu.be/ZMAISVy-6ao 10 | 11 | Papers: 12 | * http://dx.doi.org/10.3929/ethz-a-010566547 (IROS 2015) 13 | * http://dx.doi.org/10.1177/0278364917728574 (IJRR 2017) 14 | 15 | Please also have a look at the wiki: https://github.com/ethz-asl/rovio/wiki 16 | 17 | ### Install without opengl scene ### 18 | Dependencies: 19 | * ros 20 | * kindr (https://github.com/ethz-asl/kindr) 21 | * lightweight_filtering (as submodule, use "git submodule update --init --recursive") 22 | 23 | ``` 24 | #!command 25 | 26 | catkin build rovio --cmake-args -DCMAKE_BUILD_TYPE=Release 27 | ``` 28 | 29 | ### Install with opengl scene ### 30 | Additional dependencies: opengl, glut, glew (sudo apt-get install freeglut3-dev, sudo apt-get install libglew-dev) 31 | ``` 32 | #!command 33 | 34 | catkin build rovio --cmake-args -DCMAKE_BUILD_TYPE=Release -DMAKE_SCENE=ON 35 | ``` 36 | 37 | ### Euroc Datasets ### 38 | The rovio_node.launch file loads parameters such that ROVIO runs properly on the Euroc datasets. The datasets are available under: 39 | http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets 40 | 41 | ### Further notes ### 42 | * Camera matrix and distortion parameters should be provided by a yaml file or loaded through rosparam 43 | * The cfg/rovio.info provides most parameters for rovio. The camera extrinsics qCM (quaternion from IMU to camera frame, Hamilton-convention) and MrMC (Translation between IMU and Camera expressed in the IMU frame) should also be set there. They are being estimated during runtime so only a rough guess should be sufficient. 44 | * Especially for application with little motion fixing the IMU-camera extrinsics can be beneficial. This can be done by setting the parameter doVECalibration to false. Please be carefull that the overall robustness and accuracy can be very sensitive to bad extrinsic calibrations. 45 | -------------------------------------------------------------------------------- /cfg/euroc_cam0.yaml: -------------------------------------------------------------------------------- 1 | ###### Camera Calibration File For Cam 0 of Euroc Datasets ###### 2 | image_width: 752 3 | image_height: 480 4 | camera_name: cam0 5 | camera_matrix: 6 | rows: 3 7 | cols: 3 8 | data: [458.654, 0.0, 367.215, 0.0, 457.296, 248.375, 0.0, 0.0, 1.0] 9 | distortion_model: plumb_bob 10 | distortion_coefficients: 11 | rows: 1 12 | cols: 5 13 | data: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0] 14 | 15 | ###### Original Calibration File ###### 16 | ## General sensor definitions. 17 | #sensor_type: camera 18 | #comment: VI-Sensor cam0 (MT9M034) 19 | # 20 | ## Sensor extrinsics wrt. the body-frame. 21 | #T_BS: 22 | # cols: 4 23 | # rows: 4 24 | # data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, 25 | # 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, 26 | # -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, 27 | # 0.0, 0.0, 0.0, 1.0] 28 | # 29 | ## Camera specific definitions. 30 | #rate_hz: 20 31 | #resolution: [752, 480] 32 | #camera_model: pinhole 33 | #intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv 34 | #distortion_model: radial-tangential 35 | #distortion_coefficients: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] 36 | 37 | 38 | -------------------------------------------------------------------------------- /cfg/euroc_cam1.yaml: -------------------------------------------------------------------------------- 1 | ###### Camera Calibration File For Cam 1 of Euroc Datasets ###### 2 | image_width: 752 3 | image_height: 480 4 | camera_name: cam1 5 | camera_matrix: 6 | rows: 3 7 | cols: 3 8 | data: [457.587, 0.0, 379.999, 0.0, 456.134, 255.238, 0.0, 0.0, 1.0] 9 | distortion_model: plumb_bob 10 | distortion_coefficients: 11 | rows: 1 12 | cols: 5 13 | data: [-0.28368365, 0.07451284, -0.00010473, -3.55590700e-05, 0.0] 14 | 15 | ###### Original Calibration File ###### 16 | ## General sensor definitions. 17 | #sensor_type: camera 18 | #comment: VI-Sensor cam1 (MT9M034) 19 | # 20 | ## Sensor extrinsics wrt. the body-frame. 21 | #T_BS: 22 | # cols: 4 23 | # rows: 4 24 | # data: [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556, 25 | # 0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024, 26 | # -0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038, 27 | # 0.0, 0.0, 0.0, 1.0] 28 | # 29 | ## Camera specific definitions. 30 | #rate_hz: 20 31 | #resolution: [752, 480] 32 | #camera_model: pinhole 33 | #intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv 34 | #distortion_model: radial-tangential 35 | #distortion_coefficients: [-0.28368365, 0.07451284, -0.00010473, -3.55590700e-05] 36 | -------------------------------------------------------------------------------- /cfg/p22021_equidist.yaml: -------------------------------------------------------------------------------- 1 | image_width: 752 2 | image_height: 480 3 | camera_name: cam0 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [470.3492376936415, 0.0, 369.1539326770507, 0.0, 468.5354540935562, 243.7389899263136, 0.0, 0.0, 1.0] 8 | distortion_model: equidistant 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 4 12 | data: [-0.0028217288073635748, 0.01424046033816201, -0.017070691627755135, 13 | 0.01198286934702205] 14 | rectification_matrix: 15 | rows: 3 16 | cols: 3 17 | data: [0.9999662253908055, 0.0012167240196736123, -0.008128201543077645, -0.00123142707670882, 0.9999976143402003, -0.0018041344913520734, 0.008125987018183612, 0.0018140828448797777, 0.9999653381184831] 18 | projection_matrix: 19 | rows: 3 20 | cols: 4 21 | data: [431.8121136491133, 0.0, 395.5353126525879, 0.0, 0.0, 431.8121136491133, 235.97241592407227, 0.0, 0.0, 0.0, 1.0, 0.0] 22 | -------------------------------------------------------------------------------- /cmake/DetectSIMD.cmake: -------------------------------------------------------------------------------- 1 | # Detect the preprocessor directives which are set by the compiler. 2 | execute_process(COMMAND ${CMAKE_CXX_COMPILER} -march=native -dM -E -x c /dev/null 3 | OUTPUT_VARIABLE PREPROCESSOR_DIRECTIVES) 4 | 5 | set(IS_SSE_ENABLED FALSE) 6 | set(IS_NEON_ENABLED FALSE) 7 | if (PREPROCESSOR_DIRECTIVES MATCHES "__SSSE3__") 8 | add_definitions(-mssse3) 9 | set(IS_SSE_ENABLED TRUE) 10 | # For both armv7 and armv8, __ARM_NEON is used as preprocessor directive. 11 | elseif (PREPROCESSOR_DIRECTIVES MATCHES "__ARM_ARCH 7") 12 | add_definitions(-mfpu=neon) # Needs to be set for armv7. 13 | set(IS_NEON_ENABLED TRUE) 14 | elseif (PREPROCESSOR_DIRECTIVES MATCHES "__ARM_ARCH 8") 15 | set(IS_NEON_ENABLED TRUE) 16 | else() 17 | message(WARNING "No SIMD instruction set detected.") 18 | endif() 19 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /include/rovio/Camera.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Autonomous Systems Lab 3 | * All rights reserved. 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 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #ifndef ROVIO_CAMERA_HPP_ 30 | #define ROVIO_CAMERA_HPP_ 31 | 32 | #include 33 | 34 | #include "lightweight_filtering/State.hpp" 35 | #include "lightweight_filtering/common.hpp" 36 | #include "rovio/CameraCalibration.hpp" 37 | 38 | namespace rovio{ 39 | 40 | class Camera{ 41 | public: 42 | 43 | Eigen::Matrix3d K_; //!< Intrinsic parameter matrix. 44 | 45 | /** \brief Distortion model of the camera. 46 | * */ 47 | DistortionModel type_; 48 | 49 | //@{ 50 | /** \brief Distortion Parameter. */ 51 | double k1_,k2_,k3_,k4_,k5_,k6_; 52 | double p1_,p2_,s1_,s2_,s3_,s4_; 53 | //@} 54 | 55 | /** \brief Constructor. 56 | * 57 | * Initializes the camera object as pinhole camera, i.e. all distortion coefficients are set to zero. 58 | * The intrinsic parameter matrix is set equal to the identity matrix. 59 | * */ 60 | Camera(); 61 | 62 | /** \brief Destructor. 63 | * */ 64 | virtual ~Camera(); 65 | 66 | /** \brief Loads and sets the distortion model and the corresponding distortion coefficients from yaml-file. 67 | * 68 | * @param filename - Path to the yaml-file, containing the distortion model and distortion coefficient data. 69 | */ 70 | bool loadCalibrationFromFile(const std::string& filename); 71 | 72 | /** \brief Initialize camera model from CameraCalibration struct. 73 | * 74 | * @param calibration - Camera calibration struct. 75 | */ 76 | bool setCalibration(const CameraCalibration& calibration); 77 | 78 | /** \brief Distorts a point on the unit plane (in camera coordinates) according to the Radtan distortion model. 79 | * 80 | * @param in - Undistorted point coordinates on the unit plane (in camera coordinates). 81 | * @param out - Distorted point coordinates on the unit plane (in camera coordinates). 82 | */ 83 | void distortRadtan(const Eigen::Vector2d& in, Eigen::Vector2d& out) const; 84 | 85 | /** \brief Distorts a point on the unit plane (in camera coordinates) according to the Radtan distortion model and 86 | * outputs additionally the corresponding jacobian matrix (input to output). 87 | * 88 | * @param in - Undistorted point coordinates on the unit plane (in camera coordinates). 89 | * @param out - Distorted point coordinates on the unit plane (in camera coordinates). 90 | * @param J - Jacobian matrix of the distortion process (input to output). 91 | */ 92 | void distortRadtan(const Eigen::Vector2d& in, Eigen::Vector2d& out, Eigen::Matrix2d& J) const; 93 | 94 | /** \brief Distorts a point on the unit plane (in camera coordinates) according to the Equidistant distortion model. 95 | * 96 | * @param in - Undistorted point coordinates on the unit plane (in camera coordinates). 97 | * @param out - Distorted point coordinates on the unit plane (in camera coordinates). 98 | */ 99 | void distortEquidist(const Eigen::Vector2d& in, Eigen::Vector2d& out) const; 100 | 101 | /** \brief Distorts a point on the unit plane (in camera coordinates) according to the Equidistant distortion model 102 | * and outputs additionally the corresponding jacobian matrix (input to output). 103 | * 104 | * @param in - Undistorted point coordinates on the unit plane (in camera coordinates). 105 | * @param out - Distorted point coordinates on the unit plane (in camera coordinates). 106 | * @param J - Jacobian matrix of the distortion process (input to output). 107 | */ 108 | void distortEquidist(const Eigen::Vector2d& in, Eigen::Vector2d& out, Eigen::Matrix2d& J) const; 109 | 110 | /** \brief Distorts a point on the unit plane (in camera coordinates) according to the Fov distortion model. 111 | * 112 | * @param in - Undistorted point coordinates on the unit plane (in camera coordinates). 113 | * @param out - Distorted point coordinates on the unit plane (in camera coordinates). 114 | */ 115 | void distortFov(const Eigen::Vector2d& in, Eigen::Vector2d& out) const; 116 | 117 | /** \brief Distorts a point on the unit plane (in camera coordinates) according to the Fov distortion model 118 | * and outputs additionally the corresponding jacobian matrix (input to output). 119 | * 120 | * @param in - Undistorted point coordinates on the unit plane (in camera coordinates). 121 | * @param out - Distorted point coordinates on the unit plane (in camera coordinates). 122 | * @param J - Jacobian matrix of the distortion process (input to output). 123 | */ 124 | void distortFov(const Eigen::Vector2d& in, Eigen::Vector2d& out, Eigen::Matrix2d& J) const; 125 | 126 | /** \brief Distorts a point on the unit plane, according to the set distortion model (#ModelType) and to the set 127 | * distortion coefficients. 128 | * 129 | * @param in - Undistorted point coordinates on the unit plane (in camera coordinates). 130 | * @param out - Distorted point coordinates on the unit plane (in camera coordinates). 131 | */ 132 | void distort(const Eigen::Vector2d& in, Eigen::Vector2d& out) const; 133 | 134 | /** \brief Distorts a point on the unit plane, according to the set distortion model (#ModelType) and to the set 135 | * distortion coefficients. Outputs additionally the corresponding jacobian matrix (input to output). 136 | * 137 | * @param in - Undistorted point coordinates on the unit plane (in camera coordinates). 138 | * @param out - Distorted point coordinates on the unit plane (in camera coordinates). 139 | * @param J - Jacobian matrix of the distortion process (input to output). 140 | */ 141 | void distort(const Eigen::Vector2d& in, Eigen::Vector2d& out, Eigen::Matrix2d& J) const; 142 | 143 | /** \brief Outputs the (distorted) pixel coordinates corresponding to a given bearing vector, 144 | * using the set distortion model. 145 | * 146 | * @param vec - Bearing vector (in camera coordinates | unit length not necessary). 147 | * @param c - (Distorted) pixel coordinates. 148 | * @return True, if process successful. 149 | */ 150 | bool bearingToPixel(const Eigen::Vector3d& vec, cv::Point2f& c) const; 151 | 152 | /** \brief Outputs the (distorted) pixel coordinates corresponding to a given bearing vector, 153 | * using the set distortion model. Outputs additionally the corresponding jacobian matrix (input to output). 154 | * 155 | * @param vec - Bearing vector (in camera coordinates | unit length not necessary). 156 | * @param c - (Distorted) pixel coordinates. 157 | * @param J - Jacobian matrix of the distortion process (input to output). 158 | * @return True, if process successful. 159 | */ 160 | bool bearingToPixel(const Eigen::Vector3d& vec, cv::Point2f& c, Eigen::Matrix& J) const; 161 | 162 | /** \brief Outputs the (distorted) pixel coordinates corresponding to a given NormalVectorElement-Object 163 | * (bearing vector), using the set distortion model. 164 | * 165 | * @param n - NormalVectorElement-Object. 166 | * @param c - (Distorted pixel) coordinates. 167 | * @return True, if process successful. 168 | */ 169 | bool bearingToPixel(const LWF::NormalVectorElement& n, cv::Point2f& c) const; 170 | 171 | /** \brief Outputs the (distorted) pixel coordinates corresponding to a given NormalVectorElement-Object 172 | * (bearing vector), using the set distortion model. 173 | * 174 | * @param n - NormalVectorElement-Object. 175 | * @param c - (Distorted) pixel coordinates. 176 | * @param J - Jacobian matrix of the distortion process (input to output). 177 | * @return True, if process successful. 178 | */ 179 | bool bearingToPixel(const LWF::NormalVectorElement& n, cv::Point2f& c, Eigen::Matrix& J) const; 180 | 181 | /** \brief Get the bearing vector, corresponding to a specific (distorted) pixel. 182 | * 183 | * @param c - (Distorted) pixel. 184 | * @param vec - Bearing vector (unit length). 185 | * @return True, if process successful. 186 | */ 187 | bool pixelToBearing(const cv::Point2f& c,Eigen::Vector3d& vec) const; 188 | 189 | /** \brief Get the NormalVectorElement-Object (bearing vector) corresponding to a specific (distorted) pixel. 190 | * 191 | * @param c - (Distorted) pixel. 192 | * @param vec - NormalVectorElement-Object. 193 | * @return True, if process successful. 194 | */ 195 | bool pixelToBearing(const cv::Point2f& c,LWF::NormalVectorElement& n) const; 196 | 197 | /** \brief Function testing the camera model by randomly mapping bearing vectors to pixel coordinates and vice versa. 198 | */ 199 | void testCameraModel(); 200 | }; 201 | 202 | } 203 | 204 | 205 | #endif /* ROVIO_CAMERA_HPP_ */ 206 | -------------------------------------------------------------------------------- /include/rovio/CameraCalibration.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Autonomous Systems Lab 3 | * All rights reserved. 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 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from 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 ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #ifndef ROVIO_CAMERA_CALIBRATION_HPP_ 30 | #define ROVIO_CAMERA_CALIBRATION_HPP_ 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include "rovio/FilterConfiguration.hpp" 37 | #include "rovio/Memory.hpp" 38 | 39 | namespace rovio { 40 | 41 | /** \brief Distortion model of the camera. 42 | * */ 43 | enum class DistortionModel { 44 | RADTAN, //!< Radial tangential distortion model. 45 | EQUIDIST, //!< Equidistant distortion model. 46 | FOV, //!< Field of view model. 47 | NUM //!< NOT A DISTORTION MODEL! 48 | }; 49 | 50 | static constexpr size_t NUM_DISTORTION_MODELS = 51 | static_cast(DistortionModel::NUM); 52 | 53 | const std::array NUM_DISTORTION_MODEL_PARAMS = { 54 | {/*RADTAN (k1, k2, p1, p2, k3)*/ 5u, 55 | /*EQUIDIST (k1, k2, k3, k4)*/ 4u, 56 | /*FOV (w)*/ 1u}}; 57 | 58 | const std::array DISTORTION_MODEL_NAME = { 59 | {/*RADTAN*/ "plumb_bob", 60 | /*EQUIDIST*/ "equidistant" 61 | /*FOV*/ "fov"}}; 62 | 63 | // YAML keywords. 64 | static const std::string CAMERA_MATRIX = "camera_matrix"; 65 | static const std::string DIST_COEFFS = "distortion_coefficients"; 66 | static const std::string DATA = "data"; 67 | 68 | struct CameraCalibration { 69 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 70 | 71 | CameraCalibration() {} 72 | 73 | CameraCalibration(const std::string &calibration_yaml_file) { 74 | loadFromFile(calibration_yaml_file); 75 | } 76 | 77 | /** \brief Does this struct contain intrinsics and distortion parameter? 78 | * */ 79 | bool hasIntrinsics_ = false; 80 | 81 | /** \brief Camera intrinsics. 82 | * */ 83 | Eigen::Matrix3d K_; 84 | 85 | /** \brief Camera distortion model. 86 | * */ 87 | DistortionModel distortionModel_; 88 | 89 | /** \brief Variable size distortion param vector. 90 | * */ 91 | Eigen::VectorXd distortionParams_; 92 | 93 | /** \brief Optional camera extrinsics. This information might also be 94 | * contained in the rovio filter settings, however if they are set here, it 95 | * will overwrite the existing settings. 96 | * */ 97 | bool hasExtrinsics_ = false; 98 | Eigen::Vector3d MrMC_; 99 | kindr::RotationQuaternionPD qCM_; 100 | 101 | /** \brief Returns the size of the distortion_parameter vector. 102 | * */ 103 | size_t getNumDistortionParam() { 104 | return NUM_DISTORTION_MODEL_PARAMS[static_cast(distortionModel_)]; 105 | } 106 | 107 | /** \brief Sets the camera extrinsics. 108 | */ 109 | void setCameraExtrinsics(const Eigen::Vector3d &MrMC, 110 | const kindr::RotationQuaternionPD &qCM) { 111 | hasExtrinsics_ = true; 112 | MrMC_ = MrMC; 113 | qCM_ = qCM; 114 | } 115 | 116 | /** \brief Sets the camera extrinsics based on the filter settings. 117 | */ 118 | void getCameraExtrinsicsFromFilterConfiguration( 119 | const int camID, const FilterConfiguration &filter_configuration) { 120 | CHECK(filter_configuration.getqCM_xFromCamera(camID, &(qCM_.x()))); 121 | CHECK(filter_configuration.getqCM_yFromCamera(camID, &(qCM_.y()))); 122 | CHECK(filter_configuration.getqCM_zFromCamera(camID, &(qCM_.z()))); 123 | CHECK(filter_configuration.getqCM_wFromCamera(camID, &(qCM_.w()))); 124 | 125 | CHECK(filter_configuration.getMrMC_xFromCamera(camID, &(MrMC_.x()))); 126 | CHECK(filter_configuration.getMrMC_yFromCamera(camID, &(MrMC_.y()))); 127 | CHECK(filter_configuration.getMrMC_zFromCamera(camID, &(MrMC_.z()))); 128 | 129 | hasExtrinsics_ = true; 130 | } 131 | 132 | /** \brief Loads and sets intrinsics, the distortion model and the 133 | * corresponding distortion coefficients from yaml-file. 134 | * 135 | * @param filename - Path to the yaml-file, containing the distortion model 136 | * and distortion coefficient data. 137 | */ 138 | void loadFromFile(const std::string &calibration_yaml_file); 139 | 140 | /** \brief Loads and sets the distortion parameters {k1_, k2_, k3_, p1_, p2_} 141 | * for the Radtan distortion model from yaml-file. 142 | * @param filename - Path to the yaml-file, containing the distortion 143 | * coefficients. 144 | */ 145 | void loadRadTanDistortion(const std::string &calibration_yaml_file); 146 | 147 | /** \brief Loads and sets the distortion parameters {k1_, k2_, k3_, k4_} for 148 | * the Equidistant distortion model from yaml-file. 149 | * @param filename - Path to the yaml-file, containing the distortion 150 | * coefficients. 151 | */ 152 | void loadEquidistDistortion(const std::string &calibration_yaml_file); 153 | 154 | /** \brief Loads and sets the distortion parameter {w} for 155 | * the Fov distortion model from yaml-file. 156 | * @param filename - Path to the yaml-file, containing the distortion 157 | * coefficients. 158 | */ 159 | void loadFovDistortion(const std::string &calibration_yaml_file); 160 | 161 | /** \brief Loads and sets the intrinsic parameter matrix K_ from yaml-file. 162 | * 163 | * @param filename - Path to the yaml-file, containing the intrinsic 164 | * parameter matrix coefficients. 165 | */ 166 | void loadCameraMatrix(const std::string &calibration_yaml_file); 167 | }; 168 | 169 | typedef Aligned CameraCalibrationVector; 170 | 171 | } // namespace rovio 172 | 173 | #endif // ROVIO_CAMERA_CALIBRATION_HPP_ 174 | -------------------------------------------------------------------------------- /include/rovio/CoordinateTransform/FeatureOutput.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Autonomous Systems Lab 3 | * All rights reserved. 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 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #ifndef ROVIO_FEATUREOUTPUT_HPP_ 30 | #define ROVIO_FEATUREOUTPUT_HPP_ 31 | 32 | #include "lightweight_filtering/common.hpp" 33 | #include "lightweight_filtering/CoordinateTransform.hpp" 34 | #include "rovio/RobocentricFeatureElement.hpp" 35 | #include "rovio/MultiCamera.hpp" 36 | 37 | namespace rovio { 38 | 39 | class FeatureOutput: public LWF::State{ 40 | public: 41 | static constexpr unsigned int _fea = 0; 42 | FeatureOutput(){ 43 | static_assert(_fea+1==E_,"Error with indices"); 44 | this->template getName<_fea>() = "fea"; 45 | } 46 | virtual ~FeatureOutput(){}; 47 | 48 | //@{ 49 | /** \brief Get/Set the distance parameter 50 | * 51 | * @return a reference to distance parameter of the feature. 52 | */ 53 | inline FeatureDistance& d(){ 54 | return this->template get<_fea>().distance_; 55 | } 56 | inline const FeatureDistance& d() const{ 57 | return this->template get<_fea>().distance_; 58 | } 59 | //@} 60 | 61 | //@{ 62 | /** \brief Get/Set the feature coordinates 63 | * 64 | * @return a reference to the feature coordinates 65 | */ 66 | inline FeatureCoordinates& c() { 67 | return this->template get<_fea>().coordinates_; 68 | } 69 | inline const FeatureCoordinates& c() const{ 70 | return this->template get<_fea>().coordinates_; 71 | } 72 | //@} 73 | }; 74 | 75 | template 76 | class FeatureOutputCT:public LWF::CoordinateTransform{ 77 | public: 78 | typedef LWF::CoordinateTransform Base; 79 | typedef typename Base::mtInput mtInput; 80 | typedef typename Base::mtOutput mtOutput; 81 | int ID_; 82 | FeatureOutputCT(){ 83 | ID_ = -1; 84 | }; 85 | virtual ~FeatureOutputCT(){}; 86 | void setFeatureID(int ID){ 87 | ID_ = ID; 88 | } 89 | void evalTransform(mtOutput& output, const mtInput& input) const{ 90 | output.template get() = input.template get(ID_); 91 | } 92 | void jacTransform(MXD& J, const mtInput& input) const{ 93 | J.setIdentity(); 94 | } 95 | }; 96 | 97 | template 98 | class TransformFeatureOutputCT:public LWF::CoordinateTransform{ 99 | public: 100 | typedef LWF::CoordinateTransform Base; 101 | typedef typename Base::mtInput mtInput; 102 | typedef typename Base::mtOutput mtOutput; 103 | int outputCamID_; 104 | int ID_; 105 | bool ignoreDistanceOutput_; 106 | MultiCamera* mpMultiCamera_; 107 | TransformFeatureOutputCT(MultiCamera* mpMultiCamera){ 108 | mpMultiCamera_ = mpMultiCamera; 109 | outputCamID_ = 0; 110 | ID_ = -1; 111 | ignoreDistanceOutput_ = false; 112 | }; 113 | virtual ~TransformFeatureOutputCT(){}; 114 | void setFeatureID(int ID){ 115 | ID_ = ID; 116 | } 117 | void setOutputCameraID(int camID){ 118 | CHECK_GE(camID, 0); 119 | outputCamID_ = camID; 120 | } 121 | void evalTransform(mtOutput& output, const mtInput& input) const{ 122 | CHECK_NOTNULL(mpMultiCamera_); 123 | 124 | CHECK_NOTNULL(input.CfP(ID_).mpCamera_); 125 | CHECK_GE(input.CfP(ID_).camID_, 0); 126 | 127 | input.updateMultiCameraExtrinsics(mpMultiCamera_); 128 | mpMultiCamera_->transformFeature(outputCamID_,input.CfP(ID_),input.dep(ID_),output.c(),output.d()); 129 | if(input.CfP(ID_).trackWarping_ && input.CfP(ID_).com_warp_nor()){ 130 | const int camID = input.CfP(ID_).camID_; 131 | 132 | const QPD qDC = input.qCM(outputCamID_)*input.qCM(camID).inverted(); // TODO: avoid double computation 133 | const V3D CrCD = input.qCM(camID).rotate(V3D(input.MrMC(outputCamID_)-input.MrMC(camID))); 134 | const V3D CrCP = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getVec(); 135 | const V3D DrDP = qDC.rotate(V3D(CrCP-CrCD)); 136 | const double d_out = DrDP.norm(); 137 | const LWF::NormalVectorElement nor_out(DrDP); 138 | const Eigen::Matrix J_nor_DrDP = nor_out.getM().transpose()/d_out; 139 | const Eigen::Matrix J_DrDP_CrCP = MPD(qDC).matrix(); 140 | const Eigen::Matrix J_CrCP_nor = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getM(); 141 | output.c().set_warp_nor(J_nor_DrDP*J_DrDP_CrCP*J_CrCP_nor*input.CfP(ID_).get_warp_nor()); 142 | } 143 | } 144 | void jacTransform(MXD& J, const mtInput& input) const{ 145 | J.setZero(); 146 | const int camID = input.CfP(ID_).camID_; 147 | CHECK_GE(camID, 0); 148 | 149 | if(camID != outputCamID_){ 150 | CHECK_NOTNULL(mpMultiCamera_); 151 | input.updateMultiCameraExtrinsics(mpMultiCamera_); 152 | const QPD qDC = input.qCM(outputCamID_)*input.qCM(camID).inverted(); // TODO: avoid double computation 153 | const V3D CrCD = input.qCM(camID).rotate(V3D(input.MrMC(outputCamID_)-input.MrMC(camID))); 154 | const V3D CrCP = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getVec(); 155 | const V3D DrDP = qDC.rotate(V3D(CrCP-CrCD)); 156 | const double d_out = DrDP.norm(); 157 | const LWF::NormalVectorElement nor_out(DrDP); // TODO: test if Jacobian works, this new setting of vector is dangerous 158 | 159 | const Eigen::Matrix J_d_DrDP = nor_out.getVec().transpose(); 160 | const Eigen::Matrix J_nor_DrDP = nor_out.getM().transpose()/d_out; 161 | const Eigen::Matrix J_DrDP_qDC = gSM(DrDP); 162 | const Eigen::Matrix J_DrDP_CrCP = MPD(qDC).matrix(); 163 | const Eigen::Matrix J_DrDP_CrCD = -MPD(qDC).matrix(); 164 | 165 | const Eigen::Matrix J_qDC_qDB = Eigen::Matrix3d::Identity(); 166 | const Eigen::Matrix J_qDC_qCB = -MPD(qDC).matrix(); 167 | const Eigen::Matrix J_CrCD_qCB = gSM(CrCD); 168 | const Eigen::Matrix J_CrCD_BrBC = -MPD(input.qCM(camID)).matrix(); 169 | const Eigen::Matrix J_CrCD_BrBD = MPD(input.qCM(camID)).matrix(); 170 | 171 | const Eigen::Matrix J_CrCP_d = input.CfP(ID_).get_nor().getVec()*input.dep(ID_).getDistanceDerivative(); 172 | const Eigen::Matrix J_CrCP_nor = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getM(); 173 | 174 | J.template block<2,2>(mtOutput::template getId(),mtInput::template getId(ID_)) = J_nor_DrDP*J_DrDP_CrCP*J_CrCP_nor; 175 | J.template block<2,1>(mtOutput::template getId(),mtInput::template getId(ID_)+2) = J_nor_DrDP*J_DrDP_CrCP*J_CrCP_d; 176 | if(!ignoreDistanceOutput_){ 177 | J.template block<1,2>(mtOutput::template getId()+2,mtInput::template getId(ID_)) = J_d_DrDP*J_DrDP_CrCP*J_CrCP_nor; 178 | J.template block<1,1>(mtOutput::template getId()+2,mtInput::template getId(ID_)+2) = J_d_DrDP*J_DrDP_CrCP*J_CrCP_d; 179 | } 180 | 181 | if(input.aux().doVECalibration_ && camID != outputCamID_){ 182 | J.template block<2,3>(mtOutput::template getId(),mtInput::template getId(camID)) = J_nor_DrDP*(J_DrDP_qDC*J_qDC_qCB+J_DrDP_CrCD*J_CrCD_qCB); 183 | J.template block<2,3>(mtOutput::template getId(),mtInput::template getId(outputCamID_)) = J_nor_DrDP*J_DrDP_qDC*J_qDC_qDB; 184 | J.template block<2,3>(mtOutput::template getId(),mtInput::template getId(camID)) = J_nor_DrDP*J_DrDP_CrCD*J_CrCD_BrBC; 185 | J.template block<2,3>(mtOutput::template getId(),mtInput::template getId(outputCamID_)) = J_nor_DrDP*J_DrDP_CrCD*J_CrCD_BrBD; 186 | if(!ignoreDistanceOutput_){ 187 | J.template block<1,3>(mtOutput::template getId()+2,mtInput::template getId(camID)) = J_d_DrDP*(J_DrDP_qDC*J_qDC_qCB+J_DrDP_CrCD*J_CrCD_qCB); 188 | J.template block<1,3>(mtOutput::template getId()+2,mtInput::template getId(outputCamID_)) = J_d_DrDP*J_DrDP_qDC*J_qDC_qDB; 189 | J.template block<1,3>(mtOutput::template getId()+2,mtInput::template getId(camID)) = J_d_DrDP*J_DrDP_CrCD*J_CrCD_BrBC; 190 | J.template block<1,3>(mtOutput::template getId()+2,mtInput::template getId(outputCamID_)) = J_d_DrDP*J_DrDP_CrCD*J_CrCD_BrBD; 191 | } 192 | } 193 | } else { 194 | J.template block<2,2>(mtOutput::template getId(),mtInput::template getId(ID_)) = Eigen::Matrix2d::Identity(); 195 | J.template block<1,1>(mtOutput::template getId()+2,mtInput::template getId(ID_)+2) = Eigen::Matrix::Identity(); 196 | } 197 | } 198 | }; 199 | 200 | } 201 | 202 | 203 | #endif /* ROVIO_FEATUREOUTPUT_HPP_ */ 204 | -------------------------------------------------------------------------------- /include/rovio/CoordinateTransform/FeatureOutputReadable.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Autonomous Systems Lab 3 | * All rights reserved. 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 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #ifndef ROVIO_FEATUREOUTPUTREADABLE_HPP_ 30 | #define ROVIO_FEATUREOUTPUTREADABLE_HPP_ 31 | 32 | #include "lightweight_filtering/common.hpp" 33 | #include "lightweight_filtering/CoordinateTransform.hpp" 34 | #include "rovio/CoordinateTransform/FeatureOutput.hpp" 35 | 36 | namespace rovio { 37 | 38 | class FeatureOutputReadable: public LWF::State,LWF::ScalarElement>{ 39 | public: 40 | static constexpr unsigned int _bea = 0; 41 | static constexpr unsigned int _dis = _bea+1; 42 | FeatureOutputReadable(){ 43 | static_assert(_dis+1==E_,"Error with indices"); 44 | this->template getName<_bea>() = "bea"; 45 | this->template getName<_dis>() = "dis"; 46 | } 47 | virtual ~FeatureOutputReadable(){}; 48 | 49 | //@{ 50 | /** \brief Get/Set the distance parameter 51 | * 52 | * @return a reference to distance parameter of the feature. 53 | */ 54 | inline Eigen::Vector3d& bea(){ 55 | return this->template get<_bea>(); 56 | } 57 | inline const Eigen::Vector3d& bea() const{ 58 | return this->template get<_bea>(); 59 | } 60 | //@} 61 | 62 | //@{ 63 | /** \brief Get/Set the feature coordinates 64 | * 65 | * @return a reference to the feature coordinates 66 | */ 67 | inline double& dis(){ 68 | return this->template get<_dis>(); 69 | } 70 | inline const double& dis() const{ 71 | return this->template get<_dis>(); 72 | } 73 | //@} 74 | }; 75 | 76 | class FeatureOutputReadableCT:public LWF::CoordinateTransform{ 77 | public: 78 | typedef LWF::CoordinateTransform Base; 79 | typedef typename Base::mtInput mtInput; 80 | typedef typename Base::mtOutput mtOutput; 81 | FeatureOutputReadableCT(){}; 82 | virtual ~FeatureOutputReadableCT(){}; 83 | void evalTransform(mtOutput& output, const mtInput& input) const{ 84 | output.bea() = input.c().get_nor().getVec(); 85 | output.dis() = input.d().getDistance(); 86 | } 87 | void jacTransform(MXD& J, const mtInput& input) const{ 88 | J.setZero(); 89 | J.template block<3,2>(mtOutput::template getId(),mtInput::template getId()) = input.c().get_nor().getM(); 90 | J(mtOutput::template getId(),mtInput::template getId()+2) = input.d().getDistanceDerivative(); 91 | } 92 | }; 93 | 94 | } 95 | 96 | 97 | #endif /* ROVIO_FEATUREOUTPUTREADABLE_HPP_ */ 98 | -------------------------------------------------------------------------------- /include/rovio/CoordinateTransform/LandmarkOutput.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Autonomous Systems Lab 3 | * All rights reserved. 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 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #ifndef ROVIO_LANDMARKOUTPUTIMU_HPP_ 30 | #define ROVIO_LANDMARKOUTPUTIMU_HPP_ 31 | 32 | #include "lightweight_filtering/common.hpp" 33 | #include "lightweight_filtering/CoordinateTransform.hpp" 34 | #include "rovio/MultiCamera.hpp" 35 | 36 | namespace rovio { 37 | 38 | class LandmarkOutput: public LWF::State>{ 39 | public: 40 | static constexpr unsigned int _lmk = 0; 41 | LandmarkOutput(){ 42 | static_assert(_lmk+1==E_,"Error with indices"); 43 | this->template getName<_lmk>() = "lmk"; 44 | } 45 | virtual ~LandmarkOutput(){}; 46 | 47 | //@{ 48 | /** \brief Get/Set the feature as 3D vector 49 | * 50 | * @return a reference to the landmark 51 | */ 52 | inline V3D& lmk(){ 53 | return this->template get<_lmk>(); 54 | } 55 | inline const V3D& lmk() const{ 56 | return this->template get<_lmk>(); 57 | } 58 | //@} 59 | }; 60 | 61 | template 62 | class LandmarkOutputImuCT:public LWF::CoordinateTransform{ 63 | public: 64 | typedef LWF::CoordinateTransform Base; 65 | typedef typename Base::mtInput mtInput; 66 | typedef typename Base::mtOutput mtOutput; 67 | int ID_; 68 | MultiCamera* mpMultiCamera_; 69 | LandmarkOutputImuCT(MultiCamera* mpMultiCamera){ 70 | mpMultiCamera_ = mpMultiCamera; 71 | ID_ = -1; 72 | }; 73 | virtual ~LandmarkOutputImuCT(){}; 74 | void setFeatureID(int ID){ 75 | ID_ = ID; 76 | } 77 | void evalTransform(mtOutput& output, const mtInput& input) const{ 78 | input.updateMultiCameraExtrinsics(mpMultiCamera_); 79 | // BrBP = BrBC + qCB^T(d_in*nor_in) 80 | const V3D CrCP = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getVec(); 81 | output.template get() = mpMultiCamera_->BrBC_[input.CfP(ID_).camID_] + mpMultiCamera_->qCB_[input.CfP(ID_).camID_].inverseRotate(CrCP); 82 | } 83 | void jacTransform(MXD& J, const mtInput& input) const{ 84 | J.setZero(); 85 | input.updateMultiCameraExtrinsics(mpMultiCamera_); 86 | const V3D CrCP = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getVec(); 87 | const Eigen::Matrix J_CrCP_nor = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getM(); 88 | const Eigen::Matrix J_CrCP_d = input.CfP(ID_).get_nor().getVec()*input.dep(ID_).getDistanceDerivative(); 89 | const M3D mBC = MPD(mpMultiCamera_->qCB_[input.CfP(ID_).camID_].inverted()).matrix(); 90 | 91 | J.template block<3,2>(mtOutput::template getId(),mtInput::template getId(ID_)) = mBC*J_CrCP_nor; 92 | J.template block<3,1>(mtOutput::template getId(),mtInput::template getId(ID_)+2) = mBC*J_CrCP_d; 93 | 94 | if(input.aux().doVECalibration_){ 95 | J.template block<3,3>(mtOutput::template getId(),mtInput::template getId()) = M3D::Identity(); 96 | J.template block<3,3>(mtOutput::template getId(),mtInput::template getId()) = mBC*gSM(CrCP); 97 | } 98 | } 99 | }; 100 | 101 | } 102 | 103 | 104 | #endif /* ROVIO_LANDMARKOUTPUTIMU_HPP_ */ 105 | -------------------------------------------------------------------------------- /include/rovio/CoordinateTransform/PixelOutput.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Autonomous Systems Lab 3 | * All rights reserved. 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 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #ifndef ROVIO_PIXELOUTPUT_HPP_ 30 | #define ROVIO_PIXELOUTPUT_HPP_ 31 | 32 | #include "lightweight_filtering/common.hpp" 33 | #include "lightweight_filtering/CoordinateTransform.hpp" 34 | #include "rovio/CoordinateTransform/FeatureOutput.hpp" 35 | 36 | namespace rovio { 37 | 38 | class PixelOutput: public LWF::State>{ 39 | public: 40 | static constexpr unsigned int _pix = 0; 41 | PixelOutput(){ 42 | } 43 | virtual ~PixelOutput(){}; 44 | cv::Point2f getPoint2f() const{ 45 | return cv::Point2f(static_cast(this->get<_pix>()(0)),static_cast(this->get<_pix>()(1))); 46 | } 47 | }; 48 | 49 | class PixelOutputCT:public LWF::CoordinateTransform{ 50 | public: 51 | typedef LWF::CoordinateTransform Base; 52 | typedef typename Base::mtInput mtInput; 53 | typedef typename Base::mtOutput mtOutput; 54 | PixelOutputCT(){ 55 | }; 56 | virtual ~PixelOutputCT(){}; 57 | void evalTransform(mtOutput& output, const mtInput& input) const{ 58 | cv::Point2f c = input.c().get_c(); 59 | output.template get() = Eigen::Vector2d(c.x,c.y); 60 | } 61 | void jacTransform(MXD& J, const mtInput& input) const{ 62 | J.setZero(); 63 | J.template block<2,2>(mtOutput::template getId(),mtInput::template getId()) = input.c().get_J(); 64 | } 65 | }; 66 | 67 | } 68 | 69 | 70 | #endif /* ROVIO_PIXELOUTPUT_HPP_ */ 71 | -------------------------------------------------------------------------------- /include/rovio/CoordinateTransform/RovioOutput.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Autonomous Systems Lab 3 | * All rights reserved. 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 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #ifndef ROVIOOUTPUT_HPP_ 30 | #define ROVIOOUTPUT_HPP_ 31 | 32 | #include "lightweight_filtering/common.hpp" 33 | #include "lightweight_filtering/CoordinateTransform.hpp" 34 | 35 | namespace rovio { 36 | 37 | class StandardOutput: public LWF::State,3>,LWF::QuaternionElement>{ 38 | public: 39 | static constexpr unsigned int _pos = 0; /**%Body, expressed in World). 49 | * 50 | * @return the position vector WrWB (World->Body, expressed in World). 51 | */ 52 | inline V3D& WrWB(){ 53 | return this->template get<_pos>(); 54 | } 55 | inline const V3D& WrWB() const{ 56 | return this->template get<_pos>(); 57 | } 58 | //@} 59 | 60 | //@{ 61 | /** \brief Get the absolute velocity vector of the camera, expressed in camera coordinates. 62 | * 63 | * @return the absolute velocity vector of the Body-Frame BvB, expressed in Body-Coordinates. 64 | */ 65 | inline V3D& BvB(){ 66 | return this->template get<_vel>(); 67 | } 68 | inline const V3D& BvB() const{ 69 | return this->template get<_vel>(); 70 | } 71 | //@} 72 | 73 | //@{ 74 | /** \brief Get the absolute angular velocity of the camera (angular velocity of the solid), expressed in camera coordinates. 75 | * 76 | * @return the absolute angular velocity of the Body-Frame BwWB, expressed in Body-Coordinates. 77 | */ 78 | inline V3D& BwWB(){ 79 | return this->template get<_ror>(); 80 | } 81 | inline const V3D& BwWB() const{ 82 | return this->template get<_ror>(); 83 | } 84 | //@} 85 | 86 | //@{ 87 | /** \brief Get the quaternion qBW, expressing the World-Frame in Body-Coordinates (World Coordinates->Body Coordinates). 88 | * 89 | * @return the quaternion qBW (World Coordinates->Body Coordinates). 90 | */ 91 | inline QPD& qBW(){ 92 | return this->template get<_att>(); 93 | } 94 | inline const QPD& qBW() const{ 95 | return this->template get<_att>(); 96 | } 97 | //@} 98 | }; 99 | 100 | template 101 | class CameraOutputCT:public LWF::CoordinateTransform{ 102 | public: 103 | typedef LWF::CoordinateTransform Base; 104 | typedef typename Base::mtInput mtInput; 105 | typedef typename Base::mtOutput mtOutput; 106 | int camID_; 107 | CameraOutputCT(){ 108 | camID_ = 0; 109 | }; 110 | virtual ~CameraOutputCT(){}; 111 | void evalTransform(mtOutput& output, const mtInput& input) const{ 112 | // WrWC = WrWM + qWM*(MrMC) 113 | // CwWC = qCM*MwWM 114 | // CvC = qCM*(MvM + MwWM x MrMC) 115 | // qCW = qCM*qWM^T 116 | output.WrWB() = input.WrWM()+input.qWM().rotate(input.MrMC(camID_)); 117 | output.BwWB() = input.qCM(camID_).rotate(V3D(input.aux().MwWMmeas_-input.gyb())); 118 | output.BvB() = input.qCM(camID_).rotate(V3D(-input.MvM() + gSM(V3D(input.aux().MwWMmeas_-input.gyb()))*input.MrMC(camID_))); 119 | output.qBW() = input.qCM(camID_)*input.qWM().inverted(); 120 | } 121 | void jacTransform(MXD& J, const mtInput& input) const{ 122 | J.setZero(); 123 | J.template block<3,3>(mtOutput::template getId(),mtInput::template getId()) = M3D::Identity(); 124 | J.template block<3,3>(mtOutput::template getId(),mtInput::template getId()) = 125 | -gSM(input.qWM().rotate(input.MrMC(camID_))); 126 | J.template block<3,3>(mtOutput::template getId(),mtInput::template getId()) = 127 | -MPD(input.qCM(camID_)*input.qWM().inverted()).matrix(); 128 | J.template block<3,3>(mtOutput::template getId(),mtInput::template getId()) = -MPD(input.qCM(camID_)).matrix(); 129 | J.template block<3,3>(mtOutput::template getId(),mtInput::template getId()) = MPD(input.qCM(camID_)).matrix() 130 | * gSM(input.MrMC(camID_)); 131 | J.template block<3,3>(mtOutput::template getId(),mtInput::template getId()) = -MPD(input.qCM(camID_)).matrix(); 132 | if(input.aux().doVECalibration_){ 133 | J.template block<3,3>(mtOutput::template getId(),mtInput::template getId(camID_)) = 134 | MPD(input.qWM()).matrix(); 135 | J.template block<3,3>(mtOutput::template getId(),mtInput::template getId(camID_)) = 136 | MPD(input.qCM(camID_)).matrix()*gSM(V3D(input.aux().MwWMmeas_-input.gyb())); 137 | J.template block<3,3>(mtOutput::template getId(),mtInput::template getId(camID_)) = 138 | -gSM(input.qCM(camID_).rotate(V3D(input.aux().MwWMmeas_-input.gyb()))); 139 | J.template block<3,3>(mtOutput::template getId(),mtInput::template getId(camID_)) = 140 | -gSM(input.qCM(camID_).rotate(V3D(-input.MvM() + gSM(V3D(input.aux().MwWMmeas_-input.gyb()))*input.MrMC()))); 141 | J.template block<3,3>(mtOutput::template getId(),mtInput::template getId(camID_)) = 142 | M3D::Identity(); 143 | } 144 | } 145 | void postProcess(MXD& cov,const mtInput& input){ 146 | cov.template block<3,3>(mtOutput::template getId(),mtOutput::template getId()) += input.aux().wMeasCov_; 147 | } 148 | }; 149 | 150 | template 151 | class ImuOutputCT:public LWF::CoordinateTransform{ 152 | public: 153 | typedef LWF::CoordinateTransform Base; 154 | typedef typename Base::mtInput mtInput; 155 | typedef typename Base::mtOutput mtOutput; 156 | ImuOutputCT(){}; 157 | virtual ~ImuOutputCT(){}; 158 | void evalTransform(mtOutput& output, const mtInput& input) const{ 159 | // WrWM = WrWM 160 | // MwWM = MwWM 161 | // MvM = MvM 162 | // qMW = qWM^T 163 | output.WrWB() = input.WrWM(); 164 | output.BwWB() = input.aux().MwWMmeas_-input.gyb(); 165 | output.BvB() = -input.MvM(); // minus is required! 166 | output.qBW() = input.qWM().inverted(); 167 | } 168 | void jacTransform(MXD& J, const mtInput& input) const{ 169 | J.setZero(); 170 | J.template block<3,3>(mtOutput::template getId(),mtInput::template getId()) = M3D::Identity(); 171 | J.template block<3,3>(mtOutput::template getId(),mtInput::template getId()) = -MPD(input.qWM().inverted()).matrix(); 172 | J.template block<3,3>(mtOutput::template getId(),mtInput::template getId()) = -M3D::Identity(); 173 | J.template block<3,3>(mtOutput::template getId(),mtInput::template getId()) = -M3D::Identity(); 174 | } 175 | void postProcess(MXD& cov,const mtInput& input){ 176 | cov.template block<3,3>(mtOutput::template getId(),mtOutput::template getId()) += input.aux().wMeasCov_; 177 | } 178 | }; 179 | 180 | } 181 | 182 | 183 | #endif /* ROVIOOUTPUT_HPP_ */ 184 | -------------------------------------------------------------------------------- /include/rovio/CoordinateTransform/YprOutput.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Autonomous Systems Lab 3 | * All rights reserved. 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 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #ifndef ROVIO_YPROUTPUT_HPP_ 30 | #define ROVIO_YPROUTPUT_HPP_ 31 | 32 | #include "lightweight_filtering/common.hpp" 33 | #include "lightweight_filtering/CoordinateTransform.hpp" 34 | 35 | namespace rovio { 36 | 37 | class AttitudeOutput: public LWF::State{ 38 | public: 39 | static constexpr unsigned int _att = 0; 40 | AttitudeOutput(){ 41 | } 42 | virtual ~AttitudeOutput(){}; 43 | }; 44 | 45 | class YprOutput: public LWF::State>{ 46 | public: 47 | static constexpr unsigned int _ypr = 0; 48 | YprOutput(){ 49 | } 50 | virtual ~YprOutput(){}; 51 | 52 | 53 | }; 54 | 55 | class AttitudeToYprCT:public LWF::CoordinateTransform{ 56 | public: 57 | typedef LWF::CoordinateTransform Base; 58 | typedef typename Base::mtInput mtInput; 59 | typedef typename Base::mtOutput mtOutput; 60 | AttitudeToYprCT(){}; 61 | virtual ~AttitudeToYprCT(){}; 62 | void evalTransform(mtOutput& output, const mtInput& input) const{ 63 | output.template get() = kindr::EulerAnglesZyxD(input.template get()).vector(); 64 | } 65 | void jacTransform(MXD& J, const mtInput& input) const{ 66 | kindr::EulerAnglesZyxD zyx(input.template get()); 67 | J = zyx.getMappingFromLocalAngularVelocityToDiff()*MPD(input.template get().inverted()).matrix(); 68 | } 69 | }; 70 | 71 | } 72 | 73 | 74 | #endif /* ROVIO_YPROUTPUT_HPP_ */ 75 | -------------------------------------------------------------------------------- /include/rovio/FeatureDistance.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Autonomous Systems Lab 3 | * All rights reserved. 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 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #ifndef FEATUREDISTANCE_HPP_ 30 | #define FEATUREDISTANCE_HPP_ 31 | 32 | namespace rovio { 33 | 34 | /** \brief Class allowing the computation of some distance. Different parametrizations are implemented. 35 | */ 36 | class FeatureDistance{ 37 | public: 38 | double p_; /** 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | namespace rovio { 39 | struct FilterConfiguration : public boost::property_tree::ptree { 40 | typedef boost::property_tree::ptree ptree; 41 | 42 | FilterConfiguration() : boost::property_tree::ptree() {} 43 | 44 | FilterConfiguration(const std::string &config_file) 45 | : boost::property_tree::ptree() { 46 | loadFromFile(config_file); 47 | } 48 | 49 | void loadFromFile(const std::string &config_file) { 50 | try { 51 | ptree *propertyTreePtr = this; 52 | read_info(config_file, *propertyTreePtr); 53 | } catch (boost::property_tree::ptree_error &e) { 54 | std::cout << "Unable to load the filter configuration from " 55 | << config_file << "! Exception: " << e.what() << std::endl; 56 | } 57 | } 58 | 59 | #define GETTER_AND_SETTER(name, settings_string, data_type) \ 60 | inline data_type get##name(const data_type &default_value) { \ 61 | return get(#settings_string, default_value); \ 62 | } \ 63 | inline bool get##name(data_type *value_ptr) const { \ 64 | CHECK_NOTNULL(value_ptr); \ 65 | try { \ 66 | *value_ptr = get(#settings_string); \ 67 | } catch (...) { \ 68 | return false; \ 69 | } \ 70 | return true; \ 71 | } \ 72 | inline void set##name(const data_type &value) { \ 73 | put(#settings_string, value); \ 74 | } 75 | 76 | #define CAMERA_GETTER_AND_SETTER(name, camera_variable_string, data_type) \ 77 | \ 78 | inline bool get##name##FromCamera(const int camID, data_type *value_ptr) \ 79 | const { \ 80 | CHECK_NOTNULL(value_ptr); \ 81 | \ 82 | const std::string value_key = \ 83 | "Camera" + std::to_string(camID) + "." + #camera_variable_string; \ 84 | try { \ 85 | *value_ptr = get(value_key); \ 86 | } catch (...) { \ 87 | LOG(ERROR) << "Unable to find camera variable at " << value_key; \ 88 | return false; \ 89 | } \ 90 | return true; \ 91 | } \ 92 | \ 93 | inline void set##name##ForCamera(const int camID, const data_type &value) { \ 94 | const std::string value_key = \ 95 | "Camera" + std::to_string(camID) + "." + #camera_variable_string; \ 96 | put(value_key, value); \ 97 | } 98 | 99 | GETTER_AND_SETTER(DoFrameVisualization, ImgUpdate.doFrameVisualisation, bool); 100 | GETTER_AND_SETTER(DoVisualization, PoseUpdate.doVisualization, bool); 101 | 102 | // Camera extrinsics. 103 | CAMERA_GETTER_AND_SETTER(qCM_x, qCM_x, double); 104 | CAMERA_GETTER_AND_SETTER(qCM_y, qCM_y, double); 105 | CAMERA_GETTER_AND_SETTER(qCM_z, qCM_z, double); 106 | CAMERA_GETTER_AND_SETTER(qCM_w, qCM_w, double); 107 | CAMERA_GETTER_AND_SETTER(MrMC_x, MrMC_x, double); 108 | CAMERA_GETTER_AND_SETTER(MrMC_y, MrMC_y, double); 109 | CAMERA_GETTER_AND_SETTER(MrMC_z, MrMC_z, double); 110 | 111 | // IMU prediction noise. 112 | // Covariance parameter for position prediction [m^2/s]. 113 | GETTER_AND_SETTER( 114 | PositionCovarianceX, Prediction.PredictionNoise.pos_0, double); 115 | GETTER_AND_SETTER( 116 | PositionCovarianceY, Prediction.PredictionNoise.pos_1, double); 117 | GETTER_AND_SETTER( 118 | PositionCovarianceZ, Prediction.PredictionNoise.pos_2, double); 119 | 120 | // Covariance parameter of attitude prediction [rad^2/s]. 121 | GETTER_AND_SETTER( 122 | GyroCovarianceX, Prediction.PredictionNoise.att_0, double); 123 | GETTER_AND_SETTER( 124 | GyroCovarianceY, Prediction.PredictionNoise.att_1, double); 125 | GETTER_AND_SETTER( 126 | GyroCovarianceZ, Prediction.PredictionNoise.att_2, double); 127 | // Covariance parameter of gyroscope bias prediction [rad^2/s^3]. 128 | GETTER_AND_SETTER( 129 | GyroBiasCovarianceX, Prediction.PredictionNoise.gyb_0, double); 130 | GETTER_AND_SETTER( 131 | GyroBiasCovarianceY, Prediction.PredictionNoise.gyb_1, double); 132 | GETTER_AND_SETTER( 133 | GyroBiasCovarianceZ, Prediction.PredictionNoise.gyb_2, double); 134 | // Covariance parameter of velocity prediction [m^2/s^3]. 135 | GETTER_AND_SETTER(AccCovarianceX, Prediction.PredictionNoise.vel_0, double); 136 | GETTER_AND_SETTER(AccCovarianceY, Prediction.PredictionNoise.vel_1, double); 137 | GETTER_AND_SETTER(AccCovarianceZ, Prediction.PredictionNoise.vel_2, double); 138 | // Covariance parameter of accelerometer bias prediction [m^2/s^5]. 139 | GETTER_AND_SETTER( 140 | AccBiasCovarianceX, Prediction.PredictionNoise.acb_0, double); 141 | GETTER_AND_SETTER( 142 | AccBiasCovarianceY, Prediction.PredictionNoise.acb_1, double); 143 | GETTER_AND_SETTER( 144 | AccBiasCovarianceZ, Prediction.PredictionNoise.acb_2, double); 145 | 146 | GETTER_AND_SETTER(ImageMaskPath, ImgUpdate.imageMaskPath, std::string); 147 | }; 148 | 149 | } // namespace rovio 150 | 151 | #endif // ROVIO_FILTER_CONFIGURATION_HPP_ 152 | -------------------------------------------------------------------------------- /include/rovio/ImagePyramid.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Autonomous Systems Lab 3 | * All rights reserved. 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 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #ifndef IMAGEPYRAMID_HPP_ 30 | #define IMAGEPYRAMID_HPP_ 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include "rovio/FeatureCoordinates.hpp" 38 | 39 | namespace rovio{ 40 | 41 | /** \brief Halfsamples an image. 42 | * 43 | * @param imgIn - Input image. 44 | * @param imgOut - Output image (halfsampled). 45 | */ 46 | inline void halfSample(const cv::Mat& imgIn,cv::Mat& imgOut){ 47 | imgOut.create(imgIn.rows/2,imgIn.cols/2,imgIn.type()); 48 | const int refStepIn = imgIn.step.p[0]; 49 | const int refStepOut = imgOut.step.p[0]; 50 | uint8_t* imgPtrInTop; 51 | uint8_t* imgPtrInBot; 52 | uint8_t* imgPtrOut; 53 | for(int y=0; y class ImagePyramidMask { 65 | public: 66 | ImagePyramidMask(){}; 67 | virtual ~ImagePyramidMask(){}; 68 | cv::Mat masks_[n_levels]; /** 110 | class ImagePyramid{ 111 | public: 112 | ImagePyramid(){}; 113 | virtual ~ImagePyramid(){}; 114 | cv::Mat imgs_[n_levels]; /**& operator=(const ImagePyramid &rhs) { 143 | for(unsigned int i=0;i=0 && l2>=0); 161 | 162 | FeatureCoordinates cOut; 163 | cOut.set_c((centers_[l1]-centers_[l2])*pow(0.5,l2)+cIn.get_c()*pow(0.5,l2-l1)); 164 | if(cIn.mpCamera_ != nullptr){ 165 | if(cIn.com_warp_c()){ 166 | cOut.set_warp_c(cIn.get_warp_c()); 167 | } 168 | } 169 | // Set to invalid value, different from the default constructed invalid value. 170 | cOut.camID_ = -2; 171 | cOut.mpCamera_ = nullptr; 172 | return cOut; 173 | } 174 | 175 | /** \brief Extract FastCorner coordinates 176 | * 177 | * @param candidates - List of the extracted corner coordinates 178 | * (defined on pyramid level 0). 179 | * @param masks - Pointer to first element of a fixed size array 180 | * containing image masks to be applied to the corner detection. 181 | * @param l - Pyramid level at which the corners should be 182 | * extracted. 183 | * @param detectionThreshold - Detection threshold of the used 184 | * cv::FastFeatureDetector. See 185 | * http://docs.opencv.org/trunk/df/d74/classcv_1_1FastFeatureDetector.html 186 | */ 187 | void detectFastCorners(FeatureCoordinatesVec &candidates, 188 | const cv::Mat *masks, const int l, 189 | const int detectionThreshold) const { 190 | CHECK_NOTNULL(masks); 191 | std::vector keypoints; 192 | #if (CV_MAJOR_VERSION < 3) 193 | cv::FastFeatureDetector feature_detector_fast(detectionThreshold, true); 194 | if (!masks[l].empty()) { 195 | CHECK_EQ(masks[l].cols, imgs_[l].cols); 196 | CHECK_EQ(masks[l].rows, imgs_[l].rows); 197 | feature_detector_fast.detect(imgs_[l], keypoints, masks[l]); 198 | } else { 199 | feature_detector_fast.detect(imgs_[l], keypoints); 200 | } 201 | #else 202 | auto feature_detector_fast = 203 | cv::FastFeatureDetector::create(detectionThreshold, true); 204 | if (!masks[l].empty()) { 205 | CHECK_EQ(masks[l].cols, imgs_[l].cols); 206 | CHECK_EQ(masks[l].rows, imgs_[l].rows); 207 | feature_detector_fast->detect(imgs_[l], keypoints, masks[l]); 208 | } else { 209 | feature_detector_fast->detect(imgs_[l], keypoints); 210 | } 211 | #endif 212 | candidates.reserve(candidates.size() + keypoints.size()); 213 | for (auto it = keypoints.cbegin(), end = keypoints.cend(); it != end; ++it) { 214 | candidates.push_back( 215 | levelTranformCoordinates(FeatureCoordinates(cv::Point2f(it->pt.x, it->pt.y)),l,0)); 216 | } 217 | } 218 | }; 219 | 220 | } 221 | 222 | 223 | #endif /* IMAGEPYRAMID_HPP_ */ 224 | -------------------------------------------------------------------------------- /include/rovio/Memory.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ROVIO_MEMORY_HPP_ 2 | #define ROVIO_MEMORY_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | namespace rovio { 11 | 12 | template