├── .gitignore ├── 3rdparty └── line_descriptor │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ ├── line_descriptor │ │ └── descriptor_custom.hpp │ └── line_descriptor_custom.hpp │ └── src │ ├── LSDDetector_custom.cpp │ ├── binary_descriptor_custom.cpp │ ├── binary_descriptor_matcher.cpp │ ├── bitarray_custom.hpp │ ├── bitops_custom.hpp │ ├── draw_custom.cpp │ ├── precomp_custom.hpp │ └── types_custom.hpp ├── CMakeLists.txt ├── GPL_License.txt ├── README.md ├── app └── imagesStVO.cpp ├── build.sh ├── config ├── aux │ ├── help.png │ ├── legend.png │ ├── legend_comp.png │ └── legend_full.png ├── config │ ├── config.yaml │ ├── config_euroc.yaml │ ├── config_fast.yaml │ ├── config_full.yaml │ ├── config_kitti.yaml │ └── config_reduced.yaml ├── dataset_params │ ├── asusxtion_params.yaml │ ├── dataset_params.yaml │ ├── euroc_params.yaml │ ├── kitti00-02.yaml │ ├── kitti03.yaml │ ├── kitti04-10.yaml │ └── perceptin_params.yaml ├── scene_config.ini └── scene_config_indoor.ini ├── include ├── auxiliar.h ├── config.h ├── dataset.h ├── gridStructure.h ├── lineIterator.h ├── matching.h ├── pinholeStereoCamera.h ├── sceneRepresentation.h ├── stereoFeatures.h ├── stereoFrame.h ├── stereoFrameHandler.h └── timer.h └── src ├── auxiliar.cpp ├── config.cpp ├── dataset.cpp ├── featureMatching.cpp ├── gridStructure.cpp ├── lineIterator.cpp ├── matching.cpp ├── pinholeStereoCamera.cpp ├── sceneRepresentation.cpp ├── stereoFeatures.cpp ├── stereoFrame.cpp ├── stereoFrameHandler.cpp └── timer.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | */app/other* 2 | *build* 3 | CMakeLists.txt.user 4 | /app/other/bumblebeeSVO_comp.cpp 5 | /app/other/bumblebeeSVO.cpp 6 | /app/other/bumblebeeSVO.h 7 | /app/other/kittiHistograms.cpp 8 | /app/other/kittiSVO_eval.cpp 9 | /app/other/kittiSVO.h 10 | /app/other/kittiSVO_optims.cpp 11 | /app/other/kittiSVO_paper_bak.cpp 12 | /app/other/kittiSVO_paper.cpp 13 | /app/other/kittiSVO_paper_parallel.cpp 14 | /app/other/outliersSVO.cpp 15 | /app/other/simulatedSVO.cpp 16 | /lib/libstvo.so 17 | *lib* 18 | *future-todo* 19 | -------------------------------------------------------------------------------- /3rdparty/line_descriptor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project( line-descriptor ) 2 | 3 | cmake_minimum_required(VERSION 2.7) 4 | find_package( OpenCV 3 REQUIRED) 5 | 6 | if(COMMAND cmake_policy) 7 | cmake_policy(SET CMP0003 NEW) 8 | endif(COMMAND cmake_policy) 9 | link_directories(${OpenCV_LIBS_DIR}) 10 | include_directories(${OpenCV2_INCLUDE_DIRS}) 11 | 12 | SET(BUILD_SHARED_LIBS ON) 13 | SET(CMAKE_MODULE_PATH $ENV{CMAKE_MODULE_PATH}) 14 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -O3 -mtune=native -march=native") 15 | 16 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/build) 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | include_directories( include ${OpenCV_INCLUDE_DIRS} ) 20 | list(APPEND LINK_LIBS ${OpenCV_LIBS} ) 21 | file(GLOB_RECURSE all_include_files RELATIVE "${CMAKE_SOURCE_DIR}" *.h *.hpp) 22 | 23 | link_directories(${CMAKE_SOURCE_DIR}/src/) 24 | file(GLOB_RECURSE all_source_files RELATIVE "${CMAKE_SOURCE_DIR}src/" *.cpp ) 25 | 26 | add_custom_target( linedesc_includes DEPENDS ${all_include_files} SOURCES ${all_source_files} ) 27 | 28 | add_library( linedesc ${all_source_files} ) 29 | target_link_libraries( linedesc ${LINK_LIBS} ) 30 | 31 | -------------------------------------------------------------------------------- /3rdparty/line_descriptor/README.md: -------------------------------------------------------------------------------- 1 | Binary descriptors for lines extracted from an image 2 | ==================================================== -------------------------------------------------------------------------------- /3rdparty/line_descriptor/include/line_descriptor_custom.hpp: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2013, OpenCV Foundation, all rights reserved. 14 | // Third party copyrights are property of their respective owners. 15 | // 16 | // Redistribution and use in source and binary forms, with or without modification, 17 | // are permitted provided that the following conditions are met: 18 | // 19 | // * Redistribution's of source code must retain the above copyright notice, 20 | // this list of conditions and the following disclaimer. 21 | // 22 | // * Redistribution's in binary form must reproduce the above copyright notice, 23 | // this list of conditions and the following disclaimer in the documentation 24 | // and/or other materials provided with the distribution. 25 | // 26 | // * The name of the copyright holders may not be used to endorse or promote products 27 | // derived from this software without specific prior written permission. 28 | // 29 | // This software is provided by the copyright holders and contributors "as is" and 30 | // any express or implied warranties, including, but not limited to, the implied 31 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 32 | // In no event shall the Intel Corporation or contributors be liable for any direct, 33 | // indirect, incidental, special, exemplary, or consequential damages 34 | // (including, but not limited to, procurement of substitute goods or services; 35 | // loss of use, data, or profits; or business interruption) however caused 36 | // and on any theory of liability, whether in contract, strict liability, 37 | // or tort (including negligence or otherwise) arising in any way out of 38 | // the use of this software, even if advised of the possibility of such damage. 39 | // 40 | //M*/ 41 | 42 | #ifndef __OPENCV_LINE_DESCRIPTOR_HPP__ 43 | #define __OPENCV_LINE_DESCRIPTOR_HPP__ 44 | 45 | #include "line_descriptor/descriptor_custom.hpp" 46 | 47 | /** @defgroup line_descriptor Binary descriptors for lines extracted from an image 48 | 49 | Introduction 50 | ------------ 51 | 52 | One of the most challenging activities in computer vision is the extraction of useful information 53 | from a given image. Such information, usually comes in the form of points that preserve some kind of 54 | property (for instance, they are scale-invariant) and are actually representative of input image. 55 | 56 | The goal of this module is seeking a new kind of representative information inside an image and 57 | providing the functionalities for its extraction and representation. In particular, differently from 58 | previous methods for detection of relevant elements inside an image, lines are extracted in place of 59 | points; a new class is defined ad hoc to summarize a line's properties, for reuse and plotting 60 | purposes. 61 | 62 | Computation of binary descriptors 63 | --------------------------------- 64 | 65 | To obtatin a binary descriptor representing a certain line detected from a certain octave of an 66 | image, we first compute a non-binary descriptor as described in @cite LBD . Such algorithm works on 67 | lines extracted using EDLine detector, as explained in @cite EDL . Given a line, we consider a 68 | rectangular region centered at it and called *line support region (LSR)*. Such region is divided 69 | into a set of bands \f$\{B_1, B_2, ..., B_m\}\f$, whose length equals the one of line. 70 | 71 | If we indicate with \f$\bf{d}_L\f$ the direction of line, the orthogonal and clockwise direction to line 72 | \f$\bf{d}_{\perp}\f$ can be determined; these two directions, are used to construct a reference frame 73 | centered in the middle point of line. The gradients of pixels \f$\bf{g'}\f$ inside LSR can be projected 74 | to the newly determined frame, obtaining their local equivalent 75 | \f$\bf{g'} = (\bf{g}^T \cdot \bf{d}_{\perp}, \bf{g}^T \cdot \bf{d}_L)^T \triangleq (\bf{g'}_{d_{\perp}}, \bf{g'}_{d_L})^T\f$. 76 | 77 | Later on, a Gaussian function is applied to all LSR's pixels along \f$\bf{d}_\perp\f$ direction; first, 78 | we assign a global weighting coefficient \f$f_g(i) = (1/\sqrt{2\pi}\sigma_g)e^{-d^2_i/2\sigma^2_g}\f$ to 79 | *i*-th row in LSR, where \f$d_i\f$ is the distance of *i*-th row from the center row in LSR, 80 | \f$\sigma_g = 0.5(m \cdot w - 1)\f$ and \f$w\f$ is the width of bands (the same for every band). Secondly, 81 | considering a band \f$B_j\f$ and its neighbor bands \f$B_{j-1}, B_{j+1}\f$, we assign a local weighting 82 | \f$F_l(k) = (1/\sqrt{2\pi}\sigma_l)e^{-d'^2_k/2\sigma_l^2}\f$, where \f$d'_k\f$ is the distance of *k*-th 83 | row from the center row in \f$B_j\f$ and \f$\sigma_l = w\f$. Using the global and local weights, we obtain, 84 | at the same time, the reduction of role played by gradients far from line and of boundary effect, 85 | respectively. 86 | 87 | Each band \f$B_j\f$ in LSR has an associated *band descriptor(BD)* which is computed considering 88 | previous and next band (top and bottom bands are ignored when computing descriptor for first and 89 | last band). Once each band has been assignen its BD, the LBD descriptor of line is simply given by 90 | 91 | \f[LBD = (BD_1^T, BD_2^T, ... , BD^T_m)^T.\f] 92 | 93 | To compute a band descriptor \f$B_j\f$, each *k*-th row in it is considered and the gradients in such 94 | row are accumulated: 95 | 96 | \f[\begin{matrix} \bf{V1}^k_j = \lambda \sum\limits_{\bf{g}'_{d_\perp}>0}\bf{g}'_{d_\perp}, & \bf{V2}^k_j = \lambda \sum\limits_{\bf{g}'_{d_\perp}<0} -\bf{g}'_{d_\perp}, \\ \bf{V3}^k_j = \lambda \sum\limits_{\bf{g}'_{d_L}>0}\bf{g}'_{d_L}, & \bf{V4}^k_j = \lambda \sum\limits_{\bf{g}'_{d_L}<0} -\bf{g}'_{d_L}\end{matrix}.\f] 97 | 98 | with \f$\lambda = f_g(k)f_l(k)\f$. 99 | 100 | By stacking previous results, we obtain the *band description matrix (BDM)* 101 | 102 | \f[BDM_j = \left(\begin{matrix} \bf{V1}_j^1 & \bf{V1}_j^2 & \ldots & \bf{V1}_j^n \\ \bf{V2}_j^1 & \bf{V2}_j^2 & \ldots & \bf{V2}_j^n \\ \bf{V3}_j^1 & \bf{V3}_j^2 & \ldots & \bf{V3}_j^n \\ \bf{V4}_j^1 & \bf{V4}_j^2 & \ldots & \bf{V4}_j^n \end{matrix} \right) \in \mathbb{R}^{4\times n},\f] 103 | 104 | with \f$n\f$ the number of rows in band \f$B_j\f$: 105 | 106 | \f[n = \begin{cases} 2w, & j = 1||m; \\ 3w, & \mbox{else}. \end{cases}\f] 107 | 108 | Each \f$BD_j\f$ can be obtained using the standard deviation vector \f$S_j\f$ and mean vector \f$M_j\f$ of 109 | \f$BDM_J\f$. Thus, finally: 110 | 111 | \f[LBD = (M_1^T, S_1^T, M_2^T, S_2^T, \ldots, M_m^T, S_m^T)^T \in \mathbb{R}^{8m}\f] 112 | 113 | Once the LBD has been obtained, it must be converted into a binary form. For such purpose, we 114 | consider 32 possible pairs of BD inside it; each couple of BD is compared bit by bit and comparison 115 | generates an 8 bit string. Concatenating 32 comparison strings, we get the 256-bit final binary 116 | representation of a single LBD. 117 | */ 118 | 119 | #endif 120 | -------------------------------------------------------------------------------- /3rdparty/line_descriptor/src/LSDDetector_custom.cpp: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2014, Biagio Montesano, all rights reserved. 14 | // Third party copyrights are property of their respective owners. 15 | // 16 | // Redistribution and use in source and binary forms, with or without modification, 17 | // are permitted provided that the following conditions are met: 18 | // 19 | // * Redistribution's of source code must retain the above copyright notice, 20 | // this list of conditions and the following disclaimer. 21 | // 22 | // * Redistribution's in binary form must reproduce the above copyright notice, 23 | // this list of conditions and the following disclaimer in the documentation 24 | // and/or other materials provided with the distribution. 25 | // 26 | // * The name of the copyright holders may not be used to endorse or promote products 27 | // derived from this software without specific prior written permission. 28 | // 29 | // This software is provided by the copyright holders and contributors "as is" and 30 | // any express or implied warranties, including, but not limited to, the implied 31 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 32 | // In no event shall the Intel Corporation or contributors be liable for any direct, 33 | // indirect, incidental, special, exemplary, or consequential damages 34 | // (including, but not limited to, procurement of substitute goods or services; 35 | // loss of use, data, or profits; or business interruption) however caused 36 | // and on any theory of liability, whether in contract, strict liability, 37 | // or tort (including negligence or otherwise) arising in any way out of 38 | // the use of this software, even if advised of the possibility of such damage. 39 | // 40 | //M*/ 41 | 42 | #include "precomp_custom.hpp" 43 | 44 | //using namespace cv; 45 | namespace cv 46 | { 47 | namespace line_descriptor 48 | { 49 | 50 | Ptr LSDDetectorC::createLSDDetectorC() 51 | { 52 | return Ptr( new LSDDetectorC() ); 53 | } 54 | 55 | /* compute Gaussian pyramid of input image */ 56 | void LSDDetectorC::computeGaussianPyramid( const Mat& image, int numOctaves, int scale ) 57 | { 58 | /* clear class fields */ 59 | gaussianPyrs.clear(); 60 | 61 | /* insert input image into pyramid */ 62 | cv::Mat currentMat = image.clone(); 63 | //cv::GaussianBlur( currentMat, currentMat, cv::Size( 5, 5 ), 1 ); 64 | gaussianPyrs.push_back( currentMat ); 65 | 66 | /* fill Gaussian pyramid */ 67 | for ( int pyrCounter = 1; pyrCounter < numOctaves; pyrCounter++ ) 68 | { 69 | /* compute and store next image in pyramid and its size */ 70 | pyrDown( currentMat, currentMat, Size( currentMat.cols / scale, currentMat.rows / scale ) ); 71 | gaussianPyrs.push_back( currentMat ); 72 | } 73 | } 74 | 75 | /* check lines' extremes */ 76 | inline void checkLineExtremes( cv::Vec4f& extremes, cv::Size imageSize ) 77 | { 78 | 79 | if( extremes[0] < 0 ) 80 | extremes[0] = 0; 81 | 82 | if( extremes[0] >= imageSize.width ) 83 | extremes[0] = (float)imageSize.width - 1.0f; 84 | 85 | if( extremes[2] < 0 ) 86 | extremes[2] = 0; 87 | 88 | if( extremes[2] >= imageSize.width ) 89 | extremes[2] = (float)imageSize.width - 1.0f; 90 | 91 | if( extremes[1] < 0 ) 92 | extremes[1] = 0; 93 | 94 | if( extremes[1] >= imageSize.height ) 95 | extremes[1] = (float)imageSize.height - 1.0f; 96 | 97 | if( extremes[3] < 0 ) 98 | extremes[3] = 0; 99 | 100 | if( extremes[3] >= imageSize.height ) 101 | extremes[3] = (float)imageSize.height - 1.0f; 102 | } 103 | 104 | /* requires line detection (only one image) */ 105 | void LSDDetectorC::detect( const Mat& image, CV_OUT std::vector& keylines, int scale, int numOctaves, const Mat& mask ) 106 | { 107 | if( mask.data != NULL && ( mask.size() != image.size() || mask.type() != CV_8UC1 ) ) 108 | throw std::runtime_error( "Mask error while detecting lines: please check its dimensions and that data type is CV_8UC1" ); 109 | 110 | else 111 | detectImpl( image, keylines, numOctaves, scale, mask ); 112 | } 113 | 114 | /* requires line detection (more than one image) */ 115 | void LSDDetectorC::detect( const std::vector& images, std::vector >& keylines, int scale, int numOctaves, 116 | const std::vector& masks ) const 117 | { 118 | /* detect lines from each image */ 119 | for ( size_t counter = 0; counter < images.size(); counter++ ) 120 | { 121 | if( masks[counter].data != NULL && ( masks[counter].size() != images[counter].size() || masks[counter].type() != CV_8UC1 ) ) 122 | throw std::runtime_error( "Masks error while detecting lines: please check their dimensions and that data types are CV_8UC1" ); 123 | 124 | else 125 | detectImpl( images[counter], keylines[counter], numOctaves, scale, masks[counter] ); 126 | } 127 | } 128 | 129 | /* implementation of line detection */ 130 | void LSDDetectorC::detectImpl( const Mat& imageSrc, std::vector& keylines, int numOctaves, int scale, const Mat& mask ) const 131 | { 132 | cv::Mat image; 133 | if( imageSrc.channels() != 1 ) 134 | cvtColor( imageSrc, image, COLOR_BGR2GRAY ); 135 | else 136 | image = imageSrc.clone(); 137 | 138 | /*check whether image depth is different from 0 */ 139 | if( image.depth() != 0 ) 140 | throw std::runtime_error( "Error, depth image!= 0" ); 141 | 142 | /* create a pointer to self */ 143 | LSDDetectorC *lsd = const_cast( this ); 144 | 145 | /* compute Gaussian pyramids */ 146 | lsd->computeGaussianPyramid( image, numOctaves, scale ); 147 | 148 | /* create an LSD extractor */ 149 | cv::Ptr ls = cv::createLineSegmentDetector(); 150 | 151 | /* prepare a vector to host extracted segments */ 152 | std::vector > lines_lsd; 153 | 154 | /* extract lines */ 155 | for ( int i = 0; i < numOctaves; i++ ) 156 | { 157 | std::vector octave_lines; 158 | ls->detect( gaussianPyrs[i], octave_lines ); 159 | lines_lsd.push_back( octave_lines ); 160 | } 161 | 162 | /* create keylines */ 163 | int class_counter = -1; 164 | for ( int octaveIdx = 0; octaveIdx < (int) lines_lsd.size(); octaveIdx++ ) 165 | { 166 | float octaveScale = pow( (float)scale, octaveIdx ); 167 | for ( int k = 0; k < (int) lines_lsd[octaveIdx].size(); k++ ) 168 | { 169 | KeyLine kl; 170 | cv::Vec4f extremes = lines_lsd[octaveIdx][k]; 171 | 172 | /* check data validity */ 173 | checkLineExtremes( extremes, gaussianPyrs[octaveIdx].size() ); 174 | 175 | /* fill KeyLine's fields */ 176 | kl.startPointX = extremes[0] * octaveScale; 177 | kl.startPointY = extremes[1] * octaveScale; 178 | kl.endPointX = extremes[2] * octaveScale; 179 | kl.endPointY = extremes[3] * octaveScale; 180 | kl.sPointInOctaveX = extremes[0]; 181 | kl.sPointInOctaveY = extremes[1]; 182 | kl.ePointInOctaveX = extremes[2]; 183 | kl.ePointInOctaveY = extremes[3]; 184 | kl.lineLength = (float) sqrt( pow( extremes[0] - extremes[2], 2 ) + pow( extremes[1] - extremes[3], 2 ) ); 185 | 186 | /* compute number of pixels covered by line */ 187 | LineIterator li( gaussianPyrs[octaveIdx], Point2f( extremes[0], extremes[1] ), Point2f( extremes[2], extremes[3] ) ); 188 | kl.numOfPixels = li.count; 189 | 190 | kl.angle = atan2( ( kl.endPointY - kl.startPointY ), ( kl.endPointX - kl.startPointX ) ); 191 | kl.class_id = ++class_counter; 192 | kl.octave = octaveIdx; 193 | kl.size = ( kl.endPointX - kl.startPointX ) * ( kl.endPointY - kl.startPointY ); 194 | kl.response = kl.lineLength / max( gaussianPyrs[octaveIdx].cols, gaussianPyrs[octaveIdx].rows ); 195 | kl.pt = Point2f( ( kl.endPointX + kl.startPointX ) / 2, ( kl.endPointY + kl.startPointY ) / 2 ); 196 | 197 | keylines.push_back( kl ); 198 | } 199 | } 200 | 201 | /* delete undesired KeyLines, according to input mask */ 202 | if( !mask.empty() ) 203 | { 204 | for ( size_t keyCounter = 0; keyCounter < keylines.size(); keyCounter++ ) 205 | { 206 | KeyLine kl = keylines[keyCounter]; 207 | if( mask.at( (int) kl.startPointY, (int) kl.startPointX ) == 0 && mask.at( (int) kl.endPointY, (int) kl.endPointX ) == 0 ) 208 | { 209 | keylines.erase( keylines.begin() + keyCounter ); 210 | keyCounter--; 211 | } 212 | } 213 | } 214 | 215 | } 216 | 217 | // Overload detect and detectImpl with LSDDetectorC Options 218 | void LSDDetectorC::detect( const Mat& image, CV_OUT std::vector& keylines, int scale, int numOctaves, LSDOptions opts, const Mat& mask ) 219 | { 220 | if( mask.data != NULL && ( mask.size() != image.size() || mask.type() != CV_8UC1 ) ) 221 | throw std::runtime_error( "Mask error while detecting lines: please check its dimensions and that data type is CV_8UC1" ); 222 | 223 | else 224 | detectImpl( image, keylines, numOctaves, scale, opts, mask ); 225 | } 226 | 227 | void LSDDetectorC::detectImpl( const Mat& imageSrc, std::vector& keylines, int numOctaves, int scale, LSDOptions opts, const Mat& mask ) const 228 | { 229 | cv::Mat image; 230 | if( imageSrc.channels() != 1 ) 231 | cvtColor( imageSrc, image, COLOR_BGR2GRAY ); 232 | else 233 | image = imageSrc.clone(); 234 | 235 | /*check whether image depth is different from 0 */ 236 | if( image.depth() != 0 ) 237 | throw std::runtime_error( "Error, depth image!= 0" ); 238 | 239 | /* create a pointer to self */ 240 | LSDDetectorC *lsd = const_cast( this ); 241 | 242 | /* compute Gaussian pyramids */ 243 | lsd->computeGaussianPyramid( image, numOctaves, scale ); 244 | 245 | /* create an LSD extractor */ 246 | cv::Ptr ls = cv::createLineSegmentDetector( opts.refine, 247 | opts.scale, 248 | opts.sigma_scale, 249 | opts.quant, 250 | opts.ang_th, 251 | opts.log_eps, 252 | opts.density_th, 253 | opts.n_bins); 254 | 255 | /* prepare a vector to host extracted segments */ 256 | std::vector > lines_lsd; 257 | 258 | /* extract lines */ 259 | for ( int i = 0; i < numOctaves; i++ ) 260 | { 261 | std::vector octave_lines; 262 | ls->detect( gaussianPyrs[i], octave_lines ); 263 | lines_lsd.push_back( octave_lines ); 264 | } 265 | 266 | /* create keylines */ 267 | int class_counter = -1; 268 | for ( int octaveIdx = 0; octaveIdx < (int) lines_lsd.size(); octaveIdx++ ) 269 | { 270 | float octaveScale = pow( (float)scale, octaveIdx ); 271 | for ( int k = 0; k < (int) lines_lsd[octaveIdx].size(); k++ ) 272 | { 273 | KeyLine kl; 274 | cv::Vec4f extremes = lines_lsd[octaveIdx][k]; 275 | 276 | /* check data validity */ 277 | checkLineExtremes( extremes, gaussianPyrs[octaveIdx].size() ); 278 | 279 | /* check line segment min length */ 280 | double length = (float) sqrt( pow( extremes[0] - extremes[2], 2 ) + pow( extremes[1] - extremes[3], 2 ) ); 281 | if( length > opts.min_length ) 282 | { 283 | /* fill KeyLine's fields */ 284 | kl.startPointX = extremes[0] * octaveScale; 285 | kl.startPointY = extremes[1] * octaveScale; 286 | kl.endPointX = extremes[2] * octaveScale; 287 | kl.endPointY = extremes[3] * octaveScale; 288 | kl.sPointInOctaveX = extremes[0]; 289 | kl.sPointInOctaveY = extremes[1]; 290 | kl.ePointInOctaveX = extremes[2]; 291 | kl.ePointInOctaveY = extremes[3]; 292 | kl.lineLength = length; 293 | 294 | /* compute number of pixels covered by line */ 295 | LineIterator li( gaussianPyrs[octaveIdx], Point2f( extremes[0], extremes[1] ), Point2f( extremes[2], extremes[3] ) ); 296 | kl.numOfPixels = li.count; 297 | 298 | kl.angle = atan2( ( kl.endPointY - kl.startPointY ), ( kl.endPointX - kl.startPointX ) ); 299 | kl.class_id = ++class_counter; 300 | kl.octave = octaveIdx; 301 | kl.size = ( kl.endPointX - kl.startPointX ) * ( kl.endPointY - kl.startPointY ); 302 | kl.response = kl.lineLength / max( gaussianPyrs[octaveIdx].cols, gaussianPyrs[octaveIdx].rows ); 303 | kl.pt = Point2f( ( kl.endPointX + kl.startPointX ) / 2, ( kl.endPointY + kl.startPointY ) / 2 ); 304 | 305 | keylines.push_back( kl ); 306 | } 307 | } 308 | } 309 | 310 | /* delete undesired KeyLines, according to input mask */ 311 | if( !mask.empty() ) 312 | { 313 | for ( size_t keyCounter = 0; keyCounter < keylines.size(); keyCounter++ ) 314 | { 315 | KeyLine kl = keylines[keyCounter]; 316 | if( mask.at( (int) kl.startPointY, (int) kl.startPointX ) == 0 && mask.at( (int) kl.endPointY, (int) kl.endPointX ) == 0 ) 317 | { 318 | keylines.erase( keylines.begin() + keyCounter ); 319 | keyCounter--; 320 | } 321 | } 322 | } 323 | 324 | } 325 | 326 | // Overload with fast detection 327 | void LSDDetectorC::detectFast( const Mat& image, CV_OUT std::vector& keylines, int scale, int numOctaves, LSDOptions opts, const Mat& mask ) 328 | { 329 | if( mask.data != NULL && ( mask.size() != image.size() || mask.type() != CV_8UC1 ) ) 330 | throw std::runtime_error( "Mask error while detecting lines: please check its dimensions and that data type is CV_8UC1" ); 331 | 332 | else 333 | detectImplFast( image, keylines, numOctaves, scale, opts, mask ); 334 | } 335 | 336 | void LSDDetectorC::detectImplFast( const Mat& imageSrc, std::vector& keylines, int numOctaves, int scale, LSDOptions opts, const Mat& mask ) const 337 | { 338 | cv::Mat image; 339 | if( imageSrc.channels() != 1 ) 340 | cvtColor( imageSrc, image, COLOR_BGR2GRAY ); 341 | else 342 | image = imageSrc.clone(); 343 | 344 | /*check whether image depth is different from 0 */ 345 | if( image.depth() != 0 ) 346 | throw std::runtime_error( "Error, depth image!= 0" ); 347 | 348 | /* create a pointer to self */ 349 | LSDDetectorC *lsd = const_cast( this ); 350 | 351 | /* compute Gaussian pyramids */ 352 | lsd->computeGaussianPyramid( image, numOctaves, scale ); 353 | 354 | /* create an LSD extractor */ 355 | cv::Ptr ls = cv::createLineSegmentDetector( opts.refine, 356 | opts.scale, 357 | opts.sigma_scale, 358 | opts.quant, 359 | opts.ang_th, 360 | opts.log_eps, 361 | opts.density_th, 362 | opts.n_bins); 363 | 364 | /* prepare a vector to host extracted segments */ 365 | std::vector > lines_lsd; 366 | 367 | /* extract lines */ 368 | for ( int i = 0; i < numOctaves; i++ ) 369 | { 370 | std::vector octave_lines; 371 | ls->detect( gaussianPyrs[i], octave_lines ); 372 | lines_lsd.push_back( octave_lines ); 373 | } 374 | 375 | /* create keylines */ 376 | int class_counter = -1; 377 | for ( int octaveIdx = 0; octaveIdx < (int) lines_lsd.size(); octaveIdx++ ) 378 | { 379 | float octaveScale = pow( (float)scale, octaveIdx ); 380 | for ( int k = 0; k < (int) lines_lsd[octaveIdx].size(); k++ ) 381 | { 382 | KeyLine kl; 383 | cv::Vec4f extremes = lines_lsd[octaveIdx][k]; 384 | 385 | /* check data validity */ 386 | checkLineExtremes( extremes, gaussianPyrs[octaveIdx].size() ); 387 | 388 | /* check line segment min length */ 389 | double length = (float) sqrt( pow( extremes[0] - extremes[2], 2 ) + pow( extremes[1] - extremes[3], 2 ) ); 390 | if( length > opts.min_length ) 391 | { 392 | /* fill KeyLine's fields */ 393 | kl.startPointX = extremes[0] * octaveScale; 394 | kl.startPointY = extremes[1] * octaveScale; 395 | kl.endPointX = extremes[2] * octaveScale; 396 | kl.endPointY = extremes[3] * octaveScale; 397 | kl.sPointInOctaveX = extremes[0]; 398 | kl.sPointInOctaveY = extremes[1]; 399 | kl.ePointInOctaveX = extremes[2]; 400 | kl.ePointInOctaveY = extremes[3]; 401 | kl.lineLength = length; 402 | 403 | /* compute number of pixels covered by line */ 404 | LineIterator li( gaussianPyrs[octaveIdx], Point2f( extremes[0], extremes[1] ), Point2f( extremes[2], extremes[3] ) ); 405 | kl.numOfPixels = li.count; 406 | 407 | kl.angle = atan2( ( kl.endPointY - kl.startPointY ), ( kl.endPointX - kl.startPointX ) ); 408 | kl.class_id = ++class_counter; 409 | kl.octave = octaveIdx; 410 | kl.size = ( kl.endPointX - kl.startPointX ) * ( kl.endPointY - kl.startPointY ); 411 | kl.response = kl.lineLength / max( gaussianPyrs[octaveIdx].cols, gaussianPyrs[octaveIdx].rows ); 412 | kl.pt = Point2f( ( kl.endPointX + kl.startPointX ) / 2, ( kl.endPointY + kl.startPointY ) / 2 ); 413 | 414 | keylines.push_back( kl ); 415 | } 416 | } 417 | } 418 | 419 | /* delete undesired KeyLines, according to input mask */ 420 | if( !mask.empty() ) 421 | { 422 | for ( size_t keyCounter = 0; keyCounter < keylines.size(); keyCounter++ ) 423 | { 424 | KeyLine kl = keylines[keyCounter]; 425 | if( mask.at( (int) kl.startPointY, (int) kl.startPointX ) == 0 && mask.at( (int) kl.endPointY, (int) kl.endPointX ) == 0 ) 426 | { 427 | keylines.erase( keylines.begin() + keyCounter ); 428 | keyCounter--; 429 | } 430 | } 431 | } 432 | 433 | } 434 | } 435 | 436 | 437 | } 438 | 439 | -------------------------------------------------------------------------------- /3rdparty/line_descriptor/src/bitarray_custom.hpp: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2014, Mohammad Norouzi, Ali Punjani, David J. Fleet, 14 | // all rights reserved. 15 | // Third party copyrights are property of their respective owners. 16 | // 17 | // Redistribution and use in source and binary forms, with or without modification, 18 | // are permitted provided that the following conditions are met: 19 | // 20 | // * Redistribution's of source code must retain the above copyright notice, 21 | // this list of conditions and the following disclaimer. 22 | // 23 | // * Redistribution's in binary form must reproduce the above copyright notice, 24 | // this list of conditions and the following disclaimer in the documentation 25 | // and/or other materials provided with the distribution. 26 | // 27 | // * The name of the copyright holders may not be used to endorse or promote products 28 | // derived from this software without specific prior written permission. 29 | // 30 | // This software is provided by the copyright holders and contributors "as is" and 31 | // any express or implied warranties, including, but not limited to, the implied 32 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 | // In no event shall the Intel Corporation or contributors be liable for any direct, 34 | // indirect, incidental, special, exemplary, or consequential damages 35 | // (including, but not limited to, procurement of substitute goods or services; 36 | // loss of use, data, or profits; or business interruption) however caused 37 | // and on any theory of liability, whether in contract, strict liability, 38 | // or tort (including negligence or otherwise) arising in any way out of 39 | // the use of this software, even if advised of the possibility of such damage. 40 | // 41 | //M*/ 42 | 43 | #ifndef __OPENCV_BITARRAY_HPP 44 | #define __OPENCV_BITARRAY_HPP 45 | 46 | #ifdef _MSC_VER 47 | #pragma warning( disable : 4267 ) 48 | #endif 49 | 50 | #include "types_custom.hpp" 51 | #include 52 | #include 53 | #include 54 | 55 | /* class defining a sequence of bits */ 56 | class bitarray 57 | { 58 | 59 | public: 60 | /* pointer to bits sequence and sequence's length */ 61 | UINT32 *arr; 62 | UINT32 length; 63 | 64 | /* constructor setting default values */ 65 | bitarray() 66 | { 67 | arr = NULL; 68 | length = 0; 69 | } 70 | 71 | /* constructor setting sequence's length */ 72 | bitarray( UINT64 _bits ) 73 | { 74 | init( _bits ); 75 | } 76 | 77 | /* initializer of private fields */ 78 | void init( UINT64 _bits ) 79 | { 80 | length = (UINT32) ceil( _bits / 32.00 ); 81 | arr = new UINT32[length]; 82 | erase(); 83 | } 84 | 85 | /* destructor */ 86 | ~bitarray() 87 | { 88 | if( arr ) 89 | delete[] arr; 90 | } 91 | 92 | inline void flip( UINT64 index ) 93 | { 94 | arr[index >> 5] ^= ( (UINT32) 0x01 ) << ( index % 32 ); 95 | } 96 | 97 | inline void set( UINT64 index ) 98 | { 99 | arr[index >> 5] |= ( (UINT32) 0x01 ) << ( index % 32 ); 100 | } 101 | 102 | inline UINT8 get( UINT64 index ) 103 | { 104 | return ( arr[index >> 5] & ( ( (UINT32) 0x01 ) << ( index % 32 ) ) ) != 0; 105 | } 106 | 107 | /* reserve menory for an UINT32 */ 108 | inline void erase() 109 | { 110 | memset( arr, 0, sizeof(UINT32) * length ); 111 | } 112 | 113 | }; 114 | 115 | #endif 116 | -------------------------------------------------------------------------------- /3rdparty/line_descriptor/src/bitops_custom.hpp: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2014, Mohammad Norouzi, Ali Punjani, David J. Fleet, 14 | // all rights reserved. 15 | // Third party copyrights are property of their respective owners. 16 | // 17 | // Redistribution and use in source and binary forms, with or without modification, 18 | // are permitted provided that the following conditions are met: 19 | // 20 | // * Redistribution's of source code must retain the above copyright notice, 21 | // this list of conditions and the following disclaimer. 22 | // 23 | // * Redistribution's in binary form must reproduce the above copyright notice, 24 | // this list of conditions and the following disclaimer in the documentation 25 | // and/or other materials provided with the distribution. 26 | // 27 | // * The name of the copyright holders may not be used to endorse or promote products 28 | // derived from this software without specific prior written permission. 29 | // 30 | // This software is provided by the copyright holders and contributors "as is" and 31 | // any express or implied warranties, including, but not limited to, the implied 32 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 | // In no event shall the Intel Corporation or contributors be liable for any direct, 34 | // indirect, incidental, special, exemplary, or consequential damages 35 | // (including, but not limited to, procurement of substitute goods or services; 36 | // loss of use, data, or profits; or business interruption) however caused 37 | // and on any theory of liability, whether in contract, strict liability, 38 | // or tort (including negligence or otherwise) arising in any way out of 39 | // the use of this software, even if advised of the possibility of such damage. 40 | // 41 | //M*/ 42 | 43 | #ifndef __OPENCV_BITOPTS_HPP 44 | #define __OPENCV_BITOPTS_HPP 45 | 46 | #include "precomp_custom.hpp" 47 | 48 | #ifdef _MSC_VER 49 | # include 50 | # define popcnt __popcnt 51 | # pragma warning( disable : 4267 ) 52 | #else 53 | # define popcnt __builtin_popcount 54 | 55 | #endif 56 | 57 | /* LUT */ 58 | const int lookup[] = 59 | { 60 | 0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4, 61 | 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 62 | 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 63 | 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 64 | 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 65 | 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 66 | 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 67 | 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, 68 | 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 69 | 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 70 | 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 71 | 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, 72 | 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 73 | 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, 74 | 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, 75 | 4, 5, 5, 6, 5, 6, 6, 7, 5, 6, 6, 7, 6, 7, 7, 8 76 | }; 77 | 78 | namespace cv 79 | { 80 | namespace line_descriptor 81 | { 82 | /*matching function */ 83 | inline int match( UINT8*P, UINT8*Q, int codelb ) 84 | { 85 | int i, output = 0; 86 | for( i = 0; i <= codelb - 16; i += 16 ) 87 | { 88 | output += popcnt( *(UINT32*) (P+i) ^ *(UINT32*) (Q+i) ) + 89 | popcnt( *(UINT32*) (P+i+4) ^ *(UINT32*) (Q+i+4) ) + 90 | popcnt( *(UINT32*) (P+i+8) ^ *(UINT32*) (Q+i+8) ) + 91 | popcnt( *(UINT32*) (P+i+12) ^ *(UINT32*) (Q+i+12) ); 92 | } 93 | for( ; i < codelb; i++ ) 94 | output += lookup[P[i] ^ Q[i]]; 95 | return output; 96 | } 97 | 98 | /* splitting function (b <= 64) */ 99 | inline void split( UINT64 *chunks, UINT8 *code, int m, int mplus, int b ) 100 | { 101 | UINT64 temp = 0x0; 102 | int nbits = 0; 103 | int nbyte = 0; 104 | UINT64 mask = (b == 64) ? 0xFFFFFFFFFFFFFFFFull : ( ( UINT64_1 << b ) - UINT64_1 ); 105 | 106 | for ( int i = 0; i < m; i++ ) 107 | { 108 | while ( nbits < b ) 109 | { 110 | temp |= ( (UINT64) code[nbyte++] << nbits ); 111 | nbits += 8; 112 | } 113 | 114 | chunks[i] = temp & mask; 115 | temp = b == 64 ? 0x0 : temp >> b; 116 | nbits -= b; 117 | 118 | if( i == mplus - 1 ) 119 | { 120 | b--; /* b <= 63 */ 121 | mask = ( ( UINT64_1 << b ) - UINT64_1 ); 122 | } 123 | } 124 | } 125 | 126 | /* generates the next binary code (in alphabetical order) with the 127 | same number of ones as the input x. Taken from 128 | http://www.geeksforgeeks.org/archives/10375 */ 129 | inline UINT64 next_set_of_n_elements( UINT64 x ) 130 | { 131 | UINT64 smallest, ripple, new_smallest; 132 | 133 | smallest = x & -(signed) x; 134 | ripple = x + smallest; 135 | new_smallest = x ^ ripple; 136 | new_smallest = new_smallest / smallest; 137 | new_smallest >>= 2; 138 | return ripple | new_smallest; 139 | } 140 | 141 | /* print code */ 142 | inline void print_code( UINT64 tmp, int b ) 143 | { 144 | for ( long long int j = ( b - 1 ); j >= 0; j-- ) 145 | { 146 | printf( "%llu", (long long int) tmp / (UINT64) ( 1 << j ) ); 147 | tmp = tmp - ( tmp / (UINT64) ( 1 << j ) ) * (UINT64) ( 1 << j ); 148 | } 149 | 150 | printf( "\n" ); 151 | } 152 | 153 | inline UINT64 choose( int n, int r ) 154 | { 155 | UINT64 nchooser = 1; 156 | for ( int k = 0; k < r; k++ ) 157 | { 158 | nchooser *= n - k; 159 | nchooser /= k + 1; 160 | } 161 | 162 | return nchooser; 163 | } 164 | } 165 | } 166 | 167 | #endif 168 | -------------------------------------------------------------------------------- /3rdparty/line_descriptor/src/draw_custom.cpp: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2014, Biagio Montesano, all rights reserved. 14 | // Third party copyrights are property of their respective owners. 15 | // 16 | // Redistribution and use in source and binary forms, with or without modification, 17 | // are permitted provided that the following conditions are met: 18 | // 19 | // * Redistribution's of source code must retain the above copyright notice, 20 | // this list of conditions and the following disclaimer. 21 | // 22 | // * Redistribution's in binary form must reproduce the above copyright notice, 23 | // this list of conditions and the following disclaimer in the documentation 24 | // and/or other materials provided with the distribution. 25 | // 26 | // * The name of the copyright holders may not be used to endorse or promote products 27 | // derived from this software without specific prior written permission. 28 | // 29 | // This software is provided by the copyright holders and contributors "as is" and 30 | // any express or implied warranties, including, but not limited to, the implied 31 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 32 | // In no event shall the Intel Corporation or contributors be liable for any direct, 33 | // indirect, incidental, special, exemplary, or consequential damages 34 | // (including, but not limited to, procurement of substitute goods or services; 35 | // loss of use, data, or profits; or business interruption) however caused 36 | // and on any theory of liability, whether in contract, strict liability, 37 | // or tort (including negligence or otherwise) arising in any way out of 38 | // the use of this software, even if advised of the possibility of such damage. 39 | // 40 | //M*/ 41 | 42 | #include "precomp_custom.hpp" 43 | 44 | namespace cv 45 | { 46 | namespace line_descriptor 47 | { 48 | /* draw matches between two images */ 49 | void drawLineMatches( const Mat& img1, const std::vector& keylines1, const Mat& img2, const std::vector& keylines2, 50 | const std::vector& matches1to2, Mat& outImg, const Scalar& matchColor, const Scalar& singleLineColor, 51 | const std::vector& matchesMask, int flags ) 52 | { 53 | 54 | if(img1.type() != img2.type()) 55 | { 56 | std::cout << "Input images have different types" << std::endl; 57 | CV_Assert(img1.type() == img2.type()); 58 | } 59 | 60 | /* initialize output matrix (if necessary) */ 61 | if( flags == DrawLinesMatchesFlags::DEFAULT ) 62 | { 63 | /* check how many rows are necessary for output matrix */ 64 | int totalRows = img1.rows >= img2.rows ? img1.rows : img2.rows; 65 | 66 | /* initialize output matrix */ 67 | outImg = Mat::zeros( totalRows, img1.cols + img2.cols, img1.type() ); 68 | 69 | } 70 | 71 | /* initialize random seed: */ 72 | srand( (unsigned int) time( NULL ) ); 73 | 74 | Scalar singleLineColorRGB; 75 | if( singleLineColor == Scalar::all( -1 ) ) 76 | { 77 | int R = ( rand() % (int) ( 255 + 1 ) ); 78 | int G = ( rand() % (int) ( 255 + 1 ) ); 79 | int B = ( rand() % (int) ( 255 + 1 ) ); 80 | 81 | singleLineColorRGB = Scalar( R, G, B ); 82 | } 83 | 84 | else 85 | singleLineColorRGB = singleLineColor; 86 | 87 | /* copy input images to output images */ 88 | Mat roi_left( outImg, Rect( 0, 0, img1.cols, img1.rows ) ); 89 | Mat roi_right( outImg, Rect( img1.cols, 0, img2.cols, img2.rows ) ); 90 | img1.copyTo( roi_left ); 91 | img2.copyTo( roi_right ); 92 | 93 | /* get columns offset */ 94 | int offset = img1.cols; 95 | 96 | /* if requested, draw lines from both images */ 97 | if( flags != DrawLinesMatchesFlags::NOT_DRAW_SINGLE_LINES ) 98 | { 99 | for ( size_t i = 0; i < keylines1.size(); i++ ) 100 | { 101 | KeyLine k1 = keylines1[i]; 102 | //line( outImg, Point2f( k1.startPointX, k1.startPointY ), Point2f( k1.endPointX, k1.endPointY ), singleLineColorRGB, 2 ); 103 | line( outImg, Point2f( k1.sPointInOctaveX, k1.sPointInOctaveY ), Point2f( k1.ePointInOctaveX, k1.ePointInOctaveY ), singleLineColorRGB, 2 ); 104 | 105 | } 106 | 107 | for ( size_t j = 0; j < keylines2.size(); j++ ) 108 | { 109 | KeyLine k2 = keylines2[j]; 110 | line( outImg, Point2f( k2.sPointInOctaveX + offset, k2.sPointInOctaveY ), Point2f( k2.ePointInOctaveX + offset, k2.ePointInOctaveY ), singleLineColorRGB, 2 ); 111 | } 112 | } 113 | 114 | /* draw matches */ 115 | for ( size_t counter = 0; counter < matches1to2.size(); counter++ ) 116 | { 117 | if( matchesMask[counter] != 0 ) 118 | { 119 | DMatch dm = matches1to2[counter]; 120 | KeyLine left = keylines1[dm.queryIdx]; 121 | KeyLine right = keylines2[dm.trainIdx]; 122 | 123 | Scalar matchColorRGB; 124 | if( matchColor == Scalar::all( -1 ) ) 125 | { 126 | int R = ( rand() % (int) ( 255 + 1 ) ); 127 | int G = ( rand() % (int) ( 255 + 1 ) ); 128 | int B = ( rand() % (int) ( 255 + 1 ) ); 129 | 130 | matchColorRGB = Scalar( R, G, B ); 131 | 132 | if( singleLineColor == Scalar::all( -1 ) ) 133 | singleLineColorRGB = matchColorRGB; 134 | } 135 | 136 | else 137 | matchColorRGB = matchColor; 138 | 139 | /* draw lines if necessary */ 140 | // line( outImg, Point2f( left.startPointX, left.startPointY ), Point2f( left.endPointX, left.endPointY ), singleLineColorRGB, 2 ); 141 | // 142 | // line( outImg, Point2f( right.startPointX + offset, right.startPointY ), Point2f( right.endPointX + offset, right.endPointY ), singleLineColorRGB, 143 | // 2 ); 144 | // 145 | // /* link correspondent lines */ 146 | // line( outImg, Point2f( left.startPointX, left.startPointY ), Point2f( right.startPointX + offset, right.startPointY ), matchColorRGB, 1 ); 147 | 148 | line( outImg, Point2f( left.sPointInOctaveX, left.sPointInOctaveY ), Point2f( left.ePointInOctaveX, left.ePointInOctaveY ), singleLineColorRGB, 2 ); 149 | 150 | line( outImg, Point2f( right.sPointInOctaveX + offset, right.sPointInOctaveY ), Point2f( right.ePointInOctaveX + offset, right.ePointInOctaveY ), singleLineColorRGB, 151 | 2 ); 152 | 153 | /* link correspondent lines */ 154 | line( outImg, Point2f( left.sPointInOctaveX, left.sPointInOctaveY ), Point2f( right.sPointInOctaveX + offset, right.sPointInOctaveY ), matchColorRGB, 1 ); 155 | } 156 | } 157 | } 158 | 159 | /* draw extracted lines on original image */ 160 | void drawKeylines( const Mat& image, const std::vector& keylines, Mat& outImage, const Scalar& color, int flags ) 161 | { 162 | if( flags == DrawLinesMatchesFlags::DEFAULT ) 163 | outImage = image.clone(); 164 | 165 | for ( size_t i = 0; i < keylines.size(); i++ ) 166 | { 167 | /* decide lines' color */ 168 | Scalar lineColor; 169 | if( color == Scalar::all( -1 ) ) 170 | { 171 | int R = ( rand() % (int) ( 255 + 1 ) ); 172 | int G = ( rand() % (int) ( 255 + 1 ) ); 173 | int B = ( rand() % (int) ( 255 + 1 ) ); 174 | 175 | lineColor = Scalar( R, G, B ); 176 | } 177 | 178 | else 179 | lineColor = color; 180 | 181 | /* get line */ 182 | KeyLine k = keylines[i]; 183 | 184 | /* draw line */ 185 | line( outImage, Point2f( k.startPointX, k.startPointY ), Point2f( k.endPointX, k.endPointY ), lineColor, 1 ); 186 | } 187 | } 188 | 189 | } 190 | } 191 | -------------------------------------------------------------------------------- /3rdparty/line_descriptor/src/precomp_custom.hpp: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2014, Biagio Montesano, all rights reserved. 14 | // Third party copyrights are property of their respective owners. 15 | // 16 | // Redistribution and use in source and binary forms, with or without modification, 17 | // are permitted provided that the following conditions are met: 18 | // 19 | // * Redistribution's of source code must retain the above copyright notice, 20 | // this list of conditions and the following disclaimer. 21 | // 22 | // * Redistribution's in binary form must reproduce the above copyright notice, 23 | // this list of conditions and the following disclaimer in the documentation 24 | // and/or other materials provided with the distribution. 25 | // 26 | // * The name of the copyright holders may not be used to endorse or promote products 27 | // derived from this software without specific prior written permission. 28 | // 29 | // This software is provided by the copyright holders and contributors "as is" and 30 | // any express or implied warranties, including, but not limited to, the implied 31 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 32 | // In no event shall the Intel Corporation or contributors be liable for any direct, 33 | // indirect, incidental, special, exemplary, or consequential damages 34 | // (including, but not limited to, procurement of substitute goods or services; 35 | // loss of use, data, or profits; or business interruption) however caused 36 | // and on any theory of liability, whether in contract, strict liability, 37 | // or tort (including negligence or otherwise) arising in any way out of 38 | // the use of this software, even if advised of the possibility of such damage. 39 | // 40 | //M*/ 41 | 42 | #ifndef __OPENCV_PRECOMP_H__ 43 | #define __OPENCV_PRECOMP_H__ 44 | 45 | #ifdef _MSC_VER 46 | #pragma warning( disable : 4267 ) 47 | #endif 48 | 49 | #define _USE_MATH_DEFINES 50 | 51 | #include 52 | #include "opencv2/core/utility.hpp" 53 | #include 54 | #include 55 | #include 56 | #include "opencv2/core.hpp" 57 | 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | #include 69 | 70 | #include "bitarray_custom.hpp" 71 | #include "bitops_custom.hpp" 72 | #include "types_custom.hpp" 73 | 74 | #include "line_descriptor_custom.hpp" 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /3rdparty/line_descriptor/src/types_custom.hpp: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2014, Mohammad Norouzi, Ali Punjani, David J. Fleet, 14 | // all rights reserved. 15 | // Third party copyrights are property of their respective owners. 16 | // 17 | // Redistribution and use in source and binary forms, with or without modification, 18 | // are permitted provided that the following conditions are met: 19 | // 20 | // * Redistribution's of source code must retain the above copyright notice, 21 | // this list of conditions and the following disclaimer. 22 | // 23 | // * Redistribution's in binary form must reproduce the above copyright notice, 24 | // this list of conditions and the following disclaimer in the documentation 25 | // and/or other materials provided with the distribution. 26 | // 27 | // * The name of the copyright holders may not be used to endorse or promote products 28 | // derived from this software without specific prior written permission. 29 | // 30 | // This software is provided by the copyright holders and contributors "as is" and 31 | // any express or implied warranties, including, but not limited to, the implied 32 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 | // In no event shall the Intel Corporation or contributors be liable for any direct, 34 | // indirect, incidental, special, exemplary, or consequential damages 35 | // (including, but not limited to, procurement of substitute goods or services; 36 | // loss of use, data, or profits; or business interruption) however caused 37 | // and on any theory of liability, whether in contract, strict liability, 38 | // or tort (including negligence or otherwise) arising in any way out of 39 | // the use of this software, even if advised of the possibility of such damage. 40 | // 41 | //M*/ 42 | 43 | #if defined _MSC_VER && _MSC_VER <= 1700 44 | #include 45 | #else 46 | #include 47 | #endif 48 | 49 | #ifndef __OPENCV_TYPES_HPP 50 | #define __OPENCV_TYPES_HPP 51 | 52 | #ifdef _MSC_VER 53 | #pragma warning( disable : 4267 ) 54 | #endif 55 | 56 | /* define data types */ 57 | typedef uint64_t UINT64; 58 | typedef uint32_t UINT32; 59 | typedef uint16_t UINT16; 60 | typedef uint8_t UINT8; 61 | 62 | /* define constants */ 63 | #define UINT64_1 ((UINT64)0x01) 64 | #define UINT32_1 ((UINT32)0x01) 65 | 66 | #endif 67 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project( stvo-pl ) 2 | 3 | cmake_minimum_required(VERSION 2.7) 4 | find_package(OpenCV 3 REQUIRED) 5 | find_package(Boost REQUIRED COMPONENTS regex thread system filesystem) 6 | 7 | if(COMMAND cmake_policy) 8 | cmake_policy(SET CMP0003 NEW) 9 | endif(COMMAND cmake_policy) 10 | link_directories(${OpenCV_LIBS_DIR}) 11 | include_directories(${OpenCV2_INCLUDE_DIRS}) 12 | 13 | set(DEFAULT_HAS_MRPT ON) 14 | set(HAS_MRPT ${DEFAULT_HAS_MRPT} CACHE BOOL "Build the PointGrey Bumblebee2 SVO application that employs the MRPT library") 15 | 16 | SET(BUILD_SHARED_LIBS ON) 17 | SET(CMAKE_MODULE_PATH $ENV{CMAKE_MODULE_PATH}) 18 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3 -mtune=native -march=native") 19 | 20 | add_definitions(-DBOOST_NO_CXX11_SCOPED_ENUMS) 21 | 22 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/build) 23 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 24 | 25 | # MRPT library (optional, only with representation purposes) 26 | if(HAS_MRPT) 27 | find_package(MRPT REQUIRED base opengl gui hwdrivers) 28 | set(MRPT_DONT_USE_DBG_LIBS 1) #use release libraries for linking even if "Debug" CMake build 29 | add_definitions(-DHAS_MRPT) 30 | endif(HAS_MRPT) 31 | 32 | # YAML library 33 | FIND_PACKAGE(yaml-cpp REQUIRED CONFIG PATHS ${YAML_PATHS}) 34 | 35 | # Include dirs 36 | include_directories( 37 | include 38 | ${Eigen3_INCLUDE_DIRS} 39 | ${OpenCV_INCLUDE_DIRS} 40 | ${Boost_INCLUDE_DIRS} 41 | ${YAML_CPP_INCLUDE_DIR} 42 | ${PROJECT_SOURCE_DIR}/3rdparty/line_descriptor/include/ 43 | ) 44 | 45 | # Set link libraries 46 | list(APPEND LINK_LIBS 47 | ${OpenCV_LIBS} 48 | ${Boost_LIBRARIES} 49 | ${YAML_CPP_LIBRARIES} 50 | ${PROJECT_SOURCE_DIR}/3rdparty/line_descriptor/lib/liblinedesc.so 51 | ) 52 | 53 | # Set source files 54 | if(HAS_MRPT) 55 | list(APPEND SOURCEFILES 56 | src/sceneRepresentation.cpp 57 | ) 58 | endif() 59 | 60 | list(APPEND SOURCEFILES 61 | src/auxiliar.cpp 62 | src/config.cpp 63 | src/dataset.cpp 64 | src/gridStructure.cpp 65 | src/lineIterator.cpp 66 | src/matching.cpp 67 | src/pinholeStereoCamera.cpp 68 | src/stereoFeatures.cpp 69 | src/stereoFrame.cpp 70 | src/stereoFrameHandler.cpp 71 | src/timer.cpp 72 | ) 73 | 74 | # List all files (headers) contained by StVO-PL library 75 | file(GLOB_RECURSE all_include_files RELATIVE "${CMAKE_SOURCE_DIR}" *.h *.hpp) 76 | 77 | # Visualize the files of this directory in IDE creating an custom empty target 78 | add_custom_target( stvo_includes DEPENDS ${all_include_files} SOURCES ${all_include_files} ) 79 | 80 | # Create StVO-PL library 81 | add_library(stvo SHARED ${SOURCEFILES}) 82 | 83 | if(HAS_MRPT) 84 | target_link_libraries(stvo ${LINK_LIBS} ${MRPT_LIBS}) 85 | else() 86 | target_link_libraries(stvo ${LINK_LIBS}) 87 | endif() 88 | 89 | # Applications 90 | add_executable ( imagesStVO app/imagesStVO.cpp ) 91 | target_link_libraries( imagesStVO stvo ) 92 | 93 | 94 | 95 | 96 | 97 | 98 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PL-StVO # 2 | 3 | This code contains an algorithm to compute stereo visual odometry by using both point and line segment features. 4 | 5 | **Authors:** [Ruben Gomez-Ojeda](http://mapir.isa.uma.es/mapirwebsite/index.php/people/164-ruben-gomez) 6 | and [David Zuñiga-Noël](http://mapir.isa.uma.es/mapirwebsite/index.php/people/270) 7 | and [Javier Gonzalez-Jimenez](http://mapir.isa.uma.es/mapirwebsite/index.php/people/95-javier-gonzalez-jimenez) 8 | 9 | **Related publication:** [*Robust Stereo Visual Odometry through a Probabilistic Combination of Points and Line Segments*](http://mapir.isa.uma.es/mapirwebsite/index.php/people/164-ruben-gomez) 10 | 11 | If you use PL-StVO in your research work, please cite: 12 | 13 | @InProceedings{Gomez2015, 14 | Title = {Robust Stereo Visual Odometry through a Probabilistic Combination of Points and Line Segments}, 15 | Author = {Gomez-Ojeda, Ruben and Gonzalez-Jimenez, Javier}, 16 | Booktitle = {Robotics and Automation (ICRA), 2016 IEEE International Conference on}, 17 | Year = {2016}, 18 | Publisher = {IEEE} 19 | } 20 | 21 | The provided code is published under the General Public License Version 3 (GPL v3). More information can be found in the "GPU_LICENSE.txt" also included in the repository. 22 | 23 | Please do not hesitate to contact the authors if you have any further questions. 24 | 25 | 26 | ## 1. Prerequisites and dependencies 27 | 28 | ### OpenCV 3.x.x 29 | It can be easily found at http://opencv.org. 30 | 31 | ### Eigen3 32 | http://eigen.tuxfamily.org 33 | 34 | ### Boost 35 | Installation on Ubuntu 16.04: 36 | ``` 37 | sudo apt-get install libboost-dev 38 | ``` 39 | 40 | ### YAML 41 | Installation on Ubuntu 16.04: 42 | ``` 43 | sudo apt install libyaml-cpp-dev 44 | ``` 45 | 46 | ### MRPT 47 | In case of using the provided representation class. 48 | Download and install instructions can be found at: http://www.mrpt.org/ 49 | 50 | #### Known Issues: 51 | If working with the most recent versions of the MRPT library you might find some issues due to hard refactoring, for which we recommend to use this version instead (the last one we tested): 52 | ``` 53 | https://github.com/MRPT/mrpt/tree/0c3d605c3cbf5f2ffb8137089e43ebdae5a55de3 54 | ``` 55 | 56 | ### Line Descriptor 57 | We have modified the [*line_descriptor*](https://github.com/opencv/opencv_contrib/tree/master/modules/line_descriptor) module from the [OpenCV/contrib](https://github.com/opencv/opencv_contrib) library (both BSD) which is included in the *3rdparty* folder. 58 | 59 | 60 | 61 | 62 | ## 2. Configuration and generation 63 | A CMakeLists.txt file is included to detect external dependencies and generate the project. 64 | 65 | The project builds "imagesStVO", a customizable application where the user must introduce the inputs to the SVO algorithm, and then process the provided output. 66 | 67 | 68 | 69 | ## 3. Usage 70 | 71 | ### Datasets configuration 72 | We employ an environment variable, *${DATASETS_DIR}*, pointing the directory that contains our datasets. Each sequence from each dataset must contain in its root folder a file named *dataset_params.yaml*, that indicates at least the camera model and the subfolders with the left and right images. We provide dataset parameters files for several datasets and cameras with the format *xxxx_params.yaml*. 73 | 74 | ### Configuration files 75 | For running VO we can load the default parameters file or employ the *config_xxxx.yaml* files provided for every dataset. 76 | 77 | ### VO Application 78 | Usage: ./imagesStVO [options] 79 | Options: 80 | -c Config file 81 | -o Offset (number of frames to skip in the dataset directory 82 | -n Number of frames to process the sequence 83 | -s Parameter to skip s-1 frames (default 1) 84 | 85 | A full command would be: 86 | 87 | ./imagesStVO kitti/00 -c ../config/config_kitti.yaml -o 100 -s 2 -n 1000 88 | 89 | where we are processing the sequence 00 from the KITTI dataset (in our dataset folders) with the custom config file, with an offset *-c* allowing to skip the first 100 images, a parameter *-s* to consider only one every 2 images, and a parameter *-n* to only consider 1000 input pairs. 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | -------------------------------------------------------------------------------- /app/imagesStVO.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #ifdef HAS_MRPT 24 | #include 25 | #endif 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | #include "dataset.h" 32 | #include "timer.h" 33 | 34 | using namespace StVO; 35 | 36 | void showHelp(); 37 | bool getInputArgs(int argc, char **argv, std::string &dataset_name, int &frame_offset, int &frame_number, int &frame_step, std::string &config_file); 38 | 39 | int main(int argc, char **argv) 40 | { 41 | 42 | // read inputs 43 | string dataset_name, config_file; 44 | int frame_offset = 0, frame_number = 0, frame_step = 1; 45 | if (!getInputArgs(argc, argv, dataset_name, frame_offset, frame_number, frame_step, config_file)) { 46 | showHelp(); 47 | return -1; 48 | } 49 | 50 | if (!config_file.empty()) Config::loadFromFile(config_file); 51 | 52 | // read dataset root dir fron environment variable 53 | boost::filesystem::path dataset_path(string( getenv("DATASETS_DIR"))); 54 | if (!boost::filesystem::exists(dataset_path) || !boost::filesystem::is_directory(dataset_path)) { 55 | cout << "Check your DATASETS_DIR environment variable" << endl; 56 | return -1; 57 | } 58 | 59 | dataset_path /= dataset_name; 60 | if (!boost::filesystem::exists(dataset_path) || !boost::filesystem::is_directory(dataset_path)) { 61 | cout << "Invalid dataset path" << endl; 62 | return -1; 63 | } 64 | 65 | string dataset_dir = dataset_path.string(); 66 | PinholeStereoCamera* cam_pin = new PinholeStereoCamera((dataset_path / "dataset_params.yaml").string()); 67 | Dataset dataset(dataset_dir, *cam_pin, frame_offset, frame_number, frame_step); 68 | 69 | // create scene 70 | Matrix4d Tcw, T_inc = Matrix4d::Identity(); 71 | Vector6d cov_eig; 72 | Matrix6d cov; 73 | Tcw = Matrix4d::Identity(); 74 | Tcw << 1, 0, 0, 0, 0, 0, 1, 0, 0, -1, 0, 0, 0, 0, 0, 1; 75 | 76 | #ifdef HAS_MRPT 77 | sceneRepresentation scene("../config/scene_config.ini"); 78 | scene.initializeScene(Tcw, false); 79 | #endif 80 | 81 | Timer timer; 82 | 83 | // initialize and run PL-StVO 84 | int frame_counter = 0; 85 | double t1; 86 | StereoFrameHandler* StVO = new StereoFrameHandler(cam_pin); 87 | Mat img_l, img_r; 88 | while (dataset.nextFrame(img_l, img_r)) 89 | { 90 | if( frame_counter == 0 ) // initialize 91 | StVO->initialize(img_l,img_r,0); 92 | else // run 93 | { 94 | // PL-StVO 95 | timer.start(); 96 | StVO->insertStereoPair( img_l, img_r, frame_counter ); 97 | StVO->optimizePose(); 98 | t1 = timer.stop(); 99 | 100 | T_inc = StVO->curr_frame->DT; 101 | cov = StVO->curr_frame->DT_cov; 102 | cov_eig = StVO->curr_frame->DT_cov_eig; 103 | 104 | // update scene 105 | #ifdef HAS_MRPT 106 | scene.setText(frame_counter,t1,StVO->n_inliers_pt,StVO->matched_pt.size(),StVO->n_inliers_ls,StVO->matched_ls.size()); 107 | scene.setCov( cov ); 108 | scene.setPose( T_inc ); 109 | scene.setImage(StVO->curr_frame->plotStereoFrame()); 110 | scene.updateScene(StVO->matched_pt, StVO->matched_ls); 111 | #endif 112 | 113 | // console output 114 | cout.setf(ios::fixed,ios::floatfield); cout.precision(8); 115 | cout << "Frame: " << frame_counter << "\tRes.: " << StVO->curr_frame->err_norm; 116 | cout.setf(ios::fixed,ios::floatfield); cout.precision(3); 117 | cout << " \t Proc. time: " << t1 << " ms\t "; 118 | if( Config::adaptativeFAST() ) cout << "\t FAST: " << StVO->orb_fast_th; 119 | if( Config::hasPoints()) cout << "\t Points: " << StVO->matched_pt.size() << " (" << StVO->n_inliers_pt << ") " ; 120 | if( Config::hasLines() ) cout << "\t Lines: " << StVO->matched_ls.size() << " (" << StVO->n_inliers_ls << ") " ; 121 | cout << endl; 122 | 123 | // update StVO 124 | StVO->updateFrame(); 125 | } 126 | 127 | frame_counter++; 128 | } 129 | 130 | // wait until the scene is closed 131 | #ifdef HAS_MRPT 132 | while( scene.isOpen() ); 133 | #endif 134 | 135 | return 0; 136 | } 137 | 138 | void showHelp() { 139 | cout << endl << "Usage: ./imagesStVO [options]" << endl 140 | << "Options:" << endl 141 | << "\t-c Config file" << endl 142 | << "\t-o Offset (number of frames to skip in the dataset directory" << endl 143 | << "\t-n Number of frames to process the sequence" << endl 144 | << "\t-s Parameter to skip s-1 frames (default 1)" << endl 145 | << endl; 146 | } 147 | 148 | bool getInputArgs(int argc, char **argv, std::string &dataset_name, int &frame_offset, int &frame_number, int &frame_step, std::string &config_file) { 149 | 150 | if( argc < 2 || argc > 10 || (argc % 2) == 1 ) 151 | return false; 152 | 153 | dataset_name = argv[1]; 154 | int nargs = argc/2 - 1; 155 | for( int i = 0; i < nargs; i++ ) 156 | { 157 | int j = 2*i + 2; 158 | if( string(argv[j]) == "-o" ) 159 | frame_offset = stoi(argv[j+1]); 160 | else if( string(argv[j]) == "-n" ) 161 | frame_number = stoi(argv[j+1]); 162 | else if( string(argv[j]) == "-s" ) 163 | frame_step = stoi(argv[j+1]); 164 | else if (string(argv[j]) == "-c") 165 | config_file = string(argv[j+1]); 166 | else 167 | return false; 168 | } 169 | 170 | return true; 171 | } 172 | -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | echo "Building 3rdparty/line_descriptor ... " 2 | cd 3rdparty/line_descriptor 3 | mkdir build 4 | cd build 5 | cmake .. 6 | make -j 7 | cd ../../../ 8 | 9 | echo "Building StVO-PL ... " 10 | mkdir build 11 | cd build 12 | cmake .. 13 | make -j 14 | -------------------------------------------------------------------------------- /config/aux/help.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rubengooj/stvo-pl/baecb5f684cae8f71d60d5d230a5e908292434a8/config/aux/help.png -------------------------------------------------------------------------------- /config/aux/legend.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rubengooj/stvo-pl/baecb5f684cae8f71d60d5d230a5e908292434a8/config/aux/legend.png -------------------------------------------------------------------------------- /config/aux/legend_comp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rubengooj/stvo-pl/baecb5f684cae8f71d60d5d230a5e908292434a8/config/aux/legend_comp.png -------------------------------------------------------------------------------- /config/aux/legend_full.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rubengooj/stvo-pl/baecb5f684cae8f71d60d5d230a5e908292434a8/config/aux/legend_full.png -------------------------------------------------------------------------------- /config/config/config.yaml: -------------------------------------------------------------------------------- 1 | # StVO-PL options 2 | # ----------------------------------------------------------------------------------------------------- 3 | has_points : true # true if using points 4 | has_lines : true # true if using line segments 5 | use_fld_lines : false # true if using FLD detector 6 | lr_in_parallel : true # true if detecting and matching features in parallel 7 | pl_in_parallel : true # true if detecting points and line segments in parallel 8 | best_lr_matches : true # true if double-checking the matches between the two images 9 | adaptative_fast : true # true if using adaptative fast_threshold 10 | use_motion_model : false # true if using constant motion model 11 | #TODO: adaptative fld threshold 12 | 13 | # Tracking parameters 14 | # ----------------------------------------------------------------------------------------------------- 15 | # Point features 16 | max_dist_epip : 0.0 # max. epipolar distance in pixels 17 | min_disp : 1.0 # min. disparity (avoid points in the infinite) 18 | min_ratio_12_p : 0.75 # min. ratio between the first and second best matches 19 | 20 | # Line segment features 21 | line_sim_th : 0.75 # threshold for cosine similarity 22 | stereo_overlap_th : 0.75 23 | f2f_overlap_th : 0.75 24 | min_line_length : 0.025 # min. line length (relative to img size) 25 | line_horiz_th : 0.1 # parameter to avoid horizontal lines (pixels) 26 | min_ratio_12_l : 0.75 # parameter to avoid outliers in line matching 27 | ls_min_disp_ratio : 0.7 # min ratio between min(disp)/max(disp) for a LS 28 | 29 | # Adaptative FAST parameters 30 | fast_min_th : 5 # min. value for FAST threshold 31 | fast_max_th : 50 # max. value for FAST threshold 32 | fast_inc_th : 5 # base increment for the FAST threshold 33 | fast_feat_th : 50 # base number of features to increase/decrease FAST threshold 34 | fast_err_th : 0.5 # threshold for the optimization error 35 | 36 | # TODO: add similar adaptative FAST for FLD (take care with the min and max values regarding performance) 37 | 38 | # RGB-D features 39 | rgbd_min_depth : 0.5 40 | rgbd_max_depth : 3.0 41 | 42 | # Optimization parameters 43 | # ----------------------------------------------------------------------------------------------------- 44 | homog_th : 1e-7 # avoid points in the infinite 45 | min_features : 10 # min. number of features to perform StVO 46 | max_iters : 5 # max. number of iterations in the first stage of the optimization 47 | max_iters_ref : 10 # max. number of iterations in the refinement stage 48 | min_error : 1e-7 # min. error to stop the optimization 49 | min_error_change : 1e-7 # min. error change to stop the optimization 50 | inlier_k : 4.0 # factor to discard outliers before the refinement stage 51 | 52 | # Feature detection parameters 53 | # ----------------------------------------------------------------------------------------------------- 54 | matching_stereo : 0 # 0 - pure descriptor based | 1 - window based plus descriptor 55 | matching_s_ws : 10 # size of the windows (in pixels) to look for stereo matches (if matching_stereo=1) 56 | matching_f2f_ws : 3 # size of the windows (in pixels) to look for f2f matches 57 | 58 | # ORB detector 59 | orb_nfeatures : 1200 # number of ORB features to detect 60 | orb_scale_factor : 1.2 # pyramid decimation ratio for the ORB detector 61 | orb_nlevels : 4 # number of pyramid levels 62 | orb_edge_th : 19 # size of the border where the features are not detected 63 | orb_wta_k : 2 # number of points that produce each element of the oriented BRIEF descriptor 64 | orb_score : 1 # 0 - HARRIS | 1 - FAST 65 | orb_patch_size : 31 # size of the patch used by the oriented BRIEF descriptor. 66 | orb_fast_th : 20 # default FAST threshold 67 | # LSD parameters 68 | lsd_nfeatures : 300 # number of LSD lines detected (set to 0 if keeping all lines) 69 | lsd_refine : 0 # the way of refining or not the detected lines 70 | lsd_scale : 1.2 # scale of the image that will be used to find the lines 71 | lsd_sigma_scale : 0.6 # sigma for Gaussian filter 72 | lsd_quant : 2.0 # bound to the quantization error on the gradient norm 73 | lsd_ang_th : 22.5 # gradient angle tolerance in degrees 74 | lsd_log_eps : 1.0 # detection threshold (only for advanced refinement) 75 | lsd_density_th : 0.6 # minimal density of aligned region points in the enclosing rectangle 76 | lsd_n_bins : 1024 # number of bins in pseudo-ordering of gradient modulus 77 | -------------------------------------------------------------------------------- /config/config/config_euroc.yaml: -------------------------------------------------------------------------------- 1 | # StVO parameters 2 | # --------------- 3 | # kf decision (SLAM) parameters 4 | min_entropy_ratio : 0.85 5 | max_kf_t_dist : 5.0 6 | max_kf_r_dist : 15.0 7 | 8 | # StVO-PL options 9 | # ----------------------------------------------------------------------------------------------------- 10 | has_points : true # true if using points 11 | has_lines : true # true if using line segments 12 | use_fld_lines : false # true if using FLD detector 13 | lr_in_parallel : true # true if detecting and matching features in parallel 14 | pl_in_parallel : true # true if detecting points and line segments in parallel 15 | best_lr_matches : true # true if double-checking the matches between the two images 16 | adaptative_fast : true # true if using adaptative fast_threshold 17 | use_motion_model : false # true if using constant motion model 18 | 19 | # Tracking parameters 20 | # ----------------------------------------------------------------------------------------------------- 21 | # Point features 22 | max_dist_epip : 1.0 # max. epipolar distance in pixels 23 | min_disp : 1.0 # min. disparity (avoid points in the infinite) 24 | min_ratio_12_p : 0.9 # min. ratio between the first and second best matches 25 | 26 | # Line segment features 27 | line_sim_th : 0.75 # threshold for cosine similarity 28 | stereo_overlap_th : 0.75 29 | f2f_overlap_th : 0.75 30 | min_line_length : 0.025 # min. line length (relative to img size) 31 | line_horiz_th : 0.1 # parameter to avoid horizontal lines (pixels) 32 | min_ratio_12_l : 0.9 # parameter to avoid outliers in line matching 33 | ls_min_disp_ratio : 0.7 # min ratio between min(disp)/max(disp) for a LS 34 | 35 | # Adaptative FAST parameters 36 | fast_min_th : 5 # min. value for FAST threshold 37 | fast_max_th : 50 # max. value for FAST threshold 38 | fast_inc_th : 5 # base increment for the FAST threshold 39 | fast_feat_th : 50 # base number of features to increase/decrease FAST threshold 40 | fast_err_th : 0.5 # threshold for the optimization error 41 | 42 | # Optimization parameters 43 | # ----------------------------------------------------------------------------------------------------- 44 | homog_th : 1e-7 # avoid points in the infinite 45 | min_features : 10 # min. number of features to perform StVO 46 | max_iters : 5 # max. number of iterations in the first stage of the optimization 47 | max_iters_ref : 10 # max. number of iterations in the refinement stage 48 | min_error : 1e-7 # min. error to stop the optimization 49 | min_error_change : 1e-7 # min. error change to stop the optimization 50 | inlier_k : 4.0 # factor to discard outliers before the refinement stage 51 | 52 | # Feature detection parameters 53 | # ----------------------------------------------------------------------------------------------------- 54 | matching_strategy : 3 55 | matching_s_ws : 10 56 | matching_f2f_ws : 3 57 | 58 | # ORB detector 59 | orb_nfeatures : 800 60 | orb_scale_factor : 1.2 61 | orb_nlevels : 4 62 | orb_edge_th : 19 63 | orb_wta_k : 2 # was set to 4 64 | orb_score : 1 # 0 - HARRIS | 1 - FAST 65 | orb_patch_size : 31 66 | orb_fast_th : 20 # default FAST threshold 67 | # LSD parameters 68 | lsd_nfeatures : 300 # set to 0 if keeping all lines 69 | lsd_refine : 0 70 | lsd_scale : 1.2 71 | lsd_sigma_scale : 0.6 72 | lsd_quant : 2.0 73 | lsd_ang_th : 22.5 74 | lsd_log_eps : 1.0 75 | lsd_density_th : 0.6 76 | lsd_n_bins : 1024 77 | -------------------------------------------------------------------------------- /config/config/config_fast.yaml: -------------------------------------------------------------------------------- 1 | # StVO-PL options 2 | # ----------------------------------------------------------------------------------------------------- 3 | has_points : true # true if using points 4 | has_lines : true # true if using line segments 5 | use_fld_lines : false # true if using FLD detector 6 | lr_in_parallel : true # true if detecting and matching features in parallel 7 | pl_in_parallel : true # true if detecting points and line segments in parallel 8 | best_lr_matches : true # true if double-checking the matches between the two images 9 | adaptative_fast : true # true if using adaptative fast_threshold 10 | use_motion_model : false # true if using constant motion model 11 | 12 | # Tracking parameters 13 | # ----------------------------------------------------------------------------------------------------- 14 | # Point features 15 | max_dist_epip : 1.0 # max. epipolar distance in pixels 16 | min_disp : 1.0 # min. disparity (avoid points in the infinite) 17 | min_ratio_12_p : 0.75 # min. ratio between the first and second best matches 18 | 19 | # Line segment features 20 | line_sim_th : 0.75 # threshold for cosine similarity 21 | stereo_overlap_th : 0.85 22 | f2f_overlap_th : 0.85 23 | min_line_length : 0.05 # min. line length (relative to img size) 24 | line_horiz_th : 1.0 # parameter to avoid horizontal lines (pixels) 25 | min_ratio_12_l : 0.75 # parameter to avoid outliers in line matching 26 | ls_min_disp_ratio : 0.7 # min ratio between min(disp)/max(disp) for a LS 27 | 28 | # Adaptative FAST parameters 29 | fast_min_th : 5 # min. value for FAST threshold 30 | fast_max_th : 50 # max. value for FAST threshold 31 | fast_inc_th : 5 # base increment for the FAST threshold 32 | fast_feat_th : 50 # base number of features to increase/decrease FAST threshold 33 | fast_err_th : 0.5 # threshold for the optimization error 34 | 35 | # Optimization parameters 36 | # ----------------------------------------------------------------------------------------------------- 37 | homog_th : 1e-7 # avoid points in the infinite 38 | min_features : 10 # min. number of features to perform StVO 39 | max_iters : 5 # max. number of iterations in the first stage of the optimization 40 | max_iters_ref : 10 # max. number of iterations in the refinement stage 41 | min_error : 1e-7 # min. error to stop the optimization 42 | min_error_change : 1e-7 # min. error change to stop the optimization 43 | inlier_k : 4.0 # factor to discard outliers before the refinement stage 44 | 45 | # Feature detection parameters 46 | # ----------------------------------------------------------------------------------------------------- 47 | matching_strategy : 3 48 | matching_s_ws : 10 49 | matching_f2f_ws : 3 50 | 51 | # ORB detector 52 | orb_nfeatures : 600 53 | orb_scale_factor : 1.2 54 | orb_nlevels : 1 55 | orb_edge_th : 19 56 | orb_wta_k : 2 # was set to 4 57 | orb_score : 1 # 0 - HARRIS | 1 - FAST 58 | orb_patch_size : 31 59 | orb_fast_th : 20 # default FAST threshold 60 | # LSD parameters 61 | lsd_nfeatures : 100 # set to 0 if keeping all lines 62 | lsd_refine : 0 63 | lsd_scale : 1.2 64 | lsd_sigma_scale : 0.6 65 | lsd_quant : 2.0 66 | lsd_ang_th : 22.5 67 | lsd_log_eps : 1.0 68 | lsd_density_th : 0.6 69 | lsd_n_bins : 1024 70 | -------------------------------------------------------------------------------- /config/config/config_full.yaml: -------------------------------------------------------------------------------- 1 | # StVO-PL options 2 | # ----------------------------------------------------------------------------------------------------- 3 | has_points : true # true if using points 4 | has_lines : true # true if using line segments 5 | use_fld_lines : false # true if using FLD detector 6 | lr_in_parallel : true # true if detecting and matching features in parallel 7 | pl_in_parallel : true # true if detecting points and line segments in parallel 8 | best_lr_matches : true # true if double-checking the matches between the two images 9 | adaptative_fast : true # true if using adaptative fast_threshold 10 | use_motion_model : false # true if using constant motion model 11 | #TODO: adaptative fld threshold 12 | 13 | # Tracking parameters 14 | # ----------------------------------------------------------------------------------------------------- 15 | # Point features 16 | max_dist_epip : 0.0 # max. epipolar distance in pixels 17 | min_disp : 1.0 # min. disparity (avoid points in the infinite) 18 | min_ratio_12_p : 0.75 # min. ratio between the first and second best matches 19 | 20 | # Line segment features 21 | line_sim_th : 0.75 # threshold for cosine similarity 22 | stereo_overlap_th : 0.75 23 | f2f_overlap_th : 0.75 24 | min_line_length : 0.025 # min. line length (relative to img size) 25 | line_horiz_th : 0.1 # parameter to avoid horizontal lines (pixels) 26 | min_ratio_12_l : 0.75 # parameter to avoid outliers in line matching 27 | ls_min_disp_ratio : 0.7 # min ratio between min(disp)/max(disp) for a LS 28 | 29 | # Adaptative FAST parameters 30 | fast_min_th : 5 # min. value for FAST threshold 31 | fast_max_th : 50 # max. value for FAST threshold 32 | fast_inc_th : 5 # base increment for the FAST threshold 33 | fast_feat_th : 50 # base number of features to increase/decrease FAST threshold 34 | fast_err_th : 0.5 # threshold for the optimization error 35 | 36 | # TODO: add similar adaptative FAST for FLD (take care with the min and max values regarding performance) 37 | 38 | # RGB-D features 39 | rgbd_min_depth : 0.5 40 | rgbd_max_depth : 3.0 41 | 42 | # Optimization parameters 43 | # ----------------------------------------------------------------------------------------------------- 44 | homog_th : 1e-7 # avoid points in the infinite 45 | min_features : 10 # min. number of features to perform StVO 46 | max_iters : 5 # max. number of iterations in the first stage of the optimization 47 | max_iters_ref : 10 # max. number of iterations in the refinement stage 48 | min_error : 1e-7 # min. error to stop the optimization 49 | min_error_change : 1e-7 # min. error change to stop the optimization 50 | inlier_k : 4.0 # factor to discard outliers before the refinement stage 51 | 52 | # Feature detection parameters 53 | # ----------------------------------------------------------------------------------------------------- 54 | matching_stereo : 0 # 0 - pure descriptor based | 1 - window based plus descriptor 55 | matching_s_ws : 10 # size of the windows (in pixels) to look for stereo matches (if matching_stereo=1) 56 | matching_f2f_ws : 3 # size of the windows (in pixels) to look for f2f matches 57 | 58 | # ORB detector 59 | orb_nfeatures : 1200 # number of ORB features to detect 60 | orb_scale_factor : 1.2 # pyramid decimation ratio for the ORB detector 61 | orb_nlevels : 4 # number of pyramid levels 62 | orb_edge_th : 19 # size of the border where the features are not detected 63 | orb_wta_k : 2 # number of points that produce each element of the oriented BRIEF descriptor 64 | orb_score : 1 # 0 - HARRIS | 1 - FAST 65 | orb_patch_size : 31 # size of the patch used by the oriented BRIEF descriptor. 66 | orb_fast_th : 20 # default FAST threshold 67 | # LSD parameters 68 | lsd_nfeatures : 300 # number of LSD lines detected (set to 0 if keeping all lines) 69 | lsd_refine : 0 # the way of refining or not the detected lines 70 | lsd_scale : 1.2 # scale of the image that will be used to find the lines 71 | lsd_sigma_scale : 0.6 # sigma for Gaussian filter 72 | lsd_quant : 2.0 # bound to the quantization error on the gradient norm 73 | lsd_ang_th : 22.5 # gradient angle tolerance in degrees 74 | lsd_log_eps : 1.0 # detection threshold (only for advanced refinement) 75 | lsd_density_th : 0.6 # minimal density of aligned region points in the enclosing rectangle 76 | lsd_n_bins : 1024 # number of bins in pseudo-ordering of gradient modulus 77 | -------------------------------------------------------------------------------- /config/config/config_kitti.yaml: -------------------------------------------------------------------------------- 1 | # StVO-PL options 2 | # ----------------------------------------------------------------------------------------------------- 3 | has_points : true # true if using points 4 | has_lines : true # true if using line segments 5 | use_fld_lines : false # true if using FLD detector 6 | 7 | lr_in_parallel : true # true if detecting and matching features in parallel 8 | pl_in_parallel : true # true if detecting points and line segments in parallel 9 | best_lr_matches : true # true if double-checking the matches between the two images 10 | adaptative_fast : true # true if using adaptative fast_threshold 11 | 12 | use_motion_model : false # true if using constant motion model 13 | 14 | # Tracking parameters 15 | # ----------------------------------------------------------------------------------------------------- 16 | # Point feature 17 | max_dist_epip : 0.0 # max. epipolar distance in pixels 18 | min_disp : 1.0 # min. disparity (avoid points in the infinite) 19 | min_ratio_12_p : 0.75 # min. ratio between the first and second best matches 20 | 21 | # Line segment features 22 | line_sim_th : 0.75 # threshold for cosine similarity 23 | stereo_overlap_th : 0.75 24 | f2f_overlap_th : 0.75 25 | min_line_length : 0.025 # min. line length (relative to img size) 26 | line_horiz_th : 0.1 # parameter to avoid horizontal lines (pixels) 27 | min_ratio_12_l : 0.75 # parameter to avoid outliers in line matching 28 | ls_min_disp_ratio : 0.7 # min ratio between min(disp)/max(disp) for a LS 29 | 30 | # Adaptative FAST parameters 31 | fast_min_th : 7 # min. value for FAST threshold 32 | fast_max_th : 30 # max. value for FAST threshold 33 | fast_inc_th : 5 # base increment for the FAST threshold 34 | fast_feat_th : 50 # base number of features to increase/decrease FAST threshold 35 | fast_err_th : 0.5 # threshold for the optimization error 36 | 37 | # Optimization parameters 38 | # ----------------------------------------------------------------------------------------------------- 39 | homog_th : 1e-7 # avoid points in the infinite 40 | min_features : 10 # min. number of features to perform StVO 41 | max_iters : 5 # max. number of iterations in the first stage of the optimization 42 | max_iters_ref : 10 # max. number of iterations in the refinement stage 43 | min_error : 1e-7 # min. error to stop the optimization 44 | min_error_change : 1e-7 # min. error change to stop the optimization 45 | inlier_k : 1.2 # factor to discard outliers before the refinement stage 46 | 47 | # Feature detection parameters 48 | # ----------------------------------------------------------------------------------------------------- 49 | matching_stereo : 0 # 0 - pure descriptor based | 1 - window based plus descriptor 50 | matching_s_ws : 10 # size of the windows (in pixels) to look for stereo matches (if matching_stereo=1) 51 | matching_f2f_ws : 3 # size of the windows (in pixels) to look for f2f matches 52 | 53 | # ORB detector 54 | orb_nfeatures : 2000 # number of ORB features to detect 55 | orb_scale_factor : 1.2 # pyramid decimation ratio for the ORB detector 56 | orb_nlevels : 1 # number of pyramid levels 57 | orb_edge_th : 19 # size of the border where the features are not detected 58 | orb_wta_k : 2 # number of points that produce each element of the oriented BRIEF descriptor 59 | orb_score : 1 # 0 - HARRIS | 1 - FAST 60 | orb_patch_size : 31 # size of the patch used by the oriented BRIEF descriptor. 61 | orb_fast_th : 20 # default FAST threshold 62 | 63 | # LSD parameters 64 | lsd_nfeatures : 100 # number of LSD lines detected (set to 0 if keeping all lines) 65 | lsd_refine : 0 # the way of refining or not the detected lines 66 | lsd_scale : 1.2 # scale of the image that will be used to find the lines 67 | lsd_sigma_scale : 0.6 # sigma for Gaussian filter 68 | lsd_quant : 2.0 # bound to the quantization error on the gradient norm 69 | lsd_ang_th : 22.5 # gradient angle tolerance in degrees 70 | lsd_log_eps : 1.0 # detection threshold (only for advanced refinement) 71 | lsd_density_th : 0.6 # minimal density of aligned region points in the enclosing rectangle 72 | lsd_n_bins : 1024 # number of bins in pseudo-ordering of gradient modulus 73 | -------------------------------------------------------------------------------- /config/config/config_reduced.yaml: -------------------------------------------------------------------------------- 1 | # StVO-PL options 2 | # ----------------------------------------------------------------------------------------------------- 3 | has_points : true # true if using points 4 | has_lines : true # true if using line segments 5 | lr_in_parallel : true # true if detecting and matching features in parallel 6 | pl_in_parallel : true # true if detecting points and line segments in parallel 7 | 8 | # Tracking parameters 9 | # ----------------------------------------------------------------------------------------------------- 10 | # Point features 11 | min_ratio_12_p : 0.75 # min. ratio between the first and second best matches 12 | # Line segment features 13 | min_ratio_12_l : 0.75 # parameter to avoid outliers in line matching 14 | 15 | # Optimization parameters 16 | # ----------------------------------------------------------------------------------------------------- 17 | inlier_k : 4.0 # factor to discard outliers before the refinement stage 18 | 19 | # Feature detection parameters 20 | # ----------------------------------------------------------------------------------------------------- 21 | # ORB detector 22 | orb_nfeatures : 1200 # number of ORB features to detect 23 | orb_nlevels : 4 # number of pyramid levels 24 | orb_fast_th : 20 # default FAST threshold 25 | # LSD parameters 26 | lsd_nfeatures : 300 # number of LSD lines detected (set to 0 if keeping all lines) 27 | -------------------------------------------------------------------------------- /config/dataset_params/asusxtion_params.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | cam_bl: 1.0 3 | cam_width: 640 4 | cam_height: 480 5 | cam_model: Pinhole 6 | cam_fx: 570.3422241210938 7 | cam_fy: 570.3422241210938 8 | cam_cx: 319.5 9 | cam_cy: 239.5 10 | cam_d0: 0.0 11 | cam_d1: 0.0 12 | cam_d2: 0.0 13 | cam_d3: 0.0 14 | images_subfolder_l: image_l/ 15 | images_subfolder_r: image_r/ 16 | -------------------------------------------------------------------------------- /config/dataset_params/dataset_params.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | cam_cx: 601.8873000000 3 | cam_cy: 183.1104000000 4 | cam_d0: 0.0 5 | cam_d1: 0.0 6 | cam_d2: 0.0 7 | cam_d3: 0.0 8 | cam_fx: 707.0912 9 | cam_fy: 707.0912 10 | cam_bl: 0.54 11 | cam_height: 376 12 | cam_model: Pinhole 13 | cam_width: 1241 14 | rx: 0.0 15 | ry: 0.0 16 | rz: 0.0 17 | tx: 0.0 18 | ty: 0.0 19 | tz: 0.0 20 | images_subfolder_l: image_2/ 21 | images_subfolder_r: image_3/ 22 | -------------------------------------------------------------------------------- /config/dataset_params/euroc_params.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | Kl: [458.654, 457.296, 367.215, 248.375] 3 | Kr: [457.587, 456.134, 379.999, 255.238] 4 | Dl: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] 5 | Dr: [-0.28368365, 0.07451284, -0.00010473, -3.55590700e-05] 6 | Rl: [0.9999663475300330, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176] 7 | Rr: [0.9999633526194376, -0.003625811871560086, 0.007755443660172947, 0.003680398547259526, 0.9999684752771629, -0.007035845251224894, -0.007729688520722713, 0.007064130529506649, 0.9999451734846440] 8 | cam_bl: 0.110077842 9 | cam_height: 480 10 | cam_model: Pinhole 11 | cam_width: 752 12 | R: [ 9.99997256e-01, 2.31206719e-03, 3.76008102e-04, 13 | -2.31713572e-03, 9.99898049e-01, 1.40898358e-02, 14 | -3.43393121e-04, -1.40906685e-02, 9.99900663e-01 ] 15 | t: [ -0.11007381, 0.00039912, -0.0008537 ] 16 | images_subfolder_l: cam0/data/ 17 | images_subfolder_r: cam1/data/ 18 | -------------------------------------------------------------------------------- /config/dataset_params/kitti00-02.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | cam_cx: 607.1928 3 | cam_cy: 185.2157 4 | cam_d0: 0.0 5 | cam_d1: 0.0 6 | cam_d2: 0.0 7 | cam_d3: 0.0 8 | cam_fx: 718.856 9 | cam_fy: 718.856 10 | cam_bl: 0.537165719 11 | cam_height: 376 12 | cam_model: Pinhole 13 | cam_width: 1241 14 | rx: 0.0 15 | ry: 0.0 16 | rz: 0.0 17 | tx: 0.0 18 | ty: 0.0 19 | tz: 0.0 20 | images_subfolder_l: image_2/ 21 | images_subfolder_r: image_3/ 22 | -------------------------------------------------------------------------------- /config/dataset_params/kitti03.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | cam_cx: 609.5593 3 | cam_cy: 172.854 4 | cam_d0: 0.0 5 | cam_d1: 0.0 6 | cam_d2: 0.0 7 | cam_d3: 0.0 8 | cam_fx: 721.5377 9 | cam_fy: 721.5377 10 | cam_bl: 0.537150588 11 | cam_height: 375 12 | cam_model: Pinhole 13 | cam_width: 1242 14 | rx: 0.0 15 | ry: 0.0 16 | rz: 0.0 17 | tx: 0.0 18 | ty: 0.0 19 | tz: 0.0 20 | images_subfolder_l: image_2/ 21 | images_subfolder_r: image_3/ 22 | -------------------------------------------------------------------------------- /config/dataset_params/kitti04-10.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | cam_cx: 601.8873 3 | cam_cy: 183.1104 4 | cam_d0: 0.0 5 | cam_d1: 0.0 6 | cam_d2: 0.0 7 | cam_d3: 0.0 8 | cam_fx: 707.0912 9 | cam_fy: 707.0912 10 | cam_bl: 0.537150653 11 | cam_height: 370 12 | cam_model: Pinhole 13 | cam_width: 1226 14 | rx: 0.0 15 | ry: 0.0 16 | rz: 0.0 17 | tx: 0.0 18 | ty: 0.0 19 | tz: 0.0 20 | images_subfolder_l: image_2/ 21 | images_subfolder_r: image_3/ 22 | -------------------------------------------------------------------------------- /config/dataset_params/perceptin_params.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | Kl: [436.74812357841103, 433.86322279759912, 312.91964494989088, 229.11948801387757] 3 | Kr: [442.68650063581009, 439.32829876133417, 316.70314700945681, 237.35428785057542] 4 | cam_bl: 0.065 5 | cam_width: 640 6 | cam_height: 480 7 | cam_model: Pinhole 8 | Rl: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 ] 9 | Rr: [ 0.99998826205868663, -0.0038547708511774536, -0.0029353852442241856, 0.0038411041866586192, 0.99998182398315805, -0.0046473241702847112, 0.0029532462603597126, 0.0046359944997152831, 0.99998489283164882] 10 | Dl: [-0.35672321427959869, 0.10329057327105647, 0.0, 0.0 ] 11 | Dr: [-0.35616361614027825, 0.10277417306254764, 0.0, 0.0 ] 12 | R: [ 0.99998826205868707184, 0.00384110418665848177, 0.00295324626036013544, 13 | -0.00385477085117759063, 0.99998182398315793584, 0.00463599449971529177, 14 | -0.00293538524422376403, -0.00464732417028469996, 0.99998489283164893049 ] 15 | t: [ -0.06445734907815808823, 0.00033933833228201360, 0.00007254340573538721 ] 16 | images_subfolder_l: image_l/ 17 | images_subfolder_r: image_r/ 18 | -------------------------------------------------------------------------------- /config/scene_config.ini: -------------------------------------------------------------------------------- 1 | [Scene] 2 | hasGT = true 3 | hasComparison = true 4 | hasText = false 5 | hasAxes = true 6 | hasLegend = false 7 | hasHelp = false 8 | hasImg = true 9 | hasLines = false 10 | hasPoints = false 11 | isKitti = true 12 | sazim = -90.0 13 | selev = 0.0 14 | szoom = 200.0 15 | sfrust = 0.5 16 | -------------------------------------------------------------------------------- /config/scene_config_indoor.ini: -------------------------------------------------------------------------------- 1 | [Scene] 2 | hasGT = true 3 | hasComparison = true 4 | hasText = false 5 | hasAxes = true 6 | hasLegend = false 7 | hasHelp = false 8 | hasImg = true 9 | hasLines = true 10 | hasPoints = true 11 | isKitti = true 12 | szoom = 10.0 13 | sfrust = 0.06 14 | sbb = 1.0 15 | -------------------------------------------------------------------------------- /include/auxiliar.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | using namespace cv; 32 | using namespace line_descriptor; 33 | 34 | #include 35 | using namespace std; 36 | 37 | #include 38 | #include 39 | #include 40 | using namespace Eigen; 41 | 42 | typedef Matrix Vector6d; 43 | typedef Matrix Matrix6d; 44 | 45 | // Kinematics functions 46 | Matrix3d skew(Vector3d v); 47 | Matrix3d fast_skewexp(Vector3d v); 48 | Vector3d skewcoords(Matrix3d M); 49 | Matrix3d skewlog(Matrix3d M); 50 | MatrixXd kroen_product(MatrixXd A, MatrixXd B); 51 | Matrix3d v_logmap(VectorXd x); 52 | MatrixXd diagonalMatrix(MatrixXd M, unsigned int N); 53 | 54 | 55 | Matrix4d inverse_se3(Matrix4d T); 56 | Matrix4d expmap_se3(Vector6d x); 57 | Vector6d logmap_se3(Matrix4d T); 58 | Matrix6d adjoint_se3(Matrix4d T); 59 | Matrix6d uncTinv_se3(Matrix4d T, Matrix6d covT ); 60 | Matrix6d unccomp_se3(Matrix4d T1, Matrix6d covT1, Matrix6d covTinc ); 61 | Vector6d reverse_se3(Vector6d x); 62 | 63 | 64 | Vector3d logarithm_map_so3(Matrix3d R); 65 | MatrixXd der_logarithm_map(Matrix4d T); 66 | MatrixXd der_logarithm_map_appr(Matrix4d T, double delta); 67 | double diffManifoldError(Matrix4d T1, Matrix4d T2); 68 | bool is_finite(const MatrixXd x); 69 | bool is_nan(const MatrixXd x); 70 | double angDiff(double alpha, double beta); 71 | double angDiff_d(double alpha, double beta); 72 | 73 | // Auxiliar functions and structs for vectors 74 | double vector_stdv_mad( VectorXf residues); 75 | double vector_stdv_mad( vector residues); 76 | double vector_stdv_mad( vector residues, double &median); 77 | double vector_mean_mad(vector v, double stdv, double K); 78 | double vector_stdv_mad_nozero( vector residues); 79 | double vector_mean(vector v); 80 | double vector_stdv(vector v); 81 | double vector_stdv(vector v, double v_mean); 82 | void vector_mean_stdv_mad( vector residues, double &mean, double &stdv ); 83 | 84 | double robustWeightCauchy(double norm_res); 85 | 86 | struct compare_descriptor_by_NN_dist 87 | { 88 | inline bool operator()(const vector& a, const vector& b){ 89 | return ( a[0].distance < b[0].distance ); 90 | } 91 | }; 92 | 93 | struct compare_descriptor_by_NN12_dist 94 | { 95 | inline bool operator()(const vector& a, const vector& b){ 96 | return ( a[1].distance - a[0].distance > b[1].distance-b[0].distance ); 97 | } 98 | }; 99 | 100 | struct compare_descriptor_by_NN12_ratio 101 | { 102 | inline bool operator()(const vector& a, const vector& b){ 103 | return ( a[0].distance / a[1].distance > b[0].distance / b[1].distance ); 104 | } 105 | }; 106 | 107 | struct sort_descriptor_by_queryIdx 108 | { 109 | inline bool operator()(const vector& a, const vector& b){ 110 | return ( a[0].queryIdx < b[0].queryIdx ); 111 | } 112 | }; 113 | 114 | struct sort_descriptor_by_2nd_queryIdx 115 | { 116 | inline bool operator()(const vector& a, const vector& b){ 117 | return ( a[1].queryIdx < b[1].queryIdx ); 118 | } 119 | }; 120 | 121 | struct sort_descriptor_by_trainIdx 122 | { 123 | inline bool operator()(const vector& a, const vector& b){ 124 | return ( a[0].trainIdx < b[0].trainIdx ); 125 | } 126 | }; 127 | 128 | struct sort_confmat_by_score 129 | { 130 | inline bool operator()(const Vector2d& a, const Vector2d& b){ 131 | return ( a(1) > b(1) ); 132 | } 133 | }; 134 | 135 | struct sort_lines_by_response 136 | { 137 | inline bool operator()(const KeyLine& a, const KeyLine& b){ 138 | return ( a.response > b.response ); 139 | } 140 | }; 141 | 142 | struct sort_lines_by_length 143 | { 144 | inline bool operator()(const KeyLine& a, const KeyLine& b){ 145 | return ( a.lineLength > b.lineLength ); 146 | } 147 | }; 148 | 149 | struct sort_flines_by_length 150 | { 151 | inline bool operator()(const Vec4f& a, const Vec4f& b){ 152 | return ( sqrt(pow(a(0)-a(2),2.0)+pow(a(1)-a(3),2.0)) > sqrt(pow(b(0)-b(2),2.0)+pow(b(1)-b(3),2.0)) ); 153 | } 154 | }; 155 | -------------------------------------------------------------------------------- /include/config.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #pragma once 24 | #include 25 | 26 | class Config 27 | { 28 | 29 | public: 30 | 31 | Config(); 32 | ~Config(); 33 | 34 | static void loadFromFile( const std::string &config_file ); 35 | 36 | static Config& getInstance(); 37 | 38 | // Keyframe selection parameters (for SLAM, if any) 39 | static double& minEntropyRatio() { return getInstance().min_entropy_ratio; } 40 | static double& maxKFTDist() { return getInstance().max_kf_t_dist; } 41 | static double& maxKFRDist() { return getInstance().max_kf_r_dist; } 42 | 43 | // flags 44 | static bool& hasPoints() { return getInstance().has_points; } 45 | static bool& hasLines() { return getInstance().has_lines; } 46 | static bool& useFLDLines() { return getInstance().use_fld_lines; } 47 | static bool& lrInParallel() { return getInstance().lr_in_parallel; } 48 | static bool& plInParallel() { return getInstance().pl_in_parallel; } 49 | static bool& bestLRMatches() { return getInstance().best_lr_matches; } 50 | static bool& adaptativeFAST() { return getInstance().adaptative_fast; } 51 | static bool& useMotionModel() { return getInstance().use_motion_model; } 52 | 53 | // points detection and matching 54 | static int& matchingStrategy() { return getInstance().matching_strategy; } 55 | static int& matchingSWs() { return getInstance().matching_s_ws; } 56 | static int& matchingF2FWs() { return getInstance().matching_f2f_ws; } 57 | 58 | 59 | static int& orbNFeatures() { return getInstance().orb_nfeatures; } 60 | static double& orbScaleFactor() { return getInstance().orb_scale_factor; } 61 | static int& orbNLevels() { return getInstance().orb_nlevels; } 62 | static int& orbEdgeTh() { return getInstance().orb_edge_th; } 63 | static int& orbWtaK() { return getInstance().orb_wta_k; } 64 | static int& orbScore() { return getInstance().orb_score; } 65 | static int& orbPatchSize() { return getInstance().orb_patch_size; } 66 | static int& orbFastTh() { return getInstance().orb_fast_th; } 67 | static int& fastMinTh() { return getInstance().fast_min_th; } 68 | static int& fastMaxTh() { return getInstance().fast_max_th; } 69 | static int& fastIncTh() { return getInstance().fast_inc_th; } 70 | static int& fastFeatTh() { return getInstance().fast_feat_th; } 71 | static double& fastErrTh() { return getInstance().fast_err_th; } 72 | static double& maxDistEpip() { return getInstance().max_dist_epip; } 73 | static double& minDisp() { return getInstance().min_disp; } 74 | static double& minRatio12P() { return getInstance().min_ratio_12_p; } 75 | 76 | static double& rgbdMinDepth() { return getInstance().rgbd_min_depth; } 77 | static double& rgbdMaxDepth() { return getInstance().rgbd_max_depth; } 78 | 79 | 80 | // lines detection and matching 81 | static int& lsdNFeatures() { return getInstance().lsd_nfeatures; } 82 | static int& lsdRefine() { return getInstance().lsd_refine; } 83 | static double& lsdScale() { return getInstance().lsd_scale; } 84 | static double& lsdSigmaScale() { return getInstance().lsd_sigma_scale; } 85 | static double& lsdQuant() { return getInstance().lsd_quant; } 86 | static double& lsdAngTh() { return getInstance().lsd_ang_th; } 87 | static double& lsdLogEps() { return getInstance().lsd_log_eps; } 88 | static double& lsdDensityTh() { return getInstance().lsd_density_th; } 89 | static int& lsdNBins() { return getInstance().lsd_n_bins; } 90 | static double& lineHorizTh() { return getInstance().line_horiz_th; } 91 | static double& minLineLength() { return getInstance().min_line_length; } 92 | static double& minRatio12L() { return getInstance().min_ratio_12_l; } 93 | static double& stereoOverlapTh() { return getInstance().stereo_overlap_th; } 94 | static double& f2fOverlapTh() { return getInstance().f2f_overlap_th; } 95 | static double& lineSimTh() { return getInstance().line_sim_th; } 96 | static double& lsMinDispRatio() { return getInstance().ls_min_disp_ratio; } 97 | 98 | // optimization 99 | static double& homogTh() { return getInstance().homog_th; } 100 | static int& minFeatures() { return getInstance().min_features; } 101 | static int& maxIters() { return getInstance().max_iters; } 102 | static int& maxItersRef() { return getInstance().max_iters_ref; } 103 | static double& minError() { return getInstance().min_error; } 104 | static double& minErrorChange() { return getInstance().min_error_change; } 105 | static double& inlierK() { return getInstance().inlier_k; } 106 | 107 | // SLAM parameters (keyframe selection) 108 | double min_entropy_ratio; 109 | double max_kf_t_dist; 110 | double max_kf_r_dist; 111 | 112 | // flags 113 | bool has_points; 114 | bool has_lines; 115 | bool lr_in_parallel; 116 | bool pl_in_parallel; 117 | bool best_lr_matches; 118 | bool adaptative_fast; 119 | bool use_fld_lines; 120 | bool use_motion_model; 121 | 122 | // points detection and matching 123 | int matching_strategy; 124 | int matching_s_ws; 125 | int matching_f2f_ws; 126 | 127 | int orb_nfeatures; 128 | double orb_scale_factor; 129 | int orb_nlevels; 130 | int orb_edge_th; 131 | int orb_wta_k; 132 | int orb_score; 133 | int orb_patch_size; 134 | int orb_fast_th; 135 | 136 | int fast_min_th; 137 | int fast_max_th; 138 | int fast_inc_th; 139 | int fast_feat_th; 140 | double fast_err_th; 141 | 142 | double max_dist_epip; 143 | double min_disp; 144 | double min_ratio_12_p; 145 | double stereo_overlap_th; 146 | double f2f_overlap_th; 147 | double line_sim_th; 148 | 149 | double rgbd_min_depth; 150 | double rgbd_max_depth; 151 | 152 | // lines detection and matching 153 | int lsd_nfeatures; 154 | int lsd_refine; 155 | double lsd_scale; 156 | double lsd_sigma_scale; 157 | double lsd_quant; 158 | double lsd_ang_th; 159 | double lsd_log_eps; 160 | double lsd_density_th; 161 | int lsd_n_bins; 162 | double line_horiz_th; 163 | double min_line_length; 164 | double min_ratio_12_l; 165 | double ls_min_disp_ratio; 166 | 167 | // optimization 168 | double homog_th; 169 | int min_features; 170 | int max_iters; 171 | int max_iters_ref; 172 | double min_error; 173 | double min_error_change; 174 | double inlier_k; 175 | 176 | }; 177 | 178 | -------------------------------------------------------------------------------- /include/dataset.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #pragma once 24 | 25 | //STL 26 | #include 27 | #include 28 | 29 | //OpenCV 30 | #include 31 | 32 | #include "pinholeStereoCamera.h" 33 | 34 | namespace StVO { 35 | 36 | class Dataset { 37 | public: 38 | 39 | // Constructor 40 | Dataset(const std::string &dataset_path, const PinholeStereoCamera &cam, int offset = 0, int nmax = 0, int step = 1); 41 | 42 | // Destrcutor 43 | virtual ~Dataset(); 44 | 45 | // Reads next frame in the dataset sequence, returning true if image was successfully loaded 46 | bool nextFrame(cv::Mat &img_l, cv::Mat &img_r); 47 | 48 | // Returns if there are images still available in the sequence 49 | inline bool hasNext(); 50 | 51 | private: 52 | 53 | std::list images_l, images_r; 54 | const PinholeStereoCamera &cam; 55 | }; 56 | 57 | } // namespace StVO 58 | 59 | -------------------------------------------------------------------------------- /include/gridStructure.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #pragma once 24 | 25 | //STL 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace StVO { 32 | 33 | void getLineCoords(double x1, double y1, double x2, double y2, std::list> &line_coords); 34 | 35 | struct GridWindow { 36 | std::pair width, height; 37 | }; 38 | 39 | class GridStructure { 40 | public: 41 | 42 | int rows, cols; 43 | 44 | GridStructure(int rows, int cols); 45 | 46 | ~GridStructure(); 47 | 48 | std::list& at(int x, int y); 49 | 50 | void get(int x, int y, const GridWindow &w, std::unordered_set &indices) const; 51 | 52 | void clear(); 53 | 54 | private: 55 | 56 | std::vector>> grid; 57 | std::list out_of_bounds; 58 | }; 59 | 60 | } // namespace StVO 61 | -------------------------------------------------------------------------------- /include/lineIterator.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #pragma once 24 | 25 | //STL 26 | #include 27 | 28 | namespace StVO { 29 | 30 | class LineIterator { 31 | public: 32 | 33 | LineIterator(const double x1_, const double y1_, const double x2_, const double y2_); 34 | 35 | bool getNext(std::pair &pixel); 36 | 37 | private: 38 | 39 | const bool steep; 40 | double x1, y1, x2, y2; 41 | 42 | double dx, dy, error; 43 | 44 | int maxX, ystep; 45 | int y, x; 46 | }; 47 | 48 | } // namespace StVO 49 | -------------------------------------------------------------------------------- /include/matching.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #pragma once 24 | 25 | //STL 26 | #include 27 | #include 28 | 29 | //OpenCV 30 | #include 31 | 32 | #include "gridStructure.h" 33 | 34 | namespace StVO { 35 | 36 | typedef std::pair point_2d; 37 | typedef std::pair line_2d; 38 | 39 | inline double dot(const std::pair &a, const std::pair &b) { 40 | return (a.first*b.first + a.second*b.second); 41 | } 42 | 43 | inline void normalize(std::pair &v) { 44 | double magnitude = std::sqrt(dot(v, v)); 45 | 46 | v.first /= magnitude; 47 | v.second /= magnitude; 48 | } 49 | 50 | int matchNNR(const cv::Mat &desc1, const cv::Mat &desc2, float nnr, std::vector &matches_12); 51 | 52 | int match(const cv::Mat &desc1, const cv::Mat &desc2, float nnr, std::vector &matches_12); 53 | 54 | int distance(const cv::Mat &a, const cv::Mat &b); 55 | 56 | //Points 57 | int matchGrid(const std::vector &points1, const cv::Mat &desc1, const GridStructure &grid, const cv::Mat &desc2, const GridWindow &w, std::vector &matches_12); 58 | 59 | //Lines 60 | int matchGrid(const std::vector &lines1, const cv::Mat &desc1, const GridStructure &grid, const cv::Mat &desc2, const std::vector> &directions2, const GridWindow &w, std::vector &matches_12); 61 | 62 | } // namesapce StVO 63 | -------------------------------------------------------------------------------- /include/pinholeStereoCamera.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #pragma once 24 | 25 | using namespace std; 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | using namespace cv; 32 | using namespace line_descriptor; 33 | 34 | #include 35 | using namespace Eigen; 36 | 37 | // Pinhole model for a Stereo Camera in an ideal configuration (horizontal) 38 | class PinholeStereoCamera 39 | { 40 | 41 | private: 42 | int width, height; 43 | double fx, fy; 44 | double cx, cy; 45 | double b; 46 | Matrix3d K; 47 | bool dist; 48 | Matrix d; 49 | Mat Kl, Kr, Dl, Dr, Rl, Rr, Pl, Pr, R, t, Q; 50 | Mat undistmap1l, undistmap2l, undistmap1r, undistmap2r; 51 | 52 | public: 53 | 54 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 55 | 56 | PinholeStereoCamera(const std::string ¶ms_file); 57 | 58 | PinholeStereoCamera( int width_, int height_, double fx_, double fy_, double cx_, double cy_, double b_, 59 | double d0 = 0.0, double d1 = 0.0, double d2 = 0.0, double d3 = 0.0, double d4 = 0.0); 60 | PinholeStereoCamera( int width_, int height_, double fx_, double fy_, double cx_, double cy_, double b_, Mat Rl, Mat Rr, 61 | double d0 = 0.0, double d1 = 0.0, double d2 = 0.0, double d3 = 0.0, double d4 = 0.0); 62 | 63 | //PinholeStereoCamera(int width_, int height_, double b_, Mat Kl_, Mat Kr_, Mat Rl_, Mat Rr_, Mat Dl_, Mat Dr_, bool equi ); 64 | PinholeStereoCamera(int width_, int height_, double b_, Mat Kl_, Mat Kr_, Mat R_, Mat t_, Mat Dl_, Mat Dr_, bool equi ); 65 | 66 | ~PinholeStereoCamera(); 67 | 68 | // Image rectification 69 | void rectifyImage( const Mat& img_src, Mat& img_rec) const; 70 | void rectifyImagesLR( const Mat& img_src_l, Mat& img_rec_l, const Mat& img_src_r, Mat& img_rec_r ) const; 71 | 72 | // Proyection and Back-projection 73 | Vector3d backProjection_unit(const double &u, const double &v, const double &disp, double &depth); 74 | Vector3d backProjection(const double &u, const double &v, const double &disp); 75 | Vector2d projection(const Vector3d &P); 76 | Vector3d projectionNH(Vector3d P); 77 | Vector2d nonHomogeneous( Vector3d x); 78 | 79 | // Getters 80 | inline const int getWidth() const { return width; }; 81 | inline const int getHeight() const { return height; }; 82 | inline const Matrix3d& getK() const { return K; }; 83 | inline const double getB() const { return b; }; 84 | inline const Matrix getD() const { return d; }; 85 | inline const double getFx() const { return fx; }; 86 | inline const double getFy() const { return fy; }; 87 | inline const double getCx() const { return cx; }; 88 | inline const double getCy() const { return cy; }; 89 | 90 | }; 91 | 92 | -------------------------------------------------------------------------------- /include/sceneRepresentation.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #include 24 | using namespace std; 25 | 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | using namespace mrpt; 36 | using namespace mrpt::gui; 37 | using namespace mrpt::poses; 38 | using namespace mrpt::utils; 39 | using namespace mrpt::math; 40 | using namespace mrpt::opengl; 41 | using namespace mrpt::maps; 42 | 43 | #include 44 | using namespace cv; 45 | 46 | #include 47 | using namespace Eigen; 48 | 49 | #include 50 | using namespace StVO; 51 | 52 | class sceneRepresentation{ 53 | 54 | public: 55 | 56 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 57 | 58 | sceneRepresentation(); 59 | sceneRepresentation(string configFile); 60 | ~sceneRepresentation(); 61 | void initialize3DScene(Matrix4d x_0); 62 | // void initialize3DSceneLines(Matrix4d x_0); 63 | // void initialize3DSceneGT(Matrix4d x_0); 64 | 65 | void initializeScene(Matrix4d x_0, bool has_gt); 66 | // void initializeScene(Matrix4d x_0, Matrix4d x_0gt); 67 | 68 | bool updateScene(); 69 | bool updateScene(list matched_pt, list matched_ls); 70 | void plotPointsCovariances(); 71 | void plotLinesCovariances(); 72 | 73 | void setText(int frame_, float time_, int nPoints_, int nPointsH_, int nLines_, int nLinesH_); 74 | void setCov(MatrixXd cov_); 75 | void setPose(Matrix4d x_); 76 | void setGT(Matrix4d xgt_); 77 | void setComparison(Matrix4d xcomp_); 78 | void setImage(const Mat &image_); 79 | void setImage(const string &image_); 80 | void setLegend(); 81 | void setHelp(); 82 | void setPoints(CMatrixFloat pData_); 83 | void setLines(CMatrixFloat lData_); 84 | void setStereoCalibration(Matrix3d K_, float b_); 85 | void setKF(); 86 | void setKF(Matrix4d Tfw); 87 | 88 | bool waitUntilClose(); 89 | bool isOpen(); 90 | bool getYPR(float &yaw, float &pitch, float &roll); 91 | bool getPose(Matrix4d &T); 92 | 93 | private: 94 | 95 | CMatrixDouble getPoseFormat(Matrix4d T); 96 | CMatrixDouble33 getCovFormat(MatrixXd cov_); 97 | CPose3D getPoseXYZ(VectorXd x); 98 | 99 | CDisplayWindow3D* win; 100 | COpenGLScenePtr theScene; 101 | COpenGLViewportPtr image, legend, help; 102 | opengl::CSetOfObjectsPtr bbObj, bbObj1, srefObj, srefObj1, gtObj, srefObjGT, elliObjL, elliObjP; 103 | opengl::CEllipsoidPtr elliObj; 104 | opengl::CSetOfLinesPtr lineObj; 105 | opengl::CPointCloudPtr pointObj; 106 | 107 | //CPointsMapPtr pointsObj; 108 | 109 | opengl::CFrustumPtr frustObj, frustObj1; 110 | opengl::CAxisPtr axesObj; 111 | 112 | 113 | float sbb, saxis, srad, sref, sline, sfreq, szoom, selli, selev, sazim, sfrust, slinef; 114 | CVectorDouble v_aux, v_aux_, v_aux1, v_aux1_, v_auxgt, gt_aux_, v_auxgt_; 115 | CPose3D pose, pose_0, pose_gt, pose_ini, ellPose, pose1, change, frustumL_, frustumR_; 116 | Matrix4d x_ini; 117 | mrptKeyModifier kmods; 118 | int key; 119 | CMatrixDouble33 cov3D; 120 | bool hasCamFix, hasText, hasCov, hasGT, hasChange, hasImg, hasLines, hasPoints, hasFrustum, hasComparison, hasLegend, hasHelp, hasAxes, hasTraj, isKitti; 121 | 122 | Matrix4d x, xgt, xcomp; 123 | MatrixXd cov, W; 124 | unsigned int frame, nPoints, nPointsH, nLines, nLinesH; 125 | float time; 126 | string img, img_legend, img_help; 127 | CMatrixFloat lData, pData; 128 | CImage img_mrpt_legend, img_mrpt_image, img_mrpt_help; 129 | 130 | float b, sigmaP, sigmaL, f, cx, cy, bsigmaL, bsigmaP; 131 | 132 | cv::Size img_sz; 133 | 134 | }; 135 | 136 | -------------------------------------------------------------------------------- /include/stereoFeatures.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #pragma once 24 | #include 25 | #include 26 | using namespace Eigen; 27 | 28 | namespace StVO{ 29 | 30 | class PointFeature 31 | { 32 | 33 | public: 34 | 35 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 36 | 37 | PointFeature( Vector3d P_, Vector2d pl_obs_); 38 | PointFeature( Vector2d pl_, double disp_, Vector3d P_ ); 39 | PointFeature( Vector2d pl_, double disp_, Vector3d P_, int idx_ ); 40 | PointFeature( Vector2d pl_, double disp_, Vector3d P_, int idx_, int level ); 41 | PointFeature( Vector2d pl_, double disp_, Vector3d P_, int idx_, int level, Matrix3d covP_an_ ); 42 | PointFeature( Vector2d pl_, double disp_, Vector3d P_, Vector2d pl_obs_ ); 43 | PointFeature( Vector2d pl_, double disp_, Vector3d P_, Vector2d pl_obs_, 44 | int idx_, int level_, double sigma2_, Matrix3d covP_an_, bool inlier_ ); 45 | ~PointFeature(){}; 46 | 47 | PointFeature* safeCopy(); 48 | 49 | int idx; 50 | Vector2d pl, pl_obs; 51 | double disp; 52 | Vector3d P; 53 | bool inlier; 54 | int level; 55 | double sigma2 = 1.0; 56 | Matrix3d covP_an; 57 | 58 | }; 59 | 60 | class LineFeature 61 | { 62 | 63 | public: 64 | 65 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 66 | 67 | LineFeature( Vector3d sP_, Vector3d eP_, Vector3d le_obs_); 68 | 69 | LineFeature( Vector3d sP_, Vector3d eP_, Vector3d le_obs_, Vector2d spl_obs_, Vector2d epl_obs_); 70 | 71 | LineFeature( Vector2d spl_, double sdisp_, Vector3d sP_, 72 | Vector2d epl_, double edisp_, Vector3d eP_, 73 | Vector3d le_, int idx_); 74 | 75 | LineFeature( Vector2d spl_, double sdisp_, Vector3d sP_, 76 | Vector2d epl_, double edisp_, Vector3d eP_, 77 | Vector3d le_, double angle_, int idx_); 78 | 79 | LineFeature( Vector2d spl_, double sdisp_, Vector3d sP_, 80 | Vector2d epl_, double edisp_, Vector3d eP_, 81 | Vector3d le_, double angle_, int idx_, int level); 82 | 83 | /*LineFeature( Vector2d spl_, double sdisp_, Vector3d sP_, 84 | Vector2d epl_, double edisp_, Vector3d eP_, 85 | Vector3d le_, double angle_, int idx_, int level, Matrix3d covS_an_, Matrix3d covE_an_);*/ 86 | 87 | LineFeature( Vector2d spl_, double sdisp_, Vector3d sP_, 88 | Vector2d epl_, double edisp_, Vector3d eP_, Vector3d le_); 89 | 90 | LineFeature( Vector2d spl_, double sdisp_, Vector3d sP_, 91 | Vector2d epl_, double edisp_, Vector3d eP_, 92 | Vector3d le_, Vector3d le_obs_); 93 | 94 | /*LineFeature( Vector2d spl_, double sdisp_, Vector3d sP_, 95 | Vector2d epl_, double edisp_, Vector3d eP_, 96 | Vector3d le_, double angle_, int idx_, int level, Vector2d spr_, Vector2d epr_);*/ 97 | 98 | LineFeature( Vector2d spl_, double sdisp_, Vector3d sP_, Vector2d spl_obs_, double sdisp_obs_, 99 | Vector2d epl_, double edisp_, Vector3d eP_, Vector2d epl_obs_, double edisp_obs_, 100 | Vector3d le_, Vector3d le_obs_, double angle_, int idx_, int level_, bool inlier_, double sigma2_, 101 | Matrix3d covE_an_, Matrix3d covS_an_); 102 | 103 | ~LineFeature(){}; 104 | 105 | LineFeature* safeCopy(); 106 | 107 | int idx; 108 | Vector2d spl,epl, spl_obs, epl_obs; 109 | double sdisp, edisp, angle, sdisp_obs, edisp_obs; 110 | Vector3d sP,eP; 111 | Vector3d le, le_obs; 112 | bool inlier; 113 | 114 | int level; 115 | double sigma2 = 1.0; 116 | 117 | Matrix3d covE_an, covS_an; 118 | 119 | 120 | 121 | }; 122 | 123 | } 124 | -------------------------------------------------------------------------------- /include/stereoFrame.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | using namespace std; 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | using namespace cv; 41 | using namespace line_descriptor; 42 | 43 | #include 44 | using namespace Eigen; 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | #define GRID_ROWS 48 52 | #define GRID_COLS 64 53 | 54 | typedef Matrix Matrix6d; 55 | typedef Matrix Vector6d; 56 | 57 | namespace StVO{ 58 | 59 | class StereoFrame 60 | { 61 | 62 | public: 63 | 64 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 65 | 66 | StereoFrame(); 67 | StereoFrame(const Mat img_l_, const Mat img_r_, const int idx_, PinholeStereoCamera *cam_ ); 68 | ~StereoFrame(); 69 | 70 | void extractStereoFeatures( double llength_th, int fast_th = 20 ); 71 | void extractRGBDFeatures( double llength_th, int fast_th = 20 ); 72 | 73 | void detectStereoPoints(int fast_th = 20); 74 | void detectPointFeatures( Mat img, vector &points, Mat &pdesc, int fast_th = 20 ); 75 | void matchStereoPoints(vector points_l, vector points_r, Mat &pdesc_l_, Mat pdesc_r, bool initial = false ); 76 | void matchPointFeatures(BFMatcher* bfm, Mat pdesc_1, Mat pdesc_2, vector> &pmatches_12); 77 | 78 | void detectStereoLineSegments(double llength_th); 79 | void detectLineFeatures( Mat img, vector &lines, Mat &ldesc, double min_line_length ); 80 | void matchStereoLines(vector lines_l, vector lines_r, Mat &ldesc_l_, Mat ldesc_r, bool initial = false ); 81 | void matchLineFeatures(BFMatcher* bfm, Mat ldesc_1, Mat ldesc_2, vector> &lmatches_12 ); 82 | void filterLineSegmentDisparity(Vector2d spl, Vector2d epl, Vector2d spr, Vector2d epr , double &disp_s, double &disp_e); 83 | void filterLineSegmentDisparity(double &disp_s, double &disp_e); 84 | 85 | double lineSegmentOverlapStereo( double spl_obs, double epl_obs, double spl_proj, double epl_proj ); 86 | double lineSegmentOverlap( Vector2d spl_obs, Vector2d epl_obs, Vector2d spl_proj, Vector2d epl_proj ); 87 | void pointDescriptorMAD( const vector> matches, double &nn_mad, double &nn12_mad ); 88 | void lineDescriptorMAD( const vector> matches, double &nn_mad, double &nn12_mad ); 89 | 90 | Mat plotStereoFrame(); 91 | 92 | int frame_idx; 93 | Mat img_l, img_r; 94 | Matrix4d Tfw; 95 | Matrix4d DT; 96 | 97 | Matrix6d Tfw_cov; 98 | Vector6d Tfw_cov_eig; 99 | double entropy_first; 100 | 101 | Matrix6d DT_cov; 102 | Vector6d DT_cov_eig; 103 | double err_norm; 104 | 105 | vector< PointFeature* > stereo_pt; 106 | vector< LineFeature* > stereo_ls; 107 | 108 | vector points_l, points_r; 109 | vector lines_l, lines_r; 110 | Mat pdesc_l, pdesc_r, ldesc_l, ldesc_r; 111 | 112 | PinholeStereoCamera *cam; 113 | 114 | double inv_width, inv_height; // grid cell 115 | }; 116 | 117 | } 118 | -------------------------------------------------------------------------------- /include/stereoFrameHandler.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #pragma once 24 | #include 25 | #include 26 | 27 | typedef Matrix Matrix6d; 28 | typedef Matrix Vector6d; 29 | 30 | class StereoFrame; 31 | 32 | namespace StVO{ 33 | 34 | class StereoFrameHandler 35 | { 36 | 37 | public: 38 | 39 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 40 | 41 | StereoFrameHandler( PinholeStereoCamera* cam_ ); 42 | ~StereoFrameHandler(); 43 | 44 | void initialize( const Mat img_l_, const Mat img_r_, const int idx_); 45 | void insertStereoPair(const Mat img_l_, const Mat img_r_, const int idx_); 46 | void updateFrame(); 47 | 48 | void f2fTracking(); 49 | void matchF2FPoints(); 50 | void matchF2FLines(); 51 | double f2fLineSegmentOverlap( Vector2d spl_obs, Vector2d epl_obs, Vector2d spl_proj, Vector2d epl_proj ); 52 | 53 | bool isGoodSolution( Matrix4d DT, Matrix6d DTcov, double err ); 54 | void optimizePose(); 55 | void resetOutliers(); 56 | void setAsOutliers(); 57 | 58 | void plotStereoFrameProjerr(Matrix4d DT, int iter); 59 | void plotLeftPair(); 60 | 61 | // adaptative fast 62 | int orb_fast_th; 63 | double llength_th; 64 | 65 | // slam-specific functions 66 | bool needNewKF(); 67 | void currFrameIsKF(); 68 | 69 | //list< boost::shared_ptr > matched_pt; 70 | //list< boost::shared_ptr > matched_ls; 71 | list< PointFeature* > matched_pt; 72 | list< LineFeature* > matched_ls; 73 | 74 | StereoFrame* prev_frame; 75 | StereoFrame* curr_frame; 76 | PinholeStereoCamera *cam; 77 | 78 | int n_inliers, n_inliers_pt, n_inliers_ls; 79 | 80 | // slam-specific variables 81 | bool prev_f_iskf; 82 | double entropy_first_prevKF; 83 | Matrix4d T_prevKF; 84 | Matrix6d cov_prevKF_currF; 85 | int N_prevKF_currF; 86 | 87 | // bool recurse; 88 | 89 | private: 90 | 91 | void prefilterOutliers( Matrix4d DT ); 92 | void removeOutliers( Matrix4d DT ); 93 | void gaussNewtonOptimization(Matrix4d &DT, Matrix6d &DT_cov, double &err_, int max_iters); 94 | void gaussNewtonOptimizationRobust(Matrix4d &DT, Matrix6d &DT_cov, double &err_, int max_iters); 95 | void gaussNewtonOptimizationRobustDebug(Matrix4d &DT, Matrix6d &DT_cov, double &err_, int max_iters); 96 | void levenbergMarquardtOptimization(Matrix4d &DT, Matrix6d &DT_cov, double &err_, int max_iters); 97 | void optimizeFunctions(Matrix4d DT, Matrix6d &H, Vector6d &g, double &e); 98 | void optimizeFunctionsRobust(Matrix4d DT, Matrix6d &H, Vector6d &g, double &e); 99 | void optimizePoseDebug(); 100 | 101 | }; 102 | 103 | } 104 | -------------------------------------------------------------------------------- /include/timer.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #pragma once 24 | 25 | //STL 26 | #include 27 | 28 | namespace StVO { 29 | 30 | class Timer { 31 | public: 32 | 33 | static constexpr double SECONDS = 1e-9; 34 | static constexpr double MILLISECONDS = 1e-6; 35 | static constexpr double NANOSECONDS = 1.0; 36 | 37 | Timer(double scale = MILLISECONDS); 38 | virtual ~Timer(); 39 | 40 | void start(); 41 | 42 | double stop(); 43 | 44 | private: 45 | 46 | std::chrono::high_resolution_clock::time_point start_t; 47 | bool started; 48 | double scale; 49 | }; 50 | 51 | } // namespace StVO 52 | -------------------------------------------------------------------------------- /src/auxiliar.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #include 24 | 25 | #define PI std::acos(-1.0) 26 | 27 | /* Kinematics functions */ 28 | 29 | Matrix3d skew(Vector3d v){ 30 | 31 | Matrix3d skew; 32 | 33 | skew(0,0) = 0; skew(1,1) = 0; skew(2,2) = 0; 34 | 35 | skew(0,1) = -v(2); 36 | skew(0,2) = v(1); 37 | skew(1,2) = -v(0); 38 | 39 | skew(1,0) = v(2); 40 | skew(2,0) = -v(1); 41 | skew(2,1) = v(0); 42 | 43 | return skew; 44 | } 45 | 46 | Matrix3d fast_skewexp(Vector3d v){ 47 | Matrix3d M, s, I = Matrix3d::Identity(); 48 | double theta = v.norm(); 49 | if(theta==0.f) 50 | M = I; 51 | else{ 52 | s = skew(v)/theta; 53 | M << I + s * sin(theta) + s * s * (1.f-cos(theta)); 54 | } 55 | return M; 56 | } 57 | 58 | Vector3d skewcoords(Matrix3d M){ 59 | Vector3d skew; 60 | skew << M(2,1), M(0,2), M(1,0); 61 | return skew; 62 | } 63 | 64 | Matrix3d skewlog(Matrix3d M){ 65 | Matrix3d skew; 66 | double val = (M.trace() - 1.f)/2.f; 67 | if(val > 1.f) 68 | val = 1.f; 69 | else if (val < -1.f) 70 | val = -1.f; 71 | double theta = acos(val); 72 | if(theta == 0.f) 73 | skew << 0,0,0,0,0,0,0,0,0; 74 | else 75 | skew << (M-M.transpose())/(2.f*sin(theta))*theta; 76 | return skew; 77 | } 78 | 79 | MatrixXd kroen_product(MatrixXd A, MatrixXd B){ 80 | unsigned int Ar = A.rows(), Ac = A.cols(), Br = B.rows(), Bc = B.cols(); 81 | MatrixXd AB(Ar*Br,Ac*Bc); 82 | for (unsigned int i=0; i0.00001) 97 | V << I + ((1-cos(theta))/theta2)*W + ((theta-sin(theta))/theta3)*W*W; 98 | else 99 | V << I; 100 | return V; 101 | } 102 | 103 | MatrixXd diagonalMatrix(MatrixXd M, unsigned int N){ 104 | MatrixXd A = MatrixXd::Zero(N,N); 105 | for(unsigned int i = 0; i < N; i++ ){ 106 | A(i,i) = M(i,i); 107 | } 108 | return A; 109 | } 110 | 111 | 112 | 113 | Matrix4d inverse_se3(Matrix4d T){ 114 | Matrix4d Tinv = Matrix4d::Identity(); 115 | Matrix3d R; 116 | Vector3d t; 117 | t = T.block(0,3,3,1); 118 | R = T.block(0,0,3,3); 119 | Tinv.block(0,0,3,3) = R.transpose(); 120 | Tinv.block(0,3,3,1) = -R.transpose() * t; 121 | return Tinv; 122 | } 123 | 124 | Matrix4d expmap_se3(Vector6d x){ 125 | Matrix3d R, V, s, I = Matrix3d::Identity(); 126 | Vector3d t, w; 127 | Matrix4d T = Matrix4d::Identity(); 128 | w = x.tail(3); 129 | t = x.head(3); 130 | double theta = w.norm(); 131 | if( theta < 0.000001 ) 132 | R = I; 133 | else{ 134 | s = skew(w)/theta; 135 | R = I + s * sin(theta) + s * s * (1.0f-cos(theta)); 136 | V = I + s * (1.0f - cos(theta)) / theta + s * s * (theta - sin(theta)) / theta; 137 | t = V * t; 138 | } 139 | T.block(0,0,3,4) << R, t; 140 | return T; 141 | } 142 | 143 | Vector6d logmap_se3(Matrix4d T){ 144 | Matrix3d R, Id3 = Matrix3d::Identity(); 145 | Vector3d Vt, t, w; 146 | Matrix3d V = Matrix3d::Identity(), w_hat = Matrix3d::Zero(); 147 | Vector6d x; 148 | Vt << T(0,3), T(1,3), T(2,3); 149 | w << 0.f, 0.f, 0.f; 150 | R = T.block(0,0,3,3); 151 | double cosine = (R.trace() - 1.f)/2.f; 152 | if(cosine > 1.f) 153 | cosine = 1.f; 154 | else if (cosine < -1.f) 155 | cosine = -1.f; 156 | double sine = sqrt(1.0-cosine*cosine); 157 | if(sine > 1.f) 158 | sine = 1.f; 159 | else if (sine < -1.f) 160 | sine = -1.f; 161 | double theta = acos(cosine); 162 | if( theta > 0.000001 ){ 163 | w_hat = theta*(R-R.transpose())/(2.f*sine); 164 | w = skewcoords(w_hat); 165 | Matrix3d s; 166 | s = skew(w) / theta; 167 | V = Id3 + s * (1.f-cosine) / theta + s * s * (theta - sine) / theta; 168 | } 169 | t = V.inverse() * Vt; 170 | x.head(3) = t; 171 | x.tail(3) = w; 172 | return x; 173 | } 174 | 175 | Matrix6d adjoint_se3(Matrix4d T){ 176 | Matrix6d AdjT = Matrix6d::Zero(); 177 | Matrix3d R = T.block(0,0,3,3); 178 | AdjT.block(0,0,3,3) = R; 179 | AdjT.block(0,3,3,3) = skew( T.block(0,3,3,1) ) * R ; 180 | AdjT.block(3,3,3,3) = R; 181 | return AdjT; 182 | } 183 | 184 | Matrix6d uncTinv_se3(Matrix4d T, Matrix6d covT ){ 185 | Matrix6d covTinv = Matrix6d::Zero(); 186 | Matrix6d adjTinv; 187 | adjTinv = adjoint_se3( inverse_se3(T) ); 188 | covTinv = adjTinv * covT * adjTinv.transpose(); 189 | return covTinv; 190 | } 191 | 192 | Matrix6d unccomp_se3(Matrix4d T1, Matrix6d covT1, Matrix6d covTinc ){ 193 | Matrix6d covT2 ; // covariance of T2 = T1 * inverse(Tinc) 194 | Matrix6d adjT1 = adjoint_se3(T1); 195 | covT2 = covT1 + adjT1 * covTinc * adjT1.transpose(); 196 | return covT2; 197 | } 198 | 199 | Vector6d reverse_se3(Vector6d x){ 200 | Vector6d x_out; 201 | x_out.head(3) = x.tail(3); 202 | x_out.tail(3) = x.head(3); 203 | return x_out; 204 | } 205 | 206 | 207 | 208 | Vector3d logarithm_map_so3(Matrix3d R){ 209 | Matrix3d Id3 = Matrix3d::Identity(); 210 | Vector3d w; 211 | Matrix3d V = Matrix3d::Identity(), w_hat = Matrix3d::Zero(); 212 | w << 0.f, 0.f, 0.f; 213 | double cosine = (R.trace() - 1.f)/2.f; 214 | if(cosine > 1.f) 215 | cosine = 1.f; 216 | else if (cosine < -1.f) 217 | cosine = -1.f; 218 | double sine = sqrt(1.0-cosine*cosine); 219 | if(sine > 1.f) 220 | sine = 1.f; 221 | else if (sine < -1.f) 222 | sine = -1.f; 223 | double theta = acos(cosine); 224 | if( theta > 0.000001 ){ 225 | w_hat << theta*(R-R.transpose()) / (2.f*sine); 226 | w = skewcoords(w_hat); 227 | } 228 | return w; 229 | } 230 | 231 | MatrixXd der_logarithm_map(Matrix4d T) 232 | { 233 | 234 | MatrixXd dlogT_dT = MatrixXd::Zero(6,12); 235 | 236 | // Approximate derivative of the logarithm_map wrt the transformation matrix 237 | Matrix3d L1 = Matrix3d::Zero(); 238 | Matrix3d L2 = Matrix3d::Zero(); 239 | Matrix3d L3 = Matrix3d::Zero(); 240 | Matrix3d Vinv = Matrix3d::Identity(); 241 | Vector6d x = logmap_se3(T); 242 | 243 | // estimates the cosine, sine, and theta 244 | double b; 245 | double cos_ = 0.5 * (T.block(0,0,3,3).trace() - 1.0 ); 246 | if(cos_ > 1.f) 247 | cos_ = 1.f; 248 | else if (cos_ < -1.f) 249 | cos_ = -1.f; 250 | double theta = acos(cos_); 251 | double theta2 = theta*theta; 252 | double sin_ = sin(theta); 253 | double cot_ = 1.0 / tan( 0.5*theta ); 254 | double csc2_ = pow( 1.0/sin(0.5*theta) ,2); 255 | 256 | // if the angle is small... 257 | if( cos_ > 0.9999 ) 258 | { 259 | b = 0.5; 260 | L1(1,2) = -b; 261 | L1(2,1) = b; 262 | L2(0,2) = b; 263 | L2(2,0) = -b; 264 | L3(0,1) = -b; 265 | L3(1,0) = b; 266 | // form the full derivative 267 | dlogT_dT.block(3,0,3,3) = L1; 268 | dlogT_dT.block(3,3,3,3) = L2; 269 | dlogT_dT.block(3,6,3,3) = L3; 270 | dlogT_dT.block(0,9,3,3) = Vinv; 271 | } 272 | // if not... 273 | else 274 | { 275 | // rotation part 276 | double k; 277 | Vector3d a; 278 | a(0) = T(2,1) - T(1,2); 279 | a(1) = T(0,2) - T(2,0); 280 | a(2) = T(1,0) - T(0,1); 281 | k = ( theta * cos_ - sin_ ) / ( 4 * pow(sin_,3) ); 282 | a = k * a; 283 | L1.block(0,0,3,1) = a; 284 | L2.block(0,1,3,1) = a; 285 | L3.block(0,2,3,1) = a; 286 | // translation part 287 | Matrix3d w_skew = skew( x.tail(3) ); 288 | Vinv += w_skew * (1.f-cos_) / theta2 + w_skew * w_skew * (theta - sin_) / pow(theta,3); 289 | Vinv = Vinv.inverse().eval(); 290 | // dVinv_dR 291 | Vector3d t; 292 | Matrix3d B, skew_t; 293 | MatrixXd dVinv_dR(3,9); 294 | t = T.block(0,3,3,1); 295 | skew_t = skew( t ); 296 | // - form a 297 | a = (theta*cos_-sin_)/(8.0*pow(sin_,3)) * w_skew * t 298 | + ( (theta*sin_-theta2*cos_)*(0.5*theta*cot_-1.0) - theta*sin_*(0.25*theta*cot_+0.125*theta2*csc2_-1.0))/(4.0*theta2*pow(sin_,4)) * w_skew * w_skew * t; 299 | // - form B 300 | Vector3d w; 301 | Matrix3d dw_dR; 302 | w = x.tail(3); 303 | dw_dR.row(0) << -w(1)*t(1)-w(2)*t(2), 2.0*w(1)*t(0)-w(0)*t(1), 2.0*w(2)*t(0)-w(0)*t(2); 304 | dw_dR.row(1) << -w(1)*t(0)+2.0*w(0)*t(1), -w(0)*t(0)-w(2)*t(2), 2.0*w(2)*t(1)-w(1)*t(2); 305 | dw_dR.row(2) << -w(2)*t(0)+2.0*w(0)*t(2), -w(2)*t(1)+2.0*w(1)*t(2), -w(0)*t(0)-w(1)*t(1); 306 | B = -0.5*theta*skew_t/sin_ - (theta*cot_-2.0)*dw_dR/(8.0*pow(sin_,2)); 307 | // - form dVinv_dR 308 | dVinv_dR.col(0) = a; 309 | dVinv_dR.col(1) = -B.col(2); 310 | dVinv_dR.col(2) = B.col(1); 311 | dVinv_dR.col(3) = B.col(2); 312 | dVinv_dR.col(4) = a; 313 | dVinv_dR.col(5) = -B.col(0); 314 | dVinv_dR.col(6) = -B.col(1); 315 | dVinv_dR.col(7) = B.col(0); 316 | dVinv_dR.col(8) = a; 317 | // form the full derivative 318 | dlogT_dT.block(3,0,3,3) = L1; 319 | dlogT_dT.block(3,3,3,3) = L2; 320 | dlogT_dT.block(3,6,3,3) = L3; 321 | dlogT_dT.block(0,9,3,3) = Vinv; 322 | dlogT_dT.block(0,0,3,9) = dVinv_dR; 323 | } 324 | 325 | return dlogT_dT; 326 | 327 | } 328 | 329 | MatrixXd der_logarithm_map_appr(Matrix4d T, double delta) 330 | { 331 | 332 | MatrixXd dlogT_dT = MatrixXd::Zero(6,12); 333 | // Approximate derivative of the logarithm_map wrt the transformation matrix 334 | int k = 0; 335 | for( int j = 0; j < 4; j++) 336 | { 337 | for(int i = 0; i < 3; i++) 338 | { 339 | Matrix4d Taux = T; 340 | Taux(i,j) += delta; 341 | dlogT_dT.col(k) = ( logmap_se3(Taux)-logmap_se3(T) ) / delta; 342 | k++; 343 | } 344 | } 345 | return dlogT_dT; 346 | 347 | } 348 | 349 | double diffManifoldError(Matrix4d T1, Matrix4d T2){ 350 | return ( logmap_se3(T1)-logmap_se3(T2) ).norm(); 351 | } 352 | 353 | bool is_finite(const MatrixXd x){ 354 | return ((x - x).array() == (x - x).array()).all(); 355 | } 356 | 357 | bool is_nan(const MatrixXd x){ 358 | for(unsigned int i = 0; i < x.rows(); i++){ 359 | for(unsigned int j = 0; j < x.cols(); j++){ 360 | if(std::isnan(x(i,j))) 361 | return true; 362 | } 363 | } 364 | return false; 365 | } 366 | 367 | double angDiff(double alpha, double beta){ 368 | double theta = alpha - beta; 369 | if(theta>PI) 370 | theta -= 2.f * PI; 371 | if(theta<-PI) 372 | theta += 2.f * PI; 373 | return theta; 374 | } 375 | 376 | double angDiff_d(double alpha, double beta){ 377 | double theta = alpha - beta; 378 | if(theta > 180.0) 379 | theta -= 360.0; 380 | if(theta<-PI) 381 | theta += 360.0; 382 | return theta; 383 | } 384 | 385 | /* Auxiliar functions and structs for vectors */ 386 | 387 | void vector_mean_stdv_mad( vector residues, double &mean, double &stdv ) 388 | { 389 | 390 | mean = 0.f; 391 | stdv = 0.f; 392 | 393 | if( residues.size() != 0 ) 394 | { 395 | // Return the standard deviation of vector with MAD estimation 396 | int n_samples = residues.size(); 397 | vector residues_ = residues; 398 | sort( residues.begin(),residues.end() ); 399 | double median = residues[ n_samples/2 ]; 400 | for( int i = 0; i < n_samples; i++) 401 | residues[i] = fabsf( residues[i] - median ); 402 | sort( residues.begin(),residues.end() ); 403 | stdv = 1.4826 * residues[ n_samples/2 ]; 404 | 405 | // return the mean with only the best samples 406 | int k = 0; 407 | for( int i = 0; i < n_samples; i++) 408 | if( residues_[i] < 2.0 * stdv ) 409 | { 410 | mean += residues_[i]; 411 | k++; 412 | } 413 | 414 | 415 | if( k >= int( 0.2 * residues.size()) ) 416 | mean /= double(k); 417 | else 418 | { 419 | k = 0; 420 | mean = 0.f; 421 | for( int i = 0; i < n_samples; i++) 422 | { 423 | mean += residues_[i]; 424 | k++; 425 | } 426 | mean /= double(k); 427 | } 428 | } 429 | 430 | } 431 | 432 | double vector_stdv_mad( VectorXf residues) 433 | { 434 | // Return the standard deviation of vector with MAD estimation 435 | int n_samples = residues.size(); 436 | sort( residues.derived().data(),residues.derived().data()+residues.size()); 437 | double median = residues( n_samples/2 ); 438 | residues << ( residues - VectorXf::Constant(n_samples,median) ).cwiseAbs(); 439 | sort(residues.derived().data(),residues.derived().data()+residues.size()); 440 | double MAD = residues( n_samples/2 ); 441 | return 1.4826 * MAD; 442 | } 443 | 444 | double vector_stdv_mad( vector residues) 445 | { 446 | if( residues.size() != 0 ) 447 | { 448 | // Return the standard deviation of vector with MAD estimation 449 | int n_samples = residues.size(); 450 | sort( residues.begin(),residues.end() ); 451 | double median = residues[ n_samples/2 ]; 452 | for( int i = 0; i < n_samples; i++) 453 | residues[i] = fabsf( residues[i] - median ); 454 | sort( residues.begin(),residues.end() ); 455 | double MAD = residues[ n_samples/2 ]; 456 | return 1.4826 * MAD; 457 | } 458 | else 459 | return 0.0; 460 | } 461 | 462 | double vector_stdv_mad( vector residues, double &median) 463 | { 464 | if( residues.size() != 0 ) 465 | { 466 | // Return the standard deviation of vector with MAD estimation 467 | int n_samples = residues.size(); 468 | sort( residues.begin(),residues.end() ); 469 | median = residues[ n_samples/2 ]; 470 | for( int i = 0; i < n_samples; i++) 471 | residues[i] = fabsf( residues[i] - median ); 472 | sort( residues.begin(),residues.end() ); 473 | double MAD = residues[ n_samples/2 ]; 474 | return 1.4826 * MAD; 475 | } 476 | else 477 | return 0.0; 478 | } 479 | 480 | double vector_mean_mad(vector v, double stdv, double K) 481 | { 482 | double sum = 0.0; 483 | int k = 0; 484 | for( int i = 0; i < v.size(); i++ ) 485 | if( v[i] < K * stdv ) 486 | { 487 | sum += v[i]; 488 | k++; 489 | } 490 | if( k > 0 ) 491 | return sum / k; 492 | else 493 | return sum; 494 | } 495 | 496 | double vector_mean(vector v) 497 | { 498 | double sum = 0.0; 499 | for( int i = 0; i < v.size(); i++ ) 500 | sum += v[i]; 501 | return sum / v.size(); 502 | } 503 | 504 | double vector_stdv(vector v) 505 | { 506 | double mean = 0.0, e = 0.0; 507 | for( int i = 0; i < v.size(); i++ ) 508 | mean += v[i]; 509 | mean /= v.size(); 510 | for( int i = 0; i < v.size(); i++ ) 511 | e += (v[i] - mean)*(v[i] - mean); 512 | return sqrt(1.0/v.size()*e); 513 | } 514 | 515 | double vector_stdv(vector v, double v_mean) 516 | { 517 | double e = 0.0; 518 | for( int i = 0; i < v.size(); i++ ) 519 | e += (v[i] - v_mean)*(v[i] - v_mean); 520 | return sqrt(1.0/v.size()*e); 521 | } 522 | 523 | double vector_stdv_mad_nozero( vector residues) 524 | { 525 | if( residues.size() != 0 ) 526 | { 527 | // Return the standard deviation of vector with MAD estimation 528 | int n_samples = residues.size(); 529 | sort( residues.begin(),residues.end() ); 530 | // filter zeros 531 | vector residues_f; 532 | for( int i = 0; i < n_samples; i++ ) 533 | if( residues[i] > 0.0f ) 534 | residues_f.push_back( residues[i] ); 535 | 536 | // estimate robust stdv 537 | n_samples = residues_f.size(); 538 | if( n_samples != 0 ) 539 | { 540 | double median = residues_f[ n_samples/2 ]; 541 | for( int i = 0; i < n_samples; i++) 542 | residues_f[i] = fabsf( residues_f[i] - median ); 543 | sort( residues_f.begin(),residues_f.end() ); 544 | double MAD = residues_f[ n_samples/2 ]; 545 | return 1.4826 * MAD; 546 | } 547 | else 548 | return 0.0; 549 | } 550 | else 551 | return 0.0; 552 | } 553 | 554 | /* Robust weight functions */ 555 | 556 | double robustWeightCauchy( double norm_res ) 557 | { 558 | // Cauchy 559 | return 1.0 / ( 1.0 + norm_res * norm_res ); 560 | 561 | // Smooth Truncated Parabola 562 | /*if( norm_res <= 1.0 ) 563 | return 1.0 - norm_res * norm_res; 564 | else 565 | return 0.0;*/ 566 | 567 | // Tukey's biweight 568 | /*if( norm_res <= 1.0 ) 569 | return pow( 1.0 - norm_res*norm_res ,2.0); 570 | else 571 | return 0.0;*/ 572 | 573 | // Huber loss function 574 | /*if( norm_res <= 1.0 ) 575 | return 1.0; 576 | else 577 | return 1.0 / norm_res;*/ 578 | 579 | // Welsch 580 | //return exp( - norm_res*norm_res ); 581 | 582 | 583 | } 584 | 585 | 586 | 587 | 588 | 589 | 590 | 591 | 592 | 593 | 594 | 595 | 596 | 597 | 598 | 599 | 600 | 601 | 602 | 603 | 604 | 605 | 606 | 607 | 608 | 609 | 610 | 611 | 612 | 613 | 614 | -------------------------------------------------------------------------------- /src/config.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #include 24 | 25 | //STL 26 | #include 27 | 28 | //Boost 29 | #include 30 | 31 | //YAML 32 | #include 33 | 34 | using namespace std; 35 | 36 | Config::Config() 37 | { 38 | 39 | // kf decision (SLAM) parameters 40 | min_entropy_ratio = 0.85; 41 | max_kf_t_dist = 5.0; 42 | max_kf_r_dist = 15.0; 43 | 44 | // StVO-PL options 45 | // ----------------------------------------------------------------------------------------------------- 46 | has_points = true; // true if using points 47 | has_lines = true; // true if using line segments 48 | use_fld_lines = false; // true if using FLD detector 49 | lr_in_parallel = true; // true if detecting and matching features in parallel 50 | pl_in_parallel = true; // true if detecting points and line segments in parallel 51 | best_lr_matches = true; // true if double-checking the matches between the two images 52 | adaptative_fast = true; // true if using adaptative fast_threshold 53 | use_motion_model = false; // true if using constant motion model 54 | 55 | // Tracking parameters 56 | // ----------------------------------------------------------------------------------------------------- 57 | // Point features 58 | max_dist_epip = 1.0; // max. epipolar distance in pixels 59 | min_disp = 1.0; // min. disparity (avoid points in the infinite) 60 | min_ratio_12_p = 0.9; // min. ratio between the first and second best matches 61 | 62 | // Line segment features 63 | line_sim_th = 0.75; // threshold for cosine similarity 64 | stereo_overlap_th = 0.75; 65 | f2f_overlap_th = 0.75; 66 | min_line_length = 0.025; // min. line length (relative to img size) 67 | line_horiz_th = 0.1; // parameter to avoid horizontal lines (pixels) 68 | min_ratio_12_l = 0.9; // parameter to avoid outliers in line matching 69 | ls_min_disp_ratio = 0.7; // min ratio between min(disp)/max(disp) for a LS 70 | 71 | // Adaptative FAST parameters 72 | fast_min_th = 5; // min. value for FAST threshold 73 | fast_max_th = 50; // max. value for FAST threshold 74 | fast_inc_th = 5; // base increment for the FAST threshold 75 | fast_feat_th = 50; // base number of features to increase/decrease FAST threshold 76 | fast_err_th = 0.5; // threshold for the optimization error 77 | 78 | // Optimization parameters 79 | // ----------------------------------------------------------------------------------------------------- 80 | homog_th = 1e-7; // avoid points in the infinite 81 | min_features = 10; // min. number of features to perform StVO 82 | max_iters = 5; // max. number of iterations in the first stage of the optimization 83 | max_iters_ref = 10; // max. number of iterations in the refinement stage 84 | min_error = 1e-7; // min. error to stop the optimization 85 | min_error_change = 1e-7; // min. error change to stop the optimization 86 | inlier_k = 4.0; // factor to discard outliers before the refinement stage 87 | 88 | // Feature detection parameters 89 | // ----------------------------------------------------------------------------------------------------- 90 | matching_strategy = 0; // 0 - pure descriptor based | 1 - window based plus descriptor 91 | matching_s_ws = 10; // size of the windows (in pixels) to look for stereo matches (if matching_stereo=1) 92 | matching_f2f_ws = 3; // size of the windows (in pixels) to look for f2f matches 93 | 94 | // ORB detector 95 | orb_nfeatures = 1200; // number of ORB features to detect 96 | orb_scale_factor = 1.2; // pyramid decimation ratio for the ORB detector 97 | orb_nlevels = 4; // number of pyramid levels 98 | orb_edge_th = 19; // size of the border where the features are not detected 99 | orb_wta_k = 2; // number of points that produce each element of the oriented BRIEF descriptor 100 | orb_score = 1; // 0 - HARRIS | 1 - FAST 101 | orb_patch_size = 31; // size of the patch used by the oriented BRIEF descriptor. 102 | orb_fast_th = 20; // default FAST threshold 103 | // LSD parameters 104 | lsd_nfeatures = 300; // number of LSD lines detected (set to 0 if keeping all lines) 105 | lsd_refine = 0; // the way of refining or not the detected lines 106 | lsd_scale = 1.2; // scale of the image that will be used to find the lines 107 | lsd_sigma_scale = 0.6; // sigma for Gaussian filter 108 | lsd_quant = 2.0; // bound to the quantization error on the gradient norm 109 | lsd_ang_th = 22.5; // gradient angle tolerance in degrees 110 | lsd_log_eps = 1.0; // detection threshold (only for advanced refinement) 111 | lsd_density_th = 0.6; // minimal density of aligned region points in the enclosing rectangle 112 | lsd_n_bins = 1024; // number of bins in pseudo-ordering of gradient modulus 113 | } 114 | 115 | Config::~Config(){} 116 | 117 | Config& Config::getInstance() 118 | { 119 | static Config instance; // Instantiated on first use and guaranteed to be destroyed 120 | return instance; 121 | } 122 | 123 | template 124 | inline T loadSafe(const YAML::Node &config, std::string param, T default_value = T()) { 125 | 126 | if (YAML::Node parameter = config[param]) 127 | return parameter.as(); 128 | else 129 | return default_value; 130 | } 131 | 132 | void Config::loadFromFile( const string &config_file ) 133 | { 134 | 135 | if (!boost::filesystem::exists(config_file) || !boost::filesystem::is_regular(config_file)) { 136 | cout << "[Config->loadFromFile] Invalid config file, keeping default params..." << endl; 137 | return; 138 | } 139 | 140 | YAML::Node config = YAML::LoadFile(config_file); 141 | 142 | Config::minEntropyRatio() = loadSafe(config, "min_entropy_ratio", Config::minEntropyRatio()); 143 | Config::maxKFTDist() = loadSafe(config, "max_kf_t_dist", Config::maxKFTDist()); 144 | Config::maxKFRDist() = loadSafe(config, "max_kf_r_dist", Config::maxKFRDist()); 145 | 146 | Config::hasPoints() = loadSafe(config, "has_points", Config::hasPoints()); 147 | Config::hasLines() = loadSafe(config, "has_lines", Config::hasLines()); 148 | Config::useFLDLines() = loadSafe(config, "use_fld_lines", Config::useFLDLines()); 149 | Config::lrInParallel() = loadSafe(config, "lr_in_parallel", Config::lrInParallel()); 150 | Config::plInParallel() = loadSafe(config, "pl_in_parallel", Config::plInParallel()); 151 | Config::bestLRMatches() = loadSafe(config, "best_lr_matches", Config::bestLRMatches()); 152 | Config::adaptativeFAST() = loadSafe(config, "adaptative_fast", Config::adaptativeFAST()); 153 | Config::useMotionModel() = loadSafe(config, "use_motion_model", Config::useMotionModel()); 154 | 155 | Config::maxDistEpip() = loadSafe(config, "max_dist_epip", Config::maxDistEpip()); 156 | Config::minDisp() = loadSafe(config, "min_disp", Config::minDisp()); 157 | Config::minRatio12P() = loadSafe(config, "min_ratio_12_p", Config::minRatio12P()); 158 | 159 | Config::lineSimTh() = loadSafe(config, "line_sim_th", Config::lineSimTh()); 160 | Config::stereoOverlapTh() = loadSafe(config, "stereo_overlap_th", Config::stereoOverlapTh()); 161 | Config::f2fOverlapTh() = loadSafe(config, "f2f_overlap_th", Config::f2fOverlapTh()); 162 | Config::minLineLength() = loadSafe(config, "min_line_length", Config::minLineLength()); 163 | Config::lineHorizTh() = loadSafe(config, "line_horiz_th", Config::lineHorizTh()); 164 | Config::minRatio12L() = loadSafe(config, "min_ratio_12_l", Config::minRatio12L()); 165 | Config::lsMinDispRatio() = loadSafe(config, "ls_min_disp_ratio", Config::lsMinDispRatio()); 166 | 167 | Config::fastMinTh() = loadSafe(config, "fast_min_th", Config::fastMinTh()); 168 | Config::fastMaxTh() = loadSafe(config, "fast_max_th", Config::fastMaxTh()); 169 | Config::fastIncTh() = loadSafe(config, "fast_inc_th", Config::fastIncTh()); 170 | Config::fastFeatTh() = loadSafe(config, "fast_feat_th", Config::fastFeatTh()); 171 | Config::fastErrTh() = loadSafe(config, "fast_err_th", Config::fastErrTh()); 172 | 173 | Config::rgbdMinDepth() = loadSafe(config, "rgbd_min_depth", Config::rgbdMinDepth()); 174 | Config::rgbdMaxDepth() = loadSafe(config, "rgbd_max_depth", Config::rgbdMaxDepth()); 175 | 176 | Config::homogTh() = loadSafe(config, "homog_th", Config::homogTh()); 177 | Config::minFeatures() = loadSafe(config, "min_features", Config::minFeatures()); 178 | Config::maxIters() = loadSafe(config, "max_iters", Config::maxIters()); 179 | Config::maxItersRef() = loadSafe(config, "max_iters_ref", Config::maxItersRef()); 180 | Config::minError() = loadSafe(config, "min_error", Config::minError()); 181 | Config::minErrorChange() = loadSafe(config, "min_error_change", Config::minErrorChange()); 182 | Config::inlierK() = loadSafe(config, "inlier_k", Config::inlierK()); 183 | 184 | Config::matchingStrategy() = loadSafe(config, "matching_strategy", Config::matchingStrategy()); 185 | Config::matchingSWs() = loadSafe(config, "matching_s_ws", Config::matchingSWs()); 186 | Config::matchingF2FWs() = loadSafe(config, "matching_f2f_ws", Config::matchingF2FWs()); 187 | 188 | Config::orbNFeatures() = loadSafe(config, "orb_nfeatures", Config::orbNFeatures()); 189 | Config::orbScaleFactor() = loadSafe(config, "orb_scale_factor", Config::orbScaleFactor()); 190 | Config::orbNLevels() = loadSafe(config, "orb_nlevels", Config::orbNLevels()); 191 | Config::orbEdgeTh() = loadSafe(config, "orb_edge_th", Config::orbEdgeTh()); 192 | Config::orbWtaK() = loadSafe(config, "orb_wta_k", Config::orbWtaK()); 193 | Config::orbScore() = loadSafe(config, "orb_score", Config::orbScore()); 194 | Config::orbPatchSize() = loadSafe(config, "orb_patch_size", Config::orbPatchSize()); 195 | Config::orbFastTh() = loadSafe(config, "orb_fast_th", Config::orbFastTh()); 196 | 197 | Config::lsdNFeatures() = loadSafe(config, "lsd_nfeatures", Config::lsdNFeatures()); 198 | Config::lsdRefine() = loadSafe(config, "lsd_refine", Config::lsdRefine()); 199 | Config::lsdScale() = loadSafe(config, "lsd_scale", Config::lsdScale()); 200 | Config::lsdSigmaScale() = loadSafe(config, "lsd_sigma_scale", Config::lsdSigmaScale()); 201 | Config::lsdQuant() = loadSafe(config, "lsd_quant", Config::lsdQuant()); 202 | Config::lsdAngTh() = loadSafe(config, "lsd_ang_th", Config::lsdAngTh()); 203 | Config::lsdLogEps() = loadSafe(config, "lsd_log_eps", Config::lsdLogEps()); 204 | Config::lsdDensityTh() = loadSafe(config, "lsd_density_th", Config::lsdDensityTh()); 205 | Config::lsdNBins() = loadSafe(config, "lsd_n_bins", Config::lsdNBins()); 206 | } 207 | -------------------------------------------------------------------------------- /src/dataset.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #include "dataset.h" 24 | 25 | //STL 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | //Boost 34 | #include //Note: using boost regex instead of C++11 regex as it isn't supported by the compiler until gcc 4.9 35 | #include 36 | #include 37 | 38 | //OpenCV 39 | #include 40 | #include 41 | 42 | //YAML 43 | #include 44 | 45 | #include "pinholeStereoCamera.h" 46 | 47 | namespace StVO { 48 | 49 | void getSortedImages(const boost::filesystem::path &img_dir, std::function filter, 50 | std::function comparator, std::vector &img_paths) { 51 | 52 | // get a sorted list of files in the img directories 53 | if (!boost::filesystem::exists(img_dir) || 54 | !boost::filesystem::is_directory(img_dir)) 55 | throw std::runtime_error("[Dataset] Invalid images subfolder"); 56 | 57 | // get all files in the img directories 58 | std::list all_imgs; 59 | for(auto& entry : boost::make_iterator_range(boost::filesystem::directory_iterator(img_dir), {})) { 60 | boost::filesystem::path filename_path = entry.path().filename(); 61 | if (boost::filesystem::is_regular_file(entry.status()) && 62 | (filename_path.extension() == ".png" || 63 | filename_path.extension() == ".jpg" || 64 | filename_path.extension() == ".jpeg" || 65 | filename_path.extension() == ".pnm" || 66 | filename_path.extension() == ".tiff")) { 67 | all_imgs.push_back(filename_path.string()); 68 | } 69 | } 70 | 71 | // sort 72 | img_paths.clear(); 73 | img_paths.reserve(all_imgs.size()); 74 | for (const std::string &filename : all_imgs) 75 | if (!filter(filename)) img_paths.push_back(filename); 76 | 77 | if (img_paths.empty()) 78 | throw std::runtime_error("[Dataset] Invalid image names?"); 79 | 80 | std::sort(img_paths.begin(), img_paths.end(), comparator); 81 | 82 | for (std::string &filename : img_paths) 83 | filename = (img_dir / filename).string(); 84 | } 85 | 86 | Dataset::Dataset(const std::string &dataset_path, const PinholeStereoCamera &cam, int offset, int nmax, int step) 87 | : cam(cam) { 88 | 89 | boost::filesystem::path dataset_base(dataset_path); 90 | if (!boost::filesystem::exists(dataset_base) || 91 | !boost::filesystem::is_directory(dataset_base)) 92 | throw std::runtime_error("[Dataset] Invalid directory"); 93 | 94 | boost::filesystem::path dataset_params = dataset_base / "dataset_params.yaml"; 95 | if (!boost::filesystem::exists(dataset_params)) 96 | throw std::runtime_error("[Dataset] Dataset parameters not found"); 97 | YAML::Node dataset_config = YAML::LoadFile(dataset_params.string()); 98 | 99 | // setup image directories 100 | boost::filesystem::path img_l_dir = dataset_base / dataset_config["images_subfolder_l"].as(); 101 | boost::filesystem::path img_r_dir = dataset_base / dataset_config["images_subfolder_r"].as(); 102 | 103 | boost::regex expression("^[^0-9]*([0-9]+\\.?+[0-9]*)[^0-9]*\\.[a-z]{3,4}$"); 104 | boost::cmatch what; 105 | 106 | auto filename_filter = [&expression, &what](const std::string &s) { 107 | return !boost::regex_match(s.c_str(), what, expression); 108 | }; 109 | 110 | auto sort_by_number = [&expression, &what](const std::string &a, const std::string &b) { 111 | double n1, n2; 112 | 113 | if (boost::regex_match(a.c_str(), what, expression)) 114 | n1 = std::stod(what[1]); 115 | else 116 | throw std::runtime_error("[Dataset] Unexpected behaviour while sorting filenames"); 117 | 118 | if (boost::regex_match(b.c_str(), what, expression)) 119 | n2 = std::stod(what[1]); 120 | else 121 | throw std::runtime_error("[Dataset] Unexpected behaviour while sorting filenames"); 122 | 123 | return (n1 < n2); 124 | }; 125 | 126 | std::vector img_l_paths, img_r_paths; 127 | getSortedImages(img_l_dir, filename_filter, sort_by_number, img_l_paths); 128 | getSortedImages(img_r_dir, filename_filter, sort_by_number, img_r_paths); 129 | 130 | if (img_l_paths.size() != img_r_paths.size()) 131 | throw std::runtime_error("[Dataset] Left and right images"); 132 | 133 | // decimate sequence 134 | offset = std::max(0, offset); 135 | nmax = (nmax <= 0) ? std::numeric_limits::max() : nmax; 136 | step = std::max(1, step); 137 | for (int i = 0, ctr = 0; (i + offset) < img_l_paths.size() && ctr < nmax; i += step, ctr++) { 138 | images_l.push_back(img_l_paths[i + offset]); 139 | images_r.push_back(img_r_paths[i + offset]); 140 | } 141 | } 142 | 143 | Dataset::~Dataset() { 144 | 145 | } 146 | 147 | bool Dataset::nextFrame(cv::Mat &img_l, cv::Mat &img_r) { 148 | if (!hasNext()) return false; 149 | 150 | img_l = cv::imread(images_l.front(), CV_LOAD_IMAGE_UNCHANGED); 151 | img_r = cv::imread(images_r.front(), CV_LOAD_IMAGE_UNCHANGED); 152 | cam.rectifyImagesLR(img_l, img_l, img_r, img_r); 153 | images_l.pop_front(); 154 | images_r.pop_front(); 155 | 156 | return (!img_l.empty() && !img_r.empty()); 157 | } 158 | 159 | bool Dataset::hasNext() { 160 | return !(images_l.empty() || images_r.empty()); 161 | } 162 | 163 | } // namespace StVO 164 | 165 | -------------------------------------------------------------------------------- /src/featureMatching.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "featureMatching.h" 3 | 4 | //STL 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | //OpenCV 11 | #include 12 | #include 13 | 14 | #include "config.h" 15 | 16 | namespace StVO { 17 | 18 | int matchNNR(const cv::Mat &desc1, const cv::Mat &desc2, float nnr, std::vector &matches_12) { 19 | 20 | int matches = 0; 21 | matches_12.resize(desc1.rows, -1); 22 | 23 | std::vector> matches_; 24 | cv::Ptr bfm = cv::BFMatcher::create(cv::NORM_HAMMING, false); // cross-check 25 | bfm->knnMatch(desc1, desc2, matches_, 2); 26 | 27 | if (desc1.rows != matches_.size()) 28 | throw std::runtime_error("[MapHandler->matchNNR] Different size for matches and descriptors!"); 29 | 30 | for (int idx = 0; idx < desc1.rows; ++idx) { 31 | if (matches_[idx][0].distance < matches_[idx][1].distance * nnr) { 32 | matches_12[idx] = matches_[idx][0].trainIdx; 33 | matches++; 34 | } 35 | } 36 | 37 | return matches; 38 | } 39 | 40 | int match(const cv::Mat &desc1, const cv::Mat &desc2, float nnr, std::vector &matches_12) { 41 | 42 | matches_12.clear(); 43 | if (Config::bestLRMatches()) { 44 | int matches; 45 | std::vector matches_21; 46 | if (Config::lrInParallel()) { 47 | auto match_12 = std::async(std::launch::async, &matchNNR, 48 | std::cref(desc1), std::cref(desc2), nnr, std::ref(matches_12)); 49 | auto match_21 = std::async(std::launch::async, &matchNNR, 50 | std::cref(desc2), std::cref(desc1), nnr, std::ref(matches_21)); 51 | matches = match_12.get(); 52 | match_21.wait(); 53 | } else { 54 | matches = matchNNR(desc1, desc2, nnr, matches_12); 55 | matchNNR(desc2, desc1, nnr, matches_21); 56 | } 57 | 58 | for (int i1 = 0; i1 < matches_12.size(); ++i1) { 59 | int &i2 = matches_12[i1]; 60 | if (i2 >= 0 && matches_21[i2] != i1) { 61 | i2 = -1; 62 | matches--; 63 | } 64 | } 65 | 66 | return matches; 67 | } else 68 | return matchNNR(desc1, desc2, nnr, matches_12); 69 | } 70 | 71 | } // namesapce StVO 72 | -------------------------------------------------------------------------------- /src/gridStructure.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #include "gridStructure.h" 24 | 25 | //STL 26 | #include 27 | #include 28 | 29 | #include "lineIterator.h" 30 | 31 | namespace StVO { 32 | 33 | void getLineCoords(double x1, double y1, double x2, double y2, std::list> &line_coords) { 34 | line_coords.clear(); 35 | 36 | LineIterator it(x1, y1, x2, y2); 37 | 38 | std::pair p; 39 | while (it.getNext(p)) 40 | line_coords.push_back(p); 41 | } 42 | 43 | GridStructure::GridStructure(int rows, int cols) 44 | : rows(rows), cols(cols) { 45 | 46 | if (rows <= 0 || cols <= 0) 47 | throw std::runtime_error("[GridStructure] invalid dimension"); 48 | 49 | grid.resize(cols, std::vector>(rows)); 50 | } 51 | 52 | GridStructure::~GridStructure() { 53 | 54 | } 55 | 56 | std::list& GridStructure::at(int x, int y) { 57 | 58 | if (x >= 0 && x < cols && 59 | y >= 0 && y < rows) 60 | return grid[x][y]; 61 | else 62 | return out_of_bounds; 63 | } 64 | 65 | void GridStructure::get(int x, int y, const GridWindow &w, std::unordered_set &indices) const { 66 | 67 | int min_x = std::max(0, x - w.width.first); 68 | int max_x = std::min(cols, x + w.width.second + 1); 69 | 70 | int min_y = std::max(0, y - w.height.first); 71 | int max_y = std::min(rows, y + w.height.second + 1); 72 | 73 | for (int x_ = min_x; x_ < max_x; ++x_) 74 | for (int y_ = min_y; y_ < max_y; ++y_) 75 | indices.insert(grid[x_][y_].begin(), grid[x_][y_].end()); 76 | } 77 | 78 | void GridStructure::clear() { 79 | 80 | for (int x = 0; x < cols; ++x) 81 | for (int y = 0; y < rows; ++y) 82 | grid[x][y].clear(); 83 | } 84 | 85 | } //namesapce StVO 86 | -------------------------------------------------------------------------------- /src/lineIterator.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #include "lineIterator.h" 24 | 25 | //STL 26 | #include 27 | #include 28 | 29 | //Implementation of Bresenham's line drawing Algorithm 30 | //Adapted from: https://rosettacode.org/wiki/Bitmap/Bresenham%27s_line_algorithm#C.2B.2B 31 | 32 | namespace StVO { 33 | 34 | LineIterator::LineIterator(const double x1_, const double y1_, const double x2_, const double y2_) 35 | : x1(x1_), y1(y1_), x2(x2_), y2(y2_), steep(std::abs(y2_ - y1_) > std::abs(x2_ - x1_)) { 36 | 37 | if (steep) { 38 | std::swap(x1, y1); 39 | std::swap(x2, y2); 40 | } 41 | 42 | if(x1 > x2) { 43 | std::swap(x1, x2); 44 | std::swap(y1, y2); 45 | } 46 | 47 | dx = x2 - x1; 48 | dy = std::abs(y2 - y1); 49 | 50 | error = dx / 2.0; 51 | ystep = (y1 < y2) ? 1 : -1; 52 | 53 | x = static_cast(x1); 54 | y = static_cast(y1); 55 | 56 | maxX = static_cast(x2); 57 | } 58 | 59 | bool LineIterator::getNext(std::pair &pixel) { 60 | 61 | if (x > maxX) 62 | return false; 63 | 64 | if (steep) 65 | pixel = std::make_pair(y, x); 66 | else 67 | pixel = std::make_pair(x, y); 68 | 69 | error -= dy; 70 | if (error < 0) { 71 | y += ystep; 72 | error += dx; 73 | } 74 | 75 | x++; 76 | return true; 77 | } 78 | 79 | } // namespace StVO 80 | -------------------------------------------------------------------------------- /src/matching.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #include "matching.h" 24 | 25 | //STL 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | //OpenCV 33 | #include 34 | #include 35 | 36 | #include "config.h" 37 | #include "gridStructure.h" 38 | 39 | namespace StVO { 40 | 41 | int matchNNR(const cv::Mat &desc1, const cv::Mat &desc2, float nnr, std::vector &matches_12) { 42 | 43 | int matches = 0; 44 | matches_12.resize(desc1.rows, -1); 45 | 46 | std::vector> matches_; 47 | cv::Ptr bfm = cv::BFMatcher::create(cv::NORM_HAMMING, false); // cross-check 48 | bfm->knnMatch(desc1, desc2, matches_, 2); 49 | 50 | if (desc1.rows != matches_.size()) 51 | throw std::runtime_error("[matchNNR] Different size for matches and descriptors!"); 52 | 53 | for (int idx = 0; idx < desc1.rows; ++idx) { 54 | if (matches_[idx][0].distance < matches_[idx][1].distance * nnr) { 55 | matches_12[idx] = matches_[idx][0].trainIdx; 56 | matches++; 57 | } 58 | } 59 | 60 | return matches; 61 | } 62 | 63 | int match(const cv::Mat &desc1, const cv::Mat &desc2, float nnr, std::vector &matches_12) { 64 | 65 | if (Config::bestLRMatches()) { 66 | int matches; 67 | std::vector matches_21; 68 | if (Config::lrInParallel()) { 69 | auto match_12 = std::async(std::launch::async, &matchNNR, 70 | std::cref(desc1), std::cref(desc2), nnr, std::ref(matches_12)); 71 | auto match_21 = std::async(std::launch::async, &matchNNR, 72 | std::cref(desc2), std::cref(desc1), nnr, std::ref(matches_21)); 73 | matches = match_12.get(); 74 | match_21.wait(); 75 | } else { 76 | matches = matchNNR(desc1, desc2, nnr, matches_12); 77 | matchNNR(desc2, desc1, nnr, matches_21); 78 | } 79 | 80 | for (int i1 = 0; i1 < matches_12.size(); ++i1) { 81 | int &i2 = matches_12[i1]; 82 | if (i2 >= 0 && matches_21[i2] != i1) { 83 | i2 = -1; 84 | matches--; 85 | } 86 | } 87 | 88 | return matches; 89 | } else 90 | return matchNNR(desc1, desc2, nnr, matches_12); 91 | } 92 | 93 | int distance(const cv::Mat &a, const cv::Mat &b) { 94 | 95 | // adapted from: http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel 96 | 97 | const int *pa = a.ptr(); 98 | const int *pb = b.ptr(); 99 | 100 | int dist = 0; 101 | for(int i = 0; i < 8; i++, pa++, pb++) { 102 | unsigned int v = *pa ^ *pb; 103 | v = v - ((v >> 1) & 0x55555555); 104 | v = (v & 0x33333333) + ((v >> 2) & 0x33333333); 105 | dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24; 106 | } 107 | 108 | return dist; 109 | } 110 | 111 | int matchGrid(const std::vector &points1, const cv::Mat &desc1, const GridStructure &grid, const cv::Mat &desc2, const GridWindow &w, std::vector &matches_12) { 112 | 113 | if (points1.size() != desc1.rows) 114 | throw std::runtime_error("[matchGrid] Each point needs a corresponding descriptor!"); 115 | 116 | int matches = 0; 117 | matches_12.resize(desc1.rows, -1); 118 | 119 | int best_d, best_d2, best_idx; 120 | std::vector matches_21, distances; 121 | 122 | if (Config::bestLRMatches()) { 123 | matches_21.resize(desc2.rows, -1); 124 | distances.resize(desc2.rows, std::numeric_limits::max()); 125 | } 126 | 127 | for (int i1 = 0; i1 < points1.size(); ++i1) { 128 | 129 | best_d = std::numeric_limits::max(); 130 | best_d2 = std::numeric_limits::max(); 131 | best_idx = -1; 132 | 133 | const std::pair &coords = points1[i1]; 134 | cv::Mat desc = desc1.row(i1); 135 | 136 | std::unordered_set candidates; 137 | grid.get(coords.first, coords.second, w, candidates); 138 | 139 | if (candidates.empty()) continue; 140 | for (const int &i2 : candidates) { 141 | if (i2 < 0 || i2 >= desc2.rows) continue; 142 | 143 | const int d = distance(desc, desc2.row(i2)); 144 | 145 | if (Config::bestLRMatches()) { 146 | if (d < distances[i2]) { 147 | distances[i2] = d; 148 | matches_21[i2] = i1; 149 | } else continue; 150 | } 151 | 152 | if (d < best_d) { 153 | best_d2 = best_d; 154 | best_d = d; 155 | best_idx = i2; 156 | } else if (d < best_d2) 157 | best_d2 = d; 158 | } 159 | 160 | if (best_d < best_d2 * Config::minRatio12P()) { 161 | matches_12[i1] = best_idx; 162 | matches++; 163 | } 164 | } 165 | 166 | if (Config::bestLRMatches()) { 167 | for (int i1 = 0; i1 < matches_12.size(); ++i1) { 168 | int &i2 = matches_12[i1]; 169 | if (i2 >= 0 && matches_21[i2] != i1) { 170 | i2 = -1; 171 | matches--; 172 | } 173 | } 174 | } 175 | 176 | return matches; 177 | } 178 | 179 | int matchGrid(const std::vector &lines1, const cv::Mat &desc1, 180 | const GridStructure &grid, const cv::Mat &desc2, const std::vector> &directions2, 181 | const GridWindow &w, 182 | std::vector &matches_12) { 183 | 184 | if (lines1.size() != desc1.rows) 185 | throw std::runtime_error("[matchGrid] Each line needs a corresponding descriptor!"); 186 | 187 | int matches = 0; 188 | matches_12.resize(desc1.rows, -1); 189 | 190 | int best_d, best_d2, best_idx; 191 | std::vector matches_21, distances; 192 | 193 | if (Config::bestLRMatches()) { 194 | matches_21.resize(desc2.rows, -1); 195 | distances.resize(desc2.rows, std::numeric_limits::max()); 196 | } 197 | 198 | for (int i1 = 0; i1 < lines1.size(); ++i1) { 199 | 200 | best_d = std::numeric_limits::max(); 201 | best_d2 = std::numeric_limits::max(); 202 | best_idx = -1; 203 | 204 | const line_2d &coords = lines1[i1]; 205 | cv::Mat desc = desc1.row(i1); 206 | 207 | const point_2d sp = coords.first; 208 | const point_2d ep = coords.second; 209 | 210 | std::pair v = std::make_pair(ep.first - sp.first, ep.second - sp.second); 211 | normalize(v); 212 | 213 | std::unordered_set candidates; 214 | grid.get(sp.first, sp.second, w, candidates); 215 | grid.get(ep.first, ep.second, w, candidates); 216 | 217 | if (candidates.empty()) continue; 218 | for (const int &i2 : candidates) { 219 | if (i2 < 0 || i2 >= desc2.rows) continue; 220 | 221 | if (std::abs(dot(v, directions2[i2])) < Config::lineSimTh()) 222 | continue; 223 | 224 | const int d = distance(desc, desc2.row(i2)); 225 | 226 | if (Config::bestLRMatches()) { 227 | if (d < distances[i2]) { 228 | distances[i2] = d; 229 | matches_21[i2] = i1; 230 | } else continue; 231 | } 232 | 233 | if (d < best_d) { 234 | best_d2 = best_d; 235 | best_d = d; 236 | best_idx = i2; 237 | } else if (d < best_d2) 238 | best_d2 = d; 239 | } 240 | 241 | if (best_d < best_d2 * Config::minRatio12P()) { 242 | matches_12[i1] = best_idx; 243 | matches++; 244 | } 245 | } 246 | 247 | if (Config::bestLRMatches()) { 248 | for (int i1 = 0; i1 < matches_12.size(); ++i1) { 249 | int &i2 = matches_12[i1]; 250 | if (i2 >= 0 && matches_21[i2] != i1) { 251 | i2 = -1; 252 | matches--; 253 | } 254 | } 255 | } 256 | 257 | return matches; 258 | } 259 | 260 | } //namesapce StVO 261 | -------------------------------------------------------------------------------- /src/pinholeStereoCamera.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | PinholeStereoCamera::PinholeStereoCamera(const string ¶ms_file) { 31 | // read content of the .yaml dataset configuration file 32 | if (!boost::filesystem::exists(params_file) || !boost::filesystem::is_regular(params_file)) 33 | throw std::runtime_error("[PinholeSteroCamera] Invalid parameters file"); 34 | 35 | YAML::Node dset_config = YAML::LoadFile(params_file); 36 | 37 | // setup camera 38 | YAML::Node cam_config = dset_config["cam0"]; 39 | 40 | width = cam_config["cam_width"].as(); 41 | height = cam_config["cam_height"].as(); 42 | b = cam_config["cam_bl"].as(); 43 | 44 | string camera_model = cam_config["cam_model"].as(); 45 | if( camera_model == "Pinhole" ) 46 | { 47 | // if EuRoC or Falcon yaml file 48 | if( cam_config["Kl"].IsDefined() ) 49 | { 50 | dist = true; 51 | vector Kl_ = cam_config["Kl"].as>(); 52 | vector Kr_ = cam_config["Kr"].as>(); 53 | vector Dl_ = cam_config["Dl"].as>(); 54 | vector Dr_ = cam_config["Dr"].as>(); 55 | Kl = ( Mat_(3,3) << Kl_[0], 0.0, Kl_[2], 0.0, Kl_[1], Kl_[3], 0.0, 0.0, 1.0 ); 56 | Kr = ( Mat_(3,3) << Kr_[0], 0.0, Kr_[2], 0.0, Kr_[1], Kr_[3], 0.0, 0.0, 1.0 ); 57 | // load rotation and translation 58 | vector R_ = cam_config["R"].as>(); 59 | vector t_ = cam_config["t"].as>(); 60 | R = Mat::eye(3,3,CV_64F); 61 | t = Mat::eye(3,1,CV_64F); 62 | int k = 0; 63 | for( int i = 0; i < 3; i++ ) 64 | { 65 | t.at(i,0) = t_[i]; 66 | for( int j = 0; j < 3; j++, k++ ) 67 | R.at(i,j) = R_[k]; 68 | } 69 | // load distortion parameters 70 | int Nd = Dl_.size(); 71 | Dl = Mat::eye(1,Nd,CV_64F); 72 | Dr = Mat::eye(1,Nd,CV_64F); 73 | for( int i = 0; i < Nd; i++ ) 74 | { 75 | Dl.at(0,i) = Dl_[i]; 76 | Dr.at(0,i) = Dr_[i]; 77 | } 78 | // if dtype is equidistant (now it is default) 79 | // initialize undistort rectify map OpenCV 80 | if(cam_config["dtype"].IsDefined()) 81 | { 82 | stereoRectify( Kl, Dl, Kr, Dr, cv::Size(width,height), R, t, Rl, Rr, Pl, Pr, Q, cv::CALIB_ZERO_DISPARITY, 0 ); 83 | cv::fisheye::initUndistortRectifyMap( Kl, Dl, Rl, Pl, cv::Size(width,height), CV_16SC2, undistmap1l, undistmap2l ); 84 | cv::fisheye::initUndistortRectifyMap( Kr, Dr, Rr, Pr, cv::Size(width,height), CV_16SC2, undistmap1r, undistmap2r ); 85 | } 86 | else 87 | { 88 | stereoRectify( Kl, Dl, Kr, Dr, cv::Size(width,height), R, t, Rl, Rr, Pl, Pr, Q, cv::CALIB_ZERO_DISPARITY, 0 ); 89 | initUndistortRectifyMap( Kl, Dl, Rl, Pl, cv::Size(width,height), CV_16SC2, undistmap1l, undistmap2l ); 90 | initUndistortRectifyMap( Kr, Dr, Rr, Pr, cv::Size(width,height), CV_16SC2, undistmap1r, undistmap2r ); 91 | } 92 | 93 | fx = Pl.at(0,0); 94 | fy = Pl.at(1,1); 95 | cx = Pl.at(0,2); 96 | cy = Pl.at(1,2); 97 | 98 | K << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0; 99 | } else { 100 | fx = std::abs(cam_config["cam_fx"].as()); 101 | fy = std::abs(cam_config["cam_fy"].as()); 102 | cx = cam_config["cam_cx"].as(); 103 | cy = cam_config["cam_cy"].as(); 104 | 105 | double d0 = cam_config["cam_d0"].as(), 106 | d1 = cam_config["cam_d1"].as(), 107 | d2 = cam_config["cam_d2"].as(), 108 | d3 = cam_config["cam_d3"].as(); 109 | 110 | dist = (d0 != 0.0 ); 111 | d << d0, d1, d2, d3, 0.0; 112 | Kl = ( Mat_(3,3) << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0 ); 113 | Dl = ( Mat_(1,5) << d(0), d(1), d(2), d(3), d(4) ); 114 | Pl = ( Mat_(3,4) << fx, 0.0, cx, 0.0, 0.0, fx, cy, 0.0, 0.0, 0.0, 1.0, 0.0 ); 115 | K << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0; 116 | 117 | // initialize undistort rectify map OpenCV 118 | initUndistortRectifyMap( Kl, Dl, cv::Mat_::eye(3,3), Pl, cv::Size(width,height), CV_16SC2, undistmap1l, undistmap2l ); 119 | undistmap1r = undistmap1l; 120 | undistmap2r = undistmap2l; 121 | } 122 | } 123 | else 124 | throw std::runtime_error("[PinholeStereoCamera] Invalid cam_model"); 125 | } 126 | 127 | PinholeStereoCamera::PinholeStereoCamera( int width_, int height_, double fx_, double fy_, double cx_, double cy_, double b_, 128 | double d0, double d1, double d2, double d3, double d4) : 129 | width(width_), height(height_), fx(fx_), fy(fy_), cx(cx_), cy(cy_), b(b_), dist( d0 != 0.0 ) 130 | { 131 | d << d0, d1, d2, d3, d4; 132 | Kl = ( Mat_(3,3) << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0 ); 133 | Dl = ( Mat_(1,5) << d(0), d(1), d(2), d(3), d(4) ); 134 | Pl = ( Mat_(3,4) << fx, 0.0, cx, 0.0, 0.0, fx, cy, 0.0, 0.0, 0.0, 1.0, 0.0 ); 135 | K << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0; 136 | // initialize undistort rectify map OpenCV 137 | initUndistortRectifyMap( Kl, Dl, cv::Mat_::eye(3,3), Pl, cv::Size(width,height), CV_16SC2, undistmap1l, undistmap2l ); 138 | undistmap1r = undistmap1l; 139 | undistmap2r = undistmap2l; 140 | } 141 | 142 | PinholeStereoCamera::PinholeStereoCamera( int width_, int height_, double fx_, double fy_, double cx_, double cy_, double b_, Mat Rl_, Mat Rr_, 143 | double d0, double d1, double d2, double d3, double d4 ) : 144 | width(width_), height(height_), fx(fx_), fy(fy_), cx(cx_), cy(cy_), b(b_), dist( d0 != 0.0 ), Rl(Rl_), Rr(Rr_) 145 | { 146 | d << d0, d1, d2, d3, d4; 147 | Kl = ( Mat_(3,3) << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0 ); 148 | Dl = ( Mat_(1,5) << d(0), d(1), d(2), d(3), d(4) ); 149 | K << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0; 150 | // initialize undistort rectify map OpenCV 151 | initUndistortRectifyMap( Kl, Dl, Rl, Pl, cv::Size(width,height), CV_16SC2, undistmap1l, undistmap2l ); 152 | initUndistortRectifyMap( Kr, Dr, Rr, Pr, cv::Size(width,height), CV_16SC2, undistmap1r, undistmap2r ); 153 | } 154 | 155 | PinholeStereoCamera::PinholeStereoCamera( int width_, int height_, double b_, Mat Kl_, Mat Kr_, Mat R_, Mat t_, Mat Dl_, Mat Dr_, bool equi) : 156 | width(width_), height(height_), Kl(Kl_), Kr(Kr_), b(b_), R(R_), t(t_), Dl(Dl_), Dr(Dr_) 157 | { 158 | 159 | // initialize undistort rectify map OpenCV 160 | if(equi) 161 | { 162 | stereoRectify( Kl, Dl, Kr, Dr, cv::Size(width,height), R, t, Rl, Rr, Pl, Pr, Q, cv::CALIB_ZERO_DISPARITY, 0 ); 163 | cv::fisheye::initUndistortRectifyMap( Kl, Dl, Rl, Pl, cv::Size(width,height), CV_16SC2, undistmap1l, undistmap2l ); 164 | cv::fisheye::initUndistortRectifyMap( Kr, Dr, Rr, Pr, cv::Size(width,height), CV_16SC2, undistmap1r, undistmap2r ); 165 | } 166 | else 167 | { 168 | stereoRectify( Kl, Dl, Kr, Dr, cv::Size(width,height), R, t, Rl, Rr, Pl, Pr, Q, cv::CALIB_ZERO_DISPARITY, 0 ); 169 | initUndistortRectifyMap( Kl, Dl, Rl, Pl, cv::Size(width,height), CV_16SC2, undistmap1l, undistmap2l ); 170 | initUndistortRectifyMap( Kr, Dr, Rr, Pr, cv::Size(width,height), CV_16SC2, undistmap1r, undistmap2r ); 171 | } 172 | 173 | dist = true; 174 | 175 | fx = Pl.at(0,0); 176 | fy = Pl.at(1,1); 177 | cx = Pl.at(0,2); 178 | cy = Pl.at(1,2); 179 | 180 | K << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0; 181 | 182 | } 183 | 184 | PinholeStereoCamera::~PinholeStereoCamera() { 185 | 186 | } 187 | 188 | void PinholeStereoCamera::rectifyImage( const Mat& img_src, Mat& img_rec) const 189 | { 190 | if(dist) 191 | remap( img_src, img_rec, undistmap1l, undistmap2l, cv::INTER_LINEAR); 192 | else 193 | img_src.copyTo(img_rec); 194 | } 195 | 196 | void PinholeStereoCamera::rectifyImagesLR( const Mat& img_src_l, Mat& img_rec_l, const Mat& img_src_r, Mat& img_rec_r ) const 197 | { 198 | if(dist) 199 | { 200 | remap( img_src_l, img_rec_l, undistmap1l, undistmap2l, cv::INTER_LINEAR); 201 | remap( img_src_r, img_rec_r, undistmap1r, undistmap2r, cv::INTER_LINEAR); 202 | } 203 | else 204 | { 205 | img_src_l.copyTo(img_rec_l); 206 | img_src_r.copyTo(img_rec_r); 207 | } 208 | } 209 | 210 | // Proyection and Back-projection (internally we are supposed to work with rectified images because of the line segments) 211 | Vector3d PinholeStereoCamera::backProjection_unit( const double &u, const double &v, const double &disp, double &depth ) 212 | { 213 | Vector3d P_unit; 214 | P_unit(0) = (u-cx)/fx; 215 | P_unit(1) = (v-cy)/fy; 216 | P_unit(2) = 1.0; 217 | depth = b*fx/disp; 218 | return P_unit; 219 | } 220 | 221 | Vector3d PinholeStereoCamera::backProjection( const double &u, const double &v, const double &disp ) 222 | { 223 | Vector3d P; 224 | double bd = b/disp; 225 | P(0) = bd*(u-cx); 226 | P(1) = bd*(v-cy); 227 | P(2) = bd*fx; 228 | return P; 229 | } 230 | 231 | Vector2d PinholeStereoCamera::projection(const Vector3d &P ) 232 | { 233 | Vector2d uv_unit; 234 | uv_unit(0) = cx + fx * P(0) / P(2); 235 | uv_unit(1) = cy + fy * P(1) / P(2); 236 | return uv_unit; 237 | } 238 | 239 | Vector3d PinholeStereoCamera::projectionNH( Vector3d P ) 240 | { 241 | Vector3d uv_proj; 242 | uv_proj(0) = cx * P(2) + fx * P(0); 243 | uv_proj(1) = cy * P(2) + fy * P(1); 244 | uv_proj(2) = P(2); 245 | return uv_proj; 246 | } 247 | 248 | Vector2d PinholeStereoCamera::nonHomogeneous( Vector3d x) 249 | { 250 | Vector2d x_; x_ << x(0) / x(2), x(1) / x(2); 251 | return x_; 252 | } 253 | -------------------------------------------------------------------------------- /src/stereoFeatures.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #include 24 | 25 | namespace StVO{ 26 | 27 | // Point feature 28 | 29 | PointFeature::PointFeature( Vector3d P_, Vector2d pl_obs_) : 30 | P(P_), pl_obs(pl_obs_), level(0) 31 | {} 32 | 33 | PointFeature::PointFeature( Vector2d pl_, double disp_, Vector3d P_ ) : 34 | pl(pl_), disp(disp_), P(P_), inlier(true), level(0) 35 | {} 36 | 37 | PointFeature::PointFeature( Vector2d pl_, double disp_, Vector3d P_, int idx_ ) : 38 | pl(pl_), disp(disp_), P(P_), inlier(true), idx(idx_), level(0) 39 | {} 40 | 41 | PointFeature::PointFeature( Vector2d pl_, double disp_, Vector3d P_, int idx_, int level_ ) : 42 | pl(pl_), disp(disp_), P(P_), inlier(true), idx(idx_), level(level_) 43 | { 44 | for( int i = 0; i < level; i++ ) 45 | sigma2 *= Config::orbScaleFactor(); 46 | sigma2 = 1.f / (sigma2*sigma2); 47 | } 48 | 49 | PointFeature::PointFeature( Vector2d pl_, double disp_, Vector3d P_, int idx_, int level_, Matrix3d covP_an_ ) : 50 | pl(pl_), disp(disp_), P(P_), inlier(true), idx(idx_), level(level_), covP_an(covP_an_) 51 | { 52 | for( int i = 0; i < level; i++ ) 53 | sigma2 *= Config::orbScaleFactor(); 54 | sigma2 = 1.f / (sigma2*sigma2); 55 | } 56 | 57 | PointFeature::PointFeature( Vector2d pl_, double disp_, Vector3d P_, Vector2d pl_obs_ ) : 58 | pl(pl_), disp(disp_), P(P_), pl_obs(pl_obs_), inlier(true), level(0) 59 | {} 60 | 61 | PointFeature::PointFeature( Vector2d pl_, double disp_, Vector3d P_, Vector2d pl_obs_, 62 | int idx_, int level_, double sigma2_, Matrix3d covP_an_, bool inlier_ ) : 63 | pl(pl_), disp(disp_), P(P_), pl_obs(pl_obs_), inlier(inlier_), level(level_), sigma2(sigma2_), covP_an(covP_an_) 64 | {} 65 | 66 | PointFeature* PointFeature::safeCopy(){ 67 | return new PointFeature( pl, disp, P, pl_obs, idx, level, sigma2, covP_an, inlier ); 68 | } 69 | 70 | 71 | 72 | // Line segment feature 73 | 74 | LineFeature::LineFeature( Vector3d sP_, Vector3d eP_, Vector3d le_obs_) : 75 | sP(sP_), eP(eP_), le_obs(le_obs_), level(0) 76 | {} 77 | 78 | 79 | LineFeature::LineFeature( Vector3d sP_, Vector3d eP_, Vector3d le_obs_, Vector2d spl_obs_, Vector2d epl_obs_) : 80 | sP(sP_), eP(eP_), le_obs(le_obs_), spl_obs(spl_obs_), epl_obs(epl_obs_), level(0) 81 | {} 82 | 83 | 84 | LineFeature::LineFeature( Vector2d spl_, double sdisp_, Vector3d sP_, 85 | Vector2d epl_, double edisp_, Vector3d eP_, Vector3d le_) : 86 | spl(spl_), sdisp(sdisp_), sP(sP_), epl(epl_), edisp(edisp_), eP(eP_), le(le_), inlier(true), level(0) 87 | {} 88 | 89 | LineFeature::LineFeature( Vector2d spl_, double sdisp_, Vector3d sP_, 90 | Vector2d epl_, double edisp_, Vector3d eP_, 91 | Vector3d le_, Vector3d le_obs_ ) : 92 | spl(spl_), sdisp(sdisp_), sP(sP_), epl(epl_), edisp(edisp_), eP(eP_), le(le_), le_obs(le_obs_), inlier(true), level(0) 93 | {} 94 | 95 | LineFeature::LineFeature( Vector2d spl_, double sdisp_, Vector3d sP_, 96 | Vector2d epl_, double edisp_, Vector3d eP_, 97 | Vector3d le_, int idx_) : 98 | spl(spl_), sdisp(sdisp_), sP(sP_), epl(epl_), edisp(edisp_), eP(eP_), le(le_), inlier(true), idx(idx_), level(0) 99 | {} 100 | 101 | LineFeature::LineFeature( Vector2d spl_, double sdisp_, Vector3d sP_, 102 | Vector2d epl_, double edisp_, Vector3d eP_, 103 | Vector3d le_, double angle_, int idx_) : 104 | spl(spl_), sdisp(sdisp_), sP(sP_), epl(epl_), edisp(edisp_), eP(eP_), le(le_), inlier(true), idx(idx_), angle(angle_), level(0) 105 | {} 106 | 107 | LineFeature::LineFeature( Vector2d spl_, double sdisp_, Vector3d sP_, 108 | Vector2d epl_, double edisp_, Vector3d eP_, 109 | Vector3d le_, double angle_, int idx_, int level_) : 110 | spl(spl_), sdisp(sdisp_), sP(sP_), epl(epl_), edisp(edisp_), eP(eP_), le(le_), inlier(true), idx(idx_), angle(angle_), level(level_) 111 | { 112 | for( int i = 0; i < level; i++ ) 113 | sigma2 *= Config::lsdScale(); 114 | sigma2 = 1.f / (sigma2*sigma2); 115 | } 116 | 117 | LineFeature::LineFeature( Vector2d spl_, double sdisp_, Vector3d sP_, Vector2d spl_obs_, double sdisp_obs_, 118 | Vector2d epl_, double edisp_, Vector3d eP_, Vector2d epl_obs_, double edisp_obs_, 119 | Vector3d le_, Vector3d le_obs_, double angle_, int idx_, int level_, bool inlier_, double sigma2_, 120 | Matrix3d covE_an_, Matrix3d covS_an_) : 121 | 122 | spl(spl_), sdisp(sdisp_), sP(sP_), spl_obs(spl_obs_), sdisp_obs(sdisp_obs_), 123 | epl(epl_), edisp(edisp_), eP(eP_), epl_obs(epl_obs_), edisp_obs(edisp_obs_), 124 | le(le_), le_obs(le_obs_), angle(angle_), idx(idx_), level(level_), inlier(inlier_), sigma2(sigma2_), covE_an(covE_an_), covS_an(covS_an_) 125 | { 126 | for( int i = 0; i < level; i++ ) 127 | sigma2 *= Config::lsdScale(); 128 | sigma2 = 1.f / (sigma2*sigma2); 129 | } 130 | 131 | LineFeature* LineFeature::safeCopy(){ 132 | return new LineFeature( spl, sdisp, sP, spl_obs, sdisp_obs, 133 | epl, edisp, eP, epl_obs, edisp_obs, 134 | le, le_obs, angle, idx, level, inlier, sigma2, covE_an, covS_an ); 135 | } 136 | 137 | /*LineFeature::LineFeature( Vector2d spl_, double sdisp_, Vector3d sP_, 138 | Vector2d epl_, double edisp_, Vector3d eP_, 139 | Vector3d le_, double angle_, int idx_, int level_, Vector2d spr_, Vector2d epr_ ) : 140 | spl(spl_), sdisp(sdisp_), sP(sP_), epl(epl_), edisp(edisp_), eP(eP_), le(le_), inlier(true), idx(idx_), angle(angle_), level(level_), spr(spr_), epr(epr_) 141 | { 142 | for( int i = 0; i < level+1; i++ ) 143 | sigma2 *= Config::lsdScale(); 144 | sigma2 = 1.f / (sigma2*sigma2); 145 | }*/ 146 | 147 | /*LineFeature::LineFeature( Vector2d spl_, double sdisp_, Vector3d sP_, 148 | Vector2d epl_, double edisp_, Vector3d eP_, 149 | Vector3d le_, double angle_, int idx_, int level_, Matrix3d covS_an_, Matrix3d covE_an_) : 150 | spl(spl_), sdisp(sdisp_), sP(sP_), epl(epl_), edisp(edisp_), eP(eP_), le(le_), inlier(true), idx(idx_), angle(angle_), level(level_), covS_an(covS_an_), covE_an(covS_an_) 151 | { 152 | for( int i = 0; i < level; i++ ) 153 | sigma2 *= Config::lsdScale(); 154 | sigma2 = 1.f / (sigma2*sigma2); 155 | }*/ 156 | 157 | 158 | } 159 | -------------------------------------------------------------------------------- /src/timer.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** Stereo VO and SLAM by combining point and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2016-2018, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2016-2018, David Zuñiga-Noël, University of Malaga ** 7 | ** Copyright(c) 2016-2018, MAPIR group, University of Malaga ** 8 | ** ** 9 | ** This program is free software: you can redistribute it and/or modify ** 10 | ** it under the terms of the GNU General Public License (version 3) as ** 11 | ** published by the Free Software Foundation. ** 12 | ** ** 13 | ** This program is distributed in the hope that it will be useful, but ** 14 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 15 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 16 | ** GNU General Public License for more details. ** 17 | ** ** 18 | ** You should have received a copy of the GNU General Public License ** 19 | ** along with this program. If not, see . ** 20 | ** ** 21 | *****************************************************************************/ 22 | 23 | #include "timer.h" 24 | 25 | //STL 26 | #include 27 | #include 28 | 29 | namespace StVO { 30 | 31 | Timer::Timer(double scale) : started(false), scale(scale) { } 32 | Timer::~Timer() { } 33 | 34 | void Timer::start() { 35 | 36 | started = true; 37 | start_t = std::chrono::high_resolution_clock::now(); 38 | } 39 | 40 | double Timer::stop() { 41 | 42 | std::chrono::high_resolution_clock::time_point end_t = std::chrono::high_resolution_clock::now(); 43 | 44 | if (!started) 45 | throw std::logic_error("[Timer] Stop called without previous start"); 46 | 47 | started = false; 48 | std::chrono::duration elapsed_ns = end_t - start_t; 49 | return elapsed_ns.count()*scale; 50 | } 51 | 52 | } // namespace StVO 53 | --------------------------------------------------------------------------------