├── .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 <opencv2/features2d/features2d.hpp>
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<double,2,3>& 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<double,2,2>& 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<RobocentricFeatureElement>{
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<typename STATE>
76 | class FeatureOutputCT:public LWF::CoordinateTransform<STATE,FeatureOutput>{
77 | public:
78 | typedef LWF::CoordinateTransform<STATE,FeatureOutput> 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<mtOutput::_fea>() = input.template get<mtInput::_fea>(ID_);
91 | }
92 | void jacTransform(MXD& J, const mtInput& input) const{
93 | J.setIdentity();
94 | }
95 | };
96 |
97 | template<typename STATE>
98 | class TransformFeatureOutputCT:public LWF::CoordinateTransform<STATE,FeatureOutput>{
99 | public:
100 | typedef LWF::CoordinateTransform<STATE,FeatureOutput> Base;
101 | typedef typename Base::mtInput mtInput;
102 | typedef typename Base::mtOutput mtOutput;
103 | int outputCamID_;
104 | int ID_;
105 | bool ignoreDistanceOutput_;
106 | MultiCamera<STATE::nCam_>* mpMultiCamera_;
107 | TransformFeatureOutputCT(MultiCamera<STATE::nCam_>* 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<double,2,3> J_nor_DrDP = nor_out.getM().transpose()/d_out;
132 | const Eigen::Matrix<double,3,3> J_DrDP_CrCP = MPD(qDC).matrix();
133 | const Eigen::Matrix<double,3,2> 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<double,1,3> J_d_DrDP = nor_out.getVec().transpose();
150 | const Eigen::Matrix<double,2,3> J_nor_DrDP = nor_out.getM().transpose()/d_out;
151 | const Eigen::Matrix<double,3,3> J_DrDP_qDC = gSM(DrDP);
152 | const Eigen::Matrix<double,3,3> J_DrDP_CrCP = MPD(qDC).matrix();
153 | const Eigen::Matrix<double,3,3> J_DrDP_CrCD = -MPD(qDC).matrix();
154 |
155 | const Eigen::Matrix<double,3,3> J_qDC_qDB = Eigen::Matrix3d::Identity();
156 | const Eigen::Matrix<double,3,3> J_qDC_qCB = -MPD(qDC).matrix();
157 | const Eigen::Matrix<double,3,3> J_CrCD_qCB = gSM(CrCD);
158 | const Eigen::Matrix<double,3,3> J_CrCD_BrBC = -MPD(input.qCM(camID)).matrix();
159 | const Eigen::Matrix<double,3,3> J_CrCD_BrBD = MPD(input.qCM(camID)).matrix();
160 |
161 | const Eigen::Matrix<double,3,1> J_CrCP_d = input.CfP(ID_).get_nor().getVec()*input.dep(ID_).getDistanceDerivative();
162 | const Eigen::Matrix<double,3,2> J_CrCP_nor = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getM();
163 |
164 | J.template block<2,2>(mtOutput::template getId<mtOutput::_fea>(),mtInput::template getId<mtInput::_fea>(ID_)) = J_nor_DrDP*J_DrDP_CrCP*J_CrCP_nor;
165 | J.template block<2,1>(mtOutput::template getId<mtOutput::_fea>(),mtInput::template getId<mtInput::_fea>(ID_)+2) = J_nor_DrDP*J_DrDP_CrCP*J_CrCP_d;
166 | if(!ignoreDistanceOutput_){
167 | J.template block<1,2>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_fea>(ID_)) = J_d_DrDP*J_DrDP_CrCP*J_CrCP_nor;
168 | J.template block<1,1>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_fea>(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<mtOutput::_fea>(),mtInput::template getId<mtInput::_vea>(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<mtOutput::_fea>(),mtInput::template getId<mtInput::_vea>(outputCamID_)) = J_nor_DrDP*J_DrDP_qDC*J_qDC_qDB;
174 | J.template block<2,3>(mtOutput::template getId<mtOutput::_fea>(),mtInput::template getId<mtInput::_vep>(camID)) = J_nor_DrDP*J_DrDP_CrCD*J_CrCD_BrBC;
175 | J.template block<2,3>(mtOutput::template getId<mtOutput::_fea>(),mtInput::template getId<mtInput::_vep>(outputCamID_)) = J_nor_DrDP*J_DrDP_CrCD*J_CrCD_BrBD;
176 | if(!ignoreDistanceOutput_){
177 | J.template block<1,3>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_vea>(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<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_vea>(outputCamID_)) = J_d_DrDP*J_DrDP_qDC*J_qDC_qDB;
179 | J.template block<1,3>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_vep>(camID)) = J_d_DrDP*J_DrDP_CrCD*J_CrCD_BrBC;
180 | J.template block<1,3>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_vep>(outputCamID_)) = J_d_DrDP*J_DrDP_CrCD*J_CrCD_BrBD;
181 | }
182 | }
183 | } else {
184 | J.template block<2,2>(mtOutput::template getId<mtOutput::_fea>(),mtInput::template getId<mtInput::_fea>(ID_)) = Eigen::Matrix2d::Identity();
185 | J.template block<1,1>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_fea>(ID_)+2) = Eigen::Matrix<double,1,1>::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::VectorElement<3>,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<FeatureOutput,FeatureOutputReadable>{
77 | public:
78 | typedef LWF::CoordinateTransform<FeatureOutput,FeatureOutputReadable> 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<mtOutput::_bea>(),mtInput::template getId<mtInput::_fea>()) = input.c().get_nor().getM();
90 | J(mtOutput::template getId<mtOutput::_dis>(),mtInput::template getId<mtInput::_fea>()+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<LWF::VectorElement<3>>{
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<typename STATE>
62 | class LandmarkOutputImuCT:public LWF::CoordinateTransform<STATE,LandmarkOutput>{
63 | public:
64 | typedef LWF::CoordinateTransform<STATE,LandmarkOutput> Base;
65 | typedef typename Base::mtInput mtInput;
66 | typedef typename Base::mtOutput mtOutput;
67 | int ID_;
68 | MultiCamera<STATE::nCam_>* mpMultiCamera_;
69 | LandmarkOutputImuCT(MultiCamera<STATE::nCam_>* 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<mtOutput::_lmk>() = 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<double,3,2> J_CrCP_nor = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getM();
88 | const Eigen::Matrix<double,3,1> 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<mtOutput::_lmk>(),mtInput::template getId<mtInput::_fea>(ID_)) = mBC*J_CrCP_nor;
92 | J.template block<3,1>(mtOutput::template getId<mtOutput::_lmk>(),mtInput::template getId<mtInput::_fea>(ID_)+2) = mBC*J_CrCP_d;
93 |
94 | if(input.aux().doVECalibration_){
95 | J.template block<3,3>(mtOutput::template getId<mtOutput::_lmk>(),mtInput::template getId<mtInput::_vep>()) = M3D::Identity();
96 | J.template block<3,3>(mtOutput::template getId<mtOutput::_lmk>(),mtInput::template getId<mtInput::_vea>()) = 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<LWF::VectorElement<2>>{
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<float>(this->get<_pix>()(0)),static_cast<float>(this->get<_pix>()(1)));
46 | }
47 | };
48 |
49 | class PixelOutputCT:public LWF::CoordinateTransform<FeatureOutput,PixelOutput>{
50 | public:
51 | typedef LWF::CoordinateTransform<FeatureOutput,PixelOutput> 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<mtOutput::_pix>() = 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<mtOutput::_pix>(),mtInput::template getId<mtInput::_fea>()) = 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<LWF::TH_multiple_elements<LWF::VectorElement<3>,3>,LWF::QuaternionElement>{
38 | public:
39 | static constexpr unsigned int _pos = 0; /**<Idx. Position Vector WrWB: Pointing from the World-Frame to the Body-Frame, expressed in World-Coordinates.*/
40 | static constexpr unsigned int _vel = _pos+1; /**<Idx. Velocity Vector BvB: Absolute velocity of the of the Body-Frame, expressed in Body-Coordinates.*/
41 | static constexpr unsigned int _ror = _vel+1; /**<Idx. Angular velocity BwWB: Absolute angular velocity of the Body frame (of the solid), expressed in Body coordinates.*/
42 | static constexpr unsigned int _att = _ror+1; /**<Idx. Quaternion qBW: World coordinates to Body coordinates.*/
43 | StandardOutput(){
44 | }
45 | virtual ~StandardOutput(){};
46 |
47 | //@{
48 | /** \brief Get the position vector pointing from the World-Frame to the Body-Frame, expressed in World-Coordinates (World->%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<typename STATE>
101 | class CameraOutputCT:public LWF::CoordinateTransform<STATE,StandardOutput>{
102 | public:
103 | typedef LWF::CoordinateTransform<STATE,StandardOutput> 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<mtOutput::_pos>(),mtInput::template getId<mtInput::_pos>()) = M3D::Identity();
124 | J.template block<3,3>(mtOutput::template getId<mtOutput::_pos>(),mtInput::template getId<mtInput::_att>()) =
125 | -gSM(input.qWM().rotate(input.MrMC(camID_)));
126 | J.template block<3,3>(mtOutput::template getId<mtOutput::_att>(),mtInput::template getId<mtInput::_att>()) =
127 | -MPD(input.qCM(camID_)*input.qWM().inverted()).matrix();
128 | J.template block<3,3>(mtOutput::template getId<mtOutput::_vel>(),mtInput::template getId<mtInput::_vel>()) = -MPD(input.qCM(camID_)).matrix();
129 | J.template block<3,3>(mtOutput::template getId<mtOutput::_vel>(),mtInput::template getId<mtInput::_gyb>()) = MPD(input.qCM(camID_)).matrix()
130 | * gSM(input.MrMC(camID_));
131 | J.template block<3,3>(mtOutput::template getId<mtOutput::_ror>(),mtInput::template getId<mtInput::_gyb>()) = -MPD(input.qCM(camID_)).matrix();
132 | if(input.aux().doVECalibration_){
133 | J.template block<3,3>(mtOutput::template getId<mtOutput::_pos>(),mtInput::template getId<mtInput::_vep>(camID_)) =
134 | MPD(input.qWM()).matrix();
135 | J.template block<3,3>(mtOutput::template getId<mtOutput::_vel>(),mtInput::template getId<mtInput::_vep>(camID_)) =
136 | MPD(input.qCM(camID_)).matrix()*gSM(V3D(input.aux().MwWMmeas_-input.gyb()));
137 | J.template block<3,3>(mtOutput::template getId<mtOutput::_ror>(),mtInput::template getId<mtInput::_vea>(camID_)) =
138 | -gSM(input.qCM(camID_).rotate(V3D(input.aux().MwWMmeas_-input.gyb())));
139 | J.template block<3,3>(mtOutput::template getId<mtOutput::_vel>(),mtInput::template getId<mtInput::_vea>(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<mtOutput::_att>(),mtInput::template getId<mtInput::_vea>(camID_)) =
142 | M3D::Identity();
143 | }
144 | }
145 | void postProcess(MXD& cov,const mtInput& input){
146 | cov.template block<3,3>(mtOutput::template getId<mtOutput::_ror>(),mtOutput::template getId<mtOutput::_ror>()) += input.aux().wMeasCov_;
147 | }
148 | };
149 |
150 | template<typename STATE>
151 | class ImuOutputCT:public LWF::CoordinateTransform<STATE,StandardOutput>{
152 | public:
153 | typedef LWF::CoordinateTransform<STATE,StandardOutput> 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<mtOutput::_pos>(),mtInput::template getId<mtInput::_pos>()) = M3D::Identity();
171 | J.template block<3,3>(mtOutput::template getId<mtOutput::_att>(),mtInput::template getId<mtInput::_att>()) = -MPD(input.qWM().inverted()).matrix();
172 | J.template block<3,3>(mtOutput::template getId<mtOutput::_vel>(),mtInput::template getId<mtInput::_vel>()) = -M3D::Identity();
173 | J.template block<3,3>(mtOutput::template getId<mtOutput::_ror>(),mtInput::template getId<mtInput::_gyb>()) = -M3D::Identity();
174 | }
175 | void postProcess(MXD& cov,const mtInput& input){
176 | cov.template block<3,3>(mtOutput::template getId<mtOutput::_ror>(),mtOutput::template getId<mtOutput::_ror>()) += 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<LWF::QuaternionElement>{
38 | public:
39 | static constexpr unsigned int _att = 0;
40 | AttitudeOutput(){
41 | }
42 | virtual ~AttitudeOutput(){};
43 | };
44 |
45 | class YprOutput: public LWF::State<LWF::VectorElement<3>>{
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<AttitudeOutput,YprOutput>{
56 | public:
57 | typedef LWF::CoordinateTransform<AttitudeOutput,YprOutput> 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<mtOutput::_ypr>() = kindr::EulerAnglesZyxD(input.template get<mtInput::_att>()).vector();
64 | }
65 | void jacTransform(MXD& J, const mtInput& input) const{
66 | kindr::EulerAnglesZyxD zyx(input.template get<mtInput::_att>());
67 | J = zyx.getMappingFromLocalAngularVelocityToDiff()*MPD(input.template get<mtInput::_att>().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_; /**<Distance parameter*/
39 |
40 | /** \brief Specifies the parametrization type
41 | */
42 | enum Type{
43 | REGULAR, /**<Regular distance p = d*/
44 | INVERSE, /**<Inverse distance p = 1/d*/
45 | LOG, /**<Logarithmic distance p = ln(d)*/
46 | HYPERBOLIC /**<Hyperbolic distance p = asinh(d)*/
47 | } type_;
48 |
49 | /** \brief Constructor. Create a FeatureDistance object with a desired \ref Type.
50 | *
51 | * @param type - enum \ref Type.
52 | */
53 | FeatureDistance(const Type& type = REGULAR);
54 |
55 | /** \brief Destructor
56 | */
57 | virtual ~FeatureDistance();
58 |
59 | /** \brief Set the \ref Type type_ using the enum \ref Type.
60 | *
61 | * @param type - Enum \ref Type.
62 | */
63 | void setType(const Type& type);
64 |
65 | /** \brief Get the set \ref Type.
66 | *
67 | * @return the set \ref Type.
68 | */
69 | Type getType();
70 |
71 | /** \brief Set the \ref Type type_ using the integer value of the enum \ref Type.
72 | *
73 | * @param type - Integer value of the enum \ref Type.
74 | */
75 | void setType(const int& type);
76 |
77 | /** \brief Returns the distance based on the set \ref Type type_.
78 | *
79 | * @return Distance value.
80 | */
81 | void setParameter(const double& d);
82 |
83 | /** \brief Returns the distance based on the set \ref Type type_.
84 | *
85 | * @return Distance value.
86 | */
87 | double getDistance() const;
88 |
89 | /** \brief Returns the derivative of the distance w.r.t. the parameter.
90 | *
91 | * @return d derived w.r.t. p
92 | */
93 | double getDistanceDerivative() const;
94 |
95 | /** \brief Returns the derivative of the parameter w.r.t. the distance.
96 | *
97 | * @return p derived w.r.t. d
98 | */
99 | double getParameterDerivative() const;
100 |
101 | /** \brief Returns the derivative of the parameter w.r.t. the distance w.r.t. the parameter.
102 | *
103 | * @return p derived w.r.t. d and then derived w.r.t. p
104 | */
105 | double getParameterDerivativeCombined() const;
106 |
107 | /** \brief Converts from an other feature distance parametrization
108 | *
109 | * @param other "other" feature distance, the type is beeing considered during conversion
110 | */
111 | void getParameterDerivativeCombined(FeatureDistance other);
112 |
113 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
114 |
115 | /** \brief Set the distance parameter for a desired distance for a regular parametrization.
116 | *
117 | * @param Distance value.
118 | */
119 | void setParameterRegular(const double& d);
120 |
121 | /** \brief Returns the distance based for a regular parametrization.
122 | *
123 | * @return Distance value.
124 | */
125 | double getDistanceRegular() const;
126 |
127 | /** \brief Returns the derivative of the distance w.r.t. the parameter based for a regular parametrization.
128 | *
129 | * @return d derived w.r.t. p
130 | */
131 | double getDistanceDerivativeRegular() const;
132 |
133 | /** \brief Returns the derivative of the parameter w.r.t. the distance based for a regular parametrization.
134 | *
135 | * @return p derived w.r.t. d
136 | */
137 | double getParameterDerivativeRegular() const;
138 |
139 | /** \brief Returns the derivative of the parameter w.r.t. the distance w.r.t. the parameter based for a regular parametrization.
140 | *
141 | * @return p derived w.r.t. d and then derived w.r.t. p
142 | */
143 | double getParameterDerivativeCombinedRegular() const;
144 |
145 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
146 |
147 | /** \brief Ensures that a value is non-zero by setting it to a small value
148 | *
149 | * @param p - Input
150 | * @return Garanteed non-zero output
151 | */
152 | double makeNonZero(const double& p) const;
153 |
154 | /** \brief Set the distance parameter for a desired distance for a inverse parametrization.
155 | *
156 | * @param Distance value.
157 | */
158 | void setParameterInverse(const double& d);
159 |
160 | /** \brief Returns the distance based for a Inverse parametrization.
161 | *
162 | * @return Distance value.
163 | */
164 | double getDistanceInverse() const;
165 |
166 | /** \brief Returns the derivative of the distance w.r.t. the parameter based for a Inverse parametrization.
167 | *
168 | * @return d derived w.r.t. p
169 | */
170 | double getDistanceDerivativeInverse() const;
171 |
172 | /** \brief Returns the derivative of the parameter w.r.t. the distance based for a Inverse parametrization.
173 | *
174 | * @return p derived w.r.t. d
175 | */
176 | double getParameterDerivativeInverse() const;
177 |
178 | /** \brief Returns the derivative of the parameter w.r.t. the distance w.r.t. the parameter based for a Inverse parametrization.
179 | *
180 | * @return p derived w.r.t. d and then derived w.r.t. p
181 | */
182 | double getParameterDerivativeCombinedInverse() const;
183 |
184 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
185 |
186 | /** \brief Set the distance parameter for a desired distance for a log parametrization.
187 | *
188 | * @param Distance value.
189 | */
190 | void setParameterLog(const double& d);
191 |
192 | /** \brief Returns the distance based for a Log parametrization.
193 | *
194 | * @return Distance value.
195 | */
196 | double getDistanceLog() const;
197 |
198 | /** \brief Returns the derivative of the distance w.r.t. the parameter based for a Log parametrization.
199 | *
200 | * @return d derived w.r.t. p
201 | */
202 | double getDistanceDerivativeLog() const;
203 |
204 | /** \brief Returns the derivative of the parameter w.r.t. the distance based for a Log parametrization.
205 | *
206 | * @return p derived w.r.t. d
207 | */
208 | double getParameterDerivativeLog() const;
209 |
210 | /** \brief Returns the derivative of the parameter w.r.t. the distance w.r.t. the parameter based for a Log parametrization.
211 | *
212 | * @return p derived w.r.t. d and then derived w.r.t. p
213 | */
214 | double getParameterDerivativeCombinedLog() const;
215 |
216 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
217 |
218 | /** \brief Set the distance parameter for a desired distance for a hyperbolic parametrization.
219 | *
220 | * @param Distance value.
221 | */
222 | void setParameterHyperbolic(const double& d);
223 |
224 | /** \brief Returns the distance based for a Hyperbolic parametrization.
225 | *
226 | * @return Distance value.
227 | */
228 | double getDistanceHyperbolic() const;
229 |
230 | /** \brief Returns the derivative of the distance w.r.t. the parameter based for a Hyperbolic parametrization.
231 | *
232 | * @return d derived w.r.t. p
233 | */
234 | double getDistanceDerivativeHyperbolic() const;
235 |
236 | /** \brief Returns the derivative of the parameter w.r.t. the distance based for a Hyperbolic parametrization.
237 | *
238 | * @return p derived w.r.t. d
239 | */
240 | double getParameterDerivativeHyperbolic() const;
241 |
242 | /** \brief Returns the derivative of the parameter w.r.t. the distance w.r.t. the parameter based for a Hyperbolic parametrization.
243 | *
244 | * @return p derived w.r.t. d and then derived w.r.t. p
245 | */
246 | double getParameterDerivativeCombinedHyperbolic() const;
247 | };
248 |
249 | }
250 |
251 |
252 | #endif /* FEATUREDISTANCE_HPP_ */
253 |
--------------------------------------------------------------------------------
/include/rovio/ImagePyramid.hpp:
--------------------------------------------------------------------------------
1 | /*
2 | * Copyright (c) 2014, Autonomous Systems Lab
3 | * All rights reserved.
4 | *
5 | * Redistribution and use in source and binary forms, with or without
6 | * modification, are permitted provided that the following conditions are met:
7 | * * Redistributions of source code must retain the above copyright
8 | * notice, this list of conditions and the following disclaimer.
9 | * * Redistributions in binary form must reproduce the above copyright
10 | * notice, this list of conditions and the following disclaimer in the
11 | * documentation and/or other materials provided with the distribution.
12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
13 | * names of its contributors may be used to endorse or promote products
14 | * derived from this software without specific prior written permission.
15 | *
16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 | *
27 | */
28 |
29 | #ifndef IMAGEPYRAMID_HPP_
30 | #define IMAGEPYRAMID_HPP_
31 |
32 | #include <opencv2/features2d/features2d.hpp>
33 | #include <opencv2/imgproc/imgproc.hpp>
34 | #include <opencv2/highgui/highgui.hpp>
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<imgOut.rows; ++y){
52 | imgPtrInTop = imgIn.data + 2*y*refStepIn;
53 | imgPtrInBot = imgIn.data + (2*y+1)*refStepIn;
54 | imgPtrOut = imgOut.data + y*refStepOut;
55 | for(int x=0; x<imgOut.cols; ++x, ++imgPtrOut, imgPtrInTop += 2, imgPtrInBot += 2)
56 | {
57 | *imgPtrOut = (imgPtrInTop[0]+imgPtrInTop[1]+imgPtrInBot[0]+imgPtrInBot[1])/4;
58 | }
59 | }
60 | }
61 |
62 | /** \brief Image pyramid with selectable number of levels.
63 | *
64 | * @tparam n_levels - Number of pyramid levels.
65 | * @todo: complete rule of three
66 | */
67 | template<int n_levels>
68 | class ImagePyramid{
69 | public:
70 | ImagePyramid(){};
71 | virtual ~ImagePyramid(){};
72 | cv::Mat imgs_[n_levels]; /**<Array, containing the pyramid images.*/
73 | cv::Point2f centers_[n_levels]; /**<Array, containing the image center coordinates (in pixel), defined in an
74 | image centered coordinate system of the image at level 0.*/
75 |
76 | /** \brief Initializes the image pyramid from an input image (level 0).
77 | *
78 | * @param img - Input image (level 0).
79 | * @param useCv - Set to true, if opencv (cv::pyrDown) should be used for the pyramid creation.
80 | */
81 | void computeFromImage(const cv::Mat& img, const bool useCv = false){
82 | img.copyTo(imgs_[0]);
83 | centers_[0] = cv::Point2f(0,0);
84 | for(int i=1; i<n_levels; ++i){
85 | if(!useCv){
86 | halfSample(imgs_[i-1],imgs_[i]);
87 | centers_[i].x = centers_[i-1].x-pow(0.5,2-i)*(float)(imgs_[i-1].rows%2);
88 | centers_[i].y = centers_[i-1].y-pow(0.5,2-i)*(float)(imgs_[i-1].cols%2);
89 | } else {
90 | cv::pyrDown(imgs_[i-1],imgs_[i],cv::Size(imgs_[i-1].cols/2, imgs_[i-1].rows/2));
91 | centers_[i].x = centers_[i-1].x-pow(0.5,2-i)*(float)((imgs_[i-1].rows%2)+1);
92 | centers_[i].y = centers_[i-1].y-pow(0.5,2-i)*(float)((imgs_[i-1].cols%2)+1);
93 | }
94 | }
95 | }
96 |
97 | /** \brief Copies the image pyramid.
98 | */
99 | ImagePyramid<n_levels>& operator=(const ImagePyramid<n_levels> &rhs) {
100 | for(unsigned int i=0;i<n_levels;i++){
101 | rhs.imgs_[i].copyTo(imgs_[i]);
102 | centers_[i] = rhs.centers_[i];
103 | }
104 | return *this;
105 | }
106 |
107 | /** \brief Transforms pixel coordinates between two pyramid levels.
108 | *
109 | * @Note Invalidates camera and bearing vector, since the camera model is not valid for arbitrary image levels.
110 | * @param cIn - Input coordinates
111 | * @param cOut - Output coordinates
112 | * @param l1 - Input pyramid level.
113 | * @param l2 - Output pyramid level.
114 | * @return the corresponding pixel coordinates on pyramid level l2.
115 | */
116 | FeatureCoordinates levelTranformCoordinates(const FeatureCoordinates& cIn, const int l1, const int l2) const{
117 | assert(l1<n_levels && l2<n_levels && l1>=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<double>::max()) const{
140 | std::vector<cv::KeyPoint> 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<int nCam>
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<nCam;i++){
51 | qCB_[i].setIdentity();
52 | BrBC_[i].setZero();
53 | }
54 | };
55 | virtual ~MultiCamera(){};
56 |
57 | /** \brief Sets the extrinsics of the i'th camera
58 | *
59 | * @param i - Camera index
60 | * @param BrBC - Translational extrinsic parameter
61 | * @param qCB - Rotational extrinsic parameter
62 | */
63 | void setExtrinsics(const int i, const V3D& BrBC, const QPD& qCB){
64 | BrBC_[i] = BrBC;
65 | qCB_[i] = qCB;
66 | }
67 |
68 | /** \brief Loads and sets the distortion model and the corresponding distortion coefficients from yaml-file for camera i
69 | *
70 | * @param i - Camera index
71 | * @param filename - Path to the yaml-file, containing the distortion model and distortion coefficient data.
72 | */
73 | void load(const int i, const std::string& filename){
74 | cameras_[i].load(filename);
75 | }
76 |
77 | /** \brief Transforms feature coordinates from one camera frame to another
78 | *
79 | * @param u - Camera index of output frame
80 | * @param cIn - Feature coordinates to be transformed
81 | * @param dIn - Corresponding distance
82 | * @param cOut - Transformed feature coordinates
83 | * @param dOut - Corresponding distance of output
84 | * @todo avoid double computation
85 | */
86 | void transformFeature(const int i, const FeatureCoordinates& vecIn, const FeatureDistance& dIn, FeatureCoordinates& vecOut, FeatureDistance& dOut) const{
87 | if(vecIn.camID_ != i){
88 | // D = Destination camera frame.
89 | // qDC = qDM*qCM^T
90 | // CrCD = qCM*(MrMD-MrMC)
91 | // DrDP = qDC*(d_in*nor_in-CrCD)
92 | // d_out = ||DrDP||
93 | // nor_out = DrDP/d_out
94 | const QPD qDC = qCB_[i]*qCB_[vecIn.camID_].inverted();
95 | const V3D CrCD = qCB_[vecIn.camID_].rotate(V3D(BrBC_[i]-BrBC_[vecIn.camID_]));
96 | const V3D CrCP = dIn.getDistance()*vecIn.get_nor().getVec();
97 | const V3D DrDP = qDC.rotate(V3D(CrCP-CrCD));
98 | dOut.setParameter(DrDP.norm());
99 | vecOut.nor_.setFromVector(DrDP);
100 | vecOut.valid_c_ = false;
101 | vecOut.valid_nor_ = true;
102 | vecOut.camID_ = i;
103 | vecOut.mpCamera_ = &cameras_[i];
104 | vecOut.trackWarping_ = vecIn.trackWarping_;
105 | vecOut.pixelCov_.setZero();
106 | vecOut.eigenVector1_.setZero();
107 | vecOut.eigenVector2_.setZero();
108 | vecOut.sigma1_ = 0.0;
109 | vecOut.sigma2_ = 0.0;
110 | vecOut.sigmaAngle_ = 0.0;
111 | if(vecIn.trackWarping_){ // Invalidate warping
112 | vecOut.valid_warp_c_ = false;
113 | vecOut.valid_warp_nor_ = false;
114 | vecOut.isWarpIdentity_ = false;
115 | } else {
116 | vecOut.warp_c_ = vecIn.warp_c_;
117 | vecOut.valid_warp_c_ = vecIn.valid_warp_c_;
118 | vecOut.warp_nor_ = vecIn.warp_nor_;
119 | vecOut.valid_warp_nor_ = vecIn.valid_warp_nor_;
120 | vecOut.isWarpIdentity_ = vecIn.isWarpIdentity_;
121 | }
122 | } else {
123 | vecOut = vecIn;
124 | dOut = dIn;
125 | }
126 | }
127 | };
128 |
129 | }
130 |
131 |
132 | #endif /* ROVIO_MULTICAMERA_HPP_ */
133 |
--------------------------------------------------------------------------------
/include/rovio/MultilevelPatch.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_MULTILEVELPATCH_HPP_
30 | #define ROVIO_MULTILEVELPATCH_HPP_
31 |
32 | #include "rovio/Patch.hpp"
33 | #include "rovio/FeatureCoordinates.hpp"
34 | #include "rovio/ImagePyramid.hpp"
35 |
36 | namespace rovio{
37 |
38 | /** \brief A multilevel patch class.
39 | *
40 | * @tparam nLevels - Number of pyramid levels on which the feature is defined.
41 | * @tparam patchSize - Edge length of the patches in pixels. Value must be a multiple of 2!.
42 | * Note: The patches edge length (in pixels) is the same for each patch, independently from the pyramid level.
43 | */
44 | template<int nLevels,int patchSize>
45 | class MultilevelPatch{
46 | public:
47 | static const int nLevels_ = nLevels; /**<Number of pyramid levels on which the feature is defined.*/
48 | Patch<patchSize> patches_[nLevels_]; /**<Array, holding the patches on each pyramid level.*/
49 | bool isValidPatch_[nLevels_]; /**<Array, specifying if there is a valid patch stored at the corresponding location in \ref patches_.*/
50 | mutable Eigen::Matrix3f H_; /**<Hessian matrix, corresponding to the multilevel patches.*/
51 | mutable float e0_; /**<Smaller eigenvalue of H_.*/
52 | mutable float e1_; /**<Larger eigenvalue of H_.*/
53 | mutable float s_; /**<Shi-Tomasi score of the multilevel patch feature. @todo define and store method of computation*/
54 |
55 | /** Constructor
56 | */
57 | MultilevelPatch(){
58 | reset();
59 | }
60 |
61 | /** Destructor
62 | */
63 | virtual ~MultilevelPatch(){}
64 |
65 | /** \brief Resets the MultilevelPatch.
66 | *
67 | * @param idx - feature ID
68 | * @initTime - Time at initialization.
69 | */
70 | void reset(){
71 | H_.setIdentity();
72 | e0_ = 0;
73 | e1_ = 0;
74 | s_ = 0;
75 | for(unsigned int i = 0;i<nLevels_;i++){
76 | isValidPatch_[i] = false;
77 | }
78 | }
79 |
80 | /** \brief Computes and sets the multilevel Shi-Tomasi Score \ref s_, considering a defined pyramid level interval.
81 | *
82 | * @param l1 - Start level (l1<l2)
83 | * @param l2 - End level (l1<l2)
84 | */
85 | void computeMultilevelShiTomasiScore(const int l1 = 0, const int l2 = nLevels_-1) const{
86 | H_.setZero();
87 | int count = 0;
88 | for(int i=l1;i<=l2;i++){
89 | if(isValidPatch_[i]){
90 | H_ += pow(0.25,i)*patches_[i].getHessian();
91 | count++;
92 | }
93 | }
94 | if(count > 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<l2)
152 | * @param l2 - End pyramid level (l1<l2)
153 | * @return the RMSE value for the patches in the set pyramid level interval.
154 | */
155 | float computeAverageDifference(const MultilevelPatch<nLevels,patchSize>& 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<patchSize; ++y){
161 | for(int x=0; x<patchSize; ++x, ++it_patch, ++it_patch_in){
162 | offset += *it_patch - *it_patch_in;
163 | }
164 | }
165 | }
166 | offset = offset/(patchSize*patchSize*(l2-l1+1));
167 | float error = 0.0f;
168 | for(int l = l1; l <= l2; l++){
169 | const float* it_patch = patches_[l].patch_;
170 | const float* it_patch_in = mp.patches_[l].patch_;
171 | for(int y=0; y<patchSize; ++y){
172 | for(int x=0; x<patchSize; ++x, ++it_patch, ++it_patch_in){
173 | error += std::pow(*it_patch - *it_patch_in - offset,2);
174 | }
175 | }
176 | }
177 | error = error/(patchSize*patchSize*(l2-l1+1));
178 | return std::sqrt(error);
179 | }
180 |
181 | /** \brief Checks if the MultilevelPatchFeature's patches are fully located within the corresponding images.
182 | *
183 | * @param pyr - Image pyramid, which should be checked to fully contain the patches.
184 | * @param l - Maximal pyramid level which should be checked (Note: The maximal level is the critical level.)
185 | * @param mpCoor - Coordinates of the patch in the reference image (subpixel coordinates possible).
186 | * @param mpWarp - Affine warping matrix. If nullptr not warping is considered.
187 | * @param withBorder - If true, the check is executed with the expanded patch dimensions (incorporates the general patch dimensions).
188 | * If false, the check is only executed with the general patch dimensions.
189 | */
190 | static bool isMultilevelPatchInFrame(const ImagePyramid<nLevels>& 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<patchSize>::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<nLevels>& 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<int patchSize>
42 | class Patch {
43 | public:
44 | float patch_[patchSize*patchSize] __attribute__ ((aligned (16))); /**<Array, containing the intensity values of the patch.*/
45 | float patchWithBorder_[(patchSize+2)*(patchSize+2)] __attribute__ ((aligned (16))); /**<Array, containing the intensity values of the expanded patch.
46 | This expanded patch is necessary for the intensity gradient calculation.*/
47 | mutable float dx_[patchSize*patchSize] __attribute__ ((aligned (16))); /**<Array, containing the intensity gradient component in x-direction for each patch pixel.*/
48 | mutable float dy_[patchSize*patchSize] __attribute__ ((aligned (16))); /**<Array, containing the intensity gradient component in y-direction for each patch pixel.*/
49 | mutable Eigen::Matrix3f H_; /**<Hessian matrix of the patch (necessary for the patch alignment).*/
50 | mutable float s_; /**<Shi-Tomasi Score (smaller eigenvalue of H_).*/
51 | mutable float e0_; /**<Smaller eigenvalue of H_.*/
52 | mutable float e1_; /**<Larger eigenvalue of H_.*/
53 | mutable bool validGradientParameters_; /**<True, if the gradient parameters (patch gradient components dx_ dy_, Hessian H_, Shi-Thomasi Score s_) have been computed.
54 | \see computeGradientParameters()*/
55 | /** \brief Constructor
56 | */
57 | Patch(){
58 | static_assert(patchSize%2==0,"Patch patchSize must be a multiple of 2");
59 | validGradientParameters_ = false;
60 | s_ = 0.0;
61 | e0_ = 0.0;
62 | e1_ = 0.0;
63 | }
64 | /** \brief Destructor
65 | */
66 | virtual ~Patch(){}
67 |
68 | /** \brief Computes the gradient parameters of the patch (patch gradient components dx_ dy_, Hessian H_, Shi-Tomasi Score s_, Eigenvalues of the Hessian e0_ and e1_).
69 | * The expanded patch patchWithBorder_ must be set.
70 | * Sets validGradientParameters_ afterwards to true.
71 | */
72 | void computeGradientParameters() const{
73 | if(!validGradientParameters_){
74 | H_.setZero();
75 | const int refStep = patchSize+2;
76 | float* it_dx = dx_;
77 | float* it_dy = dy_;
78 | const float* it;
79 | Eigen::Vector3f J;
80 | J.setZero();
81 | for(int y=0; y<patchSize; ++y){
82 | it = patchWithBorder_ + (y+1)*refStep + 1;
83 | for(int x=0; x<patchSize; ++x, ++it, ++it_dx, ++it_dy){
84 | J[0] = 0.5 * (it[1] - it[-1]);
85 | J[1] = 0.5 * (it[refStep] - it[-refStep]);
86 | J[2] = 1;
87 | *it_dx = J[0];
88 | *it_dy = J[1];
89 | H_ += J*J.transpose();
90 | }
91 | }
92 | const float dXX = H_(0,0)/(patchSize*patchSize);
93 | const float dYY = H_(1,1)/(patchSize*patchSize);
94 | const float dXY = H_(0,1)/(patchSize*patchSize);
95 |
96 | e0_ = 0.5 * (dXX + dYY - sqrtf((dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY)));
97 | e1_ = 0.5 * (dXX + dYY + sqrtf((dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY)));
98 | s_ = e0_+e1_;
99 | validGradientParameters_ = true;
100 | }
101 | }
102 |
103 | /** \brief Extracts and sets the patch intensity values (patch_) from the intensity values of the
104 | * expanded patch (patchWithBorder_).
105 | */
106 | void extractPatchFromPatchWithBorder(){
107 | float* it_patch = patch_;
108 | float* it_patchWithBorder;
109 | for(int y=1; y<patchSize+1; ++y, it_patch += patchSize){
110 | it_patchWithBorder = patchWithBorder_ + y*(patchSize+2) + 1;
111 | for(int x=0; x<patchSize; ++x)
112 | it_patch[x] = it_patchWithBorder[x];
113 | }
114 | }
115 |
116 | /** \brief Returns the Shi-Tomasi Score s_.
117 | *
118 | * Computes and sets the gradient parameters, which are the patch gradient components dx_ dy_, the Hessian H_ and the Shi-Tomasi Score s_.
119 | * \see computeGradientParameters()
120 | * @return the Shi-Tomasi Score s_ ( smaller eigenvalue of the Hessian H_ ).
121 | */
122 | float getScore() const{
123 | computeGradientParameters();
124 | return s_;
125 | }
126 |
127 | /** \brief Returns the Hessian Matrix H_.
128 | *
129 | * Computes and sets the gradient parameters, which are the patch gradient components dx_ dy_, the Hessian H_ and the Shi-Tomasi Score s_.
130 | * @return the Hessian Matrix H_ .
131 | */
132 | Eigen::Matrix3f getHessian() const{
133 | computeGradientParameters();
134 | return H_;
135 | }
136 |
137 | /** \brief Draws the patch into an image.
138 | *
139 | * @param drawImg - Image in which the patch should be drawn.
140 | * @param c - Pixel coordinates of the left upper patch corner.
141 | * @param stretch - %Patch drawing magnification factor.
142 | * @param withBorder - Draw either the patch patch_ (withBorder = false) or the expanded patch
143 | * patchWithBorder_ (withBorder = true).
144 | */
145 | void drawPatch(cv::Mat& drawImg,const cv::Point2i& c,int stretch = 1,const bool withBorder = false) const{
146 | const int refStepY = drawImg.step.p[0];
147 | const int refStepX = drawImg.step.p[1];
148 | uint8_t* img_ptr;
149 | const float* it_patch;
150 | if(withBorder){
151 | it_patch = patchWithBorder_;
152 | } else {
153 | it_patch = patch_;
154 | }
155 | for(int y=0; y<patchSize+2*(int)withBorder; ++y, it_patch += patchSize+2*(int)withBorder){
156 | img_ptr = (uint8_t*) drawImg.data + (c.y+y*stretch)*refStepY + c.x*refStepX;
157 | for(int x=0; x<patchSize+2*(int)withBorder; ++x)
158 | for(int i=0;i<stretch;++i){
159 | for(int j=0;j<stretch;++j){
160 | img_ptr[x*stretch*refStepX+i*refStepY+j*refStepX+0] = (uint8_t)(it_patch[x]);
161 | if(drawImg.channels() == 3){
162 | img_ptr[x*stretch*refStepX+i*refStepY+j*refStepX+1] = (uint8_t)(it_patch[x]);
163 | img_ptr[x*stretch*refStepX+i*refStepY+j*refStepX+2] = (uint8_t)(it_patch[x]);
164 | }
165 | }
166 | }
167 | }
168 | }
169 |
170 | /** \brief Draws the patch borders into an image.
171 | *
172 | * @param drawImg - Image in which the patch borders should be drawn.
173 | * @param c - Coordinates of the patch in the reference image.
174 | * @param s - Scaling factor.
175 | * @param color - Line color.
176 | */
177 | void drawPatchBorder(cv::Mat& drawImg,const FeatureCoordinates& c,const float s, const cv::Scalar& color) const{
178 | const double half_length = s*patchSize/2;
179 | if(c.isInFront() && c.com_warp_c()){
180 | cv::Point2f c1 = c.get_patchCorner(half_length,half_length).get_c();
181 | cv::Point2f c2 = c.get_patchCorner(half_length,-half_length).get_c();
182 | cv::line(drawImg,c1,c2,color,1);
183 | c1 = c.get_patchCorner(-half_length,-half_length).get_c();
184 | cv::line(drawImg,c2,c1,color,1);
185 | c2 = c.get_patchCorner(-half_length,half_length).get_c();
186 | cv::line(drawImg,c1,c2,color,1);
187 | c1 = c.get_patchCorner(half_length,half_length).get_c();
188 | cv::line(drawImg,c2,c1,color,1);
189 | }
190 | }
191 |
192 | /** \brief Checks if a patch at a specific image location is still within the reference image.
193 | *
194 | * @param img - Reference Image.
195 | * @param c - Coordinates of the patch in the reference image.
196 | * @param withBorder - Check, using either the patch-patchSize of Patch::patch_ (withBorder = false) or the patch-patchSize
197 | * of the expanded patch Patch::patchWithBorder_ (withBorder = true).
198 | * @return true, if the patch is completely located within the reference image.
199 | */
200 | static bool isPatchInFrame(const cv::Mat& img,const FeatureCoordinates& c,const bool withBorder = false){
201 | if(c.isInFront() && c.com_warp_c()){
202 | const int halfpatch_size = patchSize/2+(int)withBorder;
203 | if(c.isNearIdentityWarping()){
204 | if(c.get_c().x < halfpatch_size || c.get_c().y < halfpatch_size || c.get_c().x > 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<RobocentricFeatureElement,RobocentricFeatureElement,3>{
39 | public:
40 | typedef LWF::ElementBase<RobocentricFeatureElement,RobocentricFeatureElement,3> 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<double> 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<typename FILTERSTATE>
47 | class RovioFilter:public LWF::FilterBase<ImuPrediction<FILTERSTATE>,
48 | ImgUpdate<FILTERSTATE>,
49 | PoseUpdate<FILTERSTATE,(int)(FILTERSTATE::mtState::nPose_>0)-1,(int)(FILTERSTATE::mtState::nPose_>1)*2-1>,
50 | VelocityUpdate<FILTERSTATE>>{
51 | public:
52 | typedef LWF::FilterBase<ImuPrediction<FILTERSTATE>,
53 | ImgUpdate<FILTERSTATE>,
54 | PoseUpdate<FILTERSTATE,(int)(FILTERSTATE::mtState::nPose_>0)-1,(int)(FILTERSTATE::mtState::nPose_>1)*2-1>,
55 | VelocityUpdate<FILTERSTATE>> 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<mtState::nCam_> 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<mtState::nCam_;camID++){
93 | cameraCalibrationFile_[camID] = "";
94 | stringRegister_.registerScalar("Camera" + std::to_string(camID) + ".CalibrationFile",cameraCalibrationFile_[camID]);
95 | doubleRegister_.registerVector("Camera" + std::to_string(camID) + ".MrMC",init_.state_.aux().MrMC_[camID]);
96 | doubleRegister_.registerQuaternion("Camera" + std::to_string(camID) + ".qCM",init_.state_.aux().qCM_[camID]);
97 | doubleRegister_.removeScalarByVar(init_.state_.MrMC(camID)(0));
98 | doubleRegister_.removeScalarByVar(init_.state_.MrMC(camID)(1));
99 | doubleRegister_.removeScalarByVar(init_.state_.MrMC(camID)(2));
100 | doubleRegister_.removeScalarByVar(init_.state_.qCM(camID).toImplementation().w());
101 | doubleRegister_.removeScalarByVar(init_.state_.qCM(camID).toImplementation().x());
102 | doubleRegister_.removeScalarByVar(init_.state_.qCM(camID).toImplementation().y());
103 | doubleRegister_.removeScalarByVar(init_.state_.qCM(camID).toImplementation().z());
104 | for(int j=0;j<3;j++){
105 | doubleRegister_.removeScalarByVar(init_.cov_(mtState::template getId<mtState::_vep>(camID)+j,mtState::template getId<mtState::_vep>(camID)+j));
106 | doubleRegister_.removeScalarByVar(init_.cov_(mtState::template getId<mtState::_vea>(camID)+j,mtState::template getId<mtState::_vea>(camID)+j));
107 | doubleRegister_.registerScalar("Init.Covariance.vep",init_.cov_(mtState::template getId<mtState::_vep>(camID)+j,mtState::template getId<mtState::_vep>(camID)+j));
108 | doubleRegister_.registerScalar("Init.Covariance.vea",init_.cov_(mtState::template getId<mtState::_vea>(camID)+j,mtState::template getId<mtState::_vea>(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<mtState::nPose_;i++){
114 | doubleRegister_.removeScalarByVar(init_.state_.poseLin(i)(0));
115 | doubleRegister_.removeScalarByVar(init_.state_.poseLin(i)(1));
116 | doubleRegister_.removeScalarByVar(init_.state_.poseLin(i)(2));
117 | doubleRegister_.removeScalarByVar(init_.state_.poseRot(i).toImplementation().w());
118 | doubleRegister_.removeScalarByVar(init_.state_.poseRot(i).toImplementation().x());
119 | doubleRegister_.removeScalarByVar(init_.state_.poseRot(i).toImplementation().y());
120 | doubleRegister_.removeScalarByVar(init_.state_.poseRot(i).toImplementation().z());
121 | for(int j=0;j<3;j++){
122 | doubleRegister_.removeScalarByVar(init_.cov_(mtState::template getId<mtState::_pop>(i)+j,mtState::template getId<mtState::_pop>(i)+j));
123 | doubleRegister_.removeScalarByVar(init_.cov_(mtState::template getId<mtState::_poa>(i)+j,mtState::template getId<mtState::_poa>(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<mtState::_pop>(std::get<1>(mUpdates_).inertialPoseIndex_)+j,mtState::template getId<mtState::_pop>(std::get<1>(mUpdates_).inertialPoseIndex_)+j));
131 | std::get<1>(mUpdates_).doubleRegister_.registerScalar("init_cov_qWI",init_.cov_(mtState::template getId<mtState::_poa>(std::get<1>(mUpdates_).inertialPoseIndex_)+j,mtState::template getId<mtState::_poa>(std::get<1>(mUpdates_).inertialPoseIndex_)+j));
132 | std::get<1>(mUpdates_).doubleRegister_.registerScalar("pre_cov_IrIW",mPrediction_.prenoiP_(mtPrediction::mtNoise::template getId<mtPrediction::mtNoise::_pop>(std::get<1>(mUpdates_).inertialPoseIndex_)+j,mtPrediction::mtNoise::template getId<mtPrediction::mtNoise::_pop>(std::get<1>(mUpdates_).inertialPoseIndex_)+j));
133 | std::get<1>(mUpdates_).doubleRegister_.registerScalar("pre_cov_qWI",mPrediction_.prenoiP_(mtPrediction::mtNoise::template getId<mtPrediction::mtNoise::_poa>(std::get<1>(mUpdates_).inertialPoseIndex_)+j,mtPrediction::mtNoise::template getId<mtPrediction::mtNoise::_poa>(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<mtState::_pop>(std::get<1>(mUpdates_).bodyPoseIndex_)+j,mtState::template getId<mtState::_pop>(std::get<1>(mUpdates_).bodyPoseIndex_)+j));
141 | std::get<1>(mUpdates_).doubleRegister_.registerScalar("init_cov_qVM",init_.cov_(mtState::template getId<mtState::_poa>(std::get<1>(mUpdates_).bodyPoseIndex_)+j,mtState::template getId<mtState::_poa>(std::get<1>(mUpdates_).bodyPoseIndex_)+j));
142 | std::get<1>(mUpdates_).doubleRegister_.registerScalar("pre_cov_MrMV",mPrediction_.prenoiP_(mtPrediction::mtNoise::template getId<mtPrediction::mtNoise::_pop>(std::get<1>(mUpdates_).bodyPoseIndex_)+j,mtPrediction::mtNoise::template getId<mtPrediction::mtNoise::_pop>(std::get<1>(mUpdates_).bodyPoseIndex_)+j));
143 | std::get<1>(mUpdates_).doubleRegister_.registerScalar("pre_cov_qVM",mPrediction_.prenoiP_(mtPrediction::mtNoise::template getId<mtPrediction::mtNoise::_poa>(std::get<1>(mUpdates_).bodyPoseIndex_)+j,mtPrediction::mtNoise::template getId<mtPrediction::mtNoise::_poa>(std::get<1>(mUpdates_).bodyPoseIndex_)+j));
144 | }
145 | }
146 | int ind;
147 | for(int i=0;i<FILTERSTATE::mtState::nMax_;i++){
148 | ind = mtState::template getId<mtState::_fea>(i);
149 | doubleRegister_.removeScalarByVar(init_.cov_(ind,ind));
150 | doubleRegister_.removeScalarByVar(init_.cov_(ind+1,ind+1));
151 | ind = mtState::template getId<mtState::_fea>(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;camID<mtState::nCam_;camID++){
184 | if (!cameraCalibrationFile_[camID].empty()) {
185 | multiCamera_.cameras_[camID].load(cameraCalibrationFile_[camID]);
186 | }
187 | }
188 | for(int i=0;i<FILTERSTATE::mtState::nMax_;i++){
189 | init_.state_.dep(i).setType(depthTypeInt_);
190 | }
191 | };
192 |
193 | /** \brief Destructor
194 | */
195 | virtual ~RovioFilter(){};
196 | // void resetToImuPose(V3D WrWM, QPD qMW, double t = 0.0){
197 | // init_.state_.initWithImuPose(WrWM,qMW);
198 | // reset(t);
199 | // }
200 |
201 | /** \brief Resets the filter with an accelerometer measurement.
202 | *
203 | * @param fMeasInit - Accelerometer measurement.
204 | * @param t - Current time.
205 | */
206 | void resetWithAccelerometer(const V3D& fMeasInit, double t = 0.0){
207 | init_.initWithAccelerometer(fMeasInit);
208 | reset(t);
209 | }
210 |
211 | /** \brief Resets the filter with an external pose.
212 | *
213 | * @param WrWM - Position Vector, pointing from the World-Frame to the IMU-Frame, expressed in World-Coordinates.
214 | * @param qMW - Quaternion, expressing World-Frame in IMU-Coordinates (World Coordinates->IMU 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 <memory>
35 |
36 | namespace rovio {
37 |
38 | template<typename FILTER>
39 | class RovioScene{
40 | public:
41 | typedef typename FILTER::mtFilterState mtFilterState;
42 | typedef typename mtFilterState::mtState mtState;
43 | std::shared_ptr<FILTER> mpFilter_;
44 |
45 | rovio::Scene mScene;
46 | cv::Mat patch_;
47 | std::shared_ptr<SceneObject> mpSensor_[mtState::nCam_];
48 | std::shared_ptr<SceneObject> mpGroundtruth_;
49 | std::shared_ptr<SceneObject> mpLines_[mtState::nCam_];
50 | std::shared_ptr<SceneObject> mpDepthVar_[mtState::nCam_];
51 | std::shared_ptr<SceneObject> 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<void()> f){
57 | mScene.addKeyboardCB(Key,f);
58 | }
59 | void addSpecialKeyboardCB(int Key, std::function<void()> 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<FILTER> mpFilter){
63 | initGlut(argc,argv,mScene);
64 | mScene.init(argc, argv,mVSFileName,mFSFileName);
65 | mpFilter_ = mpFilter;
66 |
67 | std::shared_ptr<SceneObject> 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<SceneObject> 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;camID<mtState::nCam_;camID++){
78 | mpSensor_[camID] = mScene.addSceneObject();
79 | mpSensor_[camID]->makeCoordinateFrame(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;i<mtState::nMax_;i++){
104 | mpPatches_[i] = mScene.addSceneObject();
105 | mpPatches_[i]->initTexture(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<float>();
121 | mpGroundtruth_->draw_ = true;
122 | } else {
123 | mpGroundtruth_->draw_ = false;
124 | }
125 |
126 | for(unsigned int i=0;i<mtState::nMax_;i++){
127 | mpPatches_[i]->draw_ = false;
128 | }
129 |
130 | for(unsigned int camID=0;camID<mtState::nCam_;camID++){
131 | mpSensor_[camID]->W_r_WB_ = state.WrWC(camID).template cast<float>();
132 | mpSensor_[camID]->q_BW_ = state.qCW(camID);
133 |
134 | std::vector<Eigen::Vector3f> points;
135 | std::vector<Eigen::Vector3f> 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;i<mtState::nMax_;i++){
142 | if(filterState.fsm_.isValid_[i] && filterState.fsm_.features_[i].mpCoordinates_->camID_ == camID){
143 | mpPatches_[i]->draw_ = true;
144 | d = state.dep(i);
145 | const double sigma = cov(mtState::template getId<mtState::_fea>(i)+2,mtState::template getId<mtState::_fea>(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<float>());
168 | mpLines_[camID]->prolonge(cornerVec[1].cast<float>());
169 | mpLines_[camID]->prolonge(cornerVec[1].cast<float>());
170 | mpLines_[camID]->prolonge(cornerVec[3].cast<float>());
171 | mpLines_[camID]->prolonge(cornerVec[3].cast<float>());
172 | mpLines_[camID]->prolonge(cornerVec[2].cast<float>());
173 | mpLines_[camID]->prolonge(cornerVec[2].cast<float>());
174 | mpLines_[camID]->prolonge(cornerVec[0].cast<float>());
175 |
176 | mpDepthVar_[camID]->prolonge(pos_far.cast<float>());
177 | mpDepthVar_[camID]->prolonge(pos_near.cast<float>());
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<float>());
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 <opencv2/features2d/features2d.hpp>
34 | #include "GL/glew.h"
35 | #include <GL/freeglut.h>
36 | #include <memory>
37 | #include <fstream>
38 | #include <functional>
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<int N>
103 | struct Arrayf{
104 | Arrayf(){
105 | for(int i=0;i<N;i++){
106 | data_[i] = 0.0f;
107 | }
108 | }
109 | Arrayf(const Eigen::Matrix<float,N,1>& in){
110 | fromEigen(in);
111 | }
112 | void fromEigen(const Eigen::Matrix<float,N,1>& in){
113 | for(int i=0;i<N;i++){
114 | data_[i] = in(i);
115 | }
116 | }
117 | Eigen::Matrix<float,N,1> toEigen(){
118 | Eigen::Matrix<float,N,1> out;
119 | for(int i=0;i<N;i++){
120 | out(i) = data_[i];
121 | }
122 | return out;
123 | }
124 | float data_[N];
125 | };
126 |
127 | struct Vertex
128 | {
129 | Vertex():
130 | pos_(Eigen::Vector3f(0.0f,0.0f,0.0f)),
131 | normal_(Eigen::Vector3f(0.0f,0.0f,1.0f)),
132 | color_(Eigen::Vector4f(1.0f,1.0f,1.0f,1.0f)){}
133 | Vertex(float x, float y, float z):
134 | pos_(Eigen::Vector3f(x,y,z)),
135 | normal_(Eigen::Vector3f(0.0f,0.0f,1.0f)),
136 | color_(Eigen::Vector4f(1.0f,1.0f,1.0f,1.0f)){}
137 | Vertex(const Eigen::Vector3f& vec):
138 | pos_(vec),
139 | normal_(Eigen::Vector3f(0.0f,0.0f,1.0f)),
140 | color_(Eigen::Vector4f(1.0f,1.0f,1.0f,1.0f)){}
141 | Arrayf<3> 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<Eigen::Vector3f>& line);
160 | void makePoints();
161 | void makePoints(const std::vector<Eigen::Vector3f>& points);
162 | void prolonge(const Eigen::Vector3f& point);
163 | void prolonge(const std::vector<Eigen::Vector3f>& 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<Vertex> vertices_;
177 | std::vector<unsigned int> 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<unsigned char, std::function<void()>> keyboardCallbacks_;
192 | std::map<int, std::function<void()>> 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<SceneObject> 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<void()> f);
203 | void KeyboardCB(unsigned char Key, int x, int y);
204 | void addKeyboardCB(unsigned char Key, std::function<void()> 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<std::shared_ptr<SceneObject>> 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<LWF::VectorElement<3>>{
42 | public:
43 | typedef LWF::State<LWF::VectorElement<3>> 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<VelocityUpdateMeasAuxiliary>{
62 | public:
63 | VelocityUpdateMeasAuxiliary(){
64 | };
65 | virtual ~VelocityUpdateMeasAuxiliary(){};
66 | };
67 |
68 | /** \brief Empty measurement
69 | */
70 | class VelocityUpdateMeas: public LWF::State<LWF::VectorElement<3>,VelocityUpdateMeasAuxiliary>{
71 | public:
72 | typedef LWF::State<LWF::VectorElement<3>,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<LWF::VectorElement<3>>{
93 | public:
94 | typedef LWF::State<LWF::VectorElement<3>> 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<Start entry, dimension of detection>
112 | */
113 | class VelocityOutlierDetection: public LWF::OutlierDetection<LWF::ODEntry<VelocityInnovation::template getId<VelocityInnovation::_vel>(),3>>{
114 | public:
115 | virtual ~VelocityOutlierDetection(){};
116 | };
117 |
118 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
119 |
120 | /** \brief Class, holding the zero velocity update
121 | */
122 | template<typename FILTERSTATE>
123 | class VelocityUpdate: public LWF::Update<VelocityInnovation,FILTERSTATE,VelocityUpdateMeas,
124 | VelocityUpdateNoise,VelocityOutlierDetection,false>{
125 | public:
126 | typedef LWF::Update<VelocityInnovation,FILTERSTATE,VelocityUpdateMeas,
127 | VelocityUpdateNoise,VelocityOutlierDetection,false> 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<mtInnovation::_vel>(),mtState::template getId<mtState::_vel>()) = 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<mtInnovation::_vel>(),mtNoise::template getId<mtNoise::_vel>()) = 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<typename STATE>
42 | class ZeroVelocityInnovation: public LWF::State<LWF::VectorElement<3>>{
43 | public:
44 | typedef LWF::State<LWF::VectorElement<3>> 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<ZeroVelocityUpdateMeasAuxiliary>{
57 | public:
58 | ZeroVelocityUpdateMeasAuxiliary(){
59 | };
60 | virtual ~ZeroVelocityUpdateMeasAuxiliary(){};
61 | };
62 |
63 | /** \brief Empty measurement
64 | */
65 | template<typename STATE>
66 | class ZeroVelocityUpdateMeas: public LWF::State<ZeroVelocityUpdateMeasAuxiliary>{
67 | public:
68 | typedef LWF::State<ZeroVelocityUpdateMeasAuxiliary> 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<typename STATE>
81 | class ZeroVelocityUpdateNoise: public LWF::State<LWF::VectorElement<3>>{
82 | public:
83 | typedef LWF::State<LWF::VectorElement<3>> 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<typename STATE>
96 | class ZeroVelocityOutlierDetection: public LWF::OutlierDetection<LWF::ODEntry<ZeroVelocityInnovation<STATE>::template getId<ZeroVelocityInnovation<STATE>::_vel>(),3>>{
97 | public:
98 | virtual ~ZeroVelocityOutlierDetection(){};
99 | };
100 |
101 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
102 |
103 | /** \brief Class, holding the zero velocity update
104 | */
105 | template<typename FILTERSTATE>
106 | class ZeroVelocityUpdate: public LWF::Update<ZeroVelocityInnovation<typename FILTERSTATE::mtState>,FILTERSTATE,ZeroVelocityUpdateMeas<typename FILTERSTATE::mtState>,
107 | ZeroVelocityUpdateNoise<typename FILTERSTATE::mtState>,ZeroVelocityOutlierDetection<typename FILTERSTATE::mtState>,false>{
108 | public:
109 | typedef LWF::Update<ZeroVelocityInnovation<typename FILTERSTATE::mtState>,FILTERSTATE,ZeroVelocityUpdateMeas<typename FILTERSTATE::mtState>,
110 | ZeroVelocityUpdateNoise<typename FILTERSTATE::mtState>,ZeroVelocityOutlierDetection<typename FILTERSTATE::mtState>,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<mtInnovation::_vel>() = state.MvM()+noise.template get<mtInnovation::_vel>();
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<mtInnovation::_vel>(),mtState::template getId<mtState::_vel>()) = 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<mtInnovation::_vel>(),mtNoise::template getId<mtNoise::_vel>()) = 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 <string>
33 | #include <iostream>
34 | #include <exception>
35 | #include <typeinfo>
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 | <?xml version="1.0" encoding="UTF-8"?>
2 | <launch>
3 | <node pkg="rovio" type="rovio_node" name="rovio" output="screen">
4 | <param name="filter_config" value="$(find rovio)/cfg/rovio.info"/>
5 | <param name="camera0_config" value="$(find rovio)/cfg/euroc_cam0.yaml"/>
6 | <param name="camera1_config" value="$(find rovio)/cfg/euroc_cam1.yaml"/>
7 | </node>
8 | </launch>
9 |
--------------------------------------------------------------------------------
/launch/rovio_rosbag_node.launch:
--------------------------------------------------------------------------------
1 | <?xml version="1.0" encoding="UTF-8"?>
2 | <launch>
3 | <node pkg="rovio" type="rovio_rosbag_loader" name="rovio" output="screen">
4 | <param name="filter_config" value="$(find rovio)/cfg/rovio.info"/>
5 | <param name="camera0_config" value="$(find rovio)/cfg/euroc_cam0.yaml"/>
6 | <param name="camera1_config" value="$(find rovio)/cfg/euroc_cam1.yaml"/>
7 | <param name="rosbag_filename" value="/home/michael/datasets/euroc/01_easy/dataset.bag"/>
8 | <param name="imu_topic_name" value="/imu0"/>
9 | <param name="cam0_topic_name" value="/cam0/image_raw"/>
10 | <param name="cam1_topic_name" value="/cam1/image_raw"/>
11 | </node>
12 | </launch>
--------------------------------------------------------------------------------
/launch/valgrind_rovio.launch:
--------------------------------------------------------------------------------
1 | <?xml version="1.0" encoding="UTF-8"?>
2 | <launch>
3 | <node pkg="rovio" type="rovio_node" name="rovio" output="screen" launch-prefix="valgrind --leak-check=full">
4 | </node>
5 | </launch>
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 | <?xml version="1.0"?>
2 | <package format="2">
3 | <name>rovio</name>
4 | <version>0.0.0</version>
5 | <description>
6 |
7 | rovio
8 |
9 | </description>
10 | <url></url>
11 | <author>bloeschm</author>
12 | <maintainer email="bloeschm@ethz.ch">bloeschm</maintainer>
13 | <license>BSD License</license>
14 | <buildtool_depend>catkin</buildtool_depend>
15 |
16 | <depend>cmake_modules</depend>
17 | <depend>lightweight_filtering</depend>
18 | <depend>kindr</depend>
19 | <depend>roscpp</depend>
20 | <depend>roslib</depend>
21 | <depend>cv_bridge</depend>
22 | <depend>message_generation</depend>
23 | <depend>message_runtime</depend>
24 | <depend>geometry_msgs</depend>
25 | <depend>sensor_msgs</depend>
26 | <depend>std_msgs</depend>
27 | <depend>nav_msgs</depend>
28 | <depend>tf</depend>
29 | <depend>rosbag</depend>
30 | <depend>yaml_cpp_catkin</depend>
31 | </package>
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<double,2,2> 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<float>();
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<double>();
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_<sigma2_){ // Get larger axis on index 1
188 | const double temp = sigma1_;
189 | sigma1_ = sigma2_;
190 | sigma2_ = temp;
191 | sigmaAngle_ += 0.5*M_PI;
192 | eigenVector1_ = es_.eigenvectors().col(1).real();
193 | eigenVector2_ = es_.eigenvectors().col(0).real();
194 | } else {
195 | eigenVector1_ = es_.eigenvectors().col(0).real();
196 | eigenVector2_ = es_.eigenvectors().col(1).real();
197 | }
198 | }
199 |
200 | void FeatureCoordinates::drawPoint(cv::Mat& drawImg, const cv::Scalar& color, const float s) const{
201 | cv::Size size(s,s);
202 | cv::ellipse(drawImg,get_c(),size,0,0,360,color,-1,8,0);
203 | }
204 |
205 | void FeatureCoordinates::drawEllipse(cv::Mat& drawImg, const cv::Scalar& color, double scaleFactor, const bool withCenterPoint) const{
206 | if(withCenterPoint) drawPoint(drawImg,color);
207 | cv::ellipse(drawImg,get_c(),cv::Size(std::max(static_cast<int>(scaleFactor*sigma1_+0.5),1),std::max(static_cast<int>(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<double,3,2> V;
235 | // V.col(0) = -C2v1;
236 | // V.col(1) = C2v2;
237 | // Eigen::JacobiSVD<Eigen::Matrix<double,3,2>> 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 <complex>
3 | #include <iostream>
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 <memory>
31 |
32 | #include <Eigen/StdVector>
33 |
34 | #pragma GCC diagnostic push
35 | #pragma GCC diagnostic ignored "-Wunused-parameter"
36 | #include <ros/ros.h>
37 | #include <ros/package.h>
38 | #include <geometry_msgs/Pose.h>
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<rovio::FilterState<nMax_,nLevels_,patchSize_,nCam_,nPose_>> mtFilter;
78 |
79 | #ifdef MAKE_SCENE
80 | static rovio::RovioScene<mtFilter> 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<mtFilter> 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<mtFilter> 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 <ros/package.h>
30 | #include <rosbag/bag.h>
31 | #include <rosbag/view.h>
32 | #include <memory>
33 | #include <iostream>
34 | #include <locale>
35 | #include <string>
36 | #include <Eigen/StdVector>
37 | #include "rovio/RovioFilter.hpp"
38 | #include "rovio/RovioNode.hpp"
39 | #include <boost/foreach.hpp>
40 | #include <boost/date_time/posix_time/posix_time.hpp>
41 | #include <boost/date_time/posix_time/posix_time_io.hpp>
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<rovio::FilterState<nMax_,nLevels_,patchSize_,nCam_,nPose_>> 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<mtFilter> 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<mtFilter> 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<std::string> 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;camID<mtFilter::mtState::nCam_;camID++){
166 | extrinsics_topic_name[camID] = rovioNode.pubExtrinsics_[camID].getTopic();
167 | }
168 | std::string imu_bias_topic_name = rovioNode.pubImuBias_.getTopic();
169 | std::string pcl_topic_name = rovioNode.pubPcl_.getTopic();
170 | std::string u_rays_topic_name = rovioNode.pubMarkers_.getTopic();
171 | std::string patch_topic_name = rovioNode.pubPatch_.getTopic();
172 |
173 | topics.push_back(std::string(imu_topic_name));
174 | topics.push_back(std::string(cam0_topic_name));
175 | topics.push_back(std::string(cam1_topic_name));
176 | rosbag::View view(bagIn, rosbag::TopicQuery(topics));
177 |
178 |
179 | bool isTriggerInitialized = false;
180 | double lastTriggerTime = 0.0;
181 | for(rosbag::View::iterator it = view.begin();it != view.end() && ros::ok();it++){
182 | if(it->getTopic() == imu_topic_name){
183 | sensor_msgs::Imu::ConstPtr imuMsg = it->instantiate<sensor_msgs::Imu>();
184 | if (imuMsg != NULL) rovioNode.imuCallback(imuMsg);
185 | }
186 | if(it->getTopic() == cam0_topic_name){
187 | sensor_msgs::ImageConstPtr imgMsg = it->instantiate<sensor_msgs::Image>();
188 | if (imgMsg != NULL) rovioNode.imgCallback0(imgMsg);
189 | }
190 | if(it->getTopic() == cam1_topic_name){
191 | sensor_msgs::ImageConstPtr imgMsg = it->instantiate<sensor_msgs::Image>();
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;camID<mtFilter::mtState::nCam_;camID++){
202 | if(rovioNode.forceExtrinsicsPublishing_) bagOut.write(extrinsics_topic_name[camID],ros::Time::now(),rovioNode.extrinsicsMsg_[camID]);
203 | }
204 | if(rovioNode.forceImuBiasPublishing_) bagOut.write(imu_bias_topic_name,ros::Time::now(),rovioNode.imuBiasMsg_);
205 | if(rovioNode.forcePclPublishing_) bagOut.write(pcl_topic_name,ros::Time::now(),rovioNode.pclMsg_);
206 | if(rovioNode.forceMarkersPublishing_) bagOut.write(u_rays_topic_name,ros::Time::now(),rovioNode.markerMsg_);
207 | if(rovioNode.forcePatchPublishing_) bagOut.write(patch_topic_name,ros::Time::now(),rovioNode.patchMsg_);
208 | lastSafeTime = rovioNode.mpFilter_->safe_.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 <assert.h>
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<patchSize_> 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;i<imgSize_;i++){
23 | for(int j=0;j<imgSize_;j++, ++img_ptr){
24 | *img_ptr = i*dy_+j*dx_;
25 | }
26 | }
27 | img2_ = cv::Mat::zeros(imgSize_,imgSize_,CV_8UC1);
28 | img_ptr = (uint8_t*) img2_.data;
29 | for(int i=0;i<imgSize_;i++){
30 | for(int j=0;j<imgSize_;j++, ++img_ptr){
31 | if(j<imgSize_/2 & i<imgSize_/2){
32 | *img_ptr = 0;
33 | } else {
34 | *img_ptr = 255;
35 | }
36 | }
37 | }
38 | }
39 | virtual ~PatchTesting() {}
40 | };
41 |
42 | // Test constructors
43 | TEST_F(PatchTesting, constructors) {
44 | Patch<(this->patchSize_)> 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<N;n++){
177 | c_.set_c(cv::Point2f(patchSize_/2+1,patchSize_/2+1)+patchOrigins[n]);
178 | c_.set_warp_identity();
179 | p_.extractPatchFromImage(img1_,c_,true);
180 | float* patch_ptr = p_.patchWithBorder_;
181 | for(int i=0;i<patchSize_+2;i++){
182 | for(int j=0;j<patchSize_+2;j++, ++patch_ptr){
183 | ASSERT_NEAR(*patch_ptr,(float)((i+patchOrigins[n].y)*dy_+(j+patchOrigins[n].x)*dx_),1e-6);
184 | }
185 | }
186 | }
187 | }
188 |
189 | // Test extractWarpedPatchFromImage
190 | TEST_F(PatchTesting, extractWarpedPatchFromImage) {
191 | Eigen::Matrix2f aff;
192 | aff << cos(M_PI/6.0), -sin(M_PI/6.0), sin(M_PI/6.0), cos(M_PI/6.0);
193 | c_.set_warp_c(aff);
194 | c_.set_c(cv::Point2f(imgSize_/2,imgSize_/2));
195 | p_.extractPatchFromImage(img1_,c_,true);
196 | float* patch_ptr = p_.patchWithBorder_;
197 | for(int i=0;i<patchSize_+2;i++){
198 | for(int j=0;j<patchSize_+2;j++, ++patch_ptr){
199 | ASSERT_NEAR(*patch_ptr,((cos(M_PI/6.0)*(i-patchSize_/2-0.5)+sin(M_PI/6.0)*(j-patchSize_/2-0.5)+0.5*imgSize_-0.5)*dy_
200 | +(-sin(M_PI/6.0)*(i-patchSize_/2-0.5)+cos(M_PI/6.0)*(j-patchSize_/2-0.5)+0.5*imgSize_-0.5)*dx_),1e-4);
201 | }
202 | }
203 | }
204 |
205 | // Test extractPatchFromPatchWithBorder
206 | TEST_F(PatchTesting, extractPatchFromPatchWithBorder) {
207 | c_.set_c(cv::Point2f(patchSize_/2+1,patchSize_/2+1));
208 | c_.set_warp_identity();
209 | p_.extractPatchFromImage(img1_,c_,true);
210 | p_.extractPatchFromPatchWithBorder();
211 | float* patch_ptr = p_.patch_;
212 | for(int i=0;i<patchSize_;i++){
213 | for(int j=0;j<patchSize_;j++, ++patch_ptr){
214 | ASSERT_EQ(*patch_ptr,(float)((i+1)*dy_+(j+1)*dx_));
215 | }
216 | }
217 | }
218 |
219 | // Test computeGradientParameters
220 | TEST_F(PatchTesting, computeGradientParameters) {
221 | c_.set_c(cv::Point2f(patchSize_/2+1,patchSize_/2+1+0.5));
222 | c_.set_warp_identity();
223 | p_.extractPatchFromImage(img1_,c_,true);
224 | p_.computeGradientParameters();
225 | float* it_dx = p_.dx_;
226 | float* it_dy = p_.dy_;
227 | for(int i=0;i<patchSize_;i++){
228 | for(int j=0;j<patchSize_;j++, ++it_dx, ++it_dy){
229 | ASSERT_EQ(*it_dx,float(dx_));
230 | ASSERT_EQ(*it_dy,float(dy_));
231 | }
232 | }
233 | ASSERT_EQ(p_.H_(0,0),dx_*dx_*patchSize_*patchSize_);
234 | ASSERT_EQ(p_.H_(0,1),dx_*dy_*patchSize_*patchSize_);
235 | ASSERT_EQ(p_.H_(0,2),dx_*patchSize_*patchSize_);
236 | ASSERT_EQ(p_.H_(1,0),dx_*dy_*patchSize_*patchSize_);
237 | ASSERT_EQ(p_.H_(1,1),dy_*dy_*patchSize_*patchSize_);
238 | ASSERT_EQ(p_.H_(1,2),dy_*patchSize_*patchSize_);
239 | ASSERT_EQ(p_.H_(2,0),dx_*patchSize_*patchSize_);
240 | ASSERT_EQ(p_.H_(2,1),dy_*patchSize_*patchSize_);
241 | ASSERT_EQ(p_.H_(2,2),patchSize_*patchSize_);
242 |
243 | c_.set_c(cv::Point2f(imgSize_/2,imgSize_/2));
244 | c_.set_warp_identity();
245 | p_.extractPatchFromImage(img2_,c_,true);
246 | p_.computeGradientParameters();
247 | double s = 0.5 * (patchSize_ + patchSize_ - sqrtf((patchSize_ + patchSize_) * (patchSize_ + patchSize_) - 4 * (patchSize_ * patchSize_ - 1 * 1)))
248 | + 0.5 * (patchSize_ + patchSize_ + sqrtf((patchSize_ + patchSize_) * (patchSize_ + patchSize_) - 4 * (patchSize_ * patchSize_ - 1 * 1)));
249 | s = s*(255*0.5)*(255*0.5)/(patchSize_*patchSize_);
250 | ASSERT_EQ(p_.s_,s);
251 | }
252 |
253 | int main(int argc, char **argv) {
254 | ::testing::InitGoogleTest(&argc, argv);
255 | return RUN_ALL_TESTS();
256 | }
257 |
--------------------------------------------------------------------------------
/srv/SrvResetToPose.srv:
--------------------------------------------------------------------------------
1 | # Initial IMU pose after reset.
2 | # T_WM transforms points from IMU frame (M) to World frame (W)
3 | # Conventions: W_p = T_WM * M_p
4 | # Check wiki page "Coordinate Frames and Notation" for more information about frames
5 | geometry_msgs/Pose T_WM
6 | ---
7 | std_msgs/Empty nothing
--------------------------------------------------------------------------------