├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── Doxyfile.in ├── LICENSE ├── README.md ├── cfg ├── euroc_cam0.yaml ├── euroc_cam1.yaml ├── p22021_equidist.yaml └── rovio.info ├── include └── rovio │ ├── Camera.hpp │ ├── CoordinateTransform │ ├── FeatureOutput.hpp │ ├── FeatureOutputReadable.hpp │ ├── LandmarkOutput.hpp │ ├── PixelOutput.hpp │ ├── RovioOutput.hpp │ └── YprOutput.hpp │ ├── FeatureCoordinates.hpp │ ├── FeatureDistance.hpp │ ├── FeatureManager.hpp │ ├── FeatureStatistics.hpp │ ├── FilterStates.hpp │ ├── ImagePyramid.hpp │ ├── ImgUpdate.hpp │ ├── ImuPrediction.hpp │ ├── MultiCamera.hpp │ ├── MultilevelPatch.hpp │ ├── MultilevelPatchAlignment.hpp │ ├── Patch.hpp │ ├── PoseUpdate.hpp │ ├── RobocentricFeatureElement.hpp │ ├── RovioFilter.hpp │ ├── RovioNode.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 ├── FeatureCoordinates.cpp ├── FeatureDistance.cpp ├── Scene.cpp ├── feature_tracker_node.cpp ├── rovio_node.cpp ├── rovio_rosbag_loader.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://bitbucket.org/bloesch/lightweight_filtering.git 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.6) 2 | project(rovio) 3 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -march=native") 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 | 15 | add_subdirectory(lightweight_filtering) 16 | 17 | ##################### Find, include, and compile library ##################### 18 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 19 | if(0) 20 | find_package(OpenMP REQUIRED) 21 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 22 | endif() 23 | find_package(Eigen3 REQUIRED) 24 | include_directories(${EIGEN3_INCLUDE_DIR}) 25 | 26 | find_package(OpenCV REQUIRED COMPONENTS core highgui imgproc features2d) 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(yaml_cpp_catkin QUIET) 48 | if(${yaml_cpp_catkin_FOUND}) 49 | message(STATUS "Found yaml_cpp_catkin, using instead of system library.") 50 | set(YamlCpp_LIBRARIES ${yaml_cpp_catkin_LIBRARIES}) 51 | set(YamlCpp_INCLUDE_DIRS ${yaml_cpp_catkin_INCLUDE_DIRS}) 52 | else() 53 | message(STATUS "No yaml_cpp_catkin, using yaml-cpp system library instead.") 54 | pkg_check_modules(YamlCpp REQUIRED yaml-cpp>=0.5) 55 | endif() 56 | 57 | 58 | ##################### Install ROS stuff ##################### 59 | find_package(catkin REQUIRED COMPONENTS 60 | lightweight_filtering 61 | kindr 62 | roscpp 63 | roslib 64 | cv_bridge 65 | message_generation 66 | nav_msgs 67 | geometry_msgs 68 | sensor_msgs 69 | std_msgs 70 | tf 71 | rosbag 72 | ) 73 | 74 | add_service_files( 75 | DIRECTORY srv 76 | FILES SrvResetToPose.srv 77 | ) 78 | 79 | generate_messages(DEPENDENCIES 80 | std_msgs 81 | geometry_msgs 82 | ) 83 | 84 | catkin_package( 85 | INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} 86 | LIBRARIES ${PROJECT_NAME} 87 | CATKIN_DEPENDS 88 | lightweight_filtering 89 | kindr 90 | roscpp 91 | roslib 92 | cv_bridge 93 | message_runtime 94 | nav_msgs 95 | geometry_msgs 96 | sensor_msgs 97 | std_msgs 98 | tf 99 | rosbag 100 | yaml_cpp_catkin 101 | ) 102 | 103 | include_directories(include ${catkin_INCLUDE_DIRS} ${YamlCpp_INCLUDE_DIRS}) 104 | 105 | if(MAKE_SCENE) 106 | add_library(${PROJECT_NAME} src/Camera.cpp src/FeatureCoordinates.cpp src/FeatureDistance.cpp src/Scene.cpp) 107 | else() 108 | add_library(${PROJECT_NAME} src/Camera.cpp src/FeatureCoordinates.cpp src/FeatureDistance.cpp) 109 | endif() 110 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${YamlCpp_LIBRARIES} ${OpenMP_EXE_LINKER_FLAGS} ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} ${GLEW_LIBRARY} ${OpenCV_LIBRARIES}) 111 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} rovio_generate_messages_cpp) 112 | 113 | add_executable(rovio_node src/rovio_node.cpp) 114 | target_link_libraries(rovio_node ${PROJECT_NAME}) 115 | 116 | add_executable(rovio_rosbag_loader src/rovio_rosbag_loader.cpp) 117 | target_link_libraries(rovio_rosbag_loader ${PROJECT_NAME}) 118 | add_dependencies(rovio_rosbag_loader ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 119 | 120 | add_executable(feature_tracker_node src/feature_tracker_node.cpp) 121 | target_link_libraries(feature_tracker_node ${PROJECT_NAME}) 122 | add_dependencies(feature_tracker_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 123 | 124 | if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/gtest/") 125 | message(STATUS "Building GTests!") 126 | option(BUILD_GTEST "build gtest" ON) 127 | #add_subdirectory(gtest gtest) 128 | enable_testing() 129 | include_directories(${gtest_SOURCE_DIR}/include ${gtest_SOURCE_DIR}) 130 | add_executable(test_patch src/test_patch.cpp src/Camera.cpp src/FeatureCoordinates.cpp src/FeatureDistance.cpp) 131 | target_link_libraries(test_patch gtest_main gtest pthread ${catkin_LIBRARIES} ${YamlCpp_LIBRARIES}) 132 | add_test(test_patch test_patch) 133 | add_executable(test_mlp src/test_mlp.cpp src/Camera.cpp src/FeatureCoordinates.cpp src/FeatureDistance.cpp) 134 | target_link_libraries(test_mlp gtest_main gtest pthread ${catkin_LIBRARIES} ${YamlCpp_LIBRARIES}) 135 | add_test(test_mlp test_mlp) 136 | endif() 137 | -------------------------------------------------------------------------------- /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 | # README # 2 | 3 | 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. 4 | 5 | Video: https://youtu.be/ZMAISVy-6ao 6 | 7 | Papers: 8 | * http://dx.doi.org/10.3929/ethz-a-010566547 (IROS 2015) 9 | * http://dx.doi.org/10.1177/0278364917728574 (IJRR 2017) 10 | 11 | Please also have a look at the wiki: https://github.com/ethz-asl/rovio/wiki 12 | 13 | ### Install without opengl scene ### 14 | Dependencies: 15 | * ros 16 | * kindr (https://github.com/ethz-asl/kindr) 17 | * lightweight_filtering (as submodule, use "git submodule update --init --recursive") 18 | 19 | ``` 20 | #!command 21 | 22 | catkin build rovio --cmake-args -DCMAKE_BUILD_TYPE=Release 23 | ``` 24 | 25 | ### Install with opengl scene ### 26 | Additional dependencies: opengl, glut, glew (sudo apt-get install freeglut3-dev, sudo apt-get install libglew-dev) 27 | ``` 28 | #!command 29 | 30 | catkin build rovio --cmake-args -DCMAKE_BUILD_TYPE=Release -DMAKE_SCENE=ON 31 | ``` 32 | 33 | ### Euroc Datasets ### 34 | The rovio_node.launch file loads parameters such that ROVIO runs properly on the Euroc datasets. The datasets are available under: 35 | http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets 36 | 37 | ### Further notes ### 38 | * Camera matrix and distortion parameters should be provided by a yaml file or loaded through rosparam 39 | * 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. 40 | * 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. 41 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 "lightweight_filtering/common.hpp" 33 | #include 34 | #include "lightweight_filtering/State.hpp" 35 | 36 | namespace rovio{ 37 | 38 | class Camera{ 39 | public: 40 | /** \brief Distortion model of the camera. 41 | * */ 42 | enum ModelType{ 43 | RADTAN, //!< Radial tangential distortion model. 44 | EQUIDIST, //!< Equidistant distortion model. 45 | DS //!< Double sphere distortion model. 46 | } type_; 47 | 48 | Eigen::Matrix3d K_; //!< Intrinsic parameter matrix. 49 | 50 | //@{ 51 | /** \brief Distortion Parameter. */ 52 | double k1_,k2_,k3_,k4_,k5_,k6_; 53 | double p1_,p2_,s1_,s2_,s3_,s4_; 54 | //@} 55 | 56 | //@{ 57 | /** \brief Radius (as ratio of images shortest side) within which features can be initalized. */ 58 | double valid_radius_; 59 | //@} 60 | 61 | /** \brief Constructor. 62 | * 63 | * Initializes the camera object as pinhole camera, i.e. all distortion coefficients are set to zero. 64 | * The intrinsic parameter matrix is set equal to the identity matrix. 65 | * */ 66 | Camera(); 67 | 68 | /** \brief Destructor. 69 | * */ 70 | virtual ~Camera(); 71 | 72 | /** \brief Loads and sets the intrinsic parameter matrix K_ from yaml-file. 73 | * 74 | * @param filename - Path to the yaml-file, containing the intrinsic parameter matrix coefficients. 75 | */ 76 | void loadCameraMatrix(const std::string& filename); 77 | 78 | /** \brief Loads and sets the distortion parameters {k1_, k2_, k3_, p1_, p2_} for the Radtan distortion model from 79 | * yaml-file. 80 | * @param filename - Path to the yaml-file, containing the distortion coefficients. 81 | */ 82 | void loadRadtan(const std::string& filename); 83 | 84 | /** \brief Loads and sets the distortion parameters {k1_, k2_, k3_, k4_} for the Equidistant distortion model from 85 | * yaml-file. 86 | * @param filename - Path to the yaml-file, containing the distortion coefficients. 87 | */ 88 | void loadEquidist(const std::string& filename); 89 | 90 | /** \brief Loads and sets the distortion parameters {k1_, k2_} for the Double Sphere distortion model from 91 | * yaml-file. 92 | * @param filename - Path to the yaml-file, containing the distortion coefficients. 93 | */ 94 | void loadDoubleSphere(const std::string& filename); 95 | 96 | /** \brief Loads and sets the distortion model and the corresponding distortion coefficients from yaml-file. 97 | * 98 | * @param filename - Path to the yaml-file, containing the distortion model and distortion coefficient data. 99 | */ 100 | void load(const std::string& filename); 101 | 102 | /** \brief Distorts a point on the unit plane (in camera coordinates) according to the Radtan distortion model. 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 | */ 107 | void distortRadtan(const Eigen::Vector2d& in, Eigen::Vector2d& out) const; 108 | 109 | /** \brief Distorts a point on the unit plane (in camera coordinates) according to the Radtan distortion model and 110 | * outputs additionally the corresponding jacobian matrix (input to output). 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 | * @param J - Jacobian matrix of the distortion process (input to output). 115 | */ 116 | void distortRadtan(const Eigen::Vector2d& in, Eigen::Vector2d& out, Eigen::Matrix2d& J) const; 117 | 118 | /** \brief Distorts a point on the unit plane (in camera coordinates) according to the Equidistant distortion model. 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 | */ 123 | void distortEquidist(const Eigen::Vector2d& in, Eigen::Vector2d& out) const; 124 | 125 | /** \brief Distorts a point on the unit plane (in camera coordinates) according to the Equidistant distortion model 126 | * and outputs additionally the corresponding jacobian matrix (input to output). 127 | * 128 | * @param in - Undistorted point coordinates on the unit plane (in camera coordinates). 129 | * @param out - Distorted point coordinates on the unit plane (in camera coordinates). 130 | * @param J - Jacobian matrix of the distortion process (input to output). 131 | */ 132 | void distortEquidist(const Eigen::Vector2d& in, Eigen::Vector2d& out, Eigen::Matrix2d& J) const; 133 | 134 | /** \brief Distorts a point on the unit plane (in camera coordinates) according to the Double Sphere distortion model. 135 | * 136 | * @param in - Undistorted point coordinates on the unit plane (in camera coordinates). 137 | * @param out - Distorted point coordinates on the unit plane (in camera coordinates). 138 | */ 139 | void distortDoubleSphere(const Eigen::Vector2d& in, Eigen::Vector2d& out) const; 140 | 141 | /** \brief Distorts a point on the unit plane (in camera coordinates) according to the Double Sphere distortion model 142 | * and outputs additionally the corresponding jacobian matrix (input to output). 143 | * 144 | * @param in - Undistorted point coordinates on the unit plane (in camera coordinates). 145 | * @param out - Distorted point coordinates on the unit plane (in camera coordinates). 146 | * @param J - Jacobian matrix of the distortion process (input to output). 147 | */ 148 | void distortDoubleSphere(const Eigen::Vector2d& in, Eigen::Vector2d& out, Eigen::Matrix2d& J) const; 149 | 150 | /** \brief Distorts a point on the unit plane, according to the set distortion model (#ModelType) and to the set 151 | * distortion coefficients. 152 | * 153 | * @param in - Undistorted point coordinates on the unit plane (in camera coordinates). 154 | * @param out - Distorted point coordinates on the unit plane (in camera coordinates). 155 | */ 156 | void distort(const Eigen::Vector2d& in, Eigen::Vector2d& out) const; 157 | 158 | /** \brief Distorts a point on the unit plane, according to the set distortion model (#ModelType) and to the set 159 | * distortion coefficients. Outputs additionally the corresponding jacobian matrix (input to output). 160 | * 161 | * @param in - Undistorted point coordinates on the unit plane (in camera coordinates). 162 | * @param out - Distorted point coordinates on the unit plane (in camera coordinates). 163 | * @param J - Jacobian matrix of the distortion process (input to output). 164 | */ 165 | void distort(const Eigen::Vector2d& in, Eigen::Vector2d& out, Eigen::Matrix2d& J) const; 166 | 167 | /** \brief Outputs the (distorted) pixel coordinates corresponding to a given bearing vector, 168 | * using the set distortion model. 169 | * 170 | * @param vec - Bearing vector (in camera coordinates | unit length not necessary). 171 | * @param c - (Distorted) pixel coordinates. 172 | * @return True, if process successful. 173 | */ 174 | bool bearingToPixel(const Eigen::Vector3d& vec, cv::Point2f& c) const; 175 | 176 | /** \brief Outputs the (distorted) pixel coordinates corresponding to a given bearing vector, 177 | * using the set distortion model. Outputs additionally the corresponding jacobian matrix (input to output). 178 | * 179 | * @param vec - Bearing vector (in camera coordinates | unit length not necessary). 180 | * @param c - (Distorted) pixel coordinates. 181 | * @param J - Jacobian matrix of the distortion process (input to output). 182 | * @return True, if process successful. 183 | */ 184 | bool bearingToPixel(const Eigen::Vector3d& vec, cv::Point2f& c, Eigen::Matrix& J) const; 185 | 186 | /** \brief Outputs the (distorted) pixel coordinates corresponding to a given NormalVectorElement-Object 187 | * (bearing vector), using the set distortion model. 188 | * 189 | * @param n - NormalVectorElement-Object. 190 | * @param c - (Distorted pixel) coordinates. 191 | * @return True, if process successful. 192 | */ 193 | bool bearingToPixel(const LWF::NormalVectorElement& n, cv::Point2f& c) const; 194 | 195 | /** \brief Outputs the (distorted) pixel coordinates corresponding to a given NormalVectorElement-Object 196 | * (bearing vector), using the set distortion model. 197 | * 198 | * @param n - NormalVectorElement-Object. 199 | * @param c - (Distorted) pixel coordinates. 200 | * @param J - Jacobian matrix of the distortion process (input to output). 201 | * @return True, if process successful. 202 | */ 203 | bool bearingToPixel(const LWF::NormalVectorElement& n, cv::Point2f& c, Eigen::Matrix& J) const; 204 | 205 | /** \brief Get the bearing vector, corresponding to a specific (distorted) pixel. 206 | * 207 | * @param c - (Distorted) pixel. 208 | * @param vec - Bearing vector (unit length). 209 | * @return True, if process successful. 210 | */ 211 | bool pixelToBearing(const cv::Point2f& c,Eigen::Vector3d& vec) const; 212 | 213 | /** \brief Get the NormalVectorElement-Object (bearing vector) corresponding to a specific (distorted) pixel. 214 | * 215 | * @param c - (Distorted) pixel. 216 | * @param vec - NormalVectorElement-Object. 217 | * @return True, if process successful. 218 | */ 219 | bool pixelToBearing(const cv::Point2f& c,LWF::NormalVectorElement& n) const; 220 | 221 | /** \brief Function testing the camera model by randomly mapping bearing vectors to pixel coordinates and vice versa. 222 | */ 223 | void testCameraModel(); 224 | }; 225 | 226 | } 227 | 228 | 229 | #endif /* ROVIO_CAMERA_HPP_ */ 230 | -------------------------------------------------------------------------------- /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 | outputCamID_ = camID; 119 | } 120 | void evalTransform(mtOutput& output, const mtInput& input) const{ 121 | input.updateMultiCameraExtrinsics(mpMultiCamera_); 122 | mpMultiCamera_->transformFeature(outputCamID_,input.CfP(ID_),input.dep(ID_),output.c(),output.d()); 123 | if(input.CfP(ID_).trackWarping_ && input.CfP(ID_).com_warp_nor()){ 124 | const int& camID = input.CfP(ID_).camID_; 125 | const QPD qDC = input.qCM(outputCamID_)*input.qCM(camID).inverted(); // TODO: avoid double computation 126 | const V3D CrCD = input.qCM(camID).rotate(V3D(input.MrMC(outputCamID_)-input.MrMC(camID))); 127 | const V3D CrCP = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getVec(); 128 | const V3D DrDP = qDC.rotate(V3D(CrCP-CrCD)); 129 | const double d_out = DrDP.norm(); 130 | const LWF::NormalVectorElement nor_out(DrDP); 131 | const Eigen::Matrix J_nor_DrDP = nor_out.getM().transpose()/d_out; 132 | const Eigen::Matrix J_DrDP_CrCP = MPD(qDC).matrix(); 133 | const Eigen::Matrix J_CrCP_nor = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getM(); 134 | output.c().set_warp_nor(J_nor_DrDP*J_DrDP_CrCP*J_CrCP_nor*input.CfP(ID_).get_warp_nor()); 135 | } 136 | } 137 | void jacTransform(MXD& J, const mtInput& input) const{ 138 | J.setZero(); 139 | const int& camID = input.CfP(ID_).camID_; 140 | if(camID != outputCamID_){ 141 | input.updateMultiCameraExtrinsics(mpMultiCamera_); 142 | const QPD qDC = input.qCM(outputCamID_)*input.qCM(camID).inverted(); // TODO: avoid double computation 143 | const V3D CrCD = input.qCM(camID).rotate(V3D(input.MrMC(outputCamID_)-input.MrMC(camID))); 144 | const V3D CrCP = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getVec(); 145 | const V3D DrDP = qDC.rotate(V3D(CrCP-CrCD)); 146 | const double d_out = DrDP.norm(); 147 | const LWF::NormalVectorElement nor_out(DrDP); // TODO: test if Jacobian works, this new setting of vector is dangerous 148 | 149 | const Eigen::Matrix J_d_DrDP = nor_out.getVec().transpose(); 150 | const Eigen::Matrix J_nor_DrDP = nor_out.getM().transpose()/d_out; 151 | const Eigen::Matrix J_DrDP_qDC = gSM(DrDP); 152 | const Eigen::Matrix J_DrDP_CrCP = MPD(qDC).matrix(); 153 | const Eigen::Matrix J_DrDP_CrCD = -MPD(qDC).matrix(); 154 | 155 | const Eigen::Matrix J_qDC_qDB = Eigen::Matrix3d::Identity(); 156 | const Eigen::Matrix J_qDC_qCB = -MPD(qDC).matrix(); 157 | const Eigen::Matrix J_CrCD_qCB = gSM(CrCD); 158 | const Eigen::Matrix J_CrCD_BrBC = -MPD(input.qCM(camID)).matrix(); 159 | const Eigen::Matrix J_CrCD_BrBD = MPD(input.qCM(camID)).matrix(); 160 | 161 | const Eigen::Matrix J_CrCP_d = input.CfP(ID_).get_nor().getVec()*input.dep(ID_).getDistanceDerivative(); 162 | const Eigen::Matrix J_CrCP_nor = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getM(); 163 | 164 | J.template block<2,2>(mtOutput::template getId(),mtInput::template getId(ID_)) = J_nor_DrDP*J_DrDP_CrCP*J_CrCP_nor; 165 | J.template block<2,1>(mtOutput::template getId(),mtInput::template getId(ID_)+2) = J_nor_DrDP*J_DrDP_CrCP*J_CrCP_d; 166 | if(!ignoreDistanceOutput_){ 167 | J.template block<1,2>(mtOutput::template getId()+2,mtInput::template getId(ID_)) = J_d_DrDP*J_DrDP_CrCP*J_CrCP_nor; 168 | J.template block<1,1>(mtOutput::template getId()+2,mtInput::template getId(ID_)+2) = J_d_DrDP*J_DrDP_CrCP*J_CrCP_d; 169 | } 170 | 171 | if(input.aux().doVECalibration_ && camID != outputCamID_){ 172 | 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); 173 | J.template block<2,3>(mtOutput::template getId(),mtInput::template getId(outputCamID_)) = J_nor_DrDP*J_DrDP_qDC*J_qDC_qDB; 174 | J.template block<2,3>(mtOutput::template getId(),mtInput::template getId(camID)) = J_nor_DrDP*J_DrDP_CrCD*J_CrCD_BrBC; 175 | J.template block<2,3>(mtOutput::template getId(),mtInput::template getId(outputCamID_)) = J_nor_DrDP*J_DrDP_CrCD*J_CrCD_BrBD; 176 | if(!ignoreDistanceOutput_){ 177 | 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); 178 | J.template block<1,3>(mtOutput::template getId()+2,mtInput::template getId(outputCamID_)) = J_d_DrDP*J_DrDP_qDC*J_qDC_qDB; 179 | J.template block<1,3>(mtOutput::template getId()+2,mtInput::template getId(camID)) = J_d_DrDP*J_DrDP_CrCD*J_CrCD_BrBC; 180 | J.template block<1,3>(mtOutput::template getId()+2,mtInput::template getId(outputCamID_)) = J_d_DrDP*J_DrDP_CrCD*J_CrCD_BrBD; 181 | } 182 | } 183 | } else { 184 | J.template block<2,2>(mtOutput::template getId(),mtInput::template getId(ID_)) = Eigen::Matrix2d::Identity(); 185 | J.template block<1,1>(mtOutput::template getId()+2,mtInput::template getId(ID_)+2) = Eigen::Matrix::Identity(); 186 | } 187 | } 188 | }; 189 | 190 | } 191 | 192 | 193 | #endif /* ROVIO_FEATUREOUTPUT_HPP_ */ 194 | -------------------------------------------------------------------------------- /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 | #include 35 | #include "rovio/FeatureCoordinates.hpp" 36 | 37 | namespace rovio{ 38 | 39 | /** \brief Halfsamples an image. 40 | * 41 | * @param imgIn - Input image. 42 | * @param imgOut - Output image (halfsampled). 43 | */ 44 | void halfSample(const cv::Mat& imgIn,cv::Mat& imgOut){ 45 | imgOut.create(imgIn.rows/2,imgIn.cols/2,imgIn.type()); 46 | const int refStepIn = imgIn.step.p[0]; 47 | const int refStepOut = imgOut.step.p[0]; 48 | uint8_t* imgPtrInTop; 49 | uint8_t* imgPtrInBot; 50 | uint8_t* imgPtrOut; 51 | for(int y=0; y 68 | class ImagePyramid{ 69 | public: 70 | ImagePyramid(){}; 71 | virtual ~ImagePyramid(){}; 72 | cv::Mat imgs_[n_levels]; /**& operator=(const ImagePyramid &rhs) { 100 | for(unsigned int i=0;i=0 && l2>=0); 118 | 119 | FeatureCoordinates cOut; 120 | cOut.set_c((centers_[l1]-centers_[l2])*pow(0.5,l2)+cIn.get_c()*pow(0.5,l2-l1)); 121 | if(cIn.mpCamera_ != nullptr){ 122 | if(cIn.com_warp_c()){ 123 | cOut.set_warp_c(cIn.get_warp_c()); 124 | } 125 | } 126 | cOut.camID_ = -1; 127 | cOut.mpCamera_ = nullptr; 128 | return cOut; 129 | } 130 | 131 | /** \brief Extract FastCorner coordinates 132 | * 133 | * @param candidates - List of the extracted corner coordinates (defined on pyramid level 0). 134 | * @param l - Pyramid level at which the corners should be extracted. 135 | * @param detectionThreshold - Detection threshold of the used cv::FastFeatureDetector. 136 | * See http://docs.opencv.org/trunk/df/d74/classcv_1_1FastFeatureDetector.html 137 | * @param valid_radius - Radius inside which a feature is considered valid (as ratio of shortest image side) 138 | */ 139 | void detectFastCorners(FeatureCoordinatesVec & candidates, int l, int detectionThreshold, double valid_radius = std::numeric_limits::max()) const{ 140 | std::vector keypoints; 141 | #if (CV_MAJOR_VERSION < 3) 142 | cv::FastFeatureDetector feature_detector_fast(detectionThreshold, true); 143 | feature_detector_fast.detect(imgs_[l], keypoints); 144 | #else 145 | auto feature_detector_fast = cv::FastFeatureDetector::create(detectionThreshold, true); 146 | feature_detector_fast->detect(imgs_[l], keypoints); 147 | #endif 148 | 149 | candidates.reserve(candidates.size()+keypoints.size()); 150 | for (auto it = keypoints.cbegin(), end = keypoints.cend(); it != end; ++it) { 151 | 152 | const double x_dist = it->pt.x - imgs_[l].cols/2.0; 153 | const double y_dist = it->pt.y - imgs_[l].rows/2.0; 154 | const double max_valid_dist = valid_radius*std::min(imgs_[l].cols, imgs_[l].rows); 155 | 156 | if((x_dist*x_dist + y_dist*y_dist) > (max_valid_dist*max_valid_dist)){ 157 | continue; 158 | } 159 | 160 | candidates.push_back( 161 | levelTranformCoordinates(FeatureCoordinates(cv::Point2f(it->pt.x, it->pt.y)),l,0)); 162 | } 163 | } 164 | }; 165 | 166 | } 167 | 168 | 169 | #endif /* IMAGEPYRAMID_HPP_ */ 170 | -------------------------------------------------------------------------------- /include/rovio/MultiCamera.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_MULTICAMERA_HPP_ 30 | #define ROVIO_MULTICAMERA_HPP_ 31 | 32 | #include "lightweight_filtering/common.hpp" 33 | #include "rovio/Camera.hpp" 34 | #include "rovio/FeatureCoordinates.hpp" 35 | 36 | namespace rovio{ 37 | 38 | template 39 | class MultiCamera{ 40 | public: 41 | V3D BrBC_[nCam]; //!< Translational extrinsic parameter 42 | QPD qCB_[nCam]; //!< Rotational extrinsic parameter 43 | Camera cameras_[nCam]; //!< Camera array 44 | 45 | /** \brief Constructor. 46 | * 47 | * Initializes the multicamera object with all cameras at the same point 48 | * */ 49 | MultiCamera(){ 50 | for(unsigned int i=0;i 45 | class MultilevelPatch{ 46 | public: 47 | static const int nLevels_ = nLevels; /** patches_[nLevels_]; /** 0){ 95 | const float dXX = H_(0,0)/(count*patchSize*patchSize); 96 | const float dYY = H_(1,1)/(count*patchSize*patchSize); 97 | const float dXY = H_(0,1)/(count*patchSize*patchSize); 98 | e0_ = 0.5 * (dXX + dYY - sqrtf((dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY))); 99 | e1_ = 0.5 * (dXX + dYY + sqrtf((dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY))); 100 | s_ = e0_+e1_; 101 | } else { 102 | e0_ = 0; 103 | e1_ = 0; 104 | s_ = -1; 105 | } 106 | } 107 | 108 | /** \brief Computes and sets the Shi-Tomasi Score \ref s_, considering only one specific pyramid level. 109 | * 110 | * @param l - Considered pyramid level. 111 | */ 112 | void getSpecificLevelScore(const int l) const{ 113 | if(isValidPatch_[l]){ 114 | s_ = patches_[l].getScore(); 115 | } else { 116 | s_ = -1; 117 | } 118 | } 119 | 120 | /** \brief Draws the patches of the MultilevelPatch into an image. 121 | * 122 | * @param drawImg - Image in which should be drawn. 123 | * @param c - Center pixel coordinates of the patch (on level 0) 124 | * @param stretch - %Patch drawing magnification factor. 125 | * @param withBorder - Draw either the patches Patch::patch_ (withBorder = false) or the expanded patches 126 | * Patch::patchWithBorder_ (withBorder = true) . 127 | */ 128 | void drawMultilevelPatch(cv::Mat& drawImg,const cv::Point2i& c,int stretch = 1,const bool withBorder = false) const{ 129 | for(int l=nLevels_-1;l>=0;l--){ 130 | if(isValidPatch_[l]){ 131 | cv::Point2i corner = cv::Point2i((patchSize/2+(int)withBorder)*(pow(2,nLevels_-1)-pow(2,l)),(patchSize/2+(int)withBorder)*(pow(2,nLevels_-1)-pow(2,l))); 132 | patches_[l].drawPatch(drawImg,c+corner,stretch*pow(2,l),withBorder); 133 | } 134 | } 135 | } 136 | 137 | /** \brief Draws the patch borders into an image. 138 | * 139 | * @param drawImg - Image in which the patch borders should be drawn. 140 | * @param s - Scaling factor. 141 | * @param color - Line color. 142 | */ 143 | void drawMultilevelPatchBorder(cv::Mat& drawImg,const FeatureCoordinates& c,const float s, const cv::Scalar& color) const{ 144 | patches_[0].drawPatchBorder(drawImg,c,s*pow(2.0,nLevels-1),color); 145 | } 146 | 147 | /** \brief Computes the RMSE (Root Mean Squared Error) with respect to the patches of an other MultilevelPatch 148 | * for an specific pyramid level interval. 149 | * 150 | * @param mp - \ref MultilevelPatch, which patches should be used for the RMSE computation. 151 | * @param l1 - Start pyramid level (l1& mp, const int l1, const int l2) const{ 156 | float offset = 0.0f; 157 | for(int l = l1; l <= l2; l++){ 158 | const float* it_patch = patches_[l].patch_; 159 | const float* it_patch_in = mp.patches_[l].patch_; 160 | for(int y=0; y& pyr,const FeatureCoordinates& c, const int l = nLevels-1,const bool withBorder = false){ 191 | if(!c.isInFront() || !c.com_warp_c()) return false; 192 | const auto coorTemp = pyr.levelTranformCoordinates(c,0,l); 193 | return Patch::isPatchInFrame(pyr.imgs_[l],coorTemp,withBorder); 194 | } 195 | 196 | /** \brief Extracts a multilevel patch from a given image pyramid. 197 | * 198 | * @param pyr - Image pyramid from which the patch data should be extracted. 199 | * @param l - Patches are extracted from pyramid level 0 to l. 200 | * @param mpCoor - Coordinates of the patch in the reference image (subpixel coordinates possible). 201 | * @param mpWarp - Affine warping matrix. If nullptr not warping is considered. 202 | * @param withBorder - If true, both, the general patches and the corresponding expanded patches are extracted. 203 | */ 204 | void extractMultilevelPatchFromImage(const ImagePyramid& pyr,const FeatureCoordinates& c, const int l = nLevels-1,const bool withBorder = false){ 205 | for(unsigned int i=0;i<=l;i++){ 206 | const auto coorTemp = pyr.levelTranformCoordinates(c,0,i); 207 | isValidPatch_[i] = true; 208 | patches_[i].extractPatchFromImage(pyr.imgs_[i],coorTemp,withBorder); 209 | } 210 | } 211 | }; 212 | 213 | } 214 | 215 | 216 | #endif /* ROVIO_MULTILEVELPATCH_HPP_ */ 217 | -------------------------------------------------------------------------------- /include/rovio/Patch.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_PATCH_HPP_ 30 | #define ROVIO_PATCH_HPP_ 31 | 32 | #include "lightweight_filtering/common.hpp" 33 | #include "rovio/FeatureCoordinates.hpp" 34 | 35 | namespace rovio{ 36 | 37 | /** \brief %Patch with selectable patchSize. 38 | * 39 | * @tparam patchSize - Edge length of the patch in pixels. Value must be a multiple of 2! 40 | */ 41 | template 42 | class Patch { 43 | public: 44 | float patch_[patchSize*patchSize] __attribute__ ((aligned (16))); /** img.cols-halfpatch_size || c.get_c().y > img.rows-halfpatch_size){ 205 | return false; 206 | } else { 207 | return true; 208 | } 209 | } else { 210 | for(int y=0; y<2*halfpatch_size; y += 2*halfpatch_size-1){ 211 | for(int x=0; x<2*halfpatch_size; x += 2*halfpatch_size-1){ 212 | const float dx = x - halfpatch_size + 0.5; 213 | const float dy = y - halfpatch_size + 0.5; 214 | const float wdx = c.get_warp_c()(0,0)*dx + c.get_warp_c()(0,1)*dy; 215 | const float wdy = c.get_warp_c()(1,0)*dx + c.get_warp_c()(1,1)*dy; 216 | const float c_x = c.get_c().x+wdx - 0.5; 217 | const float c_y = c.get_c().y+wdy - 0.5; 218 | const int u_r = floor(c_x); 219 | const int v_r = floor(c_y); 220 | if(u_r < 0 || v_r < 0 || u_r >= img.cols-1 || v_r >= img.rows-1){ 221 | return false; 222 | } 223 | } 224 | } 225 | return true; 226 | } 227 | } else { 228 | return false; 229 | } 230 | } 231 | 232 | /** \brief Extracts a patch from an image. 233 | * 234 | * @param img - Reference Image. 235 | * @param c - Coordinates of the patch in the reference image (subpixel coordinates possible). 236 | * @param withBorder - If false, the patch object is only initialized with the patch data of the general patch (Patch::patch_). 237 | * If true, the patch object is initialized with both, the patch data of the general patch (Patch::patch_) 238 | * and the patch data of the expanded patch (Patch::patchWithBorder_). 239 | */ 240 | void extractPatchFromImage(const cv::Mat& img,const FeatureCoordinates& c,const bool withBorder = false){ 241 | assert(isPatchInFrame(img,c,withBorder)); 242 | const int halfpatch_size = patchSize/2+(int)withBorder; 243 | const int refStep = img.step.p[0]; 244 | 245 | // Get Pointers 246 | uint8_t* img_ptr; 247 | float* patch_ptr; 248 | if(withBorder){ 249 | patch_ptr = patchWithBorder_; 250 | } else { 251 | patch_ptr = patch_; 252 | } 253 | 254 | if(c.isNearIdentityWarping()){ 255 | const int u_r = floor(c.get_c().x); 256 | const int v_r = floor(c.get_c().y); 257 | 258 | // compute interpolation weights 259 | const float subpix_x = c.get_c().x-u_r; 260 | const float subpix_y = c.get_c().y-v_r; 261 | const float wTL = (1.0-subpix_x)*(1.0-subpix_y); 262 | const float wTR = subpix_x * (1.0-subpix_y); 263 | const float wBL = (1.0-subpix_x)*subpix_y; 264 | const float wBR = subpix_x * subpix_y; 265 | for(int y=0; y<2*halfpatch_size; ++y){ 266 | img_ptr = (uint8_t*) img.data + (v_r+y-halfpatch_size)*refStep + u_r-halfpatch_size; 267 | for(int x=0; x<2*halfpatch_size; ++x, ++img_ptr, ++patch_ptr) 268 | { 269 | *patch_ptr = wTL*img_ptr[0]; 270 | if(subpix_x > 0) *patch_ptr += wTR*img_ptr[1]; 271 | if(subpix_y > 0) *patch_ptr += wBL*img_ptr[refStep]; 272 | if(subpix_x > 0 && subpix_y > 0) *patch_ptr += wBR*img_ptr[refStep+1]; 273 | } 274 | } 275 | } else { 276 | for(int y=0; y<2*halfpatch_size; ++y){ 277 | for(int x=0; x<2*halfpatch_size; ++x, ++patch_ptr){ 278 | const float dx = x - halfpatch_size + 0.5; 279 | const float dy = y - halfpatch_size + 0.5; 280 | const float wdx = c.get_warp_c()(0,0)*dx + c.get_warp_c()(0,1)*dy; 281 | const float wdy = c.get_warp_c()(1,0)*dx + c.get_warp_c()(1,1)*dy; 282 | const float u_pixel = c.get_c().x+wdx - 0.5; 283 | const float v_pixel = c.get_c().y+wdy - 0.5; 284 | const int u_r = floor(u_pixel); 285 | const int v_r = floor(v_pixel); 286 | const float subpix_x = u_pixel-u_r; 287 | const float subpix_y = v_pixel-v_r; 288 | const float wTL = (1.0-subpix_x) * (1.0-subpix_y); 289 | const float wTR = subpix_x * (1.0-subpix_y); 290 | const float wBL = (1.0-subpix_x) * subpix_y; 291 | const float wBR = subpix_x * subpix_y; 292 | img_ptr = (uint8_t*) img.data + v_r*refStep + u_r; 293 | *patch_ptr = wTL*img_ptr[0]; 294 | if(subpix_x > 0) *patch_ptr += wTR*img_ptr[1]; 295 | if(subpix_y > 0) *patch_ptr += wBL*img_ptr[refStep]; 296 | if(subpix_x > 0 && subpix_y > 0) *patch_ptr += wBR*img_ptr[refStep+1]; 297 | } 298 | } 299 | } 300 | if(withBorder){ 301 | extractPatchFromPatchWithBorder(); 302 | } 303 | validGradientParameters_ = false; 304 | } 305 | }; 306 | 307 | } 308 | 309 | 310 | #endif /* ROVIO_PATCH_HPP_ */ 311 | -------------------------------------------------------------------------------- /include/rovio/RobocentricFeatureElement.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 ROBOCENTRICFEATUREELEMENT_HPP_ 30 | #define ROBOCENTRICFEATUREELEMENT_HPP_ 31 | 32 | #include "lightweight_filtering/common.hpp" 33 | #include "rovio/FeatureCoordinates.hpp" 34 | #include "rovio/FeatureDistance.hpp" 35 | 36 | namespace rovio{ 37 | 38 | class RobocentricFeatureElement: public LWF::ElementBase{ 39 | public: 40 | typedef LWF::ElementBase Base; 41 | using typename Base::mtDifVec; 42 | using typename Base::mtGet; 43 | using Base::name_; 44 | mutable LWF::NormalVectorElement::mtDifVec norDifTemp_; 45 | mutable MXD norCovMatTemp_; 46 | mutable LWF::NormalVectorElement norTemp_; 47 | FeatureCoordinates coordinates_; 48 | FeatureDistance distance_; 49 | RobocentricFeatureElement():distance_(FeatureDistance::REGULAR), norCovMatTemp_(3,3){} 50 | RobocentricFeatureElement(const Camera* mpCameras, const FeatureDistance::Type depthType):coordinates_(mpCameras),distance_(depthType), norCovMatTemp_(3,3){} 51 | RobocentricFeatureElement(const RobocentricFeatureElement& other):distance_(other.distance_.type_), norCovMatTemp_(3,3){ 52 | coordinates_ = other.coordinates_; 53 | distance_ = other.distance_; 54 | } 55 | 56 | /** \brief Destructor 57 | */ 58 | virtual ~RobocentricFeatureElement(){}; 59 | 60 | void boxPlus(const mtDifVec& vecIn, RobocentricFeatureElement& stateOut) const{ 61 | if(&stateOut != this){ 62 | stateOut.coordinates_ = coordinates_; 63 | stateOut.distance_ = distance_; 64 | } 65 | coordinates_.get_nor().boxPlus(vecIn.template block<2,1>(0,0),norTemp_); 66 | stateOut.coordinates_.set_nor(norTemp_,false); 67 | stateOut.distance_.p_ = distance_.p_ + vecIn(2); 68 | } 69 | void boxMinus(const RobocentricFeatureElement& stateIn, mtDifVec& vecOut) const{ 70 | coordinates_.get_nor().boxMinus(stateIn.coordinates_.get_nor(),norDifTemp_); 71 | vecOut.template block<2,1>(0,0) = norDifTemp_; 72 | vecOut(2) = distance_.p_-stateIn.distance_.p_; 73 | } 74 | void boxMinusJac(const RobocentricFeatureElement& stateIn, MXD& matOut) const{ 75 | matOut.setIdentity(); 76 | coordinates_.get_nor().boxMinusJac(stateIn.coordinates_.get_nor(),norCovMatTemp_); 77 | matOut.template block<2,2>(0,0) = norCovMatTemp_; 78 | } 79 | void print() const{ 80 | std::cout << "Bearing vector: " << coordinates_.get_nor().getVec().transpose() << " ,depth-parameter: " << distance_.p_ << std::endl; 81 | } 82 | void setIdentity(){ 83 | norTemp_.setIdentity(); 84 | coordinates_.set_nor(norTemp_); 85 | coordinates_.set_warp_identity(); 86 | distance_.p_ = 1.0; 87 | } 88 | void setRandom(unsigned int& s){ 89 | norTemp_.setRandom(s); 90 | coordinates_.set_nor(norTemp_); 91 | std::default_random_engine generator (s); 92 | std::normal_distribution distribution (0.0,1.0); 93 | distance_.p_ = distribution(generator) + 1.0; 94 | s++; 95 | } 96 | void fix(){ 97 | norTemp_ = coordinates_.get_nor(); 98 | norTemp_.fix(); 99 | coordinates_.set_nor(norTemp_,false); 100 | } 101 | mtGet& get(unsigned int i = 0){ 102 | assert(i==0); 103 | return *this; 104 | } 105 | const mtGet& get(unsigned int i = 0) const{ 106 | assert(i==0); 107 | return *this; 108 | } 109 | void registerElementToPropertyHandler(LWF::PropertyHandler* mpPropertyHandler, const std::string& str){ 110 | coordinates_.nor_.registerElementToPropertyHandler(mpPropertyHandler,str + name_); // Does not work properly since when loading the info file the validity of nor_/c_ is not adapted 111 | mpPropertyHandler->doubleRegister_.registerScalar(str + name_ + "_d", distance_.p_); 112 | } 113 | }; 114 | 115 | } 116 | 117 | 118 | #endif /* ROBOCENTRICFEATUREELEMENT_HPP_ */ 119 | -------------------------------------------------------------------------------- /include/rovio/RovioFilter.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_ROVIO_FILTER_HPP_ 30 | #define ROVIO_ROVIO_FILTER_HPP_ 31 | 32 | #include "lightweight_filtering/common.hpp" 33 | #include "lightweight_filtering/FilterBase.hpp" 34 | #include "rovio/FilterStates.hpp" 35 | #include "rovio/ImgUpdate.hpp" 36 | #include "rovio/PoseUpdate.hpp" 37 | #include "rovio/VelocityUpdate.hpp" 38 | #include "rovio/ImuPrediction.hpp" 39 | #include "rovio/MultiCamera.hpp" 40 | 41 | namespace rovio { 42 | /** \brief Class, defining the Rovio Filter. 43 | * 44 | * @tparam FILTERSTATE - \ref rovio::FilterState 45 | */ 46 | template 47 | class RovioFilter:public LWF::FilterBase, 48 | ImgUpdate, 49 | PoseUpdate0)-1,(int)(FILTERSTATE::mtState::nPose_>1)*2-1>, 50 | VelocityUpdate>{ 51 | public: 52 | typedef LWF::FilterBase, 53 | ImgUpdate, 54 | PoseUpdate0)-1,(int)(FILTERSTATE::mtState::nPose_>1)*2-1>, 55 | VelocityUpdate> Base; 56 | using Base::init_; 57 | using Base::reset; 58 | using Base::predictionTimeline_; 59 | using Base::safe_; 60 | using Base::front_; 61 | using Base::readFromInfo; 62 | using Base::boolRegister_; 63 | using Base::intRegister_; 64 | using Base::doubleRegister_; 65 | using Base::mUpdates_; 66 | using Base::mPrediction_; 67 | using Base::stringRegister_; 68 | using Base::subHandlers_; 69 | using Base::updateToUpdateMeasOnly_; 70 | typedef typename Base::mtFilterState mtFilterState; 71 | typedef typename Base::mtPrediction mtPrediction; 72 | typedef typename Base::mtState mtState; 73 | rovio::MultiCamera multiCamera_; 74 | std::string cameraCalibrationFile_[mtState::nCam_]; 75 | int depthTypeInt_; 76 | 77 | /** \brief Constructor. Initializes the filter. 78 | */ 79 | RovioFilter(){ 80 | updateToUpdateMeasOnly_ = true; 81 | std::get<0>(mUpdates_).setCamera(&multiCamera_); 82 | init_.setCamera(&multiCamera_); 83 | depthTypeInt_ = 1; 84 | subHandlers_.erase("Update0"); 85 | subHandlers_["ImgUpdate"] = &std::get<0>(mUpdates_); 86 | subHandlers_.erase("Update1"); 87 | subHandlers_["PoseUpdate"] = &std::get<1>(mUpdates_); 88 | subHandlers_.erase("Update2"); 89 | subHandlers_["VelocityUpdate"] = &std::get<2>(mUpdates_); 90 | boolRegister_.registerScalar("Common.doVECalibration",init_.state_.aux().doVECalibration_); 91 | intRegister_.registerScalar("Common.depthType",depthTypeInt_); 92 | for(int camID=0;camID(camID)+j,mtState::template getId(camID)+j)); 106 | doubleRegister_.removeScalarByVar(init_.cov_(mtState::template getId(camID)+j,mtState::template getId(camID)+j)); 107 | doubleRegister_.registerScalar("Init.Covariance.vep",init_.cov_(mtState::template getId(camID)+j,mtState::template getId(camID)+j)); 108 | doubleRegister_.registerScalar("Init.Covariance.vea",init_.cov_(mtState::template getId(camID)+j,mtState::template getId(camID)+j)); 109 | } 110 | doubleRegister_.registerVector("Camera" + std::to_string(camID) + ".MrMC",init_.state_.MrMC(camID)); 111 | doubleRegister_.registerQuaternion("Camera" + std::to_string(camID) + ".qCM",init_.state_.qCM(camID)); 112 | } 113 | for(int i=0;i(i)+j,mtState::template getId(i)+j)); 123 | doubleRegister_.removeScalarByVar(init_.cov_(mtState::template getId(i)+j,mtState::template getId(i)+j)); 124 | } 125 | } 126 | if(std::get<1>(mUpdates_).inertialPoseIndex_>=0){ 127 | std::get<1>(mUpdates_).doubleRegister_.registerVector("IrIW",init_.state_.poseLin(std::get<1>(mUpdates_).inertialPoseIndex_)); 128 | std::get<1>(mUpdates_).doubleRegister_.registerQuaternion("qWI",init_.state_.poseRot(std::get<1>(mUpdates_).inertialPoseIndex_)); 129 | for(int j=0;j<3;j++){ 130 | std::get<1>(mUpdates_).doubleRegister_.registerScalar("init_cov_IrIW",init_.cov_(mtState::template getId(std::get<1>(mUpdates_).inertialPoseIndex_)+j,mtState::template getId(std::get<1>(mUpdates_).inertialPoseIndex_)+j)); 131 | std::get<1>(mUpdates_).doubleRegister_.registerScalar("init_cov_qWI",init_.cov_(mtState::template getId(std::get<1>(mUpdates_).inertialPoseIndex_)+j,mtState::template getId(std::get<1>(mUpdates_).inertialPoseIndex_)+j)); 132 | std::get<1>(mUpdates_).doubleRegister_.registerScalar("pre_cov_IrIW",mPrediction_.prenoiP_(mtPrediction::mtNoise::template getId(std::get<1>(mUpdates_).inertialPoseIndex_)+j,mtPrediction::mtNoise::template getId(std::get<1>(mUpdates_).inertialPoseIndex_)+j)); 133 | std::get<1>(mUpdates_).doubleRegister_.registerScalar("pre_cov_qWI",mPrediction_.prenoiP_(mtPrediction::mtNoise::template getId(std::get<1>(mUpdates_).inertialPoseIndex_)+j,mtPrediction::mtNoise::template getId(std::get<1>(mUpdates_).inertialPoseIndex_)+j)); 134 | } 135 | } 136 | if(std::get<1>(mUpdates_).bodyPoseIndex_>=0){ 137 | std::get<1>(mUpdates_).doubleRegister_.registerVector("MrMV",init_.state_.poseLin(std::get<1>(mUpdates_).bodyPoseIndex_)); 138 | std::get<1>(mUpdates_).doubleRegister_.registerQuaternion("qVM",init_.state_.poseRot(std::get<1>(mUpdates_).bodyPoseIndex_)); 139 | for(int j=0;j<3;j++){ 140 | std::get<1>(mUpdates_).doubleRegister_.registerScalar("init_cov_MrMV",init_.cov_(mtState::template getId(std::get<1>(mUpdates_).bodyPoseIndex_)+j,mtState::template getId(std::get<1>(mUpdates_).bodyPoseIndex_)+j)); 141 | std::get<1>(mUpdates_).doubleRegister_.registerScalar("init_cov_qVM",init_.cov_(mtState::template getId(std::get<1>(mUpdates_).bodyPoseIndex_)+j,mtState::template getId(std::get<1>(mUpdates_).bodyPoseIndex_)+j)); 142 | std::get<1>(mUpdates_).doubleRegister_.registerScalar("pre_cov_MrMV",mPrediction_.prenoiP_(mtPrediction::mtNoise::template getId(std::get<1>(mUpdates_).bodyPoseIndex_)+j,mtPrediction::mtNoise::template getId(std::get<1>(mUpdates_).bodyPoseIndex_)+j)); 143 | std::get<1>(mUpdates_).doubleRegister_.registerScalar("pre_cov_qVM",mPrediction_.prenoiP_(mtPrediction::mtNoise::template getId(std::get<1>(mUpdates_).bodyPoseIndex_)+j,mtPrediction::mtNoise::template getId(std::get<1>(mUpdates_).bodyPoseIndex_)+j)); 144 | } 145 | } 146 | int ind; 147 | for(int i=0;i(i); 149 | doubleRegister_.removeScalarByVar(init_.cov_(ind,ind)); 150 | doubleRegister_.removeScalarByVar(init_.cov_(ind+1,ind+1)); 151 | ind = mtState::template getId(i)+2; 152 | doubleRegister_.removeScalarByVar(init_.cov_(ind,ind)); 153 | doubleRegister_.removeScalarByVar(init_.state_.dep(i).p_); 154 | doubleRegister_.removeScalarByVar(init_.state_.CfP(i).nor_.q_.toImplementation().w()); 155 | doubleRegister_.removeScalarByVar(init_.state_.CfP(i).nor_.q_.toImplementation().x()); 156 | doubleRegister_.removeScalarByVar(init_.state_.CfP(i).nor_.q_.toImplementation().y()); 157 | doubleRegister_.removeScalarByVar(init_.state_.CfP(i).nor_.q_.toImplementation().z()); 158 | std::get<0>(mUpdates_).intRegister_.registerScalar("statLocalQualityRange",init_.fsm_.features_[i].mpStatistics_->localQualityRange_); 159 | std::get<0>(mUpdates_).intRegister_.registerScalar("statLocalVisibilityRange",init_.fsm_.features_[i].mpStatistics_->localVisibilityRange_); 160 | std::get<0>(mUpdates_).intRegister_.registerScalar("statMinGlobalQualityRange",init_.fsm_.features_[i].mpStatistics_->minGlobalQualityRange_); 161 | std::get<0>(mUpdates_).boolRegister_.registerScalar("doPatchWarping",init_.state_.CfP(i).trackWarping_); 162 | } 163 | std::get<0>(mUpdates_).doubleRegister_.removeScalarByVar(std::get<0>(mUpdates_).outlierDetection_.getMahalTh(0)); 164 | std::get<0>(mUpdates_).doubleRegister_.registerScalar("MahalanobisTh",std::get<0>(mUpdates_).outlierDetection_.getMahalTh(0)); 165 | std::get<0>(mUpdates_).outlierDetection_.setEnabledAll(true); 166 | std::get<1>(mUpdates_).outlierDetection_.setEnabledAll(true); 167 | boolRegister_.registerScalar("Common.verbose",std::get<0>(mUpdates_).verbose_); 168 | mPrediction_.doubleRegister_.removeScalarByStr("alpha"); 169 | mPrediction_.doubleRegister_.removeScalarByStr("beta"); 170 | mPrediction_.doubleRegister_.removeScalarByStr("kappa"); 171 | boolRegister_.registerScalar("PoseUpdate.doVisualization",init_.plotPoseMeas_); 172 | reset(0.0); 173 | } 174 | 175 | /** \brief Reloads the camera calibration for all cameras and resets the depth map type. 176 | */ 177 | void refreshProperties(){ 178 | if(std::get<0>(mUpdates_).useDirectMethod_){ 179 | init_.mode_ = LWF::ModeIEKF; 180 | } else { 181 | init_.mode_ = LWF::ModeEKF; 182 | } 183 | for(int camID = 0;camIDIMU Coordinates) 215 | * @param t - Current time. 216 | */ 217 | void resetWithPose(V3D WrWM, QPD qMW, double t = 0.0) { 218 | init_.initWithImuPose(WrWM, qMW); 219 | reset(t); 220 | } 221 | 222 | /** \brief Sets the transformation between IMU and Camera. 223 | * 224 | * @param R_VM - Rotation matrix, expressing the orientation of the IMU in Camera Cooridinates (IMU Coordinates -> Camera Coordinates). 225 | * @param VrVM - Vector, pointing from the camera frame to the IMU frame, expressed in IMU Coordinates. 226 | * @param camID - ID of the considered camera. 227 | */ 228 | void setExtrinsics(const Eigen::Matrix3d& R_CM, const Eigen::Vector3d& CrCM, const int camID = 0){ 229 | kindr::RotationMatrixD R(R_CM); 230 | init_.state_.aux().qCM_[camID] = QPD(R); 231 | init_.state_.aux().MrMC_[camID] = -init_.state_.aux().qCM_[camID].inverseRotate(CrCM); 232 | } 233 | }; 234 | 235 | } 236 | 237 | 238 | #endif /* ROVIO_ROVIO_FILTER_HPP_ */ 239 | -------------------------------------------------------------------------------- /include/rovio/RovioScene.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_ROVIOSCENE_HPP_ 30 | #define ROVIO_ROVIOSCENE_HPP_ 31 | 32 | #include "lightweight_filtering/common.hpp" 33 | #include "rovio/Scene.hpp" 34 | #include 35 | 36 | namespace rovio { 37 | 38 | template 39 | class RovioScene{ 40 | public: 41 | typedef typename FILTER::mtFilterState mtFilterState; 42 | typedef typename mtFilterState::mtState mtState; 43 | std::shared_ptr mpFilter_; 44 | 45 | rovio::Scene mScene; 46 | cv::Mat patch_; 47 | std::shared_ptr mpSensor_[mtState::nCam_]; 48 | std::shared_ptr mpGroundtruth_; 49 | std::shared_ptr mpLines_[mtState::nCam_]; 50 | std::shared_ptr mpDepthVar_[mtState::nCam_]; 51 | std::shared_ptr mpPatches_[mtState::nMax_]; 52 | RovioScene(){ 53 | patch_ = cv::Mat::zeros(mtState::patchSize_*pow(2,mtState::nLevels_-1),mtState::patchSize_*pow(2,mtState::nLevels_-1),CV_8UC1); 54 | } 55 | virtual ~RovioScene(){}; 56 | void addKeyboardCB(unsigned char Key, std::function f){ 57 | mScene.addKeyboardCB(Key,f); 58 | } 59 | void addSpecialKeyboardCB(int Key, std::function f){ 60 | mScene.addSpecialKeyboardCB(Key,f); 61 | } 62 | void initScene(int argc, char** argv, const std::string& mVSFileName,const std::string& mFSFileName,std::shared_ptr mpFilter){ 63 | initGlut(argc,argv,mScene); 64 | mScene.init(argc, argv,mVSFileName,mFSFileName); 65 | mpFilter_ = mpFilter; 66 | 67 | std::shared_ptr mpGroundplane1(mScene.addSceneObject()); 68 | mpGroundplane1->makeGroundPlaneMesh(0.25,40); 69 | mpGroundplane1->setColorFull(Eigen::Vector4f(0.6f,0.6f,0.6f,1.0f)); 70 | mpGroundplane1->lineWidth_ = 1.0f; 71 | mpGroundplane1->W_r_WB_(2) = -1.0f; 72 | std::shared_ptr mpGroundplane2(mScene.addSceneObject()); 73 | mpGroundplane2->makeGroundPlaneMesh(1.0,10); 74 | mpGroundplane2->setColorFull(Eigen::Vector4f(0.8f,0.8f,0.8f,1.0f)); 75 | mpGroundplane2->lineWidth_ = 3.0f; 76 | mpGroundplane2->W_r_WB_(2) = -1.0f; 77 | for(int camID=0;camIDmakeCoordinateFrame(1.0f); 80 | mpSensor_[camID]->lineWidth_ = 5.0f; 81 | 82 | mpLines_[camID] = mScene.addSceneObject(); 83 | mpLines_[camID]->makeLine(); 84 | mpLines_[camID]->lineWidth_ = 2.0f; 85 | mpLines_[camID]->mode_ = GL_LINES; 86 | 87 | mpDepthVar_[camID] = mScene.addSceneObject(); 88 | mpDepthVar_[camID]->makeLine(); 89 | mpDepthVar_[camID]->lineWidth_ = 3.0f; 90 | mpDepthVar_[camID]->mode_ = GL_LINES; 91 | } 92 | mpGroundtruth_ = mScene.addSceneObject(); 93 | mpGroundtruth_->makeCoordinateFrame(1.0f); 94 | mpGroundtruth_->lineWidth_ = 5.0f; 95 | Eigen::Vector4f color; 96 | for(int i=0;i<3;i++){ 97 | color = Eigen::Vector4f(0.0f,0.0f,0.0f,1.0f); 98 | color(i) = 0.6f; 99 | mpGroundtruth_->vertices_[i*2].color_.fromEigen(color); 100 | mpGroundtruth_->vertices_[i*2+1].color_.fromEigen(color); 101 | } 102 | mpGroundtruth_->allocateBuffer(); 103 | for(int i=0;iinitTexture(mtState::patchSize_*pow(2,mtState::nLevels_-1),mtState::patchSize_*pow(2,mtState::nLevels_-1)); 106 | } 107 | 108 | mScene.setView(Eigen::Vector3f(-5.0f,-5.0f,5.0f),Eigen::Vector3f(0.0f,0.0f,0.0f)); 109 | mScene.setYDown(); 110 | } 111 | void setIdleFunction(void (*idleFunc)()){ 112 | mScene.setIdleFunction(idleFunc); 113 | } 114 | void drawScene(mtFilterState& filterState){ 115 | const mtState& state = filterState.state_; 116 | const MXD& cov = filterState.cov_; 117 | 118 | if(filterState.plotPoseMeas_){ 119 | mpGroundtruth_->q_BW_ = filterState.state_.qCW_ext(); 120 | mpGroundtruth_->W_r_WB_ = filterState.state_.WrWC_ext().template cast(); 121 | mpGroundtruth_->draw_ = true; 122 | } else { 123 | mpGroundtruth_->draw_ = false; 124 | } 125 | 126 | for(unsigned int i=0;idraw_ = false; 128 | } 129 | 130 | for(unsigned int camID=0;camIDW_r_WB_ = state.WrWC(camID).template cast(); 132 | mpSensor_[camID]->q_BW_ = state.qCW(camID); 133 | 134 | std::vector points; 135 | std::vector lines; 136 | mpLines_[camID]->clear(); 137 | mpDepthVar_[camID]->clear(); 138 | FeatureDistance d; 139 | double d_far,d_near; 140 | const float s = pow(2.0,mtState::nLevels_-1)*0.5*mtState::patchSize_; 141 | for(unsigned int i=0;icamID_ == camID){ 143 | mpPatches_[i]->draw_ = true; 144 | d = state.dep(i); 145 | const double sigma = cov(mtState::template getId(i)+2,mtState::template getId(i)+2); 146 | d.p_ -= sigma; 147 | d_far = d.getDistance(); 148 | if(d.getType() == FeatureDistance::INVERSE && (d_far > 1000 || d_far <= 0.0)) d_far = 1000; 149 | d.p_ += 2*sigma; 150 | d_near = d.getDistance(); 151 | const LWF::NormalVectorElement middle = state.CfP(i).get_nor(); 152 | Eigen::Vector3d cornerVec[4]; 153 | const double s = mtState::patchSize_*std::pow(2.0,mtState::nLevels_-2); 154 | if(!filterState.fsm_.features_[i].mpCoordinates_->com_warp_nor()){ 155 | mpPatches_[i]->draw_ = false; 156 | continue; 157 | } 158 | for(int x=0;x<2;x++){ 159 | for(int y=0;y<2;y++){ 160 | cornerVec[y*2+x] = filterState.fsm_.features_[i].mpCoordinates_->get_patchCorner(s*(2*x-1),s*(2*y-1)).get_nor().getVec()*state.dep(i).getDistance(); 161 | } 162 | } 163 | const Eigen::Vector3d pos = middle.getVec()*state.dep(i).getDistance(); 164 | const Eigen::Vector3d pos_far = middle.getVec()*d_far; 165 | const Eigen::Vector3d pos_near = middle.getVec()*d_near; 166 | 167 | mpLines_[camID]->prolonge(cornerVec[0].cast()); 168 | mpLines_[camID]->prolonge(cornerVec[1].cast()); 169 | mpLines_[camID]->prolonge(cornerVec[1].cast()); 170 | mpLines_[camID]->prolonge(cornerVec[3].cast()); 171 | mpLines_[camID]->prolonge(cornerVec[3].cast()); 172 | mpLines_[camID]->prolonge(cornerVec[2].cast()); 173 | mpLines_[camID]->prolonge(cornerVec[2].cast()); 174 | mpLines_[camID]->prolonge(cornerVec[0].cast()); 175 | 176 | mpDepthVar_[camID]->prolonge(pos_far.cast()); 177 | mpDepthVar_[camID]->prolonge(pos_near.cast()); 178 | if(filterState.fsm_.features_[i].mpStatistics_->trackedInSomeFrame()){ 179 | std::next(mpDepthVar_[camID]->vertices_.rbegin())->color_.fromEigen(Eigen::Vector4f(0.0f,1.0f,0.0f,1.0f)); 180 | mpDepthVar_[camID]->vertices_.rbegin()->color_.fromEigen(Eigen::Vector4f(0.0f,1.0f,0.0f,1.0f)); 181 | for(int j=0;j<8;j++){ 182 | std::next(mpLines_[camID]->vertices_.rbegin(),j)->color_.fromEigen(Eigen::Vector4f(0.0f,1.0f,0.0f,1.0f)); 183 | } 184 | } else if(filterState.fsm_.features_[i].mpStatistics_->inSomeFrame()){ 185 | std::next( mpDepthVar_[camID]->vertices_.rbegin())->color_.fromEigen(Eigen::Vector4f(1.0f,0.0f,0.0f,1.0f)); 186 | mpDepthVar_[camID]->vertices_.rbegin()->color_.fromEigen(Eigen::Vector4f(1.0f,0.0f,0.0f,1.0f)); 187 | for(int j=0;j<8;j++){ 188 | std::next(mpLines_[camID]->vertices_.rbegin(),j)->color_.fromEigen(Eigen::Vector4f(1.0f,0.0f,0.0f,1.0f)); 189 | } 190 | } else { 191 | std::next( mpDepthVar_[camID]->vertices_.rbegin())->color_.fromEigen(Eigen::Vector4f(0.5f,0.5f,0.5f,1.0f)); 192 | mpDepthVar_[camID]->vertices_.rbegin()->color_.fromEigen(Eigen::Vector4f(0.5f,0.5f,0.5f,1.0f)); 193 | for(int j=0;j<8;j++){ 194 | std::next(mpLines_[camID]->vertices_.rbegin(),j)->color_.fromEigen(Eigen::Vector4f(0.5f,0.5f,0.5f,1.0f)); 195 | } 196 | } 197 | 198 | mpPatches_[i]->clear(); 199 | mpPatches_[i]->makeTexturedRectangle(1.0f,1.0f); 200 | filterState.fsm_.features_[i].mpMultilevelPatch_->drawMultilevelPatch(patch_,cv::Point2i(0,0),1,false); 201 | mpPatches_[i]->setTexture(patch_); 202 | for(int x=0;x<2;x++){ 203 | for(int y=0;y<2;y++){ 204 | mpPatches_[i]->vertices_[y*2+x].pos_.fromEigen(cornerVec[y*2+x].cast()); 205 | } 206 | } 207 | mpPatches_[i]->allocateBuffer(); 208 | mpPatches_[i]->W_r_WB_ = mpSensor_[camID]->W_r_WB_; 209 | mpPatches_[i]->q_BW_ = mpSensor_[camID]->q_BW_; 210 | } 211 | mpLines_[camID]->W_r_WB_ = mpSensor_[camID]->W_r_WB_; 212 | mpLines_[camID]->q_BW_ = mpSensor_[camID]->q_BW_; 213 | mpLines_[camID]->allocateBuffer(); 214 | mpDepthVar_[camID]->W_r_WB_ = mpSensor_[camID]->W_r_WB_; 215 | mpDepthVar_[camID]->q_BW_ = mpSensor_[camID]->q_BW_; 216 | mpDepthVar_[camID]->allocateBuffer(); 217 | } 218 | } 219 | } 220 | }; 221 | 222 | } 223 | 224 | 225 | #endif /* ROVIO_ROVIOSCENE_HPP_ */ 226 | -------------------------------------------------------------------------------- /include/rovio/Scene.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_SCENE_HPP_ 30 | #define ROVIO_SCENE_HPP_ 31 | 32 | #include "lightweight_filtering/common.hpp" 33 | #include 34 | #include "GL/glew.h" 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | namespace rovio{ 41 | 42 | struct PersProjInfo{ 43 | float FOV_; 44 | float width_; 45 | float height_; 46 | float zNear_; 47 | float zFar_; 48 | }; 49 | 50 | struct DirectionalLight 51 | { 52 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 53 | Eigen::Vector3f color_; 54 | float ambientIntensity_; 55 | Eigen::Vector3f direction_; 56 | float diffuseIntensity_; 57 | }; 58 | 59 | 60 | static bool ReadShaderFile(const char* pFileName, std::string& outFile){ 61 | std::ifstream f(pFileName); 62 | bool ret = false; 63 | if (f.is_open()) { 64 | std::string line; 65 | while (getline(f, line)) { 66 | outFile.append(line); 67 | outFile.append("\n"); 68 | } 69 | f.close(); 70 | ret = true; 71 | } 72 | return ret; 73 | } 74 | 75 | static Eigen::Matrix4f InitTransform(const Eigen::Vector3f& v,const kindr::RotationQuaternionPF& q){ 76 | Eigen::Matrix4f m = Eigen::Matrix4f::Identity(); 77 | m.block<3,3>(0,0) = kindr::RotationMatrixPF(q).matrix(); 78 | m.block<3,1>(0,3) = -q.rotate(v); 79 | return m; 80 | } 81 | 82 | static Eigen::Matrix4f InitInverseTransform(const Eigen::Vector3f& v,const kindr::RotationQuaternionPF& q){ 83 | Eigen::Matrix4f m = Eigen::Matrix4f::Identity(); 84 | m.block<3,3>(0,0) = kindr::RotationMatrixPF(q).inverted().matrix(); 85 | m.block<3,1>(0,3) = v; 86 | return m; 87 | } 88 | 89 | static Eigen::Matrix4f InitPersProjTransform(const PersProjInfo& p){ 90 | Eigen::Matrix4f m; 91 | const float ar = p.width_ / p.height_; 92 | const float zRange = p.zFar_ - p.zNear_; 93 | const float tanHalfFOV = tanf(p.FOV_ / 2.0f); 94 | 95 | m(0,0) = 1.0f/(tanHalfFOV * ar); m(0,1) = 0.0f; m(0,2) = 0.0f; m(0,3) = 0.0; 96 | m(1,0) = 0.0f; m(1,1) = 1.0f/tanHalfFOV; m(1,2) = 0.0f; m(1,3) = 0.0; 97 | m(2,0) = 0.0f; m(2,1) = 0.0f; m(2,2) = (-p.zNear_ - p.zFar_)/zRange ; m(2,3) = 2.0f*p.zFar_*p.zNear_/zRange; 98 | m(3,0) = 0.0f; m(3,1) = 0.0f; m(3,2) = -1.0f; m(3,3) = 0.0; 99 | return m; 100 | } 101 | 102 | template 103 | struct Arrayf{ 104 | Arrayf(){ 105 | for(int i=0;i& in){ 110 | fromEigen(in); 111 | } 112 | void fromEigen(const Eigen::Matrix& in){ 113 | for(int i=0;i toEigen(){ 118 | Eigen::Matrix out; 119 | for(int i=0;i pos_; 142 | Arrayf<3> normal_; 143 | Arrayf<4> color_; 144 | }; 145 | 146 | class SceneObject{ 147 | public: 148 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 149 | SceneObject(); 150 | virtual ~SceneObject(); 151 | void initTexture(const int& cols, const int& rows); 152 | void setTexture(const int& cols, const int& rows, const float* ptr); 153 | void setTexture(const cv::Mat& img); 154 | void allocateBuffer(); 155 | void makeCubeFull(const float& l); 156 | void makeTetrahedron(const float& l); 157 | void makeCubeMesh(const float& l); 158 | void makeLine(); 159 | void makeLine(const std::vector& line); 160 | void makePoints(); 161 | void makePoints(const std::vector& points); 162 | void prolonge(const Eigen::Vector3f& point); 163 | void prolonge(const std::vector& points); 164 | void clear(); 165 | void makeCoordinateFrame(const float& l); 166 | void makeRectangle(const float& x,const float& y); 167 | void makeTexturedRectangle(const float& x,const float& y); 168 | void makeGroundPlane(const float& l,const int& N,const Eigen::Vector4f& color1,const Eigen::Vector4f& color2); 169 | void makeGroundPlaneMesh(const float& l,const int& N); 170 | void setColorFull(const Eigen::Vector4f& color); 171 | const Eigen::Matrix4f GetWorldTrans(); 172 | void CalcNormals(); 173 | void loadOptions(); 174 | Eigen::Vector3f W_r_WB_; 175 | kindr::RotationQuaternionPF q_BW_; 176 | std::vector vertices_; 177 | std::vector indices_; 178 | GLuint VBO_; 179 | GLuint IBO_; 180 | GLuint textureID_; 181 | GLenum mode_; 182 | bool mbCullFace_; 183 | float lineWidth_; 184 | float pointSize_; 185 | bool useTexture_; 186 | bool draw_; 187 | }; 188 | 189 | class Scene{ 190 | public: 191 | std::map> keyboardCallbacks_; 192 | std::map> specialKeyboardCallbacks_; 193 | Scene(); 194 | virtual ~Scene(); 195 | int init(int argc, char** argv, const std::string& mVSFileName,const std::string& mFSFileName); 196 | std::shared_ptr addSceneObject(); 197 | void makeTestScene(); 198 | void RenderSceneCB(); 199 | void setView(const Eigen::Vector3f& pos, const Eigen::Vector3f& target); 200 | void setYDown(float step = 0.1); 201 | void SpecialKeyboardCB(int Key, int x, int y); 202 | void addSpecialKeyboardCB(int Key, std::function f); 203 | void KeyboardCB(unsigned char Key, int x, int y); 204 | void addKeyboardCB(unsigned char Key, std::function f); 205 | void MotionCB(int x, int y); 206 | void MouseCB(int button, int state, int x, int y); 207 | void AddShader(GLuint ShaderProgram, const char* pShaderText, GLenum ShaderType); 208 | void CompileShaders(const std::string& mVSFileName,const std::string& mFSFileName); 209 | void setIdleFunction(void (*idleFunc)()); 210 | void (*mIdleFunc)(); 211 | private: 212 | float stepScale_; 213 | 214 | std::vector> mSceneObjects_; 215 | 216 | GLuint V_TF_B_location_; 217 | GLuint W_TF_B_location_; 218 | GLuint lightColor_location_; 219 | GLuint lightAmbientIntensity_location_; 220 | GLuint lightDiffuseIntensity_location_; 221 | GLuint lightDirection_location_; 222 | GLuint useTexture_location_; 223 | GLuint sampler_location_; 224 | 225 | PersProjInfo mPersProjInfo_; 226 | DirectionalLight mDirectionalLight_; 227 | 228 | Eigen::Vector3f W_r_WC_; 229 | kindr::RotationQuaternionPF q_CW_; 230 | 231 | Eigen::Matrix4f C_TF_W_; 232 | Eigen::Matrix4f V_TF_C_; 233 | Eigen::Matrix4f V_TF_B_; 234 | Eigen::Matrix4f W_TF_B_; 235 | 236 | Eigen::Vector2i mMousePos_; 237 | bool enableMouseMotion_; 238 | }; 239 | 240 | static Scene* mpScene = nullptr; 241 | 242 | static void initGlut(int argc, char** argv, Scene& scene){ 243 | glutInit(&argc, argv); 244 | glutInitDisplayMode(GLUT_DOUBLE|GLUT_RGBA); 245 | glutInitWindowSize(1600, 900); 246 | glutInitWindowPosition(100, 100); 247 | glutCreateWindow("Scene"); 248 | 249 | mpScene = &scene; 250 | glutDisplayFunc([](){mpScene->RenderSceneCB();}); 251 | glutIdleFunc([](){ 252 | if(mpScene->mIdleFunc != nullptr){ 253 | mpScene->mIdleFunc(); 254 | } 255 | mpScene->RenderSceneCB(); 256 | }); 257 | glutSpecialFunc([](int Key, int x, int y){mpScene->SpecialKeyboardCB(Key,x,y);}); 258 | glutMotionFunc([](int x, int y){mpScene->MotionCB(x,y);}); 259 | glutMouseFunc([](int button, int state, int x, int y){mpScene->MouseCB(button,state,x,y);}); 260 | glutKeyboardFunc([](unsigned char Key, int x, int y){mpScene->KeyboardCB(Key,x,y);}); 261 | } 262 | 263 | } 264 | 265 | 266 | #endif /* ROVIO_SCENE_HPP_ */ 267 | -------------------------------------------------------------------------------- /include/rovio/VelocityUpdate.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_VELOCITYUPDATE_HPP_ 30 | #define ROVIO_VELOCITYUPDATE_HPP_ 31 | 32 | #include "lightweight_filtering/common.hpp" 33 | #include "lightweight_filtering/Update.hpp" 34 | #include "lightweight_filtering/State.hpp" 35 | #include "rovio/FilterStates.hpp" 36 | 37 | namespace rovio { 38 | 39 | /** \brief Class, defining the innovation. 40 | */ 41 | class VelocityInnovation: public LWF::State>{ 42 | public: 43 | typedef LWF::State> Base; 44 | using Base::E_; 45 | static constexpr unsigned int _vel = 0; 46 | VelocityInnovation(){ 47 | static_assert(_vel+1==E_,"Error with indices"); 48 | this->template getName<_vel>() = "vel"; 49 | }; 50 | virtual ~VelocityInnovation(){}; 51 | inline V3D& vel(){ 52 | return this->template get<_vel>(); 53 | } 54 | inline const V3D& vel() const{ 55 | return this->template get<_vel>(); 56 | } 57 | }; 58 | 59 | /** \brief Class, dummy auxillary class for Zero velocity update 60 | */ 61 | class VelocityUpdateMeasAuxiliary: public LWF::AuxiliaryBase{ 62 | public: 63 | VelocityUpdateMeasAuxiliary(){ 64 | }; 65 | virtual ~VelocityUpdateMeasAuxiliary(){}; 66 | }; 67 | 68 | /** \brief Empty measurement 69 | */ 70 | class VelocityUpdateMeas: public LWF::State,VelocityUpdateMeasAuxiliary>{ 71 | public: 72 | typedef LWF::State,VelocityUpdateMeasAuxiliary> Base; 73 | using Base::E_; 74 | static constexpr unsigned int _vel = 0; 75 | static constexpr unsigned int _aux = _vel+1; 76 | VelocityUpdateMeas(){ 77 | static_assert(_aux+1==E_,"Error with indices"); 78 | this->template getName<_vel>() = "vel"; 79 | this->template getName<_aux>() = "aux"; 80 | }; 81 | virtual ~VelocityUpdateMeas(){}; 82 | inline V3D& vel(){ 83 | return this->template get<_vel>(); 84 | } 85 | inline const V3D& vel() const{ 86 | return this->template get<_vel>(); 87 | } 88 | }; 89 | 90 | /** \brief Class holding the update noise. 91 | */ 92 | class VelocityUpdateNoise: public LWF::State>{ 93 | public: 94 | typedef LWF::State> Base; 95 | using Base::E_; 96 | static constexpr unsigned int _vel = 0; 97 | VelocityUpdateNoise(){ 98 | static_assert(_vel+1==E_,"Error with indices"); 99 | this->template getName<_vel>() = "vel"; 100 | }; 101 | virtual ~VelocityUpdateNoise(){}; 102 | inline V3D& vel(){ 103 | return this->template get<_vel>(); 104 | } 105 | inline const V3D& vel() const{ 106 | return this->template get<_vel>(); 107 | } 108 | }; 109 | 110 | /** \brief Outlier Detection. 111 | * ODEntry 112 | */ 113 | class VelocityOutlierDetection: public LWF::OutlierDetection(),3>>{ 114 | public: 115 | virtual ~VelocityOutlierDetection(){}; 116 | }; 117 | 118 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 119 | 120 | /** \brief Class, holding the zero velocity update 121 | */ 122 | template 123 | class VelocityUpdate: public LWF::Update{ 125 | public: 126 | typedef LWF::Update Base; 128 | using Base::doubleRegister_; 129 | using Base::intRegister_; 130 | using Base::meas_; 131 | typedef typename Base::mtState mtState; 132 | typedef typename Base::mtFilterState mtFilterState; 133 | typedef typename Base::mtInnovation mtInnovation; 134 | typedef typename Base::mtMeas mtMeas; 135 | typedef typename Base::mtNoise mtNoise; 136 | typedef typename Base::mtOutlierDetection mtOutlierDetection; 137 | 138 | QPD qAM_; // Rotation between IMU (M) and coordinate frame where the velocity is expressed in (A) 139 | 140 | /** \brief Constructor. 141 | * 142 | * Loads and sets the needed parameters. 143 | */ 144 | VelocityUpdate(){ 145 | qAM_.setIdentity(); 146 | intRegister_.removeScalarByStr("maxNumIteration"); 147 | doubleRegister_.removeScalarByStr("alpha"); 148 | doubleRegister_.removeScalarByStr("beta"); 149 | doubleRegister_.removeScalarByStr("kappa"); 150 | doubleRegister_.removeScalarByStr("updateVecNormTermination"); 151 | doubleRegister_.registerQuaternion("qAM",qAM_); 152 | }; 153 | 154 | /** \brief Destructor 155 | */ 156 | virtual ~VelocityUpdate(){}; 157 | 158 | /** \brief Compute the inovvation term 159 | * 160 | * @param mtInnovation - Class, holding innovation data. 161 | * @param state - Filter %State. 162 | * @param meas - Not Used. 163 | * @param noise - Additive discrete Gaussian noise. 164 | * @param dt - Not used. 165 | */ 166 | void evalInnovation(mtInnovation& y, const mtState& state, const mtNoise& noise) const{ 167 | y.vel() = qAM_.rotate(state.MvM()) + meas_.vel() + noise.vel(); // Velocity of state has a minus sign 168 | } 169 | 170 | /** \brief Computes the Jacobian for the update step of the filter. 171 | * 172 | * @param F - Jacobian for the update step of the filter. 173 | * @param state - Filter state. 174 | * @param meas - Not used. 175 | * @param dt - Not used. 176 | */ 177 | void jacState(MXD& F, const mtState& state) const{ 178 | F.setZero(); 179 | F.template block<3,3>(mtInnovation::template getId(),mtState::template getId()) = MPD(qAM_).matrix(); 180 | } 181 | 182 | /** \brief Computes the Jacobian for the update step of the filter w.r.t. to the noise variables 183 | * 184 | * @param G - Jacobian for the update step of the filter. 185 | * @param state - Filter state. 186 | * @param meas - Not used. 187 | * @param dt - Not used. 188 | */ 189 | void jacNoise(MXD& G, const mtState& state) const{ 190 | G.setZero(); 191 | G.template block<3,3>(mtInnovation::template getId(),mtNoise::template getId()) = Eigen::Matrix3d::Identity(); 192 | } 193 | }; 194 | 195 | } 196 | 197 | 198 | #endif /* ROVIO_VELOCITYUPDATE_HPP_ */ 199 | -------------------------------------------------------------------------------- /include/rovio/ZeroVelocityUpdate.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_ZEROVELOCITYUPDATE_HPP_ 30 | #define ROVIO_ZEROVELOCITYUPDATE_HPP_ 31 | 32 | #include "lightweight_filtering/common.hpp" 33 | #include "lightweight_filtering/Update.hpp" 34 | #include "lightweight_filtering/State.hpp" 35 | #include "rovio/FilterStates.hpp" 36 | 37 | namespace rovio { 38 | 39 | /** \brief Class, defining the innovation. 40 | */ 41 | template 42 | class ZeroVelocityInnovation: public LWF::State>{ 43 | public: 44 | typedef LWF::State> Base; 45 | using Base::E_; 46 | static constexpr unsigned int _vel = 0; 47 | ZeroVelocityInnovation(){ 48 | static_assert(_vel+1==E_,"Error with indices"); 49 | this->template getName<_vel>() = "vel"; 50 | }; 51 | virtual ~ZeroVelocityInnovation(){}; 52 | }; 53 | 54 | /** \brief Class, dummy auxillary class for Zero velocity update 55 | */ 56 | class ZeroVelocityUpdateMeasAuxiliary: public LWF::AuxiliaryBase{ 57 | public: 58 | ZeroVelocityUpdateMeasAuxiliary(){ 59 | }; 60 | virtual ~ZeroVelocityUpdateMeasAuxiliary(){}; 61 | }; 62 | 63 | /** \brief Empty measurement 64 | */ 65 | template 66 | class ZeroVelocityUpdateMeas: public LWF::State{ 67 | public: 68 | typedef LWF::State Base; 69 | using Base::E_; 70 | static constexpr unsigned int _aux = 0; 71 | ZeroVelocityUpdateMeas(){ 72 | static_assert(_aux+1==E_,"Error with indices"); 73 | this->template getName<_aux>() = "aux"; 74 | }; 75 | virtual ~ZeroVelocityUpdateMeas(){}; 76 | }; 77 | 78 | /** \brief Class holding the update noise. 79 | */ 80 | template 81 | class ZeroVelocityUpdateNoise: public LWF::State>{ 82 | public: 83 | typedef LWF::State> Base; 84 | using Base::E_; 85 | static constexpr unsigned int _vel = 0; 86 | ZeroVelocityUpdateNoise(){ 87 | static_assert(_vel+1==E_,"Error with indices"); 88 | this->template getName<_vel>() = "vel"; 89 | }; 90 | virtual ~ZeroVelocityUpdateNoise(){}; 91 | }; 92 | 93 | /** \brief Outlier Detection. 94 | */ 95 | template 96 | class ZeroVelocityOutlierDetection: public LWF::OutlierDetection::template getId::_vel>(),3>>{ 97 | public: 98 | virtual ~ZeroVelocityOutlierDetection(){}; 99 | }; 100 | 101 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 102 | 103 | /** \brief Class, holding the zero velocity update 104 | */ 105 | template 106 | class ZeroVelocityUpdate: public LWF::Update,FILTERSTATE,ZeroVelocityUpdateMeas, 107 | ZeroVelocityUpdateNoise,ZeroVelocityOutlierDetection,false>{ 108 | public: 109 | typedef LWF::Update,FILTERSTATE,ZeroVelocityUpdateMeas, 110 | ZeroVelocityUpdateNoise,ZeroVelocityOutlierDetection,false> Base; 111 | using Base::doubleRegister_; 112 | using Base::intRegister_; 113 | typedef typename Base::mtState mtState; 114 | typedef typename Base::mtFilterState mtFilterState; 115 | typedef typename Base::mtInnovation mtInnovation; 116 | typedef typename Base::mtMeas mtMeas; 117 | typedef typename Base::mtNoise mtNoise; 118 | typedef typename Base::mtOutlierDetection mtOutlierDetection; 119 | 120 | /** \brief Constructor. 121 | * 122 | * Loads and sets the needed parameters. 123 | */ 124 | ZeroVelocityUpdate(){ 125 | intRegister_.removeScalarByStr("maxNumIteration"); 126 | doubleRegister_.removeScalarByStr("alpha"); 127 | doubleRegister_.removeScalarByStr("beta"); 128 | doubleRegister_.removeScalarByStr("kappa"); 129 | doubleRegister_.removeScalarByStr("updateVecNormTermination"); 130 | }; 131 | 132 | /** \brief Destructor 133 | */ 134 | virtual ~ZeroVelocityUpdate(){}; 135 | 136 | /** \brief Compute the inovvation term 137 | * 138 | * @param mtInnovation - Class, holding innovation data. 139 | * @param state - Filter %State. 140 | * @param meas - Not Used. 141 | * @param noise - Additive discrete Gaussian noise. 142 | * @param dt - Not used. 143 | */ 144 | void evalInnovation(mtInnovation& y, const mtState& state, const mtNoise& noise) const{ 145 | y.template get() = state.MvM()+noise.template get(); 146 | } 147 | 148 | /** \brief Computes the Jacobian for the update step of the filter. 149 | * 150 | * @param F - Jacobian for the update step of the filter. 151 | * @param state - Filter state. 152 | * @param meas - Not used. 153 | * @param dt - Not used. 154 | */ 155 | void jacState(MXD& F, const mtState& state) const{ 156 | F.setZero(); 157 | F.template block<3,3>(mtInnovation::template getId(),mtState::template getId()) = Eigen::Matrix3d::Identity(); 158 | } 159 | 160 | /** \brief Computes the Jacobian for the update step of the filter w.r.t. to the noise variables 161 | * 162 | * @param G - Jacobian for the update step of the filter. 163 | * @param state - Filter state. 164 | * @param meas - Not used. 165 | * @param dt - Not used. 166 | */ 167 | void jacNoise(MXD& G, const mtState& state) const{ 168 | G.setZero(); 169 | G.template block<3,3>(mtInnovation::template getId(),mtNoise::template getId()) = Eigen::Matrix3d::Identity(); 170 | } 171 | }; 172 | 173 | } 174 | 175 | 176 | #endif /* ROVIO_ZEROVELOCITYUPDATE_HPP_ */ 177 | -------------------------------------------------------------------------------- /include/rovio/exceptions.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_EXCEPTION_HPP_ 30 | #define ROVIO_EXCEPTION_HPP_ 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #define ROVIO_THROW(exceptionType) { \ 38 | throw exceptionType(__FUNCTION__,__FILE__,__LINE__); \ 39 | } 40 | 41 | namespace rovio { 42 | struct ExceptionBase : public std::exception 43 | { 44 | std::string message_; 45 | std::string function_; 46 | std::string file_; 47 | int line_; 48 | ExceptionBase(std::string message,std::string function, std::string file, int line){ 49 | message_ = message; 50 | function_ = function; 51 | file_ = file; 52 | line_ = line; 53 | } 54 | virtual ~ExceptionBase(){}; 55 | virtual const char * what () const throw () 56 | { 57 | return (file_ + ":" + std::to_string(line_) + ": " + function_ + "()" + " " + message_).c_str(); 58 | } 59 | }; 60 | 61 | struct CameraNullPtrException : public ExceptionBase 62 | { 63 | CameraNullPtrException(std::string function, std::string file, int line): ExceptionBase("Camera pointer is null!",function,file,line){} 64 | }; 65 | } 66 | 67 | /* Usage: 68 | * ROVIO_THROW(rovio::CameraNullPtrException); 69 | */ 70 | 71 | #endif /* ROVIO_EXCEPTION_HPP_ */ 72 | -------------------------------------------------------------------------------- /launch/rovio_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /launch/rovio_rosbag_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/valgrind_rovio.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /mainpage.dox: -------------------------------------------------------------------------------- 1 | /*! \mainpage Documentation 2 | * 3 | * \section intro_sec Introduction 4 | * ROVIO is a visual-inertial odometry framework which, by directly using pixel 5 | * intensity errors of image patches, achieves accurate tracking 6 | * performance while exhibiting a very high level of robustness. 7 | * 8 | * After detection, the tracking of the multilevel patch features is 9 | * closely coupled to the underlying extended Kalman filter (EKF) 10 | * by directly using the intensity errors as innovation term during 11 | * the update step. A purely robocentric approach is followed, 12 | * where the location of 3D landmarks are always estimated 13 | * with respect to the current camera pose. Furthermore, the landmark 14 | * positions are decomposed into a bearing vector and 15 | * a distance parametrization whereby a minimal 16 | * representation of differences on a corresponding σ-Algebra is employed 17 | * in order to achieve better consistency and to improve the 18 | * computational performance. 19 | * 20 | * Due to the robocentric, inverse- 21 | * distance (selectable) landmark parametrization, the framework does not 22 | * require any initialization procedure, leading to a truly power- 23 | * up-and-go state estimation system. 24 | * 25 | * 26 | * 27 | */ 28 | 29 | \cond ----------- Document the the rovio namespace. ------------ \endcond 30 | /*! \namespace rovio */ -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rovio 4 | 0.0.0 5 | 6 | 7 | rovio 8 | 9 | 10 | 11 | bloeschm 12 | bloeschm 13 | BSD License 14 | catkin 15 | 16 | cmake_modules 17 | lightweight_filtering 18 | kindr 19 | roscpp 20 | roslib 21 | cv_bridge 22 | message_generation 23 | message_runtime 24 | geometry_msgs 25 | sensor_msgs 26 | std_msgs 27 | nav_msgs 28 | tf 29 | rosbag 30 | yaml_cpp_catkin 31 | 32 | -------------------------------------------------------------------------------- /shaders/shader.fs: -------------------------------------------------------------------------------- 1 | #version 300 es 2 | 3 | precision highp float; 4 | 5 | in vec4 Color0; 6 | in vec3 Normal0; 7 | in vec2 TexCoord0; 8 | 9 | out vec4 FragColor; 10 | 11 | struct DirectionalLight 12 | { 13 | vec3 Color; 14 | float AmbientIntensity; 15 | float DiffuseIntensity; 16 | vec3 Direction; 17 | }; 18 | 19 | uniform DirectionalLight gDirectionalLight; 20 | uniform sampler2D gSampler; 21 | uniform bool useTexture; 22 | 23 | void main() 24 | { 25 | vec4 AmbientColor = vec4(gDirectionalLight.Color, 1.0f) * gDirectionalLight.AmbientIntensity; 26 | 27 | float DiffuseFactor = dot(normalize(Normal0), -gDirectionalLight.Direction); 28 | 29 | vec4 DiffuseColor; 30 | 31 | if (DiffuseFactor > 0.0f) { 32 | DiffuseColor = vec4(gDirectionalLight.Color, 1.0f) * gDirectionalLight.DiffuseIntensity * DiffuseFactor; 33 | } 34 | else { 35 | DiffuseColor = vec4(0.0f, 0.0f, 0.0f, 0.0f); 36 | } 37 | 38 | if(!useTexture){ 39 | FragColor = Color0 * (AmbientColor + DiffuseColor); 40 | } 41 | else { 42 | FragColor = texture2D(gSampler, TexCoord0.st) * (AmbientColor + DiffuseColor); 43 | } 44 | } 45 | -------------------------------------------------------------------------------- /shaders/shader.vs: -------------------------------------------------------------------------------- 1 | #version 300 es 2 | 3 | precision highp float; 4 | 5 | layout (location = 0) in vec3 Position; 6 | layout (location = 1) in vec3 Normal; 7 | layout (location = 2) in vec4 Color; 8 | 9 | uniform mat4 V_TF_B; 10 | uniform mat4 W_TF_B; 11 | 12 | out vec4 Color0; 13 | out vec3 Normal0; 14 | out vec2 TexCoord0; 15 | 16 | void main() 17 | { 18 | gl_Position = V_TF_B * vec4(Position, 1.0f); 19 | Color0 = Color; 20 | Normal0 = (W_TF_B * vec4(Normal, 0.0)).xyz; 21 | TexCoord0 = Color.xy; 22 | } 23 | -------------------------------------------------------------------------------- /src/FeatureCoordinates.cpp: -------------------------------------------------------------------------------- 1 | #include "rovio/FeatureCoordinates.hpp" 2 | 3 | namespace rovio{ 4 | 5 | FeatureCoordinates::FeatureCoordinates(){ 6 | mpCamera_ = nullptr; 7 | trackWarping_ = false; 8 | resetCoordinates(); 9 | } 10 | 11 | FeatureCoordinates::FeatureCoordinates(const cv::Point2f& pixel){ 12 | mpCamera_ = nullptr; 13 | trackWarping_ = false; 14 | resetCoordinates(); 15 | c_ = pixel; 16 | valid_c_ = true; 17 | } 18 | 19 | FeatureCoordinates::FeatureCoordinates(const LWF::NormalVectorElement& nor){ 20 | mpCamera_ = nullptr; 21 | trackWarping_ = false; 22 | resetCoordinates(); 23 | nor_ = nor; 24 | valid_nor_ = true; 25 | } 26 | 27 | FeatureCoordinates::FeatureCoordinates(const Camera* mpCamera): mpCamera_(mpCamera){ 28 | trackWarping_ = false; 29 | resetCoordinates(); 30 | } 31 | 32 | FeatureCoordinates::~FeatureCoordinates(){}; 33 | 34 | void FeatureCoordinates::resetCoordinates(){ 35 | valid_c_ = false; 36 | valid_nor_ = false; 37 | set_warp_identity(); 38 | camID_ = -1; 39 | } 40 | 41 | bool FeatureCoordinates::com_c() const{ 42 | if(!valid_c_){ 43 | assert(mpCamera_ != nullptr); 44 | if(valid_nor_ && mpCamera_->bearingToPixel(nor_,c_)){ 45 | valid_c_ = true; 46 | } 47 | } 48 | return valid_c_; 49 | } 50 | 51 | const cv::Point2f& FeatureCoordinates::get_c() const{ 52 | if(!com_c()){ 53 | std::cout << " \033[31mERROR: No valid coordinate data!\033[0m" << std::endl; 54 | } 55 | return c_; 56 | } 57 | 58 | bool FeatureCoordinates::com_nor() const{ 59 | if(!valid_nor_){ 60 | assert(mpCamera_ != nullptr); 61 | if(valid_c_ && mpCamera_->pixelToBearing(c_,nor_)){ 62 | valid_nor_ = true; 63 | } 64 | } 65 | return valid_nor_; 66 | } 67 | 68 | const LWF::NormalVectorElement& FeatureCoordinates::get_nor() const{ 69 | if(!com_nor()){ 70 | std::cout << " \033[31mERROR: No valid coordinate data!\033[0m" << std::endl; 71 | } 72 | return nor_; 73 | } 74 | 75 | Eigen::Matrix FeatureCoordinates::get_J() const{ 76 | assert(mpCamera_ != nullptr); 77 | if(!mpCamera_->bearingToPixel(get_nor(),c_,matrix2dTemp_)){ 78 | matrix2dTemp_.setZero(); 79 | std::cout << " \033[31mERROR: No valid coordinate data!\033[0m" << std::endl; 80 | } 81 | return matrix2dTemp_; 82 | } 83 | 84 | void FeatureCoordinates::set_c(const cv::Point2f& c, const bool resetWarp){ 85 | c_ = c; 86 | valid_c_ = true; 87 | valid_nor_ = false; 88 | if(trackWarping_ && resetWarp){ 89 | valid_warp_c_ = false; 90 | valid_warp_nor_ = false; 91 | } 92 | } 93 | 94 | void FeatureCoordinates::set_nor(const LWF::NormalVectorElement& nor, const bool resetWarp){ 95 | nor_ = nor; 96 | valid_nor_ = true; 97 | valid_c_ = false; 98 | if(trackWarping_ && resetWarp){ 99 | valid_warp_c_ = false; 100 | valid_warp_nor_ = false; 101 | } 102 | } 103 | 104 | bool FeatureCoordinates::com_warp_c() const{ 105 | if(!valid_warp_c_){ 106 | if(valid_warp_nor_ && com_c() && com_nor()){ 107 | matrix2dTemp_ = get_J(); 108 | warp_c_ = (matrix2dTemp_*warp_nor_).cast(); 109 | valid_warp_c_ = true; 110 | } 111 | } 112 | return valid_warp_c_; 113 | } 114 | 115 | Eigen::Matrix2f& FeatureCoordinates::get_warp_c() const{ 116 | if(!com_warp_c()){ 117 | std::cout << " \033[31mERROR: No valid warping data in get_warp_c!\033[0m" << std::endl; 118 | } 119 | return warp_c_; 120 | } 121 | 122 | bool FeatureCoordinates::com_warp_nor() const{ 123 | if(!valid_warp_nor_){ 124 | if(valid_warp_c_ && com_c() && com_nor()){ 125 | matrix2dTemp_ = get_J(); 126 | fullPivLU2d_.compute(matrix2dTemp_); 127 | if(fullPivLU2d_.rank() == 2){ 128 | warp_nor_ = fullPivLU2d_.inverse()*warp_c_.cast(); 129 | valid_warp_nor_ = true; 130 | } 131 | } 132 | } 133 | return valid_warp_nor_; 134 | } 135 | 136 | Eigen::Matrix2d& FeatureCoordinates::get_warp_nor() const{ 137 | if(!com_warp_nor()){ 138 | std::cout << " \033[31mERROR: No valid warping data in get_warp_nor!\033[0m" << std::endl; 139 | } 140 | return warp_nor_; 141 | } 142 | 143 | FeatureCoordinates FeatureCoordinates::get_patchCorner(const double x, const double y) const{ 144 | FeatureCoordinates temp; // TODO: avoid temp 145 | get_nor().boxPlus(get_warp_nor()*Eigen::Vector2d(x,y),norTemp_); 146 | temp.set_nor(norTemp_); 147 | temp.mpCamera_ = mpCamera_; 148 | temp.camID_ = camID_; 149 | return temp; 150 | } 151 | 152 | void FeatureCoordinates::set_warp_c(const Eigen::Matrix2f& warp_c){ 153 | warp_c_ = warp_c; 154 | valid_warp_c_ = true; 155 | valid_warp_nor_ = false; 156 | isWarpIdentity_ = false; 157 | } 158 | 159 | void FeatureCoordinates::set_warp_nor(const Eigen::Matrix2d& warp_nor){ 160 | warp_nor_ = warp_nor; 161 | valid_warp_nor_ = true; 162 | valid_warp_c_ = false; 163 | isWarpIdentity_ = false; 164 | } 165 | 166 | void FeatureCoordinates::set_warp_identity(){ 167 | warp_c_.setIdentity(); 168 | valid_warp_c_ = true; 169 | valid_warp_nor_ = false; 170 | isWarpIdentity_ = true; 171 | } 172 | 173 | bool FeatureCoordinates::isInFront() const{ 174 | return valid_c_ || (valid_nor_ && nor_.getVec()[2] > 0); 175 | } 176 | 177 | bool FeatureCoordinates::isNearIdentityWarping() const{ 178 | return isWarpIdentity_ || (com_warp_c() && (get_warp_c()-Eigen::Matrix2f::Identity()).norm() < 1e-6); 179 | } 180 | 181 | void FeatureCoordinates::setPixelCov(const Eigen::Matrix2d& cov){ 182 | pixelCov_ = cov; 183 | es_.compute(cov); 184 | sigmaAngle_ = std::atan2(es_.eigenvectors()(1,0).real(),es_.eigenvectors()(0,0).real()); 185 | sigma1_ = sqrt(es_.eigenvalues()(0).real()); 186 | sigma2_ = sqrt(es_.eigenvalues()(1).real()); 187 | if(sigma1_(scaleFactor*sigma1_+0.5),1),std::max(static_cast(scaleFactor*sigma2_+0.5),1)),sigmaAngle_*180/M_PI,0,360,color,1,8,0); 208 | } 209 | 210 | void FeatureCoordinates::drawLine(cv::Mat& drawImg, const FeatureCoordinates& other, const cv::Scalar& color, int thickness) const{ 211 | cv::line(drawImg,get_c(),other.get_c(),color,thickness); 212 | } 213 | 214 | void FeatureCoordinates::drawText(cv::Mat& drawImg, const std::string& s, const cv::Scalar& color) const{ 215 | cv::putText(drawImg,s,get_c(),cv::FONT_HERSHEY_SIMPLEX, 0.4, color); 216 | } 217 | 218 | bool FeatureCoordinates::getDepthFromTriangulation(const FeatureCoordinates& other, const V3D& C2rC2C1, const QPD& qC2C1, FeatureDistance& d, const double minDistance){ 219 | const V3D C2v1 = qC2C1.rotate(get_nor().getVec()); 220 | const V3D C2v2 = other.get_nor().getVec(); 221 | const double a = 1.0-pow(C2v1.dot(C2v2),2.0); 222 | if(a < 1e-6){ 223 | return false; 224 | } 225 | 226 | const double distance = -C2v1.dot((M3D::Identity()-C2v2*C2v2.transpose())*C2rC2C1) / a; 227 | if(distance < minDistance){ 228 | return false; 229 | } 230 | d.setParameter(distance); 231 | return true; 232 | 233 | // Possible alternative -> investigate 234 | // Eigen::Matrix V; 235 | // V.col(0) = -C2v1; 236 | // V.col(1) = C2v2; 237 | // Eigen::JacobiSVD> svd(V, Eigen::ComputeThinU | Eigen::ComputeThinV); 238 | // Eigen::Vector2d distances = svd.solve(C2rC2C1); 239 | } 240 | 241 | float FeatureCoordinates::getDepthUncertaintyTau(const V3D& C1rC1C2, const float d, const float px_error_angle){ 242 | const V3D C1fP = get_nor().getVec(); 243 | float t_0 = C1rC1C2(0); 244 | float t_1 = C1rC1C2(1); 245 | float t_2 = C1rC1C2(2); 246 | float a_0 = C1fP(0) * d - t_0; 247 | float a_1 = C1fP(1) * d - t_1; 248 | float a_2 = C1fP(2) * d - t_2; 249 | float t_norm = std::sqrt(t_0 * t_0 + t_1 * t_1 + t_2 * t_2); 250 | float a_norm = std::sqrt(a_0 * a_0 + a_1 * a_1 + a_2 * a_2); 251 | float alpha = std::acos((C1fP(0) * t_0 + C1fP(1) * t_1 + C1fP(2) * t_2) / t_norm); 252 | float beta = std::acos(( a_0 * (-t_0) + a_1 * (-t_1) + a_2 * (-t_2) ) / (t_norm * a_norm)); 253 | float beta_plus = beta + px_error_angle; 254 | float gamma_plus = M_PI - alpha - beta_plus; // Triangle angles sum to PI. 255 | float d_plus = t_norm * std::sin(beta_plus) / std::sin(gamma_plus); // Law of sines. 256 | return (d_plus - d); // Tau. 257 | } 258 | } 259 | -------------------------------------------------------------------------------- /src/FeatureDistance.cpp: -------------------------------------------------------------------------------- 1 | #include "rovio/FeatureDistance.hpp" 2 | #include 3 | #include 4 | 5 | namespace rovio { 6 | 7 | FeatureDistance::FeatureDistance(const Type& type){ 8 | setType(type); 9 | p_ = 1.0; 10 | } 11 | 12 | FeatureDistance::~FeatureDistance(){}; 13 | 14 | void FeatureDistance::setType(const Type& type){ 15 | type_ = type; 16 | } 17 | 18 | FeatureDistance::Type FeatureDistance::getType(){ 19 | return type_; 20 | } 21 | 22 | void FeatureDistance::setType(const int& type){ 23 | switch(type){ 24 | case 0: 25 | type_ = REGULAR; 26 | break; 27 | case 1: 28 | type_ = INVERSE; 29 | break; 30 | case 2: 31 | type_ = LOG; 32 | break; 33 | case 3: 34 | type_ = HYPERBOLIC; 35 | break; 36 | default: 37 | std::cout << "Invalid type for distance parameterization: " << type << std::endl; 38 | type_ = REGULAR; 39 | break; 40 | } 41 | } 42 | 43 | void FeatureDistance::setParameter(const double& d){ 44 | switch(type_){ 45 | case REGULAR: 46 | setParameterRegular(d); 47 | break; 48 | case INVERSE: 49 | setParameterInverse(d); 50 | break; 51 | case LOG: 52 | setParameterLog(d); 53 | break; 54 | case HYPERBOLIC: 55 | setParameterHyperbolic(d); 56 | break; 57 | default: 58 | setParameterRegular(d); 59 | break; 60 | } 61 | } 62 | 63 | double FeatureDistance::getDistance() const{ 64 | switch(type_){ 65 | case REGULAR: 66 | return getDistanceRegular(); 67 | case INVERSE: 68 | return getDistanceInverse(); 69 | case LOG: 70 | return getDistanceLog(); 71 | case HYPERBOLIC: 72 | return getDistanceHyperbolic(); 73 | default: 74 | return getDistanceRegular(); 75 | } 76 | } 77 | 78 | double FeatureDistance::getDistanceDerivative() const{ 79 | switch(type_){ 80 | case REGULAR: 81 | return getDistanceDerivativeRegular(); 82 | case INVERSE: 83 | return getDistanceDerivativeInverse(); 84 | case LOG: 85 | return getDistanceDerivativeLog(); 86 | case HYPERBOLIC: 87 | return getDistanceDerivativeHyperbolic(); 88 | default: 89 | return getDistanceDerivativeRegular(); 90 | } 91 | } 92 | 93 | double FeatureDistance::getParameterDerivative() const{ 94 | switch(type_){ 95 | case REGULAR: 96 | return getParameterDerivativeRegular(); 97 | case INVERSE: 98 | return getParameterDerivativeInverse(); 99 | case LOG: 100 | return getParameterDerivativeLog(); 101 | case HYPERBOLIC: 102 | return getParameterDerivativeHyperbolic(); 103 | default: 104 | return getParameterDerivativeRegular(); 105 | } 106 | } 107 | 108 | double FeatureDistance::getParameterDerivativeCombined() const{ 109 | switch(type_){ 110 | case REGULAR: 111 | return getParameterDerivativeCombinedRegular(); 112 | case INVERSE: 113 | return getParameterDerivativeCombinedInverse(); 114 | case LOG: 115 | return getParameterDerivativeCombinedLog(); 116 | case HYPERBOLIC: 117 | return getParameterDerivativeCombinedHyperbolic(); 118 | default: 119 | return getParameterDerivativeCombinedRegular(); 120 | } 121 | } 122 | 123 | void FeatureDistance::getParameterDerivativeCombined(FeatureDistance other){ 124 | setParameter(other.getDistance()); 125 | } 126 | 127 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 128 | 129 | void FeatureDistance::setParameterRegular(const double& d){ 130 | p_ = d; 131 | } 132 | 133 | double FeatureDistance::getDistanceRegular() const{ 134 | return p_; 135 | } 136 | 137 | double FeatureDistance::getDistanceDerivativeRegular() const{ 138 | return 1.0; 139 | } 140 | 141 | double FeatureDistance::getParameterDerivativeRegular() const{ 142 | return 1.0; 143 | } 144 | 145 | double FeatureDistance::getParameterDerivativeCombinedRegular() const{ 146 | return 0.0; 147 | } 148 | 149 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 150 | 151 | double FeatureDistance::makeNonZero(const double& p) const{ 152 | if(p < 1e-6){ 153 | if(p >= 0){ 154 | return 1e-6; 155 | } else if (p > -1e-6){ 156 | return -1e-6; 157 | } 158 | } 159 | return p; 160 | } 161 | 162 | void FeatureDistance::setParameterInverse(const double& d){ 163 | p_ = 1/makeNonZero(d); 164 | } 165 | 166 | double FeatureDistance::getDistanceInverse() const{ 167 | const double p_temp = makeNonZero(p_); 168 | return 1/p_temp; 169 | } 170 | 171 | double FeatureDistance::getDistanceDerivativeInverse() const{ 172 | const double p_temp = makeNonZero(p_); 173 | return -1.0/(p_temp*p_temp); 174 | } 175 | 176 | double FeatureDistance::getParameterDerivativeInverse() const{ 177 | const double p_temp = makeNonZero(p_); 178 | return -p_temp*p_temp; 179 | } 180 | 181 | double FeatureDistance::getParameterDerivativeCombinedInverse() const{ 182 | const double p_temp = makeNonZero(p_); 183 | return -2*p_temp; 184 | } 185 | 186 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 187 | 188 | void FeatureDistance::setParameterLog(const double& d){ 189 | p_ = std::log(d); 190 | } 191 | 192 | double FeatureDistance::getDistanceLog() const{ 193 | return std::exp(p_); 194 | } 195 | 196 | double FeatureDistance::getDistanceDerivativeLog() const{ 197 | return std::exp(p_); 198 | } 199 | 200 | double FeatureDistance::getParameterDerivativeLog() const{ 201 | return std::exp(-p_); 202 | } 203 | 204 | double FeatureDistance::getParameterDerivativeCombinedLog() const{ 205 | return -std::exp(-p_); 206 | } 207 | 208 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 209 | 210 | void FeatureDistance::setParameterHyperbolic(const double& d){ 211 | p_ = std::asinh(d); 212 | } 213 | 214 | double FeatureDistance::getDistanceHyperbolic() const{ 215 | return std::sinh(p_); 216 | } 217 | 218 | double FeatureDistance::getDistanceDerivativeHyperbolic() const{ 219 | return std::cosh(p_); 220 | } 221 | 222 | double FeatureDistance::getParameterDerivativeHyperbolic() const{ 223 | return 1/std::sqrt(std::pow(std::sinh(p_),2)+1); // p = asinh(d) 224 | } 225 | 226 | double FeatureDistance::getParameterDerivativeCombinedHyperbolic() const{ 227 | return -std::sinh(p_)/std::pow(std::pow(std::sinh(p_),2)+1,1.5)*std::cosh(p_); 228 | } 229 | } 230 | -------------------------------------------------------------------------------- /src/feature_tracker_node.cpp: -------------------------------------------------------------------------------- 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 | #include "../include/rovio/featureTracker.hpp" 30 | 31 | int main(int argc, char** argv) { 32 | ros::init(argc, argv, "FeatureTrackerNode"); 33 | ros::NodeHandle nh; 34 | rovio::FeatureTrackerNode featureTrackerNode(nh); 35 | ros::spin(); 36 | return 0; 37 | } 38 | -------------------------------------------------------------------------------- /src/rovio_node.cpp: -------------------------------------------------------------------------------- 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 | 30 | #include 31 | 32 | #include 33 | 34 | #pragma GCC diagnostic push 35 | #pragma GCC diagnostic ignored "-Wunused-parameter" 36 | #include 37 | #include 38 | #include 39 | #pragma GCC diagnostic pop 40 | 41 | #include "rovio/RovioFilter.hpp" 42 | #include "rovio/RovioNode.hpp" 43 | #ifdef MAKE_SCENE 44 | #include "rovio/RovioScene.hpp" 45 | #endif 46 | 47 | #ifdef ROVIO_NMAXFEATURE 48 | static constexpr int nMax_ = ROVIO_NMAXFEATURE; 49 | #else 50 | static constexpr int nMax_ = 25; // Maximal number of considered features in the filter state. 51 | #endif 52 | 53 | #ifdef ROVIO_NLEVELS 54 | static constexpr int nLevels_ = ROVIO_NLEVELS; 55 | #else 56 | static constexpr int nLevels_ = 4; // // Total number of pyramid levels considered. 57 | #endif 58 | 59 | #ifdef ROVIO_PATCHSIZE 60 | static constexpr int patchSize_ = ROVIO_PATCHSIZE; 61 | #else 62 | static constexpr int patchSize_ = 6; // Edge length of the patches (in pixel). Must be a multiple of 2! 63 | #endif 64 | 65 | #ifdef ROVIO_NCAM 66 | static constexpr int nCam_ = ROVIO_NCAM; 67 | #else 68 | static constexpr int nCam_ = 1; // Used total number of cameras. 69 | #endif 70 | 71 | #ifdef ROVIO_NPOSE 72 | static constexpr int nPose_ = ROVIO_NPOSE; 73 | #else 74 | static constexpr int nPose_ = 0; // Additional pose states. 75 | #endif 76 | 77 | typedef rovio::RovioFilter> mtFilter; 78 | 79 | #ifdef MAKE_SCENE 80 | static rovio::RovioScene mRovioScene; 81 | 82 | void idleFunc(){ 83 | ros::spinOnce(); 84 | mRovioScene.drawScene(mRovioScene.mpFilter_->safe_); 85 | } 86 | #endif 87 | 88 | int main(int argc, char** argv){ 89 | ros::init(argc, argv, "rovio"); 90 | ros::NodeHandle nh; 91 | ros::NodeHandle nh_private("~"); 92 | 93 | std::string rootdir = ros::package::getPath("rovio"); // Leaks memory 94 | std::string filter_config = rootdir + "/cfg/rovio.info"; 95 | 96 | nh_private.param("filter_config", filter_config, filter_config); 97 | 98 | // Filter 99 | std::shared_ptr mpFilter(new mtFilter); 100 | mpFilter->readFromInfo(filter_config); 101 | 102 | // Force the camera calibration paths to the ones from ROS parameters. 103 | for (unsigned int camID = 0; camID < nCam_; ++camID) { 104 | std::string camera_config; 105 | if (nh_private.getParam("camera" + std::to_string(camID) 106 | + "_config", camera_config)) { 107 | mpFilter->cameraCalibrationFile_[camID] = camera_config; 108 | } 109 | } 110 | mpFilter->refreshProperties(); 111 | 112 | // Node 113 | rovio::RovioNode rovioNode(nh, nh_private, mpFilter); 114 | rovioNode.makeTest(); 115 | 116 | #ifdef MAKE_SCENE 117 | // Scene 118 | std::string mVSFileName = rootdir + "/shaders/shader.vs"; 119 | std::string mFSFileName = rootdir + "/shaders/shader.fs"; 120 | mRovioScene.initScene(argc,argv,mVSFileName,mFSFileName,mpFilter); 121 | mRovioScene.setIdleFunction(idleFunc); 122 | mRovioScene.addKeyboardCB('r',[&rovioNode]() mutable {rovioNode.requestReset();}); 123 | glutMainLoop(); 124 | #else 125 | ros::spin(); 126 | #endif 127 | return 0; 128 | } 129 | -------------------------------------------------------------------------------- /src/rovio_rosbag_loader.cpp: -------------------------------------------------------------------------------- 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 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include "rovio/RovioFilter.hpp" 38 | #include "rovio/RovioNode.hpp" 39 | #include 40 | #include 41 | #include 42 | #define foreach BOOST_FOREACH 43 | 44 | #ifdef ROVIO_NMAXFEATURE 45 | static constexpr int nMax_ = ROVIO_NMAXFEATURE; 46 | #else 47 | static constexpr int nMax_ = 25; // Maximal number of considered features in the filter state. 48 | #endif 49 | 50 | #ifdef ROVIO_NLEVELS 51 | static constexpr int nLevels_ = ROVIO_NLEVELS; 52 | #else 53 | static constexpr int nLevels_ = 4; // // Total number of pyramid levels considered. 54 | #endif 55 | 56 | #ifdef ROVIO_PATCHSIZE 57 | static constexpr int patchSize_ = ROVIO_PATCHSIZE; 58 | #else 59 | static constexpr int patchSize_ = 8; // Edge length of the patches (in pixel). Must be a multiple of 2! 60 | #endif 61 | 62 | #ifdef ROVIO_NCAM 63 | static constexpr int nCam_ = ROVIO_NCAM; 64 | #else 65 | static constexpr int nCam_ = 1; // Used total number of cameras. 66 | #endif 67 | 68 | #ifdef ROVIO_NPOSE 69 | static constexpr int nPose_ = ROVIO_NPOSE; 70 | #else 71 | static constexpr int nPose_ = 0; // Additional pose states. 72 | #endif 73 | 74 | typedef rovio::RovioFilter> mtFilter; 75 | 76 | int main(int argc, char** argv){ 77 | ros::init(argc, argv, "rovio"); 78 | ros::NodeHandle nh; 79 | ros::NodeHandle nh_private("~"); 80 | 81 | std::string rootdir = ros::package::getPath("rovio"); // Leaks memory 82 | std::string filter_config = rootdir + "/cfg/rovio.info"; 83 | 84 | nh_private.param("filter_config", filter_config, filter_config); 85 | 86 | // Filter 87 | std::shared_ptr mpFilter(new mtFilter); 88 | mpFilter->readFromInfo(filter_config); 89 | 90 | // Force the camera calibration paths to the ones from ROS parameters. 91 | for (unsigned int camID = 0; camID < nCam_; ++camID) { 92 | std::string camera_config; 93 | if (nh_private.getParam("camera" + std::to_string(camID) 94 | + "_config", camera_config)) { 95 | mpFilter->cameraCalibrationFile_[camID] = camera_config; 96 | } 97 | } 98 | mpFilter->refreshProperties(); 99 | 100 | // Node 101 | rovio::RovioNode rovioNode(nh, nh_private, mpFilter); 102 | rovioNode.makeTest(); 103 | double resetTrigger = 0.0; 104 | nh_private.param("record_odometry", rovioNode.forceOdometryPublishing_, rovioNode.forceOdometryPublishing_); 105 | nh_private.param("record_pose_with_covariance_stamped", rovioNode.forcePoseWithCovariancePublishing_, rovioNode.forcePoseWithCovariancePublishing_); 106 | nh_private.param("record_transform", rovioNode.forceTransformPublishing_, rovioNode.forceTransformPublishing_); 107 | nh_private.param("record_extrinsics", rovioNode.forceExtrinsicsPublishing_, rovioNode.forceExtrinsicsPublishing_); 108 | nh_private.param("record_imu_bias", rovioNode.forceImuBiasPublishing_, rovioNode.forceImuBiasPublishing_); 109 | nh_private.param("record_pcl", rovioNode.forcePclPublishing_, rovioNode.forcePclPublishing_); 110 | nh_private.param("record_markers", rovioNode.forceMarkersPublishing_, rovioNode.forceMarkersPublishing_); 111 | nh_private.param("record_patch", rovioNode.forcePatchPublishing_, rovioNode.forcePatchPublishing_); 112 | nh_private.param("reset_trigger", resetTrigger, resetTrigger); 113 | 114 | std::cout << "Recording"; 115 | if(rovioNode.forceOdometryPublishing_) std::cout << ", odometry"; 116 | if(rovioNode.forceTransformPublishing_) std::cout << ", transform"; 117 | if(rovioNode.forceExtrinsicsPublishing_) std::cout << ", extrinsics"; 118 | if(rovioNode.forceImuBiasPublishing_) std::cout << ", imu biases"; 119 | if(rovioNode.forcePclPublishing_) std::cout << ", point cloud"; 120 | if(rovioNode.forceMarkersPublishing_) std::cout << ", markers"; 121 | if(rovioNode.forcePatchPublishing_) std::cout << ", patch data"; 122 | std::cout << std::endl; 123 | 124 | rosbag::Bag bagIn; 125 | std::string rosbag_filename = "dataset.bag"; 126 | nh_private.param("rosbag_filename", rosbag_filename, rosbag_filename); 127 | bagIn.open(rosbag_filename, rosbag::bagmode::Read); 128 | 129 | rosbag::Bag bagOut; 130 | std::size_t found = rosbag_filename.find_last_of("/"); 131 | std::string file_path = rosbag_filename.substr(0,found); 132 | std::string file_name = rosbag_filename.substr(found+1); 133 | if(file_path==rosbag_filename){ 134 | file_path = "."; 135 | file_name = rosbag_filename; 136 | } 137 | 138 | std::stringstream stream; 139 | boost::posix_time::time_facet* facet = new boost::posix_time::time_facet(); 140 | facet->format("%Y-%m-%d-%H-%M-%S"); 141 | stream.imbue(std::locale(std::locale::classic(), facet)); 142 | stream << ros::Time::now().toBoost() << "_" << nMax_ << "_" << nLevels_ << "_" << patchSize_ << "_" << nCam_ << "_" << nPose_; 143 | std::string filename_out = file_path + "/rovio/" + stream.str(); 144 | nh_private.param("filename_out", filename_out, filename_out); 145 | std::string rosbag_filename_out = filename_out + ".bag"; 146 | std::string info_filename_out = filename_out + ".info"; 147 | std::cout << "Storing output to: " << rosbag_filename_out << std::endl; 148 | bagOut.open(rosbag_filename_out, rosbag::bagmode::Write); 149 | 150 | // Copy info 151 | std::ifstream src(filter_config, std::ios::binary); 152 | std::ofstream dst(info_filename_out, std::ios::binary); 153 | dst << src.rdbuf(); 154 | 155 | std::vector topics; 156 | std::string imu_topic_name = "/imu0"; 157 | nh_private.param("imu_topic_name", imu_topic_name, imu_topic_name); 158 | std::string cam0_topic_name = "/cam0/image_raw"; 159 | nh_private.param("cam0_topic_name", cam0_topic_name, cam0_topic_name); 160 | std::string cam1_topic_name = "/cam1/image_raw"; 161 | nh_private.param("cam1_topic_name", cam1_topic_name, cam1_topic_name); 162 | std::string odometry_topic_name = rovioNode.pubOdometry_.getTopic(); 163 | std::string transform_topic_name = rovioNode.pubTransform_.getTopic(); 164 | std::string extrinsics_topic_name[mtFilter::mtState::nCam_]; 165 | for(int camID=0;camIDgetTopic() == imu_topic_name){ 183 | sensor_msgs::Imu::ConstPtr imuMsg = it->instantiate(); 184 | if (imuMsg != NULL) rovioNode.imuCallback(imuMsg); 185 | } 186 | if(it->getTopic() == cam0_topic_name){ 187 | sensor_msgs::ImageConstPtr imgMsg = it->instantiate(); 188 | if (imgMsg != NULL) rovioNode.imgCallback0(imgMsg); 189 | } 190 | if(it->getTopic() == cam1_topic_name){ 191 | sensor_msgs::ImageConstPtr imgMsg = it->instantiate(); 192 | if (imgMsg != NULL) rovioNode.imgCallback1(imgMsg); 193 | } 194 | ros::spinOnce(); 195 | 196 | if(rovioNode.gotFirstMessages_){ 197 | static double lastSafeTime = rovioNode.mpFilter_->safe_.t_; 198 | if(rovioNode.mpFilter_->safe_.t_ > lastSafeTime){ 199 | if(rovioNode.forceOdometryPublishing_) bagOut.write(odometry_topic_name,ros::Time::now(),rovioNode.odometryMsg_); 200 | if(rovioNode.forceTransformPublishing_) bagOut.write(transform_topic_name,ros::Time::now(),rovioNode.transformMsg_); 201 | for(int camID=0;camIDsafe_.t_; 209 | } 210 | if(!isTriggerInitialized){ 211 | lastTriggerTime = lastSafeTime; 212 | isTriggerInitialized = true; 213 | } 214 | if(resetTrigger>0.0 && lastSafeTime - lastTriggerTime > resetTrigger){ 215 | rovioNode.requestReset(); 216 | rovioNode.mpFilter_->init_.state_.WrWM() = rovioNode.mpFilter_->safe_.state_.WrWM(); 217 | rovioNode.mpFilter_->init_.state_.qWM() = rovioNode.mpFilter_->safe_.state_.qWM(); 218 | lastTriggerTime = lastSafeTime; 219 | } 220 | } 221 | } 222 | 223 | bagOut.close(); 224 | bagIn.close(); 225 | 226 | 227 | return 0; 228 | } 229 | -------------------------------------------------------------------------------- /src/test_patch.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | #include 3 | 4 | #include "rovio/Patch.hpp" 5 | 6 | using namespace rovio; 7 | 8 | class PatchTesting : public virtual ::testing::Test { 9 | protected: 10 | static const int patchSize_ = 2; 11 | static const int imgSize_ = patchSize_+4; 12 | static const int nMax_ = 20; 13 | static const int dx_ = 2; 14 | static const int dy_ = 3; 15 | cv::Mat img1_; 16 | cv::Mat img2_; 17 | Patch p_; 18 | FeatureCoordinates c_; 19 | PatchTesting(){ 20 | img1_ = cv::Mat::zeros(imgSize_,imgSize_,CV_8UC1); 21 | uint8_t* img_ptr = (uint8_t*) img1_.data; 22 | for(int i=0;ipatchSize_)> p; 45 | ASSERT_EQ(p.validGradientParameters_,false); 46 | ASSERT_EQ(p.s_,0.0); 47 | } 48 | 49 | // Test isPatchInFrame 50 | TEST_F(PatchTesting, isPatchInFrame) { 51 | c_.set_c(cv::Point2f(0,0)); 52 | c_.set_warp_identity(); 53 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),false); 54 | c_.set_c(cv::Point2f(patchSize_/2-0.1,patchSize_/2-0.1)); 55 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),false); 56 | c_.set_c(cv::Point2f(patchSize_/2,patchSize_/2)); 57 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),true); 58 | c_.set_c(cv::Point2f(patchSize_/2+0.1,patchSize_/2+0.1)); 59 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),true); 60 | c_.set_c(cv::Point2f(imgSize_-patchSize_/2-0.1,imgSize_-patchSize_/2-0.1)); 61 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),true); 62 | c_.set_c(cv::Point2f(imgSize_-patchSize_/2,imgSize_-patchSize_/2)); 63 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),true); 64 | c_.set_c(cv::Point2f(imgSize_-patchSize_/2+0.1,imgSize_-patchSize_/2+0.1)); 65 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),false); 66 | c_.set_c(cv::Point2f(imgSize_,imgSize_)); 67 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),false); 68 | c_.set_c(cv::Point2f(0,0)); 69 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),false); 70 | c_.set_c(cv::Point2f(patchSize_/2+1-0.1,patchSize_/2+1-0.1)); 71 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),false); 72 | c_.set_c(cv::Point2f(patchSize_/2+1,patchSize_/2+1)); 73 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),true); 74 | c_.set_c(cv::Point2f(patchSize_/2+1+0.1,patchSize_/2+1+0.1)); 75 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),true); 76 | c_.set_c(cv::Point2f(imgSize_-patchSize_/2-1-0.1,imgSize_-patchSize_/2-1-0.1)); 77 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),true); 78 | c_.set_c(cv::Point2f(imgSize_-patchSize_/2-1,imgSize_-patchSize_/2-1)); 79 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),true); 80 | c_.set_c(cv::Point2f(imgSize_-patchSize_/2-1+0.1,imgSize_-patchSize_/2-1+0.1)); 81 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),false); 82 | c_.set_c(cv::Point2f(imgSize_,imgSize_)); 83 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),false); 84 | } 85 | 86 | // Test isPatchInFrame 87 | TEST_F(PatchTesting, isPatchInFrameWithWarping) { 88 | Eigen::Matrix2f aff; 89 | aff.setIdentity(); 90 | c_.set_warp_identity(); 91 | c_.set_c(cv::Point2f(0,0)); 92 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),false); 93 | c_.set_c(cv::Point2f(patchSize_/2-0.1,patchSize_/2-0.1)); 94 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),false); 95 | c_.set_c(cv::Point2f(patchSize_/2,patchSize_/2)); 96 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),true); 97 | c_.set_c(cv::Point2f(patchSize_/2+0.1,patchSize_/2+0.1)); 98 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),true); 99 | c_.set_c(cv::Point2f(imgSize_-patchSize_/2-0.1,imgSize_-patchSize_/2-0.1)); 100 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),true); 101 | c_.set_c(cv::Point2f(imgSize_-patchSize_/2,imgSize_-patchSize_/2)); 102 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),true); 103 | c_.set_c(cv::Point2f(imgSize_-patchSize_/2+0.1,imgSize_-patchSize_/2+0.1)); 104 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),false); 105 | c_.set_c(cv::Point2f(imgSize_,imgSize_)); 106 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),false); 107 | c_.set_c(cv::Point2f(0,0)); 108 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),false); 109 | c_.set_c(cv::Point2f(patchSize_/2+1-0.1,patchSize_/2+1-0.1)); 110 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),false); 111 | c_.set_c(cv::Point2f(patchSize_/2+1,patchSize_/2+1)); 112 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),true); 113 | c_.set_c(cv::Point2f(patchSize_/2+1+0.1,patchSize_/2+1+0.1)); 114 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),true); 115 | c_.set_c(cv::Point2f(imgSize_-patchSize_/2-1-0.1,imgSize_-patchSize_/2-1-0.1)); 116 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),true); 117 | c_.set_c(cv::Point2f(imgSize_-patchSize_/2-1,imgSize_-patchSize_/2-1)); 118 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),true); 119 | c_.set_c(cv::Point2f(imgSize_-patchSize_/2-1+0.1,imgSize_-patchSize_/2-1+0.1)); 120 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),false); 121 | c_.set_c(cv::Point2f(imgSize_,imgSize_)); 122 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),false); 123 | aff << 0, -1, 1, 0; 124 | c_.set_warp_c(aff); 125 | c_.set_c(cv::Point2f(0,0)); 126 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),false); 127 | c_.set_c(cv::Point2f(patchSize_/2-0.1,patchSize_/2-0.1)); 128 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),false); 129 | c_.set_c(cv::Point2f(patchSize_/2,patchSize_/2)); 130 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),true); 131 | c_.set_c(cv::Point2f(patchSize_/2+0.1,patchSize_/2+0.1)); 132 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),true); 133 | c_.set_c(cv::Point2f(imgSize_-patchSize_/2-0.1,imgSize_-patchSize_/2-0.1)); 134 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),true); 135 | c_.set_c(cv::Point2f(imgSize_-patchSize_/2,imgSize_-patchSize_/2)); 136 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),false); // should be true, but the implementation is just simpler like this (more reliable) 137 | c_.set_c(cv::Point2f(imgSize_-patchSize_/2+0.1,imgSize_-patchSize_/2+0.1)); 138 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),false); 139 | c_.set_c(cv::Point2f(imgSize_,imgSize_)); 140 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),false); 141 | c_.set_c(cv::Point2f(0,0)); 142 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),false); 143 | c_.set_c(cv::Point2f(patchSize_/2+1-0.1,patchSize_/2+1-0.1)); 144 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),false); 145 | c_.set_c(cv::Point2f(patchSize_/2+1,patchSize_/2+1)); 146 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),true); 147 | c_.set_c(cv::Point2f(patchSize_/2+1+0.1,patchSize_/2+1+0.1)); 148 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),true); 149 | c_.set_c(cv::Point2f(imgSize_-patchSize_/2-1-0.1,imgSize_-patchSize_/2-1-0.1)); 150 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),true); 151 | c_.set_c(cv::Point2f(imgSize_-patchSize_/2-1,imgSize_-patchSize_/2-1)); 152 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),false); // should be true, but the implementation is just simpler like this (more reliable) 153 | c_.set_c(cv::Point2f(imgSize_-patchSize_/2-1+0.1,imgSize_-patchSize_/2-1+0.1)); 154 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),false); 155 | c_.set_c(cv::Point2f(imgSize_,imgSize_)); 156 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_,true),false); 157 | aff << cos(M_PI/6.0), -sin(M_PI/6.0), sin(M_PI/6.0), cos(M_PI/6.0); 158 | c_.set_warp_c(aff); 159 | c_.set_c(cv::Point2f((sin(M_PI/6.0)+cos(M_PI/6.0))*(patchSize_/2-0.5)+0.5+1e-6,(sin(M_PI/6.0)+cos(M_PI/6.0))*(patchSize_/2-0.5)+0.5+1e-6)); 160 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),true); 161 | c_.set_c(cv::Point2f((sin(M_PI/6.0)+cos(M_PI/6.0))*(patchSize_/2-0.5)+0.5+1e-6,(sin(M_PI/6.0)+cos(M_PI/6.0))*(patchSize_/2-0.5)+0.5-1e-6)); 162 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),false); 163 | c_.set_c(cv::Point2f((sin(M_PI/6.0)+cos(M_PI/6.0))*(patchSize_/2-0.5)+0.5-1e-6,(sin(M_PI/6.0)+cos(M_PI/6.0))*(patchSize_/2-0.5)+0.5+1e-6)); 164 | ASSERT_EQ(p_.isPatchInFrame(img1_,c_),false); 165 | } 166 | 167 | // Test extractPatchFromImage 168 | TEST_F(PatchTesting, extractPatchFromImage) { 169 | const int N = 6; 170 | cv::Point2f patchOrigins[N] = {cv::Point2f(0,0), 171 | cv::Point2f(0.5,0), 172 | cv::Point2f(0,0.5), 173 | cv::Point2f(0.5,0.5), 174 | cv::Point2f(1.5544,0.5234), 175 | cv::Point2f(imgSize_-patchSize_-2,imgSize_-patchSize_-2)}; 176 | for(unsigned int n=0;n