├── .gitignore ├── CMakeLists.txt ├── FindEigen3.cmake ├── README.md ├── apps ├── CMakeLists.txt └── pyinterface.cpp ├── images ├── flow_image.png ├── flow_image_combined.png └── flow_topdown.png ├── include └── libviso2 │ ├── filter.h │ ├── matcher.h │ ├── matrix.h │ ├── reconstruction.h │ ├── timer.h │ ├── triangle.h │ ├── viso.h │ ├── viso_mono.h │ └── viso_stereo.h ├── python ├── python_example_flow.py └── python_example_vo.py └── src ├── libkitti ├── kitti.cpp └── kitti.h ├── libviso2 ├── filter.cpp ├── matcher.cpp ├── matrix.cpp ├── reconstruction.cpp ├── triangle.cpp ├── viso.cpp ├── viso_mono.cpp └── viso_stereo.cpp └── sparseflow ├── camera.cpp ├── camera.h ├── utils_flow.cpp └── utils_flow.h /.gitignore: -------------------------------------------------------------------------------- 1 | *.idea 2 | *.txt 3 | *.cmake 4 | *.out 5 | cmake-build-* 6 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | project(sparse-scene-flow) 3 | 4 | # Build Options 5 | option(SHOW_PATH "Show path of libs and headers" ON) 6 | #set(OpenCV_DIR "/home/osep/local/share/OpenCV" CACHE PATH "Path to local OpenCV e.g. USER/local/share/OpenCV") 7 | 8 | # Configuration 9 | # Compiler Flags 10 | set(CMAKE_CXX_COMPILER_FLAGS "${CMAKE_CXX_COMPILER_FLAGS} -Wall") # Enables all compiler warnings 11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") # Generate position independent code 12 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") # Enable c++11 13 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -msse3") 14 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g") 15 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") 16 | set(CMAKE_CXX_FLAGS_DEBUG ${CMAKE_CXX_FLAGS_DEBUG}) 17 | set(CMAKE_CXX_FLAGS_RELEASE ${CMAKE_CXX_FLAGS_RELEASE}) 18 | 19 | #find_package(Boost 1.58.0 REQUIRED COMPONENTS program_options filesystem) 20 | #find_package(OpenCV 3.2.0 REQUIRED COMPONENTS core highgui imgproc calib3d) 21 | find_package(pybind11 REQUIRED) 22 | 23 | list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}") 24 | find_package(Eigen3 REQUIRED) 25 | 26 | include_directories(${EIGEN3_INCLUDE_DIR}) 27 | include_directories(include ${PROJECT_SOURCE_DIR}) 28 | 29 | # Include own files 30 | file(GLOB PROJ_SRC_FILES ${CMAKE_SOURCE_DIR}/src/*.cpp) 31 | include_directories(${CMAKE_SOURCE_DIR}/src) 32 | 33 | file(GLOB SFLOW_SRC_FILES ${CMAKE_SOURCE_DIR}/src/sparseflow/*.cpp) 34 | set(SFLOW_SRC_DIR ${CMAKE_SOURCE_DIR}/src/sparseflow) 35 | include_directories(${SFLOW_SRC_DIR}) 36 | 37 | # Libraries 38 | add_library (viso2 39 | src/libviso2/matrix.cpp 40 | src/libviso2/viso.cpp 41 | src/libviso2/viso_mono.cpp 42 | src/libviso2/viso_stereo.cpp 43 | src/libviso2/matcher.cpp 44 | src/libviso2/reconstruction.cpp 45 | src/libviso2/triangle.cpp 46 | src/libviso2/filter.cpp 47 | 48 | include/libviso2/viso.h 49 | include/libviso2/viso_mono.h 50 | include/libviso2/viso_stereo.h 51 | include/libviso2/matcher.h 52 | include/libviso2/reconstruction.h 53 | include/libviso2/triangle.h 54 | include/libviso2/filter.h 55 | ) 56 | 57 | add_library (kitti 58 | src/libkitti/kitti.cpp 59 | src/libkitti/kitti.h 60 | ) 61 | 62 | target_link_libraries(viso2) 63 | target_link_libraries(kitti) 64 | 65 | add_subdirectory(apps) 66 | 67 | # Debug infos 68 | if(SHOW_PATH) 69 | # message(STATUS "Boost_LIBRARIES: " ${Boost_LIBRARIES}) 70 | # message(STATUS ${OpenCV_LIBS}) 71 | message(STATUS "Project Source Dirs: " ${PROJECT_SOURCE_DIR}) 72 | message(STATUS "OpenCV Include Dirs: " ${OpenCV_INCLUDE_DIRS}) 73 | message(STATUS "Eigen Include Dirs: " ${EIGEN3_INCLUDE_DIR}) 74 | endif() 75 | -------------------------------------------------------------------------------- /FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | # 13 | # This module reads hints about search locations from 14 | # the following enviroment variables: 15 | # 16 | # EIGEN3_ROOT 17 | # EIGEN3_ROOT_DIR 18 | 19 | # Copyright (c) 2006, 2007 Montel Laurent, 20 | # Copyright (c) 2008, 2009 Gael Guennebaud, 21 | # Copyright (c) 2009 Benoit Jacob 22 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 23 | 24 | if(NOT Eigen3_FIND_VERSION) 25 | if(NOT Eigen3_FIND_VERSION_MAJOR) 26 | set(Eigen3_FIND_VERSION_MAJOR 2) 27 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 28 | if(NOT Eigen3_FIND_VERSION_MINOR) 29 | set(Eigen3_FIND_VERSION_MINOR 91) 30 | endif(NOT Eigen3_FIND_VERSION_MINOR) 31 | if(NOT Eigen3_FIND_VERSION_PATCH) 32 | set(Eigen3_FIND_VERSION_PATCH 0) 33 | endif(NOT Eigen3_FIND_VERSION_PATCH) 34 | 35 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 36 | endif(NOT Eigen3_FIND_VERSION) 37 | 38 | macro(_eigen3_check_version) 39 | # file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 40 | 41 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 42 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 43 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 44 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 45 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 46 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 47 | 48 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 49 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 50 | set(EIGEN3_VERSION_OK FALSE) 51 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 52 | set(EIGEN3_VERSION_OK TRUE) 53 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 54 | 55 | if(NOT EIGEN3_VERSION_OK) 56 | 57 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 58 | "but at least version ${Eigen3_FIND_VERSION} is required") 59 | endif(NOT EIGEN3_VERSION_OK) 60 | endmacro(_eigen3_check_version) 61 | 62 | if (EIGEN3_INCLUDE_DIR) 63 | 64 | # in cache already 65 | _eigen3_check_version() 66 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 67 | 68 | else (EIGEN3_INCLUDE_DIR) 69 | 70 | # search first if an Eigen3Config.cmake is available in the system, 71 | # if successful this would set EIGEN3_INCLUDE_DIR and the rest of 72 | # the script will work as usual 73 | find_package(Eigen3 ${Eigen3_FIND_VERSION} NO_MODULE QUIET) 74 | 75 | if(NOT EIGEN3_INCLUDE_DIR) 76 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 77 | HINTS 78 | ENV EIGEN3_ROOT 79 | ENV EIGEN3_ROOT_DIR 80 | PATHS 81 | ${CMAKE_INSTALL_PREFIX}/include 82 | ${KDE4_INCLUDE_DIR} 83 | PATH_SUFFIXES eigen3 eigen 84 | ) 85 | endif(NOT EIGEN3_INCLUDE_DIR) 86 | 87 | if(EIGEN3_INCLUDE_DIR) 88 | _eigen3_check_version() 89 | endif(EIGEN3_INCLUDE_DIR) 90 | 91 | include(FindPackageHandleStandardArgs) 92 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 93 | 94 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 95 | 96 | endif(EIGEN3_INCLUDE_DIR) 97 | 98 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Sparse Scene Flow 2 | 3 | This repository contains code for sparse scene flow estimation using stereo cameras, proposed by P. Lenz etal.: Sparse Scene Flow Segmentation for Moving Object Detection in 4 | Urban Environments, Intelligent Vehicles Symposium (IV), 2011. 5 | This method can be used as a component in your 6 | visual object tracking / 3D reconstruction / SLAM applications 7 | as an alternative to dense (and typically expensive to compute) scene flow methods. 8 | 9 | Note: The repository contains scene flow estimator only, there is no implementation for scene flow clustering or object tracking provided in this repository. 10 | 11 | ![Alt text](images/flow_image_combined.png?raw=true "Scene flow, image-view and top-down view.") 12 | 13 | If you want to know what is the difference between scene and optical flow, 14 | see [this quora thread](https://www.quora.com/What-is-the-difference-between-scene-flow-and-optical-flow). 15 | ## Demo Video 16 | [Click here to watch the video](https://www.youtube.com/watch?v=SavxW1UuGKM). 17 | 18 | ## Prerequisite 19 | In order to run the code, your setup has to meet the following minimum requirements (tested versions in parentheses. Other versions might work, too): 20 | 21 | * GCC 4.8.4 22 | * Eigen (3.x) 23 | * pybind11 24 | 25 | ## Install 26 | ### Compiling the code using CMake 27 | 0. `mkdir build` 28 | 0. `cmake ..` 29 | 0. `make all` 30 | 31 | ### Running the sparse flow app 32 | 0. Download KITTI 33 | 0. See python/python_example.py to see how to use visual odometry estimator 34 | 35 | ## Remarks 36 | * External libraries 37 | * The tracker ships the following external modules: 38 | * **libviso2** - egomotion estimation, feature matching (http://www.cvlibs.net/software/libviso/) 39 | 40 | * For optimal performance, run the sf-estimator in `release` mode. 41 | 42 | UPDATE (Jan'20): I added bindings for python and removed most of the "old" exmaple code in order to shrink the dependencies to the minimum. See the python example. 43 | 44 | If you have any issues or questions about the code, please contact me https://www.vision.rwth-aachen.de/person/13/ 45 | 46 | ## Citing 47 | 48 | If you find this code useful in your research, you should cite: 49 | 50 | @inproceedings{Lenz2011IV, 51 | author = {Philip Lenz and Julius Ziegler and Andreas Geiger and Martin Roser}, 52 | title = {Sparse Scene Flow Segmentation for Moving Object Detection in Urban Environments}, 53 | booktitle = {Intelligent Vehicles Symposium (IV)}, 54 | year = {2011} 55 | } 56 | 57 | ## License 58 | 59 | GNU General Public License (http://www.gnu.org/licenses/gpl.html) 60 | 61 | Copyright (c) 2017 Aljosa Osep 62 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 63 | 64 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 65 | 66 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 67 | -------------------------------------------------------------------------------- /apps/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # add_executable(sparseflow ${PROJ_SRC_FILES} ${SFLOW_SRC_FILES} "sparseflow.cpp") 2 | # target_link_libraries(sparseflow ${OpenCV_LIBS} ${Boost_LIBRARIES} kitti viso2) 3 | # install(TARGETS sparseflow DESTINATION bin) 4 | 5 | pybind11_add_module(pyinterface pyinterface.cpp) 6 | target_link_libraries(pyinterface PUBLIC ${Boost_LIBRARIES} viso2) -------------------------------------------------------------------------------- /apps/pyinterface.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2017. All rights reserved. 3 | Computer Vision Group, Visual Computing Institute 4 | Technical University Munich, Germany 5 | 6 | This file is part of the rwth_mot framework. 7 | Authors: Aljosa Osep (aljosa.osep -at- tum.de) 8 | 9 | rwth_mot framework is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 3 of the License, or any later version. 12 | 13 | rwth_mot framework is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | rwth_mot framework; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #include 23 | 24 | // libviso 25 | #include 26 | 27 | // Eigen 28 | #include 29 | 30 | // pybind 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | int add(int i, int j) { 37 | return i + j; 38 | } 39 | 40 | // Note: images get allocated, addresses returned via references 41 | // User needs to clear mem adter this 42 | void pybind_to_raw(const pybind11::array_t &left1, uint8_t*& left_img_1, int &rows, int &cols) { 43 | 44 | auto left1_buff = left1.unchecked<2>(); // x must have ndim = 3; can be non-writeable 45 | rows = left1_buff.shape(0); 46 | cols = left1_buff.shape(1); 47 | 48 | // TODO: check that dims match 49 | 50 | // Alloc buffers for 4 images 51 | left_img_1 = (uint8_t*)malloc(cols*rows*sizeof(uint8_t)); 52 | int32_t k=0; 53 | for (ssize_t i = 0; i 64 | GetMatches(libviso2::Matcher *M, uint8_t *left_img_data, uint8_t *right_img_data, int rows, int cols, bool only_push) { 65 | int32_t dims[] = {cols, rows, cols}; 66 | 67 | // Push images 68 | M->pushBack(left_img_data, right_img_data, dims, false); 69 | 70 | std::vector matches; 71 | if (!only_push) { 72 | // do matching 73 | M->matchFeatures(2); // 2 ... quad matching 74 | 75 | // Get matches 76 | // quad matching 77 | matches = M->getMatches(); 78 | } 79 | 80 | return matches; 81 | } 82 | 83 | Eigen::Vector4d ProjTo3D_(float u1, float u2, float v, float f, float cu, float cv, float baseline) { 84 | 85 | const double d = std::fabs(u2 - u1); // Disparity 86 | 87 | Eigen::Vector4d ret(std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 88 | std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); 89 | 90 | if (d > 0.0001) { 91 | double X = ((u1 - cu) * baseline) / d; 92 | double Y = ((v - cv) * baseline) / d; 93 | double Z = f * baseline / d; 94 | ret = Eigen::Vector4d(X, Y, Z, 1.0); 95 | } 96 | 97 | return ret; 98 | } 99 | 100 | std::vector GetSceneFlow(std::vector quad_matches, 101 | const Eigen::Matrix4d Tr, 102 | float f, float cu, float cv, float baseline, float dt, float max_velocity_ms) { 103 | 104 | std::vector velocity_info; 105 | 106 | // Project matches to 3D 107 | for (const auto &match:quad_matches) { 108 | 109 | Eigen::Vector4d p3d_c = ProjTo3D_(match.u1c, match.u2c, match.v1c, f, cu, cv, baseline); // p3d curr frame 110 | Eigen::Vector4d p3d_p = ProjTo3D_(match.u1p, match.u2p, match.v1p, f, cu, cv, baseline); // p3d prev frame 111 | 112 | if (std::isnan(p3d_c[0]) || std::isnan(p3d_p[0])) { 113 | continue; 114 | } 115 | 116 | const Eigen::Vector4d p3d_c_orig = p3d_c; 117 | const Eigen::Vector4d p3d_p_orig = p3d_p; 118 | 119 | // Project prev to curr frame using ego estimate 120 | p3d_p = Tr * p3d_p; 121 | 122 | // Project to ground 123 | //p3d_c.head<3>() = camera.ground_model()->ProjectPointToGround(p3d_c.head<3>()); 124 | //p3d_p.head<3>() = camera.ground_model()->ProjectPointToGround(p3d_p.head<3>()); 125 | 126 | Eigen::Vector3d delta = (p3d_c - p3d_p).head<3>(); 127 | SUN::utils::scene_flow::VelocityInfo vi; 128 | vi.p = Eigen::Vector2i(match.u1c, match.v1c); 129 | vi.p_3d = p3d_c_orig.head<3>(); 130 | vi.p_vel = delta; 131 | vi.p_prev = p3d_p_orig.head<3>(); 132 | vi.p_prev_to_curr = p3d_p.head<3>(); 133 | velocity_info.push_back(vi); 134 | 135 | // float max_dist = 20.0; //90; 136 | // float max_lat = 15.0; //30; 137 | // if (std::fabs(p3d_c[0]) > max_lat || std::fabs(p3d_c[2]) > max_dist || std::fabs(p3d_p[0]) > max_lat || std::fabs(p3d_p[2]) > max_dist) { 138 | // continue; 139 | // } 140 | // 141 | // Eigen::Vector3d delta = (p3d_c - p3d_p).head<3>(); 142 | // 143 | // if (delta.norm()*(1.0/dt) < max_velocity_ms) { 144 | // //velocity_map.at(match.v1c, match.u1c) = cv::Vec3f(delta[0], delta[1], delta[2]); 145 | // SUN::utils::scene_flow::VelocityInfo vi; 146 | // vi.p = Eigen::Vector2i(match.u1c, match.v1c); 147 | // vi.p_3d = p3d_c_orig.head<3>(); 148 | // vi.p_vel = delta; 149 | // vi.p_prev = p3d_p_orig.head<3>(); 150 | // vi.p_prev_to_curr = p3d_p.head<3>(); 151 | // velocity_info.push_back(vi); 152 | // } 153 | } 154 | 155 | return velocity_info; //std::make_tuple(velocity_map, velocity_info); 156 | } 157 | 158 | 159 | class VOEstimator { 160 | public: 161 | VOEstimator() { 162 | // 163 | } 164 | 165 | ~VOEstimator() { 166 | if (vo_ != nullptr) { 167 | delete vo_; 168 | vo_ = nullptr; 169 | } 170 | 171 | if (sf_matcher_ != nullptr) { 172 | delete sf_matcher_; 173 | sf_matcher_ = nullptr; 174 | } 175 | 176 | std::cout << "Cleared mem, bye!" << std::endl; 177 | } 178 | 179 | void init(const pybind11::array_t &left, const pybind11::array_t &right, float focal_len, float cu, float cv, float baseline, 180 | bool compute_scene_flow = false) { 181 | std::cout << "Initing stuff" << std::endl; 182 | 183 | //libviso2::VisualOdometryStereo::parameters param; 184 | param_.calib.f = focal_len; 185 | param_.calib.cu = cu; 186 | param_.calib.cv = cv; 187 | param_.base = baseline; 188 | 189 | vo_ = new libviso2::VisualOdometryStereo(param_); 190 | 191 | // Only push the first image pair 192 | uint8_t *left_img_1, *right_img_1; 193 | int rows, cols; 194 | pybind_to_raw(left, left_img_1, rows, cols); 195 | pybind_to_raw(right, right_img_1, rows, cols); 196 | int32_t dims[] = {cols, rows, cols}; 197 | libviso2::Matrix frame_to_frame_motion; 198 | vo_->process(left_img_1, right_img_1, dims); 199 | 200 | // Init scene-flow matcher 201 | if (compute_scene_flow == true ){ 202 | libviso2::Matcher::parameters matcher_params; 203 | 204 | matcher_params.nms_n = 5; // non-max-suppression: min. distance between maxima (in pixels) 205 | matcher_params.nms_tau = 50; // non-max-suppression: interest point peakiness threshold 206 | matcher_params.match_binsize = 50; // matching bin width/height (affects efficiency only) 207 | matcher_params.match_radius = 200; // matching radius (du/dv in pixels) 208 | matcher_params.match_disp_tolerance = 1; // du tolerance for stereo matches (in pixels) 209 | matcher_params.outlier_disp_tolerance = 5; // outlier removal: disparity tolerance (in pixels) 210 | matcher_params.outlier_flow_tolerance = 5; // outlier removal: flow tolerance (in pixels) 211 | matcher_params.multi_stage = 1; // 0=disabled,1=multistage matching (denser and faster) 212 | matcher_params.half_resolution = 0; // 0=disabled,1=match at half resolution, refine at full resolution 213 | matcher_params.refinement = 2; // refinement (0=none,1=pixel,2=subpixel) 214 | 215 | // bucketing parameters 216 | //matcher_params.max_features = 4; 217 | //matcher_params.bucket_width = 10; 218 | //matcher_params.bucket_height = 10; 219 | 220 | // Make sf matcher open for busisness 221 | this->sf_matcher_ = new libviso2::Matcher(matcher_params); 222 | this->sf_matcher_->pushBack(left_img_1, right_img_1, dims, false); 223 | } 224 | 225 | // Free mem 226 | free(left_img_1); 227 | free(right_img_1); 228 | } 229 | 230 | pybind11::array_t compute_pose(const pybind11::array_t &left, const pybind11::array_t &right) { 231 | std::cout << "Proc stuff" << std::endl; 232 | 233 | uint8_t *left_img_1, *right_img_1; 234 | int rows, cols; 235 | pybind_to_raw(left, left_img_1, rows, cols); 236 | pybind_to_raw(right, right_img_1, rows, cols); 237 | int32_t dims[] = {cols, rows, cols}; 238 | libviso2::Matrix frame_to_frame_motion; 239 | bool flag = vo_->process(left_img_1, right_img_1, dims); 240 | 241 | 242 | if (flag) { 243 | frame_to_frame_motion = vo_->getMotion(); //libviso2::Matrix::inv(viso->getMotion()); 244 | } else { 245 | std::cout << "VO error! Abort! TODO: handle this!" << std::endl; 246 | // TODO: handle the exception 247 | } 248 | 249 | // Free mem 250 | free(left_img_1); 251 | free(right_img_1); 252 | 253 | // Convert transformation -> numpy array 254 | pybind11::array_t result = pybind11::array_t(4*4); 255 | auto buf3 = result.request(); 256 | double *ptr3 = (double *)buf3.ptr; 257 | for (int i=0; i<4; i++) { 258 | for (int j=0; j<4; j++) { 259 | ptr3[i*4 + j] = frame_to_frame_motion.val[i][j]; 260 | } 261 | } 262 | 263 | result.resize({4, 4}); 264 | 265 | return result; 266 | } 267 | 268 | pybind11::array_t compute_flow(const pybind11::array_t &left, const pybind11::array_t &right, 269 | float dt, float velocity_thresh) { 270 | 271 | 272 | libviso2::Matrix pose = libviso2::Matrix::eye(4); 273 | Eigen::Matrix ego = Eigen::MatrixXd::Identity(4,4); // Current pose 274 | 275 | uint8_t *left_img_1, *right_img_1; 276 | int rows, cols; 277 | pybind_to_raw(left, left_img_1, rows, cols); 278 | pybind_to_raw(right, right_img_1, rows, cols); 279 | int32_t dims[] = {cols, rows, cols}; 280 | 281 | // Push images 282 | sf_matcher_->pushBack(left_img_1, right_img_1, dims, false); 283 | 284 | std::vector matches; 285 | 286 | // Get quad matches 287 | sf_matcher_->matchFeatures(2); // 2 ... quad matching 288 | auto quad_matches = sf_matcher_->getMatches(); 289 | 290 | auto sparse_flow_info = GetSceneFlow(quad_matches, ego, param_.calib.f, param_.calib.cu, param_.calib.cv, param_.base, 291 | dt, velocity_thresh); 292 | 293 | // Free mem 294 | free(left_img_1); 295 | free(right_img_1); 296 | 297 | int row_len = 2*3; //6+2; 298 | pybind11::array_t result = pybind11::array_t(sparse_flow_info.size()*row_len); 299 | auto buf3 = result.request(); 300 | double *ptr3 = (double *)buf3.ptr; 301 | for (int i=0; i(sparse_flow_info.size()), row_len}); 321 | 322 | return result; 323 | } 324 | 325 | // Returns: Nx4 matrix, rows correspond to indices of matched points (curr, prev 326 | pybind11::array_t get_matches_indices() { 327 | // int32_t i1p; // feature index (for tracking) 328 | // int32_t i2p; // feature index (for tracking) 329 | // int32_t i1c; // feature index (for tracking) 330 | // int32_t i2c; // feature index (for tracking) 331 | 332 | const auto &quad_matches = sf_matcher_->getMatches(); 333 | const int num_matches = quad_matches.size(); 334 | 335 | std::cout << "get_matches_indices num_matches: " << num_matches << std::endl; 336 | 337 | const int row_len = 4; 338 | pybind11::array_t result = pybind11::array_t(num_matches*row_len); 339 | auto buf = result.request(); 340 | int *ptr = (int*)buf.ptr; 341 | 342 | for (int i=0; i(num_matches), row_len}); 354 | return result; 355 | } 356 | 357 | // Returns: Nx8 matrix, rows correspond to coords of matched points (curr (u, v), prev (u, v)) 358 | pybind11::array_t get_matches_coords() { 359 | // float u1p, v1p; // u,v-coordinates in previous left image 360 | // float u2p, v2p; // u,v-coordinates in previous right image 361 | // float u1c, v1c; // u,v-coordinates in current left image 362 | // float u2c, v2c; // u,v-coordinates in current right image 363 | 364 | const auto &quad_matches = sf_matcher_->getMatches(); 365 | const int num_matches = quad_matches.size(); 366 | const int row_len = 8; 367 | pybind11::array_t result = pybind11::array_t(num_matches*row_len); 368 | auto buf = result.request(); 369 | float *ptr = (float*)buf.ptr; 370 | 371 | for (int i=0; i(num_matches), row_len}); 387 | return result; 388 | } 389 | 390 | private: 391 | libviso2::VisualOdometryStereo *vo_ = nullptr; 392 | libviso2::Matcher *sf_matcher_ = nullptr; 393 | libviso2::VisualOdometryStereo::parameters param_; 394 | }; 395 | 396 | namespace py = pybind11; 397 | 398 | PYBIND11_MODULE(pyinterface, m) { 399 | m.doc() = R"pbdoc( 400 | Pybind11 example plugin 401 | ----------------------- 402 | .. currentmodule:: cmake_example 403 | .. autosummary:: 404 | :toctree: _generate 405 | add 406 | subtract 407 | )pbdoc"; 408 | 409 | m.def("add", &add, R"pbdoc( 410 | Add two numbers 411 | Some other explanation about the add function. 412 | )pbdoc"); 413 | 414 | py::class_(m, "VOEstimator") 415 | .def(py::init<>()).def("init", &VOEstimator::init).def("compute_pose", &VOEstimator::compute_pose).def("compute_flow", &VOEstimator::compute_flow).def("get_matches_indices", &VOEstimator::get_matches_indices).def("get_matches_coords", &VOEstimator::get_matches_coords); 416 | 417 | m.def("subtract", [](int i, int j) { return i - j; }, R"pbdoc( 418 | Subtract two numbers 419 | Some other explanation about the subtract function. 420 | )pbdoc"); 421 | 422 | #ifdef VERSION_INFO 423 | m.attr("__version__") = VERSION_INFO; 424 | #else 425 | m.attr("__version__") = "dev"; 426 | #endif 427 | } -------------------------------------------------------------------------------- /images/flow_image.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aljosaosep/sparse-scene-flow/def35ae4147298cc40f5b204d79a14996cf06ff8/images/flow_image.png -------------------------------------------------------------------------------- /images/flow_image_combined.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aljosaosep/sparse-scene-flow/def35ae4147298cc40f5b204d79a14996cf06ff8/images/flow_image_combined.png -------------------------------------------------------------------------------- /images/flow_topdown.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aljosaosep/sparse-scene-flow/def35ae4147298cc40f5b204d79a14996cf06ff8/images/flow_topdown.png -------------------------------------------------------------------------------- /include/libviso2/filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2012. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libelas. 7 | Authors: Julius Ziegler, Andreas Geiger 8 | 9 | libelas is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 3 of the License, or any later version. 12 | 13 | libelas is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libelas; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef __FILTER_H__ 23 | #define __FILTER_H__ 24 | 25 | #include 26 | #include 27 | 28 | // define fixed-width datatypes for Visual Studio projects 29 | #if defined(_MSC_VER) && (_MSC_VER < 1600) 30 | typedef __int8 int8_t; 31 | typedef __int16 int16_t; 32 | typedef __int32 int32_t; 33 | typedef __int64 int64_t; 34 | typedef unsigned __int8 uint8_t; 35 | typedef unsigned __int16 uint16_t; 36 | typedef unsigned __int32 uint32_t; 37 | typedef unsigned __int64 uint64_t; 38 | #else 39 | #include 40 | #endif 41 | 42 | // fast filters: implements 3x3 and 5x5 sobel filters and 43 | // 5x5 blob and corner filters based on SSE2/3 instructions 44 | namespace filter { 45 | 46 | // private namespace, public user functions at the bottom of this file 47 | namespace detail { 48 | void integral_image( const uint8_t* in, int32_t* out, int w, int h ); 49 | void unpack_8bit_to_16bit( const __m128i a, __m128i& b0, __m128i& b1 ); 50 | void pack_16bit_to_8bit_saturate( const __m128i a0, const __m128i a1, __m128i& b ); 51 | 52 | // convolve image with a (1,4,6,4,1) row vector. Result is accumulated into output. 53 | // output is scaled by 1/128, then clamped to [-128,128], and finally shifted to [0,255]. 54 | void convolve_14641_row_5x5_16bit( const int16_t* in, uint8_t* out, int w, int h ); 55 | 56 | // convolve image with a (1,2,0,-2,-1) row vector. Result is accumulated into output. 57 | // This one works on 16bit input and 8bit output. 58 | // output is scaled by 1/128, then clamped to [-128,128], and finally shifted to [0,255]. 59 | void convolve_12021_row_5x5_16bit( const int16_t* in, uint8_t* out, int w, int h ); 60 | 61 | // convolve image with a (1,2,1) row vector. Result is accumulated into output. 62 | // This one works on 16bit input and 8bit output. 63 | // output is scaled by 1/4, then clamped to [-128,128], and finally shifted to [0,255]. 64 | void convolve_121_row_3x3_16bit( const int16_t* in, uint8_t* out, int w, int h ); 65 | 66 | // convolve image with a (1,0,-1) row vector. Result is accumulated into output. 67 | // This one works on 16bit input and 8bit output. 68 | // output is scaled by 1/4, then clamped to [-128,128], and finally shifted to [0,255]. 69 | void convolve_101_row_3x3_16bit( const int16_t* in, uint8_t* out, int w, int h ); 70 | 71 | void convolve_cols_5x5( const unsigned char* in, int16_t* out_v, int16_t* out_h, int w, int h ); 72 | 73 | void convolve_col_p1p1p0m1m1_5x5( const unsigned char* in, int16_t* out, int w, int h ); 74 | 75 | void convolve_row_p1p1p0m1m1_5x5( const int16_t* in, int16_t* out, int w, int h ); 76 | 77 | void convolve_cols_3x3( const unsigned char* in, int16_t* out_v, int16_t* out_h, int w, int h ); 78 | } 79 | 80 | void sobel3x3( const uint8_t* in, uint8_t* out_v, uint8_t* out_h, int w, int h ); 81 | 82 | void sobel5x5( const uint8_t* in, uint8_t* out_v, uint8_t* out_h, int w, int h ); 83 | 84 | // -1 -1 0 1 1 85 | // -1 -1 0 1 1 86 | // 0 0 0 0 0 87 | // 1 1 0 -1 -1 88 | // 1 1 0 -1 -1 89 | void checkerboard5x5( const uint8_t* in, int16_t* out, int w, int h ); 90 | 91 | // -1 -1 -1 -1 -1 92 | // -1 1 1 1 -1 93 | // -1 1 8 1 -1 94 | // -1 1 1 1 -1 95 | // -1 -1 -1 -1 -1 96 | void blob5x5( const uint8_t* in, int16_t* out, int w, int h ); 97 | }; 98 | 99 | #endif 100 | -------------------------------------------------------------------------------- /include/libviso2/matcher.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2012. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef __MATCHER_H__ 23 | #define __MATCHER_H__ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include "matrix.h" 35 | 36 | namespace libviso2 { 37 | class Matcher { 38 | 39 | public: 40 | 41 | // parameter settings 42 | struct parameters { 43 | 44 | int32_t nms_n; // non-max-suppression: min. distance between maxima (in pixels) 45 | int32_t nms_tau; // non-max-suppression: interest point peakiness threshold 46 | int32_t match_binsize; // matching bin width/height (affects efficiency only) 47 | int32_t match_radius; // matching radius (du/dv in pixels) 48 | int32_t match_disp_tolerance; // dv tolerance for stereo matches (in pixels) 49 | int32_t outlier_disp_tolerance; // outlier removal: disparity tolerance (in pixels) 50 | int32_t outlier_flow_tolerance; // outlier removal: flow tolerance (in pixels) 51 | int32_t multi_stage; // 0=disabled,1=multistage matching (denser and faster) 52 | int32_t half_resolution; // 0=disabled,1=match at half resolution, refine at full resolution 53 | int32_t refinement; // refinement (0=none,1=pixel,2=subpixel) 54 | double f, cu, cv, base; // calibration (only for match prediction) 55 | 56 | // default settings 57 | parameters() { 58 | nms_n = 3; 59 | nms_tau = 50; 60 | match_binsize = 50; 61 | match_radius = 200; 62 | match_disp_tolerance = 2; 63 | outlier_disp_tolerance = 5; 64 | outlier_flow_tolerance = 5; 65 | multi_stage = 1; 66 | half_resolution = 1; 67 | refinement = 1; 68 | } 69 | }; 70 | 71 | // constructor (with default parameters) 72 | Matcher(parameters param); 73 | 74 | // deconstructor 75 | ~Matcher(); 76 | 77 | // intrinsics 78 | void setIntrinsics(double f, double cu, double cv, double base) { 79 | param.f = f; 80 | param.cu = cu; 81 | param.cv = cv; 82 | param.base = base; 83 | } 84 | 85 | // structure for storing matches 86 | struct p_match { 87 | float u1p, v1p; // u,v-coordinates in previous left image 88 | int32_t i1p; // feature index (for tracking) 89 | float u2p, v2p; // u,v-coordinates in previous right image 90 | int32_t i2p; // feature index (for tracking) 91 | float u1c, v1c; // u,v-coordinates in current left image 92 | int32_t i1c; // feature index (for tracking) 93 | float u2c, v2c; // u,v-coordinates in current right image 94 | int32_t i2c; // feature index (for tracking) 95 | p_match() {} 96 | 97 | p_match(float u1p, float v1p, int32_t i1p, float u2p, float v2p, int32_t i2p, 98 | float u1c, float v1c, int32_t i1c, float u2c, float v2c, int32_t i2c) : 99 | u1p(u1p), v1p(v1p), i1p(i1p), u2p(u2p), v2p(v2p), i2p(i2p), 100 | u1c(u1c), v1c(v1c), i1c(i1c), u2c(u2c), v2c(v2c), i2c(i2c) {} 101 | }; 102 | 103 | // computes features from left/right images and pushes them back to a ringbuffer, 104 | // which interally stores the features of the current and previous image pair 105 | // use this function for stereo or quad matching 106 | // input: I1,I2 .......... pointers to left and right image (row-aligned), range [0..255] 107 | // dims[0,1] ...... image width and height (both images must be rectified and of same size) 108 | // dims[2] ........ bytes per line (often equals width) 109 | // replace ........ if this flag is set, the current image is overwritten with 110 | // the input images, otherwise the current image is first copied 111 | // to the previous image (ring buffer functionality, descriptors need 112 | // to be computed only once) 113 | void pushBack(uint8_t *I1, uint8_t *I2, int32_t *dims, const bool replace); 114 | 115 | // computes features from a single image and pushes it back to a ringbuffer, 116 | // which interally stores the features of the current and previous image pair 117 | // use this function for flow computation 118 | // parameter description see above 119 | void pushBack(uint8_t *I1, int32_t *dims, const bool replace) { pushBack(I1, 0, dims, replace); } 120 | 121 | // match features currently stored in ring buffer (current and previous frame) 122 | // input: method ... 0 = flow, 1 = stereo, 2 = quad matching 123 | // Tr_delta: uses motion from previous frame to better search for 124 | // matches, if specified 125 | void matchFeatures(int32_t method, Matrix *Tr_delta = 0); 126 | 127 | // feature bucketing: keeps only max_features per bucket, where the domain 128 | // is split into buckets of size (bucket_width,bucket_height) 129 | void bucketFeatures(int32_t max_features, float bucket_width, float bucket_height); 130 | 131 | // return vector with matched feature points and indices 132 | std::vector getMatches() { return p_matched_2; } 133 | 134 | // given a vector of inliers computes gain factor between the current and 135 | // the previous frame. this function is useful if you want to reconstruct 3d 136 | // and you want to cancel the change of (unknown) camera gain. 137 | float getGain(std::vector inliers); 138 | 139 | private: 140 | 141 | // structure for storing interest points 142 | struct maximum { 143 | int32_t u; // u-coordinate 144 | int32_t v; // v-coordinate 145 | int32_t val; // value 146 | int32_t c; // class 147 | int32_t d1, d2, d3, d4, d5, d6, d7, d8; // descriptor 148 | maximum() {} 149 | 150 | maximum(int32_t u, int32_t v, int32_t val, int32_t c) : u(u), v(v), val(val), c(c) {} 151 | }; 152 | 153 | // u/v ranges for matching stage 0-3 154 | struct range { 155 | float u_min[4]; 156 | float u_max[4]; 157 | float v_min[4]; 158 | float v_max[4]; 159 | }; 160 | 161 | struct delta { 162 | float val[8]; 163 | 164 | delta() {} 165 | 166 | delta(float v) { 167 | for (int32_t i = 0; i < 8; i++) 168 | val[i] = v; 169 | } 170 | }; 171 | 172 | // computes the address offset for coordinates u,v of an image of given width 173 | inline int32_t getAddressOffsetImage(const int32_t &u, const int32_t &v, const int32_t &width) { 174 | return v * width + u; 175 | } 176 | 177 | // Alexander Neubeck and Luc Van Gool: Efficient Non-Maximum Suppression, ICPR'06, algorithm 4 178 | void nonMaximumSuppression(int16_t *I_f1, int16_t *I_f2, const int32_t *dims, std::vector &maxima, int32_t nms_n); 179 | 180 | // descriptor functions 181 | inline uint8_t saturate(int16_t in); 182 | 183 | void filterImageAll(uint8_t *I, uint8_t *I_du, uint8_t *I_dv, int16_t *I_f1, int16_t *I_f2, const int *dims); 184 | 185 | void filterImageSobel(uint8_t *I, uint8_t *I_du, uint8_t *I_dv, const int *dims); 186 | 187 | inline void computeDescriptor(const uint8_t *I_du, const uint8_t *I_dv, const int32_t &bpl, const int32_t &u, const int32_t &v, uint8_t *desc_addr); 188 | 189 | inline void 190 | computeSmallDescriptor(const uint8_t *I_du, const uint8_t *I_dv, const int32_t &bpl, const int32_t &u, const int32_t &v, uint8_t *desc_addr); 191 | 192 | void computeDescriptors(uint8_t *I_du, uint8_t *I_dv, const int32_t bpl, std::vector &maxima); 193 | 194 | void getHalfResolutionDimensions(const int32_t *dims, int32_t *dims_half); 195 | 196 | uint8_t *createHalfResolutionImage(uint8_t *I, const int32_t *dims); 197 | 198 | // compute sparse set of features from image 199 | // inputs: I ........ image 200 | // dims ..... image dimensions [width,height] 201 | // n ........ non-max neighborhood 202 | // tau ...... non-max threshold 203 | // outputs: max ...... vector with maxima [u,v,value,class,descriptor (128 bits)] 204 | // I_du ..... gradient in horizontal direction 205 | // I_dv ..... gradient in vertical direction 206 | // WARNING: max,I_du,I_dv has to be freed by yourself! 207 | void computeFeatures(uint8_t *I, const int32_t *dims, int32_t *&max1, int32_t &num1, int32_t *&max2, int32_t &num2, uint8_t *&I_du, uint8_t *&I_dv, 208 | uint8_t *&I_du_full, uint8_t *&I_dv_full); 209 | 210 | // matching functions 211 | void computePriorStatistics(std::vector &p_matched, int32_t method); 212 | 213 | void createIndexVector(int32_t *m, int32_t n, std::vector *k, const int32_t &u_bin_num, const int32_t &v_bin_num); 214 | 215 | inline void findMatch(int32_t *m1, const int32_t &i1, int32_t *m2, const int32_t &step_size, 216 | std::vector *k2, const int32_t &u_bin_num, const int32_t &v_bin_num, const int32_t &stat_bin, 217 | int32_t &min_ind, int32_t stage, bool flow, bool use_prior, double u_ = -1, double v_ = -1); 218 | 219 | void matching(int32_t *m1p, int32_t *m2p, int32_t *m1c, int32_t *m2c, 220 | int32_t n1p, int32_t n2p, int32_t n1c, int32_t n2c, 221 | std::vector &p_matched, int32_t method, bool use_prior, Matrix *Tr_delta = 0); 222 | 223 | // outlier removal 224 | void removeOutliers(std::vector &p_matched, int32_t method); 225 | 226 | // parabolic fitting 227 | bool parabolicFitting(const uint8_t *I1_du, const uint8_t *I1_dv, const int32_t *dims1, 228 | const uint8_t *I2_du, const uint8_t *I2_dv, const int32_t *dims2, 229 | const float &u1, const float &v1, 230 | float &u2, float &v2, 231 | Matrix At, Matrix AtA, 232 | uint8_t *desc_buffer); 233 | 234 | void relocateMinimum(const uint8_t *I1_du, const uint8_t *I1_dv, const int32_t *dims1, 235 | const uint8_t *I2_du, const uint8_t *I2_dv, const int32_t *dims2, 236 | const float &u1, const float &v1, 237 | float &u2, float &v2, 238 | uint8_t *desc_buffer); 239 | 240 | void refinement(std::vector &p_matched, int32_t method); 241 | 242 | // mean for gain computation 243 | inline float mean(const uint8_t *I, const int32_t &bpl, const int32_t &u_min, const int32_t &u_max, const int32_t &v_min, const int32_t &v_max); 244 | 245 | // parameters 246 | parameters param; 247 | int32_t margin; 248 | 249 | int32_t *m1p1, *m2p1, *m1c1, *m2c1; 250 | int32_t *m1p2, *m2p2, *m1c2, *m2c2; 251 | int32_t n1p1, n2p1, n1c1, n2c1; 252 | int32_t n1p2, n2p2, n1c2, n2c2; 253 | uint8_t *I1p, *I2p, *I1c, *I2c; 254 | uint8_t *I1p_du, *I2p_du, *I1c_du, *I2c_du; 255 | uint8_t *I1p_dv, *I2p_dv, *I1c_dv, *I2c_dv; 256 | uint8_t *I1p_du_full, *I2p_du_full, *I1c_du_full, *I2c_du_full; // only needed for 257 | uint8_t *I1p_dv_full, *I2p_dv_full, *I1c_dv_full, *I2c_dv_full; // half-res matching 258 | int32_t dims_p[3], dims_c[3]; 259 | 260 | std::vector p_matched_1; 261 | std::vector p_matched_2; 262 | std::vector ranges; 263 | }; 264 | } 265 | #endif 266 | 267 | -------------------------------------------------------------------------------- /include/libviso2/matrix.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef MATRIX_H 23 | #define MATRIX_H 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #if defined(_MSC_VER) && (_MSC_VER < 1600) 32 | typedef __int8 int8_t; 33 | typedef __int16 int16_t; 34 | typedef __int32 int32_t; 35 | typedef __int64 int64_t; 36 | typedef unsigned __int8 uint8_t; 37 | typedef unsigned __int16 uint16_t; 38 | typedef unsigned __int32 uint32_t; 39 | typedef unsigned __int64 uint64_t; 40 | #else 41 | #include 42 | #endif 43 | 44 | #define endll endl << endl // double end line definition 45 | 46 | typedef double FLOAT; // double precision 47 | //typedef float FLOAT; // single precision 48 | 49 | namespace libviso2 { 50 | class Matrix { 51 | 52 | public: 53 | 54 | // constructor / deconstructor 55 | Matrix(); // init empty 0x0 matrix 56 | Matrix(const int32_t m, const int32_t n); // init empty mxn matrix 57 | Matrix(const int32_t m, const int32_t n, const FLOAT *val_); // init mxn matrix with values from array 'val' 58 | Matrix(const Matrix &M); // creates deepcopy of M 59 | ~Matrix(); 60 | 61 | // assignment operator, copies contents of M 62 | Matrix &operator=(const Matrix &M); 63 | 64 | // copies submatrix of M into array 'val', default values copy whole row/column/matrix 65 | void getData(FLOAT *val_, int32_t i1 = 0, int32_t j1 = 0, int32_t i2 = -1, int32_t j2 = -1); 66 | 67 | // set or get submatrices of current matrix 68 | Matrix getMat(int32_t i1, int32_t j1, int32_t i2 = -1, int32_t j2 = -1); 69 | 70 | void setMat(const Matrix &M, const int32_t i, const int32_t j); 71 | 72 | // set sub-matrix to scalar (default 0), -1 as end replaces whole row/column/matrix 73 | void setVal(FLOAT s, int32_t i1 = 0, int32_t j1 = 0, int32_t i2 = -1, int32_t j2 = -1); 74 | 75 | // set (part of) diagonal to scalar, -1 as end replaces whole diagonal 76 | void setDiag(FLOAT s, int32_t i1 = 0, int32_t i2 = -1); 77 | 78 | // clear matrix 79 | void zero(); 80 | 81 | // extract columns with given index 82 | Matrix extractCols(std::vector idx); 83 | 84 | // create identity matrix 85 | static Matrix eye(const int32_t m); 86 | 87 | void eye(); 88 | 89 | // create diagonal matrix with nx1 or 1xn matrix M as elements 90 | static Matrix diag(const Matrix &M); 91 | 92 | // returns the m-by-n matrix whose elements are taken column-wise from M 93 | static Matrix reshape(const Matrix &M, int32_t m, int32_t n); 94 | 95 | // create 3x3 rotation matrices (convention: http://en.wikipedia.org/wiki/Rotation_matrix) 96 | static Matrix rotMatX(const FLOAT &angle); 97 | 98 | static Matrix rotMatY(const FLOAT &angle); 99 | 100 | static Matrix rotMatZ(const FLOAT &angle); 101 | 102 | // simple arithmetic operations 103 | Matrix operator+(const Matrix &M); // add matrix 104 | Matrix operator-(const Matrix &M); // subtract matrix 105 | Matrix operator*(const Matrix &M); // multiply with matrix 106 | Matrix operator*(const FLOAT &s); // multiply with scalar 107 | Matrix operator/(const Matrix &M); // divide elementwise by matrix (or vector) 108 | Matrix operator/(const FLOAT &s); // divide by scalar 109 | Matrix operator-(); // negative matrix 110 | Matrix operator~(); // transpose 111 | FLOAT l2norm(); // euclidean norm (vectors) / frobenius norm (matrices) 112 | FLOAT mean(); // mean of all elements in matrix 113 | 114 | // complex arithmetic operations 115 | static Matrix cross(const Matrix &a, const Matrix &b); // cross product of two vectors 116 | static Matrix inv(const Matrix &M); // invert matrix M 117 | bool inv(); // invert this matrix 118 | FLOAT det(); // returns determinant of matrix 119 | bool solve(const Matrix &M, FLOAT eps = 1e-20); // solve linear system M*x=B, replaces *this and M 120 | bool lu(int32_t *idx, FLOAT &d, FLOAT eps = 1e-20); // replace *this by lower upper decomposition 121 | void svd(Matrix &U, Matrix &W, Matrix &V); // singular value decomposition *this = U*diag(W)*V^T 122 | 123 | // print matrix to stream 124 | friend std::ostream &operator<<(std::ostream &out, const Matrix &M); 125 | 126 | // direct data access 127 | FLOAT **val; 128 | int32_t m, n; 129 | 130 | private: 131 | 132 | void allocateMemory(const int32_t m_, const int32_t n_); 133 | 134 | void releaseMemory(); 135 | 136 | inline FLOAT pythag(FLOAT a, FLOAT b); 137 | 138 | }; 139 | } 140 | 141 | #endif // MATRIX_H 142 | -------------------------------------------------------------------------------- /include/libviso2/reconstruction.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef RECONSTRUCTION_H 23 | #define RECONSTRUCTION_H 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #define _USE_MATH_DEFINES 30 | #include 31 | 32 | #include "matcher.h" 33 | #include "matrix.h" 34 | 35 | namespace libviso2 { 36 | class Reconstruction { 37 | 38 | public: 39 | 40 | // constructor 41 | Reconstruction(); 42 | 43 | // deconstructor 44 | ~Reconstruction(); 45 | 46 | // a generic 3d point 47 | struct point3d { 48 | float x, y, z; 49 | 50 | point3d() {} 51 | 52 | point3d(float x, float y, float z) : x(x), y(y), z(z) {} 53 | }; 54 | 55 | // set calibration parameters (intrinsics), must be called at least once 56 | // input: f ....... focal length (assumes fu=fv) 57 | // cu,cv ... principal point 58 | void setCalibration(FLOAT f, FLOAT cu, FLOAT cv); 59 | 60 | // takes a set of monocular feature matches (flow method) and the egomotion 61 | // estimate between the 2 frames Tr, tries to associate the features with previous 62 | // frames (tracking) and computes 3d points once tracks gets lost. 63 | // point types: 0 ..... everything 64 | // 1 ..... road and above 65 | // 2 ..... only above road 66 | // min_track_length ... min number of frames a point needs to be tracked for reconstruction 67 | // max_dist ........... maximum point distance from camera 68 | // min_angle .......... avoid angles smaller than this for reconstruction (in degrees) 69 | void update(std::vector p_matched, Matrix Tr, int32_t point_type = 1, int32_t min_track_length = 2, double max_dist = 30, 70 | double min_angle = 2); 71 | 72 | // return currently computed 3d points (finished tracks) 73 | std::vector getPoints() { return points; } 74 | 75 | private: 76 | 77 | struct point2d { 78 | float u, v; 79 | 80 | point2d() {} 81 | 82 | point2d(float u, float v) : u(u), v(v) {} 83 | }; 84 | 85 | struct track { 86 | std::vector pixels; 87 | int32_t first_frame; 88 | int32_t last_frame; 89 | int32_t last_idx; 90 | }; 91 | 92 | enum result { 93 | UPDATED, FAILED, CONVERGED 94 | }; 95 | 96 | bool initPoint(const track &t, point3d &p); 97 | 98 | bool refinePoint(const track &t, point3d &p); 99 | 100 | double pointDistance(const track &t, point3d &p); 101 | 102 | double rayAngle(const track &t, point3d &p); 103 | 104 | int32_t pointType(const track &t, point3d &p); 105 | 106 | result updatePoint(const track &t, point3d &p, const FLOAT &step_size, const FLOAT &eps); 107 | 108 | void computeObservations(const std::vector &p); 109 | 110 | bool computePredictionsAndJacobian(const std::vector::iterator &P_begin, const std::vector::iterator &P_end, point3d &p); 111 | 112 | void testJacobian(); 113 | 114 | // calibration matrices 115 | Matrix K, Tr_cam_road; 116 | 117 | std::vector tracks; 118 | std::vector Tr_total; 119 | std::vector Tr_inv_total; 120 | std::vector P_total; 121 | std::vector points; 122 | 123 | FLOAT *J; // jacobian 124 | FLOAT *p_observe, *p_predict; // observed and predicted 2d points 125 | 126 | }; 127 | } 128 | 129 | #endif // RECONSTRUCTION_H 130 | -------------------------------------------------------------------------------- /include/libviso2/timer.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef __TIMER_H__ 23 | #define __TIMER_H__ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | class Timer { 36 | 37 | public: 38 | 39 | Timer() {} 40 | 41 | ~Timer() {} 42 | 43 | void start (std::string title) { 44 | desc.push_back(title); 45 | push_back_time(); 46 | } 47 | 48 | void stop () { 49 | if (time.size()<=desc.size()) 50 | push_back_time(); 51 | } 52 | 53 | void plot () { 54 | stop(); 55 | float total_time = 0; 56 | for (int32_t i=0; i<(int32_t)desc.size(); i++) { 57 | float curr_time = getTimeDifferenceMilliseconds(time[i],time[i+1]); 58 | total_time += curr_time; 59 | std::cout.width(30); 60 | std::cout << desc[i] << " "; 61 | std::cout << std::fixed << std::setprecision(1) << std::setw(6); 62 | std::cout << curr_time; 63 | std::cout << " ms" << std::endl; 64 | } 65 | std::cout << "========================================" << std::endl; 66 | std::cout << " Total time "; 67 | std::cout << std::fixed << std::setprecision(1) << std::setw(6); 68 | std::cout << total_time; 69 | std::cout << " ms" << std::endl << std::endl; 70 | } 71 | 72 | void reset () { 73 | desc.clear(); 74 | time.clear(); 75 | } 76 | 77 | private: 78 | std::vector desc; 79 | std::vector time; 80 | 81 | void push_back_time () { 82 | timeval curr_time; 83 | gettimeofday(&curr_time,0); 84 | time.push_back(curr_time); 85 | } 86 | 87 | float getTimeDifferenceMilliseconds(timeval a,timeval b) { 88 | return ((float)(b.tv_sec -a.tv_sec ))*1e+3 + 89 | ((float)(b.tv_usec-a.tv_usec))*1e-3; 90 | } 91 | }; 92 | 93 | #endif 94 | -------------------------------------------------------------------------------- /include/libviso2/triangle.h: -------------------------------------------------------------------------------- 1 | /*****************************************************************************/ 2 | /* */ 3 | /* (triangle.h) */ 4 | /* */ 5 | /* Include file for programs that call Triangle. */ 6 | /* */ 7 | /* Accompanies Triangle Version 1.6 */ 8 | /* July 28, 2005 */ 9 | /* */ 10 | /* Copyright 1996, 2005 */ 11 | /* Jonathan Richard Shewchuk */ 12 | /* 2360 Woolsey #H */ 13 | /* Berkeley, California 94705-1927 */ 14 | /* jrs@cs.berkeley.edu */ 15 | /* */ 16 | /* Modified by Andreas Geiger, 2011 */ 17 | /*****************************************************************************/ 18 | 19 | /*****************************************************************************/ 20 | /* */ 21 | /* How to call Triangle from another program */ 22 | /* */ 23 | /* */ 24 | /* If you haven't read Triangle's instructions (run "triangle -h" to read */ 25 | /* them), you won't understand what follows. */ 26 | /* */ 27 | /* Triangle must be compiled into an object file (triangle.o) with the */ 28 | /* TRILIBRARY symbol defined (generally by using the -DTRILIBRARY compiler */ 29 | /* switch). The makefile included with Triangle will do this for you if */ 30 | /* you run "make trilibrary". The resulting object file can be called via */ 31 | /* the procedure triangulate(). */ 32 | /* */ 33 | /* If the size of the object file is important to you, you may wish to */ 34 | /* generate a reduced version of triangle.o. The REDUCED symbol gets rid */ 35 | /* of all features that are primarily of research interest. Specifically, */ 36 | /* the -DREDUCED switch eliminates Triangle's -i, -F, -s, and -C switches. */ 37 | /* The CDT_ONLY symbol gets rid of all meshing algorithms above and beyond */ 38 | /* constrained Delaunay triangulation. Specifically, the -DCDT_ONLY switch */ 39 | /* eliminates Triangle's -r, -q, -a, -u, -D, -Y, -S, and -s switches. */ 40 | /* */ 41 | /* IMPORTANT: These definitions (TRILIBRARY, REDUCED, CDT_ONLY) must be */ 42 | /* made in the makefile or in triangle.c itself. Putting these definitions */ 43 | /* in this file (triangle.h) will not create the desired effect. */ 44 | /* */ 45 | /* */ 46 | /* The calling convention for triangulate() follows. */ 47 | /* */ 48 | /* void triangulate(triswitches, in, out, vorout) */ 49 | /* char *triswitches; */ 50 | /* struct triangulateio *in; */ 51 | /* struct triangulateio *out; */ 52 | /* struct triangulateio *vorout; */ 53 | /* */ 54 | /* `triswitches' is a string containing the command line switches you wish */ 55 | /* to invoke. No initial dash is required. Some suggestions: */ 56 | /* */ 57 | /* - You'll probably find it convenient to use the `z' switch so that */ 58 | /* points (and other items) are numbered from zero. This simplifies */ 59 | /* indexing, because the first item of any type always starts at index */ 60 | /* [0] of the corresponding array, whether that item's number is zero or */ 61 | /* one. */ 62 | /* - You'll probably want to use the `Q' (quiet) switch in your final code, */ 63 | /* but you can take advantage of Triangle's printed output (including the */ 64 | /* `V' switch) while debugging. */ 65 | /* - If you are not using the `q', `a', `u', `D', `j', or `s' switches, */ 66 | /* then the output points will be identical to the input points, except */ 67 | /* possibly for the boundary markers. If you don't need the boundary */ 68 | /* markers, you should use the `N' (no nodes output) switch to save */ 69 | /* memory. (If you do need boundary markers, but need to save memory, a */ 70 | /* good nasty trick is to set out->pointlist equal to in->pointlist */ 71 | /* before calling triangulate(), so that Triangle overwrites the input */ 72 | /* points with identical copies.) */ 73 | /* - The `I' (no iteration numbers) and `g' (.off file output) switches */ 74 | /* have no effect when Triangle is compiled with TRILIBRARY defined. */ 75 | /* */ 76 | /* `in', `out', and `vorout' are descriptions of the input, the output, */ 77 | /* and the Voronoi output. If the `v' (Voronoi output) switch is not used, */ 78 | /* `vorout' may be NULL. `in' and `out' may never be NULL. */ 79 | /* */ 80 | /* Certain fields of the input and output structures must be initialized, */ 81 | /* as described below. */ 82 | /* */ 83 | /*****************************************************************************/ 84 | 85 | /*****************************************************************************/ 86 | /* */ 87 | /* The `triangulateio' structure. */ 88 | /* */ 89 | /* Used to pass data into and out of the triangulate() procedure. */ 90 | /* */ 91 | /* */ 92 | /* Arrays are used to store points, triangles, markers, and so forth. In */ 93 | /* all cases, the first item in any array is stored starting at index [0]. */ 94 | /* However, that item is item number `1' unless the `z' switch is used, in */ 95 | /* which case it is item number `0'. Hence, you may find it easier to */ 96 | /* index points (and triangles in the neighbor list) if you use the `z' */ 97 | /* switch. Unless, of course, you're calling Triangle from a Fortran */ 98 | /* program. */ 99 | /* */ 100 | /* Description of fields (except the `numberof' fields, which are obvious): */ 101 | /* */ 102 | /* `pointlist': An array of point coordinates. The first point's x */ 103 | /* coordinate is at index [0] and its y coordinate at index [1], followed */ 104 | /* by the coordinates of the remaining points. Each point occupies two */ 105 | /* REALs. */ 106 | /* `pointattributelist': An array of point attributes. Each point's */ 107 | /* attributes occupy `numberofpointattributes' REALs. */ 108 | /* `pointmarkerlist': An array of point markers; one int per point. */ 109 | /* */ 110 | /* `trianglelist': An array of triangle corners. The first triangle's */ 111 | /* first corner is at index [0], followed by its other two corners in */ 112 | /* counterclockwise order, followed by any other nodes if the triangle */ 113 | /* represents a nonlinear element. Each triangle occupies */ 114 | /* `numberofcorners' ints. */ 115 | /* `triangleattributelist': An array of triangle attributes. Each */ 116 | /* triangle's attributes occupy `numberoftriangleattributes' REALs. */ 117 | /* `trianglearealist': An array of triangle area constraints; one REAL per */ 118 | /* triangle. Input only. */ 119 | /* `neighborlist': An array of triangle neighbors; three ints per */ 120 | /* triangle. Output only. */ 121 | /* */ 122 | /* `segmentlist': An array of segment endpoints. The first segment's */ 123 | /* endpoints are at indices [0] and [1], followed by the remaining */ 124 | /* segments. Two ints per segment. */ 125 | /* `segmentmarkerlist': An array of segment markers; one int per segment. */ 126 | /* */ 127 | /* `holelist': An array of holes. The first hole's x and y coordinates */ 128 | /* are at indices [0] and [1], followed by the remaining holes. Two */ 129 | /* REALs per hole. Input only, although the pointer is copied to the */ 130 | /* output structure for your convenience. */ 131 | /* */ 132 | /* `regionlist': An array of regional attributes and area constraints. */ 133 | /* The first constraint's x and y coordinates are at indices [0] and [1], */ 134 | /* followed by the regional attribute at index [2], followed by the */ 135 | /* maximum area at index [3], followed by the remaining area constraints. */ 136 | /* Four REALs per area constraint. Note that each regional attribute is */ 137 | /* used only if you select the `A' switch, and each area constraint is */ 138 | /* used only if you select the `a' switch (with no number following), but */ 139 | /* omitting one of these switches does not change the memory layout. */ 140 | /* Input only, although the pointer is copied to the output structure for */ 141 | /* your convenience. */ 142 | /* */ 143 | /* `edgelist': An array of edge endpoints. The first edge's endpoints are */ 144 | /* at indices [0] and [1], followed by the remaining edges. Two ints per */ 145 | /* edge. Output only. */ 146 | /* `edgemarkerlist': An array of edge markers; one int per edge. Output */ 147 | /* only. */ 148 | /* `normlist': An array of normal vectors, used for infinite rays in */ 149 | /* Voronoi diagrams. The first normal vector's x and y magnitudes are */ 150 | /* at indices [0] and [1], followed by the remaining vectors. For each */ 151 | /* finite edge in a Voronoi diagram, the normal vector written is the */ 152 | /* zero vector. Two REALs per edge. Output only. */ 153 | /* */ 154 | /* */ 155 | /* Any input fields that Triangle will examine must be initialized. */ 156 | /* Furthermore, for each output array that Triangle will write to, you */ 157 | /* must either provide space by setting the appropriate pointer to point */ 158 | /* to the space you want the data written to, or you must initialize the */ 159 | /* pointer to NULL, which tells Triangle to allocate space for the results. */ 160 | /* The latter option is preferable, because Triangle always knows exactly */ 161 | /* how much space to allocate. The former option is provided mainly for */ 162 | /* people who need to call Triangle from Fortran code, though it also makes */ 163 | /* possible some nasty space-saving tricks, like writing the output to the */ 164 | /* same arrays as the input. */ 165 | /* */ 166 | /* Triangle will not free() any input or output arrays, including those it */ 167 | /* allocates itself; that's up to you. You should free arrays allocated by */ 168 | /* Triangle by calling the trifree() procedure defined below. (By default, */ 169 | /* trifree() just calls the standard free() library procedure, but */ 170 | /* applications that call triangulate() may replace trimalloc() and */ 171 | /* trifree() in triangle.c to use specialized memory allocators.) */ 172 | /* */ 173 | /* Here's a guide to help you decide which fields you must initialize */ 174 | /* before you call triangulate(). */ 175 | /* */ 176 | /* `in': */ 177 | /* */ 178 | /* - `pointlist' must always point to a list of points; `numberofpoints' */ 179 | /* and `numberofpointattributes' must be properly set. */ 180 | /* `pointmarkerlist' must either be set to NULL (in which case all */ 181 | /* markers default to zero), or must point to a list of markers. If */ 182 | /* `numberofpointattributes' is not zero, `pointattributelist' must */ 183 | /* point to a list of point attributes. */ 184 | /* - If the `r' switch is used, `trianglelist' must point to a list of */ 185 | /* triangles, and `numberoftriangles', `numberofcorners', and */ 186 | /* `numberoftriangleattributes' must be properly set. If */ 187 | /* `numberoftriangleattributes' is not zero, `triangleattributelist' */ 188 | /* must point to a list of triangle attributes. If the `a' switch is */ 189 | /* used (with no number following), `trianglearealist' must point to a */ 190 | /* list of triangle area constraints. `neighborlist' may be ignored. */ 191 | /* - If the `p' switch is used, `segmentlist' must point to a list of */ 192 | /* segments, `numberofsegments' must be properly set, and */ 193 | /* `segmentmarkerlist' must either be set to NULL (in which case all */ 194 | /* markers default to zero), or must point to a list of markers. */ 195 | /* - If the `p' switch is used without the `r' switch, then */ 196 | /* `numberofholes' and `numberofregions' must be properly set. If */ 197 | /* `numberofholes' is not zero, `holelist' must point to a list of */ 198 | /* holes. If `numberofregions' is not zero, `regionlist' must point to */ 199 | /* a list of region constraints. */ 200 | /* - If the `p' switch is used, `holelist', `numberofholes', */ 201 | /* `regionlist', and `numberofregions' is copied to `out'. (You can */ 202 | /* nonetheless get away with not initializing them if the `r' switch is */ 203 | /* used.) */ 204 | /* - `edgelist', `edgemarkerlist', `normlist', and `numberofedges' may be */ 205 | /* ignored. */ 206 | /* */ 207 | /* `out': */ 208 | /* */ 209 | /* - `pointlist' must be initialized (NULL or pointing to memory) unless */ 210 | /* the `N' switch is used. `pointmarkerlist' must be initialized */ 211 | /* unless the `N' or `B' switch is used. If `N' is not used and */ 212 | /* `in->numberofpointattributes' is not zero, `pointattributelist' must */ 213 | /* be initialized. */ 214 | /* - `trianglelist' must be initialized unless the `E' switch is used. */ 215 | /* `neighborlist' must be initialized if the `n' switch is used. If */ 216 | /* the `E' switch is not used and (`in->numberofelementattributes' is */ 217 | /* not zero or the `A' switch is used), `elementattributelist' must be */ 218 | /* initialized. `trianglearealist' may be ignored. */ 219 | /* - `segmentlist' must be initialized if the `p' or `c' switch is used, */ 220 | /* and the `P' switch is not used. `segmentmarkerlist' must also be */ 221 | /* initialized under these circumstances unless the `B' switch is used. */ 222 | /* - `edgelist' must be initialized if the `e' switch is used. */ 223 | /* `edgemarkerlist' must be initialized if the `e' switch is used and */ 224 | /* the `B' switch is not. */ 225 | /* - `holelist', `regionlist', `normlist', and all scalars may be ignored.*/ 226 | /* */ 227 | /* `vorout' (only needed if `v' switch is used): */ 228 | /* */ 229 | /* - `pointlist' must be initialized. If `in->numberofpointattributes' */ 230 | /* is not zero, `pointattributelist' must be initialized. */ 231 | /* `pointmarkerlist' may be ignored. */ 232 | /* - `edgelist' and `normlist' must both be initialized. */ 233 | /* `edgemarkerlist' may be ignored. */ 234 | /* - Everything else may be ignored. */ 235 | /* */ 236 | /* After a call to triangulate(), the valid fields of `out' and `vorout' */ 237 | /* will depend, in an obvious way, on the choice of switches used. Note */ 238 | /* that when the `p' switch is used, the pointers `holelist' and */ 239 | /* `regionlist' are copied from `in' to `out', but no new space is */ 240 | /* allocated; be careful that you don't free() the same array twice. On */ 241 | /* the other hand, Triangle will never copy the `pointlist' pointer (or any */ 242 | /* others); new space is allocated for `out->pointlist', or if the `N' */ 243 | /* switch is used, `out->pointlist' remains uninitialized. */ 244 | /* */ 245 | /* All of the meaningful `numberof' fields will be properly set; for */ 246 | /* instance, `numberofedges' will represent the number of edges in the */ 247 | /* triangulation whether or not the edges were written. If segments are */ 248 | /* not used, `numberofsegments' will indicate the number of boundary edges. */ 249 | /* */ 250 | /*****************************************************************************/ 251 | 252 | struct triangulateio { 253 | float *pointlist; /* In / out */ 254 | float *pointattributelist; /* In / out */ 255 | int *pointmarkerlist; /* In / out */ 256 | int numberofpoints; /* In / out */ 257 | int numberofpointattributes; /* In / out */ 258 | 259 | int *trianglelist; /* In / out */ 260 | float *triangleattributelist; /* In / out */ 261 | float *trianglearealist; /* In only */ 262 | int *neighborlist; /* Out only */ 263 | int numberoftriangles; /* In / out */ 264 | int numberofcorners; /* In / out */ 265 | int numberoftriangleattributes; /* In / out */ 266 | 267 | int *segmentlist; /* In / out */ 268 | int *segmentmarkerlist; /* In / out */ 269 | int numberofsegments; /* In / out */ 270 | 271 | float *holelist; /* In / pointer to array copied out */ 272 | int numberofholes; /* In / copied out */ 273 | 274 | float *regionlist; /* In / pointer to array copied out */ 275 | int numberofregions; /* In / copied out */ 276 | 277 | int *edgelist; /* Out only */ 278 | int *edgemarkerlist; /* Not used with Voronoi diagram; out only */ 279 | float *normlist; /* Used only with Voronoi diagram; out only */ 280 | int numberofedges; /* Out only */ 281 | }; 282 | 283 | void triangulate(char *,triangulateio *,triangulateio *,triangulateio *); 284 | void trifree(int *memptr); 285 | 286 | -------------------------------------------------------------------------------- /include/libviso2/viso.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef VISO_H 23 | #define VISO_H 24 | 25 | #include "matrix.h" 26 | #include "matcher.h" 27 | 28 | namespace libviso2 { 29 | class VisualOdometry { 30 | 31 | public: 32 | 33 | // camera parameters (all are mandatory / need to be supplied) 34 | struct calibration { 35 | double f; // focal length (in pixels) 36 | double cu; // principal point (u-coordinate) 37 | double cv; // principal point (v-coordinate) 38 | calibration() { 39 | f = 1; 40 | cu = 0; 41 | cv = 0; 42 | } 43 | }; 44 | 45 | // bucketing parameters 46 | struct bucketing { 47 | int32_t max_features; // maximal number of features per bucket 48 | double bucket_width; // width of bucket 49 | double bucket_height; // height of bucket 50 | bucketing() { 51 | max_features = 2; 52 | bucket_width = 50; 53 | bucket_height = 50; 54 | } 55 | }; 56 | 57 | // general parameters 58 | struct parameters { 59 | Matcher::parameters match; // matching parameters 60 | VisualOdometry::bucketing bucket; // bucketing parameters 61 | VisualOdometry::calibration calib; // camera calibration parameters 62 | }; 63 | 64 | // constructor, takes as input a parameter structure: 65 | // do not instanciate this class directly, instanciate VisualOdometryMono 66 | // or VisualOdometryStereo instead! 67 | VisualOdometry(parameters param); 68 | 69 | // deconstructor 70 | ~VisualOdometry(); 71 | 72 | // call this function instead of the specialized ones, if you already have 73 | // feature matches, and simply want to compute visual odometry from them, without 74 | // using the internal matching functions. 75 | bool process(std::vector p_matched_) { 76 | p_matched = p_matched_; 77 | return updateMotion(); 78 | } 79 | 80 | // returns transformation from previous to current coordinates as a 4x4 81 | // homogeneous transformation matrix Tr_delta, with the following semantics: 82 | // p_t = Tr_delta * p_ {t-1} takes a point in the camera coordinate system 83 | // at time t_1 and maps it to the camera coordinate system at time t. 84 | // note: getMotion() returns the last transformation even when process() 85 | // has failed. this is useful if you wish to linearly extrapolate occasional 86 | // frames for which no correspondences have been found 87 | Matrix getMotion() { return Tr_delta; } 88 | 89 | // returns previous to current feature matches from internal matcher 90 | std::vector getMatches() { return matcher->getMatches(); } 91 | 92 | // returns the number of successfully matched points, after bucketing 93 | int32_t getNumberOfMatches() { return p_matched.size(); } 94 | 95 | // returns the number of inliers: num_inliers <= num_matched 96 | int32_t getNumberOfInliers() { return inliers.size(); } 97 | 98 | // returns the indices of all inliers 99 | std::vector getInlierIndices() { return inliers; } 100 | 101 | // given a vector of inliers computes gain factor between the current and 102 | // the previous frame. this function is useful if you want to reconstruct 3d 103 | // and you want to cancel the change of (unknown) camera gain. 104 | float getGain(std::vector inliers_) { return matcher->getGain(inliers_); } 105 | 106 | // streams out the current transformation matrix Tr_delta 107 | friend std::ostream &operator<<(std::ostream &os, VisualOdometry &viso) { 108 | Matrix p = viso.getMotion(); 109 | os << p.val[0][0] << " " << p.val[0][1] << " " << p.val[0][2] << " " << p.val[0][3] << " "; 110 | os << p.val[1][0] << " " << p.val[1][1] << " " << p.val[1][2] << " " << p.val[1][3] << " "; 111 | os << p.val[2][0] << " " << p.val[2][1] << " " << p.val[2][2] << " " << p.val[2][3]; 112 | return os; 113 | } 114 | 115 | protected: 116 | 117 | // calls bucketing and motion estimation 118 | bool updateMotion(); 119 | 120 | // compute transformation matrix from transformation vector 121 | Matrix transformationVectorToMatrix(std::vector tr); 122 | 123 | // compute motion from previous to current coordinate system 124 | // if motion could not be computed, resulting vector will be of size 0 125 | virtual std::vector estimateMotion(std::vector p_matched) = 0; 126 | 127 | // get random and unique sample of num numbers from 1:N 128 | std::vector getRandomSample(int32_t N, int32_t num); 129 | 130 | Matrix Tr_delta; // transformation (previous -> current frame) 131 | bool Tr_valid; // motion estimate exists? 132 | Matcher *matcher; // feature matcher 133 | std::vector inliers; // inlier set 134 | double *J; // jacobian 135 | double *p_observe; // observed 2d points 136 | double *p_predict; // predicted 2d points 137 | std::vector p_matched; // feature point matches 138 | 139 | private: 140 | 141 | parameters param; // common parameters 142 | }; 143 | } 144 | #endif // VISO_H 145 | 146 | -------------------------------------------------------------------------------- /include/libviso2/viso_mono.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef VISO_MONO_H 23 | #define VISO_MONO_H 24 | 25 | #include "viso.h" 26 | 27 | namespace libviso2 { 28 | class VisualOdometryMono : public VisualOdometry { 29 | 30 | public: 31 | 32 | // monocular-specific parameters (mandatory: height,pitch) 33 | struct parameters : public VisualOdometry::parameters { 34 | double height; // camera height above ground (meters) 35 | double pitch; // camera pitch (rad, negative=pointing down) 36 | int32_t ransac_iters; // number of RANSAC iterations 37 | double inlier_threshold; // fundamental matrix inlier threshold 38 | double motion_threshold; // directly return false on small motions 39 | parameters() { 40 | height = 1.0; 41 | pitch = 0.0; 42 | ransac_iters = 2000; 43 | inlier_threshold = 0.00001; 44 | motion_threshold = 100.0; 45 | } 46 | }; 47 | 48 | // constructor, takes as inpute a parameter structure 49 | VisualOdometryMono(parameters param); 50 | 51 | // deconstructor 52 | ~VisualOdometryMono(); 53 | 54 | // process a new image, pushs the image back to an internal ring buffer. 55 | // valid motion estimates are available after calling process for two times. 56 | // inputs: I ......... pointer to rectified image (uint8, row-aligned) 57 | // dims[0] ... width of I 58 | // dims[1] ... height of I 59 | // dims[2] ... bytes per line (often equal to width) 60 | // replace ... replace current image with I, without copying last current 61 | // image to previous image internally. this option can be used 62 | // when small/no motions are observed to obtain Tr_delta wrt 63 | // an older coordinate system / time step than the previous one. 64 | // output: returns false if motion too small or an error occured 65 | bool process(uint8_t *I, int32_t *dims, bool replace = false); 66 | 67 | private: 68 | 69 | template 70 | struct idx_cmp { 71 | idx_cmp(const T arr) : arr(arr) {} 72 | 73 | bool operator()(const size_t a, const size_t b) const { return arr[a] < arr[b]; } 74 | 75 | const T arr; 76 | }; 77 | 78 | std::vector estimateMotion(std::vector p_matched); 79 | 80 | Matrix smallerThanMedian(Matrix &X, double &median); 81 | 82 | bool normalizeFeaturePoints(std::vector &p_matched, Matrix &Tp, Matrix &Tc); 83 | 84 | void fundamentalMatrix(const std::vector &p_matched, const std::vector &active, Matrix &F); 85 | 86 | void EtoRt(Matrix &E, Matrix &K, std::vector &p_matched, Matrix &X, Matrix &R, Matrix &t); 87 | 88 | int32_t triangulateChieral(std::vector &p_matched, Matrix &K, Matrix &R, Matrix &t, Matrix &X); 89 | 90 | std::vector getInlier(std::vector &p_matched, Matrix &F); 91 | 92 | // parameters 93 | parameters param; 94 | }; 95 | } 96 | 97 | #endif // VISO_MONO_H 98 | 99 | -------------------------------------------------------------------------------- /include/libviso2/viso_stereo.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef VISO_STEREO_H 23 | #define VISO_STEREO_H 24 | 25 | #include "viso.h" 26 | 27 | namespace libviso2 { 28 | class VisualOdometryStereo : public VisualOdometry { 29 | 30 | public: 31 | 32 | // stereo-specific parameters (mandatory: base) 33 | struct parameters : public VisualOdometry::parameters { 34 | double base; // baseline (meters) 35 | int32_t ransac_iters; // number of RANSAC iterations 36 | double inlier_threshold; // fundamental matrix inlier threshold 37 | bool reweighting; // lower border weights (more robust to calibration errors) 38 | parameters() { 39 | base = 1.0; 40 | ransac_iters = 200; 41 | inlier_threshold = 2.0; 42 | reweighting = true; 43 | } 44 | }; 45 | 46 | // constructor, takes as inpute a parameter structure 47 | VisualOdometryStereo(parameters param); 48 | 49 | // deconstructor 50 | ~VisualOdometryStereo(); 51 | 52 | // process a new images, push the images back to an internal ring buffer. 53 | // valid motion estimates are available after calling process for two times. 54 | // inputs: I1 ........ pointer to rectified left image (uint8, row-aligned) 55 | // I2 ........ pointer to rectified right image (uint8, row-aligned) 56 | // dims[0] ... width of I1 and I2 (both must be of same size) 57 | // dims[1] ... height of I1 and I2 (both must be of same size) 58 | // dims[2] ... bytes per line (often equal to width) 59 | // replace ... replace current images with I1 and I2, without copying last current 60 | // images to previous images internally. this option can be used 61 | // when small/no motions are observed to obtain Tr_delta wrt 62 | // an older coordinate system / time step than the previous one. 63 | // output: returns false if an error occured 64 | bool process(uint8_t *I1, uint8_t *I2, int32_t *dims, bool replace = false); 65 | 66 | using VisualOdometry::process; 67 | 68 | 69 | private: 70 | 71 | std::vector estimateMotion(std::vector p_matched); 72 | 73 | enum result { 74 | UPDATED, FAILED, CONVERGED 75 | }; 76 | 77 | result updateParameters(std::vector &p_matched, std::vector &active, std::vector &tr, double step_size, double eps); 78 | 79 | void computeObservations(std::vector &p_matched, std::vector &active); 80 | 81 | void computeResidualsAndJacobian(std::vector &tr, std::vector &active); 82 | 83 | std::vector getInlier(std::vector &p_matched, std::vector &tr); 84 | 85 | double *X, *Y, *Z; // 3d points 86 | double *p_residual; // residuals (p_residual=p_observe-p_predict) 87 | 88 | // parameters 89 | parameters param; 90 | }; 91 | } 92 | 93 | #endif // VISO_STEREO_H 94 | 95 | -------------------------------------------------------------------------------- /python/python_example_flow.py: -------------------------------------------------------------------------------- 1 | 2 | from mpl_toolkits.mplot3d import Axes3D 3 | 4 | import matplotlib.pyplot as plt 5 | import matplotlib.image as mpimg 6 | import numpy as np 7 | 8 | import open3d as o3d 9 | 10 | import pyinterface 11 | import os, sys 12 | 13 | seq = 1 14 | left_dir = "/home/aljosa/data/kitti_tracking/training/image_02/%04d"%seq 15 | right_dir = "/home/aljosa/data/kitti_tracking/training/image_03/%04d"%seq 16 | #print (images[0].dtype) 17 | # Show images 18 | #for img in images: 19 | # plt.imshow(img) 20 | # plt.show() 21 | 22 | # Call our VO fnc 23 | focal = 721.537700 24 | cu = 609.559300 25 | cv = 172.854000 26 | baseline = 0.532719 27 | 28 | T_acc = np.identity(4) 29 | 30 | 31 | p_acc = np.array([0.0, 0.0, 0.0, 1.0]) 32 | p_0 = np.array([0.0, 0.0, 0.0, 1.0]) 33 | 34 | poses = p_acc 35 | 36 | pts_acc = None 37 | 38 | vo = pyinterface.VOEstimator() 39 | for frame in range(0, 350): 40 | print ("--- fr %d ---"%frame) 41 | l1 = os.path.join(left_dir, "%06d.png"%frame) 42 | r1 = os.path.join(right_dir, "%06d.png"%frame) 43 | 44 | # Read images 45 | images = [mpimg.imread(l1), mpimg.imread(r1)] 46 | 47 | # Conv to grayscale 48 | images = [np.mean(x, -1) for x in images] 49 | 50 | if frame == 0: 51 | vo.init(images[0], images[1], focal, cu, cv, baseline, True) 52 | else: 53 | T = vo.compute_pose(images[0], images[1]) 54 | 55 | F = vo.compute_flow(images[0], images[1], 0.1, 40.0) 56 | print (F.shape) 57 | 58 | # Update accumulated transf and compute current pose 59 | T_acc = T_acc.dot(np.linalg.inv(T)) 60 | p_acc = T_acc.dot(p_0) 61 | poses = np.vstack((poses, p_acc)) 62 | 63 | # Accumulate scene flow 64 | if pts_acc is None: 65 | pts_acc = F 66 | else: 67 | # pts_acc[:, 0:3] = T_acc.dot(pts_acc[0:3]) 68 | # pts_acc[:, 3:6] = T_acc.dot(pts_acc[3:6]) 69 | 70 | # print ("==== 2 ===== ") 71 | # print (F[:, 0:3].shape) 72 | # print (np.ones((F.shape[0], 1)).shape) 73 | 74 | # o3d_pc = o3d.geometry.PointCloud() 75 | # o3d_pc.points = o3d.utility.Vector3dVector(F[:, 0:3]) 76 | # objs = [] 77 | # objs.append(o3d_pc) 78 | # o3d.visualization.draw_geometries(objs) 79 | 80 | conmat = np.hstack((F[:, 0:3], np.ones((F.shape[0], 1)))) 81 | pts_tmp = T_acc.dot(np.transpose(conmat)) 82 | F[:, 0:3] = np.transpose(pts_tmp)[:, 0:3] 83 | 84 | #T_acc.dot(pts_acc[0:3]) 85 | #T_acc.dot(pts_acc[3:6]) 86 | pts_acc = np.vstack((pts_acc, F)) 87 | 88 | # # Show the poses 89 | # fig = plt.figure() 90 | # ax = fig.gca(projection='3d') 91 | # ax.scatter3D(poses[:, 0], poses[:, 1], poses[:, 2]) 92 | # plt.xlabel("X") 93 | # plt.ylabel("Y") 94 | # plt.show() 95 | 96 | # # Show the accumulated points 97 | # fig = plt.figure() 98 | # ax = fig.gca(projection='3d') 99 | # ax.scatter3D(pts_acc[:, 0], pts_acc[:, 1], pts_acc[:, 2], c="red") 100 | # #ax.scatter3D(pts_acc[:, 3], pts_acc[:, 4], pts_acc[:, 5], c="green") 101 | # plt.xlabel("X") 102 | # plt.ylabel("Y") 103 | # #plt.zlabel("Z") 104 | # #ax.set_yticks(np.arange(0, 1.2, 0.2)) 105 | # #ax.set_xticks(np.asarray([25, 100, 200, 300, 500, 700])) 106 | # # ax.set_ylim([-3.0, 3.0]) 107 | # # ax.set_xlim([-10.0, 10.0]) 108 | # # ax.set_zlim([0.0, 50.0]) 109 | # ax.view_init(elev=-72, azim=-91) 110 | # #plt.axis('equal') 111 | # plt.legend() 112 | # plt.show() 113 | 114 | # Point cloud 115 | o3d_pc = o3d.geometry.PointCloud() 116 | o3d_pc.points = o3d.utility.Vector3dVector(pts_acc[:, 0:3]) 117 | objs = [] 118 | objs.append(o3d_pc) 119 | o3d.visualization.draw_geometries(objs) -------------------------------------------------------------------------------- /python/python_example_vo.py: -------------------------------------------------------------------------------- 1 | 2 | from mpl_toolkits.mplot3d import Axes3D 3 | 4 | import matplotlib.pyplot as plt 5 | import matplotlib.image as mpimg 6 | import numpy as np 7 | 8 | import pyinterface 9 | import os, sys 10 | 11 | seq = 1 12 | left_dir = "/home/aljosa/data/kitti_tracking/training/image_02/%04d"%seq 13 | right_dir = "/home/aljosa/data/kitti_tracking/training/image_03/%04d"%seq 14 | #print (images[0].dtype) 15 | # Show images 16 | #for img in images: 17 | # plt.imshow(img) 18 | # plt.show() 19 | 20 | # Call our VO fnc 21 | focal = 721.537700 22 | cu = 609.559300 23 | cv = 172.854000 24 | baseline = 0.532719 25 | 26 | T_acc = np.identity(4) 27 | 28 | 29 | p_acc = np.array([0.0, 0.0, 0.0, 1.0]) 30 | p_0 = np.array([0.0, 0.0, 0.0, 1.0]) 31 | 32 | poses = p_acc 33 | 34 | vo = pyinterface.VOEstimator() 35 | for frame in range(0, 350): 36 | l1 = os.path.join(left_dir, "%06d.png"%frame) 37 | r1 = os.path.join(right_dir, "%06d.png"%frame) 38 | 39 | # Read images 40 | images = [mpimg.imread(l1), mpimg.imread(r1)] 41 | 42 | # Conv to grayscale 43 | images = [np.mean(x, -1) for x in images] 44 | 45 | if frame == 0: 46 | vo.init(images[0], images[1], focal, cu, cv, baseline) 47 | else: 48 | T = vo.compute_pose(images[0], images[1]) 49 | print (T) 50 | 51 | # Update accumulated transf and compute current pose 52 | T_acc = T_acc.dot(np.linalg.inv(T)) 53 | p_acc = T_acc.dot(p_0) 54 | poses = np.vstack((poses, p_acc)) 55 | 56 | # Show the poses 57 | fig = plt.figure() 58 | ax = fig.gca(projection='3d') 59 | ax.scatter3D(poses[:, 0], poses[:, 1], poses[:, 2]) 60 | plt.xlabel("X") 61 | plt.ylabel("Y") 62 | plt.show() 63 | -------------------------------------------------------------------------------- /src/libkitti/kitti.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2017. All rights reserved. 3 | Computer Vision Group, Visual Computing Institute 4 | RWTH Aachen University, Germany 5 | 6 | This file is part of the rwth_mot framework. 7 | Authors: Francis Engelmann, Aljosa Osep (engelmann, osep -at- vision.rwth-aachen.de) 8 | 9 | rwth_mot framework is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 3 of the License, or any later version. 12 | 13 | rwth_mot framework is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | rwth_mot framework; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #include "kitti.h" 23 | 24 | // Boost includes 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | namespace SUN { 32 | namespace utils { 33 | namespace KITTI { 34 | 35 | // ------------------------------------------------------------------------------- 36 | // +++ Calibration Implementation +++ 37 | // ------------------------------------------------------------------------------- 38 | Calibration::Calibration() { 39 | // 40 | } 41 | 42 | Calibration::Calibration(const std::string &filename) { 43 | if (!this->Open(filename)) { 44 | throw ("Could not Open calibration file: "+filename); 45 | } 46 | } 47 | 48 | Calibration::Calibration(const Calibration &calibration) { 49 | this->P_ = calibration.GetP(); 50 | 51 | this->pos_cam_0_ = calibration.GetPosCam0(); 52 | this->pos_cam_1_ = calibration.GetPosCam1(); 53 | this->pos_cam_2_ = calibration.GetPosCam2(); 54 | this->pos_cam_3_ = calibration.GetPosCam3(); 55 | 56 | this->proj_cam_0_ = calibration.GetProjCam0(); 57 | this->proj_cam_1_ = calibration.GetProjCam1(); 58 | this->proj_cam_2_ = calibration.GetProjCam2(); 59 | this->proj_cam_3_ = calibration.GetProjCam3(); 60 | 61 | this->R_rect_ = calibration.getR_rect(); 62 | 63 | this->Tr_imu_velo_ = calibration.getTr_imu_velo(); 64 | this->Tr_velo_cam_ = calibration.getTr_velo_cam(); 65 | 66 | this->baseline_ = std::sqrt( ((pos_cam_2_-pos_cam_3_).dot(pos_cam_2_-pos_cam_3_)) ); 67 | } 68 | 69 | 70 | // The calib files in odometry and tracking differ, so two cases are needed...beeeerk 71 | bool Calibration::Open(const std::string &filename) { 72 | calib_file.open(filename.c_str()); 73 | if (!calib_file.is_open()) { 74 | std::cout << "ERROR: Could not Open calibration file: " << filename << std::endl; 75 | return false; 76 | } 77 | // Read all lines of kitti calib file 78 | std::vector calibLines; 79 | std::string line; 80 | while (getline(calib_file,line)) { 81 | calibLines.push_back(line); 82 | } 83 | //calibrationFile_.close(); 84 | // Split the line 85 | if (calibLines.size() == 2) { // 86 | LabelsIO::ConvertLineToMatrix(calibLines.at(0), P_); 87 | LabelsIO::ConvertLineToMatrix(calibLines.at(0), proj_cam_2_); 88 | LabelsIO::ConvertLineToMatrix(calibLines.at(1), proj_cam_3_); 89 | } else { // 90 | LabelsIO::ConvertLineToMatrix(calibLines.at(1), P_); 91 | LabelsIO::ConvertLineToMatrix(calibLines.at(0), proj_cam_0_); 92 | LabelsIO::ConvertLineToMatrix(calibLines.at(1), proj_cam_1_); 93 | LabelsIO::ConvertLineToMatrix(calibLines.at(2), proj_cam_2_); 94 | LabelsIO::ConvertLineToMatrix(calibLines.at(3), proj_cam_3_); 95 | if (calibLines.size() == 7) { 96 | LabelsIO::ConvertLineToMatrix(calibLines.at(4), R_rect_); 97 | R_rect_(0,3) = 0.0f; R_rect_(1,3) = 0.0f; R_rect_(2,3) = 0.0f; 98 | LabelsIO::ConvertLineToMatrix(calibLines.at(5), Tr_velo_cam_); 99 | LabelsIO::ConvertLineToMatrix(calibLines.at(6), Tr_imu_velo_); 100 | } else if (calibLines.size() == 5) { // This is the case for the odometry dataset. 101 | LabelsIO::ConvertLineToMatrix(calibLines.at(4), Tr_velo_cam_); 102 | R_rect_ = Eigen::Matrix::Identity(); 103 | } else { 104 | Tr_velo_cam_ = Eigen::Matrix::Identity(); 105 | R_rect_ = Eigen::Matrix::Identity(); 106 | } 107 | } 108 | 109 | this->computeCameraCenters(); 110 | // Compute baselines as Euclidean-distance between corresponding camera-centers. 111 | this->baseline_ = sqrt( ((pos_cam_2_-pos_cam_3_).dot(pos_cam_2_-pos_cam_3_)) ); 112 | return true; 113 | } 114 | 115 | double Calibration::b(void) const { 116 | // Compute baseline as Euclidean-distance between corresponding camera-centers. 117 | return baseline_; 118 | } 119 | 120 | void Calibration::computeCameraCenters() { 121 | // Physical order of the cameras: 2 0 3 1 (Camera 0 is reference) 122 | // Camera 0 and 1 are grayscale, 2 and 3 are color, left and right respectively. 123 | 124 | // Compute camera positions t using given projection P = K*[R|t] 125 | // Assuming that R = 0, correct? 126 | 127 | pos_cam_0_(2,0) = proj_cam_0_(2,3); 128 | pos_cam_0_(0,0) = (proj_cam_0_(0,3) - proj_cam_0_(0,2)*pos_cam_0_(2,0)) / proj_cam_0_(0,0); 129 | pos_cam_0_(1,0) = (proj_cam_0_(1,3) - proj_cam_0_(1,2)*pos_cam_0_(2,0)) / proj_cam_0_(1,1); 130 | 131 | pos_cam_1_(2,0) = proj_cam_1_(2,3); 132 | pos_cam_1_(0,0) = (proj_cam_1_(0,3) - proj_cam_1_(0,2)*pos_cam_1_(2,0)) / proj_cam_1_(0,0); 133 | pos_cam_1_(1,0) = (proj_cam_1_(1,3) - proj_cam_1_(1,2)*pos_cam_1_(2,0)) / proj_cam_1_(1,1); 134 | 135 | pos_cam_2_(2,0) = proj_cam_2_(2,3); 136 | pos_cam_2_(0,0) = (proj_cam_2_(0,3) - proj_cam_2_(0,2)*pos_cam_2_(2,0)) / proj_cam_2_(0,0); 137 | pos_cam_2_(1,0) = (proj_cam_2_(1,3) - proj_cam_2_(1,2)*pos_cam_2_(2,0)) / proj_cam_2_(1,1); 138 | 139 | pos_cam_3_(2,0) = proj_cam_3_(2,3); 140 | pos_cam_3_(0,0) = (proj_cam_3_(0,3) - proj_cam_3_(0,2)*pos_cam_3_(2,0)) / proj_cam_3_(0,0); 141 | pos_cam_3_(1,0) = (proj_cam_3_(1,3) - proj_cam_3_(1,2)*pos_cam_3_(2,0)) / proj_cam_3_(1,1); 142 | } 143 | 144 | Eigen::Matrix Calibration::GetTr_cam0_cam2() const { 145 | Eigen::Matrix m = Eigen::Matrix::Identity();; 146 | m.block<3,1>(0,3) = pos_cam_2_.block<3,1>(0,0); 147 | return m; 148 | } 149 | 150 | // ------------------------------------------------------------------------------- 151 | // +++ LabelsIO Implementation +++ 152 | // ------------------------------------------------------------------------------- 153 | LabelsIO::LabelsIO() 154 | { 155 | 156 | } 157 | 158 | int LabelsIO::ReadLabels(const std::string &filename, std::vector &labels) { 159 | std::ifstream file(filename.c_str(), std::ios::in); 160 | if (file.fail()) { 161 | std::cout << "Could not read labels file " << filename << std::endl; 162 | return -1; 163 | } 164 | 165 | unsigned int numObjects = 0; 166 | 167 | //read line by line 168 | std::string line; 169 | while (getline (file,line)) { 170 | std::vector strings(17); // Each line has 30 space-seperated entries 171 | boost::split(strings,line,boost::is_any_of(" ")); 172 | TrackingLabel label; 173 | label.frame = atoi( (strings.at(0)).c_str() ); 174 | label.trackId = atoi( (strings.at(1)).c_str() ); 175 | if (label.trackId+1 > numObjects) { 176 | numObjects=label.trackId+1; 177 | } 178 | std::string type = strings.at(2); 179 | for (auto &c:type) c = std::toupper(c); 180 | 181 | if (type.compare("CAR")==0) { 182 | label.type = CAR; 183 | } else if (type.compare("VAN")==0) { 184 | label.type = VAN; 185 | } else if (type.compare("TRUCK")==0) { 186 | label.type = TRUCK; 187 | } else if (type.compare("PEDESTRIAN")==0) { 188 | label.type = PEDESTRIAN; 189 | } else if (type.compare("PERSON_SITTING")==0) { 190 | label.type = PERSON_SITTING; 191 | } else if (type.compare("CYCLIST")==0) { 192 | label.type = CYCLIST; 193 | } else if (type.compare("TRAM")==0) { 194 | label.type = TRAM; 195 | } else if (type.compare("MISC")==0) { 196 | label.type = MISC; 197 | } else if (type.compare("DONTCARE")==0) { 198 | label.type = DONT_CARE; 199 | } else if (type.compare("UNKNOWN")==0) { 200 | label.type = UNKNOWN_TYPE; 201 | } else if (type.compare("BACKGROUND")==0) { 202 | label.type = STATIC_BACKGROUND; 203 | } else { 204 | label.type = MISC; 205 | } 206 | label.truncated = atof( (strings.at(3)).c_str() ); 207 | label.occluded = static_cast(atoi( (strings.at(4)).c_str() )); 208 | label.alpha = atof( (strings.at(5)).c_str() ); 209 | label.boundingBox2D[0] = atof( (strings.at(6)).c_str() ); 210 | label.boundingBox2D[1] = atof( (strings.at(7)).c_str() ); 211 | label.boundingBox2D[2] = atof( (strings.at(8)).c_str() ); 212 | label.boundingBox2D[3] = atof( (strings.at(9)).c_str() ); 213 | label.dimensions[0] = atof( (strings.at(10)).c_str() ); 214 | label.dimensions[1] = atof( (strings.at(11)).c_str() ); 215 | label.dimensions[2] = atof( (strings.at(12)).c_str() ); 216 | label.location[0] = atof( (strings.at(13)).c_str() ); 217 | label.location[1] = atof( (strings.at(14)).c_str() ); 218 | label.location[2] = atof( (strings.at(15)).c_str() ); 219 | label.rotationY = atof( (strings.at(16)).c_str() ); 220 | 221 | if (strings.size() >= 18) // This is a bit hacky solution ... 222 | label.score = atof( (strings.at(17)).c_str() ); 223 | else 224 | label.score = 0.0; 225 | 226 | labels.push_back(label); 227 | } 228 | return numObjects; 229 | } 230 | 231 | 232 | int LabelsIO::ReadLabels3DOP(const std::string &filename, std::vector &labels) { 233 | 234 | labels.clear(); 235 | 236 | std::ifstream file(filename.c_str(), std::ios::in); 237 | if (file.fail()) { 238 | std::cout << "Could not read labels file " << filename << std::endl; 239 | return -1; 240 | } 241 | 242 | unsigned int numObjects = 0; 243 | 244 | //read line by line 245 | std::string line; 246 | while (getline (file,line)) { 247 | std::vector strings(17); // Each line has 30 space-seperated entries 248 | boost::split(strings,line,boost::is_any_of(" ")); 249 | TrackingLabel label; 250 | 251 | // Get frame from filename! 252 | auto frame_str = filename.substr(7, 2); 253 | label.frame = atoi(frame_str.c_str()); 254 | 255 | label.trackId = -1; //atoi( (strings.at(1)).c_str() ); 256 | if (label.trackId+1 > numObjects) { 257 | numObjects=label.trackId+1; 258 | } 259 | std::string type = strings.at(0); 260 | if (type.compare("Car")==0) { 261 | label.type = CAR; 262 | } else if (type.compare("Van")==0) { 263 | label.type = VAN; 264 | } else if (type.compare("Truck")==0) { 265 | label.type = TRUCK; 266 | } else if (type.compare("Pedestrian")==0) { 267 | label.type = PEDESTRIAN; 268 | } else if (type.compare("Person_sitting")==0) { 269 | label.type = PERSON_SITTING; 270 | } else if (type.compare("Cyclist")==0) { 271 | label.type = CYCLIST; 272 | } else if (type.compare("Tram")==0) { 273 | label.type = TRAM; 274 | } else if (type.compare("Misc")==0) { 275 | label.type = MISC; 276 | } else if (type.compare("DontCare")==0) { 277 | label.type = DONT_CARE; 278 | } else if (type.compare("UNKNOWN")==0) { 279 | label.type = UNKNOWN_TYPE; 280 | } else { 281 | label.type = MISC; 282 | } 283 | label.truncated = atof( (strings.at(1)).c_str() ); 284 | label.occluded = static_cast(atoi( (strings.at(2)).c_str() )); 285 | label.alpha = atof( (strings.at(3)).c_str() ); 286 | label.boundingBox2D[0] = atof( (strings.at(4)).c_str() ); 287 | label.boundingBox2D[1] = atof( (strings.at(5)).c_str() ); 288 | label.boundingBox2D[2] = atof( (strings.at(6)).c_str() ); 289 | label.boundingBox2D[3] = atof( (strings.at(7)).c_str() ); 290 | label.dimensions[0] = atof( (strings.at(8)).c_str() ); 291 | label.dimensions[1] = atof( (strings.at(9)).c_str() ); 292 | label.dimensions[2] = atof( (strings.at(10)).c_str() ); 293 | label.location[0] = atof( (strings.at(11)).c_str() ); 294 | label.location[1] = atof( (strings.at(12)).c_str() ); 295 | label.location[2] = atof( (strings.at(13)).c_str() ); 296 | label.rotationY = atof( (strings.at(14)).c_str() ); 297 | label.score = atof( (strings.at(15)).c_str() ); 298 | 299 | 300 | labels.push_back(label); 301 | } 302 | return numObjects; 303 | } 304 | 305 | int LabelsIO::WriteLabels(std::vector &labels, const std::string &filename) { 306 | 307 | std::ofstream file(filename.c_str(), std::ofstream::app); 308 | if (!file.is_open()) { 309 | std::cout << "Could not write labels file: " << filename << std::endl; 310 | return -1; 311 | } 312 | 313 | LabelsIO sun_io; 314 | unsigned int numObjects = 0; 315 | 316 | for (const auto &label:labels) { 317 | // Generate label string based on the value of LabelType type; 318 | std::string category_string = "UNKNOWN"; 319 | if (label.type==CAR) { 320 | category_string = "Car"; 321 | } else if (label.type==VAN) { 322 | category_string = "Van"; 323 | } else if (label.type==TRUCK) { 324 | category_string = "Truck"; 325 | } else if (label.type==PEDESTRIAN) { 326 | category_string = "Pedestrian"; 327 | } else if (label.type==PERSON_SITTING) { 328 | category_string = "Person_Sitting"; 329 | } else if (label.type==CYCLIST) { 330 | category_string = "Cyclist"; 331 | } else if (label.type==TRAM) { 332 | category_string = "Tram"; 333 | } else if (label.type==MISC) { 334 | category_string = "Misc"; 335 | } else if (label.type==DONT_CARE) { 336 | category_string = "DontCare"; 337 | } 338 | 339 | sun_io.WriteStringToFile(std::to_string(label.frame), file); 340 | sun_io.WriteStringToFile(std::to_string(label.trackId), file); 341 | sun_io.WriteStringToFile(category_string, file); 342 | sun_io.WriteStringToFile(std::to_string(label.truncated), file); 343 | sun_io.WriteStringToFile(std::to_string(label.occluded), file); 344 | sun_io.WriteStringToFile(std::to_string(label.alpha), file); 345 | sun_io.WriteStringToFile(std::to_string(label.boundingBox2D[0]), file); 346 | sun_io.WriteStringToFile(std::to_string(label.boundingBox2D[1]), file); 347 | sun_io.WriteStringToFile(std::to_string(label.boundingBox2D[2]), file); 348 | sun_io.WriteStringToFile(std::to_string(label.boundingBox2D[3]), file); 349 | sun_io.WriteStringToFile(std::to_string(label.dimensions[0]), file); 350 | sun_io.WriteStringToFile(std::to_string(label.dimensions[1]), file); 351 | sun_io.WriteStringToFile(std::to_string(label.dimensions[2]), file); 352 | sun_io.WriteStringToFile(std::to_string(label.location[0]), file); 353 | sun_io.WriteStringToFile(std::to_string(label.location[1]), file); 354 | sun_io.WriteStringToFile(std::to_string(label.location[2]), file); 355 | sun_io.WriteStringToFile(std::to_string(label.rotationY), file); 356 | sun_io.WriteStringToFile(std::to_string(label.score), file); 357 | 358 | file << "\n"; 359 | 360 | if (label.trackId+1 > numObjects) { 361 | numObjects=label.trackId+1; 362 | } 363 | } 364 | return numObjects; 365 | } 366 | 367 | void LabelsIO::ConvertLineToMatrix(std::string &line, Eigen::Matrix &matrix) { 368 | std::vector strings;//(3*4+1); 369 | boost::trim(line); 370 | boost::split(strings,line,boost::is_any_of(" ")); 371 | size_t cols,rows; 372 | unsigned char offset = 1; 373 | if (strings.size() == 13) { 374 | cols = 4; rows = 3; 375 | } else if (strings.size() == 12) { 376 | cols = 4; rows = 3; 377 | offset = 0; 378 | } else if (strings.size() == 10) { 379 | cols = 3; rows = 3; 380 | } else { 381 | std::cout << "Error while converting line to matrix." << std::endl; 382 | std::cout << "Using size " << strings.size() << std::endl; 383 | } 384 | for (size_t i=0; i &matrix) { 393 | matrix = Eigen::MatrixXd::Identity(4,4); 394 | Eigen::Matrix tmp; 395 | LabelsIO::ConvertLineToMatrix(line, tmp); 396 | matrix.block<3,4>(0,0) = tmp; 397 | } 398 | 399 | void LabelsIO::WriteStringToFile(std::string text, std::ofstream &file) { 400 | file << text; 401 | file << " "; 402 | } 403 | } 404 | } 405 | } -------------------------------------------------------------------------------- /src/libkitti/kitti.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2017. All rights reserved. 3 | Computer Vision Group, Visual Computing Institute 4 | RWTH Aachen University, Germany 5 | 6 | This file is part of the rwth_mot framework. 7 | Authors: Francis Engelmann, Aljosa Osep (engelmann, osep -at- vision.rwth-aachen.de) 8 | 9 | rwth_mot framework is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 3 of the License, or any later version. 12 | 13 | rwth_mot framework is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | rwth_mot framework; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef GOT_UTILS_KITTI_H 23 | #define GOT_UTILS_KITTI_H 24 | 25 | // Eigen includes 26 | #include 27 | 28 | // C/C++ includes 29 | #include 30 | #include 31 | #include 32 | 33 | namespace SUN { 34 | namespace utils { 35 | namespace KITTI { 36 | 37 | enum CategoryType { 38 | CAR = 0, 39 | VAN = 1, 40 | TRUCK = 2, 41 | PEDESTRIAN = 3, 42 | PERSON_SITTING = 4, 43 | CYCLIST = 5, 44 | TRAM = 6, 45 | MISC = 7, 46 | DONT_CARE = 8, 47 | UNKNOWN_TYPE = 9, 48 | STATIC_BACKGROUND = 10, 49 | NOISE = 11, 50 | }; 51 | 52 | typedef enum LabelOcclusion 53 | { 54 | NOT_SPECIFIED = -1, 55 | FULLY_VISIBLE = 0, 56 | PARTLY_OCCLUDED = 1, 57 | LARGELY_OCCLUDED = 2, 58 | UNKNOWN = 3 59 | } LabelOccluded; 60 | 61 | //! Structs 62 | struct TrackingLabel 63 | { 64 | unsigned int frame; // The frame in which this label occurs 65 | int trackId; // The tracklet id, same physical objects have same trackId, can be -1 if dont care 66 | CategoryType type; 67 | float truncated; 68 | LabelOccluded occluded; 69 | double alpha; // Not used here 70 | double boundingBox2D[4]; // Pixel coordinates of 2d bounding box - not used in this code 71 | double dimensions[3]; // Height, width, depth [m] in 3D 72 | double location[3]; // XYZ position [m] in 3D 73 | double rotationY; // Rotation around y-axe 74 | double score; 75 | }; 76 | 77 | // ------------------------------------------------------------------------------- 78 | // +++ LabelsIO Class +++ 79 | // ------------------------------------------------------------------------------- 80 | 81 | /** 82 | * @brief Class used for parsing KITTI dataset calibration files. 83 | * @author Francis Engelmann (englemann@vision.rwth-aachen.de) 84 | */ 85 | class LabelsIO 86 | { 87 | public: 88 | /** 89 | * @brief empty IO constructor 90 | */ 91 | LabelsIO(); 92 | 93 | /** 94 | * @brief readLabels 95 | * @param filename 96 | * @param labels 97 | * @return the number of distinctive objects (i.e. tracks) in the scene 98 | */ 99 | static int ReadLabels(const std::string &filename, std::vector &labels); 100 | 101 | // This parses 3DOP detections in Francis/Joerg format. Duplicate func, so remove when not needed anymore! 102 | static int ReadLabels3DOP(const std::string &filename, std::vector &labels); 103 | 104 | /** 105 | * @brief ReadLabels 106 | * @param labels 107 | * @param filename 108 | * @return the number of distinctive objects (i.e. tracks) in the scene 109 | */ 110 | static int WriteLabels(std::vector &labels, const std::string &filename); 111 | 112 | /** 113 | * @brief ConvertLineToMatrix 114 | * @param line[in] the space-seperated line 115 | * @param matrix[out] the output 3x4 matrix 116 | */ 117 | static void ConvertLineToMatrix(std::string &line, Eigen::Matrix &matrix); 118 | static void ConvertLineToMatrix(std::string &line, Eigen::Matrix &matrix); 119 | 120 | protected: 121 | void WriteStringToFile(std::string text, std::ofstream &file); 122 | }; 123 | 124 | // ------------------------------------------------------------------------------- 125 | // +++ Calibration Class +++ 126 | // ------------------------------------------------------------------------------- 127 | 128 | /** 129 | * @brief Class used for parsing KITTI dataset calibration files. 130 | * @author Francis Engelmann (englemann@vision.rwth-aachen.de) 131 | */ 132 | class Calibration 133 | { 134 | public: 135 | /** 136 | * @brief empty constructor 137 | */ 138 | Calibration(); 139 | 140 | Calibration(const Calibration &calibration); 141 | 142 | /** 143 | * @brief Calibration constructor, reads specified calib file 144 | * @param filename - the path e.g. training/calib/0000.txt 145 | */ 146 | explicit Calibration(const std::string& filename); 147 | 148 | /** 149 | * @brief Opens and reads the specified calib file 150 | * @param filename - the path e.g. training/calib/0000.txt 151 | * @return wether speciifed file could be opened or not 152 | */ 153 | bool Open(const std::string &filename); 154 | 155 | /** 156 | * @brief reads the focal-length from 157 | * @return focal-length 158 | */ 159 | double f(void) const { return P_(0,0); } 160 | 161 | /** 162 | * @brief c_u 163 | * @return Principal-point (horizontal direction) 164 | */ 165 | double c_u(void) const { return P_(0,2); } 166 | 167 | /** 168 | * @brief c_v 169 | * @return Principal-point (vertical direction) 170 | */ 171 | double c_v(void) const { return P_(1,2); } 172 | 173 | /** 174 | * @brief computes the base-line from pose matrix 175 | * @return base-line between color cameras 176 | */ 177 | double b(void) const; 178 | 179 | Eigen::Matrix GetTr_cam0_cam2() const; 180 | 181 | Eigen::Matrix GetK(void) const { return proj_cam_2_.block<3,3>(0,0);} 182 | Eigen::Matrix GetP(void) const { return P_; } 183 | Eigen::Matrix GetProjCam0(void) const { return proj_cam_0_; } 184 | Eigen::Matrix GetProjCam1(void) const { return proj_cam_1_; } 185 | Eigen::Matrix GetProjCam2(void) const { return proj_cam_2_; } 186 | Eigen::Matrix GetProjCam3(void) const { return proj_cam_3_; } 187 | 188 | Eigen::Matrix GetPosCam0(void) const { return pos_cam_0_; } 189 | Eigen::Matrix GetPosCam1(void) const { return pos_cam_1_; } 190 | Eigen::Matrix GetPosCam2(void) const { return pos_cam_2_; } 191 | Eigen::Matrix GetPosCam3(void) const { return pos_cam_3_; } 192 | 193 | Eigen::Matrix getR_rect(void) const { return R_rect_; } 194 | Eigen::Matrix getTr_velo_cam(void) const { return Tr_velo_cam_; } 195 | Eigen::Matrix getTr_imu_velo(void) const { return Tr_imu_velo_; } 196 | 197 | void computeCameraCenters(); 198 | 199 | private: 200 | std::ifstream calib_file; 201 | 202 | // Projection matrices, 3x4 203 | Eigen::Matrix P_; 204 | Eigen::Matrix proj_cam_0_; 205 | Eigen::Matrix proj_cam_1_; 206 | Eigen::Matrix proj_cam_2_; 207 | Eigen::Matrix proj_cam_3_; 208 | 209 | // [R|t] matrices, 4x4 210 | Eigen::Matrix R_rect_; 211 | Eigen::Matrix Tr_velo_cam_; 212 | Eigen::Matrix Tr_imu_velo_; 213 | 214 | // Position of camera centers 215 | Eigen::Matrix pos_cam_0_; 216 | Eigen::Matrix pos_cam_1_; 217 | Eigen::Matrix pos_cam_2_; 218 | Eigen::Matrix pos_cam_3_; 219 | 220 | double baseline_; 221 | }; 222 | 223 | } 224 | } 225 | } 226 | 227 | 228 | #endif //GOT_UTILS_KITTI_H 229 | -------------------------------------------------------------------------------- /src/libviso2/reconstruction.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #include 23 | #include 24 | 25 | using namespace std; 26 | 27 | namespace libviso2 { 28 | Reconstruction::Reconstruction() { 29 | K = Matrix::eye(3); 30 | 31 | // First transformation matrix 32 | Tr_total.push_back(Matrix::eye(4)); 33 | 34 | // First inverse transformation matrix 35 | Tr_inv_total.push_back(Matrix::eye(4)); 36 | } 37 | 38 | Reconstruction::~Reconstruction() { 39 | } 40 | 41 | void Reconstruction::setCalibration(FLOAT f, FLOAT cu, FLOAT cv) { 42 | FLOAT K_data[9] = {f, 0, cu, 0, f, cv, 0, 0, 1}; 43 | K = Matrix(3, 3, K_data); 44 | FLOAT cam_pitch = -0.08; 45 | FLOAT cam_height = 1.6; 46 | Tr_cam_road = Matrix(4, 4); 47 | Tr_cam_road.val[0][0] = 1; 48 | Tr_cam_road.val[1][1] = +cos(cam_pitch); 49 | Tr_cam_road.val[1][2] = -sin(cam_pitch); 50 | Tr_cam_road.val[2][1] = +sin(cam_pitch); 51 | Tr_cam_road.val[2][2] = +cos(cam_pitch); 52 | Tr_cam_road.val[0][3] = 0; 53 | Tr_cam_road.val[1][3] = -cam_height; 54 | Tr_cam_road.val[2][3] = 0; 55 | 56 | // First camera projection matrix 57 | P_total.push_back(K * Matrix::eye(4).getMat(0, 0, 2, 3)); 58 | } 59 | 60 | void 61 | Reconstruction::update(vector p_matched, Matrix Tr, int32_t point_type, int32_t min_track_length, double max_dist, double min_angle) { 62 | 63 | // update transformation vector 64 | Matrix Tr_total_curr; 65 | if (Tr_total.size() == 0) Tr_total_curr = Matrix::inv(Tr); 66 | else Tr_total_curr = Tr_total.back() * Matrix::inv(Tr); 67 | Tr_total.push_back(Tr_total_curr); 68 | Tr_inv_total.push_back(Matrix::inv(Tr_total_curr)); 69 | 70 | // update projection vector 71 | Matrix P_total_curr = K * Matrix::inv(Tr_total_curr).getMat(0, 0, 2, 3); 72 | P_total.push_back(P_total_curr); 73 | 74 | // current frame 75 | int32_t current_frame = Tr_total.size() - 1; // 0-based frame number 76 | 77 | // create index vector 78 | int32_t track_idx_max = 0; 79 | for (vector::iterator m = p_matched.begin(); m != p_matched.end(); m++) 80 | if (m->i1p > track_idx_max) 81 | track_idx_max = m->i1p; 82 | for (vector::iterator t = tracks.begin(); t != tracks.end(); t++) 83 | if (t->last_idx > track_idx_max) 84 | track_idx_max = t->last_idx; 85 | int32_t *track_idx = new int32_t[track_idx_max + 1]; 86 | for (int32_t i = 0; i <= track_idx_max; i++) 87 | track_idx[i] = -1; 88 | for (int32_t i = 0; i < tracks.size(); i++) 89 | track_idx[tracks[i].last_idx] = i; 90 | 91 | // associate matches to tracks 92 | for (vector::iterator m = p_matched.begin(); m != p_matched.end(); m++) { 93 | 94 | // track index (-1 = no existing track) 95 | int32_t idx = track_idx[m->i1p]; 96 | 97 | // add to existing track 98 | if (idx >= 0 && tracks[idx].last_frame == current_frame - 1) { 99 | 100 | tracks[idx].pixels.push_back(point2d(m->u1c, m->v1c)); 101 | tracks[idx].last_frame = current_frame; 102 | tracks[idx].last_idx = m->i1c; 103 | 104 | // create new track 105 | } else { 106 | track t; 107 | t.pixels.push_back(point2d(m->u1p, m->v1p)); 108 | t.pixels.push_back(point2d(m->u1c, m->v1c)); 109 | t.first_frame = current_frame - 1; 110 | t.last_frame = current_frame; 111 | t.last_idx = m->i1c; 112 | tracks.push_back(t); 113 | } 114 | } 115 | 116 | // copy tracks 117 | vector tracks_copy = tracks; 118 | tracks.clear(); 119 | 120 | // devise tracks into active or reconstruct 3d points 121 | for (vector::iterator t = tracks_copy.begin(); t != tracks_copy.end(); t++) { 122 | 123 | // track has been extended 124 | if (t->last_frame == current_frame) { 125 | 126 | // push back to active tracks 127 | tracks.push_back(*t); 128 | 129 | // track lost 130 | } else { 131 | 132 | // add to 3d reconstruction 133 | if (t->pixels.size() >= min_track_length) { 134 | 135 | // 3d point 136 | point3d p; 137 | 138 | // try to init point from first and last track frame 139 | if (initPoint(*t, p)) { 140 | if (pointType(*t, p) >= point_type) 141 | if (refinePoint(*t, p)) 142 | if (pointDistance(*t, p) < max_dist && rayAngle(*t, p) > min_angle) 143 | points.push_back(p); 144 | } 145 | } 146 | } 147 | } 148 | 149 | //cout << "P: " << points.size() << endl; 150 | //testJacobian(); 151 | 152 | delete track_idx; 153 | } 154 | 155 | bool Reconstruction::initPoint(const track &t, point3d &p) { 156 | 157 | // projection matrices 158 | Matrix P1 = P_total[t.first_frame]; 159 | Matrix P2 = P_total[t.last_frame]; 160 | 161 | // observations 162 | point2d p1 = t.pixels.front(); 163 | point2d p2 = t.pixels.back(); 164 | 165 | // triangulation via orthogonal regression 166 | Matrix J(4, 4); 167 | Matrix U, S, V; 168 | for (int32_t j = 0; j < 4; j++) { 169 | J.val[0][j] = P1.val[2][j] * p1.u - P1.val[0][j]; 170 | J.val[1][j] = P1.val[2][j] * p1.v - P1.val[1][j]; 171 | J.val[2][j] = P2.val[2][j] * p2.u - P2.val[0][j]; 172 | J.val[3][j] = P2.val[2][j] * p2.v - P2.val[1][j]; 173 | } 174 | J.svd(U, S, V); 175 | 176 | // return false if this point is at infinity 177 | float w = V.val[3][3]; 178 | if (fabs(w) < 1e-10) 179 | return false; 180 | 181 | // return 3d point 182 | p = point3d(V.val[0][3] / w, V.val[1][3] / w, V.val[2][3] / w); 183 | return true; 184 | } 185 | 186 | bool Reconstruction::refinePoint(const track &t, point3d &p) { 187 | 188 | int32_t num_frames = t.pixels.size(); 189 | J = new FLOAT[6 * num_frames]; 190 | p_observe = new FLOAT[2 * num_frames]; 191 | p_predict = new FLOAT[2 * num_frames]; 192 | 193 | int32_t iter = 0; 194 | Reconstruction::result result = UPDATED; 195 | while (result == UPDATED) { 196 | result = updatePoint(t, p, 1, 1e-5); 197 | if (iter++ > 20 || result == CONVERGED) 198 | break; 199 | } 200 | 201 | delete J; 202 | delete p_observe; 203 | delete p_predict; 204 | 205 | if (result == CONVERGED) 206 | return true; 207 | else 208 | return false; 209 | } 210 | 211 | double Reconstruction::pointDistance(const track &t, point3d &p) { 212 | int32_t mid_frame = (t.first_frame + t.last_frame) / 2; 213 | double dx = Tr_total[mid_frame].val[0][3] - p.x; 214 | double dy = Tr_total[mid_frame].val[1][3] - p.y; 215 | double dz = Tr_total[mid_frame].val[2][3] - p.z; 216 | return sqrt(dx * dx + dy * dy + dz * dz); 217 | } 218 | 219 | double Reconstruction::rayAngle(const track &t, point3d &p) { 220 | Matrix c1 = Tr_total[t.first_frame].getMat(0, 3, 2, 3); 221 | Matrix c2 = Tr_total[t.last_frame].getMat(0, 3, 2, 3); 222 | Matrix pt(3, 1); 223 | pt.val[0][0] = p.x; 224 | pt.val[1][0] = p.y; 225 | pt.val[2][0] = p.z; 226 | Matrix v1 = c1 - pt; 227 | Matrix v2 = c2 - pt; 228 | FLOAT n1 = v1.l2norm(); 229 | FLOAT n2 = v2.l2norm(); 230 | if (n1 < 1e-10 || n2 < 1e-10) 231 | return 1000; 232 | v1 = v1 / n1; 233 | v2 = v2 / n2; 234 | return acos(fabs((~v1 * v2).val[0][0])) * 180.0 / M_PI; 235 | } 236 | 237 | int32_t Reconstruction::pointType(const track &t, point3d &p) { 238 | 239 | // project point to first and last camera coordinates 240 | Matrix x(4, 1); 241 | x.val[0][0] = p.x; 242 | x.val[1][0] = p.y; 243 | x.val[2][0] = p.z; 244 | x.val[3][0] = 1; 245 | Matrix x1c = Tr_inv_total[t.first_frame] * x; 246 | Matrix x2c = Tr_inv_total[t.last_frame] * x; 247 | Matrix x2r = Tr_cam_road * x2c; 248 | 249 | // point not visible 250 | if (x1c.val[2][0] <= 1 || x2c.val[2][0] <= 1) 251 | return -1; 252 | 253 | // below road 254 | if (x2r.val[1][0] > 0.5) 255 | return 0; 256 | 257 | // road 258 | if (x2r.val[1][0] > -1) 259 | return 1; 260 | 261 | // obstacle 262 | return 2; 263 | } 264 | 265 | Reconstruction::result Reconstruction::updatePoint(const track &t, point3d &p, const FLOAT &step_size, const FLOAT &eps) { 266 | 267 | // number of frames 268 | int32_t num_frames = t.pixels.size(); 269 | 270 | // extract observations 271 | computeObservations(t.pixels); 272 | 273 | // compute predictions 274 | vector::iterator P_begin = P_total.begin() + t.first_frame; 275 | vector::iterator P_end = P_begin + num_frames - 1; 276 | 277 | if (!computePredictionsAndJacobian(P_begin, P_end, p)) 278 | return FAILED; 279 | 280 | // init 281 | Matrix A(3, 3); 282 | Matrix B(3, 1); 283 | 284 | // fill matrices A and B 285 | for (int32_t m = 0; m < 3; m++) { 286 | for (int32_t n = 0; n < 3; n++) { 287 | FLOAT a = 0; 288 | for (int32_t i = 0; i < 2 * num_frames; i++) 289 | a += J[i * 3 + m] * J[i * 3 + n]; 290 | A.val[m][n] = a; 291 | } 292 | FLOAT b = 0; 293 | for (int32_t i = 0; i < 2 * num_frames; i++) 294 | b += J[i * 3 + m] * (p_observe[i] - p_predict[i]); 295 | B.val[m][0] = b; 296 | } 297 | 298 | // perform elimination 299 | if (B.solve(A)) { 300 | p.x += step_size * B.val[0][0]; 301 | p.y += step_size * B.val[1][0]; 302 | p.z += step_size * B.val[2][0]; 303 | if (fabs(B.val[0][0]) < eps && fabs(B.val[1][0]) < eps && fabs(B.val[2][0]) < eps) 304 | return CONVERGED; 305 | else 306 | return UPDATED; 307 | } 308 | return FAILED; 309 | } 310 | 311 | void Reconstruction::computeObservations(const vector &pixels) { 312 | for (int32_t i = 0; i < pixels.size(); i++) { 313 | p_observe[i * 2 + 0] = pixels[i].u; 314 | p_observe[i * 2 + 1] = pixels[i].v; 315 | } 316 | } 317 | 318 | bool Reconstruction::computePredictionsAndJacobian(const vector::iterator &P_begin, const vector::iterator &P_end, point3d &p) { 319 | 320 | // for all frames do 321 | int32_t k = 0; 322 | for (vector::iterator P = P_begin; P <= P_end; P++) { 323 | 324 | // precompute coefficients 325 | FLOAT a = P->val[0][0] * p.x + P->val[0][1] * p.y + P->val[0][2] * p.z + P->val[0][3]; 326 | FLOAT b = P->val[1][0] * p.x + P->val[1][1] * p.y + P->val[1][2] * p.z + P->val[1][3]; 327 | FLOAT c = P->val[2][0] * p.x + P->val[2][1] * p.y + P->val[2][2] * p.z + P->val[2][3]; 328 | FLOAT cc = c * c; 329 | 330 | // check singularities 331 | if (cc < 1e-10) 332 | return false; 333 | 334 | // set jacobian entries 335 | J[k * 6 + 0] = (P->val[0][0] * c - P->val[2][0] * a) / cc; 336 | J[k * 6 + 1] = (P->val[0][1] * c - P->val[2][1] * a) / cc; 337 | J[k * 6 + 2] = (P->val[0][2] * c - P->val[2][2] * a) / cc; 338 | J[k * 6 + 3] = (P->val[1][0] * c - P->val[2][0] * b) / cc; 339 | J[k * 6 + 4] = (P->val[1][1] * c - P->val[2][1] * b) / cc; 340 | J[k * 6 + 5] = (P->val[1][2] * c - P->val[2][2] * b) / cc; 341 | 342 | // set prediction 343 | p_predict[k * 2 + 0] = a / c; // u 344 | p_predict[k * 2 + 1] = b / c; // v 345 | 346 | k++; 347 | } 348 | 349 | // success 350 | return true; 351 | } 352 | 353 | void Reconstruction::testJacobian() { 354 | cout << "=================================" << endl; 355 | cout << "TESTING JACOBIAN" << endl; 356 | FLOAT delta = 1e-5; 357 | vector P; 358 | Matrix A(3, 4); 359 | A.setMat(K, 0, 0); 360 | P.push_back(A); 361 | A.setMat(Matrix::rotMatX(0.1) * Matrix::rotMatY(0.1) * Matrix::rotMatZ(0.1), 0, 0); 362 | A.val[1][3] = 1; 363 | A.val[1][3] = 0.1; 364 | A.val[1][3] = -1.5; 365 | P.push_back(K * A); 366 | cout << P[0] << endll; 367 | cout << P[1] << endll; 368 | J = new FLOAT[6 * 2]; 369 | p_observe = new FLOAT[2 * 2]; 370 | p_predict = new FLOAT[2 * 2]; 371 | 372 | point3d p_ref(0.1, 0.2, 0.3); 373 | 374 | FLOAT p_predict1[4]; 375 | FLOAT p_predict2[4]; 376 | point3d p1 = p_ref; 377 | 378 | for (int32_t i = 0; i < 3; i++) { 379 | point3d p2 = p_ref; 380 | if (i == 0) p2.x += delta; 381 | else if (i == 1) p2.y += delta; 382 | else p2.z += delta; 383 | cout << endl << "Checking parameter " << i << ":" << endl; 384 | cout << "param1: "; 385 | cout << p1.x << " " << p1.y << " " << p1.z << endl; 386 | cout << "param2: "; 387 | cout << p2.x << " " << p2.y << " " << p2.z << endl; 388 | computePredictionsAndJacobian(P.begin(), P.end(), p1); 389 | memcpy(p_predict1, p_predict, 4 * sizeof(FLOAT)); 390 | computePredictionsAndJacobian(P.begin(), P.end(), p2); 391 | memcpy(p_predict2, p_predict, 4 * sizeof(FLOAT)); 392 | for (int32_t j = 0; j < 4; j++) { 393 | cout << "num: " << (p_predict2[j] - p_predict1[j]) / delta; 394 | cout << ", ana: " << J[j * 3 + i] << endl; 395 | } 396 | } 397 | 398 | delete J; 399 | delete p_observe; 400 | delete p_predict; 401 | cout << "=================================" << endl; 402 | } 403 | } -------------------------------------------------------------------------------- /src/libviso2/viso.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #include 23 | 24 | #include 25 | 26 | using namespace std; 27 | 28 | namespace libviso2 { 29 | VisualOdometry::VisualOdometry(parameters param) : param(param) { 30 | J = 0; 31 | p_observe = 0; 32 | p_predict = 0; 33 | matcher = new Matcher(param.match); 34 | Tr_delta = Matrix::eye(4); 35 | Tr_valid = false; 36 | srand(0); 37 | } 38 | 39 | VisualOdometry::~VisualOdometry() { 40 | delete matcher; 41 | } 42 | 43 | bool VisualOdometry::updateMotion() { 44 | 45 | // estimate motion 46 | vector tr_delta = estimateMotion(p_matched); 47 | 48 | // on failure 49 | if (tr_delta.size() != 6) 50 | return false; 51 | 52 | // set transformation matrix (previous to current frame) 53 | Tr_delta = transformationVectorToMatrix(tr_delta); 54 | Tr_valid = true; 55 | 56 | // success 57 | return true; 58 | } 59 | 60 | Matrix VisualOdometry::transformationVectorToMatrix(vector tr) { 61 | 62 | // extract parameters 63 | double rx = tr[0]; 64 | double ry = tr[1]; 65 | double rz = tr[2]; 66 | double tx = tr[3]; 67 | double ty = tr[4]; 68 | double tz = tr[5]; 69 | 70 | // precompute sine/cosine 71 | double sx = sin(rx); 72 | double cx = cos(rx); 73 | double sy = sin(ry); 74 | double cy = cos(ry); 75 | double sz = sin(rz); 76 | double cz = cos(rz); 77 | 78 | // compute transformation 79 | Matrix Tr(4, 4); 80 | Tr.val[0][0] = +cy * cz; 81 | Tr.val[0][1] = -cy * sz; 82 | Tr.val[0][2] = +sy; 83 | Tr.val[0][3] = tx; 84 | Tr.val[1][0] = +sx * sy * cz + cx * sz; 85 | Tr.val[1][1] = -sx * sy * sz + cx * cz; 86 | Tr.val[1][2] = -sx * cy; 87 | Tr.val[1][3] = ty; 88 | Tr.val[2][0] = -cx * sy * cz + sx * sz; 89 | Tr.val[2][1] = +cx * sy * sz + sx * cz; 90 | Tr.val[2][2] = +cx * cy; 91 | Tr.val[2][3] = tz; 92 | Tr.val[3][0] = 0; 93 | Tr.val[3][1] = 0; 94 | Tr.val[3][2] = 0; 95 | Tr.val[3][3] = 1; 96 | return Tr; 97 | } 98 | 99 | vector VisualOdometry::getRandomSample(int32_t N, int32_t num) { 100 | 101 | // init sample and totalset 102 | vector sample; 103 | vector totalset; 104 | 105 | // create vector containing all indices 106 | for (int32_t i = 0; i < N; i++) 107 | totalset.push_back(i); 108 | 109 | // add num indices to current sample 110 | sample.clear(); 111 | for (int32_t i = 0; i < num; i++) { 112 | int32_t j = rand() % totalset.size(); 113 | sample.push_back(totalset[j]); 114 | totalset.erase(totalset.begin() + j); 115 | } 116 | 117 | // return sample 118 | return sample; 119 | } 120 | } -------------------------------------------------------------------------------- /src/libviso2/viso_mono.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #include 23 | 24 | using namespace std; 25 | 26 | namespace libviso2 { 27 | VisualOdometryMono::VisualOdometryMono(parameters param) : param(param), VisualOdometry((VisualOdometry::parameters) param) { 28 | } 29 | 30 | VisualOdometryMono::~VisualOdometryMono() { 31 | } 32 | 33 | bool VisualOdometryMono::process(uint8_t *I, int32_t *dims, bool replace) { 34 | matcher->pushBack(I, dims, replace); 35 | matcher->matchFeatures(0); 36 | matcher->bucketFeatures(param.bucket.max_features, param.bucket.bucket_width, param.bucket.bucket_height); 37 | p_matched = matcher->getMatches(); 38 | return updateMotion(); 39 | } 40 | 41 | vector VisualOdometryMono::estimateMotion(vector p_matched) { 42 | 43 | // get number of matches 44 | int32_t N = p_matched.size(); 45 | if (N < 10) 46 | return vector(); 47 | 48 | // create calibration matrix 49 | double K_data[9] = {param.calib.f, 0, param.calib.cu, 0, param.calib.f, param.calib.cv, 0, 0, 1}; 50 | Matrix K(3, 3, K_data); 51 | 52 | // normalize feature points and return on errors 53 | Matrix Tp, Tc; 54 | vector p_matched_normalized = p_matched; 55 | if (!normalizeFeaturePoints(p_matched_normalized, Tp, Tc)) 56 | return vector(); 57 | 58 | // initial RANSAC estimate of F 59 | Matrix E, F; 60 | inliers.clear(); 61 | for (int32_t k = 0; k < param.ransac_iters; k++) { 62 | 63 | // draw random sample set 64 | vector active = getRandomSample(N, 8); 65 | 66 | // estimate fundamental matrix and get inliers 67 | fundamentalMatrix(p_matched_normalized, active, F); 68 | vector inliers_curr = getInlier(p_matched_normalized, F); 69 | 70 | // update model if we are better 71 | if (inliers_curr.size() > inliers.size()) 72 | inliers = inliers_curr; 73 | } 74 | 75 | // are there enough inliers? 76 | if (inliers.size() < 10) 77 | return vector(); 78 | 79 | // refine F using all inliers 80 | fundamentalMatrix(p_matched_normalized, inliers, F); 81 | 82 | // denormalise and extract essential matrix 83 | F = ~Tc * F * Tp; 84 | E = ~K * F * K; 85 | 86 | // re-enforce rank 2 constraint on essential matrix 87 | Matrix U, W, V; 88 | E.svd(U, W, V); 89 | W.val[2][0] = 0; 90 | E = U * Matrix::diag(W) * ~V; 91 | 92 | // compute 3d points X and R|t up to scale 93 | Matrix X, R, t; 94 | EtoRt(E, K, p_matched, X, R, t); 95 | 96 | // normalize 3d points and remove points behind image plane 97 | X = X / X.getMat(3, 0, 3, -1); 98 | vector pos_idx; 99 | for (int32_t i = 0; i < X.n; i++) 100 | if (X.val[2][i] > 0) 101 | pos_idx.push_back(i); 102 | Matrix X_plane = X.extractCols(pos_idx); 103 | 104 | // we need at least 10 points to proceed 105 | if (X_plane.n < 10) 106 | return vector(); 107 | 108 | // get elements closer than median 109 | double median; 110 | smallerThanMedian(X_plane, median); 111 | 112 | // return error on large median (litte motion) 113 | if (median > param.motion_threshold) 114 | return vector(); 115 | 116 | // project features to 2d 117 | Matrix x_plane(2, X_plane.n); 118 | x_plane.setMat(X_plane.getMat(1, 0, 2, -1), 0, 0); 119 | 120 | Matrix n(2, 1); 121 | n.val[0][0] = cos(-param.pitch); 122 | n.val[1][0] = sin(-param.pitch); 123 | Matrix d = ~n * x_plane; 124 | double sigma = median / 50.0; 125 | double weight = 1.0 / (2.0 * sigma * sigma); 126 | double best_sum = 0; 127 | int32_t best_idx = 0; 128 | 129 | // find best plane 130 | for (int32_t i = 0; i < x_plane.n; i++) { 131 | if (d.val[0][i] > median / param.motion_threshold) { 132 | double sum = 0; 133 | for (int32_t j = 0; j < x_plane.n; j++) { 134 | double dist = d.val[0][j] - d.val[0][i]; 135 | sum += exp(-dist * dist * weight); 136 | } 137 | if (sum > best_sum) { 138 | best_sum = sum; 139 | best_idx = i; 140 | } 141 | } 142 | } 143 | t = t * param.height / d.val[0][best_idx]; 144 | 145 | // compute rotation angles 146 | double ry = asin(R.val[0][2]); 147 | double rx = asin(-R.val[1][2] / cos(ry)); 148 | double rz = asin(-R.val[0][1] / cos(ry)); 149 | 150 | // return parameter vector 151 | vector tr_delta; 152 | tr_delta.resize(6); 153 | tr_delta[0] = rx; 154 | tr_delta[1] = ry; 155 | tr_delta[2] = rz; 156 | tr_delta[3] = t.val[0][0]; 157 | tr_delta[4] = t.val[1][0]; 158 | tr_delta[5] = t.val[2][0]; 159 | return tr_delta; 160 | } 161 | 162 | Matrix VisualOdometryMono::smallerThanMedian(Matrix &X, double &median) { 163 | 164 | // set distance and index vector 165 | vector dist; 166 | vector idx; 167 | for (int32_t i = 0; i < X.n; i++) { 168 | dist.push_back(fabs(X.val[0][i]) + fabs(X.val[1][i]) + fabs(X.val[2][i])); 169 | idx.push_back(i); 170 | } 171 | 172 | // sort elements 173 | sort(idx.begin(), idx.end(), idx_cmp &>(dist)); 174 | 175 | // get median 176 | int32_t num_elem_half = idx.size() / 2; 177 | median = dist[idx[num_elem_half]]; 178 | 179 | // create matrix containing elements closer than median 180 | Matrix X_small(4, num_elem_half + 1); 181 | for (int32_t j = 0; j <= num_elem_half; j++) 182 | for (int32_t i = 0; i < 4; i++) 183 | X_small.val[i][j] = X.val[i][idx[j]]; 184 | return X_small; 185 | } 186 | 187 | bool VisualOdometryMono::normalizeFeaturePoints(vector &p_matched, Matrix &Tp, Matrix &Tc) { 188 | 189 | // shift origins to centroids 190 | double cpu = 0, cpv = 0, ccu = 0, ccv = 0; 191 | for (vector::iterator it = p_matched.begin(); it != p_matched.end(); it++) { 192 | cpu += it->u1p; 193 | cpv += it->v1p; 194 | ccu += it->u1c; 195 | ccv += it->v1c; 196 | } 197 | cpu /= (double) p_matched.size(); 198 | cpv /= (double) p_matched.size(); 199 | ccu /= (double) p_matched.size(); 200 | ccv /= (double) p_matched.size(); 201 | for (vector::iterator it = p_matched.begin(); it != p_matched.end(); it++) { 202 | it->u1p -= cpu; 203 | it->v1p -= cpv; 204 | it->u1c -= ccu; 205 | it->v1c -= ccv; 206 | } 207 | 208 | // scale features such that mean distance from origin is sqrt(2) 209 | double sp = 0, sc = 0; 210 | for (vector::iterator it = p_matched.begin(); it != p_matched.end(); it++) { 211 | sp += sqrt(it->u1p * it->u1p + it->v1p * it->v1p); 212 | sc += sqrt(it->u1c * it->u1c + it->v1c * it->v1c); 213 | } 214 | if (fabs(sp) < 1e-10 || fabs(sc) < 1e-10) 215 | return false; 216 | sp = sqrt(2.0) * (double) p_matched.size() / sp; 217 | sc = sqrt(2.0) * (double) p_matched.size() / sc; 218 | for (vector::iterator it = p_matched.begin(); it != p_matched.end(); it++) { 219 | it->u1p *= sp; 220 | it->v1p *= sp; 221 | it->u1c *= sc; 222 | it->v1c *= sc; 223 | } 224 | 225 | // compute corresponding transformation matrices 226 | double Tp_data[9] = {sp, 0, -sp * cpu, 0, sp, -sp * cpv, 0, 0, 1}; 227 | double Tc_data[9] = {sc, 0, -sc * ccu, 0, sc, -sc * ccv, 0, 0, 1}; 228 | Tp = Matrix(3, 3, Tp_data); 229 | Tc = Matrix(3, 3, Tc_data); 230 | 231 | // return true on success 232 | return true; 233 | } 234 | 235 | void VisualOdometryMono::fundamentalMatrix(const vector &p_matched, const vector &active, Matrix &F) { 236 | 237 | // number of active p_matched 238 | int32_t N = active.size(); 239 | 240 | // create constraint matrix A 241 | Matrix A(N, 9); 242 | for (int32_t i = 0; i < N; i++) { 243 | Matcher::p_match m = p_matched[active[i]]; 244 | A.val[i][0] = m.u1c * m.u1p; 245 | A.val[i][1] = m.u1c * m.v1p; 246 | A.val[i][2] = m.u1c; 247 | A.val[i][3] = m.v1c * m.u1p; 248 | A.val[i][4] = m.v1c * m.v1p; 249 | A.val[i][5] = m.v1c; 250 | A.val[i][6] = m.u1p; 251 | A.val[i][7] = m.v1p; 252 | A.val[i][8] = 1; 253 | } 254 | 255 | // compute singular value decomposition of A 256 | Matrix U, W, V; 257 | A.svd(U, W, V); 258 | 259 | // extract fundamental matrix from the column of V corresponding to the smallest singular value 260 | F = Matrix::reshape(V.getMat(0, 8, 8, 8), 3, 3); 261 | 262 | // enforce rank 2 263 | F.svd(U, W, V); 264 | W.val[2][0] = 0; 265 | F = U * Matrix::diag(W) * ~V; 266 | } 267 | 268 | vector VisualOdometryMono::getInlier(vector &p_matched, Matrix &F) { 269 | 270 | // extract fundamental matrix 271 | double f00 = F.val[0][0]; 272 | double f01 = F.val[0][1]; 273 | double f02 = F.val[0][2]; 274 | double f10 = F.val[1][0]; 275 | double f11 = F.val[1][1]; 276 | double f12 = F.val[1][2]; 277 | double f20 = F.val[2][0]; 278 | double f21 = F.val[2][1]; 279 | double f22 = F.val[2][2]; 280 | 281 | // loop variables 282 | double u1, v1, u2, v2; 283 | double x2tFx1; 284 | double Fx1u, Fx1v, Fx1w; 285 | double Ftx2u, Ftx2v; 286 | 287 | // vector with inliers 288 | vector inliers; 289 | 290 | // for all matches do 291 | for (int32_t i = 0; i < (int32_t) p_matched.size(); i++) { 292 | 293 | // extract matches 294 | u1 = p_matched[i].u1p; 295 | v1 = p_matched[i].v1p; 296 | u2 = p_matched[i].u1c; 297 | v2 = p_matched[i].v1c; 298 | 299 | // F*x1 300 | Fx1u = f00 * u1 + f01 * v1 + f02; 301 | Fx1v = f10 * u1 + f11 * v1 + f12; 302 | Fx1w = f20 * u1 + f21 * v1 + f22; 303 | 304 | // F'*x2 305 | Ftx2u = f00 * u2 + f10 * v2 + f20; 306 | Ftx2v = f01 * u2 + f11 * v2 + f21; 307 | 308 | // x2'*F*x1 309 | x2tFx1 = u2 * Fx1u + v2 * Fx1v + Fx1w; 310 | 311 | // sampson distance 312 | double d = x2tFx1 * x2tFx1 / (Fx1u * Fx1u + Fx1v * Fx1v + Ftx2u * Ftx2u + Ftx2v * Ftx2v); 313 | 314 | // check threshold 315 | if (fabs(d) < param.inlier_threshold) 316 | inliers.push_back(i); 317 | } 318 | 319 | // return set of all inliers 320 | return inliers; 321 | } 322 | 323 | void VisualOdometryMono::EtoRt(Matrix &E, Matrix &K, vector &p_matched, Matrix &X, Matrix &R, Matrix &t) { 324 | 325 | // hartley matrices 326 | double W_data[9] = {0, -1, 0, +1, 0, 0, 0, 0, 1}; 327 | double Z_data[9] = {0, +1, 0, -1, 0, 0, 0, 0, 0}; 328 | Matrix W(3, 3, W_data); 329 | Matrix Z(3, 3, Z_data); 330 | 331 | // extract T,R1,R2 (8 solutions) 332 | Matrix U, S, V; 333 | E.svd(U, S, V); 334 | Matrix T = U * Z * ~U; 335 | Matrix Ra = U * W * (~V); 336 | Matrix Rb = U * (~W) * (~V); 337 | 338 | // convert T to t 339 | t = Matrix(3, 1); 340 | t.val[0][0] = T.val[2][1]; 341 | t.val[1][0] = T.val[0][2]; 342 | t.val[2][0] = T.val[1][0]; 343 | 344 | // assure determinant to be positive 345 | if (Ra.det() < 0) Ra = -Ra; 346 | if (Rb.det() < 0) Rb = -Rb; 347 | 348 | // create vector containing all 4 solutions 349 | vector R_vec; 350 | vector t_vec; 351 | R_vec.push_back(Ra); 352 | t_vec.push_back(t); 353 | R_vec.push_back(Ra); 354 | t_vec.push_back(-t); 355 | R_vec.push_back(Rb); 356 | t_vec.push_back(t); 357 | R_vec.push_back(Rb); 358 | t_vec.push_back(-t); 359 | 360 | // try all 4 solutions 361 | Matrix X_curr; 362 | int32_t max_inliers = 0; 363 | for (int32_t i = 0; i < 4; i++) { 364 | int32_t num_inliers = triangulateChieral(p_matched, K, R_vec[i], t_vec[i], X_curr); 365 | if (num_inliers > max_inliers) { 366 | max_inliers = num_inliers; 367 | X = X_curr; 368 | R = R_vec[i]; 369 | t = t_vec[i]; 370 | } 371 | } 372 | } 373 | 374 | int32_t VisualOdometryMono::triangulateChieral(vector &p_matched, Matrix &K, Matrix &R, Matrix &t, Matrix &X) { 375 | 376 | // init 3d point matrix 377 | X = Matrix(4, p_matched.size()); 378 | 379 | // projection matrices 380 | Matrix P1(3, 4); 381 | Matrix P2(3, 4); 382 | P1.setMat(K, 0, 0); 383 | P2.setMat(R, 0, 0); 384 | P2.setMat(t, 0, 3); 385 | P2 = K * P2; 386 | 387 | // triangulation via orthogonal regression 388 | Matrix J(4, 4); 389 | Matrix U, S, V; 390 | for (int32_t i = 0; i < (int) p_matched.size(); i++) { 391 | for (int32_t j = 0; j < 4; j++) { 392 | J.val[0][j] = P1.val[2][j] * p_matched[i].u1p - P1.val[0][j]; 393 | J.val[1][j] = P1.val[2][j] * p_matched[i].v1p - P1.val[1][j]; 394 | J.val[2][j] = P2.val[2][j] * p_matched[i].u1c - P2.val[0][j]; 395 | J.val[3][j] = P2.val[2][j] * p_matched[i].v1c - P2.val[1][j]; 396 | } 397 | J.svd(U, S, V); 398 | X.setMat(V.getMat(0, 3, 3, 3), 0, i); 399 | } 400 | 401 | // compute inliers 402 | Matrix AX1 = P1 * X; 403 | Matrix BX1 = P2 * X; 404 | int32_t num = 0; 405 | for (int32_t i = 0; i < X.n; i++) 406 | if (AX1.val[2][i] * X.val[3][i] > 0 && BX1.val[2][i] * X.val[3][i] > 0) 407 | num++; 408 | 409 | // return number of inliers 410 | return num; 411 | } 412 | } -------------------------------------------------------------------------------- /src/libviso2/viso_stereo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #include 23 | 24 | using namespace std; 25 | 26 | namespace libviso2 { 27 | VisualOdometryStereo::VisualOdometryStereo(parameters param) : param(param), VisualOdometry(param) { 28 | matcher->setIntrinsics(param.calib.f, param.calib.cu, param.calib.cv, param.base); 29 | } 30 | 31 | VisualOdometryStereo::~VisualOdometryStereo() { 32 | } 33 | 34 | bool VisualOdometryStereo::process(uint8_t *I1, uint8_t *I2, int32_t *dims, bool replace) { 35 | 36 | // push back images 37 | matcher->pushBack(I1, I2, dims, replace); 38 | 39 | // bootstrap motion estimate if invalid 40 | if (!Tr_valid) { 41 | matcher->matchFeatures(2); 42 | matcher->bucketFeatures(param.bucket.max_features, param.bucket.bucket_width, param.bucket.bucket_height); 43 | p_matched = matcher->getMatches(); 44 | updateMotion(); 45 | } 46 | 47 | // match features and update motion 48 | if (Tr_valid) matcher->matchFeatures(2, &Tr_delta); 49 | else matcher->matchFeatures(2); 50 | matcher->bucketFeatures(param.bucket.max_features, param.bucket.bucket_width, param.bucket.bucket_height); 51 | p_matched = matcher->getMatches(); 52 | return updateMotion(); 53 | } 54 | 55 | vector VisualOdometryStereo::estimateMotion(vector p_matched) { 56 | 57 | // return value 58 | bool success = true; 59 | 60 | // compute minimum distance for RANSAC samples 61 | double width = 0, height = 0; 62 | for (vector::iterator it = p_matched.begin(); it != p_matched.end(); it++) { 63 | if (it->u1c > width) width = it->u1c; 64 | if (it->v1c > height) height = it->v1c; 65 | } 66 | double min_dist = min(width, height) / 3.0; 67 | 68 | // get number of matches 69 | int32_t N = p_matched.size(); 70 | if (N < 6) 71 | return vector(); 72 | 73 | // allocate dynamic memory 74 | X = new double[N]; 75 | Y = new double[N]; 76 | Z = new double[N]; 77 | J = new double[4 * N * 6]; 78 | p_predict = new double[4 * N]; 79 | p_observe = new double[4 * N]; 80 | p_residual = new double[4 * N]; 81 | 82 | // project matches of previous image into 3d 83 | for (int32_t i = 0; i < N; i++) { 84 | double d = max(p_matched[i].u1p - p_matched[i].u2p, 0.0001f); 85 | X[i] = (p_matched[i].u1p - param.calib.cu) * param.base / d; 86 | Y[i] = (p_matched[i].v1p - param.calib.cv) * param.base / d; 87 | Z[i] = param.calib.f * param.base / d; 88 | } 89 | 90 | // loop variables 91 | vector tr_delta; 92 | vector tr_delta_curr; 93 | tr_delta_curr.resize(6); 94 | 95 | // clear parameter vector 96 | inliers.clear(); 97 | 98 | // initial RANSAC estimate 99 | for (int32_t k = 0; k < param.ransac_iters; k++) { 100 | 101 | // draw random sample set 102 | vector active = getRandomSample(N, 3); 103 | 104 | // clear parameter vector 105 | for (int32_t i = 0; i < 6; i++) 106 | tr_delta_curr[i] = 0; 107 | 108 | // minimize reprojection errors 109 | VisualOdometryStereo::result result = UPDATED; 110 | int32_t iter = 0; 111 | while (result == UPDATED) { 112 | result = updateParameters(p_matched, active, tr_delta_curr, 1, 1e-6); 113 | if (iter++ > 20 || result == CONVERGED) 114 | break; 115 | } 116 | 117 | // overwrite best parameters if we have more inliers 118 | if (result != FAILED) { 119 | vector inliers_curr = getInlier(p_matched, tr_delta_curr); 120 | if (inliers_curr.size() > inliers.size()) { 121 | inliers = inliers_curr; 122 | tr_delta = tr_delta_curr; 123 | } 124 | } 125 | } 126 | 127 | // final optimization (refinement) 128 | if (inliers.size() >= 6) { 129 | int32_t iter = 0; 130 | VisualOdometryStereo::result result = UPDATED; 131 | while (result == UPDATED) { 132 | result = updateParameters(p_matched, inliers, tr_delta, 1, 1e-8); 133 | if (iter++ > 100 || result == CONVERGED) 134 | break; 135 | } 136 | 137 | // not converged 138 | if (result != CONVERGED) 139 | success = false; 140 | 141 | // not enough inliers 142 | } else { 143 | success = false; 144 | } 145 | 146 | // release dynamic memory 147 | delete[] X; 148 | delete[] Y; 149 | delete[] Z; 150 | delete[] J; 151 | delete[] p_predict; 152 | delete[] p_observe; 153 | delete[] p_residual; 154 | 155 | // parameter estimate succeeded? 156 | if (success) return tr_delta; 157 | else return vector(); 158 | } 159 | 160 | vector VisualOdometryStereo::getInlier(vector &p_matched, vector &tr) { 161 | 162 | // mark all observations active 163 | vector active; 164 | for (int32_t i = 0; i < (int32_t) p_matched.size(); i++) 165 | active.push_back(i); 166 | 167 | // extract observations and compute predictions 168 | computeObservations(p_matched, active); 169 | computeResidualsAndJacobian(tr, active); 170 | 171 | // compute inliers 172 | vector inliers; 173 | for (int32_t i = 0; i < (int32_t) p_matched.size(); i++) 174 | if (pow(p_observe[4 * i + 0] - p_predict[4 * i + 0], 2) + pow(p_observe[4 * i + 1] - p_predict[4 * i + 1], 2) + 175 | pow(p_observe[4 * i + 2] - p_predict[4 * i + 2], 2) + pow(p_observe[4 * i + 3] - p_predict[4 * i + 3], 2) < 176 | param.inlier_threshold * param.inlier_threshold) 177 | inliers.push_back(i); 178 | return inliers; 179 | } 180 | 181 | VisualOdometryStereo::result 182 | VisualOdometryStereo::updateParameters(vector &p_matched, vector &active, vector &tr, double step_size, double eps) { 183 | 184 | // we need at least 3 observations 185 | if (active.size() < 3) 186 | return FAILED; 187 | 188 | // extract observations and compute predictions 189 | computeObservations(p_matched, active); 190 | computeResidualsAndJacobian(tr, active); 191 | 192 | // init 193 | Matrix A(6, 6); 194 | Matrix B(6, 1); 195 | 196 | // fill matrices A and B 197 | for (int32_t m = 0; m < 6; m++) { 198 | for (int32_t n = 0; n < 6; n++) { 199 | double a = 0; 200 | for (int32_t i = 0; i < 4 * (int32_t) active.size(); i++) { 201 | a += J[i * 6 + m] * J[i * 6 + n]; 202 | } 203 | A.val[m][n] = a; 204 | } 205 | double b = 0; 206 | for (int32_t i = 0; i < 4 * (int32_t) active.size(); i++) { 207 | b += J[i * 6 + m] * (p_residual[i]); 208 | } 209 | B.val[m][0] = b; 210 | } 211 | 212 | // perform elimination 213 | if (B.solve(A)) { 214 | bool converged = true; 215 | for (int32_t m = 0; m < 6; m++) { 216 | tr[m] += step_size * B.val[m][0]; 217 | if (fabs(B.val[m][0]) > eps) 218 | converged = false; 219 | } 220 | if (converged) 221 | return CONVERGED; 222 | else 223 | return UPDATED; 224 | } else { 225 | return FAILED; 226 | } 227 | } 228 | 229 | void VisualOdometryStereo::computeObservations(vector &p_matched, vector &active) { 230 | 231 | // set all observations 232 | for (int32_t i = 0; i < (int32_t) active.size(); i++) { 233 | p_observe[4 * i + 0] = p_matched[active[i]].u1c; // u1 234 | p_observe[4 * i + 1] = p_matched[active[i]].v1c; // v1 235 | p_observe[4 * i + 2] = p_matched[active[i]].u2c; // u2 236 | p_observe[4 * i + 3] = p_matched[active[i]].v2c; // v2 237 | } 238 | } 239 | 240 | void VisualOdometryStereo::computeResidualsAndJacobian(vector &tr, vector &active) { 241 | 242 | // extract motion parameters 243 | double rx = tr[0]; 244 | double ry = tr[1]; 245 | double rz = tr[2]; 246 | double tx = tr[3]; 247 | double ty = tr[4]; 248 | double tz = tr[5]; 249 | 250 | // precompute sine/cosine 251 | double sx = sin(rx); 252 | double cx = cos(rx); 253 | double sy = sin(ry); 254 | double cy = cos(ry); 255 | double sz = sin(rz); 256 | double cz = cos(rz); 257 | 258 | // compute rotation matrix and derivatives 259 | double r00 = +cy * cz; 260 | double r01 = -cy * sz; 261 | double r02 = +sy; 262 | double r10 = +sx * sy * cz + cx * sz; 263 | double r11 = -sx * sy * sz + cx * cz; 264 | double r12 = -sx * cy; 265 | double r20 = -cx * sy * cz + sx * sz; 266 | double r21 = +cx * sy * sz + sx * cz; 267 | double r22 = +cx * cy; 268 | double rdrx10 = +cx * sy * cz - sx * sz; 269 | double rdrx11 = -cx * sy * sz - sx * cz; 270 | double rdrx12 = -cx * cy; 271 | double rdrx20 = +sx * sy * cz + cx * sz; 272 | double rdrx21 = -sx * sy * sz + cx * cz; 273 | double rdrx22 = -sx * cy; 274 | double rdry00 = -sy * cz; 275 | double rdry01 = +sy * sz; 276 | double rdry02 = +cy; 277 | double rdry10 = +sx * cy * cz; 278 | double rdry11 = -sx * cy * sz; 279 | double rdry12 = +sx * sy; 280 | double rdry20 = -cx * cy * cz; 281 | double rdry21 = +cx * cy * sz; 282 | double rdry22 = -cx * sy; 283 | double rdrz00 = -cy * sz; 284 | double rdrz01 = -cy * cz; 285 | double rdrz10 = -sx * sy * sz + cx * cz; 286 | double rdrz11 = -sx * sy * cz - cx * sz; 287 | double rdrz20 = +cx * sy * sz + sx * cz; 288 | double rdrz21 = +cx * sy * cz - sx * sz; 289 | 290 | // loop variables 291 | double X1p, Y1p, Z1p; 292 | double X1c, Y1c, Z1c, X2c; 293 | double X1cd, Y1cd, Z1cd; 294 | 295 | // for all observations do 296 | for (int32_t i = 0; i < (int32_t) active.size(); i++) { 297 | 298 | // get 3d point in previous coordinate system 299 | X1p = X[active[i]]; 300 | Y1p = Y[active[i]]; 301 | Z1p = Z[active[i]]; 302 | 303 | // compute 3d point in current left coordinate system 304 | X1c = r00 * X1p + r01 * Y1p + r02 * Z1p + tx; 305 | Y1c = r10 * X1p + r11 * Y1p + r12 * Z1p + ty; 306 | Z1c = r20 * X1p + r21 * Y1p + r22 * Z1p + tz; 307 | 308 | // weighting 309 | double weight = 1.0; 310 | if (param.reweighting) 311 | weight = 1.0 / (fabs(p_observe[4 * i + 0] - param.calib.cu) / fabs(param.calib.cu) + 0.05); 312 | 313 | // compute 3d point in current right coordinate system 314 | X2c = X1c - param.base; 315 | 316 | // for all paramters do 317 | for (int32_t j = 0; j < 6; j++) { 318 | 319 | // derivatives of 3d pt. in curr. left coordinates wrt. param j 320 | switch (j) { 321 | case 0: 322 | X1cd = 0; 323 | Y1cd = rdrx10 * X1p + rdrx11 * Y1p + rdrx12 * Z1p; 324 | Z1cd = rdrx20 * X1p + rdrx21 * Y1p + rdrx22 * Z1p; 325 | break; 326 | case 1: 327 | X1cd = rdry00 * X1p + rdry01 * Y1p + rdry02 * Z1p; 328 | Y1cd = rdry10 * X1p + rdry11 * Y1p + rdry12 * Z1p; 329 | Z1cd = rdry20 * X1p + rdry21 * Y1p + rdry22 * Z1p; 330 | break; 331 | case 2: 332 | X1cd = rdrz00 * X1p + rdrz01 * Y1p; 333 | Y1cd = rdrz10 * X1p + rdrz11 * Y1p; 334 | Z1cd = rdrz20 * X1p + rdrz21 * Y1p; 335 | break; 336 | case 3: 337 | X1cd = 1; 338 | Y1cd = 0; 339 | Z1cd = 0; 340 | break; 341 | case 4: 342 | X1cd = 0; 343 | Y1cd = 1; 344 | Z1cd = 0; 345 | break; 346 | case 5: 347 | X1cd = 0; 348 | Y1cd = 0; 349 | Z1cd = 1; 350 | break; 351 | } 352 | 353 | // set jacobian entries (project via K) 354 | J[(4 * i + 0) * 6 + j] = weight * param.calib.f * (X1cd * Z1c - X1c * Z1cd) / (Z1c * Z1c); // left u' 355 | J[(4 * i + 1) * 6 + j] = weight * param.calib.f * (Y1cd * Z1c - Y1c * Z1cd) / (Z1c * Z1c); // left v' 356 | J[(4 * i + 2) * 6 + j] = weight * param.calib.f * (X1cd * Z1c - X2c * Z1cd) / (Z1c * Z1c); // right u' 357 | J[(4 * i + 3) * 6 + j] = weight * param.calib.f * (Y1cd * Z1c - Y1c * Z1cd) / (Z1c * Z1c); // right v' 358 | } 359 | 360 | // set prediction (project via K) 361 | p_predict[4 * i + 0] = param.calib.f * X1c / Z1c + param.calib.cu; // left u 362 | p_predict[4 * i + 1] = param.calib.f * Y1c / Z1c + param.calib.cv; // left v 363 | p_predict[4 * i + 2] = param.calib.f * X2c / Z1c + param.calib.cu; // right u 364 | p_predict[4 * i + 3] = param.calib.f * Y1c / Z1c + param.calib.cv; // right v 365 | 366 | // set residuals 367 | p_residual[4 * i + 0] = weight * (p_observe[4 * i + 0] - p_predict[4 * i + 0]); 368 | p_residual[4 * i + 1] = weight * (p_observe[4 * i + 1] - p_predict[4 * i + 1]); 369 | p_residual[4 * i + 2] = weight * (p_observe[4 * i + 2] - p_predict[4 * i + 2]); 370 | p_residual[4 * i + 3] = weight * (p_observe[4 * i + 3] - p_predict[4 * i + 3]); 371 | } 372 | } 373 | } 374 | -------------------------------------------------------------------------------- /src/sparseflow/camera.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2017. All rights reserved. 3 | Computer Vision Group, Visual Computing Institute 4 | RWTH Aachen University, Germany 5 | 6 | This file is part of the rwth_mot framework. 7 | Authors: Aljosa Osep (osep -at- vision.rwth-aachen.de) 8 | 9 | rwth_mot framework is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 3 of the License, or any later version. 12 | 13 | rwth_mot framework is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | rwth_mot framework; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #include "camera.h" 23 | 24 | // eigen 25 | #include 26 | 27 | namespace SUN { 28 | namespace utils { 29 | 30 | Camera::Camera() { 31 | this->P_.setIdentity(); 32 | this->Rt_.setIdentity(); 33 | this->Rt_inv_.setIdentity(); 34 | this->K_.setIdentity(); 35 | this->K_inv_.setIdentity(); 36 | reference_pose_.setZero(); 37 | } 38 | 39 | bool Camera::ComputeMeasurementCovariance3d(const Eigen::Vector3d &pos_3d, Eigen::Matrix2d c2d_mat, const Eigen::Matrix& P_left_cam, const Eigen::Matrix& P_right_cam, Eigen::Matrix3d &covariance_out) { 40 | //! Compute inverse of pixel-measurement covariance matrix. 41 | auto inv_tmp = c2d_mat.inverse().eval(); 42 | c2d_mat = inv_tmp; 43 | 44 | //! Compute Jacobians of projection matrices of both cameras. 45 | double wl = P_left_cam(2,0)*pos_3d[0] + P_left_cam(2,1)*pos_3d[1] + P_left_cam(2,2)*pos_3d[2] + P_left_cam(2,3); 46 | double wl2 = wl * wl; 47 | 48 | double wr = P_right_cam(2,0)*pos_3d[0] + P_right_cam(2,1)*pos_3d[1] + P_right_cam(2,2)*pos_3d[2] + P_right_cam(2,3); 49 | double wr2 = wr * wr; 50 | 51 | Eigen::Matrix FL; 52 | FL.setZero(); 53 | 54 | Eigen::Matrix FR; 55 | FR.setZero(); 56 | 57 | for (int i = 0; i < 2; i++) { 58 | FL(i,0) = P_left_cam(i,0) / wl - (P_left_cam(i,0) * pos_3d[0] + P_left_cam(i,1) * pos_3d[1] + P_left_cam(i,2) * pos_3d[2] + P_left_cam(i,3)) * P_left_cam(2,0) / wl2; 59 | FL(i,1) = P_left_cam(i,1) / wl - (P_left_cam(i,0) * pos_3d[0] + P_left_cam(i,1) * pos_3d[1] + P_left_cam(i,2) * pos_3d[2] + P_left_cam(i,3)) * P_left_cam(2,1) / wl2; 60 | FL(i,2) = P_left_cam(i,2) / wl - (P_left_cam(i,0) * pos_3d[0] + P_left_cam(i,1) * pos_3d[1] + P_left_cam(i,2) * pos_3d[2] + P_left_cam(i,3)) * P_left_cam(2,2) / wl2; 61 | 62 | FR(i,0) = P_right_cam(i,0) / wr - (P_right_cam(i,0) * pos_3d[0] + P_right_cam(i,1) * pos_3d[1] + P_right_cam(i,2) * pos_3d[2] + P_right_cam(i,3)) * P_right_cam(2,0) / wr2; 63 | FR(i,1) = P_right_cam(i,1) / wr - (P_right_cam(i,0) * pos_3d[0] + P_right_cam(i,1) * pos_3d[1] + P_right_cam(i,2) * pos_3d[2] + P_right_cam(i,3)) * P_right_cam(2,1) / wr2; 64 | FR(i,2) = P_right_cam(i,2) / wr - (P_right_cam(i,0) * pos_3d[0] + P_right_cam(i,1) * pos_3d[1] + P_right_cam(i,2) * pos_3d[2] + P_right_cam(i,3)) * P_right_cam(2,2) / wr2; 65 | } 66 | 67 | //! Sum left and right part and compute the inverse. 68 | Eigen::Matrix3d FLTFL = (FL.transpose()*c2d_mat*FL); 69 | Eigen::Matrix3d FRTFR = (FR.transpose()*c2d_mat*FR); 70 | Eigen::Matrix3d sum = FLTFL+FRTFR; 71 | 72 | bool invertible; 73 | double determinant; 74 | sum.computeInverseAndDetWithCheck(covariance_out, determinant, invertible); 75 | return invertible; 76 | } 77 | 78 | bool Camera::ComputeMeasurementCovariance3d(const Eigen::Vector3d &pos_3d, double c_2d, const Eigen::Matrix& P_left_cam, const Eigen::Matrix& P_right_cam, Eigen::Matrix3d &covariance_out) { 79 | // Compute pixel cov. mat. 80 | Eigen::Matrix2d c2d_mat; 81 | c2d_mat.setIdentity(); 82 | c2d_mat *= c_2d; 83 | 84 | bool invertible = ComputeMeasurementCovariance3d(pos_3d, c2d_mat, P_left_cam, P_right_cam, covariance_out); 85 | assert(invertible); 86 | return invertible; 87 | } 88 | 89 | void Camera::init(const Eigen::Matrix &reference_projection_matrix, const Eigen::Matrix4d &odometry_matrix, int width, int height) { 90 | /// Set up K, Rt matrices and their inverses 91 | this->K_ = reference_projection_matrix.block<3,3>(0,0); 92 | this->K_inv_ = K_.inverse(); 93 | 94 | /// Little kitti hack, set t part to 0 95 | /// because t is specified relative to camera 0 96 | // TODO (Aljosa) sort this out! 97 | auto ref_proj_mat_copy = reference_projection_matrix; 98 | ref_proj_mat_copy.col(3).head<3>() = Eigen::Vector3d(0.0,0.0,0.0); 99 | 100 | /// Compute reference pose (pose in the first frame) 101 | reference_pose_(2,0) = ref_proj_mat_copy(2,3); 102 | reference_pose_(0,0) = (ref_proj_mat_copy(0,3) - ref_proj_mat_copy(0,2)*reference_pose_(2,0)) / ref_proj_mat_copy(0,0); 103 | reference_pose_(1,0) = (ref_proj_mat_copy(1,3) - ref_proj_mat_copy(1,2)*reference_pose_(2,0)) / ref_proj_mat_copy(1,1); 104 | reference_pose_(3,0) = 1.0; 105 | 106 | this->ApplyPoseTransform(odometry_matrix); 107 | 108 | this->P_.setIdentity(); 109 | this->P_.block(0,0,3,4) = reference_projection_matrix; 110 | 111 | /// width, height 112 | width_ = width; 113 | height_ = height; 114 | } 115 | 116 | void Camera::init(const Eigen::Matrix3d &K, const Eigen::Matrix4d &Rt, int width, int height) { 117 | /// Set up K, Rt matrices and their inverses 118 | this->Rt_ = Rt; 119 | this->Rt_inv_ = Rt.inverse(); 120 | this->K_ = K; 121 | this->K_inv_ = K_.inverse(); 122 | 123 | /// Compute projection matrix 124 | this->ApplyPoseTransform(Rt); 125 | 126 | /// width, height 127 | width_ = width; 128 | height_ = height; 129 | } 130 | 131 | Eigen::Vector4d Camera::CameraToWorld(const Eigen::Vector4d &p_camera) const { 132 | return Rt_*p_camera; 133 | } 134 | 135 | Eigen::Vector4d Camera::WorldToCamera(const Eigen::Vector4d &p_world) const { 136 | return Rt_inv_*p_world; 137 | } 138 | 139 | Eigen::Vector3i Camera::CameraToImage(const Eigen::Vector4d &p_camera) const { 140 | Eigen::Vector3d proj_point = K_*p_camera.head<3>(); 141 | proj_point /= proj_point[2]; // Normalize - last value must be 1. 142 | return proj_point.cast(); 143 | } 144 | 145 | Eigen::Vector3i Camera::WorldToImage(const Eigen::Vector4d &p_world) const { 146 | Eigen::Vector4d p_camera = this->WorldToCamera(p_world); 147 | Eigen::Vector3d p_camera_3 = p_camera.head(3); 148 | Eigen::Vector3d proj_point = K_*p_camera_3; 149 | 150 | proj_point /= proj_point[2]; // Normalize - last value must be 1. 151 | return proj_point.cast(); 152 | } 153 | 154 | Eigen::Vector4d Camera::GetCameraPose() const { 155 | return Rt_*reference_pose_; 156 | } 157 | 158 | Ray Camera::GetRayCameraSpace(int u, int v) const { 159 | Ray ray; 160 | ray.origin = Eigen::Vector3d(0.0,0.0,0.0); 161 | ray.direction = (K_inv_*Eigen::Vector3d(u,v,1.0)).normalized(); 162 | return ray; 163 | } 164 | 165 | bool Camera::IsPointInFrontOfCamera(const Eigen::Vector4d& point) const { 166 | Eigen::Vector4d camera_pose = GetCameraPose(); 167 | Eigen::Vector3d camera_plane_normal = R().col(2); //row(2); 168 | double camera_plane_d = 0; 169 | camera_plane_d -= camera_pose[0]*camera_plane_normal[0]; 170 | camera_plane_d -= camera_pose[1]*camera_plane_normal[1]; 171 | camera_plane_d -= camera_pose[2]*camera_plane_normal[2]; 172 | 173 | if((camera_plane_normal.dot(point.head<3>()) + camera_plane_d) > 0) 174 | return true; 175 | else 176 | return false; 177 | } 178 | 179 | // Setters / Getters 180 | const Eigen::Matrix3d& Camera::K() const { 181 | return K_; 182 | } 183 | 184 | const Eigen::Matrix3d& Camera::K_inv() const { 185 | return K_inv_; 186 | } 187 | 188 | Eigen::Matrix3d Camera::R() const { 189 | return Rt_.block<3,3>(0,0); 190 | } 191 | 192 | const Eigen::Matrix4d& Camera::Rt() const { 193 | return Rt_; 194 | } 195 | 196 | const Eigen::Matrix4d& Camera::Rt_inv() const { 197 | return Rt_inv_; 198 | } 199 | 200 | const Eigen::Matrix4d& Camera::P() const { 201 | return P_; 202 | } 203 | 204 | int Camera::width() const { 205 | return width_; 206 | } 207 | 208 | int Camera::height() const { 209 | return height_; 210 | } 211 | 212 | void Camera::set_ground_model(const std::shared_ptr ground_model) { 213 | ground_model_ = ground_model; 214 | } 215 | 216 | const std::shared_ptr Camera::ground_model() const { 217 | return ground_model_; 218 | } 219 | 220 | Eigen::Vector4d Camera::PlaneCameraToWorld(const Eigen::Vector4d &plane_in_camera_space) const { 221 | Eigen::Vector3d t_cam = this->Rt().col(3).head<3>(); 222 | Eigen::Vector3d plane_n_world = this->R()*plane_in_camera_space.head<3>(); 223 | double plane_d_world = plane_in_camera_space[3] - plane_n_world.dot(t_cam); 224 | plane_n_world.normalize(); 225 | 226 | Eigen::Vector4d plane_in_world_space; 227 | plane_in_world_space.head<3>() = plane_n_world; 228 | plane_in_world_space[3] = plane_d_world; 229 | return plane_in_world_space; 230 | } 231 | 232 | double Camera::ComputeGroundPlaneHeightInWorldSpace(double pose_plane_x, double pose_plane_z, const Eigen::Vector4d &plane_camera_space) const { 233 | Eigen::Vector4d plane_world = this->PlaneCameraToWorld(plane_camera_space); 234 | const double plane_d_world = plane_world[3]; 235 | double a = plane_world[0]; 236 | double b = plane_world[1]; 237 | double c = plane_world[2]; 238 | 239 | // Simply solve for height. 240 | double height_world = (-plane_d_world - a * pose_plane_x - c * pose_plane_z) / b; 241 | return height_world; 242 | } 243 | 244 | Eigen::Vector3d Camera::ProjectPointToGroundInWorldSpace(const Eigen::Vector3d &p_world, const Eigen::Vector4d &plane_camera_space) const { 245 | Eigen::Vector4d plane_world = this->PlaneCameraToWorld(plane_camera_space); 246 | const double plane_d_world = plane_world[3]; 247 | double a = plane_world[0]; 248 | double b = plane_world[1]; 249 | double c = plane_world[2]; 250 | double d = plane_world[3]; 251 | 252 | Eigen::Vector3d p_proj = p_world; 253 | double dist = DistancePointToGroundInWorldSpace(p_world, plane_camera_space); // This is correct (bud bad design) 254 | p_proj -= dist*Eigen::Vector3d(a,b,c); // Substract plane_normal*dist_to_plane, and we have a projected point! 255 | 256 | return p_proj; 257 | } 258 | 259 | double Camera::DistancePointToGroundInWorldSpace(const Eigen::Vector3d &p_world, const Eigen::Vector4d &plane_camera_space) const { 260 | Eigen::Vector4d plane_world = this->PlaneCameraToWorld(plane_camera_space); 261 | const double plane_d_world = plane_world[3]; 262 | double a = plane_world[0]; 263 | double b = plane_world[1]; 264 | double c = plane_world[2]; 265 | double d = plane_world[3]; 266 | Eigen::Vector3d p_proj = p_world; 267 | 268 | // -------------- 269 | double x = p_world[0]; 270 | double y = p_world[1]; 271 | double z = p_world[2]; 272 | double dist = a * x + b * y + c * z + d; //(std::abs(a * x + b * y + c * z + d)) / 273 | // -------------- 274 | 275 | return dist; 276 | } 277 | 278 | Eigen::Vector3d Camera::BackprojectTo3D(const double u, const double v, const double depth) const { 279 | double u0 = this->K_(0,2); 280 | double v0 = this->K_(1,2); 281 | double f_u = this->K_(0,0); 282 | double f_v = this->K_(1,1); 283 | 284 | return Eigen::Vector3d((u - u0) / f_u * depth, (v - v0) / f_v * depth, depth); 285 | } 286 | 287 | const double Camera::f_u() const { 288 | return K_(0,0); 289 | } 290 | 291 | const double Camera::f_v() const { 292 | return K_(1,1); 293 | } 294 | 295 | const double Camera::c_u() const { 296 | return K_(0,2); 297 | } 298 | 299 | const double Camera::c_v() const { 300 | return K_(1,2); 301 | } 302 | 303 | void Camera::ApplyPoseTransform(const Eigen::Matrix4d &Rt) { 304 | this->Rt_ = Rt; 305 | this->Rt_inv_ = Rt.inverse(); 306 | } 307 | } 308 | } 309 | -------------------------------------------------------------------------------- /src/sparseflow/camera.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2017. All rights reserved. 3 | Computer Vision Group, Visual Computing Institute 4 | RWTH Aachen University, Germany 5 | 6 | This file is part of the rwth_mot framework. 7 | Authors: Aljosa Osep (osep -at- vision.rwth-aachen.de) 8 | 9 | rwth_mot framework is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 3 of the License, or any later version. 12 | 13 | rwth_mot framework is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | rwth_mot framework; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef GOT_CAMERA_H 23 | #define GOT_CAMERA_H 24 | 25 | // std 26 | #include 27 | 28 | // eigen 29 | #include 30 | #include 31 | 32 | // Forward declarations. 33 | namespace SUN { namespace utils { class GroundModel; } } 34 | 35 | namespace SUN { 36 | namespace utils { 37 | 38 | struct Ray { 39 | Eigen::Vector3d origin; 40 | Eigen::Vector3d direction; 41 | }; 42 | 43 | /** 44 | * @brief Camera class. Stores camera intrinsics, extrinsics and provides some conveniance functions. 45 | * @author Aljosa (osep@vision.rwth-aachen.de) 46 | */ 47 | class Camera { 48 | public: 49 | Camera(); 50 | 51 | /** 52 | * @brief This method initializes camera: K, [R|t] and their inverse matrices, reference pose, width, height. 53 | * With this initialization method, reference_projection_matrix is assumed to be proj. matrix in the reference frame, 54 | * which is K*I 3x4 matrix. odometry_matrix represents camera-frame transformation from reference frame to current frame. 55 | * This matrix can be obtained e.g. using visual odometry method (or any other approach that provides robot's odometry). 56 | */ 57 | void init(const Eigen::Matrix &reference_projection_matrix, const Eigen::Matrix4d &odometry_matrix, int width, int height); 58 | void init(const Eigen::Matrix3d &K, const Eigen::Matrix4d &odometry_matrix, int width, int height); 59 | 60 | /** 61 | * @brief Updates camera's internal state (pose) 62 | */ 63 | void ApplyPoseTransform(const Eigen::Matrix4d &Rt); 64 | 65 | /** 66 | * @brief Maps point from camera coord. frame to global (reference) frame. 67 | * @return Transformed point. 68 | * @todo Do we need to take scene_Rectification_transform_ into account? 69 | */ 70 | Eigen::Vector4d CameraToWorld(const Eigen::Vector4d &p_camera) const; 71 | 72 | /** 73 | * @brief Projects a plane, specified in local camera coord. sys. to 'world' or reference coord. sys. 74 | * @return Plane params (A, B, C, D; normal and dist.) in world space. 75 | */ 76 | Eigen::Vector4d PlaneCameraToWorld(const Eigen::Vector4d &plane_in_camera_space) const; 77 | 78 | /** 79 | * @brief Very useful function, given 2D pose (world space) on specified ground plane (given in camera space), 80 | * it returns height of this point in world coord. frame. 81 | * Note: pose_plane_x, pose_plane_z are given in world coord. frame, not camera! 82 | * 83 | * @return Returns y-component (height) of this point in world coord. space. 84 | */ 85 | double ComputeGroundPlaneHeightInWorldSpace(double pose_plane_x, double pose_plane_z, const Eigen::Vector4d &plane_camera_space) const; 86 | 87 | Eigen::Vector3d ProjectPointToGroundInWorldSpace(const Eigen::Vector3d &p_world, const Eigen::Vector4d &plane_camera_space) const; 88 | 89 | double DistancePointToGroundInWorldSpace(const Eigen::Vector3d &p_world, const Eigen::Vector4d &plane_camera_space) const; 90 | 91 | /** 92 | * @brief Maps point from world (reference) coord. frame to this camera frame. 93 | * @return Transformed point. 94 | * @todo Do we need to take scene_Rectification_transform_ into account? 95 | */ 96 | Eigen::Vector4d WorldToCamera(const Eigen::Vector4d &p_world) const; 97 | 98 | /** 99 | * @brief Project a 3d point, given in camera space, to 2d image plane and return image coordinates. 100 | * In principle, this would we only multiplication with K matrix, however, our scene cloud might be transformed/rectified, 101 | * so we take scene_rectification_transformation into account as well. 102 | * @return Transformed point. 103 | */ 104 | Eigen::Vector3i CameraToImage(const Eigen::Vector4d &p_camera) const; 105 | 106 | Eigen::Vector3i WorldToImage(const Eigen::Vector4d &p_world) const; 107 | 108 | /** 109 | * @brief compute the 3d camera coordinates of the point in 2d image plane 110 | * @param u,v Coordinates of the pixe; in the image domain 111 | * @returns 3d point in camera coordinates 112 | * @author Alina Shigabudnova 113 | */ 114 | Eigen::Vector3d BackprojectTo3D(const double u, const double v, const double depth) const; 115 | 116 | /** 117 | * @brief Returns camera pose (center of proj.) in global (reference) frame. 118 | * @return Camera pose in world coordinates. 119 | */ 120 | Eigen::Vector4d GetCameraPose() const; 121 | 122 | /** 123 | * @brief Casts ray from camera reference pose through specified pixel. 124 | * @return Ray structure, with origin and direction vectors. 125 | */ 126 | Ray GetRayCameraSpace(int u, int v) const; 127 | 128 | /** 129 | * @brief Checks if a 3d point (given in world space) is in front of this camera plane. 130 | * @return True, if it is in front of camera, else otherwise. 131 | */ 132 | bool IsPointInFrontOfCamera(const Eigen::Vector4d& point) const; 133 | 134 | 135 | /** 136 | * @brief TODO. 137 | * @return TODO. 138 | */ 139 | static bool ComputeMeasurementCovariance3d(const Eigen::Vector3d &pos_3d, Eigen::Matrix2d c2d_mat, const Eigen::Matrix& P_left_cam, const Eigen::Matrix& P_right_cam, Eigen::Matrix3d &covariance_out); 140 | 141 | /** 142 | * @brief TODO. 143 | * @return TODO. 144 | */ 145 | static bool ComputeMeasurementCovariance3d(const Eigen::Vector3d &pos_3d, double c_2d, const Eigen::Matrix& P_left_cam, const Eigen::Matrix& P_right_cam, Eigen::Matrix3d &covariance_out); 146 | 147 | // Getters / Setters 148 | const Eigen::Matrix3d& K() const; 149 | const Eigen::Matrix3d& K_inv() const; 150 | Eigen::Matrix3d R() const; 151 | int width() const; 152 | int height() const; 153 | 154 | const Eigen::Matrix4d& Rt() const; 155 | const Eigen::Matrix4d& Rt_inv() const; 156 | const Eigen::Matrix4d& P() const; 157 | 158 | const std::shared_ptr ground_model() const; 159 | void set_ground_model(const std::shared_ptr ground_model); 160 | 161 | const double f_u() const; 162 | const double f_v() const; 163 | const double c_u() const; 164 | const double c_v() const; 165 | 166 | private: 167 | Eigen::Matrix4d Rt_; // Extrinsic [R|t] matrix 168 | Eigen::Matrix4d P_; // Projection matrix: P = K*[R|t] 169 | 170 | Eigen::Matrix4d Rt_inv_; // Inverse pf [R|t] 171 | 172 | Eigen::Matrix3d K_; // Intrinsic matrix K 173 | Eigen::Matrix3d K_inv_; // Inverse of intrinsic matrix 174 | 175 | Eigen::Vector4d reference_pose_; // Reference pose: initial pose of the camera (in the first frame) 176 | 177 | std::shared_ptr ground_model_; // Represents scene 'ground' wrt. camera. 178 | 179 | int width_; 180 | int height_; 181 | }; 182 | } 183 | } 184 | 185 | #endif 186 | -------------------------------------------------------------------------------- /src/sparseflow/utils_flow.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2017. All rights reserved. 3 | Computer Vision Group, Visual Computing Institute 4 | RWTH Aachen University, Germany 5 | 6 | This file is part of the rwth_mot framework. 7 | Authors: Aljosa Osep (osep -at- vision.rwth-aachen.de) 8 | 9 | rwth_mot framework is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 3 of the License, or any later version. 12 | 13 | rwth_mot framework is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | rwth_mot framework; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | 23 | #include 24 | 25 | #include "utils_flow.h" 26 | 27 | namespace SUN { 28 | namespace utils { 29 | namespace scene_flow { 30 | libviso2::Matcher *InitMatcher() { 31 | // parameters 32 | libviso2::Matcher::parameters matcher_params; 33 | 34 | matcher_params.nms_n = 5; // non-max-suppression: min. distance between maxima (in pixels) 35 | matcher_params.nms_tau = 50; // non-max-suppression: interest point peakiness threshold 36 | matcher_params.match_binsize = 50; // matching bin width/height (affects efficiency only) 37 | matcher_params.match_radius = 200; // matching radius (du/dv in pixels) 38 | matcher_params.match_disp_tolerance = 1; // du tolerance for stereo matches (in pixels) 39 | matcher_params.outlier_disp_tolerance = 5; // outlier removal: disparity tolerance (in pixels) 40 | matcher_params.outlier_flow_tolerance = 5; // outlier removal: flow tolerance (in pixels) 41 | matcher_params.multi_stage = 1; // 0=disabled,1=multistage matching (denser and faster) 42 | matcher_params.half_resolution = 0; // 0=disabled,1=match at half resolution, refine at full resolution 43 | matcher_params.refinement = 2; // refinement (0=none,1=pixel,2=subpixel) 44 | 45 | // bucketing parameters 46 | //matcher_params.max_features = 4; 47 | //matcher_params.bucket_width = 10; 48 | //matcher_params.bucket_height = 10; 49 | 50 | // create matcher instance 51 | auto *M = new libviso2::Matcher(matcher_params); 52 | return M; 53 | } 54 | 55 | Eigen::Vector4d ProjTo3D(float u1, float u2, float v, const SUN::utils::Camera &camera, float baseline) { 56 | const double f = camera.f_u(); // Focal 57 | const double cu = camera.c_u(); 58 | const double cv = camera.c_v(); 59 | const double d = std::fabs(u2 - u1); // Disparity 60 | 61 | Eigen::Vector4d ret(std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 62 | std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); 63 | 64 | if (d > 0.0001) { 65 | double X = ((u1 - cu) * baseline) / d; 66 | double Y = ((v - cv) * baseline) / d; 67 | double Z = f * baseline / d; 68 | ret = Eigen::Vector4d(X, Y, Z, 1.0); 69 | } 70 | 71 | return ret; 72 | } 73 | 74 | // Eigen::Vector4d ProjTo3D_(float u1, float u2, float v, float f, float cu, float cv, float baseline) { 75 | // 76 | // const double d = std::fabs(u2 - u1); // Disparity 77 | // 78 | // Eigen::Vector4d ret(std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 79 | // std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); 80 | // 81 | // if (d > 0.0001) { 82 | // double X = ((u1 - cu) * baseline) / d; 83 | // double Y = ((v - cv) * baseline) / d; 84 | // double Z = f * baseline / d; 85 | // ret = Eigen::Vector4d(X, Y, Z, 1.0); 86 | // } 87 | // 88 | // return ret; 89 | // } 90 | 91 | // std::vector 92 | // GetMatches(libviso2::Matcher *M, const cv::Mat &color_left, const cv::Mat &color_right, const SUN::utils::Camera &camera, float baseline, 93 | // bool only_push) { 94 | // 95 | // cv::Mat grayscale_left, grayscale_right; 96 | // cv::cvtColor(color_left, grayscale_left, CV_BGR2GRAY); 97 | // cv::cvtColor(color_right, grayscale_right, CV_BGR2GRAY); 98 | // const int32_t width = color_left.cols; 99 | // const int32_t height = color_left.rows; 100 | // 101 | // // Convert input images to uint8_t buffer 102 | // uint8_t *left_img_data = (uint8_t *) malloc(width * height * sizeof(uint8_t)); 103 | // uint8_t *right_img_data = (uint8_t *) malloc(width * height * sizeof(uint8_t)); 104 | // int32_t k = 0; 105 | // for (int32_t v = 0; v < height; v++) { 106 | // for (int32_t u = 0; u < width; u++) { 107 | // left_img_data[k] = (uint8_t) grayscale_left.at(v, u); 108 | // right_img_data[k] = (uint8_t) grayscale_right.at(v, u); 109 | // k++; 110 | // } 111 | // } 112 | // 113 | // int32_t dims[] = {width, height, width}; 114 | // 115 | // // Push images 116 | // M->pushBack(left_img_data, right_img_data, dims, false); 117 | // 118 | // std::vector matches; 119 | // if (!only_push) { 120 | // 121 | // // do matching 122 | // M->matchFeatures(2); // 2 ... quad matching 123 | // 124 | // // Get matches 125 | // // quad matching 126 | // matches = M->getMatches(); 127 | // } 128 | // 129 | // free(left_img_data); 130 | // free(right_img_data); 131 | // 132 | // return matches; 133 | // } 134 | // 135 | // std::tuple > 136 | // GetSceneFlow(std::vector quad_matches, const Eigen::Matrix4d Tr, const SUN::utils::Camera &camera, 137 | // float baseline, float dt, float max_velocity_ms) { 138 | // 139 | // 140 | // cv::Mat velocity_map(camera.height(), camera.width(), CV_32FC3); 141 | // velocity_map.setTo(cv::Vec3f(std::numeric_limits::quiet_NaN(), 142 | // std::numeric_limits::quiet_NaN(), 143 | // std::numeric_limits::quiet_NaN())); 144 | // 145 | // 146 | // std::vector velocity_info; 147 | // 148 | // // Project matches to 3D 149 | // for (const auto &match:quad_matches) { 150 | // 151 | // Eigen::Vector4d p3d_c = ProjTo3D(match.u1c, match.u2c, match.v1c, camera, baseline); // p3d curr frame 152 | // Eigen::Vector4d p3d_p = ProjTo3D(match.u1p, match.u2p, match.v1p, camera, baseline); // p3d prev frame 153 | // 154 | // if (std::isnan(p3d_c[0]) || std::isnan(p3d_p[0])) continue; 155 | // 156 | // const Eigen::Vector4d p3d_c_orig = p3d_c; 157 | // const Eigen::Vector4d p3d_p_orig = p3d_p; 158 | // 159 | // // Project prev to curr frame using ego estimate 160 | // p3d_p = Tr * p3d_p; 161 | // 162 | // // Project to ground 163 | // //p3d_c.head<3>() = camera.ground_model()->ProjectPointToGround(p3d_c.head<3>()); 164 | // //p3d_p.head<3>() = camera.ground_model()->ProjectPointToGround(p3d_p.head<3>()); 165 | // 166 | // int max_dist = 90; 167 | // int max_lat = 30; 168 | // if (std::fabs(p3d_c[0]) > max_lat || std::fabs(p3d_c[2]) > max_dist || std::fabs(p3d_p[0]) > max_lat || std::fabs(p3d_p[2]) > max_dist) 169 | // continue; 170 | // 171 | // Eigen::Vector3d delta = (p3d_c - p3d_p).head<3>(); 172 | // 173 | // if (delta.norm()*(1.0/dt) < max_velocity_ms) { 174 | // 175 | // velocity_map.at(match.v1c, match.u1c) = cv::Vec3f(delta[0], delta[1], delta[2]); 176 | // 177 | // VelocityInfo vi; 178 | // vi.p = Eigen::Vector2i(match.u1c, match.v1c); 179 | // vi.p_3d = p3d_c_orig.head<3>(); 180 | // vi.p_vel = delta; 181 | // vi.p_prev = p3d_p_orig.head<3>(); 182 | // vi.p_prev_to_curr = p3d_p.head<3>(); 183 | // velocity_info.push_back(vi); 184 | // } 185 | // } 186 | // 187 | // return std::make_tuple(velocity_map, velocity_info); 188 | // } 189 | // 190 | // Eigen::Matrix4d EstimateEgomotion(libviso2::VisualOdometryStereo &viso, const cv::Mat &color_left, const cv::Mat &color_right) { 191 | // cv::Mat grayscale_left, grayscale_right; 192 | // cv::cvtColor(color_left, grayscale_left, CV_BGR2GRAY); 193 | // cv::cvtColor(color_right, grayscale_right, CV_BGR2GRAY); 194 | // const int32_t width = color_left.cols; 195 | // const int32_t height = color_left.rows; 196 | // 197 | // // Convert input images to uint8_t buffer 198 | // uint8_t* left_img_data = (uint8_t*)malloc(width*height*sizeof(uint8_t)); 199 | // uint8_t* right_img_data = (uint8_t*)malloc(width*height*sizeof(uint8_t)); 200 | // int32_t k=0; 201 | // for (int32_t v=0; v(v,u); 204 | // right_img_data[k] = (uint8_t)grayscale_right.at(v,u); 205 | // k++; 206 | // } 207 | // } 208 | // 209 | // Eigen::Matrix egomotion_eigen = Eigen::MatrixXd::Identity(4,4); // Current pose 210 | // int32_t dims[] = {width,height,width}; 211 | // if (viso.process(left_img_data,right_img_data,dims)) { 212 | // libviso2::Matrix frame_to_frame_motion = viso.getMotion(); //libviso2::Matrix::inv(viso->getMotion()); 213 | // for(size_t i=0; i<4; i++){ 214 | // for(size_t j=0; j<4; j++){ 215 | // egomotion_eigen(i,j) = frame_to_frame_motion.val[i][j]; 216 | // } 217 | // } 218 | // } 219 | // 220 | // free(left_img_data); 221 | // free(right_img_data); 222 | // return egomotion_eigen; 223 | // } 224 | } 225 | } 226 | } 227 | -------------------------------------------------------------------------------- /src/sparseflow/utils_flow.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2017. All rights reserved. 3 | Computer Vision Group, Visual Computing Institute 4 | RWTH Aachen University, Germany 5 | 6 | This file is part of the rwth_mot framework. 7 | Authors: Aljosa Osep (osep -at- vision.rwth-aachen.de) 8 | 9 | rwth_mot framework is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 3 of the License, or any later version. 12 | 13 | rwth_mot framework is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | rwth_mot framework; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef GOT_SPARSE_FLOW_H 23 | #define GOT_SPARSE_FLOW_H 24 | 25 | // libviso 26 | #include 27 | 28 | // eigen 29 | #include 30 | 31 | // cv 32 | //#include 33 | //#include 34 | 35 | // utils 36 | #include "camera.h" 37 | 38 | namespace libviso2 { class VisualOdometryStereo; } 39 | 40 | namespace SUN { 41 | namespace utils { 42 | namespace scene_flow { 43 | 44 | struct VelocityInfo { 45 | Eigen::Vector2i p; 46 | Eigen::Vector3d p_3d; 47 | Eigen::Vector3d p_vel; 48 | 49 | Eigen::Vector3d p_prev; 50 | Eigen::Vector3d p_prev_to_curr; 51 | }; 52 | 53 | libviso2::Matcher *InitMatcher(); 54 | 55 | // Eigen::Vector4d ProjTo3D(float u1, float u2, float v, const SUN::utils::Camera &camera, float baseline); 56 | // // Eigen::Vector4d ProjTo3D_(float u1, float u2, float v, float f, float cu, float cv, float baseline); 57 | // 58 | // std::vector 59 | // GetMatches(libviso2::Matcher *M, const cv::Mat &color_left, const cv::Mat &color_right, 60 | // const SUN::utils::Camera &camera, 61 | // float baseline, bool only_push); 62 | // 63 | // std::tuple > 64 | // GetSceneFlow(std::vector quad_matches, const Eigen::Matrix4d Tr, const SUN::utils::Camera &camera, 65 | // float baseline, float dt, float max_velocity_ms=40.0); 66 | // 67 | // Eigen::Matrix4d EstimateEgomotion(libviso2::VisualOdometryStereo &viso, const cv::Mat &color_left, const cv::Mat &color_right); 68 | 69 | } 70 | } 71 | } 72 | 73 | 74 | #endif //GOT_SPARSE_FLOW_H 75 | --------------------------------------------------------------------------------